Perception, planning and AI

1.1 Depth from two cameras

Two cameras separated by a baseline B see the same point with a horizontal shift, the disparity d (in pixels). With the focal length f in pixels, the depth is Z = f B / d. The error in depth for a disparity error Δd is:

ΔZ ≈ Z² Δd / (f B)

1.2 Choosing sensors

Key terms

• lidar
• stereo
• RGB-D
• baseline
• disparity
• field of view
• sensor fusion
• GNSS

2.1 The pinhole camera

A point of the world (X, Y, Z) in the camera frame (Z along the viewing direction) lands on the pixel:

u = f_x X / Z + c_x, v = f_y Y / Z + c_y

f_x, f_y (focal lengths in pixels) and (c_x, c_y) (optical centre) are the intrinsic parameters, with the lens distortion. One pixel at distance Z covers Z/f metres. For f = 800 px and Z = 1 m, a point 0.1 m to the right of the axis lands 80 px from the centre, and one pixel is 1.25 mm.

2.2 Calibration

• Intrinsic calibration: film a checkerboard in many poses; the software finds f, c and the distortion.
• Hand-eye calibration: where is the camera with respect to the robot? Eye-in-hand (camera on the flange): find the fixed transform flange → camera. Eye-to-hand (camera fixed in the cell): find base → camera. The robot sees a marker from several poses and the equation A X = X B is solved for the unknown transform X. Without it, a perfect detection gives a missed grasp.

Both use the homogeneous matrices of the modelling course: the object pose in the base frame = ^baseT_flange · ^flangeT_camera · ^cameraT_object.

2.3 From pixels to objects

2.4 Visual servoing

Instead of “see, compute the pose, move blindly”, visual servoing moves the robot while looking, so the error of calibration does not matter. The error e is computed in the image and driven to zero with the camera velocity v_c:

v_c = −λ L⁺ e

where L is the “interaction matrix” linking the image features to the camera velocity (it is also a Jacobian, and its pseudo-inverse is the same tool as in the course on singularities). Two families: IBVS (error measured in the image, robust to calibration, but the camera path can be odd) and PBVS (error in the 3D pose, straight camera path, but sensitive to calibration).

Key terms

• pinhole model
• intrinsic parameters
• hand-eye calibration
• ArUco / AprilTag
• ICP
• object detection
• visual servoing

3.1 The idea

A robot never knows its state exactly. It has a prediction (from the model and the commands: “I drove 1 m forward”) which drifts, and a measurement (from a sensor: “the lidar says I am at 0.9 m”) which is noisy. A filter combines them, trusting each according to its uncertainty (its variance).

3.2 The one-dimensional Kalman filter

1. Predict: x ← x + u (the motion), and the uncertainty grows: P ← P + Q (process noise Q).
2. Gain: K = P / (P + R) (measurement noise R).
3. Update: x ← x + K (z − x), P ← (1 − K) P.

3.3 The family

Key terms

• prediction / update
• variance
• Kalman gain
• EKF
• particle filter
• complementary filter
• dead reckoning

4.1 Odometry and its drift

A differential-drive robot (see the mobile robot page) integrates the wheel increments to update its pose (x, y, θ). Every small error accumulates and never goes away:

• A distance error of 1 % means 1 m of error after 100 m.
• A heading error of 1° gives a sideways error of 50 m × tan 1° ≈ 0.87 m after 50 m. Heading errors are the worst, because they rotate the whole future path.
• Wheel slip, uneven floors and wheel-diameter errors cause these errors. See the wheels page.

4.2 Maps

Occupancy grid update: each laser beam says “free” for the cells it crosses and “occupied” for the cell where it stops. The probabilities are combined in the log-odds form, where an observation is simply added: l ← l + l_obs. Repeated observations make a cell confidently free or occupied, and noise averages out.

4.3 SLAM: both at once

To localise, the robot needs a map; to build a map, it needs to know where it is. SLAM (Simultaneous Localisation And Mapping) solves both together:

1. Move and predict the new pose with the odometry (which drifts).
2. Scan matching: align the new lidar scan on the map (ICP or correlation) to correct the pose.
3. Add the scan to the map.
4. Loop closure: when the robot recognises a place seen before, add a constraint between the two poses and optimise the whole trajectory (pose-graph optimisation): the map snaps into shape and the drift disappears.

Key terms

• odometry
• occupancy grid
• SLAM
• scan matching
• loop closure
• pose graph
• AMCL
• kidnapped robot

Odometry on the mobile robot page →

5.1 Planning as graph search

Cut the map into cells: each free cell is a node, each move to a neighbour is an edge with a cost (1 for a straight move, 1.41 for a diagonal one). Finding a path becomes finding the cheapest route in a graph.

• Dijkstra: expands the nodes in order of their cost from the start g. It always finds the shortest path, but explores in all directions, like a circle of water spreading.
• A*: expands the nodes in order of f = g + h, where h is a heuristic estimate of the remaining cost to the goal (for example the straight-line distance). If h never over-estimates (it is admissible), A* still finds the shortest path, but explores far fewer nodes: it is drawn towards the goal.
• Greedy best-first (f = h only): very fast, but not optimal.

5.2 Costmaps and real maps

• Costmap: instead of free/occupied, each cell has a cost that rises close to obstacles (an “inflation” layer of one robot radius plus a margin). The planner then keeps a safe distance without special code.
• Replanning: D* Lite and similar algorithms repair the path when a new obstacle is seen, without starting over.
• Smoothing: a grid path has staircase corners; it is smoothed (splines, shortcuts) and the kinematics of the robot are respected (a car cannot turn on the spot: lattice planners, Hybrid A*, Dubins curves).
• Road maps: for fleets, the map is a graph of lanes and intersections; A* runs on this much smaller graph.

5.3 Global and local planners

Pure pursuit: aim at a point of the path at a “look-ahead” distance l in front of the robot; with the angle α to this point, drive on a circle of curvature κ = 2 sin α / l. DWA tries many pairs (speed, turn rate) reachable within the next fraction of a second, simulates each, and chooses the best one that avoids obstacles and progresses to the goal.

Key terms

• graph search
• Dijkstra
• A*
• heuristic
• admissible
• costmap
• pure pursuit
• DWA

6.1 Configuration space

A mobile robot is a point on a 2D map. An arm is a point in a space with one axis per joint: the configuration space (C-space, see the joint space of the modelling course). In C-space the arm is a single point q, and every obstacle of the workspace becomes a forbidden region that is hard to draw: the set of all q where the arm touches something. Planning a motion = finding a continuous curve of free points from q_start to q_goal.

6.2 Checking a collision

A planner needs thousands of collision checks per second. The robot and the scene are approximated by simple shapes (spheres, capsules, boxes, convex hulls) and arranged in trees so that most pairs are rejected quickly (libraries: FCL, Bullet, Coal). Distances (signed distance fields) are also used by optimisation methods. A motion between two free configurations is checked by testing several points in between; the margin must cover the gaps.

6.3 Sampling-based planners

These planners are probabilistically complete: if a path exists, the probability of finding it tends to 1 with time. Their weaknesses: narrow passages (a random sample rarely falls in them), jagged paths that need smoothing, and variable time.

6.4 Optimisation and the full pipeline

• Trajectory optimisation (CHOMP, TrajOpt, STOMP, MPC): start from a rough path and minimise a cost (length, smoothness, distance to obstacles, torque) by gradient steps. Gives smooth, short paths, but may get stuck in a local minimum: it is often initialised with an RRT solution.
• Potential fields: the goal attracts, obstacles repel. Simple and reactive, but local minima trap the robot (a U-shaped obstacle).
• Inverse kinematics for the goal: the goal is usually a tool pose, so the planner first needs the joint solutions (the several aspects of lesson 6 of the modelling course) and may try several.
• Time parameterisation: the geometric path then receives a speed profile that respects the joint limits (see the trajectory lesson of the control course).

Key terms

• configuration space
• collision checking
• PRM
• RRT
• probabilistically complete
• narrow passage
• trajectory optimisation
• potential field

A robot has skills (navigate, detect, grasp, place…). Something must decide which skill when, and what to do when one fails. That layer is called task planning, behaviour control or the “mission” level.

7.1 Finite state machines

A finite state machine (FSM) has states (“Search”, “Approach”, “Grasp”, “Carry”) and transitions triggered by events (“object found”, “grasp failed”, “battery low”). It is simple, visual and easy to certify. Its weakness: with many behaviours the number of transitions explodes (every state may need a link to “battery low” and “stop”), and the diagram becomes spaghetti.

7.2 Behaviour trees

A behaviour tree (BT) is a tree that is “ticked” from the root several times per second. Each node returns Success, Failure or Running. The main nodes:

7.3 Symbolic task planning

When the sequence of actions is not known in advance (“make the table ready”), a task planner searches for it. The world is described by symbols (“on(cup, shelf)”, “gripper_empty”) and each action by preconditions and effects, in a language such as PDDL. The planner searches (like A*) for a sequence that leads from the current state to the goal state. Variants: hierarchical task networks (HTN), task and motion planning (TAMP) which checks that each symbolic step is geometrically feasible.

7.4 Humans in the loop

• Teleoperation: a person drives the robot remotely (surgery, bomb disposal, space, inspection), with force feedback (haptics) and delay compensation.
• Shared autonomy: the human gives the goal, the robot handles the details; or the robot asks for help when it is uncertain.
• Fleet management: a central system assigns missions to many robots, handles traffic, charging and errors (lesson 9).

Key terms

• skill
• state machine
• behaviour tree
• sequence / fallback
• decorator
• PDDL
• TAMP
• teleoperation

8.1 Where learning fits

8.2 Reinforcement learning (RL)

An agent (the robot) observes a state s, chooses an action a with its policy π(a|s), receives a reward r and a new state. It learns, by trial and error, a policy that maximises the sum of future rewards, the return, discounted by γ (0 < γ < 1, “the future counts a bit less”):

G = r₀ + γ r₁ + γ² r₂ + …

• Algorithms: Q-learning and DQN (discrete actions), policy gradients, PPO, SAC (continuous actions such as joint torques), model-based RL (learn a model, plan inside it).
• Reward design is the hard part: a badly defined reward gets exploited (“reward hacking”: the robot finds a cheat that scores high without doing the task).
• Sample inefficiency: millions of trials are needed, impossible on a real robot. So training runs in simulation (thousands of robots in parallel on a GPU), then the policy is moved to the real robot: sim-to-real.
• Domain randomisation: randomise masses, friction, delays, lighting in simulation so that the policy is robust to the reality gap. This made legged robots walk and run on rough terrain and drones fly very fast.

8.3 Imitation learning

Instead of a reward, learn from demonstrations (teleoperation, hand guiding, motion capture). Behaviour cloning trains a network to predict the demonstrator's action from the observation (images + joint states). Problem: small errors push the robot into states never seen in the demos, and the errors compound; remedies are DAgger (ask the expert to correct the robot's own runs), more diverse data, and action “chunks” predicted by transformer or diffusion policies. 50 to 200 good demonstrations can be enough to learn a task such as folding a cloth or inserting a plug.

8.4 Foundation models

• Language models break a spoken order into steps and choose among the robot's skills (“bring me a drink” → go to kitchen, open fridge, grasp can…). The robot's skills and their success estimates keep the plan feasible.
• Vision-language-action models (VLA) take camera images and an instruction and output robot actions directly, trained on large datasets from many robots. They generalise to new objects and instructions better than task-specific policies, but are slow, data hungry and not yet reliable enough for everything.

Key terms

• supervised learning
• reinforcement learning
• policy
• reward
• sim-to-real
• domain randomisation
• behaviour cloning
• VLA

9.1 Fleets

In a warehouse hundreds of AMRs or AGVs share lanes. A fleet manager receives orders, assigns each to a robot (nearest, least loaded, enough battery), reserves lanes and intersections to avoid deadlocks (two robots facing each other in a corridor), sends robots to charge, and handles errors. Interoperability standards (VDA 5050, MassRobotics) let robots from different brands share one manager.

9.2 Swarms

A swarm is a group of many simple robots with no leader: each follows local rules and the group behaviour emerges, as with ants or birds. Advantages: robust (losing a robot costs little), scalable, cheap robots. Examples: drone light shows, agricultural micro-robots, warehouse sorting robots, robots that build structures, search and rescue.

9.3 Robots and people

• Human–robot interaction (HRI): legible motion (the person can guess where the robot goes), speech, gestures, screens, lights. A robot that explains what it will do is trusted more.
• Collaboration: handing over an object, carrying a load together, one robot per worker assisting with heavy tasks. The robot must predict the human's intent and keep safe distances (course “Robots in the real world”).
• Social navigation: pass on the right, do not cut through a group, keep a comfortable distance (about 1 m from a standing adult).
• Ethics and trust: privacy of cameras, responsibility in accidents, bias in learned systems, effects on jobs. They are part of engineering a robot, not an afterthought.

Key terms

• fleet manager
• task allocation
• MAPF
• deadlock
• swarm
• consensus
• HRI
• social navigation