使用手册 - 定位导航

1. 什么是SLAM

SLAM（Simultaneous Localization and Mapping）是指同时定位与地图构建，它是机器人在未知环境中自主导航的关键技术。SLAM 架构主要包括前端和后端两部分。

1. 前端：前端主要负责实时估计机器人的位姿（位置和姿态），即确定机器人在当前时刻的具体位置和方向。常用的前端里程计方法有：

• VO（Visual Odometry）：视觉里程计，通过计算相邻图像之间的特征点匹配来估计位姿的变化。
• Lidar（激光雷达）：通过测量激光脉冲与环境中物体的反射距离来计算位姿的变化。
• IMU（Inertial Measurement Unit）：惯性测量单元，通过内置的陀螺仪和加速度计等传感器测量机器人的加速度和角速度，结合初始位置信息来估计位姿的变化。
• wheel-odometry：轮式里程计，通过计算机器人轮子的转速和转过的角度来估计位姿的变化。

1. 后端：后端主要负责将前端得到的位姿信息进行整合、优化和存储，同时构建地图。后端回环检测是 SLAM 中的一个关键步骤，用于检查当前估计的位姿是否与先前已经估计过的位姿相一致。如果检测到回环（即两个不同的位置实际上是同一个位置），则说明当前地图存在错误，需要进行纠正。后端优化的目标是使得地图中的点能够准确地反映出机器人实际经过的路径，提高 SLAM 的精度和鲁棒性。

总之，SLAM 架构的前端里程计负责实时估计位姿，后端回环检测和优化负责整合位姿信息、构建地图并纠正错误。前端和后端可以独立设计，前端提供优化约束，后端实现优化求解。

我们用一个框架图来说明基于优化的SLAM原理。在建图时，传感器（如激光雷达）数据输入前端，前端据此进行局部建图（当前可见区域），通过闭环检测（从不同角度观察到同一个路标），后端进行全局建图（移动机器人的历史位置），并使用全局优化来纠正局部建图的误差（累积误差）。

图 1-1 基于优化的SLAM经典框架

Detour使用激光雷达作为定位手段，传感器数据即激光雷达的点云。

图 1-2 激光雷达的点云

激光雷达每扫描一圈则输出一个测量对象。如果激光雷达的角分辨率是0.167度，扫描角度范围是275度，扫描一圈则得到1647个测量数据，每个数据表示雷达在此角度扫描到的物体距离。测量对象由测量数据构成，称之为点云。

图1-2中青色线就是点云，在Detour中放大激光地图，可见这些线由点组成，如图1-3，红色点即点云，是激光雷达在选中关键帧（双红色圆圈处）位置得到的测量对象。

图 1-3 放大后的点云

1.1 建图问题与定位问题

由于工厂环境相对稳定，我们把SLAM拆分为建图问题和定位问题以简化论述。即先建立激光地图（建图问题），然后根据激光地图定位导航（定位问题）。

假设我们已建图，则图1-1的框架可表述为：前端里程计和后端回环检测。

1.2 前端里程计

移动机器人的里程计是用于测量机器人在运动过程中所经过的距离或位置的装置。它类似于轿车仪表中的里程表，可以记录机器人移动的总距离或者行驶的路径。 里程计通常使用编码器、激光测距仪、红外传感器、摄像头等传感器来测量机器人的移动距离或位置。编码器可以检测机器人的轮子旋转次数，从而计算出机器人移动的距离。激光测距仪和红外传感器可以通过测量距离来确定机器人的位置。摄像头可以通过图像处理技术来识别标记点或者特征点，从而计算出机器人的位置和移动距离。 和轿车仪表类比，里程计可以被视为移动机器人的“仪表盘”，它可以提供机器人的位置、速度、方向等信息，帮助机器人掌握自己的运动状态，避免迷路或者重复经过同一个位置。里程计还可以用于计算机器人的行程时间，以便更好地规划路线和任务。

Detour使用激光里程计，通过计算相邻点云的位移增量（距离和角度的变化量）来推算移动机器人下一个位姿，从而得到移动机器人的航迹，该算法称为“扫描到扫描匹配”，如图1-4。

图 1-4 扫描到扫描匹配

由于机器人相邻两个位姿对应的雷达扫描轮廓存在较大的关联性，在预测位姿附近范围内将当前帧雷达数据与前一帧雷达数据进行匹配，以匹配位姿为机器人位姿的更新量。

备注：得益于Detour的算法，雷达帧间匹配精度非常高，激光雷达的角度匹配准确。如果单线激光雷达未调平，或地面起伏，使点云相对于实际有一定畸变而带入误差，形成激光里程计的累积误差。如使用多线激光雷达，通过算法可进一步抵消单线激光雷达的装配误差以及地面起伏问题。

激光里程计的累积误差由后端回环检测修正。

1.3 后端回环检测

闭环检测是在程序后台持续运行的，传感器每输入一帧雷达数据，都要对其进行闭环检测。当闭环检测中匹配得分超过设定阈值就判定闭环，此时将闭环约束加入整个建图约束中，并对全局位姿约束进行一次全局优化，这样就能得出全局建图结果。

由于SLAM是持续建图和优化，因此回环检测不仅用在建图问题，也用在定位问题。

图 1-5 关键帧锁定

在图1-5中，雷达帧数据与关键帧（图中红色实心点）的配准置信度是0.35（关键帧右侧青色数字），大于置信度0.3阈值，则移动机器人以该关键帧坐标来修正里程计累积误差。

当雷达帧数据与关键帧不匹配时（置信度小于0.3），则回环检测失败，移动机器人继续以激光里程计定位，其累积误差导致移动机器人逐渐偏离规划路线。

图 1-6 回环检测失败

图1-6中，雷达帧数据与关键帧匹配置信度低于0.3，因此回环检测失败（匹配失败关键帧右侧有青色置信度数字）。

备注：图1-6匹配失败现象，应为右侧的输送线移门直线造成，该线段与左侧矮墙轮廓相似，因此点云配置分数较低。将输送线区域标记为“移动物体区域”后，关键帧锁定失败现象有显著好转（在第二排库位靠近左侧矮墙区域）。

2. 里程计

2.1 激光里程计

激光里程计是一种使用激光技术来测量距离的设备。其工作原理是利用激光的高强度和单色性，通过测量激光从发射器到目标反射回来的时间差来计算出距离。 具体来说，激光里程计通常由一个激光发射器和一个接收器组成。激光发射器会发出一束高强度的激光，这束激光经过一定距离后到达目标，然后被目标反射回来。接收器接收到反射回来的激光后，通过计算接收到反射光信号的时间与激光发射器发射光信号的时间之间的差值，来计算出距离。

激光里程计具有以下几个优点：

1. 测量精度高：角度测量精度高，平移精度低。
2. 不受环境影响：激光里程计不受地面、底盘轮系等因素的影响，可以在各种环境下进行测量。
3. 抗干扰能力强：激光里程计的抗干扰能力较强，不易受到电磁波等干扰。

但是，激光里程计也存在一些缺点：

1. 激光雷达装配：单线激光雷达需要正确安装，多线激光雷达可抵消安装误差及地面起伏（地面相对于底盘的倾斜小于1度）。
2. 环境干扰：容易受到环境变化和人员/车辆走动的干扰，算法能够抵消一部分干扰情况，无法处理（激光雷达视野）全遮盖情况。

Detour建图时，相邻关键帧距离一般在2米以内，激光里程计在回环检测配合下能够很好地执行定位导航任务。如果环境轮廓改变（也包括雷达视野受限）、雷达附近有快速移动物体时，回环检测失败，从经验来看，激光里程计在2米后可观察到累积误差，当回环检测持续失效时，累积误差使移动机器人偏离其规划路线，随后超出关键帧搜索范围而导致定位丢失。

2.2 轮里程计

轮里程计（Odometry）是一种通过计算轮子滚动圈数来估计移动距离的方法。轮里程计主要依赖于轮子的转速和圈数来计算移动距离，常用于自主移动机器人（如无人车、无人机等）的定位与建图。 轮里程计的工作原理是：当轮子在运动过程中，通过对轮子的转速进行计数，可以计算出轮子在一定时间内转动的圈数。由于轮子的周长已知，因此可以通过计算圈数来估算出移动的距离。同时，可以通过对轮子在运动过程中的角度信息进行积分，得到移动的距离。

轮里程计具有以下优点和缺点：

优点：

1. 结构简单：轮里程计不需要额外的传感器，只需要轮子和转速计数器，结构简单，易于实现。
2. 实时性好：轮里程计可以根据实时的转速和圈数信息计算出移动距离，具有较好的实时性。

缺点：

1. 误差累积：轮里程计的误差主要来源于轮子转速计数器的精度和轮子在运动过程中的滑移。这些误差会随着运动过程的持续而累积，导致最终的定位误差越来越大。
2. 对轮子参数敏感：轮里程计的精度受到轮子半径、周长等参数的影响，如果这些参数发生变化，可能导致定位误差。由于驱动轮有一定厚度，因此其对地面无运动的“接触面”较难判定，轮子越宽越难判定，因此叉车的轮里程计并不可靠（驱动轮厚，原地旋转也会造成位移）。

当轮里程计结合 6 轴 IMU（Inertial Measurement Unit，惯性测量单元）时，可以有效提高其精度。6 轴 IMU 包含了三轴角速度传感器和三轴加速度传感器，可以实时测量机器人在三维空间中的运动状态。通过将 IMU 的数据与轮里程计的数据进行融合，可以消除部分误差，提高定位精度。

融合后的轮里程计可以作为 SLAM（Simultaneous Localization and Mapping，同时定位与建图）的前端里程计使用。在 SLAM 过程中，机器人需要同时进行自身定位和环境建图。轮里程计可以为 SLAM 系统提供相对定位信息，而 IMU 数据可以用于消除轮里程计的误差，从而提高 SLAM 系统的整体性能。在实际应用中，通常还会结合其他传感器（如激光雷达、视觉传感器等）的数据，以进一步提高 SLAM 系统的精度和鲁棒性。

2.3 地纹里程计

地纹里程计（Ground Texture Odometry）是一种通过视觉传感器（相机）收集的地面纹理数据来估计移动距离的方法。地纹里程计的工作原理是：当机器人在地面上移动时，相机会收集地面纹理信息。通过对这些纹理信息进行分析，可以估计机器人在地面上的移动距离。通常，地纹里程计需要对地面进行预先建图，以便在运动过程中识别和匹配地面纹理。

地纹里程计具有以下优点和缺点：

优点：

1. 平移精度较高：地纹里程计的精度主要取决于传感器的分辨率和地面纹理的独特性。在理想情况下，地纹里程计可以达到较高的平移精度。
2. 结构简单：地纹里程计不需要额外的传感器，只要地面没有重复性纹理，且相机在补光后能看清地面细节，地纹里程计绝不会失效。结构简单，易于实现。
3. 实时性高：Detour的地纹算法使用GPU加速，实时性远高于激光里程计，典型时延在5ms以内。

缺点：

1. 角度精度较低：受限于相机视野，行走2米就会产生不小的角度偏差，5米后基本不可用。
2. 受地面纹理影响较大：如花纹钢这种有重复性纹理的地面，会导致地纹SLAM回环检测失效。
3. 实时性较差：地纹里程计需要对地面纹理进行实时分析和匹配，计算量较大，可能导致实时性较差。

地纹里程计可以作为 SLAM（Simultaneous Localization and Mapping，同时定位与建图）的前端里程计使用。在 SLAM 过程中，机器人需要同时进行自身定位和环境建图。地纹里程计可以为 SLAM 系统提供相对定位信息，但由于其实时性和精度的限制，通常需要与其他传感器（如激光雷达、视觉传感器等）的数据进行融合，以提高 SLAM 系统的整体性能。

3. 最佳实践

对于具有稳定环境轮廓，人车分离的场景，激光SLAM是性价比最优方案。对于人车混合作业环境，应使用轮里程计或地纹里程计作为前端里程计，避免激光雷达被环境干扰时，后端回环检测短时失效，激光里程计累积误差导致丢定位的问题。

3.1 轮里程计和地纹里程计的选择

地纹里程计相当于VSLAM（视觉SLAM），定位精度正负1mm，要求相机支持60fps以上视频流，且视野大于150mmW * 150mmL。由于定位速率高，因此能兼容污损地面，具有很强的鲁棒性。由于角分辨率较低，需要与激光SLAM搭配使用。是理想的前端里程计。

地纹里程计需要地面纹理，比如硬化混凝土、金刚砂混凝土等地面。环氧地面没有纹理，但回环不能保证。应根据作业环境特点选择合适的里程计。

轮里程计需要IMU，后者则需要标定，对底盘装配一致性有精度要求，车队间偏差通过外参标定修正，标定工作量较地纹里程计大。

3.2 雷达视野考量

大面积的平面库（超过3列）、库位附近有动态环境（卷帘门、拉门等），应考虑雷达视野。对于无（有腿）托盘干涉的车型，可将激光雷达安装在车体角上，以获得250度以上视野。对于有托盘干涉的车型，应考虑前后双导航雷达，以获得更好视野，以降低雷达视野受限所带来的风险。

3.3 用于定位的环境轮廓考量

无论上不上轮里程计或地纹里程计，勘查作业环境时应注意以下事项：

1. 设轮里程计精度为5米（Y轴）偏差10mm，则每3米应有良好的定位轮廓，极少数情况可以有小于5m间隔的定位轮廓。
2. 使用地纹里程计时，应每10米有良好的定位轮廓，以防止底盘或地面原因（打滑），脱轨时找回定位（地纹有视野范围约束）。
3. 良好的定位轮廓，是指点云有“线”和“角点”的特征。点云中的线由若干点组成，在Detour上表现为一条线段。点云中的角点（Corner Point）是指在点云数据中具有特殊位置和特征的点。角点通常用于描述点云中的局部特征，比如边缘、角落和转折点等。多个角度不同的线是最好的特征，角点其次。在环境勘查和定位质量分析中，应关注“线”和“角点”，判断能否构成可用的定位轮廓。
4. 对于环境轮廓不良的区域，可用反光棒应对（Detour支持无反 & 有反）。反光棒部署区域不应有强反光特征的物体存在，比如1米内的不锈钢物体、0.2米内的烤漆钣金物体。

4. 参数说明

4.1 参数一览表

激光地图参数

表4-1 激光地图参数一览

参数名	含义
scoreThres	匹配分数阈值。得分高于该阈值的匹配为有效配准。计算方式见4.2。
RScoreThres	反光板匹配分数阈值。与scoreThres类似。
GregThres	触发全局匹配的阈值。得分低于该阈值的匹配，会触发一次全局配准。

4.2 参数公式与详解

scoreThres

$scoreThres=\max({\frac {psum}{preobservedLen+1}},{\frac {sumw}{observedLen+1}})$

其中：${\textstyle preobservedLen}$表示前一帧点云数量。$observedLen$表示当前帧点云数量。当前帧中每个点$p_{i}$到前一帧点云中的最近的一个点$q_{j}$的距离为$d_{i}$，计算得到的权重${\textstyle w_{i}=ComputeWeight(d_{i})}$是一个介于0、1之间的数值，点之间距离越小，$w_{i}$越大。$sumw$和$psum$分别指当前帧和前一帧的所有点云对应的权重之和。