#2 SLAM_Modeling motion（运动建模）

基本模型

一般的时态预测模型可以写成以下形式：

$$\mathbf{x}_{n}=f_{n}\left(\mathbf{x}_{n-1}, \mathbf{u}_{n}, \mathbf{i}\right), \quad \mathbf{i} \sim \mathcal{N}\{0, \mathbf{Q}\}$$

其中的参数定义描述为：

• $\mathbf{x}_{n}$：是在$t_{n}\triangleq n \Delta t$时的状态空间。
• $\mathbf{x}_{n-1}$：上一个空间状态。
• $f_{n}$：一般是非线性函数
• $\mathbf{u}_{n}$：是运动控制信号
• i：是个扰动期望轨迹的随机脉冲矢量，一般认为是高斯协方差Q

为了方便表述和简化符号，我们通常会引入了箭头赋值操作符"←"，意思是左边的项被更新为右边操作的结果：

$$\mathbf{x} \leftarrow f(\mathbf{x}, \mathbf{u}, \mathbf{i}), \quad \mathbf{i} \sim \mathcal{N}\{0, \mathbf{Q}\}$$

除了运动模型，我们还要计算估计的运动误差e，使我们能够去最小化误差量。而误差可以用不同的公式定义，选择在简单性和可行性下进行。这里我选择较为简化的模型：

$$\mathbf{e}=h\left(\mathbf{x}_{n-1}, \mathbf{x}_{n}, \mathbf{u}_{n}\right)$$

其中的参数定义描述为：

• $\mathbf{u}_{n}$：某些运动测量量，例如里程计、IMU等
• $\mathbf{x}_{n}$和$\mathbf{x}_{n-1}$：分别是当前和上一个状态空间
• h()：则是误差函数

状态空间中的运动误差

在多数情况下，扰动脉冲向量 i 在状态空间中被认为是可加的：

$$\mathbf{x}_{n}=f_{x}\left(\mathbf{x}_{n-1}, \mathbf{u}_{n}\right)+\mathbf{i}_{x}, \quad \mathbf{i}_{x} \sim \mathcal{N}\left\{0, \mathbf{Q}_{x}\right\}$$

通过利用雅可比矩阵的线性近似与一般形式联系起来：

$$f(\mathbf{x}, \mathbf{u}, \mathbf{i}), \quad \mathbf{F}_{\mathbf{i}}=\left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\mathbf{x}, \mathbf{u}, 0}$$

可知：

$$\mathbf{x}_{n}=f\left(\mathbf{x}_{n-1}, \mathbf{u}_{n}, 0\right)+\mathbf{i}_{x}, \quad \mathbf{i}_{x}=\mathbf{F}_{\mathbf{i}} \mathbf{i} \sim \mathcal{N}\left\{0, \mathbf{Q}_{x}\right\}, \quad \mathbf{Q}_{x}=\mathbf{F}_{\mathbf{i}} \mathbf{Q} \mathbf{F}_{\mathbf{i}}^{\top}$$

由于扰动在状态空间中产生，因此运动误差可以改写为：

$$\begin{aligned} \mathbf{e}_{x}=h_{x}\left(\mathbf{x}_{n-1}, \mathbf{x}_{n}, \mathbf{u}_{n}\right) & \triangleq \mathbf{x}_{n}-f_{x}\left(\mathbf{x}_{n-1}, \mathbf{u}_{n}\right) \\ &=\mathbf{x}_{n}-f\left(\mathbf{x}_{n-1}, \mathbf{u}_{n}, 0\right) \sim \mathcal{N}\left\{0, \mathbf{Q}_{x}\right\} \end{aligned}$$

控制空间中的运动误差

摄动脉冲被认为在控制空间中是可加的话：

$$\mathbf{x} \leftarrow f_{u}\left(\mathbf{x}, \mathbf{u}+\mathbf{i}_{u}\right), \quad \mathbf{i}_{u} \sim \mathcal{N}\left\{0, \mathbf{Q}_{u}\right\}$$

这样的扰动有一个定义明确的协方差矩阵Qu，它允许我们在控制空间中反推来表示运动误差，

$$\mathbf{e}_{u}=h_{u}\left(\mathbf{x}_{n-1}, \mathbf{x}_{n}, \mathbf{u}_{n}\right) \triangleq \mathbf{u}-f_{u}^{-1}\left(\mathbf{x}_{n-1}, \mathbf{x}_{n}\right) \sim \mathcal{N}\left\{0, \mathbf{Q}_{u}\right\}$$

摄动空间中的运动误差

当摄动在状态空间或控制空间中都不实际时，我们需要找到一种方法来表示运动误差。在摄动空间中直接表示这些误差也是有意义的，这样就可以用适当的协方差矩阵Q保持噪声定义，

$$\mathbf{e}_{i}=h_{i}\left(\mathbf{x}_{n-1}, \mathbf{x}_{n}, \mathbf{u}_{n}\right) \triangleq f_{i}^{-1}\left(\mathbf{x}_{n-1}, \mathbf{x}_{n}, \mathbf{u}_{n}\right) \sim \mathcal{N}\{0, \mathbf{Q}\}$$

对运动误差定义的要求

我们要求，误差的协方差必须与运动模型的自由度数具有相同的秩。这有两个影响：

• 运动误差e的协方差矩阵Q的最终形式必须是满秩的：

$$\operatorname{rank}(\mathbf{Q})=\operatorname{dim}(\mathbf{e})$$

• 运动模型中的所有自由度都必须有一个非零的不确定性编码在协方差矩阵中：

$$\operatorname{rank}(\mathbf{Q})=\operatorname{DoF}(f(\mathbf{x}, \mathbf{u}))$$

常见的运动模型

$$\mathbf{x}_{n}=f_{n}\left(\mathbf{x}_{n-1}, \mathbf{u}_{n}, \mathbf{i}\right), \quad \mathbf{i} \sim \mathcal{N}\{0, \mathbf{Q}\}$$

以上是运动模型的一般形式是，而常见的运动模型，则指定了状态向量 x 的内容，控制向量 u ， 扰动向量 i ，以及实现模型 f 的非线性代数。

等速度模型和变量

当没有控制信号可用时比较有用。例如，手持扫描。

匀速情况下

在没有扰动的情况下，它相当于空间中一个自由物体的运动，完全不受任何力的影响，甚至不受重力的影响。力的作用是未知的，包含在摄动项 $v_{i}$ 和 $w_{i}$ 中。

$$\mathbf{x}=\left[\begin{array}{l} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \boldsymbol{\omega} \end{array}\right], \quad \quad \mathbf{i}=\left[\begin{array}{l} \mathbf{v}_{i} \\ \boldsymbol{\omega}_{i} \end{array}\right]$$

$$\begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\mathbf{v} \Delta t \\ \mathbf{v} \leftarrow \mathbf{v}+\mathbf{v}_{i} \\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\{\omega \Delta t\} \\ \boldsymbol{\omega} \leftarrow \boldsymbol{\omega}+\omega_{i} \end{array}$$

在这里，我们使用：

• 四元数$\mathbf{q}$来表示三维空间中的方向。
• $\mathbf{p}$为位置空间
• $\mathbf{v}$为线速度
• $\mathbf{\omega }$为角速度
• 以及摄动项 $v_{i}$ 和 $w_{i}$

匀加速情况下

当运动非常平滑时很有用，因为这里不允许加速度突然改变。

$$\mathbf{x}=\left[\begin{array}{l} \mathbf{p} \\ \mathbf{v} \\ \mathbf{a} \\ \mathbf{q} \\ \boldsymbol{\omega} \\ \boldsymbol{\alpha} \end{array}\right], \quad \mathbf{i}=\left[\begin{array}{l} \mathbf{a}_{i} \\ \boldsymbol{\alpha}_{i} \end{array}\right]$$

$$\begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\mathbf{v} \Delta t+\frac{1}{2} \mathbf{a} \Delta t^{2}\\ \mathbf{v} \leftarrow \mathbf{v}+\mathbf{a} \Delta t\\ \mathbf{a} \leftarrow \mathbf{a}+\mathbf{a}_{i}\\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\boldsymbol{\omega} \Delta t+\frac{1}{2} \boldsymbol{\alpha} \Delta t^{2}\right\}\\ \omega \leftarrow \omega+\alpha \Delta t\\ \boldsymbol{\alpha} \leftarrow \boldsymbol{\alpha}+\boldsymbol{\alpha}_{i} \end{array}$$

在这里，我们使用：

• $\mathbf{a}$和$\mathbf{\alpha }$分别是线加速度和角加速度

里程计模型

通常在我们的移动机器人上都会装有速度编码器（encoder）或者装有视觉里程计来测量控制u作用于机器人的局部位移。一般来说，当我们有测量自我运动的方法时我们也可以把它整合到轨迹中。

2D情况下

控制信号为：$u=[\Delta p,\Delta \theta]\in \mathbb{R^{3} }$，则运动模型为：

$$\begin{aligned} \mathbf{x}=\left[\begin{array}{l} \mathbf{p} \\ \theta \end{array}\right], & \mathbf{u}=\left[\begin{array}{c} \Delta \mathbf{p} \\ \Delta \theta \end{array}\right], \quad \mathbf{i}=\left[\begin{array}{c} \Delta \mathbf{p}_{i} \\ \Delta \theta_{i} \end{array}\right] \\ \mathbf{p} & \leftarrow \mathbf{p}+\mathbf{R}\{\theta\}\left(\Delta \mathbf{p}+\Delta \mathbf{p}_{i}\right) \\ \theta & \leftarrow \theta+\Delta \theta+\Delta \theta_{i} \end{aligned}$$

其中$R\left \{ \theta \right \}$是与方向角θ相关的旋转矩阵。注意，这正好对应于旧机器人姿态与里程计增量之间的帧组合，x←x⊕(u + i)。

3D情况下

控制信号为：$u=[\Delta p,\Delta \theta]\in \mathbb{R^{6} }$，则运动模型为：

$$\begin{array}{l} \mathbf{x}=\left[\begin{array}{l} \mathbf{p} \\ \mathbf{q} \end{array}\right], \quad \mathbf{u}=\left[\begin{array}{c} \Delta \mathbf{p} \\ \Delta \boldsymbol{\theta} \end{array}\right], \quad \mathbf{i}=\left[\begin{array}{c} \Delta \mathbf{p}_{i} \\ \Delta \boldsymbol{\theta}_{i} \end{array}\right]\\ \mathbf{p} \leftarrow \mathbf{p}+\mathbf{R}\{\mathbf{q}\}\left(\Delta \mathbf{p}+\Delta \mathbf{p}_{i}\right)\\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\Delta \boldsymbol{\theta}+\Delta \boldsymbol{\theta}_{i}\right\} \end{array}$$

其中$R\left \{ q \right \}$是与四元数q相关的旋转矩阵。这正好对应于旧机器人姿态与里程计增量之间的帧组合，x ← x ⊕ [∆p +∆pi , q{∆θ +∆θi} ]。

IMU模型

惯性测量单元(IMU)和相关的运动模型对于有或没有接触地面的敏捷平台都是有效的。IMU测量的加速度 $a_{S}$ 和角速率 $ω_{S}$ 作为控制信号，用于预测机器人的位姿。

$$\mathbf{x}=\left[\begin{array}{c} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} \end{array}\right], \quad \mathbf{u}=\left[\begin{array}{l} \mathbf{a}_{S} \\ \boldsymbol{\omega}_{S} \end{array}\right], \quad \mathbf{i}=\left[\begin{array}{l} \mathbf{v}_{i} \\ \boldsymbol{\theta}_{i} \\ \mathbf{a}_{i} \\ \boldsymbol{\omega}_{i} \end{array}\right]$$

$$\begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\mathbf{v} \Delta t+\frac{1}{2}\left(\mathbf{R}\{\mathbf{q}\}\left(\mathbf{a}_{S}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t^{2} \\ \mathbf{v} \leftarrow \mathbf{v}+\left(\mathbf{R}\{\mathbf{q}\}\left(\mathbf{a}_{S}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t+\mathbf{v}_{i} \\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\left(\omega_{S}-\omega_{b}\right) \Delta t+\boldsymbol{\theta}_{i}\right\} \\ \mathbf{a}_{b} \leftarrow \mathbf{a}_{b}+\mathbf{a}_{i} \\ \omega_{b} \leftarrow \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{i} \end{array}$$

其中p, v, q分别是IMU参考系的位置、速度和方向四元数，$a_{b}$ 和$ω_{b}$分别是加速度计和陀螺仪的偏差，$v_{i}$ 和 $θ_{i}$ 是由测量噪声在时间步长上积分而产生的摄动脉冲∆t, $a_{i}$和$w_{i}$为随机偏差。