本發(fā)明涉及一種基于凸松弛全局優(yōu)化算法的機器人手眼標(biāo)定方法，涉及工業(yè)機器人視覺標(biāo)定技術(shù)領(lǐng)域，用于關(guān)節(jié)臂坐標(biāo)測量機，機器人手術(shù)，服務(wù)機器人技術(shù)領(lǐng)域。

背景技術(shù)：

近年來，隨著科學(xué)技術(shù)的迅猛發(fā)展，機器人已經(jīng)逐步進(jìn)入我們的社會生活中，如在智能裝配、自主導(dǎo)航、逆向工程、焊接工程等領(lǐng)域得到了廣泛應(yīng)用。機器人手眼標(biāo)定作為機器人視覺的關(guān)鍵技術(shù)之一，一直以來都是機器視覺領(lǐng)域中的研究熱點。在機器人手眼視覺系統(tǒng)中，往往需要將攝像機與機器人的手臂結(jié)合起來，才能獲得空間中目標(biāo)物體相對于機器人執(zhí)行器的相對位置信息，然后精確控制機器人執(zhí)行相應(yīng)的動作。這一過程中的必要環(huán)節(jié)是獲取攝像機與機器人末端執(zhí)行器的坐標(biāo)變換關(guān)系，即手眼標(biāo)定問題。

1989年，Tsai在機器人視覺關(guān)鍵技術(shù)中，提出手眼標(biāo)定問題并建立手眼標(biāo)定誤差模型。同年，Shiu提出了基于軸角變換的手眼標(biāo)定閉環(huán)線性解法，分步求出手眼變換矩陣中的旋轉(zhuǎn)矩陣和平移向量。1995年，Horaud提出了基于單位四元數(shù)的一步非線性法和兩步線性法，該算法用單位四元數(shù)參數(shù)化旋轉(zhuǎn)矩陣，簡化了手眼標(biāo)定方程求解步驟。1999年，Daniilidis采用對偶四元數(shù)和螺旋理論的方法得出了線性閉環(huán)解法，該算法簡化了計算步驟，但對測量噪聲較為敏感。同年，Andreff提出了基于矩陣直積的線性閉環(huán)解法，該算法適用于標(biāo)定小角度移動的測量場合，但求解的結(jié)果通常情況下并不滿足旋轉(zhuǎn)矩陣的正交和單位特性，需要對其再加入正交約束。2001年，Andreff以機器人視覺伺服系統(tǒng)為背景，提出利用運動恢復(fù)結(jié)構(gòu)(Structure from motion)的算法求解相機的方位信息，擺脫了手眼標(biāo)定過程中對靶標(biāo)的依賴，擴展了手眼標(biāo)定的應(yīng)用場景。2006年，Strobl利用歐氏運動群SE(3)描述機械手的剛體變換，提出一種自適應(yīng)加權(quán)優(yōu)化的手眼標(biāo)定計算模型。

此外，考慮到標(biāo)定數(shù)據(jù)的選擇同樣會影響手眼變換矩陣的求解精度，2008年，Schmidt在Tsai建立的標(biāo)定誤差數(shù)學(xué)模型基礎(chǔ)上，提出利用矢量化編碼技術(shù)構(gòu)建手眼標(biāo)定數(shù)據(jù)篩選機制，提高了手眼變換矩陣的求解精度。2010年，Malti提出一種改進(jìn)的對偶四元數(shù)算法，為了避免標(biāo)定數(shù)據(jù)中粗大誤差對求解過程的影響，增加隨機抽樣一致性算法(RANSAC)對數(shù)據(jù)進(jìn)行預(yù)篩選，增強了標(biāo)定結(jié)果的精度和魯棒性。同年，毛劍飛基于四元數(shù)理論，給出了手眼標(biāo)定算法幾何意義的對照，對標(biāo)定方程各種解的情況進(jìn)行了討論。2011年，Zhao提出采用凸優(yōu)化理論計算手眼變換矩陣的方法，提高了計算結(jié)果的魯棒性，但求解過程較為復(fù)雜。同年，王君臣基于極大似然估計(Maximum Likelihood)的概念建立測度目標(biāo)函數(shù)，提出一種手眼標(biāo)定非線性優(yōu)化算法。2015年，王金橋提出利用遺傳算法解決關(guān)節(jié)臂視覺檢測系統(tǒng)中的手眼標(biāo)定問題。以上研究，有的只關(guān)注于手眼標(biāo)定方程的參數(shù)化方法或者求解算法問題，還有的只是針對影響標(biāo)定方程求解精度的某些因素進(jìn)行分析，綜合考慮標(biāo)定方程的求解算法及誤差影響因素的研究還比較少，而事實上，標(biāo)定數(shù)據(jù)的篩選和標(biāo)定方程的求解算法都會直接影響手眼變換矩陣的求解精度，因此有必要綜合在一起進(jìn)行研究。

因此，需要一種基于四元數(shù)理論的凸松弛全局最優(yōu)化手眼標(biāo)定算法，該方法不需要初值估計，且可以保證手眼標(biāo)定結(jié)果收斂到全局最優(yōu)解，從而提高手眼變換矩陣的求解精度和魯棒性。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于提供一種基于四元數(shù)理論的凸松弛全局最優(yōu)化手眼標(biāo)定方法，采用RANSAC算法對標(biāo)定數(shù)據(jù)中旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選，求解方程時提出一種基于四元數(shù)理論的凸松弛全局最優(yōu)化手眼標(biāo)定算法。該方法不需要初值估計，且可以保證手眼標(biāo)定結(jié)果收斂到全局最優(yōu)解，從而提高手眼變換矩陣的求解精度和魯棒性。

為了實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案如下：

一種基于凸松弛全局優(yōu)化算法的機器人手眼標(biāo)定方法，包括以下步驟：

S1:在求解標(biāo)定方程之前，設(shè)定角度閾值初值，利用自適應(yīng)RANSAC算法對標(biāo)定數(shù)據(jù)集Cⁿ中旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選。

S2:利用四元數(shù)法參數(shù)化手眼變換矩陣X，將S1中篩選出的滿足角度閾值要求的靶標(biāo)世界坐標(biāo)系到攝像機坐標(biāo)系的變換矩陣A和機械手末端執(zhí)行器坐標(biāo)系到機械手基坐標(biāo)系的變換矩陣B代入手眼標(biāo)定方程AX＝XB，建立多項式幾何誤差目標(biāo)函數(shù)和約束條件。

S3:采用基于線性矩陣不等式(LMI)凸松弛全局優(yōu)化算法對S2參數(shù)化的手眼標(biāo)定方程進(jìn)行求解，得到滿足標(biāo)定方程的全局最優(yōu)參數(shù)值，然后利用該參數(shù)值得到全局最優(yōu)的手眼變換矩陣。

本發(fā)明考慮到機械手末端相對運動旋轉(zhuǎn)軸之間的夾角對標(biāo)定方程求解精度的影響，首先利用隨機抽樣一致性(Random Sample And Consensus，RANSAC)算法對標(biāo)定數(shù)據(jù)中旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選，再利用四元數(shù)參數(shù)化旋轉(zhuǎn)矩陣，建立多項式幾何誤差目標(biāo)函數(shù)和約束，采用基于線性矩陣不等式(linear matrix inequality，LMI)凸松弛全局優(yōu)化算法求解全局最優(yōu)手眼變換矩陣。

本發(fā)明采用RANSAC算法對標(biāo)定數(shù)據(jù)中旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選，可減小標(biāo)定數(shù)據(jù)的選擇對標(biāo)定方程求解精度的影響；提出一種基于四元數(shù)理論的凸松弛全局最優(yōu)化手眼標(biāo)定算法，有效解決了針對機器人運動學(xué)正解及相機的外參數(shù)標(biāo)定存在誤差時，基于非線性最優(yōu)化的手眼標(biāo)定算法無法確保目標(biāo)函數(shù)收斂到全局最小值的問題，提高了標(biāo)定算法的魯棒性。

應(yīng)當(dāng)理解，前述大體的描述和后續(xù)詳盡的描述均為示例性說明和解釋，并不應(yīng)當(dāng)用作對本發(fā)明所要求保護(hù)內(nèi)容的限制。

附圖說明

參考隨附的附圖，本發(fā)明更多的目的、功能和優(yōu)點將通過本發(fā)明實施方式的如下描述得以闡明，其中：

圖1示意性示出手眼標(biāo)定基本原理示意圖。

圖2示意性示出手眼標(biāo)定實驗裝置圖。

圖3示意性示出本發(fā)明的整體方法流程圖。

圖4示意性示出兩種不同優(yōu)化算法的手眼標(biāo)定幾何誤差比較圖。

圖5示意性示出兩種不同優(yōu)化算法的手眼標(biāo)定幾何誤差棒比較圖。

具體實施方式

通過參考示范性實施例，本發(fā)明的目的和功能以及用于實現(xiàn)這些目的和功能的方法將得以闡明。然而，本發(fā)明并不受限于以下所公開的示范性實施例；可以通過不同形式來對其加以實現(xiàn)。說明書的實質(zhì)僅僅是幫助相關(guān)領(lǐng)域技術(shù)人員綜合理解本發(fā)明的具體細(xì)節(jié)。

在下文中，將參考附圖描述本發(fā)明的實施例。在附圖中，相同的附圖標(biāo)記代表相同或類似的部件，或者相同或類似的步驟。

下面結(jié)合附圖與實施例對本發(fā)明作進(jìn)一步說明。

圖3所示為本發(fā)明的整體方法流程圖。如圖3所示，一種基于凸松弛全局優(yōu)化算法的機器人手眼標(biāo)定方法，包括以下步驟：

S1:考慮到機械手末端相對運動旋轉(zhuǎn)軸之間的夾角對標(biāo)定方程求解精度的影響，在求解標(biāo)定方程之前，設(shè)定角度閾值初值，利用自適應(yīng)RANSAC算法對標(biāo)定數(shù)據(jù)集Cⁿ中旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選。

S2:利用四元數(shù)法參數(shù)化手眼變換矩陣X，將S1中篩選出的滿足角度閾值要求的靶標(biāo)世界坐標(biāo)系到攝像機坐標(biāo)系的變換矩陣A和機械手末端執(zhí)行器坐標(biāo)系到機械手基坐標(biāo)系的變換矩陣B代入手眼標(biāo)定方程AX＝XB，建立多項式幾何誤差目標(biāo)函數(shù)和約束條件。

S3:采用基于線性矩陣不等式(LMI)凸松弛全局優(yōu)化算法對S2參數(shù)化的手眼標(biāo)定方程進(jìn)行求解，得到滿足標(biāo)定方程的全局最優(yōu)參數(shù)值，然后利用該參數(shù)值得到全局最優(yōu)的手眼變換矩陣。

為了進(jìn)一步驗證本發(fā)明提出的算法精度和魯棒性，設(shè)計實測實驗，所使用的實驗裝置如圖2所示，包括機械手、平面靶標(biāo)、工業(yè)相機和鏡頭，所述機械手采用日本電裝公司DENSO機械手VS-6577GM，X、Y、Z各方向的重復(fù)定位精度為±0.02mm；所述工業(yè)相機采用凱視佳公司UD274M/C型號的CCD工業(yè)相機，分辨率為1628×1236，像元尺寸為4.4μm；所述鏡頭選用COMPUTAR 12mm鏡頭；所述平面靶標(biāo)采用11mm×11mm棋盤格平面靶標(biāo)；所述鏡頭用于對11mm×11mm棋盤格平面靶標(biāo)進(jìn)行相機參數(shù)標(biāo)定。

具體的，根據(jù)S1，首先將攝像機固定在機械手末端執(zhí)行器法蘭盤上，利用DENSO機械手帶動攝像機每次選取N＝15個不同位姿對平面靶標(biāo)拍照成像，得到靶標(biāo)世界坐標(biāo)系到不同姿態(tài)的攝像機坐標(biāo)系的變換矩陣A_i和不同姿態(tài)下機械手末端執(zhí)行器坐標(biāo)系到機械手基坐標(biāo)系的變換矩陣B_i，對其兩兩進(jìn)行組合可以得到M＝N(N-1)/2組手眼標(biāo)定數(shù)據(jù)集Cⁿ。在標(biāo)定數(shù)據(jù)集Cⁿ中利用自適應(yīng)RANSAC算法預(yù)先對標(biāo)定數(shù)據(jù)進(jìn)行角度篩選，得到滿足閾值要求的標(biāo)定數(shù)據(jù)集C_X。

具體地，根據(jù)S2，參數(shù)化手眼變換矩陣X，以最小化標(biāo)定方程AX＝XB為幾何誤差目標(biāo)函數(shù)，以單位四元數(shù)的性質(zhì)為約束條件，建立關(guān)于手眼變換矩陣X的多元多項式優(yōu)化問題。

具體地，根據(jù)S3，將篩選得到的標(biāo)定數(shù)據(jù)集C_X中的變換矩陣A_j和B_j代入手眼標(biāo)定多項式優(yōu)化問題，利用凸松弛全局優(yōu)化算法求解變換矩陣X，進(jìn)行誤差評定。

為了更清楚的描述本發(fā)明的技術(shù)方案，以下將列舉一具體實施例描述步驟S1，需要說明的是，本具體實施例并不能用來限制本發(fā)明，在此不做詳細(xì)贅述。

進(jìn)一步地，所述S1中的標(biāo)定數(shù)據(jù)集Cⁿ的篩選方法，篩選機械手末端執(zhí)行器的相對運動數(shù)據(jù)步驟為：

S101：建立標(biāo)定數(shù)據(jù)篩選模型。Tsai根據(jù)一系列引理得出的標(biāo)定方程中旋轉(zhuǎn)矩陣的誤差模型表示為：

其中，

其中，∠(r_B12,r_B23)表示兩次相對運動的單位旋轉(zhuǎn)軸的夾角，θ_B12，θ_B23分別表示機械手末端執(zhí)行器從位姿1到位姿2以及從位姿2到位姿3的旋轉(zhuǎn)角，由旋轉(zhuǎn)矩陣的誤差公式可知：增大機械手末端執(zhí)行器兩次相對運動旋轉(zhuǎn)軸之間的夾角或者增大末端執(zhí)行器兩次位姿變化的旋轉(zhuǎn)角度可以改善手眼變換矩陣中旋轉(zhuǎn)矩陣的求解精度，當(dāng)機械手末端執(zhí)行器兩次相對運動旋轉(zhuǎn)軸之間的夾角接近90°時旋轉(zhuǎn)矩陣的均方根誤差趨于最小值。因此，定義r_ij和r_kl分別表示機械手末端執(zhí)行器從位姿i到位姿j及從位姿k到位姿l的單位旋轉(zhuǎn)軸，θ_ij,kl表示兩次相對運動的單位旋轉(zhuǎn)軸的夾角，如下式所示，當(dāng)θ_ij,kl接近90°或者θ_t接近0°時，旋轉(zhuǎn)矩陣的誤差最小：

θ_ij,kl＝∠(r_ij,r_kl)，θ_t＝||90-θ_ij,kl||

S102：將攝像機固定在機械手末端執(zhí)行器上，采集標(biāo)定數(shù)據(jù)。利用機械手帶動攝像機每次選取N個不同位姿對平面靶標(biāo)拍照成像，兩兩進(jìn)行組合可以得到M＝N(N-1)/2組手眼標(biāo)定數(shù)據(jù)集Cⁿ，利用羅德里格旋轉(zhuǎn)公式將標(biāo)定數(shù)據(jù)集Cⁿ中的變換矩陣A，B表示成軸角形式。

S103：利用自適應(yīng)RANSAC算法對標(biāo)定數(shù)據(jù)集Cⁿ中機械手末端執(zhí)行器兩次相對運動旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選。由于至少需要兩組旋轉(zhuǎn)軸非平行的標(biāo)定數(shù)據(jù)就可以唯一確定手眼變換矩陣，所以設(shè)定最少數(shù)據(jù)點n＝2，設(shè)定滿足角度閾值要求的內(nèi)點比例初值w₀＝0.1，K次抽樣中所有樣本均為壞樣本的概率z＝0.02，角度閾值初值θ₀＝5°，終止RANSAC抽樣的條件為滿足角度閾值的標(biāo)定數(shù)據(jù)集C_X≥15，采用自適應(yīng)算法抽樣并更新w₀和θ₀，直到標(biāo)定數(shù)據(jù)集C_X≥15，記下此時的角度閾值θ_t，終止抽樣，將篩選得到的標(biāo)定數(shù)據(jù)集C_X代入S2求解手眼變換矩陣X。

為了更清楚的描述本發(fā)明的技術(shù)方案，以下將列舉一具體實施例描述步驟S2，需要說明的是，本具體實施例并不能用來限制本發(fā)明，在此不做詳細(xì)贅述。

進(jìn)一步地，所述S2四元數(shù)法參數(shù)化手眼標(biāo)定方程的步驟為：

S201：圖1是手眼標(biāo)定基本原理示意圖。如圖1所示，定義A₁，A₂表示靶標(biāo)世界坐標(biāo)系到兩個不同姿態(tài)的攝像機坐標(biāo)系的變換矩陣，B₁，B₂表示為兩次不同姿態(tài)的機械手末端執(zhí)行器坐標(biāo)系到機械手基坐標(biāo)系的變換矩陣，X表示攝像機坐標(biāo)系到機械手末端執(zhí)行器坐標(biāo)系的變換矩陣，則手眼關(guān)系可以表示為：

AX＝XB

其中，A、B、X都為4×4的矩陣，展開為旋轉(zhuǎn)矩陣和平移向量的形式為：

用四元數(shù)法參數(shù)化變換矩陣X中的旋轉(zhuǎn)矩陣R_x表示為：

其中，旋轉(zhuǎn)矩陣R(q_x)可以表示為：

S202：以最小化標(biāo)定方程AX＝XB為幾何誤差目標(biāo)函數(shù)，以單位四元數(shù)的性質(zhì)為約束條件，建立關(guān)于變量X(q_x,t_x)多元多項式優(yōu)化問題：

S203：利用S3中LMI凸松弛全局優(yōu)化算法把上述多項式函數(shù)優(yōu)化問題松弛為半正定規(guī)劃問題求解。經(jīng)解算可得目標(biāo)函數(shù)f為4次7元的多項式函數(shù)，由85個不同的單項式組成，由于求解過程中至少會得到2個全局最優(yōu)解，因此需要增加約束條件q_x≥0，為了增加求解的數(shù)值穩(wěn)定性，需先將A_i，B_i中平移向量歸一化，再根據(jù)實際要求對變量t_x加入線性約束，將平移向量的模限制在有限的空間內(nèi)(例如，可設(shè)||t_x||²≤1),從而確保半正定規(guī)劃的內(nèi)點法可以高效的進(jìn)行解算。

為了更清楚的描述本發(fā)明的技術(shù)方案，以下將列舉一具體實施例描述步驟S3，需要說明的是，本具體實施例并不能用來限制本發(fā)明，在此不做詳細(xì)贅述。

進(jìn)一步地，所述S3中，選用的基于線性矩陣不等式(LMI)凸松弛全局優(yōu)化算法是由JanHeller研究的，在本發(fā)明中應(yīng)用該算法僅針對對偶四元數(shù)的參數(shù)化方法解決機器人手眼標(biāo)定問題，其步驟為：

設(shè)W_i(X)為X＝(x₁,x₂,…,x_m,)∈C^m上的標(biāo)量多元多項式，則多元多項式的優(yōu)化問題通?？梢悦枋鰹椋?/p>

minW₀(X)

s.t.W_i(X)≥0，i＝1,...,n

其中X＝(x₁,x₂,...,x_m)^T∈C^m

對于一個δ階松弛多項式優(yōu)化問題W(X)，定義v_δ(X)為t階多項式的基，表示為{1,x₁,x₂,…,x_n,x₁²,x₁x₂,…,x₁x_n,x₂²,x₂x₃,…,x_n²,…,x₁^t,…,x_n^t}，即v_δ(X)表示x中的元素x_i互乘得到的次數(shù)不高于t的所有單項式和常量1構(gòu)成的集合。

定義M_δ(y)，M_δ(Wy)為按照基v_δ(X)排列順序組成的半正定矩陣(k<t)，例如，當(dāng)n＝2，t＝2時，W(X)＝3-2x₁-x₁²-x²時，則半正定矩陣M₂(y)和M₁(Wy)可以表示為：

因而，多項式LMI優(yōu)化方法一般可以歸納為如下3個步驟：

S301：使用提升變量線性化目標(biāo)函數(shù)和約束項W(X)：使用替代目標(biāo)函數(shù)W₀(X)和約束項W_i(X)中的單項式

S302：添加半正定矩陣約束：按照δ階多項式的基v_δ(X)的排列順序，添加半正定矩陣約束M_δ(y)≥0，M_δ(Wy)≥0。

S303：將凸松弛多項式優(yōu)化問題轉(zhuǎn)化為半正定規(guī)劃問題求解：修改松弛系數(shù)δ，每一階松弛后，可以使用對偶內(nèi)點算法求解由前兩步組成的新半正定規(guī)劃問題，直到求解速度和精度滿足要求為止。實際上，一般當(dāng)δ≤4時，LMI全局優(yōu)化算法即可收斂到全局最優(yōu)解。

為了證明算法的可靠性，結(jié)合圖4、圖5對2種算法進(jìn)行比較分析。圖4示意性示出兩種不同優(yōu)化算法的手眼標(biāo)定幾何誤差比較圖，隨機抽取一組實測實驗數(shù)據(jù)，兩種手眼標(biāo)定算法在M＝105種組合下的手眼標(biāo)定幾何誤差的分散情況如圖4所示，由圖可知：本發(fā)明的全局優(yōu)化算法在M＝105種組合下的手眼標(biāo)定幾何誤差均小于非線性最優(yōu)化算法。

圖5示意性示出兩種不同優(yōu)化算法的手眼標(biāo)定幾何誤差棒圖。每次機械手變換Q＝15個不同的位置，進(jìn)行10組完全獨立的重復(fù)標(biāo)定實驗，每組實驗中對應(yīng)的變換矩陣A，B標(biāo)定數(shù)據(jù)集有M＝105種組合，采用以目的為導(dǎo)向的精度評價方法評估各標(biāo)定算法的求解精度，定義手眼標(biāo)定幾何誤差ε_X如下所示，

其中i＝1,...,M；

由圖5可知，從手眼標(biāo)定幾何誤差的精度和穩(wěn)定性上看，全局優(yōu)化算法(Global Optimization,GO)的每組觀測誤差值均優(yōu)于非線性迭代優(yōu)化算法(Nonlinear,OL)。10組測量數(shù)據(jù)中，GO算法的手眼變換舉證誤差平均值最大為1.4mm，標(biāo)準(zhǔn)差小于0.16mm，而OL算法的手眼變換矩陣誤差平均值最大已超過1.6mm，標(biāo)準(zhǔn)差接近0.2mm，即10組獨立重復(fù)實測實驗中本發(fā)明的全局優(yōu)化算法的手眼標(biāo)定幾何數(shù)據(jù)的誤差棒均小于非線性最優(yōu)化算法的手眼標(biāo)定幾何誤差棒。因此，本發(fā)明采用的全局優(yōu)化算法有效提高了標(biāo)定算法的魯棒性。

本發(fā)明考慮到機械手末端相對運動旋轉(zhuǎn)軸之間的夾角對標(biāo)定方程求解精度的影響，首先利用隨機抽樣一致性(Random Sample And Consensus，RANSAC)算法對標(biāo)定數(shù)據(jù)中旋轉(zhuǎn)軸之間的夾角進(jìn)行預(yù)篩選，再利用四元數(shù)參數(shù)化旋轉(zhuǎn)矩陣，建立多項式幾何誤差目標(biāo)函數(shù)和約束，采用基于線性矩陣不等式(linear matrix inequality，LMI)凸松弛全局優(yōu)化算法求解全局最優(yōu)手眼變換矩陣，從而提高了標(biāo)定算法的魯棒性。

結(jié)合這里披露的本發(fā)明的說明和實踐，本發(fā)明的其他實施例對于本領(lǐng)域技術(shù)人員都是易于想到和理解的。說明和實施例僅被認(rèn)為是示例性的，本發(fā)明的真正范圍和主旨均由權(quán)利要求所限定。