自由曲面拋光機(jī)器人的位姿規(guī)劃和控制

明恒強(qiáng), 殷躍紅

(上海交通大學(xué) 機(jī)械與動力工程學(xué)院,上海 200240,E-mail:1362525207@sjtu.edu.cn)

光學(xué)曲面零件包括球面、非球面以及自由曲面幾種類型,由于自由曲面光學(xué)元件能簡化系統(tǒng)結(jié)構(gòu)并增加設(shè)計自由度,因此在改善光學(xué)系統(tǒng)的光學(xué)性能中有著不可替代的作用[1]。氣囊拋光由于具備可擴(kuò)展性強(qiáng)、邊緣效應(yīng)處理優(yōu)秀、工藝組合方便等優(yōu)點(diǎn)[2],因此在光學(xué)加工領(lǐng)域也有較廣泛的應(yīng)用。而對于拋光設(shè)備,隨著光學(xué)元件所需口徑的不斷增大,機(jī)器人相對于專門的數(shù)控機(jī)床在成本和靈活性等方面更具優(yōu)勢[3],同時結(jié)合計算機(jī)控制光學(xué)表面成型技術(shù)原理,將機(jī)器人與數(shù)控拋光技術(shù)的結(jié)合也是近幾年的研究熱點(diǎn)[4]。

機(jī)器人拋光系統(tǒng)研發(fā)的首要任務(wù)便是解決拋光工具的位姿規(guī)劃[5]。西班牙馬德里理工大學(xué)[6]研發(fā)的拋光機(jī)器人系統(tǒng)描述了基于CAD系統(tǒng)數(shù)據(jù)的自動規(guī)劃與編程系統(tǒng),提出了刀具軸規(guī)劃的通用方法。廈門大學(xué)王健等[7]分析CCOS的坐標(biāo)轉(zhuǎn)換過程,建立控制模型并進(jìn)行運(yùn)動學(xué)解算,獲得了較好的加工效果。不過這些都是基于小工具拋光頭的位姿規(guī)劃方案,而對于氣囊拋光軸的位姿并沒有完整的規(guī)劃、驗(yàn)證以及控制流程;同時傳統(tǒng)的工業(yè)機(jī)器人在特定的拋光軌跡下會出現(xiàn)關(guān)節(jié)轉(zhuǎn)角突變,造成部分加工區(qū)域駐留時間和機(jī)器人姿態(tài)的不確定性。

基于上述分析,本文以自研拋光系統(tǒng)為例證對象,通過分析工件面形和氣囊拋光軸位姿的數(shù)學(xué)關(guān)系進(jìn)行軌跡規(guī)劃和修整,同時求出機(jī)器人運(yùn)動學(xué)正逆解,并提出一種新的機(jī)器人增量運(yùn)動學(xué)求解方案以避免關(guān)節(jié)轉(zhuǎn)角突變,結(jié)合ROS系統(tǒng)下的仿真驗(yàn)證實(shí)現(xiàn)氣囊拋光軸的位姿控制。

1 氣囊拋光機(jī)器人系統(tǒng)的建立

本研究搭建了如圖1所示的機(jī)器人拋光系統(tǒng),自研拋光機(jī)器人能實(shí)現(xiàn)刀具軸軸向量的全方位位姿控制,機(jī)器人刀具軸為自研氣囊拋光工具,通過氣路系統(tǒng)調(diào)節(jié)氣囊壓力控制拋光接觸力,工件端由大理石固定臺和力傳感器等器件組成。

圖1 機(jī)器人拋光系統(tǒng)

拋光機(jī)器人的前三軸可以看作是標(biāo)準(zhǔn)SCARA構(gòu)型,四五關(guān)節(jié)采用的是歐拉角控制方式實(shí)現(xiàn)刀具軸的姿態(tài)控制,第六軸則為刀具軸。該機(jī)器人相比于傳統(tǒng)六軸工業(yè)機(jī)器人,剛性更優(yōu),運(yùn)動精度更高,穩(wěn)定性更好,所需控制的誤差變量數(shù)目也更少,因此更適合拋光加工。

超精密拋光機(jī)理研究能夠給出刀具的轉(zhuǎn)速和在工件不同位置下的駐留時間,同時結(jié)合壓深能夠?qū)崿F(xiàn)不同大小的材料去除量和面形修形。本機(jī)器人拋光系統(tǒng)則基于此機(jī)理,采用離線編程的方式,根據(jù)給定的面形數(shù)據(jù)和駐留時間等參數(shù)進(jìn)行刀具軸姿態(tài)求解,結(jié)合運(yùn)動學(xué)分析和控制系統(tǒng)實(shí)現(xiàn)位姿控制。

各坐標(biāo)系如圖1所示,其中機(jī)器人的基坐標(biāo)系位于關(guān)節(jié)二下方的大理石臺面上;刀具坐標(biāo)系,即氣囊拋光的待解算目標(biāo),其中主要控制的是Z6軸,也就是氣囊拋光工具的軸向量;工件坐標(biāo)系則是為了描述工件的面形數(shù)據(jù)和姿態(tài)。

2 氣囊拋光軸的位姿求解

2.1 自由曲面拋光路徑規(guī)劃

拋光路徑一般采用機(jī)床常規(guī)加工路徑——光柵路徑,自由曲面由于豎直向曲率的變化目前還未證明其他路徑有更好的均勻性,因此本文采用光柵路徑作為加工路徑。工件面形可如式(1)表示。

F(x,y,z)=0

(1)

本文以自由曲面中相對復(fù)雜的非球面作為研究對象,提出一種對自由曲面具備普適性的位姿計算方法,研究對象面形函數(shù)如式(2)所示。

(2)

非球面元件各項(xiàng)系數(shù)如表1所示,其中工件的頂點(diǎn)曲率半徑為R0,則C=-1/R0。

表1 非球面元件參數(shù)表

該面形下的光柵路徑加工軌跡如圖2所示。

圖2 待加工元件加工光柵路徑

2.2 氣囊拋光軸位置計算

拋光軸位置即氣囊球心,也就是刀具坐標(biāo)系原點(diǎn)。得到接觸點(diǎn)位置后,需要映射到工件坐標(biāo)系下的機(jī)器人刀具控制點(diǎn)坐標(biāo),由于加工前需進(jìn)行調(diào)平,保證了工件坐標(biāo)系和基坐標(biāo)系的平行性,所以由工件坐標(biāo)系到基坐標(biāo)系只是平移的坐標(biāo)轉(zhuǎn)換問題,因此刀位點(diǎn)計算的重點(diǎn)便是工件坐標(biāo)系下的刀具位姿求解,氣囊拋光軸方程表達(dá)式如式(3)表示。

(3)

其中:(xc,yc,zc)表示氣囊拋光球心的位置即刀具軸的位置控制點(diǎn),需要根據(jù)接觸點(diǎn)得到刀位點(diǎn),其與工件表面接觸點(diǎn)的關(guān)系如式(4)所示。

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

(4)

其中:(x0,y0,z0)為氣囊和工件表面的接觸點(diǎn),由拋光路徑和面形給出。N為法向量,Rcur為曲率半徑,Dpre為壓深。曲率半徑為機(jī)器人固有參數(shù),壓深結(jié)合面形數(shù)據(jù)以及拋光去除量所給定,皆為已知條件。下面求解法向量,需結(jié)合切平面方程,如式(5)給出。

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

(5)

該點(diǎn)位置的法向量可以通過N=(A,B,C)來表示,其中各部分參數(shù)可以通過式(6)求解。

(6)

根據(jù)式(4)、(6)可以得到加工控制點(diǎn)在工件坐標(biāo)系下的坐標(biāo),求得控制點(diǎn)拋光路徑如圖3所示。

圖3 氣囊拋光軸控制點(diǎn)路徑

2.3 拋光刀具軸姿態(tài)計算

每一個接觸點(diǎn)處的拋光方向和刀柄姿態(tài)的關(guān)系是固定的,控制拋光方向即控制拋光軸軸線在當(dāng)前點(diǎn)的朝向,也就是要控制刀柄在當(dāng)前位置局部坐標(biāo)系下的姿態(tài),局部坐標(biāo)系定義如圖4所示,其Xp軸在工件坐標(biāo)系XY平面的投影為刀具軸前進(jìn)的方向,Zp軸為當(dāng)前位置的法向。

圖4 局部坐標(biāo)系定義

同時,氣囊拋光工具的軸線需與工件表面接觸點(diǎn)的法線方向保持一定的夾角,該夾角需要在加工過程中保持不變來保證每一個刀位點(diǎn)獲得相同的切速度以及拋光力,配合合適的刀具軸轉(zhuǎn)速即可完成拋光加工[8]。

如圖5所示,加工進(jìn)動角βc角在整個加工過程中保持不變,而對于拋光方向可以根據(jù)加工機(jī)理自行定義,本研究以單程拋光方向變化幅值為30°的正弦波為例,則刀具軸朝向的規(guī)劃如圖6所示。

圖5 氣囊拋光軸與工件表面姿態(tài)關(guān)系

圖6 拋光軸的朝向變化

局部坐標(biāo)系下氣囊拋光軸的姿態(tài)可由式(7)表示,其中姿態(tài)描述為單位向量。

(7)

其中:γc為與運(yùn)動方向的夾角,即刀具軸在局部坐標(biāo)系下的朝向,如圖6所示?？梢酝ㄟ^坐標(biāo)旋轉(zhuǎn)進(jìn)行局部坐標(biāo)系到工件坐標(biāo)系的轉(zhuǎn)化,旋轉(zhuǎn)齊次矩陣表示為式(8)。

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

(8)

對于各軸旋轉(zhuǎn)角度,θ1為拋光軸的運(yùn)動方向,與所采用的路徑有關(guān),即Xp軸與工件坐標(biāo)系XZ平面的夾角,光柵路徑下為0°或180°,θ2和θ3通過幾何關(guān)系可以發(fā)現(xiàn)只與工件當(dāng)前位置的法向量N有關(guān),法向量在工件坐標(biāo)系中表達(dá)設(shè)為(xz,yz,zz),則旋轉(zhuǎn)角度可通過下式進(jìn)行求解:

(9)

本研究定義拋光軸與法向量的夾角βc為15°,根據(jù)式(8)、(9)可以獲得刀具軸姿態(tài),再結(jié)合2.2節(jié)所得到的刀具軸位置,即可獲得如圖7所示的機(jī)器人氣囊拋光軸位姿。

圖7 機(jī)器人的位姿規(guī)劃

3 氣囊拋光軸的位姿控制

3.1 拋光機(jī)器人運(yùn)動學(xué)分析

本文采用MDH方法[9]進(jìn)行運(yùn)動學(xué)的建模,機(jī)器人包含5+1個自由度,齊次變換矩陣如式(10)所示。

(10)

根據(jù)機(jī)器人構(gòu)型,建立圖8的D-H坐標(biāo)系。

圖8 機(jī)器人各關(guān)節(jié)坐標(biāo)系定義

第一軸控制豎直方向運(yùn)動,二、三、四軸互為平行軸,第五軸為45°傾角軸,即Z4和Z5方向夾角為45°,第六軸即刀具軸,D-H參數(shù)如表2所示。

表2 機(jī)器人D-H參數(shù)表

據(jù)此可以求得坐標(biāo)轉(zhuǎn)換矩陣如式(11)表示。

(11)

關(guān)于逆運(yùn)動學(xué)求解,可以發(fā)現(xiàn)由于45°傾角的機(jī)械設(shè)計導(dǎo)致公式推導(dǎo)方法求逆解較為困難,因此本研究提出適用于該機(jī)械臂的幾何逆解求解方法。關(guān)節(jié)一轉(zhuǎn)角與移動距離為線性關(guān)系因此不再表述其求解關(guān)系,二、三軸如圖9所示控制位置,并基于此幾何關(guān)系進(jìn)行逆解求解。

圖9 SCARA型機(jī)器人運(yùn)動學(xué)解算

根據(jù)余弦定理可以獲得如下表達(dá)式:

(12)

繼而如式(13)求得關(guān)節(jié)三的轉(zhuǎn)角。

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

(13)

其中:

(14)

正負(fù)號由坐標(biāo)系定義,如果定義右手坐標(biāo)系則取正,左手坐標(biāo)系則取負(fù),根據(jù)幾何關(guān)系可以進(jìn)一步求得關(guān)節(jié)二的轉(zhuǎn)角。

(15)

對四五關(guān)節(jié)采用幾何方法求解,四五關(guān)節(jié)決定了氣囊拋光軸的姿態(tài),氣囊拋光軸的姿態(tài)由歐拉角來表示。由于控制的是刀具軸的朝向,因此只需要通過章動角和進(jìn)動角表示即可。

如圖10所示,章動角為與基坐標(biāo)系Z軸方向的夾角,設(shè)為β;進(jìn)動角為拋光軸向量在基坐標(biāo)系XY面上的投影與XZ面的夾角,設(shè)為α。

圖10 氣囊拋光軸姿態(tài)表示

由于機(jī)械設(shè)計的位姿解耦性,可以發(fā)現(xiàn)章動角只與關(guān)節(jié)五有關(guān),則關(guān)節(jié)五轉(zhuǎn)角的求解如式(16)所示。

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

(16)

關(guān)節(jié)四的轉(zhuǎn)角與進(jìn)動角有關(guān),而關(guān)節(jié)二、三、五都會導(dǎo)致進(jìn)動角變化,關(guān)節(jié)二和三的轉(zhuǎn)角增量和所造成的進(jìn)動角增量幅值相同,需要求解關(guān)節(jié)五導(dǎo)致的進(jìn)動角增量,通過幾何描述可以如圖11表述。

圖11 關(guān)節(jié)五與進(jìn)動角增量的關(guān)系

根據(jù)幾何關(guān)系可以求出關(guān)節(jié)五所造成的進(jìn)動角增量Δα,如式(17)所示。

(17)

則關(guān)節(jié)四的逆解如式(18)所示。

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

(18)

3.2 多解問題處理

在SCARA左右手坐標(biāo)系確定的情況下會得到兩組逆解,如圖12所示,對于目標(biāo)姿態(tài),關(guān)節(jié)五會得到兩組解,每一組解都可以再通過關(guān)節(jié)四順時針或者逆時針運(yùn)行到目標(biāo)姿態(tài),因此有四組增量解。

圖12 姿態(tài)多解幾何表達(dá)

因此本文提出一種增量式求解算法進(jìn)行多解選擇,將各關(guān)節(jié)實(shí)際運(yùn)轉(zhuǎn)角度表述為多圈絕對位置,逆解相關(guān)的運(yùn)動學(xué)計算描述在單圈絕對值空間下,即滿足四五關(guān)節(jié)角度的計算空間是-180°～180°,該算法的求解流程為:

(1) 根據(jù)所需的歐拉角α和β角計算得到兩組逆解,先判斷關(guān)節(jié)五的轉(zhuǎn)向,若關(guān)節(jié)五當(dāng)前為起始位置,即關(guān)節(jié)五單圈絕對值空間下為0°,則需判斷關(guān)節(jié)四的相對運(yùn)動角度增量大小,即:

(19)

該式表述為:當(dāng)關(guān)節(jié)五多解運(yùn)動增量相同時,選擇關(guān)節(jié)四運(yùn)動增量小的運(yùn)動方案。若關(guān)節(jié)五當(dāng)前非起始位置,則需要判斷軸五的角度增量大小,即:

(20)

(2) 若關(guān)節(jié)五當(dāng)前非起始位置,軸四此時已經(jīng)確定單圈絕對空間下的位置,需要進(jìn)行軸四相對運(yùn)動增量比較獲得最優(yōu)解,即:

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

(21)

以一種30°幅值的正弦波加工速度方向光柵軌跡為例進(jìn)行算法驗(yàn)證,如圖13所示得到了角度四、五的逆解,可以看到關(guān)節(jié)轉(zhuǎn)角連續(xù)。而對于工業(yè)六軸機(jī)器人而言,各關(guān)節(jié)角度的限制令部分關(guān)節(jié)會產(chǎn)生轉(zhuǎn)角變化360°的突變現(xiàn)象,該增量式運(yùn)動學(xué)解算方法能有效避免該問題的現(xiàn)象。

圖13 關(guān)節(jié)四的角度變化曲線

3.3 加工軌跡修整

由于柵格拋光軌跡在換程時會出現(xiàn)拐點(diǎn),會導(dǎo)致機(jī)器人運(yùn)行時出現(xiàn)抖動現(xiàn)象,因此需要對軌跡進(jìn)行修整,過程為:根據(jù)拐點(diǎn)兩端以及拐點(diǎn)處法向量得到圓心和經(jīng)過點(diǎn),圓弧插補(bǔ)運(yùn)算[10]獲得插值點(diǎn),Z方向補(bǔ)充抬刀以避免加工到工件待加工區(qū)域外圍。姿態(tài)通過線性插值即可,對此主要做位置修整,圓弧插補(bǔ)計算的幾何關(guān)系如圖14所示。

圖14 空間圓弧坐標(biāo)系

圓弧插補(bǔ)的三點(diǎn)確定平面F123,平面方程如下:

k11x+k12y+k13z+k14=0

(22)

其中:

(23)

(x1,y1,z1)為P1點(diǎn)坐標(biāo),同理也可以求出兩個垂直平分面F12、F13的表達(dá)方程,在此不再贅述,因?yàn)槿齻€面交于圓心,所以根據(jù)面的方程可以得到圓心坐標(biāo)相關(guān)的矩陣表達(dá)式如式(24)所示。

(24)

其中:(x0,y0,z0)為圓心坐標(biāo),圓的半徑為:

(25)

新坐標(biāo)系中(見圖14),X1軸指向P1點(diǎn),Z1軸垂直于F123面,以此建立O1-X1Y1Z1坐標(biāo)系。在新坐標(biāo)系下圓的表達(dá)式為平面圓表達(dá)式,但需要進(jìn)行坐標(biāo)轉(zhuǎn)換,齊次變換矩陣為式(26)。

(26)

各部分為新坐標(biāo)系各軸在原坐標(biāo)系下的單位方向向量,P1為O1點(diǎn)的機(jī)器人基坐標(biāo)系下的坐標(biāo),即(x0,y0,z0)T,X方向的單位向量表達(dá)為式(27)。

(27)

其中:

(28)

Z方向的單位向量表達(dá)為式(29)所示。

(29)

Y方向的單位向量表達(dá)為式(30)所示。

Y1=X1×Z1

(30)

圓弧插補(bǔ)是對角度的插補(bǔ),根據(jù)新坐標(biāo)系下的三個控制點(diǎn)計算插補(bǔ)過程旋轉(zhuǎn)角度Ω:

(31)

通過梯形速度規(guī)劃[11]可以獲得歸一化參數(shù)λ與時間的關(guān)系,其中λ∈[0,1],根據(jù)角度變化獲得新坐標(biāo)系下插補(bǔ)坐標(biāo),如式(32)表示。

(32)

得到新坐標(biāo)系下的插值點(diǎn)坐標(biāo)后,再通過齊次變換矩陣即可獲得工件坐標(biāo)系下的原坐標(biāo)。除了起始點(diǎn),圓弧插補(bǔ)需要中間點(diǎn),對于中間點(diǎn)的計算如式(33)表示。

(33)

其中:rm為兩點(diǎn)間距離的一半,即插補(bǔ)的圓半徑,i代表當(dāng)前點(diǎn),dfzi表示當(dāng)前位置的加工進(jìn)動角大小,同時插補(bǔ)后對Z方向進(jìn)行正弦波補(bǔ)充抬刀,以避免加工到工件待加工區(qū)域外側(cè),修整后的軌跡如圖15所示。

圖15 修整后的機(jī)器人末端軌跡

至此已完成氣囊拋光軸的位姿規(guī)劃,再根據(jù)運(yùn)動學(xué)逆解可以獲得各個關(guān)節(jié)角度的變化,輸入到控制器中即可實(shí)現(xiàn)軌跡執(zhí)行,總的算法流程如圖16。

圖16 氣囊拋光軸位姿規(guī)劃和控制流程

4 位姿驗(yàn)證以及實(shí)驗(yàn)例證

本研究所采用的控制器為固高公司的GEN運(yùn)動控制卡,采用PVT模式輸入各個關(guān)節(jié)的位置和時間信息,運(yùn)動控制卡會自行計算其他參數(shù),而這一控制模式在ROS系統(tǒng)中可以通過ros_control功能包模擬出來[12]。同時為了將軌跡規(guī)劃算法驗(yàn)證和運(yùn)動學(xué)驗(yàn)證逐步驗(yàn)證,利用ROS系統(tǒng)的MoveIt!軟件進(jìn)行笛卡爾空間和關(guān)節(jié)空間下的仿真[13],在此不再贅述ROS系統(tǒng)和相關(guān)配置,以圖15所示軌跡為示例,如圖17所示為放大后的運(yùn)動軌跡仿真展示。

圖17 ROS仿真結(jié)果

仿真結(jié)果表明軌跡規(guī)劃驗(yàn)證正確,由于仿真軌跡與理想軌跡誤差較小,因此不再展示仿真軌跡,圖18為仿真所得到的末端位置誤差數(shù)據(jù)。

圖18 ROS仿真末端位置誤差

可發(fā)現(xiàn)仿真得到的最大位置誤差為0.031 mm,均值誤差接近于0。本研究進(jìn)一步進(jìn)行拋光設(shè)備軌跡規(guī)劃的驗(yàn)證實(shí)驗(yàn),實(shí)驗(yàn)裝備如圖19所示。

圖19 機(jī)器人拋光系統(tǒng)

底部為加工工件臺,上部為拋光機(jī)器人,并已安裝氣囊拋光軸,將圖15所示的拋光軌跡逆解后輸入到控制器中并通過運(yùn)動控制卡捕獲編碼器信息,正解后得到機(jī)器人的末端位置誤差如圖20所示。

圖20 機(jī)器人運(yùn)動位置誤差

可以發(fā)現(xiàn)實(shí)際運(yùn)行過程中的最大位置誤差在0.035 mm左右,與仿真所得數(shù)據(jù)相比,最大位置誤差大小接近,但機(jī)器人實(shí)際實(shí)驗(yàn)數(shù)據(jù)更能反應(yīng)光柵軌跡的運(yùn)動過程,其誤差變化體現(xiàn)了光柵的來回?fù)Q向運(yùn)動過程。可見運(yùn)行時的刀具軸位姿與所規(guī)劃的位姿基本保持一致,規(guī)劃和控制結(jié)果基本相符。

5 結(jié)論

將機(jī)器人與氣囊拋光技術(shù)結(jié)合實(shí)現(xiàn)對自由曲面的拋光加工是當(dāng)下的熱點(diǎn)研究問題,由于自研拋光機(jī)器人在具備工業(yè)機(jī)器人優(yōu)勢的基礎(chǔ)上提升其加工性能,因此本文以該設(shè)備為實(shí)驗(yàn)設(shè)備,分析了工件面形到氣囊拋光軸的數(shù)學(xué)變換關(guān)系,推導(dǎo)了由工件自由曲面面形到氣囊拋光軸位姿的解算算法,提出了新型氣囊拋光機(jī)器人拋光自由曲面的刀具軸控制方法,并在確定的面形上得到計算驗(yàn)證和實(shí)驗(yàn)驗(yàn)證,同時也提出了一種能更大程度模擬運(yùn)動控制卡控制模式的ROS系統(tǒng)仿真方案,最后本研究根據(jù)理論成果進(jìn)行了拋光實(shí)驗(yàn)并得到了理想的位姿控制效果。