Abstract
In this paper, a three-dimensional path planning problem of an unmanned aerial vehicle under constant thrust is studied based on the artificial fluid method. The effect of obstacles on the original fluid field is quantified by the perturbation matrix, the streamlines can be regarded as the planned path for the unmanned aerial vehicle, and the tangential vector and the disturbance matrix of the artificial fluid method are improved. In particular, this paper addresses a novel algorithm of constant thrust fitting which is proposed through the impulse compensation, and then the constant thrust switching control scheme based on the isochronous interpolation method is given. It is proved that the planned path can avoid all obstacles smoothly and swiftly and reach the destination eventually. Simulation results demonstrate the effectiveness of this method.
1. Introduction
Unmanned aerial vehicles have the characteristics of high flexibility, low cost, high safety, and strong concealment. These unique superior performances have enabled the rapid development of unmanned aerial vehicle technology and gradually become the representative technology of the world’s cutting-edge technology. They are widely used in various fields such as military reconnaissance, urban express delivery, terrain exploration, and environmental monitoring. The purpose of the unmanned aerial vehicle three-dimensional path planning is to find the optimal path between the initial position and the target position under the constraints of the unmanned aerial vehicle and the environment.
However, unmanned aerial vehicles still face a challenge; that is, it is difficult to ensure the safety and reliability of flight paths because unmanned aerial vehicles encounter obstacles and threats, such as high-rise buildings and enemy air-defense systems. To address the challenge, a path planning problem has been actively studied, which derives a flight path for an unmanned aerial vehicle from a point to another with respect to navigating a region of interest safely. Various operational constraints such as the maximum energy level of an unmanned aerial vehicle and safety distance from an object (e.g., a building or another unmanned aerial vehicle) are often imposed into the problem.
Following the importance of the path planning in unmanned aerial vehicle deployment, various approaches based on exact and heuristic approaches have been proposed. Model predicted control (MPC) method [1], Voronoi method [2], intelligent algorithms (e.g., genetic algorithm and particle swarm optimization) [3–5], rapidly exploring random tree (RRT) method [6], and artificial potential field (APF) [7] are some of the typical algorithms. The improved rapidly exploring random tree (RRT) method produces a time parameterized set of control inputs to make the robot move from the initial point to the destination, which proves to be efficient for 3D path planning. Artificial potential field (APF) method has the advantages of simple principle and real-time computation [8, 9], but there exists local minimum when the robot enters into a concave area. Besides, it is hard to obtain a feasible path sometimes even if the magnitude of the attractive or repulsive force is regulated.
The intelligent algorithms, such as particle swarm optimization [10], evolutionary algorithms [5], and ant colony algorithm [11], are also widely used in 3D path planning. These methods can be easily employed in different environments, but it is possible to trap in a local optimum. However, the abovementioned drawback can be relieved when the intelligent methods are improved or combined with other methods [12–16]. These traditional approaches are improved to solve the three-dimensional path planning problem. However, the calculation of these algorithms tends to increase exponentially if the planning space enlarges. Besides, the planned path may be not smooth enough for the robot to track. As a result, extra strategy of the smoothing path is usually needed. Several research studies targeted 3D path planning in order to plan a feasible and smooth path. A novel algorithm based on the disturbed fluid and trajectory propagation is developed to solve the 3D path planning problem of an unmanned aerial vehicle in static environment [17]. The core path graph algorithm [18] calculates the core path graph where arcs are minimum-length trajectories satisfying geometrical constraints and searches the optimal trajectory between two arbitrary nodes of the graph. However, multiple quadratics should be resolved, resulting in low computational efficiency. In addition, constant thrust collision avoidance maneuver in path planning is studied in our previous studies [19, 20].
In this paper, a novel algorithm of constant thrust fitting is proposed through the impulse compensation for constant thrust maneuver of an unmanned aerial vehicle, and the tangential vector and the disturbance matrix of the artificial fluid method is improved by combing the interfered fluid dynamical system. Although the physical characteristics of the modified streamlines are broadened, they still conform to the basic properties of fluid flow, i.e., smoothness, impenetrability, and accessibility.
The rest of the paper is organized as follows. Section 2 focuses on the calculation of the shortest distance between the unmanned aerial vehicle and the obstacle. Section 3 explains 3D path planning based on the improved artificial fluid method. Section 4 describes constant thrust collision avoidance maneuver. The simulation results are given in Section 5. Section 6 concludes the paper.
2. Calculation of the Shortest Distance between Unmanned Aerial Vehicle and Obstacle
The purpose of an unmanned aerial vehicle is to avoid obstacles and reach the destination. In this paper, the obstacle is described approximately as a 3D space surface. We define the relative motion coordinate system as the path planning space, where the origin is the center of the obstacle, and is taken as the position of the unmanned aerial vehicle relative to the obstacle. Suppose the parametric equations of the obstacle’s space curved surface as follows:where and . Assuming that is any point on the parametric surface (1), is transformed as , then the normal vector at point iswhere and are the partial derivatives of on and . Obviously, in order to get the minimum distance between and , should be parallel to the normal vector , that is to say,
Equation (3) can be written as follows:where are the nonlinear equations. Then, equation (4) can be written as follows:
The derivative matrix of equation (5) can be written as the following form:
Equation (6) is a first-order partial differential equation, and we can directly find its analytical solution or use the toolbox in MATLAB to get its analytical solution. We assume that is the analytical solution of equation (6), then the following results can be obtained by using the Taylor formula of multivariate functions:where and , , and is the point within the line segment between and . In order to calculate quickly, the linear part of equation (7) is considered instead of equation (7):
The solution of equation (8) can be seen as an approximate solution of . Then, equation (11) can be transformed into the form of a matrix equation:
Thus, the least square solution of can be obtained as follows:
Therefore, the point on the obstacle’s space curved surface minimizes the distance between the unmanned aerial vehicle and the obstacle. The shortest distance between the unmanned aerial vehicle to the obstacle is , where represents the modular of the vector from to .
3. 3D Path Planning Based on the Artificial Fluid Method
Based on the description in Section 2, the procedure for 3D path planning is as follows. First, the perturbation matrix is calculated [17]. Next, we calculate the disturbed fluid velocity by modifying the target velocity . Then, the planned path is obtained by the recursive integration of . Finally, constant thrust collision avoidance maneuver is studied and the switching control scheme based on the isochronous interpolation method is given. To describe the influence of the obstacle on the original flow, the perturbation matrix is defined as follows:where is a identity matrix, is a column vector given by equation (2), is a tangential vector (the partial derivative of on ) at the point , is a saturation function defining the orientation of tangential velocity, is the 2-norm of a vector or a matrix, and and are defined as the weight of and , respectively:where is the repulsive parameter, is the tangential parameter, is the maximum radius of the unmanned aerial vehicle, and is a small positive threshold of the saturation function . Then, the disturbed fluid velocity can be calculated by
3.1. The Planned Path Can Avoid Obstacles Safely
To avoid possible collisions, an unmanned aerial vehicle cannot approach obstacles indefinitely, so we introduce the maximum radius of the unmanned aerial vehicle , i.e., the distance between the boundary of the unmanned aerial vehicle and the obstacle should be greater than . Suppose that and is a monotonically decreasing function:
It can be inferred that , then , and ; therefore, . can be simplified as
Because vectors and are perpendicular exactly, i.e., , and the equation means that , the path is outside of the minimum permitted distance and there is no collision.
3.2. The Planned Path Can Reach the Destination Eventually
Because the goal of the path planning is to make the unmanned aerial vehicle reach the destination safely, the velocity of the unmanned aerial vehicle should have a component in the direction of the target velocity, i.e., velocity and should satisfy , and the planned path will converge to the target point. Besides, should be satisfied if the unmanned aerial vehicle is near to the destination :where is the inner product of vectors, where denotes the angle between and . It is obvious that when the unmanned aerial vehicle approaches the destination, then :
As and hold, we infer . From equation (14), we infer . Therefore, holds.
When the unmanned aerial vehicle approaches the destination, thus and . From equation (11), it can be inferred that . Therefore, holds.
3.3. Analysis of the Disturbed Fluid Velocity
The modified velocity defined by equation (15) can be expressed as
It can be seen from equation (20) that consists three parts: can be called the target velocity; is taken as the repulsive velocity; can be called the tangential velocity. Similarly, the perturbation matrix can be divided into three parts: attractive matrix , repulsive matrix , and tangential matrix . It can be analyzed that the magnitudes of repulsive and tangential velocities increase with and , respectively. Therefore, we can readjust the shape of the path by changing parameters and . This method is similar to the virtual force method or then the artificial potential field method to some degree. However, the perturbation matrix by this method can describe the effect of obstacles on path more objectively, considering the shape of obstacles and the position of the unmanned aerial vehicle.
4. Constant Thrust Collision Avoidance Maneuver
Suppose that the time of the unmanned aerial vehicle’s collision avoidance maneuver is and the shortest switching time interval is . There are shortest switching time intervals and target maneuver positions, and represents the time of the i-th thrust arc. The process of collision avoidance maneuver can be considered as the system state variables change from a nonzero initial state to a desired state :
Suppose that the actual constant thrusts of the unmanned aerial vehicle is , the maximum thrusts is , and the theoretical continuous thrusts is . The thrusters can provide different sizes of constant thrust to meet different thrust requirements. There are different sizes of constant thrust which can be denoted as follows:
The size of the constant thrust is calculated as follows: there are thrust levels which can be selected; , and the level of the constant thrust can be calculated as follows, taking the i- thrust arc as example:where [] means bracket function and means the absolute value of .
4.1. Constant Thrust Fitting through the Impulse Compensation
The constant thrust fitting should be discussed in several categories; for convenience, let us take the -th thrust arc as example.
Case 1. If the theoretical working time in the -th thrust arc , then the actual constant thrust is .
Case 2. If the theoretical working time in the -th thrust arc , then the constant thrust fitting should be discussed in several subcategories.
Case 3. If the theoretical working time in the -th thrust arc and can be any one of the shortest switching time interval in the -th thrust arc; without loss of generality, we suppose that is the first shortest switching time interval, and the impulse error in the -th thrust arc can be calculated. Suppose that there are thrust levels which can be selected: , and the level of the constant thrust can be calculated as follows:where [] means bracket function and means the absolute value of . Then, the impulse error can be calculated as follows:Suppose that the value of the impulse compensation threshold is a positive constant :(1)If the impulse error satisfies the following condition: and the actual constant thrust of the unmanned aerial vehicle can be calculated as follows: then the unmanned aerial vehicle will not carry out impulse compensation.(2)If the impulse error satisfies the following condition: then the unmanned aerial vehicle should carry out impulse compensation, and the size of the constant thrust impulse compensation can be calculated as follows:
Case 4. If the theoretical working time in the -th thrust arc , then the impulse error in the -th thrust arc can be calculated as follows:Furthermore, if there exist shortest switching time intervals satisfying the following conditions, without loss of generality, we suppose that these time intervals are the first shortest switching time intervals, taking the -th shortest switching time interval as an example:then the size of the impulse compensation can be calculated as follows:(1)If the impulse error satisfies the following condition: and the actual constant thrust of the unmanned aerial vehicle can be calculated as follows, taking the -th shortest switching time interval as an example: then the unmanned aerial vehicle will not carry out impulse compensation.(2)Suppose that Furthermore, if the impulse error satisfies the following condition: and the actual constant thrust of the unmanned aerial vehicle can be calculated as follows: then the unmanned aerial vehicle will not carry out impulse compensation.(3)If the impulse error satisfies the following condition: then the unmanned aerial vehicle should carry out impulse compensation, and the size of the constant thrust impulse compensation can be calculated as follows:
Case 5. If the theoretical working time in the -th thrust arc and can be any shortest switching time interval in the -th thrust arc; without loss of generality, we suppose that is the first shortest switching time interval, and the impulse error in the -th thrust arc can be calculated as follows:Furthermore, if there exist shortest switching time intervals satisfying the following conditions, without loss of generality, we suppose that these time intervals are the first shortest switching time interval, taking the -th shortest switching time interval as an example:Then, the size of the impulse compensation can be calculated as follows:(1)If the impulse error satisfies the following condition: and the actual constant thrust of the unmanned aerial vehicle can be calculated as follows, taking the -th shortest switching time interval as an example: then the unmanned aerial vehicle will not carry out impulse compensation.(2)Suppose that Furthermore, if the impulse error satisfies the following condition: and the actual constant thrust of the unmanned aerial vehicle can be calculated as follows: then the unmanned aerial vehicle will not carry out impulse compensation.(3)If the impulse error satisfies the following condition: then the unmanned aerial vehicle should carry out impulse compensation, and the size of the constant thrust impulse compensation can be calculated as follows:
4.2. Compare Fuel Consumption and Design Switch Control Laws
The fuel consumption under the theoretical continuous thrust and under the actual constant thrust is compared from the perspective of impulse compensation. We have already calculated the different impulse compensation according to the different conditions in Section 4, then the fuel savings under the actual constant thrust can be calculated as follows. Without loss of generality, taking the fuel savings in Case 5 as an example, we suppose that the mass flow rate of the propellant of the unmanned aerial vehicle’s thruster is assumed to be . Since the impulse error is as follows:then the fuel savings in the -th thrust arc can be calculated as follows:
There are three types of time intervals in each thrust arc: the accelerating time intervals, the zero-thrust time intervals, and the decelerating time intervals. The task of the unmanned aerial vehicle collision avoidance maneuver is converted into the calculation of the number and sequence of three types of time intervals, respectively. In this section, the fuel consumption under the theoretical continuous thrust and the actual constant thrust is calculated and compared by using the method proposed in this paper. At last, the actual constant thrust switch control laws are obtained through the isochronous interpolation method, without loss of generality and taking Case 5 as an example. If the impulse error satisfies the following condition:then the total number of the accelerating time intervals and the decelerating time intervals is and the number of zero-thrust time intervals is . The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust .
If the impulse error satisfies the following condition:then the total number of the accelerating time intervals and the decelerating time intervals is and the number of zero-thrust time intervals is . The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust .
If the impulse error satisfies the following condition:then the total number of the accelerating time intervals and the decelerating time intervals is and the number of zero-thrust time intervals is . The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust .
At last, the switch control laws for the collision avoidance maneuver can be given. For convenience, let us take the time intervals in the -th thrust arc for an example:
5. Simulations
In order to present some of the results of the algorithms presented, we evaluated them in some nontrivial simulation scenarios. Suppose that is anunmanned aerial vehicle position and obstacle centers are , with axis lengths , , and and index parameters, , and . Then, we can construct the function as follows [17]:where parameters , , , , , and determine the shape and size of the obstacle: if and , the obstacle is a sphere; if and , the obstacle is regarded as a cone; if and are variables meeting the condition and holds, the obstacle is a circular truncated cone approximately, where are the radii of the two bases.
Suppose that there are seven obstacles in flight space as shown in Table 1 and and . The starting position of the unmanned aerial vehicle is , and the target position is .
It can be seen from Table 1 that there are seven obstacles in the environment, of which four obstacles can be simplified as spheres, two obstacles can be simplified as cones, and one obstacle can be simplified as a cylinder.
It can be seen from Figure 1 that there are two planned paths for the unmanned aerial vehicle. The blue color curve is the planned path of the original algorithm, and the path length is 65.98288. The red color curve is the planned path of the improved algorithm, and the path length is 64.38021. Both planned paths of the two algorithms can avoid all obstacles and reach the target position smoothly. For the obstacle 3, the planned path by the original algorithm is to pass from the side of obstacle 3, but the planned path by the improved algorithm is to fly over the obstacle 3; this is because the tangential vector of the improved algorithm is parallel to the plane , so the length of the planned path is shorter.
It can be seen from Figure 2 that there are two flying height curves for the unmanned aerial vehicle. The blue color curve is the flying height curve of the original algorithm, and the red color curve is the flying height curve of the improved algorithm. The flying height of the improved algorithm is higher than the original algorithm, and the highest point is 6.97 at . It shows that the improved algorithm has better climbing performance.
5.1. Unmanned Aerial Vehicle Flying Away from the Trap Area
Suppose the starting point of the drone is , and the destination is . Set seven obstacles in the flight space, and their information is listed in Table 2. and for each obstacle.
It can be seen from Figure 3 that there are two flying height curves for an unmanned aerial vehicle. The blue color curve is the flying height curve of the original algorithm, and the red color curve is the flying height curve of the improved algorithm. The planned path by the original algorithm can be stuck in the trap area formed by obstacles 3 and 4, but the unmanned aerial vehicle can fly over the trap area based on the improved algorithm. This is because the improved algorithm takes the unmanned aerial vehicle’s climbing ability into account. When the unmanned aerial vehicle encounters a trap area, it does not need to leave the trap area horizontally. This avoids the selection of the virtual target and simplifies the calculation process, resulting in greater flexibility.
5.2. Comparison of Different Saturation
In the process of unmanned aerial vehicle 3D path planning, with the change of saturation , different paths will be planned. Therefore, we need to design a reasonable saturation and give an optimal three-dimensional path. The different saturation are given in Table 3.
It can be seen from Figure 4 that there are four planned paths according to different saturation , and all four paths can bypass obstacles to reach the destination. It can be found that each path facing obstacles is different in the climbing height and the deflection angle. Further, the number of iterations, the length, and the highest point of each planned path are calculated and given in Table 4.
Path 1 tends to bypass obstacles horizontally. The maximum height of the unmanned aerial vehicle is only 4.71, but the path length is the longest. As the threshold value gradually decreases, the lengths of path 2 and path 3 are gradually shortened, and the maximum height of the corresponding unmanned aerial vehicle gradually increases, reaching 5.41 and 6.43, respectively. In path 4, the threshold is set to 0, which means that path 4 will tend to the shortest distance indefinitely, avoiding all obstacles and reaching the destination, but the rate of the climb drops a lot.
Figure 5 shows the flight height curves of the above four planned paths. For path 1, path 2, and path 3, the unmanned aerial vehicle reaches the highest flying height near the obstacle 3. For path 4, the unmanned aerial vehicle reaches the highest flying height near the obstacle 1. Therefore, we need to find a suitable threshold to make the unmanned aerial vehicle not only to have the ability to climb, but also to make the path as short as possible to ensures efficiency and safety of the planned path.
The results in Figure 6 show the constant thrust fitting of . According to the proposed criterion of this article, the unmanned aerial vehicle should carry out impulse compensation and the size of the constant thrust impulse compensation is the same, but the time of the constant thrust impulse compensation is different. The switch control laws can be given according to the sizes and the directions of the thrust accelerations of the unmanned aerial vehicle. The switch control law is given as follows:
6. Conclusions
This paper deals with the three-dimensional path planning problem of the unmanned aerial vehicle under constant thrust. The tangential vector and the disturbance matrix of the artificial fluid method are improved. A novel algorithm of constant thrust fitting is proposed through the impulse compensation, and then the constant thrust switching control scheme based on the isochronous interpolation method is given. It is proved that the planned path can avoid all obstacles smoothly and swiftly and reach the destination eventually. The simulation results show that the switch control laws can effectively guarantee the unmanned aerial vehicle moving along the planned path.
Data Availability
The data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the National Natural Science Foundation of China (no. 61304088), the Fundamental Research Funds for the Central Universities (no. 2013QNA37), the China Postdoctoral Science Foundation (no. 2015M581886), and the Jiangsu Province Postdoctoral Research Foundation (no. 1501080B).