矿井环境下无人机视觉PSOFastSLAM算法的实现

刘书池　杨维

摘 要：为了实现无人机在无GPS的矿井环境下进行自主飞行，达到无人机的精准定位，提出了基于RaoBlackwellized粒子滤波器的快速同步定位与地图创建（fast simultaneous location and mapping， FastSLAM）算法。首先设计了一种适用于矿井环境下的人工路标，建立起了无人机的SLAM算法数学模型，接着提出一种改进算法—PSOFastSLAM算法提高准确性，对无人机的位姿和路标位置进行估计，实现无人机的精准定位和地图绘制。最后对进行仿真实验，仿真结果证明PSOFastSLAM算法有效改善了FastSLAM算法粒子退化的問题，提高了井下无人机定位精度。

关键词：同步定位与地图构建；无人机；FastSLAM算法；粒子群优化

DOI：10.15938/j.jhust.2018.04.014

中图分类号： TD679

文献标志码： A

文章编号： 1007-2683（2018）04-0075-07

Abstract：In order to realize the autonomous flight of Unmanned Aerial Vehicle （UAV） in the mine environment without GPS and also precise positioning it， an algorithm called Fast Simultaneous Location and Mapping （FastSLAM） which is based on RaoBlackwellized particle filter is proposed. Firstly， an artificial road sign suitable for underground mine was designed and the mathematical model of Simultaneous Location and Mapping （SLAM） algorithm for UAV was established. Then an improved algorithm called PSOFastSLAM was proposed to improve the accuracy， to estimate the pose of UAV and the position of guideposts and to achieve the accurate positioning and map building for UAV. Finally， the simulation experiment is simulated， simulation results show that PSOFastSLAM algorithm can improve the particle degeneration problem in FastSLAM algorithm effectively and improve the positioning accuracy of UAV.

Keywords：simultaneous localization and map building； unmanned aerial vehicle； fastSLAM algorithm； particle swarm optimization

0 引 言

由于无人机（unmanned aerial vehicle， UAV）具有定点悬停、低速飞行、执行任务能力强等优点，其被越来越多地运用在各个领域中。利用无人机对矿井下的环境进行监测，将井下瓦斯浓度、温度、气压指数等信息实时返回监控中心，对未来实现少人、无人自动化采矿具有重大意义。因此，准确估计无人机姿态以及环境特征显得尤为重要。

同时定位与地图构建（simultaneous location and mapping， SLAM），是指无人机在部分已知或完全未知的环境下通过外部传感器感知周围环境，对其位姿和环境特征位置进行估计，在实现自身定位更新的同时建造环境地图。

基于扩展卡尔曼滤波（extended kalman filter， EKF）的SLAM算法将非线性函数的泰勒展开式进行一阶截断[1]，忽略了高阶部分的误差，会导致滤波发散，而且当环境特征数目偏大时，算法的效率将大大降低。因此Montemerlo等人提出了基于RaoBlackwellized粒子滤波器的FastSLAM算法，利用一个粒子滤波器估计机器人位姿，N个卡尔曼滤波器评估环境特征的位置[2]。然而，粒子滤波算法需要大量样本才能很准确地接近位姿和环境特征位置的联合后验概率分布，且基于序列重要性采样（sequential importance sampling，SIS）会导致样本退化问题，因此很多学者对FastSLAM算法提出了改进。文[3]利用无迹卡尔曼滤波算法取代EKF对环境特征进行估计，减小EKF的线性化误差，提高算法精度，但未很好的解决粒子退化问题。文[4]将遗传算法融入FastSLAM算法的递归估计过程中，对粒子进行优化。文[5]降低FastSLAM算法维度，减小了所需粒子样本数量，但这无疑也增大了机器人位置估计误差。

本文将粒子群优化算法运用到FastSLAM中，在采样过程中考虑到最新的观测值，对采样过程进行优化，加速粒子收敛，有效解决了粒子退化的问题，提高系统状态估计的效率和精度，实现矿井环境下无人机定位及巷道二维地图构建。由于采用自然路标进行定位需要有明显的线特征或点特征，而矿井下的光线昏暗，可提取特征很少，因此利用人工路标将大大减小特征提取的难度。二维码获取方式简单，且包含更丰富的信息，有较好的自我纠错能力，因此使得矿井下无人机快速准确地运行SLAM算法。

1 礦井下无人机飞行模型

矿井下无人机飞行模型如图1所示。在高度为3m，宽度为4m的矿井巷道顶部沿中间线贴上人工路标，为了计算算法误差，先手工测量好路标坐标。

1.3 人工路标

二维码是根据定位需要来设计其外部轮廓、内部模块、定位模块，二维码的外轮廓与内部模块大多都为正方形[9]。当无人机所带红外摄像头对此类二维码进行识别时，较远距离的成像将导致图像边缘平滑，很难选取合适的阈值对图像进行二值化。因此本文将人工路标的形状设计成正五边形，自身不带定位信息，仅通过对外轮廓的五条线段进行识别就可以确定二维码的位置，并增加一个直径较大的矩形定位模块以确定二维码的方向。其内部模块为圆形，以矩阵方式排列。其编码系统采用BCH码，能有较好的纠错能力。所用人工路标如图4所示。

通过拟合后可以得到定位模块及外轮廓顶点的相对位置，确定二维码的方向，还能通过直线函数求得直线交点坐标，即顶点坐标[10]。经过单应矩阵将二值化后的图像其映射到一个标准二维码图像，与存储好的标准图案进行对比，就可以在已知单位模块的位置上通过像素点进行识别，当位置上存在大于阈值个数的像素点和它相邻点的像素值不同时，即可确定该位置的单位模块取值为1，相反则取值为0，根据编码规则即可得到路标的位置信息。

2 改进的FastSLAM算法

无人机在宽阔的井下巷道行驶时，通过FastSLAM算法来逼近SLAM的后验概率，这种利用粒子构成位置假设的方法使得其对数据关联不敏感，复杂度较小，但容易出现粒子退化问题[11]，造成井下无人机位姿不准确，对于快速飞行的无人机来说，有可能出现偏离航线而撞击巷道壁等后果，因此需要解决粒子退化的问题，提高定位精度。

3 仿真结果及分析

针对本文所提出的PSOFastSLAM算法，在MatlabR2014b的环境下进行仿真实验。设定无人机在创建好的矿井地图中飞行，假设巷道宽4m，无人机的速度为1m/s，在飞行过程中对设置在巷道顶上的二维码进行观测，其观测范围半径为1m（巷道顶壁）。设定路径点构成无人机的实际路径，无人机的起始位置坐标为（0，0），然后根据FastSLAM算法和PSOFastSLAM算法对路径及路标位置进行仿真估计，得到估计路径及路标位置，如图5、图6所示。其中，蓝色实线为实际路径，绿色星点为实际路标位置，黑色实线为估计路径，红色圆点为估计路标位置。本文算法的一些参数为：c1=1.5，c2=1.4，ω=0.8。

由图5可看出在起始位置附近，FastSLAM算法能很好的对井下无人机位姿和路标位置进行估计，但随着粒子样本的退化和误差积累，路标的估计位置和无人机估计路径都出现较大偏移。而从图6可以看出改进后的算法粒子样本退化缓慢，有较好的粒子收敛性，估计路径与实际路径几乎重合，较原算法而言，对无人机的估计路径和路标位置都能进行更准确的估计。

为了更准确地评估两种算法的性能，对两种算法进行多次实验，表1记录了两种算法在粒子数分别为10、50时不同参数的平均误差和均方误差。可以看出当粒子数相同时PSOFastSLAM算法对无人机路径、航向角以及路标位置的估计误差都明显小于FastSLAM算法，能更好的满足矿井下定位精度高的要求。此外，当粒子数为10时的PSOFastSLAM算法得到的无人机位置和路标位置估计的均方误差均小于粒子数为50时的FastSLAM算法的估计误差，因此，PSOFastSLAM在保证高精度的同时可以减少粒子数目，大幅度减少运算时间。

图7为路径估计均方根（root mean square ，RMS），FastSLAM算法得到的最大RMS为0.025m，到后期有发散现象，而PSOFastSLAM得到的最大RMS为0.015m，且收敛较快。在无人机行驶过程中，环境特征通过数据关联进行匹配，当无人机处于某一位置时匹配到的环境特征越多，则估计出的无人机位置误差则更小，反之误差就会更大。

PSOFastSLAM算法改善了FastSLAM算法中的粒子退化现象。

图8为路标位置估计均方根，由于粒子样本退化，路标位置误差增大，FastSLAM算法的最大RMS为0.18m，而PSOFastSLAM算法最大RMS为0.06m，且每个路标误差相当，说明改进后的算法精度更高。

采用式（17） 来评定两种算法的退化程度，随着算法的运行次数变化，随着Neff的值变大粒子退化现象有所减轻，本文的PSOFastSLAM算法和FastSLAM算法退化性能如图9所示。可以看出随着迭代次数的增加，PSOFastSLAM的有效因子Neff始终比FastSLAM的有效因子多，说明

4 结 论

本文结合矿井环境下的实际情况，设计了一种适用于矿井下的人工路标，采用了一种基于人工二维码的无人机视觉PSOFastSLAM算法对无人机进行定位，为了解决粒子退化问题引入了粒子群优化算法，在迭代过程中考虑到粒子本身及全局的影响，减少了所需粒子数，使得粒子更快更有效地接近系统状态的后验概率分布，提高算法精度。仿真结果表明，该方法有较好的一致性和状态估计精度，并能满足矿井环境下的高实时性要求。同时，本文只介绍了单个无人机在矿井下的定位，但在实际情况中会出现多个无人机同时工作的局面。因此，在未来的研究中可以进行多个无人机协作定位，避免算法陷入局部最优，进一步提升算法的可靠性。

参 考 文 献：

[1] LEE Y H， JUN C H， CHOI H， et al. The Analysis of Effect of Observation Models and Data Associations on the Consistency of EKF SLAM[J]. Molecular Therapy the Journal of the American Society of Gene Therapy， 2009， 17（3）：430-438.

[2] MONTEMERLO M S. Fastslam： a Factored Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association[C]// Journal of Machine Learning Research， 2004：2004.

[3] 张国良， 汤文俊， 敬斌，等. 辅助粒子滤波算法改进的UFastSLAM算法[J]. 哈尔滨工业大学学报， 2012， 44（11）：123-128.

[4] 周武， 赵春霞. 一种基于遗传算法的FastSLAM 2.0算法[J]. 机器人， 2009， 31（1）：25-32.

[5] ZIKOS N， PETRIDIS V. LSLAM： Reduced Dimensionality FastSLAM with Unknown Data Association[C]// Robotics and Automation （ICRA）， 2011 IEEE International Conference on. IEEE， 2011：4074-4079.

[6] DURRANTWHYTE H，BAILEY T. Simultaneous Localization and Mapping： part I[J]. IEEE Robotics & Automation Magazine， 2006：13.

[7] SHI J， PAN S， WANG X. Unscented FastSLAM for UAV[J]. Journal of New Industrialization，2014，4：2529-2532

[8] 王超磊， 王田苗， 梁建宏，等. 小型無人直升机单目视觉FastSLAM研究[J]. 高技术通讯， 2013， 23（10）：1061-1067.

[9] 郑睿， 原魁， 李园. 一种用于移动机器人室内定位与导航的二维码[J]. 高技术通讯， 2008， 18（4）：369-374.

[10]杨峰. 移动机器人基于多传感器的地图构建和导航研究[D]. 合肥：合肥工业大学， 2008.

[11]邓伟， 梁岚珍， 浦剑涛. 大范围环境中EKFSLAM问题的改进算法[J]. 计算机仿真， 2014， 31（11）：345-348.

[12]KENNEDY J， EBERHART R. Particle Swarm Optimization[C]// Neural Networks， 1995. Proceedings.， IEEE International Conference on. IEEE， 1995：1942-1948 vol.4.

[13]ZHANG F， CAO J， XU Z. An Improved Particle Swarm Optimization Particle Filtering Algorithm[C]//Communications， Circuits and Systems （ICCCAS）， 2013 International Conference on. IEEE， 2013： 173-177.

[14]HAYDAR Anhan， EMRE ner Tartan， FIKRET Ar. Square Root Unscented Based FastSlam Optimized by Particle Swarm Optimization Passive Congregation[C]// IEEE International Conference on Mechatronics and Automation. IEEE， 2013：469-475.

[15]马家辰， 张琦， 谢玮，等. 基于粒子群优化的移动机器人SLAM方法[J]. 北京理工大学学报， 2013， 33（11）：513-517.

[16]刘利枚， 蔡自兴. 基于改进的粒子群优化的FastSLAM方法[J]. 高技术通讯， 2011， 21（4）：422-427.

[17]YING L， HE B， ZHANG S， et al. A Modified FastSLAM with Simple Particle Swarm Optimization and Consistent Mapping for AUVs[C]// OCEANS 2014TAIPEI. IEEE， 2014：1-5.

[18]于金霞， 汤永利， 许景民. 基于多样性向导的自适应重采样粒子滤波研究[J]. 计算机科学， 2012， 39（6）：231-234.

[19]曲丽萍， 王宏建， 边信黔. 基于自适应重采样的同步定位与地图构建 [J]. 探测与控制学报， 2012， 34（3）：76-81

[20]王伟， 陈华庆， 韩卫. 无人机自主导航控制的FastSLAM算法研究[J]. 计算机仿真， 2015， 32（8）：68-72.

（编辑：关 毅）