融合深度信息的RatSLAM仿生算法研究

卞程远， 凌有铸， 陈孟元

(安徽工程大学 电气工程学院， 安徽 芜湖 241000)

同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)最早由Smith、Self和Cheeseman于1988年提出。因为其具有重要的理论与应用价值，被公认为是实现移动机器人自主导航的关键。SLAM算法主要分为基于概率的算法[1-3]和基于仿生的算法[4]。

基于概率的算法主要分为3种：卡尔曼滤波(Kalman Filter，KF)算法、最大期望(Expectation Maximzation，EM)算法和粒子滤波(Particle Filter，PF)算法。概率算法的优势是能够很好地处理传感器与环境的模糊性，具有很好的SLAM性能并能够建立高准确度、高精度的笛卡尔地图，但是这些方法不能解决整个地图的构建和导航问题。

相比于基于概率的算法，基于仿生的算法在一定程度上显示出了它的优越性。自然界中的许多生物虽然没有高精度的传感器，但是仍具有强大的导航能力，并且能够解决整个SLAM问题，这是因为生物对其生物地图的构建依赖于鲁棒性处理策略而不是对世界的精确描述。在许多的仿生算法中，基于鼠类的定位与导航算法——RatSLAM仿生导航算法[5]，备受青睐。RatSLAM模型提出的一种基于复合位姿表征的啮齿动物海马区扩展模型，该模型中将路径积分和视觉关联过程集成到位姿细胞模型中，使机器人具有更新预测状态能力，通过直接根据机器人位姿的核心表征和存储在位姿感知细胞及视觉细胞网络中的环境表征对应关系，建立存储环境空间、视觉、时间、行为和变化等信息的经历地图[6-8]。张潇等[9]利用现有的传感器技术改进现有的RatSLAM模型，在现有的RatSLAM基础上加入光学双轴传感器和MIMU信息，建立了航位推算模型。Berkvens等[10]将磁定位的概念引入RatSLAM模型，提高了机器人定位的精确度，但由于金属、电气设备的干扰，提出的这种地磁RatSLAM并不具备较高的实用性。文献[11]提出将FAB-MAP(fast appearance-based mapping)引入RatSLAM的视觉里程计，这种基于历史模型的闭环检测算法可以过滤视觉里程计的误报信息，进一步提高RatSLAM系统的稳定性。

尽管上述的算法相比于原始的RatSLAM算法在性能上有了进一步的提升，但是相比于原始的RatSLAM仿生算法，不管是在软件设计还是硬件搭建上都复杂很多，且由于无法直接得到图像的深度信息，使得机器人在导航过程中导航准确度较低。因此，本文提出用Kinect传感器[11]代替传统的RGB摄像头来采集图像，深度传感器Kinect在获取彩色图像的同时获取图像深度信息，将深度信息融入传统的RatSLAM算法，以实现更加精准的导航。

1 算法原理与设计

1.1 RatSLAM模型

RatSLAM系统主要包含4大部分：位姿感知细胞、局部场景细胞、视觉里程计和经历图。该系统的核心是位姿感知细胞和经历图。机器人的位姿信息被编码在一个名为位姿感知细胞的单独的CAN模块(模拟方向细胞、位姿感知细胞的定位功能的数学模型[13-14])中，通过位姿感知细胞各自的处理模块，机器人中心信息和非中心信息刺激影响位姿细胞的活动。RGB-D视觉模板是通过对Kinect相机采集的RGB-D图像的特征提取得到的，其中包含了对图像深度信息的提取。里程计信息通过获取加速度计、陀螺仪的速度和角速度得到。在导航过程中，位姿感知细胞实时地获取里程计的信息用以更新位姿，RGB-D视觉模板的作用是对位姿感知细胞进行修正，即通过比较相机采集到的同一场景的两张图像的特征来实现闭环检测，进而实现对位姿感知细胞的姿态修正。图1是改进后的RatSLAM算法结构框图。

图1 改进后的RatSLAM算法结构框图

1.1.1 位姿表征

为了形成位姿感知细胞群，用于模型化头方向细胞和位置细胞的竞争吸引子网络结构被综合起来形成一个三维结构体。位姿细胞以(x′,y′,θ′)的样式排列，使得机器人能够同时应用x′、y′、θ′表征多个位姿估计。(x′,y′)表示网络平面坐标，θ′表示方向。

1.1.2 内部动态过程

连续吸引子网络CAN控制着位姿细胞的激活程度。其动态过程经历三个阶段：

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

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

(1)

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

(2)

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

(2)全局抑制。因为多个位姿假设(由多个活动包表示)的完成需要时间，并且会进一步被视觉输入加强，所以抑制是相对缓慢的，即相互竞争的活动包会在相当长的一段时间内同时存在。抑制后的活动水平为

(3)

这里的抑制常数φ控制全局抑制水平，兴奋度水平被限制成一个非负数。

(3)归一化。归一化是保持位姿细胞在获得视觉输入和路径积分输入后的总活性，归一化后位姿感知细胞的活动水平为

(4)

1.1.3 路径积分

路径积分就是通过转换每个位姿感知细胞基于里程计和(x′,y′,θ′)坐标活动来更新姿态细胞的活动。在路径积分后，被更新的位姿感知细胞活性为

(5)

(6)

这里kx′、ky′和kθ′是路径积分常数。v、w分别是传感器获得的速度和角速度。

1.2 深度图像的获取与处理

微软于2010年推出一款深度像机Kinect，能在获取空间图像的同时得到其深度信息。这款相机设计的最初是应用于Xbox 360游戏主机作为体感输入设备。因其能够获得场景图像的深度信息，被用作机器人视觉SLAM，取得了很好的效果。本文使用KinectV1(下文简称Kinect)作为深度视觉传感器。

1.3 RGB-D视觉模板创建

1.3.1 图像深度信息提取

图2 相机的针孔模型示意图

图像深度信息提取采用坐标标定技术[15]。Kinect相机采用了针孔模型，如图2所示。设投影中心O为相机的坐标原点，将平面z=f作为图像平面，相机中心O到图像平面的垂线作为摄像机主轴，主轴与图像平面的交点o称为主点。

如图2所示，视觉场景中的点C(x,y,z)在图像平面上的投影为c点，假设图像的像素宽度和像素高度分别为cx和cy，则fx=f/cx和fy=f/cy分别表示水平像素和垂直像素的焦距，由相似三角形原理得到图像平面上c点图像坐标为(fxx/z,fyy/z)，可以用式(7)齐次坐标的矩阵形式来表示空间点与图像投影点的关系：

(7)

式(7)中描述的主点即为图像坐标的原点的理想情况，但实际上由于受到相机生产时的精度影响，主点常常会偏离图像平面中心几个像素。设(μ0,ν0)为主点的图像坐标，式(7)可以改写为

(8)

场景中的物体空间坐标被定义在相机坐标上，但是实际实验中，相机一直在移动，因此场景中的点空间坐标需要定义在世界坐标系上。场景物体的空间坐标需要经过一个旋转(R)和平移(t)变换才能表示在相机坐标系中。此时，式(8)可表示为

(9)

式中，fx、fy、μ0、ν0为相机内部参数，R、t为相机外部参数，μ为比例系数，(xw,yw,zw)为世界坐标系坐标。上式表明，任意给定世界坐标系中的一点C(xw,yw,zw)就可以得到其在相机图像中的成像点坐标c(x,y)，即图像中获得了场景的深度信息。由于Kinect视角比较窄，图像的变形比较小，因此本文中忽略了透镜引发的图像畸变。

1.3.2RGB-D视觉模板创建

(10)

式中，hi为参考直方图，c为当前直方图，下标j为直方图中的二进制序号。

(11)

(12)

(13)

常数dm为一个距离门限值，大于该值的位姿感知细胞是不活跃的。

1.4 RGB-D视觉模板修正位姿感知细胞

机器人在行走的过程中对采集到的每一幅图像进行特征检测，以描述符的形式将检测到的图像特征表述出来，之后将每一幅图像的深度信息与描述符存储起来形成一个新的RGB-D视觉模板。

局部场景细胞是一个一维细胞，每个细胞对应一个特定的RGB-D视觉模板，每一个新的场景，算法会建立一个新的RGB-D视觉模板，与此同时，建立局部场景细胞与位姿感知细胞之间的连接。更新后的连接强度为

(14)

式中，Qi为局部场景细胞的活动水平，Px′y′θ′为位姿感知细胞的活动水平。

在建立局部场景细胞与位姿感知细胞间的连接后，局部场景细胞就会往位姿感知细胞中注入活性，用来保持或者校正它的位姿信息。在路径积分后，机器人获得其位姿信息，但由于路径积分时，在位置和方向上的累计误差，可能导致获取的位姿信息不准确，这时，RGB-D视觉模板就起到了修正作用。加入了深度信息后的视觉模板对位姿细胞进行修正，即

(15)

式中，ΔPx′y′θ′为位姿感知细胞的活动变化。

1.5 经历图的构建与修正

(16)

当前位姿感知细胞的活性决定经历能级Ei的各组成要素，即

(17)

利用当前位姿感知细胞的位姿信息、RGB-D视觉模板与经历图进行匹配，当到达经历过的地方时，进行一次闭环检测，并对经历图进行修正，经历位姿改变为

(18)

式中，α为修正常数，Nf为经历i到其他经历的连接数，Nt为从其他经历到经历i的连接数。

2 实验结果与分析

2.1 实验平台及场景

图3 机器人走过的真实路径

本实验基于实验室的轮式机器人。上位机采用戴尔燃7000系列笔记本电脑，Windows7操作系统，i5处理器，4 GB内存。试验场景选取在室内，大小为9 m×6 m。实验场景为图3中的室内环境。

2.2 仿真结果与分析

2.2.1 经历图的构建

图4 经历图

分别对传统的RatSLAM算法和改进后融合深度信息的RatSLAM算法使用轮式机器人在图3场景下试验多圈进行验证。图3标出了机器人走过的真实路线，图4为两种算法构建的经历图。

从经历图中可以看出，在第一圈经历图的构建上传统RatSLAM算法和改进后的RatSLAM算法差别不大，因为这两种算法都是依靠里程计信息构建的经历图，但是在第二圈的经历图的构建上，传统RatSLAM算法在闭环检测的作用下，实现了对经历图的修正，但是相对于改进后的RatSLAM算法，该算法中缺少图像深度信息，导致闭环过程中有的关键点没有检测到闭环，使得构建的经历图与真实路径相差很大，失真严重。改进后融合了深度信息的RatSLAM算法在经历图的构建上就体现了很大的优势：在第二圈上，由于融合了深度信息，其在闭环检测上体现出了更大的优势，更多的关键点被检测出来，用来对经历图的修正。但是由于机器人的方向不能发生突变，导致在整个经历图的构建过程中出现很多次的修正。从试验结果可以看出，本文提出的改进算法导航的准确度更高。

2.2.2 导航误差分析

机器人导航误差分析的是构建经历图时产生的误差。本文中采用均方根误差(Root Mean Square Error，RMSE)的方法对导航的精度进行误差分析，且每隔0.02 m做一次采样，分析如下：

(19)

其中，N表示采样的次数，ui表示第i次采样对应的经历图的位置，ui0表示第i次采样时的真实位置。为了验证改进后算法导航的准确率，本文在相同的条件下做了3组对比试验：传统的RatSLAM算法组、利用Kinect相机实现的RGBD-SLAM组、融合深度信息改进后的RatSLAM组。3组对比试验的结果如图5所示。

图5 3种算法误差对比

从试验结果图可以看出，改进后的RatSLAM算法误差较小，导航精度较高，该算法利用闭环检测对经历图进行纠正时效果最好，使得构建的经历图在纠正后回到了真实的位置，如图5中的c点，该点处误差为0，即表示该点处的经历为真实位置。而传统的RatSLAM算法和RGBD-SLAM算法在经历图的构建上误差较大，且随着实验路程的增加，误差呈现出增大的趋势。3种算法实验的误差数据统计如表1所示。

表1 3种算法实验的误差数据统计

3 结 论

本文在传统RatSLAM算法原理的基础上，用深度传感器Kinect替代普通的RGB相机，实现了硬件上的创新；对于深度传感器采集的图像，先是用图像直方图法对图像的特征进行提取，同时用相机标定技术对图像的深度信息进行提取，将采集到的图像特征和深度信息制成RGB-D视觉模板，通过视觉模板实现闭环检测，用于实现对位姿感知细胞位姿的纠正，最终实现对经历图的纠正，实现了算法上的创新。结果表明改进后的算法在经历图的构建上比传统的RatSLAM更加精准，且比单纯的RGBD-SLAM导航的误差小，大大提高了导航的精度。

由于提取了图像的深度信息，并将深度信息融合进RatSLAM算法中，导致改进后的算法在实时性上较传统的RatSLAM算法稍差一点。同时本文没有考虑光照、地形等外部条件对RatSLAM模型的影响，这些因素严重时会导致环境重构和机器人定位失败。所以，对算法进一步改进，以提高导航的实时性，同时将光照和地形等外部因素的影响考虑进去，则是下一步研究的重点。