定位點(diǎn)分布對相機(jī)位姿解算精度影響分析

王高杰，郝向陽，歐陽文，鄭 凱，李朋月

（信息工程大學(xué) 地理空間信息學(xué)院，鄭州 450000）

0 引言

基于單目相機(jī)的視覺導(dǎo)航過程，需要進(jìn)行相機(jī)位姿的實(shí)時(shí)解算。使用人工標(biāo)志的視覺導(dǎo)航方式，需要通過識別人工標(biāo)志獲取精確的3 維點(diǎn)，利用物點(diǎn)與像點(diǎn)的透視關(guān)系式解算出相機(jī)中心相對于世界坐標(biāo)系的位置和姿態(tài)，是視覺導(dǎo)航1 種重要的手段。在人工標(biāo)志無法密集布設(shè)的情況下，標(biāo)志點(diǎn)的分布對于相機(jī)位姿估計(jì)具有一定的影響；分析定位點(diǎn)的構(gòu)型與位姿解算精度的關(guān)系，對于設(shè)計(jì)更加實(shí)用準(zhǔn)確的人工標(biāo)志分布方案，以及位姿解算過程中的點(diǎn)位選擇真有重要意義。

目前受限于計(jì)算機(jī)技術(shù)的發(fā)展，無論對室內(nèi)導(dǎo)航還是室外導(dǎo)航，都很難進(jìn)行高速率的場景建模。因此，廣泛運(yùn)用基于人工地標(biāo)的識別定位方法來實(shí)現(xiàn)定位[1]。當(dāng)前比較有代表性的是由密西根大學(xué)阿普里爾（April）實(shí)驗(yàn)室開發(fā)的阿普里爾·塔格（AprilTag）視覺基準(zhǔn)系統(tǒng)[2]。AprilTag 可以識別單個或者多個人工標(biāo)志，很好地解決了傳統(tǒng)的人工標(biāo)志受環(huán)境、光照等條件影響時(shí)，識別出錯率較高的問題，可以進(jìn)一步提升識別效率與定位精度[3]；但是實(shí)現(xiàn)這一目標(biāo)對于AprilTag 標(biāo)志布設(shè)的要求比較高。

本文使用精確標(biāo)定的相機(jī)獲取圖像信息，在奧普蒂·特拉克（OptiTrack）運(yùn)動捕捉系統(tǒng)下，獲得準(zhǔn)確的人工標(biāo)志定位點(diǎn)坐標(biāo)以及相機(jī)中心的概略位置和相機(jī)姿態(tài)，重點(diǎn)研究人工標(biāo)志定位點(diǎn)的分布對于單目位姿解算精度的影響，并對視覺導(dǎo)航過程中，人工標(biāo)志的布設(shè)，以及位姿解算過程中，點(diǎn)位的選擇提出合理的方案和建議。

1 原理及流程

本節(jié)對相機(jī)標(biāo)定、標(biāo)志點(diǎn)坐標(biāo)提取、n 點(diǎn)透視問題（n point perspective problem，PnP）和點(diǎn)位構(gòu)型的描述進(jìn)行介紹。實(shí)驗(yàn)流程如圖1 所示。

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

主要包括相機(jī)的標(biāo)定、標(biāo)志點(diǎn)準(zhǔn)確3 維坐標(biāo)的獲取、圖像中定位點(diǎn)坐標(biāo)的提取、相機(jī)位姿的解算和精度的分析。

1.1 相機(jī)標(biāo)定

實(shí)驗(yàn)中采用Canon 65X 相機(jī)，拍攝照片的分辨率為3 264×2 448 個像素。文獻(xiàn)[4]的相機(jī)標(biāo)定方法，相對于其他標(biāo)定方法，具有操作簡單、標(biāo)定板制作容易、對應(yīng)點(diǎn)匹配簡單等特點(diǎn)，因而本文主要采用張正友相機(jī)標(biāo)定方法。文獻(xiàn)[4]相機(jī)標(biāo)定流程如下：讀取相機(jī)拍攝的棋盤格照片、提取角點(diǎn)并繪圖、生成角點(diǎn)像素坐標(biāo)、最大似然求單應(yīng)矩陣、建立約束方程、奇異值分解（singular value decomposition,SVD）得唯一解、喬列斯基（Cholesky）分解得到內(nèi)參矩陣、計(jì)算外參數(shù)矩陣、標(biāo)定完成。

如圖2 所示，從不同角度拍攝12 張棋盤格照片，利用開源計(jì)算機(jī)視覺庫（OpenCV）中的標(biāo)定函數(shù)calibrateCamera 進(jìn)行相機(jī)標(biāo)定。輸入的參數(shù)包括：12 張棋盤格照片路徑，棋盤格大小8×11 格，棋盤格每個單元寬度為20 mm。

圖2 棋盤格照片

相機(jī)標(biāo)定結(jié)果如表1 所示。

表1 相機(jī)標(biāo)定結(jié)果

表1 中：fx和 fy為x 和y 方向上的焦距；u0和 v0為像主點(diǎn)坐標(biāo)；k1和 k2為徑向畸變系數(shù)；重投影誤差為0.181 4 個像素。

標(biāo)定結(jié)果精度較高，滿足實(shí)驗(yàn)的要求。

1.2 OptiTrack 運(yùn)動捕捉系統(tǒng)

OptiTrack 是基于多相機(jī)的運(yùn)動捕捉系統(tǒng)，在相機(jī)標(biāo)定以及系統(tǒng)坐標(biāo)系建立完成之后，對于空間中的1 個點(diǎn)，如果它能同時(shí)為2 部攝像機(jī)所見，則根據(jù)同一時(shí)刻2 部攝像機(jī)所拍攝的圖像和攝像機(jī)參數(shù)，可以確定這一點(diǎn)在空間中的位置，精度可達(dá)到亞毫米級[5]。OptiTrack 系統(tǒng)如圖3 所示。本文實(shí)驗(yàn)使用反光標(biāo)志球代替人工標(biāo)志的定位點(diǎn)，通過OptiTrack 系統(tǒng)獲得標(biāo)志點(diǎn)的準(zhǔn)確空間位置以及相機(jī)中心的大致位置和相機(jī)姿態(tài)。

圖3 OptiTrack 系統(tǒng)

首先，對OptiTrack 系統(tǒng)進(jìn)行初始化標(biāo)定，系統(tǒng)6 個相機(jī)視場采樣點(diǎn)如圖4 所示，標(biāo)定好的系統(tǒng)如圖5 所示。

圖4 OptiTrack 系統(tǒng)標(biāo)定時(shí)相機(jī)視場采樣點(diǎn)

圖5 標(biāo)定好的OptiTrack 系統(tǒng)

在空間中密集布設(shè)反光標(biāo)志球作為定位點(diǎn)，為相機(jī)位姿的解算提供準(zhǔn)確的物點(diǎn)坐標(biāo)。

1.3 PnP 問題

文獻(xiàn)[6]于1981 年首先提出n 個3D 空間點(diǎn)求解相機(jī)位姿問題，也稱為PnP 問題。PnP 問題就是指如下的物體定位問題：假設(shè)相機(jī)模型為小孔模型，相機(jī)已完成標(biāo)定過程，如果有1 幅在物體坐標(biāo)系下坐標(biāo)已知的n 個空間點(diǎn)的照片，且這n 個圖像點(diǎn)的像素平面坐標(biāo)為已知，確定這n 個空間點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)。PnP 算法的目的，是利用人工標(biāo)志的位置信息、人工標(biāo)志上特征點(diǎn)之間的位置關(guān)系以及提取到的特征點(diǎn)坐標(biāo)和相機(jī)內(nèi)參數(shù)，來解算相機(jī)坐標(biāo)系和世界坐標(biāo)系之間的相對位姿。

本文使用OpenCV 中的solvePnP 函數(shù)進(jìn)行相機(jī)位姿的解算，solvePnP 函數(shù)里有4 種解法：透視3 點(diǎn)算法（perspective-three-point, P3P）、高效透視n 點(diǎn)算法（efficient perspective-n-point, EPnP）、迭代法和直接最小二乘方法（direct least-squares, DLS）。

DLS 適用于定位點(diǎn)較多的情況，根據(jù)文獻(xiàn)[7]，在標(biāo)志點(diǎn)密集布設(shè)、照片中標(biāo)志點(diǎn)足夠多、標(biāo)志點(diǎn)3維坐標(biāo)精度小于1 mm、相機(jī)焦距標(biāo)定精度20 μm、像點(diǎn)提取的誤差為0.5 個像素時(shí)，位置計(jì)算的精度可以達(dá)到毫米級，姿態(tài)計(jì)算精度不超過1′，可以滿足實(shí)驗(yàn)的要求。

P3P 算法[8]使用3 對點(diǎn)求解旋轉(zhuǎn)角和平移向量，利用第4 個已知點(diǎn)對來驗(yàn)證得到的位姿，雖然依賴于線性模型，對系統(tǒng)精度要求較高，但是在沒有較大誤差時(shí)能夠得到較好的結(jié)果。

EPnP 算法[9]充分挖掘位姿估計(jì)中特有的約束，引入適當(dāng)中間變量來表示點(diǎn)集，將問題在盡量減少信息損失情況下，轉(zhuǎn)換為絕對定向問題，進(jìn)而得到閉式解法，EPnP 算法是目前穩(wěn)定高效的PnP問題解法。對于世界坐標(biāo)系下的標(biāo)志點(diǎn)w( , ,iP i=1 2… , n)和相機(jī)坐標(biāo)系下的對應(yīng)點(diǎn) PiC(i =1, 2,… , n)，可以構(gòu)造 1 組虛擬的控制點(diǎn) Viw(i =1, 2,…, n)來表示，即：

式中 ViC為 Viw在相機(jī)坐標(biāo)系下的坐標(biāo)。所以，在求出 ViC后，問題便轉(zhuǎn)化為絕對定向問題，即

式中： R為世界坐標(biāo)系相對于相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣；t為世界坐標(biāo)系相對于相機(jī)坐標(biāo)系的平移向量。

迭代法選擇像方殘差或者物方殘差作為目標(biāo)函數(shù)，不斷優(yōu)化求解，得到高精度的位姿解算結(jié)果，即

式中Vi=O′O′T/O′TO′，O為權(quán)矩陣。不斷迭代求解R和t，當(dāng)相鄰2 次計(jì)算的目標(biāo)函數(shù)差值小于給定的閾值時(shí)，結(jié)束運(yùn)算，此時(shí)的R和t為最優(yōu)解?；诟鞣N算法的特點(diǎn)，使用DLS 算法以及全部定位點(diǎn)解算相機(jī)的位姿真值，分別使用P3P、EPnP 和迭代算法計(jì)算定位點(diǎn)不同分布情況下，相機(jī)位姿解算的結(jié)果，并進(jìn)行精度的評定和分析。

1.4 定位點(diǎn)分布的表達(dá)方式

為描述空間中定位點(diǎn)相對于相機(jī)的分布情況，參考衛(wèi)星導(dǎo)航定位中精度衰減因子（dilution of precision, DOP）的定義[10]。對于EPnP 和迭代法，采用4 個定位點(diǎn)進(jìn)行相機(jī)位姿解算時(shí)，系數(shù)矩陣表達(dá)為

式中 p（iji = 1, 2, 3, 4；j =1, 2, 3）表示相機(jī)與定位點(diǎn)之間方向矢量的余弦。

由最小二乘知，定位誤差的協(xié)因數(shù)陣為

式中 q（iji = 1, 2, 3, 4；j =1, 2, 3, 4）為i 點(diǎn)與j 點(diǎn)的互協(xié)因數(shù)。

本文選擇PDOP 來描述定位點(diǎn)的分布情況，即

對于P3P 算法，采用4 個定位點(diǎn)進(jìn)行相機(jī)位姿解算時(shí)，只考慮前3 個點(diǎn)的構(gòu)型，系數(shù)矩陣具體表達(dá)為

由最小二乘知，定位誤差的協(xié)因數(shù)陣為

PDOP 的計(jì)算方式為

2 結(jié)果及分析

2.1 相機(jī)相對位姿的解算

首先通過相機(jī)拍攝的棋盤格照片進(jìn)行相機(jī)標(biāo)定，在OptiTrack 系統(tǒng)中布設(shè)反光標(biāo)志球，記錄反光標(biāo)志準(zhǔn)確的空間3 維坐標(biāo)作為物點(diǎn)的真實(shí)坐標(biāo)，而后采用相機(jī)拍攝反光標(biāo)志照片，手動提取出定位點(diǎn)的像素平面坐標(biāo)，利用OpenCV 函數(shù)庫進(jìn)行相機(jī)相對位姿的解算。實(shí)驗(yàn)過程中，通過改變相機(jī)的位置或者反光標(biāo)志的分布情況，研究定位點(diǎn)分布對于位姿解算精度的影響。

2.2 相機(jī)位置解算精度分析

選擇3 個實(shí)驗(yàn)場景進(jìn)行數(shù)據(jù)分析。圖6、圖7、圖8 依次為場景1、場景2、場景3 的布設(shè)情況。場景1、場景2 和場景3 中，相機(jī)距離定位點(diǎn)概略距離分別為1.6、2.5 和1.9 m，場景1 和場景2 中設(shè)置8 個定位點(diǎn)，場景3 中設(shè)置12 個定位點(diǎn)，同一場景中，定位點(diǎn)分布在不同平面上并保持一定距離。

圖6 場景1 布設(shè)情況

圖7 場景2 布設(shè)情況

圖8 場景3 布設(shè)情況

在3 個實(shí)驗(yàn)場景下，針對所有的定位點(diǎn)組合計(jì)算對應(yīng)的PDOP 值，分別使用P3P 算法、EPnP 算法以及迭代法計(jì)算相機(jī)位置，選擇正確的解算結(jié)果，分析各個方向上定位誤差以及整體定位誤差與PDOP 值之間的關(guān)系。

每個場景下相機(jī)的位置以及3 種解算方法得到的數(shù)據(jù)情況如表2 所示。

表2 相機(jī)的位置信息

4 個定位點(diǎn)的不同分布對應(yīng)不同的PDOP 值，以場景1 為例，EPNP 算法各個方向上的誤差具體分布情況如圖9 所示；P3P 算法各個方向上的誤差具體分布情況如圖10 所示；迭代算法各個方向上的誤差具體分布情況如圖11 所示。

圖9 EPnP 算法3 個方向上的定位誤差隨PDOP 值變化

圖10 P3P 算法3 個方向上的定位誤差隨PDOP 值變化

圖11 迭代算法3 個方向上的定位誤差隨PDOP 值變化

由圖9～圖11 可知：在PDOP 值較小即定位點(diǎn)構(gòu)型良好的時(shí)候，各個方向的定位誤差都較小，但沒有明顯的規(guī)律，這是由于受到了像點(diǎn)提取誤差、相機(jī)焦距誤差等誤差的影響；EPnP 算法和迭代算法中PDOP 值小于10 的點(diǎn)各個方向上的定位誤差均小于10 mm，所占的比例超過了90%，P3P 算法中PDOP 值小于40 的點(diǎn)各個方向上的定位誤差均小于40 mm，所占的比例超過了95%，隨著PDOP值的增大，3 種方法計(jì)算得到的3 個方向上的定位誤差均變大，說明定位點(diǎn)的構(gòu)型會對位置解算的精度產(chǎn)生影響。

使用3 種算法在3 個場景下的整體定位精度隨著PDOP 值的變化情況分別如圖12～圖14所示。

圖12 場景1 中3 種算法定位整體誤差隨PDOP 值變化情況

圖13 場景2 中3 種算法定位整體誤差隨PDOP值變化情況

圖14 場景3 中3 種算法定位整體誤差隨PDOP 值變化情況

圖中的整體定位誤差表示定位點(diǎn)的真實(shí)坐標(biāo)與位姿解算得到的坐標(biāo)之間的幾何距離。圖12～圖14表明：在PDOP 值較小的情況下，定位整體誤差受到像點(diǎn)提取誤差等其他因素的影響，誤差分布沒有明顯的規(guī)律，但隨著PDOP 值的增大，定位誤差整體呈現(xiàn)增大的趨勢，進(jìn)一步說明了定位點(diǎn)的構(gòu)型會對位置解算精度產(chǎn)生影響，且2 者呈正相關(guān)。

在3 個場景中，相機(jī)與定位點(diǎn)的距離有一定差異，為研究距離對于相機(jī)位姿解算精度的影響，對比分析同種算法在3 個不同場景中，相機(jī)定位整體誤差隨PDOP 值變化的情況。利用1 組或幾組定位點(diǎn)組合來解算相機(jī)位置受到其他因素如相機(jī)標(biāo)定誤差、定位點(diǎn)提取誤差等的影響，得到的結(jié)果受偶然因素影響并不可靠，所以對比分析同種算法在3 個場景下，整體定位誤差隨PDOP 值的變化情況，為保證數(shù)據(jù)的可靠性，將PDOP 值限定在合理范圍之內(nèi)。使用EPnP 算法時(shí)整體定位誤差隨PDOP 值變化情況如圖15 所示；使用P3P 算法時(shí)，整體定位誤差隨PDOP 值變化情況如圖16 所示；使用迭代算法時(shí)，整體定位誤差隨PDOP 值變化情況如圖17 所示。

圖15 EPnP 算法整體定位誤差隨PDOP 值變化情況

圖16 P3P 算法整體定位誤差隨PDOP 值變化情況

圖17 迭代算法整體定位誤差隨PDOP 值變化情況

如圖15～圖17 所示：使用EPnP 算法計(jì)算3 個場景中相機(jī)的位置后，取PDOP 小于16 的數(shù)據(jù)部分進(jìn)行分析，場景1 中93.3%的結(jié)果整體定位誤差小于20 mm，場景2 中31.2%的結(jié)果整體定位誤差小于20 mm，場景3 中43.7%的結(jié)果整體定位誤差小于20 mm；使用P3P 算法計(jì)算3 個場景中相機(jī)的位置后，取PDOP 小于30 的數(shù)據(jù)部分進(jìn)行分析，場景1 中77%的結(jié)果整體定位誤差小于20 mm，場景2 中32.2%的結(jié)果整體定位誤差小于20 mm，場景3 中59%的結(jié)果整體定位誤差小于20 mm；使用迭代算法計(jì)算3 個場景中相機(jī)的位置后，取PDOP 小于30 的數(shù)據(jù)部分進(jìn)行分析，場景1 中87.5%的結(jié)果整體定位誤差小于20 mm，場景2 中50%的結(jié)果整體定位誤差小于20 mm，場景3 中75%的結(jié)果整體定位誤差小于20 mm。

根據(jù)相機(jī)在系統(tǒng)坐標(biāo)系下的真實(shí)位置及定位點(diǎn)的分布情況，場景1 中相機(jī)與定位點(diǎn)的距離最近，場景2 中相機(jī)與定位點(diǎn)距離最遠(yuǎn)。由以上分析可知，在PDOP 值較小的情況下，3 種算法分別計(jì)算得到的相機(jī)位置整體誤差均體現(xiàn)為：場景1 中整體定位誤差最小，場景3 中整體定位誤差次之，場景2 中整體定位誤差最大。所以相機(jī)與定位點(diǎn)之間的距離，對整體定位誤差產(chǎn)生影響，且在一定范圍內(nèi)，距離越小整體定位誤差越小。

2.3 相機(jī)姿態(tài)解算精度分析

在場景1 中，針對所有的定位點(diǎn)組合計(jì)算對應(yīng)的PDOP 值，分別使用P3P 算法、EPnP 算法以及迭代法計(jì)算相機(jī)姿態(tài)，選擇正確的解算結(jié)果，分析俯仰角、方位角、橫滾角誤差與PDOP 值之間的關(guān)系。

4 個定位點(diǎn)的不同分布對應(yīng)于不同的 PDOP值，EPnP 算法解算結(jié)果，角度誤差隨PDOP 值的變化情況如圖18 所示；P3P 算法解算結(jié)果，角度誤差隨PDOP 值的變化情況如圖19 所示；迭代算法解算結(jié)果，角度誤差隨 PDOP 值的變化情況如圖20 所示。

圖18 EPnP 算法姿態(tài)角誤差隨PDOP 值變化

圖19 P3P 算法姿態(tài)角誤差隨PDOP 值變化

圖中的姿態(tài)角誤差表示姿態(tài)角真值與解算結(jié)果差的絕對值。圖18～圖20 表明：在PDOP 值小于20 的情況下，3 種算法解算的姿態(tài)角誤差小于1°的所占比例高于92%，在此范圍內(nèi)，誤差分布和PDOP 值沒有明顯的規(guī)律，但隨著PDOP 值的增大，姿態(tài)角解算誤差整體呈現(xiàn)增大的趨勢，說明了定位點(diǎn)的構(gòu)型會對姿態(tài)解算精度產(chǎn)生影響，且2 者呈正相關(guān)。

圖20 迭代算法姿態(tài)角誤差隨PDOP 值變化

根據(jù)實(shí)驗(yàn)結(jié)果，人工標(biāo)志的布設(shè)應(yīng)充分考慮點(diǎn)位的分布情況，盡量避免過于密集的布設(shè)，應(yīng)依據(jù)現(xiàn)場環(huán)境的情況、相機(jī)與人工標(biāo)志的距離和角度、人工標(biāo)志的最大識別距離等情況進(jìn)行標(biāo)志布設(shè)，使得鄰近的標(biāo)志點(diǎn)構(gòu)型良好，保證每張照片中都存在至少4 個構(gòu)型良好的標(biāo)志點(diǎn)；照片包含標(biāo)志點(diǎn)較多時(shí)，可以考慮根據(jù)相機(jī)的概略位置，選擇構(gòu)型良好的標(biāo)志點(diǎn)進(jìn)行位姿解算，以提高解算精度。

3 結(jié)束語

視覺傳感器具有廉價(jià)、信息豐富、易于獲得等特點(diǎn)，視覺導(dǎo)航是今后導(dǎo)航定位領(lǐng)域的熱點(diǎn)問題。本文使用精確標(biāo)定的相機(jī)獲取圖像信息，提取出標(biāo)志點(diǎn)的像點(diǎn)坐標(biāo)，在OptiTrack 運(yùn)動捕捉系統(tǒng)下，獲得準(zhǔn)確的人工標(biāo)志定位點(diǎn)坐標(biāo)，進(jìn)行相機(jī)位姿的解算，研究人工標(biāo)志定位點(diǎn)的分布對于單目位姿解算精度的影響。實(shí)驗(yàn)結(jié)果表明：當(dāng)PDOP 值較小時(shí)，位姿解算精度主要受像點(diǎn)提取誤差、相機(jī)標(biāo)定誤差等誤差的影響，但當(dāng) PDOP值大于一定閾值時(shí)，隨著PDOP 值增大，位姿解算誤差持續(xù)增大，定位點(diǎn)的構(gòu)型成為影響位姿解算精度的主要原因；在一定范圍內(nèi)，相機(jī)與定位點(diǎn)之間的距離越小，定位誤差越小。