一種考慮關(guān)節(jié)躍度約束的實(shí)時(shí)刀具路徑光順?biāo)惴?/h1>
2024-03-31 05:14:27杜煦常澤鑫鄭軍強(qiáng)任鵬飛
中國(guó)機(jī)械工程 2024年2期
杜煦 常澤鑫 鄭軍強(qiáng) 任鵬飛
摘要 ：針對(duì)機(jī)器人加工過程中關(guān)節(jié)躍度超過約束的問題，提出一種考慮機(jī)器人關(guān)節(jié)躍度約束的刀具路徑光順?biāo)惴ā２捎糜邢逈_激響應(yīng)濾波器插值算法對(duì)五軸刀具路徑進(jìn)行光順處理，一步完成了路徑光順和速度規(guī)劃。建立了關(guān)節(jié)與刀具之間的速度、加速度和躍度關(guān)系，通過調(diào)整時(shí)間常數(shù)保證了關(guān)節(jié)躍度不超過約束?？紤]機(jī)器人關(guān)節(jié)加速度約束后，該算法計(jì)算出的時(shí)間常數(shù)更小。仿真和實(shí)驗(yàn)結(jié)果表明該算法能減小關(guān)節(jié)的跟蹤誤差，提高機(jī)器人的工作穩(wěn)定性。
關(guān)鍵詞 ：關(guān)節(jié)躍度約束；五軸刀具路徑光順；有限沖激響應(yīng)濾波器插值；機(jī)器人加工
中圖分類號(hào) ：TH161；TP242
DOI：10.3969/j.issn.1004-132X.2024.02.012
開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：
A Real-time Tool Path Smoothing Algorithm Considering Joint
Jerk Constraints
DU Xu 1 CHANG Zexin 1 ZHENG Junqiang 2 REN Pengfei 1
1.School of Mechanical Engineering，Zhejiang Sci-Tech University，Hangzhou，310018
2.School of Mechanical Engineering，Hangzhou Dianzi University，Hangzhou，310018
Abstract ： Aiming at the problems of joint jerk exceeding constraints in robot machining， a tool path smoothing algorithm was proposed considering joint jerk constraints. The FIR filter interpolation was used to smooth the five-axis tool paths. Path smoothing and speed planning were achieved in one step. The relationship of velocity， acceleration， and jerk between joint and tool was established， and the joint jerk did not exceed the constraint by adjusting the time constant. Time constant calculated by the proposed method was smaller than considering the acceleration constraints of robot joint. Simulation and experimental results show that the proposed algorithm may reduce the tracking error of the joint and improve the stability of the robot joint in machining.
Key words ： joint jerk constraint； five-axis tool path smoothing； finite impulse response（FIR） filter interpolation； robot machining
0 引言
與數(shù)控機(jī)床相比，機(jī)器人具有柔性高、工作空間大等優(yōu)點(diǎn)，在復(fù)雜零件的加工中得到了越來越廣的應(yīng)用 ?［1-2］ 。在機(jī)器人的加工任務(wù)中，刀具路徑命令通常為離散的線性命令（G01） ?［3］ 。轉(zhuǎn)角處的刀具路徑是切向不連續(xù)的，這導(dǎo)致關(guān)節(jié)的速度和加速度也不連續(xù)，因此刀具在每個(gè)轉(zhuǎn)角往往需要減速到零，這不僅降低了加工效率，還會(huì)引起機(jī)器人關(guān)節(jié)的振動(dòng) ?［4］ 。為解決小線段路徑帶來的問題，通常對(duì)路徑進(jìn)行光順處理，以實(shí)現(xiàn)機(jī)器人關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng) ?［5］ 。
五軸刀具路徑光順一般分為兩步，首先使用參數(shù)曲線分別對(duì)刀尖和刀軸的路徑進(jìn)行光順處理 ?［6-7］ ，再使用數(shù)值方法對(duì)光順后的參數(shù)曲線進(jìn)行同步 ?［8-9］ 。參數(shù)曲線插補(bǔ)計(jì)算復(fù)雜，難以在線計(jì)算 ?［10-12］ ，且參數(shù)曲線與直線路徑的連接處通常是高階不連續(xù)的，導(dǎo)致機(jī)器人關(guān)節(jié)產(chǎn)生振動(dòng)，影響加工質(zhì)量 ?［13］ 。
TAJIMA等 ?［14-16］ 采用有限沖激響應(yīng)（finite impulse response，F(xiàn)IR）濾波器分別與刀尖速度和刀軸速度作卷積運(yùn)算，并通過重疊運(yùn)動(dòng)學(xué)曲線一步實(shí)現(xiàn)了路徑光順和速度規(guī)劃。與傳統(tǒng)方法相比，這種實(shí)時(shí)插補(bǔ)算法減少了計(jì)算量，有利于數(shù)控系統(tǒng)的實(shí)時(shí)插補(bǔ)，但沒有考慮關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束。LIU等 ?［17］ 采用FIR濾波插值算法在工作空間中對(duì)刀尖運(yùn)動(dòng)進(jìn)行插值，在關(guān)節(jié)空間中對(duì)刀軸運(yùn)動(dòng)進(jìn)行插值。此算法雖然可以直接滿足關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束，但不適用于機(jī)器人。SUN等 ?［18］ 將基于FIR濾波的光順?biāo)惴ㄓ糜跈C(jī)器人加工，通過機(jī)器人關(guān)節(jié)位移對(duì)刀具位移的導(dǎo)數(shù)，建立關(guān)節(jié)與刀具之間的速度和加速度的關(guān)系，通過關(guān)節(jié)的速度和加速度約束來調(diào)整FIR濾波器的時(shí)間常數(shù)（保證機(jī)器人關(guān)節(jié)的速度和加速度在約束內(nèi)），并在六自由度機(jī)器人上進(jìn)行了驗(yàn)證。上述方法都沒有考慮關(guān)節(jié)的躍度約束，而機(jī)器人的耦合特性會(huì)導(dǎo)致某些時(shí)刻的關(guān)節(jié)躍度超過約束 ?［19-20］ ，對(duì)機(jī)器人造成沖擊，影響加工質(zhì)量。
一種考慮關(guān)節(jié)躍度約束的實(shí)時(shí)刀具路徑光順?biāo)惴ā?煦 常澤鑫 鄭軍強(qiáng)等
中國(guó)機(jī)械工程 第35卷 第2期 2024年2月
針對(duì)上述問題，本文提出一種考慮關(guān)節(jié)躍度約束的刀具路徑光順?biāo)惴āＪ褂肍IR濾波器分別對(duì)刀尖和刀軸運(yùn)動(dòng)進(jìn)行濾波，得到躍度有限的刀具運(yùn)動(dòng)，再通過調(diào)整FIR濾波的時(shí)間常數(shù)實(shí)現(xiàn)刀尖和刀軸的運(yùn)動(dòng)同步；建立了關(guān)節(jié)約束和切向運(yùn)動(dòng)學(xué)約束之間的關(guān)系，通過調(diào)整FIR濾波器的時(shí)間常數(shù)來滿足關(guān)節(jié)的速度、加速度和躍度約束;在五自由度加工機(jī)器人上進(jìn)行了仿真和實(shí)驗(yàn)。
1 基于FIR濾波的路徑光順
1.1 FIR濾波器原理
FIR 濾波器是一個(gè)有限長(zhǎng)度的單位脈沖響應(yīng)濾波器，在拉普拉斯域中，其傳遞函數(shù)定義為
G n（s）= 1－ exp （sT n） sT n ??（1）
式中，T n為第n（n=1，2，…，N）個(gè) FIR 濾波器的時(shí)間常數(shù)；N為濾波器的個(gè)數(shù)。
求式（1）的拉普拉斯逆變換，可以得到時(shí)域上的脈沖信號(hào)：
g（t）= δ（t）－δ（t－T n） T n ??（2）
δ（t）= 1 ?t≥0 0 ?t<0
式（2）表示的是單位脈沖信號(hào)，任意曲線與 FIR 濾波器作卷積運(yùn)算時(shí)，曲線與X軸圍成的面積不會(huì)發(fā)生改變，即卷積運(yùn)算前后曲線的積分結(jié)果不變。利用該特性，將速度曲線與 FIR 濾波器進(jìn)行卷積運(yùn)算，則卷積前后的刀具位移不會(huì)改變。速度曲線與多個(gè)濾波器卷積運(yùn)算后，便可得到高階連續(xù)的運(yùn)動(dòng)學(xué)曲線。濾波后的速度曲線由濾波前的速度曲線與多個(gè)濾波器卷積運(yùn)算后得到，即
v′（t）=v（t） g 1（t） g 2（t） … g n（t） ?（3）
n=1時(shí)，生成的是梯形速度曲線，加速度不連續(xù)，會(huì)導(dǎo)致加工產(chǎn)生沖擊。n=2時(shí)，生成的是 S 形速度曲線，加速度連續(xù)且速度曲線較平滑，廣泛用于機(jī)床和機(jī)器人的加工系統(tǒng)。 本文采用2個(gè)濾波器與速度曲線進(jìn)行卷積，運(yùn)動(dòng)學(xué)曲線如圖1所示，圖中，T ??v 1 為卷積前的運(yùn)動(dòng)時(shí)間，T ??v 2 、T ??v 3 分別為第一個(gè)和第二個(gè)濾波器的時(shí)間常數(shù)。
1.2 切向約束下的時(shí)間常數(shù)計(jì)算
加工中，工藝決定了刀具的最大速度、最大加速度和最大躍度即刀具的切向運(yùn)動(dòng)學(xué)約束。 FIR 濾波器的時(shí)間常數(shù)決定了濾波后刀具的速度、加速度和躍度的大小，因此需要計(jì)算出滿足刀具切向運(yùn)動(dòng)學(xué)約束的時(shí)間常數(shù)。以相鄰的刀具路徑為例來說明切向約束下計(jì)算時(shí)間常數(shù)的過程。首先計(jì)算刀尖位移和刀具角位移：
L n=| P ??n+1 － P ?n|
θ n= arccos | O ?n· O ??n+1 | ??（4）
式中，L n為第n段刀尖路徑的長(zhǎng)度； P ?n、 P ??n+1 分別是刀尖路徑上第n個(gè)和第n+1個(gè)路徑點(diǎn)的坐標(biāo)；θ n為第n段刀軸路徑的角度； O ?n、 O ??n+1 分別為刀軸路徑上第n個(gè)和第 n+1 個(gè)路徑點(diǎn)的坐標(biāo)。
以刀尖運(yùn)動(dòng)為例，根據(jù)切向約束計(jì)算出 FIR 濾波器時(shí)間常數(shù)的范圍：
T ??v1 ?≥L n/v ??max
T ??v2 ?≥v ??max ?/a ??max
T ??v3 ?≥a ??max ?/j ??max ????（5）
式中，v ??max ?、a ??max ?、j ??max ?分別為切向的速度、加速度和躍度 約束。
為生成時(shí)間最優(yōu)的運(yùn)動(dòng)學(xué)軌跡，式（5）中各式取等號(hào)。
1.3 時(shí)間同步
計(jì)算出時(shí)間常數(shù)后，還需要對(duì)刀尖和刀軸的運(yùn)動(dòng)進(jìn)行時(shí)間同步。刀尖的運(yùn)動(dòng)時(shí)間T ?v 、刀軸的運(yùn)動(dòng)時(shí)間T ?ω 分別為
T ?v =∑ 3 n=1 T ??v n ??（6）
T ?ω =∑ 3 n=1 T ??ω n ??（7）
如果T ?ω >T ?v ，則調(diào)整刀尖運(yùn)動(dòng)的時(shí)間常數(shù)T ??v 1 ，同步刀軸運(yùn)動(dòng)。T ??v 1 按下式調(diào)整計(jì)算：
T′ ???v 1 =T ?ω －T ?v +T ??v 1 ??（8）
調(diào)整后，T ??v 1 增大到T′ ???v 1 ，T ??v 2 、T ??v 3 不變，因此最大速度v ??max ?、加速度a ??max ?和躍度j ??max ??會(huì)減小，這意味著同步后的刀尖運(yùn)動(dòng)依然滿足切向運(yùn)動(dòng)學(xué)約束。如果T ?ω T′ ???ω 1 =T ?v －T ?ω +T ??ω 1 ??（9）
如果T ?ω =T ?v ，則刀尖運(yùn)動(dòng)的時(shí)間常數(shù)和刀軸運(yùn)動(dòng)的時(shí)間常數(shù)不需要調(diào)整。
1.4 光順誤差控制
1.2節(jié)和1.3節(jié)計(jì)算了路徑點(diǎn)之間的刀具運(yùn)動(dòng)，生成的刀具速度在路徑轉(zhuǎn)角處為零，這種運(yùn)動(dòng)造成效率損失。通常的處理方法是在轉(zhuǎn)角處重疊相鄰路徑的速度曲線，以避免轉(zhuǎn)角處的刀具速度降為零，并生成光滑的運(yùn)動(dòng)軌跡。首先利用下式將刀尖速度分解為X、Y、Z方向的速度：
v x（t） v y（t） v z（t） ?=v（t） ?l x l y l z ?=v（t） ?P ??n+1 － P ?n | P ??n+1 － P ?n| ??（10）
利用球面線性插值，將刀軸角速度分解為i、j、k方向的角速度 ?［15］ ：
v i v j v k = ω（t）（ O ??n+1 ?cos ?θ（t）－ O ?n cos （θ n－θ（t））） ?sin ?θ n
θ n= arccos （ O ?n· O ??n+1 ） ?（11）
通過轉(zhuǎn)角時(shí)，分別重疊X、Y、Z、i、j、k方向的速度曲線。重疊速度曲線前，刀具沿線性路徑運(yùn)動(dòng)。重疊速度曲線后，刀具會(huì)在轉(zhuǎn)角處偏離原始的線性路徑，產(chǎn)生幾何偏差。因此需要選擇合適的重疊時(shí)間T ??vc ?以滿足刀尖和刀軸的光順誤差 約束。
以刀尖運(yùn)動(dòng)為例，如圖2所示，當(dāng)相鄰路徑的速度v n=v ?n+1 時(shí)，轉(zhuǎn)角輪廓關(guān)于轉(zhuǎn)角平分線對(duì)稱，最大的光順誤差出現(xiàn)在輪廓的中點(diǎn) P ?m。刀尖光順誤差ε ??TCP ?的計(jì)算公式為
ε ??TCP ?=| P ?m－ P ?n|=|s 2 l ?2－s 1 l ?1|=
T 3 ??vc ??48T ??v2 ?T ??v3 ??V ???????0≤T ??vc ?≤2T ??v3
4T 2 ??v3 ?－6T ??v3 ?T ??vc ?+3T 2 ??vc ??24T ??v2 ??V 2T ??v3 ?＜T ??vc ?≤T ??v2 ?+T ??v3 ????（12）
V= v 2 n+v 2 ?n+1 －v nv ?n+1 ?cos ?θ ??TCP
θ ??TCP ?= arccos （ l ?1· l ?2）
式中， l ?1、 l ?2分別為 P ??n-1 ?P ?n和 P ?n P ??n+1 的方向向量；s 1為從 P ?m減速到 P ?n的距離；s 2為從 P ?n加速到 P ?m的距離；θ ??TCP ?為 l ?1和 l ?2的夾角。
由式（12）可以看出，隨著重疊時(shí)間T ??vc ?的增加，刀尖光順誤差逐漸增大。為滿足刀尖光順誤差約束ε ?p ???max ?，根據(jù)式（12）得到轉(zhuǎn)角重疊時(shí)間T ??vc ?的計(jì)算公式：
T ??vc ?= ??3 ?48T ??v2 ?T ??v3 ?ε ?p ???max ??V ???????0≤T ??vc ?≤2T ??v3
T ??v3 ?+ 3 ?8T ??v2 ?ε ?p ???max ??V － T 2 ??v3 ??3 ??2T ??v3 ?＜T ??vc ?≤T ??v2 ?+T ??v3 ????（13）
并選擇滿足對(duì)應(yīng)公式范圍的值。如果v n≠v ?n+1 ，則轉(zhuǎn)角輪廓不再關(guān)于角平分線對(duì)稱，在這種情況下，式（12）計(jì)算的輪廓中點(diǎn)誤差將大于最大的光順誤差，因此通過式（13）計(jì)算出的重疊時(shí)間T ??vc ?依然可以使刀尖光順誤差滿約束ε ?p ???max ?。
同理，可以近似計(jì)算出刀軸的重疊運(yùn)動(dòng)時(shí)間T ??ωc ?。最后選擇T ??vc ?和T ??ωc ?中的較小值作為刀尖和刀軸運(yùn)動(dòng)的重疊時(shí)間，保證光順誤差在約束范圍內(nèi)，則重疊時(shí)間T ?c 為
T ?c = min （T ??vc ?，T ??ωc ?） ?（14）
2 關(guān)節(jié)約束下的時(shí)間常數(shù)調(diào)整
機(jī)器人關(guān)節(jié)和刀具之間存在復(fù)雜的運(yùn)動(dòng)關(guān)系，在滿足切向約束的情況下，關(guān)節(jié)的速度、加速度和躍度有可能超過關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束。 建立關(guān)節(jié)運(yùn)動(dòng)和刀尖運(yùn)動(dòng)的關(guān)系：
q · = ?d q ?d t = ?d q ?d s ??d s ?d t =q′s · ??（15）
q ?¨ =q″s · ?2+q′s ?¨ ??（16）
q … =q s · ?3+3q″s · s ?¨ +q′s … ??（17）
式中，q · 、q ?¨ 、q … 分別為關(guān)節(jié)的速度、加速度和躍度；q′、q″、q 分別為關(guān)節(jié)位移關(guān)于刀尖位移s的一階、二階和三階導(dǎo)數(shù)；s · 、s ?¨ 和s … 分別為刀尖的速度、加速度和躍度。
2.1 關(guān)節(jié)速度約束
關(guān)節(jié)的速度需要滿足：
|q · ?n|=|q′ ?n|s · ≤|q′ ?n|s · ???max ?≤q · ???max ，n ??（18）
n=1，2，…，5
s · ???max ?=L/T ??v1 ???（19）
式中，q′ ?n為第n個(gè)關(guān)節(jié)的位移關(guān)于刀尖位移的一階導(dǎo)數(shù)； q · ???max ，n 為第n個(gè)關(guān)節(jié)的速度約束；s · ???max ?為路徑的最大速度；L為路徑的長(zhǎng)度。
結(jié)合式（18）和式（19），可得
T ??v1 ?≥L|q′ ?n|/q · ???max ，n ??（20）
q′ ?n= ?d q n ?d s = q n（s+ d s）－q n（s） ?d s ??（21）
2.2 關(guān)節(jié)加速度約束
已知關(guān)節(jié)的加速度約束q ?¨ ???max ，n ，通過式（16）得到關(guān)節(jié)加速度需要滿足的范圍：
－q ?¨ ???max ，n ≤q′ ?ns ?¨ +q″ ns · ?2≤q ?¨ ???max ，n ??（22）
進(jìn)一步可以得到
|q′ ?n||s ?¨ |≤|q′ ?n||s ?¨ ???max ?|≤
|q ?¨ ???max ，n －q″ ns · ?2| ??q ?¨ ?n≥0
|q ?¨ ???max ，n +q″ ns · ?2| q ?¨ ?n<0 ??（23）
結(jié)合式（22）、式（23）可以得到T ??v 2 的計(jì)算 公式：
T ??v2 ?≥ ?|q′ ?n||s · ???max ?| |q ?¨ ???max ，n －q″ ns · ?2| ???q ?¨ ?n≥0
|q′ ?n||s · ???max ?| |q ?¨ ???max ，n +q″ ns · ?2| ?q ?¨ ?n<0 ??（24）
文獻(xiàn)［18］計(jì)算的時(shí)間常數(shù)T′ ???v 2 為
T′ ???v2 ?≥ |q′ ?n||s · ???max ?| q ?¨ ???max ，n －|q″ n|s · ?2 ??（25）
對(duì)比式（24）和式（25）可以看出，q ?¨ ?n＜0，q″ ?n<0，q ?¨ ?n≥0，q″ ?n>0時(shí)，T′ ???v 2 =T ??v 2 ；q ?¨ ?n≥0，q″ ?n<0時(shí)，
|q′ ?n||s · ???max ?| |q ?¨ ???max ，n －q″ ?ns · ?2| < |q′ ?n||s · ???max ?| q ?¨ ???max ，n －|q″ n|s · ?2 ??（26）
q ?¨ ?n<0，q″ ?n>0時(shí)
|q′ ?n||s · ???max ?| |q ?¨ ???max ，n +q″ ns · ?2| < |q′ ?n||s · ???max ?| q ?¨ ???max ，n －|q″ ?n|s · ?2 ??（27）
由式（26）、式（27）可以看出T′ ???v 2 >T ??v 2 。因此本文方法計(jì)算的時(shí)間常數(shù)T ??v 2 小于文獻(xiàn)［18］的方法，即在相同的關(guān)節(jié)加速度約束q ?¨ ???max ?下，本文的方法縮短了刀具的運(yùn)動(dòng)時(shí)間。
2.3 關(guān)節(jié)躍度約束
關(guān)節(jié)的躍度需要滿足下式：
－q … ???max ，n ≤q ??ns · ?3+3q″ ?ns · s ?¨ +q′ ?ns … ≤q … ???max ，n ??（28）
同理，可得
|q′ ?n||s … |=|q′ ?n||s … ???max ?|≤
|q … ???max ，n －q ??ns · ?3－3q″ ?ns · s ?¨ | ??q … ≥0
|q … ???max ，n +q ???ns · ?3+3q″ ?ns · s ?¨ | q … <0 ??（29）
結(jié)合式（28）和式（29），得到T ??v 3 ?的計(jì)算公式：
T ??v 3 ≥ ?|q′ ?n||s ?¨ ???max ?| |q … ???max ，n －q ??ns · ?3－3q″ ?ns · s ?¨ | ???q … ?n≥0
|q′ ?n||s ?¨ ???max ?| |q … ???max ，n +q ??ns · ?3+3q″ ?ns · s ?¨ | ?q … ?n<0 ??（30）
式（30）約束了路徑點(diǎn)之間的關(guān)節(jié)躍度。在轉(zhuǎn)角處重疊躍度曲線會(huì)出現(xiàn)關(guān)節(jié)躍度超過約束的情況 ?［17］ 。如圖3所示，圖中，藍(lán)色、 紅色曲線分別代表第一段路徑和第二段路徑的關(guān)節(jié)躍度。圖3 a 中，黑色虛線表示刀具運(yùn)動(dòng)到路徑角點(diǎn)的時(shí)刻，可以看出，重疊前關(guān)節(jié)的躍度在約束內(nèi)。圖3 b 中，黑色虛線分別代表刀具進(jìn)入轉(zhuǎn)角和離開轉(zhuǎn)角的時(shí)刻， 藍(lán)綠色曲線表示路徑轉(zhuǎn)角處的關(guān)節(jié)躍度，可以看出，在路徑轉(zhuǎn)角處重疊躍度曲線會(huì)導(dǎo)致關(guān)節(jié)躍度超過運(yùn)動(dòng)學(xué)約束。為解決這個(gè)問題，將關(guān)節(jié)躍度約束q … ???max ?設(shè)為原始約束的1/2，以使在轉(zhuǎn)角處重疊相鄰路徑的躍度曲線時(shí)能保證躍度不會(huì)超過約束。
3 仿真結(jié)果和實(shí)驗(yàn)結(jié)果
3.1 仿真結(jié)果
在五自由度加工機(jī)器人上對(duì)本文算法進(jìn)行驗(yàn)證。如圖4所示，五自由度加工機(jī)器人由2-UPR-RPU并聯(lián)機(jī)構(gòu)（2條UPR支鏈和1條RPU支鏈組成）和2個(gè)串聯(lián)的移動(dòng)平臺(tái)組成， O ?r X ?r Y ?r Z ?r為機(jī)器人坐標(biāo)系， OXYZ 為工件坐標(biāo)系。
刀尖的速度、加速度和躍度約束分別為 v ??max =50 mm/s， a ??max =500 mm/s 2， j ??max =10 000 mm/s 3。 刀軸的角速度、角加速度和角躍度約束分別為 ω ??max =10°/s， ω ?· ??max =100°/s 2， ω ??¨ ??max =2000 °/s 3。 刀尖、刀軸的光順誤差約束分別設(shè)為 ε ?p ?max =0.02 mm， ε ?o ?max =0.02°，關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束如表1所示。光順后的路徑如圖5所示。
為驗(yàn)證本文提出方法的有效性，將本文方法與文獻(xiàn)［18］的方法進(jìn)行對(duì)比，圖6所示為刀尖的速度曲線和刀軸的角速度曲線，兩種方法的刀尖的最大速度都為48.2 mm/s，刀軸的最大速度都為10°/s，均滿足切向的運(yùn)動(dòng)學(xué)約束。仿真中，關(guān)節(jié)1、2、3、5的躍度均未超過約束。關(guān)節(jié)4的躍度曲線如圖7所示，文獻(xiàn)［18］方法在時(shí)刻A～E超過了躍度約束，而本文方法將關(guān)節(jié)躍度限制在約束范圍內(nèi)，驗(yàn)證了本文方法的有效性。此外，本文方法的加工時(shí)間為13.215 s，文獻(xiàn)［18］的方法為13.169 s，將關(guān)節(jié)4的躍度減小后，本文方法的加工時(shí)間依然與文獻(xiàn)［18］的方法非常接近。
如圖8所示，本文方法的 T ??v2 為2.355 s，文獻(xiàn)［18］方法的 T ??v2 為2.370 s，本文方法計(jì)算的時(shí)間常數(shù)比文獻(xiàn)［18］方法的短0.015 s，證明了式（26）和（27）的有效性。
3.2 實(shí)驗(yàn)結(jié)果
圖9所示為五自由度加工機(jī)器人實(shí)驗(yàn)平臺(tái)，通過上位機(jī)計(jì)算關(guān)節(jié)的運(yùn)動(dòng)數(shù)據(jù)，Dspace1202實(shí)時(shí)控制系統(tǒng)將位置命令發(fā)送給驅(qū)動(dòng)器。每個(gè)關(guān)節(jié)由直線電機(jī)驅(qū)動(dòng)，關(guān)節(jié)的位置數(shù)據(jù)由編碼器獲得，實(shí)驗(yàn)的加工路徑和采用的運(yùn)動(dòng)學(xué)約束與3.1節(jié) 相同。
關(guān)節(jié)4的跟蹤誤差如圖10所示，表2具體給出了時(shí)刻A～E的跟蹤誤差。結(jié)合圖7可知，文 獻(xiàn)［18］方法在這5個(gè)時(shí)刻的關(guān)節(jié)躍度超過了約束，而本文方法的關(guān)節(jié)躍度始終在約束范圍內(nèi)。表2的數(shù)據(jù)說明本文方法在這5個(gè)時(shí)刻的跟蹤誤差小于文獻(xiàn)［18］方法，且E時(shí)刻本文方法的跟蹤誤差減小了14.57%。實(shí)驗(yàn)結(jié)果說明本文方法可將關(guān)節(jié)躍度限制在約束范圍內(nèi)，并減小關(guān)節(jié)的跟蹤誤差，證明了本文方法的有效性。
4 結(jié)論
（1）首先通過關(guān)節(jié)運(yùn)動(dòng)對(duì)刀具運(yùn)動(dòng)的導(dǎo)數(shù)建立了機(jī)器人關(guān)節(jié)躍度和刀具躍度之間的關(guān)系，然后使用關(guān)節(jié)的躍度約束重新調(diào)整了FIR濾波器的時(shí)間常數(shù)，保證機(jī)器人的關(guān)節(jié)躍度都在約束范圍內(nèi)。
（2）考慮關(guān)節(jié)加速度約束時(shí)，本文的時(shí)間常數(shù)小于現(xiàn)有方法，提高了加工效率。實(shí)驗(yàn)結(jié)果表明，在幾乎不降低加工效率的情況下，本文的跟蹤誤差最大減小了14.57%，在保證機(jī)器人加工效率的同時(shí)減小了關(guān)節(jié)的跟蹤誤差。
參考文獻(xiàn) ：
［1］ ?PENG ?J F， HUANG P S， DING Y， et al. An Analytical Method for Decoupled Local Smoothing of Linear Paths in Industrial Robots［J］. Robotics and Computer-integrated Manufacturing， 2021， 72：102193.
［2］ ?YANG ?J X， LI D W， YE C C， et al. An Analytical C3 Continuous Tool Path Corner Smoothing Algorithm for 6R Robot Manipulator［J］. Robotics and Computer-integrated Manufacturing， 2020， 64（64）：101947.
［3］ ?SHEN ?X， XIE F G， LIU X J， et al. A Smooth and Undistorted Toolpath Interpolation Method for 5-DoF Parallel Kinematic Machines［J］. Robotics and Computer-integrated Manufacturing， 2019， 57：347-356.
［4］ ?LIU ?H T， LI G X， XIAO J L. A C3 Continuous Toolpath Corner Smoothing Method for a Hybrid Machining Robot［J］. Journal of Manufacturing Processes， 2022， 75：1072-1088.
［5］ ?LI ?G X， LIU H T， LIU S J， et al. A General C2 Continuous Toolpath Corner Smoothing Method for a 5-DOF Hybrid Robot［J］. Mechanism and Machine Theory， 2022， 169：104640.
［6］ ?畢慶貞. 復(fù)雜曲面零件數(shù)控加工的關(guān)鍵問題——解讀《復(fù)雜曲面零件五軸數(shù)控加工理論與技術(shù)》［J］.中國(guó)機(jī)械工程，2018，29（14）：1758-1763.
BI Qingzhen. Key Issues in CNC Machining of Complex Surface Parts-interpretation of ?Theory and Technology of Five-axis CNC Machining of Complex Surface Parts ［J］. China Mechanical Engineering， 2018， 29（14）：1758-1763.
［7］ ?HUANG ?J， DU X， ZHU L M. Real-time Local Smoothing for Five-axis Linear Toolpath Considering Smoothing Error Constraints［J］. International Journal of Machine Tools and Manufacture， 2018， 124：67-79.
［8］ ?HUANG ?J， LU Y A， ZHU L M. Real-time Feedrate Scheduling for Five-axis Machining by Simultaneously Planning Linear and Angular Trajectories［J］. International Journal of Machine Tools and Manufacture， 2018， 135：78-96.
［9］ ?YUEN ?A， ZHANG K， ALTINTAS Y. Smooth Trajectory Generation for Five-axis Machine Tools［J］. International Journal of Machine Tools and Manufacture， 2013， 71：11-19.
［10］ ?WANG ?W X， HU C X， ZHOU K， et al. Local ?Asymmetrical ?Corner Trajectory Smoothing with Bidirectional Planning and Adjusting Algorithm for CNC Machining［J］. Robotics and Computer- integrated ?Manufacturing， 2021， 68：102058.
［11］ ?TAJIMA ?S， SENCER B. Kinematic Corner Smoothing for High Speed Machine Tools［J］. International Journal of Machine Tools and Manufacture， 2016， 108：27-43.
［12］ ?TAJIMA ?S， SENCER B. Global Tool-path Smoothing for CNC Machine Tools with Uninterrupted Acceleration［J］. International Journal of Machine Tools and Manufacture， 2017， 121：81-95.
［13］ ?XIE ?Z H， XIE F G， LIU X J， et al. Global G3 Continuity Toolpath Smoothing for a 5-DoF Machining Robot with Parallel Kinematics［J］. Robotics and Computer-integrated Manufacturing， 2021， 67：102018.
［14］ ?TAJIMA ?S， SENCER B， SHAMOTO E. Accurate Interpolation of Machining Tool-paths Based on FIR Filtering［J］. Precision Engineering， 2018， 52：332-344.
［15］ ?TAJIMA ?S， SENCER B. Accurate Real-time Interpolation of 5-axis Tool-paths with Local Corner Smoothing［J］. International Journal of Machine Tools and Manufacture， 2019， 142：1-15.
［16］ ?TAJIMA ?S， SENCER B. Online Interpolation of ?5-axis ?Machining Toolpaths with Global Blending［J］. International Journal of Machine Tools and Manufacture， 2022， 175：103862.
［17］ ?LIU ?Y， WAN M， QIN X B， et al. FIR Filter-based Continuous Interpolation of G01 Commands with Bounded Axial and Tangential Kinematics in Industrial Five-axis Machine Tools［J］. International Journal of Mechanical Sciences， 2020， 169：105325.
［18］ ?SUN ?H W， YANG J X， LI D W， et al. An On-line Tool Path Smoothing Algorithm for 6R Robot Manipulator with Geometric and Dynamic Constraints［J］. Science China Technological Sciences， 2021， 64（9）：1907-1919.
［19］ ?楊敏， 趙現(xiàn)朝， 鐘澤杉，等. 復(fù)雜約束下的五軸數(shù)控系統(tǒng)自適應(yīng)速度規(guī)劃［J］. 機(jī)械工程學(xué)報(bào)， 2020， 56（11）：161-171.
YANG Min， ZHAO Xianchao， ZHONG Zeshan， et al. Adaptive Velocity Planning under Complex Constraints for 5-axis CNC Systems［J］. Journal of Mechanical Engineering， 2020， 56（11）：161-171.Symbol`@@
［20］ ?鐘澤杉， 楊敏， 趙現(xiàn)朝，等. 軸空間多約束下的五軸B樣條路徑速度規(guī)劃［J］. 計(jì)算機(jī)集成制造系統(tǒng)， 2021， 27（1）：137-148.
ZHONG Zeshan， YANG Min， ZHAO Xianchao， et al. B-spline Path Velocity Planning Considering Multi-constraints in Axis Space for Five-axis Machine Tool［J］. Computer Integrated Manufacturing Systems， 2021， 27（1）：137-148.
（ 編輯 張 洋 ）
作者簡(jiǎn)介 ：
杜 煦 ，女，1986年生，副教授、碩士研究生導(dǎo)師。研究方向?yàn)閿?shù)控加工技術(shù)與裝備。發(fā)表論文14篇。E-mail：duxu@zstu.edu.cn。
鄭軍強(qiáng) （通信作者），男，1986年生，講師，碩士研究生導(dǎo)師。研究方向?yàn)闄C(jī)器人技術(shù)及其應(yīng)用。發(fā)表論文11篇。E-mail：zhengjunqiang@hdu.edu.cn。
杜煦 常澤鑫 鄭軍強(qiáng) 任鵬飛
摘要 ：針對(duì)機(jī)器人加工過程中關(guān)節(jié)躍度超過約束的問題，提出一種考慮機(jī)器人關(guān)節(jié)躍度約束的刀具路徑光順?biāo)惴ā２捎糜邢逈_激響應(yīng)濾波器插值算法對(duì)五軸刀具路徑進(jìn)行光順處理，一步完成了路徑光順和速度規(guī)劃。建立了關(guān)節(jié)與刀具之間的速度、加速度和躍度關(guān)系，通過調(diào)整時(shí)間常數(shù)保證了關(guān)節(jié)躍度不超過約束?？紤]機(jī)器人關(guān)節(jié)加速度約束后，該算法計(jì)算出的時(shí)間常數(shù)更小。仿真和實(shí)驗(yàn)結(jié)果表明該算法能減小關(guān)節(jié)的跟蹤誤差，提高機(jī)器人的工作穩(wěn)定性。
關(guān)鍵詞 ：關(guān)節(jié)躍度約束；五軸刀具路徑光順；有限沖激響應(yīng)濾波器插值；機(jī)器人加工
中圖分類號(hào) ：TH161；TP242
DOI：10.3969/j.issn.1004-132X.2024.02.012
開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：
A Real-time Tool Path Smoothing Algorithm Considering Joint
Jerk Constraints
DU Xu 1 CHANG Zexin 1 ZHENG Junqiang 2 REN Pengfei 1
1.School of Mechanical Engineering，Zhejiang Sci-Tech University，Hangzhou，310018
2.School of Mechanical Engineering，Hangzhou Dianzi University，Hangzhou，310018
Abstract ： Aiming at the problems of joint jerk exceeding constraints in robot machining， a tool path smoothing algorithm was proposed considering joint jerk constraints. The FIR filter interpolation was used to smooth the five-axis tool paths. Path smoothing and speed planning were achieved in one step. The relationship of velocity， acceleration， and jerk between joint and tool was established， and the joint jerk did not exceed the constraint by adjusting the time constant. Time constant calculated by the proposed method was smaller than considering the acceleration constraints of robot joint. Simulation and experimental results show that the proposed algorithm may reduce the tracking error of the joint and improve the stability of the robot joint in machining.
Key words ： joint jerk constraint； five-axis tool path smoothing； finite impulse response（FIR） filter interpolation； robot machining
0 引言
與數(shù)控機(jī)床相比，機(jī)器人具有柔性高、工作空間大等優(yōu)點(diǎn)，在復(fù)雜零件的加工中得到了越來越廣的應(yīng)用 ?［1-2］ 。在機(jī)器人的加工任務(wù)中，刀具路徑命令通常為離散的線性命令（G01） ?［3］ 。轉(zhuǎn)角處的刀具路徑是切向不連續(xù)的，這導(dǎo)致關(guān)節(jié)的速度和加速度也不連續(xù)，因此刀具在每個(gè)轉(zhuǎn)角往往需要減速到零，這不僅降低了加工效率，還會(huì)引起機(jī)器人關(guān)節(jié)的振動(dòng) ?［4］ 。為解決小線段路徑帶來的問題，通常對(duì)路徑進(jìn)行光順處理，以實(shí)現(xiàn)機(jī)器人關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng) ?［5］ 。
五軸刀具路徑光順一般分為兩步，首先使用參數(shù)曲線分別對(duì)刀尖和刀軸的路徑進(jìn)行光順處理 ?［6-7］ ，再使用數(shù)值方法對(duì)光順后的參數(shù)曲線進(jìn)行同步 ?［8-9］ 。參數(shù)曲線插補(bǔ)計(jì)算復(fù)雜，難以在線計(jì)算 ?［10-12］ ，且參數(shù)曲線與直線路徑的連接處通常是高階不連續(xù)的，導(dǎo)致機(jī)器人關(guān)節(jié)產(chǎn)生振動(dòng)，影響加工質(zhì)量 ?［13］ 。
TAJIMA等 ?［14-16］ 采用有限沖激響應(yīng)（finite impulse response，F(xiàn)IR）濾波器分別與刀尖速度和刀軸速度作卷積運(yùn)算，并通過重疊運(yùn)動(dòng)學(xué)曲線一步實(shí)現(xiàn)了路徑光順和速度規(guī)劃。與傳統(tǒng)方法相比，這種實(shí)時(shí)插補(bǔ)算法減少了計(jì)算量，有利于數(shù)控系統(tǒng)的實(shí)時(shí)插補(bǔ)，但沒有考慮關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束。LIU等 ?［17］ 采用FIR濾波插值算法在工作空間中對(duì)刀尖運(yùn)動(dòng)進(jìn)行插值，在關(guān)節(jié)空間中對(duì)刀軸運(yùn)動(dòng)進(jìn)行插值。此算法雖然可以直接滿足關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束，但不適用于機(jī)器人。SUN等 ?［18］ 將基于FIR濾波的光順?biāo)惴ㄓ糜跈C(jī)器人加工，通過機(jī)器人關(guān)節(jié)位移對(duì)刀具位移的導(dǎo)數(shù)，建立關(guān)節(jié)與刀具之間的速度和加速度的關(guān)系，通過關(guān)節(jié)的速度和加速度約束來調(diào)整FIR濾波器的時(shí)間常數(shù)（保證機(jī)器人關(guān)節(jié)的速度和加速度在約束內(nèi)），并在六自由度機(jī)器人上進(jìn)行了驗(yàn)證。上述方法都沒有考慮關(guān)節(jié)的躍度約束，而機(jī)器人的耦合特性會(huì)導(dǎo)致某些時(shí)刻的關(guān)節(jié)躍度超過約束 ?［19-20］ ，對(duì)機(jī)器人造成沖擊，影響加工質(zhì)量。
一種考慮關(guān)節(jié)躍度約束的實(shí)時(shí)刀具路徑光順?biāo)惴ā?煦 常澤鑫 鄭軍強(qiáng)等
中國(guó)機(jī)械工程 第35卷 第2期 2024年2月
針對(duì)上述問題，本文提出一種考慮關(guān)節(jié)躍度約束的刀具路徑光順?biāo)惴āＪ褂肍IR濾波器分別對(duì)刀尖和刀軸運(yùn)動(dòng)進(jìn)行濾波，得到躍度有限的刀具運(yùn)動(dòng)，再通過調(diào)整FIR濾波的時(shí)間常數(shù)實(shí)現(xiàn)刀尖和刀軸的運(yùn)動(dòng)同步；建立了關(guān)節(jié)約束和切向運(yùn)動(dòng)學(xué)約束之間的關(guān)系，通過調(diào)整FIR濾波器的時(shí)間常數(shù)來滿足關(guān)節(jié)的速度、加速度和躍度約束;在五自由度加工機(jī)器人上進(jìn)行了仿真和實(shí)驗(yàn)。
1 基于FIR濾波的路徑光順
1.1 FIR濾波器原理
FIR 濾波器是一個(gè)有限長(zhǎng)度的單位脈沖響應(yīng)濾波器，在拉普拉斯域中，其傳遞函數(shù)定義為
G n（s）= 1－ exp （sT n） sT n ??（1）
式中，T n為第n（n=1，2，…，N）個(gè) FIR 濾波器的時(shí)間常數(shù)；N為濾波器的個(gè)數(shù)。
求式（1）的拉普拉斯逆變換，可以得到時(shí)域上的脈沖信號(hào)：
g（t）= δ（t）－δ（t－T n） T n ??（2）
δ（t）= 1 ?t≥0 0 ?t<0
式（2）表示的是單位脈沖信號(hào)，任意曲線與 FIR 濾波器作卷積運(yùn)算時(shí)，曲線與X軸圍成的面積不會(huì)發(fā)生改變，即卷積運(yùn)算前后曲線的積分結(jié)果不變。利用該特性，將速度曲線與 FIR 濾波器進(jìn)行卷積運(yùn)算，則卷積前后的刀具位移不會(huì)改變。速度曲線與多個(gè)濾波器卷積運(yùn)算后，便可得到高階連續(xù)的運(yùn)動(dòng)學(xué)曲線。濾波后的速度曲線由濾波前的速度曲線與多個(gè)濾波器卷積運(yùn)算后得到，即
v′（t）=v（t） g 1（t） g 2（t） … g n（t） ?（3）
n=1時(shí)，生成的是梯形速度曲線，加速度不連續(xù)，會(huì)導(dǎo)致加工產(chǎn)生沖擊。n=2時(shí)，生成的是 S 形速度曲線，加速度連續(xù)且速度曲線較平滑，廣泛用于機(jī)床和機(jī)器人的加工系統(tǒng)。 本文采用2個(gè)濾波器與速度曲線進(jìn)行卷積，運(yùn)動(dòng)學(xué)曲線如圖1所示，圖中，T ??v 1 為卷積前的運(yùn)動(dòng)時(shí)間，T ??v 2 、T ??v 3 分別為第一個(gè)和第二個(gè)濾波器的時(shí)間常數(shù)。
1.2 切向約束下的時(shí)間常數(shù)計(jì)算
加工中，工藝決定了刀具的最大速度、最大加速度和最大躍度即刀具的切向運(yùn)動(dòng)學(xué)約束。 FIR 濾波器的時(shí)間常數(shù)決定了濾波后刀具的速度、加速度和躍度的大小，因此需要計(jì)算出滿足刀具切向運(yùn)動(dòng)學(xué)約束的時(shí)間常數(shù)。以相鄰的刀具路徑為例來說明切向約束下計(jì)算時(shí)間常數(shù)的過程。首先計(jì)算刀尖位移和刀具角位移：
L n=| P ??n+1 － P ?n|
θ n= arccos | O ?n· O ??n+1 | ??（4）
式中，L n為第n段刀尖路徑的長(zhǎng)度； P ?n、 P ??n+1 分別是刀尖路徑上第n個(gè)和第n+1個(gè)路徑點(diǎn)的坐標(biāo)；θ n為第n段刀軸路徑的角度； O ?n、 O ??n+1 分別為刀軸路徑上第n個(gè)和第 n+1 個(gè)路徑點(diǎn)的坐標(biāo)。
以刀尖運(yùn)動(dòng)為例，根據(jù)切向約束計(jì)算出 FIR 濾波器時(shí)間常數(shù)的范圍：
T ??v1 ?≥L n/v ??max
T ??v2 ?≥v ??max ?/a ??max
T ??v3 ?≥a ??max ?/j ??max ????（5）
式中，v ??max ?、a ??max ?、j ??max ?分別為切向的速度、加速度和躍度 約束。
為生成時(shí)間最優(yōu)的運(yùn)動(dòng)學(xué)軌跡，式（5）中各式取等號(hào)。
1.3 時(shí)間同步
計(jì)算出時(shí)間常數(shù)后，還需要對(duì)刀尖和刀軸的運(yùn)動(dòng)進(jìn)行時(shí)間同步。刀尖的運(yùn)動(dòng)時(shí)間T ?v 、刀軸的運(yùn)動(dòng)時(shí)間T ?ω 分別為
T ?v =∑ 3 n=1 T ??v n ??（6）
T ?ω =∑ 3 n=1 T ??ω n ??（7）
如果T ?ω >T ?v ，則調(diào)整刀尖運(yùn)動(dòng)的時(shí)間常數(shù)T ??v 1 ，同步刀軸運(yùn)動(dòng)。T ??v 1 按下式調(diào)整計(jì)算：
T′ ???v 1 =T ?ω －T ?v +T ??v 1 ??（8）
調(diào)整后，T ??v 1 增大到T′ ???v 1 ，T ??v 2 、T ??v 3 不變，因此最大速度v ??max ?、加速度a ??max ?和躍度j ??max ??會(huì)減小，這意味著同步后的刀尖運(yùn)動(dòng)依然滿足切向運(yùn)動(dòng)學(xué)約束。如果T ?ω T′ ???ω 1 =T ?v －T ?ω +T ??ω 1 ??（9） 如果T ?ω =T ?v ，則刀尖運(yùn)動(dòng)的時(shí)間常數(shù)和刀軸運(yùn)動(dòng)的時(shí)間常數(shù)不需要調(diào)整。 1.4 光順誤差控制 1.2節(jié)和1.3節(jié)計(jì)算了路徑點(diǎn)之間的刀具運(yùn)動(dòng)，生成的刀具速度在路徑轉(zhuǎn)角處為零，這種運(yùn)動(dòng)造成效率損失。通常的處理方法是在轉(zhuǎn)角處重疊相鄰路徑的速度曲線，以避免轉(zhuǎn)角處的刀具速度降為零，并生成光滑的運(yùn)動(dòng)軌跡。首先利用下式將刀尖速度分解為X、Y、Z方向的速度： v x（t） v y（t） v z（t） ?=v（t） ?l x l y l z ?=v（t） ?P ??n+1 － P ?n | P ??n+1 － P ?n| ??（10） 利用球面線性插值，將刀軸角速度分解為i、j、k方向的角速度 ?［15］ ： v i v j v k = ω（t）（ O ??n+1 ?cos ?θ（t）－ O ?n cos （θ n－θ（t））） ?sin ?θ n θ n= arccos （ O ?n· O ??n+1 ） ?（11） 通過轉(zhuǎn)角時(shí)，分別重疊X、Y、Z、i、j、k方向的速度曲線。重疊速度曲線前，刀具沿線性路徑運(yùn)動(dòng)。重疊速度曲線后，刀具會(huì)在轉(zhuǎn)角處偏離原始的線性路徑，產(chǎn)生幾何偏差。因此需要選擇合適的重疊時(shí)間T ??vc ?以滿足刀尖和刀軸的光順誤差 約束。 以刀尖運(yùn)動(dòng)為例，如圖2所示，當(dāng)相鄰路徑的速度v n=v ?n+1 時(shí)，轉(zhuǎn)角輪廓關(guān)于轉(zhuǎn)角平分線對(duì)稱，最大的光順誤差出現(xiàn)在輪廓的中點(diǎn) P ?m。刀尖光順誤差ε ??TCP ?的計(jì)算公式為 ε ??TCP ?=| P ?m－ P ?n|=|s 2 l ?2－s 1 l ?1|= T 3 ??vc ??48T ??v2 ?T ??v3 ??V ???????0≤T ??vc ?≤2T ??v3 4T 2 ??v3 ?－6T ??v3 ?T ??vc ?+3T 2 ??vc ??24T ??v2 ??V 2T ??v3 ?＜T ??vc ?≤T ??v2 ?+T ??v3 ????（12） V= v 2 n+v 2 ?n+1 －v nv ?n+1 ?cos ?θ ??TCP θ ??TCP ?= arccos （ l ?1· l ?2） 式中， l ?1、 l ?2分別為 P ??n-1 ?P ?n和 P ?n P ??n+1 的方向向量；s 1為從 P ?m減速到 P ?n的距離；s 2為從 P ?n加速到 P ?m的距離；θ ??TCP ?為 l ?1和 l ?2的夾角。 由式（12）可以看出，隨著重疊時(shí)間T ??vc ?的增加，刀尖光順誤差逐漸增大。為滿足刀尖光順誤差約束ε ?p ???max ?，根據(jù)式（12）得到轉(zhuǎn)角重疊時(shí)間T ??vc ?的計(jì)算公式： T ??vc ?= ??3 ?48T ??v2 ?T ??v3 ?ε ?p ???max ??V ???????0≤T ??vc ?≤2T ??v3 T ??v3 ?+ 3 ?8T ??v2 ?ε ?p ???max ??V － T 2 ??v3 ??3 ??2T ??v3 ?＜T ??vc ?≤T ??v2 ?+T ??v3 ????（13） 并選擇滿足對(duì)應(yīng)公式范圍的值。如果v n≠v ?n+1 ，則轉(zhuǎn)角輪廓不再關(guān)于角平分線對(duì)稱，在這種情況下，式（12）計(jì)算的輪廓中點(diǎn)誤差將大于最大的光順誤差，因此通過式（13）計(jì)算出的重疊時(shí)間T ??vc ?依然可以使刀尖光順誤差滿約束ε ?p ???max ?。 同理，可以近似計(jì)算出刀軸的重疊運(yùn)動(dòng)時(shí)間T ??ωc ?。最后選擇T ??vc ?和T ??ωc ?中的較小值作為刀尖和刀軸運(yùn)動(dòng)的重疊時(shí)間，保證光順誤差在約束范圍內(nèi)，則重疊時(shí)間T ?c 為 T ?c = min （T ??vc ?，T ??ωc ?） ?（14） 2 關(guān)節(jié)約束下的時(shí)間常數(shù)調(diào)整 機(jī)器人關(guān)節(jié)和刀具之間存在復(fù)雜的運(yùn)動(dòng)關(guān)系，在滿足切向約束的情況下，關(guān)節(jié)的速度、加速度和躍度有可能超過關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束。 建立關(guān)節(jié)運(yùn)動(dòng)和刀尖運(yùn)動(dòng)的關(guān)系： q · = ?d q ?d t = ?d q ?d s ??d s ?d t =q′s · ??（15） q ?¨ =q″s · ?2+q′s ?¨ ??（16） q … =q s · ?3+3q″s · s ?¨ +q′s … ??（17） 式中，q · 、q ?¨ 、q … 分別為關(guān)節(jié)的速度、加速度和躍度；q′、q″、q 分別為關(guān)節(jié)位移關(guān)于刀尖位移s的一階、二階和三階導(dǎo)數(shù)；s · 、s ?¨ 和s … 分別為刀尖的速度、加速度和躍度。 2.1 關(guān)節(jié)速度約束 關(guān)節(jié)的速度需要滿足： |q · ?n|=|q′ ?n|s · ≤|q′ ?n|s · ???max ?≤q · ???max ，n ??（18） n=1，2，…，5 s · ???max ?=L/T ??v1 ???（19） 式中，q′ ?n為第n個(gè)關(guān)節(jié)的位移關(guān)于刀尖位移的一階導(dǎo)數(shù)； q · ???max ，n 為第n個(gè)關(guān)節(jié)的速度約束；s · ???max ?為路徑的最大速度；L為路徑的長(zhǎng)度。 結(jié)合式（18）和式（19），可得 T ??v1 ?≥L|q′ ?n|/q · ???max ，n ??（20） q′ ?n= ?d q n ?d s = q n（s+ d s）－q n（s） ?d s ??（21） 2.2 關(guān)節(jié)加速度約束 已知關(guān)節(jié)的加速度約束q ?¨ ???max ，n ，通過式（16）得到關(guān)節(jié)加速度需要滿足的范圍： －q ?¨ ???max ，n ≤q′ ?ns ?¨ +q″ ns · ?2≤q ?¨ ???max ，n ??（22） 進(jìn)一步可以得到 |q′ ?n||s ?¨ |≤|q′ ?n||s ?¨ ???max ?|≤ |q ?¨ ???max ，n －q″ ns · ?2| ??q ?¨ ?n≥0 |q ?¨ ???max ，n +q″ ns · ?2| q ?¨ ?n<0 ??（23） 結(jié)合式（22）、式（23）可以得到T ??v 2 的計(jì)算 公式： T ??v2 ?≥ ?|q′ ?n||s · ???max ?| |q ?¨ ???max ，n －q″ ns · ?2| ???q ?¨ ?n≥0 |q′ ?n||s · ???max ?| |q ?¨ ???max ，n +q″ ns · ?2| ?q ?¨ ?n<0 ??（24） 文獻(xiàn)［18］計(jì)算的時(shí)間常數(shù)T′ ???v 2 為 T′ ???v2 ?≥ |q′ ?n||s · ???max ?| q ?¨ ???max ，n －|q″ n|s · ?2 ??（25） 對(duì)比式（24）和式（25）可以看出，q ?¨ ?n＜0，q″ ?n<0，q ?¨ ?n≥0，q″ ?n>0時(shí)，T′ ???v 2 =T ??v 2 ；q ?¨ ?n≥0，q″ ?n<0時(shí)， |q′ ?n||s · ???max ?| |q ?¨ ???max ，n －q″ ?ns · ?2| < |q′ ?n||s · ???max ?| q ?¨ ???max ，n －|q″ n|s · ?2 ??（26） q ?¨ ?n<0，q″ ?n>0時(shí) |q′ ?n||s · ???max ?| |q ?¨ ???max ，n +q″ ns · ?2| < |q′ ?n||s · ???max ?| q ?¨ ???max ，n －|q″ ?n|s · ?2 ??（27） 由式（26）、式（27）可以看出T′ ???v 2 >T ??v 2 。因此本文方法計(jì)算的時(shí)間常數(shù)T ??v 2 小于文獻(xiàn)［18］的方法，即在相同的關(guān)節(jié)加速度約束q ?¨ ???max ?下，本文的方法縮短了刀具的運(yùn)動(dòng)時(shí)間。 2.3 關(guān)節(jié)躍度約束 關(guān)節(jié)的躍度需要滿足下式： －q … ???max ，n ≤q ??ns · ?3+3q″ ?ns · s ?¨ +q′ ?ns … ≤q … ???max ，n ??（28） 同理，可得 |q′ ?n||s … |=|q′ ?n||s … ???max ?|≤ |q … ???max ，n －q ??ns · ?3－3q″ ?ns · s ?¨ | ??q … ≥0 |q … ???max ，n +q ???ns · ?3+3q″ ?ns · s ?¨ | q … <0 ??（29） 結(jié)合式（28）和式（29），得到T ??v 3 ?的計(jì)算公式： T ??v 3 ≥ ?|q′ ?n||s ?¨ ???max ?| |q … ???max ，n －q ??ns · ?3－3q″ ?ns · s ?¨ | ???q … ?n≥0 |q′ ?n||s ?¨ ???max ?| |q … ???max ，n +q ??ns · ?3+3q″ ?ns · s ?¨ | ?q … ?n<0 ??（30） 式（30）約束了路徑點(diǎn)之間的關(guān)節(jié)躍度。在轉(zhuǎn)角處重疊躍度曲線會(huì)出現(xiàn)關(guān)節(jié)躍度超過約束的情況 ?［17］ 。如圖3所示，圖中，藍(lán)色、 紅色曲線分別代表第一段路徑和第二段路徑的關(guān)節(jié)躍度。圖3 a 中，黑色虛線表示刀具運(yùn)動(dòng)到路徑角點(diǎn)的時(shí)刻，可以看出，重疊前關(guān)節(jié)的躍度在約束內(nèi)。圖3 b 中，黑色虛線分別代表刀具進(jìn)入轉(zhuǎn)角和離開轉(zhuǎn)角的時(shí)刻， 藍(lán)綠色曲線表示路徑轉(zhuǎn)角處的關(guān)節(jié)躍度，可以看出，在路徑轉(zhuǎn)角處重疊躍度曲線會(huì)導(dǎo)致關(guān)節(jié)躍度超過運(yùn)動(dòng)學(xué)約束。為解決這個(gè)問題，將關(guān)節(jié)躍度約束q … ???max ?設(shè)為原始約束的1/2，以使在轉(zhuǎn)角處重疊相鄰路徑的躍度曲線時(shí)能保證躍度不會(huì)超過約束。 3 仿真結(jié)果和實(shí)驗(yàn)結(jié)果 3.1 仿真結(jié)果 在五自由度加工機(jī)器人上對(duì)本文算法進(jìn)行驗(yàn)證。如圖4所示，五自由度加工機(jī)器人由2-UPR-RPU并聯(lián)機(jī)構(gòu)（2條UPR支鏈和1條RPU支鏈組成）和2個(gè)串聯(lián)的移動(dòng)平臺(tái)組成， O ?r X ?r Y ?r Z ?r為機(jī)器人坐標(biāo)系， OXYZ 為工件坐標(biāo)系。 刀尖的速度、加速度和躍度約束分別為 v ??max =50 mm/s， a ??max =500 mm/s 2， j ??max =10 000 mm/s 3。 刀軸的角速度、角加速度和角躍度約束分別為 ω ??max =10°/s， ω ?· ??max =100°/s 2， ω ??¨ ??max =2000 °/s 3。 刀尖、刀軸的光順誤差約束分別設(shè)為 ε ?p ?max =0.02 mm， ε ?o ?max =0.02°，關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束如表1所示。光順后的路徑如圖5所示。 為驗(yàn)證本文提出方法的有效性，將本文方法與文獻(xiàn)［18］的方法進(jìn)行對(duì)比，圖6所示為刀尖的速度曲線和刀軸的角速度曲線，兩種方法的刀尖的最大速度都為48.2 mm/s，刀軸的最大速度都為10°/s，均滿足切向的運(yùn)動(dòng)學(xué)約束。仿真中，關(guān)節(jié)1、2、3、5的躍度均未超過約束。關(guān)節(jié)4的躍度曲線如圖7所示，文獻(xiàn)［18］方法在時(shí)刻A～E超過了躍度約束，而本文方法將關(guān)節(jié)躍度限制在約束范圍內(nèi)，驗(yàn)證了本文方法的有效性。此外，本文方法的加工時(shí)間為13.215 s，文獻(xiàn)［18］的方法為13.169 s，將關(guān)節(jié)4的躍度減小后，本文方法的加工時(shí)間依然與文獻(xiàn)［18］的方法非常接近。 如圖8所示，本文方法的 T ??v2 為2.355 s，文獻(xiàn)［18］方法的 T ??v2 為2.370 s，本文方法計(jì)算的時(shí)間常數(shù)比文獻(xiàn)［18］方法的短0.015 s，證明了式（26）和（27）的有效性。 3.2 實(shí)驗(yàn)結(jié)果 圖9所示為五自由度加工機(jī)器人實(shí)驗(yàn)平臺(tái)，通過上位機(jī)計(jì)算關(guān)節(jié)的運(yùn)動(dòng)數(shù)據(jù)，Dspace1202實(shí)時(shí)控制系統(tǒng)將位置命令發(fā)送給驅(qū)動(dòng)器。每個(gè)關(guān)節(jié)由直線電機(jī)驅(qū)動(dòng)，關(guān)節(jié)的位置數(shù)據(jù)由編碼器獲得，實(shí)驗(yàn)的加工路徑和采用的運(yùn)動(dòng)學(xué)約束與3.1節(jié) 相同。 關(guān)節(jié)4的跟蹤誤差如圖10所示，表2具體給出了時(shí)刻A～E的跟蹤誤差。結(jié)合圖7可知，文 獻(xiàn)［18］方法在這5個(gè)時(shí)刻的關(guān)節(jié)躍度超過了約束，而本文方法的關(guān)節(jié)躍度始終在約束范圍內(nèi)。表2的數(shù)據(jù)說明本文方法在這5個(gè)時(shí)刻的跟蹤誤差小于文獻(xiàn)［18］方法，且E時(shí)刻本文方法的跟蹤誤差減小了14.57%。實(shí)驗(yàn)結(jié)果說明本文方法可將關(guān)節(jié)躍度限制在約束范圍內(nèi)，并減小關(guān)節(jié)的跟蹤誤差，證明了本文方法的有效性。 4 結(jié)論 （1）首先通過關(guān)節(jié)運(yùn)動(dòng)對(duì)刀具運(yùn)動(dòng)的導(dǎo)數(shù)建立了機(jī)器人關(guān)節(jié)躍度和刀具躍度之間的關(guān)系，然后使用關(guān)節(jié)的躍度約束重新調(diào)整了FIR濾波器的時(shí)間常數(shù)，保證機(jī)器人的關(guān)節(jié)躍度都在約束范圍內(nèi)。 （2）考慮關(guān)節(jié)加速度約束時(shí)，本文的時(shí)間常數(shù)小于現(xiàn)有方法，提高了加工效率。實(shí)驗(yàn)結(jié)果表明，在幾乎不降低加工效率的情況下，本文的跟蹤誤差最大減小了14.57%，在保證機(jī)器人加工效率的同時(shí)減小了關(guān)節(jié)的跟蹤誤差。 參考文獻(xiàn) ： ［1］ ?PENG ?J F， HUANG P S， DING Y， et al. An Analytical Method for Decoupled Local Smoothing of Linear Paths in Industrial Robots［J］. Robotics and Computer-integrated Manufacturing， 2021， 72：102193. ［2］ ?YANG ?J X， LI D W， YE C C， et al. An Analytical C3 Continuous Tool Path Corner Smoothing Algorithm for 6R Robot Manipulator［J］. Robotics and Computer-integrated Manufacturing， 2020， 64（64）：101947. ［3］ ?SHEN ?X， XIE F G， LIU X J， et al. A Smooth and Undistorted Toolpath Interpolation Method for 5-DoF Parallel Kinematic Machines［J］. Robotics and Computer-integrated Manufacturing， 2019， 57：347-356. ［4］ ?LIU ?H T， LI G X， XIAO J L. A C3 Continuous Toolpath Corner Smoothing Method for a Hybrid Machining Robot［J］. Journal of Manufacturing Processes， 2022， 75：1072-1088. ［5］ ?LI ?G X， LIU H T， LIU S J， et al. A General C2 Continuous Toolpath Corner Smoothing Method for a 5-DOF Hybrid Robot［J］. Mechanism and Machine Theory， 2022， 169：104640. ［6］ ?畢慶貞. 復(fù)雜曲面零件數(shù)控加工的關(guān)鍵問題——解讀《復(fù)雜曲面零件五軸數(shù)控加工理論與技術(shù)》［J］.中國(guó)機(jī)械工程，2018，29（14）：1758-1763. BI Qingzhen. Key Issues in CNC Machining of Complex Surface Parts-interpretation of ?Theory and Technology of Five-axis CNC Machining of Complex Surface Parts ［J］. China Mechanical Engineering， 2018， 29（14）：1758-1763. ［7］ ?HUANG ?J， DU X， ZHU L M. Real-time Local Smoothing for Five-axis Linear Toolpath Considering Smoothing Error Constraints［J］. International Journal of Machine Tools and Manufacture， 2018， 124：67-79. ［8］ ?HUANG ?J， LU Y A， ZHU L M. Real-time Feedrate Scheduling for Five-axis Machining by Simultaneously Planning Linear and Angular Trajectories［J］. International Journal of Machine Tools and Manufacture， 2018， 135：78-96. ［9］ ?YUEN ?A， ZHANG K， ALTINTAS Y. Smooth Trajectory Generation for Five-axis Machine Tools［J］. International Journal of Machine Tools and Manufacture， 2013， 71：11-19. ［10］ ?WANG ?W X， HU C X， ZHOU K， et al. Local ?Asymmetrical ?Corner Trajectory Smoothing with Bidirectional Planning and Adjusting Algorithm for CNC Machining［J］. Robotics and Computer- integrated ?Manufacturing， 2021， 68：102058. ［11］ ?TAJIMA ?S， SENCER B. Kinematic Corner Smoothing for High Speed Machine Tools［J］. International Journal of Machine Tools and Manufacture， 2016， 108：27-43. ［12］ ?TAJIMA ?S， SENCER B. Global Tool-path Smoothing for CNC Machine Tools with Uninterrupted Acceleration［J］. International Journal of Machine Tools and Manufacture， 2017， 121：81-95. ［13］ ?XIE ?Z H， XIE F G， LIU X J， et al. Global G3 Continuity Toolpath Smoothing for a 5-DoF Machining Robot with Parallel Kinematics［J］. Robotics and Computer-integrated Manufacturing， 2021， 67：102018. ［14］ ?TAJIMA ?S， SENCER B， SHAMOTO E. Accurate Interpolation of Machining Tool-paths Based on FIR Filtering［J］. Precision Engineering， 2018， 52：332-344. ［15］ ?TAJIMA ?S， SENCER B. Accurate Real-time Interpolation of 5-axis Tool-paths with Local Corner Smoothing［J］. International Journal of Machine Tools and Manufacture， 2019， 142：1-15. ［16］ ?TAJIMA ?S， SENCER B. Online Interpolation of ?5-axis ?Machining Toolpaths with Global Blending［J］. International Journal of Machine Tools and Manufacture， 2022， 175：103862. ［17］ ?LIU ?Y， WAN M， QIN X B， et al. FIR Filter-based Continuous Interpolation of G01 Commands with Bounded Axial and Tangential Kinematics in Industrial Five-axis Machine Tools［J］. International Journal of Mechanical Sciences， 2020， 169：105325. ［18］ ?SUN ?H W， YANG J X， LI D W， et al. An On-line Tool Path Smoothing Algorithm for 6R Robot Manipulator with Geometric and Dynamic Constraints［J］. Science China Technological Sciences， 2021， 64（9）：1907-1919. ［19］ ?楊敏， 趙現(xiàn)朝， 鐘澤杉，等. 復(fù)雜約束下的五軸數(shù)控系統(tǒng)自適應(yīng)速度規(guī)劃［J］. 機(jī)械工程學(xué)報(bào)， 2020， 56（11）：161-171. YANG Min， ZHAO Xianchao， ZHONG Zeshan， et al. Adaptive Velocity Planning under Complex Constraints for 5-axis CNC Systems［J］. Journal of Mechanical Engineering， 2020， 56（11）：161-171.Symbol`@@ ［20］ ?鐘澤杉， 楊敏， 趙現(xiàn)朝，等. 軸空間多約束下的五軸B樣條路徑速度規(guī)劃［J］. 計(jì)算機(jī)集成制造系統(tǒng)， 2021， 27（1）：137-148. ZHONG Zeshan， YANG Min， ZHAO Xianchao， et al. B-spline Path Velocity Planning Considering Multi-constraints in Axis Space for Five-axis Machine Tool［J］. Computer Integrated Manufacturing Systems， 2021， 27（1）：137-148. （ 編輯 張 洋 ） 作者簡(jiǎn)介 ： 杜 煦 ，女，1986年生，副教授、碩士研究生導(dǎo)師。研究方向?yàn)閿?shù)控加工技術(shù)與裝備。發(fā)表論文14篇。E-mail：duxu@zstu.edu.cn。 鄭軍強(qiáng) （通信作者），男，1986年生，講師，碩士研究生導(dǎo)師。研究方向?yàn)闄C(jī)器人技術(shù)及其應(yīng)用。發(fā)表論文11篇。E-mail：zhengjunqiang@hdu.edu.cn。
