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

Robust Integrated Planning and Control for Quadrotors in Dynamic Environments via NMPC with CBF Penalties
May 31, 2026Zeinab Shayan, Mohammadreza Izadi, Reza FaieghiNonlinear MPC Control Barrier Functions
Autonomous Vehicles Laboratory, Department of Aerospace Engineering, Toronto Metropolitan University, Toronto, Canada.
WAND: Learning Robust Navigation under Complex Wind Disturbances and Dense Obstacles for Quadrotors
Oct 8, 2026Zhonghan Tang, Chenhui Li, Shuai Liang +5Disturbance Rejection Robot Navigation
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
GustPilot: A Hierarchical DRL-INDI Framework for Wind-Resilient Quadrotor Navigation
Mar 20, 2026Amir Atef Habel, Roohan Ahmed Khan, Fawad Mehboob +2UAV Navigation Quadrotor Control
Intelligent Space Robotics Laboratory, Center for Digital Engineering, Skolkovo Institute of Science and Technology, Moscow, Russia.

May 31, 2026J/K move · Enter open · S save

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

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

Open paper AskPDF arXiv

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.

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