2. 上海电力大学 自动化工程学院，上海 200090;
3. 上海交通大学 电气工程系，上海 200240
2. School of Automation Engineering, Shanghai University of Electric Power, Shanghai 200090, China;
3. Department of Electrical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China
智能移动巡检设备中的同时定位和地图创建(simultaneous localization and mapping, SLAM)技术是实现电力设备状态信息智能感知与自动化作业的核心问题[1], 其难点在于如何通过自身携带的传感器, 采用滤波方法实现未知环境地图构建和精准定位.传统的粒子滤波算法(particle filter, PF)[2]虽适用于非线性、非高斯和非平稳的场景, 但受次优选择原则的限制, 往往导致部分有效样本遗失引起样本贫化现象; 而采用重采样方法则易引起粒子退化、降低采样粒子的多样性[2].为解决上述问题, 近年来国内外学者提出了各种改进方案, 其中群优化理论因其强寻优性能发展良好.它通过粒子快速、智能的集中至高似然区域, 改善样本贫化和粒子多样性丧失现象[3-4], 如基于果蝇算法优化的粒子滤波算法[3]、基于蝙蝠算法的粒子滤波方法[4]、基于多策略差分布谷鸟算法的粒子滤波方法[5]、基于萤火虫算法(firefly algorithm, FA)[6]优化的粒子滤波算法等.尽管如此, 基于群智能优化理论改进的粒子滤波算法在计算过程中仍存在盲目搜索、未能权衡好粒子前后期寻优能力的问题[5-6], 因此导致寻优结果出现偏差, 难免陷入早熟或欠收敛.此外, 群样本的计算量大幅增加会导致融合算法和计算过程过于复杂.
针对上述问题, 本文基于粒子滤波运行机制及仿生算法寻优性能对融合算法进行改进, 即通过实时反馈最新时刻观测与状态值重定义萤火虫亮度公式、构建加入观测过程噪声和相对亮度信息的非线性衰减函数, 进而采用分步重采样策略调整粒子分布, 不仅可以弥补标准萤火虫算法过早收敛的不足, 提高预测精度, 而且能够改善传统粒子滤波重采样过程带来的多样性丧失问题, 抑制样本的贫化度.仿真验证表明, 所提出算法改善了传统PF算法的滤波精度, 且应用到基于机器人运动模型的SLAM时提高了定位与导航精度, 同时有良好的稳定性.
1 基于改进萤火虫算法的粒子滤波算法优化标准粒子滤波采用先验概率分布函数预测系统后验概率分布函数, 通过调整粒子的权值和位置信息修正先验分布, 逼近系统状态量与观测量的真值[7], 其参数化表达式为
|
(1) |
其中: wt-1i为t-1时刻粒子i的权重, xti为t时刻粒子i的估计值, p(yt|xti)、p(xti|xt-1i)和q(xti|xt-1i, yt)为系统观测、状态模型和先验概率分布函数, 
FA通过荧光亮度和位置的更新寻求最优解, PF采用更新后的粒子权值和位置逼近状态真实的后验概率密度[7].因此本文将两者结合, 将仿生算法的强寻优特性思想引入PF采样粒子的位置优化中, 重定义仿生算法中的萤火虫亮度, 同时改进步长因子调整机制, 提高融合算法的寻优精度.
为解决标准FA应用到粒子滤波非线性非高斯模型时计算复杂和粒子预测精度下降等问题[8], 采用最新时刻观测和状态值作为反馈信息对当前目标预估结果进行修正, 并重新定义亮度表达, 有
|
(2) |
其中: I为重定义后的相对亮度; pdist(·)euc为欧氏距离; zprei、znewi、xprei、xnewi为第i粒子的预测观测值、最新观测值、预测状态值和最新状态值, 在机器人定位与建图时, zprei、znewi、xprei、xnewi代表第i粒子的预测路标值、最新时刻路标值、预测位姿值和最新时刻位姿值.重定义后公式每次迭代个体粒子最新信息, 不需将每个粒子与最优粒子作比较, 减少了寻求最优解产生的计算成本.同时, 最新时刻粒子观测和状态信息的加入提高了当前及下一时刻目标预测的准确性, 从而有效地提高了系统整体的滤波精度.
在实际模拟萤火虫算法运行初期, 个体间距较大, 较高的全局寻优能力可避免萤火虫早熟、陷入局部极值.在算法迭代后期, 个体可移动步长逐渐变小, 较强的局部寻优能力有利于萤火虫算法后期的收敛, 不会因可移动间距过大而跳过全局最优解.因此, 固定的步长因子α会降低滤波整体的性能, 导致预测精度低和收敛速度慢, 且易在极值点附近产生“振荡”等问题[9].针对以上问题, 本文拟构造非线性衰减函数, 加入观测过程的噪声值, 同时引入萤火虫相对亮度信息, 保证模型对于实际目标函数有较好的近似, 兼顾了收敛速度与稳定误差, 实现α值的自适应调整, 有
|
(3) |
其中:衰减的指数函数exp[-f(I)]满足标准FA中α要求取值范围[0, 1], 且引入了改进后的相对亮度信息f(I); I(i)、I(j)为粒子i、j的相对亮度, f(I)=I(i)/I(j)为相邻两个粒子相对亮度的比率.当f(I) < 1即I(i) < I(j)时, 函数α(i)发散, α(i)随f(I)值的减小而增大, 进而个体的可移动步长变大, 全局寻优能力增强[10]; 反之, f(I)≥ 1即I(i) ≥ I(j)时, 步长因子函数α(i)呈收敛状态, 且α(i)越小, 个体的移动与搜索区域越小, 局部寻优精度越高, 收敛速度越快.改进后粒子i被j吸引并向其移动的位置更新公式为
|
(4) |
其中: xi、xj分别为粒子i和j所在空间位置; β0为萤火虫最大吸引度; γ为光强吸收系数, 典型值为1; rij为粒子i、j的空间距离; 
|
(5) |
|
(6) |
其中: ε=rand-0.5.模拟算法迭代运行时xi2不断进行累加操作, 即
为解决重采样造成的样本退化和预估精度降低等问题[11], 本文提出一种基于偏差修正指数加权平均(exponentially weighted average, EWA)算法[12]的筛选粒子方案, 进而有效使用剩余大权值粒子完成复制和新添, 保证最终粒子数目满足初始总数.
本文针对标准PF重采样直接剔除小粒子而降低滤波性能的问题[13], 设计如下步骤的粒子筛选策略:
1) 计算粒子权重并按升序排列.
2) 采用基于偏差修正的EWA优化算法计算排序后粒子集的平均值分布曲线, 其表达式为
|
(7) |
其中: δi、δi-1为前i、i-1个粒子的平均权重, β为调超参数, wti为第i个粒子的权重, 1-βi为偏差修正项.
3) 计算δi曲线平均值δ并判断粒子权重wti与δ的大小, 若wti≥ δ, 则该粒子留下, 否则舍弃.
采用指数加权平均优化算法考虑到大权值粒子是影响估计精度的决定性因素, 并且引入偏差修正项降低EWA前期的运算误差, 提高了粒子筛选的准确性, 也保证了复制与新添粒子的有效性.
筛选后剩余的粒子为部分大权值粒子, 已不满足总数N, 因此设计复制过程增加有效粒子[13].复制时需确定筛选后剩余粒子的复制次数n=wti/δ, δ为基于偏差修正EWA筛选粒子方案得到的平均值, n为1时复制1次, n>1时向上取整, 复制粒子n次.然而, 粒子权重分布失衡会导致复制后仍不满足总数N, 为此设计新添粒子方案:对剩余粒子的权值降序排列, 利用高斯变异算子依次对xi进行随机扰动从而产生新粒子x'i, 直至满足总数N.新添粒子公式如下:
|
(8) |
其中: xi、x'i为加入随机扰动前、后粒子i的位置; μ为扰动控制因子; M(0, 1)是均值为0、方差为1的高斯分布随机变量; xi × μ × M(0, 1)为高斯扰动项.上述分步重采样改进策略在保证样本多样性的同时解决了传统PF重采样引起的粒子贫化问题, 且提高了算法的滤波能力.
2 粒子滤波仿真实验与分析为了验证本文算法有较好的滤波性能, 采用Matlab 2016a仿真软件, 基于单变量动态变化滤波模型分别进行粒子数目为50、100和150时标准粒子滤波(PF)、萤火虫算法优化粒子滤波(FA-PF)[8]、蝙蝠算法优化粒子滤波(BA-PF)[4]、改进萤火虫算法优化粒子滤波(IFA-PF) 4种算法的仿真, 并给出滤波精度和运行成本的对比和分析.状态方程和观测方程如下:
|
(9) |
其中: w(t)、v(t)为状态方程和观测方程的零均值高斯噪声, 方差为R和Q; x(t)、y(t)为t时刻的状态和观测值.设R、Q均为1, 状态量x初始值为0, 采样步长为100, 萤火虫最大吸引度和光强吸收系数为1, 扰动控制因子为0.01, 可调参数β为0.8.粒子数N为50、100和150, 分别对PF、BA-PF、FA-PF、IFA-PF进行仿真, 对应的状态跟踪结果和误差结果如图 1 ~ 图 3所示.
|
图 1 N=50仿真结果 |
|
图 2 N=100仿真结果 |
|
图 3 N=150仿真结果 |
由PF、BA-PF、FA-PF、IFA-PF这4种算法状态值的预测结果可知:当粒子数不同时, N=50时4种算法预测曲线与真实状态值偏差最大; N增至100时, 相比粒子数为50时预测效果要好; 当N=150时, 预测曲线与真实结果重合度最高, 该仿真结果符合粒子数增加时非线性非高斯模型预测精度变高的理论[9].当粒子数目相同时, IFA-PF预测曲线与真实状态值曲线拟合度最好, BA-PF和FA-PF效果较好, 传统PF效果最差. PF、BA-PF、FA-PF、IFA-PF这4种算法状态值的预测误差绝对值曲线表明: IFA-PF的误差绝对值最小, BA-PF和FA-PF预测误差较低, 传统PF预测误差值最大, 进一步验证了预测结果的可靠性. BA-PF和FA-PF分别将标准蝙蝠算法与萤火虫算法的位置更新机制引入粒子滤波中, 使得部分粒子能够智能地移动至高似然区域, 一定程度改善了传统粒子滤波预测精度差的问题; IFA-PF将粒子最新时刻观测与状态值引入到相对亮度公式, 提高了预测精度; 利用当前时刻相对亮度即时调整步长, 进一步提高了算法的局部搜索能力和更新粒子的有效性, 改善了传统PF重采样导致粒子多样性丧失的现象, 提高了滤波精度.当粒子数N=100、采样步长t=25时, PF、BA-PF、FA-PF、IFA-PF粒子多样性的仿真结果如图 4所示.
|
图 4 粒子相对位置状态分布图 |
由图 4可见, 标准粒子滤波算法重采样后位置分布相对集中, 多样性丧失程度较大.而本文算法粒子在快速、智能地集中至高似然区域的同时, 在空间其他位置也保留了部分粒子, 改善了样本贫化和粒子多样性丧失的现象.
为了进一步验证本文滤波算法的有效性, 选取均方根误差和算法运行时间为预测精度和运行成本的指标[14].参数均方根误差公式如下:
|
(10) |
不同粒子数目下4种算法的RMSE和预测精度提高率见表 1.
|
表 1 4种算法的均方根误差和提高率对比 |
由表 1可见:相比于传统PF, IFA-PF的预测精度提高了29.4 % ~ 47.2 %, PF在粒子数为100时获得3.004 3的RMSE值, 但IFA-PF在粒子数为50时即可完成; PF在粒子数目为150时RMSE为2.441 5, IFA-PF在粒子数为100时即可得到1.884 3的RMSE, 表明IFA-PF可用更少的粒子数目达到更高的预测精度.由粒子数为100时连续仿真4种算法30次的结果可知, 传统PF仿真用时最短, 为0.307 6 s; FA-PF用时最长为0.750 0 s; BA-PF用时为0.700 2 s; IFA-PF用时为0.641 2 s.该数据表明:相比FA-PF, 本文算法的收敛速度提高了14.54 %, 其原因在于: FA-PF在传统PF上引入萤火虫算法, 在提高寻优精度的同时也增加了因算法多次迭代而产生的执行时间, 但IFA-PF通过自适应调整步长即时更新粒子位置, 提高了算法收敛至最优值的速度, 从而在一定程度上降低了算法的运行时间.综上所述, 本文提出的IFA-PF在大幅度提高滤波精度的同时也减少了算法的运算代价.
3 定位与建图实验验证将本文算法应用到SLAM中, 并开展基于机器人运动模型的定位和建图实验.基于Rao- Blackwellised粒子滤波的FastSLAM因定位精度较高、鲁棒性较好等特性已发展成SLAM技术的主流算法[13], 因此开展传统FastSLAM、BA-FastSLAM、FA-FastSLAM算法与本文IFA-FastSLAM算法在移动机器人定位与建图方面其精度和稳定性的实验研究.为了实现机器人在平面上的运转, 采用如下运动模型[14]:
|
(11) |
其中: xt - 1(i)、yt - 1(i)、θt-1(i)为t-1时刻第i个粒子的位置坐标和航向角; Δ t为t-1时刻到t时刻的时间间隔; v、w为机器人运动过程的速度和角速度; wx、wy和wθ分别为机器人运动时x、y方向及航向角对应的噪声项.移动机器人运动参数设置如表 2所示.
|
|
表 2 移动机器人定位与建图实验运动参数设置 |
本文模拟移动机器人定位与建图真实情况建立实验环境, 设置运动范围为100 m × 80 m, 其中人为设定15个航向点和31个路标点.移动机器人从环境中心(0, 0)点出发沿顺时针运动, 通过实时更新运动控制与环境观测信息完成定位与建图, 效果如图 5所示.
|
图 5 4种算法的定位与建图效果 |
由图 5可见, 传统Fast-SLAM, BA-FastSLAM、FA-FastSLAM算法对运行轨迹与环境路标的预测效果较好, 本文算法IFA-FastSLAM预测效果最好.其主要原因在于: BA-FastSLAM、FA-FastSLAM利用仿生寻优思想优化传统Fast-SLAM, 一定程度上提高了机器人定位与建图的精度; IFA-FastSLAM将改进FA算法引入采样粒子位置优化中, 即时修正机器人位姿与环境路标点, 同时采用分步重采样降低了粒子的匮乏度, 从而提高了粒子向高似然概率区域移动的智能性, 实质上是驱使机器人运动到定位精度更高的位置, 所以IFA-FastSLAM对运动轨迹和路标点的定位精度更高.
为了进一步验证改进算法应用到机器人定位与建图上的可行性, 采用均方根误差均值和均方根误差方差评价定位与建图的准确性和稳定性, 并结合运行时间验证本文算法的实时性. 图 6为当机器人速度为3 m/s时, 8 000个采样时刻的传统FastSLAM、BA-FastSLAM、FA-FastSLAM和IFA- FastSLAM算法单次运行时得到的均方根误差. 表 3为连续运行20次的均方根误差均值和方差. 图 7为不同移动速度下4种算法的均方根误差和运行时间.
|
图 6 4种算法预测值与真实值的均方根误差对比 |
|
|
表 3 4种算法对比结果 |
|
图 7 FastSLAM、BA-FastSLAM、FA-FastSLAM与IFA-FastSLAM算法实时性对比 |
由图 6可知:在机器人运动过程中, 传统FastSLAM的定位误差随时间非线性增长, 其误差值最高可至4 m左右, 与真实路径和路标点偏差较大; IFA-FastSLAM在定位过程的误差始终保持在1 m以下, 表明本文算法可实现实时定位与构建路标点的精准性.由表 3数据可知, 相比传统FastSLAM, IFA-FastSLAM算法的定位精度提高了81.4 %, 而均方根方差体现算法预测的稳定性, IFA-FastSLAM的稳定性比传统FastSLAM提高了80.3 %.由图 7中相同速度下4种算法运行时间可知, 标准FastSLAM用时最短, BA-FastSLAM用时较短, 本文算法IFA-FastSLAM用时较长, FA-FastSLAM用时最长.该数据表明:相比标准FastSLAM, BA-FastSLAM和FA-FastSLAM算法定位与建图时运行成本较高, 主要原因是引入仿生寻优机制的标准FastSLAM提高了定位准确性与稳定性的同时也增加了因算法循环迭代而产生的运行时间; 相比FA-FastSLAM, IFA-FastSLAM运行代价较低, 其原因是本文算法提出的自适应步长加快了粒子定位至最优位置的速度, 从而一定程度上提高了算法的执行效率.由图 7中不同速度下均方根误差和运行时间可知, 4种算法随着移动速度的增加, 运行时间降低、算法误差小幅度增加, 且4种算法中传统Fast-SLAM误差增幅最大, FA-FastSLAM增幅较小, 本文算法IFA-FastSLAM增幅最小.其主要原因是: IFA-FastSLAM将改进FA算法引入采样粒子位置优化中, 利用自适应步长因子降低了标准FA算法迭代寻优产生的运算复杂度, 同时提高粒子向高似然概率区域移动的智能性与快速性.该结果表明, 提速后相比FA-FastSLAM算法, 本文算法在高定位与建图精度时运行时间也较少, 因此实时性更好.综上分析, 本文所提出算法在定位与建图上保持高定位精度与稳定性的同时还具有较低的运行代价, 实现了未知场景中移动机器人自身方位和周围环境的精确估计, 进而合理决策与规划动作和路径, 同时也是AR、VR、无人驾驶等智能移动平台环境感知与自动化作业应用与发展的基础[15].
4 结论本文采用IFA-PF算法改进了萤火虫发光与步长调整机制, 并通过改进重采样过程进一步优化了粒子位姿.结果表明:该算法在粒子数为150的单变量动态滤波模型中可获得低至1.288 5的均方根误差, 表明其预测精度优于标准粒子滤波和基于萤火虫算法的粒子滤波算法.在机器人运动模型的定位与建图实验中, 本文采用的IFA-FastSLAM算法可实现0.416 0和0.208 9的均方根误差均值和方差, 在具备了较高定位精度的同时还具有良好的稳定性.上述算法为智能移动设备环境感知与自动化作业提供了一种精确定位和地图构建的方法, 为工业机器人、自动巡检设备的智能化水平奠定了基础.
| [1] |
Cadena C, Carlone L, Carrillo H, et al. Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age[J]. IEEE Transactions on Robotics, 2016, 32(6): 1309-1332. DOI:10.1109/TRO.2016.2624754 |
| [2] |
李天成, 范红旗, 孙树栋. 粒子滤波理论、方法及其在多目标跟踪中的应用[J]. 自动化学报, 2015, 41(12): 1981-2002. (Li T C, Fan H Q, Sun S D. Particle filter theory and method and its application in multi-target tracking[J]. Acta Automatica Scinica, 2015, 41(12): 1981-2002.) |
| [3] |
韩锟, 张赫. 基于果蝇优化算法改进的粒子滤波及其在目标跟踪中的应用[J]. 湖南大学学报:自然科学版, 2018, 45(10): 130-138. (Han K, Zhang H. Improved particle filter based on fruit fly optimization and its application in target tracking[J]. Journal of Huan University: Natural Sciences, 2018, 45(10): 130-138.) |
| [4] |
陈志敏, 田梦楚, 吴盘龙. 基于蝙蝠算法的粒子滤波法研究[J]. 物理学报, 2017, 66(5): 47-56. (Chen Z M, Tian M C, Wu P L. Intelligent particle filter based on bat algorithm[J]. Acta Physica Sinica, 2017, 66(5): 47-56.) |
| [5] |
黄辰, 费继友, 王丽颖, 等. 基于多策略差分布谷鸟算法的粒子滤波方法[J]. 农业机械学报, 2018, 49(4): 265-272. (Huang C, Fei J Y, Wang L Y, et al. Particle filter method based on multi-strategy difference cuckoo search algorithm[J]. Transactions of the Chinese Society for Agricultural, 2018, 49(4): 265-272.) |
| [6] |
朱奇光, 肖亚昆, 陈卫东, 等. 基于萤火虫算法改进移动机器人定位方法研究[J]. 仪器仪表学报, 2016, 37(2): 323-329. (Zhu Q G, Xiao Y K, Chen W D, et al. Research on the improved mobile robot localization approach based on firefly algorithm[J]. Chinese Journal of Scientific Instrument, 2016, 37(2): 323-329. DOI:10.3969/j.issn.0254-3087.2016.02.012) |
| [7] |
Zuo J, Jia Y, Zhang Y, et al. Adaptive iterated particle filter[J]. Electronics Letters, 2013, 49(12): 742-744. DOI:10.1049/el.2012.4506 |
| [8] |
田梦楚, 薄煜明, 陈志敏, 等. 萤火虫算法智能优化粒子滤波[J]. 自动化学报, 2016, 42(1): 89-97. (Tian M C, Bo Y M, Chen Z M, et al. Firefly algorithm intelligence optimized particle filter[J]. Acta Automatica Scinica, 2016, 42(1): 89-97.) |
| [9] |
Darwish S M. Combining firefly algorithm and Bayesian classifier: New direction for automatic multilabel image annotation[J]. IET Image Processing, 2016, 10(10): 763-772. DOI:10.1049/iet-ipr.2015.0492 |
| [10] |
田梦楚, 薄煜明, 吴盘龙, 等. 基于萤火虫优化粒子滤波的新型机动目标跟踪算法[J]. 控制与决策, 2017, 32(10): 1758-1766. (Tian M C, Bo Y M, Wu P L, et al. Novel target tracking method based on firefly algorithm optimized particle filter[J]. Control and Decision, 2017, 32(10): 1758-1766.) |
| [11] |
Wasir A A, Durrani S, Kennedy R A, et al. Particle filters for joint timing and carrier estimation: Improved resampling guidelines and weighted bayesian cramer-rao bounds[J]. IEEE Transactions on Communications, 2012, 60(5): 1407-1419. DOI:10.1109/TCOMM.2012.022912.100559 |
| [12] |
Wang X, Zhang C, Liu F, et al. Exponentially weighted particle filter for simultaneous localization and mapping based on magnetic field measurements[J]. IEEE Transactions on Instrumentation and Measurement, 2017, 66(7): 1658-1667. DOI:10.1109/TIM.2017.2664538 |
| [13] |
Havangi R, Taghirad H D, Nekoui M A, et al. A square root unscented FastSLAM with improved proposal distribution and resampling[J]. IEEE Transactions on Industrial Electronics, 2014, 61(5): 2334-2345. DOI:10.1109/TIE.2013.2270211 |
| [14] |
张毅, 郑潇峰, 罗元, 等. 基于高斯分布重采样的Rao-Blackwellized粒子滤波SLAM算法[J]. 控制与决策, 2016, 31(12): 2299-2304. (Zhang Y, Zheng X F, Luo Y, et al. SLAM algorithm with Gaussian distributed resampling Rao-Blackwellized particle filter[J]. Control and Decision, 2016, 31(12): 2299-2304.) |
| [15] |
Bresson G, Alsayed Z, Yu L, et al. Simultaneous localization and mapping: A survey of current trends in autonomous driving[J]. IEEE Transactions on Intelligent Vehicles, 2017, 2(3): 194-220. DOI:10.1109/TIV.2017.2749181 |