Mathematical Foundations for Robotics

Mathematical Foundations for Robotics

The pursuit of building intelligent machines that interact physically with the world demands a rigorous mathematical framework. Robotics, at its core, is an applied science that translates abstract mathematical concepts into concrete physical actions. Every computation a robot performs—from interpreting sensor data to planning a path through a cluttered environment—rests upon mathematical foundations that have been refined over centuries. Understanding these foundations is not merely an academic exercise; it is the essential prerequisite for anyone seeking to advance the field of embodied intelligence.

This chapter establishes the mathematical toolkit that will accompany us throughout the entirety of this book. We begin with linear algebra, which provides the language for describing spatial transformations, system dynamics, and sensor measurements. We then proceed to probability theory, which addresses the fundamental challenge of reasoning under uncertainty—a challenge that pervades every real-world robotics application. Finally, we introduce optimization theory, which formalizes the problem of finding the best action or solution among alternatives. Together, these three mathematical pillars form the foundation upon which all subsequent topics, from perception to planning to control, are constructed.

Linear Algebra: The Language of Spatial Computation

Linear algebra is the mathematical discipline that studies vectors, vector spaces, and linear transformations. In robotics, linear algebra appears at every level of abstraction: at the lowest level, it describes how sensor readings are transformed into useful measurements; at the intermediate level, it captures the kinematic and dynamic relationships between robot joints and end-effectors; at the highest level, it enables us to formulate and solve the large-scale optimization problems that arise in motion planning and control.

Vector Spaces and Their Significance

A vector space (also called a linear space) is a collection of objects called vectors, together with two operations—vector addition and scalar multiplication—that satisfy a specific set of axioms. The most familiar example is $\mathbb{R}^n$, the space of $n$-dimensional real vectors, where vectors are ordered tuples of real numbers. However, vector spaces are far more general: the set of polynomials of degree at most $n$, the set of continuous functions on an interval, and the set of all possible robot configurations are all examples of vector spaces.

In robotics, we frequently work with several distinct vector spaces simultaneously. The configuration space $\mathcal{C}$ of a robot arm with $n$ joints is naturally identified with $\mathbb{R}^n$, where each point represents a particular setting of joint positions. The workspace, by contrast, is the subset of $\mathbb{R}^3$ (or $\mathbb{SE}(3)$ when we account for orientation) that the robot's end-effector can reach. Understanding the relationship between these spaces—how the robot's configuration maps to end-effector pose—is one of the central problems in robotics, and it is fundamentally a question in linear algebra.

The concept of a basis is crucial for practical computation. A basis for an $n$-dimensional vector space is a set of $n$ linearly independent vectors such that every vector in the space can be written uniquely as a linear combination of the basis vectors. The standard basis for $\mathbb{R}^n$, consisting of vectors with a single 1 and all other entries 0, is the most common choice, but alternative bases often provide computational or interpretive advantages.

Consider the problem of representing the orientation of a rigid body in three-dimensional space. While a rotation matrix requires nine parameters and is subject to orthogonality constraints, the axis-angle representation uses only three parameters—the direction of the rotation axis and the angle of rotation. Neither representation forms a vector space in the traditional sense (rotations do not add together in a way that preserves their group structure), but the tangent space at any rotation can be identified with $\mathbb{R}^3$, and the basis vectors of this tangent space correspond to infinitesimal rotations about the coordinate axes. This observation leads directly to the mathematical framework of Lie groups, which generalizes linear algebra to the curved spaces of rotations and rigid body transformations.

Matrices: Linear Transformations and Systems

A matrix is a rectangular array of numbers that represents a linear transformation between vector spaces. If $A$ is an $m \times n$ matrix, it defines a function $T: \mathbb{R}^n \to \mathbb{R}^m$ by matrix-vector multiplication: given a vector $\mathbf{x} \in \mathbb{R}^n$, the transformed vector is $\mathbf{y} = A\mathbf{x}$, where the $i$-th component of $\mathbf{y}$ is given by the dot product of the $i$-th row of $A$ with $\mathbf{x}$.

Matrices appear throughout robotics for diverse purposes. In kinematics, the Jacobian matrix $J(\mathbf{q})$ relates joint velocities to end-effector velocities: if $\mathbf{q} \in \mathbb{R}^n$ is the vector of joint positions and $\mathbf{p} \in \mathbb{R}^3$ is the position of the end-effector, then

$$\dot{\mathbf{p}} = J(\mathbf{q})\dot{\mathbf{q}}$$

The Jacobian is fundamental to velocity-level control, singularity analysis, and manipulability measures. When the robot approaches a singularity—a configuration where the Jacobian loses full rank—certain directions of motion become impossible or require infinite joint velocities. Detecting and avoiding singularities is essential for practical robot control.

In dynamics, matrices encode the mass properties of the manipulator. The mass matrix $M(\mathbf{q})$ appears in the Euler-Lagrange equations of motion:

$$M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}$$

where $C(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}}$ represents Coriolis and centrifugal forces, $\mathbf{g}(\mathbf{q})$ is the gravity vector, and $\boldsymbol{\tau}$ is the vector of applied joint torques. The mass matrix is symmetric and positive definite, which has important implications for the numerical methods used to simulate and control robot motion.

Matrix Decompositions: Computational Workhorses

While matrices provide a compact notation for linear transformations, the numerical solution of matrix equations requires careful algorithmic design. Matrix decompositions (also called factorizations) express a matrix as a product of matrices with special structure, enabling efficient and numerically stable computation.

LU decomposition factors a matrix $A$ as $A = LU$, where $L$ is lower triangular and $U$ is upper triangular. This decomposition is the foundation of Gaussian elimination and enables the rapid solution of linear systems $A\mathbf{x} = \mathbf{b}$ once the decomposition is computed. For robotics applications involving real-time control, the efficiency of LU decomposition is critical—it allows us to solve linear systems with $O(n^2)$ operations after an $O(n^3)$ preprocessing step.

Cholesky decomposition is a specialized factorization applicable to symmetric positive definite matrices. If $A$ is symmetric positive definite, it can be written as $A = LL^T$, where $L$ is lower triangular with positive diagonal entries. Cholesky decomposition requires roughly half the computational work of LU decomposition and produces better-conditioned factors. In robotics, the mass matrix, covariance matrices, and information matrices that arise in state estimation are all symmetric positive definite, making Cholesky decomposition the method of choice for their factorization.

```python import numpy as np

def solve_linear_system(A, b): """ Solve the linear system Ax = b using Cholesky decomposition. Assumes A is symmetric positive definit