Efficient Energy Flight Path Planning Algorithm Using 3-D Visibility Roadmap for Small Unmanned Aerial Vehicle

Algorithm 1. RoadmapCreate

Input: Elevation matrix M in R³.

Output:

1.   FOR (each cut layer in the map; )

2.     cutPlane = Slice(M, H_min, H_max, k); //the parametric equation of the cut plane.

3.     FOR (each obstacle; )

4.       IF Obstacle -> highestNode(Z) < k Dcut //check if the obstacle highest node is lower than the cut plane

5.       THEN

6.         For (each edge of the obstacle)

7.         tentativeNode = FindIntersection (cutPlane, edge);

8.         newNode = enlarge(tentativeNode, Dsafe);

9.         N = Add(newNode,obstacleIndex, facetIndex);

10.     ELSE

11.       newNode = [Obstacle -> highestNode(X)

12.             Obstacle -> highestNode(X)

13.             kDcut]

14.       N = Add(newNode,obstacleIndex, facetIndex1, facetIndex2);

15.       BREAK;

16. FOR (Each pair of Node in set N; i = 1,NumberOfNode; j = i + 1,NumberOfNode)

17.   IF (LOScheck() is TRUE)

18.   THEN

19.     P = Add(N(i), N(j));

20.     P = Add(N(j), N(i));

21.  R = (N,P);

22.  SaveToFile(R);

Algorithm 1. RoadmapCreate