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 comprising: a plurality of sensors having a maximum range of perception capability, the sensors providing information regarding an environment in a vicinity of a vehicle; and at least one processor and memory configured to: determine an actual trajectory of the vehicle; use the information provided by the sensors to determine environmental constraints on movement of the vehicle; determine a first potential trajectory for the vehicle using a first goal location that is based on the maximum range of perception capability of the plurality of sensors; determine whether the first potential trajectory satisfies dynamic constraints on the vehicle and the environmental constraints; use the first potential trajectory as a planned trajectory of the vehicle in response to determining that the first potential trajectory satisfies the dynamic constraints on the vehicle and the environmental constraints; in response to determining that the first potential trajectory does not satisfy the dynamic constraints on the vehicle and the environmental constraints, determine a second potential trajectory for the vehicle based on a reduced range that is smaller than the maximum range of perception capability, and use the second potential trajectory as the planned trajectory of the vehicle; and generate control parameters for controlling movement of the vehicle that correspond to minimizing a deviation between the actual trajectory and the planned trajectory.

2. The system of claim 1, wherein the dynamic constraints include at least one of: a minimum limit of velocity; a maximum limit of velocity; a minimum limit of acceleration in a longitudinal direction; a maximum limit of acceleration in a longitudinal direction; a minimum limit of acceleration in a lateral direction; and a maximum limit of acceleration in a lateral direction.

3. The system of claim 1, wherein the environmental constraints define a free space corridor within which the vehicle can travel without collision.

4. The system of claim 1, wherein the at least one processor and memory are further configured to: determine if the deviation between the actual trajectory and the planned trajectory is greater than a predetermined threshold; determine a remaining steering intervention time; and estimate an updated reference trajectory based on the remaining steering intervention time in response to the deviation being greater than the predetermined threshold.

5. The system of claim 1, wherein the at least one processor and memory are further configured to minimize the deviation between the actual trajectory and the planned trajectory of the vehicle by minimizing a cost function based on a single track model of the vehicle.

6. The system of claim 1, wherein the at least one processor and memory are further configured to estimate a compensating curvature which is applied to the actual trajectory in order to redirect the vehicle to the planned trajectory in response to the deviation between the actual trajectory and the planned trajectory of the vehicle increasing.

7. The system of claim 1, wherein the plurality of sensors include at least one of an optical camera, a Radar system, and a Lidar System.

8. A method comprising: generating, with a plurality of sensors having a maximum range of perception capability, information regarding an environment in a vicinity of a vehicle; determining, with at least one processor, an actual trajectory of the vehicle; using, with the at least one processor, the information generated by the sensors to determine environmental constraints on movement of the vehicle; determining, with the at least one processor, a first potential trajectory for the vehicle using a first goal location that is based on the maximum range of perception capability of the plurality of sensors; determining, with the at least one processor, whether the first potential trajectory satisfies dynamic constraints on the vehicle and the environmental constraints; using, with the at least one processor, the first potential trajectory as a planned trajectory of the vehicle in response to determining that the first potential trajectory satisfies the dynamic constraints on the vehicle and the environmental constraints; in response to determining that the first potential trajectory does not satisfy the dynamic constraints on the vehicle and the environmental constraints, determining, with the at least one processor, a second potential trajectory for the vehicle based on a reduced range that is smaller than the maximum range of perception capability, and use the second potential trajectory as the planned trajectory; and generating, with the at least one processor, control parameters for controlling movement of the vehicle that correspond to minimizing a deviation between the actual trajectory and the planned trajectory.

9. The method of claim 8, wherein the dynamic constraints include at least one of: a minimum limit of velocity; a maximum limit of velocity; a minimum limit of acceleration in a longitudinal direction; a maximum limit of acceleration in a longitudinal direction; a minimum limit of acceleration in a lateral direction; and a maximum limit of acceleration in a lateral direction.

10. The method of claim 8, wherein the environmental constraints define a free space corridor within which the vehicle can travel without collision.

11. The method of claim 8, further comprising: determining, with the at least one processor, if the deviation between the actual trajectory and the planned trajectory is greater than a predetermined threshold; determining, with the at least one processor, a remaining steering intervention time; and estimating, with the at least one processor, an updated reference trajectory based on the remaining steering intervention time in response to the deviation being greater than the predetermined threshold.

12. The method of claim 8, further comprising minimizing, with the at least one processor, the deviation between the actual trajectory and the planned trajectory of the vehicle by minimizing a cost function based on a single track model of the vehicle.

13. The method of claim 8, further comprising estimating, with the at least one processor, a compensating curvature which is applied to the actual trajectory in order to redirect the vehicle to the planned trajectory in response to the deviation between the actual trajectory and the planned trajectory of the vehicle increasing.

14. The method of claim 8, wherein the plurality of sensors include at least one of an optical camera, a Radar system, and a Lidar System.

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

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}px\left(t\right)\\ py\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{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 t.sub.f. 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:

(34) $\begin{array}{cc}J={?}_0^{T_f}{\stackrel{?}{\mathrm{px}}\left(t\right)}^2+{\stackrel{?}{\mathrm{py}}\left(t\right)}^{2}dt& \left(2\right)\end{array}$

(35) 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.

(36) 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. If 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.

(37) 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 106. 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.

(38) 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.

(39) 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).

(40) 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:

(41) $\begin{array}{cc}\begin{array}{c}{.Math.}_{\left(j\right)=1}^M{x}_{FR}^{\left(j\right)}={\underset{?}{X}}_{FR}\\ {.Math.}_{\left(j\right)=1}^M{y}_{\mathrm{FR},L}^{\left(j\right)}={\underset{\_}{Y}}_{\mathrm{FR},L}\\ {.Math.}_{\left(j\right)=1}^M{y}_{\mathrm{FR},R}^{\left(j\right)}=Y_{\mathrm{FR},R}\end{array}& \left(3\right)\end{array}$
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.

(42) 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.

(43) 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.

(44) 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)

(45) 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.

(46) 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:

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

(48) 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:

(49) $\begin{array}{cc}\underset{\_}{\mathrm{PX}}\left(t\right)=?.Math.\underset{\_}{?}& \left(6\right)\end{array}$$\underset{\_}{\mathrm{PY}}\left(t\right)=?.Math.\underset{\_}{?}$$\underset{\_}{\mathrm{PX}}\left(t\right)=[{\begin{array}{cccc}\widehat{\mathrm{px}}\left(t\right)& \widehat{\stackrel{.}{\mathrm{px}\left(t\right)}}& \widehat{\stackrel{..}{\mathrm{px}}}\left(t\right)& \widehat{\stackrel{...}{\mathrm{px}}}\left(t\right)]\end{array}}^T$$\underset{\_}{\mathrm{PY}}\left(t\right)=[{\begin{array}{cccc}\widehat{\mathrm{py}}\left(t\right)& \widehat{\stackrel{.}{\mathrm{py}\left(t\right)}}& \widehat{\stackrel{..}{\mathrm{py}}}\left(t\right)& \widehat{\stackrel{...}{\mathrm{py}}}\left(t\right)]\end{array}}^T$

(50) 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.

(51) 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.

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

(53) $\begin{array}{cc}e\left(t\right)=\left[\begin{array}{c}{e}_x\left(t\right)\\ e_y\left(t\right)\\ e_{?}\left(t\right)\end{array}\right]=\left[\begin{array}{c}\widehat{\mathrm{px}}\left(t\right)-\mathrm{px}\left(t\right)\\ \widehat{\mathrm{py}}\left(t\right)-\mathrm{py}\left(t\right)\\ \mathrm{atan}\left(\frac{\widehat{\mathrm{dy}}}{\widehat{\mathrm{dx}}}\right)-?\left(t\right)\end{array}\right]& \left(7\right)\end{array}$

(54) 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.

(55) 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:

(56) $\begin{array}{cc}\begin{array}{c}?t_r=t_f-t\\ \left[\begin{array}{c}?px\\ ?py\end{array}\right]=\left[\begin{array}{c}p{x}_g-px\left(t\right)\\ p{y}_g-py\left(t\right)\end{array}\right]\end{array}& \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.f 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.

(57) 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.

(58) 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.

(59) 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.

(60) According to the single track model, a vector ? 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

(61) $\begin{array}{cc}\begin{array}{c}{\stackrel{.Math.}{e}}_y={\stackrel{?}{e}}_y\left[-\frac{2c_f}{m{v}_x}-\frac{2c_r}{m{v}_x}\right]+e_{?}\left[\frac{2c_f}{m}+\frac{2c_r}{m}\right]+{\stackrel{?}{e}}_{?}\left[-\frac{2c_f{l}_f}{m{v}_x}+\frac{2c_r{l}_r}{m{v}_x}\right]+{?}^{*}\left[-\frac{2c_f{l}_f}{m{v}_x}+\frac{2c_r{l}_r}{m{v}_x}\right]+\frac{2c_f?}{m}+g\sin ?\\ {\stackrel{.Math.}{e}}_{?}={\stackrel{.}{e}}_y\left[-\frac{2c_f{l}_f}{I_z{v}_x}+\frac{2c_r{l}_r}{I_z{v}_x}\right]+e_{?}\left[\frac{2c_f{l}_f}{I_z}-\frac{2c_r{l}_r}{I_z}\right]+{\stackrel{?}{e}}_{?}\left[-\frac{2c_f{l}_f^2}{I_z{v}_x}-\frac{2c_r{l}_r^2}{I_z{v}_x}\right]+{?}^{*}\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?}{I_z}\end{array}& \left(9\right)\end{array}$

(62) 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, ?.sup.* 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.

(63) The equation of motion (9) can be vectorized as follows by introducing a state transmission matrix A and input matrices B, B.sub.? and B.sub.?:
{dot over (?)}=A?+B?+B.sub.??.sup.*+B.sub.? sin ?

(64) $\begin{array}{cc}\begin{array}{c}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\left(l_f^2{c}_f+l_r^2{c}_r\right)}{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_{?}=\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_{?}=\left[\begin{array}{c}0\\ g\\ 0\\ 0\end{array}\right]\end{array}& \left(10\right)\end{array}$

(65) 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:
?.sub.k=M?.sub.k+?u.sub.k

(66) 0$\begin{array}{cc}\begin{array}{c}M=\left[\begin{array}{c}A\\ A^2\\ .Math.\\ A^N\end{array}\right]\\ ?=\left[\begin{array}{cccc}B& 0& .Math.& 0\\ \mathrm{AB}& B& .Math.& 0\\ .Math.& .Math.& ?& .Math.\\ A^{N-1}B& A^{N-2}B& .Math.& B\end{array}\right]\end{array}& \left(11\right)\end{array}$

(67) 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.

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

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

(70) {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:

(71) $\begin{array}{cc}\begin{array}{c}R=r_u\\ \stackrel{?}{R}=\left[\begin{array}{cccc}R& 0& .Math.& 0\\ 0& ?& & .Math.\\ .Math.& & R& 0\\ 0& .Math.& 0& R\end{array}\right]\end{array}& \left(13\right)\end{array}$

(72) 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)

(73) An example for a quadratic cost function is shown in FIG. 9(b). The minimization of the cost function can be formulated as follows:

(74) $\begin{array}{cc}\begin{array}{c}\frac{1}{2}{u}^{T}Hu+F^{T}u.fwdarw.\min \\ \mathrm{subjected}\mathrm{to}\\ A_c{u}_k?b_c+B_c{x}_k\end{array}& \left(16\right)\end{array}$

(75) 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).

(76) 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:

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

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

(79) $\begin{array}{cc}\begin{array}{c}{?}_{FF}=\frac{l_f+l_r}{RoC}+\left(\frac{\frac{m{l}_r}{l_f+l_r}}{2c_f}-\frac{\frac{{\mathrm{ml}}_f}{l_f+l_r}}{2c_r}\right)a_y^{*}\\ a_y^{*}={\stackrel{?}{?}}^{*2}RoC+g\sin ?\end{array}& \left(18\right)\end{array}$

(80) ?.sub.FF is a feed forward steering angle, RoC is the radius of curvature, a.sub.y.sup.* is a desired lateral acceleration, ?* is the desired yaw rate, and ? is the road angle.

(81) 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.

(82) 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 FIGS. 10 (b) and (c), respectively, a compensating direction 77 and the compensating curvature 67 are calculated in order to compensate the current direction 69 and the current curvature 73 and to redirect the host vehicle 11 to the reference trajectory 27 as shown in FIG. 10 (a).

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

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

(85) 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 77 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.

(86) 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 a.sub.0, a.sub.1, a.sub.2, a.sub.3, a.sub.4, a.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 m position at segment j y.sub.FR,R.sup.(j) boundary of right lateral m position at segment j 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 lateral velocity deviation m/s lateral acceleration deviation m/s.sup.2 yaw rate deviation rad/s 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 m. to the front axle l.sub.r distance between the center of gravity m. to the rear axle {dot over (?)}* 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 kg/m.sup.2 around the vertical axis v.sub.x Feedback longitudinal velocity m/s A single track model state transition matrix B state input matrix for the steering angle input B{dot over (.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.?.sub.y weight for the lateral velocity deviation q.sub.e.sub.? weight for the yaw angle deviation q.sub.?.sub.? 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 ?* compensated curvature