Path planning in mobile robots

Abstract

A computer-implemented method of planning a path for a mobile robot in the presence of a set of obstacles comprises: computing for each obstacle a probabilistic obstacle position distribution; computing at least one path-independent function as a combination of the probabilistic obstacle position distributions; and for at least one candidate path, determining an upper bound on the total probability of obstacle collision along that path, by aggregating the path-independent function based on an area defined by the candidate path and a mobile robot shape, wherein the path-independent function is independent of the candidate path.

Claims

1. A computer-implemented method of planning a path for a mobile robot in a presence of a set of obstacles, the method comprising: computing for each obstacle a probabilistic obstacle position distribution; computing at least one path-independent function as a combination of the probabilistic obstacle position distributions; for at least one candidate path, determining an upper bound on a total probability of obstacle collision along that path, by aggregating the path-independent function based on an area defined by the candidate path and a mobile robot shape, wherein the path-independent function is independent of the candidate path; wherein the defined area is a surface of a spacetime tube defined by the candidate path and the mobile robot shape, and the path-independent function is integrated or otherwise aggregated over that surface; wherein the spacetime tube is determined as a set of spacetime voxels, wherein the path-independent function is determined by projecting faces of the voxels into a spacetime plane for each obstacle and integrating the probabilistic obstacle position distribution for that obstacle over each projection of the voxel face for that obstacle to compute a probability value for that voxel face, and wherein the path-independent function is aggregated over the surface of the spacetime tube by summing the probability values computed for the voxel faces; and based on the at least one candidate path, planning a path for the mobile robot and maneuvering the mobile robot along the planned path.

2. The method of claim 1, wherein: for each of a plurality of candidate paths, an upper bound on the total probability of obstacle collision along that path is determined, by aggregating the path-independent function based on an area defined by that candidate path and the mobile robot shape, wherein the upper bound is computed for each candidate path without recomputing the path-independent function; and planning the path comprises selecting, from the plurality of candidate paths based on the upper bounds calculated for each of the plurality of candidate paths, the path for navigating the mobile robot through the set of obstacles.

3. The method of claim 2, wherein all of the candidate paths are from a common start point to a common endpoint.

4. The method according to claim 3 wherein the upper bound or a risk of collision is provided as a safety parameter, and the method comprises receiving a threshold for safety and deselecting any of the candidate paths which do not satisfy the threshold for safety.

5. The method of claim 1, wherein the upper bound for the or each candidate path is an upper bound on the sum of individual obstacle collision probabilities along that path, but is computed without determining any of the individual obstacle collision probabilities, and instead by aggregating the path-independent function over the defined area for that path.

6. The method of claim 1, wherein the defined area over which the path-independent function is aggregated is a swept area A defined in two-dimensional space by the candidate path and the mobile robot shape.

7. The method of claim 6, wherein the path-independent function is determined as
G.sub.=.sub.kB.sub.k,*p.sub.k where B.sub.k, is a delta-function ridge along a bounding contour of a shape B.sub.k of the obstacle k and p.sub.k is the probabilistic obstacle position distribution for the obstacle; k and wherein the path-independent function G.sub.is aggregated as
A.sub.(r)G.sub.(r) where A.sub.o(x) approximates a delta-function ridge along a bounding contour of the swept area A; or
G=.sub.k.sub.kI.sub.B.sub.k*p.sub.k where, ${}_k=\frac{1}{\mathrm{area}\left(B_k\right)},$ I.sub.B.sub.k is an indicator function of a shape Bk of an obstacle k and p.sub.k is the probabilistic obstacle position distribution for the obstacle, k wherein G is aggregated as
I.sub.A(r)G(r) wherein I.sub.A is the indicator function for the swept area A.

8. The method of claim 1, wherein the spacetime tube is defined by the candidate path and an expanded mobile robot shape, the expanded mobile robot shape being defined by the mobile robot shape and an obstacle shape.

9. The method of claim 1, wherein the upper bound for each candidate path is an upper bound on the sum of individual obstacle collision probabilities along that path, but is computed without determining any of the individual obstacle collision probabilities, and instead by integrating the path-independent function over the defined area for that path.

10. A data processing system configured to plan a path for a mobile robot and comprising: a computer; and memory holding computer code which when executed by the computer carries out a method of planning a path for a mobile robot in a presence of a set of obstacles; the method comprising: computing for each obstacle a probabilistic obstacle position distribution; computing at least one path-independent function as a combination of the probabilistic obstacle position distributions; for each of a plurality of candidate paths, determining an upper bound on a total probability of obstacle collision along that path, by aggregating the path-independent function based on an area defined by the candidate path and a mobile robot shape, wherein the path-independent function is independent of the candidate path, and, wherein the upper bound is computed for each candidate path without recomputing the path-independent function, wherein the defined area is a surface of a spacetime tube defined by the candidate path and the mobile robot shape, and the path-independent function is integrated or otherwise aggregated over that surface, wherein the spacetime tube is determined as a set of spacetime voxels, wherein the path-independent function is determined by projecting faces of the voxels into a spacetime plane for each obstacle and integrating the probabilistic obstacle position distribution for that obstacle over each projection of the voxel face for that obstacle to compute a probability value for that voxel face, and wherein the path-independent function is aggregated over the surface of the spacetime tube by summing the probability values computed for the voxel faces; selecting, from the plurality of candidate paths based on the upper bounds calculated for each of the plurality of candidate paths, a path for navigating the mobile robot through the set of obstacles; and based on the path selected from the plurality of candidate paths, maneuvering the mobile robot along the path selected from the plurality of candidate paths.

11. A computer program product comprising instructions embodied in a non-transitory computer readable medium which, when executed by a computer, cause the computer to carry out a method of planning a path for a mobile robot in a presence of a set of obstacles, the method comprising: computing for each obstacle a probabilistic obstacle position distribution; computing at least one path-independent function as a combination of the probabilistic obstacle position distributions; for each of a plurality of candidate paths, determining an upper bound on a total probability of obstacle collision along that path, by aggregating the path-independent function based on an area defined by the candidate path and a mobile robot shape, wherein the path-independent function is independent of the candidate path, and, wherein the upper bound is computed for each candidate path without recomputing the path-independent function, wherein the defined area is a surface of a spacetime tube defined by the candidate path and the mobile robot shape, and the path-independent function is integrated or otherwise aggregated over that surface, determining projections, onto a second spacetime plane defined by a second time, of points or regions within a spacetime volume based on position information for each obstacle at a first time and a predicted set of obstacle trajectories, the first time defining a first spacetime plane separated from the second spacetime plane by the spacetime volume, wherein the spacetime tube has a side-surface lying within the spacetime volume and extending from the first spacetime plane to the second spacetime plane, wherein at least some of said points or regions lie on the side-surface of the spacetime tube, wherein the upper bound is determined, between the first and second times, based on the probabilistic obstacle position distribution for the obstacle, as applied to the projections of the side-surface points or regions; selecting, from the plurality of candidate paths based on the upper bounds calculated for each of the plurality of candidate paths, a path for navigating the mobile robot through the set of obstacles; and based on the path selected from the plurality of candidate paths, maneuvering the mobile robot along the path selected from the plurality of candidate paths.

12. The computer program product of claim 11, wherein projections of points or regions within the spacetime volume, onto the second spacetime plane, are determined for each of multiple obstacles based on the predicted set of obstacle trajectories and position information for that obstacle at the first time; and wherein a risk of collision is estimated for the candidate path based on a probabilistic obstacle position distribution p.sub.k for each obstacle k as applied to the projections of points or regions on the side-surface of the spacetime tube as determined for that obstacle, wherein a path-independent function is computed at each of the points or regions within the spacetime volume based on the probabilistic obstacle position distribution for each obstacle, as applied to the projection of that point or region determined for that obstacle.

13. The computer program product of claim 12, wherein multiple candidate paths are determined as corresponding spacetime tubes, each having a side-surface lying within the spacetime volume and extending from the first spacetime plane to the second spacetime plane, and a risk of collision is estimated for each candidate path, and wherein the risk of collision between the first and second times is computed for each of the candidate paths based on the path-independent function as computed at points or regions on the surface of the corresponding spacetime tube, wherein the path-independent function is independent of the candidate paths and the risk of collision is estimated for each candidate path without recomputing the path-independent function.

14. The computer program product of claim 13, wherein the risk of collision is estimated for the or each candidate path based on an integral of the path-independent function computed over the side-surface of the spacetime tube.

15. The computer program product of claim 14, wherein the path-independent function is a path-independent obstacle tensor field Fcomputed as: $F\left(r,t\right)=\underset{k}{.Math.}{F}_k\left(r,t\right),$ wherein F.sub.k(r, t) is a tensor component for obstacle k computed based on, p.sub.k(r.sub.k, T) i.e. the probabilistic obstacle position distribution p.sub.k for obstacle k as applied to the projection (r.sub.k, T) of the point (r, t) on the second spacetime plane, t=T Tbeing the second time.

16. The computer program product of claim 14, wherein the risk of collision is estimated based additionally on a second integral computed over a top-surface of the spacetime tube which lies within the second spacetime plane.

Description

BRIEF DESCRIPTION OF FIGURES

(1) For a better understanding of the present invention, and to show how embodiments of the same may be carried into effect, reference is made to the following Figures by way of non-limiting example in which:

(2) FIG. 1 is a representation based on data taken from an aerial view of a car-park illustrating the robot planning problem, with trajectories shown in shades of grey according to risk of collision.

(3) FIG. 2 is a diagram illustrating a Minkowski Sum showing a path with swept area A, dilated by Minkowski sum with obstacle of shape B.sub.K, to give the expanded swept area A.sub.K.

(4) FIG. 3 are graphs illustrating convolution and the Minkowski sum.

(5) FIG. 4 is a graphical representation of a contour convolution in which the indicator function of the Minkowski sum of sets A and B is approximated via contour convolution.

(6) FIGS. 5A and 5B illustrates visualisation of paths for two different simulated.

(7) FIG. 6 illustrates visualisation of paths in an environment with more obstacles.

(8) FIG. 7 is a graph comparing the complexity and performance of the FPR (Fast Path Risk) computation described herein of the bound F.sub.D on collision risk, using the car park data from FIG. 1, with a Laugier computation.

(9) FIG. 8 is a representation of data from an aerial view of the car-park with the vehicles represented as elliptical shapes.

(10) FIG. 9 is a plot of bounds on collision risk plotted for elliptical and rectangular bounding obstacle shapes in the car-park data of FIGS. 1 and 8.

(11) FIG. 10 shows a spacetime representation of a robot path.

(12) FIG. 11 shows a robot path determined as a spacetime tube within a spacetime volume.

(13) FIG. 12 illustrates certain principles of path projection in the context of a spacetime collision analysis.

(14) FIG. 13 shows an expanded spacetime tube.

(15) FIG. 14 illustrates certain principles of path projection for an expanded spacetime tube.

(16) FIG. 15 shows a plan view of a t=T spacetime plane into which an expanded spacetime tube has been projected.

(17) FIGS. 16A and 16B illustrates certain principles of path-independent accumulators in the context of a spacetime analysis.

(18) FIGS. 17-19 illustrate the principles of estimating collision risk in a tensor field approach.

(19) FIG. 20 compares different approximation of the function pos( . . . ).

(20) FIG. 21 shows a voxelized spacetime tube.

(21) FIGS. 22-24 illustrate principles of path projection in a voxel context.

(22) FIG. 25 is a schematic block diagram of an on-board control system for an AV.

(23) FIG. 26 is a flow chart of a path planning method.

(24) FIG. 27 is a visualisation of the motion planning problem for mobile robots.

DETAILED DESCRIPTION

(25) As mobile robots and autonomous vehicles become increasingly prevalent in human centred environments, there is a need to develop techniques which seek to guarantee collision free behaviour, or at least bound the risk. Perceptual modules which receive sensory input data provide only noisy estimates of objects and it is appropriate to represent the environment probabilistically. Embodiments of the present invention introduce methods of path planning which enable a number of candidate paths to be ranked according to a safety parameter. A Fast Path Risk brackets (FPR) algorithm is described herein which efficiently calculates a bound on the probability of collision which can be used to stratify candidate trajectories according to their degree of risk. Then, within the user-defined threshold for an acceptable level of primary risk, motion synthetic techniques might be utilised to optimise for secondary criteria such as comfort or efficiency.

(26) Embodiments of the present invention provide a number of different novel and useful aspects. One contribution is the use in the FPR algorithm of a convolution trick to factor integrals which are used to compute the bound on the collision risk. As a result, given K obstacles and N candidate paths, the computational load is reduced to O (N+K) compared with conventional techniques at O (NK). According to another aspect, candidate paths may be ranked in order of safety based on the upper bound on the total probability of at least one collision between the robot and K obstacles.

(27) According to another aspect, candidate paths may be filtered (e.g. deselected) on the basis of a user-defined threshold, and then a path selected based on other parameters (for example comfort and efficiency) from the filtered (non-deselected) set.

(28) Example embodiments of the present invention are described in detail below. First some useful context to the invention is described.

(29) FIG. 25 shows a highly schematic functional block diagram of certain functional components embodied in an on-board computer system A1 of an AV (ego vehicle), namely a data processing component A2, simulation component A5 and an AV planner A6. The AV is given as a non-limiting example of a mobile robot.

(30) The data processing component A2 receives sensory input in the form of sensor data from an on-board sensor system A8 of the AV. The on-board sensor system A8 can take different forms but generally comprises a variety of sensors such as image capture devices (cameras), LiDAR units etc., 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 other actors (vehicles, pedestrians etc.) within that environment.

(31) Note however that the present-techniques are not limited to using image data and the like captured using on-board optical sensors (image capture devices, LiDAR etc.) of the AV itself. The method can alternatively or additionally be applied using externally-captured sensor data, for example CCTV images etc. captured by external image capture units in the vicinity of the AV. In that case, at least some of the sensor inputs used to implement the method may be received by the AV from external sensor data sources via one or more wireless communication links. Moreover, the techniques described herein can be implemented off-board, that is in a computer system such as a simulator which is to execute path planning for modelling or experimental purposes. In that case, the sensory data may be taken from computer programs running as part of a simulation stack. In either context, a perception module A17 may operate on the sensor data to identify objects, as part of the data processing system A2.

(32) The data processing system A2 processes the sensor data in order to extract such information therefrom. This will generally involve various forms of machine learning (ML)/artificial intelligence (AI) processing. Functions of the data processing system A2 that are relevant in the present context include localization (block A10) and object detection (block A12). Localization is performed to provide awareness of the surrounding environment and the AV's location within it. A variety of localization techniques may be used to this end, including visual and map-based localization. By way of example, reference is made to United Kingdom patent Application No. 1812658.1 entitled Vehicle Localization, which is incorporated herein by reference in its entirety.

(33) Object detection is applied to the sensor data to detect and localize external objects within the environment such as vehicles, pedestrians and other external actors which may contribute obstacles to the AV. This may for example comprise a form of 3D bounding box detection, wherein a location, orientation and size of objects within the environment and/or relative to the ego vehicle is estimated. This can for example be applied to (3D) image data such as RGBD (red green blue depth), LiDAR point cloud etc. This allows the location and other physical properties of such external actors to be determined on the map. Object detection may be carried out using a perceptual model in a perception module A17. In an on-board context, the perceptual model utilises the received sensor data. In an off-board context the perceptual model may utilise data derived from actual or simulated sensors, pre-stored or generated by a computer program. Perceptual models and their limitations are discussed later.

(34) The AV planner A6 uses the extracted information about the ego's surrounding environment and the external agents within it as a basis for planning path for the AV. The AV planner A6 makes various high-level decisions and then increasingly lower-level decisions that are needed to implement the higher-level decisions. The end result is a series of real-time, low level action decisions. In order to implement those decisions, the AV planner A6 generates control signals, which are input, at least in part, to a drive mechanism A16 of the AV, in order to control the speed and heading of the vehicle (e.g. though steering, breaking, accelerating, changing gear) etc. Control signals are also generated to execute secondary actions such as signalling.

(35) Similar control signals may be generated in an off-board simulators and supplied to computer programs implementing models of other parts of the AV.

(36) A scenario extraction component A3 determines an encountered driving scenario for the ego vehicle using outputs of the data processing component A2. The determined driving scenario comprises driving scenario parameters which are extracted from the captured sensor data, and which provide a representation of a real-world scenario that has been encountered by the AV that is concise but sufficiently detailed to be used as a basis for realistic simulations.

(37) The simulator A5 receives the parameters of the encountered driving scenario and can run simulations based on those parameters. These are simulations of what might happen in the encountered driving scenario under different assumptions. These simulations are used as a basis for AV planning, in which the AV planner A6 runs multiple simulations with the aim of determining a globally optimal sequence of manoeuvres to be taken in the encountered driving scenario to execute a defined goal (i.e. achieve a desired outcome, such as reaching a particular location on the map). In one example described in our Patent Application No. 1816853.4, the contents of which are incorporated by reference, the simulations are run as part of a Monte Carlo Tree Search (MTCS) executed by a manoeuvre selection component A7 of the AV planner A6.

(38) When the ego vehicle is travelling, it must plan which manoeuvres to perform in order to execute a defined goal.

(39) When the ego vehicle encounters an actual driving scenario it needs to navigate, the planning module A6 reasons systematically about the different possible outcomes of different ego manoeuvres (i.e. manoeuvres which the AV planner A6 of the ego vehicle might take) in the encountered driving scenario. For example a manoeuvre may determine that the AV needs to travel from its present location (starting point SP) to a certain endpoint EP. The determined manoeuvre is supplied to the simulator A5 which generates a number of candidate paths as will be described, each associated with a computed bound on collision risk. The candidate paths may be returned to the planner A6 to select a suitable path to execute.

(40) For example, the paths can be synthesised for each goal using a Rapidly Exploring Random Tree (RRT) model. A space of predicted paths (search space) is defined based on the reference location for each goal and the current location r.sub.0 of the external vehicle. The search space is then randomly sampled (based on randomized input parameters), in order to determine the set of n paths, and a likelihood of each of those paths. To simulate n paths for each goal, the relevant parameters of the RRT are randomised n times to perform n appropriately based random searches of the search space. In the present disclosure, the probabilistic risk of collision along a given trajectory may be calculated, and used to rank the candidate trajectories by safety. This in turn provides the likelihood of each sampled path on the assumption that the external vehicle is more likely to take safer paths to execute the goal in question.

(41) The approach described herein is based on probabilistic robotics, wherein environmental uncertainty is posed probabilistically in the form of occupancy grids. However, the present disclosure seeks to relax several assumptions. Firstly, the fact that collision probabilities are not independent from cell to cell in an occupancy grid representation is taken into account. Instead, the probability of collision at one cell is directly coupled to that at a nearby cell, via the latent variable of true object pose. Secondly, the grid representation is reconsidered and an alternative computational approach is adopted that allows paths and object shapes to be modelled in a continuous fashion, deferring approximations to a much later stage in the procedure.

(42) A key contribution of the techniques described herein is a faster procedure for calculating integrals associated with the calculation of collision risk. This is achieved through the use of convolutions over boundary shapes. Given the shapes and uncertain location of obstacles in an environment, the problem is to estimate the risk of collision from taking a sequence of actions (i.e., traversing along a candidate path), given a description of the objects in the environment for a set of hypothetical paths. This risk can be posed in terms of the probability distribution for environment by the obstacles on the path swept by a moving robot during a specified time interval along that path.

(43) FIG. 27 is a visualisation of the motion planning problem faced by a robot AV. The three curves represent possible paths C1, C2, C3 that the robot could traverse. The environment is represented by the two white cars O.sub.1, O.sub.2 whose position is known only up to probabilities, depicted by the distribution (over the centroids). The outcome of the computations is an assignment of risk of collision to each curve which defines a rank ordering (here C1 is most preferred and C3 is least preferred).

(44) Both tethered and dynamic obstacle approaches are disclosed herein.

(45) 1. Tethered Obstacles

(46) The details of the tethered obstacle method will now be described approach will now be described.

(47) In the present context it is assumed that freespace in the plane is defined deterministically, and that a robot of known shape translates and rotates in freespace. In addition, discrete tethered obstacles k=1 . . . , K of known shape and uncertain location are introduced. The problem is then, over a (short) time-interval t[0, . . . , T] to:

(48) Generate N hypothetical paths in configuration space for the robot.

(49) Bound the probabilistic risk of collision, for each path: a bound Fr) is computed on the risk of collision for each path. The FPR algorithm introduced here computes a bound F.sub.T on risk of collision, which is then available for use in motion synthesis. Its computational complexity is O(N+K), compared with the O(NK) of earlier techniques.

(50) The following inputs to the risk computation are assumed:

(51) Freespace: it is assumed that freespace F is defined to be a subset of .sup.2. Then within F, obstacles are defined stochastically as below.

(52) Robot: assumed to have a deterministic spatial extent and to be manoeuvrable in translation and rotation. Over the interval t[0, . . . , T], it sweeps out the set A in the plane of r=(x, y).

(53) Obstacle shape: the k.sup.th obstacle is assumed to have deterministic shape and mean orientation represented by the set B.sub.K.

(54) Tethered obstacle: tethering here means specifying a probability distribution pk(r) for the location of the k.sup.th obstacle, taking into account: i) variability arising from any possible motion over the time-interval [0, . . . , T]; and ii) modelled uncertainty in object detector output. Obstacle locations are assumed to be mutually independentarising from independent sensor observations.

(55) Obstacle rotation: treated as part of its shape so, to the extent that an obstacle may rotate over the time interval [0, . . . , T], that rotation must be absorbed in a deterministic expansion of the set B.sub.K.

(56) This treatment of obstacle rotation is a limitation of the framework described herein which, however, is reasonable if the time interval [0, . . . , T] is short, so rotation is limited.

(57) The Probabilistic Obstacle Framework: Laugier's Model

(58) Consider an environment consisting of a set of K obstacles. These obstacles are typically detected by perceptual modules whose outputs are uncertain, hence we treat them as probabilistic. The position of each obstacle centre (or suitable canonical point, with respect to which all geometric computations can be reframed) is a random variable given by the density function P.sub.K(r), where r=(x, y)E.sup.2

(59) The probability of collision between the robot and the kth obstacle (which we will sometimes write as obstacle k), if that obstacle were a point obstacle, could be written as
P.sub.D(k)=.sub.Ap.sub.k(r)(Equation 1)
where A is the swept area of the robot, along a path and over time t[0.sub.1, T.sub.2].

(60) Now the total probability of collision is computed using the techniques of the Laugier et al paper reference earlier as

(61) $P_D=1-\underset{k=1}{\overset{k}{.Math.}}\left(1-P_D\left(k\right)\right).$
which must be recomputed for each swept path A of N hypothesised paths. However, when P.sub.D<<1, as expected in practical, relatively safe situations, P.sub.D can be bounded by a close approximation, P.sub.DF.sub.D where
F.sub.D=.sub.k=1.sup.Kp.sub.D(k)(Equation 3)

(62) The inventors have perceived that the computation of F.sub.D in equation 3 can be made more efficient, i.e., of order O (1), by factoring it as:
F.sub.DI.sub.A(r)G(r) where G=.sub.k=1.sup.Kp.sub.k(r)(Equation 4)
where I.sub.A denotes the indicator function for points belonging to the set A. The computation is O (N+K), not O (NK) since G can be precomputed and re-used for all hypothesised paths A. Most obstacles are not mere points. When an obstacle has a shape, B.sub.k, situated at the origin, then at position r the displaced object would be described as
B.sub.k(r)=B.sub.k+r={r+r,r}(Equation 5).

(63) So, the probability of collision with the obstacle may be rewritten as
P.sub.D(k)=.sub.A.sub.kp.sub.k(r)(Equation 6)
where A.sub.k=AB.sub.k, the Minkowski sum of the robot shape and the obstacle shape, as in FIG. 2.

(64) In prior search based planning algorithms, the Minkowski sums for A.sub.K must be recomputed for each new path A that is conjectured, and for every obstacle, B.sub.K. This becomes inefficient when K, the number of obstacles, is large. In the present disclosure, there is presented a way to replace these repeated O (K) computations by an O (1) computation.

(65) Minkowski Sum and the Convolution Trick

(66) Note that the integral in Equation 6 can be rewritten as the convolution of two functions, evaluated at the origin,
P.sub.D(k)=[I.sub.A.sub.k*{tilde over (p)}.sub.k(r)](0)(Equation 7)
where f.sup. denotes the reflection of a function, i.e., f.sup.(r)=f(r), and where the notation [ . . . ](r) means that the function defined in square brackets is evaluated at the location r. (So in this instance (7), the function in square brackets is a convolution of two functions, and is applied at the origin r=0.)

(67) The inventors have noted that a connection between the Minkowski sum of two sets, and the convolution operator * applied to the indicator functions of the two sets may be used to advantage in this context. The connection is laid out in Equation 8.
Z=XY.Math.Z=supp(I.sub.X*.sub.Y)(Equation 8)
where supp(f) is the support of the function f. In particular,
A.sub.k=supp(I.sub.A*I.sub.B.sub.k)(Equation 9)

(68) In general, it need not be the case that the indicator of a Minkowski sum is simply (proportional to) the convolution of the two indicator functions (see FIG. 3), but over a restricted portion of the planecorresponding to the case when the obstacle B.sub.K lies inside the robot path A, the following equality holds,
I.sub.A.sub.k(r)=.sub.k[I.sub.A*.sub.B.sub.k](r), when B.sub.k(r)A(Equation 10)
where

(69) $=\frac{1}{\mathrm{area}\left(B_k\right)}.$
The expression on the right of this equation is everywhere positive as it is a convolution of indicator functions which are positive.

(70) This provides a formula for calculating I.sub.A.sub.k when B.sub.k(r)A. The following sets out a corresponding calculation for the case when B.sub.k(r)A.

(71) Using Contours to Bound the Integral.

(72) The complementary component of the Minkowski sum, I.sub.A.sub.k can be bounded using a convolution of using the bounding contours of the obstacles, B.sub.K, and of the robot, A.sub.K, to obtain an upper bound on any integral of the form .sub.A.sub.kf(r), and in particular on the collision probability in Equation 2.

(73) Given the set A, the delta function ridge around its boundary A is calculated as,
A.sub.(r)=|g.sub.(r)*I.sub.A(r)|(Equation 11)
where g.sub.(r) is a normalised isotropic 2D Gaussian function with a (small) diameter . Similarly, B.sub.(k,)(r) is defined as the delta function ridge around B.sub.K. Now, the indicator function for the Minkowski sum is bounded in the complementary condition, and in the limit that .fwdarw.0, by the convolution of the two delta function ridge functions as follows,
I.sub.A.sub.k(r)[A.sub.*](r), when B.sub.k(r)A(Equation 12)

(74) This is illustrated in FIG. 4 which is an illustration in 1D of the fact that the indicator of a Minkowski sum is not generally equal to the convolution of the two indicators, but they do share the same support.

(75) As with equation 10, the right hand side of the inequalities in 12 is everywhere positive. So, now the complementary expressions in Equations 10 and 12 can be combined into a single bound on the indicator function of the Minkowski sum,
I.sub.A.sub.k(r)[A.sub.*](r)+.sub.k[I.sub.A*.sub.B.sub.k](r)(Equation 13)
As this bounds holds everywhere, we may write,
I.sub.A.sub.k[A.sub.*]+.sub.k[I.sub.A*.sub.B.sub.k](Equation 14)
Returning to the earlier expression for the collision probability, in equation 2,
P.sub.D(k)[[[A.sub.*]+.sub.k[I.sub.A*.sub.B.sub.k]]*{tilde over (p)}.sub.k](0)(Equation 15)
Using the associativity of the convolution operator, this may be rewritten as,
P.sub.D(k)[[A.sub.**{tilde over (p)}.sub.k](0)+.sub.k[I.sub.A*.sub.B.sub.k*{tilde over (p)}.sub.k](0)](Equation 16)
Finally,
P.sub.D(k)A.sub.[B.sub.k,*p.sub.k]+I.sub.A[.sub.kI.sub.B.sub.k*p.sub.k](Equation 17)
Summing up over obstacles, the bound on number of collisions is,
F.sub.DA.sub.(r)G.sub.(r)+I.sub.A(r)G(r)(Equation 18)
where G.sub.=.sub.kB.sub.k,*p.sub.k and G=.sub.k.sub.kI.sub.B.sub.k*p.sub.k.

(76) Note that G and G.sub. are independent of A and do not need to be recomputed every time A changes. So, the repeated computation of the bound (Equation 18) for N different swept paths A would indeed have complexity be O(N+K). That is, the path computations are independent of the number of obstaclesonce the initial calculations of G and G_ have been carried out. The above convolution test gives a Fast Path Risk (FPR) method to calculate the bound on the number of collisions, summarised in Algorithm 1. (Shown in Annexe A).

(77) Proof of the Contour Convolution Trick:

(78) That the indicator function for the Minkowski sum (in the limit that .fwdarw.0), in the inequality of Equation 10, is indeed bounded can be shown as follows
I.sub.A.sub.k(x)[A.sub.*](x)(Equation 19)
In the case that B.sub.K(x)A=, both sides of equation 17 are 0. Elsewhere, and given that B.sub.K(x)A, the contours of A and B.sub.K must intersect at least twice. In that case,
[A.sub.*](x)=.sub.xA.sub.(x)B.sub.k,(xx)(Equation 20)
integrates across two or more contour intersections, and the integral at each intersection (contours crossing at an angle ) has the form,

(79) $\begin{array}{cc}{g}_{}\left(x\right)g_{}\left(x\cos +y\sin \right)\mathrm{dxdy}=\frac{1}{\sin }1& \left(\mathrm{Equation}21\right)\end{array}$

(80) In the limit when .fwdarw.0, since the indicator function I.sub.AK (x) has value 1 whenever the contours intersect, the formula 17 does indeed hold.

(81) FIG. 26 is a flow chart illustrating a planning method comprising various aspects described herein. At step S10 a manoeuvre is received from the planner A6. Multiple paths are generated to avoid K obstacles at step S11 eg. by an RRT algorithm. Factors GG and G are calculated across all obstacles. Although these are shown in order S12, S13, it will readily be appreciated that they could be calculated in a different order, or indeed simultaneously (as they are not interdependent). At step S14 the upper bound F.sub.D is computed for each path using equation 18. In each computation for each path, the same value of G.sub. or G is utilised for the obstacles which have been detected for this set of candidate paths. At step S15 the paths are ranked in order of the computed upper bound. The ranked paths may be returned from the simulator to the planner. A user-defined threshold is received at step S16. This user-defined threshold may be a threshold on an acceptable level of primary risk, and may therefore set a certain value of F.sub.D below which paths are considered not safe enough. At step S17 a set of ranked paths is selected which satisfy the user-defined threshold. At step S18 on operating path is chosen from the selected set of ranked paths, using a path selection parameter, for example comfort or efficiency.

(82) The ability to receive a user-defined threshold on primary risk opens up many useful possibilities. An externally defined threshold of risk may be applied and candidate paths (across all obstacles) which do not satisfy that level of risk may be deselected. This enables for example a regulatory or insurance body to apply a threshold of safety, and the techniques described herein ensure that that can be met in the path planning. It further enables candidate paths which have not been deselected to remain available such that other path selection parameters can be utilised, for example comfort or efficiency as mentioned. This provides a convenient way of handling the potential difficulty of optimising both for a primary risk (collisions) and other nevertheless important factors. This has a number of possible applications.

(83) For example, on-board sensors are responsible for detecting the environment and providing sensor data which is supplied to object detection algorithms. Object detection or recognition may be hampered by a number of aspects for example, it is possible that accuracy of recognition may differ between different object classes. Put simply, some objects may be more reliably detected than others. The present approach enables an operating path to be selected from candidate paths which achieves a minimum level of safety for all object recognition classes regardless of their accuracy. It may be that in path planning the robot can avoid collisions with obstacles in certain object classes at a higher level of safety (due to increased accuracy of detection for example), but at least a minimum level of safety is guaranteed across all object classes.

(84) Shape models for obstacles have been mentioned earlier. One way of determining the shape of an obstacle is to access a shape model for that kind of obstacle. For example, if the obstacle has been identified as a vehicle, a shape model may be a rectangle or ellipse of certain dimensions. According to another example, pedestrians (and other small moving objects) may be modelled as zero dimensional shapes. Note that this does not mean they become invisible with respect to the planning, but are mathematically treated as a zero dimensional shape. In effect, they have been reduced to point obstacles.

(85) Another way of determining the shape would be to use the object recognition algorithms to actually determine a particular shape for each obstacle. This is likely to be more complex but the techniques described herein could also be used in such a circumstance.

(86) Experiments have been carried out to demonstrate that sets of randomly generated trajectories, for a particular environment configuration, can be efficiently labelled by the FPR algorithm, according to the bound F.sub.D on the risk of collision for each path. The FPR algorithm is agnostic as to the motion planner used to synthesise the candidate trajectories and is generally compatible with state of the art methods for motion planning (as outlined for example in C. Katrakazas, M. Quddus, W.-H. Chen, and L. Deka, Real-time motion planning methods for autonomous on-road driving, Transportation Rsch Part C: Emerging Technologies, 60, 416-442, 2015. A Closed Loop variant CL-RRT of the RRT algorithm is used to generate hypothetical paths in the environment, draw from the kinodynamicmodel for a particular robot. See for example Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How, Real-time motion planning with applications to autonomous urban driving, IEEE Trans. Control Systems Tech., 17, 5, 1105-1118, 2009 and S. M. LaValle, Rapidly-exploring random trees: A new tool for path planning, Tech Rep., 1998. This has the advantage of generating typically smooth paths, that are plausible paths for that robot. Then the risk bound is calculated for each generated path.

(87) First the results demonstrate the FPR algorithm for three simulated environments, then for a real environment taken from an aerial view. In each case, higher risk paths take tighter lines around obstacles, as would be expected. The experiments show: i) how close the bound on risk is to the true risk; and ii) that the use in FPR of the convolution trick, which improves computational complexity from O(NK) to O(N+K) as explained earlier, leads to substantial reductions in practical computation times.

(88) Simulated Environments

(89) In the experiments, a 2D simulation is used in which a rectangular robot of size 2 m4 m navigates along continuous paths, defined as the progression of full pose over time (though visualisations only depict the centroid). A simulation scenario is defined as a collection of obstacles within the environment, each specified as a shape (a subset of 2), and pose, together with positional uncertainty, as well as start and target poses for the ego vehicle.

(90) The three simulated scenarios used here resemble sections of a car park: two each with just two car as obstacles (FIGS. 5A and 5B), and a third with a more complex arrangement (FIG. 6). In each case, 400 paths are generated at random by CL-RRT. The uncertainty over each obstacle's position is modelled as a two-dimensional Gaussian distribution with standard deviation 30 cm, which is 15% of each obstacle's width. FIGS. 5A and 5B show paths for two different simulated environments. A set of candidate paths are generated with fixed start SP and end poses EP. The dark objects are obstacles, O1. O2 etc. whose position is known with an uncertainty visualised here as a shaded halo. Risk bounds F.sub.D on each path are represented by different shades of grey, on the logarithmic scale S2 shown. Safer paths maintain greater clearance around obstacles as expected. Some risky paths involve squeezing through a narrow gap. In the second scenario (FIG. 5B), the robot AV is able to squeeze through a narrow gap, but at some considerable risk of collision. In FIG. 6, a more complex environment is shown with more obstacles O.sub.i, and again higher values of the bound F.sub.D on the risk of collision correspond to tighter clearances.

(91) FIG. 6 shows paths with varying values of F.sub.D, with tighter clearances when the bound F.sub.D on collision risk is high. Risk bounds are shown on same scale as FIG. 5.

(92) Real Aerial Data

(93) In FIG. 1 an aerial view of a car park is shown, with a set of candidate paths for the robot AV generated by CL-RRT, between fixed start and end points. Obstacle vehicle shapes O.sub.i are represented as bounding rectangles. The obstacles here are 35 cars with shapes given by the bounding boxes shown in a fixed line, and uncertainties in location visualised as a slightly darker fuzzy halo around each bounding box. The robot vehicle AV has a set of candidate paths, evaluated here as high risk (dark grey) to low risk (white) according to the scale shown. Bounds on collision risk are represented by different shades of grey as on the logarithmic scale S1 shown. Some higher risk paths involve squeezing through a narrow gap. Error in estimated position of the obstacle-cars is Gaussian with standard deviation of 0.3 m. The candidate paths are shown in different shades of grey according to the computed value of the bound on collision risk. This turns out to include safer paths with collision risk down to 10.sup.5 and below, and riskier paths, above 10.sup.2 risk of collision, that involve squeezing through a narrow gap.

(94) Performance Evaluation

(95) Experimental results given here illustrate the computational benefits of the FPR approach for evaluating bounds on risk. In FIG. 7, empirical data is presented regarding the computational complexity of the present method compared to the baseline of computing the integral in Equation 6) directly. FIG. 7 shows that computing the FPR bound on risk is significantly faster than exact computation via Laugier integrals. This is true in absolute terms, thanks to the use of efficiently implemented convolution, in place of Minkowski sum. It is also true in complexity, giving almost constant computation time with respect to the number K of obstacles, cf. linear growth. In absolute terms, even for one obstacle, average runtime is 9 times faster for FPR, owing to the use of efficiently implemented convolution in place of Minkowski sum. Then for increasing numbers of obstacles, runtime becomes almost constant with the number of obstacles at 9.4 mc/path, cf. linear growth for direct computation of the Laugier integrals (6) for each obstacle. This is consistent with O(N+K) complexity c.f O(NK) complexity (for N evaluated paths), as expected from the theory outlined above.

(96) A consideration of how tight is the bound on collision risk.

(97) The FPR algorithm computes the bound F.sub.D on the exact risk P.sub.D computed as in (Equation 2) and (6). The slack in the bound is evaluated in several situations to see how close the bound is in practice.

(98) Consider the simplest interesting situation with one square robot and an identical square obstacle, at various relative orientations and distances (from 0.75 of a square diameter up to 1.75 diameters), and where p1(r) is Gaussian with s.d. of 0.25 of a square diameter.

(99) Statistics are averaged over all the paths in the car-park data, representing the car-obstacle shapes B.sub.k as rectangles.

(100) The rectangle shapes above are replaced with bounding ellipses, as in FIG. 8. Bounds F.sub.D on collision risk are calculated here using elliptical rather than rectangular bounding shapes for the obstacles. This produces tighter bounds, so risk values F.sub.D are generally lower than for FIG. 1.

(101) For the two squares, the bound is quite tight, ceding a factor of just 1.9 in risk on average. For the car park, the bounds are rather loose, with a factor of 8.9 of slack on average. When ellipses are used in place of rectangles, the slack is tightened to a factor of 4.7 on average. These slack factors occur more or less uniformly at high and low risk, as FIG. 9 shows. In FIG. 9, bounds F.sub.D on collision risk are plotted for elliptical and rectangular bounding obstacle shapes. Risk bounds have, on average, a factor of up to an order of magnitude slack for rectangular shapes, and that is true both at low and high risk. Bounds are tighter for elliptical obstacle shapes. One reason on why the the bound is slacker for rectangles than ellipses is because configurations in which the robot rectangle is almost tangential to an obstacle rectangle occur frequently, causing a spike in the contour-convolution (see FIG. 4). This effect is abated when the bounding obstacle shapes are ellipses, since extended tangency is avoided.

(102) Implementation Details

(103) As described above, an augmented version of the CL-RRT planner, with probabilistic sampling is used in the above examples, similar to other approaches for heuristically biasing RRT growth as described for example in C. Urmson and R. Simmons, Approaches for heuristically biasing rrt growth, in IEEE Int. Conf. IROS, 2, 1178-1183, 2003. Tree nodes are chosen for expansion according to their scores. It discretises steering and accelerator input ranges when expanding the random tree, to generate realisable trajectories, and in order to restrict abrupt steering or velocity changes. Nodes in the RRT are scored based on their proximity to the goal, and similarity to its orientation and velocity. The k.sup.th tethered obstacle is made deterministic, for the purposes of CL-RRT, taking the shape Bk at the mean location over pk. Throughout the experiments freespace is discretized on a grid with a resolution of 5 cm/px. The constant for Gaussian convolution, =1 grid square, is just big enough for good numerical behaviour. All of the numerical computations are implemented using the GPU-enabled Python module for linear algebra CuPy.

(104) The approach described herein to safe motion planning bounds the risk of collision for candidate paths in configuration space. An acceptably safe path is then taken to be one that satisfies F.sub.D0, for some user-supplied threshold on risk. The FPR algorithm builds on a probabilistic framework for calculating collision risk, and uses the convolution trick to render these computations in linear time in N and K. Amongst trajectories deemed safe enough, there is then freedom to optimise for other criteria such as rider comfort and travel time.

(105) Returning to FIG. 23, there is additionally shown an object tracking block A14 within the data processing system A2. Object tracking is used to track any movement of detected objects within the environment. The result is an observed trace () of each object that is determined over time by way of the object tracking. The observed trace is a history of the moving object, which captures the path of the moving object over time, and may also capture other information such as the object's historic speed, acceleration etc. at different points in time.

(106) 2. Dynamic Obstacles

(107) The tethered obstacle method described above is applicable to stationary obstacles but also moving obstacles in certain contexts. As noted, in the tethered obstacle method, any obstacle motion is accounted for in the level of uncertainty ascribed to each obstacle's location within the xy-plane, and the area of the xy-plane over which the motion planning is performed constitutes a collision space for that method.

(108) However, the probability density function p.sub.k(r) is not time-variable: the fact that the probability of a moving obstacle being at a given location may be different at different times is not explicitly accounted for. This is a reasonable framework in certain practice contexts, particularly where the time interval [0, . . . , T] over which the analysis is performed is relatively short, such that each obstacle exhibits at most limited motion over that time period.

(109) However, for longer time periods, a spacetime motion planning approach may be preferable, in which motion planning is performed in three-dimensional (3D) spacetime (xyt-space). That is, 3D space having two spatial dimensions (x, y) and one dimension of time t. This may be referred to herein as a dynamic obstacle method as it can account for a greater range of obstacle motion though explicit time-dependency. In this context, the collision space is a volume in 3D spacetime.

(110) FIG. 10 shows a schematic illustration of a spacetime coordinate system, in which motion of a mobile robot in the xy-plane over a time interval [0, T] is represented. The robot is is in the form of an autonomous vehicle (AV) in the present example, but as noted above can take other forms.

(111) The time interval [0, T] is the time interval over which collision risk is evaluated for multiple candidate paths and potentially multiple obstacles. The AV is assumed to have a known shape denoted by reference numeral 1002. At any given time t[0, T], the AV is represented by the area of the AV shape 1002 in the spacetime plane defined by t (lying parallel to the xy-plane), at a location in that plane which depends on the AV's speed and direction of motion.

(112) A spacetime plane can, in general, have any orientation within spacetime. However, hereinbelow, the term spacetime plane (or simply plane) is used to refer to a spacetime plane of constant time i.e. lying parallel to the xy-plane and defined by a given point in time (unless otherwise indicated). Physically, this corresponds to a snapshot of the spatial area over which the motion planning analysis is performed at that time. The planes t=0 and t=T may be referred to as the starting and destination planes respectively.

(113) FIG. 11 shows a spacetime volume 1102 which represents a path travelled by the AV as a spacetime tube 1102. The spacetime tube 1102 is analogous to the swept area A of the tethered obstacle method above and is a volume of spacetime traversed by the robot in moving along the path in the time interval [0, T].

(114) The geometric projection of the spacetime tube 1102 onto the spacetime plane t=0 is the swept area A on which the tethered obstacle method is based. Note this is only shown for the sake of comparison with the above tethered obstacle methodthe geometric projection of the spacetime volume 1102 (i.e. the projecting along rays extending parallel to t=0) is not used or computed in the dynamic method described herein.

(115) The cross-sectional area of the spacetime volume 1102 within any given spacetime plane (i.e. the intersection of the spacetime volume 1102 with that spacetime plane) is the area defined by the shape of the AV 1002 in the xy-plane.

(116) The spacetime tube 1102 representing the AV's path in spacetime is contained within a spacetime volume 1104 which separates the spacetime plane t=0 from the spacetime plane t=T. The spacetime volume 1104 is the collision space in which motion planning is performed for the dynamic method.

(117) Note that, whilst, in a strict mathematical sense, a spacetime plane extends to infinity, in the present context the term plane may be used to refer to a finite area within spacetime, corresponding to the spatial area over which the analysis is performed. That is, the term plane may be used as shorthand to refer to a finite portion of a plane in the strict mathematical sense. Hence, the collision space 1104 over which motion planning is performed is also a finite volume.

(118) As shown in FIG. 12, uncertainty in relation to moving obstacles is accounted for in terms of the uncertainty with which each obstacle's destination can be predicted. The destination of an obstacle means its position (location) at time t=T, i.e. at the end of the time interval over which the motion analysis is performed.

(119) The start of the time interval t=0 is a current time, and it is assumed that the xy position of each obstacle at t=0 (starting position) is known exactly (with zero uncertainty). For obstacle k, the starting position is denoted r.sub.k. The starting position defines a spacetime reference point (r.sub.k, 0) for that obstacle on the plane t=0.

(120) It is also assumed that each obstacle travels at constant velocity, i.e. the speed and direction of its motion does not change within the time interval [0, T]. Under this assumption, any obstacle trajectory will be described as a ray, i.e. a straight line (strictly, half-line) extending upwardly (direction of increasing t) from the reference point (r.sub.k, 0) (see FIG. 14 described below). However, the speed and direction is not assumed to be knownhence, in this model, a predicted set of obstacle trajectories is given by the set of rays extending upwardly from the reference point (r.sub.k, 0) having different orientations in spacetime. By way of example, FIG. 12 shows an example obstacle trajectory as a ray 1202 extending upwardly from (r.sub.k, 0). The ray 1202 intersects the plane t=T at spacetime point (r.sub.k,T), where r.sub.k denotes the spatial destination of obstacle k if it were to follow the trajectory 1202. Note that, since r.sub.k is known, the ray 1202 is fully defined by the intersection (r.sub.k,T) with the destination planehence the uncertainty in the obstacle's trajectory can be fully captured as the uncertainty in the obstacle's destination at t=T.

(121) The uncertainty in the obstacle's destination is captured in a probabilistic obstacle location function defined over the two-dimensional (2D) area of the destination plane t=T. In the present example, this function is a probability density p.sub.k (r,T) defined over all possible obstacle destinations in the t=T plane (destination probability density).

(122) The destination probability density is determined based on the object tracking performed at block A14 in FIG. 23.

(123) As shown in FIG. 13, in order to account for non-point obstacles, the spacetime tube 1102 can be expanded for obstacle k based on a determined (e.g. measured or assumed) shape of obstacle k. This is achieved by expanding the area of the AV 1102 in the xy-plane, with the expanded spacetime tube being determined as the volume of spacetime swept by the expanded area in the interval [0, T]. The expanded area for obstacle k is denoted .sub.k, and can be determined as the Minkowski sum of the xy AV shape with the xy shape of obstacle B.sub.k as described above. The parallel projection of the expanded tube .sub.k into the plane t=0 gives the area A.sub.k underlying the tethered obstacle method. This is shown in FIG. 13 for reference, however, it is again noted that this is merely for the sake of comparisonthe parallel-projected area is not used as a basis for the dynamic method.

(124) Rather, it is an area A.sub.k in the destination plane t=T that underlies the dynamic obstacle method.

(125) As shown in FIG. 14, the area A.sub.k is the perspective projection (or shadow) of the spacetime tube .sub.k onto the destination plane, relative to the reference point (r.sub.k, 0) for obstacle k. That is to say, the area obtained by casting rays upwardly from (r.sub.k, 0) and projecting .sub.k onto t=T along those rays. As noted above, the trajectory of obstacle k is assumed to be a ray of unknown orientation cast upwardly from (r.sub.k, 0). Therefore, the set of obstacle destinations with the projected area A.sub.k defines the subset of rays which intersect the expanded spacetime tube .sub.k, and hence the subset of trajectories for obstacle k which result in a collision at some time within the interval [0, T] (a collision occurs whenever the canonical point describing obstacle k intersects the surface of the expanded spacetime tube .sub.k, because this implies the AV and the obstacle are at the same position at the same time)

(126) FIG. 15 shows a plan view of the destination plane t=T. For a single obstacle k, the risk of collision with the AV in [0, T] is given by:
P.sub.D(k)=.sub.A.sub.kp.sub.k(r,T)(2.1)
That is, the integral of the destination probability density over the perspective projected area A.sub.k (in contrast to the tethered method, in which the integral is over the expanded swept area A.sub.k). Note, this is still a two-dimensional surface integral with the 2D position r as the only variabletime T is a constant.

(127) As for the tethered method, with multiple obstacles, the total collision risk for a given path is given by:

(128) $\begin{array}{cc}{P}_D=1-\underset{k}{.Math.}\left(1-P_D\left(k\right)\right)& \left(2.2\right)\end{array}$
where the product term is over all detected obstacles. The total risk of collision for a given path can therefore be computed by computing the integral of (2.1) for each path, and then the outputs as per (2.2). This may be viable in certain contexts, however as for the tethered obstacle method, the complexity scales as O(NK), where N is the total number of paths and K is the total number of obstacles. Hence, for a large number of paths, this requires significant additional compute resources for each additional obstacle in order to complete the necessary computations within the required time frame.

(129) To increase the efficiency, and achieve O(N+K) complexity, the dynamic method is also based on the upper bound on the total collision risk:

(130) $\begin{array}{cc}{P}_D{F}_D=\underset{k}{.Math.}{P}_D\left(k\right).& \left(2.3\right)\end{array}$

(131) As described above, the tethered obstacle method used the convolution trick to aggregate all of the obstacles into a single path-independent functionsG and G.sub.which can then be re-used for different candidate paths, by (in that case) integrating over the swept area of the path.

(132) The same core principle is applied in the dynamic method, but the path-independent function is different.

(133) Note the terms path-independent function and accumulator are used interchangeably herein, to describe a function which provides an accumulated measure of collision risk at points within the collision space (2D area for the teethed method; the 3D spacetime volume 1104 for the dynamic method) taking into account all of the K obstacles under consideration. The accumulator is defined such that the total risk of collision along any given path can be computed as an integral (or other aggregation) of the accumulator over (or otherwise based on) an area defined by that path and the known xy shape of the AV. For the tethered method, the area in question is the (non-expanded) swept area A.

(134) For the dynamic method described herein, the path risk is given by the integral of the accumulator over the surface .sub.k of the expanded spacetime tube .sub.k.

(135) The principles of accumulators in the dynamic method are illustrated in FIGS. 16A and 16B. This considers three obstacles k=1,2,3 merely by way of example. For each obstacle, a reference point (r.sub.k, 0) (its starting location at t=0) is known. An accumulator denoted G is defined at every spacetime point in the collision space, where G(r,t) accumulates the contribution to the total collision risk at point (r,t) for all obstacles. The contribution for each obstacle k is assessed based on the destination density function p.sub.k for that obstacle at destination point (r.sub.k,T), that point being the perspective projection of (r,t) onto t=T from the reference point (r.sub.k, 0) for obstacle k. That is, (r.sub.k,T) is the intersection, with t=T, of an obstacle trajectory, in the predicted set of obstacle trajectories, defined by (r.sub.k, 0) and (r,t)which, in the present example, is a ray cast from (r.sub.k, 0) through (r,t). Physically, this reflects the fact that an obstacle on the trajectory defined by (r.sub.k, 0) through (r,t) would end up at destination r.sub.k at time T.

(136) As illustrated in FIG. 16B, given a spacetime tube , the total collision risk for that path, taking into account all obstacles, is estimated by aggregating the accumulator over the surface of the spacetime tube, for example by integrating it over the spacetime tube surface as:
.sub.(r,t)G(r,t).(2.4)
Two variants of the dynamic method are described below and are referred to as the tensor field method and voxel method respectively.

(137) In the tensor field method, an accumulated (summed) obstacle tensor field accumulator is defined, which is integrated over the surface of each spacetime tube under consideration. In the voxel method, a coarser aggregation over al based on a voxelization of the collision space (this may be referred to as a coarse approximation of the integral of (2.4) herein). As described below, each voxel face in the collision space is projected onto t=T for each obstacle k, and the probability density p.sub.k (r,T) for obstacle k is integrated over the area of the voxel face projection in t=T to obtain an associated probability value, also referred to herein as a fragment (in FIG. 16A, if (r,t) is the centre point of the voxel face, the projected voxel face for obstacle k would be a quadrilateral on t=T centred on (r.sub.k,T)see below). The integral of the accumulator over the surface can then be approximated based on a summation of the probability values associated with the voxel faces that approximate the surface of the spacetime tube .

(138) 3. Tensor Method (FDPR I)

(139) The tensor method allows accumulation over multiple obstacles under the assumption they have the same shape, such that a single expanded spacetime volume (expanded by the common obstacle shape) can be considered for each path for those multiple obstacles. Where obstacles have different shapes, safe planning can be achieved by choosing a conservative obstacle shape for the expansion of the spacetime tube 1102, which is large enough to completely contain every obstacle under consideration.

(140) For obstacles with a common shape, .sub.k= for each obstacle k, which is the candidate path spacetime tube expanded by the common obstacle shape.

(141) 3D fields are defined, for each obstacle k, which can be legitimately summed in order to obtain a path-independent accumulator, which in turn can be integrated over the surface .

(142) With reference to FIG. 17, an estimate of the probability of collision with obstacle k, is given by the integral over surface of the AV's space-time tube , to accumulate the contribution from p.sub.k(r.sub.k,T), where (r.sub.k,T) is the point at which a ray from r.sub.k, through , strikes the t=T plane (r.sub.k being the obstacle destination for that obstacle trajectory at time t=T). This will give (an upper bound on) the collision probability, as the integral of the accumulator effectively integrates p.sub.k(r,T) over the shadow A.sub.k for each obstacle k simultaneously. This is an upper bound because, for a sufficiently convoluted , a ray exiting the spacetime tube could re-enter the spacetime tube H and exit again. This is again acceptable, as an upper bound is conservative, as needed for safety.

(143) With reference to FIG. 18, first it is assumed that, for all k, the initial position of the obstacle r.sub.k lies outside the spacetime tube H, otherwise the probability of collision is 1.

(144) The spacetime tube H has a side surface .sub.side extending from the plane t=0 through the collision space 1104 to the plane t=T so as to connect those planes, and a top-surface (end cap) lying within the t=T plane. The other end cap at t=0 can be ignored because of the assumption that all r.sub.k lie outside that end cap, hence no rays pass through it. Thus for the present purposes:
=.sub.side.sub.end

(145) The integral over surface is the sum of integrals over the side-surface .sub.side and the end cap of n on the t=T plane, denoted .sub.top, so:
P.sub.DF.sub.D=P.sub.side+P.sub.end(3.1)
where P.sub.side=.sub.kP.sub.side(k); P.sub.end=.sub.kP.sub.end.(3.2)

(146) The top-surface integral is straightforward to compute directly:
P.sub.end=.sub..sub.endp(r,T),(3.4)
where p(r,T)=.sub.kp.sub.k(r,T).(3.5)
In order to determine the side-surface integral, an obstacle tensor filed accumulator is defined in the following manner.

(147) With reference to FIG. 19, the integral over the side-surface .sub.side takes into account projection cosines {circumflex over (R)}.Math.n and {circumflex over (R)}.Math.{circumflex over (t)} where n=n(r,t) is the normal to .sub.side at (r,t), {circumflex over (t)} is a unit vector parallel to the time axis (normal to the plane t=T), and {circumflex over (R)} is a unit vector along the ray 1202.

(148) To prepare for the surface integral, the probability distribution p.sub.k(r,T) on t=T is extended to p.sub.k(R) in xyt-space where R=(r,t), by propagating it along rays through the reference point r.sub.k on t=0:
p.sub.k(R)=p.sub.k(r.sub.k,T)(3.6)
where (r.sub.k,T) is the perspective projection of R=(r,t) from the reference point (r.sub.k, 0) for obstacle k onto t=T:

(149) $\begin{array}{cc}{r}_k^{}=r_k+\left(r-r_k\right)\frac{T}{t}.& \left(3.6\right)\end{array}$

(150) The integral over the side-surface can now be expressed in R-space as:

(151) $\begin{array}{cc}{P}_{\mathrm{side}}\left(k\right)={}_{{}_{\mathrm{side}}}\frac{T}{t}\left(\frac{1}{{\hat{R}}_k.Math.\hat{t}}\right)p_k\left(r,t\right)\mathrm{pos}\left({\hat{R}}_k,.Math.n\right)& \left(3.8\right)\end{array}$$\begin{array}{cc}\mathrm{where}{\hat{R}}_k={\hat{R}}_k\left(r,t\right)=\frac{R-\left(r_k,0\right)}{.Math.R-\left(r_k,0\right).Math.},a\mathrm{unit}\mathrm{vector}\mathrm{in}\mathrm{the}\mathrm{ray}\mathrm{direction}\mathrm{in}\mathrm{xyt}-\mathrm{space}.& \left(3.9\right)\end{array}$

(152) Here, {circumflex over (R)}.sub.k.Math.n and {circumflex over (R)}.sub.k.Math.{circumflex over (t)} are projection cosines and pos( . . . ) suppresses negative arguments so as to only count rays passing out of .sub.side:

(153) $\mathrm{pos}\left(u\right)=\{\begin{array}{cc}u& u>0\\ 0& \mathrm{otherwise}\end{array}$

(154) This prevents double counting of rays: any rays which enter the spacetime volume within the interval [0, T] but do not exit it within that interval will intersect the plane t=T within the top-surface .sub.top. Such rays are therefore accounted for in the integral over the top-surface .sub.top, hence the side-surface integral only needs to include rays which exit the spacetime volume through the side surface .sub.side.

(155) The term

(156) $\frac{T}{t}$
scales for projection onto the t=T plane. Writing (3.8) in terms of a vector field f.sub.k:
P.sub.side(k)=.sub..sub.sidepos(f.sub.k(r,t).Math.n)(3.10)
where f.sub.k(r,t)=f.sub.k(r,t){circumflex over (R)}.sub.k The obstacle vector field,(3.11)
and

(157) 0$\begin{array}{cc}{f}_k\left(r,t\right)=\frac{T}{t}\left(\frac{1}{{\hat{R}}_k.Math.\hat{t}}\right)p_k\left(r,t\right)\mathrm{The}\mathrm{obstacle}\mathrm{scalar}\mathrm{field}.& \left(3.12\right)\end{array}$
Summing Over Obstacles

(158) Ideally obstacles would be combined in P.sub.side=.sub.kP.sub.side(k) by summing over k in (3.10) to obtain P.sub.side in terms of a combined field f(r,t). However, the nonlinearity of pos( . . . ) prevents this. Instead, by approximating pos( . . . ) and extending vector fields f.sub.k: to tensor fields F.sub.k:, obstacle probability distributions combine into one tensor field F as follows. Approximating to 4th polynomial order in u,
pos(u)Pos(u)=(u+1).sup.2,u[1,1],(3.12)
noting this is an upper bound so pos(u)Pos(u), which will ensure that obstacle collision probabilities in (3.10) continue to be upper boundswhich is conservative, as required for safety. Whereas terms of the form pos(e.Math.n) are nonlinear in unit vector e=(e.sub.x, e.sub.y, e.sub.z), the approximate form pos(e.Math.n) can be expressed as a linear function of a tensor
(e)=(1,e.sub.x,e.sub.y,e.sub.t,e.sub.x.sup.2,e.sub.y.sup.2,e.sub.t.sup.2,e.sub.xe.sub.y,e.sub.xe.sub.t,e.sub.ye.sub.t)(3.13)
as Pos(e.Math.n)=(e)D(n).sup.T(3.14)
where D=diag(,,,,,,,,,)(3.15)
This in turn allows (3.10) to be approximated as

(159) $\begin{array}{cc}{P}_{\mathrm{side}}\left(k\right){}_{{}_{\mathrm{side}}}{f}_k\left(r,t\right)\mathrm{Pos}\left({\hat{R}}_k.Math.n\right)& \left(3.16\right)\end{array}$$\begin{array}{cc}={}_{{}_{\mathrm{side}}}{F}_k\left(r,t\right)D{\left(n\right)}^{.Math.}T& \left(3.17\right)\end{array}$$\begin{array}{cc}\mathrm{where}{F}_k\left(r,t\right)=f_k\left(r,t\right)\left({\hat{R}}_k\right)\mathrm{the}\mathrm{obstacle}\mathrm{tensor}\mathrm{field}.& \left(3.18\right)\end{array}$
By expanding to tensors, linearity with respect to the obstacle tensor has been achieved, which in turn means that summation across obstacles is possible:
P.sub.side.sub..sub.sideF(r,t)D(n){circumflex over ()}T(3.19)
where F(r,t)=.sub.kF.sub.k(r,t) . . . The summed tensor field,(3.20)
Here, it can be seen that the summed tensor has the required properties of an accumulatorit accumulates the contribution to collision risk from all obstacles at spacetime points within the collision space, and is path-independent, i.e. independent of any given spacetime tube; once computed, the summed obstacle field can be re-used for different paths, without having to re-compute it.

(160) Path-dependency is only introduced when the summed tensor field is integrated as per (3.19) for a given spacetime tubewhich comes from the fact that the integral is over .sub.side and also the term (n).sup.T which depends on the normal, n=n(r,t), to the side-surface .sub.side at (r,t). The summed tensor field itself is path-independent.

(161) Although F is a ten-component tensor, only nine components need to be stored for each spacetime point (r,t) for which it is computed, as F.sub.1=F.sub.5+F.sub.6+F.sub.7. A coarser approximation of pos( . . . ) could be used to yield a tensor field of lower dimensionality, further reducing the storage space required to store it. By way of illustration, FIG. 20 compares the quadratic approximation of (3.12) with a linear approximation of pos( . . . ).

(162) It is important to note that a number of the equations given above are provided in order to explain the principles of the tensor method. However, only a subset of the mathematical expressions given above actually describe computations that are performed in carrying out the tensor method. The algorithmic steps of the tensor method are given in Table 1 below, which indicates which of the mathematical expressions above actually correspond to algorithmic operations.

(163) TABLE-US-00001 TABLE 1 the algorithmic steps of FDPR I method. Given: a planning interval 0 t T , a common obstacle shape B, and moving obstacles k = 1, . . . , K, each obstacle in uniform motion from r.sub.k to an end point drawn randomly from p.sub.k(r,T), a robot path expressed as a space-time tube (not containing any r.sub.k), from a set of N possible paths, Compute a bound F.sub.D on the total collision risk P.sub.D with time complexity O(K + N), as follows. Preparation [summing over K obstacles] 1. Compute summed destination distribution p(r,T) as in (3.5) 2. Compute vector fields f.sub.k(r,t) for each obstacle, as in (3.11) 3. Computer obstacle tensor fields F.sub.k(r,t) as in (3.18) and sum them to obtain F(r,t). For each path [iterate over N paths] 1. Extract from the surfaces .sub.end and .sub.side 2. Compute the normal field n over .sub.side 3. Compute F.sub.D = .sub..sub.end p(r,t) + .sub..sub.side F(r,t)D(n).sup.T

(164) Returning to FIG. 26, the method can be implemented in the same way as described above with reference to this figure, but with steps S12 and S13 replaced with the preparations steps 1-3 given in table 1 (computation of summed distribution and summed tensor field), with F.sub.D being computed based thereon at step S14.

(165) 4. The Voxel Method (FDPR II)

(166) As illustrated in FIG. 21, in the voxel method, the collision space 1104 is discretized as a set of predefined voxels (volume elements) 2102, which are spacetime cuboids arranges as a 3D array. Note that only a selection of the voxels are shownhowever a 3D voxel grid is defined over the whole collision space 1104.

(167) As shown in FIG. 22, obstacle k is once again represented in terms of its starting location r.sub.k at t=0 and the probability (density) distribution p.sub.k (r,T) over its destination at time t=T. The spacetime tube is represented as a subset of the voxels within the collision space (again, only a selection of the voxels that make-up the spacetime tube are shown).

(168) Hence, in this model, .sub.side is represented a discrete set of voxel faces that make up the side surface, and .sub.top is represented as a discrete set of voxel faces lying in the t=T plane.

(169) The notation c is used to denote a voxel face c on the surface of the spacetime volume .

(170) Note that, in FIG. 22 and the later figures, the ray from (r.sub.k,0) though (r,t) is denoted {circumflex over (r)}, where:
{circumflex over (r)}={circumflex over (R)}.sub.k
as defined above.

(171) In FIG. 23, a specimen ray {circumflex over (r)} from the start position of the obstacle to the destination plane t=T exits the spacetime tube through voxel V.

(172) FIG. 24 shows an expanded view, in which it can be seen that the ray {circumflex over (r)} exits voxel V through vertical face c (the exit face for ray {circumflex over (r)}). The perspective projection of the exit face c, from (r.sub.k, 0) onto the destination plane t=T, is a quadrilateral lying in the destination plane t=T, denoted .sub.k(c). That is, face c projects from the starting point r.sub.k for obstacle k on the starting plane t=0 to the quadrilateral .sub.k(c) on the destination plane t=T. In general, the side surface may be made up of both vertically and horizontally aligned voxel faces, and a ray could exit through any of these.

(173) To allow systematic consideration of voxel faces, a convention is adopted herein by which a standard normal n.sub.c to any given voxel face c is defined as lying in the direction of the positive coordinate axisi.e. in the +x, +y, or +t direction depending on the orientation of the voxel face c. Hence, the standard normal n.sub.c to the exit face c in the example of FIG. 24 lies in the positive x direction.

(174) Within this framework, collision risk can be computed from voxels as follows.

(175) Using the voxel representation of the spacetime volume , the risk F.sub.D is computed principally by summing up over the voxel faces that comprise the boundary of (i.e. the surface ). As in (3.1), F.sub.D is a sum of the end component P.sub.end which could easily be computed as in the tensor method or based on voxels as described below. The side component P.sub.side is computed from voxels as follows. For each obstacle k the side probability is given by:
P.sub.side(k)=c.sub..sub.sideq.sub.k(c,v(c,)),(4.1)
where the sum is over voxel faces c on the side-surface .sub.side of the voxelized spacetime tube , and q.sub.k(c, v) is a fragment of collision probability for a face c is defined as:

(176) $\begin{array}{cc}{q}_k\left(c,v\right)=\{\begin{array}{cc}{}_{r{}_k\left(c\right)}{p}_k\left(r,T\right)& \mathrm{if}\left(R_c-\left(r_k,0\right)\right).Math.\mathrm{vn}0\\ 0& \mathrm{otherwise}\end{array}& \left(4.2\right)\end{array}$
where suffix v{+1, 1} denotes the direction of the outward normal from voxel face c (i.e. extending outwardly from the surface of the spacetime tube ) relative to the standard normal n.sub.c in the positive coordinate direction. In other words, v=+1 if the outward normal from that voxel face is aligned with the relevant coordinate axis (x, y or t) and v=1 if the outward normal is anti-aligned with the relevant coordinate axis.

(177) In (4.2), R.sub.c denotes the centre point of voxel face c, which is a point in spacetime.

(178) The integral in (4.2) can be efficiently computed using integral images. Integral images are known in the field of computer vision and provide an efficient way of generating the sum of values in a rectangular subset of a grid. These are well-understood in the context of the known Viola-Jones framework, therefore further details are not discussed herein.

(179) Note, for faces on t=T, .sub.k(c)=c and v=1, hence the summation over the end-surface can be expressed as:
P.sub.end(k)=c.sub..sub.endq.sub.k(c,v(c,)).(4.3)
noting that the fragment q.sub.k(c, v) is always .sub.r.sub.k.sub.(c)p.sub.k(r,T) for faces on t=T.

(180) It is now possible sum over obstacles, and achieve O(N+K) complexity as before, without repeating the summation over obstacles for each path to be evaluated, since it follows from (4.1) and (4.2) that:
P.sub.side=.sub.kP.sub.side(k)=.sub.k.sub.c.sub.sideq.sub.k(c,v(c,)),(4.4)
so
P.sub.side=.sub.c.sub.sideq(c,v(c,)),(4.5)
where
q(c,v)=.sub.kq.sub.k(c,v) for all c, for v=1,(4.6)

(181) Note that the fragment-sums q(c, v) may be pre-computed for all c in the voxel grid, for both values v=1, without reference to any particular space-time volume .

(182) Hence, q(c, v) is a valid accumulator, which can be summed over the surface of any given voxelized spacetime tube, in order to compute the collision risk for that path.

(183) All projections .sub.c(k) can be prepared offline and stored in efficiently by computing and storing a fixed table of projections each denoted (c). To compute the fixed table, all voxel faces in the collision space are projected onto t=T from an arbitrary reference point r.sub.o in t=0. In order to compute .sub.k(c) for a desired reference point (r.sub.k, 0), each pre-computed projection (c) in the fixed table of projections can simply be translated in the t=T plane to obtain the corresponding face projection .sub.k(c) for desired reference point r.sub.k as:
.sub.k(c)=(c)+(r.sub.kr.sub.o)
provided the r.sub.k are restricted to lie on grid points of the voxel grid on t=0.

(184) The precomputation of summed probability fragments q(c, v) can be performed at the start of execution, or as the paths are evaluated and cached on the fly. In any event, once computed, they are shared by all obstacle-groups.

(185) The voxel-based FDPR algorithm is similar to the tensor field version FDPR I, but with different preparation steps, and the final, per-path step changed to become simply:
Compute F.sub.D=.sub.cq(c,v) where =.sub.side.sub.end.3.

(186) Unlike the previous tensor version FDPR I, the voxel-based method is O(N+K) even for a set of K obstacles which do not share the same deterministic shape, i.e., they have different .sub.k. This is because the pre-computation of voxel projections can provide a speed-up of the online algorithm even when the obstacle shape, and hence the expanded spacetime volume .sub.k, is different for different obstacles k.

(187) A further speed up can be attained where a group of obstacles share the same obstacle shape (which may be an assumed or modelled shape), because in that event, even more computations can be re-used for different obstacles. Where some obstacles share the same shape, one group of obstacles with a common shape can share the same .sub.k. The per-group work is the identification of all c, for the shared by that group, and the computation of v(c, ) for each c.

(188) As before, it is noted that a number of the mathematical expressions set out above are given by way of explanation of the principles of the voxel method. Only a subset of these expressions actually correspond to computations that are performed in carrying out the voxel method.

(189) Table 2 below summarized the algorithmic steps that are carried out, with reference to the subset of equations that correspond to computations performed in carrying out those steps.

(190) TABLE-US-00002 TABLE 2 algorithmic steps of FDPR II method. Given: a planning interval 0 t T , obstacle shapes B.sub.k, and moving obstacles k = 1, . . . , K, in uniform motion from r.sub.k to an end point drawn randomly from p.sub.k(r,T), robot paths, each expressed as space-time tubes (not containing any r.sub.k), from set of N possible paths, Compute a bound P.sub.D on the total collision risk P.sub.D with the time complexity O(K + N), as follows. Prep [once for all, setting upthe algorithm for a given voxel grid] Precompute table of projections (c) Prep [summing over k = 1, . . . , K obstacles] 1. Translate the (c) to r.sub.k, to form the .sub.k(c), for each obstacle k. 2. For all c, and v = 1, compute q.sub.k(c,v) as in (4.2), and sum them to obtain q(c,v). For each path [iterate over N paths] 1. Determine the set of boundary faces c . 2. Compute the outward normals at each c , and hence compute the v(c,). 3. Compute F.sub.D = .sub.c q(c,v(c,))

(191) Returning to FIG. 26, the method can be implemented in the same way as described above with reference to this figure, but with steps S12 and S13 replaced with the preparations steps 1-3 given in table 2 (computation of voxel face projections (c) and .sub.k(c)), with F.sub.D being computed based thereon at step S14.

(192) 5. Dynamic ObstaclesDirect Integration Method

(193) It is emphasised that the use of a path-independent accumulator is optional for the dynamic method. A viable approach is to compute F.sub.D directly, by actually computing each of the individual obstacle collision probabilities P.sub.D(k) for each obstacle k and then summing them to obtain F.sub.D as in equation (2.1), or even computing the product of equation (2.2) that F.sub.D is linearly approximating. In that event, P.sub.D(k) is computed by projecting the spacetime volume onto the t=T plane, and computing the integral of p.sub.k (r,T) over the projected area.

(194) 6. Extensions

(195) Although in the above, it is assumed that the starting position r.sub.k of each obstacle k at t=0 is known exactly, this is not a pre-requisite. In the case that there is uncertainty in the starting position, a probability distribution over starting positions can be defined for obstacle k:
p.sub.k(r.sub.k,0).

(196) The collision risk for a particular path can for example be computed by sampling possible starting points from this collision {r.sub.k} for each obstacle, and the above steps can be carried out for each possible combination of obstacle starting points to determine a collision risk for each combination. The total collision risk is then given by the collision risk for each combination for obstacle starting points and the probability of that combination of starting points.

(197) Moreover, whilst in the above, obstacles are assumed to follow straight-line trajectories in spacetime (rays), this is not essential. The obstacle trajectories in the predicted set of obstacle trajectories could have a curved spacetime geometry (physically corresponding to accelerating obstacles). The same principles as above apply, only now the projection of a point (r,t) in spacetime onto t=T becomes the intersection, with the destination plane t=T, of a curved obstacle spacetime trajectory (rather than a ray) extending from (r.sub.k, 0) and passing through (r,t).

(198) At the hardware level, the on-board computer system A1 of the AV comprises execution hardware capable of executing algorithms encoded in software (computer programs) to carry out the above functions. Whilst the execution hardware can be general purpose or special purpose execution hardware, or any combination thereof, it will generally comprise one or more processors such as central processing units (CPUs) and which may operate in conjunction with specialized hardware such as, but not limited to, accelerators (e.g. GPU(s)), field programmable gate-arrays (FPGAs) or other programmable hardware, and/or application-specific integrated circuits (ASICs) etc. Given the need to perform complex data processing operations, often using sophisticated and complex ML/AI models, with sufficient accuracy and speed (often in real-time) to ensure safe and reliable operation, the on-board computer system may be highly sophisticated, possibly with specialized computer hardware tailored to implement the models and algorithms in question. Particularly given the speed at which innovation is progressing in the field of AI, it will be appreciated that the architecture of the AV on-board computer system A1 at both the hardware level and the functional/software level may take numerous forms. Herein, functional components and the like embodied in a computer systemsuch as the data processing component A2, prediction component A4 and AV planner A6are high-level representation of particular functionality implemented by the computer system, i.e. functions performed by whatever (combination of) general purpose and/or specialized hardware of the computer system that is appropriate in the circumstances. As noted, the invention also contemplates an off-board simulation environment where similar code may be executed on off-board computer hardware.

(199) Annex A

(200) TABLE-US-00003 Algorithm 1: FPR algorithm for the bound on collision risk given N paths, and K obstacles. Data: N instances of path A, B.sub.1:K .Math. p.sub.1:K(r), = 1 Result: F.sub.D // Compute G.sub. for k in l to K do | B.sub.k,(r) = |g.sub.(r) * I.sub.B.sub.k(r)| end G.sub.(r) = .sub.k=1.sup.K B.sub.k,(r) * p.sub.k(r) // Compute G $G\left(r\right)={.Math.}_{k=1}^K\frac{1}{\mathrm{area}\left(B_k\right)}{I}_{B_k}\left(r\right)*p_k\left(r\right)$ for each A do | // Compute A.sub. | A.sub.(r) = |g.sub.(r) * I.sub.A(r)| | // Integrate over a box around A with margins 4 | F.sub.D = .sub.2 A.sub.(r)G.sub.(r) + I.sub.A(r)G(r) end