劉書池　楊維

摘 要：為了實現(xiàn)無人機在無GPS的礦井環(huán)境下進行自主飛行，達到無人機的精準定位，提出了基于RaoBlackwellized粒子濾波器的快速同步定位與地圖創(chuàng)建（fast simultaneous location and mapping， FastSLAM）算法。首先設計了一種適用于礦井環(huán)境下的人工路標，建立起了無人機的SLAM算法數(shù)學模型，接著提出一種改進算法—PSOFastSLAM算法提高準確性，對無人機的位姿和路標位置進行估計，實現(xiàn)無人機的精準定位和地圖繪制。最后對進行仿真實驗，仿真結果證明PSOFastSLAM算法有效改善了FastSLAM算法粒子退化的問題，提高了井下無人機定位精度。

關鍵詞：同步定位與地圖構建；無人機；FastSLAM算法；粒子群優(yōu)化

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）具有定點懸停、低速飛行、執(zhí)行任務能力強等優(yōu)點，其被越來越多地運用在各個領域中。利用無人機對礦井下的環(huán)境進行監(jiān)測，將井下瓦斯?jié)舛?、溫度、氣壓指?shù)等信息實時返回監(jiān)控中心，對未來實現(xiàn)少人、無人自動化采礦具有重大意義。因此，準確估計無人機姿態(tài)以及環(huán)境特征顯得尤為重要。

同時定位與地圖構建（simultaneous location and mapping， SLAM），是指無人機在部分已知或完全未知的環(huán)境下通過外部傳感器感知周圍環(huán)境，對其位姿和環(huán)境特征位置進行估計，在實現(xiàn)自身定位更新的同時建造環(huán)境地圖。

基于擴展卡爾曼濾波（extended kalman filter， EKF）的SLAM算法將非線性函數(shù)的泰勒展開式進行一階截斷[1]，忽略了高階部分的誤差，會導致濾波發(fā)散，而且當環(huán)境特征數(shù)目偏大時，算法的效率將大大降低。因此Montemerlo等人提出了基于RaoBlackwellized粒子濾波器的FastSLAM算法，利用一個粒子濾波器估計機器人位姿，N個卡爾曼濾波器評估環(huán)境特征的位置[2]。然而，粒子濾波算法需要大量樣本才能很準確地接近位姿和環(huán)境特征位置的聯(lián)合后驗概率分布，且基于序列重要性采樣（sequential importance sampling，SIS）會導致樣本退化問題，因此很多學者對FastSLAM算法提出了改進。文[3]利用無跡卡爾曼濾波算法取代EKF對環(huán)境特征進行估計，減小EKF的線性化誤差，提高算法精度，但未很好的解決粒子退化問題。文[4]將遺傳算法融入FastSLAM算法的遞歸估計過程中，對粒子進行優(yōu)化。文[5]降低FastSLAM算法維度，減小了所需粒子樣本數(shù)量，但這無疑也增大了機器人位置估計誤差。

本文將粒子群優(yōu)化算法運用到FastSLAM中，在采樣過程中考慮到最新的觀測值，對采樣過程進行優(yōu)化，加速粒子收斂，有效解決了粒子退化的問題，提高系統(tǒng)狀態(tài)估計的效率和精度，實現(xiàn)礦井環(huán)境下無人機定位及巷道二維地圖構建。由于采用自然路標進行定位需要有明顯的線特征或點特征，而礦井下的光線昏暗，可提取特征很少，因此利用人工路標將大大減小特征提取的難度。二維碼獲取方式簡單，且包含更豐富的信息，有較好的自我糾錯能力，因此使得礦井下無人機快速準確地運行SLAM算法。

1 礦井下無人機飛行模型

礦井下無人機飛行模型如圖1所示。在高度為3m，寬度為4m的礦井巷道頂部沿中間線貼上人工路標，為了計算算法誤差，先手工測量好路標坐標。

1.3 人工路標

二維碼是根據定位需要來設計其外部輪廓、內部模塊、定位模塊，二維碼的外輪廓與內部模塊大多都為正方形[9]。當無人機所帶紅外攝像頭對此類二維碼進行識別時，較遠距離的成像將導致圖像邊緣平滑，很難選取合適的閾值對圖像進行二值化。因此本文將人工路標的形狀設計成正五邊形，自身不帶定位信息，僅通過對外輪廓的五條線段進行識別就可以確定二維碼的位置，并增加一個直徑較大的矩形定位模塊以確定二維碼的方向。其內部模塊為圓形，以矩陣方式排列。其編碼系統(tǒng)采用BCH碼，能有較好的糾錯能力。所用人工路標如圖4所示。

通過擬合后可以得到定位模塊及外輪廓頂點的相對位置，確定二維碼的方向，還能通過直線函數(shù)求得直線交點坐標，即頂點坐標[10]。經過單應矩陣將二值化后的圖像其映射到一個標準二維碼圖像，與存儲好的標準圖案進行對比，就可以在已知單位模塊的位置上通過像素點進行識別，當位置上存在大于閾值個數(shù)的像素點和它相鄰點的像素值不同時，即可確定該位置的單位模塊取值為1，相反則取值為0，根據編碼規(guī)則即可得到路標的位置信息。

2 改進的FastSLAM算法

無人機在寬闊的井下巷道行駛時，通過FastSLAM算法來逼近SLAM的后驗概率，這種利用粒子構成位置假設的方法使得其對數(shù)據關聯(lián)不敏感，復雜度較小，但容易出現(xiàn)粒子退化問題[11]，造成井下無人機位姿不準確，對于快速飛行的無人機來說，有可能出現(xiàn)偏離航線而撞擊巷道壁等后果，因此需要解決粒子退化的問題，提高定位精度。

3 仿真結果及分析

針對本文所提出的PSOFastSLAM算法，在MatlabR2014b的環(huán)境下進行仿真實驗。設定無人機在創(chuàng)建好的礦井地圖中飛行，假設巷道寬4m，無人機的速度為1m/s，在飛行過程中對設置在巷道頂上的二維碼進行觀測，其觀測范圍半徑為1m（巷道頂壁）。設定路徑點構成無人機的實際路徑，無人機的起始位置坐標為（0，0），然后根據FastSLAM算法和PSOFastSLAM算法對路徑及路標位置進行仿真估計，得到估計路徑及路標位置，如圖5、圖6所示。其中，藍色實線為實際路徑，綠色星點為實際路標位置，黑色實線為估計路徑，紅色圓點為估計路標位置。本文算法的一些參數(shù)為：c1=1.5，c2=1.4，ω=0.8。

由圖5可看出在起始位置附近，F(xiàn)astSLAM算法能很好的對井下無人機位姿和路標位置進行估計，但隨著粒子樣本的退化和誤差積累，路標的估計位置和無人機估計路徑都出現(xiàn)較大偏移。而從圖6可以看出改進后的算法粒子樣本退化緩慢，有較好的粒子收斂性，估計路徑與實際路徑幾乎重合，較原算法而言，對無人機的估計路徑和路標位置都能進行更準確的估計。

為了更準確地評估兩種算法的性能，對兩種算法進行多次實驗，表1記錄了兩種算法在粒子數(shù)分別為10、50時不同參數(shù)的平均誤差和均方誤差。可以看出當粒子數(shù)相同時PSOFastSLAM算法對無人機路徑、航向角以及路標位置的估計誤差都明顯小于FastSLAM算法，能更好的滿足礦井下定位精度高的要求。此外，當粒子數(shù)為10時的PSOFastSLAM算法得到的無人機位置和路標位置估計的均方誤差均小于粒子數(shù)為50時的FastSLAM算法的估計誤差，因此，PSOFastSLAM在保證高精度的同時可以減少粒子數(shù)目，大幅度減少運算時間。

圖7為路徑估計均方根（root mean square ，RMS），F(xiàn)astSLAM算法得到的最大RMS為0.025m，到后期有發(fā)散現(xiàn)象，而PSOFastSLAM得到的最大RMS為0.015m，且收斂較快。在無人機行駛過程中，環(huán)境特征通過數(shù)據關聯(lián)進行匹配，當無人機處于某一位置時匹配到的環(huán)境特征越多，則估計出的無人機位置誤差則更小，反之誤差就會更大。

PSOFastSLAM算法改善了FastSLAM算法中的粒子退化現(xiàn)象。

圖8為路標位置估計均方根，由于粒子樣本退化，路標位置誤差增大，F(xiàn)astSLAM算法的最大RMS為0.18m，而PSOFastSLAM算法最大RMS為0.06m，且每個路標誤差相當，說明改進后的算法精度更高。

采用式（17） 來評定兩種算法的退化程度，隨著算法的運行次數(shù)變化，隨著Neff的值變大粒子退化現(xiàn)象有所減輕，本文的PSOFastSLAM算法和FastSLAM算法退化性能如圖9所示?？梢钥闯鲭S著迭代次數(shù)的增加，PSOFastSLAM的有效因子Neff始終比FastSLAM的有效因子多，說明

4 結 論

本文結合礦井環(huán)境下的實際情況，設計了一種適用于礦井下的人工路標，采用了一種基于人工二維碼的無人機視覺PSOFastSLAM算法對無人機進行定位，為了解決粒子退化問題引入了粒子群優(yōu)化算法，在迭代過程中考慮到粒子本身及全局的影響，減少了所需粒子數(shù)，使得粒子更快更有效地接近系統(tǒng)狀態(tài)的后驗概率分布，提高算法精度。仿真結果表明，該方法有較好的一致性和狀態(tài)估計精度，并能滿足礦井環(huán)境下的高實時性要求。同時，本文只介紹了單個無人機在礦井下的定位，但在實際情況中會出現(xiàn)多個無人機同時工作的局面。因此，在未來的研究中可以進行多個無人機協(xié)作定位，避免算法陷入局部最優(yōu)，進一步提升算法的可靠性。

參 考 文 獻：

[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]. 哈爾濱工業(yè)大學學報， 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]. 合肥：合肥工業(yè)大學， 2008.

[11]鄧偉， 梁嵐珍， 浦劍濤. 大范圍環(huán)境中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]馬家辰， 張琦， 謝瑋，等. 基于粒子群優(yōu)化的移動機器人SLAM方法[J]. 北京理工大學學報， 2013， 33（11）：513-517.

[16]劉利枚， 蔡自興. 基于改進的粒子群優(yōu)化的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]王偉， 陳華慶， 韓衛(wèi). 無人機自主導航控制的FastSLAM算法研究[J]. 計算機仿真， 2015， 32（8）：68-72.

（編輯：關 毅）