第三章机器人运动学
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
T2= A1 A2
同理，若A3表示第三个连杆相对于第二个连杆的位置和姿态，则有
T3= A1 A2 A3
称这些A矩阵的乘积为T矩阵，其前置上标若为0，则可省略。对于六 连杆机械手，有下列T矩阵
T6= A1 A2 A3 A4 A5 A6
手爪坐标系
机械手的运动方向
原点由矢量p表示。
图3.1 矢量n,o,a和p
c5 s5 0 0
A5
0
s5 0
0 c5 0
1 0 0 0 0 1
式中 ：ci=cosi si=sini
c6 s6 0 0
A6
0
0s6
0 c6
0
1
0
0 0
0 1
§ 3.2 机器人运动学正问题 机器人运动学正问题是已知机器人各关节、各连杆 参数及各关节变量，求机器人手端坐标在基础坐标中的 位置和姿态。
第三章 机器人运动学
§ 3.1 机器人运动方程的表示
机器人的机械手看作是一系列由关节连接起来的连杆构成的。为机 械手的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间 的相对位置和姿态。通常把描述一个连杆与下一个连杆间相对关系的 齐次变换叫做A矩阵。一个A矩阵就是一个描述连杆坐标系间相对平移 和旋转的齐次变换。如果A1表示第一个连杆对于基系的位置和姿态， A2表示第二个连杆相对于第一个连杆的位置和姿态，则第二个连杆在 基系中的位置和姿态可由下列矩阵的乘积给出
坐标系原点沿x i轴的偏移距离(是z i-1轴和zi两轴间的最小距离） i ：绕x i轴（右手规则）由z i-1轴转向zi轴的偏角。 对于转动关节，di、ai、αi是关节参数，θi是关节变量。 移动关节的关节参数是θi、 ai、αi， di是关节变量。
3.1.3 建立i坐标系和i-1 坐标系的齐次变换矩阵
将第i个坐标系表示的点ri在 i-1坐标系表示，需建立i坐标系 和i—1坐标系的齐次变换矩阵 ，需经过以下交换： (1)将坐标系Oi-1xi-1yi-1zi-1绕zi-1轴 转θi角，使xi-1轴与xi平行并指向 同一方向； (2)将坐标系Oi-1xi-1yi-1zi-1沿zi-1轴 平移距离di ，使xi-1轴与Oixiyizi 的xi轴重合；
固联坐标系后置（zi位于 i+1关节轴上），变换公式
i1Ai Rot(zi1,i )Trans(0,0, di )Trans(ai ,0,0)Rot(xi ,i )
cosi sini 0 01 0 0 0 1 0 0 ai 1 0
0 0
A i1 i
sin
0
0 1 0
c2 0 0
0
d2 1
1 0 0 0
A3
0 0
0
1 0 0
0 1 0
0
d3 1
c4 0 s4 0
A4
s4 0
0 1
c4 0
0 0
0
θi :绕z i轴由x i-1轴向x i轴的关节角；
di：从x i-1 轴和z i轴的交点到第i坐标系的原点沿z i 轴的距离
固联坐标系前置（zi位于i关节轴上），变换公式
A i 1 i
Rot ( xi1 , i1 )Trans( xi1 , ai1 )Trans(zi , di )Rot (zi , i )
接近矢量a：z轴设在手指接近物体的方向，称为接近矢量
方向矢量o：y轴设在两手指的连线方向，称为方位矢量
法线矢量n：x轴由右手系确定， 即 n = o a ，称为法向矢量。
因此，变换T6具有下列元素。
nx ox ax px
T6
ny
nz
oy oz
ay az
p
y
pz
c2 s2 0 0
A2
0
0s2
0 c2
0
1 0 0
d
2
0
1
c3 s3 0 a2
A3
s3
0
c3 0
0
0
1 0
0
0
0
1
c4 s4 0 a3
A4
0
0s4
0 c4
0
1 0 0
d
4
0
1
坐标系的建立有两种方式： 固联坐标系后置 固联坐标系前置
固联坐标系后置
n关节机器人需建立n+1个坐标
系，其中参考(机座)坐标系为
O0x0y0z0 ，机械手末端的坐标系为 Onxnynzn，第i关节上的坐标系为Oi1x i-1y i-1z i-1。坐标系Si置于连杆 Li的远离基座的关节上，故称固联 坐标系后置。
两杆间的距离di：关节轴上两个法线的距离
夹角θi：关节轴上两个法线的夹角
Ai-1
Ai
ai
ai1
di
i
ai
i
3.1.1 Denavt-Hartenberg(D-H)表示法
为 描 述 相 邻 杆 件 间 平 移 和 转 动 的 关 系 。 Denavt 和 Hartenberg (1955)提出了一种为关节链中的每一杆件建立 附体坐标系的矩阵方法。D-H方法是为每个关节处的杆件坐 标系建立4 4齐次变换矩阵，表示它与前一杆件坐标系的关 系。这样逐次变换，用“手部坐标”表示的末端执行器可被 变换并用机座坐标表示。
s1c2 c1s2
s1s2 c1c2
l2
(s1c2
c1s2
)
l1s1
s12
c12
l2 s12
l1s1
0
0
1
0 0
1
式中，c12=cos(1+ 2)， s12=sin(1+ 2)
容易验证上式的正确性，即：末端位置为[ 姿态为1+ 2 ；
连杆i 1 2 3 4 5 6
θi
θ1(90°) θ2(0°)
θ3(-90°) θ4(0°)
θ5(0°) θ6(0°)
αi-1
ai-1
di
0°
0
0
-90°
0
d2
0°
a2
0
-90°
a3
d4
90°
0
0
-90°
0
0
c1 s1 0 0
A1
s1 0
c1 0
0 0 1 0
0
0
0 1
i
0Ti A1A2 A3 A4 A5 A6 Aj j 1
ni
0
oi 0
ai 0
pi 1
0 Ri 0
0 Pi 1
固联坐标系前置
连杆Li的固联坐标系Si的zi轴 置于i关节的旋转（或移动）轴 上，即坐标系Si置于连杆Li的靠
近基座的关节上，故称固联坐 标系前置。
(3)将坐标系Oi-1xi-1yi-1zi-1沿xi轴平移 距离ai ，使两坐标系的原点重合； (4)将坐标系Oi-1xi-1yi-1zi-1绕xi轴转αi 角，使两坐标系完全重合。
i坐标系和i—l坐标系的 齐次变换矩阵i-1Ai可以根据矩 阵的合成规则得到，i-1Ai称为 相邻坐标系i和i—1的D-H变换 矩阵。即
0
0
0
1
六连杆机械手的T矩阵（ T6 ）可由指定其16个元素的 数值来决定。在这16个元素中，只有12个元素具有实际 含义。
3.1.1 Denavt-Hartenberg(D-H)表示法
机械手由一串用转动或平移关节连
接的刚体(杆件)组成。每一对关节–杆件
构成一个自由度。杆件的编号由手臂的 固定基座开始，固定基座可看成杆件0， Ai
i
0
0
cosi sini cosi cosi
sin i 0
sin i sini sin i cosi
c os i 0
ai cosi
ai
s
in
i
di 1
对于在第i坐标系中的点ri在第i—1坐标系中表示为:
ri1 i1Airi
确定第i坐标系相对于机座坐标系的位置的齐次变换矩阵i-1Ti是 各齐次变换矩阵Ai的连乘积，可表示成
sin i 0
sin i sin i sin i cosi
cosi 0
ai cosi
ai
sin
i
di 1
可得如下变换矩阵：
c1 0 s1 0
A1
s1 0
0 1
c1 0
0 0
0
0
0 1
c2 0 s2 0
A2
s2 0
确定和建立每个坐标系应根据 下面3条规则:
转动关节连杆四参数示意图
①z i-1轴沿着第i关节的运动轴； ②x i轴垂直于z i-1轴和z i轴并指向 离开z i-1轴的方向 ③y i轴按右手坐标系的要求建立。
按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要z0轴沿 着第i关节运动轴。第n坐标系可放在手的任何部位、只要xn轴与zn-1轴垂直。
l2c12
l1c1
；l2s12 l2s1
]T ,
第三章 机器人运动学
例 建立下图所示PUMA机器人相邻坐标系间的转换矩阵 。
PUMA 560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕 参考点的位置，后3个关节确定手腕的方位。
第三章 机器人运动学
PUMA机器人的连杆及关节参数表
第一个运动体是杆件1，依次类推，最后
一个杆件与工具相连；关节1处于连接杆
件1和基座之间，每个杆件至多与另外两
个杆件相联，而不构成闭环。
ai
任何杆件i都可以用两个尺度表征，如上图所示，
杆件i的长度ai，是杆件上两个关节轴线的最短距离； 杆件i的扭转角αi ，是两个关节轴线的夹角。
Ai+1
i
两个杆件的相对位置由两个参数决定：
i
0
0
c os i 0 0
0
00
1
0
0 0
1
0
0
0
c os i
sin i
0
1 0
00 10
0 0
1 0
di 1
0 0
0 0
1 0
0 0
1
0
sin i 0
c os i 0
0 1
cosi
s
in
[例]确定下图所示机器人的位置和姿态 解：用D—H法建立坐标系转换矩阵，首先 列出各连轩及关节参数，如下表所示。
斯坦福机器人的连杆及关节参数表
斯坦福机器人及其坐标系图
将表中的参数分别代入i坐标系和i—l坐标系的齐次变换矩阵i-1Ai
c osi
i1 Ai
s
in
i
0
0
cosi sin i cosi cosi
机器人机械手上坐标系的配置取决于机械手连杆连接的类型。有两种连接—— 转动关节和棱柱联轴节。现在来考虑棱柱联轴节（平动关节）的情况。下图示
出其特征参数 , d和 。
棱柱关节连杆四参数示意图 9
3.1.2 几何参数定义
3.1.2 几何参数定义
根据上述对杆件参数及坐标 系的定义，描述串联机器人相邻 坐标系之间的关节关系可归结如 下4个参数： θi：绕z i-1轴（右手规则）由x i-1轴向x i轴的关节角； di：从第i—1坐标系的原点到z i-1轴和x i轴的交点沿z i-1轴的距离； ai：从z i-1和x i轴的交点到第i
解：建立的坐标系如右图，这是二维坐 标系(在三维空间中，各坐标系的z轴垂 直于纸面)，其相邻坐标系的变换矩阵 为
c1 s1 01 0 l1 c1 s1 l1c1
A1 R T z, x,l1 s1
c1
00 1 0 s1
c1
l1s1
0 0 10 0 1 0 0 1
cos i
sin i cos i1
si
n
i
si 0
n
i 1
sin i
cos i cos i1 cos i sin i1
0
0
sin i1 cos i1
0
a i 1
d
i
si
Байду номын сангаасn
i
1
di cos i1
1
例 建立右图所示机器人相邻坐标 系间的转换矩阵
zi-1与zi的公垂线为xi-1, zi与zi+1的公垂线为xi轴,
αi-1:为zi-1与zi的交错角；
αi:绕x i轴（右手规则）由z i轴转向zi+1轴的偏角；
ai-1：从第i-1坐标系原点到x i-1轴和z i的交点沿x i-1 轴的偏移距离
ai：从第i坐标系原点到x i轴和z i+1的交点沿x i轴的 偏移距离
c2 s2 l2c2
A2
s2
c2
l2
s2
0 0 1
c1 s1 l1c1c2 s2 l2c2
T A1A2 s1
c1
l1s1
s2
c2
l2
s2
0 0 1 0 0 1
c1c2 s1s2 c1s2 s1c2 l2 (c1c2 s1s2 ) l1c1 c12 s12 l2c12 l1c1
同理，若A3表示第三个连杆相对于第二个连杆的位置和姿态，则有
T3= A1 A2 A3
称这些A矩阵的乘积为T矩阵，其前置上标若为0，则可省略。对于六 连杆机械手，有下列T矩阵
T6= A1 A2 A3 A4 A5 A6
手爪坐标系
机械手的运动方向
原点由矢量p表示。
图3.1 矢量n,o,a和p
c5 s5 0 0
A5
0
s5 0
0 c5 0
1 0 0 0 0 1
式中 ：ci=cosi si=sini
c6 s6 0 0
A6
0
0s6
0 c6
0
1
0
0 0
0 1
§ 3.2 机器人运动学正问题 机器人运动学正问题是已知机器人各关节、各连杆 参数及各关节变量，求机器人手端坐标在基础坐标中的 位置和姿态。
第三章 机器人运动学
§ 3.1 机器人运动方程的表示
机器人的机械手看作是一系列由关节连接起来的连杆构成的。为机 械手的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间 的相对位置和姿态。通常把描述一个连杆与下一个连杆间相对关系的 齐次变换叫做A矩阵。一个A矩阵就是一个描述连杆坐标系间相对平移 和旋转的齐次变换。如果A1表示第一个连杆对于基系的位置和姿态， A2表示第二个连杆相对于第一个连杆的位置和姿态，则第二个连杆在 基系中的位置和姿态可由下列矩阵的乘积给出
坐标系原点沿x i轴的偏移距离(是z i-1轴和zi两轴间的最小距离） i ：绕x i轴（右手规则）由z i-1轴转向zi轴的偏角。 对于转动关节，di、ai、αi是关节参数，θi是关节变量。 移动关节的关节参数是θi、 ai、αi， di是关节变量。
3.1.3 建立i坐标系和i-1 坐标系的齐次变换矩阵
将第i个坐标系表示的点ri在 i-1坐标系表示，需建立i坐标系 和i—1坐标系的齐次变换矩阵 ，需经过以下交换： (1)将坐标系Oi-1xi-1yi-1zi-1绕zi-1轴 转θi角，使xi-1轴与xi平行并指向 同一方向； (2)将坐标系Oi-1xi-1yi-1zi-1沿zi-1轴 平移距离di ，使xi-1轴与Oixiyizi 的xi轴重合；
固联坐标系后置（zi位于 i+1关节轴上），变换公式
i1Ai Rot(zi1,i )Trans(0,0, di )Trans(ai ,0,0)Rot(xi ,i )
cosi sini 0 01 0 0 0 1 0 0 ai 1 0
0 0
A i1 i
sin
0
0 1 0
c2 0 0
0
d2 1
1 0 0 0
A3
0 0
0
1 0 0
0 1 0
0
d3 1
c4 0 s4 0
A4
s4 0
0 1
c4 0
0 0
0
θi :绕z i轴由x i-1轴向x i轴的关节角；
di：从x i-1 轴和z i轴的交点到第i坐标系的原点沿z i 轴的距离
固联坐标系前置（zi位于i关节轴上），变换公式
A i 1 i
Rot ( xi1 , i1 )Trans( xi1 , ai1 )Trans(zi , di )Rot (zi , i )
接近矢量a：z轴设在手指接近物体的方向，称为接近矢量
方向矢量o：y轴设在两手指的连线方向，称为方位矢量
法线矢量n：x轴由右手系确定， 即 n = o a ，称为法向矢量。
因此，变换T6具有下列元素。
nx ox ax px
T6
ny
nz
oy oz
ay az
p
y
pz
c2 s2 0 0
A2
0
0s2
0 c2
0
1 0 0
d
2
0
1
c3 s3 0 a2
A3
s3
0
c3 0
0
0
1 0
0
0
0
1
c4 s4 0 a3
A4
0
0s4
0 c4
0
1 0 0
d
4
0
1
坐标系的建立有两种方式： 固联坐标系后置 固联坐标系前置
固联坐标系后置
n关节机器人需建立n+1个坐标
系，其中参考(机座)坐标系为
O0x0y0z0 ，机械手末端的坐标系为 Onxnynzn，第i关节上的坐标系为Oi1x i-1y i-1z i-1。坐标系Si置于连杆 Li的远离基座的关节上，故称固联 坐标系后置。
两杆间的距离di：关节轴上两个法线的距离
夹角θi：关节轴上两个法线的夹角
Ai-1
Ai
ai
ai1
di
i
ai
i
3.1.1 Denavt-Hartenberg(D-H)表示法
为 描 述 相 邻 杆 件 间 平 移 和 转 动 的 关 系 。 Denavt 和 Hartenberg (1955)提出了一种为关节链中的每一杆件建立 附体坐标系的矩阵方法。D-H方法是为每个关节处的杆件坐 标系建立4 4齐次变换矩阵，表示它与前一杆件坐标系的关 系。这样逐次变换，用“手部坐标”表示的末端执行器可被 变换并用机座坐标表示。
s1c2 c1s2
s1s2 c1c2
l2
(s1c2
c1s2
)
l1s1
s12
c12
l2 s12
l1s1
0
0
1
0 0
1
式中，c12=cos(1+ 2)， s12=sin(1+ 2)
容易验证上式的正确性，即：末端位置为[ 姿态为1+ 2 ；
连杆i 1 2 3 4 5 6
θi
θ1(90°) θ2(0°)
θ3(-90°) θ4(0°)
θ5(0°) θ6(0°)
αi-1
ai-1
di
0°
0
0
-90°
0
d2
0°
a2
0
-90°
a3
d4
90°
0
0
-90°
0
0
c1 s1 0 0
A1
s1 0
c1 0
0 0 1 0
0
0
0 1
i
0Ti A1A2 A3 A4 A5 A6 Aj j 1
ni
0
oi 0
ai 0
pi 1
0 Ri 0
0 Pi 1
固联坐标系前置
连杆Li的固联坐标系Si的zi轴 置于i关节的旋转（或移动）轴 上，即坐标系Si置于连杆Li的靠
近基座的关节上，故称固联坐 标系前置。
(3)将坐标系Oi-1xi-1yi-1zi-1沿xi轴平移 距离ai ，使两坐标系的原点重合； (4)将坐标系Oi-1xi-1yi-1zi-1绕xi轴转αi 角，使两坐标系完全重合。
i坐标系和i—l坐标系的 齐次变换矩阵i-1Ai可以根据矩 阵的合成规则得到，i-1Ai称为 相邻坐标系i和i—1的D-H变换 矩阵。即
0
0
0
1
六连杆机械手的T矩阵（ T6 ）可由指定其16个元素的 数值来决定。在这16个元素中，只有12个元素具有实际 含义。
3.1.1 Denavt-Hartenberg(D-H)表示法
机械手由一串用转动或平移关节连
接的刚体(杆件)组成。每一对关节–杆件
构成一个自由度。杆件的编号由手臂的 固定基座开始，固定基座可看成杆件0， Ai
i
0
0
cosi sini cosi cosi
sin i 0
sin i sini sin i cosi
c os i 0
ai cosi
ai
s
in
i
di 1
对于在第i坐标系中的点ri在第i—1坐标系中表示为:
ri1 i1Airi
确定第i坐标系相对于机座坐标系的位置的齐次变换矩阵i-1Ti是 各齐次变换矩阵Ai的连乘积，可表示成
sin i 0
sin i sin i sin i cosi
cosi 0
ai cosi
ai
sin
i
di 1
可得如下变换矩阵：
c1 0 s1 0
A1
s1 0
0 1
c1 0
0 0
0
0
0 1
c2 0 s2 0
A2
s2 0
确定和建立每个坐标系应根据 下面3条规则:
转动关节连杆四参数示意图
①z i-1轴沿着第i关节的运动轴； ②x i轴垂直于z i-1轴和z i轴并指向 离开z i-1轴的方向 ③y i轴按右手坐标系的要求建立。
按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要z0轴沿 着第i关节运动轴。第n坐标系可放在手的任何部位、只要xn轴与zn-1轴垂直。
l2c12
l1c1
；l2s12 l2s1
]T ,
第三章 机器人运动学
例 建立下图所示PUMA机器人相邻坐标系间的转换矩阵 。
PUMA 560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕 参考点的位置，后3个关节确定手腕的方位。
第三章 机器人运动学
PUMA机器人的连杆及关节参数表
第一个运动体是杆件1，依次类推，最后
一个杆件与工具相连；关节1处于连接杆
件1和基座之间，每个杆件至多与另外两
个杆件相联，而不构成闭环。
ai
任何杆件i都可以用两个尺度表征，如上图所示，
杆件i的长度ai，是杆件上两个关节轴线的最短距离； 杆件i的扭转角αi ，是两个关节轴线的夹角。
Ai+1
i
两个杆件的相对位置由两个参数决定：
i
0
0
c os i 0 0
0
00
1
0
0 0
1
0
0
0
c os i
sin i
0
1 0
00 10
0 0
1 0
di 1
0 0
0 0
1 0
0 0
1
0
sin i 0
c os i 0
0 1
cosi
s
in
[例]确定下图所示机器人的位置和姿态 解：用D—H法建立坐标系转换矩阵，首先 列出各连轩及关节参数，如下表所示。
斯坦福机器人的连杆及关节参数表
斯坦福机器人及其坐标系图
将表中的参数分别代入i坐标系和i—l坐标系的齐次变换矩阵i-1Ai
c osi
i1 Ai
s
in
i
0
0
cosi sin i cosi cosi
机器人机械手上坐标系的配置取决于机械手连杆连接的类型。有两种连接—— 转动关节和棱柱联轴节。现在来考虑棱柱联轴节（平动关节）的情况。下图示
出其特征参数 , d和 。
棱柱关节连杆四参数示意图 9
3.1.2 几何参数定义
3.1.2 几何参数定义
根据上述对杆件参数及坐标 系的定义，描述串联机器人相邻 坐标系之间的关节关系可归结如 下4个参数： θi：绕z i-1轴（右手规则）由x i-1轴向x i轴的关节角； di：从第i—1坐标系的原点到z i-1轴和x i轴的交点沿z i-1轴的距离； ai：从z i-1和x i轴的交点到第i
解：建立的坐标系如右图，这是二维坐 标系(在三维空间中，各坐标系的z轴垂 直于纸面)，其相邻坐标系的变换矩阵 为
c1 s1 01 0 l1 c1 s1 l1c1
A1 R T z, x,l1 s1
c1
00 1 0 s1
c1
l1s1
0 0 10 0 1 0 0 1
cos i
sin i cos i1
si
n
i
si 0
n
i 1
sin i
cos i cos i1 cos i sin i1
0
0
sin i1 cos i1
0
a i 1
d
i
si
Байду номын сангаасn
i
1
di cos i1
1
例 建立右图所示机器人相邻坐标 系间的转换矩阵
zi-1与zi的公垂线为xi-1, zi与zi+1的公垂线为xi轴,
αi-1:为zi-1与zi的交错角；
αi:绕x i轴（右手规则）由z i轴转向zi+1轴的偏角；
ai-1：从第i-1坐标系原点到x i-1轴和z i的交点沿x i-1 轴的偏移距离
ai：从第i坐标系原点到x i轴和z i+1的交点沿x i轴的 偏移距离
c2 s2 l2c2
A2
s2
c2
l2
s2
0 0 1
c1 s1 l1c1c2 s2 l2c2
T A1A2 s1
c1
l1s1
s2
c2
l2
s2
0 0 1 0 0 1
c1c2 s1s2 c1s2 s1c2 l2 (c1c2 s1s2 ) l1c1 c12 s12 l2c12 l1c1