未知環境下UCAV SLAM研究*

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

(空軍工程大學 信息與導航學院，陜西 西安 710077)

0 引言

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

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

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

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

1 航空SLAM模型分析

航空SLAM模型[4]主要包括狀態模型和觀測模型，其中狀態模型包含了運行體的運動狀態及環境特征狀態；觀測模型表示了觀測與狀態的關系，是運行體實現位姿校正基礎。

1.1 狀態模型

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

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

(1)

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

(2)

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

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

(3)

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

(4)

(5)

經過以上內容的分析，可建立航空SLAM的狀態模型。

1.2 觀測模型

觀測模型用于在已知k時刻運行體位姿Xv(k)與觀測路標狀態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)

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

經過以上內容的分析，可建立航空SLAM的觀測模型。

2 UCAV SLAM涉及的濾波技術

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

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

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

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

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

3 UCAV SLAM需解決的問題

UCAV的運行特點及作戰背景，使其在未知環境下SLAM較其他運行體復雜，具體實現上有許多關鍵問題有待解決，具體如下：

(1) 傳感器的選擇

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

(2) 數據關聯問題

數據關聯是指UCAV找出當前時刻觀測到的路標與路標數據庫中路標的對應關系。數據關聯的根本是存在路標數據庫，在具體構建上需解決如何選擇環境中特征作為路標、如何描述路標和如何將路標歸類等問題。以得到的數據庫為基礎，在具體關聯上，UCAV需解決如何快速識別環境特征及數據高效檢索等問題。

(3) 算法高效性問題

UCAV未知環境下自主導航必然面臨復雜環境。要實現UCAV高精度的定位和地圖構建，需不間斷感知環境信息，用于更新或添加路標，這導致估計狀態維數的增加，最終增加了模型解算的時間，為此需尋求一種高效的濾波方法，以適應于UCAV SLAM實時性要求。

(4) 地圖構建問題

地圖的構建是UCAV實現SLAM的重要環節，其與UCAV定位相輔相成。在具體構建上需考慮選擇的地圖如何便于創建和維護、如何實現環境的唯一表征、如何便于識別并利于UCAV完成自身位姿確定和路徑規劃等問題。

4 結束語

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

參考文獻：

[1] 吳德偉,邰能建,戚君宜. 基于認知理論的UCAV智能導航研究新進展[J]. 空軍工程大學學報:自然科學版, 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]. 航空學報, 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].東南大學學報：自然科學版,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環境感知與地形建模方法研究[D].哈爾濱：哈爾濱工業大學,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].哈爾濱：哈爾濱工業大學,2011.

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

[12] 祝繼華,鄭南寧,袁澤劍,等.基于中心差分粒子濾波的SLAM算法[J].自動化學報,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] 朱磊,樊繼壯,趙杰,等. 未知環境下的移動機器人SLAM方法[J]. 華中科技大學學報：自然科學版,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.