An unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit

Abstract

The application discloses an unmanned vehicle positioning method combining an NDT algorithm and an inertial measurement unit, which comprises the following steps: performing NDT matching on a current frame point cloud collected by a radar and a prior map to obtain an initial pose and a transformation matrix; and fusing the transformation matrix obtained in the last frame and IMU data between two radar data to give an initial transformation prediction matrix of the next frame NDT matching and to perform pose solution of the next frame.

Description

Technical Field

[0001] This invention relates to the field of positioning and navigation technology for unmanned vehicles, and specifically to a positioning method for unmanned vehicles that combines the NDT algorithm and an inertial measurement unit. Background Technology

[0002] In the field of autonomous vehicle positioning and navigation technology, odometry and prior map-based positioning and navigation are two mainstream methods for obtaining pose. However, for mobile vehicles in industrial applications (such as industrial AGVs and unmanned forklifts), prior map-based positioning and navigation is often required to obtain relatively fixed and repetitive movement routes. Therefore, matching current radar scanning data with prior maps is a prevalent positioning method in current industrial environments. In matching algorithms, the NDT algorithm, by modeling the distribution of point cloud data, can more comprehensively capture environmental features. This transformation method based on normal distribution makes the algorithm more suitable for handling complex and changing environments.

[0003] However, traditional NDT algorithms mostly rely on the assumption of uniform motion for pose estimation, requiring a relatively accurate prediction matrix before registration. In practical applications, using only radar data as the raw input, the prediction model alone struggles to provide a prediction transformation matrix that matches the current motion state, resulting in low stability of the entire positioning system. Especially when the vehicle undergoes rapid acceleration, deceleration, or rotational motion with high angular velocity, NDT matching not only requires significant computational resources, reducing the real-time performance of the system, but also easily leads to decreased positioning accuracy or even positioning loss, resulting in low product usability. Summary of the Invention

[0004] To address the shortcomings of the existing technologies, this invention patent proposes a localization method for unmanned vehicles that combines the NDT algorithm and an inertial measurement unit (IMU). The method uses IMU data to correct the predicted rotation matrix, aiming to provide stable localization output even when the vehicle is in a situation with complex motion, thereby improving the robustness of the system.

[0005] This application employs the following technical solution to achieve the above effects:

[0006] A method for localizing unmanned vehicles combining the NDT algorithm and an inertial measurement unit includes the following steps:

[0007] S1. Start the unmanned mobile vehicle, load the environmental point cloud map, and acquire radar data and IMU data;

[0008] S2. Initialize the positioning data of the first frame of radar to obtain the initial pose of the moving vehicle;

[0009] S3. Align the timestamps of the radar data and the IMU data;

[0010] S4. Extract the IMU data within one radar scan cycle time preceding the current frame time, calculate the rotation angle within that time period, and convert it into a homogeneous transformation matrix.

[0011] S5. Use the uniform motion model to obtain the predicted value of the homogeneous transformation matrix of the current frame, and multiply it with the homogeneous transformation matrix obtained in step S4 to obtain the final predicted value of the homogeneous transformation matrix.

[0012] S6. Use the prediction matrix to perform NDT registration for the current frame to obtain the pose at the new time step.

[0013] S7. Use the homogeneous transformation matrix obtained from registration as the new prediction value, and repeat steps S3-S6 to calculate the pose for the next frame.

[0014] Furthermore, step S4 specifically includes:

[0015] S41. Synchronize the time between the IMU and the lidar sensors;

[0016] S42. Extract all IMU data within a radar scan cycle time period preceding the current frame time;

[0017] S43. Calculate the rotation angle of the lidar between two frames using integration;

[0018] S44. Convert the calculated angle data into a homogeneous transformation matrix.

[0019] Furthermore, the turning angle in step S43 is: Δt is the integration time, and ω is the angular velocity.

[0020] Furthermore, the homogeneous transformation matrix in step S44 is:

[0021]

[0022] Furthermore, obtaining the predicted value of the homogeneous transformation matrix of the current frame using the uniform motion model specifically includes:

[0023] S51. Given the homogeneous transformation matrix of the current frame and the homogeneous transformation matrix of the previous frame, calculate the pure translation transformation matrix between the current frame and the previous frame using the transformation relationship.

[0024] S52. Since it is assumed that the motion is uniform, the pure translation transformation matrix at the next moment is the same as that calculated in S62.

[0025] S53. Obtain the predicted value of the pure translation transformation matrix for the next frame;

[0026] S54. Multiply the predicted value of the pure translation transformation matrix obtained in step S53 with the pure rotation homogeneous transformation matrix in S44 to obtain the final predicted homogeneous transformation matrix.

[0027] Furthermore, the pure translation transformation matrix between the current frame and the previous frame in step S51 is:

[0028]

[0029] Furthermore, the predicted value of the pure translation transformation matrix in step S53 is:

[0030]

[0031] ΔT k+1 =ΔT k

[0032] Where, ΔT k+1 Let be the pure translation transformation matrix at time k+1. This is the pure translation transformation matrix of the previous frame.

[0033] Furthermore, the method also includes: caching the IMU data via a queue before executing step S3.

[0034] Compared with the prior art, the present invention has the following beneficial effects: The present invention adds an IMU, which records the rotation angle of the lidar, performs integral interpolation on the angular velocity data between two radar frames, and adds a rotation homogeneous transformation matrix to the uniform motion model, so that the system can provide stable positioning output even in the case of complex motion of the vehicle, improve the stability of the unmanned vehicle positioning during driving, and improve the robustness of the system. Attached Figure Description

[0035] Figure 1 This is a flowchart of pose calculation that integrates the NDT algorithm and inertial unit provided in an embodiment of the present invention.

[0036] Figure 2 This is a schematic diagram illustrating the IMU time synchronization and interpolation calculation principle in an embodiment of the present invention.

[0037] Figure 3 This is a diagram of the uniform motion model used in the pose calculation process. Detailed Implementation

[0038] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0039] When performing pose calculation using lidar and IMU (inertial measurement unit), this invention requires a 3D point cloud map of the environment. The method to obtain the 3D point cloud map can utilize mainstream open-source algorithms such as LIO-SAM and FAST-LIO. Based on the environmental map, this invention can be used for pose calculation and localization.

[0040] Example

[0041] Move the unmanned vehicle to the vicinity of the mapping origin, ensuring its orientation aligns with the mapping starting point. Then, start the vehicle. The positioning process is as follows:

[0042] S1. Start the unmanned mobile vehicle, load the environmental point cloud map, and acquire radar data and IMU data;

[0043] S2. Initialize the positioning data of the first frame of radar to obtain the initial pose of the moving vehicle;

[0044] S3, cached IMU data queue;

[0045] S4. Align the timestamps of the radar data and the IMU data;

[0046] S5. Extract the IMU data within one radar scan cycle time preceding the current frame time, calculate the rotation angle within that time period, and convert it into a homogeneous transformation matrix.

[0047] S6. Use the uniform motion model to obtain the predicted value of the homogeneous transformation matrix of the current frame, and multiply it with the homogeneous transformation matrix obtained by the IMU to obtain the final predicted value of the homogeneous transformation matrix.

[0048] S7. Use the prediction matrix to perform NDT registration for the current frame to obtain the pose at the new time step.

[0049] S8. Use the homogeneous transformation matrix obtained from registration as the new prediction value, and repeat steps S5-S7 to calculate the pose for the next frame.

[0050] Specifically, the method for obtaining the homogeneous transformation matrix of the IMU data in step S5 includes the following steps:

[0051] S51. Time synchronization is performed between the IMU and LiDAR sensors. Since the IMU and LiDAR are two relatively independent sensors with different sampling frequencies, after synchronizing the IMU time with the LiDAR timeline, multiple IMU angular velocity information will be available between two LiDAR frames, such as... Figure 2 As shown;

[0052] S52. Extract all IMU data within a radar scan cycle time period preceding the current frame time;

[0053] S53. Calculate the rotation angle traversed by the lidar between the two frames using integration. Since in industrial applications, moving vehicles generally do not experience pitch or roll motion, their corresponding angular velocities are negligible. Only the yaw angle is extracted. The formula for calculating the rotation angle is:

[0054] S54. Convert the calculated angle data into a homogeneous transformation matrix, as follows:

[0055]

[0056] Specifically, the method for obtaining the predicted values ​​of the uniform model and the homogeneous transformation matrix in step S6 includes the following steps:

[0057] S61. Predict the homogeneous translation matrix of the next frame using a uniform velocity model.

[0058] S62. Given the homogeneous transformation matrix of the current frame. Homogeneous transformation matrix of the previous frame Then, by using transformation relationships, such as Figure 3 As shown, the pure translation transformation matrix between the current frame and the previous frame is:

[0059]

[0060] S63. Since the assumption of uniform motion is made, it can be assumed that the pure translation transformation matrix at the next moment is the same as that calculated in S62, that is: ΔT k+1 =ΔT k ;

[0061] S64. The predicted value of the pure translation transformation matrix for the next frame is:

[0062] S65. Multiply the homogeneous translation matrix obtained in the previous step with the homogeneous rotation matrix obtained in S54 to obtain the final predicted homogeneous transformation matrix:

[0063]

[0064] This method obtains an initial pose and transformation matrix by performing NDT matching between the current frame point cloud acquired by radar and a prior map. In the next frame calculation, the transformation matrix obtained from the previous frame is fused with IMU data from the two radar frames to provide the initial transformation prediction matrix for the next frame's NDT matching, enabling pose calculation for the next frame. This method not only reduces the computational resource consumption of traditional NDT matching localization algorithms but also improves the stability of the entire localization system.

[0065] This embodiment also provides an unmanned vehicle positioning device, including: a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the unmanned vehicle positioning method.

[0066] This embodiment also provides a computer storage medium storing an executable program, which is executed by a processor to implement the steps of the unmanned vehicle positioning method.

[0067] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for locating unmanned vehicles combining the NDT algorithm and an inertial measurement unit, characterized in that, Including the following steps: S1. Start the unmanned mobile vehicle, load the environmental point cloud map, and acquire radar data and IMU data; S2. Initialize the positioning data of the first frame of radar to obtain the initial pose of the moving vehicle; S3. Align the timestamps of the radar data and the IMU data; S4. Extract the IMU data within one radar scan cycle time preceding the current frame time, calculate the rotation angle within that time period, and convert it into a homogeneous transformation matrix. S5. Use the uniform motion model to obtain the predicted value of the homogeneous transformation matrix of the current frame, and multiply it with the homogeneous transformation matrix obtained in step S4 to obtain the final predicted homogeneous transformation matrix. S6. Use the predicted homogeneous transformation matrix to perform NDT registration of the current frame to obtain the pose at the new time step. S7. Use the homogeneous transformation matrix obtained from registration as the new prediction value, and repeat steps S3-S6 to calculate the pose for the next frame.

2. The unmanned vehicle localization method combining NDT algorithm and inertial measurement unit according to claim 1, characterized in that, Step S4 specifically includes: S41. Synchronize the time between the IMU and the lidar sensors; S42. Extract all IMU data within a radar scan cycle time period preceding the current frame time; S43. Calculate the rotation angle of the lidar between two frames using integration; S44. Convert the calculated angle data into a homogeneous transformation matrix.

3. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 2, characterized in that, The turning angle in step S43 is: , For integration time, ω is the angular velocity.

4. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 3, characterized in that, The homogeneous transformation matrix in step S44 is: 。 5. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 1, characterized in that, Obtaining the predicted homogeneous transformation matrix of the current frame using a uniform motion model specifically includes: S51. Given the homogeneous transformation matrix of the current frame and the homogeneous transformation matrix of the previous frame, calculate the pure translation transformation matrix between the current frame and the previous frame using the transformation relationship. S52. Since the assumption is that the motion is uniform, the pure translation transformation matrix at the next moment is the same as that calculated in S51. S53. Obtain the predicted value of the pure translation transformation matrix for the next frame; S54. Multiply the predicted value of the pure translation transformation matrix obtained in step S53 with the homogeneous transformation matrix in S44 to obtain the final predicted homogeneous transformation matrix.

6. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 5, characterized in that, The pure translation transformation matrix between the current frame and the previous frame in step S51 is: 。 7. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 6, characterized in that, The predicted value of the pure translation transformation matrix in step S53 is: in, Let be the pure translation transformation matrix at time k+1. This is the pure translation transformation matrix of the previous frame.

8. The unmanned vehicle localization method combining NDT algorithm and inertial measurement unit according to claim 1, characterized in that, Also includes: Before executing step S3, the IMU data is cached through a queue.

9. A positioning device for unmanned vehicles, characterized in that, include: A memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the unmanned vehicle positioning method according to any one of claims 1-8.

10. A computer storage medium, characterized in that, The computer storage medium stores an executable program, which is executed by a processor to implement the steps of the unmanned vehicle positioning method according to any one of claims 1-8.

Images