一種基于POE公式的最小模型與視覺(jué)測(cè)量的機(jī)器人標(biāo)定方法

石懷濤，張執(zhí)錦，祁若龍，馮大闊

(1.沈陽(yáng)建筑大學(xué)高檔石材數(shù)控加工裝備與技術(shù)國(guó)家地方聯(lián)合工程實(shí)驗(yàn)室，遼寧 沈陽(yáng) 110168；2.沈陽(yáng)建筑大學(xué)機(jī)械工程學(xué)院，遼寧 沈陽(yáng) 110168；3.中國(guó)建筑第七工程局有限公司，河南 鄭州 450000)

隨著機(jī)器人應(yīng)用范圍越來(lái)越廣泛，對(duì)于機(jī)器人的定位精度有了更高的要求，尤其是機(jī)器人的絕對(duì)定位精度問(wèn)題亟待解決。機(jī)器人的幾何誤差是影響自身絕對(duì)定位精度的主要因素，包括連桿的制造、裝配精度以及運(yùn)動(dòng)學(xué)誤差等。其中影響最大的是運(yùn)動(dòng)學(xué)誤差，因此機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定成為一種提高機(jī)器人絕對(duì)定位精度的有效方法[1]。

運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定主要分為以下四個(gè)步驟:①機(jī)器人運(yùn)動(dòng)學(xué)建模；②末端位姿真實(shí)值測(cè)量；③運(yùn)動(dòng)學(xué)參數(shù)辨識(shí)；④誤差補(bǔ)償[2]。筆者重點(diǎn)研究運(yùn)動(dòng)學(xué)標(biāo)定的前兩個(gè)步驟，建模就是通過(guò)尋找最優(yōu)函數(shù)來(lái)描述運(yùn)動(dòng)學(xué)參數(shù)誤差，運(yùn)動(dòng)學(xué)標(biāo)定模型應(yīng)該滿足:完整性、連續(xù)性、最小性[3]。對(duì)于運(yùn)動(dòng)學(xué)標(biāo)定建模，DH模型是使用最早、應(yīng)用最廣泛的模型，但是DH模型在相鄰關(guān)節(jié)接近平行時(shí)，會(huì)產(chǎn)生奇異性問(wèn)題[4]，為了消除這個(gè)問(wèn)題，很多研究人員采用了MDH模型[5-6]，先后提出了各種具有冗余參數(shù)的運(yùn)動(dòng)學(xué)模型，例如，S模型、CPC和MCPC模型，除了以上模型外，F(xiàn).C.Park[7]和K.Okamura[8]提出一種基于POE公式的標(biāo)定模型，機(jī)器人關(guān)節(jié)軸線(轉(zhuǎn)動(dòng)關(guān)節(jié)或移動(dòng)關(guān)節(jié))可以用旋量表示，由于李代數(shù)與李群之間的光滑映射，使得這個(gè)模型避免了奇異性。Y.Lou等[9]提出了基于POE公式建立的模型中存在的關(guān)節(jié)偏置誤差可以轉(zhuǎn)化為關(guān)節(jié)旋量誤差，無(wú)需識(shí)別。R.He等[10-11]給出三種運(yùn)動(dòng)學(xué)誤差模型，并給出了誤差模型的顯示表達(dá)式，使其更加簡(jiǎn)單。Y.Wang等[12]對(duì)基于POE公式的標(biāo)定方法進(jìn)行了擴(kuò)展，實(shí)現(xiàn)了冗余混合機(jī)器人的運(yùn)動(dòng)學(xué)標(biāo)定。以上提出的模型滿足了完整性與連續(xù)性，但仍存在參數(shù)冗余的現(xiàn)象，未滿足最小性。

測(cè)量方式是運(yùn)動(dòng)學(xué)標(biāo)定至關(guān)重要的一環(huán)，傳統(tǒng)標(biāo)定方法中的測(cè)量方式主要通過(guò)三坐標(biāo)測(cè)量機(jī)[13]、激光跟蹤儀[14]等測(cè)量機(jī)器人末端實(shí)際位姿，存在測(cè)量繁瑣、測(cè)量時(shí)間較長(zhǎng)、人工參與過(guò)多等難題，難以實(shí)現(xiàn)自動(dòng)在線測(cè)量。隨著機(jī)器視覺(jué)技術(shù)的發(fā)展，國(guó)內(nèi)外學(xué)者對(duì)基于視覺(jué)位姿測(cè)量的標(biāo)定方法展開(kāi)一系列的研究：X.Zhang[15]采用一個(gè)固定位置的立體攝像機(jī)和一個(gè)附著在機(jī)器人末端執(zhí)行器上的平面標(biāo)記實(shí)現(xiàn)機(jī)器人在線標(biāo)定。Y.Meng等[16]提出了一種只需要參考坐標(biāo)系上精確標(biāo)尺長(zhǎng)度的標(biāo)定方法，該方法的不足之處在于每個(gè)機(jī)器人關(guān)節(jié)構(gòu)型都需要已知的長(zhǎng)度。DENG Xun[17]提出了一種將攝像機(jī)和激光投影儀安裝在機(jī)械手上的特殊機(jī)器人視覺(jué)系統(tǒng)的標(biāo)定方法。高學(xué)海等[18]提出一種更具通用性的求解目標(biāo)位姿參數(shù)的方法。趙汝進(jìn)等[19]提出一種新型基于單目視覺(jué)的位姿測(cè)量方法，通過(guò)建立直線圖像匹配函數(shù)，無(wú)需檢測(cè)成像直線端點(diǎn)。

以上提及的運(yùn)動(dòng)學(xué)標(biāo)定模型中存在奇異性、參數(shù)冗余的現(xiàn)象，并且目前的標(biāo)定方法通常是離線進(jìn)行的，但運(yùn)動(dòng)學(xué)參數(shù)誤差經(jīng)常隨著載荷和周圍環(huán)境的變化而變化，因此在線獨(dú)立測(cè)量是提高位姿精度必不可少的方式。為了解決上述問(wèn)題，筆者提出了一種基于POE公式的最小模型與視覺(jué)測(cè)量的機(jī)器人標(biāo)定方法。首先，建立機(jī)器人基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)，標(biāo)定系統(tǒng)無(wú)需外部設(shè)備即可對(duì)機(jī)器人末端位姿的實(shí)際值進(jìn)行自動(dòng)測(cè)量；然后建立機(jī)器人正運(yùn)動(dòng)學(xué)模型，在此基礎(chǔ)上推導(dǎo)機(jī)器人誤差模型，并對(duì)誤差模型進(jìn)行最小化處理，獲得運(yùn)動(dòng)學(xué)標(biāo)定方程，標(biāo)定過(guò)程基于最小二乘法對(duì)于參數(shù)進(jìn)行辨識(shí)；最后，對(duì)MOTOMAN-MH80機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)標(biāo)定仿真，仿真結(jié)果表明筆者所提方法能夠有效提高機(jī)器人的絕對(duì)定位精度。

1 基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)

傳統(tǒng)機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)采取的末端位姿測(cè)量方式大多采用激光跟蹤儀、三坐標(biāo)測(cè)量機(jī)等大型精密儀器，但是傳統(tǒng)測(cè)量方式存在測(cè)量繁瑣、測(cè)量時(shí)間較長(zhǎng)、難以實(shí)現(xiàn)在線自動(dòng)標(biāo)定等難題。為了解決上述問(wèn)題，通過(guò)安裝在機(jī)器人末端執(zhí)行器上的雙目相機(jī)以及棋盤(pán)格式標(biāo)定板共組基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)，能夠?qū)C(jī)器人實(shí)際末端位姿進(jìn)行自動(dòng)測(cè)量，進(jìn)而實(shí)現(xiàn)在線自動(dòng)標(biāo)定。

基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)如圖1所示。六自由度機(jī)器人末端安裝有雙目相機(jī)，下端布有棋盤(pán)格式的標(biāo)定板作為輔助測(cè)量工具，可對(duì)相機(jī)的內(nèi)外參數(shù)進(jìn)行標(biāo)定。假設(shè)已知標(biāo)定板的坐標(biāo)系C相對(duì)于機(jī)器人基坐標(biāo)系S的位姿轉(zhuǎn)化矩陣STC，通過(guò)雙目相機(jī)與標(biāo)定板可以確定機(jī)器人工具坐標(biāo)系E相對(duì)于標(biāo)定板坐標(biāo)系C的位姿轉(zhuǎn)化矩陣為ETC，從而求得機(jī)器人工具坐標(biāo)系E相對(duì)于機(jī)器人基坐標(biāo)系S的位姿轉(zhuǎn)化矩陣為：STE=STC·CTE，即機(jī)器人實(shí)際末端位姿，該標(biāo)定系統(tǒng)能夠?qū)崿F(xiàn)機(jī)器人的在線自動(dòng)標(biāo)定。

圖1 基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)Fig.1 Calibration system based on vision measurement

2 機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定方程

2.1 正向運(yùn)動(dòng)學(xué)

基于指數(shù)積(POE)公式的標(biāo)定模型只需要建立基坐標(biāo)系S和工具坐標(biāo)系T，基坐標(biāo)系相對(duì)于工具坐標(biāo)系的位姿需要用一個(gè)齊次變換矩陣表示，所有的齊次變換矩陣組成一個(gè)剛體運(yùn)動(dòng)群，這個(gè)群的元素表示為

(1)

其中，R∈SO(3)是一個(gè)特殊的正交矩陣，并且b∈R3。

每個(gè)關(guān)節(jié)軸線都可以用旋量表示，對(duì)于一個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)，旋量坐標(biāo)定義為

(2)

其中，w為關(guān)節(jié)軸線在基坐標(biāo)系單位方向向量；p為關(guān)節(jié)軸線上任意一點(diǎn)在基坐標(biāo)系中的位置。

(3)

給出POE正向運(yùn)動(dòng)學(xué)公式為

(4)

其中，

(5)

對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)：

(6)

(7)

對(duì)于移動(dòng)關(guān)節(jié)：

R=I3,b=qv.

(8)

初始變換矩陣gst(0)也可以用指數(shù)形式表達(dá)：

(9)

因此，POE正向運(yùn)動(dòng)學(xué)模型也可以表示為

(10)

恒等式鄰域內(nèi)指數(shù)映射的逆映射成為對(duì)數(shù)映射，如果式(1)滿足條件：trace(R)≠-1,1+2cos(φ)=trace(R)，而且|φ|<π，則有：

(11)

(12)

(13)

2.2 運(yùn)動(dòng)學(xué)誤差模型

文中的誤差模型的建立選取基坐標(biāo)系作為參考坐標(biāo)系，在基坐標(biāo)系中來(lái)描述機(jī)器人末端位姿誤差，誤差模型為

(14)

機(jī)器人末端位姿誤差可以由實(shí)際測(cè)量位姿與名義位姿得到，可以表示為

(15)

如果末端位姿誤差足夠小，δf·f-1可以表示為

(16)

取一階近似，式(15)可以寫(xiě)為

(17)

文獻(xiàn)[9]指出關(guān)節(jié)零位誤差可以不用單獨(dú)辨識(shí)，因此運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定只需要辨識(shí)和補(bǔ)償關(guān)節(jié)旋量誤差和初始位姿變換旋量誤差，因此，機(jī)器人運(yùn)動(dòng)學(xué)誤差模型可以簡(jiǎn)化為

(18)

因此，運(yùn)動(dòng)學(xué)參數(shù)辨識(shí)就是求解如式(19)的最小二乘問(wèn)題：

(19)

2.3 誤差模型最小化

圖2 輔助坐標(biāo)系Fig.2 Auxiliary coordinate system

(20)

轉(zhuǎn)換矩陣gi可以由旋量坐標(biāo)ξi求得，對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)，且關(guān)節(jié)軸線不經(jīng)過(guò)基坐標(biāo)系原點(diǎn)O，基坐標(biāo)系相對(duì)于輔助坐標(biāo)系的轉(zhuǎn)換矩陣gi可以表示為

(21)

如果關(guān)節(jié)軸線經(jīng)過(guò)基坐標(biāo)系原點(diǎn)O，則pi和vi都是零向量，通過(guò)選擇向量e=[0,0,1]T作為一個(gè)旋轉(zhuǎn)軸，輔助坐標(biāo)系A(chǔ)i可以由基坐標(biāo)系繞旋轉(zhuǎn)軸e旋轉(zhuǎn)α=arccos([0,0,1]wi)，則有：

(22)

通過(guò)以上分析可以得出一個(gè)n自由度機(jī)器人的正向運(yùn)動(dòng)學(xué)模型[19]：

(23)

(24)

(25)

經(jīng)過(guò)最小化處理最終可以得到滿足最小性的理想誤差模型：

(26)

機(jī)器人運(yùn)動(dòng)學(xué)誤差模型可以整理為

y=Hx.

(27)

其中，

y=[δf·f-1]∨,

(28)

(29)

H=[J1,J2,…Jn,Jn+1],

(30)

(31)

2.4 運(yùn)動(dòng)學(xué)標(biāo)定方程

通過(guò)提出的基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)對(duì)機(jī)器人在不同構(gòu)型下的不同末端位姿進(jìn)行m次測(cè)量，可以得到與(27)形式相同的式子：

(32)

進(jìn)一步可以得到式(27)的最小二乘解為

(33)

3 機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定仿真

通過(guò)對(duì)MOTOMAN-MH80機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)標(biāo)定仿真，驗(yàn)證筆者提出的基于POE公式的最小模型與視覺(jué)測(cè)量的機(jī)器人標(biāo)定方法的有效性。

3.1 建立機(jī)器人旋量模型

根據(jù)MOTOMAN-MH80機(jī)器人的標(biāo)準(zhǔn)尺規(guī)圖，可以建立如圖3所示的旋量模型。

圖3 MOTOMAN-MH80機(jī)器人旋量示意圖Fig.3 Schematic spinor diagram of MOTOMAN-MH80 robot

各個(gè)關(guān)節(jié)軸線矢量分別為：w1=(0,0,1)T,w2=(0,1,0)T,w3=(0,-1,0)T,w4=(-1,0,0)T,w5=(0,-1,0)T,w6=(-1,0,0)T。

各個(gè)關(guān)節(jié)軸線上指定點(diǎn)的位置矢量分別為：P1=(0,0,0)T,P2=(145,0,0)T,P3=(145,0,870)T,P4=(1 170,0,1 080)T,P5=(1 170,0,1 080)T,P6=(1 170,0,1 080)T。由式(2)可得MOTOMAN-MH80機(jī)器人的運(yùn)動(dòng)學(xué)參數(shù)理論值，因此MOTOMAN-MH80機(jī)器人的運(yùn)動(dòng)學(xué)參數(shù)理論值和實(shí)際值(預(yù)設(shè)值)如表1所示。

表1 MOTOMAN-MH80機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)理論值與實(shí)際值Table 1 The theoretical and measured values of kinematic parameters for MOTOMAN-MH80 robot

3.2 運(yùn)動(dòng)學(xué)標(biāo)定仿真

通過(guò)以上分析可知，MOTOMAN-MH80機(jī)器人總共需要辨識(shí)42個(gè)參數(shù)，至少需要7個(gè)不同構(gòu)型下的末端位姿才能辨識(shí)這些參數(shù)。在仿真過(guò)程中，一共隨機(jī)生成了100個(gè)末端位姿，其中有50個(gè)為測(cè)量位姿，均使棋盤(pán)格式標(biāo)定板在相機(jī)的可視區(qū)域內(nèi)，另外50個(gè)為測(cè)試位姿，用于驗(yàn)證本標(biāo)定方法的有效性。MOTOMAN-MH80機(jī)器人的運(yùn)動(dòng)學(xué)標(biāo)定模型只考慮了機(jī)器人自身運(yùn)動(dòng)學(xué)參數(shù)誤差，忽略了基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)中的雙目相機(jī)的測(cè)量誤差，由于其測(cè)量精度較低，會(huì)影響標(biāo)定結(jié)果。因此，在本次仿真中需要施加隨機(jī)測(cè)量噪聲，以提高本次仿真的真實(shí)性。假設(shè)隨機(jī)測(cè)量噪聲的位置量和姿態(tài)量分別服從[-0.05 mm，0.05 mm]和[-0.001 rad，0.001 rad]的均勻分布。

本次MOTOMAN-MH80機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定仿真流程如圖4所示。

圖4 機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定仿真流程圖Fig.4 The simulation process of robot kinematic calibration

(1)在基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)下隨機(jī)生成50個(gè)附加隨機(jī)測(cè)量噪聲的末端位姿，即末端位姿測(cè)量值，然后根據(jù)機(jī)器人旋量模型生成機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)理論值，并預(yù)設(shè)實(shí)際值；

(2)已知機(jī)器人50個(gè)末端位姿測(cè)量值與運(yùn)動(dòng)學(xué)參數(shù)實(shí)際值，通過(guò)機(jī)器人逆運(yùn)動(dòng)學(xué)可以求解出50組關(guān)節(jié)角度；

(3)利用求出的50組關(guān)節(jié)角度和運(yùn)動(dòng)學(xué)參數(shù)理論值通過(guò)機(jī)器人正向運(yùn)動(dòng)學(xué)可計(jì)算出50個(gè)理論末端位姿；

(4)通過(guò)式(33)可以計(jì)算出參數(shù)誤差x；

(5)更新運(yùn)動(dòng)學(xué)參數(shù)；

(6)預(yù)設(shè)迭代次數(shù)并重復(fù)以上步驟，直到達(dá)到最大迭代次數(shù)k完成本次標(biāo)定仿真。

在運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定仿真每次迭代過(guò)程中，會(huì)更新運(yùn)動(dòng)學(xué)參數(shù)。關(guān)節(jié)旋量ξ和初始位姿旋量ξst更新方法如下:

(34)

ξst=ξst+Δξst.

(35)

為了更好地證明筆者提出的標(biāo)定方法的有效性，計(jì)算出MOTOMAN-MH80機(jī)器人標(biāo)定補(bǔ)償前后在隨機(jī)生成的50個(gè)測(cè)試位姿處的末端姿態(tài)誤差和末端位置誤差，如圖5所示。在隨機(jī)產(chǎn)生的測(cè)試點(diǎn)上機(jī)器人的末端位置和姿態(tài)誤差都得到了明顯的降低，平均值分別由7.41 mm和0.057 rad降低到0.051 mm和0.004 2 rad。這說(shuō)明筆者提出的基于POE公式的最小模型與視覺(jué)測(cè)量的機(jī)器人標(biāo)定方法能有效提高機(jī)器人的絕對(duì)定位精度。

圖5 末端位姿誤差Fig.5 Terminal pose error

4 結(jié) 論

(1)在機(jī)器人基坐標(biāo)系與棋盤(pán)格式標(biāo)定板坐標(biāo)系位姿變換關(guān)系已知前提下，通過(guò)安裝在機(jī)器人末端的雙目相機(jī)和棋盤(pán)式標(biāo)定板建立基于視覺(jué)測(cè)量的標(biāo)定系統(tǒng)，無(wú)需外部設(shè)備即可對(duì)機(jī)器人末端位姿的實(shí)際值進(jìn)行自動(dòng)測(cè)量，使得本標(biāo)定方法的適用范圍更廣泛、實(shí)用性更強(qiáng)、自動(dòng)化程度更高。

(2)使用基于指數(shù)積(POE)公式的運(yùn)動(dòng)學(xué)模型，并對(duì)其進(jìn)行最小化處理，消除冗余參數(shù)，滿足機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定模型的完整性、連續(xù)性、極小性的特點(diǎn)，并克服采用D-H等建模方法產(chǎn)生的奇異性、建模過(guò)程復(fù)雜等問(wèn)題，該模型的參數(shù)都是獨(dú)立的，并且避免了傳統(tǒng)模型迭代過(guò)程中歸一化、正交化的操作。

(3)經(jīng)運(yùn)動(dòng)學(xué)標(biāo)定仿真，在隨機(jī)產(chǎn)生50個(gè)測(cè)試位姿處，機(jī)器人末端位姿精度有了明顯提高，驗(yàn)證了筆者所提標(biāo)定方法的有效性。