Enhanced radar inertial positioning method and system based on visual descriptor

Abstract

The invention discloses an enhanced radar inertial positioning method and system based on a visual descriptor, and the method comprises the steps: carrying out the preprocessing of a collected laser radar point cloud, and aligning each frame of image to a timestamp of a corresponding point cloud frame; carrying out feature extraction on the laser radar point cloud, projecting each feature to a picture plane to complete descriptor construction, carrying out sampling on a matching line and a plane, carrying out feature matching elimination according to a Hamming distance between descriptors of matched features, and completing visual descriptor auxiliary elimination; modeling is carried out on point features according to the descriptor distance, modeling is carried out on a matching line and a matching surface, and finally point-line and point-surface matching adaptive covariance modeling is completed; and finally, executing pre-integration by using the inertial data, performing scanning matching between the point clouds according to the adaptive covariance, and inputting a pre-integration result and a scanning matching result into a pose map to obtain pose estimation and complete positioning. According to the method, mismatching in point cloud features can be accurately eliminated, and the positioning precision is improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of SLAM (Simultaneous Localization and Mapping), and mainly relates to an enhanced radar inertial positioning method and system based on a visual descriptor. BACKGROUND

[0002] When a laser radar scans a repetitive geometric environment or a degenerative environment (such as a tunnel), it is easy to cause mis-matching between point clouds, which brings great challenges to the radar inertial positioning method. The existing radar inertial positioning method usually removes mis-matching from a geometric perspective, which causes the positioning method to fail to exhibit sufficient compatibility in scenes with inconsistent scales. In addition, the existing method lacks adaptive covariance modeling of matching confidence in different environments, and therefore it is difficult to provide sufficient robustness for the positioning system. SUMMARY

[0003] The application is exactly aimed at the defects of the existing radar inertial positioning method in mis-matching removal and adaptive covariance modeling of matching, and proposes an enhanced radar inertial positioning method and system based on a visual descriptor. First, the laser radar point cloud collected in each frame is preprocessed, and each picture is aligned to the timestamp of the corresponding point cloud frame. Feature extraction is performed on the laser radar point cloud, each feature is projected onto the picture plane to complete the construction of the descriptor, sampling is performed on the matching line and plane, feature matching removal is performed according to the Hamming distance between the descriptors of the matching features, and visual descriptor assisted removal is completed. Then, the point features are modeled according to the descriptor distance, the matching line and the matching plane are modeled, and finally the adaptive covariance modeling of point-line and point-plane matching is completed. Finally, the pre-integration between two frames of point clouds is performed using inertial data, the scan matching between the point clouds is performed according to the adaptive covariance, the pre-integration result and the scan matching result are input into the pose graph, the final pose estimation is obtained, and the positioning is completed. The method of the application can accurately remove mis-matching in point cloud features, thereby improving the positioning accuracy.

[0004] In order to achieve the above purpose, the technical scheme adopted by the application is as follows: an enhanced radar inertial positioning method based on a visual descriptor, comprising the following steps:

[0005] S1, point cloud preprocessing and picture timestamp alignment: the point cloud data of each frame is preprocessed to remove motion distortion in the point cloud, a frame of laser radar point cloud data closest to the picture timestamp is determined, inertial data between the picture timestamp and the laser radar point cloud timestamp is extracted, and the relative pose during the period from the picture frame timestamp to the laser radar frame timestamp is estimated according to high-frequency inertial information. Each frame of laser radar point cloud is aligned to the corresponding picture timestamp;

[0006] S2, visual descriptor aided elimination: feature extraction is performed on the point cloud data after step S1 preprocessing, and the point cloud is divided into a corner point set and a face point set to form a matching line and a matching surface; for each feature point, a visual descriptor is constructed using the image information of step S1, after completing point cloud feature matching, sampling is performed in the matching line and the matching surface, and false matching is eliminated according to the Hamming distance between the descriptors; the visual descriptor is constructed as follows: using the point cloud information under the image timestamp obtained in step S1, the point cloud is converted to the camera coordinate system according to the calibration parameters between the camera and the laser radar, and the point cloud is mapped to the two-dimensional image coordinate system according to the camera projection model, the rotation invariant BRIEF descriptor is used to describe the neighborhood of the projected point cloud, and the construction of the descriptor is completed;

[0007] S3, constructing an adaptive covariance model of point-line and point-surface matching: according to the Hamming distance obtained in step S2, an adaptive covariance ellipsoid model is established for the point cloud feature points, a covariance model of the matching line is established according to the structure of any two points on the matching line, a covariance model of the matching surface is established based on the structure of any three points on the matching surface, and finally an adaptive covariance model of point-line and point-surface matching is recursively constructed;

[0008] S4, pose estimation: based on the point-line and point-surface adaptive covariance model constructed in step S3, scan matching is performed on the continuous two frames of laser radar point clouds, inertial measurement data between the two frames is collected and pre-integrated, and the integrated result and the scan matching result are jointly optimized to obtain the final pose estimation, and the positioning is completed.

[0009] As an improvement of the present application, the step S1 removes the motion distortion in the point cloud, which specifically comprises: taking the first scanned point as an anchor point, calculating the rotation amount and translation amount of all points except the anchor point relative to the anchor point within the scanning time interval of the carrier motion, and applying motion compensation to the point cloud coordinates of all points;

[0010] The carrier motion pose information at the i+1 moment is calculated as follows:

[0011]

[0012] Wherein:

[0013]

[0014] Wherein And The translation, velocity and rotation of the carrier relative to the world coordinate system at the i moment are respectively And The translation and rotation of the carrier relative to the world coordinate system at the i+1 moment are respectively, and δt is the time interval between the i moment and the i+1 moment. and are the measured acceleration and angular velocity at the i-th time, is the measured angular velocity at the i+1-th time, and are the bias of the accelerometer and gyroscope at the i-th time, is the bias of the accelerometer at the i+1-th time, g w is the gravity acceleration in the world coordinate system.

[0015] For each frame of laser radar data, time stamp alignment is performed. The frame of laser radar point cloud data closest to the picture time stamp is determined; all angular velocity and acceleration inertial information between the picture time stamp and the laser radar point cloud time stamp is extracted. Based on the inertial information, the relative pose during the picture frame time stamp to the laser radar frame time stamp is iteratively calculated repeatedly using the formula in the above removing point cloud motion distortion, so as to realize accurate alignment of the picture frame and the laser radar frame in time.

[0016] As an improvement of the present application, in step S2, the point set with a smoothness exceeding a threshold value in the point cloud is referred to as a corner point, and otherwise as a surface point; for the corner point, two nearest neighbor points in the point cloud form a matching line, and a point-line matching is constructed; for the surface point, three nearest neighbor points in the point cloud form a matching surface, and a point-surface matching is constructed; the calculation method of the smoothness is specifically:

[0017]

[0018] where S represents p i point nearest neighbor point set, S n is the number of points in the set.

[0019] As another improvement of the present application, in step S2, for each corner point and each surface point, a descriptor of the corner point and the surface point is calculated respectively as a basis for judging whether it is a false match, which is specifically:

[0020] For each corner point, the length of the original line segment is extended to four times the initial length, and the extended line segment is constructed with the center point as the symmetric center; laser radar point clouds satisfying specific geometric constraint conditions are screened from around the extended line segment; the screened point clouds are fitted using a random sample consensus algorithm, and after generating a refined line segment, the closest fitting point to the original corner point is determined as a reference point for subsequent descriptor calculation; in the vertical direction of the refined line segment, five points are sampled above and below the reference point respectively with the reference point as the center, and the sampling interval adopts an exponential decreasing manner; by calculating the Hamming distance between the sampling points and the original corner point descriptor, false matches in the corner-point-line are removed.

[0021] For each face point, the original plane is expanded to a parallelogram with the center point as the reference and two boundary vectors as the expansion plane region, the point cloud located on the expansion plane is screened, the screened point cloud is finely fitted by using principal component analysis (PCA) method to generate a fine plane, and the original three nearest points are projected onto the expansion plane and marked as nearest neighbor points respectively; sampling is performed in the horizontal direction and the vertical direction on the fine plane respectively, three points are sampled in each direction, the sampling direction is aligned with the two edges of the parallelogram, and the center of the plane is taken as the symmetric center; the horizontal direction adopts an exponential increasing sampling interval method. By calculating the Hamming distance between the sampling points and the original face point descriptor, the false matching in the face point-plane is removed.

[0022] As another improvement of the application, the sampling interval of each corner point and face point in the vertical direction in step S2 adopts an exponential decreasing method, specifically:

[0023] y e,n = y e,0 * e -λn

[0024] x e,n = x e,n-1 + y e,n

[0025] wherein y e,n is the sampling interval of the nth sampling point, y e,0 is the initial sampling interval, λ is the attenuation coefficient, and x e,n is the position of the nth sampling point;

[0026] For each face point, the sampling interval in the horizontal direction adopts an exponential increasing sampling interval method, and the specific calculation method is:

[0027] y p,n = y p,0 * r n

[0028]

[0029] wherein y p,n is the sampling interval of the nth sampling point, y p,0 is the initial sampling interval, r is the growth factor, x p,0 is the position of the initial point, and x p,n is the position of the nth sampling point.

[0030] As another improvement of the application, in step S3, the adaptive covariance model of point-line is specifically:

[0031]

[0032] where S e is the Plank coordinate of the matching line, p e is the coordinate of the corner point, r e is the point-line distance residual, is the covariance matrix of the matching line, is the covariance matrix of the corner point.

[0033] The adaptive covariance model of the point-plane Specifically,

[0034]

[0035] where π p = (n, q) is the coordinate of the matching plane, n is the normal vector of the matching plane, q is the distance from the plane to the origin, p p is the coordinate of the plane point, r p is the point-plane distance residual, is the covariance matrix of the matching plane, is the covariance matrix of the plane point.

[0036] In order to achieve the above purpose, the technical scheme adopted by the present application is: an enhanced radar inertial positioning system based on a visual descriptor, comprising a computer program, which, when executed by a processor, implements the steps of any of the above-mentioned methods.

[0037] Compared with the prior art, the present application has the beneficial effects that: the prior art only relies on geometric methods to remove point cloud mismatching, and the present application improves the mismatch removal rate of radar point cloud by combining the descriptor in the picture information; in addition, the prior art usually sets the matching confidence to a single constant value, ignoring the modeling of the matching confidence, and the present application improves the robustness of the positioning algorithm in complex environments by adaptively modeling the matching covariance. BRIEF DESCRIPTION OF DRAWINGS

[0038] Figure 1 is a step flowchart of the method of the present application;

[0039] Figure 2 is a schematic diagram of corner point region sampling in step S2 of the method of the present application;

[0040] Figure 3 is a schematic diagram of plane point region sampling in step S2 of the method of the present application;

[0041] Figure 4 is a schematic diagram of adaptive covariance modeling of the corner point, matching line and point-line residual in step S3 of the method of the present application;

[0042] Figure 5It is a schematic diagram of adaptive covariance modeling of surface points, matching surfaces and point-surface residuals in step S3 of the method of the present invention. DETAILED DESCRIPTION

[0043] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0044] Example 1

[0045] An enhanced radar inertial positioning method based on visual descriptors, such as Figure 1 As shown, the specific steps include:

[0046] Step S1, point cloud preprocessing, image timestamp alignment: Preprocess each frame of the lidar point cloud and align each frame of the lidar point cloud information to the corresponding image timestamp based on the high-frequency inertial information.

[0047] First, motion distortion is removed from each frame of the point cloud. Using the first scanned point as the anchor point, combined with high-frequency inertial measurement data, the rotation and translation of all points in the point cloud frame, excluding the anchor point, relative to the carrier's motion during the anchor point scanning interval are calculated. Through repeated recursion, the rotation and translation of all points, excluding the anchor point, relative to the carrier's motion during the anchor point scanning interval are calculated. This motion compensation is then applied to the point cloud coordinates of these points, effectively removing motion distortion from the point cloud.

[0048] Then, timestamp alignment is performed on each frame of the lidar point cloud. The frame of lidar point cloud data closest to the image timestamp is determined. If the image timestamp is more than 0.1 seconds away from the closest lidar point cloud frame, the alignment is considered a failure. All angular velocity and acceleration inertial information between the image timestamp and the lidar point cloud timestamp is extracted. Based on this inertial information, the formula for removing point cloud motion distortion is used below to repeatedly iteratively calculate the relative pose from the image frame timestamp to the lidar frame timestamp, thereby achieving precise temporal alignment between the image frame and the lidar frame. If the translation in the relative pose exceeds 0.15 meters, the alignment is also considered a failure.

[0049] In this example, a 32-line laser radar with a horizontal resolution of 0.02 degrees is used. Each line scans 1800 points. The first point scanned by the first line is used as the anchor point, and the relative motion of all remaining points relative to the anchor point is calculated. Given the pose information at time i, the inertial information between time i and time i+1 is used to calculate the carrier motion pose information at time i+1 as follows:

[0050]

[0051] in:

[0052]

[0053] where and are the translation, velocity and rotation of the carrier relative to the world coordinate system at the i-th time instant, and are the translation and rotation of the carrier relative to the world coordinate system at the (i+1)-th time instant, and δt is the time interval between the i-th and (i+1)-th time instants. and are the measured acceleration and angular velocity at the i-th time instant, is the measured angular velocity at the (i+1)-th time instant. and are the biases of the accelerometer and gyroscope at the i-th time instant, is the bias of the accelerometer at the (i+1)-th time instant, and g w is the gravitational acceleration in the world coordinate system.

[0054] Step S2, feature extraction, descriptor construction, region sampling and false match elimination: based on the point cloud after preprocessing in step S1, feature extraction is performed, and the point cloud is divided into a corner point set and a face point set. For each feature point, the pixel information in the corresponding image determined in the timestamp alignment in step S1 is used to construct a visual descriptor. After completing the point cloud feature matching, sampling is performed in the matched line segments and face regions, and false matching is eliminated according to the Hamming distance between the descriptors.

[0055] For each frame of radar point cloud, the smoothness of each point in the point cloud is calculated, which is defined as the geometric gradient between the point and its nearest neighbor points. Take p i point in the point cloud as an example, the smoothness calculation method is as follows:

[0056]

[0057] where S represents the set of nearest neighbor points of p i point, and S n is the number of points in the set.

[0058] The set of points in the point cloud whose smoothness exceeds 1.2 is called corner points, and vice versa. For corner points, find two nearest neighbor points in the point cloud to form a matching line and construct point-line matching; for face points, find three nearest neighbor points in the point cloud to form a matching face and construct point-face matching.

[0059] For each point in the point cloud, a corresponding visual descriptor is constructed. First, the point cloud after removing motion distortion is compensated by using the relative pose between the image frame and the lidar frame, to obtain the point cloud information without the image timestamp. Then, the point cloud is converted to the camera coordinate system according to the calibration parameters between the camera and the lidar. At the same time, the point cloud is mapped to the two-dimensional image coordinate system according to the camera projection model. Finally, the Steer BRIEF descriptor is used to describe the neighborhood of the projected point cloud, and the sampling region is 256 pairs of pixels in a 31*31 image region.

[0060] For each corner point, a potential point-line match is determined. First, the descriptor of the corner point is calculated as the basis for judging whether it is a false match. As shown in Figure 2 , the length of the original line segment S e is extended to four times the initial length, with the center point as the symmetry center C e , to construct the extended line segment S e '. On this basis, the lidar point cloud that meets certain geometric constraint conditions is screened from around the extended line segment, and the screening criteria include two conditions: the included angle of the triangle formed by the point and the endpoints of the line segment is not more than 85°, and the distance of the point to the extended line segment is less than 1 centimeter. Then, the random sample consensus algorithm is used to fit the screened point cloud to generate a refined line segment. After fitting is completed, the closest fitting point to the original corner point is further determined as the reference point for subsequent descriptor calculation. Finally, in the vertical direction of the refined line segment, five points are sampled above and below the reference point, respectively, with the reference point as the center, and the sampling interval adopts an exponential decreasing manner.

[0061] y e,n = y e,0 * e -λn

[0062] x e,n = x e,n-1 + y e,n

[0063] where y e,n is the sampling interval of the nth sampling point, y e,0 is the initial sampling interval, which is 2 pixels in this embodiment. The smaller the value, the greater the density of the initial sampling points. λ is the attenuation coefficient, and x e,n is the position of the nth sampling point.

[0064] For each face point, a potential point-face match is determined. First, the descriptor of the face point is calculated as the basis for judging whether it is a false match. As shown in Figure 3As shown, the original plane is expanded to an extended plane region with the center point as the reference, and a parallelogram composed of two boundary vectors as the extended plane region. The point cloud located on the extended plane is screened. The screening criteria include two conditions: each point must be located inside the plane region when projected onto the extended plane, and the perpendicular distance of each point to the extended plane must be less than 1 centimeter. For the screened point cloud, principal component analysis (PCA) method is used for fine plane fitting to generate a fine plane. Subsequently, the original three nearest points (A p , B p , C p ) are projected onto the extended plane and are respectively marked as the nearest neighbor points. Sampling is performed in the horizontal and vertical directions on the fine plane, respectively, with three points sampled in each direction. The sampling direction is aligned with the two edges of the parallelogram, and the plane center is the symmetry center. The sampling interval in the vertical direction is consistent with the point-line matching introduction, and the sampling interval strategy in the horizontal direction is exponentially increasing, which is calculated as follows:

[0065] y p,n =y p,0 *r n

[0066]

[0067] where y p,n is the sampling interval of the nth sampling point, y p,0 is the initial sampling interval, which is 1 pixel in this embodiment. The smaller the value, the greater the density of the initial sampling points. r is the growth factor, x p,0 is the position of the initial point, and x p,n is the position of the nth sampling point.

[0068] For point-line matching, a total of 11 sampled points are projected into the picture plane as potential optimal matching points. To evaluate the effectiveness of the matching, the Hamming distance is used as the evaluation standard, which calculates the number of consistent bits in two descriptor binary numbers. Therefore, the smaller the Hamming distance, the more similar the two binary numbers. The minimum Hamming distance between the corner point and all the sampling points is compared with a preset threshold to ensure that false matches are effectively filtered out. In this embodiment, the threshold is set to 30. According to statistics, among the total of 125 pictures, 552 pairs of point-line false matches were tested, and this threshold eliminated 542 pairs of false matches, with a false match elimination rate of 98.2%.

[0069] For point-line matching, the 7x7 sampling matrix point cloud is projected to the line plane and its corresponding descriptors are computed. Then, the Hamming distance between the descriptors of each point in the sampling matrix and the original line point descriptors are computed. If the Hamming distance between at least one point in the sampling matrix and the line point is small enough, the matching is considered valid; otherwise, it is considered as an invalid matching and is rejected. In this embodiment, the threshold is 25. According to statistics, 918 pairs of point-line false matchings in a total of 452 pictures are tested, and the threshold rejects 903 false matchings, with a false matching rejection rate of 98.4%.

[0070] Step S3, point covariance modeling, line / surface covariance modeling and residual covariance modeling: using the Hamming distance obtained in step S2, the feature points of the point cloud are adaptively modeled by the covariance ellipsoid; then, the covariance model of the matching line is established according to the structure of any two points on the matching line, and the covariance model of the matching surface is established based on the structure of any three points on the matching surface; finally, the adaptive covariance models of point-line and point-surface matching are recursively constructed.

[0071] As shown in Figure 4 , for a corner point, its nearest neighbor points in the previous and next adjacent scanning frames are found, and a straight line is formed therefrom to determine the linear structure direction thereof. Then, two arbitrary basis vectors orthogonal to the linear structure direction are constructed as auxiliary reference coordinate system, and the three direction vectors are defined as The eigenvector V e is defined as follows:

[0072]

[0073] The eigenvalue matrix L e of the corner point is defined as follows:

[0074]

[0075] where z e is the distance of the corner point, h e,n is the minimum value of the sampling point Hamming distance, N e is the number of sampling points, a e and b e are the two eigenvalues along the axes. The greater the ratio of a e and b e , the flatter the ellipse in the covariance modeling of the corner point. The covariance matrix of the corner point is defined as follows:

[0076]

[0077] As shown in Figure 5As shown, for a point, the normal vector of the plane composed of the nearest neighbors is the principal axis, then two arbitrary basis vectors orthogonal to the linear structure direction are constructed as the auxiliary reference coordinate system, the three direction vectors are defined as Eigen vector V p is defined as follows:

[0078]

[0079] Eigen value matrix L of a point p and covariance matrix is defined as follows:

[0080]

[0081] where z p is the distance of the corner point, h p,n is the minimum value of the Hamming distance of the sampling points, N p is the number of sampling points, a p and b p are the two eigenvalues along the axis. The greater the ratio of a p and b p , the more the ellipse in the covariance modeling of the point is flat.

[0082] As shown in Figure 4 , for a matching line constructed by two points (A e and B e ), after covariance modeling of the two points respectively, the covariance modeling of the matching line is constructed

[0083]

[0084] where the function g represents the function of constructing the two points into a matching line, and are the adaptive covariance modeling of the two points respectively.

[0085] As shown in Figure 5 , for a matching surface constructed by three points (A p , B p and C p ), after covariance modeling of the three points respectively, the covariance modeling of the matching surface is constructed

[0086]

[0087] where the function f represents the function of constructing the three points into a matching surface, and are the adaptive covariance modeling of the three points respectively.

[0088] As shown in Figure 4As shown, for the corner point p e , point-line distance residual r e is defined as follows:

[0089] r e =mp e ×d

[0090] Where (m, d) is the Planck coordinate of the line. Residual covariance modeling is defined as follows:

[0091]

[0092] Among them, S e is the Planck coordinate of the matching line, p e are the coordinates of the corner points.

[0093] like Figure 5 As shown, for the surface point p p , point-surface distance residual r p is defined as follows:

[0094] r p =(p p -q)·n

[0095] Where n is the normal vector of the matching surface, and q is the distance from the plane to the origin. is defined as follows:

[0096]

[0097] Among them, π p =(n,q) is the coordinate of the matching surface, p p is the coordinate of the surface point.

[0098] Step S4: Scan Matching, Inertial Pre-Integration, and Pose Graph Optimization: Using the adaptive covariance constructed in step S3, scan matching is performed on two consecutive LiDAR point cloud frames. Simultaneously, inertial measurement data between the two frames is collected and pre-integrated. This integration result, along with the scan matching results, is fed into the pose graph optimization framework to obtain the final pose estimate.

[0099] Set the state variables to the position and attitude of the carrier, and use the Gauss-Newton iteration method to optimize the point-line residual and point-surface residual according to the covariance of the residual to complete the scan matching:

[0100]

[0101] Where δX is the number of iterations in the optimization problem, K and Λ are the sets of point-line matching and point-surface matching, respectively, and J e and J pThe Jacobian matrix of the state quantity corresponding to the residual pair.

[0102] The inertial data between two continuous frames of laser radar point clouds is pre-integrated, and the scan matching result and the pre-integrated result are sent into pose graph optimization to obtain an optimal estimation of the pose.

[0103] The pre-integration component from i time to j time α ij , β ij and γ ij are defined as follows:

[0104]

[0105] where R it is the rotation matrix from t time to i time, and is the measured acceleration and angular velocity at t time, and is the zero offset of the accelerometer and gyroscope at t time, γ it is the pre-integration component from t time to i time, and Ω is represented as:

[0106]

[0107] where x is the projection of a vector to its skew-symmetric matrix.

[0108] By using factor graph optimization, the variables to be estimated are set as the pose information at each time, and the residual edges are set as the pre-integration residual and the scan matching residual, and pose graph optimization is performed, which is defined as:

[0109] min X {r I +r S}

[0110] where X is the pose information of the carrier at each time, r I is the pre-integration residual between two adjacent poses, and r S is the scan matching residual between two adjacent poses.

[0111] Test Example 1

[0112] An open source dataset and measured data are used for algorithm evaluation, which collects 20Hz picture information, 10Hz radar information and 200Hz IMU information, and collects visual, inertial and radar information outdoors. All sequences can be run in real time on the robot operating system (ROS) platform, the external parameters between sensors have been calibrated, and the trigger time sequence has been aligned.

[0113] The proposed algorithm is tested using the M2DGR dataset, wherein the Gate_03, Street_02 and Street_09 sequences are outdoor large-scale scenes, the Room_01 sequence is an indoor small-scale scene, and the Hall_02 sequence is recorded in an environment in a corridor. The ground truth trajectories are provided for each sequence in the dataset, and the estimated trajectories are compared with the ground truth trajectories to evaluate the algorithm. Finally, the root mean square positioning error (in meters) of the proposed method and the basic inertial radar positioning method is given. The results are shown in Table 1 as follows:

[0114] Table 1

[0115] Sequence Inertial radar method The present method Lift Gate_03 0.27 0.20 25.9％ Street_02 3.58 2.24 37.4％ Street_09 2.25 1.97 12.4％ Room_01 0.09 0.07 22.2％ Hall_02 2.85 2.45 15.7％ Average - - 22.7％

[0116] As can be seen from Table 1, the proposed method improves the average positioning accuracy by about 22.7% by virtue of the visual descriptor assisted false match elimination strategy and the adaptive covariance modeling strategy.

[0117] In summary, the proposed method provides an enhanced radar inertial positioning method based on visual descriptors, generates descriptors for point clouds using picture information captured by a camera, and eliminates false matches by Hamming distance between the descriptors. In addition, the matching pairs are adaptively modeled by combining the Hamming distance between the descriptors and the distance factor, thereby improving the robustness of the positioning method in repetitive geometric environments and degraded environments.

[0118] It should be noted that the above content only illustrates the technical idea of the present application and cannot limit the protection scope of the present application. For ordinary skilled persons in the art, several improvements and refinements can be made without departing from the principles of the present application, and these improvements and refinements fall within the protection scope of the claims of the present application.

Claims

1. An enhanced radar inertial positioning method based on visual descriptors is characterized by: The steps include: S1. Point cloud preprocessing and image timestamp alignment: Preprocess each frame of point cloud data to remove motion distortion in the point cloud, determine the frame of lidar point cloud data closest to the image timestamp, extract the inertial data between the image timestamp and the lidar point cloud timestamp, estimate the relative pose from the image frame timestamp to the lidar frame timestamp based on the high-frequency inertial information, and align each frame of lidar point cloud information to the corresponding image timestamp; S2, visual descriptor-assisted elimination: Feature extraction is performed on the point cloud data preprocessed in step S1, and the point cloud is divided into a set of corner points and a set of surface points to form matching lines and matching surfaces; for each feature point, a visual descriptor is constructed using the image information in step S1, and after completing the point cloud feature matching, sampling is performed on the matching lines and matching surfaces, and false matches are eliminated based on the Hamming distance between descriptors; the construction of the visual descriptor is specifically as follows: using the point cloud information under the image timestamp obtained in step S1, according to the calibration parameters between the camera and the lidar, the point cloud is converted to the camera coordinate system, and at the same time, according to the camera projection model, the point cloud is mapped to the two-dimensional image coordinate system, and the rotation-invariant BRIEF descriptor is used to describe the features of the projected point cloud neighborhood to complete the construction of the descriptor; S3. Constructing adaptive covariance models for point-line and point-surface matching: Based on the Hamming distance obtained in step S2, adaptive covariance ellipsoid modeling is performed on the feature points of the point cloud. A covariance model of the matching line is established based on the structure of any two points on the matching line. A covariance model of the matching surface is established based on the structure of any three points on the matching surface. Finally, adaptive covariance models for point-line and point-surface matching are recursively constructed. S4, pose estimation: Based on the point-line and point-surface adaptive covariance models constructed in step S3, scan matching is performed on two consecutive frames of lidar point clouds. The inertial measurement data between the two frames is collected and pre-integrated. The integral result is combined with the scan matching result to optimize the pose graph to obtain the final pose estimation and complete positioning.

2. The enhanced radar inertial positioning method based on visual descriptors according to claim 1, wherein: The step S1 of removing motion distortion in the point cloud specifically comprises: taking the first scanned point as the anchor point, calculating the rotation and translation of all points except the anchor point relative to the carrier during the scanning time interval of the anchor point, and applying motion compensation to the point cloud coordinates of all points; The carrier motion posture information at the i+1th moment is specifically calculated as follows: in: in and are the translation, velocity and rotation of the carrier relative to the world coordinate system at the i-th moment, and are the translation and rotation of the carrier relative to the world coordinate system at the i+1th moment, δt is the time interval between the i-th moment and the i+1-th moment, and are the acceleration and angular velocity measured at the i-th moment, is the angular velocity measured at the i+1th moment, and are the zero bias of the accelerometer and gyroscope at the i-th moment, is the zero bias of the accelerometer at the i+1th moment, g w is the gravitational acceleration in the world coordinate system.

3. The enhanced radar inertial positioning method based on visual descriptors according to claim 1, wherein: In step S2, the point set whose smoothness exceeds the threshold in the point cloud is called a corner point, and the point set whose smoothness exceeds the threshold is called a surface point. For a corner point, the two nearest neighboring points in the point cloud form a matching line to construct a point-line matching. For a surface point, the three nearest neighboring points in the point cloud form a matching surface to construct a point-surface matching. The smoothness is calculated as follows: Where S represents p i The set of neighboring points, S n is the number of points in the set.

4. The enhanced radar inertial positioning method based on visual descriptors according to claim 3, characterized in that: In step S2, for each corner point and each surface point, the descriptors of the corner point and the surface point are calculated respectively as the basis for determining whether it is a mismatch, specifically: For each corner point, the length of the original line segment is extended to four times the initial length. The extended line segment is constructed with the center point as the center of symmetry. The lidar point cloud that meets the specific geometric constraints is selected from the extended line segment. The selected point cloud is fitted using a random sampling consistency algorithm. After generating a refined line segment, the fitting point closest to the original corner point is determined as the reference point for subsequent descriptor calculations. In the vertical direction of the refined line segment, five points are sampled above and below the reference point, with the sampling spacing decreasing exponentially. For each face point, the original plane is expanded, and a parallelogram consisting of two boundary vectors with the center point as the reference is used as the extended plane area. The point cloud located on the extended plane is screened, and the principal component analysis (PCA) method is used to perform refined plane fitting on the screened point cloud to generate a refined plane. The three original nearest points are projected onto the extended plane and marked as nearest neighbor points respectively. Sampling is performed on the refined plane in the horizontal and vertical directions, with three points sampled in each direction. The sampling direction is aligned with the two sides of the parallelogram, with the center of the plane as the center of symmetry. The horizontal direction adopts an exponentially increasing sampling spacing method.

5. The enhanced radar inertial positioning method based on visual descriptors according to claim 4, characterized in that: In step S2, the sampling interval of each corner point and surface point in the vertical direction is exponentially decreased, specifically: and e,n =and e,0 *and -λn x e,n =x e,n-1 +y e,n where y e,n is the sampling interval of the nth sampling point, y e,0 is the initial sampling interval, λ is the attenuation coefficient, x e,n is the position of the nth sampling point; For each surface point, an exponentially increasing sampling spacing method is used in the horizontal direction. The specific calculation method is: y p,n =y p,0 *r n where y p,n is the sampling interval of the nth sampling point, y p,0 is the initial sampling interval, r is the growth factor, x p,0 is the position of the initial point, x p,n is the position of the nth sampling point.

6. The enhanced radar inertial positioning method based on visual descriptors according to claim 1, wherein: In step S3, the point-line adaptive covariance model Specifically: Among them, S e is the Planck coordinate of the matching line, p e is the coordinate of the corner point, r e is the point-line distance residual, is the covariance matrix of the matching lines, is the covariance matrix of the corner points; The point-surface adaptive covariance model Specifically: Among them, π p =(n,q) is the coordinate of the matching surface, n is the normal vector of the matching surface, q is the distance from the plane to the origin, p p is the coordinate of the surface point, r p is the point-to-surface distance residual.

7. An enhanced radar inertial positioning system based on visual descriptors, comprising a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images