第三章 机器人运动学

机器人的位姿
机器人位姿的表示
例：右图所示两坐标系的
姿态为：
ｚ0 ｘ1 ｏ0 ｘ0 ｙ0
ｚ1 ｏ1 ｙ1
0 1 0 1 0 0 R01 0 0 1
机器人的位姿
机器人的坐标系 手部坐标系（手爪坐标系）——机器人手部的坐标系， 也称机器人位姿坐标系，它表示机器人手部在指定坐 标系中的位置和姿态。 杆件坐标系（参考坐标系）——机器人指定杆件的坐 标系，它是在机器人每个活动杆件上固定的坐标系， 随杆件的运动而运动。 机座坐标系（基准坐标系）——机器人机座的坐标系， 它是机器人各活动杆件及手部的公共参考坐标系。
例：已知B坐标系的初始位置与A坐 标系重合，首先把B坐标系沿A坐标 系的x轴移动12个单位，并沿y轴移 动6个单位，再绕z轴旋转30°，求 平移变换矩阵和旋转变换矩阵。假 设某点在B坐标系中的矢量 r 5i 9 j 0k 求该点在A坐标系中的矢量。
B
例：已知B坐标系的初始位置与A坐标系重合，首先把B坐标 系沿A坐标系的x轴移动12个单位，并沿y轴移动6个单位，再 绕z轴旋转30°，求平移变换矩阵和旋转变换矩阵。假设某点 在B坐标系中的矢量 5i 9 0k ，求该点在A坐标系中的矢 rB j 量。 解：由题意可得平移变换矩阵和旋转变换矩阵分别为：
cos 30 12 6， p AB RAB sin30 0 0 则：
sin30 cos 30 0
0 0.866 0.5 0 0 0.5 0.866 0 1 0 0 1
12 0.866 0.5 0 5 11.830 rA p AB RAB rB 6 0.5 0.866 0 9 13.794 0 0 0 1 0 0
用向量表示这个关系式，其一般可表示为
式中
f
r f ( ) 表示向量函数。
(3-3)
已知机器人的关节变量 ，求其手爪位置的 运动学问题称为正运动学（direct kinematics）。
该公式被称为运动方程式。
运动学基本问题--逆问题 如果，给定机器人的手爪位置，求 为了到达这个预定的位置，机器人的关 节变量的运动学问题称为逆运动学 （inverse kinematics）。 其运动方程式以2自由度机械手为 例，通过以下分析说明。
1 0 则：M p Trans( p x , p y , pz ) 0 0 0 1 0 0 0 0 1 0 px py E pz 0 1 pij 1
齐次坐标变换
单独旋转变换的齐次矩阵为：
cos sin 0 sin cos 0
机器人的位姿
机器人的坐标系 手部坐标系{h} 机座坐标系{0} 杆件坐标系{i} i=1，…，n
第3章 机器人运动学
三、齐次变换及运算
齐次变换及运算
直角坐标变换
坐标之间的变换关系
平移变换
ｚj ｚi ｏi
旋转变换
ｘi
ｘj
ｏj
ｙj
ｙi
直角坐标变换
齐次变换及运算
平移变换 设i坐标系和j坐标系具有相同的姿态，但它俩 的坐标原点不重合，若空间有一点在i坐标系和j 坐标系中分别用矢量 和 r 表示，则它们之 ri j 间有以下关系： ｚj
式中，M ij ——齐次坐标变换矩阵， 它是一个4×4的矩阵。
齐次坐标变换
齐次坐标变换矩阵的意义 若将齐次坐标变换矩阵分块，则有：
cos sin M ij 0 0
sin cos 0 0
0 0 1 0
px z , py Rij pz 0 1
xi p x cos y p sin i y z i pz 0
sin cos 0
0 x j y 0 j 1 z j
直角坐标变换
齐次变换及运算
如图为逆运动学问题（知位置，求分量），可得
2
(3-4)
L2 sin 2 y 1 arctan( ) arctan( ) x L1 L2 cos 2
式中
(3-5)
2 ( x 2 y 2 ) L1 L2 2 arccos 2 L1 L2
3 0 1 0 0 0 1 5 1 0 0 9 0 1 0 0
第3章 机器人运动学
四、机器人运动学方程
机器人运动学方程
运动学方程建立步骤 （1）建立坐标系 （2）写位姿矩阵 （3）建立方程
机器人运动学方程
x i cos y si n i zi 0 1 0
si n cos 0 0
0 0 1 0
px x j y py j pz z j 1 1
齐次坐标变换
i坐标系通过先平移，后绕z旋转 变成j坐标系，则变换矩阵为：
pij 1
意义：左上角的3×3矩阵是两个坐标系之间的旋转变 换矩阵，它描述了姿态关系；右上角的3×1矩阵是 两个坐标系之间的平移变换矩阵，它描述了位置关 系，所以齐次坐标变换矩阵又称为位姿矩阵。
齐次坐标变换
单独平移变换的齐次矩阵为：
px 已知：pij p y p z
ｐ(ｘ,ｙ,ｚ)
ｏ ｘ
ｙ
机器人的位姿
姿态可以用坐标系三个 坐标轴两两夹角的余弦值（ 三个h坐标轴的单位矢量）组
ｏ ｘ ｚ ｚh ｘh ｏ ｐ(ｘ,ｙ,ｚ) h ｙh ｙ
成3×3的姿态矩阵来描述。
cos(x , x h ) cos(x , yh ) cos(x , z h ) cos(y , x ) cos(y , y ) cos(y , z ) R h h h cos(z , x h ) cos(z , yh ) cos(z , z h )
x r y
1 2
因此，利用上述两个向量来 描述一下这个2自由度机器 人的运动学问题。 手爪位置的各分量， x L1 cos1 L2 cos(1 2 ) 按几何学可表示为：
y L1 sin 1 L2 sin(1 2 )
直角坐标变换
xi p x cos y p sin i y z i pz 0
sin cos 0
0 x j y 0 j 1 z j
齐次变换矩阵（D-H矩阵）
sin cos 0 0 0 0 1 0
z 已知：Rij,
0 0 1
0 z,θ 0 Rij 0 0 1
则：M zR
cos sin Rot( z , ) 0 0
0 1
齐次坐标变换
当空间有任意多个坐标系时，若已 知相邻坐标系之间的齐次坐标变换 矩阵，则由坐标变换原理可知：
运动学基本问题
数学模型：
手的运动→位姿变化→位姿矩阵r
关节运动→参数变化→关节变量θ， 运动学方程： r=f(θ) 正问题：已知θ，求r。 逆问题：已知r，求θ 。
运动学基本问题
运动学基本问题--正问题 以2自由度为例
图3－1所示为2自由度机器人手部的连杆机构。
我们引入向量分别表示手爪 位置和关节变量，
M AB Trans(3,5,9) Rot( x A ,90 ) Rot( z A ,90 )
1 0 0 0 3 1 0 1 0 5 0 cos90 0 1 9 0 sin90 0 0 1 0 0 0 0 0 sin90 cos90 0 0 cos90 0 sin90 0 0 1 0 sin90 cos90 0 0 0 0 0 0 1 0 0 1
{n}
M0n M01 M12 Mi 1i Mn1n
由此可知，建立机器人的坐标系， 可以通过齐次坐标变换，将机器人 手部在空间的位置和姿态用齐次坐 标变换矩阵描述出来，从而建立机 器人的运动学方程。
{i}
{i-1}
{0}
齐次坐标变换
例：已知B坐标系是绕A坐标系的z轴旋转90°，再绕x轴旋转 90°，最后沿矢量 pA 3i 5 j 9k 平移得到的，求A坐标系 与B坐标系之间的齐次坐标变换矩阵。 解：由于变换始终是相对于原来的参考坐标系，即有：
旋转变换
齐次变换及运算
——旋转变换矩阵，是一个3×3的矩阵，其中的每个 元素就是i坐标系和j坐标系相应坐标轴夹角θ的余弦值，它 表明了姿态（方向）。θ角的正负按右手法则确定，即由轴 的矢端看，逆时钟为正。
直角坐标变换
齐次变换及运算
联合变换 设i坐标系j和坐标系之间存在先平移变换， 后旋转变换，则空间任一点在i坐标系和j坐 标系中的矢量之间就有以下关系：
(3-6)
同样，如果用向量表示上述关系式，其一般可表示
为
f (r )
1
(3-7)
如图所示，机器人到达给定的手爪位置
r
有两个姿态满足要求，即图中的 也是其解。 这时 1和 2 变成为另外的值。即逆运动学的解不是惟一 的，可以有多个解。
第3章 机器人运动学
二、机器人的位置和姿态
机器人的位置和姿态
机器人位姿的表示 机器人的位姿主要是指机 器人手部在空间的位置和姿态 ，有时也会用到其它各个活动 杆件在空间的位置和姿态。
机器人的位姿
机器人位姿的表示
位置可以用一个3×1的位置矩阵来描述。
ｚ
px x p py y p z z
ri pij rj
称上式为坐标平移方程。
ｘi
ｚi ｏi
ri r j
pij
ｘj
ｙi
ｏj
ｙj
直角坐标变换
齐次变换及运算
旋转变换 设i坐标系和j坐标系的原点重合，但它俩的姿态不 同，则j坐标系就可以看成是由i坐标系旋转变换而来的。
ｚi
ｚj
ｙj ｏi ｘi ｏj
ｙi
ｘj
直角坐标变换
第三章 机器人运动 学
第3章 机器人运动学
一、机器人运动学基本问题 二、机器人的位置和姿态 三、齐次变换及运算 四、机器人运动学方程 五、运动学方程求解 六、微分变换和雅可比矩阵
第3章 机器人运动学
一、机器人运动学基本问题
运动学基本问题
运动学研究的问题： 手在空间的运动与各个 关节的运动之间的关系。 正问题：已知关节运动，求 手的运动。 逆问题：已知手的运动，求 关节运动。
ｚ ｏ ｘ ｙ
cos si n M ij 0 0
Байду номын сангаас
si n cos 0 0
0 0 1 0
px py pz 1
i j
齐次坐标变换
齐次变换矩阵（D-H矩阵） 由此可得联合变换的齐次坐标方程为：
ri rj M ij 1 1
ri pij Rij rj
称上式为直角坐标系中的坐标联合变换方程。
例
若j坐标系是i坐标系先沿矢量 pij pxi py j pz k 平移，再绕z轴旋转θ角得到的，则空间任一点在i 坐标系和j坐标系中的矢量和对应的变换矩阵之间 z 就有 ri pij Rij, rj ，写成矩阵形式则为：