A Unified NMPC Scheme for MAVs Navigation with 3D Collision Avoidance under Position Uncertainty

This letter proposes a novel Nonlinear Model Predictive Control (NMPC) framework for Micro Aerial Vehicle (MAV) autonomous navigation in indoor enclosed environments. The introduced framework allows us to consider the nonlinear dynamics of MAVs, nonlinear geometric constraints, while guarantees real-time performance. Our first contribution is to reveal underlying planes within a 3D point cloud, obtained from a 3D lidar scanner, by designing an efficient subspace clustering method. The second contribution is to incorporate the extracted information into the nonlinear constraints of NMPC for avoiding collisions. Our third contribution focuses on making the controller robust by considering the uncertainty of localization in NMPC using Shannon's entropy to define the weights involved in the optimization process. This strategy enables us to track position or velocity references or none in the event of losing track of position or velocity estimations. As a result, the collision avoidance constraints are defined in the local coordinates of the MAV and it remains active and guarantees collision avoidance, despite localization uncertainties, e.g., position estimation drifts. The efficacy of the suggested framework has been evaluated using various simulations in the Gazebo environment.