Trajectory Planner CDT-RRT* for Car-Like Mobile Robots toward Narrow and Cluttered Environments

. 2021 Jul 15;21(14):4828. doi: 10.3390/s21144828

Algorithm 1 BuildTree*

(q_{0}, x_{0}, g o a l)

TW . INIT (q_{0}, g o a l);

TX . INIT (x_{0}, g o a l);

while

t_{l i m i t}

q_{r a n d} \leftarrow

RandomSample();

[Q_{n e w}, Q_{n e a r}, t y p e] \leftarrow

ExtWorkSpace

(TW, q_{r a n d});

Q_{n e a r l i s t} \leftarrow

NearNodeList

(Q_{n e w}, B_{r})

[X_{p}, X_{m i n}] \leftarrow

FindParentState*

(Q_{n e a r l i s t}, q_{n e w}, t y p e);

X_{p} = n i l

then

continue

;
end if

TW . Add (Q_{n e a r}, Q_{n e w});

TX . Add (X_{p}, X_{m i n});

ReConnectTrees

(Q_{n e w}, X_{m i n});

end while

return;