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

Peng Liu; Jingyan Wang; Qipeng Ye; Wen Li; Jinya Su; Zuo Wang; Shihua Li; Yunda Yan

cs.ROOct 2, 2026

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

Authors: Peng Liu, Jingyan Wang, Qipeng Ye, Wen Li, Jinya Su, Zuo Wang, Shihua Li, Yunda Yan

Organizations: School of Automation, Key Laboratory of Measurement and Control of CSE, Ministry of Education, Institute of Intelligent Unmanned Systems, Southeast University, Nanjing 210096, China. · School of Electrical Engineering and Automation, Xiamen University of Technology, Xiamen 361024, China. · Department of Computer Science, University College London, London, WC1E 6BT, UK.

Abstract

LiDAR-based quadrotor navigation in cluttered environments remains challenging under external disturbances, particularly when obstacle-aware motion generation and disturbance-rejection control are handled in separate layers. This article presents disturbance-resilient integrated planning and control (DR-IPC), which combines lightweight path guidance with nonlinear model predictive control (NMPC) to directly generate angular velocity and thrust. An interconnected extended Kalman filter and nonlinear disturbance observer jointly provide filtered state estimates and reconstructed disturbances for NMPC prediction. The resulting formulation unifies nonlinear quadrotor dynamics, actuator constraints, local motion generation, and penalised safe-flight-corridor residuals without requiring a separate trajectory-optimization stage. Gazebo and MARSIM simulations, together with indoor and outdoor experiments, validate DR-IPC under wind, suspended payloads, narrow passages, ball impacts and reactive avoidance of a dynamic obstacle. In multi-goal navigation with disturbances, DR-IPC increases the number of completed missions from 1/10 to 9/10 in Gazebo and reduces the altitude RMSE from 0.34 to 0.01 m in experiments. The complete system operates onboard at 100 Hz. Supplementary videos are available on the project page https://drpp316.github.io/DR-IPC-Page/, and the source code will be released.

Figures & tables

Fig. 1: Real-world validation of the proposed disturbance-resilient navigation in cluttered environments. (a)–(d) flight routes and snapshots in the outdoor wooded scene during daytime and nighttime; (e)–(h) flight routes and snapshots in two real-life factory scenarios.

Fig. 2: Pipeline of the proposed DR-IPC. The frontend provides collision-free geometric guidance, the EKF+NDO supplies filtered states and reconstructed disturbances, and the NMPC backend jointly optimizes local motion and angular-velocity/thrust commands with SFC penalties and input constraints. The main contributions are highlighted in red.

Fig. 3: Planning and control under disturbances. (a) in the HPC, the planner uses obstacle information, whereas the controller does not; disturbance-induced deviations therefore may result in collision; (b) IPC uses obstacle information to jointly adjust local motion and control actions (the dashed and solid curves represent two feasible collision-avoidance routes).

Fig. 4: Ball-impact hover experiment. (a) experimental setup; (b)–(i) IPC and DR-IPC transient responses at $t=3$ , 4, 5, 8 s, respectively.

Method	Function	Architecture	Obstacle-aware planning	Estimated disturbance in prediction	Nonlinear MPC model	Cluttered real-world flight	Onboard perception
Minimum-snap [ 25 ]	Planning and control	Hierarchical	$\times$	$\times$	–	$\checkmark$	$\times$
EGO-Planner [ 41 ]	Planning	Hierarchical	$\checkmark$	$\times$	–	$\checkmark$	Camera
Obstacle-aware NMPC [ 17 ]	Planning and control	Integrated	$\checkmark$	$\times$	$\checkmark$	$\times$	$\times$
CBF-MPC [ 1 ]	Planning and control	Integrated	$\checkmark$	$\times$	$\times$	$\times$	$\times$
MPCC [ 29 ]	Planning and control	Integrated	$\times$	$\times$	$\checkmark$	$\times$	$\times$
IPC [ 21 ]	Planning and control	Integrated	$\checkmark$	$\times$	$\times$	$\checkmark$	LiDAR

TABLE I: Capability comparison of representative quadrotor methods. (

\checkmark

: yes;

\times

: no; –: not applicable or not reported. )

Fig. 5: Indoor multi-waypoint navigation with disturbances. (a)–(e) DR-IPC transient responses at $t=4$ , 11, 15, 24, 28 s, respectively.

Fig. 6: Quadrotor platform and coordinate frames.

Fig. 7: Narrow-passage navigation under constant wind. (a) IPC trajectory; (b) DR-IPC trajectory; (c) altitude responses with

p_{z}^{r}=1

m; (d) DR-IPC disturbance estimates.

Module	Implementation
Operating system	Ubuntu 20.04 and ROS1 Noetic
Localization	FAST-LIO2 [ 37 ]
Point-cloud input	Registered LiDAR cloud from FAST-LIO2
Local mapping	ROG-Map-style occupancy grid [ 28 ]
Path search	A* graph search [ 9 ]
SFC construction	Polyhedral SFC [ 19 ]

TABLE II: Software implementation of DR-IPC.

Metric	NDO	EKF+NDO	Reduction
Disturbance-vector RMSE (m/s 2 ) $\downarrow$	0.260	0.036	86.1%
Disturbance-rate RMS (m/s 3 ) $\downarrow$	11.6	6.1	47.3%
Angular velocity RMS (rad/s) $\downarrow$	0.81	0.45	44.4%
$w_{y}$ saturation ratio (%) $\downarrow$	36.0%	2.1%	33.9 pp
Attitude RMS ( ∘ ) $\downarrow$	11.3	8.6	23.7%

TABLE III: Interconnected-observer ablation under wind and a suspended payload. (

\downarrow

: lower is better; pp: percentage points.)

Fig. 8: Interconnected observer ablation. (a) top-view trajectories and local wind region; (b)–(d) disturbance estimates

\hat{d}_{x},\hat{d}_{y},\hat{d}_{z}

and ground truths; (e) angular velocity command

w_{y}

and its limits; (f) attitude excitation.

Case	$N$	$\Delta t$ (ms)	$\mathbf{w}_{\max}$ (rad/s)	$T_{\max}$ (N)	$\bm{\Lambda}$	$\mathbf{Q}$	$\mathbf{R}$
Simulation	$20$	$50$	$(1.0,1.0,1.5)$	$20.2$	$2\mathbf{I}_{3}$	$2\mathbf{I}_{10}$	$\mathbf{I}_{10}$
Experiment	$20$	$50$	$(1.0,1.0,1.5)$	$25.7$	$\mathbf{I}_{3}$	$\mathbf{I}_{10}$	$5\mathbf{I}_{10}$

TABLE IV: DR-IPC parameters used in simulations and experiments.

Fig. 9: Multi-goal Gazebo environment with disturbances.

Method	Disturbance	Completed missions $\uparrow$	Reached goals $\uparrow$	Average events $\downarrow$
IPC	$\times$	10/10	40/40	1
IPC	$\checkmark$	1/10	9/40	5
DR-IPC	$\checkmark$	9/10	37/40	0.1

TABLE V: Multi-goal navigation over ten Gazebo trials. (events: collisions or falls; average events: events in completed missions divided by the completed missions;

\uparrow/\downarrow

: higher/lower is better; bold: best.)

Fig. 10: Navigation in the MARSIM forest environment. (a) overall navigation; (b) local view I; (c) local view II; (d) multi-waypoint autonomous navigation.

Component	Model	Mass (kg)
Onboard computer	ASUS NUC 13 (i5-1340P)	$\left\}\vbox{\vrule height=0.0pt,width=0.0pt}\textnormal{\hskip 1.84995pt1.705}\right.$
LiDAR	Livox Mid-360S
Flight controller	CUAV V5 Nano
Battery	ACE Tattu 4S-2300 mAh
Frame	Totem Q250
Radio receiver	RadioLink R12DSE

TABLE VI: Hardware used in the experiments.

Fig. 11: Position responses and disturbance estimates. (a) position responses; (b) DR-IPC disturbance estimates.

Fig. 12: Navigation through an archway. (a)–(c) top-view trajectories of IPC without disturbance, IPC with disturbance, and DR-IPC with disturbance, respectively, where red segments indicate penetration into the inflated obstacle regions; (d) altitude responses; (e) SFC-violation depth.

Method	Disturbance	Passage	$p_{z}^{\min}$ (m)	$\operatorname{RMSE}(p_{z})$ (m)	Out-of-SFC time $T_{\mathrm{out}}$ (s)
IPC	$\times$	$\checkmark$	0.25	0.17	3.13
IPC	$\checkmark$	$\times$	0.27	0.34	18.77
DR-IPC	$\checkmark$	$\checkmark$	0.97	0.01	2.98

TABLE VII: Archway-navigation results. (bold: best;

p_{z}^{r}=1

m.)

Fig. 13: LiDAR point-cloud maps and payload-delivery routes in the factory. (a) factory route 1; (b) factory route 2. Red curves denote the executed trajectories, green circles and orange stars mark the loading and delivery points, respectively, and inset images show representative obstacles.

Fig. 14: Indoor multi-waypoint navigation with disturbances and a dynamic obstacle. (a) IPC; (b) DR-IPC.

Fig. 15: DR-IPC under a dynamic obstacle. (a) trajectory and replanning; (b) nearest obstacle distance; (c) angular velocity; (d) thrust.

Fig. 16: Computation time. (a) per-cycle computation time; (b) breakdown of deadline-miss events.

Disturbance Rejection

See all 13 papers

May 31, 2026

Robust Integrated Planning and Control for Quadrotors in Dynamic Environments via NMPC with CBF Penalties

This paper presents a new robust integrated planning and control (IPC) strategy for multirotor uncrewed aerial vehicles. We propose a nonlinear model predictive control (NMPC) formulation that embeds control barrier functions (CBFs) as exponential penalties, improving feasibility while ensuring smooth obstacle avoidance under tight input bounds. The penalty weights provide a practical tuning knob to trade off tracking accuracy against avoidance aggressiveness. We enhance the system robustness by employing a high-gain disturbance observer (HGDO) to estimate and compensate for external disturbances. We also incorporate a Kalman filter (KF) for computationally efficient, real-time prediction of obstacle motion, enabling avoidance of moving obstacles. Comparative studies against both conventional NMPC and NMPC with hard CBF constraints, validated in Gazebo and hardware experiments, demonstrate superior feasibility, safety, and robustness. To the best of our knowledge, this is the first hardware-validated NMPC-CBF IPC framework, offering a practical step toward safe quadrotor deployment in dynamic environments.

Zeinab Shayan, Mohammadreza Izadi, Reza Faieghi

Autonomous Vehicles Laboratory, Department of Aerospace Engineering, Toronto Metropolitan University, Toronto, Canada.

Nonlinear MPC Control Barrier Functions Robust Control Quadrotor Control Dynamic Obstacle Avoidance

Ask PDF

Oct 8, 2026

WAND: Learning Robust Navigation under Complex Wind Disturbances and Dense Obstacles for Quadrotors

Robust navigation in cluttered environments remains a fundamental challenge for quadrotors, particularly when strong wind disturbances arise, which perturb vehicle dynamics, limit control authority, and substantially increase collision risk. Existing learning-based navigation policies typically rely on obstacle perception and proprioceptive observations, requiring the policy to infer time-varying disturbance effects implicitly and thereby limiting robustness under partial observability. This paper proposes WAND (Wind-Aware Navigation with Disturbance Estimation), a reinforcement learning framework for navigation under time-varying wind disturbances in dense obstacle fields. Specifically, WAND estimates wind-induced disturbance acceleration from historical proprioceptive states using a Temporal Convolutional Network (TCN). This estimation is integrated into the policy via a zero-initialized residual module, WindAdapter, while simultaneously providing feedforward compensation for low-level control. The dual use of the estimate couples disturbance-conditioned navigation with feedforward disturbance rejection. Across 12 wind-disturbed simulation settings, WAND improved the observed success rate by 8.3 percentage points on average relative to feedforward compensation alone. Controlled opposite-crosswind experiments further showed wind-direction-dependent trajectory adaptation. In indoor fan-induced flight tests, WAND succeeded in 18 of 20 trials, demonstrating the feasibility of real-time onboard navigation.

Zhonghan Tang, Chenhui Li, Shuai Liang +5

Department of Electronic Engineering and Information Science, University of Science and Technology of China, Hefei 230027, China · Shanghai Artificial Intelligence Laboratory, Shanghai 200232, China · Northwestern Polytechnical University, Xi’an 710072, China +1

Disturbance Rejection Robot Navigation Quadrotor Control Robust Control Robust RL

Ask PDF

Mar 20, 2026

GustPilot: A Hierarchical DRL-INDI Framework for Wind-Resilient Quadrotor Navigation

Wind disturbances remain a key barrier to reliable autonomous navigation for lightweight quadrotors, where the rapidly varying airflow can destabilize both planning and tracking. This paper introduces GustPilot, a hierarchical wind-resilient navigation stack in which a deep reinforcement learning (DRL) policy generates inertial-frame velocity reference for gate traversal. At the same time, a geometric Incremental Nonlinear Dynamic Inversion (INDI) controller provides low-level tracking with fast residual disturbance rejection. The INDI layer achieves this by providing incremental feedback on both specific linear acceleration and angular acceleration rate, using onboard sensor measurements to reject wind disturbances rapidly. Robustness is obtained through a two-level strategy, wind-aware planning learned via fan-jet domain randomization during training, and rapid execution-time disturbance rejection by the INDI tracking controller. We evaluate GustPilot in real flights on a 50g quad-copter platform against a DRL-PID baseline across four scenarios ranging from no-wind to fully dynamic conditions with a moving gate and a moving disturbance source. Despite being trained only in a minimal single-gate and single-fan setup, the policy generalizes to significantly more complex environments (up to six gates and four fans) without retraining. Across 80 experiments, DRL-INDI achieves a 94.7% versus 55.0% for DRL-PID as average Overall Success Rate (OSR), reduces tracking RMSE up to 50%, and sustains speeds up to 1.34 m/s under wind disturbances up to 3.5 m/s. These results demonstrate that combining DRL-based velocity planning with structured INDI disturbance rejection provides a practical and generalizable approach to wind-resilient autonomous flight navigation.

Amir Atef Habel, Roohan Ahmed Khan, Fawad Mehboob +2

Intelligent Space Robotics Laboratory, Center for Digital Engineering, Skolkovo Institute of Science and Technology, Moscow, Russia.

UAV Navigation Quadrotor Control Robust RL Feedback Control

Ask PDF

Abstract

Disturbance Rejection

Explore similar work

Robust Integrated Planning and Control for Quadrotors in Dynamic Environments via NMPC with CBF Penalties

WAND: Learning Robust Navigation under Complex Wind Disturbances and Dense Obstacles for Quadrotors

GustPilot: A Hierarchical DRL-INDI Framework for Wind-Resilient Quadrotor Navigation