吴 怡， 李希胜,2

(1．北京科技大学 自动化学院，北京 100083; 2．北京市工业波谱成像工程技术研究中心，北京 100083)

0 引 言

行人航迹推算(pedestrian dead reckoning,PDR)技术具有定位自主性和连续性的优势，但由于存在累计误差，需要与其他定位技术配合使用。地磁定位技术利用天然磁信息，实现自主定位[1]。融合PDR与地磁指纹进行室内定位算法设计，一方面，地磁能够有效修正PDR累计误差；另一方面，可以通过PDR减少地磁的定位漂移。粒子滤波在PDR/地磁融合定位中应用普遍[2]，但存在权重退化和粒子贫化的问题。

本文改进萤火虫算法，设计粒子滤波过程，通过实验验证优化粒子滤波的效果。

1 算法整体架构

融合PDR/地磁指纹的室内定位主要分为三部分工作：PDR结果估计，地磁指纹建立以及粒子滤波融合。流程框图如图1所示。PDR根据人体运动生理特征，利用惯性信息进行步态检测，步长估计以及航向更新，根据当前的位置更新下一时刻行人位置[3～5]。磁图信息采集选用路径连续采集方式，匀速沿规划通道行走，每跨一步标记一个地磁点，跨步之间的数据作为该地磁点指纹信息[6]，粒子滤波融合阶段，PDR作为预测信息，地磁作为量测信息[7]，输出定位结果。

图1 算法流程框图

2 萤火虫自适应粒子滤波

2.1 自适应权重

粒子滤波中，粒子表示对当前状态的假设，其权重表示发生的近似概率。本文提出自适应权重求解方法，以地磁信息和PDR约束两个方面计算权重，根据两种定位方式的特点设计自适应因子，得到最终的粒子权重系数。算法的整体计算流程如下：

1)计算地磁权重与PDR权重

粒子的权重是由量测值的后验概率模型P(sk|zk)计算得来的，其中zk表示量测信息，sk表示状态信息即粒子信息，由于实际应用中无法准确获得P(sk|zk)，一般认为其服从高斯分布。计算地磁权重与PDR权重

(1)

(2)

psk=psk-1+lk-1f(ψk-1)

(3)

式中lk-1为第k-1步的步长，ψk-1为第k-1步的航向，f(ψk-1)为正余弦函数。

2)计算自适应因子

PDR定位过程中，航向发挥着至关重要的作用，可以根据航向变化情况设计PDR权重的自适应因子，求解公式如式(4)，其中，εk表示前k步的转向次数

(4)

式中ρp,0为初始的PDR自适应因子，取值在0～1之间，初始航向具有较高的信任度，本文定义ρp,0=0.9，εmax为最终转向次数，由具体实验确定。

地磁定位中，可以由磁干扰强度决定地磁自适应因子的取值。地磁变化越大，磁干扰越大，地磁自适应因子取值应越小，具体计算如式(5)所示

(5)

得到自适应因子后，将地磁权重与PDR权重进行归一化，公式如式(6)。归一化后的权重乘以相应的自适应因子，得到粒子的最终权重，如式(7)

(6)

Wj=ρpWp,j+ρmWm,j

(7)

2.2 萤火虫策略重采样

萤火虫算法是一种启发式群智能寻优算法，目前已有的群智能算法优化粒子滤波大多存在时效性差的问题，为此，本文改进萤火虫算法的亮度以及吸引度，平衡搜索能力和寻优速度。

萤火虫的亮度表征粒子所处位置的优劣，粒子滤波中，粒子优劣由权重确定，因此，定义亮度为粒子的权重，如式(8)所示

Ij=Wj

(8)

萤火虫的吸引度可视为粒子个体的寻优搜索范围。边缘粒子距离种群目标较远，难以获取有效亮度信息，易成为失效粒子。然而粒子滤波中，边缘粒子是重采样的主要修正对象，为此，本文对吸引度公式进行如下改进

Bj=B0e-NWj

(9)

式中B0为最大吸引度，N为粒子的样本数目。权重越小，粒子搜索范围越宽，越容易获取距离较远粒子的有效亮度信息。

为加快寻优进程，选用亮度最高粒子作为最优位置，得到位置更新公式

sj=sj+Bj(sbest-sj)+α×(rand-0.5)

(10)

式中sbest为亮度最高粒子位置，α为步长参数。

2.3 自适应粒子分布

萤火虫算法策略的重采样过程有效缓解了粒子贫化，然而随着粒子滤波次数的增加，粒子逐渐聚集，量测约束失效，影响定位精度。为此，本文提出自适应粒子分布，定义差异度参数，判断粒子聚集程度。粒子的聚集程度可以由粒子间距方差来表示，因此差异度参数定义为

(11)

sj=Sk-1+4lk×(rand-0.5)

(12)

萤火虫自适应粒子滤波算法整体流程如下：

1)粒子初始化：采样N个粒子，得到初始粒子集{(s0,j),j=1,2,3,…,N};

2)更新粒子状态：sk,j=sk-1,j+lk-1f(ψk-1);

3)计算自适应权重，并进行归一化;

4)计算有效粒子数Neff，设定有效粒子阈值为2N/3，判断是否重采样;

6)当χk

7)转至步骤(2)，进行下一步求解，直至结束。

3 实验结果与分析

3.1 地磁图构建

为验证本文算法的实际应用价值，以购物商场作为实验环境。初始采样点间隔为0.8 m，克里金插值[8]后间隔为0.2 m，绘制的地磁强度基准图如图2所示。

图2 地磁强度基准图

3.2 实验结果与分析

为验证算法的性能，除采用标准粒子滤波算法对比外，另外加入现有的群智能优化算法：萤火虫优化粒子滤波算法作为参照，将萤火虫算法整体嵌入粒子滤波中。三种算法定位结果如图3(a)所示，累计误差分布概率如图3(b)所示。

图3 定位结果对比

由图3可以看出，标准粒子滤波算法由于粒子贫化的原因，结果偏离真实路径。萤火虫优化粒子滤波算法由于地磁扰动的存在，定位结果单点波动较大。而本文提出的算法，有效降低了累计误差，缓解了单点漂移的情况。

如表1所示，本文提出的粒子滤波算法定位效果最佳，定位精度达到1.1 m，相比于现有的优化粒子滤波算法，降低了时间成本的同时，提高了定位精度，验证了萤火虫自适应粒子滤波的有效性。

表1 算法定位结果参数

4 结 论

本文研究了一种基于粒子滤波的融合PDR/地磁的室内定位算法，萤火虫自适应粒子滤波算法。根据PDR与地磁定位的特点，设计自适应权重的求解方式，改进萤火虫算法的亮度和吸引度，设计重采样过程，根据粒子聚集程度定义差异度参数，进行自适应粒子分布。实验结果表明：优化算法能够有效改善滤波定位效果，达到1.1 m室内定位精度。