Motion planning, the problem of generating dynamically feasible trajectories that avoid obstacles in unstructured environments, for Micro Aerial Vehicles (MAVs), especially quadrotors, has attracted significant attention recently [1, 2, 3, 4]. When the MAV attitude and dynamics are taken into account, the problem is challenging because there are no simple geometric conditions for identifying collision-free configurations . Existing planning approaches usually model the MAV as a sphere or prism, which allows obtaining a simple configuration space (C-space) by inflating the obtacles with the robot size. As a result, the robot can be treated as a single point in C-space and the collision-checking even for trajectories that take dynamics into account is simplified. Even though this spherical model assumption is widely used in motion planing, it is very conservative since it invalidates many trajectories whose feasbility depends on the robot attitude (Fig. 1). Several prior works have demonstrated aggressive maneuvers for quadrotors that pass through narrow gaps [6, 7, 8] but, instead of solving the planning problem, those works focus on trajectory generation with given attitude constraints. Those constraints are often hand-picked beforehand or obtained using gap detection algorithms which only works for specific cases.
We are interested in designing a planner that considers the robot’s actual shape and dynamics in order to obtain aggressive trajectories in cluttered environments. Since quadrotors are under-actuated systems, they cannot translate and rotate independently. Thus, planners for fully-actuated system like spacecraft [9, 10] or omni-directional aerial vehicle  are not suitable for quadrotors. This paper builds on our previous search-based trajectory planning approach  that utilizes motion primitives to discretize the control space and obtain a dynamically feasible resolution-complete (i.e., optimal in the discretized space) trajectory in cluttered environments. We extend our previous work by explicitly computing the robot attitude along the motion primitives and using it to enforce collision constraints. Furthermore, to reduce computation time for searching in high-dimensional (velocity, acceleration, jerk, etc.) space, we propose a novel hierarchical planning process that refines a dynamically feasible trajectory from a prior trajectory in lower dimensional space. The paper makes the following contributions:
A graph search algorithm that uses motion primitives to take attitude constraints into account and compute a dynamically feasible resolution-complete trajectory for a quadrotor is developed.
A hierarchical refinement process that uses prior lower-dimensional trajectories as heuristics to accelerate planning in higher dimensions is proposed.
The effect of motion primitive discretization parameters on the computation time, smoothness, and optimality of the generated trajectories is analyzed.
The code used in this work is open-sourced at https://github.com/sikang/mpl_ros. Users can easily test our planner and benchmark the performance against other planning algorithms.
Ii Related Works
Trajectories for MAVs or, more generally, differentially flat systems  are usually represented as piecewise polynomials in time since their derivatives can be used to obtain explicit expressions for the system states and control inputs . When collision avoidance is taken into account, more constraints need to be added to the problem formulation to guarantee safety either through anchoring waypoints as in [2, 13] or building a safe flight corridor as in [4, 15, 16]
. These approaches require planning in a C-space in which the robot’s attitude does not affect collision checking. As described in the previous section, conservative symmetrical approximations of the robot body may ignore trajectories whose feasibility depends on the robot attitude. Hence, planning in SE(3) is necessary in order to obtain agile trajectories in cluttered environments. Planning with 6 degrees of freedom has been addressed in several works[17, 18] via sampling techniques but these do not translate immediately to our problem, where the rotation and translation are coupled and a smooth, deterministic trajectory is desired. Methods based on motion primitives are a promising approach for planning dynamically feasible and collision-free trajectories. For example, lattice search with pre-defined primitives [19, 20] may be used to plan trajecotires for non-circular robots in obstacle cluttered environments. In our previous work , we developed a global planning approach for quadrotors based on lattice search by using motion primitives generated via optimal control . In this work, we extend  to account for attitude constraints by explicitly computing the robot attitude along the motion primitives based on the desired acceleration and gravity.
While randomized sampling approaches have been effective at solving very high dimensional planning problems, they take a long time to converge to an optimal solution  and intermediate solution quality might be unpredictable. Hence, randomized approaches are not suitable for fast navigation in unknown environments where frequent, predictable re-planning is necessary. Traditional graph search techniques are considered inefficient in high dimensional spaces but appropriate heuristic design [23, 24, 25] may accelerate their speed. Using weighted heuristics, however, produces sub-optimal solutions and does not always reduce planning time . An interesting, alternative idea for accelerating motion planning is based on adaptive dimensionality , which exploits preliminary search results in lower dimensions to accelerate the planning process in high dimensions. In , we used a trajectory refinement step that obtains a smooth (higher dimensional) trajectory from a trajectory planned in a lower dimensional space even though this refinement step can potentially lead to unsafe and infeasible trajectories. In this work, we use a hierarchical planning procedure—plan a trajectory in low dimensional space and use it as a heuristic to guide the search in high dimensional space—to replace the refinement step, while guaranteeing dynamical feasibility, safety, and resolution completeness.
Iii Motion Planning with Attitude Constraints
In this section, we introduce our trajectory planning framework based on motion primitives. While our previous work  guarantees safety, dyanmical feasibility and optimality, it assumes a spherical robot body. Here, we introduce a way to account for the robot attitude during planning based on the desired acceleration and gravity. Since the quadrotor yaw is decoupled and does not affect system dyanmics, we assume it remains constant during planning.
Iii-a System Dynamics in Planning
Before introducing the planning approach, we inspect the relation between polynomial trajectories and system dynamics. The position in of the quadrotor can be defined as a differentially flat output as described in . The associated velocity , acceleration and jerk can be obtained by taking derivatives with respect to time as respectively. The desired trajectory for the geometric SE(3) controller as described in  can be written as . According to , we assume the force and angular velocity are our control inputs to the quadrotor. Ignoring feedback control errors, the desired mass-normalized force in the inertial frame can be obtained as
where is the gravitational acceleration and is the z-axis of the inertial world frame. Similar to , given a specific yaw , the desired orientation in SO(3) can be written as where
which is assumed to be not parallel to . The associated angular velocity in the inertial frame, , can be calculated as
Therefore, the desired angular velocity in body frame is obtained as:
Once the desired force , orientation and angular velocity are defined, it is straightforward to compute the desired control inputs for the quadrotor system. Notice that: 1) orientation is algebraically related to the desired acceleration and gravity and 2) angular velocity is algebraically related to the desired jerk.
Iii-B Search-based Planning using Motion Primitives
As mentioned in the previous section, the desired trajectory can be defined as
and each component of can be represented by a polynomial parameterized in time . Position can be defined as
where are the coefficients. The corresponding velocity, acceleration and jerk can be obtained by taking the derivative of (8). A polynomial trajectory from one state to the other within a specified time duration is called a motion primitive. Our approach uses primitives generated as the solutions to an optimal control problem  to build a graph from an initial state to a goal state and search for the optimal sequence of primitives. Technical details and proof of optimality can be found in our previous work . In this paper, we give the explicit solution for generating the optimal trajectory using jerk as the control input.
We define the state
as a subset of the trajectory that excludes the jerk. From an initial state , we apply a constant jerk input from a pre-defined control set for a short duration . The resulting curve between and the end state is a motion primitive such that for the system state can be written as
The finite control input set and duration define a graph , where is the set of reachable states in and is the set of edges connecting those states. The states in are generated by applying each element of at each state iteratively, and each element in is a primitive as defined in (10). A breadth-first-search (BFS) of a finite horizon leads to the graphs shown in Fig. 2.
We are interested in finding a trajectory from to that is optimal in terms of total control effort and time taken to reach the goal. According to , a desired optimal trajectory is obtained as
where is the weight that decides the trade-off between effort and time.
For the primitive defined in (10), and . Thus, the cost of a primitive of applying from state is defined as
The cost of the individual primitive is independent of the current state and only depends on the set and . In addition, it can be shown by Pontryagin’ minimum principle that (10) is the optimal solution of (11). Details of the proof can be found in . Therefore, search for an optimal trajectory of (11) is equivalent to find the optimal solution to the following deterministic shortest path problem:
Given an initial state , a goal region , a free space and motion primitives based on a finite set of control inputs with duration , choose a sequence of control inputs of length such that:
We are able to solve this problem through a graph search algorithm like A*. The optimal trajectory can be recovered by applying the optimal control solution with (10) from the start as
When planning dynamic trajectories, traditional distance-based heuristics are not effective since short-distance trajectories may require sudden changes in velocity, acceleration or orientation. Instead, we use a heuristic, proposed in , which is based on the solution of a Linear Quadratic Minimum Time (LQMT) problem and takes trajectory smoothness into account. Given the current state and the goal state , the LQMT solution provides an explicit formula for the as described in Appendix A.
Iii-C Feasibility Checking
When checking if a motion primitive is contained in the free space in Problem 1, we need to consider both dynamical constraints that arise from system dynamics and geometric constraints due to physical obstacles.
Iii-C1 Dynamically Feasible Primitives
The dynamical constraints on a quadrotor system are the min/max thrust and torques that can be provided by the motors . However, it is hard to examine the true specification for each quadrotor and apply correct non-linear constraints. In fact, it is reasonable to utilize the property of differential flatness and apply velocity, acceleration, and jerk constraints on each axis independently. This leads to componentwise inequalities of the form:
Polynomial expressions for allow us to check (15) in closed-form for each axis by finding the min/max value on time interval . The latter is equivalent to finding the roots of the corresponding derivatives. Thus, we can guarantee that the planned trajectories always stay within the bounds .
Iii-C2 Collision Free Primitives
Traditional collision checking is implemented in occupancy grid maps where the free and occupied spaces are discretized into cells. The robot is usually assumed to have a spherical shape. By inflating occupied cells with the radius of the robot, we are able to treat the robot as a single cell and check the occupancy of cells only along a trajectory. As mentioned in Section I, this process is too conservative and not suitable for planning agile trajectories in cluttered environments since it fails to take the actual robot shape and attitude into account. In this paper, we model the quadrotor as an ellipsoid in with radius and height and the obstacle map as a point cloud (Fig. 3). Given a quadrotor state , its body configuration at can be obtained as
and the orientation can be calculated from and gravity as shown in (2).
Checking whether the quadrotor hits obstacles while following a trajectory is equivalent to checking if there is any obstacle inside the ellipsoid along the trajectory. In other words, we need to verify that the intersection between and the point cloud is empty:
Instead of checking through every point in , it is more efficient to use KD-tree  to crop a subset of at first and then check the intersection between and obstacles inside . The subset is created by looking for neighbor points around within radius , assuming .
Since the contour of an ellipsoid following a primitive is not convex, we sample states in time along a primitive and consider the primitive collision-free if
where is the -th sampled state on .
In sum, the explicit formulation of the feasibility constraints in Problem 1 is written as:
Iv Trajectory Refinement
In the proposed planning approach, the dimension of the state space increases with increasing requirements on the continuity of the final trajectory. More precisely, if continuity is required for the final trajectory, jerk should be used as a control input and the state space of the associated second order system would be (position, velocity acceleration). Generally, planning in higher dimensional spaces (e.g., snap input) requires more time and memory to explore and store states. In this section, we introduce a hierarchical approach to planning a feasible trajectory in high dimensional space by utilizing guidance from a trajectory planned in lower dimensional space. We show that the overall computation time of this hierarchical planning is shorter than the total time it takes to plan a optimal trajectory directly. Due to the fact that the final trajectory is calculated from a trajectory in lower dimensional space, similar to the refinement process in , we call this hierarchical planning process–trajectory refinement.
Iv-a Trajectories Planned in Different Control Spaces
Denote the trajectories planned using velocity, acceleration or jerk inputs as respectively. Given the same start and goal, dynamics constraints and discretization, examples of the optimal trajectories in each case are plotted in Fig. 4, where the control effort of the whole trajectory is measured as
Denote the execution and computation time of the trajectory as and accordingly. From the planning results in Fig. 4, two conclusions can be drawn with increasing :
The execution time increases, i.e ;
The computation time increases, i.e .
Note that the computation time increases dramatically as increases.
Iv-B Using Trajectories as Heuristics
Denote the prior trajectory in lower dimensional space as . It is easy to obtain the set of a sequence of waypoints from start to goal as , of which each element is evaluated on at the time . When searching for a trajectory in higher dimensional space () , we propose to use the following heuristic function:
where is execution time of and is the control effort from to along at order as (21). This formulation is consistent with the cost function defined before in (11). As the prior trajectory is in the lower dimensional space, for is always zero. Thus is the execution time between and :
Fig. 5 shows an example of applying (22) to search a trajectory using acceleration with a prior trajectory planned using velocity. Apparently, the new trajectory will try to stick close to the prior trajectory due to the effect of (22). In fact, the heuristic function defined in (22) is not admissible since sometimes it is not the under-estimation of the actual cost-to-goal. However, with the help of (22), we are able to search for trajectories in higher dimensional space in a much faster speed since it tends to search the neighboring regions of the given trajectory instead of exploring the whole state space. In order to guarantee optimality with the inadmissible heuristic in (22), we can combine it with the consistent LQMT heuristic , and use multi-heuristic A* .
The results of applying (22) for the same planning tasks in Fig. 4 are given in Fig. 6, in which is used as heuristic to plan for both trajectory and . Comparing Fig. 6 to 4, the total cost of control effort and execution time, namely , of the new trajectories in Fig. 6 are greater than the optimal trajectories in Fig. 4, but the computation time are much less.
V-a 2-D Planning
2-D planning is efficient and useful in 2.5-D environments where the obstacles are vertical to the floor. We start by showing 2-D planning tasks of flying though gaps with different widths. In Fig. 7 shows how planned trajectories using jerk as a control input vary as the gap in a wall is shrinking (left wall moves closer to the right wall from (a) to (f)). Accordingly, the angle of the desired roll at the gap increases. Assume the robot has radius m, height m, and the maximum acceleration in each axis is . Denoting the roll along trajectory as , according to (1) and (2), we have
since the desired acceleration in -axis is zero. In other words, the smallest gap that the robot can pass through using 2-D planning is approximately equal to (which is approximately equal to m).
V-B 3-D Planning
By adding control in the -axis, we are able to plan in 3-D space and relax the constraint in (25) as follows:
When , can be arbitrary. Letting , we are able to reduce the gap width even more as shown in Fig. 7.
Another example of 3-D planning using a window with a rectangular hole in the middle is considered. By modifying the angle of the window’s inclination , we are able to verify the planner’s capability to generate agile trajectories as shown in Fig. 8.
There are a few parameters that significantly affect the planning performance including computation time, continuity and dynamics constraints. In this section, we analyze these relationships and provide guidance on how to set the parameters in our planner. In the above examples of 2-D and 3-D planning, we used the following settings:
As described in , a larger results in faster trajectories. The scale of should be comparable to the scale of the associated control effort. Here we use . The motion primitive duration should not be too small or too large. For moderate flight speeds, we find s to be a reasonable choice. A small makes the graph dense and requires more explorations to reach the goal, while a large may easily result in searching failure since the graph may be too sparse to cover the feasible region. The discretization in the control space also affects the density of the graph as shown in Fig. 2. Its effect is similar to – finer discretization in leads to a slower but more complete search and smoother trajectories and vice versa.
Vi-a Simulation Results
The proposed planner is used to generate trajectories in complicated environments as shown in Fig. 9. A geometric model of the environment is converted into a point cloud and used to construct an obstacle KD-tree.
In general, the computation time for finding the optimal trajectories in Fig. 9 is slow (Table I). As proposed in Section IV, we plan trajectories using acceleration control at first, based on which we plan the trajectory using jerk control. As shown in Table I, the computation time for hierarchical planning is much less than that for planning in the original 9 dimensional space with jerk input. We can also see in Fig. 10 that the refinement process tends to explore fewer states. As expected, the refined trajectory has a higher cost compared to the optimal trajectory .
Vi-B Real World Experiments
The experiments is aiming to demonstrate the feasibility of planned aggressive trajectories with a real robot. We use AscTec Hummingbird as our quadrotor platform, we also use VICON motion capture system to localize the quadrotor and the obstacle map is obtained by depth sensor in advance to generate trajectories. Using the robust feedback control , the robot is able to avoid hitting obstacles by following the planned trajectory. Fig. 11 shows several snapshots of the flight where the quadrotor needs to roll at in order to pass through the gap without hitting the white board.
The control errors in velocity and roll are plotted in Fig. 12. The commanded roll includes the feedback attitude errors such that it is not as smooth as the desired roll from the planned trajectory. The robot is able to track velocity properly up to , but clearly there exists lag in the attitude control. This is because the actual robot is not able to achieve specified angular velocity instantly due to the dynamics. A more accurate model for the quadrotor is to use snap as the control input instead of the jerk. The trajectory planned using the snap as the control input is straightforward to solve following the same pipeline as proposed in this paper, which has also been implemented in our open-sourced planner.
In this work, we extend our previous motion-primitive-based planning algorithm  to enable aggressive flight with attitude constraints in cluttered environments for an under-actuated quadrotor system. We also presented a hierarchical refinement process that uses prior lower-dimensional trajectories as heuristics to accelerate planning in higher dimensions. Our planner is the first to plan dynamic trajectories in cluttered environments in SE(3) while guaranteeing safety, trajectory smoothness, and optimality. We believe that in future work, it is possible to integrate the planner with onboard sensing, state estimation, and feedback control to obtain a fully autonomous quadrotor system that is able to fly aggressively but safely in unknown cluttered environments.
Linear Quadratic Minimum Time for Jerk Control
The heuristic function for graph search is an under-estimation of actual cost from the current state to the goal state by relaxing the dynamics and obstacles constraints. We try to find a state-to-state optimal trajectory of Problem 2, whose cost serves as the cost-to-go heuristic . The explicit solution for the optimal cost for velocity, acceleration control has been shown in . Here we show the explicit solution for jerk control.
Given a current state , the goal state , find the optimal trajectory according to the cost function
The minimum of can be derived by taking the derivative with respect to and finding the root of
Therefore, . The coefficients in (30) are derived as follows:
(1) Fully Defined
(2) Partially Defined
(3) Partially Defined
-  S. Liu, M. Watterson, S. Tang, and V. Kumar, “High speed navigation for quadrotors with limited onboard sensing,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016.
-  C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in Robotics Research. Springer, 2016, pp. 649–666.
-  H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuous-time trajectory optimization for online uav replanning,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct 2016, pp. 5332–5339.
-  R. Deits and R. Tedrake, “Efficient mixed-integer planning for uavs in cluttered environments,” in Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA), 2015.
-  J. Canny, B. Donald, J. Reif, and P. Xavier, “On the complexity of kinodynamic planning,” in Foundations of Computer Science, 29th Annual Symposium on, 1988, pp. 306–316.
-  D. Falanga, E. Mueggler, M. Faessler, and D. Scaramuzza, “Aggressive quadrotor flight through narrow gaps with onboard sensing and computing,” arXiv preprint arXiv:1612.00291, 2016.
-  G. Loianno, C. Brunner, G. McGrath, and V. Kumar, “Estimation, control, and planning for aggressive flight with a small quadrotor with a single camera and imu,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 404–411, April 2017.
-  T. Hirata and M. Kumon, “Optimal path planning method with attitude constraints for quadrotor helicopters,” in Int. Conf. on Advanced Mechatronic Systems, 2014, pp. 377–381.
-  I. Garcia and J. P. How, “Trajectory optimization for satellite reconfiguration maneuvers with position and attitude constraints,” in American Control Conference, 2005, pp. 889–894 vol. 2.
-  M. Watterson, T. Smith, and V. Kumar, “Smooth trajectory generation on SE(3) for a free flying space robot,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2016, pp. 5459–5466.
-  D. Brescianini and R. D’Andrea, “Design, modeling and control of an omni-directional aerial vehicle,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), May 2016.
-  S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Search-based motion planning for quadrotors using linear quadratic minimum time control,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2017.
-  D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA), 2011.
-  M. J. Van Nieuwstadt and R. M. Murray, “Real time trajectory generation for differentially flat systems,” 1997.
-  S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1688–1695, July 2017.
-  J. Chen, T. Liu, and S. Shen, “Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016, pp. 1476–1483.
-  J. Ichnowski and R. Alterovitz, “Fast nearest neighbor search in se (3) for sampling-based motion planning,” in Algorithmic Foundations of Robotics XI. Springer, 2015, pp. 197–214.
-  N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in IEEE Int. Conf. on Robotics and Automation, 2009, pp. 489–494.
-  M. Pivtoraiko, R. A. Knepper, and A. Kelly, “Differentially constrained mobile robot motion planning in state lattices,” Journal of Field Robotics, vol. 26, no. 3, pp. 308–333, 2009.
-  B. MacAllister, J. Butzke, A. Kushleyev, H. Pandey, and M. Likhachev, “Path planning for non-circular micro aerial vehicles in constrained environments,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp. 3933–3940.
-  M. Mueller, M. Hehn, and R. D’Andrea, “A computationally efficient motion primitive for quadrocopter trajectory generation,” IEEE Trans. on Robotics (T-RO), vol. 31, no. 6, pp. 1294–1310, 2015.
-  S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The International Journal of Robotics Research (IJRR), 2011.
E. A. Hansen and R. Zhou, “Anytime heuristic search,”
Journal of Artificial Intelligence Research, vol. 28, pp. 267–297, 2007.
-  M. Likhachev, G. Gordon, and S. Thrun, “ARA* : Anytime A* with Provable Bounds on Sub-Optimality,” in Advances in Neural Information Processing Systems, 2004, pp. 767–774.
-  S. Aine, S. Swaminathan, V. Narayanan, V. Hwang, and M. Likhachev, “Multi-heuristic a,” The International Journal of Robotics Research, vol. 35, no. 1-3, pp. 224–243, 2016.
-  C. M. Wilt and W. Ruml, “When does weighted A* fail?” 2012.
-  K. Gochev, B. Cohen, J. Butzke, A. Safonova, and M. Likhachev, “Path planning with adaptive dimensionality,” in Fourth annual symposium on combinatorial search, 2011.
-  T. Lee, M. Leoky, and N. H. McClamroch, “Geometric tracking control of a quadrotor uav on se (3),” in 49th IEEE conference on decision and control (CDC). IEEE, 2010, pp. 5420–5425.
-  M. Hehn and R. D’Andrea, “Quadrocopter trajectory generation and control,” IFAC Proceedings Volumes, vol. 44, no. 1, 2011.
-  R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),” in IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, May 9-13 2011.