機(jī)器人原理及控制技術(shù)第0304章 運(yùn)動(dòng)學(xué)方程與逆運(yùn)動(dòng)方程課件

第三章

運(yùn)動(dòng)學(xué)方程

ChapterⅢKinematicEquations

3.1引言3.8T6的說(shuō)明3.2姿態(tài)描述3.9各種A矩陣的說(shuō)明3.3歐拉角3.10根據(jù)A矩陣來(lái)確定T6

3.4搖擺、俯仰和偏轉(zhuǎn)3.11斯坦福機(jī)械手的運(yùn)動(dòng)方程3.5位置的確定3.12肘機(jī)械手的運(yùn)動(dòng)方程3.6圓柱坐標(biāo)3.13小結(jié)3.7球坐標(biāo)第三章運(yùn)動(dòng)學(xué)方程

ChapterⅢKinemat3.1引言(Introduction)本章，我們采用齊次變換來(lái)描述在各種坐標(biāo)系中機(jī)械手的位置與方向。首先介紹各種正交坐標(biāo)系的齊次變換。然后介紹在非正交關(guān)節(jié)坐標(biāo)系中描述機(jī)械手末端的齊次變換。注意，對(duì)任何數(shù)目關(guān)節(jié)的各種機(jī)械手均可以這樣進(jìn)行。描述一個(gè)連桿與下一個(gè)連桿之間關(guān)系的齊次變換稱(chēng)A矩陣。A矩陣是描述連桿坐標(biāo)系之間的相對(duì)平移和旋轉(zhuǎn)的齊次變換。連續(xù)變換的若干A矩陣的積稱(chēng)為T(mén)矩陣，對(duì)于一個(gè)六連桿（六自由度）機(jī)械手有

T6=A1A2A3A4A5A6

（3.1）六連桿的機(jī)械手有六個(gè)自由度，其中三個(gè)自由度用來(lái)確定位置，三個(gè)自由度用來(lái)確定方向。Ｔ６表示機(jī)械手在基坐標(biāo)中的位置與方向。則變換矩陣Ｔ６有下列元素

nxoxaxpxnyoyaypyT6=nzozazpz

（3.2）

00013.1引言(Introduction)

如圖3.1所示，機(jī)器人的末端執(zhí)行器（手爪）的姿態(tài)（方向）由n、o、a三個(gè)旋轉(zhuǎn)矢量描述，其坐標(biāo)位置由平移矢量p描述，這就構(gòu)成了式（3.2）中的變換矩陣T。由于n、o、a三個(gè)旋轉(zhuǎn)矢量是正交矢量，所以有n=o×a圖3.1末端執(zhí)行器的描述如圖3.1所示，機(jī)器人的末端執(zhí)行器（手爪）的3.2姿態(tài)描述(SpecificationofOrientation)

對(duì)式（3.2）中16個(gè)元素一一賦值就可確定T6。假定機(jī)械手可以到達(dá)要求的位置，而單位旋轉(zhuǎn)矢量o和a正交，即

o·o＝1

（3.3）a·a＝1（3.4）o·a＝0（3.5）a形成單位向量

aa（3.6）|a|構(gòu)成與o和a正交的nno×a（3.7）

在o和a形成的平面上旋轉(zhuǎn)o，使得o與n和a正交

oa×n（3.8）單位向量o是oo（3.9）|o|根據(jù)第二章給出的一般性的旋轉(zhuǎn)矩陣Ｒot(k,θ)，它把機(jī)械手末端的姿態(tài)規(guī)定為繞k軸旋轉(zhuǎn)θ角。3.2姿態(tài)描述(SpecificationofOr3.３歐拉角(EulerAngles)

姿態(tài)變更常用繞x,y或z軸的一系列旋轉(zhuǎn)來(lái)確定。歐拉角描述方法是：先繞z軸旋轉(zhuǎn)?，然后繞新的y(即y/)軸旋轉(zhuǎn)θ，最后繞更新的z(z//)軸旋轉(zhuǎn)ψ（見(jiàn)圖3.2）歐拉變換Euler(?,θ,ψ)可以通過(guò)連乘三個(gè)旋轉(zhuǎn)矩陣來(lái)求得Euler(?,θ,ψ)＝Ｒot(z,?)Ｒot(y,θ)Ｒot(z,ψ)（3.10）在一系列旋轉(zhuǎn)中，旋轉(zhuǎn)的次序是重要的。應(yīng)注意，旋轉(zhuǎn)序列如果按相反的順序進(jìn)行，則是繞基坐標(biāo)中的軸旋轉(zhuǎn)：繞z軸旋轉(zhuǎn)ψ，接著繞y軸旋轉(zhuǎn)θ，最后再一次繞z軸旋轉(zhuǎn)?，結(jié)果如圖3.3所示，它與圖3.2是一致的。3.３歐拉角(EulerAngles)姿態(tài)變xx’x’’x’’’yy’’y’θ?θ?ψψψθz’’’z’’z’zy’’’圖3.2歐拉角?0xx’x’’x’’’yy’’y’θ?θ?ψψψθz’’’z’’z’zy’’’圖3.3基于基坐標(biāo)的歐拉角?0?θxx’x’’x’’’yy’’y’θ?θ?ψψψθz’’’z’3.4搖擺、俯仰和偏轉(zhuǎn)(Roll,PitchandYaw)

搖擺、俯仰和偏轉(zhuǎn)為另一種旋轉(zhuǎn)。如圖3.4所示，就像水中航行的一條小船一樣，繞著它前進(jìn)的方向（z軸）旋轉(zhuǎn)?稱(chēng)為搖擺，繞著它的橫向中軸（y軸）旋轉(zhuǎn)θ稱(chēng)為俯仰，繞著它甲板的垂直向上的方向（x軸）旋轉(zhuǎn)ψ

稱(chēng)為偏轉(zhuǎn)。借助于這種旋轉(zhuǎn)來(lái)描述機(jī)械手的末端執(zhí)行器如圖3.5所示。規(guī)定旋轉(zhuǎn)的次序?yàn)?/p>

RPY(?,θ,ψ)＝Ｒot(z,?)Ｒot(y,θ)Ｒot(x,ψ)（3.12）

即，繞x軸旋轉(zhuǎn)ψ，接著繞y軸旋轉(zhuǎn)θ，最后繞z軸旋轉(zhuǎn)?

，這個(gè)變換如下

cosθ0sinθ0100001000cosψ–sinψ0RPY(?,θ,ψ)=Ｒot(z,?)–sinθ0cosθ00sinψcosψ0

（3.13）00010001

cos?–sin?00cosθsinθsinψsinθcosψ0sin?cos?000cosψ–sinψ0RPY(?,θ,ψ)=0010

-sinθcosθsinψcosθcosψ0

（3.14）00010001

3.4搖擺、俯仰和偏轉(zhuǎn)(Roll,Pitcha圖3.4搖擺、俯仰和偏轉(zhuǎn)角圖3.5機(jī)械手的末端執(zhí)行器的搖擺、俯仰和偏轉(zhuǎn)圖3.4搖擺、俯仰和偏轉(zhuǎn)角圖3.5機(jī)械手的末端執(zhí)行RPY(?，θ，ψ)=cos?cosθcos?sinθsinψ–sin?cosψcos?sinθcosψ+sin?sinψ0sin?cosθsin?sinθsinψ+cos?cosψsin?sinθcosψ–cos?sinψ0

-sinθcosθsinψcosθcosψ00

0

01

（3.15）RPY(?，θ，ψ)=（3.15）3.5位置的確定(SpecificationofPosition)

一旦方向被確定之后，用一個(gè)相應(yīng)的p向量的位移變換可得到機(jī)器人末端執(zhí)行器在基坐標(biāo)中的位置：

100px

010py

T6=

001pz

（3.16）0001

旋轉(zhuǎn)變換矩陣3.5位置的確定(Specificationof3.6圓柱坐標(biāo)(CylindricalCoordinates)

如圖3.6所示，在圓柱坐標(biāo)中確定機(jī)械手的位置是沿x軸平移r，接著繞z軸旋轉(zhuǎn)α，最后沿著z軸平移z。Cyl(z,α,r)=Trans(0,0,z)Rot(z,α)Trans(r,0,0)

cosα-sinα00100rsinαcosα000100Cyl(z,α,r)=Trans(0,0,z)0010001000010001（3.17）

1000cosα-sinα0rcosα0100sinαcosα0rsinαCyl(z,α,r)=001z001000010001

（3.18）zaCzyxBAoαrn圖3.6圓柱坐標(biāo)注意：圓柱坐標(biāo)只能繞z軸旋轉(zhuǎn)3.6圓柱坐標(biāo)(CylindricalCoordicosα-sinα0rcosαsinαcosα0rsinαCyl(z,α,r)=001z（3.19）0001

如用一個(gè)繞z軸旋轉(zhuǎn)-α的變換矩陣右乘式（3.19）,結(jié)果如下

cosα-sinα0rcosαcos(-α)-sin(-α)0

0sinαcosα0rsinαsin(-α)cos(-α)0

0Cyl(z,α,r)=

001z0000（3.20）00110001cosα-sinα0rcosαcosαsinα

00sinαcosα0rsinα-sinαcosα00Cyl(z,α,r)=001z0000

（3.21）00010001100rcosα010rsinαCyl(z,α,r)=001z（3.22）0001

上式表明平移矢量未變，旋轉(zhuǎn)矩陣為單位陣，此時(shí)末端坐標(biāo)的姿態(tài)未變，而只是改變了它的空間位置。

3.7球坐標(biāo)(SphericalCoordinates)

如圖3.7所示，用球坐標(biāo)來(lái)確定位置向量的方法是：沿著z軸平移γ，然后繞y軸旋轉(zhuǎn)β，最后繞z軸旋轉(zhuǎn)α。Sph(α,β,γ)=Rot(z,α)Rot(y,β)Trans(0,0,γ)（3.23）cosβ0sinβ0100001001100Sph(α,β,γ)=Rot(z,α)-sinβ0cosβ0001γ00010001

（3.24）αaonzyx圖3.7球坐標(biāo)γβ

3.7球坐標(biāo)(SphericalCoordinatecosα-sinα00cosβ0sinβrsinβsinαcosα000100Sph(α,β,γ)=0010-sinβ0cosβrcosβ（3.25）00010001

cosαcosβ-sinαcosαsinβγcosαsinβsinαcosβcosαsinαsinβγsinαsinβSph(α,β,γ)=-sinβ0cosβγcosβ（3.26）0001

同樣，如果不希望改變末端坐標(biāo)的姿態(tài)，而只是改變其空間位置，我們可以用Rot(y,-β)和Rot(z,-α)右乘式（3.26）Sph(α,β,γ)=Rot(z,α)Rot(y,β)Trans(0,0,γ)Rot(y,-β)Rot(z,-α)（3.27）100γcosαsinβ010γsinαsinβSph(α,β,γ)=001γcosβ（3.28）0001

3.7T6的確定(SpecificationofT6)

T6可以用旋轉(zhuǎn)和平移的方法來(lái)確定。

T6=[平移][旋轉(zhuǎn)]

（3.29）

表3.1各種平移與旋轉(zhuǎn)的表達(dá)式

[Translation]Eqn[Rotation]Eqn

px,py,pzoxoyozaxay

az

Rot(k,θ)2.32Cyl(z,α,r)3.22Euler(?,θ,ψ)3.11Sph(α,β,γ)3.26RPY(?,θ,ψ)3.12

我們已經(jīng)研究過(guò)的各種平移與旋轉(zhuǎn)的式子，總結(jié)在表3.1中。如果我們使用Cyl和Sph的非旋轉(zhuǎn)的形式，那么矩陣積（3.29）僅僅是一個(gè)平移變換。

3.7T6的確定(Specificationof3.9各種A矩陣的確定(SpecificationofmatricesA)

現(xiàn)在考慮方程(3.1)右邊各A矩陣的確定。串聯(lián)桿型機(jī)械手是由一系列通過(guò)連桿與其活動(dòng)關(guān)節(jié)連接在一起所組成。如圖3.8所示，任何一個(gè)連桿都可以用兩個(gè)量來(lái)描述：一個(gè)是公共垂線(xiàn)距離an，另一個(gè)是與an垂直的平面上兩個(gè)軸的夾角αn，習(xí)慣上稱(chēng)an為連桿長(zhǎng)度，αn稱(chēng)為連桿的扭轉(zhuǎn)角。圖3.8連桿的長(zhǎng)度與扭轉(zhuǎn)角3.9各種A矩陣的確定(Specification

如圖3.9所示，在每個(gè)關(guān)節(jié)軸上有兩個(gè)連桿與之相連，即關(guān)節(jié)軸有兩個(gè)公垂線(xiàn)與之垂直，每一個(gè)連桿一個(gè)。兩個(gè)相連的連桿的相對(duì)位置用dn和θn確定，dn是沿著n關(guān)節(jié)軸兩個(gè)垂線(xiàn)的距離，θn是在垂直這個(gè)關(guān)節(jié)軸的平面上兩個(gè)被測(cè)垂線(xiàn)之間的夾角，dn和θn分別稱(chēng)作連桿之間的距離及夾角。圖3.9連桿參數(shù)xn-1如圖3.9所示，在每個(gè)關(guān)為了描述連桿之間的關(guān)系，我們對(duì)每個(gè)連桿賦一個(gè)坐標(biāo)系。

轉(zhuǎn)動(dòng)關(guān)節(jié)：關(guān)節(jié)變量為θn。連桿n的坐標(biāo)原點(diǎn)設(shè)在關(guān)節(jié)n和關(guān)節(jié)n+1軸之間的公共垂線(xiàn)與關(guān)節(jié)n+1軸的交點(diǎn)上。在關(guān)節(jié)軸相交的情況下（無(wú)公垂線(xiàn)），這個(gè)原點(diǎn)就在兩個(gè)關(guān)節(jié)軸的相交點(diǎn)上（an＝0）。如果兩個(gè)關(guān)節(jié)軸平行（有無(wú)數(shù)條公垂線(xiàn)），則原點(diǎn)的選擇要使下一個(gè)連桿的關(guān)節(jié)距離為0（dn＝0），連桿n的z軸與n+1關(guān)節(jié)軸在一條直線(xiàn)上。x軸與任何存在的公共垂線(xiàn)成一條直線(xiàn)，并且沿著這條垂線(xiàn)從n關(guān)節(jié)指向n+1關(guān)節(jié)。在相交關(guān)節(jié)的情況下，x軸的方向平行或者逆平行zn-1×zn的向量叉積，應(yīng)該注意，這個(gè)條件對(duì)于沿著關(guān)節(jié)n和n+1之間垂線(xiàn)的x軸同樣滿(mǎn)足。當(dāng)xn-1和xn平行，且有相同的指向時(shí)，則對(duì)于第n個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)θn＝0。連桿本身的參數(shù)連桿長(zhǎng)度an連桿兩個(gè)軸的公垂線(xiàn)距離（x方向）連桿扭轉(zhuǎn)角αn連桿兩個(gè)軸的夾角（x軸的扭轉(zhuǎn)角）連桿之間的參數(shù)連桿之間的距離dn相連兩連桿公垂線(xiàn)距離（z方向平移距）連桿之間的夾角θn相連兩連桿公垂線(xiàn)的夾角（z軸旋轉(zhuǎn)角）表3.2連桿參數(shù)為了描述連桿之間的關(guān)系，我們對(duì)每個(gè)連桿棱形關(guān)節(jié)：關(guān)節(jié)變量為dn。關(guān)節(jié)軸的方向就是關(guān)節(jié)的運(yùn)動(dòng)方向。與轉(zhuǎn)動(dòng)關(guān)節(jié)不同，軸的運(yùn)動(dòng)方向被確定了，但在空間的位置并沒(méi)有確定（見(jiàn)圖2.10）。對(duì)于棱形關(guān)節(jié)，連桿長(zhǎng)度an沒(méi)有意義，所以被設(shè)置為0。棱形關(guān)節(jié)坐標(biāo)的z軸（zn）與下一個(gè)連桿的軸在一條直線(xiàn)上，x軸（xn）平行或逆平行棱形關(guān)節(jié)軸的方向（zn-1）與zn的叉積。對(duì)于棱形關(guān)節(jié)，當(dāng)dn=0時(shí)，定義為0位置（即坐標(biāo)原點(diǎn)）。因此棱形關(guān)節(jié)坐標(biāo)原點(diǎn)與上一個(gè)關(guān)節(jié)（n-1）坐標(biāo)原點(diǎn)重合，上一個(gè)關(guān)節(jié)的z軸（zn-1）與棱形關(guān)節(jié)的軸向相同，其關(guān)節(jié)長(zhǎng)度an-1為上一個(gè)關(guān)節(jié)的軸線(xiàn)與zn-1的公垂線(xiàn)長(zhǎng)度，xn-1軸向?yàn)楣咕€(xiàn)向下一個(gè)關(guān)節(jié)延伸的方向。圖3.10棱型關(guān)節(jié)的連桿參數(shù)an-1棱形關(guān)節(jié)：關(guān)節(jié)變量為dn。關(guān)節(jié)軸的方向就是關(guān)節(jié)的運(yùn)動(dòng)方向。與根據(jù)上述模式用下列旋轉(zhuǎn)和位移我們可以建立相鄰的n-1和n坐標(biāo)系之間的關(guān)系：繞zn-1旋轉(zhuǎn)一個(gè)角度θn沿zn-1位移一個(gè)距離dn沿著被旋轉(zhuǎn)的xn-1即xn位移an繞xn旋轉(zhuǎn)的扭轉(zhuǎn)角為αn這四個(gè)齊次變換的積為A矩陣，即An=Rot(z,θ)Trans(0,0,d)Trans(a,0,0)Rot(x,α)（3.30）cosθ-sinθ00100a1000sinθcosθ0001000cosα-sinα0An=0010001d0sinαcosα0（3.31）000100010001

cosθ-sinθcosαsinθsinαacosθsinθcosθcosα-cosθsinαasinθAn=0sinαcosαd（3.32）0001根據(jù)上述模式用下列旋轉(zhuǎn)和位移我們可以建立相鄰的

對(duì)于棱形關(guān)節(jié)，an=0，則式（3.32）A矩陣簡(jiǎn)化為

cosθ-sinθcosαsinθsinα0sinθcosθcosα-cosθsinα0An=0sinαcosαd（3.33）0001

一旦給機(jī)械手各連桿坐標(biāo)系都賦了值，各種固定的連桿參數(shù)可以確定：對(duì)于后面是旋轉(zhuǎn)關(guān)節(jié)的連桿參數(shù)為d,a和α，對(duì)于后面是棱形關(guān)節(jié)的連桿參數(shù)為θ和α。根據(jù)這些參數(shù)，α的正弦和余弦也可以求出。這樣，對(duì)于旋轉(zhuǎn)關(guān)節(jié)，A矩陣變成了關(guān)節(jié)變量θ的函數(shù)?；蛟诶庑侮P(guān)節(jié)的情況下，變成了d的函數(shù)。一旦這些值給出，對(duì)于六個(gè)Ai變換矩陣的值就可以確定。對(duì)于棱形關(guān)節(jié)，an=0，則式（3.10根據(jù)A矩陣來(lái)確定T6

(SpecificationofT6inTermsoftheAmatrices)

機(jī)械手的坐標(biāo)變換圖如圖3.11所示，機(jī)械手的末端（即連桿坐標(biāo)系6）相對(duì)于連桿坐標(biāo)系n-1的描述用n-1T6表示，即：

n-1T6=AnAn+1???A6（3.34）0zA1A2A3A4A5A60EX0T61T62T63T64T65T6圖3.11機(jī)械手的坐標(biāo)變換圖3.10根據(jù)A矩陣來(lái)確定T6

(Specificati

機(jī)械手的末端相對(duì)于基坐標(biāo)系（用T6表示）用下式給出

T6=A1A2A3A4A5A6（3.35）如果機(jī)械手用變換矩陣Z與參考坐標(biāo)系相聯(lián)系，機(jī)械手末端執(zhí)行器用E來(lái)描述，末端執(zhí)行器的位置和方向相對(duì)參考坐標(biāo)系用X來(lái)描述，如圖3.11所示有X=ZT6E（3.36）由此可以得到T6的表達(dá)式T6=Z-1XE-1（3.37）機(jī)械手的末端相對(duì)于基坐標(biāo)系（用T6表示）用下3.11斯坦福機(jī)械手的運(yùn)動(dòng)方程

(KinematicEquationsfortheStanfordManipulator)

斯坦福機(jī)械手及其各關(guān)節(jié)坐標(biāo)的設(shè)置如圖3.12所示。將角θ的正弦和余弦簡(jiǎn)化

sinθi=Sicosθi=Cisin(θi+θj)=Sijcos(θi+θj)=Cij注：將所有關(guān)節(jié)x軸的方向設(shè)置一致，可簡(jiǎn)化坐標(biāo)變換。圖3.12斯坦福機(jī)械手坐標(biāo)系3.11斯坦福機(jī)械手的運(yùn)動(dòng)方程

(KinematicE表3.2斯坦福機(jī)械手連桿參數(shù)

LinkVariableαadcosαsinα

1θ1

-90°

000-12θ290°

0d20

13d30°0d3104θ4

-90°

000-15θ590°00016θ60°0010表3.2斯坦福機(jī)械手連桿參數(shù)斯坦福機(jī)械手的A變換如下：

C10-S10S10C10A1=0-100（3.38）0001

C20S20S20-C20A2=010d2（3.39）0001

10000100A3=001d3（3.40）0001

斯坦福機(jī)械手的A變換如下：C40-S40S40C40A4=0-100（3.41）0001

C50S50S50-C50A5=0100（3.42）0001

C6

-S600S6C600A6=0010（3.43）0001

斯坦福機(jī)械手A變換的積如下所示，這些是從連桿6開(kāi)始，然后逐步回到基坐標(biāo)。

C6

-S600S6C6005T6=0010（3.44）0001

C5C6-C5S6S50S5C6-S5S6-C50

4T6=S6C600（3.45）0001

C4C5C6-S4S6

-C4C5S6-S4C6C4S50S4C5C6+C4S6

-S4C5S6+C4C6S4S503T6=

-S5C6S5S6C50（3.46）0001斯坦福機(jī)械手A變換的積如下所示，這些是從連桿6開(kāi)始，然后逐步C4C5C6-S4S6

-C4C5S6-S4C6C4S50S4C5C6+C4S6

-S4C5S6+C4C6S4S502T6=-S5C6S5S6C5d3（3.47）0001

C2(C4C5C6-S4S6)-S2S5C6-C2(C4C5S6+S4C6)+S2S5S6

S2(C4C5C6-S4S6)+C2S5C6-S2(C4C5S6+S4C6)-C2S5S6

1T6=S4C5C6+C4C6-S4C5S6+C4C6

00

C2C4S5+S2C5S2d3S2C4S5-C2C5

-C2d3S4S5d2（3.48）01C4C5C6-nxoxaxpxnyoyaypyT6=nzozazpz（3.49）0001其中nx=C1[C2(C4C5C6-S4S6)-S2S5C6]-S1(S4C5S6+C4S6)ny=S1[C2(C4C5C6-S4S6)-S2S5C6]+C1(S4C5S6+C4S6)nz=-S2(C4C5C6-S4S6)-C2S5C6ox=C1[-C2(C4C5S6+S4C6)+S2S5C6]-S1(-S4C5S6+C4S6)oy=S1[-C2(C4C5C6+S4C6)+S2S5S6]+C1(-S4C5S6+C4S6)oz=S2(C4C5C6+S4C6)+C2S5S6ax=C1(C2C4S5+S2C5)–S1S4C5ay=S1(C2C4S5+S2C5)+C1S4S5az=–S2C4S5+C2C5px=C1S2d3–S1d2py=S1S2d3+C1d2pz=C2d3

3.12肘機(jī)械手的運(yùn)動(dòng)方程

(KineamticEquationsforanElbowManipulator)

肘機(jī)械手及其各關(guān)節(jié)坐標(biāo)的設(shè)置如圖3.13所示。z1z0z2z3z4z5、z6xa3a4a2圖3.13肘機(jī)械手的坐標(biāo)系x0y03.12肘機(jī)械手的運(yùn)動(dòng)方程

(KineamticEq

表3.3肘機(jī)械手的連桿參數(shù)

LinkVariableαadcosαsinα1θ190°

00012θ20a20

103θ30a30

104θ4

-90°

a400-15θ590°00016θ600010注：在以下的T矩陣中用變量θ23=θ2+θ3和θ234=θ23+θ4來(lái)進(jìn)行簡(jiǎn)化。

肘機(jī)械手的A變換如下：C10S10S10-C1

0A1=0100（3.50）0001

C2

-S20C2S2S2C20S2a2A2=0110（3.51）0001

C3

-S30C3a3S3C30S3a3A3=0010（3.52）0001肘機(jī)械手的A變換如下：C40-S4C4a4S40C4S4a4A4=0-100（3.53）0001

C50S50S50-C50A5=0-100（3.54）0001

C6

-S600S6C600A6=0010（3.55）0001機(jī)器人原理及控制技術(shù)第0304章運(yùn)動(dòng)學(xué)方程與逆運(yùn)動(dòng)方程課件

為了得到T6，我們從連桿6開(kāi)始來(lái)算A矩陣的積，逐步往回計(jì)算到基坐標(biāo)。

C6

-S600S6C6005T6=0010（3.55）0001C5C6-C5S6S50S5C6-S5S6-C504T6=S6C600（3.56）0001

C4C5C6-S4S6

-C4C5S6-S4C6C4S5C4a4S4C5C6+C4S6

-S4C5S6+C4C6S4S5S4a4

3T6=-S5C6S5S6C50（3.57）0001

為了得到T6，我們從連桿6開(kāi)始來(lái)算A矩陣的積，逐C34C5C6-S34S5

-C34C5S6-S34C6