定位點分布對相機位姿解算精度影響分析

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

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

0 引言

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

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

本文使用精確標定的相機獲取圖像信息，在奧普蒂·特拉克（OptiTrack）運動捕捉系統下，獲得準確的人工標志定位點坐標以及相機中心的概略位置和相機姿態，重點研究人工標志定位點的分布對于單目位姿解算精度的影響，并對視覺導航過程中，人工標志的布設，以及位姿解算過程中，點位的選擇提出合理的方案和建議。

1 原理及流程

本節對相機標定、標志點坐標提取、n 點透視問題（n point perspective problem，PnP）和點位構型的描述進行介紹。實驗流程如圖1 所示。

圖1 實驗流程

主要包括相機的標定、標志點準確3 維坐標的獲取、圖像中定位點坐標的提取、相機位姿的解算和精度的分析。

1.1 相機標定

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

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

圖2 棋盤格照片

相機標定結果如表1 所示。

表1 相機標定結果

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

標定結果精度較高，滿足實驗的要求。

1.2 OptiTrack 運動捕捉系統

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

圖3 OptiTrack 系統

首先，對OptiTrack 系統進行初始化標定，系統6 個相機視場采樣點如圖4 所示，標定好的系統如圖5 所示。

圖4 OptiTrack 系統標定時相機視場采樣點

圖5 標定好的OptiTrack 系統

在空間中密集布設反光標志球作為定位點，為相機位姿的解算提供準確的物點坐標。

1.3 PnP 問題

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

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

DLS 適用于定位點較多的情況，根據文獻[7]，在標志點密集布設、照片中標志點足夠多、標志點3維坐標精度小于1 mm、相機焦距標定精度20 μm、像點提取的誤差為0.5 個像素時，位置計算的精度可以達到毫米級，姿態計算精度不超過1′，可以滿足實驗的要求。

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

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

式中 ViC為 Viw在相機坐標系下的坐標。所以，在求出 ViC后，問題便轉化為絕對定向問題，即

式中： R為世界坐標系相對于相機坐標系的旋轉矩陣；t為世界坐標系相對于相機坐標系的平移向量。

迭代法選擇像方殘差或者物方殘差作為目標函數，不斷優化求解，得到高精度的位姿解算結果，即

式中Vi=O′O′T/O′TO′，O為權矩陣。不斷迭代求解R和t，當相鄰2 次計算的目標函數差值小于給定的閾值時，結束運算，此時的R和t為最優解。基于各種算法的特點，使用DLS 算法以及全部定位點解算相機的位姿真值，分別使用P3P、EPnP 和迭代算法計算定位點不同分布情況下，相機位姿解算的結果，并進行精度的評定和分析。

1.4 定位點分布的表達方式

為描述空間中定位點相對于相機的分布情況，參考衛星導航定位中精度衰減因子（dilution of precision, DOP）的定義[10]。對于EPnP 和迭代法，采用4 個定位點進行相機位姿解算時，系數矩陣表達為

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

由最小二乘知，定位誤差的協因數陣為

式中 q（iji = 1, 2, 3, 4；j =1, 2, 3, 4）為i 點與j 點的互協因數。

本文選擇PDOP 來描述定位點的分布情況，即

對于P3P 算法，采用4 個定位點進行相機位姿解算時，只考慮前3 個點的構型，系數矩陣具體表達為

由最小二乘知，定位誤差的協因數陣為

PDOP 的計算方式為

2 結果及分析

2.1 相機相對位姿的解算

首先通過相機拍攝的棋盤格照片進行相機標定，在OptiTrack 系統中布設反光標志球，記錄反光標志準確的空間3 維坐標作為物點的真實坐標，而后采用相機拍攝反光標志照片，手動提取出定位點的像素平面坐標，利用OpenCV 函數庫進行相機相對位姿的解算。實驗過程中，通過改變相機的位置或者反光標志的分布情況，研究定位點分布對于位姿解算精度的影響。

2.2 相機位置解算精度分析

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

圖6 場景1 布設情況

圖7 場景2 布設情況

圖8 場景3 布設情況

在3 個實驗場景下，針對所有的定位點組合計算對應的PDOP 值，分別使用P3P 算法、EPnP 算法以及迭代法計算相機位置，選擇正確的解算結果，分析各個方向上定位誤差以及整體定位誤差與PDOP 值之間的關系。

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

表2 相機的位置信息

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

根據相機在系統坐標系下的真實位置及定位點的分布情況，場景1 中相機與定位點的距離最近，場景2 中相機與定位點距離最遠。由以上分析可知，在PDOP 值較小的情況下，3 種算法分別計算得到的相機位置整體誤差均體現為：場景1 中整體定位誤差最小，場景3 中整體定位誤差次之，場景2 中整體定位誤差最大。所以相機與定位點之間的距離，對整體定位誤差產生影響，且在一定范圍內，距離越小整體定位誤差越小。

2.3 相機姿態解算精度分析

在場景1 中，針對所有的定位點組合計算對應的PDOP 值，分別使用P3P 算法、EPnP 算法以及迭代法計算相機姿態，選擇正確的解算結果，分析俯仰角、方位角、橫滾角誤差與PDOP 值之間的關系。

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

圖18 EPnP 算法姿態角誤差隨PDOP 值變化

圖19 P3P 算法姿態角誤差隨PDOP 值變化

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

圖20 迭代算法姿態角誤差隨PDOP 值變化

根據實驗結果，人工標志的布設應充分考慮點位的分布情況，盡量避免過于密集的布設，應依據現場環境的情況、相機與人工標志的距離和角度、人工標志的最大識別距離等情況進行標志布設，使得鄰近的標志點構型良好，保證每張照片中都存在至少4 個構型良好的標志點；照片包含標志點較多時，可以考慮根據相機的概略位置，選擇構型良好的標志點進行位姿解算，以提高解算精度。

3 結束語

視覺傳感器具有廉價、信息豐富、易于獲得等特點，視覺導航是今后導航定位領域的熱點問題。本文使用精確標定的相機獲取圖像信息，提取出標志點的像點坐標，在OptiTrack 運動捕捉系統下，獲得準確的人工標志定位點坐標，進行相機位姿的解算，研究人工標志定位點的分布對于單目位姿解算精度的影響。實驗結果表明：當PDOP 值較小時，位姿解算精度主要受像點提取誤差、相機標定誤差等誤差的影響，但當 PDOP值大于一定閾值時，隨著PDOP 值增大，位姿解算誤差持續增大，定位點的構型成為影響位姿解算精度的主要原因；在一定范圍內，相機與定位點之間的距離越小，定位誤差越小。