LiDAR-inertia-vision fusion SLAM switching positioning method applied to degraded environment

Abstract

The invention relates to the field of mobile robot positioning, in particular to a LiDAR-inertia-vision fusion SLAM switching positioning method applied to a degraded environment. According to the method, through a multi-sensor system composed of a monocular camera, an inertial measurement unit and a laser radar which are installed on a mobile robot, original measurement data such as images, angular velocity, linear acceleration and dense space point cloud are acquired, a system state initialization model is constructed, and the multi-sensor system is used to acquire the measurement data. According to the method, the vision-inertia poses and the laser radar poses or linear interpolation of the vision-inertia poses and the laser radar poses are fused, the degradation state can be effectively recognized in real time, and multi-sensor system information switching can be carried out, so that the problem of positioning failure of the laser radar and the vision-inertia odometer module in the degradation environment can be effectively solved; the overall visual SLAM positioning precision and robustness are improved, and the long-term stable operation requirements under the severe conditions of single structure, complex illumination, sparse texture and the like are met.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning, and in particular to a LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment. Background Art

[0002] With the rapid growth of mobile robot applications in complex environments, the need for high-precision positioning technology is becoming increasingly urgent, especially in long corridors and tunnels with simple structural features, as well as in specific scenarios with poor lighting conditions or drastically changing motion. Traditional LiDAR (Light Detection and Ranging) and visual SLAM (Simultaneous Localization and Mapping) methods both have significant drawbacks. LiDAR technology is prone to positioning degradation in environments lacking geometric features, while visual SLAM technology is prone to positioning failure in scenes with rapid motion, drastic lighting changes, and texture loss, resulting in unstable and unreliable mobile robot operation. Existing technologies mostly utilize the fusion of LiDAR and visual sensors. While this approach mitigates the degradation issues of individual sensors to some extent, practical applications still present challenges such as reliance on heuristic parameter adjustments, untimely degradation detection, and the spread of degradation information leading to overall positioning failure. For example, long-term degradation can lead to the accumulation of system errors and ultimately positioning divergence. Therefore, there is an urgent need to design a non-heuristic sensor state switching fusion SLAM technology suitable for degraded environments, which can effectively identify the degraded state in real time and switch sensor information, effectively solve the problems of mobile robot positioning failure and decreased positioning accuracy in sensor-degraded environments, and ensure the long-term stable operation of mobile robots under harsh conditions. Summary of the Invention

[0003] In degraded environments of LiDAR and visual SLAM, the sensor fusion method generally suffers from the problem of untimely recognition of degradation states and long-term propagation of degradation errors, resulting in a serious decrease in positioning accuracy. Therefore, the present invention proposes a LiDAR-inertial-vision fusion SLAM switching positioning method for degraded environments to solve the problem of mobile robot positioning failure in degraded environments.

[0004] The present invention is implemented by adopting the following technical solution: a LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment, comprising the following steps:

[0005] Step 1: Use the monocular camera, inertial measurement unit, and lidar installed on the mobile robot to obtain images, angular velocity and linear acceleration, and dense spatial point clouds respectively;

[0006] Step 2: Construct the reprojection residual of the monocular camera based on the acquired image , construct the inertial measurement residual of the inertial measurement unit based on the obtained angular velocity and linear acceleration ;

[0007] Step 3: Based on the reprojection residual constructed in step 2 and inertial measurement residuals , further joint optimization is performed, and the current frame pose increment is output after nonlinear optimization through the visual-inertial odometry module;

[0008] Step 4: Based on the dense spatial point cloud output by the lidar in step 1, calculate the current frame pose increment of the lidar in the world coordinate system;

[0009] Step 5: Based on the visual-inertial optimization results from step 3 and the dense spatial point cloud acquired by the lidar in step 1, the working status of the visual-inertial odometry module and lidar in the current frame are evaluated. Fault detection of the visual-inertial odometry module and degradation detection of the lidar are performed respectively.

[0010] Step 6: Based on the degradation detection results of the lidar and the fault detection results of the visual-inertial odometry module, combined with the state posture at the previous moment , select the initial pose estimate , and use this as the initial value to perform Scan-to-Map optimization, and finally update the image frame at the current moment The state pose of ;

[0011] Step 7: The image frame calculated in step 6 The state pose of As the initial estimate of the newly added image frame, it is added to the factor graph containing the poses of all historical image frames. The factor graph combines the lidar residual, the visual-inertial joint residual, and the initial pose estimate. And construct a closed-loop factor through loop closure detection as the observation constraint of the edge in the graph, and then perform incremental optimization on the factor graph to output the final pose estimation result that is globally consistent .

[0012] The specific process of step 2 of the above-mentioned LiDAR-inertial-vision fusion SLAM switching positioning method applied to degraded environments is as follows:

[0013] For a monocular camera, feature points are extracted from each frame of the acquired image to obtain pixel coordinate observation values on the image plane. z i k = [ u i k , v i k ] , For image frames Middle The pixel coordinate observation value of the feature point, and Represent the horizontal and vertical coordinates on the image plane respectively;

[0014] Then, the matching points in multiple frames are triangulated to obtain the first The three-dimensional coordinates of the feature points in the world coordinate system , combined with the image frame Posture T c w k = [ R c w k | t c w k ] And the camera intrinsic parameter matrix , the three-dimensional coordinates Projecting back to the image frame , we get the pixel coordinate prediction value: ,in, For image frames Middle The pixel coordinate prediction value of the feature point, is the normalized projection function under the monocular camera model, and are the rotation matrix and position movement respectively;

[0015] According to the difference between the predicted pixel coordinate value and the observed pixel coordinate value, the first The visual residual of feature points is: ; Image frame The visual residuals of all feature points in are concatenated in order to form a visual residual vector: r v k = [ r v , 1 k , r v , 2 k , … , r v , i k ， … , r v , n k ] ,in, For image frames Middle The visual residual of feature points, Represents an image frame The number of feature points observed in For image frames The total residual used for visual optimization is the reprojection residual of the monocular camera. ;

[0016] Inertial measurement unit in two consecutive image frames The time interval between [ t k , t k + 1 ] Internal, output angular velocity and linear acceleration , between image frames, the inertial measurement unit observations are moved along its own time axis Integrate and construct the pre-integrated observation across frames, which is defined as follows: α ̂ k + 1 k = ∫∫ t ∈ [ t k , t k + 1 ] R t k ( a ̂ t − b a t − n a ) d t 2 β ̂ k + 1 k = ∫ t ∈ [ t k , t k + 1 ] R t k ( a ̂ t − b a t − n a ) d t c ̂ k + 1 k = ∫ t ∈ [ t k , t k + 1 ] 1 2 Oh ( oh ̂ t − b oh t − n w ) c t k d t ,

[0017] in, 、 、 Respectively represent the image frames Integrate to image frame Position pre-integration quantity, velocity pre-integration quantity and rotation pre-integration quantity; Indicates the moment The posture is converted to the image frame The rotation matrix of the coordinate system; and Separate moments Accelerometer bias and gyroscope bias; and Respectively represent the Gaussian white noise introduced in the acceleration and angular velocity observations; Indicates that the image frame Points to time The rotation integral of ; It is an operator that maps a three-dimensional angular velocity vector to an antisymmetric matrix;

[0018] Based on the pre-integrated observations and combined with the current estimated state variables, the inertial measurement residual term for sliding window optimization is constructed. The corresponding inertia optimization variables are: ,in, 、 、 Represents image frames Position, speed, and attitude at the moment; 、 Represents an image frame The accelerometer bias and gyroscope bias at the moment, and the inertial measurement residual are defined as follows: r i m u = [ R w k ( p k + 1 w − p k w − v k w D t k + 1 2 g w ( D t k ) 2 ) − α ̂ k + 1 k R w k ( v k + 1 w − v k w + g w D t k ) − β ̂ k + 1 k 2 ⋅ V e c ( ( c ̂ k + 1 k ) − 1 ⊗ ( q k w ) − 1 ⊗ q k + 1 w ) b a k + 1 − b a k b oh k + 1 − b oh k ] ,in, and Represents image frames and The position in the world coordinate system at the corresponding moment; and Represents image frames and The speed in the world coordinate system at the corresponding moment; Represents the gravitational acceleration defined in the world coordinate system; ; Represents the rotation matrix that transforms the world coordinate system to the IMU coordinate system corresponding to the image frame; and Represents image frames and The posture in the world coordinate system at the corresponding moment; Represents the vector form of the extracted rotation error; Represents multiplication operation; , Represented in image frames and The accelerometer bias at the corresponding moment, , Represented in image frames and The gyroscope bias at the corresponding moment.

[0019] The specific process of step 3 of the above-mentioned LiDAR-inertial-vision fusion SLAM switching positioning method applied to degraded environments is as follows:

[0020] First, the reprojection residual and inertial measurement residuals The unified structure is a joint residual function, and the joint state variables are defined in the sliding window, which can be expressed as: ,in, represents the index set of key frames in the sliding window, Represents the index set of three-dimensional feature points observed in the sliding window;

[0021] The joint residual function is defined as follows: f v ( x v ) = [ r v r i m u ] , nonlinear optimization is performed on the joint residual function, and the iterative process is as follows: ,in, is the Jacobian matrix of the joint residual with respect to the joint state variables; it can be further simplified as: ,in, represents the Hessian matrix of the joint residual, is the current residual vector, is the update increment of the joint state variable;

[0022] From Update Increment Extract image frames Corresponding sub-state variable increment , and obtain the pose change in the form of Lie algebra , pose change Get the image frame through index mapping Relative to the image frame The pose increment is expressed as: ,in, represents the lifting operation of Lie algebra from vector to matrix, Represents an image frame Relative to image frames in visual-inertial optimization The pose increment of .

[0023] The specific process of step 4 of the above-mentioned LiDAR-inertial-vision fusion SLAM switching positioning method applied to degraded environments is as follows:

[0024] The lidar outputs the dense spatial point cloud of the current frame at a fixed frequency, denoted as ,in Indicates the The three-dimensional coordinate points of the laser points are classified according to the curvature characteristics of the points, and the dense space point cloud is divided into a set of edge feature points. and the plane feature point set , used to construct subsequent lidar residual terms, ;

[0025] Assume that the three-dimensional coordinates of the laser point used for edge feature matching in the current frame are , the three-dimensional coordinates of the edge feature points matched in the corresponding map are ; Assume that the three-dimensional coordinates of the laser point used for plane feature matching in the current frame are , the coordinates of the plane feature points matched in the corresponding map are ; The residual of constructing edge features is: , the residual of the plane feature is: , the edge residuals and plane residuals corresponding to all feature points in the current frame are summarized to form the residual vector of the lidar: d l = [ d 1 e , … , d m e , d 1 p , … , d n p ] ⊤ , construct the lidar state variable in the current frame and set it to: ,in, Represents the rotation matrix of the current frame lidar, Represents the translation vector of the lidar in the current frame. The lidar state variable is used as input to construct the lidar residual function: , perform nonlinear optimization on the lidar residual function, iterating using the Levenberg–Marquardt algorithm: ,in, is the Jacobian matrix of the residual with respect to the lidar state variables, is the damping coefficient, the process can be simplified as: ,in, represents the Hessian matrix of the lidar residual, is the update increment of the state variable;

[0026] From Update Increment Extract the pose increment in Lie algebra form , which is converted into the pose increment of the current frame relative to the previous frame through exponential mapping, expressed as: ,in, Represents an image frame In the lidar relative to the image frame The pose increment of .

[0027] The specific process of step 5 of the above-mentioned LiDAR-inertial-vision fusion SLAM switching positioning method applied to degraded environments is as follows:

[0028] In the visual-inertial odometry module, when any of the following occurs, including the joint residual Hessian matrix The minimum eigenvalue mutation of the sliding window is used to construct The number of feature points drops sharply, and the accelerometer bias Gyroscope bias Abnormal growth and posture increment A violent fluctuation occurs, which will cause the visual-inertial odometry module status to Determined to be in a failed state;

[0029] For LiDAR degradation detection based on neural networks, we first use the BEV projection method to map the dense spatial point cloud onto the ground plane and construct a structured two-dimensional grid representation. Specifically, a fixed spatial projection area is defined in the LiDAR coordinate system: x ∈ [ x m i n , x m a x ] , y ∈ [ y m i n , y m a x ] , the spatial projection area is divided into grid cells, each grid cell covers an area of: , for each three-dimensional coordinate point , calculate its grid projection index in the BEV map as: ,in Indicates the grid projection index of the three-dimensional coordinate point in the BEV map, in each grid cell In the 3D coordinate system, three local features are counted for all the points falling into the area: number of points, mean height, and mean reflection intensity.

[0030] Combine the above three local features to form a tensor ; The image frame The tensor is denoted as , used to represent image frames The BEV encoding input is used as the input of the neural network, and the neural network outputs a probability value between 0 and 1, which indicates the confidence that the current frame is in a degraded state.

[0031] The specific process of step 6 of the above-mentioned LiDAR-inertial-vision fusion SLAM switching positioning method applied to degraded environments is as follows:

[0032] First, let a length be Status buffer queue , used to record the degradation confidence of the neural network output of the past 5 frames: Q s = [p k − 4 , p k − 3 , p k − 2 , p k − 1 , p k ] ,in, p k ∈ [ 0 , 1 ] For image frames Degradation confidence, set a fixed threshold , as the judgment image frame The demarcation criteria for being in a degraded state;

[0033] When the visual-inertial odometry module fails or the degradation confidence meets And there are no less than 4 frames in the buffer queue that meet , then the current lidar status is judged to be good, and the lidar pose increment is used as the initial pose estimate ,in, represents the mapping operation between Lie groups and Lie algebras, Represents an image frame 's posture;

[0034] If the visual-inertial odometry module works properly but has degraded confidence , and there are at least 3 frames in the buffer queue that meet , it is judged that it is at the beginning or end of the laser radar degradation stage, and the initial pose estimate is constructed by linear interpolation of the laser radar pose increment and the visual-inertial pose increment. ,in, For the current buffer queue The confidence mean in ;

[0035] If the current state does not belong to the above two cases, the visual-inertial pose increment is directly used as the initial pose estimate. ;

[0036] After completing the construction of the initial pose estimate, The Scan-to-Map optimization is performed in the starting state. The specific optimization strategy is switched according to the state combination of the LiDAR and the visual-inertial odometry module. The optimization process is as follows:

[0037] If the current frame degradation confidence satisfies , and there are at least 4 frames in the buffer queue that meet , judge that the current lidar is in a non-degenerate state. At this time, the lidar residual is used to build an optimization model, and the state update is performed on all degrees of freedom. The state increment ;

[0038] If the visual-inertial odometry module works normally and the current frame degradation confidence satisfies , and there are at least 3 frames in the buffer queue that meet , it is judged that the laser radar is in a slightly degraded state. At this time, the pseudo residual term constructed by visual pose propagation is introduced, and the optimization target is constructed together with the laser pose residual. The solution of its state increment is: , l v o ∈ ( 0 , 1 ] is the weighted coefficient of the visual prior residual term;

[0039] Then, the state increment calculated by the above optimization process is used , update the current pose status: ;

[0040] If the visual-inertial odometry module fails and the current frame degradation confidence satisfies , and all 5 frames in the buffer queue meet , it is judged that the lidar is in a completely degraded state. At this time, the optimization process of the current frame is skipped and the initial estimate is directly retained as the final state, which is expressed as follows: .

[0041] The present invention addresses the problems of traditional visual SLAM methods in degraded environments, such as untimely sensor state recognition, fusion methods relying on heuristic parameter adjustment, and overall positioning failure caused by degradation error diffusion. The proposed solution has the following technical effects:

[0042] First, by introducing a degradation detection model based on BEV projection point cloud encoding and lightweight neural network construction, the degradation confidence of the lidar can be output in real time, effectively avoiding the false detection and missed detection problems caused by the traditional method of relying on fixed thresholds, and improving the robustness and environmental adaptability of degradation recognition.

[0043] Secondly, the state buffer mechanism is used to smoothly determine the degradation state of consecutive frames, avoiding system oscillations caused by frequent switching of initial pose estimation, ensuring the continuity and stability of estimation during state switching, which is particularly suitable for scenarios where the state hovers near the critical value.

[0044] Finally, an initial pose selection strategy based on multi-source information state determination was constructed. This strategy adaptively selects the visual-inertial pose, the lidar pose, or a linear interpolation of the two as the starting estimate for Scan-to-Map optimization, based on the visual-inertial odometry module state and lidar degradation confidence in the current frame. This enhances the estimation process's tolerance to degradation interference. In scenarios with mild lidar degradation, a joint visual-inertial residual collaborative constraint was introduced to effectively improve the accuracy of the optimized solution. In fully degraded scenarios, the initial value was retained and optimization was skipped, reducing the risk of further propagation of erroneous information.

[0045] The present invention can effectively address the positioning failure problem of lidar and visual-inertial odometry modules in degraded environments, improve the overall SLAM positioning accuracy and robustness, and is suitable for long-term stable operation requirements under harsh conditions such as single structure, complex lighting, and sparse texture. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is a flow chart of the LiDAR-inertial-vision fusion SLAM sensor state switching method in a degraded environment. DETAILED DESCRIPTION

[0047] A LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment includes the following steps:

[0048] Step 1: A multi-sensor system consisting of a monocular camera, an inertial measurement unit (IMU, including a gyroscope and accelerometer), and a laser radar (LiDAR) mounted on the mobile robot acquires raw measurement data, including images, angular velocity and linear acceleration, and dense spatial point clouds. The monocular camera is mounted on the front of the mobile robot to acquire images; the IMU is mounted at the center of mass of the mobile robot to measure its angular velocity and linear acceleration in three-dimensional space; and the LiDAR is mounted on top of the mobile robot to acquire a dense spatial point cloud of the current environment.

[0049] Step 2: Calculate the reprojection residual of the monocular camera Inertial measurement residuals with IMU .

[0050] For a monocular camera, feature points are extracted from each frame of the acquired image to obtain pixel coordinate observation values on the image plane, which are recorded as:

[0051] z i k = [ u i k , v i k ] (1),

[0052] in, For image frames Middle The pixel coordinate observation value of the feature point, and Represent the horizontal and vertical coordinates on the image plane respectively.

[0053] Then, by triangulating the matching points in multiple frames of images, the three-dimensional coordinates of the feature points in the world coordinate system are obtained. . Combined image frames Posture T c w k = [ R c w k | t c w k ] And the camera intrinsic parameter matrix , the three-dimensional coordinates Projecting back to the image frame , we get the pixel coordinate prediction value:

[0054] (2),

[0055] in, For image frames Middle The pixel coordinate prediction value of the feature point, is the normalized projection function under the monocular camera model, and They are the rotation matrix and position movement, respectively, describing the pose transformation from the world coordinate system to the camera coordinate system.

[0056] According to the difference between the predicted pixel coordinate value and the observed pixel coordinate value, the first The visual residual of feature points is:

[0057] (3),

[0058] The current image frame The visual residuals of all feature points in are concatenated in order to form a visual residual vector:

[0059] r v k = [ r v , 1 k , r v , 2 k , … , r v , i k ， … , r v , n k ] (4),

[0060] in, For image frames Middle The visual residual of feature points, Represents an image frame The number of feature points observed in For image frames The total residual used for visual optimization is the reprojection residual of the monocular camera. .

[0061] Inertial measurement unit (IMU) in two consecutive image frames The time interval between [ t k , t k + 1 ] Output angular velocity at high frequency and linear acceleration , given the low frame rate of the monocular camera, in order to achieve state propagation between image frames, the high-frequency observation values of the IMU are propagated along the time axis of the IMU itself between image frames. Integrate and construct the pre-integrated observation across frames, which is defined as follows:

[0062] α ̂ k + 1 k = ∫∫ t ∈ [ t k , t k + 1 ] R t k ( a ̂ t − b a t − n a ) d t 2 β ̂ k + 1 k = ∫ t ∈ [ t k , t k + 1 ] R t k ( a ̂ t − b a t − n a ) d t c ̂ k + 1 k = ∫ t ∈ [ t k , t k + 1 ] 1 2 Oh ( oh ̂ t − b oh t − n w ) c t k d t (5),

[0063] in, 、 、 Respectively represent the image frames Integrate to image frame Position pre-integration quantity, velocity pre-integration quantity and rotation pre-integration quantity; Indicates the moment The posture is converted to the image frame The rotation matrix of the coordinate system; and Separate moments Accelerometer bias and gyroscope bias; and Respectively represent the Gaussian white noise introduced in the acceleration and angular velocity observations; Indicates that the image frame Points to time The rotation integral of ; It is an operator that maps a three-dimensional angular velocity vector to an antisymmetric matrix.

[0064] Based on the pre-integrated observations and the current estimated state variables, the inertial measurement residual term for sliding window optimization is constructed. The corresponding inertia optimization variables are: (6),

[0065] in, 、 、 Represents image frames Position, speed, and attitude at the moment; 、 Represents an image frame The accelerometer bias and gyroscope bias at that moment. During the pre-integration process, it is assumed that the bias is constant within the integration interval.

[0066] The inertial measurement residual of the IMU is defined as follows:

[0067] r i m u = [ R w k ( p k + 1 w − p k w − v k w D t k + 1 2 g w ( D t k ) 2 ) − α ̂ k + 1 k R w k ( v k + 1 w − v k w + g w D t k ) − β ̂ k + 1 k 2 ⋅ V e c ( ( c ̂ k + 1 k ) − 1 ⊗ ( q k w ) − 1 ⊗ q k + 1 w ) b a k + 1 − b a k b oh k + 1 − b oh k ] (7),

[0068] in, and Represents image frames and The position in the world coordinate system at the corresponding moment; and Represents image frames and The speed in the world coordinate system at the corresponding moment; Represents the gravitational acceleration defined in the world coordinate system; ; Represents the rotation matrix that transforms the world coordinate system to the IMU coordinate system corresponding to the image frame; and Represents image frames and The posture in the world coordinate system at the corresponding moment; Represents the vector form of the extracted rotation error; Represents multiplication operation; , Represented in image frames and The accelerometer bias at the corresponding moment, , Represented in image frames and The gyroscope bias at the corresponding moment.

[0069] Step 3: Reprojection residual of the monocular camera constructed in step 2 Inertial measurement residuals with IMU , further construct a joint optimization problem, and obtain the current frame pose increment output by the visual-inertial odometry module through nonlinear optimization calculation.

[0070] First, the reprojection residual of the monocular camera Inertial measurement residuals with IMU The unified structure is a joint residual function. The joint state variables are defined in the sliding window and expressed as:

[0071] (8),

[0072] in, represents the index set of key frames in the sliding window, Represents the index set of three-dimensional feature points observed in the sliding window; Indicates the The three-dimensional coordinates of the feature points in the world coordinate system.

[0073] The joint residual function is defined as follows:

[0074] f v ( x v ) = [ r v r i m u ] (9),

[0075] Nonlinear optimization is performed on the joint residual function, and the iterative process is as follows:

[0076] (10),

[0077] in, is the Jacobian matrix of the joint residual with respect to the joint state variables.

[0078] It can be further simplified as:

[0079] (11),

[0080] in, represents the joint residual Hessian matrix, is the joint residual vector, Update increment for the joint state variable.

[0081] From Update Increment Extract image frames Corresponding sub-state variable increment , and obtain the pose change in the form of Lie algebra . Pose change Get the image frame through index mapping Relative to the image frame The pose increment is expressed as:

[0082] (12),

[0083] in, represents the lifting operation of Lie algebra from vector to matrix, Represents an image frame Relative to image frames in visual-inertial optimization This pose increment will be used as a candidate initial value for the subsequent visual-inertial odometry module state estimation. Whether to use this initial value will be dynamically selected based on the results of lidar degradation detection and visual-inertial odometry module fault detection.

[0084] Step 4: Based on the dense spatial point cloud output by the lidar in step 1, perform geometric feature extraction and perform a scan-to-map residual minimization to obtain the pose increment of the lidar in the current frame in the world coordinate system.

[0085] The lidar outputs the dense spatial point cloud of the current frame at a fixed frequency, denoted as ,in Indicates the The three-dimensional coordinates of the laser points reflect the intersection of the laser beam and the environmental objects. Further, the dense space point cloud is classified according to the curvature characteristics of the points, and the dense space point cloud is divided into a set of edge feature points. and the plane feature point set , used to construct subsequent lidar residual terms; .

[0086] Assume that the three-dimensional coordinates of the laser point used for edge feature matching in the current frame are , the three-dimensional coordinates of the edge feature points matched in the corresponding map are ; Assume that the three-dimensional coordinates of the laser point used for plane feature matching in the current frame are , the coordinates of the plane feature points matched in the corresponding map are .

[0087] First, the residual of the edge feature is constructed as:

[0088] (13),

[0089] The residual of the planar feature is:

[0090] (14),

[0091] The edge residuals and plane residuals corresponding to all feature points in the current frame are summarized separately to form the residual vector of the lidar:

[0092] d l = [ d 1 e , … , d m e , d 1 p , … , d n p ] ⊤ (15),

[0093] Construct the lidar state variable in the current frame and set it to:

[0094] (16),

[0095] in, Represents the rotation matrix of the current frame lidar, Represents the translation vector of the lidar in the current frame. Using the lidar state variable as input, construct the lidar residual function:

[0096] (17),

[0097] A nonlinear optimization is performed on the lidar residual function using the Levenberg–Marquardt algorithm for iteration:

[0098] (18),

[0099] in, is the Jacobian matrix of the residual with respect to the lidar state variables, is the damping coefficient. The process can be simplified as:

[0100] (19),

[0101] in, represents the Hessian matrix of the lidar residual, The update increment of the state variable.

[0102] Further, from the update increment Extract the pose increment in Lie algebra form , which is converted into the pose increment of the current frame relative to the previous frame through exponential mapping, expressed as:

[0103] (20),

[0104] in, Indicates that the laser radar is in the image frame Relative to the image frame The pose increment of is its minimum representation under Lie algebra. This pose increment will be used for degradation judgment and multimodal pose fusion in subsequent steps.

[0105] Step 5: To ensure the system maintains stable positioning capabilities in harsh environments, this step evaluates the operating status of the visual-inertial odometry module and lidar in the current frame based on the visual-inertial optimization results from Step 3 and the dense spatial point cloud of the current frame acquired by the lidar in Step 1. This step determines whether there are any abnormal observation information or sudden changes in estimation results. This step performs fault detection on the visual-inertial odometry module and degradation detection on the lidar, respectively. The test results serve as the basis for determining whether to use visual-inertial or lidar in subsequent estimation processes and are used to determine the data source for the initial pose estimation of the current frame.

[0106] In the visual-inertial odometry module, the minimum eigenvalue of the Hessian matrix, the number of feature points, the bias change, and the pose increment are used as the judgment indicators. The status of the visual-inertial odometry module in the current frame is expressed as When any of the following occurs, including the joint residual Hessian matrix The minimum eigenvalue mutation of the sliding window is used to construct The number of feature points drops sharply, and the accelerometer bias Gyroscope bias Abnormal growth and posture increment There is a severe fluctuation, the system is about to It is judged as a failure state and is set to "fail".

[0107] At the same time, for LiDAR, degradation detection is performed based on the following neural network. First, the input features of the neural network are as follows:

[0108] To adapt the sparse and irregular LiDAR dense spatial point cloud to the input requirements of the convolutional neural network, the BEV (Bird's-Eye View) projection method is used to map the dense spatial point cloud onto the ground plane (XY plane) and construct a structured two-dimensional grid image representation. Specifically, a fixed spatial projection area is defined in the LiDAR coordinate system:

[0109] x ∈ [ x m i n , x m a x ] , y ∈ [ y m i n , y m a x ] (twenty one),

[0110] To accommodate the typical sensing range of most outdoor mobile platforms, the boundary parameters are set: That is, with the laser radar as the center, a square area covering 15 meters in front, behind, left and right is constructed, with a total range of rice.

[0111] The spatial projection area is divided into grid cells, each grid cell covers an area of: .

[0112] For dense space point clouds The three-dimensional coordinates of the laser point , calculate its grid projection index in the BEV map as:

[0113] (twenty two),

[0114] in Indicates the grid projection index of the 3D coordinate point in the BEV map.

[0115] In each grid cell In the algorithm, three local features are counted for all three-dimensional coordinate points falling into the area: the number of points, which is used to measure the spatial density in the area; the mean height, which reflects the average height of the points in the area and captures the terrain undulation and geometric structure; and the mean reflection intensity, which represents the change of material or object type.

[0116] Combine the above three local features to form a tensor ; The image frame The tensor is denoted as , used to represent image frames The BEV encoding input is used as the input to the neural network. The three local features correspond to three feature maps. The values of each local feature are then normalized so that their range is mapped to the interval [0, 1].

[0117] The architecture of the neural network is as follows:

[0118] The purpose of this degradation detection neural network is to use the BEV (bird's eye view) LiDAR input to make a real-time judgment on whether the current frame is in a degradation state. The input of the neural network is a The tensor corresponds to three types of local features for each grid: point density, height mean, and reflection intensity. By dividing the ground area into regular areas and normalizing them, the BEV representation can convert sparse point clouds into structured tensors without losing their geometric distribution, making them easier for neural network processing.

[0119] The neural network consists of two convolutional blocks, each containing a Convolutional layer, batch normalization layer, ReLU activation function and The first convolution block increases the number of channels from 3 to 16 and the spatial resolution from Downsample to ; The second convolution block further increases the number of channels to 32 and reduces the resolution to .

[0120] After convolutional feature extraction, the neural network compresses the spatial dimensions into a 32-dimensional global vector through a global average pooling operation. This global vector is fed into a fully connected layer containing a single neuron, followed by a sigmoid activation function, which outputs a probability value between 0 and 1, indicating the confidence that the current frame is in a degraded state. This probability can be further compared with a threshold to drive the system to enable visual-inertial surrogate estimation or other redundancy mechanisms.

[0121] The training algorithm is as follows:

[0122] The degradation detection neural network is trained end-to-end using supervised learning. Given a set of lidar point cloud tensors encoded with bird's eye view (BEV) , and the corresponding degenerate labels , the degradation probability of the neural network output The error between the true label and the target is optimized by the binary cross entropy loss function. The loss function is defined as follows:

[0123] (twenty three),

[0124] This loss function has a smooth gradient guide for the neural network output close to the target probability value, which is conducive to stabilizing the training process. During training, iterative optimization is performed in small batches. Small batches include samples, namely: (24), among which, Indicates the Small batches; Indicates the number of samples; Indicates the BEV tensor input of samples; Indicates the corresponding degenerate label.

[0125] The total loss of each mini-batch is the average of the BCE losses within the batch, plus a regularization term to suppress overfitting:

[0126] (25),

[0127] in, Indicates the The total loss function value of a small batch; Indicates the BCE loss value of samples; represents all trainable parameters in the neural network, is the regularization coefficient.

[0128] The model training uses the Adam optimizer, fixes the initial learning rate, and monitors the performance indicators on the validation set during training to avoid overfitting. After the network converges, its output It can be used as an estimate of the probability of degradation.

[0129] In actual deployment, a fixed threshold strategy is adopted: When the current frame is in a degraded state, the redundancy mechanism of the SLAM system is triggered.

[0130] In addition, considering that the degradation index is close to the threshold in actual application, it is easy to cause switching instability, so a state buffer mechanism is introduced. The state buffer mechanism maintains a historical state queue with a preset length. , the current state is judged into three categories: "normal", "degradation start / end", and "complete degradation", to prevent the system from oscillating due to frequent switching of the estimated initial value.

[0131] Step 6: Initial pose estimation and Scan-to-Map optimization. This step is based on the lidar degradation detection results and the visual-inertial odometer module fault detection results, combined with the state pose of the previous moment. , choose a suitable initial pose estimate , and use this as the initial value to perform Scan-to-Map optimization, and finally update the image frame at the current moment The state pose of The specific process is as follows:

[0132] First, protect a length of Status buffer queue , used to record the degradation confidence of the neural network output for the past 5 frames:

[0133] Q s = [p k − 4 , p k − 3 , p k − 2 , p k − 1 , p k ] (26),

[0134] in, p k ∈ [ 0 , 1 ] For image frames Degradation confidence of . Set a fixed threshold , as the judgment image frame The demarcation criteria for a degraded state.

[0135] When the visual-inertial odometry module fails (i.e. ), or image frame Degradation confidence meets And there are no less than 4 frames in the buffer queue that meet , then the current lidar status is judged to be good, and the lidar pose increment is used as the initial pose estimate, which is defined as follows:

[0136] (27),

[0137] in, Represents the mapping operation between Lie groups and Lie algebras.

[0138] If the visual-inertial odometry module works properly ( ), but there is a current frame confidence , and there are at least 3 frames in the buffer queue that meet , it is judged to be in the "start" or "end" state of lidar degradation. At this time, the initial pose estimate is constructed by linear interpolation of the lidar pose increment and the visual-inertial pose increment, which is expressed as follows:

[0139] (28),

[0140] in, For the current buffer queue The mean of the degradation confidence in .

[0141] If the current state does not fall into the above two situations, the visual-inertial pose increment is directly used as the initial pose estimate, which is defined as follows:

[0142] (29),

[0143] After completing the construction of the initial pose estimate, The Scan-to-Map optimization is performed as the starting state. The specific optimization strategy is switched according to the state combination of the LiDAR and Visual-Inertial Odometry modules. The optimization process is as follows:

[0144] If the current frame confidence satisfies , and there are at least 4 frames in the buffer queue that meet , judge that the current lidar is in a non-degenerate state. At this time, the lidar residual is used to build an optimization model, and the state update is performed on all degrees of freedom. The state increment .

[0145] If the visual-inertial odometry module works normally and the current frame confidence satisfies , and there are at least 3 frames in the buffer queue that meet , the laser radar is judged to be in a slightly degraded state. At this time, the pseudo residual term constructed by visual pose propagation is introduced and combined with the laser pose residual to jointly construct the optimization target. The solution for its state increment is:

[0146] (30),

[0147] in, and Represent the update increments of lidar and visual-inertial respectively, l v o ∈ ( 0 , 1 ] is the weighted coefficient of the visual prior residual term.

[0148] Then, the state increment calculated by the above optimization process is used , update the current state pose:

[0149] (31);

[0150] If the visual-inertial odometry module fails and the current frame confidence meets , and all 5 frames in the buffer queue meet , it is judged that the LiDAR is in a completely degraded state. At this time, the optimization process of the current frame is skipped and the initial pose estimate is directly retained as the final state pose, which is expressed as follows:

[0151] (32).

[0152] Step 7: The current frame state pose calculated in step 6 As the initial estimate of the newly added image frame, it is added to the factor graph containing the poses of all historical image frames. This factor graph combines the lidar residual, the visual-inertial joint residual, and the initial pose estimate. , and in this step, loop closure factors are constructed through loop closure detection as observation constraints on the edges in the graph. Then, incremental optimization is performed on the factor graph based on the iSAM2 (Incremental Smoothing and Mapping) algorithm to output the final pose estimation result that is globally consistent. , used for subsequent navigation control and high-precision map construction.

Claims

1. A LiDAR-inertial-vision fusion SLAM switching positioning method applied to degraded environments, characterized by: The following steps are involved: Step 1: Use the monocular camera, inertial measurement unit, and lidar installed on the mobile robot to obtain images, angular velocity and linear acceleration, and dense spatial point clouds respectively; Step 2: Construct the reprojection residual of the monocular camera based on the acquired image , construct the inertial measurement residual of the inertial measurement unit based on the obtained angular velocity and linear acceleration ; Step 3: Based on the reprojection residual constructed in step 2 and inertial measurement residuals , further joint optimization is performed, and the current frame pose increment is output after nonlinear optimization through the visual-inertial odometry module; Step 4: Based on the dense spatial point cloud output by the lidar in step 1, calculate the current frame pose increment of the lidar in the world coordinate system; Step 5: Based on the visual-inertial optimization results from step 3 and the dense spatial point cloud acquired by the lidar in step 1, the working status of the visual-inertial odometry module and lidar in the current frame are evaluated. Fault detection of the visual-inertial odometry module and degradation detection of the lidar are performed respectively. Step 6: Based on the degradation detection results of the lidar and the fault detection results of the visual-inertial odometry module, combined with the state posture at the previous moment , select the initial pose estimate , and use this as the initial value to perform Scan-to-Map optimization, and finally update the image frame at the current moment The state pose of ; Step 7: The image frame calculated in step 6 The state pose of As the initial estimate of the newly added image frame, it is added to the factor graph containing the poses of all historical image frames. The factor graph combines the lidar residual, the visual-inertial joint residual, and the initial pose estimate. And construct a closed-loop factor through loop closure detection as the observation constraint of the edge in the graph, and then perform incremental optimization on the factor graph to output the final pose estimation result that is globally consistent .

2. The LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment according to claim 1, characterized in that: The specific process of step 2 is: For a monocular camera, feature points are extracted from each frame of the acquired image to obtain pixel coordinate observation values on the image plane. , For image frames Middle The pixel coordinate observation value of the feature point, and Represent the horizontal and vertical coordinates on the image plane respectively; Then, the matching points in multiple frames are triangulated to obtain the first The three-dimensional coordinates of the feature points in the world coordinate system , combined with the image frame Posture And the camera intrinsic parameter matrix , the three-dimensional coordinates Projecting back to the image frame , we get the pixel coordinate prediction value: ,in, For image frames Middle The pixel coordinate prediction value of the feature point, is the normalized projection function under the monocular camera model, and are the rotation matrix and position movement respectively; According to the difference between the predicted pixel coordinate value and the observed pixel coordinate value, the first The visual residual of feature points is: ; Image frame The visual residuals of all feature points in are concatenated in order to form a visual residual vector: ,in, For image frames Middle The visual residual of feature points, Represents an image frame The number of feature points observed in For image frames The total residual used for visual optimization is the reprojection residual of the monocular camera. ; Inertial measurement unit in two consecutive image frames The time interval between Internal, output angular velocity and linear acceleration , between image frames, the inertial measurement unit observations are moved along its own time axis Integrate and construct the pre-integrated observation across frames, which is defined as follows: ,in, 、 、 Respectively represent the image frames Integrate to image frame Position pre-integration quantity, velocity pre-integration quantity and rotation pre-integration quantity; Indicates the moment The posture is converted to the image frame The rotation matrix of the coordinate system; and Separate moments Accelerometer bias and gyroscope bias; and Respectively represent the Gaussian white noise introduced in the acceleration and angular velocity observations; Indicates that the image frame Points to time The rotation integral of ; It is an operator that maps a three-dimensional angular velocity vector to an antisymmetric matrix; Based on the pre-integrated observations and combined with the current estimated state variables, the inertial measurement residual term for sliding window optimization is constructed. The corresponding inertia optimization variables are: ,in, 、 、 Represents image frames Position, speed, and attitude at the moment; 、 Represents an image frame The accelerometer bias and gyroscope bias at the moment, and the inertial measurement residual are defined as follows: ,in, and Represents image frames and The position in the world coordinate system at the corresponding moment; and Represents image frames and The speed in the world coordinate system at the corresponding moment; Represents the gravitational acceleration defined in the world coordinate system; ; Represents the rotation matrix that transforms the world coordinate system to the IMU coordinate system corresponding to the image frame; and Represents image frames and The posture in the world coordinate system at the corresponding moment; Represents the vector form of the extracted rotation error; Represents multiplication operation; , Represented in the image frame and The accelerometer bias at the corresponding moment, , Represented in image frames and The gyroscope bias at the corresponding moment.

3. The LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment according to claim 2, characterized in that: The specific process of step three is: First, the reprojection residual and inertial measurement residuals The unified structure is a joint residual function, and the joint state variables are defined in the sliding window, which can be expressed as: ,in, represents the index set of key frames in the sliding window, Represents the index set of three-dimensional feature points observed in the sliding window; The joint residual function is defined as follows: , nonlinear optimization is performed on the joint residual function, and the iterative process is as follows: ,in, is the Jacobian matrix of the joint residual with respect to the joint state variables; further simplified as: ,in, represents the Hessian matrix of the joint residual, is the current residual vector, is the update increment of the joint state variable; From Update Increment Extract image frames Corresponding sub-state variable increment , and obtain the pose change in the form of Lie algebra , pose change Get the image frame through index mapping Relative to the image frame The pose increment is expressed as: ,in, represents the lifting operation of Lie algebra from vector to matrix, Represents an image frame Relative to image frames in visual-inertial optimization The pose increment of .

4. The LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment according to claim 3, characterized in that: The specific process of step four is: The lidar outputs the dense spatial point cloud of the current frame at a fixed frequency, denoted as ,in Indicates the The three-dimensional coordinate points of the laser points are classified according to the curvature characteristics of the points, and the dense space point cloud is divided into a set of edge feature points. and the plane feature point set , used to construct subsequent lidar residual terms, ; Assume that the three-dimensional coordinates of the laser point used for edge feature matching in the current frame are , the three-dimensional coordinates of the edge feature points matched in the corresponding map are ; Assume that the three-dimensional coordinates of the laser point used for plane feature matching in the current frame are , the coordinates of the plane feature points matched in the corresponding map are ; The residual of constructing edge features is: , the residual of the plane feature is: , the edge residuals and plane residuals corresponding to all feature points in the current frame are summarized to form the residual vector of the lidar: , construct the lidar state variable in the current frame and set it to: ,in, Represents the rotation matrix of the current frame lidar, Represents the translation vector of the lidar in the current frame. The lidar state variable is used as input to construct the lidar residual function: , perform nonlinear optimization on the lidar residual function, iterating using the Levenberg–Marquardt algorithm: ,in, is the Jacobian matrix of the residual with respect to the lidar state variables, is the damping coefficient, the process is simplified to: ,in, represents the Hessian matrix of the lidar residual, is the update increment of the state variable; From Update Increment Extract the pose increment in Lie algebra form , which is converted into the pose increment of the current frame relative to the previous frame through exponential mapping, expressed as: ,in, Represents an image frame In the lidar relative to the image frame The pose increment of .

5. The LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment according to claim 4, characterized in that: The specific process of step five is: In the visual-inertial odometry module, when any of the following occurs, including the joint residual Hessian matrix The minimum eigenvalue mutation of the sliding window is used to construct The number of feature points drops sharply, and the accelerometer bias Gyroscope bias Abnormal growth and posture increment A violent fluctuation occurs, which will cause the visual-inertial odometry module status to Determined to be in a failed state; For LiDAR degradation detection based on neural networks, we first use the BEV projection method to map the dense spatial point cloud onto the ground plane and construct a structured two-dimensional grid representation. Specifically, a fixed spatial projection area is defined in the LiDAR coordinate system: , the spatial projection area is divided into grid cells, each grid cell covers an area of: , for each three-dimensional coordinate point , calculate its grid projection index in the BEV map as: ,in Indicates the grid projection index of the three-dimensional coordinate point in the BEV map, in each grid cell In the 3D coordinate system, three local features are counted for all the points falling into the area: number of points, mean height, and mean reflection intensity. Combine the above three local features to form a tensor ; The image frame The tensor is denoted as , used to represent image frames The BEV encoding input is used as the input of the neural network, and the neural network outputs a probability value between 0 and 1, which indicates the confidence that the current frame is in a degraded state.

6. The LiDAR-inertial-vision fusion SLAM switching positioning method applied to a degraded environment according to claim 5, characterized in that: The specific process of step six is: First, let a length be Status buffer queue , used to record the degradation confidence of the neural network output of the past 5 frames: ,in, For image frames Degradation confidence, set a fixed threshold , as the judgment image frame The demarcation criteria for being in a degraded state; When the visual-inertial odometry module fails or the degradation confidence meets And there are no less than 4 frames in the buffer queue that meet , then the current lidar status is judged to be good, and the lidar pose increment is used as the initial pose estimate ,in, represents the mapping operation between Lie groups and Lie algebras, Represents an image frame 's posture; If the visual-inertial odometry module works properly but has degraded confidence , and there are at least 3 frames in the buffer queue that meet , it is judged that it is at the beginning or end of the laser radar degradation stage, and the initial pose estimate is constructed by linear interpolation of the laser radar pose increment and the visual-inertial pose increment. ,in, For the current buffer queue The confidence mean in ; If the current state does not belong to the above two cases, the visual-inertial pose increment is directly used as the initial pose estimate. ; After completing the construction of the initial pose estimate, The Scan-to-Map optimization is performed in the starting state. The specific optimization strategy is switched according to the state combination of the LiDAR and the visual-inertial odometry module. The optimization process is as follows: If the current frame degradation confidence satisfies , and there are at least 4 frames in the buffer queue that meet , judge that the current lidar is in a non-degenerate state. At this time, the lidar residual is used to build an optimization model, and the state update is performed on all degrees of freedom. The state increment ; If the visual-inertial odometry module works normally and the current frame degradation confidence satisfies , and there are at least 3 frames in the buffer queue that meet , it is judged that the laser radar is in a slightly degraded state. At this time, the pseudo residual term constructed by visual pose propagation is introduced, and the optimization target is constructed together with the laser pose residual. The solution of its state increment is: , is the weighted coefficient of the visual prior residual term; Then, the state increment calculated by the above optimization process is used , update the current pose status: ; If the visual-inertial odometry module fails and the current frame degradation confidence satisfies , and all 5 frames in the buffer queue meet , it is judged that the lidar is in a completely degraded state. At this time, the optimization process of the current frame is skipped and the initial estimate is directly retained as the final state, which is expressed as follows: .

Images