02 / 06 · VFH+ with VFH* look-ahead · 2D lidar
Every tick: how far can the drone fly in each of 72 directions, which of those are open, and which opening leads somewhere a few metres on.
How one control tick works
01 · How far each way?
Each bin holds the distance the drone could fly along it before touching an obstacle grown by its own radius plus a safety margin (the dotted circles), done exactly as a ray-disc test.
The margin grows with speed, using the larger of measured and commanded speed, because PX4 is still catching up with the command.
02 · Which ways are open?
A bin closes when its distance drops below 1.6 m and reopens only beyond 2.5 m (the two rings), so a narrow gap does not flicker open and shut.
The July version summed obstacle density per bin, which meant one thing for a known cylinder and another for ten lidar returns on the same pillar. Distance per bin means the same whatever the sensor sends.
03 · Candidates
The centre of each narrow opening; for a wide one, a direction 20° in from each side, plus the goal if it lies between them.
04 · Look before choosing
With more than one candidate, it steps 1.5 m along each, redoes the histogram from there, and carries on: up to 40 expansions, 6 steps deep. A path costs its length plus 0.3 m per 5° of turning.
The branch being flown is kept unless another is 0.5 m shorter. Without the look-ahead: 45 of 56 held-out worlds instead of 56.
05 · How fast?
No faster than it can still stop short of what lies ahead, given PX4's reaction time. With the lidar, it also does not move toward a direction outside the sensor's 270° view: every lidar crash in the lagged sim was a drone backing into the blind spot.
openblockedchosenflown
Try it
Click or tap to move the goal. Switch the look-ahead off to see plain VFH+ choose on the spot. A 2D re-implementation of vfh_core.py on the known map, same defaults, modelled PX4 lag.
Every tick, for each of 72 bins of 5°. Numbers are the defaults in vfh_core.py; for an obstacle of radius r, D is the distance to its centre and φ its offset from the bin's bearing.
free distance along bin k
dk = minobstacles D cos φ − √(R2 − D2 sin2 φ), for |φ| ≤ arcsin(R / D)
with
| Paper | Here | Why | |
|---|---|---|---|
| Map | certainty grid around the robot | cylinder list, or lidar returns kept 10 s | the memory does the grid's job for the lidar |
| Bin value | summed c²(a − b·d²) | free distance along the bin | same meaning whatever the sensor returns |
| Robot dynamics | masked histogram from turning radius | speed-dependent margin, turning cost, braking limit | a multirotor turns on the spot; it cannot change velocity quickly |
| Look-ahead cost | angular costs, discount λ | metres of path, distance-to-goal estimate | ranks branches when the budget runs out |
| Out of budget | n/a | best estimate per branch, 0.5 m switch hysteresis | stable choice while the lidar memory changes |
| Goal bin | n/a | stays open if what blocks it is past the goal | a pillar just behind the goal used to close the way |
| known map | worst clearance | 2D lidar | worst clearance | |
|---|---|---|---|---|
| July | 51/56 clean | 0.59 m | 41/56 clean, 2 crashes | −0.18 m |
| now | 56/56 clean | 0.89 m | 56/56 clean | 0.80 m |
| now, no look-ahead | 45/56 | 0.62 m | 51/56 | 0.80 m |
| now, no speed margin | 56/56 | 0.37 m | 56/56 | 0.61 m |
| now, no lidar memory | 55/56 | 0.80 m |
| world | known map, now | 2D lidar, now |
|---|---|---|
| course | 16 s, 1.40 m | 16 s, 0.78 m |
| gate | 16 s, 1.45 m | 16 s, 1.63 m |
| corridor | 18 s, 1.41 m | 17 s, 1.65 m |
| trap | 16 s, 1.40 m | 16 s, 0.72 m |
| cup | 18 s, 1.35 m | 18 s, 1.65 m |
| wall | 20 s, 1.38 m | 21 s, 1.54 m |
| forest9 | 29 s, 1.11 m | 26 s, 1.53 m |
| forest10 | 24 s, 1.32 m | 24 s, 1.08 m |
| forest14 | 25 s, 1.27 m | 23 s, 1.59 m |
| forest22 | 25 s, 1.04 m | 16 s, 0.17 m (reflown twice: 1.00 m, 0.72 m) |
All 20 flights of the second batch reached the goal. The lidar cannot see the 1.2 m pillars at all, so in lidar mode they are cleared by altitude alone; in one forest22 flight PX4's height estimate drifted 0.5 m and the drone passed 0.17 m over one.
RViz from the July version: the direction fan around the drone, red for blocked, and the path flown.
Python · numpy · ROS 2 Humble · PX4 v1.14 · Gazebo Garden · 270° 2D lidar