Multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU

Abstract

The invention discloses a multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU, and the method comprises the steps: constructing a three-dimensional space rectangular coordinate system based on a plurality of UWB base stations, measuring the distance between a label and the UWB base stations through bilateral bidirectional distance measurement, and obtaining a corresponding distance variable; establishing a nonlinear equation set through a trilateral positioning algorithm, substituting a distance variable, converting the nonlinear equation set into a linear error equation set, and obtaining a group of three-dimensional coordinates of the label through a Newton iteration method; obtaining a nonlinear equation set through the time difference of arrival, obtaining a linear error equation set based on Taylor expansion approximation, and obtaining another group of three-dimensional coordinates of the label through a least square method; performing weighted average on the two groups of three-dimensional coordinates to obtain an initial three-dimensional coordinate; and constructing a state transition model based on the initial three-dimensional coordinate, the velocity component and the acceleration component, predicting the state of the next moment based on Kalman filtering, updating the three-dimensional coordinate of the label, and outputting a final three-dimensional space positioning result. According to the invention, high-precision three-dimensional positioning can be realized.

Description

Technical Field

[0001] The present invention relates to the field of target positioning technology, and in particular to a multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU. Background Art

[0002] At present, the commonly used indoor positioning technologies include WiFi positioning, Bluetooth positioning, ultra-wideband positioning (UWB), infrared positioning, inertial measurement positioning technology (IMU), etc. These positioning technologies have their own advantages and disadvantages. Among them, WiFi positioning has the advantages of wide deployment, direct use of existing basic WiFi facilities, and low cost, but the positioning range is small and greatly affected by environmental interference; Bluetooth positioning has the advantages of high accuracy and low energy consumption, but the positioning range is small; infrared positioning has the advantages of high accuracy and no influence from electromagnetic interference, but 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 the deployment cost is high and affected by non-line of sight; IMU has the advantages of high positioning accuracy, no reliance on external signal sources and strong real-time performance, but there is drift error.

[0003] The current mainstream method is to tightly combine the two positioning systems of UWB and IMU to complement their advantages and disadvantages. By using UWB to compensate for the drift error of IMU, IMU improves the measurement accuracy of UWB and reduces the non-line-of-sight error. However, this method generally requires the deployment of a large number of UWB base stations, which is prone to high costs. If you want to reduce the deployment cost while ensuring high-precision measurement, you also need to combine multiple algorithms to process the acquired data and reduce the measurement error by combining different algorithms. In addition, this mainstream method can generally only be applied to two-dimensional planes and has relatively few applicable scenarios. Summary of the invention

[0004] In order to overcome the defects and shortcomings of the prior art, the present invention provides a multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU. The present invention establishes a nonlinear equation group through a three-sided positioning algorithm, substitutes the distance variable into a linear error equation group, and obtains a set of three-dimensional coordinates of the tag through the Newton iteration method; obtains a nonlinear equation group through the arrival time difference, obtains a linear error equation group based on Taylor expansion approximation, obtains another set of three-dimensional coordinates of the tag through the least squares method, and solves the nonlinear equation groups obtained by the two different ranging methods through different mathematical iteration methods, which can significantly reduce the measurement error and has high accuracy. In addition, the instantaneous acceleration vector and velocity vector are obtained through the IMU to construct an equation to predict the state at the next moment, and the positioning data of the UWB can be corrected, thereby minimizing the non-line-of-sight error of the UWB to the greatest extent.

[0005] The second object of the present invention is to provide a multi-algorithm collaborative three-dimensional positioning system based on UWB and IMU;

[0006] A third object of the present invention is to provide a computer readable storage medium;

[0007] A fourth object of the present invention is to provide a computer device.

[0008] In order to achieve the above object, the present invention adopts the following technical solutions:

[0009] The present invention provides a multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU, comprising the following steps:

[0010] A three-dimensional rectangular coordinate system is constructed based on multiple UWB base stations. The UWB base station and the tag receive and transmit signals simultaneously. The distance between the tag and the UWB base station is measured through bilateral two-way ranging to obtain the corresponding distance variable.

[0011] A nonlinear equation group is established through the three-sided positioning algorithm, and the distance variable is substituted into the linear error equation group. A set of three-dimensional coordinates of the tag is obtained through the Newton iteration method;

[0012] The nonlinear equations are obtained by the arrival time difference, and the linear error equations are obtained based on the Taylor expansion approximation. 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 average of the two sets of three-dimensional coordinates of the label;

[0014] The axial velocity component and acceleration component are obtained through the inertial navigation system. The state transfer model is constructed based on the initial three-dimensional coordinates, velocity component, and acceleration component. The state at the next moment is predicted based on the Kalman filter and the three-dimensional coordinates of the label are updated to output the final three-dimensional spatial positioning result.

[0015] As a preferred technical solution, a nonlinear equation group is established through a three-sided positioning algorithm, and the distance variable is substituted into a linear error equation group, which specifically includes:

[0016] Four UWB base stations that can send and receive signals simultaneously 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 Respectively represent the axis lengths of each coordinate axis in the rectangular coordinate system;

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

[0018] The established nonlinear equations are expressed as:

[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 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 by Newton iteration method, which specifically includes:

[0022] Create a function vector:

[0023]

[0024] Create a vector of variables:

[0025]

[0026] Calculate function vector For variable vector The partial derivative matrix of , we get the Jacobian matrix:

[0027]

[0028] Where J represents the Jacobian matrix;

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

[0030] Constructing variable vectors Among them, x dk ,y dk 、z dk Respectively represent the horizontal, vertical and vertical coordinates of the label during the kth iteration;

[0031] Set the initial variable vector

[0032] Compute the function vector:

[0033]

[0034] Compute the Jacobian matrix:

[0035]

[0036] Solving the linear error equations Solved

[0037] Update the variable vector:

[0038]

[0039] Set the convergence threshold to cvgval, when judging When the iteration stops and the coordinates NR(x nr ,y nr ,z nr ), as a set of three-dimensional coordinates of the labels obtained by Newton iteration method.

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

[0041] Four UWB base stations that can send and receive signals simultaneously 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 Respectively represent the axis lengths of each coordinate axis in the 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] Set up the distance equation system:

[0048]

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

[0050]

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

[0052]

[0053] Among them, T AD Indicates the timestamp of the signal received by tag D from UWB base station A, T BD Indicates the timestamp of the tag D receiving the signal from the UWB base station B, T CD It indicates the timestamp when tag D receives the signal from UWB base station C, and c represents the speed of light.

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

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

[0056] Substitute into the matrix to obtain b,

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

[0058]

[0059] Update the initial coordinates:

[0060]

[0061] Set the convergence threshold to cvgval, when judging When the iteration stops and the coordinates LS(x ls ,y ls ,z ls ), as another set of three-dimensional coordinates of the labels obtained by the least squares method.

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

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

[0064] Calculate 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 ) represents a set of three-dimensional coordinates of the label obtained by Newton iteration method, (x 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 based on Kalman filtering and the three-dimensional coordinates of the label are updated to output the final three-dimensional spatial positioning result, which specifically includes:

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

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

[0076] Create the initial state vector:

[0077]

[0078] Among them, 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] Create the initial covariance matrix:

[0080]

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

[0082]

[0083] Where Δt is the time interval;

[0084] Build the process noise covariance matrix:

[0085]

[0086] Create the observation matrix:

[0087]

[0088] Build the observation noise covariance matrix:

[0089]

[0090] Compute the predicted status:

[0091]

[0092] Compute the forecast 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] Create the observation vector:

[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 space positioning result.

[0104] The present invention also provides a multi-algorithm collaborative three-dimensional positioning system based on UWB and IMU, including: a three-dimensional space rectangular coordinate system construction module, a distance variable construction module, a first linear error equation group construction module, a first three-dimensional coordinate calculation module, a second linear error equation group construction module, a second three-dimensional coordinate calculation module, an initial three-dimensional coordinate construction module, a state transfer model construction module, and a final three-dimensional space positioning result output module;

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

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

[0107] The first linear error equation group building module is used to establish a nonlinear equation group through a three-sided positioning algorithm, and substitute the distance variable into the linear error equation group;

[0108] The first set of three-dimensional coordinate calculation module is used to obtain a set of three-dimensional coordinates of the tag through Newton iteration method;

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

[0110] The second set of three-dimensional coordinate calculation module is used to obtain another set of three-dimensional coordinates of the label by least square method;

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

[0112] The state transfer model building module is used to obtain the axial velocity component and the acceleration component through the inertial navigation system, and build the state transfer model based on the initial three-dimensional coordinates, the velocity component, and the 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 tag, and output the final three-dimensional spatial positioning result.

[0114] The present invention also provides a computer-readable storage medium storing a program, which, when executed by a processor, implements the multi-algorithm collaborative three-dimensional positioning 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 a program executable by the processor. When the processor executes the program stored in the memory, the multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU as described above is implemented.

[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 through only four UWB positioning base stations, one UWB tag, and one IMU module, and has a simpler structure.

[0118] (2) On the one hand, the present invention measures the distance between the tag and the base station by the bilateral two-way ranging method, and then establishes a nonlinear equation group by the trilateral positioning method, and further obtains the minimum error by the Newton iteration method; on the other hand, a nonlinear equation group is established by the time difference ranging method, and further obtains the minimum error by the least squares method; the nonlinear equation groups obtained by the two different ranging methods are solved by different mathematical iterative methods, which can significantly reduce the measurement error and has high accuracy.

[0119] (3) The present invention obtains instantaneous acceleration vector and velocity vector through IMU to construct equations to predict the state at the next moment, and can correct the UWB positioning data, thereby minimizing the non-line-of-sight error of UWB.

[0120] (4) The present invention reduces static noise error and dynamic noise error by adopting Kalman filtering, so that the present invention can achieve high-precision three-dimensional positioning in any complex environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0121] Figure 1 It is a flow chart of a multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU of the present invention;

[0122] Figure 2 A schematic diagram of a spatial coordinate system constructed for the present invention;

[0123] Figure 3 It is a schematic diagram of a process of measuring the distance between a tag and a base station by using double-sided two-way ranging (DS-TWR) of the present invention;

[0124] Figure 4 This is a schematic diagram of a process of adopting the three-side positioning method and iterating with the Newton iteration method in the present invention;

[0125] Figure 5 This is a schematic diagram of the present invention using the TDOA positioning method and iterating using the least squares method;

[0126] Figure 6It is a schematic diagram of the flow of Kalman filtering of the present invention. DETAILED DESCRIPTION

[0127] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0128] Example 1

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

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

[0131] In this embodiment, four UWB base stations that can simultaneously send and receive signals construct a finite space rectangular coordinate system, and the lengths of each axis are L x , L y , L z , that is, the coordinates of the four base stations are S(0,0,0), A(L 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 , D receives the signal timestamp T D1 and the timestamp T of the response signal D2 , the timestamp T of the signal received by A A2 , the time stamp T of A sending the signal again A3 , the time stamp T of D receiving the signal again D3 ; The time difference between A sending a signal and receiving a signal is T round1 , the time difference between A receiving the signal and sending the signal again is T round2 , the time difference between D receiving the signal and sending 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] Among them, 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 from B to D is Dis B , the distance from C to D is Dis C .

[0142] S2: Figure 4 As shown, a nonlinear equation system is established through the three-sided positioning algorithm, and the distance variable Dis is substituted A , distance variable Dis B , distance variable Dis C Transformed into a set of linear error equations, and then a set of three-dimensional coordinates of the label is obtained through Newton iteration method, including:

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

[0144]

[0145] Create a function vector:

[0146]

[0147] Create a vector of variables:

[0148]

[0149] Create the Jacobian matrix (function vector For variable vector The partial derivative matrix of

[0150]

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

[0152] Variable vector Among them, x dk ,y dk 、z dk Respectively represent the horizontal, vertical and vertical coordinates of the label during the kth iteration;

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

[0154] Set the initial variable vector

[0155] Compute the function vector:

[0156]

[0157] Compute the Jacobian matrix:

[0158]

[0159] Solving the linear error equations Solved

[0160] Update the variable vector:

[0161]

[0162] The variable vector is continuously updated so that the preset value is constantly approaching the solution of the equation system. When this variable is less than the convergence threshold, it means that the solution of the equation system has been approached.

[0163] Determine whether it converges:

[0164] Set the convergence threshold to cvgval, when Stop the iteration and get 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 iteration method, and the parameter is determined when the convergence threshold is reached.

[0165] S3: Figure 5 As shown, a nonlinear equation group is obtained by using the time difference of arrival (TDOA), and a linear error equation group is obtained by using Taylor expansion approximation, and then another set of three-dimensional coordinates of the tag is obtained by the least square 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 the signal received from base station B BD , the timestamp T of receiving the signal from base station C 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] Set up the distance equation system:

[0172]

[0173] Where c is the speed of light;

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

[0175]

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

[0177]

[0178] Least squares solution:

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

[0180] Substitute into the matrix to obtain b,

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

[0182]

[0183] Update the initial coordinates:

[0184]

[0185] Determine whether it converges:

[0186] Set the convergence threshold to cvgval, when Stop the iteration and get 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, and the parameter is determined when the convergence threshold is reached;

[0187] S4: performing weighted average of the two sets of three-dimensional coordinates of the tag to obtain the initial three-dimensional coordinates of the tag;

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

[0189] Calculate 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: Figure 6 As shown, the axial velocity component and acceleration component are obtained through the inertial navigation system (IMU), the initial three-dimensional coordinates and the velocity component and acceleration component are used to construct a state transfer model, and the state at the next moment is predicted through Kalman filtering and the three-dimensional coordinates of the tag are updated;

[0198] In this embodiment, for a total data set {x1, x2, ..., x 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] Population standard deviation:

[0204]

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

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

[0207] Create the initial state vector:

[0208]

[0209] Among them, 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 (IMU);

[0210] Create the initial covariance matrix:

[0211]

[0212] Among them, P ij (i=1,2,3; j=1,2,3) is the uncertainty of the initial state, that is, from the upper left corner to the lower right corner, they are the uncertainties of position, velocity and acceleration in the initial state, which can generally be larger values, such as 1000;

[0213] Create a state transfer matrix:

[0214]

[0215] Where Δt is the time interval;

[0216] Build the process noise covariance matrix:

[0217]

[0218] Create the observation matrix:

[0219]

[0220] Build the observation noise covariance matrix:

[0221]

[0222] Compute the predicted status:

[0223]

[0224] Compute the forecast 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 Create the observation vector:

[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, is the final three-dimensional space positioning result, that is, the state vector after the initial state vector passes through Δt.

[0235] The step numbers in this embodiment are only provided for the convenience of explanation and description, and no limitation is imposed on the order of the steps. The execution order of the steps 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 three-dimensional positioning system based on UWB and IMU, which is used to implement the multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU in the above-mentioned embodiment 1. The system includes: a three-dimensional space rectangular coordinate system construction module, a distance variable construction module, a first linear error equation group construction module, a first group of three-dimensional coordinate calculation module, a second linear error equation group construction module, a second group of three-dimensional coordinate calculation module, an initial three-dimensional coordinate construction module, a state transition model construction module, and a final three-dimensional space positioning result output module;

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

[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 two-way ranging to obtain the corresponding distance variable;

[0240] In this embodiment, the first linear error equation group building module is used to establish a nonlinear equation group through a three-sided positioning algorithm, and substitute the distance variable into the linear error equation group;

[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 tag by Newton iteration method;

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

[0243] In this embodiment, the second set of three-dimensional coordinate calculation module is used to obtain another set of three-dimensional coordinates of the label by least square 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 average of two sets of three-dimensional coordinates of the label;

[0245] In this embodiment, the state transfer model construction module is used to obtain the axial velocity component and the acceleration component through the inertial navigation system, and to construct the state transfer model based on the initial three-dimensional coordinates, the velocity component, and the 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 tag, 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, CD or other storage medium, and the storage medium stores one or more programs. When the program is executed by the processor, the multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU of embodiment 1 is implemented.

[0249] Example 4

[0250] This embodiment provides a computing device, which can be a desktop computer, a laptop computer, a smart phone, a PDA handheld terminal, a tablet computer or other terminal device with a display function. The computing device includes a processor and a memory, and the memory stores one or more programs. When the processor executes the program stored in the memory, the multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU of Example 1 is implemented.

[0251] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU, characterized in that: The steps include: A three-dimensional rectangular coordinate system is constructed based on multiple UWB base stations. The UWB base station and the tag receive and transmit signals simultaneously. The distance between the tag and the UWB base station is measured through bilateral two-way ranging to obtain the corresponding distance variable. A nonlinear equation group is established through the three-sided positioning algorithm, and the distance variable is substituted into the linear error equation group. A set of three-dimensional coordinates of the tag is obtained through the Newton iteration method; The nonlinear equations are obtained by the arrival time difference, and the linear error equations are obtained based on the Taylor expansion approximation. 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 average of the two sets of three-dimensional coordinates of the label; The axial velocity component and acceleration component are obtained through the inertial navigation system. The state transfer model is constructed based on the initial three-dimensional coordinates, velocity component, and acceleration component. The state at the next moment is predicted based on the Kalman filter and the three-dimensional coordinates of the label are updated to output the final three-dimensional spatial positioning result.

2. The multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU according to claim 1 is characterized in that: The nonlinear equations are established by the three-sided positioning algorithm, and the distance variables are substituted into the linear error equations, which include: Four UWB base stations that can send and receive signals simultaneously 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 Respectively represent the axis lengths of each coordinate axis in the rectangular coordinate system; Let the label coordinates be D(x d ,y d ,z d ); The established nonlinear equations are expressed as: 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 Represents the distance variable from UWB base station C to tag D.

3. The multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU according to claim 2 is characterized in that: A set of three-dimensional coordinates of the label is obtained through Newton iteration method, including: Create a function vector: Create a variable vector: Calculate function vector For variable vector The partial derivative matrix of , we get the Jacobian matrix: Where J represents the Jacobian matrix; The iterative process of Newton's iteration method is: Constructing variable vectors Among them, x dk ,y dk 、z dk Respectively represent the horizontal, vertical and vertical coordinates of the label during the kth iteration; Set the initial variable vector Compute the function vector: Compute the Jacobian matrix: Solving the linear error equations Solved Update the variable vector: Set the convergence threshold to cvgval, when judging When the iteration stops and the coordinates NR(x nr ,y nr ,z nr ), as a set of three-dimensional coordinates of the labels obtained by Newton iteration method.

4. The multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU according to claim 1, characterized in that: The nonlinear equations are obtained by the arrival time difference, and the linear error equations are obtained based on the Taylor expansion approximation, including: Four UWB base stations that can send and receive signals simultaneously 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 Respectively represent the axis lengths of each coordinate axis in the 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 Set up the distance equation system: Taylor expansion to the initial coordinates (x d0 ,y d0 ,z d0 ) to linearize the equation: After linearization, the original system of equations can be expressed in matrix form: Among them, T AD Indicates the timestamp of the signal received by tag D from UWB base station A, T BD Indicates the timestamp of the tag D receiving the signal from the UWB base station B, T CD It indicates the timestamp when tag D receives the signal from UWB base station C, and c represents the speed of light.

5. The multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU according to claim 4 is characterized in that: Another set of three-dimensional coordinates of the label is obtained by the least squares method, including: Set the initial coordinates (x d0 ,y d0 ,z d0 )=(L x / 2,L y / 2,L z / 2); Substitute into the matrix to obtain b, The error vector is obtained by the least squares method: Update the initial coordinates: Set the convergence threshold to cvgval, when judging When the iteration stops and the coordinates LS(x ls ,y ls ,z ls ), as another set of three-dimensional coordinates of the labels obtained by the least squares method.

6. The multi-algorithm collaborative three-dimensional positioning 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 the two sets of three-dimensional coordinates of the label, including: The error of one set of three-dimensional coordinates of the label obtained by the Newton iteration method is σ1, and the error of another set of three-dimensional coordinates of the label obtained by the least squares method is σ2; Calculate 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 with ls Among them, (x nr ,y nr ,z nr ) represents a set of three-dimensional coordinates of the label obtained by Newton iteration method, (x 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 three-dimensional positioning method based on UWB and IMU according to claim 1, characterized in that: Based on the initial three-dimensional coordinates, velocity components, and acceleration components, a state transfer model is constructed. Based on the Kalman filter, the state at the next moment is predicted and the three-dimensional coordinates of the label are updated. The final three-dimensional spatial positioning result is output, which includes: The standard deviation of the position observation noise is obtained from the stationary state Acceleration observation noise standard deviation The standard deviation of the position process noise is obtained from the motion state Speed ​​process noise standard deviation Acceleration process noise standard deviation Create the initial state vector: Among them, 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; Create the initial covariance matrix: Among them, P ij is the uncertainty of the initial state, i=1,2,3, j=1,2,3; Create a state transfer matrix: Where Δt is the time interval; Build the process noise covariance matrix: Create the observation matrix: Build the observation noise covariance matrix: Compute the predicted status: Compute the forecast covariance matrix: Calculate the Kalman filter gain: Create the observation vector: 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 space positioning result.

8. A multi-algorithm collaborative three-dimensional positioning system based on UWB and IMU, characterized in that: include: A three-dimensional rectangular coordinate system construction module, a distance variable construction module, a first linear error equation group construction module, a first three-dimensional coordinate calculation module, a second linear error equation group construction module, a second three-dimensional coordinate calculation module, an initial three-dimensional coordinate construction module, a state transfer model construction module, and a final three-dimensional space positioning result output module; The three-dimensional rectangular coordinate system construction module is used to construct a three-dimensional rectangular coordinate system based on multiple UWB base stations, and the UWB base stations and tags receive and transmit signals simultaneously; The distance variable construction module is used to measure the distance between the tag and the UWB base station through bilateral two-way ranging to obtain the corresponding distance variable; The first linear error equation group building module is used to establish a nonlinear equation group through a three-sided positioning algorithm, and substitute the distance variable into the linear error equation group; The first set of three-dimensional coordinate calculation module is used to obtain a set of three-dimensional coordinates of the tag through Newton iteration method; The second linear error equation group construction module is used to obtain a nonlinear equation group through the arrival time difference, and obtain a linear error equation group based on Taylor expansion approximation; The second set of three-dimensional coordinate calculation module is used to obtain another set of three-dimensional coordinates of the label by least square method; The initial three-dimensional coordinate construction module is used to obtain the initial three-dimensional coordinates of the label by weighted average of the two sets of three-dimensional coordinates of the label; The state transfer model building module is used to obtain the axial velocity component and the acceleration component through the inertial navigation system, and build the state transfer model based on the initial three-dimensional coordinates, the velocity component, and the 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 tag, 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 a processor, a multi-algorithm collaborative three-dimensional positioning method based on UWB and IMU is implemented as described in any one of claims 1 to 7.

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

Images