An optimized algorithm for automatic parking dead reckoning

Abstract

The present invention discloses an optimized algorithm for automatic parking dead reckoning, which overcomes the problem in the prior art that errors occur when calculating the driving distance, coordinates, and heading angle in the mainstream dead reckoning scheme, resulting in the influence on the positioning accuracy. The method includes the following steps: S1: Eliminate the invalid wheel speed pulses and calculate the driving distance Δs using the valid wheel speed pulses; S2: Use the Kalman filtering technology to calculate the vehicle heading angle change amount #imgabs0#; S3: Calculate the vehicle positioning using the kinematic equation of the vehicle. For the low-speed scenario of automatic parking, Δs and #imgabs1# are optimized respectively to improve the accuracy of dead reckoning.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and particularly relates to an optimized algorithm for automatic parking dead reckoning. Background Art

[0002] The dead reckoning of a vehicle in an automatic parking scenario refers to calculating the horizontal and vertical coordinates and the heading angle of the vehicle relative to the zero position by using on-vehicle sensors. The existing dead reckoning solutions include the following several types:

[0003] One: Using differential GPS or a high-precision IMU (Inertial Measurement Unit). The former can directly output the absolute coordinates of the vehicle, but it cannot be used in places with obstructions, such as underground parking lots. The latter is not affected by the external environment and can calculate the positioning by integrating the lateral and longitudinal accelerations output by the IMU and the yaw angular velocity around the Z-axis. However, the high-precision IMU has a high cost and is not suitable for mass-produced vehicles to be equipped with, and it is not practical.

[0004] Two: Relying on the on-vehicle surround-view camera to identify the parking space position and perform visual dead reckoning. The existing surround-view camera on the vehicle can be used to collect images. However, the visual sensor is not suitable for use in parking spaces without painted lines, and the accuracy is easily affected by changes in light.

[0005] Three: It is the current mainstream dead reckoning solution. It calculates the driving distance by using the wheel speed pulses, calculates the change in the heading angle by integrating the yaw angular velocity signal output by the on-vehicle gyroscope, and then calculates the vehicle positioning by using the vehicle kinematic equation. This solution is not affected by the environment and light, and the cost of the sensor is low, and it can be used for large-scale mass production, solving the problems existing in the first and second solutions. However, this solution does not consider the situation that invalid pulses will appear in the wheel pulses output by the rotary encoder in the low-speed parking scenario, which will cause errors in distance calculation; when calculating the change in the vehicle heading angle, the yaw angular velocity is directly integrated, resulting in the calculation error of the heading angle gradually increasing with the increase in distance. Summary of the Invention

[0006] The object of the present invention is to overcome the problem that errors will occur when calculating the driving distance, coordinates, and heading angle in the mainstream dead reckoning solution in the prior art, resulting in the influence on the positioning accuracy. A optimized algorithm for automatic parking dead reckoning is provided. For the low-speed scenario of automatic parking, Δs and are respectively optimized to improve the accuracy of dead reckoning.

[0007] To achieve the above object, the present invention adopts the following technical solutions: An optimized algorithm for automatic parking dead reckoning, including the following steps:

[0008] S1: Eliminate invalid wheel speed pulses and use valid wheel speed pulses to calculate the traveled distance Δs;

[0009] S2: Use Kalman filtering technology to calculate the change in vehicle heading angle

[0010] S3: Calculate vehicle positioning using the vehicle's kinematic equations.

[0011] The present invention can calculate the speed of the vehicle based on the traditional dead reckoning algorithm, and calculate the speed of the vehicle based on the traditional dead reckoning algorithm. Optimize and filter out invalid sensor measurements to improve the accuracy of dead reckoning.

[0012] Preferably, the step S1 of eliminating invalid pulses includes:

[0013] S1.1: Determine whether the increase in the rear wheel pulse between two samplings is greater than 1. If so, remove the redundant pulses and only keep one pulse;

[0014] S1.2: Determine whether the number of pulses added by the wheel on one side is greater than 1 when no pulse appears on the wheel on the other side. If so, remove the redundant pulses and keep only one pulse.

[0015] According to calculations, if the number of pulses on a rear wheel increases by more than 1 between two samples, the required wheel speed is much greater than the maximum wheel speed of the outer wheel. Therefore, during parking, there will not be a situation where the number of pulses increases by more than 1 between two consecutive samples. If this happens, only one will be taken.

[0016] Moreover, if there is no increase in pulses on one wheel while the other wheel has increased by 2 or more pulses, this situation will only occur if the speed of the outer wheel is greater than 2 times the speed of the inner wheel, which is not realistic.

[0017] Preferably, the step S1.2 is further expressed as:

[0018] S1.2.1: Set the rear axle center speed range to [-VS, -a] and [a, VS], and determine whether the rear axle center speed is within the speed range;

[0019] S1.2.2: If it is, then you need to remove the redundant pulses and keep only one pulse. If it is not, then you do not need to remove the redundant pulses.

[0020] VS represents the maximum vehicle speed. The value of a can be different according to different vehicles, but they are all values close to 0. During the actual parking process, it is very easy for the vehicle to stop while moving forward and then reverse. In this case, it is possible that there is no pulse on one side while there are two pulses on the other side, or there are two pulses within one sampling period. Therefore, when filtering out invalid pulses, a speed range needs to be set.

[0021] Preferably, step S2 is further expressed as:

[0022] S2.1: Measure the yaw rate to obtain the measured value Z of the yaw rate;

[0023] S2.2: Use the Kalman filter to obtain the predicted value ω′ of the yaw rate;

[0024] S2.3: Use the Kalman filter to fuse the measured value Z and the predicted value ω′ to obtain the fused yaw rate ω1;

[0025] S2.4: Calculate the vehicle heading angle using the fused yaw rate ω1

[0026] Through the process of fusing the predicted yaw rate and the yaw rate measured by the gyroscope using the Kalman filter, the fused ω1 is obtained, and then the change in the heading angle is calculated to finally optimize the accuracy of dead reckoning.

[0027] Preferably, step S2.2 is further expressed as:

[0028] S2.2.1: Take the state variables where ω represents the yaw rate at the previous moment, represents the derivative of ω;

[0029] S2.2.2: Predict the state variables to obtain the predicted value of the yaw rate:

[0030] X′ = FX + u

[0031] P′ = FPF′ + Q

[0032] In the formula, F is the state transition matrix, u is the control input, P is the state covariance matrix, and Q is the process noise matrix;

[0033] S2.2.3: Calculate the difference y between the predicted value and the measured value: ]>

[0034] y = z - Hx′

[0035] In the formula, H is the measurement matrix,

[0036] S2.2.4: Fuse the predicted value with the measured value to obtain the yaw rate ω1 after fusion:

[0037]

[0038] Where:

[0039] K = P'H'S -1

[0040] S = HP'H' + R

[0041] In the formula, R represents the measurement noise matrix, and K represents the Kalman gain.

[0042] The present invention uses the Kalman filtering algorithm to fuse the measured value of the sensor with the predicted value according to the vehicle state, obtain the optimal estimate of the yaw rate, and then perform integral operation, which can greatly increase the estimation accuracy.

[0043] Preferably, the step S3 is further expressed as:

[0044] S3.1: Establish the vehicle kinematic equation;

[0045] S3.2: Discretize the vehicle kinematic equation and convert it into a recurrence formula independent of the vehicle speed, and calculate the current vehicle coordinates.

[0046] Therefore, the present invention has the following beneficial effects: 1. Based on the traditional dead reckoning algorithm, for the low-speed scenario of automatic parking, optimize Δs and respectively to improve the accuracy of dead reckoning; 2. Can filter out invalid sensor measurement values when calculating Δs to reduce the distance calculation error; 3. Can use the Kalman filtering algorithm when calculating , fuse the measured value of the sensor with the predicted value according to the vehicle state, obtain the optimal estimate of the yaw rate, and then perform integral operation to increase the estimation accuracy. Brief Description of the Drawings

[0047] Figure 1 is the specific operation flowchart of the method of the present invention.

[0048] Figure 2 is the schematic diagram of the wheel rotation during parking in the present invention.

[0049] Figure 3 is the schematic diagram of the vehicle coordinate system principle during parking in the present invention. Detailed Embodiment

[0050] The present invention will be further described in detail below in conjunction with the drawings and specific embodiments:

[0051] As Figure 1 shown in the embodiments, an optimized algorithm for automatic parking dead reckoning can be seen. Its operation process is as follows: Step 1, eliminate the invalid wheel speed pulses and calculate the traveled distance Δs using the valid wheel speed pulses; Step 2, use the Kalman filtering technology to calculate the vehicle heading angle change Step 3, calculate the vehicle positioning using the kinematic equation of the vehicle. The present invention can, based on the traditional dead reckoning algorithm, optimize Δs and respectively for the low-speed scenario of automatic parking, filter out invalid sensor measurement values, and improve the accuracy of dead reckoning.

[0052] The technical solution of the present application will be further described below through specific examples.

[0053] In this embodiment, the maximum vehicle speed is Vehicle Speed (VS) = 2 km / h, the controller sampling time is 20 ms, the maximum steering wheel angle is Steer Wheel Angle = 500 degrees, the transmission ratio of the powertrain is Steer Ratio = 15, the wheelbase is L = 2.8 m, and the track width is Wk = 1.6 m.

[0054] Without considering sensor noise and failure, the distances traveled by the two rear wheels are calculated respectively through the rotary encoders of the two rear wheels. The calculation formula is as follows:

[0055] Δs = NumOfPulse * DistancePerGear

[0056] DistancePerGear = C tire / NumOfGear

[0057] In the formula, NumOfPulse is the number of pulses traveled by the wheel in two sampling periods, DistancePerGear refers to the distance represented by each pulse of the encoder, C tire refers to the rolling circumference of the tire, and NumOfGear refers to the number of teeth of the rotary encoder in one week, which is fixed for the same vehicle.

[0058] Sampling refers to the controller collecting the number of pulses, and the distance traveled by the center point of the rear axle is the average of the distances traveled by the two rear wheels.

[0059] First step: Eliminate the invalid wheel speed pulses and calculate the traveled distance Δs using the valid wheel speed pulses

[0060] In the parking condition, there are scenarios of frequent starts and stops and switching between forward and reverse. For common optical rotary encoders, invalid pulses will occur in low-speed scenarios. That is, actually the wheel only rotates by 1 tooth, but the sensor emits 2 pulse signals, resulting in an error in Δs.

[0061] Consider the most special case, that is, the vehicle travels at the maximum vehicle speed of 2 km / h and the maximum steering angle of 500 degrees, as Figure 2 shown. When the vehicle's steering wheel is at the maximum steering angle, between two sampling time points, the front and rear positions of the vehicle's rear axle travel. In the figure, T0 refers to the first sampling time, T1 refers to the second sampling time, and R 后轴中心端 , Ri, and Ro respectively refer to the driving radii of the center point of the rear axle, the inner wheel, and the outer wheel. Within the time of Δt, the angles turned by the inner and outer wheels are the same, but due to different radii, the driving path of the outer wheel is the largest.

[0062] According to the Ackermann steering relationship, the turning radius of the center of the rear axle is:

[0063] R 后轴中心端 = L / (tan(SteerWheelAngle / SteerRatio))

[0064] Substituting the data in this embodiment, we get:

[0065]

[0066] And the driving radius of the outer side is:

[0067] Ro = R 后轴中心端 + Wk / 2

[0068] Substituting the data in this embodiment, we get:

[0069] Ro = 4 + 0.8 = 4.8 m

[0070] Therefore, the speed of the outer wheel is:

[0071] Speed_Ro = VehicleSpeed × Ro / R

[0072] Substituting the data in this embodiment, we get:

[0073]

[0074] Substituting the data in this embodiment, it can be obtained that when the vehicle travels at the maximum vehicle speed of 2 km / h and the maximum steering angle of 500 degrees, the speed of the outer wheel is 2.4 km / h.

[0075] (1) If the increase in the number of pulses of a certain rear wheel is > 1 between two samplings, the required wheel speed at this time (taking an increase of two pulses in the rear wheel as an example) is:

[0076] Vehicle_wheel = DistancePerGear × 2 / Δt

[0077] In the formula, 2 represents an increase of two pulses in the rear wheel, Δt represents the sampling interval time, which is 20 ms in this embodiment, and DistancePerGear is taken as 4 in this embodiment.

[0078] It is obtained that Vehicle_wheel = 2 m / s = 7.2 km / h, which is much greater than 2.4 km / h. Therefore, during the parking process, there will be no situation where the number of pulses increases > 1 in two consecutive samplings. If this occurs, the redundant pulses need to be removed and only one pulse is retained.

[0079] (2) When there is no increase in pulses on one side of the wheel, the number of pulses on the other side has increased by 2 or more.

[0080] The running radius of the inner wheel:

[0081] Ri = R 后轴中心端 -Wk / 2

[0082] Substituting the data in this embodiment, we get:

[0083] Ri = 4 - 0.8 = 3.2 m

[0084] Therefore, the speed of the inner wheel is:

[0085] Speed_Ri = VehicleSpeed × Ri / R

[0086] Substituting the data in this embodiment, we get:

[0087]

[0088] It is obtained that the speed of the inner wheel is 1.6 km / h. Only when the speed of the outer wheel is > 2 times the speed of the inner wheel can the above situation occur. Therefore, if the number of pulses increased on the other side is greater than 1 when there is no pulse on one side of the wheel, the redundant pulses are removed and only one pulse is retained, thereby filtering out invalid pulses.

[0089] However, during the actual parking process, it is very easy for the vehicle to stop while moving forward and then reverse. In this case, it is possible that no pulse appears on one side while two pulses appear on the other side, or two pulses appear within one sampling period. Therefore, when filtering out invalid pulses, a speed range is set. In this embodiment, the speed range is: [0.2 km / h, 2 km / h] or [-2 km / h, -0.2 km / h].

[0090] This speed range is the speed of the center point of the rear axle. The speed of the center point of the rear wheels is the average of the speeds of the inner and outer wheels. That is, if the speed of the center point of the rear axle is within the range of [0.2 km / h, 2 km / h] or [-2 km / h, -0.2 km / h], and no pulse appears on one side while two pulses appear on the other side, or two pulses appear within one sampling period, then the invalid pulses need to be removed; if the speed of the center point of the rear axle is within [-0.2 km / h, 0.2 km / h], and no pulse appears on one side while two pulses appear on the other side, or two pulses appear within one sampling period, then the invalid pulses do not need to be removed.

[0091] Step 2: Use the Kalman filtering technology to calculate the change in the vehicle's heading angle

[0092] Take the state quantity where ω represents the fused yaw rate obtained at the previous moment, represents the derivative of ω.

[0093] The in-vehicle IMU can measure the longitudinal acceleration a of the vehicle. In the case of low speed, ignoring the tire side slip angle and the center of mass side slip angle of the vehicle, then:

[0094]

[0095] where, δ is the front wheel steering angle.

[0096] Use the Kalman filter to predict the state quantity to obtain the predicted value of the yaw rate:

[0097] X′ = FX + u

[0098] P′ = FPF′ + Q

[0099] where F is the state transition matrix, u is the control input, P is the state covariance matrix, and Q is the process noise matrix.

[0100] Calculate the difference y between the predicted value and the measured value:

[0101] y = z - HX′

[0102] In the formula, H is the measurement matrix,

[0103] The predicted value and the measured value are fused to obtain the yaw rate ω1 after fusion:

[0104]

[0105] P1 = (I - KH)P′

[0106] Where:

[0107] K = P′H′S -1

[0108] S = HP′H′ + R1

[0109] In the formula, R1 represents the measurement noise matrix, K represents the Kalman gain, and this process is continuously repeated to obtain the real-time yaw rate, where X1 represents X in the next period and P1 represents P in the next period.

[0110] Step 3: Calculate the vehicle positioning using the kinematic equation of the vehicle

[0111] The vehicle coordinate system is as Figure 3 shown, and the kinematic equation of the vehicle is as follows:

[0112]

[0113] In the formula, x, y, respectively refer to the abscissa, ordinate, and heading angle of the center point of the vehicle's rear axle, v is the vehicle speed at the center point of the rear axle, and the coordinate origin refers to the position at the program initialization, respectively represent the derivatives of x, y, respectively.

[0114] The kinematic equation of the vehicle is discretized and transformed into a recurrence formula independent of the vehicle speed v as follows:

[0115]

[0116] Where, Thus, the current coordinates of the vehicle are obtained.

[0117] In the formula, x t+1 、y t+1 、 refer to the current x coordinate, y coordinate, and heading angle, x t 、y t 、 refer to the x coordinate, y coordinate, and heading angle in the previous period, and Δs refers to the distance change of the wheels between two sampling periods, It refers to the change in the heading angle between two sampling periods.

[0118] Based on the traditional dead reckoning algorithm, for the low-speed scenario of automatic parking, this application designs algorithms respectively for Δs and to optimize, filter out invalid sensor measurement values, and improve the accuracy of dead reckoning. For Δs, the traditional algorithm calculates the distance traveled by the wheels based on the pulse changes of the two rear wheels, and then takes the average as the distance traveled by the center point of the rear axle. However, it does not consider the possible invalid pulses of the sensors. The rotary encoder may generate invalid pulses under low-speed working conditions, that is, the actual traveling distance is only the distance of one sensor tooth, but two or more pulse signals are sent out, which will cause errors in distance calculation. In view of this, the present invention combines the characteristics of the low-speed working condition of parking and designs an algorithm to filter out invalid pulses.

[0119] For The in-vehicle gyroscope can directly measure the yaw angular velocity of the vehicle. The traditional algorithm directly integrates the measured value to obtain the change in the vehicle's heading angle, that is, However, there is an error between the gyroscope data and the true value, and this error will be amplified during the integration process. Therefore, the error of the traditional algorithm becomes larger as the distance of dead reckoning is farther. The present invention uses the Kalman filter algorithm to fuse the measured value of the sensor and the predicted value based on the vehicle state to obtain the optimal estimate of the yaw angular velocity, and then performs the integration operation, which can make The estimation accuracy of is greatly increased.

[0120] The above-described embodiments are only a preferred solution of the present invention, and do not impose any form of limitation on the present invention. There are other variants and modifications without exceeding the technical solutions recorded in the claims.

Claims

1. An optimized algorithm for automatic parking dead reckoning, characterized in that, It includes the following steps: S1: Eliminate invalid wheel speed pulses and calculate the driving distance Δs using valid wheel speed pulses. Eliminating invalid pulses includes: S1.1: Determine whether the increase in rear wheel pulses between two samplings is greater than 1. If so, eliminate the redundant pulses and only retain one pulse; S1.2: Determine whether the number of pulses increased by the other wheel is greater than 1 when there are no pulses on one side of the wheels. If so, eliminate the redundant pulses and only retain one pulse; S2: Calculate the change in vehicle heading angle using Kalman filtering technology S3: Calculate vehicle positioning using the kinematic equation of the vehicle.

2. The optimized algorithm for automatic parking dead reckoning according to claim 1, characterized in that, In step S1 described above, eliminating invalid pulses includes: S1.1: Determine whether the increase in rear wheel pulses between two samplings is greater than 1. If so, eliminate the redundant pulses and only retain one pulse; S1.2: Determine whether the number of pulses increased by the other wheel is greater than 1 when there are no pulses on one side of the wheels. If so, eliminate the redundant pulses and only retain one pulse.

3. An optimized algorithm for automatic parking dead reckoning according to claim 2, characterized in that Step S1.2 described above is further expressed as: S1.2.1: Set the speed intervals of the center point of the rear axle [-VS, -a], [a, VS], and determine whether the speed of the center point of the rear axle is within the speed interval; where VS represents the maximum speed of the vehicle, and a has different values according to different vehicles, but they are all values greater than 0; S1.2.2: If it is within, then redundant pulses need to be eliminated and only one pulse is retained. If not, redundant pulses do not need to be eliminated.

4. An optimized algorithm for automatic parking dead reckoning according to claim 1 or 2 or 3, characterized in that, Step S2 described above is further expressed as: S2.1: Measure the yaw angular velocity to obtain the measured value Z of the yaw angular velocity; S2.2: Use Kalman filtering to obtain the predicted value ω′ of the yaw angular velocity; S2.3: Use Kalman filtering to fuse the measured value Z and the predicted value ω′ to obtain the fused yaw angular velocity ω1; S2.4: Calculate the vehicle heading angle using the fused yaw rate ω1 5. An optimized algorithm for automatic parking dead reckoning according to claim 4, characterized in that Step S2.2 described above is further expressed as: S2.2.1: Obtain the state quantity where ω represents the yaw rate at the previous moment, represents the derivative of ω; S2.2.2: Predict the state quantity to obtain the predicted value of the yaw angular velocity: X′ = FX + u P′ = FPF′ + Q where F is the state transition matrix, u is the control input, P is the state covariance matrix, and Q is the process noise matrix; S2.2.3: Calculate the difference y between the predicted value and the measured value: y = z - HX′ where H is the measurement matrix, S2.2.4: Fuse the predicted value and the measured value to obtain the fused yaw angular velocity ω1: Where: K = P'H'S -1 S = HP′H′ + R1 In the formula, R1 represents the measurement noise matrix, and K represents the Kalman gain.

6. An optimized algorithm for automatic parking dead reckoning according to claim 1 or 2, characterized in that Step S3 described above is further expressed as: S3.1: Establish the kinematic equation of the vehicle; S3.2: Discretize the kinematic equation of the vehicle and convert it into a recurrence formula independent of vehicle speed, and calculate the current vehicle coordinates.

Images