基于單目視覺的位置姿態(tài)測量系統(tǒng)精度檢校方法

朱莊生，袁學(xué)忠

（北京航空航天大學(xué) 慣性技術(shù)重點(diǎn)實(shí)驗(yàn)室 新型慣性儀表與導(dǎo)航系統(tǒng)技術(shù)國防重點(diǎn)學(xué)科實(shí)驗(yàn)室，北京 100191）

航空遙感在高精度基礎(chǔ)測繪、軍事偵察、環(huán)境監(jiān)測等領(lǐng)域都起著重要的作用，航空遙感中需要高精度位置姿態(tài)測量系統(tǒng)（Position and Orientation System,POS）為載荷提供實(shí)時、連續(xù)、高精度的位置、姿態(tài)信息進(jìn)行運(yùn)動補(bǔ)償，實(shí)現(xiàn)遙感圖像像質(zhì)的退化抑制[1]。

高精度POS是航空遙感系統(tǒng)的關(guān)鍵設(shè)備之一，為解決高精度POS實(shí)時精度[2]（POS AV610，航向精度0.03°，俯仰精度、橫滾精度0.005°）的檢校問題，目前學(xué)者們提出的方法主要采用空中三角檢校法[3]，利用航空測繪相機(jī)通過光束法區(qū)域網(wǎng)平差計算相機(jī)曝光時刻高精度位置和姿態(tài)數(shù)據(jù)，并用其檢校POS的位置和姿態(tài)精度。由于該方法受天氣環(huán)境、飛行平臺的平穩(wěn)度等隨機(jī)因素的影響，無法作為檢?；鶞?zhǔn)，主要用于定量分析，而且實(shí)現(xiàn)成本高；由于星敏感器可以實(shí)現(xiàn)高精度姿態(tài)測量[4]（航向和俯仰精度 0.0003°～0.003°，橫滾精度 0.002°～0.03°），其精度與高精度 POS相當(dāng)，但星敏感器多用于星載環(huán)境，在機(jī)載和地面環(huán)境使用中，由于受到大氣層環(huán)境的影響以及地面光污染的影響，姿態(tài)精度會進(jìn)一步降低；偽衛(wèi)星檢校方法[5]是通過在一定空間范圍內(nèi)布置若干模擬衛(wèi)星信號發(fā)射器，模擬全球定位系統(tǒng)的檢校方法，該方法主要提供位置精度檢校，姿態(tài)的檢校實(shí)現(xiàn)困難且精度低。

隨著計算機(jī)視覺技術(shù)的快速發(fā)展，視覺測量技術(shù)在工業(yè)應(yīng)用中發(fā)揮了重要作用。該技術(shù)利用三維空間與二維圖像的映射關(guān)系，通過相機(jī)拍攝的圖像計算被測目標(biāo)的幾何尺寸、空間位置及姿態(tài)。該測量方法已成為工業(yè)中位姿測量研究的熱點(diǎn)。

單目視覺測量是指利用一臺相機(jī)獲取一幅圖片實(shí)現(xiàn)對被測目標(biāo)的三維測量，單目系統(tǒng)結(jié)構(gòu)簡單，同時避免了立體視覺中圖像點(diǎn)匹配困難以及需要對測量系統(tǒng)進(jìn)行現(xiàn)場校準(zhǔn)等不足，近幾年關(guān)于這方面的研究非?；钴S，目前在零件檢測、形變測量、裝備裝配、交會對接、組合導(dǎo)航等方面都有廣泛應(yīng)用[6-7]。文獻(xiàn)[8]針對大型結(jié)構(gòu)變形測量，優(yōu)化了正交迭代算法，使用單目視覺方法在3 m距離，實(shí)現(xiàn)姿態(tài)角0.006°的測量精度；文獻(xiàn)[9]針對物體三維測量，提出基于多視圖三維重構(gòu)的位姿求解方法，配合高精度點(diǎn)陣靶標(biāo)，使用單目視覺在1 m測量距離，實(shí)現(xiàn)橫滾角0.0003°的測量精度；兩篇文獻(xiàn)都側(cè)重于視覺算法的研究，使用設(shè)計的靶標(biāo)代替了被測物。文獻(xiàn)[10]雖然針對大型裝備裝配，設(shè)計輔助靶標(biāo)及安裝基座，利用精確可控的運(yùn)動裝置標(biāo)定輔助靶標(biāo)及安裝基座之間的相對關(guān)系，但文獻(xiàn)在安裝上將輔助靶標(biāo)和運(yùn)動裝置坐標(biāo)系重合，忽略了安裝誤差，最終使用單目視覺在3 m測量距離，實(shí)現(xiàn)姿態(tài)角0.01°的測量精度。

目前大量文獻(xiàn)關(guān)注于使用高精度合作靶標(biāo)提高視覺測量的精度，而忽略了靶標(biāo)和被測物之間的位姿轉(zhuǎn)換問題。實(shí)際應(yīng)用中，靶標(biāo)和被測物不能完全等同，兩者坐標(biāo)系轉(zhuǎn)換誤差嚴(yán)重影響視覺測量的精度，因此本文提出單目視覺檢校方案，配備高精度合作靶標(biāo)，并提出聯(lián)合標(biāo)定方法標(biāo)定靶標(biāo)和 IMU之間坐標(biāo)轉(zhuǎn)換矩陣，為高精度POS提供姿態(tài)精度檢校。

1 高精度視覺測量對靶標(biāo)精度指標(biāo)需求分析

高精度視覺測量中影響姿態(tài)測量精度的因素主要有相機(jī)分辨率、相機(jī)內(nèi)參標(biāo)定誤差、特征點(diǎn)中心提取誤差、測量距離、靶標(biāo)尺寸和靶標(biāo)加工精度[11]。在相機(jī)視場確定時，相機(jī)分辨率取最大值；相機(jī)內(nèi)參標(biāo)定和特征點(diǎn)中心提取目前已有成熟算法[12]；當(dāng)測量距離和靶標(biāo)尺寸恒定且最優(yōu)的情況下，靶標(biāo)加工精度決定了視覺測量的最終精度。下面針對視覺測量的姿態(tài)精度要求，對靶標(biāo)的加工精度需求進(jìn)行分析。

式(1)為相機(jī)小孔成像模型[9]：

式中，u、v為空間特征點(diǎn)在圖片投影點(diǎn)的像素坐標(biāo)，u0、v0為像素坐標(biāo)系原點(diǎn)，s為尺度因子，f為鏡頭焦距，ddxy、為像元在x、y方向的物理尺寸，R、T為被測物相對相機(jī)的旋轉(zhuǎn)平移關(guān)系，為靶標(biāo)特征點(diǎn)在靶標(biāo)坐標(biāo)系的三維坐標(biāo)，式(1)展開得：

方向余弦矩陣按繞相機(jī)坐標(biāo)系 X、Y、Z軸分別旋轉(zhuǎn)α、β、γ角度構(gòu)成，將方向余弦矩陣代入式(2)，并求歐拉角對靶標(biāo)的偏導(dǎo)數(shù)，可以得到歐拉角和靶標(biāo)加工精度的關(guān)系。三個歐拉角互相耦合，解算的關(guān)系式復(fù)雜，且誤差與角度關(guān)系不明顯，因?yàn)橄鄼C(jī)橫滾角測量精度最高，所以只對橫滾角誤差和靶標(biāo)加工精度之間的關(guān)系進(jìn)行分析，取俯仰、航向角αβ==0°，橫滾角γ=10°；為簡化書寫預(yù)先消去代入角度參數(shù)后值為0的項(xiàng)得：

根據(jù)國產(chǎn)POS產(chǎn)品尺寸168 mm×187 mm×135 mm，靶標(biāo)尺寸設(shè)計為150 mm×150 mm；由視場可確定相機(jī)成像面尺寸約為6.5 mm×6.5 mm；為了實(shí)現(xiàn)對高精度POS的檢校，按照檢?；鶞?zhǔn)比檢校對象優(yōu)于一個數(shù)量級的原則，其姿態(tài)檢校系統(tǒng)檢校精度需優(yōu)于 1.8″。由式(5)可知，靶標(biāo)精度必須優(yōu)于2.4 μm；本文選用加工精度1 μm的標(biāo)準(zhǔn)halcon點(diǎn)陣靶標(biāo)。

圖1 靶標(biāo)實(shí)物圖Fig.1 Target physical picture

2 相機(jī)與POS設(shè)備的時間同步

高頻輸出是POS的特點(diǎn)之一，POS正常工作頻率為200Hz，POS通過全球定位系統(tǒng)（Global Positioning System, GPS）提供的秒脈沖（Pulses Per Second, PPS）基準(zhǔn)時間信號與IMU數(shù)據(jù)采集時刻進(jìn)行嚴(yán)格對準(zhǔn)實(shí)現(xiàn)時間同步，時間同步精度為5 μs[14]，POS每一包數(shù)據(jù)都會記錄對應(yīng)的GPS時間標(biāo)簽。

當(dāng)對POS進(jìn)行精度檢校時，檢校系統(tǒng)的輸出和POS輸出必須進(jìn)行時間同步。當(dāng)POS以1 (°)/s的速度轉(zhuǎn)動，時間對準(zhǔn)誤差1 ms，導(dǎo)致的檢校誤差即為0.001°。因此，相機(jī)和POS之間必須進(jìn)行高精度的時間同步。

相機(jī)提供硬件觸發(fā)工作模式，即也可以利用GPS的PPS脈沖信號控制相機(jī)拍照，實(shí)現(xiàn)相機(jī)和POS的時間同步。PPS脈沖信號為1 Hz，通過分頻處理，控制相機(jī)以5 Hz頻率輸出，并通過程序記錄下相機(jī)被觸發(fā)時刻的時間。相機(jī)實(shí)際成像時刻和觸發(fā)時刻之間有個曝光時間，曝光主要影響圖片亮度，將外界光照調(diào)至最優(yōu)時，曝光時間可以設(shè)置為微秒級的恒定值，最后通過匹配時間標(biāo)簽完成數(shù)據(jù)匹配。

3 基于正交矢量和動態(tài)濾波的聯(lián)合標(biāo)定法

高精度合作靶標(biāo)配合相機(jī)實(shí)現(xiàn)了高精度姿態(tài)測量，而本文目的是獲取POS的姿態(tài)數(shù)據(jù)，POS主要由IMU、導(dǎo)航計算機(jī)和GPS天線組成，IMU為主要位姿傳感器。為了將相機(jī)測得的靶標(biāo)姿態(tài)數(shù)據(jù)轉(zhuǎn)到IMU坐標(biāo)系上，靶標(biāo)和IMU剛性固連，需要標(biāo)定靶標(biāo)坐標(biāo)系和IMU坐標(biāo)系之間的位姿轉(zhuǎn)換矩陣關(guān)系。

3.1 正交矢量法

孫長庫[15]等人利用相機(jī)的高精度姿態(tài)測量，將相機(jī)的姿態(tài)數(shù)據(jù)和IMU姿態(tài)數(shù)據(jù)進(jìn)行融合，以抑制IMU的角度漂移。在融合過程中，需要標(biāo)定出IMU和轉(zhuǎn)臺之間的相對姿態(tài)以及靶標(biāo)和轉(zhuǎn)臺之間的相對姿態(tài)，因此提出雙矢量正交和三圖快速標(biāo)定法分別標(biāo)定。

將轉(zhuǎn)臺作為目標(biāo)坐標(biāo)系，首先將靶標(biāo)和IMU都安裝在轉(zhuǎn)臺上，轉(zhuǎn)臺分別繞X、Y軸勻速轉(zhuǎn)動，此時轉(zhuǎn)臺有X、Y軸的恒定角速率，IMU同時輸出X、Y軸的角速率，轉(zhuǎn)臺轉(zhuǎn)動初始到結(jié)束時刻的姿態(tài)變化由相機(jī)拍攝靶標(biāo)獲取，靶標(biāo)姿態(tài)變化矩陣化為四元數(shù)，可提取靶標(biāo)在繞X、Y軸角速率；通過X、Y軸矢量叉乘，得到Z軸角速率?？色@取轉(zhuǎn)臺和IMU之間的姿態(tài)關(guān)系，靶標(biāo)和轉(zhuǎn)臺之間的姿態(tài)關(guān)系，但是該方法沒有位置信息。

3.2 動態(tài)濾波法

在行人導(dǎo)航和增強(qiáng)現(xiàn)實(shí)（Augmented Reality, AR）領(lǐng)域，研究人員將IMU和相機(jī)固連作為組合傳感器使用，為了融合相機(jī)和IMU的數(shù)據(jù)，該領(lǐng)域提出了基于神經(jīng)網(wǎng)絡(luò)[16]的標(biāo)定方法和基于卡爾曼濾波的標(biāo)定方法[17-20]。基于卡爾曼濾波的方法為了將地理坐標(biāo)系和靶標(biāo)坐標(biāo)系相關(guān)聯(lián)，將靶標(biāo)如圖2所示水平放置，即認(rèn)為靶標(biāo)坐標(biāo)系與地理坐標(biāo)系水平面重合，兩坐標(biāo)系之間只相差一個航向角，因此只需估計相對位置和一個航向角，導(dǎo)致標(biāo)定精度有限。

圖2 動態(tài)濾波標(biāo)定示意圖Fig.2 Dynamic filter calibration schematic

濾波狀態(tài)量為：

濾波量測量為：

3.3 聯(lián)合標(biāo)定法

針對以上兩個方法的不足，本文提出基于正交矢量和動態(tài)濾波的聯(lián)合標(biāo)定法，即先采用文獻(xiàn)[15]提出的方法標(biāo)定相對姿態(tài)，再使用擴(kuò)展卡爾曼濾波方法標(biāo)定相對位置。標(biāo)定示意圖如圖3所示。

圖3 聯(lián)合標(biāo)定法標(biāo)定示意圖Fig.3 Joint calibration method calibration diagram

① 由正交矢量法標(biāo)定相對姿態(tài)。以轉(zhuǎn)臺坐標(biāo)系B系作為參考坐標(biāo)系，轉(zhuǎn)臺分別繞X軸、Y軸勻速轉(zhuǎn)動，旋轉(zhuǎn)角速度在B系分別表示、；同時IMU輸出角速度、。正交矢量叉乘可以得到第三個矢量。為轉(zhuǎn)臺坐標(biāo)系和IMU坐標(biāo)系之間的相對姿態(tài)，則有：

文獻(xiàn)[15]提供了該部分的優(yōu)化方法。

IMU繞轉(zhuǎn)臺X軸旋轉(zhuǎn)時，相機(jī)在旋轉(zhuǎn)運(yùn)動的初始時刻和終止時刻分別采集一幅靶標(biāo)圖像，求解靶標(biāo)坐標(biāo)系w系的姿態(tài)變化量,并從中提取w系 X軸方向的單位向量 u1。同理，繞Y軸可得 u2。u1、u2矢量叉乘可解得靶標(biāo)坐標(biāo)系w系和轉(zhuǎn)臺坐標(biāo)系B系之間的相對姿態(tài)關(guān)系：

由轉(zhuǎn)臺和IMU姿態(tài)關(guān)系、靶標(biāo)和轉(zhuǎn)臺姿態(tài)關(guān)系，最終可得到靶標(biāo)和IMU之間的相對姿態(tài)關(guān)系：

② 動態(tài)濾波法（EKF）標(biāo)定相對位置。由正交矢量法標(biāo)定出相對姿態(tài)后，濾波狀態(tài)量中就可去掉這一項(xiàng)，此時濾波狀態(tài)量為：

式中各參數(shù)意義同式(6)。狀態(tài)方程由IMU輸出的線性加速度和影響IMU加速度漂移誤差的高斯白噪聲來驅(qū)動，詳見文獻(xiàn)[17]：

此時量測矩陣H為：

4 實(shí) 驗(yàn)

4.1 實(shí)驗(yàn)設(shè)備

① 相機(jī)：選用相機(jī)（如圖4左）型號為AVT gt2450，500萬像素黑白相機(jī)，成像分辨率為2448pixel×2050 pixel，最大幀頻率15 Hz，鏡頭焦距25 mm。

② POS：選用實(shí)驗(yàn)室自研的光纖POS產(chǎn)品（如圖4右），其實(shí)時航向精度0.03°，俯仰、橫滾精度0.005°。和POS AV610 精度相當(dāng)。

圖4 相機(jī)、北航POS實(shí)物圖Fig.4 Camera (left) and POS (right)

4.2 實(shí) 驗(yàn)

4.2.1 IMU-靶標(biāo)標(biāo)定實(shí)驗(yàn)

實(shí)驗(yàn)流程：

① 首先將靶標(biāo)和IMU固連，分別將 IMU的X、Y、Z軸與靶標(biāo)平面垂直安裝固定（IMU的Y軸和靶標(biāo)安裝示意圖如圖5所示）。將IMU固定在轉(zhuǎn)臺上，固定相機(jī)，使標(biāo)定過程中，靶標(biāo)始終在相機(jī)視場內(nèi)。

圖5 IMU的Y軸檢校安裝示意圖Fig.5 IMU’s Y-axis calibration installation diagram

② 按照3.3節(jié)中的①所述，操作轉(zhuǎn)臺，采集轉(zhuǎn)臺、相機(jī)、IMU的數(shù)據(jù)，對IMU和靶標(biāo)相對姿態(tài)進(jìn)行標(biāo)定，實(shí)驗(yàn)如圖6左圖所示。

③ POS工作后，將IMU和靶標(biāo)在相機(jī)視場內(nèi)做隨機(jī)運(yùn)動，采集POS和相機(jī)數(shù)據(jù)，按照3.3節(jié)中的②所述方法，標(biāo)定IMU和靶標(biāo)之間的轉(zhuǎn)換矩陣，實(shí)驗(yàn)如圖6右圖所示。

圖6 標(biāo)定系統(tǒng)實(shí)物圖Fig.6 Physical picture of calibration system

因標(biāo)定精度無法衡量，因此對視覺檢校系統(tǒng)精度進(jìn)行評判。

4.2.2 精度檢校實(shí)驗(yàn)

因?yàn)閷?shí)際測量中，視覺橫滾角測量精度比航向、俯仰精度高，因此使用相機(jī)橫滾角精度分別檢校POS的三軸姿態(tài)精度。因檢校系統(tǒng)直接輸出的是IMU相對相機(jī)的姿態(tài)，無法和POS輸出直接進(jìn)行比較，因此實(shí)驗(yàn)中比較POS的姿態(tài)變化量。

① 靜態(tài)實(shí)驗(yàn)

1）實(shí)驗(yàn)流程

將IMU和靶標(biāo)固定在大理石臺上，相機(jī)固定并拍攝靶標(biāo)，靜止測試15 min，將IMU繞Y軸轉(zhuǎn)動一個角度，靜止測試15 min。實(shí)驗(yàn)共進(jìn)行了6組。

2）數(shù)據(jù)處理

將相機(jī)兩次測試的靶標(biāo)姿態(tài)經(jīng)式(17b)轉(zhuǎn)到IMU坐標(biāo)系下，計算IMU姿態(tài)變化量即為檢校系統(tǒng)的姿態(tài)輸出，同時用POS后處理數(shù)據(jù)計算IMU的姿態(tài)變化，比較檢校系統(tǒng)和后處理數(shù)據(jù)精度。

3）數(shù)據(jù)分析

因?yàn)镻OS后處理精度（航向精度0.005°，俯仰、橫滾精度0.0025°）比實(shí)時精度高，一般用后處理數(shù)據(jù)來檢校實(shí)時數(shù)據(jù)精度。本文以POS后處理數(shù)據(jù)作為基準(zhǔn)，計算視覺檢校系統(tǒng)的RMS值來說明其精度。

圖7為視覺檢校系統(tǒng)和POS后處理數(shù)據(jù)做差的結(jié)果圖。表1為視覺檢校系統(tǒng)誤差統(tǒng)計結(jié)果。實(shí)驗(yàn)結(jié)果表明，POS后處理的航向角靜態(tài)STD優(yōu)于1.6″，視覺檢校輸出的靜態(tài)STD優(yōu)于0.8″，視覺檢校系統(tǒng)的RMS值優(yōu)于2.1″。

圖7 視覺檢校系統(tǒng)誤差Fig.7 Visual calibration system error

表1 視覺檢校系統(tǒng)精度實(shí)驗(yàn)結(jié)果Tab.1 Experiment results of visual calibration system

圖8 動態(tài)實(shí)驗(yàn)圖Fig.8 Dynamic experiment diagram

② 動態(tài)實(shí)驗(yàn)

1）實(shí)驗(yàn)流程

將POS通過工裝固定在單軸轉(zhuǎn)臺（精度0.001°），將靶標(biāo)和IMU的Z軸垂直安裝，相機(jī)從上方拍攝靶標(biāo)，檢校POS航向精度?？刂妻D(zhuǎn)臺以0.5 (°)/s的速度從0°轉(zhuǎn)到70°，再從70°轉(zhuǎn)回0°（超過70°靶標(biāo)會出相機(jī)視場）。實(shí)驗(yàn)共進(jìn)行3次，實(shí)驗(yàn)如圖8所示。

2）數(shù)據(jù)處理

將相機(jī)測量數(shù)據(jù)經(jīng)標(biāo)定出的IMU和靶標(biāo)間轉(zhuǎn)換矩陣轉(zhuǎn)到IMU坐標(biāo)系，將運(yùn)動前靜止時刻姿態(tài)設(shè)為0，計算POS和視覺檢校系統(tǒng)的姿態(tài)變化量，結(jié)果如圖9所示。圖10為圖9中方框部分放大圖。

三組實(shí)驗(yàn)共有6組動態(tài)數(shù)據(jù)，每一組動態(tài)數(shù)據(jù)每隔1 s取POS后處理數(shù)據(jù)和視覺檢校輸出，計算RMS結(jié)果，6組動態(tài)數(shù)據(jù)RMS值如表2所示。

實(shí)驗(yàn)結(jié)果表明，視覺檢校系統(tǒng)RMS優(yōu)于4.40″，可以用于POS的實(shí)時姿態(tài)精度檢校。

圖9 POS與檢校系統(tǒng)航向變化圖Fig.9 Yaw change diagram of POS and calibration system

圖10 航向局部放大圖Fig.10 Partial enlargement of yaw

表2 航向角誤差Tab.2 Yaw error

5 總 結(jié)

本文針對高精度POS的實(shí)時檢校問題，提出單目視覺檢校方法。分析了合作靶標(biāo)精度對姿態(tài)測量精度的影響，利用靶標(biāo)實(shí)現(xiàn)視覺高精度姿態(tài)測量；提出基于正交矢量和動態(tài)濾波的聯(lián)合標(biāo)定法，標(biāo)定出靶標(biāo)和IMU之間的轉(zhuǎn)換矩陣，實(shí)現(xiàn)基于單目視覺的高精度POS姿態(tài)精度的檢校。實(shí)驗(yàn)結(jié)果表明，本文提出的視覺檢校系統(tǒng)可以用于POS系統(tǒng)的實(shí)時姿態(tài)精度檢校。