Unmanned aerial vehicle fusion positioning method based on four-anchor-point UWB pose dynamic quality evaluation

Abstract

The invention provides an unmanned aerial vehicle fusion positioning method based on four-anchor-point UWB pose dynamic quality evaluation, and the method comprises the following steps: defining an unmanned aerial vehicle coordinate system and a world coordinate system, and selecting a first frame point cloud coordinate system of a laser radar as the world coordinate system; defining a state transition model required by front-end state estimation; obtaining a front-end laser radar-inertial odometer factor by using an iterative Kalman filter; based on the four-anchor-point ultra wide band, the accurate unmanned aerial vehicle position and attitude are obtained to serve as ultra wide band factors; based on the obtained ultra-wideband factor, judging the position and attitude quality of the ultra-wideband factor, and determining whether the ultra-wideband factor participates in factor graph optimization or not by adopting an event triggering mechanism; and factor graph optimization is used to obtain the accurate position and attitude of the unmanned aerial vehicle after optimization. And the calculation burden of an airborne computer of the unmanned aerial vehicle is reduced, and the accurate unmanned aerial vehicle position and attitude obtained by combining the four-anchor-point ultra wide band are combined, so that the unmanned aerial vehicle real-time positioning with higher precision and lower calculation burden is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and specifically to an unmanned aerial vehicle fusion positioning method based on dynamic quality assessment of the pose of four anchor points UWB. Background Technique

[0002] Simultaneous localization and mapping technology is one of the key technologies for realizing localization and mapping in a degraded perception environment. This technology refers to the situation where, given the position and attitude of the robot itself and the positions of the surrounding environment, the robot uses sensor information to construct an incremental map and simultaneously determines its own position in the global map. LiDAR can accurately and quickly obtain the surrounding environment, has the advantages of not relying on lighting conditions and anti-interference, and has now become the core sensor for realizing a weakened perception environment. However, in an environment with insufficient geometric constraints (such as a narrow tunnel or a single and extended wall surface), LiDAR is prone to problems such as similar environments or the LiDAR itself being affected by noise, occlusion, multipath interference, etc., and its performance often drops significantly.

[0003] In recent years, in order to overcome the limitations of LiDAR positioning in an indoor degraded environment, researchers have widely adopted the method of multi-source sensor fusion to achieve high-precision positioning. Sensors such as cameras, inertial navigation systems (INS), and ultra-wideband (UWB) have become the mainstream choices for fusing with LiDAR. Among them, ultra-wideband (UWB) has been widely used in the fields of indoor positioning and integrated navigation. It has the ability of absolute positioning and high ranging accuracy, and can achieve centimeter-level positioning accuracy in a line-of-sight environment. Compared with GPS (Global Positioning System), the ultra-wideband (UWB) sensor has a low cost and is suitable for a wide range of scenarios, including indoor environments where GPS (Global Positioning System) signals are blocked.

[0004] At present, there have been many works integrating UltraWideBand (UWB) with laser SLAM (simultaneous localization and mapping) or visual SLAM (simultaneous localization and mapping). However, most of them only use the ranging information of UltraWideBand (UWB) to obtain one-dimensional constraints. Although this is very effective for correcting the position and attitude information of the robot in a single direction, when the robot needs to perform actions such as turning in a complex perception-weakening scenario, it is unable to correct the three-dimensional position and attitude information such as the rotation angle of the robot. Therefore, there is still much room for improvement in the positioning accuracy of such a multi-sensor fusion system that only relies on the ranging information of UltraWideBand (UWB).

[0005] In addition, there are currently many lidar-UWB SLAM (simultaneous localization and mapping) methods that utilize the position and attitude information of four-anchor-point UltraWideBand (UWB) combined with lidar. However, since the anchor-point information of UltraWideBand (UWB) is involved in the fusion throughout the process, it has a great impact on the real-time performance of the system. Also, because UltraWideBand (UWB) data information is required throughout the process, there are also great limitations on the layout position of the UltraWideBand (UWB) anchor points.

[0006] Solutions similar to the present invention are as follows: 1. The Chinese invention patent with the authorization number CN116608851B discloses a technology in the field of robot positioning and navigation, namely an underground pipeline dual-robot step-by-step collaborative navigation method, system, and server. In the present invention, two robots alternately perform positioning. Each robot is equipped with an IMU unit, a UWB unit, and a server. The absolute azimuth angle of the robot in a stationary state is measured by the IMU unit. After another robot advances to a new node in the direction of the target point, the relative azimuth angle and distance between the two robots are measured by the UWB unit, and the server calculates the position of the other robot based on the data measured by the IMU unit and the UWB unit to calculate the position of the next node and generate corresponding control instructions to control the robot to advance to the next node, realizing the step-by-step advancement of the two robots. The present invention can solve the problems of rapid accumulation of pure inertial navigation errors and the need to pre-arrange base stations for UWB positioning, and has the advantages of low error, low cost, low power consumption, and high degrees of freedom.

[0007] Disadvantages of this method: The two robots used in this method are built-in with IMU and UWB units and a server, which solves the problems of the need to deploy base stations in advance for UWB and cumulative error problems. However, lidar odometry is not added. Therefore, during the driving process of the robot, there is a lack of local high-precision trajectories.

[0008] 2. The authorized patent number CN110879071B discloses a high-precision positioning system based on vehicle-road cooperation and a vehicle-mounted positioning terminal. The vehicle-mounted positioning terminal includes a UWB module, an IMU module, a wireless communication module, and an operation and processing module. The vehicle-mounted positioning terminal is used to process local signals output from the UWB module and the IMU module, receive remote signals from the field part, and perform corresponding position calculation, fusion, and output.

[0009] Disadvantages of this method: When the GPS (Global Positioning System) signal is missing, pure IMU dead reckoning will be affected by zero bias and random noise, while pure ultra-wideband may be interfered by electromagnetic waves in the environment. Although the fusion of the two can improve the accuracy of long-distance positioning, the short-term accuracy is not high. Especially in a nearly stationary scenario, the above interference will have a more obvious impact on the positioning result.

[0010] 3. The paper "NLOS Identification-and Correction-Focused Fusion of UWB and LiDAR-SLAM Based on Factor Graph Optimization for High-Precision Positioning with Reduced Drift" proposes a robot positioning method that fuses UWB ranging information with four anchor points.

[0011] Disadvantages of this method: This method relies on the four-anchor-point UWB signal throughout the process to correct the position and attitude of the robot, which limits the usable space range and cannot be used in environments such as long corridors. Moreover, for the on-board computer of the robot, the computational load is too large.

[0012] In summary, the defects and deficiencies of the prior art: 1. In scenarios with poor geometric constraints (such as underground tunnels, long corridors, etc.), the autonomous planning and navigation method that does not rely on external signals has relatively high positioning accuracy for short-term drones. However, as the robot moves, the cumulative error of the lidar becomes too large, resulting in sensor failure. To address the technical problem that a single-anchor ultra-wideband (UWB) cannot provide three-dimensional position and attitude information for correction, the present invention adopts a multi-anchor ultra-wideband positioning technology that can effectively correct three-dimensional position and attitude information such as the rotation angle of the drone.

[0013] 2. The four-anchor Ultra Wide Band (UWB) positioning method is a low-cost and high-precision positioning method. The positioning entity calculates its position by receiving ranging information from 4 UWB anchors and using the TDOA principle. However, the current algorithm using four-anchor UWB needs to integrate UWB information throughout the process, which has a great impact on the real-time performance of the system and imposes an excessive computational burden on the on-board computer of the drone. Summary of the Invention

[0014] To solve the existing technical problems, the main object of the present invention is to provide a drone fusion positioning method based on the dynamic quality evaluation of the pose of four-anchor UWB. By adopting an event-triggered factor graph optimization based on the attitude quality of ultra-wideband, the computational burden on the on-board computer of the drone is reduced, and combined with the accurate position and attitude of the drone obtained by four-anchor ultra-wideband, real-time positioning of the drone with higher accuracy and lower computational burden is achieved.

[0015] To achieve the above technical features, the object of the present invention is realized as follows: The drone fusion positioning method based on the dynamic quality evaluation of the pose of four-anchor UWB includes the following steps: Step 1: Define the drone coordinate system and the world coordinate system, and select the first-frame point cloud coordinate system of the lidar as the world coordinate system; Step 2: Define the state transition model required for front-end state estimation; Step 3: Use an iterative Kalman filter to obtain the front-end lidar-inertial odometer factor; Step 4: Obtain the accurate position and attitude of the drone based on four-anchor ultra-wideband as the ultra-wideband factor; Step 5: Based on the ultra-wideband factor obtained in Step 4, judge its position and attitude quality, and adopt an event-triggered mechanism to determine whether the ultra-wideband factor participates in factor graph optimization; Step 6: Use factor graph optimization to obtain the optimized accurate position and attitude of the drone.

[0016] The Step 1 includes lidar-inertial odometer and ultra-wideband positioning factors; Define the world coordinate system W and the drone coordinate system B. The state of the drone at time includes information related to the attitude and speed of the drone, expressed as: ; ; In the formula, represents the rotation matrix, represents the position of the drone, represents the speed, Denote the IMU bias; the transformation matrix from the UAV coordinate system B to the world coordinate system W is expressed as: .

[0017] In step 2, the state estimation uses a tightly coupled Kalman filter, including a state transition model and a measurement model. The state transition model is as follows: ; In the formula, denotes the state at time , is the time interval between two moments, is the state transition function, which depends on the current state , the control input and the process noise .

[0018] Step 2 also includes defining the measurement model required for the front-end state estimation. The measurement model of the radar data is as follows: ; In the formula, denotes the true coordinate of the th point in the radar coordinate system, denotes the measured coordinate of the th point in the radar coordinate system, denotes the measurement noise of the th point.

[0019] Step 3 includes using an iterative Kalman filter to obtain the tightly coupled attitude estimation of the UAV based on lidar-inertial odometry; In the ideal case, the relationship between the th point and a certain reference plane or reference point should satisfy certain constraint conditions, such that the value of the function is zero, and the optimization problem is as follows: ; In the formula, the state variable is adjusted by minimizing this residual to ensure that the radar points are correctly projected onto the plane segment of the global map; The method using the tightly coupled extended Kalman filter framework first uses the IMU data to propagate the state estimation when there is no new radar data, maintaining a continuous estimation of the robot state. After receiving new radar data, the system performs iterative updates to refine the state prediction, align it with the actual measurement values, calculate the residual between the predicted value and the measurement value, and iteratively adjust the estimated value until it converges to a stable state.

[0020] In step 3, after converging the front lidar-inertial odometry factors required for converting the attitude estimation output by the Kalman filter, the optimal state estimation and covariance formulas are as follows: ; In the formula, represents the state before update, represents the state after update, represents the updated covariance matrix of the error, is the Kalman gain, is the identity matrix, is the measurement matrix that maps the state variables to the measurement space, is the error covariance matrix.

[0021] In step 4, covariance evaluation is performed on the attitude information derived from UWB; high covariance indicates that the data is unavailable; After deploying the UWB anchors, first obtain prior knowledge by manually measuring the angular orientation between the base station and the world coordinate system, and then perform the corresponding projection transformation. It is necessary to ensure the quality of the received UWB measurement data. Once the UWB measurement data is verified, linear interpolation is performed according to the timestamp of the lidar to synchronize with the timestamp of the lidar-inertial odometry attitude information.

[0022] In step 5, after an event is triggered, the UWB factor is integrated into the optimization process; The constructed factor graph includes two factor types and one variable type. The variable represents the state of the robot at a specific time interval. The factors include: lidar-inertial odometry attitude factor and UWB attitude factor. When the robot approaches the UWB settings of the four anchors, covariance evaluation is performed on the attitude information derived from UWB; high covariance indicates that the data is unavailable. The effective position and attitude information is integrated as UWB factors at 5-meter intervals. The factor graph is optimized by incremental smoothing and Bayesian tree after node insertion.

[0023] In step 6, factor graph optimization is used, and the specific formula is as follows: Factor graph optimization is finally transformed into a non-linear least squares problem: ; Among them, represents the total number of UWB anchors ； and respectively represent the number of UWB measurement residuals and lidar-SLAM measurement residuals; is used to identify the UWB anchor; and They respectively correspond to the indices of the ultra-wideband factor and the lidar-SLAM factor in the factor graph, the measurement residuals related to the ultra-wideband sensor, and the range residuals are expressed as ; the measurement residuals related to the lidar sensor, including the translation residuals and the rotation residuals ; Finally, this least squares problem is calculated to obtain the optimized precise position and attitude of the UAV.

[0024] The present invention has the following beneficial effects: 1. The present invention mainly solves the problem of correcting the cumulative error of the radar inertial odometer by combining the position and attitude information of four-anchor ultra-wideband (UltraWideBand, UWB) in the satellite positioning signal rejection scenario. Through the front-end iterative Kalman filter, the radar-inertial measurement unit is tightly coupled and sent as a radar-inertial measurement unit factor into the factor graph optimization at the back end. At the same time, when the robot receives the ultra-wideband factor, the received ultra-wideband factor is judged to determine whether to participate in the factor graph optimization. After judging that the ultra-wideband factor is available, the ultra-wideband factor and the radar-inertial measurement unit factor are optimized together in the factor graph. This not only improves the positioning accuracy of the UAV in the perception degradation environment, but also greatly saves the computing burden of the UAV on-board computer, thereby ensuring the robustness of the system when the GPS (GlobalPositioningSystem) signal is rejected and providing strong support for the autonomous navigation planning of the UAV.

[0025] 2. Enhance the ability to correct three-dimensional position and attitude information: Aiming at the technical problem that single-anchor ultra-wideband (UltraWideBand, UWB) positioning cannot provide correction of three-dimensional position and attitude information, the present invention adopts multi-anchor UWB positioning technology, which can effectively correct the three-dimensional position and attitude information such as the rotation angle of the UAV. In complex perception-weakening scenarios, such as when the UAV needs to turn, etc., the present invention can maintain high positioning accuracy, thereby improving the operation reliability and safety of the UAV in these scenarios. This improvement directly solves the limitation of single-anchor ultra-wideband (UltraWideBand, UWB) positioning in correcting three-dimensional position and attitude, and improves the navigation performance of the UAV in complex environments.

[0026] 3. Event-triggered mechanism based on UltraWideBand (UWB) factor quality improves real-time performance: The present invention introduces an event-triggered mechanism based on UWB factor quality, ensuring that only high-quality UWB data is used for the optimization process. This intelligent decision-making mechanism avoids the negative impact of invalid data on positioning accuracy and reduces the computational burden on the drone's on-board computer through an optimization algorithm. By reducing the computational burden, the real-time performance of the system is directly improved, enabling the drone to complete positioning calculations in a shorter time, enhancing the drone's response speed and task execution efficiency.

[0027] 4. The present invention uses a tight coupling of four-anchor UltraWideBand (UWB) and Lidar-inertia odometry (LIO), and adopts an event-triggered mechanism to ensure a low computational burden for the drone. It can also be used in unmanned systems with high computational burdens such as unmanned vehicles. Brief Description of the Drawings

[0028] The present invention will be further described below in conjunction with the drawings and embodiments.

[0029] Figure 1 It is the system flowchart of the present invention.

[0030] Figure 2 It is the factor graph optimization process of the present invention.

[0031] Figure 3 It is the system block diagram of the present invention. Detailed Embodiments

[0032] The embodiments of the present invention will be further described below in conjunction with the drawings.

[0033] Embodiment 1: The present invention mainly proposes a method to correct the cumulative error of the radar inertial odometer by combining the position and attitude information of four-anchor UltraWideBand (UWB). Through an event-triggered factor graph optimization based on the attitude quality of UltraWideBand (UWB), the computational burden on the drone's on-board computer is reduced. Combining the accurate drone position and attitude obtained from the four-anchor UltraWideBand (UWB), real-time positioning of the drone with higher accuracy and lower computational burden is achieved.

[0034] The present invention mainly solves the problem of correcting the cumulative error of the radar inertial odometer by combining the position and attitude information of four-anchor-point ultra-wideband (UWB) in the scenario where satellite positioning signals are denied. Through the front-end iterative Kalman filter, the tight coupling of the radar-inertial measurement unit is used as a radar-inertial measurement unit factor and sent into the factor graph optimization at the back end. At the same time, when the robot receives the UWB factor, the received UWB factor is judged to decide whether to participate in the factor graph optimization. After determining that the UWB factor is available, the UWB factor and the radar-inertial measurement unit factor are used for factor graph optimization together. This not only improves the positioning accuracy of the drone in the perception-degraded environment, but also greatly saves the computing burden of the drone's on-board computer, thereby ensuring the robustness of the system when the GPS (Global Positioning System) signal is denied and providing strong support for the autonomous navigation planning of the drone.

[0035] The overall system process is as Figure 1 shown. Among them, the two parts of the system are respectively the tight coupling factor of the radar-inertial odometer obtained based on the extended Kalman filter and an event-triggered factor graph optimization based on the UWB attitude quality.

[0036] The steps of the drone fusion positioning method for dynamic quality evaluation of the position and attitude of four-anchor-point UWB include: Step 1: Define the drone coordinate system and the world coordinate system, and select the first-frame point cloud coordinate system of the lidar as the world coordinate system; Step 2: Define the models required for front-end state estimation; Step 3: Use an iterative Kalman filter to obtain the front-end lidar-inertial odometer factor; Step 4: Obtain the accurate position and attitude of the drone based on four-anchor-point ultra-wideband as the UWB factor; Step 5: Based on the UWB factor obtained above, adopt an event-triggered mechanism to judge its position and attitude quality and decide whether the UWB factor participates in the factor graph optimization; Step 6: Use factor graph optimization to obtain the optimized accurate position and attitude of the drone.

[0037] Embodiment 2: The specific steps of the drone fusion positioning method for dynamic quality evaluation of the position and attitude of four-anchor-point UWB include: Step 1: Define the drone state, coordinate system and world coordinate system, and select the first-frame point cloud coordinate system of the lidar as the world coordinate system: The present invention relates to a method for robot positioning using factor graphs, including Lidar-IMU and Ultra-Wideband (UWB) positioning factors. In the present invention, a world coordinate system is defined. and the robot body coordinate system , the state of the unmanned aerial vehicle (UAV) at time includes information related to the attitude and velocity of the UAV, expressed as: ; In the formula, represents the rotation matrix, represents the position of the UAV, represents the velocity, represents the IMU bias; the transformation matrix from the UAV coordinate system B to the world coordinate system W is expressed as: .

[0038] Among them, this method first estimates a high-precision odometer at the front end of the Lidar-IMU using an iterative Kalman filter, and then uses it as the Lidar-IMU factor in graph optimization.

[0039] Step 2: Define the state transition model required for front-end state estimation: State estimation uses a tightly coupled Kalman filter, including a state transition model and a measurement model. The state transition model is as follows: ; In the formula, represents the state at time , is the time interval between two moments, is the state transition function, which depends on the current state , the control input and the process noise .

[0040] Step 3: Define the measurement model required for front-end state estimation: The measurement model of radar (LiDAR) data is as follows: ; In the formula, represents the true coordinate of the th point in the radar coordinate system, represents the measured coordinate of the th point in the radar coordinate system, represents the measurement noise of the th point.

[0041] Step 4: Use the iterative Kalman filter to obtain the tight-coupled attitude estimation of the UAV based on LiDAR-inertial odometry: In the ideal case, the relationship between the th point (considering noise) and a certain reference plane or reference point should satisfy certain constraint conditions, such that the value of the function is zero, and the optimization problem is as follows: ; In the formula, the state variable is adjusted by minimizing this residual to ensure that the radar points are correctly projected onto the plane segment of the global map; The method using the tight-coupled extended Kalman filter framework first uses IMU data to propagate the state estimation when there is no new radar data, maintaining a continuous estimation of the robot's state. After receiving new radar data, the system performs iterative updates to refine the state prediction to align it with the actual measurements, calculates the residual between the predicted value and the measured value, and iteratively adjusts the estimation until it converges to a stable state.

[0042] Step 5: Convert the attitude estimation output by the Kalman filter into the required front-end LiDAR-inertial odometry factor.

[0043] After convergence, the optimal state estimation and covariance formulas are as follows: ; In the formula, represents the state before update, represents the state after update, represents the updated covariance matrix of the error, is the Kalman gain, is the identity matrix, is the measurement matrix that maps the state variable to the measurement space, is the error covariance matrix.

[0044] In an environment with insufficient geometric constraints, such as a narrow corridor with scarce features, the performance of LiDAR-IMU SLAM often degrades significantly. To mitigate this challenge, integrating sensors that provide global reference measurements can help with drift correction. In indoor applications where there is a lack of GPS (Global Positioning System) signals, an Ultra-WideBand (UWB) positioning system is used to correct drift.

[0045] Step 6: Obtain the accurate position and attitude of the UAV based on the four-anchor UWB as the UWB factor, where the covariance of the attitude information derived from the UWB is evaluated; high covariance indicates that the data is unavailable.

[0046] In step 4, covariance evaluation is performed on the attitude information exported by UWB; high covariance indicates that the data is unavailable. After deploying the UWB anchors, first obtain prior knowledge by manually measuring the angular orientation between the base station and the world coordinate system, and then perform the corresponding projection transformation. It is necessary to ensure the quality of the received UltraWideBand (UWB) measurement data because these data may be affected by noise, interference, and outliers due to challenging environmental conditions. Once the UltraWideBand (UWB) measurement data is verified, linear interpolation is performed according to the timestamp of the lidar to synchronize with the timestamp of the LiDAR-IMU attitude information.

[0047] Step 7: Based on the UWB factors obtained above, an event-triggered mechanism is adopted to judge the position and attitude quality, and determine whether the UWB factors participate in the factor graph optimization.

[0048] After the event is triggered, the UWB factors are integrated into the optimization process. In the factor graph optimization, it is not necessary to continuously use the UltraWideBand (UWB) factors because of the gradually accumulating drift inherent in the radar-inertial odometry. In the proposed process, the UWB factors are only used in the optimization process when the estimated position covariance exceeds the received UltraWideBand (UWB) attitude.

[0049] The constructed factor graph includes two factor types and one variable type. The variable represents the state of the robot at a specific time interval and is represented by a node in the graph. The factors include: (a) Lidar-IMU attitude factor, (b) UltraWideBand (UWB) attitude factor. When the robot approaches the UltraWideBand (UWB) settings of the four anchors, covariance evaluation is performed on the attitude information exported by UWB; high covariance indicates that the data is unavailable. The effective position and attitude information is integrated as UltraWideBand (UWB) factors at 5-meter intervals. The graph is optimized using incremental smoothing and Bayesian trees (iSAM2) after node insertion.

[0050] Step 8: Use factor graph optimization, and the specific formula is as follows.

[0051] The factor graph optimization is finally transformed into a non-linear least squares problem: ; where represents the total number of UWB anchors ； and represent the number of UWB measurement residuals and lidar-SLAM measurement residuals respectively; For identifying UWB anchors; and respectively correspond to the indices of the ultra-wideband factor and the lidar-SLAM factor in the factor graph, the measurement residuals related to the ultra-wideband sensor, and the range residuals are expressed as ; the measurement residuals related to the lidar sensor, including the translation residuals and the rotation residuals ; Step 9: Based on the lidar-inertial odometry factor obtained in Step 4 and the ultra-wideband factor obtained in Step 6, obtain the optimized accurate position and attitude of the UAV.

[0052] The main innovation of the present invention lies in: (1) Ultra-wideband (UWB) position and attitude dynamic quality assessment event trigger mechanism: The present invention adopts an event trigger mechanism based on the quality of the ultra-wideband (UWB) factor, intelligently determines and selects high-quality ultra-wideband (UWB) data to participate in the optimization process, thereby improving the positioning accuracy and reducing the computing burden of the on-board computer.

[0053] (2) Low-cost high-precision positioning: The four-anchor ultra-wideband (UWB) positioning method adopted by the present invention is a low-cost and high-precision positioning method. Through optimizing the algorithm and calculation process, it realizes reducing the hardware cost and computing resource requirements of the system without sacrificing the positioning accuracy, ensuring that the UAV can stably perform tasks in perception-degraded environments such as long corridor environments.

[0054] Example 3: The present invention has been experimentally verified through relevant publicly available datasets that the UAV positioning of this system is more accurate than the original scheme, and the real-time performance of the system is better.

[0055] Select the LIO-SAM open-source dataset. According to the walking trajectory of the robot, use GPS (Global Positioning System) data as the ground truth, and generate simulated four-anchor ultra-wideband (UWB) data based on the ground truth. The method for generating simulation data refers to the communication and ranging performance of NoopLoopUWB. Along the route of the trajectory, set a group of four ultra-wideband (UWB) anchors every 100m. In the real situation, there is noise interference in the ultra-wideband (UWB) anchors, so a Gaussian distribution noise is added to each of the four anchors.

[0056] The Park dataset of the LIO-SAM dataset provides a complete GPS trajectory, which is used as the ground truth reference. In the trajectory display obtained in this experiment, to evaluate the effectiveness of the algorithm of the present invention, in the Park dataset, the method of the present invention optimizes the original trajectory to make it closer to the ground truth. The positioning results are shown in the following table: Table 1 Comparison of positioning accuracy results

[0057] Table 2 shows the processing times of FAST-LIO2, LIO-SAM, LINS, and the method of the present invention in all datasets. The method of the present invention and FAST-LIO2 both integrate the odometer and mapping, and update the map immediately at each step of the odometer update. Therefore, the total time ("total" shown in Table 2) calculates all possible processes that occur in the odometer, including point-to-map matching, state estimation, and mapping. On the other hand, both LIO-SAM and LINS are based on an architecture of an independent odometer (including feature extraction and rough pose estimation) and mapping (such as incremental smoothing and mapping in LIO-SAM and map refinement in LINS), and the average processing time for each LiDAR scan is composed of these two parts ("Odo") and ("Map"). This experiment of the present invention was carried out in the open-source dataset of LIO_SAM, and the calculation times are sorted as shown in Table 2.

[0058] Table 2 Comparison of calculation time results

[0059] Since the method of the present invention additionally uses the UltraWideBand (UWB) factor to improve the positioning accuracy of the unmanned vehicle in a perception-degraded environment, the time of the present invention is not always optimal compared with FAST_LIO2, but the present invention improves the positioning accuracy in a perception-degraded environment. Compared with LINS and LIO_SAM, the real-time performance of the algorithm of the present invention is very excellent.

[0060] Example 4: In the environment, deploy four-anchor UltraWideBand (UWB) anchors. Preferably, the four UWB anchors are deployed in the spatial area where the lidar sensor is prone to degradation. Preferably, two UWB anchors are deployed on each side of the corridor, forming a quadrilateral.

[0061] The unmanned aerial vehicle body is equipped with a lidar sensor, an inertial odometer, UWB, and an on-board computer. The lidar point cloud information, the acceleration, angular velocity, and attitude information of the robot, etc. are collected and transmitted to the on-board computer for processing.

[0062] Step 1: Define the coordinate system of the unmanned aerial vehicle and the world coordinate system.

[0063] The coordinate system of the UAV can be confirmed at startup, and it is necessary to confirm its relationship with the world coordinate system. In this method, the coordinate system of the first frame of point cloud of the lidar is selected as the world coordinate system.

[0064] Step 2: Deploy four-anchor-point ultra-wideband in the perception degradation scenario, and install an ultra-wideband anchor at each of the four points of an approximate square.

[0065] Step 3: Define the state transition model and measurement model required for front-end state estimation.

[0066] Step 4: Use iterative Kalman filtering based on the state transition model and measurement model to obtain the lidar-inertial odometry factor as one of the inputs to the factor graph.

[0067] Step 5: When the UAV passes through the area where the four-anchor-point ultra-wideband is deployed and receives the position and attitude of the UAV measured by the four-anchor-point ultra-wideband, trigger the event trigger mechanism based on the factor quality of the ultra-wideband. After evaluating the factor quality of the ultra-wideband, decide whether it participates in the factor graph optimization.

[0068] Step 6: After determining its availability, perform factor graph optimization together with the lidar-inertial odometry factor. The calculation formula is as follows: ; Finally, calculate this least squares problem to obtain the optimized accurate position and attitude of the UAV.

Claims

1. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment is characterized by: The following steps are involved: Step 1: Define the drone coordinate system and the world coordinate system, and select the first frame point cloud coordinate system of the lidar as the world coordinate system; Step 2: Define the state transition model required for front-end state estimation; Step 3: Use iterative Kalman filter to obtain the front-end lidar-inertial odometer factor; Step 4: Based on the four-anchor UWB, the accurate UAV position and attitude are obtained as UWB factors; Step 5: Based on the ultra-wideband factor obtained in step 4, the position and attitude quality are judged, and an event trigger mechanism is used to determine whether the ultra-wideband factor participates in the factor graph optimization; Step 6: Use factor graph optimization to obtain the precise optimized position and attitude of the drone.

2. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 1 is characterized in that: The step 1 includes radar-inertial odometer and ultra-wideband positioning factors; Define the world coordinate system W and the drone coordinate system B. The drone is at time Status Including the information about the attitude and speed of the drone, expressed as: ； In the formula, represents the rotation matrix, Indicates the position of the drone. Indicates speed, Represents the IMU bias; the transformation matrix from the drone coordinate system B to the world coordinate system W It is expressed as: .

3. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 1 is characterized in that: The state estimation in step 2 uses a tightly coupled Kalman filter, including a state transfer model and a measurement model. The state transfer model is as follows: ； In the formula, Indicates time status, is the time interval between two moments, is the state transition function, depending on the current state , control input and process noise .

4. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 3 is characterized in that: The step 2 also includes defining the measurement model required for front-end state estimation. The measurement model of radar data is as follows: ； In the formula, Indicates the radar coordinate system The real coordinates of the points, Indicates the radar coordinate system The measured coordinates of the points, Indicates The measurement noise of each point.

5. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 4 is characterized in that: The step 3 includes using an iterative Kalman filter to obtain a tightly coupled attitude estimation of the UAV based on a lidar-inertial odometer; Ideally, the The relationship between a point and a reference plane or reference point should satisfy certain constraints, so that The value of the function is zero, and the optimization problem is as follows: ； In the formula, the state variable Adjustments are made by minimizing this residual to ensure that the radar points are correctly projected onto the planar segments of the global map; The method using the tightly coupled extended Kalman filter framework first uses the IMU data to propagate state estimates when there is no new radar data, maintaining a continuous estimate of the robot state. Upon receiving new radar data, the system performs iterative updates to refine the state prediction to align it with the actual measurements, and calculates the residual between the predicted and measured values ​​to iteratively adjust the estimate until it converges to a stable state.

6. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 5 is characterized in that: In step 3, the front-end lidar-inertial odometer factor required for the attitude estimation conversion output by the Kalman filter is converged. After convergence, the optimal state estimation and covariance formula are as follows: ； In the formula, Indicates the status before the update. Indicates the updated status. represents the updated covariance matrix of the error, is the Kalman gain, is the identity matrix, is the measurement matrix that maps the state variables to the measurement space, is the error covariance matrix.

7. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 6 is characterized in that: In step 4, the covariance of the attitude information derived from the ultra-wideband is evaluated; a high covariance indicates that the data is unavailable; After deploying the UWB anchors, we first obtain prior knowledge by manually measuring the angular orientation between the base station and the world coordinate system, and then implement the corresponding projection transformation. The quality of the received UWB measurement data must be ensured. Once the UWB measurement data is verified, it is linearly interpolated according to the timestamp of the lidar to synchronize with the timestamp of the radar-inertial odometer attitude information.

8. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 7 is characterized in that: In step 5, after the event is triggered, the ultra-wideband factor is integrated into the optimization process; The constructed factor graph includes two factor types and one variable type. The variable represents the state of the robot at a specific time interval. The factors include: radar-inertial odometer attitude factor and ultra-wideband attitude factor. When the robot approaches the ultra-wideband setting of four anchor points, the covariance evaluation of the attitude information derived from the ultra-wideband is performed; high covariance indicates that the data is unavailable. The valid position and attitude information is integrated as ultra-wideband factors at intervals of 5 meters. The factor graph uses incremental smoothing and Bayesian tree for post-node insertion optimization.

9. The UAV fusion positioning method based on four-anchor UWB posture dynamic quality assessment according to claim 8 is characterized in that: The factor graph optimization is used in step 6, and the specific formula is as follows: Factor graph optimization is ultimately transformed into a nonlinear least squares problem: ； in, Indicates the total number of UWB anchor points ； and They represent the number of UWB measurement residuals and LiDAR-SLAM measurement residuals respectively; Used to identify UWB anchor points; and The corresponding indices of the UWB factor and the LiDAR-SLAM factor in the factor graph, respectively, are the measurement residuals associated with the UWB sensor and the range residuals are expressed as ; Measurement residuals associated with the lidar sensor, including translation residuals and the rotation residual ; Finally, the least squares problem is calculated to obtain the precise optimized position and attitude of the UAV.

Images