Unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit

Abstract

The invention discloses an unmanned vehicle positioning method combining an NDT algorithm and an inertial measurement unit, and the method comprises the steps: carrying out the NDT matching of a current frame point cloud collected by a radar and a prior map, obtaining an initial pose and a transformation matrix, and carrying out the calculation of a next frame, and fusing the transformation matrix obtained by the previous frame and IMU data between two frames of radar data, giving an initial transformation prediction matrix matched with the NDT of the next frame, and carrying out attitude calculation of the next frame. The method not only reduces the computing resource consumption of the traditional NDT matching positioning algorithm, but also improves the stability of the whole positioning system.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation of unmanned vehicles, and in particular to an unmanned vehicle positioning method combining an NDT algorithm and an inertial measurement unit. Background Art

[0002] In the field of positioning and navigation technology for unmanned vehicles, odometer technology and positioning and navigation technology based on prior maps are two mainstream methods for obtaining position and posture. For mobile vehicles in industrial application scenarios (such as industrial AGVs, unmanned forklifts, etc.), it is often necessary to use positioning and navigation technology based on prior maps to obtain relatively fixed and repetitive moving routes. Therefore. Using the current radar scanning data to match the prior map is a more mainstream positioning method in the current industrial application environment. In the matching algorithm, the NDT algorithm can capture environmental features more comprehensively by modeling the distribution of point cloud data. This transformation method based on normal distribution makes the algorithm more suitable for processing complex and changeable environments.

[0003] However, most traditional NDT algorithms use the uniform motion assumption for pose estimation, and a relatively accurate prediction matrix needs to be provided before alignment. In actual applications, it is difficult to provide a prediction transformation matrix that matches the current motion state by relying solely on the prediction model, using only the data of a single sensor such as radar as the original input, making it difficult to provide a prediction transformation matrix that matches the current motion state, making the entire positioning system unstable. Especially when the vehicle is undergoing rapid acceleration, rapid deceleration, and rotational motion with a large angular velocity, NDT matching not only requires high computing resources and reduces the real-time positioning of the system, but is also prone to problems such as reduced positioning accuracy and even positioning loss, and its usability in products is not high. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the present invention patent designs an unmanned vehicle positioning method that combines the NDT algorithm and the inertial measurement unit, and uses the IMU data to correct the predicted rotation matrix. The purpose is that even when the vehicle is in a scenario with complex movement, the system can provide stable positioning output and improve the robustness of the system.

[0005] This application adopts the following technical solutions to achieve the above effects:

[0006] A method for positioning an unmanned vehicle combining an NDT algorithm and an inertial measurement unit, comprising the steps of:

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

[0008] S2, initializing the positioning data of the first frame of radar to obtain the initial position and posture of the moving vehicle;

[0009] S3, aligning the timestamp of the radar data with the timestamp of the IMU data;

[0010] S4, extract the IMU data within a radar scanning cycle time pushed forward from the current frame time, calculate the rotation angle within this time, and convert it into a homogeneous transformation matrix;

[0011] S5, using the uniform motion model to obtain the homogeneous transformation matrix prediction value of the current frame, and multiplying it with the homogeneous transformation matrix obtained in step S4 to obtain the final homogeneous transformation matrix prediction value;

[0012] S6, using the prediction matrix to perform NDT registration of the current frame to obtain the pose at a new moment;

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

[0014] Furthermore, the step S4 specifically includes:

[0015] S41, time synchronization of the IMU and laser radar sensors;

[0016] S42, extracting all IMU data within a radar scanning cycle time pushed forward from the current frame time;

[0017] S43, calculating the rotation angle of the laser radar between two frames by using integration;

[0018] S44, converting the calculated rotation 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 of step S44 is:

[0021]

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

[0023] S51, if the homogeneous transformation matrix of the current frame and the homogeneous transformation matrix of the previous frame are known, the pure translation transformation matrix between the current frame and the previous frame is calculated by using the transformation relationship;

[0024] S52, since it is assumed that the uniform speed is running, the pure translation transformation matrix at the next moment is the same as that calculated in S62;

[0025] S53, obtaining a predicted value of a pure translation transformation matrix of the next frame;

[0026] S54, multiplying the predicted value of the pure translation transformation matrix obtained in step S53 by the pure rotation homogeneous transformation matrix in S44 to obtain a 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 is the pure translation transformation matrix at time k+1, is the pure translation transformation matrix of the previous frame.

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

[0034] Compared with the prior art, the present invention has the following beneficial effects: the present invention adds an IMU, records the turning angle of the laser radar through the IMU, performs integral interpolation operations on the angular velocity data between two frames of radar time, and then adds a rotational homogeneous transformation matrix to the uniform motion model, so that the system can provide stable positioning output even in scenarios with complex vehicle motion, thereby improving the positioning stability of unmanned vehicles during driving and improving the robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 The figure is a flow chart of posture solution integrating NDT algorithm and inertial unit provided in an embodiment of the present invention.

[0036] Figure 2 This is a schematic diagram of 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 posture solution process. DETAILED DESCRIPTION

[0038] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0039] When the present invention uses lidar and IMU (inertial measurement unit) for posture settlement, it is necessary to first provide a three-dimensional point cloud map of the environment. The method for obtaining the three-dimensional point cloud map can utilize the current mainstream open source algorithms, such as LIO-SAM, FAST-LIO, etc. On the basis of having an environmental map, the present invention can be used for posture solution and positioning.

[0040] Example

[0041] Move the unmanned vehicle to the vicinity of the origin of the map, ensure that the orientation is consistent with the starting point of the map, and then start the vehicle. The positioning process is as follows:

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

[0043] S2, initializing the positioning data of the first frame of radar to obtain the initial position and posture of the moving vehicle;

[0044] S3, cache IMU data queue;

[0045] S4, aligning the timestamp of the radar data with the timestamp of the IMU data;

[0046] S5, extracting the IMU data within a radar scanning cycle time pushed forward from the current frame time, calculating the rotation angle within the time, and converting it into a homogeneous transformation matrix;

[0047] S6, using the uniform motion model to obtain the homogeneous transformation matrix prediction value of the current frame, and multiplying it with the homogeneous transformation matrix obtained by the IMU to obtain the final homogeneous transformation matrix prediction value;

[0048] S7, using the prediction matrix to perform NDT registration of the current frame to obtain the pose at a new moment;

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

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

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

[0052] S52, extracting all IMU data within a radar scanning cycle time pushed forward from the current frame time;

[0053] S53. Calculate the rotation angle of the laser radar between the two frames using integration. In industrial application environments, there is basically no pitch and roll motion in mobile vehicles, and the corresponding angular velocity can be ignored. Only the yaw angle is extracted. The formula for calculating the rotation angle is:

[0054] S54, converting the calculated rotation angle data into a homogeneous transformation matrix, the specific form is as follows:

[0055]

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

[0057] S61, predicting the pure translation homogeneous transformation matrix of the next frame using a uniform velocity model;

[0058] S62: The homogeneous transformation matrix of the current frame is known And the homogeneous transformation matrix of the previous frame Then use the transformation relationship, such as Figure 3 As shown, the pure translation transformation matrix between the current frame and the previous frame is:

[0059]

[0060] S63. Since it is assumed that the uniform speed operation is in progress, 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 of the next frame is obtained as follows:

[0062] S65, multiplying the pure translation homogeneous transformation matrix obtained in the previous step and the pure rotation homogeneous transformation matrix in S54, the final prediction homogeneous transformation matrix can be obtained as follows:

[0063]

[0064] This method performs NDT matching between the current frame point cloud collected by the radar and the prior map to obtain an initial posture and transformation matrix. When calculating the next frame, the transformation matrix obtained in the previous frame is fused with the IMU data between the two frames of radar data to give the initial transformation prediction matrix for the next frame NDT matching and perform posture solution for the next frame. This method not only reduces the computing resource consumption of the traditional NDT matching positioning algorithm, but also improves the stability of the entire positioning system.

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

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

[0067] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. An unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit, characterized in that: Includes steps: S1. Start the unmanned mobile vehicle, load the environmental point cloud map, and obtain radar data and IMU data; S2, initializing the positioning data of the first frame of radar to obtain the initial position and posture of the moving vehicle; S3, aligning the timestamp of the radar data with the timestamp of the IMU data; S4, extract the IMU data within a radar scanning cycle time pushed forward from the current frame time, calculate the rotation angle within this time, and convert it into a homogeneous transformation matrix; S5, using the uniform motion model to obtain the homogeneous transformation matrix prediction value of the current frame, and multiplying it with the homogeneous transformation matrix obtained in step S4 to obtain the final homogeneous transformation matrix prediction value; S6, using the prediction matrix to perform NDT registration of the current frame to obtain the pose at a new moment; S7. Use the homogeneous transformation matrix obtained by registration as a new prediction value, repeat steps S3-S6, and calculate the pose of the next frame.

2. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 1, characterized in that: The step S4 specifically includes: S41, time synchronization of the IMU and laser radar sensors; S42, extracting all IMU data within a radar scanning cycle time pushed forward from the current frame time; S43, calculating the rotation angle of the laser radar between two frames by using integration; S44, converting the calculated rotation 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: Δt is the integration time and ω 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 of step S44 is:

5. The unmanned vehicle positioning method combining NDT algorithm and inertial measurement unit according to claim 1, characterized in that: The use of the uniform motion model to obtain the homogeneous transformation matrix prediction value of the current frame specifically includes: S51, if the homogeneous transformation matrix of the current frame and the homogeneous transformation matrix of the previous frame are known, the pure translation transformation matrix between the current frame and the previous frame is calculated by using the transformation relationship; S52, since it is assumed that the uniform speed is running, the pure translation transformation matrix at the next moment is the same as that calculated in S62; S53, obtaining a predicted value of a pure translation transformation matrix of the next frame; S54, multiplying the predicted value of the pure translation transformation matrix obtained in step S53 by the pure rotation homogeneous transformation matrix in S44 to obtain a 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: ΔT k+1 =ΔT k Where, ΔT k+1 is the pure translation transformation matrix at time k+1, is the pure translation transformation matrix of the previous frame.

8. The unmanned vehicle positioning 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. An unmanned vehicle positioning device, characterized in that: include: A memory, a processor and a computer program stored in the memory, wherein the processor implements the steps of the unmanned vehicle positioning method according to any one of claims 1 to 8 when executing the computer program.

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

Images