第3章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)第3章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)第三章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)3.2工業(yè)機(jī)器人的動(dòng)力學(xué)3.3工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃第三章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)正向運(yùn)動(dòng)學(xué)：所有關(guān)節(jié)變量已知，可用正向運(yùn)動(dòng)學(xué)來(lái)確定機(jī)器人末端手部的位姿。逆向運(yùn)動(dòng)學(xué)：對(duì)于給定的機(jī)器人手部的位姿，可用逆向運(yùn)動(dòng)學(xué)來(lái)計(jì)算每一個(gè)關(guān)節(jié)變量的值。3.1.1工業(yè)機(jī)器人位姿描述1.點(diǎn)的位置描述如圖3.1，空間任一點(diǎn)P的位置在直角坐標(biāo)系｛A｝中可用（3ⅹ1）的位置矢量Ap表示為:其中Px、Py、Pz

是點(diǎn)P的三個(gè)位置坐標(biāo)分量。3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)3.1.1工業(yè)機(jī)器人位姿描述其中Px2.點(diǎn)的齊次坐標(biāo)上述坐標(biāo)用（4ⅹ1）列陣表示，稱為三維空間點(diǎn)P的齊次坐標(biāo)，形如齊次坐標(biāo)并不唯一，列陣每一項(xiàng)分別乘以一個(gè)非零因子ω時(shí)都表示P點(diǎn)。2.點(diǎn)的齊次坐標(biāo)齊次坐標(biāo)并不唯一，列陣每一項(xiàng)分別乘以一個(gè)非零3、坐標(biāo)軸方向的描述直角坐標(biāo)系中，可用、、用齊次坐標(biāo)來(lái)描述x、y、z軸的方向：

規(guī)定：以列陣，且a2+b2+c2=1表示某矢量的方向。中第四個(gè)元素不為零，如列陣表示x,y,z軸的單位向量則表示空間某點(diǎn)的位置。3、坐標(biāo)軸方向的描述用齊次坐標(biāo)來(lái)描述x、y、z軸的方向：如圖3.2中矢量的方向可表示為其中a=cosα，b=cosβ，c=cosγv點(diǎn)坐標(biāo)為：如圖3.2中矢量的方向可表示為其中a=cosα，b=cos4、動(dòng)坐標(biāo)系位姿的描述用位姿矩陣對(duì)動(dòng)坐標(biāo)系原點(diǎn)位置和坐標(biāo)系各軸方向進(jìn)行描述，如原始的直角坐標(biāo)系可描述為

如描述一個(gè)任意坐標(biāo)系R，則用其三個(gè)坐標(biāo)軸xR、yR、zR在原始坐標(biāo)系中表示的矢量齊次列陣，和列陣[0001]T組成。4、動(dòng)坐標(biāo)系位姿的描述如描述一個(gè)任意坐標(biāo)系R，則用其三個(gè)坐5、剛體位姿的描述機(jī)器人每一個(gè)連桿都可看做一個(gè)剛體。給定剛體上某一點(diǎn)的位置和該剛體在空中的姿態(tài)，則剛體在空間上的位姿是唯一確定的，可用唯一一個(gè)位姿矩陣進(jìn)行描述。如圖3.3剛體o’x’y’z’是固連于剛體的一個(gè)坐標(biāo)系，稱為動(dòng)坐標(biāo)系。剛體Q在固定坐標(biāo)系OXYZ中的位置的齊次坐標(biāo)形式為：5、剛體位姿的描述如圖3.3剛體o’x’y’z’是固連于剛體剛體的位姿表示為齊次矩陣：分別為x’,y’,z’坐標(biāo)軸的單位向量：剛體的位姿表示為齊次矩陣：分別為x’,y’,z’坐標(biāo)軸的單位6、手部位姿的描述如圖3.4機(jī)器人手的位姿可用固連于手的坐標(biāo)系{B}的位姿表示(3)姿態(tài)矢量：手指連線方向的矢量（4）法相矢量：(1)原點(diǎn)：手部中心點(diǎn)為原點(diǎn)OB(2)接近矢量：關(guān)節(jié)軸方向的單位向量即法向矢量同時(shí)垂直于接近矢量和姿態(tài)矢量。{B}：6、手部位姿的描述(3)姿態(tài)矢量：手指連線方向的矢量（4）手部位置矢量為從固定參考坐標(biāo)系OXYZ原點(diǎn)指向手部坐標(biāo)系{B}原點(diǎn)的矢量P，手部的位姿矩陣為：手部位置矢量為從固定參考坐標(biāo)系OXYZ原點(diǎn)指向手部坐標(biāo)系{B任何一種物體在空間的位置和姿態(tài)都可以用齊次矩陣來(lái)表示。圖3.5楔塊Q在圖a的情況可用6個(gè)點(diǎn)來(lái)描述：使Q①繞z軸旋轉(zhuǎn)90°：Rot（z，90°）②再繞y軸旋轉(zhuǎn)90°：Rot（y，90°）③再沿x軸方向平移4：Trans（4，0，0）楔塊變?yōu)閳D（b）狀態(tài)。7.目標(biāo)物位姿的描述任何一種物體在空間的位置和姿態(tài)都可以用齊次矩陣來(lái)表示。使Q剛體的平移、旋轉(zhuǎn)運(yùn)動(dòng)均可由齊次變換矩陣表示，剛體變換后的位姿可由其原始描述矩陣乘以齊次變換矩陣得到。平移的齊次變換如圖3.6，A點(diǎn)（x，y，z）平移至A’（x’，y’，z’）即即：記為其中稱為平移算子。注：①算子左乘，表示點(diǎn)的平移是相對(duì)固定坐標(biāo)系進(jìn)行坐標(biāo)變換。②算子右乘，表示點(diǎn)的平移是相對(duì)動(dòng)坐標(biāo)系進(jìn)行的坐標(biāo)變換。3.1.2齊次變換及運(yùn)算剛體的平移、旋轉(zhuǎn)運(yùn)動(dòng)均可由齊次變換矩陣表示，剛體變換后的位姿2.旋轉(zhuǎn)的齊次變換如圖3.7，A點(diǎn)繞z軸旋轉(zhuǎn)角后移至A’，即推導(dǎo)：設(shè)A點(diǎn)在xoy平面上投影的長(zhǎng)度為r，與x軸夾角為α則

即z坐標(biāo)未變，故z’=z2.旋轉(zhuǎn)的齊次變換如圖3.7，A點(diǎn)繞z軸旋轉(zhuǎn)角后移至A’，即寫成矩陣形式為記為同理：寫成矩陣形式為記為同理：(3.24)圖3.8中為任意過(guò)原點(diǎn)的單位矢量，若A點(diǎn)繞其旋轉(zhuǎn)齊次變換矩陣為Rot（k,θ）其在三個(gè)坐標(biāo)軸上分量為kx，ky，kz，且旋轉(zhuǎn)θ角，則可以證明，(3.24)圖3.8中為任意過(guò)原點(diǎn)的單位矢量，若A點(diǎn)繞其注：

①

該式為一般旋轉(zhuǎn)齊次變換通式，概括了繞X、Y、Z軸進(jìn)行旋轉(zhuǎn)變換的情況。反之,當(dāng)給出一個(gè)旋轉(zhuǎn)齊次變換矩陣,則可求得及θ角。②適用于點(diǎn)、矢量、

坐標(biāo)系、

物體的旋轉(zhuǎn)。③

左乘是相對(duì)固定坐標(biāo)系的變換；右乘是相對(duì)動(dòng)坐標(biāo)系的變換。

3、平移加旋轉(zhuǎn)的齊次變換用旋轉(zhuǎn)算子乘上平移算子即是旋轉(zhuǎn)加平移的齊次變換算子。注：②適用于點(diǎn)、矢量、坐標(biāo)系、物體的旋轉(zhuǎn)。3、平移加旋連桿長(zhǎng)度：連桿兩端關(guān)節(jié)軸線的公垂線長(zhǎng)度an。連桿扭角：連桿兩端關(guān)節(jié)軸線的夾角αn即將一條軸線沿公垂線平移至另一條軸線上的垂足時(shí)，兩條直線的夾角。3.1.3工業(yè)機(jī)器人的連桿參數(shù)和齊次變換矩陣機(jī)器人運(yùn)動(dòng)學(xué)研究的是桿件尺寸、運(yùn)動(dòng)副類型、桿件相互關(guān)系（包括位移關(guān)系、速度關(guān)系和加速度關(guān)系）等。連桿參數(shù)及連桿坐標(biāo)系建立如圖3-9某機(jī)器人手臂連桿n，兩端有關(guān)節(jié)n和n＋1。連桿長(zhǎng)度：連桿兩端關(guān)節(jié)軸線的公垂線長(zhǎng)度an。3.1.3工業(yè)機(jī)每個(gè)連桿可以由四個(gè)參數(shù)來(lái)描述：連桿長(zhǎng)度、扭角、連桿轉(zhuǎn)角、連桿距離。前兩個(gè)是連桿自身參數(shù)，后兩個(gè)表示與相鄰連桿的連接關(guān)系。旋轉(zhuǎn)關(guān)節(jié)θn改變,為關(guān)節(jié)變量，其它三個(gè)參數(shù)不變；滑動(dòng)關(guān)節(jié)dn改變,為關(guān)節(jié)變量。如圖3.10，相鄰連桿n與n-1的關(guān)系參數(shù)可由連桿轉(zhuǎn)角和連桿距離描述。沿關(guān)節(jié)n軸線兩個(gè)公垂線間的距離dn即為連桿距離。垂直于關(guān)節(jié)n軸線的平面內(nèi)兩個(gè)公垂線的夾角θn即為連桿轉(zhuǎn)角。每個(gè)連桿可以由四個(gè)參數(shù)來(lái)描述：連桿長(zhǎng)度、扭角、連桿轉(zhuǎn)角、連桿連桿坐標(biāo)系：①連桿n坐標(biāo)系的坐標(biāo)原點(diǎn)：位于n+1關(guān)節(jié)軸線上，是關(guān)節(jié)n+1的軸線與關(guān)節(jié)n軸線公垂線的垂足。②Z軸：與n+1關(guān)節(jié)軸線重合。③X軸：與公垂線重合;方向?yàn)閺膎指向n+1關(guān)節(jié)。④Y軸：由Z軸和X軸按右手螺旋法則確定。連桿坐標(biāo)系：(1)令n-1繞Zn-1軸旋轉(zhuǎn)θn角,使Xn-1與Xn平行,算子為Rot(z,θn)。(2)沿Zn-1軸平移dn,使Xn-1與Xn重合,算子為Trans(0,0,dn)。(3)沿Xn軸平移an,使兩個(gè)坐標(biāo)系原點(diǎn)重合,算子為Trans(an,0,0)。(4)繞Xn軸旋轉(zhuǎn)αn角,使得n-1系與n系重合,算子為Rot(x,αn)。2.連桿坐標(biāo)系之間的變換矩陣n-1坐標(biāo)系與n坐標(biāo)系間關(guān)系可以視為n坐標(biāo)系是由n-1坐標(biāo)系經(jīng)由一系列的平移、旋轉(zhuǎn)變化得到。(1)令n-1繞Zn-1軸旋轉(zhuǎn)θn角,使Xn-1與Xn平即：實(shí)際中，多數(shù)機(jī)器人連桿參數(shù)取特殊值，如αn=0、dn=0，計(jì)算一般簡(jiǎn)單。即：實(shí)際中，多數(shù)機(jī)器人連桿參數(shù)取特殊值，如αn=0、dn=0齊次變換矩陣Ai表示連桿i坐標(biāo)系相對(duì)于連桿坐標(biāo)系i-1的位姿變換矩陣。如A1表示連桿1相對(duì)連桿0（基座），A2矩陣表示連桿1坐標(biāo)系相對(duì)于連桿1坐標(biāo)系的位姿變換。連桿2相對(duì)固定坐標(biāo)系的位姿可用可用A2和A1的乘積表示T2=A1A2依此類推,對(duì)于六連桿機(jī)器人，有下列矩陣：T6=A1A2A3A4A5A6上述等式稱為機(jī)器人運(yùn)動(dòng)學(xué)方程。Ｔ６表示手部坐標(biāo)相對(duì)于固定參考系的位姿。3.1.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程齊次變換矩陣Ai表示連桿i坐標(biāo)系相對(duì)于連桿坐標(biāo)系i-1的位姿或前三列表示手部的姿態(tài)；或第四列表示手部中心點(diǎn)的位置。

或前三列表示手部的姿態(tài)；或第四列表示手部中心點(diǎn)的位置。正向運(yùn)動(dòng)學(xué)：已知各個(gè)關(guān)節(jié)的變量，求手部的位姿。圖3.11為SCARA裝配機(jī)器人，其三個(gè)關(guān)節(jié)軸線是相互平行的。{0}、{1}、{2}、{3}分別表示固定坐標(biāo)系、連桿1的動(dòng)坐標(biāo)系、連桿2的動(dòng)坐標(biāo)系、連桿3的動(dòng)坐標(biāo)系。原點(diǎn)分別位于關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3和手部中心。連桿運(yùn)動(dòng)為旋轉(zhuǎn)運(yùn)動(dòng),連桿參數(shù)θn為變量,其余參數(shù)均為常量。參數(shù)見(jiàn)表3-2.2.正向運(yùn)動(dòng)學(xué)及實(shí)例正向運(yùn)動(dòng)學(xué)：已知各個(gè)關(guān)節(jié)的變量，求手部的位姿。2.正向運(yùn)動(dòng)連桿轉(zhuǎn)角變量θn連桿間距dn連桿長(zhǎng)度an連桿扭角αn1θ1010002θ2010003θ30200表3-2連桿轉(zhuǎn)角變量θn連桿間距dn連桿長(zhǎng)度an連桿扭角αn1θ10該平面關(guān)節(jié)型機(jī)器人的運(yùn)動(dòng)學(xué)方程為T3=A1A2A3

A1——連桿1的坐標(biāo)系相對(duì)于固定坐標(biāo)系的齊次變換矩陣；A2——連桿2的坐標(biāo)系相對(duì)于連桿1坐標(biāo)系的齊次變換矩陣；A3——手部坐標(biāo)系相對(duì)于連桿2坐標(biāo)系的齊次變換矩陣。T3為手部坐標(biāo)系的位姿。A1，A2，A3相乘可以得到T3表達(dá)式矩陣（包括轉(zhuǎn)角變量θ1，θ2，θ3）T3=A1A2A3該平面關(guān)節(jié)型機(jī)器人的運(yùn)動(dòng)學(xué)方程為T3=A1A2A3T3為手如圖3.11（b），轉(zhuǎn)角變量分別為θ1=30°,θ2=-60°,θ3=-30°時(shí)，代入可得：如圖3.11（b），轉(zhuǎn)角變量分別為θ1=30°,θ2=-6已知手部的位姿，求出關(guān)節(jié)變量，也稱逆運(yùn)動(dòng)學(xué)。3.反向運(yùn)動(dòng)學(xué)及實(shí)例其運(yùn)動(dòng)學(xué)方程為：如圖3.12為六自由度STANFORD機(jī)器人，其連桿坐標(biāo)系圖如圖3.13。坐標(biāo)系0與坐標(biāo)系1原點(diǎn)重合。已知手部的位姿，求出關(guān)節(jié)變量，也稱逆運(yùn)動(dòng)學(xué)。3.反向運(yùn)動(dòng)學(xué)及現(xiàn)在給出T6矩陣及各桿的參數(shù)a、α、d，求關(guān)節(jié)變量θ1～θ6，其中θ3＝d3。（坐標(biāo)系3相對(duì)2的參數(shù)為平移量）其中A1為坐標(biāo)系{1}相對(duì)于固定坐標(biāo)系轉(zhuǎn)換矩陣。相當(dāng)于固定坐標(biāo)系{0}的Z0軸旋轉(zhuǎn)，然后繞自身坐標(biāo)系X1軸做-90的旋轉(zhuǎn)。如下圖。其中分為轉(zhuǎn)角θ1為0和不為0兩種情況。現(xiàn)在給出T6矩陣及各桿的參數(shù)a、α、d，求關(guān)節(jié)變量θ1～θ6列出A1的逆矩陣，有展開(kāi)方程兩邊矩陣，對(duì)比對(duì)應(yīng)項(xiàng)，可求得θ1，再利用求得θ2.同樣可順次求得θ3～θ6.列出A1的逆矩陣，有展開(kāi)方程兩邊矩陣，對(duì)比對(duì)應(yīng)項(xiàng)上述求解過(guò)程稱為分離變量法。逆解求解可能存在的問(wèn)題：解不存在和有多重解。解不存在：一般是給定的工作位置落到了工作區(qū)域之外時(shí)，則解不存在。有多重解時(shí)：1由于實(shí)際關(guān)節(jié)活動(dòng)范圍的限制，機(jī)器人有多組解時(shí)，可能有某些解不能達(dá)到。2非零的連桿參數(shù)越多，達(dá)到某一目標(biāo)的方式越多，運(yùn)動(dòng)學(xué)逆解的數(shù)目越多。3在避免碰撞的前提下，按“最短路程”的原則來(lái)?yè)駜?yōu)。根據(jù)連桿的尺寸大小不同，應(yīng)遵循“多移動(dòng)小關(guān)節(jié)，少移動(dòng)大關(guān)節(jié)”的原則。上述求解過(guò)程稱為分離變量法。有多重解時(shí)：3.2.1工業(yè)機(jī)器人的動(dòng)力學(xué)分析(1)工業(yè)機(jī)器人速度雅可比矩陣雅可比矩陣是一個(gè)多元函數(shù)的偏導(dǎo)矩陣，機(jī)器人的速度分析和靜力學(xué)分析常遇到雅克比矩陣。以圖3.14二自由度機(jī)器人為例。機(jī)器人為手部坐標(biāo)(x,y)相對(duì)于關(guān)節(jié)變量(θ1,θ2)有3.2工業(yè)機(jī)器人的動(dòng)力學(xué)即3.2.1工業(yè)機(jī)器人的動(dòng)力學(xué)分析3.2工業(yè)機(jī)器人的動(dòng)力學(xué)即求微分:寫成矩陣為令則其中運(yùn)算得：其中s1表示sinθ1，c1表示cosθ1，s12表示sin(θ1+θ2)，c12表示cos(θ1+θ2)偏導(dǎo)數(shù)矩陣J即為速度雅可比矩陣求微分:寫成矩陣為令則其中運(yùn)算得：其中s1表示sinθ1，c對(duì)于n自由度機(jī)器人，關(guān)節(jié)變量q=［q1q2…qn］T，當(dāng)關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)時(shí)，qi=θi；當(dāng)關(guān)節(jié)為移動(dòng)關(guān)節(jié)時(shí)，qi=di，則dq=[dq1dq2…dqn]T反映關(guān)節(jié)空間的微小運(yùn)動(dòng)。由X=X(q)可知，dX=J(q)dq其中J(q)是(6×n)的偏導(dǎo)數(shù)矩陣，稱為n自由度機(jī)器人速度雅可比矩陣。

因?yàn)楸硎镜氖?個(gè)自由度上的速度，所以是6列矩陣對(duì)于n自由度機(jī)器人，關(guān)節(jié)變量q=［q1q2…qn］T，當(dāng)關(guān)(2)工業(yè)機(jī)器人速度分析即:

其中：V——機(jī)器人末端在操作空間中的廣義速度，;J(q)——速度雅可比矩陣;——機(jī)器人關(guān)節(jié)在關(guān)節(jié)空間中的關(guān)節(jié)速度。

把上式兩邊各除以dt，得二自由度手部速度為(2)工業(yè)機(jī)器人速度分析即:其中：J(q)——速度若已知關(guān)節(jié)上與是時(shí)間的函數(shù),則可求出該機(jī)器人手部在某一時(shí)刻的速度V=f(t),即手部瞬時(shí)速度。反之,給定機(jī)器人手部速度,可由V=J(q)q解出相應(yīng)的關(guān)節(jié)速度，式中為機(jī)器人逆速度雅可比矩陣。逆速度雅可比J-1出現(xiàn)奇異解的情況如下:①工作域邊界上的奇異:機(jī)器人手臂全部伸開(kāi)或全部折回時(shí),叫奇異形位。該位置產(chǎn)生的解稱為工作域邊界上的奇異。②工作域內(nèi)部奇異:機(jī)器人兩個(gè)或多個(gè)關(guān)節(jié)軸線重合引起的奇異。當(dāng)出現(xiàn)奇異形位時(shí),會(huì)產(chǎn)生退化現(xiàn)象,即在某空間某個(gè)方向(或子域)上,不管機(jī)器人關(guān)節(jié)速度怎樣選擇,手部也不可能動(dòng)。若已知關(guān)節(jié)上與是時(shí)間的函數(shù),則可求出該機(jī)器人手部在各關(guān)節(jié)的驅(qū)動(dòng)力矩（或力）與末端操作器施加的力（廣義力，包括力和力矩）之間的關(guān)系就是機(jī)器人操作臂力控制的基礎(chǔ)。靜力平衡：假定各關(guān)節(jié)“鎖定”，機(jī)器人成為一個(gè)機(jī)構(gòu)。該“鎖定”用的關(guān)節(jié)力與手部所支持的載荷或受到外界環(huán)境作用力取得靜力平衡。求解這種“鎖定用”的關(guān)節(jié)力，或求解在已知驅(qū)動(dòng)力矩作用下手部的輸出力就是對(duì)機(jī)器人操作臂的靜力計(jì)算。3.2.2.工業(yè)機(jī)器人靜力學(xué)分析各關(guān)節(jié)的驅(qū)動(dòng)力矩（或力）與末端操作器施加的力（廣義力，包括力圖3.15單個(gè)桿件受力分析靜力平衡條件：桿上所受合力、合力矩為零。手部端點(diǎn)的力和力矩可寫成一個(gè)6維矢量1.操作臂的靜力已知末桿受力（力矩），可先分析末桿對(duì)上一連桿的力和力矩，依次分析反推，直至分析第一連桿對(duì)基座力和力矩，從而計(jì)算全部連桿的受力情況。和各有關(guān)于軸的三個(gè)分量。圖3.15單個(gè)桿件受力分析1.操作臂的靜力已知末桿受力（力矩各關(guān)節(jié)驅(qū)動(dòng)力矩、力可寫成n維矢量的形式。即2.機(jī)器人力雅克比矩陣忽略各關(guān)節(jié)摩擦力和桿件重力有-廣義關(guān)節(jié)力矩；F-機(jī)器人手部端點(diǎn)力；—（n×6）階雅克比矩陣。機(jī)器人力雅克比正好是速度雅克比的轉(zhuǎn)置。體現(xiàn)了手部端點(diǎn)受力和各關(guān)節(jié)驅(qū)動(dòng)力矩的關(guān)系。各關(guān)節(jié)驅(qū)動(dòng)力矩、力可寫成n維矢量的形式。2.機(jī)器人力雅克比3、

機(jī)器人靜力計(jì)算的兩類問(wèn)題①

已知外界對(duì)手部作用力F′，求滿足靜力平衡條件的關(guān)節(jié)驅(qū)動(dòng)力矩τ()。②

已知關(guān)節(jié)驅(qū)動(dòng)力矩τ,確定機(jī)器人手部對(duì)外界環(huán)境的作用力F或負(fù)荷質(zhì)量。當(dāng)自由度n>6時(shí)，力雅可比可能不是方陣，JT沒(méi)有逆解，一般情況下不一定能得到惟一的解。

3、機(jī)器人靜力計(jì)算的兩類問(wèn)題②已知關(guān)節(jié)驅(qū)動(dòng)力矩τ,確定3.2.3.工業(yè)機(jī)器人動(dòng)力學(xué)分析1.動(dòng)力學(xué)分析的兩類問(wèn)題①給出已知的軌跡點(diǎn)的關(guān)節(jié)變量即機(jī)器人的關(guān)節(jié)位置、速度和加速度，求相應(yīng)的關(guān)節(jié)力矩向量τ，用以實(shí)現(xiàn)對(duì)機(jī)器人的動(dòng)態(tài)控制。②已知關(guān)節(jié)驅(qū)動(dòng)力矩，求機(jī)器人系統(tǒng)的相應(yīng)的各瞬時(shí)的運(yùn)動(dòng),用于模擬機(jī)器人運(yùn)動(dòng)。動(dòng)力學(xué)分析方法:有拉格朗日方法、牛頓-歐拉方法、高斯方法、凱恩方法等。其中,拉格朗日方法不僅求解復(fù)雜的系統(tǒng)動(dòng)力學(xué)方程簡(jiǎn)單,而且容易理解。3.2.3.工業(yè)機(jī)器人動(dòng)力學(xué)分析①給出已知的軌跡點(diǎn)的關(guān)2.拉格朗日方程定義拉格朗日函數(shù)EK-機(jī)械系統(tǒng)動(dòng)能，EP-勢(shì)能的函數(shù)，勢(shì)能EP是的函數(shù)。的函數(shù)。動(dòng)能EK是關(guān)節(jié)變量和因此L是和拉格朗日方程為：Fi-關(guān)節(jié)廣義驅(qū)動(dòng)力（移動(dòng)關(guān)節(jié)為驅(qū)動(dòng)力，轉(zhuǎn)動(dòng)關(guān)節(jié)為驅(qū)動(dòng)力矩）。i=1.2.3……n2.拉格朗日方程EK-機(jī)械系統(tǒng)動(dòng)能，EP-勢(shì)能的函數(shù)，勢(shì)能E建立動(dòng)力學(xué)方程步驟：1)選取坐標(biāo)系，選定獨(dú)立的廣義關(guān)節(jié)變量(i=1.2.....n)；2)選定相應(yīng)的廣義力Fi；3)求各構(gòu)件的動(dòng)能與勢(shì)能，構(gòu)造拉格朗日函數(shù)；4)代入拉格朗日方程，求得機(jī)器人的動(dòng)力學(xué)方程。建立動(dòng)力學(xué)方程步驟：關(guān)節(jié)空間：n個(gè)自由度操作臂末端位姿x是由n個(gè)關(guān)節(jié)變量決定的，這n個(gè)關(guān)節(jié)變量叫n維關(guān)節(jié)矢量q，q所構(gòu)成的空間稱關(guān)節(jié)空間。操作空間：末端操作器的位姿是在直角坐標(biāo)系空間中描述的，這個(gè)空間叫操作空間。3.關(guān)節(jié)空間和操作空間動(dòng)力學(xué)針對(duì)圖3.17二自由度機(jī)器人，關(guān)節(jié)空間動(dòng)力學(xué)方程為：關(guān)節(jié)空間：n個(gè)自由度操作臂末端位姿x是由n個(gè)關(guān)節(jié)變量決定的，是（n×1）離心力和哥氏力矢量：是（n×1）的重力矢量：是（n×1）離心力和哥氏力矢量：是（n×1）的重力矢量：在笛卡爾操作空間中，可用末端操作器的位姿矢量來(lái)表示機(jī)器人的動(dòng)力學(xué)方程：其中：——為操作空間的慣性矩陣；——離心力和哥氏力矢量；F——廣義操作力矢量?！亓κ噶?；兩個(gè)空間之間的關(guān)系：在笛卡爾操作空間中，可用末端操作器的位姿矢量來(lái)表示機(jī)器人的動(dòng)3.3.1路徑和軌跡機(jī)器人的軌跡：指操作臂在運(yùn)動(dòng)過(guò)程中的位移、速度和加速度。路徑：是機(jī)器人位姿的一定序列，而不考慮機(jī)器人位姿參數(shù)隨時(shí)間變化的因素。見(jiàn)圖3.183.3工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃3.3.1路徑和軌跡3.3工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃①運(yùn)動(dòng)軌跡的描述—對(duì)機(jī)器人的任務(wù)，及運(yùn)動(dòng)軌跡的描述；②根據(jù)已經(jīng)確定的軌跡參數(shù),在計(jì)算機(jī)上模擬所要求的軌跡。③對(duì)軌跡進(jìn)行實(shí)際計(jì)算，即在運(yùn)行時(shí)間內(nèi)按一定的速率計(jì)算出位置、速度和加速度，從而生成運(yùn)動(dòng)軌跡。是指根據(jù)作業(yè)任務(wù)要求確定軌跡參數(shù)，并實(shí)時(shí)計(jì)算和生成運(yùn)動(dòng)軌跡三個(gè)軌跡規(guī)劃的一般問(wèn)題3.3.2軌跡規(guī)劃規(guī)劃中，要規(guī)定機(jī)器人運(yùn)動(dòng)起始點(diǎn)和終止點(diǎn)，而且要給出中間點(diǎn)（路徑點(diǎn)）的位姿及路徑點(diǎn)之間的時(shí)間分配，即給出兩個(gè)路徑點(diǎn)之間的運(yùn)動(dòng)時(shí)間。①運(yùn)動(dòng)軌跡的描述—對(duì)機(jī)器人的任務(wù)，及運(yùn)動(dòng)軌跡的描述；是指軌跡規(guī)劃：在關(guān)節(jié)空間中：將所有關(guān)節(jié)變量表示為時(shí)間函數(shù)。用其一、二階導(dǎo)數(shù)描述機(jī)器人的預(yù)期動(dòng)作。在直角坐標(biāo)空間中：將手部位姿參數(shù)表示為時(shí)間函數(shù)，相應(yīng)的關(guān)節(jié)位置，速度，加速度由手部信息導(dǎo)出。以二自由度平面關(guān)節(jié)機(jī)器人為例，解釋軌跡規(guī)劃基本原理。軌跡規(guī)劃：以二自由度平面關(guān)節(jié)機(jī)器人為例，解釋軌跡規(guī)劃基本原理A點(diǎn)：；B點(diǎn)：圖3.19中，兩桿均以最大速率運(yùn)動(dòng)圖3.20中，兩個(gè)關(guān)節(jié)運(yùn)動(dòng)用公共因子做歸一化處理，速率分別為和運(yùn)動(dòng)均勻。下桿2s到達(dá)，上桿3秒到達(dá)，路徑不均勻。A點(diǎn)：；B點(diǎn)：圖3.19中，兩桿均以最大速率圖3.21中，手部沿AB直線運(yùn)動(dòng)，可用插值法；將直線分為n份，逐點(diǎn)計(jì)算出相應(yīng)的角。顯然運(yùn)動(dòng)精度與點(diǎn)數(shù)有關(guān)，屬于直角坐標(biāo)空間的規(guī)劃。圖3.21中，手部沿AB直線運(yùn)動(dòng)，可用插值法；將直線分為n份3.3.3關(guān)節(jié)空間的軌跡規(guī)劃利用受控參數(shù)在關(guān)節(jié)空間中對(duì)機(jī)器人的運(yùn)動(dòng)進(jìn)行軌跡規(guī)劃有許多方法，多次多項(xiàng)式函數(shù)和拋物線過(guò)渡線性函數(shù)法是常用的兩種。三次多項(xiàng)式軌跡規(guī)劃已知機(jī)器人初始位姿，通過(guò)求解逆運(yùn)動(dòng)學(xué)方程，可以求得對(duì)應(yīng)的關(guān)節(jié)變量。若關(guān)節(jié)在ti時(shí)刻變量值為θi，tf時(shí)刻，關(guān)節(jié)變量為θf(wàn)，起、終速度均為0，以上可構(gòu)成四個(gè)邊界條件以求解三次多項(xiàng)式方程中的四個(gè)未知量：一階導(dǎo)數(shù)為：3.3.3關(guān)節(jié)空間的軌跡規(guī)劃一階導(dǎo)數(shù)為：已知條件為：分別代入上兩式得：求得的三次多項(xiàng)式方程可作為ti-tf軌跡規(guī)劃。軌跡通過(guò)一系列點(diǎn)時(shí)，每一段末端位、速可作為下一段初始條件，形成多段三次軌跡。這種軌跡速度連續(xù)。如還要求加速度連續(xù)，方程采用五次多項(xiàng)式，邊界條件增至六個(gè)。已知條件為：分別代入上兩式得：求得的三次多項(xiàng)式方程可作為t2.拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡圖3.22表示，機(jī)器人關(guān)節(jié)以恒定速度完成起、終點(diǎn)之間的運(yùn)動(dòng)，軌跡方程為一次多項(xiàng)式，但在起點(diǎn)、終點(diǎn)處須有速度過(guò)渡的規(guī)劃，以產(chǎn)生連續(xù)的速度切換?？捎脪佄锞€實(shí)現(xiàn)過(guò)渡。邊界條件同前，A、B為拋物線與直線過(guò)渡點(diǎn)，拋物線與直線部分的過(guò)渡段對(duì)稱。2.拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡邊界條件同前，A、B為拋物線得拋物線方程:可見(jiàn)：拋物線運(yùn)動(dòng)段加速度為常數(shù)。將邊界條件，代入得：即：則方程簡(jiǎn)化為：得拋物線方程:可見(jiàn)：拋物線運(yùn)動(dòng)段加速度為常數(shù)。將邊界條件設(shè)線性段常量速度為ω，初速、末速均為0，代入上式得：可得：把c2代入得：設(shè)線性段常量速度為ω，初速、末速均為0，代入上式得：可得：把進(jìn)而求出過(guò)渡時(shí)間：由即可得即以上求得起點(diǎn)過(guò)渡曲線。此時(shí)，無(wú)直線段。進(jìn)而求出過(guò)渡時(shí)間：由即可得即以上求得起點(diǎn)過(guò)渡曲線。此如初始時(shí)間不為零，可平移時(shí)間軸使初始時(shí)間為零。終點(diǎn)拋物線和起點(diǎn)拋物線對(duì)稱，但加速方向相反，即：，可得到如初始時(shí)間不為零，可平移時(shí)間軸使初始時(shí)間為零。，可得到作業(yè)：1-9作業(yè)：1-9第3章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)第3章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)第三章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)3.2工業(yè)機(jī)器人的動(dòng)力學(xué)3.3工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃第三章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)正向運(yùn)動(dòng)學(xué)：所有關(guān)節(jié)變量已知，可用正向運(yùn)動(dòng)學(xué)來(lái)確定機(jī)器人末端手部的位姿。逆向運(yùn)動(dòng)學(xué)：對(duì)于給定的機(jī)器人手部的位姿，可用逆向運(yùn)動(dòng)學(xué)來(lái)計(jì)算每一個(gè)關(guān)節(jié)變量的值。3.1.1工業(yè)機(jī)器人位姿描述1.點(diǎn)的位置描述如圖3.1，空間任一點(diǎn)P的位置在直角坐標(biāo)系｛A｝中可用（3ⅹ1）的位置矢量Ap表示為:其中Px、Py、Pz

是點(diǎn)P的三個(gè)位置坐標(biāo)分量。3.1工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)3.1.1工業(yè)機(jī)器人位姿描述其中Px2.點(diǎn)的齊次坐標(biāo)上述坐標(biāo)用（4ⅹ1）列陣表示，稱為三維空間點(diǎn)P的齊次坐標(biāo)，形如齊次坐標(biāo)并不唯一，列陣每一項(xiàng)分別乘以一個(gè)非零因子ω時(shí)都表示P點(diǎn)。2.點(diǎn)的齊次坐標(biāo)齊次坐標(biāo)并不唯一，列陣每一項(xiàng)分別乘以一個(gè)非零3、坐標(biāo)軸方向的描述直角坐標(biāo)系中，可用、、用齊次坐標(biāo)來(lái)描述x、y、z軸的方向：

規(guī)定：以列陣，且a2+b2+c2=1表示某矢量的方向。中第四個(gè)元素不為零，如列陣表示x,y,z軸的單位向量則表示空間某點(diǎn)的位置。3、坐標(biāo)軸方向的描述用齊次坐標(biāo)來(lái)描述x、y、z軸的方向：如圖3.2中矢量的方向可表示為其中a=cosα，b=cosβ，c=cosγv點(diǎn)坐標(biāo)為：如圖3.2中矢量的方向可表示為其中a=cosα，b=cos4、動(dòng)坐標(biāo)系位姿的描述用位姿矩陣對(duì)動(dòng)坐標(biāo)系原點(diǎn)位置和坐標(biāo)系各軸方向進(jìn)行描述，如原始的直角坐標(biāo)系可描述為

如描述一個(gè)任意坐標(biāo)系R，則用其三個(gè)坐標(biāo)軸xR、yR、zR在原始坐標(biāo)系中表示的矢量齊次列陣，和列陣[0001]T組成。4、動(dòng)坐標(biāo)系位姿的描述如描述一個(gè)任意坐標(biāo)系R，則用其三個(gè)坐5、剛體位姿的描述機(jī)器人每一個(gè)連桿都可看做一個(gè)剛體。給定剛體上某一點(diǎn)的位置和該剛體在空中的姿態(tài)，則剛體在空間上的位姿是唯一確定的，可用唯一一個(gè)位姿矩陣進(jìn)行描述。如圖3.3剛體o’x’y’z’是固連于剛體的一個(gè)坐標(biāo)系，稱為動(dòng)坐標(biāo)系。剛體Q在固定坐標(biāo)系OXYZ中的位置的齊次坐標(biāo)形式為：5、剛體位姿的描述如圖3.3剛體o’x’y’z’是固連于剛體剛體的位姿表示為齊次矩陣：分別為x’,y’,z’坐標(biāo)軸的單位向量：剛體的位姿表示為齊次矩陣：分別為x’,y’,z’坐標(biāo)軸的單位6、手部位姿的描述如圖3.4機(jī)器人手的位姿可用固連于手的坐標(biāo)系{B}的位姿表示(3)姿態(tài)矢量：手指連線方向的矢量（4）法相矢量：(1)原點(diǎn)：手部中心點(diǎn)為原點(diǎn)OB(2)接近矢量：關(guān)節(jié)軸方向的單位向量即法向矢量同時(shí)垂直于接近矢量和姿態(tài)矢量。{B}：6、手部位姿的描述(3)姿態(tài)矢量：手指連線方向的矢量（4）手部位置矢量為從固定參考坐標(biāo)系OXYZ原點(diǎn)指向手部坐標(biāo)系{B}原點(diǎn)的矢量P，手部的位姿矩陣為：手部位置矢量為從固定參考坐標(biāo)系OXYZ原點(diǎn)指向手部坐標(biāo)系{B任何一種物體在空間的位置和姿態(tài)都可以用齊次矩陣來(lái)表示。圖3.5楔塊Q在圖a的情況可用6個(gè)點(diǎn)來(lái)描述：使Q①繞z軸旋轉(zhuǎn)90°：Rot（z，90°）②再繞y軸旋轉(zhuǎn)90°：Rot（y，90°）③再沿x軸方向平移4：Trans（4，0，0）楔塊變?yōu)閳D（b）狀態(tài)。7.目標(biāo)物位姿的描述任何一種物體在空間的位置和姿態(tài)都可以用齊次矩陣來(lái)表示。使Q剛體的平移、旋轉(zhuǎn)運(yùn)動(dòng)均可由齊次變換矩陣表示，剛體變換后的位姿可由其原始描述矩陣乘以齊次變換矩陣得到。平移的齊次變換如圖3.6，A點(diǎn)（x，y，z）平移至A’（x’，y’，z’）即即：記為其中稱為平移算子。注：①算子左乘，表示點(diǎn)的平移是相對(duì)固定坐標(biāo)系進(jìn)行坐標(biāo)變換。②算子右乘，表示點(diǎn)的平移是相對(duì)動(dòng)坐標(biāo)系進(jìn)行的坐標(biāo)變換。3.1.2齊次變換及運(yùn)算剛體的平移、旋轉(zhuǎn)運(yùn)動(dòng)均可由齊次變換矩陣表示，剛體變換后的位姿2.旋轉(zhuǎn)的齊次變換如圖3.7，A點(diǎn)繞z軸旋轉(zhuǎn)角后移至A’，即推導(dǎo)：設(shè)A點(diǎn)在xoy平面上投影的長(zhǎng)度為r，與x軸夾角為α則

即z坐標(biāo)未變，故z’=z2.旋轉(zhuǎn)的齊次變換如圖3.7，A點(diǎn)繞z軸旋轉(zhuǎn)角后移至A’，即寫成矩陣形式為記為同理：寫成矩陣形式為記為同理：(3.24)圖3.8中為任意過(guò)原點(diǎn)的單位矢量，若A點(diǎn)繞其旋轉(zhuǎn)齊次變換矩陣為Rot（k,θ）其在三個(gè)坐標(biāo)軸上分量為kx，ky，kz，且旋轉(zhuǎn)θ角，則可以證明，(3.24)圖3.8中為任意過(guò)原點(diǎn)的單位矢量，若A點(diǎn)繞其注：

①

該式為一般旋轉(zhuǎn)齊次變換通式，概括了繞X、Y、Z軸進(jìn)行旋轉(zhuǎn)變換的情況。反之,當(dāng)給出一個(gè)旋轉(zhuǎn)齊次變換矩陣,則可求得及θ角。②適用于點(diǎn)、矢量、

坐標(biāo)系、

物體的旋轉(zhuǎn)。③

左乘是相對(duì)固定坐標(biāo)系的變換；右乘是相對(duì)動(dòng)坐標(biāo)系的變換。

3、平移加旋轉(zhuǎn)的齊次變換用旋轉(zhuǎn)算子乘上平移算子即是旋轉(zhuǎn)加平移的齊次變換算子。注：②適用于點(diǎn)、矢量、坐標(biāo)系、物體的旋轉(zhuǎn)。3、平移加旋連桿長(zhǎng)度：連桿兩端關(guān)節(jié)軸線的公垂線長(zhǎng)度an。連桿扭角：連桿兩端關(guān)節(jié)軸線的夾角αn即將一條軸線沿公垂線平移至另一條軸線上的垂足時(shí)，兩條直線的夾角。3.1.3工業(yè)機(jī)器人的連桿參數(shù)和齊次變換矩陣機(jī)器人運(yùn)動(dòng)學(xué)研究的是桿件尺寸、運(yùn)動(dòng)副類型、桿件相互關(guān)系（包括位移關(guān)系、速度關(guān)系和加速度關(guān)系）等。連桿參數(shù)及連桿坐標(biāo)系建立如圖3-9某機(jī)器人手臂連桿n，兩端有關(guān)節(jié)n和n＋1。連桿長(zhǎng)度：連桿兩端關(guān)節(jié)軸線的公垂線長(zhǎng)度an。3.1.3工業(yè)機(jī)每個(gè)連桿可以由四個(gè)參數(shù)來(lái)描述：連桿長(zhǎng)度、扭角、連桿轉(zhuǎn)角、連桿距離。前兩個(gè)是連桿自身參數(shù)，后兩個(gè)表示與相鄰連桿的連接關(guān)系。旋轉(zhuǎn)關(guān)節(jié)θn改變,為關(guān)節(jié)變量，其它三個(gè)參數(shù)不變；滑動(dòng)關(guān)節(jié)dn改變,為關(guān)節(jié)變量。如圖3.10，相鄰連桿n與n-1的關(guān)系參數(shù)可由連桿轉(zhuǎn)角和連桿距離描述。沿關(guān)節(jié)n軸線兩個(gè)公垂線間的距離dn即為連桿距離。垂直于關(guān)節(jié)n軸線的平面內(nèi)兩個(gè)公垂線的夾角θn即為連桿轉(zhuǎn)角。每個(gè)連桿可以由四個(gè)參數(shù)來(lái)描述：連桿長(zhǎng)度、扭角、連桿轉(zhuǎn)角、連桿連桿坐標(biāo)系：①連桿n坐標(biāo)系的坐標(biāo)原點(diǎn)：位于n+1關(guān)節(jié)軸線上，是關(guān)節(jié)n+1的軸線與關(guān)節(jié)n軸線公垂線的垂足。②Z軸：與n+1關(guān)節(jié)軸線重合。③X軸：與公垂線重合;方向?yàn)閺膎指向n+1關(guān)節(jié)。④Y軸：由Z軸和X軸按右手螺旋法則確定。連桿坐標(biāo)系：(1)令n-1繞Zn-1軸旋轉(zhuǎn)θn角,使Xn-1與Xn平行,算子為Rot(z,θn)。(2)沿Zn-1軸平移dn,使Xn-1與Xn重合,算子為Trans(0,0,dn)。(3)沿Xn軸平移an,使兩個(gè)坐標(biāo)系原點(diǎn)重合,算子為Trans(an,0,0)。(4)繞Xn軸旋轉(zhuǎn)αn角,使得n-1系與n系重合,算子為Rot(x,αn)。2.連桿坐標(biāo)系之間的變換矩陣n-1坐標(biāo)系與n坐標(biāo)系間關(guān)系可以視為n坐標(biāo)系是由n-1坐標(biāo)系經(jīng)由一系列的平移、旋轉(zhuǎn)變化得到。(1)令n-1繞Zn-1軸旋轉(zhuǎn)θn角,使Xn-1與Xn平即：實(shí)際中，多數(shù)機(jī)器人連桿參數(shù)取特殊值，如αn=0、dn=0，計(jì)算一般簡(jiǎn)單。即：實(shí)際中，多數(shù)機(jī)器人連桿參數(shù)取特殊值，如αn=0、dn=0齊次變換矩陣Ai表示連桿i坐標(biāo)系相對(duì)于連桿坐標(biāo)系i-1的位姿變換矩陣。如A1表示連桿1相對(duì)連桿0（基座），A2矩陣表示連桿1坐標(biāo)系相對(duì)于連桿1坐標(biāo)系的位姿變換。連桿2相對(duì)固定坐標(biāo)系的位姿可用可用A2和A1的乘積表示T2=A1A2依此類推,對(duì)于六連桿機(jī)器人，有下列矩陣：T6=A1A2A3A4A5A6上述等式稱為機(jī)器人運(yùn)動(dòng)學(xué)方程。Ｔ６表示手部坐標(biāo)相對(duì)于固定參考系的位姿。3.1.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程齊次變換矩陣Ai表示連桿i坐標(biāo)系相對(duì)于連桿坐標(biāo)系i-1的位姿或前三列表示手部的姿態(tài)；或第四列表示手部中心點(diǎn)的位置。

或前三列表示手部的姿態(tài)；或第四列表示手部中心點(diǎn)的位置。正向運(yùn)動(dòng)學(xué)：已知各個(gè)關(guān)節(jié)的變量，求手部的位姿。圖3.11為SCARA裝配機(jī)器人，其三個(gè)關(guān)節(jié)軸線是相互平行的。{0}、{1}、{2}、{3}分別表示固定坐標(biāo)系、連桿1的動(dòng)坐標(biāo)系、連桿2的動(dòng)坐標(biāo)系、連桿3的動(dòng)坐標(biāo)系。原點(diǎn)分別位于關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3和手部中心。連桿運(yùn)動(dòng)為旋轉(zhuǎn)運(yùn)動(dòng),連桿參數(shù)θn為變量,其余參數(shù)均為常量。參數(shù)見(jiàn)表3-2.2.正向運(yùn)動(dòng)學(xué)及實(shí)例正向運(yùn)動(dòng)學(xué)：已知各個(gè)關(guān)節(jié)的變量，求手部的位姿。2.正向運(yùn)動(dòng)連桿轉(zhuǎn)角變量θn連桿間距dn連桿長(zhǎng)度an連桿扭角αn1θ1010002θ2010003θ30200表3-2連桿轉(zhuǎn)角變量θn連桿間距dn連桿長(zhǎng)度an連桿扭角αn1θ10該平面關(guān)節(jié)型機(jī)器人的運(yùn)動(dòng)學(xué)方程為T3=A1A2A3

A1——連桿1的坐標(biāo)系相對(duì)于固定坐標(biāo)系的齊次變換矩陣；A2——連桿2的坐標(biāo)系相對(duì)于連桿1坐標(biāo)系的齊次變換矩陣；A3——手部坐標(biāo)系相對(duì)于連桿2坐標(biāo)系的齊次變換矩陣。T3為手部坐標(biāo)系的位姿。A1，A2，A3相乘可以得到T3表達(dá)式矩陣（包括轉(zhuǎn)角變量θ1，θ2，θ3）T3=A1A2A3該平面關(guān)節(jié)型機(jī)器人的運(yùn)動(dòng)學(xué)方程為T3=A1A2A3T3為手如圖3.11（b），轉(zhuǎn)角變量分別為θ1=30°,θ2=-60°,θ3=-30°時(shí)，代入可得：如圖3.11（b），轉(zhuǎn)角變量分別為θ1=30°,θ2=-6已知手部的位姿，求出關(guān)節(jié)變量，也稱逆運(yùn)動(dòng)學(xué)。3.反向運(yùn)動(dòng)學(xué)及實(shí)例其運(yùn)動(dòng)學(xué)方程為：如圖3.12為六自由度STANFORD機(jī)器人，其連桿坐標(biāo)系圖如圖3.13。坐標(biāo)系0與坐標(biāo)系1原點(diǎn)重合。已知手部的位姿，求出關(guān)節(jié)變量，也稱逆運(yùn)動(dòng)學(xué)。3.反向運(yùn)動(dòng)學(xué)及現(xiàn)在給出T6矩陣及各桿的參數(shù)a、α、d，求關(guān)節(jié)變量θ1～θ6，其中θ3＝d3。（坐標(biāo)系3相對(duì)2的參數(shù)為平移量）其中A1為坐標(biāo)系{1}相對(duì)于固定坐標(biāo)系轉(zhuǎn)換矩陣。相當(dāng)于固定坐標(biāo)系{0}的Z0軸旋轉(zhuǎn)，然后繞自身坐標(biāo)系X1軸做-90的旋轉(zhuǎn)。如下圖。其中分為轉(zhuǎn)角θ1為0和不為0兩種情況?，F(xiàn)在給出T6矩陣及各桿的參數(shù)a、α、d，求關(guān)節(jié)變量θ1～θ6列出A1的逆矩陣，有展開(kāi)方程兩邊矩陣，對(duì)比對(duì)應(yīng)項(xiàng)，可求得θ1，再利用求得θ2.同樣可順次求得θ3～θ6.列出A1的逆矩陣，有展開(kāi)方程兩邊矩陣，對(duì)比對(duì)應(yīng)項(xiàng)上述求解過(guò)程稱為分離變量法。逆解求解可能存在的問(wèn)題：解不存在和有多重解。解不存在：一般是給定的工作位置落到了工作區(qū)域之外時(shí)，則解不存在。有多重解時(shí)：1由于實(shí)際關(guān)節(jié)活動(dòng)范圍的限制，機(jī)器人有多組解時(shí)，可能有某些解不能達(dá)到。2非零的連桿參數(shù)越多，達(dá)到某一目標(biāo)的方式越多，運(yùn)動(dòng)學(xué)逆解的數(shù)目越多。3在避免碰撞的前提下，按“最短路程”的原則來(lái)?yè)駜?yōu)。根據(jù)連桿的尺寸大小不同，應(yīng)遵循“多移動(dòng)小關(guān)節(jié)，少移動(dòng)大關(guān)節(jié)”的原則。上述求解過(guò)程稱為分離變量法。有多重解時(shí)：3.2.1工業(yè)機(jī)器人的動(dòng)力學(xué)分析(1)工業(yè)機(jī)器人速度雅可比矩陣雅可比矩陣是一個(gè)多元函數(shù)的偏導(dǎo)矩陣，機(jī)器人的速度分析和靜力學(xué)分析常遇到雅克比矩陣。以圖3.14二自由度機(jī)器人為例。機(jī)器人為手部坐標(biāo)(x,y)相對(duì)于關(guān)節(jié)變量(θ1,θ2)有3.2工業(yè)機(jī)器人的動(dòng)力學(xué)即3.2.1工業(yè)機(jī)器人的動(dòng)力學(xué)分析3.2工業(yè)機(jī)器人的動(dòng)力學(xué)即求微分:寫成矩陣為令則其中運(yùn)算得：其中s1表示sinθ1，c1表示cosθ1，s12表示sin(θ1+θ2)，c12表示cos(θ1+θ2)偏導(dǎo)數(shù)矩陣J即為速度雅可比矩陣求微分:寫成矩陣為令則其中運(yùn)算得：其中s1表示sinθ1，c對(duì)于n自由度機(jī)器人，關(guān)節(jié)變量q=［q1q2…qn］T，當(dāng)關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)時(shí)，qi=θi；當(dāng)關(guān)節(jié)為移動(dòng)關(guān)節(jié)時(shí)，qi=di，則dq=[dq1dq2…dqn]T反映關(guān)節(jié)空間的微小運(yùn)動(dòng)。由X=X(q)可知，dX=J(q)dq其中J(q)是(6×n)的偏導(dǎo)數(shù)矩陣，稱為n自由度機(jī)器人速度雅可比矩陣。

因?yàn)楸硎镜氖?個(gè)自由度上的速度，所以是6列矩陣對(duì)于n自由度機(jī)器人，關(guān)節(jié)變量q=［q1q2…qn］T，當(dāng)關(guān)(2)工業(yè)機(jī)器人速度分析即:

其中：V——機(jī)器人末端在操作空間中的廣義速度，;J(q)——速度雅可比矩陣;——機(jī)器人關(guān)節(jié)在關(guān)節(jié)空間中的關(guān)節(jié)速度。

把上式兩邊各除以dt，得二自由度手部速度為(2)工業(yè)機(jī)器人速度分析即:其中：J(q)——速度若已知關(guān)節(jié)上與是時(shí)間的函數(shù),則可求出該機(jī)器人手部在某一時(shí)刻的速度V=f(t),即手部瞬時(shí)速度。反之,給定機(jī)器人手部速度,可由V=J(q)q解出相應(yīng)的關(guān)節(jié)速度，式中為機(jī)器人逆速度雅可比矩陣。逆速度雅可比J-1出現(xiàn)奇異解的情況如下:①工作域邊界上的奇異:機(jī)器人手臂全部伸開(kāi)或全部折回時(shí),叫奇異形位。該位置產(chǎn)生的解稱為工作域邊界上的奇異。②工作域內(nèi)部奇異:機(jī)器人兩個(gè)或多個(gè)關(guān)節(jié)軸線重合引起的奇異。當(dāng)出現(xiàn)奇異形位時(shí),會(huì)產(chǎn)生退化現(xiàn)象,即在某空間某個(gè)方向(或子域)上,不管機(jī)器人關(guān)節(jié)速度怎樣選擇,手部也不可能動(dòng)。若已知關(guān)節(jié)上與是時(shí)間的函數(shù),則可求出該機(jī)器人手部在各關(guān)節(jié)的驅(qū)動(dòng)力矩（或力）與末端操作器施加的力（廣義力，包括力和力矩）之間的關(guān)系就是機(jī)器人操作臂力控制的基礎(chǔ)。靜力平衡：假定各關(guān)節(jié)“鎖定”，機(jī)器人成為一個(gè)機(jī)構(gòu)。該“鎖定”用的關(guān)節(jié)力與手部所支持的載荷或受到外界環(huán)境作用力取得靜力平衡。求解這種“鎖定用”的關(guān)節(jié)力，或求解在已知驅(qū)動(dòng)力矩作用下手部的輸出力就是對(duì)機(jī)器人操作臂的靜力計(jì)算。3.2.2.工業(yè)機(jī)器人靜力學(xué)分析各關(guān)節(jié)的驅(qū)動(dòng)力矩（或力）與末端操作器施加的力（廣義力，包括力圖3.15單個(gè)桿件受力分析靜力平衡條件：桿上所受合力、合力矩為零。手部端點(diǎn)的力和力矩可寫成一個(gè)6維矢量1.操作臂的靜力已知末桿受力（力矩），可先分析末桿對(duì)上一連桿的力和力矩，依次分析反推，直至分析第一連桿對(duì)基座力和力矩，從而計(jì)算全部連桿的受力情況。和各有關(guān)于軸的三個(gè)分量。圖3.15單個(gè)桿件受力分析1.操作臂的靜力已知末桿受力（力矩各關(guān)節(jié)驅(qū)動(dòng)力矩、力可寫成n維矢量的形式。即2.機(jī)器人力雅克比矩陣忽略各關(guān)節(jié)摩擦力和桿件重力有-廣義關(guān)節(jié)力矩；F-機(jī)器人手部端點(diǎn)力；—（n×6）階雅克比矩陣。機(jī)器人力雅克比正好是速度雅克比的轉(zhuǎn)置。體現(xiàn)了手部端點(diǎn)受力和各關(guān)節(jié)驅(qū)動(dòng)力矩的關(guān)系。各關(guān)節(jié)驅(qū)動(dòng)力矩、力可寫成n維矢量的形式。2.機(jī)器人力雅克比3、

機(jī)器人靜力計(jì)算的兩類問(wèn)題①

已知外界對(duì)手部作用力F′，求滿足靜力平衡條件的關(guān)節(jié)驅(qū)動(dòng)力矩τ()。②

已知關(guān)節(jié)驅(qū)動(dòng)力矩τ,確定機(jī)器人手部對(duì)外界環(huán)境的作用力F或負(fù)荷質(zhì)量。當(dāng)自由度n>6時(shí)，力雅可比可能不是方陣，JT沒(méi)有逆解，一般情況下不一定能得到惟一的解。

3、機(jī)器人靜力計(jì)算的兩類問(wèn)題②已知關(guān)節(jié)驅(qū)動(dòng)力矩τ,確定3.2.3.工業(yè)機(jī)器人動(dòng)力學(xué)分析1.動(dòng)力學(xué)分析的兩類問(wèn)題①給出已知的軌跡點(diǎn)的關(guān)節(jié)變量即機(jī)器人的關(guān)節(jié)位置、速度和加速度，求相應(yīng)的關(guān)節(jié)力矩向量τ，用以實(shí)現(xiàn)對(duì)機(jī)器人的動(dòng)態(tài)控制。②已知關(guān)節(jié)驅(qū)動(dòng)力矩，求機(jī)器人系統(tǒng)的相應(yīng)的各瞬時(shí)的運(yùn)動(dòng),用于模擬機(jī)器人運(yùn)動(dòng)。動(dòng)力學(xué)分析方法:有拉格朗日方法、牛頓-歐拉方法、高斯方法、凱恩方法等。其中,拉格朗日方法不僅求解復(fù)雜的系統(tǒng)動(dòng)力學(xué)方程簡(jiǎn)單,而且容易理解。3.2.3.工業(yè)機(jī)器人動(dòng)力學(xué)分析①給出已知的軌跡點(diǎn)的關(guān)2.拉格朗日方程定義拉格朗日函數(shù)EK-機(jī)械系統(tǒng)動(dòng)能，EP-勢(shì)能的函數(shù)，勢(shì)能EP是的函數(shù)。的函數(shù)。動(dòng)能EK是關(guān)節(jié)變量和因此L是和拉格朗日方程為：Fi-關(guān)節(jié)廣義驅(qū)動(dòng)力（移動(dòng)關(guān)節(jié)為驅(qū)動(dòng)力，轉(zhuǎn)動(dòng)關(guān)節(jié)為驅(qū)動(dòng)力矩）。i=1.2.3……n2.拉格朗日方程EK-機(jī)械系統(tǒng)動(dòng)能，EP-勢(shì)能的函數(shù)，勢(shì)能E建立動(dòng)力學(xué)方程步驟：1)選取坐標(biāo)系，選定獨(dú)立的廣義關(guān)節(jié)變量(i=1.2.....n)；2)選定相應(yīng)的廣義力Fi；3)求各構(gòu)件的動(dòng)能與勢(shì)能，構(gòu)造拉格朗日函數(shù)；4)代入拉格朗日方程，求得機(jī)器人的動(dòng)力學(xué)方程。建立動(dòng)力學(xué)方程步驟：關(guān)節(jié)空間：n個(gè)自由度操作臂末端位姿x是由n個(gè)關(guān)節(jié)變量決定的，這n個(gè)關(guān)節(jié)變量叫n維關(guān)節(jié)矢量q，q所構(gòu)成的空間稱關(guān)節(jié)空間。操作空間：末端操作器的位姿是在直角坐標(biāo)系空間中描述的，這個(gè)空間叫操作空間。3.關(guān)節(jié)空間和操作空間動(dòng)力學(xué)針對(duì)圖3.17二自由度機(jī)器人，關(guān)節(jié)空間動(dòng)力學(xué)方程為：關(guān)節(jié)空間：n個(gè)自由度操作臂末端位姿x是由n個(gè)關(guān)節(jié)變量決定的，是（n×1）離心力和哥氏力矢量：是（n×1）