CN108958245A - A kind of unmanned vehicle path tracking algorithm based on time series - Google Patents

Abstract

The invention discloses a kind of unmanned vehicle path tracking algorithm, it is applied in unmanned fleet's driving process, path trace when evolution, which comprises the judgment models of vehicle and expected path relative status；The mathematical model that car speed and slogan banner rate correction value based on time series calculate；To the calculation of vehicle subsequent time state.By using this algorithm, reduction widens practicability, error is avoided to amplify to hardware necessity degree of vehicle itself when unmanned vehicle can be made to carry out path trace.

Description

Unmanned vehicle path tracking algorithm based on time series

The technical field is as follows:

the invention relates to a path tracking algorithm, in particular to a calculation method for tracking an optimal path after an optimal path equation when a vehicle changes lanes is known.

Technical background:

the path tracking algorithm adopts the principle of time sequence correction, can realize path following under the condition of less input quantity, has low hardware requirement, and has higher adaptability and wide application range. With the progress of traffic, the new energy unmanned vehicle technology is bound to become the future traffic development trend. In some special environments, formation change of unmanned fleets is needed, and lanes need to be changed from vehicles in the change process. In the lane changing process of the automobile, an optimal track can be planned, but due to factors such as control errors and the like, the automobile is difficult to completely drive along the track route, and more deviations always occur. On the basis of the principle that the state of the vehicle, including the speed and the magnitude and the direction of the yaw rate, can be regarded as a certain value in a discrete time sequence, the invention designs an error correction method to ensure that the unmanned vehicle can keep better path tracking.

The invention discloses a path tracking method of an unmanned automobile (hereinafter referred to as a comparison document 1) by retrieving a Chinese invention patent No. CN201710481831.9, which is disclosed in No. 11 of 2017, 08 and month 11, wherein centimeter-level high-precision satellite differential positioning is adopted, a turning point is judged in a path, a turning path tracking method is adopted to drive when the turning point meets, and otherwise, a diameter path tracking method is adopted to drive. The chinese invention patent No. cn201710379676.x, published in 2017 at 9/12 (hereinafter referred to as "comparison document 2"), discloses a path tracking control method, which determines a lateral error through two preview points and a heading, thereby implementing a path tracking control method.

When the path tracking strategy is utilized, the requirement on hardware of a vehicle is high, the fault tolerance is poor, the economic applicability is low, and the path fitting degree is not good; by adopting the method, the related test quantity is large, error accumulation is easy to occur, and the economic applicability is poor.

The invention content is as follows:

the invention provides a path tracking method for an unmanned vehicle in a lane changing process, and aims to solve the problems that the existing path tracking method is complex in principle, large in calculated amount and difficult to maintain form stability between vehicles.

The specific operation steps of the invention are as shown in the attached figure 1 of the specification, and the following steps are carried out:

and S1, acquiring a known optimal lane-changing switching path equation and the position coordinates of the current vehicle, wherein A, B and C are taken as the starting position, the current position and the target position of the vehicle as shown in the specification and figure 2.

And S2, calculating the expected speed size and direction from the time parameter equation of the optimal path.

S3, comparing the position, the speed difference and the direction included angle of the vehicle relative to the path at the same moment; judging whether the difference exceeds a preset threshold value, if not, keeping the current state; if so, the routine goes to S4.

And S4, calculating a correction value according to the positive and negative values of the included angle and the speed difference, and superposing the correction value on the current state to be used as the initial state of the next moment, namely continuously correcting the state of each moment to approach or reach the expected state.

The specific steps of S1 are as follows:

and S11, establishing a first coordinate system xoy by taking the center of mass of the vehicle starting point as an origin and the speed direction as a y axis.

And S12, establishing a dynamic coordinate system x 'o' y 'by taking the center of mass in the vehicle path tracking process as the origin and the speed direction as the y' axis, and marking the dynamic coordinate system as a second coordinate system.

S13, the external input expected path curve equation is as follows:

R_ideal(t_i)＝(x_ideal(t_i),y_ideal(t_i)) (1)

wherein R is_ideal(t_i) Representing the ideal path curve, x, of the vehicle_ideal(t_i),y_ideal(t_i) Respectively, representing their coordinate values for the first coordinate system. The current speed and the yaw rate of the automobile are obtained by the feedback of the vehicle-mounted sensor.

The specific steps of S2 are as follows:

s21, calculating the gradient of the expected path curve equation relative to the first coordinate system, wherein the gradient is the expected speed at each moment and the direction is the expected speed direction v at each moment_ideal(t_i) The specific calculation is as follows:

s22, for a desired yaw rate ω_ideal(t_i) The calculation is as follows:

wherein, Delta theta_ideal(t_i) Indicating the expectation of two adjacent time instants, at represents the difference between two adjacent times,andtime t in x, y directions, respectively_iFirst derivative of the parametric equation of (1), v_x(t_i) And v_y(t_i) Is the velocity resolution for the corresponding direction.

The S3 concrete steps are as follows:

s31, the actual speed and the actual yaw rate v (t) of the vehicle can be obtained in the same time_i),ω(t_i) The yaw rate is positive clockwise and negative counterclockwise.

S32, calculating the intersection point of the target route and the second coordinate system, and making y_ideal(t_i) 0, the corresponding position abscissa x'_ideal(t_i) Artificially defining a smaller radius r>And 0, taking the center of mass of the vehicle as a circle with the radius r, and regarding the vehicle as being on the path when the path has an intersection point with the circle. The vehicle position determination condition is obtained:

s33, calculating the direction included angle between the actual speed and the expected speed of the slave vehicle in the formation changing process

S34, calculating the cosine of the included angle between the expected speed direction and the x' -axis direction of the second coordinate system as

Wherein e is_x′Is the unit vector of the x' axis, ε (t)_i) Is the angle between the desired speed direction and the x' -axis direction of the second coordinate system.

S35, setting an included angle threshold value theta_threshCldSetting a steering judgment condition

Δθ＝θ(t_i)-θ_threshold(7)

Yaw rate adjustment mode determination condition

ψ(t_i)＝cos(ε(t_i)) (8)

Order to

S36, setting a speed threshold value v_thresholdDetermination conditions for speed adjustment

Δv＝|v_ideal(t_i)|-|v(t_i)| (11)

f(t_i)＝max(|Δv|-v_threshold,0)·Δv (12)

Obtaining the judgment conditions required in the aboveAnd f (t)_i) After that, S4 is executed.

The detailed step of S4 is as follows:

s41, calculating the directional control function of speed and yaw rateThe specific calculation process is as follows:

s42, performing a resting calculation on the current state as a vehicle driving state at the next time, specifically calculating as follows:

wherein, g₁(t_i) And g₂(t_i) The adjustment functions are respectively corresponding to the speed and the yaw rate, and different calculation modes are provided for different types of vehicles; v (t)_i+1) And ω (t)_i+1) Is t_i+1The velocity at the moment and the yaw rate.

Description of the drawings:

FIG. 1: vehicle path tracking algorithm flow chart

FIG. 2: labeling schematic diagram for tracking various state quantities of vehicle path

A, B, C: vehicle departure point, current point and target point

R_ideal(t_i): expectation of following a path

θ(t_i): angle between actual speed and desired speed

ε(t_i): the desired speed direction forms an angle with the x' -axis direction of the second coordinate system

v₀,v(t_i),v_ideal(t_i): initial velocity, t_iActual speed and desired speed of time

e_x′: unit vector of x' axis.

Claims (3)

1. An unmanned vehicle path tracking algorithm based on time series changes the step length of the time series according to the speed, the position, the yaw rate and the road shape. And (3) externally inputting a vehicle path equation on a time series, and calculating the expected running state of the vehicle in real time, wherein the expected running state comprises vehicle position, speed and angular speed information. The correction amount for the vehicle actual state at the next time is obtained in conjunction with the vehicle actual state. Wide application range and high economic applicability.

2. The path tracking process of claim 1, wherein the vehicle steering processing equation is:

wherein,ti Δ θ is the angle between the desired and actual speed directions, ε (t)_i) The angle of the desired velocity direction on the x' axis.

3. The path tracking of claim 1, wherein the vehicle acceleration processing equation is:

f(t_i)＝max(|Δv|-v_thershold，0)·Δv

where Δ v is the difference between the desired velocity and the actual velocity, v_thersholdIs an adjustable threshold value, which is a constant.