蔣燦燦，魏洪興，張宇超

(北京航空航天大學(xué) 機(jī)械工程及自動化學(xué)院，北京 100191)

0 引言

模塊化機(jī)器人是一類由關(guān)節(jié)模塊、連桿模塊及基座等構(gòu)件，根據(jù)任務(wù)要求搭建成的具有不同自由度構(gòu)型的可重構(gòu)機(jī)器人，它可以根據(jù)任務(wù)要求重新構(gòu)型，而每種構(gòu)型對應(yīng)著不同的數(shù)學(xué)模型，因此，研究一種快速通用的運(yùn)動學(xué)建模方法至關(guān)重要。目前運(yùn)動學(xué)最常用的方法是D-H建模[1]，但此方法對構(gòu)型的依賴很大，每改變一次構(gòu)型就要重新做一次D-H建模，不適合構(gòu)型可變的模塊化機(jī)器人。

旋量分析方法[2]是利用李群李代數(shù)、旋量和矩陣運(yùn)算等數(shù)學(xué)手段結(jié)合剛體的運(yùn)動規(guī)律得出的，是一種不依賴于構(gòu)型的通用解算方法[3]。采用旋量描述運(yùn)動學(xué)問題有兩個好處：一是使用整體坐標(biāo)系，只需要建立基礎(chǔ)坐標(biāo)系和末端工具坐標(biāo)系進(jìn)行描述，避免了使用局部坐標(biāo)系所造成的奇異性；二是結(jié)合圖論的關(guān)聯(lián)矩陣對剛體運(yùn)動進(jìn)行幾何描述，大大簡化了對機(jī)構(gòu)的分析。本文利用旋量分析方法對模塊化機(jī)器人進(jìn)行運(yùn)動學(xué)分析及仿真，并以其中一種模塊化機(jī)器人構(gòu)型進(jìn)行驗證，得到運(yùn)動位姿、末端軌跡和各關(guān)節(jié)參數(shù)與時間的關(guān)系，從而證明這種方法的通用性和準(zhǔn)確性。

1 模塊化機(jī)器人構(gòu)型

模塊化機(jī)器人是由一套具有不同尺寸和性能特征的模塊組合而成，使用這些模塊能夠快速搭建出各種不同的構(gòu)型。本文設(shè)計了一個模塊庫，內(nèi)含關(guān)節(jié)模塊和輔助模塊兩種。關(guān)節(jié)模塊集成了驅(qū)動器、傳感器、編碼器和剎車等功能部件，關(guān)節(jié)分為4種類型，每種類型在尺寸、扭矩、質(zhì)量和其他參數(shù)上均不同，但具有一個共同特點，即4種關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)。輔助模塊包括連桿模塊、輪軸模塊、手爪模塊和主板模塊等。將關(guān)節(jié)模塊和輔助模塊通過CAN總線連接，可以構(gòu)成不同構(gòu)型的機(jī)器人。

使用上述模塊庫搭建成的不同構(gòu)型的機(jī)械臂如圖1所示。

圖1 不同類型的機(jī)械臂

2 正運(yùn)動學(xué)自動建模

2.1 基于圖論的關(guān)聯(lián)矩陣表達(dá)方法

基于圖論表達(dá)的關(guān)聯(lián)矩陣包含了各關(guān)節(jié)輸入面、輸出面及各連桿類型和幾何結(jié)構(gòu)等信息，為后續(xù)分析奠定了基礎(chǔ)。Chen等人[4-5]研究的基于圖論的關(guān)聯(lián)矩陣表達(dá)方法雖然很詳盡但非常復(fù)雜，不適合本文研究的純旋轉(zhuǎn)關(guān)節(jié)機(jī)器人，為了簡化，在構(gòu)建機(jī)器人的機(jī)構(gòu)簡圖時用線表示連桿、點表示關(guān)節(jié)。五自由度機(jī)械臂構(gòu)型及結(jié)構(gòu)簡圖如圖2所示。圖2中,J1～J5為旋轉(zhuǎn)關(guān)節(jié)，l1～l4為兩個關(guān)節(jié)之間的距離。

本文研究的關(guān)節(jié)模塊全部是旋轉(zhuǎn)關(guān)節(jié)，將基礎(chǔ)坐標(biāo)系建立在如圖2所示的關(guān)節(jié)1的輸入面，末端坐標(biāo)系建立在關(guān)節(jié)5的輸出面，計算得到的該機(jī)械臂對應(yīng)的關(guān)聯(lián)矩陣表達(dá)式為[6]：

其中：z為關(guān)節(jié)在輸出面z方向上的坐標(biāo);J1～J5均取值為1。

圖2 五自由度機(jī)器人構(gòu)型及結(jié)構(gòu)簡圖

2.2 正運(yùn)動學(xué)建模

對于n個旋轉(zhuǎn)關(guān)節(jié)構(gòu)成的串聯(lián)機(jī)械臂，其自由度數(shù)等于關(guān)節(jié)個數(shù)。定義一個n行的列向量θ(θ為關(guān)節(jié)矢量)，其表達(dá)式為：

θ=[θ1,θ2,…，θn]T.

其中：θi為第i個關(guān)節(jié)轉(zhuǎn)動的角度，(°)，i=0,1,…,n。

各關(guān)節(jié)轉(zhuǎn)動一定的角度后，機(jī)械臂到達(dá)一個末端位姿，將末端位姿表達(dá)成一個指數(shù)積形式[7-8]，得到機(jī)械臂的正運(yùn)動學(xué)建模方程為：

(1)

2.3 正運(yùn)動學(xué)算法驗證

以圖2所示的五自由度機(jī)器人為例，基于Matlab[9]編寫通用的計算程序，得到正運(yùn)動學(xué)圖形用戶界面。在界面中輸入自由度、初始位姿、關(guān)節(jié)初始變量和關(guān)節(jié)運(yùn)動信息，軟件會根據(jù)公式(1)進(jìn)行計算，并在用戶界面上顯示計算結(jié)果。

對于五自由度機(jī)械臂，選擇自由度為5，設(shè)定好末端初始位姿、各關(guān)節(jié)軸線方向向量和軸線上任一點的坐標(biāo)，給定一組關(guān)節(jié)矢量θ=[-90,0,45,-45,0]T，計算得到如圖3所示的五自由度機(jī)械臂的正運(yùn)動學(xué)計算界面及機(jī)械臂末端初始位姿：

3 模塊化機(jī)器人逆運(yùn)動學(xué)分析

逆運(yùn)動學(xué)問題，即給定末端工具坐標(biāo)系相對于基礎(chǔ)坐標(biāo)系的期望位姿，計算到達(dá)該位姿的各關(guān)節(jié)轉(zhuǎn)角的值。逆運(yùn)動學(xué)求得的解有解析解和數(shù)值解兩種，解析解比較準(zhǔn)確全面，但是并非所有構(gòu)型都存在解析解；數(shù)值解具有一定的精確度，可以求解所有構(gòu)型。

采用旋量理論求機(jī)器人的運(yùn)動學(xué)逆解主要包括兩種方法：求解析解的子問題法[10]和求數(shù)值解的迭代逆解算法[11]。為了求得任意給定構(gòu)型機(jī)械臂的運(yùn)動學(xué)逆解，本文采用迭代逆解算法，此方法更加方便編程且對構(gòu)型具有通用性。

圖3 五自由度機(jī)械臂的正運(yùn)動學(xué)計算界面

3.1 逆運(yùn)動學(xué)迭代算法

對式(1)進(jìn)行一系列矩陣變換、伴隨變換和李群李代數(shù)映射，得到：

(2)

將式(2)簡化為：

D=Jdθ.

(3)

將式(3)改寫成迭代形式，得到逆運(yùn)動學(xué)迭代算法公式：

dθ=J+D
θm+1=θm+dθm+1.

(4)

其中：J+為雅可比矩陣J的廣義逆矩陣，計算時采用J+=JT(JJT)-1；θm為第m次迭代過程中得到的關(guān)節(jié)矢量。

迭代逆解算法流程如圖4所示。

3.2 逆運(yùn)動學(xué)算法驗證

編寫Matlab程序得到逆運(yùn)動學(xué)解算的用戶界面，輸入初始參數(shù)、當(dāng)前關(guān)節(jié)變量和期望位姿，就可以得到逆運(yùn)動學(xué)解。

該實例的逆運(yùn)動學(xué)解算用戶界面如圖5所示，得到的到達(dá)該位姿的一組關(guān)節(jié)變量值θ=(-90.000 1,0.000 0,44.999 9,-45.000 0,0.000 0)T。將此關(guān)節(jié)變量代入公式(1)，得到位置矢量為[-0.203 2 8.584 0 599.788 6]。

圖4 迭代逆解算法流程

圖5 例1的逆運(yùn)動學(xué)解算用戶界面

在初始軸線方向、軸上點和期望位姿相同的條件下，再給定另一組(例2)初始關(guān)節(jié)變量θ0=[0 45 0 -90 90]T，計算得到的解算用戶界面如圖6所示[12]，到達(dá)該位姿的一組關(guān)節(jié)變量值為θ=[-89.999 9,0.000 1,44.999 9,-45.000 1,0.000 1]T。將此關(guān)節(jié)變量代入公式(1)，得到位置矢量為[-0.207 2 8.584 1 599.791 0]。

圖6 例2的逆運(yùn)動學(xué)解算用戶界面

上述兩個實例說明在給定精度和迭代次數(shù)的條件下，對于相同的軸線方向、軸線上點和期望位姿，不同的初始關(guān)節(jié)變量下得到的逆運(yùn)動學(xué)解是不同的，期望位姿越接近當(dāng)前位姿，迭代速度越快。需要注意的是，迭代精度不能設(shè)置太高，否則復(fù)雜的逆運(yùn)動學(xué)算法會使計算時間變長，反而不滿足要求。另外，程序默認(rèn)的迭代次數(shù)為20，如果設(shè)定次數(shù)太多，會非常耗時。當(dāng)?shù)螖?shù)達(dá)到20，但是沒有得到符合精度要求的關(guān)節(jié)角時，程序會返回NULL。

4 軌跡規(guī)劃

機(jī)器人的軌跡規(guī)劃包括笛卡爾空間軌跡規(guī)劃[13]和關(guān)節(jié)空間軌跡規(guī)劃[14-15]，前者的規(guī)劃軌跡由機(jī)械臂末端位姿的笛卡爾空間坐標(biāo)組成，有可能具有奇異點；后者將運(yùn)動過程中的每一個關(guān)節(jié)變量變成光滑的函數(shù)，不會出現(xiàn)奇異點，本文在關(guān)節(jié)空間進(jìn)行三次多項式插值[16]。

已知機(jī)器人的初始位姿和期望位姿，給定各關(guān)節(jié)起始變量，利用公式(4)可以解得關(guān)節(jié)終止變量。為了將每個關(guān)節(jié)變量從起始變量到終止變量的變化過程轉(zhuǎn)化為光滑的函數(shù)，采用三次多項式插值，需要設(shè)定4個約束。在給定起始和終止關(guān)節(jié)的角度和速度情況下，對應(yīng)的三次多項式及4個約束條件為：

其中:a0、a1、a2、a3為參數(shù)；t為運(yùn)動時間；tf為運(yùn)動終止時間；θf為運(yùn)動終止時刻的關(guān)節(jié)變量。

求解得到各參數(shù)：

5 仿真及驗證結(jié)果

使用Matlab編程，對五自由度模塊化機(jī)器人進(jìn)行仿真[17]，并驗證旋量運(yùn)動學(xué)算法和運(yùn)動規(guī)劃算法的準(zhǔn)確性和通用性。

設(shè)定初始關(guān)節(jié)矢量為θ0=[0 -90 0 -90 0]T，當(dāng)電機(jī)驅(qū)動關(guān)節(jié)轉(zhuǎn)過角度矢量θ=[-45 0 90 0 60]T后，機(jī)械臂由初始位姿移動到期望位姿，由Matlab構(gòu)建的機(jī)械臂模型的初始位姿和期望位姿如圖7所示。

圖7 機(jī)械臂模型的初始位姿和期望位姿

進(jìn)行仿真運(yùn)動，得到機(jī)械臂從初始位姿運(yùn)動到期望位姿的過程中各關(guān)節(jié)角度、角速度、角加速度與時間的關(guān)系曲線，如圖8～圖10所示，機(jī)械臂初始位姿到期望位姿的軌跡如圖11所示。

圖8 各關(guān)節(jié)角度與時間的關(guān)系

圖9 各關(guān)節(jié)角速度與時間的關(guān)系

圖10 各關(guān)節(jié)角加速度與時間的關(guān)系

圖11 初始位姿到期望位姿的軌跡

6 結(jié)語

本文以李群李代數(shù)和矩陣運(yùn)算等為基礎(chǔ)，采用旋量計算可重構(gòu)構(gòu)型的運(yùn)動學(xué)，對模塊化機(jī)器人的運(yùn)動學(xué)進(jìn)行了分析。根據(jù)機(jī)器人的特點，設(shè)計了簡化的關(guān)聯(lián)矩陣，給出了逆運(yùn)動學(xué)的數(shù)值解算法，并采用關(guān)節(jié)空間的三次多項式插值進(jìn)行仿真運(yùn)動，從而驗證了運(yùn)動學(xué)算法和規(guī)劃算法。

參考文獻(xiàn)：

[1] 項有元,陳萬米,鄒國柱.基于D-H算法的自主機(jī)器人機(jī)械臂建模方法研究[J].工業(yè)控制計算機(jī),2014,27(7):113-115.

[2] 常宗瑜,潘同慶,楊咸啟,等.基于指數(shù)積公式的雙肘桿機(jī)構(gòu)的運(yùn)動分析[G]//中國機(jī)構(gòu)與機(jī)器科學(xué)國際會議暨海峽兩岸第四屆機(jī)構(gòu)學(xué)研討會論文集.大連：中國機(jī)械工程學(xué)會機(jī)構(gòu)學(xué)專業(yè)委員會，2008:123-125.

[3] Zhen H,Liu J F,Zeng D X.A general methodology for mobility analysis of mechanisms based on constraint screw theory[J].Science in China(Series E:Technological Sciences),2009,52(5):1337-1347.

[4] Chen I M,Yang G.Automatic model generation for modular reconfigurable robot dynamics[J].Journal of Dynamic Systems Measurement & Control,1998,120(3):346.

[5] Chen I M,Song H Y,Chen G,et al.Kernel for modular robot applications:automatic modeling techniques[J].International Journal of Robotics Research,1999,18(2):225-242.

[6] 王永甲.可重構(gòu)模塊化機(jī)器人構(gòu)型設(shè)計理論與運(yùn)動學(xué)研究[D].南京：南京理工大學(xué),2008：17-23.

[7] 賈慶軒,左仲海,陳鋼,等.基于旋量理論的一種分支型模塊化機(jī)器人運(yùn)動學(xué)建模研究[J].機(jī)械制造,2015,53(2):14-16.

[8] 呂曉俊.機(jī)器人機(jī)構(gòu)逆運(yùn)動學(xué)問題的模塊化算法研究[D].南京：東南大學(xué),2006：6-17.

[9] 王勝,程武山.基于Matlab的模塊化機(jī)器人運(yùn)動學(xué)仿真[J].上海工程技術(shù)大學(xué)學(xué)報,2014,28(1):52-57.

[10] 趙杰,劉玉斌,蔡鶴皋.一種運(yùn)動旋量逆解子問題的求解及其應(yīng)用[J].機(jī)器人,2005,27(2):163-167.

[11] 王憲,楊國梁,張方生,等.基于牛頓-拉夫遜迭代法的6自由度機(jī)器人逆解算法[J].傳感器與微系統(tǒng),2010,29(10):116-118.

[12] 張艷麗,李樹軍,李景奎.可重構(gòu)模塊化機(jī)器人逆運(yùn)動學(xué)建模[J].機(jī)械設(shè)計,2008,25(11):29-31.

[13] 朱清智,張毅.關(guān)節(jié)型機(jī)器手臂笛卡爾軌跡規(guī)劃[J].空間控制技術(shù)與應(yīng)用,2016,42(4):24-29.

[14] 董輝,仲曉帆,黃勝.一種六關(guān)節(jié)機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃方法[J].浙江工業(yè)大學(xué)學(xué)報,2015,43(3):336-339.

[15] 王濤.基于Matlab仿真的機(jī)器手臂笛卡爾軌跡規(guī)劃[J].湖南文理學(xué)院學(xué)報(自科版),2015(4):20-24.

[16] 潘磊,錢煒,張志艷,等.四自由度機(jī)械臂運(yùn)動學(xué)分析及Matlab仿真[J].機(jī)械科學(xué)與技術(shù),2013,32(3):421-425.

[17] 馮飛,張洛平,張波.四自由度機(jī)器人Matlab仿真實例[J].河南科技大學(xué)學(xué)報:自然科學(xué)版,2008,29(3):24-26.