基于Wi-Fi指纹定位的智能车仿生声纳SLAM算法研究

摘要

仿真实验表明，采用基于Wi-Fi指纹定位的BatSLAM模型，不仅实现了智能车室内二维经验图的构建，而且提高了定位的准确性和精度，改善经验图的失真问题，实现经验图的优化效果。

引言

同步定位与地图构建(Simultaneous Location and Mapping，SLAM) 就是运动载体利用自身携带的传感器获得周围环境的地图，同时确定自己在地图中的位置^[1]。智能车在很多领域得到了大量应用，例如室内进行快递的运送，餐厅内食物的传送等，均使用了SLAM技术。

SLAM问题应用最成功的是概率的方法，如卡尔曼滤波算法，扩展卡尔曼滤波算法，粒子滤波算法^[2]等。虽然有些传统的SLAM算法使用声纳传感器采集信息，但是更多被视觉传感器^[3]、激光测距传感器^[4]所替代。使用声纳传感器采集信息，常常得到的是周围环境的粗略信息。生物声纳，能够实现在复杂的环境下的定位和导航，因为它从回声中提取更多的信息，可以模仿蝙蝠声纳的发射与接收过程，利用仿生声纳系统实现复杂环境下的定位与导航。

概率方法广泛应用于SLAM的同时，澳大利亚昆士兰大学Michael等提出了一种基于复合位姿表征的啮齿动物海马区扩展模型（RatSLAM）[5]。RatSLAM的路径积分是通过里程计获取速度与角度信息，同时使用视觉传感器对里程计得到的信息进行修正与更新，绘制精确的经验图。文献^[6]已经证明在RatSLAM基础上，可以使用仿生声纳模板来代替RatSLAM系统中的视觉模板，用双耳平滑耳蜗图来代替视觉图像，使用BatSLAM模型来构建二维经验图。

科学技术在不断发展，目前提高定位精度的方法越来越多，当前主流定位技术主要是无线局域网（Wi-Fi）、蓝牙、惯性定位等。文献[7]提出了基于RSSI的室内定位技术来解决当前定位系统中成本高、定位精度低、不能适应室内环境多样性等问题。文献[8]提出了在RSSI技术的基础上引入TOA（Time of Arrive，到达时间）技术，从而实现基于TOA的指纹定位系统，利用无线信号的连续性改进定位精度。

本文将BatSLAM与Wi-Fi指纹定位相结合，通过提高定位的准确性和精度，改善经验图的失真问题，实现经验图的优化效果。

1 算法原理与设计

本文设计了1种可以提高智能车室内定位精度的改进仿生声纳SLAM的算法，在原有的BatSLAM的基础上，使用Wi-Fi指纹模板对位姿细胞进行辅助修正，从而提高定位精度。首先使用所设计的仿生声纳硬件系统来采集智能车接收到的回波信息，对采集到的回波信息进行处理后，将耳蜗能量谱作为仿生声纳模板，构建的仿生声纳模板生成局部场景细胞。智能车的位姿状态用位姿感知细胞进行表示。利用智能车的里程计和转角传感器来不断更新位姿感知细胞，利用局部场景细胞来实现对位姿细胞的修正，当仿生声纳模板识别失败时，此时使用Wi-Fi指纹模板来实现位姿细胞的修正。本文首先使用仿生声纳模板进行修正，辅助使用Wi-Fi指纹模板修正，不但可以提高定位精度，而且可以实现经验图的优化。

改进后的仿生声纳SLAM定位算法的结构框图如图1所示。

图1 改进后的仿生声纳SLAM算法的结构框图

1.1仿生声纳系统采集回波信号

蝙蝠在进化过程中形成了完善的声纳系统。该系统中必不可少的是发声系统和听觉系统。连续发射频率可调(CTFM)声纳是1种宽带主动式声学探测方式，在机器人导航，以及导盲等领域得到广泛应用。从仿生学的角度模仿蝙蝠的嘴巴和耳朵，利用CTFM模型构建仿生声纳系统来进行声纳信号的发射和数据采集。仿生声纳系统主要由信号发生模块，回波采集模块组成。

1.1.1 信号发生模块

仿生声纳系统的关键是模仿蝙蝠声纳脉冲来设计适应于该系统的探测波源。基于STM32可以快捷地构造所需波形。信号发生器使用直接数字式（DDS），首先对波形进行采样，将采样后得到的数据点进行量化并存入存储器，将存储其中地址对应的波形数据点，通过DAC转换成模拟量进行输出[8]。

蝙蝠发出的声音信号，脉冲主要分为3种类型：恒频（CF）、调频（FM）和复频(CF-FM)。在对生物声纳探测的研究过程中，CF脉冲和FM脉冲的使用最为普遍。CF脉冲持续时间长，具有能量值大的优点，适合对目标的远距离探测。FM脉冲属于宽频带脉冲，频率一般由高到低，适合对近距离物体的识别和定位。为获得侦测、识别和定位的能力，蝙蝠很多时候发出的脉冲是由CF和FM的混合而成。CF脉冲的频率为c，具有长度和发送速度可调的特性。FM脉冲频率为(c-d)-(c+d)（c>d），频率随时间线性变换。为更加符合蝙蝠啁啾脉冲的特征，将FM脉冲的幅值进行半波调制。FM脉冲的函数表达式如式（1）所示。

（1）

其中， 为（c+d），a为频率相对于时间的变换率，T为脉冲时间长度。这2种波形通过STM32的DA芯片转换成模拟量，经过电压放大与驱动电路后使用超声波换能器进行输出。

1.1.2 回波采集模块

信号发生器发出仿生声纳信号来对前方的障碍物进行检测，通过超声波换能器来接收反射波并将反射波信号转化为电信号，电信号经过信号调理电路进行电压放大以及滤波处理，通过数模转换器转换为数字信号送至STM32，STM32接收到的回波信号分别为，。

1.2 仿生声纳模板的构建

STM32接收到的信号分别为，。耳蜗图作为局部视图模板，其构建过程如下：对于接收到的信号使用1组Gammachirp滤波器处理，利用Gammachirp听觉滤波器组来模拟人类耳蜗基底膜，分别得到左耳与右耳的耳蜗能量谱，计算公式如式（2）式（3）所示。

（2）

（3）

其中 ，，分别为左耳与右耳耳蜗能量谱，为一阶低通滤波，。

在使用耳蜗谱作为仿生声纳模板时，这里使用高斯滤波器进行平滑处理，进一步排除细小位置改变带来的影响。2个平滑和二次采样的单声道耳蜗图连接起来，形成1个双耳耳蜗图如式（4）所示。

（4）

其中， 为左侧二次采样耳蜗图， 为右侧二次采样耳蜗图， 高斯平滑滤波器。

使用标准化双耳平滑耳蜗图，计算公式如式（5）所示。

（5）

将标准化双耳平滑耳蜗图作为当前仿生声纳模版，计算公式如式（6）所示。

（6）

1.3 位姿感知细胞

智能车到达室内具体位置时，其处于空间中的位姿感知细胞网络（X，Y和Θ）中。此时会利用连续吸引子网络（CAN）来实时控制智能车在（X，Y和Θ）空间中的活动情况。路径积分是指通过不断地获取智能车的里程计、转角传感器的数据以及智能车在（X，Y和Θ）空间中的位姿变化情况来的实现位姿感知细胞的更新，并且利用生成的仿生声纳模板来实现位姿感知细胞的位姿修正。

1.3.1 CAN模型

位姿细胞内部动态过程主要包括兴奋度更新，对所有细胞的全局抑制。使用1个三维离散高斯分布来创建兴奋度权重矩阵，如式(7)所示。

(7)

其中，k_m，k_n分别表示位置与方向的常数。a 、b 、c 为 x`、y` 、Θ` 的分布系数；

由于兴奋性造成的位姿感知细胞的活动变化如式(8)所示。

(8)

其中N_{x`</sub>，N<sub>y`}, N_θ`, 为 空间中的位姿感知细胞三维矩阵元素。对于位姿感知细胞的活动变化的全局抑制如式(9)所示。

(9)

其中k_m，k_n抑制常数φ控制全局抑制水平，且兴奋度水平被限制在非负数范围。

1.3.2 路径积分

路径积分主要对位姿感知细胞活性进行更新。路径积分令位姿感知细胞的性能可以独立于感知变化速率和里程计速度与角速传感器获取的角速度，不但可以获取较为准确的智能车运行轨迹，而且不需要改变具体的参数。通过路径积分，位姿感知细胞活性得到更新如式（10）所示。

(10)

其中为在x` ，y` ，θ` 方向被四舍五入后的整数偏差由式(11)计算得到。

(11)

其中是路径积分常数，v表示加速度计速度，w表示陀螺仪得到的角速度。

1.3.3 仿生声纳模板修正位姿感知细胞

采用双耳平滑耳蜗图作为仿生声纳模板，局部场景细胞由1个一维细胞阵列组成，每个细胞单元对应1个特定的仿生声纳模板。在路径积分时，由于位置和方向上的累计误差，使用仿生声纳模板来进行修正。对于每个新的场景，算法会建立新的仿生声纳模板，同时建立局部场景细胞与位姿感知细胞间的联系。进行更新的连接强度如式(12)所示。

(12)

其中，G_i为局部场景细胞的活动水平，为位姿感知细胞的活动水平。

对于每一个双耳平滑耳蜗图使用绝对差值和(SAD)模块进行处理产生局部场景。SAD模块将当前耳蜗图与模板库中的耳蜗图进行比较，通过计算所有模板中耳蜗图的像素亮度与当前耳蜗图的像素亮度之差，如式(13)所示。

(13)

其中， 表示当前耳蜗图的像素亮度， 表示数据库中的第i个仿生声纳模板的像素亮度。如果像素亮度之差低于某一阈值，当前耳蜗图被识别出来，对位姿感知细胞进行校正，如式(14)所示。

(14)

如果像素亮度之差高于某一阈值 ，当前耳蜗图成为新的仿生声纳模板并加入到模板库中。阈值 如式(15)所示。

(15)

其中，n_G为数据库中的仿生声纳模板的数量，α_t为调整系统的比例因子，α_t设置为0.5。

1.4 Wi-Fi指纹模版修正位姿感知细胞

本文所设计Wi-Fi指纹定位算法与传统的位置指纹定位算法相类似，是将实际环境中的具体位置和Wi-Fi指纹相联系，具体的位置与特定的Wi-Fi指纹相对应。本文选取的Wi-Fi指纹特征是具体位置检测得到的基站信号的RSS(信号接收强度)。基于RSS(信号接收强度)的Wi-Fi指纹定位算法主要包括离线采集阶段和在线定位阶段。

1.4.1离线训练阶段

离线训练阶段的主要工作是完成离线指纹数据库的构建，主要包括3个部分：选取参考点、采集指纹、构建指纹数据库。首先绘制出室内环境的地图，接下来合理的选取进行测试的参考点，同时对各个参考点接收到的不同AP（无线接入点）的RSSI(信号接收强度的平均值)进行统计，不同AP的RSSI作为参考点的特征，并将其记录在Wi-Fi指纹数据库中。

离线指纹数据库的具体构建过程：在Wi-Fi装置调试完成后首先设置N个AP，接下来在室内环境中保持相同的距离设置参考点,共设置w个参考点，参考点的坐标为 ，最后在每个参考点上采集不同AP的RSSI，并且将每个AP的均值存储于Wi-Fi指纹模板数据库中，Wi-Fi指纹数据库如式（16）所示：

(16)

其中， 表示所选取参考点的集合， , 为参考点的位置，参考点的数目为w； 为所设置的无线接入点，即观测点的集合；N为观测点的数目， 是Wi-Fi指纹全部均值的集合，如式（17）所示。

（17）

其中，为第j个AP在位置V_i处的均值。

离线阶段的原理图如图2所示。

图2 离线训练阶段原理图

1.4.2在线定位阶段

在线定位阶段主要是利用智能车自身携带的通讯设备在室内空间中的具体位置对各个AP的RSSI进行采集，采集完成后使用一定的匹配算法，将采集到的实时数据与离线的Wi-Fi指纹数据库中的数据进行对比，通过对比可以在离线的Wi-Fi指纹数据库中得到与实时数据相似的Wi-Fi指纹，从而完成智能车的实时定位，本文采用的在线指纹匹配算法是K加权近邻算法。

K加权近邻算法是将采集到的实时数据与离线的Wi-Fi指纹数据库中的数据进行对比，通过求解采集的实时数据（不同AP的RSSI）与Wi-Fi指纹数据库中相应AP的RSSI间的欧式距离，然后将匹配获取的K个与实时定位点相似的Wi-Fi指纹，得到这K个Wi-Fi指纹的位置坐标，并且将这些位置坐标根据相似度属性给予不同的权重值，最后根据这些权重值对位置坐标进行加权求和，从而获取最终的定位。K加权近邻算法如图所示。

首先采集到实时的第i个Wi-Fi位置指纹，其与指纹数据库中的离线Wi-Fi指纹间的欧式距离为D_i，如式（18）所示。

（18）

其中，是第j个AP在第i个参考点的信号接收强度的平均值， 是实时采集的参考点在第j个AP的信号接收强度的平均值。

接下来将获取的 进行排序，得到K个欧式距离最小的参考点，再根据这些参考点的相似度属性赋予不同的加权系数u_i，如式（19）所示。

（19）

最后对这K个参考点进行加权求和，定位坐标如式（20）所示。

（20）

(X_i,Y_i)是K个Wi-Fi指纹对应的坐标。在线阶段原理图如图3所示。

图3 在线定位阶段原理图

1.4.3 Wi-Fi指纹模板修正

对于每个新的场景，算法会建立新的Wi-Fi指纹模板。同时建立Wi-Fi指纹与位姿感知细胞间的联系。进行更新的连接强度如式(21)所示。

(21)

其中，Y_i指的是第i个Wi-Fi指纹模板。将当前Wi-Fi指纹和数据库中Wi-Fi指纹模板进行对比，若低于阈值则对位姿感知细胞进行修正，如式(22)所示。

(22)

如果高于阈值y，当前Wi-Fi指纹成为新的Wi-Fi指纹模板并加入到Wi-Fi指纹模板库中。

1.5 经验图的构建与校正

每个经验关联经验能级E_i，位姿细胞 P_xi,yi,θi和局部场景细胞 G_i，与Wi-Fi指纹模板 Yi，第i个经验如式(23)所示。

(23)

当前位姿感知细胞的活性或当前的Wi-Fi 指纹决定经验能级 E_i的各组成要素，如式（24）所示。

(24)

其中，X`<sub>pc</sub>,Y`_pc, θ`<sub>pc</sub>为最大活性姿态细胞坐标；,为与该经验相关的位姿感知细胞坐标；R<sub>a</sub>为(X`,Y`)平面的区域常数；θa为θ`维上区域常数：为当前局部场景细胞； Gⁱ为与经验i相关的局部场景细胞, 为当前的音频感知哈希， 与经验i相关的音频感知哈希。

使用当前位姿信息、Wi-Fi指纹模板和仿生声纳模板与经验图进行匹配时，当到达经验过的地方时，进行1次闭环检测，并对经验图进行校正，经验位姿改变如式(25)所示。

(25)

其中， 为校正常数， 为经验i到其他经验的连接数； 为从其他经验到经验i的连接数。