Source-linked AI summary
Real-Time Trajectory Replanning for MAVs using Uniform B-splines and a 3D Circular Buffer
Vladyslav Usenko, Lukas von Stumberg, Andrej Pangercic, Daniel Cremers
TL;DR
MAV trajectory planners generally assume static environments and prior maps, motivating local replanning for unmodeled obstacles. The paper combines uniform B-spline optimization with a moving 3D circular buffer, and reports successful obstacle avoidance with shorter computation times than polynomial splines in the cited comparison.
Problem
Existing MAV trajectory-generation methods assume static environments and prior map knowledge, while safe navigation requires replanning around unpredicted obstacles.
Method
The approach replans a local trajectory toward an offline global trajectory using uniform B-splines and a robocentric fixed-area 3D circular buffer for local occupancy mapping.
Results
The proposed approach achieves similar success fractions to polynomial splines without restarts, with significantly shorter computation times.
Takeaways & Limitations
The system successfully avoids obstacles and reaches the goal in all presented simulated and real-world experiments.
Abstract
from arXiv · showhide
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.
I. INTRODUCTION
The paper addresses real-time local replanning for MAVs that must avoid unpredicted obstacles while remaining close to a preplanned global trajectory. It combines B-spline trajectory optimization with a robocentric 3D circular buffer for local mapping.
- MAVs require replanning that generates collision-free trajectories when unpredicted obstacles appear during navigation.
- The lower planning level must avoid unmodeled, possibly dynamic obstacles while keeping the MAV close to the global path.
- The approach represents local trajectories with B-splines and maintains local environmental information in a robocentric fixed-size 3D circular buffer.
- The circular buffer cannot model arbitrarily large occupancy maps, but faster lookup and measurement insertion suit real-time replanning.
- The study evaluates the system in simulated and real-world experiments and provides an open-source implementation.
- The listed contributions include B-spline optimization, high-performance 3D circular-buffer mapping and collision checking, and system evaluation against existing methods.
II. RELATED WORK
Related work spans search-based planning with smoothing, optimization-based trajectory generation, motion primitives, and occupancy representations for 3D mapping. The paper positions its method within optimization-based approaches while using uniform B-splines and a local voxel-style buffer.
- Trajectory-generation methods include search-based planning followed by smoothing, optimization-based approaches, and motion-primitive-based approaches.
- Search-based methods construct a nonsmooth graph path and then compute a smooth polynomial, B-spline, or discrete trajectory to follow it.
- Optimization-based methods minimize smoothness and collision costs; this work belongs to that class but represents trajectories with uniform B-splines.
- Sampling-based and motion-primitive approaches depend substantially on the selected discretization scheme for finding feasible trajectories.
- Voxel grids provide constant-time element access but have a large memory footprint and therefore map only small fixed-size volumes.
- Octrees reduce memory use through pruning but make element access logarithmic in the number of nodes rather than constant time.
- Voxel hashing allocates memory for narrow surface bands, but its advantage decreases when full measurements or dense information must be stored.
- The paper uses quintic B-splines because the trajectory must remain continuous through the fourth derivative of position, or snap.
A. Uniform B-splines
Uniform B-splines represent the trajectory using fixed-spaced control points and locally evaluated basis functions. Their matrix form supports derivative computation and closed-form smoothness costs, while uniformity permits precomputed quantities.
- The B-spline value of degree k −1 is evaluated from basis functions associated with control points.
- Uniform B-splines use a fixed time interval ∆t between control points, simplifying basis-function computation.
- For quintic uniform B-splines, p(t) depends on six neighboring control points within each interval [ti, ti+1).
- The De Boor–Cox matrix representation allows the trajectory function and its time derivatives, including velocity and acceleration, to be computed.
- Integrals over squared time derivatives, such as squared acceleration, can be computed in closed form.
- For uniform B-splines, matrix Q is constant and can be precomputed for determining integrals over squared derivatives.
B. Comparison with polynomial trajectory representation
Uniform B-splines provide smooth, locally adjustable trajectories with fewer optimization constraints than polynomial splines, making them suitable for local replanning. Their main limitation is less direct enforcement of boundary conditions and control-point passage.
- Smoothness and optimization: Degree-five B-splines achieve continuity through the fourth position derivative, whereas polynomial splines require degree nine or greater.Polynomial splines also require five boundary constraints at each segment endpoint.
- Smoothness and optimization: B-splines guarantee smooth trajectories for arbitrary control points and localize trajectory changes to only a few segments.These properties reduce the number of variables and constraints needed during optimization.
- Smoothness and optimization: Position, time derivatives, and integrals over squared derivatives can be evaluated in closed form for both B-splines and polynomial splines.
- Boundary constraints: B-spline trajectories do not pass through their control points, complicating boundary constraints and requiring iterative optimization for non-zero endpoint derivatives.Static zero-derivative endpoints can be enforced by repeating the endpoint control point k +1 times.
- Boundary constraints: Polynomial splines are more suitable when planning control points must lie on the trajectory, but this requirement is less important for local replanning around unmodeled obstacles.
IV. LOCAL ENVIRONMENT MAP USING 3D CIRCULAR BUFFER
The local environment map uses a robocentric, fixed-size 3D circular buffer that stores recent occupancy information around the MAV. Moving the buffer by changing its offset avoids copying the full volume, while addressing uses voxel indices and optional bitwise operations.
- Circular-buffer design: A robocentric 3D circular buffer maintains local occupancy information while accommodating the limited field of view of onboard sensors.The model combines recent measurements with information retained over time.
- Addressing: The volume is discretized into voxels of size r, mapping 3D points to integer voxel indices and indices back to voxel centers.
- Addressing: A continuous array of size N and offset index o define the stored volume and its coordinate-system location.These structures support checking whether a voxel lies inside the volume and finding its stored-array address.
- Addressing: When N = 2^p, voxel-addressing functions can use bitwise operations instead of divisions.The relevant operators are bitwise and, bitwise negation, and boolean not.
- Buffer motion: Changing the offset o and clearing only the updated volume region keeps the buffer centered around the camera without copying large amounts of data.
B. Measurement insertion
Measurement insertion assumes range sensors and updates the occupancy buffer by marking measured points and raycasting toward the sensor. This supports local mapping of previously unmapped obstacles during flight.
- Sensor assumptions: The occupancy buffer accepts measurements from Lidar, RGB-D cameras, and stereo cameras through raycast operations.
- Voxel updates: Measured points inside the volume are marked as occupied, while points outside it identify the closest interior voxel as a free ray.
- Voxel updates: Marked voxels are raycast toward the sensor origin using a 3D variant of Bresenham’s line algorithm.The algorithm is used to increase raycasting efficiency.
- Voxel updates: The volume elements are subsequently updated using hit and miss probabilities.
C. Distance map computation
The system computes an Euclidean distance transform for fast trajectory collision checking and optimizes a cost combining endpoint tracking, collision avoidance, smoothness, and derivative limits. Endpoint tracking remains a soft constraint when obstacles block the desired path.
- Distance map: The Euclidean distance transform lets a bounding-sphere approximation of the drone be collision-checked with one lookup query.Distance queries and gradients use trilinear interpolation.
- Distance map: The EDT is computed with an O(n) algorithm, where n = N^3 is the voxel count and complexity is cubic in the volume size along one axis.
- Optimization objective: The total optimization cost combines endpoint deviation, collision, squared-derivative, and derivative-limit terms.These terms are Eep, Ec, Eq, and El, respectively.
- Endpoint cost: The endpoint term penalizes final position and velocity deviations from desired values, usually taken from the global trajectory.
- Endpoint cost: Because endpoint tracking is a soft constraint, the targeted position or velocity may remain unattained when obstacles block the path.
B. Collision cost function
The collision cost penalizes trajectory points near obstacles using a distance-threshold function integrated along the trajectory. Distances are obtained from a precomputed Euclidean distance transform, while derivative smoothness is also incorporated into the optimization.
- Collision penalty: Trajectory points within the threshold distance τ of obstacles receive a collision penalty, while farther points incur zero cost.The cost is evaluated through a line integral along the trajectory.
- Distance evaluation: The collision cost uses the nearest-obstacle distance d(x), threshold τ, and weighting parameter λc.Distances come from the precomputed EDT with trilinear interpolation, and the line integral uses the rectangle method.
- Derivative smoothness: Quadratic derivative cost penalizes the integral of squared acceleration, jerk, and snap.This smoothness cost has a closed-form solution for B-spline trajectory segments.
D. Derivative limit cost function
The derivative-limit cost keeps velocity, acceleration, and higher derivatives bounded for feasible MAV execution by replacing hard constraints with a soft penalty. The replanner initializes from a global trajectory and directly optimizes B-spline control points.
- Derivative feasibility: Velocity, acceleration, and higher derivatives must remain below their specified maximum limits for trajectory feasibility.The paper includes this requirement in optimization as a derivative constraint.
- Soft limits: The proposed soft constraint penalizes derivative values exceeding their limits, allowing unconstrained optimization algorithms to minimize the resulting cost.This formulation avoids enforcing the derivative bounds as hard constraints.
- Initialization: The replanner starts from a global polynomial-spline trajectory and optimizes six fixed initial control points plus C additional control points.The global trajectory is generated to avoid mapped obstacles before local replanning begins.
- Implementation: Endpoint constraints use the global trajectory’s position and velocity at t_ep, while derivative limits are set 20% higher to support lateral deviation.The collision cost uses measurements stored in the circular buffer.
- Comparison: B-spline optimization performs similarly to polynomial splines without restarts, while computation times are lower through direct control-point optimization.The comparison attributes the timing advantage to avoiding transformation into an unconstrained optimization form.
- Optimization: MMA and BFGS produced similar performance when used through the selected optimization interface.Both algorithms were tested for the replanning optimization.
VI. RESULTS
The evaluation separately measures mapping and trajectory optimization, then tests the complete system in simulation and on real hardware. The circular buffer is faster for insertion, while B-spline optimization achieves similar success with shorter computation times than polynomial alternatives.
- Evaluation design: The experiments evaluate mapping, trajectory optimization, the complete system in multiple simulated environments, and real-hardware operation.The study separates component comparisons from end-to-end evaluation.
- Mapping performance: Circular-buffer insertion is more than an order of magnitude faster than the octree implementation at 0.1 m resolution.Both methods process depth maps sub-sampled to 160 × 120 from a real-world dataset.
- Mapping scope: The circular buffer maps only a limited space, but this limitation is considered insignificant because replanning needs a bounded neighborhood around the drone.The comparison uses the same 0.1 m resolution for both mapping approaches.
- Optimization performance: Across nine forest environments with ten random start-end pairs, the proposed optimizer achieves success fractions similar to polynomial splines without restarts.Each environment measures success fraction, normalized path length, and computation time after straight-line initialization.
- Optimization performance: The proposed approach has significantly shorter computation times than polynomial splines without restarts.The paper attributes this to direct optimization of control points using unconstrained optimization.
- Trajectory behavior: When an obstacle interrupts the global path, the local trajectory smoothly avoids it and returns to the global trajectory afterward.In collision-free regions, the local trajectory aligns with the global trajectory.
C. System simulation
The system was evaluated in realistic simulation and on real hardware, using onboard sensing and the proposed B-spline and circular-buffer components for local replanning. Across the presented experiments, the drone avoided obstacles while remaining near the global path and reaching its goal, although outdoor robustness was limited by RGB-D camera accuracy.
- Simulation: The realistic simulation used the Rotors simulator, a VGA RGB-D camera at 20 FPS, and a controller modified to receive uniform B-spline control points.The controller continued using the last available control points when no new commands were received.
- Simulation: RGB-D measurements were inserted into the circular buffer and used by the optimization procedure to replan around obstacles intentionally intersecting the global path.The drone began in free space while the global path was computed through an obstacle-populated world.
- Simulation: In all simulation experiments, the drone computed local trajectories that avoided collisions and stayed close to the global path.The study also reported timings for the operations involved in trajectory replanning.
- Real-world experiments: Outdoor experiments used an AscTec Neo with stereo motion estimation and RGB-D obstacle mapping on a 2.1 GHz Intel i7 onboard computer.The drone mapped trees using onboard sensors while following a straight global path toward a goal 1 m above the ground.
- Results: Across the presented experiments, the drone avoided obstacles and reached the goal, but outdoor robustness was limited by the accuracy of available RGB-D cameras.The complete system and specific subsystems were evaluated in realistic simulations and on real hardware.