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

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

1 引言

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

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

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

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

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

2 原理

2.1 視覺位姿估計模型

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

圖1 坐標系示意圖

本文視覺位姿估計模型：

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

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

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

2.2 直接線性變換

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

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

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

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

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

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

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

2.3 光束法平差模型優(yōu)化

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

圖2 重投影誤差

單個點的重投影誤差為

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

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

依據(jù)旋轉(zhuǎn)平移矩陣[R|T]組成元素特性，設(shè)代價函數(shù)變量為各軸旋轉(zhuǎn)角度和平移距離，即pe=[Ax，Ay，Az，Tx，Ty，Tz]T，分別對代價函數(shù)Fui，F(xiàn)vi求一階偏導(dǎo)數(shù)，則可得到雅可比矩陣：

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

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

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

3 實驗

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

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

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

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

圖4 視覺位姿估計實驗結(jié)果圖

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

4 結(jié)語

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