Laser radar, millimeter wave radar and inertia fusion self-positioning method and device

Abstract

The invention provides a laser radar, millimeter wave radar and inertia fusion self-positioning method and device. The method comprises the following steps: firstly, estimating radar speed by using millimeter wave radar 4D point cloud data so as to obtain carrier speed, and carrying out system initialization; performing system prediction, performing motion distortion correction on a frame of laser point cloud by using a discrete inertial pose, and extracting plane features to perform point-to-surface matching based on spatial nearest neighbor; and finally, laser positioning degradation detection is carried out, and system updating is carried out in sequence according to observation equations of the laser radar and the millimeter wave radar. According to the invention, the speed of the self-positioning system is directly constrained and updated through the Doppler information of the low-cost millimeter-wave radar, and the positioning robustness is greatly improved when the geometric features of the laser radar sensor are degraded.

Description

Technical Field

[0001] The embodiments of the present invention relate to the fields of mobile measurement and robotics, and in particular to a self-positioning method and device that integrates laser radar, millimeter-wave radar, and inertial sensing. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology is a core component of autonomous robot navigation. Its estimation methods are mainly divided into optimization-based graph optimization frameworks and filtering-based probabilistic inference frameworks. Among them, SLAM solutions based on iterative error state Kalman filtering exhibit significant advantages in computational efficiency and positioning accuracy due to their state propagation properties. In recent years, with the significant reduction in the manufacturing cost of LiDAR (Light Detection and Ranging) sensors, tightly coupled SLAM systems that integrate LiDAR with inertial measurement units (IMUs) have been widely used in autonomous driving and mobile robotics. However, these systems often suffer from systematic failures such as pose estimation divergence and positioning trajectory drift in scenarios with degraded LiDAR geometry (such as long straight corridors and tunnels) or in harsh environments such as smoke and dust, severely limiting their robustness in complex industrial scenarios. Summary of the Invention

[0003] In order to overcome the shortcomings of the existing technology, the present invention provides a self-positioning method and equipment that integrates laser radar, millimeter-wave radar and inertial sensing to achieve robust and accurate self-positioning of unmanned platforms without external signal sources.

[0004] The self-positioning method designed by the present invention by integrating laser radar, millimeter wave radar and inertial radar includes the following steps: Acquire time-synchronized laser point cloud data, millimeter-wave radar 4D point cloud data, and inertial measurement data; Estimate radar velocity based on millimeter-wave radar 4D point cloud data, calculate carrier velocity, and initialize the state of the iterative error Kalman filter system; Use discrete inertial poses to correct motion distortion of a frame of laser point cloud, analyze the distribution characteristics of the point cloud, and obtain a subset of plane feature point cloud that conforms to the plane geometry characteristics. Match the plane feature points of the current frame with the reference plane in the map to obtain a point-to-plane matching pair. Based on point-to-surface matching and millimeter-wave radar velocity estimation, the iterative error Kalman filter system is updated through the observation equations of lidar point-to-surface and millimeter-wave radar velocity in turn, and the position and posture of the carrier are output to obtain the matching point cloud map and achieve self-positioning. Furthermore, estimating the radar speed based on the millimeter-wave radar 4D point cloud data includes: Based on the assumption that most targets in the environment are static, the relationship between the radial velocity of a static target and the radar velocity is established:

[0005] in Indicates the radial velocity of the static target point, represents the radar speed, represents the three-dimensional coordinates of the point, Represents the vector modulus; The expressions of radial velocity measurement and radar velocity are arranged into a matrix form. After removing outliers using the random sampling consistency algorithm, the radar velocity is obtained by the least squares method. The matrix form is expressed as follows:

[0006] in , represents point P i The direction vector, i =1 2 ... n .

[0007] Furthermore, the system initialization includes: Initialize the system state, including posture, velocity, gravity direction and inertial measurement bias; Set the velocity to the calculated carrier velocity, the gravity direction to the direction vector of the average linear acceleration within 5 seconds, and the gyroscope measurement bias to the average angular velocity within 5 seconds.

[0008] Furthermore, the motion distortion correction of the laser point cloud includes: Each laser point searches for two system poses that are close in time, obtains the pose of the laser point at that moment through linear interpolation, and projects the laser point to the end of the frame based on the pose to complete motion distortion correction.

[0009] Furthermore, the point cloud is downsampled and a kd-tree is constructed. Then, a K-nearest neighbor query is performed on each downsampled point, the covariance matrix of the point is calculated, and the eigenvalue decomposition of the covariance matrix is ​​performed. When the ratio of the two smallest eigenvalues ​​to the largest eigenvalue is less than the set threshold, the point is determined to be a plane feature point.

[0010] Furthermore, the plane feature points of the current frame are projected into the reference coordinate system, and a spatial nearest neighbor search is performed in the matching point cloud map. Matching pairs whose distance between the feature point and the nearest matching point is greater than a certain threshold are eliminated, and the local plane equation is obtained by fitting using several nearest matching points:

[0011] in Represents the local fitted plane normal vector , and finally get the point-to-face matching pair , and d are the plane parameters corresponding to the map matching points fitted.

[0012] Furthermore, before the system update, laser positioning degradation detection is performed, specifically including: Construct the Jacobian matrix based on the laser point matching information to detect whether the laser positioning is degraded; If degradation occurs, the laser Jacobian matrix is ​​reconstructed using the truncated singular value decomposition method, and the system is updated sequentially through the laser point-to-surface observation equation and the millimeter-wave radar velocity observation equation.

[0013] Based on the same inventive concept, the present invention also provides a self-positioning device that integrates laser radar, millimeter-wave radar, and inertial sensing, including: LiDAR, used to obtain laser point cloud data; Millimeter-wave radar, used to acquire 4D point cloud data; Inertial measurement unit (IMU), used to obtain acceleration and angular velocity data; Processor for executing robust self-localization method combining lidar, millimeter-wave radar and inertial fusion.

[0014] Furthermore, the laser radar, millimeter wave radar and IMU achieve data synchronization through a hardware time synchronization mechanism.

[0015] Furthermore, an STM32 microcontroller is used to achieve data synchronization.

[0016] The advantages of the present invention are that: the present invention utilizes the Doppler information of low-cost millimeter-wave radar, adds radar velocity update to the iterative error Kalman filter framework, directly constrains and updates the velocity of the self-positioning system, and greatly improves the robustness of positioning when the geometric features of the lidar sensor are degraded. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a flow chart of a robust self-positioning method that integrates laser radar, millimeter-wave radar, and inertial navigation, provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0018] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0019] This embodiment provides a self-positioning method that integrates laser radar, millimeter wave radar and inertial radar. The flow chart is as shown in the attached figure. Figure 1 The method specifically includes the following steps: Step 1: Acquire time-synchronized laser point cloud data, millimeter-wave radar 4D point cloud data, and inertial measurement data, including: Step 1.1: Obtain sequential laser frame data with x, y, z coordinates and measurement time through the lidar; Step 1.2: Obtain serial radar frame data with x, y, z, doppler and measurement time through millimeter wave radar; Step 1.3: Obtain IMU data with time stamps through the inertial measurement unit, including three-axis acceleration data and three-axis angular velocity data.

[0020] This embodiment uses hardware to achieve time synchronization, specifically using an STM32 microcontroller for time synchronization.

[0021] Step 2: Use the millimeter-wave radar 4D point cloud data to estimate the radar velocity, thereby obtaining the carrier velocity, and perform system initialization, including: Step 2.1: Based on the assumption that most targets in the environment are static, use the millimeter-wave radar 4D point cloud data to estimate the radar velocity. The radial velocity of a static target point is related to the radar velocity as follows: (1) in Indicates the radial velocity of the static target point, represents the radar speed, represents the three-dimensional coordinates of the point, Represents the vector modulus. According to formula (1), each radial velocity measurement and the expression of the radar velocity are arranged into a matrix form, as shown in formula (2). Considering the influence of a small number of dynamic points, false objects and other outliers on the estimation, the random sampling consistency algorithm is first used to remove the outliers, and then the least squares method is used to solve formula (2) to obtain the radar velocity Specific selections of relevant parameters in the random sampling consensus algorithm in this embodiment include: a minimum number of sampling points of 3, a distance threshold of 0.15, a confidence level of 0.9999, and an inlier ratio of 0.15.

[0022] (2) in , represents point P i The direction vector, i =1 2 ... n .

[0023] Step 2.2: Calculate the carrier velocity based on the radar velocity, the external parameters between the radar and the carrier (including the rotation matrix and translation vector from the radar to the carrier coordinate system), and the carrier angular velocity (obtained from the inertial measurement unit): (3) in is the rotation matrix and translation vector from the carrier to the radar coordinate system, is the current angular velocity of the carrier.

[0024] Step 2.3: If the iterative error Kalman filter system has not been initialized, the system state quantity This includes initialization of the pose, velocity, gravity direction, and inertial measurement bias. The velocity is set to the calculated carrier velocity, the gravity direction is set to the direction vector of the average linear acceleration over the past 5 seconds, the gyroscope measurement bias is set to the average angular velocity over the past 5 seconds, and all other variables are set to zero or the identity matrix. The reference coordinate system is defined with the system's initial position as the origin and the Z axis as the direction opposite to the direction of gravity.

[0025] Step 3: Perform iterative error Kalman filter system prediction, use discrete inertial pose to correct motion distortion of a frame of laser point cloud, and extract plane features for point-to-surface matching based on spatial nearest neighbors, including: Step 3.1: Use inertial measurements to make system predictions: (4) in is a set of inertial measurements within the laser frame time span, is the kinematic equation. Motion distortion correction is performed based on the system's predicted pose. Each laser point searches for two temporally adjacent system poses, obtains the pose at that point through linear interpolation, and projects the laser point to the end of the frame based on this pose to complete motion distortion correction.

[0026] Step 3.2: Use principal component analysis to determine the geometric features of the local point cloud and extract planar features. The extraction process is as follows. The point cloud is downsampled and a kd tree is constructed to speed up the subsequent K-nearest neighbor query. Next, a K-nearest neighbor query is performed on each downsampled point, and the covariance matrix C of the point is calculated: (5) in is one of the nearest neighbor points found. is the average value of all neighboring points. Perform eigenvalue decomposition to obtain eigenvalues ,in .when and If the distance between the feature point and the nearest matching point is greater than a certain threshold, the point is considered to be a plane feature point. In this embodiment, the threshold is set to 0.05. According to the predicted pose, the plane feature points of the current frame are projected into the reference coordinate system, and a spatial nearest neighbor search is performed in the matching point cloud map. Matching pairs where the distance between the feature point and the nearest matching point is greater than a certain threshold are eliminated, and the local plane equation is obtained by fitting using the nearest 5 matching points: (6) in Represents the local fitted plane normal vector , and finally get the point-to-face matching pair .

[0027] Step 4: Perform laser positioning degradation detection and perform iterative error Kalman filter system updates based on the observation equations of the laser radar and millimeter wave radar, including: Step 4.1: Based on the laser point matching information, construct the laser Jacobian matrix H and perform laser positioning degradation detection; Step 4.2: Update the system in sequence according to the time stamp at the end of the laser point cloud frame and the time stamp at the millimeter wave radar velocity. The constructed laser point-to-surface observation equation and millimeter wave radar velocity observation equation are as follows: (7) (8) in Represents the characteristic points of the laser plane; and d are the plane parameters corresponding to the map matching points fitted; Represents the external parameters of the LiDAR to vehicle coordinate system; Represents antisymmetric matrix operations; and Respectively represent the rotation and translation from the carrier coordinate system to the reference coordinate system; Indicates the gyroscope bias; are the rotation matrix and translation vector from the radar to the carrier coordinate system. When updating the system based on the lidar point-to-surface observation equation, if laser positioning degradation occurs, the laser Jacobian matrix is ​​reconstructed using the truncated singular value decomposition method to participate in the update. After the system update, the laser frame point cloud is projected into the reference coordinate system, and the current frame point cloud is added to the map. Downsampling is performed at a certain resolution to obtain a matching point cloud map.

Claims

1. A self-positioning method combining laser radar, millimeter wave radar and inertial sensing, characterized in that: The following steps are involved: Acquire time-synchronized laser point cloud data, millimeter-wave radar 4D point cloud data, and inertial measurement data; Estimate radar velocity based on millimeter-wave radar 4D point cloud data, calculate carrier velocity, and initialize the state of the iterative error Kalman filter system; Use discrete inertial poses to correct motion distortion of a frame of laser point cloud, analyze the distribution characteristics of the point cloud, and obtain a subset of plane feature point cloud that conforms to the plane geometry characteristics. Match the plane feature points of the current frame with the reference plane in the map to obtain a point-to-plane matching pair. Based on point-to-surface matching and millimeter-wave radar velocity estimation, the iterative error Kalman filter system is updated through the observation equations of lidar point-to-surface and millimeter-wave radar velocity in turn, and the position and posture of the carrier are output to obtain the matching point cloud map and achieve self-positioning.

2. The self-positioning method of laser radar, millimeter wave radar and inertial fusion according to claim 1 is characterized by: The method of estimating radar speed based on millimeter wave radar 4D point cloud data includes: Based on the assumption that most targets in the environment are static, the relationship between the radial velocity of a static target and the radar velocity is established: in Indicates the radial velocity of the static target point, represents the radar speed, represents the three-dimensional coordinates of the point, Represents the vector modulus; The expressions of radial velocity measurement and radar velocity are arranged into a matrix form. After removing outliers using the random sampling consistency algorithm, the radar velocity is obtained by the least squares method. The matrix form is expressed as follows: in , represents point P i The direction vector, i =1 2 ... n .

3. The self-positioning method of laser radar, millimeter wave radar and inertial fusion according to claim 1 is characterized by: The system initialization includes: Initialize the system state, including posture, velocity, gravity direction and inertial measurement bias; Set the velocity to the calculated carrier velocity, the gravity direction to the direction vector of the average linear acceleration within 5 seconds, and the gyroscope measurement bias to the average angular velocity within 5 seconds.

4. The self-positioning method of laser radar, millimeter wave radar and inertial fusion according to claim 1 is characterized by: The motion distortion correction of the laser point cloud includes: Each laser point searches for two system poses that are close in time, obtains the pose of the laser point at that moment through linear interpolation, and projects the laser point to the end of the frame based on the pose to complete motion distortion correction.

5. The self-positioning method of laser radar, millimeter wave radar and inertial fusion according to claim 1 is characterized in that: The point cloud is downsampled and a kd-tree is constructed. Then, a K-nearest neighbor query is performed on each downsampled point, the covariance matrix of the point is calculated, and the eigenvalue decomposition of the covariance matrix is ​​performed. When the ratio of the two smallest eigenvalues ​​to the largest eigenvalue is less than the set threshold, the point is determined to be a plane feature point.

6. The self-positioning method of laser radar, millimeter wave radar and inertial fusion according to claim 1 or 5, characterized in that: Project the current frame's plane feature points to the reference coordinate system, perform a spatial nearest neighbor search in the matching point cloud map, eliminate matching pairs where the distance between the feature point and the nearest matching point is greater than a certain threshold, and use the nearest matching points to fit the local plane equation: in Represents the local fitted plane normal vector , and finally get the point-to-face matching pair , and d are the plane parameters corresponding to the map matching points fitted.

7. The self-positioning method of laser radar, millimeter wave radar and inertial fusion according to claim 1 is characterized by: Before performing a system update, perform a laser positioning degradation test, including: Construct the Jacobian matrix based on the laser point matching information to detect whether the laser positioning is degraded; If degradation occurs, the laser Jacobian matrix is ​​reconstructed using the truncated singular value decomposition method, and the system is updated sequentially through the laser point-to-surface observation equation and the millimeter-wave radar velocity observation equation.

8. A self-positioning device that integrates laser radar, millimeter wave radar and inertial, characterized in that: include: LiDAR, used to obtain laser point cloud data; Millimeter-wave radar, used to acquire 4D point cloud data; Inertial measurement unit, used to obtain acceleration and angular velocity data; A processor, configured to execute the method according to any one of claims 1 to 7.

9. The device according to claim 8, characterized in that: The laser radar, millimeter wave radar and IMU achieve data synchronization through a hardware time synchronization mechanism.

10. The device according to claim 7, characterized in that: The STM32 microcontroller is used to achieve data synchronization.

Images