Distributed Reactive Collision Avoidance - University of Washington

30
Figure 4.3: Example of a borderline case for five vehicles where no conflict-free constantspeed
maneuver exists. The circle is the allowable set (radius ‖v i ‖) and the blue regions
are the collision cones (shown with all equal velocities, of magnitude greater than ‖v i ‖).
The allowable space for v ′ i is a disk, centered at the origin, of radius v i,max . The optimal
solution to each region must lie on its boundary because the zero-cost point is not
feasible (otherwise there would be no conflicts). This optimal solution must either be a
vertex between two collision cones, the nearest point on a single collision cone, or the vertex
between one collision cone and the edge of the allowable space. An example of this
optimization is shown in Fig. 4.4.
The first step of the algorithm is to compute each of the nearest points on each collision
cone (there is a right and a left solution for each cone), which is v ′ i = ĉĉ T ṽ + v j . This
list of points is checked to make sure each ‖v ′ i‖ ≤ v i,max . If this is not the case, then v ′ i is
replaced with
√
v i ′ = v j − ĉĉ T v j ± ĉ (ĉ T v j ) 2 − vj Tv j + vi,max 2 , (4.11)
where only the solution with the smaller ∆v i is kept. These points account for the only
possible optima on the intersection of a cone and the edge of the space. Next, all of the twocone
intersection points are found using linear equations (keeping only those with ‖v ′ i‖ ≤
v i,max ). All of these points are ordered by increasing ∆v i and checked consecutively for