機器人空間三點圓弧算法的研究與實現

曾輝 柳賀

摘 要：本文介紹了一種針對機器人空間圓弧的插補方法。機器人TCP經歷的空間任意三點,求出空間圓弧的圓心和半徑，繼而推導出機器人控制系統中任意空間圓弧的實現方法，該算法不僅理論上可使所有插補點均在圓弧上，而且采用了矢量算法，避免了插補方向和過象限的判斷。

關鍵詞：機器人；空間圓弧；插補；矢量

中圖分類號：TP242 文獻標識碼：A 引言

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

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

本研究以上述需求為出發點，基于機器人TCP經歷的空間任意三點，推導出了機器人控制系統中任意空間圓弧的實現方法，并將該算法轉化為機器人語言進行現場應用。

1 由空間任意三點求圓弧的圓心和半徑

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

（1）

設坐標系 O-X Y Z 上各坐標軸的單位坐標矢量分別為i , j , k ,則有：

（2）

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

（3）

則在平面I內垂直于的向量為：

（4）

則在平面I內過中點N1（XN1，YN1，ZN1），和垂直的直線L1為：

（5）

則在平面I內垂直于的向量為：

（6）

則在平面I內過中點N2（XN2，YN2，ZN2），和垂直的直線L2為：

（7）

直線L1和直線L2的交點即為所求圓弧的圓心C( Xc ,Yc , Zc)，由CP、CQ、CR任一求出半徑r。

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

2插補算法

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

在平面I內建立新的二維坐標系XOY。以CP為新的Y軸，以nXCP為X軸，對X、Y分別進行單位化，就可得到新的坐標系O-XY到原坐標系O-XYZ的轉換矩陣。

（7）

（8）

（9）

（10）

則轉換矩陣

（11）

這樣就把空間圓弧算法轉化為平面I的圓弧算法。

平面圓弧上的任一點M的O-XY坐標系下坐標（Xm，Ym）都能轉化為空間的O-XYZ坐標系的坐標（Xm，Ym，Zm）。

（12）

3 終點判別

圓弧插補的終點判別只需算出圓心角θ和步距角δ,以兩者的商作為插補次數。

圓心角θ的計算則要考慮如圖3所示的 (圓弧 PQR )和 (圓弧 PQR )兩種情況。

當時,

當時，

（14）

從圖3可以看出,當時，矢量

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

當時，矢量與方向相反。因而可用公式：

（15）

的正負來判斷的范圍,當時, ；當時，。式中為矢量在各坐標軸方向上的分量。

算出δ和θ后，插補次數(不包括 P點)。

4仿真分析

在空間任意給定三點，首先算出圓心，然后根據上述算法畫出圓弧。

時的圓弧如圖4所示。

時的圓弧如圖5所示。

5機器人實現

對于6自由度機器人來說，通過圓弧的插補算法實時獲取圓弧的插補位置,然后經過機器人逆解，獲得機器人的關節角度，驅動各軸電機運動到相應位置，到達TCP所要求的位置和方位，改方法成功應用到自己開發的ER-165點焊與ER6弧焊機器人，經激光跟蹤儀測定，跟隨誤差都能控制在+-0.02mm以內。

參考文獻

[1] 孫樹棟.工業機器人技術基礎[M] 西安：西安工業大學出版社，2006.

[2] 楊橋.奇瑞QH-165點焊機器人焊接系統淺析[J] .機器人技術與應用2010，3：10-11.

[3] 熊有倫 . 機器人技術基礎 [ M ] . 武 漢 : 華 中理工大學 出 版社 , 1996.

[4]葉伯生 . 計 算機 數 控系 統原 理、 編 程 與操 作 [ M ] . 武 漢 : 華中理工大學出版社 , 1999.

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