Behaviour and Decision-Making

Robotics Fundamentals

Course 1 · Chapter 8 · Behaviour and Decision-Making

Robotics Fundamentals 7 ended with controllers that make a robot do exactly what it is told: hold a speed, hold a heading, follow a line. This chapter is about what it is told. Something has to decide that now is the time to follow the line, that a wall in front overrides the line, that the battery is low and the dock matters more than the floor, and that the robot has been stuck for ten seconds and should try something else. That something is the robot's behaviour layer, the “think” of the sense-think-act loop of Robotics Fundamentals 1. We start with the cheapest kind of decision, a reactive behaviour that maps sensors straight to motors, and with arbitration, the rule for choosing between behaviours that disagree. Then we build the main tool for organising behaviours over time, the finite state machine, including how to stop a noisy sensor from making it flicker. We put numbers on the classic behaviours (line following, obstacle avoidance, wall following, homing to a dock), move on to simple planning on a grid map (occupancy grids, breadth-first search, and A* worked out by hand on a real grid), take a first look at localisation (belief, particle filters, and a worked trilateration from three beacons), and finish with behaviour trees and the unglamorous but vital subject of testing and failure handling.

The thresholds, gains, speeds, probabilities and sensor figures in this chapter are illustrative or typical values chosen so that the arithmetic is easy to follow, not data for a particular robot or sensor. The formulas (differential-drive wheel speeds, the repulsive potential, the log-odds update, breadth-first search, A* with the Manhattan heuristic, the particle weight and the trilateration equations) are standard. Every number in the worked examples was recomputed, and the search examples were actually run on the grids shown. The code is deliberately short, hardware-neutral Python-style pseudocode, used only for an arbitration rule, a state machine and A*; real board code is the job of Programming for Robotics.

1. Reactive behaviours

A reactive behaviour computes its output from the sensor readings of this instant, with no map of the world and no memory of the past: sense, then act. The line follower of Robotics Fundamentals 7 section 12 is one (line position in, turn rate out); so is a bumper that makes a robot back up. There is no model to get out of date, so reactive behaviours are fast (one sensor read and a little arithmetic), cheap, and robust against a changing world. Their limits are equally clear: with no memory they cannot count, remember where they have been, or pursue a goal that is out of sensor range, and two simple rules can chase each other in circles.

Braitenberg vehicles

The neuroscientist Valentino Braitenberg described a series of imaginary vehicles in which a few wires from sensors to wheels produce behaviour that looks purposeful. They are the cleanest picture of a reactive robot: the “program” is only the wiring. Take two light sensors (reading 0 to 1, larger meaning brighter) and two wheel motors, each wheel speed set by v = b + g × sensor, where b is a base speed and g a gain.

UNCROSSED (left sensor drives the left wheel, right sensor the right wheel): the brighter side speeds up ITS OWN wheel, so the vehicle turns AWAY from the light CROSSED (left sensor drives the right wheel, right sensor the left wheel): the brighter side speeds up the OPPOSITE wheel, so the vehicle turns TOWARD the light Light to the left: sL = 0.8, sR = 0.3. b = 0.05 m/s, g = 0.4 m/s per unit of reading, track width L = 0.15 m (differential drive of Robotics Fundamentals 4: w = ( vR - vL ) / L ) UNCROSSED: vL = 0.05 + 0.4 x 0.8 = 0.37 m/s vR = 0.05 + 0.4 x 0.3 = 0.17 m/s v = 0.27 m/s w = ( 0.17 - 0.37 ) / 0.15 = -1.333 rad/s (clockwise: turns right, away from the light) CROSSED: vL = 0.17 m/s vR = 0.37 m/s v = 0.27 m/s w = ( 0.37 - 0.17 ) / 0.15 = +1.333 rad/s (counter-clockwise: turns left, toward the light) A NEGATIVE gain (brighter means slower) makes the vehicle slow down as it nears the source and stop beside it, which is the "loves the light" version; the crossed, positive-gain version charges at it. Four wires, two sensors, no processor, no memory, and the vehicle already shows approach, avoidance and stopping.

Real robots use the same idea with ordinary distance sensors: two forward-angled ultrasonic or infrared sensors steer a robot away from whichever side has the nearer obstacle: the nearer the obstacle on the left, the faster the left wheel is driven, so the robot swings right (the “uncrossed” wiring, with a reading of nearness in place of brightness). The weakness is the same too: a reactive vehicle in a dead-end corridor sees the same left and right readings, turns neither way, and drives into the end. Something with a little memory or a little goal has to take over, which is what the rest of the chapter provides.

Arbitration: when behaviours disagree

A real robot has several reactive behaviours at once (follow the line, avoid obstacles, stop if the bumper is hit), and at any moment they may ask for different things. Rodney Brooks's subsumption architecture (1980s) organises them in layers of priority, where a higher layer can suppress (override) the output of those below it. The simplest practical form is priority arbitration: list the behaviours from the most to the least important, ask each in turn, and obey the first one that has something to say. Safety behaviours sit at the top; the pleasant, goal-seeking behaviours sit below; a default behaviour at the bottom always has an answer.

priority behaviour active when command it issues (v in m/s, w in rad/s, counter-clockwise positive) -------- ------------------ -------------------------- ------------------------------------------------------------ 1 STOP (bumper) the bumper is pressed (0, 0) 2 AVOID (front) front distance < 0.30 m v = 0 and turn toward the clearer side: w = +1.0 if left > right, else -1.0 3 LINE (array) a line is seen v = 0.18, w = Kp x e = 0.08 x ( -line position in mm ) (Robotics Fundamentals 7 section 12) 4 WANDER (nothing) always (0.15, 0) Asked in order from 1 to 4: the FIRST behaviour that has a command wins, and every behaviour below it is suppressed (it is not even asked).
def stop_b(s): return (0.0, 0.0) if s["bump"] else None def avoid_b(s): if s["front"] < 0.30: return (0.0, 1.0 if s["left"] > s["right"] else -1.0) return None def line_b(s): if s["line_x"] is None: return None # no line seen: nothing to say return (0.18, 0.08 * (-s["line_x"])) # e = 0 - x, in millimetres def wander_b(s): return (0.15, 0.0) BEHAVIOURS = [("STOP", stop_b), ("AVOID", avoid_b), ("LINE", line_b), ("WANDER", wander_b)] # highest priority first def arbitrate(s): for name, behaviour in BEHAVIOURS: command = behaviour(s) if command is not None: return name, command return "STOP", (0.0, 0.0) # if nothing answers, stand still
Four sensor snapshots, run through the rule above: bump front left right line x (mm) winner command (v, w) why True 0.10 0.9 0.4 +3 STOP (0.00, 0.00) the bumper outranks everything False 0.22 0.9 0.4 +5 AVOID (0.00, +1.00) 0.22 < 0.30, so AVOID beats the line; left (0.9) is clearer than right (0.4) False 0.80 0.9 0.4 -8 LINE (0.18, +0.64) e = +8 mm, w = 0.08 x 8 = 0.64 rad/s: the line is on the left, so turn left False 0.80 0.9 0.4 none WANDER (0.15, 0.00) nothing else has a command

The alternative to winner-takes-all is to blend the commands (a weighted average, or the vector sum used by potential fields in section 3). Blending gives smooth, graded behaviour, but averages can be worse than either input: if AVOID says turn left at +1.0 rad/s and a goal behaviour says turn right at −1.0 rad/s, the average is 0.0 and the robot drives straight into the obstacle between them. Use priorities for anything safety-related, and blending only among behaviours that are all fine with each other.

2. Finite state machines

Reactive rules have no memory. Many tasks do: I am cleaning, I have been told to go home, I am charging. A finite state machine (FSM) adds exactly the memory needed: the robot is always in exactly one of a small, named set of states, and each state says what the robot is doing (which behaviours are active, which targets are set). Events (a bump, a low battery, an arrival, a timer running out) cause transitions from one state to another. Everything the machine has to remember is the name of its current state, which makes it easy to test: for every state and every event, you can write down what happens, and a missing entry is a visible gap rather than a surprise.

A worked example: a vacuum robot

DOCKED --start--> CLEAN --battery_low--> RETURN --docked--> CHARGE --full--> DOCKED | ^ bump | | avoid_done v | AVOID --battery_low--> (RETURN) A timeout in AVOID, RETURN or CHARGE moves the machine to FAULT; FAULT --reset--> DOCKED
State What the robot does in this state Events it responds to Next state --------- ---------------------------------------------------- ---------------------------- ---------- DOCKED wheels stopped, waiting on the charger start CLEAN CLEAN cover the floor (zigzag), brushes on bump AVOID battery_low RETURN AVOID back up 0.1 m and turn 90 degrees avoid_done CLEAN battery_low RETURN timeout (5 s) FAULT RETURN drive to the dock (homing, section 3); a bump is docked CHARGE handled INSIDE this state and does not leave it timeout (120 s) FAULT CHARGE wheels stopped, charging full DOCKED timeout (8 hours) FAULT FAULT wheels stopped, lights flashing, error logged reset DOCKED Any (state, event) pair that is not in the table is ignored (for example, bump while in CHARGE). A table like this IS the program: it can be read, reviewed and tested row by row.
TABLE = { ("DOCKED", "start"): "CLEAN", ("CLEAN", "bump"): "AVOID", ("CLEAN", "battery_low"): "RETURN", ("AVOID", "avoid_done"): "CLEAN", ("AVOID", "battery_low"): "RETURN", ("AVOID", "timeout"): "FAULT", ("RETURN", "docked"): "CHARGE", ("RETURN", "timeout"): "FAULT", ("CHARGE", "full"): "DOCKED", ("CHARGE", "timeout"): "FAULT", ("FAULT", "reset"): "DOCKED", } TIMEOUT = {"AVOID": 5.0, "RETURN": 120.0, "CHARGE": 28800.0} # seconds allowed in each state class Machine: def __init__(self): self.state, self.entered = "DOCKED", 0.0 def handle(self, event, now): nxt = TABLE.get((self.state, event)) # no entry: the event is ignored if nxt is not None: print(f"{now:6.1f} s {self.state} --{event}--> {nxt}") # log every transition self.state, self.entered = nxt, now def tick(self, now): # call once per loop limit = TIMEOUT.get(self.state) if limit is not None and now - self.entered > limit: self.handle("timeout", now) # A short run: m = Machine(); m.handle("start", 0); m.handle("bump", 10); m.tick(16); m.handle("reset", 20) # 0.0 s DOCKED --start--> CLEAN 10.0 s CLEAN --bump--> AVOID # 16.0 s AVOID --timeout--> FAULT 20.0 s FAULT --reset--> DOCKED (6 s in AVOID is more than the 5 s allowed)

Debouncing a decision: timers and hysteresis

Events come from noisy sensors, and a machine that reacts to every flicker flips between states. Robotics Fundamentals 7 section 2 met this with bang-bang control; the cure is the same, with one more tool. Three standard defences, used together:

  • Require the condition to hold for a time (a debounce timer or a count of consecutive samples) before generating the event.
  • Use two thresholds (hysteresis): enter a condition at one level, leave it at a different, safer level, so a reading sitting right on the line cannot toggle the machine.
  • Add a minimum time in a state (a dwell time) so that a transition cannot immediately be reversed.
Battery charge (%), read 10 times a second, noisy by about +-2 around a true value of 19 or 20 that is slowly falling. Rule A (naive): battery_low when the reading is below 20. Rule B (debounced): battery_low only when 5 readings IN A ROW are below 20 (that is 0.5 s of agreement). Rule C: Rule B to enter, and the condition is cleared only when the reading is back at 25 or more. sample 0 1 2 3 4 5 6 7 8 9 10 11 12 13 time (s) 0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 1.1 1.2 1.3 reading (%) 21 19 21 20 19 21 19 18 19 18 19 18 18 17 count below 20 0 1 0 0 1 0 1 2 3 4 5 6 7 8 Rule A: the condition turns on at sample 1, off at 2, on at 4, off at 5, on at 6: 5 changes between 0.1 s and 0.6 s, and a machine wired to it would flip between CLEAN and RETURN five times. Rule B: first reaches a count of 5 at sample 10, t = 1.0 s: ONE event. Rule C: the same single event, and no further events until the charge reaches 25 or more. The price of debouncing is delay. Sizing it: battery: 0.5 s at a speed of 0.2 m/s is 0.1 m, which is irrelevant for a battery warning. a cliff (floor-edge) sensor: the same 0.5 s would carry the robot 0.1 m, which is over the edge. Use 3 samples at 5 ms = 15 ms, which is 3 mm at 0.2 m/s. So the debounce time is chosen per sensor, from what the consequences of a false trigger and of a late one each cost.

Avoiding state explosion

A state machine grows badly when it must track several independent things in one set of states. Suppose the robot has 4 drive modes, 3 battery conditions (ok, low, critical) and 2 payload conditions (loaded, empty). One machine that has to name every combination needs 4 × 3 × 2 = 24 states, and each new feature multiplies the count. Three small machines running side by side, one for each independent aspect, need only 4 + 3 + 2 = 9 states, and each is simple to read. A second tool is the hierarchical state machine: a state such as CLEAN contains its own sub-states (zigzag, edge cleaning, spiral), and a rule written once on the outer state (“battery_low leaves CLEAN”) covers all of them, instead of being repeated on each sub-state. The advice: one machine per independent concern, group states under a parent when they share transitions, and if the table starts to look like a plate of spaghetti, the design (not the drawing) needs another look.

3. Classic behaviours, with numbers

Line following

The controller is the PID of Robotics Fundamentals 7 section 12, fed with the weighted line position of Robotics Fundamentals 5 section 4; this section adds the decisions around it. With e = −x (x in mm, positive to the right) and u = Kp e a turn rate, wheel speeds follow from vR = v + uL/2 and vL = v − uL/2. Slowing on bends keeps the sensor array on the line: v = vmax / (1 + c |e|).

Robot: vmax = 0.30 m/s, c = 0.05 per mm, Kp = 0.08 rad/s per mm, track width L = 0.15 m. The array reports the line at x = -12 mm (to the LEFT of centre). e = -x = +12 mm u = 0.08 x 12 = +0.96 rad/s (counter-clockwise: turn left, toward the line) v = 0.30 / ( 1 + 0.05 x 12 ) = 0.30 / 1.6 = 0.1875 m/s vR = 0.1875 + 0.96 x 0.075 = 0.2595 m/s vL = 0.1875 - 0.96 x 0.075 = 0.1155 m/s (the right wheel faster: the robot swings left) Decisions around the controller: FOLLOW -- line lost for 0.3 s (3 readings of no line, debounced) --> SEARCH: spin toward the side where the line was LAST seen at w = 1.0 rad/s. SEARCH -- line found --> FOLLOW. SEARCH for more than 3 s ( 3 x 1.0 rad/s = 3 rad = 172 degrees of turning ) --> STOP and report "line lost". Last-seen side matters: a sharp corner makes the line leave the array on the side it turned to, so the memory of x(last) is the only clue. A junction (all sensors dark for a stretch) is a different EVENT, not a lost line, and needs its own rule (go straight, take the left branch, or stop).

Obstacle avoidance

The simplest avoider is a three-state machine: DRIVE, TURN, and a recovery state. Use a threshold with hysteresis: start turning when the front distance falls below 0.30 m, and only go back to driving when it has risen above 0.45 m (a 0.15 m band), so the robot does not twitch at the edge of the threshold. Choose the turn direction toward the side with more room (the left and right readings, as in section 1), and keep turning in that direction until the front is clear, rather than re-deciding every loop.

The threshold itself comes from a stopping-distance budget (section 7): at 0.5 m/s the robot needs about 0.10 m to stop, so 0.30 m leaves a margin of about three. The sensor must also see that far without a blind spot (Robotics Fundamentals 5: an ultrasonic sensor has a minimum range below which it reads nonsense).

Stuck and escape recovery. A reactive avoider can trap itself: a corner or the legs of a chair make it avoid left, then right, then left. Detect the loop and break it: if there are 3 avoid events in 10 seconds (a count with a timer, like the debounce above), enter ESCAPE: reverse 0.30 m (at 0.10 m/s, 3 s), turn through a larger, randomised angle (90 to 180 degrees), and then resume. If ESCAPE has happened 3 times without progress (position has not changed by more than 0.5 m, which needs odometry from Robotics Fundamentals 6), the robot has no more tricks: it stops and reports being stuck.

DRIVE --front < 0.30 m--> TURN --front > 0.45 m--> DRIVE DRIVE or TURN --3 avoids in 10 s--> ESCAPE (reverse 0.30 m, turn 90 to 180 degrees) --done--> DRIVE

Potential fields: a blended way to avoid

A potential field replaces if-then rules with forces. The goal pulls the robot with an attractive force; every obstacle within a range d0 pushes it away with a repulsive force that grows rapidly as the obstacle gets close. The robot steers along the sum. The usual repulsive force comes from a potential U = ½ k (1/d − 1/d0)2 for d < d0 (zero beyond), whose slope gives a force, pointing away from the obstacle, of magnitude:

F(rep) = k ( 1/d - 1/d0 ) / d^2 for d < d0, else 0 F(att) = k(a) x ( distance to the goal ), usually capped at some maximum Worked example (typical, illustrative): k = 0.01, d0 = 0.5 m, k(a) = 0.2. Robot at the origin facing +x (forward); y positive to the left. goal 2.0 m straight ahead: F(att) = 0.2 x 2.0 = 0.40 along +x obstacle A, 0.25 m ahead (at x = 0.25): F = 0.01 x ( 4 - 2 ) / 0.0625 = 0.01 x 32 = 0.32, pushing back along -x obstacle B, 0.40 m to the left (at y = +0.40): F = 0.01 x ( 2.5 - 2 ) / 0.16 = 0.01 x 3.125 = 0.03125, pushing to the right along -y Sum: Fx = 0.40 - 0.32 = 0.08 Fy = -0.03125 magnitude 0.086, direction atan2( -0.03125, 0.08 ) = -21.3 degrees (21.3 degrees to the RIGHT of straight ahead) With a heading controller of gain 1.5 per second: w = 1.5 x ( -21.3 degrees = -0.372 rad ) = -0.559 rad/s (clockwise: the robot veers right, between the two obstacles) Weakness: LOCAL MINIMA. A goal directly behind a wide wall: the pull forward and the push back cancel, and the robot sits still. Potential fields are a good local avoider, not a planner (section 4).

Wall following

Following a wall (to explore a room, or to find a door) is a position-style loop like the line follower: the error is the distance to the wall minus the wanted distance, and the output is a turn rate. With the wall on the right and a sensor looking at it, if the robot is too far (e = d − dref > 0) it must turn right (clockwise, negative), so u = −Kp e. A single sensor has a flaw: it cannot tell a robot that is close and parallel from one that is far and angled toward the wall (both give the same reading, but the second is about to hit it). With two sensors a distance s apart along the robot's side, the difference gives the angle: θ = atan((dF − dR)/s), and the perpendicular distance is dperp = ((dF + dR)/2) cos θ. A positive θ means the front is farther from the wall than the rear, so the robot is heading away from it.

Wall on the right, wanted distance 0.30 m, speed v = 0.20 m/s, track width L = 0.15 m. One sensor: d = 0.38 m, e = +0.08 m, Kp = 2.0 rad/s per m: u = -2.0 x 0.08 = -0.16 rad/s vR = 0.20 + ( -0.16 ) x 0.075 = 0.188, vL = 0.20 - ( -0.16 ) x 0.075 = 0.212 m/s (turns right, toward the wall) Two sensors 0.10 m apart: dF = 0.40 m (front), dR = 0.34 m (rear): theta = atan( ( 0.40 - 0.34 ) / 0.10 ) = atan( 0.6 ) = 30.96 degrees = 0.5404 rad (the front is farther: heading AWAY from the wall) d(perp) = ( ( 0.40 + 0.34 ) / 2 ) x cos( 30.96 degrees ) = 0.37 x 0.857 = 0.3173 m error = 0.3173 - 0.30 = +0.0173 m (the distance is almost right, but the angle is wrong) u = -( Kp x error + Ktheta x theta ) = -( 2.0 x 0.0173 + 1.5 x 0.5404 ) = -( 0.035 + 0.811 ) = -0.845 rad/s (a firm right turn, toward the wall) vR = 0.20 - 0.845 x 0.075 = 0.137 m/s, vL = 0.20 + 0.845 x 0.075 = 0.263 m/s (a single sensor reading nearly the right distance would have asked for only about -0.035 rad/s, and the robot would have drifted off) Corners: an INSIDE corner (wall ahead): the front sensor sees it, so give the obstacle behaviour priority and turn left (away) by 90 degrees. an OUTSIDE corner (the wall ends): the reading jumps up suddenly; cap the correction at about +-1 rad/s and curve gently toward where the wall was.

Homing and docking

To return to a charger, a robot needs something to find: an infrared beacon on the dock that gives the bearing β (angle to the dock, counter-clockwise positive) and a rough range r. The rule is the line follower once more: turn in proportion to the bearing, w = Kb β, and slow down as the range shrinks, v = min(vmax, kv r), so the last centimetres are covered gently enough for the contacts to meet.

Kb = 1.5 per second, vmax = 0.25 m/s, kv = 0.5 per second. The beacon is 20 degrees to the left, 1.2 m away. w = 1.5 x ( 20 degrees = 0.349 rad ) = 0.524 rad/s (counter-clockwise: turn left) v = min( 0.25, 0.5 x 1.2 = 0.60 ) = 0.25 m/s At range 0.30 m: v = min( 0.25, 0.15 ) = 0.15 m/s. At range 0.10 m: v = 0.05 m/s. Stop on contact (charging current detected) or at r < 0.05 m. The last metre is made forgiving mechanically: a funnel-shaped dock guides a robot that arrives a few centimetres off-line. If docking fails (timeout in RETURN, or contact not made), back off 0.5 m and try again, at most 3 times, then FAULT (the table above): a rule of the form "try, check, retry a limited number of times, then report" belongs in every behaviour.

4. Simple planning: grid maps and finding a path

Reactive behaviours and state machines choose what to do now. To choose how to get there when the goal is out of sight (the kitchen is down the corridor, and past the table) the robot needs a map and a planner that searches the map for a route.

Grid maps and occupancy grids

The simplest map is a grid: the floor is cut into squares of a chosen size, and each cell is free, blocked, or (before the robot has looked) unknown. World coordinates become cell indices by division: column = floor(x / cell), row = floor(y / cell). With cells of 0.25 m, the point (x = 1.30 m, y = 0.60 m) lies in column floor(5.2) = 5, row floor(2.4) = 2. The cell size is a trade-off between detail and memory: a 20 m by 20 m floor is 80 × 80 = 6,400 cells at 0.25 m but 400 × 400 = 160,000 cells at 0.05 m (at one byte per cell, 160 kB; it also takes 25 times as many cells for a planner to search). To stop the planner from steering the robot's centre next to a wall, inflate each obstacle by the robot's radius (0.15 m is 0.6 of a 0.25 m cell, so one extra cell of margin round every blocked cell), which lets the planner treat the robot as a point.

Sensors are noisy and a cell is rarely certain, so a map built from measurements stores a probability of occupancy in each cell (an occupancy grid). Every cell starts at 0.5, “don't know”. Each time a range sensor beam ends in a cell (a hit), that cell becomes more likely to be occupied; each cell the beam passes through on the way becomes more likely to be free. Updating the probabilities directly means a small multiplication-and-division for every reading. The neat trick is to store log-odds instead, where each new reading just adds a number.

odds = p / ( 1 - p ) log-odds l = ln( odds ) back to a probability: p = 1 / ( 1 + e^(-l) ) (p = 0.5 is l = 0: no information) Sensor model (typical, illustrative): a hit says "occupied" with 70 % confidence, a pass-through says "free" with 70 %. l(hit) = ln( 0.7 / 0.3 ) = +0.847 l(free) = ln( 0.3 / 0.7 ) = -0.847 Update for every reading: l = l + l(reading) (the prior l = 0 adds nothing) One cell, four readings in turn: hit, hit, free, hit reading hit hit free hit l 0.847 1.695 0.847 1.695 p 0.700 0.845 0.700 0.845 (net: three hits and one free = two net hits, 2 x 0.847 = 1.695) Check with Bayes directly on the first two hits: 0.7 x 0.7 / ( 0.7 x 0.7 + 0.3 x 0.3 ) = 0.49 / 0.58 = 0.845 (the same) A cell a beam passes through three times without a hit: l = -3 x 0.847 = -2.542, p = 1 / ( 1 + e^2.542 ) = 0.073 (very probably free) Clamp l to +-4 (p between 0.018 and 0.982): otherwise a cell seen as a wall for 20 readings (l = 16.9) would need about 20 contradicting readings to change its mind, which is far too slow after a chair has been moved.

Searching the grid: breadth-first search

A graph search treats each free cell as a node, joined to its 4 neighbours (up, right, down, left) by edges of cost 1. Breadth-first search (BFS) explores outward from the start in rings: first the cells 1 move away, then those 2 moves away, and so on, using a first-in, first-out queue. The first time it reaches the goal it has used the fewest moves, so for equal-cost moves it always finds a shortest path. Its weakness is that it explores in all directions, including the wrong ones. The example grid below is used for BFS and then for A*; S is the start, G the goal, # a blocked cell, and rows and columns are numbered from 0 at the top left. A cell is written (row, column).

c0 c1 c2 c3 c4 c5 Start S = (2,0). Goal G = (1,5). One wall in column 3 with a single gap at row 3. r0 . . . ### . . Each cell is 0.25 m. Moves: up, right, down, left (the order used by both searches). r1 . . . ### . G r2 S . . ### . . r3 . . . . . . r4 . . . ### . . BFS: the number of moves from S to each cell, found by the rings of the search (the goal is reached in 8 moves): c0 c1 c2 c3 c4 c5 r0 2 3 4 ### 8 9 r1 1 2 3 ### 7 [8] G r2 [0] 1 2 ### 6 7 S r3 1 2 3 4 5 6 r4 2 3 4 ### 6 7 BFS takes cells off its queue in order of distance and stops when G comes off the queue: 25 of the 26 free cells were taken off (only (0,5), at distance 9, was never reached). The shortest path has 8 moves, one of several: (2,0) (2,1) (2,2) (3,2) (3,3) (3,4) (2,4) (1,4) (1,5). Cost 8 moves x 0.25 m = 2.0 m, about 10 s at 0.20 m/s.

A*: searching with a guess

A* (“A star”) keeps the shortest-path guarantee but explores mainly toward the goal. For each cell it tracks g, the cost of the best path found so far from the start, and a heuristic h, an estimate of the remaining cost to the goal. It always expands the open cell with the smallest f = g + h. On a 4-connected grid the standard heuristic is the Manhattan distance h = |row − rowgoal| + |col − colgoal| (the number of moves if there were no walls). It never overestimates the true remaining cost (admissible, and consistent), which is what guarantees that the first path found is a shortest one. With h = 0 A* becomes Dijkstra's algorithm (BFS with unequal costs); the larger the heuristic stays below the truth, the more the search is guided. If diagonal moves are allowed (cost 1.414), use the octile distance instead, or Manhattan will overestimate.

The rules used below, so that the hand-work is repeatable: pick the open cell with the lowest f; if tied, the lowest h (closer to the goal); if still tied, the one added to the open list first. For each neighbour (in the order up, right, down, left) that is not a wall: new g = g(current) + 1, and if that is better than the neighbour's recorded g (or it has none), record it and add it to the open list with its f. Goal G = (1,5), so h(r,c) = |r − 1| + |c − 5|, and h(S) = 1 + 5 = 6.

# expand g h f neighbours added or improved (cell: g, h, f) 1 (2,0) 0 6 6 (1,0): 1,5,6 (2,1): 1,5,6 (3,0): 1,7,8 2 (1,0) 1 5 6 (0,0): 2,6,8 (1,1): 2,4,6 (tie f = 6 with (2,1): (1,1) has the lower h, 4, so it goes next) 3 (1,1) 2 4 6 (0,1): 3,5,8 (1,2): 3,3,6 4 (1,2) 3 3 6 (0,2): 4,4,8 (2,2): 4,4,8 (to the right is the wall) 5 (2,1) 1 5 6 (2,2): 2,4,6 (improved from g = 4 to 2) (3,1): 2,6,8 6 (2,2) 2 4 6 (3,2): 3,5,8 (the last cell with f = 6: the wall makes every route longer than the straight-line 6) 7 (0,2) 4 4 8 none (f jumps to 8: among f = 8, the lowest h is 4 ) 8 (0,1) 3 5 8 none (h = 5 ties with (3,2); (0,1) was added first) 9 (3,2) 3 5 8 (3,3): 4,4,8 (4,2): 4,6,10 10 (3,3) 4 4 8 (3,4): 5,3,8 (through the gap in the wall) 11 (3,4) 5 3 8 (2,4): 6,2,8 (3,5): 6,2,8 (4,4): 6,4,10 12 (2,4) 6 2 8 (1,4): 7,1,8 (2,5): 7,1,8 13 (1,4) 7 1 8 (0,4): 8,2,10 (1,5): 8,0,8 14 (1,5) 8 0 8 GOAL reached, path cost g = 8 Order of expansion on the grid (. = never expanded; 14 cells expanded, against 25 for BFS): c0 c1 c2 c3 c4 c5 r0 . 8 7 ### . . r1 2 3 4 ### 13 14 (G) r2 1 5 6 ### 12 . (1 is S) r3 . . 9 10 11 . r4 . . . ### . . Path back from G by following the recorded parents: (1,5) <- (1,4) <- (2,4) <- (3,4) <- (3,3) <- (3,2) <- (2,2) <- (2,1) <- (2,0). Cost 8 moves = 2.0 m. Notice that f stayed at 6 (the straight-line guess) while the search ran toward the wall, then rose to 8 once it had to go round: the true cost is the Manhattan guess plus 2, the detour through the gap.
from heapq import heappush, heappop def astar(grid, start, goal): # grid: list of strings, "#" = blocked; cells are (row, col) def h(cell): return abs(cell[0] - goal[0]) + abs(cell[1] - goal[1]) # Manhattan distance def neighbours(cell): for dr, dc in ((-1, 0), (0, 1), (1, 0), (0, -1)): # up, right, down, left r, c = cell[0] + dr, cell[1] + dc if 0 <= r < len(grid) and 0 <= c < len(grid[0]) and grid[r][c] != "#": yield (r, c) g = {start: 0} parent = {start: None} closed = set() added = 0 # counts insertions, to break ties: first in, first out open_list = [(h(start), h(start), 0, start)] # (f, h, order, cell): the heap pops the lowest f, then h, then order while open_list: f, hh, order, cell = heappop(open_list) if cell in closed: continue # an out-of-date entry for a cell already expanded closed.add(cell) if cell == goal: path = [] while cell is not None: path.append(cell) cell = parent[cell] return path[::-1], g[goal] for nxt in neighbours(cell): new_g = g[cell] + 1 if nxt not in g or new_g < g[nxt]: g[nxt], parent[nxt] = new_g, cell added += 1 heappush(open_list, (new_g + h(nxt), h(nxt), added, nxt)) return None, None # open list empty: the goal cannot be reached

Practical notes. Path cost need not be distance only: a planner can charge more for cells near walls (inflation zones), for turns (many planners add a small penalty per change of direction, which would favour the straight run along row 3 over the staircase above), or for rough floor; A* simply minimises the total. Replanning: when the robot finds a new obstacle on the path (a chair that was not on the map), mark the cell blocked and search again from the robot's current cell, which on a small grid takes a fraction of a second; incremental methods (D* and its relatives) reuse earlier work. Path following: the planner gives a list of cells; turning it into motion is the job of the controllers of Robotics Fundamentals 7 (steer toward the next waypoint with a heading controller, speed limited near obstacles).

5. Localisation

A map is only useful if the robot knows where it is on it. Robotics Fundamentals 6 showed that dead reckoning drifts and that an absolute fix pulls the error back. Localisation is the art of combining the two: the robot keeps a belief, a probability distribution over where it might be (“probably near (2.1, 1.0), within about 0.2 m”), and repeats two steps. Predict: as the wheels turn, move the belief along with the odometry, and make it wider, because every movement adds uncertainty. Update: when a sensor reading arrives (a distance to a wall, a beacon range, a landmark), keep the possible positions that agree with it and discard those that do not, making the belief narrower. The Kalman filter of Robotics Fundamentals 6 section 5 is this loop for a single bell-curve belief. When the belief is not a single bump (the robot could be in either of two identical corridors), something more flexible is needed.

The particle filter, conceptually

A particle filter represents the belief as a cloud of guesses (particles), each a complete possible pose, often several hundred to thousands (a typical range, depending on the map). In each loop: 1. Move every particle by the odometry reading plus some random noise, so the cloud spreads like the uncertainty. 2. Weight each particle by how well the real sensor reading matches what that particle would read from its own pose on the map; a particle that agrees gets a high weight, one that disagrees a weight near zero. 3. Resample: draw a new cloud, picking particles in proportion to their weights, so that good guesses are copied and bad ones die out. The cloud collapses onto the poses that fit the evidence, and the robot's best estimate is the cloud's average (if it is one group). A cloud that starts spread evenly over the whole map can find the robot's place without being told, which dead reckoning cannot do (“the kidnapped robot” problem).

A robot on a corridor, 1-D for clarity. A wall is at x = 5.0 m; a range sensor reads the distance to it. The robot's real position is x = 2.1 m (unknown to it), so it measures z = 2.9 m. Five particles, equally weighted. Weight rule: w = exp( -( z - predicted range )^2 / ( 2 sigma^2 ) ), with sensor noise sigma = 0.3 m (typical). particle x predicted range 5.0 - x z - predicted weight w normalised expected copies in a new cloud of 5 1.0 4.0 -1.1 0.0012 0.001 0.00 1.5 3.5 -0.6 0.1353 0.090 0.45 2.0 3.0 -0.1 0.9460 0.629 3.14 2.5 2.5 +0.4 0.4111 0.273 1.37 3.0 2.0 +0.9 0.0111 0.007 0.04 sum of weights = 1.5047; normalised = each w / 1.5047; expected copies = 5 x normalised Weighted mean position = 1.0 x 0.001 + 1.5 x 0.090 + 2.0 x 0.629 + 2.5 x 0.273 + 3.0 x 0.007 = 2.098 m (the truth is 2.1 m) A typical resampled cloud: 2.0, 2.0, 2.0, 2.5, 1.5 (random: the numbers of copies fluctuate around the expected values). After the next move and reading, it narrows further. Cost: the work grows with the number of particles; too few and the right answer may not be present in the cloud at all. Real implementations add a few random particles to recover from being wrong.

Trilateration from beacons

With range beacons at known places (Robotics Fundamentals 6 section 8) the robot can compute a position directly. Each measured range di puts the robot on a circle around beacon i. Two circles cross at two points; the third circle picks one. Subtracting the circle equations pairwise cancels the squared terms and leaves two straight-line equations. For beacon A at the origin, B on the x axis at (b, 0) and C on the y axis at (0, c), the solution has a closed form:

Circles: x^2 + y^2 = dA^2 ( x - b )^2 + y^2 = dB^2 x^2 + ( y - c )^2 = dC^2 Subtract the first from each of the others: x = ( dA^2 - dB^2 + b^2 ) / ( 2 b ) y = ( dA^2 - dC^2 + c^2 ) / ( 2 c ) Worked example: A = (0, 0), B = (4, 0), C = (0, 3) metres. The robot is really at (1.5, 2.0), so the true ranges are 2.500, 3.202 and 1.803 m; it measures dA = 2.50, dB = 3.20, dC = 1.80 m. x = ( 2.50^2 - 3.20^2 + 4^2 ) / ( 2 x 4 ) = ( 6.25 - 10.24 + 16 ) / 8 = 12.01 / 8 = 1.501 m y = ( 2.50^2 - 1.80^2 + 3^2 ) / ( 2 x 3 ) = ( 6.25 - 3.24 + 9 ) / 6 = 12.01 / 6 = 2.002 m position (1.501, 2.002): within 2 mm of (1.5, 2.0) Sensitivity: add 0.10 m of error to dB alone (3.30): x = ( 6.25 - 10.89 + 16 ) / 8 = 1.420 m, y unchanged: an error of 0.080 m in position. add 0.10 m of error to dA alone (2.60): x = 1.565 m, y = 2.087 m: an error of 0.108 m. A range error of 0.1 m gives roughly a position error of 0.1 m (here between 0.08 and 0.11): typical of the geometry; it gets WORSE when the robot is outside the beacons or the beacons lie almost in a line. With more than three beacons, find the position that minimises the sum of squared range errors (least squares): the extra ranges average the noise down (Robotics Fundamentals 5 section 9). With only two beacons, or three in a straight line, there are two answers, a mirror pair; the third beacon must be off the line of the other two.

SLAM, in concept only

SLAM (simultaneous localisation and mapping) is the problem of building a map and locating the robot on it at the same time, with neither given in advance. It is a chicken-and-egg problem: to add what the sensor sees to the map, you need to know where the robot is; to know where the robot is, you need the map. The solution, in outline: the robot moves and measures, estimates its pose from odometry, matches each new scan against the map so far (or against landmarks it has seen before), and corrects both the map and its pose estimate together. Its hardest moment is loop closure: coming back to a place already seen, recognising it, and pulling the whole drifted trail back into line. Common forms include filters (Kalman, particle) and graph SLAM, which keeps all the poses and measurements as a network of constraints and finds the arrangement that fits them best. SLAM with a lidar is what many floor robots use; it is implemented in ready-made software packages, and building one from scratch is far beyond this course. What matters here is why it exists and what it needs: good range data, a sensible odometry, and a world that holds still.

6. Behaviour trees, briefly

A state machine says where the robot is and what causes it to move on. A behaviour tree says what it should try. The tree is run (“ticked”) from the top at every loop; each node returns success, failure or running. A Selector (fallback) node tries its children left to right and stops at the first that does not fail; a Sequence node runs its children in order and stops at the first that does not succeed; the leaves are conditions (a sensor test) and actions.

ROOT: Selector (try children left to right; stop at the first that succeeds or is still running) |-- Sequence "recharge" | |-- Condition: battery < 20 % | `-- Action: go to the dock |-- Sequence "avoid" | |-- Condition: front distance < 0.30 m | `-- Action: turn away `-- Action: clean the floor (the default: it only gets a turn if the two above did not apply)
Finite state machine Behaviour tree -------------------------------------------------------- -------------------------------------------------------- Natural for modes with a clear sequence (docked, cleaning, Natural for prioritised alternatives and reusable pieces ("try charging) and for exactly-specified transitions this, else that") that are re-evaluated every tick Transitions are explicit arrows: easy to check, but many Adding a behaviour means adding a branch; the tree does not arrows as the machine grows (state explosion) need a new arrow to every other state Remembers its state between ticks Re-checks conditions from the top each tick (reactive), unless a node returns "running" Best for small robots, hardware-level safety logic Best for larger sets of behaviours, as in games and big robots

Neither is better in general, and they are often combined: a state machine for the robot's operating modes, a behaviour tree (or the priority list of section 1) inside the mode that needs many alternatives. Behaviour trees are named here so that you recognise them; building one is a topic for Programming for Robotics or a software course.

7. Testing and failure handling

A robot's behaviour layer is the part most likely to meet situations nobody planned for, and the part where a mistake moves a heavy object near people. Four habits turn “it worked once” into “it fails safely”.

  • Timeouts. Every state that waits for something (an arrival, a dock, a door) gets a maximum time, after which the machine takes a defined exit (the table of section 2: AVOID 5 s, RETURN 120 s, CHARGE 8 hours). Without them a robot can wait forever in a state that no event will ever end.
  • A watchdog. A watchdog timer is a countdown that the main program must keep resetting ("kicking") while it runs healthily; if the program freezes or crashes, the countdown reaches zero and hardware (not the frozen software) cuts the motor power. A typical choice is 5 times the loop period (100 ms for a 20 ms loop).
  • Safe stop. Define what “safe” means for each failure: usually motors off and brakes on, but an arm that holds a load or a robot on a slope may need to be held, not released (Robotics Fundamentals 10). The safe state must also be the state the hardware falls into when power or signal is lost, not only a state that software can choose.
  • Logging. Write down every state transition with its time and its cause, and the key sensor values at a fixed rate. When the robot does something strange, the log is the only witness. At 20 records per second of 40 bytes, that is 800 B/s or 2.9 MB per hour, easily stored on a card. A good log lets you replay a failure in simulation.
Stopping distance budget (typical, illustrative values): speed v = 0.5 m/s, braking deceleration a = 2.0 m/s^2. Reaction: sensor period 50 ms + processing 20 ms + motor command 10 ms = 80 ms -> 0.5 x 0.08 = 0.040 m travelled before braking even starts Braking: v^2 / ( 2 a ) = 0.25 / 4 = 0.0625 m Total: 0.040 + 0.0625 = 0.1025 m. With a safety margin of 2, the stop threshold must be at least 0.205 m, so the 0.30 m used in section 3 has a margin of 0.30 / 0.1025 = 2.9. At 1.0 m/s: 0.08 + 1.0 / 4 = 0.33 m, MORE than the 0.30 m threshold: the robot cannot stop in time. Either the threshold goes up or the speed comes down. The largest speed that stops in 0.30 m is about 0.95 m/s, with NO margin: v x 0.08 + v^2 / 4 = 0.30. Watchdog: the program freezes while the robot is moving at 0.5 m/s. With a 100 ms watchdog the robot goes on for 0.5 x 0.1 = 0.05 m before power is cut, and then needs 0.0625 m more to stop: 0.1125 m. Allow for it in the clearance that you leave around people and walls. Habits: test each behaviour alone, with wheels off the ground; then in a simulator; then on the floor at low speed with a hand on the stop switch; replay logged sensor data through the state machine and check each transition against the table.

8. Challenges

Challenge 1: Reactive behaviours, arbitration and state machines
(a) A Braitenberg vehicle has sensors reading sL = 0.2 and sR = 0.7 (the light is on the right), b = 0.1 m/s, g = 0.5 m/s per unit and track width L = 0.12 m. Find both wheel speeds, the forward speed and the turn rate for the uncrossed and the crossed wiring, and say which way each turns. (b) Using the four behaviours of section 1 (STOP, AVOID, LINE, WANDER), give the winner and the command for these snapshots: (i) no bump, front 0.25, left 0.3, right 0.8, line at +4 mm; (ii) no bump, front 0.9, line at −10 mm; (iii) no bump, front 0.9, no line. (c) A delivery robot has the states WAIT, TO_PICKUP, LOADING, TO_DROP, UNLOADING, HOLD_PICKUP and HOLD_DROP, and the events order, arrived, loaded, unloaded, blocked and clear. Write its transition table (WAIT goes to TO_PICKUP on order; arriving moves on in turn; blocked on either trip goes to the matching HOLD state, and clear goes back to that trip), then trace the events order, arrived, loaded, blocked, clear, arrived, unloaded and give every state visited. Explain why two HOLD states were needed. (d) A battery reading is sampled every 0.2 s: 24, 19, 22, 19, 18, 19, 19, 18, 19, 19. battery_low needs 4 readings in a row below 20. At which sample, and at what time, does it fire, against the naive rule (the first reading below 20)? (e) A robot has 5 drive modes, 3 battery conditions, 2 payload conditions and 2 communications conditions. How many states does a single machine need, and how many do four separate machines need?
View solution
Challenge 2: Line following, obstacle avoidance, wall following and homing
(a) A line follower has vmax = 0.40 m/s, c = 0.04 per mm, Kp = 0.06 rad/s per mm, L = 0.18 m, and the line is at x = +15 mm. Find e, the turn rate, the speed, both wheel speeds, and the direction of turn. If the line is then lost and the robot spins toward the last-seen side at 1.0 rad/s for the 3 s limit, how many degrees does it turn, and which way? (b) A potential field with k = 0.02, d0 = 0.60 m and ka = 0.30: the goal is 1.5 m straight ahead, obstacle A is 0.30 m straight ahead, obstacle B is 0.45 m to the right. Find the three forces, their sum, its size and direction, and say which way the robot veers. (c) Wall following, wall on the right, wanted distance 0.35 m, two sensors 0.12 m apart reading dF = 0.28 m and dR = 0.36 m, with Kp = 2.5 and Ktheta = 1.2, v = 0.20 m/s, L = 0.14 m. Find theta (and which way the robot is heading relative to the wall), the perpendicular distance, the error, the turn rate command, and both wheel speeds. (d) Homing with Kb = 1.2 per second, vmax = 0.20 m/s, kv = 0.4 per second: the beacon is 35 degrees to the right at 0.8 m. Find w and v, and the speed at a range of 0.2 m.
View solution
Challenge 3: Occupancy grids, planning, localisation and stopping safely
(a) An occupancy grid uses a sensor model of 80 % confidence (a hit means occupied with p = 0.8, a pass-through free with p = 0.8). Find the log-odds step, and the log-odds and probability of one cell after the readings hit, hit, free, hit, hit. Then after 4 pass-throughs from the prior of 0.5. What does clamping at ±4 give as the probability limits? (b) On the grid with rows S . # . . / . . # . G / . . . # . / # . . . . (S at (0,0), G at (1,4), # blocked, cells 0.2 m), use moves in the order up, right, down, left. Find the Manhattan distance from S to G, then by BFS the number of moves to G and the distance written in every reached cell, and then run A* with the tie rules of section 4 (lowest f, then lowest h, then first added): list the expansions with g, h, f, the path found and its length in metres, and compare the number of expansions with BFS. (c) Beacons A = (0, 0), B = (5, 0), C = (0, 4) measure dA = 3.20, dB = 3.90 and dC = 2.50 m. Find the position and check it against the true position (2.0, 2.5). (d) A wall is at x = 6.0 m and the range reading is z = 3.6 m with sigma = 0.5 m. Particles sit at x = 1, 2, 3, 4 and 5 m. Find each predicted range, weight and normalised weight, the expected copies in a cloud of 5, and the weighted mean position. (e) A robot at 0.8 m/s with deceleration 2.5 m/s^2 and a total reaction time of 60 ms: find the stopping distance, the stop threshold with a margin of 2, and the largest speed at which a 0.30 m threshold still leaves a margin of 2.
View solution

Quick reference

  • Reactive behaviour: sensors straight to motors, no map and no memory; fast and robust, but it cannot plan or remember. Braitenberg: wheel speed = b + g × sensor; crossed wiring turns toward the stimulus, uncrossed turns away.
  • Arbitration: priority list (first behaviour with a command wins; safety first, default last) for anything that matters; blending only among behaviours that agree, because an average of two avoids can steer straight at the obstacle.
  • State machine: states, events, a transition table (the table is the program; unlisted pairs are ignored); log every transition; give waiting states a timeout.
  • Debounce: require the condition for a time or a count (5 samples at 0.1 s), use hysteresis (enter at 20 %, leave at 25 %) and a dwell time; size the delay to the cost of a late reaction (battery 0.5 s, cliff 15 ms).
  • State explosion: independent aspects multiply (4 × 3 × 2 = 24) when put in one machine, but add (4 + 3 + 2 = 9) as separate machines; use hierarchy for shared transitions.
  • Line following: e = −x, u = Kp e, vR,L = v ± uL/2, v = vmax/(1 + c|e|); add lost-line search toward the last-seen side with a time limit.
  • Obstacle avoidance: stop at 0.30 m, resume above 0.45 m (hysteresis), turn toward the clearer side, 3 avoids in 10 s means ESCAPE. Potential field: Frep = k(1/d − 1/d0)/d2 for d < d0; sum with the goal pull; beware local minima.
  • Wall following: u = −(Kp(dperp − dref) + Kθθ) for a wall on the right; θ = atan((dF − dR)/s), dperp = ((dF + dR)/2) cos θ. Homing: w = Kbβ, v = min(vmax, kv r); retry a limited number of times.
  • Occupancy grid: store log-odds; l = l + l(reading); p = 1/(1 + e−l); l(hit) = ln(0.7/0.3) = +0.847 for a 70 % model; clamp at ±4. Column = floor(x/cell), row = floor(y/cell); inflate obstacles by the robot radius.
  • BFS finds a shortest path (equal costs) but explores everywhere (25 cells in the example). A* expands the lowest f = g + h with the Manhattan h = |Δrow| + |Δcol| (admissible), and found the same 8-move, 2.0 m path with 14 expansions.
  • Localisation: belief = predict (wider) + update (narrower). Particle filter: move, weight by w = exp(−(z − zpred)2/2σ2), resample. Trilateration: x = (dA2 − dB2 + b2)/2b, y = (dA2 − dC2 + c2)/2c. SLAM builds the map and the pose together; loop closure fixes the drift.
  • Behaviour tree: selector (first child that does not fail), sequence (all must succeed), re-evaluated every tick; state machines suit modes, trees suit prioritised alternatives.
  • Failure handling: timeouts, a hardware watchdog (about 5 times the loop period), a defined safe stop, and a log of every transition. Stopping distance = v × latency + v2/2a; keep a margin of 2 or more.

Next, Robotics Fundamentals 9 moves from a robot that drives to a robot that reaches: arms and manipulators. Joints and links, forward kinematics (where is the hand, given the joint angles?) and inverse kinematics (what joint angles put the hand there?) worked out by hand for a two-link arm, grippers, and the torque a joint must supply to hold a payload at arm's length, using the motor sizing of Robotics Fundamentals 3 and the position control and feedforward of Robotics Fundamentals 7.