TWI602725B - Method and apparatus for vehicle path tracking with error correction - Google Patents

Description

Vehicle trajectory tracking device and method with path error correction

The present invention relates to a vehicle trajectory tracking device and method, and more particularly to a vehicle trajectory tracking device and method with path error correction.

Advanced-Driver Assistance System (ADAS) is one of the main foundations in autonomous vehicles, such as obstacle recognition sensing, assisted driving decisions, workshop and in-vehicle communication, etc. The important subsystems mainly obtain the vehicle's own information and surrounding road condition information through various sensors, and then the electronic control unit performs information analysis and processing to generate corresponding control signals.

Please refer to FIG. 9 , which is a control logic diagram of a conventional vehicle trajectory tracking. The figure shows that a controller 100 in the advanced driving assistance system can compare the known target trajectory 200 and the actual trajectory of the vehicle traveled by the feedback. The amount of error between the target trajectory 200 and the actual trajectory is estimated based on the amount of error, and the steering angle is used to control the vehicle to appropriately adjust the moving path. The purpose is to expect the vehicle to track the target trajectory 200 as much as possible to perform motion.

Referring again to FIG. 10, when the vehicle automatically adjusts its own travel path according to the control logic in FIG. 9, the motion of the first modified path 310 or the second modified path 320 may actually occur. The first correction path 310 represents that during the correction process, because the correction is too slow, the vehicle has a significant difference from the target trajectory 200 when changing paths (such as steering). The other case is the second correction path 320, which means that during the correction process, the target track 200 is corrected again because the amplitude is too large and the offset is excessive.

The first correction path 310 or the second correction path 320 problem as described in FIG. 10 may occur because the controller 100 only compares the amount of error between the target trajectory 200 and the actual trajectory, and does not consider the vehicle itself during the correction process. Dynamic changes. When the motion process of the vehicle is like the first correction path 310 or the second correction path 320, the offset amplitude is obvious and frequent in a short time, which may cause discomfort to the passenger on the vehicle or risk of overturning the vehicle.

SUMMARY OF THE INVENTION A primary object of the present invention is to provide a vehicle trajectory tracking method that can maintain a stable steering of a vehicle and avoid accidents such as vehicle overturning.

In order to achieve the foregoing objective, the method of the present invention comprises: extracting real-time information of the vehicle and a target trajectory; establishing a predicted trajectory according to the real-time information of the vehicle; determining a yaw rate of the vehicle according to the instantaneous information of the vehicle; calculating the yaw of the vehicle a steering angle corresponding to the threshold value; estimating a lateral error correction amount corresponding to the steering angle; determining whether the lateral error correction amount is not greater than an error value between the target trajectory and the predicted trajectory; The error correction amount is not greater than the error value of the target trajectory and the predicted trajectory, and the steering angle is controlled by the steering angle corresponding to the lateral error correction amount; when the lateral error correction amount is greater than the error value of the target trajectory and the predicted trajectory, the error is The steering angle corresponding to the value controls the steering of the vehicle.

The error value of the predicted trajectory and the target trajectory is used as a reasonable error correction threshold, and an error correction amount is estimated by referring to the vehicle real-time information, and the error correction amount can be controlled according to the error correction amount without exceeding the correction threshold The deflection range of the vehicle avoids accidents caused by the excessive steering angle of the vehicle, so that the vehicle can smoothly track the target trajectory according to various road conditions.

Referring to FIG. 1 and FIG. 2, the present invention establishes a predicted trajectory T _P according to the instantaneous information of the vehicle, and the predicted trajectory T _{P is} compared with a target trajectory T _G obtained by the vehicle to obtain an error value ê, and according to the instant of the vehicle. Information to calculate the lateral error correction To determine whether the error value ê or the lateral error correction amount should be utilized The corresponding steering angle controls the vehicle change path.

First, FIG. 2, the target trajectory T _G is represented by a solid line, which can be established according to the prior art, and therefore will not be described again; the predicted trajectory T _P is established in the present invention, which will be further explained later; Lateral error correction calculated from the vehicle's real-time information That is, corresponding to a planned trajectory T _E , wherein, when controlling the vehicle to travel, the calculation result is selected to advance according to the predicted trajectory T _P or the planned trajectory T _E , so that the actual moving path of the vehicle is maintained as much as possible with the target trajectory T _{G is} consistent. Detailed techniques of the method of the present invention will be described in detail below.

Referring to FIG. 3, the method of the present invention includes the following steps:

S31: Capture vehicle real-time information and a target trajectory T _G , wherein the vehicle real-time information may include, but is not limited to, vehicle speed, lateral acceleration, driver vehicle model, driving situation, and the like.

S32: Establish a predicted trajectory T _P , and use the vehicle real-time information to estimate the predicted trajectory T _P . In this embodiment, the model shown in FIG. 4 is taken as an example to illustrate that the center of the wheel of the front wheel of the vehicle (indicated by a rectangular pattern) indicates its current position by (x, y), and the predicted position at the next time is Said that , , D represents the length of the track center (x, y) to the length of the steering instant, Represents the angle of the front wheel corner. Therefore, based on the historical trajectory of the vehicle from the past to the present position, the predicted position at different times can be continuously estimated. Thereby, a predicted trajectory T _{P is} formed, which is different from the target trajectory T _G , that is, there is an error value ê of the target trajectory T _G and the predicted trajectory T _P .

S33: Judging the vehicle yaw rate threshold According to the real-time information of the vehicle, the yaw rate of the vehicle corresponding to the current vehicle real-time information is judged by means of table lookup. Because it is judged by the look-up table method, the existing vehicle model can be equipped with a simulation program, and a comparison table is pre-established, and the comparison table records the vehicle yaw rate threshold corresponding to different states of the vehicle. . In this embodiment, the comparison table records the vehicle yaw rate threshold corresponding to different vehicle speeds respectively. , that is, according to the instantaneous speed of the vehicle, check the table to determine the yaw rate threshold corresponding to the instantaneous speed. .

S34: Calculate the steering angle, and the yaw rate threshold obtained according to the look-up table , calculate a corresponding steering angle In this embodiment, the bicycle model of FIG. 5 is used, and the steering angle is calculated according to the following formula. : among them, Represents the yaw rate, L represents the vehicle length, v _x represents the longitudinal speed, and v _ch represents the vehicle speed.

S35: Estimating the lateral error correction amount When the steering angle Calculated, combined with the instantaneous speed of the vehicle, calculate the lateral error correction It can be obtained by using the vehicle lateral motion state space equation listed below: The meanings of the known parameters are as follows: v _ch : vehicle speed v _x : longitudinal vehicle speed v _y : lateral vehicle speed C _{α f} : front wheel steering stiffness C _{α r} : rear wheel steering stiffness L : vehicle length m : vehicle mass a : front Axis to center of gravity length b : rear axle to center of gravity Iz : moment of inertia y: vehicle lateral displacement : yaw angle r : yaw rate ( ) : front wheel angle, where lateral error correction That is, in the above equation .

S36: Judging the lateral error correction amount Is it not greater than the error value of the target trajectory and the predicted trajectory ê ( ).

S37: If the lateral error correction amount Not more than the error value ê of the target trajectory and the predicted trajectory, with the lateral error correction amount Corresponding steering angle Control vehicle deflection. As shown in FIG. 6, the error value ê of the target trajectory T _G and the predicted trajectory T _P is greater than the calculated lateral error correction amount. Therefore, the lateral error correction amount can be utilized Corresponding steering angle The vehicle is controlled such that the vehicle advances along the planned trajectory T _E and is closer to the target trajectory T _G .

S38: If the lateral error correction amount The error value ê greater than the target trajectory and the predicted trajectory, and the steering angle corresponding to the error value ê Control vehicle deflection. As shown in Figure 7, if the calculated lateral error correction amount When the error value is greater than ê, it is assumed that the error correction amount is still used. Corresponding steering angle Controlling the vehicle proceeds along the planned trajectory T _E, apparent trajectory of the vehicle relative to the target T _G will be deflected too far, is bound to be corrected back again to the target track T _G, it will not use the steering angle . Conversely, in this case, the corresponding steering angle will be calculated based on the error value ê of the target trajectory and the predicted trajectory. With the steering angle Control vehicle deflection.

Please refer to FIG. 8 , which is a block diagram of a vehicle trajectory tracking device with path error correction according to the present invention. The device is installed in a vehicle and can perform the method shown in FIG. 3 , and the device includes:

The plurality of sensors 10 are respectively configured to respectively sense different kinds of vehicle real-time information, and the types of the sensors 10 may include a vehicle speed sensor, an acceleration sensor, a lens, a positioning device (GPS), and an inertial measurer. One or more combinations of different types such as (IMU);

a trajectory prediction unit 20, based on the vehicle real-time information, establishing a predicted trajectory T _P ;

A trajectory correction unit 30 calculates a lateral error correction amount based on the vehicle real-time information And determine the lateral error correction Whether it is greater than an error value ê to determine the output corresponding to the lateral error correction Steering angle Or the steering angle corresponding to the error value ê Let the vehicle change the path according to the steering angle of the output.

The trajectory correction unit 30 can perform the detailed processes of steps S33 to S38, and therefore will not be described again. The trajectory prediction unit 20 and the trajectory correction unit 30 may be integrated into the same unit or implemented by a single microprocessor.

In summary, the present invention establishes a predicted trajectory T _P according to the instantaneous information of the vehicle, and the predicted trajectory T _{P is} compared with a target trajectory T _G obtained by the vehicle to obtain an error value ê, which represents the steering of the vehicle. The maximum allowable amount, when the vehicle's corner does not exceed the maximum allowable amount, can ensure the smooth running of the vehicle, avoid dangerous situations such as vehicle overturning, and passengers can enjoy a more comfortable feeling. The present invention estimates an error correction amount with reference to the vehicle real-time information. Under the premise that the vehicle corner does not exceed the maximum allowable amount, the vehicle can be controlled according to the error correction amount to make the motion path closer to a target trajectory.

10‧‧‧ Sensors
20‧‧‧Track prediction unit
30‧‧‧Track Correction Unit
100‧‧‧ Controller
200‧‧‧target trajectory
310‧‧‧First amendment path
320‧‧‧Second amendment path
T _P ‧‧‧ predicted trajectory
T _G ‧‧‧target trajectory
T _E ‧‧‧ planning track

Figure 1: Schematic diagram of the control logic of the present invention. Figure 2: Schematic diagram of the path of vehicle trajectory tracking of the present invention. Figure 3: Flow chart of the method of the present invention. Figure 4: Schematic diagram of the calculation of the estimated predicted trajectory of the present invention. Figure 5: Schematic diagram of the present invention using the bicycle model to calculate the steering angle. Figure 6: The present invention is based on lateral error correction A schematic diagram of controlling the travel of the vehicle. Figure 7: Schematic diagram of the present invention for controlling vehicle travel based on the error value ê of the target trajectory and the predicted trajectory. Figure 8 is a block diagram of the apparatus of the present invention. Figure 9: Control logic diagram of traditional vehicle trajectory tracking. Figure 10: Schematic diagram of the path of traditional vehicle trajectory tracking.

Claims (10)

A vehicle trajectory tracking method with path error correction includes: capturing vehicle real-time information and a target trajectory; establishing a predicted trajectory according to the vehicle real-time information; and determining a vehicle yaw rate threshold based on the vehicle real-time information; Calculating a steering angle corresponding to the vehicle yaw rate threshold; estimating a lateral error correction amount corresponding to the steering angle; determining whether the lateral error correction amount is not greater than a ratio between the target trajectory and the predicted trajectory An error value; when the lateral error correction amount is not greater than an error value of the target trajectory and the predicted trajectory, controlling a vehicle steering with a steering angle corresponding to the lateral error correction amount; when the lateral error correction amount is greater than the target trajectory The error value with the predicted trajectory controls the steering of the vehicle with the steering angle corresponding to the error value. The vehicle trajectory tracking method with path error correction according to claim 1, in the step of determining the yaw rate threshold of the vehicle, determining the corresponding yaw rate according to the vehicle real-time information in a pre-established comparison table Threshold value. The vehicle trajectory tracking method with path error correction according to claim 2, the comparison table records the vehicle yaw rate threshold corresponding to different vehicle speeds, and uses a real vehicle speed to perform a table lookup to obtain the corresponding yaw rate threshold. The vehicle trajectory tracking method with path error correction according to any one of claims 1 to 3, wherein in the step of calculating a steering angle corresponding to the vehicle yaw rate threshold value, the steering angle is calculated according to the following formula: among them, Represents the yaw rate, L represents the vehicle length, v _x represents the longitudinal speed, v _ch represents the vehicle speed, and δ _f is the steering angle. In the vehicle trajectory tracking method with path error correction according to claim 4, in the step of estimating the lateral error correction amount corresponding to the steering angle, the lateral error correction amount is estimated according to the following equation: Among them, the parameters of the above equations represent: v _ch : vehicle speed v _x : longitudinal vehicle speed v _y : lateral vehicle speed C _{α f} : front wheel steering stiffness C _{α r} : rear wheel steering stiffness L : vehicle length m : vehicle mass a : Front axle to center of gravity length b : rear axle to center of gravity Iz : moment of inertia y: vehicle lateral displacement ψ : yaw angle r : yaw rate ( ) δ _f : front wheel angle : Lateral error correction amount. In the vehicle trajectory tracking method with path error correction according to claim 5, in the step of establishing the predicted trajectory, the current position of the vehicle is represented as (x, y), and the predicted position of the next time is expressed as ( , ),among them , , D represents the length of the track center (x, y) to a turning instant, and δ _f represents the angle of the front wheel corner. A vehicle trajectory tracking device with path error correction, comprising: a plurality of sensors for respectively sensing a plurality of different vehicle real-time information; a trajectory prediction unit for establishing a predicted trajectory according to the vehicle real-time information; and a trajectory correction unit for calculating a current information based on the vehicle The lateral error correction amount is determined, and whether the lateral error correction amount is greater than an error value, the error value is an error of a target trajectory and the predicted trajectory; when the lateral error correction amount is not greater than the error value, the side is output a steering angle corresponding to the error correction amount; when the lateral error correction amount is greater than the error value, outputting a steering angle corresponding to the error value; wherein the trajectory correction unit calculates the error correction amount according to the vehicle instant information And determining a vehicle yaw rate threshold value, calculating a steering angle corresponding to the vehicle yaw rate threshold value, and estimating the lateral error correction amount according to the steering angle. The vehicle trajectory tracking device with path error correction according to claim 7, wherein the plurality of sensors comprise a combination of one or more of a vehicle speed sensor, an acceleration sensor, a lens, a positioning device, and an inertial measurer. The vehicle trajectory tracking device with path error correction according to claim 7, wherein the trajectory prediction unit and the trajectory correction unit are integrated into a single microprocessor. The vehicle trajectory tracking device with path error correction according to claim 7, wherein the vehicle instant information includes a combination of one or more of a vehicle speed, a lateral acceleration, a driver vehicle model, and a driving situation.