Method of road vehicle trajectory planning

Abstract

The present disclosure relates to a method of trajectory planning for maneuvers for an ego vehicle (E) equipped with a sensor systems, a prediction systems, a control system, and a decision-making system. The method includes determining possible lateral motion trajectories of a requested maneuver, longitudinal safety critical zones which correspond to each of the determined possible lateral motion trajectories, a longitudinal motion trajectory of the requested maneuver, lateral safety critical zones which correspond to the determined longitudinal motion trajectory of the requested maneuver, and a lateral motion trajectory of the requested maneuver. The present disclosure also relates to a driver assistance system arranged to perform the method and a vehicle including such a system.

Claims

1. A method of trajectory planning for maneuvers for an ego vehicle (E) including a sensor system to measure a position of the vehicle on a road as well as relative positions and velocities of surrounding traffic participants and objects, a prediction system to estimate motion trajectories of surrounding traffic participants and objects over a defined time horizon, a control system capable of following a planned trajectory, and a decision-making system to provide a desired maneuver, the method comprising: using receding horizon control applying a finite prediction horizon for determining possible lateral motion trajectories of a requested maneuver, wherein determining possible lateral motion trajectories of the requested maneuver comprises generating a set of possible lateral motion trajectories for the requested maneuver by initiating lateral motion of the maneuver over a discrete time interval; longitudinal safety critical zones for each relevant surrounding traffic participant and object which correspond to each of the determined possible lateral motion trajectories; a longitudinal motion trajectory of the requested maneuver; lateral safety critical zones for each relevant surrounding traffic participant and object which correspond to the determined longitudinal motion trajectory of the requested maneuver; and a lateral motion trajectory of the requested maneuver from the determined possible lateral motion trajectories.

2. The method according to claim 1 wherein the discrete time interval is defined as i[1, . . . , min(N,N_g)(n_o+n_min)] where N denotes a prediction horizon, N_g denotes a time instance for which a targeted inter-vehicle traffic gap is no longer appropriate, n_o denotes a predicted time it takes for any surrounding vehicles constituting a tight inter-vehicle traffic gap to smoothly adjust their velocities in order to allow the ego vehicle (E) to enter the targeted inter-vehicle traffic gap, and n_min denotes a minimum time it takes for the ego vehicle (E) to laterally move into the targeted inter-vehicle traffic gap.

3. The method according to claim 1 further comprising defining, when the set of possible lateral motion trajectories of the maneuver has been generated, a corresponding longitudinal safety critical zone for each relevant surrounding traffic participant and object as a miscellaneous shape comprising a triangular, semi-circular, or ellipsoid shape.

4. The method according to claim 3 further comprising defining, for each possible lateral motion trajectory of the requested maneuver and corresponding safety critical zones, maximum and minimum bounds on the longitudinal position of the ego vehicle (E) at each time instance as a longitudinal safety corridor, wherein a longitudinal distance which the ego vehicle (E) should maintain to each relevant surrounding traffic participant and object is defined with respect to each possible lateral motion trajectory of the requested maneuver and corresponding longitudinal safety critical zones.

5. The method according to claim 4 further comprising determining, for each longitudinal safety corridor, whether there exists a longitudinal trajectory which allows the ego vehicle (E) to be positioned within the corridor over the prediction horizon and selecting, from the determined existing longitudinal trajectories for all longitudinal safety corridors, one of the determined existing longitudinal trajectories.

6. The method according to claim 1 further comprising updating the safety critical zones of each relevant surrounding traffic participant and object according to the longitudinal motion of the requested maneuver utilizing the determined longitudinal motion trajectory of the requested maneuver.

7. The method according to claim 1 wherein determining the lateral motion trajectory of the requested maneuver further comprises formulating lateral motion trajectory planning as a standard Quadratic Program optimization problem and solving the problem.

8. The method according to claim 1 further comprising utilizing at least part of one of the determined longitudinal and lateral motion trajectories of the requested maneuver to determine longitudinal and/or lateral control signals for control of the ego vehicle (E).

9. The method according to claim 1 further comprising utilizing at least part of one of the determined longitudinal and lateral motion trajectories of the requested maneuver to determine longitudinal and lateral control signals for control of the ego vehicle (E) and combining the longitudinal and lateral control signals to a combined control signal for combined longitudinal and lateral control of the ego vehicle (E).

10. A driver assistance system configured to plan a trajectory for a maneuver for an ego vehicle, wherein the vehicle includes a sensor system to measure a vehicle position on a road as well as the relative positions and velocities of surrounding traffic participants and objects, a prediction system to estimate motion trajectories of the surrounding traffic participants and objects over a defined time horizon, a control system configured to follow a planned trajectory, and a decision-making system to provide a desired maneuver, the driver assistance system configured to use receding horizon control applying a finite prediction horizon to: determine possible lateral motion trajectories of a requested maneuver, wherein, to determine possible lateral motion trajectories of the requested maneuver, the driver assistance system is further configured to generate a set of possible lateral motion trajectories for the requested maneuver by initiating lateral motion of the maneuver over a discrete time interval; determine longitudinal safety critical zones for each relevant surrounding traffic participant and object which correspond to each of the determined possible lateral motion trajectories; determine a longitudinal motion trajectory of the requested maneuver; determine lateral safety critical zones for each relevant surrounding traffic participant and object which correspond to the determined longitudinal motion trajectory of the requested maneuver; and determine a lateral motion trajectory of the requested maneuver from the determined possible lateral motion trajectories.

11. The driver assistance system according to claim 10 wherein at least part of one of the determined longitudinal and lateral motion trajectories of the requested maneuver is utilized for one or more of the following: deciding whether the requested maneuver should be initiated or aborted; performing the requested maneuver, autonomously or driver supervised; providing driver guidance for performing the requested maneuver; and providing driver warning and/or information when the requested maneuver could and should not be performed.

12. The driver assistance system according to claim 10 wherein the corresponding longitudinal safety critical zone for each relevant surrounding traffic participant and object comprises a miscellaneous shape comprising a triangular, semi-circular, or ellipsoid shape.

13. A vehicle comprising the driver assistance system according to claim 10.

14. A method of trajectory planning for maneuvers for an ego vehicle (E) including a sensor system to measure a position of the vehicle on a road as well as relative positions and velocities of surrounding traffic participants and objects, a prediction system to estimate motion trajectories of the surrounding traffic participants and objects over a defined time horizon, a control system configured to follow a planned trajectory, and a decision-making system to provide a desired maneuver, the method comprising: determining possible lateral motion trajectories of a requested maneuver, wherein determining possible lateral motion trajectories of the requested maneuver comprises generating a set of possible lateral motion trajectories for the requested maneuver by initiating lateral motion of the maneuver over a discrete time interval; determining longitudinal safety critical zones for each relevant surrounding traffic participant and object which correspond to each of the determined possible lateral motion trajectories; determining a longitudinal motion trajectory of the requested maneuver; determining lateral safety critical zones for each relevant surrounding traffic participant and object which correspond to the determined longitudinal motion trajectory of the requested maneuver; and determining a lateral motion trajectory of the requested maneuver from the determined possible lateral motion trajectories.

15. The method of claim 14 further comprising using receding horizon control applying a finite prediction horizon for determining the possible lateral motion trajectories, determining the longitudinal safety critical zones, determining the longitudinal motion trajectory, determining the lateral safety critical zones, and determining the lateral motion trajectory.

16. The method according to claim 14 wherein the discrete time interval is defined as i[1, . . . , min(N,N_g)(n_o+n_min)] where N denotes a prediction horizon, N_g denotes a time instance for which a targeted inter-vehicle traffic gap is no longer appropriate, n_o denotes a predicted time it takes for any surrounding vehicles constituting a tight inter-vehicle traffic gap to smoothly adjust their velocities in order to allow ego vehicle (E) to enter the targeted inter-vehicle traffic gap, and n_min denotes a minimum time it takes for the ego vehicle (E) to laterally move into the targeted inter-vehicle traffic gap.

17. The method according to claim 14 further comprising defining, when the set of possible lateral motion trajectories of the maneuver has been generated, a corresponding longitudinal safety critical zone for each relevant surrounding traffic participant and object as a triangular, semi-circular, or ellipsoid shape.

18. The method according to claim 17 further comprising defining, for each possible lateral motion trajectory of the requested maneuver and corresponding safety critical zones, maximum and minimum bounds on the longitudinal position of ego vehicle (E) at each time instance as a longitudinal safety corridor, wherein a longitudinal distance which the ego vehicle (E) should maintain to each relevant surrounding traffic participant and object is defined with respect to each possible lateral motion trajectory of the requested maneuver and corresponding longitudinal safety critical zones.

19. The method according to claim 14 further comprising updating the safety critical zones of each relevant surrounding traffic participant and object according to the longitudinal motion of the requested maneuver utilizing the determined longitudinal motion trajectory of the requested maneuver.

20. The method according to claim 14 wherein the corresponding longitudinal safety critical zone for each relevant surrounding traffic participant and object comprises a miscellaneous shape comprising a triangular, semi-circular, or ellipsoid shape.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) In the following, embodiments herein will be described in greater detail by way of example only with reference to attached drawings.

(2) FIG. 1 is a schematic illustration of vehicles travelling on a one-way road with two lanes with rectangular safety critical zones (dashed) and triangular safety critical zones (dotted) for each surrounding vehicle;

(3) FIG. 2 is a schematic illustration of the maximum and minimum bounds on the ego vehicle's longitudinal position depending on its planned lateral position for rectangular and triangular safety critical zones for each surrounding vehicle;

(4) FIG. 3 is a schematic illustration of a lane change maneuver according to a first scenario;

(5) FIG. 4 Top: illustrates longitudinal position trajectory for the ego vehicle's lane change maneuver according to a first scenario, respecting the longitudinal safety corridor defined by the triangular safety critical zones while the longitudinal safety corridor defined by the rectangular safety critical zones is violated;

(6) FIG. 4 Bottom: illustrates the ego vehicle's longitudinal velocity and acceleration trajectory for the lane change maneuver according to a first scenario;

(7) FIG. 5 Top: illustrates lateral position trajectory for the ego vehicle's lane change maneuver, according to a first scenario, respecting the lateral safety corridor defined by the triangular safety critical zones while the lateral safety corridor defined by the rectangular safety critical zones is violated;

(8) FIG. 5 Bottom: illustrates the ego vehicle's lateral velocity and acceleration trajectory for the lane change maneuver according to a first scenario;

(9) FIG. 6 is a schematic illustration of the lane change maneuver according to a second scenario;

(10) FIG. 7 Top: illustrates longitudinal position trajectory for the ego vehicle's lane change maneuver according to a second scenario, respecting the longitudinal safety corridor defined by the triangular safety critical zones while the longitudinal safety corridor defined by the rectangular safety critical zones is violated;

(11) FIG. 7 Bottom: illustrates the ego vehicle's longitudinal velocity and acceleration trajectory for the lane change maneuver according to a second scenario;

(12) FIG. 8 Top: illustrates alateral position trajectory for the ego vehicle's lane change maneuver according to a second scenario, respecting the lateral safety corridor defined by the triangular safety critical zones while the lateral safety corridor defined by the rectangular safety critical zones is violated;

(13) FIG. 8 Bottom: illustrates the ego vehicle's lateral velocity and acceleration trajectory for the lane change maneuver according to a second scenario;

(14) FIG. 9 is a table illustrating Initial conditions for the two considered scenarios of the lane change traffic situation, x.sub.0 denotes the relative position of the ego vehicle and the surrounding vehicles S.sub.q, q={1, 2, 3};

(15) FIG. 10 is a table illustrating general design parameters for the longitudinal trajectory planning optimization problem; and

(16) FIG. 11 is a table illustrating general design parameters for the lateral trajectory planning optimization problem.

(17) FIG. 12 is a flow chart illustrating an exemplary embodiment of the method as described herein.

(18) FIG. 13 is a block diagram illustrating an exemplary ego vehicle and exemplary embodiments of the systems as described herein.

(19) Still other features of embodiments herein will become apparent from the following detailed description considered in conjunction with the accompanying drawings. It is to be understood, however, that the drawings are designed solely for purposes of illustration and not as a definition of the limits hereof, for which reference should be made to the appended claims. It should be further understood that the drawings are not necessarily drawn to scale and that, unless otherwise indicated, they are merely intended to conceptually illustrate the structures and procedures described herein.

DETAILED DESCRIPTION

(20) This disclosure is based on the realization that in order to increase the autonomous capability of commercial ADAS and eventually allow for fully autonomous driving functionality, a vehicle system should be able to plan how the vehicle should perform different maneuvers in terms of e,g., longitudinal and lateral position, velocity and acceleration trajectories, in order to safely and comfortably follow its planned route while accounting for, and adapting to, the surrounding environment. For that reason, this disclosure is concerned with trajectory planning for an ego vehicle E. In particular, the disclosure proposes a novel trajectory planning method which is applicable to both normal and high risk driving situations.

(21) The trajectories generated using the proposed method may also be used in order to make decisions if a certain maneuver should be performed or not, and also, if required, in order to decide if an initiated maneuver should be aborted. Aborting a maneuver may be appropriate if the predictions made when initiating the maneuver are no longer valid. The trajectories generated may further be used once the maneuver is performed by longitudinal and/or lateral control of ego vehicle, either by an ADAS-system where a vehicle driver supervises the maneuver or where the maneuver is executed autonomously. The trajectories generated may still further be used to provide information/support/guidance to a vehicle driver once the vehicle driver wishes to perform a maneuver. It is further feasible to use the trajectories generated to provide a warning to a vehicle driver and/or perform brakingand/or steering-interventions in order to assist a vehicle driver to abort or complete a maneuver initiated by the vehicle driver should the vehicle driver act in a manner which is judged as unsafe from a comparison with the trajectories generated in accordance with the proposed method.

(22) Since a safe trajectory is used to realize automated ego vehicles E, trajectory planning approaches based on numerical optimization are attractive because the framework provides a means to systematically handle system objectives and constraints. For that reason, herein is presented a trajectory planning method using an algorithm in the Model Predictive Control (MPC) framework. In particular, it extends an extant algorithm by allowing the algorithm to plan trajectories which account for motion dependent safety critical zones of miscellaneous shape defined by both the planned lateral and longitudinal motion of the automated ego vehicle, which the automated ego vehicle E should not enter in order to avoid collisions with surroundings traffic participants and objects.

(23) As such, the algorithm of the proposed method does not only account for rectangular safety critical zones which are defined by, e,g., a time gap which the automated vehicle should maintain to surrounding traffic participants, but rather allows for safety critical zones defined by both the planned lateral and longitudinal motion of the automated vehicle. An automated ego vehicle E is thereby able to efficiently utilize the free road space and traverse dense traffic situation in a self-assertive manner rather than exhibit an excessively conservative behavior.

(24) Firstly will be described the considered trajectory planning problem.

(25) To successfully negotiate traffic situations, an intelligent vehicle system which controls the ego vehicle, E, should be able to plan motion trajectories which allow ego vehicle E to follow its desired route while: avoiding collisions with all surrounding traffic participants and objects, respecting physical and design limitations as well as the traffic rules.

(26) In addition, ego vehicle E should be able to traverse dense traffic situations in a self-assertive manner rather than excessively conservative.

(27) As an illustrative example, consider the traffic situation depicted in FIG. 1 in which ego vehicle E and the preceding vehicle S.sub.1 drive in the lane L.sub.1 and two surrounding vehicles S.sub.2 and S.sub.3 drive in the left adjacent lane L.sub.2. By restricting ego vehicle E to remain outside the safety critical zones it is possible to plan motion trajectories which ensure that ego vehicle E avoids collision conflicts with all surrounding vehicles. However, when e,g., planning a lane change maneuver from L.sub.1 to L.sub.2 in the inter vehicle traffic gap between S.sub.2 and S.sub.3 the maneuver might be unfeasible with respect to rectangular safety critical zones (dashed) defined by, e,g., a time gap which ego vehicle E should maintain to the surrounding vehicles, as illustrated in the top plot of FIG. 1. This while a safe maneuver might exist for safety critical zones which are defined differently, e,g., triangular safety critical zones (dotted) as illustrated in the bottom plot of FIG. 1. Hence, when planning a motion trajectory, e,g., to execute a lane change maneuver, the safety critical zones which ego vehicle E should respect in order to avoid collision conflicts should account for both the planned lateral and longitudinal motion of E.

(28) With reference to FIG. 12, which shows a flow chart of an exemplary embodiment of the method 100 as describe herein, to account for safety critical zones of miscellaneous shape defined by both the lateral and longitudinal motion of ego vehicle E, the trajectory planning algorithm plans a given maneuver, e,g., a lane change maneuver in a certain inter-vehicle traffic gap, in five main steps:

(29) A. Determine 110 the possible lateral motion trajectories of the maneuver.

(30) B. Determine 120 the longitudinal safety critical zones which correspond to each of the possible lateral trajectories.

(31) C. Determine 130 the longitudinal motion trajectory of the maneuver.

(32) D. Determine 140 the lateral safety critical zones which correspond to the planned longitudinal trajectory.

(33) E. Determine 150 the lateral motion trajectory of the maneuver.

(34) Further details regarding each step of the algorithm will be provided in the following.

(35) The trajectory planning algorithm is formulated based on the following set of assumptions:

(36) A1 Ego vehicle E is equipped with sensor systems which measure its position on the road as well as, e,g., the relative positions and velocities of surrounding traffic participants and objects. Such sensor systems may comprise sensors such as, e,g., one or more of a Radio Detection And Ranging (RADAR) sensor, a Light Detection And Ranging (LIDAR) sensor, a Light Amplification by Stimulated Emission of Radiation (LASER) sensor, a SOund NAvigation and Ranging (SONAR) sensor (e,g., ultrasound), an InfraRed (IR) sensor, an image sensor (e,g., camera), vehicle location devices (e,g., GPS) or any combinations thereof.
A2 Ego vehicle E is equipped with prediction systems which estimate the motion trajectories of surrounding traffic participants and objects over a time horizon.
A3 Ego vehicle E is equipped with low-level control systems capable of following the planned trajectory.
A4 Ego vehicle E is equipped with a decision-making system which provides a desired maneuver request, e,g., a left lane change maneuver.

(37) With reference to FIG. 13, which illustrates an exemplary ego vehicle (E) 200, it should be noted that any driver assistance system and/or advanced driver assistance system 210, sensor system 220, prediction system 230, control system 240, and/or decision-making system 250, as well as any other system, device, unit or the like described herein may comprise and/or be implemented in or by one or more appropriately programmed processors (e.g., one or more microprocessors including central processing units (CPU)) and associated memory and/or storage, which may include operating system software, application software and/or any other suitable program, code or instructions executable by the processor(s) for controlling operation thereof, for providing and/or controlling interaction and/or cooperation between the various features and/or components described herein, and/or for performing the particular algorithms represented by the various functions and/or operations described herein.

(38) Examples of the assumed decision-making system, low-level control system, prediction system, and the necessary sensor technology are extant. Furthermore, uncertainties resulting from the sensor and prediction systems can be taken into account by, e,g., increasing the safety margins which ego vehicle E should maintain to the surrounding traffic participants and objects over the prediction horizon in relation to the confidence level of the assumed systems. In addition, the re-planning nature of receding horizon MPC allows changes in the perceived environment to be accounted for at each time instance.

(39) In the following will be provided further details regarding the step of determining the possible lateral motion trajectories of the maneuver.

(40) In order to account for the lateral motion of ego vehicle E when determining the longitudinal safety critical zones, i.e., the distance which ego vehicle E should maintain to each relevant traffic participant and object at each time instant, some lateral motion trajectories of the maneuver should be determined. For instance, if ego vehicle E should traverse an intersection or a roundabout, some possible lateral trajectories can be defined such that they follow the shape of a typical maneuver in the corresponding traffic environment. Furthermore, by considering a lane change maneuver as laterally moving ego vehicle E from its current lane into its target lane, lateral lane change trajectories can be generated, e,g., as splines or by a standard LQ-controller tracking some reference function, e,g., a step function from E's current to its desired lateral position, e,g., the center of the adjacent lane, or the rightmost (if a left lane change) or leftmost (if a right lane change) intersection point of the anticipated safety critical zones of the vehicles which constitutes the targeted inter-vehicle traffic gap.

(41) A set of possible lateral motion trajectories for the maneuver can thus be generated by initiating the lateral motion of the maneuver over a discrete time interval which may be defined as
i[1, . . . ,min(N,N.sub.g)(n.sub.o+n.sub.min)];(1)
where N denotes the prediction horizon, N.sub.g denotes the time instance for which the targeted inter-vehicle traffic gap is no longer appropriate, n.sub.o denotes the predicted time it takes for the vehicles constituting a tight inter-vehicle traffic gap to smoothly adjust their velocities in order to allow ego vehicle E to enter the gap, and n.sub.min denotes the minimum time it takes for ego vehicle E to laterally move into the targeted inter-vehicle traffic gap.

(42) In the following will be provided further details regarding the step of determining the longitudinal safety critical zones which correspond to each of the possible lateral trajectories.

(43) When the set of possible lateral motion trajectories of the maneuver has been generated, the corresponding longitudinal safety critical zone for each relevant surrounding traffic participant and object can be defined as an appropriate miscellaneous shape, e,g., rectangular, triangular, or ellipsoid. For instance, the safety critical zone of a surrounding vehicle can, e,g., be defined as a box which covers the vehicle's body and two rectangular/triangular/semicircular areas as illustrated in FIG. 2. The safety margin which defines the front most position of the rectangle/triangle/semicircle is denoted by m.sub.f, which may be defined as
m.sub.f(S.sub.jk)=max(S.sub.jk,S.sub.jkvS.sub.jk),k=1, . . . ,N,(2)
where vS.sub.j denotes the longitudinal velocity of the surrounding vehicle S.sub.j, and S.sub.j and S.sub.j respectively denotes the minimum distance and time gap which ego vehicle E should maintain to S.sub.j with respect to S.sub.j 's front. Similarly, the safety margin which defines the rearmost position of the rectangle/triangle/semicircle is denoted by m.sub.r, which may be defined as
m.sub.r(S.sub.jk)=max(S.sub.jk,S.sub.jkvS.sub.jk),k=1, . . . ,N,(3)
where S.sub.j and S.sub.j respectively denotes the minimum distance and time gap which ego vehicle E should maintain to S.sub.j with respect to S.sub.j 's rear.

(44) For each possible lateral motion trajectory of the maneuver and corresponding longitudinal safety critical zones, the maximum and minimum bounds on ego vehicle E's longitudinal position at each time instance can thus be defined as a longitudinal safety corridor. As such, the longitudinal distance which ego vehicle E should maintain to each surrounding vehicle is defined with respect to its planned lateral motion trajectory. For instance, when planning a left lane change maneuver, the upper bound on ego vehicle E's longitudinal position throughout the maneuver can be determined as

(45) $\begin{array}{cc}{x}_{{\max }_k}=\{\begin{array}{cc}\underset{j{}_{L_1}}{\min }{x}_{r_{s_{j_k}}}& \mathrm{if}{y}_k{c}_k-\frac{w}{2},\\ \underset{j{}_{L_1},{}_{L_2}}{\min }{x}_{r_{s_{j_k}}}& \mathrm{if}{c}_k-\frac{w}{2}<y_k<c_k+\frac{w}{2},\\ \underset{j{}_{L_2}}{\min }{x}_{r_{s_{j_k}}}& \mathrm{if}{y}_k{c}_k+\frac{w}{2},\\ & k=1,\mathrm{.Math.},N,\end{array}& \left(4\right)\end{array}$
where

(46) $x_{r_{s_j}}$
denotes the longitudinal safe position with respect to the rear of the j-th surrounding vehicle, y denotes ego vehicle E's planned lateral position, c denotes the center-line of ego vehicle E's current and target lane, and w denotes the width of ego vehicle E.

(47) The sets F.sub.L.sub.1 and F.sub.L.sub.2 respectively contains the preceding surrounding vehicles in the lanes L.sub.1 and L.sub.2.

(48) Similarly, the lower boundary on the longitudinal position of ego vehicle E is determined by

(49) $\begin{array}{cc}{x}_{{\min }_k}=\{\begin{array}{cc}\underset{j{}_{L_1}}{\max }{x}_{f_{s_{j_k}}}& \mathrm{if}{y}_k{c}_k-\frac{w}{2},\\ \underset{j{}_{L_1},{}_{L_2}}{\max }{x}_{f_{s_{j_k}}}& \mathrm{if}{c}_k-\frac{w}{2}<y_k<c_k+\frac{w}{2},\\ \underset{j{}_{L_2}}{\max }{x}_{f_{s_{j_k}}}& \mathrm{if}{y}_k{c}_k+\frac{w}{2},\\ & k=1,\mathrm{.Math.},N,\end{array}& \left(5\right)\end{array}$
where

(50) $x_{f_{s_j}}$
denotes the longitudinal safe position with respect to the front of the j-th surrounding vehicle and the sets R.sub.L.sub.1 and R.sub.L.sub.2 respectively contains the rear surrounding vehicles in the lanes L.sub.1 and L.sub.2

(51) Hence, if the safety critical zones of surrounding vehicles are defined by, e,g., a triangular or a semicircular shape, ego vehicle E is allowed to squeeze into inter-vehicle traffic gaps since the longitudinal distance which it should maintain to each surrounding vehicle depends on its planned lateral position as depicted in FIG. 2.

(52) In the following will be provided further details regarding the step of determining the longitudinal motion trajectory of the maneuver.

(53) For each longitudinal safety corridor it should be determined if there exists a longitudinal trajectory which allow ego vehicle E to be positioned within the corridor over the prediction horizon and selected, from the determined existing longitudinal trajectories for all longitudinal safety corridors, the most appropriate longitudinal trajectory. To simply determine whether such a longitudinal trajectory exist, the reachable set of ego vehicle E can crudely be approximated by a set of trajectories which, e,g., is generated by an ACC function. To allow for safe and smooth maneuvers which are within the capability of the assumed low-level control systems of ego vehicle E, the most appropriate trajectory is selected as the trajectory which fulfills the following set of constraints:

(54) $\begin{array}{cc}{x}_{{\min }_k}{x}_k{x}_{{\max }_k};k=1,\mathrm{.Math.},N& \left(6a\right)\\ v_{x_{{\min }_k}}{v}_{x_k}{v}_{x_{{\max }_k}};k=1,\mathrm{.Math.},N& \left(6b\right)\\ a_{x_{{\min }_k}}{a}_{x_k}{a}_{x_{{\max }_k}};k=1,\mathrm{.Math.},N& \left(6c\right)\\ a_{x_{{\min }_k}}{a}_{x_k}{a}_{x_{{\max }_k}};k=1,\mathrm{.Math.},N& \left(6d\right)\end{array}$
while minimizing the cost function

(55) $\begin{array}{cc}{J}_x=\underset{k=1}{\overset{N}{.Math.}}{{}_k\left(v_{x_k}-v_{x_{{\mathrm{des}}_k}}\right)}^2+a_{x_k}^2+{}_k{a}_{x_k}^2;& \left(7\right)\end{array}$

(56) Where x, v.sub.x, .sub.x, and .sub.x respectively denotes ego vehicle E's longitudinal position, velocity, acceleration, and jerk, v.sub.x.sub.des denotes ego vehicle E's desired longitudinal velocity, and , , and are positive weights.

(57) When the most appropriate longitudinal motion trajectory and corresponding safety corridor have been determined it is possible to refine the trajectory by formulating and solving a standard Quadratic Program (QP) optimization problem

(58) $\begin{array}{cc}\underset{w}{\min }J=\frac{1}{2}{w}^t\mathrm{Hw}+d^{T}w,& \left(8a\right)\\ \mathrm{subject}\mathrm{to}& \\ H_{\mathrm{eq}}w=k_{\mathrm{eq}};& \left(8b\right)\\ H_{in}w{k}_{in};& \left(8c\right)\end{array}$

(59) In the following will be provided further details regarding the step of determining the lateral safety critical zones which correspond to the planned longitudinal trajectory.

(60) Utilizing the longitudinal motion trajectory, the safety critical zone of each surrounding vehicle can be updated according to the longitudinal motion of the maneuver. As such a lateral safety corridor can be created in a similar manner as for the longitudinal safety corridor described above.

(61) In the following will be provided further details regarding the step of determining the lateral motion trajectory of the maneuver.

(62) To finalize the trajectory planning of a certain maneuver, the lateral trajectory planning problem is formulated as a standard QP optimization problem (8) with the cost function defined as
J.sub.y=.sub.k=1.sup.N.sub.kv.sub.y.sub.k.sup.2+.sub.ka.sub.y.sub.k.sup.2+.sub.ka.sub.y.sub.k.sup.2,(9)
where v.sub.y, a.sub.y, and a.sub.y respectively denotes ego vehicle E's lateral velocity, acceleration, and jerk, and , , and are positive weights. Furthermore, the lateral dynamics of ego vehicle E is modeled by a simple double integrator in order to linearly express the lateral motion of ego vehicle E as

(63) $\begin{array}{cc}{y}_{k+1}=y_k+v_{y_k}{t}_s+a_{y_k}\frac{t_s^2}{2};k=0,\mathrm{.Math.},N& \left(10a\right)\\ v_{y_{k+1}}=v_{y_k}+a_{y_k}{t}_s;k=0,\mathrm{.Math.},N& \left(10b\right)\end{array}$
where y denotes ego vehicle E's lateral position and t.sub.s denotes the discrete sampling time. To allow for safe and smooth maneuvers which are within the capability of the assumed low-level control systems of ego vehicle E, the system (10) is subjected to the following set of constraints

(64) $\begin{array}{cc}{y}_{{\min }_k}{y}_k{y}_{{\max }_k};k=1,\mathrm{.Math.},N& \left(11a\right)\\ v_{y_{{\min }_k}}{v}_{y_k}{v}_{y_{{\max }_k}};k=1,\mathrm{.Math.},N& \left(11b\right)\\ a_{y_{{\min }_k}}{a}_{y_k}{a}_{y_{{\max }_k}};k=1,\mathrm{.Math.},N& \left(11c\right)\\ a_{y_{{\min }_k}}{a}_{y_k}{a}_{y_{{\max }_k}};k=1,\mathrm{.Math.},N& \left(11d\right)\end{array}$

(65) The optimization problem has N optimization variables, i.e., control input a.sub.y, and 10N linear constraints corresponding to vehicle dynamics (10) as well as system limitations and design constraints (11).

(66) In the following section is elaborated how the proposed trajectory planning algorithm may be evaluated in a simulated traffic situation as illustrated in FIGS. 1-2. Two scenarios of the traffic situation are considered, referred to as Scenario 1 and Scenario 2, for which the initial conditions are given in FIG. 9, which is a table illustrating Initial conditions for the two considered scenarios of the lane change traffic situation, where Axo denotes the relative position of the ego vehicle and the surrounding vehicles S.sub.q, q={1, 2, 3}.

(67) In both scenarios, the aim of the trajectory planning algorithm is to plan a lane change maneuver which allows ego vehicle E to perform a left lane change in the inter-vehicle traffic gap between surrounding vehicles S.sub.2 and S.sub.3. As indicated in the table of FIG. 9, Scenario 1 thus entails ego vehicle E to decelerate in order to perform the lane change maneuver while Scenario 2 entails ego vehicle E to accelerate in order to perform the maneuver.

(68) For ease of illustration it is assumed in both scenarios that surrounding vehicles S.sub.1; S.sub.2; and S.sub.3 drive at constant longitudinal velocity without performing lane change maneuvers over the prediction horizon. However, any predicted behavior i.e., trajectory, of the surrounding vehicles can be incorporated into the trajectory planning algorithm when defining the longitudinal and lateral safety corridors as described above.

(69) The trajectory planning algorithm has been implemented as a longitudinal and a lateral QP optimization problem in CVXGEN interfaced with Matlab. The general design parameters for the longitudinal and lateral trajectory planning optimization problems are given in FIGS. 10 and 11 respectively. Thus, FIG. 10 is a table illustrating general design parameters for the longitudinal trajectory planning optimization problem and FIG. 11 is a table illustrating general design parameters for the lateral trajectory planning optimization problem.

(70) As indicated in the table of FIG. 9, in Scenario 1 all vehicles i.e., E; S.sub.1; S.sub.2; and S.sub.3 initially drive at the same longitudinal velocity, and surrounding vehicles S.sub.2 and S.sub.3 are positioned behind E. Hence ego vehicle E should reduce its velocity in order to reach the inter-vehicle traffic gap between surrounding vehicles S.sub.2 and S.sub.3 and perform the lane change maneuver.

(71) FIG. 3 provides a schematic illustration of the planned lane change maneuver while FIGS. 4-5 respectively shows the longitudinal and lateral motion trajectories. From the figures it can be seen that when the safety critical zones for each surrounding vehicle are defined as triangular areas, a lane change maneuver can be successfully planned while the maneuver is unfeasible with respect to rectangular safety critical zones. Furthermore, FIGS. 4-5 show that ego vehicle E decelerates in order to reach the inter-vehicle traffic gap between surrounding vehicles S.sub.2 and S.sub.3, and once it is aligned with the gap it performs the lateral motion into the target lane and increases its longitudinal velocity in order to maintain a safe distance to surrounding vehicles S.sub.2 and S.sub.3.

(72) FIG. 6 provides a schematic illustration of the planned lane change maneuver for Scenario 2 in which ego vehicle E and surrounding vehicle S1 initially drive at 15 m/s while surrounding vehicles S.sub.2 and S.sub.3 drive at 25 m/s as indicated in the table of FIG. 9. As shown in FIGS. 7-8 ego vehicle E should accelerate in order to reach the inter-vehicle traffic gap between surrounding vehicles S.sub.2 and S.sub.3, and once it is aligned with the gap it performs the lateral motion into the target lane. From FIGS. 6-8 it can further be seen that when the safety critical zones for each surrounding vehicle are defined as triangular areas, a lane change maneuver can be successfully planned while the maneuver is unfeasible with respect to rectangular safety critical zones.

(73) In both Scenario 1 and Scenario 2, the planned trajectory entails ego vehicle E to not be fully positioned in the target lane at the end of the prediction horizon. Subsequently, if neither surrounding vehicles S.sub.2 nor S.sub.3 adjusts their velocity to allow ego vehicle E to complete the lane change maneuver while its execution, ego vehicle E should be able to abort the maneuver and return to its original lane. Hence, if a trajectory which entails ego vehicle E to be fully positioned in the target lane cannot be planned prior to ego vehicle E being positioned at a minimum distance to surrounding vehicle S.sub.1 for which it can smoothly adjust its trajectory and safely follow surrounding vehicle S.sub.1 in the original lane, the maneuver should be aborted.

(74) Thus, as shown above a trajectory planning algorithm that accounts for safety critical zones of miscellaneous shape defined by both the planned lateral and longitudinal motion of the automated vehicle may be used for the method presented herein. As such, the proposed Method is able to generate trajectories which allow the ego vehicle, especially if partly or highly automated, to efficiently utilize the free road space, e,g., by safely, smoothly, and self-assertively squeeze into an inter-vehicle traffic gap in the target lane in order to perform a tight lane change maneuver.

(75) As described above the method of trajectory planning for maneuvers for an ego vehicle E equipped with: sensor systems to measure its position on the road as well as the relative positions and velocities of surrounding traffic participants and objects; prediction systems to estimate the motion trajectories of surrounding traffic participants and objects over a defined time horizon; low-level control systems capable of following a planned trajectory; and a decision-making system to provide a desired maneuver request, comprises the steps of, using receding horizon control applying a finite prediction horizon, for determining:

(76) possible lateral motion trajectories of a requested maneuver;

(77) longitudinal safety critical zones which correspond to each of the determined possible lateral motion trajectories;

(78) a longitudinal motion trajectory of the requested maneuver;

(79) lateral safety critical zones which correspond to the determined longitudinal motion trajectory of the requested maneuver;

(80) a lateral motion trajectory of the requested maneuver.

(81) According to some embodiments the method further comprises generating a set of possible lateral motion trajectories for the requested maneuver by initiating the lateral motion of the maneuver over a discrete time interval.

(82) According to further embodiments the discrete time interval is defined as i [1, . . . , min(N, N.sub.g)(n.sub.o+n.sub.min)] where N denotes the prediction horizon, N.sub.g denotes the time instance for which the targeted inter-vehicle traffic gap is no longer appropriate, n.sub.o denotes the predicted time it takes for any surrounding vehicles constituting a tight inter-vehicle traffic gap to smoothly adjust their velocities in order to allow ego vehicle E to enter the gap, and n.sub.min denotes the minimum time it takes for ego vehicle E to laterally move into the targeted inter-vehicle traffic gap.

(83) According to yet some embodiments the method further comprises defining, when the set of possible lateral motion trajectories of the maneuver has been generated, a corresponding longitudinal safety critical zone for each relevant surrounding traffic participant and object as an appropriate miscellaneous shape, such as rectangular, triangular, or ellipsoid.

(84) According to still further embodiments the method further comprises defining, for each possible lateral motion trajectory of the requested maneuver and corresponding safety critical zones, the maximum and minimum bounds on the longitudinal position of ego vehicle E at each time instance as a longitudinal safety corridor, such that the longitudinal distance which ego vehicle E should maintain to each relevant surrounding traffic participant and object is defined with respect to each possible lateral motion trajectory of the requested maneuver and corresponding safety critical zones.

(85) According to yet further embodiments the method further comprises determining, for each longitudinal safety corridor, if there exists a longitudinal trajectory which allow ego vehicle E to be positioned within the corridor over the prediction horizon and selecting, from the determined existing longitudinal trajectories for all longitudinal safety corridors, the most appropriate longitudinal trajectory.

(86) According to yet some embodiments the method further comprises updating the safety critical zones of each relevant surrounding traffic participant and object according to the longitudinal motion of the requested maneuver utilizing the determined longitudinal motion trajectory of the requested maneuver.

(87) According to still some embodiments the step of determining a lateral motion trajectory of the requested maneuver further comprises formulating the lateral motion trajectory planning problem as a standard Quadratic Program optimization problem and solving this problem.

(88) According to yet further embodiments the method further comprises utilizing at least part of one of the determined longitudinal and lateral motion trajectories of the requested maneuver to determine longitudinal and/or lateral control signals for control of the ego vehicle.

(89) According to still some further embodiments the method further comprises the step of combining the longitudinal and lateral control signals to a combined control signal for combined longitudinal and lateral control of the ego vehicle.

(90) The proposed method is also considered to be a building block for Advanced Driver Assistance Systems (ADAS) and eventually highly automated vehicles which are safe, smooth, and self-assertive.

(91) This, in accordance with the present application is also envisaged an Advanced Driver Assistance System arranged to perform the method of trajectory planning for maneuvers as described in the foregoing.

(92) In accordance with the present application is also envisaged an Advanced Driver Assistance System where at least part of one of the determined longitudinal and lateral motion trajectories of the requested maneuver is utilized for one or more of the following: deciding if a maneuver should be initiated or aborted; performing a maneuver, autonomously or driver supervised; provide driver guidance for performing a maneuver; provide driver warning and/or information when a maneuver could and should not be performed.

(93) Here it is possible to choose to only perform/evaluate or use part of either trajectory, i.e., part of the prediction time to: inform, warn, support or evaluate a vehicle driver, i.e., if the driver requires information in order to perform a maneuver, requires a warning because a high risk maneuver has been initiated, requires support through the maneuver being performed by an ADAS whilst the driver supervises performance thereof, performance/abortion of an autonomous maneuver, or for evaluating the behavior of the driver, e,g., in terms of which margins the driver keeps to surrounding objects during a certain type of maneuver.

(94) In accordance with the present application is also envisaged a vehicle comprising an Advanced Driver Assistance Systems arranged to implement the method of trajectory planning for maneuvers as described in the foregoing.

(95) The above-described embodiments may be varied within the scope of the following claims.

(96) Thus, while there have been shown and described and pointed out fundamental novel features of the embodiments herein, it will be understood that various omissions and substitutions and changes in the form and details of the devices illustrated, and in their operation, may be made by those skilled in the art. For example, it is expressly intended that all combinations of those elements and/or method steps which perform substantially the same function in substantially the same way to achieve the same results are equivalent. Moreover, it should be recognized that structures and/or elements and/or method steps shown and/or described in connection with any disclosed form or embodiment herein may be incorporated in any other disclosed or described or suggested form or embodiment as a general matter of design choice.