Multi-Sensor Based Real-Time Robot Localization and Color Map Fusion Mapping Method

Abstract

The present invention proposes a method for real-time localization and color map fusion mapping of a robot based on multi-sensors, which quickly collects surrounding environment information and constructs a color global map in real time. The vision, laser, and inertial sensor acquisition modules read the information of the sensors, accumulate the laser point cloud data to form a point cloud scan frame, and then perform preprocessing to obtain the extracted feature point cloud. At the same time, the data collected by different sensors are time synchronized. The collected laser and inertial data are used to perform real-time self-localization of the robot, and a single-frame point cloud map is constructed at the same time. Using the RGB information collected by the vision sensor, texture and color rendering are performed on the constructed global point cloud map to construct a single-frame color map.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular, to a method for real-time self-localization and color dense map fusion mapping of a robot based on multi-sensors. Background Art

[0002] With the development of technology in recent years, the application of mobile robots in the industrial field and the civilian field has been continuously deepened, and the positioning and navigation capabilities are the key and foundation of mobile robots.

[0003] Currently, the mainstream simultaneous localization and mapping technologies mainly include lidar-based methods, camera-based methods, and multi-sensor fusion methods. Among them, the multi-sensor fusion methods mostly directly couple and optimize the results of lidar odometry and visual odometry to obtain a relatively accurate pose, and then output an unrendered point cloud map or semantic map after pose correction. The advantage of this is that it can adapt to a more diverse environment and avoid sensor degradation. The defect is that the computational cost is relatively high, and only the feature point part of the visual information is utilized, without achieving full utilization. Summary of the Invention

[0004] The present invention proposes a method for real-time localization and color map fusion mapping of a robot based on multi-sensors, which can generate self-localization information based on the movement of the robot and a real-time color global map, and can be uploaded to the server side through the network and remotely called for viewing, getting rid of the visual limitations of the operator and comprehensively mastering the change information of the entire environment.

[0005] To solve the above technical problems, the technical solution adopted by the present invention is:

[0006] A method for real-time localization and color map fusion mapping of a robot based on multi-sensors, comprising the following steps:

[0007] Step 1: Read the data of the visual sensor, lidar sensor, and inertial sensor, synchronize the images, lidar point clouds, and imu pose data from different sources to obtain the data to be fused under a unified time system, and use the first 20 frames of data collected by the inertial sensor for initialization to establish an initial pose estimate.

[0008] Step 2: Due to the point-by-point scanning characteristic of the lidar, each point in the point cloud data has its own independent acquisition time. A single frame of lidar point cloud often contains tens of thousands of points, that is, tens of thousands of consecutive different timestamps. There is a non-negligible time error between the first point and the last point collected in the same frame. For a moving robot, this time error will inevitably lead to inevitable spatial error. To eliminate this error, we use an imu sensor whose acquisition frequency is much higher than that of the lidar sensor, and perform forward propagation on the inertial data collected by the imu sensor to obtain the prior estimation of the robot pose at each moment. After obtaining the prior estimation of the robot pose between each moment, by interpolating the moments, calculate the relative motion of the robot between the acquisition moment of each lidar point and the end moment of the lidar point cloud frame acquisition. Transfer the lidar point from the robot coordinate system at the acquisition moment to the coordinate system at the end moment of the lidar point cloud frame acquisition. After performing the above operations on each lidar point in the current frame, obtain the point cloud at the end moment of the lidar point cloud frame acquisition after eliminating the inter-frame motion distortion;

[0009] Step 3: Since the point cloud at the end moment of the lidar point cloud frame acquisition is obtained by directly using the imu information to eliminate the motion distortion of the downsampled point cloud data, it will inevitably be affected by the acquisition data errors of the lidar sensor and the inertial imu sensor. The errors accumulate over time, ultimately leading to tracking loss, and the accuracy of both positioning and mapping cannot be guaranteed at all. To eliminate the influence of sensor errors and obtain a more accurate pose estimation, we use an error-state iterative Kalman filter to compare the information collected between different frames. During the movement of the robot, a landmark often exists in many consecutive frames simultaneously, and its relative position to the robot is constantly changing. For each point in the newly collected lidar point cloud frame, extract the five points closest to the current position in the map and fit a small plane. Through iterative optimization, continuously reduce the distance between the lidar point and its corresponding fitted plane until it is less than the threshold, so as to obtain the optimal estimation of the robot movement at the end moment of the lidar point cloud frame acquisition; Using the calculated optimal estimation, transfer the image collected by the visual sensor at the end moment of the lidar point cloud frame to the normalized coordinate system and finally transfer it to the radar coordinate system. For each point in the current frame, use the pre-calibrated rotation and translation relationship between the lidar sensor and the visual sensor to transfer from the three-dimensional space coordinates to the pixel coordinate system, and assign RGB information to each lidar point within the field of view of the visual sensor to obtain the colored point cloud after RGB rendering; To solve the problem of different data acquisition frequencies of the lidar and the camera, we use time-based frame interpolation processing. Use the two frames of pictures before and after the acquisition moment of the lidar sensor, and use the dense optical flow method to estimate the visual data at the acquisition moment of the lidar sensor to achieve the time synchronization of the lidar sensor and the visual sensor;

[0010] Step 4: Through the optimal estimation of the robot's motion at the end moment of point cloud frame acquisition for the RGB-rendered colored point cloud, transfer the point cloud from the robot's motion coordinate system at the end moment of point cloud frame acquisition to an independent world coordinate system, thereby constructing the colored point cloud map of the current frame; in order to facilitate the nearest neighbor search for points in the map, organize the points in the map in the form of a K-dimensional tree, so that each newly acquired point can find its nearest neighbor in the map in the shortest time; through time accumulation, accumulate the multi-frame information collected by the robot, obtain the optimal estimation of the robot's motion between consecutive frames and the colored point cloud maps of each frame, realize the self-positioning of the robot, and establish a global colored map.

[0011] A further improvement of the technical solution of the present invention lies in: The specific steps of step 2 are as follows:

[0012] Step 2.1: Establish an imu kinematic model. The two most important physical quantities in the description of the robot's motion are the pose at the current moment and the displacement relative to the origin of the coordinate system. However, the pose and displacement cannot be directly measured by sensors. We choose to use the angular velocity and acceleration data measured by the inertial imu sensor to integrate over the time dimension to obtain the required pose and displacement states. Therefore, establish a state expression equation in the world coordinate system:

[0013]

[0014] There are a total of 6 state variables in the established state expression equation in the world coordinate system, namely the pose R of the robot in the world coordinate system I , that is, the rotation amount of the robot relative to the initial state, represented by a 3*3 rotation matrix; the displacement amount p of the robot relative to the origin in the world coordinate system I , represented by a 3*1 translation vector; the instantaneous velocities v of the robot on the x, y, and z axes I , represented by a 3*1 velocity vector; the deviations b ω and b a of the data collected by the robot's inertial imu sensor in angular velocity and acceleration; the gravity vector g of the robot's world coordinate system relative to the real world coordinate system;

[0015] Step 2.2: For the established state equation, pre-integrate the imu data to establish a continuous imu kinematic model; among them, the first-order differential component of the pose of the robot in the world coordinate system is equal to the anti-symmetric matrix of the angular velocity vector after noise elimination multiplied by the rotation matrix of the robot relative to the initial state; the first-order differential component of the displacement of the robot in the world coordinate system is equal to the instantaneous velocity of the robot on the x, y, and z axes; the first-order differential component of the velocity of the robot is equal to the rotation matrix of the robot relative to the initial state multiplied by the acceleration vector after noise elimination plus the gravity vector in the world coordinate system of the robot; the deviations b ω and b a of the data collected by the robot inertial imu sensor in angular velocity and acceleration are a random walk process with Gaussian noise n bω and n ba ; the first-order differential component of the gravity vector g in the world coordinate system of the robot is 0, and the imu kinematic equation of the robot can be obtained as follows:

[0016]

[0017]

[0018]

[0019]

[0020]

[0021]

[0022] Discretizing the above equations gives

[0023]

[0024] Step 2.3: In Step 1, initialization is performed to obtain the initial estimate of the robot's motion state. When the robot receives the input from the imu sensor, it can perform forward propagation according to the discrete kinematic model established in Step 2.2, and iteratively obtain the prior estimate of the robot's pose at each frame time through the initial estimate and the acceleration and angular velocity data collected by the imu sensor in each frame; the specific iterative equation is as follows

[0025] x i+1 = x i + (Δtf(x i , u i , w i ))

[0026]

[0027] where the control quantity ui The acceleration data a collected by the IMU sensor m and the angular velocity data ω m , and the error quantity w is the cumulative error n of the IMU sensor ω 、n a and the error n of the data collected by the IMU sensor bω 、n ba ;

[0028] Since the working frequency of the IMU sensor is much higher than that of the lidar sensor, the motion state of the robot will continuously propagate forward during two frames of the lidar sensor until the lidar sensor collects a complete frame.

[0029] Step 2.4: After the lidar sensor collects a complete frame of point cloud, interpolation calculation is performed based on the motion states of the robot at each moment obtained by forward propagation of the IMU sensor data to obtain the motion state of the robot corresponding to each laser point acquisition moment, and calculate the relative motion state of the robot from each laser point acquisition moment to the end moment of the point cloud frame acquisition

[0030] where j is the acquisition moment of each radar point and k is the acquisition moment of the point cloud frame; since the motion state of the robot obtained by forward propagation is based on the IMU sensor coordinate system, we should first transfer the laser point collected at moment j from the lidar coordinate system at moment j to the IMU sensor coordinate system, then use the relative motion of the robot from moment j to moment k to transfer the laser point from the IMU sensor coordinate system at moment j to the IMU sensor coordinate system at moment k, and finally transfer the laser point from the IMU sensor coordinate system at moment k to the lidar coordinate system. The coordinate transformation process of the laser point is shown in the following formula: where

[0031]

[0032] where I T L is the rotation and translation relationship between the lidar coordinate system and the IMU sensor coordinate system. Since there is a fixed physical connection relationship between the lidar sensor and the IMU sensor, so I T L is a fixed quantity and is obtained through pre-calibration.

[0033] A further improvement of the technical solution of the present invention lies in: The specific steps of step 3 are as follows:

[0034] Step 3.1: The point cloud at the end moment k of the point cloud frame acquisition obtained in step 2.4 It is calculated from the robot's motion state directly integrated with the information of the IMU sensor after the point cloud collected by the lidar is preprocessed and downsampled. Therefore, it is affected by the double errors of both the lidar and the IMU sensor, and due to the forward propagation mechanism, the error of the robot's motion state will accumulate continuously. Therefore, we construct an error-state iterative Kalman filter to optimize the robot's motion state and reduce the error caused by the sensor; the point cloud at the end time k of the point cloud frame acquisition is transformed into the IMU sensor coordinate system, and then using the prior estimate of the robot's motion at time k obtained by forward propagation, the estimated global coordinate system coordinate point of the laser point j at the end time k of the lidar point cloud frame acquisition can be obtained

[0035]

[0036] For each lidar point, the nearest plane or edge should be defined by the nearby points on its map. Thus, the residual is defined as the distance between the estimated global coordinate system coordinate point of the laser point j at the end time k of the lidar point cloud frame acquisition and the nearest plane or edge on the map:

[0037]

[0038] where G j is equal to the normal vector of the small plane fitted by the 5 points on the map closest to the laser point j G q j is the projection point of the estimated global coordinate system coordinate point on the fitted plane.

[0039] Step 3.2: When the sensor error is 0, the prior estimate of the robot state obtained by the forward propagation of the IMU data is the actual motion state of the robot. At this time, the estimated global coordinate system coordinate point is the point on the actual map. Therefore and G q j are on the same fitted plane, and the residual is equal to 0. However, in actual calculation, we can find that due to the error influence, at any time is not equal to 0. Use an error iterative Kalman filter to iteratively optimize so as to obtain the estimate of the robot's motion state when the residual is less than the threshold as the best estimate; according to the robot motion state iterative equation constructed in Step 2.3, take the true value of the motion state as x i , and establish the dynamic model of the error state as follows

[0040]

[0041] The covariance of the white noise w is recorded as Q, then the covariance of the forward propagation is It can be calculated iteratively according to the following formula

[0042]

[0043] Assuming laser point measurement value The corresponding true value is The error between the true value and the observed value is the original measurement noise Then there is Will Substituting into the residual calculation equation, we can get

[0044]

[0045] exist Performing a first-order Taylor expansion at , we can get a first-order approximation of the residual equation

[0046]

[0047] The residual equation of the laser point corresponding to the true value is expressed as the Jacobian matrix of the observed value residual equation and the true value residual equation and v j The form of the sum, where v j Represents the original measurement noise The resulting observation error is the amount of interference to be eliminated through iterative optimization;

[0048] The obtained observation error equation is combined with the established state error equation to obtain the maximum a posteriori estimate of the observation error that needs to be optimized The maximum a posteriori estimate of the observation error is optimized by the Kalman filter using the least squares method, and the result is converged and less than the threshold. The corresponding motion state estimation value at this time The optimal state estimate

[0049] Step 3.3: Estimate the optimal state obtained in step 3.2 Assuming that this is the true value of the robot's motion state at the end of this point cloud frame acquisition time k, the state transformation matrix of the robot relative to the world coordinate system at time k can be obtained: Including rotation matrix and translation vectors Pixels collected by the camera The timestamp information is used to calculate the relative motion state of the robot between the camera image acquisition time q and the laser radar point cloud frame acquisition end time k, and the state transition matrix between the corresponding camera image acquisition time q and the laser radar point cloud frame acquisition end time k is obtained. Transfer the laser points in the lidar coordinate system at time k to the lidar coordinate system at time q. The formula is as follows

[0050]

[0051] to obtain the point cloud of laser points at time q After that, using the coordinate conversion relationship between the pre-calibrated lidar sensor and the vision camera sensor C T I and the camera internal parameter I, transfer the laser points from the xyz coordinate system of the lidar sensor to the normalized coordinate system of the camera, and finally to the pixel coordinate system of the camera. The formula is as follows:

[0052]

[0053]

[0054]

[0055] Since the acquisition frequencies of the camera and the lidar are different, there is no exactly corresponding camera frame at the end of the lidar scan. Therefore, it is impossible to perform precise RGB rendering on the lidar point cloud. To obtain the camera data at the end of the lidar scan, we use the dense optical flow method to obtain the camera data at the end of the lidar scan. Since the acquisition interval time between two adjacent frames is short, it can be assumed that the gray level of the same object in the two frames remains unchanged. By the displacement of each optical flow field between the front and back frames, the camera data at the lidar time is estimated. The formula is as follows

[0056]

[0057] Filter according to the pixel coordinates corresponding to the laser points. The laser points whose pixel coordinates exceed the image size are the points that fall outside the camera's field of view blind area. These laser points are filtered out, and the RGB values corresponding to the remaining laser points are calculated by bilinear interpolation for the pixel points collected by the vision camera. The formula is as follows

[0058]

[0059] Take the RGB values calculated by interpolation as the attributes of the lidar point cloud and save it as a colored lidar point cloud

[0060] A further improvement of the technical solution of the present invention is that: step 4 specifically includes the following steps:

[0061] Step 4.1: For the lidar point cloud with RGB attributes obtained Then, it is successively converted back to the radar coordinate system at time k to obtain the point cloud. As the state is updated, the state transformation matrix of the robot's inertial coordinate system relative to the global frame world coordinate system at time k can be obtained. Meanwhile, it is known that the pre-calibrated I T L is the rotation and translation relationship between the lidar coordinate system and the imu sensor coordinate system. By transferring the point cloud from the radar coordinate system at time k to the world coordinate system under the global frame, the colored point cloud for mapping can be obtained.

[0062] Step 4.2: Organize the map in the form of a k-d tree. The k-d tree is a variant of the binary tree used to store points in a space of dimension k. Our lidar points are segmented according to the three dimensions of x, y, and z to construct a k-d tree with k = 3.

[0063] After the first frame of colored point cloud is stored in the map, the x coordinate values of each point in this frame are traversed and sorted. The median point is used as the root node. All points with x coordinate values less than the root node are placed in the left subtree, and all points with x coordinate values greater than the root node are placed in the right subtree. After completing the first layer of segmentation, the y coordinate values of the points in the left and right subtrees are traversed and sorted separately. The median points are used as the root nodes of the second layer respectively. All points with y coordinate values less than the corresponding root node are placed in the left subtree of the corresponding root node, and all points with y coordinate values greater than the corresponding root node are placed in the right subtree of the corresponding root node. After completing the second layer of segmentation, the z coordinate values of the four subtrees are traversed and sorted respectively, and the cycle continues until each subtree of the last layer contains one point, that is, the initialization construction of the k-d tree is completed.

[0064] Starting from the second frame, whenever a new point cloud is stored in the map, each point in the point cloud recursively searches downward from the root node, and the coordinate on the division axis of the new point is compared with the points stored on the tree nodes until a leaf node is found and then a new tree node is appended. After all the points in a frame are updated, it is judged upward from the leaf node whether the subtree is unbalanced, that is, whether the number of nodes in one side subtree is much more than the other side. If imbalance is detected, the corresponding subtree is rebuilt.

[0065] Step 4.3: As the robot moves, the optimal estimation of the robot's motion state among multiple frames is visually displayed to obtain the real-time positioning effect during the robot's movement. The colored point clouds for mapping in multiple frames are accumulated in the same map to obtain the corresponding map. When the robot completes traversing the current environment, a global colored map can be established.

[0066] Due to the adoption of the above technical solution, the technical progress achieved by the present invention is as follows:

[0067] The present invention proposes a method for real-time positioning and color map fusion mapping of a robot based on multi-sensors. By reading the data of the robot sensor group, the self-positioning of the robot is carried out in real time, and at the same time, a global color map is constructed.

[0068] Compared with other methods, the present invention proposes a fast laser-vision data fusion method, which can render texture and color for the point cloud frames collected by the lidar at a very small time cost. By using the coupling of laser information and vision information, the error caused by a single sensor is avoided, and a global color map is constructed. At the same time, due to the relatively complex industrial field environment, it is difficult for laser sensors and vision sensors to extract the same feature point set. This algorithm directly uses the complete downsampled point cloud frames for map construction and uses the pre-calibrated external sensor parameters for data matching. Brief Description of the Drawings

[0069] Figure 1 is the overall flowchart of the algorithm;

[0070] Figure 2 is the data diagram of the laser sensor;

[0071] Figure 3 is the data diagram of the vision sensor;

[0072] Figure 4 is the feature matching algorithm diagram;

[0073] Figure 5 is the data fusion algorithm diagram;

[0074] Figure 6 is the rendering diagram of the global map. Detailed Description of the Preferred Embodiment

[0075] The following will describe in detail the specific implementation manners of the present invention with reference to the accompanying drawings.

[0076] The overall system flowchart of the present invention is as shown in Figure 1 Step 1: Read the sensor data. The sensors used are the OAK-D Lite camera, the Livox Horizon solid-state lidar, and the BMI088 inertial sensor.

[0077] Step 2: Preprocess the lidar point cloud data. Accumulate the received point cloud data according to the scanning time to form point cloud frames, and perform voxel-based downsampling to reduce the computational amount.

[0078] Step 3: Initialize using the read inertial sensor data, construct a motion model of the inertial sensor data, and perform forward propagation on the point cloud data to obtain time-aligned point cloud frames.

[0079] Step 4: For each point in the time-aligned point cloud frame obtained in step 3, backpropagate using the inertial sensor data to obtain a coordinate-aligned point cloud frame.

[0080] Step 5: Construct the residual and error iterative Kalman filter for the point cloud frame aligned with the coordinate system, continuously iteratively optimize the residual to obtain the optimal estimate of the inter-frame pose and the point cloud frame after pose optimization.

[0081] Step 6: Preprocess the camera data. First, synchronize the time coordinate system of the received camera data according to the internal time difference of each sensor. Then, use the dense optical flow method to perform frame interpolation processing for precise time synchronization.

[0082] Step 7: Synchronize the coordinate systems of the precisely time-synchronized camera data and the pose-optimized point cloud frames to construct fused camera and lidar data.

[0083] Step 8: Transfer the multi-frame fused color point cloud data into an independent global coordinate system to construct a global color map.

[0084] The step 1 specifically includes the following steps:

[0085] Step 1.1: Read the data from the BMI088 inertial sensor, including acceleration, angular velocity, and timestamps of each sampling moment. Since the lidar and inertial sensor are integrated together, the internal time of the sensor is in the same time system.

[0086] Step 1.2: Read the data from the Livox Horizon LiDAR, including the XYZ information and reflection intensity of the laser point, and the internal timestamp of each laser point acquisition time. The data collected by the LiDAR is visualized as follows: Figure 2 shown.

[0087] Step 1.3: Read the data from the OAK-D Lite camera, including the RGB value of each pixel in the current frame in the camera's field of view and the internal timestamp of the current frame acquisition time. The data collected by the camera is visualized as follows Figure 3 shown.

[0088] Described step 2 comprises the following specific steps:

[0089] Step 2..1: Accumulate the laser points collected by the lidar over time to form a laser point cloud frame.

[0090] Step 2.2: Perform a preliminary screening of the laser point cloud frame to remove points at the edge of the field of view, points with too low reflection intensity, and points in the blind area of the field of view.

[0091] Step 2.3: Divide the laser point cloud into cells at 0.1 cm intervals, sum the weighted points in each cell, and construct a sparse point cloud.

[0092] Step 3 specifically includes the following steps:

[0093] Step 3.1: Use the first 20 frames of sensor data for initialization to eliminate zero drift.

[0094] Step 3.2: Pre-integrate the IMU data to obtain the discrete kinematic model of the IMU.

[0095] Step 3.3: Integrate the IMU data obtained for each frame to obtain the initial estimates of the position and velocity at each moment.

[0096] Step 3.4: Continue the integration until a frame of point cloud is scanned, and use the estimated pose at this time as the initial pose estimate at the end of the laser point cloud frame scan.

[0097] Step 3.5: Directly transform each laser point in the current frame into the coordinate system at the end of the point cloud frame scan.

[0098] Step 4 specifically includes the following steps:

[0099] Step 4.1: For each laser point in the current frame, interpolate the corresponding IMU state at the acquisition moment according to the initial estimate obtained in Step 3.3.

[0100] Step 4.2: Perform backpropagation, subtract the displacement information in the motion states at the end of the point cloud frame and the acquisition moments of each laser point, and the relative displacement of each laser point can be obtained.

[0101] Step 4.3: Use the relative displacement to correct the motion of each laser point to obtain the positions of each laser point at the end of the optimized frame.

[0102] Step 5 includes the following steps:

[0103] Step 5.1: For each laser point, screen the 5 nearest points from the map and fit a small plane.

[0104] Step 5.2: Calculate the distance from the laser point to the fitted plane as the residual of the point.

[0105] [[ID=4·2]]Step 5.3: Based on the IMU motion model and the residual of the laser point, construct an error iterative Kalman filter.

[0106] Step 5.4: Iteratively optimize the motion state of the IMU to obtain the motion state when the residual is less than the threshold.

[0107] Step 5.5: Use the motion state obtained in Step 5.4 as the optimal pose estimation for the current frame.

[0108] Step 5.6: Use the optimal pose estimation to perform motion compensation on the point cloud frame to obtain the point cloud frame in the optimal motion state.

[0109] The said Step 6 includes the following steps:

[0110] Step 6.1: When receiving the first frame of lidar data and camera data, calculate the time difference between the internal clocks of the two sensors.

[0111] Step 6.2: Perform time compensation on the camera clock and convert it to the time system of the lidar.

[0112] Step 6.3: For the acquired point cloud frame, obtain two visual frames before and after the end moment of the lidar frame.

[0113] Step 6.4: For the two visual frames before and after, calculate the offsets of all points on the image to form a dense optical flow field.

[0114] Step 6.5: Estimate the motion of the image at the pixel level through the dense optical flow field.

[0115] Step 6.6: Use the motion estimation of the image to calculate the estimated visual frame at the end moment of the lidar frame.

[0116] The said Step 7 includes the following steps:

[0117] Step 7.1: Perform pre-calibration on the lidar and camera data.

[0118] Step 7.2: Extract the line feature information in the lidar frame and the visual frame.

[0119] Step 7.3: Make an initial estimate of the transformation matrix between the two sensor coordinate systems.

[0120] Step 7.4: Based on the initial estimate, perform matching of line features between different sensors.

[0121] Step 7.5: Set the deviation between the line features of different sensors as the residual.

[0122] Step 7.6: Iteratively optimize the transformation matrix to obtain the optimal estimate of the transformation matrix with a residual less than the threshold. The visualization result of the optimal estimate is as Figure 4 shown.

[0123] Step 7.7: Multiply the XYZ coordinates of the lidar points by the transformation matrix and transfer them to the camera coordinate system.

[0124] Step 7.8: Multiply the xy coordinates of the lidar points by the camera internal parameters to convert them into pixel coordinates.

[0125] Step 7.9: Eliminate the laser points outside the camera's field of view.

[0126] Step 7.10: For each laser point within the camera's field of view, obtain the RGB values of the four closest points to it.

[0127] Step 7.11: Perform a weighted sum of these four points to calculate the RGB value of the pixel coordinates corresponding to the laser point.

[0128] Step 7.12: Save the calculated RGB value as the attribute of the laser point.

[0129] Step 7.13: Transfer the laser points with the obtained RGB attributes to the lidar coordinate system to form a single-frame color point cloud map. The visualization result is as Figure 5 shown.

[0130] The said Step 8 includes the following steps:

[0131] Step 8.1: Transfer the single-frame color point cloud obtained in Step 7.12 to an independent global coordinate system, that is, publish it to the global map.

[0132] Step 8.2: Iterate Steps 3.3 - 8.1 until the acquisition process ends. The result is as Figure 6 shown.

Claims

1. A method for real-time localization and color map fusion mapping of a robot based on multi-sensors, characterized in that Including the following steps: Step 1: Read the data of the vision sensor, lidar sensor, and inertial sensor, synchronize the images, lidar point clouds, and imu pose data from different sources in terms of time to obtain the data to be fused under a unified time system, and use the first 20 frames of data collected by the inertial sensor for initialization to establish an initial pose estimate; Step 2: Based on the high working frequency of the imu sensor, the imu sensor is selected as the inertial sensor. Propagate the inertial data collected by the imu sensor forward to obtain the prior estimate of the robot pose at each moment. After obtaining the prior estimate of the robot pose between each moment, calculate the relative motion of the robot between the collection moment of each lidar point and the end moment of the lidar point cloud frame by interpolating the moments, and transfer the lidar points from the robot coordinate system at the collection moment to the coordinate system at the end moment of the lidar point cloud frame. After performing the above operations on each lidar point in the current frame, the point cloud at the end moment of the lidar point cloud frame with inter-frame motion distortion eliminated is obtained; Step 3: Use the error-state iterative Kalman filter to compare the information collected between different frames. During the movement of the robot, a landmark often exists in many consecutive frames at the same time, and its relative position to the robot is constantly changing. For each point in the newly collected lidar point cloud frame, extract the five points closest to the current position in the map and fit a small plane. Through iterative optimization, continuously reduce the distance between the lidar point and its corresponding fitted plane until it is less than the threshold, so as to obtain the optimal estimate of the robot movement at the end moment of the lidar point cloud frame. Using the calculated optimal estimate, transfer the image collected by the vision sensor at the end moment of the lidar point cloud frame to the normalized coordinate system and finally to the radar coordinate system. For each point in the current frame, use the pre-calibrated rotation and translation relationship between the lidar sensor and the vision sensor to transfer from the three-dimensional space coordinates to the pixel coordinate system, and assign RGB information to each lidar point within the field of view of the vision sensor to obtain the colored point cloud after RGB rendering. Use time-based frame interpolation processing, use the two frames of pictures before and after the collection moment of the lidar sensor, and use the dense optical flow method to estimate the vision data at the collection moment of the lidar sensor to achieve the time synchronization of the lidar sensor and the vision sensor; Step 4: Through the optimal estimate of the robot movement at the end moment of the lidar point cloud frame for the colored point cloud after RGB rendering, transfer the point cloud from the robot movement coordinate system at the end moment of the lidar point cloud frame to an independent world coordinate system, thereby constructing the colored point cloud map of the current frame. Organize the points in the map in the form of a K-dimensional tree, so that each newly collected point can find its nearest neighbor point in the map in the shortest time. Through time accumulation, accumulate the multi-frame information collected by the robot to obtain the optimal estimate of the robot movement between consecutive frames and the colored point cloud maps of each frame, realize the self-localization of the robot and establish a global colored map.

2. The method for real-time positioning and color map fusion mapping of a robot based on multi-sensors according to claim 1, characterized in that The specific steps of step 2 include the following steps: Step 2.1: Establish an IMU kinematic model. For the description of the robot's motion, the two most important physical quantities are the pose at the current moment and the displacement relative to the origin of the coordinate system. The pose and displacement states to be obtained are calculated by integrating the angular velocity and acceleration data measured by the IMU sensor over the time dimension. Therefore, the state expression equation in the world coordinate system is established as follows: There are a total of 6 state variables in the state equation for establishing the world coordinate system, namely the pose R of the robot in the world coordinate system I , that is, the rotation amount of the robot relative to the initial state, which is represented by a 3*3 rotation matrix; the displacement p of the robot relative to the origin in the world coordinate system I , which is represented by a 3*1 translation vector; the instantaneous velocity v of the robot on the x, y, and z axes I , which is represented by a 3*1 velocity vector; the deviations b of the data collected by the robot's inertial imu sensor in terms of angular velocity and acceleration ω and b a ; the gravity vector g of the robot's world coordinate system relative to the true world coordinate system; Step 2.2: For the established state equation, perform pre-integration on the imu data to establish a continuous imu kinematic model; among them, the first-order differential component of the pose of the robot in the world coordinate system is equal to the skew-symmetric matrix of the angular velocity vector after noise elimination multiplied by the rotation matrix of the robot relative to the initial state; the first-order differential component of the displacement of the robot in the world coordinate system is equal to the instantaneous velocity of the robot on the x, y, and z axes; the first-order differential component of the velocity of the robot is equal to the rotation matrix of the robot relative to the initial state multiplied by the acceleration vector after noise elimination plus the gravity vector in the robot world coordinate system; the deviations b ω and b a of the model for the data collected by the robot inertial imu sensor are a random walk process with Gaussian noise n bω and n ba ; the first-order differential component of the gravity vector g in the robot world coordinate system is 0, and thus the imu kinematic equation of the robot is as follows: Discretizing the above equations gives Step 2.3: In Step 1, initialization is performed to obtain the initial estimate of the robot's motion state. When the robot receives the input from the IMU sensor, it can perform forward propagation according to the discrete kinematic model established in Step 2.

2. Through the initial estimate and the acceleration and angular velocity data collected by the IMU sensor for each frame, the prior estimate of the robot's pose at each frame moment is iteratively obtained. The specific iterative equation is as follows x i+1 = x i + (Dtf(x i , u i , w i )) Among them, the control quantity u i is the acceleration data a m and the angular velocity data ω m collected by the imu sensor, and the error quantity w is the cumulative error n ω of the imu sensor a and the error n bω of the data collected by the imu sensor ba ; Step 2.4: After the lidar sensor has collected a complete frame of point cloud, interpolation calculations are performed based on the robot motion states at various moments obtained by forward propagation of the imu sensor data, to obtain the robot motion state corresponding to each laser point acquisition moment, and calculate the relative motion state of the robot from each laser point acquisition moment to the end of the point cloud frame acquisition moment where j is the acquisition moment of each radar point and k is the acquisition moment of the point cloud frame; the laser points acquired at moment j are first transferred from the lidar coordinate system at moment j to the imu sensor coordinate system, and then the relative motion of the robot from moment j to moment k is used to transfer the laser points from the imu sensor coordinate system at moment j to the imu sensor coordinate system at moment k. Finally, the laser points are transferred from the imu sensor coordinate system at moment k to the lidar coordinate system. The coordinate transformation process of the laser points is shown by the following formula: Among them I T L is the rotation and translation relationship between the lidar coordinate system and the imu sensor coordinate system. Since there is a fixed physical connection between the lidar sensor and the imu sensor, so I T L is a fixed quantity and is obtained through pre-calibration.

3. The method for real-time positioning and color map fusion mapping of a robot based on multi-sensors according to claim 2, wherein The specific steps of Step 3 are as follows: Step 3.1: Construct an error-state iterative Kalman filter to optimize the motion state of the robot and reduce the errors caused by sensors; the point cloud at the end time k of the point cloud frame acquisition is transformed into the imu sensor coordinate system. Then, using the prior estimate of the robot motion at time k obtained by forward propagation, the estimated global coordinate point of the laser point j at the end time k of the lidar point cloud frame acquisition can be obtained The nearest plane or edge should be defined by nearby points on its map, and the residual is defined as the estimated global coordinate point of the laser point j at the end time k of the lidar point cloud frame acquisition from the nearest plane or edge on the map: where G j is equal to the normal vector of the small plane fitted by the five points on the map that are closest to the laser point j G q j is the estimated coordinate point in the global coordinate system is the projection point on the fitted plane; Step 3.2: When the sensor error is 0, the prior estimate of the robot state obtained by the forward propagation of the imu data is the actual motion state of the robot. At this time, the coordinates of the point in the global coordinate system are estimated. That is the point on the actual map. Therefore, and G q j are in the same fitting plane, and the residual is equal to 0. Due to the influence of the error, at any moment is not equal to 0. An error iterative Kalman filter is used to iteratively optimize so as to obtain the estimate of the robot motion state when the residual is less than the threshold as the best estimate. According to the robot motion state iterative equation constructed in Step 2.3, the true value of the motion state is taken as x i , and the dynamic model of the error state is established as follows Denote the covariance of white noise \(w\) as \(Q\), then the covariance of the forward propagation can be iteratively calculated according to the following formula Assumed laser spot measurement value The corresponding true value is The error between the true value and the observed value, the original measurement noise is Then there is Substitute into the residual calculation equation, and we can get At perform a first-order Taylor expansion to obtain a first-order approximation of the residual equation Express the residual equation of the laser point corresponding to the true value as the sum of the Jacobian matrix of the observed value residual equation and the true value residual equation and v j where v j represents the observation error generated by the original measurement noise which is an interference quantity to be eliminated through iterative optimization; The obtained observation error equation is combined with the established state error equation to obtain the maximum a posteriori estimate of the observation error that needs to be optimized The maximum a posteriori estimate of the observation error is optimized by the Kalman filter using the least squares method, and the result is converged and less than the threshold. The corresponding motion state estimation value at this time The optimal state estimate Step 3.3: Take the optimal state estimation obtained in Step 3.2 Assume it is the true value of the robot's motion state at the end time k of this point cloud frame acquisition. The state transformation matrix of the robot relative to the world coordinate system at time k can be obtained including the rotation matrix and the translation vector Calculate the relative motion state of the robot between the camera image acquisition time q and the end time k of the lidar point cloud frame using the timestamp information of the pixel points collected by the camera, and obtain the state transformation matrix corresponding to the camera image acquisition time q and the end time k of the lidar point cloud frame Transfer the laser points in the lidar coordinate system at time k to the lidar coordinate system at time q. The formula is as follows Obtain the laser point cloud at time q After that, using the coordinate transformation relationship between the pre-calibrated lidar sensor and the vision camera sensor C T I and the camera internal parameter I, transfer the laser points from the xyz coordinate system of the lidar sensor to the normalized coordinate system of the camera, and finally to the pixel coordinate system of the camera. The formula is as follows: Use the dense optical flow method to obtain the camera data at the end of the lidar scan. Since the acquisition interval time between two adjacent frames is short, it can be assumed that the gray level of the same object in the two frames remains unchanged. The camera data at the lidar moment is estimated through the displacements of the optical flow fields in the front and back frames. The formula is as follows Filter according to the pixel coordinates corresponding to the laser points. The laser points whose pixel coordinates exceed the image size are the points that fall outside the camera's field of view blind area. These laser points are filtered out, and the RGB values corresponding to the pixel coordinates of the remaining laser points are calculated by the bilinear interpolation method for the pixel points collected by the visual camera. The formula is as follows Take the interpolated RGB values as the attributes of the laser point cloud and save them as a colored laser point cloud 4. The method for real-time positioning and color map fusion mapping of a robot based on multi-sensors according to claim 3, wherein The specific steps of Step 4 are as follows: Step 4.1: The laser point cloud with RGB attributes obtained is then successively transformed back into the radar coordinate system at time k to obtain the point cloud As the state is updated, the state transformation matrix of the robot inertial coordinate system relative to the global frame world coordinate system at time k can be obtained Meanwhile, it is known that the pre-calibrated I T L is the rotation and translation relationship between the lidar coordinate system and the imu sensor coordinate system. By transferring the point cloud from the radar coordinate system at time k to the world coordinate system under the global frame, the colored point cloud for mapping can be obtained Step 4.2: Organize the map in the form of a k-d tree. The k-d tree is a variant of the binary tree and is used to store points in a space of dimension k. Our lidar points are divided according to the three dimensions of x, y, and z to construct a k-d tree with k = 3; Color point cloud in the first frame After storing in the map, the x-coordinate values of each point in this frame are traversed and sorted, and the median point is used as the root node. All points with x-coordinate values less than the root node are placed in the left subtree, and all points with x-coordinate values greater than the root node are placed in the right subtree; after completing the first-level split, the y-coordinate values of the midpoints of the left and right subtrees are traversed and sorted respectively, and the median point is used as the root node of the second layer respectively. All points with y-coordinate values less than the corresponding root node are placed in the subtree to the left of the corresponding root node, and all points with y-coordinate values greater than the corresponding root node are placed in the subtree to the right of the corresponding root node; after completing the second-level split, the z-coordinate values of the four subtrees are traversed and sorted respectively, and the cycle is repeated until each subtree of the last layer contains a point, that is, the initialization construction of the k-dimensional tree is completed; Starting from the second frame, whenever new point clouds are stored in the map, each point in the point cloud recursively searches downward from the root node, and the coordinate of the new point on the division axis is compared with the points stored on the tree nodes until a leaf node is found and then a new tree node is appended; after all the points in a frame are updated, it is judged upward from the leaf node whether the subtree is unbalanced, that is, whether the number of nodes in one side subtree is much more than that in the other side. If unbalance is detected, the corresponding subtree is reconstructed; Step 4.3: As the robot moves, the optimal estimation of the robot's motion state among multiple frames is visually displayed to obtain the real-time positioning effect during the robot's motion. The multi-frame color point clouds used for mapping are accumulated in the same map to obtain the corresponding map. When the robot finishes traversing the current environment, a global color map can be established.

Images