沈永福，王希彬

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

基于邊界的無人機主動SLAM算法

沈永福1，王希彬2

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

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

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

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

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

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

1 基于邊界的主動SLAM

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

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

1.1 候選邊界點選取

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

如圖1所示，無人機用三角形表示，攝像機作為機載傳感器，考慮攝像機的運動特性，假設(shè)其探索區(qū)域為一扇形，周圍黑色的點表示邊界點。將環(huán)境分為大小相等的小單元，每一個單元內(nèi)的邊界點分為相同的組，取每組中的邊界點的重心的位置作為候選邊界點，這樣就可大大減少候選邊界點的數(shù)量。

圖1 邊界點及其分組Fig.1 Boundary points and their groups

1.2 預選邊界點評價

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

選擇目標評價函數(shù)J最大的邊界點作為目標點，使得無人機朝該點運動一步。

1.3 朝目標點運動，并使用EKF算法更新系統(tǒng)狀態(tài)

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

狀態(tài)預測：

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

狀態(tài)更新：

2 簡化的無人機運動模型

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

圖2 無人機二維模型Fig.2 2D model of UAV

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

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

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

無人機與第i地標的測量方程為

3 仿真試驗

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

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

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

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

4 結(jié)論

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

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

[2]張恒，樊曉平.移動機器人同步定位與地圖構(gòu)建過程中的軌跡規(guī)劃研究[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]張恒，樊曉平，劉艷麗.移動機器人同步定位與地圖構(gòu)建研究進展[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].機器人，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]吳曉琳，宋萌，苑晶，等.通訊范圍受限條件下的多機器人主動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].模式識別與人工智能，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]苑晶，黃亞樓，陶通，等.基于局部子地圖方法的多機器人主動同時定位與地圖創(chuàng)建[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)境中移動機器人主動同時定位與地圖構(gòu)建技術(shù)研究[D].天津：南開大學，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-），男，工程師，碩士。