視覺里程計中的相機(jī)姿態(tài)和高度實時測量方法*

曹　毓，張小虎，馮　瑩

（1.國防科學(xué)技術(shù)大學(xué)航天科學(xué)與工程學(xué)院，長沙410073；2.國防科學(xué)技術(shù)大學(xué)光電科學(xué)與工程學(xué)院，長沙410073）

視覺里程計中的相機(jī)姿態(tài)和高度實時測量方法*

曹毓1*，張小虎1，馮瑩2

（1.國防科學(xué)技術(shù)大學(xué)航天科學(xué)與工程學(xué)院，長沙410073；2.國防科學(xué)技術(shù)大學(xué)光電科學(xué)與工程學(xué)院，長沙410073）

在視覺里程計的應(yīng)用中，實時準(zhǔn)確的獲得相機(jī)姿態(tài)和高度數(shù)據(jù)有助于提高視覺定位的精度。而現(xiàn)有解決方案要么成本過高，要么精度無法滿足要求，為此提出了基于路面激光掃描的相機(jī)外參數(shù)實時測量方法。該方法將兩臺二維激光掃描儀相互正交安裝且向下掃描，對獲得的沿著兩個方向的路面掃描線使用RANSAC算法估計出直線方程，根據(jù)兩直線方程求得道路平面方程，并以該平面為參考獲得相機(jī)相對路面的姿態(tài)和高度數(shù)據(jù)。室內(nèi)實驗結(jié)果表明:靜態(tài)條件下對姿態(tài)的測量誤差最大約0.1°，高度測量誤差最大6 mm；室外動態(tài)實驗結(jié)果表明:與傳統(tǒng)的慣性測量方法不同，相機(jī)外參數(shù)測量結(jié)果不受車輛加減速運動的影響，且其動態(tài)姿態(tài)測量精度明顯高于精度為1°的慣性測量系統(tǒng)。由于該方法獲得的姿態(tài)和高度數(shù)據(jù)是以道路平面為參考基準(zhǔn)，尤其適用于單目視覺里程計中以輔助提高定位精度。

計算機(jī)視覺；視覺里程計；激光掃描；姿態(tài)測量；高度測量

EEACC:7230doi:10.3969/j.issn.1004-1699.2015.09.015

使用相機(jī)實現(xiàn)運動平臺位置解算的方法稱為視覺定位法或視覺測程法［1-2］（Visual Odometry），相應(yīng)產(chǎn)品稱為視覺里程計［3-6］（Visual Odometer）。它比慣性導(dǎo)航系統(tǒng)成本低廉，更利于推廣應(yīng)用；與碼盤里程計相比不會因車輪打滑而引入定位誤差；此外視覺里程計還具有結(jié)構(gòu)簡單、便于系統(tǒng)集成等優(yōu)點。然而實際應(yīng)用中發(fā)現(xiàn):視覺定位的精度敏感于相機(jī)內(nèi)外參數(shù)標(biāo)定的效果［3］，尤其是平臺運動過程中難免的震動和顛簸會使得相機(jī)外參（姿態(tài)和高度）隨機(jī)變化，這會導(dǎo)致顯著的定位誤差。因此相機(jī)外參的實時精確獲取是視覺里程計實現(xiàn)高精度定位的前提，這也是當(dāng)前視覺里程計研究的熱點問題之一:文獻(xiàn)［7］提出了MVO/SINS導(dǎo)航方案，將單目視覺里程計和慣性導(dǎo)航系統(tǒng)組合使用，得到了1 421 m行駛距離內(nèi)誤差4%的定位結(jié)果；此外文獻(xiàn)［8］提出了基于全景相機(jī)和“視覺羅盤”原理的單目視覺定位方法，該方法通過提取和跟蹤全景圖像中標(biāo)志特征實現(xiàn)運動平臺航向的實時校正，提高了定位精度。

為提高視覺定位精度，目前最常使用的相機(jī)姿態(tài)測量手段為慣性測量系統(tǒng)［7］或者GPS［9］，前者價格昂貴且工作前準(zhǔn)備時間較長，后者易受衛(wèi)星信號干擾或遮擋的影響。此外，在動態(tài)實時獲得相機(jī)高度的研究方面，目前還未見有效的測量方法的報道。為此，本文提出了基于正交二維激光掃描的動態(tài)條件下相機(jī)姿態(tài)和高度測量方法［10-11］。該方法以道路平面法線方向而非傳統(tǒng)的重力場方向作為姿態(tài)測量參考基準(zhǔn)，不但得到的姿態(tài)數(shù)據(jù)更有利于提高視覺定位的精度，而且可同時求解出相機(jī)的高度。室內(nèi)外實驗結(jié)果均表明:本文方法在保證測量精度的同時，具有測量頻率較高、環(huán)境適應(yīng)性強(qiáng)、不受車輛加速影響等優(yōu)點。

1　相機(jī)姿態(tài)和高度測量原理

圖1為基于正交二維激光掃描的相機(jī)姿態(tài)和高度測量方法原理示意圖。圖中X-Y-Z為掃描儀系統(tǒng)坐標(biāo)系CS-scanner，X軸和Y軸平行于路面，Z軸垂直于路面且向上為正向。兩臺二維激光掃描儀S1和S2相互正交放置且掃描平面平行于Z軸向下掃描路面。P為相機(jī)的安裝位置點，p點為P在道路平面上的投影點，顯然線段Pp垂直于道路平面，其長度即為相機(jī)的高度。在一個掃描周期內(nèi)由掃描儀S1和S2可得到兩條路面掃描線，利用RANSAC算法對其實施直線估計，分別得到對應(yīng)的直線L1和L2的表達(dá)式方程。由于L1和L2為路面剖面直線，根據(jù)L1和L2的方程可得道路平面在X-Y-Z坐標(biāo)系下的表達(dá)式方程。在獲得道路平面方程后，以道路平面的法線方向為參考可獲得相機(jī)姿態(tài)，通過計算P點到道路平面的距離可獲得相機(jī)高度。在一般的應(yīng)用背景下，即使路面局部存在破損，其整體平坦的特性也不會改變，這為采用RANSAC算法提取路面剖面直線方法的可行性提供了保障。

基于正交二維激光掃描的相機(jī)姿態(tài)和高度測量方法的流程如圖2所示。

圖1　相機(jī)姿態(tài)和高度測量原理示意圖

圖2　相機(jī)姿態(tài)和高度測量方法流程圖

2　姿態(tài)和高度解算公式

由于本文方法是以道路平面作為參考基準(zhǔn)來獲取相機(jī)外參數(shù)據(jù)，因此首先需要由兩條垂直相交的路面剖面直線得到道路平面的表達(dá)式方程，再在此基礎(chǔ)上推導(dǎo)出相機(jī)的姿態(tài)和高度計算公式。

2.1道路平面方程的推導(dǎo)

假設(shè)經(jīng)RANSAC算法獲得的道面剖線方程分別為:

其中系數(shù)a1，b1，c1，a2，b2，c2已通過RANSAC算法估計得到。

由于直線L1和L2是掃描儀S1和S2的掃描平面與道面的交線，因此在理想情況下根據(jù)直線L1和L2的方程可得道路平面的表達(dá)式方程。然而由于RANSAC算法的估計誤差和掃描儀測距誤差等因素的影響，L1和L2往往為異面直線。此時可令道路平面通過二者公垂線段的中點，公垂線方向為平面法線方向，進(jìn)而解出平面方程。L1和L2的公垂線方程可通過異面直線的公垂線公式求得［12］。

設(shè)異面直線L1和L2的方程分別為:

則公垂線LC的方程為:

令LC與L1和L2交點分別為M和N，其坐標(biāo)為:

在式（2）～式（4）中:

為利用異面直線的公垂線定理，將式（1）中直線L1和L2的方程改寫為:

對比式（2）和式（6），可得t1和t2分別為:

結(jié)合式（3），可得直線L1和L2的公垂線LC的方程為:

LC與L1和L2的交點M和N的坐標(biāo)為:

令道路平面通過異面直線L1和L2的公垂線段的中點。由式（9）可得到公垂線段MN中點K的坐標(biāo)為:

又有公垂線的方向向量｛A，B，C｝為道路平面的法線向量，可確定掃描儀系統(tǒng)坐標(biāo)系CS-scanner下（其定義如圖1的X-Y-Z所示）道路平面方程（點法式形式表示）如下:

2.2相機(jī)姿態(tài)的解算

相機(jī)姿態(tài)角是以掃描儀系統(tǒng)坐標(biāo)系CS-scanner下道路平面的法線向量為參考基準(zhǔn)獲得的。不妨令路面坐標(biāo)系為CG-plane，且該坐標(biāo)系的原點與CS-scanner的原點重合。CG-plane與通常意義下的世界坐標(biāo)系CG不同，主要區(qū)別在于CG-plane中Z軸垂直于道路平面，而CG中Z軸方向一般與重力加速度方向重合。兩個坐標(biāo)系的差別視道路平面在世界坐標(biāo)系CG下的傾斜程度而定，當(dāng)?shù)烂嫠綍r二者無區(qū)別。令:

CG-plane=RGS-scannerCS-scanner（12）

其中RGS-scanner為掃描儀系統(tǒng)坐標(biāo)系到路面坐標(biāo)系的變換矩陣，坐標(biāo)軸旋轉(zhuǎn)次序約定為X→Y→Z，不考慮相機(jī)航向角，RGS-scanner可表示為:

其中φ和θ分別為相機(jī)的俯仰角和橫滾角。之所以不考慮航向角，是因為視覺里程計本質(zhì)上屬于相對定位方法，雖然它可以解算出航向信息，但是長時間定位后該航向會出現(xiàn)顯著漂移［3］。因此現(xiàn)有的視覺定位法大多利用GPS等其他手段［7-9］獲得真實的航向數(shù)據(jù)以提高長時間定位的精度。

由于式（12）中未知數(shù)有兩個，只需要知道任意一個三維坐標(biāo)點Q分別在CG-plane與CS-scanner坐標(biāo)系下的坐標(biāo)，即可由式（12）和式（13）求出φ和θ。針對該問題，我們提出并采用了“單位球相交”的方法，獲得了坐標(biāo)系旋轉(zhuǎn)前后Q點的坐標(biāo)映射關(guān)系。該方法原理如圖3所示。

圖3　單位球相交法計算姿態(tài)的原理示意圖

以坐標(biāo)系CG-plane和CS-scanner的公共原點O為中心作一個半徑為1的單位半球，分別與兩個坐標(biāo)系的Z軸和ZG-plane軸相交于點Q和′，可知Q和′在各自坐標(biāo)系中的坐標(biāo)均為（0，0，-1）。只需知道點′在CS-scanner坐標(biāo)系下的坐標(biāo)值 （x，y，z）即可解出RGS-scanner，進(jìn)而求得?和θ。此時有:

由前文的分析知:ZG-plane軸與兩條路面剖線的公垂線LC平行，由式（8）知LC的方向向量為｛A，B，C｝，因此ZG-plane軸在CS-scanner坐標(biāo)系下的直線方程為:

聯(lián)立ZG-plane軸和單位半球面的方程:

最終解得點Q′在CS-scanner坐標(biāo)系下的坐標(biāo)值，公式中sign為符號函數(shù):

將式（14）的矩陣形式展開，得到:

由式（18）的1式，得:

由式（18）的2式×cosθ+3式×sinθ，得:

θ=asin（x）（20）

將式（17）的結(jié)果分別代入式（19）和式（20），得到φ和θ:

2.3相機(jī)高度的解算

設(shè)相機(jī)安裝點P在掃描儀系統(tǒng)坐標(biāo)系CS-scanner下的坐標(biāo)為（xC，yC，zC），由式（11）給出的道路平面方程結(jié)合點到平面的距離公式，可算得相機(jī)距離道路平面的高度HCam.為:

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

實驗中使用了兩臺德國SICK公司的二維激光掃描儀（型號:LMS291-S05），最大掃描頻率75 Hz，測距量程80 m。利用星網(wǎng)宇達(dá)公司的慣性姿態(tài)測量系統(tǒng)ADU5600來檢驗方法的角度測量精度，其姿態(tài)測量頻率為100 Hz，標(biāo)稱靜態(tài)姿態(tài)測量精度0.2°，動態(tài)精度1°。實驗設(shè)備架設(shè)方式如圖4所示，分別在室內(nèi)靜態(tài)和室外動態(tài)條件下測試了方法的精度。

圖4　實驗設(shè)備架設(shè)方式

3.1室內(nèi)靜態(tài)實驗

室內(nèi)靜態(tài)實驗場景如圖5所示。由于室內(nèi)的地面為水平面，此時掃描儀系統(tǒng)坐標(biāo)系CS-scanner與世界坐標(biāo)系CG可重合，為此圖中采用重力錘獲得了相機(jī)安裝點P在地面的垂直投影點p，用激光測距機(jī)測量Pp的長度即得P點真實高度值。

圖5　室內(nèi)靜態(tài)實驗場景

圖6給出了實驗中兩臺正交架設(shè)的掃描儀一次同步掃描的掃描點分布圖，圖中兩條直線為使用RANSAC算法估計得到的路面剖線L1和L2。

實驗共實施6組。為檢驗算法對高度的測量精度，使用萊卡公司DISTO-A5型激光測距機(jī)（測距精度1.5 mm）測量了每組實驗中對應(yīng)的相機(jī)安裝點P的實際高度值。實驗詳細(xì)結(jié)果如表1所示。表1中每組數(shù)據(jù)均為一次實驗中靜態(tài)多次測量平均值，可見6組實驗中對姿態(tài)的測量誤差最大約0.1°，高度測量誤差最大6 mm。

實驗結(jié)果誤差的主要原因應(yīng)該是掃描儀的安裝標(biāo)定精度不夠所致。此外設(shè)備自身精度限制也是導(dǎo)致存在誤差的重要因素:由于ADU5600姿態(tài)測量系統(tǒng)的靜態(tài)精度為0.2°，有必要使用更高精度的設(shè)備來進(jìn)一步檢驗方法的姿態(tài)測量精度。

圖6　掃描點分布及RANSAC估計出的直線

表1　靜態(tài)實驗結(jié)果數(shù)據(jù)

3.2室外動態(tài)實驗

為驗證方法在實際應(yīng)用中的效果，選擇三輪車作為移動搭載平臺，在一平坦開闊場地實施室外的動態(tài)實驗，實驗設(shè)備安裝方式如圖7所示。

圖7　室外實驗設(shè)備實物

為驗證車輛顛簸狀態(tài)下本文方法的姿態(tài)和高度測量精度，在路面設(shè)置了5個障礙物，其相對位置擺放如圖8所示。圖8中1～4號障礙物放置于三輪車的后輪行駛軌跡上，用以產(chǎn)生車輛沿橫滾方向的顛簸；5號障礙物置于前輪行駛軌跡上，用以產(chǎn)生車輛沿俯仰方向的顛簸。

圖8　室外動態(tài)實驗場景

實驗中得到的車輛俯仰角、橫滾角及相機(jī)高度變化曲線分別如圖9～圖11所示?？擅黠@看出兩種方法獲得的姿態(tài)測量結(jié)果曲線的差別。

圖9　俯仰角變化曲線

圖10　橫滾角變化曲線

圖11　相機(jī)高度變化曲線

圖9、圖10和圖11中的數(shù)字1～5分別指示了對應(yīng)序號的路面障礙物引發(fā)車輛顛簸所導(dǎo)致的姿態(tài)和高度曲線的變化。圖9中字母A到E代表車輛的五個狀態(tài)階段:A為車輛靜止、駕駛員在車下階段，B為車輛靜止、駕駛員在車上階段，C為車輛運動階段，D為車輛停止、駕駛員在車上階段，E為車輛停止、駕駛員在車下階段?？梢娙说纳舷萝嚲鶎?yīng)了車輛俯仰角的變化。由圖9中的階段E發(fā)現(xiàn)，車輛雖已處于靜止?fàn)顟B(tài)，但是ADU的俯仰角測量結(jié)果明顯出現(xiàn)了向上漂移，這是由于車輛突然減速停止導(dǎo)致姿態(tài)傳感器出現(xiàn)測量誤差引起的。相反的，本文方法測量結(jié)果不受車輛運動狀態(tài)影響。同時觀察圖9、圖10和圖11的曲線波動情況發(fā)現(xiàn):車輛橫滾角變化主要受1～4號障礙物的影響，而5號障礙物主要影響了車輛俯仰角的變化；相機(jī)的高度在車輛每經(jīng)過障礙物時均發(fā)生較大變化。因此，以上實驗結(jié)果均與實際的實驗情況吻合。

由于實驗中用以對比驗證精度的姿態(tài)傳感器本身精度較低，無法準(zhǔn)確定量給出本文方法的姿態(tài)測量精度，但是由結(jié)果數(shù)據(jù)結(jié)合實驗條件不難得出結(jié)論，本方法的姿態(tài)測量精度明顯高于ADU5600姿態(tài)測量系統(tǒng)。此外，由于作者未尋找到能實現(xiàn)動態(tài)條件下平臺高度測量的有效方法或設(shè)備，因此方法在動態(tài)條件下的高度解算精度暫無法得到驗證。

4　結(jié)論

本文研究了基于正交二維激光掃描的動態(tài)平臺姿態(tài)和高度測量方法。首先詳細(xì)分析了該方法的原理和理論公式推導(dǎo)過程，然后通過實驗對該方法的測量精度加以驗證。室內(nèi)外的實驗結(jié)果表明:該方法對相機(jī)姿態(tài)和高度的測量精度高、動態(tài)響應(yīng)速度快，測量結(jié)果不受車輛加減速運動的影響，其姿態(tài)測量精度明顯高于動態(tài)精度為1°的ADU5600姿態(tài)測量系統(tǒng)。

然而，本文工作在如下幾個方面還有待改進(jìn)和進(jìn)一步完善，具體如下:

首先，受硬件水平及實驗測試條件等限制，實驗中并沒有準(zhǔn)確得到本文方法精度的定量結(jié)果，進(jìn)一步的高精度實驗驗證工作有待深入開展。比如:對掃描儀的安裝實施更為精確的標(biāo)定，換用更高精度的慣性姿態(tài)測量設(shè)備重做實驗，等等。

其次，算法暫時還沒有做到完全實時化。本文方法的數(shù)據(jù)處理速度與RANSAC算法的迭代次數(shù)設(shè)置有關(guān)。對于開闊路面，一般選擇20～30次迭代即可滿足精度要求，此時對于一般配置的計算機(jī)（酷睿雙核2.6 GHz CPU，內(nèi)存2 GB），在MATLAB程序環(huán)境下的實時測量頻率約為60 Hz～65 Hz，稍低于掃描儀75 Hz的數(shù)據(jù)采集頻率。下一步計劃將算法程序在C語言環(huán)境下編寫或硬件化實現(xiàn)，可大幅提高處理速度，并滿足一般場合下對測量帶寬的需求。

最后，本文方法使用了兩臺二維激光掃描儀，這使得系統(tǒng)結(jié)構(gòu)較為笨重，且成本也相對較高（但即使這樣，相對于慣性測量系統(tǒng)仍然具有明顯的價格優(yōu)勢）。我們正在研究只使用單臺二維激光掃描儀實現(xiàn)同樣測量效果的方案可行性，該工作現(xiàn)已取得初步進(jìn)展，預(yù)期可使方法向?qū)嵱没较蜻~進(jìn)一大步。

［1］ Nister D，Naroditsky O，Bergen J.Visual Odometry［C］//Proc Int Conf Computer Vision and Pattern Recognition.2004:652-659.

［2］ Friedrich Fraundorfer，Davide Scaramuzza.Visual Odometry Part I: TheFirst30YearsandFundamentals［J］.IEEERBOTICS&AutomationMagazine，2011，11:80-92.

［3］ 曹毓，馮瑩，趙立雙，等.相機(jī)姿態(tài)安裝誤差對單目視覺定位精度的影響［J］.傳感器與微系統(tǒng)，2012，31（12）:23-26.

［4］ 王景川，陳衛(wèi)東，胡仕煜，等.基于近紅外視覺的機(jī)器人室外定位系統(tǒng)［J］.機(jī)器人，2010，32（1）:97-103.

［5］ 潘良晨，陳衛(wèi)東.室內(nèi)移動機(jī)器人的視覺定位方法研究［J］.機(jī)器人，2008，28（5）:504-509.

［6］ 張霄漢，陳小平，李嘉玲，等.一種基于視覺的步行機(jī)器人Monte Carlo自定位系統(tǒng)［J］.機(jī)器人，2006，28（4）:415-421

［7］ Feng Guohu，Wu Wenqi，Cao Juliang.Algorithm for Monocular Visual Odometry/SINS Integrated Navigation［J］.Journal of Chinese Inertial Technology，2011，19（3）:302-306.

［8］ Davide Scaramuzza，Siegwart R.Monocular Omnidirectional Visual Odometry for Outdoor Ground Vehicles［J］.ICVS，2008:206-215.

［9］ 陳艷，張漫，馬文強(qiáng)，等.基于GPS和機(jī)器視覺的組合導(dǎo)航定位方法［J］.農(nóng)業(yè)工程學(xué)報，2011，27（3）:126-130.

［10］曹毓，馮瑩，楊云濤，等.RANSAC直線估計方法在路面三維點云優(yōu)化中的應(yīng)用［J］.紅外與激光工程，2012，41（11）:3108-3112.

［11］馮瑩，曹毓，雷兵，等.一種動態(tài)平臺姿態(tài)和高度測量方法:中國，CN201310038521.1［P］.2013-01-31/2013-05-08.

［12］劉程熙，彭家寅.異面直線公垂線方程的求法［J］.內(nèi)江師范學(xué)院學(xué)報，2009，24（8）:90-92.

曹毓（1982-），男，安徽五河人，在站博士后，工程師.研究方向為光學(xué)傳感技術(shù)，計算成像技術(shù)，m15273136488@ 163.com。

Real-Time Measurement of Camera Attitude and Height in Visual Odometer*

CAO Yu1*，ZHANG Xiaohu1，F(xiàn)ENG Ying2
（1.College of Aerospace Science and Engineering，National University of Defense Technology，Changsha 410073，China；2.College of Optoelectronic Science and Engineering，National University of Defense Technology，Changsha 410073，China）

In the application of visual odometer，acquiring the high-precision attitude and height of camera in real time helps to improve the visual positioning accuracy.But existing solutions are either expensive or low in precision，so we bring forward an real-time method to measure the camera's external parameters based on the laser scanning of road surface.Two 2D laser scanner orthogonal installed and downward scan，then the linear equation was estimated using the RANSAC algorithm.After that，road plane equation is obtained，which is the reference of the camera's attitude and altitude data.the indoor experiment results showed that:in the static conditions，the measurement error of the attitude is about 0.1 degree，maximum height measurement error is about 6 mm；the outdoor dynamic experimental results showed that:different from the traditional method of inertial measurement，camera external parameters measurement results are not affected by the vehicle deceleration influence，and the attitude measurement precision is significantly higher than the inertial measurement system which has accuracy of 1 degree.Owing to obtaining the attitude and altitude data as the road plane for reference，the method is especially suitable for assist monocular visual odometry to improving the positioning accuracy.

computer vision；visual odometer；laser scanning；attitude measurement；height measurement

TP393

A

1004-1699（2015）09-1354-07

項目來源:博士后基金項目（2014m562649）

2015-05-05修改日期:2015-07-09