The mathematics behind robotics
The maths a roboticist actually uses: rigid-body dynamics, forward and inverse kinematics, Jacobians, Lagrangian mechanics, control theory, trajectory planning and state estimation.
You do not need to be a mathematician to build robots, but a small set of ideas comes back again and again: where things are, how they move, what forces that takes, how to control it and how sure we are about all of it. This post walks through that set, one equation at a time, with a small example wherever it helps.
The toolbox at a glance
| Branch | What it is used for in robotics |
|---|---|
| Linear algebra | Positions, rotations, transformations, solving equations |
| Calculus | Velocities, accelerations, optimisation |
| Differential equations | How a robot's state changes over time |
| Probability | Noisy sensors, uncertainty, localisation |
| Optimisation | Inverse kinematics, trajectory planning, SLAM |
1. Describing position and orientation
A rigid body in 3D has 6 degrees of freedom: 3 for position and 3 for orientation. We store orientation as a rotation matrix \(\mathbf{R}\) and position as a vector \(\mathbf{t}\), and combine them into one homogeneous transformation:
$$ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \qquad \begin{bmatrix} \mathbf{p}_A \\ 1 \end{bmatrix} = \mathbf{T}_{AB} \begin{bmatrix} \mathbf{p}_B \\ 1 \end{bmatrix} $$
The nice part: moving through several frames is just matrix multiplication, \(\mathbf{T}_{AC} = \mathbf{T}_{AB}\,\mathbf{T}_{BC}\). In ROS, the tf2 library does exactly this for every frame of the robot.
2. Kinematics: motion without forces
Forward kinematics answers: given the joint angles, where is the tool? For a two-link planar arm with link lengths \(l_1, l_2\):
$$ x = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2), \qquad y = l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2) $$
Example. With \(l_1 = 1.0\) m, \(l_2 = 0.8\) m, \(\theta_1 = 30^\circ\) and \(\theta_2 = 45^\circ\): \(x = 0.866 + 0.207 = 1.07\) m and \(y = 0.500 + 0.773 = 1.27\) m.
For real arms with many joints, the Denavit–Hartenberg (DH) convention gives each joint a frame using four numbers, and the tool pose is the product of one transformation per joint:
$$ \mathbf{T}_{0n} = \mathbf{T}_{01}(\theta_1)\,\mathbf{T}_{12}(\theta_2)\cdots\mathbf{T}_{(n-1)n}(\theta_n) $$
Inverse kinematics (IK) asks the opposite: which joint angles put the tool here? It is harder because there may be several solutions (elbow up or elbow down), none (the target is out of reach) or infinitely many (redundant arms). Simple arms have closed-form solutions; general arms are solved numerically, as in the next section.
3. The Jacobian: from joint speeds to tool speed
The Jacobian \(\mathbf{J}\) tells how the tool moves when the joints move a little:
$$ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\,\dot{\mathbf{q}}, \qquad \mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}} $$
For the two-link arm:
$$ \mathbf{J} = \begin{bmatrix} -l_1 \sin\theta_1 - l_2 \sin(\theta_1+\theta_2) & -l_2 \sin(\theta_1+\theta_2) \\ \phantom{-}l_1 \cos\theta_1 + l_2 \cos(\theta_1+\theta_2) & \phantom{-}l_2 \cos(\theta_1+\theta_2) \end{bmatrix}, \qquad \det \mathbf{J} = l_1 l_2 \sin\theta_2 $$
When \(\det\mathbf{J} = 0\) (here: the arm fully stretched or folded, \(\theta_2 = 0^\circ\) or \(180^\circ\)) the arm is at a singularity and loses the ability to move in some direction.
The Jacobian also gives a simple numerical IK: repeat \(\Delta\mathbf{q} = \mathbf{J}^{+}\,(\mathbf{x}_\text{target} - \mathbf{x})\) until the error is small, where \(\mathbf{J}^{+}\) is the pseudo-inverse. And it links forces too: the joint torques needed to push with force \(\mathbf{F}\) are \(\boldsymbol{\tau} = \mathbf{J}^\top \mathbf{F}\).
4. Mobile robots: the differential drive
Most indoor robots have two driven wheels of radius \(r\), a distance \(L\) apart. From the wheel speeds \(\omega_l, \omega_r\) we get the robot's forward speed \(v\) and turning rate \(\omega\):
$$ v = \frac{r}{2}(\omega_r + \omega_l), \qquad \omega = \frac{r}{L}(\omega_r - \omega_l) $$
and the pose \((x, y, \theta)\) changes as
$$ \dot{x} = v\cos\theta, \qquad \dot{y} = v\sin\theta, \qquad \dot{\theta} = \omega $$
Example. \(r = 0.05\) m, \(L = 0.30\) m, left wheel at 10 rad/s, right at 12 rad/s: \(v = 0.55\) m/s and \(\omega = 0.33\) rad/s, so the robot drives forward while turning gently left.
Other drive types: Ackermann steering (car-like, cannot turn on the spot) and omnidirectional mecanum wheels (can move sideways).
5. Dynamics: adding forces
A single rigid body obeys the Newton–Euler equations:
$$ \sum \mathbf{F} = m\,\mathbf{a}_{c}, \qquad \sum \boldsymbol{\tau} = \mathbf{I}\,\boldsymbol{\alpha} + \boldsymbol{\omega} \times (\mathbf{I}\,\boldsymbol{\omega}) $$
where \(\mathbf{a}_c\) is the acceleration of the centre of mass (the average position of the body's mass) and \(\mathbf{I}\) is the inertia tensor (how mass is spread around the centre).
For a whole arm it is easier to work with energy. The Lagrangian is kinetic minus potential energy, \(\mathcal{L} = T - V\), and each joint follows
$$ \frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial \dot{q}_i}\right) - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i $$
Doing this for every joint gives the standard form of robot dynamics:
$$ \underbrace{\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}}_{\text{inertia}} + \underbrace{\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}}_{\text{Coriolis, centrifugal}} + \underbrace{\mathbf{G}(\mathbf{q})}_{\text{gravity}} = \boldsymbol{\tau} $$
This equation tells you how strong the motors must be, and it is the model inside many advanced controllers.
6. Control: making it happen
A controller compares what you want with what you measure and computes a command. The workhorse is PID:
$$ u(t) = K_p\, e(t) + K_i \int_0^t e(\tau)\,d\tau + K_d\,\frac{de(t)}{dt} $$
- \(K_p\) reacts to the current error, \(K_i\) removes a steady leftover error, \(K_d\) damps overshoot.
Modern control writes the system in state-space form, \(\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}\), which handles many inputs and outputs at once. The system is stable when all eigenvalues of \(\mathbf{A}\) (or of the closed loop) have negative real parts. LQR then finds the feedback \(\mathbf{u} = -\mathbf{K}\mathbf{x}\) that balances accuracy against effort by minimising
$$ J = \int_0^\infty \left(\mathbf{x}^\top \mathbf{Q}\,\mathbf{x} + \mathbf{u}^\top \mathbf{R}\,\mathbf{u}\right) dt $$
7. Trajectory planning: smooth motion
Jumping straight to a target jerks the motors. Instead we plan a smooth path in time. A cubic polynomial from \(q_0\) to \(q_f\) in time \(t_f\), starting and ending at rest, is
$$ q(t) = q_0 + (q_f - q_0)\left[\,3\left(\frac{t}{t_f}\right)^2 - 2\left(\frac{t}{t_f}\right)^3\,\right] $$
Position and speed are continuous, and the speed is zero at both ends. Quintic polynomials also make the acceleration smooth.
8. Estimation: how sure are we?
Sensors are noisy, so robots keep a belief instead of a single answer. The Kalman filter does this in two steps, over and over:
$$ \text{Predict:}\quad \hat{\mathbf{x}}^- = \mathbf{F}\hat{\mathbf{x}} + \mathbf{B}\mathbf{u}, \qquad \mathbf{P}^- = \mathbf{F}\mathbf{P}\mathbf{F}^\top + \mathbf{Q} $$
$$ \text{Update:}\quad \mathbf{K} = \mathbf{P}^-\mathbf{H}^\top(\mathbf{H}\mathbf{P}^-\mathbf{H}^\top + \mathbf{R})^{-1}, \qquad \hat{\mathbf{x}} = \hat{\mathbf{x}}^- + \mathbf{K}(\mathbf{z} - \mathbf{H}\hat{\mathbf{x}}^-), \qquad \mathbf{P} = (\mathbf{I} - \mathbf{K}\mathbf{H})\mathbf{P}^- $$
Predict with the motion model, then correct with the measurement \(\mathbf{z}\). The Kalman gain \(\mathbf{K}\) decides whom to trust more: the model (small \(\mathbf{Q}\)) or the sensor (small \(\mathbf{R}\)).
SLAM (simultaneous localisation and mapping) extends this to estimating the robot's whole path \(\mathbf{x}_{1:t}\) and the map \(\mathbf{m}\) together. With the usual assumptions, the posterior factors into motion and measurement terms:
$$ p(\mathbf{x}_{0:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \;\propto\; p(\mathbf{x}_0) \prod_{k=1}^{t} \underbrace{p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)}_{\text{motion model}}\; \underbrace{p(\mathbf{z}_k \mid \mathbf{x}_k, \mathbf{m})}_{\text{measurement model}} $$
Filter-based SLAM (EKF-SLAM, FastSLAM) updates this belief step by step; graph-based SLAM and bundle adjustment find the most likely path and map with optimisation. The Visual SLAM guide goes into that in detail.
9. Cameras: from 3D to pixels
A camera projects a world point \((X, Y, Z)\) to a pixel \((u, v)\):
$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}}_{\mathbf{K}\ \text{(intrinsics)}} \underbrace{\begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix}}_{\text{extrinsics}} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} $$
\(s\) is the depth of the point, \(f_x, f_y\) the focal lengths in pixels and \((c_x, c_y)\) the image centre. Calibration estimates \(\mathbf{K}\) and the lens distortion once per camera.
Where to learn more
- Books: Robot Modeling and Control (Spong, Hutchinson, Vidyasagar), Introduction to Robotics (Craig), Robotics, Vision and Control (Corke), Probabilistic Robotics (Thrun, Burgard, Fox).
- Software: the Robotics Toolbox for Python or MATLAB to try kinematics, Eigen for linear algebra in C++, tf2 and MoveIt in ROS 2, and Gazebo for simulation.
The equations change little from one robot to the next. Once these feel familiar, most robotics papers and libraries become much easier to read.