An antenna array-based single base station UWB / inertial fusion navigation method

Abstract

This invention discloses a single-base station UWB / inertial fusion navigation method based on an antenna array, belonging to the field of integrated navigation. This invention collects... k The system uses distance data between the UWB base station and the tag, carrier phase data received by the UWB base station array antenna, accelerometer data, and gyroscope data. It determines whether the navigation system is initialized; if so, it uses the carrier phase data received by the array antenna to calculate the tag's azimuth and elevation angles in the UWB coordinate system, and performs pre-integration between adjacent UWB measurement frames using inertial sensor data. It then optimizes the carrier navigation information by combining UWB ranging, angle measurement errors, and inertial pre-integration errors, and outputs the carrier's navigation information. This invention overcomes the shortcomings of multi-base station UWB / inertial fusion methods in terms of base station installation and deployment, and time synchronization, and improves navigation accuracy by performing global optimization of the carrier's pose through tight coupling.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation, and specifically relates to a single-base station UWB / inertial fusion navigation method based on an antenna array. Background Technology

[0002] Currently, Global Navigation Satellite Systems (GNSS) can provide accurate positioning information in open outdoor environments, but they cannot provide continuous and reliable navigation information indoors or underground. Therefore, positioning and navigation technologies under satellite unavailability conditions have become a research hotspot. Inertial sensors have high output frequencies, strong autonomy, and strong anti-interference capabilities, but inertial navigation systems experience rapid attitude divergence over long periods of operation. Ultra-wideband (UWB) positioning technology does not have accumulated errors and possesses advantages such as low transmission power, high transmission rate, and strong penetration capability. Therefore, fusing UWB with inertial sensor information can achieve accurate and reliable navigation information estimation, showing broad development prospects.

[0003] Most existing UWB / inertial fusion navigation methods are based on multi-base station positioning systems, using time of arrival (TOA) and time difference of arrival (TDOA) methods for positioning, and fusing information from inertial sensors using Kalman filtering. Multi-base station UWB positioning systems are complex to install and deploy, costly, and require strict clock synchronization. Furthermore, fusion methods based on Kalman filtering only estimate navigation information at the current moment, and are not globally optimal, thus limiting the accuracy of the integrated navigation system. Summary of the Invention

[0004] To address the technical problems mentioned in the background, this invention proposes a single-base station UWB / inertial fusion navigation method based on an antenna array. This method overcomes the shortcomings of multi-base station UWB positioning systems by using single-base station positioning technology and achieves tight coupling of UWB / inertial information within a graph optimization framework using inertial pre-integration technology, thereby obtaining globally optimal navigation information.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A single-base station UWB / inertial fusion navigation method based on an antenna array includes the following steps:

[0007] (1) Collect distance data between UWB base station and tag at time k. Carrier phase data received by UWB base station array antenna j∈[0,n-1], accelerometer data and gyroscope data in, Let j be the carrier phase received by the antenna element numbered j, and n be the number of antenna elements.

[0008] (2) Determine whether the navigation system has completed initialization. If it has not completed initialization, perform initialization to obtain the initial value of the gyroscope zero bias and the estimated value of the gravity component, and jump to step (1); if it has completed initialization, proceed to step (3).

[0009] (3) Using the carrier phase data received by the UWB base station array antenna, calculate the azimuth and elevation angle information of the tag in the UWB coordinate system;

[0010] (4) Use inertial sensor data to perform pre-integration between adjacent UWB measurement frames;

[0011] (5) Combine UWB ranging, angle measurement error and inertial pre-integration error to optimize the solution of the carrier navigation information;

[0012] (6) Output the navigation information of the carrier.

[0013] Furthermore, the initialization method in step (2) is as follows:

[0014] Initially at rest t s At a given moment, using the zero-velocity assumption at rest, the average value of the gyroscope output is calculated as the initial value for the gyroscope's zero bias:

[0015]

[0016] in, This is the initial value for zero bias of the gyroscope. Let Δt be the output value of the gyroscope in the inertial sensor coordinate system at time i, and Δt be the sampling period of the inertial sensor.

[0017] Using the zero-velocity assumption, the mean value of the accelerometer output is calculated as the gravity component in the inertial sensor coordinate system at the initial moment:

[0018]

[0019] in, This represents the estimated value of the gravity component in the inertial sensor coordinate system at the initial moment. Let be the output value of the accelerometer in the inertial sensor coordinate system at time i;

[0020] By rotating the inertial sensor coordinate system at the initial moment... Aligning the rotation matrix with the gravity vector in the local inertial frame, such that the gravity vector in the inertial sensor coordinate system is perpendicular to the local horizontal plane at the initial moment, is denoted as... The initial inertial sensor coordinate system after rotation is used as the navigation coordinate system to complete the initialization.

[0021] Furthermore, the specific method of step (3) is as follows:

[0022] Calculate the unambiguous carrier phase difference between two sets of carriers using the carrier phases received by three antenna elements that are not on the same straight line:

[0023]

[0024] Where m1, m2, and m3 are the numbers of three antenna array elements that are not on the same straight line. These are the carrier phase differences between antenna array elements m1 and m2 and between antenna array elements m1 and m3 at time k, respectively.

[0025] via carrier phase difference The coordinates and geometric arrangement of the antenna elements in the UWB coordinate system were determined to obtain the azimuth angle of the tag in the UWB coordinate system at time k. and pitch angle

[0026] Furthermore, the specific method of step (4) is as follows:

[0027] Inertial sensor data acquired at time k and Includes accelerometer data from time k-1 to time k. and gyroscope data in

[0028] i = 0, 1, 2, ..., (t(k) - t(k-1)) / Δt, where t(k) is the sampling time at time k, and t(k-1) is the sampling time at time k-1, i.e., the sampling time corresponding to the previous frame UWB measurement; the inertial sensor measurement model is:

[0029]

[0030]

[0031] Where, n a and n ω The white noise from the accelerometer and gyroscope, respectively, b ai and b ωi These are the zero biases of the accelerometer and gyroscope, respectively, and their derivatives are white noise. For the ideal value measured by the accelerometer, For the ideal value measured by the gyroscope, g W This is the gravity vector in the navigation coordinate system. This is the rotation matrix from the navigation coordinate system to the inertial sensor coordinate system at the sampling time;

[0032] The pre-integration between two adjacent inertial frames is:

[0033]

[0034]

[0035]

[0036] in, Pre-integration for position, For the velocity pre-integration, the rotation is represented by the quaternion γ. For rotational pre-integration; initially, and =0, Let R(γ) be a unit quaternion, and let R(γ) denote the transformation of the quaternion into a rotation matrix. Represents the multiplication operation of quaternions;

[0037] Between two adjacent UWB measurement frames, there are several inertial sensor data points. Using the above formula, an iterative method is employed to pre-integrate all the inertial sensor data between two adjacent UWB measurement frames, thereby ensuring that the frequency of the UWB measurement data is consistent with the inertial pre-integration frequency, thus obtaining the inertial pre-integration between two adjacent UWB measurement frames. and

[0038] Furthermore, the specific method of step (5) is as follows:

[0039] Establish the optimization variable X:

[0040] X = [x0, x1, ..., x l ]

[0041] in, l is the sequence number of the last frame. These represent the carrier position, velocity, quaternion, accelerometer and gyroscope zero bias in the k-th frame, respectively, with the carrier's body coordinate system coinciding with the inertial sensor coordinate system;

[0042] Establish the optimization function:

[0043] in, For inertial pre-integration error, This indicates that the inertial pre-integration error term is the inertial pre-integration error between the (k-1)th frame UWB measurement and the kth frame UWB measurement, where B is the set of all inertial measurements. This represents the covariance of the inertial pre-integration error; These are UWB ranging error, azimuth measurement error, and elevation measurement error, respectively. These represent the ranging error, azimuth measurement error, and elevation angle measurement error of the UWB measurement in the k-th frame, respectively, where U is the set of all UWB measurements. These are the covariances of UWB ranging error, azimuth measurement error, and elevation measurement error, respectively. This indicates that the error term is converted into Mahalanobis distance, where P is the covariance corresponding to the error term;

[0044] The inertial pre-integration error between two adjacent UWB measurement frames is obtained by the difference between the pre-integration prediction value and the pre-integration estimate value:

[0045]

[0046] in, Let be the rotation matrix from the navigation coordinate system to the body coordinate system at time k-1; These represent the positions of the aircraft's coordinate system in the navigation coordinate system at times k and k-1, respectively.

[0047] The velocities of the body coordinate system in the navigation coordinate system at times k and k-1 are respectively; Δt k The time interval between two adjacent UWB measurement frames; These are the quaternions representing the rotations of the body coordinate system in the navigation coordinate system at time k and time k-1, respectively. The accelerometer zero biases at time k and k-1 in the body coordinate system are respectively; The gyroscope zero bias in the body coordinate system at time k and time k-1 are respectively, [γ]. xyz This represents taking the x, y, and z components of the quaternion γ;

[0048] The ranging error of the k-th frame UWB measurement is obtained by the difference between the distance prediction value of the tag and the UWB base station and the distance measurement value:

[0049]

[0050] in, This represents the position of the tag in the inertial sensor coordinate system. The extrinsic parameters between the UWB coordinate system and the inertial sensor coordinate system at the initial moment were all obtained through calibration.

[0051] This represents the distance measurement between the tag and the UWB base station at time k;

[0052] The azimuth measurement error of the k-th frame UWB measurement is obtained by the difference between the predicted azimuth value and the measured azimuth value of the tag in the UWB coordinate system:

[0053]

[0054] in,[·] x 、[·]y These represent the x and y coordinates of the three-dimensional position vector, respectively. The azimuth angle measurement of the tag at time k in the UWB coordinate system obtained in step (3);

[0055] The pitch angle measurement error of the k-th frame UWB measurement is obtained by the difference between the predicted pitch angle value and the measured pitch angle value of the tag in the UWB coordinate system:

[0056]

[0057] in,[·] z This indicates taking the z-axis coordinate value of the three-dimensional position vector. The pitch angle measurement value of the label in the UWB coordinate system at time k obtained in step (3);

[0058] The Gauss-Newton method is used to iteratively solve the optimization function. The iteration stops when the error converges or the maximum number of iterations is set, and the optimal estimate of the system optimization variables is obtained, thus obtaining the navigation information of the carrier.

[0059] The beneficial effects of adopting the above technical solution are as follows:

[0060] 1. This invention overcomes the shortcomings of multi-base station UWB / inertial fusion methods in terms of base station installation and deployment, time synchronization, etc., by using single-base station positioning technology.

[0061] 2. This invention utilizes inertial pre-integration technology to achieve tight coupling between UWB and inertial information within a graph optimization framework, and performs global optimization of the carrier navigation information, thereby improving navigation accuracy. Attached Figure Description

[0062] Figure 1 This is a flowchart of a single-base station UWB / inertial fusion navigation method based on an antenna array, as described in an embodiment of the present invention. Detailed Implementation

[0063] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0064] A single-base station UWB / inertial fusion navigation method based on antenna array, such as... Figure 1 As shown, the steps are as follows:

[0065] Step 1: Collect distance data between the UWB base station and the tag at time k. Carrier phase data received by UWB base station array antenna j∈[0,n-1], accelerometer data and gyroscope data

[0066] Step 2: Determine whether the navigation system has completed initialization. If it has not completed initialization, perform initialization to obtain the initial value of the gyroscope zero bias and the estimated value of the gravity component, and then jump to Step 1; if it has completed initialization, proceed to Step 3.

[0067] Step 3: Calculate the azimuth and elevation angles of the tag in the UWB coordinate system using the carrier phase data received by the UWB base station array antenna;

[0068] Step 4: Perform pre-integration between adjacent UWB measurement frames using inertial sensor data;

[0069] Step 5: Combine UWB ranging, angle measurement error, and inertial pre-integration error to optimize the carrier navigation information;

[0070] Step 6: Output the navigation information of the carrier.

[0071] In this embodiment, step 1 above can be implemented using the following preferred solution:

[0072] Carrier phase data received by UWB base station array antenna j∈[0,n-1] Let be the carrier phase received by antenna element j, and n be the number of antenna elements.

[0073] In this embodiment, step 2 above can be implemented using the following preferred solution:

[0074] The initialization method is as follows:

[0075] Initially at rest t s At a given moment, using the zero-velocity assumption at rest, the average value of the gyroscope output is calculated as the initial value for the gyroscope's zero bias:

[0076]

[0077] in, This is the initial value for zero bias of the gyroscope. Let Δt be the output value of the gyroscope in the inertial sensor coordinate system at time i, and Δt be the sampling period of the inertial sensor.

[0078] Using the zero-velocity assumption, the mean value of the accelerometer output is calculated as the gravity component in the inertial sensor coordinate system at the initial moment:

[0079]

[0080] in, This represents the estimated value of the gravity component in the inertial sensor coordinate system at the initial moment. Let be the output value of the accelerometer in the inertial sensor coordinate system at time i.

[0081] By rotating the inertial sensor coordinate system at the initial moment... Aligning the rotation matrix with the gravity vector in the local inertial frame, such that the gravity vector in the inertial sensor coordinate system is perpendicular to the local horizontal plane at the initial moment, is denoted as... The initial inertial sensor coordinate system after rotation is used as the navigation coordinate system to complete the initialization.

[0082] In this embodiment, step 3 above can be implemented using the following preferred solution:

[0083] Calculate the unambiguous carrier phase difference between two sets of carriers using the carrier phases received by three antenna elements that are not on the same straight line:

[0084]

[0085] Where m1, m2, and m3 are the numbers of three antenna array elements that are not on the same straight line. These are the carrier phase differences between antenna elements m1 and m2 and between antenna elements m1 and m3 at time k, respectively.

[0086] via carrier phase difference The coordinates and geometric arrangement of the antenna elements in the UWB coordinate system were determined to obtain the azimuth angle of the tag in the UWB coordinate system at time k. and pitch angle

[0087] For example, the antenna elements of a single UWB base station are uniformly arranged in a circle with radius R, and the azimuth angle of the tag in the UWB coordinate system... and pitch angle The solution method is as follows:

[0088]

[0089] in,

[0090]

[0091] Where λ is the signal wavelength. These represent the distances between antenna array elements m1 and m2, and between antenna array elements m1 and m3, respectively.

[0092] In this embodiment, step 4 above can be implemented using the following preferred solution:

[0093] The inertial sensor data collected at time k and Includes accelerometer data from time k-1 to time k. and gyroscope data Where i = 0, 1, 2, ..., (t(k) - t(k-1)) / Δt, t(k) is the sampling time corresponding to time k, and t(k-1) is the sampling time corresponding to time k-1, i.e., the sampling time corresponding to the previous frame UWB measurement. Inertial sensor measurement model:

[0094]

[0095]

[0096] Where, n a and n ω These are the white noises from the accelerometer and gyroscope, respectively. and These are the zero biases of the accelerometer and gyroscope, respectively, and their derivatives are white noise. For the ideal value measured by the accelerometer, For the ideal value measured by the gyroscope, g W This is the gravity vector in the navigation coordinate system. This is the rotation matrix from the navigation coordinate system to the inertial sensor coordinate system at the sampling time.

[0097] The pre-integration between two adjacent inertial frames is:

[0098]

[0099]

[0100]

[0101] in, Pre-integration for position, For the velocity pre-integration, the rotation is represented by the quaternion γ. For rotational pre-integration; initially, and =0, Let R(γ) be a unit quaternion, and let R(γ) denote the transformation of the quaternion into a rotation matrix. This represents quaternion multiplication. Between two adjacent UWB measurement frames, there is a number of inertial sensor data points. Using the above formula, an iterative method is employed to pre-integrate all the inertial sensor data between two adjacent UWB measurement frames, thereby ensuring that the frequency of the UWB measurement data matches the inertial pre-integration frequency, thus obtaining the inertial pre-integration between two adjacent UWB measurement frames. and

[0102] In this embodiment, step 5 above can be implemented using the following preferred solution:

[0103] Establish the optimization variable X:

[0104] X = [x0, x1, ..., x l ]

[0105] in, l is the sequence number of the last frame. These represent the carrier position, velocity, quaternion, accelerometer and gyroscope zero bias in the k-th frame, respectively, with the carrier's body coordinate system coinciding with the inertial sensor coordinate system.

[0106] Establish the optimization function:

[0107]

[0108] in, For inertial pre-integration error, This indicates that the inertial pre-integration error term is the inertial pre-integration error between the (k-1)th frame UWB measurement and the kth frame UWB measurement, where B is the set of all inertial measurements. This represents the covariance of the inertial pre-integration error; These are UWB ranging error, azimuth measurement error, and elevation measurement error, respectively. These represent the ranging error, azimuth measurement error, and elevation angle measurement error of the UWB measurement in the k-th frame, respectively, where U is the set of all UWB measurements. These are the covariances of UWB ranging error, azimuth measurement error, and elevation measurement error, respectively. This indicates that the error term is converted into Mahalanobis distance, where P is the covariance corresponding to the error term.

[0109] The inertial pre-integration error between two adjacent UWB measurement frames is obtained by the difference between the pre-integration prediction value and the pre-integration estimate value:

[0110]

[0111] in, Let be the rotation matrix from the navigation coordinate system to the body coordinate system at time k-1; These represent the positions of the aircraft's coordinate system in the navigation coordinate system at times k and k-1, respectively. The velocities of the body coordinate system in the navigation coordinate system at times k and k-1 are respectively; Δt k The time interval between two adjacent UWB measurement frames; These are the quaternions representing the rotations of the body coordinate system in the navigation coordinate system at time k and time k-1, respectively. The accelerometer zero biases at time k and k-1 in the body coordinate system are respectively; The gyroscope zero bias in the body coordinate system at time k and time k-1 are respectively, [γ]. xyz This represents taking the x, y, and z components of the quaternion γ.

[0112] The ranging error of the k-th frame UWB measurement is obtained by the difference between the distance prediction value of the tag and the UWB base station and the distance measurement value:

[0113]

[0114] in, This represents the position of the tag in the inertial sensor coordinate system. The extrinsic parameters between the UWB coordinate system and the inertial sensor coordinate system at the initial moment were all obtained through calibration. This represents the distance measurement between the tag and the UWB base station at time k.

[0115] The azimuth measurement error of the k-th frame UWB measurement is obtained by the difference between the predicted azimuth value and the measured azimuth value of the tag in the UWB coordinate system:

[0116]

[0117] in,[·] x 、[·] y These represent the x and y coordinates of the three-dimensional position vector, respectively. The azimuth angle measurement of the tag at time k in the UWB coordinate system obtained in step (3) is given.

[0118] The pitch angle measurement error of the k-th frame UWB measurement is obtained by the difference between the predicted pitch angle value and the measured pitch angle value of the tag in the UWB coordinate system:

[0119]

[0120] in,[·] z This indicates taking the z-axis coordinate value of the three-dimensional position vector. The pitch angle measurement of the label at time k in the UWB coordinate system obtained in step (3) is given.

[0121] The Gauss-Newton method is used to iteratively solve the optimization function. The iteration stops when the error converges or the maximum number of iterations is set. For example, the maximum number of iterations is set to 30 to obtain the optimal estimate of the system's optimization variables and obtain the navigation information of the carrier.

[0122] In summary, this invention collects distance data between the UWB base station and the tag at time k, carrier phase data received by the UWB base station array antenna, accelerometer data, and gyroscope data; determines whether the navigation system is initialized; if initialized, it uses the carrier phase data received by the array antenna to calculate the azimuth and elevation angles of the tag in the UWB coordinate system, and uses inertial sensor data to perform pre-integration between adjacent UWB measurement frames; it combines UWB ranging, angle measurement errors, and inertial pre-integration errors to optimize the carrier navigation information; and outputs the carrier's navigation information. This invention overcomes the shortcomings of multi-base station UWB / inertial fusion methods in terms of base station installation and deployment, time synchronization, etc., and improves navigation accuracy by performing global optimization of the carrier pose through tight coupling.

[0123] The above embodiments are merely illustrative of the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solutions based on the technical concept proposed in this invention shall fall within the scope of protection of this invention.

Claims

1. A single-base station UWB / inertial fusion navigation method based on an antenna array, characterized in that, Includes the following steps: (1) Collect distance data between UWB base station and tag at time k. Carrier phase data received by the UWB base station array antenna accelerometer data and gyroscope data ;in, Let j be the carrier phase received by the antenna element numbered j, and n be the number of antenna elements. (2) Determine whether the navigation system has completed initialization. If it has not completed initialization, perform initialization to obtain the initial value of the gyroscope zero bias and the estimated value of the gravity component, and jump to step (1); if it has completed initialization, proceed to step (3). (3) Using the carrier phase data received by the UWB base station array antenna, calculate the azimuth and elevation angle information of the tag in the UWB coordinate system; (4) Use inertial sensor data to perform pre-integration between adjacent UWB measurement frames; (5) The carrier navigation information is optimized by combining UWB ranging, angle measurement error, and inertial pre-integration error; the specific method is as follows: Establish optimization variable X: in, , where l is the sequence number of the last frame. , , , , These represent the carrier position, velocity, quaternion, accelerometer and gyroscope zero bias in the k-th frame, respectively, with the carrier's body coordinate system coinciding with the inertial sensor coordinate system; Establish the optimization function: in, For inertial pre-integration error, This indicates that the inertial pre-integration error term is the inertial pre-integration error between the (k-1)th frame UWB measurement and the kth frame UWB measurement, where B is the set of all inertial measurements. This represents the covariance of the inertial pre-integration error; , , These are UWB ranging error, azimuth measurement error, and elevation measurement error, respectively. , , These represent the ranging error, azimuth measurement error, and elevation angle measurement error of the UWB measurement in the k-th frame, respectively, where U is the set of all UWB measurements. , , These are the covariances of UWB ranging error, azimuth measurement error, and elevation measurement error, respectively. This indicates that the error term is converted to Mahalanobis distance. This is the covariance corresponding to the error term; The inertial pre-integration error between two adjacent UWB measurement frames is obtained by the difference between the pre-integration prediction value and the pre-integration estimate value: in, Let be the rotation matrix from the navigation coordinate system to the body coordinate system at time k-1; , These represent the positions of the aircraft's coordinate system in the navigation coordinate system at times k and k-1, respectively. , These represent the velocities of the body coordinate system in the navigation coordinate system at times k and k-1, respectively. The time interval between two adjacent UWB measurement frames; , These are the quaternions representing the rotations of the body coordinate system in the navigation coordinate system at time k and time k-1, respectively. , The accelerometer zero biases in the body coordinate system at time k and k-1 are respectively; , The gyroscope zero biases at time k and time k-1 in the body coordinate system are respectively. Indicates taking quaternions The x, y, and z components; The ranging error of the k-th frame UWB measurement is obtained by the difference between the distance prediction value of the tag and the UWB base station and the distance measurement value: in, This represents the position of the tag in the inertial sensor coordinate system. The extrinsic parameters between the UWB coordinate system and the inertial sensor coordinate system at the initial moment were all obtained through calibration. This represents the distance measurement between the tag and the UWB base station at time k; The azimuth measurement error of the k-th frame UWB measurement is obtained by the difference between the predicted azimuth value and the measured azimuth value of the tag in the UWB coordinate system: in, , These represent the x and y coordinates of the three-dimensional position vector, respectively. The azimuth angle measurement of the tag at time k in the UWB coordinate system obtained in step (3); The pitch angle measurement error of the k-th frame UWB measurement is obtained by the difference between the predicted pitch angle value and the measured pitch angle value of the tag in the UWB coordinate system: in, This indicates taking the z-axis coordinate value of the three-dimensional position vector. The pitch angle measurement value of the label in the UWB coordinate system at time k obtained in step (3); The Gauss-Newton method is used to iteratively solve the optimization function. The iteration stops when the error converges or the maximum number of iterations is set, and the optimal estimate of the system optimization variables is obtained to obtain the navigation information of the carrier. (6) Output the navigation information of the carrier.

2. The single-base station UWB / inertial fusion navigation method based on antenna array according to claim 1, characterized in that, The initialization method in step (2) is as follows: Initially at rest At a given moment, using the zero-velocity assumption at rest, the average value of the gyroscope output is calculated as the initial value for the gyroscope's zero bias: in, This is the initial value for zero bias of the gyroscope. Let be the output value of the gyroscope at time i in the inertial sensor coordinate system. This is the sampling period of the inertial sensor; Using the zero-velocity assumption, the mean value of the accelerometer output is calculated as the gravity component in the inertial sensor coordinate system at the initial moment: in, This represents the estimated value of the gravity component in the inertial sensor coordinate system at the initial moment. Let be the output value of the accelerometer in the inertial sensor coordinate system at time i; By rotating the inertial sensor coordinate system at the initial moment... Align the rotation with the gravity vector in the local inertial frame, such that the gravity vector in the inertial sensor coordinate system is perpendicular to the local horizontal plane at the initial moment. Let the rotation matrix be denoted as... The initial inertial sensor coordinate system after rotation is used as the navigation coordinate system to complete the initialization.

3. The single-base station UWB / inertial fusion navigation method based on antenna array according to claim 2, characterized in that, The specific method for step (3) is as follows: Calculate the unambiguous carrier phase difference between two sets of carriers using the carrier phases received by three antenna elements that are not on the same straight line: in, , , Number the three antenna array elements that are not on the same straight line. , Antenna elements at time k , Between and antenna elements , The carrier phase difference between them; via carrier phase difference , The coordinates and geometric arrangement of the antenna array elements in the UWB coordinate system are calculated to determine the azimuth angle of the tag in the UWB coordinate system at time k. and pitch angle .

4. The single-base station UWB / inertial fusion navigation method based on antenna array according to claim 3, characterized in that, The specific method of step (4) is as follows: Inertial sensor data acquired at time k and Includes accelerometer data from time k-1 to time k. and gyroscope data ,in , Let k be the sampling time. Let k-1 be the sampling time corresponding to time k-1, i.e., the sampling time corresponding to the previous frame UWB measurement; the inertial sensor measurement model is: in, and These are the white noises from the accelerometer and gyroscope, respectively. and These are the zero biases of the accelerometer and gyroscope, respectively, and their derivatives are white noise. For the ideal value measured by the accelerometer, This represents the ideal value measured by the gyroscope. This is the gravity vector in the navigation coordinate system. , This is the rotation matrix from the navigation coordinate system to the inertial sensor coordinate system at the sampling time; The pre-integration between two adjacent inertial frames is: in, Pre-integration for position, Pre-integration for velocity, rotation using quaternions express, For rotational pre-integration; initially, and =0, For unit quaternions, This represents the transformation of a quaternion into a rotation matrix. Represents the multiplication operation of quaternions; Between two adjacent UWB measurement frames, there are several inertial sensor data points. Using the above formula, an iterative method is employed to pre-integrate all the inertial sensor data between two adjacent UWB measurement frames, thereby ensuring that the frequency of the UWB measurement data is consistent with the inertial pre-integration frequency, thus obtaining the inertial pre-integration between two adjacent UWB measurement frames. , and .

Images