Localization for Ground Robots: On Manifold Representation, Integration, Re-Parameterization, and Optimization
摘要
本文专注于通过概率融合里程计和单目相机进行地面机器人定位的任务。具体而言，(1) 提出了一种新的方法，通过参数重表示的方法形成motion manifold (2) 使用轮式里程计进行6D的整合 (3) 重新参数化流形等式，减少误差。最后，提出一种基于流形辅助的滑动窗口估计器的完整定位算法，其中使用轮式里程计、单目相机以及可选的IMU。
符号标记和传感器模型
符号
在这项工作中，我们假设关于全局参考框架{G}的地面机器人，其车轮始终与路面接触。我们使用{C}和{O}来表示相机和里程计的坐标系。
里程计坐标系{O}的中心位于机器人的轮子中间，其中x轴向前，z轴向上，如图2所示。
另外，我们使用\({ }^{\mathbf{A}} \mathbf{p _ { B }}\)和\({ }^{\mathbf{A}}_{\mathbf{B}}\bar {\mathbf{q}}\)来表示B系相对于A系的位置和旋转。\({ }^{A}_{B} \mathbf{R}\)表示与\({ }^{\mathbf{A}}_{\mathbf{B}}\bar {\mathbf{q}}\)对应的旋转矩阵。
另外，
- \(\hat{a}\)表示估计值
- \(\tilde{a}\)表示误差
- \(a^{T}\)表示转置
- \(\dot{a}\)表示微分
- \(\|a\|\)表示二范数
- \(e_i\)是 3x1的向量，其中第i个元素为1，其他为0
- \(e_{ij}=[e_i,e_j]\)
轮式里程计观测模型
与[1,6,21]相似，在时间t，经过标定后的轮式里程计测量值如下：
其中，
- \({}^{\mathbf{O}(t)} {\mathbf{v}}\)表示车轮坐标系{0}的线速度在车轮坐标系{0}的表示
- \({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)表示车轮坐标系{0}的角速度在车轮坐标系{0}的表示
- \(n_{v o}\) and \(n_{\omega o}\)是测量噪声
- \(\mathbf{n}_{v}=\left[\begin{array}{lll}n_{v o} & 0 & 0\end{array}\right]^{\top}\)
- \(\mathbf{n}_{\omega}=\left[\begin{array}{lll}0 & 0 & n_{\omega o}\end{array}\right]^{\top}\)
等式(1)清晰的展示了轮式里程计测量仅提供2D的运动信息，即向前的线速度和关于yaw角的旋转速度。因此，通过使用等式1提供的测量值，理论上可以进行基于平面表面的位姿整合，而执行6D姿态集成。
基于流形的6D位姿整合
流形表示
为了允许使用车轮测量测量的6D姿态集成，我们首先通过参数方程式进行运动建模。特别的，我们选用了通过二次多项式来对3D位置p的运动流形进行近似：
其中，
流形的参数如下：
我们注意到，传统方法[1,6,21]假设平面环境，等同于流形参数为\(\mathbf{m}=[c, 0,0,0,0,0]^{T}\)。他们的设计选择不能代表室外路面的一般情况，因此不适合高精度估计
现有的工作如[6,21]展示了使用里程计观测对6D定位的困难，这主要是轮速计仅提供2D的测量，仅使用这些测量值，在6D空间进行姿态估计是不可行的。
然而，使用如式(4)的流形参数定义，6D的位姿估计变得可行，为了进一步描述，我们记姿态微分方程：
其中，
为了进行旋转积分，由轮速计提供的角速度\({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)需要知道。然而，显然式(1)中的轮速计只能提供向量\({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)的第三个元素。
为了获取另外两个元素，需要使用流形的参数表达，首先：
这个式子的物理意义是，机器人的旋转矢量与机器人所处位置的流形的法向量是垂直的。
这个式子表达的是，运动流形\(\mathcal{M}\)已经显式定义了地面机器人的roll和pitch，其应该与\({}_{\mathbf{O}(t)}^{G} {\mathbf{R}}\)具有一致性。换句话说，流形的梯度向量应该与机器人z轴方向平行。
因此，对式(7)进行微分，得到：
通过使用\(\mathcal{M}(t)=\mathcal{M}\left({ }^{\mathrm{G}} \mathbf{p}_{\mathbf{O}(t)}\right)\)作为缩写，然后将式（5）和（7）代入式（8），得到：
根据上述等式，\({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)的前两个元素可以被计算，而第3个元素不能被识别，因为\({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)被\([e_3]_{\times}\)左乘，其中：
另一方面，\({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)被\([e_3]_{\times}\)的第三个元素起始可以直接从轮速计中读取。这些观测结果验证了我们将轮速计观测和流形表示进行结合的动机，依赖于他们的互补特性，以实现6D的位姿估计。
为了从式（11）寻找\({}^{\mathbf{O}(t)} {\mathbf{\omega}}\)的前两个元素，为了实现这个目的，基于式（7）可以写出如下：
这是基于两个向量平行得到的，不信的话等式两边同时叉乘一个\(({}_{\mathbf{O}(t)}^{G} {\mathbf{R}} e_3)\)
结果，等式（11）变成：
其中，还用到了如下性质：
且有：\({}^{\mathbf{O}^{(t)}} \boldsymbol{\omega}_{12}=\mathbf{e}_{12}^{T} {}^{\mathbf{O}^{(t)}} \boldsymbol{\omega}\)
通过考虑里程计的测量，我们有：
通过整合式（18），就可以计算3D的旋转估计了。一旦旋转被计算，就可以进一步通过积分计算位置：
我们同样注意到我们的流形表示，如式（2），其隐式定义了运动模型的积分位置必须满足\(\mathcal{M}\left({ }^{\mathbf{G}} \mathbf{p}_{\mathbf{O}(t)}\right)=0\)。
实际上，等式（19）确实满足这个流形约束，为了展示，我们记：
应用到了链式求导，矩阵求导法则，以及等式（13）
其中，在等式（21）中\({}_{\mathbf{G}}^{\mathbf{O}(t)} \mathbf{R}^{T} \mathbf{e}_{3}\)与\(\nabla \mathcal{M}\left({ }^{\mathbf{G}} \mathbf{p}_{\mathbf{O}(t)}\right)\)成比例，（根据等式13的结果）。
可以发现，等式（19）中\({ }^{\mathrm{G}} \mathbf{v}_{\mathbf{O}(t)}\)的表达明确的满足等式（21）中的manifold约束（天向速度为0呗）。
随着等式（18）和（19），我们可以进行在流形上的基于里程计的6D位姿整合。整个过程不需要IMU的使用，为了直观地解释这个过程，我们注意到，执行6D姿态估计需要使用传感器测量值或者运动约束来解析6个自由度。在我们的系统中，两个自由度来自轮式里程计测量，两个自由度来自车辆约束（非完整性约束），最后两个自由度由流形等式决定（如等式（18）），并且直觉的解释与数学推导相符。
非完整性约束：来源于等式（19），这个约束表明局部线速度只有在x轴有非零值。 然而当底盘是麦克纳姆轮时，机器人不受该约束，并且必须重新设计方程。然而，这类型的底盘不常用于商业户外机器人或车辆。
状态和误差状态预测
本节，将描述使用提出的流形表示来进行状态和误差状态的传播。由于积分过程需要姿态(位置和方向向量)和流形方程的显式表示，我们定义状态向量如下
状态向量微分方程如下：
其中，
我们指出，流形参数的运动动力学将在后面的小节中详细描述，在式（25）这里先忽略。
进一步的，有：
其中，
式（27）到（29）可以基于轮式里程计进行数值积分（如龙格库塔）。
为了描述误差状态的传播细节，我们首先对式（18）进行一阶泰勒展开，得到：
其中，
这是角速度对状态和噪声求导
本文定义的误差角\(\delta \theta\)是local的[48]，因此，有：
这个推导可以参见深蓝多传感器融合ppt
进一步的，参考附录，我们可以写出等式（35），关于误差角的微分方程：
附录A：
因为\(\delta \theta\)是误差状态\(\tilde{x}\)的一部分，式（35)的前两项可以结合，因此得到：
关于位置项，基于等式（19），可以直接得到如下：
对于流形参数，我们有：
结合等式（35）（38）（39），误差状态的微分方程已经推导完成，我们可以进行6D的误差状态集成，把所有的等式放到向量中，我们可以得到误差状态模型：
其中，
需要注意的是：
- \((\cdot)_{[:, i]}\)表示矩阵的第i列
我们注意到，\(\mathbf{F}_{c}\)和\(\mathbf{G}_{c}\)是连续时间下的误差状态转移矩阵和噪声雅可比矩阵，为了实现离散时间的概率估计器，则需要离散时间下的误差状态转移矩阵\(\boldsymbol{\Phi}(t, \tau)\)，这可以通过积分得到：
数值积分的具体实现和剩下的步骤是离散时间局部化估计的标准步骤，可以参考[12,48]
流形重新参数化
等式（2）定义了全局参考坐标系的运动流形，其数值上是正确的，并且能够很容易地给出一个随机估计量。然而，这种表示方式使得流形参数m的传播过程极具挑战性。具体地说，很难描述流形的运动动力学，它随时间而变化。文献[24]简单使用\(\dot{\mathbf{m}}=\mathbf{n}_{\omega m}\)，其中\(\mathbf{n}_{\omega m}\)是零均值高斯噪声向量。然而，这个方法受限于大规模应用。为了说明细节，让我们以下面的方程为例：
等式绘制如图3所示。
实际上，等式（44）可以考虑为一个二维的流形，如果一个二次二维流形可以用如下方程表示：
等式（44）实际上是一个分段二次二维流形，其分段形式的参数是：
可以清晰的看见，当\(x=1000\)时，会有明显的jump，在所估计的流形参数，由于我们选择了二次表示来模拟一个非二次方程。
如果我们能用无穷多个多项式来近似流形，这个问题就不会产生了，然而，这在计算上是不可行的。因此，流形参数的正常传播过程(见Eq. 25)无法捕捉这种类型的变化。
虽然流形表示在此条件下仍然可行，但通过去除和重新初始化流形参数，这肯定不是高精度姿态估计的首选。为此，我们建议将Eq. 45修改为：
其中，\(x_{o}\)是固定的常值参数，可以通过当前\(x\)的估计来得到。如果当\(x=1000\)，我们得到一个估计的\(x_{o}=999.9\)，来代替等式（46）所展示的jump，即我们的流形参数为：
通过使用参数\(x_{o}\)，流形参数的jump大幅减小。这使得流形的传播等式（状态、误差状态估计）变得可行。
通过用上面的例子说明我们的动机和高级概念，我们介绍了我们的正式数学-局部流形表示和重新参数化的数学方程。通过假设前一个重新参数化步骤是在时间t k时执行的，下一个步骤是在t k+1时触发的，我们有：
其中，等式（49）：
- \(\mathbf{R}_{o}\)和\(\mathbf{x}_{o}\)是m(tk)的固定重参数化参数
- \(\mathbf{R}_{1}\)和\(\mathbf{x}_{1}\)是m(tk+1)的固定重参数化参数
- \(\gamma, \Xi,\boldsymbol{\Psi}\)都是关于\(\delta \mathbf{R}\)和\(\delta \mathbf{x}\)的函数
为了选择重新参数化参数，类似于等式（48），我们为\(\mathbf{x}_{o}\)的选择\({ }^{\mathbf{G}} \mathbf{p}_{\mathbf{O}\left(t_{k}\right)}\)的第一次估计和为\(\mathbf{x}_{1}\)选择\({ }^{\mathbf{G}} \mathbf{p}_{\mathbf{O}\left(t_{k+1}\right)}\)的第一次估计
等式（49）的详细推导和 \(\mathbf{R}_{o}，\mathbf{R}_{1}\)的选择参见附录C。
因此，通过式（49），对于状态估计及其相应的不确定性，我们能够在任何时间t周围重新参数化流形表示。我们注意到，这类似于用逆深度参数化重新参数化SLAM特征[15][35]。重要的是要指出，在我们的重新参数化过程中，\(\mathbf{R}_{o}，\mathbf{x}_{o}, \mathbf{R}_{1} ，\mathbf{x}_{1}\)被用作固定的常量向量和矩阵，而不是随机变量。这样就不需要计算它们对应的雅可比矩阵，这也是SLAM特征重参数化中广泛使用的技巧[35]，[15]。
需要指出的是，我们的数值微分和重表示都是基于参数\(m\)的，如（Sec. IV-A - IV-B and Appendix B），并且这个参数\(m\)还未被任何重新参数化的步骤之前。
一旦引入重参数化，在\(t_{k+1}\)时刻存储在状态向量中的流形变为\(m(t_{k+1})\)，而在状态积分中使用的相应流形参数仍为\(m\)，这是由于全局流形表示保持不变的事实，\(\mathbf{m}\)和\(\mathbf{m}(t_{k+1})\)的关系如下：
因此，在状态预测和雅可比计算过程中，我们可以简单应用\(\mathbf{m}=\left(\prod_{i=0}^{k} \mathbf{\Lambda}\left(t_{i}\right)\right)^{-1} \mathbf{m}\left(t_{k}\right)\)，这允许明确考虑\(\mathbf{m}(t_{k+1})\)的不确定性，而不产生额外的计算成本和复杂的软件设计
通过定义流形的重新参数化过程，我们重新讨论了运动流形的运动动力学特征问题。由于地面机器人所在的表面上导航的流形实际会随着时间的推移而变化，为了保证准确性，必须明确地建模这一事实，具体地说，我们建议在重新参数化过程中考虑这一点：
其中，
- \(\mathcal{N}\left(0, \sigma_{i, w m}^{2}\right)\)表示零均值且方差为\(\sigma_{i, w m}^{2}\)的高斯分布。
- 特别的，标准差\(\sigma_{i, w m}\)可以定义如下：
其中，\(\alpha_{i,p},\alpha_{i,q}\)是控制参数。需要指出的是，在我们提出的公式中，流形不确定性(如\(\sigma_{i, w m}\))是空间位移的函数(见Eq. 54)。这与VIO文献[12]、[30]中的标准噪声传播方程不同，在这些方程中，噪声的特征是时间函数。因为运动流形是一个空间概念而不是时间概念，所以我们的设计选择更适合我们的定位问题。
定位算法
为了实现地面机器人的高精度定位，在本节中，我们提出了一种详细的定位算法，用于融合来自单目摄像机、车轮里程表和可选IMU的测量数据。具体地说，我们提出了一种基于滑动窗口迭代优化的算法来最小化来自传感器测量和流形约束的代价函数。在接下来的内容中，我们首先描述我们提出的不使用IMU的方法，与IMU相关的额外操作将在第二节中讨论。
数据插值
在本文中，我们假设车轮里程表和摄像机传感器在硬件上完全同步，摄像机与机器人刚性连接。在介绍概率传感器融合的细节之前，我们首先注意到我们的系统中需要数据插值，这是由于提出的系统中的一些操作需要执行基于里程计的位姿积分（两帧图像之间）。然而，由于多传感器系统的特性，我们可能无法在捕获图像的同时精确地获取里程计信息。为此，我们建议通过插值图像时间戳前后的最近测量值来计算额外的虚拟车轮里程表测量值。由于车轮里程表测量的频率相对较高(例如，在我们的例子中是100Hz)，而且在人造环境中的地面机器人通常处于平滑运动状态，因此我们选择应用线性插值。
图像处理
一旦接收到新图像，我们继续执行位姿积分，通过车轮里程表测量和流形表示计算相应的预测位姿，一旦通过姿态预测检测到足够的平移或旋转位移(例如，在我们的测试中，20厘米和3度)，新图像将被处理，否则它将被丢弃。
对于特征处理，提取FAST特征[49]，计算FREAK[50]描述符，这是由于它们在低成本处理器上的效率，然后是特征匹配和RANSAC几何验证步骤
状态向量与迭代优化
为了说明定位算法，首先介绍状态向量，在时间\(t_k\)，状态向量为：
为了更简单的表示，我们在本节的介绍中忽略了传感器的外部参数。然而，在我们的一些实际实验中，当离线传感器外部标定精度不高时，这些参数被明确地建模在我们的公式中，并用于优化。
其中，\(x\)在等式（22）中定义，\(\mathrm{O}_{k}\)式在k时刻下滑动窗口中的位姿：
当在\(t_{k+1}\)记录到新的图像，将会执行pose积分来计算\(\mathbf{X} \mathbf{O}_{k+1}\)，随后，我们使用如下cost func来调整我们的状态：
其中，
- \(\boldsymbol{\eta}_{k}\)和\(\boldsymbol{\Sigma}_{k}\)分别是在之前的时间步中估计的先验信息向量和矩阵。
- \(\|\mathbf{a}\|_{\Sigma}\)由\(\mathbf{a}^{T} \mathbf{\Sigma} \mathbf{a}\)计算得到
- \(\mathbf{f}_{k+1}\)是被包含到优化过程中的视觉landmarks的集合
- \(\mathbf{S}_{i, j}\)表示关键帧和观测到的特征的匹配对集合
- \(\gamma_{i, j}\)是计算的视觉重投影残差向量
- \(\boldsymbol{\psi}_{i}\)是与运动流形相关联的残差
- \(\boldsymbol{\beta}_{i}\)是在\(t_k\)和\(t_{k+1}\)时间内，基于轮速里程计测量的位姿预测残差（Sec. IV-B）
特别的，视觉重投影残差计算如下：
其中，
- \(\mathbf{Z}_{i j}\)表示与位姿\(i\)和视觉landmark \(\mathbf{f}_{j}\)相关的相机观测 （图像点）
- \(\boldsymbol{\Sigma}_{C}\)是观测的信息矩阵
- 函数\(h(\cdot)\)表示经过校准的相机模型[20]