A multi-modal autonomous navigation and positioning system

Abstract

The present invention discloses a multi-modal autonomous navigation and positioning system, specifically related to the field of driverless. It includes a data acquisition layer, a data processing layer, and an application layer. The data acquisition layer uses the SLAM method of lidar, camera, and IMU, and performs joint operations on the point cloud features of the lidar, IMU data, and the feature information extracted and optimized by the camera, improving the accuracy of the entire system in unstructured environments. It also performs key frame matching to reduce the computational load of the entire system, achieving a balance between accuracy and computational load, and ensuring the accuracy and real-time performance of the system. The data processing layer processes and integrates the data generated by the data acquisition layer, renders the texture of the map through minimum photometric error and feature matching, constructs the map geometric structure, and realizes precise positioning and mapping. The application layer uploads the pre-trained model to the autonomous decision-making system to realize the final autonomous navigation and positioning system.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation, and proposes a multi-modal autonomous navigation and positioning system for real-time synchronous positioning, mapping and coloring frameworks. Background Art

[0002] Currently, the mainstream solution for driverless in the industry is to combine GNSS with slam algorithms for positioning and navigation. In complex environments with weak GNSS signals such as tunnels, underground mines, strong light, and heavy fog, its absolute positioning fails, and relative positioning will produce huge errors, resulting in frequent disasters, reducing the safety of driverless, unable to meet actual needs, and restricting the development of the entire driverless industry. Summary of the Invention

[0003] In order to overcome the defects of the prior art, the present invention provides a multi-modal autonomous navigation and positioning system. To achieve the above object, the present invention provides the following technical solutions.

[0004] The invention fully considers that the stability of the slam algorithm in the traditional mode is poor, resulting in error accumulation. It solves the problems of real-time synchronous positioning, 3D mapping, and map rendering based on the tight-coupling fusion of LiDAR, inertial, and visual measurements. Based on the multi-sensor parallel technology integrating vision, lidar, and inertial measurement units, this patent proposes a real-time synchronous positioning, mapping, and coloring framework, constructs a multi-modal autonomous navigation and positioning system, avoids the problem of frequent accidents in the case of weak satellite signals or no satellite signals, and solves the problem of low accuracy in single-sensor data capture and processing.

[0005] The technical solution adopted by the present invention is: a real-time synchronous positioning, mapping, and coloring framework, a multi-modal autonomous navigation and positioning system, for use in complex environments with weak GNSS signals. The system includes a data acquisition layer, a data processing layer, and an application layer:

[0006] The data acquisition layer uses the SLAM method of lidar, camera, and IMU, and performs operations on the point cloud features of the lidar together with the IMU data and the feature information extracted and optimized by the camera, improving the accuracy of the entire system in unstructured environments. It also performs key frame matching to reduce the computational amount of the entire system, achieving a balance between accuracy and computational amount, and ensuring the accuracy and real-time performance of the system.

[0007] The data processing layer processes and integrates the data generated by the data acquisition layer, renders the texture of the map through minimum photometric error and feature matching, constructs the map geometric structure, and realizes accurate positioning and mapping.

[0008] The application layer uploads the pre-trained model to the autonomous decision-making system to realize the final autonomous navigation and positioning system.

[0009] The data acquisition layer includes the following steps:

[0010] 1. Extract feature information using a super camera, IMU, and lidar

[0011] 2. For incoming LiDAR scans, motion distortion caused by continuous movement within a frame is compensated by backpropagation from the IMU.

[0012] 3. Project a certain number of points (i.e., tracking points) in the global map onto the current image. Subscribe to the point cloud information after motion distortion correction of the current LiDAR frame.

[0013] In the data processing layer, the following steps are included:

[0014] 1. Organize the data generated by the data acquisition layer and perform feature extraction and IMU pre-integration.

[0015] And define the complete state vector X ∈ R 29 as:

[0016] X = G R T I , G p T I , G v T ,b T g ,b T a , G g T , I R T C , I p T C , I t C ,φ T T (1)

[0017] where G g ∈ R 3 is the gravity vector expressed in the global frame (i.e., the first LiDAR frame), I t C is the time offset between the IMU and the camera, assuming that the LiDAR has been synchronized with the IMU, and φ is the camera intrinsic matrix.

[0018] P = G p x , G p y , G p z ,c​r ,c g ,c b T = G p T ,c T T (2)

[0019] 2. The steps for constructing the map geometric structure are as follows:

[0020] (1) For the incoming LiDAR scans, the motion distortion caused by continuous movement within the frame is compensated by backpropagation from the IMU.

[0021] (2) The error-state iterative Kalman filter (ESIKF) is used to minimize the point-to-plane residuals to estimate the state of the system.

[0022] (3) In the converged state, the points of this scan are appended to the global map, and the corresponding voxels are marked as active or inactive.

[0023] 3. The steps for rendering the map texture are as follows:

[0024] (1) Frame-to-frame optical flow is used to track the map points and optimize the projection error of the system state for tracking the map points by minimizing Perspective-n-Point (PnP).

[0025] Assume that m map points ρ = {P 1 ,..., P m} are tracked in the last frame image I K-1 , and their projections are {ρ 1k -1,..., ρ mk-1}. Their positions in the current image IK are represented as {ρ 1k ,... ρ mk}.

[0026] Perspective-n-Point projection error: Taking the s-th point P S = G P S T ,C T S ∈ ρ as an example, the projection error is calculated as follows:

[0027]

[0028]

[0029] where the current state estimate value in each ESIKF iteration, is calculated as follows:​​

[0030]

[0031]

[0032]

[0033] where and are respectively G p s and ρ sk true values. Then, we obtain the first-order Taylor expansion of the true zero residual

[0034]

[0035]

[0036]

[0037]

[0038]

[0039]

[0040]

[0041]

[0042]

[0043]

[0044]

[0045] K = (H T R -1 H + P -1 ) -1 H T R -1

[0046]

[0047] (2) Further refine the state estimate of the system by minimizing the frame-to-map photometric error between the tracking points.

[0048] Taking the s-th tracking point P s ∈ρ as an example, the photometric error is calculated as follows:[[]]

[0049]

[0050] Consider Y s and c s for measurement noise:

[0051]

[0052]

[0053]

[0054]

[0055]

[0056]

[0057]

[0058]

[0059]

[0060]

[0061] Denote H, R, and P as similar:

[0062]

[0063] R = diag(∑β 1 ,..., ∑β m )

[0064]

[0065]

[0066] This frame-to-map VIO ESIKF update is iterated until convergence. Then the converged state estimate is used for: rendering the texture of the map; updating the current set of tracked points P for the next frame; serving as the starting point for IMU propagation in the next frame of LIO or VIO update.

[0067]

[0068] (3) Using the converged state estimate and the original input image, we perform texture rendering to update the color of the points in the global map.

[0069] First, retrieve all points in all activated voxels. Assume there are a total of n points, denoted as ζ = {P 1 , …, P n}. If point Ps falls within the current image frame, and its observed color γ is obtained by linearly interpolating the RGB color values of adjacent pixels on the current image frame s and covariance Σ nγs . Through Bayesian update, the color of the newly observed point on the image is fused with the existing color value c recorded in the map s to obtain the updated color value and the covariance of c s :

[0070]

[0071]

[0072]

[0073] (4) After texture rendering is completed, we update the set of tracked points P: if the PnP reprojection error or photometric error calculated by the points in the set P is large, it is deleted from the set of tracked points; if the point is projected onto the current image frame and there are no other tracked points nearby (for example, setting a radius of 50 pixels), it is added to the set P

[0074] In the application layer, the following steps are included:

[0075] The application layer imports models trained with a large amount of data: pedestrians, locomotives, other obstacles, etc. into the autonomous decision-making system

[0076] Its autonomous decision-making system identifies the received video information and performs image processing. The image processor is built based on the NVIDIA Jetson TX2 platform. The GAN algorithm in Deeplearning enhances the image, and the Roberts operator template is selected for edge detection. Pedestrian obstacles are detected through the Mask RCNN network, and then the road condition analyzer sends "the presence and distance of obstacles" to the autonomous decision-making system through the 485 bus, and controls the frequency converter according to the signal to finally control the start and stop of the locomotive motor

[0077] Compared with the prior art, the beneficial effects of the present invention:

[0078] 1. The slam algorithm in the traditional mode has poor stability, resulting in error accumulation. This patent constructs a multi-modal autonomous navigation and positioning system based on the multi-sensor parallel technology that integrates vision, lidar, and inertial measurement unit, avoiding accident rates in the case of weak satellite signals or no satellite signals, and solving the problem of low accuracy in single-sensor data capture and processing

[0079] 2. By proposing a real-time synchronization positioning, mapping and coloring framework, a multi-modal autonomous navigation and positioning system solves the problems of real-time synchronization positioning, 3D mapping and map rendering based on the tight-coupling fusion of LiDAR, inertial and visual measurements, improves the robustness and accuracy of the system, avoids accident rates in cases of weak or no satellite signals, and solves the problem of low accuracy in single-sensor data capture and processing. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 It is a hierarchical schematic diagram of the system of the present invention DETAILED DESCRIPTION OF THE INVENTION

[0081] The present invention will be further described below. The technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention.

[0082] A multi-modal autonomous navigation and positioning system according to an embodiment of the present invention specifically relates to the field of driverless, and includes a data acquisition layer, a data processing layer, and an application layer;

[0083] First, the data acquisition layer uses the SLAM method of lidar, camera and IMU, and performs joint operations on the point cloud features of the lidar, the IMU data, and the feature information extracted and optimized by the camera, so as to improve the accuracy of the entire system in an unstructured environment. And key frame matching is also performed to reduce the computational load of the entire system, achieving a balance between accuracy and computational load, and ensuring the accuracy and real-time performance of the system.

[0084] After that, the data processing layer processes and integrates the data generated by the data acquisition layer, renders the texture of the map through the minimum photometric error and feature matching, constructs the map geometric structure, realizes precise positioning and mapping, and transmits it to the application layer.

[0085] The application layer uploads the pre-trained model to the autonomous decision-making system to realize the final autonomous navigation and positioning system.

[0086] The data acquisition layer includes the following steps:

[0087] 1. Extract feature information using a super camera, IMU, and lidar.

[0088] 2. For the laser point cloud of the current frame, calculate the motion compensation: calculate the relative motion of the radar with respect to the initial moment at the receiving moment of each laser beam, multiply the laser point coordinates by the coordinate system conversion relationship, and obtain the pose of the laser point at each moment, and convert it to the coordinate system of the laser point at the initial moment.

[0089] (1) Subscribe to the original point cloud data collected by the lidar and the original data collected by the IMU: Subscribe to the IMU increment for pose estimation. When new IMU data is transmitted, estimate the pose at the current moment through the previous IMU integration data and the currently obtained data; subscribe to the original point cloud data collected by the lidar.

[0090] (2) Find paired data: Taking the lidar as the reference, find the IMU data that contains the start and end times of a frame of lidar points.

[0091] (3) Integrate these IMU data: Taking the first IMU as the reference coordinate, integrate all the IMU data in sequence, and each IMU obtains a corresponding pose.

[0092] (4) Calculate the pose of the lidar at the end time relative to the first IMU

[0093] If the odometer queue is exactly synchronized with the lidar data, assuming that the times of the i-th and j-th data are t s , t e , then solve for the poses p s and p e at times t s and p e as follows:

[0094] p s = OdomList[i]

[0095] p e = OdomList[j]

[0096] If the above situation does not exist, that is, there is no corresponding pose at times t s and t e , then calculate the poses at m times by linear interpolation of the odometer: Assume that there are poses at times l and k, and l < s < k, then obtain p s and p e The formulas are as follows:

[0097] p l = OdomList[l]

[0098] p k = OdomList[k]

[0099]

[0100] During the time period between t s and t e , a total of m poses {p s , ps+1,..., p s+m-2 , p e}. Then, linearly interpolate the poses corresponding to each point of the lidar in segments among the known m + 2 poses.

[0101] x′ i =(p x , p y )

[0102]

[0103] angle = atan2(p y , p x )

[0104] Note that this pose is in the IMU coordinate system and needs to be converted to the pose of the lidar coordinate system relative to the first IMU coordinate system.

[0105] For the points of the entire point cloud, calculate their poses of the lidar coordinate system relative to the first IMU according to the processing method in step 3, then convert them to the lidar coordinate system at the last moment, and finally repackage them into a frame of lidar data and publish it.

[0106] 3. Project a certain number of points (i.e., tracking points) in the global map onto the current image. Subscribe to the point cloud information after motion distortion correction of the current lidar frame.

[0107] In the data processing layer, the following steps are included:

[0108] 1. Organize the data generated by the data acquisition layer, and perform feature extraction and IMU pre-integration.

[0109] Perform pre-integration on the IMU information and propagate the results: Subscribe to the lidar and IMU odometry, calculate the IMU odometry at the current moment according to the change increment of the IMU odometry from the previous moment's lidar odometry to the current moment's IMU odometry, and display the local IMU odometry trajectory through rviz.

[0110] (1) Receive the lidar odometry data, unify the data into a specified format and save the timestamp of the current lidar odometry.

[0111] (2) Remove the IMU increment data in the IMU queue whose timestamp is earlier than the lidar odometry timestamp; calculate the relative pose transformation between the IMU odometry corresponding to the start and end times in the IMU queue; combine the previously received lidar odometry data with the data obtained from the previously calculated relative pose transformation and publish it.

[0112] (3) Subscribe to the raw IMU data and add it to the queue, use the result obtained after optimization at the previous moment as the starting value of the integration, perform integration calculation on the data in the IMU queue, and publish the IMU increment.

[0113] (4) The formula definitions for the IMU angular velocity and acceleration are as follows:

[0114] Angular velocity:

[0115] Acceleration:

[0116] The IMU observed source data at time t is and and and both are affected by white noise n t and the IMU bias b t . The matrix is the transformation matrix from the world coordinate system to the robot coordinate system. The gravity constant vector g belongs to the world coordinate system W.

[0117] (5) Initialize the system for judgment: Whenever 100 frames of laser odometry data are received, the optimizer performs a reset. Initialization is different from reset. When performing a reset, the current speed and pose are consistent with the speed and pose obtained from the previous optimization, and the noise model adopts the marginal distribution of the model after the previous optimization; assign initial values to the variable nodes, perform optimization and update the state at the previous moment; initialize the pre-integrator using the currently optimized state and calculate the IMU data after the current frame.

[0118] (6) Infer the motion of the robot through the measurements of the IMU. The calculation formulas for the position, speed, and rotation of the robot at time t + Δt are as follows:

[0119]

[0120]

[0121]

[0122] where it is assumed that the angular velocity and acceleration of the robot remain unchanged during the integration process.

[0123] Obtain the relative motion between different timestamps through IMU pre-integration. The position change Δp mn , speed change Δv mn and rotation change ΔR mn obtained by pre-integration are calculated as follows:

[0124]

[0125]

[0126]

[0127] and the complete state vector \(X\in\mathbb{R}\) 29 is defined as:

[0128] \(X = \left[\begin{array}{c} G \mathbb{R} T I , G p T I , G v T , b T g , b T a , G g T , I \mathbb{R} T C , I p T C , I t C , \varphi T \end{array}\right] T (1)

[0129] where G g\in\mathbb{R} 3 is the gravity vector expressed in the global frame (i.e., the first LiDAR frame), I t C is the time offset between the IMU and the camera, assuming that the LiDAR is already synchronized with the IMU, and \(\varphi\) is the camera intrinsic matrix.

[0130] P=\left[\begin{array}{c} G p x , G p y , G p z ,c r ,c g ,c b \end{array}\right] T =\left[\begin{array}{c} G p T ,c T \end{array}\right] T (2)

[0131] 2. The construction of the map geometry includes the following steps:

[0132] (1) For the incoming LiDAR scan, the motion distortion caused by continuous movement within the frame is compensated by backpropagation from the IMU.

[0133] (2) The state of the system is estimated by minimizing the point-to-plane residuals using the Error-State Iterated Kalman Filter (ESIKF).

[0134] (3) In the convergence state, the points of the scan are appended to the global map, and the corresponding voxels are marked as active or inactive.

[0135] 4. The texture of its rendered map includes the following steps:

[0136] (1) Use frame-to-frame optical flow to track map points and optimize the projection error of the system state tracking map points by minimizing Perspective-n-Point (PnP).

[0137] Assume that m map points ρ = {P 1 ,..., P m} are tracked in the last frame image I K-1 and projected as {ρ 1k-1 ,..., ρ mk-1}, and their positions in the current image IK are represented as {ρ 1k ,... ρ mk}.

[0138] Perspective-n-Point projection error: Taking the s-th point P S = G P S T , C T S ∈ ρ as an example, the projection error is calculated as follows:

[0139]

[0140]

[0141] where is the current state estimate value in each ESIKF iteration, The calculation method is as follows:

[0142]

[0143]

[0144]

[0145] where and are respectively G p s and the true value of ρ sk . Then, we obtain the first-order Taylor expansion of the true zero residual

[0146]

[0147]

[0148]

[0149]

[0150]

[0151]

[0152]

[0153]

[0154]

[0155]

[0156] K = (H T R -1 H + P -1 ) -1 H T R -1

[0157]

[0158] (2) Further refine the state estimation of the system by minimizing the frame-to-map photometric error between the tracking points.

[0159] Taking the s-th tracking point P s ∈ρ as an example, the photometric error is calculated as follows:

[0160]

[0161] Considering the measurement noise of Y s and c s :

[0162]

[0163]

[0164]

[0165]

[0166]

[0167]

[0168]

[0169]

[0170]

[0171]

[0172] H, R, is similar to P:

[0173]

[0174] R = diag(∑β 1 ,..., ∑β m )

[0175]

[0176]

[0177] This frame-to-map VIO-ESIKF update is iterated until convergence. Then the converged state estimate is used for: rendering the texture of the map; updating the current set of tracked points P for use in the next frame; serving as the starting point for IMU propagation in the next frame of LIO or VIO update.

[0178]

[0179] (3) Using the converged state estimate and the original input image, we perform texture rendering to update the colors of the points in the global map.

[0180] First, all points in all activated voxels are retrieved. Suppose there are a total of n points, denoted as ζ = {P 1 , …, P n}. If the point P s falls within the current image frame, its observed color γ s and covariance Σ nγs are obtained by linearly interpolating the RGB color values of the adjacent pixels on the current image frame. Through Bayesian update, the color of the newly observed points on the image is fused with the existing color value c s recorded in the map to obtain the updated color value and the covariance of c s :

[0181]

[0182]

[0183]

[0184] (4) After the texture rendering is completed, we update the set of tracked points P: if the PnP reprojection error or photometric error calculated for the points in the point set P is large, then it is deleted from the set of tracked points; if a point is projected onto the current image frame and there are no other tracked points nearby (for example, setting a radius of 50 pixels), then it is added to the point set P.

[0185] In the application layer, the following steps are included:

[0186] The application layer imports the models completed by training with a large amount of data: pedestrians, locomotives, other obstacles, etc. into the autonomous decision-making system.

[0187] Its autonomous decision-making system identifies the received video information and performs image processing. The image processor is built based on the NVIDIA Jetson TX2 platform. The GAN algorithm in Deeplearning is used to enhance the image, and the Roberts operator template is selected for edge detection. The Mask R-CNN network is used to detect pedestrian obstacles, and then the road condition analyzer sends "the presence or absence of obstacles and the distance" to the autonomous decision-making system through the 485 bus, and controls the frequency converter according to the signal to finally control the start and stop of the locomotive motor.

[0188] The present invention is not limited to the foregoing specific embodiments. The present invention extends to any new feature or any new combination disclosed in this specification, as well as any new method or process step or any new combination disclosed. If those skilled in the art make non-substantial changes or improvements without departing from the spirit of the present invention, they should fall within the scope of protection of the claims of the present invention.

Claims

1. A multi-modal autonomous navigation and positioning system, characterized in that, it includes: a data acquisition layer, a data processing layer, and an application layer; The data acquisition layer uses the SLAM method of lidar, camera, and IMU to perform joint operations on the point cloud features of the lidar, IMU data, and the feature information extracted and optimized by the camera, and perform key frame matching; The data processing layer processes and integrates the data generated by the data acquisition layer, renders the texture of the map through the minimum photometric error and feature matching, and constructs the map geometric structure; The data processing layer includes: Sort out the data generated by the data acquisition layer, perform feature extraction and IMU pre-integration; Construct the geometric structure of the global map, record the input LiDAR scan, and estimate the state of the system by minimizing the residual of the point to the plane; Construct the texture of the map, render the RGB color of each point with the input image, and update the system state by minimizing the frame-to-frame PnP reprojection error and the frame-to-map photometric error; The data processing layer includes: Use frame-to-frame optical flow to track map points and optimize the projection error of the system state tracking map points by minimizing Perspective-n-Point; Further refine the state estimation of the system by minimizing the frame-to-map photometric error between the tracked points; Using the converged state estimation and the original input image, we perform texture rendering to update the color of the points in the global map; After the texture rendering is completed, update the set of tracked points P. If the PnP reprojection error or photometric error calculated by the points in the point set P is large, delete it from the set of tracked points. If there are no other tracked points near the projection of the point onto the current image frame, add it to the point set P; The application layer uploads the pre-trained model to the autonomous decision-making system for the autonomous navigation and positioning system.

2. A multi-modal autonomous navigation and positioning system according to claim 1, characterized in that, The data acquisition layer includes: Use a super camera, IMU, and lidar to extract feature information; For the incoming LiDAR scan, the motion distortion caused by continuous movement within the frame is compensated by IMU backpropagation; Project a certain number of points in the global map onto the current image, and subscribe to the point cloud information after the motion distortion correction of the current laser frame.

3. A multi-modal autonomous navigation and positioning system according to claim 2, characterized in that, The data acquisition layer includes: Subscribe to IMU increments for pose estimation. When new IMU data is passed in, estimate the pose at the current moment through the previous IMU integration data and the current obtained data; Subscribe to the original point cloud data collected by the lidar; Based on the lidar, find the IMU data at the start and end times of the points included in one frame of the lidar; Based on the first IMU as the reference coordinate, integrate all the IMU data in sequence, and each IMU obtains a corresponding pose; Calculate the attitude of its lidar coordinate system relative to the first IMU, then convert it to the lidar coordinate system at the last moment, and finally repackage it into a frame of laser data and publish it.

4. A multimodal autonomous navigation and positioning system according to claim 1, characterized in that, the data processing layer includes: Receiving lidar odometry data, unifying the data into a specified format and saving the timestamp of the current lidar odometry; Removing IMU incremental data in the IMU queue whose timestamp is earlier than the lidar odometry timestamp; calculating the relative pose transformation between the IMU odometers corresponding to the start and end times in the IMU queue; Combining and publishing the previously received lidar odometry data and the data obtained from the previously calculated relative pose transformation; Subscribing to the raw IMU data and adding it to the queue, using the result optimized at the previous moment as the starting value of the integration, performing integration calculation on the data in the IMU queue, and publishing the IMU increment; Whenever 100 frames of lidar odometry data are received, the optimizer is reset once. Different from initialization, when resetting, the current speed and pose are consistent with the speed and pose obtained from the previous optimization, and the noise model adopts the marginal distribution of the model optimized previously; Assigning initial values to the variable nodes, performing optimization and updating the state at the previous moment; Initializing the pre-integrator with the currently optimized state and calculating the IMU data after the current frame; Inferring the movement of the robot through the measurements of the IMU.

5. A multimodal autonomous navigation and positioning system according to claim 1, characterized in that, the data processing layer includes: For the incoming LiDAR scan, the motion distortion caused by continuous movement within the frame is compensated by backpropagation of the IMU; Using the error state iterative Kalman filter (ESIKF) to minimize the point-to-plane residuals to estimate the state of the system; In the convergence state, the points of this scan are appended to the global map, and the corresponding voxels are marked as active or inactive.

6. A multimodal autonomous navigation and positioning system according to claim 1, characterized in that, the application layer includes: Uploading the pre-trained model to the autonomous decision-making system; Its autonomous decision-making system identifies the received video information and performs image processing; Detecting pedestrian obstacles through the MaskRCNN network, and then the road condition analyzer sends the presence and distance of obstacles to the autonomous decision-making system through the 485 bus, and controls the frequency converter according to the signal to finally control the start and stop of the locomotive motor.

Images