《自动驾驶与机器人中的SLAM技术》ch2：基础数学知识

目录

[2.1 几何学](#2.1 几何学)

向量的内积和外积

旋转矩阵

旋转向量

四元数

李群和李代数

[SO(3)上的 BCH 线性近似式](#SO(3)上的 BCH 线性近似式)

[2.2 运动学](#2.2 运动学)

李群视角下的运动学

[SO(3) + t 上的运动学](#SO(3) + t 上的运动学)

线速度和加速度

扰动模型和雅可比矩阵

典型算例：对向量进行旋转

典型算例：旋转的复合

[2.3 滤波器和最优化理论](#2.3 滤波器和最优化理论)

状态估计问题与最小二乘

[KF 卡尔曼滤波（线性系统）](#KF 卡尔曼滤波（线性系统）)

[EKF 扩展卡尔曼 （非线性系统）](#EKF 扩展卡尔曼 （非线性系统）)

最优化方法和图优化​编辑

优化和滤波

---

2.1 几何学

向量的内积和外积

旋转矩阵

旋转向量

四元数

三维旋转也可以由单位四元数 表示。注意：单位四元数的逆等于其共轭 。即 。任意的旋转都可以由两个互为相反数的四元数表示。

旋转向量和四元数的转换关系如下：

李群和李代数

SO(3)上的 BCH 线性近似式

和 的括号里面只能是 ，或者 ，或者 。如果是 或者没有括号，表示省略。

2.2 运动学

李群视角下的运动学

SO(3) + t 上的运动学

其中 t 为平移向量。

线速度和加速度

注意：能被各种传感器（车速传感器，轮速计）测量到的速度是车体系速度，。

线速度的变换式：

加速度的变换式：

在实际的处理中，由于测量传感器只能测量离散化的值，在精度不高的应用场景中，我们通常会选择忽略后面三项，只保留最简单的转换关系。

扰动模型和雅可比矩阵

典型算例：对向量进行旋转

设扰动 对应的李代数为 ：

对 进行泰勒展开并保留一阶项：

右扰动：

左扰动：

典型算例：旋转的复合

的一阶线性近似式 （视觉SLAM十四讲，p82）：

① 对 求导，对 进行右扰动：

其中第 3 行的 ，根据 的一阶线性近似式得：

② 对 求导，对 进行右扰动：

其中第 2 行的 ，根据 的一阶线性近似式得：

2.3 滤波器和最优化理论

状态估计问题与最小二乘

注意：这里的运动噪声为 ，观测噪声为 ，后续噪声的符号会变化，但表示的意义不变。

KF 卡尔曼滤波（线性系统）

EKF 扩展卡尔曼 （非线性系统）

矢量函数 在 点处进行线性化 。 在某一点 进行线性化的意思是：矢量函数 对状态 的雅可比矩阵，代入状态 的具体值。

为运动方程在上一时刻状态 进行线性化得到的雅可比矩阵，即运动方程对状态 的雅可比矩阵，代入上一时刻状态 的具体值：

为观测方程在当前时刻预测状态 进行线性化得到的雅可比矩阵，即观测方程对状态 的雅可比矩阵，代入当前时刻预测状态 的具体值：

这一块内容可以参考《机器人学中的状态估计》p89页，内容如下：

最优化方法和图优化

优化和滤波