Laser vision inertial fusion positioning method for positioning error distribution and trajectory optimization

Abstract

The invention provides a laser vision inertial fusion positioning method for positioning error distribution and trajectory optimization, which comprises the following steps: S1, calculating error distribution of pose estimation when an inertial unit, a laser radar and a visible light camera are independently used for pose estimation; s2, according to characteristics of different scenes, different fusion weights are distributed to the inertial unit, the laser radar and the visible light camera; and S3, using a spline curve to model the motion trajectory of the vehicle, and optimizing a trajectory equation in combination with pose estimation results of different weights. According to the method, instability of the whole system caused by failure of a certain sensor is avoided, and the robustness of pose estimation is improved; the optimization quantity is a continuous trajectory equation instead of a discrete pose, the vehicle pose at any moment can be inquired, and the flexibility of pose estimation is improved; the number of the sensors can be increased or decreased at will, and the calculated amount caused by the sensors cannot be increased exponentially, so that the expandability of the system is improved.

Description

Technical Field

[0001] The invention provides a laser vision-inertial fusion positioning method for positioning error distribution and trajectory optimization, belonging to the technical field of vehicle control. Background Art

[0002] In scenarios where there is no external positioning source, the vehicle needs to rely on its own sensors to determine the vehicle's position and posture, which is used to control the vehicle's movement, go to a predetermined location, avoid obstacles during driving, etc. Related background technologies include:

[0003] Patent application number CN202410430748.9 provides an autonomous driving fusion positioning method and system. The invention uses an inertial unit, lidar and vector map information to jointly optimize the vehicle posture, but not all scenarios have pre-established vector maps, so this method is not universal.

[0004] Patent application number CN202410432672.3 provides a fusion positioning method and related equipment of lidar and millimeter-wave radar. The invention uses lidar and millimeter-wave radar to jointly optimize the vehicle posture. The method only uses millimeter-wave radar to screen the point cloud of the lidar, which can improve the stability of the positioning results in the scenario described in the invention, but it is difficult to improve the accuracy of the positioning results in the lidar point cloud degraded scenario and multi-rotation scenario. Summary of the invention

[0005] The current vehicle posture estimation method mainly relies on various sensors, such as inertial units, lidar, visible light cameras, etc. When using a single sensor for posture estimation, there are defects such as low positioning accuracy, poor stability, and poor scalability. The present invention intends to use the above sensors at the same time to improve the accuracy, stability and scalability of posture estimation.

[0006] The laser visual inertial fusion positioning method for positioning error distribution and trajectory optimization includes the following steps:

[0007] S1: Calculate the error distribution of pose estimation when using inertial unit, lidar, and visible light camera alone. Specifically:

[0008] (1) Calculate the error distribution when using inertial units for pose estimation;

[0009] The Kalman filter based on error state is used to estimate the position and posture of the inertial unit. The state quantity estimated by this method is the error state:

[0010]

[0011] in, represents the error state, x i+1Indicates the real state, Represents the estimated state. Linearizing and expanding the above equation can obtain the propagation equation of the error state:

[0012]

[0013] in, and F w are two sparse matrices, w i Represents noise, and its covariance matrix is ​​Q. Then we can get the error distribution of inertial unit pose estimation:

[0014]

[0015] (2) Calculate the error distribution when using LiDAR for pose estimation;

[0016] The laser radar pose is estimated by point cloud matching. For two consecutive frames of point cloud (current frame and previous frame), the kd-tree data structure is established for the previous frame point cloud. For each point in the current frame point cloud, the nearest neighbor search algorithm is used to search for the k nearest points in the kd-tree data structure of the previous frame point cloud. The RANSAC plane fitting algorithm is used to fit these k nearest points into a plane and save the normal vector of the plane. After traversing all points in the current frame point cloud, the normal vectors of all planes can be saved in a 3-row and N-column matrix by column, where N represents the number of points in the current frame point cloud.

[0017] NN T Matrix decomposition can obtain the eigenvalue matrix ∑ and the corresponding eigenvector matrix Q, then the error distribution of the lidar pose estimation can be expressed as:

[0018] cov LiDAR =Q∑ -1 Q T

[0019] (3) Calculate the error distribution when using a visible light camera for pose estimation;

[0020] The pose estimation of the visible light camera is performed by feature matching and sliding window optimization. For k key frames within a period of time, the two-dimensional feature points of the k key frame images are extracted, these feature points are tracked using the optical flow method, and the three-dimensional feature points corresponding to these two-dimensional feature points are calculated by triangulation. The camera pose and three-dimensional feature point coordinates corresponding to the key frames in the current time window are jointly optimized, and the objective function is established as the least squares optimization problem of the reprojection error of all feature points, and the ceres solver is used to solve it. By calling the covariance matrix of the solved problem in the ceres solver, the error distribution cov of the visible light camera pose estimation can be directly obtained.camera .

[0021] S2: According to the characteristics of different scenes, different fusion weights are assigned to the inertial unit, lidar, and visible light camera.

[0022] The inertial unit is a proprioceptive sensor, and the quality of the measurement data does not depend on the geometric or texture characteristics of the scene. Therefore, the error distribution of the inertial unit pose estimation is relatively stable in different scenes.

[0023] LiDAR is an external perception sensor that mainly measures the geometric structure of the scene. Therefore, the error distribution of LiDAR pose estimation varies greatly in different scenarios. In scenes with rich geometric structures, the accuracy of LiDAR pose estimation is better and a higher fusion weight can be assigned; however, in scenes with repeated or single geometric structures, the accuracy of LiDAR pose estimation is poor and a lower fusion weight needs to be assigned.

[0024] Visible light cameras are also external perception sensors that mainly measure the texture information of the scene. Therefore, the error distribution of visible light camera pose estimation varies greatly in different scenes. In scenes with rich textures and appropriate lighting, the accuracy of visible light camera pose estimation is better and a higher fusion weight can be assigned; however, in scenes with missing textures or poor lighting conditions, the accuracy of visible light camera pose estimation is poor and a lower fusion weight needs to be assigned.

[0025] S3: Use spline curves to model the vehicle's motion trajectory, combine the pose estimation results with different weights, and optimize the trajectory equation.

[0026] The discrete vehicle motion state can be expressed as a time series of postures. After fitting the sequence with a spline curve, a continuous vehicle trajectory equation can be obtained. For the posture time series obtained by the inertial unit, lidar, and visible light camera, as well as the corresponding fusion weights, the trajectory optimization objective function is established:

[0027]

[0028] Where Γ represents the trajectory equation, N sensor Indicates the number of sensors, N i represents the number of poses contained in the i-th sensor, Γ(t j ) indicates t j The pose obtained by the trajectory equation at the moment, T i (t j ) indicates that the i-th sensor is at t j The estimated pose at the moment, cov i，j Indicates that the i-th sensor is at t j Error distribution of moment estimates.

[0029] After the optimization is completed, the trajectory equation of the vehicle within the time series range is obtained, and the corresponding vehicle posture can be obtained by inputting different time periods.

[0030] The present invention improves the accuracy of posture estimation by fusing data from multiple sensors. The specific technical effects are:

[0031] (1) It avoids the instability of the entire system due to the failure of a certain sensor and improves the robustness of pose estimation;

[0032] (2) The optimization quantity is a continuous trajectory equation rather than a discrete pose, which allows the vehicle pose to be queried at any time, thus improving the flexibility of pose estimation.

[0033] (3) The number of sensors can be increased or decreased at will, and the resulting computational effort will not increase exponentially, thus improving the scalability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 is a flow chart of the method of the present invention;

[0035] Figure 2 Fitting a plane schematic diagram for the k closest points of the embodiment;

[0036] Figure 3 A schematic diagram of triangulating two-dimensional feature points into three-dimensional feature points in an embodiment;

[0037] Figure 4 A schematic diagram of trajectories for optimizing multiple sensor pose sequences according to an embodiment.

[0038] Specific implementation method

[0039] The specific technical solution of the present invention is explained in conjunction with embodiments.

[0040] Example 1

[0041] The sensors used in this embodiment include: an inertial unit, a laser radar, and two visible light cameras.

[0042] Process such as Figure 1 As shown, the following steps are included:

[0043] S1: Calculate the error distribution of pose estimation when using inertial unit, lidar, and visible light camera alone for pose estimation.

[0044] (1) Calculate the error distribution when using inertial units for pose estimation.

[0045] The Kalman filter based on error state is used to estimate the position and posture of the inertial unit. The state quantity estimated by this method is the error state:

[0046]

[0047] in, represents the error state, x i+1 Indicates the real state, Represents the estimated state. Linearizing and expanding the above equation can obtain the propagation equation of the error state:

[0048]

[0049] in, and F w are two sparse matrices, w i Represents noise, and its covariance matrix is ​​Q. Then we can get the error distribution of inertial unit pose estimation:

[0050]

[0051] (2) Calculate the error distribution when using lidar for pose estimation.

[0052] The laser radar pose is estimated by point cloud matching. For two consecutive frames of point cloud (current frame and previous frame), the previous frame point cloud is used to establish a kd-tree data structure. For each point in the current frame point cloud, the nearest neighbor search algorithm is used to search for the five closest points in the kd-tree data structure of the previous frame point cloud. The RANSAC plane fitting algorithm is used to fit these five closest points into a plane, such as Figure 2 As shown, the normal vector of the plane is saved. After traversing all points in the point cloud of the current frame, the normal vectors of all planes can be saved column by column in a matrix of 3 rows and N columns, where N represents the number of points in the point cloud of the current frame.

[0053] NN T Matrix decomposition can obtain the eigenvalue matrix ∑ and the corresponding eigenvector matrix Q, then the error distribution of the lidar pose estimation can be expressed as:

[0054] cov LiDAR =QΣ -1 Q T

[0055] (3) Calculating the error distribution when using a visible light camera for pose estimation

[0056] The pose estimation of the visible light camera is performed by feature matching and sliding window optimization. For 10 key frames within a period of time, the 2D feature points of the 10 key frame images are extracted, these feature points are tracked using the optical flow method, and the 3D feature points corresponding to these 2D feature points are calculated using triangulation, such as Figure 3The camera pose and 3D feature point coordinates corresponding to the key frame in the current time window are jointly optimized, and the objective function is established as the least squares optimization problem of the reprojection error of all feature points, and the ceres solver is used to solve it. By calling the covariance matrix of the solved problem in the ceres solver, the error distribution cov of the visible light camera pose estimation can be directly obtained. camera .

[0057] S2: According to the characteristics of different scenes, different fusion weights are assigned to the inertial unit, lidar, and visible light camera.

[0058] The inertial unit is a proprioceptive sensor, and the quality of the measurement data does not depend on the geometric or texture characteristics of the scene. Therefore, the error distribution of the inertial unit pose estimation is relatively stable in different scenes.

[0059] LiDAR is an external perception sensor that mainly measures the geometric structure of the scene. Therefore, the error distribution of LiDAR pose estimation varies greatly in different scenarios. In scenes with rich geometric structures, the accuracy of LiDAR pose estimation is better and a higher fusion weight can be assigned; however, in scenes with repeated or single geometric structures, the accuracy of LiDAR pose estimation is poor and a lower fusion weight needs to be assigned.

[0060] Visible light cameras are also external perception sensors that mainly measure the texture information of the scene. Therefore, the error distribution of visible light camera pose estimation varies greatly in different scenes. In scenes with rich textures and appropriate lighting, the accuracy of visible light camera pose estimation is better and a higher fusion weight can be assigned; however, in scenes with missing textures or poor lighting conditions, the accuracy of visible light camera pose estimation is poor and a lower fusion weight needs to be assigned.

[0061] S3: Use b-spline curves to model the vehicle's motion trajectory, combine the pose estimation results with different weights, and optimize the trajectory equation.

[0062] The discrete vehicle motion state can be expressed as a time series of position and posture. After fitting the sequence with a B-spline curve, a continuous vehicle trajectory equation can be obtained, such as Figure 4 As shown. For the pose time series obtained by the inertial unit, lidar, and visible light camera, as well as the corresponding fusion weights, the trajectory optimization objective function is established:

[0063]

[0064] Where Γ represents the b-spline trajectory equation, N i represents the number of poses contained in the i-th sensor, Γ(t j ) indicates t jThe pose obtained by the trajectory equation at the moment, T i (t j ) indicates that the i-th sensor is at t j The estimated pose at the moment, cov i，j Indicates that the i-th sensor is at t j Error distribution of moment estimates.

[0065] After the optimization is completed, the trajectory equation of the vehicle within the time series range is obtained, and the corresponding vehicle posture can be obtained by inputting different time periods.

[0066] Advantages of this embodiment: Fusion of data from multiple sensors improves the accuracy of pose estimation, avoids instability of the entire system due to failure of a certain sensor, improves the robustness of pose estimation, the optimization quantity is a continuous trajectory equation rather than a discrete pose, and the vehicle pose at any time can be queried, which improves the flexibility of pose estimation.

[0067] Example 2

[0068] The sensors used in this embodiment include: one inertial unit, two laser radars, and four visible light cameras.

[0069] S1: Calculate the error distribution of pose estimation when using inertial unit, lidar, and visible light camera alone for pose estimation.

[0070] (1) Calculate the error distribution when using inertial units for pose estimation.

[0071] The Kalman filter based on error state is used to estimate the position and posture of the inertial unit. The state quantity estimated by this method is the error state:

[0072]

[0073] in, represents the error state, x i+1 Indicates the real state, Represents the estimated state. Linearizing and expanding the above equation can obtain the propagation equation of the error state:

[0074]

[0075] in, and F w are two sparse matrices, w i Represents noise, and its covariance matrix is ​​Q. Then we can get the error distribution of inertial unit pose estimation:

[0076]

[0077] (2) Calculate the error distribution when using lidar for pose estimation.

[0078] The laser radar pose is estimated by point cloud matching. For two consecutive frames of point cloud (current frame and previous frame), the previous frame point cloud is used to establish a kd-tree data structure. For each point in the current frame point cloud, the nearest neighbor search algorithm is used to search for the 10 closest points in the kd-tree data structure of the previous frame point cloud. The RANSAC plane fitting algorithm is used to fit these 10 closest points into a plane, such as Figure 2 As shown, the normal vector of the plane is saved. After traversing all points in the point cloud of the current frame, the normal vectors of all planes can be saved column by column in a matrix of 3 rows and N columns, where N represents the number of points in the point cloud of the current frame.

[0079] NN T Matrix decomposition can obtain the eigenvalue matrix ∑ and the corresponding eigenvector matrix Q, then the error distribution of the lidar pose estimation can be expressed as:

[0080] cov LiDA R R =QΣ -1 Q T

[0081] (3) Calculate the error distribution when using a visible light camera for pose estimation.

[0082] The pose estimation of the visible light camera is performed by feature matching and sliding window optimization. For 20 key frames within a period of time, the 2D feature points of the 20 key frame images are extracted, these feature points are tracked using the optical flow method, and the 3D feature points corresponding to these 2D feature points are calculated using triangulation, such as Figure 3 The camera pose and 3D feature point coordinates corresponding to the key frame in the current time window are jointly optimized, and the objective function is established as the least squares optimization problem of the reprojection error of all feature points, and the ceres solver is used to solve it. By calling the covariance matrix of the solved problem in the ceres solver, the error distribution cov of the visible light camera pose estimation can be directly obtained. camera .

[0083] S2: According to the characteristics of different scenes, different fusion weights are assigned to the inertial unit, lidar, and visible light camera.

[0084] The inertial unit is a proprioceptive sensor, and the quality of the measurement data does not depend on the geometric or texture characteristics of the scene. Therefore, the error distribution of the inertial unit pose estimation is relatively stable in different scenes.

[0085] LiDAR is an external perception sensor that mainly measures the geometric structure of the scene. Therefore, the error distribution of LiDAR pose estimation varies greatly in different scenarios. In scenes with rich geometric structures, the accuracy of LiDAR pose estimation is better and a higher fusion weight can be assigned; however, in scenes with repeated or single geometric structures, the accuracy of LiDAR pose estimation is poor and a lower fusion weight needs to be assigned.

[0086] Visible light cameras are also external perception sensors that mainly measure the texture information of the scene. Therefore, the error distribution of visible light camera pose estimation varies greatly in different scenes. In scenes with rich textures and appropriate lighting, the accuracy of visible light camera pose estimation is better and a higher fusion weight can be assigned; however, in scenes with missing textures or poor lighting conditions, the accuracy of visible light camera pose estimation is poor and a lower fusion weight needs to be assigned.

[0087] S3: Use polynomial spline curves to model the vehicle's motion trajectory, combine the pose estimation results with different weights, and optimize the trajectory equation.

[0088] The discrete vehicle motion state can be expressed as a time series of position and posture. After fitting the sequence with a polynomial spline curve, a continuous vehicle trajectory equation can be obtained, such as Figure 4 As shown. For the pose time series obtained by the inertial unit, lidar, and visible light camera, as well as the corresponding fusion weights, the trajectory optimization objective function is established:

[0089]

[0090] Where Γ represents the polynomial spline trajectory equation, N i represents the number of poses contained in the i-th sensor, Γ(t j ) indicates t j The pose obtained by the trajectory equation at the moment, T i (t j ) indicates that the i-th sensor is at t j The estimated pose at the moment, cov i，j Indicates that the i-th sensor is at t j Error distribution of moment estimates.

[0091] After the optimization is completed, the trajectory equation of the vehicle within the time series range is obtained, and the corresponding vehicle posture can be obtained by inputting different time periods.

[0092] Advantages of the implementation method: Fusion of data from multiple sensors improves the accuracy of pose estimation, avoids instability of the entire system due to failure of a sensor, improves the robustness of pose estimation, and the optimization quantity is a continuous trajectory equation rather than a discrete pose, which allows querying the vehicle pose at any time and improves the flexibility of pose estimation. At the same time, the number of sensors can be increased or decreased at will, and the resulting computational effort will not increase exponentially, which improves the scalability of the system.

Claims

1. A laser visual-inertial fusion positioning method for positioning error distribution and trajectory optimization, characterized in that: The following steps are involved: S1: Calculate the error distribution of pose estimation when using inertial unit, lidar, and visible light camera alone; S2: According to the characteristics of different scenes, different fusion weights are assigned to the inertial unit, lidar, and visible light camera respectively; S3: Use spline curves to model the vehicle's motion trajectory, combine the pose estimation results with different weights, and optimize the trajectory equation.

2. The laser visual inertial fusion positioning method for positioning error distribution and trajectory optimization according to claim 1, characterized in that: S1 includes the following: (1) Calculate the error distribution when using inertial units for pose estimation; The Kalman filter based on error state is used to estimate the position and posture of the inertial unit, and the estimated state quantity is the error state: in, represents the error state, x i+1 Indicates the real state, Represents the estimated state; linearize and expand the above equation to obtain the propagation equation of the error state: in, and F w are two sparse matrices, w i Represents noise, whose covariance matrix is ​​Q; then the error distribution of inertial unit pose estimation is obtained: (2) Calculate the error distribution when using LiDAR for pose estimation; The laser radar pose is estimated by point cloud matching. For two consecutive point cloud frames, i.e., the current frame and the previous frame, the previous frame point cloud is used to establish a kd-tree data structure. For each point in the current frame point cloud, the nearest neighbor search algorithm is used to search for the k nearest points in the kd-tree data structure of the previous frame point cloud. The RANSAC plane fitting algorithm is used to fit these k nearest points into a plane, and the normal vector of the plane is saved. After traversing all points in the current frame point cloud, the normal vectors of all planes are saved column by column in a 3-row and N-column matrix, where N represents the number of points in the current frame point cloud. NN T Matrix decomposition is performed to obtain the eigenvalue matrix ∑ and the corresponding eigenvector matrix Q. The error distribution of the lidar pose estimation is expressed as: those LiDAR =Q∑ -1 Q T (3) Calculating the error distribution when using a visible light camera for pose estimation The pose of the visible light camera is estimated by feature matching and sliding window optimization. For k key frames within a period of time, the two-dimensional feature points of the k key frame images are extracted, and these feature points are tracked using the optical flow method, and the three-dimensional feature points corresponding to these two-dimensional feature points are calculated by triangulation. The camera pose and three-dimensional feature point coordinates corresponding to the key frames in the current time window are jointly optimized, and the objective function is established as the least squares optimization problem of the reprojection error of all feature points, and the ceres solver is used to solve it. The error distribution cov of the pose estimation of the visible light camera is obtained by calling the covariance matrix of the solved problem in the ceres solver. camera .

3. The laser visual inertial fusion positioning method with positioning error distribution and trajectory optimization according to claim 1, characterized in that: In S2, the inertial unit is a proprioceptive sensor. The quality of the measurement data does not depend on the geometric or texture characteristics of the scene. The error distribution of the inertial unit pose estimation remains stable in different scenes. LiDAR is an external perception sensor that measures the geometric structure of the scene. In scenes with rich geometric structures, it is assigned a high fusion weight; but in scenes with repeated or single geometric structures, it is assigned a low fusion weight. Visible light cameras are also external perception sensors that measure the texture information of the scene. In scenes with rich textures and appropriate lighting, they are assigned higher fusion weights; but in scenes with missing textures or poor lighting conditions, they are assigned lower fusion weights.

4. The laser visual inertial fusion positioning method with positioning error distribution and trajectory optimization according to claim 1, characterized in that: The specific methods of S3 are: The discrete vehicle motion state is represented as a time series of postures. The sequence is fitted with a spline curve to obtain a continuous vehicle trajectory equation. For the posture time series obtained by the inertial unit, lidar, and visible light camera, as well as the corresponding fusion weights, the trajectory optimization objective function is established: Where Γ represents the trajectory equation, N sensor Indicates the number of sensors, N i represents the number of poses contained in the i-th sensor, Γ(t j ) indicates t j The pose obtained by the trajectory equation at the moment, T i (t j ) indicates that the i-th sensor is at t j The estimated pose at the moment, cov i，j Indicates that the i-th sensor is at t j Error distribution of moment estimates; After the optimization is completed, the trajectory equation of the vehicle within the time series range is obtained, and the corresponding vehicle posture can be obtained by inputting different time periods.

Images