面向ARM同构处理器的单目视觉SLAM方法

王俊丰，曾 碧，郭植星

（广东工业大学 计算机学院，广东 广州 510006）

0 引言

同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术是指搭载特定传感器的物体，对未知环境进行地图构建的同时估计自己的运动，是智能移动机器人的关键技术之一。按所搭载的传感器类型不同，可以将SLAM 主要分为激光和视觉两大类。激光传感器因能直接获取环境的距离信息，在早期SLAM 的研究发展中得到关注，目前激光SLAM 的理论研究和工程应用都已比较成熟。伴随着计算机视觉技术的发展，相机因其成本低廉、体积更小且能提供更加丰富的环境语义信息而备受关注，视觉传感器在近年来的SLAM 研究中开始占据主导地位。

然而，视觉SLAM 一方面为机器人感知环境提供了更多元的数据支撑，另一方面也要求机载平台有更高的运行效率来满足系统的实时性需要，而这无疑在提高整体方案的工程实现成本。以视觉SLAM 的代表作——ORBSLAM［1-3］为例，其经历了三个版本的迭代更新，虽然系统整体稳健性得到了提升，但最新版本工程量大，软件编译和运行都有了更高的要求，已无法直接在树莓派等嵌入式设备上编译运行。

尽管主流的视觉SLAM 方法在x86-64 处理器上表现良好，但却没有针对ARM 处理器等资源有限的嵌入式系统进行优化。本文以ORB-SLAM 为基础，以树莓派为实验平台，研究一种面向ARM 同构处理器的单目视觉SLAM 方法，有效解决视觉SLAM 在树莓派上编译困难、单目视觉尺度模糊、实时效率低、存储空间与运算能力有限等问题，助力视觉SLAM 技术在嵌入式系统平台的落地应用。

1 相关工作

一个完整的视觉SLAM 框架包含前端、后端、回环检测和地图构建［4］。由于嵌入式设备资源有限，前期面向嵌入式设备的视觉SLAM 方法主要研究前端里程计的应用，而不是完整的视觉SLAM 方法。以树莓派为例，Sukvichai等［5］提出了一种单目相机融合IMU 和压力传感器的视觉里程计方法，通过提取Shi-Tomasi 特征并使用KLT 光流跟踪来估计运动的单应矩阵，但该方法无法在树莓派上实时运行；而ARM-VO 则是一种可在树莓派上实时运行的视觉里程计算法，该方法同样采用光流跟踪，但其使用了ARM自带的NEON 协处理器对算法进行硬件加速，同时采用固定相机高度的方法来恢复单目视觉尺度信息，板载运行的实时性优于ORB-SLAM2［6］。近年来，随着硬件设备的发展，衍生出了众多面向ARM+GPU［7］和ARM+FPGA［8］等异构处理器的视觉SLAM 方法。得益于硬件加速，这类方法可以在嵌入式设备上进行应用。但是硬件资源的增加提高了整体方案的实现成本，众多传统的ARM 同构多核处理器平台仍无法运行主流的视觉SLAM 方法。

一方面，目前主流的视觉SLAM 方法大部分都是面向x86-64 处理器设计的，且在该平台上表现优越，如ORBSLAM、LSD-SLAM［9］和DSO-SLAM［10］等。ORB-SLAM 是特征法的代表之一，通过提取具备光照变化不敏感、旋转不变性和尺度不变性等优点的像素点作为特征，为后端优化估计提供了稳定且准确的初始值，因此该方法的稳定性强，鲁棒性高。但是，特征法在纹理不丰富的场景下容易丢失，特征点的提取和描述子的计算过程亦相对耗时。Fu等［11］基于ORB-SLAM 进行优化，提出了一种与描述子无关的特征匹配方法来避免无效且耗时的描述子计算。LSD-SLAM、DSO-SLAM 则采用直接法，基于两帧图像之间灰度不变的假设，通过最小化相邻图像之间的灰度误差来估计相机的运动。直接法只关注光度信息而忽略特征，速度快且适用于纹理信息较少的场景，但因其对光照和动态干扰敏感，且由于缺少图像特征信息而无法实现回环检测和重定位功能，因此，直接法的精度一般劣于特征法。Forster 等［12］提出的SVO 方法，基于直接法跟踪又依赖于特征法进行优化，兼顾了两者的优点，但该方法只适用于平面运动，且不是一个完整的视觉SLAM 方法。

另一方面，视觉传感器存在着一些固有缺陷，如尺度不确定性、高速运动视觉模糊等。相比于双目、深度相机，单目相机无需处理图像同步和相机联合标定等问题，计算量更低，性价比更高，因此单目相机的应用也更为广泛。然而，仅采用单目相机的系统都存在尺度不确定性问题，无法直接恢复具有绝对尺度信息的三维结构和相机运动。解决这一问题，至少要引入一个绝对尺度作为先验信息。Zhou 等［13］提出了一种基于地平面的单目视觉里程计绝对尺度估计方法，该方法通过RANSAC 方法拟合地平面并计算出归一化尺度下的地面高度，根据已知相机绝对高度信息求解归一化尺度和真实尺度的比例关系，进而恢复视觉尺度。显然，这类方法限制了相机的运动范围，不具备普遍适用性。此外，Eigen 等［14］提出基于深度学习进行单目深度估计，但计算量大且不适用于嵌入式设备。针对快速运动导致视觉模糊等问题，一般采用视觉与惯性测量单元（Inertial Measurement Unit，IMU）融合的方法，如VINS［15］。利用IMU 能够跟随运动速度变化，不受运动场景和运动速度的限制而准确测量出角速度和线性加速度的优点，弥补视觉失效的问题。

本文提出一种面向ARM 同构处理器的单目视觉SLAM 方法（Visual SLAM Method for ARM，ARM-SLAM）。具体地，以ORB-SLAM 为基础，解决以树莓派为代表的嵌入式设备存在编译困难、单目视觉尺度模糊、实时性低、计算量大等问题。首先，基于纯视觉构建无尺度的初始地图，通过对齐IMU 带尺度的测量数据来恢复视觉尺度因子；其次，基于良好的关键帧特征点，对后续帧采取快速跟踪策略，通过减少特征提取与描述子的计算量来提升传统特征法的运行速度；最后，对后端优化问题规模进行滑动限制并及时推送历史数据作边缘处理，有效地避免了长时间运行下嵌入式设备本地计算量和存储量的激增。

2 面向ARM同构处理器的视觉SLAM方法

本文提出的ARM-SLAM 方法，选取树莓派作为嵌入式设备代表，以单目相机和IMU 作为系统输入，以ORBSLAM 为基础，面向嵌入式设备进行优化。该方法的主要步骤如下：

（1）纯视觉初始化构建无尺度的初始地图，将IMU 带绝对尺度的测量数据与各关键帧的位姿对齐，以尺度因子作为显式变量参与迭代优化，进而恢复单目视觉的尺度信息。

（2）一方面，基于OpenMP 对特征法的工程实现采取并行加速；另一方面，基于快速跟踪策略，有选择地进行特征点提取，仅对关键帧才进行描述子计算，避免无效计算，提高实时帧率。

（3）基于滑动窗口算法对旧关键帧进行边缘化，有效地限制了优化问题规模，对历史数据仅作缓存处理且不参与后续的任何优化，避免了因后端非实时性优化而影响前端的实时性跟踪。

（4）嵌入式设备存储空间有限，如果边缘设备存在，则对历史数据进行序列化并作周期性推送，否则，根据本地剩余存储空间大小对本地地图进行裁剪。

图1 展示了ARM-SLAM 方法的整体框架，主要包含尺度因子、快速跟踪、滑动窗口与数据推送等四大部分。

Fig.1 Overall method flow diagram of ARM-SLAM图1 ARM-SLAM 方法整体流程

2.1 尺度因子

假设纯视觉在初始化阶段所估计的相机位姿是准确的，本文通过对齐IMU 带尺度的测量信息求解视觉尺度因子。与传统方法不同，该方法无需场景高度信息作为先验，更具普遍适用性。

本文约定：(·)w表示世界坐标系，(·)c表示相机坐标系，(·)b表示IMU 本体坐标系。表示相机坐标系到本体坐标系的旋转。表示含噪声的测量数据，表示无尺度数据。

假设起始速度为0，如式（1）所示，可以根据IMU 所测量的加速度，估计下一时刻的位移和速度。

其中，bat表示加速度偏置，nat表示加速度测量噪声，gw表示重力向量，Δtk表示相邻帧的间隔时间。

因为世界坐标系下的位姿会随后端算法的优化而在不断更新调整，所以式（1）会因而对IMU 数据进行反复的计算。为了避免上述问题，如式（2）所示，本文采用IMU预积分模型［16-17］，使得式（1）中的积分项是相对于第k 时刻的位姿进行计算，而不是世界坐标系。

其中，s 是视觉尺度因子。将式（3）代入到式（2）中，可以得到IMU 预积分增量的预测值，如式（5）所示。

由于IMU 的测量数据是离散的，因此，如式（6）所示，可以采用中值积分对式（2）的IMU 预积分增量进行计算，得到观测值。

式（7）定义了问题所要估计的状态量，包含了世界坐标系下各关键帧的速度、重力向量和视觉尺度因子。

式（8）定义了相邻两帧之间的IMU 预积分测量值与估计值之间的残差。

令残差为零，并转为Hx=b形式，如式（9）所示。

因此，可以通过构建最小二乘问题，在系统初始化阶段，求解出包含视觉尺度因子在内的参数，如式（10）所示。

2.2 快速跟踪

基于特征法的视觉SLAM 方法会对所有图像进行特征点提取和描述子计算，该操作相对耗时且只有被选为关键帧之后，才会对该图像的描述子进行使用。本文一方面基于OpenMP 对特征点提取和描述子计算环节采取了并行加速；另一方面，为了节省计算以保证实时性，本文利用系统持续运动过程中相邻图像具有的连贯性和相似性，提出了一种基于稀疏光流算法对已知特征点进行快速跟踪的方法。该方法无需对所有图像帧都进行特征点提取和匹配，有效地节省了计算量。

假设相邻帧之间满足以下特性：①灰度不变性：实际场景目标在相邻帧之间所表示的像素灰度值保持不变。②邻域内光流一致性：同一邻域内的像素点具有一致的运动速度。

在t时刻，图像中像素点(x，y) 的灰度可以表示为I(x，y，t)，假设t+dt时刻该像素点运动到(x+dx，y+dy)处，根据灰度不变性，可得式（11）。

对式（11）等号右边进行泰勒展开，可得式（12）。

将式（12）代入式（11），可得式（13）。

将式（13）转成矩阵形式，可得式（14）。

其中，Ix、Iy和It是已知的，分别表示灰度在像素x、y轴和时间上的偏导。根据领域内光流一致性，可以假设在一个大小为w*w的窗口内，所有的像素点都具有相同的运动。因此，可以构建一个关于变量u和v的超定线性方程，并用最小二乘法进行求解，如式（15）所示。

然而，当相机运动较快导致两张图像的差异明显时，仅对原图的求解容易得到一个局部极小值，因此需要引入图像金字塔对同一个图像进行缩放，当像素运动较快时，从金字塔顶层图像看来，仍然是一个相对较小的运动。本文从金字塔顶层的图像开始计算，然后把上一层的结果作为下一层的初始值，如此迭代至底层原图，最终得到最优解。同时，光流跟踪存在一定的匹配错误，需要检查跟踪点是否满足对极约束，并使用RANSAC 方法消除误跟踪点。如图2 所示，随着时间的推移，从关键帧所提取的特征点将逐渐跟丢，则需重新进行特征点提取。

Fig.2 Diagram of fast tracking图2 快速跟踪示意图

已知地图空间点的三维坐标以及在当前帧下的像素投影，可以采用PnP［18］算法对当前相机的位姿进行估计。该算法的伪代码表示如下。

Input：set last keyframe as reference frame Ir，current camera frame Ic，use the map points and feature points of the key frame as the initial set mpts，kpts.

2.3 滑动窗口

系统长时间运行下，关键帧和地图点数量会逐步增多，后端优化问题的计算规模也会随之增大，进而影响前端跟踪的实时性，这一问题在嵌入式设备上的表现尤为突出。因此，本文采取滑动窗口策略来限制待优化的关键帧数量，控制光束平差法（Bundle Adjustment，BA）优化问题规模。

当窗口内帧数超过预设上限，将执行滑动策略：如图3（a），如果窗内第二帧不属于关键帧，则直接删除该帧观测数据，但保留该帧IMU 数据以确保IMU 预积分的连贯性；反之，如图3（b），如果第二帧属于关键帧时，则边缘化［19］最旧帧，将该帧的观测数据转为先验信息。

Fig.3 Sliding window strategy图3 滑动窗口策略

2.4 数据推送

一方面，随着时间推移，本地地图的数据量必然递增，而嵌入式设备因资源有限也必然无法满足SLAM 的运行需要。另一方面，SLAM 作为一种底层技术，所构建的地图除了用于自身定位之外，也要为更上层的应用提供用户级交互所需的数据基础，换言之，SLAM 地图数据需要上传共享。因此，为了进一步解决本地平台资源是有限的问题，本文对地图数据采用异地缓存处理的方法，对滑动窗口外的历史数据进行周期性序列化推送。

本文基于COVINS［20］构建一个边缘计算平台，一方面为上层应用提供地图接口，另一方面可以构建后端优化平台对缓存的历史数据进行深度优化，包含位置识别、地图融合和回环检测等功能，以此在边缘平台构建全域地图。值得注意的是，回环检测属于一种长期的数据关联，当边缘计算存在时，前端设备将不进行回环检测。这样，在满足实际业务需要的同时，更可以降低本地计算量，减少对实时性的影响。

3 实验及分析

实验以树莓派4B 作为目标机，搭载型号为BCM2835的64 位4 核处理器，内存为4G，如图4 所示。同时，设置一台PC 电脑作为边缘计算设备以及数据展示之用，其搭载型号为i7-11700的64位16核处理器，内存为16G。

Fig.4 Raspberry Pi 4B bare board图4 树莓派4B裸板

实验将从嵌入式设备的交叉编译、实时性对本文方法进行测试，并在TUM Visual-Inertial 数据集上对方法的轨迹误差进行比较。实验采用鱼眼单目相机，这种相机允许桶形畸变存在，能达到180°的超大视角，实现近距离拍摄更大范围的场景。实验相机图像分辨率为512x512、16 位灰度图，帧率为20Hz。同时，实验采用型号为Bosch BMI160 的六轴IMU，采集实时加速度和角速度数据，帧率为200Hz。

3.1 交叉编译

主流视觉SLAM 方法由于代码工程量大且编译耗时，在一般的个人电脑上进行开满线程编译可能会导致电脑假死情况，在树莓派这类资源有限的嵌入式平台即便只采用单线程进行编译也会因内存不足而被终止，因此只能选择交叉编译方式进行程序发布。此外，ROS 机器人操作系统平台所涉及的依赖库配置，大部分是绝对路径，不利于传统的交叉编译工具设置。

因此，为了解决视觉SLAM 编译困难的问题，本文基于Docker 容器引擎技术，在x86-64 电脑上运行arm64v8/ubuntu20.04 环境，虚拟了一个树莓派环境进行代码工程编译。相比于传统的交叉编译方式，Docker 容器上的操作与目标机平台上一致，而无需深究项目具体的每一个依赖项。

表1 展示了同一套代码工程分别在树莓派、PC 和Docker 上的编译结果和耗时情况。虽然同时开启多个线程进行编译，将有利于缩短编译时间，但同时也要求平台有更高的运行内存支持。树莓派单线程编译，PC 满线程编译，两种情况的失败原因都是因为内存不足导致编译器终止了编译过程。基于Docker 生成ARM64 容器环境，可以确保在软件环境一致情况下完成编译任务，直接推送到树莓派上即可运行。

然而，由于Docker 是基于QEMU 虚拟化技术实现在x86 架构下运行ARM 架构程序，所有指令都需要通过QEMU 进行软件动态代码翻译，实际性能相比于x86 程序将有所降低。因此，虽然Docker 借助于PC 硬件资源可以完成编译任务，但其耗时明显高于编译PC 版程序。

Table 1 Compilation results of code engineering on each platform表1 各平台代码工程编译情况

3.2 实验效果分析

实验以室内长走廊作为测试场景，并采用单目鱼眼相机和IMU 传感器作为系统输入，图5（彩图扫OSID 码可见，下同）展示了实验场景原图以及其特征提取、快速跟踪效果。

图6 实验表明，在长走廊场景下，纯视觉方法因为平均计算帧率低且场景相似，容易出现跟踪丢失以及回环检测误判，由于尺度缺失，从走廊往房间返回时，轨迹无法回到起点房间，轨迹误差较大。

图7 展示了ARM-SLAM 运行效果。得益于IMU 数据，在初始化时恢复了单目视觉的尺度因子，使得本方法能确保经历长走廊运动后，仍能返回最初的房间内，做到全局轨迹一致。同时，视觉与IMU 融合，使本方法在有效帧率较低的情况下仍能保持对特征点的有效跟踪。但值得注意的是，由于ARM-SLAM 没有针对每帧都进行特征点提取，因此其点云地图相比于ORB-SLAM3更稀疏。

表2 展示了不同场景下，ORB-SLAM3 和ARM-SLAM方法各自在树莓派上运行的平均帧率情况。因纯视觉方法在运行期间容易丢失而无法跑完整个实验，因此实验均采用视觉融合IMU 方案。实验结果表明，ORB-SLAM3 方法的平均帧率约为6.22fps，而在同一场景下，ARM-SLAM方法的平均帧率则为10.53fps，速度相对提升了69.29%。这得益于ARM-SLAM 方法基于OpenMP 对关键帧的特征提取和描述子的计算环节进行了并行加速，同时对非关键帧采取了快速跟踪策略，进而提升了整体的实时帧率。

Fig.5 Schematic of experimental operation图5 实验运行效果

Table 2 Comparison of the average frame rate of ORB-SLAM3 and ARM-SLAM running on Raspberry Pi表2 ORB-SLAM3和ARM-SLAM 在树莓派上运行的平均帧率比较

Fig.6 Trajectory map generated by pure visual method图6 纯视觉方法运行轨迹

Fig.7 Trajectory map generated by ARM-SLAM method图7 ARM-SLAM 方法运行轨迹

图8 展示了在corridor-1 场景下，滑动窗口和数据推送的轨迹效果。实验从房间出发，经历长走廊，并先后进入多个不同的房间，最终回到起始房间。其中，细线表示全局轨迹，是边缘缓存本地数据并作处理后的结果；粗线表示本地轨迹，随着时间的推移而逐渐消逝，本实验设置本地仅保留最近的100帧数据，因此最后只有起始房间附近才有本地轨迹数据。同样的场景下，不进行数据推送并进行历史数据删除时，程序占用内存约1.02GB；而如果开启推送，程序内存占用则降低到764MB。如此证明，本地设备仅存储与维护有限的本地地图时，能有效避免了本地存储和计算量的激增。

Fig.8 Schematic of sliding and pushing trajectory data图8 滑动推送轨迹

3.3 轨迹误差分析

由于TUM Visual-Inertial 数据集中缺少长走廊部分的真实数据，因此，实验环节仅对房间内的轨迹进行误差分析，包括单目相机运动开始和结束时在房间内的真实轨迹情况。

图9 展示了单目相机运动开始和结束时在房间内的真实轨迹情况，以及树莓派在该场景下分别运行ORBSLAM3和ARM-SLAM 的轨迹对齐效果。

表3 展示了ORB-SLAM3 和ARM-SLAM 方法的在同一场景下运行结果的绝对轨迹误差（Absolute Trajectory Error，ATE）情况。

Table 3 Absolute trajectory error comparison表3 绝对轨迹误差比较

得益于实时帧率的提高，ARM-SLAM 方法整体绝对轨迹误差均小于ORB-SLAM3，仅在最小值部分略高于ORB-SLAM3，反映了ARM-SLAM 在树莓派上运行的精度和轨迹全局一致性表现更优。

4 结语

本文通过IMU 恢复视觉尺度因子、快速跟踪策略、滑动窗口与数据推送等方式对ORB-SLAM 进行优化，研发出一种面向ARM 同构处理器的单目视觉SLAM 方法，并在以树莓派为代表的嵌入式设备上进行了实验验证，解决ORB-SLAM 在树莓派上编译困难、单目视觉尺度模糊、实时效率低、存储空间与运算能力有限等问题。该方法体现了低成本视觉SLAM 方法的优势，为进一步在嵌入式设备上的应用提供了有效途径。在未来的研究中，可以结合边缘计算设备，进一步从前端机体剥离非实时性约束的后端优化计算，逐步从单一智能体的有限作业转为多智能体协同作业，实现视觉SLAM 在虚拟现实、增强现实和搜索救援等大范围场景下的应用。

Fig.9 Schematic of trajectory alignment图9 轨迹对齐示意图