機器人學(xué)第七章(機器人動力學(xué)的凱恩方法)

第七章機器人動力學(xué)的凱恩方法7.1引言機器人動力學(xué)凱恩方程方法是建立在凱恩動力學(xué)方程根底上的，因而本章首先介紹凱恩動力學(xué)方程。7.1.1質(zhì)點系的凱恩動力學(xué)方程設(shè)一質(zhì)點系具有n個質(zhì)點，該質(zhì)點系的動力學(xué)普遍方程為〔7-1〕式中——作用于第i質(zhì)點主動力矢量；——質(zhì)點i的質(zhì)量；——質(zhì)點i的加速度矢量；——質(zhì)點i在參考坐標(biāo)系中的位置矢量；——質(zhì)點i的微分位移；“·”——數(shù)量積符號。設(shè)質(zhì)點系為完全系，即它具有l(wèi)個自由度和l個廣義坐標(biāo)，那么〔7-2〕式中――廣義坐標(biāo)；t——時間變量；質(zhì)點i的線速度為式中〔7-3〕凱恩〔kane〕定義為質(zhì)點I相對于廣義速度的偏速度。微分可表示為〔7-4〕將〔7-4〕代入〔7-1〕式，得交換求和符號，得因為是獨立變量，故j=1,2,．．．，l〔7-5〕或這就是質(zhì)點系的凱恩動力學(xué)方程(KaneDynamicsEquation),可以改寫為〔7-6〕圖7-1剛體中點i圖7-1剛體中點i的速度如圖7-1所示將剛體看成是由n個質(zhì)點組成的。設(shè)剛體的質(zhì)心為C，以C為力的簡化中心并設(shè)作用于剛體的主動力的合力為，合力矩為：〔7-7〕〔7-8〕當(dāng)剛體以角速度旋轉(zhuǎn)時，其中點i的速度為其中——點到質(zhì)心C的位置矢量；——質(zhì)心C的線速度。點對廣義速度的偏速度為或〔7-9〕式中——質(zhì)心C相對于的偏速度：〔7-10〕——剛體相對于的偏角速度：〔7-11〕于是作用在剛體上相對于的廣義力為或〔7-12〕相對于的廣義慣性力為而式中動量矩用剛體的慣性張量表示為〔7-13〕因此〔7-14〕得廣義慣性力表示為〔7-15〕將〔7-12〕和〔7-15〕式合并，從而得到剛體的凱恩動力學(xué)方程為〔7-16〕式中I——剛體相對于質(zhì)心C的慣性張量。7.2機器人桿件速度、加速度及偏速度的遞推計算公式圖7-2n自由度機器人的桿件坐標(biāo)系x1x0x2xn圖7-2n自由度機器人的桿件坐標(biāo)系x1x0x2xnznena2z2e2z0q1=e0z1e1d1C1a1C2——指定桿件坐標(biāo)系各軸方向的單位矢量，共有i=0，1，2，．．．，n個，它們均是以桿件坐標(biāo)系描述的常矢量，；——以桿件坐標(biāo)系｛i-1｝的原點為始點到以｛i｝系原點為終點的矢量，但它是以｛i｝系描述的矢量；——以桿件坐標(biāo)系｛i｝描述的第i號桿件質(zhì)心的位置矢量；兩相鄰坐標(biāo)系{i-1}及{i}中速度、加速度等的關(guān)系可用變換矩陣中的旋轉(zhuǎn)子矩陣及相聯(lián)系。仿照第六章處理桿件坐標(biāo)系及桿件質(zhì)心的速度及加速度的方法，并考慮到坐標(biāo)系設(shè)置方法上的區(qū)別，不難得到如下所述的速度及加速度遞推計算公式：〔7-17〕式中——廣義坐標(biāo)對時間的1階導(dǎo)數(shù)，即關(guān)節(jié)軸的數(shù)量速度；——廣義坐標(biāo)對時間的2階導(dǎo)數(shù)，即關(guān)節(jié)軸的數(shù)量加速度；——關(guān)節(jié)類型識別符號；〔7-18〕與第六章相同，令〔7-19〕式中g(shù)——重力加速度。上式是假定絕對參考系的軸垂直于地面且指向向上的。假設(shè)軸垂直于地面，那么其中負(fù)號表示軸指向地心〔于重力場同方向〕。偏速度的遞推公式為：〔7-20〕例7-1如圖7-3所示的平面包2自由度機器人，、為，試用〔7-17〕及〔7-20〕式計算各桿的速度、加速度及偏速度。桿件的質(zhì)心均在桿件的末端。解：，，圖7-3平面2自由度機器人，，，圖7-3平面2自由度機器人，，。式中g(shù)——重力加速度。軸與重力場反向，故g取正。i=1時：i=2時：，，〔同上式〕此例所得各質(zhì)心的速度及加速度的計算結(jié)果與上一章的計算方法得到的結(jié)果是完全相同〔例7-2將用此例的偏速度〕。7.3關(guān)節(jié)驅(qū)動力或力矩的求解以下分末端執(zhí)行器有無負(fù)載的兩種情形進行討論。11no1nxnp~nf~nn~nCnenonx1ne1nZnZ圖7-4末端桿件廣義力j計算簡圖設(shè)末端桿件為旋轉(zhuǎn)關(guān)節(jié)，相對于的廣義力計算簡圖如圖7-4所示。圖中——作用在末端桿件坐標(biāo)系中的主動力;——在末端桿件坐標(biāo)系中的主動力矩;、——坐標(biāo)系{n}及{n-1}中z軸的單位矢量，且應(yīng)用凱恩方程〔7-16〕：j=1，2，…，n有〔7-21〕式中——桿件n的質(zhì)量，集中于質(zhì)心；——桿件n相對于質(zhì)心的慣性張量。利用偏速度的公式〔7-20〕，上式的左端為得〔關(guān)節(jié)n的力矩〕考慮〔7-21〕式的右端，所以〔7-22〕為了導(dǎo)出第n-1號桿質(zhì)心的廣義公式，需要第n號桿到n-1號桿講的受力圖，22no2nxnp~1~nf2~nn1nCncnonx1nZ2nZnZ圖7-5第n-1號桿件廣義力的計算簡圖1no1nRnnnfR~11nxnf~nnnnR~1nR~nn~圖中——在{n-1}坐標(biāo)系中表示的第n-1號桿件的主動力；——在{n-1}坐標(biāo)系中表示的第n-1號桿件的主動力矩；——在{n-1}坐標(biāo)系中表示的第n號桿件的主動力的反作用力；——在{n-1}坐標(biāo)系中表示的第n號桿件的主動力矩的反作用力矩；其余符號與圖7-3相同。相對于的廣義力為〔7-23〕將各偏速度代入〔7-23〕式(1)~(9)各項。利用三矢積(混合積)的性質(zhì)作變換〔參見〔7-21〕式的變換過程〕，可以發(fā)現(xiàn)：總計此式的8項之和時，(1)+(2)+(3)+(4)+(5)+(6)+(7)+(8)=0，因此〔關(guān)節(jié)n-1的力矩〕所以〔7-24〕仿上述推導(dǎo)過程，可得旋轉(zhuǎn)關(guān)節(jié)力矩的一般公式為〔7-25〕7.3.2有負(fù)載時關(guān)節(jié)力或力矩當(dāng)在第i號桿件上作用有系統(tǒng)的外載荷時，應(yīng)向質(zhì)心簡化，求出合力和合力矩，于是〔7-26〕假設(shè)n-1號關(guān)節(jié)為移動關(guān)節(jié)，那么公式〔7-23〕式的9項之和為0，所余為第一項，即因此〔關(guān)節(jié)n-1的力〕〔7-27〕仿前有〔7-28〕公式〔7-26〕及〔7-26〕就是關(guān)節(jié)力矩〔j為轉(zhuǎn)動關(guān)節(jié)〕及格關(guān)節(jié)力〔j為移動關(guān)節(jié)〕的計算公式例7-2設(shè)如圖8-3所示機器人兩桿件的質(zhì)量分別為，試用凱恩動力學(xué)方法求解兩個關(guān)節(jié)的力矩。解：引用例7-1所計算的速度，加速度及偏速度，并考慮到，那么由此得關(guān)節(jié)1、2的驅(qū)動力矩為此結(jié)果與牛頓—歐拉算法的、是一致的。假設(shè)將、中的、分解出來，寫成矩陣形式，那么得式中的M就是拉格朗日動力學(xué)方法的廣義質(zhì)量矩陣。7.4含有閉鏈機構(gòu)機器人的動力學(xué)計算方法當(dāng)機器人含有閉鏈機構(gòu)時，如用牛頓—歐拉算法或拉格朗日算法，通常要將閉鏈在某處切開，使其變成幾個開鏈，然后求出在切開處的約束力及約束力矩，再按開鏈情況分別處理，但用凱恩動力學(xué)算法，那么不再需要求出切開處的約束力及約束力矩，因而比擬方便。設(shè)含有閉鏈機構(gòu)的機器人具有k個從動關(guān)節(jié)，n個主動關(guān)節(jié)。從動關(guān)節(jié)變量是主動關(guān)節(jié)變量的函數(shù)：；〔7-29〕式中——從動關(guān)節(jié)變量，i=1，2，…，k，共k個；——主動關(guān)節(jié)變量，i=，,2，…，k，共n個?！?-29〕式給出了機構(gòu)的幾何約束及運動約束條件，找出這些約束之后，再將閉鏈分成假設(shè)干支路，每一路都按開鏈的凱恩動力學(xué)方法計算，求出所有桿件的速度及偏速度(i=1，2，…，n+k；j=1，2，…，n)。如果關(guān)節(jié)i是主動關(guān)節(jié)，那么可以用本章第二節(jié)的方法求速度及偏速度。如果關(guān)節(jié)i是從動關(guān)節(jié)，偏線速度及偏角速度可用以下方法計算：〔7-30〕式中應(yīng)根據(jù)已求出的，共k個關(guān)系式求得，其他各項仍按本章第二節(jié)相應(yīng)公式，最后求出各從動關(guān)節(jié)力或力矩。例如圖7-6所示的機器人，主動關(guān)節(jié)變量為，其中閉鏈的平面平行連桿機構(gòu)中，從動關(guān)節(jié)變量為，利用此機構(gòu)的幾何約束：及運動約束：圖7-6具有閉鏈機構(gòu)的機器人圖7-7閉鏈的等效開鏈圖7-7閉鏈的等效開鏈7.5機器人動力學(xué)自動建模軟件系統(tǒng)機器人實時控制和實時仿真迫切需要精確、高效的動力學(xué)模型。但由于機器人動力學(xué)模型方程的非線性性和強耦合性，其建模過程十分復(fù)雜和困難。從計算機軟件設(shè)計的觀點看來，機器人動力學(xué)模型算法可分為三類：數(shù)字法、符號法以及數(shù)字—符號法。在數(shù)字法中，所有的變量都表示為實數(shù)，每個變量占據(jù)一個內(nèi)存，這種方法計算量很大，難以在實時控制和實時仿真中應(yīng)用；在符號法中，所有的變量均表示為符號，這種方法需要先進的計算機及復(fù)雜的計算機軟件的支持，并需要較大內(nèi)存；數(shù)字—符號法將局部變量處理為實數(shù)，將局部變量處理為符號，取得了比擬好的效果。Vukobratovic對數(shù)字—符號模型法作出了重要奉獻，他提出了面向計算機的數(shù)字—符號模型算法[4,5]。在應(yīng)用數(shù)字—符號法進行動力學(xué)模型研究方面已作出了不少杰出的研究[6~10]。7.5.1動力學(xué)模型方程封閉形式的機器人開鏈系統(tǒng)〔見圖7-2〕的動力學(xué)方程的一般形式可表示為〔7-31〕式中：式中，為驅(qū)動器廣義驅(qū)動力向量；為慣性矩陣；為離心力和哥氏力效應(yīng)矩陣；為重力效應(yīng)矩陣；為機器人的廣義坐標(biāo)、廣義速度、廣義加速度向量。一、慣性矩陣現(xiàn)設(shè)機器人各構(gòu)件的廣義速度均為零，無重力作用，并設(shè)構(gòu)件i的廣義加速度為一個單位值，其余各構(gòu)件的廣義加速度均為零。即(7-32)式中，表示非構(gòu)件i的廣義加速度，即除構(gòu)件i以外其它構(gòu)件的廣義加速度。將上式代入式(7-31)，那么有慣性矩陣[H]中的第i列等于等于驅(qū)動力矢量，即(7-33)式中，表示慣性矩陣的第i列。為了求得驅(qū)動器的廣義驅(qū)動力向量，可將式〔7-33〕代入一組動力學(xué)遞推公式，動力學(xué)遞推公式既可采用基于牛頓-歐拉方程的動力學(xué)遞推公式又可采用基于凱恩方程的動力學(xué)遞推公式。(7-34)式中，為構(gòu)件i的角速度向量；為構(gòu)件i的角加速度向量；為構(gòu)件i坐標(biāo)原點的加速度向量；為構(gòu)件i質(zhì)心加速度向量；為構(gòu)件i-1作用在構(gòu)件i上的關(guān)節(jié)反力矩；為構(gòu)件i-1作用在構(gòu)件i上的關(guān)節(jié)反力；為構(gòu)件i質(zhì)心所受慣性力矩；為構(gòu)件i質(zhì)心所受慣性力；為第i坐標(biāo)系到i+1坐標(biāo)系的變換矩陣；為關(guān)節(jié)類型識別符號。在這里和以下，利用遞推公式〔7-34〕求驅(qū)動器的廣義驅(qū)動力向量時，廣義坐標(biāo)被處理為符號量，因而得到的模型矩陣各元素均為廣義坐標(biāo)的函數(shù)。廣義坐標(biāo)是由遞推公式中的變換矩陣中引入的，為了便于輸出模型矩陣各元素的實時代碼，引入符號〔7-35〕例如：，那么變換矩陣可改寫為式〔7-32〕中，將i的值從1變化到n，重復(fù)上述過程，可求得慣性矩陣[H]的所有n列元素。二、離心力效應(yīng)矩陣現(xiàn)設(shè)機器人各構(gòu)件的廣義加速度均為零，無重力作用，并設(shè)構(gòu)件i的廣義速度為一個單位值，其余各構(gòu)件的廣義速度均為零。即(7-36)將上式代入式(7-31)，那么有(7-37)式中，表示矩陣[C]中所有的第i行、第i列元素組成的列向量。將i的值從1變化到n，重復(fù)上述過程，可求得矩陣[C]中的所有對角元素，即對應(yīng)于離心力效應(yīng)的所有元素。三、哥氏力效應(yīng)矩陣現(xiàn)設(shè)機器人各構(gòu)件的廣義加速度均為零，無重力作用，并設(shè)構(gòu)件i和構(gòu)件j的廣義速度為一個單位值，其余各構(gòu)件的廣義速度均為零。即(7-38)將上式代入式(7-31)，那么有(7-39)由于，于是有(7-40)上式中，已由式〔7-37〕求得。改變i和j的值，；重復(fù)上述過程，可求得矩陣[C]中的所有的，即對應(yīng)于哥氏力效應(yīng)的所有元素。四、重力效應(yīng)矩陣設(shè)機器人各構(gòu)件的廣義速度和廣義加速度均為零，并設(shè)根底以重力加速度g向上加速，即(7-41)此時對應(yīng)于根底坐標(biāo)系Z0軸垂直向上的情況。將式〔7-41〕代入式〔7-31〕，可得(7-42)于是可得重力效應(yīng)矩陣。五、雅可比矩陣當(dāng)機器人手部末端夾持器有外力作用時，機器人動力學(xué)方程變?yōu)椋骸?-43〕式中：，分別為作用于機器人末端夾持器的外力、外力矩向量；[J]為雅可比矩陣。由于〔7-44〕可設(shè)機器人中構(gòu)件i的廣義速度為一個單位值，其余各構(gòu)件的廣義速度為零。即〔7-45〕將上式代入式〔7-44〕，那么有〔7-46〕為了求得機器人手部末端夾持器的線速度和角速度，可將式〔7-45〕代入一組運動學(xué)遞推公式〔7-47〕利用遞推公式〔7-47〕求末端夾持器的線速度和角速度時，廣義坐標(biāo)被處理為符號量，因此得到的雅可比矩陣各元素均為廣義坐標(biāo)的函數(shù)。式〔7-45〕中，將i的值從1變化到n，重復(fù)上述過程，可得雅可比矩陣的所有n列元素。7.5.2動力學(xué)模型的自動生成將機器人所有運動學(xué)和動力學(xué)參數(shù)處理為數(shù)字量，將廣義坐標(biāo)處理為符號量，由運動學(xué)和動力學(xué)遞推公式不難得出：任一運動學(xué)、動力學(xué)變量可表示為〔7-48〕式中——項數(shù)——系數(shù)——對應(yīng)于正弦、余弦、廣義坐標(biāo)函數(shù)的整數(shù)冪指數(shù)為了便于對多項式進行數(shù)字–符號混合運算，引入冪指數(shù)矩陣(7-49)于是式(7-48)可表示為(7-50)可定義其如下的根本乘加運算法那么由上述的數(shù)值－符號處理方法，可以得到數(shù)值–符號表示的運動學(xué)、動力學(xué)變量，進而可得到數(shù)值–符號表示的動力學(xué)模型矩陣的各個矩陣元素。定義如下的結(jié)構(gòu)StructModel_element{doubleu;intm[3][n];StructModel_element*Link;}由此結(jié)構(gòu)可構(gòu)成一首尾相連的線性鏈表，通過指針Link可將一個個結(jié)構(gòu)串聯(lián)起來，鏈表的終結(jié)標(biāo)志是將最后一個結(jié)構(gòu)的Link指針指向NULL(空指針)。由此可形成鏈表長度可以動態(tài)變化的動態(tài)數(shù)據(jù)結(jié)構(gòu)，運動學(xué)和動力學(xué)變量都可以用上述動態(tài)數(shù)據(jù)結(jié)構(gòu)表示和存儲。在鏈表中增加、刪除鏈表元素可以通過修改指針方便實現(xiàn)。動力學(xué)模型方程式(7-31)中的模型矩陣有如下性質(zhì)[4](1)H矩陣對稱正定，即：(2)C矩陣a.對稱性，即：b.反對稱性，即：當(dāng)i，j≥k當(dāng)i=j≥k利用上述性質(zhì)能較大地減少所需計算矩陣元素的數(shù)目。所需計算H矩陣元素個數(shù)減少，所需計算C矩陣元素個數(shù)減少。通過上述的轉(zhuǎn)化，可以將復(fù)雜的數(shù)字-符號混合處理過程轉(zhuǎn)化為簡單的矩陣運算過程，降低了建模過程的復(fù)雜性，容易通過計算機程序?qū)崿F(xiàn)自動建模，并可同時進行代碼優(yōu)化。7.5.3自動建模軟件系統(tǒng)基于前述的方法，我們開發(fā)了機器人動力學(xué)自動建模軟件系統(tǒng)DMAGS。該軟件系統(tǒng)采用C語言編程，并采用了面向?qū)ο蟮某绦蛟O(shè)計技術(shù)，可通過人機對話方式輸入機器人運動學(xué)和動力學(xué)參數(shù)，該軟件系統(tǒng)可按照Fortran語言、Pascal語言、C語言三種格式輸出動力學(xué)模型矩陣各元素的實時代碼。實時代碼的輸出需要遍歷整個線性鏈表，將一個個結(jié)構(gòu)串聯(lián)表示的多項式轉(zhuǎn)換為對應(yīng)的實時代碼形式輸出。軟件系統(tǒng)的算法流程框圖如圖7-8所示。7.5.4數(shù)值實例一3自由度機器人運動學(xué)參數(shù)和動力學(xué)參數(shù)如下：１２３mi經(jīng)我們編制的機器人動力學(xué)模型程序DMRNS運行后，自動生成模型中各矩陣元素，并以實時代碼的形式輸出，其結(jié)果如下：GG[1]=-147.000Y1Y2Y2+58.800X1X1Y2+-88.200X2X2Y1+-58.800X1X2X3X3Y2+-58.800X2X2X3X3Y1+-58.800X2X2Y1Y3Y3+-58.800X1X2Y2Y3Y3+7.5.5結(jié)論文中從采用遞推的計算結(jié)構(gòu)，減少中間變換及動力學(xué)解耦的思想出發(fā)，提出了用數(shù)字-符號法建立機器人動力學(xué)模型的新方法。該方法可適用于任意自由度帶有移動或轉(zhuǎn)動關(guān)節(jié)的機器人動力學(xué)建模，該方法對硬件要求低，也不需要復(fù)雜的符號處理軟件支持。由于建模和代碼優(yōu)化過程是離線進行的，因而大大減少了在線計算量，提高了計算效率。該方法在機器人的實時控制和實時仿真中有著廣闊的應(yīng)用前景。本章參考文獻王庭樹.機器人運動學(xué)及動力學(xué).西安：西安電子科技大學(xué)出版社，1991范守文,徐禮鉅.機器人運動學(xué)的數(shù)字—符號法研究.吉林工業(yè)大學(xué)學(xué)報，1992.8，P5-8范守文,徐禮鉅.機器人動力學(xué)自動建模軟件系統(tǒng).電子科技大學(xué)學(xué)報,1995.24(6):249-254VukobratovicM,KircanskiN.Real-timedynamicsofmanipulationrobots.NewYork:Spinger-Verlag,1984VukobratovicM,KircanskiN.Computeraidedprocedureofformingofrobotmotionequatio