Real-time Trajectory Generation for Quadrotors using B-spline based Non-uniform Kinodynamic Search

TL;DR

This paper introduces a fast, safe, and smooth trajectory planning method for quadrotors in cluttered environments using B-spline representation and a novel non-uniform kinodynamic search, enabling online replanning.

Contribution

It presents a new non-uniform kinodynamic search strategy combined with B-spline control point graph-search for efficient, feasible trajectory generation in complex environments.

Findings

Outperforms state-of-the-art methods in simulation and hardware tests.

Ensures dynamical feasibility through B-spline convex hull property.

Supports online local and global replanning with dynamic initial and goal states.

Abstract

In this paper, we propose a time-efficient approach to generate safe, smooth and dynamically feasible trajectories for quadrotors in obstacle-cluttered environment. By using the uniform B-spline to represent trajectories, we transform the trajectory planning to a graph-search problem of B-spline control points in discretized space. Highly strict convex hull property of B-spline is derived to guarantee the dynamical feasibility of the entire trajectory. A novel non-uniform 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 time-allocation and be away from obstacles. Non-static 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 comparing with the state-of-the-art method.

Tables

Table 1. TABLE I: Comparison of Trajectory Generation on 50 Random Maps

	Traj.	Traj.	Comp.	Avg.	Avg.	Min.	Avg.
	Length	Time	Time	Vel.	Acc.	Dis.	Dis.
	(m)	(s)	(ms)	(m/s)	(m/$s^2$)	(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

Table 2. TABLE II: Comparison of Replanning on Large Random Maps

	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

Equations

$$\mathbf{c}(t)=\sum _{i=0}^n{\mathbf{p}}_i{B}_{i,k}(t)$$

$${\mathbf{c}}_i(u(t))={\mathbf{b}}_6^{\top }{\mathrm{M}}_6{\mathbf{P}}_i$$

$$\begin{array}{rl}{\mathbf{E}}_{{}_i}^c& =\sum _{l=1}^4{\int }_{t_i}^{t_{i+1}}{w}_l{\left(\frac{d{\mathbf{c}}_i(u(t))}{d^{l}t}\right)}^{2}dt\\ & =\sum _{l=1}^4{w}_l{\mathbf{P}}_i^{\top }{\mathrm{M}}_6^{\top }{\mathrm{Q}}_l{\mathrm{M}}_6{\mathbf{P}}_i\end{array}$$

$${\mathbf{c}}_i(u)={\mathbf{b}}_k^{\top }{\mathrm{M}}_k{\mathbf{P}}_i={\mathbf{b}}_k^{\top }{\mathrm{B}}_k{\mathbf{Q}}_i$$

$${\mathbf{Q}}_i={{\mathrm{B}}_k}^{-1}{\mathrm{M}}_k{\mathbf{P}}_i$$

$${\frac{d{\mathbf{c}}_i(u)}{d{t}^l}|}_{u=0}={{\mathbf{b}}_{6-l}^{\top }|}_{u=0}{\mathrm{M}}_{6-l}{\mathrm{S}}_{\mathrm{l}}{\mathbf{P}}_i={\mathbf{p}}^{(l)},l=0,1,2,3$$

$$\underset{\mathcal{S}\subset \mathcal{V}}{\min }J(\mathcal{S})=\sum _{i=0}^{n-k+1}\lambda {\mathbf{E}}_i^c+(n-k+1)\Delta t$$

$$f(d)=\left\{\begin{array}[]{l}0,\\ d-\tau,\\ {v_{\max}}\cdot\Delta t,\end{array}\right.\begin{array}[]{*{20}{c}}\begin{array}[]{r}d\leq\tau\\ \tau<d\leq\tau+{v_{\max}}\cdot\Delta t\\ d\geq\tau+{v_{\max}}\cdot\Delta t\end{array}\end{array}{\rm{}}$$

$$HeuristicCost(p_{nbr})=DiagDis(p_{nbr},p_{goal})/v_{\max }$$

Full text

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 time-allocation 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. Time-allocation, 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 sub-optimality of the trajectory.

In this paper, these aspects are considered simultaneously. Uniform B-spline is utilized to represent trajectory and convert the trajectory generation problem into control point placement. A time-efficient non-uniform kinodynamic search method based on Euclidean signed distance field (ESDF) is proposed, ensuring the safety and reasonable time-allocation, and achieving optimality in time duration and control cost. Main contributions of this paper are summarized as follows:

• •

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

• •

A time-efficient non-uniform kinodynamic search algorithm based on ESDF, which generates smooth, safe and reasonably time-allocated 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 time-parameterized curve due to its differentially flat [1]. When collision avoidance is considered in cluttered environments, curves such as B-splines [2], Bézier [3] and piecewise polynomials [4, 5] are used to represent shape-constrained trajectories.

Many existing planning methods take a two-step pipeline, i.e. a collision-free path is planned at first, then the smoothness and time-allocation of the relevant trajectory are optimized based on the shape of the path. At the front-end, sampling-based [6, 7] and searching-based [8, 9] methods are used to plan a collision-free path. In the back-end, gradient-based methods [10] and several other methods [3, 11] 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 front-end methods without dynamic consideration.

Some methods consider both the trajectory shape and the dynamics in parallel. Usenko et al. [14] used an optimization-based method that integrated the smoothness, dynamic constraints and obstacle-avoidance into the cost function of optimization but faced a low success fraction. Liu et al. [15] proposed a primitive-based search method, the dynamical feasibility was guaranteed but met the problem of time-efficiency when high-order control inputs were required.

The time-allocation of trajectory is another significant factor that affects the quality of trajectories. Ding et al. [13] refined the control points of B-spline 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 time-indexed 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 time-allocation of the trajectory, which shows noticeable benefits.

III B-spline based Trajectory Representation

Since the snap of the trajectory of quadrotor should be continuous [1], quintic uniform B-spline is selected to represent the trajectory of quadrotors. The B-spline is smooth and has some useful properties, which are suitable for our non-uniform kinodynamic search process.

III-A Local Control Property and Kinodynamic Search

Search-based trajectory generation methods need to evaluate the cost of candidate trajectory increments frequently, and the local properties of B-spline curves are tailored to this. The value of a B-spline curve of degree $k-1$ can be evaluated by the following equation:

[TABLE]

where ${\bm{p}_{i}}\in{{\rm{\mathbb{R}}}^{3}}$ are the control points corresponding to ${t_{i}},i\in\{0,...,n\}$ and ${B_{i,k}}(t)$ are the blending functions which can be computed by the De Boor-cox recursive formula [17].

Note that for uniform B-spline, the time interval between ${t_{i}}$ is fixed to be $\Delta t$, and the ${B_{i,k}}(t)$ is no-zero only in the interval of $t\in[{t_{i}},{t_{i+k-1}})$, which means that a curve span in $t\in[{t_{i}},{t_{i+1}})$ only depends on local $k$ control points $[{\bm{p}_{i}},...,{\bm{p}_{i+k-1}}]$, it is called local control property. Therefore, a quintic B-spline curve with $n$ control points can be divided into $n-5$ spans (Fig. 2 (a)).

We define $s(t)=(t-{t_{0}})/\Delta t$ as a uniform time representation, $u(t)=s(t)-i\in[0,1]$ as a normalized parameter corresponding to the $i$-th span of the curve ${\bm{c}_{i}}(u(t))$, where $i$ is the integer part of $s(t)$. Therefore, the B-spline curve can be evaluated by the following matrix representation as mentioned in [18]:

[TABLE]

where {\bm{b}_{6}^{\top}}=[\begin{array}[]{*{20}{c}}1&u&{{u^{2}}}&{...}&{{u^{5}}}\end{array}] is the basis vector, ${\rm M_{6}}\in{\mathbb{R}^{6\times 6}}$ is a constant basis matrix of quintic uniform B-spline and {\bm{P}_{i}}=[\begin{array}[]{*{20}{c}}{{\bm{p}_{i}}}&{{\bm{p}_{i+1}}}&{...}&{{\bm{p}_{i+5}}}\end{array}]^{\top}\in{\mathbb{R}^{6\times 3}} is the vector of local control points of $i$-th span.

The derivatives of curve with respect to time can be also expressed as matrix representation. And the derivative curves are also B-spline curves with a lower degree, whose control points are a constant linear combination of higher degree B-spline’s control points.

The integral over squared time derivatives of the $i$-th span, which expresses the control cost of quadrotor trajectory [1], can be also locally computed in closed form during the search process:

[TABLE]

where ${Q_{l}}=\frac{1}{{{{\left({\Delta t}\right)}^{2l-1}}}}\int_{0}^{1}{\left({\frac{{d\bm{b}}}{{{d^{l}}u}}}\right){{\left({\frac{{d\bm{b}}}{{{d^{l}}u}}}\right)}^{\top}}du}$ is a constant matrix and can be computed in advance. The local control property and the matrix representation of $\bm{E}_{{}_{i}}^{c}$ enable incremental calculation of the cost during the search process.

One understanding of Eq. 2 is that a span of quintic B-spline can be regarded as weighted combinations of six local control points, and the weight is the time-varying basis, $\bm{b}^{\top}_{6}(u){{\rm{M}}_{6}}$. Fig. 2 (b) shows the weight of each control point that contributes to a span when the uniform time parameter $u\in[0,1]$. The weights of ${\bm{p}_{3}}$ and ${\bm{p}_{4}}$ are much higher than other control points, so the span will be close to the connection line between ${\bm{p}_{3}}$ and ${\bm{p}_{4}}$ 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.

III-B 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 B-spline, 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 B-spline in a nonconservative way, so as to assess more accuracy in feasibility checking.

The convex hull property of B-spline means that a span of B-spline ${\bm{c}_{i}}(u)$ lies within the convex hull of its local control points ${\bm{p}_{i+j}}(j=0,...,k-1)$, where $k-1$ is the degree of B-spline (see Fig. 2 (c)). Since the derivatives of a B-spline curve are also B-spline curves with a lower degree [17], they share the same property: local control property and convex hull property. Thus the dynamical feasibility and collision-free constraints can both be expressed as bounds on the placement of control points.

In the previous work [13], the convex hull property of B-spline 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. $\left\|{\bm{p}_{{}_{i+j}}^{\prime}}\right\|<{v_{\max}}$ and $\left\|{\bm{p}_{{}_{i+j}}^{\prime\prime}}\right\|<{a_{\max}}$. However, since the convex hull of uniform B-spline 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 ${\bm{Q}_{i}}={\rm{L}}{\bm{P}_{i}}$, so that the convex hull formed by ${\bm{Q}_{i}}$ 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 B-spline into a Bézier curve representation to achieve tighter convex hull(Fig. 2 (d)). Given:

[TABLE]

Thus:

[TABLE]

where ${\rm B_{k}}$ is the constant basis matrix of Bézier curve, and ${\bm{Q}_{i}}$ 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. $\left\|{\bm{q}_{{}_{i+j}}^{\prime}}\right\|<{v_{\max}}$ and $\left\|{\bm{q}_{{}_{i+j}}^{\prime\prime}}\right\|<{a_{\max}}$, the span of B-spline trajectory is judged to be dynamically feasible (Fig. 2 (d)).

III-C Non-static 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 non-static initial state should be allowed. Given the non-static state of the quadrotor, position and its derivative, $\left\{{\bm{p},{\bm{p}^{\prime}},{\bm{p}^{\prime\prime}},{\bm{p}^{\prime\prime\prime}}}\right\}$ as the beginning of trajectory, the first five control points can be obtained by solving the following equation set:

[TABLE]

and the 6-th 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 Non-uniform Kinodynamic Search

As discussed in the previous section, the whole B-spline trajectory can be expressed as a set of B-spline spans. Successive spans share some same control points, i.e. the last $k-1$ control points of span ${\bm{c}_{i}}$ are the first $k-1$ control points of ${\bm{c}_{i+1}}$. 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 B-spline control points, which generates a collision-free, dynamically feasible and cost-optimized B-spline trajectory between start and goal state. Such a problem can be treated as a graph optimization problem and solved by our search-based algorithm. For convenience, we call it BNUK (B-spline based Non-uniform Kinodynamic) search (Alg. 1).

IV-A Algorithm Overview

The graph structure ${\cal G}:=({\cal V}{,}{\cal E})$ is chosen as follows. Vertices ${\cal V}$ is the cell center of voxels in discretized 3-D space, indicating control points, and edges ${\cal E}$ is their connections. Each vertex is connected to its neighbors in 26 directions like the usual A* search algorithm in 3-D 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 ${\cal S}:=\left\{{{\bm{p}_{1}},{\bm{p}_{2}},...,{\bm{p}_{n}}}\right\}$ from the graph ${\cal G}$, which satisfy the dynamical feasibility, and minimizes the cost of trajectory as given below,

[TABLE]

which is a weight of control cost and trajectory duration. The cost can be evaluated incrementally due to the local control property.

IV-B Non-uniform Node Expansion

Node expansion refers to finding neighboring nodes of the current node and is implemented in function $NodeExpansion({p_{cur}})$ in Alg. 1.

Different from traditional path finding as A* search, the output of BNUK search, i.e. a set of control points ${\cal S}$ is not required to be connected in 3-D discrete space. Therefore, we propose a novel non-uniform 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 $\Delta t$. 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 $f(d)$ to mapping distance $d$ to step length of node expansion:

[TABLE]

where $\tau$ is a parameter related to the radius of the quadrotor. This function is limited under ${v_{\max}}\cdot\Delta t$, 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 $\tau$, meaning that the node is always expanding in the collision-free area. This avoids time-consuming obstacle inflation and also improves safety.

The non-uniform 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 non-free or dangerous area. Secondly, since the trajectory duration is a part of cost function, equal to ${(n-k+1)\Delta t}$, 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 time-allocated, slowly in obstacle-dense area and fast in obstacle-sparse areas.

IV-C 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 $Retrieve()$ 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 collision-free checking, it is time-consuming 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:

[TABLE]

where $DiagDis()$ denotes the diagonal distance. This heuristic function is admissible since it underestimates the optimal cost from $p_{nbr}$ to $p_{end}$, and is efficient in speeding up the search process.

V Experiments

V-A Comparison with State-of-the-Art Method

In this section, we compared our BNUK search with a state-of-the-art method proposed by Gao et al. [3] in two ways, trajectory generation on random maps and navigation simulation on large maps.

Gao’s method uses the fast marching in the velocity field transformed from ESDF to find a time-indexed 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 time-allocation, but in different ways and trajectory representation, resulting in different performance in time-efficiency 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 $20m\times 20m\times 4m$ and resolution is $0.2m$. The maximum velocity and acceleration are set as $1.6m/s$ and $1.6m/{s^{2}}$. 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 time-efficient, 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 $50m\times 50m\times 5m$ and resolution is $0.2m$, the local sensing map size is $20m\times 20m\times 5m$. We set the same maximum velocity and acceleration as $1.6m/s$ and $1.6m/{s^{2}}$, same start and goal are set so that the trajectory length is longer than $50m$. 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 time-efficient. The replanning times of Gao’s method are higher than ours because it encountered failures during the trajectory optimization process.

V-B Flight Experiment

Flight experiment is done to demonstrate the collision avoidance and real-time 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 $6m\times 6m\times 2m$, and the resolution is $0.1m$. The maximum velocity and acceleration are set as $0.6m/s$ and $0.6m/{s^{2}}$. 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 $42ms$. The max velocity, acceleration, and jerk of trajectory are $0.53m/s$, $0.43m/{s^{2}}$ and $0.63m/{s^{3}}$, thus the whole trajectory is dynamically feasible and smooth. The minimum distance to the obstacle is $0.80m$ 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 time-efficient search-based approach to generate safe, smooth and dynamically feasible trajectories for quadrotors. Benefiting from ESDF-based non-uniform search, our algorithm generates trajectories away from obstacles and has reasonable time-allocation. The highly strict convex hull property we derived makes the trajectory dynamically feasible and not too conservative. We have also enabled non-zero 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 non-uniform rational B-spline curve in trajectory planning.