機(jī)器人機(jī)械手臂的結(jié)構(gòu)設(shè)計(jì)和運(yùn)動(dòng)學(xué)分析

=v-機(jī)器人機(jī)械手臂的結(jié)構(gòu)設(shè)計(jì)和運(yùn)動(dòng)學(xué)分析摘要在本文中，我們說明了具有六自由度的機(jī)器人的機(jī)械手臂可以分為兩部分：以前三個(gè)關(guān)節(jié)相連的手臂為主要定位和最后三個(gè)關(guān)節(jié)相連的手腕為主要方向。建議把五個(gè)機(jī)械手臂和兩個(gè)機(jī)械手腕作為商業(yè)機(jī)器人機(jī)械手的基本結(jié)構(gòu)。這種簡化可以導(dǎo)致不同組合的手臂和手腕為相應(yīng)配置的通用算法的逆運(yùn)動(dòng)學(xué)。對于機(jī)器人的機(jī)械手逆運(yùn)動(dòng)學(xué)的數(shù)值解和封閉形式的解決方案，本文對此給出了非常有效和方便的方法。關(guān)鍵詞:運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)機(jī)械手臂機(jī)械手腕機(jī)器人1關(guān)鍵詞:運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)機(jī)械手臂機(jī)械手腕機(jī)器人一個(gè)機(jī)械手包括被關(guān)節(jié)連接在一起的一系列鏈接，在機(jī)械手設(shè)計(jì)過程中，運(yùn)動(dòng)鏈的選擇在機(jī)械和控制器的設(shè)計(jì)過程中尤為重要，為了控制機(jī)械手的末端執(zhí)行器，六個(gè)自由度有以下要求:三個(gè)自由度確定位置，另外三個(gè)自由度確定方向。每個(gè)機(jī)械手關(guān)節(jié)處可以提供一個(gè)自由度，如果它是在適當(dāng)?shù)奈恢煤头较蛱峁┝鶄€(gè)正交自由度，那么機(jī)械手至少要有六個(gè)關(guān)節(jié)點(diǎn)。機(jī)械手的結(jié)構(gòu)取決于關(guān)節(jié)的不同組合。工業(yè)機(jī)器人可能出現(xiàn)的結(jié)構(gòu)的變化可以確定如下:V=6DF其中：V是變化的數(shù)目DF是自由度范圍變化的數(shù)目這些因素表明，大量的不同的鏈接可以構(gòu)造，例如六軸46656鏈接是可能的。然而，大量數(shù)據(jù)也表明了它不適合運(yùn)動(dòng)的原因。我們可以把六自由度的機(jī)器人機(jī)械手分為兩部分:由前三個(gè)關(guān)節(jié)和相關(guān)鏈接組成的手臂和后三個(gè)關(guān)節(jié)和相關(guān)鏈接組成的手腕，之后運(yùn)動(dòng)鏈的變化會極大地減少。Lien已經(jīng)研究了手臂和手腕的結(jié)構(gòu)，有20種手臂和八種手腕的結(jié)構(gòu)成果。在本文中,我們把20種不同的手臂和12種不同的手腕結(jié)構(gòu)減縮為通用的12種手臂和5種手腕結(jié)構(gòu)，最后我們作出結(jié)論,5種手臂和2種手腕是工商業(yè)機(jī)械手的基本結(jié)構(gòu)。這種簡化會導(dǎo)致逆運(yùn)動(dòng)學(xué)的通用算法的配置，形成不同的組合手臂和手腕。2機(jī)器人機(jī)械手的結(jié)構(gòu)設(shè)計(jì)本文對最佳工作環(huán)境和簡單結(jié)構(gòu)，我們假設(shè)如下：a：六自由度的機(jī)器人可分為兩個(gè)部分:和前三個(gè)關(guān)節(jié)連接組成的和相關(guān)連接稱為臂，這個(gè)連接的其余關(guān)節(jié)和相關(guān)連接被稱為的手腕。b：兩個(gè)鏈接是由一個(gè)低副連接，只有轉(zhuǎn)動(dòng)和線性關(guān)節(jié)用于機(jī)器人的機(jī)械手。c：關(guān)節(jié)軸是垂直的或相互平行的。根據(jù)作者的文獻(xiàn)，假設(shè)機(jī)器人機(jī)械手適用于大多數(shù)工商業(yè)，我們可以分別考慮的其結(jié)構(gòu)手臂和手腕。2.1機(jī)械手臂的結(jié)構(gòu)2.1.1圖形表示畫一個(gè)機(jī)器人的側(cè)視圖或正視圖是復(fù)雜的，不能夠明確各部分如何移動(dòng)的關(guān)系。畫一個(gè)機(jī)器人在平面的圖太簡單，不能夠明確表示其結(jié)構(gòu)。我們忽略這些問題，用一個(gè)簡單的三維

圖來表示機(jī)器人機(jī)械手的結(jié)構(gòu)和運(yùn)動(dòng)。2.1.2關(guān)節(jié)的組合我們用R表示轉(zhuǎn)動(dòng)關(guān)節(jié)和用L代表線性組合，這個(gè)不同的組合可以得到的關(guān)節(jié)如下:(1)RRR(1)RRRRlRmH1W||RR||RR⑶RLRfl1L且|lL1R町L||R(5)LRRL1RJ.RL1R\\RL\\R±R婦IRM(7)LLRLLL±RL±L||RL||L1R⑵RRLRifllL?±R\\L引叩L(4)RLLR±LLLJ?1Lj|L?||L11RII5L(6)LRLL±R1LLIR^LL|j/?1L51犬IIL(8)LLLL1L1LLLL\]LL^LIL12345678RRRRRLRLRRLLLRRLRLLLRLLL根據(jù)不同的平行組合，前面每個(gè)組合有四個(gè)種類的子組合。然后32組合可以到達(dá):如果第二個(gè)關(guān)節(jié)是一個(gè)線性組合并且兩個(gè)其他關(guān)節(jié)是垂直于它，那么這兩個(gè)關(guān)節(jié)之間的關(guān)系被認(rèn)為是平行或垂直。(1)田』上時(shí)f⑵瓦Hn￡XrS.jE]X(3)w叫?!：⑷L.板叫;LL-tXftjkJL]±總之，有36種可能的簡單的三個(gè)關(guān)節(jié)臂組合。36種可能的組合中有9種是一個(gè)或兩個(gè)自由度，剩下有7種是平面機(jī)制，因此有20個(gè)可能的空間簡單的手臂組合。讓我們考慮R1#L2±L3第一組合允許旋轉(zhuǎn)的垂直軸，第二個(gè)關(guān)節(jié)是一個(gè)垂直線性組合，第三個(gè)是一個(gè)水平線性聯(lián)合組合，這個(gè)手臂定義了一個(gè)典型的圓柱機(jī)器人，更改關(guān)節(jié)的順序,這樣(a)垂直線性聯(lián)合優(yōu)于平行線性組合，或(b)垂直線性組合遵循水平，將導(dǎo)致沒有變化的運(yùn)動(dòng)手臂，在這種情況下有兩個(gè)聯(lián)系，兩個(gè)“等效”標(biāo)準(zhǔn)的圓柱連接。在所有這些情況下，兩個(gè)或兩個(gè)以上的等效聯(lián)系存在,連接關(guān)節(jié)將在其中線性組合，平行于中間的一個(gè)旋轉(zhuǎn)接頭，計(jì)算只有一個(gè)鏈接的20個(gè)組合中的8個(gè)，剩下的12個(gè)類別的鏈接是用不同的圖所示。我們獲得相同的結(jié)果作為參考。2.1.35種基本類型的機(jī)械手的手臂雖然有12個(gè)有用的和不同的手臂配置可以被用來設(shè)計(jì)機(jī)器人機(jī)械手的手臂，在實(shí)踐中只有一些是實(shí)用的，是常用的。我們發(fā)現(xiàn)大多數(shù)商用工業(yè)

機(jī)器人根據(jù)他們的特點(diǎn)，運(yùn)動(dòng)和手臂幾何外形可以被分解成只有五組。這五種組合如下：LCartesian(L±LlrL)QlmdriralSpherical(??1J?1L)Arriculaied(fl1fl||R)DoiibtEcylindrical(L||Kj|R)2.2機(jī)械手手腕的結(jié)構(gòu)2.2.1關(guān)節(jié)類型我們用前三個(gè)關(guān)節(jié)，機(jī)器人機(jī)械手臂要完成的主要任務(wù)是定位，然后我們使用后三個(gè)聯(lián)合提供這三個(gè)自由度的方向，并參考相關(guān)的連接作為手腕。一個(gè)完整的機(jī)械手的手腕必須包含三個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)，因?yàn)閯傮w的取向有三個(gè)自由度,例如，第一個(gè)旋轉(zhuǎn)關(guān)于x軸,然后繞y軸旋轉(zhuǎn)，最后關(guān)于z軸旋轉(zhuǎn)。2.2.2關(guān)節(jié)和鏈接的結(jié)合因?yàn)槭滞蟮亩ㄎ恢挥腥齻€(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)是簡單的,它的組合是眾多組合之一。從RRR的組合來看，我們知道只有四個(gè)之中的一個(gè)配置可用于完成機(jī)器人的手腕的定位。R〃R〃R是一個(gè)平面機(jī)制，R±R#R和R#R±R不能表達(dá)三個(gè)自由度的機(jī)器人手腕的取向，所以只有R±R±R結(jié)構(gòu)可以完成定位的任務(wù)。如果我們考慮第一個(gè)和第三個(gè)接頭之間的關(guān)系，在R±R±R結(jié)構(gòu)中，有兩種配置關(guān)系:平行和垂直如果我們讓x,y,z軸順序不同，我們可以得到多種手腕配置。在平行類型中，我們可以得到6種組合如下：12S456Z-X-zZ-Y-ZX-Y-Xx-z-xV-X-YY-Z-Y在垂直類型中，我們可以得到6種組合如下:12345§Z-Y-XZ-X-YX-Y-Zw"-y以下是12種臂配置:I,jeifliff7.田」.用審I,jeifliff7.田」.用審3.1RFir.1-12timesofAimconiifurarinr在這些配置當(dāng)中，有一些等效情況，例如：如果我們關(guān)于Z軸把Z-X-Z手腕轉(zhuǎn)90°，將會變?yōu)閆-Y-Z手腕，此外有不同的空間位置的配置也可視為等價(jià)，例如:Y-Z-Y手腕可以等價(jià)為Y-Z-X手腕,因?yàn)槿绻P(guān)于Z軸把最后的關(guān)節(jié)旋轉(zhuǎn)90°,我們可以得到相同的結(jié)構(gòu)。在所有這些情況下，兩個(gè)或兩個(gè)以上的等效情況存在，那么我們可以把12種手腕配置減少到5種有用的配置，被用來設(shè)計(jì)機(jī)器人機(jī)械手的手腕。在5種類型中，有2種手腕配置,例如：Z-Y-Z和Y-X-Z，它們常用在工業(yè)實(shí)踐中。X,Y和Z軸的序列會影響復(fù)雜的逆運(yùn)動(dòng)學(xué)的解決方案。2.3典型的機(jī)器人機(jī)械手結(jié)構(gòu)在工業(yè)實(shí)踐中，我們可以使用5種手臂配置和2種手腕配置，它們是結(jié)合十種不同的六自由度機(jī)器人的機(jī)械手，當(dāng)然，我們也可以考慮其他12種手臂類型中的7種和5種手腕類型中的3種來建立一個(gè)新的機(jī)器人機(jī)械手。但大多數(shù)并沒有應(yīng)用在工業(yè)實(shí)踐中。Doublecylindrical(1%)Spherical

U3%)5種主要類型的手臂配置在當(dāng)前的使用情況Doublecylindrical(1%)Spherical

U3%)3機(jī)器人機(jī)械手逆運(yùn)動(dòng)學(xué)的解決方案3.1一般原則找到一個(gè)機(jī)器人機(jī)械手的逆運(yùn)動(dòng)學(xué)方程起初似乎是一項(xiàng)艱巨的任務(wù)，但當(dāng)機(jī)械手被分成兩個(gè)部分后，變得相對簡單了。機(jī)械手通過關(guān)節(jié)鏈接轉(zhuǎn)動(dòng)連接在一起，其位置和定向之間的關(guān)系可以用一個(gè)矩陣表示:』sinft』sinftA.=JQ0-sin&fcoscr{

cos&cosarf

sina,0sinOfsina；a：cos&&sinoffatiin0,cos%df01其中：ej表示第i個(gè)關(guān)節(jié)變量1表示第i個(gè)關(guān)節(jié)偏移量ai表示第i個(gè)鏈接的長度ai表示第i個(gè)鏈接的扭轉(zhuǎn)角機(jī)器人機(jī)械手的末端執(zhí)行器的位置和方向是矩陣:(2)由結(jié)合律矩陣可以重新集結(jié)成兩個(gè)子集，它分別代表的是手臂和手腕:=T*=T*^2■出由4A6)(3)其中土.七AU14）aaa5==⑴這個(gè)矩陣指定參考框架,a代表手臂的尖端,w代表手腕的尖端，即機(jī)械手末端執(zhí)行器的中心。前面的末端執(zhí)行器的矩陣可以寫成一個(gè)4x4齊次矩陣，它由一個(gè)方向子矩陣R和一個(gè)位置向量組成，如下：67/Il(8我們可以直接得到向量0P的分析方法。細(xì)節(jié)在下面提到:a勝=耕-腫67/Il(8從方程（4）中可以得到如下方程：

嚴(yán)饑如色)打伊但判(10)我們可以獲得。1,e2,e3,前三個(gè)關(guān)節(jié)變量可以從以下方程算的：TOC\o"1-5"\h\z?p(S鞏如=牙(11)機(jī)器人的末端執(zhí)行器的方向可以視為機(jī)械手產(chǎn)品的手臂和手腕的方向:澎=源汰=用5(12)從方程(12)和(5)，可以得到：次三％尸炯三邛(13)其中泌-⑶頃：'叫(17?)=rotationpartofmatrices(A]A243)(15)(沮)=rotationpartatmatrices(Aa&/)(16)我們可以求解方程(13)得到最后的三個(gè)關(guān)節(jié)變量e4,e5,e6I.Z-F-Z(Eulerangles)iype2.V-X-2(R-P-Yangles)type2種主要類型的手腕配置在當(dāng)前的使用3.2不同的方法機(jī)器人機(jī)械手有兩種解決方案:封閉形式的解決方案和數(shù)值解決方案，因?yàn)樗麄兊牡匦圆煌瑪?shù)值解決方案通常都比相應(yīng)的封閉形式的解決方案慢得多，以至于對大多數(shù)時(shí)候使用我們不感興趣的數(shù)值方法來解決運(yùn)動(dòng)學(xué)。但總的來說，數(shù)值算法比封閉形式的解決方案更容易得到。在本文中，我們提出了算法的兩個(gè)解決方案。3.2.1封閉形式的解決方案在封閉形式的解決方案中，問題的關(guān)鍵是得到手臂尖端的位置p，在手腕軸的一點(diǎn)上，找到手臂間斷的位置是容易的。但是在手腕軸偏移量是復(fù)雜的，因?yàn)槭滞蟮倪\(yùn)動(dòng)將大大影響機(jī)械手的末端執(zhí)行器的位置。

卜面，我們使用RRR+歐拉角和RRR+R-P-Y角為例來描述如何得到手臂末端的位置。RRR+歐拉角法圖6顯示了RRR+歐拉角機(jī)器人機(jī)械手和為D-H代表的坐標(biāo)系統(tǒng)。圖6這個(gè)圖顯示了手臂和手腕向量之間的關(guān)系。6P是機(jī)器人機(jī)械手末端執(zhí)行器坐標(biāo)框架中心的位置向量。一個(gè)代表結(jié)束效應(yīng)器的法向方向，aP美聯(lián)社是從原點(diǎn)聯(lián)接點(diǎn)的手臂和手腕測量得出的手臂矢量。&P是從手臂和手腕的連接點(diǎn)到中心端測量的具有相同方向的手腕向量。從手臂末端測量W的機(jī)器人機(jī)械手中心的位置，都與0號框架有關(guān)，我們可以得到：驢=(17)這說明末端執(zhí)行器的總的轉(zhuǎn)換是手臂末端基本轉(zhuǎn)換的總和加上從手臂末端到末端執(zhí)行器的中心的轉(zhuǎn)換的總和。從方程(17)中，我們可以得到工的位置如下：(IS)可以從方程(10)和(11)獲得前三個(gè)組合變量。1，e2,e3,聯(lián)合方程(13)可以得到后三個(gè)組合變量。4,e5,e6。詳細(xì)的解決方案顯示在附錄。RRR+R-P-Y角法圖7顯示了RRR+R-P-Y角法機(jī)器人機(jī)械手和坐標(biāo)系統(tǒng)。

圖7歐拉角不同于R-P-Y角，因?yàn)殛P(guān)節(jié)4的運(yùn)動(dòng)影響了向量P,這里是一個(gè)例子演示了如何4解決手腕軸偏移。在前面的例子中P和A是一樣的，wP2是從關(guān)節(jié)中心的末端執(zhí)行器測量的手腕向量。手臂的位置，可以從下列方程組計(jì)算解決：可2護(hù)=護(hù)一況舛_x衛(wèi)崖也—豚X電IOp二舉m業(yè)”i電x時(shí)然后我們可以從方程（10）和（11）獲得。1，e2,e3,從方程（13）獲得。4,e5,e6。（c）一般的封閉算法第一步：找到末端執(zhí)行器方向向量：A=（AAA）XYZ第二步：如果有一些代替了手腕結(jié)構(gòu)，用矢量代數(shù)來確定手臂的位置向量P，然后提示轉(zhuǎn)到W步驟4,否則繼續(xù)步驟3。第三步：直接使用矢量A的方法計(jì)算手臂向量aP第四步：用方程（10）和（11）中的向量計(jì)算前三個(gè)關(guān)節(jié)變量e1,e2,e3。第五步：用方程（13）計(jì)算后三個(gè)關(guān)節(jié)變量e4,e5,e6。這種方法表明，該計(jì)算能夠最低限度減少整體的問題，使計(jì)算成為獨(dú)立的步驟從而降低錯(cuò)誤和減少工作量。3.2.2數(shù)值解該算法對數(shù)值的解決方案如下：第一步：假設(shè)后三個(gè)關(guān)節(jié)變量e4,e5,e6由現(xiàn)有的最佳逼近，應(yīng)該從先前的點(diǎn)計(jì)算。

第二步：從方程(10)(11)計(jì)算手臂的三個(gè)變量。1,02,03。第三步：參考第二步計(jì)算出的手臂關(guān)節(jié)變量，從方程(13)計(jì)算腕關(guān)節(jié)變量。第四步:用第二步和第三步計(jì)算出的組合變量，計(jì)算機(jī)器人機(jī)械手末端執(zhí)行器的位置和方向。第五步：如果給定值和計(jì)算值之間的誤差小于預(yù)先指定的值，停止計(jì)算，否則就從第二步繼續(xù)計(jì)算。上面的計(jì)算過程是手臂和手腕交替地運(yùn)動(dòng)，滿足末端執(zhí)行器的位置和定向規(guī)范，如果保持手腕(或手臂)固定，每次只能夠移動(dòng)手臂(或手腕)。該方法實(shí)現(xiàn)了PUMA600機(jī)器人機(jī)械手，人們已經(jīng)發(fā)現(xiàn)四數(shù)量迭代足夠達(dá)到精度要求并且這個(gè)數(shù)字已經(jīng)固定在逆運(yùn)動(dòng)學(xué)解決方案中。該算法的優(yōu)點(diǎn)是用相同的算法解決不同種類的機(jī)器人，但這方法比封閉形式的解決方案需要這么多更多的計(jì)算時(shí)間，所以它不適合實(shí)時(shí)控制機(jī)器人的機(jī)械手。4結(jié)論各種機(jī)器人配置的可能是非常大的，方法A強(qiáng)調(diào)機(jī)器人機(jī)械手可分離成主要子系統(tǒng)的重要性，數(shù)字處理各種機(jī)器人可能是模塊化的，因此大大簡化了常用手臂和手腕的結(jié)構(gòu)。研究發(fā)現(xiàn)只存在12種類型的手臂結(jié)構(gòu)和5種不同類型的手腕結(jié)構(gòu)，在這些結(jié)構(gòu)類型中，有5種手臂結(jié)構(gòu)和2種手腕結(jié)構(gòu)在工業(yè)中應(yīng)用，均使用派生的變換矩陣法和逆運(yùn)動(dòng)的解決方法。本文給出了封閉形式和數(shù)值解的逆運(yùn)動(dòng)學(xué)這兩個(gè)算法。5參考文獻(xiàn)LHJrWarnecke,nndR.D,Schraft,IndustrialRobots(】FSLtd,Bedford.J9也).TerjeK.Lien,Bafiestynngforuniverseitehtmdterings-automtert(NTHtTrondheim,Norwayj1^80).V,Milenkovic&B.Huai堪HKincmaUcsofMajorRobotLinkage"13/AinlernationalSynipasitimon{ndiatrialRobotsandRot^r72.31*(1983),P-CoiffetandM.Chirou?etAn/ntroduciianfaRobotTechnology(Kog^nPageLid.,Ltmdan,1983).J.Denavit,andR,S.HartenbergH"AKinemMieNotationforLawPairMechanismsBasedonMatrices*'J.Appt,MechanicsTrans.ASMEV.77,215-221(June,1955).D，LPiepertllTheKinematicsofManipulatorsUnderComputerControl/1AIM72(StanfordpCalihSianfordUniversityArtificialIntelligenceLaboratory).7rR.P,Paul.RobotManipulators:Mathematics,Pro§rariimin^tantiControl(TheM]TPress,Cambridge,MA,1982).&C.GSLee,FundatnentaiofR&bciics{Addison-Wesley,London1983).6附錄PUMA600和RRR+歐拉角法的逆運(yùn)動(dòng)學(xué)與封閉形式的解決方法，在下面具體給出了應(yīng)用。6.1坐標(biāo)系統(tǒng)和鏈接參數(shù)下表給出了鏈接參數(shù)：

JainiVariableaa(mm)d(mm)Range(decree)Ie.-90.00.0GO-160.0to+16000.04餐0l敏5-225.0to+43.0'3為W.O43?。0.0-45.0to+225.004-90。0-0GO-110.0to+m.o%900g0.0-W0to+100.00A0.00.056.5^21^.0to+266.0在這個(gè)例子中，我們將XYZ坐標(biāo)框架從關(guān)節(jié)點(diǎn)4移到關(guān)節(jié)點(diǎn)5,XYZXYZXYZ坐標(biāo)333333,444，555框架的起點(diǎn)可能重合在一個(gè)點(diǎn)。這使得齊次變換矩陣A3與在D-H坐標(biāo)系矩陣表達(dá)形式不同，然而這種變化可能使手臂和手腕之間分離概念更清晰。圖8圖8從上圖8中，我們可以獲得均勻A3變換矩陣如下:&=RDT(&3y)TRANS(r-鼻3)ROT?！?)cos務(wù)0sin03a3sin務(wù)sin務(wù)cos務(wù)0sin03a3sin務(wù)sin務(wù)001-CDS0-dacos色0(A-l)使用方程(1)可以獲得其他的轉(zhuǎn)換矩陣，所有的變換矩陣如下:Ci0Q001—J30叱sCj001由0fl010與001000000的-yoi其中其中0七°〕f00-志=001F0000%00I0&_0001--0001_q=cos0t\Si=sin&;cif后cos0+們);Sy■sin(艮+q).=A2A.一勺00-一s0alc2510們0Jis0"a0T00001-0001_.0001Jc30Jj如3js0-Cj-地勺0100000]。1宣3—占1門占MA—t/jS(StC33ClS\S22明互&3+a2slc2+?善1-S220匕陌隊(duì)七3—a2^20001(A-3)0—十40一一。50Ss0~0cA00_勺00-1000000001_001_"4.^3#6C6F00qoo001而0001-C4€5S6-S4C6C,J5J66^3隊(duì)匚牌右+ONb—E.C*羯*￡?耳勺扁％d^SjE；；FC&0(A-4)6.2手臂關(guān)節(jié)變量。1,02,03的計(jì)算6.2.1找到末端執(zhí)行器的方向矢量A,轉(zhuǎn)換矩陣的末端執(zhí)行器的機(jī)器人機(jī)械手給定如下:NSA,000(A-5)從方程(5),我們直接獲取方向向量：口="hA.??「「(A-6)6.2.2從方程(5)計(jì)算手臂位置向量aP，如下:(A-7)看到矢量關(guān)系在圖4中，〃'一1掃'遇一1毒1見(A-8)或者根據(jù)方程(8)得到：>1「匾I為Py~MAy乙—閩孔弓-IdfijAj)_p上一俗?9)或者(A-10)(A-11)(A-12)6.2.3從方程(9)(10)中，計(jì)算。1,02,03：?P+a2cjc2—^2JL"23—嚀Z從方程(13)把向量aP分量為:口Nl電3?P+a2cjc2—^2JL"23—嚀Z從方程(13)把向量aP分量為:口Nl電3+電Cl處一.點(diǎn)=《心口硒世23+妃g+d亦I=御出3如3-蜘由(A-14)(A-15)(A-16)為了計(jì)算01，方程(14)和(15)分別乘以-s1和cl，得到:-4E飽導(dǎo)3-叫匚國隊(duì)十4$：=(A-17)隊(duì)CjElE日+叱匚曲￡衛(wèi)+義老n￡』與(A-19)為了解決方程的形式，我們使用三角函數(shù):(A-20)(A21)(A-22)其中(A必)從方程(19)可以得到皿呼對LZ4)其中(A25)方程(24)縮寫到(A-26)其中心7)cosza+sin2or—lhcos(0——士VI一djr綜上所述，最后得到:伉*"（將卜或旬弟eB）為了計(jì)算。2,方程（14）和（15）分別乘以cl和si,得到:日3。端3+勤杏七一d嘩心=8巴門（A-30）a3j?j2]+a2JiQ+d2slci-（A，31）和ee：：-We3沁(A-33)(A34)_■一竺N(A-33)(A34)讓/〔?A.?，.En得到口、_dG+電的^23~TOC\o"1-5"\h\z從方程（16）得到：…（W+（*?（心）g-臉頊丹％"從上面方程延伸出右面方程：,'七o_+思+此:-弱讓制得到-此""一815如上所述，使用同樣的方法解決這個(gè)類型的方程，我們可以獲得：ABeqk味市（A雀）如果B>0，02的約束條件為：0<0-^<180°（A-39）4放（A，40）為了計(jì)算02,方程（33）被方程（34）分為：（A-41）C2J岌.竺％或者頃5"或者頃5"捋

四=k（務(wù)巖）S(A-42)6.3手腕關(guān)節(jié)變量。4,05,06的計(jì)算從方程(15)和(A-3)得到:從方程(15)和(A-3)得到:*叫f1處3J|c23sis23L”30C2i(A44)MSx混=NySyAy(A-46)從方程(14)和(A-5)得到：Lm￡!」自旋轉(zhuǎn)矩陣是正交的，它的逆矩陣也等于它的轉(zhuǎn)置，或們。23^lc13Hi—3/3(折廣、"=—t以0(AH7)_CjS^j5'iJ23-然后右邊的方程(12)L從方程(16)和(A-4)得到:(抵)T歡=一珀0c23c】3FCiJ1J13c4c5c6-sas6c4c5c6-sas6S^5C6+3&-國(A-45)■虬■N,■性4」A(A-48)其中NWI?Cl63凡+Si喝Ny—j23A/z河十=TiM+們丹Ny-c國3乂+玩$歸Ny+c^NxSWI—c1+s■***■&=F&+玖&5心pct十￡|$涉§十點(diǎn)心=。|臼3丹.十七以一