GB2570887A

GB2570887A - A system for a vehicle

Info

Publication number: GB2570887A
Application number: GB1801968.7A
Authority: GB
Inventors: Montanaro Umberto; Fallah Saber; Dianati Mehrdad; Dixit Shilp
Original assignee: University of Surrey; University of Warwick; Jaguar Land Rover Ltd
Current assignee: University of Surrey; University of Warwick; Jaguar Land Rover Ltd
Priority date: 2018-02-07
Filing date: 2018-02-07
Publication date: 2019-08-14
Anticipated expiration: 2038-02-07
Also published as: GB2570887B; GB201801968D0; DE102019201124A1

Abstract

A set of reachable vehicle states, which include position, is determined by module 20 depending on a vehicle constraint. Of these, a set of allowable vehicle positions is determined by module 22 using data indicative of a feature of a nearby road segment. One allowable position is set as a target vehicle position by module 24, and a feasible trajectory to that position is determined by module 26. The vehicle may be controlled to follow the trajectory using module 18. The road segment feature may relate to obstacles, lane boundaries or driving rules. The allowable positions may be determined using an artificial potential field, with potential values within a certain range indicating allowable positions. The vehicle constraint may include state variables such as heading, position or velocity, or input constraints such as steering angle or acceleration. The feasible trajectory may be generated using a Model Predictive control (MPC) strategy.

Description

The present disclosure relates to a system for a vehicle and particularly, but not exclusively, to a system for a vehicle operable in an autonomous mode. Aspects of the invention relate to a system, to a method, and to a vehicle.

BACKGROUND

The first generation of vehicles having autonomous driving modes were capable of performing tasks such as lane-keeping, maintaining a distance to a vehicle in front, providing a lane departure warning, etc. This helped in increasing occupant comfort and reducing the workload on the driver. A human driver is still needed to either initiate, or intervene while performing, more challenging but equally common manoeuvres such as lane changing, merging, overtaking, etc. For example, overtaking a vehicle in front represents a template of such complex manoeuvres as it both: combines lateral and longitudinal motion of an overtaking vehicle (subject vehicle) while avoiding a slower moving obstacle vehicle (lead vehicle); and, includes the sub-manoeuvres of lanechanging, lane-keeping, and merging in a sequential manner.

The inherently intricate structure of overtaking stems from its dependence on a large number of factors such as road conditions, weather, traffic density, type of overtaking vehicle, type of vehicle to be overtaken, relative velocity between vehicles, distance travelled, etc., which makes classification and standardisation relatively difficult. Moreover, in order to perform overtaking manoeuvres accurate information relating to road and lane availability, lead vehicle trajectory, road conditions, etc., is needed.

There have been a number of proposed methods for planning trajectories to perform a complex autonomous manoeuvre such as an autonomous overtaking manoeuvre by treating it as a moving-obstacle avoidance problem. For example, incremental, search-based algorithms such as ‘Rapidly exploring Random Trees’ (RRT) have been proposed for planning trajectories for a complex autonomous manoeuvre; however, RRT algorithms are disadvantageous in that the planned trajectories can be jerky, which in turn may lead to reduced occupant comfort. Furthermore, the computation times for most search-based algorithms are dependent on surrounding traffic density which makes them less suitable for automotive applications.

If accurate information regarding the road and surrounding obstacles is available, potential field based techniques are shown to be successful at generating trajectories that avoid stationary or moving obstacles; however, these methods do not incorporate vehicle dynamics and so cannot ensure how feasible it is for the vehicle to actually follow the planned trajectory.

Model Predictive Control (MPC) methods have the ability to formulate vehicle dynamics and obstacle avoidance constraints as a finite-horizon constrained optimisation problem. These methods are disadvantageous in that obstacle avoidance constraints for trajectory planning are generally non-convex, which greatly limits the feasibility and uniqueness of the solution to the optimisation problem. Techniques such as convexification, changing the reference frame, and approximation have been proposed to address this issue. Nevertheless, the optimisation problem formulated by these approaches often require constraint equalities that change in realtime, which makes them difficult to implement on a vehicle electronic control unit (ECU).

The concept of motion primitives included within an MPC framework has been proposed to plan obstacle avoidance trajectories. However, as these motion primitives are computed offline and accessed via a look-up table, only a subset of all feasible trajectories may be available for trajectory planning.

It has been suggested to generate planned overtaking trajectories, for example, by directing the subject vehicle along virtual target points located at sufficient distances around the lead vehicle, thus reformulating trajectory planning into a navigation problem. However, in such methods the subject vehicle is modelled as a point mass with no dynamics, and so these methods are unsuitable for complex-manoeuvre trajectory planning of autonomous vehicles.

It is an aim of the present invention to address disadvantages associated with the prior art.

SUMMARY OF THE INVENTION

According to an aspect of the present invention there is provided a system for a vehicle operable in an autonomous mode, the system being for planning a trajectory of the vehicle. The system comprises a receiver configured to receive road segment data from at least one subsystem of the vehicle, the road segment data being indicative of at least one feature of a road segment in the vicinity of the vehicle. The system comprises a reachable state module configured to determine a set of reachable vehicle states within a prescribed period of time in dependence on at least one vehicle constraint, each of the reachable vehicle states including a reachable vehicle position. The system comprises an allowable position module configured to determine a plurality of allowable vehicle positions in the road segment in dependence on the received road segment data, the plurality of allowable vehicle positions being within the determined set of reachable vehicle positions. The system comprises a target state module configured to determine a target vehicle state having a target vehicle position in the road segment, the target vehicle position corresponding to one of the plurality of determined allowable vehicle positions. The system comprises a trajectory generation module configured to determine a feasible trajectory for the vehicle to travel to the target vehicle position in dependence on the at least one vehicle constraint so as to plan the vehicle trajectory.

The invention provides a situational awareness and trajectory planning system for performing complex autonomous manoeuvres such as overtaking on a motorway. The system is advantageous in that it is free from non-convex obstacle avoidance constraints, it ensures the feasibility of the proposed trajectory, and it is real-time implementable. That is, the system is implementable on an electronic control unit (ECU) of the vehicle and functions within the processor cycle time of the ECU.

The system may comprise an output configured to send a control signal to control the vehicle to follow the feasible trajectory.

The at least one road segment feature may comprise at least one of: a current trajectory of the vehicle; road and/or lane boundaries; and, driving rules.

The at least one road segment feature may comprise at least one obstacle.

The at least one obstacle may include a further vehicle, and the road segment data includes at least one of a velocity, a position, and an orientation of the further vehicle.

The further vehicle may be ahead of, and in a same lane as, the vehicle.

The at least one obstacle may include at least one of a pothole, a speed bump, and a blocked lane.

The allowable position module may be configured to define an artificial potential field larger than the set of reachable vehicle positions. The allowable position module may be configured to generate a net potential function describing the artificial potential field in dependence on the received road segment data. A potential value of the net potential function may vary across the artificial potential field.

The allowable vehicle positions may be determined to be corresponding positions in the artificial potential field in which the net potential function value lies in a prescribed interval of potential values.

The net potential function may include a contribution from one or more of:

a lane velocity potential function including information relating to a velocity associated with each lane of the road;

a road potential function including information relating to boundaries of the road;

a lane potential function including information relating to boundaries and/or a centre of each lane of the road; and, a further vehicle potential function including information relating to vehicle dynamics of a further vehicle in the road segment.

The at least one vehicle constraint may include one or more current vehicle state variables.

The current vehicle state variables may include a current vehicle position, a current vehicle heading angle, and a current vehicle velocity.

The at least one vehicle constraint may include one or more admissible vehicle state variables.

The admissible vehicle state variables may include an admissible vehicle heading angle, an admissible vehicle velocity, and an admissible vehicle lateral position.

The at least one vehicle constraint may include one or more vehicle input constraints.

The vehicle input constraints may include an admissible vehicle steering angle, and an admissible vehicle acceleration.

The at least one vehicle constraint includes one or more vehicle dynamics constraints.

The vehicle dynamics constraints may include one or more dimensions of the vehicle, a vehicle mass, a vehicle mass distribution, a type of terrain over which the vehicle is travelling, a vehicle tyre type, and a vehicle tyre pressure.

The reachable vehicle states may include a reachable vehicle velocity, a reachable vehicle heading angle, a reachable vehicle longitudinal displacement, and a reachable vehicle lateral displacement.

The set of reachable vehicle states may be selected from a predetermined space of potential sets of reachable vehicle states. Each potential set of reachable vehicle states may have a different combination of values of one or more of the vehicle constraints and/or the prescribed period of time.

The set of reachable vehicle states may be determined at predefined intervals based on values of the at least one vehicle constraint received via vehicle-to-everything communication.

The set of reachable vehicle states may be determined using a continuous time nonlinear kinematic bicycle model.

The target vehicle position may correspond to the allowable vehicle position farthest from a current vehicle position. The allowable vehicle position may correspond to one of the reachable vehicle positions in the set of reachable vehicle states.

The target state module may determine a target vehicle velocity along the feasible trajectory.

The target vehicle velocity may comprise a substantially uniform longitudinal velocity.

The target vehicle states may include a target vehicle velocity and/or a target vehicle heading angle.

The at least one sensor or subsystem of the vehicle may include at least one of:

a vehicle navigation subsystem;

an ultrasonic sensor;

a radar sensor;

a LiDAR sensor;

a vision sensor; and, a vehicle-to-everything transceiver.

The trajectory generation module may be configured to apply a Model Predictive Control strategy to generate the feasible trajectory.

According to another aspect of the present invention there is provided a method for a vehicle operable in an autonomous mode, the method being for planning a road trajectory of the vehicle. The method comprises receiving road segment data from at least one sensor or subsystem of the vehicle, the road segment data being indicative of at least one feature of a road segment in the vicinity of the vehicle. The method comprises determining a set of reachable vehicle states within a prescribed period of time in dependence on at least one vehicle constraint, each of the reachable vehicle states including a reachable vehicle position. The method comprises determining a plurality of allowable vehicle positions in the road segment in dependence on the received road segment data. The method comprises determining a target vehicle state having a target vehicle position in the road segment, the target vehicle position corresponding to one of the determined allowable vehicle positions that is within the determined set of reachable vehicle positions. The method comprises determining a feasible trajectory for the vehicle to travel to the target vehicle position in dependence on the at least one vehicle constraint so as to plan the vehicle trajectory.

According to another aspect of the present invention there is provided a vehicle comprising a system as described above.

According to another aspect of the present invention there is provided a non-transitory, computer-readable storage medium storing instructions thereon that when executed by one or more processors causes the one or more processors to carry out the method described above.

Within the scope of this application it is expressly intended that the various aspects, embodiments, examples and alternatives set out in the preceding paragraphs, in the claims and/or in the following description and drawings, and in particular the individual features thereof, may be taken independently or in any combination. That is, all embodiments and/or features of any embodiment can be combined in any way and/or combination, unless such features are incompatible. The applicant reserves the right to change any originally filed claim or file any new claim accordingly, including the right to amend any originally filed claim to depend from and/or incorporate any feature of any other claim although not originally claimed in that manner.

BRIEF DESCRIPTION OF THE DRAWINGS

One or more embodiments of the invention will now be described, by way of example only, with reference to the accompanying drawings, in which:

Figure 1 is a schematic plan view of a (subject) vehicle including a system according to an embodiment of an aspect of the invention, together with subsystems and sensors providing inputs to the system, the system being for planning a trajectory for the subject vehicle to perform an overtaking manoeuvre on a lead vehicle;

Figure 2 shows the system of Figure 1 in greater detail, in particular the component modules of the system;

Figure 3 is a schematic plan view illustrating the sequential sub-manoeuvres involved in the overtaking manoeuvre for which the system of Figure 1 plans a trajectory;

Figure 4 is a schematic plan view of the road geometry, in particular the coordinate frame and range, for the overtaking manoeuvre for which the system of Figure 1 plans a trajectory;

Figure 5 is a flow diagram showing the steps of a method undertaken by the system of Figure 1 to plan the trajectory for the overtaking manoeuvre;

Figure 6 is a schematic plan view of a subsection of the road of Figure 4 showing the region of all the positions that the subject vehicle may reach within a prescribed period of time from its current position;

Figures 7a and 7b show perspective and plan views, respectively, of a contour plot illustrating a potential field in the vicinity of the subject vehicle as determined by the system of Figure 1;

Figure 8 shows reference target points on the road for the subject vehicle relative to the lead vehicle at a plurality of time steps as determined by the system of Figure 1; and,

Figure 9 illustrates planned trajectories against the actual trajectories of the subject vehicle during the overtaking manoeuvre, the planned trajectories being determined by the system of Figure 1.

DETAILED DESCRIPTION

The present invention provides a system and method for a vehicle operable in an autonomous mode. In particular, the invention provides a system and method for planning a trajectory of the vehicle to undertake a complex manoeuvre. Specifically, the embodiment described below is for performing an overtaking manoeuvre; however, the described system and method are equally suitable for planning trajectories for the vehicle to undertake many different types of complex manoeuvre. Other such complex manoeuvres include taking a motorway exit, lane change, merging into a lane with traffic, and overtaking multiple vehicles at a time.

The invention involves determining a set of so-called reachable states of the vehicle; that is, determining the set of possible positions and heading angles the vehicle may reach from its current position in a prescribed period of time. The invention also includes using received data relating to the road segment surrounding the vehicle to determine a set of so-called allowable positions for the vehicle, i.e. positions of the vehicle that ensure surrounding obstacles and road limits are avoided, where the allowable positions are within the set of reachable states. The set of allowable positions may be determined using potential functions. The invention then involves determining a target state of the vehicle, where the target state includes a target vehicle position that corresponds to one of the allowable positions. The invention also involves generating a trajectory for the vehicle for it to transition from its current state to the target state in a feasible manner. The feasible trajectory may be determined using a Model Predictive Control strategy.

Figure 1 shows a vehicle 10 that is operable in an autonomous mode. The vehicle 10 includes an autonomous or semi-autonomous system 12 that is configured to plan a trajectory for the vehicle 10, control the vehicle 10 to undertake the planned trajectory.

When the vehicle 10 operates in the autonomous mode, the system 12 automatically controls movement of the vehicle 10, e.g. steering and acceleration/deceleration. In the autonomous mode, the system 12 may issue command signals to an electrical power assisted steering (EPAS) system of the vehicle 10 or, in alternative arrangements, the system 12 may otherwise be associated with a steering mechanism of the vehicle 10 such that the system 12 may make adjustments to the steering direction of the vehicle 10. In the autonomous mode, the system 12 may modify, manage or otherwise control a position of the vehicle 10 relative to one or more features that are provided upon a surface on which the vehicle 10 is travelling, for example white lines on a road denoting the boundaries of a “lane”.

In contrast, when the vehicle 10 operates in a manual mode, a driver controls the movement of the vehicle 10. In manual mode, the autonomous system 12 is not in control of the vehicle movement and is in a deactivated state.

The vehicle 10 includes a number of subsystems and sensors 14 that provide data to the autonomous system 12 so that it may plan a vehicle trajectory. The subsystems/sensors 14 include radar sensors 14a, acoustic sensors 14b, cameras/vision sensors 14c and LiDAR sensors 14d that receive data from a segment of road in the vicinity of the vehicle 10. In particular, the received data may include information related to features of the road segment, including road and/or lane boundaries, and stationary or moving obstacles. For example, stationary obstacles may be in the form of speed bumps or potholes, and moving obstacles may be in form of other vehicles or pedestrians.

The subsystems/sensors 14 also include a navigation subsystem 14e for providing information regarding the surrounding road segment. In addition, the subsystems/sensors 14 may include one or more transceivers for communicating with other objects or vehicles in the vicinity of the vehicle 10. In general, this may be referred to as a vehicle-to-everything (V2X) communication subsystem 14f, and may include one or more of vehicle-to-vehicle (V2V) communication, vehicle-to-pedestrian (V2P) communication, vehicle-to-device (V2D) communication and vehicle-to-grid (V2G) communication.

Figure 2 shows the system 12 in greater detail. In particular, the system 12 includes a trajectory planning component 16 and a trajectory tracking and vehicle actuation component 18. In turn, the trajectory planning component 16 includes a reachable state module 20, an allowable position module or perception module 22, a target state module 24 and a trajectory generation module 26, and the operation of each of these will be described in detail below. The system 12 has a receiver 28 configured to receive data from the subsystems and sensors 14 for use in determining a trajectory for the vehicle 10. The generated vehicle trajectory is output from the trajectory planning component 16 and may be used in controlling actuation of the vehicle 10 to track the planned trajectory. An output 29 from the trajectory tracking and vehicle actuation component 18 is indicative of current vehicle states after actuation based on sensor measurements and estimation of current vehicle states, and this output is looped back to the trajectory planning module 16 and to the trajectory tracking and vehicle actuation module 18 after actuation of the vehicle 10.

As mentioned above, in the present embodiment the system plans a trajectory for the vehicle 10, referred to as the subject vehicle (SV) 10, to perform an overtaking manoeuvre on a vehicle in front, which is referred to as the lead vehicle (LV). Figure 3 shows the sequence of sub-manoeuvres involved in the overtaking manoeuvre. In the described embodiment, the SV 10 and LV 30 are travelling along a two-lane highway or motorway 32. In particular, the motorway 32 includes a ‘slow lane’ 34 and a ‘fast lane’ 36 separated by dotted line markings 38 and having a boundary limit 40, 42 at each side of the motorway 32, and the SV 10 and LV 30 are travelling in the slow lane 34 with the SV 10 behind the LV 30. The illustrated overtaking manoeuvre includes three sequential sub-manoeuvres, namely: (i) lane changing 44; (ii) lane keeping 46; and, (iii) lane merging 48. Figure 3 also illustrates an appropriate minimum distance 50 that the SV 10 should stay behind the LV 30 before initiating an overtaking manoeuvre, and also an appropriate minimum distance 52 that the SV 10 should be in front of the LV 30 before performing a merge to complete an overtaking manoeuvre. In addition, Figure 3 shows an appropriate minimum lateral distance 54 that vehicles travelling beside one another (during a lane keeping manoeuvre, for example) in the fast and slow lanes 34, 36 should maintain. The system 12 plans a trajectory for the SV 10 to overtake the LV 30 while avoiding the LV 30, keeping within the road boundaries 40, 42, and also adhering to the appropriate minimum distances to maintain between vehicles.

Figure 4 shows the road geometry, in particular the coordinate frame and range, for the overtaking manoeuvre performed by the SV 10 in the presently-described embodiment. In addition to a coordinate inertial-frame (l-frame) 60, three further coordinate frames are exploited, namely, a vehicle-frame (V-Frame) 62, an obstacleframe (O-frame) 64, and a road-frame (R-frame) 66. The V-frame is located in the centre of gravity of the SV 10 and follows the Roll-Pitch-Yaw (RPY) convention. Similarly, the O-frame is located at the centre of gravity of the LV 30 and follows the RPY convention, while the R-frame is located at the projection of the origin of Vframe onto the innermost (rightmost) edge 42 of the road 32 with x-axis in the direction of the travel. A generic point on the road is denoted as w=(X, Y), w_r=(X_r,Y_r), Wy=(Xy,Yyj), or i/i/₀=(X₀, Y₀)when expressed in the I, R, V or O frame, respectively. 5_W is the width of the lane 34. The shaded area 68 denotes a rectangle moving along the R-frame 66 with vertices V=(Vi,V2>^v3>^v4)’ ^{ancl its use wil1} be described in detail below.

With reference to Figure 5, a method 70 undertaken by the system 12 to plan a trajectory for the SV 10 to perform an overtaking manoeuvre on the LV 30 is now described. In particular, the method uses a framework of potential field-like functions and Model Predictive Control (MPC) for performing an overtaking manoeuvre on a motorway. In summary, the framework includes an artificial potential field (APF) or allowable position module 22, a target generation block or target state module 24, and the trajectory generation block or module 26. The APF is used to map the surrounding area of the SV 10. Contrary to typical potential field approaches where an obstacle’s position is used to identify zones that should not be entered, the described method combines an obstacle’s position, orientation and relative velocity to create a map of allowable zones surrounding the SV 10. At every sampling instant, the target state module 24 identifies the most desirable point of the road which is compatible with the dynamics of the SV 10 and computes the reference state set point (e.g. velocity, lateral position and heading angle) to be tracked. To achieve this aim, the target state module 24 combines the allowable zones or points in the potential field with the vehicle dynamics capability of the SV 10 which is captured through the reachable set of the SV 10 from its current state. The trajectory generation module 26 uses an MPC strategy to generate feasible trajectories and steer the SV 10 to the required reference (target) states. The MPC approach provides closed-loop stability while ensuring the persistent feasibility of the optimisation problem needed by any model predictive control formulation. Therefore, in the described system 12 the onus of obstacle avoidance is lies with the perception or allowable position module 22, and the onus of feasible trajectory lies with the MPC or trajectory generation module 26.

At step 72, the reachable state module 20 determines a set of reachable vehicle states within a prescribed period of time. That is, the reachable state module 20 determines a 13 set of all the possible vehicle states that the SV 10 may assume by the prescribed period. Each reachable vehicle state in the set includes a reachable vehicle velocity, a reachable vehicle heading angle, a reachable vehicle longitudinal displacement, and a reachable vehicle lateral displacement.

The reachable set is determined in dependence on at least one vehicle constraint. In particular, the vehicle constraints include one or more current vehicle state variables such as a current vehicle position, a current vehicle heading angle, and a current vehicle velocity. Also, the vehicle constraints include admissible state variables such as an admissible vehicle heading angle, an admissible vehicle velocity, and an admissible vehicle lateral position. In addition, the vehicle constraints include vehicle input constraints such as an admissible vehicle steering angle, and an admissible vehicle acceleration. Furthermore, the vehicle constraints include vehicle dynamics constraints such as one or more dimensions of the vehicle, a vehicle mass, a vehicle mass distribution, a type of terrain over which the vehicle is travelling, a vehicle tyre type, and a vehicle tyre pressure. Figure 6 illustrates the V-frame region 74 of positions that the SV 10 may reach in the case where the SV 10 is travelling at a constant velocity of 33.33m/s for a prescribed time of 1.6s with admissible steering inputs. In particular, Figure 6 shows a convex approximation of the reachable set. Note that the exact reachable set is a non-convex set that is difficult to compute and plot, and so a convex approximation is used here instead.

The set of reachable vehicle states may be selected from a predetermined space of potential sets of reachable vehicle states, wherein each potential set of reachable vehicle states has a different combination of values of one or more of the vehicle constraints and/or the prescribed period of time. In such a case, the set of reachable states is calculated offline and stored within an on-board memory of the vehicle 10. Alternatively, the set of reachable vehicle states may be determined at predefined intervals based on values of the vehicle constraints and updated at frequent intervals via vehicle-to-everything communication.

Mathematically, for a system with states x, inputs u, and outputs y subject to system dynamics described by

X = f(x,u) y = h(x, u) where f and h are the state and output functions (linear or nonlinear), respectively, then the reachable set denoted by 5?(L; x₀) for a system described by the state and output equations above is defined as the collection of all states that may be reached with a prescribed time t* when the initial state is x(0) = x₀ by applying admissible inputs, i.e.

52(L;x₀)= x(t*;x₀,u(·)) where u(·) e 'll represent the input to the above system in the time range t = [0, t*j.

At step 76 the system 12, and in particular the trajectory planning component 16 receives road segment data from the sensors and subsystems 14. The received road segment data is indicative of at least one feature of a road segment in the vicinity of the SV 10. The road segment data includes data relating to obstacles in the road segment. In this case, one of the obstacles is another vehicle, namely the LV 30. The road segment data includes the current velocity, relative position and orientation of the LV 30. At motorway cruising speed, an overtaking manoeuvre may be initiated approximately 50m behind the LV 30 and end approximately 50m in front of the LV 30. Hence, the SV 10 needs to have accurate situational awareness (via the received road segment data) of the surrounding obstacles in the road segment to plan trajectories that avoid the obstacles.

The road segment data may also include data indicative of non-moving objects such as potholes, speed bumps, and blocked lanes. As mentioned above, the road segment data also includes data indicative of the road boundaries 40, 42 and the lane boundary 38. Furthermore, the road segment data includes data indicative of driving rules such as a speed limit of the road 32 and which lanes may be used to perform an overtaking manoeuvre. In addition, the road segment data includes a current vehicle state or current vehicle trajectory.

At step 78, the allowable position module 24 determines a plurality of allowable vehicle positions in the road segment 68. This determination is made in dependence on the received road segment data. The plurality of allowable vehicle positions is within the determined set of reachable vehicle positions. In the described embodiment, an APF is used to determine the allowable set. In particular, the APF is used to map the surrounding region, specifically the region 68, of the SV 10. The APF is used to create a map of allowable zones surrounding the SV 10. The APF is larger than the set of reachable vehicle positions 74.

Potential field like perception may be shaped in such a way that it guides towards desired driver behaviour. The surrounding environment (road segment) is described through the use of a potential field where several road elements (e.g. road limits, road markers, other road users) are considered for shaping a potential function so as to include driving rules and guide the SV 10 through the allowable road regions.

A net potential function U_r is generated by combining several potential functions where the design of each potential function is intended to incorporate one or more driving rules. In particular, a road potential function U_rOad is designed to keep the SV 10 away from the road limits/boundaries 40, 42, a lane potential function Ui_ane is designed for lane-keeping, and a lane velocity potential function U_vei is designed such that the SV 10 occupies the innermost (slowest) lane 34 when more than one lane is available. In addition, a car potential function U_car is designed such that the SV 10 either maintains an appropriate distance to the LV 30 or, if the other lane 36 is available, moves to a faster lane. The net potential function U_r is generated by superimposing these individual potential functions to create a perception map that may be used for overtaking in a human-like manner. The construction of the individual potential functions is discussed below.

Lane Potential Function

Different lanes on a road have an implicit velocity associated with them, i.e. the velocity progressively increases from the innermost lane 34 to the outermost lane 36. A higher speed may be indicative of a higher risk, and so each lane 34, 36 of the road 32 is apportioned a certain potential to describe its associated level of risk. In the present embodiment, this is described by the function

Uvei,l(.^wr) Y (yiane.iOw) ^— Uane.l Ow)) where γ is a gain factor, v_{lane i} is the nominal velocity of the A lane, and U_{vei i} is the potential caused by the lane velocity of the i^th lane.

Road Potential Function

The road potential is designed such that the boundaries 40, 42 have the highest (infinite) potential and the centre of the road has the lowest potential. In the present embodiment, this is described by the function η / 1 \² t/road,i(w_r)=-^^T^J where η is a scaling factor, r_b is the y-coordinate of one of the road edges 40, 42.

Lane Potential Function

A lane potential function creates a virtual barrier between the lanes 34, 36 to direct the SV 10 towards the lane centre. In the present embodiment, this is described by the Gaussian function below to achieve the desired behaviour:

E|anc,i(^r) ^— ·4[_3ηθθΧρ where ri_lane provides the amplitude of the lane-divider potential, T_1;i is the y-coordinate of the A lane division, and σ is a scaling factor.

Car Potential Function

The LV 30 is modelled as a rectangular area with virtual triangular wedges, also referred to as buffer zones, appended to the front and rear of the LV 30. The location, i.e. x and y coordinate, of the vertex of the triangle behind the LV 30 is calculated 5 based on the velocity of the LV 30 and the headway time fit. By denoting B\_v as the set of coordinates containing the LV 30 and the two triangular wedges, Yukawa function is used to describe the potential because of the LV 30 and the triangular wedges as below:

/_e~^aKd\ ^car(Wr) ^— -^car ( ) where a is a Yukawa scaling factor, ri_car is the Yukawa amplitude, and K_d is the Euclidean distance to the nearest coordinate of the obstacle denoted by Si_v, given as k_d = min ||fi₀ - w_r||.

These individual potentials are superimposed to obtain an overall perception map or net potential function of the area surrounding the SV 10 and given by t/_r(w_r) + t/_roa(j + t/i_ane + U_car where ^lane

Manes

-Σ i=i ^lane,t and

Manes i/vel= i=i

L/vel,i

In practice, the net potential field is studied in the V-frame so as to facilitate trajectory planning, and so the function i/_r(w_r) given above is (linearly) transformed to the Vframe to give a net potential function i/_v(w_v) A i/_r(T_r ^v(w_v)).

By assigning a threshold limit t/_all, the allowable regions or positions of the road surrounding the SV 10 are expressed as

Gv = {w_v £ Tj(Biv): E_v(w_v) < L/_aii}·

This provides a set of allowable regions and the SV 10 needs to plan trajectories that keep it within this set so as to reduce risk.

The set §_v does not consider vehicle dynamics of the SV 10, and so some regions of the road with an allowable potential may not be reachable in practice. The method for selecting reference points in the set of allowable regions which are compatible with the dynamics of the SV 10 is described below.

Figure 7 shows an exemplary simulation carried out by the allowable position module 22. In particular, Figure 7(a) provides a three-dimensional view in the R-frame of the entire potential function and the local minima at the centre of each lane for guiding the SV 10 may be seen together with a trapezoidal field created by the LV 30. Figure 7(b) depicts the level curves for the same time instant in the R-frame. The LV 30 is shown with buffer zones 31, i.e. triangular appendages, where the potential field rapidly increases to prevent the SV 10 from getting too close to the LV 30 during different phases of an overtaking manoeuvre.

Returning to Figure 5, at step 80 the target state module 24 determines a target vehicle state having a target vehicle position in the road segment 68. The target vehicle position corresponds to one of the plurality of determined allowable vehicle positions calculated at step 78. For example, in the described embodiment the target vehicle position is the allowable vehicle position that is furthest from the vehicle 10. The target vehicle state also includes a target vehicle velocity and a target vehicle heading angle.

The dynamics of the SV 10 in the l-frame while driving on the road 32 at a desired speed v_des are represented with a linear kinematic bicycle model. By denoting the vehicle states of interest as x = [X, Y, ψ,ν_χ]^τ, where ip is the heading angle, v_x is the longitudinal velocity, X,Y are the longitudinal and lateral displacement of the SV 10, the dynamics of the SV 10 are described by x = A_cx + B_cu where u = (a_x,5_f) e 'll is the control action with a_x being longitudinal acceleration and S_f being front steering angle. The system matrices A_c and B_c are given by

0 01’ . _ 0 0 r^dcs0 ^c 0 0 00’ .0 0 00 .

-o0

R = ⁰⁰ ^c 0 V_des/L_wb ’

Lio where L_wb is the wheelbase of the SV 10.

In ideal motorway cruising situations, the dynamics of the system may be described by x = [v_x, 0,0,0]⁷ and manoeuvres (e.g. lane change, merge, etc.) may be regarded as transitions from one set of states to another set of states. In such ideal scenarios the object of the SV 10 is to adjust its trajectory to avoid obstacles while ensuring that the vehicle’s speed does not exceed the desired longitudinal velocity v_des. Starting from an initial position w₀ = (Χθ,/θ) and travelling at v_des, using admissible control actions from the set {(a_x,5_f): a_x < 0, (a_x,8_f) e U_v}, the set 5? of states reachable without exceeding the desired velocity v_des in the time interval t* of the system may be determined using the equation for 52(l.;x₀) given above. Consequently, the set of reachable lateral and longitudinal coordinates for the SV 10 in the V-frame is

7?v = Tv(Proj_w(5?)), where, in general, for a set Γ c ]R^na+nb the projection operation is defined as Proj_a(f) = (a e ]R.^na:3b e fR^nb, (a, b) e Γ}.

Then using the equations given above for g_v and 5?_v the allowable zones or regions surrounding the SV 10 which are reachable with respect to the vehicle state and vehicle dynamics is ^V,all - Sv ⁿ

The reference target coordinates w_v = (X_V,Y_V~) are chosen from 52_Vall with the aim to maximise the distance travelled by the SV 10 in the time interval t*, i.e.

w_v = argmax ||w_v — w_v0||.

^wv^eSV,all

Also, it may be beneficial for the SV 10 to traverse the longitudinal distance from X_v0 to X_v at uniform longitudinal velocity, and so the target velocity is selected as v — © t*

Furthermore, as the SV 10 is assumed to be travelling on a straight road in the described embodiment, the target heading angle of the SV 10 remains ψ = 0. Thus, stacking the reference targets for each state the target state vector for the system x = [Χ_ν,Ϋ_ν,ψ,ν_χ] is obtained.

As the region B\_v (the set of coordinates containing the LV 30 and the two triangular wedges that the SV 10 should not enter), i.e. the unallowable region, surrounding the LV 30 moves in the R-frame 66 with speed v_lv - v_x, at each step the allowable, reachable region and reference targets change accordingly. An exemplary simulation of the reference targets carried out by the target state module 24 for some time steps are shown in Figure 8 in the l-frame 60, the reference targets being shown as crosses, together with the position of the SV 10 (indicated by a square) and the LV 30 (indicated by a diamond). In particular, Figure 8 shows that the target references selected by the SV 10 for overtaking the LV 30. Specifically, the SV 10 moves behind the LV 30 and then the centre of the fast lane becomes the region of lowest potential and is compatible with the subject vehicle dynamics, thus allowing the SV 10 to move to this lane to overtake the LV 30. After the SV 10 moves sufficiently ahead of the LV

30, the centre of the slow lane once again becomes the region of lowest potential and is also reachable by the SV 10. Therefore, the SV 10 may merge back into the slow lane at a distance sufficiently ahead of the LV 30 so as to complete the overtaking manoeuvre.

Returning again to Figure 5, at step 82 the trajectory generation module 26 configured to determine a feasible trajectory for the SV 10 to travel to the target vehicle position in dependence on one or more of the vehicle constraints outlined above. In particular, the target states x which are generated using the above approach at each time step result in piecewise references. For example, if it is determined that a lane change is needed, Y_v will change from the centre of one lane to another. In the described embodiment, an MPC method is used to plan the trajectory for directing the SV 10 from its current state to an allowable target state in an admissible manner, i.e. by considering vehicle dynamics, state constraints and input constraints. The advantage of the described MPC strategy is its ability to steer the state of a constrained system toward any setpoint (i.e. the desired target steady state) whether or not it belongs to the terminal set. The method guarantees the asymptotic convergence of the system state to any admissible target steady state. Furthermore, if the steady state is not admissible, then the control strategy steers the system to the closest admissible steady state.

In the present embodiment, the dynamics of the state (X) depends only on v_x, and so it is possible to further simplify the system for trajectory generation to reduce computational time. In particular, the reduced order system is

where x_r = [Υ,ψ,ν_χ]^τ is the system state, u = [αχ,^]⁷ is the input, and the system matrices are and

	Ό	^dcs	0’
•^rc	0	0	0
	.0	0	0.

^des/I'wb

This reduced order system captures the relevant dynamics of the system and is suitable for planning allowable and feasible trajectories. Moreover, the reduced system is in a steady state while the SV 10 is driving unobstructed on straight motorways as x_r(t) = [0,0,0]⁷. Furthermore, the reference vector(s) calculated above corresponds to a steady state condition for the reduced order system expressed as x_rss = [Ϋ,ψ,υ_χ]^τ.

By discretising the reduced order system with a sampling time T_s, the following discrete time system is obtained:

x_r(/c + 1) = i4x_r(/c) + Bu(/c) y_r(/c) = Cx_r(/c) + Du(k) where the matrices A,B,C,D are constant, the pair (A,B) is stabilisable and the system is subject to hard constraints expressed as z = (x_r,u_r) e Z = (z e ]R^nx+nu: A_zz < b_z} where A_z e R^^xrt+n₁J _anc| ]j_{z e} j^n_{Zj w}j_th p_ei_ng the number of constraints, so that Z is a non-empty compact convex polyhedron containing the origin of its interior. In the described embodiment, the set hard constraints above are obtained from the set of states and inputs satisfying the following inequalities:

'A,min — ^xr — -A,max ^umin — ^u — ^umax

At each discrete time instant k the system is solved by setting the target state as x_s = x_rss and the initial state as x_s(0) = x_r(/c · T_s). Then the reference trajectory [y*,fy,v/]^Tfor t e [k · T_s, (k + N) T_s], with N being the prediction horizon, is computed by applying the optimal solution u' to the dynamic system given above.

The reference points dynamically generated in Figure 8 are used directly for generating trajectories. Figure 9 illustrates planned trajectories and the actual path of the SV 10 shown in the reference frame of the LV 30 in an exemplary simulation undertaken by the trajectory generation module 26. In particular, Figure 9 shows that the overtaking manoeuvre is initiated about 50m behind the LV 30 thus maintaining the 23 margins expected from a typical motorway manoeuvre for given speeds of the vehicles and a given headway time. Furthermore, the planned trajectories from the trajectory generation module 26 or MPC module are stable and do not demonstrate diverging behaviour. By assuming perfect trajectory tracking, the actual trajectory followed by the SV 10 is shown to follow a smooth path from the centre of the slow lane to the centre of the fast lane. The trajectories planned by the novel MPC are persistently feasible which provides an additional safety net to the autonomous driving functionality.

Many modifications may be made to the above examples without departing from the scope of the present invention as defined in the accompanying claims.

Although a continuous time, nonlinear kinematic bicycle model is used in the abovedescribed embodiment, higher-order vehicle models may also be used to calculate the set of reachable states.

Although the system 12 and method 70 are described above with reference to a motorway overtaking manoeuvre, they are equally applicable to other types of relatively complex manoeuvres that a vehicle may undertake.

Although the system 12 is described above as generating trajectories while travelling at close to constant speed such that the vehicle 10 does not accelerate or decelerate during the lane change sub-manoeuvre, in different embodiments feasible trajectories are generated for a varying longitudinal velocity.

Claims

1. A system for a vehicle operable in an autonomous mode, the system for planning a trajectory of the vehicle, the system comprising:

a receiver configured to receive road segment data from at least one subsystem of the vehicle, the road segment data being indicative of at least one feature of a road segment in the vicinity of the vehicle;

a reachable state module configured to determine a set of reachable vehicle states within a prescribed period of time in dependence on at least one vehicle constraint, each of the reachable vehicle states including a reachable vehicle position;

an allowable position module configured to determine a plurality of allowable vehicle positions in the road segment in dependence on the received road segment data, the plurality of allowable vehicle positions being within the determined set of reachable vehicle positions;

a target state module configured to determine a target vehicle state having a target vehicle position in the road segment, the target vehicle position corresponding to one of the plurality of determined allowable vehicle positions; and, a trajectory generation module configured to determine a feasible trajectory for the vehicle to travel to the target vehicle position in dependence on the at least one vehicle constraint so as to plan the vehicle trajectory.

2. A system according to Claim 1, the system comprising a vehicle actuation module configured to send a control signal to control the vehicle to follow the feasible trajectory.

3. A system according to Claim 1 or Claim 2, wherein the at least one road segment feature comprises at least one of:

a current trajectory of the vehicle;

road and/or lane boundaries; and, driving rules.

4. A system according to any previous claim, wherein the at least one road segment feature comprises at least one obstacle.

5. A system according to Claim 4, wherein the at least one obstacle includes a further vehicle, and the road segment data includes at least one of a velocity, a position, and an orientation of the further vehicle.

6. A system according to Claim 5, wherein the further vehicle is ahead of, and in a same lane as, the vehicle.

7. A system according to any of Claims 4 to 6, wherein the at least one obstacle includes at least one of a pothole, a speed bump, and a blocked lane.

8. A system according to any previous claim, wherein the allowable position module is configured to:

define an artificial potential field larger than the set of reachable vehicle positions; and, generate a net potential function describing the artificial potential field in dependence on the received road segment data, wherein a potential value of the net potential function varies across the artificial potential field.

9. A system according to Claim 8, wherein the allowable vehicle positions are determined to be corresponding positions in the potential field in which the net potential function value lies in a prescribed interval of potential values.

10. A system according to Claim 9, wherein the net potential function includes a contribution from one or more of:

a lane potential function including information relating to boundaries and/or a centre of each lane of the road; and, a further vehicle potential function including information relating to vehicle dynamics of a further vehicle in the road.

11. A system according to any previous claim, wherein the at least one vehicle constraint includes one or more current vehicle state variables.

12. A system according to Claim 11, wherein the current vehicle state variables include a current vehicle position, a current vehicle heading angle, and a current vehicle velocity.

13. A system according to any previous claim, wherein the at least one vehicle constraint includes one or more admissible vehicle state variables.

14. A system according to Claim 13, wherein the admissible vehicle state variables include an admissible vehicle heading angle, an admissible vehicle velocity, and an admissible vehicle lateral position.

15. A system according to any previous claim, wherein the at least one vehicle constraint includes one or more vehicle input constraints.

16. A system according to Claim 15, wherein the vehicle input constraints include an admissible vehicle steering angle, and an admissible vehicle acceleration.

17. A system according to any previous claim, wherein the at least one vehicle constraint includes one or more vehicle dynamics constraints.

18. A system according to Claim 17, wherein the vehicle dynamics constraints include one or more dimensions of the vehicle, a vehicle mass, a vehicle mass distribution, a type of terrain over which the vehicle is travelling, a vehicle tyre type, and a vehicle tyre pressure.

19. A system according to any previous claim, wherein the reachable vehicle states include a reachable vehicle velocity, a reachable vehicle heading angle, a reachable vehicle longitudinal displacement, and a reachable vehicle lateral displacement.

20. A system according to any previous claim, wherein the set of reachable vehicle states is selected from a predetermined space of potential sets of reachable vehicle states, wherein each potential set of reachable vehicle states has a different combination of values of one or more of the vehicle constraints and/or the prescribed period of time.

21. A system according to any previous claim, wherein the set of reachable vehicle states is determined at predefined intervals based on values of the at least one vehicle constraint received via vehicle-to-everything communication.

22. A system according to any previous claim, wherein the set of reachable vehicle states is determined using a continuous time nonlinear kinematic bicycle model.

23. A system according to any previous claim, wherein the target vehicle position corresponds to the allowable vehicle position farthest from a current vehicle position, the allowable vehicle position corresponding to one of the reachable vehicle positions in the set of reachable vehicle states.

24. A system according to any previous claim, wherein the target state module determines a target vehicle velocity along the feasible trajectory.

25. A system according to Claim 24, wherein the target vehicle velocity comprises a substantially uniform longitudinal velocity.

26. A system according to any previous claim, wherein the target vehicle states include a target vehicle velocity and/or a target vehicle heading angle.

27. A system according to any previous claim, wherein the at least one subsystem of the vehicle includes at least one of:

a vehicle navigation subsystem;

an ultrasonic sensor;

a radar sensor;

a LiDAR sensor;

a vision sensor; and, a vehicle-to-everything transceiver.

28. A system according to any previous claim, wherein the trajectory generation module is configured to apply a Model Predictive Control strategy to generate the feasible trajectory.

29. A method for a vehicle operable in an autonomous mode, the method being for planning a trajectory of the vehicle, the method comprising:

receiving road segment data from at least one subsystem of the vehicle, the road segment data being indicative of at least one feature of a road segment in the vicinity of the vehicle;

determining a set of reachable vehicle states within a prescribed period of time in dependence on at least one vehicle constraint, each of the reachable vehicle states including a reachable vehicle position;

determining a plurality of allowable vehicle positions in the road segment in dependence on the received road segment data;

5 determining a target vehicle state having a target vehicle position in the road segment, the target vehicle position corresponding to one of the determined allowable vehicle positions that is within the determined set of reachable vehicle positions; and,

10 determining a feasible trajectory for the vehicle to travel to the target vehicle position in dependence on the at least one vehicle constraint so as to plan the vehicle trajectory.

30. A vehicle comprising a system according to any of Claims 1 to 28.

31. A non-transitory, computer-readable storage medium storing instructions thereon that when executed by one or more processors causes the one or more processors to carry out the method of Claim 29.