Optimal Trajectory Planning for Unmanned Aerial Vehicles in Three Dimensional Space
T HIS paper generates an optimal path from an initial configuration (position, heading, and flight path angle) to a final configuration in three dimensional (3 D) space for a turn rateconstrained fixed wing unmanned aerial vehicle (UAV) flying both in the absence and presence of wind. Because the problem is fundamental to 3 D waypoint following problems in complex environments, it is a challenge to generate an optimal trajectory that is computationally fast for online implementation. Dubins [1] showed that, in a two dimensional (2 D) plane, for fixed initial and final positions and orientations, the shortest path for a constant speed and turn radius constrained vehicle consists of three consecutive path segments, each of which is a circle ofminimum turn radius (C) or a straight line (S) In 3 D space, Sussmann [2] used a maximum principle on manifolds to show that every minimizer in 3D is either a helicoidal arc or a concatenation of three pieces, each of which is a circle or a straight line. In coordinated path planning of multiple UAVs in 3 D space for sufficiently far apart points, Shanmugavel et al. [3] computedCCSC type paths for a 3 D Dubins problem. Babaei andMortazavi [4] used two planes in which the 3 D path obtained is also ofCCSC type. The solutions of [3,4] according to Sussmann [2] are not optimal. A suboptimal trajectory was also generated for the 3 DDubins problem in [5] based on the assumption of independent bounded control over the altitude velocity and turn rate in the plane. In [6] using an iterative algorithm, a suboptimal path consisting of a maximum of five different segments, each of which is either a circle or a straight line, was generated for the 3 D Dubins problem with the flight path angle constraint. The algorithm is stated to be suboptimal only when the distance between the two points projected onto the X Y plane is large; otherwise, this solution is very far from the optimal one. In [7] using Pythagorean hodograph curves, flyable paths were designed between 3 D waypoints satisfying maximum curvature and torsion bound. The 3 D waypoint following problem was discussed in a cluttered environment in [8] where collision free waypoints were generated by rapidly exploring random trees. Then, the unnecessary waypoints were removed by a simple path pruning algorithm, and piecewise linear paths were obtained. To develop a smooth continuous curvature path from these linear segments, cubic Bezier spiral curves was used, and the generated smooth path also satisfied the curvature constraint of the vehicle. The presence ofwind has been considered in some papers [9 18] but mostly in the 2 D plane. In [19,20] considering the steady wind effect at high altitude, a 3 D path was generated numerically based on the Pontryagin minimum principle. With the same assumption as in [3,4] that is, when the points are situated sufficiently far away, a 3 D CSC path is generated using geometry. As per [2] thisCSC path is only a candidate optimal path. To establish the optimality of the CSC path for the points situated sufficiently far apart, the problem is formulated in the optimal control framework and solved using the multiple shooting method coupled with nonlinear programming (MS NP) [21] The optimal path generated by the MS NP technique completely matches with the CSC path generated by 3 D geometry. If the generated CSC path demands steep climb or descent (when the vertical separation between the points is significantly higher than their horizontal separation) it may not be flyable due to flight path angle constraints. This issue is addressed by including the flight path angle constraint and solved numerically using the MS NP technique to obtain the optimal path. Some preliminary work on this problem is available in [22] for the absence of wind condition. The present paper also extends the idea of generating a 3 D optimal path in the presence of steady and time varying wind, considering wind flow in horizontal plane [23] In summary, the paper develops the time optimal path in 3 D space both in the absence and presence of wind for a constant speed, fixedwing UAV for given initial and final configurations. Unlike existing iterative methods, which yield suboptimal paths and are computationally more intensive, the geometrical method proposed here is computationally simple and fast and hence implementable in real time when the final configuration changes en route. This is useful for 3 D waypoint following problems in dynamically changing environments. Finally, using a six degree of freedom (DOF) UAV model, this path is tracked by an autopilot consisting of proportional integral derivative controllers.