激光SLAM工作流程

一、前言

本文从应用角度说明Detour的激光SLAM，以便读者从原理角度理解功能，能够在实践中有效定位问题，采取正确的方法处理。

二、激光里程计

参考链接：

Detour简介 - MDCS wiki

安装Detour - MDCS wiki

MDCS参数表:Detour - MDCS wiki [2.2]

使用手册 - 分析定位问题 - MDCS wiki

激光里程计是SLAM的“前端模块”。激光里程计接收来自激光雷达的点云（雷达扫描一圈的测量输出），将相邻点云配准，得到相对位移，积分位移得到机器人位姿。

激光里程计由2个模块构成，由里程计线程执行：

• 帧间位移：计算相邻点云的位移，积分为机器人位姿。
• 局部地图：维护一个机器人航迹上的“小地图”，修正帧间位移计算的累积误差。

2.1 运行流程

1. 接收点云：等待激光雷达驱动推送的点云帧（雷达扫描一周输出的点云）。
2. 预测位姿：根据机器人的运动速度、角度估计下一个位姿。
3. 更新遮罩：点云中的“移动物体”（包括噪音）被遮罩包裹，在后续计算中被过滤。“移动物体”指低速移动的物体（速度低于1m/s，且在移动）。
4. 帧间位移：计算与上一个点云帧的相对位移。
5. 纠正畸变：纠正点云的运动畸变。
6. 地图配准：点云帧与局部地图的关键帧配准，修正帧间位移的积分累积误差。
7. 质量检查：检查里程计计算质量。

2.2 接收点云

里程计启动时等待点云帧，如果超过10秒则输出："still waiting for data from xxx after 10s..."。表示激光雷达没输出，一般是驱动（或驱动配置参数）错误引起。

里程计运行时，如果出现跳帧（点云数据丢失）则输出："dangerous interval: xxx, time lag= xxx ms"。一般是算力低导致里程计来不及处理数据，比如CPU过热后降频；或者激光雷达短暂离线。

当跳帧严重时，输出："... , lstepInc->xxx"。lstep将快速增大。此时机器人位姿很可能与实际位姿出现显著偏差，Detour地图界面上的当前点云（青色）与地图轮廓不再贴合。

如果点云输出延迟超过1秒，输出："Bad interval compared to time lag, lag=xxx, timebudge=xxx, interval= {interval} -> xxx"。以及"Too large time lag and interval, xxx ms, restart, lstep+=xxx ->xxx"。出现这类日志表示激光雷达输出有严重问题，应检查相关硬件（激光雷达、CPU算力）。

备注：lstep是激光地图的关键帧锁定步长。正常情况下，lstep=2（Detour状态栏缩写为"ls"）表示关键帧及时锁定，机器人位姿是精确的。当关键帧不能及时锁定时，比如较差的点云质量，lstep会增加。可将lstep当做“置信度”，参见 Detour-API - MDCS wiki 的 2.1.1。

Detour地图界面将点云帧显示为青色点，这些点构成扫描线。

图 1-1 点云形成的扫描线

地图上的白色轮廓线由关键帧的点云构成，框选关键帧，其点云显示为红色点。

图 1-2 关键帧的点云

根据青色扫描线与白色轮廓线可判断机器人位姿是否正确：重合则没问题，脱开则位姿错误。

2.3 预测位姿

根据机器人的移动速度和角度，预测机器人下一个位姿。AGV是低速车辆，其航迹是平滑的，因此预测相对简单，不会输出错误日志。

2.4 更新遮罩

遮罩用于过滤“移动物体”：

• 低速移动：这些物体的移动速度低于1米/秒。比如拖着料车的牵引车，行人等。
• 持续移动：处于运动状态，或者短暂停下（小于30秒）后移动。

以下不属于“移动物体”：

• 快速移动：移动速度高于1米/秒。比如焊装机器手、总装车间下线的成品轿车（驶去测试区）、室外马路上的车辆。
• 长时停下：驻留时间大于30秒。比如车间参观人群，在雷达旁长时间站立，然后四散离开。

还有“相对静止”：

• 平库货物。堆放在平库的货物，过一段时间才会被移走。

遮罩不会过滤后两类物体。“快速移动”和“长时停下”物体一般在物流通道和生产区，可将这些区域标记为“物品移动区域”，过滤区域内的点云，与遮罩的动态算法形成互补。

注意：不要滥用“物品移动区域”。原则上仅用于物流通道，“相对静止”物体对里程计是非常重要的，如果滥用“物品移动区域”，可能抹掉可用点云，使里程计缺少计算所需点云，造成定位失败率上升。

如果遮罩过滤后的点云数量低于 minPointsAmount，输出："* observed xxx->xxx, too few after masking, reset mask, lstep+=10->xxx"。minPointsAmount是点云配准所需的最少点云数量，过滤后点云低于该阈值时重启遮罩。

2.5 帧间位移

帧间位移包含了点云（无反SLAM）和反射点（有反SLAM）。当帧间点云配准分数低于 ScoreThres时，输出："Bad Seq, t=xxx ms, cnt=xxx, interval=xxx, score=配准分数, n:xxx-xxx, lstepinc+=5 ->xxx"。这条日志表示点云变化剧烈，无法计算帧间位移，里程计必须重启。

这个问题要结合运行场景来看，以下是可能性，但不是穷举，仅供参考：

• 环境导致：比如机器人下坡，雷达俯仰前后的观测环境剧烈变化。
• 人为因素：一群人站在雷达前，然后散开；或者人蹲在雷达前操作面板，然后走开。
• 近距离遮挡：机器人在狭窄走道行走（雷达视野受限），经过近邻立柱时一侧视野被遮挡；或者靠墙旋转。

一般来说，ScoreThres不需要调整，只有在特殊环境下（有固定的点云突变场景，且连续出现）才要调整。对于人为因素，可耦合轮里程计应对，避免近距离遮挡（一群人，物流车等快速移动物体）而“带漂”里程计（相对运动造成）。

注意：帧间位移是个相对概念。当周围环境是稳定的，机器人移动时，帧间位移计算不会有问题。当周围环境是动态的，机器人静止时，相对移动的物体会“拉偏”里程计。比如，AGV在车道旁库位停车，有一辆牵引车从雷达前经过，牵引车相对运动会拉偏里程计。在人车混合作业的场景中，如果雷达安装在低位（地牛、潜伏车等），应该耦合轮里程计来增强SLAM鲁棒性。如果AGV未配置IMU（仅用驱动器编码器输出角度误差较大），可在停车时停止里程计（通过API）。

里程计除了计算帧间位移，还保留了最近N次帧间位移配准分数。历史数据如果较差，也会重启里程计，并输出以下日志：

• "sequential xxx history score bad, avg={分数均值}, restart(lvl=xxx), lstepinc+=15->{xxx}"
• "xxx history score={分数均值}, too low, restart(lvl=xxx), lstepinc+=1=xxx"

这种情况一般与环境有关，比如靠近墙体旋转。可降低 GoodScore 应对。

提示：分析里程计问题时抓住“点云变化程度”这个点。帧间位移配准分数与点云变化程度负相关，变化小分数就高，否则相反。而点云变化程度与雷达视野正相关，如靠墙旋转时，近距离墙体遮挡旋转中的雷达，点云会剧烈变化。

2.6 纠正畸变

基于运动学纠正点云（的运动）畸变。相关参数是 correctionType、correctionLvl，这些参数由懒书维护，用户无须关心。

提示：如果不确定参数默认值，可用Detour.exe创建一个新配置，即见默认值。

2.7 地图配准

局部地图由里程计维护，用户无须关心。

激光里程计会将当前位姿提交给激光地图，由后者搜索附近关键帧，如果能锁定关键帧，则更新当前位姿。

2.8 质量检查

如果帧间配准分数低于 ScoreThres，则立即重启点云。如果里程计历史分数均值低于 GoodThres，则保留高质量点云后重启（部分重启）。当看到里程计重启日志时（odometry输出日志中有"restart"关键词），可降低GoodThres应对。

三、激光地图

参考以下链接：

使用手册 - 激光SLAM＋反光板建图手册 - MDCS wiki

激光地图编辑指南 - MDCS wiki

MDCS参数表:Detour - MDCS wiki [2.3]

激光地图在SLAM被称为“后端模块”。激光地图以较慢频率（相对于前端模块）将点云帧与关键帧配准，在配准分数及格的集合中取分数最高的锁定，然后以这个关键帧来修正里程计的位姿。

3.1 运行流程

1. 取任务：从任务队列取配准任务。
2. 有反配准：如果机器人在“匹配困难区域”，则执行有反SLAM。
3. 关键帧配准：在给定位姿附近搜索可锁定关键帧。

3.2 取任务

激光里程计通过队列向激光地图提交回环任务。

3.3 有反配准

当机器人在“匹配困难区域”（涂黄区域），或者Detour模式切到“更新匹配困难区域”时，使用反光棒/板进行配准。

执行有反配准时，在Detour地图界面上，相关关键帧旁会出现青色文字，表示配准分数，格式为："R{分数}"或"Rr{分数}"。

图 3-1 锁定关键帧

当关键帧配准分数及格，但是机器人位姿偏差过大时，日志输出："反光板配准信度{分数}，但位置偏差过大(ls=xxx vs xxx)，xy:{位置误差}/{最大位置误差},dth:{角度偏差}/{最大角度偏差}，过滤"。

当关键帧配准分数及格，但是机器人位姿落在区域外，日志输出："反光板配准配准信度{分数}，但位置处于无效区域（见标记的浅蓝位置），xy:{位置误差},dth:{角度误差}，过滤"。

图 3-2 关键帧不能锁定

无论有反或无反，地图对位姿误差的判断方法是一样的：

• 最大位置误差：baseErrorXY + lstep * step_eror_xy
• 最大角度误差：baseErrorTh + lstep * step_error_th

以位置为例，baseErrorXY是基础范围（误差圆半径），误差随着lstep的增加而扩大，增量是step_error_xy。对于SLAM不良路段，会出现连续几个关键帧锁不上的情况，遇到配准分数及格的关键帧时误差过大而被过滤。此时可增加step_error_xy来应对。

激光里程计的角分辨率为0.1度，角度精度比位置更高，一般不用调整Th参数。

当锁定关键帧时，日志输出："R({地图文件名}) {xxx}:{xxx}(l={lstep}), from {x坐标},{y坐标},{角度} to:{x坐标},{y坐标},{角度}, delta={位置误差}/{角度误差}({最大位置误差}/{最大角度误差}), s={配准分数}"。

提示：日志中的位置坐标，可以在Detour地图界面找到，方法是将鼠标指针放到地图上，状态栏显示的“鼠标位置”就是鼠标在地图的坐标。用这个方法可以在地图上找到日志输出的位置，结合工作环境推断雷达当时观测到的环境轮廓。

关键帧的搜索半径由 frame_distant 参数决定：

图 3-3 frame_distant

该参数默认值是 5000(mm)，表示以当前位姿为圆心，搜索半径为5米。对于关键帧比较近，且环境特征非常明显的场景，比如位于墙角的立库出口，相邻出库口距离很近，可能会锁错关键帧（如锁到旁边出入口的关键帧），可降低 frame_distant 应对。

提示：理解关键帧配准分数非常重要。点云帧与关键帧点云的配准分数越高，该关键帧被选用的可能越大。上例所举的误锁例子，由相邻关键帧的点云相近引起，缩小关键帧搜索半径，能够提高定位精度。

3.4 关键帧配准

非“匹配困难区域”使用无反SLAM，将点云帧与附近关键帧配准。如果配准分数低于全局定位阈值（GrepThres + lstep * GrepThresK）时，执行全局重定位。Detour地图界面上会显示被搜索关键帧的配准分数，锁定关键帧显示为“实心红圈”，旁边是配准分数，格式是"PC{分数}"（PC表示点云）。全局重定位时锁定的关键帧，分数前缀是"G"。

无反SLAM的输出日志与有反一样，不再复述。

提示：什么时候用全局重定位？如果机器人所在位置的环境高度稳定，且特征明显时，可以用全局重定位。一般情况下应该用“指定位置重定位”（Detour界面对应“手动重定位”），找回定位概率要高于全局重定位。Detour会自动记录最新位置，如果AGV掉电维修时移动到其他位置，那么启动时定位会丢失（Detour启动时读入上次开机时的最新位置），如果维修点是固定的，建议用指定位置重定位；如果维修点特征明显，也可用全局重定位。

四、3D激光雷达

参考链接：

安装Detour - MDCS wiki [4.3] [5.1.1]

MDCS参数表:Detour - MDCS wiki [2.2.2]

3D激光雷达（PlanarRotary3DLidar、PlanarMid360）从3D点云中提取“垂直结构”特征，并将其二项化为2D点云。

墙、大树干、电控箱等都是“垂直结构”，它们是稳定的特征点，形成二向化点云。

3D雷达在3D点云中提取“垂直结构”的流程如下：

1. 水平切块：以aziRes为分割单位，将3D点云切分为 360 / aziRes 个块（切蛋糕）。
2. 垂直切片：以altRes为分割单位，对分块的3D点云进行垂直切分。
3. 分块寻找垂直结构：遍历水平切块，寻找其中的“垂直结构”。想象用一个圆筒在点云块中移动，如果圆筒中存在若干点，满足：垂直方向上至少有 minScans 个点，且高度大于 minVStruct，且这些点之间的“空洞”高度不大于 maxVHole。那么这些点构成一个“垂直结构”。
4. 垂直结构拟合为向量：将垂直结构的点云拟合为向量，该向量为一条直线，与Z平面形成夹角。
5. 过滤特征：分块中可能存在若干个垂直结构，将代表它们的向量夹角为显著性（越垂直于XY平面的向量越显著），并根据向量到 ZFadeout（距离雷达水平面的高度）的距离，计算出向量权重并排序。然后取 pivotPos（默认值0.9）位置的向量，将其权重（显著性）乘以 pivotFac（默认值0.05），得到权重阈值。凡显著性大于权重阈值的向量均是特征点，随后将其投影到XY平面成为二向化特征点。
6. 合并二向化特征点：将相邻距离小于 merge_dist 的二向化特征点合并。

3D雷达输入3D点云，输出二向化点云，后者包含了丰富的3D空间结构特征，相比于2D雷达，能够显著提高SLAM鲁棒性。

提示：增加 merge_dist 可“打薄”点云轮廓线，对于近距离成像性能较差的雷达，可通过这个参数减低近距离点云“跳动”现象。

对于存在“分层”垂直结构的场景，比如1.6米以下是机台，2.5米以上是管道，其间是不稳定的结构，可用 selZ 来选择不同层的垂直结构。selZ是一个数组，本例可设为 selZ=[1500, 2600]。把selZ的每个下标想象为"大刀"，刀在指定高度水平挥出，被砍中的“垂直结构”就是特征点，否则过滤。

室外存在“空旷”场景，比如厂房外的车道，两侧是厂房外墙，刚栽种的行道树（树干直径不超过10厘米）；又如停车场，两边是简单隔墙。前者是“长走廊”场景，两侧外墙形成平行线，只有行道树可供里程计使用；后者是“平库”场景，缺少垂直结构。

对于“长走廊”场景，树冠是里程计的唯一依据，设 minVStruct=20、minMustVStruct=0、filterStdE=50，将树叶作为垂直结构，可提供足够数量的特征点。由于树冠随季节有显著变化，这个方法不能用于生产，应耦合轮里程计应对。

对于“平库”场景，设 allowSinglePoint=true，启用“单点”功能，结合selZ使用。“单点”功能在selZ给出的高度附近寻找点，如有则将其转为特征向量，向量垂直于XY平面，显著性比垂直结构远低。利用这个功能，可以把“马路牙子”作为特征，selZ选择高度50（马路牙子高度一般在100~120mm）。由于室外地面起伏较大，雷达会扫描到地面，在扫描马路牙子同时还会形成一片地面噪音，可将地面标记为“移动物品区域”（涂红），或者修改 pivotPos & pivotFac，建议值 pivotPos=0.5、pivotFac=0.5，过滤地面噪音。

五、耦合轮里程计

参考链接：

使用手册 - 同时使用激光、地纹、二维码、轮编里程计和IMU进行鲁棒定位 - MDCS wiki

Detour-API - MDCS wiki [2.4.1]

对于雷达安装在低位的AGV，比如潜伏举升、地牛等，实践证明仅靠激光雷达是较难满足搬运要求的。低位雷达容易受到移动物干扰，在人车混杂的工作场景下很容易被干扰。最佳实践是激光雷达加轮里程计，后者根据IMU（提供可靠的角度观测）和驱动轮编码器计算机器人位姿，在激光不良的路段提供另一种定位手段，与激光形成互补。

Detour的“紧耦合”功能可融合多个异构里程计，输出置信度最高的里程计位姿。里程计类型包括（但不限于）激光、地纹、轮里程计、GNSS（北斗）。

紧耦合对里程计置信度的评估分为两个方面：

• 自拟合质量：根据里程计的历史数据（位姿），评估其输出位姿质量（内部数据一致性）。
• 耦合一致性：根据预测速度（机器人运动加速度），评估里程计位姿与预测位姿的误差。

紧耦合输出以下指标：

• 紧耦合方差（TCVar）：反应多里程计位姿的一致性程度。
• 耦合分数：包括耦合因子，表示与系统状态的一致性（耦合一致性）；自拟合质量，表示里程计内部一致性（自拟合质量）。

使用紧耦合状态的以下属性分析耦合质量：

• fitexplain（最佳里程计）："{里程计名称}:{历史位姿总数}L/{耦合分数}"。紧耦合正常工作时，被耦合的里程计轮流被选中，耦合分数大于1。
• discarding（丢弃里程计）：置信度不合格的里程计被丢弃，出现在这个属性。
• CouplingScore（耦合分数）：包括耦合因子和自拟合质量，取值范围[0, 1]，可用100分来理解：大于0.85表现良好，低于0.35极差，中间的分别是一般、合格、不合格。
• err（定位误差圆）：位置估计的标准差（mm）。

注意：使用 Detour + DetourLite组合时，Detour不会显示DetourLite的紧耦合状态，要用 "/getstat" 接口来获得统计对象，然后看其中的TC（紧耦合）状态。

通过“外部定位源”输出里程计位姿时，要注意发布频率。发布频率越高，里程计轨迹越平滑，则自拟合质量越高。高频发布的里程计会“美化”其性能，导致紧耦合一直选用该里程计，失去了鲁棒性。比如，10Hz扫描频率的激光雷达，耦合低成本IMU的轮编里程计，后者以50Hz发布，紧耦合大概率只用轮里程计，应把轮里程计的发布频率降到10Hz。

提示：激光雷达 + 轮里程计组合，激光雷达 time_bias_ms 经验值是 -20（默认值），可使激光雷达与轮里程计更好耦合。