A radar-aided visual inertial odometry method and system for visual line features

Abstract

The application discloses a radar-assisted visual inertial odometry method and system of a visual line feature, extracts a line feature for each frame of image, and tracks the line feature in adjacent frames; integral operation is performed by using inertial measurement unit information to remove motion distortion in a radar scanning point cloud; features with corner point, face point and ground point labels are extracted from the de-distorted point cloud; visual line feature coordinates are solved, the solved visual line feature coordinates include a visual ground line feature, a visual vertical visual line feature and remaining line features, the vertical visual line feature is solved by using the corner point, the ground visual line feature is solved by using the ground point, and the remaining line features are calculated according to the face point; finally, the re-projection error of the obtained visual line feature coordinates and inertial measurement unit pre-integral residual error are sent into a tight-coupling visual inertial nonlinear optimization state estimator based on graph optimization, the estimator can more accurately estimate three-dimensional coordinates of the line feature, and positioning precision is greatly improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of SLAM (Simultaneous Localization and Mapping), and mainly relates to a radar-assisted visual line feature visual inertial odometry method and system. BACKGROUND

[0002] In the extremely derived visual inertial odometry problem of monocular visual SLAM, most of the errors are derived from the depth estimation of point features. In view of this problem, the traditional radar-assisted visual inertial odometry does not use the multi-view geometry method to estimate the depth of the features, but uses the radar scanning depth to register the features, which greatly reduces the errors caused by the depth estimation of the point features.

[0003] Line features are generally used in textureless environments where point features are missing, and they bear the function of feature supplement in the system. However, compared with point features, the depth solving of high-degree line features will introduce higher-dimensional errors, and even cause the system to diverge. Therefore, a more efficient and accurate method is urgently needed to avoid the errors caused by line feature triangulation. SUMMARY

[0004] The application is exactly aimed at the inaccurate line feature estimation in the prior art, and proposes a radar-assisted visual line feature visual inertial odometry method and system, which uses radar point cloud information to fit visual line features. First, line features are extracted from each frame of image, and the line features in adjacent frames are tracked. The inertial measurement unit information is used for integral operation to remove the motion distortion in the radar scanning point cloud. The de-distorted point cloud is extracted to have labeled features, and the labels are divided into corner points, face points and ground points. The coordinates of the visual line features are solved, and the solved coordinates of the visual line features include visual ground line features, visual vertical line features and the remaining line features. The corner points are used to solve the vertical visual line features, the ground points are used to solve the ground visual line features, and the remaining line features are calculated according to the face points. Finally, the re-projection error of the obtained visual line feature coordinates and the inertial measurement unit pre-integration residual error are sent to a tight coupling visual inertial nonlinear optimization state estimator based on graph optimization. The estimator obtains the optimal attitude estimation of the motion carrier by minimizing the loss function, can more accurately estimate the three-dimensional coordinates of the line features, and greatly improves the positioning accuracy.

[0005] In order to achieve the above purpose, the technical scheme adopted by the application is as follows: a radar-assisted visual line feature visual inertial odometry method, comprising the following steps:

[0006] S1, extracting and tracking image line features: extracting line features from each frame of image, and tracking line features in adjacent frames;

[0007] S2, remove point cloud motion distortion: use inertial measurement unit information to do integral operation, remove the motion distortion in the point cloud detected by the radar; Specifically: for any one frame of radar point cloud, taking the first point scanned by the radar as the starting point, integrating the high-frequency inertial measurement unit information, calculating the time relationship of all subsequent points except the first point to the first point, and calculating the coordinates of the points using the integral results of the corresponding inertial measurement unit information, and finally integrating all point clouds to obtain a new point cloud without distortion;

[0008] S3, point cloud feature extraction: extracting features with labels from the point cloud after step S2, the labels are at least corner points, face points and ground points;

[0009] S4, solving the coordinates of visual line features: the solved coordinates of visual line features include visual ground line features, visual vertical visual line features and other line features,

[0010] Solving the visual ground line feature: based on the ground points of step S3, calculating the depth of the sampling points using the ground point cloud information, and fitting the ground feature according to the three-dimensional sampling point coordinates to obtain;

[0011] Solving the visual vertical line feature: based on the corner points of step S3, using the method of straight line fitting and line segment clipping to solve the coordinates of the visual vertical line feature;

[0012] Solving the remaining line feature: based on the face points of step S3, sampling the two-dimensional visual line feature into multiple three-dimensional sampling points, calculating the depth of the sampling points using the face point information, and fitting the remaining line feature according to the three-dimensional sampling point coordinates to obtain;

[0013] S5, tight coupling positioning: sending the visual line feature coordinates obtained in step S4 and the inertial measurement unit pre-integration residual into a tight coupling visual inertial nonlinear optimization estimator based on graph optimization, which obtains the optimal attitude estimation of the motion carrier by minimizing the loss function.

[0014] As an improvement of the application, in step S1, LSD is used to extract line features from each frame of image; for each adjacent two frames of image, the association of line features in adjacent frames is established according to the coordinates of the extracted line features, and the line features on adjacent images are tracked.

[0015] As an improvement of the application, step S2 specifically includes the following steps:

[0016] S21: for any one frame of point cloud, assuming that the position and attitude component quaternion of the carrier at time i is The first point l1 in the point cloud is taken as the starting point, and the position and attitude component quaternion at this time is The midpoint integral inertial measurement unit data is obtained:

[0017]

[0018] wherein:

[0019]

[0020] wherein is the velocity of the carrier at time t, and denotes the acceleration and angular velocity information provided by the inertial measurement unit at time t, denotes the angular velocity information provided by the inertial measurement unit at time t+1, and are the biases of the gyroscope and accelerometer at the same time, g w denotes the gravity acceleration in the world coordinate system, and δt corresponds to the time interval from i to i+1;

[0021] S22: calculate the relative translation and rotation matrix from i to i+1 and rotation matrix

[0022]

[0023] S23: remove the motion distortion from i to i+1 in the second point according to the relative rotation and translation:

[0024]

[0025] wherein f1 and f2 respectively denote the three-dimensional coordinates of the point cloud before and after removing the distortion;

[0026] S24: loop steps S21-S23 to remove the motion distortion of each point in the point cloud based on the first point, and finally obtain a new de-distorted point cloud.

[0027] As another improvement of the present application, the step S3 specifically comprises the following steps:

[0028] S31: convert the point cloud into a distance image, and screen out the line bundle close to the ground. If it is found that the distance between the corresponding points of adjacent line bundles does not change much, it is considered as a ground point; otherwise, it is a non-ground point, and step S32 is entered:

[0029] S32: for each point in the non-ground point set, calculate the curvature near it. If the curvature is large, it is considered as a corner point; otherwise, it is considered as a face point;

[0030] S33: in the direction of laser line bundle scanning, if a continuous corner point subset or face point subset with little depth change is found, use the radar point cloud object segmentation method to assign the same label to it, which is considered as the same cluster or class.

[0031] As another improvement of the present application, the curvature of the point in the non-ground point set in step S32 is specifically:

[0032]

[0033] Wherein h n represents the curvature value of the nth point in the point cloud, r n represents the distance value of the nth point in the point cloud to the origin of coordinates.

[0034] As another improvement of the present application, in the solving process of the visual vertical line feature in step S4,

[0035] When there are two intersecting planes in the field of view, which intersect at a straight line, the corner point cloud is projected onto the image plane, and the corner points close to the vertical line feature are screened out; if more than three corner points are scanned in a line bundle, the two leftmost points and the two rightmost points are selected and fitted into two straight lines, and then the included angle of the two straight lines is detected, if the included angle tends to 90 degrees, the intersection point is regarded as an effective point, and the fitting point coordinates of the vertical visual line feature at this time are solved;

[0036] When there is only a single intersecting plane in the field of view, the corner points are projected onto the image plane and the corner point set close to the visual line feature is screened out, the line feature is cropped, and the fitting point coordinates of the vertical visual line feature at this time are solved;

[0037] After obtaining the fitting point coordinates under the two conditions, if the fitting points are all from the same label, a loss function similar to the ground line feature solving is used for optimization to obtain the optimal Plank coordinates.

[0038] As another improvement of the present application, in step S5, the obtained coordinates of each three-dimensional line feature are sent into a tight coupling visual inertial nonlinear optimization estimator based on graph optimization, the inertial measurement unit pre-integration residual and the line feature re-projection error are set as residual terms, and the optimal estimation of the attitude is obtained by minimizing the residual terms;

[0039] The state variables to be estimated in the sliding window are as follows:

[0040] X=[x0,x1…x S ] Τ

[0041]

[0042] Wherein x k is the state of the inertial measurement unit coordinate system at time k, including the translation, rotation of the inertial measurement unit coordinate system to the world coordinate system at this time, the velocity in the world coordinate system and the zero offset.

[0043] Minimizing the following function to solve state variables:

[0044] arg min X {e prior +e IMU +e pro}

[0045] Where e prior represents the marginalized prior information in the sliding window, e IMU represents the pre-integrated residual, e pro represents the line feature re-projection residual; the line feature residual is defined as follows:

[0046]

[0047] Where d(f,l) is the distance from point f to line l, And are the two end points of the observed line segment, l=(l1,l2,l3) Τ .

[0048] In order to achieve the above purpose, the technical scheme adopted by the present application is: a radar-assisted visual line feature visual inertial odometry system comprising a computer program, which realizes the steps of the method according to any one of the above when executed by a processor.

[0049] Compared with the prior art, the present application has the beneficial effects of improving positioning accuracy and improving mapping quality. The present application discloses a radar-assisted visual line feature visual inertial odometry method and system, which is used for visual inertial positioning. The present method directly solves the visual three-dimensional feature by using radar point cloud information, avoiding the error introduced by the triangulation line feature step based on multi-view geometry. Compared with the traditional visual inertial odometry method based on triangulation line feature, the positioning accuracy is improved, and a more accurate line feature map can be established. BRIEF DESCRIPTION OF DRAWINGS

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

[0051] Figure 2 is a vertical line feature solving schematic diagram in the solving process of the visual vertical line feature of the method of the present application, in which the two intersecting planes are visible;

[0052] Figure 3 is a vertical line feature solving schematic diagram in the solving process of the visual vertical line feature of the method of the present application, in which a single intersecting plane is visible;

[0053] Figure 4 is a line feature clipping principle schematic diagram in the solving process of the visual vertical line feature of the method of the present application;

[0054] Figure 5 is a vertical line feature map constructed based on the method of the present application in the test example of the present application;

[0055] Figure 6 is a vertical line feature map constructed based on the method of the present application in the test example of the present application. DETAILED DESCRIPTION

[0056] The present application will be further clarified by the following examples and figures, which should not be construed as limiting the scope of the present application, which is only limited by the appended claims.

[0057] Example 1

[0058] Compared with point features, line features can supplement additional geometric information in non-texture or weak texture environments and have stronger surrounding environment description capability, and thus are widely used in visual inertial odometry. However, the line feature solving method based on triangulation is extremely susceptible to pixel errors, causing inaccurate line feature estimation. The present application discloses a radar-aided visual line feature visual inertial odometry method, as shown in FIG. 1, which specifically comprises the following steps: Figure 1

[0059] Step S1, extracting and tracking image line features: extracting line features for each frame of image and tracking line features in adjacent frames.

[0060] For each frame of picture from the camera, line features are extracted using LSD (Line Segment Detector), and for each adjacent two frames of image, the association of line features in adjacent frames is established according to the coordinates of the extracted line features using LBD (Line Binary Descriptor) algorithm, the association between line features is determined, and line features on adjacent images are tracked.

[0061] Step S2, removing point cloud motion distortion using inertial measurement unit information: using inertial measurement unit information to do integral operation to remove motion distortion in the point cloud detected by radar.

[0062] For any frame of radar point cloud, the first point scanned by radar is taken as the starting point, and high-frequency inertial measurement unit information is integrated. For all points after the first point, the time relationship thereof to the first point is calculated, and the coordinate of the point is calculated using the integral result of the corresponding inertial measurement unit information, and then all point clouds are integrated to obtain a new point cloud without distortion.

[0063] For any frame of point cloud, it is assumed that the position and attitude component quaternion of the carrier at time i is ​At this time, the first point l1 in the point cloud is scanned, and is recorded as the starting point. Similarly, it is assumed that the second point l2 is scanned at the i+1 time, and the position and attitude component quaternion at this time are The midpoint integral inertial measurement unit data can be obtained as follows:

[0064]

[0065] Wherein:

[0066]

[0067] Wherein is the velocity of the carrier at time t, and represents the acceleration and angular velocity information provided by the inertial measurement unit at time t, represents the angular velocity information provided by the inertial measurement unit at time t+1, and is the zero offset of the gyroscope and accelerometer at the same time, and the derivative is set as Gaussian white noise, g w represents the gravity acceleration in the world coordinate system, and δt corresponds to the time interval from i to i+1, which is equal to the sampling period of the inertial measurement unit.

[0068] Then, the relative translation from i to i+1 is calculated and the rotation matrix

[0069]

[0070] According to the relative rotation and translation, the motion distortion of the second point from i to i+1 is removed:

[0071]

[0072] Wherein f1 and f2 represent the three-dimensional coordinates of the point cloud before and after removing the distortion, respectively.

[0073] The above steps are repeated to remove the motion distortion of each point in the point cloud based on the first point, and finally a new de-distorted point cloud is obtained.

[0074] Step S3, point cloud feature extraction: extracting labeled features from the de-distorted point cloud from step S2, which are divided into corner points, face points and ground points.

[0075] S31, ground point extraction: convert the point cloud into a distance image, and construct the distance image of the point cloud in the form of an image. Since the ground is generally low in the field of view, the points of the line bundle that are high are removed, and the line bundle close to the ground is screened. The positions and depths of the remaining points in the distance image are analyzed, and a distance difference threshold is set. If the distance of the points of adjacent line bundles does not change much, they are considered as ground points. The ground point set is denoted as Γ g .

[0076] S32, corner point and face point extraction: for the non-ground points that do not belong to the ground point set Γ g in step S31, the curvature of each point is calculated:

[0077]

[0078] where h n represents the curvature value of the nth point in the point cloud, and r n represents the distance value of the nth point in the point cloud to the origin. In this formula, the distance change of the adjacent ten points on the same line bundle is calculated. If the distance change is found to be too large, it is considered that the curvature near the point is large, and it is considered as a corner point. Otherwise, it is considered as a face point. The corner point and face point sets are denoted as Γ e and Γ p .

[0079] S33, corner point and face point labeling: in the direction of laser line bundle scanning, if a continuous corner point subset or face point subset with little depth change is found, the same label is given to these corner point and face point subsets using the laser point cloud object segmentation method, and these subsets are considered as the same cluster or class.

[0080] Step S4, solving the visual line feature coordinates by point cloud position information: the solved visual line feature coordinates include visual ground line features, visual vertical visual line features, and other linear features.

[0081] a) visual ground line feature solving: sampling the two-dimensional visual line feature into multiple three-dimensional sampling points. The depth of the sampling points is calculated using the ground point cloud information, and finally the ground line feature is fitted according to the three-dimensional sampling point coordinates.

[0082] The line feature is decomposed into two end points and several middle points using the fitting method. First, the ground point set Γ goutliers in the radar point cloud, then project the two end points of the visual line feature and the cleaned radar point cloud onto a unit circle. By nearest neighbor algorithm, three radar points nearest to the two end points can be found. If the three points are close enough, the depth value of the end points can be determined by calculating the distance from the plane determined by the three radar points to the three-dimensional end points. After successfully solving the two end points, the line feature is sampled into several midpoints, and the depth of the midpoints is solved in a similar way to the end points. The set of midpoints is denoted as Γ mid .

[0083] The Plank coordinates of the line feature L are solved by minimizing the following loss function:

[0084]

[0085] where r dot , r proj and r dis represent the dot product residual, the line feature re-projection error and the point-to-line distance residual, respectively; denotes the gravitational acceleration of the coordinate system; denotes the observation of the line feature in the image; denotes the three-dimensional coordinates of the midpoints in the set Γ.

[0086] In this formula, the dot product residual r dot has the physical meaning that the ground line feature is orthogonal to the gravitational acceleration. Let L = (n, v), where n denotes the normal vector of the plane determined by the line feature and the coordinate origin, and v denotes the direction vector of the line feature. The dot product residual has the following definition:

[0087]

[0088] The re-projection residual r proj is defined as the distance from the two end point measurements to the re-projected straight line, which is widely used in line feature based beam adjustment.

[0089] The point-to-line distance residual r dis denotes the distance from the midpoint of the line feature to the line. First, the line feature is converted to the coordinate system with the point as the origin, denoted as L' = (n', v'). The point-to-line distance residual is defined as follows:

[0090]

[0091] where [-f] × is the three-dimensional midpoint coordinates converted into the corresponding skew-symmetric matrix. In the optimization, the line segment coordinates obtained by the two end points are used as the initial value, and the information of the midpoints is used to minimize the loss function. Thus, the Plank coordinates of the ground line feature are solved.

[0092] b) Visual vertical line feature solution: Collect the corner points around the line feature, and use the line fitting and line segment clipping methods to solve the coordinates of the line feature.

[0093] In outdoor environments, two typical examples are considered. For example... Figure 2 As shown, there are two intersecting planes in the field of view, which intersect at a straight line. In this case, the corner point cloud is first projected onto the image plane, and corner points that are close to the vertical line feature are selected. If more than three corner points are scanned in a line bundle, the two leftmost points and the two rightmost points are selected and fitted into two straight lines. Then the angle between the two straight lines is detected. If the angle is close to 90 degrees, the intersection point is considered a valid point. At this time, the intersection point of the two straight lines can be represented by the following formula:

[0094] x=(c2*b1-c1*b2)(a1*b2-a2*b1)

[0095] y=(c1*a2-c2*a1)(a1*b2-a2*b1)

[0096] in:

[0097] a1x + b1y + c1 = 0

[0098] a²x + b²y + c² = 0

[0099] (x,y) represents the intersection of the two-dimensional fitting, and (a,b,c) represents the coefficients of the three-dimensional line. Thus, the coordinates of the fitting points for the vertical visual line feature in this case are obtained.

[0100] The second scenario is when there is only a single intersecting plane in the field of view, such as... Figure 3 As shown. Similar to the first case, the corner points are first projected onto the image plane, and a set of corner points close to visual line features is selected. This information disadvantage can be compensated for by using a straight line from the camera's optical center to the observation point, i.e., line feature clipping. For example... Figure 4 As shown, two corner points on a 3D green line can determine a straight line. Given the camera intrinsic parameter matrix K, the direction of the ray from the optical center to a point on the image can be represented as:

[0101]

[0102] in This is represented by the normalized coordinates of the observed values ​​at that point. Finally, the least squares method is used to fit the intersection points:

[0103] l=(A Τ A) -1 A Τ b

[0104] in

[0105]

[0106] where L = (n, v) represents the line feature in Plücker coordinates determined by the corner. Thus, the fitted point coordinates of the vertical visual line feature in the second case are obtained.

[0107] After obtaining the fitted point coordinates in both cases, if all the fitted points are from the same label, the optimal Plücker coordinates are obtained by optimizing using a similar loss function as that for the ground line feature. Different from the ground line feature, the dot product residual component of the vertical line feature is considered to be consistent with the direction of the gravitational acceleration:

[0108]

[0109] where v represents the direction vector of the line feature.

[0110] c) Rest of the visual line feature solving: The two-dimensional visual line feature is sampled and divided into multiple three-dimensional sampling points. The depth of the sampling point is calculated using the face point information, and finally the rest of the line feature is fitted according to the three-dimensional sampling point coordinates. For the rest of the line feature that is not successfully identified as a ground feature or a vertical feature, the face point point cloud is used to solve it. The solving method is consistent with the ground feature, and the line feature is solved using the fitting method, which decomposes the line feature into two end points and several middle points. After successfully solving the two end points, the line feature is sampled into several middle points, and if all the fitted points are from the same label of the face point point cloud, the feature is optimized using the fitting method.

[0111] Step S5, tight coupling positioning: the visual line feature coordinates obtained in step S4 and the inertial measurement unit pre-integration residual are sent into a tight coupling visual inertial nonlinear optimization estimator based on graph optimization, and the optimization framework is used to minimize the loss function to obtain the optimal attitude estimation of the motion carrier.

[0112] The state variables to be estimated in the sliding window are as follows:

[0113] X = [x0, x1…x S ] Τ

[0114]

[0115] where x k is the state of the inertial measurement unit coordinate system k, including the translation of the inertial measurement unit coordinate system to the world coordinate system, rotation, velocity in the world coordinate system, and zero offset. Unlike the state variable, the line feature is represented using Plücker coordinates as a measurement value.

[0116] The state variable is solved by minimizing the following function:

[0117] arg min X{e prior +e IMU +e pro}

[0118] where e prior is the marginalized prior information in the sliding window, e IMU is the pre-integrated residual, e pro is the line feature re-projection residual. The line feature residual is defined as follows:

[0119]

[0120] where d(f, l) is the distance from point f to line l, and are the two end points of the observed line segment, l = (l1, l2, l3) Τ .

[0121] Test Example

[0122] The open source dataset M2DGR is used to evaluate the algorithm of the method and the triangulation method. The dataset collects 15Hz picture information, 10Hz radar information and 200Hz inertial measurement unit information, uses a mobile trolley as a carrier, and collects visual, inertial measurement unit and radar information indoors and outdoors. All sequences can be run in real time on the robot operating system (ROS) platform, the external parameters between the sensors have been calibrated and the trigger time sequence has been aligned.

[0123] Figure 5 is the vertical line feature map obtained by the method. As can be seen from the figure, the radar information can effectively correct the position of the visual line feature to obtain an accurate three-dimensional line feature position estimate, and more accurately describe the surrounding environment. Accurate three-dimensional line feature estimation can further increase the positioning accuracy of the system.

[0124] Figure 6 is the vertical line feature map obtained by the triangulation method. As can be seen from the figure, due to the large pixel error based on the triangulation method of multi-view geometry, the surrounding environment cannot be accurately described, so a large line feature position estimation deviation is brought in a large scale scene.

[0125] The following Table 1 is the root mean square positioning error (unit: meter) result of using the method and the non-radar assisted point line visual odometry in the M2DGR dataset.

[0126] Table 1

[0127]

[0128]

[0129] As can be seen from Table 1, the average positioning accuracy is improved by about 8.4% by virtue of the strategy of radar-assisted visual line feature in the method of the application. In summary, compared with the triangulation method, the method of the application can more accurately estimate the three-dimensional coordinates of the line feature.

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

Claims

1. A visual inertial odometry method based on radar-assisted visual line features, characterized in that, Includes the following steps: S1. Extract and track image line features: Extract line features for each frame of the image and track line features in adjacent frames; S2. Removing motion distortion from point cloud: Using inertial measurement unit (IMU) information for integration, motion distortion in the radar-detected point cloud is removed. Specifically, for any frame of radar point cloud, starting from the first point scanned by the radar, the high-frequency IMU information is integrated. For all subsequent points except the first point, the time relationship from the first point to the first point is calculated. The coordinates of the point are calculated using the integration result of the corresponding IMU information. Finally, all point clouds are integrated to obtain a new frame of point cloud with distortion removed. S3. Point cloud feature extraction: Extract labeled features from the point cloud after distortion removal in step S2. The labels are at least divided into corner points, face points and ground points. S4. Solving for visual line feature coordinates: The solved visual line feature coordinates include visual ground line features, visual vertical line features, and other line shape features. The solution for the visual ground line features is as follows: based on the ground points in step S3, the depth of the sampling points is calculated using ground point cloud information, and the ground features are fitted according to the coordinates of the three-dimensional sampling points to obtain the depth. The solution for the visual vertical line feature is as follows: Based on the corner point in step S3, the coordinates of the visual vertical line feature are solved using the method of line fitting and line segment clipping. The solution for the remaining linear features is as follows: Based on the face points in step S3, the two-dimensional visual linear features are sampled and divided into multiple three-dimensional sampling points. The depth of the sampling points is calculated using the face point information, and the remaining linear features are obtained by fitting the coordinates of the three-dimensional sampling points. S5. Tightly Coupled Positioning: The visual line feature coordinates obtained in step S4 and the pre-integrated residual of the inertial measurement unit are fed into the tightly coupled visual-inertial nonlinear optimization estimator based on graph optimization. The optimal attitude estimate of the moving vehicle is obtained by minimizing the loss function.

2. The visual inertial odometry method based on radar-assisted visual line features as described in claim 1, characterized in that: In step S1, LSD is used to extract line features for each frame of image; for each two adjacent frames of image, LBD is used to establish the association of line features in adjacent frames based on the coordinates of the extracted line features, and to track line features on adjacent images.

3. The visual inertial odometry method based on radar-assisted visual line features as described in claim 2, characterized in that: Step S2 specifically includes the following steps: S21: For any frame of point cloud, assume that the quaternions of the position and attitude components of the carrier at time i are... The first point in the point cloud was detected. Starting from this point, the position and attitude components quaternions are... The data obtained from the midpoint integral inertial measurement unit is as follows: ； in: ； in Let be the velocity of the carrier at time t. and This indicates the acceleration and angular velocity information provided by the inertial measurement unit at time t. This indicates the angular velocity information provided by the inertial measurement unit at time t+1. and For the zero bias of the gyroscope and accelerometer at the same time, Expressed as gravitational acceleration in the world coordinate system. This corresponds to the time interval from time i to time i+1; S22: Calculation arrive Time relative translation and rotation matrix : ； S23: Based on the relative rotation and translation, remove the motion distortion from time i to i+1 at the second point: ； in and These represent the three-dimensional coordinates of the point cloud before and after distortion removal, respectively. S24: Repeat steps S21-S23, using the first point as a reference, remove the motion distortion of each point in the point cloud, and finally obtain a new frame of distorted point cloud.

4. The visual inertial odometry method based on radar-assisted visual line features as described in claim 1, characterized in that: Step S3 specifically includes the following steps: S31: Convert the point cloud into a distance image, filter out the line bundles close to the ground. If the distance between corresponding points of adjacent line bundles does not change much, they are considered ground points; otherwise, they are non-ground points, and proceed to step S32: S32: For each point in the non-ground point set, calculate the curvature of its vicinity. If the curvature is large, it is considered a corner point; otherwise, it is considered a surface point. S33: If a subset of corner points or surface points with little change in continuous depth is found according to the direction of laser beam scanning, the radar point cloud object segmentation method is used to assign them the same label and regard them as the same cluster or class.

5. The visual inertial odometry method based on radar-assisted visual line features as described in claim 4, characterized in that: The curvature of the non-ground point concentration points in step S32 is specifically as follows: ； in This represents the curvature value of the nth point in the point cloud. This represents the distance from the nth point in the point cloud to the origin.

6. The visual inertial odometry method based on radar-assisted visual line features as described in claim 1, characterized in that: In the process of solving the visual vertical line features in step S4. When there are two intersecting planes in the field of view that intersect at a straight line, the corner point cloud is projected onto the image plane, and the corner points that are closer to the vertical line features are selected. If more than three corner points are scanned in a line bundle, the two leftmost points and the two rightmost points are selected and fitted into two straight lines. Then the angle between the two straight lines is detected. If the angle is close to 90 degrees, the intersection point is considered a valid point, and the coordinates of the fitted point of the vertical visual line feature at this time are calculated. When there is only a single intersecting plane in the field of view, the corner points are projected onto the image plane and the set of corner points close to the visual line features is selected. The line features are then clipped, and the coordinates of the fitting points of the vertical visual line features at this time are obtained. After obtaining the coordinates of the fitted points in both cases, if all the fitted points come from the same label, an optimization is performed using a loss function similar to that used for solving ground line features to obtain the optimal Planck coordinates.

7. The visual inertial odometry method based on radar-assisted visual line features as described in claim 1, characterized in that: In step S5, the obtained feature coordinates of each three-dimensional line are fed into a tightly coupled nonlinear optimization estimator based on graph optimization. The pre-integration residual of the inertial measurement unit and the reprojection error of the line feature are set as residual terms. The optimal estimate of the attitude is obtained by minimizing the residual terms. The following are all the state variables to be estimated for the sliding window: ； in Inertial measurement unit coordinate system The state at any given moment includes the translation and rotation from the inertial measurement unit coordinate system to the world coordinate system, the velocity in the world coordinate system, and the zero bias. Solve for the state variables by minimizing the following loss function: ； in This is represented as marginalized prior information within the sliding window. Indicates the pre-integral residual. This represents the linear feature reprojection residual; the linear feature residual is defined as follows: ； in For point to line distance, and The two endpoints of the observed line segment, .

8. A radar-assisted visual line feature-based visual inertial odometry system, comprising a computer program, characterized in that: When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1-7 above.

Images