Vision-assisted multi-paragraph radar inertial positioning method and system in degraded environment

Abstract

The invention discloses a vision-assisted multi-section radar inertial positioning method and system in a degraded environment, and the method comprises the steps: carrying out the scanning matching of each frame of radar point cloud, and transmitting an obtained radar odometer residual error into a radar inertial visual odometer; detecting a degraded section, if the detection is successful, sending the degraded section into depth registration, otherwise, executing a radar odometer, and sending an obtained relative error of the radar odometer into pose map optimization; calculating an inertial measurement unit pre-integration residual error between adjacent radar point cloud frames, and respectively sending the inertial measurement unit pre-integration residual error into depth registration, a radar inertial visual odometer and pose map optimization; feature tracking is carried out on each frame of picture, depth registration is carried out, and a visual re-projection residual error is sent into a radar inertial visual odometer; executing a radar inertial visual odometer in the degeneration paragraph, and sending an obtained pose residual error in the paragraph into pose map optimization; and finally, performing pose map optimization to obtain a pose result. According to the invention, the positioning precision can be greatly improved, and a more accurate degraded environment structure can be obtained.

Description

Technical Field

[0001] The invention belongs to the technical field of SLAM (Simultaneous Localization and Mapping), and in particular relates to a visually assisted multi-segment radar inertial positioning method and system in a degraded environment. Background Art

[0002] When the carrier moves in a geometrically degraded environment such as a tunnel or corridor, the structure it perceives has a weak variability, which causes the radar-inertial SLAM system to diverge or even collapse. In order to deal with the problem of degraded environments, many methods attempt to supplement the radar-inertial SLAM system with additional visual information throughout the process. Although these methods can alleviate the impact of degraded environments to a certain extent, they neither fully consider the temporal continuity of the degraded environment nor selectively add additional visual information in time, resulting in the inability to cope with complex degraded environments. Summary of the invention

[0003] The present invention is aimed at the problems existing in the prior art, and proposes a multi-section radar inertial positioning method and system with visual assistance in a degraded environment. First, scan matching is performed on each frame of radar point cloud, and the obtained radar odometer residual is sent to the radar inertial visual odometer; the degraded section is detected, and if the detection is successful, it is sent to the depth registration, otherwise the radar odometer is performed, and the obtained radar odometer relative error is sent to the pose graph optimization; then the inertial measurement unit pre-integration residual between adjacent radar point cloud frames is calculated, and is sent to the depth registration, radar inertial visual odometer and pose graph optimization respectively; then feature tracking is implemented for each frame of the picture, and depth registration is performed, and the visual reprojection residual is sent to the radar inertial visual odometer; the radar inertial visual odometer is performed in the degraded section, and the obtained pose residual in the section is sent to the pose graph optimization; finally, the pose graph optimization is performed to obtain the pose result. The present invention can greatly improve the positioning accuracy and obtain a more accurate degraded environment structure.

[0004] In order to achieve the above object, the technical solution adopted by the present invention is: a multi-segment radar inertial positioning method assisted by vision in a degraded environment, comprising the following steps:

[0005] S1: Perform scan matching and degraded segment detection on each frame of the radar point cloud, and send the obtained radar odometer residual to the radar inertial visual odometer; when a degraded segment is detected, perform depth registration on the degraded segment result; when no degraded segment is detected, perform radar odometer, and send the obtained radar odometer relative error to the pose graph optimization;

[0006] S2: Receive inertial data from the inertial measurement unit, collect inertial data between adjacent radar point cloud frames, calculate the inertial measurement unit pre-integration residual between two adjacent radar point cloud frames, and perform depth registration, radar inertial visual odometer and pose graph optimization operations respectively;

[0007] S3: Implement feature tracking for each frame of the image from the camera, perform depth registration on the degraded segment result obtained in step S1 and the pre-integrated residual of the inertial measurement unit calculated in step S2, and obtain the visual reprojection residual;

[0008] S4: According to the radar odometer residual obtained in step S1, the inertial measurement unit pre-integration residual in step S2 and the visual reprojection residual in step S3, the radar inertial visual odometer in the degraded section is executed to obtain the pose residual in the section;

[0009] S5: Perform pose graph optimization according to the relative error of the radar odometer in step S1, the pre-integration residual of the inertial measurement unit in step S2, and the intra-segment pose residual in step S4 to obtain the final pose result.

[0010] As an improvement of the present invention, the scan matching in step S1 is specifically as follows: according to the curvature of each point in the point cloud, the current frame point cloud is divided into corner points and surface points, and for each current frame corner point and surface point, the two nearest neighboring points of the current frame corner point and the three nearest neighboring points of the current frame surface point are found in the local map, and the distance d from the current frame corner point to the straight line determined by the two nearest neighboring points is minimized. e And the distance d from the current frame point to the plane determined by the three nearest neighboring points p , get the optimal global pose of the current frame, and the corresponding distance residual is defined as follows:

[0011]

[0012] in and represents the jth corner point and face point in the current frame, and are the two nearest neighbors of the corner point, and are the three nearest neighbor points of the surface point.

[0013] As another improvement of the present invention, the specific method for detecting the degraded segment in step S1 is: setting up a buffer mechanism to determine whether there is a degraded segment according to the degradation properties of a single frame; wherein the least squares problem involved in the optimal estimation of the pose is defined as:

[0014]

[0015] Among them J i Expressed as the residual term f iRelative to the Jacobian matrix of the variable to be estimated, the iterative amount δx is redefined as:

[0016]

[0017] Among them, H i is the related Hessian matrix. By comparing the eigenvalue of the Hessian matrix with the preset value, the degradation condition and degradation degree of freedom of the current frame are determined.

[0018] As another improvement of the present invention, the calculation method of the pre-integration residual in step S2 is specifically as follows: collect the inertial angular velocity and acceleration between any two frames of radar point clouds, and calculate the inertial angular velocity and acceleration according to the velocity v at time t. t , translation amount p t and the rotation R t By integrating the inertial data within the Δt interval, we can obtain the velocity v at time t+Δt. t+Δt , translation amount p t+Δt and the rotation R t+Δt :

[0019]

[0020] in and are the measured values ​​of acceleration and angular velocity at that moment, and is the zero bias and white noise of the accelerometer, and is the gyroscope's zero bias and white noise, exp is the exponential map of the Lie group, g is the gravitational acceleration; by repeating the IMU pre-integration process at different times, the posture transformation relationship between any two frames of radar point clouds is solved.

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

[0022] S31: extracting feature points with obvious pixel gradients from each image, tracking the feature points using optical flow, and establishing a feature correspondence between the current image frame and the previous image frame;

[0023] S32: For any visual feature, use the radar point cloud information to align its depth; according to the external parameters between the camera and the lidar, convert the radar point cloud to the camera coordinate system; use the projection relationship to project the point cloud in the camera coordinate system to spherical coordinates; given the spherical coordinates of any visual feature, find the three nearest neighbor points of the feature from the spherical radar point cloud, and determine the depth of the feature by calculating the distance from the visual feature to the plane determined by the three nearest neighbor points.

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

[0025] S41, double sliding window advancement: use inner window and outer window to model the state problem in the degraded section, where the flexible-sized inner window only optimizes the state variables in the degraded section, and the fixed-sized outer window is responsible for managing the local map constructed by several radar point cloud scans; when the window enters the degraded section from the non-degraded section, the outer window advances forward normally without being affected by the degraded section, and the inner window is the intersection of the outer window and the degraded section, and its size increases with time; when the window leaves the degraded section and returns to the non-degraded section, the outer window still advances forward normally, and the size of the inner window decreases with time;

[0026] S42, degraded section optimization: apply radar odometer residual, visual reprojection residual and inertial measurement unit pre-integration residual, estimate the carrier state quantity in the inner window, and solve the optimal state variable in the inner window by minimizing the radar odometer residual, visual reprojection residual and inertial measurement unit pre-integration residual.

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

[0028] S51: fix the pose in the segment obtained in step S4 and regard it as a single node, regard each pose in the non-degenerate segment as a node, and perform pose graph optimization; wherein the residual term between nodes is the inertial measurement unit pre-integration residual and the radar odometer residual:

[0029]

[0030] Where X n is the position state variable of the system in the non-degenerate segment, D s is the initial posture state variable in the degenerate segment, Represents the measured rotation matrix and measured translation from the end point to the starting point in the degenerate segment, is a pair of two-position state variables in time order within a non-degenerate segment, is the pairing that links the starting point of a degenerate segment to its neighboring state variable in a non-degenerate segment, is a pairing that links the endpoint of a degenerate segment to its neighboring state variable in a nondegenerate segment;

[0031] S52: Merge the posture state variables in the non-degenerate segment of step S51 with the posture state variables in the degenerate segment obtained in step S4 to obtain the final trajectory.

[0032] In order to achieve the above-mentioned purpose, the present invention also adopts a technical solution: a visually assisted multi-segment radar inertial synchronous positioning and mapping system in a degraded environment, including a computer program, which, when executed by a processor, implements the steps of any of the methods described above.

[0033] Compared with the prior art, the present invention has the following beneficial effects: the present invention proposes a method and system for multi-section radar inertial positioning with visual assistance in a degraded environment. The method uses a method based on a buffer mechanism and eigenvalues ​​to detect degraded sections, obtains non-degraded sections and degraded sections, and for degraded sections, uses a double sliding window method to model the optimization problem, introduces visual reprojection residuals only in degraded sections, and finally treats the pose nodes in the degraded sections as fixed independent nodes, uses pose graph optimization to merge the pose nodes in the degraded sections and non-degraded sections, and obtains the final estimated trajectory. The method of the present invention is suitable for radar inertial SLAM, and by segmenting the degraded sections, the temporal consistency of the degraded sections is taken into account, and visual residual information is selectively applied only in the degraded sections; using the method of the present invention, compared with the traditional method of using visual information throughout the process, the positioning accuracy is improved, and a more accurate degraded environment map can be established. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a schematic diagram of the steps of the method of the present invention;

[0035] Figure 2 is a schematic diagram of degraded paragraph detection in step S1 of the method of the present invention;

[0036] Figure 3 It is a schematic diagram of the double sliding window advancement strategy in step S4 of the method of the present invention;

[0037] Figure 4 is a schematic diagram of pose graph optimization in step S4 of the method of the present invention;

[0038] Figure 5 It is a trend chart of the number of feature tracking times in the Floor sequence in the test example of the present invention as the number of pictures changes;

[0039] Figure 6 is a schematic diagram of the tunnel map constructed in the Tunnel sequence in the test case of this method;

[0040] Figure 7 It is a tunnel map constructed in the Tunnel sequence using the visual information method throughout the test example of the present invention. DETAILED DESCRIPTION

[0041] 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.

[0042] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as those generally understood by those skilled in the art in the art to which the present invention belongs. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with the meanings in the context of the prior art, and will not be interpreted with idealized or overly formal meanings unless defined as herein.

[0043] Example 1

[0044] A vision-assisted multi-segment radar inertial positioning method in degraded environments, such as Figure 1 As shown, the following steps are included:

[0045] Step S1: scan matching, degraded segment detection and radar odometry.

[0046] Scan matching is performed on each frame of radar point cloud, and the obtained radar odometry residual is sent to the radar inertial visual odometry; the degraded segment results are detected. If a degraded segment is detected, the result is deeply aligned, otherwise the radar odometry is performed, and the obtained radar odometry relative error is sent to the pose graph optimization in the subsequent step.

[0047] Use a curvature-based method to cluster the current point cloud, and consider points with curvature higher than the threshold as corner points, and vice versa as surface points. There are sharper geometric changes near corner points, and gentler geometric changes near surface points. Build a local map by collecting several radar scans covered by the outer window. For each current frame corner point and surface point, find the two nearest neighbors of the current frame corner point and the three nearest neighbors of the current frame surface point in the local map. Use the distance between the current frame corner point and the surface point as the residual term, and determine that the corner point should be located on the straight line determined by the two nearest neighbors, and the surface point should be located on the plane determined by the three nearest neighbors. Therefore, by minimizing the distance d from the current frame corner point to the determined straight line e And the distance d from the current frame point to the determined plane p , get the optimal global pose of the current frame, and the corresponding distance residual is defined as follows:

[0048]

[0049] in and represents the jth corner point and face point in the current frame, and are the two nearest neighbors of the corner point, and are the three nearest neighbor points of the surface point.

[0050] The eigenvalue-based method is used to determine whether there are degenerate properties and degenerate degrees of freedom in the scan matching optimization of the current frame. Specifically, a buffer mechanism is established to determine whether there are degenerate sections based on the degenerate properties of a single frame. The least squares problem involved in the optimal pose estimation can be defined as:

[0051]

[0052] Among them J i Expressed as the residual term f i The Jacobian matrix with respect to the variables to be estimated.

[0053] The calculation of the iteration amount δx can be redefined as:

[0054]

[0055] Among them, H i is the related Hessian matrix. By comparing the eigenvalue of the Hessian matrix with the preset value, the degradation condition and degradation degree of freedom of the current frame are determined. The continuous degraded frames after the non-degraded frame are defined as the starting point of the degraded segment, and the continuous non-degraded frames after the degraded frame are defined as the end point of the degraded segment.

[0056] According to engineering experience, the thresholds corresponding to the three eigenvalues ​​from small to large can be [0.12, 0.27, 0.48] respectively. If all three eigenvalues ​​are greater than the set threshold, it is considered to be a non-degenerate situation and the degenerate degrees of freedom are recorded; otherwise, it is considered to be a degenerate situation.

[0057] Figure 2 is a schematic diagram of degenerate paragraph detection. Figure 2 As shown, if the non-degenerate state is always maintained during the iteration process, it is called a completely non-degenerate state and is represented by the letter a; if the non-degenerate state is changed to the degenerate state during the iteration process, it is called the starting degenerate state and is represented by the letter c; if the degenerate state is always maintained during the iteration process, it is called a completely degenerate state and is represented by the letter b; if the degenerate state is changed to the non-degenerate state during the iteration process, it is called the ending degenerate state and is represented by the letter d.

[0058] Figure 2 In the example, a buffer mechanism is set up in chronological order, and the transition process from the completely non-degraded state a to the beginning degraded state c and then to the completely degraded state b is regarded as the beginning of the degradation segment; the transition process from the completely degraded state b to the end degraded state d and then to the completely non-degraded state a is regarded as the end of the degradation segment. The beginning of each degradation segment corresponds to an end.

[0059] According to the pose at each moment obtained by scan matching, the transformation relationship between them is calculated in chronological order. According to the results of scan matching optimization at time i and time j, the relative rotation ΔR between the two frames is calculated. i,j and relative translation Δp i,j , get the radar odometer, the calculation method is as follows:

[0060]

[0061] Where R i and R j represents the rotation amount at two moments, p i and p j Represents the translation between two moments. This relative relationship is fed into the subsequent pose graph optimization framework.

[0062] Step S2: Calculate the pre-integration residual.

[0063] Collect the inertial angular velocity and acceleration between any two frames of radar point cloud, and calculate the posture transformation relationship between the two moments. t , translation amount p t and the rotation R t By integrating the inertial data within the Δt interval, we can obtain the velocity v at time t+Δt. t+Δt , translation amount p t+Δt and the rotation R t+Δt :

[0064]

[0065] in and are the measured values ​​of acceleration and angular velocity at that moment, and is the zero bias and white noise of the accelerometer, and is the zero bias and white noise of the gyroscope, exp represents the exponential mapping of the Lie group, g is the acceleration due to gravity.

[0066] By repeating the above process, the pose transformation relationship between any two frames of radar point cloud is solved, and the transformation relationship is sent to the deep registration module, the radar inertial visual odometry within the degraded section, and the pose graph optimization.

[0067] Step S3: Feature tracking and depth registration.

[0068] S31: Extract feature points with obvious pixel gradients for each image, and use a uniform distribution mechanism to evenly distribute the extracted feature points on the image, with no more than 150 feature points on a single frame. Use optical flow to track these feature points and establish feature correspondence between the current image frame and the previous image frame. If there is an image with less than 30 tracking feature point pairs, add new feature points to the image frame to ensure that the number of tracking feature point pairs is greater than 60.

[0069] S32: For any visual feature, use the dedistorted radar point cloud information to align its depth. Dedistort the point cloud through the inertial measurement unit pre-integration transformation. Take a single frame point cloud as an example, take the first scanned point as the origin, apply the pre-integration transformation relationship to the remaining points in the point cloud, and complete the motion compensation of the remaining point cloud.

[0070] According to the external parameters between the camera and the lidar, the radar point cloud is converted to the camera coordinate system. Using the projection relationship, the point cloud in the camera coordinate system is further projected to the spherical coordinates. Given the spherical coordinates of any visual feature, find the three nearest neighbors of the feature from the spherical radar point cloud. Determine the depth of the feature by calculating the distance from the visual feature to the plane determined by the three nearest neighbors. If the calculated depth is less than 0.1 meters or greater than the maximum range of the lidar, use the multi-view triangulation method to solve the depth of the feature.

[0071] Step S4: Degrade the laser inertial visual odometry within the segment.

[0072] S41, double sliding window advancement: Figure 3 As shown in the figure, the state problem in the degraded section is modeled using an inner window and an outer window, where the flexible-sized inner window only optimizes the state variables in the degraded section, and the fixed-sized outer window is responsible for managing the local map constructed by several radar point cloud scans and sending it to step 1.1 for current frame scan matching. The maximum size of the inner window is equal to the outer window size, which is set to 10 in this example.

[0073] like Figure 3 As shown in the first and fifth rows of , when the window does not involve the interior of the degenerate paragraph, the optimization problem does not exist, so there is no inner window, and the outer window advances normally. Figure 3 As shown in the third row of , when the window is completely inside the degenerate paragraph, the optimization problem exists and the two windows completely overlap.

[0074] like Figure 3 As shown in the second row of , when the window enters the degenerate segment from the non-degenerate segment, the outer window is not affected by the degenerate segment and moves forward normally, while the inner window is the intersection of the outer window and the degenerate segment, and its size increases with time. Figure 3As shown in the fourth row of , when the window leaves the degenerate segment and returns to the non-degenerate segment, the outer window still moves forward normally, and the size of the inner window decreases over time.

[0075] S42, Degraded Segment Optimization: Estimate the pose variables in the degraded segment, and estimate the carrier state quantity in the inner window by applying the radar odometer residual, visual reprojection residual and inertial measurement unit pre-integration residual. D is defined as:

[0076]

[0077] where x k is the state of the carrier at the kth moment, including the rotation amount R k , translation amount p k , accelerometer bias and gyro bias μ L is the inverse depth of the Lth feature that does not participate in depth registration, S is the size of the inner window, and is set to 10 in this embodiment.

[0078] By minimizing the radar odometry residual r L , visual reprojection residual r V and the IMU pre-integration residual r I , solve the optimal state variable in the inner window. The least squares loss function of this problem is as follows:

[0079]

[0080] in It indicates the pairing of two poses in the inner window in chronological order. is the tracking relationship of visual features. ρ is the robust kernel function, is the spherical coordinate of the visual feature observed for the first time at the i-th moment at the j-th moment, Σ V ,Σ I and Σ L Represent the covariance matrix of the corresponding residuals. Calculated by pre-integration of the inertial measurement unit, and is the pose transformation measurement from the camera coordinate system to the nearest radar coordinate system at the i-th and j-th moments.

[0081] The visual reprojection residual is defined as follows:

[0082]

[0083] in:

[0084]

[0085] b 1 and b 2 Any set of basis that constitutes the tangent plane of the visual feature on the sphere, is the measured coordinate of the feature on the spherical surface at time j. i ,p i ] and [R j ,p j ] is the pose state variable at the i-th and j-th moments, ν is the inverse depth of the feature, R cb is the rotation matrix from the carrier coordinate system to the camera coordinate system, p bc It is the translation from the camera coordinate system to the carrier coordinate system.

[0086] According to the degenerate degree of freedom result, in order to prevent the state increment from increasing along the degenerate degree of freedom direction to the radar odometer residual, the least squares problem formula (2) is rewritten as:

[0087]

[0088] Where Λ is the eigenvalue matrix that sets the eigenvalues ​​related to the detected degenerate dimensions to 0, and Q is the matrix composed of the eigenvectors corresponding to the eigenvalues. The pre-integration residual of the inertial measurement unit is shown in formula (4).

[0089] Step S5: pose graph optimization.

[0090] like Figure 4 As shown, the pose in the degenerate segment obtained from step S4 is fixed and regarded as a single node, and each pose in the non-degenerate segment is regarded as a node, and pose graph optimization is performed. Among them, the inertial measurement unit pre-integration residual and radar odometer residual are applied between the start and end points of the degenerate segment and the nodes in the nearest non-degenerate segment. The overall least squares problem is defined as:

[0091]

[0092] Where X n is the position state variable of the system in the non-degenerate segment, D s is the initial pose state variable in the degenerate segment. Represents the measured rotation matrix and measured translation from the end point to the starting point in the degenerate segment, is a pair of two-position state variables in time order within a non-degenerate segment, is the pairing that links the starting point of a degenerate segment to its neighboring state variable in a non-degenerate segment, is a pair that links the endpoint of a degenerate segment to its adjacent state variable in a non-degenerate segment.

[0093] The pose state variables in the non-degenerate segment are merged with the pose state variables in the degenerate segment to obtain the final trajectory.

[0094] Test Case

[0095] The method is evaluated using open source datasets and measured data, which collects 10Hz image information, 10Hz radar information, and 200Hz IMU information, and collects visual, inertial, and radar information outdoors. All sequences can run in real time on the Robot Operating System (ROS) platform, and the external parameters between sensors have been calibrated and the trigger time series have been aligned.

[0096] Figure 5 This is a trend chart of the number of feature tracking times in the Floor sequence as the number of images changes. This trend chart records the number of visual tracking times in degraded and non-degraded environments, where the degraded environments are separated by vertical lines, corresponding to frames 122 to 207, 528 to 598, and 708 to 879, respectively. As can be seen from the figure, compared with non-degraded environments, the number of visual feature tracking times in degraded environments is higher. Therefore, this figure proves the effectiveness of visual features in geometrically degraded environments.

[0097] Figure 6 It is the tunnel map constructed by this method in the Tunnel sequence, and the length of the tunnel map is 1521 meters; Figure 7 This is a tunnel map constructed in the Tunnel sequence using the visual information method throughout the process. The map is 1168 meters long, and the actual length of the tunnel is 1580 meters. As can be seen from the figure, compared with the tunnel map constructed using the visual information method throughout the process, the tunnel map constructed by this method is more in line with the actual situation in terms of estimated length and corner angles. Therefore, the two comparison pictures prove that this method achieves more accurate positioning and mapping in a degraded environment by adding visual information only in degraded sections.

[0098] The following table compares the root mean square positioning error (in meters) of the present method and the method using visual information throughout the process. As shown in Table 1, the present method improves the average positioning accuracy by about 18.5% by virtue of the multi-segment strategy in the degraded environment.

[0099] Table 1

[0100] sequence Full visual approach This method promote Factory 1.43 1.43 - Ocean 1.45 1.45 - Sewerage 2.95 1.51 48.8％ Corridor 3.03 3.01 0.6％ Floor 3.85 2.45 36.3％ Block 0.11 0.11 - Handheld 3.93 4.21 -7.1％ Tunnel 28.91 8.66 70.0％ average - - 18.5％

[0101] In summary, the method of the present invention detects degraded paragraphs based on the buffer mechanism and the eigenvalue method to obtain non-degraded paragraphs and degraded paragraphs; for the degraded paragraphs, the optimization problem is modeled based on the double sliding window method, and the visual reprojection residual is introduced only in the degraded paragraphs; finally, by treating the pose node in the degraded paragraph as a fixed independent node, the pose nodes in the degraded paragraph and the non-degraded paragraph are optimized and merged using the pose graph to obtain the final estimated trajectory, which greatly improves the positioning accuracy.

[0102] It should be noted that the above content merely 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 visually assisted multi-segment radar inertial positioning method in a degraded environment, characterized in that: The steps include: S1: Perform scan matching and degraded segment detection on each frame of the radar point cloud, and send the obtained radar odometer residual to the radar inertial visual odometer; when a degraded segment is detected, perform depth registration on the degraded segment result; when no degraded segment is detected, perform radar odometer, and send the obtained radar odometer relative error to the pose graph optimization; S2: Receive inertial data from the inertial measurement unit, collect inertial data between adjacent radar point cloud frames, calculate the inertial measurement unit pre-integration residual between radar point cloud frames, and perform depth registration, radar inertial visual odometer and pose graph optimization operations respectively; S3: Implement feature tracking for each frame of the image from the camera, perform depth registration on the degraded segment result obtained in step S1 and the pre-integrated residual of the inertial measurement unit calculated in step S2, and obtain the visual reprojection residual; S4: According to the radar odometer residual obtained in step S1, the inertial measurement unit pre-integration residual in step S2 and the visual reprojection residual in step S3, the radar inertial visual odometer in the degraded section is executed to obtain the pose residual in the section; S5: Perform pose graph optimization according to the relative error of the radar odometer in step S1, the pre-integration residual of the inertial measurement unit in step S2, and the intra-segment pose residual in step S4 to obtain the final pose result.

2. The visually assisted multi-segment radar inertial positioning method in a degraded environment as claimed in claim 1, characterized in that: The scan matching in step S1 is specifically as follows: according to the curvature of each point in the point cloud, the current frame point cloud is divided into corner points and surface points, and for each current frame corner point and surface point, the two nearest neighboring points of the current frame corner point and the three nearest neighboring points of the current frame surface point are found in the local map, and the distance d from the current frame corner point to the straight line determined by the two nearest neighboring points is minimized. e And the distance d from the current frame point to the plane determined by the three nearest neighboring points p , get the optimal global pose of the current frame, and the corresponding distance residual is defined as follows: in and represents the jth corner point and face point in the current frame, and are the two nearest neighbors of the corner point, and are the three nearest neighbor points of the surface point.

3. The visually assisted multi-segment radar inertial positioning method in a degraded environment as claimed in claim 2, characterized in that: The specific method of degraded segment detection in step S1 is: establish a buffer mechanism, and determine whether there is a degraded segment according to the degradation properties of a single frame; wherein the least squares problem involved in the optimal estimation of the pose is defined as: Among them J i Expressed as the residual term f i Relative to the Jacobian matrix of the variable to be estimated, the iterative amount δx is redefined as: Among them, H i is the related Hessian matrix. By comparing the eigenvalue of the Hessian matrix with the preset value, the degradation condition and degradation degree of freedom of the current frame are determined.

4. The visually assisted multi-segment radar inertial positioning method in a degraded environment as claimed in claim 1, characterized in that: The calculation method of the pre-integration residual in step S2 is specifically as follows: collect the inertial angular velocity and acceleration between any two frames of radar point cloud, and calculate the velocity v at time t according to the velocity v at time t. t , translation amount p t and the rotation R t By integrating the inertial data within the Δt interval, we can obtain the velocity v at time t+Δt. t+Δt , translation amount p t+Δt and the rotation R t+Δt : in and are the measured values ​​of acceleration and angular velocity at that moment, and is the zero bias and white noise of the accelerometer, and is the zero bias and white noise of the gyroscope, exp is the exponential mapping of the Lie group, g is the gravitational acceleration; by repeating the IMU pre-integration process at different times, the posture transformation relationship between any two frames of radar point clouds is solved.

5. The visually assisted multi-segment radar inertial positioning method in a degraded environment as claimed in claim 1, characterized in that: The step S3 specifically includes the following steps: S31: extracting feature points with obvious pixel gradients from each image, tracking the feature points using optical flow, and establishing a feature correspondence between the current image frame and the previous image frame; S32: For any visual feature, use the radar point cloud information to align its depth; according to the external parameters between the camera and the lidar, convert the radar point cloud to the camera coordinate system; use the projection relationship to project the point cloud in the camera coordinate system to spherical coordinates; given the spherical coordinates of any visual feature, find the three nearest neighbor points of the feature from the spherical radar point cloud, and determine the depth of the feature by calculating the distance from the visual feature to the plane determined by the three nearest neighbor points.

6. The visually assisted multi-segment radar inertial positioning method in a degraded environment as claimed in claim 1, characterized in that: The step S4 specifically includes the following steps: S41, double sliding window advancement: use inner window and outer window to model the state problem in the degraded section, where the flexible-sized inner window only optimizes the state variables in the degraded section, and the fixed-sized outer window is responsible for managing the local map constructed by several radar point cloud scans; when the window enters the degraded section from the non-degraded section, the outer window advances forward normally without being affected by the degraded section, and the inner window is the intersection of the outer window and the degraded section, and its size increases with time; when the window leaves the degraded section and returns to the non-degraded section, the outer window still advances forward normally, and the size of the inner window decreases with time; S42, degraded section optimization: apply radar odometer residual, visual reprojection residual and inertial measurement unit pre-integration residual, estimate the carrier state quantity in the inner window, and solve the optimal state variable in the inner window by minimizing the radar odometer residual, visual reprojection residual and inertial measurement unit pre-integration residual.

7. The visually assisted multi-segment radar inertial positioning method in a degraded environment as claimed in claim 6, characterized in that: The step S5 specifically includes the following steps: S51: fix the pose in the segment obtained in step S4 and regard it as a single node, regard each pose in the non-degenerate segment as a node, and perform pose graph optimization; wherein the residual term between nodes is the inertial measurement unit pre-integration residual and the radar odometer residual: Where X n is the position state variable of the system in the non-degenerate segment, D s is the initial posture state variable in the degenerate segment, Represents the measured rotation matrix and measured translation from the end point to the starting point in the degenerate segment, is a pair of two-position state variables in time order within a non-degenerate segment, is the pairing that links the starting point of a degenerate segment to its neighboring state variable in a non-degenerate segment, is a pairing that links the endpoint of a degenerate segment to its neighboring state variable in a nondegenerate segment; S52: Merge the posture state variables in the non-degenerate segment of step S51 with the posture state variables in the degenerate segment obtained in step S4 to obtain the final trajectory.

8. A multi-segment radar inertial positioning system for visually assisted operation in degraded environments, comprising a computer program characterized by: When the computer program is executed by a processor, the steps of any of the above methods are implemented.

Images