03 / 06 · NMPC · CasADi + IPOPT
Plan the next two seconds of flight, fly only the first step, then plan again. Twenty times a second, on a simulated multirotor in PX4 and Gazebo.
How one control tick works
01 · Look two seconds ahead
Every 50 ms the planner simulates the next 2 seconds: 20 steps of 0.1 s, a point mass it steers with acceleration.
Aimed straight at the goal, that plan flies through a pillar. The red dots are the steps that land inside a pillar's padding.
02 · Make it an optimization
Get to the goal, use little effort, change smoothly, and stay out of every pillar padded by 0.5 m (the dashed rings). IPOPT solves it in about 15 ms of the 50 ms budget.
The orange plan is the answer: it bends around the padding instead of through it.
03 · Fly one step, then re-plan
Only the first step is flown. Next tick it solves again from where the drone really is, starting from the old plan shifted by one step.
The faint lines are the plans from the last second. Each lasted 50 ms before the next one replaced it.
04 · Stalls, and the way out
With a wall dead ahead, braking to a hover is a local optimum and the plan stalls. When the plan reaches less than 0.6 m ahead, it re-solves from two guesses bowed 1.5 m to each side and keeps the cheaper result.
Blue: the two guesses, the kept one solid. Switch this off in the playground below and the drone hovers in front of the wall for good.
plan · 20 stepsinside paddingescape guessesflown
Try it
Click or tap to move the goal; the next solve aims there, like the live goal in RViz. Turn anti-stall off on the wall to see the local optimum. A 2D re-implementation of mpc_core.py: same model, cost, soft constraints and defaults, solved by projected gradient descent instead of IPOPT, with perfect velocity tracking as in sim_offline.py.
RViz draws the predicted horizon at every tick. It is the most useful view for debugging MPC: when the orange line looks wrong, the next second of flight will be wrong too.
Re-solved every 50 ms. Numbers are the defaults in mpc_params.yaml; sk is a slack the solver pays dearly for.
minimize over u0 … u19
Σk |pk − g|2 + 0.1 |uk|2 + 0.4 |uk − uk−1|2 + 1000 sk + 2000 sk2 + 8 |pN − g|2 + 2 |vN|2
subject to
The core has no ROS in it, so every claim above is checked in about 6 seconds with pytest, on rollouts with perfect velocity tracking.
| Area | Test | Asserts |
|---|---|---|
| Straight line | converges, command toward goal | ends within 0.7 m of a goal 6 m out, never more than 0.2 m off the line |
| Limits | speed and accel hold | |v| ≤ 1.5 m/s and |u| ≤ 2 m/s² every tick, and it really reaches 90% of 1.5 m/s |
| altitude floor | a goal at 0.1 m: the plan stops at 0.4 m, within 5 cm | |
| overspeed stays feasible | starting at 3.4 m/s still solves, and the command is clamped to 1.5 m/s | |
| Obstacles | detour a tall pillar | reaches the goal around it, more than 0.2 m from its surface |
| fly over a low one | a 1.2 m pillar: no sideways detour over 0.5 m, above its lid whenever over it | |
| no fly-over without a top | the same pillar seen by lidar: goes around instead | |
| squeezed but feasible | starting inside the padding still solves, slack > 0.01, does not accelerate inward | |
| Solver | warm start helps | fewer IPOPT iterations than a cold start |
| failure reported | max_iter 1 fails cleanly: no command, failure counted, caller brakes | |
| Obstacle list | keep nearest, pad empty | 21 obstacles → the nearest 10; none → 10 far-away dummies |
| Stuck detector | slow ticks, near goal, oscillation | 40 slow ticks = stuck; never near the goal; jitter of 5 cm = stuck |
| Frames | ENU ↔ NED, world rows | round trip is exact; the world file's pillars come out swapped and with real tops |
a
Speed, altitude and pillar distance are soft. If a bump or an EKF jump puts the drone past a limit, a hard constraint would make the whole problem infeasible at the exact moment avoidance matters most. With slack, it still solves and steers back. Only the dynamics and the acceleration box stay hard, and the output is still clamped to 1.5 m/s.
b
The drone is outside a pillar when the larger of two gaps, beside it or above its top, is positive. An exact max broke IPOPT's step at the boundary, so the code uses a log-sum-exp smooth max (β = 10), shifted by ln 2 / β so it is only ever more cautious than the exact one.
c
With pillar heights known, the 1.2 m pillar plus padding sits below the 2 m cruise, so the plan goes straight over with no detour. A 2D LiDAR only sees one plane, so scanned points become pillars of unknown height and the drone stays in the plane. Same solver: the top is just set to infinity.
d
Each solve starts from the last plan shifted one step, which cuts iterations (a unit test checks it). If IPOPT fails, the drone brakes to a hover and logs it, and the next solve cold-starts from that same brake-to-hover trajectory.
Python · CasADi · IPOPT · ROS 2 Humble · PX4 v1.14 · Gazebo Garden