雙目立體視覺的無人機位姿估計算法及驗證

張 梁，徐錦法，夏青元

(南京航空航天大學直升機旋翼動力學國家級重點實驗室，210016南京)

導航在無人機自主飛行中具有關鍵作用，大多采用慣導、GPS、天文、多普勒等方式實現，也有采用激光、紅外與超聲波等測距方式實現.慣性導航與GPS組合是目前應用最為廣泛的導航方式.盡管如此，由于GPS借助衛星定位，受地形干擾及人為控制，天文及多普勒導航方式亦如此.激光、紅外等測距設備雖然測量精度高，但由于自重較大，也不宜應用于微小型無人機.隨著計算機技術發展，視覺技術發展迅速，近年來各國均已開展深入研究，因其導航方式不受地形天氣等因素干擾而得到廣泛重視［1-3］.

無人機視覺導航攝像頭可用下視方式或前視方式采集圖像信息.下視安裝方式已在無人機視覺導引著陸中得到成功應用，文獻［4］就是用下視方式安裝攝像頭，應用單目視覺攝像機實現了微型四旋翼飛行器的自主著陸導引.前視方式也有成功應用實例，通過安裝前視攝像頭使微小型無人機在城市環境甚至室內環境中具有自主飛行能力，發揮交通監控、目標跟蹤、火災監測等重要作用.文獻［5］是前視方式安裝攝像頭的應用案例，應用立體視覺和光流算法實現了無人直升機的自主飛行控制系統，并進行了地面試驗驗證.

單目視覺、雙目視覺或多目視覺可以估計飛行器的位姿信息，由于單目視覺無法得到目標物體的深度信息，就難以得到全部位姿信息，而雙目視覺可以得到攝像機坐標系下的目標物體三維坐標，通過位姿估計能快速導引無人機飛行.文獻［6-8］采用雙目立體視覺技術完成了視覺里程計設計，并與GPS等其他導航技術融合實現了無人運載車輛地面運動控制.

近年來，國內在無人機視覺導航方面也開展了深入研究.文獻［9］提出了基于特定標志的無人機視覺導引著艦方案，文獻［10］提出了在機場跑道安裝紅外導引標志的視覺導航方法.盡管研究進展卓有成效，但實際應用中具有一定的局限性，因為這兩種方案均需特定導引標志，只局限于導引著陸.文獻［11］提出了立體視覺與慣性導航相融合的組合導航方法，但側重于組合導航方式探討，還未進行實際應用研究.

本文運用視覺里程計概念，提出了基于雙目視覺的無人機運動位置和姿態的計算方法.雙目立體攝像機能得到兩組立體圖像序列，通過提取相匹配的特征點，經運算得到同一時刻攝像機坐標系下的特征點坐標值，再利用前后兩時刻相匹配的特征點坐標值得到無人機運動位置和姿態的估計值，進而實現無人機運動控制.

1 攝像機模型及坐標系關系

攝像機模型決定了空間物體某點與圖像平面一點的對應關系，這一幾何模型的參數稱為攝像機參數，可通過實驗和計算得到，即標定［12］過程.標定的目的是得到攝像機參數，包括內參與外參，內參只與攝像機內部結構有關，外參由攝像機與世界坐標系關系決定，具體參數可見式(1).攝像機模型不僅可用于校正攝像機畸變，還可用于攝像機運動軌跡估算.現以針孔攝像機模型為例進行說明.

攝像機模型涉及的坐標系如圖1所示，攝像機坐標系為Xc-Yc-Zc，圖像坐標系為u-v，Xc軸與u軸平行，Yc軸與v軸平行，Zc軸與光心重合，便于圖像坐標與攝像機模型坐標轉換.

圖1 各坐標系間關系

根據透視投影關系，基本針孔攝像機模型的空間任意一點P與圖像平面對應關系可表示為

式中:zc是攝像機坐標系下Z軸坐標;u、v是圖像坐標系下像素坐標值;dx、dy是圖像坐標系下每一個像素在x軸和y軸方向的物理尺寸，單位為mm;u0、v0是攝像機光軸與圖像平面的交點坐標值;f是攝像機焦距;R、t分別是世界坐標系相對于攝像機坐標系的旋轉矩陣和平移向量;xw、yw、zw分別是該特征點在世界坐標系下坐標值;αx= f/dx、αy=f/dy;M稱為投影矩陣，而A和［R｜t］分別稱為攝像機的內參和外參，上標 ～表示齊次向量.

本文通過雙目相機采集圖像，定義左相機坐標系為基準參考坐標系.相機經過標定，所采集圖像均經過了畸變校正以及雙目立體校正［13］.

2 特征點提取與匹配

應用較廣泛的特征點提取算法有Harris算法、SUSAN算法、SIFT算法及其各種衍生算法. Harris算法和SUSAN算法基于灰度匹配，而SIFT算法基于特征匹配.SUSAN算法簡單、實時性好，但是精度相對差;SIFT算法基于圖像特征實現匹配，精度相對高，但是計算較復雜，實時性差; Harris算法居于兩種算法之間，兼顧了實時性與精度要求，因此本文采用Harris算法［14-15］作為特征點提取算法.

本文所采集圖像分辨率大小為640×480，結合Matlab的CornerDetector工具箱實現特征點提取，相比Harris算法計算流程，設置相對簡單.角點提取過程中閾值設置為0.000 02，采用13×13窗口進行局部最大值搜索操作，保留具有最大特征值的特征點，使得特征點分布相對分散，提高特征點匹配準確度，由此每幅圖像可得到大約430個特征點.

圖像特征點提取后，左右視圖特征點如何匹配是進行攝像機三維重建的關鍵，前后幀圖像特征點匹配是實現位姿估計的前提.

運動估計需要前后時刻左右相機共4幅圖像進行匹配，以此作為一個運動估計周期內的特征點集.因此，首先進行當前時刻左右圖像特征點匹配，以左圖像上的特征點為基準，尋找右圖像上對應的特征點，從而得到匹配點集.由于左右圖像經過立體校正，實現了行對準，左右視圖匹配過程中對應特征點搜索范圍可限制在橫軸上，減小搜索時間.之前時刻左右圖像的特征點匹配方法相同，從而得到之前時刻左右相機匹配點集.得到這兩個匹配點集之后，在這兩個匹配點集中尋找前后時刻左相機中相匹配特征點，同樣以當前時刻左圖像上的特征點集為基準，尋找之前時刻左圖像上的匹配特征點，從而得到匹配點集，相當于求得了當前時候與之前時刻兩匹配點集的交集.由此可得到一個運動估計周期內4副圖像的初始匹配特征點集.

圖像中特征點匹配采用歸一化互相關［16］(NCC)實現，基于特征點周圍灰度信息，計算特征點灰度相關系數，相關系數值越大，匹配度越高.對灰度值及其方差的平均化處理能消除光照變化的敏感性.選定待匹配圖像中特征點，以特征點為中心建立(n+1，n+1)窗口W，則兩特征點歸一化互相關系數可表示為

式中:I1、I2分別表示兩窗口內圖像灰度值分別表示窗口中像素灰度平均值.

運用 Matlab/matchfeatures工具箱能快速實現NCC特征點匹配計算.NCC矩形窗大小選擇25×25.工具箱中閾值設定越低精度越高，但是匹配點相對較少，因此通過實驗設定閾值為13.經過4幅圖像的匹配之后，最終得到約100個匹配特征點集，作為位姿估計的初始匹配點集.

3 特征點三維重建

立體視覺方法獲取二維圖像，能實現三維幾何形狀物體重建［17］，即由得到的攝像機坐標系下物體特征點三維坐標值可實現攝像機位置和姿態估計.如圖2所示，假定兩攝像機內參相同，且兩坐標系僅在x軸相差一段距離B，即基線距離，公共視場空間點P(xc，yc，zc)在左右攝像機坐標系下坐標分別為P(xc，yc，zc)和P(xc-B，yc，zc)，對應特征點在兩圖像平面內的坐標為pL(uL，vL)、pR(uR，vR)，由于圖像經過立體校正，故有vL=vR.

圖2 雙目視覺圖像重建示意

由圖2可知，根據相似三角形原理以及攝像機投影方程公式，可得如下關系:

根據攝像機投影方程

可得到空間點P在左相機坐標系的三維坐標為

據此，由左、右像平面的pL和pR坐標可計算得到空間點P在攝像機坐標系內三維坐標值，實現特征點三維重建.

4 運動位姿估計

單目攝像機能用前后幀圖像計算姿態變化，但由于缺少深度尺度因子，不能僅憑攝像機計算得到位置變化，需要其他傳感器輔助，而雙目攝像機能得到特征點深度信息，就可憑雙目攝像機得出位置和姿態變化，優勢明顯.雙目視覺圖像經圖像處理、特征點提取及匹配后可得到一組特征點集，利用特征點集就能計算出坐標變換旋轉矩陣和平移向量.

如圖3所示，以左攝像機為基準，攝像機坐標系下前后時刻對應特征點關系為

圖3 立體視覺位姿估計示意

式中:Pc表示當前幀中任意特征點的攝像機坐標系三維坐標值;Pp表示前一幀中與當前幀對應特征點的攝像機坐標系三維坐標值;上標 ～表示齊次向量;R、t分別表示攝像機坐標系旋轉和平移量.

由式(4)可知，定義如下性能代價函數:

運用最小二乘法原理可求解得到旋轉和平移量.旋轉矩陣R是單位正交矩陣，需滿足、i=1，2，3、j=1，2，3且i≠j.由此可知，上述代價函數并非簡單線性方程，不能使用線性最小二乘通用方法求解.

由于代價函數的非線性特性，為求解帶約束代價函數，通常應用拉格朗日乘子法將代價函數與約束條件相結合［17］，通過3個或4個特征點，利用奇異值分解法求解旋轉與平移量.本文采用L-M迭代算法［12］進行迭代求解.旋轉矩陣R可用歐拉角表示，如式(6)所示，代價函數中變量轉換為歐拉角.

式中s表示sin，c表示cos，rx、ry、rz分別表示歐拉角.

L-M算法是一種迭代求解法，能有效求解非線性最小二乘問題，利用該方法可以選擇相對精確的估計初值，有效降低迭代次數.通過調用L-M算法，可以省去矩陣奇異值分解等操作，即使R、t初值取值I、0也可快速實現收斂.實驗發現，迭代次數不需超過10次即可得到較為精確的初步結果.

隨機 抽 樣 一 致 算 法 (random sample consensus，RANSAC)［12］能有效實現野值和內點集的區分，具體步驟為:1)隨機選擇最小特征點集;2)確定模型估計參數;3)計算滿足設定閾值ε的特征點數量;4)重復1)至3)步驟N遍，選取滿足閾值的具有最多特征點的那次估計作為最大內點集.應用RANSAC算法，結合代價函數計算，得到具有最大內點集的旋轉與平移量，以此作為位姿估計的迭代初始值，再用L-M迭代算法得到相對精確的旋轉與平移量.

確定一個平面至少需要3個特征點，應用RANSAC算法隨機選取3個特征點，收斂精度選為ε=10-6，用L-M迭代算法計算代價函數，設定RANSAC距離閾值為2個像素，超過距離閾值即被認為外點，反之則為內點.用RANSAC算法重復計算20次，得到具有最大內點集的R、t.再利用最大內點集進行迭代計算，最大內點集的R、t作為迭代初值，繼續進行L-M迭代計算，收斂精度為ε=10-8，最終得到R、t.

利用前后幀圖像僅能得到前后時刻的相機相對位置，無法得到相對于世界坐標系的相機位置.要想得到相對于世界坐標系的相對位置，需建立攝像機坐標系與世界坐標系之間的對應關系.設定攝像機起始時刻的位置，找出攝像機在任意時刻與起始時刻的對應關系，即可得到任意時刻攝像機在世界坐標系中的位置.

由式(4)可得出某一特征點在任意k時刻可表示為

根據攝像機標定有

由此可導出~P1=［R0｜t0］~P1w=T1~P1w，其中~Pcam為特征點在攝像機坐標系下坐標值，R0、t0由標定得到.若世界坐標系與攝像機坐標系的起始幀位置一致，則取R0=I、t0=0代入式(7)得

式中TT-k即表示任意k時刻攝像機與世界坐標系之間的旋轉與平移量.

TT-k中的t向量為世界坐標系下三軸位移量(tx，ty，tz)，三軸線速度由k時刻與k-1時刻位移量差值得到

式中ΔT表示兩幀圖像之間采樣間隔.

飛行器姿態角定義為機體坐標系與世界坐標系間相對角度，對TT-k取逆即為旋轉矩陣R的歐拉角，即

式中θ為俯仰角，ψ為偏航角，φ為滾轉角.由旋轉矩陣對應元素可得出姿態角，即

5 實驗與結果

為驗證前述視覺圖像處理算法和位姿估計算法的正確性，應用PointGray公司Bumblebee2立體視覺攝像機設備進行了位姿估計驗證實驗. Bumblebee2采用3.8 mm焦距Sony鏡頭，具有65°水平視場(HFOV)，采用CCD感光元件，曝光形式為整幀曝光，最大分辨率下具有20FPS幀率. Bumblebee2設備精度高，且標定參數已知，如表1所示.依據前述圖像處理算法和位姿估計算法，用C語言和m語言在Matlab環境下編程實現.本文在實驗室環境進行了一系列圖像采集和處理，把相應算法和軟件應用于微小型無人機以驗證算法的正確性和可行性.

表1 Bumblebee2圖像標定參數(640×480)

位姿估計算法流程如圖4所示，以離線方式完成圖像序列位置和姿態估計.

由于算法處于驗證階段，實驗環境為工作室室內環境，光照條件固定，易于實現算法驗證.實驗采用固定相機方法，將相機固定于三腳架之上，保持相機靜止，連續采集80幀立體圖像，幀間隔為0.2 s，所采集圖像像素大小為640×480，圖像均經過畸變校正與立體校正，圖5所示為立體圖像的特征點檢測與匹配樣例.

圖4 位姿估計計算流程

圖5 立體圖像特征點檢測與匹配

位姿估計采用前述迭代算法計算得到，且累積誤差能控制在較小的范圍內，如圖6所示為實驗采集到圖像的靜態偏移測試結果，圖7為實驗得到的姿態估計結果.圖中所標示的設定值是用AP800電子螺距尺測量得到的姿態角.實驗時相機固定于三腳架上，手動移動云臺，同時記錄螺距尺上顯示數據.圖像數據幀之間的間隔時間為0.2 s，共采樣80幀立體圖像.由圖可知，位姿估計值與設定值偏差較小，角度誤差小于3°，偏航角與俯仰角僅在設定值變化階段有較大誤差，這與視場變化時特征點匹配計算誤差有關，攝像機自身變化過快，影響相鄰圖像幀匹配特征點的精確尋找.滾轉角誤差相對大些.

圖8為位置估計結果，實驗時手持相機沿工作室小隔間隔板運動，采集圖像后進行運動軌跡估計.圖像數據幀之間的間隔時間為0.2 s，共采樣170幀立體圖像.工作室隔間長1.2 m，寬0.8 m，估計得到的運動軌跡與此尺寸相符，圖中所示軌跡與X軸和Z軸均存在夾角是因為起始位置與隔間隔板存在夾角.

圖6 靜態偏移測試

圖7 姿態角響應過程

圖8 平移運動軌跡

6 結 語

基于立體視覺估計無人飛行器的位置和姿態是無人飛行器導航系統的關鍵環節，以雙目攝像機為實驗設備，應用視覺里程計原理能實現雙目攝像機的標定、立體圖像校正、特征點提取與匹配、攝像機位置和姿態估計，為無人飛行器的導航提供了一條新的技術途徑.

雙目視覺圖像可得到物體立體信息，利用圖像處理得到的特征點集可估計出無人飛行器的位置和姿態，可使飛行器不依賴于特定的導引標志實現未知復雜環境下的自主導航.實驗結果表明位姿估計誤差較小，能滿足導航需求.

［1］JOHNSON E N，CALISE A J，WATANABE Y，et al. Real-time vision-based relative aircraft navigation［J］. Journal ofAerospace Computing，Information，and Communication，2007，4(4):707-738.

［2］SATTIGERI R J，CALISE A J，KIN B S，et al.6-DOF nonlinear simulation of vision-based formation flight［C］//AIAA Guidance， Navigation， and Control Conference and Exhibit.Reston VA:AIAA，2005，DOI: 10.251416.2005-6002.

［3］DE WAGTER C，BIJNENS B，MULDER J A.Visiononly control of a flapping mav on mars［C］//AIAA Guidance，Navigation and ControlConference and Exhibit.Reston VA:AIAA，2007:AIAA2007-6853.

［4］KENDOUL F，NONAMI K，FANTONI I，et al.An adaptive vision-based autopilot for mini flying machines guidance，navigation and control［J］.Autonomous Robots，2009，27(3):165-188.

［5］HRABAR S，SUKHATME G S，CORKE P，et al. Combined optic-flow and stereo-based navigation of urban canyons for a UAV［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS). Edmonton，Alberta:［s.n.］，2005:3309-3316.

［6］LATEGAHN H，DERENDARZ W，GRAF T，et al. Occupancy grid computation from dense stereo and sparse structure and motion points forautomotive applications［C］//Intelligent Vehicles Symposium (IV).Piscataway:IEEE，2010:819-824.

［7］KITT B，GEIGER A，LATEGAHN H.Visual odometry based on stereo image sequences with RANSAC-based outlierrejection scheme［C］//IntelligentVehicles Symposium(IV).Piscataway:IEEE，2010:486-492.

［8］AGRAWAL M，KONOLIGE K.Real-time localization in outdoor environments using stereo vision and inexpensive GPS［C］//International Conference on Pattern Recognition.Piscataway:IEEE，2006，3:1603-1608.

［9］潘順良，王曉劍，沈為群，等.視覺導引的無人直升機自主著艦系統仿真［J］.航天控制，2008，26(2): 63-67.

［10］徐貴力，倪立學，程月華.基于合作目標和視覺的無人飛行器全天候自動著陸導引關鍵技術［J］.航空學報，2008，29(2):437-442.

［11］李建，李小民，錢克昌，等.基于雙目視覺和慣性器件的微小型無人機運動狀態估計方法研究［J］.航空學報，2011，32(11):2310-2317.

［12］HARTLEY R，ZISSERMAN A，EBRARY I.Multiple view geometry in computer vision［M］.Cambridge: Cambridge Univ Press，2003:151-361.

［13］FUSIELLO A，TRUCCO E，VERRI A.A compact algorithm for rectification of stereo pairs［J］.Machine Vision and Applications，2000，12(1):16-22.

［14］GONZALEZ R C，WOODS R E.Digital image processing［M］.San Antonio，TX:Pearson Education，2002:399-550.

［15］HARRIS C，STEPHENS M.A combined corner and edge detector［C］//Procedings of Fourth Alvey Vision Conference.Manchester，UK:［s.n.］，1988:147-151.

［16］TSAI D M，LIN C T.Fast normalized cross correlation for defect detection［J］.Pattern Recognition Letters，2003，24(15):2625-2631.

［17］馬頌德，張正友.計算機視覺:計算理論與算法基礎［M］.北京:科學出版社，1998:282.