一種改進的視覺羅盤姿態測量方法*

徐偉杰，李 平，韓 波

(浙江大學控制科學與工程系，杭州 310027)

姿態測量在移動機器人運動控制中非常重要，例如旋翼機型無人機實現懸停控制需要精準的姿態信息。傳統的航姿測量方法［1－2］主要依賴MEMS慣性傳感器(如陀螺儀和加速度計)，近十年來，由于攝像機具有成本低、能耗低和重量輕等優點，以標定過的攝像機作為傳感器的視覺姿態測量方法成為當前研究熱點。但是多數方法需要移動機器人工作環境中存在特定結構物體如道路、跑道的邊緣線［3］，地平線等［4－8］，或者人工圖標［9］。非結構化工作環境中不存在特定結構物體和人工圖標，無法使用前述方法測量姿態。

針對非結構化工作環境，學者們提出了各種稱為“視覺羅盤”［10－12］的姿態測量方法，如 Labrosse 等人［10］使用全景攝像機，通過全景圖的快速匹配測量航向角(yaw)；Sturm 等人［11］使用普通攝像機，創建環境的一維柱狀顏色模式分布圖，通過顏色模式匹配測量航向角；與前兩種只能測量航向角的方法不同，Montiel等人［12］提出將基于擴展卡爾曼濾波(Extended Kalman Filter，EKF)的同步定位與地圖創建(Simultaneous Localization and Mapping，SLAM)［13］方法應用于姿態測量，以攝像機平滑旋轉模型假設和主動搜索匹配技術［14］為核心思想，能夠同時測量出航向角、俯仰角(pitch)和橫滾角(roll)。

基于EKF-SLAM的視覺羅盤姿態測量方法存在兩個問題。第1個問題是由于使用EKF，需要對非線性模型進行線性化，會引入線性化截斷誤差。可以用無跡卡爾曼濾波(Unscented Kalman Filter，UKF)取代EKF，使測量精度得以提高。第2個問題是在實際應用中攝像機可能受到沖擊擾動而旋轉不平滑，需要提高平滑旋轉模型中系統噪聲的先驗方差設定值，但是隨之導致主動搜索匹配區域急劇擴大，造成匹配計算量增大和易受與路標相似的圖像特征影響匹配錯誤發生率升高，目前仍然沒有解決方案。

針對匹配計算量增大和匹配錯誤發生率升高，本文給出一種改進的視覺羅盤姿態測量方法:首先使用多分辨率路標選取策略初始化新路標，然后使用逐層主動搜索匹配算法減少匹配計算量和降低匹配錯誤發生率，最后結合平滑轉動模型得到的姿態先驗信息，使用最小抽樣集為2個匹配點的RANSAC算法去除匹配野值，提高姿態測量精確度。實驗結果表明，改進的方法與原方法相比具有計算量更小、姿態測量更精確的優點。

1 EKF-SLAM的視覺羅盤

1.1 狀態向量

定義系統狀態向量x及其協方差矩陣P形式如下:

式中攝像機C的狀態向量xC=［qWC，ωC］T由四元數qWC和角速度ωC構成，qWC以世界坐標系W為參考系，ωC以攝像機坐標系C為參考系。地圖M的狀態向量 xM=［y1，…，yN］T由N個路標yi構成，以世界坐標系W為參考系。

如圖1，空間點X到光心OC的距離d遠大于攝像機平移距離‖t‖時，可認為X是無窮遠的。由于X到OC的方向矢量m由方位角θ和高度角φ唯一確定:

因此令路標參數為 y=［θ，φ］T。

圖1 視覺羅盤示意圖

1.2 運動模型

定義系統運動模型 f(xk－1，αk)如下:

式中攝像機運動模型fC(xC，k－1，αk)具體形式如下:

角加速度 αk=［αx，αy，αz］T是系統噪聲，符合零均值高斯分布N(0，σ2αI3×3)，αα是 αk的先驗方差。考慮到攝像機非平滑旋轉時實際方差^σα較大，需要提高先驗方差σα的設定值。

1.3 觀測模型

世界坐標系W到攝像機坐標系C的旋轉矩陣為RWC=R(qWC)。路標y所對應方向矢量m(θ，φ)在攝像機坐標系C下坐標為:

若攝像機不存在光學畸變，則mC成像為非畸變的像hu:

結合式(2)、式(5)、式(6)和式(7)，攝像機姿態為時路標yi的觀測函數h(，yi，vi，k):

式中觀測噪聲vi，k～N(0，Ri，k)，Ri，k是觀測噪聲協方差矩陣。

當圖像中可視路標數量小于一定數量時，需要從圖像中路標最稀疏的區域里選取圖像特征znew，初始化路標 ynew，更新狀態向量 xnew和協方差矩陣Pnew。

1.4 EKF-SLAM 流程

EKF-SLAM流程分為狀態預測、觀測預測、觀測匹配和狀態更新4步。

(1)狀態預測 根據k－1時刻狀態和協方差的估計值，由運動模型(3)預測時刻狀態、協方差的估計值:

(2)觀測預測 預測各路標yi觀測值和協方差Si，k的估計值:

式中觀測方程Jacobian矩陣Hi，k=［?h/，0，…，0，?h/?yi，0，…，0］，Ri，k是觀測噪聲協方差矩陣。

(3)觀測匹配 在視覺EKF-SLAM中，以路標yi的描述子為11像素×11像素的圖像模板L11(yi)為例，使用主動搜索匹配對式(13)預測可見路標yi進行模板匹配。以觀測值為中心，殘差協方差Si，k確定主動搜索的區域Ω(Si，k)，計算模板L11(yi)與區域內各像素(u，v)處局部圖像L11(u，v)的正則化相關系數CC(u，v)。取像素(u*，v*)=maxCC(u，v)＞CCth，作為匹配點=［u*，v*］T，其CCth中為相關系數閾值。把所有構成k時刻觀測向量:

(4)狀態更新 觀測向量≠?時，更新狀態和協方差矩陣的估計值:

其中Kk為Kalman濾波增益，觀測Jacobian矩陣Hk=［…，Hi，k，…］T，觀測噪聲協方差矩陣 Rk=diag(…，Ri，k，…)。式(17) 得到的中通常必須單位化。

2 改進的視覺羅盤姿態測量方法

針對設定較大的先驗系統噪聲方差時存在匹配計算量大和匹配錯誤的問題，本文對原視覺羅盤姿態測量方法進行改進。首先初始化路標時構建高斯圖像金字塔，選取各層圖像中均為Shi-Tomasi角點的特征為路標；然后按照自頂層圖像到底層圖像的順序逐層主動搜索匹配，減少匹配計算量和匹配錯誤發生率；最后結合由平滑旋轉模型得到的姿態先驗信息，使用2點RANSAC去除匹配野值。

2.1 多分辨率圖像路標選取

構建L層圖像金字塔［15］，以輸入圖像L為基層，逐層降采樣至原尺寸的 2－2l，l≤L－1，如圖 2 所示。本文中輸入圖像原始分辨率為像素，構建L=3層圖像金字塔，各層圖像L0、L1和L2對應分辨率320像素×240像素、160像素×120像素和80像素×60像素。

圖2 圖像金字塔示意圖

Shi-Tomasi角點［16］在圖像模板匹配中具有良好的定位性能。其提取過程首先計算自相關函數矩陣M:

式中w(u，v)為高斯核函數加權系數、是圖像在x和y方向的偏導。然后計算M的特征值λ1、λ2。若像素min(λ1，λ2)取到局部極大值且大于設定閾值，可確認為Shi-Tomasi角點。

改進的方法選取在L0、L1和L2中同為Shi-Tomasi角點的圖像特征作為路標yi，取11像素×11像素的模板(yi)(yi)和(yi)作為路標yi的描述子，用于逐層主動搜索匹配。

2.2 逐層主動搜索匹配

在EKF-SLAM流程中，式(13)和式(14)得到觀測值、殘差協方差矩陣Si的預測值后進行主動搜索匹配。與原方法具有單一分辨率模板不同，改進的方法具有多分辨率模板，可以在圖像金字塔各層圖像中進行匹配。由圖2可知，在圖像金字塔頂層圖像LL中，坐標尺度變換使得主動搜索區域內像素個數減少到圖像L中的2－2L。在其余各層圖像Ll，l＜L中，在 Ll+1中匹配點坐標尺度變換附近主動搜索匹配。自頂層圖像到底層圖像進行逐層主動搜索匹配可以減少匹配計算量并降低匹配錯誤發生率。

算法1逐層主動搜索匹配算法

輸入:圖像 Ll和路標模板{(yi)}，l≤L，預測的觀測值、殘差協方差矩陣Si。

輸出:匹配點首先，在第L層圖像中進行主動搜索匹配。進行坐標尺度變換，在以為中心的搜索區域Ω()內計算與(yi)的相關度CC(u，v)。搜索區域Ω()定義為:

取像素(u*，v*)=maxCC(u，v)＞CCth，作為的匹配點=［u*，v*］T。閾值CCth根據經驗設定，如0.8～0.9。然后，在第l＜L層圖像中進行主動搜索匹配。進行坐標尺度變換→，在以為中心的搜索區域 Ω(r)內計算與(yi)的相關度CC(u，v)。搜索區域Ω(r)定義為:

半徑r=3像素。取像素(u*，v*)=maxCC(u，v)＞CCth作為的匹配點=［u*，v*］T。若l=0 停止，作為輸出；反之令l=l－1重復進行主動搜索匹配。

2.3 2點RANSAC算法去除匹配野值

逐層主動搜索匹配無法保證零匹配錯誤率，需要使用RANSAC算法去除匹配野值。由于抽樣匹配點數量越大，RANSAC算法的抽樣迭代次數越大，因此要減少抽樣匹配點數量。考慮到只需2個匹配點對能夠唯一確定攝像機姿態，又有EKF-SLAM運動模型提供的姿態先驗信息可以利用，本文提出抽樣集為2個數據點的2點RANSAC去除匹配野點算法，可以減少抽樣迭代次數。

算法2 2點RANSAC算法

輸入:匹配點集{}，狀態向量和協方差矩陣。

輸出:匹配點集內點集。

令=?；

循環j=1，…，nhyp，nhyp為最大抽樣次數:

從匹配點集{}隨機抽樣集{}，令觀測向量

根據更新后的狀態向量重新進行預測各路標的觀測值zi|(j)=h(x(j))；

循環結束。

用匹配點內值集取代式(15)中的，按照式(16)、式(17)、式(18)更新狀態和協方差估計值。

3 實驗與結果分析

為驗證本文改進的方法的有效性，根據應用背景用四旋翼無人機的機載攝像機在室外環境中采集視頻用于實驗。攝像機為更換焦距2.5 mm廣角鏡頭后的Logitech QuickCam Pro4000，水平視角約為90°，安裝角略偏向下方，通過USB線纜與地面PC相連，采集的視頻分辨率320像素×240像素，幀率30 frame/s。視頻采集時四旋翼無人機繞機體垂直中軸旋轉約一周，位置保持在較小范圍內。

圖3 實驗用的四旋翼無人機

改進的方法的主要目的是減少匹配計算量和匹配錯誤發生。圖4(a)為處理一幀圖像改進的方法計算時間t2和原方法計算時間t2，圖4(b)為計算時間t1和t2的比值。EKF-SALM濾波收斂后，比值t1/t2始終在0.5左右，因此改進的方法有效減少計算量。圖5為視頻中第2兩種方法路標匹配的結果。路標2附近有相似的圖像特征，改進的方法使用逐層主動搜索匹配結果正確，而原方法使用標準主動搜索匹配結果錯誤，因此使用逐層主動搜索匹配算法有利于避免匹配錯誤發生。

圖4 兩種方法處理一幀圖像消耗時間及比值

圖5 兩種方法路標匹配的結果

圖6 用姿態測量數據對圖像進行單應映射

4 結論

基于EKF-SLAM的視覺羅盤姿態測量方法在系統噪聲先驗方差設定為大值時存在匹配計算量增大和匹配錯誤發生率升高的問題，本文給出一種改進的視覺羅盤姿態測量方法。首先使用多分辨率路標選取策略初始化新路標，然后使用逐層主動搜索匹配算法減少匹配計算量和降低匹配錯誤發生率，最后結合平滑轉動模型得到的姿態先驗信息，使用最小抽樣集為2個匹配點的RANSAC算法去除匹配野值，提高姿態測量精確度。實驗結果證明，改進的方法與原方法相比具有計算量小、測量姿態精確的優點。

攝像機在旋轉過程中總存在位移，本文實驗環境中路標空間點到攝像機的距離較遠，可以認為是無窮遠的，但是實際環境中某些路標空間點(如障礙物)到攝像機的距離較近，不能認為是無窮遠的。在以后的研究中，將討論如何把2點RANSAC去除匹配野值和近距離路標空間點檢測結合進行。

［1］史智寧，韓波，徐玉，等.基于重力場自適應互補濾波的無人直升機水平姿態估計方法［J］.傳感技術學報，2009，22(7):993－996.

［2］杜繼永，黃國榮，張鳳鳴，等.基于低成本MEMS器件的捷聯航姿系統設計［J］.傳感技術學報，2011，23(11):1662－1666.

［3］Sasa S，Gomi H，Ninomiya T，et al.Position and Attitude Estimation Using Image Processing of Runway［C］//Proceedings of 38th Aerospace Sciences Meeting and Exhibit AIAA，2000:1－10.

［4］Cornall T，Greg E.Measuring Horizon Angle from Video on a Small Unmanned Air Vehicle［C］//International Conference on Autono-mous Robots and Agents，2004:13－15.

［5］McGee G T，Sengupta R，Hdrick K.Obstacle Detection for Small Autonomous Aircraft Using Sky Segmentation［C］//IEEE International Conference on Robotics and Automation，2005:4679－4684.

［6］Ettinger S M，Nechyba M C，Ifju P G，et al.Towards Flight Autonomy:Vision-Based Horizon Detection for Micro Air Vehicles［C］//IEEE/RSJ International Conference on Intelligent Robots and System，2002，3:2134－2140.

［7］Bao Guiqiu，Xiong Shenshu，Zhou Zhaoying.Vision-Based Horizon Extraction for Micro Air Vehicle Flight Control［J］.IEEE Transactions on Instrumentation and Measurement，2005，54(3):1067－1072.

［8］Dusha D，Boles W，Walker R.Attitude Estimation for a Fixed-Wing Aircraft Using Horizon Detection and Optical Flow［C］//9th Biennial Conference of the Australian Pattern Recognition Society on Digital Image Computing Techniques and Applications，2007:485－492.

［9］Sharp C S，Shakernia O，Sastry S S.A Vision System for Landing an Unmanned Aerial Vehicle［C］//IEEE International Conference on，Robotics and Automation，2001，2:1720－1727.

［10］Labrosse F.The Visual Compass:Performance and Limitations of an Appearance-Based Method［J］.Journal of Field Robotics，2006，23(10):913－941.

［11］Sturm J，Visser A.An Appearance-Based Visual Compass for Mobile Robots［J］.Robotics and Autonomous Systems，2009，57(5):536－545.

［12］Montiel J M M，Davison A J.A Visual Compass Based on SLAM［C］//IEEE International Conference on Robotics and Automation，2006:1917－1922.

［13］Dissanayake M W M G，Newman P，Clark S，et al.A Solution to the Simultaneous Localization and Map Building(SLAM)problem［J］.IEEE Transactions on Robotics and Automation，2001，17(3):229－241.

［14］Davison A J.Real-Time Simultaneous Localization and Mapping with a Single Camera［C］//Proceedings of ICCV，2003:1403－1410.

［15］Burt P J.Fast Filter Transforms for Image Processing［C］//Computer Vision，Graphics and Image Processing，1981，16:20－51.

［16］Tomasi C，Kanade T.Detection and Tracking of Point Features［R］.Carnegie Mellon University Technical Report CMU－CS－91－132，1991.

［17］Hartley R，Zisserman A.Multiple View Geometry［M］.Cambridge University Press，2000.