01-基础概念-数学工具包

作者：姚博文 · 2026-09-15

数学工具包（Math Toolkit：向量·矩阵·微积分·线性代数·概率）

定位：智能小车用到的数学一站式地图，每块数学都绑定一个具体机器人任务（变换/里程计/控制/滤波），全部配可直接运行的 NumPy 示例 | 适用：智能小车/移动机器人开发

1\. 概述

1.1 这篇解决什么问题

自主导航小车的软件链路——里程计、定位、规划、控制、滤波——**全部是数学**，只是穿上了代码的外衣：

| 链路（见 00-路线图） | 用到的核心数学 | 代码里的典型位置 |

| --- | --- | --- |

| 里程计 | 积分（速度累积成位移） | 差速运动学 / odometry 节点 |

| 定位 | 齐次变换 + 高斯 + 贝叶斯（EKF） | AMCL / EKF / SLAM |

| 规划 | 线性方程（速度分解）、最小二乘（曲线拟合） | A* 后处理、DWA 轨迹采样 |

| 控制 | 导数（变化率）、泰勒线性化 | PID / LQR / MPC |

| 传感器标定 | 最小二乘、SVD | 相机内参、激光外参 |

这篇**不是数学教材**，每块数学只回答三个问题：

1. **解决什么机器人问题**（直觉）；

2. **代码里怎么用**（NumPy 可运行示例）；

3. **什么坑会踩**（排查方法）。

1.2 在自主导航链路中的位置

💡 **怎么用这篇**：后续文档里每遇到一个陌生公式（比如 06-定位与状态估计 里的 EKF 更新方程），回本篇对应小节查直觉和代码，再回原文档看应用。每个小节末尾都有「**用于后续文档**」标注，说明哪篇文档会用到这块数学。

1.3 与后续文档的挂钩

| 数学块（小节） | 用到的后续文档 | 具体用途 |

| --- | --- | --- |

| 2.1 向量与矩阵 | 05-移动机器人运动学模型、04-传感器融合 | 速度向量分解、测量向量投影 |

| 2.2 旋转矩阵/齐次变换 | 04-里程计与IMU、04-相机与标定、06-激光SLAM | 位姿传播、相机外参、地图↔车体帧 |

| 2.3 微积分 | 04-里程计与IMU、05-PID与经典控制、05-状态空间与LQR | 里程积分、PID D 项、Jacobian 线性化 |

| 2.4 线性代数 | 04-相机与标定、05-状态空间与LQR、06-定位与状态估计 | SVD 标定、特征值判稳、位姿估计 |

| 2.5 概率与统计 | 06-定位与状态估计、04-传感器融合、07-局部规划 | EKF/粒子滤波、融合权重、不确定性 |

1.4 环境要求

· Python ≥ 3.8 + NumPy（必需）；SciPy（可选，个别示例用到）；

· 本篇所有代码块可直接复制运行（`pip install numpy` 即可）；

· 不需要 ROS2；ROS2 集成点集中在第 4 节。

2\. 基本原理

2.1 向量与矩阵：三个运算的几何意义

2.1.1 点积（Dot Product / Inner Product）

**定义**：$\mathbf{a}\cdot\mathbf{b} = \sum_i a_i b_i = \|\mathbf{a}\|\,\|\mathbf{b}\|\cos\theta$

**几何意义**：两个向量"有多一致"——一个向量在另一个方向上的**投影 × 另一个的长度**。

机器人里最常用的三个用法：

· **求夹角**：$\cos\theta = \dfrac{\mathbf{a}\cdot\mathbf{b}}{\|\mathbf{a}\|\,\|\mathbf{b}\|}$，比如激光点方向与车头方向的夹角（