机器人空间三点圆弧功能的实现.doc

机器人空间三点圆弧功能的实现第35卷2007拄第8期8月华中科技大学(自然科学版)j.huazhonguniv.ofsci.8ltech.(naturescienceedition)vo1.35no.8aug.2007机器人空间三点圆弧功能的实现叶伯生(华中科技大学国家数控系统工程技术研究中心,湖北武汉430074)摘要:基于机器人终端执行器经历的空间任意三点,求出空间圆弧的圆心和半径,继而推导出机器人控制系统中任意空间圆弧的实现方法.该算法不仅理论上可使所有插补点均落在圆弧上,而且由于采用了矢量算法,并以三点的次序作为圆弧的方向,从而避免了插补方向和过象限的判断,可以使用统一的实时插补递推公式.研究成果已在实际机器人中得到应用该成果同样适用于需要空间圆弧功能的机床数控系统.关键词:机器人;空间圆弧;插补;矢量;误差中图分类号:tp242文献标识码:a文章编号:167卜4512(2007)08000504implementationofarcinterpolationinarobotbyusingthreearbitrarypointsyebosheng(nationalncsystemengineeringresearchcenter,huazhonguniversityofscienceandtechnology,wuhan430074,china)abstract:amethodisproposedtocalculatethecenterandradiusofanarcinaspacethroughthreearbitrarypoints.thecorrespondinginterpolationalgorithmforanarbitraryspecialarcinarobotcontrolsystemwasdeduced.allinterpolatedpointscouldfallonthecurvethroughthealgorithmtheoretical一1y.aunifiedrecurrenceformulacouldbeusedwithoutthedirectionjudgment,becausevectoralgorithmwasusedandthesequenceofthreepointswastakenastheinterpolationdirection.theresulthasbeenappliedtoapracticalrobot.thisachievecanbealsousedincncsystemsinmachinetools.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-pllc-plr;(1)c-p.一(丽).(2)收稿日期:20060712.作者简介:叶伯生(1966一),男,副教授;武汉,华中科技大学国家数控系统工程技术研究中心(430074).e-mail:?6?华中科技大学(自然科学版)第35卷图1空间三点决定的圆弧由式(1)得(x一xc)+(yyc)+(zzc)一r(一0,1,2).(3)设坐标系0xyz上各坐标轴的单位坐标矢量分别为i,-,k,则有.:(x.一xc,y0一yc,z.一zc),p0一(x1一x0,y1一y0,z1一zo),p1p一(x2一x1,y2yl,z2一z1).于是.xpop1一lijkljx.一xcy0一yczozcj,ixlx.y1一y0z1z.i-?+p0p1plp;:i-,kilx1一x.y1一y0z1一z.i.1x2一x1y2一ylz2一z1j由式(2)得(y0一yc)(z一z.)一(一zc)(y一yo3/e(y一yo)(z2一z)一(z一z.)(y2y1)一e(z.一zc)(xlxo)(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为33矩阵,a.一2(x.一x1),ao.一2(y0一y1),a02=2(zoz1),alo一2(xlx2).a11:2(y1一y2),al22(z1一z2),一口02(口o2a11一a01al2)/8,a2i=:ao2(a0oai2一al)2lf)/8,a22一a00(a02口11一a0la12)+abl(a)l!一a.2a1.)/8;b为31矩阵,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的秩rantc42)的情况.在这种情况下,应去掉矩阵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一zlx2一y2一z2一描述空间三点圆弧-6_.机器人的控制系统读入此段程序后,首先求出圆心和半径,然后再计算插补点的坐标.设,l一丽一+,则一(ylyo)(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一ixtxc.,y一yc硼zzc则rf一(z一zc)一硼(yyc);f=硼(xxc)(zfzc);(8)lz一(yyc)一(xxc).设经过一个插补周期后,机器人的终端执行器从p(x,y,z)沿圆弧切向移动ft后(其中:f为编程速度;t为插补周期),到达p:+(x:+,t+,z:+),女图2所示,贝urx斗1一x+x一x+em;1一y+y+en;(9)lz1一z+=z+e/,式中efy/(+l).可以证明+z一(硼)?(xxc)+(yyc).+(z一zc)+(xxc)+(y一yc)+(z一zc)一(硼)r,.从而,eft/er(+硼).(10)从图2可以看出,点p:+并不在圆弧上,为使所有插补点都落在圆弧上,需对式(9)进行修正.连接cpi+交圆弧于p点,以p代替+作为插补点,则插补点始终在圆弧上.图2空间圆弧插补原理在直角acpp:+中,i+iziiz+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)令gr/(r+)1/2一l/1+(/r),(12)并把式(9)代人式(11),得插补递推公式rx1一xc+g(x+em一xc);y一yc+g(y+enfyc);(13)iz斗1一zc+g(z+e/zc).2.3终点判别圆弧插补的终点判别只需算出圆心角和步距角,以两者的商作为插补次数,参考图2,有一arcsin(ft/r)ft/r.(14)圆心角的计算则要考虑如图3所示的丌(圆弧p.pp)和>丌(圆弧p.pp)两种情况.当丌时,02arcsin(x2一xo)+(y2一y.)+(z2一z.)/(2r);(15)当>丌时,02丌一2arcsin(x2一xo)+(y2一)+(z一)/(2r).(16)从图3可以看出,当丌时,矢量.p.p.ppo图3圆心角的计算与圆弧所在平面的法矢量,l(一)方向相同;当>丌时,矢量.与,l方向相反.因而可用公式h一训1+删1+伽1(17)的正负来判断的范围,当h0时,丌;当h<o时,>丌.式中,1,硼1为矢量cp.p.在各坐标轴方向上的分量,1一(yoyc)(z2一zn)一(一zc)(y2一y.);1一(一zc)(x2一xo)一(18)(x.一xc)(z2一);硼1一(xoxc)(y2一y0)一(yoyc)(x2一xo).算出和后,插补次数(不包括点)neo/a+1.(19)2.4误差分析由插补递推过程知,插补点总在圆弧上,算法没有累积误差.?8?华中科技大学(自然科学版)第35卷从图2和式(14)可知,每次插补走过的步距角是不变的,因而每个插补周期走过的弦长是不变的,即进给速度是恒定的.只是弦长lppl稍稍小于切线段长度lpp:+l(=ft),相对误差为很小,可以忽略其高次项)(1pplippi)/lppl=(l2rsin(8/2)/(f丁)=12sin(8/2)/81218/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),即xo3:x.,yo一yo,zro3一z0.g.=+1,由式(8)求m,z.h.由式(13)确定下一插补点.i.若<n,则转g.j.xen3=x2,yn一y2,zr一z2,结束.步骤ae在插补预处理中完成.因而实时插补过程由式(8)和式(13)构成,共有9次乘法运算,计算量不大,在主频166mhz的工控机上验证,平均每次的插补运行时间约为0.1ms.大部分机器人是关节机器人,系统控制的是关节坐标轴.因此,用上述方法算得插补动点后,还得通过坐标变换把动点的位置和方位转换为关节坐标,然后控制相应的关节转动一定的角度,到达所要求的位置和方位.不同结构的机器人,其变换矩阵会不同,但对于任一给定的机器人,其位置和方位与关节的关系是一定的.因而只要插补点的位置在机器人的运动范围内,总可以找到相应关节坐标系下的解83.误差分析和实时性分析表明,本文研究的算法能满足机器人控制的精度及实时性要求,本研究成果已成功应用于华中i型教学机器人.事实上,在机械加工中,如果机床数控系统具有空间任意圆弧插补功能,可达到简化编程,提高加工效率和精度的目的.因此,本研究成果也可应用于高性能机床数控系统.参考文献1熊有伦.机器人技术基础m.武汉:华中理工大学出版社,1996.2叶伯生.计算机数控系统原理,编程与操作m.武汉:华中理工大学出版社,1999.3秦开怀,金建新,宾鸿赞.cnc系统中任意三维椭圆弧的高