An automatic driving vehicle accumulated error reduction method based on kalman filtering

Abstract

The application discloses a kind of automatic driving vehicle cumulative error reduction method based on Kalman filter, to improve the accuracy of vehicle trajectory prediction, reduce cumulative error.System obtains the first attitude information vector Φ k1 And second attitude information vector Φ k2 Of vehicle by wheel encoder and vehicle GPS.For the above two attitude vectors are predicted and solved by the Kalman filter constructed, and the optimal attitude information vector Φ opt After correction is obtained.The optimal attitude information vector Φ opt It is brought into the start-stop situation of automatic driving vehicle, and the distance calculated is compared with the set threshold value, so as to judge calibration according to the error size.Simulation test shows that the actual position data of vehicle is calibrated by the above steps at each stop to reduce the cumulative error of vehicle in multiple start-stop mode, the system effectively reduces the superposition of each trajectory error, improves the accuracy of long-distance multi-segment driving trajectory prediction.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of autonomous driving and intelligent transportation systems (ITS), and specifically relates to a multi-sensor data fusion method and an automatic driving vehicle cumulative error reduction method based on Kalman filtering. BACKGROUND

[0002] With the rapid development of autonomous driving technology, the accuracy requirements of positioning and path recognition are also constantly improving. The traditional global positioning system (GPS) has good coverage and ease of use, but in complex urban environments, due to signal interference and environmental shielding, the positioning accuracy of GPS is insufficient, and it cannot meet the needs of autonomous vehicles in high-precision, real-time scenarios. Therefore, technologies that fuse multiple sensor data, such as inertial navigation systems, lidar, and vehicle-mounted cameras, have been widely used to improve the robustness and accuracy of the system.

[0003] Existing multi-sensor fusion methods are usually based on the extended Kalman filter (EKF) algorithm, which predicts the vehicle state through a dynamic system model and updates the state estimate value combined with actual measurement data. However, these methods still have high cumulative errors when dealing with complex traffic scenarios, such as stop-and-go.

[0004] To solve the above problems, the present application predicts and fuses data under the extended Kalman filter based on GPS and wheel encoders to obtain the optimal attitude information vector of the autonomous vehicle, and sets an error threshold in the stop-and-go situation to calibrate it, thereby improving the trajectory recognition accuracy of the autonomous vehicle in such repeated start-stop scenarios. SUMMARY

[0005] The purpose of the present application is to address the problem of high cumulative errors in trajectory recognition of autonomous vehicles in complex urban environments due to insufficient GPS positioning accuracy and environmental shielding. To achieve the above purpose, the present application provides the following technical solutions,

[0006] 1. A Kalman filter-based autonomous vehicle cumulative error reduction method and system, characterized in that the method comprises:

[0007] Step 1: generating a first attitude information vector Φ of the autonomous vehicle in the navigation coordinate system from the wheel encoder of the vehicle and the inertial measurement unit k1 ;

[0008] Step 2: obtaining a second attitude information vector Φ from the vehicle-mounted positioning system GPS of the autonomous vehicle k2 ;

[0009] Step 3, predict and fuse the above two attitude information vectors according to the constructed Kalman filter to obtain the corrected optimal attitude information vector Φ opt as the Kalman filter result;

[0010] Step 4, the Kalman filter result obtained in the above step 3, that is, the corrected optimal attitude information vector Φ opt as a parameter into the start-stop situation environment of the autonomous vehicle, thereby updating and calibrating the trajectory error.

[0011] The first attitude information vector Φ k1 of the autonomous vehicle in the navigation coordinate system is generated from the wheel encoder of the vehicle in step 1

[0012] Step 11, obtain the left and right wheel speed information v l and v r of the autonomous vehicle;

[0013] Step 12, generate the vehicle lateral speed v x and the vehicle longitudinal speed v y in the vehicle body coordinate system.

[0014] Step 13, convert the above vehicle lateral speed v x and vehicle longitudinal speed v y into a speed vector V sdv in the vehicle body coordinate system, that is, V sdv = [v x , v y , 0] T

[0015] Step 14, convert the above speed vector V sdv in the vehicle body coordinate system into the first attitude information vector Φ k1 in the GPS-based navigation coordinate system.

[0016] The second attitude information vector Φ k2 is obtained from the vehicle positioning system GPS of the autonomous vehicle in step 2

[0017] Step 21, set the data transmission rate to 10Hz to receive vehicle GPS positioning data, including latitude positioning information of the autonomous vehicle, λ GPS longitude positioning information of the autonomous vehicle;

[0018] Step 22, convert the above data into the slip angle α sa of the autonomous vehicle, the relative azimuth angle β ra and the relative travel distance d GPS ;

[0019] Step 23, the above phase information is obtained in the east direction and the north direction of the navigation coordinate system respectively ge and v gn , to obtain the second attitude information vector Φ k2 .

[0020] In step 3, the above two kinds of sensor data are predicted and fused through the equation set of the extended Kalman filter, including the prediction equation, the state estimation equation, the estimation mean square error equation and the Kalman gain equation, to obtain the optimal attitude information vector Φ opt , characterized in that:

[0021] (1) the prediction step predicts the system through the dynamic model of the system and the process covariance error.

[0022] The prediction equation is

[0023] (2) the state estimation update equation is

[0024] (3) the Kalman filter gain equation is

[0025] (4) the estimation mean square error equation is

[0026] wherein, and are the prior state estimation of the autonomous vehicle position at time steps k-1 and k, and are the state estimation of the autonomous vehicle position at time steps k-1 and k respectively, u k-1 is the control input, is the nonlinear prediction function of the system state. μ k is the observation value at time step k, and h is the nonlinear function of the observation model known by the system. λ k is an adaptive factor. B is the preset control matrix of the system. H k is the Jacobian matrix of the observation model, is its transpose, R k is the measurement noise mean square error matrix defined at time step k. is the prior error covariance matrix at time step k, P k-1 is the error covariance matrix at time step k-1.

[0027] Step 31, before the above listed extended Kalman filter is used, the position state estimation value of the autonomous vehicle is initialized as and the prediction mean square error matrix is initialized as wherein is the initial velocity mean square deviation.

[0028] Step 32, after initialization, the first attitude information vector Φ k1 into the prior state of the state estimation update equation of the Kalman filter According to the second attitude information vector Φ k2 into the observation value μ of the time step k of the Kalman filter k , that is, the attitude can be calculated based on the Kalman filter constructed in advance, and the optimal attitude information vector Φ is obtained opt .

[0029] Step 4, the Kalman filter result obtained in the above step 3, that is, the optimal attitude information vector Φ opt is taken as a parameter to update and calibrate the trajectory error in the parking and starting of the autonomous vehicle.

[0030] The specific idea is: set the distance of the autonomous vehicle relative to the pedestrian line traffic sign when the autonomous vehicle stops as D0, set the distance measurement error of the autonomous vehicle as D e , set D T as the distance change threshold of the autonomous vehicle, the threshold is related to the state estimation result of the Kalman filter constructed above, D T = K t ∫ΔΦ opt (t)dt, K t is a preset relevant proportion coefficient, ΔΦ opt (t) is the difference value of the optimal attitude information vector obtained in the above step 3 Kalman filter, ΔΦ opt (t) = Φ opt (t1) - Φ opt (t0), t1-t0=20s. After the autonomous vehicle stops in front of the pedestrian traffic sign and restarts driving, every 20s interval, the relative position information d pcl of the autonomous vehicle and the pedestrian line traffic sign position d GPS recorded by GPS are obtained, according to the similar coordinate transformation in the above steps 1 and 2 and the difference value, the relative distance change D i (i=1,2,3,...,10) of the autonomous vehicle when starting and stopping is obtained. If D i >D T , (i=1,2,...,10), it is considered that the error caused by the starting and stopping of the autonomous vehicle affects the subsequent driving position trajectory estimation of the vehicle. Then the relative distance change D i of the autonomous vehicle when starting and stopping calculated at present is obtained.The mean value D of (i=1, 2,..., 10) is subtracted from the mean value D at this time t D0 to obtain a difference value AD, if the difference value is greater than the automatic driving vehicle distance measurement error D e , it is also considered that the automatic driving vehicle causes error influence on the subsequent driving position trajectory estimation after starting and stopping.

[0031] In view of the influence of the above error on the position trajectory, the current driving section, i.e. when the position error is too large, the position trajectory estimation is focused on using the wheel encoder. And it is considered that the automatic driving vehicle resets the vehicle-mounted GPS for repositioning and calibration to eliminate the cumulative error of the automatic driving vehicle in the last section, so as to ensure that the automatic driving vehicle reduces the positioning error or other factors that cause the driving position distance to be larger than the real position error after starting from the stop state in a long distance and multiple driving sections.

[0032] The effective effect of the present application limits the cumulative error within the current trajectory section, avoids its propagation to the subsequent trajectory, thereby effectively improving the accuracy of the trajectory of the automatic driving vehicle and reducing the cumulative error of the complete trajectory of the automatic driving vehicle. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 The main process steps of the present application.

[0034] Figure 2 It is a data fusion flowchart.

[0035] Figure 3 It is a simulation road map for vehicle driving in a complex urban environment.

[0036] Figure 4 The simulation schematic diagram of the vehicle in the starting and stopping state at the pedestrian line traffic sign.

[0037] Figure 5 It is a KF trajectory recognition curve graph when there are 2 starting and stopping points.

[0038] Figure 6 It is a KF trajectory recognition curve graph when there are 4 starting and stopping points. DETAILED DESCRIPTION

[0039] After the automatic driving system of the automatic driving vehicle is started in the simulation platform, the automatic driving vehicle starts driving in the case that there are large buildings as obstacles around. It is set that there will be 4 pedestrian line traffic signs in the driving process to make the automatic driving vehicle start and stop in the middle. Before each start and stop, the vehicle wheel encoder model is predicted and combined with the vehicle-mounted GPS of the automatic driving vehicle to measure the position of the automatic driving vehicle. The present application provides a multi-sensor data fusion method based on an extended Kalman filtering algorithm, and the specific steps are as followsFigure 1 The error is accumulated and eliminated for each start-stop, so as to obtain a more accurate trajectory distance estimation curve for a complete path.

[0040] Step 1, generating a first attitude information vector Φ of the autonomous vehicle in the navigation coordinate system from the wheel encoders of the vehicle and the inertial measurement unit k1 ;

[0041] Step 11, obtaining the left and right wheel speed information v l and v r of the autonomous vehicle.

[0042] Here, the wheel rotation information ω l = n l / θ res and ω r = n r / θ res of the autonomous vehicle is obtained, wherein θ res represents the resolution of the wheel encoder. Finally, the left wheel speed v l and the right wheel speed v r of the autonomous vehicle at present can be obtained by multiplying the rotation speed by the wheel arc length l.

[0043] Step 12, generating the lateral speed v x and the longitudinal speed v y of the vehicle in the vehicle body coordinate system.

[0044] Here, the left and right wheel speed information v l and v r obtained above can be used to obtain the motion and direction of the vehicle through kinematic analysis of the vehicle. The motion equation of the moving vehicle can be expressed as wherein d is the distance between the left and right wheels, i.e. the vehicle width of the vehicle chassis.

[0045] Step 13, converting the above lateral speed v x and longitudinal speed v y of the vehicle into a speed vector V sdv in the vehicle body coordinate system, i.e. V sdv = [v x , v y , 0] T

[0046] Step 14, converting the above speed vector V sdv in the vehicle body coordinate system into a first attitude information vector Φ k1 in the GPS-based navigation coordinate system.

[0047] Specifically, the inertial measurement unit (IMU) acquires the first heading angle α, the first pitch angle β, and the first roll angle γ of the autonomous vehicle, thereby constructing the rotation matrix R from the vehicle coordinate system to the navigation coordinate system. k The expression is

[0048]

[0049] Then the first attitude information vector Φ k1 =R k V sdv =[v ke ,v kn ,v ku ], here v ke ,v kn ,v ku The velocity components of the autonomous vehicle in the east, north, and sky directions in the navigation coordinate system.

[0050] Step 2: Obtain the second attitude information vector Φ from the vehicle's onboard GPS positioning system. k2 Specifically, it includes:

[0051] Step 21: While the autonomous vehicle is in motion, set it to receive vehicle GPS positioning data at a data transmission rate of 10Hz. The positioning data includes... For the latitude and longitude positioning information of autonomous vehicles, λ GPS Longitude location information for autonomous vehicles.

[0052] Step 22, the above Latitude and longitude positioning information of autonomous vehicles, λ GPS The longitude positioning information of the autonomous vehicle and the Earth's radius parameters under GPS are converted into the slip angle α of the autonomous vehicle. sa Relative azimuth angle β ra and relative driving distance d GPS Its union expression is as follows:

[0053] d GPS =R GPS β ra

[0054] Step 23, adjust the aforementioned relative azimuth angle β ra and relative driving distance d GPS Rates were obtained in the east and north directions in the navigation coordinate system, respectively. and ΔT GPS By measuring the time for the positioning system, the second attitude information vector Φ can be obtained. k2 =[v ge ,vgn ,0]。

[0055] Step 3, according to the constructed Kalman filter, the first attitude information vector Φ k1 of the autonomous vehicle at the same time is predicted and fused with the second attitude information vector Φ k2 of the autonomous vehicle, to obtain the corrected optimal attitude information vector Φ opt as the Kalman filtering result.

[0056] The principle knowledge about the extended Kalman filter can be referred to the related art, which will not be further described here; the prediction ability of the extended Kalman filter can be divided into two key stages: prediction and state update. Figure 2 is the data fusion flowchart of the Kalman filter in this step. The equation set of the extended Kalman filter in the embodiment of the present application includes a prediction equation, a state estimation equation, an estimated mean square error equation and a Kalman gain equation:

[0057] (1) The prediction step predicts the system through the dynamic model of the system and the process covariance error.

[0058] The prediction equation is

[0059] (2) The state estimation update equation is

[0060] (3) The Kalman filter gain equation is

[0061] (4) The estimated mean square error equation is

[0062] The parameters of each equation are explained as follows:

[0063] In the prediction equation, is the prior state estimation result of the position of the autonomous vehicle at time step k, is the state estimation at the last time step k-1, u k-1 is the control input at the last time step k-1. is a nonlinear prediction function of the system state, wherein the linearization here is I is a unit matrix, A is a preset 3*3-dimensional state transition matrix, and dt is the update period of the extended Kalman filter.

[0064] In the state estimation update equation, is the posterior state estimation of the position of the autonomous vehicle at time step k. μ k is the observation value at time step k, and h is a nonlinear function of the known observation model of the system. An adaptive factor λ kParameters of the filter are adjusted to adapt to changes in system characteristics. Used to adjust the measurement noise covariance R and system noise covariance Q.

[0065] In the Kalman filter gain equation, the adaptive factor formula is: Where A is the above-mentioned 3*3 dimensional state transition matrix, B is the system preset control matrix. k is the Jacobian matrix of the observation model, is its transpose, R k is the measurement noise variance matrix defined at time step k, is the defined measurement noise parameter. is the prior error covariance matrix at time step k, The calculation formula is Q k-1 is the defined system noise variance matrix Q, is the defined system noise parameter. P k-1 is the error covariance matrix at the last time step k-1, Q k-1 is the process noise covariance matrix at the last time step k-1, F k is the Jacobian matrix of the system state;

[0066] Step 31, before using the above-mentioned extended Kalman filter equation group, the position state estimation value of the autonomous vehicle at k=1 is initialized to and the predicted mean square error matrix is initialized to Where is the initial velocity mean square error of the autonomous vehicle system navigation coordinate system in the X, Y, and Z positive half axes respectively.

[0067] Step 32, after initialization, the above-mentioned and P0 value are substituted into the prediction equation and the estimated mean square error equation P k-1 , the first attitude information vector Φ k1 in step 1 is brought into the prior state in the state estimation update equation of the Kalman filter, and then the second attitude information vector Φ k2 is brought into the observation value μ k at time step k of the Kalman filter, that is, based on the pre-constructed Kalman filter, the attitude estimation is performed to obtain as the corrected optimal attitude information vector Φ opt .

[0068] Step 4, the Kalman filter result obtained in step 3, that is, the corrected optimal attitude information vector Φ optThe parameters are brought into the automatic driving parking start environment, so as to update and calibrate the trajectory error.

[0069] Figure 3 The route used in the simulation environment is a traffic road in the city, which contains some environmental disturbances, such as simulated large buildings beside the road, which will reduce the GPS positioning accuracy. The total length between the starting point and the ending point is about 200 meters, and 4 pedestrian crossings are set as the vehicle start and stop points. The simulation aims to identify the positioning error of GPS by processing the dynamic movement data collected by GPS and vehicle wheel encoders.

[0070] In the process of repeated start and stop of the autonomous vehicle, in view of the problem of large wheel encoder estimation error caused by wheel slip in the start and stop state of the vehicle, software filtering is used to correct the wheel encoder.

[0071] The specific method is: assuming that the autonomous vehicle is 1 meter away from the pedestrian line traffic sign in front of each stop, a total of four pedestrian line traffic signs need to be started and stopped by the autonomous vehicle, and the simulation of the start and stop environment of the autonomous vehicle in the driving process is as follows Figure 4 .

[0072] Let the initial distance of the autonomous vehicle relative to the pedestrian line traffic sign be D0, and let the distance measurement error of the autonomous vehicle be D e , and let D T be the distance change threshold of the autonomous vehicle, which is related to the state estimation result of the Kalman filter constructed above, D T = K t ∫ΔΦ opt (t)dt, K t is a preset relevant proportion coefficient, and ΔΦ opt (t) is the difference value of the optimal attitude information vector obtained in the Kalman filtering in step 3 above, ΔΦ opt (t) = Φ opt (t1) - Φ opt (t0), t1-t0=20s. After the autonomous vehicle stops in front of the pedestrian traffic sign and starts driving again, every 20s interval, the position of the pedestrian line traffic sign d pcl recorded by GPS and the relative position information d GPS of the autonomous vehicle are obtained, and the two parameters are differentiated according to the similar coordinate transformation in steps 1 and 2 above, to obtain the relative distance change D i (i=1, 2, 3...10) of the autonomous vehicle during measurement, if the relative distance change of the position measured by GPS is relatively stable, that is, the difference between each adjacent difference from D1 to D 10 is less than D T , that is, Di ≤D T For each i = 1, 2, ..., 10, we assume that the error caused by the autonomous vehicle's start-stop operation is negligible. If D i >D T If i = 1, 2, ..., 10, then we assume that the start-stop error of the autonomous vehicle affects the subsequent trajectory estimation of the vehicle. Then, we obtain the calculated relative distance change D during the start-stop measurement of the autonomous vehicle. i The mean of (i = 1, 2, ..., 10) Use the mean at this time Subtract K t D0 yields the difference ΔD. If this difference is greater than the distance measurement error D of the autonomous vehicle... e Therefore, we believe that the errors caused by the start-stop of autonomous vehicles affect the estimation of the vehicle's subsequent driving position trajectory.

[0073] To address the impact of start-stop errors on the position trajectory, for the current driving segment, i.e., when the calculated position error is too large from the stop, the focus is on using wheel encoders for position trajectory estimation. It is also recognized that the autonomous vehicle accumulates errors due to complex obstacles in the city during the previous driving segment at the start-stop time. Therefore, the onboard GPS is reset and recalibrated to eliminate the accumulated errors from the previous segment. This ensures that when the autonomous vehicle travels a long distance, the problem of large discrepancies between the driving position distance and the actual position caused by positioning errors or other factors after each start-stop state is reduced.

[0074] The motion test results obtained by selecting 2 and 4 parking and starting points are as follows: Figure 5 and Figure 6 As shown, for tests with four parking and starting points, compared to tests with two parking and starting points, the trajectory estimation is dynamically updated each time, using the parking point as a reference point, through Extended Kalman Filter (EKF) and the specific methods described above. This ensures that subsequent trajectory points accurately reflect the actual position of the vehicle. After each parking, the accumulated error is limited to the current trajectory segment by recalibrating the position information, preventing it from propagating to subsequent trajectories, thus achieving a higher accuracy in trajectory estimation.

Claims

1. A method for reducing the cumulative error of autonomous vehicles based on Kalman filtering, characterized in that, The method includes: Step 1: Generate the first attitude information vector Φ of the autonomous vehicle in the navigation coordinate system from the vehicle's wheel encoders and inertial measurement unit. k1 ; Step 2: Obtain the second attitude information vector Φ from the vehicle's onboard GPS positioning system. k2 ; Step 3: Based on the constructed Kalman filter, predict and fuse the two attitude information vectors to obtain the corrected optimal attitude information vector Φ. opt As a result of Kalman filtering; Step 4: The Kalman filter result obtained in Step 3, i.e., the corrected optimal attitude information vector Φ, is then processed. opt The parameters are used as inputs into the parking and starting conditions of autonomous vehicles to update and calibrate the trajectory error. In step 4, the Kalman filter result obtained in step 3, i.e., the corrected optimal attitude information vector Φ, is used. opt The trajectory error is updated and calibrated by incorporating parameters into the parking and starting conditions of the autonomous vehicle; the feature is that: the distance between the autonomous vehicle and the pedestrian traffic sign when it stops is set to D0, and the distance measurement error of the autonomous vehicle is set to D... e Let D T The distance change threshold for the autonomous vehicle is defined, and this threshold is related to the state estimation result of the Kalman filter constructed above, D. T =K t ∫ΔΦ opt (t)dt,K t ΔΦ is the preset relevant proportionality coefficient. opt (t) represents the difference between the optimal attitude information vectors obtained in step 3 of the above Kalman filtering process, ΔΦ. opt (t)=Φ opt (t1)-Φ opt (t0), t1-t0=20s; After the autonomous vehicle stops and restarts before the pedestrian crossing traffic sign, the position d of the pedestrian crossing traffic sign is recorded by GPS every 20s. pcl Relative position information d of autonomous vehicles GPS Based on the coordinate transformations similar to those in steps 1 and 2 above, and by taking the difference, the relative distance change D during the start-stop measurement of the autonomous vehicle is obtained. i If D i >D T It is assumed that the error caused by the start-stop of the autonomous vehicle affects the estimation of the vehicle's subsequent driving position trajectory; then, the relative distance change D during the start-stop measurement of the autonomous vehicle is calculated. i mean Use the mean at this time Subtract K t D0 yields the difference ΔD. If the difference is greater than the distance measurement error D of the autonomous vehicle... e It is also believed that the error caused by the start-stop of autonomous vehicles affects the estimation of the vehicle's subsequent driving position trajectory. To address the impact of errors on the position trajectory, for the current driving segment, i.e., when the calculated position error is too large from the stop, the focus is on using wheel encoders for position trajectory estimation. It is also believed that because the measurement error accumulated during the previous driving segment was too large, the on-board GPS is reset and recalibrated to eliminate the accumulated error of the autonomous vehicle in the previous segment. This ensures that when the autonomous vehicle travels a long distance in multiple segments, the problem of large discrepancies between the driving position distance and the actual position caused by positioning errors or other factors is reduced after starting from the stop state.

2. The method for reducing the cumulative error of an autonomous vehicle based on Kalman filtering according to claim 1, characterized in that: In step 1, the first attitude information vector Φ of the autonomous vehicle in the navigation coordinate system is generated from the vehicle's wheel encoder. k1 ; Specifically, this includes: Step 11, obtaining the left and right wheel speed information v of the autonomous vehicle. l and v r Step 12: Generate the vehicle's lateral velocity v in the vehicle body coordinate system. x and vehicle longitudinal speed v y Step 13, the lateral velocity v of the vehicle mentioned above x and vehicle longitudinal speed v y Convert to velocity vector V in vehicle body coordinate system sdv V sdv =[v x ,v y ,0] T Step 14, convert the velocity vector V in the above vehicle body coordinate system. sdv Transformed into the first attitude information vector Φ in the GPS navigation coordinate system k1 .

3. The method for reducing the cumulative error of an autonomous vehicle based on Kalman filtering according to claim 1, characterized in that: In step 2, the second attitude information vector Φ is obtained from the vehicle's GPS positioning system. k2 ; Specifically, this includes: Step 21, setting the data transmission rate to receive vehicle GPS positioning data at 10Hz, including... For the latitude and longitude positioning information of autonomous vehicles, λ GPS Step 22: Convert the longitude positioning information of the autonomous vehicle into the slip angle α of the autonomous vehicle. sa Relative azimuth angle β ra and relative driving distance d GPS Step 23: Obtain the rate v in the eastward and northward directions of the information in the navigation coordinate system. ge and v gn The second attitude information vector Φ is obtained. k2 .

4. The method for reducing the cumulative error of an autonomous vehicle based on Kalman filtering according to claim 1, characterized in that: In step 3, the two types of sensor data are predicted and fused using the equations of the extended Kalman filter, including the prediction equation, state estimation equation, estimation mean square error equation, and Kalman gain equation, to obtain the optimal attitude information vector Φ. opt Specifically, it includes: (1) The prediction step uses the system's dynamic model and process covariance error to predict the system; the prediction equation is: (2) The state estimation update equation is: (3) The Kalman filter gain equation is: (4) The equation for estimating the root mean square error is: in, and It is the prior state estimation of the autonomous vehicle pose at time steps k-1 and k. and These are the pose state estimates of the autonomous vehicle at time steps k-1 and k, respectively. k-1 It is a control input. It is a nonlinear prediction function of the system state; μ k λ is the observation value at time step k, h is the nonlinear function of the known observation model of the system; k It is an adaptive factor; H k It is the Jacobian matrix of the observation model. It is its transpose, R k It is the custom measurement noise mean square error matrix at time step k; The prior error covariance matrix P at time step k k-1 It is the error covariance matrix at time step k-1.

5. The method for reducing the cumulative error of an autonomous vehicle based on Kalman filtering according to claim 4, characterized in that: Before using the extended Kalman filters listed above, the estimated position state of the autonomous vehicle is initialized as follows: The prediction mean squared error matrix is ​​initialized to in The initial velocity mean square error.

6. The method for reducing the cumulative error of an autonomous vehicle based on Kalman filtering according to claim 4, characterized in that: After initialization, the first attitude information vector Φ from step 1 above is... k1 Substitute the prior states into the state estimation update equation of the Kalman filter Then, based on the second attitude information vector Φ k2 The observed value μ at time step k of the Kalman filter k This allows for attitude estimation based on a pre-built Kalman filter, yielding... The corrected optimal attitude information vector Φ opt .

Images