Algorithm 2. Trajectory optimization method.

Input: original path $L$, C-space, obstacles $T_i$, start point $p$, target point $q$

Output: optimized path ${\mathit{L}}^{\mathit{o}\mathit{p}\mathit{t}}$

${\mathit{L}}^{\mathit{o}\mathit{p}\mathit{t}}=\left[\right]$

${\mathit{L}}_{\mathit{t}\mathit{e}\mathit{m}\mathit{p}}=\left[\right]$

for $l=1$ to $n_l$ do

if l = 1 then

$L^{opt}.append\left(G_L^l\right)$

$star{t}_{temp}=G_L^l$

continue

end if

$en{d}_{temp}$ = $G_L^l$

if $l=n_l$ then

$L^{opt}.append\left(en{d}_{temp}\right)$

return $L^{opt}$

end if

check whether the path between $star{t}_{temp}$ and $en{d}_{temp}$ intersects with any obstacle $T_i$

if intersection then

$L^{opt}.append\left(G_L^{l-1}\right)$

$star{t}_{temp}=G_L^{l-1}$

else

continue

end if

end for