Obstacle avoidance

Why this matters

UAT 322 covered path planning on a map with A* and checking whether the drone can stop in time. In a real warehouse, though, a box may be left in the aisle or a pallet may stick out, and none of that is on the map. The drone must react to what its sensors see right now. Reactive methods are fast, but they have weaknesses you need to know. Without that knowledge, a drone may freeze in the middle of an aisle or oscillate between racks.

Potential fields

Khatib (1986) proposed the potential field, in which the goal attracts the robot and obstacles repel it, and the robot moves along the resultant force. The idea is simple and runs in real time, but Koren and Borenstein (1991) analysed its inherent limitations: getting stuck in a local minimum where forces cancel, failing to pass between closely spaced obstacles, and oscillating in narrow passages.

import math

def fly(start, goal=(10, 0), obstacle=(5, 0), k_att=1.0, k_rep=5.0, rho0=2.5, step=0.05):
    x, y = start
    for i in range(2000):
        fx, fy = k_att * (goal[0] - x), k_att * (goal[1] - y)
        dx, dy = x - obstacle[0], y - obstacle[1]
        d = math.hypot(dx, dy)
        if d < rho0:
            m = k_rep * (1 / d - 1 / rho0) / d ** 2
            fx, fy = fx + m * dx / d, fy + m * dy / d
        f = math.hypot(fx, fy)
        if f < 1e-3:
            return f"stuck at ({x:.2f}, {y:.2f}) after {i} steps"
        x, y = x + step * fx / max(f, 1), y + step * fy / max(f, 1)
        if math.hypot(goal[0] - x, goal[1] - y) < 0.1:
            return f"reached goal after {i} steps"
    return "timeout"

print("start on the line:", fly((0, 0)))
print("start 0.5 m to the side:", fly((0, 0.5)))

start on the line: stuck at (4.17, 0.00) after 88 steps
start 0.5 m to the side: reached goal after 233 steps

Vector field histogram

The VFH of Borenstein and Koren (1991) turns the obstacles around the robot into a polar histogram of obstacle density in each direction, then selects the “valley” below a threshold that is closest to the goal direction. It does not get trapped the way a potential field does, because it decides from the whole surroundings.

ranges = [6.0] * 36                                   # m per 10° sector, starting at 0°, clockwise
for k in (34, 35, 0, 1, 2): ranges[k] = 1.2           # box blocking the way ahead
for k in (3, 4): ranges[k] = 2.5
for k in (31, 32, 33): ranges[k] = 2.8
THRESH, GOAL_DEG = 3.0, 0

def angle_diff(a, b):
    return abs((a - b + 180) % 360 - 180)

blocked = [k * 10 for k, r in enumerate(ranges) if r < THRESH]
free = [k * 10 for k, r in enumerate(ranges) if r >= THRESH]
best = min(free, key=lambda a: angle_diff(a, GOAL_DEG))
print("blocked headings:", blocked)
print(f"chosen heading {best} deg ({angle_diff(best, GOAL_DEG)} deg from goal)")

blocked headings: [0, 10, 20, 30, 40, 310, 320, 330, 340, 350]
chosen heading 50 deg (50 deg from goal)

Flight-controller systems

PX4 Collision Prevention uses range data from sensors on the flight controller, or from the companion computer via MAVLink. It divides the surroundings into 72 sectors, works in Position mode, and slows and stops before a collision, with the distance set by CP_DIST and the delay by CP_DELAY. If range data stops for 0.5 s, movement is restricted; if it stops for 5 s, the vehicle switches to HOLD. ArduPilot has a similar Object Avoidance system. These systems prevent collisions but do not plan a way around.

For moving objects such as forklifts or other drones, the velocity obstacle of Fiorini and Shiller (1998) finds the set of velocities that would cause a future collision if both kept their current velocity, then chooses a velocity outside that set (extended in Module 4).

Module lab

Common mistakes

Summary

• Potential fields are simple but get stuck in local minima, cannot pass narrow gaps and oscillate.
• VFH uses a polar histogram to choose the free valley closest to the goal.
• PX4 Collision Prevention slows and stops before a collision and switches to HOLD after 5 s without range data.
• Reactive avoidance must work alongside path planning, and velocity obstacles for moving objects.

Check your understanding

1. What causes a local minimum in a potential field?
2. How does VFH choose a heading?
3. The free sectors nearest the goal are at 40° and 320°, and the goal is at 10°. Which heading should be chosen?
4. Into how many sectors does PX4 Collision Prevention divide the surroundings?
5. What does PX4 do if range data stops for 5 s?