同步定位与地图构建（SLAM）是自主移动机器人的核心技术。它涉及同时构建机器人环境的地图，并确定机器人在该地图中的位置。SLAM计算量巨大，在资源受限的机器人（如消费级家用机器人）上部署时，通常需要采用技术手段来简化计算。
其中一项技术是使用低精度浮点运算，即减少表示小数点后数字的位数。该技术在深度学习中很流行，将位数减半（从标准的32位降至16位）可以使计算效率翻倍，而对精度影响甚微。
但是，将低精度算术应用于SLAM更为复杂。基于深度学习的分类模型是离散值的，而SLAM涉及求解具有连续值函数的非线性优化问题，这需要更高的精度。
在某中心，我们通过设计一种新颖的混合精度求解器来解决这个问题，该求解器在SLAM算法中的非线性优化问题中结合了64位（fp64）、32位（fp32）和16位（fp16）精度。这项创新为更快、更绿色的设备端导航铺平了道路。
通用框架
SLAM算法有两个关键组成部分：视觉里程计（VO）和回环检测（LC）。视觉里程计根据最新的观测数据，实时估计机器人的姿态，即其在地图上的方向和位置。当机器人识别出它到达了一个之前访问过的地方时，它会通过全局校正其地图和位置估计来闭合回环。
视觉里程计和回环检测都涉及求解非线性优化问题，分别是束调整（BA）和位姿图优化（PGO）。为了高效求解它们，SLAM系统通常使用近似方法，将这些非线性问题重构为一系列线性化优化问题。如果目标是找到姿态估计值x，那么每个线性问题都旨在最小化线性化误差函数，该函数是当前误差函数及其一阶修正项之和。一阶修正项是雅可比矩阵（函数一阶导数矩阵）与姿态估计更新值的乘积。这些线性问题通常通过因式分解求解，使用乔列斯基（Cholesky）或QR方法。每个线性化优化问题的解就是当前姿态估计的更新值。
一般过程是从x的当前近似值开始，计算误差函数和雅可比矩阵，求解一个线性优化问题，并相应地更新x，重复此过程直到满足某些停止标准。在每次迭代中，误差函数的值被称为残差，因为它是上一次迭代后剩余的误差。
在BA和PGO的非线性优化中，最昂贵的计算是雅可比矩阵的计算（约占优化时间的15%）和线性问题的求解（约占60%）。简单地从头到尾用半精度（fp16）求解任一问题都会导致精度降低，有时甚至会出现数值不稳定。
为了缓解这些困难，我们对矩阵进行正则化和缩放，以避免溢出和秩亏。秩亏发生在雅可比矩阵的列线性相关时。通过仔细的实验，我们进一步确定了那些必须以高于fp16的精度执行的计算，并提出了一种混合精度非线性优化求解器。
我们发现，为了匹配纯双精度求解的精度，以下两个组件必须以高于fp16的精度计算：
- 残差必须用单精度或更高精度评估；
- x的更新，这是一个六自由度的位置-角度更新，必须用双精度执行。
尽管这个通用的优化框架同时适用于BA和PGO，但由于线性问题中矩阵的结构和属性不同，这两个应用的细节也有所不同。因此，我们为相关的线性系统提出了两种混合精度求解策略。
视觉里程计
对于视觉里程计，传统上人们使用基于滤波器的方法，但这种方法可能存在较大的线性化误差。近年来，基于非线性优化的方法变得更加流行。这些方法通过最小化误差函数来估计机器人的位置和姿态，该误差函数是地标的重投影与图像帧中观测值之间的差值。这个过程被称为束调整，因为我们正在调整一束光线，使投影与观测匹配。
基于BA的视觉里程计在一个包含固定数量（关键）帧的滑动窗口上运行。平均而言，新的关键帧以10Hz的频率到达。挑战在于在给定的时间预算内解决BA问题。一种流行的方法是求解等价于线性化优化问题的正规方程；这涉及到对海塞矩阵（Hessian，即残差二阶导数矩阵）的近似。
BA问题涉及两组未知状态变量：一组表示机器人的姿态，另一组表示地标位置。减少BA问题计算负担的一种方法是边缘化相机姿态和地标之间的约束，并首先关注相机姿态。在SLAM领域，这个过程被称为舒尔补或地标边缘化。
这个边缘化步骤可以极大地减小需要求解的线性系统的规模。对于一个50帧的BA问题，雅可比矩阵的大小通常是5,500 x 1,000，海塞矩阵的大小是1,000 x 1,000。解耦约束将线性系统的规模减小到300 x 300，足够小以至于可以用直接或迭代求解器求解。然而，这种策略需要同时构建海塞矩阵和执行部分边缘化步骤，这在实际应用中开销很大。
我们的混合精度线性求解器混合了单精度和半精度，基于共轭梯度正规方程残差（CGNR）方法，这是一种直接应用于线性优化问题的迭代方法，无需显式构建海塞矩阵。
正如在通用框架中一样，将所有计算简单地转换为半精度会导致精度降低。在我们的实验中，我们发现，如果以半精度计算矩阵-向量乘积，而所有其他操作以单精度执行，我们将保持SLAM管道的整体精度。
矩阵-向量乘积是CGNR迭代中的主要计算，通常占计算成本的83%（以浮点运算次数计）。这意味着，如果运行在某中心V100 GPU上，与单精度线性求解器相比，混合精度求解器可以节省至少41%的求解时间。
回环检测
在SLAM流程中，来自VO的局部姿态估计通常会出现较大的漂移，尤其是在长时间运行的情况下。回环检测可以纠正这种漂移。
对于真实世界的地图估计，如果没有LC校正，平均轨迹误差可能在0.1米的数量级，这在实际应用中是不可接受的。应用LC校正后，该误差可降低到10^-4米。
LC调整涉及解决一个全局PGO问题。与BA问题类似，它也是一个非线性优化问题，并且可以在相同的混合精度框架内求解。但是，由PGO问题产生的线性系统比BA问题产生的线性系统规模更大、更稀疏。
随着越来越多的回环被闭合，问题规模可能从几百个姿态增长到几千个姿态。如果我们用矩阵的行数来衡量矩阵的大小，在回环检测期间，其规模可能从100的量级增长到10,000的量级。在双精度下直接求解这种规模的稀疏矩阵具有挑战性，特别是考虑到设备端应用的时间和计算限制。对于一个真实的轨迹估计，在满CPU使用率下，PGO问题的求解时间可能长达8秒。
这导致了为PGO问题设计混合精度求解器的不同策略。由于雅可比矩阵的稀疏性，我们的混合精度方法仍然基于迭代的CGNR方法。但为了加速CGNR迭代的收敛，我们在每次迭代中应用一个静态的不完全乔列斯基预处理器。乔列斯基分解将一个对称线性系统分解为两个三角矩阵的乘积，这意味着它们的所有非零值都集中在矩阵对角线的一侧。这个分解步骤开销很大，所以我们只对整个问题执行一次。计算成本主要取决于预处理器的应用，这涉及求解两个三角系统。在我们的时间分析中，这一步消耗了每次线性求解中约50%的计算量。
为了加速优化，我们不是以半精度计算矩阵-向量乘积，而是以半精度求解三角系统，同时保持所有其他操作为单精度。使用这种混合精度求解器，我们几乎可以匹配全精度求解器的精度，同时平均减少26%的计算时间。
我们在VO和LC应用上的结果表明，由于半精度算术的高效和低能耗特性，混合精度求解器可以使设备端SLAM更快、更环保。
致谢 以下人员对这项工作做出了同等贡献：某中心硬件部门应用科学家Tong Qin；某中心硬件部门应用科学经理Sankalp Dayal；某中心设备部门软件开发工程师Joydeep Biswas；某中心硬件部门副总裁兼杰出工程师Varada Gopalakrishnan；某中心设备部门高级首席工程师Adam Fineberg；某中心硬件部门软件、机器学习和移动性高级经理Rahul Bakshi。
</div>
