Multi-machine cooperative ultra wide band UWB positioning method and system for high-altitude building scene

Through the multi-machine collaborative ultra-wideband UWB positioning method, combined with the Kalman filter and the Levenberg-Marquardt optimization algorithm, the problem of positioning accuracy and stability in high-altitude buildings after the disaster is solved, and high-precision and low-cost drone positioning is achieved. For small drones, the positioning accuracy reaches the centimeter level.

CN119031323APending Publication Date: 2024-11-26TSINGHUA SHENZHEN INTERNATIONAL GRADUATE SCHOOL
View PDF 0 Cites 6 Cited by

Patent Information

Application Number
CN202411147879.2
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-21
Publication Date
2024-11-26

AI Technical Summary

Technical Problem

In the scene of high-altitude buildings after the disaster, existing positioning technology is difficult to achieve high-precision positioning, especially based on lidar and visual methods, which are greatly affected by environmental factors. The traditional UWB solution algorithm has poor stability and high cost and computing resources, making it difficult to apply to Small drone.

Method used

Multi-machine collaborative ultra-wideband UWB positioning method is adopted, combined with error state Kalman filter and Levenberg-Marquardt nonlinear optimization algorithm, multilateral positioning is performed through UWB base station and IMU data, and position parameters are optimized using Jacobian matrix and least squares method to achieve high Real-time positioning of precision.

Benefits of technology

It realizes the positioning of high-precision, low-cost and low computing resources in post-disaster high-altitude building scenarios, avoids local optimal solutions, is suitable for small drones, with positioning accuracy reaching the centimeter level, and significantly improves stability and accuracy.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119031323A_ABST
    Figure CN119031323A_ABST
Patent Text Reader

Abstract

The invention provides a multi-machine cooperative UWB positioning method and system for a high-altitude building scene. The method comprises the steps of initializing a UWB system, collecting UWB ranging and IMU data, executing multilateral positioning and LM algorithm preliminary resolving, constructing a Jacobian matrix, iteratively optimizing position parameters, performing track plotting to update a pose, predicting and correcting a Kalman filter state, performing fusion processing on the UWB and IMU data, and finally outputting an accurate real-time position. According to the method, the error state Kalman filter ESKF and the improved LM nonlinear optimization algorithm are utilized, IMU data and UWB distance measurement data are fused, falling into a local optimal solution is effectively avoided, and stability and accuracy are ensured. The method is suitable for a multi-unmanned aerial vehicle cooperative positioning task and is not influenced by environmental factors such as dust, centimeter-level positioning precision is verified, and the applicability and high efficiency of the method in a post-disaster high-altitude building exploration task are shown.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to wireless positioning technology, and in particular to a multi-machine collaborative ultra-wideband (UWB) positioning method and system for high-altitude building scenarios. Background Art

[0002] The booming drone technology in recent years has become an effective means of responding to disaster rescue exploration missions. The use of drones for exploration and rescue missions not only ensures the safety of rescuers, but also saves manpower and material costs. In drone exploration missions, a single drone may be limited by the angle of the sensor and cannot observe the target area from multiple different directions. When faced with a large-scale search mission, it cannot effectively cover the entire reconnaissance area. Multiple drones are often used to explore high-altitude buildings after disasters.

[0003] A positioning method based on laser radar, which obtains three-dimensional point cloud data of the environment by deploying a laser radar sensor on a drone; preliminarily estimates the drone's position based on a position estimation algorithm such as a scan matching method or graph optimization; and obtains the final position information by fusing the IMU accelerometer and accelerometer data based on an error state Kalman filter. By fusing laser radar and IMU data, this method has higher robustness while maintaining high positioning accuracy. The positioning method based on laser radar cannot achieve high positioning accuracy in high-altitude outdoor environments and requires large computing resources; it is not suitable for small drones, and increases the cost of the drone while increasing the load.

[0004] A UWB and vision fusion positioning method for collaborative positioning of multiple unmanned aerial vehicles comprises the following steps: obtaining UWB, IMU, binocular camera and altitude-fixing radar data; fusing the UWB, IMU and altitude-fixing radar data through an unscented Kalman filter to estimate the posture of the unmanned aerial vehicle in a global coordinate system; fusing the IMU and binocular camera data to form a visual inertial odometer to estimate the posture of the unmanned aerial vehicle in an inertial system, realize visual positioning, output the posture of the unmanned aerial vehicle and environmental landmarks, and simultaneously create and broadcast key frame messages; construct a posture optimization graph, use a global unary posture edge to represent the UWB positioning measurement result, use a binary incremental edge to represent the posture result measured by the visual inertial odometer, calculate the minimum value of the cost function, and obtain the optimal estimate of the posture state vector.

[0005] In post-disaster high-rise building scenes, visual methods are greatly affected by environmental factors such as smoke and dust.

[0006] In the post-disaster high-altitude building positioning scenario, it is difficult for drones used for indoor exploration tasks to obtain high-precision GPS satellite positioning positions, with large altitude positioning errors and lack of stability. Positioning methods based on lidar are costly, highly dependent on the environment, and difficult to deploy on small drones; visual positioning methods are also highly dependent on the environment. In the post-disaster environment, affected by environmental factors such as smoke and dust, visual methods are difficult to achieve good positioning results; UWB solution algorithms based on traditional linear least squares rely on the formation of UWB base station drones and have poor stability; optimization algorithms based on traditional Gauss-Newton methods rely on the selection of optimization parameters such as the initial optimization point. When the UWB tag drone is far away from the initial optimization point, it may fall into a local optimal solution and affect positioning.

[0007] It should be noted that the information disclosed in the above background technology section is only used for understanding the background of the present application, and therefore may include information that does not constitute prior art known to ordinary technicians in the field. Summary of the invention

[0008] The main purpose of the present invention is to solve the problems existing in the above-mentioned background technology and to provide a multi-machine collaborative ultra-wideband (UWB) positioning method for high-altitude building scenes, especially for post-disaster high-altitude building scenes.

[0009] To achieve the above object, the present invention adopts the following technical solutions:

[0010] In a first aspect of the present invention, a multi-machine collaborative ultra-wideband (UWB) positioning method for high-altitude building scenes comprises the following steps:

[0011] S1: Initialize the multi-machine cooperative UWB positioning system, configure the parameters of the error state Kalman filter ESKF and LM (Levenberg-Marquardt) nonlinear optimization algorithm, and prepare to fuse IMU data and UWB ranging data;

[0012] S2: Start the UWB radio frequency module to collect the ranging signals between multiple UWB base station drones and UWB tag drones to provide raw data for multilateral positioning; at the same time, use the IMU sensor to collect the acceleration and angular velocity data of the UWB tag drone to provide the motion model input for the state Kalman filter ESKF;

[0013] S3: Combine the UWB ranging data and IMU data obtained in step S2 to perform a multi-lateration positioning process, start the LM algorithm to perform a preliminary real-time positioning solution, and estimate the relative position error between the UWB base station and the UWB tag;

[0014] S4: Based on the LM algorithm, a Jacobian matrix is ​​constructed, which contains the relationship between the measured distance from the UWB base station drone to the UWB tag drone and the predicted position; the descent vector of the position parameter of the UWB tag relative to the observation error is solved, and the position parameter is updated by the least squares method to prepare the observation data of the UWB tag for the prediction step of the state Kalman filter ESKF;

[0015] S5: performing iterative optimization of the LM algorithm to solve the UWB multilateration positioning problem until a preset condition is met, preferably the sum of squares of the error vector is less than a preset tolerance or a maximum number of iterations is reached;

[0016] S6: Use the IMU data collected in step S2 to update the posture information of the UWB tag drone through track calculation, and provide the latest motion state for the state Kalman filter ESKF state update step;

[0017] S7: Input the position parameters of the UWB tag optimized in step S5 as observation data into the state Kalman filter ESKF, and perform state prediction and error correction in combination with the motion state updated in step S6;

[0018] S8: Update the position and velocity estimates of the UWB tag drone in the state Kalman filter ESKF based on the prediction and observation data, and integrate them to achieve robust processing of UWB ranging errors and IMU measurement noise;

[0019] S9: Combining the results of the state Kalman filter ESKF fusion processing in step S8 and the position parameters optimized by the LM algorithm in step S5, the precise real-time position of the UWB tag drone is output to complete high-precision positioning.

[0020] The second aspect of the present invention is a multi-machine cooperative ultra-wideband (UWB) positioning system for high-altitude building scenes, comprising:

[0021] A plurality of UWB radio frequency modules, including a plurality of UWB base stations respectively mounted on the plurality of UWB base station drones and a UWB tag mounted on the UWB tag drone, for measuring the distance between the plurality of UWB base stations and the UWB tags;

[0022] Inertial measurement unit IMU, carried on the UWB tag drone, is used to provide moving speed and direction information to assist UWB positioning;

[0023] Multiple UWB base station drones and at least one UWB tag drone;

[0024] The main control module communicates with the multiple UWB radio frequency modules and the inertial measurement unit IMU, and is used to execute the multi-machine collaborative ultra-wideband UWB positioning method.

[0025] A third aspect of the present invention is a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the multi-machine collaborative ultra-wideband (UWB) positioning method is implemented.

[0026] The present invention has the following beneficial effects:

[0027] The present invention provides a multi-machine collaborative ultra-wideband UWB positioning method and system for high-altitude building scenes, which is particularly suitable for high-altitude building scenes after disasters, and overcomes the shortcomings of traditional positioning technology in such complex environments. The present invention realizes high-precision real-time positioning through a lightweight sensor positioning scheme, which is not affected by environmental factors such as dust. Through the improved Levenberg-Marquardt nonlinear optimization algorithm, the present invention effectively avoids the problem that the traditional algorithm may fall into a local optimal solution when the drone is far away from the initial optimization point, and ensures the stability and accuracy of the positioning system in large-scale dynamic collaborative positioning. The multi-drone position control program verified under the Gazebo simulation platform demonstrates centimeter-level positioning accuracy in the simulation scene, proving that the present invention only requires a UWB radio frequency module combined with an IMU to achieve high-precision positioning, and is suitable for post-disaster high-altitude building exploration tasks. In addition, the multi-machine collaborative UWB positioning system shows high positioning accuracy in indoor environments, especially the accuracy of height estimation, and has the characteristics of low cost, lightweight and low computing resources. It is suitable for small exploration drones, compatible with a variety of drones, and makes full use of UWB and IMU sensor devices for effective three-dimensional position estimation.

[0028] Other beneficial effects of the embodiments of the present invention will be further described below. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 This is a physical picture of a drone according to an embodiment of the present invention.

[0030] Figure 2 This is a multi-UAV simulation scene diagram of an embodiment of the present invention.

[0031] Figure 3 This is a diagram of the take-off field for a UAV according to an embodiment of the present invention.

[0032] Figure 4 This is a structural diagram of an improved LM nonlinear optimization algorithm according to an embodiment of the present invention.

[0033] Figure 5 Positioning curve diagram for the traditional LM nonlinear optimization algorithm.

[0034] Figure 6 This is a positioning curve diagram of the improved LM nonlinear optimization algorithm according to an embodiment of the present invention.

[0035] Figure 7This is a structural diagram of a multi-machine collaborative positioning system according to an embodiment of the present invention.

[0036] Figure 8 This is a UWB tag drone positioning position curve diagram of the multi-machine collaborative positioning system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0037] The following is a detailed description of the embodiments of the present invention. It should be emphasized that the following description is only exemplary and is not intended to limit the scope and application of the present invention.

[0038] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0039] See also Figures 1 to 8 The embodiment of the present invention provides a multi-machine collaborative ultra-wideband (UWB) positioning method for high-altitude building scenes, comprising the following steps:

[0040] S1: Initialize the multi-machine cooperative UWB positioning system, configure the parameters of the error state Kalman filter ESKF and LM (Levenberg-Marquardt) nonlinear optimization algorithm, and prepare to fuse IMU data and UWB ranging data;

[0041] S2: Start the UWB radio frequency module to collect the ranging signals between multiple UWB base station drones and UWB tag drones to provide raw data for multilateral positioning; at the same time, use the IMU sensor to collect the acceleration and angular velocity data of the UWB tag drone to provide the motion model input for the state Kalman filter ESKF;

[0042] S3: Combine the UWB ranging data and IMU data obtained in step S2 to perform a multi-lateration positioning process, start the LM algorithm to perform a preliminary real-time positioning solution, and estimate the relative position error between the UWB base station and the UWB tag;

[0043] S4: Based on the LM algorithm, a Jacobian matrix is ​​constructed, which contains the relationship between the measured distance from the UWB base station drone to the UWB tag drone and the predicted position; the descent vector of the position parameter of the UWB tag relative to the observation error is solved, and the position parameter is updated by the least squares method to prepare the observation data of the UWB tag for the prediction step of the state Kalman filter ESKF;

[0044] S5: performing iterative optimization of the LM algorithm to solve the UWB multi-lateration positioning problem until a preset condition is met, where the preset condition is preferably that the sum of squares of the error vector is less than a preset tolerance or a maximum number of iterations is reached;

[0045] S6: Use the IMU data collected in step S2 to update the posture information of the UWB tag drone through track calculation, and provide the latest motion state for the state Kalman filter ESKF state update step;

[0046] S7: Input the position parameters of the UWB tag optimized in step S5 as observation data into the state Kalman filter ESKF, and perform state prediction and error correction in combination with the motion state updated in step S6;

[0047] S8: Update the position and velocity estimates of the UWB tag drone in the state Kalman filter ESKF based on the prediction and observation data, and integrate them to achieve robust processing of UWB ranging errors and IMU measurement noise;

[0048] S9: Combining the results of the state Kalman filter ESKF fusion processing in step S8 and the position parameters optimized by the LM algorithm in step S5, the precise real-time position of the UWB tag drone is output to complete high-precision positioning.

[0049] The multi-machine collaborative ultra-wideband UWB positioning method of the present invention can well meet the needs of multi-machine collaborative positioning in post-disaster high-altitude building exploration tasks, provide a lightweight sensor positioning solution, and achieve high-precision positioning that is not affected by environmental factors such as dust. Compared with the traditional multilateral positioning method based on LM nonlinear optimization, the improved LM nonlinear optimization algorithm of the present invention will not fall into the local optimal solution, and the positioning system will not fail during the long-distance dynamic flight of the UAV.

[0050] In a preferred embodiment, in step S3: for the UWB base station and the UWB tag, the positioning error is calculated, which is based on the difference between the measured distance from the UWB base station drone to the UWB tag drone and the predicted position; an error vector is defined to summarize the positioning errors between all UWB base stations and UWB tags, so as to build a foundation for the subsequent least squares problem; in step S4: a Jacobian matrix is ​​constructed, which contains the partial derivative information of the measured distance from the UWB base station drone to the UWB tag drone with respect to the predicted position parameters; a least squares problem is constructed, the goal of which is to minimize the sum of squares of the error vector to solve the optimal position parameters; The error vector is expanded by a first order Taylor to approximate the linear behavior of the nonlinear error function near the current estimation point; the descent vector is solved, which is used to adjust the position parameters in each iteration to reduce the prediction error; in step S5: during the iterative optimization process, the radius of the trust optimization area, that is, the optimization step size, is adjusted according to the sum of the squares of the error vector until the termination condition is met; the tolerance and the maximum number of iterations are set as the termination condition to ensure the convergence and stability of the algorithm; a position feedback mechanism is introduced to use the solution of the current positioning cycle as the initial optimization point of the next cycle to enhance the algorithm's insensitivity to the initial parameter selection and improve robustness.

[0051] In a preferred embodiment, in step S3, the system modeling of multilateral positioning is implemented based on the following method, including: determining a distance measurement method based on the arrival time TOA principle, which is used to calculate the straight-line distance between UWB radio frequency modules; wherein the role of the UWB base station is a UWB radio frequency module with a known position, and the UWB tag is a UWB radio frequency module to be located; using the timestamps of the signal sent by the UWB base station and the signal received by the UWB tag, the distance between the two is calculated by the known UWB signal wave speed; based on the distance measurement value from the UWB base station to the UWB tag, a multilateral positioning equation group for determining the position of the UWB tag is constructed; the multilateral positioning equation group is converted into a matrix form, a coefficient matrix and a constant vector are constructed, and a linear equation group that can be used for numerical solution is formed; the linear equation group is solved by a numerical method to obtain the three-dimensional position coordinates of the UWB tag.

[0052] In a preferred embodiment, in step S4: define state variables to provide a basic data structure for prediction and updating of ESKF, preferably, the state variables include displacement, velocity, rotation, gyroscope bias, accelerometer bias and gravity; in step S5: establish a discrete time motion equation of the nominal state variables to describe the evolution of the UAV state over time, including the update of displacement, velocity and rotation; in step S7: in the ESKF, perform state prediction and error correction based on the predicted data and the observed data, and update the position and velocity estimation of the UAV using the observation equation and the motion model based on the IMU data.

[0053] In a preferred embodiment, during the multilateral positioning process, the acceleration data and angular velocity data measured by the IMU are used to predict the nominal state of the UAV based on the Euler numerical integration method, and the state variables at the next moment are calculated; during track calculation, the IMU data is used for short-term track calculation to fill the posture changes during the UWB data update interval; in the state Kalman filter ESKF, the position and velocity estimates of the UAV are updated based on the predicted data and observed data, combined with the high-frequency data of the IMU and the low-frequency data of the UWB, to achieve precise tracking.

[0054] In a preferred embodiment, in step S6, when the IMU data transmission frequency is higher than the UWB RF module data transmission frequency, the IMU data is used to perform short-time dead reckoning to update the drone posture information.

[0055] In a preferred embodiment, in step S7, the Jacobian matrix of the observation equation is obtained by the chain rule, and the Jacobian matrix is ​​used to convert changes in the observation space into changes in the state space, thereby updating the state estimate.

[0056] In a preferred embodiment, in step S8, robust processing of UWB ranging error and IMU measurement noise is achieved by fusion in ESKF, including: quantifying the noise in the IMU data using a Gaussian noise model and a covariance matrix, constructing a Jacobian matrix of the observation equation to convert changes in the observation space into the state space; calculating the Kalman gain, which combines the prediction covariance matrix and the observation noise covariance matrix; updating the state estimate and the covariance matrix, fusing the observed data and the predicted state, and reducing the impact of the UWB ranging error and the IMU measurement noise.

[0057] An embodiment of the present invention also provides a multi-machine collaborative ultra-wideband UWB positioning system for high-altitude building scenarios, including: multiple UWB radio frequency modules, including multiple UWB base stations respectively mounted on multiple UWB base station drones and UWB tags mounted on UWB tag drones, for measuring the distance between multiple UWB base stations and UWB tags; an inertial measurement unit IMU, mounted on the UWB tag drone, for providing moving speed and direction information to assist UWB positioning; multiple UWB base station drones and at least one UWB tag drone; a main control module, communicating with the multiple UWB radio frequency modules and the inertial measurement unit IMU, for executing the multi-machine collaborative ultra-wideband UWB positioning method.

[0058] In the above preferred embodiments of the present invention, a multilateral positioning method based on an improved LM nonlinear optimization algorithm is proposed. A multi-machine cooperative UWB positioning method and system based on an error state Kalman filter fusion IMU data designed on the basis of the improved LM nonlinear optimization algorithm is proposed.

[0059] The specific embodiments of the present invention are further described below.

[0060] System composition

[0061] The multi-machine collaborative positioning system includes a software simulation system and a real-time positioning algorithm system.

[0062] The software simulation system runs on ubuntu20.04 (Linux operating system mainly for desktop applications) operating system and ROS-noetic (open source robot operating system) software environment, using Gazebo as the physical simulation platform. Based on ROS, the UWB ranging simulation model is designed, and the scene is built according to the real indoor scene in the simulation environment for algorithm verification.

[0063] The real-time positioning algorithm runs on the Ubuntu 20.04 (Linux operating system for desktop applications) operating system and ROS-noetic (open source robot operating system) software environment. The algorithm consists of a multilateral positioning module and an error state Kalman filter module.

[0064] System Modeling

[0065] The straight-line distance between the transmitting end and the receiving end of the UWB radio frequency module can be measured based on TOA to calculate the straight-line distance according to the signal transmission time, or based on the signal strength model to measure the straight-line distance according to the attenuation of the signal strength. The UWB radio frequency module used in the present invention measures the distance based on TOA.

[0066] In the UWB positioning system, the UWB RF module with a known real position is called a UWB base station, and the UWB RF module with an estimated position is called a UWB tag. Assuming the UWB wave speed is v, the timestamp when the UWB signal is sent from the UWB base station is stamp1, and the timestamp when the UWB tag receives the UWB signal is stamp2, the measured distance is:

[0067]

[0068] The multi-lateration positioning method measures the distance from each UWB base station to the UWB tag, and obtains the position of the measured UWB tag through the relationship equation between coordinates and distance.

[0069] Suppose there are N UWB base stations, and the coordinates of the i-th base station are p i (x i ,y i ,z i ), d i is the distance between the UWB base station and the UWB tag, p(x, y, z) is the coordinate of the UWB tag to be determined, and we can get:

[0070]

[0071] Starting from the second row, we subtract the first row row by row to obtain the following system of equations:

[0072]

[0073] in, Writing equation (3) in matrix form yields:

[0074]

[0075] make

[0076]

[0077] have to

[0078] Ax=b (7)

[0079] Among them, A is a (N-1,3)-dimensional matrix and b is an N-1-dimensional column vector.

[0080] System simulation platform

[0081] A simulation platform was built based on gazebo, a UWB ranging model with Gaussian noise was established in gazebo, a multi-UAV simulation based on PX4 was designed, and a 3D model of indoor building positioning was built based on blender to simulate the post-disaster high-altitude multi-aircraft collaboration scenario.

[0082] Different modules in the simulation design communicate with each other using Topic in the ROS system (a communication mechanism in the ROS operating system).

[0083] UWB ranging model

[0084] The UWB RF module measures the distance between the UWB base station drone and the UWB tag drone based on the TOA principle. Through actual measurement, it is found that the ranging error of the UWB RF module approximately obeys the Gaussian distribution.

[0085] Let d be the actual distance between UWB modules, then the distance measurement value is:

[0086]

[0087] is the UWB measurement distance obtained based on TOA, and ε is the distance measurement error that satisfies the following Gaussian distribution:

[0088] ε~N(u,σ 2 ) (9)

[0089] First, add the drone real position plug-in to the drone SDF file, obtain the three-dimensional position of each UWB base station drone and UWB tag drone based on ROS, calculate the distance between each UWB base station drone and UWB tag drone, and then add Gaussian noise as the distance measurement signal of the UWB RF module.

[0090] Multi-UAV Simulation Design

[0091] Iris drone based on PX4 ( Figure 1 ) designs a multi-UAV simulation platform, and the present invention develops a multi-UAV position control program based on this platform.

[0092] On the Gazebo simulation platform, the unlocking status and posture information of each drone in the simulation environment are obtained in real time based on MAVROS and PX4. The keyboard control node is designed based on ROS, and the position of the drone in the simulation is controlled by reading the keyboard input in real time. The keyboard control instructions are shown in the table.

[0093] Table 1 Keyboard control program control instructions

[0094]

[0095] The principle of selecting the drone to be controlled is as follows:

[0096] By default, drone 0 is a UWB tag drone. When the program starts, it controls the UWB tag drone by default. When the number of the drone with the corresponding number is entered, the C++ program written based on ROS sets the control command of the subsequent keyboard position control command to the current drone. When a new number is entered again, it switches to controlling the drone with the newly entered number.

[0097] The principle of controlling the position of the drone is as follows:

[0098] Assume that the current position of the drone is p(x, y, z), the keyboard control command i / I controls the drone to move forward k units, and the current quaternion of the drone is q. After entering the control command, the position of the drone is:

[0099] p'=p+qkq -1 (10)

[0100] Then based on MAVROS, control the drone to fly to position point p'.

[0101] Indoor building positioning 3D model

[0102] A 3D indoor model was built based on the blender modeling platform as a multi-UAV simulation scene ( Figure 2 ), the indoor scene size is: 16m×24m×4m.

[0103] How multiple drones fly in a simulation scenario Figure 3 shown.

[0104] Real-time positioning algorithm system

[0105] The real-time positioning algorithm designed by the present invention consists of a multilateral positioning module and an error state Kalman filter module. The multilateral positioning module uses the improved LM nonlinear optimization algorithm as the core to perform real-time positioning calculation on the UWB ranging signal, fuses the positioning calculation result with the accelerometer and gyroscope data of the IMU to estimate the position of the UWB tag drone, and outputs the real-time positioning result.

[0106] The LM nonlinear least squares optimization algorithm has the characteristics of fast convergence speed and convenient parameter adjustment, and is widely used in fields such as nonlinear optimization problems. Taking into account that the traditional LM nonlinear optimization algorithm relies on the selection of initial optimization parameters, the present invention improves the LM nonlinear optimization algorithm by introducing position feedback design.

[0107] For the i-th UWB base station and UWB tag, the positioning error is:

[0108]

[0109] Define the error vector:

[0110]

[0111] The Jacobian matrix is:

[0112]

[0113] Construct a least squares problem:

[0114]

[0115] Expand f(p) into the first order Taylor:

[0116] f(p+Δp)≈f(p)+J(p)Δp(15)

[0117] The descending vector is taken as:

[0118]

[0119] Among them, u k The radius of the trust optimization region, also known as the optimization step size.

[0120] Taking the derivative of formula (16) and setting the derivative to zero can simplify it to:

[0121] (J(p) T J(p)+u kI) Δp = -J(p) T f(p) (17)

[0122] Where I is the unit matrix, the new parameters are:

[0123] p new =p+Δp=p-[J(p) T J(p)+u k I] -1 J(p) T f(p) (18)

[0124] During the iteration process, when the new error vector sum of squares is smaller than the original one, the radius of the trust optimization area u is reduced. k , otherwise increase the radius u of the trust optimization area k . Set the tolerance ∈ and the maximum number of iterations max_iter. When the sum of squares of the error vector is less than the tolerance ∈ or the number of iterations is greater than the maximum number of iterations max_iter, the iteration stops.

[0125] The above is the calculation process of the LM nonlinear optimization algorithm for a positioning cycle. By feeding back the positioning point of the positioning cycle as the initial optimization point of the next positioning cycle, an improved LM nonlinear optimization algorithm is designed. The structure diagram of the algorithm is shown in Figure 4 shown.

[0126] This method does not rely on the selection of initial optimization point parameters. In the positioning scenario of large-scale UAV dynamic flight, the multilateral positioning algorithm will not fall into the wrong local optimal solution, causing algorithm distortion.

[0127] The position solution problem in the present invention sets the initial step length u of the LM nonlinear optimization algorithm k =0.5, tolerance∈=1×10 -6 , the maximum number of iterations max_iter = 100, and the initial optimization parameter is set to the take-off point of the drone. The above process can be used to solve the multilateral positioning problem.

[0128] The following simulation is performed to verify the advantages of the improved LM nonlinear optimization algorithm designed in the present invention over the traditional LM nonlinear optimization algorithm.

[0129] The four UWB base station drones are initially located at coordinates x1 = (10, 0, 0), x2 = (0, -22, 0), x3 = (-10, 0, 0), and x4 = (0, 6, 0). Based on the multi-drone position control program designed by the present invention, the four UWB base station drones take off and perform fixed-altitude flight, and their positions are respectively located at coordinates x1 = (10, 0, 10), x2 = (0, -22, 6), x3 = (-10, 0, 10), and x4 = (0, 6, 6).

[0130] Based on the multi-UAV simulation position control program designed in the present invention, the UWB tag UAV takes off to (0,0,2), and controls the UAV to fly vertically upward from this point to (0,0,15), obtains the solution result of the multilateral positioning algorithm based on the traditional LM nonlinear optimization algorithm, and draws the positioning position curve and the real position curve as shown in Figure 5 shown.

[0131] from Figure 5 It can be seen that when the drone rises to (0,0,13.5), the estimated position of the positioning system deviates seriously from the actual position curve. This is because the optimization initial point is far away from the actual position point and falls into a local optimal solution. At this time, the positioning system fails. The multilateral positioning system based on the LM method is not suitable for dynamic positioning problems. The positioning results of the improved LM nonlinear optimization algorithm are as follows Figure 6 shown.

[0132] Compared with the multilateral positioning method based on the traditional LM nonlinear optimization, this method will not fall into the local optimal solution, and the positioning system will not fail during the long-distance dynamic flight of the UAV.

[0133] The accelerometer and gyroscope carried by IMU are used to measure the acceleration and angular velocity of the robot. As a lightweight sensor device, IMU is often carried on drones and unmanned vehicles and combined with other sensors for fusion positioning. Based on the multilateral positioning system based on the improved LM method, the UWB fusion positioning system is designed by fusing the acceleration data and angular velocity data measured by IMU based on the error state Kalman filter, so as to further improve the positioning accuracy of the UWB positioning system in the multi-machine collaborative positioning scenario.

[0134] In ESKF, the original state variable is usually called the nominal state variable, and the state variable in ESKF is called the error state variable. Let the state variable be:

[0135] x=[p,v,R,b g ,b a ,g] T (19)

[0136] Among them, p is displacement, v is velocity, R is rotation, b g is the gyroscope bias, b a is the accelerometer zero bias, and g is gravity.

[0137] The discrete-time equation of motion for the nominal state variables can be written as:

[0138]

[0139] b g (t+Δt)=b g (t) (23)

[0140] b a (t+Δt)=b a (t) (24)

[0141] g(t+Δt)=g(t) (25)

[0142] The discrete form of the error state variable is:

[0143] δp(t+Δt)=δp+δvΔt (26)

[0144]

[0145] δb g (t+Δt)=δb g +η g (29)

[0146] δb a (t+Δt)=δb a +η a (30)

[0147] δg(t+Δt)=δg (31)

[0148] Write the above discrete form into matrix form:

[0149] δx(t+Δt)=Fδx+w,w~N(0,Q) (32)

[0150] Among them, w is Gaussian noise, and the covariance matrix Q is:

[0151] Q=diag(03,Cov(η v ),Cov(η θ ),Cov(η g ),Cov(η a ),03) (33)

[0152] The matrix F is:

[0153]

[0154] The prediction process of the ESKF includes the prediction of the nominal state based on the IMU integral and the prediction of the error state variable:

[0155] P pred =FPF T +Q (35)

[0156] Formula (35) is the prediction of covariance, where P predis the predicted covariance matrix, and P is the corrected covariance matrix. Since the error state variable is reset to zero in each update iteration, the prediction of the error state variable is not considered in the design of the error state Kalman filter.

[0157] The observation equation is abstracted as h, v is the observation noise, z is the observation data, and V is the covariance matrix of the noise. Here the observation data is the three-dimensional positioning position estimated by the multilateral positioning method:

[0158] z=h(x)+v,v~N(0,V)(36)

[0159] Error state update process:

[0160] K=P pred H T (HP pred H T +V) -1 (37)

[0161] δx=K(zh(x pred )) (38)

[0162] x=x pred +δx (39)

[0163] P=(I-KH)P pred (40)

[0164] H is obtained by the chain rule:

[0165]

[0166] The data transmission frequency (100Hz) of the IMU sensor used in the simulation experiment is greater than the data transmission frequency (50Hz) of the UWB radio module. In the engineering implementation, the IMU data is subjected to short-term trajectory calculation to update the robot's posture. The state variables at the next moment are obtained by measuring the acceleration data and angular velocity data of the IMU. The trajectory calculation based on the Euler numerical integration method is as follows:

[0167]

[0168] The structure of the multi-machine collaborative positioning system is as follows: Figure 7 shown.

[0169] Also based on the multi-UAV position control program designed by the present invention, the four UWB base station UAVs perform fixed-altitude flight after takeoff, and their positions are respectively located at coordinates x1=(10,0,10), x2=(0,-22,6), x3=(-10,0,100, x4=(0,6,6), and the UWB tag UAV is located at the coordinate point (0,0,2) after takeoff.

[0170] The UWB tag drone is controlled to fly in a circle from the coordinate point (0,0,2). The multi-machine collaborative positioning system designed above estimates the position of the UWB tag drone and draws the position estimation curve of the UWB tag drone as shown in the figure. Figure 8 As shown in the figure, the root mean square error of positioning is 0.0410m, and the positioning accuracy reaches the centimeter level.

[0171] In the present invention, the multi-machine collaborative UWB positioning system accurately estimates the position of the UWB tag drone by deploying multiple UWB base station drones. The UWB base station drone flies outside a high-altitude building and uses outdoor positioning methods such as GPS for positioning. The UWB tag drone calculates the position through the UWB ranging signal of each UWB base station drone, and fuses the preliminary position estimate obtained by the position solution with the accelerometer and angular velocity meter of the IMU on the tag drone, thereby estimating the position of the UWB tag drone. The multi-machine collaborative UWB positioning system has high positioning accuracy in the indoor environment of a post-disaster building and is more accurate in height estimation. It has lower cost, lighter weight, low computing resources, is suitable for deployment on small exploration drones, and can be compatible with more types of drones. Using UWB and IMU sensor equipment, the multi-machine collaborative UWB positioning system can effectively estimate the three-dimensional position of the drone.

[0172] The present invention can well meet the needs of multi-machine collaborative positioning in post-disaster high-altitude building exploration missions, provides a lightweight sensor positioning solution, and achieves high-precision positioning that is not affected by environmental factors such as dust.

[0173] The multi-machine cooperative UWB positioning system designed by the present invention has the following functions:

[0174] 1. Low computational complexity, achieving high-precision positioning with low computing resources.

[0175] 2. The algorithm is highly efficient and has a high positioning frequency.

[0176] 3. Achieve high-precision real-time positioning in complex environments.

[0177] The technical advantages of the present invention mainly include:

[0178] 1. A high-precision UWB positioning system is implemented, which overcomes the shortcomings of the traditional LM nonlinear optimization algorithm and can be applied to dynamic collaborative positioning of multiple UAVs over a large range.

[0179] 2. A multi-machine collaborative UWB positioning system was implemented, and the multi-UAV position control program under the Gazebo simulation platform was verified. The positioning accuracy reached the centimeter level in the simulation scenario. The positioning system only requires the IMU of the UWB RF module to achieve high-precision positioning, which can be applied to the positioning problem of post-disaster high-altitude building exploration missions.

[0180] The above content is a further detailed description of the present invention in combination with specific / preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, without departing from the concept of the present invention, it can also make several substitutions or modifications to these described embodiments, and these substitutions or modifications should be regarded as belonging to the protection scope of the present invention. In the description of this specification, the description of reference terms "an embodiment", "some embodiments", "preferred embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in combination with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily target the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In the absence of mutual contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples. Although the embodiments of the present invention and their advantages have been described in detail, it should be understood that various changes, substitutions and modifications can be made in the present invention without departing from the scope of protection of the patent application.

Claims

1. A multi-machine collaborative ultra-wideband (UWB) positioning method for high-altitude building scenes, characterized in that: The following steps are involved: S1: Initialize the multi-machine cooperative UWB positioning system, configure the parameters of the error state Kalman filter ESKF and LM (Levenberg-Marquardt) nonlinear optimization algorithm, and prepare to fuse IMU data and UWB ranging data; S2: Start the UWB radio frequency module to collect the ranging signals between multiple UWB base station drones and UWB tag drones to provide raw data for multilateral positioning; at the same time, use the IMU sensor to collect the acceleration and angular velocity data of the UWB tag drone to provide the motion model input for the state Kalman filter ESKF; S3: Combine the UWB ranging data and IMU data obtained in step S2 to perform a multi-lateration positioning process, start the LM algorithm to perform a preliminary real-time positioning solution, and estimate the relative position error between the UWB base station and the UWB tag; S4: Based on the LM algorithm, a Jacobian matrix is ​​constructed, which contains the relationship between the measured distance from the UWB base station drone to the UWB tag drone and the predicted position; the descent vector of the position parameter of the UWB tag relative to the observation error is solved, and the position parameter is updated by the least squares method to prepare the observation data of the UWB tag for the prediction step of the state Kalman filter ESKF; S5: performing iterative optimization of the LM algorithm to solve the UWB multilateration positioning problem until a preset condition is met, preferably the sum of squares of the error vector is less than a preset tolerance or a maximum number of iterations is reached; S6: Use the IMU data collected in step S2 to update the posture information of the UWB tag drone through track calculation, and provide the latest motion state for the state Kalman filter ESKF state update step; S7: Input the position parameters of the UWB tag optimized in step S5 as observation data into the state Kalman filter ESKF, and perform state prediction and error correction in combination with the motion state updated in step S6; S8: Update the position and velocity estimates of the UWB tag drone in the state Kalman filter ESKF based on the prediction and observation data, and integrate them to achieve robust processing of UWB ranging errors and IMU measurement noise; S9: Combining the results of the state Kalman filter ESKF fusion processing in step S8 and the position parameters optimized by the LM algorithm in step S5, the precise real-time position of the UWB tag drone is output.

2. The multi-machine cooperative ultra-wideband (UWB) positioning method for high-altitude building scenes as claimed in claim 1 is characterized in that: In step S3: for the UWB base station and the UWB tag, the positioning error is calculated, which is based on the difference between the measured distance from the UWB base station drone to the UWB tag drone and the predicted position; an error vector is defined to summarize the positioning errors between all UWB base stations and UWB tags, which builds a foundation for the subsequent least squares problem; In step S4: construct a Jacobian matrix containing partial derivative information of the measured distance from the UWB base station drone to the UWB tag drone with respect to the predicted position parameters; construct a least squares problem, the goal of which is to minimize the sum of squares of the error vector to solve the optimal position parameters; Perform a first-order Taylor expansion on the error vector to approximate the linear behavior of the nonlinear error function near the current estimation point; Solve for the descent vector, which is used to adjust the location parameters at each iteration to reduce the prediction error; In step S5: during the iterative optimization process, the radius of the trusted optimization area, that is, the optimization step size, is adjusted according to the sum of the squares of the error vectors until the termination condition is met; a position feedback mechanism is introduced to use the solution of the current positioning cycle as the initial optimization point of the next cycle to improve the algorithm's insensitivity to the initial parameter selection.

3. The multi-machine collaborative ultra-wideband (UWB) positioning method for high-altitude building scenes as described in claim 1 or 2, characterized in that: In step S3, the system modeling of the multilateration positioning is implemented by the following method, including: Determine a distance measurement method based on the time of arrival TOA principle to calculate the straight-line distance between UWB radio frequency modules; the role of the UWB base station is the UWB radio frequency module with a known position, and the UWB tag is the UWB radio frequency module to be located; Using the timestamps of the signal sent by the UWB base station and the signal received by the UWB tag, the distance between the two is calculated using the known UWB signal wave speed; Based on the distance measurement from the UWB base station to the UWB tag, a multilateral positioning equation group is constructed for determining the position of the UWB tag; Convert the multilateral positioning equations into matrix form, construct coefficient matrices and constant vectors, and form a linear equation system that can be solved numerically; The linear equations are solved by numerical methods to obtain the three-dimensional position coordinates of the UWB tag.

4. The multi-machine collaborative ultra-wideband (UWB) positioning method for high-altitude building scenes according to any one of claims 1 to 3, characterized in that: In step S4: defining state variables to provide a basic data structure for prediction and updating of the ESKF; preferably, the state variables include displacement, velocity, rotation, gyroscope bias, accelerometer bias and gravity; In step S5: establishing a discrete time motion equation of the nominal state variables to describe the evolution of the drone state over time, including the update of displacement, velocity and rotation; In step S7: In the ESKF, state prediction and error correction are performed based on the predicted data and the observed data, and the position and velocity estimates of the UAV are updated using the observation equation and the motion model based on the IMU data.

5. The multi-machine cooperative ultra-wideband (UWB) positioning method for high-altitude building scenes according to any one of claims 1 to 4, characterized in that: In the process of multilateral positioning, the acceleration data and angular velocity data measured by the IMU are used to predict the nominal state of the UAV based on the Euler numerical integration method, and the state variables at the next moment are calculated; when calculating the track, the IMU data is used for short-term track calculation to fill the posture changes during the UWB data update interval; in the state Kalman filter ESKF, the position and velocity estimates of the UAV are updated based on the predicted data and observed data, combined with the high-frequency data of the IMU and the low-frequency data of the UWB.

6. The multi-machine cooperative ultra-wideband (UWB) positioning method for high-altitude building scenes according to any one of claims 1 to 5, characterized in that: In step S6: when the IMU data transmission frequency is higher than the UWB RF module data transmission frequency, the IMU data is used to perform short-term trajectory estimation to update the drone posture information.

7. The multi-machine cooperative ultra-wideband (UWB) positioning method for high-altitude building scenes according to any one of claims 1 to 6, characterized in that: In step S7: the Jacobian matrix of the observation equation is obtained by the chain rule, and the matrix is ​​used to convert the changes in the observation space into the changes in the state space, and then update the state estimate.

8. The multi-machine cooperative ultra-wideband (UWB) positioning method for high-altitude building scenes according to any one of claims 1 to 7, characterized in that: In step S8: the robust processing of UWB ranging error and IMU measurement noise is realized by fusing in the ESKF, including: quantifying the noise in the IMU data using the Gaussian noise model and the covariance matrix, constructing the Jacobian matrix of the observation equation to convert the change in the observation space into the state space; calculating the Kalman gain, which combines the prediction covariance matrix and the observation noise covariance matrix; updating the state estimate and the covariance matrix, fusing the observation data and the predicted state to reduce the influence of the UWB ranging error and the IMU measurement noise.

9. A multi-machine collaborative ultra-wideband (UWB) positioning system for post-disaster high-altitude building scenes, characterized in that: include: A plurality of UWB radio frequency modules, including a plurality of UWB base stations respectively mounted on the plurality of UWB base station drones and a UWB tag mounted on the UWB tag drone, for measuring the distance between the plurality of UWB base stations and the UWB tags; Inertial measurement unit IMU, carried on the UWB tag drone, is used to provide moving speed and direction information to assist UWB positioning; Multiple UWB base station drones and at least one UWB tag drone; A main control module communicates with the multiple UWB radio frequency modules and the inertial measurement unit IMU, and is used to execute the multi-machine collaborative ultra-wideband UWB positioning method as described in any one of claims 1 to 8.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the multi-machine collaborative ultra-wideband (UWB) positioning method as described in any one of claims 1 to 8 is implemented.

Citation Information

Cited By

  • Cross-region accurate indoor positioning method

    CN119835758A

  • Inspection method of industrial inspection system based on multi-modal fusion

    CN120368983A

  • Multi-mobile-machine cooperative control method and device, electronic equipment and storage medium

    CN120491564A

  • Control method and system suitable for low-altitude unmanned aerial vehicle landing parking apron

    CN120560319A

  • Connector integrating high-precision positioning circuit and system and positioning method thereof

    CN121486964A