倍率控制下的機(jī)器人軌跡生成算法

游 瑋 詹慶榮 方 明，

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

0 引言

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

1 倍率控制簡介

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

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

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

圖1 軌跡生成

2 倍率軌跡生成算法

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

無論是關(guān)節(jié)空間的運(yùn)動(dòng)還是笛卡爾空間內(nèi)的運(yùn)動(dòng)，在完成機(jī)器人軌跡規(guī)劃后均可以得到各關(guān)節(jié)位置隨時(shí)間t的變化曲線。假設(shè)當(dāng)前機(jī)器人某個(gè)軸倍率值為r（r∈[0，1]），則相當(dāng)于這個(gè)時(shí)刻對位置變化方程中的t進(jìn)行了參數(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）為對位置變化方程中的進(jìn)行了參數(shù)化。

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

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

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

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

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

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

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

2.2 倍率速度規(guī)劃

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

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

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

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

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

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

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

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

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

式中：tcmax為加速段最大時(shí)長。

反之，則有公式（19）。

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

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

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

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

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

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

2.3 倍率加速度校核

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

圖2 加速度校核

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

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

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

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

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

3 軌跡生成仿真實(shí)驗(yàn)

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

圖3 混成軌跡

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

圖4 全速運(yùn)動(dòng)部分規(guī)劃結(jié)果

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

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

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

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

圖6 倍率變化

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

圖7 倍率的速度圖

圖8 倍率的加速度圖

4 結(jié)論

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