27、用于太空漫游车应用的单目同时定位与地图构建

用于太空漫游车应用的单目同时定位与地图构建

1. 引言

同时定位与地图构建（SLAM）是在创建环境地图的同时确定漫游车在地图上位置的过程。它在智能漫游车研究及其他领域发挥着重要作用。漫游车的传感器对SLAM算法影响很大，常见的传感器有激光、声纳和视觉相机等。相比之下，相机成本低、重量轻，在某些条件下使用更方便，因此单目相机作为单一视觉传感器的单目SLAM受到越来越多的关注。

单目SLAM虽不能直接获取观测地标深度信息，但它所需计算资源比双目或全景SLAM少。它与从运动恢复结构（SFM）问题密切相关，不过SFM有时是离线算法，需对所有图像进行批量处理，计算时间随图像数量增加而增长；而单目SLAM能实时顺序估计相机3D运动，更适合长序列实时定位。

在SLAM中，面临两个主要问题：一是如何从视觉相机中提取重要特征并构建周围环境地图，许多特征提取算法如Shi - Tomasi、Harris、SIFT、SURF、FAST等被应用于此，同时要选择合适特征作为可靠地标并设计良好的特征匹配算法；二是如何匹配特征地图以估计漫游车状态，常见算法有基于滤波的SLAM和基于关键帧的SLAM，扩展卡尔曼滤波器（EKF）用于解决估计问题，还有一些改进滤波器提高性能。

研究提出了一种新的单目SLAM来估计视觉传感器状态，扩展到从稀疏特征创建地图的工作中。使用Oriented FAST and Rotated BRIEF（ORB）检测特征，提出新的特征检测算法获取合适数量的地标，并应用Modified coVariance Extended Kalman Filter（MVEKF）递归估计系统状态。此外，该算法还应用于太空漫游车，因为月球和火星没有像地球那样的全球定位系统（GPS），SLAM可辅助定位。

2. 相关工作

1980年，Moravec开展了从视觉数据预测漫游车运动的研究，他还提出了Moravec角检测器，是流行的Harris角检测器的前身。此后，SFM和视觉SLAM不断发展。

目前有许多单目SLAM的研究，其中Davison等人在2007年开发了首个使用单相机的SLAM系统，能以30FPS运行，用标准EKF框架处理相机敏捷运动，使用Shi和Tomasi的检测算子自动检测点特征；Klein和Murray在2008年提出的系统，因滤波方法的线性化误差，主张优化稀疏选择的关键帧，使用FAST角检测提取特征并进行全局束调整。

基于这些工作，其他研究者也提出了改进方法。如Lemaire等人提出鲁棒的兴趣点匹配算法；Karlsson等人用尺度不变特征变换（SIFT）提取特征；Wang等人应用SURF算法；Strasdat等人提出新的视觉SLAM架构；Civera等人将单点RANSAC与EKF结合。

本研究基于Davison等人和Civera等人的工作，设计了新的单目SLAM系统，提出改进措施并进行实验，还提出了新的月球漫游车定位模块。

3. 提出的系统和算法

3.1 系统初始化

系统初始化主要进行参数化。SLAM系统状态由相机状态和所有特征点位置两部分组成。用W表示世界坐标系（静态参考系），C表示视觉运动坐标系。
相机状态表示为：
[x_c = [r_{WC}, q_{WC}, v_W, w_C]^T]
其中，(r_{WC})是运动相机的位置，(q_{WC})是相机在世界坐标系W中的方向，(v_W)和(w_C)分别是线性和角速度。具体为：
[r_{WC} = [x_r, y_r, z_r]^T]
[q_{WC} = [q_1, q_2, q_3, q_4]^T]
[v_W = [v_x, v_y, v_z]^T]
[w_C = [w_x, w_y, w_z]^T]

地图中的每个特征用逆深度参数化的状态向量表示：
[f^W_i = [x^W_i, y^W_i, z^W_i, \alpha_i, \beta_i, \rho_i]^T]
其中，(\alpha_i)和(\beta_i)分别表示方位角和仰角，用于定义单位方向向量(m(\alpha_i, \beta_i))，(\rho_i = 1/d_i)表示点沿射线的深度倒数。单位方向向量为：
[m(\alpha_i, \beta_i) = [\cos\beta_i \sin\alpha_i, -\sin\beta_i, \cos\beta_i\cos\alpha_i]^T]

6D特征通常转换为地图上的3D欧几里得点：
[f^W_{i,E} =
\begin{bmatrix}
X^W_i \
Y^W_i \
Z^W_i
\end{bmatrix}
=
\begin{bmatrix}
x^W_i \
y^W_i \
z^W_i
\end{bmatrix}
+ \frac{1}{\rho_i} m(\alpha_i, \beta_i)]

系统状态向量为：
[x = [x_c, f^W_1, f^W_2, f^W_3, \cdots, f^W_N]^T]
协方差矩阵表示为：
[P =
\begin{bmatrix}
P_{xx}, P_{xf} \
P_{fx}, P_{ff}
\end{bmatrix}^T]

3.2 地图管理

地图管理负责添加新特征或移除影响SLAM系统结果的无用特征。提出了基于网格的特征检测策略并修改特征删除准则。
将图像分成M个网格，应用ORB检测器检测选定网格中的最小特征，随机提取特征直到满足数量要求。选择网格子区域的表达式为：
[grid_k =
\begin{cases}
randperm(1, 2, \cdots, M) 1 & k \leq N \
randperm(minE{f {k1}, f_{k2}, \cdots, f_{kM}}) 1 & k > N
\end{cases}]
其中，(grid_k)是第k次选择的网格索引，(randperm())生成随机整数排列，(f {kj})表示每个网格中的特征数量。

原特征删除准则是正确匹配数与匹配尝试数的比率低于阈值则删除地标，但存在两个缺点：移除被遮挡地标后重新观测有额外成本；移除旧特征会导致闭环任务中无法识别先前映射区域，使映射发散。研究对该准则进行了改进。

相机运动的线性和角速度模型为：
[x_{k + 1} = g(x_k, u) =
\begin{bmatrix}
r_{WC_{k + 1}} \
q_{WC_{k + 1}} \
v_{W_{k + 1}} \
w_{C_{k + 1}}
\end{bmatrix}
=
\begin{bmatrix}
r_{WC_k} + (v_{W_k} + m \cdot V_W) \Delta t \
q_{WC_{k + 1}} + q((w_{C_k} + n \cdot W_C) \Delta t) \
v_{W_{k + 1}} + V_W \
w_{C_{k + 1}} + W_C
\end{bmatrix}]
其中，(V_W)和(W_C)是脉冲线性和角速率，(q((w_{C_k} + n \cdot W_C) \Delta t))是从旋转向量计算的四元数，(u)表示线性和角速度的变化，(m)和(n)是加速度的动态参数。
[u =
\begin{bmatrix}
V_W \
W_C
\end{bmatrix}
=
\begin{bmatrix}
a_W \Delta t \
b_C \Delta t
\end{bmatrix}]
(\Delta t)是采样时间间隔，(a_W)和(b_C)是零均值高斯变量。

MVEKF估计的系统状态模型为：
[\hat{x} {k|k - 1} =
\begin{bmatrix}
g(\hat{x} {k - 1|k - 1}) \
\hat{f}^W_1 \
\vdots \
\hat{f}^W_1
\end{bmatrix}]
[P_{k|k - 1} = G_x P_{k - 1|k - 1} G_x^T + G_u Q G_u^T]
其中，(G_x)和(G_u)是运动模型(g)关于相机状态(\hat{x}_c)和控制参数(u)的导数，(Q)是协方差矩阵的噪声。

3.3 特征搜索与匹配

单目SLAM系统使用带两个径向畸变参数的针孔相机模型测量地标。将6D特征从逆深度世界坐标系W投影到相机坐标系C的方程为：
[h^C_i =
\begin{bmatrix}
x^C_i \
y^C_i \
z^S_i
\end{bmatrix}
= R_{CW}(q_{WC_i}) (\rho f^W_i - r_{WC_i}) + m(\alpha_i, \beta_i)]
其中，(R_{CW}(q_{WC_i}))是旋转矩阵。

针孔模型为：
[h_u =
\begin{bmatrix}
u_u \
v_u
\end{bmatrix}
=
\begin{bmatrix}
u_0 - \frac{f}{d_x} \frac{x^C_i}{z^C_i} \
v_0 - \frac{f}{d_x} \frac{x^C_i}{z^C_i}
\end{bmatrix}]
其中，(f)是相机焦距，((u_0, v_0))是图像中心坐标，(d_x)和(d_y)是每个像素的单位尺寸。

无畸变点经径向畸变模型转换后的畸变像素坐标为：
[h_d =
\begin{bmatrix}
u_d \
v_d
\end{bmatrix}
=
\begin{bmatrix}
u_0 + \frac{u_u - u_0}{\sqrt{1 + k_1 r^2 + k_2 r^4}} \
v_0 + \frac{v_u - v_0}{\sqrt{1 + k_1 r^2 + k_2 r^4}}
\end{bmatrix}]
[r = \sqrt{d_x (u_d - u_0)^2 + d_y (v_d - v_0)^2}]

使用主动搜索原理，根据系统状态的预测概率分布自动找到对应搜索区域。6D特征(f_i)投影的期望位置(\hat{h} i(u^E_i, v^E_i))的协方差矩阵为：
[\hat{h}_i = h_i(\hat{x} {k|k - 1})]
[S_i = H_i P_{k|k - 1} H_i^T + R_i]
搜索区域边界为：
[\begin{bmatrix}
s_x \
s_y
\end{bmatrix}
=
\begin{bmatrix}
2n \sqrt{S_i(1, 1)} \
2n \sqrt{S_i(2, 2)}
\end{bmatrix}]
其中，(n)是期望搜索区域的数量。实际测量值(z_i)在95%概率的搜索区域内搜索。

特征匹配过程在主动搜索原理基础上进行了改进，系统会根据系统地图特征与新观测的匹配情况，决定是否使用ORB提取的描述符进行汉明匹配，以提高单目SLAM的鲁棒性和效率。

下面是系统流程的mermaid流程图：

graph TD;
    A[系统初始化] --> B[读取新图像];
    B --> C[地图管理];
    C --> D[系统状态预测];
    D --> E[加载新图像到估计系统];
    E --> F[投影6D特征到2D点搜索测量值];
    F --> G[检查观测数据];
    G --> H[系统状态更新];
    H --> I[估计相机新状态和稀疏特征位置];
    I --> J{是否结束};
    J -- 否 --> B;
    J -- 是 --> K[结束];

表格展示主要算法及其作用：
| 算法 | 作用 |
| ---- | ---- |
| ORB | 检测特征，减少系统初始化时间 |
| MVEKF | 递归估计系统状态 |
| 基于网格的特征检测策略 | 更好地分布图像中的特征 |
| 改进的特征删除准则 | 避免原准则的缺点 |
| 基于主动搜索的特征匹配 | 提高特征匹配的鲁棒性和效率 |

4. 实验设计与结果

为了验证所提出的单目 SLAM 系统的性能，进行了一系列实验。实验主要关注系统在不同场景下的定位精度、特征匹配的准确性以及地图构建的一致性。

4.1 实验设置

实验使用了模拟的太空漫游车场景，通过设置不同的光照条件、地形复杂度和相机运动速度来模拟实际的太空环境。相机采用单目视觉相机，按照系统流程进行图像采集和处理。

实验中对系统初始化参数进行了仔细调整，确保系统能够稳定运行。同时，为了对比不同算法的性能，还设置了对照组，使用传统的 SLAM 算法进行实验。

4.2 实验结果分析

• 定位精度 ：通过与已知的真实位置进行对比，测量系统的定位误差。结果表明，所提出的单目 SLAM 系统在不同场景下都能保持较高的定位精度，尤其是在复杂地形和光照变化较大的情况下，定位误差明显小于传统算法。这得益于 MVEKF 对系统状态的递归估计以及改进的特征匹配算法，能够更准确地处理数据关联问题。
• 特征匹配准确性 ：统计特征匹配的正确匹配率和错误匹配率。在实验中，改进的基于网格的特征检测策略和特征匹配算法显著提高了特征匹配的准确性。即使在相机运动剧烈的情况下，系统也能通过使用 ORB 提取的描述符进行汉明匹配，有效地减少了错误匹配的发生，提高了数据关联的可靠性。
• 地图构建一致性 ：观察地图构建的结果，检查地图的一致性和完整性。实验结果显示，改进的特征删除准则避免了原准则的缺点，使得地图能够更好地反映实际环境。移除无用特征时不会导致地图的不一致性，并且在闭环任务中能够正确识别先前映射的区域，保证了地图的连续性和准确性。

以下是实验结果的表格对比：
| 指标 | 本文方法 | 传统方法 |
| ---- | ---- | ---- |
| 定位误差（平均） | 较小 | 较大 |
| 特征正确匹配率 | 较高 | 较低 |
| 地图一致性 | 好 | 一般 |

5. 结论

本研究提出了一种新的单目 SLAM 系统，用于太空漫游车的定位和地图构建。通过对现有工作的改进，在系统初始化、地图管理和特征匹配等方面提出了新的策略和算法。

• 系统优势 ：使用 ORB 检测特征减少了系统初始化时间，基于网格的特征检测策略使特征分布更均匀，改进的特征删除准则避免了地图不一致性问题，MVEKF 递归估计系统状态提高了定位精度。同时，改进的特征匹配算法增强了系统在复杂环境下的鲁棒性和效率。
• 应用前景 ：该系统对于太空探索具有重要意义，尤其是在月球和火星等没有 GPS 的环境中，能够为漫游车提供准确的定位和地图信息，帮助漫游车更好地完成任务。此外，该方法也可以推广到其他领域的移动机器人定位和地图构建中。

未来的研究可以进一步考虑多传感器融合，结合激光雷达、惯性测量单元等传感器，提高系统的可靠性和适应性。同时，可以对算法进行优化，减少计算资源的消耗，以满足实时性要求更高的应用场景。

总结一下本研究的关键步骤如下：
1. 系统初始化：参数化系统，确定相机状态和特征点位置的表示方法。
2. 地图管理：使用基于网格的特征检测策略添加新特征，用改进的准则移除无用特征。
3. 特征搜索与匹配：采用带径向畸变的针孔相机模型测量地标，基于主动搜索原理和改进的匹配算法进行特征匹配。
4. 系统状态估计：使用 MVEKF 递归估计系统状态。
5. 实验验证：通过模拟实验验证系统在定位精度、特征匹配准确性和地图构建一致性方面的性能。

通过以上步骤，实现了一个高效、准确的单目 SLAM 系统，为太空漫游车的应用提供了有力的支持。

下面是整个系统工作流程的总结 mermaid 流程图：

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px;

    A([开始]):::startend --> B(系统初始化):::process
    B --> C(地图管理):::process
    C --> D(特征搜索与匹配):::process
    D --> E(系统状态估计):::process
    E --> F{是否继续}:::decision
    F -->|是| C
    F -->|否| G([结束]):::startend

通过这个流程图可以清晰地看到系统从初始化到持续运行，再到最终结束的整个过程，各个环节相互关联，共同实现了单目 SLAM 系统的功能。