基于景象匹配的低精度MEMS 航向修正與組合算法

朱啟舉，樊振輝，梅春波，楊鵬翔，盧寶峰，楊朝明

（1.西安現(xiàn)代控制技術(shù)研究所，西安 710065；2.西北工業(yè)大學，電子信息學院，西安 710072）

視覺景象匹配導航系統(tǒng)（Scene Matching Navigation System,SMNS）的研究起源于美國戰(zhàn)斧式巡航導彈的末端導航與制導研究，并逐漸發(fā)展成為一種自主導航技術(shù)[1]。受益于其結(jié)構(gòu)簡單、自主、高精度的特性，在衛(wèi)星拒止的復雜應用環(huán)境下，近幾年視覺景象匹配導航在國內(nèi)軍事工業(yè)領(lǐng)域得到廣泛研究[2,3]。

作為一種量測手段，現(xiàn)有的景象匹配導航算法只能輸出位置信息，如高動態(tài)飛行器常用的捷聯(lián)慣導系統(tǒng)/合成孔徑雷達（Strapdown Inertial Navigation System/Synthetic Aperture Radar,SINS/SAR）緊組合導航系統(tǒng)，其中的SAR 景象匹配模塊僅能提供載體位置、載體至回波中心的偽距/偽距率兩種量測[4]。此外，基于光學或紅外傳感器的景象匹配模塊僅能提供絕對位置信息[3,5]。受限于成本、體積與功耗等因素，目前低成本、輕量化微機電系統(tǒng)（Micro-Electro-Mechanical System,MEMS）是巡飛彈彈載慣性傳感器的主流產(chǎn)品，然而此類MEMS 航向角發(fā)散速度較快。在衛(wèi)星拒止環(huán)境下如何充分利用景象匹配結(jié)果，設(shè)計出一種航向角修正算法，將大大提高巡飛類彈藥的自主導航能力，能夠為其長時間巡航提供保障。

隨著國內(nèi)外導航技術(shù)的快速發(fā)展，涌現(xiàn)出諸多航向量測方法，如基于仿生沙蟻導航的偏振光相機能夠利用太陽偏振光定向[6]，星敏感器可利用恒星星圖匹配實現(xiàn)定位定向，地磁傳感器能夠提供北向角等[7]。然而此三種傳感器有著無法忽視的缺點[8]，偏振光定向功能受限于天氣、云霧、與太陽角度等，且目前尚無實際應用的案例，星敏感器一般只能工作于太空，近地空間難以應用，而地磁傳感器極易受周邊環(huán)境的干擾。此外，新增傳感器必然給組合導航系統(tǒng)帶來冗余，如果能利用景象匹配結(jié)果，設(shè)計一種航向角修正算法，對導航系統(tǒng)的設(shè)計是一種最優(yōu)的解決方案。

有鑒于此，本文利用景象匹配結(jié)果提出了一種航向角測量方法，并根據(jù)匹配點數(shù)量設(shè)計了一種航向角修正與組合算法。為了驗證本文算法的有效性，搭建了由慣性/視覺/高度計組合裝置組成的實驗系統(tǒng)，并進行了掛飛實驗，結(jié)果表明本文算法的精度與時間均滿足實時應用需求。

1 系統(tǒng)架構(gòu)

視覺景象匹配導航系統(tǒng)的研究起源于導彈的末制導[1]，其流程如圖1 所示，可以概括為三步：1）利用相對高度、導航坐標系下的姿態(tài)信息，將實時感知圖像進行地理信息修正，使其與基準圖的像素坐標系平行、空間分辨率相同；2）將地理信息修正后的實時圖與基準圖進行特征提取與匹配，得到實時圖在基準圖上的像素坐標；3）根據(jù)基準圖預存的地理坐標，將匹配所得的像素坐標轉(zhuǎn)化為經(jīng)緯度，并作為量測與MEMS 導航解算結(jié)果進行組合。

圖1 視覺景象匹配導航系統(tǒng)流程圖Fig.1 Flow chart of scene matching navigation system

定位是視覺景象匹配導航系統(tǒng)的首要功能，本文在視覺景象匹配導航結(jié)果的基礎(chǔ)上，設(shè)計了定向算法與組合算法，如圖1 中藍色部分所示。定向算法利用特征匹配后的匹配點對關(guān)系、相對高度兩種量測信息，結(jié)合基準圖空間分辨率、相機內(nèi)參等參數(shù)完成定向。

2 航向誤差模型與組合算法

2.1 基于匹配點幾何關(guān)系的航向誤差模型

實時圖根據(jù)SINS 導航解算得到的姿態(tài)進行地理信息修正，即正射投影。如果導航解算生成的航向角存在較大誤差（航向角記為φ），利用該姿態(tài)投影出的圖像必然存在誤差，如圖2(b)所示。本文設(shè)計了一種基于匹配點對的航向角修正算法，通過求取SINS 提供的航向角與航向真值之間的差值間接地計算出航向角，而兩者之間的差值通過實時圖與基準圖之間匹配點的集合關(guān)系完成計算。

圖2 圖像匹配關(guān)系Fig.2 Image matching relationship

基準圖與實時圖在經(jīng)過匹配及篩選流程后，能夠得到匹配點對，如圖2 所示。基準圖與實時圖上的紅色方框表示一對匹配點，藍色方框表示另一對匹配點。通過求解匹配點對橫縱像素距離的反正切值，能夠分別計算出基準圖與實時圖中兩個像素連線與正上方的角度，如圖2 中的角a與角b。

根據(jù)圖2 所示的幾何關(guān)系可以看出，角b減去角a即為實時圖與基準圖之間航向角差值，記為角c?？紤]到基準圖橫縱方向為正東正北，角c即為航向真值與導航解算航向角φ之間的差值。角c加上φ就等于航向角真值。角a，b的計算方法為：

其中，Ib表示基準圖；In表示地理信息修正后的實時圖；上標中的u、v分別表示匹配點的橫縱像素坐標，數(shù)字表示匹配點的序號。航向角真值的計算公式為：

為了提高航向真值的計算精度，采用多對點計算并取均值的方式計算角c，如圖3 所示，此時航向角真值的計算公式為：

圖3 多對點對的航向角計算關(guān)系Fig.3 Course angle calculation of multiple matching points

式(3)即可獨立完成航向角計算?？紤]到投影過程引入了水平兩角度的誤差，導致式(1)計算的差值里包含了水平兩角度誤差所引起的投影誤差。由于三個姿態(tài)角的耦合關(guān)系，無法將航向角單獨分離出來，即當水平姿態(tài)角誤差較大的情況下，地理信息修正后的圖像In特征點位置存在誤差，從而引起式(1)與式(3)計算出的航向角存在誤差。為解決該問題，本文設(shè)計了基于匹配點集透視變換關(guān)系的航向誤差模型，并采用優(yōu)化的方法求解出高精度航向角。首先給出基準圖In與原始圖Ic的關(guān)系為：

其中，K表示相機內(nèi)參；(u,v) 表示二維像素坐標；表示相機坐標系（c系）至導航坐標系（n系）之間的旋轉(zhuǎn)矩陣，可通過慣性導航解算的航向角φ、俯仰角γ、滾轉(zhuǎn)角θ計算得到。圖4 為坐標系示意圖，導航坐標系On-xn yn zn近似以相機光心為中心，xn yn zn三軸指向北天東；相機坐標系Oc-xc yc zc以相機光心為中心，xc yc zc三軸指向相機右上前（左手坐標系）；以相機光心為中心構(gòu)建局部坐標系Ow-xw yw zw，其為正下視坐標系，xw yw zw分別指向南東地（左手坐標系）。

圖4 坐標系示意圖Fig.4 Diagram of the coordinates

由于在長航時過程中，慣性導航系統(tǒng)提供的姿態(tài)角存在較大誤差，可根據(jù)式(1)與式(4)構(gòu)建出航向角誤差模型[9]：

式(5)右側(cè)ai、ib的求解包含了式(4)的模型，即包含了航向角這一待優(yōu)化變量，其優(yōu)化求解過程見2.3節(jié)。

2.2 基于匹配點重投影的航向誤差模型

景象匹配完成后，能夠得到基準圖與地理信息修正后實時圖的匹配關(guān)系，此時二者僅存在平移關(guān)系（姿態(tài)誤差將作為優(yōu)化變量）。根據(jù)基準圖預存的地理信息，可以計算出相機光心的絕對坐標。令高度計提供的相對高度?H為基準圖匹配點在局部坐標系下zw向坐標，并通過基準圖空間分辨率計算出基準圖上匹配點在局部坐標系下的xw、yw向坐標，最終可得到基準圖匹配點在局部坐標系下的三維坐標P(xw,yw,zw)。

根據(jù)相機成像模型，可知局部坐標系至像素坐標系的計算公式為：

其中，t表示局部坐標系與相機坐標系之間的平移，由圖4 可看出平移量為0；Ie為成像模型計算出匹配點的像素坐標。局部坐標系的三軸與導航坐標系的三軸相平行，軸間對應關(guān)系見圖4。根據(jù)MEMS 導航解算提供的姿態(tài)角，計算公式為：

需要注意的是由于局部坐標系為左手坐標系，按照航向、滾轉(zhuǎn)、俯仰的旋轉(zhuǎn)順序構(gòu)建旋轉(zhuǎn)矩陣時，需要將滾轉(zhuǎn)角與航向角取為負值，則根據(jù)模型預測Ie與原始值Ic（真值）可以構(gòu)建重投影誤差模型：

根據(jù)相對高度與相機視場角的幾何關(guān)系[10]，得到修正后的相對高度?H*為：

其中，F(xiàn)OV表示水平視場角度；Lirt表示投影后圖像水平方向尺寸；εbenc表示基準圖空間分辨率。

2.3 航向修正與組合算法

式(3)對匹配點之間的距離存在要求，當匹配點之間的距離較近時，會帶來較大的航向角誤差。理想情況下，實時圖像四個角附近特征點的精度最高。式(5)與式(8)需要較多的匹配點，匹配點數(shù)量較少時使用優(yōu)化算法會導致較大誤差。因此，考慮匹配點之間的距離與匹配點數(shù)量，本文設(shè)計的航向角修正與組合算法如下：

1）當匹配點對數(shù)小于等于15 時：

Step.1 實時圖與基準圖經(jīng)過匹配流程后，得到匹配點對；

Step.2 以第1 對匹配點為基準，遍歷剩余匹配點，當距離大于30 像素時，分別計算ai、bi、ci，如圖3所示，否則不進行運算。以剩余匹配點為基準，重復上述與第1 對匹配點同樣的流程，最終所有ci取平均即可得到c。

Step.3 將c作為航向角修正值，與導航解算的航向角φ疊加即為修正后的航向角。

2）當匹配點對數(shù)大于15 時：

Step.1 計算出航向角，使用式(5)構(gòu)建誤差模型；

Step.2 根據(jù)匹配點對計算尺度比S，進一步修正?H，在此基礎(chǔ)上計算三維坐標P(xw,yw,zw)，并根據(jù)匹配關(guān)系構(gòu)建重投影誤差模型；

Step.3 聯(lián)合優(yōu)化，如式(11)所示。

由于優(yōu)化算法不同，式(11)所述的聯(lián)合優(yōu)化分兩步進行。首先，根據(jù)式(7)利用導航解算姿態(tài)角計算出旋轉(zhuǎn)矩陣。對于式(8)，本文采用左乘擾動模型計算三維姿態(tài)角的優(yōu)化增量，該過程使用慣性導航系統(tǒng)提供的姿態(tài)角作為初值計算初始矩陣。其次，由于式(5)所示的待優(yōu)化函數(shù)涉及旋轉(zhuǎn)矩陣內(nèi)變量的反正切值，如果考慮歐拉角間的耦合關(guān)系，則對于角φ的導函數(shù)沒有顯式表達式。同時考慮到圖1 所示的視覺景象匹配導航系統(tǒng)一般應用在水平姿態(tài)角變化較小的場景，本文忽略耦合關(guān)系，直接將角φ視為待優(yōu)化變量，并將式(8)優(yōu)化后的水平姿態(tài)角作為常值，通過式(7)代入式(4)，即可得到包含唯一變量的優(yōu)化函數(shù)，采用高斯牛頓法即可完成優(yōu)化求解。

本文僅更新航向量測，以誤差作為狀態(tài)量，并采用常規(guī)的松組合卡爾曼濾波完成航向角量測與慣導系統(tǒng)的組合。首先，構(gòu)建相對旋轉(zhuǎn)矩陣：

然后，計算航向誤差量測量：

由于存在量測滯后，需通過線性外推完成量測更新，然后對外推量測更新結(jié)果得到的狀態(tài)量回推，得到姿態(tài)更新結(jié)果[11]。

3 實驗分析與結(jié)果

3.1 實驗過程與結(jié)果

本文搭建的掛飛實驗平臺如圖5所示，其中圖5(a)為掛飛試驗平臺，圖5(b)為各傳感器安裝位置，圖5(c)-(g)分別為GNSS、MEMS、處理器1、相機、處理器2。各傳感器配置為：相機（1920×1080，30 Hz）、MEMS（30 Hz，陀螺儀零偏：10°/h，隨機噪聲：）、GNSS（10 Hz）、處理器1（SK1207B）和處理器2（Nvidia NX,Ubuntu-18.04）。處理器1 用于數(shù)據(jù)對齊，處理器2 用于運行本文算法。GNSS 信息經(jīng)過利用載波相位差分技術(shù)（Real time kinematic,RTK）處理后可提供高精度位置信息。如圖1 所示的系統(tǒng)架構(gòu)，地理信息修正需要輸入相機相對于地面的高度，本文RTK 測量絕對高度，并使用無人機飛行過程中的絕對高度與起飛前的絕對高度之差作為相對高度。考慮到在有限的高度下，相機所感知的圖像對應實際區(qū)域較小，且相機的分辨率達到2 K，地形高度引起的誤差可以忽略。同時RTK 定位精度為厘米級，使用雙衛(wèi)星接收機提供精確的航向作為對比參考值。

圖5 實驗平臺實物圖Fig.5 The experimental platform

慣性/視覺景象匹配導航裝置如圖6 所示，其中OI-xI yI zI表示慣性坐標系（載體坐標系），Oc-xc yc zc表示相機坐標系。兩種坐標系在實驗前經(jīng)過單獨標定與聯(lián)合標定，溫度對標定內(nèi)參的影響可以忽略。

圖6 慣性/視覺景象匹配導航裝置Fig.6 Inertial/vision scene matching navigation device

為了驗證本文算法的有效性，在陜西省西安市錦湖街道進行了無人機載飛行實驗。實驗經(jīng)度范圍約為108.8942°～108.9101°，緯度范圍約為34.1224°～34.1385°。無人機飛行期間，高度約為150～500 m，速度約為12 m/s，水平姿態(tài)變化范圍為-30°～-90°，航向變化范圍為-180°～180°，飛行時間約30 min。作為參考的衛(wèi)星圖像來自于Googlemap，分辨率為645.8 mm。特征提取與匹配算法使用點特征，實驗匹配效果如圖7 所示。

圖7 匹配效果Fig.7 Matching results

數(shù)據(jù)分析過程使用21 min 1 s 至27 min 37 s 之間的數(shù)據(jù)，此階段導航系統(tǒng)解算的航向誤差為2.5°～4.5°。相比前期數(shù)據(jù)，此階段的航向誤差較大，具備對本文算法進行驗證的條件。采集不同角度、不同高度的實時圖像共397 張，采集頻率為1 Hz。以慣性/RTK 松組合提供的航向角為基準（雙衛(wèi)星接收機提供航向量測），最終計算出的航向誤差如圖8 所示。在處理過程中，考慮到匹配點誤差較大時必然導致航向角誤差大，本文將誤差絕對值大于15°的結(jié)果舍棄。

圖8 航向誤差結(jié)果Fig.8 Results of the course error

在所有圖像中，滿足2.3 節(jié)所述規(guī)則的圖像有392張，即滿足至少有兩對匹配點距離大于30 像素，有效率為98.74%。在有效圖片中，滿足2.3 節(jié)條件（1）的數(shù)量共計為303 張，滿足條件（2）的數(shù)量為89 張?；谟行D像，得到航向角的均方根誤差（Root Mean Square Error,RMSE）為0.7423°；最大誤差絕對值為7.591°，這是由于匹配點誤差較大，導致計算出的航向角誤差也較大；最小誤差絕對值為0.002°。

3.2 實驗分析

實驗結(jié)果表明，本文算法解算出的航向角具備一定精度，且有效圖像解算出的航向角相比導航解算航向角有較大提高，驗證了本文算法的有效性。

在對原始實時圖像進行地理信息修正的過程中，會利用慣性導航解算結(jié)果進行投影處理，使實時圖與基準圖處于相同的坐標系[12]。但隨著時間累積，慣性導航系統(tǒng)提供的航向角必然存在較大誤差，投影之后的圖像與基準圖存在誤差，而本文算法利用匹配點的幾何關(guān)系計算出航向角誤差，與導航系統(tǒng)解算的航向角相疊加即可計算出修正后的航向角。由于姿態(tài)角存在耦合關(guān)系，本文設(shè)計了兩種誤差模型來優(yōu)化航向角，考慮到優(yōu)化模型需要大量的樣本才能保證精度，因此基于匹配點數(shù)量設(shè)計了一種匹配規(guī)則，確保了航向角的精度。

同時，需要指出的是，本文算法的適用性受限于前端“特征匹配”的結(jié)果。首先，需滿足在景象匹配步驟后得到匹配點對，即本文算法不適用于傳統(tǒng)的線特征匹配結(jié)果。同時，匹配成功是本文算法的前提，匹配不成功無法應用本文算法。因季節(jié)、氣候、光照、雨雪霧霾等因素導致基準圖與實時圖存在較大差異，亦或沙漠、海洋、戈壁等少紋理特征的區(qū)域?qū)е碌钠ヅ涫?，本文亦無法解決。本文僅基于“特征匹配”結(jié)果設(shè)計了一種航向角修正與融合算法。此外，本文算法的精度取決于匹配誤差，應確保匹配像素誤差低于1.5 像素，考慮到常規(guī)的SURF（Speeded Up Robust Feature,加速穩(wěn)健特征）家族點特征匹配算法、方向梯度特征通道（Chanel Feature of Orientated Gradient,CFOG）區(qū)域特征算法等均能滿足該要求[13]，可以認為本文所設(shè)計的算法具備一定的通用性。

4 結(jié)論

本文針對低精度MEMS 在長航時過程中航向角易發(fā)散的問題，提出了一種利用景象匹配結(jié)果修正航向角的算法。首先，基于匹配點幾何關(guān)系設(shè)計了航向角誤差模型，通過匹配點對與圖像縱軸的正切值之差計算航向修正值。然后，基于匹配點重投影誤差設(shè)計了航向角誤差模型，通過實時圖與基準圖的匹配關(guān)系構(gòu)建了重投影誤差，通過優(yōu)化計算航向角的最優(yōu)值?；谏鲜鰞煞N模型，本文根據(jù)匹配點數(shù)量設(shè)計了一種航向角修正與組合算法。飛行實驗結(jié)果表明，長航時過程中修正后的航向角誤差為0.7423°（RMSE），更新頻率優(yōu)于1 Hz。滿足條件的有效圖像數(shù)量比例為98.74%，驗證了本文算法的有效性。同時，與傳統(tǒng)僅能定位的景象匹配算法相比，本文設(shè)計了一種航向計算算法，且該算法對傳感器精度依賴性較低，具備一定的通用性。