一種利用全站儀和檢校場的相機/慣導(dǎo)空間標(biāo)定方法

賈 俊，朱 鋒，張小紅,2

（1.武漢大學(xué) 測繪學(xué)院，武漢 430079；2.地球空間信息技術(shù)協(xié)同創(chuàng)新中心，武漢 430079）

0 引言

隨著智能化時代的到來，自動駕駛和移動機器人領(lǐng)域蓬勃發(fā)展：如谷歌的無人駕駛汽車[1]；又如配備有大量不同類型傳感器的測量機器人，這些傳感器用以執(zhí)行環(huán)境感知和導(dǎo)航定位服務(wù)。多傳感器融合已成為了未來發(fā)展的趨勢。然而傳感器的組合，首先要面臨標(biāo)定的問題，傳統(tǒng)的全球定位系統(tǒng)（global positioning system, GPS）和慣導(dǎo)組合系統(tǒng)的標(biāo)定，可以通過測量桿臂和慣導(dǎo)初始對準(zhǔn)獲得傳感器間平移參數(shù)和旋轉(zhuǎn)參數(shù)，而視覺和慣性測量裝置（inertial measurement unit, IMU）間平移參數(shù)不能直接量取，旋轉(zhuǎn)參數(shù)計算受限于觀測條件和組合系統(tǒng)解算精度，目前常用的標(biāo)定方法通常不能獲得穩(wěn)定、可靠的標(biāo)定結(jié)果。

視覺傳感器和慣性傳感器[2]的組合具有互補的特點。通過合并來自2 個不同源的信息，可以獲得魯棒的相機位姿運動，便于視覺識別和跟蹤興趣目標(biāo)。相機和慣性傳感器的組合已經(jīng)在增強現(xiàn)實（augmented reality, AR）[3]和即時定位與地圖構(gòu)建（simultaneous localization and mapping, SLAM）[4]等領(lǐng)域中得到了廣泛的應(yīng)用。由于相機的光學(xué)中心是隨著焦距不斷改變的，通常無法直接量取相機和慣導(dǎo)之間的桿臂。為了解決標(biāo)定問題，許多學(xué)者提出了不同的解決方案：21 世紀(jì)初，文獻[5]提出將相機和 IMU 安裝在具有高分辨率編碼器的擺上來解算相機和IMU 的旋轉(zhuǎn)參數(shù)；文獻[6]利用簡單無源轉(zhuǎn)臺來標(biāo)定圖像解算相機和 IMU的平移參數(shù)；2007 年，文獻[7]采用基于卡爾曼濾波的遞歸算法，同時解算相機和IMU 的旋轉(zhuǎn)參數(shù)和平移參數(shù)；2011 年，文獻[8]利用相機和IMU時間同步的輸出，來估計傳感器間的旋轉(zhuǎn)參數(shù)和平移參數(shù)，這種自標(biāo)定方法構(gòu)成了之后標(biāo)定發(fā)展的框架；2013 年，文獻[9]發(fā)布了卡利布爾（Kalibr）工具箱，該工具箱可以提供多傳感器間的離線標(biāo)定，成為了相機和 IMU 標(biāo)定的主流工具；2017年，香港科技大學(xué)團隊在文斯（VINS）開源代碼中，提出了在線標(biāo)定方法[10]，其標(biāo)定結(jié)果為，平移參數(shù)優(yōu)于2 cm，旋轉(zhuǎn)參數(shù)優(yōu)于1°。上述校準(zhǔn)方法屬于自標(biāo)定的范疇。自標(biāo)定需要IMU 和相機輸出數(shù)據(jù)并建立復(fù)雜的數(shù)學(xué)模型，觀測條件也會影響結(jié)果的準(zhǔn)確性。此外，上述自標(biāo)定方案不適用于大尺寸和桿臂較長的相機和IMU，因為慣導(dǎo)不能充分激勵，導(dǎo)致結(jié)果不穩(wěn)定。

為了優(yōu)化上述方案，解決存在的問題，本文方案使用全站儀和標(biāo)定場來簡化標(biāo)定過程。全站儀和標(biāo)定場可以提供相機和IMU 之間的轉(zhuǎn)換關(guān)系。這種相機和慣性傳感器不需要移動的標(biāo)定方案，適用于大型IMU 和相機組合的應(yīng)用，如機器人、車載導(dǎo)航、機載導(dǎo)航等。

1 慣導(dǎo)/相機空間標(biāo)定的原理與方法

自標(biāo)定的缺點是模型復(fù)雜，不適用于大型傳感器的標(biāo)定；因此提出了新的解決方案：利用全站儀觀測慣導(dǎo)的角點，利用相機匹配到標(biāo)定場的3 維控制點，以世界坐標(biāo)系為標(biāo)定的媒介，首先構(gòu)建慣導(dǎo)坐標(biāo)系和相機坐標(biāo)系在世界坐標(biāo)系下的關(guān)系，進而得到慣導(dǎo)和相機的外參數(shù)。為了方便之后的表達，本文定義在任意時刻從，A 坐標(biāo)系轉(zhuǎn)換到B坐標(biāo)系的變換可以表示為表示相對旋轉(zhuǎn)，表示相對平移。

1.1 相機和慣導(dǎo)內(nèi)參數(shù)標(biāo)定

在標(biāo)定外參數(shù)之前，首先對相機和慣導(dǎo)的內(nèi)參數(shù)進行標(biāo)定。由于慣導(dǎo)參數(shù)尺寸在出廠前會嚴(yán)格標(biāo)定給出，而變焦相機其光心在沒有固定焦距前是可變的，因此內(nèi)參數(shù)標(biāo)定主要是對相機光心位置的確定。公認(rèn)的相機標(biāo)定方法是使用矩陣實驗室（MATLAB）[11]中的相機標(biāo)定工具箱。其原理是利用單應(yīng)性矩陣[12]構(gòu)建3 維空間和像素平面的關(guān)系，單應(yīng)性矩陣估計可表示為

式中：H 為單應(yīng)性矩陣；A 為相機的內(nèi)參數(shù)矩陣；r1、 r2表示2 個平面之間的旋轉(zhuǎn)矩陣；t 表示平移矩陣。通過估計標(biāo)定板平面在相機鏡頭前不同位置處，相對于相片平面的旋轉(zhuǎn)和平移，來得到單應(yīng)性矩陣，進一步可以獲得相機的內(nèi)參數(shù)矩陣，內(nèi)參數(shù)矩陣可表示為

式中：（ u0, v0）為像主點坐標(biāo)在像平面上的投影；fx和 fy分別為u、v 軸中的尺度因子；s 為2 個圖像軸的偏度。

1.2 世界坐標(biāo)系到相機坐標(biāo)系的相對姿態(tài)

利用相機標(biāo)定場來獲取世界坐標(biāo)系到相機坐標(biāo)系之間的旋轉(zhuǎn)和平移。本文采用 N 點透視（perspective-n-point，PNP）的方法[13]，其原理是利用標(biāo)定場中3 維控制點X 投影到相機像素平面u 多組同名點的映射關(guān)系，解算3×4 維度大小的投影矩陣P，映射關(guān)系可表示為

式中：（ R11,…, R33）為旋轉(zhuǎn)矩陳的元素；（T1,T2,T3）表示3 個軸向（前右上坐標(biāo)系）的平移；Xw、Yw、Zw為在世界坐標(biāo)系下控制點的坐標(biāo)。矩陣表示旋轉(zhuǎn)和平移，有6 個自由度，表征6 個外參數(shù)（3 個表示旋轉(zhuǎn)，3 個表示平移），有3 個或3 個以上對應(yīng)同名點，利用非線性最小二乘估計可以求解，求解過程可表示為：

式中：er為重投影誤差；ur為重投影得像素點坐標(biāo)；參數(shù)向量wc為旋轉(zhuǎn)和平移待求參數(shù)組成（和）。

1.3 世界坐標(biāo)系到慣導(dǎo)坐標(biāo)系的相對姿態(tài)

全站儀可以測量慣導(dǎo)傳感器的角點在世界坐標(biāo)系下的坐標(biāo)。利用3 維空間變換[14]得到世界坐標(biāo)系和慣導(dǎo)坐標(biāo)系的相對姿態(tài)，空間變換可表示為

式中：（Xw,wY , Zw）為標(biāo)定場中的3 維控制點坐標(biāo)；（XI,IY , ZI） 為大型IMU 傳感器的角點，通常為8 個角點構(gòu)成1 個立方體。3×4 維度的矩陣為赫爾默特（Helmert）變換，在這2 組數(shù)據(jù)之間有6 個自由度（3 個方向和3 個平移）。由于式（7）中的等式并不是線性的，需要使用非線性最小二乘和3 個或更多的對應(yīng)點來解算，可表示為：

式中：H 為赫爾默特（Helmert）變換； X ′為仿射變換后的估計值；參數(shù)向量 Iw由2 個坐標(biāo)系的旋轉(zhuǎn)和平移參數(shù)組成（）即最終得到的結(jié)果。

1.4 慣導(dǎo)坐標(biāo)系到相機坐標(biāo)系的相對姿態(tài)

結(jié)合1.3 節(jié)中的IMU 坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換關(guān)系（）以及1.2 節(jié)中世界到相機坐標(biāo)系的轉(zhuǎn)換關(guān)系（），可以進一步得到從IMU 到相機坐標(biāo)系的轉(zhuǎn)換關(guān)系（），旋轉(zhuǎn)平移變換可表示為：

2 標(biāo)定流程與精度分析方法

2.1 標(biāo)定流程

第1 步，雙目自標(biāo)定。本文采用棋盤格玻璃板作為標(biāo)定板，借助MATLAB 的雙目標(biāo)定工具箱，獲得2 個相機的內(nèi)參數(shù)和相對姿態(tài)，要求利用脈沖信號控制2 臺相機，以相同頻率拍攝相片；同時移動標(biāo)定板，使其處于不同的位置和角度。

第2 步，全站儀測量標(biāo)定場靶標(biāo)點。標(biāo)定場的靶標(biāo)點坐標(biāo)是由精密儀器測量的，為了驗證其精度，本文選取了20 多個靶標(biāo)點作為檢核點，利用毫米級誤差的全站儀進行測量，估計其誤差。

第3 步，全站儀測量慣導(dǎo)設(shè)備角點。標(biāo)定針對的是大型慣導(dǎo)，其角點參數(shù)在慣導(dǎo)坐標(biāo)系下為已知。為了獲得其在世界坐標(biāo)系下的坐標(biāo)，在慣導(dǎo)的角點貼上反射片，利用全站儀測得其坐標(biāo)。

第4 步，相機攝像匹配靶標(biāo)點。1.2 節(jié)指出，需要得到3 維控制點在相機影像中同名點的坐標(biāo)。靶標(biāo)點圓心為3 維控制點，且靶標(biāo)點都有明確的編號。利用霍夫變換[15]提取圓心的算法，即可以得到靶標(biāo)點的像素坐標(biāo)，接著利用編號與對應(yīng)的3 維控制點進行匹配。

第5 步，標(biāo)定參數(shù)解算。按照章節(jié)1 中的原理，利用觀測得到的結(jié)果進行標(biāo)定參數(shù)解算。求得到旋轉(zhuǎn)和平移參數(shù)后，按照內(nèi)外符合精度評價結(jié)果，其中PNP 的原理已在1.2 節(jié)中進行了介紹；采用多次實驗結(jié)果的標(biāo)準(zhǔn)差（standard deviation,STD）作為評價內(nèi)符合精度的重要標(biāo)準(zhǔn)。標(biāo)定方案的流程如圖1 所示。

2.2 精度評定方法

好的標(biāo)定參數(shù)結(jié)果對于多傳感器融合的應(yīng)用至關(guān)重要。自標(biāo)定由于外部信息較少，僅通過輸出結(jié)果的均值和標(biāo)準(zhǔn)差很難評定自身的精度。由于有標(biāo)定場和全站儀的輔助，可有多個標(biāo)定結(jié)果的評定方法。

圖1 標(biāo)定方案流程

評定方法1：利用已知的IMU 尺寸參數(shù)。使用全站儀和自粘反射片來測量IMU 的角點坐標(biāo)。通過測量的角點解算的IMU 長、寬、高和實際傳感器的長、寬、高參數(shù)進行比較，來評價全站儀的測量精度。

評定方法 2：選擇標(biāo)定場的靶標(biāo)點作為檢查點，利用全站儀測量來獲取靶標(biāo)點十字絲中心的3 維控制點坐標(biāo)。將全站儀測得的檢查點坐標(biāo)與標(biāo)定場預(yù)先給定的精確控制點坐標(biāo)進行對比，檢核標(biāo)定場中控制點的精度。

評定方法4：為了更好地評定相機和慣導(dǎo)之間的旋轉(zhuǎn)，將旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角進行分析，標(biāo)準(zhǔn)的轉(zhuǎn)換順序是Z→X→Y。標(biāo)定得到左右相機與IMU傳感器之間的旋轉(zhuǎn)參數(shù)和平移參數(shù)后，計算結(jié)果的STD，用STD 來評價結(jié)果的穩(wěn)定性。

評定方法5：利用標(biāo)定結(jié)果計算標(biāo)定的左右相機之間的基線長度，與用MATLAB 進行雙目相機標(biāo)定得到的基線長度做對比。同時也可以通過比較，直接用卷尺測量相機與IMU 之間的桿臂來驗證標(biāo)定平移參數(shù)的準(zhǔn)確性。

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

3.1 實驗設(shè)備條件

雙目相機采用的是加拿大普安特-格雷（Point Grey）公司的 Grasshopper3U3 系列 GS3-U3-28S5M-C 的工業(yè)相機，以及FA1 英寸系列長通道HC1605A 鏡頭。相片的分辨率是1 920×1 200 個像素，焦距大約是16 mm，1 個像素的大小為4.54 μm。慣性傳感器IMU 為加拿大諾瓦泰爾（Novatel）系列的SPAN—FSAS 高精度捷聯(lián)慣導(dǎo)系統(tǒng)，它的慣導(dǎo)坐標(biāo)系中心在它的IMU 幾何中心處，尺寸參數(shù)出廠時嚴(yán)格標(biāo)定。相機、IMU 和全站儀如圖2、圖3所示。

圖2 雙目相機和慣導(dǎo)

圖3 實驗全站儀

全站儀采用天宇CTS 型號的免棱鏡全站儀，測點精度在毫米級。

標(biāo)定場位于中國湖北省武漢市洪山區(qū)武漢大學(xué)測繪學(xué)院。標(biāo)定場內(nèi)建立了精確的坐標(biāo)系，每個靶標(biāo)點十字絲中心的3 維控制點坐標(biāo)為已知。靶標(biāo)點和相機拍攝影像如圖4、圖5 所示。

圖4 靶標(biāo)點示意圖

圖5 標(biāo)定拍攝相片

3.2 實驗結(jié)果分析

通過大量的標(biāo)定實驗，驗證了該標(biāo)定方案的可行性和可靠性。為了更好地反映此方案的準(zhǔn)確性，對校準(zhǔn)結(jié)果進行了評估。

第1 步，驗證全站儀的測量精度。使用全站儀測量IMU 的角點，計算IMU 的長、寬、高，并將測量結(jié)果與IMU 出廠參數(shù)進行比較，評價全站儀的測量精度，如圖6 所示。

通過幾組實驗對比，可以看出全站儀的測量精度在毫米級，與全站儀規(guī)定的測量精度一致。

圖6 IMU 參數(shù)與全站儀測量結(jié)果差值

第2 步，驗證全站儀標(biāo)定場的控制點精度。考慮到隨著時間的推移以及其他外界因素，會影響控制點的準(zhǔn)確性，選取了20 個控制點作為檢驗點。將檢查點測量得到的坐標(biāo)與標(biāo)定場給出的坐標(biāo)進行比較，誤差分析如圖7 所示。

圖7 檢核點坐標(biāo)與量測結(jié)果較差

從圖7 可以看出，檢查點與控制點坐標(biāo)之間的誤差在毫米級?？紤]到全站儀的測量誤差，可以證明控制點的坐標(biāo)是可靠的。

第3 步，驗證得到的相機坐標(biāo)系和世界坐標(biāo)系外參數(shù)的準(zhǔn)確性。將3 維點重投影成像素坐標(biāo)，并將計算出的像素坐標(biāo)與觀測值做比較，實驗中1 個像素的大小是4.54 μm×4.54 μm，左右相機重投影誤差如圖8 所示。

圖8 重投影誤差分析

從圖8 可以看出，重投影的精度基本上在2 個像素以內(nèi)，這證明了結(jié)果的可靠性。

第4 步，驗證該標(biāo)定方案結(jié)果的準(zhǔn)確性。為了更好地表達旋轉(zhuǎn)的穩(wěn)定性，選擇將不同組之間標(biāo)定的旋轉(zhuǎn)矩陣與其中1 組旋轉(zhuǎn)矩陣的轉(zhuǎn)置相乘并轉(zhuǎn)換成歐拉角顯示。這種表示方法的好處是更加直觀，而且在大多數(shù)相機和慣導(dǎo)組合應(yīng)用場景中，相機和慣導(dǎo)的坐標(biāo)系在俯仰角上有90°的旋轉(zhuǎn)，如果直接轉(zhuǎn)成歐拉角，會出現(xiàn)萬向鎖的問題；而選擇旋轉(zhuǎn)矩陣與另1 個旋轉(zhuǎn)矩陣的轉(zhuǎn)置相乘，則可以很好地規(guī)避這個問題。左右相機平移參數(shù)和旋轉(zhuǎn)參數(shù)標(biāo)定結(jié)果如圖9 所示。

圖9 多次實驗結(jié)果標(biāo)定結(jié)果

從圖9 可以看出，旋轉(zhuǎn)和平移非常穩(wěn)定的標(biāo)定結(jié)果，旋轉(zhuǎn)參數(shù)的STD 在0.5°左右，平移參數(shù)的STD 在5 mm 左右，旋轉(zhuǎn)參數(shù)與自標(biāo)定的結(jié)果相當(dāng)，平移參數(shù)優(yōu)于目前厘米級的自標(biāo)定精度。

第5 步，由于相機和慣導(dǎo)固定在參數(shù)已知的鋼板上，利用鋼尺量測相機和IMU 固定點之間的桿臂作為參考值，同時比較雙目相機之間的基線結(jié)果和MATLAB 的結(jié)果。為了減少參考的誤差，參考值多次計算取平均，鋼尺量測參考值標(biāo)定精度的分析結(jié)果如表1 所示，MATLAB 參考值標(biāo)定精度分析結(jié)果如表2 所示。

表1 鋼尺量測桿臂和標(biāo)定方案結(jié)果對比 單位:m

MATLAB 相機標(biāo)定工具箱作為公認(rèn)的標(biāo)定工具，與本文標(biāo)定方案的比較，可以進一步驗證方案的可靠性。上述結(jié)論中，高程誤差相對于平面誤差較小，這是由于估計變焦相機光心時，光心在相機中的高程是固定的，因此高程方向上的誤差更小。與目前常用的標(biāo)定方法比較：2017 年VINS 自標(biāo)定方法平移參數(shù)優(yōu)于2 cm，旋轉(zhuǎn)參數(shù)優(yōu)于1°，而常用的Kalibr 工具箱，平移參數(shù)標(biāo)定精度通常在厘米級，旋轉(zhuǎn)參數(shù)標(biāo)定精度優(yōu)于1°；在對本次實驗設(shè)備進行標(biāo)定時，由于相機和慣導(dǎo)的桿臂過長，慣導(dǎo)在激勵過程中受到相機位置的約束，多次標(biāo)定結(jié)果不穩(wěn)定。對比本文提出的標(biāo)定方法，可以看到相比于自標(biāo)定，借助精密外部測量儀器全站儀和標(biāo)定場可以獲得更好的標(biāo)定結(jié)果。

表2 MATLAB 求解的基線與新標(biāo)定方案的比較單位：m

4 結(jié)束語

多源傳感器的發(fā)展和應(yīng)用，離不開高精度的傳感器外參標(biāo)定結(jié)果。隨著移動測量機器人和自動駕駛對定位精度要求越來越高，傳感器間外參標(biāo)定的好壞顯得至關(guān)重要。與現(xiàn)在廣泛使用的自標(biāo)定方式不同，本文提出的標(biāo)定方案借助測量儀器、無需慣導(dǎo)的輸出，不用對大量的觀測數(shù)據(jù)進行建模處理，同時規(guī)避了多傳感器時間同步的問題。這種標(biāo)定方法適用于大型IMU 或相機與IMU 之間桿臂過長的情況，因為在這種情況下，慣導(dǎo)移動不便，且移動距離由于桿臂的原因受限，不能讓IMU 充分激勵達到良好的標(biāo)定效果。通過實驗和結(jié)果分析，驗證了本文提出的標(biāo)定方法的準(zhǔn)確性和可靠性。