05 / 06 · Dynamic Window Approach · holonomic · 2D lidar
Try every velocity the drone can reach in the next instant, fly each one forward in imagination, throw out the ones that crash, and keep the best. Sideways included.
How one tick works
01 · The window
Classic DWA searches forward speed and turn rate, like a car. A multirotor can strafe, so HOLO-DWA searches the planar velocity directly and keeps the nose, and the lidar's forward arc, on the goal.
The window is what can be reached in 0.2 s at 1 m/s²: a 5 × 5 patch of velocity space, 0.1 m/s apart (the box, bottom right).
02 · Roll out and mask
Every candidate is rolled forward 3 s at constant velocity against the current scan. Any that would come within 0.3 m of a return, or is faster than it could still brake from, √(2·a·room), is dropped (red).
03 · Score
0.3·heading + 0.3·clearance + 0.4·velocity. Heading is the cosine to the goal. Clearance probes a fixed 1.5 m along the direction; measuring along the predicted path instead made slow candidates look safe and rewarded creeping into a cylinder.
Velocity rewards the goal-directed part with a floor for any motion, so the drone does not drift diagonally in the open but can still slide along a wall to find the gap.
04 · Arrive, don't orbit
Within 2 m, a candidate is scored by how close it passes, then by matching v = √(2·0.5·d). The baseline's bonus for entering the goal circle rewarded blasting through it, and a fly-by that missed got no pull at all and orbited forever.
lidar returnsrejectedchosenflown
Try it
Click or tap to move the goal. Switch to the baseline scoring to see what the five rounds fixed. A 2D port of dwa_core.py with the live config from scanner.py, on the dwa_test arena with a simulated 270° lidar.
Every 0.2 s, with the live config from scanner.py. v is a candidate velocity, p the drone, ĝ the unit vector to the goal g, S the lidar points.
choose
v* = argmaxv 0.3 cos∠(v, g − p) + 0.3 clip((cdir(v) − 0.3) / 0.5, 0, 1) + 0.4 max(v · ĝ, 0.5 |v|) / 1.5
over
Each round: fly, read the logs, find the mechanism, fix it, fly again. The window, the mask and the rollout stayed identical to the baseline; only the scoring, one sensing fix and a few limits changed.
| round | problem found | fix |
|---|---|---|
| baseline | While accelerating, the speed reward roughly equals the heading penalty, so noise flips the choice into diagonal drift toward the wall | velocity reward → blend (goal component + scalar floor) |
| iter 1 | Creep trap: slow means a short ray means high clearance, rewarding a crawl into a cylinder | clearance as a fixed 1.5 m probe along the direction |
| iter 2 | Tick replay showed the point cloud did not match the world: a lidar mirror bug (Gazebo +angle is left, the transform assumed right) | flip_y=True, proven by a frame check (0.08 m vs 0.32 m) |
| iter 3 | Avoidance perfect, but orbiting the goal: 10000 + speed rewards blasting through the circle | braking-curve bonus 10000 − |v − √(2·a·d)| |
| iter 4 | 15 runs, 0 collisions, but 2 fly-bys: a ray that misses the 0.5 m circle gets no pull | continuous basin within 2 m |
| iter 5 | — | final check: 3 batches × 5 runs, all passed |
| reached | collisions | closest obstacle | average time | |
|---|---|---|---|---|
| baseline | 1 / 5 | 44+ episodes | contact (0.0 m) | 90 s, mostly timeouts |
| tuned | 15 / 15 | 0 | 0.66 m | 18 s |
The collision count is a 2D proxy: the nearest lidar return closer than 0.3 m, counted on rising edges. The extra runs in the repo's demo folder are replays of logged headless flights, because on this WSL2 machine any live rendering degraded the flight itself (18 s clean headless against 55–90 s wandering with a GUI).
Python · numpy · ROS 2 Humble · PX4 v1.14 · Gazebo · 270° 2D lidar