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

Abstract

The present invention discloses a multi-segment radar inertial positioning method and system with visual assistance in a degraded environment. The method performs scan matching on each frame of radar point cloud, and feeds the obtained radar odometer residual into the radar inertial visual odometer; detects the degraded segment, and if the detection is successful, feeds it into the depth registration, otherwise performs radar odometer, and feeds the obtained radar odometer relative error into the pose graph optimization; calculates the inertial measurement unit pre-integration residuals between adjacent radar point cloud frames, and feeds them into the depth registration, radar inertial visual odometer, and pose graph optimization respectively; then implements feature tracking on each frame, performs depth registration, and feeds the visual reprojection residual into the radar inertial visual odometer; performs radar inertial visual odometer within the degraded segment, and feeds the obtained intra-segment pose residual into the pose graph optimization; finally, performs pose graph optimization to obtain the pose result. The present invention can significantly improve positioning accuracy and obtain a more accurate degraded environment structure.

Description

Technical Field

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

[0002] When a vehicle moves through geometrically degraded environments, such as tunnels and corridors, the perceived structures are weakly variable, causing radar-inertial SLAM systems to diverge or even crash. To address this degraded environment, many methods attempt to supplement radar-inertial SLAM systems with additional visual information throughout the entire process. While these methods can mitigate the impact of degraded environments to a certain extent, they neither fully consider the temporal continuity of degraded environments nor selectively add additional visual information over time, resulting in an inability to cope with complex degraded environments. Summary of the Invention

[0003] The present invention addresses the problems existing in the prior art and proposes a multi-segment radar inertial positioning method and system with visual assistance in a degraded environment. First, scan matching is performed on each frame of the radar point cloud, and the obtained radar odometry residual is fed into the radar inertial visual odometry. Degraded segments are detected. If the detection is successful, it is fed into depth registration. Otherwise, radar odometry is performed, and the obtained radar odometry relative error is fed into pose graph optimization. Then, the inertial measurement unit pre-integration residuals between adjacent radar point cloud frames are calculated and fed into depth registration, radar inertial visual odometry, and pose graph optimization respectively. Feature tracking is then performed on each frame of the image, depth registration is performed, and the visual reprojection residual is fed into the radar inertial visual odometry. Radar inertial visual odometry is performed within the degraded segment, and the obtained intra-segment pose residual is fed into pose graph optimization. Finally, pose graph optimization is performed to obtain the pose result. The present invention can significantly improve positioning accuracy and obtain more accurate degraded environment structures.

[0004] To achieve the above object, the present invention adopts a technical solution: a multi-segment radar inertial positioning method with visual assistance in a degraded environment, comprising the following steps:

[0005] S1: Perform scan matching and degenerate segment detection on each frame of the radar point cloud, and feed the obtained radar odometry residual into the radar inertial visual odometry. When a degenerate segment is detected, perform depth registration on the degenerate segment result. When no degenerate segment is detected, perform radar odometry and feed the obtained radar odometry relative error into the pose graph optimization.

[0006] S2: Receives inertial data from the inertial measurement unit, collects inertial data between adjacent radar point cloud frames, calculates the inertial measurement unit pre-integration residual between two adjacent radar point cloud frames, and performs depth registration, radar inertial visual odometry, and pose graph optimization operations respectively;

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

[0008] S4: Based on the radar odometry residual obtained in step S1, the inertial measurement unit pre-integration residual in step S2, and the visual reprojection residual in step S3, perform the degraded radar inertial visual odometry in the segment to obtain the pose residual in the segment;

[0009] S5: Perform pose graph optimization based on 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 a corner point, and are the three nearest neighbor points of a surface point.

[0013] As another improvement of the present invention, the specific method for detecting degraded sections in step S1 is: establishing a buffer mechanism and determining whether a degraded section exists based on the degradation properties of a single frame; wherein the least squares problem involved in optimal pose estimation 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 cloud, 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, the velocity v at time t+Δt is obtained t+Δt , translation amount p t+Δt and the rotation R t+Δt :

[0019]

[0020] in and are the acceleration and angular velocity measurements at that moment, and is the accelerometer bias and white noise, and is the gyroscope's zero bias and white noise, exp is the exponential map of the Lie group, g is the acceleration of gravity; by repeating the IMU pre-integration process at different times, the pose 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; based on 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, step S4 specifically includes the following steps:

[0025] S41. Dual sliding window advancement: Use inner and outer windows to model the state problem within the degraded section. The flexible-sized inner window optimizes only the state variables within the degraded section, while the fixed-sized outer window manages the local map constructed by multiple radar point cloud scans. When the window enters the degraded section from a non-degraded section, the outer window advances forward normally without being affected by the degraded section. The inner window is the intersection of the outer window and the degraded section, and its size increases over time. When the window leaves the degraded section and returns to the non-degraded section, the outer window continues to advance forward normally, while the inner window size decreases over time.

[0026] S42, Degraded Section Optimization: Apply radar odometry residuals, visual reprojection residuals, and inertial measurement unit pre-integration residuals to estimate the carrier state variables within the inner window, and solve the optimal state variables within the inner window by minimizing the radar odometry residuals, visual reprojection residuals, and inertial measurement unit pre-integration residuals.

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

[0028] S51: The pose in the segment obtained in step S4 is fixed and regarded as a single node. Each pose in the non-degenerate segment is regarded as a node, and pose graph optimization is performed; wherein the residual terms between nodes are the inertial measurement unit pre-integration residual and the radar odometry residual:

[0029]

[0030] where X n is the posture 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, For the pairing of two pose state variables in time order within a non-degenerate segment, is a pair 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 adjacent state variable in a non-degenerate segment;

[0031] S52: Merge the pose state variables in the non-degenerate segment of step S51 with the pose 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 existing technology, the present invention has the following beneficial effects: the present invention proposes a visually assisted multi-section radar inertial positioning method and system in a degraded environment. The method uses a buffer mechanism and eigenvalue-based method to detect degraded sections, and obtains non-degraded sections and degraded sections. For the degraded sections, the optimization problem is modeled based on a double sliding window method, and visual reprojection residuals are introduced only in the degraded sections. Finally, by treating the pose nodes in the degraded sections as a fixed independent node, the pose nodes in the degraded sections and the non-degraded sections are optimized and merged using the pose graph to obtain the final estimated trajectory. The method of the present invention is suitable for radar inertial SLAM. 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. Compared with the traditional method of using visual information throughout the process, the positioning accuracy of the method of the present invention is improved, and a more accurate degraded environment map can be established. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 1 is a schematic flow chart 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 Schematic diagram of the dual sliding window advancement strategy in step S4 of the method of the present invention;

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

[0038] Figure 5 This is a trend chart showing the number of feature tracking times in the Floor sequence as a function of the number of images in the test example of the present invention;

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

[0040] Figure 7 This 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 described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[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 commonly 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 their meanings in the context of the prior art and, unless defined as such, will not be interpreted in an idealized or overly formal sense.

[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 fed into the radar inertial visual odometry; the degraded segment results are detected. If a degraded segment is detected, the result is depth-aligned. Otherwise, radar odometry is performed, and the obtained radar odometry relative error is fed into the pose graph optimization in the subsequent step.

[0047] Use a curvature-based method to cluster the current point cloud, and regard 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 a corner point, and are the three nearest neighbor points of a surface point.

[0050] Using an eigenvalue-based approach, we determine whether there are degenerate properties and degrees of freedom in the scan matching optimization of the current frame. Specifically, we establish a buffer mechanism 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 associated Hessian matrix. By comparing the eigenvalues of the Hessian matrix with the preset values, the degradation status and degradation degrees of freedom of the current frame are determined. The consecutive degenerate frames following a non-degenerate frame are defined as the starting point of the degradation segment, and the consecutive non-degenerate frames following a degenerate frame are defined as the end point of the degradation segment.

[0056] Based on 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 thresholds, the condition is considered non-degenerate, and the degenerate degrees of freedom are recorded; otherwise, the condition is considered degenerate.

[0057] Figure 2 is a schematic diagram of degenerate paragraph detection. Figure 2 As shown in the figure, 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 a 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 an ending degenerate state and is represented by the letter d.

[0058] Figure 2 In this example, a buffer mechanism is set up in chronological order. The transition from the fully non-degraded state a to the initially degraded state c and then to the fully degraded state b is considered the beginning of a degradation segment. The transition from the fully degraded state b to the final degraded state d and then to the fully non-degraded state a is considered the end of a 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 amount of rotation 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, the velocity v at time t+Δt is obtained t+Δt , translation amount p t+Δt and the rotation R t+Δt :

[0064]

[0065] in and are the acceleration and angular velocity measurements at that moment, and is the accelerometer bias and white noise, 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 clouds is solved, and the transformation relationship is sent to the deep registration module, the radar inertial visual odometry within the degraded segment, and the pose graph optimization.

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

[0068] S31: For each image, feature points with distinct pixel gradients are extracted and evenly distributed across the image using a uniform distribution mechanism, with no more than 150 feature points per frame. These feature points are tracked using optical flow to establish feature correspondences between the current and previous frames. If the number of tracked feature point pairs for a particular image is less than 30, additional feature points are added to that frame to ensure the number of tracked feature point pairs is greater than 60.

[0069] S32: For any visual feature, depth registration is performed using the dedistorted radar point cloud. The point cloud is dedistorted using the inertial measurement unit (IMU) pre-integration transform. For a single-frame point cloud, the first scanned point is used as the origin, and the pre-integration transform is applied to the remaining points in the point cloud to complete motion compensation.

[0070] Based on the extrinsic parameters between the camera and lidar, the radar point cloud is converted to the camera coordinate system. Using projection relationships, the point cloud in the camera coordinate system is further projected to spherical coordinates. Given the spherical coordinates of any visual feature, the three nearest neighbors of that feature are found in the spherical radar point cloud. The depth of the feature is determined by calculating the distance from the visual feature to the plane defined by the three nearest neighbors. If the calculated depth is less than 0.1 meter or greater than the maximum range of the lidar, the depth of the feature is determined using multi-view triangulation.

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

[0072] S41, double sliding window advancement: Figure 3 As shown, an inner window and an outer window are used to model the state problem within the degraded segment. The flexible-sized inner window optimizes only the state variables within the degraded segment, while the fixed-sized outer window manages the local map constructed from multiple radar point cloud scans and feeds it into step 1.1 for scan matching in the current frame. The maximum size of the inner window is equal to the outer window size, 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 segment, 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 in the inner window by applying the radar odometer residual, visual reprojection residual, and inertial measurement unit pre-integration residual. The complete state X 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 p k , accelerometer bias and gyroscope 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 variables within the inner window. The least squares loss function of this problem is as follows:

[0079]

[0080] in 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] b1 and b2 form any set of bases of 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 is the translation from the camera coordinate system to the carrier coordinate system.

[0086] According to the results of degenerate degrees of freedom, in order to prevent the state increment from increasing along the direction of degenerate degrees of freedom 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 of the detected degenerate dimensions to 0, and Q is the matrix consisting of the eigenvectors corresponding to the eigenvalues. The pre-integration residual of the inertial measurement unit is shown in Equation (4).

[0089] Step S5: pose graph optimization.

[0090] like Figure 4 As shown in Figure 1, the pose in the degenerate segment obtained from step S4 is fixed and treated as a single node. Each pose in the non-degenerate segment is treated as a node, and pose graph optimization is performed. The inertial measurement unit pre-integration residual and radar odometry 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 posture 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, For the pairing of two pose state variables in time order within a non-degenerate segment, is a pair 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] This method was evaluated using open-source datasets and field-measured data. The method collects visual, inertial, and radar information outdoors at 10Hz image, 10Hz radar, and 200Hz IMU. All sequences can be run in real time on the Robot Operating System (ROS) platform. External parameters between sensors are calibrated, and trigger time sequences are aligned.

[0096] Figure 5 This is a trend chart showing the number of feature tracking times in the Floor sequence as a function of the number of images. This trend chart records the number of visual tracking times in degraded and non-degraded environments. 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 chart, the number of visual feature tracking times in degraded environments is higher than in non-degraded environments. Therefore, this chart demonstrates the effectiveness of visual features in geometrically degraded environments.

[0097] Figure 6 This is the tunnel map constructed by this method in the Tunnel sequence. The length of the tunnel map is 1521 meters. Figure 7 This is a tunnel map constructed using visual information methods throughout the tunnel sequence. The map is 1,168 meters long, while the actual tunnel length is 1,580 meters. As can be seen from the figure, compared to the tunnel map constructed using visual information methods throughout the entire tunnel, the tunnel map constructed using this method more accurately estimates the length and corner angles. Therefore, these two comparison images demonstrate that this method, by incorporating visual information only in degraded sections, achieves more accurate positioning and mapping in degraded environments.

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

[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 sections based on a buffer mechanism and an eigenvalue method to obtain non-degraded sections and degraded sections; for degraded sections, the optimization problem is modeled based on a double sliding window method, and visual reprojection residuals are introduced only within the degraded sections; finally, by treating the pose node within the degraded section as a fixed independent node, the pose nodes within the degraded section and the non-degraded section are optimized and merged using the pose graph to obtain the final estimated trajectory, greatly improving 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 scope of protection of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications all fall within the scope of protection of the claims of the present invention.

Claims

1. A multi-segment radar inertial positioning method with visual assistance in degraded environments, characterized by: The steps include: S1: Perform scan matching and degenerate segment detection on each frame of the radar point cloud, and feed the obtained radar odometry residual into the radar inertial visual odometry. When a degenerate segment is detected, perform depth registration on the degenerate segment result. When no degenerate segment is detected, perform radar odometry and feed the obtained radar odometry relative error into the pose graph optimization. S2: Receives inertial data from the inertial measurement unit, collects inertial data between adjacent radar point cloud frames, calculates the inertial measurement unit pre-integration residual between radar point cloud frames, and performs depth registration, radar inertial visual odometry, and pose graph optimization operations respectively; S3: Perform feature tracking on each frame of the image from the camera, perform depth registration on the degraded segment result obtained in step S1 and the inertial measurement unit pre-integration residual calculated in step S2, and obtain the visual reprojection residual; S4: Based on the radar odometry residual obtained in step S1, the inertial measurement unit pre-integration residual in step S2, and the visual reprojection residual in step S3, perform the degraded radar inertial visual odometry in the segment to obtain the pose residual in the segment; S41. Dual sliding window advancement: Use inner and outer windows to model the state problem within the degraded section. The flexible-sized inner window optimizes only the state variables within the degraded section, while the fixed-sized outer window manages the local map constructed by multiple radar point cloud scans. When the window enters the degraded section from a non-degraded section, the outer window advances forward normally without being affected by the degraded section. The inner window is the intersection of the outer window and the degraded section, and its size increases over time. When the window leaves the degraded section and returns to the non-degraded section, the outer window continues to advance forward normally, while the inner window size decreases over time. S42, Degraded Segment Optimization: Apply radar odometry residuals, visual reprojection residuals, and inertial measurement unit pre-integration residuals to estimate the vehicle state within the inner window. By minimizing the radar odometry residuals, visual reprojection residuals, and inertial measurement unit pre-integration residuals, the optimal state variables within the inner window are solved. S5: Perform pose graph optimization based on 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; S51: The pose in the segment obtained in step S4 is fixed and regarded as a single node. Each pose in the non-degenerate segment is regarded as a node, and pose graph optimization is performed; wherein the residual terms between nodes are the inertial measurement unit pre-integration residual and the radar odometry residual: where X n is the posture 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, A is a pair of two pose state variables in time order within a non-degenerate segment. s is a pair that links the starting point of a degenerate segment to its adjacent state variable in a non-degenerate segment, A e is a pairing that links the endpoint of a degenerate segment to its adjacent state variable in a non-degenerate segment; S52: Merge the pose state variables in the non-degenerate segment of step S51 with the pose state variables in the degenerate segment obtained in step S4 to obtain the final trajectory.

2. The visually assisted multi-stage 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. For each current frame corner point and surface point, the two nearest neighbors of the current frame corner point and the three nearest neighbors of the current frame surface point are found in the local map. The distance d between the current frame corner point and the two nearest neighbors 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 The two nearest neighbors of a corner point, and are the three nearest neighbor points of a 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 for detecting degraded sections in step S1 is: establishing a buffer mechanism and determining whether there is a degraded section based on the degradation properties of a single frame; wherein the least squares problem involved in the optimal pose estimation 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 method for visually assisted multi-segment radar inertial positioning in a degraded environment as claimed in claim 1, wherein: 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, the velocity v at time t+△t is obtained t+△t , translation amount p t+△t and the rotation R t+△t : in and are the acceleration and angular velocity measurements at that moment, and is the accelerometer bias and white noise, and is the zero bias and white noise of the gyroscope, exp is the exponential mapping of the Lie group, and g is the acceleration of gravity. By repeating the IMU pre-integration process at different times, the pose transformation relationship between any two frames of radar point cloud 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; based on 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. 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 the method according to any one of claims 1 to 5 are implemented.

Images