Online 3-Dimensional Path Planning with Kinematic Constraints in Unknown Environments Using Hybrid A* with Tree Pruning

. 2021 Feb 6;21(4):1152. doi: 10.3390/s21041152

Algorithm 1 Hybrid A*

Input:

$X_{s t a r t}, X_{g o a l}$ : Start and goal configuration

grid : Grid

$O$ : Obstacles

1:
procedure HA*
2:
$q_{s t a r t} = X_{s t a r t}$
3:
$g r i d (q_{s t a r t}) . l i s t . i n s e r t (q_{s t a r t})$ ▹ add state to the cell’s list
4:
$O p e n L i s t = P r i o r i t y Q u e u e ()$
5:
$O p e n L i s t . i n s e r t (q_{s t a r t})$
6:
while $O p e n L i s t! = \emptyset$ do
7:
$q_{e x p} = O p e n L i s t . p o p ()$
8:
if $q_{e x p} \in X_{g o a l}^{r e g i o n}$ then
9:
return $q_{e x p}$ ▹ Solution found
10:
end if
11:
$q_{n e w} = E x p a n d (q_{e x p}, O)$
12:
for each ${\hat{q}}_{n e w} \in q_{n e w}$ do
13:
if $V a l i d ({\hat{q}}_{n e w}, g r i d)$ then
14:
$g r i d ({\hat{q}}_{n e w}) . l i s t . i n s e r t ({\hat{q}}_{n e w})$
15:
$O p e n L i s t . i n s e r t ({\hat{q}}_{n e w})$
16:
end if
17:
end for
18:
end while
19:
return ∅
20:
end procedure