Survey of UAV motion planning

IET Cyber-Systems and RoboticsVolume 2, Issue 1 p. 14-21 Review ArticleOpen Access Survey of UAV motion planning Lun Quan, Corresponding Author Lun Quan lunquan@zju.edu.cn College of Control Science and Engineering, Zhejiang University, Xihu District, Hangzhou, People's Republic of ChinaSearch for more papers by this authorLuxin Han, Luxin Han ECE Department, Hong Kong University of Science and Technology, Clear Water Bay, Kowloon, Hong KongSearch for more papers by this authorBoyu Zhou, Boyu Zhou ECE Department, Hong Kong University of Science and Technology, Clear Water Bay, Kowloon, Hong KongSearch for more papers by this authorShaojie Shen, Shaojie Shen ECE Department, Hong Kong University of Science and Technology, Clear Water Bay, Kowloon, Hong KongSearch for more papers by this authorFei Gao, Fei Gao College of Control Science and Engineering, Zhejiang University, Xihu District, Hangzhou, People's Republic of ChinaSearch for more papers by this author Lun Quan, Corresponding Author Lun Quan lunquan@zju.edu.cn College of Control Science and Engineering, Zhejiang University, Xihu District, Hangzhou, People's Republic of ChinaSearch for more papers by this authorLuxin Han, Luxin Han ECE Department, Hong Kong University of Science and Technology, Clear Water Bay, Kowloon, Hong KongSearch for more papers by this authorBoyu Zhou, Boyu Zhou ECE Department, Hong Kong University of Science and Technology, Clear Water Bay, Kowloon, Hong KongSearch for more papers by this authorShaojie Shen, Shaojie Shen ECE Department, Hong Kong University of Science and Technology, Clear Water Bay, Kowloon, Hong KongSearch for more papers by this authorFei Gao, Fei Gao College of Control Science and Engineering, Zhejiang University, Xihu District, Hangzhou, People's Republic of ChinaSearch for more papers by this author First published: 01 March 2020 https://doi.org/10.1049/iet-csr.2020.0004Citations: 31AboutSectionsPDF ToolsRequest permissionExport citationAdd to favoritesTrack citation ShareShare Give accessShare full text accessShare full-text accessPlease review our Terms and Conditions of Use and check box below to share full-text version of article.I have read and accept the Wiley Online Library Terms and Conditions of UseShareable LinkUse the link below to share a full-text version of this article with your friends and colleagues. Learn more.Copy URL Share a linkShare onFacebookTwitterLinkedInRedditWechat Abstract Motion planning is a vital module for unmanned aerial vehicles (UAVs), especially in scenarios of autonomous navigation and operation. This survey delivers some recent state-of-the-art UAV motion planning algorithms and related applications. The logic flow of this survey is divided as the path finding, which is the front-end of most motion planning systems, and the trajectory optimisation, which usually serves as the back-end. Motivation, methodology, problem formulation and derivation are given in this survey, in detail. Finally, a section about real-world applications is given, where roles and effectiveness of most popular motion planning methods are revealed. 1 Introduction In recent years, micro aerial vehicles have drawn increasing attention in the robotics community. Thanks to their mobility, agility, and flexibility, quadrotors are capable of various applications, such as aerial photography, search, and rescue, surveillance and inspection. To accomplish tasks in extreme environments that may be dangerous or inaccessible for human operators, it is essential that aerial robots can autonomously navigate. Dynamic controlling, state estimation, environment perception, and motion planning are the most fundamental and essential functionalities for equipping a quadrotor with full autonomy. Among these modules, motion planning takes charge of coordinating the overall system, by having interactions with human operators and making decisions in cluttered environments. Therefore, the motion planning module is often regarded as the highest layer of an autonomous quadrotor, which builds its functionality upon other underlying modules. Fortunately, as the emerging achievements of computer vision, artificial intelligence, and micro electro mechanical systems (MEMs) sensors in the last decades, the control, estimation, and perception all become more and more reliable and applicable, making it possible to examine the robustness and effectiveness of pure motion planning methods in real-world applications. In this paper, we give a thorough review of the most recent quadrotor motion planning methods. Among all these versatile methods, we especially put emphasis on planning frameworks that have been successfully applied to real-world quadrotor platforms. In this paper, we follow the traditional workflow in robotics motion planning to roughly divide the planning problem as a front-end path finding and a back-end trajectory optimisation. We list the most advanced methods of each category proposed in recent years in Sections 2 and 3. Then we go through the unmanned aerial vehicle (UAV) applications where motion planning plays a vital role in Section 4. 2 Path finding Roughly speaking, the motion planning problem can be divided into front-end discrete path finding and back-end continuous trajectory optimisation. For the front-end path finding, methods ranging from sampling-based [1] to searching-based [2] have been proposed and applied. Before introducing those specific methods, it is important to define the state space, namely configuration space. 2.1 Configuration space The configuration space is a set of possible transformations that could be applied to robots. With the help of configuration space, motion planning problems varying in geometry and kinematics can be solved by the same planning algorithms. The detailed material can be found in [3]. A brief introduction will be shown here for integrity. For a robot with n degrees of freedom, the set of transformations, which is a manifold of dimension n, is the configuration space for that robot, denoted by . In particular, the configuration space for a 2D rigid body is called , for a 3D rigid body , and for multiple independent rigid bodies is the Cartesian product of the configuration spaces of each of them. After establishing the configuration space, obstacle region, , which is all configurations that robot collides with obstacles or each other, must be defined. As shown in Fig. 1, we also define the leftover configurations free space, denote it as . Figure 1Open in figure viewerPowerPoint Illustration of configuration space. is derived from the expansion of the obstacle according to the radius of the moving object. The advantage of this is that the moving object can be represented as a point in configuration space With the help of all definitions above, a definition of motion planning can be given as find a continuous path , such that and , where are the initial and goal configurations, respectively, or report that such a path does not exist. 2.2 Sampling-based methods Probabilistic Road-Map (PRM) [4] and Rapidly-Exploring Random Tree (RRT) [5] are two representative methods in the family of the sampling-based method. PRM method takes random samples from the free space of the robot, and uses a local planner to connect the new sample to its nearby configurations. A graph model is generated in this way, and then the graph search algorithms are applied to solve motion planning. RRT is a tree rooted by the starting configuration. Each tree node is a configuration in the free space. RRT grows by adding edges iteratively using random configurations. If the connection between the random configuration and its nearest tree node is feasible. Namely, every configuration in this connection is in the free space, which makes the random configuration become a node and an edge between it and its nearest tree node should be added. Once the tree expands to a configuration near the goal configuration, it terminates and generates a path immediately. RRT is good at efficiently finding a feasible path. However, it has been proven that RRT has no asymptotical optimality and converges to the homotopy of the first feasible solution [6]. Asymptotically optimal sampling-based methods, including RRG, PRM*, and RRT*, in which the solution converges to the global optimality as the samples increase, have also been proposed in [6]. RRG is an extension of the RRT algorithm, as it connects new samples not only to the nearest node, but also to all other nodes within a range, and a path is searched after the construction of the graph. Also, in PRM*, connections are attempted between roadmap vertices, which are within a range. RRT*, meanwhile, has a rewire mechanism to locally reconnect nodes in the tree and keep the shortest path from the root node to each leaf node. Under the same category, the approach in [7] combines a fixed final state and free final time-optimal controller with the RRT* method to ensure asymptotic optimality and kinodynamic feasibility of the path. In this method, the optimal state transition trajectory connecting two states is calculated by solving a linear quadratic regulator (LQR) problem. Another method that combines RRG and the belief roadmap was proposed by Bry and Roy [8]. In this method, a partial ordering is introduced to trade-off belief and distance while expanding the graph in the belief space. There also some methods to speed up the convergence rate, such as RRT*-Smart [9], Informed-RRT* [10]. 2.3 Searching-based methods Searching-based methods discretise the configuration space based on well-defined sampling and convert the path finding problem to graph searching, which is much simpler than the original continuous problem. The Breadth-First Search (BFS) [11] and Depth-First Search (DFS) [11] algorithms are two fundamental graph search algorithms. They models the discrete space as a graph denoted by , where V is the set of vertices, E the set of edges. BFS uses the First-In-First-Out queue, and can only get the optimal solution if the graph is a uniform-weighted graph, i.e. weights for all the edges are the same. DFS uses a Last-In-First-Out stack, and can be early-terminated when encountering the goal configuration. Optimality in motion planning is sometimes very important [12]; however, there is no guarantee for optimality in DFS. The asymptotic running time of them is . Iterative Deepening Depth-First Search [13] (ID-DFS), a depth-limited version of DFS, runs repeatedly with increasing depth limits until the goal is found. The optimality of ID-DFS is just like BFS, but it uses less memory than BFS. Dijkstra's algorithm [11] is a fast algorithm for the shortest path. Its main idea is to find the current un-visited node with the shortest distance, then mark it visited, and update the distances of its adjacent nodes. Do it repeatedly until all the nodes are visited. This algorithm is correct for graphs where weights for all edges are non-negative. The asymptotic time complexity for Dijkstra without min-priority queue is , while based on a min-priority queue implemented Fibonacci heaps, the time complexity can be reduced to . A* [11] is a widely used algorithm in path-finding. It gives a heuristic function , and expanded node with smallest every time. With an admissible , where , meaning that it never overestimates the actual cost to get to the goal, A* is guaranteed to return a least-cost path from start to goal. The time complexity of A* depends on the heuristic. A good heuristic can make the algorithm only expand a small part of nodes to get the shortest path, thus has a major effect on the practical performance. Dijkstra algorithms can be thought of as a special case of A*, where for all nodes. The greedy best-first search runs much faster by letting . However, it does not guarantee the optimality for the reason of greedy. Like ID-DFS, IDA* [13] is performing A* algorithm using a DFS-like framework, and pruning when exceeds an iterative increasing upper bound. It is beneficial when the problem is memory constrained because of the nature of DFS. When changes happen in the environment, Lifelong Planning A* [14] (LPA*), can adapt to changes without recalculating the entire graph. LPA* updates , which is inconsistent with the change. The starting point and end point of LPA* are static. D* Lite [15], which is built upon LPA*, is designed for the situation where starting point changes when the robot moves, in other words, the start configuration always follows the movement of the current robot. It works like LPA* except using an opposite search direction. It is more suitable for mobile robots because it fits the nature of the move-perceive-replann pipeline. Other typical methods based on A* include Anytime Repairing A* (ARA*) [2], Jump Point Search (JPS) [16], and hybrid A* [17]. ARA* accepts a suboptimal solution given a time limit, then reuses search efforts from previous executions to improve the optimality of the path. JPS is only applied on a uniform-cost grid lattice to prune the neighbours of a node being searched. In some cases, JPS can potentially reduce its running time by order of magnitude compared to the traditional A* without sacrificing the optimality. Hybrid A* considers the dynamic model of a robot to steer the extension of a state for generating the graph and searching for a dynamically feasible path. Methods using the state-lattice [18] construct an action space consisting of motion primitives as well as a heuristic look-up table offline. The path is then online searched by a graph search method like AD* [19]. Similarly, in [20], motion primitives are online computed using a time-optimal LQR control policy, and a search graph is online expanded. Usually, the path found by the front-end cannot be directly executed by vehicles since it may be discontinuous or contain unnatural swerves. Therefore, the path is further optimised to generate a safe, continuous, and dynamically feasible trajectory which is considered as executable. 2.4 Some other mentionable methods The potential field method assigns an artificial potential field to every point in the world using the potential field functions. The goal node has the lowest potential, while the starting node will have the maximum potential. Then UAVs plan from the highest potential to the lowest potential. One way to consider both physical feasibility and obstacle avoidance is by using the State Lattice method. State Lattice uses motion primitives, which is a motion that follows a control for some pre-defined short time slot, to fill the whole free space, converting the original continuous space into discrete space. Then this problem can be easily solved by graph search algorithms. 3 Trajectory optimisation Most path finding algorithms construct a geometric trajectory without time information. To guarantee kinodynamic feasibility of quadrotors, the initial geometric trajectory needs to be parameterised in time, which is the main objective of trajectory generation. Generally, the trajectory generation problems are formulated as minimising an objective function such as total control cost while satisfying safety and kinodynamic feasibility constraints, which can be categorised as hard-constrained and soft-constrained methods. 3.1 Hard-constrained methods The minimum-snap trajectory generation algorithm proposed by Mellinger and Kumar [21] is a pioneering work. As presented by the authors, it is possible to reduce the full state space of a quadrotor system to the 3D position, yaw angle and their derivatives, since the system enjoys the property of differential flatness. As a result, the trajectory can be represented by smooth piecewise polynomials as in (1) with bounded derivatives, i.e. (1)The optimal trajectory concerning snap can be generated by solving a quadratic programming (QP) problem that minimises the integral of squared snap as follows: (2)where is a vector of polynomial coefficients, and Q is the Hessian matrix of the objective function. Richter et al. [22] showed that the minimum snap trajectory could be obtained in a closed form. Instead of looking for the optimal polynomial coefficients directly, the coefficients are mapped to the derivatives at each segment points of the piecewise polynomial by a mapping matrix M. These derivatives are then separated into fixed and free derivatives by a selection matrix C (3)by this mapping, the objective function (2) can be written as (4)Then, the optimal free derivatives can be found in a close form (5)In their proposed framework, the dense map is built before the quadrotor flight. Then a path is found by calling RRT* in the complicated map. Based on the path, the closed-form piecewise minimum snap trajectory generation algorithm is applied to convert the path to an optimised quadrotor trajectory, which passes all vertexes of the RRT* path. This method is also often called a waypoint-based method since its shape is determined by the waypoints of the front-end path. Although the path is surely collision-free, the overshoot in the generated minimum-snap trajectory may have collisions with obstacles, as shown in Fig. 2. Therefore, the trajectory is then checked, whether it is collision-free. If the trajectory has a collision in one of the pieces, then an intermediate waypoint is added in the middle of this piece and the trajectory is re-generated. The safety of the generated trajectory is achieved by iteratively adding intermediate waypoints and checking the collision. In [22], the authors also proposed a method to do the time allocation of the piecewise polynomial trajectory. Deits and Tedrake [23] used IRIS (iterative regional inflation by semidefinite programming) to compute the convex region of safe space. Then they performed a mixed-integer optimisation to assign the polynomial trajectories to the convex regions. To ensure that the entire trajectory is within the safe space, they introduced a sums-of-squares (SOS) programming technique. Figure 2Open in figure viewerPowerPoint Illustration of the minimum-snap trajectory generation. Blue area and red area are obstacles. The black dotted line and red curve are planned path and generated trajectory, respectively. Because this method only constrains intermediate waypoints of the trajectory should pass, the trajectory inevitably overshoots in some places as shown in the red area Also inspired by IRIS, Liu et al. [24] proposed a method to generate trajectory using the concept of a safe flight corridor, which is a sequence of convex overlapping polyhedra. Jump Point Search (JPS) is first used in uniform-cost grid maps to get a piecewise initial path and reduces the running time of the A* algorithm by an order of magnitude. Then the set of convex polyhedra is found by an iteratively ellipsoid shrinking and dilating procedure. These polyhedra provide linear inequality constraints in the following QP, which generates a smooth piecewise polynomial trajectory. Compared with IRIS, the resulted trajectories are very similar, but the time consumed in finding convex regions is much faster. Chen et al. [25] proposed to generate free-space flight corridors consisting of 3D cubes by using efficient operations in the octree-based environment representation. The standard A* algorithm is used to generate an initial sequence of connected 3D grids. Using efficient operations in the octree data structure, these initial grids are then inflated to the maximal volume flight corridor. Finally, an efficient QP approach is applied to generate a smooth trajectory that fits entirely within the flight corridor and satisfies higher-order dynamical constraints. Similarly, Gao and Shen [26] proposed a method to generate trajectory directly on point clouds. A point cloud map of the environment is built incrementally using a 3D laser range finder. Based on the map, a sampling-based path finding method is adopted to generate a safe space flight corridor consisting of a sequence of spheres. The path finding algorithm utilises KD-tree for fast nearest neighbour search and is efficient. Then a smooth trajectory that fits within the spherical corridor and is kinodynamically feasible is generated in a similar manner to Chen et al. [25]. The same problem that exists for both methods in [25, 26] is that it sometimes takes many iterations to obtain a feasible solution. Their algorithms iterate while the polynomial violates feasibility constraints and more constraints are added to the QP. Although it was proven by Chen et al. [25] that a feasible trajectory can be generated within a finite number of iterations, the approach is inefficient sometimes. In [27], trajectories with piecewise constant acceleration are generated. The authors derived the velocity bound as well as the maximum deviation between the trajectory and the path concerning the limit of acceleration and minimum path clearance. Since the problem is formulated as a convex optimisation problem that is guaranteed to be feasible, no iterative procedure is needed as previous methods. However, the bound is very conservative, making the speed of the generated trajectory always deficient. Among the above-mentioned methods, one of the key factors that significantly influence the quality of the piecewise trajectory is time allocation. However, the initial path contains no time information and in optimisation, the segment times are only estimated by some naive heuristic function. In [28], a fast marching-based method in a velocity field is applied to search a time-indexed path. Since the velocity field is derived from an Euclidean signed distance field, the resulted time allocation along the path adapts better to obstacle densities in environments. Besides, the Bernstein basis is used to represent the trajectory, which results in piecewise Bezier curves. The convex hull property of the Bezier curve is utilised so that the safety and high-order dynamical feasibility can be bounded within the feasible space efficiently. 3.2 Soft-constrained methods The methods mentioned in the previous sections set hard constraints for the optimisation variables that are required to be satisfied. Among other methods, called soft-constrained methods, however, the constraints are penalised in the objective function, as shown in Fig. 3. CHOMP [29] is a trajectory optimisation method that minimises smoothness and collision costs locally on a discrete-time trajectory. With discrete waypoints as optimisation variables, the planner performs gradient descent in each step. The algorithm can find smooth and collision-free trajectories from straight-line initialisation, which might not be collision-free. However, in cluttered environments, the success rate of the method is low. STOMP [30] is also based on optimisation using a two-part objective function. In contrast, the optimisation is solved by gradient-free candidate sampling and combining the best-scoring candidates linearly. Figure 3Open in figure viewerPowerPoint Illustration of the hard-constrained and soft-constrained trajectory generations. The blue areas are obstacles and the black line is generated trajectory. The circular areas in Fig. 3a are safe flight corridors. The method in Fig. 3a sets hard constraints to limit the optimised trajectory to pass through these safe flight corridors. However, this method treats all safe space equally, so the solution space is sensitive to noise, and sometimes there is a low clearance between generated trajectory and obstacles, which may be harmful to practical application. So the method in Fig. 3b increases the effect of certain constraints on the trajectory by penalising them in the objective function as if forces from all directions push the trajectory to a safe, balanced state (a) Hard-constrained method, (b) Soft-constrained method Inspired by CHOMP, an approach based on sequential convex optimisation is presented in [31]. The workspace is broken up into convex free regions and sequential convex optimisation that penalises collisions with a hinge loss is performed. The original non-convex optimisation problem is solved by constructing and solving approximate convex subproblems repeatedly, each of which generates a step that makes progress on the original problem. Since convex optimisation algorithms minimise each subproblem efficiently, the proposed algorithm converges faster than the previous two methods. The main drawback of the method is that convex regions are difficult to compute online and thus require a pre-built map with convex regions. An online continuous-time trajectory optimisation method for local replanning is presented in [32]. Unlike previous methods, continuous-time trajectory representation is used because dynamic constraints can be more accurately expressed and it avoids numerical differentiation errors. The collision cost of the continuous-time trajectory is formulated as the line integral of the distance penalty over the arc length along the trajectory. The integral is discretised on different time stamps for numerical calculation (6)where , and is the velocity at the position . To improve the efficiency of the optimisation, the coefficients of the polynomial are mapped to end-derivatives of each segment, as proposed by Richter et al. [22]. This mapping transforms a constrained optimisation problem into an unconstrained programming problem, which is much faster to solve. To solve this non-linear optimisation problem, the gradient of the discretised distance cost is needed, which can be computed efficiently as (7)where F and G are matrixes related to and . The main drawback of this method is that it suffers from converging to a local minimum and this can only be slightly relieved by several random restarts. A similar formulation with the difference of initialising the trajectory by piecewise straight line generated by an informed sampling-based path generation algorithm is adopted in [33]. Having a voxel grid map as environment representation, a random-exploring graph is generated, in which the safe volume of a given point is evaluated by using a K-D tree to do a fast nearest neighbour search. A minimum distance path can be found by the standard A* algorithm. An informed sampling scheme in which the best path and the heuristic sampling domain are updated iteratively is also utilised to improve the efficiency of the sampling and the quality of the path. Thanks to the better quality of the initial path, this method enjoys a significantly higher success rate compared with CHOMP [29] and CT [32]. Vladyslav Usenko [34] proposed an algorithm to generate a globally smooth and collision-free trajectory and do local replanning that can handle dynamic obstacles while keeping the deviation from the global trajectory small. Instead of using polynomial as trajectory representation, they use uniform b-splines. Since the b-spline trajectory is smooth, given an arbitrary set of control points and has the property of locality, it results in fewer constraints and optimisation variables. These properties make it very efficient to do local replanning. 4 Applications 4.1 Autonomous navigation Enabling the quadrotor to navigate autonomously from a start position to a target position is the most fundamental requirement for a UAV motion planning framework. The motion planning is used for online generating safe and dynamically feasible trajectories. For this kind of mission, the simplest set-up is assuming all information about the environment is known and the map representation is pre-built, as shown in Fig. 4. Figure 4Open in figure viewerPowerPoint System architecture of the autonomous navigation under the simplest set-up. Firstly, the path searching module searches for a collision-free path on the pre-built map; then, the trajectory optimisation module optimises this path to ensure the feasibility of dynamics. Finally, the trajectory server module discretises the optimised trajectory and outputs commands to the actuator In the state-of-the-art optimisation-based quadrotor planning framework [22], the dense map of the environment is pre-built by OctoMap [35] before the planning. Then RRT* is used to find a collision-free path in the map. Based on the path, a piecewise polynomial trajectory is generated by iteratively adding collision-free intermediate waypoints. And the time duration of the trajectory is also iteratively optimised by the numeric time optimisation proposed in [22]. The final generated trajectory after several seconds calculation is executed by the quadrotor with onboard state estimation and control at a flight speed up to . In [36], a fast autonomous flight of the fixed-wing UAV is presented. In this work, control, estimation, perception, and planning modules are all running onboard a mini embedded computer. The authors propose the pushbroom stereo method to fast calculate the disparity of a pair of stereo cameras. This algorithm only estimates the depth results in a fixed distance, thus it significantly saves computation cost. Motion planning is done by selecting a local trajectory from an off-line built trajectory library, where the high acceleration manoeuvres are recorded from the aggressive manual flight. Similarly, to enable the agile and fast flight of fix-wing vehicles in cluttered environments, a two-layer motion primitive method is proposed in [37]. This method consists of two families of primitives. The first is a family of time-delay dependent 3D circular path connecting two points in space, and the second family is an aggressive turn-around (ATA) manoeuvres which the robot uses to retreat from dead-ends. Due to the limited sensing range of the UAV, conducting only local planning in the perception range is reasonable, although with t