当前位置:首页 >> 机械/仪表 >>

机器人技术第三章机器人运动学及其数学基础


I. 机器人学
? 机器人学

? 机械电子工程
? Dr. Kevin Craig

I. 机器人学
? IEEE International Conference on Robotics and Automation (ICRA) 2010 安克雷奇 文章: 分会场: 国家: 856/2034 154

47

? IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2009 圣路易斯

文章: 分会场: 国家:

936/1599 192 53

I. 机器人学
? Technical Session的主要内容
? Human robot interaction ? Medical robotics ? Sensor fusion ? Legged robots ? Underwater robots ? Manipulator motion planning ? Camera calibration ? Intelligent transportation systems ? Humanoid robot body motion ? Microrobots ? Biologically-inspired robotic devices ? Rehabilitation robotics ? Field robotics ? Grasping ? Nanorobotic manipulation ? Fish-like robot

? SLAM: Features and landmarks

? Parallel robot …………

第二章 机器人运动学及其数学基础

参考教材
? [美]付京逊《机器人学》

? [中南大学]蔡自兴《机器人学》
? [美]理查德·鲍尔《机器人操作手·数 学·编程与控制》

参考教材
? [美]付京逊《机器人学》
? 美籍华人 ? 普渡大学(Purdue University)电机工程专业著 名教授 ? 4部著作、400多篇论文 ? 第一任国际模式识别学会会长,被誉为自动模 式识别之父 ? 1985年去世

参考教材
? [中南大学]蔡自兴
? 中南大学教授,我国人工 智能和机器人领域著名专 家 ? 中国人工智能学会智能机 器人专委会理事长 ? 曾与付京逊教授一起工作 过

第一节
引言

? 串联机器人可以用一个开环关节链来建模
? 由数个驱动器驱动的转动或移动关节串联而成 ? 一端固定在基座上,另一端是自由的,安装工具(末端 执行器),用以操纵物体,或完成各种任务

? 关节的相对运动导致杆件的运动, 使末端执行器定位于所需要的方 位上 ? 在一般机器人应用问题中,人们 感兴趣的是:末端执行器相对于 固定参考坐标数的空间几何描述, 也就是机器人的运动学问题
? 机器人的运动学即是研究机器人 手臂末端执行器位置和姿态与关 节变量空间之间的关系
o
?i

n

a

运动学研究的问题
Where is my hand?

运动学正问题
Direct Kinematics HERE!

How do I put my hand here?

运动学逆问题
Inverse Kinematics: Choose these angles!

研究运动学的方法
? 哈佛大学Roger Brockett建立的指数积公 式
–运动学 –滚动接触 –非完整控制 –数学基础-刚体运动

参考文献:机器人操作的数学导论 作者:理查德·摩雷 李泽湘 夏卡恩·萨斯特里 翻译:徐卫良 钱瑞明(东南大学)

? 1955年丹纳维特(Denavit)和哈顿伯格(Hartenberg)提 出了一种采用矩阵代数方法解决机器人的运动学问题—D-H 方法,其数学基础即是齐次变换
– 具有直观的几何意义 – 能表达动力学、计算机视觉和 比例变换问题 – 为以后的比例变换、透视变换 等打下基础

?? x ? x ?? ? y y ? T? ??z ? z ? ?0 0

wx wy wz 0

px ? py ? ? pz ? ? 1?

第二节 数学基础—齐次坐标和齐次变换

2.1 点和面的齐次坐标
2.1.1 点的齐次坐标
? 一般来说,n维空间的齐次坐标表示是一个(n+1)维空间实体。有一 个特定的投影附加于n维空间,也可以把它看作一个附加于每个矢量的 特定坐标—比例系数。

?

引入齐次坐标的目的是为了表示几何变换的旋转、平移和缩放

一个点矢:

? ? ? ? v ? ai ? bj ? ck

式中i, j, k为x, y, z 轴上的单位矢量,

列矩阵

y z x a= , b= , c= ,w为比例系数 w w w

?x ? 显然,齐次坐标表达并不是唯一的,随 T y ? ? V? ? ?x y z w? w值的不同而不同。在计算机图学中,w ?z ? ? 作为通用比例因子,它可取任意正值,但 ?w? ? 在机器人的运动分析中,总是取w=1 。

[例1]:
? ? ? ? V ? 3i ? 4 j ? 5k
可以表示为: V=[3 4 5 1]T 或 V=[6 8 10 2]T

或 V=[-12 -16 -20 -4]T

? 齐次坐标与三维直角坐标的区别
? V点在ΣOXYZ坐标系中表 示是唯一的(a、b、c) ? 而在齐次坐标中表示可 以是多值的。不同的表 示方法代表的V点在空间 位置上不变。
x z o x

z

V
z y

图2-2

? 几个特定意义的齐次坐标:
? [0 0 0 n]T—坐标原点矢量的齐次坐标,n为任 意非零比例系数 ? [1 0 0 0]T — 指向无穷远处的OX轴 ? [0 1 0 0]T — 指向无穷远处的OY轴 ? [0 0 1 0]T — 指向无穷远处的OZ轴 ? [0 0 0 0]T — 没有意义

? 2个常用的公式:
点乘:

a ? b ? axbx ? ayby ? azbz
i j ay by ? ? ? a z ? (a y bz ? a z by )i ? (a z bx ? a xbz ) j ? (a x by ? a y bx )k bz k

叉乘: a ? b ? a x bx

2.1.2 平面的齐次坐标
? 平面齐次坐标由行矩阵P=[a b c d ]来表示 ? 当点v=[x y z w]T处于平面P内时,矩阵乘积PV=0,或记为

?x ? PV ? ?a b c d ?? y ? ? ax ? by ? cz ? dw ? 0 ?z ? ? ?w? ?

?0 0 与点矢

0 0?

T

?0 0 0 0? 相仿,平面

也没有意义

? 点和平面间的位置关系
设一个平行于x、y轴,且在z轴上的坐标为单位距离的平面 P可以表示为:P ? ?0 0 1 ? 1? 或 P ? ?0 0 2 ? 2?

? ?? 0 ?? 0 ? ?? 0

v 点在平面上方 v 点在平面上 有: PV= v 点在平面下方

例如:点 V=[10 20 1 1]T 必定处于此平面内,而点 V=[0 0 2 1]T 处于平 P 的上方,点V=[0 0 0 1]T处于P平面下方,因为:
?10? ?20? ?0 0 ? 10 10?? ? ? 0 ?1? ? ? ?1?

?0 ? ?0 ? ?0 0 1 ? 1?? ? ? 1 ? 0 ? 2? ? ? ?1 ?

?0 ? ?0 ? ?0 0 1 - 1?? ? ? -1 ? 0 ?0 ? ? ? ?1?

2.2 旋转矩阵及旋转齐次变换
2.2.1 旋转矩阵
设固定参考坐标系直角坐标为 ΣOxyz ,动坐标系为 ΣO?uvw , 研究旋转变换情况。
① 初始位置时,动静坐标系重合,O、O? 重合,如图。各轴对 应重合,设P点是动坐标系ΣO?uvw中的一点,且固定不变。则P 点在ΣO?uvw中可表示为: z

Puvw ? Pu iu ? Pv jv ? Pw kw
为坐标系ΣO?uvw的单位矢量, iu 、jv 、k w 则P点在Σoxyz中可表示为:
w o u x

P
v
(O')

y

Pxyz ? Px ix ? Py j y ? Pz k z Puvw ? Pxyz

② 当动坐标系ΣO?uvw绕O点回转时,求P点在固定坐标系Σoxyz 中的位置 z
w

Puvw ?已知: Pu iu ? Pv jv ? Pw kw P点在ΣO?uvw中是不变的仍然成

Pw

立,由于ΣO?uvw回转,则:

P o Pv
(O')

v y

Px ? Puvw ix ? ( Pu iu ? Pv jv ? Pw kw )ix
Py ? Puvw j y ? ( Pu iu ? Pv jv ? Pw k w ) j y
x

Pu

Pz ? Puvw k z ? ( Pu iu ? Pv jv ? Pw kw )k z

用矩阵表示为:

? Px ? ? ix i? ?P ? ? ? j i ? y? ? y ? ?k i ? ? P ? z? ? ? z ?

i x jv j y jv k z jv

图2-4 ix k w ? ? P? ? ?? ? jy k w ? ? Pv ? (2-7) ? Pw ? k z kw ? ? ? ? ?

u

? i x i? ? 定义 旋转矩阵为:R ? ? j y i? ?k i ? ? z ?

i x jv j y jv k z jv
?1

ix k w ? ? jy k w ? 则 : p xyz ? R Puvw k z kw ? ? ?

反过来: Puvw ? R ?1 Pxyz

R* R ? det R

R?为R的伴随矩阵, det R为R的行列式, R是正交矩阵, R ?1 ? R T

2.2.2 旋转齐次变换
用齐次坐标变换来表示式(2-7)
? Px ? ? ?P ? ? R ? y? ? ? ? Pz ? ? ? ? ? ? 1 ? ?0 0 0 0? ? P u ? ?P ? 0? ?? v ? 0? ? Pw ? ?? ? 1? ? 1 ?

?Pu ? ? ? P ? ? R ?1 ? v??? ? Pw ? ? ? ? ? ? 1 ? ?0 0 0

0? ? Px ? ?P ? 0? ?? y ? 0? ? Pz ? ?? ? 1? ? 1 ?

2.2.3 三个基本旋转矩阵和合成旋转矩阵
三个基本旋转矩阵

R( x, ? )
?vw绕OX轴转动?角, 即动坐标系 ? O, 求 R( x, ? ) 的旋转矩阵,也就是

求出坐标系 ? O ' ?vw中各轴单位矢量 i? , jv , kw在固定坐标系 ? Oxyz

中各轴的投影分量,很容易得到在两个坐标系重合时,有:

?1 0 0 ? ? R?? 0 1 0 ? ? ? ?0 0 1 ? ?

? i x i? ? R(x,? ) ? ? j y i? ?k i ? ? z ?

i x jv j y jv k z jv

ix k w ? ? jy k w ? k z kw ? ? ?

z
W'

0 ?1 ix ? iu ? ?0 cos? ? ?0 sin ?

? ? sin ? ? ? cos? ? ? 0
U'

w
?
V' O'

?

o u x

v

y

方向余弦阵

图2-5

z

三个基本旋转矩阵:

W'

w

0 ?1 R(x,? ) ? ? ?0 cos? ? ?0 sin ?

? ? sin ? ? ? cos? ? ? 0
u

?

o

O'

v

y

同理:

? cos? R(y,? ) ? ? ? 0 ? ?? sin ?
?cos? R(z,? ) ? ? ? sin ? ? ? 0

sin? ? 1 0 ? ? 0 cos? ? ? 0
- sin? cos? 0 0? 0? ? 1? ?

x
U'

z w
W'

v' o
O'

?

v

y

u x
U'

例1:在动坐标中有一固定点 Po'uvw ? ?1 2 3 1?T,相对固定参 ? Oxyz 考坐标系 做如下运动:① R(x, 90°);② R(z, 90°);③ R(y,90°)。求运动后点Po'uvw 在固定参考坐标系 ? Oxyz 下的位置。 解1:用画图的简单方法

? 合成旋转矩阵:

解2:用分步计算的方法 ① R(x, 90°)
?1 ?0 P' ? ? ?0 ? ?0 0 0 0 -1 1 0 0 0 0 ? ?1 ? ? 1 ? ?2? ?? 3? 0? ?? ? ? ? ? 0 ? ? 3? ? 2 ? ?? ? ? ? 1 ? ?1 ? ? 1 ?

(2-14)

② R(z, 90°)

?0 - 1 ?1 0 P '' ? ? ?0 0 ? ?0 0
?0 ?0 P ''' ? ? ?- 1 ? ?0 0 1 0 0

0 0 1 0
1 0 0 0

0? ? 1 ? ? 3? ?? 3? ?1? 0? ?? ? ? ? ? 0? ? 2 ? ? 2? ?? ? ? ? 1 ? ? 1 ? ?1 ?
0? ? 3? ? 2 ? ?1 ? ? 1 ? 0? ?? ? ? ? ? 0? ?2? ?? 3? ?? ? ? ? 1 ? ?1 ? ? 1 ?

(2-15)

③ R(y, 90°)

(2-16)

上述计算方法非常繁琐,可以通过一系列计算得到上述 结果。将式(2-14)(2-15)(2-16)联写为如下形式:

? Px ? ? ?P ? ? R 3?3 ? y? ? ? ? Pz ? ? ? ? ? ? 1 ? ?0 0

? ? Pu ? ?? P ? ?? v ? ? ? Pw ? ?? ? 0 1? ? 1 ?

R3x3为二者之间的关系矩阵,我们令:

R 3?3 ? R(y,? ) R( z,? ) R( x, ? )
定义1: 当动坐标系 ? O 'uvw 绕固定坐标系 ? Oxyz 各坐标轴顺序有限次 转动时,其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。 注意:旋转矩阵间不可以交换

? 平移齐次变换矩阵
w′ o′ v′ u′

z c
0 0 1 0 a? b? ? c? ? 1?

?1 ?0 H ? T rans(a b c) ? ? ?0 ? ?0

0 1 0 0

b o a y

注意:平移矩阵间可以交换,x 平移和旋转矩阵间不可以交换

2.2.4 相对变换
举例说明: 例1:动坐标系∑0′起始位置与固定参考坐标系∑0重合,动坐标系 ∑0′做如下运动:①R(Z,90? ) ②R(y,90? ) ③Trans(4,-3, 7), 求合成矩阵

解1:用画图的方法:
v```

z w
o(o′) v

z
w′ v′

z
o′

z 7

v″

u``` w```

y

u x

o(o′)

u′ y

o x
w″

u″

y

-3 4 x

o

y

x

解2:用计算的方法 根据定义1,我们有: T ? T rans(4 , - 3, 7) R(y,90? ) R(Z,90? )
?0 0 1 4 ? ?1 0 0 ? 3? (2-20) ? ?? ?0 1 0 7 ? ? ? 0 0 0 1 ? ? 以上均以固定坐标系多轴为变换基准,因此矩阵左乘。 如果我们做如下变换,也可以得到相同的结果:

例2:①先平移Trans (4,-3,7);②绕当前 v ? 轴转动90? ; ③绕当前 w?? 轴转动90? ;求合成旋转矩阵。

解1:用画图的方法
z

z w v u x
o(o′)
u′

z
w′ o′ v′

o′
o y

v″

y
x

w″ u″

o x
z

y

解2:用计算的方法
?0 ?1 o o T ? T rans(4 , - 3, 7) R(y,90 ) R(Z,90 ) ? ? ?0 ? ?0 0 0 1 0 1 4? 0 ? 3? ? 0 7? ? 0 1?

v```

o′ w```

u```

o x

y

(2-21)

式( 2-20 )和式( 2-21 )无论在形式上,还是在结果上都是 一致的。因此我们有如下的结论: 动坐标系在固定坐标系中的齐次变换有2种情况: 定义1:如果所有的变换都是相对于固定坐标系中各坐标轴旋 转或平移,则依次左乘,称为绝对变换。 定义2:如果动坐标系相对于自身坐标系的当前坐标轴旋转或 平移,则齐次变换为依次右乘,称为相对变换。

结果均为动坐标系在固定坐标中的位姿(位置 + 姿态)。相 对于固定坐标系,?轴相当于X轴,v轴相对于Y轴,w轴相当于Z轴。 也就是说,动坐标系绕自身坐标轴做齐次变换,要达到绕固 定坐标系相等的结果,就应该用相反的顺序。

右乘的意义:
? 机器人用到相对变换的 时候比较多 ? 例如机械手抓一个杯子, 如右图所示,手爪需要 转动一个角度才抓的牢, 相对于固定坐标系表达 太麻烦,可以直接根据 手爪的坐标系表示 ? 但也要知道在∑O中的位 姿,就用右乘的概念。
z

H

y

o
x

2.2.5 绕通过原点的任意轴旋转的齐次变换
? 有时动坐标系∑O?可能绕过原点O的分量分别为rx、ry、rz的 任意单位矢量r 转动φ角。 ? 研究这种转动的好处是可用∑O?绕某轴r 的一次转动代替绕 ∑O各坐标轴的数次转动 ? 为推导此旋转矩阵,可作下述5步变换: 1. 绕X 轴转α角,
Y

使r 轴处于XZ平面内
2. 绕Y 轴转-β角,使r 轴与OZ轴重合
A

ry
B

3. 绕OZ轴转动φ角
4. 绕Y 轴转β角 5. 绕X 轴转-α角
3
Z

r α rz
A'

O β
2 4

1 5
D B'

rx
C

X

由定义1和定义2,上述5次旋转的合成旋转矩阵为:
R r ,? ? R x , ?? R y, ? R z ,? R y, ? ? R x ,? 0 ?1 ?? ?0 cos? ? ?0 ? sin? ?cos? ? 0 ? ? ? sin? 0 ? ? cos? ? 0 sin? ? ?? cos? ? ?? ?- sin?
Y

ry
0? B ? 0? 1? ?(2-25 rx )
5
D B' C

0 sin? ? ?cos? - sin? A? ? 1 0 ? ? sin? cos? r 0 cos? ? 0 ?? ? 0 O α 0 - sin? ? ?1 0 0 ? β ?0 cos? - sin? ? rz 1 0 ? ?? ? 2 0 cos? ? 4 3 ? ?? ?0 sin? cos? ? A'
Z

1

X

由上图容易求出:
sin? ? ry r ?r
2 y 2 z

cos ? ?

rz ry2 ? rz2

rx sin? ? ? ? rx r r

OC

cos? ?

B' C OB

?

ry2 ? rz2 r

? ry2 ? rz2

带入式(2-25),得

R r ,?

? rx2 (1? cos? ) ? cos? ? ? ? rx ry (1? cos? ) ? rzsin? ?rx r z (1? cos? ) ? rysin? ? rx r z (1? cos? ) ? rysin? ? ry rz (1? cos? ) ? rx sin? ? ? 2 rz (1? cos? ) ? cos? ? ?

rx ry (1? cos? ) ? rz sin? ry2 (1? cos? ) ? cos? ry rz (1? cos? ) ? rx sin?

由该式可以推出3个基本旋转矩阵

2.2.6 齐次变换矩阵的几何意义
设,有一个手爪,即动坐标系∑O?,已知, o' ?a1 b1 c1 ? 初始位置 重合,那么∑O?在∑O中的齐次坐标变换为:
?1 ?0 T1 ? ? ?0 ? ?0 0 1 0 0 0 a1 ? 0 b1 ? ? 1 c1 ? ? 0 1?

,如果手爪转了一个角度,

?? x ? x ?? ? y T?? y ??z ? z ? ?0 0

wx wy wz 0

则: px ? py ? ? pz ? ? 1?

T反映了∑O?在∑O中的位置和姿态,即表示了该坐标系原点 和各坐标轴单位矢量在固定坐标系中的位置和姿态。 该矩阵可以由4个子矩阵组成,写成如下形式:
? ? T ?? ? ? ? R3?3 f1?3 ? P3?1 ? ? ? ?旋转矩阵 位置矢量? ? ? ? 透视矩阵 比例系数 ? ? ? w1?1 ?

R 3?3

?? x ? x ?? ?? y ? y ? ??z ? z

wx ? wy ? ? wz ? ?

为姿态矩阵(旋转矩阵),表示动坐标系 ∑O?在固定参考坐标系∑O中的姿态,即表示 ∑O?各坐标轴单位矢量在∑O各轴上的投影

P3?1 ? [ px py pz ]T

为位置矢量矩阵,代表动坐标系∑O?坐标原点 在固定参考坐标系∑O中的位置
般置为0

f1?3 ? [0 0 0] 为透视变换矩阵,在视觉中进行图像计算,一
w1?1 ? [1]
为比例系数

如果需要求解∑O在∑O?中的位置和姿态,此时的齐次变换矩 阵为 ,即求逆矩阵: T ?1 ? T? ? ( - ?) p ? ? ? T ?? -1 R 3?3 ( - v) p ? ?1 ? T ? ? T ?? ? ( - w) p ? ? 0 0 1 ? ? ?0 ?
其中:
? ? ? ? p ? pxi ? py j ? pz k ? ? ? ? ? ?xi ? ? y j ? ?z k ?

? ? ? ? v ? vx i ? vy j ? vz k
? ? ? ? w ? wxi ? wy j ? wz k

这些式子以后经常遇到, 在机器人计算中,所要 求的就是齐次变换矩阵

2.2.7 透镜成像的齐次变换
设: 以光心为原点O,光轴与y轴重合,P为物点,
? 用齐次坐标表示P ? [x p y p z p 1]T 求P?的齐次坐标,即求P?[x ?p y?p z?p 1]T yp zp zp f 根据三角形相似原理: ? ? yp? zp? zp? yp? ? f 注意yp?是负值,f 是正值,所以实际上为相减关系 zp xp yp f ? ? ? ? zp? xp? yp? yp? ? f P yp f yp ? f yp 又有 ? ? ? 1? yp? yp? ? f yp? ? ( yp f y?? f ) f ypzp xp yp ? xp? ? yp? ? zp? ? yp yp zp 1? 1? 1?
z

zp
f

y ?p

o
z ?p

P'

xp? yp? yp? ? f yp f yp ? f yp 又有 ? ? ? 1? yp? yp? ? f yp? ? ( yp? ? f ) f xp yp zp ? xp? ? yp? ? zp? ? yp yp zp 1? 1? 1? f f f ? xp xp ? ? ? ? ? ? xp ? ? ? ??xxp p? ? yp ? ? ? y ?? yp? p ? ? ? yp ? y ? ?? ?? p? ? ? 用矩阵表示: ? ? zp ?z ?? 用矩阵表示: ? T f p ? ? ? ? ? zp zp ?z ? ? ? ? ??1p? ? ? ? ?1 y? ? ?1 ? p ? ? ?1 ?? ? ? ? 1 ? ? ? ? ? f ? ? ? ?1 0 00 ?1 ? x p?? ?0 1 ?0 ? ?? 1 0 y ? ? p ? ?? ?? TfT? ? f ?0 ?0 0 01 ? z ?? ypp?? 1 ?? ? 0 0 0? 0 ? ? ? ? f ? f ?1 ?? ?
0? 0 0? 0 ? 1? 0 ? 1 0? 0? 0? ? 0? ? 1? ?

zp?

? 因此,进行机器人运动学计算时,不能省略透视矩阵,有 摄像头时,透视矩阵为 1 [0 - f 0],没有摄像头时为[0 0 0 ] 。

知识点:
1. 点和面的齐次坐标和齐次变换
2. 三个基本旋转矩阵 3. 绝对变换:如果所有的变换都是相对于固定坐标系中各坐 标轴旋转或平移,则依次左乘,称为绝对变换。 4. 相对变换:如果动坐标系相对于自身坐标系的当前坐标轴 旋转或平移,则齐次变换为依次右乘,称为相对变换。 5. 绕任意轴旋转:5步顺序 6. 透视变换

知识点:

0 ?1 R(x,? ) ? ? ?0 cos? ? ?0 sin ?
? cos? R(y,? ) ? ? ? 0 ? ?? sin ?
?cos? R(z,? ) ? ? ? sin ? ? ? 0

? ? sin ? ? ? cos? ? ? 0
sin? ? 1 0 ? ? 0 cos? ? ? 0
- sin? cos? 0 0? 0? ? 1? ?

三个基本旋 转矩阵

例题1: ∑O?与∑O初始重合,∑O?作如下运动:①绕Z轴转动30? ; ②绕X轴转动60? ;③绕Y轴转动90? 。求T。
?cos30? ? ? sin 30 R1 ? ? ? 0 ? ? ? 0
? cos90? ? 0 R3 ? ? ?? sin 90? ? ? ? 0

? sin 30? cos30? 0 0
0 sin 90? 1 0 0 cos90? 0 0

0 0? ? 0 0? 1 0? ? 0 1? ?
0? ? 0? 0? ? 1? ?

0 ?1 ?0 cos60? R2 ? ? ?0 sin 60? ? 0 ?0

0 ? sin 60? cos60? 0

0? 0? ? 0? ? 1?

? 3/4 ? 1/ 4 ? T ? R3 R2 R1 ? ?? 3 / 2 ? ? 0 ?

3/ 4 1/ 2 3/4 ? 3/2 1/ 2 0 0 0

0? ? 0? 0? ? 1? ?

例题2: ∑O?与∑O初始重合,∑O?作如下运动:①绕X轴转动90? ;②绕 w轴转动90? ;③绕Y轴转动90? 。求① T;②改变旋转顺序,如何 旋转才能获得相同的结果。

解①:

0 ?1 ?0 cos90? R1 ? ? ?0 sin 90? ? 0 ?0
? cos90? ? 0 R3 ? ? ?? sin 90? ? ? ? 0

0 - sin 90? cos90? 0

0? 0? ? 0? ? 1?
0? ? 0? 0? ? 1? ?

?cos90? ? ? sin 90 R2 ? ? ? 0 ? ? ? 0

? sin 90? cos90? 0 0

0 0? ? 0 0? 1 0? ? 0 1? ?

0 sin 90? 1 0 0 cos90? 0 0

?1 ?0 T ? R3 R1 R2 ? ? ?0 ? ?0

0 0 0 ?1 1 0 0 0

0? 0? ? 0? ? 1?

解②: ①绕Z(w)轴转动90? ; ②绕X轴转动90? ; ③绕Y轴转动90? 。

例题3: ? ? ? ? 矢量 P 在∑O?中表示为 p0? ? 3i? ? 2 j? ? 2k ? ,∑O?相对于∑O的
?0 ? 1 ?1 0 o To? ? ? ?0 0 ? ?0 0 0 10? 0 20? 奇次变换为: ? 1 1? ? 0 1?

求 : 1) 画出? 0?在 ? 0的位置和姿态 2) 求 p? 0?在 ? 0中的矢量p? 0 3) 当 ? 0?绕 ① ? 0的Y轴转90? 求此时 p? O' 在 ? 0中的矢量 ② 沿 ? 0的X轴平移20,

解:1)

z w v o o′ x y u

解:2)
p? 0 ?? To? p? 0?

?8 ? ?23? ?? ? ?3 ? ? ? ?1 ?
?1 ?0 Tr1 ? ? ?0 ? ?0 0 1 0 0 0 20? 0 0? ? 1 0? ? 0 1?

解:3)


? cos90? ? 0 R1 ? ? ?? sin 90? ? ? ? 0

0? ? 1 0 0? 0 cos90? ,0? ? 0 0 1? ? 0 sin 90?

?0 ?1 ? T ? Tr1 R 1 To? ? ? ?0 ? ?0

0 0 1 0

1 21 ? 0 20 ? ? 0 ? 10? ? 0 1 ?

p? 0 ? T p? 0?

?1 ?1 ?? ?0 ? ?0

0 0 1 0

1 21 ? ?3 ? ?23 ? ?2? ?23 ? 0 20 ? ?? ??? ? 0 ? 10? ?2? ?? 8? ?? ? ? ? 0 1 ? ?1 ? ? 1 ?

例题4:
? T3 T1 1T2 2、 如图所示,1)写出 3

T4 、 、
x3

0 T4 ;2)求

解:1)
?1 0 ?0 - 1 ? T1 ? ? ?0 0 ? ?0 0 0 - 3.5? 0 3 ? ? -1 1 ? ? 0 1 ?
o4

o3

z3
y3

z4
4

y4

x4
y2

y1

o1

?0 ?- 1 1 T2 ? ? ?0 ? ?0

0 0 -1 0

1 0 0 0
4 5 3 5 0 0

0? 3? ? 1? ? 1?
? 0? ? 5? ? 0? 1? ?

z2

x2
o2

x1

z1

z0
1

3

3.5

3 ? 0 ? 5 ? 4 2 T3 ? ?0 5 ? ?1 0 ?0 0 ?

x0

o

y0

?- 1 ?0 3 T4 ? ? ?0 ? ?0

0 0 3.5? 1 0 0? ? 0 -1 0 ? ? 0 0 1?

解2):根据定义2,绕自身旋转,右乘

0

T4 ? T1 T2
? 1

2

T3

3

T4

0 0? ?- 1 0 ? 0 0.6 - 0.8 0? ? ?? ? 0 - 0.8 - 0.6 5? ? ? 0 0 1? ?0

习题1: ∑O?与∑O初始重合,∑O?作如下运动:①绕z轴转动90? ;②绕 v′轴转动90? ;③绕x轴转动90? 。求① T;②改变旋转顺序,如 何旋转才能获得相同的结果。

0 2 1 ? 习题 : 0 0? ? 0 0 ? 1 0? 已知齐次变换矩阵? H?? ? ? 1 0 0 0? ? ? ? 0 0 0 1?

要求R(f,θ), 求f和θ值

第三章 机器人运动学
? 机器人运动学主要是把机器人相对于固定参考 系的运动作为时间的函数进行分析研究,而不 考虑引起这些运动的力和力矩 ? 也就是要把机器人的空间位移解析地表示为时 间的函数,特别是研究机器人关节变量空间和 机器人末端执行器位置和姿态之间的关系 ? 本章将讨论机器人运动学几个具有实际意义的 基本问题。

§3.1 机器人运动学所讨论的问题
§3.1.1 研究的对象
? 机器人在基本机构形式上分为两种,一种是关节式串 联机器人,另外一种是并联机器人,如图:
1972 Victor Scheinman在Unimation公司为通用;1980Westinghouse收购;1988St? ubli收购; Nokia Robotics在80年代卖出1500余台PUMA系统; Nokia的 Robotics division1990年卖出。

PUMA560

Fanuc manipulator

Hexapod

运动学研究的问题
Where is my hand?

运动学正问题
Direct Kinematics HERE!

How do I put my hand here?

运动学逆问题
Inverse Kinematics: Choose these angles!

研究的问题:
? 运动学正问题---已知杆件几何参数和关节角矢量,求操 作机末端执行器相对于固定参考作标的位置和姿态(齐 次变换问题)。 ? 运动学逆问题---已知操作机杆件的几何参数,给定操作 机末端执行器相对于参考坐标系的期望位置和姿态(位 姿),操作机能否使其末端执行器达到这个预期的位姿? 如能达到,那么操作机有几种不同形态可以满足同样的 条件?
杆件参数 关节角 运动学正问题 杆件参数 关节角 运动学正问题 逆 末端执行器

§3.2 机器人杆件,关节和它们的参数
§3.2.1 杆件,关节
? 操作机由一串用转动或平移(棱 柱形)关节连接的刚体(杆件) 组成 ? 每一对关节杆件构成一个关节— 自由度,因此N个自由度的操作 关 机就有N对关节-杆件。 节 ? 0号杆件(一般不把它当作机器 人的一部分)固联在机座上,通 常在这里建立一个固定参考坐标 系,最后一个杆件与工具相连 ? 关节和杆件均由底座向外顺序排 列,每个杆件最多和另外两个杆 件相联,不构成闭环。

杆 件

末端操作手 两自 由度 机座

关节:
? 一般说来,两个杆件间是用低付相联的 ? 只可能有6种低付关节:旋转(转动)、棱柱(移动)、 圆柱形、球形、螺旋和平面,其中只有旋转和棱柱形关 节是串联机器人操作机常见的,各种低副形状如下图所 示:

旋转

棱柱形

柱形

球形

螺旋形

平面

§3.2.2
★条件

杆件参数的设定

? 关节串联
? 每个杆件最多与 2 个杆件相连,如 Ai 与 Ai-1 和 Ai+1相连。第 i 关节的关节轴 Ai 位于2个杆 件相连接处,如图所示, i-1关节和 i+1关节 也各有一个关节轴 Ai-1 和 Ai+1。 Ai

Ai+1

Ai-1

杆件参数的定义—— li 和 ? i
★由运动学的观点来看,杆件的作用仅在于它能保 持其两端关节间的结构形态不变。这种形态由两 个参数决定,一是杆件的长度 li,一个是杆件的 扭转角 ? i ? li 关节Ai轴和Ai+1轴线 公法线的长度 Ai ? ? i 关节i轴线与i+1轴线 在垂直于 li 平面内的夹 角,有方向性,由Ai转 向Ai+1,由右手定则决 定正负

Ai+1

?i
li

杆件参数的定义—— d i和 ? i
确定杆件相对位置关系,由另外2个参数决定,一个是杆 件的偏移量 d i,一个是杆件的回转角 ? ? Li和Li-1在Ai轴线上 的交点之间的距离
i

Ai+1

di

Ai-1
? Li和Li-1之间的夹 角,由Li-1转向Li,由 右手定则决定正负, 对于旋转关节它是个 变量

Ai

?i

?i
li li ?1 di
?i

移动关节杆件参数的定义
? 确定杆件的结构形态的2个参数Li与αi与旋转关节是一样的。 确定杆件相对位置关系的2个参数则相反。这里θi为常数, di为变量。

? 上述4个参数,就确定了杆件的结构形态和相邻杆件相对位 置关系,在转动关节中,Li, αi, di是固定值,θi是变量。在 移动关节中,Li, αi, θi是固定值, di 是变量。

§3.3 机器人关节坐标系的建立
? 对于每个杆件都可以在关节轴处建立一个正规的笛卡儿 坐标系(xi, yi, zi),(i=1, 2, …, n),n是自由度数,再 加上基座坐标系,一共有(n+1)个坐标系。 ? 基座坐标系 ?O o 定义为0号坐标系(x0, y0, z0),它也是机 器人的惯性坐标系,0号坐标系在基座上的位置和方向可 任选,但Ζo 轴线必须与关节1的轴线重合,位置和方向可 任选; ? 最后一个坐标系(n关节),可以设在手的任意部位,但 必须保证 X n 与Ζn-1 垂直。

D-H关节坐标系建立原则
? 机器人关节坐标系的建立主要是为了描述机器人各杆件和终 端之间的相对运动,对建立运动方程和动力学研究是基础性 的工作。 ? 为了描述机器人各杆件和终端之间转动或移动关系,Denavit 和Hartenberg于1955年提出了一种为运动链中每个杆件建立 附体坐标系的矩阵方法(D-H方法) ,建立原则如下: ? 右手坐标系 ? 原点Oi:设在Li与Ai+1轴线的交点上 ? Zi轴: ? Xi轴: ? Yi轴: 与Ai+1关节轴重合,指向任意 与公法线Li重合,指向沿Li由Ai轴线指向Ai+1轴线 按右手定则

关节坐标系的建立原则
? 原点Oi:设在Li与 Ai+1轴线的交点上 ? Zi轴:与Ai+1关节轴 重合,指向任意 ? Xi轴:与公法线Li 重合,指向沿Li由 Ai轴线指向Ai+1轴线 ? Yi轴:按右手定则
? ? ? ?

Ai+1

Ai-1

Ai
?i
yi zi xi oi yi ?1

li li ?1 di
zi ?1 oi ?1

?i
xi ?1

杆件长度Li —沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离 杆件扭转角αi — 绕 xi 轴,由 zi-1 转向zi 杆件偏移量 di — 沿 zi-1 轴,zi-1 轴和 xi 交点至∑0i –1 坐标系原点的距离 杆件回转角θi — 绕 zi-1 轴,由 xi-1转向 xi

两种特殊情况
? 两轴相交,怎么建立坐 标系?
? 0i—Ai与Ai+1关节轴线的交 点;
? Zi—Ai+1轴线;
zi
xi yi zi - 1 oi

? Xi—Zi和Zi-1构成的平面的 ???? ? ?? ? Ζi ? 法线 ? ?Ζi-1× ?;
? Yi—右手定则;
Ai

Ai + 1

? 两轴平行,怎么建立坐标系(Ai与Ai+1平行)?
? 先建立 ∑0i-1 ? 然后建立∑0i+1 ? 最后建立 ∑0i
注意: ? 由于Ai和Ai+1平行, 所以公法线任意点 在A点位置;
Ai-1 Ai Ai+1 Ai+2

yi-1 li-1 di

zi-1 oi-1 A D xi-1

zi ( yi) zi+1 yi+1 oi ( xi) xi+1 di+1 B yi oi+1 li+1 x i C

? 按照先前的定义,di为Oi-1点和A点之间的距离,di+1为B点和C点间 的距离,这样设定可以的,但我们可以变更一下,将0i点放在C点, 定义Oi在Li+1和Ai+1轴的交点上,这样使di+1=0使计算简便,此时di= ????? ? O i-1 D

相邻关节坐标系间的齐次变换过程
——机器人运动学正解
根据上述坐标系建立原则,用下列旋转和位移我们 可以建立相邻的 Oi-1 和 Oi 坐标系之间的关系
1. 将xi-1轴绕 zi-1 轴转 ?i 角度,将其与xi轴平行; 2. 沿 zi-1轴平移距离 di , Ai-1 使 xi-1 轴与 xi 轴重合; 3. 沿 xi 轴平移距离 Li, 使两坐标系原点及x轴 重合; 4. 绕 xi 轴转 ?i 角度,两 坐标系完全重合.

Ai+1

Ai
?i
yi zi xi oi yi ?1

li li ?1 di
zi ?1 oi ?1

?i
xi ?1

i ?1

Ai ? R(Zi ?1,?i )Trans (Zi?1, di )Trans ( xi , li )R( xi ,?i )

D-H变换矩阵
i ?1

Ai

=

?cos? i ? sin ? i ? ? 0 ? ? 0

? sin ? i cos? i 0 0

0 0 1 0

0? 0? ? 0? ? 1?

?1 ?0 ? ?0 ? ?0

0 1 0 0

0 0 ? ?1 ?0 0 0? ? ? 1 d i ? ?0 ? ? 0 1 ? ?0

0 1 0 0

0 0 1 0

li ? 0? ? 0? ? 1?

0 ?1 ?0 cos? i ? ?0 sin ? i ? 0 ?0

0 ? sin ? i cos? i 0

0? 0? ? 0? ? 1?

=

?cos? i ? sin ? i ? ? 0 ? ? 0

? cos? i sin ? i cos? i cos? i sin ? i 0

sin ? i sin ? i ? sin ? i cos? i cos? i 0

li cos? i ? li sin ? i ? ? di ? ? 1 ?

机器人的运动学正解方程
0

Ti ? A1 ? A2 ???
0 1

i ?1

Ai

机器人的运动学正解方程
机械手的坐标变换图如图所示,机械手的末端(即连杆坐标系i) 相对于基座坐标系0的描述用 oTi 表示,即:
0

Ti ? A1 ? A2 ???
0 1
A2 A3 A4 A5
4T

i ?1

Ai
E X 0
6

z 0

A1

A6
5T

3T
2T 1T 0T 6 6 6

6

6

机械手的坐标变换图

举例:Stanford机器人

? 为右手坐标系
? 原点Oi: Ai与Ai+1 关节轴线的交点 ? Zi轴:与Ai+1关节轴 重合,指向任意 ? Xi轴: Zi和Zi-1构成 的面的法线 ? Yi轴:按右手定则
z5

A6 z 6
O6

y6

A5

x6 A4 d6 z4
O4

y3
O3

z3 d3 A3 z2 A1
O1 O2

x3

A2 x2 y2 z1 x1

y5
O5

x4

y4

x5

o3 , o4 , o5重合 ? d4 ? d5 ? 0

y1

d2

Li —沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离 αi — 绕 xi 轴,由 zi-1 转向zi di — 沿 zi-1 轴,zi-1 轴和 xi 交点至∑0i –1 坐标系原 点的距离 θi — 绕 zi-1 轴,由 xi-1转向 xi

z0 y0
O0

d1 x0

解:

§3.4 例题
在机器人工作台上加装一电视摄像机,摄像机可见到固联 着6DOF关节机器人的机座坐标系原点,它也可以见到被操作 物体(立方体)的中心,如果在物体中心建一局部坐标系,则 摄像机所见到的这个物体可由齐次变换矩阵T1来表示,如果摄 像机所见到的机座坐标系为矩阵T2表示。
?0 ?1 T1 ? ? ?0 ? ?0 1 0 1? 0 0 10? ? 0 -1 9 ? ? 0 0 1? ?1 0 ?0 - 1 T2 ? ? ?0 0 ? ?0 0 0 - 10? 0 20 ? ? - 1 10 ? ? 0 1 ?

y

z

x

? 试求立方体中心在机座坐标系∑0中的位置 ? 该手爪从上方把物体抓起,同时手爪的开合方向与物体的Y轴同向, 那么,求手爪相对于∑0的姿态是什么?

解1 :
已知 摄T物 ? T1 , 摄T机 ? T2 , 求机T物

有:机T物 ? 机T摄
?1 0 ?0 - 1 ?? ?0 0 ? ?0 0



-1 T物 ? (T2) T1

y

o

z

x

0 10? ?0 1 0 1 ? ?1 0 0 10? 0 20? ? ? ? - 1 10? ?0 0 - 1 9 ? ? ? ? 0 1 ? ?0 0 0 1 ?

z物 z机 y机
O机

?0 ?- 1 ?? ?0 ? ?0

1 0 0 0

0 11? 0 10? ? 1 1? ? 0 1?

x物

O物

y物

因此物体位于机座坐标系的(11,10,1)T 处,它的X,Y,Z轴分别与机座坐标系的 ∑O机根据T2画出 -Y,X,Z轴平行。

∑O物根据T1画出

解2 :
y

? s

O

x

? n

a:

? 手爪开合方向与物体 y向重合 有s ? [? 1 0 0]T

?nx sx ax ?ny sy ay ? a 实际要求 ? z ? nz sz az ? ?0 0 0

px? py? ? ? 机T 手爪 pz? ? 1?

b : 从上向下抓,指出手爪 的a方向物体z方向相反 ? 则有a ? [0 0 ? 1]T

X机

j ? i ? ? ? c: n ? s ?a ? ? ?? 1 0 ? ?0 0

k? 0? ? ? 0i ? ? j ? 0k ? [0 ? 1? ?

?1

0]T

? 0 ?1 0 ? ?? 1 0 0 ? 因此:姿态矩阵为 ? ? ? 0 - 1? ?0 ?

? 0 ? 1 0 11? ?? 1 0 0 10? 当手爪中心 机 ? ? T ? 物 与物体中心 ?0 0 -1 1 ? ? ? 重合时 0 0 0 1 ? ?

工作空间
? 工作空间: 末端操作手可以到达的空间位置集合
? 如何获得工作空间: 利用正运动学模型,改变关节 变量值 ? 可达空间: 末端操作手可以至少以一个姿态到达的 空间位置集合

? 灵活空间: 末端操作手可以以任何姿态到达的空间 位置集合

示例: 平面 3连杆机器人
x ? l1 cos ?1 ? l2 cos ??1 ? ? 2 ? ? l3 cos ??1 ? ? 2 ? ? 3 ? y ? l1 sin ?1 ? l2 sin ??1 ? ? 2 ? ? l3 sin ??1 ? ? 2 ? ? 3 ?

? ? ?1 ? ? 2 ? ? 3
?3

l1 ? l2 ? l3 , l1 ? l2 ? l3

?2
l2

l3

如何确定可达空间?首先,令 ?3变 化

l1

?1

然后 ?2变化 最终,变化?1

§3.5 机器人末端操作器位姿的其它 描述方法
? 用矩阵表示刚性体的转动简化了许多运算, 但它需要9个元素来完全描述旋转刚体的姿 态,因此矩阵并不直接得出一组完备的广 义坐标。
? 一组广义坐标应能描述转动刚体相对于参 考坐标的方向,被称为欧拉角的三个角度, φ、θ、ψ就是这种广义坐标。 ? 有几种不同的欧拉角表示方法,它们均可 描述刚体相对于固定参考系的姿态。三种 最常见的欧拉角类型列在表中

3种最常见的欧拉角类型
步1 类型1 绕OZ轴转φ角 步2 绕当前OU' 轴转θ角 步3 绕当前OW″轴转ψ角

类型2
类型3

绕OZ轴转φ角
绕OX轴转φ角

绕当前OV '轴转θ角
绕OY轴转θ角

绕当前OW″轴转ψ角
绕OZ轴转ψ角

类型1:表示法通常用于陀螺运动
0

TN ? R( Z , ? ) R( ? ?,? ) R( w??, ? )
? s? c? 0 0? 0? ? 1? ? ?1 0 ?0 c? ? ? ? 0 s? 0 ? ? s? ? ? c? ? ? ?c? ? s? ? ? ?0 ? s? c? 0 0? 0? ? 1? ?

z (w) w"
W?① ????

w′
φ ψ

v?????

?c? ?? ? s? ? ?0

v"
θ v′
φ

③ψ o ②

?c?c? ? s?c?s? ?? ? s?c? ? c?c?s? ? s?s? ?

? c?s? ? s?c?c? ? s?s? ? c?c?c? s?c?

s?s? ? ? c?s? ? ? c? ? ?

x(u)

θ φ u" u′

ψ

y (v)
u?????

类型2

绕OZ轴转φ角

绕当前OV '轴转θ角

绕当前OW″轴转ψ角

类型2:所得的转动矩阵为右乘
R ? R( Z , ? ) R( v?,? ) ?c? ?? ? s? ? ?0 ? s? c? 0 0? 0? ? 1? ? ? c? ? 0 ? ? ?- s? R( w??, ? ) 0 s? ? 1 0? ? 0 c? ? ? ?c? ? s? ? ? ?0 ? s? 0? c? 0? ? 0 1? ?

?c?c?c? ? s?s? ? c?c?s? ? s?c? c?s? ? ? ?? s ? c ? c ? ? c ? s ? ? s ? c ? s ? ? c ? c ? s ? s ? ? ? ? ? s?s? s?c? c? ? ? ? px? ? ? R ? py ? T ?? ? pz? ? ? ?0 0 0 1 ?

类型3

绕OX轴转ψ 角

绕OY轴转θ角

绕OZ轴转φ角

类型3:一般称此转动的欧拉角 为偏航角、俯仰和横滚, (这 种方法也叫做偏航、俯仰和横滚 角表示方法)这种形 式主要用 于航空工程中分析飞行器的运动, 其旋转矩阵为
R ? R(z , ? ) R( y,? ) R( x, ? ) ?c? ? s? 0? ? c? 0 s? ? ?1 0 ? ? 0 ? ?0 c? ?? s ? c ? 0 1 0 ? ? ? ? ? ? 0 1? ?0 ? ? ?? s? 0 c? ? ? ? ?0 s? ?c?c? c?s?s? ? s?c? c?s?c? ? s?s? ? ? ?? s ? c ? s ? s ? s ? ? c ? c ? s ? s ? c ? ? c ? s ? ? ? ? ? c?s? c?c? ? ? s? ? 0 ? ? s? ? ? c? ? ?

§3.6 运动学逆问题
? 正运动学问题: 已知关节角度或位移,计算 末端操作手的对应位姿. ? 逆运动学问题: 已知末端操作手的位姿,求 解对应的关节变量. ? 为什么逆运动学问题更困难?
–可能存在多解或无解 –通常需多次求解非线性超越方程

解的存在性
? 目标点应位于工作空间内 ? 可能存在多解,如何选择最合适的解?

存在双解!

求解方法
? 如果各关节可用某算法获得,一个机械手是 有解的. 算法应包含所有可能解.

? 方法

封闭形式解(解析解) 数值解

? 我们对封闭形式的解法更感兴趣 ? 代数方法 ? 几何方法

运动学逆问题的可解性
? 对于给定的机器人,能否求得它的运动学逆解的解析式 (也叫封闭解)。 ? 可解性的重要结论是:

? 所有具有转动和移动关节的系统,在一个单一串联中 总共有6个(或小于6个)自由度时,是可解的,其通 解一般是数值解,它不是解析表达式,而是利用数值 迭代原理求解,它的计算量要比解析解大。 ? 但在某些特殊情况下,如若干个关节轴线相交和或多 个关节轴线等于 0 或 90°的情况下,具有6个自由度 的机器人可得到解析解。 ? 为使机器人有解析解,一般设计时,使工业机器人足 够简单,尽量满足这些特殊条件。

运动学逆问题的多解性
? 机器人运动问题为解三角方程,解反三角函数方程时会 产生多解.显然对于真实的机器人,只有一组解与实际情 况最相对应,因此必须作出判断,以选择合适的解。

? 通常采用如下方法剔除多余解:
1 .根据关节运动空间选取合适的解。例如求得机器 人某关节角的两个解为

? i1 ? 400

? i 2 ? 400 ? 1800 ? 2200

0 ? ? 40 若该关节运动空间为 ? 100? ,则应选 i 。

2.选择一个与前一采样时间最接近的解,例如:

? i1 ? 400

? i 2 ? 400 ? 1800 ? 2200
0

若该关节运动空间为 ? 250?,且? i ?1 ? 160 ,则应选

? i ? 220 0
3.根据避障要求,选择合适的解
4.逐级剔除多余解 对于具有 n 个关节的机器人,其全部解将构成树形结构。 为简化起见,应逐级剔除多余解。这样可以避免在树形解中 选择合适的解。

运动学逆问题解法
? 迭代法——计算量大 ? 几何法——适用于自由度较少的情况 ? 反变换法

运动学逆问题解法—反变换法
Paul 等人提出的方法(1981年,也叫求逆的方 法,是解析解):
? 用未知的逆变换逐次左乘,由乘得的矩阵方程 的元素决定未知数,即用逆变换把一个未知数 由矩阵方程的右边移到左边 ? 考察方程式左、右两端对应元素相等,以产生 一个有效方程式,理论上可得到12个方程。 ? 然后求这个三角函数方程式,以求解未知数 ? 把下一个未知数移到左边 ? 重复上述过程,直到解出所有解 缺点:无法由数种可能的解中直接得出合适的解, 需要通过人为的选择

Paul 等人提出的方法
0

T6 ? A1 A2 A3 A4 A5 A6
0 1 2 3 4 5

?nx sx ax ?ny sy ay 0 T6 ? ? ? nz sz az ? ?0 0 0

px ? py ? ? pz ? ? 1?

-1 0 (0 A1) T6 ?1A2 2 A3 3 A4 4 A5 5 A6 ?1T6

? q1
? q2
?

1 -1 0 -1 0 ( A2) ( A1) T6 ?2A3 3 A4 4 A5 5 A6 ?2T6

?

?

-1 0 (4A5 )?1 (3A4 )?1 (2A3 )?1 (1A2 )-1 (0 A1) T6 ?5T6

? q5

-1 0 (5A6 )-1 ? (0 A1) T6 ? E

? q6

此时不能用反余弦 (arccos) 来求解关节角,因为这样求解 不仅关节角的符号不确定( cos? ? cos(?? ) ),而且角的精度 也难以保证( d (cos? ) / d? ? 0 ,即角度变化引起的值 ? ?0, ?180 变化不大)。
?

arctg 2( y, x) 因此,通常用四象限的反正切函数来确定 ? 值,其象限定义为:

? 0 ? ? ? 900 x, y均为正 ? ? 0 ? 0 x 为负 , y 为正 90 ? ? ? 180 ? ? ? ? tg ?1 2( x, y ) ? ? ? 0 x, y均为负 ? ?? 180 ? ? ? ?90 0 0 ? x为正, y为负? ? 90 ? ? ? 0 ? ?

例1:欧拉角表示的逆运动学求解:
?nx sx ax? ?c? ?ny sy ay? ? ? s? ? ? ? ? ? nz sz az? ? ? ?0 ? s? c? 0 0? ?1 0 ?0 c? 0? ?? 1? ?? ? 0 s? 0 ? ?c? ? s? ? s? ? ?? c? ? ?? ?0 ? s? 0 ? c? 0? 1 ) ? ( 0 1? ?

1 用R ? ( z , ?)左乘式( 1 )左右两边,可使一个 未知数?在矩阵方程的左边,

而另两个未知数在右边 ,可得: ? c? ? - s? ? ? ? 0 s? c? 0 0? 0? ? 1? ? ?nx sx ax? ?1 0 ?ny sy ay? ? ?0 c? ? ? ? ? ? nz sz az? ? ? ? 0 s? 0 ? ? s? ? ? c? ? ? ?c? ? s? ? ? ?0 ? s? 0 ? c? 0? ? 0 1? ? ? s? c?c? s?c? 0 ? ? s? ? ? c? ? ?

c?sx ? s?sy c?ax ? s?ay ? ? c? ? c?nx ? s?ny ? ? ?c?s? 或? ? s ? nx ? c ? ny ? s ? sx ? c ? sy ? s ? ax ? c ? ay ? ? ? ? ? nz sz az ? ? ? ? s?s?

c?ax ? s?ay ?10 由式中矩阵( ,3)元素相等,有 ax ?? ? tan [ ] ? tg -1 2(ax,?ay) ? ay
?1

按照前面的定义,确定 ?所在象限。 使( 1, 1 )元素和( 1, 2)元素分别对应相等 ?c? ? c?nx ? s?ny ? ?? s? ? c?sx ? s?sy ? c?sx ? s?sy ?? ? tan [ ] ? tg -1 2(?c?sx ? s?sy , c?nx ? s?ny) c?nx ? s?ny
?1

(2, 3)元素和( 3, 3)元素对应相等 ?- s? ? ? s?ax ? c?ay ?c? ? az ? s?ax ? c?ay ?? ? tan ( ) ? tg ?1 2( s?ax ? c?ay, az) az
?1

也可以用右乘的方法求 解 用左乘还是右乘,取决 于使用者的直觉 ?正运动 ? 唯一解 ? 确定象限 ?靠分子,分母的符号来 ? ? 总体来讲? 逆运动 ? 多值解? 靠结构结束条件、剔除 办法, ? ? ? ?具体分析 ?

例2:斯坦福机器人运动学逆问题解
0

T6 ? A1? A2 ? A3 ? ?? A6
0 1 2 5

?1A2 ?3A3 ? ??5 A6

? T6
1

式中: f14 ( p ) ? C1 p x ? S1 p y

f 24 ( p ) ? ? pz f 34 ( p ) ? ? S1 p x ? C1 p y

由两端矩阵元素(3,4)对应相等可得:

作三角变换:

式中:

得到:

即有:

由(1, 4)和(2, 4)元素对应相等,得:

1

A2 ?1?1T6 ?2T6

式中第四列:

2

A3?1?2 T6 ?3T6

式中第三列:

高腕

低腕

5

取前一个采样点的值
z4
O4

z5
x5

y5
O5

x4

几何解法(适用于少自由度)
原则: 将原始空间几何问题转化为若干个 平面几何问题.
y ?2 L2 L1 x

应用 “余弦定理”:

x2+y2=l12+l22 ? 2l1l2cos(180??2)

几何解法 (续)
则有:
x ? y ?l ?l cosθ2 ? 2l1l2
2 2 2 1 2 2

y ? ?

?1
x

再次利用余弦定理得到:

l22 = x2+y2+l12 - 2l1? (x2+y2) cos ?
即 cos ? = (x2+y2+l12 - l22 )/2l1? (x2+y2)

在 0? ? ? 180°范围内求解,最后利用 ?1=??? 转换为多项式

? 通常超越方程难以求解,因为变量? 通 常以cos(? ) 或 sin(? ) 的形式出现. ? 可以转换为变量 u = tan(?/2) 的多项式,

? 然后利用下式求解: cos(?) = (1-u2)/(1+u2) sin(?) =2u /(1+u2)

§3.7 微动矩阵和微动齐次变换
? 对象: 微动矩阵主要是描述机器人在微动 范围内各关节的位移运动关系

? 定义: 各关节当角度转动 小于5°,平移 在 0.1mm 以下时,微动矩阵大致可用
? 用途:误差补偿、微驱动、微操作……

设:有一机器人如图,末端执行器在机座坐标系中的 齐次变换为oTN,做微动,①绕任意轴w轴转 d? ;②绕 各坐标轴平移dx,dy,dz O
n

s a n

求: ? 0 在
N

? 00

中的位置和姿态.

Z0

? 定义
0

dTN为微动齐次变换矩阵
0

( TN) 变化后 ? Trans (dx, dy, dz) R(w, d? ) TN

O0 X0

Y0

记为 0TN ? dTN ? Trans (dx, dy, dz)R(w, d? )0 TN

在忽略高次项 dT ? [Trans (dx, dy, dz) R(w, d? ) ? E] T 的情况下:微 令? ? Trans (dx, dy, dz) R(w, d? ) ? E , 称为微动率 . 动齐次变换与 次序无关 ? dT ? ? ? T (注意,?在T的左边)

T ? dT ? Trans (dx, dy, dz) R(w, d? )T

如果1、 R(z,d?z) 2、 R(y,d?y) 3 、R(x,d?x)
0 ?1 ?0 1 ?T ? ? ?0 d?x ? ?0 0 0? ? d?x 0? ? 1 0? ? 0 1? 0 ? 1 ? 0 ? ?? d?y ? ? 0 0 d?y 1 0 0 0 1 0 0? ? 1 ?d?z 0? ?? 0? ? 0 ?? 1? ? 0 ? d?z 1 0 0 0 0? 0 0? ? 1 0? ? 0 1?

1 ? ? d?xd?y ? d?z ?? ?? d?y ? d?xd?z ? 0 ? ? 1 ? d?z 忽略高次项 ? ?? d?y ? ? 0

0? ? d?zd?xd?y ? 1 ? d?x 0? ? ? d?yd?z ? d?x 1 0? ? 0 0 1? ? d?z 1 d?x 0 0? ? d?x 0? ? 1 0? ? 0 1? d?y

? d?z

d?y

(1)

反过来,如果1、 R(z,d?z) 2、 R(x,d?x) 3 、R(y,d?y)
?1 ? d?xd?y ? d?z ? d?yd?x d?y ? d?z 1 ? d?x ? T ? R(y,d?y) R(x, d?x) R(z, d?z) ? ? ? d?y d?x 1 ? 0 0 0 ? ? d?z d?y ? 1 ? d?z 1 ? d?x ? 忽略高次项 ?? d?y d?x 1 ? 0 0 ? 0 与式( 1 )相同 0? 0? ? 0? ? 1? 0? 0? ? 0? ? 1?

因此说,微动齐次变换与次序无关

§3.7.2 微动平移和微动旋转的齐次变换
平移:
?1 ?0 Tr ? ? ?0 ? ?0 0 1 0 0 0 dx ? 0 dy ? ? 1 dz ? ? 0 1?

旋转R ?w, d? ?,绕通过原点的任意轴

? w 旋转 d? 角:

R w,d?

? rx2 (1 ? cosd? ) ? cosd? ? ? ? rx ry (1 ? cosd? ) ? rz sin d? ?rx r z (1 ? cosd? ) ? rysin d? ? rx r z (1 ? cosd? ) ? rysin d? ? ry rz (1 ? cosd? ) ? rx sin d? ? ? r 2 (1 ? cosd? ) ? cosd? ?

rx ry (1 ? cosd? ) ? rz sin d? ry2 (1 ? cosd? ) ? cosd? ry rz (1 ? cosd? ) ? rx sin d?

cos d? ? 1, sin d? ? d?

? 1 ? r d? ? z ?? ry d? ? ? 0

? rz d? 1 rx d? 0

ry d? ? rx d? 1 0

0? 0? ? 0? ? 1?

在微动范围内 , 绕任意轴转动 d? 角 ,可以看作绕 x,y,z 轴的微 转动的合成。因此:

d?x ? rx d? , d?y ? ry d? , d?z ? rz d?
? d?z d?y ? 1 ? d?z 1 ? d ? x R?w, d? ? ? ? ?? d?y d?x 1 ? 0 0 ? 0 0? 0? ? 0? ? 1?

因此:

因此微动率△=

?Trans ?dx, dy, dz ?R?w, d? ?? ? E

? d?z d?y dx ? ? 0 ? d?z ? 0 ? d ? x dy ? ?? ?? d?y d?x 0 dz ? ? ? 0 0 0 0 ? ?
微动的齐次变换:dT= △?T

?0 ?0 己知变换矩阵 T ? ? ?1 ? 求d T ?0

1 0 0 0

0 1 0 0

7? 3? ? 0? ? 1?

转动:d? ? 0.1i ? 0 j ? 0k ,
平移: dp ? 0.3i ? 0 j ? 0.6k

解:
?1 ?0 ??? ?0 ? ?0 0 1 0 0 0 0.3? ?1 0 0 ?0 1 ? 0.1 0 0? ?? 1 0.6? ?0 0.1 1 ?? 0 1 ? ?0 0 0 0? ?1 ?0 0? ??? 0 ? ?0 ? ? 1 ? ?0 0 1 0 0 0 0 1 0 0 ? ?0 0 0 0.3? ?0 0 ? 0.1 0 ? 0? ??? ? 0? ?0 0.1 0 0.6? ? ? ? 1 ? ?0 0 0 0?

0 0.3? ?0 1 0 ?0 0 ?0 0 ? 0.1 0 ? ?0 0 1 ?? dT ? ? ? T ? ? ?0 0.1 0 0.6? ?1 0 0 ? ?? 0 0 0 0 ? ? ?0 0 0

7? ? 0 ?? 0.1 3? ??? 0? ? 0 ? ? 1? ? 0

0 0 0.3? 0 0 0? ? 0 0.1 0.9? ? 0 0 0?

? 0 ?? 0.1 T ? dT ? ? ? 1 ? ? 0
?0 ?0 T ?? ?1 ? ?0 1 0 0 0 0 1 0 0

1 0 7.3? 0 1 3? ? 0 0.1 0.9? ? 0 0 1?
1 0 7.3? 0 1 3? ? 0 0.1 0.9? ? 0 0 1?

反过来:如果我们要求Σo n在Σ o o 中的齐次交换矩阵为
7? ? 0 ?? 0.1 3? ? 实际测得的为 ? ? 1 0? ? ? 1? ? 0

那么末端执行器坐标系要如何运动才能到达期望值?
0 ?1 0 ?0 1 ? 0.1 ?? 转动: ?0 0.1 1 ? 0 ?0 0 0? 0? ? 0? ? 1?

?1 ?0 ?? 平移: ?0 ? ?0

0 1 0 0

0 0 . 3? 0 0? ? 1 0.6? ? 0 1?

§3.7.3 等效微动位移的求解
? 前面研究的是:动坐标系ΣOn在ΣOo中的 变换为T,相对于基准坐标系作微平移和 微转动,来求微动齐次变换。 ? 现在我们研究:动坐标系ΣOn相对于自身 坐标系做了微位移或微转动,达到绕基 准坐标同样的效果则如何求解。

左乘,绕基准

dT=△?T (绕基准坐标系) =T?△T (绕动坐标系)
右乘, 绕动坐标轴

强调等效

? ? ? T ? T ? ?T
? ?T ? T ? ? ? T ? ? ? T ? ?T ? T ?1
?1

?n x ?n 设: T ? ? y ?nz ? ?0

sx sy sz 0
?1

ax ay az 0

px ? py ? ? pz ? ? 1?

有: T

?1

?nx ? sx ? ? ?a ? x ? ?0

ny sy ay 0

nz sz az 0

?T ? ? ?n ? ? p ? ? T ?? ? ?s ? ? P ? ? T ?? ? ?a ? ? P ? 1 ? ?

? ?T ? T ? ? ? T
s a

n

? ? ? ? ? 0 n ?d? ? p ? dp ?? ? ? ? ? ? ? ? ?? d? ?n ? s ? s ?d? ? p ? dp ?? ? ? ? ? ? ? ? ? ?d? ?n ? a ? a ?d? ? p ? dp ?? ? ? 0 0 0 0 ? ? ? ? ? ? ? ? ? ? ? ? ? 0 ? d? ? a d? ? s n d? ? p ? dp ? ? ? ? ? ? ?? ? ? ? d? ? a 0 ? d? ? n s d? ? p ? dp ? ? ? ? ? ? ? ? ? ? ? ?? d? ? s d? ? n 0 a d? ? p ? dp ? ? ? 0 0 0 ? ? ? 0 ?

? ? ? ? ? ? d? ?s ? n ? d? ?a ? n ? ? ? ? 0 d? ?a ? s ? ? ? ? d? ?s ? a ? 0

? ? ?

? ? ?

研究绕自身轴的微动率△Τ 和绕固定坐标系坐标轴的
微动率△之间是什么关系,举例说明:
例:一动坐标系相对于固定坐标系的齐 次变换为
n ?0 ?0 T ?? ?1 ? ?0

s a 1 0 0 1 0 0 0 0

p 7? 3? ? 0? ? 1?

己知相对固定坐标系的微 ?dp ? 0.3i ? 0 j ? 0.6k ? 动平移和转动 ? d? ? 0.1i ? 0 j ? 0k
求:① △与△Τ ② 求dT ③ 求与之等效的绕动坐标系的微平移和微转动

0 0.3? ? d?z d?y dx ? ?0 0 ? 0 ? d?z ? ?0 0 ? 0.1 0 ? 0 ? d ? x dy ? ? ?? 解:① △= ? ?? d?y d?x 0.6? 0 dz ? ?0 0.1 0 ? ? ? ? 0 0 0 0 0 0 0 0 ? ? ? ??

? T d? ? a ? ?0.1i ? 0 j ? 0k ??0i ? 1 j ? 0k ? ? 0 ? ? T d? ? s ? ?0.1i ? 0 j ? 0k ??1i ? 0 j ? 0k ? ? 0.1 ? ? T d? ? n ? ?0.1i ? 0 j ? 0k ??0i ? 0 j ? 1k ? ? 0 ? ? ? d? ? p ? dp ? ?0.1i ? 0 j ? 0k ? ? ?7i ? 3 j ? 0k ? ? ?0.3i ? 0 j ? 0.6k ?

? 0.1 0 0 ? ?0.3i ? 0 j ? 0.6k ? 7 3 0

i

j k

? ?0i ? 0 j ? 0.3k ? ? ?0.3i ? 0 j ? 0.6k ? ? 0.3i ? 0 j ? 0.9k

? ? ? ? n ?d? ? p ? dp ? ? ?0i ? 0 j ? 1k ??0.3i ? 0 j ? 0.9k ? ? 0.9 ? ? ? ? s ?d? ? p ? dp ? ? ?1i ? 0 j ? 0k ??0.3i ? 0 j ? 0.9k ? ? 0.3 ? ? ? ? a ?d? ? p ? dp ? ? ?0i ? 1 j ? 0k ??0.3i ? 0 j ? 0.9k ? ? 0
? 0 ? ?? d? a ? ? ?T ? ?? ?? d? s ? ? ? 0
? 0 ? 0 ?? ?? 0.1 ? ? 0

?? ? d? a 0 ?? d? n 0

?? ? ? ? ? d? s n ?d? ? p ? dp ?? ?? ? ? ? ? ? ? d? n s ?d? ? p ? dp ?? ? ? ? ?? 0 a ?d? ? p ? dp ? ? 0 0 ? ?

0 0.1 0.9? 0 0 0.3? ? 0 0 0? ? 0 0 0?

解② :

?0 0 ?0 0 dT ? ? ? T ? ? ?0 0.1 ? ?0 0 等效 ?0 1 ?0 0 dT ? T ? ?T ? ? ?1 0 ? ?0 0

0 0.3? ?0 1 0 ?0 0 1 0.1 0 ? ?? 0 0.6? ?1 0 0 ?? 0 0 ? ?0 0 0

7? ? 0 ?? 0.1 3? ??? 0? ? 0 ? ? 1? ? 0

0 0 0.3? 0 0 0? ? 0 0.1 0.9? ? 0 0 0?

0 1 0 0

7? ? 0 ? 0 3? ?? 0? ?? 0.1 ?? 1? ? 0

0 0.1 0.9? ? 0 ?? 0.1 0 0 0.3? ??? 0 0 0? ? 0 ? ? 0 0 0? ? 0

0 0 0.3? 0 0 0? ? 0 0.1 0.9? ? 0 0 0?

解③: 绕自身平移和转动

?dpT ? 0.9iT ? 0.3 jT ? 0kT ? ? d? T ? 0iT ? 0.1 jT ? 0kT

其结果等于绕固定坐标系转 动和旋转

?dp ? 0.3i ? 0 j ? 0.6k ? ? d? ? 0.1i ? 0 j ? 0k

§3.7.4 等效微动变换的普遍形式
机器人运动学方程 :

0

Tn ? A1 A2? An
0 1 n -1

定义:前一个坐标系当作当前坐标系的基准坐标系 Σ0 i 相对于Σ 0 i ?1 是动坐标系, 如果 Σ0 i 相对于Σ 0 i ?1 产生了一个微动,它的微动齐次变换为

d Ai ? ?i

i ?1

i ?1

i ?1

Ai

? 那么这么一个微动会对末端执行器产生什么影响?因为对机 器人来讲, 我们关心的是末端执行器的运动情况。
? 0 1 i ?2 d T ? A A ? Ai ?1 ∵ n 1 2

?

i ?1

?i i ?1 Ai iAi ?1?n?1An

?

∴ ?Tn ? d ?Tn ??A1?i?2 Ai?1 ?i?1 Ai ?i?1?i i?1 Ai ? iAi ?1?n?1An
? 同理如果Σ0 i ? j 相对于Σ 0 i ? j-1 产生一个微动 有:
d i ? j ?1 Ai ? j ?i ? j ?1? i ? j i ? j ?1 Ai ? j d ?Tn ??A1?i ?2 Ai ?1 i ?1 Ai ?i ? j ?2 Ai ? j ?1

?

i ? j ?1

? i ? j i ? j ?1 Ai ? j

?

i? j

Ai ? j ?1?n ?1 An

? 这是微动齐次变换的普遍形式

? 微动率的求解
按照前面讲的等效理论有:
i ?1

?i i ?1 Ai i Ai ?1?i ? j ?2 Ai ? j ?1 ?i?1Ai ?i ? j ?2 Ai? j ?1?i ? j ?1?i? j ?1


?1 ?1 i ?1 ?1 i ? j ?2 i ? i ?1? ? i ?1 A ?i ? j ? 2 A i ? j ?1? Ai ? j ?1 ? Ai ?1 Ai i i i ? j ?1 i ? j ?1 ? ? ? ?1 ?1 i ?1 i ? j ?2 i ?1 i ?1 i i ? j ?2 ? i ? j ?1? ? A ? A ? ? A A ? Ai ? j ?1 i ? j ?1 i ? j ?1 i i i i ?1 ? ?

?

??

? ?

??

?

?

? ?

?

? 这是两个普通形式 ? 如机器人末端产生一个误差, 如果在别外一 个关节上补偿, 就要采用上面的方法。

§3.7.5 微动齐次变换的意义
说明:如果我们发现末端操作器相对于基准坐标系有 了微位移(平移或转动), 我们可以认为末端操作器相对 于自己的坐标系发生了微位移。只是微动率△和△ Τ 不同 而己。其结果是等效的。

? dp 这些在进行误差补偿和微动时有用, 如产生误差 ? d? ? 如何补偿?可以反向运动末端关节来补偿

§3.7.6 误差及误差补偿
误差来源:
? ? ? ? ? 制造和检测误差 运算过程中圆整、插补、拟合造成的误差—原理性误差 构件承受的负载、加速度、重力的变形误差 传动误差 环境影响误差

误差补偿:
? 单关节补偿 ? 多关节补偿

单关节补偿:

i

?i ?

?

i ?1

Ai

?

?1

? A1
?1

i ?1

?i ?

?

i ?2

Ai ?1

? ? ?? d T ?? A ? ?? A ? ? ?? A ? ? A ? ?? d T ?? T ? ?? A ?
? ?1 ? 5 ?1 i ?1 6 6 i ?1
1 ?1 ? ?1 ? 5 ?1 i 2 1 6 6 i ?1

?1

?

?

i ?1

Ai

?

d? i

?1

? 这是一种精确的求法, 这只是一种理想的方法。满足上 述补偿实际上是很困难的, 有时几乎是不可能的

? 有时用近似方法 认为这是理论值
i ? ? ? ? ? ? T6 ? d T6 ? T6 ? d T6 ? i ??T6 ? i ?1 ? ? ? ? ? ? T ? d T ? d T6 i 6 6 ?

?

?

? ?

??

?

?

认为这是实测值

同理:

认为这是理论值

? ?T6 ? d ?T6 ? i ?1? i ?T6 ? d ?T6 ? ? T6 ? i ?1 ?1 ? ? ? ? i ? ? d T6 T6 ? d T6 ?

?

?

?

? ??

? ?

认为这是实测值

? 这种方法是不精确的, 有误差, 但是如果能满足精度要求就 可以

? 例题
?0 ?0 ? T1 ? ? ?1 ? ?0 1 0 0 0 0 1 0 0 7? 3? ? 0? ? 1?
? 0 ?? 0.1 ? ? T1 ? d T1 ? ? ? 1 ? ? 0 1 0 7.3? 0 1 3? ? 0 0.1 0.9? ? 0 0 1?

① 按精确的方法计算 ? 绕自身坐标系 补偿
i

? i ? ? Ai ? ? ? A1 ? ?? d T6 ?? A6 ? ? ? Ai ?1 ?
i ?1 ?1 ? ?1 ? 5 ?1 i

?1

? T1
?

? ??
?1

?0 ?1 ? d ?T1 ? ? ?0 ? ?0

?

0 0 1 0

1 0 ?? 0 ?0.1 0 ? 7? ?? 0 ? 3? ? 0 ?? 0 1 ?? 0

0 0 ? 0.3? 0 0 0 ? ? 0 ? 0.1 ? 0.9? ? 0 0 0 ?

?0 ?0 ?? ?0.1 ? ?0

0 ? 0.1 ? 0.9? ? 0 d?z d?y dx? ? d?z ? 0 0 ? 0.3? 0 ? d ? x dy ??? ? 0 0 0 ? ?? d?y d?x 0 dz? ? ? ? 0 0 0 ? ? 0 0 0 0?

?d? i ? 0i ? ?? 0.1? j ? 0k ? ? ?dpi ? ?0.9i ? 0.3 j ? 0k

①这是精确的 d? i , dpi 值, 可以看出与我们以前做的例题 正好相反

?dpT ? 0.9iT ? 0.3 jT ? 0kT ? ? d? T ? 0iT ? 0.1 jT ? 0kT

? 绕固定坐标系补偿
i ?1

?i ?

?

i ?2

Ai ?1

?

?1

? A2

? ? ? A ? ?? d T ?? T ?
1 ?1 ? ?1 ? 5 1 6 6

?1

? Ai ?1

?

i

? ?

?1 i ?1

Ai

?

?1

?0 ?0.1 ? 1 ? ? d ?T1 ?T1 ? ? ?0 ? ?0

?

?0 ?0 ?? ?0 ? ?0

0 0 ? 0.3? ?0 0 1 0 ? ?1 0 0 ? 7? 0 0 0 ? ?? ? 0 ? 0.1 ? 0.9? ?0 1 0 ? 3? ?? ? 0 0 0 ? ?0 0 0 1 ? 0 0 ? 0.3 ? ? 0 d?z d?y dx? ? d?z ? 0 0.1 0 ? 0 ? d ? x dy ??? ? ? 0.1 0 0.3 ? 0.9? ?? d?y d?x 0 dz? ? ? ? 0 0 0 ? ? 0 0 0 0?

?? ?

? d? i ? ?0.1i ? 0 j ? 0k ? ? ?dpi ? ?0.3i ? 0 j ? 0.6k

? d? ? 0.1i ? 0 j ? 0k ? ?dp ? 0.3i ? 0 j ? 0.6k

② 按近似的方法计算
?0 ? 0.1 1 ? 0.6? ? 0 0 ?1 ? ?0.1 0 0 0 ? 7 . 3 ? 1 i ?? ? i ? ? T1 ? d ?T1 ? d ?T1 ? ? ?0 0 0.1 ? 3.9? ? 0 0 ? ?? 0 0 0 1 ? ??0 0 ?? 0.01 0 ? 0.1 ? 0.9 ? d?z d?y ? 0 ? 0 ? ? d?z 0 0 ? 0 . 3 0 ? d?x ? ?? ?? ?? d?y d?x 0 ? 0.1 0 ? 0.01 ? 0.09? ? ? ? 0 0 ? 0 0 0 0 0 ? ?

?

??

?

? 0.3? 0 0 ? ? ? 0.1 ? 0.9? ? 0 0 ? 0
dx? dy? ? dz? ? 0?

0 ? 0.3? ?0 ? 0.1 1 ? 0.6? ?0 0 ?0.1 0 ? ?1 ? 0 0 0 0 ? 7 . 3 ? 1 i ?1 ?? ? ? i ? ? d ?T1 ? T1 ? d ?T1 ? ? ? 0 0 ? 0.1 ? 0.9? ?0 0 0.1 ? 3.9? ? ?? ? 0 0 0 0 0 0 0 1 ? ?? ? d?z d?y dx? ? 0 0 0 ? 0.3 ? ?0 ? d?z ? ?0 ? 0.01 ? 0.1 ? 0 ? d ? x dy ? 0 . 06 ? ? ?? ?? ?? d?y d?x ?0 ? 0.1 ? 0.01 0.39 - 0.9? 0 dz? ? ? ? ? 0 0 0 0 0 0 0 0 ? ? ? ?

?

??

?

多关节补偿:
目标为:0T6 , 实际为:
0

T6 ? d 0T6

? 0 A1 ? d 0 A1 ? 0 A1 ? 0? 1 0 A1 ? 0 A1 ? 0A1 1? 1 ?1 1 1 1 1 1 1 2 A ? d A ? A ? ? A ? A ? A ? 2 2 2 2 2 2 2 ?2 我们有: ? ? ? 5 5 5 5 5 5 5 6 ? A ? d A ? A ? ? A ? A ? A 6 6 6 6 6 6 ?6 ? 6

∴ 0T6 ?d 0T6 ?
0

? A ?d A ?? A
0 0 1 1 1

1 5 5 ? d A ? A ? d A6 2 2 6

? ?

?

忽略高次项,有:
T6 ?d 0T6 ?0A1?5 A6 ? 0A1 1?1 1 A2 ?5 A6 ? 0A1 1 A2 2? 2 2 A3?5 A6 ? 0A1?5 A6 6?6
绕自身
0 0 1 1 5 0 1 2 2 5 0 5 6 d T ? A ? A ? A ? A A ? A ? A ? A ? A ∴ 6 1 1 2 6 1 2 2 3 6 1 6 ?6

绕前一个坐标系
0 0 0 5 0 1 1 5 0 1 5 5 d T ? ? A ? A ? A ? A ? A ? ? ? A A ? ? 同理 6 1 1 6 1 2 2 6 1 2 6 A 6

以Σ Oi 绕自身为例, 为了消除 d 0T6 ,令多关节做微动
d?1 , d? 2 ?d? 6 , 由此产生 ?d 0T6 补偿, 多关节的微动率为
i

? i ,(绕自身)
? 0 ? d? i ?i ? ? z ?d? y ? ? 0 ? d? z 0 d? x 0 d? y ? d? x 0 0 dx ? dy ? ? dz ? ? 0?

由于

注意:我们在定义D—H坐标系时, Z轴和回转轴重 合,因此,绕X, Y是旋转不了的。 因此 d? x ? d? y ? 0,

又如果都是转动关节, 没有移动
? dx ? d y ? dz ? 0
? 0 ? d?1 ?d? 0 1 1 ? ? ?1 ? ? 0 0 ? 0 ? 0 0 0 0 0 0? 0? ? ? 0? ? 0?
5

? 0 ?d? 6 ?6 ? ? 6 ? 0 ? ? 0
0 1

? d? 6 0 0 0
5 6

0 0 0 0

0? 0? ? 0? ? 0?

令 ?d 0T6 ?

d T6 ?
0

?

?

0

A1 ?1? A6 ? ?? A1 A2 ? A6 ? 6
1 1 5 0 1 5 6

0

A1 ?1? A6 ? ?? A1 A2 ? A6 ? 6 ? 0

?

?

得到?4 ? 4? ? 0, 按照元素对应相等, 求解 d?1 , d? 2 ?d? 6 能够求解 。 这里 ? i 是Σi 相对于自身的微转角

知识点复习

数学基础知识点:
1. 点和面的齐次坐标和齐次变换
2. 三个基本旋转矩阵 3. 绝对变换:如果所有的变换都是相对于固定坐标 系中各坐标轴旋转或平移,则依次左乘,称为绝 对变换。 4. 相对变换:如果动坐标系相对于自身坐标系的当 前坐标轴旋转或平移,则齐次变换为依次右乘, 称为相对变换。 5. 绕任意轴旋转:5步顺序

运动学知识点:
关节坐标系的建立原则
? 原点Oi:设在Li与 Ai+1轴线的交点上 ? Zi轴:与Ai+1关节轴 重合,指向任意

Ai+1

Ai-1

Ai
yi
?i

zi

? Xi轴:与公法线Li 重合,指向沿Li由 Ai轴线指向Ai+1轴线
? Yi轴:按右手定则
? ? ? ?

li li ?1 di
zi ?1
oi ?1

xi
oi

yi ?1

?i

xi ?1

杆件长度Li —沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离 杆件扭转角αi — 绕 xi 轴,由 zi-1 转向zi 杆件偏移量 di — 沿 zi-1 轴,zi-1 轴和 xi 交点至∑0i –1 坐标系原点的距离 杆件回转角θi — 绕 zi-1 轴,由 xi-1转向 xi

两种特殊情况
? 两轴相交,怎么建立坐 标系?
? Oi—Ai 与 Ai+1 关 节 轴 线 的 交点;

zi

zi ?1
oi

xi
yi

? Zi—Ai+1轴线;
? Xi—Zi和Zi-1构成的平面的 法线; ? Yi—右手定则; Ai

Ai+1

相邻关节坐标系间的齐次变换过程
——机器人运动学正解
1. 将xi-1轴绕 zi-1 轴转 ?i 角 度,将其与xi轴平行; 2. 沿 zi-1轴平移距离 di ,使 xi-1 轴与 xi 轴重合; 3. 沿 xi 轴平移距离 Li,使 两坐标系原点及x轴重合; 4. 绕 xi 轴转 ?i 角度,两坐 标系完全重合.

Ai+1

Ai-1

Ai
yi
?i

zi

li li ?1 di
zi ?1
oi ?1

xi
oi

yi ?1

?i

xi ?1

i ?1

Ai ? R(Zi ?1,?i )Trans (Zi?1, di )Trans ( xi , li )R( xi ,?i )

D-H变换矩阵
i ?1

Ai

=

?cos? i ? sin ? i ? ? 0 ? ? 0

? sin ? i cos? i 0 0

0 0 1 0

0? 0? ? 0? ? 1?

?1 ?0 ? ?0 ? ?0

0 1 0 0

0 0 ? ?1 ?0 0 0? ? ? 1 d i ? ?0 ? ? 0 1 ? ?0

0 1 0 0

0 0 1 0

li ? 0? ? 0? ? 1?

0 ?1 ?0 cos? i ? ?0 sin ? i ? 0 ?0

0 ? sin ? i cos? i 0

0? 0? ? 0? ? 1?

=

?cos? i ? sin ? i ? ? 0 ? ? 0

? cos? i sin ? i cos? i cos? i sin ? i 0

sin ? i sin ? i ? sin ? i cos? i cos? i 0

ai cos? i ? ai sin ? i ? ? di ? ? 1 ?

机器人的运动学正解方程
0

Ti ? A1 ? A2 ???
0 1

i ?1

Ai

运动学逆问题
? ? ? ? ? 逆运动学的定义 逆运动学的存在性 逆运动学的可解性 逆运动学的多解性(剔除办法) 逆运动学解法(数值解、解析解)
How do I put my hand here?

运动学逆问题
Inverse Kinematics: Choose these angles!

运动学逆问题解法
? 迭代法——计算量大 ? 几何法——适用于自由度较少的情况 ? 反变换法
? 用未知的逆变换逐次左乘,由乘得的矩阵方程的元素决定 未知数,即用逆变换把一个未知数由矩阵方程的右边移到 左边 ? 考察方程式左、右两端对应元素相等,以产生一个有效方 程式,理论上可得到12个方程。 ? 然后求这个三角函数方程式,以求解未知数 ? 把下一个未知数移到左边 ? 重复上述过程,直到解出所有解 ? 通常用四象限的反正切函数来确定值 ? 缺点:无法由数种可能的解中直接得出合适的解, 需要通过人为的选择

高腕

低腕

取前一个采样点的值
z4
O4

z5
x5

y5
O5

x4

微动变换:

左乘,绕基准

dT=△?T (绕基准坐标系)

强调等效

=T?△T (绕动坐标系)
右乘, 绕动坐标轴

? ? ? T ? T ? ?T
? ?T ? T ?1 ? ? ? T ?i? i
误差补偿:

? ? ? T ? ?T ? T ? ? i

?1

i ?1

单关节补偿 多关节补偿

大作业:PUMA机器人
a2 d2 d4 d6

a3 d1

d3

求解:①建立坐标系;②给出D-H参数表;③推导正运动学、 逆运动学;④编程得出工作空间

A1

Z1

A2 a2

Z0
Y1 Y0 Z0 d1 X3 X1 d2 Y3 Z3

A3 Z2 X2 d3 d4 A5 Y5 Z4 X5 X4 Z5 d6 Y6 s

X0

a3 Y2
Z2 Y4 X2

X0
Y0

Y2

A4 A6

6 n X6 Z a

PUMA机器人的杆件参数表
关节i 1 2

θi
90 0

αi
-90 0

li
0

di
0

运动范围
-160? ~ 160?

a2

d2
0

-225? ~ 45?

3
4 5 6

90
0 0 0

90
-90 90 0

a3
0 0 0

-45? ~ 225?
-110? ~ 170?

d4
0

-100? ~ 100?

d6

-266? ~ 266?


相关文章:
机器人逆运动学
机器人逆运动学_电脑基础知识_IT/计算机_专业资料。...一、建立机器人数学模型(连杆机构),如下: >> l1...第三章 工业机器人运动学... 27页 免费 机器人...
机器人技术基础期末考试复习资料(熊有伦主编)
第三条:机器人必须保护自己,除非这种保护与以上两条...缺点:功能有限,无法进行繁复的数学运算,不接受浮点...5. 机器人运动学:从几何学的观点来处理手指位置与...
机器人技术基础(课后习题答案)
机器人技术基础(课后习题答案)_理学_高等教育_教育专区。机器人技术基础(课后...(2)关节空间运动。这种运动直接在关节空间里进行。由于动力学参数及其极限值直接...
机器人技术基础(熊有伦)复习资料
机器人技术基础(熊有伦)复习资料_工学_高等教育_教育专区。第一章 1 机器人...运动轴旋转的顺序相反并且旋 转角度相同,能得到相同的变换矩阵,都是用三个...
机器人运动学实验报告
仪器与设备 1. KLD-400 型 SCARA 教学机器人 2....矢量空间的相互变换,是机器 人运动学中的基本数学...第三章 机器人运动学 77页 1下载券 机器人运动学...
机器人正运动学方程的D-H表示法
机器人运动学方程的D-H表示法_经济/市场_经管营销_专业资料。行业分析报告,...第4章 机器人控制技术-运... 9人阅读 38页 2下载券 华科机器人原理及控制...
机器人正运动学方程的D-H表示法
机器人运动学方程的D-H表示法_数学_自然科学_专业...这已成为表示机器人和机器人运动进行建模的标准方法...第二个关节为关节 n+1,第三个关节为关节 n+2。...
机器人学第三章(机器人的机型与结构)
机器人学第三章(机器人的机型与结构)_工学_高等教育_教育专区。机器人学第...机器人学第二章(数学基础... 机器人学第四章(机器人的... 机器人学第五章...
第2章 工业机器人运动学
第2章 工业机器人运动学_数学_自然科学_专业资料。...齐次变换及运算 2-3 工业机器人连杆参数及其齐次...[知识点及其基本要求] 1、点的位置描述(掌握) 2...
机器人技术试题及答案
第2章 1、机器人系统由哪三部分组成?答:操作机、...50-57 5 机器人运动学解决什么问题?什么是正问题...求解和各种插补运算等数学方法最终生成相应的关节运动...
更多相关标签: