基于合作目標的單目視覺位姿估計*

范路生 張靜遠 王為如 吳佩璋

（1.海軍工程大學 武漢 430000）（2.中國船舶集團有限公司第七一三研究所 鄭州 450015）

1 引言

在傳統物流行業中，叉車是一種工業的搬運車輛，常常被用來裝卸、堆垛物品［2］。在新興的無人物流行業中，叉車自主叉取貨物可以節省大量的人力物力成本，提高效率［1］，貨叉與貨叉孔的高精度相對位姿估計是叉車自主叉取貨物的關鍵問題。基于視覺的位姿估計具有精度高、成本低、非接觸、實時性高、位姿信息量豐富等突出優點［3］。故本文提出了一種基于單目視覺的目標相對六維位姿估計方法，用來計算貨叉與貨叉孔的相對六維位姿。

在現有的大多數基于視覺的位姿估計方法中，有兩個基本步驟：特征提取和位姿參數求解［4］。提取的特征分為合作目標特征和非合作目標特征兩大類，基于合作目標特征的位姿估計方法需要在被測目標物上設置一些幾何結構信息已知且能夠在相機中完整清晰成像的合作標志。人為設置的標志點能大幅度提高目標的檢測精度和位姿估計精度［5～6］，具有結構簡單、特征明顯、易于分辨以及測量精度高等優點。常見的合作標志包括十字、圓形、對角、Aruco碼等［7］，相比于其他合作標志，Aru?co碼角點可以提供豐富的位姿估計對應信息，同時其內部特有的二進制編碼使得標志在錯誤檢查和修正方面保持一定的穩定性。因此本文選擇Aruco碼作為合作標志，布設在貨叉孔附近，相機布設在貨叉附近。

提取的合作目標特征用于構建2D-3D對應關系，根據構建的對應關系實現位姿估計［4］。根據被測目標的特征信息，分為基于點特征的位姿估計方法，基于直線特征的位姿估計方法，基于圓的位姿估計方法等。由于Aruco碼特征點信息豐富，本文解算方法利用的特征主要是特征點。

在已知相機內參的情況下，由特征點與其像點的對應關系求解目標空間坐標系相對于像機坐標系的相對位姿關系就是經典的PnP問題（Perspec?tive-N-Point），即N點透視問題［8］。PnP問題求解方法大致分為線性算法和非線性迭代算法兩類。P3P最多有4個解，Gao等通過吳方法［9］給出其完全解分類。Quan和Lan［10］對于P4P和P5P問題給出了線性SVD算法，他們把PnP問題轉化成多個P3P問題，算法計算復雜度為O(n5)。Fiore［11］提出了一種計算復雜度為O(n2)的線性算法，但對噪聲比較敏感。Moreno和Lepetit提出了一種計算復雜度為O(n)的高效、高精度線性算法EPnP［12］，但經過實驗驗證，EPnP算法對圖片的深度信息比較敏感，當深度較小時，該算法精度不高。另外一個典型的線性算法是直接線性變換（Direct Linear Transformation，DLT）算法［13］，它將變換矩陣12個變量當成獨立變量，忽略旋轉矩陣的正交約束，得到12個線性約束方程，當特征點個數N≥6時，該算法有唯一解。DLT算法計算速度快，但受噪聲影響較大，精度不高。經典的非線性迭代算法是以最小化某種代價函數為目標，將位姿求解問題轉換為非線性最小二乘問題，利用Gauss-Newton法或者Levenberg-Mar?quardt法等非線性優化算法進行迭代求解。非線性迭代算法精度高，具有很高的抗噪聲性能，但該方法計算量遠遠大于線性算法，求解過程非常耗時，對迭代的初值非常依賴。

本文提出了一種基于合作目標特征點定位技術的位姿估計算法，可有效快速解算貨叉與貨叉孔的相對六維位姿，該算法先利用N（N≥6）個特征點通過DLT算法求解得到貨叉與貨叉孔相對位姿的初值，再將該值作為非線性迭代運算的初值，使用重投影誤差來構造代價函數，經過數次迭代，則可得到兩者高精度的六維位姿。

2 原理

2.1 視覺位姿估計模型

按照本實驗構建相機透視投影成像位姿估計模型［14］，設相機坐標系OC?XCYCZC，基準坐標系（貨叉坐標系）O?XYZ，世界坐標系（貨叉孔坐標系）OW?XWYWZW，圖像坐標系o?uv，坐標系示意圖如圖1所示。

圖1 坐標系示意圖

本文視覺位姿估計模型：

其中K表示像機內參矩陣，[RC|TC]、[R|T]分別表示相機坐標系相對于基準坐標系的旋轉平移矩陣、基準坐標系相對于世界坐標系的旋轉平移矩陣，s是比例系數，其值是一個常數。

由于像機鏡頭是由凹、凸透鏡及其組合構成的，因此，成像平面不同區域對光線的彎曲程度也是不同的，從而造成圖像中實際點像素坐標(x，y)與理想點像素坐標(xcorr，ycorr)之間的像差，該像差會導致圖像出現徑向畸變和切向畸變，對像點產生的影響［15］可以描述為

式中，r2=x2+y2，是像素點p到圖像中心的距離，k1、k2、k3是徑向畸變參數，p1、p2是切向畸變參數。本文采用Matlab標定工具箱對相機標定，得到相機內參fx、fy、cx、cy和畸變參數。將線性模型式（1）與像差模型式（2）組合起來構成完整的針孔像機透視投影模型。

2.2 直接線性變換

根據針孔相機2D點到3D點齊次坐標矢量的映射關系，本文定義投影矩陣M：

令u=[uv1]T，XW=[XWYWZW1]T，則式（1）可以簡化為：

將投影矩陣M的第j行記作mjT，將矢量式su與MXW進行叉乘運算，可得到：

通過矩陣線性變換，化簡可得到關于M的三個方程：

忽略3×3旋轉矩陣R的正交約束，將其看作9個獨立變量，則M的每個元素都為獨立變量，依據矩陣秩的特性，通過推導得到兩個獨立方程，共有12個獨立未知變量，即基準坐標系相對于世界坐標系的旋轉平移矩陣參數。

依據式（7），每個合作目標特征點可提供兩個獨立方程，當特征點的個數N≥6時，理論中則可得到唯一解。

當N>6時，采用最小二乘的方式擬合得到12個獨立未知變量的值，求解得到基準坐標系相對于世界坐標系的旋轉平移矩陣，即貨叉與貨叉孔的相對位姿的初值。通過實驗驗證，僅采用DLT求解得到的位姿精度較低，我們可以通過最小化重投影誤差來提高精度。

2.3 光束法平差模型優化

BA（Bundle Adjustment），譯為光束法平差，其本質是一個優化模型，目的式最小化重投影誤差。重投影誤差是真實三維空間點在圖像平面上的投影ui（即圖像上的像素點）和重投影vi（用3D點按照當前計算到的投影矩陣進行第二次投影計算得到的虛擬的像素點）的差值。

圖2 重投影誤差

單個點的重投影誤差為

BA模型是將所有重投影誤差的和最小化：

針對BA模型的優化算法有很多，本文采用非線性算法Gauss-Newton對上節求得的目標間的相對位姿初值進行優化。結合位姿估計模型及求解式（7），本文利用特征點重投影誤差構造代價函數Fui，Fvi，通過多次非線性迭代優化使其最小化，可使所有特征點的重投影誤差和的最小化。

依據旋轉平移矩陣[R|T]組成元素特性，設代價函數變量為各軸旋轉角度和平移距離，即pe=[Ax，Ay，Az，Tx，Ty，Tz]T，分別對代價函數Fui，Fvi求一階偏導數，則可得到雅可比矩陣：

Gauss-Newton算法是忽略代價函數的泰勒展開式二次項及多次項：

考慮到重投影誤差ei=‖ui?vi‖ 的理想值為0，故我們直接令代價函數為0，則可以演化得到本文BA優化模型：

通過最小二乘擬合，經過數次運算，可在δpe方向上迭代直到代價函數絕對值最小，得到貨叉與貨叉孔的高精度六維位姿。

3 實驗

基于上章節算法原理，相機坐標系相對于基準坐標系的旋轉平移矩陣[RC|TC]可通過調整基準坐標系與世界坐標系完全重合，即將貨叉前端與貨叉孔前端完全重合，則可得到。視覺位姿估計實驗設計如圖3所示。

圖3 視覺位姿估計實驗流程圖

本文實驗采用USB插孔的工業級紅外攝像機，在實驗過程中，貨叉前端距離貨叉孔前端景深距離為0.5m，相機距離合作目標1.58m。相機采集圖像過程中，貨叉、貨叉孔、相機相對于地面保持靜止，經過位姿解算，得到500組實驗數據，實驗結果如下所示。

在本次實驗中，本文得到了500組貨叉與貨叉孔相對六維位姿參數實驗數據，如圖4所示，經過各參數實際值與估計的對比分析，我們可以得到，X軸平移距離Tx靜態誤差在2mm左右，動態誤差在［-0.1mm，4.1mm］區間，X軸旋轉角度Ax靜態誤差在0.05°左右，動態誤差在［-0.04°，0.12°］區間；Y軸平移距離Ty靜態誤差在-1.5mm左右，動態誤差在［0.9mm，-3.9mm］區間，Y軸旋轉角度Ay靜態誤差在0.06°左右，動態誤差在［0.1°，0.11°］區間；Z軸平移距離Tz靜態誤差在0mm左右，動態誤差在［-1.5mm，2mm］區間，Z軸旋轉角度Az靜態誤差在-0.06°左右，動態誤差在［-0.15°，0.05°］區間。

圖4 視覺位姿估計實驗結果圖

經過實驗驗證，在相機距離合作目標景深距離1.58m，貨叉距離貨叉孔0.5m時，本文位姿估計算法平移距離精度達到毫米級，旋轉角度精度在0.2°以內。

4 結語

本文以至少六個共面合作目標特征點定位技術求解貨叉與貨叉孔的相對位姿，基于針孔相機透視投影模型構建視覺位姿估計模型，采用直接線性變換解算得到兩者相對位姿測量的初值，依據最小化重投影誤差的原則構建代價函數，并用Gauss-Newton非線性迭代算法對其進行優化，提高位姿估計精度。實驗結果表明，在理想狀態下，平移距離誤差最大為4.8mm，旋轉角度誤差最大為0.2°。但同時我們也可以得到該算法估計值波動范圍較大，后期應分析原因，增加濾波算法進一步改進優化。