陳 璽，李傳軍

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

1 引言

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

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

2 空間圓弧插補(bǔ)算法

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

給定圓心坐標(biāo)，起始點(diǎn)和終止點(diǎn)坐標(biāo)，插補(bǔ)圓弧的圓心角θ，即可求出插補(bǔ)點(diǎn)。此算法分為兩種情況。圓弧的圓心角0 ＜θ＜π和π ＜θ＜2π。

（1）當(dāng)圓弧圓心角為0 ＜θ＜π時(shí)

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

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

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

設(shè)圓弧上第i個(gè)插補(bǔ)點(diǎn)為Pi，則直線P0Pi與弦PsPe的交點(diǎn)為。Ps長(zhǎng)度為：

（2）當(dāng)圓弧圓心角為π ＜θ＜2π時(shí)

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

3 圓弧插補(bǔ)算法驗(yàn)證及計(jì)算效率分析對(duì)比

3.1 圓弧插補(bǔ)算法Matlab仿真驗(yàn)證

（1）當(dāng)圓弧圓心角為0 ＜θ＜π的情況

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

圖2 圓弧插補(bǔ)軌跡Fig.2 Arc Interpolation Trajectory

（2）當(dāng)圓弧圓心角為π ＜θ＜2π時(shí)情況

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

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

（3）對(duì)于θ=2π的整圓情況

3.2 計(jì)算效率分析對(duì)比

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

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

3.3 插補(bǔ)算法樣機(jī)驗(yàn)證

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

圖3 樣機(jī)驗(yàn)證Fig.3 Experiment Validation

圖3機(jī)器人畫出的圓弧軌跡驗(yàn)證了提出算法的可行性，正確性。

4 球面插補(bǔ)算法

機(jī)器人在工作的時(shí)候，末端需要沿特定的空間球面進(jìn)行插補(bǔ)?？臻g球面插補(bǔ)可以看作機(jī)器人末端在空間上走了m段徑向的半圓。與圓弧插補(bǔ)相比，球面插補(bǔ)所要求的插補(bǔ)點(diǎn)計(jì)算更復(fù)雜，提出的空間圓弧算法具有計(jì)算量小，插補(bǔ)點(diǎn)準(zhǔn)確等特點(diǎn)。將提出的圓弧插補(bǔ)算法應(yīng)用到機(jī)器人球面插補(bǔ)之中，求得球面插補(bǔ)點(diǎn)坐標(biāo)。球面插補(bǔ)算法步驟如下：

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

圖4 球面插補(bǔ)點(diǎn)坐標(biāo)示意圖Fig.4 Spherical Interpolation Points

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

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

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

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

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

①使用本算法計(jì)算圓弧PstarPj與圓弧PjPend組成的徑向半圓中n+1個(gè)插補(bǔ)點(diǎn)坐標(biāo)。

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

③當(dāng)機(jī)器人位于Pend并且j≠m+1時(shí)，交換Pstart，Pend。j=j+1時(shí)返回至第一步求解下一段徑向半圓位姿。當(dāng)機(jī)器人位于Pend并且j=m+1時(shí)，結(jié)束插補(bǔ)運(yùn)動(dòng)。

5 球面插補(bǔ)算法仿真

5.1 球面插補(bǔ)點(diǎn)仿真

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

圖5 球面插補(bǔ)點(diǎn)仿真圖Fig.5 Spherical Interpolation Point Simulation

5.2 機(jī)器人球面插補(bǔ)運(yùn)動(dòng)仿真

通過(guò)已得到球面插補(bǔ)點(diǎn)坐標(biāo)，末端姿態(tài)，球心P0=(30，-100，100)，球心半徑R=30cm。使用Matlab Robotics Toolbox工具箱編程可以得到機(jī)器人球面插補(bǔ)運(yùn)動(dòng)仿真過(guò)程，如圖6所示。

圖6 機(jī)器人球面插補(bǔ)運(yùn)動(dòng)仿真Fig.6 Spherical Interpolation Motion Simulation of Robot

6 結(jié)論

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

（2）將這種新型空間算法應(yīng)用到更為復(fù)雜空間球面插補(bǔ)，求解球面插補(bǔ)點(diǎn)坐標(biāo)以及姿態(tài)，仿真出球面插補(bǔ)點(diǎn)。對(duì)實(shí)驗(yàn)室機(jī)器人進(jìn)行仿真模擬出機(jī)器人在球面插補(bǔ)中的運(yùn)動(dòng)過(guò)程作出驗(yàn)證，實(shí)現(xiàn)了對(duì)不同尺寸球面插補(bǔ)仿真。