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

丁猛 張中輝 劉蔚釗

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

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

0? ? 引言

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

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

1? ? KNT-ESR6B機(jī)器人

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

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

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

2.1? ? 機(jī)器人空間圓弧插補(bǔ)

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

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

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

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

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

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

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

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

2.2? ? S型加減速曲線

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

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

通過(guò)對(duì)每個(gè)階段加速度函數(shù)進(jìn)行積分運(yùn)算，即可得出速度函數(shù)，為：

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

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

通過(guò)上述對(duì)圓弧和S型曲線的分析，給定機(jī)器人圓弧軌跡的初始位置P1、中間點(diǎn)P2、終點(diǎn)P3，由公式（6）（11）可得出機(jī)器人在新坐標(biāo)系下的圓弧半徑r和圓心角θ2，則機(jī)器人運(yùn)動(dòng)的圓弧軌跡位移L=rθ2[4]。

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

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

3? ? 圓弧軌跡規(guī)劃仿真實(shí)驗(yàn)

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

在Matlab平臺(tái)基于S型加減速曲線對(duì)空間圓弧插補(bǔ)算法進(jìn)行仿真，其圓弧插補(bǔ)過(guò)程中各軸坐標(biāo)變化曲線以及圓弧插補(bǔ)的運(yùn)動(dòng)軌跡如圖6所示。

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

4? ? 結(jié)語(yǔ)

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

[參考文獻(xiàn)]

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

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

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

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

收稿日期：2020-04-09

作者簡(jiǎn)介：丁猛（1981—），男，江蘇南京人，工程師，研究方向：自動(dòng)控制。