CN106696956B - Vehicle track tracking device and method with path error correction - Google Patents

Abstract

A vehicle trajectory tracking device and method with path error correction, the method mainly includes: establishing a predicted trajectory according to the vehicle real-time information; determining a vehicle yaw rate threshold value according to the vehicle real-time information; calculating a steering angle corresponding to the vehicle yaw rate threshold value; estimating a lateral error correction amount corresponding to the steering angle, wherein when the lateral error correction amount is not greater than the error value between the target trajectory and the predicted trajectory, the steering angle corresponding to the lateral error correction amount is used to control the vehicle steering, and vice versa, when the lateral error correction amount is greater than the error value between the target trajectory and the predicted trajectory, the steering angle corresponding to the error value is used to control the vehicle steering. The present invention uses the error value as an error correction threshold, and when the correction threshold is not exceeded, the vehicle's deflection amplitude is controlled according to the error correction amount to avoid accidents caused by excessive steering amplitude of the vehicle, so that the vehicle can adapt to various road conditions to track the target trajectory and drive smoothly.

Description

Vehicle trajectory tracking device and method with path error correction

technical field

The invention relates to a vehicle trajectory tracking device and method, in particular to a vehicle trajectory tracking device and method with path error correction.

Background technique

Advanced Driver Assistance System (Advanced-Driver Assistance System, ADAS) is one of the main foundations of self-driving vehicles, such as obstacle recognition and sensing, assisted driving decision-making, workshop and in-vehicle communication, etc. The important subsystem of the vehicle mainly obtains the vehicle's own information and surrounding road condition information through various sensors, and then the electronic control unit analyzes and processes the information to generate corresponding control signals.

Please refer to FIG. 9 , which is a control logic schematic diagram of traditional vehicle trajectory tracking. It shows that a controller 100 in the advanced driving assistance system can compare the actual trajectory of the vehicle based on a known target trajectory 200 and feedback. Based on the error amount between the target trajectory 200 and the actual trajectory, a steering angle is estimated based on the error amount, and the vehicle is controlled by the steering angle to adjust the moving path appropriately, so that the vehicle can track the target trajectory 200 as much as possible to move.

Referring again to FIG. 10 , when the vehicle automatically adjusts its driving path according to the control logic shown in FIG. 9 , the movement 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, there is a significant gap between the vehicle and the target trajectory 200 when changing the path (such as turning). Another situation is the second correction path 320 , which means that during the correction process, the target trajectory 200 is corrected again after the deviation is too large due to the large amplitude.

The problem of the first correction path 310 or the second correction path 320 as shown in FIG. 10 will occur, because the controller 100 only compares the error amount between the target trajectory 200 and the actual trajectory, and does not consider the vehicle itself during the correction process. Dynamic changes. When the movement process of the vehicle is the same as the first corrected path 310 or the second corrected path 320 , due to the obvious and frequent deviation in a short period of time, it may cause discomfort to the passengers on the vehicle or the risk of the vehicle overturning.

Contents of the invention

The technical problem to be solved by the present invention is to provide a vehicle trajectory tracking method with path error correction that can maintain stable steering of the vehicle and avoid accidents such as vehicle overturning.

In order to achieve the above object, the present invention provides a vehicle trajectory tracking method with path error correction, comprising:

Capture vehicle real-time information and a target trajectory;

Establish a predicted trajectory according to the real-time information of the vehicle;

According to the real-time information of the vehicle, determine a vehicle yaw rate threshold;

calculating a steering angle corresponding to the vehicle yaw rate threshold;

Estimate the lateral error correction amount corresponding to the steering angle;

judging whether the lateral error correction amount is less than or equal to an error value between the target trajectory and the predicted trajectory;

When the lateral error correction amount is less than or equal to the error value between the target trajectory and the predicted trajectory, control the steering of the vehicle at the steering angle corresponding to the lateral error correction amount;

When the lateral error correction amount is greater than the error value between the target trajectory and the predicted trajectory, the steering of the vehicle is controlled at a steering angle corresponding to the error value.

In order to better achieve the above object, the present invention also provides a vehicle trajectory tracking device with path error correction, comprising:

A plurality of sensors are used to respectively sense a variety of different vehicle real-time information;

a trajectory prediction unit, which establishes a predicted trajectory according to the real-time information of the vehicles; and

A trajectory correction unit calculates a lateral error correction amount based on the real-time information of the vehicles, and judges whether the lateral error correction amount is greater than an error value, and the error value is the error between the target trajectory and the predicted trajectory; when the If the lateral error correction amount is less than or equal to the error value, the steering angle corresponding to the lateral error correction amount is output; when the lateral error correction amount is greater than the error value, the steering angle corresponding to the error value is output;

Wherein, when the trajectory correction unit calculates the error correction amount, it judges a vehicle yaw rate threshold value according to the real-time information of the vehicle, calculates a steering angle corresponding to the vehicle yaw rate threshold value, and estimates Calculate the lateral error correction amount.

Less than or equal to less than or equal to the technical effect of the present invention is:

The present invention regards the error value between the predicted trajectory and the target trajectory as a reasonable error correction threshold, and estimates an error correction amount by referring to the real-time information of the vehicle. On the premise of not exceeding the correction threshold, the error correction amount can be controlled The deflection range of the vehicle can avoid accidents caused by excessive steering range of the vehicle, so that the vehicle can track the target trajectory in response to various road conditions and drive smoothly.

The present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments, but not as a limitation of the present invention.

Description of drawings

Fig. 1 is a control logic schematic diagram of the present invention;

Fig. 2 is the schematic diagram of the path of vehicle trajectory tracking in the present invention;

Fig. 3 is a flow chart of the method of the present invention;

Fig. 4 is a schematic diagram of calculation of estimated and predicted trajectory of the present invention;

Fig. 5 is the schematic diagram that the present invention utilizes bicycle model to calculate steering angle;

Fig. 6 shows that the present invention is based on the amount of lateral error correction Schematic diagram of controlling the movement of the vehicle;

Fig. 7 is a schematic diagram of the present invention controlling vehicle movement according to the error value ε between the target trajectory and the predicted trajectory;

Fig. 8 is the block diagram of device of the present invention;

Fig. 9 is a control logic schematic diagram of traditional vehicle trajectory tracking;

FIG. 10 is a schematic diagram of a path for traditional vehicle trajectory tracking.

Among them, reference signs

10 sensors

20 trajectory prediction unit

30 track correction unit

100 controllers

200 target tracks

310 First corrected path

320 Second correction path

TP predicted trajectory

TG target trajectory

TE planning trajectory

Detailed ways

Below in conjunction with accompanying drawing, structural principle and working principle of the present invention are specifically described:

Please refer to Figures 1 and 2, the present invention establishes a predicted trajectory TP according to the real-time information of the vehicle, and compares the predicted trajectory TP with a target trajectory TG obtained to obtain an error value ε, and calculates the error value ε according to the real-time information of the vehicle. side error correction To decide which error value ê or lateral error correction amount should be used The corresponding steering angle controls the vehicle to change the path.

Firstly, Fig. 2 is described, the target trajectory TG is represented by a solid line, which can be established according to the prior art, so it will not be repeated; the predicted trajectory TP is established in the present invention, and will be further explained later; the present invention is based on the vehicle The lateral error correction calculated by real-time information of That is, it corresponds to a planned trajectory TE, wherein, when the vehicle is controlled to move forward, the predicted trajectory TP or the planned trajectory TE is selected according to the calculation results, so that the actual moving path of the vehicle is kept consistent with the target trajectory TG as much as possible. The detailed technique of the method of the present invention will be described in detail below.

Please refer to Fig. 3, method of the present invention comprises following steps:

S31: Retrieve vehicle real-time information and a target trajectory TG, wherein the vehicle real-time information may include but not limited to vehicle speed, lateral acceleration, driver's vehicle model, driving situation, etc.

S32: Establish a predicted trajectory TP, and use the real-time information of the vehicle to estimate the predicted trajectory TP. This embodiment is illustrated by taking the model shown in FIG. 4 as an example. The track center of the front wheel of the vehicle (shown in a rectangular pattern) is represented by (x, y) at its current position, and the predicted position at the next time is represented by said, among them D represents the length from the wheelbase center ( _x , y) to the steering instant center, and δf represents the angle of the front wheel rotation angle. Therefore, based on the historical trajectory of the vehicle from the past to the current position, the predicted position at different times can be continuously estimated In this way, a predicted trajectory TP is formed. If it is different from the target trajectory TG, there is an error value ê between the target trajectory TG and the predicted trajectory TP.

S33: Judging the vehicle yaw rate threshold value γ _th , judging the vehicle yaw rate threshold value γ _th corresponding to the current vehicle instant information through table lookup according to the vehicle instant information, because it is judged by using the table lookup method, it can be used Some vehicle models are equipped with a simulation program, and a comparison table is established in advance, and the comparison table records the vehicle yaw rate threshold value γ _th corresponding to different states of the vehicle. In this embodiment, the comparison table records vehicle yaw rate thresholds γ _th corresponding to different vehicle speeds, that is, looks up the table according to the instant vehicle speed to determine the yaw rate threshold γ _th corresponding to the instant vehicle speed.

S34: Calculate the steering angle, and calculate a corresponding steering angle δ _f according to the yaw rate threshold value γ _th obtained by looking up the table. In this embodiment, the bicycle model as shown in Figure 5 is used to calculate the steering angle δ _f according to the following formula :

in, Represents the yaw rate, L represents the length of the vehicle, v _x represents the longitudinal speed of the vehicle, and v _ch represents the speed of the vehicle.

S35: Estimate lateral error correction amount When the steering angle δ _f is calculated, combined with the instant vehicle speed, the lateral error correction amount is calculated It can be obtained by using the vehicle lateral motion state space equation listed below:

The meanings of each known parameter are as follows:

v _ch is vehicle speed v _x is longitudinal vehicle speed v _y is lateral vehicle speed

C _αf is the front wheel steering rigidity C _αr is the rear wheel steering rigidity

L is the length of the vehicle m is the mass of the vehicle

a is the length from the front axle to the center of gravity b is the length from the rear axle to the center of gravity

Iz is the moment of inertia y is the lateral displacement of the vehicle

ψ is the yaw angle r is the yaw rate δ _f is the front wheel rotation angle

Among them, the lateral error correction That is, in the above equation

S36: Determine whether the lateral error correction value e is less than or equal to the error value between the target trajectory and the predicted trajectory

S37: If the lateral error correction amount Less than or equal to the error value ê between the target trajectory and the predicted trajectory, the lateral error correction value The corresponding steering angle _δf controls the vehicle deflection. As shown in Figure 6, the error value ê between the target trajectory TG and the predicted trajectory TP is greater than the calculated lateral error correction Therefore, the lateral error correction amount can be used The corresponding steering angle _δf controls the vehicle so that the vehicle moves forward along the planned trajectory TE and approaches the target trajectory TG.

S38: If the lateral error correction amount is greater than the error value ê between the target trajectory and the predicted trajectory, the steering angle δ _{f_new} corresponding to the error value ê is used to control the vehicle deflection. As shown in Figure 7, if the calculated lateral error correction When it is greater than the error value ê, it is assumed that the error correction amount is still used The corresponding steering angle δ _f controls the vehicle to move forward along the planned trajectory TE. It can be clearly seen that the vehicle will deflect too far relative to the target trajectory TG, and it must be corrected back to the target trajectory TG again, so this steering angle δ _f will not be used. On the contrary, in this case, the corresponding steering angle δ _{f_new} will be back-calculated according to the error value ê between the target trajectory and the predicted trajectory, and the steering angle δ _{f_new} will be used to control the deflection of the vehicle.

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. The device includes:

A plurality of sensors 10 are used to sense a variety of different vehicle real-time information respectively. The types of the sensors 10 may include vehicle speed sensors, acceleration sensors, lenses, positioning devices (GPS), inertial measurement One or more combinations of different types such as IMUs;

A trajectory prediction unit 20, which establishes a predicted trajectory TP according to the real-time information of the vehicle; and

A trajectory correction unit 30, which calculates a lateral error correction amount according to the real-time information of the vehicle And judge the lateral error correction amount Whether it is greater than an error value ê, to determine the output corresponding to the lateral error correction amount The steering angle δ _f or the steering angle δ _{f_new} corresponding to the error value ê makes the vehicle change its path according to the output steering angle.

Wherein, the trajectory correction unit 30 can execute detailed processes such as steps S33-S38, so details are not repeated here. The trajectory prediction unit 20 and the trajectory correction unit 30 can be integrated into one unit, or realized by a single microprocessor.

In summary, the present invention establishes a predicted trajectory TP based on the real-time information of the vehicle, and compares the predicted trajectory TP with the obtained target trajectory TG to obtain an error value ê, which represents the maximum allowable steering of the vehicle. When the turning angle of the vehicle does not exceed the maximum allowable amount, it can ensure that the vehicle runs smoothly, avoiding dangerous situations such as vehicle overturning, and passengers can enjoy a more comfortable feeling. The present invention estimates an error correction amount by referring to the real-time information of the vehicle, and controls the vehicle according to the error correction amount under the premise that the vehicle rotation angle does not exceed the maximum allowable amount, so that its moving route is closer to a target track.

Certainly, the present invention also can have other multiple embodiments, without departing from the spirit and essence of the present invention, those skilled in the art can make various corresponding changes and deformations according to the present invention, but these corresponding Changes and deformations should all belong to the protection scope of the appended claims of the present invention.

Claims (10)

1. A method for tracking a vehicle trajectory with path error correction, comprising:

capturing vehicle instant information and a target track;

establishing a predicted track according to the vehicle instant information;

judging a vehicle yaw rate threshold value according to the vehicle instant information;

calculating a steering angle corresponding to the vehicle yaw rate threshold value;

estimating a lateral error correction corresponding to the steering angle;

judging whether the lateral error correction quantity is smaller than or equal to an error value between the target track and the predicted track;

when the lateral error correction is smaller than or equal to the error value of the target track and the predicted track, controlling the vehicle to steer by using the steering angle corresponding to the lateral error correction;

when the lateral error correction is larger than the error value of the target track and the predicted track, the steering angle corresponding to the error value is used for controlling the vehicle to steer.

2. The method of claim 1, wherein in the step of determining the threshold value of yaw rate, the corresponding threshold value of yaw rate is determined in a pre-established lookup table according to the vehicle instant message.

3. The method of claim 2, wherein the lookup table records vehicle yaw rate thresholds corresponding to different vehicle speeds, and the lookup table is performed by using an instantaneous vehicle speed to obtain the corresponding yaw rate threshold.

4. The method for tracking vehicle with path error correction according to any one of claims 1 to 3, wherein in the step of calculating the steering angle corresponding to the threshold value of the yaw rate of the vehicle, the steering angle is calculated according to the following formula:

wherein,representing yaw rate, L representing vehicle length, v_xRepresenting longitudinal vehicle speed, v_chRepresenting vehicle speed, δ_fIs the steering angle.

5. The method of claim 4, wherein in the step of estimating a lateral error correction amount corresponding to the steering angle, the lateral error correction amount is estimated according to the following equation:

wherein the parameters of the above equations represent:

v_chis the vehicle speed v_xFor longitudinal vehicle speed v_yFor sideways vehicle speed

C_αfSteering stiffness C for front wheels_αrFor rear wheel steering rigidity

L is vehicle length m is vehicle mass

a is the length from the front axle to the center of gravity, b is the length from the rear axle to the center of gravity

Iz is moment of inertia and y is vehicle lateral displacement

Psi is yaw angle r is yaw rateδ_fIs the front wheel turning angle

Is a lateral error correction.

6. The method of claim 5, wherein in the step of establishing the predicted trajectory, the current position of the vehicle is represented as (x, y) and the predicted position at the next time is represented as (x, y)WhereinD represents the center of the tread(x, y) to a steering instant center, δ_fRepresenting the angle of the front wheel turning angle.

7. A vehicle trajectory tracking device with path error correction, comprising:

the sensors are used for respectively sensing various vehicle instant messages;

a track prediction unit for establishing a predicted track according to the vehicle instant messages; and

a track correction unit, which calculates a lateral error correction according to the vehicle instant messages and judges whether the lateral error correction is larger than an error value, wherein the error value is an error between a target track and the predicted track; when the lateral error correction is smaller than or equal to the error value, outputting a steering angle corresponding to the lateral error correction; when the lateral error correction is larger than the error value, outputting a steering angle corresponding to the error value;

when the track correction unit calculates the error correction amount, a vehicle yaw rate threshold value is judged according to vehicle instant information, a steering angle corresponding to the vehicle yaw rate threshold value is calculated, and the lateral error correction amount is estimated according to the steering angle.

8. The device as claimed in claim 7, wherein the sensors include one or more of a vehicle speed sensor, an acceleration sensor, a lens, a positioning device and an inertia measurer.

9. The device of claim 7, wherein the trajectory prediction unit and the trajectory modification unit are integrated into a single microprocessor.

10. The device of claim 7, wherein the vehicle real-time information comprises one or more of vehicle speed, lateral acceleration, driver vehicle model, and driving situation.