机器人运动学

创新设计作业:设计一种类人教学机器人。要求机器人具有类似人的四肢，单片机控制。给出总体的设计方案、机械结构和传动方案、选择合适的传感器、控制方案。

第2章机器人运动学

(KinematicsofRobots)引言机器人位置与姿态的描述机器人运动学正问题机器人运动学逆问题机器人的雅可比矩阵2§2.1引言（TheIntroduction）机器人运动学正问题：定义逆问题：定义机器人动力学3基本概念（TheBasicConcepts）自由度：物体能够对坐标系进行独立运动的数目称为自由度(DOF,degreeoffreedom)。刚体具有6个自由度三个旋转自由度R1,R2,R3三个平移自由度T1,T2,T3YXZR1R2R3T1T2T34机动度：DegreeofMobility关节：Joint连杆：Link自由度由机动度构成,机动度不一定是自由度.123455个机动度，2个自由度5§2.2机器人位置与姿态的描述

(TheDescriptionofPositionandPosture)0EGBZ0Tnzyx一个物体与机械手•6位置与姿态的表示位置描述：位置矢量(positionvector)直角坐标系{A},位置矢量Ap矩阵表示矢量和表示矢量的模，单位矢量xAyAzAoApAp7一、机器人坐标系变换(CoordinateTransformation)

Ouvw：Puvw=（Pu,Pv,Pw）TOxyz：Pxyz=（Px,Py,Pz）T

当Ouvw坐标系绕一轴线转动后，均可通过一个3x3旋转矩阵R

将原坐标Puvw变换到Oxyz系中的坐标Pxyz，

即：

Pxyz=R

Puvw

8大家有疑问的，可以询问和交流可以互相讨论下，但要小声点9由矢量分量的定义有：Puvw=puiu+pvjv+pwkw

pu、pv、pw分别表示P沿Ou、Ov、Ow轴的分量

Px=ix˙P=ix˙iupu+ix

˙jvpv+ix˙kwpw

Py=iy˙P=iy˙iupu+iy

˙jvpv+iy˙kwpw

Pz=iz˙P=iz˙iupu+iz

˙jvpv+iz˙kwpw

将上式写成矩阵形式：

Px=ix˙iuix

˙jvix˙kwPu

Py=iy˙iuiy

˙jviy˙kwPv

Pz=iz˙iuiz

˙jviz˙kwPwPxyz=R

Puvw

同样，也有Puvw=QPxyz，Q＝R－1

＝RT10

如果Ouvw坐标系统绕Ox轴转动α角，变换矩阵Rx,α称为绕Ox轴转动α角的旋转矩阵，此时ix=iu，

ix˙iuix

˙jvix˙kw100Rx,α

=iy˙iuiy

˙jviy˙kw=0cosα

-sinα

iz˙iuiz

˙jviz˙kw0sinα

cosα

向量点乘：a·b=|a|·|b|·

cos(a)11类似地，绕Oy轴转动φ角和绕Oz轴转θ角的3×3旋转矩阵分别为，

cosφ

0

sinφ

Ry,φ=

0

1

0-sinφ0cosφcosθ-sinθ

0

Rz,θ

=

sinθ

cosθ

0001

矩阵Rx,α、Ry,φ和Rz,φ称为基本旋转矩阵。

任何旋转变换可以由有限个基本旋转变换合成得到。12依次左乘（如果uvw对xyz旋转）依次右乘（如果uvw绕自己的坐标轴旋转）

R=Rz,θRy,φRx,α13例题：求表示绕Oy轴转φ角，然后绕Ow轴转θ角，再绕Ou轴转α角的合成旋转矩阵。14例题：坐标系{B}的初始位姿与参考坐标系{A}相同，坐标系{B}相对于{A}的zA轴旋转30

，再沿{A}的xA轴移动12，沿{A}的yA轴移动6。求旋转矩阵。解：15二、齐次坐标和变换矩阵齐次坐标是用n+l维坐标来描述n维空间中的位置，其第n+1个分量（元素）ω称为比例因子。

P=(ωPx,ωPy,

ωPz,ω)T

在机器人学的应用中，一般将比例因子取为1。

机器人系统运动分析中，齐次变换矩阵写成以下形式：

T=R3×3

P3×1

=

旋转矩阵3×3

位置矢量3×1

O1×3

I1×1O1×3

116若三维空间的位置矢量P表示成齐次坐标，即

P=pxpypz1T,

1

0

0

0

cosβ

0sinβ

0

Tx，α

=

0cosα

-sinα

0Ty，β=

01

0

00sinαcosα

0-sinβ0cosβ0

00010001cosθ

-sinθ

0

0

100

dx

Tz，θ

=sinθcosθ

0

0Ttran=

01

0

dy

00

1

000

1dz

00010001Pxyz=TPuvw17课前提问：（1）什么是机器人运动学的正问题和逆问题？（2）机器人的坐标变换矩阵的一般形式是什么？（3）连续的变换矩阵，什么情况下依次左乘、什么情况下依次右乘？（4）什么是齐次坐标和齐次变换？18§2.3机器人运动学正问题

(TheForwardKinematicProblem)Denavit–Hartenberg(D-H)表示法191.坐标系的建立：

n关节机器人需建立n+1个坐标系，其中参考(机座)坐标系为O0x0y0z0,，机械手末端的坐标系为Onxnynzn20

串联杆型机械手是由一系列通过连杆与其活动关节连接在一起所组成。如图所示，任何一个连杆都可以用两个量来描述：一个是公共垂线距离an，另一个是与an垂直的平面上两个轴的夹角αn，习惯上称an为连杆长度，αn称为连杆的扭转角。21

如图所示，在每个关节轴上有两个连杆与之相连，即关节轴有两个公垂线与之垂直，每一个连杆一个。两个相连的连杆的相对位置用dn和θn确定，dn是沿着n关节轴两个垂线的距离，θn是在垂直这个关节轴的平面上两个被测垂线之间的夹角，dn和θn分别称作连杆之间的距离及夹角。xn-122

为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。

转动关节：关节变量为θn。连杆n的坐标原点设在关节n和关节n+1轴之间的公共垂线与关节n+1轴的交点上。在关节轴相交的情况下（无公垂线），这个原点就在两个关节轴的相交点上（an＝0）。如果两个关节轴平行（有无数条公垂线），则原点的选择要使下一个连杆的关节距离为0（dn＝0），连杆n的z轴与n+1关节轴在一条直线上。x轴与任何存在的公共垂线成一条直线，并且沿着这条垂线从n关节指向n+1关节。在相交关节的情况下，x轴的方向平行或者逆平行zn-1×zn的向量叉积，应该注意，这个条件对于沿着关节n和n+1之间垂线的x轴同样满足。当xn-1和xn平行，且有相同的指向时，则对于第n个转动关节θn＝0。

表连杆参数23

确定和建立每个坐标系的原则：

(1)zi轴沿着第i关节的运动轴;

(2)xi轴垂直于zi-1轴和zi轴并指向离开zi-1轴的方向;

(3)yi轴按右手坐标系的要求建立。

按照这些规则，第0号坐标在机座上的位置和方向可任选，只要z0轴沿着第1关节运动轴。第n坐标系可放在手的任何部位，只要xn轴与zn-1轴垂直。

24z0z2z3z4x3x2x0x4021z5x5z6x66435z1x1252、几何参数的定义描述串联机器人相邻坐标系之间的关节关系可归纳为如下4个参数：

θi:绕zi-1轴(右手规则)由xi-1轴指向xi轴的关节角;di:从第i-1坐标系的原点到zi-1轴和xi轴的交点沿zi-1轴的距离;ai:从zi-1和xi的交点到第i坐标系原点沿xi轴的偏置距离;

αi:绕xi轴(右手规则)由zi-1轴转向zi轴的偏角。

263、建立i坐标系和i-1坐标系的齐次变换矩阵：27第i坐标系相对于机座齐坐标系的次变换矩阵是各齐次变换矩阵i-1Ai的连乘积：4、得出机器人手爪到机座的变换矩阵280Tn=0A11A2....n-1Ann为手的法向矢量，o为手的滑动矢量，a为手的接近矢量，p为手的位置矢量29例题1：建立二秆机构的末端的变换矩阵同理：30最后得到的变换矩阵为：31z0z2z3z4x3x2x0x4021z5x5z6x66435

D-H参数表：

z1x1例题2:PUMA机器人的坐标变换矩阵32z0z2z3z4x3x2x0x4021z5x5z6x66435

D-H参数表：

z1x13334例题3:斯坦福机械手一、建立坐标系二、D-H参数表三、i-1Ai坐标变换矩阵0123456Z0X0Z1X1Z2X2Z3X3Z4X4Z5X5Z6X635363738斯坦福机械手A变换的积如下所示，这些是从连杆6开始，然后逐步回到基坐标。

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）

000139C4C5C6-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）

0140410Tn=0A11A2....n-1An42课前提问：（1）PUMA机器人的坐标变换矩阵D-H参数？（2）矩阵的逆矩阵如何求解？43定义：若|A|≠0,则方阵A可逆，且A*称为A的伴随阵，由行列式|A|的方阵各个元素的代数余子式所Aij构成。另一种求法：行列式变换44

§2.4机器人运动学逆问题

(TheInverseKinematicProblem)450EGBZ0Tnzyx一个物体与机械手•有向变换图GBET6Z••0Z：机器人基座相对于基础坐标系0Tn：手部端点相对于机器人基座Z：末端执行器相对于手部端点物体用变换B：物体参考坐标系相对于基础坐标系G：末端执行器对物体的抓持位置相对于物体参考坐标系

Z0TnE=BG

这个方程可以用有向变换图来表示。图的每一段弧表示一个变换。从它的定义的坐标系向外指向。用Z-1左乘和用E-1右乘方程，得到

0Tn=Z-1BGE-1机器人坐标变换关系：46

从有向变换图上我们可以直接得到上述结果，从0Tn弧线的尾部开始，沿着图形顺时针依次列出各个变换，直到0Tn弧的箭头为止。在逆变换时，我们从0T6弧的箭头开始，按逆时针方向依次列出各个变换，直到T6弧的起始点为止，则可得到0Tn的逆

0Tn-1=EG-1B-1Z

作为进一步的例子，假设一个物体B的位置不知道，但机械手移动，使得末端抓手正好定位在物体上面。然后用G-1右乘式（2.61）求出B。或者在有向变换图中从B的尾部沿着逆时针方向到达弧B的箭头，直接得到同样结果。

B=Z0TnEG-1

同样，我们可以用有向变换图求出变换的连接组。例如

Z0Tn=BGE-1BGE0T6Z图2.20有向变换图的另一种形式474849

5051同时可确定d3为

用依次左乘方程式可得以下4个方程式52计算得式中53由式中第3行第3列为0可得即解得54由式第1行3列和第2行3列可得解得由第4个方程式可得55类似的有,第1行2列和2行2列对应元素相等得可得56§2.5机器人的雅可比矩阵

(JacobianMatrix)意义：手端在基础坐标中的速度与各关节速度间的关系，以及手部与外界接触力与对应各关节力间的关系。一、雅可比矩阵的定义：

n自由度机器人，其关节变量向量可写为

Q＝

(q1,q2,…,qn)T

，手部在基础坐标中的位置和姿态为P,则：

P

＝(xeyezeθex

θey

θez

)T