自由曲面抛光机器人的位姿规划和控制

明恒强, 殷跃红

(上海交通大学 机械与动力工程学院,上海 200240,E-mail:1362525207@sjtu.edu.cn)

光学曲面零件包括球面、非球面以及自由曲面几种类型,由于自由曲面光学元件能简化系统结构并增加设计自由度,因此在改善光学系统的光学性能中有着不可替代的作用[1]。气囊抛光由于具备可扩展性强、边缘效应处理优秀、工艺组合方便等优点[2],因此在光学加工领域也有较广泛的应用。而对于抛光设备,随着光学元件所需口径的不断增大,机器人相对于专门的数控机床在成本和灵活性等方面更具优势[3],同时结合计算机控制光学表面成型技术原理,将机器人与数控抛光技术的结合也是近几年的研究热点[4]。

机器人抛光系统研发的首要任务便是解决抛光工具的位姿规划[5]。西班牙马德里理工大学[6]研发的抛光机器人系统描述了基于CAD系统数据的自动规划与编程系统,提出了刀具轴规划的通用方法。厦门大学王健等[7]分析CCOS的坐标转换过程,建立控制模型并进行运动学解算,获得了较好的加工效果。不过这些都是基于小工具抛光头的位姿规划方案,而对于气囊抛光轴的位姿并没有完整的规划、验证以及控制流程;同时传统的工业机器人在特定的抛光轨迹下会出现关节转角突变,造成部分加工区域驻留时间和机器人姿态的不确定性。

基于上述分析,本文以自研抛光系统为例证对象,通过分析工件面形和气囊抛光轴位姿的数学关系进行轨迹规划和修整,同时求出机器人运动学正逆解,并提出一种新的机器人增量运动学求解方案以避免关节转角突变,结合ROS系统下的仿真验证实现气囊抛光轴的位姿控制。

1 气囊抛光机器人系统的建立

本研究搭建了如图1所示的机器人抛光系统,自研抛光机器人能实现刀具轴轴向量的全方位位姿控制,机器人刀具轴为自研气囊抛光工具,通过气路系统调节气囊压力控制抛光接触力,工件端由大理石固定台和力传感器等器件组成。

图1 机器人抛光系统

抛光机器人的前三轴可以看作是标准SCARA构型,四五关节采用的是欧拉角控制方式实现刀具轴的姿态控制,第六轴则为刀具轴。该机器人相比于传统六轴工业机器人,刚性更优,运动精度更高,稳定性更好,所需控制的误差变量数目也更少,因此更适合抛光加工。

超精密抛光机理研究能够给出刀具的转速和在工件不同位置下的驻留时间,同时结合压深能够实现不同大小的材料去除量和面形修形。本机器人抛光系统则基于此机理,采用离线编程的方式,根据给定的面形数据和驻留时间等参数进行刀具轴姿态求解,结合运动学分析和控制系统实现位姿控制。

各坐标系如图1所示,其中机器人的基坐标系位于关节二下方的大理石台面上;刀具坐标系,即气囊抛光的待解算目标,其中主要控制的是Z6轴,也就是气囊抛光工具的轴向量;工件坐标系则是为了描述工件的面形数据和姿态。

2 气囊抛光轴的位姿求解

2.1 自由曲面抛光路径规划

抛光路径一般采用机床常规加工路径——光栅路径,自由曲面由于竖直向曲率的变化目前还未证明其他路径有更好的均匀性,因此本文采用光栅路径作为加工路径。工件面形可如式(1)表示。

F(x,y,z)=0

(1)

本文以自由曲面中相对复杂的非球面作为研究对象,提出一种对自由曲面具备普适性的位姿计算方法,研究对象面形函数如式(2)所示。

(2)

非球面元件各项系数如表1所示,其中工件的顶点曲率半径为R0,则C=-1/R0。

表1 非球面元件参数表

该面形下的光栅路径加工轨迹如图2所示。

图2 待加工元件加工光栅路径

2.2 气囊抛光轴位置计算

抛光轴位置即气囊球心,也就是刀具坐标系原点。得到接触点位置后,需要映射到工件坐标系下的机器人刀具控制点坐标,由于加工前需进行调平,保证了工件坐标系和基坐标系的平行性,所以由工件坐标系到基坐标系只是平移的坐标转换问题,因此刀位点计算的重点便是工件坐标系下的刀具位姿求解,气囊抛光轴方程表达式如式(3)表示。

(3)

其中:(xc,yc,zc)表示气囊抛光球心的位置即刀具轴的位置控制点,需要根据接触点得到刀位点,其与工件表面接触点的关系如式(4)所示。

(xc,yc,zc)=(x0,y0,z0)+(Rcur-Dpre)×N(x,y,z)

(4)

其中:(x0,y0,z0)为气囊和工件表面的接触点,由抛光路径和面形给出。N为法向量,Rcur为曲率半径,Dpre为压深。曲率半径为机器人固有参数,压深结合面形数据以及抛光去除量所给定,皆为已知条件。下面求解法向量,需结合切平面方程,如式(5)给出。

A(x-x0)+B(y-y0)+C(z-z0)=0

(5)

该点位置的法向量可以通过N=(A,B,C)来表示,其中各部分参数可以通过式(6)求解。

(6)

根据式(4)、(6)可以得到加工控制点在工件坐标系下的坐标,求得控制点抛光路径如图3所示。

图3 气囊抛光轴控制点路径

2.3 抛光刀具轴姿态计算

每一个接触点处的抛光方向和刀柄姿态的关系是固定的,控制抛光方向即控制抛光轴轴线在当前点的朝向,也就是要控制刀柄在当前位置局部坐标系下的姿态,局部坐标系定义如图4所示,其Xp轴在工件坐标系XY平面的投影为刀具轴前进的方向,Zp轴为当前位置的法向。

图4 局部坐标系定义

同时,气囊抛光工具的轴线需与工件表面接触点的法线方向保持一定的夹角,该夹角需要在加工过程中保持不变来保证每一个刀位点获得相同的切速度以及抛光力,配合合适的刀具轴转速即可完成抛光加工[8]。

如图5所示,加工进动角βc角在整个加工过程中保持不变,而对于抛光方向可以根据加工机理自行定义,本研究以单程抛光方向变化幅值为30°的正弦波为例,则刀具轴朝向的规划如图6所示。

图5 气囊抛光轴与工件表面姿态关系

图6 抛光轴的朝向变化

局部坐标系下气囊抛光轴的姿态可由式(7)表示,其中姿态描述为单位向量。

(7)

其中:γc为与运动方向的夹角,即刀具轴在局部坐标系下的朝向,如图6所示。可以通过坐标旋转进行局部坐标系到工件坐标系的转化,旋转齐次矩阵表示为式(8)。

R=Rot(Y,θ3)·Rot(X,θ2)·Rot(Z,θ1)

(8)

对于各轴旋转角度,θ1为抛光轴的运动方向,与所采用的路径有关,即Xp轴与工件坐标系XZ平面的夹角,光栅路径下为0°或180°,θ2和θ3通过几何关系可以发现只与工件当前位置的法向量N有关,法向量在工件坐标系中表达设为(xz,yz,zz),则旋转角度可通过下式进行求解:

(9)

本研究定义抛光轴与法向量的夹角βc为15°,根据式(8)、(9)可以获得刀具轴姿态,再结合2.2节所得到的刀具轴位置,即可获得如图7所示的机器人气囊抛光轴位姿。

图7 机器人的位姿规划

3 气囊抛光轴的位姿控制

3.1 抛光机器人运动学分析

本文采用MDH方法[9]进行运动学的建模,机器人包含5+1个自由度,齐次变换矩阵如式(10)所示。

(10)

根据机器人构型,建立图8的D-H坐标系。

图8 机器人各关节坐标系定义

第一轴控制竖直方向运动,二、三、四轴互为平行轴,第五轴为45°倾角轴,即Z4和Z5方向夹角为45°,第六轴即刀具轴,D-H参数如表2所示。

表2 机器人D-H参数表

据此可以求得坐标转换矩阵如式(11)表示。

(11)

关于逆运动学求解,可以发现由于45°倾角的机械设计导致公式推导方法求逆解较为困难,因此本研究提出适用于该机械臂的几何逆解求解方法。关节一转角与移动距离为线性关系因此不再表述其求解关系,二、三轴如图9所示控制位置,并基于此几何关系进行逆解求解。

图9 SCARA型机器人运动学解算

根据余弦定理可以获得如下表达式:

(12)

继而如式(13)求得关节三的转角。

θ3=arctan2[sin(θ3),cos(θ3)]

(13)

其中:

(14)

正负号由坐标系定义,如果定义右手坐标系则取正,左手坐标系则取负,根据几何关系可以进一步求得关节二的转角。

(15)

对四五关节采用几何方法求解,四五关节决定了气囊抛光轴的姿态,气囊抛光轴的姿态由欧拉角来表示。由于控制的是刀具轴的朝向,因此只需要通过章动角和进动角表示即可。

如图10所示,章动角为与基坐标系Z轴方向的夹角,设为β;进动角为抛光轴向量在基坐标系XY面上的投影与XZ面的夹角,设为α。

图10 气囊抛光轴姿态表示

由于机械设计的位姿解耦性,可以发现章动角只与关节五有关,则关节五转角的求解如式(16)所示。

θ5=±arccos[2·sin(β)-1]

(16)

关节四的转角与进动角有关,而关节二、三、五都会导致进动角变化,关节二和三的转角增量和所造成的进动角增量幅值相同,需要求解关节五导致的进动角增量,通过几何描述可以如图11表述。

图11 关节五与进动角增量的关系

根据几何关系可以求出关节五所造成的进动角增量Δα,如式(17)所示。

(17)

则关节四的逆解如式(18)所示。

θ4=α-θ2-θ3+Δα

(18)

3.2 多解问题处理

在SCARA左右手坐标系确定的情况下会得到两组逆解,如图12所示,对于目标姿态,关节五会得到两组解,每一组解都可以再通过关节四顺时针或者逆时针运行到目标姿态,因此有四组增量解。

图12 姿态多解几何表达

因此本文提出一种增量式求解算法进行多解选择,将各关节实际运转角度表述为多圈绝对位置,逆解相关的运动学计算描述在单圈绝对值空间下,即满足四五关节角度的计算空间是-180°～180°,该算法的求解流程为:

(1) 根据所需的欧拉角α和β角计算得到两组逆解,先判断关节五的转向,若关节五当前为起始位置,即关节五单圈绝对值空间下为0°,则需判断关节四的相对运动角度增量大小,即:

(19)

该式表述为:当关节五多解运动增量相同时,选择关节四运动增量小的运动方案。若关节五当前非起始位置,则需要判断轴五的角度增量大小,即:

(20)

(2) 若关节五当前非起始位置,轴四此时已经确定单圈绝对空间下的位置,需要进行轴四相对运动增量比较获得最优解,即:

abs(Δθ4)=min[abs(θ4dest-θ4now),
abs(2π-θ4dest+θ4now)]

(21)

以一种30°幅值的正弦波加工速度方向光栅轨迹为例进行算法验证,如图13所示得到了角度四、五的逆解,可以看到关节转角连续。而对于工业六轴机器人而言,各关节角度的限制令部分关节会产生转角变化360°的突变现象,该增量式运动学解算方法能有效避免该问题的现象。

图13 关节四的角度变化曲线

3.3 加工轨迹修整

由于栅格抛光轨迹在换程时会出现拐点,会导致机器人运行时出现抖动现象,因此需要对轨迹进行修整,过程为:根据拐点两端以及拐点处法向量得到圆心和经过点,圆弧插补运算[10]获得插值点,Z方向补充抬刀以避免加工到工件待加工区域外围。姿态通过线性插值即可,对此主要做位置修整,圆弧插补计算的几何关系如图14所示。

图14 空间圆弧坐标系

圆弧插补的三点确定平面F123,平面方程如下:

k11x+k12y+k13z+k14=0

(22)

其中:

(23)

(x1,y1,z1)为P1点坐标,同理也可以求出两个垂直平分面F12、F13的表达方程,在此不再赘述,因为三个面交于圆心,所以根据面的方程可以得到圆心坐标相关的矩阵表达式如式(24)所示。

(24)

其中:(x0,y0,z0)为圆心坐标,圆的半径为:

(25)

新坐标系中(见图14),X1轴指向P1点,Z1轴垂直于F123面,以此建立O1-X1Y1Z1坐标系。在新坐标系下圆的表达式为平面圆表达式,但需要进行坐标转换,齐次变换矩阵为式(26)。

(26)

各部分为新坐标系各轴在原坐标系下的单位方向向量,P1为O1点的机器人基坐标系下的坐标,即(x0,y0,z0)T,X方向的单位向量表达为式(27)。

(27)

其中:

(28)

Z方向的单位向量表达为式(29)所示。

(29)

Y方向的单位向量表达为式(30)所示。

Y1=X1×Z1

(30)

圆弧插补是对角度的插补,根据新坐标系下的三个控制点计算插补过程旋转角度Ω:

(31)

通过梯形速度规划[11]可以获得归一化参数λ与时间的关系,其中λ∈[0,1],根据角度变化获得新坐标系下插补坐标,如式(32)表示。

(32)

得到新坐标系下的插值点坐标后,再通过齐次变换矩阵即可获得工件坐标系下的原坐标。除了起始点,圆弧插补需要中间点,对于中间点的计算如式(33)表示。

(33)

其中:rm为两点间距离的一半,即插补的圆半径,i代表当前点,dfzi表示当前位置的加工进动角大小,同时插补后对Z方向进行正弦波补充抬刀,以避免加工到工件待加工区域外侧,修整后的轨迹如图15所示。

图15 修整后的机器人末端轨迹

至此已完成气囊抛光轴的位姿规划,再根据运动学逆解可以获得各个关节角度的变化,输入到控制器中即可实现轨迹执行,总的算法流程如图16。

图16 气囊抛光轴位姿规划和控制流程

4 位姿验证以及实验例证

本研究所采用的控制器为固高公司的GEN运动控制卡,采用PVT模式输入各个关节的位置和时间信息,运动控制卡会自行计算其他参数,而这一控制模式在ROS系统中可以通过ros_control功能包模拟出来[12]。同时为了将轨迹规划算法验证和运动学验证逐步验证,利用ROS系统的MoveIt!软件进行笛卡尔空间和关节空间下的仿真[13],在此不再赘述ROS系统和相关配置,以图15所示轨迹为示例,如图17所示为放大后的运动轨迹仿真展示。

图17 ROS仿真结果

仿真结果表明轨迹规划验证正确,由于仿真轨迹与理想轨迹误差较小,因此不再展示仿真轨迹,图18为仿真所得到的末端位置误差数据。

图18 ROS仿真末端位置误差

可发现仿真得到的最大位置误差为0.031 mm,均值误差接近于0。本研究进一步进行抛光设备轨迹规划的验证实验,实验装备如图19所示。

图19 机器人抛光系统

底部为加工工件台,上部为抛光机器人,并已安装气囊抛光轴,将图15所示的抛光轨迹逆解后输入到控制器中并通过运动控制卡捕获编码器信息,正解后得到机器人的末端位置误差如图20所示。

图20 机器人运动位置误差

可以发现实际运行过程中的最大位置误差在0.035 mm左右,与仿真所得数据相比,最大位置误差大小接近,但机器人实际实验数据更能反应光栅轨迹的运动过程,其误差变化体现了光栅的来回换向运动过程。可见运行时的刀具轴位姿与所规划的位姿基本保持一致,规划和控制结果基本相符。

5 结论

将机器人与气囊抛光技术结合实现对自由曲面的抛光加工是当下的热点研究问题,由于自研抛光机器人在具备工业机器人优势的基础上提升其加工性能,因此本文以该设备为实验设备,分析了工件面形到气囊抛光轴的数学变换关系,推导了由工件自由曲面面形到气囊抛光轴位姿的解算算法,提出了新型气囊抛光机器人抛光自由曲面的刀具轴控制方法,并在确定的面形上得到计算验证和实验验证,同时也提出了一种能更大程度模拟运动控制卡控制模式的ROS系统仿真方案,最后本研究根据理论成果进行了抛光实验并得到了理想的位姿控制效果。