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.
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.
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