UWB-IMU fusion trajectory tracking system and method based on GG-ESKF

Abstract

The invention discloses a UWB-IMU fusion trajectory tracking system and method based on GG-ESKF, and relates to the technical field of UWB indoor positioning. The system adopts a multi-stage processing framework: firstly, solving a positioning equation by using a nonlinear least square iterative algorithm based on a Huber loss function to obtain an initial position; secondly, collecting channel pulse response information and distance information through a wireless communication system, and establishing a dynamic environment sensing mechanism for effectively judging the current channel condition and the availability of each base station; and finally, fusing IMU information and UWB ranging data of the available base stations by adopting a GG-ESKF algorithm, and providing a corresponding optimization strategy for the number of the available base stations in combination with a dynamic geometric optimization mechanism of the base stations so as to ensure the positioning stability and reduce the influence of the number and distribution of the base stations on the tracking precision. The invention provides a fusion positioning system which combines the advantages of UWB and IMU, can track the motion trail of the target, and is suitable for most LOS and NLOS indoor mixed scenes.

Description

Technical Field

[0001] The present invention belongs to the field of UWB indoor positioning, and in particular relates to a GG-ESKF-based UWB-IMU fusion trajectory tracking system and method. Background Art

[0002] Ultra-Wide Band (UWB) technology is a technology that uses nanosecond pulse signals for wireless communication, with a positioning range of up to 25 to 250 meters. The principle of this technology is to achieve high-precision ranging and positioning by transmitting extremely short pulse signals within an extremely wide frequency band. The advantages of UWB technology include low power consumption, high anti-interference ability and security, as well as a relatively fast transmission rate. It can achieve centimeter-level positioning accuracy and has shown broad application prospects in many fields such as automobiles, industry, security monitoring, drones and robots. However, in a mixed environment of Line of Sight (LOS) and Non-Line of Sight (NLOS), UWB signals are susceptible to signal blocking and multipath effects, which leads to a decrease in positioning accuracy.

[0003] As a high-precision sensor device, the Inertial Measurement Unit (IMU) provides continuous motion information, making it suitable for real-time positioning in dynamic environments. However, its errors accumulate over time. Therefore, relying solely on UWB or IMU for indoor positioning has its own shortcomings. Fusion of UWB and IMU has become an effective way to improve indoor positioning accuracy and reliability.

[0004] While existing fusion algorithms offer advantages in improving indoor positioning accuracy under mixed LOS / NLOS conditions, they still generally suffer from strong model dependency, high computational complexity, insufficient noise adaptability, or limited real-time performance, making it difficult to achieve a balance between accuracy and efficiency in dynamic environments. Therefore, designing efficient fusion algorithms to optimize positioning accuracy in real time has become a hot research topic. Summary of the Invention

[0005] Based on this, the present invention aims to solve the problem of decreased tracking accuracy due to signal obstruction in a LOS / NLOS mixed environment. It proposes a UWB-IMU fusion trajectory tracking system and method based on the error state Kalman filter (Geometry-Graded ErrorState Kalman Filter, GG-ESKF) with base station geometry hierarchical optimization, which can achieve effective trajectory tracking of the target.

[0006] The present invention first provides a real-time positioning and tracking system based on a LOS / NLOS hybrid environment:

[0007] A real-time positioning and tracking system based on a LOS / NLOS mixed environment includes a UWB base station array, a tag with an internally integrated IMU module, a wireless serial port module, and a data processing terminal;

[0008] The UWB base station array consists of four UWB modules of the same type, distributed in the XOY plane in a rectangular pattern. The tag integrates an IMU chip based on the UWB module. The wireless serial port module is used for data transmission between the tag and the terminal. The data processing terminal is responsible for data processing and positioning tracking solutions.

[0009] Optionally, the UWB base station model is HR-RTLS1 LD600, with DW1000 as the core chip;

[0010] Optionally, the tag model is HR-RTLS1 LD600(-I), which integrates ICM-20948 internally;

[0011] Optionally, the commercial wireless serial port module is nanoUART-wl;

[0012] Optionally, the data processing terminal is configured with an R5-4600U processor, 8GB of RAM, and storage devices required for hardware control and positioning;

[0013] To solve the technical problem, the present invention also provides a real-time positioning and tracking method based on a LOS / NLOS mixed environment, which specifically includes the following steps:

[0014] Step S1, the base station array is composed of 4 UWB modules of the same model, which are distributed in a rectangular shape in the XOY plane. The base stations are arranged counterclockwise along the edge of the rectangle, namely base station 0, base station 1, base station 2 and base station 3, where the navigation coordinate system is established with base station 0 as the origin of the coordinate system. When the tag moves within the measurement range of the base station, the channel impulse response (CIR) and ranging information between the tag and each base station, as well as the tag's own IMU inertial measurement data are collected in real time through the wireless communication system. With the help of the WiFi wireless serial port communication module, the above information is transmitted to the terminal processing platform in real time to provide complete data support for subsequent data processing;

[0015] Step S2: Using the distance information between the base station and the tag, a positioning equation is established based on the geometric relationship, and a nonlinear least squares (LS) iterative algorithm based on the Huber loss function is used to solve it to obtain a rough estimate of the tag's initial position;

[0016] Step S3: To reduce the impact of obstacles and multipath effects on positioning accuracy in mixed scenarios, the signal received power is first extracted based on the CIR to perform channel classification. Then, the base station availability is dynamically evaluated based on the distance change pattern, thereby realizing the identification of the base station status.

[0017] Step S4: After obtaining the status of each base station, the system performs GG-ESKF fusion processing based on the ranging information of the valid base stations and the IMU inertial measurement data. For different available base stations, the system will comprehensively consider the Geometric Dilution of Precision (GDOP) of the base station layout and the motion status information of the IMU, and dynamically select the corresponding geometric optimization strategy to complete target tracking and positioning.

[0018] The beneficial effects of the present invention: The UWB base station module and tag module of the present invention are both commercial devices, with a lightweight system structure and low cost. They are easy to integrate into mobile platforms such as drones, AGVs and wearable devices, and can achieve target tracking with simple deployment, providing reliable technical support for indoor navigation, personnel positioning and asset tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 This is a system structure diagram of the present invention.

[0020] Figure 2 This is a base station array layout diagram of the experimental environment of the present invention.

[0021] Figure 3 It is a positioning model diagram of the present invention.

[0022] FIG4 is a schematic diagram of positioning optimization available to two base stations according to the present invention. DETAILED DESCRIPTION

[0023] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, which are only one of the embodiments of the present invention and are not all embodiments.

[0024] The present invention provides a UWB-IMU fusion trajectory tracking system and method based on GG-ESKF. The system structure is as follows: Figure 1 In this embodiment, the four UWB base stations in the figure are all HR-RTLS1 LD600, the tag model is HR-RTLS1 LD600(-I), and a nanoUART-wl wireless serial port module is used. The data processing terminal is configured with an r5-4600u processor, 8G running memory, and storage devices required for hardware control and positioning.

[0025] When using the present invention, four UWB base stations of the same model are fixedly installed on the edges of the walls around the experimental environment. The specific arrangement is as follows: Figure 2 The positioning tag is fixed on the target to be measured, and the collected positioning data is transmitted to the terminal device in real time through the wireless serial port module. Finally, the relevant data is processed using an offline algorithm to realize the target trajectory tracking function.

[0026] The detailed steps for implementing the positioning algorithm are as follows:

[0027] Step S1: In order to track the target, it is necessary to make a rough estimate of the initial position of the tag. Figure 3 As shown, four base stations s are used. i =[x i ,y i ] T (i=0,1,2,3) for label p=[x,y] T Use the distance information pr between the base station and the tag to locate. i , establish the positioning equation based on the geometric relationship:

[0028] (x i -x) 2 +(y i -y) 2 =pr i 2 (1)

[0029] Due to ranging errors, the ranging circles of multiple base stations usually do not intersect precisely at a single point, but instead form a set of points containing the true location. To this end, the present invention first uses the LS iteration method to solve the positioning equation to obtain a rough estimate of the tag's position p0, and then introduces the Huber loss function to perform nonlinear optimization on the estimated position p0.

[0030] Base station location The estimated position p0 is used as the initial estimated value of p, then the initial position Solve the following optimization problem:

[0031]

[0032] Where p represents the potential position of the target to be located, which is the variable in the optimization problem, and ρ(·) is the Huber loss function:

[0033]

[0034] Among them, χ represents the independent variable in the Huber function, and the parameter λ is the residual turning point;

[0035] Step S2: Using the variation of signal receiving power and distance, the channel status and availability of the base station are dynamically evaluated to identify the base station status. In the LOS path, when the CIR signal of the first path arrives, the energy distribution of the signal is relatively concentrated. In contrast, in the NLOS path, the overall strength of the received signal is weakened due to obstacles and multipath interference, and the energy distribution of the signal is relatively dispersed. Therefore, the system first uses the signal receiving power difference P to evaluate the channel status and availability of the base station. D Perform preliminary classification of channels.

[0036] The received power difference of the signal is:

[0037]

[0038] Where C is the CIR power, and F1, F2, and F3 are the first path amplitude values reported by the UWB transceiver. Meanwhile, in the LOS path, P D The value is small and the fluctuation range is not obvious. On the contrary, in the NLOS path, P D The value is large and the amplitude span is large. Therefore, using this conclusion, by calculating the P D The amplitude change P d , auxiliary P D Determine whether the base station is in NLOS state:

[0039]

[0040] in, is P in the window (k-N+1,k) D , for The mean of , N is the window size. Then, P D 、P d and its threshold λ P , γ P Defined as Criterion 1 (by C k,1 (denoted by), to determine whether the UWB transceiver is blocked by NLOS at time k, C k,1 It is expressed as follows:

[0041]

[0042] Among them, C k,1 =1 means LOS, C k,1 =0 indicates NLOS.

[0043] In addition, in the LOS path, the distance between the tag and the base station changes smoothly, but when the signal enters NLOS from LOS, the distance value d usually fluctuates greatly. Therefore, the rate of change of the distance between the tag and the base station at time k is used to determine whether the base station is available:

[0044]

[0045] Among them, d(k) and d(k-1) are the distance values at the current moment and the previous moment respectively; t(k) and t(k-1) are the times corresponding to the current moment and the previous moment respectively, K i is K in the window (k-N+1,k) k , K i The mean of K and threshold γ K Defined as Criterion 2 (by C k,2 ), which is expressed as follows:

[0046]

[0047] Among them, C k,2 =0 means unavailable, C k,2 =1 means available;

[0048] Step S3: Based on the ranging information from valid base stations and the IMU data, the GG-ESKF algorithm is used for fusion processing. Based on the number and layout of valid base stations, a corresponding geometric optimization strategy is implemented to complete the target trajectory tracking. When there are enough available base stations, ESKF is directly used for fusion. The measured pseudorange vector is constructed using the currently available base stations and the tag position to complete the filter update.

[0049] First, the current global state vector is predicted through inertial navigation solution and used as the reference state:

[0050]

[0051] Among them, the position of the label at time k is p k =[x k ,y k ] T , the speed is v k =[v x,k ,v y,k ] T , the acceleration is a k =[a x,k ,a y,k ] T , the time interval is ΔT, and the quaternion is expressed as q k =[q 0,k ,q 1,k ,q 2,k ,q 3,k ] T . Quaternion equation The prediction process is expressed as follows:

[0052] Calculate the rotation vector using the angular velocity and convert the rotation vector into a quaternion.

[0053]

[0054] Among them, ω k-1 is the angular velocity obtained by the gyroscope, |ω k-1 ·ΔT| represents the modulus of the rotation vector, if |ω k-1 ΔT| is very small and can be approximated using Taylor series expansion and Avoid numerical instabilities.

[0055] Update the quaternion and perform normalization:

[0056]

[0057] in, Represents a quaternion multiplication operation.

[0058] Then, the propagation equation of the system state can be obtained according to the following formula:

[0059]

[0060] in, It is the predicted value of the system state at time k based on the information at time k-1. is the estimated value of the system state at time k-1, n k-1 Is to obey n k-1 The process noise vector of ~N(0,Q), P k|k-1 is the system state prediction covariance matrix at time k based on time k-1, P k-1|k-1 is the system state estimation covariance matrix at time k-1, F and G are the state transfer matrix and noise gain matrix respectively:

[0061]

[0062] Where, ψ represents the yaw angle, R z (ψ) represents the two-dimensional rotation matrix around the z axis, a is the acceleration, b is the a is the acceleration bias, is the process noise covariance matrix, is the accelerometer noise covariance, is the gyroscope noise covariance, is the zero-bias random walk noise covariance matrix, is the accelerometer bias random walk noise covariance, is the gyroscope bias random walk noise covariance.

[0063] Similar to the global state vector, the error state vector is established:

[0064]

[0065] Among them, δp k ,δv k , δα k 、 and They are position error, velocity error, angle error, accelerometer bias random walk noise, and gyroscope bias random walk noise.

[0066] Select the available base station location r at the current moment i,k =(x i,k ,y i,k ) and label position p k =(x k ,y k ), calculate the measured pseudo-range between the base station and the tag:

[0067] pr i,k =||p k -r i,k ||+ε i,k =d i,k +ε i,k (18)

[0068] Among them, d i,k is the actual distance, ε i,k The mean is 0 and the covariance is The measurement noise, i1,i2,…,i m is the index of the available base station, Indicates index i m The measurement noise covariance of .

[0069] Then, the residual between the measured pseudorange vector Z and the predicted pseudorange vector Y is expressed as:

[0070]

[0071] in, Indicates that the index at time k is i m The measured pseudorange of Indicates that the index at time k is i m The predicted pseudorange.

[0072] The measurement matrix H is the Jacobian matrix of each pseudorange pair state vector. Specifically, for the i-th base station, the measurement matrix H i,k is the measured pseudorange pr i,k About label position p k The partial derivative of , that is:

[0073]

[0074] Calculate the Kalman gain K:

[0075] K=P k|k-1 H T (HP k|k-1 H T +R) -1 (twenty two)

[0076] Where R is the observation covariance matrix. The Kalman gain K and residual are used to update the error state, which is expressed as follows:

[0077]

[0078] in, represents the error state estimate at time k, Represents the error state estimate at time k-1.

[0079] After state propagation and measurement update, the updated error is integrated into the global state vector and the position, velocity, and angle errors are set to zero before the next iteration:

[0080]

[0081] Update the covariance matrix P k|k :

[0082] P k|k =(I-KH)P k|k-1 (25)

[0083] Where I represents the identity matrix, which has the same dimension as the system state covariance matrix.

[0084] When the number of available base stations is equal to 2 and the GDOP is large, the two-circle intersection method and the nearest point approximation method as shown in Figure 4 are used for positioning optimization. Assume that the coordinates of the two base stations are r1 = (x1, y1) and r2 = (x2, y2), and the corresponding pseudo-range measurements with the tag (x, y) are pr1 and pr2; based on the tag's last positioning result p k-1 =(x k-1 ,y k-1 ) and the current predicted position The following judgment and calculation process can be established:

[0085]

[0086] Among them, p1 and p2 are candidate points. If |pr1-pr2|<||r1-r2||<pr1+pr2, it means that the two circles intersect, then p1 and p2 are two solutions of formula (26); if ||r1-r2||=pr1+pr2or||r1-r2||=|pr1-pr2|, it means that the two circles are tangent, then p is the only solution of formula (26); if ||r1-r2||>pr1+pr2or||r1-r2||<|pr1-pr2|, it means that the two circles are separated, and formula (26) has no solution. At this time, the system uses the nearest point approximation method for position estimation: find the point closest to the center of the other circle on each circle, and select the point with distance p from the two nearest points. k-1 The nearest point is used as the final label position estimate; the point p1 on the first circle closest to the center of the second circle and the point p2 on the second circle closest to the center of the first circle are selected as candidate points, which are expressed as follows:

[0087]

[0088] When the number of available base stations is less than 2, the inertial information provided by the IMU is used for short-term position estimation to ensure trajectory continuity and system stability:

[0089] v k =v k-1 +a k-1 ·ΔT (29)

[0090]

[0091] Among them, v k-1 、a k-1 and p k-1 are the velocity, acceleration and position at time k-1, and ΔT is the time step.

[0092] At this point, the target trajectory tracking is achieved.

[0093] The above-described embodiments are merely preferred embodiments of the present invention. It should be noted that the present invention is not limited to the specific forms disclosed, nor does it exclude other possible implementations. The present invention can be applied to various combinations, variations, and environments, and can be adjusted and modified according to the above description or the technology and knowledge in the relevant fields. However, as long as they do not depart from the spirit and scope of the present invention, any changes and improvements made by those skilled in the art should fall within the scope of protection of the claims appended to the present invention.

Claims

1. A UWB-IMU fusion trajectory tracking system and method based on GG-ESKF, comprising four base stations, a tag, a wireless serial port module, and a data processing terminal; the base stations and tags are used to collect channel state information (CIR) and ranging data required for identification, as well as inertial data required for tracking. The base stations are distributed in an XOY plane in a rectangular shape; the wireless serial port module is used for data transmission between the device and the terminal; the data processing terminal is responsible for data processing and positioning solution; wherein, Contains a method for constructing a base station array: Step S1, a base station array consisting of four base stations distributed in the XOY plane, wherein the base stations are arranged counterclockwise along the edge of a rectangle, namely base station 0, base station 1, base station 2 and base station 3, wherein a navigation coordinate system is established with base station 0 as the origin of the coordinate system; The tag moves within the measurement range of the base station; Step S2: The channel state information CIR, ranging information and the tag's own inertial measurement data between the tag and each base station are collected in real time through the wireless communication system; the above information is transmitted to the terminal processing platform in real time by means of the WiFi wireless serial communication module.

2. A UWB-IMU fusion trajectory tracking system based on GG-ESKF according to claim 1, characterized in that A real-time positioning and tracking method based on a mixed LOS and NLOS environment includes the following steps: Step S1, rough estimation of initial position; using 4 base stations s i =[x i ,y i ] T (i=0,1,2,3) for label p=[x,y] T Positioning; using the distance information pr between the base station and the tag i , establish the positioning equation based on the geometric relationship: (x i -x) 2 +(y i -y) 2 =pr i 2 (1) The least squares iteration method is used to solve the positioning equation to obtain the rough estimated position p0 of the tag. The Huber loss function ρ(·) is introduced and the estimated position p0 is used as the initial estimate of p for nonlinear optimization: Among them, p represents the potential position of the target to be located, and the optimized initial position is ||·|| represents the Euclidean distance, χ represents the independent variable in the Huber function, and the parameter λ is the residual turning point. At this point, a rough estimate of the initial position is completed. Step S2, base station status identification; using the change law of signal receiving power and distance, the channel status and availability of the base station are dynamically evaluated to realize the identification of the base station status; the system first performs a preliminary classification of the channel based on the signal receiving power; in the LOS path, when the channel status information CIR arrives at the first path signal, the signal energy distribution is relatively concentrated, so the signal receiving power difference P D The value is small and the fluctuation amplitude is not obvious; on the contrary, in the NLOS path, the overall strength of the received signal is weakened due to obstacle blocking and multipath interference, so P D The value is large and the amplitude span is large; thus, the P D The amplitude change P d , auxiliary P D Determine whether the base station is in NLOS state: Where C is the power of the channel state information CIR, F1, F2 and F3 are the first path amplitude values reported by the UWB transceiver, is P in the window (k-N+1,k) D , N is the window size, k is the current moment, for The mean of P D 、P d and its threshold λ P , γ P Defined as criterion 1 to determine whether the UWB transceiver is NLOS blocked at time k: Among them, C k,1 =0 means LOS, C k,1 =1 means NLOS; The distance change rate between the tag and the base station at time k is used to determine whether the base station is available: Among them, d(k) and d(k-1) are the distance values at the current moment and the previous moment respectively, t(k) and t(k-1) are the times corresponding to the current moment and the previous moment respectively, K i is K in the window (k-N+1,k) k , N is the window size, K i The mean of Set K and threshold γ K Defined as Criterion 2: Among them, C k,2 =0 means unavailable, C k,2 =1 means available; At this point, the identification of the base station status is completed; Step S3, geometric hierarchical optimization of the target trajectory: Based on the ranging information of the effective base stations and the inertial measurement data, the GG-ESKF algorithm is used for fusion processing, and according to the number and layout of the effective base stations, the corresponding geometric optimization strategy is executed to complete the trajectory tracking of the target; When the number of available base stations is sufficient, the error state Kalman filter is directly used for fusion, and the measurement pseudo-range vector is constructed using the available base stations and tag positions at the current moment to complete the filter update; First, the current global state vector is predicted by inertial navigation solution and used as the reference state Among them, the position of the label at time k is p k =[x k ,y k ] T , the speed is v k =[v x,k ,v y,k ] T , the acceleration is a k =[a x,k ,a y,k ] T , the time interval is ΔT, and the quaternion is expressed as q k =[q 0,k ,q 1,k ,q 2,k ,q 3,k ] T ; Quaternion equation The prediction process is expressed as follows: Calculate the rotation vector using the angular velocity and convert the rotation vector into a quaternion. Among them, ω k-1 is the angular velocity obtained by the gyroscope, |ω k-1 ·ΔT| represents the modulus of the rotation vector, if |ω k-1 ΔT| is very small and can be approximated using Taylor series expansion and Avoid numerical instability; Update the quaternion and perform normalization. in, Represents quaternion multiplication operations; Then, the propagation equation of the system state can be obtained according to the following formula: in, It is the predicted value of the system state at time k based on the information at time k-1. is the estimated value of the system state at time k-1, n k-1 Is to obey n k-1 The process noise vector of ~N(0,Q), P k|k-1 is the system state prediction covariance matrix at time k based on time k-1, P k-1|k-1 is the system state estimation covariance matrix at time k-1, F and G are the state transfer matrix and noise gain matrix respectively: Where ψ represents the yaw angle, R z (ψ) represents the two-dimensional rotation matrix around the z axis, a is the acceleration, b is the a is the acceleration bias, represents the antisymmetric matrix operation, is the process noise covariance matrix, is the accelerometer noise covariance, is the gyroscope noise covariance, is the zero-bias random walk noise covariance matrix, is the accelerometer bias random walk noise covariance, is the gyroscope bias random walk noise covariance; Similar to the global state vector, the error state vector is established: Among them, δp k ,δv k , δα k 、 and They are position error, velocity error, angle error, accelerometer bias random walk noise, and gyroscope bias random walk noise; Select the available base station location r at the current moment i,k =(x i,k ,y i,k ) and label position p k =(x k ,y k ), calculate the measured pseudo-range between the base station and the tag: pr i,k =||p k -r i,k ||+ε i,k =d i,k +ε i,k (18) Among them, d i,k is the actual distance, ε i,k The mean is 0 and the covariance is The measurement noise, i1,i2,…,i m is the index of the available base station, Indicates index i m The measurement noise covariance of Then, the residual between the measured pseudorange vector Z and the predicted pseudorange vector Y is expressed as: in, Indicates that the index at time k is i m The measured pseudorange of Indicates that the index at time k is i m The predicted pseudorange of The measurement matrix H is the Jacobian matrix of each pseudorange pair state vector. Specifically, for the i-th base station at time k, the measurement matrix H i,k is the measured pseudorange pr i,k About label position p k The partial derivative of , that is: Calculate the Kalman gain K: K=P k|k-1 H T (HP k|k-1 H T +R) -1 (22) Where R is the measurement noise covariance matrix; the Kalman gain K and the residual are used to update the error state: in, represents the error state estimate at time k, represents the error state estimate at time k-1; After state propagation and measurement update, the updated error will be integrated into the global state vector , and set the position, velocity, and angle errors to zero before the next iteration: Update the covariance matrix P k|k : P k|k =(I-KH)P k|k-1 (25) Where I represents the identity matrix, whose dimension is the same as the system state covariance matrix; When the number of available base stations is equal to 2 and the GDOP is large, the two-circle intersection method and the nearest point approximation method are used for positioning optimization; assuming that the coordinates of the two base stations are r1 = (x1, y1) and r2 = (x2, y2), the corresponding pseudo-range measurements with the tag to be measured (x, y) are pr1 and pr2; based on the tag's last positioning result p k-1 =(x k-1 ,y k-1 ) and the current predicted position The following judgment and calculation process can be established: Where p represents the potential position of the target to be located, and p1 and p2 are candidate points. If |pr1-pr2|<||r1-r2||<pr1+pr2, it means that the two circles intersect, and p1 and p2 are two solutions of formula (26). If ||r1-r2||=pr1+pr2or||r1-r2||=|pr1-pr2|, it means that the two circles are tangent, and p is the only solution of formula (26). If ||r1-r2||>pr1+pr2or||r1-r2||<|pr1-pr2|, it means that the two circles are separated, and formula (26) has no solution. At this time, the system uses the nearest point approximation method to estimate the position: find the point closest to the center of the other circle on each circle, and select the point with distance p from the two nearest points. k-1 The nearest point is used as the final label position estimate; the point p1 on the first circle closest to the center of the second circle and the point p2 on the second circle closest to the center of the first circle are selected as candidate points, which are expressed as follows: When the number of available base stations is less than 2, the inertial information provided by the IMU is used for short-term position estimation to ensure trajectory continuity and system stability: v k =v k-1 +a k-1 ·ΔT (29) Among them, v k-1 、a k-1 and p k-1 are the velocity, acceleration and position at time k-1 respectively, and ΔT is the time step; At this point, the target trajectory tracking is finally achieved.

Images