基于S型曲線加減速的機器人圓弧軌跡規(guī)劃與仿真

丁猛 張中輝 劉蔚釗

摘要：軌跡規(guī)劃算法在機器人控制中的地位十分重要，其優(yōu)劣會直接影響機器人的運動速度、精度以及平穩(wěn)性。結(jié)合康尼公司設(shè)計的KNT-ESR6B機器人，針對傳統(tǒng)加減速算法的運動加速度突變所造成的機器人運動抖動問題，提出了基于S型曲線加減速的空間圓弧軌跡規(guī)劃算法，并在Matlab平臺進行了仿真實驗。

關(guān)鍵詞：機器人;軌跡規(guī)劃;S型曲線;圓弧插補

0? ? 引言

從規(guī)劃空間上可將機器人軌跡規(guī)劃分為兩種，即關(guān)節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃。在工業(yè)生產(chǎn)中，機器人運動軌跡幾乎都是直線和圓弧軌跡的組合[1]，特別是在一些對機器人運動軌跡有著嚴格要求的工業(yè)現(xiàn)場，如噴涂、焊接、打磨等，其圓弧軌跡規(guī)劃的平滑度直接決定了機器人的運動性能，所以軌跡規(guī)劃算法在機器人控制中的地位十分重要。

本文以南京康尼公司自主研發(fā)的KNT-ESR6B型六自由度機器人為研究對象，基于S型曲線加減速對其空間圓弧插補軌跡規(guī)劃進行了研究，并在Matlab平臺對其進行了仿真驗證，結(jié)果表明，機器人的圓弧運動軌跡平滑，各關(guān)節(jié)加速度平緩，滿足應(yīng)用要求。

1? ? KNT-ESR6B機器人

KNT-ESR6B機器人由機械本體和控制系統(tǒng)兩部分組成，機器人本體的6個關(guān)節(jié)由交流伺服電機驅(qū)動，從而實現(xiàn)機器人的運動;其控制系統(tǒng)則采用了“PC機+DSP運動控制卡”的開放式控制模式來設(shè)計。根據(jù)連桿坐標系的規(guī)則建立了KNT-ESR6B機器人運動學模型，如圖1所示。

通過相鄰連桿坐標系之間的關(guān)系變換矩陣，即公式（1），可得KNT-ESR6B機器人各相鄰連桿之間的變換矩陣，由此可以推導出機器人正向運動學和逆向運動學公式。

2? ? 基于S型加減速曲線的空間圓弧插補算法

2.1? ? 機器人空間圓弧插補

機器人空間圓弧插補算法比較復雜，其解決思路是將空間圓弧轉(zhuǎn)化到平面中進行處理[2]，如圖2所示，已知機器人工作空間中圓弧的起點P1（x1，y1，z1）、中間點P2（x2，y2，z2）和終點P3（x3，y3，z3），具體插補步驟如下：

（2）求坐標變換矩陣。以O(shè)1（p0）為原點，圓弧所在平面M的法向量a為O1Z1軸，向量■為O1X1軸，根據(jù)右手定則建立新坐標系O1-X1Y1Z1，其中O1Z1軸在基礎(chǔ)坐標系O-X0Y0Z0中的方向余弦即為法向量a，由公式（3）平面M的方程可求得法向量a：

最后由右手定則可知，O1Y1軸的法向量o=a×n，所以利用新坐標系O1-X1Y1Z1的各軸在基礎(chǔ)坐標系O-X0Y0Z0中的方向余弦向量，就可建立變換矩陣：

所以對于基礎(chǔ)坐標系中的任意一點0p=（0px，0py，0pz）都與新坐標系中與之唯一對應(yīng)的點1p=（1px，1py，1pz）來表示，兩者之間的轉(zhuǎn)換過程如式（10）所示：

（3）在新坐標系中計算各插補點的坐標值，然后將空間圓弧軌跡上各插補點坐標值從新坐標系O1-X1Y1Z1轉(zhuǎn)換到基礎(chǔ)坐標系O-X0Y0Z0中。如圖3所示，在新坐標系O1-X1Y1Z1的X1Y1平面中確定插補角大小，即：

在公式（11）中，由于用到了atan2（x，y）函數(shù)，該函數(shù)本身具有預先判斷目標點所在象限的功能，會對角度θ進行處理，所以不再需要對插補的方向和過象限進行判斷。

設(shè)在機器人插補周期T內(nèi)，每次插補角度增量為Δθ，則插補的次數(shù)N=θ/Δθ+1（N四舍五入取整數(shù)），根據(jù)公式（12）可求出軌跡中每個插補點1pi=（1pix，1piy，1piz）所對應(yīng)的坐標值，即：

最后根據(jù)公式（12）將平面圓弧的每個插補點坐標通過轉(zhuǎn)換矩陣映射到基座標系中，再由逆向運動學公式求出軌跡上插補點所對應(yīng)的關(guān)節(jié)角度[3]，驅(qū)動機器人運動到相應(yīng)的角度位置，就可以完成空間圓弧的軌跡規(guī)劃。

2.2? ? S型加減速曲線

S型加減速將T型加減速的3段過程拓展為7段，如圖4所示，即由加加速段、勻加速段、減加速段、勻速段、加減速段、勻減速段、減減速段組成，可使速度曲線更加趨于平滑，減小電機突然啟動時的沖擊和機器人關(guān)節(jié)機構(gòu)機械振動，保證機器人運動的平穩(wěn)。

在圖4中，t為時間坐標，ti為各階段的過渡點時刻，Ti為各階段的持續(xù)運行時間，為了簡化計算公式的復雜程度，用τi=t-ti-1（i=1，2，…，7）表示各階段的起始點作為時間零點的時間，即局部時間坐標。同時假設(shè)加加速度和減減速度都為J，且最大加速度為Am，即T1=T3=T5=T7=T，初始速度為Vs，最大速度為Vm，則通過對每段過程的分析，可得對應(yīng)軌跡段的加速度函數(shù)，分別為：

通過對每個階段加速度函數(shù)進行積分運算，即可得出速度函數(shù)，為：

對公式（14）中各階段的速度以公式（15）進行積分，即可得到運動的位移函數(shù)：

2.3? ? 基于S型加減速曲線的圓弧插補

通過上述對圓弧和S型曲線的分析，給定機器人圓弧軌跡的初始位置P1、中間點P2、終點P3，由公式（6）（11）可得出機器人在新坐標系下的圓弧半徑r和圓心角θ2，則機器人運動的圓弧軌跡位移L=rθ2[4]。

設(shè)S曲線的初始速度和終止速度為Vs，最大速度為Vm，最大加速度為Am，加加速度和減減速度為J，可得各階段的運行時間：

根據(jù)機器人空間圓弧插補的原理，設(shè)計其插補控制程序，流程圖如圖5所示。

3? ? 圓弧軌跡規(guī)劃仿真實驗

以KNT-ESR6B教學機器人為實驗平臺，在其工作空間中選定不共線的三點，即起始點P1、中間點P2、終點P3，其位姿數(shù)據(jù)如公式（17）所示（假設(shè)運動過程中姿態(tài)保持不變）：

在Matlab平臺基于S型加減速曲線對空間圓弧插補算法進行仿真，其圓弧插補過程中各軸坐標變化曲線以及圓弧插補的運動軌跡如圖6所示。

通過以上仿真圖形數(shù)據(jù)可以看出該機器人進行插補時的運行軌跡，驗證了基于S型曲線加減速的機器人空間圓弧插補算法的可靠性。

4? ? 結(jié)語

本文基于S型加減速曲線對機器人空間圓弧軌跡規(guī)劃進行了研究，通過坐標系轉(zhuǎn)換矩陣，將基坐標系下的圓弧軌跡轉(zhuǎn)換至圓弧所在平面的新坐標系下，完成了插補過程;同時針對軌跡規(guī)劃曲線的平滑性要求，運用S型加減速曲線進行升降速控制，通過仿真結(jié)果看出運用S型加減速控制使得軌跡規(guī)劃曲線趨于平滑，可以減小電機加減速時所造成的機器人運動抖動。仿真的結(jié)果驗證了所提算法的可靠性，為KNT-ESR6B機器人的控制實現(xiàn)提供了理論依據(jù)，具有一定的實用價值。

[參考文獻]

[1] 蔡自興.機器人學基礎(chǔ)[M].北京：機械工業(yè)出版社，2015.

[2] 孫瑛，程文韜，李公法，等.關(guān)節(jié)型機器人軌跡規(guī)劃算法及軌跡規(guī)劃研究現(xiàn)狀[J].長江大學學報（自科版），2016（28）：32-38.

[3] 張霞，孫強，蔡順燕，等.機器人軌跡規(guī)劃控制策略研究[J].微電機，2019（11）：76-81.

[4] 劉蕾，柳賀，曾輝.六自由度機器人圓弧平滑運動軌跡規(guī)劃[J].機械制造，2014（10）：4-5.

收稿日期：2020-04-09

作者簡介：丁猛（1981—），男，江蘇南京人，工程師，研究方向：自動控制。