等弦分割采樣的球面插補(bǔ)算法研究

陳 璽，李傳軍

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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π時情況

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

設(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）對于θ=2π的整圓情況

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

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

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

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

試驗(yàn)樣機(jī)為五軸混聯(lián)機(jī)器人，前兩個自由度為SPU+U并聯(lián)機(jī)構(gòu)，后三個自由度為串聯(lián)機(jī)構(gòu)?？刂破鲀?nèi)嵌TwinCAT 3.1控制軟件，可以將任何PC系統(tǒng)轉(zhuǎn)換為PLC、NC模塊的實(shí)時控制系統(tǒng)，其PLC內(nèi)置運(yùn)動控制庫“Tc2_MC2”，使用PTP的方式通過外部位置發(fā)生器寫入數(shù)值。實(shí)驗(yàn)采用記號筆在白紙上畫出圓弧軌跡的方法對上述圓弧插補(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ī)器人在工作的時候，末端需要沿特定的空間球面進(jìn)行插補(bǔ)。空間球面插補(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軸垂直并包含球心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個插補(bǔ)點(diǎn)坐標(biāo)，用于計(jì)算機(jī)器人插補(bǔ)過程中每段徑向半圓姿態(tài)。設(shè)第j個插補(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個插補(bǔ)點(diǎn)。令j=0。

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

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

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

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)動仿真

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

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

6 結(jié)論

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

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