基于RSSI的仿生SLAM算法研究

张志根， 凌有铸,2， 陈孟元,2*

(1.安徽工程大学 电气工程学院， 安徽 芜湖 241000；2.高端装备先进感知与智能控制教育部重点实验室， 安徽 芜湖 241000)

移动机器人在未知环境中实现定位与构图一直是这一领域的难题，目前关于SLAM(Simultaneous Localization And Mapping，同步定位与建图)的算法基于概率方向的算法研究相对成熟[1-2]，仿生SLAM的研究相对较少且不成熟。但是随着人们对动物大脑内部导航细胞研究取得的进展，在仿生算法方向也相应地取得了不错的研究成果[3]。

随着奥基夫等科学家对动物大脑中与导航有关的头细胞，位置细胞等导航细胞的不断研究，人们认为大多数哺乳动物是依靠这些导航细胞之间的信息转换实现定位与导航的[4-5]。澳大利亚科学家迈克尔·约翰·米尔福德[6-10]通过模仿动物导航细胞之间的关系建立模型，提出了一种新的基于RatSLAM的解决思路，先将路径积分和视觉信息关联起来，再将环境空间、视觉信息、时间、行为变化等结合起来建立经历。但是依靠纯视觉的RatSLAM模型，对复杂环境的适应能力比较差，文献[11]由此提出利用双轴速度传感器和MIMU构建一个新的里程计，并将它运用到RatSLAM模型中来，相对于原来的RatSLAM模型，对环境的适应能力更强了，导航精度变高了。文献[12]提出将加速度计和陀螺仪的姿态测量模块融入到原有的RatSLAM模型中，有效减少了定位误差。文献[13]通过在原有的RatSLAM模型中局部视觉场景模块引入动态阈值来改进，通过不同时刻、环境的复杂度来增强和减小阈值，使得在模板匹配上准确率更高，以此来对经历进行修正。也有很多研究学者利用不同定位融合的方式来提高定位精度，在文献[14]中将蓝牙和WiFi结合的方式改善定位精度，比单一传感器定位要好，但是由于这两种算法单独定位的误差较大，导致结合后定位误差也比较大。文献[15]提出将FAP-MAP和RatSLAM结合起来，此方法对于原有RatSLAM算法来说，提高了系统的鲁棒性。

本文在原有RatSLAM算法的基础上引入RSSI(Received Signal Strength Indication，无线接收信号强度)定位，通过无线信号的粗定位确定一个精准区域，再通过RSSI粗定位点和精准区域分别对位姿感知细胞进行修正和对视觉场景模板进行筛选，相对于上述文献中的算法，本文提出的算法定位更加精准，构图更加准确。

图1 RSSI定位结构框图

1 算法原理与设计

1.1 基于RSSI的粗定位

在线信号定位算法、信号类室内定位算法通常都分为两个部分：离线阶段和在线定位阶段。离线阶段就是采集待定位场所的信号值强度制作成指纹库，在线定位阶段就是分析实时接收的信号强度值与指纹库的信号强度值进行关联，进而估计当前所在的位置。图1是RSSI定位结构框图。

1.1.1 建立离线指纹库

离线阶段即是在实验场地等距离的位置采集信号值建立成指纹库的过程，图2是指纹采集模型。

为了建立精准的指纹库，在信号值采样的过程中，针对每个采样点对信号值持续采样，采样间隔为1 s。采用高斯滤波对采样数据进行处理来降低噪声干扰，本文采用概率在90%内的数据，公式为

(1)

(2)

rssii∈(μ-1.65σ,μ+1.65σ)，

(3)

图2 指纹采集模型

式中μ是一个参考点多次测量的数据集的均值，σ是一个参考点多次测量数据集的标准差，n是测量次数，Rssii是测量的信号强度值，rssii是有效的信号强度值。经过高斯滤波处理后的有效信号强度值取平均值作为本参考点的信号强度值，其计算公式为

(4)

式中h是有效的信号强度值的个数。

1.1.2 加权法进行定位

当移动机器人在移动的过程中，移动机器人会实时接受到信号，由下式可以得到移动机器人与指纹库中各参考点的距离：

(5)

(6)

式中(xi,yi)为参考点的位置坐标，Wi是参考点i的贡献度，其计算公式为

(7)

式中S是所有参考点求出的欧式距离之和，最终通过RSSI的粗定位，得到了一个粗定位点和一个精准的区域，这个精准区域称为R，由所有满足下式的点(xe，ye)组成：

(xe-x)2+(ye-y)2≤α2。

(8)

1.2 改进后的RatSLAM算法

视觉场景模板、位姿感知细胞、经历图是RatSLAM算法中的主要部分，位置细胞和头方向细胞组成位姿感知细胞，每一个位姿细胞代表着移动机器人的位置信息；局部场景细胞代表着移动机器人周围的场景信息；经历地图由许多个位姿细胞和局部场景细胞结合构成。当移动机器人运动时，通过视觉里程计提供的移动机器人的角速度和线速度进行路劲积分得到移动机器人的实时位姿信息，同时通过RSSI粗定位对位姿细胞进行修正。视觉传感器所获取的外部场景的信息给予局部场景细胞，同时精准区域对局部场景模板进行筛选。然后将局部场景、位姿感知细胞和路径积分后的移动机器人的运动信息处理后形成经历地图。改进的RatSLAM的结构框图如图3所示。

图3 改进后的RatSLAM结构图

1.2.1 空间位姿模型

RatSLAM采用(x′，y′，θ′)的坐标形式来说明移动机器人的位姿，其中(x′，y′)代表移动机器人运动的二维平面范围，θ′代表移动机器人的方向，无论移动机器人运动到哪里，移动机器人的位姿都可以当作一个位姿细胞。但与传统的SLAM方法又有着不同之处，RatSLAM采用三维坐标对移动机器人的位姿进行排列，当移动机器人再次回到曾经经过的地方时，会复现先前的位姿，这种三维坐标系排列的机器人位姿在复现时会呈现连续性，将各种经过排列的机器人位姿细胞群称之为位姿感知细胞网络。

RatSLAM通过竞争吸引子网络的形式来对位姿细胞进行控制，在位姿细胞的总活性不变的同时，时刻更新不同位姿细胞的活性，用活性最高的位姿细胞来表征可能的位姿。

1.2.1.1 兴奋度更新

一个三维离散高斯分布被用来创建兴奋度权重矩阵ε，每个位姿感知细胞用来描述对位姿感知细胞矩阵中其他细胞的映射行为。εabc是位姿感知细胞平面(x′，y′)中对应兴奋性的一个二维高斯分布和θ′中对应兴奋性的一维的乘积，计算公式为

εabc=e-(a2+b2)/kx′y′e-c2/kθ′，

(9)

式中kx′y′和kθ′分别为(x′，y′)平面和θ′的方差常数；a、b、c分别是x′、y′、θ′的分布系数。由于兴奋性使位姿感知细胞活动产生变化，变化公式为

(10)

式中Nx′、Ny′、Nθ′是(x′，y′，θ′)空间中的位姿感知细胞三维矩阵元素。

1.2.1.2 全局抑制

引入全局抑制变量，对位姿细胞的活性进行抑制，防止权值被无限增加。全局抑制是一个迭代的过程，经过全局抑制后的细胞活性水平为

(11)

1.2.1.3 归一化

为了保持网络上的细胞总活性不变，采用归一化的方式来实现，归一化后，位姿细胞的活性水平为

(12)

1.2.2 局部场景模板的形成

局部场景单元是用来放置机器人所走过路径过程的每个场景，在机器人运行过程中，视觉传感器不断地捕捉所有经历的场景信息，当机器人初次来到这一场景时，创建当前场景的局部场景单元。

在局部场景细胞构建过程中，对于传感器采集到的图像信息需要先通过降低图像的分辨率和灰度化进行预处理，之后对图像信息所有列的强度值求和，做归一化处理，通过这种方式将图像信息转化为更好处理的一维的行相量。采用绝对误差和方法计算当前视觉模板与之前存储的视觉模板的强度差：

(13)

式中Ix表示当前模板第x列的强度值，Ix,j表示已存储的在此时刻精准区域内的位姿细胞有关联的局部场景模板j第x列的强度值，l表示模板图像的长度。匹配度b为

(14)

当视觉里程计提供的移动机器人的实时位置(xt，yt)在精准区域内，此时设置M=1；若是不在精准区域内，则用RSSI定位替代视觉里程计提供的位置，此时设M=0。

设定阈值ξ0与匹配度b进行比较。此时设定一个模板更新值Z，当Z=1时将当前视觉模板库更新；当Z=0时，认定此模板为熟悉模板，此时不对视觉模板库更新，Z值的计算公式为

(15)

1.2.3 局部场景单元与位姿细胞的关联

RatSLAM中局部场景单元与位姿细胞有着很大的关联，这种关联能够使局部场景细胞注入能量到相对的位姿细胞中，局部场景细胞和位姿细胞的连接可以表示为

(16)

式中Vt是局部场景细胞的能量值，Pxyθ为位姿细胞的能量水平，λ为常数。

当移动机器人发生运动，来到一个陌生环境时，此时移动机器人创建一个新的局部场景单元并放到模板库中，还需要同步创建与之关联的位姿细胞，这就是连接地图的构建功能；当移动机器人运动到熟悉的场景时，同步激活与之连接的位姿细胞，当有位姿细胞被激活时，该位姿细胞的活性变化为

(17)

式中δ是一个常数，由于一个位姿细胞可能关联多个局部场景单元，nac为局部场景细胞的个数。

1.2.4 经验图的构建与校正

经验图是一个拓扑结构的地图，由具有拓扑关系的经历构成，在移动机器人运动过程中，通过移动机器人的运动信息和外部场景信息构成经历，此时的运动信息和外部场景是具有一定关联的，当移动机器人来到曾经来过的地方时，位姿细胞和局部场景细胞可以用已存在的代替，此时过去的经历被激活。

每一个经历都在经历地图中具有自己的坐标空间位置(x′,y′,θ′)。运行时生成的第一个经历设为三维空间中的原点(0,0,0)，之后根据当前时刻的位置和路径积分得到下一个时刻移动机器人的经历，可以用一个三维向量来描述：

ei={Pi,Vi,pi}，

(18)

式中pi为经历ei在经历图中的位置，经历地图的构建是不断完善的过程，当前视觉场景所对应的位姿细胞状态Pi和局部场景细胞状态Vi与之前的经历ei中的不匹配，对经历地图进行修正创建一个新的经历，比较公式为

S=μp|Pi-P|+μv|Vi-V|，

(19)

式中μp和μv是位姿细胞和局部场景细胞的匹配权值，当S小于匹配阈值证明之前经历过这个地方，大于阈值则产生一个新的经历ej。

当移动机器人回到熟悉的地方时，移动机器人会重定位，这一过程可能导致经历图发生跳跃，从当前时刻产生的经历跳转到之前认知的经历，这种跳跃可以通过每一个经历的有效位姿和当前移动机器人的位姿来实现。这一过程称为经历地图的矫正，这个过程将对经历地图中的所有经历进行位置更新：

(20)

式中α为矫正速率常数，反映从经历ei到其他经历的连接数目。

2 实验及分析

为了验证本文算法的可行性，采用如图4所示的实验平台进行实验。PC机的处理器为i5-7500，主频为3.40 GHz，内存为4 GB，系统为Windows10，移动机器人为旅行家二号。实验环境为10 m×8 m的会议室，布局如图5所示。

图4 实验平台 图5 实验环境

2.1 经历节点模板匹配图

通过让图4所示的移动机器人平台在会议室围绕实验桌移动3圈，在移动机器人移动的最后两圈增加行人，使实验环境由静态环境变为动态环境。通过实验得到局部场景模板和经历匹配图，如图6所示，其中图(a)是原有RatSLAM模型实验得出的局部场景模板和经历匹配图，图(b)是结合RSSI定位后得出的局部场景模板和经历匹配图。当移动机器人开始运动第一圈时模板数量上升很快，表示新的模板快速增长的过程，(a)、(b)两图的曲线差异几乎没有。当移动机器人开始跑后两圈时，移动机器人对实验环境有了认识，模板和认知点的数量增长曲线会平缓很多。对比(a)、(b)两图还可以得出，融合RSSI定位的RatSLAM算法产生的模板和认知点数目比没有融合的少很多，剔除了无效模板和错误的认知点，增加了该算法的时效性。

(a) RatSLAM (b) RSSI+RatSLAM图6 模板经历匹配图

2.2 经历图对比

图7是在实验过程中产生的移动机器人的经历图，图(a)是没有融合前的经历图，图(b)是融合后的经历图，对比改进前后算法的经历图。可以看出融合后的算法经历图要更加准确，由于视觉场景模板受到精准区域的筛选和粗定位对位姿细胞的修正，使得改进后的算法定位精度更高，鲁棒性更强，经历描述更加准确。

(a) RatSLAM (b) RSSI+RatSLAM图7 经历图

2.3 定位误差比较

为了更加直观地比较融合前后的定位误差，随机选取50个测试点，采用均方根误差的方法对移动机器人的定位精度进行误差分析，融合前后的误差数据如表1所示。从表中可以看出融合后的算法定位精度更高。

表1 算法定位精度表

3 结 论

本文系统研究了基于无线信号强度的移动机器人仿生SLAM问题，先是建立离线指纹库，利用了RSSI进行粗定位，在此基础上确立精准区域，然后将此算法融合于RatSLAM算法中，利用粗定位点对位姿细胞进行修正，用精准区域对视觉场景模板进行筛选。实验结果表明，改进后的移动机器人的定位精度得到了提高，经历图更加准确，但是由于外界干扰源太多，导致误差还是很大，所以如何更好地减少噪声干扰是之后研究的一个重点。