Foundational Mathematics for Robotics: Linear Algebra, Optimization, and Probability Theory

This linear algebraic toolkit, while powerful for static transformations and kinematic analysis, is insufficient for the dynamic, noisy, and constrained problems that define real-world robotics. To address these challenges, we turn to the second foundational pillar of this chapter: optimization theory, which provides the formal framework for finding optimal solutions to problems with defined objectives and constraints. Nearly every robotics workflow—from calibrating a multi-sensor array to planning a collision-free trajectory for a humanoid robot—can be formalized as an optimization problem, where the goal is to minimize a cost function that quantifies performance, subject to physical or task-specific constraints. We first introduce convex optimization, the class of problems with desirable global optimality guarantees, before moving to non-convex optimization for the complex, non-linear problems that dominate advanced robotics applications. We conclude this section with a discussion of sparse and robust optimization techniques critical for scaling to large-scale systems like autonomous vehicles and multi-robot fleets.

<!-- 结构化标签：数学基础-优化理论-凸优化 -->

1.2 Convex Optimization: Theory and Robotics Applications

Convex optimization is the most well-studied class of optimization problems, defined by two core properties: the feasible set (all points satisfying problem constraints) is a convex set, and the objective function is a convex function. A set $C$ is convex if for any two points $x, y \in C$ and any $\lambda \in [0,1]$, the linear combination $\lambda x + (1-\lambda)y$ is also in $C$. A function $f: \mathbb{R}^n \rightarrow \mathbb{R}$ is convex if its domain is a convex set and for all $x, y$ in its domain and $\lambda \in [0,1]$: $$f(\lambda x + (1-\lambda)y) \leq \lambda f(x) + (1-\lambda)f(y)$$ The critical advantage of convex optimization for robotics is that any local minimum of a convex problem is also a global minimum, eliminating the risk of converging to suboptimal solutions that plagues non-convex problems.

<!-- 电子版侧边栏：💡 实践提示 --> > 💡 Practical Tip for Robotics Engineers: Most real-time robotics control problems (e.g., model predictive control for quadrotors, walking gait optimization for humanoids) are formulated as convex quadratic programs (QPs), which can be solved in milliseconds on embedded hardware using optimized solvers like OSQP or qpOASES.

1.2.1 Core Convex Optimization Problems in Robotics

The vast majority of convex problems in robotics fall into three categories, each with mature, high-performance solvers: 1. Linear Programming (LP): Objective and constraints are linear. Used for resource allocation in multi-robot task assignment, and actuator saturation constraint handling in control. 2. Quadratic Programming (QP): Objective is a quadratic convex function, constraints are linear. The workhorse of model predictive control (MPC), where the objective is to minimize tracking error and control effort, subject to actuator and collision constraints. 3. Second-Order Cone Programming (SOCP): Extends QP to include second-order cone constraints. Used for robust motion planning with uncertainty, and vision-based state estimation with bounded sensor noise.

For example, a differential drive robot’s MPC problem can be formulated as a QP where the state vector includes position, orientation, and velocity, the objective minimizes the sum of squared tracking error to a reference path and squared control effort (linear and angular velocity), and constraints enforce maximum speed, maximum steering angle, and collision avoidance with obstacles represented as convex polytopes.

1.2.2 First-Order and Second-Order Optimization Methods

For unconstrained convex optimization problems, iterative gradient-based methods are the de facto standard for robotics applications, as they scale efficiently to high-dimensional state spaces (e.g., 100+ degree-of-freedom humanoid robots, or 10,000+ variable pose graph optimization problems).

Gradient Descent (First-Order Method) The simplest iterative method updates the parameter vector $x$ in the direction of the negative gradient of the objective function $f(x)$, scaled by a step size $\alpha$: $$x_{k+1} = x_k - \alpha \nabla f(x_k)$$ The step size can be fixed, or selected via line search to ensure sufficient decrease in the objective. For robotics parameter estimation problems (e.g., sensor calibration, kinematic parameter tuning), gradient descent is simple to implement and debug, though its convergence is slow for ill-conditioned problems (e.g., when the Hessian matrix has a large condition number).

Newton’s Method (Second-Order Method) Newton’s method uses second-order derivative information (the Hessian matrix $H = \nabla^2 f(x)$) to compute a more informed update direction, leading to quadratic convergence near the optimum: $$x_{k+1} = x_k - H^{-1} \nabla f(x_k)$$ For robotics problems with a least-squares objective (e.g., sensor calibration, bundle adjustment), the Hessian is the Gauss-Newton approximation $H \approx J^T J$, where $J$ is the Jacobian matrix of the residual terms. This approximation avoids the need to compute second derivatives of the objective, and is the basis of nearly all modern robotics optimization solvers.

<!-- 纸书格式：案例1.1 -->

Case Study 1.1: Lidar-IMU Extrinsic Calibration via Least Squares

A common robotics calibration problem is estimating the 6-DOF rigid transformation between a lidar and an IMU mounted on a robot. Given a set of matched lidar point cloud features and IMU gravity measurements, the objective is to minimize the sum of squared reprojection errors of the lidar features into the IMU gravity-aligned frame, plus the sum of squared errors between the measured IMU gravity and the expected gravity vector after transformation. This is a non-linear least squares problem, which can be solved via Newton’s method (or its quasi-Newton variants like Levenberg-Marquardt) implemented in the Ceres Solver library. The optimal solution provides the extrinsic transformation that aligns the two sensor coordinate frames, enabling accurate sensor fusion for state estimation.

```python

纸书格式：代码示例1.1：使用Ceres Solver求解激光雷达-IMU外参

电子版可点击链接查看Ceres官方文档：https://ceres-solver.googlesource.com/ceres-solver

import ceres_solver as ceres import numpy as np

定义残差块：lidar特征重投影误差 + IMU重力对齐误差

class LidarIMUCalibrationResidual(ceres.CostFunction): def __init__(self, lidar_point, imu_gravity, lidar_point_cov, imu_gravity_cov): super().__init__() self.lidar_point = lidar_point # 3D lidar特征点 self.imu_gravity = imu_gravity # IMU测量的重力向量 self.lidar_point_cov = lidar_point_cov # lidar观测协方差 self.imu_gravity_cov = imu_gravity_cov # IMU观测协方差

设置参数块：6-DOF外参（李代数表示，便于优化）

self.set_parameter_block_sizes([6])

def Evaluate(self, parameters, residuals, jacobians): xi = parameters[0] # se(3)李代数向量，表示外参

将李代数转换为SE(3)变换矩阵

T = self.se3_exp(xi)

计算lidar点变换到IMU坐标系后的坐标

point_imu = T[:3, :3] @ self.lidar_point + T[:3, 3]

计算lidar重投影残差（假设lidar点在水平面上，z坐标为0）

residual_lidar = np.array([point_imu[0], point_imu[1]])

计算IMU重力对齐残差（变换后的重力应该与z轴对齐）

gravity_imu = T[:3, :3] @ self.imu_gravity residual_imu = gravity_imu - np.array([0, 0, 9.81])

合并残差

residuals[0] = np.concatenate([residual_lidar, residual_imu])

计算雅可比矩阵（省略实现细节，可通过自动微分或李代数求导获得）

if jacobians: jacobians[0] = self.compute_jacobian(xi) return True

初始化优化问题

problem = ceres.Problem()

添加残差块（省略数据加载步骤）

problem.AddResidualBlock(...)

求解优化问题

ceres.Solve(ceres.SolverOptions(), problem, summary) print("Optimal extrinsic parameter:", summary.BriefReport()) ```

1.2.3 Constrained Optimization: Lagrange Multipliers and KKT Conditions

Most robotics problems have hard constraints: mechanical joints have angle limits, robots cannot