游 瑋 詹慶榮 方 明，

（1.埃夫特智能裝備股份有限公司，安徽 蕪湖 241007；2.安徽工程大學(xué)，安徽 蕪湖 241000）

0 引言

機器人在完成軌跡規(guī)劃后開始運動，在運動過程中有時需要動態(tài)調(diào)整機器人的運動速度，甚至還會臨時停止或啟動機器人，使機器人的運行速度出現(xiàn)過渡不平滑的現(xiàn)象[1]。此時，機器人不僅需要盡快響應(yīng)用戶的指令，而且還需要確保機器人在運動過程中始終沿著編程路徑運動，且各軸的加速度以及加加速度不能超出其允許的極限值。如果單獨設(shè)計各個軸（例如關(guān)節(jié)軸或坐標(biāo)軸）的調(diào)速方法，那么不僅計算復(fù)雜，而且很難保證合成之后仍然沿著編程路徑運行[2]。針對上述問題，常見的解決方法是笛卡爾空間軌跡規(guī)化和關(guān)節(jié)空間軌跡規(guī)劃[3]。許多學(xué)者通過優(yōu)化軌跡的拐點使軌跡過渡得更加圓滑，從而達到減少機器人頻繁啟停的目的，但會降低軌跡的跟隨精度。該文通過引入倍率控制，在機器人動態(tài)調(diào)整運動速度的過程中，對倍率值變化速度進行規(guī)劃。通過校驗倍率值變化的速度和加速度，實時修改倍率值的上限值、下限值，從而對帶有軌跡混成的運動軌跡進行規(guī)劃。再對1段典型混合軌跡進行仿真實驗，結(jié)果表明，該算法不僅可以提高機器人的路徑跟隨性能，而且還可以實現(xiàn)停止、啟動、調(diào)速以及反向等功能。

1 倍率控制簡介

機器人的倍率值（DYN）是指機器人實際運行速度與最大運行速度的百分比，當(dāng)倍率為100%時，機器人將全速運行；當(dāng)倍率為50%時，機器人應(yīng)按照最大速度的1/2 來運行。為了使機器人在動態(tài)調(diào)整速度的過程中還能保證機器人可以沿著規(guī)劃好的路徑運動，需要將這種多維空間的動態(tài)調(diào)速問題轉(zhuǎn)變?yōu)橐痪S空間的動態(tài)調(diào)速問題，這樣既可以簡化計算，又可以保證調(diào)整過程中路徑的跟隨性。

路徑的長度s是一維空間變量，對s的軌跡曲線進行調(diào)整可以保證路徑的跟隨性。但是這種調(diào)速方式也有3 個缺點：1） 需要確定s的速度、加速度以及加加速度限制條件。因為機器人不僅要滿足該限制條件，而且還要滿足關(guān)節(jié)端的限制條件，所以需要在調(diào)整過程中不停地校核關(guān)節(jié)端的約束條件，計算過程較為復(fù)雜。而且基于該方式進行調(diào)整，如果對s的速度等限制條件設(shè)置得過于保守，則調(diào)速過程太慢；如果設(shè)置得過于激進，則可能會出現(xiàn)關(guān)節(jié)端超出限制條件的情況，需要不斷對其進行修正。2） 在采用該方式的情況下，無法確定調(diào)速結(jié)束時s的位置。3） 該方式在調(diào)速臨近結(jié)束時的銜接計算較復(fù)雜。因為當(dāng)前時刻與下一時刻s的速度、加速度等一般均不為0，所以為了保證速度和加速度的連續(xù)性，至少需要構(gòu)造1 個四次曲線。

由于基于路徑長度s的調(diào)速存在上述缺點，因此，該文通過推導(dǎo)與測試，提出了基于倍率r的調(diào)速方法，整個算法的框架如圖1 所示，這種方式不僅能夠使機器人在不同速度下恒速運動，而且還能夠?qū)崿F(xiàn)停止、啟動、調(diào)速以及反向等功能。

圖1 軌跡生成

2 倍率軌跡生成算法

2.1 時間重新參數(shù)化

無論是關(guān)節(jié)空間的運動還是笛卡爾空間內(nèi)的運動，在完成機器人軌跡規(guī)劃后均可以得到各關(guān)節(jié)位置隨時間t的變化曲線。假設(shè)當(dāng)前機器人某個軸倍率值為r（r∈[0，1]），則相當(dāng)于這個時刻對位置變化方程中的t進行了參數(shù)化t'=rt，新的位置變化方程為θ（t'）=θ（rt）。對其求取前三階導(dǎo)數(shù)可得公式（1）～公式（3）。

式中：v'為速度函數(shù)參數(shù)化；a'為加速度函數(shù)參數(shù)化；為倍率函數(shù)一階導(dǎo)數(shù)；j'為加加速度函數(shù)參數(shù)化；為倍率函數(shù)二階導(dǎo)數(shù)；j為加加速度；θ（t'）為參數(shù)化后的位置變化方程；θ（rt）為對位置變化方程中的進行了參數(shù)化。

在實際運行過程中，倍率值一般會保持在某一定值，這個時刻新生成的軌跡相當(dāng)于在時長上對全速時的軌跡進行等比放縮。當(dāng)需要根據(jù)機器人關(guān)節(jié)運動速度變化實時調(diào)整倍率值時，新得到的軌跡與全速運行時的軌跡不再是等比例縮放的關(guān)系。

假設(shè)倍率的變化曲線是1 條關(guān)于時間的二次曲線，曲線方程如公式（4）所示。

式中：a、b和c為未知參數(shù)；t為時間。

為了便于分析，可以對公式（4）進行變形，使當(dāng)前時刻所對應(yīng)的t為0 s，從而得到公式（5）。

式中：r0為當(dāng)前時刻所對應(yīng)的倍率值分別為當(dāng)前時刻倍率曲線的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)。

假設(shè)當(dāng)前時刻的位置對應(yīng)全速軌跡中t0時刻的位置，則Ts時間后所對應(yīng)的全速軌跡的時刻t1如公式（6）所示。

Ts時間后所對應(yīng)的倍率及其一階導(dǎo)數(shù)、二階導(dǎo)數(shù)如公式（7）～公式（9）所示。

2.2 倍率速度規(guī)劃

為了減小沖擊，規(guī)劃出的各軸運動曲線方程應(yīng)該滿足三階連續(xù)性（即加速度連續(xù)）的要求。在使用倍率曲線對時間進行重新參數(shù)化后，根據(jù)復(fù)合函數(shù)的連續(xù)性準(zhǔn)則可知：如果要保證新生成的各軸運動曲線的三階連續(xù)性，那么時間參數(shù)化曲線也應(yīng)該滿足三階連續(xù)性的要求，即應(yīng)該保證倍率曲線（對時間參數(shù)化曲線求取一階導(dǎo)數(shù)）的二階連續(xù)性（速度連續(xù)）。

綜上所述，倍率曲線只需要速度連續(xù)即可，加速度可以跳變。因此，可以按照梯形速度曲線對倍率曲線進行規(guī)劃。梯形速度規(guī)劃的關(guān)鍵參數(shù)為最大速度vimax以及允許的加速度和減速度aimax，而這些參數(shù)可以根據(jù)當(dāng)前時刻各軸允許的最大速度和最大加速度jimax來確定。其限定條件如公式（10）～公式（12）所示。

式中：vi為i時刻的速度；為i時刻速度的一階導(dǎo)數(shù)；j為關(guān)節(jié)加速度；j'為關(guān)節(jié)加速度一階導(dǎo)數(shù)。

假設(shè)目標(biāo)倍率為rt，當(dāng)前時刻的倍率、速度和加速度分別為rs、和，利用關(guān)節(jié)加加速度限制條件推導(dǎo)出當(dāng)前時刻所允許的二階導(dǎo)數(shù)的上限、下限分別為，下一時刻的倍率、速度和加速度分別為re、，利用關(guān)節(jié)加速度以及加加速度限制條件得到下一時刻所允許的速度和加速度的上限、下限分別為以及。然而下一時刻倍率的速度及其加速度限制條件還不夠，還需要考慮以下2 點：1） 在該倍率速度下，如果按照最大減速度（或加速度）減速（或加速）至0 °/s2時，停止點的倍率是否會超出目標(biāo)倍率值。2） 在倍率十分接近目標(biāo)倍率的情況下，倍率的速度是否也接近于0。

針對第一點，可以根據(jù)梯形速度規(guī)劃形式修正其速度的上限或下限，如公式（13）～公式（14）所示。

針對第二點，根據(jù)該時刻速度與目標(biāo)速度的接近程度確定這一時刻各關(guān)節(jié)的加速度限制條件。假設(shè)當(dāng)前倍率下某一關(guān)節(jié)的速度為vc、加速度為ac，那么在目標(biāo)倍率下，該關(guān)節(jié)的速度為vt、加速度為at且調(diào)整時間為t，允許的最大加加速度為jmax，當(dāng)前的加加速度為j。根據(jù)該約束準(zhǔn)則可以得到公式（15）～公式（16）。

求解公式（15）～公式（16）可得公式（17）。

在公式（17）中，需要對ac和j的正負進行分析，分析不同情形下確定ac和j的正負號的準(zhǔn)則。

當(dāng)vt＞vc＞0 且at＞0 時，當(dāng)前的加速度ac可以大于at，加加速度應(yīng)為-jmax，此時有公式（18）。

式中：tcmax為加速段最大時長。

反之，則有公式（19）。

當(dāng)vt＞vc＞0 且at≤0 時，如果當(dāng)前的加速度ac＜0，則將加速度調(diào)整至目標(biāo)加速度，速度不僅小于vt，而且還會小于vc。因此，此時的ac必須大于0，即ac的下限為0。ac的上限可以根據(jù)公式（20）來求解。

同理可以得出，當(dāng)vt＜vc＜0 時加速度的上下限。對于倍率減小的情況，也可以按上述分析過程來確定當(dāng)前加速度的上限、下限。假設(shè)該關(guān)節(jié)在當(dāng)前位置下全速運行時的速度和加速度分別為v和a，則公式（21）～公式（23）成立。

式中：rc為tc時刻速度倍率值；vc為tc時刻的速度。

對上述分析進行匯總可以得出，當(dāng)rc＜rt時，有公式（24）～公式（25）。

式中：acmax為tc時刻加速度的最大值；acmin為tc時刻加速度最小值；amax為加速度最大值。

而當(dāng)rc＞rt時，有公式（26）、公式（27）。

2.3 倍率加速度校核

為了進一步優(yōu)化倍率速度，需要對倍率的加速度進行條件約束，其流程圖如圖2 所示。如果倍率的加速度滿足約束條件，則直接進入倍率的速度校核部分。反之，如果倍率的加速度超出其上限，則應(yīng)將倍率的加速度設(shè)置為上限。如果倍率的加速度超出其下限，則應(yīng)將倍率的加速度設(shè)置為下限。

圖2 加速度校核

首先，校核倍率速度是否超出了其上限、下限，如果沒有，則校核結(jié)束，按照公式（28）～公式（31）更新倍率及時間參數(shù)。

如果倍率速度超出了其上限、下限，則應(yīng)修改倍率加速度，使倍率速度能夠滿足限制條件。從圖2 中可以看出，這部分的流程是對稱的，左側(cè)是倍率速度過大的情況，右側(cè)是倍率速度過小的情況（兩側(cè)的處理思路是一致的），這里選擇左側(cè)倍率速度過大的情況進行分析。

如果當(dāng)前時刻和下一時刻的倍率加速度均為其下限，說明已經(jīng)使用了最大的減速能力，此時就無法再調(diào)整倍率加速度，直接進入更新參數(shù)環(huán)節(jié)。反之，將下一時刻的倍率速度設(shè)置為其倍率速度上限。然后根據(jù)公式（32）求解下一時刻的倍率加速度。

如果新求出的下一時刻的倍率加速度大于或等于其下限，就說明能夠滿足加速度約束，直接進入?yún)?shù)更新環(huán)節(jié)；反之，則令下一時刻的倍率加速度為其下限，然后按公式（33）反算當(dāng)前時刻的倍率加速度。如果其滿足當(dāng)前時刻倍率加速度的上限、下限，則直接進入?yún)?shù)更新環(huán)節(jié)；反之，則令當(dāng)前時刻的倍率加速度為其下限，然后再進入?yún)?shù)更新環(huán)節(jié)。

在完成下一時刻的倍率速度及加速度校核和調(diào)整任務(wù)后，就可以根據(jù)調(diào)整后的倍率及其一階導(dǎo)數(shù)、二階導(dǎo)數(shù)計算下一時刻的位置、速度、加速度以及加加速度并輸出相關(guān)數(shù)據(jù)。最后，將下一時刻的倍率、倍率速度和時間設(shè)置成當(dāng)前時刻的值就可以生成單個周期的在線軌跡。

3 軌跡生成仿真實驗

由上述分析可知，整個倍率控制下的軌跡規(guī)劃核心在于通過規(guī)劃1 條一階連續(xù)的倍率曲線來調(diào)整倍率值，進而調(diào)整機器人的運動軌跡。下面將以1 條帶有軌跡混成的3 段PTP 運動為參考軌跡，通過MATLAB 機器人工具箱仿真得到的軌跡，如圖3 所示。

圖3 混成軌跡

該軌跡主要分為3 段：第一段為全速運行，機器人達到最大速度后繼續(xù)運動一段距離。第二段為變倍率運行，在運動過程中不斷改變機器人的運動速度。第三段沿著原有軌跡反向運動。圖4 為機器人全速運行時的軌跡特定曲線位置-速度-加速度-加加速度圖，為了使圖像更加簡潔、清晰，圖4 中只展示機器人前3 個軸的運動曲線。

圖4 全速運動部分規(guī)劃結(jié)果

在機器人運行的過程中，可以在不同時刻通過調(diào)整倍率值來實現(xiàn)軌跡的動態(tài)調(diào)速、停止、啟動以及反向運動等功能，最終的效果如圖5 所示。圖5 中共有8 條豎虛線，點虛線是倍率開始調(diào)整的時刻，虛線是倍率調(diào)整結(jié)束的時刻。從左往右看，第一條點虛線所處時刻，機器人的倍率開始從50%增至100%。第二條點虛線所處的時刻，機器人響應(yīng)停止的指令，開始減速，直至速度為0 °/s。機器人停了約0.18 s 后，即第三條點虛線所處的時刻，啟動機器人，機器人開始加速直至達到全速狀態(tài)。在第四條虛線所處的時刻，機器人響應(yīng)反向運動的指令，開始將速度降低，直至可以反向全速運動。

圖5 倍率調(diào)整軌跡圖

由圖5 可以看出，生成的位置-速度曲線光滑且連續(xù)，加速度和加加速度都在允許的范圍內(nèi)。機器人在動態(tài)調(diào)速過程中沒有產(chǎn)生明顯的振動，在變速過程中提高了穩(wěn)定性以及機器人的路徑跟隨性能。

圖6 為整個調(diào)整過程中倍率值的變化曲線。通過檢測相關(guān)數(shù)據(jù)可以發(fā)現(xiàn)，倍率超調(diào)的現(xiàn)象僅在機器人關(guān)節(jié)運動停止的過程中出現(xiàn)（倍率降為-3.4×10-6），且這種超調(diào)很小，完全可以忽略不計。

圖6 倍率變化

圖7 和圖8 分別展示了倍率的速度和加速度變化曲線，從這2 幅圖中可以看出，不同位置處所允許的倍率速度和加速度相差較大、倍率曲線速度連續(xù)且加速度跳變，滿足預(yù)期設(shè)計的梯形速度規(guī)劃。

圖7 倍率的速度圖

圖8 倍率的加速度圖

4 結(jié)論

倍率控制下的機器人軌跡生成算法有效地解決了機器人在運動過程中動態(tài)調(diào)整機器人運動的速度，會使機器人運動路徑的跟隨性下降，導(dǎo)致機器人出現(xiàn)運動軌跡精度下降的問題。該算法通過校核倍率曲線的速度規(guī)劃和加速度來保證機器人運動軌跡跟隨的準(zhǔn)確性。通過仿真試驗得到的軌跡位置和速度曲線圖具有連續(xù)性，驗證了該方法的可行性。結(jié)果表明，該方法有效解決了傳統(tǒng)調(diào)速造成的頻繁啟停問題，既提高了機器人運動時在實時調(diào)整速度過程中的路徑跟隨性能，又實現(xiàn)了停止、啟動、調(diào)速以及反向等功能，提高了機器人的作業(yè)效率。