基于邊界的無人機(jī)主動SLAM算法

沈永福，王希彬

（1.海軍軍訓(xùn)器材研究所，北京102308；2.海軍航空工程學(xué)院控制工程系，山東煙臺264001）

基于邊界的無人機(jī)主動SLAM算法

沈永福1，王希彬2

（1.海軍軍訓(xùn)器材研究所，北京102308；2.海軍航空工程學(xué)院控制工程系，山東煙臺264001）

同步定位與地圖構(gòu)建技術(shù)是無人機(jī)實(shí)現(xiàn)真正自主導(dǎo)航的關(guān)鍵。為克服被動同步定位與地圖構(gòu)建算法的缺陷，研究了基于邊界的無人機(jī)主動同步定位與地圖構(gòu)建算法。在無人機(jī)的探測區(qū)域周圍產(chǎn)生候選邊界點(diǎn)，通過建立合理的目標(biāo)函數(shù)，從候選邊界點(diǎn)中選擇目標(biāo)點(diǎn)，控制無人機(jī)朝該目標(biāo)點(diǎn)方向運(yùn)動，再運(yùn)用擴(kuò)展卡爾曼濾波算法更新無人機(jī)的運(yùn)動狀態(tài)。通過建立的無人機(jī)簡化模型，對提出的算法和隨機(jī)同步定位與地圖構(gòu)建算法進(jìn)行對比研究，仿真結(jié)果表明該算法是有效可行的。

主動同步定位與地圖構(gòu)建；邊界；擴(kuò)展卡爾曼濾波；無人機(jī)

同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）是無人機(jī)實(shí)現(xiàn)真正自主導(dǎo)航的關(guān)鍵。其本質(zhì)就是在先驗(yàn)信息未知的條件下，利用無人機(jī)機(jī)載傳感器獲得周圍環(huán)境的地圖，同時(shí)確定無人機(jī)在地圖中的位置[1]。但是現(xiàn)有的SLAM算法都是假定運(yùn)動軌跡預(yù)先已知的，這種被動形式的SLAM與環(huán)境的未知性相矛盾。如何主動地、自適應(yīng)地探索環(huán)境成為主動SLAM研究的課題。

主動SLAM問題是由Federal、Leonard和Smith[2]首先提出的。目前，研究主動SLAM[3-7]的方法主要有最優(yōu)控制方法[8]、基于邊界的方法[9]和局部子圖法[10]。相對局部子圖和基于邊界的主動SLAM而言，基于最優(yōu)控制的方法建立的目標(biāo)函數(shù)是選擇航跡規(guī)劃點(diǎn)的基礎(chǔ)，但是容易陷入局部最優(yōu)。局部子圖的方法更適用于多無人機(jī)協(xié)同的SLAM航跡規(guī)劃，對于單個(gè)無人機(jī)而言，除了可以減小一定計(jì)算量外，并沒有明顯的優(yōu)勢。而基于邊界的方法，從某種意義上講，就是多步預(yù)測的最優(yōu)控制的無人機(jī)主動SLAM，可以極大地克服最優(yōu)控制的局部最優(yōu)現(xiàn)象。

本文研究了基于邊界的無人機(jī)主動SLAM算法，對產(chǎn)生的邊界點(diǎn)設(shè)計(jì)了一種新的目標(biāo)函數(shù)，通過對預(yù)選邊界點(diǎn)的評價(jià)確定無人機(jī)運(yùn)動的目標(biāo)點(diǎn)，并利用擴(kuò)展卡爾曼濾波（EKF）算法更新系統(tǒng)狀態(tài)。通過建立簡化的無人機(jī)運(yùn)動模型，運(yùn)用Matlab仿真驗(yàn)證了該方法的可行性和有效性。

1 基于邊界的主動SLAM

所謂邊界就是由已探測區(qū)域的邊緣點(diǎn)構(gòu)成的集合。基于邊界的方法是一種使用局部點(diǎn)規(guī)劃來解決SLAM問題的主動自適應(yīng)方法[11]，給出即時(shí)的局部目標(biāo)點(diǎn)產(chǎn)生控制輸入用于探索和作圖。首先，在已探索的區(qū)域上從邊界點(diǎn)得到預(yù)選目標(biāo)點(diǎn)，通過將邊界點(diǎn)分組，把具有相似特征的邊界點(diǎn)作為整體來對待，能夠極大地減少計(jì)算復(fù)雜性。然后，考慮各種因素，使用性能評價(jià)函數(shù)從預(yù)選目標(biāo)點(diǎn)中選擇局部最優(yōu)目標(biāo)點(diǎn)。一旦確定了目標(biāo)點(diǎn)，以預(yù)測步長的形式產(chǎn)生相應(yīng)的控制輸入，這些控制輸入會驅(qū)動無人機(jī)朝著目標(biāo)點(diǎn)運(yùn)動。在朝著局部目標(biāo)點(diǎn)的運(yùn)動過程中，無人機(jī)設(shè)法完成探索與作圖任務(wù)，并且將EKF算法用于更新系統(tǒng)的狀態(tài)。

由上述可知，基于邊界的主動SLAM方法可以分為以下3個(gè)關(guān)鍵階段。

1.1 候選邊界點(diǎn)選取

無人機(jī)在探索未知環(huán)境時(shí)，邊界點(diǎn)的數(shù)量會迅速增長，若要評價(jià)所有的邊界點(diǎn)則需要很高的計(jì)算成本，而且必要性不大。由于每一個(gè)邊界點(diǎn)通常和鄰域點(diǎn)具有相似或相同的特征，把相似或相同特征的邊界點(diǎn)編為相似的組并作為整體來評價(jià)[12]，在不損失探索和作圖性能的前提下可以達(dá)到降低計(jì)算成本的目的。

如圖1所示，無人機(jī)用三角形表示，攝像機(jī)作為機(jī)載傳感器，考慮攝像機(jī)的運(yùn)動特性，假設(shè)其探索區(qū)域?yàn)橐簧刃危車谏狞c(diǎn)表示邊界點(diǎn)。將環(huán)境分為大小相等的小單元，每一個(gè)單元內(nèi)的邊界點(diǎn)分為相同的組，取每組中的邊界點(diǎn)的重心的位置作為候選邊界點(diǎn)，這樣就可大大減少候選邊界點(diǎn)的數(shù)量。

圖1 邊界點(diǎn)及其分組Fig.1 Boundary points and their groups

1.2 預(yù)選邊界點(diǎn)評價(jià)

無人機(jī)觀測到地標(biāo)時(shí)，目標(biāo)函數(shù)的選取與邊界點(diǎn)至地標(biāo)的距離和地標(biāo)自身的不確定性有關(guān)，并且無人機(jī)在邊界點(diǎn)應(yīng)能夠觀測到更多新的區(qū)域。因此，可構(gòu)建如下目標(biāo)函數(shù)：

選擇目標(biāo)評價(jià)函數(shù)J最大的邊界點(diǎn)作為目標(biāo)點(diǎn)，使得無人機(jī)朝該點(diǎn)運(yùn)動一步。

1.3 朝目標(biāo)點(diǎn)運(yùn)動，并使用EKF算法更新系統(tǒng)狀態(tài)

確定目標(biāo)點(diǎn)后，根據(jù)無人機(jī)的運(yùn)動模型和傳感器觀測模型，使用EKF算法更新系統(tǒng)狀態(tài)變量。EKF算法通過一階泰勒級數(shù)展開來近似表示非線性模型，是非線性系統(tǒng)的線性估計(jì)，廣泛應(yīng)用于定位、地圖創(chuàng)建、導(dǎo)航和目標(biāo)跟蹤等領(lǐng)域，被認(rèn)為是解決SLAM問題非常有效的方法，它是一種包含系統(tǒng)狀態(tài)預(yù)測和更新的遞推運(yùn)算[13]。

狀態(tài)預(yù)測：

式（2）～（4）中：u(k)為控制輸入向量；z(k)為傳感器量測向量分別為f(·)相對于狀態(tài)變量X(k)和運(yùn)動噪聲向量v(k)的雅可比矩陣，即有：

狀態(tài)更新：

2 簡化的無人機(jī)運(yùn)動模型

本文的仿真模型采用了簡化的無人機(jī)平面運(yùn)動模型，見圖2。無人機(jī)的狀態(tài)Xv=[x y ψ]T，其中，x、y為無人機(jī)在二維平面中的位置，ψ為方位角；控制量U=[ΔdΔψ]T，其中，Δd為一個(gè)時(shí)間步長對應(yīng)的無人機(jī)運(yùn)動距離，Δψ為姿態(tài)變化量。

圖2 無人機(jī)二維模型Fig.2 2D model of UAV

各狀態(tài)變量的變化方程為：

無人機(jī)狀態(tài)方程為：

設(shè)第i地標(biāo)的狀態(tài)為mi=[mxmy]T，假設(shè)地標(biāo)是靜止的，則有：mi,k=mi,k-1。

無人機(jī)與第i地標(biāo)的測量方程為

3 仿真試驗(yàn)

假定環(huán)境是一個(gè)40 m×40 m的方形區(qū)域，無人機(jī)的初始位置為[0 0 0]T，并朝向x軸正向，傳感器采用CCD攝像機(jī)，探測距離為10 m，探測角度為90°，環(huán)境中均勻分布著20個(gè)地標(biāo)。目標(biāo)函數(shù)式（1）中的權(quán)值取值為：ω1=0.5，ω2=0.5。在Matlab下，分別采用本文提出的基于邊界的主動SLAM算法和隨機(jī)SLAM，應(yīng)用簡化的無人機(jī)模型采用擴(kuò)展卡爾曼濾波算法進(jìn)行仿真，仿真運(yùn)行200步以后，無人機(jī)的運(yùn)動軌跡和地標(biāo)的位置估計(jì)見圖3、4。圖中，實(shí)線為估計(jì)的無人機(jī)的運(yùn)動軌跡，“+”號為地標(biāo)的真實(shí)位置，“＊”號和橢圓分別為估計(jì)的地標(biāo)位置和方差。

圖3 基于邊界的主動SLAM算法的仿真結(jié)果Fig.3 Simulation results of active SLAM based on boundary

圖4 隨機(jī)SLAM的仿真結(jié)果Fig.4 Simulation result of random SLAM

從圖3和圖4中可以看出，隨機(jī)運(yùn)動時(shí)無人機(jī)的探測環(huán)境有限，并且無人機(jī)的運(yùn)動狀態(tài)呈現(xiàn)混亂無規(guī)律的特點(diǎn)，經(jīng)常會陷入某個(gè)區(qū)域而不能離開。而采用基于邊界的主動SLAM航跡規(guī)劃，則能夠較好地克服這種情況，無人機(jī)航跡平滑有序，就觀測地標(biāo)數(shù)而言，也大大多于采用隨機(jī)SLAM的方法。顯然，采用基于邊界的主動SLAM算法時(shí)，無人機(jī)可以探測到更大范圍的環(huán)境，并得到了較高的地標(biāo)估計(jì)值。可見，基于邊界的無人機(jī)主動SLAM明顯優(yōu)于隨機(jī)SLAM。

4 結(jié)論

本文研究了基于邊界的無人機(jī)主動SLAM算法，其實(shí)質(zhì)與基于最優(yōu)控制的方法相同，也是通過構(gòu)造合理的目標(biāo)函數(shù)來確定無人機(jī)的運(yùn)動輸入。基于最優(yōu)控制的方法從無人機(jī)的一步步長到達(dá)的區(qū)域點(diǎn)中選擇最優(yōu)的運(yùn)動目標(biāo)點(diǎn)，而基于邊界的主動SLAM算法則從傳感器探測的邊界點(diǎn)中選擇下一步運(yùn)動目標(biāo)點(diǎn)，相比而言，目標(biāo)點(diǎn)的選擇范圍可更寬，并能在一定程度上避免陷入局部最優(yōu)。本文能通過建立的無人機(jī)運(yùn)動簡化模型，運(yùn)用Matlab對提出的算法與隨機(jī)SLAM進(jìn)行仿真比較，表明了該算法的有效性和可行性。實(shí)際應(yīng)用中，外界的三維環(huán)境非常復(fù)雜，可能還包含許多動態(tài)的目標(biāo)，對此還需進(jìn)一步深入研究。

[1]NEWMAN P M.On the structure and solution of the simultaneous localization and map building problem[D]. Sydney：University of Sydney，1999.

[2]張恒，樊曉平.移動機(jī)器人同步定位與地圖構(gòu)建過程中的軌跡規(guī)劃研究[J].機(jī)器人，2006，28（3）：285-290. ZHANG HENG，F(xiàn)AN XIAOPING.Research on trajectory programming in the process of simultaneous location and mapping for mobile robots[J].Robot，2006，28（3）：285-290.（in Chinese）

[3]張恒，樊曉平，劉艷麗.移動機(jī)器人同步定位與地圖構(gòu)建研究進(jìn)展[J].數(shù)據(jù)采集與處理，2005，20（4）：458-465. ZHANG HENG，F(xiàn)AN XIAOPING，LIU YANLI.Simultaneous localization and mapping for mobile robots[J]. Journal of Data Acquisition&Processing，2005，20（4）：458-465.（in Chinese）

[4]武濤，孫鳳池，苑晶，等.一種基于線段特征的室內(nèi)環(huán)境主動SLAM方法[J].機(jī)器人，2009，31（2）：166-170. WU TAO，SUN FENGCHI，YUAN JING，et al.An active SLAM approach based on line segment feature in indoorenvironment[J].Robot，2009，31（2）：166-170.（in Chinese）

[5]吳曉琳，宋萌，苑晶，等.通訊范圍受限條件下的多機(jī)器人主動SLAM[J].系統(tǒng)工程與電子技術(shù)，2012，34（10）：2121-2128. WU XIAOLIN，SONG MENG，YUAN JING，et al.Multirobot active SLAM under limit range[J].Systems Engineering and Electronics，2012，34（10）：2121-2128.（in Chinese）

[6]陶通，黃亞樓，苑晶，等.基于協(xié)助校正方法的多機(jī)器人主動同時(shí)定位與建圖[J].模式識別與人工智能，2012，25（3）：534-543. TAO TONG，HUANG YALOU，YUAN JING，et al. Multi-robot active simultaneous localization and mapping based on cooperative correction approach[J].Patter Recognition and Artifical Intelligence，2012，25（3）：534-543.（in Chinese）

[7]苑晶，黃亞樓，陶通，等.基于局部子地圖方法的多機(jī)器人主動同時(shí)定位與地圖創(chuàng)建[J].機(jī)器人，2009，31（2）：97-103. YUAN JING，HUANG YALOU，TAO TONG，et al.Multirobot active SLAM based on local sub-map approach[J]. Robot，2009，31（2）：97-103.（in Chinese）

[8]FEDER H，LEONARD J，SMITH C.Adaptive mobile robot navigation and mapping[J].International Journal of Robotics Research，1999，18（7）：650-668.（in Chinese）

[9]Y LIU，F(xiàn) SUN，et al.A solution to active simultaneous localization and mapping problem based on optimal control [C]//Proceedings of IEEE International Conference on Mechatronics and Automation.Harbin：IEEE，2007：314-319.

[10]苑晶.未知環(huán)境中移動機(jī)器人主動同時(shí)定位與地圖構(gòu)建技術(shù)研究[D].天津：南開大學(xué)，2007. YUAN JING.Research on mobile robots active simultaneous location and mapping technologies in unknown environments[D].Tianjin：Nankai University，2007.（in Chinese）

[11]TAO TONG，HUANG YALOU，SUN FENGCHI，et al. Motion planning for SLAM based on frontier exploration [C]//Proceedings of the 2007 IEEE International Conference on Mechatronics and Automation.Harbin：IEEE，2007：2120-2125.

[12]YAMAUCHI B.A frontier-based approach for autonomous exploration[C]//IEEE International Symposium on Computational Intelligence in Robotics and Automation. Monterey，1997：146-151.

[13]WEBB T，PRAZENICA R，KURDIAL A，et al.Visionbased state estimation for autonomous micro air vehicles [J].Journal of Guidance，Control，and Dynamics，2007，30（3）：816-826.

UAV Active SLAM Algorithm Based on Boundary

SHEN Yong-fu1,WANG Xi-bin2
（1.Naval Training Equipment Institute,Beijing 102308,China; 2.Department of Control Engineering,NAAU,Yantai Shandong 264001,China）

The technology of simultaneous localization and mapping(SLAM)is the key for an unmanned aerial vehicle (UAV)to realize truly autonomous navigation.To overcome the disadvantage of the passive SLAM,the active SLAM meth?od based on boundary for UAV was studied.Firstly,the candidate boundary points were produced around the exploration area,and the destination point was selected from those points by building a proper objective function.Then the UAV moved towards this point and its movement state was updated by extended Kalman filtering(EKF)algorithm.Using a sim?plified model of UAV,comparative research was carried out between the proposed algorithm and random SLAM.The simu?lation results with Matlab showed that this algorithm was effective and feasible.

active simultaneous localization and mapping;boundary;extended kalman filtering(EKF);unmanned aerial ve?hicle(UAV)

V24

A

1673-1522（2014）05-0461-04

10.7682/j.issn.1673-1522.2014.05.013

2014-06-09；

2014-07-17

沈永福（1974-），男，工程師，碩士。