System and method for trajectory estimation

Abstract

A system for estimating a trajectory of a vehicle includes vehicle state sensors detecting a state of movement of the vehicle and environment sensors monitoring an environment of the vehicle. A free space module determines a free space corridor based on the detected state of movement of the vehicle and the monitored environment of the vehicle. A goal point determination module determines at least one goal point for the trajectory to be estimated based on the free space corridor. A trajectory planning module estimates a reference trajectory based on the at least one goal point and calculates a deviation between an actual trajectory and the reference trajectory of the vehicle. A trajectory control module minimizes the deviation between the actual trajectory and the reference trajectory of the vehicle and outputs optimal control parameters for the movement of the vehicle based on the minimized deviation.

Claims

1. A system for estimating a trajectory of a vehicle, comprising: vehicle state sensors detecting a state of movement of the vehicle, environment sensors monitoring an environment of the vehicle, a free space module which determines a free space corridor based on the detected state of movement of the vehicle and based on the monitored environment of the vehicle, a goal point determination module which determines at least one goal point for the trajectory based on the free space corridor, a trajectory planning module which estimates a reference trajectory based on the at least one goal point and which calculates a deviation between an actual trajectory and the reference trajectory of the vehicle, and a trajectory control module which minimizes the deviation between the actual trajectory and the reference trajectory of the vehicle and which outputs optimal control parameters for movement of the vehicle based on the minimized deviation, wherein the goal point determination module determines a primary goal point based on a maximum range of perception capability of the environment sensors and at least one backup goal point based on a reduced range that is smaller than the maximum range of perception capability, the trajectory planning module estimates the reference trajectory based on the primary goal point and checks the reference trajectory with respect to dynamic constraints and to constraints related to the free space corridor, and the trajectory planning module estimates a revised reference trajectory based on the backup goal point if the reference trajectory based on the primary goal point violates the dynamic constraints or the constraints related to the free space corridor.

2. The system according to claim 1, wherein the trajectory planning module determines if the deviation between the actual trajectory and the reference trajectory is greater than a predetermined threshold, determines a remaining steering intervention time, and estimates an updated reference trajectory based on the remaining steering intervention time if the deviation is greater than the predetermined threshold.

3. The system according to claim 1, comprising a lateral error determination module which determines a lateral error between a lateral position of the vehicle derived from the state of movement of the vehicle and an expected lateral position, and wherein the goal point determination module determines the at least one goal point additionally based on the lateral error.

4. The system according to claim 1, wherein the trajectory control module minimizes the deviation between the actual trajectory and the reference trajectory of the vehicle by minimizing a cost function which is based on a single track model of the vehicle.

5. The system according to claim 1, wherein the trajectory control module estimates a compensating curvature which is applied to the actual trajectory in order to redirect the vehicle to the reference trajectory if the deviation between the actual trajectory and the reference trajectory of the vehicle increases.

6. The system according to claim 1, wherein the vehicle state sensors comprise a global positioning system and a inertial measurement unit, and/or wherein the environment sensors comprise an optical camera, a Radar system and/or a Lidar System.

7. A method for estimating a trajectory of a vehicle, the method comprising: detecting a state of movement of the vehicle by using vehicle state sensors, monitoring an environment of the vehicle by using environment sensors, determining a free space corridor based on the detected state of movement of the vehicle and based on the monitored environment of the vehicle, determining at least one goal point for the trajectory based on the free space corridor, estimating a reference trajectory based on the at least one goal point, calculating a deviation between an actual trajectory and the reference trajectory of the vehicle, minimizing the deviation between the actual trajectory and the reference trajectory of the vehicle, and providing optimal control parameters for movement of the vehicle based the minimized deviation, wherein determining the at least one goal point comprises determining a primary goal point based on a maximum range of perception capability of the environment sensors and at least one backup goal point based on a reduced range that is smaller than the maximum range of perception capability, and estimating the reference trajectory comprises estimating the reference trajectory based on the primary goal point and checking the reference trajectory with respect to dynamic constraints and to constraints related to the free space corridor, and estimating a revised reference trajectory based on the backup goal point if the reference trajectory based on the primary goal point violates the dynamic constraints or the constraints related to the free space corridor.

8. The method according to claim 7, wherein estimating the reference trajectory comprises: determining if the deviation between the actual trajectory and the reference trajectory is greater than a predetermined threshold, and if the deviation is greater than the predetermined threshold: determining a remaining steering intervention time, and estimating an updated reference trajectory based on the remaining steering intervention time.

9. The method according to claim 7, comprising: determining a lateral error between a lateral position of the vehicle derived from the state of movement of the vehicle and an expected lateral position, and determining the at least one goal point additionally based on the lateral error.

10. The method according to claim 7, wherein minimizing the deviation between the actual trajectory and the reference trajectory of the vehicle comprises minimizing a cost function which is based on a single track model of the vehicle.

11. The method according to claim 7, wherein providing optimal control parameters for the movement of the vehicle comprises estimating a compensating curvature which is applied to the actual trajectory in order to redirect the vehicle to the reference trajectory if the deviation between the actual trajectory and the reference trajectory of the vehicle increases.

12. The method according to claim 7, wherein the reference trajectory is estimated when a lateral acceleration of the vehicle is smaller than an acceleration threshold and the at least one goal point is in compliance with predefined constraints.

13. The method according to claim 7, wherein determining the free space corridor for the vehicle comprises applying a convex hull technique.

14. A non-transitory computer readable medium comprising instructions for carrying out the method of claim 7.

Description

DRAWINGS

(1) Exemplary embodiments and functions of the present disclosure are described below in conjunction with the following drawings, in which:

(2) FIG. 1(a) depicts a lane keeping assistance functionality and an adaptive groove control functionality,

(3) FIG. 1(b) depicts trajectory planning for a host vehicle,

(4) FIG. 2 depicts a block diagram of a system for estimating a trajectory of a host vehicle,

(5) FIGS. 3(a)-(b) depict an occupancy grid concept in comparison to a free space corridor concept,

(6) FIGS. 4(a)-(c) depict examples for trajectory planning using a free space corridor,

(7) FIG. 5 depicts a flowchart of a method for estimating a trajectory of a host vehicle,

(8) FIG. 6 depicts trajectory planning based on a primary goal point and based on backup goal points,

(9) FIGS. 7(a)-(b) depicts a check of the reference trajectory using free space corridor constraints,

(10) FIG. 8 depicts a re-optimizing process for a trajectory,

(11) FIG. 9(a) depicts a single track model,

(12) FIG. 9(b) depicts a diagram of a cost function surface, and

(13) FIGS. 10(a)-(c) depicts the application of a compensating curvature.

DETAILED DESCRIPTION

(14) In FIGS. 1(a) and 1(b), a host vehicle 11 is depicted which moves on a road between lane markers 13. The host vehicle 11 is provided with a lane keeping assistance functionality which is represented in FIG. 1(a) by an arrow 15 being aligned in lateral direction with respect to the road. In addition, the host vehicle 11 is provided with an adaptive cruise control functionality which is represented by an arrow 17 being aligned in a longitudinal direction, i.e. along the road. The aim of a conventional lane keeping assistance system is to keep the host vehicle 11 at a center 19 between the lane markers 13. On the other hand, an adaptive cruise control system tries to maintain a distance with respect to a further vehicle driving in front of the host vehicle 11 and therefore controls the longitudinal position and velocity of the host vehicle 11. A combination of longitudinal and lateral control by combining lane keeping assistance and adaptive cruise control is usually not provided since the host vehicle 11 tends to be under-controlled by such a simple combination of independent assistance systems.

(15) Longitudinal and lateral control of the host vehicle 11 can be provided, however, by trajectory planning as depicted in FIG. 1(b). For the trajectory planning, a time horizon 21 is defined which corresponds to a range of perception capability of environment sensors 35 (see FIG. 2) being installed in the host vehicle 11. The environment sensors 35 comprise a camera, a Radar system and/or a Lidar system, for example.

(16) The time horizon 21 being indicated by T.sub.p in FIG. 1(b) is divided into time increments 23 being indicated by dt in FIG. 1(b) in order to define nodes 25 for the trajectory planning. Each node 25 comprise a position x.sub.k in longitudinal direction, a position y.sub.k in lateral direction and a yaw angle ψ.sub.k of the host vehicle 11 at a time step k. Based on the nodes 25, a reference trajectory 27 is estimated via the trajectory planning. The movement of the host vehicle 11 is controlled by a suitable input for a vehicle motion control in order to be close to the reference trajectory 27.

(17) FIG. 2 depicts an embodiment of a system 31 for estimating a trajectory of the host vehicle 11. The system comprises vehicle state sensors 33 which detect a state of movement of the host vehicle 11, i.e. a position, a speed, an acceleration and a yaw rate of the host vehicle 11. The vehicle state sensors 33 comprise, for example a global positioning system (GPS) and an inertial measurement unit (IMU). The system 31 further comprises environment sensors 35 which monitor the environment of the host vehicle 11, i.e. the position of lane markers and the free space in the direction of movement of the host vehicle 11. The free space can be determined by monitoring the position and speed of vehicles and obstacles surrounding of the host vehicle 11. As mentioned above, the environment sensors 35 can comprise an optical camera, a Radar system and/or a Lidar system.

(18) The system 31 further comprises a free space module 37 and a lateral error determination module 39 which each receive a combined input from the vehicle state sensors 33 and from the environment sensors 35. The free space corridor module 37 determines a space being free of obstacles in the direction of movement of the host vehicle 11. This space is denoted as free space corridor 38 (see FIGS. 3, 4 and 7) and will be explained in detail below. The lateral error determination module 39 determines a lateral error with respect to the center 19 of the lane (see FIG. 1(b)) based on a position of lane markers 13 monitored by the environment sensors 35 and based on the state of movement of the host vehicle 11, i.e. its position and speed.

(19) The free space corridor 38 determined by the free space module 37 and the lateral error determined by the lateral error determination module 39 are inputs for a goal point determination module 41 of the system 31. The goal point determination module 41 determines a primary goal point 55 (see FIG. 6) and backup goal points 57 which are inputs for a trajectory planning module 43 of the system 31. The primary goal point 55 is determined based on the full time horizon 21 (see FIG. 1(b)) being defined by the perception capability of the environment sensors 35, whereas the backup goal points 57 are determined based on a reduced time horizon being smaller than the full time horizon 21. The backup goal point 57 replace successively the primary goal point 55 in case that the primary goal point 55 turns out to be not suitable for trajectory planning as will be explained in detail below.

(20) The trajectory planning module 43 estimates the reference trajectory 27 (see FIG. 1(b)) based on the primary goal point 55 or based on one of the backup goal points 57 (see FIG. 6). Furthermore, the trajectory planning module 43 calculates a deviation between the reference trajectory 27 and an actual trajectory of the host vehicle 11 being determined based on the output of the vehicle state sensors 33, e.g. based on the position and speed of the host vehicle 11. The determination of the deviation between the reference trajectory 27 and the actual trajectory of the host vehicle 11 is indicated by the node 44 in FIG. 2. If the deviation between the reference trajectory 27 and the actual trajectory of the host vehicle 11 is greater than a threshold 47, a trajectory re-optimization module 49 will be activated which is associated to the trajectory planning module 43 of the system 31. The trajectory re-optimization module 49 estimates an updated reference trajectory which is also called re-optimized trajectory for the remaining distance to the goal point being used for estimating the original reference trajectory 27. The re-optimization will be described in context with FIG. 8.

(21) The system 31 further comprises a trajectory control module 45 which outputs control parameters for the movement of the vehicle to a vehicle motion control 51 being in communication with the system 31 for estimating the trajectory of the host vehicle 11. The output of the trajectory control module 45 which is also the output of the entire system 31 comprises parameters for the steering of the host vehicle 11, e.g. steering angle or steering torque.

(22) As depicted in FIG. 3(b), the free space corridor 38 being determined by the free space module 37 is represented by a set of segments 53 extending along the road in the direction of movement of the host vehicle 11. Each segment 53 is defined by a set of vertices which are connected by straight lines or curved lines in order to follow the course of the road. In detail, four vertices of each segment 53 as shown in FIG. 3 (b) and FIG. 7 (a) are each connected by two straight lines in lateral direction, whereas two curved lines represented by a polynomial connect the vertices in longitudinal direction. Furthermore, the segments are discretized by a predefined distance in lateral direction. For example, each segment 53 has a width of 0.20 m in lateral direction. In longitudinal direction, each segment 53 ranges from the position of the host vehicle 11 to one or more detected objects or up to the range of perception of the environment sensors 35. In longitudinal direction, each segment 53 takes the curvature of the road into account.

(23) The segments 53 are represented in a vehicle coordinate system 52. In contrast, an occupancy grid as shown in FIG. 3 (a) and being known from prior art is usually defined in a world coordinate system 54. Therefore, a coordinate transformation is required for the information from the occupancy grid if this information is to be used for trajectory planning of the host vehicle 11. In addition, a relatively large grid size being accompanied by a large number of cells has to be used for the occupancy grid technique in order to cover the whole area of interest for the host vehicle 11. That is, the occupancy grid needs a large amount of data for its representation which requires a corresponding large memory.

(24) Therefore, the set of segments 53 representing the free space corridor 38 (see FIG. 3 (b)) requires a much lower computational effort, i.e. less memory due to the reduced data volume and less processing since no coordinate transformations are required in comparison to the occupancy grid technique.

(25) Examples for the use of the segments 53 representing the free space corridor 38 are shown in FIG. 4(a)-(c). The free space corridor 38 is applied for lane centering (a) or lane biasing (b) with respect to the center 19 of the lane wherein the host vehicle 11 follows the reference trajectory 27 being estimated by the system 31. A further example is lane changing as shown in (c). In this case, the reference trajectory 27 leads the host vehicle 11 to a center 19 of an adjacent lane.

(26) FIG. 5 depicts a flowchart of a method 100 for estimating a trajectory of a vehicle. The method steps are performed by the trajectory planning module 43 and the trajectory control module 45 as shown in FIG. 2.

(27) In step 101, inputs are acquired which are necessary to carry out the method 100. These inputs comprise goal point information 91 being provided by the goal point determination module 41 (see FIG. 2), free space corridor information 92 provided by the free space module 37, vehicle motion information 93 provided by the vehicle state sensors 33 and time information 94 for a global time synchronization. This information 94 may comprise time stamp information from a GPS system or from an IMU system. The goal point information 91 comprises the primary goal point 55 and a predefined number of backup goal points 57 (see FIG. 6) as an output of the goal point determination module 41.

(28) In steps 102 and 103, a validation of a part of the input is performed which is received at step 101. At step 102, the received goal point information 91 is validated by checking if the goal points required for the trajectory planning are existing and available. For example, it is checked if the primary goal point 55 (see FIG. 6) or at least one backup goal point 57 are provided by the goal point determination module 41 which determines goal points only which are located within the free space corridor 38. Therefore, the goal point determination module 41 might not be able to find goal points having a reasonable position within the free space corridor 38. In this case, goal point information 91 provided by the goal point determination module 41 is an empty data set and the examination fails at step 102.

(29) At step 103, the current lateral acceleration of the host vehicle 11 is compared with a maximum lateral acceleration which may be predefined for the host vehicle 11. For example, an upper limit for the lateral acceleration with respect to comfortable driving is 0.3 g.

(30) If the examination of the goal points fails at step 102 or if the current lateral acceleration is greater than the predefined maximum lateral acceleration at step 103, the trajectory planning is terminated and a driver of the host vehicle 11 has to take over control. In the latter case, i.e. if the current lateral acceleration is greater than the maximum lateral acceleration, the host vehicle 11 is driving too fast for the system 31 and the method 100 to provide a feasible trajectory output.

(31) At steps 104 and 105, the reference trajectory 27 is estimated based on the primary goal point 55 which corresponds to the maximum range of perception capability of the environment sensors 35. The reference trajectory 27 can be represented by a fifth order polynomial for the longitudinal and for the lateral position, respectively, of the host vehicle 11. In detail, the longitudinal position px(t) and the lateral position py(t) for a given time t is represented by the following equations:

(32) $\begin{array}{cc}\left[\begin{array}{c}\mathrm{px}\left(t\right)\\ \mathrm{py}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{a}_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5\\ b_0+b_{1}t+b_2{t}^2+b_3{t}^3+b_4{t}^4+b_5{t}^5\end{array}\right]& \left(1\right)\end{array}$

(33) The coefficients a.sub.0 to a.sub.5 and b.sub.0 to b.sub.5 are determined at step 104 by using an initial condition (px.sub.0, py.sub.0, t.sub.0) representing the current position of the host vehicle 11 for the time to and using a final condition (px.sub.f, py.sub.f, t.sub.f) representing the primary goal point which is to be reached at the time tf. In addition, the coefficients a.sub.0 to a.sub.5 and b.sub.0 to b.sub.5 are subjected to the further condition of minimizing a planning cost function which is defined by:
J=∫.sub.0.sup.T.sup.f(t).sup.2+(t).sup.2dt   (2)

(34) At steps 106 to 109, the reference trajectory 27 being estimated based on the primary goal point 55 is checked for violation of certain constraints. If one of these constraints is violated, a revised reference trajectory is estimated based on the first of the backup goal points 57 (see FIG. 6). In addition, this process of checking the reference trajectory and estimating a further revised reference trajectory based on the next backup goal point is repeated iteratively.

(35) At step 106, the reference trajectory being estimated based on the primary goal point is checked against dynamic constraints. The dynamic constraints comprise minimum and maximum limits of the velocity and the acceleration in longitudinal and lateral direction for the host vehicle 11. f one of these constraints is violated, i.e. if the velocity or the acceleration of the host vehicle 11 is greater than one of these limits along the reference trajectory, a revised reference trajectory will be estimated based on the first backup goal point 57 which has a reduced distance with respect to the current position of the host vehicle 11 in comparison to the primary goal point 55 (see FIG. 6). The use of the first backup goal point 57 instead of the primary goal point 55 corresponds to a reduction of the time horizon 21 (see FIG. 1) being used for the estimation of the reference trajectory 27, i.e. for trajectory planning.

(36) The revised reference trajectory is estimated as step 108, and it is also checked for violation of the dynamic constraints at step 106. If the dynamic constraints are still violated, a further revised trajectory is estimated as step 108 based on the next backup goal point 57 having a further reduced distance with respect to the current position of the host vehicle 11 in comparison to the first backup goal point 57 (see FIG. 6). That is, the time horizon for the trajectory planning is reduced again by using the next backup goal point 57. Thereafter, the further revised trajectory is checked again for violation of the dynamic constraints at step 06. That is, this process is repeated iteratively until the dynamic constraints are not violated any more or until there is no further backup goal point 57.

(37) The check if a further backup goal point 57 is available is performed at step 109. That is, before the next revised trajectory is estimated at step 108, the existence of the next backup goal point 57 is checked. If it is determined at step 109 that no further backup goal point 57 is available, the method will be not able to plan a feasible trajectory. In this case, the method is terminated and the driver of the host vehicle 11 has to take over control.

(38) At step 107, the reference trajectory which is in compliance with the dynamic constraints is checked for a violation of free space corridor constraints. For this check, the segments 53 are used which are a representation of the free space corridor 38 as described above in context of FIGS. 3 and 4. For performing the free space corridor check, a convex hull technique is used to find the boundary constraints of the entire free space corridor 38 represented by the segments 53. As depicted in FIG. 7, for the entirety of the segments 53 shown at (a), a convex hull 60 is determined which is shown at (b). The convex hull 60 can be represented by an integrated segmentation via a single set of vertices as depicted in FIG. 7 (b).

(39) In detail, the set of segments 53 shown in FIG. 7 (a) is determined first as described above (see FIG. 3 (b)) by considering possible obstacles for the host vehicle 11 which are denoted by the small two-digit numbers in FIGS. 7 (a) and 7 (b). The convex hull 60 is wrapped around the entire set of segments 53 and comprises a set of points which specify the boundary of the free space corridor 38 as follows:
Σ.sub.(j)=1.sup.Mx.sub.FR.sup.(j)=X.sub.FR
Σ.sub.(j)=1.sup.My.sub.FR,L.sup.(j)=Y.sub.FR,L
Σ.sub.(j)=1.sup.My.sub.FR,R.sup.(j)=Y.sub.FR,R   (3)
wherein x.sub.FR.sup.(j) represents the set of arc lines in the longitudinal direction for the segment j, M is the total numbers of segments, y.sub.FR,L.sup.(j) and y.sub.FR,R.sup.(j) are the set of left and right lateral boundaries of each segment, respectively, X.sub.FR is the convex hull of the set of longitudinal arc lines, Y.sub.FR,L and Y.sub.FR,R are the convex hull of the set of left and right lateral boundary of whole segment.

(40) The reference trajectory 27 being output of step 106 is checked for the free space corridor 38 represented by the convex hull 60, i.e. it is checked if the reference trajectory 27 is located within the limits given by the area defined by the convex hull 60. If yes, the movement of the host vehicle along the trajectory will be free of collisions.

(41) If the reference trajectory violates the free space corridor constraints, a revised reference trajectory is estimated again based on the next backup goal point at step 108. This reference trajectory being estimated based on the next backup goal point 57 is checked again for violation of the dynamic constraints at step 106. Thus, the check for violation of the free space corridor constraints is also iterated in the same manner using the next backup goal point 57 as described above for the check with respect to the dynamic constraints at step 106.

(42) The reference trajectory which passes both checks for constraints at steps 106 and 107 and therefore fulfils all constraints can be regarded as trajectory having optimal coefficients of the fifth order polynomial defined above in formula (1). This set of optimal coefficients can be denoted by
α={a.sub.0, a.sub.1, a.sub.2, a.sub.3, a.sub.4, a.sub.5}
β={b.sub.0, b.sub.1, b.sub.2, b.sub.3, b.sub.4, b.sub.5}  (4)

(43) These sets α, β of coefficients can be regarded as vectors describing the longitudinal and lateral component of the reference trajectory 27 (see FIG. 1). In step 110, the optimal coefficients of the reference trajectory are therefore output for the steps of re-optimizing the reference trajectory at step 112, if required, and for the steps of trajectory control, i.e. steps 114 and 115.

(44) At step 111, the reference trajectory 27 having the optimal coefficients as described above is checked for a deviation with respect to an actual trajectory or motion of the host vehicle 11 which is derived from the output provided by the vehicle state sensors 33 (see FIG. 2). In order to calculate the deviation between the reference trajectory and the actual trajectory of the host vehicle 11, the propagation of the reference trajectory can be described for a sampling time Δk by the following transition matrix:

(45) $\begin{array}{cc}\Phi =\left[\begin{array}{cccccc}1& \Delta k& \Delta k^2& \Delta k^3& \Delta k^4& \Delta k^5\\ 0& 1& 2\Delta k& 3\Delta k^2& 4\Delta k^3& 5\Delta k^4\\ 0& 0& 2& 6\Delta k& 12\Delta k^2& 20\Delta k^3\\ 0& 0& 0& 6& 24\Delta k& 60\Delta k^2\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]& \left(5\right)\end{array}$

(46) The sampling time Δk corresponds to a current time stamp which is part of the time information 94 being input by the vehicle state sensors 33, e.g. a GPS and a IMU system. The reference trajectory for the sampling time Δk can be determined as:
PX(t)=Φ.Math.α
PY(t)=Φ.Math.β
PX(t)=[(t)(t)(t)(t)].sup.T
PY(t)=[(t)(t)(t)(t)].sup.T   (6)

(47) PX, PY are vectors of the estimated reference trajectory 27 for the longitudinal and lateral direction, respectively, and comprise the position, the velocity, the acceleration and the jerk of the planned motion of the host vehicle 11.

(48) As mentioned above, the actual trajectory of the host vehicle 11 can be determined by the output of the vehicle state sensors 33, and respective vectors corresponding to PX, PY can be determined by a synchronization with the time information 94 (see FIG. 2) and the sampling time Δk. The reference trajectory 27 and the actual trajectory of the host vehicle 11 are compared in order to calculate a deviation between these trajectories for a given time t. The deviation between the trajectories is compared with a predefined threshold at step 111.

(49) The deviation between the reference trajectory 27 and the actual trajectory of the host vehicle 11 can be described by the following deviation vector:

(50) $\begin{array}{cc}e\left(t\right)=\left[\begin{array}{c}{e}_x\left(t\right)\\ e_y\left(t\right)\\ e_{\psi }\left(y\right)\end{array}\right]=\left[\begin{array}{c}\left(t\right)-\mathrm{px}\left(t\right)\\ \left(t\right)-\mathrm{py}\left(t\right)\\ \mathrm{atan}\left(\frac{}{}\right)-\psi \left(t\right)\end{array}\right]& \left(7\right)\end{array}$

(51) The deviation vector e(t) for the instantaneous time t comprises a longitudinal distance deviation e.sub.x(t), a lateral distance deviation e.sub.y(t) and a yaw angle deviation e.sub.ψ(t), respectively.

(52) Next, a remaining steering intervention time as well as a remaining longitudinal distance to the primary or backup goal point 55, 57 being used for the reference trajectory is calculated. In detail, the remaining steering intervention time and the remaining longitudinal and lateral distances are calculated as follows:

(53) $\begin{array}{cc}\Delta t_r=t_f-t\left[\begin{array}{c}\Delta \mathrm{px}\\ \Delta \mathrm{py}\end{array}\right]=\left[\begin{array}{c}{\mathrm{px}}_g-\mathrm{px}\left(t\right)\\ {\mathrm{py}}_g-\mathrm{py}\left(t\right)\end{array}\right]& \left(8\right)\end{array}$
px.sub.g and py.sub.g are the longitudinal and the lateral coordinates of the primary or backup goal point 55, 57 being used for the calculation of the reference trajectory in the vehicle coordinate system, and px(t) and py(t) describe the feedback for the motion of the host vehicle 11. t.sub.f is the total steering intervention time, i.e. the time the host vehicle needs to arrive at the primary or backup goal point 55, 57. The remaining steering intervention time Δt.sub.r and the remaining distances are calculated at step 113 where it is decided if the remaining time and distances are sufficient in order to re-optimize the reference trajectory. If the remaining time and the remaining distances are not sufficient, the method is terminated and the driver of the vehicle has to take over control.

(54) If there is sufficient remaining time for steering intervention and if the distances to the primary or backup goal point 55, 57 are also sufficient, a re-optimization of the reference trajectory will be performed at step 112. For the re-optimization, a new reference trajectory is estimated for the remaining steering intervention time using the same primary or backup goal point 55, 57 as for the original reference trajectory 27 for which the optimal coefficients are output at step 110. That is, for the remaining steering intervention time new optimal coefficients a.sub.0 to a.sub.5 are calculated in order to redirect the host vehicle 11 to the primary or backup goal point under consideration.

(55) The process of re-optimizing the trajectory is depicted schematically in FIG. 8. In the middle, the reference trajectory 27 and the optimal position for the host vehicle 11 are shown. If the deviation between the motion of the host vehicle 11 and the reference trajectory becomes greater than the predefined threshold, the system 31 will try to re-optimize the trajectory. For the example as shown in FIG. 8, the predefined threshold is 0.5 m. Therefore, if the deviation 61 shown on the right side of the reference trajectory 27 becomes greater than 0.5 m, the system 31 checks if there is sufficient steering intervention time for the re-optimization. For the deviation 61 between the reference trajectory 27 and the trajectory 63 for the motion of the host vehicle 11 (see the right side of FIG. 8) the steering intervention time and the distances to the primary or backup goal point 55, 57 are sufficient for the estimation of a re-optimized trajectory 65 which redirects the host vehicle 11 to the primary or backup goal point. In contrast, for the deviation shown on the right side of the reference trajectory 27, the remaining steering intervention time is not sufficient and therefore no re-optimization can be performed. The trajectory on the right side of FIG. 8 demonstrates where the host vehicle will end up if additionally no control input is provided by the driver.

(56) If the deviation between the reference trajectory 27 and the motion of the host vehicle 11 is lower than the predefined threshold at step 111, trajectory control will be performed based on the use of a single track model (see FIG. 9 (a)) by the trajectory control module 45 (see FIG. 2). The trajectory control is performed at step 114 and tries to minimize the deviation between the reference trajectory and the motion of the host vehicle 11 by minimizing a cost function as described below.

(57) According to the single track model, a vector x is defined as [e.sub.y, ė.sub.y, e.sub.ψ, ė.sub.ψ], wherein e.sub.y and ė.sub.y are the lateral deviation and its rate, and wherein e.sub.ψ and ė.sub.ψ are the yaw deviation and the yaw rate deviation with respect to the reference trajectory, respectively. An equation of motion for the deviation can be formulated as

(58) $\begin{array}{cc}{\stackrel{\mathrm{.Math.}}{e}}_y={\stackrel{.}{e}}_y\left[-\frac{2c_f}{{\mathrm{mv}}_x}-\frac{2c_r}{{\mathrm{mv}}_x}\right]+e_{\psi }\left[\frac{2c_f}{m}+\frac{2c_r}{m}\right]+{\stackrel{.}{e}}_{\psi }\left[-\frac{2c_f{l}_f}{{\mathrm{mv}}_x}+\frac{2c_r{l}_r}{{\mathrm{mv}}_x}\right]+{\psi }^{*}\left[-\frac{2c_f{l}_f}{{\mathrm{mv}}_x}+\frac{2c_r{l}_r}{{\mathrm{mv}}_x}\right]+\frac{2c_f\delta }{m}+g\sin \Phi {\stackrel{\mathrm{.Math.}}{e}}_{\psi }={\stackrel{.}{e}}_y\left[-\frac{2c_f{l}_f}{I_z{v}_x}+\frac{2c_r{l}_r}{I_z{v}_x}\right]+e_{\psi }\left[\frac{2c_f{l}_f}{I_z}-\frac{2c_r{l}_r}{I_z}\right]+{\stackrel{.}{e}}_{\psi }\left[-\frac{2c_f{l}_f^2}{I_z{v}_x}-\frac{2c_r{l}_r^2}{I_z{v}_x}\right]+{\psi }^{*}\left[-\frac{2c_f{l}_f^2}{I_z{v}_x}+\frac{2c_r{l}_r^2}{I_z{v}_x}\right]+\frac{2c_f{l}_f\delta }{I_z}& \left(9\right)\end{array}$

(59) In these formulas, m is the mass of the vehicle, v.sub.x is the longitudinal velocity (see FIG. 9 (a)), I.sub.z is the yaw moment of inertia of the vehicle, ψ* is a desired yaw rate with respect to the road curvature, δ is a steering angle, c.sub.f and c.sub.r are a respective corning compliance for the front and rear tire, respectively, and I.sub.f and I.sub.r are the respective distances from a center of gravity (c.g.) of the vehicle to the front and rear axles, respectively. Furthermore, Φ is an input road angle and g is the gravitational acceleration.

(60) The equation of motion (9) can be vectorized as follows by introducing a state transmission matrix A and input matrices B, B.sub.{dot over (ψ)} and B.sub.Φ:

(61) $\begin{array}{cc}\stackrel{.}{\chi }=A\chi +B\delta +B_{\stackrel{.}{\Psi }}{\Psi }^{*}+B_{\Phi }\sin \Phi A=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{-2\left(c_f+c_r\right)}{{\mathrm{mv}}_x}& \frac{2\left(c_f+c_r\right)}{m}& -\frac{2\left(l_f{c}_f-l_r{c}_r\right)}{{\mathrm{mv}}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2\left(l_f{c}_f-l_r{c}_r\right)}{I_z{v}_x}& \frac{2\left(l_f{c}_f-l_r{c}_r\right)}{I_z}& -\frac{2(l_f^2{c}_f+l_r^2{c}_r}{I_z{v}_x}\end{array}\right]B=\left[\begin{array}{c}0\\ \frac{2c_f}{m}\\ 0\\ \frac{2l_f{c}_f}{I_z}\end{array}\right]B_{\stackrel{.}{\Psi }}=\left[\begin{array}{c}0\\ -\frac{2l_f{c}_f-2l_r{c}_r)}{{\mathrm{mv}}_x}-v_x\\ 0\\ -\frac{2l_f^2{c}_f+2l_r^2{c}_r}{I_z{v}_x}\end{array}\right]B_{\Phi }=\left[\begin{array}{c}0\\ g\\ 0\\ 0\end{array}\right]& \left(10\right)\end{array}$

(62) An optimal control output may be determined based on a Model Predictive Control scheme which tries to diminish the deviation at a steady state in which the influence of the road curvature B.sub.ψ and of the road angle B.sub.Φ is excluded. In order to derive the cost function, a prediction according to the Model Predictive Control scheme can be formulated as follows:

(63) $\begin{array}{cc}{\xi }_k=M{\chi }_k+\Theta u_{k}M=\left[\begin{array}{c}A\\ A^2\\ \mathrm{.Math.}\\ A^N\end{array}\right]\Theta =\left[\begin{array}{cccc}B& 0& \mathrm{.Math.}& 0\\ \mathrm{AB}& B& \mathrm{.Math.}& 0\\ \mathrm{.Math.}& \mathrm{.Math.}& \ddots & \mathrm{.Math.}\\ A^{N-1}B& A^{N-2}B& \mathrm{.Math.}& B\end{array}\right]& \left(11\right)\end{array}$

(64) The matrix M is a convolution matrix of the state transition matrix A as defined in (10) and describes the predictive horizon (N steps) of the state of the vehicle, and Θ is a collective controllability matrix based on the matrices A and B as also defined in (10). u.sub.k corresponds to the steering angle δ at the time step k.

(65) A cost matrix q for weighting each deviation state (vector x as defined in context of (9)) can be defined at a predicted time horizon N by the following equation:

(66) $\begin{array}{cc}q=\left[\begin{array}{cccc}{q}_{e_y}& 0& 0& 0\\ 0& q_{{\stackrel{.}{e}}_y}& 0& 0\\ 0& 0& q_{e_{\Psi }}& 0\\ 0& 0& 0& q_{{\stackrel{.}{e}}_{\Psi }}\end{array}\right]Q=C^t\underset{\_}{q}C\tilde{Q}=\left[\begin{array}{cccc}Q& 0& \mathrm{.Math.}& 0\\ \ddots & \mathrm{.Math.}& & \mathrm{.Math.}\\ \mathrm{.Math.}& & Q& 0\\ 0& \mathrm{.Math.}& 0& Q\end{array}\right]& \left(12\right)\end{array}$

(67) {tilde over (Q)} represents a cost matrix of states at for the whole predicted time horizon N, and C denotes an output matrix. Furthermore, an input rate matrix at the predicted horizon can be defined as:

(68) $\begin{array}{cc}R=r_u\tilde{R}=\left[\begin{array}{cccc}R& 0& \mathrm{.Math.}& 0\\ 0& \ddots & & \mathrm{.Math.}\\ \mathrm{.Math.}& & R& 0\\ 0& \mathrm{.Math.}& 0& R\end{array}\right]& \left(13\right)\end{array}$

(69) By calculating a Hessian matrix of the cost function and an coefficient vector F of the linear part of the function as follows
H=C.sup.T{tilde over (Q)}C+{tilde over (R)}
F=C.sup.t{tilde over (Q)}M   (14)
a quadratic cost function can be formulated for an optimization problem as
J.sub.k=u.sub.k.sup.THu.sub.k+2x.sub.k.sup.TFu.sub.k   (15)

(70) An example for a quadratic cost function is shown in FIG. 9(b). The minimization of the cost function can be formulated as follows:
½u.sup.THu+F.sup.tu.fwdarw.min
subjected to
A.sub.cu.sub.k≤b.sub.c+B.sub.cx.sub.k   (16)

(71) A.sub.c is an concatenation of an equality and inequality constraint matrix, b.sub.c is a state constraint [λ.sub.ub, λ.sub.lb], wherein λ.sub.ub is a upper boundary of the deviation state vector, λ.sub.lb is a lower boundary of the deviation state vector, B.sub.c is a concatenation of the state transition matrix at each predictive horizon [−A.sub.n, A.sub.n] and n is the associated predictive horizon (n steps).

(72) The result of the minimization provides an optimal control output for the vehicle motion control 51 (see FIG. 2) which can be represented as follows:

(73) 0$\begin{array}{cc}{u}^{*}\left(k\right)=\arg \underset{u}{\min }J\left(k\right)& \left(17\right)\end{array}$

(74) In addition, based on the single track model a compensation for the steering angle δ may be derived as follows:

(75) $\begin{array}{cc}{\delta }_{\mathrm{FF}}=\frac{l_f+l_r}{\mathrm{RoC}}+\left(\frac{\frac{{\mathrm{ml}}_r}{l_f+l_r}}{2c_f}-\frac{\frac{{\mathrm{ml}}_f}{l_f+l_r}}{2c_r}\right)a_y^{*}{a}_y^{*}={\stackrel{.}{\Psi }}^{*2}\mathrm{RoC}+g\sin \Phi & \left(18\right)\end{array}$

(76) δ.sub.FF is a feed forward steering angle, RoC is the radius of curvature, a*.sub.y is a desired lateral acceleration, ψ* is the desired yaw rate, and Φ is the road angle.

(77) At block 115, a compensating curvature 67 (see FIG. 10) is generated in order to correct the motion of the host vehicle 11 with respect to the reference trajectory 27. In FIG. 10 (a), the movement of the host vehicle 11 between the lake markers 13 is shown, whereas FIG. 10 (b) depicts a direction 69, 71 (i.e. an angle with respect to a straight movement at a steering angle of zero) of the host vehicle 11 and of the reference trajectory 27, respectively, and FIG. 10 (c) depicts a curvature 73, 75 of the movement of the host vehicle 11 and of the reference trajectory 27. The directions 69, 71 and the curvatures 73, 75 are depicted in arbitrary units over time. In addition, a compensating direction 77 and the compensating curvature 67 are shown in FIG. 10 (b) and FIG. 10 (c), respectively.

(78) In a normal situation, the host vehicle 11 should track the reference trajectory 27. If the motion of the host vehicle 11 starts to deviate from the reference trajectory 27, i.e. if a current direction 69 and a current curvature 73 of the vehicle motion deviates from the direction 71 and the curvature 75 of the reference trajectory as shown at FIG. 10 (b) and (c), respectively, a compensating direction 77 and the compensating curvature 67 are calculated in order to compensate the current directionb 69 and the current curvature 73 and to redirect the host vehicle 11 to the reference trajectory 27 as shown in FIG. 10 (a).

(79) Based on the single track model, the compensation curvature 67 may be calculated as:

(80) $\begin{array}{cc}{\kappa }^{*}=\tan \left(\frac{u^{*}+{\delta }_{\mathrm{FF}}}{l_f+l_r}\right)& \left(19\right)\end{array}$

(81) The direction of the host vehicle 11 is defined by the steering angle of the vehicle. As shown in FIG. 10 (a), the steering angle is almost zero. Correspondingly, the current direction 69 and the current curvature 73 of the host vehicle 11 are almost constant. By taking the compensating curvature 67 and the compensating direction 7 into account, the actual trajectory of the host vehicle 11 is again directed to the goal point 55, 57, i.e. to the primary goal point 55 or to one of the backup goal points 57 being described above.

(82) TABLE-US-00001 TABLE 1 Glossary of Terms Used Above Abbreviation Description Unit px.sub.0 initial longitudinal position m py.sub.0 initial lateral position m t.sub.0 initial time s px.sub.f final longitudinal position m px.sub.f final lateral position m t.sub.f final time s α.sub.0, α.sub.1, α.sub.2, α.sub.3, α.sub.4, α.sub.5 longitudinal trajectory polynomial coefficients — b.sub.0, b.sub.1, b.sub.2, b.sub.3, b.sub.4, b.sub.5 lateral trajectory polynomial coefficients — x.sub.FR.sup.(j) arc of the free space corridor segment j m y.sub.FR, L.sup.(j) boundary of left lateral position at segment j m y.sub.FR, R.sup.(j) boundary of right lateral position at segment j m j number of free space corridor segment — M total numbers of segments — X.sub.FR vector of whole set of arc length — Y.sub.FR vector of whole set of boundary of left and right lateral position — α vector of longitudinal trajectory polynomial coefficients — β vector of lateral trajectory polynomial coefficients — Δk sampling time s Φ state transition matrix — PX vector of longitudinal trajectory — PX vector of lateral trajectory — estimated longitudinal position m estimated lateral position m estimated longitudinal velocity m/s estimated lateral velocity m/s estimated longitudinal acceleration m/s.sup.2 estimated lateral acceleration m/s.sup.2 estimated longitudinal jerk m/s.sup.3 estimated lateral jerk m/s.sup.3 e.sub.x longitudinal distance deviation m e.sub.y lateral distance deviation m e.sub.ψ yaw angle deviation rad e deviation vector — px.sub.g longitudinal goal position m py.sub.g lateral goal position m Δt.sub.r remain steering intervention time s Δpx remain longitudinal distance to goal m Δpy remain lateral distance to goal m ė.sub.y lateral velocity deviation m/s ë.sub.y lateral acceleration deviation m/s.sup.2 ė.sub.ψ yaw rate deviation rad/s ë.sub.ψ yaw acceleration deviation rad/s.sup.2 c.sub.f front tire cornering stiffness coefficient N/rad c.sub.r rear tire cornering stiffness coefficient N/rad l.sub.f distance between the center of gravity to the front axle m. l.sub.r distance between the center of gravity to the rear axle m. .sub.ψ* desired yaw rate rad/s δ steering angle rad g constant of gravitational acceleration m/s.sup.2 m vehicle mass kg ϕ road angle rad I.sub.z mass moment of inertia of the vehicle around the vertical axis kg/m.sup.2 ν.sub.x Feedback longitudinal velocity m/s A single track model state transition matrix — B state input matrix for the steering angle input — B.sub.ψ state input matrix for the yaw rate input — B.sub.ϕ state input matrix for the road bank angle input — χ deviation state vector — ξ The prediction of Model Predictive Control for the — deviation state vector M convolution matrix — Θ prediction of a controllability matrix — N prediction horizon — k discretized time step — u.sub.k steering angle at time step k — q.sub.e.sub.y weight for the lateral distance deviation — q.sub.ėy weight for the lateral velocity deviation — q.sub.e.sub.ψ weight for the yaw angle deviation — q.sub.eψ weight for the yaw rate deviation — q weight matrix for the deviation state — Q quadratizing weight matrix — {tilde over (Q)} concatenated weight matrix of the deviation states for entire prediction horizon — r.sub.u weight for the input steering angle — R weight matrix for input steering angle — {tilde over (R)} Concatenated weight matrix of input steering angle — for entire prediction horizon — H Hessian matrix of the quadratic cost function — F Coefficient vector of linear part of the quadratic cost function — J quadratic cost function — u* optimal steering angle rad A.sub.c Concatenation of equality and inequality constraint matrix — b.sub.c state constraint matrix — χ.sub.ub upper boundary of the error state vector — χ.sub.lb lower boundary of theerror state vector — B.sub.c concatenation of the state transition matrix — n associated predictive horizon — a.sub.y* desired lateral acceleration m/s.sup.2 δ.sub.FF feed forward steering angle rad RoC radius of curvature m k* compensated curvature —