Method and system for integrated path planning and path tracking control of autonomous vehicle

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) To explain the technical solutions in embodiments of the present disclosure or in the conventional art more clearly, the accompanying drawings required in the embodiments will be briefly described below. Apparently, the accompanying drawings in the following description show merely some embodiments of the present disclosure, and other accompanying drawings may be derived from these accompanying drawings by a person of ordinary skill in the art without creative efforts.

(2) FIG. 1 is a flowchart of a method for integrated path planning and path tracking control of an autonomous vehicle provided in the present disclosure.

(3) FIG. 2 is a schematic diagram of a vehicle dynamic model provided in the present disclosure.

(4) FIG. 3 is a chart of an elliptical vehicle envelope curve provided in the present disclosure.

(5) FIG. 4 is a structure diagram of a system for integrated path planning and path tracking control of an autonomous vehicle provided in the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(6) The technical solutions in the embodiments of the present disclosure will be clearly and completely described below with reference to the accompanying drawings. Apparently, the described embodiments are merely a part rather than all of the embodiments of the present disclosure. All other embodiments derived from the embodiments of the present disclosure by a person of ordinary skill in the art without creative efforts shall fall within the protection scope of the present disclosure.

(7) An objective of the present disclosure is to provide a method and system for integrated path planning and path tracking control of an autonomous vehicle, thereby realizing autonomous vehicle collision avoidance and improving a response speed of the autonomous vehicle.

(8) To make the above-mentioned objective, features, and advantages of the present disclosure clearer and more comprehensible, the present disclosure will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

(9) FIG. 1 is a flowchart of a method for integrated path planning and path tracking control of an autonomous vehicle provided in the present disclosure. As shown in FIG. 1, the method for integrated path planning and path tracking control of the autonomous vehicle includes the following steps:

(10) Step 101: five input control variables and eleven system state variables of an autonomous vehicle at current time are obtained, where the five input control variables include a front wheel steering angle and four control moments at wheels; and the eleven system state variables include a lateral vehicle velocity, a yaw rate, two sideslip angles of the wheels, four relative velocities at the wheels, a horizontal coordinate of generalized vehicle coordinates, a vertical coordinate of the generalized vehicle coordinates, and a vehicle heading angle;

(11) Before the Step 101, the method further includes the following steps:

(12) Wheel parameters and vehicle parameters of the autonomous vehicle are obtained, where the wheel parameters include a wheel radius, a wheel rotation speed, a longitudinal wheel velocity, a longitudinal wheel force, a torque applied by an individual wheel, and wheel rotation inertia; and the vehicle parameters include the vehicle yaw rate and the lateral vehicle velocity.

(13) A wheel dynamic model of each wheel of the autonomous vehicle is constructed according to the wheel parameters.

(14) A unified vehicle dynamic model is constructed, which will be applied to path planning and path tracking at the same time, as well as algorithms of subsequent collision avoidance and damage reduction, as shown in FIG. 2.

(15) The dynamic model of each wheel may be described by the following formula:

(16) ${\dot{u}}_{\mathrm{rij}}=-\frac{R^2}{I_{\omega }}{f}_{\mathrm{ij}}^x+\frac{R}{I_{\omega }}{T}_{\mathrm{ij}}-{\dot{u}}_{\mathrm{ij}}$

(17) where {dot over (u)}.sub.rij represents a component of a relative slip velocity of wheel (i,j) in a longitudinal direction (u), while {dot over (u)}.sub.ij is a longitudinal acceleration mapped at each corner (at each wheel), T.sub.ij is a total torque on each wheel, u.sub.ij is a mapped velocity of a vehicle center-of-mass velocity to a wheel, i=F/R (front/rear wheel), j=L/R (left/right wheel), u.sub.r=Rω−u.sub.t, R is a wheel radius, ω is a wheel rotation speed, u.sub.t is a longitudinal wheel velocity, and f.sub.ij.sup.x is a longitudinal wheel force. It should be noted here that the wheel force may be generated by any tire dynamic model and complex dynamic behaviors of a tire can be accurately described.

(18) A vehicle dynamic model of the autonomous vehicle is constructed according to the wheel parameters and the vehicle parameters.

(19) The vehicle dynamic model is established, which may be expressed as follows with the yaw rate {dot over (r)} and the lateral velocity {dot over (v)}:

(20) $\stackrel{.}{r}=\frac{1}{I_z}\left[l_f{f}_f^y\cos \delta -l_r{f}_r^y-\frac{d_f}{2}{\tilde{f}}_f^y\sin \delta +\frac{d_r}{2}{\tilde{f}}_r^x+\left(\frac{d_f}{2}\cos \delta +l_f\sin \delta \right)f_{\mathrm{fl}}^x+\left(-\frac{d_f}{2}\cos \delta +l_f\sin \delta \right)f_{fr}^x\right]$$\stackrel{.}{v}=\frac{1}{M}\left(f_f^y\cos \delta +f_r^y+f_f^x\sin \delta \right)-ru$

(21) where f.sub.*.sup.# (*=f, r, #=x, y) represents a longitudinal (lateral) force at a front (rear) wheel, while {tilde over (f)} is a difference between tire forces on two sides in the longitudinal (lateral) direction of the front (rear) axle, and δ is a wheel steering angle.

(22) Generalized vehicle coordinates of the autonomous vehicle in a geodetic coordinate system are determined based on the wheel dynamic models and the vehicle dynamic model.

(23) Where, the generalized vehicle coordinates are determined by the lateral and longitudinal velocities at the center of mass of the vehicle and a heading angle. In a vehicle collision control model, the generalized coordinates are regarded as one of system states, and control input is realized in accordance with this position to avoid a collision.

(24) Unlike existing path planning and path tracking algorithms, the present disclosure takes the coordinates of a vehicle in the geodetic coordinate system into account:
{dot over (X)}=u cos ψ−v sin ψ
{dot over (Y)}=u sin ψ+v cos ψ

(25) where X and Y are coordinates of the center of mass of the vehicle in the geodetic coordinate system, i.e., generalized vehicle coordinates, while u is a longitudinal velocity of the center of mass, v is a value of the lateral velocity of the center of mass of the vehicle, and ψ is a vehicle heading angle. To facilitate a controller to realize and solve the real-time problem, it may be locally linearized.

(26) Step 102: a vehicle path planning-tracking integrated state model is constructed according to the five input control variables and the eleven system state variables at the current time:

(27) $\stackrel{.}{x}=𝒜x+ℬu+{ℬ}_{\varrho }\varrho$$y=𝒞x;$$\mathrm{where},H=\left[\begin{array}{ccccc}m& 0& 0& 0& \\ 0& I_z& 0& 0& \\ -{\gamma }_f^v& -{\gamma }_f^r& 1& 0& 0_{4\times 7}\\ -{\gamma }_r^v& -{\gamma }_r^r& 0& 1& \\ & 0_{7\times 4}& & & I_{7\times 7}\end{array}\right],A=\left[\begin{array}{ccccc}0& -\mathrm{mu}& g& 0_{2\times 3}& \\ 0& 0& & & \\ & 0_{6\times 2}& 0_{6\times 6}& 0_{6\times 3}& \\ k_2& 0& & & k_1\\ {\stackrel{\_}{k}}_2& 0& 0_{3\times 6}& 0& {\stackrel{\_}{k}}_1\\ 0& 1& & & 0\end{array}\right],$$B=\left[\begin{array}{ccccccc}{\beta }_1& \frac{s\delta }{R_e}& \frac{s\delta }{R_e}& 0& 0& & \\ {\beta }_2& {\beta }_3& {\beta }_4& \frac{d_r}{2R_e}& \frac{-d_r}{2R_e}& 0& 0\\ & & & 0_{2\times 5}& & 0& 0\\ 0& \frac{R_e}{I_{\omega }}& 0& 0& 0& 0& 0\\ 0& 0& \frac{R_e}{I_{\omega }}& 0& 0& 0& 0\\ 0& 0& 0& \frac{R_e}{I_{\omega }}& 0& 0& 0\\ 0& 0& 0& 0& \frac{R_e}{I_{\omega }}& 0& 0\\ & & & 0_{3\times 5}& & & \end{array}\right],\mathrm{and}C=\left[\begin{array}{cc}{I}_{2\times 2}& 0_{2\times 9}\\ 0_{3\times 8}& I_{3\times 3}\end{array}\right].$

(28) The front wheel steering angle and the four control moments at wheels are selected as system control input u=[δ T.sub.fl T.sub.fr T.sub.rl T.sub.rr], and different control input combinations are adopted to achieve the same objective.

(29) System state variables are defined as x=[v r τ.sub.f τ.sub.r u.sub.r X Y ψ], namely a lateral vehicle velocity v, a yaw rate r, wheel sideslip angles (τ.sub.f is a front wheel sideslip angle, and τ.sub.r is a rear wheel sideslip angle), relative velocities u.sub.r at wheels (specifically including mapped velocities {dot over (u)}.sub.ij of four wheels), generalized vehicle coordinates (X,Y), and a vehicle heading angle ψ. In the above formulas, {dot over (x)} is a first-order differential of the system state variables, while y is a system output variable, is an uncontrolled system input coefficient matrix, is an uncontrolled system input state, m is a total vehicle mass, I.sub.z is rotation inertia of a vehicle body around the center of mass and perpendicular to the ground axis (z-axis), γ.sub.f.sup.v is a partial derivative

(30) $\frac{\partial {\tau }_f}{\partial v}$
of the front wheel sideslip angle to the lateral velocity, γ.sub.f.sup.r is a partial derivative

(31) $\frac{\partial {\tau }_f}{\partial r}$
of the front wheel sideslip angle to the yaw rate, γ.sub.r.sup.v is a partial derivative

(32) $\frac{\partial {\tau }_r}{\partial v}$
of the rear wheel sideslip angle to the lateral velocity, γ.sub.r.sup.v is a partial derivative

(33) $\frac{\partial {\tau }_f}{\partial v}$
of the rear wheel sideslip angle to the yaw rate, I.sub.7×7 is a 7×7 unit matrix, u is the longitudinal velocity of the center of mass, k1 and k2 are parameters of a conversion function, β.sub.1, β.sub.2, β.sub.3 and β.sub.4 are vehicle kinematic parameters in a system input matrix B, s is a sideslip angle of the center of mass of the vehicle, δ is a front wheel steering angle of the vehicle, Re is a wheel rolling radius, dr is a distance between the rear axle and the center of mass of the vehicle, I.sub.2×2 is a 2×2 unit matrix, I.sub.3×3 is a 3×3 unit matrix, T.sub.fl is a torque applied by the left front wheel, T.sub.fr is a torque applied by the right front wheel, T.sub.rl is a torque applied by the left rear wheel, and T.sub.rr is a torque applied by the right rear wheel.

(34) The system output includes the vehicle sideslip angle, the yaw rate, and the generalized vehicle coordinates. The core of the path planning-tracking integrated algorithm provided in the present disclosure relies on considering and controlling the generalized vehicle coordinates in the system state, and completely decoupling the system state by defining wheel sideslip angle variables and relative velocities of wheels and locally linearizing the generalized vehicle coordinates.

(35) To realize system control, the system is further discretized:
x(k+1)=.sub.dx(k)=.sub.du(k)=(k)
y(k)=.sub.dx(k)

(36) where a subscript d means discretization of the corresponding letter, and k denotes discrete time.

(37) Based on the discretized vehicle path planning-tracking integrated state model, a unified MPC is constructed depending on an activation condition of a TTC determination algorithm in the present disclosure to solve a constraint optimization problem. The control objective of integrated path planning and tracking is achieved by comparing the TTC with a time threshold to determine whether an objective function is switched and generating a control signal of an actuator of the vehicle.

(38) The path planning and tracking of an intelligent vehicle require triggering conditions, such as vehicle steering, existence of an obstacle, and a dynamic behavior(s) of a surrounding vehicle(s).

(39) Taking a scenario induced by a dynamic behavior of a surrounding vehicle for example, TTC, namely time to collision, is defined first. Some active behaviors such as steering may not be calculated depending on TTC, but subsequent steps can still be included into the claimed scope of the present disclosure.

(40) Before calculating TTC, a vehicle collision point needs to be defined first, and the TTC may be calculated according to this point and a current vehicle speed.

(41) Step 103: external contours of two autonomous vehicles are enveloped by using elliptical envelope curves to determine elliptical vehicle envelope curves of the two autonomous vehicles, respectively.

(42) The external contour of the vehicle is enveloped by using an ellipse. FIG. 3 is a chart of elliptical vehicle envelope curves provided in the present disclosure. As shown in FIG. 3, unlike circular, square, double circular envelops and the like adopted in existing papers, elliptical envelope curves used in the present disclosure may conform to actual vehicle characteristics better.

(43) A distribution diagram of vehicles surrounding the present vehicle may be plotted according to such information as center of mass and lengths of long and short axes of each surrounding vehicle, with each vehicle being represented by an elliptical envelope curve, where a vehicle width is twice the length of the short axis and a vehicle length is twice the length of the long axis. The speed and heading information of the surrounding vehicles are obtained by means of Internet of vehicles and advanced perceptive technology. In a condition of defining current time t and a time interval T, vehicle positions are predicted in future time series [t+T t+2T . . . t+nT] according to the driving states of current surrounding vehicles, i.e., the speed and heading information of the surrounding vehicles.

(44) Step 104: TTC between the autonomous vehicle and a vehicle having a distance smaller than a distance threshold away from the autonomous vehicle is determined according to the elliptical vehicle envelope curves and driving states of the vehicles.

(45) The distance between vehicles is calculated at each time interval T. Whether a collision will occur may be determined by using the following two methods according to the calculation capability of a vehicle-mounted computer:

(46) Method 1: intersection points of different ellipses are solved according to the elliptical envelope curves of vehicles.

(47) Method 2: a Euclidean distance between extended lines of different trajectories is calculated, and if the Euclidean distance is smaller than a distance threshold S.sub.c, it is determined that a collision may occur. The extended line extends in a velocity direction of a center-of-mass.

(48) When the result of determination by using one of the above two methods is that a collision may occur, it may be obtained that TTC=t+nT, where T is the time interval for calculation defined by a user, for example, 5 ms; and n denotes the number of T, which is a positive integer. For example, if the collision will occur in 1 second, a calculated result is to be 200T. Here, time to collision t.sub.c may be defined. When TTC<t.sub.c, the path planning-tracking algorithm is activated. A setting of t.sub.c is mainly considered to avoid unnecessary vehicle behaviors caused by frequent activation of the vehicle actuating system, thereby reducing driving comfort.

(49) Step 105: an objective function of a model prediction controller (MPC) is established according to the vehicle path planning-tracking integrated state model.

(50) A system MPC planning-controller is built according to vehicle dynamic characteristics, i.e., various vehicle parameters in the vehicle dynamic model. The objective function of the MPC planning-controller is defined as follows:
J=½[Q.Math.exp(−s.sub.p)+(u.sub.p−u.sub.p.sup.−).sup.TH(u.sub.p−u.sub.p.sup.−)+u.sub.p.sup.TMu.sub.p], p=1, . . . N.sub.p

(51) where S.sub.p represents a distance between the position of the center-of-mass of the present vehicle and that of a neighboring vehicle at time p, while u.sub.p is a control input at time p, u.sub.p.sup.− is a control input at previous time, Q, H and M represent a relative distance between vehicles, smoothness of the control input and a weight matrix of the control input, respectively, and N.sub.p is a predicted time domain length of the MPC.

(52) Step 106: the objective function is solved based on the TTC, and input control variables of the MPC at the next time are determined, where the input control variables are used to increase a distance between two autonomous vehicles with minimum control energy to control a motion trajectory of each autonomous vehicle in real time.

(53) The MPC problem may be concluded as follows based on the above objective function:

(54) $\begin{array}{cc}& u^{*}=\mathrm{argmin}J\\ \mathrm{subject}\mathrm{to}& x\left(k+1\right)={𝒜}_{d}x\left(k\right)+{ℬ}_{d}u\left(k\right)+{ℬ}_{\varrho }\varrho \left(k\right);\\ & \underset{\_}{u}\le u_p\le \stackrel{\_}{u};\\ & D.Math.x_k.Math.\le E+s_k,s_k\ge 0\end{array}$

(55) where u* represents the calculated control input, and constraints include a system state constraint, upper and lower limits of the control input, and a system state stability domain. The MPC problem can be solved to obtain the system input control variables at the next time, such that the distance between two vehicles may be maximized, the input may be as small as possible and a change rate of the input may be as low as possible, thereby reducing power consumption.

(56) According to the objective function of the MPC, the MPC planning-controller can achieve a maximum distance away from a neighboring vehicle by adjusting the output of a system actuator with the consideration of the system input and dynamic constraints, thereby achieving synchronous path planning and path tracking. As can be seen, compared with relatively independent existing path planning and path tracking methods, the proposed algorithm may achieve synchronous control of planning and tracking by using a single controller.

(57) The present disclosure can cover future vehicle driving states, and the updating performed at each control interval guarantees the vehicle state used in the algorithm is consistent with the actual state.

(58) FIG. 4 is a structure diagram of a system for integrated path planning and path tracking control of an autonomous vehicle provided in the present disclosure. As shown in FIG. 4, the system for the integrated path planning and path tracking control of the autonomous vehicle includes:

(59) a current-time parameter obtaining module 401, configured to obtain five input control variables and eleven system state variables of an autonomous vehicle at current time, where the five input control variables include a front wheel steering angle and four control moments at wheels; and the eleven system state variables include a lateral vehicle velocity, a yaw rate, two sideslip angles of the wheels, four relative velocities at wheels, a horizontal coordinate of generalized vehicle coordinates, a vertical coordinate of the generalized vehicle coordinates, and a vehicle heading angle;

(60) a vehicle path planning-tracking integrated state model constructing module 402, configured to construct a vehicle path planning-tracking integrated state model according to the five input control variables and the eleven system state variables at the current time;

(61) an elliptical vehicle envelop curve determining module 403, configured to envelop external contours of two autonomous vehicles by using elliptical envelope curves to determine elliptical vehicle envelope curves of the two autonomous vehicles, respectively;

(62) a TTC determining module 404, configured to determine the TTC between the autonomous vehicle and a vehicle having a distance smaller than a distance threshold away from the autonomous vehicle according to the elliptical vehicle envelope curves and driving states of the vehicles;

(63) an objective function establishing module 405, configured to establish an objective function of an MPC according to the vehicle path planning-tracking integrated state model; and

(64) a next-time input control variable determining module 406, configured to solve the objective function based on the TTC and determine input control variables at the next time, where the input control variables are used to increase a distance between two autonomous vehicles with minimum control energy to control a motion trajectory of the autonomous vehicle in real time.

(65) The system further includes: a wheel parameter and vehicle parameter obtaining module configured to obtain wheel parameters and vehicle parameters of the autonomous vehicle, where the wheel parameters include a wheel radius, a wheel rotation speed, a longitudinal wheel velocity, a longitudinal wheel force, a torque applied by an individual wheel, and wheel rotation inertia, and the vehicle parameters include the vehicle yaw rate and the lateral vehicle velocity; a wheel dynamic model constructing module configured to construct a wheel dynamic model of each wheel of the autonomous vehicle according to the wheel parameters; a vehicle dynamic model constructing module configured to construct a vehicle dynamic model of the autonomous vehicle according to the wheel parameters and the vehicle parameters; and a generalized vehicle coordinate determining module configured to determine generalized vehicle coordinates of the autonomous vehicle in a geodetic coordinate system based on the wheel dynamic models and the vehicle dynamic model.

(66) The system further includes: a discretizing module configured to discretize the vehicle path planning-tracking integrated state model to determine a discretized vehicle path planning-tracking integrated state model.

(67) The TTC determining module 404 in the present disclosure specifically includes: a intersection-point solving unit configured to solve intersection points of ellipses according to the elliptical vehicle envelope curves of the two autonomous vehicles; and a TTC determining unit configured to determine a collision area between the autonomous vehicle and the vehicle having the distance smaller than the distance threshold away from the autonomous vehicle according to the intersection points of ellipses.

(68) The TTC determining module 404 specifically includes: an extended line determining unit configured to determine a mass point of each autonomous vehicle according to the corresponding elliptical vehicle envelope curve and determine extended lines in velocity directions of two mass points; a Euclidean distance determining unit configured to determine a Euclidean distance between the two extended lines; and a TTC determining unit configured to determine that a collision occurs when the Euclidean distance is smaller than the distance threshold and obtain the TTC between the two autonomous vehicles.

(69) According to the present disclosure, with the consideration of vehicle dynamic characteristics and constraints, autonomous switching of the vehicle collision avoidance function can be achieved autonomously by means of coordination control on vehicle actuating submodules, such as braking, steering and driving, with no need for an additional path planning module.

(70) The embodiments are described herein in a progressive manner. Each embodiment focuses on the difference from other embodiment, and the same and similar parts between the embodiments may refer to each other. Since the system disclosed in the embodiments corresponds to the method disclosed in the embodiments, the description is relatively simple, and reference can be made to the method description.

(71) Specific examples are used herein to explain the principles and embodiments of the disclosure. The foregoing description of the embodiments is merely intended to help understand the method of the present disclosure and its core ideas; besides, various modifications may be made by a person of ordinary skill in the art to specific embodiments and the scope of application in accordance with the ideas of the present disclosure. In conclusion, the content of this specification shall not be construed as a limitation to the present disclosure.