机器人空间三点圆弧功能的实现Word格式.docx
《机器人空间三点圆弧功能的实现Word格式.docx》由会员分享，可在线阅读，更多相关《机器人空间三点圆弧功能的实现Word格式.docx（13页珍藏版）》请在冰点文库上搜索。
Keywords:
robot;
areinspace;
interpolation;
vector;
error
机器人一般应用于比较恶劣或人难于企及的
环境以替代劳动者完成必要的任务,因而机器人
的编程,很多情况下是采用示教完成的口].示教过
程包括把机器人移动到几个所要求的目标点并把
这些点的位置记录在存储器中,然后定义经过这
些点的曲线轨迹及速度.当曲线轨迹为圆弧时,除
了示教圆弧起点（当前点）和终点外,至少还应知
道圆心或圆弧上一中间点.显然,示教圆心是很困
难的,因而机器人终端执行器的轨迹圆弧通常由
示教的圆弧起点,中间点及圆弧终点决定,而这三
点所决定的平面通常不一定平行于某一坐标平
面,因而需要研究空间任意三点圆弧的插补算法.
本研究以上述需求为出发点,基于机器人终
端执行器（根据使用场合的不同,可以是爪,焊枪,
电磁铁和其他装置）经历的空间任意三点,推导出
了机器人控制系统中任意空间圆弧的实现方法,
并分析了该算法的性能及特点.
1由空间任意三点求圆弧的圆心和
半径
已知空间任意三点分别为圆弧起点P.（X.,
y.,Z.）,中间点P（X,Y,Z）和圆弧终点
P（X,Y,Z）,如图1所示.设圆心为C（Xc,
y,Zc）,半径为R,则有
lc--P.l===lc--Pl—lc--Pl—R;
（1）
c--P.×
一（丽×
）.
（2）
收稿日期:
2006—07—12.
作者简介:
叶伯生（1966一）,男,副教授;
武汉,华中科技大学国家数控系统工程技术研究中心（430074）.
E-mail:
yebosh@
?
6?
华中科技大学（自然科学版）第35卷
图1空间三点决定的圆弧
由式
（1）得
（X一Xc）+（y—yc）+
（Z—Zc）一R（一0,1,2）.（3）
设坐标系0XyZ上各坐标轴的单位坐标矢量分
别为i,-,,k,则有
.:
（x.一xc,y0一yc,z.一Zc）,
P0一（X1一X0,Y1一y0,Z1一Zo）,
P1P一（X2一X1,Y2～Yl,Z2一Z1）.
于是
.
x—P
oP1一
liJ『kl
JX.一Xcy0一ycZo—ZcJ,
Ixl—x.y1一y0z1～z.I
—-----——
●—-——?
——+
P0P1×
PlP;
:
}i-,kI
lX1一X.y1一y0Z1一Z.I.
1x2一x1y2一ylz2一z1J
由式
（2）得
[（y0一yc）（Z一Z.）一（一Zc）（y一
Yo~3/E（Y一Yo）（Z2一Z）一（z一Z.）（y2～
Y1）]一E（z.一Zc）（Xl—Xo）～（X.一
Xc）（Z1一Zo）-1/Uz一Z.）（X2一X）一
（X1一X.）（Z2一Z1）]一.（4）
式（3）和式（4）化简后可得
aExcZc]一B,（5）
R一[（X.一Xc）+（—yc）+
（Z0一Zc）]1/（6）
式（5）中,A为3×
3矩阵,a..一2（x.一X1）,ao.一
2（y0一Y1）,a02=2（Zo—Z1）,alo一2（Xl—X2）.
a11:
2（Y1一Y2）,al2—2（Z1一Z2）,一
口02（口o2a11一a01al2）/8,a2I==:
ao2（a0oai2一aL）2lf）/
8,a22一[a00（a02口11一a0la12）+abl（a）l!
一
a.2a1.）]/8;
B为3×
1矩阵,b.一（++Z）一
（X}+y}+Z}）,b=（X}++Z}）一（x;
+y;
+
Z;
）,b2一口20X0+a21Y0+a22Z0.
由式（5）求得,yc,Zc,由式（6）求得R.
在求解过程中应注意平行于某一坐标平面的
圆弧的处理.此时X,,Zi（i一0,1,2）中,有一
个坐标值是不变的,容易出现detA一0（矩阵A的
秩rantc4—2）的情况.在这种情况下,应去掉矩阵
A和矩阵B的第3行以及矩阵A等于0的1列和
EX.,Yc,Z.=rr的相应1行（若X一0,则去掉A
的第1列和[X.,y.,Z.]r的第1行X.;
若Y=
0,则去掉A的第2列和Exc,Yc,Z]的第2行
Yc;
若Z:
0,则去掉A的第3列和[X.,y.,
Z.]的第3行Z）,形成一个二元一次方程组,
求解未知的两个圆心坐标.
2插补算法
2.1插补原理
任意空间圆弧插补,就是求出在一个插补周
期T内,机器人终端执行器从当前位置（X,Y,
Zi）和方位（,∞,）沿圆弧割线上截取弦长厂一
FT（F为编程速度）后,所到达的下一个插补点的
位置（X…,Yi+1,Z川）和方位（+1,a,
届+1）～.
方位的插补一般采取线性方式,即把终端执
行器在圆弧终点和起点的方位差均匀地分配到插
补的每一步,算法简单,本文不作讨论.
2.2插补递推公式
在数控机床中,平行于坐标平面的圆弧编程
般只给出起点,终点和圆心,因而需要用G02
和G03区分为顺时针圆弧和逆时针圆弧.而给定
了起点,终点和一个中间点的空间三点圆弧的走
向是确定的,因此无需用顺时针和逆时针区分.事
实上,用丽×
丽表示空间三点圆弧所在平面
的法矢量,l,则从,l的正方向看,从P到P到
P的圆弧始终是逆时针圆弧.所以,可用统一格
式ARCX.一y.一ZfX1一y1一Zl—X2一y2一Z2一描述
空间三点圆弧-6_.
机器人的控制系统读入此段程序后,首先求
出圆心和半径,然后再计算插补点的坐标.
设,l一丽×
一++,则
"
一（yl—Yo）（Z2一Z1）一
（Z.一Z0）（y2一Y1）;
一（z,一）（x2一x1）一
（7）
（X1一Xo）（Z2一Z1）;
叫一（X1一Xo）（y2一Y1）一
（y,一Yo）（X2一X1）.
空间三点圆弧上任一点P（X,Y,Z）处沿
前进方向的切矢量
第8期叶伯生:
机器人空间三点圆弧功能的实现?
7?
i+J『+llk=,l×
I
Xt—Xc
.,
y一yc
硼
Z—Zc
则
rf一（Z一Zc）一硼（Y—Yc）;
f=硼（X—Xc）～"
（Zf—Zc）;
（8）
lz一"
（Y—Yc）一（X—Xc）.
设经过一个插补周期后,机器人的终端执行
器从P（X,Y,Z）沿圆弧切向移动FT后（其
中:
F为编程速度;
T为插补周期）,到达
P:
+（x:
+,t+,z:
+）,女Ⅱ图2所示,贝U
rX斗1一X+△X一X+Em;
1一Y+△—Y+En;
（9）
lZ1一Z+△=Z+E/,
式中E—FY/（++l）.可以证明
++z一（"
硼）?
[（X—Xc）+（y—yc）.+（Z一Zc）]+
[（X—Xc）"
+（y一Yc）+（Z一Zc）∞]一
（硼）R,.
从而,
E—FT/ER（"
++硼）].（10）
从图2可以看出,点P:
+并不在圆弧上,为
使所有插补点都落在圆弧上,需对式（9）进行修
正.连接cPi+交圆弧于P点,以P…代替+
作为插补点,则插补点始终在圆弧上.
图2空间圆弧插补原理
在直角ACPP:
+中,I+Iz—IIz+
I+I,即（R+△R）一Rz+FTz,有
rx斗1一xc+R（x1一xc）/（R+FT）1/2;
JY斗1一Yc+R（1一yc）/（R+FT）/;
Iz斗一zc+R（ZI+1一zc）//（R+FT）/.
（11）
令
G—R/（R+）1/2一l/[1+（/R）],
（12）
并把式（9）代人式（11）,得插补递推公式
rX1一Xc+G（X+Em一Xc）;
Y一Yc+G（Y+Enf—yc）;
（13）
IZ斗1一Zc+G（Z+E/～Zc）.
2.3终点判别
圆弧插补的终点判别只需算出圆心角和步
距角,以两者的商作为插补次数,参考图2,有
一arcsin（FT/R）≈FT/R.（14）
圆心角的计算则要考虑如图3所示的≤丌
（圆弧P.PP）和&
gt;
丌（圆弧P.PP）两种情况.
当≤丌时,
0—2arcsin{[（X2一Xo）+（y2一
y.）+（Z2一Z.）]/（2R）};
（15）
当&
丌时,
0—2丌一2arcsin{[（X2一Xo）+（y2一
）+（Z一）]/（2R））.（16）
从图3可以看出,当≤丌时,矢量.×
P.
P
Po
图3圆心角的计算
与圆弧所在平面的法矢量,l（一×
）方向相同;
丌时,矢量.×
与
l方向相反.因而可用公式
H一训1+删1+伽1（17）
的正负来判断的范围,当H≥0时,≤丌;
当
H&
lt;
O时,&
丌.式中"
1,硼1为矢量Cp.×
P.在各坐标轴方向上的分量,
1一（yo—Yc）（Z2一Zn）一
（一Zc）（y2一Y.）;
1一（一Zc）（x2一Xo）一
（18）
（X.一Xc）（Z2一）;
硼1一（Xo—Xc）（y2一Y0）一
（yo—Yc）（X2一Xo）.
算出和后,插补次数（不包括点）
N—Eo/a~+1.（19）
2.4误差分析
由插补递推过程知,插补点总在圆弧上,算法
没有累积误差.
8?
从图2和式（14）可知,每次插补走过的步距
角是不变的,因而每个插补周期走过的弦长是不
变的,即进给速度是恒定的.只是弦长lPP…l稍
稍小于切线段长度lPP:
+l（=FT）,相对误差为
很小,可以忽略其高次项）
（1PPl—IPPI）/lPPl=
（L～2Rsin（8/2）/（F丁）=1—2sin（8/2）/8—
1—218/2一（8/2）./（31）+（8/2）/（51）～
…
3/8≈/（241）.
插补的弓高误差约为F/（8R）.
2.5插补算法步骤及实时性分析
根据以上分析,可以构造出如下空间三点圆
弧的插补算法步骤（注意:
为避免混淆插补动点与
圆弧起点,中间点和终点,此处用XE1]描述x,
用y[]描述y,用zEi]描述Z）.
a.获取已知条件:
圆弧起点P.（X.,Y.,
Zo）,中间点P（X,y,Z1）和圆弧终点Pz（Xz,
y2,Z2）,编程速度F及插补周期T.
b.由式（5）求圆弧的圆心C（X.,Y.,）,
由式（6）求圆弧半径尺.
c.由式（7）求圆弧所在平面的法矢量n的分
量u,,,由式（18）求"
1,1,1,由式（12）求
G,由式（14）求.
d.由步骤c求得的结果,根据式（10）算出
E,根据式（17）算出H.
e.根据H的正负由式（15）和式（16）算出,
并由式（19）算出插补次数N.
f.置一0,插补起点为P.（X.,Y.,Zo）,即
x[o3:
X.,y[O]一Yo,zro3一Z0.
g.=+1,由式（8）求m,,z.
h.由式（13）确定下一插补点.
i.若&
N,则转g.
j.xEN3=X2,y[N]一Y2,Z[』\r]一Z2,结
束.
步骤a～e在插补预处理中完成.因而实时插
补过程由式（8）和式（13）构成,共有9次乘法运
算,计算量不大,在主频166MHz的工控机上验
证,平均每次的插补运行时间约为0.1ms.
大部分机器人是关节机器人,系统控制的是
关节坐标轴.因此,用上述方法算得插补动点后,
还得通过坐标变换把动点的位置和方位转换为关
节坐标,然后控制相应的关节转动一定的角度,到
达所要求的位置和方位.
不同结构的机器人,其变换矩阵会不同,但对
于任一给定的机器人,其位置和方位与关节的关
系是一定的.因而只要插补点的位置在机器人的
运动范围内,总可以找到相应关节坐标系下的
解83.
误差分析和实时性分析表明,本文研究的算
法能满足机器人控制的精度及实时性要求,本研
究成果已成功应用于华中I型教学机器人.
事实上,在机械加工中,如果机床数控系统具
有空间任意圆弧插补功能,可达到简化编程,提高
加工效率和精度的目的.因此,本研究成果也可应
用于高性能机床数控系统.
参考文献
[1]熊有伦.机器人技术基础[M].武汉:
华中理工大学出
版社,1996.
[2]叶伯生.计算机数控系统原理,编程与操作[M].武
汉:
华中理工大学出版社,1999.
[3]秦开怀,金建新,宾鸿赞.CNC系统中任意三维椭圆
弧的高速插补新方法[J].华中理工大学,1992,
20（6）:
7-11.
[4]金建新.机床CNG系统中任意空间曲线的可控步长
插补方法[J].机械工程,2002（4）:
95—97.
[5]别卫春,朱志红,叶伯生.HNC—IR机器人语言解释系
统的研究与实践[J].机电一体化,2000（3）:
27—3O.
[6]叶伯生,朱志红,刘恩沧.基于PC的教学机器人控制
系统[J].机械与电子,2000（6）:
43—45.
[7]张毅,叶伯生,朱志红.HNc——IR机器人运动学
方程的研究及求逆的新方法[J].机械设计与制造.
2000（3）:
40—42.
[8]王奇志,徐新和,尹朝万.PUMA机械手逆运动方程
新的推导方法及求解[J].机器人,1998（3:
81—87.
升级会员 