第3章機器人運動學3.1機器人的位姿描述3.2齊次變換及運算3.3機器人運動學方程3.4機器人微分運動習題第3章機器人運動學運動學研究的問題：

手在空間的運動與各個關(guān)節(jié)的運動之間的關(guān)系。正問題：已知關(guān)節(jié)運動，求手的運動。逆問題：已知手的運動，求關(guān)節(jié)運動。數(shù)學模型：手的運動→位姿變化→位姿矩陣M

關(guān)節(jié)運動→參數(shù)變化→關(guān)節(jié)變量qi，i=1，…，n運動學方程：

M=f(qi)，i=1，…，n正問題：已知qi，求M。逆問題：已知M，求qi。第3章機器人運動學3.1.1機器人位姿的表示3.1.2

機器人的坐標系

3.1機器人的位姿描述第3章機器人運動學3.1.1

機器人位姿的表示機器人的位姿主要是指機器人手部在空間的位置和姿態(tài)，有時也會用到其它各個活動桿件在空間的位置和姿態(tài)。3.1機器人的位姿描述第3章機器人運動學3.1.1機器人位姿的表示位置可以用一個3×1的位置矩陣來描述。ｐ(ｘ,ｙ,ｚ)ｚｙｘｏ3.1機器人的位姿描述第3章機器人運動學3.1.1機器人位姿的表示姿態(tài)可以用坐標系三個坐標軸兩兩夾角的余弦值組成3×3的姿態(tài)矩陣來描述。

ｐ(ｘ,ｙ,ｚ)ｚｙｘｏｚhｙhｘhｏh3.1機器人的位姿描述第3章機器人運動學3.1.1機器人位姿的表示例：右圖所示兩坐標系的姿態(tài)為：ｚ0ｙ0ｘ0ｏ0ｚ1ｙ1ｘ1ｏ13.1機器人的位姿描述第3章機器人運動學3.1.2機器人的坐標系手部坐標系——參考機器人手部的坐標系，也稱機器人位姿坐標系，它表示機器人手部在指定坐標系中的位置和姿態(tài)。機座坐標系——參考機器人機座的坐標系，它是機器人各活動桿件及手部的公共參考坐標系。桿件坐標系——參考機器人指定桿件的坐標系，它是在機器人每個活動桿件上固定的坐標系，隨桿件的運動而運動。絕對坐標系——參考工作現(xiàn)場地面的坐標系，它是機器人所有構(gòu)件的公共參考坐標系。3.1機器人的位姿描述第3章機器人運動學3.1.2機器人的坐標系手部坐標系{h}機座坐標系{0}

桿件坐標系{i}

i=1，…，n絕對坐標系{B}

3.1機器人的位姿描述第3章機器人運動學3.2.1

直角坐標變換3.2.2

齊次坐標變換3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換ｚiｙiｘiｏiｚjｙjｘjｏj坐標之間的變換關(guān)系：平移變換旋轉(zhuǎn)變換3.2齊次變換及運算第3章機器人運動學1、平移變換設(shè)坐標系{i}和坐標系{j}具有相同的姿態(tài)，但它倆的坐標原點不重合，若用矢量表示坐標系{i}和坐標系{j}原點之間的矢量，則坐標系{j}就可以看成是由坐標系{i}沿矢量平移變換而來的，所以稱矢量為平移變換矩陣，它是一個3×1的矩陣，即：

ｚiｙiｘiｏiｚjｙjｘjｏj3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換1、平移變換

若空間有一點在坐標系{i}和坐標系{j}中分別用矢量

和

表示，則它們之間有以下關(guān)系：稱上式為坐標平移方程。ｚiｙiｘiｏiｚjｙjｘjｏj3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換設(shè)坐標系{i}和坐標系{j}的原點重合，但它倆的姿態(tài)不同，則坐標系{j}就可以看成是由坐標系{i}旋轉(zhuǎn)變換而來的，旋轉(zhuǎn)變換矩陣比較復(fù)雜，最簡單的是繞一根坐標軸的旋轉(zhuǎn)變換，下面以此來對旋轉(zhuǎn)變換矩陣作以說明。ｚiｙiｘiｏiｚjｙjｘjｏj3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換繞z軸旋轉(zhuǎn)θ角坐標系{i}和坐標系{j}的原點重合，坐標系{j}的坐標軸方向相對于坐標系{i}繞軸旋轉(zhuǎn)了一個θ角。θ角的正負一般按右手法則確定，即由z軸的矢端看，逆時鐘為正。ｚiｙiｘiｏiｚjｙjｘjｏjθθ3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換繞z軸旋轉(zhuǎn)θ角若空間有一點p，則其在坐標系{i}和坐標系{j}中的坐標分量之間就有以下關(guān)系：

ｚiｙiｘiｏiｚjｙjｘjｏjθ3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換繞z軸旋轉(zhuǎn)θ角若補齊所缺的有些項，再作適當變形，則有：

3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換繞z軸旋轉(zhuǎn)θ角將上式寫成矩陣的形式，則有：

3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換①繞z軸旋轉(zhuǎn)θ角

再將其寫成矢量形式，則有：稱上式為坐標旋轉(zhuǎn)方程，式中：

——p點在坐標系{i}中的坐標列陣（矢量）；

——p點在坐標系{j}中的坐標列陣（矢量）；

——坐標系{j}變換到坐標系{i}的旋轉(zhuǎn)變換矩陣，也稱為方向余弦矩陣。3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換

——旋轉(zhuǎn)變換矩陣，也稱為方向余弦矩陣，是一個3×3的矩陣，其中的每個元素就是坐標系{i}和坐標系{j}相應(yīng)坐標軸夾角的余弦值，它表明坐標系{j}相對于坐標系{i}的姿態(tài)（方向）。3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換②繞x軸旋轉(zhuǎn)α角的旋轉(zhuǎn)變換矩陣為：

ｙiｚiｘiｏiｚjｙjｘjｏjαα3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換③繞y軸旋轉(zhuǎn)β角的旋轉(zhuǎn)變換矩陣為：

ｘiｙiｚiｏiｚjｙjｘjｏjββ3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換旋轉(zhuǎn)變換矩陣的逆矩陣

旋轉(zhuǎn)變換矩陣的逆矩陣既可以用線性代數(shù)的方法求出，也可以用逆向的坐標變換求出。以繞z軸旋轉(zhuǎn)θ角為例，其逆向變換即為繞z軸旋轉(zhuǎn)-θ角，則其旋轉(zhuǎn)變換矩陣就為：3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換2、旋轉(zhuǎn)變換旋轉(zhuǎn)變換矩陣的逆矩陣

比較以下兩式：

結(jié)論：

3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換3、聯(lián)合變換設(shè)坐標系{i}和坐標系{j}之間存在先平移變換，后旋轉(zhuǎn)變換，則空間任一點在坐標系{i}和坐標系{j}中的矢量之間就有以下關(guān)系：

稱上式為直角坐標系中的坐標聯(lián)合變換方程。3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換3、聯(lián)合變換若坐標系{i}和坐標系{j}之間是先旋轉(zhuǎn)變換，后平移變換，則上述關(guān)系是應(yīng)如何變化？3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換例：已知坐標系{B}的初始位置與坐標系{A}重合，首先坐標系{B}沿坐標系{A}的x軸移動12個單位，并沿坐標系{A}的y軸移動6個單位，再繞坐標系

{A}的z軸旋轉(zhuǎn)30°，求平移變換矩陣和旋轉(zhuǎn)變換矩陣。假設(shè)某點在坐標系{B}中的矢量為：

，求該點在坐標系{A}中的矢量？3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換解：由題意可得平移變換矩陣和旋轉(zhuǎn)變換矩陣分別為：，則：

3.2齊次變換及運算第3章機器人運動學3.2.1

直角坐標變換1、齊次坐標的定義空間中任一點在直角坐標系中的三個坐標分量用表示，若有四個不同時為零的數(shù)與三個直角坐標分量之間存在以下關(guān)系：則稱是空間該點的齊次坐標。3.2.2

齊次坐標變換3.2齊次變換及運算第3章機器人運動學1、齊次坐標的定義齊次坐標的性質(zhì)Ⅰ.空間中的任一點都可用齊次坐標表示；Ⅱ.空間中的任一點的直角坐標是單值的，但其對應(yīng)的齊次坐標是多值的；Ⅲ.k是比例坐標，它表示直角坐標值與對應(yīng)的齊次坐標值之間的比例關(guān)系；Ⅳ.若比例坐標k=1，則空間任一點(x,y,z)的齊次坐標為(x,y,z,1)

，以后用到齊次坐標時，一律默認k=1

。

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）若坐標系{j}是{i}先沿矢量平移，再繞z軸旋轉(zhuǎn)θ角得到的，則空間任一點在坐標系{i}和坐標系{j}中的矢量和對應(yīng)的變換矩陣之間就有，寫成矩陣形式則為：3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）再用坐標分量等式表示，則有：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）引入齊次坐標，補齊所缺各項，再適當變形，則有：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）再將其寫成矩陣形式則有：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）由此可得聯(lián)合變換的齊次坐標方程為：

式中，——齊次坐標變換矩陣，它是一個4×4的矩陣。3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）①齊次坐標變換矩陣的意義若將齊次坐標變換矩陣分塊，則有：意義：左上角的3×3矩陣是兩個坐標系之間的旋轉(zhuǎn)變換矩陣，它描述了姿態(tài)關(guān)系；右上角的3×1矩陣是兩個坐標系之間的平移變換矩陣，它描述了位置關(guān)系，所以齊次坐標變換矩陣又稱為位姿矩陣。

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）①齊次坐標變換矩陣的意義齊次變換矩陣的通式為：

式中，

——{j}的原點在{i}中的坐標分量；

——{j}的x軸對{i}的三個方向余弦；

——{j}的y軸對{i}的三個方向余弦；

——{j}的z軸對{i}的三個方向余弦。3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）②單獨的平移或旋轉(zhuǎn)齊次坐標變換矩陣

平移變換的齊次矩陣為：3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）②單獨的平移或旋轉(zhuǎn)齊次坐標變換矩陣

旋轉(zhuǎn)變換的齊次矩陣為：3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）②單獨的平移或旋轉(zhuǎn)齊次坐標變換矩陣

同理可得：3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）③聯(lián)合變換與單步齊次變換矩陣的關(guān)系觀察以下三個齊次變換矩陣的關(guān)系：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）③聯(lián)合變換與單步齊次變換矩陣的關(guān)系經(jīng)觀察可得：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）

③聯(lián)合變換與單步齊次變換矩陣的關(guān)系任何一個齊次坐標變換矩陣均可分解為一個平移變換矩陣與一個旋轉(zhuǎn)變換矩陣的乘積，即：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）

③聯(lián)合變換與單步齊次矩陣的關(guān)系

當空間有n個坐標系時，若已知相鄰坐標系之間的齊次變換矩陣，則：

由此可知，建立機器人的坐標系，將機器人手部在空間的位姿用齊次坐標變換矩陣描述出來，從而建立機器人的運動學方程。

{0}{i-1}{i}{n}3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）

④相對變換坐標系之間多步齊次變換矩陣等于每次單獨變換的齊次變換矩陣的乘積，而相對變換則決定這些矩陣相乘的順序，其分為左乘和右乘：

Ⅰ.若坐標系之間的變換是始終相對于原來的參考坐標系，則齊次坐標變換矩陣左乘；

Ⅱ.若坐標系之間的變換是相對于當前新的坐標系，則齊次坐標變換矩陣右乘。

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）④相對變換例：已知坐標系{B}是繞坐標系{A}的zA軸旋轉(zhuǎn)90°，再繞{A}的xA軸旋轉(zhuǎn)90°，最后沿矢量：平移得到的，求坐標系{A}與坐標系{B}之間的齊次坐標變換矩陣。

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）

④相對變換解：由題意可知滿足左乘原則，即有：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）

④相對變換解：若滿足右乘原則，則有：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）⑤逆變換已知{i}通過先平移，后旋轉(zhuǎn)變成{j}，則變換矩陣為：

ｚiｙiｘiｏiｚjｙjｘjｏj3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）⑤逆變換逆變換時：變換順序顛倒；先平移，后旋轉(zhuǎn)→先旋轉(zhuǎn)，后平移。變換參數(shù)取反。旋轉(zhuǎn)（θ）

→（

-θ），平移（px，py，pz）

→（-px，-py，-pz）。3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）⑤逆變換則{j}到{i}的變換矩陣為：ｚiｙiｘiｏiｚjｙjｘjｏj3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）⑤逆變換3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）⑤逆變換3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換

2、齊次變換矩陣（D-H矩陣）⑤逆變換若齊次變換矩陣為：則：

3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換2、齊次變換矩陣（D-H矩陣）⑤逆變換若齊次變換矩陣為：3.2齊次變換及運算第3章機器人運動學3.2.2

齊次坐標變換3.3.1運動學方程建立步驟

1、建立坐標系

2、確定參數(shù)

3、相鄰桿件的位姿矩陣

4、建立方程3.3.2運動學方程的解3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟運動學方程的模型：

M=f(qi)，i=1，…，nM——機器人手在空間的位姿

qi——機器人各個關(guān)節(jié)變量3.3機器人運動學方程第3章機器人運動學1、建立坐標系①機座坐標系{0}②桿件坐標系{i}i=1，2，…，n③手部坐標系{h}3.3機器人運動學方程第3章機器人運動學oh0123關(guān)節(jié)1關(guān)節(jié)2關(guān)節(jié)3x1z1o1Zhxhx0z0o0z3x3o3y2x2o23.3.1

運動學方程建立步驟1、建立坐標系①機座坐標系{0}建立原則：z軸垂直，x軸水平，方向指向手部所在平面。x0z0o03.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟1、建立坐標系②桿件坐標系{i}，i=1，2，…，n

建立原則：

z軸與關(guān)節(jié)軸線重合，

x軸與兩關(guān)節(jié)軸線的距離重合，方向指向下一個桿件。桿件坐標系有兩種：

第一種：z軸與i+1關(guān)節(jié)軸線重合；

第二種：z軸與i關(guān)節(jié)軸線重合。3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟1、建立坐標系②桿件坐標系{i}第一種坐標系：

z軸與i+1關(guān)節(jié)軸線重合。x0z0o00123關(guān)節(jié)1關(guān)節(jié)2關(guān)節(jié)3z2x2o2x1y1o1z3x3o33.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟x0z0o00123關(guān)節(jié)1關(guān)節(jié)2關(guān)節(jié)3y2x2o2z3x3o3x1z1o11、建立坐標系②桿件坐標系{i}第二種坐標系：

z軸與i關(guān)節(jié)軸線重合。3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟1、建立坐標系③手部坐標系{h}

在第一種桿件坐標系下，{h}與{n}坐標系重合。0123關(guān)節(jié)1關(guān)節(jié)2關(guān)節(jié)3Z3hx3ho3hx0z0o0z2x2o2x1y1o13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟1、建立坐標系③手部坐標系{h}

在第二種桿件坐標系下，{h}與{n}坐標系的方向保持一致。oh0123關(guān)節(jié)1關(guān)節(jié)2關(guān)節(jié)3x1z1o1Zhxhx0z0o0z3x3o3y2x2o23.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟2、確定參數(shù)①桿件幾何參數(shù)（不變）I、桿件長度li：——兩關(guān)節(jié)軸線的距離。II、桿件扭角αi：——兩關(guān)節(jié)軸線的夾角。iliαi3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟2、確定參數(shù)②關(guān)節(jié)運動參數(shù)I、關(guān)節(jié)平移量di：——相鄰桿件的長度在關(guān)節(jié)軸線上的距離。II、關(guān)節(jié)回轉(zhuǎn)量θi：——相鄰桿件的長度在關(guān)節(jié)軸線上的夾角。ili-1i-1liθi關(guān)節(jié)idi3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟2、確定參數(shù)②關(guān)節(jié)運動參數(shù)關(guān)節(jié)變量：

di——平移關(guān)節(jié)；

θi——回轉(zhuǎn)關(guān)節(jié)。ili-1i-1liθi關(guān)節(jié)idi3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣①第一種坐標系

建立坐標系{i-1}、{i}。試分析{i-1}→{i}的變換過程。ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣①第一種坐標系I、{i-1}→{i}變換過程a、Trans（0，0，di）；b、Rot（z，θi）；c、Trans（li，0，0）；d、Rot（x，αi）。ii-1liθi關(guān)節(jié)idiXi-1Zi-1Oi-1XiZiOiαi3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣①第一種坐標系

II、單步齊次變換矩陣3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣①第一種坐標系

III、相鄰桿件的位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣①第一種坐標系

III、相鄰桿件的位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣①第一種坐標系注意：特例！！！3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣②第二種坐標系

建立坐標系{i-1}、{i}。試分析{i-1}→{i}的變換過程。ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣②第二種坐標系I、{i-1}→{i}變換過程a、Trans（li-1，0，0）；b、Rot（x，αi-1）；c、Trans（0，0，di）；d、Rot（z，θi）。ili-1i-1θi關(guān)節(jié)idiXi-1Zi-1Oi-1XiZiOiαi-13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣②第二種坐標系

II、單步齊次變換矩陣3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣②第二種坐標系

III、相鄰桿件的位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟3、相鄰桿件位姿矩陣②第二種坐標系

III、相鄰桿件的位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟4、建立方程3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟例：已知三自由度平面關(guān)節(jié)機器人如圖所示，設(shè)機器人桿件1、2、3的長度為l1，l2，l3。建立機器人的運動學方程。

l1l3l23.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（1）建立坐標系(第一種)a、機座坐標系｛0｝

b、桿件坐標系｛i｝

c、手部坐標系｛h｝（與末端桿件坐標系｛n｝重合）

l1l3l2x0y0y1x1y2x2y3hx3h3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（2）確定參數(shù)l1l3l2x0y0y1x1y2x2y3hx3hi

diθi

liαi

qi1

0θ1l1

0θ12

0θ2l2

0θ23

0θ3l3

0θ3θ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3hx3hθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3hx3hθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3hx3hθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（4）建立方程將相鄰桿件位姿矩陣依次相乘，則有：

3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（4）建立方程若用矩陣形式表示，則為：

3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（4）建立方程若用方程組形式表示，則為：

3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（1）建立坐標系(第二種)a、機座坐標系｛0｝

b、桿件坐標系｛i｝

c、手部坐標系｛h｝（與末端桿件坐標系｛n｝方向一致）

l1l3l2x0y0y1x1y2x2y3x3yhxh3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（2）確定參數(shù)ili-1αi-1

diθi

qi1

0

0

0θ1θ12l1

0

0θ2θ23l2

0

0θ3θ3l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（3）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（4）建立方程將相鄰桿件位姿矩陣依次相乘，則有：

3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（4）建立方程若用矩陣形式表示，則為：

3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟解：（4）建立方程若用方程組形式表示，則為：

3.3機器人運動學方程第3章機器人運動學3.3.1

運動學方程建立步驟運動學方程的模型：

M0h=f(qi)，i=1，…，n正問題：已知關(guān)節(jié)變量qi的值，求手在空間的位姿M0h。逆問題：已知手在空間的位姿M0h，求關(guān)節(jié)變量qi的值。3.3.2運動學方程的解3.3機器人運動學方程第3章機器人運動學1、運動學方程的正解正問題：已知關(guān)節(jié)變量qi的值，求手在空間的位姿M0h。正解特征：唯一性。用處：檢驗、校準機器人。3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解2、運動學方程的逆解逆問題：已知手在空間的位姿M0h，求關(guān)節(jié)變量qi的值。逆解特征分三種情況：多解、唯一解、無解。多解的選擇原則：最接近原則。計算方法：遞推逆變換法，即3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解例：已知四軸平面關(guān)節(jié)SCARA機器人如圖所示，試計算：（1）機器人的運動學方程；（2）當關(guān)節(jié)變量取qi=[30°，-60°，－120，90°]T時，機器人手部的位置和姿態(tài)；（3）機器人運動學逆解的數(shù)學表達式。8004003002003.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程a、建立坐標系（第一種）機座坐標系｛0｝桿件坐標系｛i｝手部坐標系｛h｝x0z0x1z1x4hz4h800400300200x2z2x3z33.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程b、確定參數(shù)i

diθi

liαi

qi1800θ1400

0θ12

0θ2300

0θ23

d30

0

0

d34-200θ400θ4800400300200x0z0x1z1x2z2x3z3x4hz4h3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程c、相鄰桿件位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程c、相鄰桿件位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程c、相鄰桿件位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程c、相鄰桿件位姿矩陣3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（1）運動學方程d、建立方程

3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（2）已知qi=[30°，-60°，－120，90°]T，代入（1）中的運動學方程，則得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式已知運動學方程，用通式表示為：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式聯(lián)立方程：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式由上面（a）、（b）兩式可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式由上面（c）、（d）兩式平方再相加可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解2、運動學方程的解解：（3）逆解數(shù)學表達式由上面（c）、（d）兩式展開可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式由上面兩式可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式由上面兩式可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式已知θ1，θ2可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式最后由（e)式可得：3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解解：（3）逆解數(shù)學表達式3.3機器人運動學方程第3章機器人運動學3.3.2運動學方程的解3.4.1

微分變換3.4.2

雅可比矩陣3.4機器人微分運動第3章機器人運動學

設(shè)機器人運動鏈中某一桿件相對于機座坐標系的位姿為，經(jīng)過微運動后該桿件的位姿變?yōu)椋粑蛔耸悄硞€變量q的函數(shù)，則：若位姿是若干個變量的函數(shù)，則：

3.4.1微分變換第3章機器人運動學3.4機器人微分運動已知一個2自由度機器人及其坐標系如圖所示。若因桿件1下關(guān)節(jié)軸承裝配或制造不當，使桿件1沿關(guān)節(jié)軸線有0.05單位的偏差，又由于兩桿件的執(zhí)行器運動不準確，旋轉(zhuǎn)執(zhí)行器使桿件1多轉(zhuǎn)一個0.01rad的偏差角，移動執(zhí)行器使桿件2移動了一個0.1單位的偏差距離。若桿件1的長度單位，試求當機器人關(guān)節(jié)變量取單位時，機器人手部位姿的偏