沈永福，王希彬

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

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

沈永福1，王希彬2

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

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

主動(dòng)同步定位與地圖構(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)動(dòng)軌跡預(yù)先已知的，這種被動(dòng)形式的SLAM與環(huán)境的未知性相矛盾。如何主動(dòng)地、自適應(yīng)地探索環(huán)境成為主動(dòng)SLAM研究的課題。

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

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

1 基于邊界的主動(dòng)SLAM

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

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

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

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

如圖1所示，無人機(jī)用三角形表示，攝像機(jī)作為機(jī)載傳感器，考慮攝像機(jī)的運(yùn)動(dòng)特性，假設(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)評(píng)價(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)評(píng)價(jià)函數(shù)J最大的邊界點(diǎn)作為目標(biāo)點(diǎn)，使得無人機(jī)朝該點(diǎn)運(yùn)動(dòng)一步。

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

確定目標(biāo)點(diǎn)后，根據(jù)無人機(jī)的運(yùn)動(dòng)模型和傳感器觀測模型，使用EKF算法更新系統(tǒng)狀態(tài)變量。EKF算法通過一階泰勒級(jí)數(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(·)相對(duì)于狀態(tài)變量X(k)和運(yùn)動(dòng)噪聲向量v(k)的雅可比矩陣，即有：

狀態(tài)更新：

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

本文的仿真模型采用了簡化的無人機(jī)平面運(yùn)動(dòng)模型，見圖2。無人機(jī)的狀態(tài)Xv=[x y ψ]T，其中，x、y為無人機(jī)在二維平面中的位置，ψ為方位角；控制量U=[ΔdΔψ]T，其中，Δd為一個(gè)時(shí)間步長對(duì)應(yīng)的無人機(jī)運(yùn)動(dòng)距離，Δψ為姿態(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下，分別采用本文提出的基于邊界的主動(dòng)SLAM算法和隨機(jī)SLAM，應(yīng)用簡化的無人機(jī)模型采用擴(kuò)展卡爾曼濾波算法進(jìn)行仿真，仿真運(yùn)行200步以后，無人機(jī)的運(yùn)動(dòng)軌跡和地標(biāo)的位置估計(jì)見圖3、4。圖中，實(shí)線為估計(jì)的無人機(jī)的運(yùn)動(dòng)軌跡，“+”號(hào)為地標(biāo)的真實(shí)位置，“＊”號(hào)和橢圓分別為估計(jì)的地標(biāo)位置和方差。

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

4 結(jié)論

本文研究了基于邊界的無人機(jī)主動(dòng)SLAM算法，其實(shí)質(zhì)與基于最優(yōu)控制的方法相同，也是通過構(gòu)造合理的目標(biāo)函數(shù)來確定無人機(jī)的運(yùn)動(dòng)輸入?；谧顑?yōu)控制的方法從無人機(jī)的一步步長到達(dá)的區(qū)域點(diǎn)中選擇最優(yōu)的運(yùn)動(dòng)目標(biāo)點(diǎn)，而基于邊界的主動(dòng)SLAM算法則從傳感器探測的邊界點(diǎn)中選擇下一步運(yùn)動(dòng)目標(biāo)點(diǎn)，相比而言，目標(biāo)點(diǎn)的選擇范圍可更寬，并能在一定程度上避免陷入局部最優(yōu)。本文能通過建立的無人機(jī)運(yùn)動(dòng)簡化模型，運(yùn)用Matlab對(duì)提出的算法與隨機(jī)SLAM進(jìn)行仿真比較，表明了該算法的有效性和可行性。實(shí)際應(yīng)用中，外界的三維環(huán)境非常復(fù)雜，可能還包含許多動(dòng)態(tài)的目標(biāo)，對(duì)此還需進(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]張恒，樊曉平.移動(dòng)機(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]張恒，樊曉平，劉艷麗.移動(dòng)機(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)境主動(dòng)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ī)器人主動(dòng)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ī)器人主動(dòng)同時(shí)定位與建圖[J].模式識(shí)別與人工智能，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ī)器人主動(dòng)同時(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)境中移動(dòng)機(jī)器人主動(dòng)同時(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-），男，工程師，碩士。