Nonlinear Model Predictive Control Obstacle Avoidance

Nonlinear MPC (CasADi + IPOPT) obstacle avoidance navigation: a multirotor in PX4 SITL + Gazebo simulation, sensing obstacles with a 2D LiDAR, re-solving a receding-horizon optimization problem at every control tick, and flying toward the goal with 3D velocity commands.

Demo

The demo has been accelerated

Rviz version

update: live goal demo with Rviz

Requirements

• WSL2 + Ubuntu 22.04 (or native), ROS 2 Humble
• PX4-Autopilot v1.14.4 + Gazebo Garden (x500_lidar_2d model)
• ros_gz bridge, Micro-XRCE-DDS-Agent, px4_msgs, tmux
• Python 3.10 + numpy + casadi (pip install casadi, includes IPOPT) Check out HOLO-DWA for a detailed installation of env(same environment, DWA algorithm).

Quick Start

pip install casadi                     # one-time
python3 MPC_OA/tools/toy_nlp.py        # verify IPOPT works (Step 0)
cd ~/ws
colcon build --packages-select mpc_oa --symlink-install
cd src/MPC
./run.sh                      # goal (12, 0), altitude 2 m, fixed obstacles, opens RViz if a display is present
./run.sh 8.0 -3.0             # custom goal_x goal_y (Gazebo world coordinates)
./run.sh 8.0 -3.0 2.5         # plus custom goal altitude
OBST_SOURCE=lidar ./run.sh    # switch obstacle source to 2D LiDAR scan
HEADLESS=1 ./run.sh           # no GUI (more stable tracking under WSL2)
./run.sh kill                 # tear everything down

Retarget the goal live from RViz: use the toolbar's 2D Goal Pose (or Publish Point) tool and click anywhere in the map - the clicked xy becomes the new goal at the configured altitude. Works mid-flight and after GOAL_REACHED (the drone takes off again toward the new goal), which makes iterative testing much faster than restarting the stack.

After a run, inspect the results:

python3 MPC_OA/tools/check_log.py     # latest flight CSV: goal reached / clearance / solve-time distribution / failure rate

How It Works

Gazebo (x500_lidar_2d, world: mpc_test)
   └─ lidar_2d_v2 (270°, 1080 beams) ──► gz scan topic
        ▼ ros_gz_bridge (→ /lidar, LaserScan)
 obstacle_sensor_node ──► /mpc/obstacles (Float32MultiArray, NED)
        │   each row (n, e, radius, top_altitude); top < 0 = unknown altitude
        │   source=fixed:  world_spec cylinder parameters (omniscient, includes
        │                  top → can be flown over)
        │   source=lidar:  scan points → sector-reduced to nearest K points →
        │                  vertical columns of unknown height
        ▼
 mpc_planner_node (20 Hz)
        │   INIT → TAKEOFF → NAVIGATE → GOAL_REACHED
        │   NAVIGATE: mpc_core.solve() re-solves the NLP → velocity from the
        │             first step of the predicted trajectory
        │   solve failure → fallback to brake-and-hover + log
        ▼
 /fmu/in/trajectory_setpoint ──► XRCE-DDS Agent ──► PX4 SITL

MPC core (mpc_core.py, CasADi + numpy, no ROS):

• Model: double integrator, state [p(3), v(3)], control u = acceleration (3), exact discretization, horizon N=20 × dt=0.1 s (2 s lookahead), multiple shooting.
• Cost: goal tracking (stage + terminal) + |u|² + smoothness |Δu|² + terminal velocity + slack penalty (L1+L2).
• Constraints: acceleration box (hard), dynamics equality (hard); velocity magnitude, altitude range, and obstacle distance are all soft slack constraints — when the measured state exceeds a speed/range bound (collision, EKF jump), the NLP remains structurally feasible and won't go Infeasible at the exact moment obstacle avoidance is needed most. On the output side, v_cmd is still hard-clamped to v_max.
• Obstacle constraint: outside the cylinder ⟺ max(horizontal overshoot, top overshoot) ≥ 0, smoothed with a log-sum-exp max (β=10, shifted right by log2/β to stay conservative). An exact fmax-based SDF makes IPOPT's constraint-boundary step-size computation fail (see WORKLOG Finding 1). Lidar columns have their top set to +∞ → automatically degenerates to a pure XY constraint, so one solver serves both modes.
• Warm start: shift the previous solution by one step; after a failure, cold-start with a dynamically consistent "brake to hover at a_max" rollout (not a hold-position guess — see WORKLOG Finding 3).
• Anti-stall: when an obstacle sits directly between the drone and the goal, "braking to a stop" is itself a local optimum (an MPC-flavored local minimum) — once the predicted trajectory is detected as stalled, re-solve with left/right arc seeds and keep whichever gives the lower cost (see WORKLOG Finding 2).
• Stuck detection: reuses APF_OA's dual-criterion StuckDetector (low speed OR displacement envelope).

The "2D sensing vs. 3D control" tradeoff: in fixed mode the top of each cylinder is known, so short columns are flown straight over (no detour, no extra climb — the advantage of an analytic 3D constraint over point-cloud repulsion); in lidar mode only the scan plane is visible, so scanned points are treated as columns of unknown height → avoidance conservatively stays in the XY plane, while the z-axis is still handled by MPC's altitude tracking and range constraint.

Files

Path	Role	How to test
MPC_OA/mpc_oa/mpc_core.py	MPC core (NLP construction/solve/warm start/anti-stall/stuck detection)	python3 -m pytest test/ (18 tests, ~6 s)
MPC_OA/mpc_oa/world_spec.py	Single source of truth for obstacle specs (same layout as apf_test, for easy comparison)	After editing, run tools/gen_world.py to regenerate
MPC_OA/mpc_oa/obstacle_sensor_node.py	Obstacle source node: fixed (cylinder params) / lidar (scan → sector reduction)	ros2 topic echo /mpc/obstacles
MPC_OA/mpc_oa/mpc_planner_node.py	Flight node: offboard state machine + per-tick NLP solve + fallback + CSV + RViz	Run ./run.sh and watch the 1 Hz status line; tools/check_log.py to inspect the CSV
MPC_OA/config/mpc_params.yaml	All parameters: N / dt / weights / v_max / safe_radius / goal, etc.	Edit, then ./run.sh kill and ./run.sh again (symlink-install, no rebuild needed)
MPC_OA/launch/mpc_nodes.launch.py	ROS-side launch for the three components (sensor + planner + RViz)	ros2 launch mpc_oa mpc_nodes.launch.py rviz:=false
MPC_OA/worlds/mpc_test.sdf	Generated Gazebo world (5 cylinders + goal marker)	run.sh syncs it into PX4 automatically on every run
MPC_OA/tools/toy_nlp.py	Step 0: a 10-line NLP verifying the casadi/IPOPT install	python3 tools/toy_nlp.py
MPC_OA/tools/gen_world.py	Generates the SDF from world_spec	python3 tools/gen_world.py
MPC_OA/tools/sim_offline.py	Offline simulation (no ROS/Gazebo needed, runs in seconds); run this before tuning	python3 tools/sim_offline.py [--trap] [--lidar]
MPC_OA/tools/check_log.py	Flight CSV summary (incl. solve-time distribution and failure rate)	python3 tools/check_log.py
MPC_OA/tools/compare_algos.py	DWA / APF / MPC head-to-head comparison (Step 5b)	python3 tools/compare_algos.py [--trap]
run.sh	One-shot tmux launch of the full stack (PX4+gz / agent / bridge / nodes)	./run.sh, tear down with ./run.sh kill
WORKLOG.md	Decision log, findings, TODOs	—

CSV columns: t, state, position, velocity, v_cmd, u, predicted first/last point, solve_ms, solver_status, fallback, max_slack, dist_goal, n_obs, stuck, stuck_episodes → logs/mpc_*.csv.

RViz: cyan Path = actual flown trajectory, orange line strip = the predicted horizon at each tick (the most instructive visualization for MPC), green sphere = goal, red/orange cylinders = obstacles, thin blue columns = lidar representative points.

Tuning

Edit config/mpc_params.yaml and re-run ./run.sh (no rebuild needed). Tuning order: sweep offline with tools/sim_offline.py first (seconds per run), and only move to Gazebo once the behavior looks right. Note that N and max_obstacles directly determine NLP size (= solve time); the budget for the 20 Hz loop is 50 ms, and the default configuration runs at p95 ≈ 15 ms.