基于合作目標(biāo)的單目視覺位姿估計(jì)*

范路生 張靜遠(yuǎn) 王為如 吳佩璋

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

1 引言

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

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

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

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

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

2 原理

2.1 視覺位姿估計(jì)模型

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

圖1 坐標(biāo)系示意圖

本文視覺位姿估計(jì)模型：

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

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

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

2.2 直接線性變換

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

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

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

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

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

依據(jù)式（7），每個(gè)合作目標(biāo)特征點(diǎn)可提供兩個(gè)獨(dú)立方程，當(dāng)特征點(diǎn)的個(gè)數(shù)N≥6時(shí)，理論中則可得到唯一解。

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

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

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

圖2 重投影誤差

單個(gè)點(diǎn)的重投影誤差為

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

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

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

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

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

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

3 實(shí)驗(yàn)

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

圖3 視覺位姿估計(jì)實(shí)驗(yàn)流程圖

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

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

圖4 視覺位姿估計(jì)實(shí)驗(yàn)結(jié)果圖

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

4 結(jié)語(yǔ)

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