Robotics: Planning and Control

This builds on Robotics, which grounded the abstract agent in a body — hardware, degrees of freedom, and perception cast as probabilistic filtering — and left off with the robot able to estimate its own pose and a map from noisy motion and range readings. That estimate is where deliberation begins. Here we take it forward into a plan, and then into the motor torques that make a drifting physical body actually follow the plan.

Planning to move

Every robot deliberation ends in a decision about how to move an effector. The point-to-point motion problem is to deliver the effector to a target location; the harder compliant motion problem has the robot move while in physical contact with an obstacle (screwing in a bulb, pushing a box). The first job is to pick a representation in which such problems can be stated, and the key step is to plan not in physical space but in configuration space.

Configuration space

Take a two-joint arm. Describing it by the Cartesian coordinates of its parts — $(x_e,y_e)$ for the elbow, $(x_g,y_g)$ for the gripper — gives a workspace representation, good for collision checking but burdened with linkage constraints: the elbow and gripper are a fixed distance apart because a rigid forearm joins them, and a planner must generate only paths that respect that nonlinear constraint. The alternative is to represent the state by the arm's joint angles, $\mathbf{q}=({\phi }_s,{\phi }_e)$ for the shoulder and elbow — its configuration. The space $\mathcal{C}$ of all configurations is configuration space, and in it a path can be a straight line: move each joint at constant velocity from start to goal.

Configuration space has its own costs. The task is usually stated in workspace coordinates, so we must map between the two. Going from configuration to workspace is easy — a chain of coordinate transforms, linear for prismatic joints and trigonometric for revolute ones — a computation called kinematics. The inverse, computing the configuration that puts the effector at a specified workspace point, is inverse kinematics, and it is hard: the solution is rarely unique, since several joint arrangements can place the gripper identically.

Velocity kinematics: the Jacobian

Kinematics maps positions; robots also need to map velocities. If the joints turn at rates $\dot{\mathbf{q}}$, how fast and in what direction does the effector move? Differentiating the forward-kinematics map $\mathbf{x}=k(\mathbf{q})$ — the effector position $\mathbf{x}$ as a function of the joint configuration $\mathbf{q}$ — by the chain rule gives a linear relation between the two velocity vectors,

The manipulator Jacobian $\mathbf{J}(\mathbf{q})$ is the matrix of partial derivatives of effector coordinates with respect to joint coordinates, one row per task dimension and one column per joint. It is the same object the extended Kalman filter linearized $f$ and $h$ with earlier — a first-order local map — here relating the joint-velocity space to the workspace-velocity space at the current configuration. Because it depends on $\mathbf{q}$, the Jacobian changes as the arm moves: the identical joint rate produces a different effector velocity in a folded pose than in an extended one. AIMA develops the kinematic map and its inverse in §25.4; the Jacobian is the derivative of that map, and the velocity relation and singularities here are its standard consequences.¹

Worked example: the Jacobian of a two-link arm

Carry the two-link planar arm through the arithmetic. With link lengths ${\ell }_1={\ell }_2=1$ and joint angles $\mathbf{q}=({\phi }_s,{\phi }_e)$, forward kinematics places the effector at

Differentiating each coordinate with respect to each angle gives the Jacobian

Evaluate at ${\phi }_s=30^{\circ }$, ${\phi }_e=60^{\circ }$, so ${\phi }_s+{\phi }_e=90^{\circ }$. Using $\sin 30^{\circ }=0.5$, $\cos 30^{\circ }=0.866$, $\sin 90^{\circ }=1$, $\cos 90^{\circ }=0$,

Now turn the joints at $\dot{\mathbf{q}}=(0.1,0.2)$ rad/s. The effector velocity is the matrix-vector product

The columns read off cleanly: turning only the shoulder ($\dot{\mathbf{q}}=(1,0)$) sweeps the effector at $(-1.5,0.866)$, the velocity of a point $2$ units out on a rotating link; turning only the elbow ($\dot{\mathbf{q}}=(0,1)$) gives $(-1,0)$, the velocity of the forearm tip about the elbow. The actual motion is their weighted sum.

Singularities

At some configurations the Jacobian loses rank — its columns become linearly dependent — and the arm cannot move its effector in certain directions no matter how it drives the joints. These are singularities. For the two-link arm, take the elbow straight, ${\phi }_e=0^{\circ }$, so both links point the same way. Then $\sin ({\phi }_s+{\phi }_e)=\sin {\phi }_s$ and the two columns of $\mathbf{J}$ become parallel, giving

With the arm fully extended, both joints can only move the effector across the outstretched line, never along it — the reachable effector velocities collapse from the full plane to a single line. The determinant ${\ell }_1{\ell }_2\sin {\phi }_e$ makes this exact: it vanishes at ${\phi }_e=0$ (and $180^{\circ }$), the boundary of the workspace and the fold-back pose. Near a singularity the trouble is quantitative, not just qualitative: to move the effector a little in the near-forbidden direction, the joints must move a lot, so joint rates blow up.

Resolved-rate control

The Jacobian runs the other way to command motion. Given a desired effector velocity ${\dot{\mathbf{x}}}_d$ — follow a straight line, track a moving target — the joint rates that produce it come from inverting the relation,

recomputed at every step as $\mathbf{q}$ changes. This is resolved-rate control: rather than solving the full nonlinear inverse kinematics for each target position, resolve the desired velocity into joint velocities through the local linear map, integrate, and repeat. It sidesteps the multiplicity of inverse kinematics because it tracks a velocity from a known starting configuration instead of jumping to an arbitrary goal pose. For a redundant arm — more joints than task dimensions, so $\mathbf{J}$ is not square — the inverse is replaced by the pseudoinverse${\mathbf{J}}^{+}={\mathbf{J}}^{\top }(\mathbf{J}{\mathbf{J}}^{\top }{)}^{-1}$, which returns the smallest joint motion achieving ${\dot{\mathbf{x}}}_d$ and leaves the extra freedom to a secondary objective (avoid a joint limit, stay clear of an obstacle). The catch is the singularity: as $\det \mathbf{J}\to 0$ the inverse's entries diverge and resolved-rate control demands impossible joint speeds, so practical controllers damp the inverse near singularities, trading a little tracking accuracy for bounded joint rates.

Obstacles complicate $\mathcal{C}$ further. It splits into free space — the configurations the robot may attain — and occupied space, the rest. An obstacle with a simple polygonal shape in the workspace can map to a wildly nonlinear, even concave, region in configuration space; the shape of the free space is generally too complex to construct explicitly. In practice a planner probes$\mathcal{C}$: generate a candidate configuration, apply the kinematics, and check for collisions in workspace coordinates to decide whether it lies in free space. This is the classic piano-mover's problem — sliding a rigid body (a piano, a robot) among obstacles reduces to path planning in the body's configuration space.

Two families of planner reduce this continuous problem to a discrete graph search.

Cell decomposition

Cell decomposition carves free space into finitely many contiguous regions, each simple enough that a path across it is trivial (a straight line). Planning then becomes a discrete graph search over cells — the search of the earlier search lessons. The simplest version is a regular grid.

The grid is trivial to implement but has three limitations. It scales badly: cell count grows exponentially with dimension $d$ — the curse of dimensionality again. It can be unsound if it uses mixed cells (part free, part occupied), since there may be no straight crossing, or incomplete if it forbids them, since the only route may pass through one. And any path through a grid is jagged, with sharp corners no real robot can execute at speed. Refinements help: recursive subdivision of mixed cells (complete if a minimum passage width is bounded, but each split spawns $2^d$ children), exact cell decomposition into irregularly-shaped simple cells, and hybrid A*, which stores the exact continuous state each cell was reached in so the recovered trajectory is smooth and executable.

The grid also invites a change of cost. A shortest path hugs obstacles — a parking space with one millimeter of clearance is no parking space at all — so we add a potential field, a function whose value grows as the configuration nears an obstacle, into the cost. Minimizing path length plus potential trades a longer path for a safer one that keeps its distance.

Skeletonization

The second family, skeletonization, reduces free space to a one-dimensional skeleton on which planning is a simple graph search. One skeleton is the Voronoi graph: the set of points equidistant from two or more obstacles. To plan, the robot moves in a straight line onto the graph, follows it to the point nearest the goal, then leaves it for the target. Voronoi paths maximize clearance but detour needlessly in open space and are hard to compute in high dimensions.

The alternative skeleton, the probabilistic roadmap, scales much better. Rather than compute a skeleton analytically, sample one: scatter many random configurations, discard those not in free space, and join two survivors by an arc when a straight line between them stays in free space. Adding the start and goal yields a graph to search.

The method is theoretically incomplete — an unlucky sample set can leave start and goal disconnected — but the failure probability shrinks with more samples, and directing samples toward promising regions (or growing a tree outward from the start, the rapidly-exploring random tree, RRT) makes it the method that scales best to high-dimensional configuration spaces. An RRT grows by repeatedly picking a random point, finding the nearest tree node, and extending a short branch toward it, so the tree reaches quickly into unexplored free space.

Planning uncertain movements

None of the planners above face the defining trait of robotics: uncertainty. It arrives from partial observability, from stochastic or unmodeled effects of actions, and from the approximations of filtering itself, which never hands the robot an exact belief.

The cheapest response is to ignore it: extract the most likely state from the belief distribution and plan a single path through it as if it were certain. This works when uncertainty is small. And because incorporating each new measurement shifts the belief, many robots replan on the fly during execution — the online replanning technique — so a stale path is repaired rather than blindly followed.

When uncertainty is not small, single paths are too brittle, and the right object is a policy. If the robot is uncertain only in its transitions but its state is fully observable, the problem is a Markov decision process, whose solution is an optimal policy (in robotics, a navigation function) telling the robot what to do in every state. Under partial observability it becomes a POMDP, whose policy is defined over the entire belief distribution — which lets the robot act on what it does not know, for instance by taking an information gathering action to resolve a critical uncertainty (impossible in an MDP, which assumes full observability). Exact POMDP solvers do not scale to continuous robot state, so practical systems fall back on heuristics such as coastal navigation, which keeps the robot near known landmarks to hold uncertainty down.

Robust methods

A different stance handles uncertainty without probabilities at all. Robust control assumes only that error is bounded, not distributed, and seeks a plan that works for every value inside the bound. Its use in assembly is fine-motion planning (FMP): moving an arm in close proximity to a static object, where the motions and features are so small the robot cannot accurately measure or control its position.

An FMP plan is a series of guarded motions, each a motion command paired with a termination condition on the sensors. The commands are typically compliant motions that let the effector slide along a surface rather than jam against it. The design insight is to exploit the geometry so that every outcome consistent with the uncertainty bound still succeeds.

Robust plans are worst-case optimal — designed for the worst outcome rather than the expected one — which is the right objective precisely when a failure during execution costs far more than any of the other costs.

Moving

So far we have planned motions; now the robot has to move. Plans from a deterministic path planner assume the robot can follow any path exactly, but robots have inertia and cannot execute arbitrary paths except at arbitrarily slow speeds. In most cases the robot exerts forces rather than commanding positions, and this section computes those forces.

Dynamics and control

The dynamic state extends the kinematic state with velocity (and possibly acceleration). Its transition model is expressed as differential equations relating a quantity to its rate of change. Planning directly in dynamic space would give better performance, but the space has higher dimension than the kinematic space, so the curse of dimensionality rules it out for all but the simplest robots. Practical systems therefore plan a kinematic path and hand it to a controller — a mechanism that generates controls in real time using feedback, to keep the robot on the planned reference path.

Keeping a robot on a path sounds trivial and is not. Suppose the controller, on seeing a deviation, applies an opposing force proportional to it. Let $y(t)$ be the reference path and $x_t$ the state; this is a P controller (proportional):

with gain parameter $K_P$ setting how hard it corrects. A P controller is, in the absence of friction, a spring law — driven back to the reference it overshoots, then overshoots the other way, and oscillates forever. Shrinking $K_P$ only slows the oscillation; it does not stop it.

A controller is stable if small perturbations leave a bounded error, and strictly stable if it returns to the reference. The P controller is stable but not strictly stable. The PD controller adds a derivative term:

The derivative term dampens the system: when the error changes rapidly it opposes the proportional term, killing the overshoot; when the error is steady it vanishes and the proportional term takes over. A PD controller tracks smoothly where a P controller thrashed. PD controllers still fail against a systematic external force — a car on a banked road pulled steadily to one side — that they never fully cancel. To address this, add a third, integral term accumulating error over time, giving the PID controller (proportional–integral–derivative):

The integral grows while a long-lived deviation persists until the control forces it to shrink, wiping out systematic error at the cost of more oscillation risk. PID controllers are the industrial standard across a wide range of control problems.

Worked example: why the derivative term damps

For example, track a reference $y(t)=0$ (hold the path) with gains $K_P=0.8$ and $K_D=0.5$, sampling error at $\Delta t=1$. Suppose the robot has overshot to $x_t$ and is still rushing further off course. Compare a P controller against a PD controller at two consecutive steps, using the discrete derivative $\dot{e}\approx (e_t-e_{t-1})/\Delta t$ with error $e_t=y(t)-x_t=-x_t$.

At both steps the error is growing more negative (the robot is accelerating away from the path), so $\dot{e}<0$ and the derivative term $K_D\dot{e}=-0.15$ adds to the corrective push in the same direction. The PD controller commands a stronger correction ($-0.550$ vs the P controller's $-0.400$) precisely because the error is worsening — it reacts to the trend, not just the present offset, and so it begins braking before the overshoot peaks. Now flip to the return swing, when the robot is racing back toward the path: there $e_t$ is shrinking in magnitude, so $\dot{e}$ has the opposite sign to $e$, and $K_D\dot{e}$ subtracts from the proportional term, easing off the throttle before the robot shoots past. That asymmetry — push harder while diverging, ease off while converging — is what converts the P controller's endless spring oscillation into the PD controller's smooth settle.

Potential-field control

The potential field met earlier as a cost term can also generate motion directly, skipping the planning phase. Define an attractive force pulling the robot toward the goal and a repellent one pushing it from obstacles; the field's single global minimum sits at the goal, and its value is the sum of distance-to-goal and proximity-to-obstacle. The robot simply descends the field. No planning was involved, and evaluating the gradient at the current configuration is cheap — compared to path planners that are exponential in the DOFs, extremely so.

But potential fields have local minima that trap the robot: it may rotate a single joint toward the goal until it wedges against the wrong side of an obstacle, the field too coarse to make it bend its elbow. Potential-field control is excellent for local motion, but global planning is sometimes still needed. And because its forces depend on positions, not velocities, it is a kinematic method that can fail if the robot moves fast.

Robotic software architectures

An architecture is a methodology for structuring the algorithms — the languages, tools, and overall philosophy for bringing programs together. A robot architecture must decide how to combine two techniques with orthogonal strengths: reactive control, sensor-driven and fast but blind to anything not sensed at the moment of decision, and deliberative planning, which sees the global picture but is slow. Most architectures put reactive techniques at the low levels and deliberative ones at the high levels; those that combine both are hybrid architectures.

The subsumption architecture

The subsumption architecture (Brooks, 1986) assembles reactive controllers out of finite state machines. Nodes may test sensor variables, arcs may emit messages to motors or to other machines, and internal clocks time the traversals — the machines are therefore augmented finite state machines (AFSMs). A four-state AFSM can generate the cyclic leg motion of a hexapod walker: the swing phase watches its sensor, and if the leg is stuck it retracts, lifts higher, and swings again. The architecture composes complex controllers bottom-up from such machines.

Its virtues are also its limits. The AFSMs are driven by raw sensor input, which works only when that input is reliable and complete; the lack of deliberation makes it hard to change the robot's task; and the interplay of dozens of AFSMs becomes impossible for a human to understand. Subsumption is rarely used in robotics today despite its historical importance, though it left its mark on later architectures.

The three-layer architecture

The most popular hybrid is the three-layer architecture: a reactive layer, an executive layer, and a deliberative layer, distinguished by how fast they think.

The reactive layer provides low-level control with a tight sensor–action loop cycling in milliseconds. The executive layer (or sequencing layer) is the glue: it accepts directives from the deliberative layer — a set of via-points from a path planner, say — decides which reactive behavior to invoke, and integrates sensor information into an internal state, hosting the localization and online-mapping routines, cycling in seconds. The deliberative layer generates global solutions by planning, using models learned or supplied, cycling in minutes. The three-way split is loose; real systems add layers for user interaction or multi-robot coordination.

The pipeline architecture

The pipeline architecture also runs many processes in parallel, but its modules resemble the three-layer ones. Data enters at the sensor interface layer; the perception layer updates the robot's world models; the planning and control layer turns those into controls; and the vehicle interface layer sends them to the hardware.

The key is that all of this happens at once. While perception digests the most recent scan, control acts on slightly older data — the way we do not switch off our motion controllers to process new sensory input, but perceive, plan, and act simultaneously. Computation is data-driven and asynchronous, and the result is fast.

Learning-based robotics

The pipeline this lesson built — localize with a filter, plan in configuration space, track with a PID loop — is model-based end to end. Every stage runs on an equation someone wrote by hand: the kinematic motion model, the sensor model, the collision predicate, the control law. That works when the model is available and accurate. The frontier past AIMA's Chapter 25 is what to do when it is not — when the contact dynamics of a hand closing on an object, or the friction of a leg on loose scree, or the mapping from raw camera pixels to a grasp, is too tangled to write down. The answer that has taken over since the mid-2010s is to learn the hard stages from data while keeping the classical scaffolding around them.²

Deep reinforcement learning for control. The clearest demonstration that a learned policy can replace a hand-derived controller is OpenAI et al., Learning Dexterous In-Hand Manipulation (2018; IJRR 2020). They trained a policy entirely in simulation to reorient a block held by a five-fingered Shadow hand, then transferred it to the physical hand with no real-robot fine-tuning. The key was domain randomization: randomizing the simulator's masses, frictions, and visual appearance so widely that the real world looks to the policy like just another random instance, closing the sim-to-real gap that had defeated earlier transfer attempts. The policy learned dynamics the team never modeled — regrasping, finger gaiting — that no PID loop over a written contact model would have produced.

Legged locomotion via RL. Hwangbo et al., Learning Agile and Dynamic Motor Skills for Legged Robots (Science Robotics, 2019), trained control policies for the ANYmal quadruped in simulation and ran them on the real machine, recovering from falls and running faster than the robot's prior hand-tuned controllers, with an actuator network learned from data to bridge the sim-to-real gap in the motors themselves. Lee et al., Learning Quadrupedal Locomotion over Challenging Terrain (Science Robotics, 2020), pushed this to blind walking over mud, snow, and rubble by training a policy in simulation and transferring it to a physical ANYmal that had never seen those surfaces. Both replace the static-versus-dynamic-stability gait analysis of this lesson with a policy that discovered its own dynamically stable gaits — but note what stayed classical: the state estimation feeding the policy is still a filter of the kind built above.

Learned visuomotor policies. Levine et al., End-to-End Training of Deep Visuomotor Policies (JMLR, 2016), trained a single convolutional network mapping raw camera images straight to motor torques for manipulation tasks like screwing a cap on a bottle, jointly optimizing perception and control rather than pipelining a separate vision module into a separate controller. This is the sharpest contrast with the three-layer and pipeline architectures: instead of perception updating a world model that planning then consumes, one network is trained so its internal representation is whatever best serves the control objective.

Imitation and modern manipulation. Where a reward is hard to specify but demonstrations are easy to collect, behavior cloning learns a policy by supervised regression from observed states to expert actions. A recent line represents the policy as a generative model over action sequences: Chi et al., Diffusion Policy (RSS, 2023), models the action distribution with a denoising diffusion process, which handles the multimodality of human demonstrations (there are several good ways to grasp a mug) better than a network that regresses to a single averaged action. These methods layer directly onto classical stacks — the learned policy still emits set-points that a PID loop tracks.

Modern SLAM. The EKF-SLAM sketched above — augment the state vector with landmark positions, update quadratically — is one point in a large design space. The canonical modern counterpart is Mur-Artal et al., ORB-SLAM (IEEE T-RO, 2015): a feature-based visual SLAM system that tracks ORB keypoints across frames, builds a sparse map, closes loops by place recognition, and refines the whole trajectory-and-map estimate with bundle adjustment — a graph optimization over all poses and points at once, rather than the single Gaussian an EKF maintains. Bundle-adjustment SLAM scales to far larger maps than the EKF's quadratic update allows and is what runs on today's drones and phones. It is the graph-relaxation successor mentioned in the SLAM section.

Learning did not replace the classical machinery; it is layered onto it. Configuration-space planning, particle and Kalman filters, and PID control remain the backbone — the parts of the problem where a model is available and a guarantee matters. Learning is added at the stages where writing the model down is the bottleneck: contact-rich manipulation, locomotion over unmodeled terrain, and perception from raw high-dimensional sensors. A modern robot is a hybrid, and every learned block in it still sits on a filter or a controller from this lesson.

Application domains

Robots are deployed across many domains. In industry and agriculture, manipulators run assembly lines (welding, part placement, painting) more cost-effectively than people, and outdoor machines harvest, mine, and strip paint off ships far faster than human crews. In transportation, autonomous straddle carriers move shipping containers, indoor gofers like the Helpmate robot carry goods through hospitals, and Kiva systems shuttle shelves in fulfillment centers. Robotic cars — spurred by the DARPA Grand and Urban Challenges, won by STANLEY and BOSS — aim to cut the million-plus annual traffic deaths. In health care, surgical robots place instruments in brains, eyes, and hearts with high precision, and rehabilitation aids assist the elderly and handicapped. In hazardous environments, robots clean nuclear waste (Chernobyl, Three Mile Island), searched the World Trade Center rubble, and clear minefields. In exploration, they reach the surface of Mars, the deep sea, and abandoned mines that they map in 3D. And in personal service the Roomba became the best-selling mobile robot of all, while entertainment (robotic soccer) and human augmentation (exoskeletons, prosthetic limbs, teleoperation) round out the list.

The common thread: the abstract agent computes an answer, while the robot must enact it in a body that is already moving, sensing imperfectly, and slipping off course. Each of the field's inventions — configuration space, particle filters, PID loops, layered architectures — adapts the abstract machinery to those physical constraints.