基于旋量理論的機(jī)器人誤差建模方法

黃勇剛，杜 力，黃茂林

(1.重慶大學(xué) 機(jī)械工程學(xué)院，重慶400044;2.重慶工商大學(xué) 機(jī)械工程學(xué)院，400067，dulicq@126.com)

精度是衡量機(jī)器人性能的一個(gè)非常重要的指標(biāo)，因此機(jī)器人誤差建模方法研究歷來受到重視，主要有矩陣法和矢量法［1］.矩陣法的基礎(chǔ)是對齊次變換矩陣的微分，這是目前較常用的建模方法［2］.基于矢量的建模方法主要有攝動(dòng)法［3］和矢量分析結(jié)合螺旋變換法［4］.攝動(dòng)法基于連桿DH 參數(shù)表示，忽略位移和角度攝動(dòng)的耦合，推導(dǎo)出機(jī)器人位姿誤差計(jì)算公式.矢量分析結(jié)合螺旋變換法亦基于D-H 參數(shù)表示，用矢量分析方法建立位姿方程，然后對D-H 參數(shù)做微分，推導(dǎo)位姿誤差與D-H 參數(shù)的關(guān)系，再采用螺旋矢量簡化公式.現(xiàn)代旋量理論在使用矩陣指數(shù)以及指數(shù)積表示剛體運(yùn)動(dòng)的基礎(chǔ)上為開鏈機(jī)器人運(yùn)動(dòng)學(xué)提供了完整的幾何描述［5-7］.Moon［8-9］在可重構(gòu)機(jī)床的精度預(yù)測和補(bǔ)償研究中，將傳統(tǒng)矩陣微分方法獲得的機(jī)床模塊誤差矩陣等效為誤差旋量，從而應(yīng)用現(xiàn)代旋量理論的指數(shù)積公式建立了機(jī)床的誤差模型.譚月勝［10］通過對指數(shù)積公式中各旋量參數(shù)的微分，得出了一種機(jī)械臂末端執(zhí)行器位姿誤差的近似計(jì)算公式.

本文直接將機(jī)器人各關(guān)節(jié)旋量誤差看成是一種假想廣義運(yùn)動(dòng)副旋量運(yùn)動(dòng)的結(jié)果，從而與關(guān)節(jié)旋量一起建立機(jī)器人含誤差旋量的運(yùn)動(dòng)學(xué)指數(shù)積公式.定義了機(jī)器人工具坐標(biāo)系的位置誤差旋量和姿態(tài)誤差旋量，并給出其計(jì)算公式.應(yīng)用Matlab編程語言，設(shè)計(jì)了一套基于旋量的誤差分析計(jì)算軟件包，以SCARA 機(jī)器人為例，進(jìn)行誤差仿真分析.使用Adams 軟件仿真驗(yàn)證了本文模型和算法的正確性.

1 基于旋量的機(jī)器人運(yùn)動(dòng)學(xué)簡介［5］

對如圖1 所示具有n 個(gè)單自由度運(yùn)動(dòng)副的一般開鏈機(jī)器人機(jī)構(gòu)，各關(guān)節(jié)運(yùn)動(dòng)均可看成是旋量，其單位旋量可用Plücker 坐標(biāo)表示成如下形式［11］:

式中:ω 為旋量軸單位矢量，r 為軸線上任一點(diǎn)的位置矢量，r×ω 為旋量軸對原點(diǎn)的矩，h 為旋量節(jié)距.

圖1 一般開鏈機(jī)器人

設(shè)參考位形下工具坐標(biāo)系在參考系中的位姿為T(0)，此時(shí)，各關(guān)節(jié)單位旋量在參考系中分別表示為:)，則機(jī)器人有以下運(yùn)動(dòng)學(xué)正解指數(shù)積公式:

θi為相對參考位形的關(guān)節(jié)變量，對轉(zhuǎn)動(dòng)副，是繞軸線轉(zhuǎn)動(dòng)的角度，按右手定則確定符號(hào)，對移動(dòng)副，則表示移動(dòng)距離.

式(1)中算子∧運(yùn)算規(guī)則如下:對三維矢量ω，表示叉乘矩陣.即

其矩陣指數(shù)可用來表示軸線過坐標(biāo)原點(diǎn)的旋轉(zhuǎn)變換，有如下的羅德里格斯公式(I 為單位陣)

其矩陣指數(shù)表示一般剛體變換，計(jì)算公式為

由于旋量是瞬時(shí)量，當(dāng)旋量軸位姿改變時(shí)，需要應(yīng)用伴隨變換計(jì)算更新其旋量.另外，旋量采用Plücker 坐標(biāo)，其在不同坐標(biāo)系中的變換也需要應(yīng)用伴隨變換.

假設(shè)A 為固定坐標(biāo)系，B 坐標(biāo)系是A 經(jīng)過旋量θ ＄運(yùn)動(dòng)得到的新坐標(biāo)系，則B 到A 的坐標(biāo)變換或剛體變換的旋量矩陣指數(shù)如式(2)，簡記為

式(1)的指數(shù)積采用后運(yùn)動(dòng)的旋量矩陣指數(shù)左乘法則，即可看成先第n 個(gè)關(guān)節(jié)(最靠近工具端)運(yùn)動(dòng)，最后到第1 個(gè)關(guān)節(jié)(連架副)運(yùn)動(dòng).假設(shè)關(guān)節(jié)運(yùn)動(dòng)順序不同，則靠近機(jī)架的關(guān)節(jié)先運(yùn)動(dòng)后會(huì)改變后面的關(guān)節(jié)單位旋量坐標(biāo)，但是通過對所有被影響的關(guān)節(jié)旋量應(yīng)用式(4)的伴隨變換，結(jié)果相同.

2 關(guān)節(jié)旋量誤差的分析和表示

由線幾何理論［11］，關(guān)節(jié)軸線在參考坐標(biāo)系下可以用其單位矢量和對原點(diǎn)的矩表示，寫成Plücker 坐標(biāo)形式，通過分析各關(guān)節(jié)軸線的Plücker坐標(biāo)誤差可得到關(guān)節(jié)單位旋量的誤差.

如圖2，設(shè)機(jī)器人某關(guān)節(jié)軸線理想Plücker 坐標(biāo)為

圖2 運(yùn)動(dòng)副軸線Plücker 會(huì)標(biāo)誤差

無其他關(guān)節(jié)誤差影響時(shí)，該關(guān)節(jié)實(shí)際Plücker 坐標(biāo)為

把由理想軸線到實(shí)際軸線的Plücker 坐標(biāo)變化看成是旋量運(yùn)動(dòng)的結(jié)果，稱其為誤差旋量，則根據(jù)式(3)的伴隨變換，有如下關(guān)系:

設(shè)單位誤差旋量表示為

則由式(5)可以求得誤差旋量.實(shí)際上此誤差旋量可以通過觀察得到，即:誤差旋量是繞理想軸線與實(shí)際軸線的公垂線轉(zhuǎn)動(dòng)角度θe，再沿公垂線移動(dòng)距離d，轉(zhuǎn)動(dòng)的角度為兩軸線的扭角，移動(dòng)距離為兩軸線的垂直距離.因此:

當(dāng)θe=0 時(shí)，即理想軸線與實(shí)際軸線平行，誤差旋量為純移動(dòng)，ωe=0，he=∞，誤差旋量退化為

當(dāng)d=0 時(shí)，理想軸線與實(shí)際軸線相交，he=0，r=rd，單位誤差旋量為

最一般的情形是理想軸線與實(shí)際軸線空間相錯(cuò)，d 和θe均不為0，he=d/θe.單位誤差旋量為

用旋量來表示關(guān)節(jié)軸線的誤差，具有良好的幾何直觀性，并且不同形式的誤差源如位置誤差和姿態(tài)誤差均被統(tǒng)一表示在誤差旋量中.

以上分析過程是將誤差在固定坐標(biāo)系下采用旋量表示.假設(shè)將理想軸線作為z 軸，將連桿方向作為x 軸建立局部坐標(biāo)系，則理想軸線在局部坐標(biāo)系下的Plücker 坐標(biāo)為

設(shè)運(yùn)動(dòng)副實(shí)際軸線Plücker 坐標(biāo)為

實(shí)際軸線在局部坐標(biāo)系下的z 方向角即為誤差旋量的大小θe，因此

由式(6)可得

由此，誤差旋量的大小θe和誤差旋量軸x、y方向余弦分別表示了實(shí)際軸線在局部坐標(biāo)系的姿態(tài)誤差，從而避免了D-H 表示法難于直接表示繞y 軸線轉(zhuǎn)動(dòng)誤差的不足［2］.誤差旋量綜合表示了連桿長度誤差和形位誤差.

雖然誤差旋量在局部坐標(biāo)系下的表示更簡單和方便，但還需要根據(jù)當(dāng)前局部坐標(biāo)系在固定坐標(biāo)系中的位姿，通過伴隨變換轉(zhuǎn)換到固定坐標(biāo)系.

3 基于旋量的機(jī)器人運(yùn)動(dòng)學(xué)誤差模型

3.1 含誤差旋量的運(yùn)動(dòng)學(xué)指數(shù)積公式

如圖1 所示一般開鏈機(jī)器人，設(shè)參考位形時(shí)，不考慮其他關(guān)節(jié)誤差的影響，由連架副開始的n個(gè)關(guān)節(jié)軸線在固定坐標(biāo)系下的誤差旋量分別為:.把這些誤差旋量看成假想廣義運(yùn)動(dòng)副，則根據(jù)式(2)的指數(shù)積公式可直接得到如下含誤差旋量的機(jī)器人運(yùn)動(dòng)學(xué)模型:

式中:θi為關(guān)節(jié)變量，是理想關(guān)節(jié)旋量的大小.而θei是誤差旋量的大小，只考慮靜態(tài)或準(zhǔn)靜態(tài)誤差情況下是常量.為機(jī)器人關(guān)節(jié)理想單位旋量.Td(0)為參考位形時(shí)工具坐標(biāo)系的理想位姿.

式(7)的正確性可用關(guān)節(jié)旋量在誤差作用下的伴隨變換證明.因?yàn)闄C(jī)器人第i 個(gè)關(guān)節(jié)單位旋量Plücker 坐標(biāo)受到其前面i 個(gè)誤差旋量的影響，根據(jù)伴隨變換，各關(guān)節(jié)實(shí)際單位旋量為

同時(shí)，在誤差作用下，參考位形時(shí)機(jī)器人實(shí)際位姿為

因此，機(jī)器人的運(yùn)動(dòng)學(xué)指數(shù)積公式為

根據(jù)式(8)和伴隨變換式(4)，可得第i 個(gè)運(yùn)動(dòng)副實(shí)際旋量矩陣指數(shù)為

將式(9)和(11)代入式(10)簡化后即可得式(7).

式(7)含誤差旋量的機(jī)器人運(yùn)動(dòng)學(xué)指數(shù)積公式具有明確的幾何意義，即:將運(yùn)動(dòng)副軸線位姿誤差看成是假想廣義運(yùn)動(dòng)副旋量運(yùn)動(dòng)的結(jié)果，在靜態(tài)和準(zhǔn)靜態(tài)下，誤差旋量的大小和節(jié)距不變，參考位形時(shí)將其與理想運(yùn)動(dòng)副旋量一起按運(yùn)動(dòng)副順序構(gòu)成指數(shù)積公式，即為機(jī)器人含誤差運(yùn)動(dòng)學(xué)方程.因此，只要已知初始位形下各運(yùn)動(dòng)副軸線的誤差旋量，即可計(jì)算出工具坐標(biāo)系的實(shí)際位姿.

3.2 工具坐標(biāo)系誤差的定義

與運(yùn)動(dòng)副軸線誤差分析類似，同樣可以將工具坐標(biāo)系的位姿誤差看成是某一誤差旋量運(yùn)動(dòng)的結(jié)果.然而此時(shí)將位置和姿態(tài)誤差統(tǒng)一表示并不直觀，此處將理想工具坐標(biāo)原點(diǎn)到實(shí)際工具坐標(biāo)原點(diǎn)的位置誤差用移動(dòng)副旋量表示，姿態(tài)誤差用軸線過理想坐標(biāo)系原點(diǎn)的轉(zhuǎn)動(dòng)副旋量表示，如圖3.

圖3 工具坐標(biāo)系位姿誤差的旋量表示

工具坐標(biāo)系理想位姿如式(1)，其實(shí)際位姿如式(7)，假設(shè)位置誤差和姿態(tài)誤差旋量均相對固定坐標(biāo)系表示，則可以看成機(jī)器人先通過理想的運(yùn)動(dòng)副旋量運(yùn)動(dòng)到工具坐標(biāo)系理想位姿，再經(jīng)過姿態(tài)誤差旋量運(yùn)動(dòng)到實(shí)際姿態(tài)，然后經(jīng)過位置誤差旋量運(yùn)動(dòng)到實(shí)際位置.設(shè)位置誤差旋量和姿態(tài)誤差旋量分別為

則如下等式成立:

根據(jù)前面誤差旋量的定義，兩種誤差旋量在理想工具坐標(biāo)系下可寫成如下形式:

根據(jù)伴隨變換，兩種誤差在不同坐標(biāo)系下的表示有如下關(guān)系

因此由式(4)，式(12)可變換為

式(15)表明，在理想工具坐標(biāo)系下表示的位置和姿態(tài)誤差旋量矩陣指數(shù)積即為傳統(tǒng)方法定義的機(jī)器人位姿誤差矩陣.因此，可以在工具理想坐標(biāo)系下定義誤差旋量為工具坐標(biāo)系理想位姿到實(shí)際位姿的誤差，即

由式(16)可以求得位置誤差旋量為

后3 個(gè)分量分別對應(yīng)位置誤差在工具理想坐標(biāo)系3 個(gè)軸向的分量大小.

可由式(16)求得姿態(tài)誤差旋量的大小如下

如果θr=0，則只有位置誤差沒有姿態(tài)誤差，如果θr不為零，則姿態(tài)誤差旋量軸在理想工具坐標(biāo)系下的方向余弦為

以上兩種誤差旋量均在理想工作坐標(biāo)系下表示.根據(jù)式(13)和(14)的伴隨變換，或是直接根據(jù)式(12)，還可以求出兩種誤差旋量在固定坐標(biāo)系下的表示.

4 基于Matlab 的誤差仿真分析

根據(jù)前面基于旋量理論的機(jī)器人位姿誤差建模過程，在Matlab 下設(shè)計(jì)了一套通用的旋量計(jì)算軟件包.利用該軟件包，可以對具體的機(jī)器人進(jìn)行誤差計(jì)算和仿真研究.

以SCARA 機(jī)器人為例，該機(jī)器人為R//R//C 開鏈機(jī)構(gòu)，C 副的轉(zhuǎn)動(dòng)自由度和移動(dòng)自由度為獨(dú)立驅(qū)動(dòng)，如圖4 所示，兩連桿長L1= L2=300 mm.固定坐標(biāo)系z 軸與第一轉(zhuǎn)動(dòng)副理想軸線重合，工具坐標(biāo)系z 軸與C 副軸線重合，x 軸為連桿方向.并記θ1=θ2=θ3=0 時(shí)為初始位形，機(jī)構(gòu)無誤差參數(shù)如下:

假設(shè)初始位形時(shí)運(yùn)動(dòng)副軸線誤差參數(shù)如下:

圖4 SCARA 機(jī)器人機(jī)構(gòu)

設(shè)工具坐標(biāo)系運(yùn)動(dòng)軌跡為:從點(diǎn)［400，400，150］開始順次直線運(yùn)動(dòng)到點(diǎn)［-400，400，50］、［-400，-400，150］、［400，-400，50］，最后回到點(diǎn)［400，400，150］，每段直線均為勻速運(yùn)動(dòng)1 s 完成，且坐標(biāo)系姿態(tài)保持繞z 軸旋轉(zhuǎn)30°不變.其軌跡曲線如圖5 所示，為一空間封閉四邊形.

圖5 仿真軌跡曲線

根據(jù)以上參數(shù)和運(yùn)動(dòng)軌跡，首先反解出關(guān)節(jié)變量，由于SCARA 機(jī)器人有兩個(gè)反解構(gòu)型，此處采用正裝配構(gòu)型，在Matlab 下仿真分析得出的工具坐標(biāo)系位置誤差在固定坐標(biāo)系下的分量曲線如圖6.

對于姿態(tài)誤差旋量，可以用旋量大小和旋量軸3 個(gè)方向余弦曲線表示.為了與ADAMS 中的仿真結(jié)果對比，此處將誤差旋量轉(zhuǎn)換為理想工具坐標(biāo)系到實(shí)際工具坐標(biāo)系的3 個(gè)羅德里格斯參數(shù)表示.轉(zhuǎn)換公式如式(17):

3 個(gè)羅德里格斯參數(shù)曲線如圖7.

圖6 Matlab 仿真位置誤差曲線

圖7 Matlab 仿真姿態(tài)誤差羅德里格斯參數(shù)曲線

5 仿真結(jié)果的ADAMS 驗(yàn)證

為了驗(yàn)證基于旋量的誤差建模方法和Matlab軟件包算法的正確性，在ADAMS 下對上節(jié)的SCARA 機(jī)器人按同樣的機(jī)構(gòu)參數(shù)進(jìn)行了誤差建模仿真分析.

為了在ADAMS 軟件下進(jìn)行誤差分析，首先按理想設(shè)計(jì)參數(shù)建立了SCARA 機(jī)器人模型，進(jìn)行反解仿真獲得關(guān)節(jié)驅(qū)動(dòng)參數(shù)樣條曲線.然后根據(jù)誤差旋量參數(shù)計(jì)算獲得參考位形下各運(yùn)動(dòng)副MARKER 的位姿，從而建立含誤差模型，再用理想模型的關(guān)節(jié)驅(qū)動(dòng)參數(shù)樣條曲線驅(qū)動(dòng)機(jī)構(gòu)，獲得工具坐標(biāo)系運(yùn)動(dòng).將誤差模型的工具坐標(biāo)系位置曲線與理想模型曲線相減，得到如圖8 所示的位置誤差在固定坐標(biāo)系下的曲線.測量理想工具坐標(biāo)系到誤差模型工具坐標(biāo)系的羅德里格斯參數(shù)，得到如圖9 所示的曲線.

圖8 Adams 仿真位置誤差曲線

圖9 Adams 仿真姿態(tài)誤差羅德里格斯參數(shù)曲線

比較圖6 和圖8 以及圖7 和圖9，可以看出，基于旋量的誤差建模仿真與用Adams 建模仿真結(jié)果相同，證明了該方法的正確性.

6 結(jié) 論

1)將運(yùn)動(dòng)副軸線的位姿誤差看成是旋量運(yùn)動(dòng)的結(jié)果，從而應(yīng)用旋量的矩陣指數(shù)以及指數(shù)積建立機(jī)器人誤差模型，具有明顯的幾何直觀性.

2)將工具坐標(biāo)系的位置誤差和姿態(tài)誤差分別用移動(dòng)副旋量和轉(zhuǎn)動(dòng)副旋量表示，并給出了其在固定坐標(biāo)系和在理想工具坐標(biāo)系中的計(jì)算公式.用旋量的大小和旋量螺旋軸的方向表示誤差，幾何意義明確.

3)在Matlab 下設(shè)計(jì)了旋量計(jì)算軟件包，并以SCARA 機(jī)器人為例，在給定運(yùn)動(dòng)副軸線誤差情況下進(jìn)行了仿真計(jì)算，并在Adams 下按相同參數(shù)進(jìn)行了對比仿真.證明了該誤差建模方法的正確性.

4)本文的誤差建模方法具有良好的幾何直觀性，可以從整體上描述機(jī)器人的實(shí)際運(yùn)動(dòng).不需要旋量在不同位形下的變換計(jì)算，簡化了分析過程，計(jì)算量減少了約2/3.與傳統(tǒng)矩陣微分方法比較，消除了誤差矩陣非齊次性引起的計(jì)算誤差.

5)該方法不僅可用于靜態(tài)和準(zhǔn)靜態(tài)誤差分析，如果考慮誤差旋量的大小和節(jié)距隨機(jī)構(gòu)運(yùn)動(dòng)過程變化，還可用于動(dòng)態(tài)誤差的分析.

［1］閻華，劉桂雄，鄭時(shí)雄.機(jī)器人位姿誤差建模方法綜述［J］.機(jī)床與液壓，2000(1):3-5.

［2］NIKU S B.機(jī)器人學(xué)導(dǎo)論——分析、系統(tǒng)及應(yīng)用［M］.孫富春，朱紀(jì)洪，劉國棟，等譯.北京:電子工業(yè)出版社，2004.

［3］SUGIMOTO K，OKADA T.Compensation of positioning errors caused by geometric deviation in robot system［C］//Robotics Research:The second international Symposium.Cambridge:MIT Press，1985:231-236.

［4］徐衛(wèi)良.機(jī)器人機(jī)構(gòu)誤差建模的攝動(dòng)法［J］.機(jī)器人，1989，3(6):39-44.

［5］理查德·摩雷，李澤湘，夏恩卡·薩思特里.機(jī)器人操作的數(shù)學(xué)導(dǎo)論［M］.徐衛(wèi)良，錢瑞明，譯.北京:機(jī)械工業(yè)出版社，1998:11-44.

［6］張付祥，付宜利，王樹國.閉鏈級(jí)聯(lián)式機(jī)器人基于旋量理論的運(yùn)動(dòng)學(xué)分析方法［J］.機(jī)械工程學(xué)報(bào)，2006，42(4):112-117.

［7］荊學(xué)東，浦耿強(qiáng)，王成燾.應(yīng)用基于運(yùn)動(dòng)螺旋的機(jī)器人正解映射求解FANUC 機(jī)器人的逆運(yùn)動(dòng)學(xué)問題［J］.機(jī)械科學(xué)與技術(shù)，2003，22(3):402-405.

［8］MOON S K，MOON Y M，SCREW S K.Theory Based Metrology for Design and Error Compensation of Machine Tools［C］//Proceedings of DETC’01.Pittsburgh，Pennsylvania:［s.n.］，2001.

［9］MOON S K.Error prediction and compensation of reconfigurable machine tool using screw kinematics［D］.［S.l.］:The university of Mchigan，2002.

［10］譚月勝，孫漢旭，賈慶軒，等.旋量理論在機(jī)械臂末端執(zhí)行器運(yùn)動(dòng)精度分析中的應(yīng)用研究［J］.機(jī)械科學(xué)與技術(shù)，2006，25(5):534-538.

［11］黃真，趙永生，趙鐵石.高等空間機(jī)構(gòu)學(xué)［M］.北京:高等教育出版社，2006:2-18.