应用旋量理论建立闭链机器人的动力学方程

收稿日期,20001120
作者简介,荆学东(1968-) 男(汉) 安徽 博士生
E -mail ,jxd 200969@sina .com
荆学东
文章编号,1003-8728(2003)01-0058-03
应用旋量理论建立闭链机器人的动力学方程
荆学东1 尚久浩2
(1上海交通大学机械工程学院 上海
200030;2西北轻工业学院 咸阳
712081)
摘
要,提出了应用旋量理论和Lagrangian 方程建立闭链机器人动力学方程的一种方法 并以FANUC 机器人为例 阐述了该方法的具体步骤O
关
键
词,机器人;旋量;指数积公式;动力学
中图分类号,O 174.21
文献标识码,A
A New method f or establishing the dynamic eguation of a closed -chain manipulator based on screw theory
JING Xue -dong 1 SHANG Jiu -hao
2
(1School of mechanical Engineering Shanghai Jiaotong University Shanghai 200030;
2
Northwest Institute of Light Industry Xianyang 712081)
Abstract ,A new method is presented for establishing the dynamic eguation of a closed -chain manipulator based on Lagrangian eguation and screw theory which is illustrated in formulating the dynamic eguation of a FANUC robot .Key words ,Robot ;Screw theory ;Closed -chain manipulator 通常采用Denavit -Hartenberg 参数法描述机器人的运动[1 2] 并以此为基础建立机器人的运动学方程 进而建立其动力学方程O 与它相比 采用运动螺旋充分利用了其几何特性 从而更适合从整体上描述机器人的运动O 旋量理论的基本原理由Chasles 和P oinsot 于19世纪初期提出O 之后 Robert S .B all 对他们提出的旋量理论进一步发展 于1900年形成了完整的旋量理论O 旋量理论较新的方法是用矩阵的指数映射描述刚体运动[3]O 基于这种方法的指数积公式将开链机器人的运动方程表示成运动旋量的指数积 从而为开链机器人提供了完整的几何描述O 基于此 文献[3]提出了应用旋量理论建立开链机器人动力学方程的方法O 本文将指数积公式应用于闭链机器人 并以此为基础提出一种建立闭链机器人动力学方程的方法O 1
刚体运动的指数坐标和旋量运动图1
旋量运动
对于刚体做一般运动 可用旋量运动来加以描述O 如图1所示 假设g
3
为刚体上的一个点 c =(c 1c 2c 3)T 为一
单位矢量 L 为过点g 沿
c 方向的有向直线 定义
先绕轴线L 旋转角度
再沿与轴线L 平行的方向移动距离c = 的运动为旋量运动O 设刚体相对于惯性坐标系A 的初始位形和运动后的位形分别为g ab (0)和g ab ( ) 则
g ab (
)=e ^
g ab (0)(1)
式中,g =e ^
=R p []01=e c ^ (1-e c ^
)(-c >g )- c []0
1
(2)
式中,c
^=0-c 3c 2c 3
0-c 1-c 2
c 1
r
L 1
0 so (3)={S 3>3
,S T =-S };e c ^
SO (3)={R 3>3
,RR T =R T R =1 det R =1}为矩阵指数 其定义为
e c ^ =1-
c ^- - n
nj
c ^
n - =1-c ^sin -c ^
2(1-cos )(3)式(3)称为Rodrigues 公式;称c
3
为旋转矩阵R =e c ^
的指数坐标O 式(2)中, ^ se (3)={(z c ^),U 3 c ^ so
(3)};在齐次坐标中 ^可写为, ^=c ^
U []
00
4>4;称se (3)中的元素为运动旋量;引入运算符V ,c ^
U []00
V
=[]U c 称 =(U c )T
6
为 ^ se (3)的运动旋量坐标;g =e ^ SE (3)={(p R ) p
3
R SO (3)}为由旋量表示的刚体变换O 变换g 的伴随变换定义为
Ac g =R ab p ^
ab R ab
0R [
]
ab
(4)
第22卷2003年第1期1月机械科学与技术
mECHANICAL SCIENCE AND T ECHNOLOG Y
V ol 22January No.1
2003
2
开链机器人的Jacobian 矩阵
设机器人的工具坐标系为T ,定义n 自由度开链机器人的关节变量0=(01,02, ,0n )T E 0(0为机器人的关节空间),位形为g T
O
(0)E SE (3),初始位形为g T O
(O),对于每个
关节z ,构造一个运动旋量E ^z ,E ^z 对应于除第z
个关节外,其它关节j 均固定于0j =O 位置时的旋量运动,设c z E R 3为
运动旋量E ^z 轴线上的单位矢量,为G z E
3为轴线上任意一点,对于转动关节和移动关节(设U z 为移动方向的单位
矢量),E ^z
的运动旋量坐标形式分别为E ^
z =
-c z >G z
c
[]
z
(5)E ^
z =
U z
[]
O (6)
利用式(2)将n 自由度开链机器人的n 个关节的运动组合可得其运动学正解映射g T O (0):0 SE (3)
g T
O
(0)=e
E ^101
e E ^
202
e
E ^n 0n
g T O (0)(7)
式(7)称为指数积公式[3],
开链机器人的物体Jocabian 矩阵为J b OT (0)E
6>n
:
V ^b OT =g T O (0)-1
n
z=18g T O
(0)80z 0-
D z = n
z=1g
T O (0)-1
8g T O
(0)
80 D
z
0-
z
故
V
b
OT =J b OT (0)0-
(8)
其中
J b OT (0)=
g T O (0)
-1
8g T O
80
D
1
V
g T O (0)
-1
8g T O
80 D
z
V
[ g T O (0)
-1
8g
T O
80
D n
]
V
=[E b 1E b 2E b 3
E b n ]
(9)
式中:E b z =Ac (g O z
)-1E z ,E O z =e E ^
z 0z e E ^
z-10z-1 e E ^
n 0n g T O
(O)3
刚体动能和广义惯性矩阵
图2
刚体质心坐标系示意图
为了用Lagrangian
方程建立机器人动力学方程,需求出机器人各连杆(刚体)的动能,如图2所示,将物体坐标系建立于刚体的质心,并设刚体位形为g cb =
R p
[]O 1,对
于刚体上点1b E 3
(在物体坐标系下描述)的空间速度U 1c
为:U 1c
=R
-1b -p -,由动能定义可得位形g cb 下刚体的瞬时动能为
T =12(V b cb )T
m1O O
1
[
]
C
b
V b cb =
12
(V b cb )T 1C Gb V b
cb (1O)式中:
1C Gb =
m1O O
1[]C b
;
1C b =-
U
0(1b )1^
2b dV =
1II 1Iy 1I 1yI
1yy
1y 1 I 1 y
1T L
J
;
1II = U
0(1b
)(y
2
b
- 2
b )dV;
1yy = U
0(1b
)( 2
b
-I 2b
)dV;
1 =
U
0(1b
)(I
2b
-y 2b )dV;
1Iy =1yI =-
U
0(1b )(I b y b )dV;
1I =1 I =-
U
0(1b )(I b b )dV;1y =1 y =-
U
0(1b )(y b b )dV
称对称矩阵1C b
为物体坐标系下的惯性张量,称对称矩
阵1C Gb 为物体坐标系下的广义惯性矩阵,4
闭链机器人的动力学方程
本文提出利用旋量理论建立闭链机器人动力学方程的方
法为:将闭链分解成开链,然后针对每条开链,利用运动螺旋的指数积公式确定机器人各连杆质心坐标系的位形;之后利用Jocabian 矩阵确定机器人的动能;最后选取机器人的主动关节变量为广义坐标,在毋须求切开处的约束力和约束力矩的前提下,用Lagrangian 方程建立闭链机器人动力学方程,下面将以FANUC 机器人为例,阐述此方法具体步骤:
(1)分解闭链,建立各连杆质心坐标系,
如图3所示,FANUC 机器人可以看成是由开链1(包括关节1~2~3~4~5和6)及开链2(包括关节1~7~8~9~4~5和6)组成,机器人各质心坐标系C z (z =1*8)的初始位形
g C z O (O)=
1
p cz (O)[]
O 1
(
11)
图3FANUC 机器人各连杆质心坐标系的初始位形图
(2)利用每个连杆基于质心坐标系的Jacobian 矩阵~
广义惯性矩阵和相关的关节变量,计算每个连杆的动能,进而求得系统的总动能,
9
5第1期荆学东等:应用旋量理论建立闭链机器人的动力学方程
设m
z 为连杆z的质量;1C z
b=
1xxz1x z1x z
1xz1z1z
1xz1z1
L J
z
为连杆z
在物体坐标系下的惯性张量o则选择这样的初始位形后,连杆z在物体坐标系下的广义惯性矩阵为
1C z Gb=
m z1O
O1C z []b
设0
1=[010*********]T~02=[010708]T,
由式(7)可得开链1中各连杆质心坐标系C z(z=1(6)和开链2中连杆7和8运动后的位形分别为
g C z O(01)=exp(E^101)exp(E^202)~exp(E^z0z)g C z O(O)
g C7O(02)=e E^101e E^707g C7O(O);g C8O(02)=e E^101e E^707e E^808g C8O(O)
由式(8)可计算出开链1和开链2的物体速度为V b
OCz=J b OCz (0)0-(0=01或02);再由式(1O)可求得连杆1(6的动能之和及连杆7和8的动能之和分别为
T16=0T1M1(01)01,T78=0T2M2(02)02
其中
M1(01)=(M1zj)6>6=
6
z=1
J T z1C z Gb J z
M2(02)=(M2zj)3>3=
8
z=7
J T z1C z Gb J z
(3)选择机器人所有的主动关节变量作为广义坐标,
将各连杆的动能改写成统一的形式:T
z=1
2
0T M z(0)0(0为
由主动关节变量构成的广义坐标向量)o
选择FNAUC机器人的主动关节变量0=[0
102030405 06]T作为广义坐标向量,将08=02-07,03=07-02分别代
入M1(0
1)和M2(0
2)
并经改写可得M1(0)和M2(0),则T16=
1
2
0T1M1(01)01=
1
2
0T M1(0)0
T78=
1
2
0T2M2(02)02=
1
2
0T M2(0)0
<
(
L
系统的总动能为
T=T16+T78=1
2
0T M(0)0(12)
式中:M(0)=M1(0)+M2(0),为了书写简便,令G=[G1G2G3G4G5G6]T=0,则式(12)可改写为
T=1
2
0T M(0)0=
1
2
G-T M(G)G-(13)
(4)计算系统总势能V(G)o 系统的势能为
V(G)=V(0)=
8
z=1m z g z(0)=
8
z=1
m z g z(G)
(5)计算系统的Lagrangian函数o 系统的Lagrangian函数为
L=1
2
G-T M(G)G--V(G)
(6)利用Lagrangian方程求解关节驱动力o 将Lagrangian函数代入Lagrangian方程
d dt 8L
8G-z
-
8L
8G z
=t z z=1,2,~,6(14)
t z作用于第z个关节的驱动力矩和其他非保守广义力o
将:8L
8G-z
=
1
2
6
k,j=1
8M kj
8G z
G-k G-j+
8
8G-z
V(G)和
d
dt
8L
8G-z
=
1
2
6
j=1
6
k=1
8
G k
M jz(G)G-k+
8
8G k
M jz(G)G-
(D k G-j+126j=1(M jz(G)+M zj(G))G~j
代入式(14)得
1
2
6
j=1
(M jz(G)G-k+M zJ(G)G~j+
1
2
6
j=1
6
k=1
8
G k
M jz(G)G-k+
8
8G k
M zj(G)G-
(D k G-j-
1
2
6
k,j=1
8M kj
8G z
G-k G-j+
8
8G-z
V(G)=t z(15)
定义机器人的惯性矩阵M G(G)=(M G
zj)6>6:M G zj=
1
2
6
j=1
(M jz(G)+M zj(G));定义科氏矩阵C(G,G-)=(C zj)6>6:C zj
=
1
2
6
k=1
8
G k
M jz(G)+
8
8G k
M zj(G)-
8
G z
M kj
(D(G)G-k
将外力分为两部分:D作用于关节z的驱动力矩z
z;@
作用于第z个广义坐标的所有其他外力-N
z(G,G
-),则式
(15)可写成如下形式
M G(G)G~+C(G,G-)G-+N(G,G-)=z(16)
式中:z=[z
1z2z3z4z5z6]T o
至此,FANUC机器人的动力学方程得以建立o只要已
知G~G-和G~,就可以通过式(16)求得主动关节的驱动力矩z o
5结论
(1)本文提出的方法为解决闭链机器人的动力学问题
提供了基础o
(2)上述方法的关键是将利用从动关节变量和主动关
节变量的关系,通过选取机器人的主动关节变量为广义坐
标,并结合各条运动链的特点,将机器人各连杆的动能改
写成统一的形式,进而确定闭链机器人的总惯性矩阵o
(3)由于工业机器人通常采用较为简单的传动机构,
与采用Denavit-Hartenberg参数法相比,应用运动螺旋坐
标描述机器人的运动将更简洁~直接,因而更易于确定机器
人的运动学方程,进而建立机器人的动力学方程,从而更有
利于从整体上研究机器人的运动o
[参考文献]
[1]吉川恒夫著.d y$制御基础论[].d社,1
[2]小岛利夫著.d y$制御[].d社,18
[3](美)理查德摩雷,(中)李泽湘,(美)夏恩卡萨斯特里著,
徐卫良,钱瑞明译.机器人操作的数学导论[].机械工业出
版社,18
[4] c i ing.Fundamentals of robotics analysis and con-
trol[].P renti c e Ha,I n c.1O
[5]a SO n T,a i S b u r y K r.robot hands and the me-
chanics of manipulator[].t e I T P re SS,185
O6机械科学与技术第22卷
应用旋量理论建立闭链机器人的动力学方程
作者：荆学东， 尚久浩
作者单位：荆学东(上海交通大学,机械工程学院,上海,200030)， 尚久浩(西北轻工业学院,咸阳,712081)
刊名：
机械科学与技术
英文刊名：MECHANICAL SCIENCE AND TECHNOLOGY
年，卷(期)：2003,22(1)
被引用次数：7次
1.吉川恒夫ロッボト制御基础论 1999
2.小岛利夫ロッボトの制御 1998
3.理查德·摩雷;李泽湘;夏恩卡·萨斯特里;徐卫良,钱瑞明机器人操作的数学导论 1998
4.Schilling R J Fundamentals of Robotics Analysis and Control 1990
5.Mason M T;Salisbury J K Jr Robot Hands and the Mechanics of Manipulator 1985
1.韩玲会应用旋量理论求解FANUC机器人的正向运动学问题[期刊论文]-现代机械2003(3)
2.张炜21世纪青少年机器人创新教育[期刊论文]-机器人技术与应用2008(5)
3.冯春时.丛爽.张耀欣.尚伟伟基于粒子群算法的并联机器人运动学参数完全自标定[会议论文]-2007
4.荆学东.浦耿强.王成焘应用基于运动螺旋的机器人正解映射求解 FANUC机器人的逆运动学问题[期刊论文]-机械科学与技术2003,22(3)
5.陈正洪.王勇.李艳.CHEN Zheng-hong.WANG Yong.LI Yan两自由度闭链机器人的神经网络自适应控制[期刊论文]-上海大学学报（英文版）2008,12(5)
6.王卓.赵杰.王卫忠基于旋量理论的可重构机器人运动学的研究[期刊论文]-机械与电子2004(7)
7.林光春.杨志刚.徐礼钜.LIN Guangchun.YANG Zhigang.XU Liju基于并行计算的杆长和惯性参数可变的平面闭链机构动力学解析模型[期刊论文]-机械工程学报2008,44(2)
8.周东辉机器人双臂协调闭链奇异性研究[会议论文]-1998
9.吕晓俊.钱瑞明.LV Xiao-jun.QIAN Rui-ming可重构机器人运动学正逆解的算法研究[期刊论文]-机械设计2006,23(3)
10.荆学东.尚久浩基于旋量理论的开链机器人动能的探讨[期刊论文]-现代机械2001(3)
1.王红旗.王泰华基于旋量理论的移动机械手动力学建模[期刊论文]-兵工学报 2010(6)
2.王红旗.王庆林移动机械手鲁棒自适应模糊控制[期刊论文]-控制与决策 2010(3)
3.许恩永.刘正士.王勇.丁乾坤基于旋量理论的排球发球机器人挡板模块的运动学和动力学方程[期刊论文]-合肥工业大学学报（自然科学版） 2008(7)
4.王红旗.胡伟.孙雪凤基于旋量理论的移动机械手建模及轨迹跟踪[期刊论文]-河南理工大学学报（自然科学版） 2010(5)
5.王红旗.张伟非完整移动机械手鲁棒自适应模糊控制[期刊论文]-控制工程 2011(1)
6.王树国.张付祥.付宜利闭链级联式机器人基于旋量理论的动力学分析[期刊论文]-哈尔滨工程大学学报。