Reactive Control for Collision Evasion with Extended Obstacles

. 2022 Jul 22;22(15):5478. doi: 10.3390/s22155478

Algorithm 1 Find a non-collision velocity command at sampling-index

k + 1

(change yaw only)

1:
The current sampling-index is k;
2:
for $v_{0} = A$ , $v_{0} = v_{0} + \frac{B}{N}$ , while $v_{0} \leq A + B$ do
3:
$V_{c}^{I} = R (ψ (k), θ (k)) * v_{0} * u$ ;
4:
if col( $V_{c}^{I}$ , $o b j e c t S e t$ )==0; then
5:
Return $V_{c}^{I}$ as the velocity command, and this algorithm is finished;
6:
end if
7:
for $ψ_{0} = - α * d t$ , $ψ_{0} = ψ_{0} + \frac{α * d t}{N}$ , while $ψ_{0} < α * d t$ do
8:
$V_{c}^{I} = R (ψ (k), θ (k)) * R (ψ_{0}) * v_{0} * u$ ;
9:
if col( $V_{c}^{I}$ , $o b j e c t S e t$ )==0; then
10:
Return $V_{c}^{I}$ as the velocity command, and this algorithm is finished;
11:
end if
12:
$V_{c}^{I} = R (ψ (k), θ (k)) * R (- ψ_{0}) * v_{0} * u$ ;
13:
if col( $V_{c}^{I}$ , $o b j e c t S e t$ )==0; then
14:
Return $V_{c}^{I}$ as the velocity command, and this algorithm is finished;
15:
end if
16:
end for
17:
end for
18:
Run Algorithm 2 (change both yaw and pitch);