移动机器人传感器与位姿估计

第5章 移动系统中使用的传感器

5.1 引言

轮式移动机器人需要使用传感器感知环境，以自主地执行任务。传感器用于应对环境中及所有机器人子系统中始终存在的不确定性与干扰。移动机器人无法精确掌握环境信息，同时对其运动模型的认识也存在缺陷（地图的不确定性、未知的运动模型、未知的动特性等）。由于执行器非理想化，动作的结果同样具有不确定性。因此，传感器的主要目的是降低这些不确定性，并实现对机器人状态以及环境状态的估计。

通常，不存在一种优雅的单一传感器解决方案（特别是在室内使用时），能够足够精确且可靠地测量所需信息（如机器人位姿）。而位姿对于定位机器人至关重要，这是移动机器人技术中最关键的挑战之一。因此，开发人员需要依赖更多传感器，并融合它们所测量的信息。这类方法可以获得质量更高、更稳健的信息。机器人位姿估计通常结合了相对传感器和绝对传感器。相对传感器提供的信息是相对于机器人坐标系的，而绝对传感器的信息则定义在某个全局坐标系中（例如地球坐标）。

利用传感器，移动机器人可以感知环境的状态。所测得的传感器信息需要进行充分的分析和解释。在现实世界中，测量值是动态变化的（例如光照的变化以及表面不同的声音或光吸收特性）。测量误差通常通过概率密度函数进行统计建模，一般假设为对称分布，甚至正态分布。然而，这种假设并不总是正确的（例如，由于超声波路径从传感器发射器到传感器接收器之间发生多重反射，超声波传感器的距离测量值可能大于真实距离）。

首先，简要描述坐标系变换。这些变换用于将传感器测量值正确映射到机器人，并在与机器人相关的坐标系中估计相关信息。然后，解释使用特定传感器在环境中估计机器人位姿的主要定位方法。最后，简要概述移动机器人技术中使用的传感器。

5.2 坐标系变换

安装在机器人上的传感器通常不在机器人的中心或机器人坐标系的原点。它们在机器人上的位置和姿态通过相对于机器人坐标系的平移向量和旋转来描述。这些变换用于将传感器坐标系中的测量量与机器人坐标相关联。

通过这些变换，我们可以描述感知方向向量（如加速度计、磁力计）或感知位置坐标（如激光测距扫描仪或相机）在机器人坐标系中的表示方式。此外，移动机器人在空间中运动，因此它们的位姿或运动可以通过适当的变换来描述。

此处给出了三维空间的一个简要概述，尽管在轮式移动机器人中通常二维就足够了（例如，由两次平移和一次旋转描述的平面运动）。首先将介绍一些关于旋转变换的表示方法，然后引入平移。

5.2.1 姿态与旋转

根据参考坐标系（例如机器人）来描述某个局部参考坐标系（例如传感器）的姿态，可通过旋转矩阵 $R$ 表示：

$$
R= \begin{bmatrix}
u_1 & u_2 & u_3 \
v_1 & v_2 & v_3 \
w_1 & w_2 & w_3
\end{bmatrix}
$$

其中局部坐标系的单位向量 $u$、$v$ 和 $w$ 在参考坐标系中表示为 $u = [u_1, u_2, u_3]^T$，$v = [v_1, v_2, v_3]^T$，$w = [w_1, w_2, w_3]^T$，且 $u \times v = w$。矩阵 $R$ 的行是体坐标系单位向量在参考坐标系单位向量 $x$、$y$ 和 $z$ 方向上的分量。矩阵 $R$ 的元素是两个坐标系各轴之间角度的余弦；因此，矩阵 $R$ 也称为方向余弦矩阵或 DCM。由于向量 $u$、$v$ 和 $w$ 是正交归一的，$R$ 的逆矩阵等于 $R$ 的转置（$\det R = 1$ 且 $R^{-1} = R^T$）。方向余弦矩阵具有九个参数来描述三个自由度，但这些参数并非相互独立，而是由六个约束关系定义的（$R$ 中每行元素的平方和为 1，且 $R$ 中任意两行的点积为零）。

局部（本体）坐标系中的向量 $v_L$ 通过以下变换从全局坐标系（参考）表示：

$$
v_L = R_{LG} v_G
$$

矩阵 $R_{LG}$ 因此将全局坐标中的矢量描述（由其分量给出）变换为局部坐标中的矢量描述。

基本旋转变换是通过绕轴 $x$、$y$ 和 $z$ 的基本旋转矩阵进行旋转得到的：

$$
R_x(\phi) = \begin{bmatrix}
1 & 0 & 0 \
0 & \cos(\phi) & \sin(\phi) \
0 & -\sin(\phi) & \cos(\phi)
\end{bmatrix}
$$

$$
R_y(\theta) = \begin{bmatrix}
\cos(\theta) & 0 & -\sin(\theta) \
0 & 1 & 0 \
\sin(\theta) & 0 & \cos(\theta)
\end{bmatrix}
$$

$$
R_z(\psi) = \begin{bmatrix}
\cos(\psi) & \sin(\psi) & 0 \
-\sin(\psi) & \cos(\psi) & 0 \
0 & 0 & 1
\end{bmatrix}
$$

其中 $R_{\text{axis}}(\text{angle})$ 表示绕某轴进行给定角度的旋转。连续旋转由旋转矩阵的乘积描述，且旋转顺序至关重要。方向余弦矩阵是刚体姿态的基本表示；但在某些情况下，其他表示方法更为合适。因此，下文将介绍另外两种方法。

欧拉角

欧拉角通过绕轴 $x$、$y$ 和 $z$ 的旋转来描述某个刚体的姿态。这些角度标记为
- $\phi$：滚动角度（绕 $x$ 轴），
- $\theta$：俯仰角度（绕 $y$ 轴），以及
- $\psi$：偏航或航向角度（绕 $z$ 轴）。

围绕 $x$、$y$ 和 $z$ 轴的旋转组合共有 12 种可能。最常用的是 3-2-1 组合，其中根据参考坐标系，机体坐标系的姿态由初始位姿获得，其中两个坐标系对齐，然后按以下顺序旋转机体坐标系：
1. 首先是绕 $z$ 轴进行偏航角 $\psi$ 的旋转，
2. 然后绕新得到的 $y$ 轴进行俯仰角 $\theta$ 的旋转，以及
3. 最后绕新得到的 $x$ 轴进行滚动角 $\phi$ 的旋转。

该旋转变换的方向余弦矩阵为

$$
R = R_x(\phi) R_y(\theta) R_z(\psi)
= \begin{bmatrix}
\cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \
\sin \phi \sin \theta \cos \psi - \cos \phi \sin \psi & \sin \phi \sin \theta \sin \psi + \cos \phi \cos \psi & \sin \phi \cos \theta \
\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi & \cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi & \cos \phi \cos \theta
\end{bmatrix}
$$

以及欧拉表示如下：

$$
\phi = \arctan\left(\frac{R_{23}}{R_{33}}\right)
$$
$$
\theta = -\arcsin(R_{13})
$$
$$
\psi = \arctan\left(\frac{R_{12}}{R_{11}}\right)
$$

使用欧拉角进行参数化并不冗余（三个自由度对应三个参数）。然而，其主要缺点是在旋转 $\theta = \pi/2$ 时出现奇异，此时绕 $z$ 轴和 $x$ 轴的旋转效果相同（二者重合）。这种现象在传统飞机陀螺仪中被称为万向节锁。在使用欧拉角表示法进行旋转仿真时，由于除以 $\cos \theta$ 而导致该奇异性出现。

四元数

四元数使用四个参数和一个约束方程来表示三维空间姿态，且不存在如欧拉角表示中的奇异性问题。从数学上讲，四元数是复数的非交换扩展，定义为

$$
q = q_0 + q_1 i + q_2 j + q_3 k
$$

其中，复数元素 $i$、$j$ 和 $k$ 通过表达式 $i^2 = j^2 = k^2 = ijk = -1$ 相关联。$q_0$ 是四元数的标量部分，而 $q_1 i + q_2 j + q_3 k$ 称为向量部分。四元数范数定义为

$$
|q| = \sqrt{q q^*} = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}
$$

其中 $q^ $ 是通过 $q^ = q_0 - q_1 i - q_2 j - q_3 k$ 得到的共轭四元数。逆四元数通过其共轭和范数值按如下方式计算：

$$
q^{-1} = \frac{q^*}{|q|^2}
$$

使用单位四元数对空间旋转进行参数化

$$
q_0 = \cos(\phi/2), \quad q_1 = e_1 \sin(\phi/2), \quad q_2 = e_2 \sin(\phi/2), \quad q_3 = e_3 \sin(\phi/2)
$$

其中向量 $e^T = [e_1, e_2, e_3]$ 是旋转轴的单位向量，$\phi$ 为绕该轴的旋转角度。对于单位四元数，有 $q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1$。

变换

$$
q_{v’} = q^{-1} \circ q_v \circ q
$$

通过四元数旋转向量 $v = [x, y, z]^T$

$$
q_v = x i + y j + z k
$$

（或等价地 $q_v = [0, x, y, z]^T$）绕轴 $e$ 旋转角度 $\phi$ 得到向量 $v’ = [x’, y’, z’]^T$，用四元数表示

$$
q_{v’} = x’ i + y’ j + z’ k
$$

其中 $\circ$ 表示定义在公式 (5.16)、(5.17) 中的四元数乘积。

四元数的另一个优势是能够相对容易地组合连续旋转。两个方向余弦矩阵的乘积可以用两个四元数的乘积等效表示。给定两个四元数，

$$
q = q_0 + q_1 i + q_2 j + q_3 k
$$
and
$$
q’ = q’_0 + q’_1 i + q’_2 j + q’_3 k
$$

首先通过由 $q’$ 定义的旋转，然后通过由 $q$ 定义的旋转，对某个向量从其初始姿态进行旋转，我们得到以下结果：

$$
q’’ = q’ \circ q = (q’_0 q_0 - q’_1 q_1 - q’_2 q_2 - q’_3 q_3)
+ i(q’_1 q_0 + q’_2 q_3 - q’_3 q_2 + q’_0 q_1)
+ j(-q’_1 q_3 + q’_2 q_0 + q’_3 q_1 + q’_0 q_2)
+ k(q’_1 q_2 - q’_2 q_1 + q’_3 q_0 + q’_0 q_3)
$$

或以向量-矩阵形式

$$
\begin{bmatrix}
q’‘_0 \ q’‘_1 \ q’‘_2 \ q’‘_3
\end{bmatrix}
= \begin{bmatrix}
q_0 & -q_1 & -q_2 & -q_3 \
q_1 & q_0 & -q_3 & q_2 \
q_2 & q_3 & q_0 & -q_1 \
q_3 & -q_2 & q_1 & q_0
\end{bmatrix}
\begin{bmatrix}
q’_0 \ q’_1 \ q’_2 \ q’_3
\end{bmatrix}
$$

四元数与方向余弦矩阵表示之间的关系由以下给出

$$
R(q) = \begin{bmatrix}
q_0^2 + q_1^2 - q_2^2 - q_3^2 & 2(q_0 q_3 + q_1 q_2) & 2(q_1 q_3 - q_0 q_2) \
2(q_1 q_2 - q_3 q_0) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_0 q_1 + q_2 q_3) \
2(q_0 q_2 + q_1 q_3) & 2(q_2 q_3 - q_0 q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2
\end{bmatrix}
$$

或相反方向

$$
q_0 = \frac{1}{2}\sqrt{1 + R_{11} + R_{22} + R_{33}}
$$
$$
q_1 = \frac{1}{4q_0}(R_{23} - R_{32})
$$
$$
q_2 = \frac{1}{4q_0}(R_{31} - R_{13})
$$
$$
q_1 = \frac{1}{4q_0}(R_{12} - R_{21})
$$

如果 $q_0$ 在公式 (5.19) 中接近零，则从方向余弦矩阵到四元数的变换（关系式 5.19–5.22）是奇异的。在这种情况下，我们可以使用等效形式（关系式 5.23–5.26）来计算四元数，从而避免数值问题：

$$
q_0 = \frac{1}{4T}(R_{32} - R_{23})
$$
$$
q_1 = T
$$
$$
q_2 = \frac{1}{4T}(R_{12} + R_{21})
$$
$$
q_3 = \frac{1}{4T}(R_{13} + R_{31})
$$

其中 $T = \frac{1}{2}\sqrt{1 + R_{11} - R_{22} - R_{33}}$

四元数与欧拉角（3-2-1 表示法）之间的关系由矩阵 $R_x(\phi)$、$R_y(\theta)$ 和 $R_z(\psi)$ 确定，该关系适用于四元数 $[\cos(\phi/2) + i \sin(\phi/2)]$、$[\cos(\theta/2) + j \sin(\theta/2)]$ 以及 $[\cos(\psi/2) + k \sin(\psi/2)]$。用于 3-2-1 旋转的四元数为

$$
q = [\cos(\psi/2) + k \sin(\psi/2)][\cos(\theta/2) + j \sin(\theta/2)][\cos(\phi/2) + i \sin(\phi/2)]
$$

或以向量形式表示

$$
q = \begin{bmatrix}
\cos(\phi/2) \cos(\theta/2) \cos(\psi/2) + \sin(\phi/2) \sin(\theta/2) \sin(\psi/2) \
\sin(\phi/2) \cos(\theta/2) \cos(\psi/2) - \cos(\phi/2) \sin(\theta/2) \sin(\psi/2) \
\cos(\phi/2) \sin(\theta/2) \cos(\psi/2) + \sin(\phi/2) \cos(\theta/2) \sin(\psi/2) \
\cos(\phi/2) \cos(\theta/2) \sin(\psi/2) - \sin(\phi/2) \sin(\theta/2) \cos(\psi/2)
\end{bmatrix}
$$

相反的变换是

$$
\phi = \arctan\left( \frac{2(q_1 q_0 + q_2 q_3)}{q_0^2 - q_1^2 - q_2^2 + q_3^2} \right)
$$
$$
\theta = -\arcsin(2(q_1 q_3 - q_2 q_0))
$$
$$
\psi = \arctan\left( \frac{2(q_3 q_0 + q_1 q_2)}{q_0^2 + q_1^2 - q_2^2 - q_3^2} \right)
$$

示例 5.1

传感器（磁力计）坐标系根据机器人坐标系进行旋转。将传感器坐标系标记为局部（L），机器人坐标系标记为全局（G）。传感器相对于机器人的姿态由两次旋转描述。假设初始时（L）与（G）对齐，然后将（L）绕 $x$ 轴旋转 $\alpha_x = 90^\circ$，最后绕新的 $y$ 轴旋转 $\alpha_y = 45^\circ$。

回答以下问题：
1. 根据机器人坐标系，由方向余弦矩阵（$R_{LG}$）表示的传感器姿态是什么？
2. 确定该变换的欧拉角（3-2-1 表示法）。
3. 确定描述该变换的四元数 $q_{LG}$。
4. 磁力计测量磁场的方向向量 $v = [0, 0, 1]^T$。该向量在机器人坐标系中的表示是什么？

解

1. 最终姿态由通过连续旋转得到的旋转矩阵描述，其中乘法顺序至关重要。

$$
R_{LG} = R_y(\alpha_y) R_x(\alpha_x)
= \begin{bmatrix}
\cos(\alpha_y) & 0 & -\sin(\alpha_y) \
0 & 1 & 0 \
\sin(\alpha_y) & 0 & \cos(\alpha_y)
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \
0 & \cos(\alpha_x) & \sin(\alpha_x) \
0 & -\sin(\alpha_x) & \cos(\alpha_x)
\end{bmatrix}
= \begin{bmatrix}
0.7071 & 0.7071 & 0 \
0 & 1 & 0.7071 \
-0.7071 & 0 & 0.7071
\end{bmatrix}
$$

矩阵 $R_{LG}$ 的行是用机器人坐标系表示的传感器轴的分量，从该旋转的图形表示中可以看出这一点。

2. 根据矩阵 $R = R_{LG}$ 计算 3-2-1 表示法的欧拉角：

$$
\phi = \arctan\left(\frac{R_{23}}{R_{33}}\right) = 90^\circ
$$
$$
\theta = -\arcsin(R_{13}) = 0^\circ
$$
$$
\psi = \arctan\left(\frac{R_{12}}{R_{11}}\right) = 45^\circ
$$

其中旋转矩阵由 $R_{LG} = R_x(\phi) R_y(\theta) R_z(\psi)$ 得到，且基本旋转 $R_x(\phi)$、$R_y(\theta)$ 和 $R_z(\psi)$ 在公式 (5.5) 中定义。

3. 通过连续旋转的四元数乘积得到四元数 $q_{LG}$：

$$
q_{LG} = q_x \circ q_y
$$

其中 $q_x$ 由旋转角度 $\phi_x = 90^\circ$ 绕旋转轴 $e_x = [1, 0, 0]^T$ 和 $q_y$ 以旋转角度 $\phi_y = 45^\circ$ 绕旋转轴 $e_y = [0, 1, 0]^T$ 根据公式 (5.10) 定义

$$
q_x = \begin{bmatrix}
\cos(\phi_x/2) \
e_{x1} \sin(\phi_x/2) \
e_{x2} \sin(\phi_x/2) \
e_{x3} \sin(\phi_x/2)
\end{bmatrix}
= \begin{bmatrix}
0.7071 \
0.7071 \
0 \
0
\end{bmatrix}
$$

$$
q_y = \begin{bmatrix}
\cos(\phi_y/2) \
e_{y1} \sin(\phi_y/2) \
e_{y2} \sin(\phi_y/2) \
e_{y3} \sin(\phi_y/2)
\end{bmatrix}
= \begin{bmatrix}
0.9239 \
0 \
0.3827 \
0
\end{bmatrix}
$$

最终的四元数（见公式 5.17 中的四元数乘积定义）为

$$
q_{LG} = q_x \circ q_y = \begin{bmatrix}
0.6533 \
0.2706 \
0.2706 \
0.6533
\end{bmatrix}
$$

该旋转角度为 $\phi = 2 \arccos(q_0) = 2 \arccos(0.6533) = 98^\circ$，绕旋转轴 $e = \frac{1}{\sin(\phi/2)} [q_1, q_2, q_3]^T = [0.8630, 0.3574, 0.3574]^T$ 进行旋转。

4. 测量方向向量 $v_L = [0, 0, 1]^T$ 位于机器人坐标系中：

$$
v_G = R_{LG} v_L = [0.7071, -0.7071, 0]^T
$$

其中 $R_{LG} = (R_{LG})^{-1} = R_{LG}^T$。

通过四元数可得到相同结果，其中旋转后的向量为

$$
q_{vG} = (q_{LG})^{-1} \circ q_{vL} \circ q_{LG}
$$

其中 $q_{vL} = [0, v_L^T]^T$ 和 $q_{LG} = (q_{LG})^{-1}$（见公式 5.9）。四元数乘积的定义见公式 (5.17)。解为

$$
q_{vG} = \begin{bmatrix}
0.6533 \
0.2706 \
0.2706 \
0.6533
\end{bmatrix}
\circ
\begin{bmatrix}
0 \
0 \
0 \
1
\end{bmatrix}
\circ
\begin{bmatrix}
0.6533 \
-0.2706 \
-0.2706 \
-0.6533
\end{bmatrix}
= \begin{bmatrix}
0 \
0.7071 \
-0.7071 \
0
\end{bmatrix}
$$

即向量 $v_G = [0.7071, -0.7071, 0]^T$（仅考虑四元数的向量部分）。

示例 5.2

初始时全局坐标系 G 和局部坐标系 L 对齐；然后将坐标系 L 绕 $x$ 轴旋转角度 $\alpha_x = 90^\circ$，再绕新得到的 $z$ 轴旋转角度 $\alpha_z = 90^\circ$。回答以下问题：
1. 坐标系 L 相对于坐标系 G 的姿态由方向余弦矩阵（$R_{LG}$）如何表示？
2. 确定该变换的欧拉角（3-2-1 表示法）。
3. 确定描述该变换的四元数 $q_{LG}$。
4. 全局坐标中的向量为 $v_G = [0, 0, 1]^T$，其在局部坐标中的表示是什么？

解

1. 该旋转的 DCM 及其图形表示为

$$
R_{LG} = \begin{bmatrix}
0 & 0 & 1 \
-1 & 0 & 0 \
0 & -1 & 0
\end{bmatrix}
$$

2. 欧拉角（3-2-1）是

$$
\phi = \arctan\left( \frac{R_{23}}{R_{33}} \right) = \text{undefined}
$$
$$
\theta = -\arcsin(R_{13}) = -90^\circ
$$
$$
\psi = \arctan\left( \frac{R_{12}}{R_{11}} \right) = \text{undefined}
$$

请注意，$\theta = -90^\circ$，这意味着使用欧拉角的参数化是奇异的，因此 $\phi$ 和 $\psi$ 未定义。使用欧拉角无法描述此姿态（旋转）。

3. 四元数 $q_{LG}$ 是

$$
q_{LG} = q_x \circ q_z
$$

其中

$$
q_x = \begin{bmatrix}
0.7071 \
0.7071 \
0 \
0
\end{bmatrix}
$$

$$
q_z = \begin{bmatrix}
0.7071 \
0 \
0 \
0.7071
\end{bmatrix}
$$

and

$$
q_{LG} = q_x \circ q_z = \begin{bmatrix}
0.5 \
-0.5 \
0.5 \
0.5
\end{bmatrix}
$$

得到绕旋转轴 $e = \frac{1}{\sin(\phi/2)} [q_1, q_2, q_3]^T = [0.5774, -0.5774, 0.5774]^T$ 的旋转角度 $\phi = 2 \arccos(0.5) = 120^\circ$。

4. 向量 $v_G = [0, 0, 1]^T$ 在局部坐标中表示为

$$
v_L = R_{LG} v_G = [1, 0, 0]^T
$$

或使用四元数

$$
q_{vL} = (q_{LG})^{-1} \circ q_{vG} \circ q_{LG} = \begin{bmatrix}
0 \
1 \
0 \
0
\end{bmatrix}
$$

其中向量部分为 $v_L = [1, 0, 0]^T$。

5.2.2 平移和旋转

为了使表述更加通用，我们用 L（表示局部坐标）标记传感器坐标，用 G（表示全局坐标）标记机器人坐标系。传感器在机器人坐标系中的位置由平移向量 $t_{LG} = [t_x, t_y, t_z]$ 和旋转矩阵 $R_{LG}$ 描述。平移向量 $t_{LG}$ 表示局部坐标原点在全局坐标中的位置，而旋转矩阵 $R_{LG}$ 描述了局部坐标系相对于全局（机器人）坐标的姿态。一个在全局坐标中给出的点 $p_G$ 可以通过变换用局部坐标来描述。

$$
p_L = R_{LG}(p_G - t_{LG})
$$

及其逆变换由以下给出

$$
p_G = (R_{LG})^{-1} p_L + t_{LG} = (R_{LG})^T p_L + t_{LG}
$$

示例 5.3

机器人配备有一个激光测距仪（LRF）传感器，用于测量传感器坐标系中最近障碍物点 $p_L = [1, 0.5, 0.4]^T \text{m}$ 的位置。此外，机器人还配备有一个磁力计，用于测量地磁场向量 $v_L = [22, 1, 42] \text{nT}$。

激光测距传感器根据机器人（全局）坐标系沿 $t_1 = [0.1, 0, 0.25]^T$ 进行平移，并绕 $z$ 轴旋转 30 度（相对于机器人坐标系）。磁力计的平移为 $t_2 = [0, 0.1, 0.2]$，其旋转由横滚角、俯仰角和偏航角分别表示为 $\phi = 0^\circ$、$\theta = 10^\circ$ 和 $\psi = 20^\circ$（欧拉表示法 3-2-1）。

回答以下问题：
1. 在机器人坐标系中，最近的障碍物点坐标是什么？
2. 地球的磁性向量由机器人坐标系表示的向量是什么？

解

1. 方向余弦矩阵为

$$
R_{LG} = \begin{bmatrix}
\cos(30^\circ) & \sin(30^\circ) & 0 \
-\sin(30^\circ) & \cos(30^\circ) & 0 \
0 & 0 & 1
\end{bmatrix}
= \begin{bmatrix}
0.866 & 0.5 & 0 \
-0.5 & 0.866 & 0 \
0 & 0 & 1
\end{bmatrix}
$$

在机器人坐标系中表示的点为

$$
p_G = (R_{LG})^T p_L + t_1 = [0.716, 0.933, 0.65]^T
$$

2. 对于向量变换，只有旋转是重要的，因此机器人坐标系中的磁向量分量通过旋转变换获得

$$
R_{LG} = R_x(0^\circ) R_y(10^\circ) R_z(20^\circ)
= \begin{bmatrix}
0.9254 & 0.3368 & -0.1736 \
-0.3420 & 0.9397 & 0 \
0.1632 & 0.0594 & 0.9848
\end{bmatrix}
$$

$$
v_G = (R_{LG})^T v_L = [26.8705, 10.8443, 37.5417]^T
$$

5.2.3 旋转坐标系的运动学

本节将介绍由四元数或方向余弦矩阵表示的刚体姿态与刚体局部轴周围角速度之间的关系。刚体分别绕其 $x$、$y$ 和 $z$ 轴以角速度 $\omega_x$、$\omega_y$ 和 $\omega_z$ 旋转。因此，刚体（例如机器人或传感器单元）的姿态相对于参考坐标系发生变化。

由四元数表示的旋转运动学

刚体旋转的时间依赖性（由微分方程给出）可以从两个四元数的乘积定义（5.17）推导得出。如果已知刚体在时间 $t$ 时的姿态 $q(t)$，则其在时间 $t + \Delta t$ 的姿态可以表示为

$$
q(t + \Delta t) = q(t) \circ \Delta q(t)
$$

其中 $\Delta q(t)$ 定义了刚体姿态从 $q(t)$ 到 $q(t + \Delta t)$ 的变化。换句话说，$\Delta q(t)$ 是物体在时间 $t + \Delta t$ 相对于其在时间 $t$ 时的姿态。因此，物体的最终姿态 $q(t + \Delta t)$ 是通过首先根据某个参考坐标系进行旋转 $q(t)$，然后再根据 $q(t)$ 进行旋转 $\Delta q(t)$ 得到的。$\Delta q(t)$ 由关系式 (5.10) 定义。

$$
\Delta q(t) = \begin{bmatrix}
\cos(\Delta \phi / 2) \
e_x \sin(\Delta \phi / 2) \
e_y \sin(\Delta \phi / 2) \
e_z \sin(\Delta \phi / 2)
\end{bmatrix}
$$

其中 $e(t) = [e_x, e_y, e_z]^T$ 是刚体在时间 $t$ 时的局部坐标中表示的旋转轴，$\Delta \phi$ 是时间段 $\Delta t$ 内的旋转角度。假设 $e(t)$ 和 $\Delta \phi$ 在时间段 $\Delta t$ 内保持不变，则四元数乘积（5.34）可根据定义（5.17）重新表述如下：

$$
q(t + \Delta t) = \left( \cos\left( \frac{\Delta \phi}{2} \right) I + \sin\left( \frac{\Delta \phi}{2} \right) \begin{bmatrix}
0 & -e_x & -e_y & -e_z \
e_x & 0 & e_z & -e_y \
e_y & -e_z & 0 & e_x \
e_z & e_y & -e_x & 0
\end{bmatrix} \right) q(t)
$$

其中 $I$ 是一个 $4 \times 4$ 单位矩阵。在短时间间隔 $\Delta t$ 内，我们可以近似表示 $\Delta \phi \approx \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2} \Delta t$，其中 $\omega(t) = [\omega_x, \omega_y, \omega_z]^T$ 是当前角速度矢量，也可写作 form $\omega(t) = \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2} e$。对于小角度，公式 (5.36) 可近似为

$$
q(t + \Delta t) \approx (I + \frac{\Delta t}{2} \Omega) q(t)
$$

其中

$$
\Omega = \begin{bmatrix}
0 & -\omega_x & -\omega_y & -\omega_z \
\omega_x & 0 & \omega_z & -\omega_y \
\omega_y & -\omega_z & 0 & \omega_x \
\omega_z & \omega_y & -\omega_x & 0
\end{bmatrix}
$$

通过将 $\Delta t$ 趋近于零，可以得到描述刚体姿态的微分方程：

$$
\frac{dq}{dt} = \lim_{\Delta t \to 0} \frac{q(t + \Delta t) - q(t)}{\Delta
t} = \frac{1}{2} \Omega q
$$

其中 $\Omega$ 中的角速度是在刚体坐标系中给出的。

用方向余弦矩阵表示的旋转运动学

刚体姿态由方向余弦矩阵表示的微分方程推导如下。类似式 (5.34) 定义，

$$
R(t + \Delta t) = \Delta R(t) R(t)
$$

其中 $R(t)$ 是刚体在时刻 $t$ 的姿态，$R(t + \Delta t)$ 是时刻 $t + \Delta t$ 的姿态，而 $\Delta R(t)$ 是相对于时刻 $t$ 的姿态而言，在时刻 $t = t + \Delta t$ 的姿态变化（即刚体在该时刻的姿态）。

姿态变化 $\Delta R(t)$ 定义为

$$
\Delta R(t) = e^{\int_{t}^{t + \Delta t} \Omega’ dt}
$$

其中

$$
\Omega’ = \begin{bmatrix}
0 & \omega_z & -\omega_y \
-\omega_z & 0 & \omega_x \
\omega_y & -\omega_x & 0
\end{bmatrix}
$$

且 $\omega(t) = [\omega_x, \omega_y, \omega_z]^T$ 是当前角速度的向量。

假设 $\Omega’$ 是时间段内的常数矩阵 $\Delta t$ 我们可以近似 $\int_{t}^{t + \Delta t} \Omega’(t) dt \approx \Omega’ \Delta t = B$。关系式 (5.41) 中的指数项用泰勒级数表示为

$$
\Delta R(t) = e^B
= (I + B + \frac{B^2}{2!} + \frac{B^3}{3!} + \cdots)
= (I + B + \frac{B^2}{2!} - \sigma^2 \frac{B}{3!} - \sigma^2 \frac{B^2}{4!} + \sigma^4 \frac{B}{5!} + \cdots)
= \left(I + \frac{\sin \sigma}{\sigma} B + \frac{1 - \cos \sigma}{\sigma^2} B^2\right)
$$

其中 $I$ 是 $3 \times 3$ 单位矩阵，且 $\sigma = \Delta t \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2}$。对于小角度 $\sigma$ 关系式 (5.43) 近似为

$$
\Delta R(t) = I + B = I + \Omega’ \Delta t
$$

可用于预测旋转矩阵（5.40）：

$$
R(t + \Delta t) = (I + \Omega’ \Delta t) R(t)
$$

通过将 $\Delta t$ 趋近于零，得到最终的微分方程：

$$
\frac{dR}{dt} = \lim_{\Delta t \to 0} \frac{R(t + \Delta t) - R(t)}{\Delta t}
= \Omega’ R
$$

为了完整性，还给出了使用欧拉角（3-2-1 表示法）进行旋转参数化的等效表达：

$$
\dot{\phi} = \omega_x + \omega_y \sin \phi \tan \theta + \omega_z \cos \phi \tan \theta
$$
$$
\dot{\theta} = \omega_y \cos \phi - \omega_z \sin \phi
$$
$$
\dot{\psi} = \omega_y \sin \phi + \omega_z \cos \phi \cos \theta
$$

可以看出，该符号表示变得奇异（第一个和第三个方程在式 (5.47) 中当 $\theta = \pm \pi/2$ 时）。

5.2.4 射影几何

投影是将一个具有 $N > 0$ 尺寸的空间变换到另一个具有 $M < N$ 尺寸的空间的过程。通常情况下，在一般射影变换下，某些信息不可避免地会丢失。然而，如果有多个关于同一物体的投影，则在某些情况下可以重建该物体在 $N$ 维空间中的原始形态。两种最常见的投影变换是透视投影和平行投影（焦点位于无穷远处的线性变换）。

根据针孔相机模型，位于 $p_C = [x_C, y_C, z_C]$ 的相机坐标系 $C$ 中的三维点 $p_P = [x_P, y_P]$ 被投影到图像（图片）坐标系 $P$ 中的二维点图 5.1 所示：

$$
p_P = \frac{1}{z_C} S p_C
$$

其中 $p_P$ 是点 $p_P$ 在齐次坐标下的表示，即 $p_P^T = [x_P, y_P, 1]$。矩阵 $S \in \mathbb{R}^{3 \times 3}$ 描述了相机内部模型：

$$
S = \begin{bmatrix}
\alpha_x f & \gamma & c_x \
0 & \alpha_y f & c_y \
0 & 0 & 1
\end{bmatrix}
$$

包含在 $S$ 中的相机内参是焦距 $f$；水平和垂直方向的缩放因子分别为 $\alpha_x$ 和 $\alpha_y$；光轴图像中心 $(c_x, c_y)$；以及倾斜度 $\gamma$。相机模型参数通常在相机标定过程中获得或优化。由于项 $z_C^{-1}$（物体沿相机坐标系 $z$ 轴方向的距离的倒数）的存在，透视相机模型（5.48）是非线性的。尽管点、直线和一般二次曲线在透视变换下保持不变（即点变换为点，直线变换为直线，二次曲线变换为二次曲线），但投影图像却是对现实的一种失真表示。一般来说，线之间的角度和距离比例并不保持不变（例如，平行线不会变换为平行线）。在某些特定的相机配置下，透视投影可以用适当的线性模型来近似；这可以实现简单的相机标定。相机模型（5.48）也可以写成

$$
p_P \propto S p_C
$$

物体的图像形成于相机光学中心后方的屏幕上（沿负 $z_C$ 半轴方向，距离为焦距 $f$），并且旋转了 180 度并缩小。在创建相机模型的视觉表示时，可以假设一个虚拟的未旋转图像形成于相机光学中心前方（在正 $z_C$ 半轴上，与光学中心的距离和实际图像相同），如图 5.1 所示。

示例 5.4

相机内部模型（5.49）具有以下参数：$\alpha_x f = \alpha_y f = 1000$，无倾斜度（$\gamma = 0$），且图像中心位于尺寸为 $1024 \times 768$ 的图像中央。给定相机坐标系，将以下三维点集投影到图像上：

$$
p_C^T \in {[-1, 1, 4], [1, 1, 5], [0, -1, 4], [-1, 1, -4], [4, 1, 5]}
$$

解 根据公式 (5.48) 将三维点投影到图像的过程在列表 5.1 中给出。结果也以图形方式显示在图 5.2 的左侧。注意，最后一个点未出现在图像中，因为它超出了图像屏幕边界。此外，第四个点也不应出现在图像中，因为该点位于相机后方。请记住，数学模型 (5.48) 未考虑可见性约束。因此，需要额外进行检查，以确保只有可见的点被投影到图像上：即投影点必须位于图像边界内，并且待投影的点必须位于相机前方。因此，实际上只有前三个点出现在图像中（见图 5.2 的右侧）。

示例 5.4（续）

列表 5.1 解 示例 5.4

% 内在相机参数和图像屏幕尺寸
alphaF = 1000; % alpha*f，单位为像素/米
s = [1024; 768]; % 屏幕尺寸，单位为像素
c = s/2; % 图像中心，单位为像素
S = [alphaF, 0, c(1); 0, alphaF, c(2); 0, 0, 1]; % 相机模型

% 相机坐标系中的 3-D 点集
pC = [-1 1 4; 1 1 5; 0 -1 4; -1 1 -4; 4 1 5].';

% 点到图像坐标系的投影
pP = (S * pC) ./ repmat(pC(3,:), 3, 1)

$$
p_P = \begin{bmatrix}
262 & 712 & 512 & 762 & 1312 \
634 & 584 & 134 & 134 & 584 \
1 & 1 & 1 & 1 & 1
\end{bmatrix}
$$

由公式 (5.48) 可知，三维空间中的每个点都被变换到二维空间中的一个点，但逆变换并不成立。由于透视变换会导致场景深度的丢失，如果没有额外的信息，图像空间中的点只能反投影到三维空间中的一条射线。如果能够以某种方式恢复场景深度，则可以重建该场景。存在多种基于深度相机、结构光、视觉线索、运动等技术来实现三维重建。此外，还可以通过从多个（至少两个）视图获取的三维点对应图像来恢复三维场景中点的位置。因此，使用立体相机可以实现三维重建。

多视图几何

多视图几何的重要性不仅在于它能够实现场景重建，还在于其某些性质可以被用于机器视觉算法的设计（例如图像点匹配和基于图像的相机位姿估计）。假设旋转矩阵 $R_{C1}^{C2}$ 和平移向量 $t_{C1}^{C2}$ 描述了两个相机之间的相对位姿（图 5.3）。在假设相机中心不重合（$t_{C1}^{C2} \neq 0$）的前提下，经过简短的数学推导可得到关系式（5.52）（假设相机具有相同的内部参数）：

$$
p_{C1} = R_{C1}^{C2} p_{C2} + t_{C1}^{C2},
$$

$$
S^{-1} p_{P1} \propto R_{C1}^{C2} S^{-1} p_{P2} + t_{C1}^{C2}
$$
$$
[t_{C1}^{C2}] \times S^{-1} p {P1} \propto [t_{C1}^{C2}] \times R {C1}^{C2} S^{-1} p_{P2}
$$
$$
0 = p_{P1}^T S^{-T} [t_{C1}^{C2}] \times R {C1}^{C2} S^{-1} p_{P2}
$$
$$
0 = p_{P1}^T F p_{P2}
$$

请注意，向量 $a^T = [a_1, a_2, a_3]$ 和 $b^T = [b_1, b_2, b_3]$ 之间的叉积写作 $a \times b = [a] \times b$，其中 $[a] \times$ 是一个反对称矩阵：

$$
[a]_\times = \begin{bmatrix}
0 & -a_3 & a_2 \
a_3 & 0 & -a_1 \
-a_2 & a_1 & 0
\end{bmatrix}
$$

矩阵 $F$ 被称为基础矩阵，用于描述对极约束（5.52）：$p_{P1}$ 位于第二幅图像中的直线 $F p_{P2}$ 上，而点 $p_{P2}$ 位于第一幅图像中的直线 $F^T p_{P1}$ 上。机器视觉中的另一个重要关系是 $p_{C1}^T E p_{C2} = 0$，其中 $E = [t_{C1}^{C2}] \times R {C1}^{C2}$ 被称为本质矩阵。本质矩阵与基础矩阵之间的关系是 $E = S^T F S$。

在已知两个标定相机之间的位姿的情况下，对极约束可用于改进属于同一世界点的多个图像中图像点的匹配。由于第一幅图像中点 $p_{P1}$ 的对应点对必须位于第二幅图像中的直线 $F^T p_{P1}$ 上，因此在整个图像区域上的二维搜索被简化为一维沿极线的搜索，从而可以显著加快匹配过程，并且还可以提高鲁棒性，因为不满足对极约束的匹配将被拒绝。

例 5.5 三维点从两个相机进行观测，其相对位移由旋转矩阵 $R_{C1}^{C2} = R_x(30^\circ) R_y(60^\circ)$ 和平移向量 $t_{C1}^{C2} = [4, -1, 2]^T$ 描述。两个相机具有相同的相机内部模型（5.49）：$\alpha_x f = \alpha_y f = 1000$，无倾斜度（$\gamma = 0$），且图像中心位于尺寸为 $1024 \times 768$ 的图像中央。投影到第一相机图像中的一组投影点为

$$
p_{P1}^T \in {[262, 634], [762, 634], [512, 134], [443, 457], [412, 284]}
$$

以及一组投影到第二相机图像中的点是

$$
p_{P2}^T \in {[259, 409], [397, 153], [488, 513], [730, 569], [115, 214]}
$$

两幅图像中的图 5.4 展示了这些点。考虑对极约束，确定所有可能的图像点对应关系。对于所有匹配点对，重建这些点在三维空间中相对于两个相机坐标系的位置

当解匹配两台相机图像间的点时，若它们之间的相对位姿已知，则属于同一世界点的点必须满足对极约束 (5.52)。式 (5.52) 的形式为 $l^T p = 0$，即齐次点 $p^T = [x, y, 1]$ 位于直线 $l^T = [a, b, c]$ 上，因为 $ax + by + c = 0$ 是直线方程。因此，可以利用对极约束 (5.52) 来求出第一幅图像中每个点 $p_{P1}$ 在第二幅图像中的对应极线 $l_{P2}$，反之亦然，也可求出第二幅图像中每个点 $p_{P2}$ 在第一幅图像中的极线 $l_{P1}$。第一幅和第二幅图像的极线如图 5.5 所示。从图 5.4 和图 5.5 中可直观地找到四种可能的点匹配。

示例 5.5—续

程序清单 5.2 来自 例 5.5 的点匹配

% 相机内参和图像屏幕尺寸
alphaF = 1000; % alpha*f，单位为像素/米
s = [1024; 768]; % 屏幕尺寸，单位为像素
c = s/2; % 图像中心，单位为像素
S = [alphaF, 0, c(1); 0, alphaF, c(2); 0, 0, 1]; % 相机模型

% 相机之间的相对位姿
R = rotX(pi/6) * rotY(pi/3); t = [4; -1; 2];

% 点集
pP1 = [262, 634; 762, 634; 512, 134; 443, 457; 412, 284].';
pP2 = [259, 409; 397, 153; 488, 513; 730, 569; 115, 214].';

N1 = size(pP1, 2); N2 = size(pP2, 2); % 点的数量

% 基础矩阵
tx = [0, -t(3), t(2); t(3), 0, -t(1); -t(2), t(1), 0];
F = S.' \ tx * R / S;

epsilon = 1e-2; % 距离误差容限
pP1 = [pP1; ones(1, N1)]; pP2 = [pP2; ones(1, N2)]; % 齐次坐标点

% P2 帧中点对应的 P1 帧中的极线
lP1 = F * pP2;
% P1 帧中点对应的 P2 帧中的极线
lP2 = F.' * pP1;

% 查找点对（在 P2 帧中评估基础约束）
pairs = [];
for i = 1:N1
    d = abs(lP2(:, i).' * pP2);
    k = find(d < epsilon);
    if ~isempty(k), pairs = [pairs, [i; k(1)]]; end
end

$$
\text{pairs} = \begin{bmatrix}
1 & 2 & 3 & 5 \
3 & 4 & 2 & 1
\end{bmatrix}
$$

奇异情况

关系（5.52）在相机中心之间无平移时变为奇异。在这种情况下，若将 $t_{C1}^{C2}$ 设为零，可由式 (5.51) 得到以下关系：

$$
p_{P1} \propto S R_{C1}^{C2} S^{-1} p_{P2}
$$

在三维空间中的所有点都被限制在单一公共平面的情况下，也可以得到类似形式的方程。不失一般性，可选择平面 $z_W = 0$：

$$
p_{P1} \propto S [r_{C1}^{W,1}, r_{C1}^{W,2}, t_{C1}^{W}] [r_{C2}^{W,1}, r_{C2}^{W,2}, t_{C2}^{W}]^{-1} S^{-1} p_{P2}
$$

其中 $R_{C}^{W} = [r_{C}^{W,1}, r_{C}^{W,2}, r_{C}^{W,3}]$。从世界平面到图像平面的变换是

$$
p_P \propto S [r_{C}^{W,1}, r_{C}^{W,2}, t_{C}^{W}] p_W
$$

其中在此特定情况下 $p_W^T = [x_W, y_W, 1]$。方程 (5.54)–(5.56) 均具有相似的形式：$p’ \propto H p$。矩阵 $H$ 被称为单应性矩阵。

三维重建

在双目相机配置中，可以根据点的两幅图像重建其三维位置。该过程需要在不同视角中找到对应的图像点，这是机器视觉中的基本问题之一。但一旦建立了点对应关系，并且已知相机视角之间的相对位姿（相机间的平移必须不为零）以及相机内部模型，则可以估计该点的三维位置。如果两个相机模型相同，则可以通过求解以下方程组（例如，使用最小二乘法）来估计该点在两个相机坐标系中的点深度（$z_{C1}$ 和 $z_{C2}$）：

$$
S^{-1} \frac{p_{P1}}{z_{C1}} - R_{C1}^{C2} S^{-1} \frac{p_{P2}}{z_{C2}} = t_{C1}^{C2}
$$

示例 5.6 对于来自例 5.5 的两幅图像之间的所有获得的点匹配，重建世界点相对于第一和第二相机坐标系的三维位置。

解 如果求解例 5.5 中每一对匹配的图像点对应的式 (5.57) 的方程组，并将解代入式 (5.48) 的逆变换，则可获得点的三维位置。该解法在 Matlab 中的实现见清单 5.3。重建的三维点在第一和第二相机坐标系中分别保存到变量 $p_{C1}$ 和 $p_{C2}$ 中。

清单 5.3 来自 示例 5.5 的三维点重建

M = size(pairs, 2);
pC1 = zeros(3, M);
for i = 1:M
    a = pairs(1, i); b = pairs(2, i);
    c1 = S \ pP1(:, a);
    c2 = -R * (S \ pP2(:, b));
    psi = [c1, c2] \ t;
    pC1(:, i) = psi(1) * c1;
end
pC2 = R.' * (pC1 - repmat(t, 1, M))

$$
p_{C1} = \begin{bmatrix}
-0.9989 & 0.9994 & 0 & -0.2999 \
0.9989 & 0.9994 & -1.0005 & -0.2999 \
3.9956 & 3.9978 & 4.0018 & 2.9994
\end{bmatrix}
$$

$$
p_{C2} = \begin{bmatrix}
-0.1372 & 0.8638 & -0.4988 & -1.0973 \
0.7333 & 0.7327 & -1.0013 & 0.1066 \
5.6930 & 3.9635 & 4.3308 & 4.3316
\end{bmatrix}
$$

在两个相机的规范立体配置中，重建问题得以简化，其中一个相机坐标系仅沿 $x$ 轴平移基线距离 $b$（如图 5.6 所示），因为极线是平行，且点 $p_{P1}$ 的极线经过该点（不仅是在另一幅图像中的点 $p_{P2}$）。

对于数字图像而言，这意味着第一幅图像中某一点的匹配点位于第二幅图像的同一图像行中。为简化起见，在相机模型（5.49）中设 $\alpha_x = \alpha_y = \alpha$ 和 $\gamma = 0$。然后可根据世界点的两幅图像得到该三维世界点在第一相机坐标系中的位置

$$
p_{C1}^T = \frac{b}{d} [x_{P1} - c_x, y_{P1} - c_y, \alpha f]
$$

其中视差被定义为 $d = x_{P1} - x_{P2}$。视差包含有关场景深度的信息，这一点可以从公式 (5.58) 的最后一个元素看出：$z_{C1} = \frac{\alpha f b}{d}$。对于所有位于相机前方的点，视差为正，$d \geq 0$。

示例 5.7 假设一个具有基线距离 $b = 0.2$ 的标准双目相机配置，以及以下内部相机模型参数：$\alpha_x f = \alpha_y f = 1000$，$c_x = 512$ 和 $c_y = 384$。重建投影到第一和第二相机图像中点 $p_{P1}^T = [351, 522]$ 和 $p_{P2}^T = [236, 522]$ 的三维点的位置。

解 由于这是标准双目相机配置，解可以直接从公式 (5.58) 获得（参见程序清单 5.4）。

程序清单 5.4 来自 例 5.5 的三维点重建

% 相机内参和图像屏幕尺寸
alphaF = 1000; % alpha*f，单位为像素/米
c = [512; 384]; % 图像中心，单位为像素
S = [alphaF, 0, c(1); 0, alphaF, c(2); 0, 0, 1]; % 相机模型

b = 0.2; % 双目相机基线，单位为米

% 匹配点
pP1 = [351; 522];
pP2 = [236; 522];

% 三维重建
d = pP1(1) - pP2(1);
pC1 = b/d * [pP1 - c; alphaF]

$$
p_{C1} = \begin{bmatrix}
-0.2800 \
0.2400 \
1.7391
\end{bmatrix}
$$

5.3 位姿测量方法

下文介绍了主要的方法论，展示了如何利用传感器来估计机器人在环境中的位姿。这些方法能够根据先前确定的位姿测量相对位姿变化，或根据基于参考的坐标系测量绝对位姿。

5.3.1 航位推算

航位推算（也称为推算定位）方法通过已知的先前姿态以及从该先前姿态测得的相对位移来估计机器人（配备相对传感器）当前的姿态。这些位移或运动增量（距离和方向）是根据经过的时间内测得的速度和航向计算得出的。这类方法的共同点是使用路径积分来估计当前姿态；因此，通常会出现各种误差的累积（如积分方法误差、测量误差、偏差、噪声等）。

移动机器人技术中使用的两种基本方法是里程计和惯性导航。

里程计

里程计用于通过积分可测量或从执行的运动指令中获取的运动增量来估计机器人位姿。在移动机器人技术中，相对运动增量通常来自安装在机器人车轮上的轴传感器（例如，增量式编码器）。利用内部运动学模型（参见公式 2.1 中的差速驱动模型），这些车轮旋转测量值与移动机器人的位置和姿态变化相关联。在已知的时间段内连续测量之间得到的位置和姿态变化也可以表示为机器人速度。在某些情况下，车轮角速度可以直接测量，或从已知的速度指令推断得出（假设实现的速度控制器是准确的）。

假设有一个采用差速驱动的机器人，其车轮上安装有增量式编码器传感器。测量值给出了左右车轮相对于此前在时间 $t^- = t - \Delta t$ 时刻姿态的相对旋转变化，即 $\Delta \alpha_L(t)$ 和 $\Delta \alpha_R(t)$。假设车轮为纯滚动，则行驶距离为

$$
\Delta d_L(t) = \Delta \alpha_L(t) R, \quad \Delta d_R(t) = \Delta \alpha_R(t) R
$$

其中 $R$ 是车轮半径。利用内部运动学模型（2.1），姿态位移和行驶距离（位置位移）为

$$
\Delta \phi(t) = \frac{\Delta d_R(t) - \Delta d_L(t)}{L} \quad \text{and} \quad \Delta d(t) = \frac{\Delta d_R(t) + \Delta d_L(t)}{2}
$$

其中 $L$ 是轮距。

机器人位姿估计通过对测量的机器人速度进行外部运动学模型（2.2）的积分获得，或通过对计算的机器人位置和方向位移求和获得。应用梯形数值积分，估计的机器人位姿为

$$
x(t) = x(t^-) + \Delta d(t) \cos\left(\phi(t^-) + \frac{\Delta \phi(t)}{2}\right)
$$
$$
y(t) = y(t^-) + \Delta d(t) \sin\left(\phi(t^-) + \frac{\Delta \phi(t)}{2}\right)
$$
$$
\phi(t) = \phi(t^-) + \Delta \phi(t)
$$

然而，由于里程计具有积分特性，会产生累积误差。误差的主要来源包括系统性误差和非确定性误差源。前者包括由近似运动学模型（如车轮半径错误）引起的误差、所采用积分方法的精度误差以及测量误差（未知偏差）；后者包括车轮打滑、噪声等。因此，里程计的独立使用仅适用于在已知初始位姿下的短期位姿估计。更常见的情况是，将里程计与绝对传感器提供的位姿测量相结合，其中里程计用于对绝对位姿测量进行预测和滤波，以获得更优的位姿估计。

在轮式移动系统中，里程计中使用的传感器通常是安装在车轮轴上的轴传感器（例如，增量式光学或磁性编码器、电位器），用于测量旋转角度或速度。

惯性导航

惯性导航系统（INS）是一种自主式技术，通过航位推算来估计车辆位置、姿态和速度。INS 包括运动传感器（加速度计）和旋转传感器（陀螺仪），其中车辆的位置和姿态相对于已知的初始姿态进行估计。

加速度计和陀螺仪的输出是空间中加速度和角速度的三维向量。为了估计机器人位置和姿态，需要对加速度测量值进行双重积分，并对角速度进行单次积分。由于恒定（系统性）误差（如传感器漂移、传感器校准不良等）的累积，积分的使用是惯性导航系统位姿误差的主要来源。恒定测量误差导致位置估计误差随时间呈二次增长，而姿态估计误差则随时间线性增长。此外，惯性导航系统中的位置估计误差依赖于姿态估计误差，因为加速度计测量的是包括重力在内的总加速度。通过从加速度测量值中减去重力加速度来获得车辆（惯性导航系统单元）的平动加速度，这需要精确知道车辆的姿态。因此，任何姿态误差都会因重力补偿不准确而导致虚拟加速错误。这一点尤其成问题，因为车辆加速度通常远小于重力加速度。在较低加速度下信噪比（SNR）较小，会引起额外的问题。这些效应可以从加速度传感器模型中明显看出，该模型包括车辆平动加速度 $a$、重力 $g = [0, 0, 0.981]^T$ 以及径向加速度。

$$
a_m = R_w^i (a + R_w^e g + \omega \times v) + a_{\text{bias}} + a_{\text{noise}}
$$

其中，$a_m$ 是传感器在其局部坐标系中的加速度测量值，$R_w^e$ 是地球坐标系与执行姿态跟踪的世界坐标系之间的旋转矩阵，$R_w^i$ 是局部惯性导航系统坐标系到世界坐标系的旋转矩阵，$\omega$ 是世界坐标系中的角速度，$v$ 是平移速度，$a_{\text{bias}}$ 是加速度偏差，而 $a_{\text{noise}}$ 是噪声。

为了估计姿态，使用了陀螺仪测量。陀螺仪传感器的模型是

$$
\omega_m = \omega_i + \omega_{\text{bias}} + \omega_{\text{noise}}
$$

其中 $\omega_m$ 是测量的角速度向量，$\omega_i$ 是物体在局部坐标系中的真实角速度向量，$\omega_{\text{bias}}$ 是传感器偏差，$\omega_{\text{noise}}$ 是传感器噪声。

为了估计惯性导航系统单元的姿态，该单元的角速度估计值由公式 (5.61) 得到如下：$\omega_i = \omega_m - \omega_{\text{bias}}$。有偏差的部分通常通过某种估计器（例如卡尔曼滤波器）在线估计。例外情况下，如果使用高质量的陀螺仪并进行了初始校准（$\omega_{\text{bias}}$ 的估计），在较短的估计周期内，可以假设偏差为常数。然而需要注意的是，偏差会随时间变化。因此，后一种方法仅适用于短期使用。最简单的校准方法是在保持惯性导航系统单元位置不变（$\omega_{\text{bias}} = \frac{1}{N} \sum_{i=1}^{N} \omega_m$，当 $\omega_i = 0$ 时）的情况下，对陀螺仪测量值（$N$ 次测量）进行平均。

然后利用四元数的旋转运动学（5.39）根据初始姿态获得惯性导航系统单元姿态的相对估计值：

$$
\frac{dq_w^i(t)}{dt} = \frac{1}{2} \Omega(t) q_w^i(t)
$$
$$
q_w^i(t) = \int_{0}^{t} \frac{dq_w^i(t)}{dt} dt + q_w^i(0)
$$

其中 $q_w^i(t)$ 是描述从世界坐标系（w）到惯性导航系统单元坐标系（i）的旋转的四元数，$q_w^i(0)$ 为初始姿态，

$$
\Omega(t) = \begin{bmatrix}
0 & -\omega_x & -\omega_y & -\omega_z \
\omega_x & 0 & \omega_z & -\omega_y \
\omega_y & -\omega_z & 0 & \omega_x \
\omega_z & \omega_y & -\omega_x & 0
\end{bmatrix}
$$

和 $\omega_i(t) = [\omega_x, \omega_y, \omega_z]^T$。为了获得方向余弦矩阵 $R_i^w = (R_w^i)^T$，使用关系式（5.18）。在实现公式（5.63）时，可以采用数值积分（5.36）。

为了估计惯性导航系统单元的位置，通过公式（5.60）计算世界坐标系中的平移加速度：

$$
a = R_w^i (a_m - a_{\text{bias}}) - R_w^e g - \omega \times v
$$

其中，世界坐标系中的角速度向量为 $\omega = R_w^i \omega_i$，偏差项 $a_{\text{bias}}$ 需要通过某种估计器（例如卡尔曼滤波器）在线估计。同时，还需要进行适当的加速度传感器校准。速度估计 $v(t)$ 和位置 $x(t)$ 通过积分获得：

$$
v(t) = \int_{0}^{t} a(t) dt + v(0)
$$
$$
x(t) = \int_{0}^{t} v(t) dt + x(0)
$$

示例 5.8 为带有惯性导航系统单元的差速机器人仿真测量加速度和角速度，该机器人沿曲线 $x(t) = \cos(t)$、$y(t