PaperlayerView exact revision on arXiv

Source-linked AI summary

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

arXiv:2610.03530v1cs.ROcs.SYeess.SY

TL;DR

LiDAR-based quadrotor navigation must contend with disturbances and noisy odometry, while obstacle-aware motion generation and disturbance-rejection control are often handled separately. DR-IPC combines disturbance reconstruction with obstacle-aware predictive control; simulations and experiments validate it across varied scenarios, and observer-ablation results show lower disturbance-estimation error and command fluctuation.

  • Problem

    LiDAR maps omit wind, payload-induced forces, and modeling errors, while odometry noise degrades feedback; existing LiDAR-based IPC methods provide only partial integration and limited passive robustness to disturbances and noisy measurements.

    Source: §I · p. 1

  • Method

    DR-IPC combines a unified NMPC backend for motion and control with an interconnected EKF+NDO that filters states and reconstructs disturbances for prediction.

    Source: §I · pp. 1–2

  • Results

    Validated in Gazebo, MARSIM, and varied indoor and outdoor scenarios; in observer ablation, disturbance-vector RMSE decreased from 0.260 to 0.036 m/s2.

    Source: §I · p. 2, B. Interconnected-Observer Ablation · p. 6

  • Takeaways & Limitations

    The paper reports improved navigation reliability and disturbance rejection over IPC in its comparative simulations and experiments.

    Source: §VII · p. 10

Abstract

from arXiv · show

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.

I. INTRODUCTION

DR-IPC addresses the difficulty of coordinating obstacle avoidance and control when disturbances and noisy odometry push a LiDAR-based quadrotor away from its nominal path. It combines disturbance reconstruction with obstacle-aware predictive control, and is validated across simulated and real-world scenarios.

I. INTRODUCTION

  • LiDAR maps provide geometric information but omit wind, payload forces, and modeling errors, while odometry noise degrades feedback state measurements.These disturbances and uncertainties can drive the vehicle away from a nominally collision-free path.Source: §I · p. 1
  • Existing LiDAR-based integrated planning and control methods offer only partial integration and limited, passive robustness to disturbances and noisy state measurements.The paper frames DR-IPC as an extension of classical IPC.Source: §I · p. 1
  • DR-IPC uses reconstructed disturbances in NMPC prediction to jointly modify collision-avoidance motion and control inputs, rather than only compensating through feedback.Its contributions include a unified NMPC backend and a nonlinear interconnected EKF+NDO.Source: §I · pp. 1–2
  • The interconnected EKF+NDO filters noisy states and reconstructs disturbances, providing noise-attenuated state estimates and smoother disturbance estimates for NMPC.The evaluations span Gazebo and MARSIM simulations and indoor and outdoor experiments involving wind, payloads, narrow passages, impacts, and dynamic-obstacle avoidance.Source: §I · p. 2

1) Hierarchical Planning and Control:

  • In hierarchical planning and control, a planner handles obstacle avoidance while a separate tracking controller lacks obstacle information and may steer toward nearby obstacles after deviations.This can degrade performance in cluttered environments under disturbances.Source: 1) Hierarchical Planning and Control · p. 2

2) Integrated Planning and Control:

Integrated planning and control jointly adjusts local motion and control in response to obstacle information, but existing approaches leave disturbance reconstruction and obstacle-aware motion planning incompletely coupled. DR-IPC combines them in an obstacle-aware NMPC framework with an interconnected observer.

2) Integrated Planning and Control:

  • Integrated schemes use global guidance and obstacle information to jointly optimize collision-aware local motion and control, unlike tracking a prescribed trajectory alone.When wind moves the vehicle from its reference, the scheme can adjust both its route around an obstacle and its control action.Source: 2) Integrated Planning and Control · p. 2

2) Disturbance-aware MPC:

  • Earlier IPC combines A* with SFC-constrained linear MPC, but relies on nominal prediction and limited disturbance compensation; hard SFC constraints can also make optimization infeasible.Obstacle-aware NMPC and CBF-based MPC use different safety mechanisms, while MPCC omits obstacle avoidance.Source: 2) Integrated Planning and Control · pp. 2–3

1) Robust MPC:

  • Robust MPC handles bounded disturbances without reconstructing them, while disturbance-aware MPC uses disturbance estimates but does not couple rejection with obstacle-aware local motion.The methods described include target-visibility constraint tightening and observer-based predictive control.Source: 1) Robust MPC · p. 3, 2) Disturbance-aware MPC · p. 3

C. Positioning of DR-IPC

  • DR-IPC combines an interconnected EKF+NDO with obstacle-aware NMPC to optimize local motion, angular velocity, and thrust without a separate trajectory-optimization stage.The observer supplies filtered states and reconstructed disturbances for prediction.Source: C. Positioning of DR-IPC · p. 3

III. MODELING AND PROBLEM FORMULATION

The model represents quadrotor position, attitude, and velocity as the state, with body-frame angular velocity and thrust as control inputs. It incorporates lumped acceleration disturbances and specifies discrete-time dynamics and measurement noise.

A. Dynamic Modeling

  • Position, velocity, and gravity are expressed in the inertial frame, while angular velocity is expressed in the body-fixed frame.The body-fixed frame is attached to the vehicle’s center of mass; attitude is represented by a unit quaternion.Source: A. Dynamic Modeling · p. 4
  • The paper defines the UAV state as x = [p^T, q^T, v^T]^T in R^10 and control input as u = [w^T, T]^T in R^4, with mass denoted m.Source: A. Dynamic Modeling · p. 4
  • The disturbance d is a lumped acceleration disturbance in the inertial frame that accounts for external forces and unmodeled dynamics.The model gives the nonlinear dynamics f and disturbance input matrix B.Source: A. Dynamic Modeling · p. 4
  • The model specifies a thrust-direction map and a quaternion-kinematics matrix for the quadrotor dynamics.The supplied passage identifies these maps without describing their entries in prose.Source: A. Dynamic Modeling · p. 4
  • The discrete-time dynamics use a sampling period Δt and include process noise η_k distributed as N(0, Q).The state update is expressed as F(x_k, u_k, d_k) = x_k + Δt[f(x_k, u_k) + Bd_k].Source: A. Dynamic Modeling · p. 4
  • LiDAR-inertial odometry provides the measurement, with H = I_10 and measurement noise ν_k distributed as N(0, R).The measurement-noise covariance is R.Source: A. Dynamic Modeling · p. 4

B. A* Guidance and Safe-Flight Corridor

A* supplies geometric guidance from the local occupancy map, while an inflated-map safe-flight corridor defines a local collision-free region. DR-IPC uses filtered state and reconstructed disturbance estimates to optimize feasible motion and control with soft corridor penalties.

B. A* Guidance and Safe-Flight Corridor

  • A* generates a collision-free geometric path from the local occupancy map and replans when a newly detected obstacle blocks it.The reference is geometric and need not satisfy quadrotor dynamics.Source: B. A* Guidance and Safe-Flight Corridor · p. 4
  • The safe-flight corridor is a local collision-free convex region constructed from an occupancy map after obstacle inflation.Its region is represented using A_k and b_k, with n_s denoting the constraint number.Source: B. A* Guidance and Safe-Flight Corridor · p. 4

C. Problem Statement

  • Noisy odometry, A*-based geometric guidance, and the local corridor are available at each update, but the true state and lumped disturbance are not.Online state filtering and disturbance reconstruction are therefore required for predictive control.Source: C. Problem Statement · p. 4
  • A hard corridor constraint may make the optimization infeasible or cause aggressive, oscillatory recovery after disturbances or map updates drive motion outside the corridor.The formulation therefore uses soft corridor penalties while jointly optimizing dynamically feasible local motion, angular velocity, and thrust under nonlinear dynamics and input constraints.Source: C. Problem Statement · p. 4

IV. DR-IPC ALGORITHM

The interconnected observer extends a linear KF+DOB architecture to the nonlinear quaternion model, coupling state filtering with disturbance reconstruction. Its disturbance estimate corrects EKF prediction, while filtered state estimates drive the NDO update; both estimates are supplied to NMPC.

A. Nonlinear Interconnected Observer

  • The EKF uses reconstructed disturbances in its state prediction, and the reduced-order NDO uses posterior attitude and velocity from the EKF.The resulting state estimate and disturbance estimate are supplied to the NMPC.Source: A. Nonlinear Interconnected Observer · p. 5
  • The modular interconnection allows either estimator to be upgraded or disabled independently.The EKF recursion includes an intermediate estimate before quaternion normalization.Source: A. Nonlinear Interconnected Observer · p. 5
  • The lumped acceleration disturbance is assumed constant or slowly varying.The cited passage directs readers to and its references for generic disturbances.Source: A. Nonlinear Interconnected Observer · p. 5

B. Disturbance- and Obstacle-Aware NMPC

The NMPC uses predicted states and inputs with path guidance and a safe-flight corridor (SFC) to generate commands. A soft SFC penalty preserves optimization feasibility when predicted motion temporarily leaves the corridor, while the disturbance estimate is held constant over the prediction horizon.

B. Disturbance- and Obstacle-Aware NMPC

  • At update k, the NMPC optimizes predicted input sequences, and the online procedure outputs angular-velocity and thrust commands.The displayed admissible-input-set passage supplies no further constraint details.Source: B. Disturbance- and Obstacle-Aware NMPC · p. 5
  • The online procedure updates the local map, replans A* when a new goal arrives or the remaining path is obstructed, then builds a horizon reference and SFC.It solves the NMPC using the state estimate and disturbance estimate, then applies the first optimal input.Source: B. Disturbance- and Obstacle-Aware NMPC · p. 5
  • An SFC residual of zero means the corridor constraint is satisfied; its soft penalty maintains OCP feasibility during temporary violations.With obstacle inflation, a small SFC violation need not imply physical collision, and the soft penalty avoids aggressive corrective motion.Source: B. Disturbance- and Obstacle-Aware NMPC · p. 5
  • The disturbance estimate is held constant over the prediction horizon, but an available disturbance model can replace this zero-order prediction with an exosystem.The passage cites and for disturbance preview and compensation.Source: B. Disturbance- and Obstacle-Aware NMPC · p. 5

V. COMPARATIVE SIMULATION

The comparative simulation section describes the simulation computing setup and identifies IPC as the comparison method. Its supplied materials also include tables on software implementation and interconnected-observer ablation.

V. COMPARATIVE SIMULATION

  • The simulations ran on a PC with an Intel i7-14700F CPU, NVIDIA GeForce RTX 4060Ti GPU, and 32 GB RAM.Software configuration and main parameters are referenced in Tables II and IV.Source: §V · pp. 5–6
  • Table III is identified as an interconnected-observer ablation under wind and a suspended payload.Its caption defines downward arrows as lower-is-better and pp as percentage points; the supplied passage gives no cell values.Source: Table III · p. 6
  • DR-IPC is compared with the state-of-the-art IPC method.The supplied passage does not report comparative outcome values.Source: §V · p. 6

A. Narrow-Passage Navigation under Constant Wind

In a narrow-passage simulation under constant crosswind, DR-IPC completes the passage without collision while IPC collides twice. DR-IPC also maintains altitude more effectively in the reported comparison.

A. Narrow-Passage Navigation under Constant Wind

B. Interconnected-Observer Ablation

Across disturbed multi-goal trials, DR-IPC completed more missions than IPC; the interconnected observer also improved disturbance estimates and smoothed control and attitude responses. The observer ablation used identical flight settings under a suspended payload and time-varying wind.

B. Interconnected-Observer Ablation

  • 0.260 to 0.036 m/s^2 disturbance-vector RMSE accompanied a 47.3% reduction in disturbance-rate RMS with EKF+NDO.Angular-velocity command RMS fell from 0.81 to 0.45 rad/s, wy saturation from 36.0% to 2.1%, and roll–pitch RMS from 11.3° to 8.6°; the comparison used identical settings with a suspended payload and time-varying wind.Source: B. Interconnected-Observer Ablation · p. 6

C. Multi-Goal Navigation

  • 9 of 10 versus 1 of 10 missions were completed by DR-IPC and IPC, respectively, under wind and payload disturbances.DR-IPC reached 37 of 40 goals versus IPC’s 9 of 40, and average collision/fall events fell from 5 to 0.1 per completed mission.Source: C. Multi-Goal Navigation · p. 7, Table V · p. 7

D. Navigation in an Unstructured MARSIM Environment

The supplied results cover MARSIM navigation, ball-impact rejection, and narrow-archway flight. DR-IPC navigated the unstructured MARSIM environment and limited altitude loss after a ball impact; the archway passage reports corrective motion and altitude oscillation for IPC without disturbances.

D. Navigation in an Unstructured MARSIM Environment

  • DR-IPC continuously updated its A* reference, NMPC prediction, and executed trajectory while navigating the unstructured MARSIM forest environment.The evaluation assessed system integration and cross-simulator applicability.Source: D. Navigation in an Unstructured… · p. 7

A. Ball-Impact Rejection in Hover

  • 0.70 m minimum altitude and recovery to the ±0.05 m altitude-error band within 2.30 s followed the ball impact under DR-IPC.IPC reached a minimum altitude of 0.08 m; the reconstructed disturbance responded rapidly at the impact time of 4 s.Source: A. Ball-Impact Rejection in Hover · p. 8

B. Suspended-Payload Transportation with Wind

  • Without disturbances, IPC eventually completed the archway passage but showed repeated corrective motion and altitude oscillations, with p_min_z = 0.25 m and RMSE_pz = 0.17 m.The supplied archway passages also describe comparisons of trajectories, altitude responses, and SFC-violation depth, but do not state DR-IPC’s disturbed-case outcome.Source: B. Suspended-Payload Transportation… · p. 8, Figure 12 · p. 8

C. Suspended-Payload Transportation with Dynamic Obstacle

In the dynamic-obstacle experiment, IPC and DR-IPC completed the task, but IPC had multiple collisions while DR-IPC briefly entered the inflated safety margin without collision. Outdoor flights completed all four routes, and computation results indicate soft real-time onboard operation with occasional deadline overruns.

C. Suspended-Payload Transportation with Dynamic Obstacle

  • Both methods completed the dynamic-obstacle task, but IPC had multiple collisions while DR-IPC briefly entered the 0.30 m inflated safety margin without collision.The soft SFC penalty allowed the brief deviation without an aggressive correction; DR-IPC’s angular-velocity and thrust commands stayed within prescribed limits.Source: C. Suspended-Payload Transportation… · pp. 8–9, Figure 14 · p. 9, Figure 15 · p. 9
  • 4046 of 4201 control cycles (96.31%) met the 10-ms deadline, supporting soft real-time onboard execution at 100 Hz with occasional overruns.Most overruns occurred during map updates and A* replanning; 153 of 155 deadline misses were isolated single-cycle overruns.Source: C. Suspended-Payload Transportation… · p. 9, Figure 16 · p. 9

D. Outdoor Experiments

  • DR-IPC completed all four wooded and factory routes without collision despite ambient airflow and payload-induced disturbances.The online pipeline included LiDAR odometry and mapping, A* planning, SFC construction, EKF+NDO estimation, and NMPC.Source: D. Outdoor Experiments · pp. 9–10, Figure 13 · p. 9

VII. CONCLUSIONS

  • The conclusion attributes disturbance-resilient navigation to an interconnected EKF+NDO and an obstacle- and disturbance-aware NMPC that jointly optimizes local motion and angular-velocity/thrust commands.The soft SFC penalizes corridor violations while avoiding excessive corrective responses.Source: §VII · p. 10
Loading 2610.03530v1…