1. Detour用户界面2. 单线激光SLAM2.1 关键帧2.2 单线激光SLAM的计算过程

1. Detour用户界面

Detour是MDCS的D，负责定位导航。Detour支持单线/多线激光、地纹等定位导航方法。

图 1-1 D的用户界面

D启动后显示“概览”标签界面，该标签窗口的主要元素如下：

版本。软件版本及编译日期。
地图文件。当前使用的地图文件。在生产环境下更新激光地图时，应同步修改D的配置文件，以加载正确的地图，请核对此处的地图文件路径是否正确。
位姿。移动机器人在地面移动，其姿态可用(x, y, theta)表示。x & y表示机器人在世界坐标的位置，θ表示机器人绕Z轴旋转角度。D使用右手坐标系，逆时针旋转为正，顺时针旋转为负。此处的ls值表示定位状态，ls=2表示回环检测成功，如果不成功则>2，数字越大表示仅凭里程计行走越久。
显示选项。勾选“跟随小车”可使机器人在地图居中，跟踪其定位过程时使用。
操作按钮。
- 全图重定位。让D根据当前雷达帧数据，在激光地图中选择一个配准置信度最高的关键帧。使用此功能时应注意有无类似场景，比如相似的走道，重定位后应检查机器人位置是否正确。
- 图上手动定位。在机器人所在位置，根据其车头指向，在地图上的大致位置拉一条线，线指向与车头朝向一致。D根据手动拉出的指示线，在附近搜索相似的关键帧。手动定位准确性比全图重定位更高，一般使用该功能重定位偏航机器人。

操作时应注意以下事项：

D窗口全屏或拉伸时，其CPU占用量会上升，因为点云数据量较大，显示越多计算量越大。调试时可放大窗口，生产时不建议调整该窗口尺寸，有可能增加CPU开销而拉低SLAM计算效率。
图1-2是D控制台界面，其内核运行在命令行窗口。新版D在启动后会隐藏命令行窗口，旧版则两者都显示。控制台界面可以关闭，如此能节省约20%处理器资源，在低配车载工控机上能有效提高定位导航稳定性。

图 1-2 D的命令行窗口

注意：关闭D的命令行窗口即关闭D，定位数据停止输出，自动驾驶（Clumsy）在300ms（默认参数）未收到定位数据时停车。如果鼠标点击该窗口文字，操作系统则认为操作者将复制文字，会挂起D进程，直到敲回车键（复制选中文字）后解除。调试时应注意此问题，避免鼠标误操作而挂起D进程。

如果要复制D命令行内容（D运行时输出日志），可点击窗口左上角图标，在窗口上下文菜单中选择“全选”（图1-3），然后复制到记事本，存为日志文件。

图 1-3 复制D命令窗口的运行日志

2. 单线激光SLAM

图 2-1 单线激光SLAM界面

选择“单线激光SLAM”标签，然后选择Lidar点云图层的“mainmap“，点击”加载图层“按钮，选择激光地图文件（后缀名为2dlm）。地图窗口显示激光地图，如图2-1所示。

说明：单线激光SLAM无需授权，因此地图窗口的“试用版”提示可以忽略。如果使用高级功能，如地纹导航，则需要授权，否则高级功能无效。

要用好单线激光SLAM，关键帧必须看懂。

2.1 关键帧

关键帧是回环检测的依据，用一个例子比喻。假设你在出租车上，你知道回家的最近路线，而司机不清楚，你告诉司机如何开。设车子在一条直路上，500米后的路口右转，直行200米到家。那么你会告诉司机直行，并根据路边经过的景物估算余下行程。比如，刚经过一个药店，大约50米后到路口，你就会提醒司机前面50米左右路口右转，司机则降低车速准备转弯。药店就是关键帧，如果您不熟悉关键帧这个术语，可替换为“路标”，内涵一样。当你看到药店时，就得到了绝对位置，然后根据车窗外移动的景物判断距离，到路口（下一个路标）又得到绝对位置，随后右转。此过程是典型的视觉SLAM过程，单线激光SLAM原理也是一样。

机器人的大脑是计算机，与人相比，计算机不擅长处理图像，但擅长计算，而人能够瞬间识别一个图像，但不擅长计算。如模拟人类开车过程来实现定位导航，应如何处理？SLAM使用ICP（Iterative Closet Point，迭代最近点）算法来识别关键帧（即路标）。

ICP（Iterative Closest Point，迭代最近点）算法是 SLAM 问题中的一种重要算法，主要用于解决机器人在运动过程中如何同时估计自身位置和周围环境结构的问题。 ICP 算法原理如下：

初始化：首先，机器人需要通过某种方式（例如 GPS、激光雷达、视觉传感器等）获取初始位置和局部地图。
局部地图匹配：在构建全局地图之前，机器人需要先利用传感器收集到的数据在本地构建一个局部地图。局部地图通常包括机器人在局部区域内的位姿（位置和姿态）信息以及该区域内的特征点（例如角点、边缘等）。
全局地图构建：当机器人运动到新的区域时，需要将新采集到的局部地图与已知的全局地图进行匹配。ICP 算法的核心思想是通过迭代计算两个点集中最近点的距离，使局部地图与全局地图之间的点集距离最小。
地图优化：当 ICP 算法收敛后，机器人已经获得了一个较为精确的位姿和全局地图。此时，可以通过一些优化算法对地图进行优化，提高地图的精度和鲁棒性。通过上述过程，ICP 算法可以帮助机器人实现在未知环境中的定位和建图。

简单而言，ICP算法可理解为你有两个图片，通过平移和旋转，将第二张图片与第一张图片完全重叠时，则判定两个图片完全一样。第一张图片就是建图时候的关键帧，第二张图片就是定位时候的点云。

图 2-2 以ICP配准的点云

图2-2中有两个点云——蓝色和红色，人类一眼可见，计算机则用ICP算法将红色点云旋转并平移，当其与蓝色点云的每个点距离最小时，则点云配置成功，即红色点云与蓝色点云完全相同。

实际应用中两个点云不可能都一样，一者机器人行走路线不可能与建图路线完全一致，二者由于激光雷达测量误差和地面（或底盘精度）起伏，得到的点云不会完全相同。因此用ICP配准点云时有个置信度，以两个点云相似程度来打分，相似度大于设定阈值时认为一致。通过计算第二个点云配准时的旋转和平移量，可得到机器人相对于关键帧的位姿。

知道关键帧是路标，回环检测即在地图中观察到同一个路标，那么就能理解关键帧在建图和定位的作用。

建图。打开D的建图功能，遥控AMR绕规划路线行走时，D将第一个点云作为关键帧，然后使用里程计推算机器人航迹，当机器人走出一定距离（默认为2米）时，D会建立第二个关键帧。当D认为当前关键帧置信度低于设定阈值时（AMR转弯、环境轮廓变化增大），也会建立第二个关键帧。基于里程计创建关键帧的过程称为“局部建图”，由于里程计累积误差，局部建图的关键帧会逐渐偏离实际位置。当我们遥控AMR顺着去路回来时（让机器人从不同角度观察到同一个关键帧），D再次看到关键帧，然后根据全局地图修正其位置，该过程称为”全局优化“。经过优化后的关键帧位置与实际一样，用于定位。
定位。定位过程与建图相似，只不过保留了观察关键帧（回环检测）动作，无需创建关键帧（建图）。

由于关键帧是根据机器人行走顺序建立的，因此关键帧之间有”拓扑“关系（从这个关键帧可走到那个关键帧），此关系能够降低关键帧搜索计算量，使车载计算机能够及时完成点云配准计算。图1-1中可见关键帧之间的拓扑关系。拓扑关系也约束了关键帧搜索范围，不会因两处环境轮廓相似，而将机器人位置瞬间”飞“去另一个地方。

2.2 单线激光SLAM的计算过程

D的计算过程如下：

读取Medulla发布的雷达点云。根据雷达帧间隔时长设置ls的增加步长，当雷达数据丢失时（间隔延长），ls会快速增加。
过滤雷达点云的无效数据。计算移动物体，包括经过雷达的行人、物流车辆，因激光雷达安装不良而扫到的地面（成一条直线，并与雷达保持一定距离而移动）。被过滤的点云在地图上以灰色显示，请注意算法只能处理一些特定场景，像行走到雷达旁停下，等待一会后离开的就不能过滤（停止一段时间被判定为固定物体，参与到点云配准计算）。
更新激光里程计。使用“scan to scan matching“算法更新激光里程计。为简化论述，我们只讨论激光里程计场景，轮里程计和地纹里程计的计算方法另外说明。
搜索关键帧。根据当前关键帧与激光里程计（当前位姿），搜索相邻的关键帧，找到时（置信度>0.3）则回环检测成功，更新激光里程计位姿。

计算过程可在D的控制台窗口观察到，如图2-3所示。

图 2-3 D的图像日志

D在回环检测失败时，会自动记录到图像日志，日志存放在log/detour-workspace子目录下（还有个lo子目录，每秒钟记录点云配准情况，该日志数量极大，调试一般看detour-workspace，lo看细节）。图形日志是png文件，由当时的命令行窗口 + 激光地图叠加而成，如图2-3。我们解读图2-3的日志：

16:54:21.453。激光里程计输出，从关键帧9120（关键帧编号后4位）跳到下一个关键帧8501，使用的激光地图是230910-3.2dlm。关键帧编号后面的括号是关键帧位姿：x、y、θ。score（配准分数）是0.690376，非常好。
激光里程计间隔几秒输出一条日志，未见错误提示，配准分数几本在0.6以上，说明环境轮廓稳定，激光雷达视野良好。

我们再看一个图像日志，此时AMR在待命点重启，见图2-4。

图 2-4 待命点重启D时的图像日志

图2-4的定位并不理想，AMR左侧关键帧为深红色，表示置信度大于0.3，但是距离超过搜索半径而无法锁定。从07:21:18.609的日志可见，下一个关键帧是0760，右边括号的step=9999，表示这是手动定位结果，配准分数0.6395749，相当高的分数，观察青色的点云与环境轮廓重合，与分数一致，因此环境轮廓没问题，但是关键帧距离远，导致回环检测失败，手动定位给的位姿。

我们来看一个回环检测失败的案例。

图 2-5 定位丢失前的图像日志

图2-5的里程计日志无异常，此时最后一条日志是10:13:37.480，锁定关键帧8329，配准分数0.3787362，关键帧右侧的分数保留小数点后2位，因此是0.38。

图 2-6 回环检测失败

图2-6，10:13:49.231，搜索4个关键帧均失败（配准分数低于0.3）。该AMR的导航雷达位于车头右上角，右侧灰色线条是输送线移门，雷达视野被遮挡大半，导致点云无法配准关键帧。结合图2-5来判断，AMR自下往上行走，在图2-5开始左旋（以前进方向参考），到图2-6时导航雷达逐渐靠近移门，视野被遮挡大半，导致回环检测失败。

由于AMR车头未见料车腿反射而形成的点云，判定是空载离开库区，因靠近输送线移门而回环检测失败，仅用激光里程计行走。

我们来看一个因环境轮廓改变而丢定位的案例。

图 2-7 倒退行走时牵引车跟随

图2-7，AMR从6点向12点倒退行走，司机驾驶牵引车跟随同向行驶。牵引车头在雷达前方形成一条线段，此时导航雷达右侧的移门打开，能观察到输送线，且未被标记为“移动物体区域”（红色区域）。导航雷达紧贴移门，且前方视野被牵引车挡住，回环检测失败。

图 2-8 里程计累积误差大于关键帧搜索范围

图2-8，01:19:30.011，最后一次回环检测，分数0.3891303。27秒后，01:19:59.528，匹配到下一个关键帧，分数0.3497557，但位置偏差过大（根据里程计位置）而被丢弃。此时ls=5，说明根据里程计推算，应有关键帧但是回环检测失败（被牵引车干扰），里程计累积误差使AMR偏离了实际位置，导致关键帧搜索失败。观察9点方向的双立柱（车身右侧），立柱靠墙位置有点云，右上角的导航雷达因车身遮挡，不可能看到立柱轮廓。以导航雷达视角看，AMR右前方点云与立柱、参考挡板相似，判断里程计被牵引车拉偏。

图 2-9 AMR丢定位跑偏

图2-9，由于里程计偏差较大，导致回环检测一直失败，01:20:51.960到01:21:01.129的日志可见。AMR一路靠激光里程计倒走出去，由于激光里程计角度精度较好，因此在出去路上未跑歪。

我们根据图像日志来跟踪AMR的实际轨迹，学会此方法可复盘丢定位过程，从而定性，进而采取改进措施，以永久消除类似问题。

图 2-10 根据点云和激光地图推断AMR偏航位移

根据时序顺序浏览图像日志，图2-10可见点云与绿框内建图轮廓相同，据此推断激光里程计的偏移量。

注意：导航雷达前方有一道灰色线段，这是牵引车车头轮廓，说明牵引车一路跟随出来。

鼠标点选AMR附近的关键帧，D状态显示鼠标位置，将其Y坐标记下。AMR实际位置在绿框内的相应轮廓（是个角点），鼠标点选其附近的关键帧，记下Y坐标。将两个Y坐标值相见，得到实际位移量，约4.4米。即AMR相对于实际位置滞后4.4米。观察点云与墙体轮廓有夹角，说明AMR角度也有偏斜。由于偏差远大于关键帧半径，回环检测一定失败，倒走到主干道转弯前就会碰到安全围栏而避障报警，或者斜走靠近墙体而避障报警。