Multi-sensor fusion positioning method suitable for vehicle limiting working condition

Abstract

The invention discloses a multi-sensor fusion positioning method suitable for a vehicle under an extreme working condition, and belongs to the technical field of fusion sensing positioning. According to the method, the correction amount is calculated after vehicle steering judgment, then the transverse speed of the wheel speed meter data is corrected according to the correction amount, and the front-end odometer which is more accurate than the front-end odometer which is obtained by directly using inertial navigation for integration is calculated through Kalman filtering according to the inertial navigation data and the corrected wheel speed meter data. The method comprises the following steps: carrying out point cloud distortion removal on laser radar data by using an obtained front-end odometer, then carrying out point cloud matching to obtain a point cloud matching pose, eliminating a position error of point cloud matching by using GPS pose position information to obtain a relatively accurate rear-end odometer, and finally carrying out rear-end optimization on the front-end odometer and the rear-end odometer to obtain a positioning result. According to the method, a relatively accurate positioning result can be obtained under the sudden turning condition of the vehicle, and the positioning under the daily vehicle working condition is not influenced.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving fusion positioning, and specifically relates to a multi-sensor fusion positioning method suitable for vehicle extreme working conditions. Background Art

[0002] Fusion positioning technology is a key technology for autonomous vehicles to achieve autonomous navigation. High-precision positioning is a prerequisite for efficient path planning and environmental perception of autonomous vehicles. In order to achieve high-precision multi-sensor fusion positioning, people have made a lot of efforts. The main methods are: (1) Directly calculate the track through inertial navigation to obtain the front-end odometer, and then use the front-end odometer to match the laser radar point cloud to obtain the pose for fusion positioning. (2) First fuse the wheel speed meter information with the inertial navigation information to obtain the front-end odometer, and then use the front-end odometer to match the laser radar point cloud to obtain the pose for fusion positioning.

[0003] However, the existing technology has the following problems: (1) It ignores the problem of large divergence of inertial navigation measurement errors, which easily leads to positioning drift and large errors in the front-end odometer. (2) Because the wheel speedometer can only collect the vehicle's front wheel angle information and the speed information in the forward direction, when the vehicle makes a sharp turn, the error of the front-end odometer becomes larger, which in turn leads to a larger final positioning error. Therefore, the problem that the wheel speedometer can only collect the longitudinal speed of the vehicle, resulting in large lateral positioning errors under sharp turns, needs to be solved urgently. Summary of the invention

[0004] In order to solve the above technical problems, the present invention provides a multi-sensor fusion positioning method suitable for vehicle extreme working conditions.

[0005] To implement the above technical solution, the details are as follows:

[0006] A multi-sensor fusion positioning method suitable for vehicle extreme working conditions includes the following steps:

[0007] S1. Determine whether the vehicle is making a sharp turn by calculating the vehicle heading angle and the front wheel turning angle. The specific steps are as follows:

[0008] S1.1. The vehicle heading angle is obtained by integrating the angular velocity of the inertial navigation system around the vertical axis over time. The expression is as follows:

[0009]

[0010] In the formula, is the vehicle heading angle; is the angular velocity of the inertial navigation system around the vertical axis; Indicates sampling time;

[0011] S1.2. Calculate the front wheel steering angle increment; the expression is as follows:

[0012]

[0013] In the formula, is the front wheel turning angle increment; is the kth sampling angle, k∈(i,j), i represents the first sampling point, and j represents the last sampling point;

[0014] S1.3, by comparing the changes in the vehicle heading angle and the front wheel turning angle with a preset threshold, determining whether the vehicle is making a sharp turn;

[0015] S2. When the vehicle makes a sharp turn, the lateral speed correction value of the wheel speed meter data is obtained by correcting the vehicle heading angle and the front wheel turning angle when the vehicle makes a sharp turn. The specific steps are as follows:

[0016] S2.1. Calculate the lateral velocity correction. The expression is as follows:

[0017]

[0018] In the formula, is the lateral velocity correction; is the correction proportional factor; if it is determined that the vehicle is not in a steering state, then =0; It represents the vehicle turning angle calculated from the angular velocity measured by inertial navigation; The front wheel turning angle measured by the vehicle's wheel speedometer;

[0019] S2.2. Correct the lateral speed of the wheel speed meter data based on the calculated lateral speed correction value; the details are as follows:

[0020] The wheel speed meter data expression is as follows:

[0021]

[0022] In the formula, The forward speed of the vehicle measured by the wheel speed meter;

[0023] Furthermore, the wheel speed meter data expression after lateral speed correction is as follows:

[0024] ;

[0025] S3, obtain the wheel speed meter data through the vehicle heading angle, the front wheel steering angle (longitudinal data) and the lateral speed correction value of the wheel speed meter data (lateral data), and perform Kalman filter calculation on the inertial navigation data and the wheel speed meter data to obtain the front-end odometer; the steps are as follows:

[0026] S3.1. Calculate the predicted position and attitude of the inertial navigation system; the expression is as follows:

[0027]

[0028] In the formula, , and Respectively expressed in The rotation, displacement and velocity at the moment, i.e. the predicted position and posture of the inertial navigation system; , and Respectively expressed in Rotation, displacement and velocity at each moment; , Respectively Angular velocity and acceleration at the moment; , They represent the angular velocity bias error and acceleration bias error of the inertial navigation system respectively; represents the acceleration due to gravity;

[0029] S3.2, calculate the prediction covariance;

[0030]

[0031] In the formula, is the prediction covariance; is the Jacobian matrix in the prediction process, which reflects the partial derivatives of the inertial navigation kinematics model to the motion state; is the Jacobian matrix of the last prediction, To predict noise;

[0032] S3.3. Calculate the Kalman filter gain by predicting the covariance; the expression is as follows:

[0033]

[0034] In the formula, is the Kalman filter gain, is the Jacobian matrix of the observation equation;

[0035] S3.4. Calculate the Kalman filter error; the expression is as follows:

[0036]

[0037] In the formula, is the Kalman filter error, is the predicted value of posture;

[0038] S3.5. Calculate the front-end odometer by calculating the Kalman filter error; the expression is as follows:

[0039]

[0040] In the formula, It is the front-end odometer; is the predicted position and attitude of the inertial navigation;

[0041] S4. Use the obtained front-end odometer to perform point cloud dedistortion on the lidar data, and then perform point cloud matching to obtain the point cloud matching pose. The specific steps are as follows:

[0042] S4.1. Use the front-end odometer to measure each point in the point cloud frame Perform point cloud compensation and point cloud dedistortion processing; the expression is as follows:

[0043]

[0044] In the formula, Represents the external parameters from the laser radar coordinate system to the inertial navigation coordinate system; Represents the transformation matrix from time tj to end time tk in the inertial navigation coordinate system; represents the point cloud after dedistortion;

[0045] S4.2. Calculate the matching pose of the laser radar point cloud; the expression is as follows:

[0046]

[0047] In the formula, Represents the calculated point cloud matching pose; Indicates the point cloud matching pose at the previous moment; the superscript T indicates the matrix transformation rank; Indicates the predicted pose at the current moment;

[0048] S5. Use GPS position information to eliminate the position error of point cloud matching and obtain an accurate backend odometer. The steps are as follows:

[0049] S5.1. Time-synchronize the point cloud data frame located by the laser radar with the GPS data to obtain the GPS data at the corresponding time of the point cloud matching posture, i.e., the GPS data after time synchronization;

[0050] S5.2. Convert the latitude and longitude coordinate system of the time-synchronized GPS data to the Mercator coordinate system, where:

[0051] Mercator coordinate system X = longitude * earth radius * 0.0174;

[0052] Mercator coordinate system Y = ln(tan((90+latitude)*0.00872))*earth radius;

[0053] The longitude and latitude here are the real-time longitude and latitude of the GPS data after time synchronization, and the radius of the earth is 6371.393 kilometers;

[0054] S5.3, align the X,Y coordinate values ​​of the Mercator coordinate system with the origin of the global coordinate system;

[0055] S5.4, aligning the X,Y coordinate values ​​of the Mercator coordinate system with the trajectory of the global coordinate system;

[0056] S5.5. Use the Mercator coordinate system converted from GPS to replace the position data of the point cloud matching, and retain the posture data of the point cloud matching to obtain an accurate backend odometer;

[0057] S6. Optimize the front-end odometer and the back-end odometer to obtain the optimization objective function of the final posture; the optimization objective function is as follows:

[0058]

[0059] In the formula, Represents the sum of the squares of the front-end odometer residual and the back-end odometer residual, which is the amount to be optimized when the objective function is minimized. , which is the positioning result.

[0060] Beneficial effects of the present invention:

[0061] (1) The present invention improves the lateral positioning accuracy during sharp turns. By judging the steering situation of the vehicle and correcting the lateral speed, the front-end odometer is ensured to maintain high accuracy and stability under complex working conditions.

[0062] (2) The present invention enhances the stability of the LiDAR point cloud data and uses the front-end odometer information to correct the oscillating point cloud, thereby improving the point cloud matching accuracy.

[0063] (3) The present invention uses GPS data to correct the laser radar point cloud matching results, comprehensively optimizes multi-sensor fusion positioning, and improves the navigation capability and safety of autonomous driving vehicles under various working conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 is a flow chart of the present invention;

[0065] Figure 2 is a diagram of the course angle change of the present invention;

[0066] Figure 3 It is the front wheel turning angle increment diagram of the present invention.

[0067] Figure 4 It is a comparison diagram before and after the lateral speed correction of the present invention;

[0068] Figure 5 It is a principle diagram of the multi-sensor fusion positioning method applicable to vehicle extreme working conditions according to the present invention;

[0069] Figure 6 This is a schematic diagram of the multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to the present invention. DETAILED DESCRIPTION

[0070] In order to better illustrate the purpose, technical solutions and advantages of the present invention, the present invention will be further described below in conjunction with specific embodiments.

[0071] This embodiment takes an unmanned logistics vehicle as an example of a sharp turn with a turning radius of 1 meter, using a wheel speed meter, inertial navigation, laser radar, and GPS positioning, as follows:

[0072] like Figure 1 As shown, a multi-sensor fusion positioning method suitable for vehicle extreme working conditions includes the following steps:

[0073] S1. Determine whether the vehicle is making a sharp turn by calculating the vehicle heading angle and the front wheel turning angle. The specific steps are as follows:

[0074] S1.1. The vehicle heading angle is obtained by integrating the angular velocity of the inertial navigation system around the vertical axis over time. The expression is as follows:

[0075]

[0076] In the formula, is the vehicle heading angle; is the angular velocity of the inertial navigation system around the vertical axis, and the vehicle heading angle change corresponds to Figure 2 The curve of Indicates the sampling time. In this example, 0.01 seconds;

[0077] S1.2. Calculate the front wheel steering angle increment; the expression is as follows:

[0078]

[0079] In the formula, is the front wheel turning angle increment; is the kth sampling angle, k∈(i, j), i represents the first sampling point, j represents the last sampling point. In this embodiment, i=1, j=10, and the front wheel steering angle increment corresponds to Figure 3 The curve of

[0080] S1.3, by comparing the changes in the vehicle heading angle and the front wheel turning angle with a preset threshold, determining whether the vehicle is making a sharp turn;

[0081] pass Figure 2It can be seen that the vehicle heading angle changes significantly around 0.002°. At this time, the front wheel steering angle threshold and the wheel heading angle threshold are both set to 0.002. Figure 2 It can be seen that at the 42nd second, the vehicle made a sharp turn;

[0082] S2. When the vehicle makes a sharp turn, the lateral speed correction value of the wheel speed meter data is obtained by correcting the vehicle heading angle and the front wheel turning angle when the vehicle makes a sharp turn. The specific steps are as follows:

[0083] S2.1. Calculate the lateral velocity correction. The expression is as follows:

[0084]

[0085] In the formula, is the lateral velocity correction; is the correction proportional factor, where S is taken as the empirical value of 100, S is the empirical value; δ is calculated in real time; It represents the vehicle turning angle calculated from the angular velocity measured by inertial navigation; The front wheel turning angle measured by the vehicle's wheel speedometer; and Known, then can be calculated in real time; if it is determined that the vehicle is not in a turning state, =0; calculate the lateral speed correction , we get the comparison diagram of lateral velocity before and after correction as shown in Figure 4 As shown;

[0086] S2.2. Correct the lateral speed of the wheel speed meter data based on the calculated lateral speed correction value; the details are as follows:

[0087] The wheel speed meter data expression is as follows:

[0088]

[0089] In the formula, The forward speed of the vehicle measured by the wheel speed meter;

[0090] Furthermore, the wheel speed meter data expression after lateral speed correction is as follows:

[0091] ;

[0092] S3, obtain the wheel speed meter data through the vehicle heading angle, the front wheel steering angle (longitudinal data) and the lateral speed correction value of the wheel speed meter data (lateral data), and perform Kalman filter calculation on the inertial navigation data and the wheel speed meter data to obtain the front-end odometer; the steps are as follows:

[0093] S3.1. Calculate the predicted position and attitude of the inertial navigation system; the expression is as follows:

[0094]

[0095] In the formula, , and Respectively expressed in The rotation, displacement and velocity at the moment, i.e. the predicted position and posture of the inertial navigation system; , and Respectively expressed in Rotation, displacement and velocity at each moment; , Respectively Angular velocity and acceleration at the moment; , They represent the angular velocity bias error and acceleration bias error of the inertial navigation system, and are taken as 1.815184e respectively. -5 and 1.7479887e -4 ; represents the acceleration due to gravity, take 9.83;

[0096] S3.2, calculate the prediction covariance;

[0097]

[0098] In the formula, is the prediction covariance; is the Jacobian matrix in the prediction process, which reflects the partial derivatives of the inertial navigation kinematics model to the motion state; is the Jacobian matrix of the last prediction, To predict noise;

[0099] S3.3. Calculate the Kalman filter gain by predicting the covariance; the expression is as follows:

[0100]

[0101] In the formula, is the Kalman filter gain, is the Jacobian matrix of the observation equation;

[0102] S3.4. Calculate the Kalman filter error; the expression is as follows:

[0103]

[0104] In the formula, is the Kalman filter error, is the predicted value of posture;

[0105] S3.5. Calculate the front-end odometer by calculating the Kalman filter error; the expression is as follows:

[0106]

[0107] In the formula, is the front-end odometer, corresponding to Figure 5 is position 1 and posture 1, is the predicted position and attitude of the inertial navigation;

[0108] S4. Use the obtained front-end odometer to perform point cloud dedistortion on the lidar data, and then perform point cloud matching to obtain the point cloud matching pose. The specific steps are as follows:

[0109] S4.1. Use the front-end odometer to measure each point in the point cloud frame Perform point cloud compensation and point cloud dedistortion processing; the expression is as follows:

[0110]

[0111] In the formula, It represents the external parameters from the laser radar coordinate system to the inertial navigation coordinate system, which can be obtained through external parameter calibration; Represents the transformation matrix from time tj to end time tk in the inertial navigation coordinate system; Represents the point cloud after dedistortion; the present invention obtains more accurate correction reference coordinates, i.e., the front-end odometer, and the dedistortion effect is more stable and effective;

[0112] S4.2. Calculate the matching pose of the laser radar point cloud; the expression is as follows:

[0113]

[0114] In the formula, Represents the calculated point cloud matching pose, corresponding to Figure 5 Position 2 and posture 2 in Indicates the point cloud matching pose at the previous moment, and the superscript T indicates the matrix rank. Indicates the predicted pose at the current moment.

[0115] S5. Use GPS position information to eliminate the position error of point cloud matching and obtain an accurate backend odometer;

[0116] S5.1. Time-synchronize the point cloud data frame located by the laser radar with the GPS data to obtain the GPS data at the corresponding time of the point cloud matching posture, i.e., the GPS data after time synchronization;

[0117] S5.2. Convert the latitude and longitude coordinate system of the time-synchronized GPS data to the Mercator coordinate system, where:

[0118] Mercator coordinate system X = longitude * earth radius * 0.0174;

[0119] Mercator coordinate system Y = ln(tan((90+latitude)*0.00872))*earth radius;

[0120] The longitude and latitude here are the real-time longitude and latitude of the GPS data after time synchronization, and the radius of the earth is 6371.393 kilometers;

[0121] S5.3, align the X,Y coordinate values ​​of the Mercator coordinate system with the origin of the global coordinate system;

[0122] S5.4, aligning the X,Y coordinate values ​​of the Mercator coordinate system with the trajectory of the global coordinate system;

[0123] S5.5. Use the Mercator coordinate system converted from GPS to replace the position data of the point cloud matching, and retain the posture data of the point cloud matching to obtain an accurate backend odometer, corresponding to Figure 2 Position 3, posture 3 in the middle;

[0124] S6. Optimize the front-end odometer and the back-end odometer to obtain the optimization objective function of the final posture; the optimization objective function is as follows:

[0125]

[0126] In the formula, Represents the sum of the squares of the front-end odometer residual and the back-end odometer residual, which is the amount to be optimized when the objective function is minimized. , which is the positioning result. Figure 5 The position is 4, the posture is 4, and the final positioning effect is as follows Figure 6 As shown, the sharp turn trajectory after lateral speed correction is closer to the actual sharp turn trajectory than the sharp turn trajectory before correction.

[0127] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the essence and scope of the technical solution of the present invention.

Claims

1. A multi-sensor fusion positioning method suitable for vehicle extreme working conditions, characterized by: The following steps are involved: S1, determining whether the vehicle is making a sharp turn by calculating the vehicle heading angle and the front wheel turning angle; S2, when the vehicle makes a sharp turn, the lateral speed correction value of the wheel speed meter data is obtained by correcting the vehicle heading angle and the front wheel turning angle when the vehicle makes a sharp turn; S3, obtaining wheel speed meter data through the vehicle heading angle, the front wheel turning angle and the lateral speed correction value of the wheel speed meter data, and performing Kalman filter calculation on the inertial navigation data and the wheel speed meter data to obtain the front-end odometer; S4, using the obtained front-end odometer to perform point cloud dedistortion on the lidar data, and then performing point cloud matching to obtain the point cloud matching pose; S5. Use GPS position information to eliminate the position error of point cloud matching and obtain an accurate backend odometer; S6. Optimize the front-end odometer and the back-end odometer to obtain the objective function of the final posture.

2. A multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to claim 1, characterized in that: The steps of determining whether the vehicle is making a sharp turn by calculating the vehicle heading angle and the front wheel turning angle are as follows: S1.

1. The vehicle heading angle is obtained by integrating the angular velocity of the inertial navigation system around the vertical axis over time. The expression is as follows: ; In the formula, is the vehicle heading angle; is the angular velocity of the inertial navigation system around the vertical axis; Indicates the sampling time; S1.

2. Calculate the front wheel steering angle increment; the expression is as follows: ; In the formula, is the front wheel turning angle increment; is the kth sampling angle, k∈(i,j), i represents the first sampling point, and j represents the last sampling point; S1.3, by comparing the changes in the vehicle heading angle and the front wheel turning angle with a preset threshold, determining whether the vehicle is making a sharp turn; The vehicle front wheel steering angle threshold and wheel heading angle threshold are both set to 0.

002.

3. The multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to claim 1, characterized in that: When the vehicle makes a sharp turn, the steps of obtaining the lateral speed correction value of the wheel speed meter data by correcting the vehicle heading angle and the front wheel turning angle when the vehicle makes a sharp turn are as follows: S2.

1. Calculate the lateral velocity correction. The expression is as follows: ; In the formula, is the lateral velocity correction; is the correction proportional factor; if it is determined that the vehicle is not in a turning state, then =0; It represents the vehicle turning angle calculated from the angular velocity measured by inertial navigation; The front wheel turning angle measured by the vehicle's wheel speedometer; S2.

2. Correct the lateral speed of the wheel speed meter data based on the calculated lateral speed correction value; the details are as follows: The wheel speed meter data expression is as follows: ; In the formula, The forward speed of the vehicle measured by the wheel speed meter; The wheel speed meter data expression after lateral speed correction is as follows: 。 4. The multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to claim 1, characterized in that: The steps of obtaining the wheel speed meter data through the vehicle heading angle, the front wheel turning angle and the lateral speed correction value of the wheel speed meter data, and performing Kalman filter calculation on the inertial navigation data and the wheel speed meter data to obtain the front-end odometer are as follows: S3.

1. Calculate the predicted position and attitude of the inertial navigation system; the expression is as follows: ; In the formula, , and Respectively expressed in The rotation, displacement and velocity at the moment, i.e. the predicted position and posture of the inertial navigation system; , and Respectively expressed in Rotation, displacement and velocity at each moment; , Respectively Angular velocity and acceleration at the moment; , They represent the angular velocity bias error and acceleration bias error of the inertial navigation system respectively; represents the acceleration due to gravity; S3.2, calculate the prediction covariance; ; In the formula, is the prediction covariance; is the Jacobian matrix in the prediction process, which reflects the partial derivatives of the inertial navigation kinematics model to the motion state; is the Jacobian matrix of the last prediction, To predict noise; S3.

3. Calculate the Kalman filter gain by predicting the covariance; the expression is as follows: ; In the formula, is the Kalman filter gain, is the Jacobian matrix of the observation equation; S3.

4. Calculate the Kalman filter error; the expression is as follows: ; In the formula, is the Kalman filter error, is the predicted value of posture; S3.

5. Calculate the front-end odometer by calculating the Kalman filter error; the expression is as follows: ; In the formula, It is the front-end odometer; is the predicted position of the inertial navigation.

5. The multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to claim 1, characterized in that: The specific steps of using the obtained front-end odometer to perform point cloud dedistortion on the lidar data and then performing point cloud matching to obtain the point cloud matching pose are as follows: S4.

1. Use the front-end odometer to measure each point in the point cloud frame Perform point cloud compensation and point cloud dedistortion processing; the expression is as follows: ; In the formula, Represents the external parameters from the laser radar coordinate system to the inertial navigation coordinate system; Represents the transformation matrix from time tj to end time tk in the inertial navigation coordinate system; represents the point cloud after dedistortion; S4.

2. Calculate the matching pose of the laser radar point cloud; the expression is as follows: ; In the formula, Represents the calculated point cloud matching pose; Indicates the point cloud matching pose at the previous moment; the superscript T indicates the matrix transformation rank; Indicates the predicted pose at the current moment.

6. The multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to claim 1, characterized in that: The steps of eliminating the position error of point cloud matching by using GPS position information to obtain an accurate backend odometer are as follows: S5.

1. Time-synchronize the point cloud data frame located by the laser radar with the GPS data to obtain the GPS data at the corresponding time of the point cloud matching posture, i.e., the GPS data after time synchronization; S5.

2. Convert the latitude and longitude coordinate system of the time-synchronized GPS data to the Mercator coordinate system, where: Mercator coordinate system X = longitude * earth radius * 0.0174; Mercator coordinate system Y = ln(tan((90+latitude)*0.00872))*earth radius; The longitude and latitude here are the real-time longitude and latitude of the GPS data after time synchronization, and the radius of the earth is 6371.393 kilometers; S5.3, align the X,Y coordinate values ​​of the Mercator coordinate system with the origin of the global coordinate system; S5.4, aligning the X,Y coordinate values ​​of the Mercator coordinate system with the trajectory of the global coordinate system; S5.

5. Use the Mercator coordinate system converted from GPS to replace the position data of the point cloud matching, and retain the posture data of the point cloud matching to obtain an accurate backend odometer.

7. The multi-sensor fusion positioning method suitable for vehicle extreme working conditions according to claim 1, characterized in that: The front-end odometer and the back-end odometer are optimized to obtain the optimization objective function of the final posture, wherein the optimization objective function is as follows: ; In the formula, Represents the sum of the squares of the front-end odometer residual and the back-end odometer residual, which is the amount to be optimized when the objective function is minimized. , which is the positioning result.

Images