陳 璽，李傳軍

（1.天津科技大學機械工程學院，天津 300202；2.天津中德應用技術(shù)大學機械工程學院，天津 300350）

1 引言

機器人在運動過程中需要沿著特定軌跡運動，如直線、圓弧、球形等?，F(xiàn)階段球面插補算法大都是通過多段圓弧插補實現(xiàn)的，針對機器人的空間圓弧插補主要有矢量法［1-8］、坐標轉(zhuǎn)換法［9-15］、等弧分割法［16］、積分法［17-18］等。所求的插補點需要用到坐標變換，積分運算，微分運算等，增加了運算量。因此對于球面插補算法需要一種計算簡單，運算速度快，插補點精確的圓弧插補算法。

本論文提出一種基于等弦采樣的新型空間圓弧插補算法，與其他算法相比運算時間大幅減小。通過仿真和樣機實驗，驗證了插補算法的正確性。把這種新型空間圓弧算法應用到球球面插補中，求得空間球面的插補點并求出末端的位姿。通過MATLAB對機器人建模并仿真出球形灌木修剪過程，驗證了該算法的有效、可行性。實現(xiàn)了不同尺寸球形灌木自動修剪的仿真。

2 空間圓弧插補算法

提出一種基于等弦采樣的新型空間圓弧插補算法。給定空間任意不共線的三個點，即可求出圓心坐標P0與圓弧半徑R。由于空間圓弧與弦長有映射關(guān)系。

給定圓心坐標，起始點和終止點坐標，插補圓弧的圓心角θ，即可求出插補點。此算法分為兩種情況。圓弧的圓心角0 ＜θ＜π和π ＜θ＜2π。

（1）當圓弧圓心角為0 ＜θ＜π時

設(shè)給定某圓弧起始點為：Ps=(xs，ys，zs)，終止點為：Pe=(xe，ye，ze)，圓心坐標為：P0=(x0，y0，z0)，圓弧半徑為R。則弦PsPe長度L為：

現(xiàn)根據(jù)圓弧起始點Ps、終止點Pe求插補點Pi的空間坐標，首先對弦PsPe等分，等分精度為ΔL，如圖1（a）所示。

圖1 空間圓弧插補原理Fig.1 Principle of Space Circular Interpolation

設(shè)圓弧上第i個插補點為Pi，則直線P0Pi與弦PsPe的交點為。Ps長度為：

（2）當圓弧圓心角為π ＜θ＜2π時

設(shè)給定某圓弧起始點為：Ps=(xs，ys，zs)，終止點為：Pe=(xe，ye，ze)，圓心坐標為：P0=(x0，y0，z0)，圓弧半徑為R?？汕蟮每臻g圓弧PeP0延長線上的點，空間圓弧PsP0延長線上的點如圖1（b）所示：可以求得Pe與Ps直徑對點坐標坐標。圓弧的圓心角均屬于0 ＜θ＜π，每一段可根據(jù)圓心角0 ＜θ＜π時進行插補。

3 圓弧插補算法驗證及計算效率分析對比

3.1 圓弧插補算法Matlab仿真驗證

（1）當圓弧圓心角為0 ＜θ＜π的情況

設(shè)圓弧的起始點：Ps=(81.55，174.76，87.38)終止點：Pe=(42.81，162.68，66.75)，圓心：P0=(70，150，75)。設(shè)弦分割精度ΔL=0.1mm繪制圓弧軌跡，如圖2（a）所示。

圖2 圓弧插補軌跡Fig.2 Arc Interpolation Trajectory

（2）當圓弧圓心角為π ＜θ＜2π時情況

這指的是中國共產(chǎn)黨在革命與建設(shè)過程中形成的思想、制度、文化等，如在革命戰(zhàn)爭年代與馬克思主義相結(jié)合，并與時俱進形成的毛澤東思想、鄧小平理論、三個代表、科學發(fā)展觀、社會主義核心價值觀等思想體系以及由此建立的中國特色社會主義制度。

設(shè)圓弧的起始點：Ps=(81.55，174.76，87.38)終止點：Pe=(97.18，137.31，34，75)，圓心：P0=(70，150，75)。則直線ps p0與圓弧的交點為=(42.81，162.68，66.75)，直線pe p0與圓弧的交點為P'e=(58.44，125.23，62.61)，設(shè)弦分割精度ΔL=0.1mm。繪制圓弧軌跡，如圖2（b）所示。

（3）對于θ=2π的整圓情況

3.2 計算效率分析對比

為了驗證提出的空間圓弧插補算法的計算效率，在Matlab中本算法與其他常見的算法文獻［2、13、16］相比較。以圖2（a）為例，在相同圓弧，插補點個數(shù)均為1000時，所用運算時間，如表1所示。測試操作系統(tǒng)為64 位Windows10，CPU 型號Intel（R）Core（TM）i7-7700@3.60GHz，32G內(nèi)存。由表1可知，使用等弦分割采樣圓弧插補算法計算插補點在計算時間上明顯低于其他算法。

表1 幾種常見的空間圓弧插補算法比較Tab.1 Comparison of Several Common Spatial Arc Interpolation Algorithms

3.3 插補算法樣機驗證

試驗樣機為五軸混聯(lián)機器人，前兩個自由度為SPU+U并聯(lián)機構(gòu)，后三個自由度為串聯(lián)機構(gòu)?？刂破鲀?nèi)嵌TwinCAT 3.1控制軟件，可以將任何PC系統(tǒng)轉(zhuǎn)換為PLC、NC模塊的實時控制系統(tǒng)，其PLC內(nèi)置運動控制庫“Tc2_MC2”，使用PTP的方式通過外部位置發(fā)生器寫入數(shù)值。實驗采用記號筆在白紙上畫出圓弧軌跡的方法對上述圓弧插補算法進行驗證。機器人所畫的圓弧軌跡，如圖3所示。坐標單位為厘米，其中圖3（a）為圓心角為0 ＜θ＜π，起始點、終止點、圓心坐標分別為PS=(-10，-150.1，135.3)、Pe=(0，-150.1，145.3)、P0=(0，-150.1，135.3)。圖3（b）為圓心角π ＜θ＜2π，起始點、終止點、圓心坐標分別為Ps=(0，-150.1，145.3)、PS=(3.42，-150.1，125.9)、P0=(0，-150.1，135.3)。圖3（c）為整圓，圓心坐標與半徑為，P0=(0，-150.1，135.3)，R=30cm。

圖3 樣機驗證Fig.3 Experiment Validation

圖3機器人畫出的圓弧軌跡驗證了提出算法的可行性，正確性。

4 球面插補算法

機器人在工作的時候，末端需要沿特定的空間球面進行插補。空間球面插補可以看作機器人末端在空間上走了m段徑向的半圓。與圓弧插補相比，球面插補所要求的插補點計算更復雜，提出的空間圓弧算法具有計算量小，插補點準確等特點。將提出的圓弧插補算法應用到機器人球面插補之中，求得球面插補點坐標。球面插補算法步驟如下：

（1）設(shè)給定空間球心坐標P0與球心半徑R，如圖4所示。平面M與基座標系z軸垂直并包含球心P0?？汕蟮们蛎娌逖a輔助點坐標P1-P6。

圖4 球面插補點坐標示意圖Fig.4 Spherical Interpolation Points

設(shè)?=atan 2(y0，x0)。則P1-P6的坐標為：

（2）球面可以做m段徑向半圓，使用等弦分割采樣空間圓弧算法求得P1-P4所在球面大圓m+1個插補點坐標，用于計算機器人插補過程中每段徑向半圓姿態(tài)。設(shè)第j個插補點為Pj=(xj，yj，zj)。則：

式中：?j—機器人末端單位向量在基座標上的投影夾角。

機器人末端姿態(tài)為：

（3）設(shè)每段徑向半圓起始點與終止點為Pstart，Pend。Pstart=P5，Pend=P6。Pj為P1-P4所在球面大圓第j個插補點。令j=0。

①使用本算法計算圓弧PstarPj與圓弧PjPend組成的徑向半圓中n+1個插補點坐標。

②求解機器人在圓弧PstarPj與圓弧PjPend插補過程中姿態(tài)。

③當機器人位于Pend并且j≠m+1時，交換Pstart，Pend。j=j+1時返回至第一步求解下一段徑向半圓位姿。當機器人位于Pend并且j=m+1時，結(jié)束插補運動。

5 球面插補算法仿真

5.1 球面插補點仿真

設(shè)球心為P0=(30，-100，100)，球心半徑為R=30cm，機器人末端在空間上走的徑向的半圓數(shù)m=20，每段徑向半圓插補次數(shù)n=100。則根據(jù)Matlab畫出的球面插補點，如圖5所示。

圖5 球面插補點仿真圖Fig.5 Spherical Interpolation Point Simulation

5.2 機器人球面插補運動仿真

通過已得到球面插補點坐標，末端姿態(tài)，球心P0=(30，-100，100)，球心半徑R=30cm。使用Matlab Robotics Toolbox工具箱編程可以得到機器人球面插補運動仿真過程，如圖6所示。

圖6 機器人球面插補運動仿真Fig.6 Spherical Interpolation Motion Simulation of Robot

6 結(jié)論

（1）提出一種基于等弦分割采樣的新型空間圓弧插補算法，為實現(xiàn)球面插補奠定理論基礎(chǔ)。該算法根據(jù)弦長分割所求得插補點均在圓弧上，無需坐標變換，積分運算，微分運算等，簡化了機器人插補過程中運算量。在Matlab中仿真并與其他常見算法進行對比，計算量大幅減小。通過樣機實驗，驗證了上述插補算法的正確性。

（2）將這種新型空間算法應用到更為復雜空間球面插補，求解球面插補點坐標以及姿態(tài)，仿真出球面插補點。對實驗室機器人進行仿真模擬出機器人在球面插補中的運動過程作出驗證，實現(xiàn)了對不同尺寸球面插補仿真。