雙S速度規劃器的改進及其在協作機器人中的應用

蘇杰

（爾智自動化科技（上海）有限公司，上海 201112）

協作機器人屬于典型的串聯機器人，其運行過程中的振動和機械沖擊對機器人精度和壽命有較大的影響。機器人軌跡規劃器的加速度不連續，會引起機器人運行過程中的機械沖擊和末端抖動。選擇雙S速度規劃器作為軌跡規劃器能夠避免出現機械沖擊，能夠提高機器人壽命、減少振動、增加精度。

雙S速度規劃器是基于7段(典型值)加加速度(Jerk,j)為分段常量的曲線。由于引入了具有階躍函數特點的加加速度，使得加速度(Acceleration, a)線性連續變化，進而控制速度(Velocity, v)平滑變化。因為加速度連續，無加速度沖擊，關節執行器在改變運動狀態過程中承受的沖擊力小，大幅度減小了機械系統的震動，保證機器人運動速度高效的同時減小了對機械本體的沖擊。

由于系統的運動約束不同，雙S速度規劃器可能不完整包含典型的7段。根據系統給定的起點速度v0,終點速度v1,位移q，最大速度vmax，最大加速度amax,最大加加速度jmax,雙S速度規劃器可能不含勻速段、勻加速段和勻減速段，或者只有加速段或減速段之一的情況。因此在規劃判定后，雙S速度規劃器可能縮減至只有2段的情況。

用于機床數控系統的雙S速度規劃器基于系統配置和響應的不同，有不少不同方向的研究，見文獻[6,7,8]。在機床數控領域，文獻[2]將不同約束和輸入參數產生的雙S速度規劃器分為8種情況，文獻[3]分為17種，已經在分類計算上頗為詳細了。不過相比之下用于協作機器人的雙S速度規劃器研究較少，文獻[1,4]雖然提出了雙S速度規劃器在碼垛機器人和協作機器人上應用，不過對于不同系統約束和輸入參數的情況沒有分類和判斷的研究，對于遇到極端值情況下的求解沒有研究。文獻[5]提出了三角函數速度曲線規劃，但是由于位置的三階導數是不連續的，很顯然也是達不到協作機器人軌跡規劃要求的。

由于起點速度v0，終點速度v1,位移q的任意性，可能存在給定的起止速度在最大加速度amax和最大加加速度jmax的約束條件下無法產生期望軌跡，或者說為了滿足期望位移而無法達到期望速度，即存在最大速度不可達情況下軌跡不能求解并規劃的問題。上述文獻對該問題的解決沒有涉及，研究是不足的。

本文為了求解該問題，對傳統雙S速度規劃器進行改善。引入了加速度衰減迭代系數，提出了一種可以有效求解的算法。這種算法除了可以判斷加速度極值不可達的情況并可以對時間周期求解以外，還可以判斷加速度段和減速度段僅存其一的情況并可以對時間周期求解。因此全面覆蓋了各種情況下符合協作機器人要求的軌跡規劃曲線的求解，確保軌跡規劃的連續性和機器人運行的平穩性。之后在協作機器人進行實施，驗證改進算法。

1 傳統雙S速度規劃器

圖1 傳統雙S速度規劃器軌跡圖

傳統雙S速度規劃器的軌跡規劃根據二次加速度變化分為典型的7段，根據加速度類型分為3個階段：加速段、勻速段和減速段。假定vlim=vmax，判斷勻加速段是否存在。首先求解加速度段時間Ta和位移qa，減速度段時間Td和位移qd，其次將加速段和減速段位移和期望位移比較，如果，則勻速段存在，反之勻速段不存在。在勻速段不存在的情況下，通過對vmax的解析和迭代，求解極限速度vlim，而在加速段和減速段不同時存在的情況下，再次迭代求解vlim，隨后反求加速段最大加速度alima、減速段最大加速度alimd和分段時間，最后由下述的軌跡規劃方程組(1)求解位移q(t)、速度v(t)、加速度a(t)：

在協作機器人軌跡規劃中，由于串聯式關節結構，機器人有多個自由度，每個關節運動位移在不同的位姿下差別很大，因此在系統給定的插補位移很小而速度變化較大時，雙S速度規劃器是無效的。此時其計算方法不適合協作機器人軌跡規劃求解。因此在軌跡規劃中，首先要根據期望起點位置、期望終點位置、期望起點速度、期望終點速度，進行如下判斷，以保證軌跡規劃有效后再進行計算：

為了在協作機器人中使用雙S曲線軌跡規劃，本文首先解析最大速度vlim，進而解析加速段最大加速度alima、減速段最大加速度alimd，解析過程中已經求解了加加速段Tj1、加速段Ta、勻加速段Tj-2Tj1、勻速段Tv、減速段Td、加減速段Tj2、勻減速段Td-2Tj2的分段周期，解析完成后帶入方程組（1）求解。

2 改進雙S速度規劃器

由于機器人關節執行器在正反轉時的最大速度vmax和vmin、最大加速度amax和amin、最大二次加速度jmax和jmin是對稱的并且為常量，因此本文有如下約束：

2.1 存在勻速段的求解

2.1.1 最大加/減速度可達的判斷條件

2.1.2 求解 Ta和 Td

2.1.3 求解 Tv

如果Tv≥0，vmin可達，存在勻速段；反之amax不可達，不存在勻速段。

2.2 不存在勻速段的求解

假定加速度amax和減速度amin均可達：

如果Ta≥2Tj且Td≥2Tj，那么假定有效。如果Ta＜2Tj或Td＜2Tj，那么意味著amax不可達或amin不可達，此時引入衰減迭代系數。

2.3 加速度或減速度不可達

令amax=kamax，采用衰減迭代，根據(11)(12)(13)重新計算Ta和Td，直到Ta≥2Tj且Td≥2Tj后退出迭代，此時會得到軌跡規劃中Ta和Td的合適值。

在上述迭代算法計算過程中遇到Td或Td小于零的情況，如果Ta＜0，意味著沒有加速段，此時求解Td和Tj2，如果Td＜0，意味著沒有減速段，此時只需要計算Ta和Tj1：

2.4 vlim、alima、alimd 值的確定

vmax、amax、amin其一可達的情況，按下式確定極值：

vmax、amax、amin其一不可達的情況，按下式確定極值：

2.5 反向運動的軌跡規劃求解

2.6 程序流程圖（圖2）

圖2 程序流程圖

3 算法實驗驗證

3.1 驗證設備

采用六自由度協作機器人對本文算法進行應用和驗證。機器人控制系統分為中央控制層、通信層和驅控層。中央控制層主要進行機械臂的正逆運動學計算、動力學計算、坐標轉換和軌跡規劃頂層控制。

中央控制系統硬件為研華MIO嵌入式單板工控機。關節執行器采用高度集成化設計并集減速器、電機、驅動器、帶散熱功能的制動器、編碼器等組件于一體的驅控一體化關節模組。本文算法在中央控制層進行計算，輸出的軌跡規劃參數求解后下發給各關節控制器。

3.2 算法驗證

以觀測效果比較明顯的第二關節進行驗證。關節最大速度vmax=180°/s，最大加/減速度amax=360°/s,最大加加速度 jmax=2160°/s。

3.2.1 速度不可達，加速度均可達

圖3 速度不可達，加速度均可達的軌跡規劃曲線

驗證機器人運動軌跡結果如圖4。

圖4 速度不可達，加速度均可達的真機驗證

3.2.2 速度和加速度均不可達

圖5 速度和加速度均不可達的軌跡規劃曲線

圖6 速度和加速度均不可達的真機驗證

4 結語

本文研究了適合協作機器人雙S速度規劃器的算法應用，詳細討論了不同情況下規劃器的參數求解問題并給出了解析方式和計算方法。通過引入衰減迭代系數，進行一次性局部衰減迭代計算，解決了速度規劃器在最大速度不可達情況下，如何高效精確地求解加/減速度段時間的問題。

通過對速度不可達而加速度均可達情況下的算法仿真驗證了本文算法的可靠性和可行性。進一步在速度和加速度均不可達情況下通過引入衰減迭代系數k，驗證了求解加/減速度段時間的高效、精確和可行性。

最后將本文算法在協作機器人系統中進行真機驗證，機器人啟停、加減速運動及軌跡算法切換均無抖動，機器人運行平穩。表明了本文算法在協作機器人軌跡規劃中的有效和可靠性。