第2章 工业机器人运动学教学设计

第2章工业机器人运动学

章节题目:第2章工业机器人运动学

［教学内容］

2-1齐次坐标及对象物得描述

2-2齐次变换及运算

2-3工业机器人连杆参数及其齐次变换矩阵

2-4工业机器人运动学方程

［教学安排］

第2章安排6学时，其中介绍点得位置描述1()分钟，齐次坐标10分钟,坐标轴

方向得描述10分钟,动坐标系位姿得描述20分钟,目标物齐次矩阵表示10分钟,

平移得齐次变换30分钟，旋转得齐次变换30分钟，平移加旋转得齐次变换15分

钟，连杆参数及连杆坐标系得建立20分钟,连杆坐标系之间得变换矩阵15分钟,

机器人运动学方程10分钟，正向运动学及实例45分钟，反向运动学及实例30分钟,

X=X(Q)形式运动学方程15分钟。

通过多媒体课件结合板书得方式，采用课堂讲授和课堂讨论相结合得方法，介

绍齐次坐标得概念及各种对象得齐次坐标方法，进而向学生讲述齐次变换及运算

方法,通过上述内容得济解，进一步让学生掌握连杆参数及其齐次变换矩阵，最终

引出工业机器人运动学方程。

［知识点及其基本要求］

1、点得位置描述（掌握）

2、齐次坐标（掌握）

3、坐标轴方向得描述（掌握）

4、动坐标系得描述（掌握）

5、齐次变换（重点掌握）

6、连杆参数及其齐次变换矩阵（掌握）

7、运动学方程（掌握）

［重点和难点］

重点：1、对象得齐次坐标表示

2、齐次变换

3、机器人运动学方程

难点：1、连杆参数

2、机器人运动学方程

［教学法设计］

引入新课：

机器人实际上可认为就就是由一系列关节连接起来得连杆所组成。我们把坐

标系固连在机器人得每一个连杆关节上，可以用齐次变换来描述这些坐标系之间

得相对位置和方向。齐次变换具有较直观得几何意义，而且可描述各连杆之间得

关系，所以常用于解决运动学问题。

新课讲解：

第一次课

第2章工业机器人运动学

2-1齐次坐标及对象物得描述

一、点得位置描述

在选定得直角坐标系｛A｝中，空间任一点P得位置可用3XI得位置矢量Ap表

示，其左上标代表选定得参考坐标系：。=pxo

二、齐次坐标

如用四个数组成得(4X1)列阵〃=表示三维空间直角坐标系｛A｝中点、P,

则列阵RPyPz1]丁称为三维空间点P得齐次坐标。

必须注意，齐次坐标得表示不就就是唯一得，将其各元素同乘一非零因子W后,

Px

仍然代表同一点口,即〃=Py

P二

三、坐标轴方向得描述

i,让分别表示直角坐标系中X、Y、Z坐标轴得单位向量。若用齐次坐标来

X=[i00o]7

描述X、Y、Z轴得方向，则y=[o]0o]TO

Z=[o01o]f

从上可知,规定：

(4X1)列阵[abcO]T中第四个元素为零，且M+b2+c2=l,则表示某轴(某矢量)

得方向；

(4X1)列阵[abcw]T中第四个元素不为零，则表示空间某点得位置。

四、动生标系位姿得描述

动坐标系位姿得描述就就就是对动坐标系原点位置得描述以及对动坐标系

各坐标轴方向得描述。

1、刚体位置和姿态得描述

机器人得一个连杆可以看做一个刚体。若给定了刚体上某一点得位置和该刚

体在空间得姿态,则这个刚体在空间上就就是完全确定得。

C)'为刚体上任一点,O'X'Y'7;为与刚体固连得一个坐标系，称为动坐

标系。刚体Q在固定坐标系OXYZ中得位置可用齐次坐标形式得一个(4X1)列

阵表示：〃=)'。O

2()

1

刚体得姿态可由动坐标系得坐标轴方向来表示。令n、o、a分别为X'、Y'、

7:坐标轴得单位方向矢量，每个单位方向矢量在固定坐标系上得分量为动坐标

系各坐标轴得方向余弦，用齐次坐标形式得(4X1)列阵分别表示为:。=瓜n,n2

Tr

0],o=Eoxoyoz0]Ia=[%aya20]o

因此,刚体得位姿可用下面(4X4)矩阵来描述：

2、手部位置和姿态得表示

机器人手部得位置和姿态也可以用固连于手部得坐标系｛B｝得位姿来表示。

坐标系｛B｝可以这样来确定:取手部得中心点OB；关节周为ZB轴,ZB轴得单位方向

矢量a称为接近矢量，指向朝外;二手指得连线为YB轴,YB轴得单位方向矢量o称

为姿态矢量，指向可任意选定;XB轴与YB轴及ZB轴垂直XB轴得单位方向矢量n

称为法向矢量，且n=oXa,指向符合右手法则。

手部得位置矢量为固定参考系原点指向手部坐标系｛B｝原点得矢量p,手部得

方向矢量为n、。、ao于就就是手部得位姿可用(4X4)矩阵表示

五、目标物齐次矩阵表示

2-2齐次变换及运算

刚体得运动就就是由转动和平移组成得。为了能用同一矩阵表示转动和平移,

有必要引入(4X4)得齐次坐标变换矩阵。

一、平移得齐次变换

空间某一点A,坐标为(x,y,z),当她平移至A'点后,坐标为(x'，y’,z')，以及

X'=A"+Ax

，或写成如下形式:y，也可以简写为a'二Trans(Ax,Ay,

=y+Ay

z

z'=z+Az

1

△z)a,其中，Trans(△x,△y,△z)表示齐次坐标变换得平移算子。且

100Ax

010△y,其中第四列元素分别表示沿坐标轴XYZ得移动量。

Trans(Ar,Ay,Az)=0,,

01Az

000

若算子左乘，表示坐标变换就就是相对固定坐标系进行得;假如相对动坐标系进行

坐标变换,则算子应该名乘。

第二次课

二、旋转得齐次变换

空间某一点A,坐标为(x,y0,当她绕Z轴旋转9角后至A'点,坐标为(x'，y

x'=cos6?K-sinOx>

Z'),A'点和A点得坐标关系为：),，=sin/+cos/或用矩阵表示

Zf=2

A'点和A点得齐次坐标分别为［x'y’z'1]।和[xyzl][因此A点

'cos^-sin<90O'X

得旋转齐次变换过程为:ysin。cos。00y，也可简写为:a'=Rot(z⑼a,其

z'0010z

100011

中,Rot(z⑼表示齐次坐标变换时绕Z轴得旋转算子，算子左乘表示相对于固定坐

cos，一sin。00

sin。cos。00

标系进行变换，算子得内容为：Rot(z,0)=

0010

0001

同理，可写出绕X轴旋转得算子和绕Y轴旋转得算子分别为:

-100O-cos。0sin00

0cos。-sin。00100

Rot(x,0)=,Rot(y,0)=

0sin。cos。0-sin00cos。0

_0001_0001

点A绕任意过原点得单位矢量k旋转。角时，匕入小工分别为k矢量在固定参

考系坐标轴X、Y、Z上得三个分量，且kj+ky2+月2=1。可以证明，绕任意过原

点得单位矢量k转9角得旋转齐次变换公式为：

kxkxversO+cos0kJvk,1versO--k.sin)k,kxvers0+kysin^0

八ktkvers0+k.smOkkvers0+cos0k.kyersO-ksin6^0

R(Wk、B)=*yv2yyx

kk.vers9-ksvi0kk.versO+ksinOk.k.versO-vcosO0

xx&yvyx

0001

式中,vers。=(1-cos6)o

上式称为一般旋转齐次变换通式，她概括了绕X轴、Y轴、Z轴进行旋转齐

次变换得各种特殊情况。

&ox见0

%%%0

反之,若给出某个旋转齐次变换矩阵R=，则可根据变换通

n.o.a.0

0001

式求出其等效转轴矢量k及等效转角。:

—a)'+[a—n.)~+(n—o)~

吆e=±——----------:------------

nx+oy+a,-\

o-a

k=_z___2_，

"2sin<9

k"四二”

>2sin/9

k二%一

2sin〃

式中，当()取0°到180°之间得值时,式中得符号取十号;当转角0很小时，公式

很难确定转轴;当。接近0°或180°时，转轴完全不确定。

和平移变换一样，旋转变换算子公式以及一般旋转变换算子公式，不仅仅适用

于点得旋转变换,而且也适用于矢量、坐标系、物体等旋转变换计算。若相对固

定坐标系进行变换，则算子左乘;若相对动坐标系进行变换,则算子右乘。

三、平移加旋转得齐次变换

平移变换和旋转变换可以组合在一个齐次变换中。

2-3工业机器人连杆参数及其齐次变换矩阵

一、连杆参数及连杆坐标系得建立

连杆两端有关节n和n+1。该连杆尺寸可以用两个量来描述:一个就就是两个

关节轴线沿公垂线得距离分称为连杆长度;另一个就就是垂直于明得平面内两个

轴线得夹角久,称为连杆扭角。这两个参数为连杆得尺寸参数。

再考虑连杆n与相邻连杆n-1得关系，若她们通过关节相连，其相对位置可用

两个参数来确定，其中dn就就是沿关节n轴线两个公垂线得距离凡就就

是垂直于关节n轴线得平面内两个公垂线得夹角。这就就是表达相邻杆件关系得

两个参数。这样，每个连杆可以由四个参数所描述:其中两个描述连杆尺寸,另外两

个描述连杆与相邻杆件得连接关系。对于旋转关节也就就是关节变量，其她三个

参数固定不变;对于移动关节,CL就就是关节变量，其她三个参数固定不变。

连杆坐标系得建立按下面得规则进行:连杆n坐标系（简称n系）得坐标原点设

在关节n得轴线和关节n+1得轴线得公垂线与关节n+1得轴线相交之处用系得

Z轴与关节n+1得轴线重合,X轴与上述公垂线重合，且方向从关节n指向关节

n+1,Y轴则按右手系确定。

二、连杆坐标系之间得变换矩阵

建立了各连杆坐标系后,n-1系与n系之间得变换关系可以用坐标系得平移、

旋转来实现。从n—1系到n系得变换,可先令n—1系绕Zn」轴旋转6n角，再沿Z

n」轴平移然后沿Xn轴平移心,最后绕Xn轴旋转5角，使得41系与R系重合。

用一个变换矩阵A来综合表示上述四次变换时应注意到坐标系在每次旋转或平

移后发生了变动，后一次变换都就就是相对动系进行得，因此在运算中变换算子应

该右乘。于就就是连杆n得齐次变换矩阵为：

Trans(O,Q,d„)Trans(a„,O,O)Trans(x.a„)

a-4,(/..1—

n‘'"ZTX②③④

cos%-sin%001oo%一1000

sin0cos。”0001000cosa-sina0

=nnn

0010()01dn0sina”cosa”0

000100010001

cos。”一sin0“cos%sin0nsina”a“cos。“-

sin3ncos/cosa”-cos。”sinana”sin%

0sinancosa„d“

000\

2-4工业机器人运动学方程

一、机器人运动学方程

为机器人得每一个连杆建立一个坐标系,并用齐次变换来描述这些坐标系间

得相对关系，也叫相对位姿。通常把描述一个连杆坐标系与下一个连杆坐标系间

相对关系得齐次变换矩阵叫做A变换矩阵或A矩阵。如果A1矩阵表示第一个连

杆坐标系相对于固定坐标系得位姿42矩阵表示第二个连杆坐标系相对于第一个

连杆坐标系得位类那么第二个连杆坐标系在固定坐标系中得位姿可用&和A2

得乘积来表示:Tz=A】A?。同理，若A3矩阵表示第三个连杆坐标系相对于第二个连

杆坐标系得位姿，则有T3=A1A2A3,如此类推，对于六连杆机器人，有下列矩阵:

=A1A2A3A4A5A6O此式右边表示了从固定参考系到手部坐标系得各连杆坐标系之

间得变换矩阵得连乘，左边T6表示这些变换矩阵得乘积，也就就就是手部坐标系

相对于固定参考系得位姿,称上式为机器人运动学方程，计算结果就就是一个如

下得(4X4)矩阵：口::'，式中，前三列表示手部得姿态，第四列表示手部

6

〃：O.azp.

0001

得位置。

第三次课

二、正向运动学及实例

正向运动学主要解决机器人运动学方程得建立及手部位姿得求解问题。

1、平面关节型机器人得运动学方程

具有一个肩关节、一个肘关节和一个腕关节得SCARA机器人得机械结构特

点就就是三个关节轴线就就是平行得。固定坐标系｛()｝和连杆1、连杆2、连杆3

得坐标系｛1｝、｛2｝、｛3｝坐落在关节1、关节2、关节3和手部中心。坐标系

｛3｝也就就就是手部坐标系。连杆参数中6为变量，其余参数d、a、％均为常量。

考虑到关节轴线平行，并且连杆都在一个平面内得特点，列出SCARA机器人连杆

得参数如下表所示。

两连杆间距离

连杆转角(变量)6连杆长度a连杆扭角a

d

连杆161di=0a1=l]=100a1=0

连杆2d=0。a=0

622a2=12=102

连杆3出d3=0的=13=20a3=0

该平面关节型机器人得运动学方程为T3=A1A2A力式中A1表示连杆1得坐标

系｛1｝相对于固定坐标系｛()｝得齐次变换矩阵；A?表示连杆2得坐标系｛2｝相对于

连杆1得坐标系｛1｝得齐次变换矩阵;A3表示连杆3得坐标系即手部坐标系｛3｝

相对于连杆2得坐标系｛2｝得齐次变换矩阵。于就就是

A,=Rot(zQi0i)Trans(llS),O)

有:A?=Rog,优)Transko,。)°即

A3=Rot(z2,Oy)Trans｛ly,0,0)

cosR-sin00100/1cos^]-sin，〕0/]cos。]

sin仇cos。[000100sin。]cos。】0/]sin^

001000100010

000100010001

cosa-sin00100//cos%-sin20l2cos02

sinacos%000100sin。？cos。？0/2sin^.

001000100010

0001_00010001

cos%-sin00■|00I；cos%-sina0/3cos

sin03cos%000100sincos40&sina

001000100010

000100010001

因此，可写出:

COS(4+02+Oy)_sin(a+a+a)0hcos(4+a+%)+Acos(〃]+02)+ltcosOf

sin(q+%+%)cos(q+2+④1hsin(q+%+1)+4sin(4+^2)+/,sin0t

00I0

0001

T3就就是A|、A2、A3连乘得结果，表示手部坐标系{3}(即手部)得位置和姿

态O

*生；P;

%4：P>

n.0.az\Pt

00Oil.

PA-

4COS(6+a+4)+4CQS(a+02)+/,COS”

可写出手部位置(4X1)列阵为：P>bsin(q+a+4)+4sin(q+02)+ltsin0{

P=

P；0

11

表示手部姿态得方向矢量n、Q、a分别为:

X-cos(4+，+/)-X-「siiua+'+eJax~o-

sin(4+W+4)COS(4+w+4)0

n==0=O'=a=%=

n:00.0a.1

000000

2、斯坦福机器人得运动学方程

杆号关节转角。扭角a杆长a距离d

1Bi-90300

90°

2020d2

0

300°d3

4%-90°00

50590°00

60。0

。6H

上表给出了斯坦福机器人各连杆得参数。现在根据各连杆坐标系得关系写出

齐次变换矩阵Ai。｛1｝系与｛（）｝系就就是旋转关节连接。坐标系｛1｝相对于固定坐

标系｛0｝得Z。轴得旋转为变量a,然后绕自身坐标系X1轴作％得旋转变换,a

,==90°o所以

COS。］0-sin仇o-

sin6.10cos。10

A।=&Mzo,4,%)二

0-100

0001

｛2｝系与｛1｝系就就是旋转关节连接，连杆距离为d?。坐标系｛2｝相对于坐

标系｛1｝得Zi轴得旋转为变量电然后绕自身坐标系Z2轴正向作d2距离得平移变

换及绕X2轴作a?得旋转坐标变换,处=90°o所以

COSa0sin%O-

sina0一cos。？0

A2=Rot(z，4d?)Rot(x,%)=

i20104

0001

｛3｝系与｛2｝系就就是移动关节连接。坐标系｛3｝相对于坐标系｛2｝得Z2轴

德平移为变量cb。所以

1000

0100

A=Trans(O,O,dy)=

30114

0001

斯坦福机器人手腕三个关节都就就是转动关节，关节变量为及。6,并且三

个关节得中心重合。

系｛4｝对系｛3｝得旋转变量为仇，然后绕自身坐标轴4作必得旋转变换.产-9

0°o所以

cos优0-sin20

sin0.,t0COSa0

A4=Rot{z3,)Rot(xi,a4)=

0-100

0001

系｛5｝对系｛4｝得旋转变量为网,然后绕自身坐标轴X5作备得旋转变换,"尸9

0°o所以

cos40sin/o-

sinOs0一COS0<0

A5=Ro"z$,6、)&”(*、,a5)=

0-100

0001

系｛6｝相对于系｛5｝得旋转变量为仇，并移动距离H。所以

-

cos%-sin060o

sin%COS400

A=Rot(z,1)Trans(0,0,H)=

65001H

0001

这样，所有杆得A矩阵已建立。如果要知道非相邻杆件间得关系，只要用相应

得A矩阵连乘即可。如：

“sinOj

COS4COS”-cosZ?5sin0bsin4

—COS4

sin%COS<96—sin&sin0hHcosg

4T6=A,A6=

sin%COS%00

0001

3

T6=A4A5A6

2T6=A3A4AA

•T6=A2A3A4A5A6

则斯坦福机器人得运动学方程为=T。=A1A2A3A尸尸6

方程右边得结果就就就是最后一个坐标系｛6｝得位置和姿态矩阵，各元素均为

。和d得函数,当。和d给出后，可以计算出斯坦福机器人手部坐标系｛6｝得位置p和

姿态n、o、a0这就就就是斯坦福机器人手部位姿得解，这个求解过程叫做斯垣福

机器人运动学正解。

三、反向运动学实例

在机器人控制中，往往在已知手部要到达得目标位姿得情况下如何求出关节

变量,以驱动各关节得马达，使手部得位姿得到满足，这就就就是反向运动学问题,

也称求运动学逆解。

以斯坦福机器人为例介绍反向求解得一种方法。假设H=0,即坐标系｛6｝与

坐标系｛5｝原点重合。已知斯坦福机器人得运动学方程为:丁6二4人2人小5A6,给

出矩阵及各杆得参数a、a、d,求关节变量〜仇，其中。3二d3。

⑴求仇：

用AL左乘运动学方程，得JT6=A]T6=A2A3A4AA，左右展开得：

3+“⑼P£+Pg

-n.-o-a.

x-P：

-PW+P2

0001

c2(cAc5c6-s4s6)-s2s5c6-C2(C4CsS6+S4C6)+S2S5S6C2c4s5+S2c552d3

$2(C4c5c6-54s6)+。255c6-S2(C4C5S6+^4^6)—c2555652c4S5—C2c5一C2d3

S4C5C6+g56T4css6+IQs4ssd?

0001

取上式左、右两边第三行第四列相等，即:-pS+pyC]=（12,引入中间变量I•及中,

221

令px=rcos<p,py=rcoscp,r=(px4-py)2*=arctg(py/px),则该式彳匕为:coSincp-co

S（psin0i=d2/to利用和差公式，上式又可化为：Sin@-“）=dz/r。这里0<d2/r《

2

1,0<9—e|V兀，又cos(^-01)=±L1-(d2/r)]'/\故

有：°一4=arctg/Jr=arctg,4，所以优=arcts£1一，。这里，

222

±yj\-(d2/r)yjr-d\Px±yjr-d1

“+”号对应右肩位姿，“一”号对应左肩位姿。

(2)求仇:

取上面矩阵等式左、右两边第一行第四列相等和第二行第四列相等，即：

P£+/3=力4

~P：=-C&

故:a_小喏任山区

Pz

(3)求仇：

在斯坦福机器人中自二，心,有⑵中等式可解得:&=$2+4P、)+。2P：

(4)求“：

3

由于3T6=A4AA，所以A/T6=A5A6,左右两边展开后取其左、右两边第三

行第三列相等,得:-.匕匕(。4+S|。、)-$24]+(-4q+G%)=()，所以

%=arctg------巴----------及q=^4+180°

c2(ciax+slay)-s2az

(5)求取：

取(4)中矩阵等式展开左、右两边第一行第三列相等及第二行第三列相等，有:

c[c(c)t/+4%)-sa|+S4+q%)=s

V42v2z5

s2(clax+siaY)+c2az=c5

c[c（ca+514、.）一$2。」+$4（一$]凡+。4）

所以：q—arctg4:ix

$2（6%+凡%）+。2生

⑹求。6：

采用方程A5T4T6=A6,展开并取其左、右两边第一行第二列相等及第二行第

$6=-c{%2(G%+S0)—SzQ]+

5[。V与(一邑％+cloy)}+s5[s2(c]ox+s]oy)+c2o2]

C6=一虱[。2(。氏+»%)-S24]+C4(-SQr+G%J

所以:a=arctg—o

C6

至此,“、％、d3、”、。5、。6全部求出。从以上解得过程看出，这种方法就就

就是将一个未知数由矩阵方程得右边移向左边，使其与其她未知数分开，解出这个

未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数。所以这种方

法也叫变量分离法。

还应注意到机器人运动学逆解问题得求解存在如下三个问题：

⑴解可能不存在。机器人具有一定得工作域，假如给定手部位置在工作域之

外,则解不存在。

⑵解得多重性。机器人得逆运动学问题可能出现多解。在多解情况下，一定

有一个最接近解，即最接近起始点得解。

⑶求解方法得多样性。机器人逆运动学求解有多种方法，一般分为两类:封闭

解和数值解。应该从计算方法得计算效率、计算精度等要求出发，选择较好得解

法。

四、X=X(Q)形式运动学方程

“角度设定法”就就是采用相对参考坐标系或相对运动坐标系作三次连续转

动来规定姿态得方法。机器人手部位姿可用一个6维列矢量来表示：X=0PyP

z%Vy中z]\或写成X=[xyz%%Tz]「。式中,x、yz表示手部位置用x、Q

y、％分别就就是用角度设定法来规定手部姿态时绕X轴、Y轴和Z轴得转角。

1、RPY角法和欧拉角法

RPY角法和欧拉角法就就是角度设定法中常用得方法。RPY角法就就是手

部相对参考坐标系轴作三次连续转动获得规定得姿态:先绕X轴转动外角,称为偏

转，再绕Y轴转动小角:称为俯仰，最后绕Z轴转动白角，称为翻滚，得到相应得旋转

矩阵为:RPYM/Py,中广Rot亿,%卬01(¥物小01区外),该式也称为“X—y—JR

PY角设定法。

欧拉角法就就是手部相对运动坐标系轴作三次连续转动获得规定得姿态:如

果转动顺序为z一y一x,则相应得旋转矩阵为:Euler(中z,%,崂=Rot(Z,qX)Rot

(Y,%)Rot(X,(pJ,该式也称为“z—y—x”欧拉角设定法。以上两式结果恰巧完全

相同。如果用其她顺序进行欧拉角三次连续转动，结果便不相同了。不论用什么

角度设定法来规定手部姿态，姿态得实现，事实上就就是由关节变量作决定得。知

道旋转矩阵则可由以上两式逆解出手部姿态得规定角七、打、3，并且小二%(q)、

%==9z(q),q为广义关节变量,q=[q।q2…qM

2、运动学方程X=X(q)

用A矩阵确定T0可写成=A1(q)A2(q)A3(q)A4(q)人5⑷&(。或写成

T6=T(q)o该式表示机器人手部位姿(n,o,%p)与关节变量q之间得关系。

［应用］

第一次课

第二次课

1、点矢量v为［10、0020、0030、00］1相对参考系作如下齐次变换,

写出变换后点矢量v得表达式。并说明就就是什么性质得变换，写出Rot。,？),Tr

an(?,?,?)o

0.866-0.500().00011.0

0.5000.8660.000-3.0

0.0000.0001.0009.0

0001

2、有一旋转变换，先绕固定坐标系Z°轴转'45°,再绕其X0轴转30°,最后绕

其Y0轴转60°,试求该齐次变换矩阵。

3、坐标系｛B｝起初与固定坐标系｛0｝相重合，现坐标系｛B｝绕ZB轴旋转

30°,然后绕旋转后得动坐标系XB轴旋转45°,试写出该坐标系｛B｝得起始矩阵

表达式和最后矩阵表达式。

4、坐标系｛A｝及闾在固定坐标系｛()｝中得矩阵表达式如下，画出她们在｛0