揭开关于机器人自主移动的神秘面纱

本文开始，我们将从介绍其中的一个关键性模块，即时定位与建图技术（Simultaneous Localization and Mapping，以下简称SLAM）入手，逐步为读者揭开关于机器人自主移动的神秘面纱。

高仙是全球较早从事自主移动技术研发与应用探索的机器人公司之一。成立至今，高仙不断打磨并完善了机器人全场景移动技术，并通过全球超过50+机型、1万+机器人终端在5000+行业的落地积累，实现了从室内到室外等不同环境、从清洁到安防等不同业务领域的广泛应用。高仙机器人全场景移动技术给越来越多的机器人赋予了自主移动必需的“躯壳”、“大脑”和“灵魂”。

SLAM问题的提出

试想人如何判断自己在周围环境中的位置？没错，眼睛！不过，双脚的移动，大脑对运动的感知等也能给你提供相对运动的信息。事实上，人就是通过综合这些感官信息来判断位置和移动的，这也是SLAM技术的灵感来源。SLAM技术的出现彻底解决了机器人领域中“我在哪儿”的问题，使机器人在未知环境中的自主移动成为可能。SLAM通过输入的多种外部和内部的传感器数据，使用算法求解出一个准确的机器人位姿（即位置和姿态，可以理解为坐标和朝向，以下简称pose），同时，将每个pose处得到的传感器数据拼接起来形成完整的地图，又可作为计算位姿的依据。

数学上的状态估计

通常在数学上，可以将SLAM建模成一个状态估计问题，即：

其中，公式(1)为运动方程，表示在k时刻，机器人的pose(x(k))由k-1时刻的pose(x(k-1))和k时刻的运动输入(u(k))所决定，由于实际物理环境总会引入误差，所以添加一个噪声量(v(k))对状态变化形成一定约束。公式(2)为观测方程，表示k时刻的机器人传感器观测(z(k))，由当前时刻的机器人pose所决定。同理，因为物理环境的影响，会带入一定的观测误差，即w(k)。

动态贝叶斯网络描述

上述过程也可以使用动态贝叶斯网络(Dynamic Bayes Network,DBN)来描述，如图1所示：

图1.动态贝叶斯网络

整个网络就是一个概率图模型，箭头的指向表示变量的依赖关系，如上图中的子图x0->x1，和u1->x1，可以用条件概率描述。

问题求解方法

由于目前激光SLAM作为成熟的算法已经在无人驾驶和机器人中得到广泛应用，因此本文主要介绍激光SLAM方案。

顾名思义，激光SLAM是以激光数据作为传感器输入的SLAM方案。图2.a展示了TOF激光雷达的测距原理，通过电机带动旋转，将激光脉冲不断投射到障碍物同时接收反射回的激光脉冲，将光速与飞行时间差相乘，求得雷达到相应障碍物的距离（TOF测距原理测距范围广。此外还有基于三角测距的激光雷达，主要针对室内的中近距离测距，图2.b展示了三角测距激光雷达的工作原理。）。

图2.a.TOF测距激光雷达的工作原理

图2.b.三角测距激光雷达的工作原理

2D与3D激光传感器

图3为2D激光传感器数据的表示。左图为工作区间在270度激光数据，可以看到激光从+135度扫描到-135度，获取了水平面上每隔一个角分辨率的测距信息。右图为ros中激光的数据显示。图4为3D激光的数据表示，相对于2D激光雷达，3D激光在垂直方向会同时发射多组激光脉冲，获得了三维空间中对障碍物的测距信息。

图3.2D激光传感器表示

图4.3D激光传感器表示

由于SLAM本质是个系统状态估计问题，即在给定系统输入的条件下，估计出机器人的pose和地图点的坐标，针对两维空间的SLAM，式(1)具体可以表示为：

通过求最大条件后验概率(MAP)，计算出相应的pose和地图点坐标，即

延伸出的两类SLAM算法

A.基于贝叶斯滤波器的方法

基于贝叶斯滤波器的方法包括Kalman filter[1]、Extended Kalman fiter、Particle filter等，是贝叶斯迭代状态估计理论，即先对机器人运动进行建模，构造出贴合物理场景的运动方程和观测方程，如轮式机器人常使用基于速度的运动模型。之后，套用卡尔曼滤波的五条公式，进行状态预测和测量更新。状态预测依靠运动方程，从当前状态估计出下一时刻的机器人pose。而测量更新，则是在机器人观测到新的点时，对之前的预测值进行修正。可以看到，该过程是一个递归估计过程，从k时刻到k+1时刻的估计。

由于卡尔曼滤波算法是针对线性系统且高斯分布的最优无偏估计，而实际场景中，机器人的运动并不满足线性特性，且噪声项不满足高斯分布，因此使用卡尔曼滤波不能精确的计算出结果。扩展卡尔曼滤波可以将线性系统约束扩展到非线性系统，获得更好的结果。然而其依旧不能逃出高斯分布的限制，因而实际中使用粒子滤波代替上述方案。粒子滤波类SLAM不依赖参数化的运动方程，使用大规模粒子点去模拟无参数化的分布，理论上可以近似各种分布。如早年的业界流行的GMapping，即采用了该方案。

滤波类算法最大的问题，是无法处理大尺度场景的建图。由于滤波类算法是基于递归计算，下一时刻的估计值依赖于上一时刻的估计，因而在大尺度场景下，由于系统参数和传感器观测的不确定性，会造成误差的逐渐累积，一旦当前时刻的估计出现偏差，之后是无法修正该误差的，其结果是最后无法获得一致性的地图。

B.基于图优化的方法

基于图优化的SLAM出现解决了一致性建图的问题。如果说滤波类SLAM是属于序惯估计的话，基于图优化则是属于批处理。图优化SLAM是目前主流的SLAM方案。其主要分为两个模块，前端和后端。如图5所示。

图5.基于图优化的激光SLAM

前端负责从里程计和激光数据中求得pose，后端采用回环检测，构造闭环约束，通过最小化观测和估计残差求得优化后的pose。

前端方面，先从里程计获得初始的pose，之后通过激光数据连续帧匹配，求得激光约束后的pose(注意这里使用了一次观测信息)。在连续帧匹配上，有ICP[2]、NDT[3]、暴力匹配[4]等算法。如目前主流的激光SLAM-cartographer采用暴力匹配作为前端方法，即使用激光扫描匹配，在机器人运动中，通过匹配前后两帧的激光变化，求得机器人的相对运动。而其中的一个关键模块是三维窗口遍历寻优。这是一种穷举搜索算法，针对平面运动的机器人，可以将pose分解为三个维度，即x轴，y轴，角度轴。通过构建三层for循环，分别对x方向、y方向、旋转角度方向进行遍历搜索，寻找到最优的pose匹配作为估计结果。

需要强调的是，前端属于局部估计，即只能估计当前时刻机器人相对上一个时刻的pose变化。事实上，由于估计所引入的误差，随着运动范围的扩大，该误差会逐渐累积，导致最后错误的结果(这个问题和滤波器类SLAM一样)。解决该问题，可以使用闭环检测(Loop Closure)[5]的图优化后端。通过判断机器人是否回到历史中的某一点，从而构建闭环约束，通过非线性优化将运动过程中的误差分散到参与优化的每个pose中，从而消除误差累积。图优化的核心是构造误差函数，我们先举一个简单的例子，如图6所示

图6.图优化约束构建

图中机器人从i点运动到j点，根据前端我们计算出了机器人的在j点的pose，即xj，则xj在xi为坐标系的坐标是z_hat_ij，此时我们又获得一个激光的观测，通过激光帧匹配得到xj在xi坐标为z_ij。假设观测和实际运动是完美的，则z_ij=z_hat_ij。

实际中，准确的xi和xj应该满足上述误差最小，该过程构造了无约束的非线性优化问题，之后就是借助常用的优化方法做梯度下降了。

我们再举一个闭环检测的例子，如图7所示：

图7.机器人运动轨迹

机器人从1点运动到4点，假设机器人运动到4点时，观测到了1点，此时，通过前端匹配计算出4点在1点的pose，即z14，同时因为机器人从1->2->3->4，通过pose间变换，估计出了4点在1点的pose，hat_z_14

实际中，都是以pose间的变换矩阵作为变量参与优化。

通过对比图优化和滤波器SLAM，两者的本质是统一的，都是通过输入观测，修正预测量，求解最大后验概率，图优化的方式只是将最大后验通过计算负对数的形式，转换成最小化带有误差项的二次型。而两者区别是，图优化技术采用了批处理方式，引入了更强的约束（闭环约束），修正了滤波器基于一阶马尔科夫假设的状态递归估计错误累积。

图8显示了闭环的效果，左图中，未使用闭环检测纠正pose，可以看到机器人行驶一周之后，在应该回到原点的位置，pose出现较大偏差，导致地图没有正确闭合。右图中，由于使用了闭环检测，纠正了pose的累积误差，使得建图与实际场景一致，取得了较好的效果。

图8.闭环优化效果

最后需要补充的是，由于激光SLAM算法框架较为固定，大同小异，那么决定一个SLAM算法的好坏就落在了对具体工程问题的处理上，如笔者在日常开发中，处理并解决了以下的问题：

A.如何有效的对激光噪点进行去噪？

B.如何克服动态环境下的定位漂移问题？

C.几何结构相似的环境如何避免闭环错误？

D.里程计初始值异常跳变如何检测？如何处理？

E.如何克服重复扫图导致的地图分辨率下降？

F.码盘里程计与激光数据时间戳虽然一致，但实际上存在延迟，如何解决？

G....

最后笔者认为，作为一名高仙的SLAM算法工程师，不仅需要深耕理论，同时对于工程问题也应该有更多的思考和改进。

正是这样的心态和追求，才奠定了高仙在机器人自主移动领域的领先地位。目前高仙拥有百万平米的建图能力，超出行业平均水平20倍。

图9.室外百万平米建图-龙东大道

图10.高仙SLAM-Mapping the World

至此，我们对SLAM技术要解决的问题，以及在机器人、无人驾驶、无人机等领域内比较成熟的激光SLAM算法有了一定的认识。SLAM对于机器人的行动和交互起到至关重要的作用，它是使智体知道自己在那里、周围环境如何以及下一步该如何行动的关键性基础。