CN-122015910-A - High-precision positioning data compensation method independent of vehicle interface

CN122015910ACN 122015910 ACN122015910 ACN 122015910ACN-122015910-A

Abstract

The invention relates to the technical field of automatic driving real vehicle testing, and discloses a high-precision positioning data compensation method independent of a vehicle interface, which comprises the steps of reference calibration, data synchronous acquisition and data fusion compensation, wherein the coordinate systems and installation parameters of a UWB base station and an INS are unified firstly; and finally, through the sub-steps of prediction, updating, estimation and correction, two types of data are fused and errors are corrected by a loose combination Kalman filtering algorithm. According to the invention, an indoor absolute reference system is constructed through UWB without depending on a vehicle interface, error accumulation of INS caused by IMU zero offset is restrained, centimeter-level high-precision positioning data is output in real time, the method is suitable for GNSS signal-free scenes such as underground garages, and the like, and the reference data requirement of automatic driving algorithm test is met.

Inventors

ZHANG HONGMEI
CHEN HAO
ZHANG YUNFEI
CHENG QIAN
LI ZHIYUN
HU RUI
OUYANG JINGFENG
LI BIN
He Manchuan
HUANG MINGYU

Assignees

中国汽车工程研究院股份有限公司
中汽院(江苏)汽车工程研究院有限公司
中汽院智能网联科技有限公司

Dates

Publication Date: 20260512
Application Date: 20260130

Claims (10)

1. A method of high accuracy positioning data compensation independent of a vehicle interface, comprising: Measuring physical coordinates of a plurality of UWB base stations deployed around a test site in a static state of all equipment, and unifying the physical coordinates of all UWB base stations to a fixed world coordinate system; A step of synchronously acquiring data, in which IMU original data and self-resolved positioning data output by an INS and real-time distance data between UWB labels arranged on the test vehicle and UWB base stations are synchronously acquired in the running process of the test vehicle, and a main control processing unit aligns the INS data with time stamps of the UWB distance data through a precise time protocol; A data fusion compensation step, wherein a main control processing unit carries out fusion processing on the synchronized INS data and UWB distance data based on a loose combination Kalman filtering algorithm, outputs positioning data of a multidimensional state vector, and comprises 3D position, speed, gesture, sensor deviation estimation and positioning quality index, and the data fusion compensation step comprises a prediction sub-step, an updating sub-step and an estimation and correction sub-step; A prediction sub-step of constructing a nominal state kinematic model through a physical kinematic formula based on IMU data output by the INS, and predicting the position, speed and posture of the vehicle at the next moment through the nominal state kinematic model to obtain a predicted value; a step of updating, in which the current absolute position of the vehicle is calculated as an observed value based on the real-time distance data of the UWB base station, and the observed value is compared with a predicted value to obtain an observed deviation; The method comprises the sub-steps of estimating and correcting, constructing an error state dynamics model under continuous time to quantify the change rule of the error state caused by observation deviation along with time, executing discrete time prediction to obtain the distribution range of the error state, wherein the discrete time prediction comprises nominal state prediction and error state covariance prediction, constructing a loose combination observation model between the error state and the observation deviation, calculating Kalman gain to determine the weight of an observation value and a predicted value, feeding the estimated error state back to the nominal state kinematics model, correcting the next prediction, resetting the error state, entering the next cycle, and outputting the corrected nominal state as the optimal navigation solution at the current moment.
2. The method for compensating high-precision positioning data independent of a vehicle interface according to claim 1, wherein the installation position offset is X, Y, Z three-dimensional offset of an INS antenna center point relative to a vehicle rear axle center point, the installation attitude angle comprises a course angle, a pitch angle and a roll angle, and the fixed world coordinate system is a North east coordinate system.
3. The method of claim 1, wherein the IMU raw data comprises acceleration data along X, Y, Z axis and angular velocity data about X, Y, Z axis, and the precision time protocol is PTP protocol.
4. The method of claim 1, wherein the nominal state of the nominal state kinematic model includes a position, a velocity, a posture quaternion, and a sensor zero offset, and the error state vector is 15-dimensional, and the error state vector includes a position error, a velocity error, a posture error angle, a gyroscope zero offset error, and an accelerometer zero offset error.
5. The method for compensating high-precision positioning data independent of a vehicle interface as set forth in claim 4, wherein the updating of the nominal state kinematic model comprises attitude updating, speed updating and position updating, the attitude updating is in a quaternion form, for a Kth IMU period, the speed updating is achieved through rotation matrix and gravity vector calculation from a carrier coordinate system to a navigation coordinate system based on the compensated angular increment and index mapping from a rotation vector to a quaternion, and the position updating is achieved through an integration method or a median integration method.
6. The vehicle interface independent high accuracy positioning data compensation method of claim 1, wherein the error state dynamics model is: wherein F is a state transition matrix, As an error term caused by the force-opposing-symmetry matrix, , Representing an antisymmetric matrix of vectors, Is zero offset correlation time constant, G is noise driving matrix, Is a system noise vector.
7. The method for compensating high-precision positioning data independent of a vehicle interface as set forth in claim 1, wherein the observation vector of the loosely-combined observation model is a difference between an absolute position calculated by UWB and a position calculated by INS, and the observation equation is: Wherein H is an observation matrix, H directly extracts the position and speed parts in the error state, Is a 15-dimensional error state, r is the observed noise of UWB.
8. The method for compensating high-precision positioning data independent of a vehicle interface as set forth in claim 1, wherein the Kalman gain is calculated in Joseph form by the following formula: Wherein, the For the state estimation covariance matrix before the measurement update at time K +1, Is the identity matrix of the same dimension as the state vector, As the product of the kalman gain and the observation matrix, To observe the propagation of noise in the state space.
9. The method for compensating high-precision positioning data independent of a vehicle interface as recited in claim 8, wherein the feedback correction comprises position correction, speed correction, attitude correction and sensor zero offset correction, and the attitude correction comprises the specific process of converting an attitude error angle into an error quaternion, multiplying the error quaternion with an attitude quaternion in a nominal state, and then carrying out normalization processing.
10. A method of vehicle interface independent high accuracy positioning data compensation as set forth in claim 4 wherein said resetting the error state comprises resetting all elements of a 15-dimensional error state vector to zero and resetting the error state covariance matrix to an initial value.

Description

High-precision positioning data compensation method independent of vehicle interface Technical Field The invention relates to the technical field of automatic driving real vehicle testing, in particular to a high-precision positioning data compensation method independent of a vehicle interface. Background In the development process of the automatic driving technology, the real vehicle test is a key link for verifying the performance advantages and disadvantages of a core algorithm (such as memory parking, automatic obstacle avoidance and the like) and guaranteeing the running safety of the vehicle. In this link, a high-precision Inertial Navigation System (INS) is an indispensable device, which can continuously output real-time position, speed and attitude (heading, pitch and roll) data of a vehicle, and these data are used as reference true values for evaluating the performance of a tested system, and are important bases for judging whether an algorithm meets design requirements. In an outdoor open environment, the INS may implement absolute position calibration by receiving signals of a Global Navigation Satellite System (GNSS). The GNSS can provide accurate absolute position information such as longitude, latitude, altitude and the like, can regularly correct the calculation error of the INS, ensures that the INS keeps centimeter-level high-precision positioning output for a long time, and meets the strict requirement of real vehicle testing on reference data. However, when the test scene is transferred from the outdoor to the closed or semi-closed indoor environments such as underground parking lots and tunnels, the GNSS signals are completely shielded by the building structures such as walls and ceilings, so that the INS cannot acquire the GNSS calibration signals, and at this time, the INS can only switch to the dead reckoning (Dead Reckoning) mode and rely on the Inertial Measurement Unit (IMU) integrated therein for data calculation. The IMU serves as a core sensing component of the INS, and has inherent small errors (i.e., zero offset), and even in a static state of the device, the gyroscope and the accelerometer of the IMU still output weak false angular velocity and acceleration signals. Because the positioning calculation of the INS is based on integral operation of the IMU original data, the tiny zero offset error can be continuously accumulated in the integral process, and is amplified like a snowball, and the positioning error of the INS can be rapidly deteriorated from the original centimeter level to the meter level within a few minutes, so that the output reference data is lost in effectiveness, and the accurate evaluation of the performance of an automatic driving algorithm is seriously influenced. In addition, many test vehicles do not open critical data interfaces such as CAN buses to external test equipment for dual consideration of safety protection (avoiding interface opening causing interference of the vehicle control system by external equipment) and technical confidentiality (preventing leakage of vehicle core parameters and operating data). The limitation causes that a tester cannot carry out auxiliary correction on the accumulated errors of the INS by acquiring real-time operation data such as the wheel speed and the steering wheel angle of the vehicle, further aggravates the problem that the INS positioning accuracy is rapidly reduced in an indoor environment, and brings significant obstruction to the real-vehicle test work of the automatic driving technology. Disclosure of Invention The invention aims to provide a high-precision positioning data compensation method independent of a vehicle interface, so as to solve the technical problems that in an indoor GNSS signal-free environment, INS accumulates positioning errors due to zero offset of an IMU and data correction cannot be obtained through the vehicle interface. In order to achieve the above purpose, the invention adopts the following technical scheme: a method of high accuracy positioning data compensation independent of a vehicle interface, comprising: Measuring physical coordinates of a plurality of UWB base stations deployed around a test site in a static state of all equipment, and unifying the physical coordinates of all UWB base stations to a fixed world coordinate system; A step of synchronously acquiring data, in which IMU original data and self-resolved positioning data output by an INS and real-time distance data between UWB labels arranged on the test vehicle and UWB base stations are synchronously acquired in the running process of the test vehicle, and a main control processing unit aligns the INS data with time stamps of the UWB distance data through a precise time protocol; A data fusion compensation step, wherein a main control processing unit carries out fusion processing on the synchronized INS data and UWB distance data based on a loose combination Kalman filtering algorithm, outputs positioning data of