Fusion positioning method for mine card in unloading port RTK failure scene

Abstract

The invention provides a fusion positioning method for a mine card in a discharge port RTK failure scene, and the method comprises the following steps: a fusion positioning system begins to take over when a task state is detected to accord with a discharge area reversing task, and carries out the external parameter verification through employing the RTK state information at an initial moment, calculating a pose transformation value for performing time synchronization on the point cloud map relocation speedometer information, the IMU-wheel speed speedometer information and the RTK state information, and synchronizing the IMU-wheel speed speedometer information and the point cloud map relocation speedometer information to an RTK information moment; inputting IMU-wheel speed odometer information, point cloud map relocation odometer information and RTK state information after time synchronization into a factor graph, and taking the RTK state information as a prior factor; all factors are multiplied, maximum posterior distribution is obtained through derivation and serves as current optimal system state information, system state information obtained after factor graph optimization is monitored, and system state information with normal optimization results is output. And the mine unmanned driving positioning requirement is met.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a fusion positioning method for a mining truck in a scenario where RTK fails at a discharge port. Background Art

[0002] In the field of unmanned mining, precise positioning of mining trucks is the key to achieving automated loading and unloading operations.

[0003] Traditional positioning systems usually rely on the global positioning system GPS and its enhanced form real-time dynamic positioning RTK to provide high-precision position data. However, in special unloading areas, due to factors such as terrain obstruction and multipath effects, RTK signals may be interfered with, resulting in jumps in positioning results, affecting the continuity and stability of positioning.

[0004] In order to reduce the dependence on RTK technology, the existing technology proposes to fuse multi-sensor data to achieve accurate positioning of mining truck autonomous driving. Although this technical solution improves the robustness of the system, the data of different sensors may have time synchronization problems, and the computing power is relatively large.

[0005] It can be seen that ensuring the time synchronization of each sensor in the process of multi-sensor data fusion is an issue worthy of study. Summary of the invention

[0006] In order to solve the above technical problems, the present invention provides a fusion positioning method for a mining truck in a scenario where RTK fails at a discharge port. The method uses LiDAR, wheel speed and IMU sensors to perform fusion positioning of the mining truck in the discharge port area. The method includes the following steps:

[0007] S1, time synchronization operation: obtain the current task status. When it is detected that the task status meets the reversing task in the unloading area, the fusion positioning system starts to take over, and uses the RTK state information at the initial moment to perform external parameter verification, calculate the posture transformation value for time synchronization of the point cloud map repositioning odometer information, IMU-wheel speed odometer information and RTK state information, and synchronize the IMU-wheel speed odometer information and the point cloud map repositioning odometer information to the RTK information time;

[0008] S2. Factor graph establishment and optimization: The IMU-wheel speed odometer information, point cloud map repositioning odometer information and RTK status information after time synchronization are input into the factor graph together, and the RTK status information is used as a priori factor; all factors are multiplied to derive the maximum a posteriori distribution as the current optimal system status information, monitor the system status information after factor graph optimization, and output the system status information with normal optimization results.

[0009] Furthermore, the external parameter verification using the RTK state information at the initial time also includes:

[0010] The initial moment of the fusion positioning system uses the current RTK state information as the prior state and obtains the initial moment position information T rtk , the point cloud map repositioning odometer obtains the optimized pose result T through registration lio ; By the T rtk and T lio Calculate and estimate the online external parameter value T of the LiDAR online , which satisfies the formula:

[0011] T online =T lio -1 *T rtk

[0012] Compare the online external parameter value T online and the external parameter value T of offline calibration offline The difference ΔT calib , which satisfies the formula:

[0013] ΔT calib =T online -1 *T offline

[0014] Compare the difference ΔT calib With threshold:

[0015] If the difference ΔT calib If it is greater than the threshold, a fault is directly reported and the vehicle is stopped;

[0016] If the difference ΔT calib If the threshold is met, the point cloud map repositioning odometer information, IMU-wheel speed odometer information and RTK status information are time synchronized, and the IMU-wheel speed odometer information and point cloud map repositioning odometer information are synchronized to the RTK information time.

[0017] Furthermore, the time synchronization of the point cloud map repositioning odometer information in the time synchronization operation includes:

[0018] Know the current RTK information time t rtk is the current reference time, extract the point cloud map repositioning odometer information of the last moment closest to the current reference time, and record the time of the point cloud map repositioning odometer information as t lio ; Extract and multiply the t lio To the rtk All the IMU-wheel speed odometer information for the time period Timu-wheel , calculate the t lio To the rtk Pose transformation value of time period It satisfies the formula:

[0019]

[0020] Using the pose transformation value The t lio The point cloud map repositions the odometer information at the time Recursively synchronize to the current RTK information time t rtk , which satisfies the formula:

[0021]

[0022] Complete the time synchronization of the point cloud map repositioning odometer information.

[0023] Furthermore, the time synchronization of the IMU-wheel speed odometer information in the time synchronization operation includes:

[0024] Know the current RTK information time t rtk The current reference time is used to extract the RTK information time t-1 of the last moment closest to the current reference time. rtk , extract and multiply the t-1 rtk To the rtk All the IMU-wheel speed odometer information for the time period T imu-wheel , calculate the t-1 rtk To the rtk Pose transformation value of time period It satisfies the formula:

[0025]

[0026] Using the pose transformation value The last moment RTK information Recursively synchronize to the current RTK information time t rtk , then the RTK information at this time is the synchronized IMU-wheel speed odometer information, which satisfies the formula:

[0027]

[0028] Complete the time synchronization of the IMU-wheel speed odometer information.

[0029] Furthermore, the factor graph establishment in S2 further includes:

[0030] The RTK state information is used as a priori factor of the factor graph and only acts on the initial state node;

[0031] The IMU-wheel speed odometer information acts as an intermediate constraint factor between adjacent state nodes;

[0032] The point cloud map repositioning odometer information is used as a unary edge factor and only acts on the state node at the corresponding moment.

[0033] Furthermore, the factor graph optimization in S2 further includes:

[0034] According to Bayes' law, the maximum posterior distribution is derived as the current optimal system state information, and the solution is obtained in Z i Under observation, the most likely system state X i , which satisfies the formula:

[0035]

[0036] Among them, x i Represents an element in the system state X set.

[0037] Furthermore, the system status information after the monitoring factor graph is optimized also includes:

[0038] If the monitoring result is normal, it will be directly used as the final output;

[0039] If the monitoring result is abnormal, the IMU-wheel speed odometer information between two adjacent moments after synchronization is used. Relocate odometer information with point cloud map Calculate the respective pose transformation values ​​ΔT imu-wheel and ΔT lio , which satisfies the formula:

[0040]

[0041] Calculate the posture transformation value ΔT imu-wheel and ΔT lio The difference between ΔT lio_imu-wheel , which satisfies the formula:

[0042] ΔT lio_imu-wheel =ΔT imu-wheel -1 *ΔT lio

[0043] Compare the difference ΔT lio_imu-wheel With threshold:

[0044] If the difference ΔT lio_imu-wheelIf the threshold is met, the verification is passed, and the IMU-wheel speed odometer information is used as the final output;

[0045] If the difference ΔT lio_imu-wheel If it is greater than the threshold, the verification fails, a fault is directly reported, and the vehicle is stopped.

[0046] Furthermore, when the takeover distance of the discharge port is greater than the set threshold and the current task status is a loading task, the fusion positioning system ends the takeover when tracking uphill.

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

[0048] The present invention utilizes LiDAR, wheel speed and IMU multiple sensor information for fusion positioning, reducing the dependence on RTK; and performs time synchronization before factor graph optimization of multiple pose sources to obtain more accurate optimization results; at the same time, adopts comparative verification operation to monitor the status of each pose source in real time to meet the precise positioning requirements of unmanned driving in mines. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 A flowchart of the fusion positioning method disclosed in an embodiment of the present invention;

[0050] Figure 2 A block diagram of the time synchronization principle disclosed in an embodiment of the present invention;

[0051] Figure 3 A block diagram of the principle of the comparative verification disclosed in an embodiment of the present invention to monitor the status of each posture source. DETAILED DESCRIPTION

[0052] 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.

[0053] The present invention aims to provide a fusion positioning method for a mining truck in the scenario where RTK fails at the discharge port. It mainly utilizes LiDAR, wheel speed and IMU sensors to fuse and locate the state of the mining truck in the discharge port area, thereby reducing dependence on RTK, integrating the advantages of other sensors, and accurately and safely predicting the positioning results of the discharge port area.

[0054] refer to Figure 1 , the method provided by the present invention comprises the following steps:

[0055] S1. Time synchronization operation

[0056] Get the current task status. When it is detected that the task status meets the reverse task in the unloading area, the fusion positioning system starts to take over and uses the RTK state information at the initial moment for external parameter verification. The pose transformation value is calculated to synchronize the point cloud map repositioning odometer information, IMU-wheel speed odometer information and RTK state information, and synchronize the IMU-wheel speed odometer information and point cloud map repositioning odometer information to the RTK information time. The following is a detailed introduction:

[0057] S11. Initial state and external parameter estimation

[0058] Get the current task status. When the task status meets the reversing task in the unloading area, the fusion positioning system takes over and starts.

[0059] At the initial moment of the fusion positioning system, the current RTK information is used as the prior state to obtain the pose information T at the initial moment. rtk , the point cloud map repositioning odometer obtains the optimized pose result T through registration lio , the two estimate the online external parameter value T of LiDAR by calculation online , which satisfies the formula:

[0060] T online =T lio -1 *T rtk

[0061] Compare online external parameter values ​​T online and the external parameter value T of offline calibration offline , calculate the difference ΔT calib , which satisfies the formula:

[0062] ΔT calib =T online -1 *T offline

[0063] Compare the difference ΔT calib With the threshold, we get:

[0064] If the difference between offline and online external parameters ΔT calib If it is too large, a fault will be reported directly and the car will be stopped;

[0065] If the difference between offline and online external parameters ΔT calib When the threshold is met, the point cloud map repositioning odometer information, wheel speed-IMU odometer information and RTK status information are time synchronized, and the IMU-wheel speed odometer information and point cloud map repositioning odometer information are synchronized to the RTK information time.

[0066] S12. Time synchronization operation

[0067] refer to Figure 2 See the principle block diagram of the time synchronization of the point cloud map relocation odometer information and the time synchronization of the IMU-wheel speed odometer information, specifically:

[0068] (1) Time synchronization operation of point cloud map relocation odometer information

[0069] Know the current RTK information time t rtk is the current reference time, extract the point cloud map repositioning odometer information of the last moment closest to the current reference time, and record the time of the point cloud map repositioning odometer information as t lio ; Extract and multiply t lio to rtk All IMU-wheel speed odometer information for time period T imu-wheel , calculate t lio to rtk Pose transformation value of time period It satisfies the formula:

[0070]

[0071] Using pose transformation value t lio Point cloud map repositioning odometer information at the moment Recursively synchronize to the current RTK information time t rtk , which satisfies the formula:

[0072]

[0073] Complete the time synchronization of point cloud map relocalization odometer information.

[0074] (2) Time synchronization operation of IMU-wheel speed odometer information

[0075] Know the current RTK information time t rtk The current reference time is used to extract the RTK information time t-1 of the last moment closest to the current reference time. rtk , extract and multiply t-1 rtk to rtk All IMU-wheel speed odometer information for time period T imu-wheel , calculate t-1 rtk to rtk Pose transformation value of time period It satisfies the formula:

[0076]

[0077] Using pose transformation value The last moment RTK information Recursively synchronize to the current RTK information time t rtk , then the RTK information at this time is the synchronized IMU-wheel speed odometer information, which satisfies the formula:

[0078]

[0079] Complete the time synchronization of IMU-wheel speed odometer information.

[0080] S2. Factor graph establishment and optimization

[0081] The time-synchronized IMU-wheel speed odometer information, point cloud map repositioning odometer information and RTK status information are input into the factor graph, and the RTK status information is used as the prior factor; all factors are multiplied to derive the maximum posterior distribution as the current optimal system status information, monitor the system status information after the factor graph is optimized, and output the system status information with normal optimization results. The following is a detailed introduction:

[0082] S21. Factor graph establishment

[0083] The IMU-wheel speed odometer information, point cloud map repositioning odometer information and RTK status information after the above S12 time synchronization are input into the factor graph together, where:

[0084] The RTK state information, as a priori factor of the factor graph, only acts on the initial state nodes;

[0085] IMU-wheel speed odometer information acts as an intermediate constraint factor between adjacent state nodes;

[0086] The point cloud map relocalization odometer information is used as a unary edge factor and only acts on the state node at the corresponding moment.

[0087] S22. Factor Graph Optimization

[0088] After constructing the factor graph according to the above S21 rules, all factors are multiplied. The maximum posterior distribution is derived according to Bayes' law and used as the current optimal system state information to solve the problem of Z i Under observation, the most likely system state X i , which satisfies the formula:

[0089]

[0090] Among them, x i Represents an element in the system state X set.

[0091] S23, real-time monitoring of system status information after factor graph optimization

[0092] refer to Figure 3 , real-time monitoring of the system status information after factor graph optimization:

[0093] If the monitoring result is normal, it will be directly used as the final output;

[0094] If the monitoring result is abnormal, the IMU-wheel speed odometer information between two adjacent moments after synchronization is used. Relocate odometer information with point cloud map Calculate the respective pose transformation values ​​ΔT imu-wheel and ΔT lio , which satisfies the formula:

[0095]

[0096] Further calculate the pose transformation value ΔT imu-wheel and ΔT lio The difference between ΔT lio_imu-wheel , which satisfies the formula:

[0097] ΔT lio_imu-wheel =ΔT imu-wheel -1 *ΔT lio

[0098] Compare the difference ΔT lio_imu-wheel And the threshold is used to determine whether the state of each pose source is abnormal:

[0099] If the difference ΔT lio_imu-wheel If the threshold is met, the verification is passed, and the IMU-wheel speed odometer information is used as the final output;

[0100] If the difference ΔT lio_imu-wheel If it is greater than the threshold, the verification fails, a fault is reported directly, and the vehicle is stopped.

[0101] When the takeover distance of the discharge port is greater than the set threshold and the current task status is a loading task, the fusion positioning system ends the takeover when tracking up the mountain.

[0102] The present invention utilizes LiDAR, wheel speed and IMU multiple sensor information for fusion positioning, reducing the dependence on RTK; and performs time synchronization before factor graph optimization of multiple pose sources to obtain more accurate optimization results; in addition, a comparative verification operation is adopted to monitor the status of each pose source in real time, so as to achieve accurate and safe prediction of the positioning results of the mine stuck in the discharge port area.

[0103] It should be noted that:

[0104] 1. The time synchronization operation in this embodiment belongs to post-processing, and the time synchronization operation can also be performed from the data plane.

[0105] 2. This embodiment uses the initial RTK state for external parameter verification, and other sensor information can also be used for external parameter verification, which does not directly rely on RTK information.

[0106] 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 fusion positioning method for a mining truck in a scenario where RTK fails at a discharge port, characterized in that: The method uses LiDAR, wheel speed and IMU sensors to fuse and locate the state of the mining truck in the discharge port area, and the method includes the following steps: S1, time synchronization operation: obtain the current task status. When it is detected that the task status meets the reversing task in the unloading area, the fusion positioning system starts to take over, and uses the RTK state information at the initial moment to perform external parameter verification, calculate the posture transformation value for time synchronization of the point cloud map repositioning odometer information, IMU-wheel speed odometer information and RTK state information, and synchronize the IMU-wheel speed odometer information and the point cloud map repositioning odometer information to the RTK information time; S2. Factor graph establishment and optimization: input the time-synchronized IMU-wheel speed odometer information, point cloud map repositioning odometer information and RTK status information into the factor graph, and use the RTK status information as a priori factor; multiply all factors to derive the maximum posterior distribution as the current optimal system status information; monitor the system status information after factor graph optimization in real time, and output the system status information with normal optimization results.

2. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 1 is characterized in that: The external parameter verification using the RTK state information at the initial time also includes: The initial moment of the fusion positioning system uses the current RTK state information as the prior state and obtains the initial moment position information T rtk , the point cloud map repositioning odometer obtains the optimized pose result T through registration lio ; By the T rtk and T lio Calculate and estimate the online external parameter value T of the LiDAR online , which satisfies the formula: T online =T lio -1 *T rtk Calculate the online external parameter value T online and the external parameter value T of offline calibration offline The difference ΔT calib , which satisfies the formula: ΔT calib =T online -1 *T offline Compare the difference ΔT calib With threshold: If the difference ΔT calib If it is greater than the threshold, a fault is directly reported and the vehicle is stopped; If the difference ΔT calib If the threshold is met, the point cloud map repositioning odometer information, IMU-wheel speed odometer information and RTK status information are time synchronized, and the IMU-wheel speed odometer information and point cloud map repositioning odometer information are synchronized to the RTK information time.

3. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 1 is characterized in that: The time synchronization of the point cloud map repositioning odometer information in the time synchronization operation includes: The current RTK information time t is known rtk is the current reference time, extract the point cloud map repositioning odometer information of the last moment closest to the current reference time, and record the time of the point cloud map repositioning odometer information as t lio ; Extract and multiply the t lio To the rtk All the IMU-wheel speed odometer information for the time period T imu-wheel , calculate the t lio To the rtk Pose transformation value of time period It satisfies the formula: Using the pose transformation value The t lio The point cloud map repositions the odometer information at the time Recursively synchronize to the current RTK information time t rtk , which satisfies the formula: Complete the time synchronization of the point cloud map repositioning odometer information.

4. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 1 is characterized in that: The time synchronization of the IMU-wheel speed odometer information in the time synchronization operation includes: The current RTK information time t is known rtk The current reference time is used to extract the RTK information time t-1 of the last moment closest to the current reference time. rtk , extract and multiply the t-1 rtk To the rtk All the IMU-wheel speed odometer information for the time period T imu-wheel , calculate the t-1 rtk To the rtk Pose transformation value of time period It satisfies the formula: Using the pose transformation value The last moment RTK information Recursively synchronize to the current RTK information time t rtk , then the RTK information at this time is the synchronized IMU-wheel speed odometer information, which satisfies the formula: Complete the time synchronization of the IMU-wheel speed odometer information.

5. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 1 is characterized in that: The factor graph establishment in S2 also includes: The RTK state information is used as a priori factor of the factor graph and only acts on the initial state node; The IMU-wheel speed odometer information acts as an intermediate constraint factor between adjacent state nodes; The point cloud map repositioning odometer information is used as a unary edge factor and only acts on the state node at the corresponding moment.

6. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 5 is characterized in that: The factor graph optimization in S2 also includes: According to Bayes' law, the maximum posterior distribution is derived as the current optimal system state information, and the solution is obtained in Z i Under observation, the most likely system state X i , which satisfies the formula: Among them, x i Represents an element in the system state X set.

7. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 5 is characterized in that: The system status information after the real-time monitoring factor graph optimization also includes: If the monitoring result is normal, it will be directly used as the final output; If the monitoring result is abnormal, the IMU-wheel speed odometer information between two adjacent moments after synchronization is used. Relocate odometer information with point cloud map Calculate the respective pose transformation values ​​ΔT imu-wheel and ΔT lio , which satisfies the formula: Calculate the posture transformation value ΔT imu-wheel and ΔT lio The difference between ΔT lio_imu-wheel , which satisfies the formula: ΔT lio_imu-wheel =ΔT imu-wheel -1 *ΔT lio Compare the difference ΔT lio_imu-wheel With threshold: If the difference ΔT lio_imu-wheel If the threshold is met, the verification is passed, and the IMU-wheel speed odometer information is used as the final output; If the difference ΔT lio_imu-wheel If it is greater than the threshold, the verification fails, a fault is directly reported, and the vehicle is stopped.

8. The fusion positioning method for mining trucks in the RTK failure scenario at the discharge port according to claim 7 is characterized in that: When the takeover distance of the discharge port is greater than the set threshold and the current task status is a loading task, the fusion positioning system ends the takeover when tracking up the mountain.

Images