Table - PMC

Skip to main content

View full-text article in PMC

. 2024 Feb 26;24(5):1520. doi: 10.3390/s24051520

Algorithm 1

{APF-GFARRT}^{*}

Algorithm

01: Initialize

X_{i n i t}

,

X_{g o a l}

, and other parameters.

02: Initialize an empty tree

T

with the root node as

X_{i n i t}

03: for

i = 1

to

M a x I t e r a t i o n s

do

04:

X_{r a n d} \leftarrow NewRandPoint ()

05:

apf_force \leftarrow APF_att (X_{r a n d}, X_{g o a l})

06:

X_{g u i d e d} \leftarrow GenerateGuided (X_{r a n d}, apf_force)

07:

D_{l i n e} \leftarrow Distance (X_{g u i d e d}, X_{s t a r t}, X_{g o a l})

08:

X_{n e a r} \leftarrow Nearest (T, X_{g u i d e d})

09: If

CollisionFree (X_{n e a r}, X_{g u i d e d})

then

10:

o b s D e n s i t y \leftarrow ObstacleDensity (X_{g u i d e d}, fixedRadius)

11:

a d a p t i v e S t e p \leftarrow FuzzyAdaptiveStep {(o b s D e n s i t y)}^{*} S t e p

12: If

findPath = = 0

then

13:

X_{n e w} \leftarrow Steer (X_{n e a r}, X_{g u i d e d}, a d a p t i v e S t e p)

14: else

15:

α \leftarrow (D_{l i n e} \geq D_{T h r e s h o l d}) ? ComputeAlpha (K, D_{l i n e}) : 1

16:

X_{n e w} \leftarrow Steer (X_{n e a r}, X_{g u i d e d}, a d a p t i v e S t e p * α)

17: end if

18:

X_{n e a r e s t} \leftarrow NearC (T, X_{n e w}, r)

19:

X_{m i n} \leftarrow ChooseBestParent (X_{n e a r e s t}, X_{n e w})

20:

AddNodeEdge (T, X_{m i n})

21:

X_{n e a r}^{n e w p a r e n t} \leftarrow UpdateParent (X_{n e w}, X_{n e a r e s t})

22:

PruneNodes (X_{n e a r}^{n e w p a r e n t}, X_{n e a r e s t})

23:

Rewire (T)

24: end if

25: if

GoalReached (X_{n e w}, X_{g u i d e d})

then

26: return

ReconstructPath (T, X_{i n i t}, X_{g o a l})

27: end if

28: end for

29: return

NoPathFound ()