tniySlam
文章介绍了Mines ParisTech.CoreSLAM开发的一种名为SLAF的机器人系统新SLAM算法。该算法是作为一部分开发的CoreSLAM以其代码的微小尺寸小于200行C语言代码而命名。它是如此之小，以至于我们决定在会议论文中发布其源代码，因此它不仅是开源的，而且是已发布的源代码！
鼓励我们这样做的是我们在实验中获得的良好性能。在简短回顾现有的SLAM算法之后，我们将介绍促使我们创建CoreSLAM算法的动机。第三节将专门介绍算法。然后，我们将介绍用于测试的机器人平台，并讨论获得的结果。
一些研究集中于这些算法在性能和计算速度上的比较6，表明该问题仍未在所有情况下得到解决。
在基于使用粒子滤波器的激光SLAM算法中，DP-SLAM是最著名的算法之一2。 DP-SLAM的工作原理是通过粒子过滤器在机器人的姿势和贴图上保持关节分布。该算法将地图与每个粒子相关联，并着重于在粒子之间共享部分地图的问题，以最大程度地减少内存（以及通过地图复制产生的时间）。 DP-SLAM的问题在于，将其集成到像我们这样的基于粒子过滤器的现有定位系统中非常复杂，这与3中所述非常相似。
在我们的审查中，我们已经看到，与我们的算法相比，大多数算法都是在慢速机器人上进行测试的-它们的速度几乎不会超过1 m / s-和/或正在使用非常昂贵的激光器（通常为SICK或IBEO激光扫描仪，射程为100米）。我们机器人的速度达到了3 m / s，再加上北洋URG04的短距离射程，为定位和制图任务带来了主要但有趣的问题。
此外，到目前为止，即使不是全部，大多数（即使不是全部）SLAM算法也使用需要很多代码行或复杂数学的方法，因此需要花费大量精力来理解和验证其操作-通常依赖于影响结果的许多不同参数。我们的工作始于一个重要的目标：制作一个简单的算法，易于理解，并且仍提供良好的性能-并且最重要的是，易于集成到现有的粒子过滤算法框架中。我们增加了最后一个要求，即我们的算法必须最终嵌入，因此它必须最小化内存消耗并在关键循环代码中使用整数计算。
算法
与DP-SLAM相反，我们决定仅使用一张地图。因此，DP-SLAM相对于CoreSLAM的优势在于，理论上的能力不会在漫长的走廊中消失，这确实是逐粒子地图概念的目标。不是没有外部过程就无法在DP-SLAM中实现的循环关闭。事实上，我们认为这种优势不值得复杂化-尤其是因为我们可以依靠平台上良好的里程表，并且鉴于我们的目标是关闭相当小的环路（探索实验室而不是走廊… ）。
由于CoreSLAM的想法是将激光信息集成到基于粒子滤波器的定位子系统中，因此我们必须编写两个主要功能：
- 扫描到地图距离函数，该函数用作检测滤波器中每个状态位置假设（粒子）的ikelihood函数。
ts_distance_scan_to_map函数的源代码在下面提供（算法2）。请注意，它仅包含扫描的所有撞击点（相对于粒子位置）的所有贴图值的简单总和。这是一个非常快速的过程，仅使用整数运算，这使我们在实时运行时不受粒子数量的限制。但是，这种估计可能性的方法意味着该地图是专门构建的。 - 地图更新功能，用于在机器人前进时生成地图，下面将详细讨论。
机器人的前进，将在下面进行详细讨论。构建与粒子滤波器兼容的地图并非易事：似然函数的峰值对于滤波器的效率非常重要，因此应易于控制。我们通过使用灰度图来实现这一点，灰度图更新包括挖一些孔，这些孔的宽度直接对应于我们的似然函数峰值。对于检测到的每个障碍物，该算法不会绘制单个点，而是会绘制一个函数，该函数的下点位于障碍物的位置（有关新创建的地图的缩放，请参见图1）。结果，构造的地图看起来不像传统地图，而是看起来像带有吸引孔的表面。通过AB过滤器（算法4的第74行）将其集成到地图中，从而使地图收敛到较新的配置文件。映射更新在粒子过滤器的最后更新位置（为似然性评估之后所有粒子的加权平均值）被调用，可以选择使用延迟（请参阅下面的讨论）。
ts_map_update函数的源代码在下面提供（算法3）。它使用函数ts_map_laser_ray（算法4），这是最棘手的部分。后者使用Bresenham算法在地图上绘制激光，并在内部使用另一种增强的Bresenham算法来计算正确的轮廓。它仅使用整数计算，而关键部分不使用整数除法。即使每步只执行一次（在我们的示例中为10 Hz），也必须确保此过程快速，因为由于地图的高分辨率（每像素1厘米），地图框架的很大一部分被激光扫描触摸。
CoreSLAM可以轻松集成到粒子过滤器中，但也可以不使用。确实，构造的地图。具有引人注目的孔和坡度，可用于任何梯度下降算法。匹配算法更容易收敛到障碍物，因为孔函数起着指导作用。我们的独立版本使用非常简单的Monte-Carlo算法将当前扫描与地图匹配，并检索机器人的更新位置。
确实，开发了独立版本是为了调整CoreSLAM的参数，即扫描到地图的集成速度（ts_map_update函数中的参数质量，请参见2），其设置为50，即“孔周围的宽度”。
地图中的影响点（设置为固定值在我们的实现中为600毫米）和地图的比例尺（在我们的示例中为1点= 1厘米）。在此版本中，可以忽略里程表，或者将其用作蒙特卡洛搜索的起点（在图5和6中，里程表被忽略，因此可以相互比较里程表和激光定位）
基于粒子滤波的版本对于管理歧义情况是必需的，因此对于重新定位也是必需的（当我们从完整地图而不是空白地图开始时）。
它也是里程表集成的最佳选择，因为我们可以通过将非线性误差模型集成到滤波器中来应对非系统性误差（如滑移）（通过将滑移概率设置为10％，我们可以得出10％的粒子使其停留在最初具有高模型噪声的位置，并且90％的粒子在具有低模型噪声的里程计中得以发展）。
粒子滤波器也是整合里程计和激光以外的其他传感器的很好框架。我们还集成了GPS（供室外使用）和指南针（目前无法获得很好的效果，在我们的案例中，传感器对电磁噪声非常敏感）。
我们将通过讨论两个有关本地化准确性和将数据集成到地图中的延迟的问题来完成CoreSLAM的演示。
**亚像素精度：**即使我们的地图分辨率为1cm，也可以测量小于1cm的位移，这是因为我们的ts_distance_scan_to_map函数考虑了多个点来进行计算。甚至可以测量1毫米的距离，因为某些激光点将落在地图的另一点。
**延迟：**延迟的概念-从激光扫描到其整合到地图之间的时间，对于测量相对于地图分辨率的小位移是必要的。例如，为了正确地测量以1cm / s的速度移动的物体的位移，如果地图分辨率为1 cm，频率为1Hz，则需要等待10次测量，然后等待时间应为10。
延迟理论公式为：
Latency
关于我们的机器人并考虑到我们的地图分辨率，公式为：延迟= Robotsperd。我们的机器人以相当高的速度（2 m / s）运行。它的速度很少会下降到0.01 m / s，即使在这种速度下，等于1的等待时间也足以进行计算。此外，前面提到的亚像素精度使任务更加容易。
因此，我们决定删除CoreSLAM的延迟管理代码，但回想一下，有必要将CoreSLAM应用于缓慢移动的对象。0.01m/s，如果不讨论循环闭合，就无法结束对算法的描述。我们的算法不涵盖闭环，但可以与任何闭环算法集成。实际上，我们目前正在开发一个tinyLoopCloser算法，该算法应该很好地补充我们的CoreSLAM。
平台说明
我们的实验平台是MinesRover（见图2），这是Mines ParisTech和SAGEM DS共同开发的自制机器人。该流动站有四个驱动轮和方向盘以及两个测距轮（见图3）。机器人的机械结构基于跷跷板的关节运动，产生了很好的里程信息，因为两个中心自由行走轮始终（至少在理论上）始终与地面保持接触。这种机械装置无需减震器，可防止打滑问题。
在图片上我们可以看到摄像机的球机，转向伺服电机，HOKUYO URG-04激光测距仪，GPS接收器（灰色正方形）和机器人前面的应急超声传感器。
该机器人由4.1 Ah的4元素锂电池（14.8V）供电。四个45W CC电动机可实现3 m / s的最高速度。
GPS接收器的定位精度可以达到1米（使用一颗EGNOS卫星）。 Hokuyo URG04激光传感器提供10Hz的水平扫描，范围为5.6米。为了检索机器人的方向，我们将指南针和GPS的结果结合在一起。超声波传感器提供了发现未被激光检测到的任何障碍物的机会（例如楼梯台阶），并且通常用作紧急停止装置。该机器人还嵌入了5轴IMU 可以提供横摆率信息和机器人的倾斜度（尚未在我们的实验中使用）。
机器人的电子装置以Qwerk模块为中心，该模块由charmedlabs.com设计。它是一个基于ARM 9微处理器和Xilinx FPGA逻辑的电路板。
它是一个6轮机器人，带有4个转向和驱动轮，以及2个配备2000点编码器的自由旋转轮。 Qwerk模块位于机器人的中心。它的200Mhz微处理器能够可靠地管理所有传感器和执行器。
Qwerk上安装了Linux操作系统，并为GPS和Hokuyo提供USB总线支持。 FPGA逻辑负责IC总线支持，里程表输入和伺服电机控制。使用的激光传感器是通过USB连接的Hokuyo URG-04LX。
虽然它是价格的理想传感器，但它具有以下缺点：
- 它的最大范围限制为5、6米，考虑到我们的机器人-3米/秒的速度，严重限制了它的速度。在我们杂乱无章的环境中，传感器的许多措施导致了0米（表示反射不好或没有反射）或模棱两可的措施（例如移动椅子的下部或墙壁旁边的地面箱子-箱子的前部或可以检测到后面的墙）
- 测量频率（10Hz）也受到限制。例如以3m / s的速度撞到墙壁上，墙壁似乎是倾斜的，因为在将激光旋转240°之后，测量的最后一点比第二点要靠近
240/360 * 3/10 = 20厘米。测量（尽管立即处于相同的距离）。在我们的机器人中，必须考虑到这一点，因此我们使用恒定的（每次扫描）纵向和旋转速度校正了每次扫描。
结论
实验是在巴黎科技大学的电子实验室中进行的。这种环境对算法来说确实具有挑战性：这是一个杂乱无章的环境，盒子和计算机躺在地上，许多桌子和架子（见图9），使用激光扫描仪很难检测和追踪。
左侧是嵌入式软件。在右侧，这是在台式机上运行的软件部分。矿用漫游者与操作员之间的通信是通过台式PC上的HMI完成的。粒子过滤算法正在台式计算机上运行，以减轻ARM9对机器人的负担。我们使用无线连接，即机器人内部的MIMO外部，可提供良好的连接质量。
图5、6和7显示了具有相同数据样本的结果。在前两个图上，我们看到了里程表和仅通过激光进行的运动估计之间的比较，并表明它们非常相似。图7中的地图是通过结合激光和测距信息而构建的，并且显示出良好的重建精度，回路几乎是闭合的。有趣的是，在本实验中观察到的滑移（见图8）已通过激光信息很好地校正了。请注意，我们实验中的机器人速度达到了2.5米/秒，并且偏航率达到150％/秒（通过测距法测量并通过激光测量确认），机器人的极高角速度受到其4个方向盘的青睐。
图5.实验过程中，仅使用测距法估算的机器人速度以及另一侧（不考虑测距法）的我们的aser CoreSLAM的速度如图7所示。请注意，各度量之间的良好匹配。我们可以观察到激光测量的较小延迟（大约1帧），考虑到传感器的性质，这是可以预见的。这表明我们的里程表非常好（并且已经过完美校准），并且激光CoreSLAM能够应付机器人的高速运动（到达在此实验中为2.5 m / s）。
灰色显示的是由机器人构建的“孔”图。以红色表示，我们覆盖了激光所进行的所有扫描。蓝色，是机器人的重建轨迹。循环闭合在这里几乎是完美的。该地图是通过结合里程表和激光（蒙特卡罗搜索）获得的信息而获得的。
请注意驾驶员撞到实验室右墙时的打滑情况。
