An indoor unmanned aerial vehicle multi-sensor fusion navigation positioning method and system

Abstract

The application discloses an indoor unmanned aerial vehicle multi-sensor fusion navigation positioning method and system, belongs to the technical field of unmanned aerial vehicle navigation positioning, and solves the technical problems of low positioning precision, poor robustness of a single sensor, UWB non-line-of-sight (NLOS) ranging deviation, IMU cumulative error and the like in an indoor environment without GNSS. The method comprises the following steps: modeling the UWB ranging deviation under the NLOS condition, describing the multi-modal characteristics of the ranging error through a mixed Gaussian observation model, combining an adaptive measurement noise adjustment (IAE) mechanism and a robust kernel function to suppress abnormal measurement; constructing a bias-aware extended Kalman filter (Bias-Aware EKF) fusion framework, and integrating position, velocity, attitude, IMU zero offset and UWB bias into a unified state vector model; fusing multi-source information of IMU, UWB, height sensor and optical flow to realize high-precision estimation of the state of the unmanned aerial vehicle; and building a simulation and physical verification platform to complete algorithm verification. The application improves the precision, robustness and long-term stability of unmanned aerial vehicle positioning in a complex indoor environment, and can be widely applied to unmanned aerial vehicle operation scenes such as indoor inspection, logistics and security.

Description

Technical Field

[0001] This invention belongs to the field of UAV navigation and positioning technology, specifically relating to an indoor UAV multi-sensor fusion navigation and positioning method and system, which is suitable for complex indoor environments without GNSS signals. It solves problems such as UWB NLOS ranging deviation, IMU cumulative error, and poor robustness of single sensors, and achieves high-precision and high-stability navigation and positioning for UAVs. Background Technology

[0002] Drones are increasingly used in indoor inspection, logistics delivery, security monitoring, and industrial testing. However, the lack of stable GNSS signals in indoor environments, coupled with complex factors such as multipath effects, obstacle obstruction, lighting changes, and electromagnetic interference, poses a significant bottleneck to the autonomous navigation and positioning of drones. A single sensor is insufficient to meet the positioning needs of indoor drones. IMUs offer advantages such as high sampling rates and fast dynamic responses, but suffer from drift errors that accumulate over time. UWB achieves high-precision ranging with its nanosecond-level pulse characteristics, but is susceptible to systematic positive bias due to NLOS propagation. Visual sensors and optical flow sensors rely on environmental texture and are prone to failure in textureless or drastically changing lighting conditions. Altitude sensors can only perform single-axis ranging and cannot provide planar position information.

[0003] Multi-sensor fusion technology, through the framework of "complementary advantages and error cancellation," has become a core means to solve the indoor UAV positioning problem. Among them, fusion algorithms represented by Kalman filtering and its improved versions are widely used. However, traditional fusion algorithms have many shortcomings: First, the modeling of UWB NLOS ranging bias is incomplete. It often assumes that the ranging error follows a zero-mean Gaussian distribution, which cannot characterize the multimodal error characteristics when LOS / NLOS coexist, resulting in serious interference from abnormal measurements on the fusion results. Second, the state-space model only estimates the UAV's motion state (position, velocity, attitude) and does not include IMU zero bias and UWB bias in the joint modeling, making it impossible to achieve online estimation and compensation of bias, resulting in poor long-term positioning accuracy. Third, the robustness of multi-source information fusion is insufficient. It lacks sensor reliability assessment and adaptive weighting mechanisms, which can easily lead to divergence in fusion results when some sensors fail. Fourth, the experimental verification scheme lacks standardized test scenarios and evaluation indicators, making it difficult to verify the engineering feasibility and generalization ability of the algorithm.

[0004] In existing technologies, suppression of UWB NLOS errors often employs single robust filtering or fixed-threshold discrimination methods, resulting in limited detection rates and suppression effectiveness. Multi-sensor fusion typically uses loose coupling, failing to fully exploit the complementary characteristics of each sensor and neglecting the temporal and spatial synchronization requirements of the sensors. Therefore, there is an urgent need for an indoor UAV navigation and positioning method capable of accurately modeling UWB NLOS bias, achieving joint state estimation of multiple sensors, and possessing adaptive robust fusion capabilities, to improve positioning accuracy and stability in complex environments. Summary of the Invention

[0005] This invention addresses the shortcomings of existing indoor UAV navigation and positioning technologies by providing an indoor UAV multi-sensor fusion navigation and positioning method and system. It solves problems such as UWB NLOS ranging deviation, IMU cumulative error, and poor robustness of multi-sensor fusion, achieving high-precision, high-stability, and long-term navigation and positioning for UAVs in complex indoor environments, while ensuring the engineering feasibility and generalization ability of the algorithm.

[0006] To achieve the above-mentioned objectives, the technical solution adopted by this invention is as follows:

[0007] A multi-sensor fusion navigation and positioning method for indoor unmanned aerial vehicles (UAVs) includes the following steps:

[0008] Step 1: To address the systematic bias and multimodal error characteristics of UWB ranging under NLOS conditions, the UWB bias is explicitly modeled in the state space. A Gaussian mixture observation model containing three Gaussian components is introduced, corresponding to the ranging errors under LOS, mild NLOS, and severe NLOS, respectively. The model parameters are determined by statistical analysis of measured data from typical NLOS scenarios such as wall occlusion, furniture occlusion, and human occlusion.

[0009] Step 2: Design an IAE adaptive measurement noise adjustment mechanism based on a sliding window, calculate the mean and standard deviation of the UWB ranging residual within the window for judgment; then combine adaptive chi-square gating and Huber-Tukey dual robust kernel function to achieve abnormal measurement suppression.

[0010] Step 3: Construct the bias-aware EKF joint state vector to overcome the limitation of traditional EKF in only estimating motion state and realize the joint estimation of motion state and sensor bias.

[0011] Step 4: Based on the strapdown inertial navigation system (SINS) calculation, establish an IMU kinematic state prediction model. Using the IMU's angular velocity and acceleration data, combined with the quaternion attitude update formula, calculate the UAV's attitude, velocity, and position changes. Model the IMU zero bias and UWB bias as a random walk model, derive the dynamic estimation equation of the bias, and realize online real-time update and compensation of the bias.

[0012] Step 5: Complete the time and space synchronization of multiple sensors, and complete the external parameter calibration of multiple sensors through hardware calibration, and uniformly convert all sensor data to the UAV body coordinate system.

[0013] Step 6: Build a simulation platform and an integrated "airborne-ground" physical verification platform to verify the algorithm's accuracy improvement and robustness advantages, and further optimize the model parameters based on the experimental results.

[0014] Furthermore, in step 2, the chi-square threshold is set to 2.5 times the square of the adaptive noise standard deviation; if the squared residual exceeds the threshold, it is marked as an NLOS outlier measurement. The Huber kernel assigns a weight of 1 to moderate outliers and weights strong outliers by a threshold or the absolute value of the residual, while the Tukey kernel directly assigns a weight of 0 to strong outliers. The weights of the two kernel functions are averaged to obtain a combined robust weight, which is used to weight and correct the UWB ranging data, outputting robust UWB ranging data.

[0015] Furthermore, step 3 constructs the bias-aware EKF joint state vector. ,in: For three-dimensional position, For three-dimensional velocity, The pose is represented by a quaternion. For IMU gyroscope zero bias, Add zero offset to the IMU, where bu is the UWB ranging offset.

[0016] Furthermore, in step 5, multi-source observation equations are constructed. Robust UWB ranging data is used to build absolute position observation constraints, a LiDAR altitude sensor is used to build Z-axis height observation constraints, and the optical flow module uses pixel motion calculations to build planar velocity observation constraints. The corresponding Jacobian matrix is ​​derived for each observation equation to adapt to the nonlinear observation update requirements of the EKF.

[0017] Furthermore, in step 5, a sensor reliability assessment and adaptive weighted fusion mechanism is designed. During the EKF observation update phase, different fusion weights are assigned to each observation residual based on the sensor reliability, with the altitude sensor receiving a high weight, thereby achieving adaptive fusion of multi-source information and completing the update of the UAV's status.

[0018] Furthermore, in step 6, the absolute trajectory error, relative pose error, root mean square error of ranging, real-time performance of the algorithm, and resource consumption rate are calculated. The proposed method is compared with baseline methods such as single UWB, loose fusion of UWB+IMU, and traditional EKF fusion to verify the accuracy improvement and robustness advantages of the algorithm. The model parameters are further optimized based on the experimental results.

[0019] Compared with the prior art, the beneficial technical effects of the present invention are as follows:

[0020] This invention accurately characterizes the multimodal error features when LOS / NLOS coexistence using a Gaussian mixture observation model. Combined with IAE adaptive noise adjustment and adaptive chi-square gating, it achieves dynamic adaptation to environmental changes. It solves the problems of IMU cumulative drift and UWB systematic bias, controlling long-term hovering positioning errors to the decimeter level, and significantly reducing ATE and RPE in trajectory tracking.

[0021] The multi-source information adaptive weighted fusion method designed in this invention improves the system's environmental adaptability and fault tolerance, and also clarifies the timestamp interpolation and spatial coordinate system calibration methods for multiple sensors, adapting to the sampling frequency and installation location of different sensors. Experiments have fully verified the algorithm's accuracy, robustness, and generalization ability, providing reliable technical support for subsequent engineering applications. Attached Figure Description

[0022] Figure 1 This is a flowchart of a navigation and positioning method for an indoor unmanned aerial vehicle (UAV) using multi-sensor fusion according to the present invention. Figure 2 This is a diagram illustrating the UWB error modeling of the present invention. Detailed Implementation

[0023] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments. These embodiments are only used to explain the present invention and are not intended to limit the scope of protection of the present invention.

[0024] This invention provides a navigation and positioning method for indoor unmanned aerial vehicles (UAVs) using multi-sensor fusion. It achieves UWB ranging bias modeling through a Gaussian mixture model, and combines IAE adaptive noise adjustment and robust kernel function anomaly suppression to construct a bias-sensing extended Kalman filter fusion framework. This framework adaptively fuses multi-source information from IMU, UWB, altitude sensor, and optical flow data, ultimately achieving high-precision and robust navigation and positioning for UAVs in complex indoor environments.

[0025] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0026] Figure 1 This is a flowchart of a navigation and positioning method for indoor unmanned aerial vehicles (UAVs) using multi-sensor fusion, provided by this invention. Figure 1 As shown, this embodiment includes the following steps:

[0027] Step 1: Before the system runs, perform unified calibration and time synchronization on all types of sensors, including IMU zero-bias calibration, UWB base station coordinate calibration, and external parameter calibration of optical flow sensor and altitude sensor. Use IMU as master clock and achieve time consistency of multi-sensor data through timestamp alignment or hardware synchronization. At the same time, complete the spatial coordinate system one and convert all sensor data to UAV coordinate system or navigation coordinate system.

[0028] Step 2 involves collecting UWB ranging data in a typical indoor environment, performing statistical analysis on the collected data, and establishing a UWB ranging error model. Specifically, a three-component Gaussian mixture model is used to describe the multimodal distribution characteristics of the ranging error. Different Gaussian components correspond to LOS, weak NLOS, and strong NLOS conditions, respectively. Through statistical analysis, the weights, mean, and variance parameters of each component are obtained, providing an accurate observation model for subsequent filtering.

[0029] Step 3: Perform adaptive chi-square gating and Huber-Tukey dual robust kernel anomaly suppression. The chi-square test threshold is adaptively updated with the current noise standard deviation as follows: the Huber kernel handles moderate outliers, and the Tukey kernel handles strong NLOS outliers. By automatically downweighting or removing outlier measurements, robust UWB observations are output.

[0030] Step 4: Construct a bias-aware state-space model, and jointly estimate the UAV's position, velocity, attitude, IMU zero bias, and UWB ranging bias as unified state variables:

[0031]

[0032] in, It's the location of the drone. It's speed. It is a posture (quaternion). It is IMU zero bias. This refers to the UWB ranging bias. Both the IMU zero bias and the UWB bias are described using a random walk model, enabling dynamic modeling and online compensation of sensor system errors.

[0033] Step 6: Based on the angular velocity and acceleration data of the IMU, the attitude, velocity and position are recursively calculated using the strapdown inertial navigation principle. At the same time, the error covariance matrix is ​​updated to obtain the prior estimate of the system.

[0034] Step 7: Construct a multi-sensor observation model, where UWB provides absolute position constraints.

[0035]

[0036] An optical flow sensor provides planar velocity information, while an altitude sensor provides vertical height constraints. By calculating individual observation residuals and combining them with sensor reliability assessment results, multi-source observations are weighted and fused, prioritizing the use of high-reliability observation information. This completes the extended Kalman filter state update process and achieves collaborative fusion of multi-sensor information.

[0037] In summary, this invention provides a navigation and positioning method for indoor UAVs using multi-sensor fusion, solving the problem of high-precision navigation and positioning for UAVs in indoor environments without GNSS. It improves the ability to suppress UWB NLOS bias and IMU cumulative error, and enhances the robustness of multi-sensor fusion. The algorithm boasts good real-time performance and high engineering feasibility, and all required sensors are low-cost commercial modules, making them easy to integrate and deploy. This invention can be widely applied to UAV operation scenarios such as indoor inspection, logistics distribution, security monitoring, industrial inspection, and indoor surveying, demonstrating significant engineering application value and market prospects.

[0038] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of these embodiments are only intended to aid in understanding the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A multi-sensor fusion navigation and positioning method for indoor unmanned aerial vehicles (UAVs), characterized in that, Includes the following steps: Step 1: Perform bias modeling for UWB ranging. To address the systematic bias problem of UWB ranging under NLOS conditions, explicitly model the UWB bias in the state space and introduce a Gaussian mixture observation model to characterize the multimodal characteristics of ranging error when LOS / NLOS coexist. Step 2: Design an adaptive measurement noise adjustment (IAE) mechanism to dynamically correct the measurement noise variance based on the consistency of the observation residuals; combine chi-square gating and robust kernel function to weighted suppress outlier measurements to obtain robust UWB ranging data; Step 3: Construct a bias-aware extended Kalman filter (Bias-Aware EKF) state vector, taking the UAV's position, velocity, attitude, IMU zero bias, and UWB bias as joint state variables, breaking through the limitation of traditional EKF only estimating motion state; establish a state prediction model based on the IMU kinematic equations, and derive the dynamic estimation equations of IMU zero bias and UWB bias. Step 4: Use UWB robust ranging data to construct absolute position observation constraints, use an altitude sensor (LiDAR) to construct altitude observation constraints, and use an optical flow module to construct velocity observation constraints, thus forming a multi-source observation equation; Step 5: In the EKF observation update phase, the weighted fusion of multi-source observation residuals is achieved through sensor reliability assessment to complete the adaptive update of the UAV status. Step 6: Build a simulation platform and an integrated "airborne-ground" physical verification platform. Conduct hovering and trajectory tracking experiments in typical scenarios such as open areas, corridors, and occluded environments. Verify the algorithm performance using metrics such as absolute trajectory error (ATE), relative pose error (RPE), and ranging RMSE, and optimize the model parameters.

2. The method according to claim 1, characterized in that, The mixed Gaussian observation model described in step 1 contains three Gaussian components, which correspond to the ranging error under LOS conditions, the ranging error under mild NLOS conditions, and the ranging error under severe NLOS conditions, respectively. The weight, mean, and standard deviation of each component are determined by statistical analysis of UWB ranging data in actual NLOS scenarios.

3. The method according to claim 1, characterized in that, The implementation process of the IAE mechanism in step 2 is as follows: the mean and standard deviation of the UWB ranging residual are calculated through a sliding window. When the deviation between the residual and the mean is greater than twice the standard deviation of the residual, the standard deviation of the measurement noise is amplified by 1.5 times. When the number of residuals in the sliding window is insufficient, the standard deviation of the reference noise under the LOS condition is used as the default value.

4. The method according to claim 1, characterized in that, In step 3, the IMU kinematic equations are calculated based on strapdown inertial navigation, and the attitude, velocity, and position changes of the UAV are calculated using the angular velocity and acceleration data of the IMU. The dynamic estimation equation of the IMU zero bias to the UWB bias is a random walk model, which realizes online real-time estimation and compensation of the bias.

5. The method according to claim 1, characterized in that, The basis for the sensor reliability assessment in step 4 is as follows: the reliability of UWB is determined by the discrimination result of chi-square gating and the magnitude of the ranging residual; the reliability of optical flow is determined by image texture features and optical flow tracking quality; and the reliability of the altitude sensor is determined by the ranging distance and environmental reflection characteristics. High-reliability sensors are given high fusion weights, while low-reliability sensors are downweighted or temporarily excluded from fusion.

6. The method according to claim 1, characterized in that, The timing synchronization method for multi-source information fusion described in step 5 is as follows: based on the sampling frequency of the IMU, the data time alignment of UWB, altitude sensor, optical flow module and IMU is achieved through timestamp interpolation and hardware synchronization; spatial alignment of multiple sensors is achieved through coordinate system calibration, and the data is uniformly converted to the UAV body coordinate system or geographic coordinate system.

Images