Real-Time Trajectory Replanning for MAVs using Uniform B-splines and a 3D Circular Buffer

TL;DR

This paper introduces a real-time local trajectory replanning method for MAVs that handles dynamic obstacles using a 3D circular buffer and uniform B-splines, enabling smooth and efficient trajectory updates in cluttered environments.

Contribution

It presents a novel real-time replanning algorithm that extends static environment planners to dynamic scenarios using a moving occupancy grid and B-spline trajectories.

Findings

Successfully handles unmodeled and dynamic obstacles in real-time

Maintains MAV proximity to global trajectory during replanning

Ensures smooth and computationally efficient trajectory updates

Abstract

In this paper, we present a real-time approach to local trajectory replanning for microaerial vehicles (MAVs). Current trajectory generation methods for multicopters achieve high success rates in cluttered environments, but assume that the environment is static and require prior knowledge of the map. In the presented study, we use the results of such planners and extend them with a local replanning algorithm that can handle unmodeled (possibly dynamic) obstacles while keeping the MAV close to the global trajectory. To ensure that the proposed approach is real-time capable, we maintain information about the environment around the MAV in an occupancy grid stored in a three-dimensional circular buffer, which moves together with a drone, and represent the trajectories by using uniform B-splines. This representation ensures that the trajectory is sufficiently smooth and simultaneously allows for efficient optimization.

Tables

Table 1. TABLE I : Comparison of different path planning approaches. All results except thouse of the presented study are taken from [ 17 ] . Our approach performs similarly to approaches using polynomial splines without restarts, which indicates that B-splines can represent trajectories similar to those represented by polynomial splines. Lower computation times of our approach can be explained by the fact that unconstrained optimization occurs directly on the control points, unlike other approaches where the problem must first be transformed into an unconstrained form.

Algorithm	Success Fraction	Mean Norm. Path Length	Mean Compute time [s]
Inf. RRT* + Poly	0.9778	1.1946	2.2965
RRT Connect + Poly	0.9444	1.6043	0.5444
CHOMP N = 10	0.3222	1.0162	0.0032
CHOMP N = 100	0.5000	1.0312	0.0312
CHOMP N = 500	0.3333	1.0721	0.5153
[17] S = 2 jerk	0.4889	1.1079	0.0310
[17] S = 3 vel	0.4778	1.1067	0.0793
[17] S = 3 jerk	0.5000	1.0996	0.0367
[17] S = 3 jerk + Restart	0.6333	1.1398	0.1724
[17] S = 3 snap + Restart	0.6222	1.1230	0.1573
[17] S = 3 snap	0.5000	1.0733	0.0379
[17] S = 4 jerk	0.5000	1.0917	0.0400
[17] S = 5 jerk	0.5000	1.0774	0.0745
Ours C = 2	0.4777	1.0668	0.0008
Ours C = 3	0.4777	1.0860	0.0011
Ours C = 4	0.4888	1.1104	0.0015
Ours C = 5	0.5111	1.1502	0.0021
Ours C = 6	0.5555	1.1866	0.0028
Ours C = 7	0.5222	1.2368	0.0038
Ours C = 8	0.4777	1.2589	0.0054
Ours C = 9	0.5777	1.3008	0.0072

Table 2. TABLE II : Mean computation time for operations involved in trajectory replanning in the simulation experiment with depth map measurements sub-sampled to 160 × 120 160 120 160\times 120 and seven control points optimized.

Operation	Computing 3D points	Moving volume	Inserting measurements	SDF computation	Trajectory optimization
Time [ms]	0.265	0.025	0.518	9.913	3.424

Equations

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

$$p(u(t))$$

$$M_6=\frac{1}{5!}\left(\begin{array}{cccccc}1& 26& 66& 26& 1& 0\\ -5& -50& 0& 50& 5& 0\\ 10& 20& -60& 20& 10& 0\\ -10& 20& 0& -20& 10& 0\\ 5& -20& 30& -20& 5& 0\\ -1& 5& -10& 10& -5& 1\end{array}\right).$$

$$p^{\prime }(u(t))$$

$$p^{\prime \prime }(u(t))$$

$$E_q$$

$$={\left(\begin{array}{c}{p}_{i-2}\\ p_{i-1}\\ p_i\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\end{array}\right)}^T{M}_6^{T}Q{M}_6\left(\begin{array}{c}{p}_{i-2}\\ p_{i-1}\\ p_i\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\end{array}\right),$$

$$Q$$

$$=\frac{1}{\Delta t^3}\left(\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 8& 12& 16& 20\\ 0& 0& 12& 24& 36& 48\\ 0& 0& 16& 36& 57.6& 80\\ 0& 0& 20& 48& 80& 114.286\end{array}\right).$$

$$insideVolume(x)$$

$$address(x)$$

$$insideVolume(x)$$

$$address(x)$$

$$E_{total}$$

$$E_{ep}$$

$$E_c$$

$$c(x)=\{\begin{array}{ll}\frac{1}{2\tau }(d(x)-\tau {)}^2& \text{mbox}ifd(x)\le \tau \\ 0& \text{mbox}ifd(x)>\tau ,\end{array}$$

$$E_{ep}$$

$$l(x)=\{\begin{array}{ll}\exp ((p^{(k)}(x){)}^2-(p_{max}^k{)}^2)-1& \text{mbox}if{p}^{(k)}(x)>p_{max}^k\\ 0& \text{mbox}if{p}^{(k)}(x)\le p_{max}^k\end{array}$$

Full text

I Introduction

In recent years, microaerial vehicles (MAVs) have gained popularity in many practical applications such as aerial photography, inspection, surveillance and even delivery of goods. Most commercially available drones assume that the path planned by the user is collision-free or provide only limited obstacle-avoidance capabilities. To ensure safe navigation in the presence of unpredicted obstacles a replanning method that generates a collision-free trajectory is required.

Formulation of the trajectory generation problem largely depends on the application and assumptions about the environment. In the case where an MAV is required to navigate a cluttered environment, possibly an indoor one, we would suggest subdividing the problem into two layers. First, we assume that a map of the environment is available and a trajectory from a specified start point to the goal point is planned in advance.

This task has been a popular research topic in recent years, and several solutions have been proposed by Achtelik et al. [1] and Richter et al. [21]. They used occupancy representation of the environment to check for collisions and searched for the valid path in a visibility graph constructed using sampling based planners. Thereafter, they followed the approach proposed by Mellinger and Kumar [14] to fit polynomial splines through the points of the planned path to generate a smooth feasible trajectory. The best algorithms of this type can compute a trajectory through tens of waypoints in several seconds.

To cope with any unmodeled, possibly dynamic, obstacle a lower planning level is required, which can generate a trajectory that keeps the MAV close to the global path and simultaneously avoids unpredicted obstacles based on an environment representation constructed from the most recent sensor measurements. This replanning level should run in several milliseconds to ensure the safety of MAVs operating at high velocities.

The proposed approach solves a similar problem as that solved by Oleynikova et al. [17], but instead of using polynomial splines for representing the trajectory we propose the use of B-splines and discuss their advantages over polynomial splines for this task. Furthermore, we propose the use of a robocentric, fixed-size three-dimensional (3D) circular buffer to maintain local information about the environment. Even though the proposed method cannot model arbitrarily large occupancy maps, as some octree implementations, faster look-up and measurement insertion operations make it better suited for real-time replanning tasks.

We demonstrate the performance of the system in several simulated and real-world experiments, and provide open-source implementation of the software to community.

The contributions of the present study are as follows:

• •

Formulation of local trajectory replanning as a B-spline optimization problem and thorough comparison with alternative representations (polynomial, discrete).

• •

High-performance 3D circular buffer implementation for local obstacle mapping and collision checking and comparison with alternative methods.

• •

System design and evaluation on realistic simulator and real hardware, in addition to performance comparison with existing methods.

In addition to analyzing the results presented in the paper, we encourage the reader to watch the demonstration video and inspect the available code, which can be found at

https://vision.in.tum.de/research/robotvision/replanning

II Related Work

In this section, we describe the studies relevant to different aspects of collision-free trajectory generation. First, we discuss existing trajectory generation strategies and their applications to MAV motion planning. Thereafter, we discuss the state-of-the-art approaches for 3D environment mapping.

II-A Trajectory generation

Trajectory generation strategies can be subdivided into three main approaches: search-based path planning followed by smoothing, optimization-based approaches and motion-primitive-based approaches.

In search-based approaches, first, a non-smooth path is constructed on a graph that represents the environment. This graph can be a fully connected grid as in [6] and [11], or be computed using a sampling-based planner (RRT, PRM) as in [21] and [3]. Thereafter, a smooth trajectory represented by a polynomial, B-spline or discrete set of points is computed to closely follow this path. This class of approaches is currently the most popular choice for large-scale path planning problems in cluttered environments where a map is available a priory.

Optimization-based approaches rely on minimizing a cost function that consists of smoothness and collision terms. The trajectory itself can be represented as a set of discrete points [25] or polynomial segments [17]. The approach presented in the present work falls into this category, but represents a trajectory using uniform B-splines.

Another group of approaches is based on path sampling and motion primitives. Sampling-based approaches were successfully used for challenging tasks such as ball juggling [15], and motion primitives were successfully applied to flight through the forest [19], but the ability of both approaches to find a feasible trajectory depends largely on the selected discretization scheme.

II-B Environment representation

To be able to plan a collision-free trajectory a representation of the environment that stores information about occupancy is required. The simplest solution that can be used in the 3D case is a voxel grid. In this representation, a volume is subdivided into regular grid of smaller sub-volumes (voxels), where each voxel stores information about its occupancy. The main drawback of this approach is its large memory-footprint, which allows for maping only small fixed-size volumes. The advantage, however, is very fast constant time access to any element.

To deal with the memory limitation, octree-based representations of the environment are used in [9] [22]. They store information in an efficient way by pruning the leaves of the trees that contain the same information, but the access times for each element become logarithmic in the number of nodes, instead of the constant time as in the voxel-based approaches.

Another popular approach to environment mapping is voxel hashing, which was proposed by Nießner et al. [16] and used in [18]. It is mainly used for storing a truncated signed distance function representation of the environment. In this case, only a narrow band of measurements around the surface is inserted and only the memory required for that sub-volume is allocated. However, when full measurements must be inserted or the dense information must be stored the advantages of this approach compared to those of the other approaches are not significant.

III Trajectory Representation using uniform B-splines

We use uniform B-spline representation for the trajectory function $p(t)$. Because, as shown in [14] and [1], the trajectory must be continuous up to the forth derivative of position (snap), we use quintic B-splines to ensure the required smoothness of the trajectory.

III-A Uniform B-splines

The value of a B-spline of degree $k-1$ can be evaluated using the following equation:

[TABLE]

where $p_{i}\in\mathbb{R}^{n}$ are control points at times $t_{i},i\in[0,..,n]$ and $B_{i,k}(t)$ are basis functions that can be computed using the De Boor – Cox recursive formula [5] [4]. Uniform B-splines have a fixed time interval $\Delta t$ between their control points, which simplifies computation of the basis functions.

In the case of quintic uniform B-splines, at time $t\in[t_{i},t_{i+1})$ the value of $p(t)$ depends only on six control points, namely $[t_{i-2},t_{i-1},t_{i},t_{i+1},t_{i+2},t_{i+3}]$. To simplify calculations we transform time to a uniform representation $s(t)=(t-t_{0})/\Delta t$, such that the control points transform into $s_{i}\in[0,..,n]$. We define function $u(t)=s(t)-s_{i}$ as time elapsed since the start of the segment. Following the matrix representation of the De Boor – Cox formula [20], the value of the function can be evaluated as follows:

[TABLE]

[TABLE]

Given this formula, we can compute derivatives with respect to time (velocity, acceleration) as follows:

[TABLE]

[TABLE]

The computation of other time derivatives and derivatives with respect to control points is also straightforward.

The integral over squared time derivatives can be computed in the closed form. For example, the integral over squared acceleration can be computed as follows:

[TABLE]

where

[TABLE]

Please note that matrix Q is constant in the case of uniform B-splines. Therefore, it can be computed in advance for determining the integral over any squared derivative (see [21] for details).

III-B Comparison with polynomial trajectory representation

In this subsection, we discuss the advantages and disadvantages of B-spline trajectory representation compared to polynomial-splines-based representation [21] [17].

To obtain a trajectory that is continuous up to the forth derivative of position, we need to use B-splines of degree five or greater and polynomial splines of at least degree nine (we need to set five boundary constraints on each endpoint of the segment). Furthermore, for polynomial splines we must explicitly include boundary constraints into optimization, while the use of B-splines guarantees the generation of a smooth trajectory for an arbitrary set of control points. Another useful property of B-splines is the locality of trajectory changes due to changes in the control points, which means that a change in one control point affects only a few segments in the entire trajectory. All these properties result in faster optimization because there are fewer variables to optimize and fewer constraints.

Evaluation of position at a given time, derivatives with respect to time (velocity, acceleration, jerk, snap), and integrals over squared time derivatives are similar for both cases because closed-form solutions are available for both cases.

The drawback of B-splines, however, is the fact that the trajectory does not pass through the control points. This makes it difficult to enforce boundary constraints. The only constraint we can enforce is a static one (all time derivatives are zero), which can be achieved by inserting the same control point $k+1$ times, where $k$ is the degree of the B-spline. If we need to set an endpoint of the trajectory to have a non-zero time derivative, an iterative optimization algorithm must be used.

These properties make polynomial splines more suitable for the cases where the control points come from planning algorithms (RRT, PRM), which means that the trajectory must pass through them, else the path cannot be guaranteed to be collision-free. For local replanning, which must account for unmodeled obstacles, this property is not very important; thus, the use of B-spline trajectory representation is a better option.

IV Local Environment Map using 3D Circular Buffer

To to avoid obstacles during flight, we need to maintain an occupancy model of the environment. On one hand, the model should rely on the most recent sensor measurements, and on the other hand it should store some information over time because the field of view of the sensors mounted on the MAV is usually limited.

We argue that for local trajectory replanning a simple solution with a robocentric 3D circular buffer is beneficial. In what follows, we discuss details pertaining to implementation and advantages from the application viewpoint.

IV-A Addressing

To enable addressing we discretize the volume into voxels of size $r$. This gives us a mapping from point $p$ in 3D space to an integer valued index $x$ that identifies a particular voxel, and the inverse operation that given an index outputs its center point.

A circular buffer consists of a continuous array of size $N$ and an offset index $o$ that defines the location of the coordinate system of the volume. With this information, we can define the functions to check whether a voxel is in the volume and find its address in the stored array:

[TABLE]

If we restrict the size of the array to $N=2^{p}$, we can rewrite these functions to use cheap bitwise operations instead of divisions:

[TABLE]

where $\&$ is a ”bitwise and,” $\sim$ is a ”bitwise negation,” and $!$ is a ”boolean not.”.

To ensure that the volume is centered around the camera, we must simply change the offset $o$ and clear the updated part of the volume. This eliminates the need to copy large amounts of data when the robot moves.

IV-B Measurement insertion

We assume that the measurements are performed using range sensors, such as Lidar, RGB-D cameras, and stereo cameras, and can be inserted into the occupancy buffer by using raycast operations.

We use an additional flag buffer to store a set of voxels affected by insertion. First, we iterate over all points in our measurements, and for the points that lie inside the volume, we mark the corresponding voxels as occupied. For the points that lie outside the volume, we compute the closest point inside the volume and mark the corresponding voxels as free rays. Second, we iterate over all marked voxels and perform raycasting toward the sensor origin. We use a 3D variant of Bresenham’s line algorithm [2] to increase the efficiency of the raycasting operation.

Thereafter, we again iterate over the volume and update the volume elements by using the hit and miss probabilities, similarly to the approach described in [9].

IV-C Distance map computation

To facilitate fast collision checking for the trajectory, we compute the Euclidean distance transform (EDT) of the occupancy volume. This way, a drone approximated by the bounding sphere can be checked for collision by one look-up query. We use an efficient $O(n)$ algorithm written by Felzenszwalb and Huttenlocher [7] to compute EDT of the volume, where $n=N^{3}$ is the number of voxels in the grid (the complexity is cubic in the size of the volume along a single axis). For querying distance and computing gradient, trilinear interpolation is used.

V Trajectory Optimization

The local replanning problem is represented as an optimization of the following cost function:

[TABLE]

where $E_{ep}$ is an endpoint cost function that penalizes position and velocity deviations at the end of the optimized trajectory segment from the desired values that usually come from the global trajectory; $E_{c}$ is a collision cost function; $E_{q}$ is the cost of the integral over the squared derivatives (acceleration, jerk, snap); and $E_{l}$ is a soft limit on the norm of time derivatives (velocity, acceleration, jerk and snap) over the trajectory.

V-A Endpoint cost function

The purpose of the endpoint cost function is to keep the local trajectory close to the global one. This is achieved by penalizing position and velocity deviation at the end of the optimized trajectory segment from the desired values that come from the global trajectory. Because the property is formulated as a soft constraint, the targeted values might not be achieved, for example, because of obstacles blocking the path. The function is defined as follows:

[TABLE]

where $t_{ep}$ is the end time of the segment, $p(t)$ is the trajectory to be optimized, $p_{ep}$ and $p^{\prime}_{ep}$ are the desired position and velocity, respectively, $\lambda_{p}$ and $\lambda_{v}$ are the weighting parameters.

V-B Collision cost function

Collision cost penalizes the trajectory points that are within the threshold distance $\tau$ to the obstacles. The cost function is computed as the following line integral:

[TABLE]

where the cost function for every point $c(x)$ is defined as follows:

[TABLE]

where $\tau$ is the distance threshold, $d(x)$ is the distance to the nearest obstacle, and $\lambda_{c}$ is a weighting parameter. The line integral is computed using the rectangle method, and distances to the obstacles are obtained from the precomputed EDT by using trilinear interpolation.

V-C Quadratic derivative cost function

Quadratic derivative cost is used to penalize the integral over square derivatives of the trajectory (acceleration, jerk, and snap), and is defined as follows:

[TABLE]

The above function has a closed-form solution for trajectory segments represented using B-splines.

V-D Derivative limit cost function

To make sure that the computed trajectory is feasible, we must ensure that velocity, acceleration and higher derivatives of position remain bounded. This requirement can be included into the optimization as a constraint $\forall t:p^{(k)}(t)<p^{k}_{max}$, but in the proposed approach, we formulate it as a soft constraint by using the following function:

[TABLE]

where $l(x)$ is defined as follows:

[TABLE]

This allows us to minimize this cost function by using any algorithm designed for unconstrained optimization.

V-E Implementation details

To run the local replanning algorithm on the drone, we first compute a global trajectory by using the approach described in [21]. This gives us a polynomial spline trajectory that avoids all mapped obstacles. Thereafter, we initialize our replanning algorithm with six fixed control points at the beginning of the global trajectory and $C$ control points that need to be optimized.

In every iteration of the algorithm we set the endpoint constraints (Sec. V-A) to be the position and velocity at $t_{ep}$ of the global trajectory. The collision cost (Sec. V-B) of the trajectory is evaluated using a circular buffer that contains measurements obtained using the RGB-D camera mounted on the drone. The weights of quadratic derivatives cost (Sec. V-C) are set to the same values as those used for global trajectory generation, and the limits (Sec. V-D) are set $20\%$ higher to ensure that the global trajectory is followed with the appropriate velocity while laterally deviating from it.

After optimization, the first control point from the points that were optimized is fixed and sent to the MAV position controller. Another control point is added to the end of the spline, which increases $t_{ep}$ and moves the endpoint further along the global trajectory.

For optimization we use [10], which provides an interface to several optimization algorithms. We have tested the MMA [24] and BFGS [13] algorithms for optimization, with both algorithms yielding similar performance.

VI Results

In this section, we present experimental results obtained using the proposed approach. First, we evaluate the mapping and the trajectory optimization components of the system separately for comparison with other approaches and justify their selection. Second, we evaluate the entire system in a realistic simulator in several different environments, and finally, present an evaluation of the system running on real hardware.

VI-A Three-dimensional circular buffer performance

We compare our implementation of the 3D circular buffer to the popular octree-based solution of [9]. Both approaches use the same resolution of 0.1 m. We insert the depth maps sub-sampled to the resolution of $160\times 120$, which come from a real-world dataset [23]. The results (Fig. 2) show that insertion of the data is more than an order of magnitude faster with the circular buffer, but only a limited space can be mapped with this approach. Because we need the map of a bounded neighborhood around the drone for local replanning, this drawback is not significant for target application.

VI-B Optimization performance

To evaluate the trajectory optimization we use the forest dataset from [17]. Each spline configuration is tested in 9 environments with 10 random start and end positions that are at least 4 m away from each other. Each environment is $10\times 10\times 10$ $m^{3}$ in size and is populated with trees with increasing density. The optimization is initialized with a straight line and after optimization, we check for collisions. For all the approaches, the success fraction, mean normalized path length, and computation time are reported (Table I).

The results of the proposed approach are similar in terms of success fraction to those achieved with polynomial splines from [17] without restarts, but the computation times with the proposed approach are significantly shorter. This is because the unconstrained optimization employed herein directly optimizes the control points, while in [17], a complicated procedure to transform a problem to the unconstrained optimization form [21] must be applied.

Another example of the proposed approach for trajectory optimization is shown in Figure 3, where a global trajectory is generated through a pre-defined set of points with an obstacle placed in the middle. The optimization is performed as described in Section V-E, with the collision threshold $\tau$ set to 0.5 m. As can be seen in the plot, the local trajectory in the collision free regions aligns with the global one, but when an obstacle is encountered, a smooth trajectory is generated to avoid it and ensure that the MAV returns to the global trajectory.

VI-C System simulation

To further evaluate our approach, we perform a realistic simulation experiment by using the Rotors simulator [8]. The main source of observations of the obstacles is a simulated RGB-D camera that produces VGA depth maps at 20 FPS. To control the MAV, we use the controller developed by Lee et al. [12], which is provided with the simulator and modified to receive trajectory messages as control points for uniform B-splines. When there are no new commands with control points, the last available control point is duplicated and inserted into the B-spline. This is useful from the viewpoint of failure case because when an MAV does not receive new control points, it will slowly stop at the last received control point.

We present the qualitative results of the simulations shown in Figures 1 and 6. The drone is initialized in free space and a global path through the world populated with obstacles is computed. In this case, the global path is computed to intersect the obstacles intensionally. The environment around the drone is mapped by inserting RGB-D measurements into the circular buffer, which is then used in the optimization procedure described above.

In all presented simulation experiments, the drone can compute a local trajectory that avoids collisions and keeps it close to the global path. The timings of the various operations involved in trajectory replanning are presented in Table II.

VI-D Real-world experiments

We evaluate our system on a multicopter in several outdoor experiments (Fig. 4). In these experiments, the drone is initialized without prior knowledge of the map and the global path is set as a straight line with its endpoint in front of the drone 1 m above the ground. The drone is required to use onboard sensors to map the environment and follow the global path avoiding trees, which serve as obstacles.

We use the AscTec Neo platform equipped with a stereo camera for estimating drone motion and an RGB-D camera (Intel Realsense R200) for obstacle mapping. All computations are performed on the drone on a 2.1 GHz Intel i7 CPU.

In all presented experiments, the drone can successfully avoid the obstacles and reach the goal position. However, the robustness of the system is limited at the moment owing to the accuracy of available RGB-D cameras that can capture outdoor scenes.

VII Conclusion

In this paper, we presented an approach to real-time local trajectory replanning for MAVs. We assumed that the global trajectory computed by an offline algorithm is provided and formulated an optimization problem that replans the local trajectory to follow the global one while avoiding unmodeled obstacles.

We improved the optimization performance by representing the local trajectory with uniform B-splines, which allowed us to perform unconstrained optimization and reduce the number of optimized parameters.

For collision checking we used the well-known concept of circular buffer to map a fixed area around the MAV, which improved the insertion times by an order of magnitude compared to those achieved with an octree-based solution.

In addition, we presented an evaluation of the complete system and specific sub-systems in realistic simulations and on real hardware.

Acknowledgments

This work has been partially supported by grant CR 250/9-2 (Mapping on Demand) of German Research Foundation (DFG) and grant 608849 (EuRoC) of European Commission FP7 Program.

We also thank the authors of [17] for providing their dataset for evaluation of the presented method.