Motion planning for mobile robots

Abstract

A computer-implemented method of planning actions for a mobile robot in the presence of occlusion The method comprises determining a first occluded region in a mobile robot field of view at a first time; sampling a first set of agent particles within the first occluded region; planning, using a robotic planner, based on a location and a motion state of a first agent particle of the first set of agent particles a first mobile robot action, the first mobile robot action accounting for predicted motion of the first agent particle; generating a first mobile robot control signal for causing execution of the first mobile robot action; determining a second occluded region in the mobile robot field of view at a second time; computing a propagated set of agent particles for the second time based on the first set of agent particles and a particle motion model; assigning an occlusion weight to each agent particle of the propagated set of agent particles with respect to the second occluded region; sampling a second set of agent particles within the second occluded region based on the propagated set of agent particles and the occlusion weight assigned to each agent particle; planning, using the robotic planner, based on a location and a motion state of a second agent particle of the second set of agent particles a second mobile robot action, the second mobile robot action accounting for predicted motion of the second agent particle; and generating a second mobile robot control signal for causing execution of the second mobile robot action.

Description

MOTION PLANNING FOR MOBILE ROBOTSFIELD

[0001] The present disclosure relates to motion planning systems and methods for autonomous vehicles and other mobile robots.BACKGROUND

[0002] A rapidly emerging technology is mobile robots, one example being 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 other agents in the environments. An autonomous vehicle or 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 sensor outputs from the sensor system. A fully-autonomous or "driverless" vehicle has sufficient decisionmaking 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.

[0003] Planning relies on perception outputs generated based on sensor measurements such as camera images, radar and / or lidar measurements to determine where other agents or objects are present in the environment so as to avoid collision with other agents or objects while making progress towards a motion planning goal (such as a predetermined location within an environment such as a road network).SUMMARY

[0004] Motion planning for a mobile robot (the ego robot) depends on several factors. Two important and competing factors are progress and safety. Occlusion is a challenge when balancing these factors. Occlusion may be caused by dynamic agents (such as vehicles, cyclists, pedestrians, animals etc.) or static objects within a mobile robot field of view. Occlusion can also occur outside of the mobile robot field of view (causes by limited sensor / perception ranges). An occluded region might contain occluded agent(s) that are not currently perceptible to the ego robot. Unsafe motion planning decisions may occur if the possibility of occluded agents is not properly considered. On the other hand, overly conservative assumptions about the presence of occluded agents can result in excessively conservative motion planning decisions that unnecessarily hinder progress.

[0005] Herein, a “visibility reasoning” mechanism is provided to support motion planning in the presence of occlusion. The present visibility mechanism uses a samplingbased approach to encode a belief over potentially unseen agents, and to track that belief over time. This sampling-based approach estimates which agents could realistically exist in occluded region(s) as time progresses. The ability to propagate beliefs over time, in a realistic and statistically rigorous manner, provides a good balance between safety and progress.

[0006] A first aspect of the present disclosure provides computer-implemented method of planning actions for a mobile robot in the presence of occlusion, the method comprising: determining a first occluded region in a mobile robot field of view at a first time; sampling a first set of agent particles within the first occluded region; planning using a robotic planner, based on a location and a motion state of a first agent particle, of the first set of agent particles a first mobile robot action, the first mobile robot action accounting for predicted motion of the first agent particle; generating a first mobile robot control signal for causing execution of the first mobile robot action; determining a second occluded region in the mobile robot field of view at a second time; computing a propagated set of agent particles for the second time based on the first set of agent particles and a particle motion model; assigning an occlusion weight to each agent particle of the propagated set of agent particles with respect to the second occluded region; sampling a second set of agent particles within the second occluded region based on the propagated set of agent particles and the occlusion weight assigned to each agent particle; planning using the robotic planner, based on alocation and a motion state of a second agent particle of the second set of agent particles, a second mobile robot action, the second mobile robot action accounting for predicted motion of the second agent particle; and generating a second mobile robot control signal for causing execution of the second mobile robot action.

[0007] In embodiments, the method may comprise detecting an agent at a previous time preceding the first time step and determining that the agent has entered the first occluded region at the first time step. An enrichment particle may be sampled in the first occluded region at the first time step corresponding to the agent detected at the previous time, wherein the first set of agent particles may comprise the enrichment particle and wherein the enrichment particle may have an inferred location and motion state based on a detected location and motion state of the agent detected at the previous time.

[0008] The method may comprise sampling a first plurality of enrichment particles with different location and / or motion states based on the location and motion state of the agent detected at the previous time.

[0009] The method may comprise detecting an intersecting portion of the first occluded region between the first occluded region and a maximum agent perception range of the field of view of the mobile robot and sampling a second plurality of enrichment particles within the intersecting portion of the first occluded region.

[0010] The method may comprise assigning the first set of agent particles, the first set of agent particles comprising the first and second plurality of enrichment particles, uniform occlusion weights.

[0011] A first subset of agent particles may be selected from the first set of agent particles. The first subset of agent particles may comprise some but not all of the first set of agent particles, and the first agent particle may be within the first subset of agent particles. The first mobile robot action may be planned accounting for predicted motion of the first subset of agent particles.

[0012] A second subset agent particles may be selected from the second set of agent particles, the second subset of agent particles may comprise some but not all of the second set of agent particles, and the second agent particle may be within the second subset of agent particles. The second mobile robot action may be planned accounting for predicted motion of the second subset of agent particles.

[0013] The method may comprise applying a clustering algorithm to sample the first subset of agent particles and the second subset of agent particles.

[0014] The first set of agent particles and the second set of agent particles may each be uniformly assigned occlusion weights.

[0015] The occlusion weight assigned to any particle of the propagated set of agent particles located outside of the second occluded region may be zero.

[0016] The method may comprise applying a sigmoid weighting function to the propagated agent particles located outside the second occluded region, whereby the occlusion weight may reduce as a sigmoid function of distance outside the second occluded region.

[0017] The first occluded region and the second occluded region may be determined using LIDAR measurements.

[0018] The first occluded region and the second occluded region may be determined using a bounding box of an occluding object.

[0019] The method may comprise determining the location and motion states of the first and second agent particles by sampling from a uniform distribution of longitudinal position within the first and second occluded regions respectively.

[0020] The method may comprise determining the location and motion states of the first and second agent particles by assigning zero lateral offset from a midline of the first and second occluded regions.

[0021] The method may comprise determining the location and motion states of the first and second agent particles by sampling from a non-uniform distribution of lateral offset from a midline of the first and second occluded regions.

[0022] The method may comprise determining the location and motion states of the first and second agent particles by sampling from a uniform distribution of speed.

[0023] The particle motion model may randomly sample the location and motion states from a uniform distribution.

[0024] According to a second aspect there is provided a computer program comprising executable instructions may be configured, when executed on one or morehardware processors, to implement a method in accordance with embodiments of the first aspect.

[0025] Further optional features of the second aspect are as defined in relation to the first aspect and may be combined in any combination.

[0026] According to a third aspect of the present disclosure there is provided a computer system, comprising one or more hardware processors configured to implement a method in accordance with the embodiments of the first aspect.

[0027] Further optional features of the third aspect are as defined in relation to the first and second aspect and may be combined in any combination.BRIEF DESCRIPTION OF THE DRAWINGS

[0028] To assist understanding of the present disclosure and to show how embodiments may be put into effect, reference is made by way of example to the accompanying drawings in which:

[0029] Figure 1 shows a schematic block diagram of an autonomous vehicle stack;

[0030] Figure 2 shows a flowchart and schematic illustration of a motion and planning method;

[0031] Figure 3 shows a schematic illustration of determining an occluded region.DETAILED DESCRIPTION

[0032] Embodiments of a planning method for mobile robots in the presence of occluded regions are described below. The described embodiments use a form of particle filtering to track beliefs about possible occluded agents within an environment over time, enabling effective planning and execution of mobile robot control actions.

[0033] Figure 1 shows a highly schematic block diagram of an example runtime stack 100 for a mobile robot. The mobile robot whose motion is planned is referred to as the ego robot below. The runtime stack 100 is shown to comprise a perception system 102, a prediction system 104, a planner 106 and a controller 108.

[0034] In a real -world deployment, the perception system 102 is coupled to an onboard sensor system 110 of the mobile robot and receives sensor outputs from the on-boardsensor system 110.

[0035] 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 mobile robot and any dynamic agents (vehicles, pedestrians, cyclists etc.) within that environment.

[0036] Hence, the sensor outputs typically comprise sensor data of multiple sensor modalities such as mono or stereo images from one or more stereo optical sensors, LiDAR, RADAR etc. Multiple sensors may be located around the vehicle e.g. to provide full 360° perception.

[0037] The perception system 102 typically comprises multiple perception components which co-operate to interpret the sensor outputs and thereby provide perception outputs to the prediction system 104. Examples of such perception components include object detectors, such as bounding box detectors, pose detectors, segmentation components etc. Data collected from multiple sensors / 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.).

[0038] An additional function of the perception system 102 is to determine any relevant occluded regions within the environment. Depending on the environment, it may be that only certain occluded region(s) are relevant. For example, when navigating a road layout, occluded road regions are more relevant than regions outside of the road limits. A predetermined map 103 of the environment, such as a high-definition road map, may be used in conjunction with the sensor outputs to determine any relevant occluded region(s) within the environment.

[0039] The perception system 102 also includes localization functionality for determining a location of the ego robot (ego location) in a frame of reference of the map 103. Various localization methods may be used, such as satellite positioning, odometry using camera or lidar data, inertial motion sensors etc.

[0040] For any given observation time, the perception system 102 is said to generatean observed scene comprising any detected agent(s) and any relevant occluded regions that have been determined. Each detected agent is represented in the output of the perception system 102 as an agent state comprising a location and a motion state (e.g. measured velocity, or velocity and acceleration etc.). For example, the observed scene may comprise the detected agent states(s) (if any), the determined occluded region(s) (if any) and the determined ego location in the map frame of reference.

[0041] The use of a predetermined map 103 is not essential. For example, road or lane boundaries could be determined by observation within the perception system 102. Predetermined map information could alternatively be combined with observed road layout information. Higher-level functions such as planning or prediction could also be performed in a local frame of reference of the ego robot, in which case the locations of the detected agent(s) and the occluded region(s) are determined relative to the ego vehicle and it is not necessary to locate the ego robot or dynamic agents in a map frame of reference.

[0042] An agent state may be a state in 2D space (such as a ‘bird’s-eye-view’ of the environment) or 3D space. Other perceived characteristics of an agent may be similarly represented in the output of the perceptions system 102. For example, the agent state may additionally include bounding box dimensions of a detected 2D or 3D bounding box around the agent.

[0043] Agent states from the perception system 102 are used by the prediction system 104 to predict future behaviour of perceived agents. For example, a trajectory of each detected agent may be predicted over a predefined planning time horizon.

[0044] In this manner, a scenario is determined using the perception system 102 and (where applicable) the map 103, with predictions about other agents incorporated from the prediction system 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 planning goal, such as a specified location or path located within the map 103.

[0045] The planner 106 plans appropriate actions for the ego robot (ego actions) in the driving scenario in a way that accounts for predicted agent motion.

[0046] For example, the planning goal could be to enter a roundabout and leave it ata desired exit; to join a road at a T-junction; to overtake a vehicle in front; or to stay in a current lane at a target speed (lane following). The planning 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 mobile robot as determined by a route planner. Progress towards the goal would in this case be measured as progress along the drivable path.

[0047] A visibility reasoning component 105 is shown within the stack 100, having an input coupled to the perception system 102 and an output coupled to the planning system 104. Using the visibility reasoning techniques described below, predictions generated by the prediction system 104 are extended to hypothesised occluded agents represented as agent “particles”. As described in more detail below, such particles are not detected by the perception system 102. Rather, the visibility reasoning component 105 samples particles within each relevant occluded region(s) representative of possible occluded (non-visible) agents. The visibility reasoning component 105 can pass a particle to the prediction system 104, and for prediction purposes, the prediction system 104 processes such particles in a similar manner to detected agents. Each particle has a particle state (comprising a location and motion state) used to predict its motion, comparable to a detected state. Predicted motion of particles passed to the prediction system 104 is accounted for by the planner 106 in the same way predicted motion of detected agents.

[0048] The controller 108 executes actions planned by the planner 106 by generating and outputting suitable control signals to an onboard actor system 112 of the mobile robot. The on-board actor system 112 provides control over primary functions such as acceleration, braking and steering angle / heading in the case of an autonomous road vehicle.

[0049] To enable reasoning into the future, the planner 106 may generate a relatively long-term ego trajectory (encapsulating a series of ego actions) over an appropriate planning time horizon (e.g. several seconds), with agent trajectories predicted over the same time horizon. This might, for example, involve generating multiple candidate ego trajectories, and evaluating each candidate trajectory against each predicted agent trajectory.

[0050] Planning systems typically operate over a sequence of planning steps, updating the planned trajectory at each planning step to account for evolution of the scenario. Typically, only a portion of the trajectory generated at any given planning step is realized by the controller 108 before the trajectory is updated in the next planning step. A trajectory may be encoded, for example, as a series of ego actions (e.g. changes in speedand heading) or series of ego states (e.g. position and motion states) that implicitly define ego actions to be taken in order to realize those states.

[0051] The example stack 100 has relatively “modular” architecture, with separable perception, prediction, planning and control systems 102-108. In other stacks, the extent to which the various stack functions are integrated or separable can vary significantly. The sub-systems themselves may also be modular, e.g. with separable planning modules within the planning system 106. For example, the planning system 106 may comprise multiple trajectory planning modules that can be applied in different physical contexts (e.g. simple lane driving vs. complex junctions or roundabouts). In other stacks, planning and control may be integrated (e.g. such stacks could plan in terms of control signals directly), whereas other stacks may be architected in a way that draws a clear distinction between the two (e.g. with planning in terms of longer-term trajectories, and with separate control optimizations to determine how best to execute a planned trajectory at the control signal level). Similarly, in some stacks, prediction and planning may be more tightly coupled in an integrated motion planning and prediction (MPP) system. Unless otherwise indicated, the perception, prediction planning and control terminology used herein does not imply any specific level of integration or modularity of those aspects. A planner used to plan ego actions might have integrated prediction logic or be supported by a modular prediction system (as in FIG. 1).

[0052] Particle filtering is used by the prediction component 104 to account for possibly occluded agents when planning and executing mobile robot control actions. Particle filtering is a known method of estimating a state based on successive observations. Particle filters are well suited to handling non-Gaussian distributions. Particles representing possible states are sampled, probability weighted based on observations, and resampled based on their assigned probability weights. This process continues iteratively to refine the state estimates as new observations are received.

[0053] In the present application of particle filtering, the visibility reasoning component 105 generates particles within determined occluded region(s) representing possible occluded agents that have associated probability weights. For example, for a sampled set of N particles there might exist e.g. {(particle 1, Pi) , (particle 2, P2) ... (particle n, Pn){ with P; being representative of the particle probability for each particle i. Weights can, for example, be assigned uniformly within an occluded region such that each particlewithin an occluded region has equal weights. Alternatively, the weights can be assigned non-uniformly to capture prior knowledge. For example, weights can also be assigned based on prior knowledge or beliefs in relation to the particles at an earlier time and / or prior using knowledge of an environment (such as a known road layout and expected driving behaviour).

[0054] Additionally, prior knowledge may be captured by sampling multiple particles to increase the concentration of particles in an area when the particle weights are assigned uniformly. This ensures that there is a higher probability of re-sampling particles from this location and that the particles at that location are able to quickly diversify to capture whether the agent may be accelerating or decelerating. The particles may represent an agent that enters an occluded region, referred to as enrichment particles in relation to figure 2. The number of particles that may be added for an agent that enters the occluded region may be tuned with the particle filter. The parameter may be a number that represents a fraction of the total N number of particles.

[0055] Figure 2 shows on the right hand side a flowchart for a mobile robot motion planning and prediction method, and on the left hand side a schematic illustration of each method step in an example planning scenario. A mobile robot 230 equipped with the runtime stack of FIG. 1 is shown. In this example, the mobile robot 230 has the form of an autonomous road vehicle in a driving scenario. The goal of the mobile robot is to join a road at a T-junction by executing a right turn manoeuvre, which will involve crossing a first lane 272 with oncoming traffic (travelling from east to west in this example) to join a second lane 274 (with traffic travelling from west to east). The mobile robot 230 is located at time t at the T-junction with a first agent 234 travelling along the second lane 274. In the first lane 272, there is an occluding object in the form of a second agent 232 occluding the field of view of the mobile robot 230.

[0056] At step 202, the perception system 102 generates an observed driving scene at a time t. This process involves detecting the first agent 234 and the second agent 232, and additionally determining an occluded region 240 caused by the second agent 232.

[0057] The first agent 234 is close to and travelling towards the occluded region 240 caused by the second agent 232 but is not occluded at time t. For simplicity, only occlusion by the second agent 232 is considered in the following examples. In practice, occlusion by multiple agents may be accounted for in the same manner. As such, an occluded regionmight be defined by one or multiple agents / objects. In some situations, multiple occluded regions at different locations may be considered. An example method for determining one or more occluded regions is described below with reference to FIG. 3

[0058] To account for the possibility of one or more unseen agents in the occluded region 240, in step 204, the visibility reasoning component 105 generates a set of N agent particles 250 at time t in the region 240 occluded by the object 232 on the roadway.

[0059] Note, time t in this context refers to the time at which the scene has been observed. The N particles 250 are representative of real agents that could exist within the occluded region 240.

[0060] To enable appropriate particle sampling, one or more relevant lane portions intersecting the occluded region 240 are identified. An occluded region might intersect multiple lanes. However, in some cases, only a subset of these will be relevant. An occluded lane portion is only considered relevant if the presence of one or more occluded agents in that lane portion could affect the planning goal. Appropriate assumption may be used to determine lane relevance (e.g. by assuming other agents follow applicable driving rules). Therefore, a determination of lane relevance can take into account the ego goal and known rules of the road. In this example, only the second lane 274 is determined to be relevant for visibility reasoning. In a more complex scenario, more than one lane might need to be considered. Possible occluded agents need only be considered in an area where they pose a realistic risk to the ego robot 230.

[0061] As will be seen in later steps, knowledge and belief relating to occluded regions is propagated forward in time by propagating particles forward in time using a form of particle filtering. This involves an initialization step to determine some initial step of particles. For simplicity, the following examples assume that, at time t, the ego robot 230 has no relevant prior knowledge or belief relating to the occluded region 240 propagated from earlier time steps. The sampling of the N particles at time t is performed to initialize the particle filtering.

[0062] It so happens that there are no occluded agents within the occluded region 240 at time t. However, this is not known to the ego agent 230 at time t, nor does it have specific knowledge or that propagated from earlier time steps. Therefore, the particle filtering is initialized based on the assumption that an occluded agent could be located at essentially any longitudinal position (position along a midline of the second lane 274) withinthe occluded region 240. An agent particle is considered to represent the centre of an agent, such that it is assumed that the agent should be visible if at least some predetermined portion of its volume is visible. The predetermined portion may be tuned to the perception system of the mobile robot. Hence, a uniform probability distribution over longitudinal position is generated along a length of the second lane 274 within the occluded region. For each of the N particles, a longitudinal position is sampled from this uniform distribution. Additionally, a speed is sampled for each particle. In some implementations, speed is sampled from a uniform distribution, e.g. between 0 and v max where v max is some assumed maximum speed, such as a known speed limit of the second lane 274. In other implementations, a non- uniform distribution over speed could be used that encodes prior knowledge about expected driver behaviour. In some implementations, speed and longitudinal position are the only quantities that are sampled, and each particle is assigned a lateral position of zero measured from the midline of the second lane 274 (meaning each sampled particle is located on the midline). In other implementations, a distribution over longitudinal position is additionally generated. For example, a gaussian distribution of lateral offset from the midline may be generated, e.g. with a mean of zero and a variance dependent on a width of the lane 274 (encoding a prior belief that agents are more likely to drive close to the midline). In any event, the agent particles are constrained to only exist on the drivable area of the roadway.

[0063] The number of particles N is sufficiently high to enable effective belief propagation based on particle filtering in the manner described below. N is an operating parameter that may be determined though routine experimentation. Depending on the context, N might be of the order of a few hundred particles.

[0064] As discussed, within the prediction system 104, sampled particles are treated similarly to as detected agents. However, not all sampled particles are passed to the prediction system 104, as running and evaluating N additional predictions would require significant computational resources with little or no benefit. Plannings typically involve some evaluation of different candidate plans (e.g. ego actions or trajectories) against each predicted agent trajectory. As a minimum, this typically requires an assessment of whether the candidate ego plan would result in a collision with any of the agents given their predicted trajectories. For example, in some planners, the complexity scales as M*P where M is the number of candidate ego plans and P is the number of predicted agent trajectories. An additional factor is the computational complexity of generating each predicted agent trajectory over the planning horizon, which may be significant particularly for a longer timehorizon. Large values of P would require significant computational resources and might not be feasible to evaluate in realtime. Therefore, instead a representative subset of particle states is passed to the prediction system 104 along with the detected agent states for prediction and planning purposes.

[0065] At step 206, a first subset of particles 252 is extracted from the N particles 250. The aim is to extract a subset of particles that is diverse in the sense of exhibiting a high spread whilst being representative of the variance within the N particles 250. Various methods can be used to extract a diverse subset. In one implementation, the subset 252 is extracted using a clustering algorithm. For example, the algorithm may extract the subset 252 by calculating the similarity between particles, based on properties such as pose, speed and acceleration. The algorithm may select one particle to be representative of each cluster of particles that fall within a set of similarity thresholds. The first subset can include a first agent particle or multiple agent particles. Other methods, such as distance maximization, could be used. Extracting an appropriately diverse particle subset for use in prediction and planning significantly reduces the computational expense with no or minimal impact on planner performance.

[0066] At step 208, the planner 108 determines an ego action to be executed. In the present example, the planner generates an ego plan in the form of an ego trajectory planned over the planning time horizon. To determine the ego trajectory, the prediction system 104 calculates predicted trajectories 254 for the subset of particles 252 as well as the detected agents 232, 234. Predictions are generated for particles based on their locations and motion states within the occluded region 240. These predictions 254 can be used by the planner of the mobile robot 230 to reason about collisions when generating control signals.

[0067] The predicted trajectories 254 may be a simple trajectory such that the agent particle is modelled to follow the constraints of the road in which it is travelling. For example, the agent particle may follow a trajectory directly along a midline of a lane at the speed limit. Alternatively, the prediction system 104 may calculate more complex trajectories taking into account a road layout or a goal to be achieved. For example, a predicted trajectory for an agent particle may be based on which roundabout exit the prediction system 104 considers to be the goal of the agent particle. Additionally, the prediction system 104 may determine that the goal of an agent particle is to leave the lane in which it is travelling by making a right turn, if feasible given the road layout.

[0068] Predictions may be generated for the detected agents 232, 234 using any suitable prediction method. One example of a suitable “inverse planning” prediction method is described in International Patent Publication No. W02020 / 079066, which generates agent trajectories on the assumption that agents plan rationally towards defined goals.

[0069] At step 210 the planner 106 passes the determined ego action to the controller 108, which generates a control signal causing the actor system 112 to execute the determined ego action. As discussed, the ego action passed to the controller 108 might correspond to only an initial portion of the planned ego trajectory.

[0070] The first mobile robot action is not the complete right turn manoeuvre. The full manoeuvre would typically involve multiple mobile robot control actions being executed at subsequent time steps until completion. Indeed, the action might be to remain stationary, if the planner 106 has determined it is unsafe to begin the right turn manoeuvre.

[0071] At step 212, the mobile robot 230 has executed the previously determined ego action and the scene has evolved in time. The observed scene is now at a second time, t+1 (as measured in discrete time steps). The perception system 102 generates an updated driving scene at a time t+1. This process involved determining a second occluded region 242 caused by the agent 232. In the updated scene, the occluded region 240 has changed as the position of the occluding object 232 in the field of view of the mobile robot 230 changed, creating the second occluded region 242. In the example of Figure 2, the occluding object is a dynamic agent and its position has also evolved in time such that it has moved along the roadway away from the mobile robot.

[0072] In one embodiment, occluded regions are treated independently for each time step such that the first occluded region is determined at the first time and the second occluded region is determined separately at the second time. The occluded regions are not tied to agents or other objects, rather they are considered by the perception system to be geometric shapes. The perception system outputs polygons to represent the occluded regions and an occupancy grid. The occupancy grid contains cells with values of 0 or 1 to indicate whether the region is occluded or not.

[0073] In other embodiments, the occluded regions are calculated geometrically using the agents and map features of a scenario. This is particularly useful in simulated scenarios. Alternatively, the occluded regions may be determined from a LiDAR point clouddirectly. This is described in more detail with reference to Figure 3.

[0074] The perception system 102 also determines that the first agent 234 that was visible in the driving scene at time t is no longer visible at time t+1. The first agent 234 is determined to be within the second occluded region 242.

[0075] At step 214, the agent 234 is represented by an enrichment particle or multiple enrichment particles 266 within the second occluded region 242. Enrichment particles 266 are populated using information from formerly detected objects. The location and motion state of an enrichment particle may be inferred based on the location and motion state of the agent when it was visible to the mobile robot.

[0076] Additional enrichment particles can be added to the occluded region 242 at the edges of the field of view of the mobile robot 230. The intersecting portion, or edge portion, of occluded region 242 can be considered to be the intersection of the occluded region 242 with the maximum perception range of the mobile robot. The additional enrichment particles may be added to an intersecting portion of the occluded region 242. The maximum agent perception range can be considered to be within the field of view of the mobile robot such that the perception system 102 of the mobile robot can perceive agents in a scene. This can be implemented to account for agents that are not within the field of view of the mobile robot or within the maximum perception range entering an occluded region. For example, the edge of the occluded region could intersect with a blind comer due to the position of the mobile robot and the structure of the road or the occluded region may extend to the maximum perception range of the mobile robot.

[0077] The N particles 250 generated at the first time, t, are propagated 270 to the second time, t+1 based on a particle motion model. It is important to note that it is the set of N particles 250 that are propagated and not the subset of particles 252 that were used for the predictions 254. Although the subset 252 can be considered sufficiently diverse for predicting trajectories, it cannot capture all of the variance within the N particles 250.

[0078] The propagation 270 of the N particles is performed over a shorter time frame compared to the predictions 254 in which the planner 106 plans over a longer time horizon. The propagation 270 of the particles is therefore less computationally expensive compared to the predictions.

[0079] The propagation 270 of each particle is based on the particle motion modelapplied to its motion state such that it represents an expected time evolution of the particles 250 from time t to t+1. For example, for particles given a choice of multiple onward lanes, the particle motion model makes a random choice according to a uniform distribution to ensure coverage. In this context, if an occluded region covered three lanes of a motorway travelling from west to east, the particles that are modelled to exist would uniformly be sampled across all lanes within a velocity range, travelling in the same direction. Alternatively, if an occluded region covered a roadway with a right lane (travelling from east to west in this example) and a left lane (travelling from west to east in this example), the particles would be uniformly sampled across both lanes, with inverse velocities travelling in opposite directions.

[0080] The N particles are propagated from time t to t+1 according to a motion model, e.g. of the form: xt+i=Ft+ixt +wt+i

[0081] Here, F represents a linear motion model, x represents the particle state and w represents noise. Other motion models can be used, e.g. constant acceleration etc.

[0082] At step 216, after propagating the N particles 250 from time t to t+1, the propagated particles are assigned occlusion weights 280 based on their locations relative to the second occluded region 242. The occlusion weights 280 for the propagated particles that are within the second occluded region 242 are non-zero. The non-zero weights can be uniformly assigned, meaning all particles within the occluded region are assigned the same occlusion weight. The particles that are now seen to exist outside of the second occluded region 242 are assigned zero weights or weights that tend to zero as distance from the occluded region increases, for example by using a sigmoid weighting function at the edge of the occluded region.

[0083] In this scenario, there is no prior knowledge of the particles before initial time t as only the occluded region is known. In this case, it is appropriate to weight them uniformly.

[0084] At step 218, the visibility reasoning component 105 resamples a set of N agent particles 250 in the region 242 occluded by the object 232 on the roadway. The knowledge and belief relating to the first occluded region 240 is propagated forward in time by propagating particles at step 215. The enrichment particles 266 are added to theresampled set of N particles at this stage.

[0085] From the N particles, and the enrichment particles 266, the visibility reasoning component 105 resamples a set of particles at time t+1 based on their associated probabilities (a particle with weight P has a probability of P of being sampled each time a sample is drawn). The same particle may be sampled more than once, resulting in duplicate particles. The probability increases with the particle probability weight. Random noise is applied to the sampled particles to vary the position and motion states, adding variation to the system. Gaussian distributions may be used to vary speed and lateral offset from the midline. For speed, this may have the effect of some particles accelerating or decelerating (with larger acceleration / deceleration values being less likely due to the normal distribution). For lateral offset, the distribution may be biased towards the mid-line, meaning over time this has a “decaying lateral offset” effect (i.e. when driving vehicles usually tend to return to the middle of the lane).

[0086] After propagation at time t+1, the weights or probabilities are updated according to whether a particle has remained within the occluded region.

[0087] After resampling, the weights of the set of remaining particles are set to be uniform across the particle set. This prevents particle degeneracy.

[0088] The position and density of the particles at the first time, t, in the first occluded region 240, is used to predict the position of the particles in the second occluded region 242 at time t+1.

[0089] The direction and extent of the propagation of the particles 270 has shifted the probability distribution of where the particles exist in the occluded region 242.

[0090] The density of the N particles in the occluded region 242 is proportional to the density of the particles at step 216, depending on their occlusion weight 280. In Figure 2, the particles 284 with a zero or almost zero occlusion weight have not been resampled in the set of N agent particles.

[0091] The occlusion weights 280 for the particles within the occluded region 242 are non-zero and uniformly assigned. Therefore, the N agent particles have been resampled uniformly according to the density and positions of particles with non-zero occlusion weights.

[0092] At step 220, a second subset 262 of diverse particles is extracted from the N agent particles resampled in step 218. As mentioned previously, the second subset 262 can contain a second agent particles or multiple agent particles.

[0093] At step 222, the planner 106 determines an ego action to be executed. In the present example, the planner generates an ego plan in the form of an ego trajectory planned over the planning time horizon. To determine the ego trajectory, the prediction system 104 calculates predicted trajectories 264 for the subset of particles 262 as well as the detected agents 232, 234. Predictions are generated for particles based on their locations and motion states within the second occluded region 242. These predictions 264 can be used by the planner of the mobile robot 230 to reason about collisions when generating control signals. As described above, predictions may be generated for the detected agents 232, 234 using any suitable prediction method.

[0094] At step 224, the planner 106 passes the determined ego action to the controller 108, which generates a control signal causing the actor system 112 to execute the determined ego action. As discussed, the ego action passed to the controller 108 might correspond to only an initial portion of the planned ego trajectory.

[0095] This process is then repeated iteratively for each time step until there is no occluded region or the manoeuvre is complete.

[0096] The occluding object 232 described with reference to Figure 2 could also be a static object in the environment, for example a stationary vehicle or roadside barrier. Alternatively, the occluding object 232 could be a dynamic agent, for example another vehicle, cyclist or pedestrian. There could also be multiple occluding objects, both static and stationary in any particular scenario. For example, there could be two moving vehicles and a tree as occluding objects in the field of view of a mobile robot in a scenario.

[0097] Figure 3 shows a scene 300 in which the mobile robot 230 determines a first occluded region 240 at a time t. The scene corresponds to the first scene of Figure 2 at step 202.

[0098] As described with reference to Figure 2, the mobile robot 230 is shown driving along the left lane of a road adjoining a main road at a T-junction. On the main road there are two agents, a first agent 232 in a roadway closest to the mobile robot 230 and asecond agent 234 in the left lane of a roadway furthest from the mobile robot 230. The first agent 232 is an occluding agent in this scenario as a portion of the roadway cannot be seen by the mobile robot 230 behind the agent 232.

[0099] It should be noted that only the ‘useful’ part of the road layout is considered when computing the occluded region 240, where the useful part includes only the lanes for which agents travelling in the lane could affect the actions of the mobile robot. It should also be noted that Figure 3 shows a representation of a scenario in a ‘real-world’ reference frame for illustration purposes only, and in practice different parts of the planner may use different reference frames. Depending on the scenario, the planner may, for example, use the real-world reference frame, an ego-centric reference frame or a reference-path frame.

[0100] To determine the occluded regions in real -world scenarios the perception system 102 uses sensor data such as LiDAR points in one embodiment. When a point is detected at a particular location, that implies an object exists in that location (light has reflected off that object to produce the LiDAR points). Therefore, a region behind this point is non-visible. Such region(s) are used to determine occlusion.

[0101] In other contexts, such as testing the stack in simulation, a different mechanism may be used to determine occluded regions. In simulation, the bounding boxes around agents and other map features, for example buildings, are used in combination with sensor configurations on the ego vehicle to determine the occluded regions. The occluded regions are provided by the perception component in the form of polygons and an occupancy grid. A first occluded region 240 of the road can be determined by the perception system 102 using a roadmap by projecting from the position of the sensors on board the mobile robot 230, e.g. a LiDAR sensor mounted on the mobile robot 230, along the outer corners of the occluding agent 232 to the road limits, defining the first occluded region 240. The outer corners of the occluding agent 232 are determined using bounding boxes. Bounding boxes denote the location and approximate extent of the objects detected in the image and are determined by the perception system 102.

[0102] The above description in relation to Figure 3 refers to example implementations in which occluding objects are treated as static. In real -world scenarios, however, both static and dynamic occluding objects may be present. Dynamic occluding agents, such as the agent 232 included in Figure 2, may be handled in the same way as described above for static objects at each timestep.

[0103] The above techniques present an occlusion-aware particle filter framework that enables a planner of an ego robot to reason about risk posed by occluded regions in driving scenarios without introducing overly conservative behaviour. The particle filter framework facilitates tracking potential unseen agents through occluded regions, such that it can be deduced if there is a realistic chance that an agent might exist in the region. The properties of the tracked particles can then be used to instantiate agents in the prediction system that the planner can use to reason about making a manoeuvre. Further, the particle filter can also be used to track previously seen agents through occluded regions.

[0104] The process begins at a time t, where the mobile robot has limited or no access to prior knowledge about the unseen agents. The limited knowledge that the mobile robot does have relates to limits on vehicle speed and the tendency that drivers have to drive close to the midline. The agents are also assumed to follow applicable driving rules.

[0105] The particle filtering method, described with reference to figure 2, allows the ego robot to safely infer that no additional agents have entered an occluded region that would affect the actions of the ego robot. The ego robot will execute the manoeuvre once it is determined that there are no particles remaining that pose a collision risk. This prevents overly conservative behaviour without compromising on safety.

[0106] References herein to components, functions, modules and the like, such as the components 102-108 of Figure 1, denote functional components of a computer system, which may be implemented at the hardware level in various ways. A computer system comprises execution hardware which may be configured to execute the method / algorithmic steps disclosed herein such as the steps of Figure 2. The term execution hardware encompasses any form / combination of hardware configured to execute the relevant method / algorithmic steps. The execution hardware may take the form of one or more processors, which may be programmable or non-programmable, or a combination of programmable and non-programmable hardware may be used. Examples of suitable programmable processors include general purpose processors based on an instruction set architecture, such as CPUs, GPUs / accelerator processors etc. Such general-purpose processors typically execute computer readable instructions held in memory coupled to or internal to the processor and carry out the relevant steps in accordance with those instructions. Other forms of programmable processors include field programmable gate arrays (FPGAs) having a circuit configuration programmable through circuit descriptioncode. Examples of non-programmable processors include application specific integrated circuits (ASICs). Code, instructions etc. may be stored as appropriate on transitory or non- transitory media (examples of the latter including solid state, magnetic and optical storage device(s) and the like).

Claims

Claims1. A computer-implemented method of planning actions for a mobile robot in the presence of occlusion, the method comprising: determining a first occluded region in a mobile robot field of view at a first time; sampling a first set of agent particles within the first occluded region; planning, using a robotic planner, based on a location and a motion state of a first agent particle of the first set of agent particles a first mobile robot action, the first mobile robot action accounting for predicted motion of the first agent particle; generating a first mobile robot control signal for causing execution of the first mobile robot action; determining a second occluded region in the mobile robot field of view at a second time; computing a propagated set of agent particles for the second time based on the first set of agent particles and a particle motion model; assigning an occlusion weight to each agent particle of the propagated set of agent particles with respect to the second occluded region; sampling a second set of agent particles within the second occluded region based on the propagated set of agent particles and the occlusion weight assigned to each agent particle; planning, using the robotic planner, based on a location and a motion state of a second agent particle of the second set of agent particles a second mobile robot action, the second mobile robot action accounting for predicted motion of the second agent particle; and generating a second mobile robot control signal for causing execution of the second mobile robot action.

2. The computer-implemented method of claim 1, comprising detecting an agent at a previous time preceding the first time step; determining that the agent has entered the first occluded region at the firsttime step; and sampling an enrichment particle in the first occluded region at the first time step corresponding to the agent detected at the previous time, wherein the first set of agent particles comprises the enrichment particle and the enrichment particle has an inferred location and motion state based on a detected location and motion state of the agent detected at the previous time.

3. The computer-implemented method of claim 2, comprising sampling a first plurality of enrichment particles with different location and / or motion states based on the location and motion state of the agent detected at the previous time.

4. The computer implemented method of claims 1 or 2, comprising detecting an intersecting portion of the first occluded region between the first occluded region and a maximum agent perception range of the field of view of the mobile robot; and sampling a second plurality of enrichment particles within the intersecting portion of the first occluded region.

5. The computer-implemented method of claims 2 to 4, comprising assigning the first set of agent particles, the first set of agent particles comprising the first and second plurality of enrichment particles, uniform occlusion weights.

6. The computer-implemented method of any preceding claim, wherein a first subset of agent particles is selected from the first set of agent particles, the first subset of agent particles comprising some but not all of the first set of agent particles, and the first agent particle is within the first subset of agent particles; and wherein the first mobile robot action is planned accounting for predicted motion of the first subset of agent particles.

7. The computer-implemented method of any preceding claim, wherein a second subset of agent particles is selected from the second set of agent particles, the second subset of agent particles comprising some but not all of the second set of agent particles, and the second agent particle is within the second subset of agent particles; and wherein the second mobile robot action is planned accounting for predicted motion of the second subset of agent particles.

8. The computer-implemented method of any of claims 5 to 7, comprisingapplying a clustering algorithm to sample the first subset of agent particles and the second subset of agent particles.

9. The computer-implemented method of any preceding claim, wherein the first set of agent particles and the second set of agent particles are each uniformly assigned occlusion weights.

10. The computer-implemented method of any preceding claim, wherein the occlusion weight assigned to any particle of the propagated set of agent particles located outside of the second occluded region is zero.

11. The computer-implemented method of claims 1 to 9, comprising applying a sigmoid weighting function to the propagated agent particles located outside the second occluded region, whereby the occlusion weight reduces as a sigmoid function of distance outside the second occluded region.

12. The computer-implemented method of any preceding claim, wherein the first occluded region and the second occluded region are determined using LIDAR measurements.

13. The computer implemented method of any of claims 1 to 11, wherein the first occluded region and the second occluded region are determined using a bounding box of an occluding object.

14. The computer-implemented method of any preceding claim, comprising determining the location and motion states of the first and second agent particles by sampling from a uniform distribution of longitudinal position within the first and second occluded regions respectively.

15. The computer-implemented method of claim 14, comprising determining the location and motion states of the first and second agent particles by assigning zero lateral offset from a midline of the first and second occluded regions.

16. The computer-implemented method of claim 14, comprising determining the location and motion states of the first and second agent particles by sampling from a non- uniform distribution of lateral offset from a midline of the first and second occluded regions.

17. The computer-implemented method of any preceding claim, comprising determining the location and motion states of the first and second agent particles bysampling from a uniform distribution of speed.

18. The computer-implemented method of claim 1, wherein the particle motion model randomly samples the location and motion states from a uniform distribution.

19. A computer program comprising executable instructions configured, when executed on one or more hardware processors, to implement the method of any preceding claim.

20. A computer system, comprising one or more hardware processors configured to implement the method of any of claims 1 to 18.

Images