Multi-sensor fusion automatic driving pose estimation method

Abstract

The application relates to a multi-sensor fusion automatic driving pose estimation method, which comprises the following steps: acquiring image data in real time and enhancing the image data to obtain enhanced images; extracting visual feature points by using a feature extraction method and tracking the visual feature points; acquiring laser point clouds to construct a local map and associating the visual feature points with the laser point clouds; based on the visual feature points and the depths of the visual feature points, using a visual re-projection and IMU pre-integration method to optimize and obtain an initial pose; based on the image data, using a bag-of-words model to match and calculate a cumulative drift amount; extracting point cloud feature points; matching the point cloud feature points with the local map, calculating a residual error, and according to a curvature, giving different residual error weights to each feature point; based on the initial pose, the residual error and the residual error weights, using a Lie algebra form to represent the pose, thereby calculating a Jacobian matrix and obtaining a real-time pose; using an incremental smoothing algorithm for overall optimization to obtain a final pose. Compared with the prior art, the application has the advantages that various sensor data are well fused to improve pose estimation accuracy and the like.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving, and in particular to a multi-sensor fusion method for autonomous driving pose estimation. Background Technology

[0002] In recent years, with the continuous development of communication, energy, and sensor technologies, autonomous driving has become a focus of national scientific and technological progress and a hot area for industrial upgrading pursued by various enterprises. Among them, pose estimation technology is one of the key points in the research field of autonomous driving. In unfamiliar environments that driverless vehicles constantly face, the first problem they need to solve is self-localization and pose estimation.

[0003] Currently, there are three main mass-production solutions for pose estimation:

[0004] The first method uses a differential signal Global Navigation Satellite System (GNSS) to obtain real-time vehicle absolute pose. This method can achieve centimeter-level positioning accuracy, providing extremely high precision in outdoor environments. However, GNSS performs poorly in environments with poor signal coverage, such as jungles, elevated roads, mountains, tunnels, underground spaces, and indoor environments. It is prone to pose estimation drift and failure, and the equipment is also expensive, which is not conducive to the mass production requirements of autonomous driving.

[0005] The second approach involves fusing high-precision maps with data from various onboard sensors to obtain the vehicle's accurate pose based on the matching results between the perceived data and the map. This method boasts high positioning accuracy and robustness, and has already achieved partial mass production in Level 2 assisted driving. However, the collection and use of high-precision maps involve national laws and regulations, requiring certain qualifications for their use. Furthermore, the production time for high-precision maps is long, and currently, they only cover some major urban roads and highways, thus limiting autonomous driving effectiveness to specific roads. Finally, urban maps are not static, requiring companies to consider hardware and map maintenance costs while developing algorithms.

[0006] The third method uses the vehicle's own sensors to obtain real-time pose estimation results through frame matching and optimization. The biggest advantage of this approach is that it does not rely on information obtained from external sources; it can estimate the vehicle's pose using only data observed by its own sensors. Currently, the mainstream sensors used in this approach include cameras, LiDAR, and inertial measurement units.

[0007] These sensors each have their advantages and limitations. Cameras can calculate their own pose changes from changes in feature points and optical flow, and can build maps using rich color and semantic information. They are inexpensive, have low power consumption, and are easy to deploy. However, cameras are highly susceptible to light variations, have poor robustness at high speeds, and monocular cameras lack scale information; even with binocular or depth cameras, their depth detection capabilities are still insufficient. LiDAR, on the other hand, measures distance by measuring the round-trip time of a laser pulse. Each laser pulse can accurately measure the distance from the sensor to the target, and it has extremely high angular and distance resolution. However, LiDAR point cloud data is large and has a low frequency, requiring high computational resources for processing, thus it cannot output high-frequency pose change results and is easily affected by adverse weather conditions such as rain, snow, and dust storms. Compared to the former two, inertial measurement units (IMUs) are unaffected by weather, have strong anti-interference capabilities, and their sampling frequencies can generally reach 100Hz or even 500Hz, enabling high-frequency pose calculation. However, IMUs have a certain zero bias, and after long-term accumulation, the system drift becomes quite severe, making it unsuitable for long-term positioning work independently.

[0008] Considering the inherent limitations of single-sensor algorithms, subsequent methods based on nonlinear optimization, such as visual inertial odometry and laser inertial odometry, have been proposed. However, while visual inertial odometry addresses the lack of camera scale and significantly increases the frequency of pose output, it exhibits poor robustness at high vehicle speeds and cannot achieve the positioning accuracy required for autonomous driving. Laser inertial odometry, despite its high positioning accuracy, cannot address the problem of z-axis drift in large-scale scenarios.

[0009] Therefore, how to provide a pose estimation algorithm that can fuse data from multiple sensors is a problem that engineers urgently need to solve. Summary of the Invention

[0010] The purpose of this invention is to provide a multi-sensor fusion method for autonomous driving pose estimation that integrates data from multiple sensors to improve the accuracy of pose estimation.

[0011] The objective of this invention can be achieved through the following technical solutions:

[0012] A multi-sensor fusion method for autonomous driving pose estimation includes the following steps:

[0013] Image data is acquired in real time, and image processing algorithms are used to enhance the image data to obtain an enhanced image;

[0014] Based on the enhanced image, visual feature points are extracted and tracked using a feature extraction method;

[0015] Real-time acquisition of laser point clouds to construct a local map, and deep association of the visual feature points with it;

[0016] Based on the visual feature points and their depth, the initial pose is optimized using visual reprojection and IMU pre-integration methods.

[0017] Based on the image data, a bag-of-words model is used for matching to calculate the cumulative drift.

[0018] Based on the laser point cloud, feature points of the point cloud are extracted using curvature and covariance.

[0019] The point cloud feature points are matched with the local map, the residuals are calculated, and different residual weights are assigned to each feature point according to the curvature.

[0020] Based on the initial pose, residual, and residual weights, the pose is represented in Lie algebra form, and the Jacobian matrix is ​​calculated to obtain the real-time pose.

[0021] Based on the real-time pose and cumulative drift, an incremental smoothing algorithm is used to optimize the pose and obtain the final pose.

[0022] Furthermore, the specific steps for extracting visual feature points include:

[0023] Extract the FAST corner points from the enhanced image as prior corner points;

[0024] A rectangular region is constructed based on the prior corner point and several adjacent pixels, and the gradient value is calculated in the rectangular region using a discrete differential operator.

[0025] Construct the covariance matrix of the prior corner points, and calculate the eigenvalues ​​based on the gradient values;

[0026] The prior corner points are sorted based on the aforementioned feature values;

[0027] The enhanced image is divided into regions according to the minimum distance, and only the prior corner point with the largest feature value is retained as the visual feature point in each region.

[0028] Furthermore, the KLT optical flow algorithm is used to track the visual feature points.

[0029] Furthermore, the specific steps for performing depth correlation on the visual feature points include:

[0030] Based on the acquired laser point cloud, a local map is constructed using multi-threaded control.

[0031] Points in the local map that are not useful for depth correlation are filtered out, and the map is divided into several regions according to its angle and position. The point with the smallest depth in each region is selected as the unique point of the region to construct a grid map.

[0032] The grid map and visual feature points are regularized onto a normalized sphere, and the nearest neighbor laser regularized points of the visual feature points on the sphere are searched.

[0033] Based on the nearest neighbor laser regularized points, query the corresponding original points on the local map and construct a plane;

[0034] The depth of the visual feature points is calculated based on the coordinates of each laser point on the plane and the coordinates of the visual feature points on the sphere, thereby completing the depth association between the visual feature points and them.

[0035] Furthermore, the KD-tree algorithm is used to search for the laser regularization points.

[0036] Furthermore, the point cloud feature points include edge feature points and planar feature points.

[0037] Furthermore, the curvature of the laser point is calculated using the LOAM method, and edge feature points are obtained based on the curvature.

[0038] Furthermore, the covariance matrix is ​​calculated using the KD tree algorithm and the curvature of the laser points, and planar feature points are obtained based on the covariance matrix.

[0039] Furthermore, the residual weights include residual weights for edge feature points and residual weights for planar feature points, and their calculation expressions are as follows:

[0040]

[0041]

[0042] in, and These are the residual weights for edge feature points and the residual weights for planar feature points, respectively. and P represents the curvature of the edge feature point and the planar feature point, respectively. i For a certain feature point in the set, F e With F p These are the set of edge feature points and the set of planar feature points, respectively. For P i The curvature of the feature points.

[0043] Furthermore, the Lie algebra is a Lie algebra based on six degrees of freedom.

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

[0045] (1) This invention utilizes an efficient method for extracting visual feature points and establishes a deep association between laser point clouds and these visual feature points to improve optimization efficiency. Furthermore, it designs a method for representing pose using Lie algebras for optimization, ultimately outputting the estimated pose. By fusing data from multiple sensors, the accuracy of pose estimation is significantly improved.

[0046] (2) This invention uses the GFFT feature extraction method with FAST feature extraction as the initial value, thus avoiding the need to calculate convolution for every point in the image. This method significantly reduces the time consumption of the visual feature extraction module without compromising the stability of the feature points. The efficient feature extraction method used in this scheme takes only 10.227 milliseconds to extract 150 feature points on a computer platform, while the traditional GFFT feature point method requires 27.0482 milliseconds, significantly improving the real-time performance of the overall framework. Regarding accuracy, since this method uses points with certain geometric features, such as FAST corner points, as candidate points, it also excludes some erroneous points calculated directly by the GFFT algorithm, making the system's stability slightly better than the GFFT feature extraction method. In actual testing, the visual odometry using feature points extracted by this invention has a root mean square error 1.62 meters smaller than the traditional VINS-Mono visual odometry method in the 744-meter dataset for pose estimation.

[0047] (3) This invention constructs a local map using laser point clouds and assigns depth information to visual feature points, thereby improving the nonlinear optimization problem of visual inertial odometry. Traditional methods require optimizing both pose and the depth of visual feature points. Since there are more than 100 visual feature points in each frame of an image, they constitute the largest proportion of the variables that need to be optimized. Therefore, this method provides depth information for some visual feature points through a local point cloud map, thereby significantly reducing the number of optimization variables, reducing optimization time, and increasing optimization accuracy. In actual testing, the average optimization time for each sliding window was 22.36 milliseconds, while optimizing the depth of all feature points would take 50.64 milliseconds. In addition, the depth information assigned by the laser radar is more accurate than the depth information calculated by the visual triangulation method, thus making the visual feature constraints in the optimization problem more reliable and the stability of nonlinear optimization stronger.

[0048] (4) This invention assigns different weights to the residuals of different feature points based on their curvature. The advantage of this method compared to the traditional LOAM algorithm is that it reduces the influence of prominent feature points, such as edge features with extremely high curvature and planar features with extremely low curvature, on the optimization solution. Otherwise, during multi-frame optimization, these prominent feature points might be continuously optimized, leading to overfitting of the optimization to these points and resulting in inaccurate pose results.

[0049] (5) This invention expresses the pose using Lie algebra and calculates the Jacobian matrix. First, compared to directly using numerical differentiation, this invention provides the Jacobian matrix of the optimization problem, i.e., it provides the direction of descent, which significantly reduces the optimization time and improves the accuracy of the optimization results. In addition, since Lie algebra can more easily calculate the derivative of the residual with respect to the pose through left and right perturbations, and does not have singularity problems compared to using Euler angles to represent rotation, it avoids the problem of extremely poor stability at singular points when using Euler angles for optimization, thereby improving the robustness of the algorithm. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0051] Figure 2 This is a system framework diagram of the method used in this invention;

[0052] Figure 3 This is a schematic diagram of the method for efficiently extracting visual feature points according to the present invention;

[0053] Figure 4 This is a schematic diagram illustrating the depth association of visual feature points based on a local point cloud map according to the present invention.

[0054] Figure 5 This is a schematic diagram illustrating the optimization of the visual inertial odometry factor map of the present invention. Detailed Implementation

[0055] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.

[0056] This embodiment provides a multi-sensor fusion method for autonomous driving pose estimation. The specific implementation steps of this method are as follows: Figure 1 As shown. To implement this method, this embodiment employs the following... Figure 2 The system architecture diagram shown combines visual inertial odometry (VIO) with laser inertial odometry (LAO). At the front end, a highly efficient method for extracting visual features is designed, and depth is assigned to some feature points using a LiDAR to improve optimization efficiency. At the back end of the LAO, a method using Lie algebra to represent pose is designed for optimization. Finally, an incremental smoothing method is used to perform overall fusion of the visual, laser, and inertial odometry data. Next, this embodiment will combine... Figure 1 The method flowchart and Figure 2 The system framework diagram used to describe this method is shown below:

[0057] S1. Acquire image data in real time and enhance the image data using an image processing algorithm to obtain an enhanced image.

[0058] Visual inertial odometry includes a camera and an IMU (Inertial Measurement Unit). First, it acquires image data from the camera, determines whether the timestamp of the image is aligned with other sensors based on the message header, and then enhances the image through the Contrast Limited Adaptive Histogram Equalization (CLAHE) algorithm, which increases the sharpness of edge regions in the image, preparing for subsequent feature point extraction.

[0059] S2. Based on the enhanced image, visual feature points are extracted and tracked using a feature extraction method.

[0060] like Figure 3 The diagram shown illustrates the method for extracting visual feature points. The specific process of this step is as follows:

[0061] 1) First, a large number of FAST (Features from Accelerated Segment Test) corner points are extracted from the image as prior corner points. The extraction method is as follows: traverse each non-edge pixel and select 16 pixels on a circle with a radius of 3 centered at that pixel. If the brightness of 12 consecutive pixels is greater than or less than a threshold, then the center point is considered a point with rapid local gray-level changes and is thus a FAST corner point. The reason for using FAST corner points is that this method is fast and can quickly provide the prior corner point positions, saving the time of GFFT (Generalized Fast Fourier Transform feature extraction) performing convolution calculations for all pixels.

[0062] 2) Each FAST corner point and its 8 neighboring pixels are grouped into a 3×3 region, and the Sobel operator is used to perform convolution calculations on each region. This method can calculate the weighted difference of gray levels in the x and y directions of the neighborhood of each FAST corner point, thereby performing edge detection. Then, the covariance matrix of each point with respect to the x and y directions is constructed, and its matrix eigenvalues ​​are calculated to represent the gradient of the current FAST corner point in the x and y directions. The maximum value of the eigenvalues ​​in the two directions is selected. All prior corner points are sorted from largest to smallest using this maximum value. The larger the eigenvalue, the higher the probability that the corner point is a strong corner point.

[0063] 3) To avoid uneven distribution of feature points extracted from FAST corner points, the image is divided into regions according to a custom minimum distance, and only the point with the largest eigenvalue is retained in each region. Specifically, the FAST corner point container, sorted by eigenvalue, is traversed from front to back. The region containing the feature is calculated based on the pixel's x and y coordinates. If a feature point already exists in the region, the current feature point is discarded, and the existing feature is retained; otherwise, the current point is retained. This ensures that each region retains the strongest feature point.

[0064] The above steps involve extracting visual feature points. A GFFT feature extraction method, using the FAST feature extraction method as an initial value, is designed to avoid calculating convolutions for every point in the image. This method significantly reduces the time consumption of the visual feature extraction module without compromising the stability of the feature points. The efficient feature extraction method used in this scheme takes only 10.227 milliseconds to extract 150 feature points on a computer platform, while the traditional GFFT feature point method requires 27.0482 milliseconds, significantly improving the real-time performance of the overall framework. Regarding accuracy, since this method uses points with certain geometric features, such as FAST corner points, as candidate points, it also excludes some erroneous points calculated directly by the GFFT algorithm, making the system's stability slightly better than the GFFT feature extraction method. In actual testing, the visual odometry using feature points extracted by this invention has a root mean square error 1.62 meters smaller than the traditional VINS-Mono visual odometry method on a 744-meter dataset.

[0065] After acquiring visual feature points, distortion is removed only from these feature points instead of from the entire image, thus reducing the algorithm's time consumption. For feature tracking, the Kanade-Lucas-Tomasi optical flow algorithm is used to track the same features across multiple frames. For detail analysis, a three-layer pyramid KLT optical flow method is used to detect feature movement at different resolutions, resulting in more accurate results.

[0066] S3. Acquire laser point clouds in real time to construct a local map, and perform deep association between the visual feature points and the map.

[0067] This process consists of three steps, as shown below:

[0068] 1) The construction of the local map of the LiDAR is controlled by a separate thread. In order to control the data processing speed and memory usage, this embodiment acquires a frame of LiDAR point cloud data every 0.5 seconds, and then performs voxel downsampling on the single frame of point cloud to reduce memory consumption. Then, the downsampled point cloud is projected into the world coordinate system through the pose of the previous frame and the observation of the IMU.

[0069]

[0070] in, For the IMU pose, P is the extrinsic parameter for laser radar to reach the IMU. W With P L These represent point clouds in the world coordinate system and the lidar coordinate system, respectively. Finally, this point cloud is added to the point cloud map maintained in the previous frame, and then a second voxel downsampling is performed to generate the point cloud map for the current frame. If the current frame is the first frame, it is directly identified as a map.

[0071] 2) Process the point cloud map, filtering out points that are useless for visual feature depth correlation. Useless points mainly include: points with an x-axis value less than 0, i.e., points behind the camera's imaging plane; points with an absolute y / x value greater than 10, i.e., points with an absolute horizontal field of view greater than 84.289°; and points with an absolute z / x value greater than 10, i.e., points with an absolute vertical field of view greater than 84.289°. The purpose of this is to reduce the number of points that need to be processed subsequently and improve correlation efficiency.

[0072] The remaining point cloud local map is divided according to angular position. Specifically, the point cloud in the forward-looking direction is divided with a resolution of 3° horizontal field of view and 3° vertical field of view. Since the horizontal and vertical field of view in the forward-looking direction are approximately 180°, a total of 3600 regions are created. Considering that the camera can more accurately extract nearby points as features, the point with the smallest depth in each region is selected as the unique selected point for that region, thus constructing a grid map.

[0073] 3) Regularize the above point cloud mesh map and visual feature points onto the camera's normalized sphere. Then, use the KD-tree algorithm to search for the three nearest neighbor LiDAR regularized points of the visual feature points on the sphere. Next, find the original points A, B, and C in the LiDAR point cloud map using the index of the nearest neighbors, and determine if the three points are collinear. If they are not collinear, connect the three points to form a plane, and consider the original point P of the visual feature points to lie on this plane. Figure 4 As shown. If the laser points found through the KD tree search exceed the set search threshold, they are considered invalid search points, meaning that the visual feature has not been successfully associated with depth.

[0074] For the i-th visual feature that has been successfully associated, the coordinates of the visual feature on the unit sphere plane are currently known. And the coordinates of the three original laser points A, B, and C obtained through KD-tree search. We need to determine the depth of feature P. First, we can calculate the normal vector of plane ABC.

[0075]

[0076] Given the unit vector from the center O of the circle to point P: Therefore, the coordinates of point P are: Since point P lies on plane ABC, therefore:

[0077] Right now:

[0078]

[0079]

[0080] The depth of the i-th feature point can be calculated using the above formula, thus completing the depth association of the feature points.

[0081] This step constructs a local map using laser point clouds, imbuing visual feature points with depth information, thereby improving the nonlinear optimization problem of visual inertial odometry. Traditional methods require optimizing both pose and the depth of visual feature points. Since each frame contains over 100 visual feature points, representing the largest proportion of the variables to be optimized, this method provides depth information for some visual feature points through a local point cloud map. This significantly reduces the number of optimization variables, decreases optimization time, and increases optimization accuracy. In actual testing, this scheme achieves an average optimization time of 22.36 milliseconds for each sliding window, compared to 50.64 milliseconds for optimizing the depth of all feature points. Furthermore, the depth information provided by laser radar is more accurate than that calculated using visual triangulation methods, making the visual feature constraints in the optimization problem more reliable and enhancing the stability of nonlinear optimization.

[0082] S4. Based on the visual feature points and their depth, the initial pose is obtained by visual reprojection and IMU pre-integration method.

[0083] This step primarily addresses the problems of visual inertial odometry through nonlinear optimization. For example... Figure 4 The diagram shown illustrates the optimization of the visual-inertial odometry factor map. The circles represent the variables to be optimized, including the initial pose x of the camera keyframe. l IMU state variables (velocity v) k Zero bias b a b g ) and the location of the road sign P i (Since the image points are known, only the depth of the landmark points is optimized). This embodiment uses the residuals of traditional visual feature points in the normalized plane, the IMU pre-integration residuals, and the prior residuals as the objective function of the optimization problem, i.e. Figure 5In the square portion, if the square is connected to the circle, it means the residual is represented by the variable indicated by the circle. By constructing keyframes and sliding windows, the number of optimization variables is reduced. However, unlike classic methods, this embodiment has already obtained the accurate depth of a large number of visual feature points through step S4, i.e. Figure 5 The dashed circles in the image. These landmarks only provide constraints for the optimization problem and do not require depth estimation. In reality, a single image frame corresponds to only one camera pose, but may contain around 200 feature points. The method described above can obtain the depth of half of these feature points, thus significantly reducing the number of variables to be optimized, allowing the optimization problem to be solved quickly and robustly.

[0084] S5. Based on the image data, a bag-of-words model is used for matching to calculate the cumulative drift.

[0085] This embodiment uses the Bag-of-Words (BoW) model for image matching. This means that during the process, an image taken at a certain location for the first time is matched with an image taken at the same location a few minutes later. Specifically, when a new keyframe is added, a descriptor is first extracted. Then, the corresponding word is searched in the dictionary based on the descriptor and compared with the previous keyframe to calculate a similarity score. If the timestamps of two keyframes differ by more than 50 seconds and the similarity score exceeds a certain threshold, the old keyframe is considered to have successfully matched the new keyframe. After the association between the new and old frames is completed, the cumulative drift can be calculated, which is the pose transformation between the different world coordinate systems of the matched new and old frames, denoted as .

[0086]

[0087] in, The pose change from the old keyframe to the new keyframe can be calculated using the Perspective-n-points (PnP) method, which incorporates Random Sample Consensus (RANSAC). The estimated poses of the old and new keyframes are known, so the cumulative drift can be calculated. Incorporating the cumulative drift into subsequent incremental smoothing optimization can effectively solve the drift problem in the z-direction of laser inertial odometry.

[0088] S6. Based on the laser point cloud, feature points of the point cloud are extracted using curvature and covariance.

[0089] In laser odometry, the point cloud of the current frame is first acquired using a lidar system. Then, distortion correction is performed on the point cloud. Specifically, based on the timestamp of each scan point and the timestamp of the first scan point in the current frame, all points are projected onto the timestamp of the first scan point in that frame. The expression is as follows:

[0090]

[0091] in, The relative pose change of the lidar from the start timestamp of the scan to the timestamp of the current point i is based on... The value is obtained by integrating the IMU measurement; p begin With p i These are the positions of the point in the frame's first timestamp and its own timestamp coordinate system, respectively.

[0092] This step involves extracting point cloud feature points, including edge and planar feature points, using curvature and covariance. First, the classic LOAM (Lidar Odometry and Mapping) method is employed. For each scan line, starting from the fifth point and traversing to the fifth-to-last point, the curvature at the current position is calculated based on the coordinates of the five points before and after each point. The calculation method is as follows:

[0093]

[0094] Among them, C j,i This represents the curvature of the i-th point on the k-th scan line. Let represent the Euclidean distance from the i-th point to the origin of the lidar coordinate system. In fact, the curvature C... j,i It represents the smoothness of a point in its neighborhood. C k,i The larger C is, the sharper the point is within its neighborhood; conversely, the smaller C is. k,i The smaller the value, the flatter the point is within its neighborhood. When the curvature exceeds a certain threshold, the point can be identified as an edge feature point.

[0095] The extraction of planar feature points involves first identifying candidate points based on the curvature calculations described above. Then, using the KD (K-Dimensional) tree algorithm, a planar candidate point and its five nearest points projected onto the local point cloud map are used to calculate the covariance matrix of the planar point relative to its five nearest points in the x, y, and z coordinates.

[0096]

[0097] in,

[0098] Eigenvalue decomposition of the covariance matrix yields three eigenvalues, λ1, λ2, and λ3, arranged from largest to smallest. The eigenvalues ​​of the covariance matrix represent the distribution of points along the corresponding eigenvector directions. Larger eigenvalues ​​indicate a relatively diffuse distribution of points along that direction, while smaller eigenvalues ​​indicate a relatively dense distribution. For a planar feature point, the calculated eigenvalues ​​should satisfy the relationship λ1 ≈ λ2 >> λ3, ensuring that the distribution of points along two orthogonal directions is relatively diffuse, while the distribution along one direction is relatively concentrated. Using this principle, a threshold can be set to filter out strong planar feature points.

[0099] S7. Match the point cloud feature points with the local map, calculate the residuals, and assign different residual weights to each feature point according to the curvature.

[0100] Based on the initial pose obtained from the visual inertial odometry, the feature points of the current frame are projected onto the world coordinate system and matched with the local point cloud map using distance. After matching, the objective function, i.e., the residual, is calculated, and different residual weights are assigned to each feature point according to curvature. This embodiment first borrows the idea of ​​the LOAM algorithm, using the distance from the edge feature point in the current frame to the nearest neighbor target line in the edge feature map, and the distance from the planar feature point in the current frame to the nearest neighbor plane in the planar feature map, as the basic values ​​of the residuals. The basic residual values ​​of the edge features are as follows:

[0101]

[0102] Where, d e,k This represents the basic value of the residual distance to the edge features. For a certain edge feature in the current frame, and This represents the two nearest neighbor points on the edge map, and the line formed by them is the target line mentioned above. The residual base value of the planar feature is calculated using the plane formed by the five nearest neighbor planar points, as shown below:

[0103]

[0104] Where p1 to p5 represent the coordinates of the five nearest neighbors. Let be the normal vector of the plane formed by the five plane points. Equation (10) is an overdetermined equation, which can be calculated using the SVD (Singular Value Decomposition) method. Then substitute it into equation (11), where d p,k The basic value of the planar feature distance residual. For a certain planar feature in the current frame, The coordinates of the centroids of the five nearest neighbors are given.

[0105] After calculating the basic residual based on the distance, this embodiment assigns a weight to each feature point based on the curvature calculated in step 8, as shown below:

[0106]

[0107]

[0108] in, and Let represent the curvature of the edge feature and the planar feature, respectively. When an edge feature has a large curvature, meaning its edge nature is very obvious, its optimization weight is reduced. Similarly, when a planar feature has a small curvature, meaning its planar nature is very strong, its optimization weight is also reduced. This ensures that the optimization problem does not always revolve around the most prominent feature while ignoring the constraints imposed by other features. Finally, the residual of merging the two types of feature points into a single nonlinear optimization problem is as follows:

[0109]

[0110] This step assigns different weights to the residuals of different feature points based on their curvature. The advantage of this method compared to the traditional LOAM algorithm is that it reduces the influence of prominent feature points, such as edge features with extremely high curvature and planar features with extremely low curvature, on the optimization solution. Otherwise, during multi-frame optimization, these prominent feature points might be continuously optimized, leading to overfitting of the overall optimization and resulting in inaccurate pose results.

[0111] S8. Based on the initial pose, residual, and residual weight, the pose is represented in Lie algebra form to calculate the Jacobian matrix and obtain the real-time pose.

[0112] In the pose optimization problem, this embodiment designs an optimization method based on the six-degree-of-freedom Lie algebra ξ to represent the pose, where the Jacobian matrix J is the result of differentiating the distance residual with respect to the pose, i.e. According to the chain rule, it can be derived that:

[0113]

[0114] in, Let the distance residual be the derivative of the distance residual with respect to the point in three-dimensional space at the foot of the perpendicular. Let be the derivative of the pose of a point in three-dimensional space with respect to the Lie algebraic form. For The specific calculation process is as follows:

[0115]

[0116] Among them, X a With Xb For two points on an edge or plane, Let represent the vector from the point to the foot of the perpendicular. On the other hand, for Based on the theory of visual SLAM (Simultaneous Localization and Mapping), the calculation results can be derived as follows:

[0117]

[0118] Among them, X i ^ denotes the antisymmetric matrix at point i. Combining equations (15), (16), and (17), we obtain:

[0119]

[0120] After completing the optimization and outputting the pose, this embodiment also designed a laser loop closure algorithm based on the ICP (Iterative Closest Point) algorithm to detect the loop closure factor in the case of no camera co-view and input it into the factor map optimization.

[0121] This step uses Lie algebra to express the pose and calculates the Jacobian matrix. First, compared to directly using numerical differentiation, this invention provides the Jacobian matrix for the optimization problem, i.e., it provides the direction of descent, significantly reducing optimization time and improving the accuracy of the results. Furthermore, since Lie algebras can more easily calculate the derivative of the residual with respect to the pose through left and right perturbations, and unlike methods using Euler angles to represent rotations, it avoids singularity issues, thus preventing the extremely poor stability at singular points encountered when using Euler angles for optimization, thereby improving the algorithm's robustness.

[0122] S9. Based on the real-time pose and cumulative drift, an incremental smoothing algorithm is used to optimize the pose and obtain the final pose.

[0123] The pose is optimized to obtain the final pose.

[0124] After obtaining the visual inertial factor, laser odometry factor, IMU pre-integration factor, visual loop closure factor, and laser loop closure factor, a Bayesian tree-based incremental smoothing algorithm is used for pose optimization. In practice, this embodiment uses the GTSAM library for implementation, and finally outputs an accurate and real-time pose.

[0125] By combining visual, laser, and IMU pre-integrating odometry with visual loop closure and laser loop closure factors in incremental smoothing optimization, this approach has the advantage of still being able to output a relatively accurate odometry result in a short time, even if some sensors fail. In pose estimation tests on high-speed national highways at speeds exceeding 60 km / h, the average translational accuracy error of this invention is only 0.543 meters, far exceeding that of traditional visual inertial odometry and laser inertial odometry. Especially in the height z-direction, the accuracy error of this invention is only 0.308 meters, while other laser inertial odometry results in errors exceeding 10 meters. This demonstrates that this invention can robustly output high-precision pose estimation results while achieving real-time performance.

[0126] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A multi-sensor fusion method for autonomous driving pose estimation, characterized in that, Includes the following steps: Image data is acquired in real time, and image processing algorithms are used to enhance the image data to obtain an enhanced image; Based on the enhanced image, visual feature points are extracted and tracked using a feature extraction method; Real-time acquisition of laser point clouds to construct a local map, and deep association of the visual feature points with it; Based on the visual feature points and their depth, the initial pose is optimized using visual reprojection and IMU pre-integration methods. Based on the image data, a bag-of-words model is used for matching to calculate the cumulative drift. Based on the laser point cloud, feature points of the point cloud are extracted using curvature and covariance. The point cloud feature points are matched with the local map, residuals are calculated, and different residual weights are assigned to each feature point according to curvature. These residual weights include residual weights for edge feature points and residual weights for planar feature points, calculated using the following expressions: in, and These are the residual weights for edge feature points and the residual weights for planar feature points, respectively. and These represent the curvatures of edge feature points and planar feature points, respectively. P i For a certain feature point in the set, F e and F p These are the set of edge feature points and the set of planar feature points, respectively. for P i The curvature of the feature points; Based on the initial pose, residual, and residual weights, the pose is represented in Lie algebra form, and the Jacobian matrix is ​​calculated to obtain the real-time pose. Based on the real-time pose and cumulative drift, an incremental smoothing algorithm is used to optimize the pose and obtain the final pose.

2. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 1, characterized in that, The specific steps for extracting visual feature points include: Extract the FAST corner points from the enhanced image as prior corner points; A rectangular region is constructed based on the prior corner point and several adjacent pixels, and the gradient value is calculated in the rectangular region using a discrete differential operator. Construct the covariance matrix of the prior corner points, and calculate the eigenvalues ​​based on the gradient values; The prior corner points are sorted based on the aforementioned feature values; The enhanced image is divided into regions according to the minimum distance, and only the prior corner point with the largest feature value is retained as the visual feature point in each region.

3. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 1, characterized in that, The KLT optical flow algorithm is used to track the visual feature points.

4. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 1, characterized in that, The specific steps for establishing a depth association between the visual feature points and them include: Based on the acquired laser point cloud, a local map is constructed using multi-threaded control. Points in the local map that are not useful for depth correlation are filtered out, and the map is divided into several regions according to its angle and position. The point with the smallest depth in each region is selected as the unique point of the region to construct a grid map. The grid map and visual feature points are regularized onto a normalized sphere, and the nearest neighbor laser regularized points of the visual feature points on the sphere are searched. Based on the nearest neighbor laser regularized points, query the corresponding laser points on the local map and form a plane; The depth of the visual feature points is calculated based on the coordinates of each laser point on the plane and the coordinates of the visual feature points on the sphere, thereby completing the depth association between the visual feature points and them.

5. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 4, characterized in that, The KD-tree algorithm is used to search for the laser regularity points.

6. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 1, characterized in that, The point cloud feature points include edge feature points and planar feature points.

7. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 6, characterized in that, The curvature of the laser point is calculated using the LOAM method, and edge feature points are obtained based on the curvature.

8. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 6, characterized in that, The covariance matrix is ​​calculated using the KD tree algorithm and the curvature of the laser points, and planar feature points are obtained based on the covariance matrix.

9. The autonomous driving pose estimation method based on multi-sensor fusion according to claim 1, characterized in that, The Lie algebra is a six-degree-of-freedom Lie algebra.

Images