Trajectory evaluation for mobile robots

Abstract

A computer-implemented method of evaluating a candidate trajectory for an ego agent in the presence of at least one external agent, comprising: determining a predicted state of the at least one external agent at a future timestep and the corresponding state of the ego vehicle along the candidate trajectory at the future timestep; determining a headway region for each of the ego agent and the at least one external agent, the headway region for each agent defining the region occupied by that agent within a predefined time duration from the predicted state, based on the predicted state of the agent; performing a first intersection check to determine if the predicted state of the at least one external agent intersects with the headway region of the ego agent; and performing a second intersection check to determine if the predicted state of the ego agent intersects with the headway region of the at least one external agent; wherein, if either of the first or second intersection checks determine that an intersection exists, the candidate trajectory is evaluated as invalid.

Description

[0001] Trajectory Evaluation for Mobile Robots

[0002] Technical Field

[0003] The present invention relates to the planning and evaluation of trajectories for mobile robots. Particularly, but not exclusively, the present invention relates to the evaluation of trajectories for autonomous vehicles.

[0004] Background

[0005] A rapidly emerging technology is autonomous vehicles (AVs) that can navigate by themselves on urban roads. Such vehicles must not only perform complex manoeuvres among people and other vehicles, but they must often do so while guaranteeing stringent constraints on the probability of adverse events occurring, such as collision with these other agents in the environments. An autonomous vehicle, also known as a self-driving vehicle, refers to a vehicle which has a sensor system for monitoring its external environment and a control system that is capable of making and implementing driving decisions automatically using those sensors. This includes in particular the ability to automatically adapt the vehicle's speed and direction of travel based on perception inputs from the sensor system. A fully- autonomous or "driverless" vehicle has sufficient decision-making capability to operate without any input from a human driver. However, the term autonomous vehicle as used herein also applies to semi-autonomous vehicles, which have more limited autonomous decision-making capability and therefore still require a degree of oversight from a human driver. Other mobile robots are being developed, for example for carrying freight supplies in internal and external industrial zones. Such mobile robots would have no people on board and belong to a class of mobile robot termed UAV (unmanned autonomous vehicle). Autonomous air mobile robots (drones) are also being developed.

[0006] Driving decisions in AVs and other mobile robots depend on a number of factors, including progress, safety and comfort. Some existing planners use constrained optimisation techniques to optimise a cost function so as to satisfy constraints on safety, progress, comfort, as well as physical feasibility. One example of a constrained optimisation-based planner is disclosed in earlier International Patent Publication No. W02021 / 152050, which is incorporated herein by reference in its entirety. This planner implements constrained optimisation in a two-stage process, where the first stage solves an optimisation problem that is similar to, but simpler than the planning problem, with the output of the first stage being used to initialise a second stage which is used to solve the full optimisation problem. This implementation of constrained optimisation includes two different types of constraints: hard constraints which relate to strict requirements such as a kinematic model and collision avoidance, and soft constraints, which relate to objectives such as progress towards a goal or comfort based on acceleration or steering.

[0007] Safety and comfort of trajectories are also important when evaluating ego trajectories in testing. A large number of real-world or simulated driving runs need to be evaluated to ensure that potentially dangerous scenarios are encountered in testing. Thus, large amounts of real or simulated driving data need to be processed in testing. It is important that compliance with requirements of safe and comfortable behaviour are tested to ensure that autonomous vehicles are meeting the requirements of safety, as well as ensuring the comfort of the user of the autonomous vehicle and other human road users with which the autonomous vehicle interacts.

[0008] Summary

[0009] The present invention relates to a method of evaluating trajectories for an ego vehicle, where a trajectory defines the motion of the ego vehicle along a driveable path.

[0010] One key factor in determining a suitable ego trajectory, in planning or in testing, is safety, where an aim of the trajectory is to avoid collision with other agents and / or objects present in the environment, and to follow road rules. However, a secondary factor which is nevertheless important for determining suitable trajectories for autonomous vehicles is comfort, i.e. comfort of a human user of an autonomous vehicle. Not all trajectories that are safe, i.e. by avoiding collision and satisfying other pre-defined safety constraints, are necessarily comfortable.

[0011] The present invention provides a way to assess trajectories to determine if they maintain appropriate distance between agents at all times so as to ensure safety as well as comfort objectives are met. This may be implemented in a planning context, by evaluating candidate trajectories generated by a planner and excluding any trajectories that fail to meet the requirements on headway. This method may also be implemented in the context of testing an autonomous vehicle stack, by evaluating the ego trajectories generated by the autonomous vehicle in real-world or simulated scenarios for compliance with the defined headway between agents. In a planning context, this evaluation may be implemented in order to select trajectory between some default behaviour which is known to be safe, such as a slowdown maneuver, and a trajectory generated by a planner to make progress towards a certain planning goal. If the planned trajectory does not satisfy the check on the required headway between agents, the ego vehicle selects the default, safe behaviour to implement, and the process is repeated until a planned trajectory that progresses towards the planning goal satisfies the headway requirement, and is therefore suitable to be taken by the ego vehicle.

[0012] A first aspect herein provides a computer-implemented method of evaluating a candidate trajectory for an ego agent in the presence of at least one external agent, comprising: determining a predicted state of the at least one external agent at a future timestep and the corresponding state of the ego vehicle along the candidate trajectory at the future timestep; determining a headway region for each of the ego agent and the at least one external agent, the headway region for each agent defining the region occupied by that agent within a predefined time duration from the predicted state, based on the predicted state of the agent; performing a first intersection check to determine if the predicted state of the at least one external agent intersects with the headway region of the ego agent; and performing a second intersection check to determine if the predicted state of the ego agent intersects with the headway region of the at least one external agent; wherein, if either of the first or second intersection checks determine that an intersection exists, the candidate trajectory is evaluated as invalid.

[0013] The method may comprise outputting an indicator that the candidate trajectory is evaluated as invalid.

[0014] The at least one external agent may comprise a plurality of external agents, the method comprising, for each external agent of the plurality of external agents: determining a predicted state of the external agent at the future timestep; determining a headway region for the external agent, defining the region occupied by that agent within the predefined time duration from the future timestep, based on the predicted state of the agent; and performing the first and second intersection checks based on the predicted state and headway region for that agent; wherein, if either of the first or second intersection checks determine, for any external agent, that an intersection exists, the candidate trajectory is evaluated as invalid.

[0015] The method may comprise: determining a plurality of predicted states for the or each external agent at the future timestep; determining a plurality of headway regions for the or each external agent, each headway region defining the region occupied by the external agent within the predefined time duration from a respective predicted states of the plurality of predicted state, based on that predicted state; and performing the first and second intersection checks based on each of the plurality of predicted states and corresponding headway regions determined for the or each external agent; wherein, if either of the first or second intersection checks determine, for any of the predicted states or corresponding headway regions of the or each external agent, that an intersection exists, the candidate trajectory is evaluated as invalid.

[0016] The method may comprise determining the respective state and corresponding headway for each of the ego agent and the at least one external agent for each of a plurality of timesteps within a predetermined time horizon, wherein the first and second intersection checks are performed for each timestep based on the respective states and headways at that timestep; wherein if either of the first or second intersection checks determine, for any timestep of the plurality of timesteps, that an intersection exists, the candidate trajectory is evaluated as invalid.

[0017] The method may comprise determining a control signal and providing the control signal to an AV controller based on the determined validity of the candidate trajectory.

[0018] Each of the predicted states of the external agent and the corresponding state of the ego agent may comprise at least a pose and a speed of the respective agent at the future timestep.

[0019] The headway region for the external agent may define the region occupied by that agent within predefined time duration, based on the external agent continuing at the speed of the predicted state.

[0020] The headway region for the external agent may define the region occupied by that agent within the predefined time duration, based on the external agent following a predicted motion profile for that agent.

[0021] The at least one external agent may be selected based on meeting an inclusion criterion of a predefined exclusion strategy.

[0022] The inclusion criterion may be based on one or more of a position of the external agent; and a classification of the external agent.

[0023] The candidate trajectory may be generated for the ego agent based on an input driveable path and one or more environmental variables. The environmental variables may be determined from annotated map data and / or generated by a perception system based on sensor data of the ego agent.

[0024] The input driveable path may be generated by an optimisation-based planner that generates driveable trajectories subject to one or more planning constraints.

[0025] The plurality of timesteps may comprise the timesteps until the ego trajectory reaches a predefined clearance line.

[0026] A set of predicted states and a corresponding set of confidence scores associated with each predicted state may be generated for the or each external agent; wherein the plurality of predicted states for the or each external agent are determined by selecting those predicted states of the set of predicted states for which the corresponding confidence score meets a predefined confidence threshold.

[0027] The method may be implemented during testing of an ego agent, wherein the candidate trajectory is a real or simulated trajectory taken by an ego agent in a real or simulated driving scenario, and wherein an invalid evaluation corresponds to a failure of the ego agent.

[0028] The method may be implemented by a planning component of the ego agent, wherein, if none of the intersection checks determine that an intersection exists, the candidate trajectory is evaluated as valid, and output to a controller component of the ego agent to control the vehicle so as to follow the candidate trajectory.

[0029] Further aspects herein provide computer system comprising memory and one or more processors configured to implement any method herein; and a computer program comprising executable instructions configured, when executed on one or more hardware processors to implement any method herein.

[0030] Brief Description of Figures

[0031] To assist in understanding the invention, and to show how embodiments of the invention may be carried into effect, reference is made by way of example only to the following figures, in which:

[0032] Figure l is a schematic block diagram showing an example autonomous driving stack;

[0033] Figure 2 shows four steps of a time-headway intersection check algorithm; Figure 3 is a flow diagram showing the steps of the time-headway intersection check algorithm;

[0034] Figure 4 is a schematic block diagram showing an example planning architecture.

[0035] Detailed Description

[0036] Described herein is a method of determining a suitable motion plan for an ego vehicle in various driving scenarios, based on the predicted behaviours of other agents present. The described method aims to generate a trajectory for an ego trajectory that satisfies a constraint in the form of a time-headway gap between the ego vehicle and other agents of the scene, where the time headway is specified as a time duration for each agent based on the speed of that agent along its predicted path, and results in a specific time-headway region in space to be maintained between each agent and any other agent for the required constraint to be met.

[0037] Described below is a planning system for autonomous vehicles and other forms of mobile robots, to plan trajectories in a range of scenarios. An example autonomous vehicle stack and its components will first be described for context. However, the planning methods and systems described below can equally be applied to other forms of mobile robot.

[0038] The term ‘trajectory’ as used herein refers to a combined path and motion profile for a given agent over a predefined time period. A trajectory comprises a sequence of states for the agent over time, with the state at each time comprising a pose and motion variables for the agent. The pose comprises the position and orientation of the agent, while the motion variables include at least a subset of speed, acceleration and jerk (rate of change of acceleration).

[0039] Figure 1 shows a highly schematic block diagram of a runtime stack 100 for an autonomous vehicle (AV). The run time stack 100 is shown to comprise a perception stack 102, a prediction stack 104, a planner 106 and a controller 108.

[0040] The perception stack 102 receives sensor outputs from an on-board sensor system 110 of the AV.

[0041] The on-board sensor system 110 can take different forms but generally comprises a variety of sensors such as image capture devices (cameras / optical sensors), LiDAR and / or RADAR unit(s), satellite-positioning sensor(s) (GPS etc.), motion sensor(s) (accelerometers, gyroscopes etc.) etc., which collectively provide rich sensor data from which it is possible to extract detailed information about the surrounding environment and the state of the AV and any external actors (vehicles, pedestrians, cyclists etc.) within that environment.

[0042] Hence, the sensor outputs typically comprise sensor data of multiple sensor modalities such as stereo images from one or more stereo optical sensors, LiDAR, RADAR etc.

[0043] Stereo imaging may be used to collect dense depth data, with LiDAR / RADAR etc. providing potentially more accurate but less dense depth data. More generally, depth data collection from multiple sensor modalities may be combined in a way that respects their respective levels (e.g. using Bayesian or non-Bayesian processing or some other statistical process etc.). Multiple stereo pairs of optical sensors may be located around the vehicle e.g. to provide full 360° depth perception. This provides a much richer source of information than is used in conventional cruise control systems.

[0044] The perception stack 102 comprises multiple perception components which co-operate to interpret the sensor outputs and thereby provide perception outputs to the prediction stack 104.

[0045] The perception outputs from the perception stack 102 are used by the prediction stack 104 to predict future behaviour of the external actors.

[0046] Predictions computed by the prediction stack 104 are provided to the planner 106, which uses the predictions to make autonomous driving decisions to be executed by the AV in a way that takes into account the predicted behaviour of the external actors.

[0047] The planner 106 implements the techniques described below to plan trajectories for the AV and determine control actions for realizing such trajectories. In particular, a core function of the planner 106 is to determine a series of control actions for controlling the AV to implement a desired goal in a given scenario. In a real-time planning context, a scenario is determined using the perception stack 102 but can also incorporate predictions about other actors generated by the prediction stack 104. A scenario is represented as a set of scenario description parameters used by the planner 106. A typical scenario would define a drivable area and would also capture predicted movements of any obstacles within the drivable area (such as other vehicles) along with a goal. A goal would be defined within the scenario, and a trajectory would then need to be planned for that goal within that scenario. In the following, obstacles are represented probabilistically in a way that reflects the level of uncertainty in their perception within the perception stack 102.

[0048] For example, the goal could be to enter a roundabout and leave it at a desired exit; to overtake a vehicle in front; or to stay in a current lane at a target speed (lane following). The goal may, for example, be determined by an autonomous route planner (not shown). A default goal may be to follow a drivable path of the AV as determined by a route planner. Progress towards the goal would in this case be measured as progress towards some future point along the drivable path.

[0049] The controller 108 executes the decisions taken by the planner 106 by providing suitable control signals to on-board actuators 112 such as motors of the AV. In particular, the controller 108 controls the actuators in order to control the autonomous vehicle to follow a trajectory computed by the planner 106.

[0050] One component of the planner 106 of the ego vehicle stack is a motion planner 408, described in more detail below, which is configured to receive a given input driveable path, which may be determined based on the road layout or by a further planning component which generates suitable paths based on planning constraints, and output a trajectory comprising a suitable motion profile, i.e. defining motion variables such as speed or acceleration of the ego vehicle along the provided driveable path. The methods described below can be implemented by the motion planner 408 to assess the trajectories generated by the motion planner 408, and determine whether a generated trajectory should be passed to the controller 108 to be implemented by the motors of the ego vehicle. However, it should be noted that the checks described below are not limited to a planning context, and may also be implemented to assess trajectories during testing of an AV stack, for example to determine whether trajectories actually taken by the ego vehicle in a real or simulated driving scenario complies with the required minimum headway between agents.

[0051] Figure 2 shows the steps of a time-headway intersection check planning method implemented at a given time by the motion planner for an ego vehicle approaching a roundabout. The timeheadway intersection check can be implemented to evaluate generated trajectories in a range of driving scenarios, including overtakes, lane changes, etc. The ego vehicle 204 is shown waiting to enter the roundabout at a current time. An external agent 202 is shown already on the roundabout. The present example is a simple driving scenario having only one external agent, though the method described herein can be applied to scenarios having many agents by repeating the method for checking the compliance of a given ego trajectory with a timeheadway requirement for each of the other agents of the scenario. The agents to be included in the time-headway intersection check algorithm (i.e. the ‘relevant agents’) may be determined based on one or more defined strategies that define the exclusion criteria for external agents. These strategies could exclude agents based on their positions, classification (e.g. car, van, bicycle, pedestrian, etc.), and / or some required clearance distance between the agent and the ego vehicle. It should be noted that an agent that is excluded from the timeheadway intersection check at any given time may be considered relevant, and therefore included in the time-headway intersection check at a different point in time, for example if a given agent moves within a certain distance of the ego vehicle 204.

[0052] At a first step, shown in Figure 2(a), the relevant agents are determined. If a strategy is defined to determine which agents should be excluded, then this strategy is applied, for example by excluding any agents that are further than some threshold distance from the ego vehicle. As shown by the dashed boundary in Figure 2(a), a relevant region of the road layout may be identified, with only those agents within this boundary included in the time-headway intersection check algorithm.

[0053] Once the agents included in the time-headway intersection check have been determined, the second step comprises determining headway regions 212 for each agent 202. The headway region defines the distance expected to be covered by the agent along a predicted trajectory within a predefined time interval. If a second agent’s predicted trajectory does not intersect with this headway, this means that the agent 202 can safely avoid collision with the other agent since it has at least a certain amount of time to take evasive action, for example by braking or by changing lanes to avoid collision. The headway region can be determined based on both safety and comfort considerations. The minimum headway region required to ensure that an agent can safely stop may not provide a comfortable experience for a human user of the vehicle, due to harsh braking, sudden turns, etc. Therefore, the minimum headway requirement should be set such that the agent can both safely and comfortably avoid collision with any forward vehicles throughout the driving scenario. The time-headway intersection check algorithm can be performed for any suitably defined minimum headway requirement, and this may be defined differently for different driving scenarios, types of agents, etc. A trajectory satisfying this requirement is referred to as ‘valid’ according to the time-headway intersection check described herein. The prediction system 104 described above can be configured to generate multiple possible predicted behaviours for the agent 202 at a given time. For example, the prediction system 104 may generate a predefined number of possible trajectories, as well as an associated probability score associated with each predicted trajectory. This is also referred to herein as multimodal prediction. Alternatively, the prediction system 104 may generate a single trajectory for the agent 202 and any other agent of the scene. In the case of a multi-modal prediction, a filtering process may be applied such that only trajectories that meet a threshold probability are considered in the time-headway intersection check. A headway region in this case is then generated for each of the trajectories that meet this threshold probability based on the output of the prediction system 104. An example probability threshold could be defined as: wherein N is the number of predicted trajectories generated for the given agents, and where a is a constant, which may be set to a suitable number according to a desired level of confidence. Setting a to a higher value results in fewer trajectories being selected. For example, a value of a = 1 results in a threshold probability of 1 / N . Where the prediction system generates five possible trajectories, for example, this means that only those trajectories having a probability greater than 1 / 5 are included in the time-headway intersection check. Lower thresholds are more likely to include more possible trajectories. The threshold may be lowered to account for less likely but still plausible agent behaviours, which would result in more conservative ego vehicle planning. However, in cases having many agents and many possible trajectories for each agent, it may become difficult to generate a suitable ego vehicle trajectory that accounts for all possible headway regions of the agents. A suitable threshold should be chosen to balance the need to account for uncertainty in agent behaviours with the need for the ego vehicle to make progress in the driving scene based on expected agent behaviours.

[0054] The headway region for each trajectory meeting the minimum probability threshold (or, in the case of unimodal prediction, the trajectory for the given agent 202) is determined based on the predefined time duration defining the headway for the given agent, and the agent’s ‘footprint’, i.e. the dimensions of the agent in the 2D plane. The agent’s footprint at each of multiple ‘nodes’ along its predicted trajectory within the predefined time duration is determined, and these footprints are combined to generate the headway region, which should be avoided by all other agents in order to ensure safe and comfortable driving.

[0055] As shown in Figure 2(b), the agent 202 has two possible headway regions 212a, 212b, each associated with a different predicted trajectory. The first headway region 212a is associated with a predicted trajectory in which the agent continues around the roundabout, passing the ego vehicle’s road, and the second headway region 212b is associated with a different predicted trajectory in which the agent exits the roundabout at the ego vehicle’s road. Each headway region is assessed separately against a possible ego trajectory according to the method described below. It should be noted that the headway region is required to be maintained ahead of the agent throughout a specified time horizon. Therefore, as the position of the agent 202 changes along the agent’s predicted trajectory, the time-headway region for the agent is also updated. A different time-headway region is defined based on the position of the agent at each timestep within the time horizon according to the given predicted trajectory. If the ego trajectory and any given time-headway region for the agent 202 fail the timeheadway intersection check then that ego trajectory is deemed not have met the constraint of the predefined time headway and is therefore not implemented as the planned trajectory for the ego vehicle 204.

[0056] A driveable path is provided as an input to the motion planner, along with the predicted trajectories for each agent. The input driveable path may be determined based on the road layout, for example taking the driveable path as the centre line of the ego vehicle’s current driving lane. Alternatively, the driveable path may be determined by a different planning component, for example an optimisation-based planner which generates a path towards a driving goal that satisfies certain constraints on safety, comfort and progress. Such a planning architecture is described in more detail below, with reference to Figure 4.

[0057] The time-headway intersection check aims to determine if a trajectory defining a motion profile for the ego vehicle along the driveable path satisfies safe and comfortable headway gaps between the ego vehicle and the other agents of the scenario. At the third step, as shown in Figure 2(c), a time-headway region 212 is defined for the agent 202 along one of the predicted trajectories for that agent that continues around the roundabout. This time-headway region is the region covered by the agent along its predicted path within a specified time duration according to its speed. This may be based on an assumption that the agent continues at its current speed, or it may be determined based on a predicted motion component of the agent’s trajectory as generated by the prediction system 104. The intersection check is performed for each timestep of a pre-defined time horizon.

[0058] It should be noted that the length of the pre-defined time horizon over which the timeheadway intersection check is performed, i.e. the number of future timesteps for which the headways of the agents are compared, is unrelated to the time duration defining the headway for each agent. The time horizon defines how far into the future the ego vehicle is looking when determining whether a given trajectory is expected to respect the required time headways, while the time duration defining the headway itself is based on the time required for each agent to be able to safely and comfortably stop or take some other action to avoid collision with another agent.

[0059] A first part of the time-headway intersection check, shown in Figure 2(c), is performed between the ego vehicle’s state along the trajectory being evaluated and the time-headway region defined for the agent 202 for each given timestep. A future state of the agent, also referred to herein as a footprint 222, and a future state (footprint) for the ego vehicle 224 are shown in Figure 2(c) for a particular timestep within the specified time horizon. The state of the ego vehicle and the state of the agent at each timestep is determined by the respective ego trajectory being assessed and predicted agent trajectory. The time-headway region 212 of the agent 202 for the given timestep is also shown. According to the first part of the timeheadway intersection check, it is determined for each timestep whether the ego vehicle’s footprint 224 intersects with the time-headway region 212 defined for the agent 202. As shown, the ego state 224 at the given timestep as determined based on the ego trajectory being assessed does intersect with the agent’s time-headway region at the corresponding timestep. That ego trajectory is therefore determined not to have met the required timeheadway constraint, and that ego trajectory is not output by the motion planner to be implemented by the ego vehicle.

[0060] The second part of the check is shown in Figure 2(d), in which the time-headway region for the ego vehicle itself is considered against the possible future agent states along their respective predicted trajectories. The time-headway region 214 of the ego vehicle is determined based on the ego trajectory being assessed, by defining a fixed time duration and determining what distance the ego vehicle would cover when following the planned ego trajectory over that time duration. The time duration may be set as a single fixed duration applied to all agents, or alternatively, different time durations may be set to define different time-headway gaps for different agents. The time-headway regions (which may alternatively be referred to herein as ‘hulls’) for the ego vehicle and for other agents of the scenario are built from trajectories in that, at each point or ‘node’ of the trajectory corresponding to the position of the agent along that trajectory at a respective timestep, a polygon is created that represents the configured time headway required to be maintained from that position and timestep. The polygon at each timestep is computed based on the position of the respective agent and the footprint of that agent. The nodes of each trajectory are spaced at equal time intervals, for example intervals of 0.1 seconds. The spacing of nodes of each trajectory is a configurable parameter of the planner.

[0061] The time-headway region of the ego vehicle is generated once only for each ego state of the candidate ego trajectory, and compared with the time-headway regions for each of the relevant agents, based on each of their predicted trajectories. In other words, the same timeheadway regions computed for the candidate ego trajectory can be computed once and used repeatedly in checks against different possible trajectories of the other agents of the scene. The footprint 222 of the agent 202, which represents the position of the agent at a future timestep along one of its predicted trajectories, is assessed to check if it intersects with the time-headway region 214 of the ego vehicle at the corresponding timestep. The check is repeated for each timestep within the time horizon, and for each relevant predicted trajectory (i.e. each trajectory meeting the predefined probability threshold) of each relevant agent, as determined based on the strategies and confidence thresholds on trajectory probabilities described above.

[0062] As with the check described above between the ego position and the agent time-headway region, the ego trajectory is considered invalid if the ego time-headway region intersects with any predicted position of any of the relevant agents of the scene at any point in time within the predefined time horizon. If no intersection exists, over the defined time horizon, between the ego time-headway as defined based on the given ego trajectory and any predicted position of any relevant agent, and no intersection is found between the time-headway regions defined for each agent and the predicted positions of the ego agent, then the ego trajectory complies with the required time-headway gap. As shown in Figure 2(d), the ego time-headway region 214 does not intersect with the predicted position 222 of the agent at the particular timestep being assessed. Nevertheless, the proposed ego trajectory to which the given ego timeheadway region 214 is associated is not valid according to the required time-headway constraint due to the intersection between the predicted ego position 224 and the agent time- headway region 212, and that ego trajectory will not be implemented by the controller 108. As described in further detail below, in most implementations, the algorithm is configured to halt as soon as one violating condition is found. In other words, as soon as it is determined, as shown in Figure 2(c), that the ego vehicle’s state along a planned trajectory intersects with the predicted headway of another agent, the algorithm is halted and no further checks are performed for that ego trajectory. In that case, the check shown in Figure 2(d) is not performed, and the ego vehicle takes an alternative trajectory according to some default behaviour which is known to be safe. The two checks shown in Figure 2(c) and 2(d) are shown to illustrate the principle of computing both types of intersection.

[0063] The time-headway intersection check ends once an intersection of the ego state with any of the agent time-headway regions, or an intersection of a predicted agent state with the ego time-headway region is found. If no intersection occurs for a given time step and agent 202, the process is repeated for each timestep of the time horizon, and for every agent identified as relevant according to the strategies described above until such an intersection is found. If the ego state intersects with any time-headway region for any single timestep and for any relevant agent of the scene, the ego trajectory is determined not to have met the required time-headway constraint. If the ego state never intersects with any of the time-headway regions identified for the different possible trajectories of the different agents of the scenario, then the ego trajectory has met the required agent time-headway constraint for the predefined time horizon over which the assessment is performed.

[0064] Figure 2 shows an example implementation of the time-headway intersection check for a single agent at a single timestep. However, as mentioned, in practice, these methods can be implemented for multiple agents, each having multiple predicted trajectories, over multiple timesteps of a predefined time horizon.

[0065] Figure 3 shows a flow diagram describing a full time-headway intersection check algorithm as implemented in a planning context for multiple agents over the multiple timesteps of the time horizon. The intersection check algorithm is also provided in pseudocode below (Algorithm 1).

[0066] At a first step S302 of the time-headway intersection check algorithm, the motion planner generates a candidate ego trajectory based on the current state of the ego vehicle 204 and the environment. The candidate trajectory comprises a path component, which is provided to the motion planner of the planning system 106 as an input driveable path, and a motion profile which is generated by the motion planner along the driveable path based on the current state of the ego vehicle and the environment. The environment includes the road layout in which the ego vehicle drives, as well as the states of other agents and any other objects present in the driving scenario. The road layout may be determined based on annotated map data and / or outputs generated by the perception system 102 based on sensor data, and could include information about the road geometry, such as the position and location of road lanes, junctions and other road features, and information such as speed limits applicable at different points within the road layout. The environmental data also includes state information for other agents of the driving scenario, including the position, speed and acceleration of the other agents. The motion profile of the candidate trajectory is generated based on the state of the ego vehicle and the environment. As one example, in a highway driving scenario, the speed of the ego vehicle along the predefined driveable path, which may be, for example, the centre line of the ego vehicle’s current road lane, is determined based on the speed limit defined for the given road lane on which the ego vehicle is driving. An example motion plan along that driveable path may be to follow the driveable path at a fixed speed below the speed limit defined for the given road lane. If a forward agent (i.e. an agent driving along the same road lane as the ego vehicle and ahead of the ego vehicle) is present within a certain distance of the ego vehicle, a possible motion plan may be to follow the agent at a fixed speed relative to the forward agent. A behaviour in which the ego vehicle travels at a constant speed unless a forward vehicle is present, and adjusts its speed once the ego vehicle is within a certain distance of the forward vehicle is called adaptive cruise control (ACC). Other behaviours may be used to generate a proposed ego motion profile along the input driveable path provided to the motion planner.

[0067] The generated ego trajectory is then used by the motion planner to determine a time-headway region for the ego vehicle according to step S304. The time-headway region for the ego vehicle is based on the determined ego trajectory and a predefined time duration, i.e. minimum time separation between the ego vehicle and any forward agent (i.e. a minimum time for the ego agent to reach the position of a forward agent). The region is determined from a current state of the ego vehicle as the estimated distance covered by the ego vehicle within this pre-defined time duration, following the candidate ego trajectory generated by the motion planner at the previous step S302. A time-headway region is determined from each ego position corresponding to each timestep along the candidate trajectory within the predetermined time horizon over which the intersection check is performed. As mentioned above, a number of strategies may be input to the motion planner, based on which the agents included in the intersection check are determined. In the case that a clearance line strategy is defined, the time horizon, which is the time period over which the intersection check is performed, and which is defined in advance, is conditioned on the clearance line strategy (step S306). The clearance line is defined at some set distance from the ego vehicle, providing a distance that is required to be cleared by the ego vehicle within the defined time horizon. In the case that according to the given ego trajectory, the given clearance distance is covered by the ego vehicle within the provided time horizon, the intersection checks are only performed up to the point where the ego vehicle reaches this line. Otherwise, the intersection checks are performed over the full time horizon. The motion planner may also record whether the ego vehicle meets the clearance line within the defined time horizon, with this being a further condition for determining a valid ego trajectory. For example, where the intersection check does not identify any time-headway intersections between the ego vehicle trajectory and the predicted trajectories for the relevant agents of the scene, but the ego vehicle trajectory also does not meet the clearance line within the time horizon, the ego trajectory may still be considered invalid since it does not make sufficient progress. Other strategies may be implemented in order to apply other conditions to the ego trajectory such that multiple safety, progress and / or comfort constraints can be met.

[0068] At step S308 any further optional strategies defining which agents to include in the intersection check are applied in order to identify relevant agents. As mentioned above and shown in Figure 2, one possible strategy is to define a region within the road layout and only include those agents positioned within that region. Alternatively, only certain classifications of agents may be considered, as determined based on the detections of the perception system 102. The perception system outputs a classification identifying the type of agent that it detects. This is typically in the form of a probability distribution, with the perception system 102 determining a probability value that a detected object belongs to each possible agent class. For example, the strategy may include agents that the perception system 102 identifies as cars and vans while excluding objects identified by the perception system as bicycles and pedestrians.

[0069] Once a set of relevant agents is determined, a first agent is selected at step S310 for assessing against the ego vehicle. The following process is applied to the first agent, and repeated for each agent of the set of relevant agents. As mentioned above, the prediction system 104 may be configured to generate single predictions for each agent (unimodal prediction) or multiple predictions for each agent (multimodal prediction).

[0070] In the case of multimodal prediction, where the multiple different trajectories are generated by the prediction system along with a probability or other confidence score associated with each trajectory, representing the confidence that the prediction system 104 assigns to each of the trajectories. At step S312, these confidence scores are compared with some threshold confidence value, and only those trajectories that meet the threshold confidence are selected as relevant. The threshold is chosen such that highly unlikely trajectories are not included in the analysis, such that the intersection check can be performed more quickly, while still accounting for some uncertainty in agent behaviours. As mentioned above, a higher threshold leads to fewer possible thresholds being considered, which may lead to a less constrained ego trajectory, with some risk of that ego trajectory breaking the time-headway requirement if the agent takes a less likely trajectory that has been excluded from the analysis. A lower threshold is safer, in that more agent trajectories are considered and avoided by the ego trajectory, but this could constrain the ego trajectory to such an extent that no suitable ego trajectory can be planned such that the required time-headway is guaranteed for all possible agent trajectories. The threshold should therefore be chosen at a reasonable level such that a suitable ego trajectory can be planned taking uncertainty into account, based on the given context and the number of agents and predicted trajectories being provided by the prediction system 104.

[0071] At step S314, the first trajectory of the relevant trajectories is selected for analysis. For this trajectory, the time-headway regions of the agent are generated by the motion planner along each timestep of the predetermined time horizon at step S316. The generation of the agent time-headway regions is the same as the generation of the ego time-headway regions, described above, wherein for each timestep, a polygon is created that represents, in space, the distance covered within the defined time duration from the position of the agent starting at that timestep.

[0072] At step S318, the intersection check is performed based on the determined ego time-headway region and the agent time-headway region for the selected agent trajectory. The intersection check is performed for each timestep within the time horizon (or until the ego vehicle reaches a clearance line, as mentioned above). At each timestep, the intersection check determines first whether the footprint of the ego vehicle, i.e. a bounding box defining the extent of the ego vehicle at the position corresponding to the given timestep along the ego trajectory determined at step S302, intersects with the time-headway region defined for the agent trajectory at the corresponding timestep. If so, then the intersection check returns a value of ‘true’ (S322). If not, the intersection check moves on to the step of checking, for the same timestep, if the agent footprint at that timestep according to the selected trajectory intersects with the ego time-headway trajectory at the corresponding timestep. If so, the intersection check returns a value of ‘true’ (S322). In this case, the ego vehicle does not take the trajectory being assessed, and may take some default trajectory according to some known safe behaviour, for example by implementing a slowdown maneuver by decelerating along a default path in a comfortable way. The intersection check is provided in pseudocode below (Algorithm 2). The process repeats for each timestep for the ego trajectory and selected agent trajectory for as long as no intersections are found. The algorithm ends as soon as any condition returns ‘true’, since the given trajectory is then deemed invalid, since it violates the time headway requirement. This leads to performance gains with no loss in safety, since any trajectory violating the time headway requirements is discarded. In alternative implementations, however, the algorithm could be configured to continue an exhaustive check of all agents and their associated trajectories in full, identifying each time-headway violation. This may be used in the context of testing, where it may be useful to evaluate some overall level of compliance of a given ego trajectory with the time headway requirement, taking all other agents and their possible behaviours into account.

[0073] If an intersection is found the algorithm returns the value ‘true’, which provides an indicator to the planner that the candidate trajectory being evaluated is not suitable to be implemented, since it does not comply with the required headway. The planner uses this indicator to determine that the ego vehicle should instead follow an alternative default trajectory.

[0074] Once the intersection check is done for the selected agent trajectory, assuming that no intersections are found, the algorithm proceeds to step S324 in which it is determined whether all trajectories for the selected agent have been checked. If further trajectories remain, the next trajectory is selected for that agent (S326), and the steps S316-S320 are repeated in order to do an intersection check for that trajectory. This is done until all trajectories for the given agent have been checked or until an intersection is found and the algorithm halts at step S322. Once all the trajectories for the selected agent have been checked, at step S328 it is determined whether all relevant agents have been checked. If further relevant agents exist that have not yet been checked, the next agent is selected (step S332), and the same process is repeated, determining which trajectories are relevant based on confidence scores, and checking each relevant trajectory in turn.

[0075] Once all trajectories for each relevant agent have been checked according to the process described above, if no time-headway intersections have been found between the ego trajectory and any of the agent trajectories, then a value of ‘false’ is returned (S330), and the ego trajectory is determined to comply with the required time headway. As mentioned above, it does not necessarily mean that the ego trajectory is suitable to be implemented by the controller 108 of the ego vehicle just because it meets the required time-headway constraints, since it may fail other requirements, such as progress towards a clearance distance. The planner may therefore combine the results of the time-headway intersection check with other conditions on the ego trajectory before determining whether to pass that ego trajectory (or controls defining that ego trajectory) to the controller in order to control the ego vehicle to move along that trajectory.

[0076] The algorithm for the time-headway intersection check as performed for multiple agents as described above is provided below.

[0077] Algorithm 1 - Time-Headway Intersection Check

[0078] TEV«- MakeTrajectory (Currentstate, Environment)

[0079] HEV«- MakeHulls(TEV)

[0080] Reason about the clearance line strategy to condition time horizon Relevant Agents «- GetRelevantAgents(agents, strategies) for agent in Relevant Agents do

[0081] C «- ConfidenceThreshold(agent. trajectories) for TAin agent, trajectories do if TA. confidence > C then

[0082] HA«- MakeHulls(TA) for HEV. in HEVdo if IntersectionCheck(HEVHAthen return True end if end for end if end for end for return False

[0083] In the above pseudocode, each trajectory of a given agent is processed in turn, and the steps of generating the agent time-headway region and performing the intersection check are conditional on the trajectory meeting the predefined confidence threshold. ‘MakeHulls’ generates a set of footprints and time-headway regions corresponding to each timestep of the given trajectory. The index i refers to each timestep within the time horizon, for which there is a respective time-headway region and footprint for both the ego agent and the other relevant agents of the scenario.

[0084] The IntersectionCheck method above is described in pseudocode below:

[0085] Algorithm 2 - IntersectionCheck

[0086] HAHull and footprint of agent

[0087] HEV\ Hull and footprint of ego vehicle if Intersects HA. hull, HEV. footprint) then return True end if return Intersects HEV. hull, HA. footprint)

[0088] The above-described time-headway intersection check method can be implemented by a motion planner as part of a planning (sub-)stack 106 of the autonomous vehicle stack. This motion planner is configured to implement the algorithm above in order to ensure that the ego trajectory satisfies the behaviour of maintaining the required distance between agents. The motion planner is configured to implement the time-headway intersection check to generate a motion component for a given input driveable path. This driveable path may be provided to the motion planner directly based on the road layout and a given driving context. For example, in a highway driving context, the default driveable path may simply be the centre line of the ego vehicle’s driving lane.

[0089] Alternatively, an optimisation-based planner may be used to generate the initial driveable path so as to generate a suitable path for the ego vehicle that satisfies certain planning goals and constraints. An example planning architecture is shown in Figure 4. According to this architecture, a path initialisation 400 is provided to the optimisation-based planner 402, where the initialisation path is generated based on a road layout that defines the road geometry and features including driving lanes, junctions, etc. The optimisation-based planner 402 then implements a constrained optimisation process in order to generate both a spatial path and a motion component for the ego vehicle according to defined constraints on planning. These constraints include safety constraints, such as constraints preventing collision with other agents or obstacles, as well as constraints requiring progress towards a planning goal. Constraints may be ‘hard’ constraints that encode requirements, such that any trajectory determined by the planner must meet those requirements, or ‘soft’ constraints, which are encouraged during optimisation, but not strictly required. An example of a hard constraint is collision avoidance. One example soft constraint encourages the ego trajectory to stay close to a midline of the ego driving lane, by penalising trajectories that deviate from the midline, without explicitly requiring the trajectory to stay on or near the midline. An example constrained optimisation-based planner that may be used in this context is described in further detail in International Patent Publication Nos. WO2021 / 152047 and W02021 / 152050, which are herein incorporated by reference in their entirety.

[0090] The optimisation-based planner 402 generates a trajectory that includes both a spatial component (path) 404 defining the ego vehicle’s pose (position and orientation) over time, and a motion component 406 comprising motion variables defining the ego vehicle’s speed, acceleration, etc. along that path. Both the spatial and motion components are determined so as to satisfy constraints on the ego vehicle’s motion imposed during the constrained optimisation. For example, the shape of the path 404 satisfies constraints on steering angle such that the path can be implemented safely by the ego vehicle, as well as avoiding collision with agents when combined with the generated motion component. This ensures that the input driveable path input to the motion planner 408 is a suitable path meeting certain guarantees on ego motion and safety. A motion profile generation component 412 of the motion planner 408 takes this input driveable path, and, as described above, generates a candidate motion component along that path according to a defined behaviour, such as adaptive cruise control, described above, thereby generating a candidate trajectory comprising the input driveable path and the candidate motion component. The candidate trajectory is provided to an intersection check module 414 implemented by the motion planner 408, which carries out the process described above with reference to Algorithms 1 and 2 and Figure 3 to determine if the candidate trajectory is valid according to the required time-headway gap. If the trajectory is not valid, i.e. if at least one time-headway intersection occurs based the candidate ego trajectory, the planner reverts to a default behaviour, such as a slowdown maneuver, as a fallback to the candidate trajectory. If no intersection is found, and the candidate ego trajectory is deemed to be valid according to the time-headway requirement, this is output as a planned trajectory 410. As noted above, however, the motion planner may include other checks and conditions that are applied to a candidate trajectory to evaluate aspects of the ego trajectory relating to safety, comfort or progress. These conditions may be applied before or after the intersection check, with a planned trajectory 410 only being provided to the controller 108 of the ego vehicle stack if all the requirements are satisfied.

[0091] Alternatively, the time-headway intersection check may be used in other contexts, for example in the context of testing, in order to evaluate an ego trajectory that was taken by the ego trajectory in a real -world or simulated driving scenario for safety and comfort according to the minimum time-headway constraint. In this case, the intersection check is performed at each timestep of the actual ego trajectory, and instead of predicted states of the external agents, the actual state of the agents are used at each timestep to generate a time-headway region. The intersection check is then performed as described above, where, once any intersection is found between the ego vehicle and the external agent, the ego trajectory is evaluated as ‘unsafe’ or ‘invalid’ or otherwise labelled as a failure trajectory, since the required conditions on time-headway were not met. This data can be used to determine which driving scenarios cause the ego vehicle to breach the required time-headway gaps, which can be improve the planner of the autonomous vehicle stack over time.

[0092] References herein to components, functions, modules and the like, including the perception stack 102, prediction stack 104, planning stack 106, controller 108, trajectory generator 412, motion planner 408, etc. denote functional components of a computer system which may be implemented at the hardware level in various ways. A computer system comprises one or more computers that may be programmable or non-programmable. A computer comprises one or more processors which carry out the functionality of the aforementioned functional components. A processor can take the form of a general-purpose processor such as a CPU (Central Processing unit) or accelerator (e.g. GPU) etc. or more specialized form of hardware processor such as an FPGA (Field Programmable Gate Array) or ASIC (Application-Specific Integrated Circuit). That is, a processor may be programmable (e.g. an instruction-based general -purpose processor, FPGA etc.) or non-programmable (e.g. an ASIC). A computer system may be implemented “onboard” a mobile robot (such as an AV) or “offboard” for example in a simulator context.

Claims

Claims1. A computer-implemented method of evaluating a candidate trajectory for an ego agent in the presence of at least one external agent, comprising: determining a predicted state of the at least one external agent at a future timestep and the corresponding state of the ego vehicle along the candidate trajectory at the future timestep; determining a headway region for each of the ego agent and the at least one external agent, the headway region for each agent defining the region occupied by that agent within a predefined time duration from the predicted state, based on the predicted state of the agent; performing a first intersection check to determine if the predicted state of the at least one external agent intersects with the headway region of the ego agent; and performing a second intersection check to determine if the predicted state of the ego agent intersects with the headway region of the at least one external agent; wherein, if either of the first or second intersection checks determine that an intersection exists, the candidate trajectory is evaluated as invalid.

2. A computer-implemented method according to claim 1, comprising outputting an indicator that the candidate trajectory is evaluated as invalid.

3. A computer-implemented method according to claim 1 or 2, wherein the at least one external agent comprises a plurality of external agents, the method comprising, for each external agent of the plurality of external agents: determining a predicted state of the external agent at the future timestep; determining a headway region for the external agent, defining the region occupied by that agent within the predefined time duration from the future timestep, based on the predicted state of the agent; and performing the first and second intersection checks based on the predicted state and headway region for that agent; wherein, if either of the first or second intersection checks determine, for any external agent, that an intersection exists, the candidate trajectory is evaluated as invalid.

4. A computer-implemented method according to any of claims 1 to 3, comprising:determining a plurality of predicted states for the or each external agent at the future timestep; determining a plurality of headway regions for the or each external agent, each headway region defining the region occupied by the external agent within the predefined time duration from a respective predicted states of the plurality of predicted state, based on that predicted state; and performing the first and second intersection checks based on each of the plurality of predicted states and corresponding headway regions determined for the or each external agent; wherein, if either of the first or second intersection checks determine, for any of the predicted states or corresponding headway regions of the or each external agent, that an intersection exists, the candidate trajectory is evaluated as invalid.

5. A computer-implemented method according to any preceding claim, comprising determining the respective state and corresponding headway for each of the ego agent and the at least one external agent for each of a plurality of timesteps within a predetermined time horizon, wherein the first and second intersection checks are performed for each timestep based on the respective states and headways at that timestep; wherein if either of the first or second intersection checks determine, for any timestep of the plurality of timesteps, that an intersection exists, the candidate trajectory is evaluated as invalid.

6. A computer-implemented method according to claims 1 or 2, comprising determining a control signal and providing control signal to an AV controller based on the determined validity of the candidate trajectory.

7. A computer-implemented method according to any preceding claim, wherein each of the predicted state of the external agent and the corresponding state of the ego agent comprise at least a pose and a speed of the respective agent at the future timestep.

8. A computer-implemented method according to claim 7, wherein the headway region for the external agent defines the region occupied by that agent within predefined time duration, based on the external agent continuing at the speed of the predicted state.

9. A computer-implemented method according to any preceding claim, wherein the headway region for the external agent defines the region occupied by that agent within thepredefined time duration, based on the external agent following a predicted motion profile for that agent.

10. A computer-implemented method according to any preceding claim, wherein the at least one external agent is selected based on meeting an inclusion criterion of a predefined exclusion strategy.

11. A computer-implemented method according to claim 10, wherein the inclusion criterion is based on one or more of a position of the external agent; and a classification of the external agent.

12. A computer-implemented method according to any preceding claim, wherein the candidate trajectory is generated for the ego agent based on an input driveable path and one or more environmental variables.

13. A computer-implemented method according to claim 12, wherein the environmental variables are determined from annotated map data and / or generated by a perception system based on sensor data of the ego agent.

14. A computer-implemented method according to claim 12, wherein the input driveable path is generated by an optimisation-based planner that generates driveable trajectories subject to one or more planning constraints.

15. A computer-implemented method according to claim 5 or any claim dependent thereon, wherein the plurality of timesteps comprise the timesteps until the ego trajectory reaches a predefined clearance line.

16. A computer-implemented method according to claim 4 or any claim dependent thereon, wherein a set of predicted states and a corresponding set of confidence scores associated with each predicted state are generated for the or each external agent; wherein the plurality of predicted states for the or each external agent are determined by selecting those predicted states of the set of predicted states for which the corresponding confidence score meets a predefined confidence threshold.

17. A computer-implemented method according to any preceding claim, implemented during testing of an ego agent, wherein the candidate trajectory is a real or simulatedtrajectory taken by an ego agent in a real or simulated driving scenario, and wherein an invalid evaluation corresponds to a failure of the ego agent.

18. A computer-implemented method according to any of claims 1-16, implemented by a planning component of the ego agent, wherein, if none of the intersection checks determine that an intersection exists, the candidate trajectory is evaluated as valid, and output to a controller component of the ego agent to control the vehicle so as to follow the candidate trajectory.

19. A computer system comprising memory and one or more processors configured to implement the method of any of claims 1-18.

20. A computer program comprising executable instructions configured, when executed on one or more hardware processors, to implement the method of any of claims 1-18.T1

Images