A multi-sensor fusion positioning and mapping method with ground constraints

Abstract

The invention discloses a multi-sensor fusion positioning and mapping method with ground constraints, which relates to simultaneous positioning and mapping processing technology and the field of robot perception. The method is based on graph optimization and realizes the joint nonlinear optimization of multi-source data of LiDAR, IMU and encoder. The original point cloud of LiDAR and the measurement value of IMU are input into the front-end module for estimating the initial posture and registering the point cloud; the processing module completes the segmentation of the ground point cloud, constructs the IMU and encoder measurement model, and eliminates the dynamic points from the registered point cloud; the loop detection module receives the dedistorted LiDAR scanning frame and the local map to complete the inter-frame matching to reduce the cumulative error of the position; finally, the back-end module optimizes the factor graph by constructing the LiDAR-IMU odometer factor, the ground plane factor, the IMU and encoder pre-integration factor and the closed-loop factor to generate a high-precision pose and a map without ghost traces. Compared with the prior art, the present invention can achieve higher-precision positioning and mapping effects, realize higher-precision global pose graph optimization, and has stronger adaptability to the environment.

Description

Technical Field

[0001] The present invention belongs to the field of simultaneous localization and mapping (SLAM) processing technology and robot perception, and specifically relates to a ground-constrained multi-sensor fusion positioning and mapping method. Background Art

[0002] With the widespread application of mobile robots in various fields, automatic positioning technology is the key technology to achieve their autonomous operation. SLAM technology is the core technology for autonomous robot positioning. This technology estimates the position and posture of the mobile robot within a certain period of time by carrying sensors such as laser radar and IMU, realizes the robot's autonomous positioning and map construction, and provides important technical support for subsequent mobile robot path planning and autonomous navigation.

[0003] At present, the mainstream SLAM algorithms are mainly divided into laser SLAM algorithms and visual SLAM algorithms. Among them, the laser SLAM algorithm can provide distance information between the robot body equipped with sensors and obstacles in the surrounding environment. It is insensitive to light, but its ranging effect on reflective objects such as glass and other media is poor. The visual SLAM algorithm has the advantages of rich information and high positioning accuracy in a small range, but it is easily affected by light. The above single sensor system will have serious positioning and mapping drift in degraded environments such as long corridors or tunnels. Therefore, integrating data from different sensors such as inertial measurement units (IMUs) or encoders to improve the robustness of SLAM systems is the focus of future research and development.

[0004] Patent CN116772828A discloses a multi-sensor fusion positioning and mapping method based on graph optimization. This method is based on graph optimization and realizes joint nonlinear optimization of multi-source data such as lidar, IMU, wheel odometer and GPS based on a sliding window. However, this method relies on GPS signals and the positioning accuracy at indoor elevation needs to be improved.

[0005] Patent No. CN16642499A discloses a multi-sensor fusion synchronous positioning and mapping method for ground mobile platforms. By integrating lidar, camera, inertial measurement unit, and global positioning system sensors, the synchronous positioning and mapping of ground mobile platforms are realized. However, this method also has odometer drift in indoor degraded environments such as tunnels or long corridors lacking features. Both of the above methods cannot remove dynamic point clouds, which will cause ghost traces in the constructed point cloud map. Summary of the invention

[0006] In view of the defects and shortcomings of the prior art, the present invention proposes a ground-constrained multi-sensor fusion positioning and mapping method, which is used to solve the problem that mapping and positioning in a degraded environment are seriously affected by environmental factors, and aims to further improve the accuracy of the positioning system.

[0007] The technical solution of the present invention is as follows: a multi-sensor fusion positioning and mapping method with ground constraints, the specific implementation steps are:

[0008] Step 1: Collect data from the surrounding area’s multi-line laser radar (LiDAR), inertial measurement unit (IMU), and chassis encoder through the mobile robot platform;

[0009] Step 2: Use the IMU and multi-line laser radar as the front end to build a laser inertial odometer through the FAST-LIO2 algorithm, and obtain the dedistorted point cloud frame;

[0010] Step 3: Extract the ground point cloud of the dedistorted point cloud frame through the RANSAC algorithm, and construct the global ground plane factor through the front-end laser inertial odometer;

[0011] Step 4: Pre-integrate the data collected by the IMU and encoder respectively, build the IMU and encoder measurement model and pre-integration model, and build the IMU and encoder pre-integration factors respectively;

[0012] Step 5: Remove the dynamic point cloud by calculating the spatiotemporal normal vector in the dedistorted point cloud frame obtained by the front end, and obtain a point cloud frame with dedistorted dynamic points;

[0013] Step 6: Extract key frames from the above poses through time and distance information, and combine the point cloud frames in step 3 with the key frames to construct a sub-graph;

[0014] Step 7: Use the global descriptor ScanContext for loop closure detection, construct the loop closure factor, and correct the accumulated error;

[0015] Step 8: Use graph optimization to jointly optimize the constructed factor residual terms and constraint equations. Considering the case of wheel slippage, the encoder factor residual terms are constrained by encoder pre-integration weights. By establishing a nonlinear least squares objective function to optimize the subgraph's posture, the final posture state and global static map are obtained.

[0016] The present invention is characterized in that:

[0017] In step 3, the global ground plane factor constructed is implemented according to the following steps:

[0018] The ground points near the mobile robot can be modeled as an infinite plane PF:(n,d), where n is the ground plane normal vector and d is the distance between the ground plane and the origin of the LiDAR coordinate system. Assume that the ground parameters PF:(n k ,d k ) corresponds to the kth LiDAR keyframe, the constraint equation for each registered point on the ground can be defined as:

[0019]

[0020] The front end can obtain the transformation from the kth lidar keyframe to the k+1th lidar keyframe (including the rotation part). and the translation part in two parts ), so the ground of the k+1th LiDAR keyframe parameter is the constraint equation to which each registration point belongs. The distance from the kth LiDAR keyframe to the ground plane can be defined as:

[0021]

[0022]

[0023] When the robot moves in an outdoor environment, the ground plane usually changes, and different ground plane parameters need to be set. The judgment of different ground heights is as follows:

[0024]

[0025] Δd k,k+1 =d k+1 -d k

[0026] where Δθ k,k+1 Represents the normal vector n k and n k+1 The angle between k,k+1 Represents the height difference between adjacent lidar frames and the ground plane.

[0027] If Δθ k,k+1 or Δd k,k+1 If the ground parameters observed in each keyframe are greater than the threshold, the two lidar keyframes are considered to belong to different ground surfaces. Finally, the deviation between the ground parameters observed in each keyframe and the estimated global ground surface is incorporated into the factor graph as the ground factor. The residual model between two lidar keyframes can be calculated as:

[0028]

[0029]

[0030] PF k and PFW denote the ground plane of the kth lidar keyframe and the global ground plane, respectively, H(PF k ) are the ground plane parameters measured from the kth lidar keyframe in world coordinates.

[0031] In step 4, the constructed IMU and encoder measurement model and pre-integration model are implemented in the following steps:

[0032] Raw accelerometer from IMU at time t and gyroscope measurements The measurement in IMU coordinates can be defined as:

[0033]

[0034]

[0035] where a t and w t are the true values ​​of the gyroscope and accelerometer, respectively. and are the bias and white noise of the accelerometer, respectively, and and They are the bias and white noise of the gyroscope, and accelerometer The additive noise in is modeled as Gaussian white noise, where σ a and σ b The standard deviation of the Gaussian white noise. Gyroscope and accelerometer The deviations in are modeled as random walks, σ ba and σ bw are the standard deviations of the Gaussian random walk.

[0036] The wheel encoder determines the shaft speed based on the pulses received by the microcontroller counter and can calculate the left rear wheel speed at time t. and right rear wheel The linear speed is:

[0037]

[0038]

[0039] in and are the measured rotation speeds of the left and right shafts, and are the true values ​​of the left and right wheel speeds, r l and r rThey are respectively represented as the left radius and right radius of the wheel, and They are and The corresponding zero-mean Gaussian white noise, therefore, the measurement model of the wheel encoder in the encoder coordinates is defined as:

[0040]

[0041]

[0042] in and They represent the linear velocity and angular velocity of the robot at time t, b represents the wheelbase, express Zero-mean Gaussian white noise, express Zero-mean Gaussian white noise;

[0043] During the actual operation of the system, the wheels may slip on the smooth road surface, resulting in inaccurate yaw angle estimation and overall positioning deviation. In order to improve the reliability of encoder measurement, the encoder optimization weight is introduced to reduce the influence of encoder constraints when the wheels slip. The encoder optimization weight is as follows:

[0044]

[0045]

[0046] in, and The moving distance from time i to time j calculated for the left encoder and the right encoder respectively, and λ is the maximum yaw angle increment threshold, which is usually set to 0.17.

[0047] The IMU pre-integration model is used to estimate the relative motion between the kth frame and the k+1th frame of the LiDAR, and is defined as follows:

[0048]

[0049]

[0050]

[0051] in and They are the displacement, velocity and attitude angle of IMU pre-integration, v k is the velocity integrated by the accelerometer during the last interval, a k and ω k is the original IMU measurement value at the kth frame, Δtk,k+1 is the time interval from the kth frame to the k+1th frame, and are the measurement values ​​of the accelerometer and gyroscope in the IMU coordinate system at the kth frame, and It is divided into the true values ​​of the accelerometer and gyroscope in the IMU coordinate system at the kth frame, and are the noises of the accelerometer and gyroscope at the kth frame respectively.

[0052] The encoder pre-integration of the increment between the kth and k+1th LiDAR frames is defined as follows:

[0053]

[0054]

[0055] in and are the encoder pre-integrated position and velocity, is the posture from the encoder coordinate system to the world coordinate system at the kth frame, and The angular velocity and linear velocity calculated by the encoder strategy model at the kth frame respectively.

[0056] In step 5, a point cloud frame with distortion removed and dynamic points is obtained, which is specifically implemented according to the following steps;

[0057] Consider a dedistorted point cloud P without registered ground points in the world coordinate system i (i∈1,...,n), its local covariance is:

[0058]

[0059]

[0060] in Indicates P i The jth point in Indicates the timestamp of the association, represents the set of neighbor points, and Respectively The corresponding minimum eigenvector and minimum eigenvalue

[0061] Finally, dynamic scoring Defined as:

[0062]

[0063] A low dynamic score of a spatial point in the point cloud indicates that it is stationary or has a very low velocity in the three-dimensional space. Therefore, the point is classified as static or dynamic according to the dynamic score.

[0064] In step eight, the constructed factor residual term and constraint equation are jointly optimized by graph optimization. Considering the case of wheel slippage, the encoder factor residual term is constrained by encoder pre-integration weight. The pose of the subgraph is optimized by establishing a nonlinear least squares objective function. Specifically, the following steps are implemented:

[0065] First, according to the measurement models of IMU and encoder, the residual models of IMU and encoder pre-integration constraint equations are constructed respectively. Considering the IMU measurement between the kth and k+1th LiDAR key frames, the residual model of IMU pre-integration factor is as follows:

[0066]

[0067] in, is the residual term of the IMU pre-integration factor, is the transformation matrix from the IMU coordinate system to the world coordinate system at the kth frame, and are the displacements calculated by MU integration of I in the world coordinate system at the kth frame and k+1th frame, respectively, W is the gravitational acceleration value in the world coordinate system, Δt k,k+1 is the time interval from the kth frame to the k+1th frame, and are the velocities calculated by IMU integration in the world coordinate system at the kth frame and the k+1th frame, and are the attitude quaternions calculated by the IMU in the world coordinate system at the kth frame and the k+1th frame, and are the biases of the accelerometer and gyroscope at the k+1th frame respectively.

[0068] In two consecutive LiDAR motion frames, the encoder pre-integration factor residual model is defined as follows:

[0069]

[0070] in is the encoder pre-integration factor residual term, is the rotation matrix from the world coordinate system to the IMU coordinate system at the kth frame, and are the poses calculated by IMU integration at the kth frame and k+1th frame respectively, and are the velocities calculated by IMU integration at the kth frame and k+1th frame respectively.

[0071] The deviation between the observed ground parameters and the estimated global ground in each keyframe is incorporated into the factor graph as the ground factor. The residual model of the ground plane factor between two lidar keyframes is defined as follows:

[0072]

[0073]

[0074] in is the residual term of the ground plane factor, PF k and PF W denote the ground plane of the kth lidar keyframe and the global ground plane, respectively, H(PF k ) are the ground plane parameters measured from the kth lidar keyframe in world coordinates.

[0075] If the correct loop constraint is formed, a pair of loop detection key frames x are obtained. i and x j , by i and x j Perform ICP matching. If the ICP fitting score is less than the threshold, the relative transformation can be obtained, and the residual model of the loop detection factor can be defined as:

[0076]

[0077] where s j,k is the scaling factor of the closed-loop factor between the state nodes i and j of the mobile robot, h(·) is the relative transformation between the above nodes, and z i,j is the loop detection measurement value.

[0078] By minimizing the sum of the IMU pre-integration factor residual, the encoder pre-integration factor residual, and the ground plane factor residual closed-loop residual, the optimized posture state X can be calculated as follows:

[0079]

[0080] where Σ (·) The noise covariance matrix representing each residual model determines the uncertainty of each measurement.

[0081] The beneficial effects of the present invention are:

[0082] 1. The present invention provides a multi-sensor fusion positioning and mapping method based on graph optimization, which tightly couples IMU, encoder, and LiDAR to achieve high-precision positioning;

[0083] 2. The encoder factor provided by the present invention effectively overcomes the positioning drift of the traditional SLAM method in a degraded environment;

[0084] 3. The global ground plane factor provided by the present invention effectively reduces the vertical error and improves the positioning accuracy in the vertical direction;

[0085] 4. The dynamic point cloud removal method provided by the present invention effectively removes ghost traces in the map and improves the quality of the point cloud map.

[0086] In summary, compared with the prior art, the present invention can achieve higher-precision positioning and mapping effects, achieve higher-precision global pose graph optimization, and has stronger adaptability to the environment. Specifically, the distinguishing feature is that the global ground factor proposed in the present invention constrains the positioning drift in the vertical direction, the dynamic point cloud removal method of the spatiotemporal normal vector effectively constructs a static point cloud map, and the encoder factor combined with the optimized weight ensures the positioning accuracy of the odometer in a degraded environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 It is a system block diagram of a ground-constrained multi-sensor fusion positioning and mapping method of the present invention;

[0088] Figure 2 This is a graph showing the absolute estimation error between the estimated value of the present invention and the estimated value of other mainstream algorithms and the true value under the KAIST dataset urban_08 sequence;

[0089] Figure 3 (a) is a graph showing the pose estimation results of the horizontal plane using the KAIST dataset urban_08 sequence and other mainstream methods;

[0090] Figure 3 (b) is a graph showing the vertical plane pose estimation of the present invention in the KAIST dataset urban_08 sequence and pose estimation results of other mainstream methods;

[0091] Figure 3 (c) is the mapping result of the present invention under the KAIST dataset urban_08 sequence;

[0092] Figure 4 (a) is a graph showing the pose estimation results of the present invention and other mainstream methods under the TUNNEL dataset;

[0093] Figure 4 (b) is the mapping result of the present invention under the TUNNEL dataset;

[0094] Figure 5 This is a graph showing the mapping results of the present invention under a self-built parking lot dataset and the mapping results of other mainstream algorithms;

[0095] Figure 6 (a) is a diagram showing the vertical positioning estimation results of the present invention and other mainstream methods under the self-built parking lot data set;

[0096] Figure 6 (b) is a diagram showing the vertical positioning estimation results of the present invention and other mainstream methods under the self-built outdoor data set. DETAILED DESCRIPTION

[0097] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0098] The invention discloses a multi-sensor fusion positioning and mapping method for ground constraints, which relates to simultaneous positioning and mapping processing technology and the field of robot perception. The method is based on graph optimization and realizes joint nonlinear optimization of multi-source data of LiDAR, IMU and encoder. The original point cloud of LiDAR and the measurement value of IMU are input into the front-end module for estimating the initial posture and registering the point cloud. The processing module completes the segmentation of the ground point cloud, constructs the IMU and encoder measurement model, and eliminates the dynamic points from the registered point cloud. The loop detection module receives the dedistorted LiDAR scanning frame and the local map to complete the inter-frame matching to reduce the cumulative error of the position. Finally, the back-end module optimizes the factor graph by constructing the LiDAR-IMU odometer factor, the ground plane factor, the IMU and encoder pre-integration factor and the closed-loop factor to generate a high-precision pose and a map without ghost traces. Compared with the prior art, the present invention can achieve higher-precision positioning and mapping effects, realize higher-precision global pose graph optimization, and has stronger adaptability to the environment.

[0099] The system block diagram of a ground-constrained multi-sensor fusion positioning and mapping method provided by the present invention is as follows: Figure 1 As shown, follow the specific implementation steps below:

[0100] Step 1: Collect data from the multi-line lidar, IMU, and chassis encoder in the surrounding area through the mobile robot platform;

[0101] Step 2: Use the IMU and multi-line laser radar as the front end to build a laser inertial odometer through the FAST-LIO2 algorithm, and obtain the dedistorted point cloud frame;

[0102] Step 3: Extract the ground point cloud of the dedistorted point cloud frame through the RANSAC algorithm, and construct the global ground plane factor through the front-end laser inertial odometer;

[0103] Step 4: Pre-integrate the data collected by the IMU and encoder respectively, build the IMU and encoder measurement model and pre-integration model, and build the IMU and encoder pre-integration factors respectively;

[0104] Step 5: Remove the dynamic point cloud by calculating the spatiotemporal normal vector in the dedistorted point cloud frame obtained by the front end, and obtain a point cloud frame with dedistorted dynamic points;

[0105] Step 6: Extract key frames from the above poses through time and distance information, and combine the point cloud frames in step 3 with the key frames to construct a sub-graph;

[0106] Step 7: Use the global descriptor ScanContext for loop closure detection, construct the loop closure factor, and correct the accumulated error;

[0107] Step 8: Use graph optimization to jointly optimize the constructed factor residual terms and constraint equations. Considering the case of wheel slippage, the encoder factor residual terms are constrained by encoder pre-integration weights. By establishing a nonlinear least squares objective function to optimize the subgraph's posture, the final posture state and global static map are obtained.

[0108] The present invention is characterized in that:

[0109] In step 3, the global ground plane factor constructed is implemented according to the following steps:

[0110] The ground points near the mobile robot can be modeled as an infinite plane PF:(n,d), where n is the ground plane normal vector and d is the distance between the ground plane and the origin of the LiDAR coordinate system. Assume that the ground parameters PF:(n k ,d k ) corresponds to the kth LiDAR keyframe, the constraint equation for each registered point on the ground can be defined as:

[0111]

[0112] The front end can obtain the transformation from the kth lidar keyframe to the k+1th lidar keyframe (including the rotation part). and the translation part in two parts ), so the ground of the k+1th LiDAR keyframe parameter is the constraint to which each registration point belongs. The distance from the kth LiDAR keyframe to the ground plane can be defined as:

[0113]

[0114]

[0115] When the robot moves in an outdoor environment, the ground plane usually changes, and different ground plane parameters need to be set. The judgment of different ground heights is as follows:

[0116]

[0117] Δd k,k+1 =d k+1 -d k

[0118] where Δθ k,k+1 Represents the normal vector n k and n k+1 The angle between k,k+1 Represents the height difference between adjacent lidar frames and the ground plane.

[0119] If Δθ k,k+1 or Δd k,k+1 If the ground parameters observed in each keyframe are greater than the threshold, the two lidar keyframes are considered to belong to different ground surfaces. Finally, the deviation between the ground parameters observed in each keyframe and the estimated global ground surface is incorporated into the factor graph as the ground factor. The residual model between two lidar keyframes can be calculated as:

[0120]

[0121]

[0122] PF k and PF W denote the ground plane of the kth lidar keyframe and the global ground plane, respectively, H(PF k ) are the ground plane parameters measured from the kth lidar keyframe in world coordinates.

[0123] In step 4, the constructed IMU and encoder measurement model and pre-integration model are implemented in the following steps:

[0124] Raw accelerometer from IMU at time t and gyroscope measurements The measurement in IMU coordinates can be defined as:

[0125]

[0126]

[0127] where a t and w t are the true values ​​of the gyroscope and accelerometer, respectively. and are the bias and white noise of the accelerometer, respectively, and and They are the bias and white noise of the gyroscope, and accelerometer The additive noise in is modeled as Gaussian white noise, where σ a and σ b The standard deviation of the Gaussian white noise. Gyroscope and accelerometer The deviation in is modeled as a random walk whose derivative is a Gaussian distribution, σ ba and σ bw are the standard deviations of the Gaussian random walk.

[0128] The wheel encoder determines the shaft speed based on the pulses received by the microcontroller counter and can calculate the left rear wheel speed at time t. and right rear wheel The linear speed is:

[0129]

[0130]

[0131] in and are the measured rotation speeds of the left and right shafts, and are the true values ​​of the left and right wheel speeds, r l and r r Respectively represent the left and right radius of the wheel, and They are and The corresponding zero-mean Gaussian white noise, therefore, the measurement model of the wheel encoder in the encoder coordinates is defined as:

[0132]

[0133]

[0134] in and They represent the linear velocity and angular velocity of the robot at time t, b represents the wheelbase, express Zero-mean Gaussian white noise, express Zero-mean Gaussian white noise;

[0135] During the actual operation of the system, the wheels may slip on the smooth road surface, resulting in inaccurate yaw angle estimation and overall positioning deviation. In order to improve the reliability of encoder measurement, the encoder optimization weight is introduced to reduce the influence of encoder constraints when the wheels slip. The encoder optimization weight is as follows:

[0136]

[0137]

[0138] in, and The moving distance from time i to time j calculated for the left encoder and the right encoder respectively, and λ is the maximum yaw angle increment threshold, which is usually set to 0.17.

[0139] The IMU pre-integration model is used to estimate the relative motion between the kth frame and the k+1th frame of the LiDAR, and is defined as follows:

[0140]

[0141]

[0142]

[0143] in and They are the displacement, velocity and attitude angle of IMU pre-integration, v k is the velocity integrated by the accelerometer during the last interval, a k and ω k is the original IMU measurement value at the kth frame, Δt k,k+1 is the time interval from the kth frame to the k+1th frame, and are the measurement values ​​of the accelerometer and gyroscope in the IMU coordinate system at the kth frame, and It is divided into the true values ​​of the accelerometer and gyroscope in the IMU coordinate system at the kth frame, and are the noises of the accelerometer and gyroscope at the kth frame respectively.

[0144] The encoder pre-integration of the increment between the kth and k+1th LiDAR frames is defined as follows:

[0145]

[0146]

[0147] in and are the encoder pre-integrated position and velocity, is the posture from the encoder coordinate system to the world coordinate system at the kth frame, and The angular velocity and linear velocity calculated by the encoder strategy model at the kth frame respectively.

[0148] In step 5, a point cloud frame with distortion removed and dynamic points is obtained, which is specifically implemented according to the following steps;

[0149] Consider a dedistorted point cloud P without registered ground points in the world coordinate system i (i∈1,...,n), its local covariance is:

[0150]

[0151]

[0152] in Indicates P i The jth point in Indicates the timestamp of the association, represents the set of neighbor points, and Respectively The corresponding minimum eigenvector and minimum eigenvalue

[0153] Finally, dynamic scoring Defined as:

[0154]

[0155] A low dynamic score of a spatial point in the point cloud indicates that it is stationary or has a very low velocity in the three-dimensional space. Therefore, the point is classified as static or dynamic according to the dynamic score.

[0156] In step eight, the constructed factor residual term and constraint equation are jointly optimized by graph optimization. Considering the case of wheel slippage, the encoder factor residual term is constrained by encoder pre-integration weight. The pose of the subgraph is optimized by establishing a nonlinear least squares objective function. Specifically, the following steps are implemented:

[0157] First, according to the measurement models of IMU and encoder, the residual models of IMU and encoder pre-integration constraint equations are constructed respectively. Considering the IMU measurement between the kth and k+1th LiDAR key frames, the residual model of IMU pre-integration factor is as follows:

[0158]

[0159] in, is the residual term of the IMU pre-integration factor, is the transformation matrix from the IMU coordinate system to the world coordinate system at the kth frame, and are the displacements calculated by MU integration of I in the world coordinate system at the kth frame and k+1th frame, respectively, W is the gravitational acceleration value in the world coordinate system, Δt k,k+1 is the time interval from the kth frame to the k+1th frame, and are the velocities calculated by IMU integration in the world coordinate system at the kth frame and the k+1th frame, and are the attitude quaternions calculated by the IMU in the world coordinate system at the kth frame and the k+1th frame, and are the biases of the accelerometer and gyroscope at the k+1th frame respectively.

[0160] In two consecutive LiDAR motion frames, the encoder pre-integration factor residual model is defined as follows:

[0161]

[0162] in is the encoder pre-integration factor residual term, is the rotation matrix from the world coordinate system to the IMU coordinate system at the kth frame, and are the poses calculated by IMU integration at the kth frame and k+1th frame respectively, and are the velocities calculated by IMU integration at the kth frame and k+1th frame respectively.

[0163] The deviation between the observed ground parameters and the estimated global ground in each keyframe is incorporated into the factor graph as the ground factor. The residual model of the ground plane factor between two lidar keyframes is defined as follows:

[0164]

[0165]

[0166] in is the residual term of the ground plane factor, PF k and PF W denote the ground plane of the kth lidar keyframe and the global ground plane, respectively, H(PF k ) are the ground plane parameters measured from the kth lidar keyframe in world coordinates.

[0167] If the correct loop constraint is formed, a pair of loop detection key frames x are obtained. i and x j , by i and x j Perform ICP matching. If the ICP fitting score is less than the threshold, the relative transformation can be obtained, and the residual model of the loop detection factor can be defined as:

[0168]

[0169] where s j,k is the scaling factor of the closed-loop factor between the state nodes i and j of the mobile robot, h(·) is the relative transformation between the above nodes, and z i,j is the loop detection measurement value.

[0170] By minimizing the sum of the IMU pre-integration factor residual, the encoder pre-integration factor residual, and the ground plane factor residual closed-loop residual, the optimized posture state X can be calculated as follows:

[0171]

[0172] where Σ (·) The noise covariance matrix representing each residual model determines the uncertainty of each measurement.

[0173] In the accuracy analysis of the comparative experimental results of the method of the present invention, the trajectory estimation value of the method proposed by the present invention is comprehensively compared with the trajectory values ​​of other advanced algorithms. The error index description is mainly: the root mean square error (RMSE) of the absolute trajectory error (ATE).

[0174] Figure 2 This is a graph of the absolute estimation error between the estimated value of the present invention and the estimated value of other mainstream algorithms and the true value under the urban_08 sequence of the KAIST dataset. In the figure, the vertical axis is the RMSE value of ATE, and the horizontal axis is the method proposed by the present invention and other mainstream methods. LIWOM-GD is the method proposed by the present invention. It can be seen from the figure that the RMSE value (2.65m) of the method proposed by the present invention is smaller than that of other methods and has higher positioning accuracy.

[0175] Figure 3 (a) is a graph showing the horizontal plane pose estimation results of the present invention and other mainstream methods in the KAIST dataset urban_08 sequence. In the figure, the horizontal plane trajectory estimated by the method proposed by the present invention is closer to the real trajectory. Figure 3(b) is a graph showing the vertical pose estimation of the present invention and other mainstream methods under the KAIST dataset urban_08 sequence. In the figure, the vertical trajectory estimated by the method proposed by the present invention is closer to the real trajectory. Compared with other methods, the maximum error of the vertical trajectory estimated by the method proposed by the present invention is 1.25m, while the maximum errors of the vertical trajectories estimated by other algorithms are 28.92m, 13.39m and 5.48m respectively. Therefore, the ground factor proposed by the present invention effectively limits the positioning error in the vertical direction and improves the positioning accuracy in the vertical direction. Figure 3 (c) is the mapping result of the present invention under the KAIST dataset urban_08 sequence. The method proposed in the present invention can build high-quality point cloud maps in challenging environments.

[0176] This paper takes the TUNNEL tunnel dataset as an example to discuss the positioning accuracy of the method proposed in this paper. Figure 4 (a) is a graph showing the pose estimation results of the present invention and other mainstream methods under the TUNNEL dataset. The initial position of the mobile platform is taken as the origin, the initial forward direction of the vehicle body is the x-axis, and the direction perpendicular to the forward direction of the vehicle body is the y-axis. The actual forward distance of the mobile platform in the y direction in the tunnel is 200m. It can be seen from the figure that the forward distance estimated by the IMU and encoder loose coupling algorithm is 196.31m, the forward distance estimated by the EKF-LOAM algorithm is 194.22m, and the forward distance estimated by the method proposed in the present invention is 198.86m. The relative distance errors estimated by the above three algorithms are 2.89%, 1.85% and 0.57% respectively. Therefore, the high positioning accuracy of the method proposed in the present invention in a degraded environment can effectively overcome the degradation of the laser odometer. Figure 4 (b) is the mapping result of the present invention under the TUNNEL dataset. The method proposed in the present invention can effectively construct a point cloud map and better describe the detailed information in the map.

[0177] Figure 5 This is a diagram showing some of the mapping results of the present invention under the self-built parking lot dataset and the mapping results of other mainstream algorithms. It can be seen from the figure that the point cloud map constructed by the method proposed in the present invention has no ghost traces compared with other methods, while the white areas of other methods have obvious ghost traces. Therefore, the point cloud map constructed by the method proposed in the present invention can effectively remove dynamic points.

[0178] Figure 6 (a) is a diagram showing the vertical positioning estimation results of the present invention and other mainstream methods under the self-built parking lot data set. It can be seen from the figure that the maximum vertical positioning error of the method proposed by the present invention is 0.06m compared with that of other methods. Figure 6(b) is a diagram showing the vertical positioning estimation of the present invention under the self-built outdoor data set and the positioning estimation results of other mainstream methods in the vertical direction. It can be seen from the figure that the maximum vertical error estimated by the method proposed by the present invention is 0.94m compared with other methods. Compared with other methods, the estimated vertical error is the smallest and the positioning accuracy in the vertical direction is higher.

[0179] In summary, compared with the prior art, the present invention can achieve higher-precision positioning and mapping effects, realize higher-precision global pose graph optimization, and has stronger adaptability to the environment.

[0180] The examples of the present invention are described above in conjunction with the accompanying drawings. Although the present invention has been disclosed as above in the form of a preferred embodiment, it is not intended to limit the present invention. Anyone familiar with this technology can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention should be based on the definition of the claims.

Claims

1. A ground-constrained multi-sensor fusion positioning and mapping method, characterized in that: The following steps are involved: Step 1: Collect data from the surrounding area’s multi-line LiDAR, inertial measurement unit (IMU), and chassis encoder through the mobile robot platform; Step 2: Use the IMU and multi-line laser radar as the front end to build a laser inertial odometer through the FAST-LIO2 algorithm, and obtain the dedistorted point cloud frame; Step 3: Use the RANSAC algorithm to extract the ground point cloud of the dedistorted point cloud frame, and construct the global ground plane factor through the front-end laser inertial odometer. The global ground plane factor is constructed as follows: The ground points near the mobile robot can be modeled as an infinite plane PF :(n,d), n is the ground plane normal vector, d is the distance between the ground plane and the origin of the laser radar coordinate system, assuming that the ground parameter PF:(n k ,d k ) corresponds to the kth LiDAR keyframe, each registered point belonging to the ground The constraint equation can be defined as: The front end can obtain the transformation from the kth lidar keyframe to the k+1th lidar keyframe, including the rotation part and pan part There are two parts, so the ground of the k+1th LiDAR keyframe parameter is the constraint to which each registration point belongs. The distance from the kth LiDAR keyframe to the ground plane can be defined as: When the robot moves in an outdoor environment, the ground plane usually changes, and different ground plane parameters need to be set. The judgment of different ground heights is as follows: where Δθ k,k+1 Represents the normal vector n k and n k+1 The angle between k,k+1 represents the height difference between adjacent lidar frames and the ground plane; If Δθ k,k+1 or Δd k,k+1 If the ground parameters observed in each keyframe are greater than the threshold, the two lidar keyframes are considered to belong to different ground surfaces. Finally, the deviation between the ground parameters observed in each keyframe and the estimated global ground surface is incorporated into the factor graph as the ground factor. The residual model between the two lidar keyframes can be calculated as: in PF k and PF W denote the ground plane of the kth lidar keyframe and the global ground plane, respectively, H(PF k ) are the ground plane parameters measured from the kth lidar keyframe in world coordinates; Step 4: Pre-integrate the data collected by the IMU and encoder respectively, build the IMU and encoder measurement model and pre-integration model, and build the IMU and encoder pre-integration factors respectively; Step 5: Remove the dynamic point cloud by calculating the spatiotemporal normal vector in the dedistorted point cloud frame obtained by the front end to obtain a point cloud frame with dedistorted dynamic points. The dynamic point cloud is removed as follows: Consider a dedistorted point cloud P without registered ground points in the world coordinate system i (i ∈ 1,...,n), its local covariance is: in Indicates P i The jth point in Indicates the timestamp of the association, represents the set of neighbor points, and Respectively The corresponding minimum eigenvector and minimum eigenvalue; Finally, dynamic scoring Defined as: A low dynamic score of a spatial point in the point cloud indicates that it is stationary or has a very low velocity in three-dimensional space; therefore, the point is classified as static or dynamic based on the dynamic score; Step 6: Extract key frames from the point cloud frames with distorted and dynamic points by using time and distance information, and combine the point cloud frames in step 3 with the key frames to construct a sub-graph; Step 7: Use the global descriptor Scan Context for loop closure detection, construct the loop closure factor, and correct the accumulated error; Step 8: Use graph optimization to jointly optimize the constructed factor residual term and constraint equation. Considering the case of wheel slippage, the encoder factor residual term is constrained by encoder pre-integration weight. The pose of the subgraph is optimized by establishing a nonlinear least squares objective function to obtain the final pose state and global static map. During the actual operation of the system, the wheels may slip on the smooth road surface, resulting in inaccurate yaw angle estimation and overall positioning deviation. In order to improve the reliability of encoder measurement, the encoder optimization weight is introduced to reduce the influence of encoder constraints when the wheels slip. as follows: in, and The moving distance from time k to time k+1 calculated for the left encoder and the right encoder respectively, b represents the wheelbase, and λ represents the maximum yaw angle increment threshold, which is set to 0.

17.

2. The ground-constrained multi-sensor fusion positioning and mapping method according to claim 1, characterized in that: The IMU and encoder pre-integration factors described in step 4 are constructed as follows: Raw accelerometer measurements from the IMU at time t and gyroscope measurements The measurement in the IMU coordinate system can be defined as: where a t and w t are the true values ​​of the gyroscope and accelerometer, respectively. and are the bias and white noise of the accelerometer, respectively, and and The bias and white noise of the gyroscope, the additive noise in the gyroscope and accelerometer and is modeled as Gaussian white noise, where σ a and σ w are the standard deviation of Gaussian white noise, the bias in the gyroscope and the accelerometer, respectively. and is modeled as a random walk whose derivative is a Gaussian distribution, σ ba and σ bw are the standard deviations of the Gaussian random walk, g W is the gravitational acceleration in the world coordinate system; The wheel encoder determines the shaft speed based on the pulses received by the microcontroller counter and can calculate the linear speed of the left and right rear wheels at time t. and for: in and are the measured speeds of the left and right rear wheels, and are the true values ​​of the left and right rear wheel speeds, r l and r r They are respectively represented by the radius of the left rear wheel and the radius of the right rear wheel, and They are and The corresponding zero-mean Gaussian white noise, therefore, the measurement model of the wheel encoder in the encoder coordinates is defined as: in and They represent the linear velocity and angular velocity of the robot at time t, b represents the wheelbase, express Zero-mean Gaussian white noise, express Zero-mean Gaussian white noise; The IMU pre-integration model is used to estimate the relative motion between the kth frame and the k+1th frame of the LiDAR, and is defined as follows: in and They are the displacement, velocity and attitude angle of IMU pre-integration, v k is the velocity integrated by the accelerometer during the last interval, Δt k,k+1 is the time interval from the kth frame to the k+1th frame, and are the measurement values ​​of the accelerometer and gyroscope in the IMU coordinate system at the kth frame, and It is divided into the true values ​​of the accelerometer and gyroscope in the IMU coordinate system at the kth frame, and are the noises of the accelerometer and gyroscope at the kth frame respectively; The encoder pre-integration of the increment between the kth and k+1th LiDAR frames is defined as follows: in and are the encoder pre-integrated position and velocity, is the posture from the encoder coordinate system to the world coordinate system at the kth frame, and The encoder measures the angular velocity and linear velocity calculated by the model at the kth frame respectively.

3. The ground-constrained multi-sensor fusion positioning and mapping method according to claim 2, characterized in that: The pose optimization method according to the graph described in step eight is as follows: First, according to the measurement models of IMU and encoder, the residual models of IMU and encoder pre-integration constraint equations are constructed respectively. Considering the IMU measurement between the kth and k+1th LiDAR key frames, the residual model of IMU pre-integration factor is as follows: in, is the residual term of the IMU pre-integration factor, is the rotation matrix from the world coordinate system to the IMU coordinate system at the kth frame, and are the displacements calculated by IMU integration in the world coordinate system at the k+1th frame and the kth frame, respectively, W is the gravitational acceleration value in the world coordinate system, Δt k,k+1 is the time interval from the kth frame to the k+1th frame, and are the velocities calculated by IMU integration in the world coordinate system at the k+1th frame and the kth frame, respectively. and are the attitude quaternions calculated by the IMU in the world coordinate system at the k+1th frame and the kth frame, respectively. and are the biases of the accelerometer and gyroscope at the k+1th frame respectively; In two consecutive LiDAR motion frames, the encoder pre-integration factor residual model is defined as follows: in is the encoder pre-integration factor residual term; If the correct loop constraint is formed, a pair of loop detection key frames x are obtained. i and x j , by i and x j Perform ICP matching. If the ICP fitting score is less than the threshold, the relative transformation can be obtained, and the residual model of the loop detection factor can be defined as: where s i,j is the scaling factor of the closed-loop factor between the state nodes i and j of the mobile robot, h ( · ) is the relative transformation between the above nodes, z i,j is the loop detection measurement value; By minimizing the sum of the IMU pre-integration factor residual, the encoder pre-integration factor residual, and the ground plane factor residual closed-loop residual, the optimized posture state X can be calculated as follows: Σ (·) The noise covariance matrix representing each residual model determines the uncertainty of each measurement.

Images