機(jī)器人視覺中目標(biāo)物的位姿估計(jì)改進(jìn)

張 磊，張?zhí)煲妫εd田，呂棟陽

（1.南通大學(xué) 機(jī)械工程學(xué)院，江蘇 南通 226019；2.約克大學(xué) 拉松德工學(xué)院，加拿大 多倫多，M3J 1P3）

目標(biāo)物的位姿估計(jì)問題是指通過數(shù)學(xué)建模等方式解出攝像機(jī)與目標(biāo)物之間的相對位置關(guān)系，在攝影測量、交通視覺導(dǎo)航及視覺機(jī)器人作業(yè)等領(lǐng)域具有重要的應(yīng)用價值。知名的波士頓動力機(jī)器人Atlas 實(shí)現(xiàn)目標(biāo)物抓起動作即先對目標(biāo)紙箱進(jìn)行位姿估計(jì)[1]。位姿估計(jì)旨在獲得目標(biāo)物在攝像機(jī)坐標(biāo)系中的位置與姿態(tài)，為基于圖像對目標(biāo)物進(jìn)行下一步操作做準(zhǔn)備。

常見的位姿估計(jì)方法是由攝像機(jī)標(biāo)定獲得內(nèi)參數(shù)，然后由n 個3D-2D 點(diǎn)對點(diǎn)的對應(yīng)關(guān)系恢復(fù)物體位姿，被稱作PNP（perspective n point）問題，由Fischler 等[2]首先提出。關(guān)于P3P、P4P、P5P 等解的存在性、唯一性、多解性、最優(yōu)解等問題已得到充分研究。近年來有不少基于PNP 解法進(jìn)行優(yōu)化或應(yīng)用的報(bào)道[3-7]。Xu[3]提出了一種基于線對應(yīng)的位姿估計(jì)方法，與PNP 的點(diǎn)對應(yīng)經(jīng)典方法對比，在精確性與魯棒性方面都有所提升；Lepetit 等[8]提出了基于封閉解的EPNP（efficient perspective-n-point）方法，在精度上相對于經(jīng)典PNP 方法有了較大提升，但該方法過程比較復(fù)雜，他人難以在其基礎(chǔ)上進(jìn)行改進(jìn)。第二類是借助攝像機(jī)標(biāo)定的方法獲得目標(biāo)物的位姿，典型的有直接線性變換（direct linear transformation，DLT）方法[9]與張正友方法[10]。這些方法需要很多的特征點(diǎn)，對于一般的目標(biāo)物，大量特征點(diǎn)的對應(yīng)是個較難的問題；而特征點(diǎn)少，此類方法的精度又會明顯下降。針對這些問題，文獻(xiàn)[11]對DLT 方法作了一定改進(jìn)。第三類是基于目標(biāo)物CAD 模型的方法，以Bhanu[12]的研究和Harris[13]的RAPiD 方法最為經(jīng)典。近期有Tsai 等[14]的研究，較為詳細(xì)的介紹可參見Lepetit 等[15]的綜述。但這類方法需要將CAD模型與圖像同時在一個圖像場景中匹配，技術(shù)上的實(shí)現(xiàn)難度增大，且邊緣點(diǎn)難對應(yīng)的問題會影響其成功率。

為了提高上述方法的精度，常采用非線性優(yōu)化，其中較為常用的是最速下降法、高斯牛頓法和列文伯格-馬夸爾特（Levenberg-Marquardt，LM）算法[16]。LM 算法[17-18]吸收了高斯牛頓法可避開求解Hessan二階導(dǎo)數(shù)矩陣的優(yōu)點(diǎn)，避免了高斯牛頓法因雅可比矩陣不滿秩而造成的麻煩；其設(shè)計(jì)了一種信賴域策略，使得迭代的步長是動態(tài)的，進(jìn)一步提高了非線性優(yōu)化的效率。Yan 等[19]提出基于三次多項(xiàng)式的代價函數(shù)，基于此設(shè)計(jì)了相關(guān)的非線性迭代算法，獲得精確的位姿估計(jì)。

本文針對經(jīng)典DLT、EPNP 等解析法存在位姿估計(jì)精度不高的問題，利用相對簡單的DLT 方法獲得估計(jì)初始值；根據(jù)LM 優(yōu)化算法的特點(diǎn)，提出了方便求解雅可比矩陣的代價函數(shù)，并引入李群李代數(shù)表達(dá)位姿微調(diào)矩陣，進(jìn)一步簡化了雅可比矩陣的求解和位姿參數(shù)值的迭代估計(jì)。

1 經(jīng)典的DLT 方法及其計(jì)算

DLT 線性模型根據(jù)攝像機(jī)小孔成像原理，不考慮鏡頭畸變等非線性因素，直接以線性變換的形式建立靶標(biāo)特征點(diǎn)的世界坐標(biāo)（X，Y，Z，1）T與相應(yīng)圖像坐標(biāo)（u1，v11）T的關(guān)系

將s消去得

傳統(tǒng)解法[16]一般將式（2）分別變形為

其中：t1=（t1，t2，t3，t4）T，t2=（t5，t6，t7，t8）T，t3=（t9，t10，t11，t12）T；P=（X，Y，Z，1）T。

對于N 個特征點(diǎn)，可列出方程組

但該方程無法使用直接的線性方程組解法，因?yàn)橛疫吶? 元素。常見的解法是采用奇異值分解法（single value decomposition，SVD）[16]。然而SVD 方法需要求解系數(shù)矩陣的奇異值及奇異向量，求解過程繁瑣。當(dāng)矩陣維數(shù)較大時，需使用數(shù)值或迭代解法，求解的過程已失去封閉解法的簡便性。

文獻(xiàn)[20]中將式（2）上下同除以t12變換為

本文利用該形式，結(jié)合文獻(xiàn)[21]的思想，得到

其中：（ui，vi）T是第i個特征點(diǎn)的圖像坐標(biāo)；=（Xi，Yi，Zi）T是第i個特征點(diǎn)的世界坐標(biāo)。該方程記為

此方程可利用廣義逆直接求出封閉解

由于t′中的元素與tm（m=1，2，3）中的元素滿足

所以需先解出t12，才可解出t1，t2，…，t11。

根據(jù)攝像機(jī)投影中的內(nèi)外參數(shù)關(guān)系，式（1）可表達(dá)為

其中：fx，fy，cx，cy為攝像機(jī)內(nèi)參數(shù)；為外參數(shù)旋轉(zhuǎn)矩陣的各行向量；px，py，pz為外參數(shù)平移向量的各元素，外參數(shù)即待求解的位姿參數(shù)，因此

按照向量叉乘、點(diǎn)乘性質(zhì)，先可解得內(nèi)參數(shù)，然后可得各外參數(shù)

研究發(fā)現(xiàn)，當(dāng)目標(biāo)物特征點(diǎn)不多時，該方法精度偏低，因此本文通過非線性優(yōu)化方法進(jìn)行了改進(jìn)。

2 基于LM 算法的簡化優(yōu)化方法

LM 算法雖然是經(jīng)典通用方法，但是其本身并沒有提供具體的設(shè)計(jì)函數(shù)、參數(shù)配置等。實(shí)際應(yīng)用時，需根據(jù)實(shí)際問題設(shè)計(jì)恰當(dāng)?shù)拇鷥r函數(shù)，方便求解雅可比矩陣J，找到合適的迭代過程。

2.1 代價函數(shù)的設(shè)計(jì)

攝像機(jī)標(biāo)定和圖像位姿估計(jì)常用平均反投影誤差衡量參數(shù)估計(jì)的偏差，即

其中：（ui，vi）為由估計(jì)的參數(shù)根據(jù)式（11）結(jié)合鏡頭畸變參數(shù)進(jìn)行反投影計(jì)算得到的第i個特征點(diǎn)的圖像坐標(biāo)；為直接檢測到的相應(yīng)控制點(diǎn)圖像坐標(biāo)，單位為像素。Err值越小代表估計(jì)參數(shù)越精確，一般基于此設(shè)計(jì)代價函數(shù)進(jìn)行非線性迭代優(yōu)化。式（15）含有根號和求平均值項(xiàng)，對非線性優(yōu)化來說不是必須的，會增加雅可比矩陣計(jì)算的復(fù)雜性，本文基于式（15）提出的代價函數(shù)為

其中：

φ（x）為反投影誤差向量，x為待求解或待優(yōu)化的未知量構(gòu)成的向量；，i 為圖像特征點(diǎn)（控制點(diǎn)）編號，共N 個。根據(jù)非線性優(yōu)化的要求[22]，可僅對φ（x）求解雅可比矩陣，且各為常值，大大簡化了雅可比矩陣的求解。

2.2 位姿估計(jì)的參數(shù)化表達(dá)

φi（x）不僅與式（11）中的諸多內(nèi)外參數(shù)有關(guān)，還與攝像機(jī)畸變系數(shù)有關(guān)，若對所有參數(shù)進(jìn)行迭代優(yōu)化，計(jì)算過程繁瑣。為避免復(fù)雜的求解過程，先通過張正友標(biāo)定方法確定攝像機(jī)內(nèi)參及畸變系數(shù)，使φi（x）僅是旋轉(zhuǎn)矩陣和平移向量的函數(shù)，記為

其中旋轉(zhuǎn)矩陣R的各行向量或列向量兩兩正交，它們雖然有9 個元素，但獨(dú)立元素僅3 個，因此不宜直接對式（18）求雅可比矩陣。

將旋轉(zhuǎn)矩陣R表達(dá)為3 個獨(dú)立變量的經(jīng)典歐拉角

其中α，β，γ 分別代表繞z 軸的翻滾、繞y 軸的俯仰、繞x 軸的偏轉(zhuǎn)。但該形式仍然不利于求解雅可比矩陣，且存在萬向節(jié)死鎖的可能而引入病態(tài)解[15-16]。本文引用近年來流行的李群李代數(shù)法[16，23]來表達(dá)相關(guān)旋轉(zhuǎn)矩陣。

根據(jù)攝像機(jī)小孔投影模型，投影矩陣由內(nèi)外參數(shù)矩陣構(gòu)成，記為MP=KE，其中K為內(nèi)參數(shù)矩陣，E為外參數(shù)矩陣。內(nèi)參數(shù)矩陣K由張正友方法標(biāo)定獲得。E的初始值由DLT 解法獲得，之后每迭代一次對E微調(diào)獲得更高精度的估計(jì)，得

其中：k 表示第k 次估計(jì)；Mk+1有與外參數(shù)矩陣類似的表達(dá)形式，定義為外參數(shù)微調(diào)矩陣。

引入李群李代數(shù)表達(dá)形式，將Mk+1基于群SE（3）的4 階方陣形式，以6 維的李群來表達(dá)。SE（3）可表達(dá)輸出的姿態(tài)空間，其指數(shù)映射可在微小變化時表達(dá)為線性映射的形式。構(gòu)建代表x，y，z 軸移動與旋轉(zhuǎn)的一組基為

將Mk+1視為群中的元素，可由這些基通過指數(shù)映射獲得，即

其中j 表示每個基及其因子的編號。因?yàn)镸k+1表達(dá)了一種微小的變化，根據(jù)指數(shù)映射泰勒展開，可用一階線性近似地表達(dá)

通過這種表達(dá)，問題轉(zhuǎn)化為求解各個αj。根據(jù)式（11）、（23）有

其中s為比例因子，即便有微小偏差，也可由ui，vi的相應(yīng)變化而彌補(bǔ)，同時考慮到的變化量很小，將其略去，s可簡化為一常數(shù)

2.3 雅可比矩陣求解及迭代優(yōu)化

J 中的每個元素（第i行第j列）根據(jù)式（26），得

由式（28）可知，對于不同的控制點(diǎn)，其雅可比偏導(dǎo)只有世界坐標(biāo)Pi不同，其余參數(shù)相同，有利于提高每次迭代的雅可比矩陣計(jì)算速度，簡化了編程。

依據(jù)LM 算法的基本思路，第k+1 次的結(jié)果可由第k次按此迭代得到

其中φ（αk）代表了第k次的反投影誤差向量。文獻(xiàn)[16，22]介紹了LM 算法的步長λ 計(jì)算方法，本文不再展開。每得到一個新的αk +1，根據(jù)式（20），（23）及時更新Ek+1=EkMk+1。迭代過程在‖αk+1-αk‖≤ε 時終止，ε 可由經(jīng)驗(yàn)確定。

得到α 終值后，可由式（19）、（20）、（23）得到最終表達(dá)目標(biāo)物姿態(tài)的3 個歐拉角。

3 實(shí)驗(yàn)與結(jié)果

本文的應(yīng)用背景是機(jī)器人面臨抓取等任務(wù)，需依靠機(jī)器人視覺先獲得目標(biāo)物位姿。為便于角點(diǎn)提取及確定姿態(tài)方位，設(shè)計(jì)了如圖1 所示的兩種木塊及其背景。木塊的頂點(diǎn)為指定控制點(diǎn)，在圖像中特征為角點(diǎn)，圖中坐標(biāo)系為目標(biāo)物的世界坐標(biāo)系，所求的位姿即該坐標(biāo)系相對攝像機(jī)的位置與姿態(tài)。

圖1 位姿估計(jì)的兩種目標(biāo)物Fig.1 Two objects for pose estimation

3.1 位姿估計(jì)精度實(shí)驗(yàn)

為驗(yàn)證本文方法的精度，選擇了經(jīng)典的DLT 方法、EPNP 方法、DLT+數(shù)值LM 方法（即將DLT 方法與通用數(shù)值解法的LM 方法相結(jié)合）與本文方法進(jìn)行對比。其中DLT 方法是線性解法；EPNP 方法雖不是典型線性解法，但仍然是一種封閉解；DLT+數(shù)值LM 方法是將DLT 方法作為數(shù)值LM 法的初始迭代值進(jìn)行的一種非線性優(yōu)化；本文方法將DLT 方法的初始值作為改進(jìn)LM 算法的初始值。圖2 為在攝像機(jī)1 倍焦距下的10 種姿態(tài)，表1 為長方體目標(biāo)物在10 種位姿下經(jīng)典的DLT 方法、EPNP 方法、DLT+數(shù)值LM 法與本文方法的位姿估計(jì)平均反投影誤差對比情況。平均反投影誤差按式（15）計(jì)算?？梢奃LT 方法反投影誤差很大，EPNP 方法誤差較小，本文方法的反投影誤差比同樣是非線性優(yōu)化的DLT+數(shù)值LM方法明顯更小。為了驗(yàn)證本文方法在更多統(tǒng)計(jì)情況下的效果，下面給出2 倍、3 倍焦距下該長方體在20種位姿下的位姿估計(jì)反投影誤差平均值，見表2。由表2 的結(jié)果可知本文方法明顯優(yōu)于其他3 種方法。

表1 1 倍焦距下長方體木塊位姿估計(jì)的平均反投影誤差比較Tab.1 Error comparison of the pose estimation of the cuboid object for 1X focal length of the camera 像素

表2 不同焦距下長方體木塊20 種位姿估計(jì)的平均反投影誤差平均值Tab.2 Mean error comparison of 20 pose estimations of the cuboid object for different focal length of the camera 像素

圖2 長方體目標(biāo)物10 種位姿Fig.2 Ten poses of the cuboid object

接下來，對更為復(fù)雜的T 形木塊在不同焦距各種姿態(tài)下的位姿估計(jì)進(jìn)行實(shí)驗(yàn)，圖3 為10 種典型T形木塊的位姿。表3 為T 形木塊在10 種位姿下DLT方法、EPNP 方法、DLT+數(shù)值LM 與本文方法的位姿估計(jì)平均反投影誤差對比情況。為了獲得大量的統(tǒng)計(jì)結(jié)果，表4 給出2 倍、3 倍焦距下T 形體在20 種位姿下的位姿估計(jì)平均反投影誤差的平均值。從表3～4 結(jié)果可知，DLT 方法的平均反投影誤差依然很大，EPNP 方法誤差較小，本文方法的平均反投影誤差比同樣是非線性估計(jì)的DLT+數(shù)值LM 方法仍然更小。

表3 1 倍焦距下T 形木塊位姿估計(jì)的平均反投影誤差比較Tab.3 Error comparison of the pose estimation of the T-shaped object for 1X focal length of the camera 像素

表4 不同焦距下T 形木塊20 種位姿估計(jì)的平均反投影誤差平均值Tab.4 Mean error comparison of 20 pose estimations of the T-shaped object for different focal length of the camera 像素

圖3 T 形目標(biāo)物10 種位姿Fig.3 Ten poses of the T-shaped object

表5 列出了6 組實(shí)驗(yàn)的最終總平均反投影誤差平均值。結(jié)果表明提出的改進(jìn)方法比經(jīng)典的幾種方法在估計(jì)精度方面均有較大提升。

表5 不同方法位姿估計(jì)的總平均反投影誤差平均值比較Tab.5 Total mean error comparison of the pose estimation for different methods 像素

3.2 本文方法合理性驗(yàn)證

本文設(shè)計(jì)了將長方體木塊與標(biāo)定板相帖合的方式，將兩者的世界坐標(biāo)系完全重疊定義，如圖4所示。將本文方法得到的位姿值與張正友標(biāo)定法得到的外參數(shù)比較以確認(rèn)本文方法的合理性。表6 列出了如圖4 所示的目標(biāo)物位姿具體值和張正友外參標(biāo)定值。通過對比可知，兩者數(shù)值十分接近，其他結(jié)果也是類似的，不一一列出。這說明了本文方法得出的結(jié)果是合理有效的。

圖4 目標(biāo)物與標(biāo)定板帖合的布局Fig.4 Alignment configuration of the object and the calibration board

表6 本文方法與張正友方法的比較Tab.6 Comparison of the proposed method and Zhang′s method

3.3 耗時對比實(shí)驗(yàn)

相比于經(jīng)典的DLT 與EPNP 解析法，非線性優(yōu)化是一個迭代過程，而且其中夾雜著雅可比矩陣計(jì)算、廣義逆矩陣計(jì)算等耗時環(huán)節(jié)，所以正常情況下非線性優(yōu)化耗時將會明顯多于封閉解析法，因此只對兩種非線性優(yōu)化進(jìn)行了耗時測試。本實(shí)驗(yàn)所運(yùn)行的計(jì)算機(jī)是普通的個人筆記本電腦，CPU 為第9 代英特爾i7-9750H，主頻2.6 GHz。

圖5 顯示了長方體木塊在1 倍焦距下的10 種位姿估計(jì)的兩種方法耗時情況。表7 列出了6 組實(shí)驗(yàn)的總體平均耗時。結(jié)果顯示本文改進(jìn)方法與DLT+數(shù)值LM 方法相比，耗時更少，表明本文改進(jìn)方法計(jì)算過程更簡便、迭代更少。本文方法的總體平均耗時均少于90 ms，最終平均耗時為67.48 ms，本文方法能滿足圖像處理領(lǐng)域軟實(shí)時的要求（允許每幀100 ms 左右處理時間）。

圖5 1 倍焦距下長方體木塊位姿估計(jì)耗時Fig.5 Time consuming of the pose estimation of the cuboid object for 1X focal length of the camera

表7 不同方法位姿估計(jì)的平均耗時比較Tab.7 Average time consuming of the pose estimation for different methods

4 結(jié)論

本文研究了精度更高、求解相對簡單的目標(biāo)物三維位姿改進(jìn)方法。實(shí)驗(yàn)結(jié)果表明，本文提出的改進(jìn)方法比經(jīng)典DLT 方法、EPNP 方法在精度上有明顯提高，也比同樣是非線性優(yōu)化的DLT+數(shù)值LM 算法精度有所提高。該方法由于簡化了雅可比矩陣計(jì)算，實(shí)現(xiàn)更為容易，非線性優(yōu)化總體耗時也比DLT +數(shù)值LM 方法更少。因此，本文研究的位姿估計(jì)改進(jìn)方法可為機(jī)器人視覺提供更準(zhǔn)確而及時的信息，滿足機(jī)器人操作目標(biāo)的實(shí)時性要求，具有良好的實(shí)際應(yīng)用價值。