Visual inertia odometer method and system based on radar-aided visual line features

Abstract

The invention discloses a visual inertial odometer method and system based on radar-aided visual line features, and the method comprises the steps: extracting the line features of each frame of image, and tracking the line features in adjacent frames; performing integral operation by using inertial measurement unit information, and removing motion distortion in radar scanning point cloud; extracting features with angular point, surface point and ground point labels from the distorted point cloud; the visual line feature coordinates are solved, the solved visual line feature coordinates comprise visual ground line features, visual vertical visual line features and other linear features, the angular points are used for solving the vertical visual line features, the ground points are used for solving ground visual line features, and the other linear features are obtained through calculation according to the surface points; and finally, the obtained re-projection error of the visual line feature coordinate and the pre-integration residual error of the inertial measurement unit are sent into a tight coupling visual inertia nonlinear optimization state estimator based on graph optimization, the estimator can more accurately estimate the three-dimensional coordinate of the line feature, and the positioning precision is greatly improved.

Description

Technical Field

[0001] The invention belongs to the technical field of SLAM (Simultaneous Localization and Mapping), and mainly relates to a visual inertial odometer method and system of radar-assisted visual line features. Background Art

[0002] In monocular vision SLAM and its derivative visual inertial odometry, most of the errors come from the depth estimation of point features. To address this problem, traditional radar-assisted visual inertial odometry does not use multi-view geometry methods to estimate the depth of features, but instead uses radar scanning depth to align features, greatly reducing the errors caused by point feature depth estimation.

[0003] Line features are generally used in textureless environments where point features are missing, and they serve as feature supplements in the system. However, compared to point features, the high-degree-of-freedom deep solution of 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 of the invention

[0004] Aiming at the problem of inaccurate line feature estimation in the prior art, the present invention proposes a radar-assisted visual line feature visual inertial odometer method and system, which uses radar point cloud information to fit visual line features. First, line features are extracted for each frame of image, and line features in adjacent frames are tracked; inertial measurement unit information is used for integration operation to remove motion distortion in radar scanning point cloud; features with labels are extracted from the dedistorted point cloud, and the labels are divided into corner points, surface points and ground points; visual line feature coordinates are solved, and the solved visual line feature coordinates include visual ground line features, visual vertical visual line features and other linear features. Corner points are used to solve vertical visual line features, ground points are used to solve ground visual line features, and other linear features are calculated based on surface points; finally, the obtained reprojection error of visual line feature coordinates and the pre-integration residual of the inertial measurement unit are sent to a tightly coupled visual inertial nonlinear optimization state estimator based on graph optimization, which can more accurately estimate the three-dimensional coordinates of line features by minimizing the loss function to obtain the optimal posture estimation of the motion carrier, and greatly improve the positioning accuracy.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is: a radar-assisted visual line feature visual inertial odometer method, comprising the following steps:

[0006] S1. Extract and track image line features: extract line features for each frame of the image and track line features in adjacent frames;

[0007] S2. Remove motion distortion of point cloud: Use the inertial measurement unit information to perform integration operations to remove motion distortion in the point cloud detected by the radar. Specifically, for any frame of radar point cloud, take the first point scanned by the radar as the starting point, integrate the high-frequency inertial measurement unit information, calculate the time relationship between all subsequent points except the first point and the first point, use the corresponding inertial measurement unit information integration result to calculate the coordinates of the point, and finally integrate all point clouds to obtain a frame of new point cloud with distortion removed.

[0008] S3, point cloud feature extraction: extracting features with labels from the point cloud dedistorted in step S2, wherein the labels are at least divided into corner points, surface points and ground points;

[0009] S4, visual line feature coordinates solution: the visual line feature coordinates solved include visual ground line features, visual vertical visual line features and other linear features,

[0010] The visual ground line feature is solved by calculating the depth of the sampling point based on the ground point in step S3 using the ground point cloud information, and fitting the ground feature according to the three-dimensional sampling point coordinates;

[0011] Solving the visual vertical line feature: based on the corner point in step S3, the coordinates of the visual vertical line feature are solved using a straight line fitting and line segment clipping method;

[0012] Solving the remaining linear features: based on the surface points in step S3, sampling the two-dimensional visual linear features, dividing them into multiple three-dimensional sampling points, calculating the depth of the sampling points using the surface point information, and fitting the remaining linear features according to the coordinates of the three-dimensional sampling points;

[0013] S5, tightly coupled positioning: The visual line feature coordinates obtained in step S4 and the pre-integrated residual of the inertial measurement unit are sent to a tightly coupled visual-inertial nonlinear optimization estimator based on graph optimization, which obtains the optimal posture estimation of the motion carrier by minimizing the loss function.

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

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

[0016] S21: For any frame of point cloud, assuming that the position and attitude component quaternion of the carrier at time i is The first point l1 scanned into the point cloud is the starting point, and the position and attitude component quaternion at this time By integrating the inertial measurement unit data at the midpoint, we get:

[0017]

[0018] in:

[0019]

[0020] in 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 bias of the gyroscope and accelerometer at the same time, g w It is expressed as the gravitational acceleration in the world coordinate system, and δt corresponds to the time interval from time i to time i+1;

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

[0022]

[0023] S23: According to the relative rotation and translation, the motion distortion from time i to time i+1 in the second point is removed:

[0024]

[0025] Where f1 and f2 represent the three-dimensional coordinates of the point cloud before and after distortion removal, respectively;

[0026] S24: looping steps S21-S23, taking the first point as a reference, removing the motion distortion of each point in the point cloud, and finally obtaining a new frame of dedistorted point cloud.

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

[0028] S31: Convert the point cloud into a distance image, filter out the line bundles close to the ground, and if it is found that the distance between the corresponding points of adjacent line bundles does not change much, it is regarded as a ground point; otherwise, it is a non-ground point, and enter step S32:

[0029] S32: For each point in the non-ground point set, calculate the curvature of its vicinity. If the curvature is large, it is regarded as a corner point; otherwise, it is regarded as a surface point;

[0030] S33: According to the direction of laser beam scanning, if a subset of corner points or a subset of surface points with little continuous depth change is found, the radar point cloud object segmentation method is used to assign the same label to them and regard them as the same cluster or class.

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

[0032]

[0033] where h n Represents the curvature value of the nth point in the point cloud, r n Indicates the distance value from the nth point in the point cloud to the coordinate origin.

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

[0035] When there are two intersecting planes in the field of view, and they intersect in a straight line, the corner point cloud is projected onto the image plane, and the corner points that are closer to the vertical line feature are screened out; if more than three corner points are scanned in a line beam, the two leftmost points and the two rightmost points are selected and fitted into two straight lines, and then the angle between the two straight lines is detected. If the angle approaches 90 degrees, the intersection point is regarded as a valid point, and the coordinates of the fitting point of the vertical visual line feature at this time are calculated;

[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 set of corner points close to the visual line features is screened out to perform line feature clipping and obtain the fitting point coordinates of the vertical visual line features at this time;

[0037] After obtaining the coordinates of the fitting points in both cases, if all the fitting points come from the same label, a loss function similar to that used in solving the ground line features is used for optimization to obtain the optimal Planck coordinates.

[0038] As another improvement of the present invention, in step S5, the obtained coordinates of each three-dimensional line feature are sent to a tightly coupled visual-inertial nonlinear optimization estimator based on graph optimization, the pre-integration residual of the inertial measurement unit and the line feature reprojection error are set as residual terms, and the optimal estimation of the posture is obtained by minimizing the residual term;

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

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

[0041]

[0042] where x k is the state of the inertial measurement unit coordinate system at time k, including the translation, rotation, velocity and zero bias of the inertial measurement unit coordinate system to the world coordinate system at that time;

[0043] Minimize the following function to solve the state quantity:

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

[0045] where e prior Represented as the marginalized prior information in the sliding window, e IMU represents the pre-integrated residual, e pro Represents the line feature reprojection 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 endpoints of the observation value segment, l=(l1,l2,l3) Τ .

[0048] In order to achieve the above-mentioned purpose, the present invention also adopts a technical solution: a radar-assisted visual line feature visual inertial odometer system, including a computer program, which implements the steps of any of the above-mentioned methods when executed by a processor.

[0049] Compared with the prior art, the beneficial effects of the present invention are reflected in the improvement of positioning accuracy and the improvement of mapping quality. The present invention discloses a radar-assisted visual line feature visual inertial odometer method and system for visual inertial positioning. The method directly solves the visual three-dimensional features 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 odometer method based on triangulated line features, the positioning accuracy is improved, and a more accurate line feature map can be established. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0051] Figure 2 It is a schematic diagram of solving vertical line features visible in two intersecting planes in the process of solving visual vertical line features by the method of the present invention;

[0052] Figure 3 It is a schematic diagram of solving the vertical line features visible on a single intersecting plane in the process of solving the visual vertical line features by the method of the present invention;

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

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

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

[0056] The present invention will be further explained below in conjunction with 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.

[0057] Example 1

[0058] Compared with point features, line features can supplement additional geometric information in textureless or weakly textured environments and have stronger ability to describe the surrounding environment. Therefore, they are widely used in visual inertial odometry. However, the line feature solution method based on triangulation is easily affected by pixel errors, resulting in inaccurate line feature estimation. The present invention discloses a visual inertial odometry method using radar-assisted visual line features, such as Figure 1 As shown, the specific steps include:

[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 the image from the camera, LSD (Line Segment Detector) is used to extract line features. For each two adjacent frames of images, the LBD (Line Binary Descriptor) algorithm is used to establish the association between the line features in adjacent frames according to the coordinates of the extracted line features, determine the association between the line features, and track the line features on adjacent images.

[0061] Step S2, using inertial measurement unit information to remove point cloud motion distortion: using inertial measurement unit information to perform integration operations to remove motion distortion in the point cloud detected by the radar.

[0062] For any frame of radar point cloud, the first point scanned by the radar is used as the starting point to integrate the high-frequency inertial measurement unit information. For all subsequent points except the first point, the time relationship to the first point is calculated, and the coordinates of the point are inferred using the corresponding inertial measurement unit information integration results. Then, all point clouds are integrated to obtain a new frame of dedistorted point cloud.

[0063] For any frame of point cloud, assuming 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 recorded as the starting point. Similarly, assuming that the second point l2 is scanned at time i+1, the position and attitude component quaternion at this time is It can be obtained by midpoint integration of the inertial measurement unit data:

[0064]

[0065] in:

[0066]

[0067] in 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 bias of the gyroscope and accelerometer at the same time, and their derivatives are set to Gaussian white noise, g w It is expressed as the gravitational acceleration in the world coordinate system. δt corresponds to the time interval from time i to time i+1, which is equivalent to the sampling period of the inertial measurement unit.

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

[0069]

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

[0071]

[0072] Where f1 and f2 represent the 3D coordinates of the point cloud before and after distortion removal, respectively.

[0073] The above steps are repeated, and the motion distortion of each point in the point cloud is removed based on the first point, and finally a new frame of dedistorted point cloud is obtained.

[0074] Step S3, point cloud feature extraction: extract labeled features from the dedistorted point cloud from step S2, which are divided into corner points, surface points and ground points.

[0075] S31, ground point extraction: Convert the point cloud into a distance image, and construct a point cloud distance image in the form of an image. Since the ground is generally lower in the field of view, remove the points with higher beams and filter out the beams close to the ground. Analyze the position and depth of the remaining points in the distance image, set the distance difference threshold, and if the distance between points in adjacent beams does not change much, they are regarded as ground points. The ground point set is denoted as Γ g .

[0076] S32, corner point and surface point extraction: for the ground point set Γ that does not belong to step S31 g For non-ground points, calculate the curvature of each point:

[0077]

[0078] where h n Represents the curvature value of the nth point in the point cloud, r n Indicates the distance value from the nth point in the point cloud to the origin of the coordinate system. In this formula, by calculating the distance change of ten adjacent points on the same line bundle, if the distance change is too large, it is considered that the curvature near the point is large and it is regarded as a corner point; otherwise, it is regarded as a surface point. The set of corner points and surface points are respectively denoted as Γ e and Γ p .

[0079] S33. Labeling corner points and surface points: According to the direction of laser beam scanning, if a subset of corner points or a subset of surface points with little continuous depth change is found, the same label is assigned to these subsets of corner points and surface points using the laser point cloud object segmentation method, and these subsets are regarded as the same cluster or class.

[0080] Step S4, solving the visual line feature coordinates through the 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 solution: Sample the 2D visual line features and divide them into multiple 3D sampling points. Use the ground point cloud information to calculate the depth of the sampling points, and finally fit the ground line features according to the 3D sampling point coordinates.

[0082] The fitting method is used to solve the line feature and decompose the line feature into two endpoints and several midpoints. First, the ground point set Γ gThe outliers in the image are removed, and then the two endpoints of the visual line feature and the radar point cloud after removal are projected onto the unit circle. Through the nearest neighbor algorithm, the three radar points closest to the two endpoints can be found. If the three points are close, the depth value of the endpoint can be calculated by calculating the distance from the plane determined by the three radar points to the three-dimensional endpoint. After successfully solving the two endpoints, the line feature is sampled into several midpoints. Similar to the endpoints, the depth of several midpoints is solved. The set of midpoints is denoted as Γ mid .

[0083] Minimize the following loss function to obtain the Planck coordinates L of the line feature:

[0084]

[0085] where r dot 、r proj and r dis They represent dot product residual, line feature reprojection error, and point-to-line distance residual respectively; represents the gravitational acceleration of the coordinate system; is the observed value of the line feature on the image; Represents the three-dimensional midpoint coordinates in a midpoint set.

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

[0087]

[0088] Reprojection residual r proj Defined as the distance between two endpoint measurements and the reprojected line, this method is widely used in beam adjustment based on line features.

[0089] The residual error of the distance from point to line is r dis Refers to the distance from the point to the line feature. First, transform the line feature into a coordinate system with the point as the origin, denoted as L′=(n′,v′). The residual distance from the point to the line is defined as follows:

[0090]

[0091] Where [-f] × The three-dimensional midpoint coordinates are converted into the corresponding skew-symmetric matrix. In the optimization, the line segment coordinates obtained from the two endpoints are set as the initial values, and the loss function is minimized using the midpoint information. At this point, the Planck coordinate solution of the ground line feature is obtained.

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

[0093] In outdoor environments, two typical examples are considered. 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 the corner points that are closer to the vertical line features are screened out. If more than three corner points are scanned in a line beam, 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 approaches 90 degrees, the intersection is regarded as a valid point. At this time, the intersection of the two straight lines can be expressed 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] a2x+b2y+c2=0

[0099] (x, y) is the intersection point of the two-dimensional fitting, and (a, b, c) represents the coefficients of the three-dimensional straight line. So far, the coordinates of the fitting points of the vertical visual line feature in this case have been obtained.

[0100] The second case is when there is only a single intersecting plane in the field of view, such as Figure 3 As shown in the first case, the corner points are first projected onto the image plane and the set of corner points close to the visual line features is screened out. Using a straight line from the camera optical center to the observation point can make up for this information disadvantage, that is, line feature clipping. Figure 4 As shown, two corner points on the three-dimensional 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 certain point on the image can be expressed as:

[0101]

[0102] in It is expressed as the normalized coordinates of the observation value at that point. Finally, the least squares method is used to fit the intersection point:

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

[0104] in

[0105]

[0106] Where L = (n, v) represents the Planck coordinates of the line feature determined by the corner point. So far, the fitting point coordinates of the vertical visual line feature in the second case are obtained.

[0107] After obtaining the coordinates of the fitting points in both cases, if all the fitting points come from the same label, the optimal Planck coordinates are obtained by optimizing using a loss function similar to that used to solve the ground line feature. Unlike the ground line feature, the dot product residual component of the vertical line feature is considered to be consistent with the direction of gravity acceleration:

[0108]

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

[0110] c) Solving the remaining visual line features: Sample the two-dimensional visual line features and divide them into multiple three-dimensional sampling points. Use the surface point information to calculate the depth of the sampling points, and finally fit the remaining linear features according to the coordinates of the three-dimensional sampling points. For the remaining linear features that have not been successfully identified as ground features or vertical features, use the surface point cloud to solve them. The solution method is the same as the ground feature. The fitting method is used to solve the line features and decompose the line features into two endpoints and several midpoints. After successfully solving the two endpoints, sample the line features into several midpoints. If all the fitting points come from the surface point cloud with the same label, use the focus of the fitting to optimize the features.

[0111] Step S5, tightly coupled positioning: the visual line feature coordinates obtained in step S4 and the pre-integrated residual of the inertial measurement unit are sent to a tightly coupled visual-inertial nonlinear optimization estimator based on graph optimization. The optimization framework obtains the optimal posture estimation of the motion carrier by minimizing the loss function.

[0112] The variables of all 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 IMU coordinate system at time k, including the translation, rotation, velocity and zero bias of the IMU coordinate system to the world coordinate system at that time. Unlike the state quantity, the Planck coordinates are used as the measurement value to represent the line feature.

[0116] Minimize the following function to solve the state quantity:

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

[0118] where e prior Represented as the marginalized prior information in the sliding window, e IMU represents the pre-integrated residual, e pro Represents the line feature reprojection 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 endpoints of the observation value segment, l=(l1,l2,l3) Τ .

[0121] Test Case

[0122] The open source dataset M2DGR is used to evaluate the algorithm of the proposed method and triangulation method. The dataset collects 15Hz image information, 10Hz radar information and 200Hz inertial measurement unit information. A mobile car is used as a carrier to collect 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 sensors have been calibrated and the trigger time series have been aligned.

[0123] Figure 5 This is the vertical line feature map obtained by this method. As can be seen from the figure, radar information can effectively correct the position of visual line features, obtain accurate three-dimensional line feature position estimation, 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 This is the vertical line feature map obtained by the triangulation method. As can be seen from the figure, due to the large pixel error of the triangulation method based on multi-view geometry, it cannot accurately depict the surrounding environment, thus causing a large deviation in the estimation of line feature positions in large-scale scenes.

[0125] Table 1 below shows the root mean square positioning error (in meters) results using this method and non-radar-assisted point-line visual odometer in the M2DGR dataset.

[0126] Table 1

[0127]

[0128]

[0129] It can be seen from Table 1 that the method of the present invention improves the average positioning accuracy by about 8.4% by using the strategy of radar-assisted visual line features. In summary, compared with the triangulation method, the method of the present invention can estimate the three-dimensional coordinates of line features more accurately.

[0130] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A radar-assisted visual line feature visual inertial odometer method, characterized in that: The steps include: S1. Extract and track image line features: extract line features for each frame of the image and track line features in adjacent frames; S2. Remove motion distortion of point cloud: Use the inertial measurement unit information to perform integration operations to remove motion distortion in the point cloud detected by the radar. Specifically, for any frame of radar point cloud, take the first point scanned by the radar as the starting point, integrate the high-frequency inertial measurement unit information, calculate the time relationship between all subsequent points except the first point and the first point, use the corresponding inertial measurement unit information integration result to calculate the coordinates of the point, and finally integrate all point clouds to obtain a frame of new point cloud with distortion removed. S3, point cloud feature extraction: extracting features with labels from the point cloud dedistorted in step S2, wherein the labels are at least divided into corner points, surface points and ground points; S4, visual line feature coordinates solution: the visual line feature coordinates solved include visual ground line features, visual vertical visual line features and other linear features, The visual ground line feature is solved by calculating the depth of the sampling point based on the ground point in step S3 using the ground point cloud information, and fitting the ground feature according to the three-dimensional sampling point coordinates; Solving the visual vertical line feature: based on the corner point in step S3, the coordinates of the visual vertical line feature are solved using a straight line fitting and line segment clipping method; Solving the remaining linear features: based on the surface points in step S3, sampling the two-dimensional visual linear features, dividing them into multiple three-dimensional sampling points, calculating the depth of the sampling points using the surface point information, and fitting the remaining linear features according to 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 sent to a tightly coupled visual-inertial nonlinear optimization estimator based on graph optimization, and the optimal posture estimation of the motion carrier is obtained by minimizing the loss function.

2. The radar-assisted visual line feature visual inertial odometer method according to claim 1, characterized in that: In step S1, LSD is used to extract line features from each frame of image; for each two adjacent frames of image, LBD is used to establish the association between line features in adjacent frames according to the coordinates of the extracted line features, and the line features on the adjacent images are tracked.

3. The radar-assisted visual line feature visual inertial odometer method according to claim 2, characterized in that: The step S2 specifically includes the following steps: S21: For any frame of point cloud, assuming that the position and attitude component quaternion of the carrier at time i is The first point l1 scanned into the point cloud is the starting point, and the position and attitude component quaternion at this time By integrating the inertial measurement unit data at the midpoint, we get: in: in 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 bias of the gyroscope and accelerometer at the same time, g w It is expressed as the gravitational acceleration in the world coordinate system, and δt corresponds to the time interval from time i to time i+1; S22: Calculate the relative translation from time i to time i+1 and the rotation matrix S23: According to the relative rotation and translation, the motion distortion from time i to time i+1 in the second point is removed: Where f1 and f2 represent the three-dimensional coordinates of the point cloud before and after distortion removal, respectively; S24: looping steps S21-S23, taking the first point as a reference, removing the motion distortion of each point in the point cloud, and finally obtaining a new frame of dedistorted point cloud.

4. The radar-assisted visual line feature visual inertial odometer method according to claim 1, characterized in that: The 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, and if it is found that the distance between the corresponding points of adjacent line bundles does not change much, it is regarded as a ground point; otherwise, it is a non-ground point, and enter 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 regarded as a corner point; otherwise, it is regarded as a surface point; S33: According to the direction of laser beam scanning, if a subset of corner points or a subset of surface points with little continuous depth change is found, the radar point cloud object segmentation method is used to assign the same label to them and regard them as the same cluster or class.

5. The radar-assisted visual line feature visual inertial odometer method according to claim 4, characterized in that: The curvature of the non-ground point concentration point in step S32 is specifically: where h n Represents the curvature value of the nth point in the point cloud, r n Indicates the distance value from the nth point in the point cloud to the coordinate origin.

6. The radar-assisted visual line feature visual inertial odometer method according to claim 1, characterized in that: In the process of solving the visual vertical line feature in step S4, When there are two intersecting planes in the field of view, and they intersect in a straight line, the corner point cloud is projected onto the image plane, and the corner points that are closer to the vertical line feature are screened out; if more than three corner points are scanned in a line beam, the two leftmost points and the two rightmost points are selected and fitted into two straight lines, and then the angle between the two straight lines is detected. If the angle approaches 90 degrees, the intersection point is regarded as a valid point, and the coordinates of the fitting 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 screened out to perform line feature clipping and obtain the fitting point coordinates of the vertical visual line features at this time; After obtaining the coordinates of the fitting points in both cases, if all the fitting points come from the same label, a loss function similar to that used in solving the ground line features is used for optimization to obtain the optimal Planck coordinates.

7. The radar-assisted visual line feature visual inertial odometer method according to claim 1, characterized in that: In the step S5, the obtained coordinates of each three-dimensional line feature are sent to a tightly coupled nonlinear optimization estimator based on graph optimization, the pre-integration residual of the inertial measurement unit and the line feature reprojection error are set as residual terms, and the optimal estimation of the attitude is obtained by minimizing the residual terms; The variables of all state variables to be estimated in the sliding window are as follows: X=[x0,x1…x S ] Τ where x k is the state of the inertial measurement unit coordinate system at time k, including the translation, rotation, velocity and zero bias of the inertial measurement unit coordinate system to the world coordinate system at that time; Minimize the following loss function to solve the state quantity: argmin X {e prior +e IMU +e pro } where e prior Represented as the marginalized prior information in the sliding window, e IMU represents the pre-integrated residual, e pro Represents the line feature reprojection residual; the line feature residual is defined as follows: Where d(f,l) is the distance from point f to line l, and are the two endpoints of the observation value segment, l=(l1,l2,l3) Τ .

8. A radar-assisted visual line feature visual inertial odometry system, comprising a computer program, characterized in that: When the computer program is executed by a processor, the steps of any of the above methods are implemented.

Images