RCS: a fast path planning algorithm for Unmanned Aerial Vehicles

TL;DR

This paper introduces a fast path planning algorithm for UAVs in polygonal environments, considering UAV motion constraints, which efficiently finds near-optimal collision-free paths with preprocessing for rapid queries.

Contribution

The paper presents a novel $O(n^2 \, \log n)$ algorithm for UAV path planning that incorporates turning and route length constraints, with a preprocessing method for quick path queries.

Findings

Algorithm runs in $O(n^2 \log n)$ time.

Preprocessing enables $O(n \log n)$ query time for path planning.

Produces paths close to the shortest feasible paths under constraints.

Abstract

Path planning is a major problem in autonomous vehicles. In recent years, with the increase in applications of Unmanned Aerial Vehicles (UAVs), one of the main challenges is path planning, particularly in adversarial environments. In this paper, we consider the problem of planning a collision-free path for a UAV in a polygonal domain from a source point to a target point. Based on the characteristics of UAVs, we assume two basic limitations on the generated paths: an upper bound on the turning angle at each turning point (maximum turning angle) and a lower bound on the distance between two consecutive turns (minimum route leg length). We describe an algorithm that runs in $O(n^2 \log n)$ time and finds a feasible path in accordance with the above limitations, where $n$ is the number of obstacle vertices. As shown by experiments, the output of the algorithm is much close to the shortest path with this requirements. We further demonstrate how to decompose the algorithm into two phases, a preprocessing and a query phase. In this way, given a fixed start point and a set of obstacles, we can preprocess a data structure of size $O(n^2)$ in $O(n^2 \log n)$ time, such that for any query target point we can find a path with the given requirements in $O(n \log n)$ time. Finally, we modify the algorithm to find a feasible (almost shortest) path that reach the target point within a given range of directions.

Tables

Table 1. Table 1: Comparison of the results of the RCS and A* algorithms on the configuration of Figure  6

Target	RCS
preprocessing time (ms)	RCS query time (ms)	RCS total time (ms)	A* running time (ms)	RCS path length	A* path length	Relative difference
$t_1$	6.28	0.26	6.54	375	487.6	470.4	3.5%
$t_2$	6.76	0.20	6.96	14183	715.5	670.2	6.3%
$t_3$	6.83	0.23	7.06	71164	998.5	952.2	4.6%
$t_4$	6.86	0.25	7.11	165713	1539.6	1467.0	4.7%
$t_5$	6.88	0.26	7.14	230670	1954.4	1860.8	4.8%

Table 2. Table 2: Illustration of the effect of changing α 𝛼 \alpha on the RCS and A* algorithms.

Max. turning angle (degree)	RCS
preprocessing time (ms)	RCS query time (ms)	RCS total time (ms)	A* running time (ms)	RCS path length	A* path length	Relative difference
80	0.20	0.44	0.64	119.8	466.1	477.6	2.4%
40	0.37	0.59	0.96	204.2	478.0	473.1	1.0%
20	0.16	0.68	0.84	1878.0	478.1	471.9	1.3%
10	0.18	0.48	0.66	5029.6	480.9	472.5	1.7%

Table 3. Table 3: Illustration of the effect of changing l 𝑙 l on the RCS and A* algorithms.

Min. leg length	RCS
preprocessing time (ms)	RCS query time (ms)	RCS total time (ms)	A* running time (ms)	RCS path length	A* path length	Relative difference
40	0.16	0.72	0.88	121.7	279.2	283.3	1.4%
20	0.20	0.75	0.95	217.8	279.2	282.0	1.0%
10	0.22	0.73	0.95	554.4	279.2	281.9	1.0%
5	0.22	0.82	1.04	789.6	279.2	283.1	1.4%

Table 4. Table 4: Illustration of the effect of increasing the complexity of scene.

Test Case	#Obstacle	RCS
preprocessing time (ms)	RCS query time (ms)	RCS total time (ms)	A* running time (ms)	RCS path length	A* path length	Relative difference
$t_1$	2	1.18	0.94	2.12	188.3	643.1	645.7	0.4%
$t_2$	4	6.56	1.18	7.74	149.6	643.2	645.8	0.4%
$t_3$	8	14.45	1.31	15.76	449.9	664.9	663.4	0.2%
$t_4$	16	33.08	2.07	35.15	318.8	747.3	669.1	10.5%
$t_5$	32	209.36	4.00	213.36	762.0	740.5	756.1	2.1%

Table 5. Table 5: Illustration of the effect of changing size of the scene.

Test Case	Scene size	Minimum leg length	RCS
prep. time (ms)	RCS query time (ms)	RCS total time (ms)	A* running time (ms)	RCS path length	A* path length	Relative difference
$t_1$	$100\times 100$	10	0.22	0.77	0.99	182.2	141.6	143.4	1.26%
$t_2$	$200\times 200$	20	0.27	0.79	1.06	233.2	284.7	287.6	1.01%
$t_3$	$400\times 400$	40	0.23	0.80	1.03	455.0	570.8	574.5	0.64%
$t_4$	$800\times 800$	80	0.22	0.80	1.02	1178.9	1143.0	1148.8	0.50%

Table 6. Table 6: Illustration of the effect of the path length on the running time of RCS and A*.

Path length	RCS preprocessing time (ms)	RCS query time (ms)	RCS total time (ms)	A* running time (ms)
100	0.0046	0.0159	0.0205	94.19
200	0.0046	0.0162	0.0208	104.21
400	0.0046	0.0174	0.0220	100.21
800	0.0046	0.0179	0.0225	95.73

Table 7. Table 7: Preprocessing and query time of the RCS algorithm for scenes of various complexities.

Test case	#Obstacles	#Vertices	Preprocessing time (ms)	Query time (ms)
1	1	3	2	3
2	2	7	25	3
3	9	39	1166	18
4	26	100	2033	26
5	84	300	28860	188
6	180	500	223134	570

Equations

$$x_{A_1}-x_{A_0}=|A_0{A}_1|\cos (\alpha +\beta ),$$

$$y_{A_1}-y_{A_0}=|A_0{A}_1|\sin (\alpha +\beta ).$$

$$x_{A_2}-x_{A_1}=|A_1{A}_2|\cos (2\alpha +\beta ),$$

$$y_{A_2}-y_{A_1}=|A_1{A}_2|\sin (2\alpha +\beta ).$$

$$x_{A_i}-x_{A_{i-1}}=|A_{i-1}{A}_i|\cos (i\alpha +\beta ),$$

$$y_{A_i}-y_{A_{i-1}}=|A_{i-1}{A}_i|\sin (i\alpha +\beta ).$$

$$x_{A_j}=e\sum _{i=1}^j\cos (i\alpha +\beta )+x_{A_0},$$

$$y_{A_j}=e\sum _{i=1}^j\sin (i\alpha +\beta )+y_{A_0},$$

$$e=\frac{x_{A_{k+1}}-x_{A_0}}{{\sum }_{i=1}^{k+1}\cos (i\alpha +\beta )}=\frac{y_{A_{k+1}}-y_{A_0}}{{\sum }_{i=1}^{k+1}\sin (i\alpha +\beta )}.$$

Full text

We describe an algorithm that runs in $O(n^{2}\log n)$ time and finds a feasible path in accordance with the above limitations, where $n$ is the number of obstacle vertices. As shown by experiments, the output of the algorithm is much close to the shortest path with this requirements. We further demonstrate how to decompose the algorithm into two phases, a preprocessing and a query phase. In this way, given a fixed start point and a set of obstacles, we can preprocess a data structure of size $O(n^{2})$ in $O(n^{2}\log n)$ time, such that for any query target point we can find a path with the given requirements in $O(n\log n)$ time. Finally, we modify the algorithm to find a feasible (almost shortest) path that reach the target point within a given range of directions.

K****eywords Path planning  $\cdot$ Unmanned aerial vehicles  $\cdot$ Polygonal domains  $\cdot$ Maximum turning angle  $\cdot$ Minimum route leg length

1 Introduction

An Unmanned Aerial Vehicle (UAV), is a flying machine without a human pilot. Due to its advantages in military, civil and industrial markets, they are largely employed in situations dangerous for human life [22, 9, 6]. Since traditional remote piloting are not adequate for current complex missions, autonomous path planning are utilized extensively for UAVs [8].

Path planning is the problem of finding a feasible path from a given source to a given target for a moving object, that optimizes a desirable objective function and taking into account different constraints. The constraints are of various types, for example, non-accessible points in the space, velocity and acceleration constraints, uncertainty about the environment and so on. This problem in its generic form is NP-hard [2] and has polynomial time algorithms only in some special cases [11].

This problem has been extensively studied in robotics and various algorithms are presented for different forms of it. In general, the problem of path planning in robotics is more complex than in UAVs because of high degrees of freedom. For example, articulated robots can move their arms, or the shapes of some robots are too complex, and the robots must move in several directions to be able to pass through some regions in the environment (Piano Mover’s Problem [19]).

In contrast to the robot path planning, the UAV path planning has several characteristics. The three major differences in path planning for UAVs are as follows. First, due to the small size of the flying object compared to the space, we usually model a UAV as a moving point in the space. This makes the problem of path planning much simpler.

Second, because of the high speed of UAVs, it is not possible for a UAV to rotate by a sharp angle in its flight path [31, 23]. This is completely different from a robot’s movement in which the robot can slow down or even stop to rotate. Because of this, the flight path of a UAV must be smooth and the path planner should avoid sharp turns. This limitation makes the path planning for UAVs different compared to the conventional path planning for robots, and increases the complexity of the path planner. The complexity magnified when the number of obstacles increases and the path planner may have to create a longer zigzag path with many rotations in order to make a smooth path. In addition to the limit on the turning angles, usually a UAV cannot have two consecutive turns in a short time. This enforces the path planner to make enough distance between two successive turns [31, 23].

The third difference in the UAV path planning is the ability of a UAV to change its flight height. Robots usually move on a surface, so the problem of robot path planning is normally defined on the plane. But given the fact that UAVs fly in the space, and sometimes there is a need for a change in altitude to avoid obstacles (for example, a UAV flying over a terrain, or a cruise missile over a sea near scattered islands). In these cases, the problem of designing a path should be solved in three-dimensional space which introduces new complexities [12, 20, 27].

In this paper, we take the first and second characteristics of UAVs path planning into account, and, as will be explained later, design a smooth path for a moving point among polygonal obstacles. However, in order to simplify explaining the algorithm in this paper, we choose the two-dimensional space. It is not too hard to generalize the approach to three-dimensional space.

The UAVs path planning algorithms are divided into two general categories, offline and online, based on the knowledge of the planner about the environment. A path planning algorithm is called offline, if the designer has complete information about the environment and obstacles in it [12, 15, 26]. On the other hand, an online algorithm knows little or nothing at all about the environment in which the movement will take place [25, 24, 15]. In the latter case, the path planning is done while the UAV moves and based on the information we get from the sensors, the path is corrected. In this paper we choose to work on offline algorithms.

In terms of the number of reported paths, the algorithms can be divided into three categories. In the first category, the path planning is performed only for one device. Most algorithms are included in this category. In the second category, the path planning is done simultaneously for a set of UAVs. In some applications, for example a salvo attack against a warship, in order to prevent air defence systems activation, target missiles must simultaneously hit the same target with different angles. In this case, the path planner algorithm finds the best paths for several UAVs all at once [29, 21, 10, 13]. In the third category, the path planning is performed for different targets, while the environment and the source point are fixed. This type of algorithms are useful when the target point is moving and we want to find the best time/place of the target to reach it, by taking advantage of fixed environment to speed up the path planning.

In this paper, we first propose a path planning algorithm for a single UAV. Later, with some changes, we split the algorithm into a preprocessing phase and a query phase in order to be able to find paths for multiple targets in a much shorter time.

In terms of the shape of the path, the algorithms are divided into two classes. In the first one, the output of the algorithm is composed of straight line segments that are connected together [7, 23, 31]. In the second class, the output consists of straight line segments and more complex curves such as arcs [20], Bezier curves [28, 21, 27] and B-Spline curves [12, 14]. The advantage of the former is the simplicity of the path and the speed of the path planner, while the latter is better to adapt to the rotation limitations of UAVs and not having sharp angle rotations. In contrast, the latter algorithms have high execution time and are rarely applicable in practice.

Based on the limitations described above that we consider for the path, we have the simplicity and speed of line segment path planners, while at the same time smoother paths are obtained that are traversable by UAVs.

The two limitations used in this paper, were also previously considered by other researchers. Szczerba et al. [23] were the first who described these two limitations. They used a heuristic search algorithm to find a path in the two-dimensional space with the above limitations that has length not more than a fixed given value and approaching the target from a desirable direction (approximately). The main problem with their algorithm, as will be explained in Section 6, is its running time. Later Zheng et al. [30] and Zheng et al. [31] considered the problem in the three-dimensional space and added two new limitations to the path, namely minimum flying height and maximum climbing/diving angle. They used an evolutionary algorithm to solve the problem, which as Szczerba et al.’s algorithm [23] has high running time when the complexity of the environment grows.

1.1 Our results

We can summarize our results as follows:

• •

We present an offline, graph based path planning algorithm, called RCS. We first make two assumptions about the required path, and then create a graph which models paths with those requirements, and finally apply a modified Dijkstra’s algorithm [4] on this graph to find the shortest feasible path from the source node to the target node. Because we find the shortest path with these properties, as we show in experimental results, the length of the result of the algorithm is much close to the length of the optimum path without the assumptions.

• •

We prove an upper bound on the time and space complexity of RCS. We show that if all inputs of the problem is given at the same time to the algorithm, we can solve the problem in $O(n^{2}\log n)$ time and $O(n^{2})$ space. In these upper bounds $n$ is the number of obstacle vertices.

• •

In another version of the problem, where several UAV’s need to be launched from a fixed source, or we need to find the best time/path to reach a moving target point, we divide the RCS algorithm into two phases, such that given the set of obstacles and the source point, we preprocess a data structure of O$(n^{2})$ size in $O(n^{2}\log n)$ time, and for any query target point we can find the result in $O(n\log n)$ time.

• •

We change the RCS algorithm in such a way to find the best path that approaches the target point from a direction within a specified range of directions. The running time and the space usage of the algorithm do not change.

A preliminary version of this paper is presented in the 7th International Conference on Computer and Knowledge Engineering [18]. In this paper the running time of the algorithm is extremely improved. Furthermore, in this paper we give a complete literature review, consider an extension of the problem in Section 5.2, and present a thorough evaluation of the algorithms.

The remaining of the paper is organized as follows. The problem is formally described in Section 2. In Section 3, we discuss the geometrical concepts needed to solve the problem. We present the algorithm to solve the problem in Section 4. The extensions of the algorithm are described in Section 5 and the experimental evaluations are presented in Section 6. Section 7 concludes the paper.

2 Problem Description

We here formally define the problem. In the plane, a set $O=\{P_{1},P_{2},\dots,P_{h}\}$ of $h$ disjoint simple polygons, called obstacles, which totally have $n$ vertices, a source point $s$, a target point $t$ and two constant values $l$ and $\alpha$ are given. Our goal is to find a path in the exterior space of polygons of $O$, called the free space, from $s$ to $t$ with the following properties:

• •

Minimum route leg length: the path consists of straight line segments, each of length at least $l$.

• •

Connectedness: the consecutive segments are connected at turning points.

• •

Maximum turning angle: the turning angles at the turning points are at most $\alpha$.

From now on, we will refer to the above three properties as simply the path requirements. Figure 1 depicts a set $O=\{P_{1},P_{2},P_{3}\}$ of three obstacles, with $n=14$ vertices, source point $s$ and target point $t$. Each path from $s$ to $t$ is composed of a chain of segments. The illustrated path $\{e_{1},e_{2},e_{3}\}$ in the figure satisfies the path requirements if $l\leqslant|e_{1}|,|e_{2}|,|e_{3}|$ and $\theta_{1},\theta_{2}\leqslant\alpha$.

3 Preliminaries

In this section, we introduce some geometric tools used for solving the problem. In Section 3.1 we define a regular chain of segments and in Section 3.2 the ray shooting data structure is briefly explained.

3.1 Regular chain of segments

In our problem we consider only paths consisting of a sequence of connected line segments. We call such a sequence a chain of segments. Each two consecutive segments have a common endpoint, called the turning point. The angle between a segment and the extension of the previous segment in the turning point is called the turning angle. (Figure 2)

We call a chain of segments with all turning angles equal to $\alpha$ and equal leg lengths a regular chain of segments. Intuitively, it is part of a regular polygon with exterior angle $\alpha$.

We here calculate the position of vertices of a regular chain of segments with different number of turning points, while the position of the start and the end points are known.

Let $C$ denote a regular chain of segments with $A_{0}$ as the starting point, $A_{k+1}$ as the end point, and with $k$ turning points. We assume the $x$-axis is the rightward horizontal line through the origin and the $y$-axis the upward vertical line through the origin. Let $A_{i}$, $1\leqslant i\leqslant k$, be the $i^{\text{th}}$ turning point of $C$. Let $e$ denote the length of the segments of $C$. For ease of illustration, we consider $A_{0}$ as an imaginary turning point and denote its previous point by $A_{-1}$ (Figure 3). Let $\beta$ denote the angle between the extension of $\overline{A_{-1}A_{0}}$ and the $x$-axis. Since $C$ is a regular chain of segments, the angle between the extension of $\overline{A_{-1}A_{0}}$ and $\overline{A_{0}A_{1}}$ is $\alpha$. Let the angle between $\overline{A_{0}A_{1}}$ and $\overline{A_{0}A_{k+1}}$ (the direct line segment from the starting point to the end point) be denoted by $\theta$.

Draw a line from $A_{0}$ parallel to the $x$-axis and project $A_{1}$ on this line. Let $H$ denote the projection point (Figure 3). In the right triangle $\vartriangle A_{0}HA_{1}$ we have

[TABLE]

[TABLE]

Similarly, we can draw a horizontal line through $A_{1}$ and project $A_{2}$ on this line and get the following equations

[TABLE]

[TABLE]

As it follows, the position of each point $A_{i}$, can be calculated from the following recurrence relations

[TABLE]

[TABLE]

Since $e=|A_{i-1}A_{i}|$, for all $1\leqslant i\leqslant k+1$, we solve the above recurrence relations as follows

[TABLE]

where $j=1,\dots,k+1$.

In a simple polygon, the sum of the interior angles with $d$ vertices is equal to $(d-2)\pi$. If we connect $A_{0}$ to $A_{k+1}$, we have a simple polygon. In this polygon, the interior angle at $A_{0}$ and $A_{k+1}$ are equal, because $C$ is part of a regular polygon. Let $d$ be the number of vertices of the polygon. Since $k$ is the number of turning points in $C$, we have $k=d-2$ and so the sum of the interior angles of the simple polygon is $(d-2)\pi=k\pi$. The interior angle of the first and the last vertices in the chain is $\theta$ and the other angles are $\pi-\alpha$. Therefore we have $k(\pi-\alpha)+2\theta=k\pi,$ or equivalently $\theta=\frac{k\alpha}{2}.$

Let the angle between $\overline{A_{0}A_{k+1}}$ and the $x$-axis be denoted by $\gamma$. If we know the position of $A_{0}$ and $A_{k+1}$, we have the value of $\gamma$. Since $\gamma=\beta+\alpha+\theta$, we can rewrite this equation to have $\beta$ on the left hand side as $\beta=\gamma-(\alpha+\theta)$. Therefore, we have the value of $\beta$. If we further know the number of turning points of $C$, i.e. $k$, the length of segments of $C$ can be computed with each of the following equations

[TABLE]

Obtaining the value of $e$, the position of each vertex in the chain will be identified by Equations 1 and 2.

With the above method, given the starting and the end points and the number of turning points, we can easily determine the position of the turning points of the regular chain of segments.

3.2 Ray shooting

One of the problems we encounter for path planning is to recognize whether a segment has any intersection with a set of segments. Formally, a set of segments $S=\{s_{1},s_{2},\dots,s_{n}\}$ are given. We are asked if segment $t$ intersects at least one of the segments of $S$.

It is obvious that this problem could be solved by checking the intersection of $t$ with each segment of $S$ in $\Theta(n)$ total time. But, if the problem must be solved a lot of times for a fixed $S$, an algorithm with less query time is more favorable.

We use a ray shooting algorithm for this problem. In the ray shooting problem, a set of segments $S$ is known. We need to preprocess $S$ such that upon receiving a query ray (its start point and direction), the first intersection position of the ray with the segments of $S$ will be detected quickly. We should also detect the case in which the ray does not intersect any segment of $S$.

By the solution of the ray shooting problem, the problem of segment intersection with a set of segments will be solved as follow: First, we preprocess $S$ for ray shooting. During the query time by receiving segment $t$, we shoot a ray from one end-point of $t$ in the direction of the other end-point. If the first intersection point of the ray with $S$ is between the two end-points of $t$, the answer to our solution is positive, that is, $t$ has an intersection with $S$. On the other hand, if the intersection point is not between the end-points of $t$ or there is not any intersection at all, the answer to our problem is negative.

There are many different algorithms for the ray shooting problem. But an old algorithm due to Chazelle [3] is well suited to our purpose. In this algorithm, after preprocessing in $O(n^{2}\log n)$ time and using $O(n^{2})$ space, the ray shooting queries are answered in $O(\log{n})$ time.

4 RCS: Path Planning Algorithm

In this section, we describe our algorithm for finding a path with the given requirements. Since we prefer to make the path smooth, we try to spread out the break points along the path evenly. Therefore, we use regular chains of segments as sub-paths when moving from one obstacle vertex to another one, and so we call our algorithm Regular Chains of Segments Path Planner, or simply RCS.

Let $u$ and $v$ be two obstacle vertices. We want to find all sub-paths in the free space from $u$ to $v$ not passing through any other obstacle vertices. To do so, we examine all regular chains of segments with different number of break points starting at $u$ and ending at $v$. If all edges of a chain do not intersect any edges of the obstacles, then the chain is considered as a valid sub-path from $u$ to $v$. In other words, we first look at the regular chain of segments without any break point, that is the straight line segment from $u$ to $v$. If the only edge of this chain does not intersect any obstacle then we consider this chain as a valid path from $u$ to $v$. Similarly, we examine the regular chain of segments with one break point, two break points, and so on. We continue this process until the distance between two adjacent break points in the chain gets smaller than $l$. If the first and the last points of a regular chain are fixed, the distance between two consecutive break points decreases when the number of break points increases (the last result of Section 3.1). Therefore there are a limited number of valid regular chains of segments from $u$ to $v$.

Assume the moving object follows a given regular chain of segments from $u$ to $v$ when traveling from $s$ to $t$. Because of the maximum turning angle property, when the object moves away from $v$, its leaving direction must be within a fixed range, determined by the direction it enters $v$. This observation is depicted in Figure 4. Following chain $c_{0}$, where the object moves from $u$ directly towards $v$, it can leave $v$ in the same direction and without any rotation at $v$, or rotate by an angle at most equal to $\alpha$, clockwise or counter-clockwise. Therefore, the object leaves $v$ in a direction in range $[\theta_{1},\theta_{2}=\theta_{1}+2\alpha]$. Similarly, for chain $c_{1}$ with seven break points, the valid leaving direction is in range $[\phi_{1},\phi_{2}=\phi_{1}+2\alpha]$; $\phi_{1}$ is the angle of the path if the object rotates at $v$ by an angle equal to $\alpha$ clockwise, and $\phi_{2}$ is the angle of the path when the object rotates by an angle equal to $\alpha$ counter-clockwise. We call this range of directions for a regular chain of segments $c$, the valid leave range of $c$, and denote it by $\operatorname{VLR}(c)$.

4.1 Graph construction

The key idea of our approach is to construct a weighted directed multigraph ${\cal G}=(V_{{\cal G}},E_{{\cal G}})$, such that any path from the starting point $s$ to the target point $t$ in the original configuration, consisting only of regular chains of segments, corresponds to a path between analogous nodes in ${\cal G}$, of the same length. The problem will then reduce to finding the shortest path in the graph.

Graph ${\cal G}$ is constructed as follows. We add the starting point $s$ and the target point $t$ to the set of obstacle vertices. For each obstacle vertex $v$, we add a corresponding node to ${\cal G}$. For each pair of nodes $u,v$ of ${\cal G}$, and for each valid regular chain of segments $c$ between the corresponding vertices, we add two directed edges, one from $u$ to $v$, and the other from $v$ to $u$. We add two directed edges because $c$ does not have a direction and the object can follow $c$ in both directions, and each edge represents one of the directions. We say edge $e=(u,v)$ is incident from $u$ and incident to $v$, which means $u$ (resp. $v$) is the head (resp. tail) of $e$. The weight of $e$, denoted by $e.w$ is equal to the length of $c$. In addition, we store a reference to $c$, denoted by $e.c$, for further uses. It is possible to have more than one valid regular chain of segments between two obstacle vertices. So, there may be multiple edges between two nodes of ${\cal G}$, and as we indicated before, ${\cal G}$ is a multigraph.

Lemma 1**.**

For a polygonal domain of $n$ vertices, the number of nodes of ${\cal G}$, $|V_{{\cal G}}|$, is $n+2$ and the number of edges, $|E_{{\cal G}}|$, is $O(n^{2})$. We can construct ${\cal G}$ in $O(n^{2}\log n)$ time.

Proof.

For each obstacle vertex, we add an equivalent node to ${\cal G}$. Furthermore, we add two equivalent nodes for $s$ and $t$. Therefore, the number of nodes of ${\cal G}$ is $n+2$.

For each pair of vertices $u$ and $v$, we have at most $\frac{2\pi}{\alpha}$ valid regular chains of segments. This is because the number of break points in a regular polygon with exterior angle $\alpha$ is $\frac{2\pi}{\alpha}$. For each valid regular chain of segments, two edges is added to ${\cal G}$. In summary, the maximum number of edges is ${n\choose 2}\cdot 2\cdot 2\pi/\alpha$ which is $O(n^{2})$.

For constructing the graph, we first preprocess the set of obstacle edges for ray shooting, as described in Section 3.2. There are $n$ segments, and the preprocessing takes $O(n^{2}\log n)$ time. Then, for each pair of obstacle vertices, and each possible regular chain of segments between them, we see whether it is a valid regular chain. The validity test is done by checking the intersection of segments of the chain with obstacle edges. If any segment of the chain has an intersection with an obstacle edge, the chain is not valid. Otherwise, it is a valid chain and we add the corresponding edges to ${\cal G}$. There are ${n\choose 2}\cdot 2\pi/\alpha$ possible chains, each consisting of at most $2\pi/\alpha$ segments, and executing a ray shooting query for each one takes $O({n\choose 2}\cdot(2\pi/\alpha)^{2}\log n)=O(n^{2}\log n)$ total time. ∎

The above bound for the number of edges of ${\cal G}$ is clearly the worst case upper bound. This is because in most cases obstacle edges block regular chains and preclude them from being valid.

Lemma 2**.**

Any path in the polygonal domain from vertex $s$ to vertex $t$ of length $l$ that consists of only regular chains of segments has a corresponding path between the analogous nodes in graph ${\cal G}$ of length $l$.

Proof.

We construct ${\cal G}$ such that any valid regular chain of segments from a vertex $u$ to a vertex $v$ has a corresponding edge between the analogous nodes in the graph with the weight equal to the length of the chain. Therefore, we can establish a one to one mapping between paths in the polygonal domain which consists of only regular chains of segments, and paths in ${\cal G}$. ∎

4.2 Finding the shortest path in the graph

From Lemma 2, we conclude that if we need the shortest path between two vertices consisting of only regular chains of segments that satisfies some properties in the polygonal domain, we can look for the shortest path in ${\cal G}$ between the analogous nodes with the same properties.

The best algorithm for finding the shortest path between two specific nodes in a weighted graph is Dijkstra’s algorithm [4], with $O(|E_{\cal G}|+|V_{\cal G}|\log|V_{\cal G}|)$ running time using Fibonacci heap data structure. But the problem here is that Dijkstra’s algorithm is not directly applicable to ${\cal G}$, because not all paths in ${\cal G}$ are acceptable.

When the object moves on a path, because of the maximum turning angle property, it could not have sharp turns at obstacle vertices. Therefore, the angle between two consecutive regular chains of segments must be less than or equal to $\alpha$. Since in Dijkstra’s algorithm, edges are selected without any concern, the result of the algorithm may not satisfy the path requirements.

It is possible to transform ${\cal G}$ into a new graph and apply Dijkstra’s algorithm to it to find the shortest path which satisfies the path requirements. But the large size of the new graph is another trouble. The graph has $O(|E_{\cal G}|)$ vertices and $O(\sum_{v\in V_{G}}\operatorname{out-deg}(v^{2}))$ edges, where $\operatorname{out-deg}(v)$ is the out-degree of $v$, i.e. the number of edges incident from $v$. If we apply Dijkstra’s algorithm on this graph, the running time of the algorithm will be $O(|E_{\cal G}||V_{\cal G}|)$ [1, 5], which in the worst case will lead to $O(n^{3})$.

We here introduce an algorithm to find the shortest path with the path requirements, whose main idea is borrowed from Boroujerdi and Uhlmann [1]. In this algorithm, no new graph is generated and we only modify Dijkstra’s algorithm to find the shortest path with the given requirements. The running time of this algorithm will be $O(|E_{\cal G}|\log|V_{\cal G}|)$, which is $O(n^{2}\log n)$ in the worst case.

Let $v$ be a node of ${\cal G}$ and $e$ be an edge incident from $v$. Let $\operatorname{angle}(e,v)$ denote the angle of the first segment of regular chain of segments $e.c$ traversed when moving away from $v$. At the beginning of the algorithm, we sort the outgoing edges of each node $v$, according to their angles, in counter-clockwise order (see Figure 5). We store the sorted list in $v.\textit{out-edges}$, a data structure capable of searching and deleting items in logarithmic time, like an AVL tree.

For each edge $e$ incident to $v$, when the object arrives at $v$ through $e.c$, only a subset of the outgoing edges of $v$ are applicable for departing from $v$. The angle of the first segment of these edges must be in range $\operatorname{VLR}(e.c)$. We denote this valid successors of $e$ by $\operatorname{VS}(e)$. If the object comes to $v$ via $e.c$, it can leave $v$ following the chain of any edge in $\operatorname{VS}(e)$, without violating the maximum turning angle requirement. It is easy to see that the edges of $\operatorname{VS}(e)$ are placed in $v.\textit{out-edges}$ consecutively. The following lemma gives an upper bound on the time needed to find $\operatorname{VS}(e)$ for an edge $e$.

Lemma 3**.**

Given a graph ${\cal G}$ such that the outgoing edges of each vertex are sorted in counter-clockwise order based on the first edge of their chains, it is possible to find $\operatorname{VS}(e)$ for an edge $e$ incident to $v$ in $O(\log(\operatorname{out-deg}(v))+|\operatorname{VS}(e)|)$ time.

Proof.

Let $v$ be a vertex of ${\cal G}$, and $e$ be an edge incident to $v$. Let $\operatorname{VLR}(e.c)=[\theta_{1},\theta_{2}]$. We can easily find the position of $\theta_{1}$ (resp. $\theta_{2}$) in $v.\textit{out-edges}$ in $O(\log(\operatorname{out-deg}(v))$ time. This is done by applying two binary searches with $\theta_{1}$ and $\theta_{2}$ as keys on $v.\textit{out-edges}$, and reporting all edges whose angles are between the keys. The binary search takes $O(\log(\operatorname{out-deg}(v)))$ time, and reporting edges in $\operatorname{VS}(e)$ sequentially takes $O(|\operatorname{VS}(e)|)$ time, which proves the claim. It is necessary to note that since the outgoing edges are arranged around $v$ circularly, $\theta_{2}$ might be smaller than $\theta_{1}$. If this is the case, the report will consist of all edges whose angle is greater than $\theta_{1}$ or smaller than $\theta_{2}$. ∎

We now describe the modified Dijkstra’s algorithm for this problem. Instead of processing nodes of the graph as Dijkstra’s algorithm, we insert edges into a priority queue and process edges with minimum priority successively. For each edge $e=(u,v)$, we assign a value $\operatorname{dist}(e)$, whose initial value is $\infty$. This value denotes the length of the shortest path to $v$, if the object starts from $s$ and finally passes through $e.c$. Furthermore, $\operatorname{dist}(e)$ plays the role of the priority in the priority queue used in the algorithm. We store another value $\operatorname{pred}(e)$, for each edge $e$, which denotes the last edge before $e$, in the shortest path to $e$. The predecessor value is initially set to null. This value helps to construct the shortest path at the end of the algorithm.

The algorithm starts by processing each edge $e$ incident from $s$, the starting point. We set $\operatorname{dist}(e)=e.w$, and insert $e$ into the priority queue, $Q$, with $\operatorname{dist}(e)$ as its priority. Now, as long as $Q$ is not empty, we remove edge $e$ with the minimum priority from $Q$. If $e$ is incident to $t$, the target point, we are done, and report $\operatorname{dist}(e)$ as the shortest path distance from $s$ to $t$, and $e$ and the sequence of predecessors as the shortest path. In the other case, where $e$ is not incident to $t$, but incident to some other node $v$, we process edges of $\operatorname{VS}(e)$, one by one. For each edge $e^{\prime}\in\operatorname{VS}(e)$, we remove $e^{\prime}$ from $v.\textit{out-edges}$, set $\operatorname{dist}(e^{\prime})=\operatorname{dist}(e)+e^{\prime}.w$ and $\operatorname{pred}(e^{\prime})=e$, and finally insert $e^{\prime}$ into $Q$ with priority $\operatorname{dist}(e^{\prime})$. We here remove $e^{\prime}$ from $v.\textit{out-edges}$ because we have found the shortest path to $e^{\prime}$ and $\operatorname{dist}(e^{\prime})$ achieves its final value. Therefore, it cannot be updated furthermore, and it is not required to be checked for other segments incident to $v$. This process continues and edges of ${\cal G}$ are explored successively until we arrive at $t$. Because ${\cal G}$ is connected, we will eventually get into $t$.

The outline of RCS Path Planning algorithm is presented in Algorithm 1.

4.3 Analysis of the algorithm

In Lines 1-11, the graph is constructed, which takes $O(n^{2}\log n)$ time, as described before in Lemma 1. Lines 12-13 sort the outgoing edges of nodes and need $O(\sum_{v\in V_{\cal G}}\operatorname{out-deg}(v)\log(\operatorname{out-deg}(v)))=O(|E_{\cal G}|\log(|V_{\cal G}|))$ time. Lines 14-18 initialize the priority queue and process the outgoing edges of the starting point, and take $O(\operatorname{out-deg}(s)\log(\operatorname{out-deg}(s)))$ time which is $O(|V_{\cal G}|\log|V_{\cal G}|)$. In Lines 19-28, each edge of the graph is inserted into $Q$ at most once, and is removed from $Q$ at most once, and removed from the list of the outgoing edges of its head at most once. These operations take $O(|E_{\cal G}|\log|E_{\cal G}|)=O(|E_{\cal G}|\log|V_{\cal G}|)$ total time. Finding $\operatorname{VS}(e)$ for each edge $e$, as proved in Lemma 3, takes $O(\log|V_{\cal G}|+|\operatorname{VS}(e)|)$ time. Since each edge is considered only in one $\operatorname{VS}(e)$, and is removed from the list of outgoing edges of its head, the total time spends for lines 25-28 is $O(|E_{\cal G}|\log|V_{\cal G}|)$. Therefore, the whole running time of the algorithm will be $O(n^{2}\log n+|E_{\cal G}|\log|V_{\cal G}|)$, which is $O(n^{2}\log n)$. In the algorithm, the space is used for storing the ray shooting data structure, ${\cal G}$ and $Q$, which is $O(n^{2}+|E_{\cal G}|)=O(n^{2})$.

The following theorem summarizes our results about finding the shortest path consisting of regular chains of segments.

Theorem 1**.**

The RCS algorithm finds the shortest path from $s$ to $t$, which consists of regular chains of segments, and satisfies the path requirements, in $O(n^{2}\log n)$ time using $O(n^{2})$ space.

5 Extensions

In this section we consider two extensions of the problem and try to use our technique to solve these extensions. In the first problem there are several target points that are to be processed with a single starting point, and we have to find the best path to reach each target point. In the second extension, we solve the problem assuming that we have to arrive at the target point from a given range of directions.

5.1 Problem in the query mode

In the original problem, it was assumed that the target point $t$ is given at the same time as the other inputs. In this section we divide the algorithm into two parts, a preprocessing phase, and a query phase. The set of obstacles and the starting point are given in the preprocessing phase, and in the query phase the target point is determined. The problem in this mode is suitable when we want to find the best time/path to reach a moving target point in a fixed environment.

We solve the problem in the query mode as follows. Given the set of obstacles and the starting point in the preprocessing phase, we construct graph ${\cal G}$ and run Dijkstra’s algorithm on ${\cal G}$ to compute $\operatorname{dist}(e)$ for all edges of ${\cal G}$. An edge $e=(u,v)$ is a candidate predecessor for each edge $e^{\prime}$ incident from $v$ if $\operatorname{angle}(e^{\prime},v)$ lies in $\operatorname{VLR}(e.c)$. It is the actual predecessor of $e^{\prime}$, if among all such candidates, $\operatorname{dist}(e)$ has the smallest value. With this in mind, for each node $v$, we keep a sorted list of ranges around $v$ such that the predecessor of edges in each range is fixed. We store this list, denoted by $v.\textit{preds}$, in an AVL tree, so that the insertion and searching takes logarithmic time. This tree can easily be constructed, as Dijkstra’s algorithm is run on ${\cal G}$. At the beginning, $v.\textit{preds}$ is empty for each node $v$. When the first edge $e$ incident to $v$ is processed, $\operatorname{VLR}(e.c)$ is added to $v.\textit{preds}$ and $e$ is linked to it. During the execution of the algorithm, when a new edge $e^{\prime}$ incident to $v$ is being processed, it may add a new range to $v.\textit{preds}$. Since the edges are processed in increasing order of their distance, $e^{\prime}$ is predecessor only in a range where no previous predecessor is assigned. Furthermore, for each edge $e$, $\operatorname{VLR}(e.c)$ covers a range of size $\alpha$. Therefore, for each edge $e$, if $e$ is predecessor for some range, it will be a connected range, which can easily be found and inserted to $v.\textit{preds}$ in $O(\log n)$ time. Since the number of incoming edges to a vertex $v$ is $O(n)$, the number of ranges in $v.\textit{preds}$ is also $O(n)$.

In the query phase, given the target point $t$, we consider each node $v$ in ${\cal G}$ and find all valid regular chains of segments from $v$ to $t$. We here need to execute a ray shooting for each segment of a chain to see if it is valid. For each valid chain, we add an edge $e$ from $v$ to $t$ in ${\cal G}$. Furthermore, we find the predecessor of $e$ using $v.\textit{preds}$ in $O(\log n)$ time and accordingly assign $\operatorname{pred}(e)$. Finally we add the distance of $s$ to $\operatorname{pred}(e)$ to the length of $|e.c|$ and store it into $\operatorname{dist}(e)$. After processing all edges to $t$, the final solution is an edge $e$ incident to $t$ with the smallest value of $\operatorname{dist}(e)$, along with the sequence of predecessors.

Theorem 2**.**

For the problem of path planning with the path requirements, we can preprocess a data structure in $O(n^{2}\log n)$ time using $O(n^{2})$ space, to answer the query for any target point $t$ in $O(n\log n)$ time. In these upper bounds $n$ is the total number of obstacle vertices.

Proof.

We construct the graph in $O(n^{2}\log n)$ time using $O(n^{2})$ space and run Dijkstra’s algorithm in $O(|E_{\cal G}|\log|V_{\cal G}|)$ time as proved in Lemma 1 and Theorem 1, respectively. In the query time, we need to find and process each edge to $t$ in $O(\log n)$ time to find a possible path to $t$. Since there are at most $O(n)$ edges to $t$, processing all edges and choosing the shortest path to $t$ takes $O(n\log n)$ time. ∎

5.2 Approaching from a given direction

In some path planning applications, it is desirable to reach the target from a given direction. For example the most damage will take place when a missile hits the target orthogonally, or a UAV landing on an aircraft carrier needs to approach it from a fixed direction. In this section we describe how we can use the method to achieve this goal.

In this version of the problem, in addition to the previous inputs, a range of directions like $\Theta=[\theta_{1},\theta_{2}]$ is given, and we want to approach the target from a direction in range $\Theta$.

In Section 4 we described how to construct a graph ${\cal G}$ that models paths only consisting of regular chains of segments. Each edge $e=(u,v)$ in this graph has the following meaning: There exists a regular chain of segments $e.c$ from obstacle vertex $u$ to obstacle vertex $v$ which leaves $u$ and arrives at $v$ in a fixed direction determined by $e.c$.

Therefore, the solution is as follows. We continue the while loop of the algorithm until the edge that is processed comes into $t$ with an angle in range $\Theta$, or $Q$ becomes empty. It is obvious that in the former case the resulting path reaches $t$ in a direction in the desirable range, and in the latter case, there is no path with the given requirements.

By the above argument we conclude the following theorem:

Theorem 3**.**

In the problem of path planning with a fixed approaching direction, a path from the starting point $s$ to the target point $t$ with the required properties can be found in $O(n^{2}\log n)$ time using $O(n^{2})$ space, where $n$ is the total number of obstacle vertices.

Furthermore, we can preprocess a data structure in $O(n^{2}\log n)$ time using $O(n^{2})$ space, to answer the query version of the problem for any target point $t$ in $O(n\log n)$ time.

6 Algorithm Evaluation

Even though the running time and the space usage of the proposed algorithms are bearable, the dependency in the upper bounds is on the number of obstacle vertices and not on the scene size. In most applications the area in which the algorithm works is extremely large. But the complexity of the scene in terms of the number of obstacle vertices is not considerable. This can be made more rigorous if we apply a simplification step on the input to decrease the number of obstacle vertices.

In this section we perform several experiments to evaluate the RCS algorithm and show its effectiveness in practice. We compare the results to the results of the algorithm designed by Szczerba et al. [23]. We chose their algorithm because they solved the most similar problem to the problem considered in this paper. Their algorithm is based on the A* search algorithm, and we called it A* hereinafter. The evaluations are performed based on seven test cases. The source codes of the RCS and A* algorithms are available at the GitHub repository hosting service [17, 16].

In the first experiment, we run RCS and A* on a fixed complex polygonal domain from a given source point to several target points. The configuration is depicted in Figure 6. In this figure, $s$ is the source point and $t_{1},\dots,t_{5}$ are five different target points. We use the maximum turning angle $\alpha=\pi/6$ and the minimum leg length $l=50$ as the parameters for this test case. The target points are selected such that paths to each one have different number of turning points.