基于凸優(yōu)化的擴(kuò)展手眼標(biāo)定算法

智向陽 ，S?ren Schwertfeger

（1.中國科學(xué)院微系統(tǒng)與信息技術(shù)研究所，上海200050；2.上?？萍即髮W(xué)信息科學(xué)與技術(shù)學(xué)院，上海201210；3.中國科學(xué)院大學(xué)北京100049）

標(biāo)定對于一個機(jī)器人系統(tǒng)來說非常關(guān)鍵。一般的，如果有兩個不同的參考坐標(biāo)系，如相機(jī)、激光雷達(dá)或者機(jī)械臂末端，需要計(jì)算這些參考坐標(biāo)系之間的變換關(guān)系。通常在機(jī)械臂抓取任務(wù)中，一個相機(jī)（眼）會被固定在機(jī)械臂末端（手），以幫助確定待抓取物體的位姿。假設(shè)已經(jīng)通過相機(jī)確定了物體的在相機(jī)坐標(biāo)系的位姿，為了確定物體在機(jī)械臂末端坐標(biāo)系下的位姿，需要標(biāo)定出相機(jī)坐標(biāo)系到機(jī)械臂末端坐標(biāo)系的變換。

此類問題在過去幾十年內(nèi)被廣泛研究，被統(tǒng)稱為手眼標(biāo)定。早期的方法是將手眼標(biāo)定分成兩個部分：旋轉(zhuǎn)標(biāo)定和平移標(biāo)定[1-2]。它們首先標(biāo)定出旋轉(zhuǎn)，接著用標(biāo)定出的旋轉(zhuǎn)來求解平移。然而，這種兩步式算法必然會導(dǎo)致旋轉(zhuǎn)標(biāo)定的誤差傳遞到平移標(biāo)定中去，所以接著出現(xiàn)了同時進(jìn)行旋轉(zhuǎn)和平移標(biāo)定的算法[3-4]。近幾年，基于迭代優(yōu)化的算法[5-6]被提出，這些算法的誤差相比來說更小，但計(jì)算復(fù)雜度往往更高。

傳統(tǒng)手眼標(biāo)定算法假定相機(jī)和機(jī)械臂末端在同一尺度下，也就是說相機(jī)位姿需要已知物體，如棋盤格[7]，來確定。然而，自然場景中往往不存在或者不方便制作這樣一個物體，所以相機(jī)位置一般由SFM確定，此時相機(jī)位置是相對的。這種情況下，傳統(tǒng)的手眼標(biāo)定算法無法使用，但是由于機(jī)械臂末端的位姿的尺度已知，相機(jī)與末端的坐標(biāo)變換關(guān)系仍然可以標(biāo)定出。這種算法一般稱為擴(kuò)展手眼標(biāo)定算法。

首次提出擴(kuò)展手眼標(biāo)定的定義并提出算法的是Andreff等人[8]。Andreff等人推導(dǎo)出手眼標(biāo)定的一種線性化公式，該公式是文獻(xiàn)[7]和本文算法的基礎(chǔ)。Andreff等人建議分開求解旋轉(zhuǎn)和平移，但如前文所述，這樣必然會導(dǎo)致旋轉(zhuǎn)誤差傳播到平移標(biāo)定中去。通過分別擴(kuò)展基礎(chǔ)等式和包含尺度因子的雙四元數(shù)等式，Schmidt等人提出兩種非線性優(yōu)化方法[9]。然而，非線性優(yōu)化需要一個好的初始化，否則可能會陷入局部最小值。Heller等人首先利用Park等人[2]的方法來估計(jì)旋轉(zhuǎn)，接著利用二階錐規(guī)劃估計(jì)平移[10]。這是第一篇嘗試用凸優(yōu)化方法解決擴(kuò)展手眼標(biāo)定問題的論文，但顯然，這種方法仍然存在誤差傳播問題。文獻(xiàn)[11]提出一種將擴(kuò)展手眼標(biāo)定融合進(jìn)三維重建過程的算法，該算法不僅提高了標(biāo)定及重建速度，且精度非常高，是目前最先進(jìn)的擴(kuò)展手眼標(biāo)定方法。

事實(shí)上，手眼標(biāo)定并不局限于標(biāo)定相機(jī)和機(jī)械臂末端。只要兩個設(shè)備固定在一起且能夠估計(jì)出多組合適的運(yùn)動，則兩個設(shè)備坐標(biāo)系之間的變換關(guān)系就可以被標(biāo)定出來。最近幾年，傳統(tǒng)手眼標(biāo)定和擴(kuò)展手眼標(biāo)定也出現(xiàn)在其他標(biāo)定領(lǐng)域。Esquivel和Koch利用手眼標(biāo)定算法標(biāo)定視野無重疊的多相機(jī)系統(tǒng)[12]。文獻(xiàn)[13]則利用手眼標(biāo)定算法標(biāo)定四足機(jī)器人的足與相機(jī)的相對姿態(tài)。

文中將凸優(yōu)化應(yīng)用到擴(kuò)展手眼標(biāo)定。凸優(yōu)化是優(yōu)化的子領(lǐng)域之一，凸的性質(zhì)保證了任意局部最小值必然是全局最小值，所以無需初始化。

1 問題描述

不同于相機(jī)標(biāo)定，手眼標(biāo)定無需待標(biāo)定的兩個坐標(biāo)系下有對應(yīng)的特征點(diǎn)，相反地，手眼標(biāo)定利用多組齊次變換對，即{A1,A2,…,An}和{ }B1,B2,…,Bn。如圖1所示，Ai和Bi分別表示相機(jī)運(yùn)動的變換和機(jī)械臂末端運(yùn)動的變換。X代表待解的從機(jī)械臂末端坐標(biāo)系到相機(jī)坐標(biāo)系的變換。通過簡單計(jì)算可知，A，B和X之間滿足如下關(guān)系：

圖1 手眼標(biāo)定示意圖

一般的，相機(jī)運(yùn)動變換可以通過特殊的物體，如棋盤格，或者SFM求解得出。但是由于沒有參考，SFM無法確定絕對尺度，也就是說SFM估計(jì)出的相機(jī)位移包含一個未知尺度因子。與此同時，機(jī)械臂末端的位姿可以通過標(biāo)定好的機(jī)械臂模型和運(yùn)動學(xué)計(jì)算。

公式（1）可以分解成旋轉(zhuǎn)和平移兩部分：

其中λ即相機(jī)位移中的未知尺度因子，也是區(qū)分?jǐn)U展手眼標(biāo)定和傳統(tǒng)手眼標(biāo)定的參數(shù)。Tsai等人證明至少兩組旋轉(zhuǎn)軸不平行的運(yùn)動才能唯一確定X[1]。當(dāng)然，由于傳感器測量噪聲的存在，更多運(yùn)動將有助于減小誤差。此外，當(dāng)λ為1時，擴(kuò)展手眼標(biāo)定退化成為傳統(tǒng)手眼標(biāo)定，也就是說傳統(tǒng)手眼標(biāo)定可以看作是擴(kuò)展手眼標(biāo)定的一種特例。因此，擴(kuò)展手眼標(biāo)定算法也可以用于求解傳統(tǒng)手眼標(biāo)定問題。

2 提出的新算法

本章節(jié)首先會引入將公式（1）線性化的基本理論，接著提出兩種凸優(yōu)化方法求解線性公式得到手眼變換結(jié)果。

2.1 線性方程

線性方程有著廣泛的應(yīng)用場景并已經(jīng)被研究數(shù)十年，所以其理論是相對完整的。如果能將公式（1）轉(zhuǎn)換成線性方程組，則將會非常容易求解。為了達(dá)到此目的，本文首先引入兩個定義：

定義1（列向量運(yùn)算符）給定一個矩陣V∈?m×n，將V的列向量表示為vec(V)∈?mn，將V的行向量表示為v1,v2,…,vn∈?n，則

此外，定義 vec-1(V)為 vec(V)的逆運(yùn)算，即vec-1(vec(V))=V。

定義2（克羅內(nèi)克積）給定兩個矩陣M∈?o×p和N∈?q×r，定義二者的克羅內(nèi)克積，M?N，為

可以證明下面的等式成立:

其中，X，Y和Z是任意的維度相容的矩陣。

利用以上兩個定義及其性質(zhì)，將公式（2）和公式（3）改寫為：

通過整合公式（7）和公式（8），可以得到公式（9）：

顯然，公式（9）有無窮多解，但事實(shí)上還有另外一個約束：RX是旋轉(zhuǎn)矩陣，則RX正交且其行列式為1。根據(jù)這個性質(zhì)求解出公式（9）后計(jì)算約束參數(shù)α。Andreff等人證明α的值為：

其中r=vec-1(v)，其中v是公式（9）的解的前9個元素，det(r)是r的行列式。當(dāng)然，由于噪聲的存在，估計(jì)出來的旋轉(zhuǎn)矩陣往往不是正交的，所以求解時需要加入正交化，本文采用QR分解來進(jìn)行正交化。

2.2 利用凸優(yōu)化求解線性方程

假設(shè)有N次手眼運(yùn)動，可以得到N組等式（9），定義：

定義L2范數(shù)代價函數(shù)為：

顯然，該函數(shù)是凸函數(shù)。值得注意的是，x=013×1是公式（9）的一個解。為了避免代價函數(shù)趨向于0，需要設(shè)置一個約束。本文約束x的第一個元素，x1，大于一個很小的正數(shù)ò，本文設(shè)置ò為10-6?？傊?，最終的凸優(yōu)化模型為：

公式（12）可以利用凸優(yōu)化工具箱，如CVX[14]，求解。

在文獻(xiàn)[5]和文獻(xiàn)[10]中，L∞范數(shù)展示出了非常好的結(jié)果，受此啟發(fā)，本文也提出了基于L∞范數(shù)的代價函數(shù)：

該函數(shù)同樣也是凸函數(shù)，定義基于L∞范數(shù)的凸優(yōu)化模型：

基于凸優(yōu)化求解擴(kuò)展手眼標(biāo)定算法的過程總結(jié)在算法1中。

算法1基于凸優(yōu)化的擴(kuò)展手眼標(biāo)定算法

輸入：N對手眼運(yùn)動變換

基于(12)或者(14)建立L2范數(shù)或L∞范數(shù)凸優(yōu)化模型；

利用CVX求解x；

2.3 消除λ

值得注意的是，手眼標(biāo)定中的旋轉(zhuǎn)可以由公式（2）求解而不受平移和λ影響。所以一些利用公式（2）求解旋轉(zhuǎn)的傳統(tǒng)手眼標(biāo)定算法，如文獻(xiàn)[1]和[2]提出的算法，仍然可以用在擴(kuò)展手眼標(biāo)定中。接著，在公式（3）中，如果λ能被消除，tX就可以簡單的求解出來。下面本文展示如何消除λ，在此之前先引入另外一個定義。

圖2 變換估計(jì)誤差vs.噪聲標(biāo)準(zhǔn)差

隨著λ被消除，則文獻(xiàn)[1]和[2]中提出的方法可以利用公式（16）來求解tX。

更重要的是，公式（16）可以轉(zhuǎn)化為線性方程：

合并公式（16）和（17），可以得到公式（1）的另一種線性表示：

顯然，也可以利用公式（18）構(gòu)建類似于（12）和（14）的凸優(yōu)化模型來求解手眼變換。

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

為了驗(yàn)證提出的新算法的性能，本文設(shè)計(jì)了仿真數(shù)據(jù)和實(shí)際數(shù)據(jù)實(shí)驗(yàn)。首先，為了和已有算法進(jìn)行對比，利用本文中提出的消除λ的方法擴(kuò)展了文獻(xiàn)[1]和文獻(xiàn)[2]提出的算法，并將其命名為“extended-Tsai”和“extended-Park”。擴(kuò)展后的算法與原始算法類似，仍然是先估計(jì)旋轉(zhuǎn)后估計(jì)平移兩個步驟。這里，將本文提出的算法命名為“CVXL2”和“CVX-L∞”，分別對應(yīng)于（12）和（14），此外，對應(yīng)的消除λ后的算法被命名為“CVX-L2-E”和“CVX-L∞-E”。為了對比，本文還實(shí)現(xiàn)了文獻(xiàn)[8]中提出的算法：首先利用SVD分解求解旋轉(zhuǎn)，然后用最小二乘法估計(jì)平移，本文將其命名為“Andreff”。硬件平臺方面，所有算法都運(yùn)行在一臺搭載英特爾酷睿i7-4790 CPU以及8G RAM的PC機(jī)上。軟件環(huán)境為運(yùn)行在64位Ubuntu 14.04上的MATLAB 2015b。

3.1 仿真數(shù)據(jù)實(shí)驗(yàn)

仿真數(shù)據(jù)的生成方式如下：首先，本文設(shè)定一個固定的變換X和尺度因子λ。然后隨機(jī)生成數(shù)個手運(yùn)動（B），接著利用等式A=XBX-1計(jì)算出眼運(yùn)動（A）。最后，用尺度因子λ縮放A中的平移。此外，為了評估算法的抗噪聲能力，本文在手眼運(yùn)動的旋轉(zhuǎn)和平移都加入了高斯隨機(jī)噪聲，噪聲的均值為0，標(biāo)準(zhǔn)差σ從以0.01的步長從0增加至0.2。對于每一個不同的σ，生成100組仿真數(shù)據(jù)，每組數(shù)據(jù)包含30對手眼運(yùn)動。

為了量化誤差方便對比各算法的性能，本文定義旋轉(zhuǎn)和平移誤差如下：

圖2（a）和2（b）展示了當(dāng)從0.0到0.2變化時，估計(jì)誤差的變化情況。從圖中可以看出，其中“CVX-L2”在旋轉(zhuǎn)和平移方面的表現(xiàn)均為最優(yōu)。另一方面，兩種基于L2范數(shù)的方法性能要優(yōu)于另外兩種基于L∞范數(shù)的方法，說明L2范數(shù)更加適用于手眼標(biāo)定這一應(yīng)用場景。

圖2（c）和2（d）分別是是當(dāng)σ為0.2時的旋轉(zhuǎn)和平移誤差箱線圖。從圖中可以看出，“CVX-L2”的分布更加密集，表明該方法相比于其他方法更加魯棒。然而，大的異常值表明“extended-Tsai”和“extended-Park”對噪聲非常敏感。

3.2 真實(shí)數(shù)據(jù)實(shí)驗(yàn)

想要準(zhǔn)確的測量相機(jī)與其他傳感器的坐標(biāo)系變換并不容易，因?yàn)橄鄼C(jī)的位姿往往無法直接測量，這就導(dǎo)致無法獲得手眼變換的真值去驗(yàn)證手眼標(biāo)定的算法的性能。一些作者[5-6，9]通過計(jì)算估計(jì)出的相機(jī)運(yùn)動A?i=XBiX-1與實(shí)際運(yùn)動Ai的差值來評估手眼標(biāo)定算法的性能，但這樣的問題在于Ai是通過SFM得到的，其本身就存在誤差。文獻(xiàn)[10]則將手動測量結(jié)果作為真值，但這種方法顯然也存在誤差。為了能夠解決沒有真值的問題，本文創(chuàng)新地提出了一種特別的標(biāo)定應(yīng)用來評估算法。

圖3 （a）為實(shí)際數(shù)據(jù)實(shí)驗(yàn)場景示意圖；（b）為棋盤格標(biāo)定法得到的相機(jī)位姿

文中假設(shè)實(shí)驗(yàn)中在標(biāo)定“兩個”相機(jī)，但事實(shí)上這兩個相機(jī)是同一個：本文搭建一個特殊的場景，場景中包含一個標(biāo)定板，如圖3（a）所示。然后，移動相機(jī)的同時采集一系列圖像，并且保證在移動過程中標(biāo)定板始終完整地出現(xiàn)在相機(jī)視野內(nèi)。采集完數(shù)據(jù)后，一方面，本文用棋盤格標(biāo)定法[7]來確定相機(jī)位姿；而另一方面，忽略場景中有棋盤格，而用SFM來確定相機(jī)位姿。本文將用棋盤格標(biāo)定的“相機(jī)”作為“手”，將用SFM標(biāo)定的“相機(jī)”作為“眼”，而“手”和“眼”其實(shí)是同一個相機(jī)，所以手眼標(biāo)定的真值是零。通過這種搭建的特殊的場景，本文克服了沒有真值的問題。

文中使用一個三星Galaxy S6手機(jī)的后置相機(jī)，關(guān)閉其自動對焦，并將分辨率設(shè)為3264×1836像素。標(biāo)定板每個格子的大小為28×28 mm。本文采集了不同位置不同角度的17張圖像。首先標(biāo)定相機(jī)的內(nèi)參和外參，標(biāo)定后相機(jī)的位姿如圖3（b）所示。計(jì)算機(jī)視覺庫Theia[15]中包含了SFM整個流程的算法，本文利用其計(jì)算相機(jī)的位姿。為了減少異常值，本文從17張圖像中去15張作為一組樣本，這樣總共有組不同的樣本。而每組中有14個獨(dú)立的相機(jī)運(yùn)動。各種算法[16]的標(biāo)定結(jié)果如圖4所示，由于平移真值是03×1，為了避免在公式（19）計(jì)算誤差時出現(xiàn)除0，本文直接將算法估計(jì)出來的平移的L2范數(shù)作為平移誤差。

圖4 實(shí)際實(shí)驗(yàn)數(shù)據(jù)下不同算法的誤差對比

圖4表明“CVX-L2-E”誤差最小，“CVX-L2”其次。該實(shí)驗(yàn)再次表明L2范數(shù)在手眼標(biāo)定這個問題上的性能要好于L∞范數(shù)?！癈VX-L2-E”的旋轉(zhuǎn)誤差較大，表明L∞范數(shù)對噪聲更加敏感。由于誤差傳播的原因，“extended-Tsai”和“extended-Park”仍然性能最差。

4 結(jié)束語

文中提出了兩種基于凸優(yōu)化的擴(kuò)展手眼標(biāo)定算法。第一種方法是將AX=XB轉(zhuǎn)化為線性方程組，然后建立L2范數(shù)或L∞范數(shù)優(yōu)化模型，用凸優(yōu)化工具箱求解。第二種方法則先消除了未知的尺度因子，然后用類似于方法一的方式求解。仿真數(shù)據(jù)實(shí)驗(yàn)和實(shí)際數(shù)據(jù)實(shí)驗(yàn)證明基于L2范數(shù)的凸優(yōu)化算法在精度方面性能最好[17]。在以后的工作中，我們將嘗試將本文提出的算法應(yīng)用到相機(jī)和其他傳感器，如3D激光雷達(dá)的標(biāo)定中去。