Intelligent car sensor cooperative positioning and error correction method

Abstract

The invention discloses an intelligent car sensor cooperative positioning and error correction method, which comprises the following steps of: acquiring data through an IMU (Inertial Measurement Unit), a GPS (Global Positioning System) and an odometer, performing preliminary prediction by utilizing inertial solution, and constructing an error state Kalman filter to fuse IMU prediction with the GPS and odometer data; adaptively adjusting process noise and measuring a noise covariance matrix by measuring an information sequence; designing a path constraint optimization method, combining kinematic constraints of the trolley, introducing physical constraints as virtual measurement into a filtering process, and further reducing errors caused by sensor noise; a real-time fault-tolerant mechanism is designed, when a certain sensor breaks down or data is abnormal, the system can be automatically switched to other sensors for compensation, and navigation precision and stability are ensured. According to the method, the problem of insufficient positioning precision of a single sensor is effectively solved, and the positioning precision and the system robustness of the intelligent trolley in a complex environment are remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle positioning technology, specifically to a method for sensor-based collaborative positioning and error correction in intelligent vehicles. Background Technology

[0002] Intelligent vehicles have wide applications in autonomous driving, logistics delivery, and indoor / outdoor navigation. Accurate positioning is crucial for intelligent vehicles to achieve autonomous navigation. Commonly used positioning sensors include inertial measurement units (IMUs), GPS receivers, and odometers. IMUs can provide high-frequency acceleration and angular velocity data, but the position and attitude obtained through integration will drift over time; GPS can provide absolute position information, but the update frequency is low and it is susceptible to multipath effects and atmospheric interference; odometers provide speed information, but errors accumulate due to wheel slippage or uneven ground. Therefore, the above-mentioned multi-sensor fusion technology is widely used in intelligent vehicle control to improve positioning accuracy.

[0003] In existing technologies, intelligent vehicle control based on multi-sensor fusion methods suffers from several drawbacks. Sensor data is easily affected by environmental anomalies (such as electromagnetic interference and vibration), and inconsistencies in protocols among different sensor types further reduce the system's fault tolerance and compatibility with abnormal data. This is especially true in dynamic scenarios where inconsistent update rates of different sensor data can easily lead to fusion failure, resulting in navigation errors and deviations. Therefore, a new intelligent vehicle control strategy based on multi-sensor fusion methods is urgently needed to overcome these shortcomings and further improve the accuracy and robustness of vehicle navigation and positioning. Summary of the Invention

[0004] Purpose of the invention: To address the problems mentioned in the background art, this invention discloses a sensor collaborative positioning and error correction method for intelligent vehicles. Based on error state Kalman filtering, and combined with dynamic noise optimization, path constraints, and real-time fault tolerance mechanisms, this method improves the adaptive capability and positioning accuracy of the vehicle control system while ensuring the implementation of safeguards when multi-sensor fusion fails, thereby enhancing the robustness and environmental adaptability of the vehicle.

[0005] Technical solution:

[0006] This invention discloses a method for collaborative positioning and error correction of a vehicle using sensors, the method comprising the following steps:

[0007] S1: The sensor collects the position and speed information data of the vehicle;

[0008] S2: The IMU collects the acceleration and angular velocity data of the vehicle, uses inertial calculation to perform preliminary state prediction on the collected data, estimates the vehicle's speed, position and attitude, and obtains IMU prediction data;

[0009] S3: Construct an error state Kalman filter, fuse the predicted data with the collected data, update the state estimate, and output the optimized position, speed and attitude information of the vehicle in real time;

[0010] S4: Adjust the error state Kalman filter process noise covariance matrix based on the measured information sequence. and measurement noise covariance matrix And input the filtering iteration;

[0011] S5: The path constraint optimization method incorporates a filtering process to reduce sensor noise errors;

[0012] S6: Design a real-time fault-tolerant mechanism, monitor the health status based on the measurement residuals of the sensors, and adjust the data fusion strategy of the error state Kalman filter in real time.

[0013] Furthermore, the sensor described in S1 includes a GPS and an odometer, with the GPS acquiring the location. Data, odometer acquisition speed Information, as input for multi-sensor fusion, is collected by the IMU in S2, which measures the acceleration of the vehicle. and angular velocity The data, IMU data is output at a high frequency to provide continuous motion information; GPS and odometer data are output at a lower frequency to correct for the IMU's accumulated errors.

[0014] Furthermore, the fusion of the predicted data and the collected data described in S3 includes:

[0015] The error state Kalman filter defines an error state vector in its state space. During initialization, the error state vector is set to zero, indicating that the nominal state is assumed to have no error at the initial moment. The prediction step updates the error state covariance using IMU data. When GPS or odometer measurement data is available, an update step is performed to correct the error state estimate. After the update step is completed, a state injection and reset operation is performed to inject the corrected error state into the nominal state and update the nominal state. After the injection is completed, the error state vector is reset to zero, the Jacobian matrix is ​​reset, and the optimized vehicle position, velocity, and attitude information are output in real time.

[0016] Furthermore, step S4 specifically includes:

[0017] Innovation sequence calculation: Calculate the measurement innovation, i.e., the difference between the actual observed value and the model prediction, in each filtering period. ;

[0018] Sliding window maintenance: Maintain a sliding window of length N to store the most recent information sequence. The window scrolls and updates as the system runs, ensuring that the statistics reflect the latest state of the system; the actual covariance matrix of the new sequence is calculated within the sliding window. Calculate the theoretical information covariance matrix ,Compare and ,like > Increase the noise parameter, and conversely decrease the process noise covariance matrix. With measurement noise covariance matrix The noise parameters, after adjustment and The matrix is ​​immediately applied to the prediction and update steps of the next filtering cycle.

[0019] Furthermore, the process noise covariance matrix With measurement noise covariance matrix The noise parameters are adjusted as follows:

[0020] Process noise covariance matrix The main characterization of the uncertainty in system models such as IMUs is achieved through the adjustment formula:

[0021]

[0022] in, It is a smoothing factor used to balance the weights of historical information and current estimates, automatically identify changes in IMU performance, and adjust the model confidence accordingly;

[0023] Measurement noise covariance matrix The adjustment formula, which characterizes the measurement accuracy of sensors such as GPS and odometers, is as follows:

[0024]

[0025] in, It is a smoothing factor for measuring noise.

[0026] Furthermore, the path constraint optimization method described in S5 specifically includes:

[0027] Three types of physical constraints are established: planar motion constraints with zero vertical velocity, attitude constraints with zero roll and pitch angles, and nonholonomic constraints with zero lateral velocity in the vehicle coordinate system.

[0028] Establish a mathematical model to transform physical constraints into linear equations;

[0029] Each constraint is represented by a constraint matrix, which is a combination of corresponding components selected from the state vector.

[0030] To integrate constraints into the filtering framework, it is proposed to reformulate them as virtual measurement forms, where the measured values ​​are set as zero vectors, representing the ideal state of the constraints;

[0031] The noise characteristics of virtual measurements are characterized by a covariance matrix, the size of which is set according to the strictness of the constraints. Stricter constraints are equipped with smaller variance values ​​to reflect higher confidence.

[0032] When constructing virtual measurements, a combined constraint measurement matrix is ​​used to integrate all individual constraints into a unified framework, ensuring that the constraints can work together to optimize the state estimation.

[0033] The constraint update stage is designed to integrate constraint information into the fusion process of error state Kalman filtering;

[0034] Performed after the standard measurement update, the contribution weight of each constraint to the state correction is determined by calculating the constraint Kalman gain;

[0035] The error state is further optimized and updated using constrained Kalman gain.

[0036] Update the covariance matrix of the error state to reflect the reduction in uncertainty introduced by the constraints.

[0037] Furthermore, the specific operation of the real-time fault-tolerance mechanism described in S6 is as follows:

[0038] Establish a sensor health monitoring system, with its status including three levels: "normal", "warning" and "fault".

[0039] Health detection based on multidimensional statistical analysis of measurement residuals;

[0040] When new sensor data arrives, the measurement residual of the sensor is calculated, and the sensor measurement residual is statistically normalized.

[0041] Implement intelligent fault detection based on normalized residuals:

[0042] Two levels of detection thresholds are set: a warning threshold with a 95% confidence level and a fault threshold with a 99% confidence level, to perform multi-level threshold judgment.

[0043] When the normalized residual exceeds the fault threshold, a fault event is recorded immediately. If a fault occurs for two consecutive cycles, or the cumulative number of faults reaches the set upper limit, the sensor status is changed to "fault".

[0044] GPS Single Failure Mode: When GPS fails, navigation is performed using a combination of IMU and odometry, with odometry assisting in suppressing IMU drift.

[0045] Odometer single-failure mode: When the odometer fails, it relies on IMU and GPS, and uses GPS location updates to correct the accumulated error of IMU.

[0046] Multi-sensor failure mode: Perform short-term dead reckoning based on IMU, while actively attempting to restore the functions of other sensors.

[0047] Beneficial effects:

[0048] 1. This invention uses error-state Kalman filtering to fuse data from the IMU, GPS, and odometer, fully utilizing the high-frequency dynamic response of the IMU and the absolute accuracy information of the GPS / odometer. This provides accurate and reliable state estimation for the intelligent vehicle, reducing errors in the subsequent vehicle control system.

[0049] 2. By dynamically optimizing the noise covariance matrix, this invention can adaptively adjust the filtering parameters according to the real-time data quality of the sensor, effectively solving the limitation of traditional Kalman filtering where noise parameters need to be manually adjusted and cannot adapt to environmental changes, and significantly improving the positioning accuracy and system robustness of the vehicle in complex dynamic environments.

[0050] 3. The present invention designs a path constraint optimization method, which combines the kinematic constraints of the vehicle and introduces physical constraints as virtual measurements into the filtering process. When sensor measurements are interfered with or abnormal, the error caused by sensor noise is further reduced, thereby improving the accuracy of the control system.

[0051] 4. The present invention designs a real-time fault-tolerant mechanism that can effectively cope with various sensor anomalies. It ensures that when a sensor fails or data is abnormal in a complex environment, the intelligent vehicle can automatically switch fault strategies for compensation and always maintain reliable positioning capabilities, thereby further ensuring the navigation accuracy and stability of the vehicle. Attached Figure Description

[0052] Figure 1 This is an overall flowchart of the present invention;

[0053] Figure 2 This is a structural diagram of the error state Kalman filter fusion of the present invention;

[0054] Figure 3 This is a flowchart of the dynamic optimization of the noise covariance matrix of the present invention;

[0055] Figure 4 This is a flowchart of the path constraint optimization method of the present invention;

[0056] Figure 5 This is a flowchart of the real-time fault-tolerance mechanism of the present invention;

[0057] Figure 6 This is a simulation experiment result diagram of the present invention. Detailed Implementation

[0058] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] like Figure 1 As shown, this invention discloses a sensor-based collaborative localization and error correction method for an intelligent vehicle, with the following steps:

[0060] Step 1: The IMU collects acceleration and angular velocity at a high frequency of 100Hz, while the GPS collects position at a low frequency of 1Hz. The odometer uses a mid-frequency sampling rate of 10Hz. All data is synchronized via timestamps and unified to the global ENU coordinate system;

[0061] Step 2: Process the IMU data using inertial calculations. The IMU data includes acceleration. and angular velocity By integrating, the state of the vehicle can be predicted. The state vector includes position. ,speed and posture (Quaternions) are used for attitude, velocity, and position updates, with updates achieved through numerical integration;

[0062] Step 3: Perform sensor fusion using Error State Kalman Filtering (ESKF). The core of ESKF is estimating the error state. Instead of the direct state First, the nominal state and error state covariance are updated using IMU data. Then, the error state is corrected using GPS and odometer measurements. Finally, the corrected error state is merged into the nominal state. After injection, the error state is cleared to zero, and the Jacobian matrix is ​​reset. This serves as the main output interface of the vehicle control system, outputting the optimized vehicle position, speed, and attitude information to the control system in real time.

[0063] Step 4: Dynamically optimize the noise covariance matrix of the error state Kalman filter. Adaptively adjust the process noise covariance matrix based on the measurement information sequence. and measurement noise covariance matrix By analyzing the difference between the actual system performance and the model predictions, the filtering parameters are dynamically optimized to improve the system's adaptability to environmental changes. The denoised and optimized parameters are then fed back into the filtering process in step 3.

[0064] Step 5: Design a path constraint optimization method to introduce the filtering process. Combine the kinematic constraints of the vehicle and introduce the physical constraints as virtual measurements into the filtering process of Step 3. Utilize the kinematic characteristics of the vehicle to further reduce the error caused by sensor noise and improve positioning accuracy.

[0065] Step 6: Design a real-time fault-tolerance mechanism. Based on the measurement residuals of each sensor, monitor its health status. When abnormal sensor data is detected, automatically adjust its weight in the data fusion process of Step 3 or isolate it. Through sensor fault detection and system reconfiguration, ensure the stable operation of the vehicle in complex environments.

[0066] like Figure 2 As shown, step 3, which uses error-state Kalman filtering to perform data fusion on the IMU, GPS, and odometer, is as follows:

[0067] Step 3.1: Perform preliminary state prediction on the acceleration and angular velocity data acquired by the IMU using inertial calculations. Specifically, attitude is represented using quaternions, and acceleration and angular velocity are integrated using numerical integration methods. The dynamic model is as follows:

[0068] Posture update:

[0069]

[0070] in, yes The attitude quaternion at time, It is a skew-symmetric matrix of angular velocity. It is a quaternion multiplication operator. It is a quaternion exponential mapping.

[0071] Speed ​​updates:

[0072]

[0073] in, yes The velocity vector at time t, It is the attitude rotation matrix. Accelerometer readings at time 10:00 It is the vector of gravitational acceleration.

[0074] Location update:

[0075]

[0076] in, yes The position vector at time , yes The position vector at any given time.

[0077] Because IMU data is generated at high frequencies, this step is performed at the IMU frequency to provide an initial estimate for subsequent fusion.

[0078] Step 3.2: Next, the preliminary predictions from the IMU are fused with the actual measurements from GPS and odometer to update the state estimate. The core of ESKF is estimating the error state. ,in It is a positional error. It's a speed error. It is attitude error. It is the accelerometer bias error. It is the gyroscope bias error.

[0079] The filtering process includes prediction and updating. The prediction step involves updating the nominal state and error state covariance using IMU data. The updating step involves correcting the error state using GPS and odometer measurements. The prediction steps are as follows:

[0080] The nominal state prediction equation is:

[0081]

[0082] in, It's the IMU's acceleration and angular velocity. It is a nonlinear state transition function

[0083] The error state prediction equation is:

[0084]

[0085] in, It is the error state prediction value at time k. It is the state transition matrix, obtained by linearizing the IMU model. It is the process noise vector.

[0086] Covariance prediction:

[0087]

[0088] in, It is the predicted value of the error state covariance matrix at time k. It is the state transition matrix. Process noise covariance matrix.

[0089] The Kalman gain is calculated using the following formula:

[0090]

[0091] in, It is the measurement matrix (the Jacobian matrix of the measurement function with respect to the state). To measure the noise covariance matrix, dynamic adjustments are made based on sensor quality.

[0092] Error status update:

[0093]

[0094] Covariance update:

[0095]

[0096] in, It is the estimated value of the error state covariance matrix at time k. It is the identity matrix

[0097] Step 3.3: State Injection and Reset. The corrected error states are merged into the nominal state, and the nominal state is updated. The calculation formula is as follows:

[0098]

[0099] Among them, for position and velocity, It's ordinary vector addition, for attitude quaternions. Its update is , It is quaternion multiplication, which is based on the attitude error vector. The small-angle quaternion obtained by the transformation.

[0100] After the injection is complete, the error status will be cleared to zero. .

[0101] Simultaneously, based on a specific reset Jacobian matrix Update the error covariance matrix to reflect the impact of the reset operation on uncertainty. Prevent repeated corrections of already corrected errors and prepare for the next filtering cycle. .

[0102] like Figure 3 As shown, the noise covariance matrix of the error-state Kalman filter is dynamically optimized. The process noise covariance matrix is ​​adaptively adjusted based on the measurement information sequence (i.e., the difference between the actual measured value and the predicted value). and measurement noise covariance matrix Process noise covariance matrix The measurement noise covariance matrix is ​​related to the error characteristics of the IMU (such as the bias noise of the accelerometer and gyroscope). This is related to the measurement error characteristics of GPS and odometers (such as GPS accuracy degradation or odometer slippage). The detailed process for dynamically optimizing the noise covariance matrix is ​​as follows:

[0103] Step 4.1: First, calculate the measurement innovation sequence, using the following formula:

[0104]

[0105] in, It measures the innovation vector. These are actual measured values. It is a predicted measurement. Nominal state estimation.

[0106] Step 4.2: Perform sliding window maintenance: Maintain a sliding window of length N=20 to store the most recent information sequence. Then, the actual covariance is estimated:

[0107]

[0108] in, It is an estimate of the actual covariance matrix of the new sequence. It refers to the size of the sliding window.

[0109] Next, we will calculate the theoretical covariance:

[0110] .

[0111] Step 4.3: Next, through comparison... and , from adaptation and For example, if > Then increase or To reflect greater uncertainty, the formula for adjusting the noise parameters is as follows:

[0112] Process noise covariance adjustment:

[0113]

[0114] Measurement noise covariance adjustment:

[0115]

[0116] in, and It is a smoothing factor, with a value range of [0,1], and is typically set to 0.9-0.95. It is the Kalman gain matrix. This dynamic optimization enables the system to adapt to environmental changes, such as GPS signal loss or increased IMU noise, by adjusting sensor weights in real time to reduce errors.

[0117] like Figure 4 As shown, combining the kinematic constraints of the vehicle, physical constraints are introduced as virtual measurements into the filtering process. The specific implementation steps of path constraint optimization are as follows:

[0118] Step 5.1: Based on the motion characteristics of the car, establish the three types of physical constraints that its motion needs to follow, (1) planar motion constraints, that is, the vertical velocity is zero ( (2) Attitude constraints, i.e., roll and pitch angles are zero. , (3) Non-holonomic constraint, that is, the lateral velocity in the vehicle coordinate system is zero. Then, these three types of constraints are integrated into a unified linear constraint equation. In, among them, , It is a constraint matrix. It is a state vector. It is noise that characterizes the confidence level of constraints, and its covariance Set as The first three 0.01 values ​​correspond to the vertical velocity constraint, roll angle constraint, and pitch angle constraint, respectively, while 0.001 represents the lateral velocity constraint. This equation... Interpreted as a virtual measurement model, the measured value is The measurement function is The measured noise is .

[0119] Step 5.2: Next, the virtual measurement is added to the update step of the error state Kalman filter, treated the same as the real sensor data, and used to correct the state. After generating the virtual measurement, its usage in the ESKF framework is as follows: After completing the IMU-based prediction step and processing the measurement updates of real sensors such as GPS and odometer, the system will execute an additional update step for this virtual measurement.

[0120] Calculate the constrained Kalman gain:

[0121]

[0122] This gain determines the weight of the virtual measurement on the state correction.

[0123] Calculate the residuals of the virtual measurements and update the state:

[0124]

[0125] in Current nominal state of computational quantity With physical constraints The degree of deviation. This residual is expressed through gain. Transformed into an error state The correction amount is used to "pull" the state estimate back onto a trajectory that conforms to the laws of physics.

[0126] Update constraint covariance:

[0127]

[0128] This operation reflects the reduction in state uncertainty after the introduction of constraints.

[0129] like Figure 5 As shown, the real-time fault tolerance mechanism monitors the health status based on the measurement residuals of each sensor. The specific steps are as follows:

[0130] Step 6.1: First, perform residual calculations for both the GPS and the odometer.

[0131] GPS residuals are calculated by directly comparing the actual measured position with the system-estimated position in the global coordinate system. If the difference is small, it indicates that the GPS measurement is consistent with the system estimate; otherwise, it suggests the possible existence of GPS error or system estimation residuals. The calculation formula is as follows:

[0132]

[0133] in, It is the position vector actually measured by the GPS receiver. It is a vector of system estimates of position.

[0134] The odometer residual compares the difference between the actual measured speed and the system-estimated speed in the vehicle coordinate system. First, the system-estimated global speed is transformed to the vehicle coordinate system, and then compared with the vehicle speed directly measured by the odometer, ensuring the comparison is performed within the same coordinate system. The calculation formula is:

[0135]

[0136] in, It is the velocity vector actually measured by the odometer. Transpose of a rotation matrix It is the system's estimated velocity vector.

[0137] Step 6.2: Calculate the normalized residuals for GPS and odometer data. The calculation formula is as follows:

[0138]

[0139] in, .

[0140] Step 6.3: Based on the normalized residual, the system implements intelligent fault detection;

[0141] The system sets two levels of detection thresholds: a warning threshold with a 95% confidence level and a fault threshold with a 99% confidence level, to perform multi-level threshold judgment.

[0142] When the sensor's normalized residual exceeds the warning threshold, the system records a warning event. If warnings occur for three consecutive cycles, the sensor status is changed to "warning".

[0143] When the normalized residual exceeds the fault threshold, a fault event is recorded immediately. If a fault occurs for two consecutive cycles, or the cumulative number of faults reaches the set upper limit, the sensor status is changed to "fault".

[0144] For sensors in a "warning" or "fault" state, the system continuously monitors their performance. If the normalized residual is below the warning threshold for 10 consecutive cycles (for GPS) or 5 cycles (for odometer), and other recovery conditions are met (such as sufficient GPS satellite count), the system restores the sensor state to "normal".

[0145] Step 6.4: Adaptive Weight Adjustment. Once a sensor anomaly is identified, the system initiates an adaptive weight adjustment mechanism. The core of this mechanism is to dynamically adjust the measurement noise covariance matrix to change the influence of the abnormal sensor in data fusion.

[0146] For normally functioning sensors, maintain their original noise characteristic parameters;

[0147] For warning status sensors, appropriately increase their noise variance and reduce their weight in status updates;

[0148] For faulty sensors, significantly increase their noise variance to minimize their impact on the fusion results.

[0149] During the weight adjustment process, the system maintains the relative balance of the weights of each sensor and ensures that the sum of the weights of all available sensors remains constant through normalization.

[0150] This design ensures that when some sensors fail, the remaining normal sensors can reasonably share the responsibility of state estimation, thus maintaining the overall performance of the system.

[0151] Step 6.5: Sensor Isolation and System Reconstruction. If a sensor malfunctions repeatedly, it will be excluded from the fusion process until normal operation is restored. The fusion strategy will be reconstructed based on currently available sensors:

[0152] GPS Single Failure Mode: When GPS fails, the system relies on IMU and odometry for combined navigation, with odometry assisting in suppressing IMU drift;

[0153] Odometer single-failure mode: When the odometer fails, the system mainly relies on the IMU and GPS, using GPS location updates to correct the accumulated error of the IMU;

[0154] Multi-sensor failure mode: When multiple external sensors fail, the system enters a pure inertial navigation mode, performs short-term dead reckoning based on the IMU, and actively attempts to restore the functions of other sensors.

[0155] Ensure the system can still operate stably in the event of sensor failure.

[0156] To ensure the feasibility and effectiveness of this invention, the comparative experimental results are as follows: Figure 6 As shown, the left side compares the trajectories fused by the sensors of the intelligent vehicle. The vehicle simulates a real trajectory. The trajectory of the vehicle corrected by the IMU alone, the trajectory after IMU and GPS fusion, and the trajectory after IMU, GPS and odometer fusion of the present invention are compared together. It can be seen that the IMU alone exhibits a serious drift phenomenon over time. Although IMU and GPS fusion can solve the drift problem, the present invention has stronger stability and accuracy compared to the present invention.

[0157] The right side shows the average positioning error statistics, which is the average error value calculated based on the intelligent vehicle trajectory comparison chart. It can be clearly seen that the error of the vehicle after the fusion of IMU, GPS and odometer is lower than that of the other two methods.

[0158] Simulation comparison charts demonstrate the performance comparison between the multi-sensor fusion positioning method proposed in this invention and existing technologies. By employing a multi-source information fusion architecture combining IMU, GPS, and odometer data, the positioning and error correction accuracy of the intelligent vehicle is improved.

[0159] The above embodiments are only for illustrating the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent transformations or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A method for sensor-based collaborative positioning and error correction for an intelligent vehicle, characterized in that, The method includes the following steps: S1: The sensor collects the position and speed information data of the vehicle; S2: The IMU collects the acceleration and angular velocity data of the vehicle, uses inertial calculation to perform preliminary state prediction on the collected data, estimates the vehicle's speed, position and attitude, and obtains IMU prediction data; S3: Construct an error state Kalman filter, fuse the predicted data with the collected data, update the state estimate, and output the optimized position, speed and attitude information of the vehicle in real time; S4: Adjust the error state Kalman filter process noise covariance matrix based on the measured information sequence. and measurement noise covariance matrix And input the filtering iteration; S5: The path constraint optimization method incorporates a filtering process to reduce sensor noise errors; S6: Design a real-time fault-tolerant mechanism, monitor the health status based on the measurement residuals of the sensors, and adjust the data fusion strategy of the error state Kalman filter in real time.

2. The intelligent vehicle sensor collaborative positioning and error correction method according to claim 1, characterized in that, The sensor mentioned in S1 includes a GPS and an odometer; the GPS acquires location information. Data, odometer acquisition speed Information, as input for multi-sensor fusion, is collected by the IMU in S2, which measures the acceleration of the vehicle. and angular velocity The data, IMU data is output at a high frequency to provide continuous motion information; GPS and odometer data are output at a lower frequency to correct for the IMU's accumulated errors.

3. The intelligent vehicle sensor collaborative positioning and error correction method according to claim 2, characterized in that, The fusion of predicted data and collected data mentioned in S3 includes: The error state Kalman filter defines an error state vector in its state space. During initialization, the error state vector is set to zero, indicating that the nominal state is assumed to have no error at the initial moment. The prediction step updates the error state covariance using IMU data. When GPS or odometer measurement data is available, an update step is performed to correct the error state estimate. After the update step is completed, a state injection and reset operation is performed to inject the corrected error state into the nominal state and update the nominal state. After the injection is completed, the error state vector is reset to zero, the Jacobian matrix is ​​reset, and the optimized vehicle position, velocity, and attitude information are output in real time.

4. The intelligent vehicle sensor collaborative positioning and error correction method according to claim 2, characterized in that, Step S4 specifically includes: Innovation sequence calculation: Calculate the measurement innovation, i.e., the difference between the actual observed value and the model prediction, in each filtering period. ; Sliding window maintenance: Maintain a sliding window of length N to store the most recent information sequence. The window scrolls and updates as the system runs, ensuring that the statistics reflect the latest state of the system; the actual covariance matrix of the new sequence is calculated within the sliding window. Calculate the theoretical information covariance matrix ,Compare and ,like > Increase the noise parameter, and conversely decrease the process noise covariance matrix. With measurement noise covariance matrix The noise parameters, after adjustment and The matrix is ​​immediately applied to the prediction and update steps of the next filtering cycle.

5. The intelligent vehicle sensor collaborative positioning and error correction method according to claim 4, characterized in that, Process noise covariance matrix With measurement noise covariance matrix The noise parameters are adjusted as follows: Process noise covariance matrix The main characterization of the uncertainty in system models such as IMUs is achieved through the adjustment formula: ； in, It is a smoothing factor used to balance the weights of historical information and current estimates, automatically identify changes in IMU performance, and adjust the model confidence accordingly; Measurement noise covariance matrix The adjustment formula, which characterizes the measurement accuracy of sensors such as GPS and odometers, is as follows: ； in, It is a smoothing factor for measuring noise.

6. The intelligent vehicle sensor collaborative positioning and error correction method according to claim 1, characterized in that, The path constraint optimization method described in S5 specifically includes: Three types of physical constraints are established: planar motion constraints with zero vertical velocity, attitude constraints with zero roll and pitch angles, and nonholonomic constraints with zero lateral velocity in the vehicle coordinate system. Establish a mathematical model to transform physical constraints into linear equations; Each constraint is represented by a constraint matrix, which is a combination of corresponding components selected from the state vector. To integrate constraints into the filtering framework, it is proposed to reformulate them as virtual measurement forms, where the measured values ​​are set as zero vectors, representing the ideal state of the constraints; The noise characteristics of virtual measurements are characterized by a covariance matrix, the size of which is set according to the strictness of the constraints. Stricter constraints are equipped with smaller variance values ​​to reflect higher confidence. When constructing virtual measurements, a combined constraint measurement matrix is ​​used to integrate all individual constraints into a unified framework, ensuring that the constraints can work together to optimize the state estimation. The constraint update stage is designed to integrate constraint information into the fusion process of error state Kalman filtering; Performed after the standard measurement update, the contribution weight of each constraint to the state correction is determined by calculating the constraint Kalman gain; The error state is further optimized and updated using constrained Kalman gain. Update the covariance matrix of the error state to reflect the reduction in uncertainty introduced by the constraints.

7. The intelligent vehicle sensor collaborative positioning and error correction method according to claim 1, characterized in that, The real-time fault-tolerance mechanism described in S6 operates as follows: Establish a sensor health monitoring system, with its status including three levels: "normal", "warning" and "fault". Health detection based on multidimensional statistical analysis of measurement residuals; When new sensor data arrives, the measurement residual of the sensor is calculated, and the sensor measurement residual is statistically normalized. Implement intelligent fault detection based on normalized residuals: Two levels of detection thresholds are set: a warning threshold with a 95% confidence level and a fault threshold with a 99% confidence level, to perform multi-level threshold judgment. When the normalized residual exceeds the fault threshold, a fault event is recorded immediately. If a fault occurs for two consecutive cycles, or the cumulative number of faults reaches the set upper limit, the sensor status is changed to "fault". GPS Single Failure Mode: When GPS fails, navigation is performed using a combination of IMU and odometry, with odometry assisting in suppressing IMU drift. Odometer single-failure mode: When the odometer fails, it relies on IMU and GPS, and uses GPS location updates to correct the accumulated error of IMU. Multi-sensor failure mode: Perform short-term dead reckoning based on IMU, while actively attempting to restore the functions of other sensors.

Images