High-precision positioning data compensation method independent of vehicle interface

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 benchmark calibration, data synchronous acquisition and data fusion compensation: unifying coordinate systems and installation parameters of a UWB base station and an INS (Inertial Navigation System); then IMU original data, positioning data and UWB distance data of the INS are synchronously collected, and timestamps are aligned; and finally, through a loose combination Kalman filtering algorithm, through sub-steps of prediction, updating, estimation and correction, fusing the two types of data and correcting errors. The method does not need to depend on a vehicle interface, an indoor absolute reference system is constructed through UWB, error accumulation caused by IMU zero offset of an INS 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 reference data requirement of automatic driving algorithm testing is met.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving vehicle testing technology, specifically to a high-precision positioning data compensation method independent of the vehicle interface. Background Technology

[0002] In the research and development of autonomous driving technology, real-vehicle testing is a crucial step in verifying the performance of core algorithms (such as memory parking and automatic obstacle avoidance) and ensuring vehicle driving safety. In this process, a high-precision inertial navigation system (INS) is indispensable. It continuously outputs real-time position, speed, and attitude (heading, pitch, roll) data of the vehicle. This data serves as the baseline for evaluating the performance of the tested system and is an important basis for determining whether the algorithm meets design requirements.

[0003] In open outdoor environments, the INS can achieve absolute position calibration by receiving signals from the Global Navigation Satellite System (GNSS). GNSS provides accurate absolute position information such as latitude, longitude, and altitude, which can periodically correct the INS's calculation errors, ensuring that the INS maintains centimeter-level high-precision positioning output over a long period of time, meeting the stringent requirements of real vehicle testing for reference data.

[0004] However, when the test scenario shifts from outdoor to enclosed or semi-enclosed indoor environments such as underground parking lots and tunnels, GNSS signals are completely blocked by building structures such as walls and ceilings. This prevents the INS from acquiring GNSS calibration signals, forcing the INS to switch to pure dead reckoning mode and rely solely on its integrated inertial measurement unit (IMU) for data processing. As the core sensing component of the INS, the IMU inherently possesses a small error (i.e., zero bias). Even when the device is stationary, the IMU's gyroscope and accelerometer will still output weak, spurious angular velocity and acceleration signals. Since the INS' positioning calculation is based on the integration of the raw IMU data, this small zero bias error accumulates continuously during integration, amplifying like a snowball. Within minutes, the INS' positioning error rapidly deteriorates from centimeter-level to meter-level, rendering the output reference data ineffective and severely impacting the accurate evaluation of the autonomous driving algorithm's performance.

[0005] Furthermore, many test vehicles, for both safety reasons (to avoid interference with the vehicle control system from external devices due to open interfaces) and technical confidentiality (to prevent the leakage of core vehicle parameters and operational data), do not expose critical data interfaces such as the CAN bus to external testing equipment. This restriction prevents testers from using real-time operational data such as wheel speed and steering wheel angle to assist in correcting the accumulated errors of the INS (Instrument System), further exacerbating the problem of rapid decline in INS positioning accuracy in indoor environments and significantly hindering the real-vehicle testing of autonomous driving technology. Summary of the Invention

[0006] The present invention aims to provide a high-precision positioning data compensation method independent of the vehicle interface, in order to solve the technical problem that the positioning error of the INS is accumulated due to the zero bias of the IMU in indoor environments without GNSS signal, and the data cannot be obtained for correction through the vehicle interface.

[0007] To achieve the above objectives, the present invention adopts the following technical solution: A high-precision positioning data compensation method independent of the vehicle interface includes: The benchmark calibration procedure involves measuring the physical coordinates of several UWB base stations deployed around the test site while all equipment is stationary, and unifying the physical coordinates of all UWB base stations to a fixed world coordinate system. At the same time, the offset of the antenna center point of the INS installed on the test vehicle from the installation position on the vehicle and the installation attitude angle are measured. The data synchronization acquisition steps involve simultaneously acquiring the raw IMU data output by the INS and the positioning data calculated by the vehicle itself during the test vehicle's operation, as well as the real-time distance data between the UWB tag set on the test vehicle and each UWB base station. The main control processing unit aligns the timestamps of the INS data and the UWB distance data through a precise time protocol. In the data fusion compensation step, the main control processing unit performs fusion processing on the synchronized INS data and UWB distance data based on the loose combination Kalman filter algorithm, and outputs a multi-dimensional state vector positioning data, including 3D position, velocity, attitude, sensor bias estimation and positioning quality index. The data fusion compensation step includes a prediction sub-step, an update sub-step and an estimation and correction sub-step. The prediction sub-step involves constructing a nominal state kinematic model based on the IMU data output by the INS using physical kinematic formulas. The nominal state kinematic model is then used to predict the vehicle's position, velocity, and attitude at the next moment, thus obtaining the predicted values. The update sub-step calculates the vehicle's current absolute position based on the real-time distance data from the UWB base station as the observation value, and compares the observation value with the predicted value to obtain the observation deviation. The estimation and correction sub-steps construct a continuous-time error state dynamic model to quantify the change in observation bias caused by the error state over time; perform discrete-time prediction to obtain the distribution range of the error state, which includes nominal state prediction and error state covariance prediction; construct a loose combination observation model between the error state and the observation bias; calculate the Kalman gain to determine the weights of the observed and predicted values; feed the estimated error state back into the nominal state kinematic model to correct the next prediction, while resetting the error state and entering the next loop; output the corrected nominal state as the optimal navigation solution for the current moment.

[0008] The principles and advantages of this solution are as follows: In practical applications, it does not rely on any vehicle data interface (such as the CAN bus), thus avoiding the limitation of test vehicles not having open interfaces due to security and confidentiality requirements, and has strong versatility; an indoor and outdoor absolute reference system is constructed through a UWB base station to replace the failed GNSS signal and provide a continuous calibration source for the INS; the IMU zero bias error is estimated and suppressed in real time through a loosely combined Kalman filter algorithm, reducing the positioning error from the meter level to the centimeter level; all compensation processes are completed in real time without post-processing, and high-quality reference data can be obtained immediately to meet the needs of autonomous driving real vehicle testing; UWB base station technology is mature and flexible in deployment, and its cost is lower than that of high-end INS equipment, achieving a balance between cost and accuracy.

[0009] Preferably, as an improvement, the installation position offset is the three-dimensional offset of the INS antenna center point relative to the vehicle rear axle center point in X, Y, and Z directions; the installation attitude angle includes the heading angle, pitch angle, and roll angle; and the fixed world coordinate system is the NE-G coordinate system.

[0010] Technical benefits: By clarifying the specific measurement benchmark and coordinate system type of INS installation parameters, we can ensure the consistency of measurement between INS data and UWB data, provide an accurate coordinate transformation basis for subsequent data fusion, and avoid correction errors caused by inconsistent benchmarks.

[0011] Preferably, as an improvement, the IMU raw data includes acceleration data along the X, Y, and Z axes and angular velocity data about the X, Y, and Z axes; the precision time protocol is the PTP protocol.

[0012] Technical benefits: Complete acquisition of vehicle 3D motion state data, providing comprehensive physical parameters for prediction sub-steps; PTP protocol has nanosecond-level time synchronization accuracy, ensuring accurate alignment of INS data and UWB data timestamps, avoiding false deviations due to time differences, and ensuring the accuracy of data fusion.

[0013] Preferably, as an improvement, the nominal state of the nominal state kinematic model includes position, velocity, attitude quaternions and sensor zero bias; the error state vector is 15-dimensional, and the error state vector includes position error, velocity error, attitude error angle, gyroscope zero bias error and accelerometer zero bias error.

[0014] Technical benefits: It comprehensively covers the core parameters of vehicle motion and the inherent error sources of sensors. The 15-dimensional error state vector accurately quantifies various errors, providing a complete mathematical basis for the algorithm to infer the root cause of errors. This ensures that error correction can specifically cover key aspects such as position, speed, attitude and sensor zero bias, avoiding omission of core error sources.

[0015] Preferably, as an improvement, the update of the nominal state kinematic model includes attitude update, velocity update, and position update. The attitude update adopts the form of quaternions and is realized for the Kth IMU cycle based on the compensated angular increment and the exponential mapping from the rotation vector to the quaternion. The velocity update is calculated by the rotation matrix from the carrier coordinate system to the navigation coordinate system and the gravity vector. The position update is realized by the integral method or the median integral method.

[0016] Technical benefits: Quaternion form avoids gimbal lock problem during attitude update and improves attitude calculation accuracy; combined with the update logic of gravity vector and precise coordinate transformation, it ensures that velocity and position calculations conform to the laws of physical motion; median integration method further improves the accuracy of position calculation and provides a more accurate basis for prediction values.

[0017] Preferably, as an improvement, the error state dynamic model is as follows:

[0018]

[0019]

[0020] Where F is the state transition matrix, The error term caused by the antisymmetric matrix is... , Represents the antisymmetric matrix of vectors. Let G be the zero partial correlation time constant, and G be the noise driving matrix. This is the system noise vector.

[0021] Technical benefits: By quantifying the evolution of error states over time, the correlation between sensor zero bias, motion parameter errors, and observation deviations is clarified, providing solid theoretical support for accurately estimating the root causes of errors and ensuring the scientific validity of error back-calculation.

[0022] Preferably, as an improvement, the observation vector of the loose combination observation model is the difference between the absolute position calculated by UWB and the position estimated by INS, and the observation equation is:

[0023] Where H is the observation matrix, and H directly extracts the position and velocity components from the error state. It is a 15-dimensional error state, where r is the observation noise of UWB.

[0024] Technical effects: It establishes a direct mapping relationship between observation bias and error state, accurately filters key error terms from the observation matrix, eliminates irrelevant interference, and quantifies the impact of UWB observation noise on the results, thereby improving the accuracy of error estimation.

[0025] Preferably, as an improvement, the Kalman gain is calculated using the Josephus form, and the calculation formula is as follows:

[0026] in, To estimate the covariance matrix of the state before the measurement update at time K+1, The identity matrix has the same dimension as the state vector. The product of the Kalman gain and the observation matrix. To observe the propagation of noise in the state space.

[0027] Technical benefits: The Josephus form of calculation helps ensure the positive definiteness and numerical stability of the error covariance matrix, avoids numerical divergence caused by calculation iteration, and improves the reliability of error correction.

[0028] Preferably, as an improvement, the feedback correction includes position correction, velocity correction, attitude correction and sensor zero bias correction. The specific process of attitude correction is as follows: convert the attitude error angle into an error quaternion, multiply it with the attitude quaternion of the nominal state, and then perform normalization processing.

[0029] Technical effects: It enables comprehensive correction of core vehicle motion parameters and sensor root cause errors. The quaternion operation of attitude correction takes into account both accuracy and mathematical rationality, ensuring that the corrected position, velocity and attitude data are accurate and continuous, while suppressing the error accumulation caused by IMU zero bias from the root.

[0030] Preferably, as an improvement, resetting the error state includes resetting all elements of the 15-dimensional error state vector to zero and resetting the error state covariance matrix to its initial value.

[0031] Technical effect: Clearing the error cache of the previous cycle avoids the interference of old errors on the prediction of the next cycle, and restarting each cycle based on the accurately corrected state ensures that the algorithm can continuously and stably suppress error accumulation, maintain centimeter-level positioning accuracy in the long term, and meet the long-term validity requirements of benchmark data for autonomous driving real vehicle testing. Attached Figure Description

[0032] Figure 1 This is a flowchart illustrating a high-precision positioning data compensation method independent of the vehicle interface. Detailed Implementation

[0033] The following detailed description illustrates the specific implementation method: The basic implementation examples are as follows: Figure 1 The diagram illustrates a high-precision positioning data compensation method independent of the vehicle interface, comprising a benchmark calibration step, a data synchronization acquisition step, and a data fusion compensation step. The benchmark calibration step ensures that the measurement standards of the two independent systems, INS and UWB, are consistent, preventing subsequent data fusion from failing due to coordinate system / installation position deviations, thus laying the foundation for accuracy. The data synchronization acquisition step simultaneously acquires motion data from the INS and distance data from the UWB, ensuring that the timestamps of the two types of data correspond one-to-one, providing a basis for comparison at the same time point for subsequent fusion. The data fusion compensation step uses the absolute position accuracy of the UWB to correct the drift error of the INS, achieving closed-loop correction through a loosely combined Kalman filter algorithm, and iteratively performing three sub-steps: prediction, update, estimation, and correction.

[0034] The benchmark calibration process involves measuring the physical coordinates of several UWB base stations deployed around the test site while all equipment is stationary. The physical coordinates of all UWB base stations are then standardized to a fixed world coordinate system. All equipment includes INS, UWB base stations, and vehicles. The INS is an OxTS or NovAtel series high-precision inertial navigation device. In this embodiment, a total station is used to measure the physical coordinates of each UWB base station. The physical coordinates include three dimensions: X, Y, and Z. The fixed world coordinate system is such as the NED coordinate system (northeast-east). UWB base station coordinate calibration is equivalent to assigning a unique and precise address to each UWB base station. Subsequently, the absolute position of the tag (i.e., the vehicle) can be deduced from the distances from the tag to multiple base stations.

[0035] Simultaneously, a total station was used to measure the offset of the antenna center point of the INS mounted on the test vehicle and its mounting attitude angles. The offset refers to the three-dimensional offset in X, Y, and Z directions relative to the rear axle center point of the vehicle, such as 1.2 meters in front of the rear axle center point, 0.3 meters to the left, and 1.5 meters in height. The mounting attitude angles include the yaw angle (direction of the vehicle's front), pitch angle (tilt of the vehicle's front up and down), and roll angle (tilt of the vehicle body left and right). The INS measures the motion state of its own mounting point; these parameters facilitate the conversion into the overall motion state of the vehicle, ensuring correspondence with the vehicle position measured by UWB.

[0036] The accuracy of the benchmark calibration step directly determines the upper limit of the entire system's performance. If the base station coordinate measurement error is 10 centimeters, no matter how high the subsequent positioning accuracy is, it cannot exceed 10 centimeters.

[0037] The data synchronization acquisition process begins with the vehicle entering the underground parking garage. Real-time monitoring is conducted during the test vehicle's operation, collecting raw IMU data output by the INS and its own calculated positioning data. The raw IMU data includes acceleration (motion acceleration along the X, Y, and Z axes) and angular velocity (rotational velocity around the X, Y, and Z axes). The INS-calculated positioning data is the position and attitude that the INS itself infers based on the IMU data. At this point, due to GNSS failure, the INS has begun to drift, meaning that the INS-calculated positioning data is its own drifted positioning data, and the accuracy gradually decreases.

[0038] The system synchronously collects real-time distance data between the UWB tags installed on the test vehicle and each UWB base station. The UWB tags on the roof of the vehicle communicate wirelessly with all surrounding UWB base stations in real time, continuously outputting the real-time distance from the tag to each base station (e.g., 5.2 meters to base station 1 and 7.8 meters to base station 2). The distance is calculated based on the time-of-flight (TOF) of the UWB signal with centimeter-level accuracy and is not affected by indoor obstruction.

[0039] The main control processing unit aligns the timestamps of INS data and UWB distance data using a precise time protocol, which is the PTP protocol. By aligning the timestamps of INS data and UWB distance data, it ensures that each INS data point (such as acceleration and angular velocity at time t1) can find the corresponding UWB distance data at time t1, thus avoiding fusion errors caused by time differences.

[0040] In the data fusion compensation step, the main control processing unit performs fusion processing on the synchronized INS data and UWB distance data based on the loose combination Kalman filter algorithm, and outputs a multi-dimensional state vector positioning data, including 3D position, velocity, attitude, sensor bias estimation and positioning quality index; the data fusion compensation step includes a prediction sub-step, an update sub-step and an estimation and correction sub-step.

[0041] The prediction sub-step leverages the high frequency and smoothness advantages of the INS to initially estimate the vehicle's real-time motion state (position, velocity, attitude). Specifically, the main control unit constructs a nominal state kinematic model based on the raw IMU data (acceleration, angular velocity) output by the INS using physical kinematic formulas. The core parameters of the nominal state kinematic model (i.e., nominal state) are... This includes the vehicle's position, velocity, attitude quaternions, and IMU sensor zero bias (gyroscope zero bias, accelerometer zero bias); it also defines a 15-dimensional error state vector. This is used for subsequent error estimation. The error state vector includes position error, velocity error, attitude error angles (heading, pitch, roll), gyroscope bias error, and accelerometer bias error. Error State Vector Represented as:

[0042] in, This represents the positional error in the Northeast Elevation (NED) coordinate system. The velocity error in the NED coordinate system. This is the attitude error angle (misalignment angle), corresponding to the rotation vector. For gyroscope zero bias error, This refers to the zero bias error of the accelerometer.

[0043] nominal state Represented as:

[0044] Where q is the attitude quaternion.

[0045] The vehicle's position, velocity, and attitude at the next moment are calculated using a nominal state kinematic model, i.e., the predicted values. The INS output frequency is high (consistent with the IMU, usually above 100Hz), and the trajectory is smooth, but due to the influence of the IMU's zero bias, the predicted values ​​will gradually drift, resulting in inaccurate prediction results.

[0046] In the time update phase, the nominal state kinematic model uses compensated IMU data for pure integration (without error terms) to achieve nominal state updates. Specifically: Attitude updates are performed using quaternions. For the Kth IMU cycle:

[0047]

[0048]

[0049]

[0050] in, For angular increments, For speed increments, Angular velocity, For acceleration, For attitude quaternions, The angle increment after compensation, For the exponential mapping from rotation vector to quaternion, at the beginning of the k-th IMU sampling period, the gyroscope has zero bias. The current estimate, This represents the cumulative error angle caused by the gyroscope's zero bias within this period.

[0051] Speed ​​updates include:

[0052]

[0053] in, This is the rotation matrix from the vehicle coordinate system (b-frame) to the navigation coordinate system (n-frame). This is the gravity vector.

[0054] Location updates include:

[0055] Position updates can also be achieved using methods such as median integration.

[0056] The update sub-step calculates the vehicle's current absolute position as the observed value based on real-time distance data from UWB base stations. This observed value is then compared with the predicted value to obtain the observation deviation. Specifically, the main control unit uses distance data from the UWB tag to multiple UWB base stations, combined with the calibrated absolute coordinates of the UWB base stations, to calculate the absolute coordinates of the UWB tag—that is, the vehicle's absolute position—through a multi-point ranging and positioning algorithm. This position is an absolute observation independent of the INS (Instrument System), with accuracy determined by UWB (centimeter-level) and unaffected by INS drift. This observed value is then compared with the predicted value to find the difference (e.g., the predicted position is at point A, and the observed position is at point B, with a difference of ΔS).

[0057] The essence of observation bias is the difference between "information actually measured by external sensors" and "information that should be measured based on INS predictions." In loosely coupled systems, although INS predicts the complete state (position, velocity, attitude), UWB only provides observation information for a portion of the state (position-dependent). Observation bias is calculated for observable quantities (position or distance), and it affects the updates of all states (including velocity, attitude, and zero bias) through the observation matrix. This is the essence of Kalman filter fusion: local observations can lead to global corrections.

[0058] The estimation and correction sub-step does not directly replace the predicted values ​​with observed values, as this would lead to data jumps and negate the smoothness advantage of the INS. Instead, it uses a loosely combined Kalman filter algorithm to infer the cause of the predicted value drift, namely the error state of the IMU sensors (such as the zero bias of the accelerometer and gyroscope), and corrects the nominal state kinematic model to ensure more accurate subsequent predictions. Specifically: A continuous-time error state dynamic model is constructed to quantify the variation of the error state (i.e., the difference between the nominal and true states) over time, leading to the observation bias ΔS, such as how IMU zero bias affects position error. Directly applying Kalman filtering to the nominal state is difficult because the nominal state is a free integral and unconstrained, while the error state is always a small quantity that can be linearized, making it suitable for the linear assumptions of Kalman filtering. In Kalman filtering, the error state is directly estimated, not the nominal state, and the observation bias mainly originates from the error state. Under the small error assumption, the linearized error state differential equation is:

[0059]

[0060]

[0061] Where F is the state transition matrix (Jacobi matrix). The error term caused by the antisymmetric matrix is... , Represents the antisymmetric matrix of vectors. G is the zero partial correlation time constant (first-order Gaussian-Markov process model), and G is the noise driving matrix. The system noise vector includes gyro angle random walk, accelerometer velocity random walk, and zero-bias drive noise.

[0062] Discrete-time prediction is performed to obtain the distribution range of the error state, providing a basis for subsequent calculation of the weights of observed and predicted values. Discrete-time prediction includes nominal state prediction and error state covariance prediction. Nominal state prediction is completed through integration of the aforementioned IMU data. Error state covariance prediction includes: (The error state prediction value is always zero)

[0063] in, This is the discrete-time state transition matrix. The noise covariance matrix of the discrete-time process;

[0064]

[0065]

[0066] in, This represents the continuous-time noise intensity.

[0067] A loosely combined observation model is constructed to connect the error state with the observation bias. For example, ΔS = 0.3m may be the result of the combined effects of "position error of 0.1m + attitude error of 0.2m caused by gyroscope zero bias". Specifically, the observation vector is the difference between the absolute position and velocity calculated by UWB and the position and velocity estimated by INS. The observation vector is:

[0068] Under the error state framework, the observation equation is:

[0069] Where H is the observation matrix and H is the filter, which extracts only the error states related to the observation bias (such as position error and velocity error) in the loose combination. This is a 15-dimensional error state, where r is the UWB observation noise. .

[0070]

[0071]

[0072] By establishing a mathematical relationship between deviation and error state, we can infer which error states are at play by looking at the deviation.

[0073] Calculate the Kalman gain to determine the weights of the observations and predictions; use the Josephus form of the Kalman gain to ensure numerical stability. The Kalman gain is:

[0074] in, To estimate the covariance matrix of the state before the measurement update at time K+1, The identity matrix has the same dimension as the state vector. The product of the Kalman gain and the observation matrix. To observe the propagation of noise in the state space.

[0075] The estimated error state (such as IMU bias and position error) is fed back into the nominal state kinematic model to correct the next prediction, while simultaneously resetting the error state and entering the next loop; the position, velocity, and bias feedbacks are as follows:

[0076]

[0077]

[0078]

[0079] Attitude feedback first converts the error angle into an error quaternion:

[0080] Then Finally, the resulting quaternion is normalized.

[0081] Reset error status:

[0082] Output corrected nominal state This is the optimal navigation solution at the current moment.

[0083] Each data cycle involves repeated prediction, updating, and correction, resulting in output data that retains the high smoothness of INS while possessing the centimeter-level accuracy of UWB, thus suppressing error accumulation.

[0084] The above descriptions are merely embodiments of the present invention, and common knowledge such as specific technical solutions and / or characteristics are not described in detail here. It should be noted that those skilled in the art can make various modifications and improvements without departing from the technical solutions of the present invention, and these should also be considered within the scope of protection of the present invention. These modifications and improvements will not affect the effectiveness of the implementation of the present invention or the practicality of the patent. The scope of protection claimed in this application should be determined by the content of its claims, and the specific embodiments described in the specification can be used to interpret the content of the claims.

Claims

1. A high-precision positioning data compensation method independent of the vehicle interface, characterized in that, include: The benchmark calibration procedure involves measuring the physical coordinates of several UWB base stations deployed around the test site while all equipment is stationary, and unifying the physical coordinates of all UWB base stations to a fixed world coordinate system. At the same time, the offset of the antenna center point of the INS installed on the test vehicle from the installation position on the vehicle and the installation attitude angle are measured. The data synchronization acquisition steps involve simultaneously acquiring the raw IMU data output by the INS and the positioning data calculated by the vehicle itself during the test vehicle's operation, as well as the real-time distance data between the UWB tag set on the test vehicle and each UWB base station. The main control processing unit aligns the timestamps of the INS data and the UWB distance data through a precise time protocol. In the data fusion compensation step, the main control processing unit performs fusion processing on the synchronized INS data and UWB distance data based on the loose combination Kalman filter algorithm, and outputs a multi-dimensional state vector positioning data, including 3D position, velocity, attitude, sensor bias estimation and positioning quality index. The data fusion compensation step includes a prediction sub-step, an update sub-step and an estimation and correction sub-step. The prediction sub-step involves constructing a nominal state kinematic model based on the IMU data output by the INS using physical kinematic formulas. The nominal state kinematic model is then used to predict the vehicle's position, velocity, and attitude at the next moment, thus obtaining the predicted values. The update sub-step calculates the vehicle's current absolute position based on the real-time distance data from the UWB base station as the observation value, and compares the observation value with the predicted value to obtain the observation deviation. The estimation and correction sub-steps construct a continuous-time error state dynamic model to quantify the change in observation bias caused by the error state over time; perform discrete-time prediction to obtain the distribution range of the error state, which includes nominal state prediction and error state covariance prediction; construct a loose combination observation model between the error state and the observation bias; calculate the Kalman gain to determine the weights of the observed and predicted values; feed the estimated error state back into the nominal state kinematic model to correct the next prediction, while resetting the error state and entering the next loop; output the corrected nominal state as the optimal navigation solution for the current moment.

2. The high-precision positioning data compensation method independent of the vehicle interface according to claim 1, characterized in that: The installation position offset is the X, Y, and Z three-dimensional offset of the INS antenna center point relative to the vehicle rear axle center point; the installation attitude angle includes the heading angle, pitch angle, and roll angle; the fixed world coordinate system is the NE-G coordinate system.

3. The high-precision positioning data compensation method independent of the vehicle interface according to claim 1, characterized in that: The raw IMU data includes acceleration data along the X, Y, and Z axes and angular velocity data around the X, Y, and Z axes; the precision time protocol is the PTP protocol.

4. The high-precision positioning data compensation method independent of the vehicle interface according to claim 1, characterized in that: The nominal state of the nominal state kinematic model includes position, velocity, attitude quaternions and sensor zero bias; the error state vector is 15-dimensional and includes position error, velocity error, attitude error angle, gyroscope zero bias error and accelerometer zero bias error.

5. The high-precision positioning data compensation method independent of the vehicle interface according to claim 4, characterized in that: The update of the nominal state kinematic model includes attitude update, velocity update, and position update. The attitude update adopts the form of quaternions. For the Kth IMU cycle, it is realized based on the compensated angular increment and the exponential mapping of the rotation vector to the quaternion. The velocity update is calculated by the rotation matrix from the carrier coordinate system to the navigation coordinate system and the gravity vector. The position update is realized by the integral method or the median integral method.

6. The high-precision positioning data compensation method independent of the vehicle interface according to claim 1, characterized in that, The error state dynamic model is as follows: Where F is the state transition matrix, The error term caused by the antisymmetric matrix is... , Represents the antisymmetric matrix of vectors. Let G be the zero partial correlation time constant, and G be the noise driving matrix. This is the system noise vector.

7. The high-precision positioning data compensation method independent of the vehicle interface according to claim 1, characterized in that: The observation vector of the loose combination observation model is the difference between the absolute position calculated by UWB and the position estimated by INS, and the observation equation is: Where H is the observation matrix, and H directly extracts the position and velocity components from the error state. It is a 15-dimensional error state, where r is the observation noise of UWB.

8. The high-precision positioning data compensation method independent of the vehicle interface according to claim 1, characterized in that: Kalman gain is calculated using the Josephus form, and the formula is as follows: in, To estimate the covariance matrix of the state before the measurement update at time K+1, The identity matrix has the same dimension as the state vector. The product of the Kalman gain and the observation matrix. To observe the propagation of noise in the state space.

9. A high-precision positioning data compensation method independent of the vehicle interface according to claim 8, characterized in that, Feedback correction includes position correction, velocity correction, attitude correction, and sensor bias correction. The specific process of attitude correction is as follows: convert the attitude error angle into an error quaternion, multiply it by the nominal state attitude quaternion, and then normalize it.

10. A high-precision positioning data compensation method independent of the vehicle interface according to claim 4, characterized in that: The reset error state includes resetting all elements of the 15-dimensional error state vector to zero and resetting the error state covariance matrix to its initial value.

Images