Realtime Trajectory Generation for Quadrotors using
Bspline based Nonuniform Kinodynamic Search
Abstract
In this paper, we propose a timeefficient approach to generate safe, smooth and dynamically feasible trajectories for quadrotors in the obstaclecluttered environment. By using the uniform Bspline to represent trajectories, we transform the trajectory planning to a graphsearch problem of Bspline control points in discretized space. Highly strict convex hull property of Bspline is derived to guarantee the dynamical feasibility of the entire trajectory. A novel nonuniform kinodynamic search strategy is adopted, and the step length is dynamically adjusted during the search process according to the Euclidean signed distance field (ESDF), making the trajectory achieve reasonable timeallocation and be away from obstacles. Nonstatic initial and goal states are allowed, therefore it can be used for online local replanning as well as global planning. Extensive simulation and hardware experiments show that our method achieves higher performance compared with the stateoftheart method.
I Introduction
Many practical applications of quadrotors require safe navigation in cluttered environments, such as search, rescue, and exploration. Motion planning is the foundation of safe navigation, which generates trajectories that are followed by the motion controller.
Safety, smoothness and good timeallocation are important aspects of evaluating the quality of the trajectory. Smooth and dynamically feasible trajectory allows it to be well tracked by the quadrotor without significant control errors. A trajectory that maintains a safe distance from obstacles as much as possible is not only conducive to avoiding unexpected collision, but also facilitates observation of the environment due to less obstruction by obstacles. Timeallocation, also called time parameterization, affects the trajectory velocity and acceleration in different segments, which in turn affects the safety and efficiency of the execution of the trajectory. And it is also considered as one of the main factors leading to suboptimality of the trajectory.
In this paper, these aspects are considered simultaneously. Uniform Bspline is utilized to represent trajectory and convert the trajectory generation problem into control point placement. A timeefficient nonuniform kinodynamic search method based on Euclidean signed distance field (ESDF) is proposed, ensuring the safety and reasonable timeallocation, and achieving optimality in time duration and control cost. Main contributions of this paper are summarized as follows:

A uniform Bspline trajectory representation for quadrotors. A highly strict convex hull property is derived for a nonconservative dynamical feasibility check.

A timeefficient nonuniform kinodynamic search algorithm based on ESDF, which generates smooth, safe and reasonably timeallocated trajectories.

Implementation on simulator and hardware, for analyzing and verifying the performance. The source code of this work will be released on https://github.com/tlb9551/BNUKsearch.
Ii Related Works
Trajectory generation for quadrotor can be transformed into the generation of the timeparameterized curve due to its differentially flat [1]. When collision avoidance is considered in cluttered environments, curves such as Bsplines [2], Bézier [3] and piecewise polynomials [4, 5] are used to represent shapeconstrained trajectories.
Many existing planning methods take a twostep pipeline, i.e. a collisionfree path is planned at first, then the smoothness and timeallocation of the relevant trajectory are optimized based on the shape of the path. At the frontend, samplingbased [6, 7] and searchingbased [8, 9] methods are used to plan a collisionfree path. In the backend, gradientbased methods [10] and several other methods [3, 11, 12] are employed to guarantee the smoothness and dynamical feasibility. However, these methods separate the trajectory shape and trajectory parameterization, are susceptible to some problems. For example, global optimal trajectory or even feasible trajectory may not be inside the homology class of path which is generated by the frontend methods without dynamic consideration.
Some methods consider both the trajectory shape and the dynamics in parallel. Usenko et al. [14] used an optimizationbased method that integrated the smoothness, dynamic constraints and obstacleavoidance into the cost function of optimization but faced a low success fraction. Liu et al. [15] proposed a primitivebased search method, the dynamical feasibility was guaranteed but met the problem of timeefficiency when highorder control inputs were required.
The timeallocation of trajectory is another significant factor that affects the quality of trajectories. Ding et al. [13] refined the control points of Bspline while the total duration remains fixed. Fernbach et al. [16] sampled the duration of segments until the related quadratic program is solved, but it may be not suitable to generate trajectories with many segments. Gao et al. [3] generated timeindexed paths according to the density of obstacle, but the final trajectories do not strictly follow the heuristics after optimization. In this paper, we simultaneously deal with kinodynamic search and timeallocation of the trajectory, which shows noticeable benefits.
Iii Bspline based Trajectory Representation
Since the snap of the trajectory of quadrotor should be continuous [1], quintic uniform Bspline is selected to represent the trajectory of quadrotors. The Bspline is smooth and has some useful properties, which are suitable for our nonuniform kinodynamic search process.
Iiia Local Control Property and Kinodynamic Search
Searchbased trajectory generation methods need to evaluate the cost of candidate trajectory increments frequently, and the local properties of Bspline curves are tailored to this. The value of a Bspline curve of degree can be evaluated by the following equation:
(1) 
where are the control points corresponding to and are the blending functions which can be computed by the De Boorcox recursive formula [17].
Note that for uniform Bspline, the time interval between is fixed to be , and the is nozero only in the interval of , which means that a curve span in only depends on local control points , it is called local control property. Therefore, a quintic Bspline curve with control points can be divided into spans (Fig. 2 (a)).
We define as a uniform time representation, as a normalized parameter corresponding to the th span of the curve , where is the integer part of . Therefore, the Bspline curve can be evaluated by the following matrix representation as mentioned in [18]:
(2) 
where is the basis vector, is a constant basis matrix of quintic uniform Bspline and is the vector of local control points of th span.
The derivatives of curve with respect to time can be also expressed as matrix representation. And the derivative curves are also Bspline curves with a lower degree, whose control points are a constant linear combination of higher degree Bspline’s control points.
The integral over squared time derivatives of the th span, which expresses the control cost of quadrotor trajectory [1], can be also locally computed in closed form during the search process:
(3) 
where is a constant matrix and can be computed in advance. The local control property and the matrix representation of enable incremental calculation of the cost during the search process.
One understanding of Eq. 2 is that a span of quintic Bspline can be regarded as weighted combinations of six local control points, and the weight is the timevarying basis, . Fig. 2 (b) shows the weight of each control point that contributes to a span when the uniform time parameter . The weights of and are much higher than other control points, so the span will be close to the connection line between and as shown in Fig. 2 (a). As a result, we can easily confirm that the trajectory mostly has no collision with obstacles as long as the connection line of successive control points does not pass through obstacles. Note that it is not a sufficient condition, and we will introduce the convex hull property to build a sufficient condition in the next subsection.
IiiB Strict Convex Hull Property and Feasibility Checking
The dynamical feasibility of trajectory guarantees that trajectory can be well tracked by the motion controller while some additional tasks are performed well [19]. As an important property of Bspline, the convex hull property can be used to conservatively evaluate the spans. Existing work is faced with limitations due to its conservativeness, but the novel strict convex hull property we derived here can be used to evaluate the spans of Bspline in a nonconservative way, so as to assess more accuracy in feasibility checking.
The convex hull property of Bspline means that a span of Bspline lies within the convex hull of its local control points , where is the degree of Bspline (see Fig. 2 (c)). Since the derivatives of a Bspline curve are also Bspline curves with a lower degree [17], they share the same property: local control property and convex hull property. Thus the dynamical feasibility and collisionfree constraints can both be expressed as bounds on the placement of control points.
In the previous work [13], the convex hull property of Bspline is directly used to check the dynamical feasibility, a span is judged to be dynamically infeasible unless all the local control points of its derivative curve are within the border of the dynamically feasible domain, i.e. and . However, since the convex hull of uniform Bspline control points does not tightly wrap the span, this method has some limitations of conservativeness as shown in Fig. 2 (c).
To solve this problem, we try to find a linear transformation of local control points , so that the convex hull formed by more tightly wrap the span. We noticed that the Bézier curve also has convex hull property and the Bézier curve is guaranteed to pass through the first and the last control points, so we transform the local span of Bspline into a Bézier curve representation to achieve tighter convex hull(Fig. 2 (d)). Given:
(4) 
Thus:
(5) 
where is the constant basis matrix of Bézier curve, and are the local control points of Bézier curve. Thus we can use the strict convex hull property for dynamically feasible checking as follows.
Dynamical feasibility checking: firstly, the control points of the derivatives curve of trajectory are evaluated. Then we computed the corresponding Bézier control points with Eq. 5. Finally, if all the Bézier control points are in the dynamically feasible domain, i.e. and , the span of Bspline trajectory is judged to be dynamically feasible (Fig. 2 (d)).
IiiC Nonstatic Initial and Goal State
In order to keep the trajectory continuous up to a higher derivative of the quadrotor when the replanning happens in flying, the nonstatic initial state should be allowed. Given the nonstatic state of the quadrotor, position and its derivative, as the beginning of trajectory, the first five control points can be obtained by solving the following equation set:
(6) 
and the 6th control point is arbitrary.
The goal control points can also be calculated by a similar approach. In our implementation, the goal state is set as a waypoint with specified velocity. Therefore, only two equations related to position and velocity, confirm the last two control points of the whole trajectory.
Iv Nonuniform Kinodynamic Search
As discussed in the previous section, the whole Bspline trajectory can be expressed as a set of Bspline spans. Successive spans share some same control points, i.e. the last control points of span are the first control points of . The increase in control points leads to an increase in the spans and total trajectory duration. Therefore, the trajectory planning is transformed as a problem to find a set of Bspline control points, which generates a collisionfree, dynamically feasible and costoptimized Bspline trajectory between start and goal state. Such a problem can be treated as a graph optimization problem and solved by our searchbased algorithm. For convenience, we call it BNUK (Bspline based Nonuniform Kinodynamic) search (Alg. 1).
Iva Algorithm Overview
The graph structure is chosen as follows. Vertices is the cell center of voxels in discretized 3D space, indicating control points, and edges is their connections. Each vertex is connected to its neighbors in 26 directions like the usual A* search algorithm in 3D space but obeys the rule of choosing specific step length between neighbors, which will be exhaustively discussed in the next subsection.
Thus the kinodynamic search problem is to find an ordered set of control points from the graph , which satisfy the dynamical feasibility, and minimizes the cost of trajectory as given below,
(7) 
which is a weight of control cost and trajectory duration. The cost can be evaluated incrementally due to the local control property.
IvB Nonuniform Node Expansion
Node expansion refers to finding neighboring nodes of the current node and is implemented in function in Alg. 1.
Different from traditional path finding as A* search, the output of BNUK search, i.e. a set of control points is not required to be connected in 3D discrete space. Therefore, we propose a novel nonuniform node expanding strategy. The step length of the node expansion, i.e. the spacing between two control points, is dynamically adjusted during the search process.
As discussed in Sec. III, the spacing length between control points will affect the derivatives of the trajectory due to the fixed time interval . We argue that it is reasonable to reduce the velocity of quadrotors in the vicinity of the obstacles for security consideration, because the actual trajectory is always deviated from the planned trajectory due to the existence of control errors. Similarly, the velocity should be increased to take full advantage of quadrotor’s dynamics when away from obstacles. Therefore the spacing length between control points needs to be set in reference to some quantity describing the distance from the obstacle.
The Euclidean signed distance field (ESDF) is employed as the reference. The cells of ESDF store the Euclidean distance to the nearest obstacle. We use the following function to mapping distance to step length of node expansion:
(8) 
where is a parameter related to the radius of the quadrotor. This function is limited under , so it does not make the spacing between control points too large to cause the trajectory exceeding the dynamical velocity limit. And step length is proportional to the distance and is equal to 0 when the distance is under , meaning that the node is always expanding in the collisionfree area. This avoids timeconsuming obstacle inflation and also improves safety.
The nonuniform node expansion benefits the search process in three aspects. Firstly, safety is improved because the step length is always shorter than the distance to the nearest obstacle, no control point will be set in nonfree or dangerous area. Secondly, since the trajectory duration is a part of cost function, equal to , expanding to a node with large step length is less costly than a node with small step length. Thus the trajectory will be naturally away from obstacles to achieve less trajectory duration. Thirdly, trajectory is reasonably timeallocated, slowly in obstacledense area and fast in obstaclesparse areas.
IvC Dynamical Feasibility Checking and Heuristic Function
The dynamical checking and cost evaluating both need the value of local control points, thus we utilize the function to get the control points of the local span. Then dynamical checking and cost evaluating are done by the method discussed in Sec.III. As for the collisionfree checking, it is timeconsuming and unnecessary so we leave it out of the search process.
The heuristic function is useful to speed up the search process due to the reduction of expanded node number, it is an estimation of cost from the current node to the goal:
(9) 
where denotes the diagonal distance. This heuristic function is admissible since it underestimates the optimal cost from to , and is efficient in speeding up the search process.
V Experiments
Va Comparison with StateoftheArt Method
In this section, we compared our BNUK search with a stateoftheart method proposed by Gao et al. [3] in two ways, trajectory generation on random maps and navigation simulation on large maps.
Traj.  Traj.  Comp.  Avg.  Avg.  Min.  Avg.  
Length  Time  Time  Vel.  Acc.  Dis.  Dis.  
(m)  (s)  (ms)  (m/s)  (m/)  (m)  (m)  
Our method  12.95  11.69  16.9  1.11  0.46  0.97  1.82 
Gao’s method [3]  12.39  11.94  68.56  1.04  0.29  0.54  1.63 
on 50 Random Maps
Gao’s method uses the fast marching in the velocity field transformed from ESDF to find a timeindexed path, the trajectories represented by piecewise Bézier curves are generated by solving a convex quadratic program. We compared our method with Gao’s method because both methods use the obstacle density information in EDSF to generate trajectories with reasonable timeallocation, but in different ways and trajectory representation, resulting in different performance in timeefficiency and trajectory quality.
We generate 50 random maps and plan trajectories from the center to side and corner with two methods, simulating local replanning that often occurs in autonomous navigation. The map size is and resolution is . The maximum velocity and acceleration are set as and . As the result shown in Tab. I, both methods generate dynamically feasible trajectories and behave similarly in terms of trajectory length and duration. The mean and minimum distance from obstacles of our methods are larger than Gao’s method, meaning that the trajectory generated by our methods is safer. In addition, our method is much more timeefficient, and the average calculation time is less than a quarter of theirs.
We also simulate the navigation in several large random forest maps with two methods. RHC (Receding Horizon Control) [20] is used as the same strategy. Replanning happens when the quadrotor arrives at the border of prior sensing horizon or when a collision is detected. The map size is and resolution is , the local sensing map size is . We set the same maximum velocity and acceleration as and , same start and goal are set so that the trajectory length is longer than . As shown in Fig. 4 (d), our trajectory (red curve) is safer than Gao’s (green curve). Tab. II shows the average data of two methods, the trajectory length is similar but our method reaches a lower duration. Our method is much more timeefficient. The replanning times of Gao’s method are higher than ours because it encountered failures during the trajectory optimization process.
Traj.  Traj.  Comp.  Replan  
Length  Time  Time  Times  
(m)  (s)  (ms)  
Our method  55.33  46.95  44.6  8.6 
Gao’s method [3]  51.75  56.94  194.27  26.3 
VB Flight Experiment
Flight experiment is done to demonstrate the collision avoidance and realtime calculation of our method. We use a quadrotor based on the Pixhawk flight controller, and an UP2 embedded board is utilized as the onboard computer. A motion capture system is used to locate the quadrotor and obstacles. The map size of our indoor flight is , and the resolution is . The maximum velocity and acceleration are set as and . We employ the controller in [21] to track the trajectory generated by our method. Fig. 6 shows the snapshots of our experiment. The time of trajectory generation is . The max velocity, acceleration, and jerk of trajectory are , and , thus the whole trajectory is dynamically feasible and smooth. The minimum distance to the obstacle is while no obstacle inflation is applied, which means the trajectory generated by our method is quite safe. We recommend readers to watch the attached video for more details.
Vi Conclusion
In this paper, we propose a timeefficient searchbased approach to generate safe, smooth and dynamically feasible trajectories for quadrotors. Benefiting from ESDFbased nonuniform search, our algorithm generates trajectories away from obstacles and has reasonable timeallocation. The highly strict convex hull property we derived makes the trajectory dynamically feasible and not too conservative. We have also enabled nonzero initial and termination states to be used so that algorithms can be applied in different scenarios. Extensions to this work may include developing informative heuristics for our search process and exploring the application of the nonuniform rational Bspline curve in trajectory planning.
References
 [1] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. IEEE Int. Conf. Robotics and Automation, May 2011, pp. 2520–2525.
 [2] E. Koyuncu and G. Inalhan, “A probabilistic bspline motion planning algorithm for unmanned helicopters flying in dense 3D environments,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, Sept. 2008, pp. 815–821.
 [3] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial,” in Proc. IEEE Int. Conf. Robotics and Automation (ICRA), May 2018, pp. 344–351.
 [4] 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.
 [5] L. Zhao, H. Wang, and W. Chen, “Optimal trajectory planning for manipulators with flexible curved links,” in International Conference on Intelligent Autonomous Systems. Springer, 2016, pp. 1013–1025.
 [6] S. Karaman and E. Frazzoli, “Samplingbased algorithms for optimal motion planning,” The international journal of robotics research, vol. 30, no. 7, pp. 846–894, 2011.
 [7] Z. Wei, W. Chen, H. Wang, and J. Wang, “Manipulator motion planning using flexible obstacle avoidance based on model learning,” International Journal of Advanced Robotic Systems, vol. 14, no. 3, p. 1729881417703930, 2017.
 [8] M. Likhachev, G. J. Gordon, and S. Thrun, “Ara*: Anytime a* with provable bounds on suboptimality,” in Advances in neural information processing systems, 2004, pp. 767–774.
 [9] Q. H. Do, L. Han, H. Tehrani Nik Nejad, and S. Mita, “Safe path planning among multi obstacles,” in Proc. IEEE Intelligent Vehicles Symp. (IV), June 2011, pp. 332–338.
 [10] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in Proc. IEEE Int. Conf. Robotics and Automation, May 2009, pp. 489–494.
 [11] J. Chen, T. Liu, and S. Shen, “Online generation of collisionfree trajectories for quadrotor flight in unknown cluttered environments,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016, pp. 1476–1483.
 [12] 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 3D complex environments,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1688–1695, July 2017.
 [13] W. Ding, W. Gao, K. Wang, and S. Shen, “Trajectory replanning for quadrotors using kinodynamic search and elastic optimization,” in Proc. IEEE Int. Conf. Robotics and Automation (ICRA), May 2018, pp. 7595–7602.
 [14] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Realtime trajectory replanning for mavs using uniform bsplines and a 3D circular buffer,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems (IROS), Sept. 2017, pp. 215–222.
 [15] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Searchbased motion planning for quadrotors using linear quadratic minimum time control,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 2872–2879.
 [16] P. Fernbach, S. Tonneau, and M. Taïx, “Croc: Convex resolution of centroidal dynamics trajectories to provide a feasibility criterion for the multi contact planning problem,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems (IROS), Oct. 2018, pp. 1–9.
 [17] L. Piegl and W. Tiller, The NURBS book. Springer Science & Business Media, 2012.
 [18] K. Qin, “General matrix representations for bsplines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000.
 [19] H. Wang, Y. Lai, and W. Chen, “The time optimal trajectory planning with limitation of operating task for the tokamak inspecting manipulator,” Fusion Engineering and Design, vol. 113, pp. 57–65, 2016.
 [20] J. Bellingham, A. Richards, and J. P. How, “Receding horizon control of autonomous aerial vehicles,” in Proc. American Control Conf. (IEEE Cat. No.CH37301), vol. 5, May 2002, pp. 3741–3746 vol.5.
 [21] T. Lee, M. Leok, and N. H. McClamroch, “Geometric tracking control of a quadrotor UAV on se(3),” in Proc. 49th IEEE Conf. Decision and Control (CDC), Dec. 2010, pp. 5420–5425.