Algorithm 2: Hybrid A*
1	Initialization of openset, closeset
2	openset.push(start)
3	while openset is not empty do
4			$x_{current}\leftarrow openset.popMinCostNode$
5			if exists RS path then
6					return the path
7			end
8			for $x_{next}$calculated by kinematic equation do
9					Collision avoidance
10					if $x_{next}$ not exists in closeset then
11							$g\leftarrow g\left(x_{current}\right)+l(x_{current},x_{next})$
12							if $x_{next}$ not exists in openset or$g<g\left(x_{next}\right)$then
13									$g\left(x_{next}\right)\leftarrow g$
14									$h\left(x_{next}\right)\leftarrow Heuristic(x_{next},x_{goal})$
15									$Pred\left(x_{next}\right)\leftarrow x_{current}$
16									if $x_{next}$ not exists in openset then
17											openset.push($x_{next}$)
18									else
19											openset.update(next node)
20									end
21							end
22					end
23			end
24	end