機(jī)器人空間三點(diǎn)圓弧算法的研究與實現(xiàn)

(安徽埃夫特智能裝備有限公司,安徽 蕪湖 241009)

機(jī)器人空間三點(diǎn)圓弧算法的研究與實現(xiàn)

曾 輝 柳 賀

(安徽埃夫特智能裝備有限公司,安徽 蕪湖 241009)

本文介紹了一種針對機(jī)器人空間圓弧的插補(bǔ)方法。機(jī)器人TCP經(jīng)歷的空間任意三點(diǎn),求出空間圓弧的圓心和半徑，繼而推導(dǎo)出機(jī)器人控制系統(tǒng)中任意空間圓弧的實現(xiàn)方法，該算法不僅理論上可使所有插補(bǔ)點(diǎn)均在圓弧上，而且采用了矢量算法，避免了插補(bǔ)方向和過象限的判斷。

機(jī)器人；空間圓??；插補(bǔ)；矢量

引言

機(jī)器人一般應(yīng)用于比較惡劣或人難于企及的環(huán)境以替代勞動者完成必要的任務(wù)，因而機(jī)器人的編程，很多情況下是采用示教完成的。示教過程包括把機(jī)器人移動到幾個所要求的目標(biāo)點(diǎn)并把這些點(diǎn)的位置記錄在存儲器中，然后定義經(jīng)過這些點(diǎn)的曲線軌跡及速度。

當(dāng)曲線軌跡為圓弧時，除了示教圓弧起點(diǎn)(機(jī)器人當(dāng)前點(diǎn)) 和終點(diǎn)外，至少還應(yīng)知道圓心或圓弧上一中間點(diǎn)。顯然，根據(jù)現(xiàn)場的應(yīng)用經(jīng)驗，機(jī)器人終端TCP的軌跡圓弧通常由示教的圓弧起點(diǎn)、中間點(diǎn)及圓弧終點(diǎn)決定，而這三點(diǎn)所決定的平面通常不一定平行于某一坐標(biāo)平面，因而需要研究空間任意三點(diǎn)圓弧的插補(bǔ)算法。

本研究以上述需求為出發(fā)點(diǎn)，基于機(jī)器人TCP經(jīng)歷的空間任意三點(diǎn)，推導(dǎo)出了機(jī)器人控制系統(tǒng)中任意空間圓弧的實現(xiàn)方法，并將該算法轉(zhuǎn)化為機(jī)器人語言進(jìn)行現(xiàn)場應(yīng)用。

1 由空間任意三點(diǎn)求圓弧的圓心和半徑

已知空間任意三點(diǎn)分別為圓弧起點(diǎn)P ( Xp ,Yp , Zp ) 、中間點(diǎn) Q ( Xq , Yq , Zq )和圓弧終點(diǎn)R( Xr , Yr , Zr ) ,如圖1所示。設(shè)圓心為C( Xc ,Yc , Zc) ,半徑為r ,則有：

設(shè)坐標(biāo)系 O-X Y Z 上各坐標(biāo)軸的單位坐標(biāo)矢量分別為i , j , k ,則有：

圖1 空間三點(diǎn)圓弧

P、Q、R確定的平面I的法向量

在求解過程中應(yīng)注意平行于任一坐標(biāo)平面的圓弧的處理，若L1平行于X平面，則N1i=0，若L2平行于X、Y平面，則N2i、N2j均等于0。在這些情況下都需要程序特殊處理。如圖2所示。

2 插補(bǔ)算法

空間給定了起點(diǎn)、終點(diǎn)和一個中間點(diǎn)的空間三點(diǎn)圓弧的走向是確定的,因此無需用順時針和逆時針來區(qū)分。事實上，用表示空間三點(diǎn)圓弧所在平面的法矢量 n ,則從 n 的正方向看,從 P 到 Q到R 的圓弧始終是逆時針圓弧。機(jī)器人的控制系統(tǒng)讀入此段程序后,首先求出圓心和半徑,然后再計算插補(bǔ)點(diǎn)的坐標(biāo)。

在平面I內(nèi)建立新的二維坐標(biāo)系X’OY’。以CP為新的Y’軸，以nXCP為X’軸，對X’、Y’分別進(jìn)行單位化，就可得到新的坐標(biāo)系O’-X’Y’到原坐標(biāo)系O-XYZ的轉(zhuǎn)換矩陣。

直線L1和直線L2的交點(diǎn)即為所求圓弧的圓心C( Xc ,Yc , Zc)，由CP、CQ、

圖3

圖4

圖5

則轉(zhuǎn)換矩陣

這樣就把空間圓弧算法轉(zhuǎn)化為平面I的圓弧算法。

平面圓弧上的任一點(diǎn)M的O’-X’Y坐標(biāo)系下坐標(biāo)（Xm’，Ym’）都能轉(zhuǎn)化為空間的O-XYZ坐標(biāo)系的坐標(biāo)（Xm，Ym，Zm）。

3 終點(diǎn)判別

圓弧插補(bǔ)的終點(diǎn)判別只需算出圓心角θ和步距角δ,以兩者的商作為插補(bǔ)次數(shù)。

時，

與圓弧所在平面的法矢量 方向相同；

4 仿真分析

在空間任意給定三點(diǎn)，首先算出圓心，然后根據(jù)上述算法畫出圓弧。

時的圓弧如圖4所示。

時的圓弧如圖5所示。

5 機(jī)器人實現(xiàn)

對于6自由度機(jī)器人來說，通過圓弧的插補(bǔ)算法實時獲取圓弧的插補(bǔ)位置,然后經(jīng)過機(jī)器人逆解，獲得機(jī)器人的關(guān)節(jié)角度，驅(qū)動各軸電機(jī)運(yùn)動到相應(yīng)位置，到達(dá)TCP所要求的位置和方位，改方法成功應(yīng)用到自己開發(fā)的ER-165點(diǎn)焊與ER6弧焊機(jī)器人，經(jīng)激光跟蹤儀測定，跟隨誤差都能控制在+-0.02mm以內(nèi)。

[1] 孫樹棟．工業(yè)機(jī)器人技術(shù)基礎(chǔ)[M] 西安：西安工業(yè)大學(xué)出版社，2006．

[2] 楊橋．奇瑞QH-165點(diǎn)焊機(jī)器人焊接系統(tǒng)淺析[J] ．機(jī)器人技術(shù)與應(yīng)用2010，3：10-11．

[3] 熊有倫 ． 機(jī)器人技術(shù)基礎(chǔ) [ M ] ． 武 漢: 華 中理工大學(xué) 出 版社 , 1996．

[4] 葉伯生 ． 計 算機(jī) 數(shù) 控系 統(tǒng)原 理、編 程 與操 作 [ M ] ． 武 漢 : 華中理工大學(xué)出版社 , 1999．

[5] 秦開懷 , 金建新 , 賓 鴻贊 ． CN C 系統(tǒng) 中任 意三 維橢 圓 弧的高速插補(bǔ)新方法 [ J] ． 華 中理工 大學(xué)學(xué) 報 , 1992, 20( 6) : 7 - 11．

TP242

：A