Dynamically Calculating Consideration Weights

The frontier moves along each active search vector simultaneously, and proportion- ally to the amount of resistance encountered along each. To slow down the pace at which the frontier travels along each vector, Ω is added to the length of the VD-VG vector, Vd, the search is to travel along, Fig. 5.6:

Vd′ = Vd+ VΩ

The increased length of the vector does not physically affect the VD-VG, and is used purely for calculating the amount the search should move along V . Rather than subtract Ω from max(Rv) to slow the wave down, as generically formulated in Section 3.3.4 (Eq. (3.12)), increasing the distance of the vector from Vd to Vd

5.2 The Windowed Dijkstra’s Search Algorithm 139

Fig. 5.5 Visualisation of how a roadmap vector is separated into unit measurements for calculating resistance, Ω. Ω is calculated from each segmentation of the vector and is summed along the complete length the search has travelled, up until either the time-step has expired or the robot’s destination line is reached.

Fig. 5.6 The resistance for each vector is added to it, therefore the search will slow down along the real vector.

will cause the time taken for the search frontier to travel along that vector, Vt, to increase, Vd′ max(Rv) > Vd max(Rv) ⇒ V_t′> Vt , if VΩ >0

Therefore, no matter how large Ω may be the speed of the search will never be nega- tive. Whereas for Eq. (3.12), if Ω > max(vr) then Sv <0, which would theoretically send the search backwards along the vector. By adding Ω to the original vector length, the speed of the search, Sv, will decrease, proved as follows:

Vd′ Vt′

> Vd Vt′

⇒max(R_v) > S_v , if VΩ >0 (5.3)

In areas of increasing collision likelihood (increased Ω), the frontier will slow down the model AMR to be more cautious. The total resistance encountered along these vectors is also considered when calculating the overall desirability of potential paths, discussed later in Section 5.2.2.

5.2.1.1 Propogating the Frontier along Roadmap Vectors

To avoid excessive calculations VΩ is calculated by summing all values of Ω, at each

unit measure along V , prior to the search moving along it. The active vectors are sorted by their relative lengths, Vd′, from 0 → N in order to eliminate the need for the frontier to be continually updated at each interval point along each active vector. Instead the frontier is propagated at a discrete interval, determined by the remaining distance to the node at the end of V (0). The frontier simultaneously moves the same distance along all other active vectors.The distance the search must move along all other active vectors is a simple ratio between the real and relative vector lengths, Fig. 5.7,

Sd(n) = Vd′(0)

_V

d(n) Vd′(n)



where {n ∈ Z | 1 ≤ n ≤ N}, and Vd′(0) is the remaining distance the frontier must move along to reach the vector’s end. V (0) is now removed from the active list, as the node at its end has been reached.

When a node is reached the nodes at the end of the associated new vectors are compared with the currently active ones. This will determine if the destination nodes of the new vectors share any destination nodes of the active vectors, i.e. to check if the frontier is propagating toward the same node along two separate vectors. As each node should only be visited once, Algorithm 2 is used to establish what actions should be taken when the end of a vector is reached.

5.2 The Windowed Dijkstra’s Search Algorithm 141

(a) (b)

(c) (d)

(e) (f)

Fig. 5.7 Visualisation of how resistance applied to the roadmap affects the speed of the search algorithm as it travels along n vectors, V (n). (a) All vectors connected to the start point of the search, ordered by increasing values of Vd′. The search moves uniformly at Sv = max(Rv) along all active Vd′ vectors, by the smallest value in the list, Vd′(1). (b) The relative distance the search moves along all associated vectors of length Vd, which form the actual VD-VG. The time period remains the same and so the speed of the search is altered along each vector accordingly, t = V_d′(1) ÷ max(Rv) ∴ Sv(n) = Vd(n) ÷ t. (c) The node at the end of V (1) is connected to V (7, 8) and adds them to the ordered list of V′

d vectors. Similarly to (a) the search moves uniformly along all vectors by the remaining distance to the end of V_d′(2). (d) V (7, 8) vectors are added to the active list and highlights the same process shown in (b). (e) V (9, 10, 11) vectors are added to the active list and highlights the same process shown in (c). (f) V (9, 10, 11) vectors are added to the active list and highlights the same process shown in (d).

Algorithm 2:Adding New Vectors 1 V ← active vector list;

2 V∗ ← new vector list;

3 if node at end of V∗ has not been previously visited then 4 if node at end of V∗ is shared with node at end of V then 5 if V_d′∗ is less than the remaining Vd′ then

6 the V∗ replaces V ; 7 else 8 V∗ is ignored; 9 else 10 add V∗ to V list; 11 else 12 V∗ is ignored;

5.2.1.2 Avoiding Exclusion Zones

As the frontier moves along the roadmap vectors, it must also not enter an EZ. When calculating VΩ, if the next unit measurement intersects an EZ, then the wave must be

terminated to prevent the search from colliding with it. In order to circumnavigate the EZ an artificial node is positioned at the frontier, just outside the EZ, Fig. 5.8, and the current vector is terminated. New vectors are created from this artificial node to the nodes connected to the destination end of the terminated vector. If any collision free vectors can be made, providing it does not intersect the EZ, these new vectors are added to the active vector list.

5.2.1.3 A New Search at Each Time-Step

The search-time of the algorithm is synchronised with the update frequency of the pedestrian model. Therefore, the search terminates once the time elapsed reaches the value of the time-step, t, used in the pedestrian model. As the search executes the time taken for the search to move along each vector is accumulated, tacc. When the search begins tacc= 0, then for every discrete move along VD-VG the time taken is added

tacc= tacc+ Vt′(0)

where Vd′(0) is the distance the frontier must move to reach the end of the vector with the shortest remaining relative distance to its end.

Before each new vector is evaluated by Algorithm 2, a inequality check is made to determine if the end of the vector can be reached in the remaining time,

t − tacc< Vt′∗ (5.4)

5.2 The Windowed Dijkstra’s Search Algorithm 143

Fig. 5.8 Visualisation of how a new vector is created if the search enters an EZ. A new vector is created between the unit measurement just outside the EZ and an available node connected to end of the terminated vector. A new vector will only be formed if it also does not intersect the EZ.

which case the new vector will automatically be added to the active list. As the search will not explore the length of the entire vector, VΩ is only calculated along

a length up until Eq. (5.4) is no longer satisfied, Fig. 5.5. The cumulative time the search would take to reach each consecutive unit measure along V is evaluated. Therefore, any additional resistance that occurs when Eq. (5.4) is not satisfied will not inflate Vd′ to include resistance for areas the search will not be conducted, and thus reduce Sv disproportionately. The frontier will make its final propagation along the remaining distance of Vd′(0) when tacc+Vt′(0) = t, and the search will terminate. A selection of the most desirable points from the search frontier at t are converted into a selection of paths, Pt(0 → N). As linear predictions are suitable for dynamic environments [17], each Pt that traces back along the VD-VG to the start point of the search, P0, is stored as a simple vector. All Pt vectors calculated for each consecutive time-step along T can then be connected to create global paths, PT. The CPP will evaluate each PT to predict the most considerate, which will then be executed by the AMR.