[發(fā)明專利]一種用于運(yùn)動(dòng)控制系統(tǒng)的圓心模式空間圓弧插補(bǔ)方法

有效

• 申請?zhí)枺?/td>: 201310694244.X
• 申請日: 2013-12-13
• 公開（公告）號: CN103676787A
• 公開（公告）日: 2014-03-26
• 發(fā)明（設(shè)計(jì)）人: 仲崇權(quán);李稚春;龔中強(qiáng);孫紅濤;劉雪梅;劉雪喆;劉鑫
• 申請（專利權(quán)）人: 大連理工計(jì)算機(jī)控制工程有限公司
• 主分類號: G05B19/41
• 分類號: G05B19/41
• 代理公司: 大連星海專利事務(wù)所 21208
• 代理人: 王樹本

1.一種用于運(yùn)動(dòng)控制系統(tǒng)的圓心模式空間圓弧插補(bǔ)方法，其特征在于：根據(jù)圓心模式給定的圓心坐標(biāo)，計(jì)算直角坐標(biāo)系OXYZ與空間圓弧所在平面建立的O′X′Y′Z′坐標(biāo)系的坐標(biāo)變換矩陣，計(jì)算整個(gè)空間圓弧弧長，在O′X′Y′平面確定滿足基于梯形曲線加減速控制的平面圓弧插補(bǔ)過程中的弧長計(jì)算公式，計(jì)算O′X′Y′平面每個(gè)插補(bǔ)周期平面圓弧插補(bǔ)點(diǎn)坐標(biāo)，計(jì)算OXYZ坐標(biāo)系下空間圓弧插補(bǔ)點(diǎn)坐標(biāo)，空間圓弧插補(bǔ)模塊將空間圓弧插補(bǔ)點(diǎn)坐標(biāo)輸出給位置閉環(huán)控制模塊，進(jìn)行位置閉環(huán)控制；插補(bǔ)方法具體步驟如下：

（A）根據(jù)圓心模式給定的圓心坐標(biāo)O′(x_o′,y_o′,z_o′)，計(jì)算直角坐標(biāo)系OXYZ與空間圓弧所在平面建立的O′X′Y′Z′坐標(biāo)系的坐標(biāo)變換矩陣M，包括以下步驟：

第一步、在O′X′Y′Z′坐標(biāo)系，以向量計(jì)算X′軸正向單位向量則i′→=O′P1→|O′P1→|---(1)]]>

式中，P₁(x₁,y₁,z₁)為空間圓弧起點(diǎn)坐標(biāo)；

第二步、計(jì)算空間圓弧所在的O′X′Y′平面單位法向向量

則k′→=O′P3→×O′P1→|O′P3→×O′P1→|(O′P3→×O′P1→>0→)k′→=O′P2→×O′P1→|O′P2→×O′P1→|(O′P3→×O′P1→=0→)---(2)]]>

式中，P₃(x₃,y₃,z₃)為空間圓弧終點(diǎn)坐標(biāo)，P₂(x₂,y₂,z₂)為空間圓弧圓心、起點(diǎn)與終點(diǎn)三點(diǎn)共線時(shí)，空間圓弧上一點(diǎn)坐標(biāo)；

第三步、計(jì)算在空間圓弧插補(bǔ)方向約束下的Z′正向單位向量

則k′→·i→·sign≤0k′→·j→·sign≤0k′→·k→·sign≤0---(3)]]>

式中，為直角坐標(biāo)系X軸正向單位向量，為直角坐標(biāo)系Y軸正向單位向量，為直角坐標(biāo)系Z軸正向單位向量，sign為空間圓弧插補(bǔ)方向符號，順時(shí)針插補(bǔ)時(shí)sign為-1，逆時(shí)針插補(bǔ)時(shí)sign為1；

第四步、根據(jù)右手定則計(jì)算Y′正向單位向量

則j′→=k′→×i′→---(4)]]>

第五步、由公式（1）、（2）、（3）和（4）得坐標(biāo)變換矩陣為，

M=ix′iy′iz′jx′jy′jz′kx′ky′kz′---(5)]]>

（B）計(jì)算整個(gè)空間圓弧弧長S_s，包括以下步驟：

第一步、計(jì)算P₃在O′X′Y′Z′坐標(biāo)系中的坐標(biāo)P_′3(x′₃,y′₃,z_′3)，

x3′y3′z3′=M·x3-xo′y3-yo′z3-zo′---(6)]]>

第二步、由公式（6）得P′₃在O′X′Y′Z′坐標(biāo)系末角度正弦值和余弦值計(jì)算公式為：

第三步、由公式（7）得空間圓弧弧長計(jì)算公式為：

（C）在O′X′Y′平面確定滿足基于梯形曲線加減速控制的平面圓弧插補(bǔ)過程中的弧長計(jì)算公式，包括以下步驟：

第一步、由公式（8）得基于梯形曲線加減速控制的圓弧插補(bǔ)的加減速結(jié)束時(shí)間節(jié)點(diǎn)計(jì)算公式為：

t1=vf-v0a1t2=t1+1vf[Ss-(v0t1+12a1t12)-12vf2a2]t3=t1+t2+vfa2---(9)]]>

式中，t₁為平面圓弧插補(bǔ)的加速結(jié)束時(shí)刻，t₂為平面圓弧插補(bǔ)的恒速結(jié)束時(shí)刻，t₃為平面圓弧插補(bǔ)的減速結(jié)束時(shí)刻，v₀為平面圓弧插補(bǔ)初始線速度，v_f為平面圓弧插補(bǔ)進(jìn)給線速度，a₁為平面圓弧插補(bǔ)加速度，a₂為平面圓弧插補(bǔ)減速度；

第二步、由公式（9）得O′X′Y′平面每個(gè)插補(bǔ)周期T加速度計(jì)算公式為：

a(iT)=a10≤i≤N10N1<i≤N2a2N2<i≤N3---(10)]]>

式中，N₁為t₁/T取整后需要的插補(bǔ)周期總個(gè)數(shù)，N₂為t₂/T取整后需要的插補(bǔ)周期總個(gè)數(shù)，N₃為t₃/T取整后需要的插補(bǔ)周期總個(gè)數(shù)，i=0,1,2…N₃；

第三步、由公式（10）得O′X′Y′平面每個(gè)插補(bǔ)周期線速度計(jì)算公式為：

v(iT)=v0+Σi=0N3a(iT)·T---(11)]]>

第四步、由公式（11）得O′X′Y′平面每個(gè)插補(bǔ)周期弧長計(jì)算公式為：

s(iT)=Σi=0N3v(iT)·T---(12)]]>

（D）計(jì)算O′X′Y′平面每個(gè)插補(bǔ)周期平面圓弧插補(bǔ)點(diǎn)坐標(biāo)(x′_i,y′_i)，

由公式（12）得O′X′Y′平面每個(gè)插補(bǔ)周期平面圓弧插補(bǔ)點(diǎn)坐標(biāo)(x′_i,y′_i)計(jì)算公式為：

xi′=|O′P1→|·cos(s(iT)|O′P1→|)yi′=|O′P1→|·sin(s(iT)|O′P1→|)---(13)]]>

（E）計(jì)算OXYZ坐標(biāo)系下空間圓弧插補(bǔ)點(diǎn)坐標(biāo)(x_i,y_i,z_i)，

由公式（5）和（13）經(jīng)過坐標(biāo)變換，得OXYZ坐標(biāo)系下空間圓弧插補(bǔ)點(diǎn)坐標(biāo)(x_i,y_i,z_i)計(jì)算公式為：

xiyizi=MTxi′yi′0+xo′yo′zo′---(14)]]>

其中M^T為M的轉(zhuǎn)換矩陣；

（F）空間圓弧插補(bǔ)模塊將空間圓弧插補(bǔ)點(diǎn)坐標(biāo)輸出給位置閉環(huán)控制模塊，進(jìn)行位置閉環(huán)控制。