Positioning and mapping method based on laser radar and vision fusion in dynamic environment

Abstract

The invention relates to the technical field of robot navigation and environmental perception positioning, and discloses a laser radar and vision fused positioning and mapping method in a dynamic environment, and the method comprises the steps: obtaining laser point cloud data of a surrounding environment, two-dimensional image data with depth information, and posture information of a robot; preprocessing the laser point cloud data and the two-dimensional image data to obtain the laser point cloud data and the preprocessed two-dimensional image data; feature values of the preprocessed laser point cloud data and the preprocessed two-dimensional image data are extracted, the extracted feature values are matched and fused to obtain fused feature point cloud data containing image feature information at the same time, the feature point cloud data and the attitude information are optimized, and the fusion feature point cloud data is obtained. Obtaining fusion data containing the feature point cloud data and the positioning information; and performing back-end optimization and loopback detection on the fused data to realize map construction and self-positioning, so that the positioning and map construction precision in a dynamic environment can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot navigation and environment perception and positioning, and in particular to a positioning and map building method of laser radar and vision fusion in a dynamic environment. Background Art

[0002] At present, the Simultaneous Localization and Mapping (SLAM) technology for dynamic environments has been widely used in many fields.

[0003] However, traditional SLAM technology based on a single sensor (lidar or visual camera) is susceptible to interference from dynamic objects, scarcity of feature points, and susceptibility to occlusion and noise, resulting in decreased positioning accuracy and unstable map construction.

[0004] Therefore, a new method is urgently needed to effectively fuse laser and visual information to improve the accuracy of positioning and mapping in dynamic environments. Summary of the invention

[0005] In view of this, the present invention provides a positioning and map construction method by integrating lidar and vision in a dynamic environment to solve the technical problem of insufficient accuracy of positioning and map construction in a dynamic environment in the prior art.

[0006] In the first aspect, the present invention provides a positioning and map construction method for laser radar and vision fusion in a dynamic environment, including: obtaining laser point cloud data of the surrounding environment, two-dimensional image data with depth information and the robot's own posture information through laser radar, depth vision camera and inertial measurement module respectively; performing time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain pre-processed laser point cloud data and two-dimensional image data; extracting feature values ​​of the pre-processed laser point cloud data and the pre-processed two-dimensional image data, and matching and fusing the extracted feature values ​​to obtain fused feature point cloud data containing image feature information at the same time, optimizing the feature point cloud data and posture information to obtain fused data containing feature point cloud data and positioning information; performing back-end optimization and loop detection on the fused data to realize map construction and self-positioning.

[0007] Optionally, the time synchronization process includes: using the precise clock synchronization protocol PTP to obtain a first hardware timestamp of a synchronization message sent by the master clock, a second hardware timestamp of a synchronization message received by the slave clock, a third hardware timestamp of a delay request message sent by the slave clock, and a fourth hardware timestamp of a delay request message received by the master clock; calculating the synchronization error and synchronization delay of the master clock and the slave clock based on the first hardware timestamp, the second hardware timestamp, the third hardware timestamp, and the fourth hardware timestamp, wherein the master clock is the clock of a processing unit that receives laser point cloud data and two-dimensional image data, and the slave clock is the clock of a laser radar or a depth vision camera; performing preliminary time synchronization on the laser point cloud data and the two-dimensional image data based on the synchronization error and the synchronization delay; using the Kalman filter algorithm to fuse multipath delay data, modeling the time offset as a dynamic system, and using the Kalman filter to estimate the time offset and drift of the clocks used to collect the laser radar and the depth vision camera; and correcting the time of the laser point cloud data and the two-dimensional image data after preliminary time synchronization based on the time offset and the drift.

[0008] Optionally, the coordinate alignment process includes: obtaining the rotation matrix and translation vector of the laser radar and the depth vision camera; obtaining the intrinsic parameter matrix of the depth vision camera; and projecting the laser point cloud data from the point cloud coordinate system to the pixel coordinate system according to the intrinsic parameter matrix, the rotation matrix and the translation vector to achieve coordinate alignment.

[0009] Optionally, the data denoising process includes: determining the threshold distance and target distance of the laser point cloud data through the Manhattan distance formula, and removing noise points based on the threshold distance and the target distance; performing image enhancement processing on the two-dimensional image data using adaptive histogram equalization, eliminating the noise of the two-dimensional image data through Gaussian filtering, and using a deep learning model to perform semantic segmentation on the two-dimensional image data to extract the semantic label of each pixel.

[0010] Optionally, the feature values ​​of the preprocessed laser point cloud data include point cloud features, and the feature values ​​of the preprocessed two-dimensional image data include color features and semantic features; correspondingly, the feature values ​​of the preprocessed laser point cloud data and the preprocessed two-dimensional image data are extracted, and the extracted feature values ​​are matched and fused to obtain fused feature point cloud data that also contains image feature information, including: associating the three-dimensional points in the preprocessed laser point cloud data and the RGB pixels in the preprocessed two-dimensional image data based on the coordinate alignment result; obtaining the color features of each three-dimensional point based on the coordinates of the associated RGB pixels, adding the color features to each three-dimensional point of the laser point cloud data to generate a color point cloud; obtaining the semantic features of each three-dimensional point based on the coordinates of the associated RGB pixels, fusing the point cloud features and the semantic features to obtain fused feature point cloud data.

[0011] Optionally, after obtaining the semantic features of each three-dimensional point based on the coordinates of the associated RGB pixels, fusing the point cloud features and the semantic features to obtain fused feature point cloud data, it also includes: obtaining a dynamic point cloud in the fused feature point cloud data based on the semantic features, and removing the dynamic point cloud.

[0012] Optionally, the fused data is subjected to back-end optimization and loop closure detection to achieve map construction and self-positioning, including: constructing the observation results of the fused data into a factor graph; optimizing the factor graph using a nonlinear optimization algorithm to obtain map feature information and positioning information; performing loop closure detection based on the map feature information and positioning information to eliminate drift errors and achieve map construction and self-positioning.

[0013] In the second aspect, the present invention provides a positioning and map construction device that integrates laser radar and vision in a dynamic environment, including: a data acquisition module, which is used to obtain laser point cloud data of the surrounding environment, two-dimensional image data with depth information and the robot's own posture information through laser radar, depth vision camera and inertial measurement module respectively; a data preprocessing module, which is used to perform time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain preprocessed laser point cloud data and preprocessed two-dimensional image data; a data fusion module, which is used to extract feature values ​​of the preprocessed laser point cloud data and the preprocessed two-dimensional image data, and match and fuse the extracted feature values ​​to obtain fused feature point cloud data that also contains image feature information, optimize the feature point cloud data and posture information, and obtain fused data containing feature point cloud data and positioning information; a positioning and mapping module, which is used to perform back-end optimization and loop detection on the fused data to achieve map construction and self-positioning.

[0014] In a third aspect, the present invention provides a computer device, comprising: a memory and a processor, the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the positioning and mapping method of laser radar and vision fusion in a dynamic environment of the above-mentioned first aspect or any corresponding embodiment thereof by executing the computer instructions.

[0015] In a fourth aspect, the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to enable a computer to execute the positioning and mapping method of laser radar and vision fusion in a dynamic environment of the above-mentioned first aspect or any corresponding embodiment thereof.

[0016] The positioning and map construction method of laser radar and vision fusion in a dynamic environment according to the embodiment of the present invention has the following effects:

[0017] The positioning and map construction method of laser radar and vision fusion in a dynamic environment of the embodiment of the present invention respectively obtains laser point cloud data of the surrounding environment, two-dimensional image data with depth information and the robot's own posture information through laser radar, depth vision camera and inertial measurement module, performs time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain pre-processed laser point cloud data and pre-processed two-dimensional image data, extracts feature values ​​of the pre-processed laser point cloud data and the pre-processed two-dimensional image data, matches and fuses the extracted feature values ​​to obtain fused feature point cloud data containing image feature information at the same time, optimizes the feature point cloud data and posture information to obtain fused data containing feature point cloud data and positioning information, performs back-end optimization and loop detection on the fused data to realize map construction and self-positioning, and can effectively improve the accuracy of positioning and mapping in a dynamic environment. The environmental information obtained by this method is accurate and the map construction quality is high. It can currently be applied to the field of navigation and positioning technology of robots in dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0019] Figure 1 is a schematic diagram of a process of positioning and map construction by laser radar and vision fusion in a dynamic environment according to an embodiment of the present invention;

[0020] Figure 2 is a principle block diagram of another positioning and map building method of laser radar and vision fusion in a dynamic environment according to an embodiment of the present invention;

[0021] Figure 3 is a principle block diagram of a positioning and map building method of laser radar and vision fusion in a dynamic environment according to another embodiment of the present invention;

[0022] Figure 4 is a principle block diagram of another positioning and map building method of laser radar and vision fusion in a dynamic environment according to an embodiment of the present invention;

[0023] Figure 5 is a flow chart of data fusion of laser point cloud and visual information according to an embodiment of the present invention;

[0024] Figure 6 is a schematic diagram of data fusion of laser point cloud and visual information according to an embodiment of the present invention;

[0025] Figure 7 is a structural block diagram of a positioning and map building device for laser radar and vision fusion in a dynamic environment according to an embodiment of the present invention;

[0026] Figure 8 It is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0027] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0028] In order to achieve high-precision positioning and high-quality map construction in a dynamic environment, this patent proposes a positioning and map construction method that integrates lidar and vision in a dynamic environment.

[0029] At present, although real-time positioning and mapping technology has made significant progress in positioning accuracy, robustness, and multi-scenario adaptability, it still has many shortcomings. For example, the biggest challenge at present is map construction in complex environments. Map construction often fails due to large errors in the collected data or there is a large error between the actual environment and the positioning and mapping. The core problems are the high cost of sensors, the complexity of data fusion, the contradiction between real-time and computing resources, and the difficulties in system integration and application. For the positioning and mapping methods that fuse lidar and visual sensors, due to the fusion of sensor data and the limitations of computing resources, positioning and mapping in actual environments often have large errors.

[0030] For example, the patent "Substation semantic map construction method based on laser SLAM and visual fusion" CN114638909A calibrates and preprocesses the data collected by the sensor, and projects the identified targets into the grid map, provides environmental cognition information for the substation, and uses the collected information to model the operation and maintenance environment map, thus realizing the construction of semantic maps and improving the adaptability and accuracy of map construction in the review environment. The present invention is essentially different from the patent in the system data collection preprocessing stage. In order to synchronously process the data collected by the two sensors, this patent unifies the clock sources of the laser radar and depth camera in the hardware, so that the two sensors can independently collect data with timestamps under the same clock as much as possible in the future, and then use the Time Synchronizer to process them to achieve time synchronization of the two data; the present invention uses PTP (Precision Time Protocol) to synchronize the sensor clock, so that sensors such as laser radar and visual cameras share the time reference at the hardware level through trigger signals, and no longer rely on hardware support; at the same time, Kalman filtering is used at the software level for time correction and alignment to achieve high-precision time synchronization and reduce the accumulation of information errors.

[0031] In addition, the patent "A method for building a map based on laser radar and visual fusion" CN 110275179A sets a laser radar and a camera on the robot, and uses visual data and laser radar data to fuse in the constructed map, which overcomes the defect that vision cannot derive the distance of buildings, and also makes up for the problem of inaccurate long-distance detection of laser radar. The way the present invention finally constructs the map is essentially different from that of the patent. When obtaining the final map, the patent relies on motion instructions to scan the environment, collect surrounding environment data to build a preliminary map, and repeatedly collect surrounding environment information to obtain a supplementary map and compare it with the preliminary map, and finally realizes the construction of the map; in the final positioning and mapping system of the present invention, the surrounding environment information is collected by the early sensors to complete the joint calibration of different types of data, and the laser point cloud and visual depth information are fused and the dynamic point cloud in the environment is eliminated through the data fusion system, and the fusion of multi-sensor data is completed. When constructing the final map, the fused data is first used to perform back-end optimization to eliminate drift errors, and further loop detection is performed to ensure the accuracy of the data, and finally, accurate self-positioning can be achieved while completing the construction of a high-precision map. Compared with this patent, the present invention has higher accuracy, higher system coupling, more efficient data utilization, and can achieve more efficient and accurate map construction in complex environments.

[0032] The technical effect of the patent of this invention lies in the method and system for preprocessing the preliminary data and constructing the final map. In the data preprocessing part, the clock source of the sensor is unified at the hardware level, and the Kalman filter (KF) method is also used at the software level for dynamic correction to achieve high-precision time synchronization, avoiding a large amount of noise signals caused by time accumulation errors during data fusion, and interference during point cloud denoising of dynamic targets in the later stage, resulting in the failure of the system to build maps in dynamic environments and a significant decrease in self-positioning accuracy. In addition, in terms of constructing the final map, the back-end optimization is still performed after obtaining the fused data, and the denoising of dynamic objects has been completed before this. The final system undergoes loop detection and uses the optimal data to construct the map, while still maintaining perception of the surrounding environment, and updating the data in real time to ensure the quality of map construction and the accuracy of its own positioning information, effectively meeting the application requirements for self-positioning and real-time map construction in dynamic environments.

[0033] According to an embodiment of the present invention, an embodiment of a positioning and map construction method for laser radar and vision fusion in a dynamic environment is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0034] In an embodiment of the present invention, a positioning and map construction method of laser radar and vision fusion in a dynamic environment is provided, which can be applied to the navigation and positioning of robots in a dynamic environment.

[0035] like Figure 1 , Figure 2 , Figure 3 , Figure 4 , Figure 5 and Figure 6 As shown, the present invention provides a positioning and map construction method of laser radar and vision fusion in a dynamic environment, which is applied to the navigation and positioning of robots in a dynamic environment, including:

[0036] Step S101, respectively obtain laser point cloud data of the surrounding environment, two-dimensional image data with depth information and the robot's own posture information through laser radar, depth vision camera and inertial measurement unit (IMU).

[0037] Specifically, the robot is equipped with a laser radar, a depth vision camera and an inertial measurement module. The laser radar senses the surrounding environment, emits and receives laser beams, and measures the time difference or phase difference of the laser signal to determine the distance to the obstacle, so as to obtain the distance information of the obstacle in front of the visually impaired person, thereby collecting high-precision laser point cloud data; the depth vision camera uses an infrared laser as a light source, and the emitted light is projected onto the object after encoding. The algorithm is used to calculate the distortion of the returned coding pattern to obtain the position and depth information of the object, and the algorithm is used to calculate the collected structured light image, which can not only obtain the original two-dimensional image information of the detection environment but also the depth information of the image, thereby obtaining two-dimensional image data with depth information; the inertial measurement module monitors the motion state information of the current environment in real time, monitors the current state information with itself as the origin, obtains accurate posture information, and completes the collection of initial information.

[0038] The collected information is sent to the robot's processing unit via wired or wireless means for calculation.

[0039] Step S102 , performing time synchronization, coordinate alignment and data denoising processing on the laser point cloud data and the two-dimensional image data to obtain pre-processed laser point cloud data and pre-processed two-dimensional image data.

[0040] Specifically, this step is to screen the collected initial information and convert and calibrate various parameters.

[0041] Since the data formats collected by LiDAR and deep vision cameras are different, the two types of data need to be pre-processed by time synchronization, coordinate alignment, data denoising, etc. before data fusion. Ensure that the collected LiDAR point cloud and visual data are in the same time base, determine that the point cloud coordinate system and pixel coordinate system are unified into a global coordinate system, and complete the unification of the two different types of data in time and space.

[0042] At the same time, it is determined to remove noise data and correct visual image enhancement and distortion.

[0043] Step S103, extracting feature values ​​of the preprocessed laser point cloud data and the preprocessed two-dimensional image data, matching and fusing the extracted feature values ​​to obtain fused feature point cloud data containing both image feature information, optimizing the feature point cloud data and posture information, and obtaining fused data containing feature point cloud data and positioning information.

[0044] Specifically, image feature information includes geometric information, color information, texture information and semantic information.

[0045] By extracting feature values ​​from laser point cloud data and two-dimensional image data and matching these two different types of data, the acquisition of geometric information and texture information in the same environment location can be achieved.

[0046] By extracting and matching the feature values ​​of the processed lidar and visual data, the point cloud feature values ​​are integrated with the color, semantics, and geometric information of the image, thereby obtaining a composition point cloud that contains geometric information (x, y, z), color information [R, G, B], and semantic information (roads, buildings, vehicles, etc.), that is, fused feature point cloud data.

[0047] At the same time, for those areas where the point cloud cannot be projected to the image coordinates due to edges or occlusions, the geometric information is retained and the color information is set to the default value of black. For dynamic scenes, the interference of the point cloud of dynamic objects is eliminated, and the point cloud that has been integrated with the color and semantic information of the visual image is combined. By adopting a tight coupling method, the fused feature point cloud data and posture information are combined to construct a joint cost function to optimize the data.

[0048] The constructed joint cost function is:

[0049]

[0050] Among them, Z LiDAR and Z Vision are the observed values ​​of the eigenvalues ​​of the laser point cloud data and the eigenvalues ​​of the two-dimensional image data, respectively, LiDAR (x) and h Vision (x) are the predicted values ​​of the eigenvalues ​​of the laser point cloud data and the eigenvalues ​​of the two-dimensional image data, respectively. J is the optimization target of the joint cost function. i and j represent the data collected under different posture information. The fusion of the laser point cloud and visual information data is completed, and then the nonlinear optimization (Levenberg-Marquardt) is used to solve the optimal posture. Finally, the fused data that has integrated the feature point cloud data and the positioning information is output.

[0051] Step S104, performing backend optimization and loop detection on the fused data to achieve map construction and self-positioning.

[0052] By performing back-end optimization on the fused data to eliminate drift errors and further performing loop detection to improve work efficiency, accurate self-positioning can be achieved while completing high-precision map construction.

[0053] The positioning and map construction method of the laser radar and vision fusion in a dynamic environment of the embodiment of the present invention obtains laser point cloud data of the surrounding environment, two-dimensional image data with depth information and the robot's own posture information through the laser radar, deep vision camera and inertial measurement module respectively, performs time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain pre-processed laser point cloud data and pre-processed two-dimensional image data, extracts feature values ​​of the pre-processed laser point cloud data and the pre-processed two-dimensional image data, matches and fuses the extracted feature values ​​to obtain fused feature point cloud data containing image feature information at the same time, optimizes the feature point cloud data and the posture information to obtain fused data containing feature point cloud data and positioning information, performs back-end optimization and loop detection on the fused data to realize map construction and self-positioning, and can effectively improve the accuracy of positioning and mapping in a dynamic environment. The environmental information obtained by this method is accurate and the map construction quality is high. It can currently be applied to the field of navigation and positioning technology of robots in dynamic environments.

[0054] In some embodiments, in step S102, the time synchronization process includes:

[0055] Step S1021, using the Precision Time Protocol (PTP) to obtain a first hardware timestamp of the synchronization message sent by the master clock, a second hardware timestamp of the synchronization message received by the slave clock, a third hardware timestamp of the delay request message sent by the slave clock, and a fourth hardware timestamp of the delay request message received by the master clock.

[0056] Step S1022, calculate the synchronization error and synchronization delay of the master clock and the slave clock based on the first hardware timestamp, the second hardware timestamp, the third hardware timestamp and the fourth hardware timestamp, wherein the master clock is the clock of the processing unit that receives the laser point cloud data and the two-dimensional image data, and the slave clock is the clock of the laser radar or the depth vision camera.

[0057] Step S1023, performing preliminary time synchronization on the laser point cloud data and the two-dimensional image data based on the synchronization error and the synchronization delay;

[0058] Step S1024, using the Kalman filter algorithm to fuse the multipath delay data, modeling the time offset as a dynamic system, and using the Kalman filter to estimate the time offset and drift of the clocks of the laser radar and the depth vision camera;

[0059] Step S1025, correcting the time of the laser point cloud data and the two-dimensional image data after preliminary time synchronization based on the time offset and the drift.

[0060] Specifically, taking the laser of a laser radar as an example, in the PTP scheme, the processing unit periodically sends a synchronization message Sync to the laser radar. The synchronization message Sync carries the first hardware timestamp of the master clock at this time. After receiving the synchronization message Sync, the laser radar records the second hardware timestamp of the slave clock at the receiving time, and then returns a delay request message Delay_Req to the processing unit. The delay request message Delay_Req carries the second hardware timestamp and the third timestamp of the slave clock sending time. After receiving the delay request message Delay_Req, the processing unit records the fourth timestamp of the master clock at this time.

[0061] Then the synchronization error and synchronization delay of the master clock and the slave clock are:

[0062]

[0063] Wherein, Offset is the synchronization error, Delay is the synchronization delay, T1, T2, T3 and T4 are the first hardware timestamp, the second hardware timestamp, the third hardware timestamp and the fourth hardware timestamp respectively.

[0064] By calculating the synchronization error and synchronization delay between the lidar and the processing unit, as well as the synchronization error and synchronization delay between the depth vision camera and the processing unit, the lidar and the depth vision camera can be synchronized with the master clock for hardware timestamp.

[0065] During the hardware timestamp synchronization process, since there is a certain interval between each synchronization, and different clocks will have clock offsets over time, it is necessary to use a software algorithm to correct the clocks of the two sensors (i.e., the lidar and the depth vision camera) during the interval.

[0066] The embodiment of the present invention uses the Kalman filter algorithm to fuse multipath delay data, models the time offset as a dynamic system, uses the Kalman filter to estimate the time offset and drift, and defines the state variable Among them, x k is the state variable, Δt k is the time offset, Δt′ k is the time drift rate, and the state transfer equation is listed as x k =F·x k-1 +w k , Where ΔT is the time step, w k is the process noise (including time jitter). Establish an observation model and define the observation value as the difference in sensor timestamps: k =t sensor1，k -t senser2，k +v k , where z k is the observed variable, vk is the measurement noise, t sensor1，k and t senser2，k is the timestamp of two sensors at the same time; list the observation equation z k =H·x k +v k , H =

[10] for filtering, first predict P k|k-1 =F·P k-1|k-1 ·F T +Q, where Represents the predicted value of the state variable, P is the covariance matrix, Q is the process noise covariance, and then the data is updated P k|k =(IK k ·H)·P k|k-1 , P k|k is the intermediate variable, K k is the Kalman gain, To measure the noise covariance matrix, the final output correction value is obtained to obtain the estimated time offset Δt k and drift rate Δt′ k , which realizes the correction of different sensor data time.

[0067] The embodiment of the present invention uses the PTP protocol to synchronize the sensor clock, so that sensors such as lidar and visual cameras share the time reference at the hardware level through trigger signals, and no longer rely on hardware support; at the same time, Kalman filtering is used at the software level to perform time correction and alignment to achieve high-precision time synchronization while reducing the accumulation of information errors.

[0068] Furthermore, in step S102, the coordinate alignment process includes:

[0069] Step S1026, obtaining the rotation matrix and translation vector of the laser radar and the depth vision camera;

[0070] Step S1027, obtaining the intrinsic parameter matrix of the depth vision camera;

[0071] Step S1028, projecting the laser point cloud data from the point cloud coordinate system to the pixel coordinate system according to the intrinsic parameter matrix, the rotation matrix and the translation vector to achieve coordinate alignment.

[0072] Specifically, for different coordinate systems, the laser radar point cloud coordinate system and the pixel coordinate system of the visual image should be unified into a global coordinate system.

[0073] First, perform external parameter calibration. Based on the geometric data information of the image collected by the depth vision camera, use the calibration tool (chessboard) to obtain a more accurate rotation matrix R and translation vector T between the lidar and the depth vision camera. Calculate the transformation matrix using the calibration results Apply it to the image data. Then perform internal calibration to obtain the internal parameter matrix of the depth vision camera. f x and f y is the focal length coordinate of the depth vision camera; c x and c y is the optical center coordinate. Project the LiDAR point cloud into the pixel coordinate system of the RGB image: p = K·(R·P LiDAR +T), where p is the coordinate of the laser radar point cloud in the pixel coordinate system, P LiDAR =[XYZ] T The coordinates of the point cloud points in the laser radar coordinate system are used to complete the data association and calibration.

[0074] Furthermore, in step S102, the process of data denoising includes:

[0075] Step S1029, determining the threshold distance and target distance of the laser point cloud data by using the Manhattan distance formula, and removing noise points based on the threshold distance and the target distance;

[0076] Step S10210, using adaptive histogram equalization to perform image enhancement processing on the two-dimensional image data, eliminating the noise of the two-dimensional image data through Gaussian filtering, and using a deep learning model to perform semantic segmentation on the two-dimensional image data to extract the semantic label of each pixel.

[0077] Specifically, for the collected laser point cloud coordinate data, the Manhattan distance formula d = |x2-x1| + |y2-y1| + |z2-z1| is used to statistically remove noise points, where x1, y1 and z1 are the coordinates of the preset points, x2, y2 and z2 are the coordinates of the target points, and d is the target distance between the two points. When the target distance between the two points is greater than the set distance threshold, the target point is removed to reduce the impact of point cloud noise on data quality, making the laser point cloud coordinate data more accurate.

[0078] The collected two-dimensional image data is enhanced by adaptive histogram equalization and noise is eliminated by Gaussian filtering. At the same time, the deep learning model YOLOv8 is used to perform semantic segmentation on the two-dimensional image data and extract the semantic label of each pixel. The semantic segmentation result can be represented by a semantic mask, where each pixel corresponds to a semantic category.

[0079] In some embodiments, the feature values ​​of the preprocessed laser point cloud data include point cloud features, and the feature values ​​of the preprocessed two-dimensional image data include color features and semantic features;

[0080] Correspondingly, in step S103, feature values ​​of the preprocessed laser point cloud data and the preprocessed two-dimensional image data are extracted, and the extracted feature values ​​are matched and fused to obtain fused feature point cloud data containing image feature information, including:

[0081] Step S1031, associating the three-dimensional points in the preprocessed laser point cloud data with the RGB pixels in the preprocessed two-dimensional image data based on the coordinate alignment result;

[0082] Step S1032, obtaining the color feature of each 3D point based on the coordinates of the associated RGB pixels, adding the color feature to each 3D point of the laser point cloud data, and generating a color point cloud;

[0083] Step S1033, obtaining the semantic features of each 3D point based on the coordinates of the associated RGB pixels, and fusing the point cloud features with the semantic features to obtain fused feature point cloud data.

[0084] Exemplarily, the process of projecting the LiDAR point cloud to the pixel coordinate system of the RGB image includes two steps:

[0085] (1) Conversion of point cloud coordinate system to camera coordinate system: Where (x l ,y l , z l ) is a point in the point cloud coordinate system, (x c ,y c , z c ) is a point in the camera coordinate system.

[0086] (2) Projection to the image plane: (u, v) is the pixel coordinate on the image plane; the depth value z c Used for occlusion processing.

[0087] Through projection, the three-dimensional points of the point cloud are associated with the RGB pixels in the image to achieve the fusion of the point cloud and the image, so that each point in the point cloud can be assigned corresponding color information [R, G, B].

[0088] The process of fusing point cloud and image color information includes:

[0089] After projecting the point cloud onto the image plane, the corresponding color value can be obtained from the image through the pixel coordinates (u, v). The color value (r, g, b) is extracted from the image using the projected pixel coordinates (u, v). Each point in the point cloud can be assigned corresponding color information.

[0090] If (u, v) is an integer, the color of the pixel is used directly; if (u, v) is a floating point number, the data is weighted by using an interpolation method such as bilinear interpolation to calculate its color value; if the pixel coordinates (u, v) exceed the image range after the point cloud point is projected onto the image plane, the point may be outside the camera's field of view and is considered a discarded point. The obtained color value (r, g, b) is added to each point in the point cloud to generate a colored point cloud.

[0091] The process of fusing point cloud and image semantic information includes:

[0092] According to the pixel coordinates of the point cloud projected onto the image plane, the corresponding pixel semantic labels are found, the image semantic features are mapped to the 3D points in the point cloud, and semantic labels (such as roads, buildings, vehicles, etc.) are assigned to each 3D point to generate enhanced 3D point cloud features and obtain fused feature point cloud data. For each point cloud point, the corresponding feature semantics is directly extracted from the image feature map using its pixel coordinates (u, v) projected onto the image. If (u, v) is a floating point number, the semantic features are calculated from the image feature map using bilinear interpolation. If there is no corresponding pixel in the point cloud (such as a sparse area of ​​the point cloud), the feature value is supplemented using nearest neighbor interpolation.

[0093] Furthermore, according to factors such as the distance between the point cloud point and the depth vision camera, the image features are weighted and fused with the point cloud features. fusion =αF image +βF point , where α and β are weight coefficients, F fusion is the eigenvalue of the fused feature point cloud data after fusion, F image is the image feature, including color feature and semantic feature, F point Point cloud features.

[0094] In some embodiments, in step S1033, after obtaining the semantic features of each 3D point based on the coordinates of the associated RGB pixels and fusing the point cloud features with the semantic features to obtain fused feature point cloud data, the method further includes:

[0095] Step S1034, obtaining the dynamic point cloud in the fused feature point cloud data based on the semantic features, and removing the dynamic point cloud.

[0096] Specifically, by extracting geometric and semantic data from each fused point cloud information, according to the semantic segmentation results, the dynamic categories (such as vehicles, pedestrians, animals, etc.) are marked as 1, and the static categories (such as ground, buildings, trees, etc.) are marked as 0. Similarly, the points that have moved in the multi-frame point cloud are marked as 1, and the points that have not changed are marked as 0. If the mask value is 1 (dynamic point), the point cloud point is deleted. If the mask value is 0 (static point), the point cloud point is retained. According to the mask value, the dynamic point cloud data is eliminated and the static scene point cloud is retained. The static scene point cloud obtained is represented as: P static ={p∈P0|M(p)=0}, where P0 is the original point cloud set, M(p) is the mask value of point p, and P static A static point cloud collection.

[0097] Optionally, step S104, performing backend optimization and loop detection on the fused data to achieve map construction and self-positioning, including:

[0098] Step S1041, constructing the observation results of the fused data into a factor graph;

[0099] Step S1042, optimizing the factor graph using a nonlinear optimization algorithm to obtain map feature information and positioning information;

[0100] Step S1043, loop detection is performed based on the map feature information and positioning information to eliminate drift errors and achieve map construction and self-positioning.

[0101] Specifically, the observation results of the fusion data are built into a factor graph, which consists of variable nodes and factor nodes. Variable nodes represent the robot's posture and feature points in the map, and factor nodes represent the constraint relationship between the observation data and the variable nodes, such as the constraint between the laser radar point cloud data and posture, and the constraint between the visual features and posture.

[0102] A nonlinear optimization algorithm (such as Bundle Adjustment) is used to optimize the factor graph and adjust the values ​​of the variable nodes to minimize the error between the observed data and the model prediction. The optimization process takes into account the error models and constraints of various sensor data to obtain the optimal pose estimation and map feature point locations.

[0103] For example, by applying a graph optimization method (Ceres Solver) to optimize the factor graph, refer to the constructed residual function where r i =y i -h(x,θ i ) outputs data as close to the observed data as possible, where r i is the residual, y iis the observed data, h(x i ,θ i ) is the model output, so as to construct a high-precision map using the optimal output point cloud data.

[0104] During the robot's motion, it detects in real time whether the currently observed feature points match the previously observed feature points, that is, whether there is a loop. By comparing the similarity of feature descriptors, matching feature point pairs are found to detect loops. When a loop is detected, a pose graph is constructed, the detected loop constraints are added to the pose graph, and then the pose graph is optimized. Pose graph optimization adjusts the robot's historical poses, eliminates drift caused by accumulated errors, and improves the consistency and accuracy of the map. At the same time, the map can be dynamically updated based on the newly acquired laser point cloud and visual data, keeping the system updated with the latest environmental information, and achieving accurate self-positioning while continuously building high-precision maps.

[0105] A positioning and map construction method for laser radar and vision fusion in a dynamic environment in an embodiment of the present invention is based on laser radar and vision fusion technology to locate and map the dynamic environment. The mapping result is less affected by the environment and has high accuracy, so that there is also a reliable composition method for dynamic environments.

[0106] By using lidar and visual cameras to collect environmental data, and using a series of algorithms to achieve the fusion of laser point cloud and visual information, the PTP protocol is used to synchronize the sensor clock, so that lidar, visual camera and other sensors share the time base at the hardware level through trigger signals and no longer rely on hardware support; at the same time, Kalman filtering is used at the software level to perform time correction and alignment to achieve high-precision time synchronization, reduce the accumulation of information errors, improve the accuracy of data collection in dynamic environments, and thus improve the accuracy of composition.

[0107] When fusion of data is performed, two different types of data feature values ​​are fused by means of tight coupling, and the fused data is optimized on the back end to further filter the information required for building a high-precision map; at the same time, by adding a loop detection mechanism, data information is updated in real time to achieve the construction of a high-precision map and accurate self-positioning. This method has a compact structure design, can effectively make full use of data information, and can achieve positioning and map construction in complex environments.

[0108] In an embodiment of the present invention, a positioning and map building device for laser radar and visual fusion in a dynamic environment is also provided, which is used to implement the above-mentioned embodiments and preferred implementation modes, and will not be repeated hereafter. As used below, the term "module" can be a combination of software and / or hardware that implements a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0109] The embodiment of the present invention provides a positioning and map building device that integrates laser radar and vision in a dynamic environment. Figure 7 As shown, including:

[0110] The data acquisition module 701 is used to obtain laser point cloud data of the surrounding environment, two-dimensional image data with depth information, and the robot's own posture information through laser radar, depth vision camera, and inertial measurement module respectively;

[0111] The data preprocessing module 702 is used to perform time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain preprocessed laser point cloud data and preprocessed two-dimensional image data;

[0112] The data fusion module 703 is used to extract feature values ​​of the pre-processed laser point cloud data and the pre-processed two-dimensional image data, match and fuse the extracted feature values ​​to obtain fused feature point cloud data containing both image feature information, optimize the feature point cloud data and posture information, and obtain fused data containing feature point cloud data and positioning information;

[0113] The positioning and mapping module 704 is used to perform back-end optimization and loop detection on the fused data to achieve map construction and self-positioning.

[0114] The laser radar and vision fusion positioning and map building device in a dynamic environment provided by the present invention respectively obtains laser point cloud data of the surrounding environment, two-dimensional image data with depth information and the posture information of the robot itself through the laser radar, the depth vision camera and the inertial measurement module, performs time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain pre-processed laser point cloud data and pre-processed two-dimensional image data, extracts characteristic values ​​of the pre-processed laser point cloud data and the pre-processed two-dimensional image data, matches and fuses the extracted characteristic values ​​to obtain fused characteristic point cloud data containing image characteristic information at the same time, optimizes the characteristic point cloud data and the posture information to obtain fused data containing characteristic point cloud data and positioning information, performs back-end optimization and loop detection on the fused data to realize map building and self-positioning, and can effectively improve the accuracy of positioning and mapping in a dynamic environment. The environmental information obtained by the method is accurate and the map building quality is high. Currently, it can be applied to the navigation and positioning technology fields of robots in dynamic environments.

[0115] In some optional implementations, the data preprocessing module 702 includes:

[0116] A time synchronization module is used to obtain, using the precise clock synchronization protocol PTP, a first hardware timestamp of a synchronization message sent by a master clock, a second hardware timestamp of a synchronization message received by a slave clock, a third hardware timestamp of a delay request message sent by a slave clock, and a fourth hardware timestamp of a delay request message received by a master clock; based on the first hardware timestamp, the second hardware timestamp, the third hardware timestamp, and the fourth hardware timestamp, the synchronization error and synchronization delay of the master clock and the slave clock are calculated, wherein the master clock is the clock of a processing unit that receives laser point cloud data and two-dimensional image data, and the slave clock is the clock of a laser radar or a depth vision camera; based on the synchronization error and synchronization delay, preliminary time synchronization is performed on the laser point cloud data and the two-dimensional image data; a Kalman filter algorithm is used to fuse multipath delay data, the time offset is modeled as a dynamic system, and a Kalman filter is used to estimate the time offset and drift of the clocks that collect laser radars and depth vision cameras; based on the time offset and drift, the time of the laser point cloud data and the two-dimensional image data after preliminary time synchronization is corrected.

[0117] In some optional implementations, the data preprocessing module 702 includes:

[0118] The coordinate alignment module is used to obtain the rotation matrix and translation vector of the laser radar and depth vision camera; obtain the intrinsic parameter matrix of the depth vision camera; project the laser point cloud data from the point cloud coordinate system to the pixel coordinate system according to the intrinsic parameter matrix, rotation matrix and translation vector to achieve coordinate alignment.

[0119] In some optional implementations, the data preprocessing module 702 includes:

[0120] The data denoising processing module is used to determine the threshold distance and target distance of the laser point cloud data through the Manhattan distance formula, and remove noise points based on the threshold distance and target distance; use adaptive histogram equalization to perform image enhancement processing on the two-dimensional image data, eliminate the noise of the two-dimensional image data through Gaussian filtering, and use a deep learning model to perform semantic segmentation on the two-dimensional image data to extract the semantic label of each pixel.

[0121] In some optional embodiments, the feature values ​​of the preprocessed laser point cloud data include point cloud features, and the feature values ​​of the preprocessed two-dimensional image data include color features and semantic features;

[0122] Correspondingly, the data fusion module 703 includes:

[0123] A pixel association module, used for associating the three-dimensional points in the preprocessed laser point cloud data with the RGB pixels in the preprocessed two-dimensional image data based on the coordinate alignment result;

[0124] A color point cloud generation module is used to obtain the color feature of each 3D point based on the coordinates of the associated RGB pixels, add the color feature to each 3D point of the laser point cloud data, and generate a color point cloud;

[0125] The feature fusion module is used to obtain the semantic features of each 3D point based on the coordinates of the associated RGB pixels, and fuse the point cloud features with the semantic features to obtain fused feature point cloud data.

[0126] In some optional implementations, the data fusion module 703 further includes:

[0127] The dynamic point cloud removal module is used to obtain the dynamic point cloud in the fused feature point cloud data based on semantic features and remove the dynamic point cloud.

[0128] In some optional implementations, the positioning and mapping module 704 includes:

[0129] A factor graph construction module is used to construct the observation results of the fused data into a factor graph;

[0130] An optimization module is used to optimize the factor graph using a nonlinear optimization algorithm to obtain map feature information and positioning information;

[0131] The loop detection module is used to perform loop detection based on map feature information and positioning information, eliminate drift errors, and realize map construction and self-positioning.

[0132] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0133] The positioning and mapping device for laser radar and vision fusion in a dynamic environment in an embodiment of the present invention is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.

[0134] An embodiment of the present invention also provides a computer device having the positioning and map building device of the above-mentioned embodiment that integrates laser radar and vision in a dynamic environment.

[0135] See also Figure 8 , Figure 8 is a schematic diagram of the structure of a computer device provided by an optional embodiment of the present invention, such as Figure 8 As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components are connected to each other using different buses for communication, and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in or on the memory to display the graphical information of the GUI on an external input / output device (such as, a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Figure 8 A processor 10 is taken as an example.

[0136] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

[0137] The memory 20 stores instructions executable by at least one processor 10, so that at least one processor 10 executes the method shown in the above embodiment.

[0138] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created according to the use of the computer device, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely arranged relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0139] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid state drive; the memory 20 may also include a combination of the above types of memory.

[0140] The computer device also includes an input device 30 and an output device 40. The processor 10, the memory 20, the input device 30 and the output device 40 may be connected via a bus or other means. Figure 8 The example of connecting through bus is taken in the following.

[0141] The input device 30 can receive input digital or character information, and generate key signal input related to the user settings and function control of the computer device, such as a touch screen, a keypad, a mouse, a track pad, a touch pad, an indicator bar, one or more mouse buttons, a trackball, a joystick, etc. The output device 40 may include a display device, an auxiliary lighting device (e.g., an LED) and a tactile feedback device (e.g., a vibration motor), etc. The above-mentioned display device includes but is not limited to a liquid crystal display, a light emitting diode, a display and a plasma display. In some optional embodiments, the display device can be a touch screen.

[0142] The embodiment of the present invention also provides a computer-readable storage medium. The method according to the embodiment of the present invention can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0143] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A positioning and map construction method for laser radar and vision fusion in a dynamic environment, characterized in that: include: The laser point cloud data of the surrounding environment, the two-dimensional image data with depth information and the robot's own posture information are obtained through the laser radar, depth vision camera and inertial measurement module respectively; Performing time synchronization, coordinate alignment and data denoising processing on the laser point cloud data and the two-dimensional image data to obtain the pre-processed laser point cloud data and the pre-processed two-dimensional image data; Extracting feature values ​​of the preprocessed laser point cloud data and the preprocessed two-dimensional image data, matching and fusing the extracted feature values ​​to obtain fused feature point cloud data containing both image feature information, optimizing the feature point cloud data and the posture information to obtain fused data containing feature point cloud data and positioning information; The fused data is subjected to back-end optimization and loop closure detection to achieve map construction and self-positioning.

2. The positioning and mapping method of laser radar and vision fusion in a dynamic environment according to claim 1 is characterized in that: The time synchronization process includes: Using the precise clock synchronization protocol PTP, obtain a first hardware timestamp of the master clock sending a synchronization message, a second hardware timestamp of the slave clock receiving the synchronization message, a third hardware timestamp of the slave clock sending a delay request message, and a fourth hardware timestamp of the master clock receiving the delay request message; Calculate the synchronization error and synchronization delay of the master clock and the slave clock based on the first hardware timestamp, the second hardware timestamp, the third hardware timestamp and the fourth hardware timestamp, wherein the master clock is the clock of the processing unit receiving the laser point cloud data and the two-dimensional image data, and the slave clock is the clock of the laser radar or the depth vision camera; Performing preliminary time synchronization on the laser point cloud data and the two-dimensional image data based on the synchronization error and the synchronization delay; The Kalman filter algorithm is used to fuse the multipath delay data, the time offset is modeled as a dynamic system, and the Kalman filter is used to estimate the time offset and drift of the clocks of the laser radar and the depth vision camera; The time of the laser point cloud data and the two-dimensional image data after preliminary time synchronization is corrected based on the time offset and the drift amount.

3. The positioning and mapping method of laser radar and vision fusion in a dynamic environment according to claim 1, characterized in that: The coordinate alignment process includes: Obtain the rotation matrix and translation vector of the laser radar and the depth vision camera; Obtaining an intrinsic parameter matrix of the depth vision camera; The laser point cloud data is projected from a point cloud coordinate system to a pixel coordinate system according to the intrinsic parameter matrix, the rotation matrix and the translation vector to achieve coordinate alignment.

4. The positioning and mapping method of laser radar and vision fusion in a dynamic environment according to claim 1, characterized in that: The data denoising process includes: Determine the threshold distance and target distance of the laser point cloud data by using the Manhattan distance formula, and remove noise points based on the threshold distance and the target distance; Adaptive histogram equalization is used to perform image enhancement processing on the two-dimensional image data, Gaussian filtering is used to eliminate noise from the two-dimensional image data, and a deep learning model is used to perform semantic segmentation on the two-dimensional image data to extract a semantic label for each pixel.

5. The positioning and mapping method of laser radar and vision fusion in a dynamic environment according to claim 1, characterized in that: The feature values ​​of the preprocessed laser point cloud data include point cloud features, and the feature values ​​of the preprocessed two-dimensional image data include color features and semantic features; Correspondingly, extracting the feature values ​​of the preprocessed laser point cloud data and the preprocessed two-dimensional image data, and matching and fusing the extracted feature values ​​to obtain fused feature point cloud data containing image feature information, includes: Associating the three-dimensional points in the preprocessed laser point cloud data with the RGB pixels in the preprocessed two-dimensional image data based on the coordinate alignment result; Acquire a color feature of each three-dimensional point based on the coordinates of the associated RGB pixels, and add the color feature to each three-dimensional point of the laser point cloud data to generate a color point cloud; The semantic features of each three-dimensional point are obtained based on the coordinates of the associated RGB pixels, and the point cloud features and the semantic features are fused to obtain fused feature point cloud data.

6. The positioning and mapping method of laser radar and vision fusion in a dynamic environment according to claim 5, characterized in that: After acquiring the semantic features of each three-dimensional point based on the coordinates of the associated RGB pixels and fusing the point cloud features with the semantic features to obtain fused feature point cloud data, the method further includes: A dynamic point cloud in the fused feature point cloud data is acquired based on the semantic feature, and the dynamic point cloud is eliminated.

7. The positioning and mapping method of laser radar and vision fusion in a dynamic environment according to claim 1, characterized in that: The fusion data is subjected to back-end optimization and loop detection to realize map construction and self-positioning, including: constructing the observation results of the fused data into a factor graph; The factor graph is optimized using a nonlinear optimization algorithm to obtain map feature information and positioning information; Based on the map feature information and the positioning information, loop detection is performed to eliminate drift errors and achieve map construction and self-positioning.

8. A positioning and map building device for laser radar and vision fusion in a dynamic environment, characterized in that: include: The data acquisition module is used to obtain laser point cloud data of the surrounding environment, two-dimensional image data with depth information, and the robot's own posture information through laser radar, depth vision camera, and inertial measurement module respectively; A data preprocessing module, used for performing time synchronization, coordinate alignment and data denoising on the laser point cloud data and the two-dimensional image data to obtain the preprocessed laser point cloud data and the preprocessed two-dimensional image data; A data fusion module is used to extract feature values ​​of the pre-processed laser point cloud data and the pre-processed two-dimensional image data, match and fuse the extracted feature values ​​to obtain fused feature point cloud data containing both image feature information, optimize the feature point cloud data and the posture information, and obtain fused data containing feature point cloud data and positioning information; The positioning and mapping module is used to perform back-end optimization and loop detection on the fused data to achieve map construction and self-positioning.

9. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the positioning and mapping method of laser radar and vision fusion in a dynamic environment according to any one of claims 1 to 7 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the positioning and map construction method of laser radar and vision fusion in a dynamic environment in any one of claims 1 to 7.

Images