CN-116164738-B - Single-base-station UWB/inertial fusion navigation method based on antenna array

CN116164738BCN 116164738 BCN116164738 BCN 116164738BCN-116164738-B

Abstract

The invention discloses a single-base-station UWB/inertia fusion navigation method based on an antenna array, and belongs to the field of integrated navigation. The method comprises the steps of collecting distance data between a UWB base station and a tag at the moment k, carrier phase data received by an array antenna of the UWB base station, accelerometer data and gyroscope data, judging whether a navigation system is initialized, if so, calculating azimuth angle and pitch angle information of the tag under a UWB coordinate system by using the carrier phase data received by the array antenna, pre-integrating adjacent UWB measurement frames by using inertial sensor data, optimally solving carrier navigation information by combining UWB ranging, angle measuring errors and inertial pre-integrating errors, and outputting the navigation information of a carrier. The invention overcomes the defects of a multi-base station UWB/inertia fusion method in the aspects of base station installation and deployment, time synchronization and the like, and carries out global optimization solution on the carrier pose in a tight coupling mode, thereby improving the navigation precision.

Inventors

YANG ZIHAN
CHEN CHONG
HUANG LU
WEI BAOGUO
CHENG JIANQIANG
LI YANING
JIA HAONAN
LI SHUANG
ZHANG HENG
LIANG XIAOHU

Assignees

中国电子科技集团公司第五十四研究所

Dates

Publication Date: 20260505
Application Date: 20230215

Claims (4)

1. The single-base station UWB/inertial fusion navigation method based on the antenna array is characterized by comprising the following steps of: (1) Collecting distance data between UWB base station and tag at k moment Carrier phase data received by UWB base station array antenna Acceleration data Gyroscope data Wherein, the method comprises the steps of, The carrier phase is received by the antenna array element with the number j, and n is the number of the antenna array elements; (2) Judging whether the navigation system is initialized, if not, initializing to obtain an initial value of zero offset of the gyroscope and an estimated value of a gravity component, and jumping to the step (1); (3) The azimuth angle and pitch angle information of the tag under the UWB coordinate system are calculated by utilizing carrier phase data received by the UWB base station array antenna; (4) Pre-integrating between adjacent UWB measurement frames by utilizing inertial sensor data; (5) The carrier navigation information is optimally solved by combining UWB ranging, angle measuring error and inertial pre-integration error, and the specific mode is as follows: establishing an optimization variable X: Wherein, the L is the sequence number of the last frame, 、、、、 The carrier position, speed, quaternion, zero offset of the accelerometer and the gyroscope of the kth frame are respectively represented, and the body coordinate system of the carrier is overlapped with the inertial sensor coordinate system; Establishing an optimization function: Wherein, the For the inertial pre-integration error, Indicating that the inertial pre-integration error term is the inertial pre-integration error between the kth-1 frame UWB measurement and the kth frame UWB measurement, B is the set of all inertial measurements, Covariance of inertial pre-integral error; 、、 UWB ranging error, azimuth angle measurement error and pitch angle measurement error, 、、 Indicating that the UWB error term is the ranging error, azimuth angle measurement error, and elevation angle measurement error of the kth frame UWB measurement, respectively, U is the set of all UWB measurements, 、、 Covariance of UWB ranging error, covariance of azimuth angle measurement error, and covariance of pitch angle measurement error, respectively; Representing the conversion of the error term into a mahalanobis distance, Covariance corresponding to the error term; the inertial pre-integration error between two adjacent UWB measurement frames is derived from the difference between the pre-integration prediction value and the pre-integration estimate value: Wherein, the A rotation matrix from a coordinate system to a machine body coordinate system is navigated at the moment k-1; 、 the positions of the machine body coordinate system under the navigation coordinate system at the k moment and the k-1 moment are respectively; 、 the speeds of the machine body coordinate system under the navigation coordinate system at the k moment and the k-1 moment are respectively; measuring a time interval between frames for two adjacent UWB; 、 The quaternion of the rotation of the machine body coordinate system under the navigation coordinate system at the moment k and the moment k-1 respectively; 、 The accelerometer zero offset is respectively set at the moment k and the moment k-1 under the machine body coordinate system; 、 zero offset of the gyroscope under the machine body coordinate system at the moment k and the moment k-1 respectively, Representation of taking quaternions X, y, z components of (c); The ranging error of the kth frame UWB measurement is derived from the difference between the distance prediction value and the distance measurement value of the tag and the UWB base station: Wherein, the For the position of the tag in the inertial sensor coordinate system, Is obtained by calibration for external parameters between the UWB coordinate system and the inertial sensor coordinate system at the initial moment, Representing a distance measurement between the tag at time k and the UWB base station; The azimuth measurement error of the kth frame UWB measurement is obtained by the difference between the azimuth predicted value and the azimuth measured value of the tag in the UWB coordinate system: Wherein, the 、 Respectively representing the coordinate values of x and y axes of the three-dimensional position vector, The azimuth angle measured value of the k-moment tag obtained in the step (3) under the UWB coordinate system; The pitch angle measurement error of the k frame UWB measurement is obtained by the difference between the pitch angle predicted value and the pitch angle measured value of the tag under the UWB coordinate system: Wherein, the Representing the z-axis coordinate values taking the three-dimensional position vector, The pitch angle measurement value of the k-moment tag obtained in the step (3) under the UWB coordinate system; carrying out iterative solution on the optimization function by using a Gaussian-Newton method, stopping iteration when the error convergence or the set maximum iteration number is reached, obtaining an optimal estimated value of a system optimization variable, and obtaining navigation information of a carrier; (6) And outputting navigation information of the carrier.
2. The antenna array-based single-base station UWB/inertial fusion navigation method of claim 1 wherein the method of initialization in step (2) is as follows: At the beginning rest At moment, calculating the average value of the gyroscope output as the initial value of zero offset of the gyroscope by using the zero speed assumption at rest: Wherein, the Is the initial value of zero offset of the gyroscope, The output value of the gyroscope at the moment i under the inertial sensor coordinate system, Sampling period of the inertial sensor; Using the zero-speed assumption, calculating the average value of accelerometer output as the gravity component in the inertial sensor coordinate system at the initial moment: Wherein, the For the estimated value of the gravity component in the inertial sensor coordinate system at the initial moment, The output value of the accelerometer at the moment i under the inertial sensor coordinate system; by rotating the initial moment inertial sensor coordinate system Aligned with the gravity vector under the local inertial system so that the gravity vector under the inertial sensor coordinate system at the initial moment is perpendicular to the local horizontal plane, and the rotation matrix is recorded as And taking the rotated initial moment inertial sensor coordinate system as a navigation coordinate system to finish initialization.
3. The single-base station UWB/inertial fusion navigation method of claim 2 wherein the specific manner of step (3) is as follows: calculating two groups of non-fuzzy carrier phase differences by using carrier phases received by three antenna array elements which are not on the same straight line: Wherein, the 、、 Three antenna elements that are not on the same line are numbered, 、 Respectively k moment antenna array elements 、 Between and antenna array elements 、 Carrier phase difference between them; by carrier phase difference 、 Solving the azimuth angle of the k-moment tag under the UWB coordinate system by the coordinate and geometric arrangement of the antenna array element under the UWB coordinate system And pitch angle 。
4. The single-base station UWB/inertial fusion navigation method based on the antenna array according to claim 3, wherein the specific mode of the step (4) is as follows: Inertial sensor data acquired at time k And Including accelerometer data from time k-1 to time k And gyroscope data Wherein , For the sampling time corresponding to the moment k, Sampling time corresponding to k-1 time, namely sampling time corresponding to UWB measurement of the last frame; the inertial sensor measurement model is: Wherein, the And White noise of the accelerometer and gyroscope respectively, And Zero bias of accelerometer and gyroscope, respectively, its derivative is white noise, For an ideal value to be measured by the accelerometer, For the ideal value to be measured by the gyroscope, For the gravity vector in the navigation coordinate system, , A rotation matrix from a sampling time navigation coordinate system to an inertial sensor coordinate system; The pre-integration between two adjacent inertial frames is: Wherein, the For the pre-integration of the position, Quaternion for rotation for speed pre-integration The representation is made of a combination of a first and a second color, For the pre-integration of the rotation, initially, And Is set to be 0, the number of the components is set to be 0, Is a unit quaternion number, Representing the conversion of the quaternion into a rotation matrix, Multiplication representing a quaternion; A plurality of inertial sensor data are arranged between two adjacent UWB measurement frames, all the inertial sensor data between the two adjacent UWB measurement frames are pre-integrated in an iterative mode through the above formula, so that the frequency of the UWB measurement data is consistent with the inertial pre-integration frequency, and the inertial pre-integration between the two adjacent UWB measurement frames is obtained 、 And 。

Description

Single-base-station UWB/inertial fusion navigation method based on antenna array Technical Field The invention belongs to the field of integrated navigation, and particularly relates to a single-base-station UWB/inertial fusion navigation method based on an antenna array. Background At present, a global navigation satellite system can provide accurate positioning information in an outdoor open environment, but in indoor, underground and other environments, a GNSS cannot provide continuous and reliable navigation information. Therefore, positioning navigation technology under the condition that satellites are not available becomes a research hotspot. The inertial sensor has high output frequency, high autonomy and high anti-interference capability, but the inertial navigation system has rapid pose divergence after long-time operation, no accumulated error exists in the UWB positioning technology, and the UWB has the advantages of low transmitting power, high transmission rate, high penetrating capability and the like, so that the accurate and reliable navigation information estimation can be realized by fusing the UWB and the inertial sensor information, and the inertial sensor information has wide development prospect. Most of the existing UWB/inertial fusion navigation methods are based on a multi-base station positioning system, positioning is performed through a method of arrival time and arrival time difference, and fusion is performed with inertial sensor information based on a Kalman filtering technology. The UWB positioning system based on multiple base stations is complex in installation and deployment, high in cost and requires strict clock synchronization, and the fusion method based on the Kalman filtering technology only estimates the navigation information at the current moment and is not a globally optimal solution, so that the accuracy of the integrated navigation system is limited. Disclosure of Invention In order to solve the technical problems mentioned in the background art, the invention provides a single-base-station UWB/inertia fusion navigation method based on an antenna array, which overcomes the defects of a multi-base-station UWB positioning system through a single-base-station positioning technology, and realizes the tight coupling of UWB/inertia information under a graph optimization framework by utilizing an inertia pre-integration technology to obtain globally optimal navigation information. In order to achieve the above purpose, the technical scheme adopted by the invention is as follows: a single-base station UWB/inertial fusion navigation method based on an antenna array comprises the following steps: (1) Collecting distance data between UWB base station and tag at k moment Carrier phase data received by UWB base station array antennaJ E [0, n-1], accelerometer dataGyroscope dataWherein, the The carrier phase is received by the antenna array element with the number j, and n is the number of the antenna array elements; (2) Judging whether the navigation system is initialized, if not, initializing to obtain an initial value of zero offset of the gyroscope and an estimated value of a gravity component, and jumping to the step (1); (3) The azimuth angle and pitch angle information of the tag under the UWB coordinate system are calculated by utilizing carrier phase data received by the UWB base station array antenna; (4) Pre-integrating between adjacent UWB measurement frames by utilizing inertial sensor data; (5) Carrying out optimization solution on the navigation information of the carrier by combining UWB ranging, angle measuring error and inertial pre-integration error; (6) And outputting navigation information of the carrier. Further, the method of initialization in step (2) is as follows: at the beginning of the rest time t s, calculating the average value of the output of the gyroscope as the initial value of zero offset of the gyroscope by using the zero speed assumption at the rest time: Wherein, the Is the initial value of zero offset of the gyroscope,The output value of the gyroscope at the moment i under the coordinate system of the inertial sensor is given, and deltat is the sampling period of the inertial sensor; Using the zero-speed assumption, calculating the average value of accelerometer output as the gravity component in the inertial sensor coordinate system at the initial moment: Wherein, the For the estimated value of the gravity component in the inertial sensor coordinate system at the initial moment,The output value of the accelerometer at the moment i under the inertial sensor coordinate system; by rotating the initial moment inertial sensor coordinate system Aligned with the gravity vector in the local inertial system so that the gravity vector in the inertial sensor coordinate system at the initial moment is perpendicular to the local horizontal plane, the rotation matrix is recorded asAnd taking the rotated initial moment inertial sensor coordinate s