劉書池　楊維

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

關(guān)鍵詞：同步定位與地圖構(gòu)建；無(wú)人機(jī)；FastSLAM算法；粒子群優(yōu)化

DOI：10.15938/j.jhust.2018.04.014

中圖分類號(hào)： TD679

文獻(xiàn)標(biāo)志碼： A

文章編號(hào)： 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 引 言

由于無(wú)人機(jī)（unmanned aerial vehicle， UAV）具有定點(diǎn)懸停、低速飛行、執(zhí)行任務(wù)能力強(qiáng)等優(yōu)點(diǎn)，其被越來(lái)越多地運(yùn)用在各個(gè)領(lǐng)域中。利用無(wú)人機(jī)對(duì)礦井下的環(huán)境進(jìn)行監(jiān)測(cè)，將井下瓦斯?jié)舛?、溫度、氣壓指?shù)等信息實(shí)時(shí)返回監(jiān)控中心，對(duì)未來(lái)實(shí)現(xiàn)少人、無(wú)人自動(dòng)化采礦具有重大意義。因此，準(zhǔn)確估計(jì)無(wú)人機(jī)姿態(tài)以及環(huán)境特征顯得尤為重要。

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

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

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

1 礦井下無(wú)人機(jī)飛行模型

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

1.3 人工路標(biāo)

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

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

2 改進(jìn)的FastSLAM算法

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

3 仿真結(jié)果及分析

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

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

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

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

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

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

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

4 結(jié) 論

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

參 考 文 獻(xiàn)：

[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] 張國(guó)良， 湯文俊， 敬斌，等. 輔助粒子濾波算法改進(jìn)的UFastSLAM算法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)， 2012， 44（11）：123-128.

[4] 周武， 趙春霞. 一種基于遺傳算法的FastSLAM 2.0算法[J]. 機(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] 王超磊， 王田苗， 梁建宏，等. 小型無(wú)人直升機(jī)單目視覺FastSLAM研究[J]. 高技術(shù)通訊， 2013， 23（10）：1061-1067.

[9] 鄭睿， 原魁， 李園. 一種用于移動(dòng)機(jī)器人室內(nèi)定位與導(dǎo)航的二維碼[J]. 高技術(shù)通訊， 2008， 18（4）：369-374.

[10]楊峰. 移動(dòng)機(jī)器人基于多傳感器的地圖構(gòu)建和導(dǎo)航研究[D]. 合肥：合肥工業(yè)大學(xué)， 2008.

[11]鄧偉， 梁嵐珍， 浦劍濤. 大范圍環(huán)境中EKFSLAM問(wèn)題的改進(jìn)算法[J]. 計(jì)算機(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)化的移動(dòng)機(jī)器人SLAM方法[J]. 北京理工大學(xué)學(xué)報(bào)， 2013， 33（11）：513-517.

[16]劉利枚， 蔡自興. 基于改進(jìn)的粒子群優(yōu)化的FastSLAM方法[J]. 高技術(shù)通訊， 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]于金霞， 湯永利， 許景民. 基于多樣性向?qū)У淖赃m應(yīng)重采樣粒子濾波研究[J]. 計(jì)算機(jī)科學(xué)， 2012， 39（6）：231-234.

[19]曲麗萍， 王宏建， 邊信黔. 基于自適應(yīng)重采樣的同步定位與地圖構(gòu)建 [J]. 探測(cè)與控制學(xué)報(bào)， 2012， 34（3）：76-81

[20]王偉， 陳華慶， 韓衛(wèi). 無(wú)人機(jī)自主導(dǎo)航控制的FastSLAM算法研究[J]. 計(jì)算機(jī)仿真， 2015， 32（8）：68-72.

（編輯：關(guān) 毅）