01 / 06 · APF · depth camera · numpy only
The goal pulls, obstacles push, and the drone follows the sum. The interesting part is what it took to make that safe on an autopilot that answers a second late.
How one control tick works
01 · The field
The goal pulls, saturating past 3 m. Each of 12 direction sectors pushes from its nearest obstacle point, so a dense camera cloud cannot out-push the goal.
A swirl turns part of the push sideways, so a pillar dead ahead becomes a go-around instead of a bounce. It keeps its side until the goal is clearly on the other one; re-picking every tick made the drone dither.
02 · The brake
Speed toward every nearby point is cut to what can still stop 0.7 m short of it, with PX4's measured reaction time: v·T + v²/2a = room.
Only the approaching part is removed, so the drone slides along a pillar instead of halting. The bar ahead of the drone is its stopping distance.
03 · The slew limit
The command may change by at most 4 m/s² per tick, which smooths the field where it is steep near obstacles.
Orange is the command, white is what the drone actually flies. PX4 follows like a 0.7 s lag behind 0.25 s of dead time, roughly twice what had been assumed.
04 · Memory of dead ends
In a cup, pull and push cancel and the drone just sits there. After 5 s without real progress, a virtual pillar is planted just goal-side of it.
Virtual pillars only push sideways and they add up, so a spot the drone keeps falling back into keeps getting steeper until it leaves for good (Park and Lee's virtual obstacle idea). Shown here at 6× speed.
pull / flowpush · commandflown
Try it
Click or tap anywhere to move the goal. Turn the swirl or the brake off and watch what each one was there for. This is a 2D re-implementation of apf_core.py with the same defaults and a modelled PX4 lag, not the flight code itself.
One tick at 20 Hz, from obstacle points to a velocity setpoint. Numbers are the defaults in apf_core.py; p is the drone, g the goal, qi the nearest obstacle point in each of 12 × 3 direction sectors, di its distance.
command, before the safety stages
v = sat1.5( Fatt + Frep + Fswirl )
where
then, in order
The first version tuned the field strong (k_rep 1.8, 3 m reach) so the drone would turn early, and checked it against an offline sim that assumed perfect velocity tracking. With the fitted lag in that sim, the old tuning clipped pillars in 17 of 24 runs: the drone reached the turn about a second late. In front of a 1.3 m gap the push was also twice the largest possible pull, so it could not get in.
So safety moved out of the field and into the brake, which knows about the lag. Once braking guarantees the drone can stop, the field only has to steer and can be soft (k_rep 0.6, 2.5 m), and the gaps open up. The brake created a new kind of local minimum, a gap too tight for the standoff, which is what the virtual pillars resolve. Depth mode needed one more piece: the camera looks along the command while the drone, a second behind, still moves along the old one, so the sensor node remembers the last 10 s of points.
| reached cleanly | under 0.35 m (prop tips) | worst clearance | |
|---|---|---|---|
| old | 12 / 55 | 43 | −0.39 m |
| new | 55 / 55 | 0 | 0.41 m |
| new, depth camera, no memory | 20 / 55 | 35 | −0.27 m |
| new, depth camera, 10 s memory | 55 / 55 | 0 | 0.41 m |
| world | old, fixed | new, fixed | new, depth camera |
|---|---|---|---|
| course | 13.4 s, 0.67 m | 13.3 s, 0.74 m | 13.8 s, 0.76 m |
| trap | 13.6 s, 0.91 m | 14.3 s, 0.56 m | 22.2 s, 0.60 m |
| gate | never arrives (0.51 m) | 14.9 s, 0.56 m | 17.4 s, 0.54 m |
| corridor | 17.3 s, 0.20 m | 19.6 s, 0.40 m | 22.6 s, 0.61 m |
| forest3 | never arrives (0.29 m) | 24.9 s, 0.54 m | 23.4 s, 0.65 m |
| forest14 | never arrives (0.26 m) | 24.8 s, 0.33 m | 45.8 s, 0.61 m |
| cup | never arrives (0.29 m) | 96 s, 0.45 m | 66 s, 0.58 m |
The price is speed near obstacles (offline mean about 13 s before, 20 s now), and the cup takes one to two minutes and a dozen pillars. Fixed mode trusts the EKF, which sits up to 0.4 m off in this SITL, so true clearance can be about 0.15 m less than the planner thinks; depth mode measures relative to the drone and does not have that problem.
RViz from the July flights: the obstacle points the planner reacts to, and the path it flew.
Python · numpy · ROS 2 Humble · PX4 v1.14 · Gazebo Garden · OAK-D Lite