搜救环境中的仿生蛇形机器人SLAM 方法

王超杰，苏 中，连晓峰，赵 旭

（1.北京信息科技大学 高动态导航技术北京市重点实验室，北京100101；2.北京工商大学 计算机与信息工程学院，北京100048；3.北京理工大学 自动化学院，北京100084）

0 引 言

与传统的移动机器人相比，蛇形机器人具有较高的运动稳定性和较强的环境适应能力，不仅可以在地震、矿灾等环境中进行灾害搜救工作，也可以应用于空间探索等领域［1，2］。

SLAM 问题在智能移动机器人领域的重要性受到了广泛的关注，出现了许多较为成熟的理论和实现方法，如EKFSLAM、FastSLAM、DP－SLAM 等［3－6］。SLAM 算法的计算和实现需要的运算量较大，难以满足实时性需求。在此情况下，本文提出一种MiniSLAM 算法，其运算量少、实时性高，可应用于蛇形机器人复杂地形环境的地图构建需要。

1 SLAM 问题描述

SLAM 最早由Smith等提出：在未知环境中移动机器人从未知的位置出发，利用传感器探测的环境信息建立环境地图并同时对自身位置定位的过程。在SLAM 问题中，机器人的位置、环境地图以及传感器数据都具有不同程度的不确定性，概率机器人学采用概率模型可精确的描述这种不确定性［7］。为了降低问题的复杂度，假定SLAM 问题服从一阶Markov过程，因此可以采用动态贝叶斯网络来描述SLAM 问题中的状态转移过程，如图1所示。

图1 SLAM 的状态转移过程

其中，环境地图M 中包含一系列的环境特征M ＝｛m1，m2…mN｝。xt＝ （x，y，θ）表示t时刻机器人的位姿，x 和y 为二维笛卡尔中机器人的位置，θ为机器人的方向角。xt＝ ｛x1，x2…xt｝为初始时刻到t时刻机器人的运动轨迹，ut＝ ｛u1，u2…ut｝和Zt＝ ｛z1，z2…zt｝表示t时刻之前的控制输入量和观测量，ut代表t－1时刻到t时刻的控制输入量，zt表示t时刻的观测量，nt＝ ｛n1，n2…nt｝为观测量对应的特征标识。

在一阶Markov过程假设下，根据条件独立性，可得

SLAM 问题的运动模型可表示为

SLAM 问题的观测模型可表示为

其中：εt——t时刻的运动噪声，服从N（0，Qt），δt为t时刻观测噪声，服从N（0，Rt）。SLAM 问题的目标在于从带噪声的控制量ut和观测量Zt中估计出环境地图M 和机器人实时位姿xt。

2 MiniSLAM 算法

根据蛇形机器人环境搜索的需要，本文提出一种MiniSLAM 算法来提高地图建立的实时性，其主要由两部分组成：位姿更新和地图更新。其中，采用Monte－Carlo算法实现当前激光扫描与地图的匹配来进行位姿更新，采用改进的Bresenham 算法进行地图更新。

2.1 Monte－Carlo算法

Monte－Carlo算法是一种基于贝叶斯滤波理论，利用粒子近似机器人在地图上位置的算法，初始假定均匀的分布于环境空间中，随着机器人移动，粒子也做相应移动，机器人每次完成环境感知后，对这些粒子重新采样。算法结束时，粒子会收敛于机器人的实际位置。Monte－Carlo算法包含以下几个阶段：

预测阶段：利用运动模型以概率密度函数的形式来预测当前机器人的位姿。假设当前的状态xk仅依赖于之前的状态xk－1和已知的控制输入uk－1，该运动模型被认定为条件密度p（xk｜xk－1，uk－1），对于一阶马尔可夫过程，先验概率密度可通过积分得到

更新阶段：根据测量模型集成传感器信息来获得后验概率密度p（xk｜Zk）。假定对于xk测量值zk与之前的测量值Zk－1是条件独立的。测量模型以似然函数p（zk｜xk）的形式给出，这种形式表示在观测值zk情况下，机器人位于xk的可能性。通过贝叶斯公式更新先验概率值，得到后验概率密度

2.2 Bresenham 地图更新改进算法

通过Monte－Carlo算法可以得到每个时刻机器人所在地图上的位置Ps，通过坐标变换可获得激光探测的障碍物坐标Pi。为实时得到完整的环境地图，提出一种基于Bresenham 算法的环境地图更新改进算法［8－11］。

图2为Bresenham地图更新算法流程，其中Ps为激光位置坐标，Pi为激光一次240°扫描范围内某一障碍物坐标，map为地图的起始位置，m＿size为地图的尺寸大小，swap（）是交换函数，需要在地图中更新Ps与Pi直线间像素点的值。将所有的激光扫描点集通过Bresenham 地图更新算法更新到地图中，获得最终的环境地图。

3 实验平台

3.1 蛇形机器人简介

本实验所采用的仿生蛇形机器人实物图和结构图分别如图3和图4所示，该机器人具有如下几个特点：①采用3D 打印而成，既缩短了加工周期又节约了成本；②通过ADAMS软件仿真，进行了机械结构设计，具有6 个正交关节和1个分体机构，腿部具有变形机构，可以进行站立、卧倒、直行、蜿蜒、蠕动、分体、翻滚等步态；③机器人采用12V，1500mA 的电池供电，确保机器人能够连续运动半小时以上；④头部安装有激光测距仪，可进行环境探测。

图2 Bresenham 地图更新算法流程

图3 仿生蛇形机器人实物

图4 仿生蛇形机器人结构

3.2 激光测距仪

实验所用激光测距仪型号为URG－04LX，其具有尺寸小、重量轻 （160g）、功耗低等优点。其性能参数见表1。

3.3 通信模块

实验过程中，通信模块采用的是串口转WIFI模块USR－WIFI232－T，激光测距仪通过串口与其连接，这样激光采集的数据就可通过网口传输到PC 端，PC 端通过对数据进行处理进行实时的地图绘制。图5 为激光与串口转WIFI模块的连接。

表1 URG－04LX激光测距仪性能参数

图5 URG－04LX与USR－WIFI232－T连接

4 实验与结果分析

实验以仿生蛇形机器人为载体，搭载URG－04LX 激光测距仪和USR－WIFI232－T串口转WIFI模块，在不同的环境下验证算法的性能。

4.1 实验1

首先在相对规整的实验室环境下验证算法性能，激光测距仪安装在蛇形机器人头部，距地面约15cm，实验中蛇形机器人按图6蓝线轨迹以大约2m／s的速度探测实验室，箭头指示机器人的运动起点。

图6 实验室环境

实验室SLAM 如图7所示。

图7 实验室SLAM

SLAM 图的左边部分对应于实验室环境的正前方部分，实验所创建的地图与实验室的真实环境完全一致，图中虚线显示的是蛇形机器人的运动轨迹，与真实运动轨迹相一致，而且轨迹几乎是闭合的，能够在开环的情况下实现闭环效果。

4.2 实验2

为了模拟震后受灾现场，在实验室中人工搭建搜救实验环境，如图8所示，地面上堆放着石头、木板、箱子等杂物来模拟废墟环境，实验中蛇形机器人按图中轨迹进行环境探测。图9为蛇形机器人在模拟搜救环境中SLAM 实验结果。

图8 人工搭建模拟搜救实验环境

通过对比模拟的搜救环境与实验所创建的地图可以看出，实验创建的地图能准确的反映出环境的特征，可以有效的完成搜救环境中地图创建的任务。

为了证明算法计算量少、实时性高，在实验硬件条件和地图分辨率相同的情况下，对MiniSLAM 算法和EKFSLAM 算法、FastSLAM 算法进行了比较。表2为3种算法的比较结果。

图9 模拟搜救环境SLAM 实验结果

由表2可知，MiniSLAM 算法处理一次扫描的时间小于激光测距仪的采样时间 （100ms），能够满足实时性的要求。MiniSLAM 算法相比于其它两种算法，处理一次激光扫描点集的时间最短，并且不会随处理扫描点集的数量变化而大幅度变化，能够满足搜救环境实时性建图的需求。

表2 不同算法处理一次扫描时间比较／ms

5 结束语

本文针对搜救环境中蛇形机器人SLAM 的需求，设计了一种简单有效的MiniSLAM 算法，该算法利用蛇形机器人运动过程中激光测距仪采集的数据进行地图构建。实验结果显示MiniSLAM 算法建图精度高、计算量少、实时性高，几乎能够在开环的情况下实现闭环效果，能够满足搜救环境中的实时建图需求。

下一阶段，将会改进算法，致力于实现闭环算法，从而能够实现自动地检测闭环并相应地纠正机器人的运动轨迹。同时也会考虑加入视觉传感器或里程计以提高创建地图的精度。

［1］WANG Nan，MA Shugen，LI Bin，et al.Simultaneous localization and mapping based on the hybrid metric－topological map in the ruins environment［J］.Robot，2013，35 （6）：762－768（in Chinese）.［王楠，马书根，李斌，等.基于拓扑米制混合地图的废墟环境同步定位与地图构建 ［J］.机器人，2013，35（6）：762－768.］

［2］WANG Nan，MA Shugen，LI Bin，et al.Simultaneous localization and mapping based on morphological characteristics of the seismic damage of the interior ruins ［J］.Chin Sci Bull（Chin Ver），2013，58 （S2）：104－111 （in Chinese）. ［王楠，马书根，李斌，等.面向废墟内部震害形态的同步定位与地图构建［J］.通报学报，2013，58 （S2）：104－111.］

［3］TU Gangyi，JIN Shijun，ZHU Xuefen，et al.Particle filter SLAM method for mobile robot［J］.Journal of Southeast University （Natural Science Edition），2010，40 （1）：117－122（in Chinese）.［涂刚毅，金世俊，祝雪芬，等.基于粒子滤波的移动机器人SLAM 算法 ［J］.东南大学学报 （自然科学版），2010，40 （1）：117－122.］

［4］ZHU Jihua，ZHENG Nanning，YUAN Zejian，et al.A SLAM approach by combining ICP algorithm and partial filter［J］.Acta Automatica Sinica，2009，35 （8）：1107－1113 （in Chinese）.［祝继华，郑南宁，袁泽剑，等.基于ICP 算法和粒子滤波的未知环境地图创建 ［J］.自动化学报，2009，35（8）：1107－1113.］

［5］ZHANG Jialong.Research on SLAM problem of indoor mobile robots based on particle filter ［D］.Harbin：Harbin Institute of Technology，2011 （in Chinese）. ［张家隆.基于粒子滤波的室内移动机器人SLAM 问题研究 ［D］.哈尔滨：哈尔滨工业大学，2011.］

［6］ZHAO Xuan，HE Bo，JI Dezhi，et al.Location and tracking of moving objects based on particle filter for mobile robot［J］.Journal of System Simulation，2008，20 （23）：6490－6497 （in Chinese）.［赵璇，何波，吉德志，等.基于粒子滤波的机器人定位及动态目标跟踪 ［J］.系统仿真学报，2008，20 （23）：6490－6497.］

［7］Steux Bruno，EL Hamzaoui Oussama.tinySLAM：A SLAM algorithm in less than 200lines C－Language program ［C］／／11th International Conference on Control，Automation，Robotics and Vision，2010：1975－1979.

［8］Soleimani B，Ashtiani M－H Z，Soleimani B H，et al.A disaster invariant feature for localization ［C］／／IEEE／RSJ International Conference on Intelligent Robots and Systems.Piscataway，USA：IEEE，2010：1096－1101.

［9］Sun R，Ma S，Li B，et al.A simultaneous localization and mapping algorithm in complex environments：SLASEM ［J］.Advanced Robotics，2011，25 （6／7）：941－962.

［10］LIU Shaogang，GUO Yunlong，JIA Heming.Rescue robot simultaneous localization and mapping based on extraction and matching of line features［J］.Journal of Jilin University （Engineering and Technology Edition），2013，43 （4）：1035－1044 （in Chinese）.［刘少刚，郭云龙，贾鹤鸣.基于直线特征提取匹配搜救机器人的同步定位与地图构建 ［J］.吉林大学学报 （工学版），2013，43 （4）：1035－1044.］

［11］GUO Shuai，MA Shugen，LI Bin，et al.Simultaneous localization and mapping through a Voronoi－diagram－based map representation ［J］.Acta Automatica Sinica，2011，37 （9）：1095－1104 （in Chinese）. ［郭帅，马书根，李斌，等.基于Voronoi地图标示方法的同步定位与地图创建 ［J］.自动化学报，2011，37 （9）：1095－1104.］