周陽，吳德偉，邰能建，杜佳

(空軍工程大學(xué) 信息與導(dǎo)航學(xué)院，陜西 西安 710077)

0 引言

隨著戰(zhàn)爭環(huán)境的日益嚴酷，UCAV(unmanned combat aerial vehicle)在戰(zhàn)場上的運用得到廣泛關(guān)注，加速了各國對其關(guān)鍵技術(shù)的研究，導(dǎo)航系統(tǒng)的自主性和智能性便是其中核心內(nèi)容。UCAV認知導(dǎo)航系統(tǒng)[1-2]可通過多種傳感方式感知外界環(huán)境，實時配置導(dǎo)航方式，模擬人類海馬機理實現(xiàn)具有知識記憶、學(xué)習(xí)及推理特性的導(dǎo)航信息處理，實現(xiàn)UCAV在最優(yōu)航跡上的高精度智能自主導(dǎo)航。其中未知環(huán)境下的自主導(dǎo)航能力決定了認知導(dǎo)航系統(tǒng)對環(huán)境的適應(yīng)性，有待研究。

Durrent-Whyte[3]首次將SLAM(simultaneous localization and mapping)作為專有名詞使用，其基本思想是將運行體定位與環(huán)境地圖創(chuàng)建融為一體，預(yù)示著運行體在沒有任何先驗信息的情況下可進行導(dǎo)航。這一經(jīng)出現(xiàn)就成為運行體自主導(dǎo)航領(lǐng)域研究熱點，同時為未知環(huán)境下UCAV自主導(dǎo)航的實現(xiàn)提供了思路。

UCAV在未知環(huán)境下的SLAM可描述為：UCAV利用機載傳感器估計自身位姿和環(huán)境特征狀態(tài)，以建立的狀態(tài)模型和觀測模型為基礎(chǔ)，通過實時獲取的觀測數(shù)據(jù)，采用具體的濾波技術(shù)得到較準確UCAV位姿和環(huán)境特征狀態(tài)。在具體實現(xiàn)中通過自身攜帶的外部傳感器識別環(huán)境中的特征，即“路標”，然后從傳感信息中計算出路標與UCAV相對位置，再依據(jù)坐標系間的轉(zhuǎn)換關(guān)系定位路標的全局位置，以此創(chuàng)建環(huán)境地圖，并在隨后的運動過程中識別和匹配這些路標，通過有效濾波技術(shù)同步更新UCAV位姿和地圖信息。

目前國外對航空SLAM的研究已取得初步成果。Jonghyuk Kim[4-5]等人研究了無人機(unmanned aerial vehicle，UAV)SLAM，分析了UAV SLAM中慣性器件位姿推算的機理，給出了路標狀態(tài)在不同坐標系下轉(zhuǎn)換關(guān)系，并指出UAV SLAM的誤差主要源于外部傳感器；David T? rnqvist[6]等人將粒子濾波(particle filter，PF)引入到航空SLAM中，實現(xiàn)了UAV高維運動模型下的自主導(dǎo)航；S.Sukkarieh等[7]人將視覺傳感器與慣性測量單元相結(jié)合應(yīng)用于航空SLAM中，并證明了該方法可有效修正慣導(dǎo)漂移；Jorge Artieda[8]等人以提取的SURF(speeded up roubust features)特征點作為路標構(gòu)建觀測模型，實現(xiàn)了基于三維視覺的UAV SLAM。目前國內(nèi)主要對移動機器人[9]及水下運行體[10]的SLAM進行了研究，且主要針對不同傳感器下實現(xiàn)原理[11]及濾波實現(xiàn)上[12]，對航空SLAM的研究較少。作為航空運行體的UCAV，其自主性依賴于具體導(dǎo)航方式，為此本文對UCAV SLAM進行了研究。

1 航空SLAM模型分析

航空SLAM模型[4]主要包括狀態(tài)模型和觀測模型，其中狀態(tài)模型包含了運行體的運動狀態(tài)及環(huán)境特征狀態(tài)；觀測模型表示了觀測與狀態(tài)的關(guān)系，是運行體實現(xiàn)位姿校正基礎(chǔ)。

1.1 狀態(tài)模型

狀態(tài)模型p(X(k)X(k-1),u(k))用于描述一個輸入控制命令u(k)下，前一時刻狀態(tài)X(k-1)與當前狀態(tài)X(k)之間的關(guān)系。狀態(tài)模型離散時間的差分方程為

X(k)=F(X(k-1),u(k),k)+Wv(k),

(1)

式中：F(·)為非線性狀態(tài)變換函數(shù)；Wv(k)為均值為零方差為Q(k)過程噪聲矢量；X(k)由運行體位姿狀態(tài)Xv(k)和路標狀態(tài)Xm(k)構(gòu)成，具體表示為

(2)

式中：Xv(k)=x,y,z,ψ,θ,γ為運行體在導(dǎo)航坐標系下位置(x,y,z)和姿態(tài)(ψ,θ,γ)構(gòu)成的狀態(tài)矢量；N為k時刻觀測到的路標個數(shù)；Xmi(k)為k時刻第i個路標在導(dǎo)航坐標系中位置，且當路標位置固定時路標狀態(tài)存在以下關(guān)系

Xmi(k)=Xmi(k-1).

(3)

Xv(k)中6自由度x，y，z，ψ，θ，γ可由慣性測量器件解算，具體計算式為[4]

(4)

(5)

經(jīng)過以上內(nèi)容的分析，可建立航空SLAM的狀態(tài)模型。

1.2 觀測模型

觀測模型用于在已知k時刻運行體位姿Xv(k)與觀測路標狀態(tài)Xm(k)條件下求k時刻觀測值Zk，其離散時間的差分方程形式為

Z(k) =H(X(k))+V(k)=

H(Xv(k),Xm(k))+V(k)，

(6)

式中：H(·)為非線性觀測模型；V(k)為均值0方差為R(k)的觀測噪聲。

圖1 路標在不同坐標系下位置示意圖Fig.1 Sketch map of landmark’s location in different coordinate systems

(7)

當坐標系確定后，路標位置的表示形式隨選取的狀態(tài)變化，在具體分析中可以極坐標或笛卡爾坐標形式給出。

經(jīng)過以上內(nèi)容的分析，可建立航空SLAM的觀測模型。

2 UCAV SLAM涉及的濾波技術(shù)

以得到的航空SLAM模型為基礎(chǔ)，UCAV SLAM實現(xiàn)必須以具體濾波技術(shù)為支撐。濾波技術(shù)主要用于對系統(tǒng)狀態(tài)進行遞歸估計，并以測量誤差為依據(jù)對狀態(tài)進行校正，使系統(tǒng)狀態(tài)逼近真實值。目前較常用的有：卡爾曼濾波[5](Kalman filter，KF)、擴展卡爾曼濾波(extended Kalman filter，EKF)[13]、粒子濾波[6]等，針對不同應(yīng)用條件及背景，學(xué)者們也對上述濾波技術(shù)進行了改進[14]。

基于KF的SLAM模型是通過不斷進行預(yù)測和修正的計算過程實現(xiàn)，適用于高斯噪聲假設(shè)下線性SLAM模型。其基本思想是：將UCAV的位姿和環(huán)境特征組成一個狀態(tài)向量，并認為被估計狀態(tài)的后驗概率在任何時刻都服從高斯分布，采用KF作最小均方誤差估計。具體實施上主要通過處理一系列帶有誤差UCAV傳感器數(shù)據(jù)得到狀態(tài)的最佳估計。

基于EKF的SLAM不限于線性系統(tǒng)及高斯噪聲影響的假設(shè)，其基本思想是：將UCAV SLAM狀態(tài)方程和觀測方程泰勒展開，舍去高階項，實現(xiàn)模型線性化，以KF原理估計UCAV位姿和環(huán)境特征狀態(tài)。該方法在運算中需求解非線性狀態(tài)函數(shù)及非線性觀測模型相對具體變量雅可比矩陣，且在模型建立中不可避免引入了線性化誤差，從根本上影響了SLAM的濾波收斂情況和狀態(tài)的估計效果，同時還面臨著隨路標數(shù)量增加出現(xiàn)高計算量和維數(shù)災(zāi)難的問題。

基于PF的SLAM是非線性條件下實現(xiàn)形式，適用于任何能用狀態(tài)空間模型表示的非線性系統(tǒng)及傳統(tǒng)卡爾曼濾波無法表示的非線性系統(tǒng)，較貝葉斯估計在線性條件下實現(xiàn)形式更具有環(huán)境適應(yīng)性[11]。該方法通過非參數(shù)化的蒙特卡羅模擬方法(Monte Carlo method)實現(xiàn)遞推貝葉斯濾波。其基本思想是：選取一個重要性概率密度來得到帶有相關(guān)權(quán)值的隨機樣本以符合UCVA位姿狀態(tài)和環(huán)境特征狀態(tài)的后驗概率分布，然后在UCAV運行過程中，以傳感器數(shù)據(jù)為基礎(chǔ)調(diào)整權(quán)值的大小和粒子的位置。該方法在實現(xiàn)過程中主要通過“狀態(tài)預(yù)測—權(quán)值更新—重采樣”的步驟完成，最終得到較準確的UCAV位姿和環(huán)境特征估計值。

針對計算復(fù)雜度和高維數(shù)據(jù)處理問題，Montmerlo等人[15]將Rao-Blackwellise粒子濾波器引入SLAM，利用粒子簇表征非線性、非高斯系統(tǒng)的狀態(tài)分布，提出了FastSLAM。FastSLAM認為當運行體狀態(tài)軌跡已知且各路標之間相互獨立時，可將狀態(tài)空間分離為獨立的部分，并邊緣化該部分的一個或者多個組成成分。其具體實現(xiàn)上將EKF和PF結(jié)合，用單一粒子濾波器實現(xiàn)運行體軌跡濾波，對軌跡中路標通過EKF估計。該方法可有效解決粒子濾波器地圖創(chuàng)建困難問題，計算效率和可擴展性也優(yōu)于全狀態(tài)的EKF-SLAM[11]。

3 UCAV SLAM需解決的問題

UCAV的運行特點及作戰(zhàn)背景，使其在未知環(huán)境下SLAM較其他運行體復(fù)雜，具體實現(xiàn)上有許多關(guān)鍵問題有待解決，具體如下：

(1) 傳感器的選擇

傳感器分為內(nèi)部傳感器和外部傳感器。內(nèi)部傳感器是實現(xiàn)UCAV位姿推算的基礎(chǔ)，其精度的高低決定了位姿估計的漂移量，進一步影響了UCAV定位及地圖創(chuàng)建的準確性。內(nèi)部傳感器通常由慣性測量器件承擔(dān)，鑒于UCAV的高機動性，必然要求所選的慣性測量器件在滿足高精度的同時保證高動態(tài)環(huán)境下的穩(wěn)定性；外部傳感器用于識別解算環(huán)境路標狀態(tài)，在具體選擇上需考傳感器如何適于高動態(tài)運行環(huán)境、如何實現(xiàn)路標準確識別及如何與UCAV配套等問題。

(2) 數(shù)據(jù)關(guān)聯(lián)問題

數(shù)據(jù)關(guān)聯(lián)是指UCAV找出當前時刻觀測到的路標與路標數(shù)據(jù)庫中路標的對應(yīng)關(guān)系。數(shù)據(jù)關(guān)聯(lián)的根本是存在路標數(shù)據(jù)庫，在具體構(gòu)建上需解決如何選擇環(huán)境中特征作為路標、如何描述路標和如何將路標歸類等問題。以得到的數(shù)據(jù)庫為基礎(chǔ)，在具體關(guān)聯(lián)上，UCAV需解決如何快速識別環(huán)境特征及數(shù)據(jù)高效檢索等問題。

(3) 算法高效性問題

UCAV未知環(huán)境下自主導(dǎo)航必然面臨復(fù)雜環(huán)境。要實現(xiàn)UCAV高精度的定位和地圖構(gòu)建，需不間斷感知環(huán)境信息，用于更新或添加路標，這導(dǎo)致估計狀態(tài)維數(shù)的增加，最終增加了模型解算的時間，為此需尋求一種高效的濾波方法，以適應(yīng)于UCAV SLAM實時性要求。

(4) 地圖構(gòu)建問題

地圖的構(gòu)建是UCAV實現(xiàn)SLAM的重要環(huán)節(jié)，其與UCAV定位相輔相成。在具體構(gòu)建上需考慮選擇的地圖如何便于創(chuàng)建和維護、如何實現(xiàn)環(huán)境的唯一表征、如何便于識別并利于UCAV完成自身位姿確定和路徑規(guī)劃等問題。

4 結(jié)束語

本文以航空SLAM模型的建立、UCAV SLAM涉及的濾波技術(shù)和待解決的問題為重點，對UCAV SLAM進行了研究，為認知導(dǎo)航系統(tǒng)用于UCAV未知環(huán)境下自主導(dǎo)航開闊思路。

參考文獻：

[1] 吳德偉,邰能建,戚君宜. 基于認知理論的UCAV智能導(dǎo)航研究新進展[J]. 空軍工程大學(xué)學(xué)報:自然科學(xué)版, 2011,12(4): 52-57.

WU De-wei, TAI Neng-jian, QI Jun-yi. A New Research Progress of UCAV Intelligent Navigation Based on Cognitive Theory [J]. Journal of Air Force Engineering University: Natural Science Edition, 2011, 12 (4):52-57.

[2] 邰能建,吳德偉,戚君宜.基于改進SIFT的高魯棒性特征點提取方法[J]. 航空學(xué)報, 2012, 33(12): 2313-2321.

TAI Neng-jian, WU De-wei, QI Jun-yi. A Method to Extract High Robust Keypoints Based on Improved SIFT[J]. Acta Aeronautica et Astronautica Sinica, 2012, 33(12): 2313-2321.

[3] DURRANT-WHYTE H,RYE D,NEBOT E.Localisation of Automatic Guided Vehicles[J].International Symposium Robotics Research，1996(7):613-625.

[4] KIM J H, SUKKARICH S. Airborne Simultaneous Localization and Map Building [C]∥Proceedings of IEEE International Conference on Robotics and Automation,2003: 406-411.

[5] Jonghyuk Kim, Salah Sukkarieh. Real-Time Implementation of Airborne Inertial-SLAM [J]. Robotics and Autonomous Systems，2007,55: 62-71.

[6] David T?rnqvist,Thomas B Sch?n,Rickard Karlsson,et al. Particle Filter SLAM with High Dimensional Vehicle Model[J].J Intell Robot Syst,2009,55:249-266.

[7] SUKKARIEH S,DURRANT-WHYTE H.Towards the Development of Simultaneous Localization and Map Building Algorthms[C]∥Proc.Int.Conference of Field and Service Robotics,2001:193-200.

[8] Jorge Artieda,Jose M Sebastian,Pascual Campoy,et al.Visual 3-D SLAM from UAVs[J].J Intell Robot Syst ,2009,55:299-321.

[9] 涂剛毅,金世俊,祝雪芬,等. 基于粒子濾波的移動機器人SLAM算法[J].東南大學(xué)學(xué)報：自然科學(xué)版,2010,40(1):117-122.

TU Gang-yi,JIN Shi-jun,ZHU Xue-fen,et al. Particle Filter SLAM Method for Mobile Robot[J].Journal of Sou Theast University：Natural Science Edition, 2010,40(1):117-122.

[10] 曲鏡圓.基于聲納的AUV環(huán)境感知與地形建模方法研究[D].哈爾濱：哈爾濱工業(yè)大學(xué),2009.

QU Jing-yuan.Rearch on Environment Perception and Terrian Modeling for AUV Based on Sonar[D].Harbin:Harbin Engineering University,2009.

[11] 林睿.基于圖像特征點的移動機器人立體視覺SLAM研究[D].哈爾濱：哈爾濱工業(yè)大學(xué),2011.

LIN Rui. Research on Image Feature-Based Mobile Robot Stereo Visual SLAM[D]. Harbin:Harbin Engineering University,2011.

[12] 祝繼華,鄭南寧,袁澤劍,等.基于中心差分粒子濾波的SLAM算法[J].自動化學(xué)報,2010,36(2):249-257.

ZHU Ji-hua,ZHENG Nan-ning,YUAN Ze-jian,et al. A SLAM Algorithm Based on Central Difference Particle Filter[J].Acta Automatica Sinica, 2010,36(2):249-257.

[13] Zahra Mohajerani. Vision-Based UAV Pose Estimation[D].America: Northeastern University,2008.

[14] 朱磊,樊繼壯,趙杰,等. 未知環(huán)境下的移動機器人SLAM方法[J]. 華中科技大學(xué)學(xué)報：自然科學(xué)版,2011,39(7):9-13.

ZHU Lei,FAN Ji-zhuang,ZHAO Jie,et al.SLAM Mehtod for Mobile Robot in Unknow Environment[J].J.Huazhong Univ.of & Tech.：Natural Science Edition, 2011,39(7):9-13.

[15] MONTEMERLO M, THRUN S, KOLLER D, et al. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem [C]∥ Proc.of the National Conference on Artifical Intelligence, AAAI, Edmonton, Canada, 2002: 593-598.