第3章机器人运动学

cf sf 0 sf cf 0 0 nx n 0 y 1 nz ox oy oz ax c c s ay az s c c s c s s s 0 c
矩阵两边对应元素相等
第三章 机器人运动学
8
3.1.3 连杆变换矩阵及其乘积
机器人机械手是由一 系列连接在一起的连 杆（杆件）构成的。 需要用两个参数来描 述一个连杆，即公共 法线距离ai和垂直于 ai所在平面内两轴的 夹角 i ；需要另外两 个参数来表示相邻两 杆的关系，即两连杆 的相对位置 d i 和两连 杆法线的夹角 i，如 图3.5所示。
（2，3） （1，3） （3，3） （2，1） （2，2） f=atan2(ay,ax) f= f+ =atan2(cfax+sfay,az) =atan2(-sfnx+cfny, -sfox+cfoy)
3.2 机械手运动方程的求解 18
如果已知一个表示任意旋转的变换，那么就能够确定其等价欧拉角。
3.1 机器人运动方向的表示
3
3.1.1 运动姿态和方向角
用旋转序列表示运动姿态
机械手的运动姿态往往由 一个绕轴x ，y 和 z 的旋转 序列来规定。这种转角的 序列，称为欧拉（Euler） 角。 欧拉角用一个绕 z 轴 旋转f角，再绕新的 y 轴 旋转θ角，最后绕新 z 的 图3.2 欧拉角的定义 轴旋转角ψ 来描述任何可 能的姿态，见图3.2。 在任何旋转序列下，旋转次序是十分重要的，结果见 (3.3)。
第三章 机器人运动学
2
3.1.1 运动姿态和方向角
因此，变换T6具有下列元素。
nx n T6 y nz 0 ox oy oz 0 ax ay az 0 px py pz 1
(3.2)
六连杆机械手的T矩阵（ T6 ）可由指定其16个元素的数 值来决定。在这16个元素中，只有12个元素具有实际含 义。
图3.8 反正切函数atan2
3.2 机械手运动方程的求解
17
3.2.1 欧拉变换解
用显式方程求各角度 要求得方程式的解，采用另一种通常能够导致显式解 答的方法。用未知逆变换左乘已知方程，对于欧拉变 换有： Rot( z,f ) 1T Rot( y, ) Rot( z, ) (3.37)
(4)
沿 zP 轴平移 di
i 1 i
{P}
{i} Trans(0 ,0, di)
T = Rot(x, i-1) Trans(ai-1 ,0,0) Rot(z, i ) Trans(0 ,0, di)
3.1 机器人运动方向的表示 11
3.1.3 连杆变换矩阵及其乘积
Rot(x,
0 1 0 c i 1 i-1) = 0 s i 1 0 0 0 s i 1 c i 1 0 0 0 0 1
, s ax sf a y cf
a tan 2 ax sf a y cf , ox sf oy cf
f atan2(n y , n x ) f f 180 at an2(n z , cfn x sfn y ) at an2( sfa x cfa y , sfo x cfo y )
Euler (f , , ) Rot ( z,f ) Rot ( y, ) Rot ( z, )
3.1 机器人运动方向的表示
4
3.1.1 运动姿态和方向角
用横滚、俯仰和偏转角表示运动姿态 另一种常用的旋转集合是横滚（roll）、俯仰（pitch） 和偏转（yaw）。
ψ
图3.3 用横滚、俯仰和偏转表示机械手运动姿态
(3.16)
3.1 机器人运动方向的表示
12
3.1.3 连杆变换矩阵及其乘积
求图示机器人的 连杆变换矩阵。 假设初始状态基 坐标系与第一个 连杆坐标系重合
i 0 1 2
i-1 0 0 90o
ai 0 0 L2
di 0 0 L1
i 0 1 2
si 0 ai 1 ci s c c c s d s i i 1 i 1 i i 1 i 1 i i 1 T i si si 1 ci s i 1 c i 1 di c i 1 0 0 1 0
3.2 机械手运动方程的求解
19
cf sf 0
sf cf 0
0 nx n 0 y 1 nz
ox oy oz
ax c 0 ay az s
tan f ny nx
s s c c s
s c s c c
Rot(z, i ) =
ci s i 0 0 1 0 0 0
si ci 0 0 0 0
0 0 0 0 1 0 0 1
1 0 Trans(ai-1 ,0,0)= 0 0
0 0 ai 1 1 0 0 0 1 0 0 0 1
0 0 1 0 0 1 0 0
px py [某姿态变换] pz 1
(3.6)
第三章 机器人运动学
7
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵及其乘积 广义连杆 相邻坐标系间及其相应连杆可以用齐次变换矩阵来表 示。要求出操作手所需要的变换矩阵，每个连杆都要 用广义连杆来描述。在求得相应的广义变换矩阵之后， 可对其加以修正，以适合每个具体的连杆。
3.1 机器人运动方向的表示 5
3.1.1 运动姿态和方向角
对于旋转次序，规定：
RPY (f , , ) Rot ( z,f ) Rot ( y, ) Rot ( x, )
(3.4)
式中，RPY表示横滚、俯仰和偏转三旋转的组合变换。也 就是说，先绕 x 轴旋转角 ψ ，再绕 y 轴旋转角θ，最后绕 z 轴旋角f 。
(3.24)
第三章 机器人运动学
15
3.2.1 欧拉变换解
得到9个隐式方程，如下：
nx cf c c sf s n y sf c c cf s 可以由(3.33)求，再由(3.31) 和(3.27)求f和这种方法的不 nz s c 足是解的不求定性和精度问题。
Trans(0 ,0, di)=
0 1 0 0 0 1 di 0 0 1
可得连杆变换通式为 ：
si ci s c i 1 i i 1 ci c i 1 T i si s i 1 ci s i 1 0 0 0 s i 1 c i 1 0 di s i 1 di c i 1 1 ai 1
13
3.2 机械手运动方程的求解
3.1.3 连杆变换矩阵及其乘积
如果机械手与参考坐标系的相对关系是由变换 Z 来 表示的，而且机械手与其端部工具的关系由变换 E 表示，那么此工具端部对参考坐标系的位置和方向 可由变换 X 表示如下：
可求得：
X ZT6 E
T6 Z 1 XE 1
(3.17)
(2,1)相等得 nx sf nycf 0
(
f a tan 2 ny , nx
1,1)(3,1)：c nxcf n y sf , s nz
a tan 2 nz , nxcf n y sf
c ox sf oy cf (2,2)(2,3)：
机器人学基础 1
3.1 机器人运动方程的表示 3.1.1 运动姿态和方向角 机械手的运动方向
原点由矢量p表示。 接近矢量a：z向矢量 方向矢量o：y向矢量 法线矢量n：它与矢量 图3.1 矢量n,o,a和p o和a一起构成一个右手 矢量集合，并由矢量的交乘所规定：n = o a。 n： y向矢量
轴i
θi
轴 i-1
连杆 i
θi-1
连杆 i-1
ai
di
i
ai-1
i-1
图3.5 转动关节连杆四参数示意图
3.1 机器人运动方向的表示 9
3.1.3 连杆变换矩阵及其乘积
3.1 机器人运动方向的表示
10
3.1.3 连杆变换矩阵及其乘积
广义变换矩阵 按照下列顺序建立相邻两连杆i 1与i之间的相对关系。 建立 {P}{Q}{R}3个坐标系，{i}和{i-1}是固定在连杆 i 和 i-1 上的坐标系 (1) 绕 xi-1 轴旋转 i-1角 {i-1} {R} Rot(x, i-1) (2) 沿 xR 轴平移 ai-1 {R} {Q} Trans(ai-1 ,0,0) (3) 绕 zQ 轴旋转 i 角 {Q} {P} Rot(z, i )
3.1 机器人运动方向的表示
14
3.2 机器人运动方程的表示
3.2.1 欧拉变换解
基本隐式方程的解
令
Euler(f , , ) T
(3.23)
由式(3.4)和(3.23)得到：
nx n y nz ox oy oz ax cf c c sf s sf c c sf s ay az s c cf c s sf c sf c s cf c s s cf s sf s c
一般用双变量反正切函数计算 角度。
(3.25) (3.26) (3.27) (3.28) (3.29) (3.30) (3.31) (3.32) (3.33)
ox cf c s sf c o y sf c s cf c oz s s ax cf s a y sf s az c
第三章 机器人运动学 3.1 机器人运动方程的表示 A矩阵：一个描述连杆坐标系间相对平移和旋 转的齐次变换 。 T矩阵：A矩阵的乘积 。 对于六连杆机械手，有下列T矩阵 ：
T6 A1 A2 A3 A4 A5 A6
(3.1)
一个六连杆机械手可具有六个自由度，每个连 杆含有一个自由度，并能在其运动范围内任意 定位与定向。
旋转举阵与Euler角表示的顺序恰好相反！ 关键是转轴是原来的坐标系还是旋转后的坐标系的坐标轴
3.1 机器人运动方向的表示
6
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标 一旦机械手的运动姿态由某个姿态变换规定之后，它在 基系中的位置就能够由左乘一个对应于矢量p的平移变换 来确定：
பைடு நூலகம்
1 0 T6 0 0
0 s 1 0 0 c 1 0 0 c 0 s
s cf 0
0 c 0 s c s
s s c c s
s c s c c
0 1 0 Rot z, f R Rot y, Rot x, 1
3.2.2 滚、仰、偏变换解
可以采用相同的方法直接从显式方程来求解 用滚动、俯仰和偏转表示的变换方程。
RPY (f , , ) Rot ( z,f ) Rot ( y, ) Rot ( x, )
c Ryx 0 s
cf Rot z , f sf 0
3.2 机械手运动方程的求解
16
3.2.1 欧拉变换解
用双变量反正切函数确定角度 在求解时，总是采用双变量反 正切函数atan2(y, x)来确定角 度。atan2提供二个自变量，即 纵坐标和横坐标，见图3.8。 当 -π≤θ≤ π，由atan2反求角 度时，同时检查y和x的符号来 确定其所在象限。这一 函数也能检验什么时候x或y为 0，并反求出正确的角度。 atan2的精确程度对其整个定义 域都是一样的。