Multi-unmanned vehicle cooperative positioning method based on graph optimization UWB / IMU / GNSS

Abstract

The invention discloses a graph optimization-based UWB / IMU / GNSS multi-unmanned vehicle cooperative positioning method, which comprises a global satellite navigation system GNSS, an ultra wide band (UWB) system, an inertial measurement unit IMU and a robot motion control system, and is characterized in that the UWB system comprises a UWB label and three UWB base stations, the UWB label is deployed on a target unmanned vehicle to be positioned, and the UWB label is deployed on the target unmanned vehicle to be positioned; and the three UWB base stations are respectively arranged on the other three unmanned vehicles as mobile base station unmanned vehicles. The GNSS module and the IMU module are deployed on the unmanned vehicle to serve as a movable UWB base station, and GNSS absolute position information and IMU relative motion information are fused by adopting an extended Kalman filtering algorithm; and according to the determined position of the unmanned vehicle in the movable base station, a distance observation value between the label and the base station is obtained based on a single-side bidirectional distance measurement method, a factor graph model fusing UWB distance measurement constraint and IMU motion constraint is constructed, and a factor graph optimization algorithm is adopted to optimize the position of the target unmanned vehicle. According to the method, the cost of a traditional fixed base station is reduced through the deployment of a mobile base station unmanned vehicle, and the precision and robustness of UWB system positioning in a complex environment are improved in combination with a multi-source sensor data fusion strategy.

Description

Technical Field

[0001] The present invention relates to the field of collaborative positioning technology, and in particular to a multi-unmanned vehicle collaborative positioning method based on UWB / IMU / GNSS graph optimization. Background Art

[0002] Collaborative positioning technology achieves higher accuracy and reliability through collaboration between multiple devices or systems. By integrating information from different positioning sensors or systems, this technology fuses multi-source data to compensate for the shortcomings of a single sensor and improve overall positioning accuracy. Data fusion is a key means of achieving this goal, combining information from multiple data sources to produce more accurate and reliable results.

[0003] In practical applications, the measurement accuracy of a single sensor is limited by its inherent principles and performance. For example, the Global Navigation Satellite System (GNSS) can receive satellite signals in open environments and provide global, absolute position information. However, in obstructed environments, such as those surrounding tall buildings in cities, indoors, or in dense forests, satellite signals are blocked, reducing the number of satellites that can be received and significantly reducing positioning accuracy. Inertial Measurement Units (IMUs) measure acceleration and angular velocity to infer position and attitude, providing real-time motion information. However, these errors accumulate over time, leading to a gradual decline in positioning accuracy during long navigation missions. Ultra-wideband (UWB) technology deploys tags and base stations, transmits and receives pulse signals, and uses the pulse signal's time of flight to calculate the distance between the tag and base station, ultimately determining the target's location. This provides high-precision positioning information. However, UWB technology suffers from high deployment costs, multipath effects, and non-line-of-sight (NLOS) issues. In environments with multiple obstacles, pulse signals cannot propagate directly but instead reach the receiver through reflection, refraction, and diffraction. This makes it impossible to accurately measure the pulse signal's true arrival time, thus affecting positioning accuracy. Summary of the Invention

[0004] The present invention aims to provide a multi-unmanned vehicle collaborative positioning method based on UWB / IMU / GNSS graph optimization. By fusing multi-source heterogeneous sensor data, the advantages of each sensor are fully utilized to improve the positioning accuracy and stability of unmanned vehicles in complex environments.

[0005] To achieve the above objectives, the present invention adopts a graph-optimized UWB / IMU / GNSS-based collaborative positioning method for multiple unmanned vehicles. This method relies on the interaction of a global satellite navigation system (GNSS), an ultra-wideband system (UWB), an inertial measurement unit (IMU), and the unmanned vehicle motion control system. The UWB system includes one UWB tag and three UWB base stations. To reduce the deployment cost of fixed UWB base stations, unmanned vehicles equipped with GNSS systems and IMU modules are used as mobile UWB base stations. Data fusion is performed through an extended Kalman filter (EKF) to provide accurate base station location information for UWB positioning. The single-sided two-way ranging (SS-TWR) method is used to measure the distance between the UWB tag and the base station. The relative motion information of the IMU and the ranging information of the UWB are fused using a factor graph. The graph optimization method effectively handles the constraint relationship between sensor data, improves the adaptability and robustness of the positioning system to complex environments, and reduces the error of UWB positioning caused by non-line-of-sight and multipath effects by fusing the relative motion information of the IMU, thereby obtaining more accurate and stable positioning results.

[0006] The following steps are involved:

[0007] GNSS receives radio signals transmitted by multiple satellites and uses pseudo-range measurement to calculate the distance between the satellite and the GNSS. When the pseudo-ranges of at least four satellites are measured, the GNSS position is solved using the least squares method.

[0008] The IMU consists of an accelerometer and a gyroscope. The accelerometer measures the carrier's acceleration based on Newton's second law; the gyroscope measures the carrier's angular velocity based on the principle of conservation of angular momentum.

[0009] An unmanned vehicle equipped with a GNSS system and IMU module acts as a mobile UWB base station. The base station remains stationary while using GNSS and IMU to measure its own three-dimensional coordinates in real time and meet positioning accuracy requirements. The UWB tag transmits a signal, which the base station receives. The distance between the tag and the base station is calculated by measuring the signal's flight time, and the tag's position is determined using trilateration.

[0010] Initialize each sensor before collecting data. Because GNSS signals may be interfered with and cause data jumps, IMUs may produce abnormal measurements due to vibration, and UWB may cause measurement deviations due to multipath effects, it is necessary to set reasonable thresholds based on the scenario and carrier motion characteristics, and use the threshold method to eliminate outliers.

[0011] Establish GNSS / UWB / IMU measurement models, including GNSS pseudorange measurement model, IMU acceleration and angular velocity measurement model, and UWB ranging model based on SS-TWR;

[0012] The cleaned GNSS absolute position information and IMU relative motion information are fused to determine the position of the mobile UWB base station unmanned vehicle. In the process of using the extended Kalman filter algorithm to fuse data, the angular velocity and acceleration measurements of the inertial measurement unit (IMU) are used as prediction quantities, and the position measurements of the global satellite navigation system (GNSS) are used as observation quantities. The prediction quantities and observation quantities are weighted and corrected by calculating the Kalman gain matrix to optimize the position of the mobile base station unmanned vehicle. The Kalman gain formula is:

[0013]

[0014] Among them, P k|k-1 is the covariance matrix of the prior estimate at time k, H k is the observation matrix, R k is the covariance matrix of the observation noise.

[0015] Define the state node based on the cleaned GNSS absolute position information and IMU relative motion information of the target unmanned vehicle to be positioned:

[0016]

[0017] Among them, p k is the position at time k, v k is the velocity at time k, q k is the attitude quaternion at time k.

[0018] According to the UWB / IMU measurement model, factor nodes and factor residuals are defined to construct a factor graph. The state node is connected to the state node at the adjacent time through the IMU factor to reflect the time evolution of the state; the UWB factor node is connected to the position node p k and UWB measurement nodes, reflecting the relationship between UWB measurement and position state; the initial state node is connected to the prior information through the prior factor; however, there will be a residual between the actual measurement value and the theoretical value estimated based on the state, and the optimal state estimation is obtained by minimizing the sum of squares of the global residual. The formula for the UWB factor residual is:

[0019] e uwb,i =d m,i -||pp i ||

[0020] where p i is the location of the i-th UWB base station, d m,i is the distance measurement, p is the position of the UWB tag;

[0021] The objective function is defined as the sum of squares of all factor residuals. The Levenberg-Marquardt method is used to continuously adjust the values ​​of state variables through cyclic iterations to converge the objective function and optimize the positioning accuracy of the target unmanned vehicle. The formula of the objective function is:

[0022]

[0023] Among them, e i (x) is the residual function of the i-th factor, W i is the observation noise covariance matrix of the factor, which represents the confidence in the observation value.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] (1) Compared to single-sensor positioning technology, the present invention integrates UWB / IMU / GNSS data to achieve collaborative positioning. Single UWB is susceptible to multipath and non-line-of-sight effects, and single GNSS is susceptible to satellite signal obstruction. Integrating IMU data improves positioning continuity and accuracy.

[0026] (2) Compared with the traditional fixed base station UWB positioning method, the present invention adopts a mobile UWB base station to determine the position of the mobile base station unmanned vehicle by fusing GNSS absolute position information and IMU relative motion information, which significantly reduces the deployment cost.

[0027] (3) Compared with other data fusion algorithms, this paper adopts a factor graph optimization algorithm. The UWB measurement error distribution is usually non-Gaussian, and the IMU error accumulation characteristic also makes the system exhibit strong nonlinearity. The factor graph flexibly models these complex characteristics by defining different factor nodes, accurately reflecting the actual situation of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a schematic diagram of the process framework of the present invention;

[0029] Figure 2 This is a flowchart of positioning of multiple UWB base stations of the present invention;

[0030] Figure 3 This is a flow chart of the UWB / IMU fusion positioning algorithm based on factor graph of the present invention;

[0031] Figure 4 Schematic diagram of the unmanned vehicle based on kinematic model constraints of the present invention. DETAILED DESCRIPTION

[0032] It should be noted that, unless there is any conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0033] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is in no way intended to limit the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0034] To make the technical solutions and advantages of the present invention more clear, the technical solutions in the embodiments of the present invention are clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention:

[0035] like Figure 1 A multi-unmanned vehicle collaborative positioning method based on UWB / IMU / GNSS based on graph optimization is shown, including the following steps:

[0036] S1: Deploy the GNSS global satellite navigation system, UWB system and IMU inertial measurement unit to obtain three-dimensional coordinate information, relative distance information, and three-axis angular rate and acceleration data respectively;

[0037] S2: Initialize the sensor. During data collection, the GNSS signal may be interfered with and cause data jumps. The IMU's zero bias affects the measurement value, and the UWB is affected by the multipath effect, resulting in measurement deviation. The collected data needs to be cleaned.

[0038] S3: Establish UWB / GNSS / IMU measurement model;

[0039] S4: Measure the absolute position information of the mobile UWB base station unmanned vehicle through the GNSS system, integrate the relative motion information of the IMU, improve the continuity and accuracy of the base station positioning, and determine the position information of the mobile UWB base station unmanned vehicle that meets the positioning accuracy;

[0040] S5: Based on the determined position of the mobile UWB base station unmanned vehicle, measure the distance between the tag and the base station, use the distance between the tag and multiple base stations to calculate the position of the UWB tag, integrate the relative motion information of the IMU, and optimize the positioning accuracy of the target unmanned vehicle carrying the tag.

[0041] The above technical solution is used to improve the traditional UWB positioning system. The unmanned vehicle carrying the GNSS system and IMU module is used as a mobile UWB base station. The GNSS absolute position information and the IMU relative motion information are fused through the EKF algorithm to reduce the measurement error caused by insufficient GNSS satellite signals, provide accurate position information of the mobile base station unmanned vehicle for UWB positioning, and reduce the deployment cost of fixed UWB base stations.

[0042] Using an improved UWB positioning system, a factor graph model is constructed based on the distance measurements between three UWB base stations and the UWB tag, as well as the relative motion information from the IMU. This data fusion compensates for the shortcomings of a single sensor and reduces positioning errors. When a target unmanned vehicle carrying a UWB tag enters a blind spot, the IMU is used for short-term positioning. Compared to a single UWB base station, which is susceptible to non-line-of-sight and multipath interference, the factor graph algorithm optimizes the continuity and stability of the target vehicle's positioning.

[0043] The specific steps of step S1 are:

[0044] S11: GNSS receives radio signals transmitted by multiple satellites, measures the time from the transmission to the reception of satellite signals, and calculates the distance between the satellite and the receiver using the pseudo-range measurement method. When the GNSS can obtain the pseudo-ranges of at least four satellites simultaneously, it performs positioning using the least squares method. The formula for the pseudo-range measurement method is:

[0045]

[0046] Among them, (x, y, z) is the position of GNSS, (x j ,y j ,z j ) is the satellite coordinate, δt is the receiver clock error, n GNSS is Gaussian noise;

[0047] S12: The IMU module has a built-in accelerometer and gyroscope. The accelerometer measures the carrier's acceleration based on Newton's second law, and the gyroscope measures the carrier's angular velocity based on the principle of conservation of angular momentum.

[0048] S13: The unmanned vehicle equipped with a GNSS system and an IMU module is used as a mobile UWB base station. When the base station measures its own three-dimensional coordinates through GNSS and IMU and meets the positioning accuracy requirements, the unmanned vehicle is stationary. At this time, the UWB tag transmits the signal and the base station receives the signal. The distance between the two is calculated by measuring the flight time of the signal from the tag to the three base stations. Based on the distance information between the tag and the three base stations, the position of the target unmanned vehicle carrying the tag is calculated using the three-sided positioning method.

[0049] The positioning of multiple UWB base stations in step S13 is as follows Figure 2 As shown, it is characterized in that it specifically includes the following steps:

[0050] S131: The UWB tag actively sends data and records the sending timestamp T0. After the UWB base station receives the data, it processes it for a delay time T reply Send a response signal to the UWB tag, and the tag receives the response signal returned by the base station at time t3;

[0051] S132: The distance formula from the UWB tag to the base station is:

[0052] D=c×T prop

[0053] Where c is the propagation speed of electromagnetic waves in the air. The formula for the one-way propagation time of the signal from the tag to the base station in unilateral two-way ranging is:

[0054]

[0055] S133: Using a tag and three base stations, a two-way ranging algorithm is used to obtain distance information between the tag and each base station. Using trilateration, a circle is drawn with each known base station coordinate as the center and the corresponding distance as the radius. The intersection of the three circles is the location of the tag.

[0056] The formula for calculating the UWB tag position coordinates in step S133 is:

[0057]

[0058] Where c×T prop1 , c×T prop2 , c×T prop3 are the distances from the UWB tag to the three known UWB base stations, (x, y) are the position coordinates of the UWB tag in the UWB coordinate system, (x1, y1), (x2, y2), and (x3, y3) are the position coordinates of the three known UWB base stations.

[0059] The specific steps of step S2 are:

[0060] S21: Initialize each sensor before collecting data, start GNSS to obtain the initial position and velocity, use the initial position output by GNSS as the initial state of the IMU, calibrate the IMU zero bias in a static state, and calculate the initial attitude based on the accelerometer measurement value. Use the two-way ranging method to eliminate the clock deviation between the UWB tag and the base station, and measure the initial distance based on the coordinates of the UWB base station.

[0061] S22: For the collected GNSS data, a reasonable pseudorange change threshold is set. When the pseudorange difference between two adjacent measurements in the data exceeds the threshold, the data is considered an outlier and removed. The GNSS data outlier removal formula is:

[0062] |ρ k+1 -ρ k |>Δρ th

[0063] Among them, |ρ k+1 -ρ k | is the difference between two adjacent pseudorange measurements, Δρ th is the set pseudorange change threshold;

[0064] S23: For the collected UWB data, calculate the mean and standard deviation of the UWB measurement distance, set a reasonable threshold, and treat the data exceeding the threshold as outliers and remove them. The UWB data outlier removal formula is:

[0065]

[0066] Among them, d j is a certain measurement distance of the UWB system, is the mean value of UWB measurement distance, α is the set threshold, σ d is the standard deviation of UWB measurement distance;

[0067] S24: Place the IMU in a stationary state, collect data for a period of time, calculate the average output of the accelerometer and gyroscope as the zero bias estimate, and subtract the zero bias value from subsequent measurement data to eliminate the influence of the zero bias;

[0068] In step S4, the absolute positioning information of the GNSS and the relative motion information of the IMU are fused through the extended Kalman filter algorithm EKF, the result of the IMU calculation is used as the state prediction of the system, and the measurement result of the GNSS is used as the observation value. The optimal state of the system is estimated through the two steps of prediction and update, which significantly improves the positioning accuracy of the mobile UWB base station;

[0069] The EKF-based GNSS / IMU fusion positioning system is characterized by comprising the following specific steps:

[0070] S41: When the system is started and the GNSS signal is good and the positioning solution is completed, the position and velocity data output by the GNSS are directly obtained, and then the covariance matrix of the position and velocity is initialized based on the positioning accuracy and velocity measurement accuracy of the GNSS itself;

[0071] S42: Place the IMU in a stationary state for a period of time, average the measurement data of the accelerometer and gyroscope, obtain initial estimates of the accelerometer bias and gyroscope bias, calculate the pitch and roll angles using the accelerometer, and calculate the heading angle using the magnetometer, and convert these Euler angles into quaternions to represent the initial attitude;

[0072] S43: defining a state vector including position, velocity, attitude quaternion, accelerometer bias, and gyroscope bias, using the parameter values ​​obtained by initializing the sensor as initial values ​​of the state vector, and then combining the covariance matrices of the position, velocity, attitude, accelerometer bias, and gyroscope bias to form an initial state covariance matrix;

[0073] S44: Define the state transition equation, which is used to describe the change of the state vector over time. It includes the state transfer function, control input and process noise. The state transition equation is:

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

[0075] Where f is the state transition function, u k is the control input, w k is the process noise;

[0076] S45: According to the state transfer equation, combined with the state estimate at the current moment, the state at the next moment is predicted, and then the state transfer matrix is ​​obtained by taking the partial derivative of the state vector. The state transfer matrix, the state covariance matrix at the current moment, and the covariance matrix of the process noise are used to predict the state covariance matrix at the next moment. The state transfer matrix is:

[0077]

[0078] Where f is the state transfer function and x is the state vector;

[0079] S46: When receiving GNSS observation values, using an observation equation to describe the relationship between the observation values ​​and the state vector, the observation equation includes an observation function and observation noise, and linearizing the observation function to obtain an observation matrix;

[0080] S47: Calculate the Kalman gain based on the observation matrix, the covariance matrix of the predicted state, and the covariance matrix of the observation noise to balance the weights of the predicted value and the observed value when updating the state, and then use the Kalman gain to update the state and covariance. The formula of the Kalman gain is:

[0081]

[0082] Among them, P k|k-1 is the predicted state covariance matrix at time k, H k is the observation matrix, R k is the covariance matrix of the observation noise, which represents the uncertainty caused by the noise in the observations;

[0083] S48: extracting a position component from the updated state estimate through state prediction and observation update, the position component being a high-precision position estimate of the mobile UWB base station at the current moment, and continuously repeating the state prediction and observation update to update the position estimate of the mobile UWB base station in real time;

[0084] S49: When encountering the situation of GNSS signal loss or poor signal quality, appropriately adjust the weight of EKF fusion data and rely more on IMU data for short-term positioning.

[0085] In step S5, factor graph optimization is used to fuse the UWB and IMU data, and the motion constraints of the IMU and the distance constraints of the UWB are expressed as factors in the factor graph. A factor graph model is constructed, and the optimal state of the system is estimated by minimizing the sum of squared errors of all factors through the factor graph optimization algorithm to determine the position coordinates of the unmanned vehicle carrying the UWB tag.

[0086] The UWB / IMU data fusion positioning algorithm process based on factor graph is as follows Figure 3 As shown, it is characterized in that it includes the following steps:

[0087] S51: Perform initial pre-integration of the IMU, convert the system state into specific position and error parameters, and add them to the factor graph as variable nodes;

[0088] S52: When UWB observation data is detected, non-line-of-sight discrimination and elimination are performed, and a UWB ranging factor is added to the line-of-sight UWB measurement value, and an IMU pre-integration factor is added at the same time;

[0089] S53: Determine whether the number of state parameters to be optimized simultaneously in the factor graph reaches the set window size. When the window is full, perform marginalization to avoid repeated calculations.

[0090] S54: Perform joint optimization of multiple states and output the optimized tag position, velocity, attitude and IMU deviation. The optimization process is the graph optimization solution process. The difference between the observed value and the predicted value calculated for each factor, i.e., the residual value, and the Jacobian matrix corresponding to the residual are calculated. The objective function is defined as the global residual sum of squares. The global residual sum of squares is minimized using the Levenberg-Marquardt method. The formula of the objective function is:

[0091]

[0092] Among them, e i (x) is the residual function of the i-th factor, W i is the observation noise covariance matrix of the factor, which represents the confidence in the observation value.

[0093] Figure 4 It is a schematic diagram of the unmanned vehicle based on kinematic model constraints of the present invention.

[0094] The unmanned vehicle based on kinematic model constraints includes: a data transmission module, a flight controller, a power supply, a motor, a servo, an ammeter, an electronic speed controller (ESC), a GNSS module, an IMU module and a UWB module.

[0095] The data transmission module transmits the status information of the unmanned vehicle to the ground control station in real time, including position, speed, attitude, battery power, etc., and at the same time receives mission instructions from the ground control station and passes these instructions to the flight controller.

[0096] The flight controller is responsible for processing data from various sensors, receiving mission instructions from the ground station through the data transmission module, extracting mission parameters, including target coordinates, mission type, travel speed and safety threshold, and generating corresponding control instructions based on the motion model and control algorithm to ensure that the unmanned vehicle can stably and safely execute mission instructions in complex environments.

[0097] The power supply provides the required electrical energy for each module of the unmanned vehicle and is the energy source for the normal operation of the unmanned vehicle.

[0098] The motor is the power source of the unmanned vehicle, converting the electrical energy transmitted by the power supply into mechanical energy to drive the unmanned vehicle forward, backward and turn.

[0099] The steering gear adjusts the steering angle of the wheels according to the control signal sent by the flight controller to control the steering of the unmanned vehicle.

[0100] The ammeter is used to monitor the current in the circuit of the unmanned vehicle in real time.

[0101] The electronic speed controller adjusts the motor's speed and output power according to the pulse width modulation (PWM) signal sent by the flight controller, and has a motor protection function. When the motor is overloaded or overheated, the ESC will automatically adjust the motor's operating state to protect the motor from damage.

[0102] The GNSS module provides global location information for the unmanned vehicle by receiving satellite signals.

[0103] The IMU module measures the acceleration, angular velocity and attitude information of the unmanned vehicle in real time.

[0104] The UWB module uses ultra-wideband signals for ranging and positioning, providing high-precision positioning information for the unmanned vehicle.

[0105] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-unmanned vehicle collaborative positioning method based on UWB / IMU / GNSS based on graph optimization, characterized by: The specific steps include: The global satellite navigation system GNSS is used to measure the global absolute position of the unmanned vehicle in the geographic coordinate system. The ultra-wideband system UWB measures the distance information between the target unmanned vehicle to be located and the other three unmanned vehicles. The inertial measurement unit IMU measures the three-axis angular velocity and acceleration of the unmanned vehicle. The ultra-wideband system UWB includes one UWB tag and three UWB base stations. The UWB tag is deployed on the target unmanned vehicle to be located, and the three UWB base stations are set up on the other three unmanned vehicles as mobile base station unmanned vehicles. Sensors are used to collect the GNSS absolute positioning information and IMU relative motion information of the four unmanned vehicles, as well as the distance between the target unmanned vehicle to be located and the other three unmanned vehicles; Establish UWB / GNSS / IMU measurement model; The extended Kalman filter algorithm is used to fuse the absolute positioning information of the global satellite navigation system (GNSS) and the relative motion information of the inertial measurement unit (IMU). The angular velocity and acceleration measurements of the IMU are used as prediction quantities, and the position measurements of the global satellite navigation system (GNSS) are used as observation quantities. The prediction quantities and observation quantities are weighted and corrected by calculating the Kalman gain matrix to optimize the position of the mobile base station unmanned vehicle. A factor graph optimization model is constructed for the unmanned vehicle to be located after the tags are deployed. The geometric distance between the UWB base station and the tags is constrained, and the kinematic constraints recursively derived from the inertial measurement unit (IMU) are converted into probability factors in the graph model. The maximum likelihood estimation is jointly solved through a nonlinear least squares optimization algorithm to obtain the optimal estimate of the position and posture of the target unmanned vehicle to be located.

2. The UWB / IMU / GNSS multi-unmanned vehicle collaborative positioning method based on graph optimization according to claim 1 is characterized by: After receiving signals from at least four satellites, the global satellite navigation system GNSS obtains the longitude, latitude and altitude information of the GNSS receiver's location based on pseudo-range measurement positioning technology; the UWB tag periodically transmits a pulse signal, the UWB base station receives and measures the arrival time of the pulse signal, calculates the distance between the tag and the base station, and solves the tag's position information based on the trilateral positioning method; the inertial measurement unit IMU measures the unmanned vehicle's linear acceleration in three-dimensional space through an accelerometer, and measures the unmanned vehicle's angular velocity around three orthogonal axes through a gyroscope.

3. The UWB / IMU / GNSS multi-unmanned vehicle collaborative positioning method based on graph optimization according to claim 1 is characterized by: For the collected GNSS data, a reasonable pseudorange change threshold is set. When the pseudorange difference between two adjacent measurements in the data exceeds the threshold, the data is regarded as an outlier and removed. The GNSS data outlier removal formula is: |r k+1 -r k |>Dr. th Among them, |ρ k+1 -ρ k | is the difference between two adjacent pseudorange measurements, Δρ th is the set pseudorange change threshold.

4. The UWB / IMU / GNSS multi-unmanned vehicle collaborative positioning method based on graph optimization according to claim 1 is characterized by: For the collected UWB data, the mean and standard deviation of the UWB measurement distance are calculated, and a reasonable threshold is set. Data exceeding the threshold is regarded as an outlier and removed. The formula for removing outliers from UWB data is: Among them, d j is a certain measurement distance of the UWB system, is the mean value of UWB measurement distance, α is the set threshold, σ d is the standard deviation of UWB measurement distance.

5. The UWB / IMU / GNSS multi-unmanned vehicle collaborative positioning method based on graph optimization according to claim 1 is characterized by: Before collecting data, each sensor is initialized to eliminate device zero bias errors. A sliding window dynamic threshold detection method is used to eliminate pseudo-range multipath errors caused by GNSS signals blocked by buildings during the collection process, acceleration spikes caused by IMU high-frequency mechanical vibrations, and measurement deviations caused by obstacles in UWB signals in non-line-of-sight propagation scenarios. The theoretical variation range of each physical quantity is derived based on the carrier motion model and sensor characteristics, and parameters such as the acceleration change rate, angular velocity fluctuation threshold, ranging deviation tolerance, and GNSS anti-error value are set to achieve real-time validity verification and outlier elimination of sensor raw data.

6. The UWB / IMU / GNSS multi-unmanned vehicle collaborative positioning method based on graph optimization according to claim 1 is characterized by: When establishing the UWB / GNSS / IMU measurement model: establish a linear relationship model between signal propagation time and spatial distance, determine the UWB ranging model, describe the clock error compensation mechanism between the satellite signal transmission time and the receiver reception time, determine the GNSS measurement model based on the satellite signal pseudorange, convert the IMU acceleration measurement value in the carrier coordinate system to the navigation coordinate system through the direction cosine matrix, and determine the IMU acceleration measurement model and angular velocity measurement model by determining the proportional difference between the actual angular velocity measurement value and the ideal angular velocity measurement value of the gyroscope, while considering the zero-article error caused by the environment and the random noise in the measurement process.

7. The UWB / IMU / GNSS multi-unmanned vehicle collaborative positioning method based on graph optimization according to claim 1, characterized in that: By numerically integrating continuous IMU measurement values, kinematic constraints on posture changes at adjacent moments are established. The distance measurements between the ultra-wideband system UWB tag and each base station are converted into geometric constraints. The motion constraints of the inertial measurement unit (IMU) and the distance constraints of the ultra-wideband system (UWB) are represented as factors in a factor graph. A factor graph model is constructed, and the Levenberg-Marquardt optimization algorithm is used to minimize the weighted sum of the squares of all factor residuals. The global optimal estimate of the posture state of the target unmanned vehicle is solved, and the position coordinates of the target unmanned vehicle are determined.

Images