Improved Distorted Configuration Space Path Planning and Its Application to Robot Manipulators

. 2020 Oct 24;20(21):6060. doi: 10.3390/s20216060

Algorithm 1. Node growing algorithm

Input: C-space,

T_{i}

N_{o c c}^{i}

N B^{i}

N P^{i}

Output: leaf nodes for each root node

Initialization:

i t e r a t i o n_{n u m}

= 1;

N o d e_{p a r e n t}

is empty.

for each obstacle

T_{i}

T = {T_{i} | i = 1, 2, \dots, n_{t}}

T_{i}

attaches with the boundary of C-space do

separate occupied nodes

N_{o c c}^{i}

into

N B^{i}

and

N P^{i}

while nodes in

N P^{i}

are not searched totally do

i t e r a t i o n_{n u m} = 1

root nodes

N o d e_{r o o t}

are nodes in

N B^{i}

, and each root node has one index as a root index

N o d e_{p a r e n t} = N o d e_{r o o t}

The root index for a node in

N o d e_{p a r e n t}

is its original root index

end if

for each node

g_{p}

N o d e_{p a r e n t}

searching one-ring neighboring nodes

N o d e_{c h i l d}

from

N P^{i}

in all possible directions

g_{p} . a p p e n d (N o d e_{c h i l d})

end for

N o d e_{p a r e n t_t e m p} = {}

for each node

g_{p}

N o d e_{p a r e n t}

for each child node

g_{n}

g_{p}

g_{n}

is searched before then

stop searching in this direction

else if

g_{n}

is searched by two or more parent nodes then

label

g_{n}

by root indexes of its all parent nodes

else if

g_{n}

is not searched before then

label

g_{n}

by the root index of its parent node

N o d e_{p a r e n t_t e m p} . a p p e n d (g_{n})

end if

end for

i t e r a t i o n_{n u m} + = 1

N o d e_{p a r e n t} = N o d e_{p a r e n t_t e m p}

end while

end if

end for