Search-based 3D Planning and Trajectory Optimization for Safe Micro Aerial Vehicle Flight Under Sensor Visibility Constraints

Matthias Nieuwenhuisen, Sven Behnke

Abstract

Safe navigation of Micro Aerial Vehicles (MAVs) requires not only obstacle-free flight paths according to a static environment map, but also the perception of and reaction to previously unknown and dynamic objects. This implies that the onboard sensors cover the current flight direction. Due to the limited payload of MAVs, full sensor coverage of the environment has to be traded off with flight time. Thus, often only a part of the environment is covered. We present a combined allocentric complete planning and trajectory optimization approach taking these sensor visibility constraints into account. The optimized trajectories yield flight paths within the apex angle of a Velodyne Puck Lite 3D laser scanner enabling low-level collision avoidance to perceive obstacles in the flight direction. Furthermore, the optimized trajectories take the flight dynamics into account and contain the velocities and accelerations along the path. We evaluate our approach with a DJI Matrice 600 MAV and in simulation employing hardware-in-the-loop.

Introduction

Most autonomous drone navigation systems assume the vehicle has perfect, omnidirectional sensing or relies entirely on a static, pre-mapped environment. However, real-world MAVs often carry lightweight sensors with limited vertical fields of view (FoV). For instance, a Velodyne Puck Lite has only a 30° vertical FoV. If a drone descends rapidly to a target, a standard planner might suggest a straight vertical drop. In this scenario, the drone moves in a direction its sensors cannot see, creating a significant safety risk if a new obstacle appears. This research addresses the challenge of planning paths that ensure the drone’s flight direction always stays within its sensor's "viewing cone."

Methods

The proposed system utilizes a two-tier approach to ensure continuous sensor coverage:

1. Visibility-Constrained Global Planning

The system performs an A* search on a 3D grid, pruning any edges that require an ascent or descent angle steeper than the sensor's FoV allows. Because a standard Euclidean distance heuristic fails to account for the horizontal "detours" required to maintain a shallow angle, the authors developed a custom admissible heuristic. This heuristic estimates the additional horizontal distance ($d_{xy}$) needed to reach a target altitude ($d_z$) safely, significantly accelerating the search process.

Custom visibility-aware heuristic for A* search, accounting for the extra horizontal distance required to maintain sensor coverage during altitude changes.

2. Trajectory Optimization

The raw path from A* is smoothed using a modified version of Covariant Hamiltonian Optimization for Motion Planning (CHOMP). A specific visibility cost is added to the optimization function. If a trajectory segment becomes too steep during smoothing, gradients push the points to "lunge out" horizontally, flattening the angle and preserving sensor coverage. The final output is a time-discretized trajectory including position, velocity, and acceleration, suitable for a low-level Model Predictive Controller (MPC).

Key Insights and Results

One of the most notable behaviors produced by this system is the "lunge" maneuver. To descend safely to a point directly below it, the drone does not drop straight down; instead, the optimizer produces a spiraling or lunging descent. This ensures that the ground and potential obstacles remain within the 30° sensor cone at all times.

The "lunge" maneuver: The optimized trajectory (yellow) avoids a direct vertical drop to ensure the flight direction remains within the sensor's field of view.

In experimental evaluations, the system demonstrated significant improvements in safety and efficiency:

  • Safety: Without visibility constraints, drones violated sensor coverage for roughly 75% of flight time in outdoor tests. With constraints enabled, 100% coverage was maintained.
  • Heuristic Efficiency: The custom visibility-aware heuristic reduced the number of nodes A* had to explore by 70% compared to a standard Euclidean heuristic during a 7m vertical ascent.
  • Accuracy: In simulation, the Absolute Trajectory Error (ATE) remained low (0.22m to 0.67m), proving the optimized trajectories are highly trackable.

Conclusion

This two-stage navigation system effectively integrates sensor FoV constraints into both global planning and local optimization. While planning in high-resolution 3D grids remains computationally intensive, the use of visibility-aware heuristics and reactive re-optimization at 10 Hz allows MAVs to safely navigate complex environments. Future work will explore multiresolution techniques and explicit prediction of dynamic obstacle paths for enhanced sense-and-avoid capabilities.

Summary updated

Abstract

Safe navigation of Micro Aerial Vehicles (MAVs) requires not only obstacle-free flight paths according to a static environment map, but also the perception of and reaction to previously unknown and dynamic objects. This implies that the onboard sensors cover the current flight direction. Due to the limited payload of MAVs, full sensor coverage of the environment has to be traded off with flight time. Thus, often only a part of the environment is covered. We present a combined allocentric complete planning and trajectory optimization approach taking these sensor visibility constraints into account. The optimized trajectories yield flight paths within the apex angle of a Velodyne Puck Lite 3D laser scanner enabling low-level collision avoidance to perceive obstacles in the flight direction. Furthermore, the optimized trajectories take the flight dynamics into account and contain the velocities and accelerations along the path. We evaluate our approach with a DJI Matrice 600 MAV and in simulation employing hardware-in-the-loop.
BibTeX
@inproceedings{nieuwenhuisen2019search,
  author = {Matthias Nieuwenhuisen and Sven Behnke},
  title = {{Search-based 3D Planning and Trajectory Optimization for Safe Micro Aerial Vehicle Flight Under Sensor Visibility Constraints}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2019},
  url = {https://openreview.net/forum?id=KBGYJRPt2n},
}
Figure