A vehicle positioning method based on LiDAR / IMU / UWB tightly coupled odometer

Abstract

The present invention discloses a vehicle positioning method based on LiDAR / IMU / UWB tightly coupled odometer technology. The method first pre-processes the original point cloud data, including point cloud distortion removal, ground and non-ground point cloud segmentation, thereby providing a basis for the pose estimation of the LiDAR key frame odometer and the subsequent point cloud mapping; then, a tightly coupled model is constructed, including a model based on IMU pre-integration and a model based on UWB distance observation, and a local factor graph is optimized, thereby providing a relatively accurate pose initial value for the LiDAR key frame odometer in an urban feature degradation environment. Finally, the LiDAR key frame is optimized, and the static edge point cloud is projected into the world coordinate system according to the locally optimized pose initial value, and the current pose is further optimized using the constraint formed by "edge point-straight line". The method has good positioning performance in a SLAM feature degradation environment and can achieve more accurate vehicle positioning.

Description

Technical Field

[0001] The present invention relates to a vehicle positioning method, in particular to a vehicle positioning method based on a LiDAR / IMU / UWB tightly coupled odometer, and belongs to the field of vehicle navigation and positioning. Background Art

[0002] LiDAR has the advantages of accurate distance measurement, being unaffected by changes in illumination, and its cost is also decreasing. Therefore, in the field of vehicle positioning, LiDAR-based SLAM technology has received more and more attention and applications. However, in complex urban environments, there are scenes with sparse features and severe homogeneity. Feature sparsity will cause LiDAR SLAM to have inaccurate pose estimation due to insufficient number of point clouds, while homogeneity will cause LiDAR SLAM to misidentify different scenes as the same scene, thus affecting subsequent pose estimation and map construction.

[0003] Therefore, SLAM technology that relies solely on LiDAR is no longer able to cope with complex urban environments, and the development of laser SLAM technology with multi-source information fusion has been widely recognized by researchers at home and abroad. Sensors commonly used to assist SLAM include inertial measurement units (IMUs) and wireless communication units. Among them, IMUs have the advantages of small size, high frequency, and low cost. They can obtain accurate posture information through integration in a short time. Integrating them into SLAM can improve the output frequency of the system state, and can also provide posture observation constraints to correct the problem of inaccurate posture estimation of LiDAR SLAM in complex urban environments to a certain extent. In addition, it can also remove the motion distortion of point clouds. Although LiDAR and IMU can complement each other through fusion, they are essentially based on recursive sensors. In LiDAR degraded environments (such as tunnels in urban roads, etc.), their positioning capabilities will still decline. Among wireless sensor units, UltraWide Band (UWB) sensors are widely used in the field of positioning due to their strong penetration, wide communication range, and high ranging accuracy. The sensor consists of a base station and a tag, and the tag estimates its own position by receiving the distance and position information from the base station. Within a good line of sight, UWB-based positioning is a centimeter-level positioning method without cumulative errors, which can effectively correct the errors of LiDAR / IMU in feature-degraded environments.

[0004] The present invention proposes a tightly coupled odometer technology based on LiDAR / IMU / UWB to solve the problem of inaccurate pose estimation of SLAM in a feature-degraded environment. First, the original point cloud is preprocessed, and a dynamic point cloud filtering method based on foreground point segmentation and intensity range filtering is adopted to achieve accurate dynamic point cloud recognition and filtering; then, local factor graph optimization is performed based on the IMU pre-integration model and the UWB distance observation model, thereby providing a relatively accurate initial estimate for the LiDAR keyframe odometer in a feature-degraded environment; finally, the LiDAR keyframe odometer will achieve pose optimization based on the initial estimate by constructing an "edge point-straight line" constraint to achieve a tightly coupled odometer output based on LiDAR / IMU / UWB. Summary of the invention

[0005] The purpose of this invention is to propose an odometer technology method based on LiDAR / IMU / UWB tight coupling. In view of the problem that the traditional LiDAR / IMU odometer technology method has the risk of failure when facing feature degradation scenarios, the advantages of UWB high ranging accuracy and no cumulative error are fully utilized, and the UWB observation factor is introduced into the LiDAR / IMU tight coupling framework. The combination of the three sensors will further improve the robustness and accuracy of the pose output.

[0006] The technical solution adopted by the present invention is as follows: a vehicle positioning method based on LiDAR / IMU / UWB tightly coupled odometer, firstly, preprocessing the original point cloud, using a dynamic point cloud filtering method based on foreground point segmentation and intensity range filtering; then, the observation model of the UWB observation value is constructed and the motion model constructed by IMU and LiDAR is optimized by factor graph; finally, the LiDAR key frame odometer pose optimization is realized by constructing the "edge point-straight line" constraint, thereby improving the positioning accuracy in the feature degraded environment. The specific steps include:

[0007] Step 1: Data preprocessing

[0008] The LiDAR used in the present invention is a 3D mechanical laser radar, and its characteristic of rotating scanning over time will cause point cloud distortion during motion. When the vehicle-mounted LiDAR undergoes rapid translation or rotation during the sampling period, the LiDAR coordinate system referenced by the point cloud will be inconsistent in space. It is generally believed that the point cloud coordinates of each frame are generated based on the same LiDAR coordinate system, so point cloud distortion will occur. Therefore, it is necessary to compensate for the distortion of the point cloud data. In reality, there are fewer scenes of uniform linear motion, and most movements are accompanied by speed changes and rotations, so linear interpolation is used to compensate the point cloud in real time. Since the IMU has high-frequency characteristics, its output frequency is one order of magnitude higher than that of the LiDAR, which enables its data to be aligned with the LiDAR point cloud on the time axis, and then the point cloud is compensated using the posture information after the IMU integration. The IMU is aligned at the initial moment of each frame of the point cloud, and the discrete incremental recursion is performed according to formulas (14)-(16), and finally the posture increment information relative to the starting moment of the current frame can be obtained. In order to more accurately compensate for the distortion of the current point cloud, the present invention combines p i The IMU pose increment information before and after is used to obtain T by linear interpolation. L (k,i), its expression is:

[0009]

[0010] Where, ΔT B (k, front) represents the time at the current point cloud p i The incremental pose of the previous and nearest IMU, ΔT B (k, back) represents the time at the current point cloud p i The incremental pose of the next and nearest IMU, t front and t back They represent the corresponding moments respectively, and t(k,i) represents the current point cloud p in the kth frame. i The moment.

[0011] The present invention introduces a random sampling consistency method to segment ground and non-ground point clouds in order to improve the processing efficiency of point cloud data. First, four points are randomly selected from the point cloud of the current frame to estimate the ground model. The ground model Ax+By+Cz+D=0 is established, and the three-dimensional coordinates of the three point clouds are respectively substituted into the plane equation to determine the coefficients A, B, C, and D. Then the distance from other point cloud points to the ground can be expressed as:

[0012]

[0013] Among them, (x i ,y i ,z i) is the point cloud p i The three-dimensional coordinates of .

[0014] Set the distance threshold to d 0 , if the distance d from the current point cloud to the plane i If it is less than or equal to the threshold, then p i After the traversal is completed, if the current number of "in-office points" is greater than the original number of "ground points", the current "in-office point" is updated to "ground point".

[0015] Repeat the above process and iterate continuously. When the final "ground point" meets the quantity requirement or reaches the maximum number of iterations, stop the iteration and mark the "ground point" at this time as the real ground point, and other points are marked as non-ground points.

[0016] Step 2: Tightly coupled model construction

[0017] (1) Establishing IMU pre-integration model

[0018] IMU can output three-axis acceleration and the three-axis angular velocity In practical applications, the measurement value of IMU will be affected by Gaussian white noise n and zero bias b, and its measurement model can be expressed as:

[0019]

[0020]

[0021] in, and They represent the angular velocity and acceleration of the IMU in the vehicle coordinate system, respectively, and b g and b a denote the random walk of angular velocity and acceleration respectively, n g and n a They represent the measurement noise of angular velocity and acceleration respectively, g represents the gravitational acceleration, and w B represents the true value of angular velocity, a W (t) represents the true value of acceleration.

[0022] In order to obtain the posture information at each moment, it is first necessary to establish a kinematic differential equation based on the IMU measurement information, which is expressed as follows:

[0023]

[0024] in, Represents the time derivative of the attitude quaternion from the world coordinate system to the vehicle body coordinate system, represents the Hamiltonian product (quaternion multiplication), represents the time derivative of velocity in the world coordinate system, aW represents the acceleration in the world coordinate system, Represents the time derivative of the position in the world coordinate system, v W Represents the speed in the world coordinate system;

[0025] Combining the above differential equations, the kinematic update equation of the IMU can be obtained as:

[0026]

[0027] in, Represents the attitude quaternion from the world coordinate system to the vehicle coordinate system at time j, represents the velocity in the world coordinate system at time j, represents the speed in the world coordinate system at time i, represents the acceleration of the vehicle coordinate system at time t, represents the position in the world coordinate system at time j, Represents the position in the world coordinate system at time i, Represents the velocity in the world coordinate system at time i;

[0028] In order to ensure the real-time performance of the factor graph model, the present invention uses the pose of the point cloud keyframe as the state node of the factor graph, and optimizes the factor graph only when the keyframe is reached. When the keyframe is not reached, the IMU data is continuously integrated. The present invention introduces an IMU pre-integration model, which is only related to the acceleration and angular velocity output by the IMU and does not depend on the node state, thereby avoiding re-integration. Its expression is:

[0029]

[0030]

[0031] In actual engineering, since the IMU measures digital signals, the discrete form of the pre-integration model is generally used:

[0032]

[0033] in, Denotes the vehicle coordinate system B at time k i The position below Denotes the vehicle coordinate system B at time k i The speed of Denotes the vehicle coordinate system B at time k i The attitude quaternion under the vehicle coordinate system is a, a represents the acceleration in the vehicle coordinate system, w represents the angular velocity in the vehicle coordinate system, and δt represents the time step;

[0034] w and a are generally obtained by the median integral method, and the expression is:

[0035]

[0036] in, represents the angular velocity of the vehicle coordinate system at time k, represents the acceleration of the vehicle coordinate system at time k, b g represents the angular velocity random walk, b a represents the accelerated random walk;

[0037] (2) Establishing a tightly coupled odometry model

[0038] The residual functions of IMU and UWB are established to constrain the node states in the factor graph. The residual function of the IMU pre-integration model is:

[0039]

[0040] Among them, r p represents the position error, r q represents the attitude error, r v represents the speed error, represents the accelerometer bias error, represents the gyroscope bias error, Represents the world coordinate system to the vehicle coordinate system B i The quaternion of

[0041] The tightly coupled UWB observation model uses distance information as the observation quantity, and its residual function is:

[0042]

[0043] in, represents the distance observation provided by the i-th UWB base station at time k, h UWB (X k ,X i ) represents the current state converted to the UWB coordinate system at time k and the position X of the i-th base station i The distance function between .

[0044] The present invention considers that the observation noise of each sensor conforms to the zero-mean Gaussian distribution. Therefore, after establishing the residual function of IMU pre-integration and UWB observation, it is converted into a least squares model for local factor graph optimization. The least squares model expression is:

[0045]

[0046] Among them, X * represents the pose to be optimized, f i represents the residual function in formulas (19) and (20), ∑i Represents the covariance of each sensor data.

[0047] The present invention optimizes the above model using the GTSAM tool library, which optimizes and solves the states in the factor graph based on the Bayesian tree. The Bayesian tree only updates the state nodes affected by the new observation factors, thereby improving the efficiency of the factor graph.

[0048] Step 3: Keyframe Optimization

[0049] After point cloud preprocessing and IMU / UWB local factor graph optimization, the non-static edge point cloud is projected into the world coordinate system according to the optimized pose, and the odometer pose is further optimized using the "edge point-straight line" constraint. In order to ensure the real-time performance of factor graph optimization, the present invention draws on the idea of ​​"key frame" in visual SLAM. When the translation or rotation of the mobile carrier exceeds the threshold th Trans and Rot When the translation or rotation does not exceed the threshold, the point cloud frame is regarded as a key frame and processed; if the translation or rotation does not exceed the threshold, the point cloud frame is not processed.

[0050] The edge point cloud projected to the world system at time k is marked as Then, with the local point cloud map M e Do matching to further optimize the pose. e Distance in space and time The edge point cloud in the closest key frame is composed of The corresponding key frame position is taken as the reference, and the distance threshold th radius To select other key frame point clouds in the world coordinate system, and together form M e In terms of time, The timestamp is taken as the reference, which is less than the time threshold th time The key frames are also added to M e Then, M e The edge point clouds in the voxel are downsampled to eliminate duplicate point clouds.

[0051] After building the local map M e After that, through KD tree in M e Find and Current point cloud The 5 nearest points And find the mean and covariance of these five points, and their expressions are:

[0052]

[0053] Combined with principal component analysis, if Ae The maximum eigenvalue of The second largest eigenvalue If it is more than 3 times of , then these 5 points are considered to constitute a line feature. The corresponding eigenvector α 1 is the direction vector of the line feature. Two representative “virtual” point clouds are extracted from the line feature, namely:

[0054]

[0055]

[0056] use The current point cloud in and "virtual" point clouds To establish the distance residual equation, its expression is:

[0057]

[0058] Construct the residual equation of all edge point clouds in the key frame at time k about their corresponding straight line features, and establish the least squares model as follows:

[0059]

[0060] Among them, n represents the total number of constraints formed by edge point clouds and line features in the current key frame.

[0061] The present invention obtains the optimized posture by solving the Gauss-Newton method, and the constraint factor of the inter-frame LiDAR odometer can be expressed as:

[0062]

[0063] in, Indicates the optimized pose corresponding to the previous key frame.

[0064] At this time, the residual function of the laser odometer is:

[0065]

[0066] Among them, h LiDAR (X k-1 ,X k ) represents the pose change of adjacent state nodes in the factor graph before LiDAR odometry optimization.

[0067] After establishing the residual function of the LiDAR odometer, it is added to the factor graph optimization model of formula (21) to realize the odometer pose estimation based on LiDAR / IMU / UWB tight coupling.

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

[0069] (1) High positioning accuracy. Compared with the traditional LiDAR / IMU fusion method, the method of the present invention introduces UWB observations, which can achieve more accurate pose estimation in a feature-degraded environment. In addition, the UWB introduced by the method of the present invention has strong anti-interference ability and better positioning robustness.

[0070] (2) Good real-time performance. Compared with the method of directly using factor graph optimization, the present invention adopts a method of optimizing key frames to ensure the real-time performance of optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is a technical block diagram of the odometer based on tight coupling of LiDAR / IMU / UWB. DETAILED DESCRIPTION

[0072] LiDAR has the advantages of accurate distance measurement, being unaffected by changes in illumination, and its cost is also decreasing. Therefore, in the field of vehicle positioning, LiDAR-based SLAM technology has received more and more attention and applications. However, in complex urban environments, there are scenes with sparse features and severe homogeneity. Feature sparsity will cause LiDAR SLAM to have inaccurate pose estimation due to insufficient number of point clouds, while homogeneity will cause LiDAR SLAM to misidentify different scenes as the same scene, thus affecting subsequent pose estimation and map construction.

[0073] Therefore, SLAM technology that relies solely on LiDAR is no longer able to cope with complex urban environments, and the development of laser SLAM technology with multi-source information fusion has been widely recognized by researchers at home and abroad. Sensors commonly used to assist SLAM include inertial measurement units (IMUs) and wireless communication units. Among them, IMUs have the advantages of small size, high frequency, and low cost. They can obtain accurate posture information through integration in a short time. Integrating them into SLAM can improve the output frequency of the system state, and can also provide posture observation constraints to correct the problem of inaccurate posture estimation of LiDAR SLAM in complex urban environments to a certain extent. In addition, it can also remove the motion distortion of point clouds. Although LiDAR and IMU can complement each other through fusion, they are essentially based on recursive sensors. In LiDAR degraded environments (such as tunnels in urban roads, etc.), their positioning capabilities will still decline. Among wireless sensor units, UltraWide Band (UWB) sensors are widely used in the field of positioning due to their strong penetration, wide communication range, and high ranging accuracy. The sensor consists of a base station and a tag, and the tag estimates its own position by receiving the distance and position information from the base station. Within a good line of sight, UWB-based positioning is a centimeter-level positioning method without cumulative errors, which can effectively correct the errors of LiDAR / IMU in feature-degraded environments.

[0074] The present invention proposes a tightly coupled odometer technology based on LiDAR / IMU / UWB to solve the problem of inaccurate pose estimation of SLAM in a feature-degraded environment. First, the original point cloud is preprocessed, and a dynamic point cloud filtering method based on foreground point segmentation and intensity range filtering is adopted to achieve accurate dynamic point cloud recognition and filtering; then, local factor graph optimization is performed based on the IMU pre-integration model and the UWB distance observation model, thereby providing a relatively accurate initial estimate for the LiDAR keyframe odometer in a feature-degraded environment; finally, the LiDAR keyframe odometer will achieve pose optimization based on the initial estimate by constructing an "edge point-straight line" constraint to achieve a tightly coupled odometer output based on LiDAR / IMU / UWB.

[0075] The purpose of this invention is to propose an odometer technology method based on LiDAR / IMU / UWB tight coupling. In view of the problem that the traditional LiDAR / IMU odometer technology method has the risk of failure when facing feature degradation scenes, the advantages of UWB high ranging accuracy and no cumulative error are fully utilized, and the UWB observation factor is introduced into the LiDAR / IMU tight coupling framework. The combination of the three sensors will further improve the robustness and accuracy of the pose output. The specific steps include:

[0076] Step 1: Data preprocessing

[0077] The LiDAR used in the present invention is a 3D mechanical laser radar, and its characteristic of rotating scanning over time will cause point cloud distortion during motion. When the vehicle-mounted LiDAR undergoes rapid translation or rotation during the sampling period, the LiDAR coordinate system referenced by the point cloud will be inconsistent in space. It is generally believed that the point cloud coordinates of each frame are generated based on the same LiDAR coordinate system, so point cloud distortion will occur. Therefore, it is necessary to compensate for the distortion of the point cloud data. In reality, there are fewer scenes of uniform linear motion, and most movements are accompanied by speed changes and rotations, so linear interpolation is used to compensate the point cloud in real time. Since the IMU has high-frequency characteristics, its output frequency is one order of magnitude higher than that of the LiDAR, which enables its data to be aligned with the LiDAR point cloud on the time axis, and then the point cloud is compensated using the posture information after the IMU integration. The IMU is aligned at the initial moment of each frame of the point cloud, and the discrete incremental recursion is performed according to formulas (14)-(16), and finally the posture increment information relative to the starting moment of the current frame can be obtained. In order to more accurately compensate for the distortion of the current point cloud, the present invention combines p i The IMU pose increment information before and after is used to obtain T by linear interpolation. L (k,i), its expression is:

[0078]

[0079] Where, ΔT B (k, front) represents the time at the current point cloud p i The incremental pose of the previous and nearest IMU, ΔT B (k, back) represents the time at the current point cloud p i The incremental pose of the next and nearest IMU, t front and t back They represent the corresponding moments respectively, and t(k,i) represents the current point cloud p in the kth frame. i The moment.

[0080] The present invention introduces a random sampling consistency method to segment ground and non-ground point clouds in order to improve the processing efficiency of point cloud data. First, four points are randomly selected from the point cloud of the current frame to estimate the ground model. The ground model Ax+By+Cz+D=0 is established, and the three-dimensional coordinates of the three point clouds are respectively substituted into the plane equation to determine the coefficients A, B, C, and D. Then the distance from other point cloud points to the ground can be expressed as:

[0081]

[0082] Among them, (xi ,y i ,z i ) is the point cloud p i The three-dimensional coordinates of .

[0083] Set the distance threshold to d 0 , if the distance d from the current point cloud to the plane i If it is less than or equal to the threshold, then p i After the traversal is completed, if the current number of "in-office points" is greater than the original number of "ground points", the current "in-office point" is updated to "ground point".

[0084] Repeat the above process and iterate continuously. When the final "ground point" meets the quantity requirement or reaches the maximum number of iterations, stop the iteration and mark the "ground point" at this time as the real ground point, and other points are marked as non-ground points.

[0085] Step 2: Tightly coupled model construction

[0086] (1) Establishing IMU pre-integration model

[0087] IMU can output three-axis acceleration and the three-axis angular velocity In practical applications, the measurement value of IMU will be affected by Gaussian white noise n and zero bias b, and its measurement model can be expressed as:

[0088]

[0089]

[0090] in, and They represent the angular velocity and acceleration of the IMU in the vehicle coordinate system, respectively, and b g and b a denote the random walk of angular velocity and acceleration respectively, n g and n a They represent the measurement noise of angular velocity and acceleration respectively, g represents the gravitational acceleration, Represents the attitude quaternion from the world coordinate system to the vehicle coordinate system, w B represents the true value of angular velocity, a W (t) represents the true value of acceleration;

[0091] In order to obtain the posture information at each moment, it is first necessary to establish a kinematic differential equation based on the IMU measurement information, which is expressed as follows:

[0092]

[0093] in, Represents the time derivative of the attitude quaternion from the world coordinate system to the vehicle body coordinate system, represents the Hamiltonian product (quaternion multiplication), represents the time derivative of velocity in the world coordinate system, a W represents the acceleration in the world coordinate system, Represents the time derivative of the position in the world coordinate system, v W Represents the speed in the world coordinate system;

[0094] Combining the above differential equations, the kinematic update equation of the IMU can be obtained as:

[0095]

[0096] in, Represents the attitude quaternion from the world coordinate system to the vehicle coordinate system at time j, represents the velocity in the world coordinate system at time j, represents the speed in the world coordinate system at time i, represents the acceleration of the vehicle coordinate system at time t, represents the position in the world coordinate system at time j, Represents the position in the world coordinate system at time i, Represents the velocity in the world coordinate system at time i;

[0097] In order to ensure the real-time performance of the factor graph model, the present invention uses the pose of the point cloud keyframe as the state node of the factor graph, and optimizes the factor graph only when the keyframe is reached. When the keyframe is not reached, the IMU data is continuously integrated. The present invention introduces an IMU pre-integration model, which is only related to the acceleration and angular velocity output by the IMU and does not depend on the node state, thereby avoiding re-integration. Its expression is:

[0098]

[0099] In actual engineering, since the IMU measures digital signals, the discrete form of the pre-integration model is generally used:

[0100]

[0101] in, Denotes the vehicle coordinate system B at time k i The position below Denotes the vehicle coordinate system B at time k i The speed of Denotes the vehicle coordinate system B at time k i The attitude quaternion under the vehicle coordinate system is a, a represents the acceleration in the vehicle coordinate system, w represents the angular velocity in the vehicle coordinate system, and δt represents the time step;

[0102] w and a are generally obtained by the median integral method, and the expression is:

[0103]

[0104] in, represents the angular velocity of the vehicle coordinate system at time k, represents the acceleration of the vehicle coordinate system at time k, b g represents the angular velocity random walk, b a represents the accelerated random walk;

[0105] (2) Establishing a tightly coupled odometry model

[0106] The residual functions of IMU and UWB are established to constrain the node states in the factor graph. The residual function of the IMU pre-integration model is:

[0107]

[0108] Among them, r p represents the position error, r q represents the attitude error, r v represents the speed error, represents the accelerometer bias error, represents the gyroscope bias error, Represents the world coordinate system to the vehicle coordinate system B i The quaternion of

[0109] The tightly coupled UWB observation model uses distance information as the observation quantity, and its residual function is:

[0110]

[0111] in, represents the distance observation provided by the i-th UWB base station at time k, h UWB (X k ,X i ) represents the current state converted to the UWB coordinate system at time k and the position X of the i-th base station i The distance function between .

[0112] The present invention considers that the observation noise of each sensor conforms to the zero-mean Gaussian distribution. Therefore, after establishing the residual function of IMU pre-integration and UWB observation, it is converted into a least squares model for local factor graph optimization. The least squares model expression is:

[0113]

[0114] Among them, X *represents the pose to be optimized, f i represents the residual function in formulas (19) and (20), ∑ i Represents the covariance of each sensor data.

[0115] The present invention optimizes the above model using the GTSAM tool library, which optimizes and solves the states in the factor graph based on the Bayesian tree. The Bayesian tree only updates the state nodes affected by the new observation factors, thereby improving the efficiency of the factor graph.

[0116] Step 3: Keyframe Optimization

[0117] After point cloud preprocessing and IMU / UWB local factor graph optimization, the non-static edge point cloud is projected into the world coordinate system according to the optimized pose, and the odometer pose is further optimized using the "edge point-straight line" constraint. In order to ensure the real-time performance of factor graph optimization, the present invention draws on the idea of ​​"key frame" in visual SLAM. When the translation or rotation of the mobile carrier exceeds the threshold th Trans and Rot When the translation or rotation does not exceed the threshold, the point cloud frame is regarded as a key frame and processed; if the translation or rotation does not exceed the threshold, the point cloud frame is not processed.

[0118] The edge point cloud projected to the world system at time k is marked as Then, with the local point cloud map M e Do matching to further optimize the pose. e Distance in space and time The edge point cloud in the closest key frame is composed of The corresponding key frame position is taken as the reference, and the distance threshold th radius To select other key frame point clouds in the world coordinate system, and together form M e In terms of time, The timestamp is taken as the reference, which is less than the time threshold th time The key frames are also added to M e Then, M e The edge point clouds in the voxel are downsampled to eliminate duplicate point clouds.

[0119] After building the local map M e After that, through KD tree in M e Find and Current point cloud The 5 nearest points And find the mean and covariance of these five points, and their expressions are:

[0120]

[0121] Combined with principal component analysis, if A e The maximum eigenvalue of The second largest eigenvalue If it is more than 3 times of , then these 5 points are considered to constitute a line feature. The corresponding eigenvector α 1 is the direction vector of the line feature. Two representative “virtual” point clouds are extracted from the line feature, namely:

[0122]

[0123]

[0124] use The current point cloud in and "virtual" point clouds To establish the distance residual equation, its expression is:

[0125]

[0126] Construct the residual equation of all edge point clouds in the key frame at time k about their corresponding straight line features, and establish the least squares model as follows:

[0127]

[0128] Among them, n represents the total number of constraints formed by edge point clouds and line features in the current key frame.

[0129] The present invention obtains the optimized posture by solving the Gauss-Newton method, and the constraint factor of the inter-frame LiDAR odometer can be expressed as:

[0130]

[0131] in, Indicates the optimized pose corresponding to the previous key frame.

[0132] At this time, the residual function of the laser odometer is:

[0133]

[0134] Among them, h LiDAr (X k-1 ,X k ) represents the pose change of adjacent state nodes in the factor graph before LiDAR odometry optimization.

[0135] After establishing the residual function of the LiDAR odometer, it is added to the factor graph optimization model of formula (21) to realize the odometer pose estimation based on LiDAR / IMU / UWB tight coupling.

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

[0137] (1) High positioning accuracy. Compared with the traditional LiDAR / IMU fusion method, the method of the present invention introduces UWB observations, which can achieve more accurate pose estimation in a feature-degraded environment. In addition, the UWB introduced by the method of the present invention has strong anti-interference ability and better positioning robustness.

[0138] (2) Good real-time performance. Compared with the method of directly using factor graph optimization, the present invention adopts a method of optimizing key frames to ensure the real-time performance of optimization.

Claims

1. A vehicle positioning method based on LiDAR / IMU / UWB tightly coupled odometer, characterized by: First, the original point cloud is preprocessed, and a dynamic point cloud filtering method based on foreground point segmentation and intensity range filtering is adopted. Then, the observation model of UWB observations and the motion model constructed by IMU and LiDAR are optimized by factor graph. Finally, the LiDAR key frame odometer pose optimization is realized by constructing "edge point-straight line" constraints, thereby improving the positioning accuracy in feature-degraded environments. The specific steps include: Step 1: Data preprocessing Combined with p i The IMU pose increment information before and after is used to obtain T by linear interpolation. L (k,i), its expression is: Where, ΔT B (k, front) represents the time at the current point cloud p i The incremental pose of the previous and nearest IMU, ΔT B (k, back) represents the time at the current point cloud p i The incremental pose of the next and nearest IMU, t front and t back They represent their corresponding moments respectively, and t(k,i) represents the current point cloud p in the kth frame. i The moment; The random sampling consistency method is introduced to segment the ground and non-ground point clouds in order to improve the processing efficiency of point cloud data. First, four points are randomly selected from the point cloud of the current frame to estimate the ground model, and the ground model Ax+By+Cz+S=0 is established. The three-dimensional coordinates of the three point clouds are respectively substituted into the plane equation to determine the coefficients A, B, C, and D. The distance from other point cloud points to the ground is expressed as: Among them, (x i ,y i ,z i ) is the point cloud p i The three-dimensional coordinates of Set the distance threshold to d0. If the distance from the current point cloud to the plane is d i If it is less than or equal to the threshold, then p i After the traversal is completed, if the current number of "in-office points" is greater than the original number of "ground points", the current "in-office point" is updated to "ground point"; Repeat the above process and iterate continuously. When the final "ground point" meets the quantity requirement or reaches the maximum number of iterations, stop the iteration and mark the "ground point" at this time as the real ground point, and other points are marked as non-ground points; Step 2: Tightly coupled model construction (1) Establishing IMU pre-integration model IMU can output three-axis acceleration and the three-axis angular velocity In practical applications, the measurement value of IMU will be affected by Gaussian white noise n and zero bias b, and its measurement model is expressed as: in, and They represent the angular velocity and acceleration of the IMU in the vehicle coordinate system, respectively, and b g and b a denote the random walk of angular velocity and acceleration respectively, n g and n a They represent the measurement noise of angular velocity and acceleration respectively, g represents the gravitational acceleration, Represents the attitude quaternion from the world coordinate system to the vehicle coordinate system, w B represents the true value of angular velocity, a W (t) represents the true value of acceleration; In order to obtain the posture information at each moment, it is first necessary to establish a kinematic differential equation based on the IMU measurement information, which is expressed as follows: in, Represents the time derivative of the attitude quaternion from the world coordinate system to the vehicle body coordinate system, represents the Hamiltonian product (quaternion multiplication), represents the time derivative of velocity in the world coordinate system, a W represents the acceleration in the world coordinate system, Represents the time derivative of the position in the world coordinate system, v W Represents the speed in the world coordinate system; Combining the above differential equations, the kinematic update equation of the IMU is obtained as follows: in, Represents the attitude quaternion from the world coordinate system to the vehicle coordinate system at time j, represents the velocity in the world coordinate system at time j, represents the speed in the world coordinate system at time i, represents the acceleration of the vehicle coordinate system at time t, represents the position in the world coordinate system at time j, Represents the position in the world coordinate system at time i, Represents the velocity in the world coordinate system at time i; In order to ensure the real-time performance of the factor graph model, the pose of the point cloud key frame is used as the state node of the factor graph. The factor graph is optimized only when the key frame is reached. When the key frame is not reached, the IMU data is continuously integrated. The IMU pre-integration model is introduced, which is only related to the acceleration and angular velocity output by the IMU and does not depend on the node state, thereby avoiding re-integration. Its expression is: in, Denotes the vehicle coordinate system B at time k i The position below Denotes the vehicle coordinate system B at time k i The speed of Table 1. Vehicle coordinate system B at the kth moment i The attitude quaternion under the vehicle coordinate system is a, a represents the acceleration in the vehicle coordinate system, w represents the angular velocity in the vehicle coordinate system, and δt represents the time step; w and a are obtained by the median integral method, and the expressions are: in, represents the angular velocity of the vehicle coordinate system at time k, represents the acceleration of the vehicle coordinate system at time k, b g represents the angular velocity random walk, b a represents the accelerated random walk; (2) Establishing a tightly coupled odometry model The residual functions of IMU and UWB are established to constrain the node states in the factor graph. The residual function of the IMU pre-integration model is: Among them, r p represents the position error, r q represents the attitude error, r v represents the speed error, represents the accelerometer bias error, represents the gyroscope bias error, Represents the world coordinate system to the vehicle body coordinate system B i The quaternion of The tightly coupled UWB observation model uses distance information as the observation quantity, and its residual function is: in, represents the distance observation provided by the i-th UWB base station at time k, h UWB (X k ,X i ) represents the current state converted to the UWB coordinate system at time k and the position X of the i-th base station i The distance function between The observation noise of each sensor conforms to the zero-mean Gaussian distribution. After establishing the residual function of IMU pre-integration and UWB observation, it is converted into a least squares model for local factor graph optimization. The expression of the least squares model is: Among them, X * represents the pose to be optimized, f i represents the residual function in formulas (19) and (20), ∑ i Represents the covariance of each sensor data; Step 3: Keyframe Optimization When the translation or rotation of the moving carrier exceeds the threshold th Trans and Rot When , the point cloud frame at this time is regarded as a key frame and optimized; if the translation or rotation does not exceed the threshold, the point cloud frame is not optimized; The edge point cloud projected to the world system at time k is marked as Then, with the local point cloud map M e Do matching to further optimize the pose; M e Distance in space and time The edge point cloud in the closest key frame is composed of The corresponding key frame position is taken as the reference, and the distance threshold th radius To select other key frame point clouds in the world coordinate system, and together form M e In terms of time, The timestamp is taken as the reference, which is less than the time threshold th time The key frames are also added to M e Then, to M e The edge point cloud in the voxel is downsampled to eliminate duplicate point clouds; After building the local map M e After that, through KD tree in M e Find and Current point cloud The 5 nearest points And find the mean and covariance of these five points, and their expressions are: Combined with principal component analysis, if A e The maximum eigenvalue of The second largest eigenvalue If it is more than 3 times of , then these 5 points are considered to constitute a line feature. The corresponding feature vector α1 is the direction vector of the line feature; two representative "virtual" point clouds are extracted from the line feature, namely: use The current point cloud in and "virtual" point clouds To establish the distance residual equation, its expression is: Construct the residual equation of all edge point clouds in the key frame at time k about their corresponding straight line features, and establish the least squares model as follows: Where n represents the total number of constraints formed by edge point clouds and line features in the current keyframe; The Gauss-Newton method is used to solve the optimized pose, and the constraint factor of the inter-frame LiDAR odometer is expressed as: in, Indicates the optimized pose corresponding to the previous key frame; At this time, the residual function of the laser odometer is: Among them, h LiDAR (X k-1 ,X k ) represents the pose change of adjacent state nodes in the factor graph before LiDAR odometer optimization; After establishing the residual function of LiDAR odometer, it is added to the factor graph optimization model to realize odometer pose estimation based on LiDAR / IMU / UWB tight coupling.

Images