Perception and navigation · Hardware Academy

From reflexes to maps

Reactive behaviors: Braitenberg vehicles

In 1984 Valentino Braitenberg showed that a vehicle with two sensors wired almost directly to two motors can look purposeful. Connect each sensor to the motor on its own side so that more stimulus means less speed, and the vehicle turns toward the stimulus; cross the connections and it turns away. Obstacle avoidance on the course robot is the same idea with numbers. Convert each ultrasonic range into a proximity p = max(0, 1 − d / 50 cm), then command left = 0.6 − 0.8·pR and right = 0.6 − 0.8·pL. An obstacle on the right slows the left wheel, so the robot turns left, away from it. There is no map and no memory, which makes it fast (a few multiplications at 100 Hz) and fragile: in a corner it can swing left and right forever.

Wall following

To follow a wall on the right at 25 cm, steer on the side-distance error: turn = Kp·(d − 25), positive meaning toward the wall. With Kp = 0.02 per cm, being 10 cm too far gives a turn command of 0.2. Pure P control weaves, because the side sensor reads the robot's angle to the wall as extra distance; a D term on the error, or the 45° ray used to estimate the wall angle, damps it. Corners need a rule of their own: if the front range drops under 25 cm, turn left in place until it clears. That rule is already a second behavior, which is where arbitration comes in.

Line following

Two reflectance sensors such as the TCRT5000 sit side by side over a 19 mm strip of electrical tape, a few millimeters above the floor. Digital outputs give an error of −1, 0 or +1; analog outputs give a proportional error e = (L − R)/(L + R), which allows a smooth PD law such as turn = 0.6·e + 0.05·de/dt around a base speed of 0.5. Slow the base speed as |e| grows so the robot does not overshoot curves. When both sensors lose the line the robot needs memory: the side where the line was last seen. Arc toward that side, widen the search after half a second, and stop after 3 s rather than wander off the table.

Bug algorithms: reaching a goal without a map

Lumelsky and Stepanov's Bug algorithms (1987) need only the goal position, odometry and a contact or range sensor. Bug0 heads for the goal and follows any obstacle it hits until it can head for the goal again: simple, but some obstacle shapes trap it in a loop. Bug1 circles each obstacle completely, remembers the boundary point closest to the goal, returns there and leaves; it always succeeds but can walk 1.5 times each obstacle's perimeter. Bug2 draws the M-line from start to goal, follows the obstacle boundary from the hit point until it meets the M-line again at a point closer to the goal, then leaves along the M-line. It reaches the goal whenever the goal is reachable, and its paths are usually much shorter than Bug1's.

Arbitration: state machines and behavior trees

A robot running several behaviors must pick one each tick. A finite state machine (FSM) does it explicitly: states such as Idle, Follow, Avoid and Recover, the events that move between them, and the action each state runs. Two details make FSMs robust. Hysteresis: enter Avoid when the front range drops under 20 cm but leave only above 30 cm, so 1 cm of sensor noise around 20 cm cannot make the state chatter. Timeouts: every state that waits for something has a limit, so the robot can never wait forever.

Rodney Brooks's subsumption architecture (1986) arbitrated by priority layers instead. Modern stacks use behavior trees: trees of Sequence, Fallback and action nodes ticked at 10-100 Hz, each returning SUCCESS, FAILURE or RUNNING. ROS 2's Nav2 runs its navigation logic on BehaviorTree.CPP because a tree stays readable with 50 behaviors, where an FSM with 50 states and hundreds of transitions does not.

Maps, SLAM and planning in brief

An occupancy grid (Moravec and Elfes, 1985) splits the floor into cells, often 5 cm, each holding the probability that it is occupied; a 10 m × 10 m room is 40,000 cells. Each range reading raises the probability at the hit cell and lowers it along the beam, usually in log-odds so an update is an addition. SLAM (simultaneous localization and mapping) breaks the chicken-and-egg problem that you need a map to localize and a pose to map: particle filters such as GMapping and graph optimizers such as Cartographer and slam_toolbox estimate both, and a loop closure (recognizing a place seen before) removes the drift accumulated around the loop.

Given a grid, Dijkstra expands cells in order of cost so far, g, and finds the shortest path. A* expands in order of f = g + h, where h estimates the remaining cost, for example the straight-line distance. If h never overestimates, A* is still optimal, and in open space it explores a narrow band toward the goal instead of a growing circle, often visiting ten times fewer cells on a 100 × 100 grid. Real planners also inflate obstacles by the robot's radius so the path keeps a safe clearance.

Bug2. The robot drives along the M-line, hits the obstacle at H, follows the boundary and leaves at L, the first point back on the M-line that is closer to the goal than H.

Interactive: drive the state machine

A line follower's state machine

Fire events and watch the machine move between Idle, Follow, Avoid and Recover while the robot below acts out the current state.

Current state: Idle

Press Start. Buttons outlined in color have a transition from the current state.

Idle action: Motors off. Waits for Start.

Every state has a way out: hysteresis on the obstacle range and a 3 s timeout on the search keep the robot from chattering or waiting forever.

Event log

No events yet.

In practice

Practice

Design a line follower with obstacle avoidance, then run it in the simulator

Design the state machine before writing code. Use a start state for "no line seen yet", a 20 cm entry and 30 cm exit threshold for obstacles, and a 3 s timeout on the search. Then implement it in the robot simulator lab. On the line track, check that the robot finds the tape, follows it, and recovers when it loses the line on a tight curve. The line track has no obstacles, so test AVOID in the open room: there is no tape there, so the worked answer stays in SEEK and AVOID, cruising and turning away from every obstacle it meets. From the lab's start position its first AVOID comes about 11 s in, with the tight turn toward the open side.

Worked answer: the design

State	Action every tick	Leaves when
SEEK (start)	Cruise at 0.5 (0.3 when something is within 50 cm), steering away from the closer side	line found → FOLLOW; front < 20 cm → AVOID
FOLLOW	P on the line error: left = 0.5 − 0.35·e, right = 0.5 + 0.35·e	front < 20 cm → AVOID; both sensors off the line for 0.3 s → RECOVER
AVOID	Tight turn toward the more open side, chosen on entry: inner wheel −0.2, outer 0.5	front > 30 cm for at least 0.8 s → RECOVER if a line has been seen (it is on the side the robot turned away from), otherwise back to SEEK
RECOVER	Arc toward the last-seen side	line found → FOLLOW; front < 20 cm → AVOID; 3 s without the line → STOP
STOP	Motors off	Only a human restarts it

Worked answer: the code (the lab calls control(sensors, memory) every tick and keeps memory between calls):

line-follow-fsm.jsjs

// Robot simulator lab: line follower with obstacle avoidance as a state machine.
// sensors.line.left/right are 0 or 1, sensors.ultrasonic.* are in cm, sensors.t is in s.
function control(sensors, memory) {
  const { line, ultrasonic, t } = sensors;
  if (memory.state === undefined) {
    memory.state = "SEEK";          // no line seen yet: cruise and avoid until the tape shows up
    memory.since = t;
    memory.lastSide = 1;            // +1: line last seen on the left, -1: on the right
    memory.lastOnLine = t;
    memory.seenLine = false;
  }
  const go = (s) => {
    if (s === "AVOID") memory.avoidDir = ultrasonic.left > ultrasonic.right ? 1 : -1;  // +1: turn left
    memory.state = s;
    memory.since = t;
  };
  const inState = t - memory.since;
  const onLine = line.left === 1 || line.right === 1;
  if (onLine) {
    memory.lastOnLine = t;
    memory.seenLine = true;
  }
  if (line.left && !line.right) memory.lastSide = 1;
  if (line.right && !line.left) memory.lastSide = -1;

  // 1. transitions (events and guards)
  switch (memory.state) {
    case "SEEK":
      if (ultrasonic.front < 20) go("AVOID");
      else if (onLine) go("FOLLOW");
      break;
    case "FOLLOW":
      if (ultrasonic.front < 20) go("AVOID");
      // short gaps are normal (sensors that straddle the line both read 0 when centered)
      else if (t - memory.lastOnLine > 0.3) go("RECOVER");
      break;
    case "AVOID":                                  // hysteresis: in at 20 cm, out at 30 cm
      if (ultrasonic.front > 30 && inState > 0.8) {
        memory.lastSide = -memory.avoidDir;        // the line is on the side we turned away from
        go(memory.seenLine ? "RECOVER" : "SEEK");
      }
      break;
    case "RECOVER":
      if (ultrasonic.front < 20) go("AVOID");
      else if (onLine) go("FOLLOW");
      else if (inState > 3) go("STOP");          // timeout: never search forever
      break;
  }

  // 2. actions (what each state does every tick)
  switch (memory.state) {
    case "SEEK": {                                 // cruise, steering away from the closer side
      const p = (d) => Math.max(0, 1 - d / 50);
      const base = ultrasonic.front < 50 ? 0.3 : 0.5;
      const turn = 0.5 * (p(ultrasonic.right) - p(ultrasonic.left));
      return { left: base - turn, right: base + turn };
    }
    case "FOLLOW": {
      const e = line.left - line.right;           // +1 line under the left sensor
      return { left: 0.5 - 0.35 * e, right: 0.5 + 0.35 * e };
    }
    case "AVOID": {                                // tight turn toward the more open side
      const d = memory.avoidDir;
      return { left: d > 0 ? -0.2 : 0.5, right: d > 0 ? 0.5 : -0.2 };
    }
    case "RECOVER":                                // arc toward the side the line was last seen
      return { left: 0.3 - 0.25 * memory.lastSide, right: 0.3 + 0.25 * memory.lastSide };
    default:
      return { left: 0, right: 0 };
  }
}

Notice the structure: transitions first, actions second, and every piece of state (current state, entry time, last-seen side, whether a line has ever been seen) lives in memory. In our runs in the lab's own physics, this controller never crashed in 60 s from 200 different starting headings in the open room, and on the line track it followed the tape for two minutes without reaching STOP. If the robot oscillates on straight track, lower the 0.35 steering gain; if it loses the line on curves, lower the base speed before raising the gain.

Real-world scenario

The robot circles forever in the corner of the test pen

A student team's exploration robot (three HC-SR04 rangers at −45°, 0° and +45°, reactive steering that blends goal-seeking with obstacle avoidance) must cross a 3 m × 3 m pen and reach a charging beacon behind a low L-shaped wall. In 4 of 10 runs it drives into the inside corner of the L and stays there, sweeping left and right, until the battery runs flat.

Symptoms

Telemetry shows the turn command flipping sign about every 1.2 s while the front range bounces between 18 cm and 35 cm
The pose trace draws the same small figure-eight in the corner over and over
Moving the beacon 30 cm to the side makes the problem vanish for that layout
Lowering the steering gains only makes the oscillation slower

Interview questions

screenHow would you make a robot follow a black line with two reflectance sensors?

I would mount the two sensors a few millimeters above the floor, straddling the line, and turn their readings into an error: with digital outputs it is −1, 0 or +1, with analog outputs (L − R)/(L + R) gives a proportional error. A PD law, for example turn = 0.6·e + 0.05·de/dt on top of a base speed of 0.5, steers back toward the line, and I would slow the base speed as the error grows so it does not overshoot curves. The important part is the lost-line case: remember which side the line was last seen on and arc or spin toward it, with a timeout of a few seconds before stopping. I would calibrate the threshold on the actual floor and tape, because carpet, glossy tile and sunlight all shift the readings.

screenWhat is hysteresis in a robot state machine, and why do you need it?

Hysteresis means using different thresholds to enter and to leave a state. If Avoid starts below 20 cm and ends above 20 cm, an ultrasonic reading that jitters by a centimeter around 20 cm flips the state every tick, and the robot twitches between two actions. With entry at 20 cm and exit at 30 cm, noise smaller than 10 cm cannot cause a transition. You can get a similar effect with a minimum time in state, for example staying in Avoid for at least 0.8 s. Both keep the actuators from chattering and make the behavior predictable enough to test.

screenWhen would you choose a behavior tree over a finite state machine?

For a handful of behaviors an FSM is the clearest choice: four states and eight transitions fit on one diagram and are easy to test. As behaviors grow, an FSM's transitions grow roughly with the square of the states, and adding one behavior means editing many transitions. A behavior tree composes behaviors hierarchically with Sequence and Fallback nodes, so adding a recovery is adding a subtree, and priorities are visible as left-to-right order. That is why Nav2 uses BehaviorTree.CPP for navigation with dozens of recovery and planning behaviors. On a microcontroller with three behaviors I would still write a switch statement.

deepCompare Dijkstra and A*. When is A* optimal, and what happens with a bad heuristic?

Both search a graph such as an occupancy grid. Dijkstra expands nodes in order of cost so far, g, so it explores a growing circle around the start and always finds the shortest path. A* expands in order of f = g + h, where h estimates the remaining cost, so it explores a narrow band toward the goal and often visits ten times fewer cells in open space. If h never overestimates the true remaining cost (admissible), A* is still optimal; Euclidean distance is admissible for any-angle moves and Manhattan distance for 4-connected grids. If h overestimates, A* gets faster but can return a longer path, and with h = 0 it degrades exactly into Dijkstra.

deepExplain SLAM to a firmware engineer in about a minute, including loop closure.

To build a map you need to know where you are, and to know where you are you need a map, so SLAM estimates both at once. The robot predicts its motion from odometry, which drifts, and corrects it by matching what the lidar or camera sees against the map built so far. Graph-based SLAM, used by Cartographer and slam_toolbox, stores poses as nodes and measurements as constraints, then solves for the poses that best satisfy all constraints. Loop closure is when the robot recognizes a place it has visited before; that one constraint pulls the whole trajectory back into shape and removes drift accumulated over the loop. The firmware engineer's part matters a lot: good timestamps and clean odometry make SLAM converge.

Quiz

References and further reading

Where the ideas in this lesson come from, and what to read next. Books are the standard texts; papers are the original sources; standards are what employers work to.

Book
Valentino Braitenberg (1984). Vehicles: Experiments in Synthetic Psychology. MIT Press.
Read: Vehicles 1-3: reactive behaviours from wiring alone.
Paper
Vladimir J. Lumelsky and Alexander A. Stepanov (1987). Path-Planning Strategies for a Point Mobile Automaton Moving Amidst Unknown Obstacles of Arbitrary Shape. Algorithmica 2, 403-430.doi:10.1007/BF01840369
Read: The Bug algorithms.
Paper
Peter E. Hart, Nils J. Nilsson and Bertram Raphael (1968). A Formal Basis for the Heuristic Determination of Minimum Cost Paths. IEEE Transactions on Systems Science and Cybernetics 4(2), 100-107.doi:10.1109/TSSC.1968.300136
Read: A*, in the original.
Paper
Alberto Elfes (1989). Using Occupancy Grids for Mobile Robot Perception and Navigation. Computer 22(6), 46-57.doi:10.1109/2.30720
Read: Occupancy grids.
Paper
Hugh Durrant-Whyte and Tim Bailey (2006). Simultaneous Localization and Mapping: Part I. IEEE Robotics and Automation Magazine 13(2), 99-110.doi:10.1109/MRA.2006.1638022
Read: The SLAM tutorial, part I.