A Multi-Algorithm Collaborative 3D Localization Method Based on UWB and IMU

Abstract

This invention discloses a multi-algorithm collaborative 3D positioning method based on UWB and IMU. The method includes: constructing a 3D Cartesian coordinate system based on multiple UWB base stations; measuring the distance between the tag and the UWB base stations through bilateral bidirectional ranging to obtain the corresponding distance variables; establishing a nonlinear equation system using a trilateration algorithm, substituting the distance variables to transform it into a linear error equation system, and obtaining a set of 3D coordinates of the tag using Newton's iteration method; obtaining a nonlinear equation system using the time difference of arrival, obtaining a linear error equation system based on Taylor expansion approximation, and obtaining another set of 3D coordinates of the tag using the least squares method; weighted averaging the two sets of 3D coordinates to obtain the initial 3D coordinates; constructing a state transition model based on the initial 3D coordinates, velocity components, and acceleration components; predicting the state at the next moment using Kalman filtering and updating the tag's 3D coordinates; and outputting the final 3D spatial positioning result. This invention can achieve high-precision 3D positioning.

Description

Technical Field

[0001] This invention relates to the field of target localization technology, and specifically to a multi-algorithm collaborative three-dimensional localization method based on UWB and IMU. Background Technology

[0002] Currently, commonly used indoor positioning technologies include WiFi positioning, Bluetooth positioning, ultra-wideband (UWB) positioning, infrared positioning, and inertial measurement unit (IMU) positioning. Each of these technologies has its own advantages and disadvantages. WiFi positioning has the advantages of wide deployment, direct utilization of existing WiFi infrastructure, and low cost, but its positioning range is small and greatly affected by environmental interference. Bluetooth positioning has the advantages of high accuracy and low power consumption, but its positioning range is small. Infrared positioning has the advantages of high accuracy and immunity to electromagnetic interference, but it is limited by line-of-sight. UWB has the advantages of high transmission rate, extremely wide bandwidth, low power consumption, low radiation, strong anti-interference ability, and high positioning accuracy, but its deployment cost is high and it is affected by non-line-of-sight limitations. IMU has the advantages of high positioning accuracy, independence from external signal sources, and strong real-time performance, but it has drift errors.

[0003] The current mainstream approach combines UWB and IMU positioning systems to compensate for each other's advantages and disadvantages. UWB compensates for IMU drift errors, while the IMU improves UWB measurement accuracy and reduces non-line-of-sight errors. However, this method typically requires deploying a large number of UWB base stations, leading to high costs. To reduce deployment costs while maintaining high-precision measurements, multiple algorithms are needed to process the acquired data, reducing measurement errors through different combinations. Furthermore, this mainstream method is generally only applicable to two-dimensional planes, limiting its applicability to various scenarios. Summary of the Invention

[0004] To overcome the shortcomings and deficiencies of existing technologies, this invention provides a multi-algorithm collaborative 3D positioning method based on UWB and IMU. This invention establishes a nonlinear equation system through a trilateration algorithm, transforms it into a linear error equation system by substituting distance variables, and obtains a set of 3D coordinates of the tag through Newton's iteration method. A nonlinear equation system is obtained through the time difference of arrival, and a linear error equation system is obtained based on Taylor expansion approximation. Another set of 3D coordinates of the tag is obtained through the least squares method. Solving the nonlinear equation systems obtained from the two different ranging methods using different mathematical iteration methods can significantly reduce measurement errors and achieve high accuracy. Furthermore, by obtaining instantaneous acceleration and velocity vectors from the IMU to construct equations to predict the state at the next moment, the UWB positioning data can be corrected, thereby minimizing the non-line-of-sight error of UWB.

[0005] The second objective of this invention is to provide a multi-algorithm collaborative 3D localization system based on UWB and IMU;

[0006] A third objective of this invention is to provide a computer-readable storage medium;

[0007] A fourth objective of this invention is to provide a computer device.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] This invention provides a multi-algorithm collaborative 3D localization method based on UWB and IMU, comprising the following steps:

[0010] A three-dimensional Cartesian coordinate system is constructed based on multiple UWB base stations. The UWB base stations and tags simultaneously receive and transmit signals. The distance between the tag and the UWB base station is measured by bilateral bidirectional ranging, and the corresponding distance variables are obtained.

[0011] A nonlinear system of equations is established using a trilateration algorithm. By substituting the distance variable, the system is transformed into a linear error system of equations. A set of three-dimensional coordinates of the tag is obtained through Newton's iteration method.

[0012] A set of nonlinear equations is obtained by the time difference of arrival, a set of linear error equations is obtained by Taylor expansion approximation, and another set of three-dimensional coordinates of the tag is obtained by the least squares method.

[0013] The initial three-dimensional coordinates of the label are obtained by weighted averaging of the two sets of three-dimensional coordinates of the label;

[0014] The axial velocity and acceleration components are obtained through the inertial navigation system. A state transition model is constructed based on the initial three-dimensional coordinates, velocity components, and acceleration components. The state at the next moment is predicted based on Kalman filtering, and the three-dimensional coordinates of the tag are updated. The final three-dimensional spatial positioning result is then output.

[0015] As a preferred technical solution, a nonlinear equation system is established using a trilateration algorithm, and then transformed into a linear error equation system by substituting distance variables. Specifically, this includes:

[0016] Four UWB base stations capable of simultaneously transmitting and receiving signals are selected, and their coordinates are represented as: S(0,0,0), A(L... x ,0,0)B(0,L y ,0)C(0,0,L) z ), L x L y L z These represent the lengths of each coordinate axis in a rectangular coordinate system;

[0017] Let the label coordinates be D(x) d ,y d ,z d );

[0018] The established system of nonlinear equations is expressed as follows:

[0019]

[0020] Among them, Dis A Dis represents the distance variable from UWB base station A to tag D. B Dis represents the distance variable from UWB base station B to tag D. C This represents the distance variable from UWB base station C to tag D.

[0021] As a preferred technical solution, a set of three-dimensional coordinates of the label is obtained through Newton's iteration method, specifically including:

[0022] Establish function vectors:

[0023]

[0024] Create a variable vector:

[0025]

[0026] Calculate function vector For variable vectors From the partial derivative matrix, we obtain the Jacobian matrix:

[0027]

[0028] Where J represents the Jacobian matrix;

[0029] The iterative process of Newton's method is as follows:

[0030] Constructing variable vectors Where, x dk y dk z dk These represent the x, y, and y coordinates of the label during the k-th iteration, respectively.

[0031] Set initial variable vector

[0032] Calculate the function vector:

[0033]

[0034] Calculate the Jacobian matrix:

[0035]

[0036] Solve the system of linear error equations Solving

[0037] Update the variable vector:

[0038]

[0039] Set the convergence threshold to cvgval, when the determination Stop the iteration and obtain the coordinates NR(x) nr ,y nr ,z nr ), which is a set of three-dimensional coordinates of the label obtained by Newton's iteration method.

[0040] As a preferred technical solution, a nonlinear equation set is obtained through the arrival time difference, and a linear error equation set is obtained based on the Taylor expansion approximation, specifically including:

[0041] Four UWB base stations capable of simultaneously transmitting and receiving signals are selected, and their coordinates are represented as: S(0,0,0), A(L... x ,0,0)B(0,L y ,0)C(0,0,L) z ), L x L y L z These represent the lengths of each coordinate axis in a rectangular coordinate system;

[0042] Let the label coordinates be D(x) d ,y d ,z d );

[0043] Calculate the time difference:

[0044] ΔT AB =T AD -T BD

[0045] ΔT AC =T AD -T CD

[0046] ΔT BC =T BD -T CD

[0047] Establish a system of distance equations:

[0048]

[0049] Taylor expansion to initial coordinates (x) d0 ,y d0 ,z d0 Linearize the equation:

[0050]

[0051] After linearization, the original system of equations can be expressed in matrix form:

[0052]

[0053] Among them, T AD T represents the timestamp of tag D receiving the signal from UWB base station A. BD T represents the timestamp of tag D receiving the signal from UWB base station B. CD This represents the timestamp when tag D receives the signal from UWB base station C, where c represents the speed of light.

[0054] As a preferred technical solution, another set of three-dimensional coordinates of the label is obtained through the least squares method, specifically including:

[0055] Set initial coordinates (x) d0 ,y d0 ,z d0 )=(L x / 2,L y / 2,L z / 2);

[0056] Substituting into the matrix, we obtain b.

[0057] The error vector is obtained using the least squares method:

[0058]

[0059] Update initial coordinates:

[0060]

[0061] Set the convergence threshold to cvgval, when the determination Stop the iteration and obtain the coordinates LS(x) ls ,y ls ,z ls ), which serves as another set of three-dimensional coordinates for the label obtained through the least squares method.

[0062] As a preferred technical solution, the initial three-dimensional coordinates of the label are obtained by weighted averaging of the two sets of three-dimensional coordinates of the label, specifically including:

[0063] The error of one set of three-dimensional coordinates of the tag obtained by Newton's iteration method is σ1, and the error of another set of three-dimensional coordinates of the tag obtained by the least squares method is σ2.

[0064] Calculate the weights:

[0065]

[0066] Weight normalization:

[0067]

[0068] Calculate the weighted average:

[0069] x av =w′1·x nr +w′2·x ls

[0070] y av =w′1·y nr +w′2·y ls

[0071] z av =w′1·z nr +w′2·z ls

[0072] Among them, (x nr ,y nr ,z nr (x) represents a set of three-dimensional coordinates of the label obtained through Newton's iteration method. ls ,y ls ,z ls ) represents another set of three-dimensional coordinates of the label obtained by the least squares method.

[0073] As a preferred technical solution, a state transition model is constructed based on the initial three-dimensional coordinates, velocity components, and acceleration components. The state at the next moment is predicted using Kalman filtering, and the three-dimensional coordinates of the label are updated. The final three-dimensional spatial positioning result is then output, specifically including:

[0074] The standard deviation of position observation noise is obtained from the static state. Acceleration observation noise standard deviation

[0075] The standard deviation of position process noise is obtained from the motion state. Speed ​​process noise standard deviation Acceleration process noise standard deviation

[0076] Establish the initial state vector:

[0077]

[0078] Wherein, the axial velocity component v x0 v y0 v z0 and acceleration component a x0 a y0 a z0 Obtained by the inertial measurement unit;

[0079] Establish the initial covariance matrix:

[0080]

[0081] Among them, P ij Given the initial uncertainty of the state, i = 1, 2, 3, j = 1, 2, 3; establish the state transition matrix:

[0082]

[0083] Where Δt is the time interval;

[0084] Establish the process noise covariance matrix:

[0085]

[0086] Establish the observation matrix:

[0087]

[0088] Establish the observation noise covariance matrix:

[0089]

[0090] Calculate the predicted state:

[0091]

[0092] Calculate the prediction covariance matrix:

[0093] P pred (t+Δt)=FP(t)F T +Q

[0094] Calculate the Kalman filter gain:

[0095] K(t+Δt)=P pred (t+Δt)H T HP pred (t+Δt)H T +R) -1

[0096] Establish observation vectors:

[0097]

[0098] Update the covariance matrix:

[0099] P(t+Δt)=(IK(t+Δt)H)P pred (t+Δt)

[0100] Where I represents the identity matrix;

[0101] Update status:

[0102]

[0103] in, This is the final three-dimensional spatial positioning result.

[0104] This invention also provides a multi-algorithm collaborative 3D positioning system based on UWB and IMU, comprising: a 3D spatial rectangular coordinate system construction module, a distance variable construction module, a first linear error equation system construction module, a first set of 3D coordinate calculation modules, a second set of linear error equation system construction modules, a second set of 3D coordinate calculation modules, an initial 3D coordinate construction module, a state transition model construction module, and a final 3D spatial positioning result output module;

[0105] The three-dimensional spatial rectangular coordinate system construction module is used to construct a three-dimensional spatial rectangular coordinate system based on multiple UWB base stations, wherein the UWB base stations and tags simultaneously receive and transmit signals;

[0106] The distance variable construction module is used to measure the distance between the tag and the UWB base station through bilateral bidirectional ranging, and obtain the corresponding distance variable;

[0107] The first linear error equation system construction module is used to establish a nonlinear equation system through a trilateration algorithm, and then transform it into a linear error equation system by substituting distance variables.

[0108] The first set of three-dimensional coordinate calculation modules is used to obtain a set of three-dimensional coordinates of the label using Newton's iteration method;

[0109] The second linear error equation system construction module is used to obtain a nonlinear equation system through the arrival time difference, and to obtain a linear error equation system based on the Taylor expansion approximation.

[0110] The second set of three-dimensional coordinate calculation modules is used to obtain another set of three-dimensional coordinates of the label using the least squares method;

[0111] The initial three-dimensional coordinate construction module is used to obtain the initial three-dimensional coordinates of the label by weighted averaging of the two sets of three-dimensional coordinates of the label;

[0112] The state transition model construction module is used to obtain the axial velocity component and acceleration component through the inertial navigation system, and to construct a state transition model based on the initial three-dimensional coordinates, velocity component, and acceleration component.

[0113] The final three-dimensional spatial positioning result output module is used to predict the state at the next moment based on Kalman filtering and update the three-dimensional coordinates of the label, and output the final three-dimensional spatial positioning result.

[0114] The present invention also provides a computer-readable storage medium storing a program that, when executed by a processor, implements the multi-algorithm collaborative 3D localization method based on UWB and IMU as described above.

[0115] The present invention also provides a computer device, including a processor and a memory for storing processor-executable programs, wherein when the processor executes the program stored in the memory, it implements the multi-algorithm collaborative three-dimensional localization method based on UWB and IMU as described above.

[0116] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0117] (1) Compared with the traditional method of improving positioning accuracy by deploying a large number of positioning base stations, the present invention can achieve high-precision three-dimensional spatial positioning with only four UWB positioning base stations, one UWB tag and one IMU module, and the structure is simpler.

[0118] (2) On the one hand, the present invention measures the distance between the tag and the base station by bilateral bidirectional ranging method, establishes a nonlinear equation system by trilateral positioning method, and further obtains the minimum error by Newton iteration method; on the other hand, it establishes a nonlinear equation system by time difference ranging method, and further obtains the minimum error by least squares method; by solving the nonlinear equation system obtained by the two different ranging methods by different mathematical iteration methods, the measurement error can be significantly reduced and the accuracy is very high.

[0119] (3) This invention obtains instantaneous acceleration and velocity vectors through IMU to construct equations to predict the state at the next moment, which can correct the UWB positioning data and thus minimize the non-line-of-sight error of UWB.

[0120] (4) The present invention reduces static noise error and dynamic noise error by using Kalman filtering, thereby enabling the present invention to achieve high-precision three-dimensional positioning in any complex environment. Attached Figure Description

[0121] Figure 1 This is a flowchart illustrating the multi-algorithm collaborative 3D localization method based on UWB and IMU of the present invention.

[0122] Figure 2 This is a schematic diagram of the spatial coordinate system constructed in this invention;

[0123] Figure 3 This is a schematic diagram illustrating the process of measuring the distance between a tag and a base station using bilateral two-way ranging (DS-TWR) according to the present invention.

[0124] Figure 4 This is a schematic diagram of the process of the present invention, which uses the trilateration method and Newton's iteration method for iteration;

[0125] Figure 5 This is a schematic diagram illustrating the TDOA positioning method and its iteration using the least squares method in this invention.

[0126] Figure 6This is a schematic diagram of the Kalman filter process of the present invention. Detailed Implementation

[0127] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0128] Example 1

[0129] like Figure 1 As shown, this embodiment provides a multi-algorithm collaborative 3D localization method based on UWB and IMU, including the following steps:

[0130] S1: As Figure 2 As shown, a three-dimensional Cartesian coordinate system is constructed using a UWB base station. The base station and the tag simultaneously receive and transmit signals, and the distance between the tag and the base station is measured by bilateral two-way ranging (DS-TWR).

[0131] In this embodiment, a finite spatial Cartesian coordinate system is constructed using four UWB base stations capable of simultaneously transmitting and receiving signals, with each axis having a length of L. x L y L z That is, the coordinates of the four base stations are S(0,0,0), A(L) and A(L) respectively. x ,0,0)B(0,L y ,0)C(0,0,L) z Let the label coordinates be D(x); d ,y d ,z d ),like Figure 3 As shown, the timestamp T of the signal sent by A is recorded. A1 The timestamp T of the received signal from D D1 and the timestamp T of the response signal D2 The timestamp T of the signal received by A A2 The timestamp T when A sends the signal again A3 The timestamp T when D receives the signal again D3 The time difference between the transmitted signal and the received signal is T. round1 The time difference between A receiving the signal and retransmitting the signal is T. round2 The time difference between D receiving the signal and transmitting the signal is T. reply1 The time difference between D sending a signal and receiving the signal again is T. reply2 ;

[0132] T round1 =T A2 -T A1

[0133] T round2 =T A3 -T A2

[0134] T reply1 =T D2 -T D1

[0135] T reply2 =T D3 -T D2

[0136] The signal flight time from A to D is:

[0137] T prop =(T round1 -T reply1 +T round2 -T reply2 ) / 4

[0138] The distance from A to D is:

[0139] Dis A =c·Tpro p

[0140] Where c is the speed of light;

[0141] Similarly, the distance Dis from B to D is... B The distance from C to D (Dis) C .

[0142] S2: As Figure 4 As shown, a system of nonlinear equations is established using a trilateration algorithm, and the distance variable Dis is substituted into the equations. A Distance variable Dis B Distance variable Dis C The equations are transformed into a system of linear error equations, and then a set of three-dimensional coordinates for the label is obtained through Newton's iteration method, specifically including:

[0143] Establish a nonlinear system of equations (i.e., a system of distance equations), expressed as:

[0144]

[0145] Establish function vectors:

[0146]

[0147] Create a variable vector:

[0148]

[0149] Construct the Jacobian matrix (function vector) For variable vectors (partial derivative matrix):

[0150]

[0151] The iterative process of Newton's method:

[0152] Variable vector Where, x dk y dk z dk These represent the x, y, and y coordinates of the label during the k-th iteration, respectively.

[0153] Where k is the iteration number, k = 0, 1, 2, ...;

[0154] Set initial variable vector

[0155] Calculate the function vector:

[0156]

[0157] Calculate the Jacobian matrix:

[0158]

[0159] Solve the system of linear error equations Solving

[0160] Update the variable vector:

[0161]

[0162] The variable vector is continuously updated to make the preset value approach the solution of the system of equations. When the variable is less than the convergence threshold, it means that the solution of the system of equations has been approached.

[0163] Determine if convergence has occurred:

[0164] Set the convergence threshold to cvgval, when Stop the iteration and obtain the coordinates NR(x) nr ,y nr ,z nr ), where (x nr ,y nr ,z nr ) represents the three-dimensional coordinates obtained by the TOA algorithm and Newton's iteration method. This parameter is determined when the convergence threshold is reached.

[0165] S3: As Figure 5 As shown, a set of nonlinear equations is obtained through the time difference of arrival (TDOA), a set of linear error equations is obtained by Taylor expansion, and another set of three-dimensional coordinates of the label is obtained by the least squares method.

[0166] In this embodiment, UWB base stations A, B, and C simultaneously transmit signals to tag D, and record the timestamp T of tag D receiving the signal from base station A. AD The timestamp T of receiving the signal from base station B BD The timestamp T of receiving the base station C signal CD ;

[0167] Calculate the time difference:

[0168] ΔT AB =T AD -T BD

[0169] ΔT AC =T AD -T CD

[0170] ΔT BC =T BD -T CD

[0171] Establish a system of distance equations:

[0172]

[0173] Where c is the speed of light;

[0174] Taylor expansion to initial coordinates (x) d0 ,y d0 ,z d0 Linearize the equation:

[0175]

[0176] After linearization, the original system of equations can be expressed in matrix form:

[0177]

[0178] Solving using the least squares method:

[0179] Set initial coordinates (x) d0 ,y d0 ,z d0 )=(L x / 2,L y / 2,L z / 2);

[0180] Substituting into the matrix, we obtain b.

[0181] The error vector is obtained using the least squares method:

[0182]

[0183] Update initial coordinates:

[0184]

[0185] Determine if convergence has occurred:

[0186] Set the convergence threshold to cvgval, when Stop the iteration and obtain the coordinates LS(x) ls ,y ls ,z ls ), (x ls ,y ls ,z ls () represents the three-dimensional coordinates obtained by the TDOA algorithm and the least squares method. This parameter is determined when the convergence threshold is reached.

[0187] S4: The initial three-dimensional coordinates of the label are obtained by taking the weighted average of the two sets of three-dimensional coordinates of the label;

[0188] In this embodiment, the initial three-dimensional coordinates of label D are obtained by weighted averaging of the two sets of three-dimensional coordinates; the error of the coordinates obtained by Newton's iteration method is σ1, and the error of the coordinates obtained by the least squares method is σ2.

[0189] Calculate the weights:

[0190]

[0191] Weight normalization:

[0192]

[0193] Calculate the weighted average:

[0194] x av =w′1·x nr +w′2·x ls

[0195] y av =w′1·y nr +w′2·y ls

[0196] Z av =w′1·z nr +w′2·z ls

[0197] S5: As Figure 6 As shown, the axial velocity and acceleration components are obtained through the inertial navigation system (IMU). The initial three-dimensional coordinates are combined with the velocity and acceleration components to construct a state transition model. The state at the next moment is predicted by Kalman filtering and the three-dimensional coordinates of the label are updated.

[0198] In this embodiment, for a total dataset {x1,x2,...,x} containing N data points... N Calculation of the population standard deviation;

[0199] Overall mean:

[0200]

[0201] The sum of the squares of the differences between each data point and the population mean:

[0202]

[0203] Overall standard deviation:

[0204]

[0205] The standard deviation of position observation noise is obtained from the static state. Acceleration observation noise standard deviation

[0206] The standard deviation of position process noise is obtained from the motion state. Speed ​​process noise standard deviation Acceleration process noise standard deviation

[0207] Establish the initial state vector:

[0208]

[0209] Wherein, the axial velocity component v x0 v y0 v z0 and acceleration component a x0 a y0 a z0 Obtained by an inertial measurement unit (IMU);

[0210] Establish the initial covariance matrix:

[0211]

[0212] Among them, P ij (i = 1, 2, 3; j = 1, 2, 3) represents the uncertainty of the initial state, that is, from the top left corner to the bottom right corner, it represents the uncertainty of position, velocity and acceleration in the initial state. Generally, a large value can be taken, such as 1000, etc.

[0213] Establish the state transition matrix:

[0214]

[0215] Where Δt is the time interval;

[0216] Establish the process noise covariance matrix:

[0217]

[0218] Establish the observation matrix:

[0219]

[0220] Establish the observation noise covariance matrix:

[0221]

[0222] Calculate the predicted state:

[0223]

[0224] Calculate the prediction covariance matrix:

[0225] P pred (t+Δt)=FP(t)F T +Q

[0226] Calculate the Kalman filter gain:

[0227] K(t+Δt)=P pred (t+Δt)H T HP pred (t+Δt)H T +R) -1 Establish observation vectors:

[0228]

[0229] Update the covariance matrix:

[0230] P(t+Δt)=(IK(t+Δt)H)P pred (t+Δt)

[0231] Where I is a 9×9 identity matrix;

[0232] Update status:

[0233]

[0234] in, The final three-dimensional spatial positioning result is the state vector after the initial state vector passes through Δt.

[0235] The step numbers in this embodiment are only set for ease of explanation and do not impose any restrictions on the order of the steps. The execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art.

[0236] Example 2

[0237] This embodiment provides a multi-algorithm collaborative 3D positioning system based on UWB and IMU, used to implement the multi-algorithm collaborative 3D positioning method based on UWB and IMU in Embodiment 1 above. The system includes: a 3D spatial rectangular coordinate system construction module, a distance variable construction module, a first linear error equation system construction module, a first set of 3D coordinate calculation modules, a second set of linear error equation system construction modules, a second set of 3D coordinate calculation modules, an initial 3D coordinate construction module, a state transition model construction module, and a final 3D spatial positioning result output module.

[0238] In this embodiment, the three-dimensional spatial rectangular coordinate system construction module is used to construct a three-dimensional spatial rectangular coordinate system based on multiple UWB base stations, wherein the UWB base stations and the tag simultaneously receive and transmit signals;

[0239] In this embodiment, the distance variable construction module is used to measure the distance between the tag and the UWB base station through bilateral bidirectional ranging, and obtain the corresponding distance variable;

[0240] In this embodiment, the first linear error equation system construction module is used to establish a nonlinear equation system through a trilateration algorithm, and then transform it into a linear error equation system by substituting distance variables.

[0241] In this embodiment, the first set of three-dimensional coordinate calculation modules is used to obtain a set of three-dimensional coordinates of the label through Newton's iteration method;

[0242] In this embodiment, the second linear error equation system construction module is used to obtain a nonlinear equation system through the arrival time difference, and to obtain a linear error equation system based on Taylor expansion approximation;

[0243] In this embodiment, the second set of three-dimensional coordinate calculation modules is used to obtain another set of three-dimensional coordinates of the label using the least squares method;

[0244] In this embodiment, the initial three-dimensional coordinate construction module is used to obtain the initial three-dimensional coordinates of the label by weighted averaging of the two sets of three-dimensional coordinates of the label;

[0245] In this embodiment, the state transition model construction module is used to obtain the axial velocity component and acceleration component through the inertial navigation system, and construct the state transition model based on the initial three-dimensional coordinates, velocity component, and acceleration component;

[0246] In this embodiment, the final three-dimensional spatial positioning result output module is used to predict the state at the next moment based on Kalman filtering and update the three-dimensional coordinates of the label, and output the final three-dimensional spatial positioning result.

[0247] Example 3

[0248] This embodiment provides a storage medium, which may be a ROM, RAM, disk, optical disk, or other storage medium. The storage medium stores one or more programs. When the programs are executed by the processor, they implement the multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU as described in Embodiment 1.

[0249] Example 4

[0250] This embodiment provides a computing device, which may be a desktop computer, laptop computer, smartphone, PDA handheld terminal, tablet computer or other terminal device with display function. The computing device includes a processor and a memory. The memory stores one or more programs. When the processor executes the program stored in the memory, it implements the multi-algorithm collaborative three-dimensional localization method based on UWB and IMU according to Embodiment 1.

[0251] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A multi-algorithm collaborative 3D localization method based on UWB and IMU, characterized in that, Includes the following steps: A three-dimensional Cartesian coordinate system is constructed based on multiple UWB base stations. The UWB base stations and tags simultaneously receive and transmit signals. The distance between the tag and the UWB base station is measured by bilateral bidirectional ranging, and the corresponding distance variables are obtained. A nonlinear system of equations is established using a trilateration algorithm. By substituting the distance variable, the system is transformed into a linear error system of equations. A set of three-dimensional coordinates of the tag is obtained through Newton's iteration method. A set of nonlinear equations is obtained by the time difference of arrival, a set of linear error equations is obtained by Taylor expansion approximation, and another set of three-dimensional coordinates of the tag is obtained by the least squares method. The initial three-dimensional coordinates of the label are obtained by weighted averaging of the two sets of three-dimensional coordinates, and the weight value of the weighted average is calculated based on the error between the two sets of three-dimensional coordinates. The axial velocity and acceleration components are obtained through the inertial navigation system. A state transition model is constructed based on the initial three-dimensional coordinates, velocity components, and acceleration components. The state at the next moment is predicted based on Kalman filtering, and the three-dimensional coordinates of the tag are updated. The final three-dimensional spatial positioning result is then output.

2. The multi-algorithm collaborative 3D localization method based on UWB and IMU according to claim 1, characterized in that, A nonlinear system of equations is established using a trilateration algorithm, and then transformed into a linear error system by substituting distance variables. Specifically, this includes: Four UWB base stations capable of simultaneously transmitting and receiving signals are selected, and their coordinates are represented as: S(0,0,0), A(L... x ,0,0)B(0,L y ,0)C(0,0,L) z ), L x L y L z These represent the lengths of each coordinate axis in a rectangular coordinate system; Let the label coordinates be D(x) d ,y d ,z d ); The established system of nonlinear equations is expressed as follows: Among them, Dis A Dis represents the distance variable from UWB base station A to tag D. B Dis represents the distance variable from UWB base station B to tag D. C This represents the distance variable from UWB base station C to tag D.

3. The multi-algorithm collaborative 3D localization method based on UWB and IMU according to claim 2, characterized in that, A set of three-dimensional coordinates of the label is obtained through Newton's iteration method, specifically including: Establish function vectors: Create a variable vector: Calculate function vector For variable vectors From the partial derivative matrix, we obtain the Jacobian matrix: Where J represents the Jacobian matrix; The iterative process of Newton's method is as follows: Constructing variable vectors Where, x dk y dk z dk These represent the x, y, and y coordinates of the label during the k-th iteration, respectively. Set initial variable vector Calculate the function vector: Calculate the Jacobian matrix: Solve the system of linear error equations Solving Update the variable vector: Set the convergence threshold to cvgval, when the determination Stop the iteration and obtain the coordinates NR(x) nr ,y nr ,z nr ), which is a set of three-dimensional coordinates of the label obtained by Newton's iteration method.

4. The multi-algorithm collaborative 3D localization method based on UWB and IMU according to claim 1, characterized in that, The nonlinear equations are obtained by using the time difference of arrival, and the linear error equations are obtained by approximation based on Taylor expansion, specifically including: Four UWB base stations capable of simultaneously transmitting and receiving signals are selected, and their coordinates are represented as: S(0,0,0), A(L... x ,0,0)B(0,L y ,0)C(0,0,L) z ), L x L y L z These represent the lengths of each coordinate axis in a rectangular coordinate system; Let the label coordinates be D(x) d ,y d ,z d ); Calculate the time difference: ΔT AB =T AD -T BD ΔT AC =T AD -T CD ΔT BC =T BD -T CD Establish a system of distance equations: Taylor expansion to initial coordinates (x) d0 ,y d0 ,z d0 Linearize the equation: After linearization, the original system of equations can be expressed in matrix form: Among them, T AD T represents the timestamp of tag D receiving the signal from UWB base station A. BD T represents the timestamp of tag D receiving the signal from UWB base station B. CD This represents the timestamp when tag D receives the signal from UWB base station C, where c represents the speed of light.

5. The multi-algorithm collaborative 3D localization method based on UWB and IMU according to claim 4, characterized in that, Another set of three-dimensional coordinates for the label is obtained using the least squares method, specifically including: Set initial coordinates (x) d0 ,y d0 ,z d0 )=(L x / 2,L y / 2,L z / 2); Substituting into the matrix, we obtain b. The error vector is obtained using the least squares method: Update initial coordinates: Set the convergence threshold to cvgval, when the determination Stop the iteration and obtain the coordinates LS(x) ls ,y ls ,z ls ), which serves as another set of three-dimensional coordinates for the label obtained through the least squares method.

6. The multi-algorithm collaborative 3D localization method based on UWB and IMU according to claim 1, characterized in that, The initial three-dimensional coordinates of the label are obtained by weighted averaging of the two sets of three-dimensional coordinates, specifically including: The error of one set of three-dimensional coordinates of the tag obtained by Newton's iteration method is σ1, and the error of another set of three-dimensional coordinates of the tag obtained by the least squares method is σ2. Calculate the weights: Weight normalization: Calculate the weighted average: x av =w′1·x nr +w′2·x ls y av =w′1·y nr +w′2·y ls With av =w′1·z nr +w′2 z ls Among them, (x nr ,y nr ,z nr (x) represents a set of three-dimensional coordinates of the label obtained through Newton's iteration method. ls ,y ls ,z ls ) represents another set of three-dimensional coordinates of the label obtained by the least squares method.

7. The multi-algorithm collaborative 3D localization method based on UWB and IMU according to claim 1, characterized in that, A state transition model is constructed based on the initial 3D coordinates, velocity components, and acceleration components. The state at the next moment is predicted using Kalman filtering, and the 3D coordinates of the label are updated. The final 3D spatial localization result is output, specifically including: The standard deviation of position observation noise is obtained from the static state. Acceleration observation noise standard deviation The standard deviation of position process noise is obtained from the motion state. Speed ​​process noise standard deviation Acceleration process noise standard deviation Establish the initial state vector: Wherein, the axial velocity component v x0 v y0 v z0 and acceleration component a x0 a y0 a z0 Obtained by the inertial measurement unit; Establish the initial covariance matrix: Among them, P ij The initial state is uncertain, i = 1, 2, 3, j = 1, 2, 3; Establish the state transition matrix: Where Δt is the time interval; establish the process noise covariance matrix: Establish the observation matrix: Establish the observation noise covariance matrix: Calculate the predicted state: Calculate the prediction covariance matrix: Calculate the Kalman filter gain: Establish observation vectors: Update the covariance matrix: P(t+Δt)=(I-K(t+Δt)H)P pred (t+Δt) Where I represents the identity matrix; Update status: in, This is the final three-dimensional spatial positioning result.

8. A multi-algorithm collaborative three-dimensional positioning system based on UWB and IMU, characterized in that, The method for implementing the multi-algorithm collaborative three-dimensional localization method based on UWB and IMU as described in any one of claims 1-7 includes: a three-dimensional spatial rectangular coordinate system construction module, a distance variable construction module, a first linear error equation system construction module, a first set of three-dimensional coordinate calculation modules, a second set of linear error equation system construction modules, a second set of three-dimensional coordinate calculation modules, an initial three-dimensional coordinate construction module, a state transition model construction module, and a final three-dimensional spatial localization result output module. The three-dimensional spatial rectangular coordinate system construction module is used to construct a three-dimensional spatial rectangular coordinate system based on multiple UWB base stations, wherein the UWB base stations and tags simultaneously receive and transmit signals; The distance variable construction module is used to measure the distance between the tag and the UWB base station through bilateral bidirectional ranging, and obtain the corresponding distance variable; The first linear error equation system construction module is used to establish a nonlinear equation system through a trilateration algorithm, and then transform it into a linear error equation system by substituting distance variables. The first set of three-dimensional coordinate calculation modules is used to obtain a set of three-dimensional coordinates of the label using Newton's iteration method; The second linear error equation system construction module is used to obtain a nonlinear equation system through the arrival time difference, and to obtain a linear error equation system based on the Taylor expansion approximation. The second set of three-dimensional coordinate calculation modules is used to obtain another set of three-dimensional coordinates of the label using the least squares method; The initial three-dimensional coordinate construction module is used to obtain the initial three-dimensional coordinates of the label by weighted averaging of the two sets of three-dimensional coordinates of the label; The state transition model construction module is used to obtain the axial velocity component and acceleration component through the inertial navigation system, and to construct a state transition model based on the initial three-dimensional coordinates, velocity component, and acceleration component. The final three-dimensional spatial positioning result output module is used to predict the state at the next moment based on Kalman filtering and update the three-dimensional coordinates of the label, and output the final three-dimensional spatial positioning result.

9. A computer-readable storage medium storing a program, characterized in that, When the program is executed by the processor, it implements the multi-algorithm collaborative three-dimensional localization method based on UWB and IMU as described in any one of claims 1-7.

10. A computer device comprising a processor and a memory for storing a processor-executable program, characterized in that, When the processor executes the program stored in the memory, it implements the multi-algorithm collaborative three-dimensional localization method based on UWB and IMU as described in any one of claims 1-7.

Images