Multi-pose source fusion positioning method based on factor graph

Abstract

The invention discloses a multi-pose source fusion positioning method based on a factor graph, and the method comprises the steps: S1, building a factor graph: taking RTK state information at a fusion starting moment as a prior factor in a current state, and building the factor graph; s2, point cloud map relocation odometer information optimization: performing forward propagation by using IMU data, and establishing a state quantity error model; carrying out backward propagation on the original point cloud of the LiDAR by utilizing a predicted value of forward propagation so as to carry out distortion removal operation, and establishing an observation equation by utilizing the point cloud after distortion removal; then IEKF is used for updating; s3, optimizing wheel speed-IMU odometer information: optimizing rotation information provided by an IMU through error Kalman, and calculating a state quantity at an IMU data moment in combination with wheel speed data; and S4, multi-pose source fusion: carrying out cumulative multiplication on the RTK priori factor, the point cloud map relocation odometer factor and the wheel speed-IMU odometer factor to obtain a system state with the maximum probability. Through fusion optimization of data of a plurality of sensors, accurate autonomous positioning information of the mine card is obtained.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a multi-pose source fusion positioning method based on a factor graph. Background Art

[0002] In the field of unmanned mining, accurate positioning of mining trucks is one of the key technologies to achieve automated transportation and improve operating efficiency. However, due to the complexity and variability of the mining environment, traditional single positioning technology is difficult to provide comprehensive and accurate location information of mining trucks. For example, the traditional odometer module and RTK technology have the following defects:

[0003] Odometer module: In unmanned driving in mines, commonly used odometer modules (such as wheel odometers, inertial measurement units, etc.) can provide relative displacement information of vehicles. However, these devices are susceptible to cumulative errors. As time goes by and the distance increases, the errors will gradually accumulate, resulting in inaccurate positioning results.

[0004] RTK technology: Although real-time kinematic differential global positioning system (RTK GPS) technology can provide high-precision location information, in a mining environment, due to factors such as terrain obstruction and multipath effects, RTK signals may be interfered with, causing positioning results to jump, affecting the continuity and stability of positioning.

[0005] In addition, current autonomous positioning algorithms include SLAM (Simultaneous Localization and Mapping) and methods based on machine learning and deep learning. Among them, methods based on machine learning and deep learning require a large amount of labeled data for training, and have weak robustness and limited adaptability to new environments. In the special environment of mines, it is very difficult to collect a large amount of high-quality training data, so the application of these methods is limited.

[0006] In summary, the current autonomous driving field has focused on the research of multiple sensor information fusion positioning methods, which have the advantage of not relying on a specific sensor. However, since each sensor has its own characteristics, how to fuse positioning and how to optimize the fusion results of different pose sources by utilizing the information differences between different sensors is still a challenging research. Summary of the invention

[0007] In order to solve the above technical problems, the present invention provides a multi-pose source fusion positioning method based on factor graph, which uses RTK, LiDAR, IMU and wheel speed sensor to fuse and locate the state of the mine card in the discharge port area, and the method includes the following steps:

[0008] S1. Factor graph establishment: The RTK state information at the start of fusion is used as the prior factor in the current state, the wheel speed-IMU odometer information is used as the intermediate constraint factor, and the point cloud map repositioning odometer information is used as the unary edge factor to establish the factor graph;

[0009] S2. Optimizing the point cloud map repositioning odometer information: forward propagating the IMU data to establish a state error model; backpropagating the LiDAR original point cloud using the predicted value of the forward propagation to perform a dedistortion operation, and establishing an observation equation using the dedistorted point cloud; then updating using IEKF to solve the residual error, and updating the final state of the point cloud map repositioning odometer after satisfying the iteration cutoff condition;

[0010] S3, the wheel speed-IMU odometer information optimization: according to the angular velocity provided by the IMU, the state is forward propagated using IMU Kinematic to obtain the observation equation of the rotation error; at the same time, according to the acceleration provided by the IMU, a measurement equation about the current rotation is established, the optimized IMU rotation information is obtained through Kalman filtering, and the wheel speed-IMU odometer information at the IMU data moment is obtained in combination with the wheel speed calculation;

[0011] S4, multi-pose source fusion: multiply the prior factor, the point cloud map repositioning odometer factor and the wheel speed-IMU odometer factor to obtain the system state with the highest probability.

[0012] Further:

[0013] State quantity error in point cloud map relocalization odometer information optimization The Jacobian matrix The calculation formula is:

[0014]

[0015] The noise amount w in the optimization of the point cloud map relocalization odometer information i The Jacobian matrix The calculation formula is:

[0016]

[0017] At the same time, the covariance with the prediction is calculated The calculation formula is:

[0018]

[0019] Among them, x i+1 represents the true value in the presence of noise, represents the predicted state with zero noise, Q i The covariance matrix representing the amount of noise.

[0020] Furthermore, the point cloud map repositioning odometer information optimization also includes:

[0021] S21. Using relative pose transformation The point cloud at other times From their respective local coordinate systems Convert to the unified coordinate system at the time of scanning To complete the de-distortion operation, it satisfies the formula:

[0022]

[0023] in, I T L Represents the external parameters from LiDAR to IMU;

[0024] S22. Using external references I T L The denoised point cloud L p j + L n j Convert from LiDAR coordinate system to IMU coordinate system, and then use the pose in the prior state quantity Convert the point cloud from the IMU coordinate system to the global coordinate system;

[0025] S23. After unifying the coordinate system, the point cloud finds several corresponding points G q j , and the vector composed of the point cloud and the corresponding point and the normal vector of the local plane The ideal state of the inner product should be 0, which satisfies the equation:

[0026]

[0027] And calculate the corresponding Jacobian matrix.

[0028] Furthermore, the updating using IEKF in S2 also includes:

[0029] S24, in the process of updating using IEKF, repeated observations are performed through iterations to gradually approach the optimal linearization point;

[0030] S25, assuming that the initial prior state of the system is The corresponding error state is δx; then at the Kth iteration, the prior value of the state vector is The error state to be determined is δx k ; Calculate δx based on the maximum a posteriori estimate k Then, the δx k Add back get Re-observe and solve the residual z and Jacobian matrix H; solve again It satisfies the formula:

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

[0032]

[0033] Among them, I represents the identity matrix, J represents the Jacobian matrix, and P represents the covariance;

[0034] S26, repeating S25 until an iteration cutoff condition is met, and then updating the final state of the point cloud map repositioning odometer information.

[0035] Furthermore, the rotation error in S3 is used as a prediction equation, which satisfies the formula:

[0036]

[0037]

[0038] Where θ represents the rotation angle, b g represents the offset, t represents the time, ω m represents the angular velocity, I represents the unit matrix, and F x The Jacobian matrix of the predicted state, F i represents the Jacobian matrix of the noise amount, and K represents the time.

[0039] Furthermore, the wheel speed-IMU odometer information optimization also includes:

[0040] S31. Establish a measurement equation that satisfies the formula:

[0041]

[0042] Among them, a t Indicates the direction of gravity, v g represents the ideal gravity direction [0 0 1],

[0043] Calculate the Jacobian matrix H, which satisfies the formula:

[0044]

[0045] S32, using the covariance matrix P, Jacobian matrix H and noise n a Calculate the Kalman gain K, which satisfies the formula:

[0046] K=P·H T ·(H·PH T +n a ) -1

[0047] S33, using the Kalman gain K to calculate the optimized rotation matrix, deviation value and prediction covariance, after obtaining the optimized rotation information, calculate the linear interpolation results of the wheel speed data of the previous and next frames at the current IMU data moment at the IMU data moment, which corresponds to the wheel speed data at the IMU data moment;

[0048] S34, dividing the motion into n equal parts, where n approaches infinity, and obtaining a posture transformation formula as a wheel speed-IMU odometer factor. The posture transformation formula is:

[0049]

[0050] Wherein, C represents the optimized rotation matrix obtained by S32, and J represents the Jacobian matrix corresponding to the rotation matrix.

[0051] Furthermore, the system state with the maximum probability is derived from Bayes' law, which satisfies the formula:

[0052]

[0053] Among them, Z i represents observation, X i Indicates the system status.

[0054] Compared with the prior art, the present invention has the following beneficial effects:

[0055] The present invention combines RTK, wheel speed, IMU and LiDAR sensor data, and uses error Kalman filtering and factor graph optimization methods to fuse and optimize these data, so as to obtain accurate autonomous positioning information of the mining truck in a short time. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a schematic diagram of the overall process disclosed in the embodiment of the present invention;

[0057] Figure 2 A flowchart of optimizing odometer information for point cloud map relocation disclosed in an embodiment of the present invention;

[0058] Figure 3 A flowchart of wheel speed-IMU odometer information optimization disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0059] In order to make the technical scheme and technical effect of the present invention clearer, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments.

[0060] The present invention aims to provide a multi-pose source fusion positioning method based on factor graph. The method utilizes RTK, LiDAR, IMU and wheel speed sensor for fusion to obtain the status of the mining truck in the discharge port area. It does not rely on a large amount of training data, can accurately predict the positioning information of the current mining truck in the discharge port area, and can cope with complex working conditions.

[0061] The method provided in this embodiment mainly includes the following steps:

[0062] S1. Factor graph establishment

[0063] The RTK state information at the beginning of the fusion is used as the prior factor of the current state, the wheel speed-IMU odometer information is used as the intermediate constraint factor, and the point cloud map repositioning odometer information is used as the unary edge factor to establish a factor graph. Among them, the introduction of RTK integrated navigation information provides a reliable starting point for the subsequent positioning process, which helps to reduce the cumulative error and improve the overall positioning accuracy. Figure 1 , estimating the state is the main task in the multi-pose source fusion positioning provided in this embodiment.

[0064] S2. Optimization of point cloud map relocation odometer information

[0065] The IMU data is used for forward propagation to establish a state error model. The predicted value of the forward propagation is used to perform backpropagation on the LiDAR original point cloud to perform a dedistortion operation, and the observation equation is established using the dedistorted point cloud. The IEKF is then used for updating and solving the residual. After the iteration cutoff condition is met, the point cloud map is updated to relocate the final state of the odometer.

[0066] refer to Figure 2 , specifically:

[0067] S21. Use IMU data for forward propagation to establish a model of state quantity error, where:

[0068] State quantity error The Jacobian matrix The calculation formula is:

[0069]

[0070] Noise amount w i The Jacobian matrix The calculation formula is:

[0071]

[0072] At the same time, the covariance with the prediction is calculated The calculation formula is:

[0073]

[0074] Among them, x i+1 represents the true value in the presence of noise, represents the predicted state with zero noise, Q i The covariance matrix representing the amount of noise.

[0075] S22, use the forward propagation prediction value to perform back propagation on the LiDAR original point cloud to perform dedistortion operation; use relative pose transformation The point cloud at other times From their respective local coordinate systems Convert to the unified coordinate system at the time of scanning To complete the de-distortion operation, it satisfies the formula:

[0076]

[0077] in, I T L Represents the external parameters from LiDAR to IMU;

[0078] Using external reference I T L The denoised point cloud L p j + L n j Convert from LiDAR coordinate system to IMU coordinate system, and then use the pose in the prior state quantity Convert the point cloud from the IMU coordinate system to the global coordinate system.

[0079] S23. After unifying the coordinate system, the point cloud finds several corresponding points G q j , and the vector composed of the point cloud and the corresponding point and the normal vector of the local plane The ideal state of the inner product should be 0, which satisfies the equation:

[0080]

[0081] And calculate the corresponding Jacobian matrix.

[0082] S24, in the process of updating using IEKF (i.e., iterative extended Kalman filter), repeated observations are performed through iteration to gradually approach the optimal linearization point;

[0083] S25, assuming that the initial prior state of the system is The corresponding error state is δx; then at the Kth iteration, the prior value of the state vector is The error state to be determined is δx k ; Calculate δx based on the maximum a posteriori estimate k Then, δx k Add back get Re-observe and solve the residual z and Jacobian matrix H; solve again It satisfies the formula:

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

[0085]

[0086] Among them, I represents the identity matrix, J represents the Jacobian matrix, and P represents the covariance;

[0087] S26, and repeat S25 until the iteration cutoff condition is met, and then update the final state of the point cloud map relocation odometer information.

[0088] S3. Optimization of wheel speed-IMU odometer information

[0089] According to the angular velocity provided by the IMU, IMU Kinematic is used to perform state forward propagation to obtain the observation equation of the rotation error. At the same time, a measurement equation about the current rotation is established based on the acceleration provided by the IMU. The optimized IMU rotation information is obtained through Kalman filtering, and the wheel speed-IMU odometer information at the moment of IMU data is obtained in combination with the wheel speed calculation.

[0090] refer to Figure 3 , specifically:

[0091] S31. Use IMU Kinematic to perform state forward propagation according to the angular velocity provided by the IMU, and further obtain the rotation error as the prediction equation, which satisfies the formula:

[0092]

[0093] Where θ represents the rotation angle, b g represents the offset, t represents the time, ω m represents the angular velocity, I represents the unit matrix, and F x The Jacobian matrix of the predicted state, F irepresents the Jacobian matrix of the noise amount, and K represents the time.

[0094] Establish a measurement equation that satisfies the formula:

[0095]

[0096] Among them, a t Indicates the direction of gravity, v g represents the ideal gravity direction [0 0 1]. In reality, it is not possible to make uniform motion, so it can be treated as noise.

[0097] Calculate the Jacobian matrix H, which satisfies the formula:

[0098]

[0099] S32, using the covariance matrix P, Jacobian matrix H and noise n a Calculate the Kalman gain K, which satisfies the formula:

[0100] K=P·H T ·(H·PH T +n a ) -1

[0101] S33. Use the Kalman gain K to calculate the optimized rotation matrix, deviation value and predicted covariance. After obtaining the optimized rotation information, calculate the linear interpolation result of the wheel speed data of the previous and next frames at the current IMU data moment at the IMU data moment, which corresponds to the wheel speed data at the IMU data moment.

[0102] S34. Divide the motion into n equal parts, where n approaches infinity, and the posture transformation formula can be obtained as the wheel speed-IMU odometer factor. The posture transformation formula is:

[0103]

[0104] Among them, C represents the optimized rotation matrix obtained by S32, and J represents the Jacobian matrix corresponding to the rotation matrix.

[0105] S4, Multi-pose Source Fusion

[0106] The above-mentioned prior factors, point cloud map relocation odometer factors and wheel speed-IMU odometer factors are multiplied to find the maximum value on the probability distribution. The obtained system state is the maximum posterior distribution derived according to Bayes' law, that is, the system state with the maximum probability, which satisfies the formula:

[0107]

[0108] Among them, Z irepresents observation, X i Indicates the system status.

[0109] In this embodiment, RTK provides high-precision position and velocity information, IMU provides accelerometer and gyroscope data, and LiDAR provides environmental point cloud data. Based on these characteristics and information differences, a fusion algorithm is reasonably designed to effectively integrate the data of each sensor, thereby improving the accuracy and reliability of state estimation.

[0110] It should be noted that this example uses information from four sensors: RTK, LiDAR, IMU, and wheel speed. In theory, it can also be combined with more sensors such as cameras and Radar for fusion positioning.

[0111] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A multi-pose source fusion positioning method based on factor graph, characterized in that: The method uses RTK, LiDAR, IMU and wheel speed sensor for fusion to obtain the state of the mine truck in the discharge port area. The method includes the following steps: S1. Factor graph establishment: The RTK state information at the start of fusion is used as the prior factor in the current state, the wheel speed-IMU odometer information is used as the intermediate constraint factor, and the point cloud map repositioning odometer information is used as the unary edge factor to establish the factor graph; S2. Optimizing the point cloud map repositioning odometer information: forward propagating the IMU data to establish a state error model; backpropagating the LiDAR original point cloud using the predicted value of the forward propagation to perform a dedistortion operation, and establishing an observation equation using the dedistorted point cloud; then updating using IEKF to solve the residual error, and updating the final state of the point cloud map repositioning odometer after satisfying the iteration cutoff condition; S3, the wheel speed-IMU odometer information optimization: according to the angular velocity provided by the IMU, the state is forward propagated using IMU Kinematic to obtain the observation equation of the rotation error; at the same time, according to the acceleration provided by the IMU, a measurement equation about the current rotation is established, the optimized IMU rotation information is obtained through Kalman filtering, and the wheel speed-IMU odometer information at the IMU data moment is obtained in combination with the wheel speed calculation; S4, multi-pose source fusion: multiply the prior factor, the point cloud map repositioning odometer factor and the wheel speed-IMU odometer factor to obtain the system state with the highest probability.

2. The multi-pose source fusion positioning method based on factor graph according to claim 1 is characterized in that: State quantity error in point cloud map relocalization odometer information optimization The Jacobian matrix The calculation formula is: The noise amount w in the optimization of the point cloud map relocalization odometer information i The Jacobian matrix The calculation formula is: At the same time, the covariance with the prediction is calculated The calculation formula is: Among them, x i+1 represents the true value in the presence of noise, represents the predicted state with zero noise, Q i The covariance matrix representing the amount of noise.

3. The multi-pose source fusion positioning method based on factor graph according to claim 2 or 1, characterized in that: The point cloud map repositioning odometer information optimization also includes: S21. Using relative posture transformation The point cloud at other times From their local coordinate systems Convert to the unified coordinate system at the time of scanning To complete the de-distortion operation, it satisfies the formula: in, I T L Represents the external parameters from LiDAR to IMU; S22. Using external references I T L The denoised point cloud L p j + L n j Convert from LiDAR coordinate system to IMU coordinate system, and then use the pose in the prior state quantity Convert the point cloud from the IMU coordinate system to the global coordinate system; S23. After unifying the coordinate system, the point cloud finds several corresponding points G q j , and the vector composed of the point cloud and the corresponding point and the normal vector of the local plane The ideal state of the inner product should be 0, which satisfies the equation: And calculate the corresponding Jacobian matrix.

4. The multi-pose source fusion positioning method based on factor graph according to claim 3 is characterized in that: The updating using IEKF in S2 also includes: S24, in the process of updating using IEKF, repeated observations are performed through iterations to gradually approach the optimal linearization point; S25, assuming that the initial prior state of the system is The corresponding error state is δx; then at the Kth iteration, the prior value of the state vector is The error state to be determined is δx k ; Calculate δx based on the maximum a posteriori estimate k Then, the δx k Add back get Re-observe and solve the residual z and Jacobian matrix H; solve again It satisfies the formula: K=(H T R -1 H+P -1 ) -1 H T R -1 Among them, I represents the identity matrix, J represents the Jacobian matrix, and P represents the covariance; S26, repeating S25 until an iteration cutoff condition is met, and then updating the final state of the point cloud map repositioning odometer information.

5. The multi-pose source fusion positioning method based on factor graph according to claim 1 is characterized in that: The rotation error in S3 is used as the prediction equation, which satisfies the formula: Where θ represents the rotation angle, b g represents the offset, t represents the time, ω m represents the angular velocity, I represents the identity matrix, and F x The Jacobian matrix of the predicted state, F i represents the Jacobian matrix of the noise amount, and K represents the time.

6. The multi-pose source fusion positioning method based on factor graph according to claim 5 is characterized in that: The wheel speed-IMU odometer information optimization also includes: S31. Establish a measurement equation that satisfies the formula: Among them, a t Indicates the direction of gravity, v g represents the ideal gravity direction [0 0 1], Calculate the Jacobian matrix H, which satisfies the formula: S32, using the covariance matrix P, Jacobian matrix H and noise n a Calculate the Kalman gain K, which satisfies the formula: K=P·H T ·(H·PH T +n a ) -1 S33, using the Kalman gain K to calculate the optimized rotation matrix, deviation value and prediction covariance, after obtaining the optimized rotation information, calculate the linear interpolation results of the wheel speed data of the previous and next frames at the current IMU data moment at the IMU data moment, which corresponds to the wheel speed data at the IMU data moment; S34, dividing the motion into n equal parts, where n approaches infinity, and obtaining a posture transformation formula as a wheel speed-IMU odometer factor. The posture transformation formula is: Wherein, C represents the optimized rotation matrix obtained by S32, and J represents the Jacobian matrix corresponding to the rotation matrix.

7. The multi-pose source fusion positioning method based on factor graph according to claim 1 is characterized in that: The system state with the maximum probability is derived from Bayes' law, which satisfies the formula: Among them, Z i represents observation, X i Indicates the system status.

Images