DR-IPC: Disturbance-Resilient Integrated Planning and Control for LiDAR-Based Quadrotor Navigation

Can a LiDAR-based quadrotor reliably navigate cluttered environments under wind, payload, and impact disturbances by jointly optimizing obstacle-aware motion and control with online disturbance estimates?

LiDAR maps encode only geometry, so wind, suspended payloads, model errors, and odometry noise can push the vehicle off a collision-free path. Hierarchical planning-and-control stacks separate obstacle avoidance from disturbance rejection: the planner sees obstacles but not disturbances, while the downstream controller corrects path errors without obstacle information, so disturbance-induced deviations can become collisions.

Prior systems use either hierarchical planning and control, such as SFC polynomial methods, minimum-snap generation, and EGO-Planner with a separate tracker, or integrated planning and control such as obstacle-aware NMPC, CBF-MPC, MPCC, and the prior IPC. Hierarchical methods do not give the controller obstacle awareness. Existing integrated methods use nominal predictions, often hard SFC constraints, and only passive or weak disturbance compensation, rather than injecting reconstructed disturbances into obstacle-aware prediction.

DR-IPC combines an interconnected EKF and nonlinear disturbance observer to filter FAST-LIO2 odometry and reconstruct lumped acceleration disturbances. The disturbance estimate is held constant over the horizon inside a nonlinear MPC that optimizes angular velocity and thrust directly under quaternion dynamics, actuator limits, A* path references, and a soft penalized safe-flight-corridor residual, with no separate trajectory-optimization stage. The full stack uses LiDAR mapping, A* replanning, polyhedral SFC construction, and ACADO/qpOASES onboard at 100 Hz.

In Gazebo multi-goal navigation with wind and payload, IPC completed 1/10 missions and 9/40 goals, while DR-IPC completed 9/10 missions and 37/40 goals, with average collision/fall events falling from 5 to 0.1 per completed mission. In narrow-passage simulation, DR-IPC reduced altitude RMSE from 0.484 m to 0.075 m, with IPC dropping to 0.042 m while DR-IPC stayed above 0.850 m. In indoor archway experiments with wind and payload, DR-IPC completed the passage with minimum altitude 0.97 m, altitude RMSE 0.01 m, and out-of-SFC time 2.98 s; IPC failed with minimum altitude 0.27 m, RMSE 0.34 m, and out-of-SFC time 18.77 s. In a ball-impact hover test, IPC fell to 0.08 m, while DR-IPC kept minimum altitude at 0.70 m and returned to a ±0.05 m altitude-error band within 2.30 s. Observer ablation reduced disturbance-vector RMSE from 0.260 to 0.036 m/s² (86.1%), disturbance-rate RMS by 47.3%, angular velocity RMS from 0.81 to 0.45 rad/s, w_y saturation ratio from 36.0% to 2.1%, and attitude RMS from 11.3° to 8.6°. In a dynamic-obstacle indoor test, 4046 of 4201 control cycles (96.31%) met the 10 ms deadline, with 95th/99th/maximum computation times of 9.13/13.71/41.08 ms. Outdoor daytime/nighttime wooded flights and two factory payload-delivery routes were completed without collision, and MARSIM unstructured forest navigation validated cross-simulator integration.

The disturbance model assumes slow variation and uses zero-order hold over the prediction horizon, so fast or abrupt disturbances may require a disturbance model. Soft SFC penalties allow brief entry into inflated safety margins, which worked in tests but may need stronger safety guarantees for certification. Evaluation uses one custom 250 mm quadrotor with limited indoor and outdoor trials; outdoor results are reported qualitatively without baseline metrics, and the source code is not yet released. The nonlinear MPC and observer stack adds computational demand and occasional deadline misses, about 3.7% of cycles in the dynamic-obstacle test.