Unmanned aerial vehicle underground space multi-modal robust positioning method based on confidence factor

Abstract

The invention discloses an unmanned aerial vehicle underground space multi-modal robust positioning method based on a confidence factor, and the method comprises a front-end stage and a rear-end stage, in the front-end stage, the measurement data of a camera, a laser radar and an I MU are preprocessed, and point-line features are extracted for the self-adaptive vision / inertia / laser radar initialization; in the rear-end stage, an I MU measurement residual error, a laser radar edge-plane residual error, a visual point-line residual error and a Manhattan structure constraint residual error are constructed; a self-adaptive confidence factor is designed, and the degradation degree of the camera and the laser radar is evaluated through the visual feature tracking times and the difference between the reference features in the laser radar point cloud and the transformed features; according to the weight of each sensor, an asymptotic non-convexity factor graph optimization function is constructed, dynamic adjustment of pose optimization is realized according to the degradation degree weight of each sensor, a positioning result is obtained, and the positioning precision and robustness of the underground space unmanned aerial vehicle are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of computer vision and multi-sensor fusion, and in particular to the technology of visual-inertial-lidar fusion, and mainly relates to a multi-modal robust positioning method for underground space of unmanned aerial vehicles based on confidence factors. Background Art

[0002] With the development of simultaneous localization and mapping (SLAM) technology, multi-sensor fusion unmanned systems have become one of the most active research areas in recent years. For IMU, it provides high-frequency measurements and is not affected by external environments such as lighting, texture, and weather, but it has time-varying bias, which requires other sensors to provide effective constraints to reduce measurement errors. For the camera, it perceives rich texture information in the environment, but the vision-based SLAM system cannot successfully track visual features in areas with weak textures or poor lighting conditions, resulting in system degradation or even failure. For LiDAR, it can provide accurate depth measurement and structural information in a perceptual sense, and is not easily affected by lighting, but its point cloud has a low resolution. In structural degradation scenarios, performance will degrade due to insufficient constraints. Therefore, in recent years, many researchers have begun to pay attention to visual-inertial-LiDAR fusion positioning systems.

[0003] Existing methods only fuse these three sensors without considering extreme conditions such as underground space. The main challenges encountered in this case include: 1) The lighting conditions in underground space are poor and the texture features are weak. The features extracted by the camera are insufficient, making the visual sensor constraints unreliable in the fusion process; 2) Due to the complexity of underground space, periodic outliers will appear in sensor measurements. The existing fusion framework lacks consideration of the degree of degradation of different sensors, resulting in low robustness of back-end optimization; 3) In the process of posture graph optimization, the existing methods lack quantitative evaluation of degraded scenes and cannot adaptively allocate the weights of different factors in the visual-inertial-lidar fusion framework. Summary of the invention

[0004] The present invention aims at the problems existing in the prior art and proposes a multimodal robust positioning method for unmanned aerial vehicle underground space based on confidence factors, which includes two stages, namely, front-end and back-end. In the front-end stage, the measurement data of the camera, lidar and IMU are preprocessed, and point and line features are extracted for adaptive visual / inertial / lidar initialization; in the back-end stage, IMU measurement residuals, lidar edge-plane residuals, visual point-line residuals and Manhattan structure constraint residuals are constructed according to the features extracted and processed in the front-end stage; an adaptive confidence factor is designed to evaluate the degree of degradation of the camera and lidar through the number of visual feature tracking and the difference between the reference feature and the transformed feature in the lidar point cloud, wherein the adaptive confidence factor includes at least a visual confidence factor and a lidar confidence factor; according to the weight of each sensor, an asymptotically non-convex factor graph optimization function is constructed, and dynamic adjustment of posture optimization is achieved according to the degradation degree weight of each sensor to obtain a positioning result, thereby improving the positioning accuracy and robustness of the unmanned aerial vehicle in the underground space.

[0005] In order to achieve the above purpose, the technical solution adopted by the present invention is: a multi-modal robust positioning method for underground space of unmanned aerial vehicles based on confidence factors, including two stages of front-end and back-end,

[0006] In the front-end stage: pre-processing the measurement data of the camera, lidar and IMU, extracting point and line features for adaptive vision / inertial / lidar initialization;

[0007] In the back-end stage: IMU measurement residuals, laser radar edge-plane residuals, visual point-line residuals and Manhattan structure constraint residuals are constructed according to the features extracted and processed in the front-end stage; then an adaptive confidence factor is constructed, and the degree of degradation of the camera and laser radar is evaluated by the number of visual feature tracking and the difference between the reference feature and the transformed feature in the laser radar point cloud. The adaptive confidence factor includes at least a visual confidence factor and a laser radar confidence factor; according to the weight of each sensor, an asymptotically non-convex factor graph optimization function is constructed, and dynamic adjustment of posture optimization is achieved according to the degradation degree weight of each sensor to obtain a positioning result; wherein,

[0008] The laser radar edge-plane residual is used to minimize the geometric error between the feature points and the target points in the point cloud;

[0009] The visual point-line residual: utilizes the geometric relationship between the point features and the line features in the image to constrain the camera's position and posture;

[0010] The Manhattan structure constrained residual: uses the Manhattan world assumption in the scene to constrain pose estimation and map optimization.

[0011] As an improvement of the present invention, the front-end stage includes a lightweight deep learning network DCE-Net for image enhancement; if the lidar is not degraded, the lidar / inertial initialization is performed to obtain the bias of the accelerometer and gyroscope, and the visual scale is restored through the depth value of the point cloud.

[0012] As an improvement of the present invention, in the back-end stage, the laser radar edge-plane residual is constructed by the point-to-edge and point-to-plane matching residuals, specifically:

[0013]

[0014] in, represents the marginal residual, represents the plane residual, T i j represents the transformation matrix between time i and time j, represents the mth edge point and nth surface point at time i, Represents the coordinates of points A, B, and C.

[0015] As an improvement of the present invention, the construction of the visual point-line residual in the back-end stage specifically includes the following steps:

[0016] S21: Detecting ORB features in the camera image, including FAST key points and BRI EF descriptors, extracting line features, and enhancing the line features, wherein the enhancement method at least includes introducing hidden layer parameter adjustment, short line removal, and broken line merging;

[0017] S22: After receiving the new visual features obtained in step S21, the visual point and line residuals are constructed through the point and line reprojection residuals:

[0018]

[0019] in, represents the residual of the point feature set a, represents the residual of the line feature set b, l b,1 ,l b,2 Represent the line features l b, The components on the x and y planes, represents the camera projection model, represents the coordinates of the tracking point features at time i and j in set a, are the projection coordinates of the two endpoints of the tracking line feature of the i-th frame on the image.

[0020] As another improvement of the present invention, in the back-end stage, the Manhattan structure constraint residual constructed is specifically:

[0021]

[0022] in, is the normalized 3D line feature, They are vertical direction error and parallel direction error respectively.

[0023] As another improvement of the present invention, in the back-end stage, the visual confidence factor and the lidar confidence factor in the adaptive confidence factor are respectively:

[0024] s visual =u / W

[0025] s lidar =exp(-d mse / λ 2 )

[0026] Among them, u represents the number of visual feature tracking, W represents the size of the sliding window, and d mse represents the average error between the reference feature and the transformed feature in the lidar point cloud, and λ is the distance scale adjustment threshold. Based on these two confidence factors, the penalty function of the visual and lidar weights is designed:

[0027]

[0028] Among them, ω visual ,ω lidar represents the weight of the residual of visual and lidar features, and θ represents the control parameter that controls the non-convexity of the penalty function.

[0029] As another improvement of the present invention, the asymptotically non-convex factor graph optimization function in the back-end stage is specifically:

[0030]

[0031] Among them, x * represents the optimized state variable, N1, N2, N3, N4, and N5 represent the number of IMU pre-integration residuals, the number of lidar edge residuals, the number of lidar plane residuals, the number of visual point feature residuals, and the number of Manhattan structure constraint residuals in the sliding window, respectively; r i imu , They represent the i-th IMU pre-integration residual, the m-th lidar edge residual, the n-th lidar plane residual, the a-th visual point feature residual, the b-th visual line feature and the b-th Manhattan structure constraint residual respectively; Φ(ω visual ) and Φ(ω lidar ) represent the penalty functions of vision and lidar weights respectively.

[0032] As a further improvement of the present invention, in the back-end stage, dynamic adjustment of posture optimization is achieved according to the degradation degree weight of each sensor, specifically, solving the asymptotically non-convex factor graph optimization function, and the solution process includes inner iteration and outer iteration:

[0033] In each outer iteration, the control parameter θ is fixed;

[0034] In each inner iteration, first fix the weight of t-1 To optimize the state variable x at time t t , and then by fixing the state variable x at time t t To optimize the weight at time t Get the residual weights of visual and lidar features at time t

[0035] By changing the value of θ to repeat the internal and external iteration process, θ is adjusted to increase the non-convexity of the optimization function, the optimal UAV posture is obtained, and positioning is achieved.

[0036] Compared with the prior art, the present invention has the following beneficial effects:

[0037] (1) The present invention introduces a lightweight DCE-Net in the preprocessing process of the front-end stage, and extracts robust point and line features at the same time, thereby improving the image quality under low illumination, making the visual feature constraints reliable while reducing the time cost.

[0038] (2) The visual / inertial / lidar asymptotically non-convex factor graph optimization function constructed by the present invention reduces the impact of abnormal values ​​of each sensor on state estimation in underground space degradation scenarios.

[0039] (3) The present invention designs an adaptive confidence factor to evaluate the degree of camera and lidar degradation by the number of visual feature tracking and the difference between reference features and transformed features in the lidar point cloud, thereby achieving dynamic adjustment of graph optimization and improving the robustness of underground space state estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 It is a schematic diagram of the working principle of the method of the present invention;

[0041] Figure 2 It is a structural schematic diagram of a robust factor graph optimization model in the back-end stage of the method of the present invention;

[0042] Figure 3 Schematic diagram of the UAV platform built in the test example of the present invention;

[0043] Figure 4 A scene diagram of the data set selected for the test example in the present invention;

[0044] Figure 5 This is a comparison chart of the trajectory results of different algorithms on the UG_03 sequence in the test example of the present invention;

[0045] Figure 6 This is a comparison chart of the trajectory results of different algorithms on the UC_02 sequence in the test example of the present invention;

[0046] Figure 7 This is a comparison chart of the trajectory results of different algorithms on the EB_01 sequence in the test example of the present invention. DETAILED DESCRIPTION

[0047] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0048] Example 1

[0049] The multi-modal robust positioning method of UAV underground space based on confidence factor works as follows Figure 1 As shown, the following steps are included:

[0050] Step S1, front-end: first, the measurement data of the camera, lidar and IMU are input into the preprocessing module, in which the IMU pre-integration assists in removing the distortion of the lidar point cloud, and uses these dedistorted point clouds to perform deep association with visual features; then, a lightweight deep learning network DCE-Net is introduced to enhance the low-light images in underground spaces; then, a feature extraction method based on LSD is adopted to improve the LSD algorithm from three aspects: hidden layer parameter adjustment, short line removal and broken line fusion, and extract robust point and line features; finally, adaptive visual / inertial / lidar initialization is performed to obtain reliable initial values. If the lidar is not degraded, the lidar / inertial initialization is performed to obtain the bias of the accelerometer and gyroscope, and the visual scale is restored through the depth value of the point cloud; otherwise, visual / inertial initialization is performed through the point and line features extracted in the previous step.

[0051] Step S2, in the back end, first, the features processed by the front end are used to construct IMU measurement residuals, lidar edge-plane residuals, visual point-line residuals and Manhattan structure constraint residuals; then, an adaptive confidence factor is designed to evaluate the degree of degradation of the camera and lidar through the number of visual feature tracking and the difference between the reference features and the transformed features in the lidar point cloud; finally, according to the weight of each sensor, an asymptotically non-convex factor graph optimization function is constructed to achieve dynamic adjustment of pose optimization according to the degradation degree weight of each sensor, and obtain high-precision and robust positioning results.

[0052] Among them, the laser radar edge-plane residual is used to minimize the geometric error between the feature points (such as edge points and plane points) in the point cloud and the target points, thereby improving the accuracy of pose estimation. After that, represents the lidar edge feature at time i, Represents the LiDAR plane feature at time i, and constructs the LiDAR edge-plane residual through point-to-edge and point-to-plane matching residuals:

[0053]

[0054] in, represents the marginal residual, represents the plane residual, T i j represents the transformation matrix between time i and time j, represents the mth edge point and nth surface point at time i, Represents the coordinates of points A, B, and C.

[0055] Visual point-line residual, utilizes the geometric relationship between point features and line features in the image to constrain the camera's pose, thereby improving the robustness and accuracy of the system.

[0056] When the visual sensor works normally, point feature extraction is performed first to detect ORB features in the image, including FAST key points and BRI EF descriptors. At the same time, line feature extraction is performed, and three strategies, including hidden layer parameter adjustment, short line removal and broken line merging, are introduced to enhance line feature extraction, which enhances the real-time performance of the system while ensuring the effective feature information of the system.

[0057] After receiving the new visual features, the visual point and line residuals are constructed through the point and line reprojection residuals:

[0058]

[0059] in, represents the residual of the point feature set a, Represents the residual of the line feature set b. b,1 ,l b,2 Represent the line features l b, Components in the x, y plane. represents the camera projection model, represents the coordinates of the tracking point features at time i and j in set a, are the projection coordinates of the two endpoints of the tracking line feature of the i-th frame on the image.

[0060] Manhattan structure constraint residual is mainly used to constrain pose estimation and map optimization by using the Manhattan world assumption in the scene. It reduces the degrees of freedom and improves robustness and accuracy by assuming that the geometric structure of the scene follows a set of global orthogonal directions.

[0061] During the operation of the drone, after receiving the line features extracted from each frame, the three Manhattan axes are used to parallelly judge each line feature. If there is a parallel relationship, the line feature is associated with the corresponding Manhattan axis. Before building a nonlinear optimization model, it is necessary to define the parallel and perpendicular structural residuals between line features. For the normalized 3D line features The vertical direction error and the parallel direction error can be defined as Further construct the structural constraints between the Manhattan axis and the associated line features, which are defined as the Manhattan structural constraint residual:

[0062]

[0063] Due to the abnormality of the sensor and the complex interference of the underground space, the measurement information of the sensor is easily seriously disturbed. In order to improve the positioning accuracy and system stability in the feature degradation environment, the visual confidence factor s is designed. visual and the lidar confidence factor s lidar , which can quantitatively reflect the quality of feature matching and evaluate the degree of degradation of the camera and lidar. The degree of degradation of the camera and lidar is evaluated by the number of visual feature tracking and the difference between the reference feature and the transformed feature in the lidar point cloud, and the visual confidence factor and lidar confidence factor are designed respectively:

[0064] s visual =u / W

[0065] s lidar =exp(-d mse / λ 2 )

[0066] Among them, u represents the number of visual feature tracking, W represents the size of the sliding window, and d mse represents the average error between the reference features and the transformed features in the lidar point cloud, and λ is the distance scale adjustment threshold, which ensures that the distance error is within a certain scale range.

[0067] The more visual features tracked in the sliding window, the more accurate the visual information is. Under these two confidence factors, the penalty function of visual and lidar weights is designed:

[0068]

[0069] Among them, ω visual ,ω lidarrepresents the weight of the residual of visual and lidar features, and θ is a control parameter that controls the non-convexity of the penalty function.

[0070] Although the factor graph can handle the heterogeneous and nonlinear measurement information of the visual-inertial-lidar system well, it cannot correct the abnormal information of the sensor. In order to reduce the impact of the degradation of the underground space environment factor graph, the visual / inertial / lidar asymptotically non-convex factor graph optimization function is constructed using the designed IMU measurement residuals, lidar edge-plane residuals, visual point-line residuals and Manhattan structure constraint residuals.

[0071] First, define the state variables to be optimized for the integrated navigation system:

[0072]

[0073] in, They represent the position, velocity and rotation of the carrier in the world coordinate system w of the i-th frame, respectively, and b a ,b g represents the bias of the accelerometer and gyroscope, λ m represents the inverse depth of the mth tracked feature. Through the nonlinear least squares optimization problem, the state x can be obtained i Considering the sensor degradation, an asymptotically non-convex optimization function is designed.

[0074]

[0075] where r(y i ,x) represents the measurement residual of each sensor, ω i represents the weight of each sensor, Φ ρ (ω i ) represents the penalty function of each sensor weight. Design visual confidence factor s visual and the lidar confidence factor s lidar , construct a robust factor graph optimization model, as follows Figure 2 As shown, circles represent nodes in the factor graph, representing each sensor; the lines between nodes represent edges in the factor graph, representing the constraint relationship between each sensor.

[0076] The asymptotically non-convex factor graph optimization function for vision / inertial / lidar is:

[0077]

[0078] Among them, x *represents the optimized state variable, N1, N2, N3, N4, and N5 represent the number of IMU pre-integration residuals, the number of lidar edge residuals, the number of lidar plane residuals, the number of visual point feature residuals, and the number of Manhattan structure constraint residuals in the sliding window, respectively; r i imu , They represent the i-th IMU pre-integration residual, the m-th lidar edge residual, the n-th lidar plane residual, the a-th visual point feature residual, the b-th visual line feature and the b-th Manhattan structure constraint residual respectively; Φ(ω visual ) and Φ(ω lidar ) represent the penalty functions of vision and lidar weights respectively.

[0079] The asymptotically non-convex factor graph optimization function of vision / inertial / lidar is solved by iterative optimization, which mainly includes inner iteration and outer iteration.

[0080] In each outer iteration, the control parameter θ is fixed.

[0081] In the inner iteration, we first fix the weight of t-1 To optimize the state variable x at time t t :

[0082]

[0083] Among them, the penalty function Φ that is independent of x is removed ρ (ω i ), The weight ω contains the residuals of visual and lidar features visual ,ω lidar Then by fixing the state variable x at time t t To optimize the weight at time t

[0084]

[0085] Among them, the residual r(y i ,x t ) is a constant in this step. Combining the above formula and the designed confidence factor, the residual weights of the visual and lidar features at time t can be obtained.

[0086]

[0087] The larger the residual factor of different sensors, the smaller the confidence, and the smaller the assigned weight. By changing the value of θ to repeat the internal and external iteration process, θ is adjusted to increase the non-convexity of the optimization function, and finally the optimal UAV posture is obtained.

[0088] Test Case

[0089] Existing public datasets lack challenging scenes such as underground spaces. In order to verify the robustness of the proposed algorithm, an unmanned aerial vehicle experimental platform was built to simulate challenging scenes such as poor lighting conditions, sparse features, and structural degradation in underground spaces for real data collection.

[0090] The UAV experimental platform built in this test case is based on the PX4 autopilot, and the flight power equipment is a 50A four-in-one ESC and a 2810-900KV brushless motor. The flight control uses CUAV V5 nano, and the power supply is a 6s5500mAh 75C aircraft model battery. The environment perception uses Intel D435i camera, Xsens MT i-7MEMS-I MU, Li vox mid360 laser radar, MTF-01 optical flow module, and is equipped with Intel airborne computer for data processing. The completed UAV platform is as follows: Figure 3 shown.

[0091] In order to fully verify the proposed method in the complex environment of underground space, multiple sets of data were collected in the underground garage (UG), experimental building (EB) and underground patio (UC) on the campus of Southeast University. Figure 4 As shown in the figure, UG has dim light, simulating the visual degradation scene of underground space; EB has a long corridor parallel structure and transparent glass, simulating the laser radar degradation scene of underground space; UC has changeable light and complex characteristics, simulating the complex scene of underground space.

[0092] To evaluate the robustness of the proposed method, experiments are conducted on an autonomously collected UAV flight dataset. In order to improve the positioning accuracy and system stability in feature-degraded environments, two adaptive confidence factors s are designed. visual and lidar To reflect the quality of feature matching and evaluate the degree of degradation of the camera and lidar. In this test case, an ablation experiment was set up by deleting these two factors to verify their impact on positioning accuracy. Three typical sequences were selected for ablation experiments, and the complete algorithm was compared with the algorithm without the visual confidence factor, without the lidar confidence factor, and with fast l ivo. "proposed-sv" means deleting the visual confidence factor s visual "proposed-s l" means deleting the lidar confidence factor s lidar , "proposed-sv-s l" means deleting two confidence factors, "proposed" means the complete method, and the comparison of trajectory results is as follows Figures 5 to 7 As shown, Figure 5 represents the UG_03 sequence, Figure 6 represents the UC_02 sequence, Figure 7 Represents the EB_01 sequence. From the visualization trajectory results, it can be seen that the complete method has stronger robustness.

[0093] Furthermore, the positioning accuracy is quantitatively evaluated by comparing the ground distance between the starting point and the end point of the positioning results of different algorithms. The real translation distance is calculated from the recorded starting point and end point coordinates and defined as the “truth”. The results are shown in Table 1, where “proposed-sv” means deleting the visual confidence factor s. visual The proposed-s l method removes the lidar confidence factor s. lidar “proposed-sv-s l” means the proposed method with two confidence factors removed, and “proposed” means the complete method.

[0094] Table 1 Network model training parameters

[0095]

[0096] It can be seen from the above table that the positioning result of the method of the present invention is closer to the real data, and the confidence factor in the present invention is crucial. The lack of any factor will lead to a decrease in the performance of the method. In addition, it can be seen from the comparison between the method of the present invention and Fast-LIVO under the self-collected data set that the positioning accuracy of the method of the present invention is much higher than that of Fast-LIVO.

[0097] In summary, the method of the present invention preprocesses the input data at the front end, uses a lightweight deep network DCE-Net to improve the quality of the image under low illumination conditions, and then extracts point and line features and initializes the system; in the back end, firstly, IMU measurement residuals, lidar edge-plane residuals, visual point and line residuals, and Manhattan structure constraint residuals are constructed, and then an adaptive confidence factor is designed, and the tracking times of visual feature points and the matching differences between reference features and target features in the lidar point cloud are used to evaluate the degradation degree of the camera and lidar respectively; finally, an asymptotically non-convex factor graph optimization function is constructed, and dynamic adjustment of posture optimization is realized according to the degradation degree weights of each sensor, thereby improving the positioning accuracy and robustness of underground space drones, and achieving better results.

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

Claims

1. A multi-modal robust positioning method for UAV underground space based on confidence factor, characterized by: It includes two stages: front-end and back-end. In the front-end stage: pre-processing the measurement data of the camera, lidar and IMU, extracting point and line features for adaptive vision / inertial / lidar initialization; In the back-end stage: IMU measurement residuals, laser radar edge-plane residuals, visual point-line residuals and Manhattan structure constraint residuals are constructed according to the features extracted and processed in the front-end stage; then an adaptive confidence factor is designed to evaluate the degree of degradation of the camera and laser radar through the number of visual feature tracking and the difference between the reference feature and the transformed feature in the laser radar point cloud, and the adaptive confidence factor includes at least a visual confidence factor and a laser radar confidence factor; according to the weight of each sensor, an asymptotically non-convex factor graph optimization function is constructed, and dynamic adjustment of posture optimization is achieved according to the degradation degree weight of each sensor to obtain a positioning result; wherein, The laser radar edge-plane residual is used to minimize the geometric error between the feature points and the target points in the point cloud; The visual point-line residual: utilizes the geometric relationship between the point features and the line features in the image to constrain the camera's position and posture; The Manhattan structure constrained residual: uses the Manhattan world assumption in the scene to constrain pose estimation and map optimization.

2. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 1, characterized in that: The front-end stage includes a lightweight deep learning network DCE-Net for image enhancement; if the lidar is not degraded, the lidar / inertial initialization is performed to obtain the bias of the accelerometer and gyroscope, and the visual scale is restored through the depth value of the point cloud; If the lidar is degraded, the extracted visual point and line features are used for visual / inertial initialization.

3. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 1 or 2, characterized in that: In the back-end stage, the lidar edge-plane residual is constructed through the point-to-edge and point-to-plane matching residuals, specifically: in, represents the marginal residual, represents the plane residual, T i j represents the transformation matrix between time i and time j, represents the mth edge point and nth surface point at time i, Represents the coordinates of points A, B, and C.

4. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 3, characterized in that: The construction of the visual point-line residual in the back-end stage specifically includes the following steps: S21: Detecting ORB features in the camera image, including FAST key points and BRIEF descriptors, extracting line features, and enhancing the line features, wherein the enhancement method at least includes introducing hidden layer parameter adjustment, short line removal, and broken line merging; S22: After receiving the new visual features obtained in step S21, the visual point and line residuals are constructed through the point and line reprojection residuals: in, represents the residual of the point feature set a, represents the residual of the line feature set b, l b,1 ,l b,2 Represent the line features l b, The components on the x and y planes, represents the camera projection model, represents the coordinates of the tracking point features at time i and j in set a, are the projection coordinates of the two endpoints of the tracking line feature of the i-th frame on the image.

5. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 4, characterized in that: In the back-end stage, the Manhattan structure constraint residual constructed is specifically: in, is the normalized 3D line feature, They are vertical direction error and parallel direction error respectively.

6. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 3, characterized in that: In the back-end stage, the visual confidence factor and the lidar confidence factor in the adaptive confidence factor are respectively: s visual =u / W s lidar =exp(-d mse / λ 2 ) Among them, u represents the number of visual feature tracking, W represents the size of the sliding window, and d mse represents the average error between the reference feature and the transformed feature in the lidar point cloud, and λ is the distance scale adjustment threshold. According to the two confidence factors, the penalty function of the visual and lidar weights is designed: Among them, ω visual ,ω lidar represents the weight of the residual of visual and lidar features, and θ represents the control parameter that controls the non-convexity of the penalty function.

7. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 5, characterized in that: The asymptotically non-convex factor graph optimization function in the back-end stage is specifically: Among them, x * represents the optimized state variable, N1, N2, N3, N4, and N5 represent the number of IMU pre-integration residuals, the number of lidar edge residuals, the number of lidar plane residuals, the number of visual point feature residuals, and the number of Manhattan structure constraint residuals in the sliding window, respectively; Respectively represent the i-th IMU pre-integration residual, the m-th lidar edge residual, the n-th lidar plane residual, the a-th visual point feature residual, the b-th visual line feature and the b-th Manhattan structure constraint residual; Φ(ω visual ) and Φ(ω lidar ) represent the penalty functions of vision and lidar weights respectively.

8. The confidence factor-based multi-modal robust positioning method for underground space of unmanned aerial vehicles according to claim 7, characterized in that: In the back-end stage, the dynamic adjustment of the posture optimization is realized according to the degradation degree weight of each sensor, specifically, solving the asymptotically non-convex factor graph optimization function, and the solution process includes inner iteration and outer iteration: In each outer iteration, the control parameter θ is fixed; In each inner iteration, first fix the weight of t-1 To optimize the state variable x at time t t , and then by fixing the state variable x at time t t To optimize the weight at time t Get the residual weights of visual and lidar features at time t By changing the value of θ to repeat the internal and external iteration process, θ is adjusted to increase the non-convexity of the optimization function, the optimal UAV posture is obtained, and positioning is achieved.

Images