徐偉杰，李 平，韓 波

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

姿態(tài)測(cè)量在移動(dòng)機(jī)器人運(yùn)動(dòng)控制中非常重要，例如旋翼機(jī)型無(wú)人機(jī)實(shí)現(xiàn)懸停控制需要精準(zhǔn)的姿態(tài)信息。傳統(tǒng)的航姿測(cè)量方法［1－2］主要依賴MEMS慣性傳感器(如陀螺儀和加速度計(jì))，近十年來(lái)，由于攝像機(jī)具有成本低、能耗低和重量輕等優(yōu)點(diǎn)，以標(biāo)定過(guò)的攝像機(jī)作為傳感器的視覺姿態(tài)測(cè)量方法成為當(dāng)前研究熱點(diǎn)。但是多數(shù)方法需要移動(dòng)機(jī)器人工作環(huán)境中存在特定結(jié)構(gòu)物體如道路、跑道的邊緣線［3］，地平線等［4－8］，或者人工圖標(biāo)［9］。非結(jié)構(gòu)化工作環(huán)境中不存在特定結(jié)構(gòu)物體和人工圖標(biāo)，無(wú)法使用前述方法測(cè)量姿態(tài)。

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

基于EKF-SLAM的視覺羅盤姿態(tài)測(cè)量方法存在兩個(gè)問(wèn)題。第1個(gè)問(wèn)題是由于使用EKF，需要對(duì)非線性模型進(jìn)行線性化，會(huì)引入線性化截?cái)嗾`差?？梢杂脽o(wú)跡卡爾曼濾波(Unscented Kalman Filter，UKF)取代EKF，使測(cè)量精度得以提高。第2個(gè)問(wèn)題是在實(shí)際應(yīng)用中攝像機(jī)可能受到?jīng)_擊擾動(dòng)而旋轉(zhuǎn)不平滑，需要提高平滑旋轉(zhuǎn)模型中系統(tǒng)噪聲的先驗(yàn)方差設(shè)定值，但是隨之導(dǎo)致主動(dòng)搜索匹配區(qū)域急劇擴(kuò)大，造成匹配計(jì)算量增大和易受與路標(biāo)相似的圖像特征影響匹配錯(cuò)誤發(fā)生率升高，目前仍然沒有解決方案。

針對(duì)匹配計(jì)算量增大和匹配錯(cuò)誤發(fā)生率升高，本文給出一種改進(jìn)的視覺羅盤姿態(tài)測(cè)量方法:首先使用多分辨率路標(biāo)選取策略初始化新路標(biāo)，然后使用逐層主動(dòng)搜索匹配算法減少匹配計(jì)算量和降低匹配錯(cuò)誤發(fā)生率，最后結(jié)合平滑轉(zhuǎn)動(dòng)模型得到的姿態(tài)先驗(yàn)信息，使用最小抽樣集為2個(gè)匹配點(diǎn)的RANSAC算法去除匹配野值，提高姿態(tài)測(cè)量精確度。實(shí)驗(yàn)結(jié)果表明，改進(jìn)的方法與原方法相比具有計(jì)算量更小、姿態(tài)測(cè)量更精確的優(yōu)點(diǎn)。

1 EKF-SLAM的視覺羅盤

1.1 狀態(tài)向量

定義系統(tǒng)狀態(tài)向量x及其協(xié)方差矩陣P形式如下:

式中攝像機(jī)C的狀態(tài)向量xC=［qWC，ωC］T由四元數(shù)qWC和角速度ωC構(gòu)成，qWC以世界坐標(biāo)系W為參考系，ωC以攝像機(jī)坐標(biāo)系C為參考系。地圖M的狀態(tài)向量 xM=［y1，…，yN］T由N個(gè)路標(biāo)yi構(gòu)成，以世界坐標(biāo)系W為參考系。

如圖1，空間點(diǎn)X到光心OC的距離d遠(yuǎn)大于攝像機(jī)平移距離‖t‖時(shí)，可認(rèn)為X是無(wú)窮遠(yuǎn)的。由于X到OC的方向矢量m由方位角θ和高度角φ唯一確定:

因此令路標(biāo)參數(shù)為 y=［θ，φ］T。

圖1 視覺羅盤示意圖

1.2 運(yùn)動(dòng)模型

定義系統(tǒng)運(yùn)動(dòng)模型 f(xk－1，αk)如下:

式中攝像機(jī)運(yùn)動(dòng)模型fC(xC，k－1，αk)具體形式如下:

角加速度 αk=［αx，αy，αz］T是系統(tǒng)噪聲，符合零均值高斯分布N(0，σ2αI3×3)，αα是 αk的先驗(yàn)方差?？紤]到攝像機(jī)非平滑旋轉(zhuǎn)時(shí)實(shí)際方差^σα較大，需要提高先驗(yàn)方差σα的設(shè)定值。

1.3 觀測(cè)模型

世界坐標(biāo)系W到攝像機(jī)坐標(biāo)系C的旋轉(zhuǎn)矩陣為RWC=R(qWC)。路標(biāo)y所對(duì)應(yīng)方向矢量m(θ，φ)在攝像機(jī)坐標(biāo)系C下坐標(biāo)為:

若攝像機(jī)不存在光學(xué)畸變，則mC成像為非畸變的像hu:

結(jié)合式(2)、式(5)、式(6)和式(7)，攝像機(jī)姿態(tài)為時(shí)路標(biāo)yi的觀測(cè)函數(shù)h(，yi，vi，k):

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

當(dāng)圖像中可視路標(biāo)數(shù)量小于一定數(shù)量時(shí)，需要從圖像中路標(biāo)最稀疏的區(qū)域里選取圖像特征znew，初始化路標(biāo) ynew，更新狀態(tài)向量 xnew和協(xié)方差矩陣Pnew。

1.4 EKF-SLAM 流程

EKF-SLAM流程分為狀態(tài)預(yù)測(cè)、觀測(cè)預(yù)測(cè)、觀測(cè)匹配和狀態(tài)更新4步。

(1)狀態(tài)預(yù)測(cè) 根據(jù)k－1時(shí)刻狀態(tài)和協(xié)方差的估計(jì)值，由運(yùn)動(dòng)模型(3)預(yù)測(cè)時(shí)刻狀態(tài)、協(xié)方差的估計(jì)值:

(2)觀測(cè)預(yù)測(cè) 預(yù)測(cè)各路標(biāo)yi觀測(cè)值和協(xié)方差Si，k的估計(jì)值:

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

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

(4)狀態(tài)更新 觀測(cè)向量≠?時(shí)，更新狀態(tài)和協(xié)方差矩陣的估計(jì)值:

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

2 改進(jìn)的視覺羅盤姿態(tài)測(cè)量方法

針對(duì)設(shè)定較大的先驗(yàn)系統(tǒng)噪聲方差時(shí)存在匹配計(jì)算量大和匹配錯(cuò)誤的問(wèn)題，本文對(duì)原視覺羅盤姿態(tài)測(cè)量方法進(jìn)行改進(jìn)。首先初始化路標(biāo)時(shí)構(gòu)建高斯圖像金字塔，選取各層圖像中均為Shi-Tomasi角點(diǎn)的特征為路標(biāo)；然后按照自頂層圖像到底層圖像的順序逐層主動(dòng)搜索匹配，減少匹配計(jì)算量和匹配錯(cuò)誤發(fā)生率；最后結(jié)合由平滑旋轉(zhuǎn)模型得到的姿態(tài)先驗(yàn)信息，使用2點(diǎn)RANSAC去除匹配野值。

2.1 多分辨率圖像路標(biāo)選取

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

圖2 圖像金字塔示意圖

Shi-Tomasi角點(diǎn)［16］在圖像模板匹配中具有良好的定位性能。其提取過(guò)程首先計(jì)算自相關(guān)函數(shù)矩陣M:

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

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

2.2 逐層主動(dòng)搜索匹配

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

算法1逐層主動(dòng)搜索匹配算法

輸入:圖像 Ll和路標(biāo)模板{(yi)}，l≤L，預(yù)測(cè)的觀測(cè)值、殘差協(xié)方差矩陣Si。

輸出:匹配點(diǎn)首先，在第L層圖像中進(jìn)行主動(dòng)搜索匹配。進(jìn)行坐標(biāo)尺度變換，在以為中心的搜索區(qū)域Ω()內(nèi)計(jì)算與(yi)的相關(guān)度CC(u，v)。搜索區(qū)域Ω()定義為:

取像素(u*，v*)=maxCC(u，v)＞CCth，作為的匹配點(diǎn)=［u*，v*］T。閾值CCth根據(jù)經(jīng)驗(yàn)設(shè)定，如0.8～0.9。然后，在第l＜L層圖像中進(jìn)行主動(dòng)搜索匹配。進(jìn)行坐標(biāo)尺度變換→，在以為中心的搜索區(qū)域 Ω(r)內(nèi)計(jì)算與(yi)的相關(guān)度CC(u，v)。搜索區(qū)域Ω(r)定義為:

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

2.3 2點(diǎn)RANSAC算法去除匹配野值

逐層主動(dòng)搜索匹配無(wú)法保證零匹配錯(cuò)誤率，需要使用RANSAC算法去除匹配野值。由于抽樣匹配點(diǎn)數(shù)量越大，RANSAC算法的抽樣迭代次數(shù)越大，因此要減少抽樣匹配點(diǎn)數(shù)量?？紤]到只需2個(gè)匹配點(diǎn)對(duì)能夠唯一確定攝像機(jī)姿態(tài)，又有EKF-SLAM運(yùn)動(dòng)模型提供的姿態(tài)先驗(yàn)信息可以利用，本文提出抽樣集為2個(gè)數(shù)據(jù)點(diǎn)的2點(diǎn)RANSAC去除匹配野點(diǎn)算法，可以減少抽樣迭代次數(shù)。

算法2 2點(diǎn)RANSAC算法

輸入:匹配點(diǎn)集{}，狀態(tài)向量和協(xié)方差矩陣。

輸出:匹配點(diǎn)集內(nèi)點(diǎn)集。

令=?；

循環(huán)j=1，…，nhyp，nhyp為最大抽樣次數(shù):

從匹配點(diǎn)集{}隨機(jī)抽樣集{}，令觀測(cè)向量

根據(jù)更新后的狀態(tài)向量重新進(jìn)行預(yù)測(cè)各路標(biāo)的觀測(cè)值z(mì)i|(j)=h(x(j))；

循環(huán)結(jié)束。

用匹配點(diǎn)內(nèi)值集取代式(15)中的，按照式(16)、式(17)、式(18)更新狀態(tài)和協(xié)方差估計(jì)值。

3 實(shí)驗(yàn)與結(jié)果分析

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

圖3 實(shí)驗(yàn)用的四旋翼無(wú)人機(jī)

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

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

圖5 兩種方法路標(biāo)匹配的結(jié)果

圖6 用姿態(tài)測(cè)量數(shù)據(jù)對(duì)圖像進(jìn)行單應(yīng)映射

4 結(jié)論

基于EKF-SLAM的視覺羅盤姿態(tài)測(cè)量方法在系統(tǒng)噪聲先驗(yàn)方差設(shè)定為大值時(shí)存在匹配計(jì)算量增大和匹配錯(cuò)誤發(fā)生率升高的問(wèn)題，本文給出一種改進(jìn)的視覺羅盤姿態(tài)測(cè)量方法。首先使用多分辨率路標(biāo)選取策略初始化新路標(biāo)，然后使用逐層主動(dòng)搜索匹配算法減少匹配計(jì)算量和降低匹配錯(cuò)誤發(fā)生率，最后結(jié)合平滑轉(zhuǎn)動(dòng)模型得到的姿態(tài)先驗(yàn)信息，使用最小抽樣集為2個(gè)匹配點(diǎn)的RANSAC算法去除匹配野值，提高姿態(tài)測(cè)量精確度。實(shí)驗(yàn)結(jié)果證明，改進(jìn)的方法與原方法相比具有計(jì)算量小、測(cè)量姿態(tài)精確的優(yōu)點(diǎn)。

攝像機(jī)在旋轉(zhuǎn)過(guò)程中總存在位移，本文實(shí)驗(yàn)環(huán)境中路標(biāo)空間點(diǎn)到攝像機(jī)的距離較遠(yuǎn)，可以認(rèn)為是無(wú)窮遠(yuǎn)的，但是實(shí)際環(huán)境中某些路標(biāo)空間點(diǎn)(如障礙物)到攝像機(jī)的距離較近，不能認(rèn)為是無(wú)窮遠(yuǎn)的。在以后的研究中，將討論如何把2點(diǎn)RANSAC去除匹配野值和近距離路標(biāo)空間點(diǎn)檢測(cè)結(jié)合進(jìn)行。

［1］史智寧，韓波，徐玉，等.基于重力場(chǎng)自適應(yīng)互補(bǔ)濾波的無(wú)人直升機(jī)水平姿態(tài)估計(jì)方法［J］.傳感技術(shù)學(xué)報(bào)，2009，22(7):993－996.

［2］杜繼永，黃國(guó)榮，張鳳鳴，等.基于低成本MEMS器件的捷聯(lián)航姿系統(tǒng)設(shè)計(jì)［J］.傳感技術(shù)學(xué)報(bào)，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.