An adaptive particle filtering algorithm for uav ultra-wideband positioning

Abstract

This invention provides an adaptive particle filter algorithm for UAV ultra-wideband positioning. First, a relative positioning model of master and slave UAVs, incorporating UAV dynamics equations and ultra-wideband measurement equations, is established. Second, a position accuracy factor is calculated based on the distribution of base stations on the master UAV and the relative positional relationship between the master and slave UAVs. The number of particles required for the current particle filter algorithm is then calculated based on this position accuracy factor. Finally, based on the calculated number of particles, a particle filter is designed to achieve real-time estimation of the UAV's position. This invention is applicable to multi-UAV collaborative operation scenarios, specifically for relative positioning between master and slave UAVs based on ultra-wideband signals. The proposed adaptive particle filter algorithm can reduce the computational burden on the UAVs by adaptively adjusting the number of particles while maintaining positioning accuracy, thus ensuring the real-time performance of the UAV's onboard computer's positioning calculations.

Description

Technical Field

[0001] This invention belongs to the field of UAV positioning, specifically relating to an adaptive particle filter algorithm for UAV ultra-wideband positioning, applicable to ultra-wideband dynamic base station real-time positioning systems in multi-UAV collaborative operation scenarios. Background Technology

[0002] Surveying and searching refers to the task of conducting focused investigations of geological structures, vital signs, and environmental conditions within a specific area based on the objectives and needs of emergency rescue. Introducing drones into surveying and searching operations allows for high-risk, unstable, and unpredictable work, protecting the lives of personnel while completing the tasks. Swarms of small drones are suitable for surveying and searching in complex environments. Due to their small size, high flexibility, and low cost, small drones can operate in confined spaces, and multiple drones significantly improve operational efficiency compared to a single drone.

[0003] Accurate positioning of micro-UAVs in confined, unknown, and GPS-denied environments is a prerequisite for subsequent tasks such as surveying and searching. Ultra-wideband (UWB) technology offers advantages such as low system complexity, low transmitted signal power spectral density, insensitivity to channel fading, low interception capability, and high positioning accuracy, making it particularly suitable for high-speed wireless access in dense, multipath-based environments such as indoors. However, traditional UWB positioning relies on deploying multiple base stations in the environment, which has limitations such as fixed positioning locations and high hardware requirements. Furthermore, during flight missions, the UAV's position estimation is susceptible to external interference from the surrounding environment. Therefore, to achieve high-precision positioning for UAVs, the design of the positioning method must consider both of these factors.

[0004] Chinese invention patent CN202011519291.7 proposes an indoor and outdoor high-precision positioning system and method for unmanned aerial vehicles (UAVs), using ultra-wideband (UWB) base stations for positioning. However, it has two problems: (1) The paper uniformly sets up three or more UWB base stations on the ground near the UAV's flight area, which limits the flight mission to the area where the base stations are located; (2) It does not consider the quantitative impact of the relative position of the base stations and tags on the positioning results. Chinese invention patent CN201810100454.4 proposes a high-precision positioning and inspection system for UAVs suitable for use in substations. The proposed method also has similar problems: (1) The UWB base stations are arranged in fixed positions in the environment, which limits the flight space of the UAV; (2) The algorithm used has a long execution time, so it is inferior to this method in terms of positioning speed. Summary of the Invention

[0005] To overcome the shortcomings of existing technologies, this invention proposes an adaptive particle filter algorithm for UAV ultra-wideband positioning, based on an ultra-wideband dynamic base station UAV positioning system, specifically for surveying and searching tasks in complex and confined spaces. This algorithm satisfies the real-time computation requirements of UAV ultra-wideband positioning. This method provides a more efficient positioning approach for ultra-wideband dynamic base station UAV positioning systems, enabling real-time, high-precision positioning of micro-UAVs in confined spaces.

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

[0007] An adaptive particle filter algorithm for UAV ultra-wideband positioning includes the following steps:

[0008] Step 1: Establish a relative positioning model for the master and slave UAVs, including a UAV dynamics model and an ultra-wideband measurement equation model;

[0009] Step 2: Calculate the position accuracy factor based on the distribution of base stations on the master UAV and the relative positional relationship between the master and slave UAVs, and calculate the number of particles required based on the position accuracy factor;

[0010] Step 3: Based on the number of particles calculated in Step 2, design a particle filter to achieve state estimation of the UAV.

[0011] Furthermore, the UAV dynamics model is as follows:

[0012]

[0013]

[0014]

[0015]

[0016]

[0017]

[0018] Where x, y, z represent the drone's position in the geodetic coordinate system, m represents the drone's mass, g represents gravitational acceleration, and l represents the distance from the drone's motor to its center of mass. x I y I z Let K represent the moments of inertia in the x, y, and z directions, respectively; φ, θ, and ψ represent the roll, pitch, and yaw angles, respectively; and K represents the moment of inertia in the x, y, and z directions, respectively. i The drag coefficient is represented by the following: i = 1, 2, 3, 4, 5, 6; Δt is the sampling time of the discrete system; k represents the time; and u1, u2, u3, u4 are the control input signals.

[0019] The above model, written in matrix form and incorporating process noise, yields:

[0020] x k+1 =f(x) k ,u k )+w k

[0021] Where, x k =[x k+1 y k+1 z k+1 x k y k z k φ k+1 θ k+1 ψ k+1 φ k θ k ψ k ] T u k =[u 1,k u 2,k u 3,k u 4,k ] T This represents the control input signal and the process noise w. k Follows Gaussian distribution Q represents the variance of the process noise distribution. This indicates a Gaussian distribution.

[0022] The measurement model for ultra-wideband ranging is as follows:

[0023]

[0024]

[0025]

[0026] Where, d i Let px represent the measurement value of the i-th ultra-wideband sensor, i = 1, 2, 3. i py i pz i These represent the positions of the i-th ultrawideband sensor in the geodetic coordinate system;

[0027] The above equations, written in matrix form and incorporating measurement noise, yield:

[0028] y k =h(x k )+v k

[0029] Among them, y k=[d1 d2 d3] T This represents the measurement vector of the ultra-wideband sensor, and the measurement noise v. k Follows Gaussian distribution R is the variance of the measurement noise distribution;

[0030] Furthermore, step 2 is specifically implemented as follows:

[0031] In the relative positioning of master and slave UAVs based on ultra-wideband signals, considering the distribution of base stations on the master UAV and the relative positional relationship between the master and slave UAVs, the PDOP is calculated as follows:

[0032] PDOP = tr((H T H) -1 )

[0033] Where H matrix is ​​the Jacobian matrix of measurement model h(x), and tr represents the trace of the matrix;

[0034] After calculating the PDOP at the current time, the number of particles n required to be generated in the resampling phase of the particle filter algorithm is adjusted as follows:

[0035]

[0036] Where ε is a parameter used to measure the accuracy of the current estimate; z 1-δ This represents the upper 1-δ quantile of the standard normal distribution, where δ is a parameter representing the expected confidence level; m is used to represent the dispersion of particles in the state space, calculated as follows:

[0037] m = L × x × PDOP

[0038] Where L represents the accuracy coefficient, and x is the state estimate given by the particle filter algorithm;

[0039] Finally, the calculated number of particles n is used as a hyperparameter input to the particle filtering algorithm, serving as the number of particles required for the resampling step at the current time.

[0040] The advantages of this invention compared to existing technologies are as follows: This invention relates to an adaptive particle filter algorithm for ultra-wideband (UWB) positioning of unmanned aerial vehicles (UAVs), applicable to UWB real-time positioning systems using multi-UAV collaborative operation scenarios. It primarily employs a particle filter for UAV positioning, while simultaneously considering the relative positions of the base station and tags, dynamically adjusting the number of particles required for the particle filter. This ensures positioning accuracy while reducing the computational burden on the UAV, resulting in more efficient real-time positioning. In the UWB real-time positioning system, the number of particles required for the particle filter is flexibly adjusted based on the UAV's real-time position, balancing both accuracy and real-time performance in the positioning task. Attached Figure Description

[0041] Figure 1 This is a flowchart of an adaptive particle filter algorithm for UAV ultra-wideband positioning according to the present invention.

[0042] Figure 2 This is a structural block diagram of the UAV ultra-wideband positioning system of the present invention. Detailed Implementation

[0043] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0044] The UAV ultra-wideband positioning system of this invention includes a master UAV and multiple small slave UAVs. The master UAV is equipped with three ultra-wideband transmitters and a visual inertial odometry (VIO). The VIO is used to locate itself and obtain global position information. The three ultra-wideband transmitters send signals to ultra-wideband receivers on the slave UAVs to measure the distance to each of the small slave UAVs. Simultaneously, the ultra-wideband system acts as a wireless communication module, transmitting the global position information to the small slave UAVs. Each small slave UAV is equipped with an ultra-wideband receiver and an inertial measurement unit (IMU). The slave UAV receives signals from the ultra-wideband transmitters on the master UAV through its ultra-wideband receiver, obtains relative distance information to the three ultra-wideband transmitters on the master UAV, and calculates its own relative position by fusing the relative distance information and the IMU measurement information. Combined with the global position information of the master UAV, its absolute position is obtained.

[0045] like Figure 1 and Figure 2 As shown, the specific implementation steps of the adaptive particle filter algorithm for UAV ultra-wideband positioning of the present invention are as follows:

[0046] Step 1: Establish a master-slave UAV relative positioning model that includes a UAV dynamics model and an ultra-wideband measurement model:

[0047] First, establish a discrete-time UAV dynamics model:

[0048]

[0049]

[0050]

[0051]

[0052]

[0053]

[0054] Where x, y, z represent the drone's position in the geodetic coordinate system, m represents the drone's mass, g represents gravitational acceleration, and l represents the distance from the drone's motor to its center of mass. x I y I z Let K represent the moments of inertia in the x, y, and z directions, respectively, and let φ, θ, and ψ represent the roll, pitch, and yaw angles, respectively. i (i = 1, 2, 3, 4, 5, 6) represent the drag coefficients, where the sequence numbers i = 1, 2, 3, 4, 5, 6, Δt is the sampling time of the discrete system, k represents the time, and u1, u2, u3, u4 are the control input signals. The above model, written in matrix form and incorporating process noise, yields:

[0055] x k+1 =f(x) k ,u k )+w k

[0056] Where, x k =[x k+1 y k+1 z k+1 x k y k z k φ k+1 θ k+1 ψ k+1 φ k θ k ψ k ] T u k =[u 1,k u 2,k u 3,k u 4,k ] T This represents the control input signal and the process noise w. k Follows Gaussian distribution Q represents the variance of the process noise distribution. This indicates a Gaussian distribution.

[0057] Secondly, establish a measurement model for ultra-wideband ranging:

[0058]

[0059]

[0060]

[0061] Where, d i (i = 1, 2, 3) represents the measurement value of the i-th ultra-wideband sensor, px i py i pz i Let represent the positions of the i-th ultra-wideband sensor in the geodetic coordinate system. The above model, written in matrix form and incorporating measurement noise, yields:

[0062] y k =h(x k )+v k

[0063] Among them, y k =[d1 d2 d3] T This represents the measurement vector of the ultra-wideband sensor, and the measurement noise v. k Follows Gaussian distribution R represents the variance of the measured noise distribution.

[0064] Step 2: Calculate the position accuracy factor based on the distribution of base stations on the master UAV and the relative positional relationship between the master and slave UAVs. Then, calculate the required number of particles based on the position accuracy factor.

[0065] To balance computational power and estimation performance, this invention adaptively adjusts the number of particles required for the current filtering solution based on the location precision factor (PDOP). In geometric positioning, the relative distance between tags on multiple base stations and the moving target significantly impacts positioning accuracy; this invention uses the location precision factor (PDOP) to measure this impact. The location precision factor (PDOP) is calculated as follows:

[0066] PDOP = tr((H T H) -1 )

[0067] Here, the H matrix is ​​the Jacobian matrix of the measurement model h(x), and tr represents the trace of the matrix.

[0068] During the resampling stage of particle filtering, the number of particles required for resampling is calculated based on the position precision factor PDOP:

[0069]

[0070] Where ε is a parameter used to measure the accuracy of the current estimate, typically taken as 0.05; z 1-δ This represents the upper 1-δ quantile of the standard normal distribution, where δ is a parameter representing the expected confidence level, typically taken as 0.01; m is used to represent the dispersion of particles in the state space, calculated as follows:

[0071] m = L × x × PDOP

[0072] Where L represents the accuracy coefficient, x represents the state estimate given by the particle filter algorithm, and PDOP is the position accuracy factor.

[0073] After calculating the number of particles n, the number of particles required for the current resampling step is set to n.

[0074] Step 3: Based on the number of particles calculated in Step 2, design a particle filter to achieve UAV state estimation. This is done by incorporating the UAV's dynamic model and the ultra-wideband sensor's measurement model into the particle filter algorithm. The particle filter uses a set of n particles. p(x) represents the probability density function k The implementation steps of the designed particle filter are as follows:

[0075] Initialize the particle set in the filter:

[0076]

[0077] For times k = 1, 2, ..., K, the following steps are executed iteratively:

[0078] a) First, based on the UAV system model and the probability density function of system noise, obtain the prior particle set. The particle value is:

[0079]

[0080] in, Represents a set The value of the i-th particle in the matrix, u k It is a control input signal. Based on Gaussian distribution Randomly generated.

[0081] b) Secondly, based on the ultra-wideband sensor measurement model and the probability density function of the measurement noise, the particle set is calculated. Weights of particles:

[0082]

[0083] in, For a collection of particles The weight of the i-th particle in the above formula. Refer to step 1 for the parameters in the above formula.

[0084] Next, normalize the particle weights:

[0085]

[0086] At this point, the particle set can be represented as

[0087] c) Then, update the particle number n based on the particle number calculated in step 2:

[0088]

[0089] in:

[0090] m = L × x k|k-1 ×PDOP

[0091] The parameters in the above formula are referenced in step 2.

[0092] d) Finally, based on particle weights Resampling produces n particles, resulting in a distribution p(x) k |y k resampled particle set We can calculate its mean and variance; the mean is the estimated value, and the variance is the covariance of the estimation error. Therefore, the estimated value of the state at time k is:

[0093]

[0094] in, For resampled particle set The particle value in the middle.

[0095] The contents not described in detail in this specification are prior art known to those skilled in the art. Those skilled in the art will readily understand that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An adaptive particle filtering method for ultra-wideband positioning of unmanned aerial vehicles (UAVs), characterized in that, Includes the following steps: Step 1: Establish a relative positioning model for the master and slave UAVs, including a UAV dynamics model and an ultra-wideband measurement equation model; Step 2: Calculate the position accuracy factor based on the distribution of base stations on the master UAV and the relative positional relationship between the master and slave UAVs, and calculate the number of particles required based on the position accuracy factor; In the relative positioning of master and slave UAVs based on ultra-wideband signals, considering the distribution of base stations on the master UAV and the relative positional relationship between the master and slave UAVs, PDOP is calculated as follows: ； in, A matrix is ​​a measurement model Jacobian matrix, Represents the trace of a matrix; After calculating the PDOP at the current time, the number of particles required for the resampling phase of the particle filter algorithm is determined. Adjusted to: ； in, It is a parameter used to measure the accuracy of the current estimate; The upper part of the standard normal distribution quantiles, It is a parameter representing the expected confidence level; The degree of dispersion of particles in the state space is represented by the following calculation: ； in, Indicates the precision coefficient. The state estimate given by the particle filter algorithm; Finally, the calculated number of particles As a hyperparameter input to the particle filter algorithm, it represents the number of particles required to be generated in the current resampling step. Step 3: Based on the number of particles calculated in Step 2, design a particle filter to achieve state estimation of the UAV.

2. The adaptive particle filtering method for UAV ultra-wideband positioning according to claim 1, characterized in that, The dynamic model of the unmanned aerial vehicle is as follows: ； in, , , This represents the drone's position in the geodetic coordinate system. Indicates the quality of the drone. Represents gravitational acceleration. This represents the distance from the drone's motor to its center of mass. , , They represent , , Moment of inertia in the direction of rotation , , These represent the roll angle, pitch angle, and yaw angle, respectively. Represents the drag coefficient, where the serial number , The sampling time of the discrete system is k, where k represents time. , , , To control the input signal; The above model, written in matrix form and incorporating process noise, yields: ； in, , Indicates control input signal, process noise Follows a Gaussian distribution , Let Variance be the variance of the process noise distribution. Indicates a Gaussian distribution; The measurement model for ultra-wideband ranging is as follows: ； in, Indicates the first Measurements from an ultra-wideband sensor, , , , They represent the first The location of an ultra-wideband sensor in the geodetic coordinate system; The above equations, written in matrix form and incorporating measurement noise, yield: ； in, Represents the measurement vector of the ultra-wideband sensor, measurement noise. Follows a Gaussian distribution , To measure the variance of the noise distribution.

Images