Unmanned aerial vehicle autonomous positioning method in satellite positioning denial environment

Abstract

The invention discloses an unmanned aerial vehicle autonomous positioning method in a satellite positioning denial environment, and the method comprises the steps: 1, predicting the motion state information of an unmanned aerial vehicle through an IMU, and obtaining the predicted pose of the unmanned aerial vehicle; 2, obtaining the relative variation of the position and attitude of the laser radar between two continuous time points; 3, constructing a mathematical model for optimizing the poses according to the predicted poses of the unmanned aerial vehicle with different weights and the relative variation of the laser radar; 4, fitting the optimal weight of the mathematical model by using a least square method, and further obtaining an optimized pose through the optimal weight; through tight coupling of the laser radar and the IMU, data of the laser radar and the IMU are fused at a more bottom layer and a more compact layer, complementary information between the data is fully mined and utilized, errors are reduced, positioning precision and reliability are improved, and particularly stable performance is kept in a complex environment and long-distance flight.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous positioning of unmanned aerial vehicles (UAVs), and in particular relates to an autonomous positioning method for UAVs in a satellite positioning denial environment. Background Art

[0002] Traditional satellite positioning systems (such as GPS) cannot function properly in environments where satellite signals are jammed or unreceivable (such as urban canyons, mountainous areas, indoors, or in hostile environments). This significantly limits the autonomous flight capabilities of drones. To overcome this problem, it is important to develop autonomous positioning technologies that do not rely on satellite signals. Visual positioning technology is a common alternative, but it has high requirements for ambient lighting conditions and textures and is prone to mismatching in dynamic environments. Furthermore, while existing loosely coupled LiDAR and IMU positioning technologies can provide certain positioning capabilities, their accuracy and reliability in complex environments still need to be improved.

[0003] In existing technologies, lidar data processing independently processes the three-dimensional point cloud data acquired by the lidar, performing operations such as filtering and downsampling to extract feature points or line features for calculating relative pose changes. IMU data processing independently processes IMU data and infers the attitude and position changes of the drone through integration operations. However, IMU data is susceptible to various interference factors, especially accelerometer errors, which accumulate over time, resulting in reduced navigation accuracy. At the back end, frameworks such as Kalman filtering and particle filtering are used to fuse the lidar's odometry results with the IMU's dead reckoning results, ultimately outputting the positioning result.

[0004] Therefore, existing technologies lack close information fusion, resulting in limited accuracy and reliability, making it difficult to fully leverage the complementary advantages of lidar and IMU. For example, in vehicle positioning, while the loose coupling strategy is simple in principle and easy to implement, it is somewhat insufficient in terms of efficient data utilization, and the cumulative effect of IMU measurement errors can weaken the algorithm's accuracy.

[0005] In loosely coupled methods, lidar and IMU data are processed independently before being fused. This fusion approach is relatively simple and fails to fully leverage the complementary advantages of the two data types. For example, in vehicle positioning, while the loosely coupled strategy is simple in principle and easy to implement, it does not efficiently utilize data, and the cumulative effect of IMU measurement errors can weaken the algorithm's accuracy. Loosely coupled algorithms are highly dependent on the lidar's point cloud matching results. In environments with unclear or dynamic features, point cloud matching is prone to errors or failures, affecting the stability of the entire positioning system.

[0006] Errors in IMU data accumulate over time, causing positioning errors to gradually increase. Loosely coupled algorithms cannot effectively address this issue, affecting long-term positioning stability. The IMU measures acceleration and angular velocity to infer the drone's attitude and position changes, but these measurements are subject to various interference factors, such as sensor noise and temperature fluctuations. As integration time increases, these errors accumulate, causing positioning results to gradually deviate from the true position. In loosely coupled systems, while lidar can correct IMU errors to a certain extent, this correction is limited due to the lack of close fusion between the two data.

[0007] In dynamic scenes, loosely coupled algorithms struggle to effectively distinguish between dynamic objects and the static environment, and are easily affected by these objects, leading to deviations in positioning results. For example, on city streets, vehicles and pedestrians move around, and the point cloud data captured by LiDAR includes information about these dynamic objects. Loosely coupled algorithms often struggle to accurately identify and eliminate the influence of these dynamic objects, treating them as part of the environment. This can lead to errors in point cloud matching and, in turn, affect the accuracy of positioning results. Summary of the Invention

[0008] The purpose of the present invention is to provide a method for autonomous positioning of unmanned aerial vehicles in a satellite positioning denied environment, so as to solve the problem of poor accuracy of existing loosely coupled positioning of lidar and IMU in complex environments.

[0009] The present invention adopts the following technical solution: a method for autonomous positioning of a UAV in a satellite positioning denial environment, comprising:

[0010] Step 1: Use IMU to predict the UAV’s motion state information, and then obtain the UAV’s predicted position and posture;

[0011] Step 2: Obtain the relative change in position and attitude of the lidar between two consecutive time points;

[0012] Step 3: Construct a mathematical model for optimizing the posture using the predicted posture of the drone and the relative changes of the lidar with different weights;

[0013] Step 4: Use the least squares method to fit the optimal weight of the mathematical model, and then obtain the optimized pose through the optimal weight.

[0014] Furthermore, the mathematical model for optimizing the posture is:

[0015]

[0016] Where, T Opt,i (k1, k1) is the optimized pose of the i-th frame, obtained by weighted fusion of IMU and LiDAR data; T True,iis the true pose of the i-th frame obtained through external calibration or high-precision benchmark; i is the data frame index, indicating the i-th time point in the sliding window; k1, k2 are the weight coefficients to be optimized, and k1+k2=1;

[0017] Among them, T Opt =K1 T IMU +K2ΔT Lidar ;

[0018] Among them, T IMU =(x IMU ,y IMU ,z IMU ,θ x,IMU ,θ y,IMU ,θ z,IMU ); where (x IMU ,y IMU ,z IMU ) is the position of the UAV predicted by the IMU through the integrated acceleration in the three-dimensional coordinate system, m; (θ x,IMU ,θ y,IMU ,θ z,IMU ) is the Euler angle attitude predicted by the IMU through the integrated angular velocity in the three-dimensional coordinate system, in radians;

[0019] Where, ΔT Lidar =(Δx Lidar ,Δy Lidar ,Δz Lidar ,Δθ x,Lidar ,Δθ y,Lidar ,Δθ z,Lidar )where (Δx Lidar ,Δy Lidar ,Δz Lidar ) is the position change calculated by LiDAR through matching two consecutive frames of point cloud in the three-dimensional coordinate system, meters; (Δθ x,Lidar ,Δθ y,Lidar ,Δθ z,Lidar ) is the attitude change calculated by LiDAR in the three-dimensional coordinate system, in radians.

[0020] The beneficial effects of the present invention are:

[0021] The present invention tightly couples the lidar and the IMU to fuse their data at a lower, more intimate level, fully tapping and utilizing the complementary information between the data, reducing errors, and improving positioning accuracy and reliability, especially maintaining stable performance in complex environments and long-distance flights. In tight coupling, the IMU's high-frequency data can be used to compensate for the lidar's motion distortion in real time, while the lidar's high-precision geometric information can be used to correct the IMU's errors in real time, thereby improving overall positioning accuracy.

[0022] The present invention adopts a multi-sensor data fusion algorithm, combining the geometric features of the lidar and the motion features of the IMU to build a more robust positioning model. When the environmental features are not obvious, the weight of the IMU data in the fusion process can be increased, and when the environmental features are rich, the weight of the lidar data can be increased. This reduces the excessive reliance on the accuracy of the lidar point cloud matching results, enhances the adaptability and stability in environments with unclear or dynamic environmental features, and enables the drone to reliably perform autonomous positioning in various environments. For example, in foggy or indoor environments, the IMU motion data can be better utilized to compensate for the lack of lidar point cloud features through a tightly coupled algorithm;

[0023] This invention effectively limits IMU error accumulation by using lidar data to promptly correct IMU errors through a tightly coupled algorithm, suppressing their growth and improving long-term positioning stability. For example, by estimating the IMU's bias and scale errors in real time and using the absolute position information from the lidar to make corrections, error accumulation is reduced.

[0024] This invention utilizes more advanced data fusion and processing algorithms to effectively identify and eliminate the influence of dynamic objects, improving positioning accuracy and reliability in dynamic scenes. For example, in dynamic scenes such as city streets, the tightly coupled algorithm can identify and eliminate point cloud data of dynamic objects in real time, thereby improving the accuracy of point cloud matching and the reliability of positioning results. DETAILED DESCRIPTION

[0025] The present invention is described in detail below with reference to specific embodiments.

[0026] The present invention discloses a method for autonomous positioning of a UAV in a satellite positioning denial environment, comprising:

[0027] Step 1: Use IMU to predict the UAV’s motion state information, and then obtain the UAV’s predicted position and posture;

[0028] Step 2: Obtain the relative change in position and attitude of the lidar between two consecutive time points;

[0029] Step 3: Construct a mathematical model for optimizing the posture using the predicted posture of the drone and the relative changes of the lidar with different weights;

[0030] Step 4: Use the least squares method to fit the optimal weight of the mathematical model, and then obtain the optimized pose through the optimal weight.

[0031] The technical solution of this invention consists of four main components: front-end IMU measurement processing, front-end radar point cloud feature extraction and matching, back-end optimization, and map layer matching. Through the coordinated work of these four components, autonomous positioning of drones is achieved in environments without satellite signals.

[0032] Front-end IMU measurement processing part:

[0033] State prediction: The high-frequency state of the drone is predicted by integrating the IMU input (angular velocity and acceleration). This state prediction is used to guide point cloud preprocessing and help quickly locate feature points. It also increases the frequency of odometer output, allowing for more timely updates of the drone's position and posture. Its mathematical expression can be expressed as:

[0034]

[0035] Here, p, v, and q represent the predicted values ​​of position, velocity, and attitude, respectively. Δt is the time interval, a is the acceleration, and ω is the angular velocity. This approach leverages the high-frequency characteristics of the IMU to provide high-frequency drone state predictions for subsequent processing, enhancing the system's real-time performance.

[0036] Pre-integration: IMU inputs are used to construct incremental position (P), velocity (V), and attitude (Q) constraints between two frames, known as IMU pre-integration constraints. This reduces computational complexity and provides relative motion constraints between two frames, enhancing system stability. The core concept is to integrate IMU measurements based on Newton's laws of motion to determine the relative motion change between the two frames. This provides important prior information for back-end optimization, helping to more accurately estimate the drone's motion state.

[0037]

[0038] Using the above pre-integration formula to calculate the relative motion increment between two frames of LiDAR data, rather than the absolute state, helps avoid computational redundancy caused by repeated integration. Compared to traditional loose coupling, it can serve as a key constraint for tight coupling, directly participating in back-end optimization and jointly optimizing the pose with LiDAR feature constraints.

[0039] Point cloud preprocessing: The raw radar point cloud is projected into a depth image for subsequent processing. Simultaneously, ground point and target segmentation is performed rapidly, outliers are removed, and feature points are extracted. This step aims to filter useful feature information from the raw point cloud to improve the efficiency and accuracy of subsequent processing. For example, ground points can be identified and removed based on their height information to reduce computational complexity. Feature points can then be extracted using features such as point cloud density and curvature, providing key points for subsequent registration.

[0040] Feature point constraint construction: Feature points are extracted from the preprocessed point cloud and observation constraints are constructed. These constraints are used in back-end joint optimization to provide key environmental feature information for drone pose estimation. Specifically, feature point constraints reflect the geometric relationship between the same feature points observed by the drone at different locations, helping to accurately determine the relative pose changes of the drone.

[0041] Ground Constraint Construction: During preprocessing, ground constraints are constructed by identifying ground points and leveraging their properties. These constraints help stabilize the drone's attitude estimation, particularly in the vertical direction. For example, during flight, ground constraints can help the drone maintain a level flight attitude, avoiding positioning errors caused by attitude tilt.

[0042] Sliding Window Construction and State Storage: A sliding window is constructed to store multiple frames of drone state. The window size can be adjusted based on computing resources and accuracy requirements. The sliding window stores information such as the drone's position, velocity, and attitude at multiple moments in time, providing a data foundation for subsequent joint optimization.

[0043] Joint Optimization: Using the observation constraints (including feature point constraints, ground constraints, and IMU pre-integration constraints) constructed from the current frame and historical frames, all states in the sliding window are jointly optimized. The goal of the optimization is to minimize the error of all constraints, thereby obtaining a more accurate estimate of the drone state. The optimization problem can be expressed as the following function:

[0044]

[0045] Among them, h IMU and h Lidar where z represents the IMU pre-integration and lidar feature observation models, z represents the actual measurement value, and Σ represents the covariance matrix. This allows for the optimal drone state estimation. After optimization, the high-frequency state obtained from the state prediction phase is refreshed and output as high-frequency state information. This step offers the advantage of comprehensively considering the constraints between multiple frames of data, effectively reducing cumulative error and improving positioning accuracy.

[0046] Pose Fusion: Fusion of high-frequency states obtained through back-end optimization with map layer matching results further improves the accuracy and reliability of drone pose estimation. For example, weighted averaging or other fusion algorithms can be used to combine pose information obtained through map layer matching with back-end optimization results to obtain the final drone pose.

[0047] Map-level matching: This process aligns the corrected feature points with the local map, optimizing the pose estimation and updating the local map. Map-level matching determines the drone's position within the map by comparing the similarity between the feature points in the current frame and those stored in the map. Simultaneously, the local map is updated based on the matching results to incorporate the latest environmental information. For example, methods such as nearest neighbor matching and feature descriptor matching can be used to align feature points with the map. This helps the drone continuously update its understanding of its surroundings during flight, improving the accuracy and completeness of its map.

[0048] Point cloud map output: Output a globally consistent point cloud map to provide environmental information support for UAV path planning, obstacle avoidance and other tasks.

[0049] Loop closure detection: During the map construction process, we check whether a trajectory is closed. This is achieved by comparing feature similarity between the current frame and previous frames. Once a closed loop is detected, the point cloud registration result at the closed location is used as a loop constraint and inserted into the factor graph. Loop closure detection helps correct accumulated errors and improve the global consistency of the map. For example, methods such as bag-of-words models and sequence similarity detection can be used to quickly detect loops.

[0050] Global Optimization: Whenever a new closed-loop constraint is inserted, the system triggers global optimization, adjusting the entire map and drone trajectory to ensure global consistency. Global optimization can use graph optimization methods, representing the map and trajectory as a graph structure and reducing errors by optimizing the nodes and edges of the graph. The benefit of this step is that it can effectively eliminate errors accumulated during long-term flight and ensure the global accuracy of the map and trajectory. The calculation formula is as follows:

[0051]

[0052] Where, T ij is the pose transformation matrix of the detected loop node, T ik and T kj The advantage of this method is that it maps the pose error to the tangent space through Lie algebra (log), which facilitates optimization calculation. In addition, it triggers global optimization after the closed-loop constraint is inserted into the factor graph, eliminating long-term accumulated errors.

[0053] Factor Graph Construction: The system maintains a global factor graph, into which each module inserts different types of factors, including IMU pre-integration factors, registration factors, and loop closure factors. The factor graph graphically represents the constraints between drone states, facilitating optimization calculations.

[0054] Parallel Optimization Calculation: Whenever a new factor node is inserted, the system performs an optimization calculation. To ensure real-time performance, the system runs in parallel on three threads: preprocessing and laser odometry occupy the main thread, and map construction and loop closure each occupy a separate thread. Parallel computing can fully utilize computing resources, improve system efficiency, and ensure that real-time performance requirements are met. The specific calculation formula is as follows:

[0055]

[0056] Where, e ij Represents a node in the factor graph (state X i , X j ), and compared with the local optimization of traditional filtering methods, this formula is beneficial to multi-threaded parallel optimization and joint optimization of all constraints, ensuring the global optimization of maps and trajectories.

[0057] Original point cloud projection: Projecting the original point cloud data into a depth image for subsequent processing. This step takes advantage of image processing technology to quickly process and analyze the point cloud.

[0058] Ground point and target segmentation: By analyzing features such as point cloud height information, ground points and target points can be quickly identified and removed, reducing the computational complexity of subsequent processing. For example, ground points typically have lower height values ​​and higher density, which can be filtered out by setting a threshold.

[0059] Outlier removal: Use statistical analysis or other methods to remove outliers from the point cloud and improve data quality. For example, calculate the neighborhood density of each point in the point cloud and identify points with a density below a certain threshold as outliers and remove them.

[0060] Feature point extraction: Extract feature points from the segmented point cloud to provide key points for subsequent registration and optimization. Feature point extraction methods include extraction algorithms based on geometric features (such as curvature, normal direction, etc.) and extraction algorithms based on statistical features.

[0061] Feature point registration: Align feature points at consecutive moments to estimate the relative motion of the 3D LiDAR. This step calculates the LiDAR's motion changes by comparing the geometric relationships between feature points at different moments. Registration methods can use the Iterative Closest Point (ICP) algorithm and its variants, or feature descriptor-based matching algorithms.

[0062] Motion Estimation Correction: Based on the feature point registration results, the feature points are corrected again to improve the accuracy of motion estimation. This step uses a feedback mechanism to adjust the feature points using the motion information obtained from the registration to reduce error accumulation.

[0063] Feature point registration with the local map: The corrected feature points are registered with the local map to further optimize the pose estimation results. This step determines the drone's position in the map by matching the feature points of the current frame with the feature points stored in the map and updates the local map.

[0064] Local map update: Based on the registration results, the local map is updated to include the latest environmental information. This helps the drone continuously update its understanding of the surrounding environment during flight, providing accurate map support for subsequent positioning and navigation.

[0065] Loop closure detection: This step checks whether the drone's flight trajectory is closed. Once a closed loop is detected, the point cloud registration result at the closed loop is used as the closed loop constraint. This step helps correct accumulated errors and improve the global consistency of the map.

[0066] Inserting closed-loop constraints into the factor graph: Inserting closed-loop constraints into the factor graph triggers global optimization computation. This step, by incorporating closed-loop constraints into the factor graph, enables the system to reoptimize the entire map and trajectory based on the new constraint relationships.

[0067] Factor Insertion and Optimization: Each module inserts its corresponding factor into the factor graph. Each time a new factor node is inserted, the system performs an optimization calculation. The goal of optimization is to minimize the error across all factors and achieve optimal drone state estimation and map construction. This step utilizes graph optimization methods, comprehensively considering various constraints to ensure global optimality of the system.

[0068] Multi-threaded parallel computing: The system runs in parallel on three threads: preprocessing and laser odometry occupy the main thread, while map building and loop closure each occupy a separate thread. Parallel computing improves system efficiency and ensures that real-time requirements are met.

[0069] UAV platform selection and assembly: A small multi-rotor unmanned aerial vehicle (UAV) platform with four rotors, a 350mm wheelbase, and a brushless motor power system was selected to ensure good flight stability and payload capacity. This platform served as a sensor carrier and experimental data acquisition platform.

[0070] Open-source flight control system installation and configuration: Install an open-source flight control system based on the STM32 architecture. This system is responsible for flight attitude control and the acquisition and output of IMU data. This open-source flight control system allows for flexible adjustment of flight parameters and precise control of the drone's flight attitude.

[0071] Edge computing module integration: The NVIDIA Xavier NX edge computing module is integrated into the drone system, providing powerful computing power for sensor data fusion processing. This module can process data from various sensors in real time, enabling rapid data fusion and analysis.

[0072] Sensor Installation and Connection: Install a 3D LiDAR (Mid 360) and a depth camera (Intel T265) and connect them to the edge computing module. The 3D LiDAR is used to obtain 3D point cloud data of the environment, while the depth camera is used to obtain depth image data, serving as a backup system to improve data acquisition reliability and redundancy.

[0073] System debugging and testing: After hardware installation, the entire hardware system is debugged and tested to ensure that all hardware components work together and that data transmission is stable and reliable. Actual flight tests are conducted to verify the performance and stability of the hardware system.

[0074] Developing drone positioning and control technology in satellite-denied environments requires implementing sensor functionality and edge computing module data processing capabilities within a software development environment. The software development environment project team selected the ROS development environment for their development. ROS (Robot Operating System) is an open-source platform for writing robotics software. It evolved from the Switchyard project, established by the Stanford University Artificial Intelligence Laboratory to support its AI robotics projects. Currently, ROS has become the most widely used robotics software development platform in the robotics research community.

[0075] After establishing the project's software development environment, in addition to developing sensor functions within the software development environment, the team also needed to build a simulation environment. The project team used Gazebo, a 3D dynamic simulator capable of creating complex indoor and outdoor environments, enabling the simulated robot to accurately and efficiently simulate its motion. Similar to a game engine, Gazebo provides a high degree of fidelity to real-world scenarios, along with a suite of sensor, user, and program interfaces. Gazebo makes it easy to visualize robot models, test robot algorithms, and perform regression testing using real-world scenarios.

[0076] Operating system and development environment configuration: Install the Linux operating system on the hardware platform and configure the ROS development environment. This includes installing ROS core components, setting environment variables, and configuring the workspace. ROS facilitates node management, message passing, and data processing.

[0077] Sensor driver and data acquisition node development: Develop corresponding ROS driver nodes for installed sensors such as 3D lidar, depth cameras, and IMUs to collect and publish sensor data. These nodes convert sensor data into ROS messages and transmit them within the ROS network.

[0078] Data fusion and processing algorithm development: Based on the ROS framework, we develop data fusion and processing algorithm nodes. These nodes receive raw data from sensors and perform data preprocessing, feature extraction, data fusion, and other operations to achieve drone pose estimation and environmental perception.

[0079] Control algorithm development: Develop flight control algorithm nodes to generate control commands based on the fused pose estimation information and environmental perception results, enabling autonomous flight control of the drone. The control algorithm can include functions such as trajectory tracking, obstacle avoidance, and path planning.

[0080] Simulation Environment Construction and Testing: Use the Gazebo simulation environment to build various test scenarios. Compile .world XML files to create complex environments such as indoor environments, densely populated buildings, caves, and collapsed structures, and configure external conditions such as lighting and weather conditions. Integrate the developed software system into the Gazebo simulation environment for algorithm testing and debugging. Through simulation testing, algorithm performance and stability can be quickly verified, reducing the risks and costs of actual flight testing.

[0081] After building the hardware platform and software environment, they were integrated to enable autonomous positioning and flight control for drones in satellite-denied environments. The system integration process included hardware interface connections, software node configuration, and data flow debugging, ensuring seamless collaboration between hardware and software.

[0082] Integration testing process

[0083] Hardware interface connection and configuration: Connect the various components of the hardware platform through corresponding interfaces, including the communication interfaces between sensors and edge computing modules, and between the flight control system and edge computing modules. Configure hardware interface parameters to ensure accurate and real-time data transmission.

[0084] Software node configuration and communication: In the ROS environment, configure the parameters of each software node and establish communication relationships between nodes. Through the topic publishing and subscription mechanism, sensor data is transmitted from the acquisition node to the processing node, and control instructions are transmitted from the control node to the flight control system.

[0085] Data flow debugging and optimization: Debug the data flow throughout the system to check whether data is transmitted smoothly between each link and whether there is any delay or loss. Based on the debugging results, optimize the data processing algorithm and communication parameters to improve the real-time performance and reliability of the system.

[0086] Functional Testing and Performance Evaluation: After the system is integrated, various functional tests are conducted, including autonomous positioning, obstacle avoidance, path planning, and tracking. Through actual flight tests and simulation tests, system performance indicators such as positioning accuracy, flight stability, and response speed are evaluated. Based on the test results, the system is further optimized and improved.

[0087] This invention fuses lidar and IMU data at a lower, more intimate level, leveraging their complementary strengths to reduce errors and improve positioning accuracy and reliability. Specifically, it leverages the high-frequency characteristics of the IMU and the high-precision geometric information of the lidar to achieve high-precision autonomous positioning in complex environments.

[0088] In the front-end IMU measurement processing, the drone's high-frequency state is predicted by integrating IMU data, and IMU pre-integration is used to construct PVQ incremental constraints between two frames. In the front-end radar point cloud feature extraction and matching, the raw radar point cloud is preprocessed to extract feature points and construct observation constraints. In the back-end optimization, sliding window technology and joint optimization methods are used to comprehensively consider the constraints between multiple frames of data to optimize the drone's state estimation.

[0089] To ensure real-time performance, the system runs in parallel on three threads: preprocessing and laser odometry occupy the main thread, while map construction and loop closure each occupy a separate thread. This parallel computing approach fully utilizes computing resources, improves system efficiency, and ensures that real-time performance requirements are met.

[0090] Through the factor graph optimization module, each module inserts the corresponding factor into the factor graph. Each time a new factor node is inserted, the system will perform an optimization calculation. The factor graph graphically represents the constraint relationship between the drone states, facilitating optimization calculations.

[0091] During the map construction process, the present invention detects whether the trajectory is closed. Once a closed loop is detected, the point cloud registration result at the closed position is inserted into the factor graph as a closed loop constraint, triggering global optimization, correcting accumulated errors, and improving the global consistency of the map.

[0092] This invention uses methods such as bag-of-words models and sequence similarity detection to quickly detect loops. Whenever a new closed-loop constraint is inserted, the system triggers a global optimization, adjusting the entire map and drone trajectory to ensure global consistency.

[0093] Based on a small multi-rotor unmanned aerial vehicle platform, this invention builds a complete hardware development environment, including an edge computing module, a three-dimensional lidar, a depth camera, etc., and realizes the development of sensor functions and data self-processing functions through the ROS development environment and the Gazebo simulation environment.

[0094] The hardware platform includes a small multi-rotor unmanned aerial vehicle (UAV), an open-source flight control system (including an IMU), an edge computing module, a 3D lidar, and a depth camera. The software environment, developed based on ROS, enables sensor data acquisition, processing, fusion, and control algorithm development. The algorithms are tested and verified using the Gazebo simulation environment.

[0095] Through the edge computing module and tightly coupled algorithm, the present invention enables the UAV to achieve autonomous perception, decision-making and execution in a satellite positioning denial environment, and has the ability to adapt to and self-process faults and obstacles.

[0096] On the hardware platform, the edge computing module provides computing power to each sensor, enabling real-time data processing and decision-making. In the software algorithm, through tightly coupled data fusion and optimization algorithms, the drone can make autonomous decisions and path planning based on environmental perception results, fly along the self-determined path, and perform self-analysis.

[0097] The present invention constructs and outputs a globally consistent high-precision point cloud map through map layer matching and factor graph optimization, providing accurate environmental information support for UAV path planning, obstacle avoidance and other tasks.

[0098] In the map layer matching phase, the corrected feature points are registered with the local map, the pose estimation results are optimized, and the local map is updated. Factor graph optimization is used to ensure the global consistency and accuracy of the map.

[0099] The present invention deeply integrates the high-precision geometric information of the lidar with the high-frequency motion data of the IMU through the tightly coupled data fusion of the lidar, significantly improving the positioning accuracy and reliability, and effectively solving the problem of autonomous positioning of drones in complex environments. At the same time, in satellite-denied environments, the present invention demonstrates strong adaptability and can cope with problems such as unclear environmental features, dynamic interference, and poor lighting conditions in complex scenes such as urban canyons, mountainous areas, and indoors, and can achieve stable flight and precise positioning without relying on external satellite signals. In addition, through a multi-threaded parallel optimization computing strategy, the system runs in parallel on three threads, scientifically allocates computing resources, and modules such as preprocessing, laser odometry, map construction, and closed loops work together to meet the needs of real-time positioning and control of drones and ensure the smooth execution of flight missions. The present invention also introduces loop detection and global optimization mechanisms to promptly detect and correct accumulated errors, ensure the global consistency of map construction, and keep the positioning results stable over the long term.

[0100] Experimental results show that the proposed method outperforms traditional loosely coupled methods in positioning accuracy across a wide range of environments. The average positioning error is reduced by approximately 40%. In urban canyon environments, the positioning error is reduced from 2.5 meters to 1.2 meters; in indoor environments, the positioning error is reduced from 1.8 meters to 0.8 meters.

[0101] The system demonstrated excellent stability during extended flight, with minimal fluctuations in positioning errors and no significant cumulative effects. A loop detection success rate reached 92%, significantly improving the global consistency of the map. Multi-threaded parallel computing ensured real-time performance, with data processing latency kept to under 50 milliseconds, meeting the real-time positioning requirements of drones.

[0102] Example: Extract feature points from the point cloud data of a light radar. These feature points can be corner points, edge points, or other significant geometric feature points. Assume that m feature points are extracted, and the coordinates of each feature point are:

[0103] p i =(x i ,y i , z i ), where i = 1, 2, ..., m.

[0104] The feature points of the previous frame are matched with the feature points in the map to obtain the relative pose change. Assume that the coordinates of the feature point in the map are p′ i =(x′ i ,y′ i ,z′ i ) Through the ICP matching algorithm, we can get the pose change of the current frame relative to the map ΔT = (Δx, Δy, Δz, Δθ x ,Δθ y ,Δθz ), in order to fuse the IMU predicted pose and the relative pose change of the lidar, a linear polynomial model can be used. Assuming that the optimized pose T Opt It can be expressed as: T Opt =k1T IMU +k2ΔT Lidar , where k1 and k2 are fusion coefficients, and the least squares method is used to calculate the parameters.

[0105] The pose predicted by the IMU, including position and attitude:

[0106] T IMU =(x IMU ,y IMU , z IMU ,θ x，IMU ,θ y，IMU ,θ z，IMU )

[0107] Relative pose change of lidar:

[0108] ΔT Lidar =(Δx Lidar ,Δy Lidar ,Δz Lidar ,Δθ x,Lidar ,Δθ y,Lidar ,Δθ z,Lidar )

[0109] Optimized pose:

[0110] T Opt =(x Opt ,y Otp ,z Opt ,θ x,Opt ,θ y,Opt ,θ z,Opt )

[0111] Each component of the optimized pose is expressed as a linear combination of the IMU predicted pose and the relative pose change of the lidar: Coefficient of determination By collecting multiple sets of data, the least square method is used to fit the optimal coefficient. Through 100 sets of data, each set of data includes T IMU , ΔT Lidar and the true pose T True , construct the following optimization problem:

[0112]

[0113] The coefficients obtained by least squares fitting are:

[0114] k 1x =0.7, k2x =0.3

[0115] k 1y =0.6, k 2y =0.4

[0116] k 1z =0.8, k 2z =0.2

[0117]

[0118] The optimized pose can be expressed as:

[0119] x Opt =0.7x IMU +0.3Δx Lidar

[0120] y opt =0.6y IMU +0.4Δy Lidar

[0121] z Opt =0.8z IMU +0.2Δ zLidar

[0122] θ x，Opt =0.9θ x，IMU +0.1Δθ x，Lidar

[0123] θ y，Opt =0.85θ y，IMU +0.15Δθ p，Lidar

[0124] θ z，Opt =0.95θ z，IMU +0.05Δθ z，Lidar

[0125] Specific calculations are performed for each component:

[0126] The thresholds for each component are:

[0127] These thresholds ensure sufficient accuracy while allowing a certain degree of error tolerance to avoid frequent switching of sensor-dominant modes due to small measurement noise, and it allows small attitude adjustments while ensuring timely response when attitude changes significantly. When more than half of the results of the component calculations are greater than the threshold, the radar is used as the dominant mode, and if less than half are used, the IMU is used as the dominant mode.

[0128] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for autonomous positioning of unmanned aerial vehicles in a satellite positioning denial environment, characterized in that: include: Step 1: Use IMU to predict the UAV’s motion state information, and then obtain the UAV’s predicted position and posture; Step 2: Obtain the relative change in position and attitude of the lidar between two consecutive time points; Step 3: Construct a mathematical model for optimizing the posture using the predicted posture of the drone and the relative changes of the lidar with different weights; Step 4: Use the least squares method to fit the optimal weight of the mathematical model, and then obtain the optimized pose through the optimal weight.

2. The method for autonomous positioning of a UAV in a satellite positioning denial environment according to claim 1, characterized in that: The mathematical model of the optimized posture is: Where, T Opt,i (k1, k1) is the optimized pose of the i-th frame, obtained by weighted fusion of IMU and LiDAR data; T True,i is the true pose of the i-th frame obtained through external calibration or high-precision benchmark; i is the data frame index, indicating the i-th time point in the sliding window; k1, k2 are weight coefficients to be optimized, and k1+k2=1; Among them, T Opt =K1 T IMU +K2ΔT Lidar ; Among them, T IMU =(x IMU ,y IMU ,z IMU ,θ x,IMU ,θ y,IMU ,θ z,IMU ); where (x IMU ,y IMU ,z IMU ) is the position of the UAV predicted by the IMU through the integrated acceleration in the three-dimensional coordinate system, m; (θ x,IMU ,θ y,IMU ,θ z,IMU ) is the Euler angle attitude predicted by the IMU through the integrated angular velocity in the three-dimensional coordinate system, in radians; Where, ΔT Lidar =(Δx Lidar ,Δy Lidar ,Δz Lidar ,Δθ x,Lidar ,Δθ y,Lidar ,Δθ z,Lidar )where (Δx Lidar ,Δy Lidar ,Δz Lidar ) is the position change calculated by LiDAR through matching two consecutive frames of point cloud in the three-dimensional coordinate system, meters; (Δθ x,Lidar ,Δθ y,Lidar ,Δθ z,Lidar ) is the attitude change calculated by LiDAR in the three-dimensional coordinate system, in radians.

Images