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

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

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

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

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

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

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

DLT 線性模型根據(jù)攝像機小孔成像原理，不考慮鏡頭畸變等非線性因素，直接以線性變換的形式建立靶標特征點的世界坐標（X，Y，Z，1）T與相應圖像坐標（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 個特征點，可列出方程組

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中：

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

2.2 位姿估計的參數(shù)化表達

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 實驗與結(jié)果

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

圖1 位姿估計的兩種目標物Fig.1 Two objects for pose estimation

3.1 位姿估計精度實驗

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

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

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

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

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

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

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

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

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

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

3.2 本文方法合理性驗證

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

圖4 目標物與標定板帖合的布局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 耗時對比實驗

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

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

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

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

4 結(jié)論

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