基于激光雷达的安保机器人自主定位与导航算法研究

■ 文/湖南万为智能机器人技术有限公司 刘琴 明振

1 引言

近年来，人工智能和机器人技术的不断提高，传统的人力警务正朝着无人值守方向发展。一些机场、银行、商场、酒店、企事业单位、展馆、仓库、码头港口以及社区都已经实现了少人或无人值班，将传统人力管理转变为机器人管理，在很大程度上减轻了人力，降低了人工成本，并且可以实现24小时连续作业，更加保证其安全性与稳定性。

目前，基于这种人工智能的机器人技术已经在市场上不断涌现，变电站、仓库、无人车都进行了相关研究或应用。文献[1]研究了激光定位的变电站智能巡检机器人导航系统，从硬件和软件上分别进行了研究，但是其专门针对室内型定位系统。文献[2]在电力物资仓库中采用了激光导航系统，该导航系统实现物资盘点、安保巡检，并分析物资储备情况，匹配应急物资，辅助物资配送工作，从有轨工作实现了无轨工作。文献[3]对激光雷达SLAM技术在无人车中的应用进行了相关研究，并对比分析了各种激光SLAM技术的优缺点，对该技术在无人车中的技术进行了总结和展望。

由此可见，基于这种SLAM自动导航技术在市场上有了一定的应用，但是在警用机器人上应用尚缺乏。因此，本文着重解决警用机器人的自主定位与导航算法研究和应用效果。

2 技术现状

现今警用机器人的发展现状表明，自动导航技术是警用机器人进一步智能化的技术难点，也是决定警用机器人广泛使用的突破点。

目前，大部分智能巡检机器人的导航技术主要包括：磁导航、惯性导航、卫星导航、视觉导航和传感器数据导航。文献[4]利用磁铁的吸附性，将磁钉作为固定标示物，埋在机器人设计的行进路径上形成导航线。该方法需设置预定路线，并不能做到自动导航。文献[5]采用了惯性器件进行导航，主要用于机器人工作在非目标区域、定位精度要求不高的场合。文献[6]针对磁导航方式的不足，将高精度差分GPS定位应用于巡检机器人导航。但GPS易受干扰，短时间内信号波动大，导航定位精度误差大，在室内也有盲区，有一定的缺陷性。文献[7]采用了摄像头，对图像进行了预处理，采用一种改进了的哈夫变换检测场景中的路径，并根据路径跟踪模块进行导航计算。由于视觉依赖于光照的影响，所以基于这种视觉的机器人导航具有时间限制。

以上几种导航方案对警用机器人有一定制约。一般情况下，警用机器人有以下特点：工作环境恶劣，需要在复杂的环境中实现自主行走和巡逻，并配合警务人员协同行动；工作时间长，需24小时不间断工作，需要较高的定位精度和稳定性；工作场景多（包括室内和室外），主要针对公共场合的安全。因此，针对警用机器人自动定位的高精度性，高稳定性和广泛适用性，本文采用了基于Time of Flight（TOF，飞行时间）结构的激光雷达设备，保证数据的可靠性和稳定性，从一定程度上保证了后续自动导航算法的准确性。

3 自主定位与导航技术基本原理

同时定位与建图（SLAM）是实现机器人在未知环境中自主行走和巡逻的关键技术。该技术主要解决机器人在未知环境中对自身所处位置的定位，并在运行过程中对周围环境建一个相对于大地参考坐标系的地图，最后根据所建地图成功避开障碍物，保证自己的正常行走和自主巡逻。

3.1 激光雷达测距扫描传感器基本原理

目前，激光雷达有三角测距激光雷达，相位法测距激光雷达和TOF（Time of Flight）测距激光雷达。本文采用的是基于SICK公司生产的一款基于室外区域安防的激光扫描器LMS111-10100。该激光采用了成熟的TOF原理，非接触式检测，可以在很大的噪声背景下获取真实信号，且在较强的太阳光下直射工作，并不被相互干扰。其中，TOF测试原理如图1所示。

图1 脉冲TOF测距仪框图

通过图1可知，该TOF激光测距是通过给目标连续发送光脉冲，然后利用激光接收器接收从目标物体返回的光，计算探测光脉冲的飞行（往返）时间来得到目标物的距离。其数学表达式如式（1）所示：

公式（1）中，C是光速，真空光速定义值为299792458m/s，是计算发射激光的时刻和激光反射回来的时刻差，是计算得到的距离。

3.2 定位和建图算法研究

Gmapping算法是目前在移动机器人领域用途最广的定位与建图算法，该算法主要采用Rao-Blackwellized粒子滤波器。Rao-Blackwellized粒子滤波器的核心思想是根据观测值和码盘里程计测量信息去估计联合后验概率密度函数，即计算得到机器人的估计轨迹值和环境地图。因此，这个联合后验概率可以用公式（2）来表示，这个方法可以分解为：

Step1：采样。粒子滤波根据状态转移函数预测生产大量的粒子，利用这些粒子的加权来逼近后验概率密度。

Step2：权重计算。随着观测值的依次到达，为每个粒子计算相应的权值。

Step3：重采样。根据权重的比例重新分布采样粒子。

Step4：地图估计。对每个采样的粒子，根据其采样的轨迹与观测计算相应的地图估计。

Gmapping算法在Rao-Blackwellized算法的基础上，不仅考虑了前一时刻机器人的里程计数据，还增加了前一时刻的观测值，其后验概率修改为。在实现过程中，主要采用自适应重采样技术来减少粒子退化的影响，其方法中主要包括扫描匹配和Grid FastSLAM算法，构建的地图格式为二维占据栅格地图。每个栅格不是由简单的二进制0或1表示，而是采用概率的形式。一般情况下，栅格空free的概率表示为P（s=1），有障碍物的概率表示为P（s=0），两者的概率和为1。为了简化这种表达，采用两个概率的比值表示某个点是否为占据状态或空闲状态，如下式所示：

上式（3）中，若空的概率比占据概率大，那么空占率则越高。

3.3路径规划

3.3.1 Lifelong Planning A＊算法

Lifelong Planning A＊（LPA＊）在典型的Dijkstra算法的基础上，提出了一种启发式搜索算法，引入了当前节点的估价函数，如下式所示：

上式（4）中，f（n）为估价函数，表示从起始节点到节点n的实际代价；g（n）为耗散函数，是起点到终点的最短路径值；h（n）为启发函数，是节点n到目标节点的估计代价。

其主要实现步骤如下：

1）将起点加入开表。

2）遍历开表，找到f值最小节点作为当前节点。将其添加到闭表，并从开表中移除。

3）对该节点的8个领域各自进行判断。如果相邻格子不在开表中，将其加入及开表中，并将父节点设置为节点和计算f，g和h值。若相邻格子已经存在于开表中，并且新的g值比旧的g值小，则把相邻格子的父节点设置为该节点，并且重新计算f值。

4）重复以上2）、3）步骤，直到终点加入开表中，表示找到路径，否则表示没有路径。

因此，采用LPA＊算法实现了机器人的全局路径规划。

3.3.2 D＊Lite算法

D＊Lite算法是在LPA＊的基础上发展而来的，主要解决环境变化时的局部路径规划。D＊Lite算法与LPA＊算法不同的是，D＊Lite是从目标节点向起始节点搜索，其全局路径规划图如下所示：

图2 全局路径规划示意图

在图2中，目标搜索首先从目标点开始搜索，以周围8领域为搜索弧，以此类推，直到找到一条全局最短路径，如图中灰色线条所示。在实际场景中，很多情况下机器人在某个地方突然遇到一个障碍物，导致全局路径不能通过，因此需要进行局部路径规划，以避开突发障碍物，进而达到目的地。因此，D＊Lite算法给出了两种解决方案。一种方案是根据新的起点位置，重新计算优先队列中所有节点的启发函数值。另一种方案是不重新计算启发函数值，而是在计算新添加到优先队列中的节点的启发函数值时，加上一个修饰符km。其中，km表示机器人移动距离的叠加。所以，D＊Lite算法采用了第一种方案，在LPA＊的基础上引入了动态搜索，节点的启发式函数值随起点位置的变化而变化的。因此，只有受障碍物影响的栅格需要重新进行处理。图3则为警用机器人在全局路径下突遇障碍物的局部路径规划示意图。图3与图2对比，图2中的全局路径被障碍物遮挡，经局部规划出一条路径后成功避开障碍物到达目标点。

图3 局部路径规划示意图

4 导航试验

本文使用的试验平台为警用机器人，搭载激光雷达传感器。

图4 激光雷达传感器

图5 警用机器人

警用机器人采用Gmapping算法进行定位，并且得到场景的栅格地图；利用D＊Lite算法进行全局和局部路径规划，实现动态实时避障，保证机器人的正常和安全行走。整个软件平台采用ubuntu14.04，ROS/indigo操作系统。经实际巡逻任务测试，对于2176＊2336分辨率的栅格地图，能保证2.5cm的导航精度，能满足警用机器人的正常定位和执行巡逻任务。经3600小时的测试，该导航算法性能良好，已经在实际中应用良好。图6是警用机器人巡逻路径规划示例图，地图是一个0和1表示的概率栅格地图，在图中以白色或黑色显示，越白则表示无障碍的概率性大，越黑则表示有障碍的概率大，红色的线是遇到突然障碍物的最短路径规划线。

图 6 警用机器人巡逻路径规划图

5 结语

本文在对比了各类导航技术的基础上，应用了一种基于TOF结构的激光雷达定位与导航算法。警用机器人采用稳定的Gmapping定位与建图技术，输出场景的栅格地图，利用D＊Lite路径规划算法得到一条最优路径，在遇到突遇障碍物也能进行局部路径规划，满足了警用机器人自主巡逻的需求，对移动警务起了很大的作用。经3600小时实地测试，该自主定位与导航算法性能良好，并已经得到广泛应用。

[1] 肖鹏，孙大庆，王明瑞，郭锐，孙勇.基于激光定位的变电站智能巡检机器人导航系统设计[J].计算机测量与控制，2012,20（6）.

[2] 王铁铮，任博翰，张昊，胡亚楠，潘.激光定位导航系统在电力物资仓库的应用[J].农网智能化，2016,18（5）.

[3]李晨曦，张军，靳欣宇，李广敬，李强.激光雷达 SLAM 技术及其在无人车中的应用研究进展[J].北京联合大学学报，2017,110（4）.

[4] 韩金华，王立权，孟庆鑫.护士助手机器人磁导航方法研究[J].仪器仪表学报，2009,30（4）.

[5] 管林波，董利达，尹俊.局域精确定位的工业移动机器人惯性导航方案[J].计算机应用，2014，34（4）.

[6]肖鹏，张彩友，冯华，栾贻青.变电站巡检机器人GPS 导航研究[J].传感器与微系统，2010,29（8）.

[7]张海波，原魁，周庆瑞.基于路径识别的移动机器人视觉导航[J].中国图象图形学报，2004,9（7）.

[8]S Kurtti ，JP Jansson ，J Kostamovaara. A Laser Scanner Chip Set for Accurate Perception Systems[J]. Advanced Microsystems for Automotive Application , 2012 ∶313-322.

[9] 辛冠希.基于RGB-D摄像机的同步定位与建图研究[D].哈尔滨工业大学，2016.

[10] Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard. Improving Grid-based SLAM with Rao-Blackwellized Particle Filters by Adaptive Proposals and Selective Resampling[C], In Proc.of the IEEE International Conference on Robotics and Automation (ICRA), 2005.

[11] 常皓，杨巍.基于全向移动模型的 Gmapping 算法[J].计量与测试技术，2016,43（10）.

[12] 王同喜，孙淑霞.基于 A ＊ 和 Bresenham 相结合的网络游戏寻路算法设计与实现[J].成都理工大学学报(自然科学版)，2007,34（4）.

[13] Koenig S, Likhachev M.D＊lite[C]// Eighteenth National Conference on Artificial Intelligence and Fourteenth Conference on Innovative Applications of Artificial Intelligence, 2002∶476-483.

[14] AL-MUTIB K,ALSULAIMAN M,EMADUDDIN M.D＊Lite based real-time multi-agent path planning in dynamic environments [C]. Computational Intelligence , Modelling and Simulation(CIMSiM)，2011 Third International Conference on. IEEE，2011：170-174.