基于正弦加減速控制算法的工業(yè)機(jī)器人位姿軌跡規(guī)劃研究

黃忠明，陳愛(ài)文，黃鳳良

（1.南京師范大學(xué) 電氣與自動(dòng)化工程學(xué)院，南京 210042；2.揚(yáng)州職業(yè)大學(xué) 電子工程學(xué)院，揚(yáng)州 225009）

0 引言

位姿軌跡規(guī)劃是通過(guò)給出有限個(gè)示教點(diǎn)規(guī)劃得到笛卡爾空間下的目標(biāo)軌跡，是實(shí)現(xiàn)機(jī)器人高效、準(zhǔn)確、平穩(wěn)地執(zhí)行作業(yè)任務(wù)、減少繁重的機(jī)器人示教工作的重要手段。位姿軌跡規(guī)劃包括位置軌跡規(guī)劃和姿態(tài)軌跡規(guī)劃，二種軌跡規(guī)劃一般分開(kāi)進(jìn)行，也有有一些學(xué)者試圖將二者同時(shí)規(guī)劃，但不管如何規(guī)劃，只有位置軌跡與姿態(tài)軌跡同時(shí)光滑連續(xù)且同步運(yùn)行，才能保證機(jī)器人末端執(zhí)行器的任務(wù)軌跡連續(xù)光滑。位姿軌跡在啟停階段會(huì)存在速度階躍的情況，這會(huì)對(duì)機(jī)器人機(jī)械本體造成沖擊，關(guān)節(jié)上的驅(qū)動(dòng)器無(wú)法補(bǔ)償如此巨大的力而導(dǎo)致電流過(guò)載，同時(shí)，機(jī)器人本體的振動(dòng)和沖擊會(huì)影響工作質(zhì)量、造成本體磨損影響機(jī)器人壽命[1]。杜亮、張鐵[2]采用位姿分離的方式，先插補(bǔ)機(jī)器人的位置點(diǎn)，再在各位置點(diǎn)上以歐拉角的方式插補(bǔ)機(jī)器人姿態(tài)，這種方法簡(jiǎn)單方便，實(shí)現(xiàn)了位姿軌跡同步規(guī)劃，提高了機(jī)器人控制的實(shí)時(shí)性，但無(wú)法實(shí)現(xiàn)啟停階段位置軌跡和姿態(tài)軌跡的（角）速度與（角）加速度的連續(xù)性。宋金華[3]采用7段S曲線加減速算法與三次樣條插值算法結(jié)合的方法，根據(jù)速度變化將軌跡分段，每段軌跡均采用三次樣條插值，并以此作為插值曲線來(lái)規(guī)劃末端執(zhí)行器的直線軌跡和圓弧軌跡，但7段S曲線加減速算法速度分段較多、算法復(fù)雜，且未提及位置規(guī)劃過(guò)程中的姿態(tài)處理方法。陳偉華、張鐵[4]研究了三次均勻有理B樣條在自由曲線軌跡中的應(yīng)用，曲線插補(bǔ)過(guò)程中同樣進(jìn)行了加減速控制，但其采用的線性加減速控制并不能使運(yùn)動(dòng)加速度連續(xù)。

本文采用正弦加減速控制算法，以笛卡爾空間下的直線軌跡為目標(biāo)位置軌跡，單位四元數(shù)表示姿態(tài)，分別規(guī)劃位置軌跡和姿態(tài)軌跡，并對(duì)兩種軌跡進(jìn)行位姿同步規(guī)劃處理，獲得位置和姿態(tài)軌跡均連續(xù)光滑且同步變化的位姿軌跡，以解決啟停階段（角）速度和（角）加速度不連續(xù)的問(wèn)題。

1 位姿軌跡規(guī)劃算法研究

1.1 位置軌跡規(guī)劃

設(shè)機(jī)器人末端執(zhí)行器從Ps點(diǎn)沿直線運(yùn)動(dòng)到Pe點(diǎn)，起始點(diǎn)Ps坐標(biāo)為（xs，ys，zs），終點(diǎn)Pe坐標(biāo)為（xe，ye，ze），采用定時(shí)插補(bǔ)的方式，設(shè)插補(bǔ)周期為Δt，運(yùn)行速度為v，從起點(diǎn)到第i個(gè)插補(bǔ)點(diǎn)的位移可表示為si=iΔs=ivΔt。將直線PsPe表示為關(guān)于是時(shí)間t的參數(shù)方程的形式，則第i個(gè)插補(bǔ)點(diǎn)的X，Y，Z坐標(biāo)分別為：

式中，Δx=xe-xs，Δy=ye-ys，Δz=ze-zs，。（xi，yi，zi）即為第i個(gè)插補(bǔ)周期t=iΔt時(shí)刻末端執(zhí)行器的三維坐標(biāo)。

本文采用正弦加減速控制算法對(duì)運(yùn)動(dòng)過(guò)程進(jìn)行加減速分段，原理如下：

1）加速階段：加速度a隨時(shí)間按正弦規(guī)律變化，從0變化到最大值，再變化為0。整個(gè)加速階段，加速度的值始終位于y坐標(biāo)軸的正半軸，而速度也會(huì)在此階段從0達(dá)到最大值，即勻速階段速度cm。

2）勻速階段：以恒定速度vm運(yùn)行至減速階段，該階段也是機(jī)器人末端執(zhí)行器運(yùn)行的主要階段。

3）減速階段：加速度a隨時(shí)間按正弦規(guī)律變化，從0變化到最小值，再變化為0。整個(gè)加速階段，加速度的值始終位于y坐標(biāo)軸的負(fù)半軸，而速度也會(huì)在勻速階段速度vm減小到0，此時(shí)機(jī)器人末端執(zhí)行器走完預(yù)設(shè)軌跡，到達(dá)目標(biāo)位置，運(yùn)行結(jié)束。

給定若干個(gè)示教點(diǎn)，設(shè)定最大速度vm，最大加速度am，則整個(gè)過(guò)程加速度的表達(dá)式為：

由式(2)積分得到速度表達(dá)式為：

由式(3)積分得到位移表達(dá)式為：

在不考慮關(guān)節(jié)空間下對(duì)笛卡爾空間預(yù)設(shè)軌跡進(jìn)一步細(xì)分的情況下，關(guān)節(jié)驅(qū)動(dòng)器驅(qū)動(dòng)各關(guān)節(jié)運(yùn)動(dòng)的進(jìn)給量就是一個(gè)插補(bǔ)周期內(nèi)機(jī)器人末端執(zhí)行器沿X，Y，Z三個(gè)方向上的變化量對(duì)應(yīng)的關(guān)節(jié)角變化量，由式(1)及si=iΔs可知，的大小與運(yùn)行完第i個(gè)插補(bǔ)周期t的位移si相關(guān)，因此，直線段的加減速劃分考察的是直線沿X，Y，Z三個(gè)方向的位移s隨運(yùn)行時(shí)間t和運(yùn)行速度v的變化情況。設(shè)機(jī)器人運(yùn)行完第i個(gè)插補(bǔ)周期t的位移為si，勻速段速度為vc，最大加速度為am，則由式(4)得加速、勻速和減速三個(gè)階段對(duì)應(yīng)的位移si分別為：

加速階段（0＜t≤t1）：

勻速階段（t1＜t≤t2）：

減速階段（t2＜t≤Tpos）：

各參數(shù)確定后，需校驗(yàn)勻速段速度vc是否“可達(dá)”，即在給定的加速度下機(jī)器人末端執(zhí)行器可以經(jīng)歷加速、勻速和減速的完整過(guò)程，當(dāng)整個(gè)運(yùn)動(dòng)過(guò)程處于臨界狀態(tài)時(shí)（末端執(zhí)行器加速到速度最大值時(shí)直接進(jìn)入減速階段），t1=Tpos-t1=t2。已知加速度段的位移為：

當(dāng)vc≤vcmax時(shí)，勻速段速度vc“可達(dá)”，即運(yùn)動(dòng)過(guò)程有勻速段，否則需要重新設(shè)定最大運(yùn)行速度vc或者最大加速度am。

1.2 姿態(tài)軌跡規(guī)劃

四元數(shù)表示的兩姿態(tài)間的軌跡規(guī)劃通常采用單位四元數(shù)球面線性插值的方法，設(shè)末端執(zhí)行器的起始Ps和結(jié)束點(diǎn)Pe對(duì)應(yīng)姿態(tài)的單位四元數(shù)分別表示為Q0和Q1，二者間的角位移為，總運(yùn)動(dòng)時(shí)間為Tort，插補(bǔ)周期為Δtt，則球面插值曲線為：

式中，Slerp函數(shù)的返回值Qu表示末端執(zhí)行器在起始姿態(tài)和結(jié)束姿態(tài)間的一個(gè)中間姿態(tài)，0≤u≤1，u的取值為，其中n=Tort/Δt。

四元數(shù)球面線性插值雖然可以實(shí)現(xiàn)兩姿態(tài)間的軌跡插值，但在啟停階段存在姿態(tài)角速度不連續(xù)的情況，因此論文同樣采用正弦加減速控制算法，對(duì)姿態(tài)角的變化過(guò)程進(jìn)行加減速控制。設(shè)定最大角速度wc，最大角加速度am，參照正弦加減速控制算法在位置軌跡規(guī)劃中的應(yīng)用，得不同階段兩姿態(tài)間角位移θ隨時(shí)間的變化關(guān)系為：

速度規(guī)劃的本質(zhì)是控制角位移的變化，即對(duì)球面線性插值公式slerp函數(shù)中的u的控制。設(shè)兩姿態(tài)間的角位移為，使代入slerp，得到隨插補(bǔ)周期t變化的四元數(shù)序列，即起始點(diǎn)姿態(tài)向結(jié)束點(diǎn)姿態(tài)變化過(guò)程中的姿態(tài)點(diǎn)序列，其余參數(shù)的確定方法與位置軌跡中的確定方法相同。最后將四元數(shù)序列轉(zhuǎn)化為姿態(tài)矩陣序列[5,6]。

1.3 位姿同步規(guī)劃

機(jī)器人運(yùn)行過(guò)程中末端執(zhí)行器的位置和姿態(tài)一般是同步變化的的，以保證末端執(zhí)行器平穩(wěn)運(yùn)行，因此需要對(duì)位置軌跡和姿態(tài)軌跡進(jìn)行同步規(guī)劃處理。設(shè)位置軌跡運(yùn)行總時(shí)間和姿態(tài)軌跡運(yùn)行總時(shí)間分別為Tpos和Tort，當(dāng)Tpos≠Tort時(shí)，通過(guò)調(diào)整位置軌跡或姿態(tài)軌跡中一方的最大速度或最大角速度值，重新規(guī)劃位置或姿態(tài)軌跡，使兩個(gè)軌跡的運(yùn)行時(shí)間相等。根據(jù)Tpos和Tort的大小關(guān)系分三種情況進(jìn)行同步處理：

1）Tpos=Tort：位置軌跡與姿態(tài)軌跡運(yùn)行時(shí)間相等，滿足位姿同步的要求。

2）Tpos＞Tort：為之軌跡運(yùn)行時(shí)間大于姿態(tài)軌跡運(yùn)行時(shí)間，此時(shí)增加Tort，使Tort=Tpos，計(jì)算相應(yīng)的勻速段角速度wc，重新規(guī)劃姿態(tài)軌跡。

3）Tpos＜Tort：位置軌跡運(yùn)行時(shí)間小于姿態(tài)軌跡運(yùn)行時(shí)間，此時(shí)增加Tpos，使Tpos=Tort，計(jì)算相應(yīng)的勻速段速度vc或角速度wc，重新規(guī)劃位置軌跡。

2 仿真結(jié)果與分析

以Cyton Gamma300七自由度機(jī)器人為實(shí)驗(yàn)對(duì)象，取末端執(zhí)行器的兩個(gè)示教點(diǎn)分別作為起始點(diǎn)和終止點(diǎn)，對(duì)應(yīng)的起始點(diǎn)位姿矩陣Ts和終止點(diǎn)位姿矩陣Te分別為：

在MATALB環(huán)境下驗(yàn)證上述基于正弦加減速控制算法的位姿軌跡規(guī)劃算法。

位姿矩陣中位置坐標(biāo)的單位為毫米（mm），姿態(tài)的單位為弧度（rad），以起始點(diǎn)為例，機(jī)器人末端執(zhí)行器的位置和姿態(tài)在矩陣中分別對(duì)應(yīng)為：

設(shè)定機(jī)器人末端執(zhí)行的最大運(yùn)行速度為300mm/s，最大運(yùn)行加速度為500mm/s2，最大姿態(tài)角速度為0.3rad/s，最大姿態(tài)角加速度為0.5rad/s2，插補(bǔ)周期為2ms，經(jīng)本文的位姿軌跡規(guī)劃算法得到機(jī)器人的不同視角下的位姿軌跡如圖1所示。

軌跡中位移、速度及加速度隨時(shí)間變化的關(guān)系如圖2所示。

圖1 直線位姿軌跡

圖2 直線位置軌跡位移、速度及加速度變化

由圖2可知，末端執(zhí)行器在直線運(yùn)動(dòng)啟停加減速階段、勻速階段的變化均符合足正弦加減速控制策略的控制規(guī)律，整個(gè)過(guò)程位移、速度及加速度連續(xù)均連續(xù)變化，符合位置規(guī)劃算法的預(yù)期目標(biāo)。從圖2中可以看出，直線位置軌跡最大運(yùn)行速度并不是設(shè)定的0.3m/s，這是因?yàn)樵谶M(jìn)行位姿同步處理時(shí)姿態(tài)軌跡運(yùn)行時(shí)間大于位置軌跡運(yùn)行時(shí)間，此時(shí)同步處理算法通過(guò)改變最大運(yùn)行段速度重新對(duì)位置軌跡進(jìn)行插補(bǔ)，從而使位置軌跡運(yùn)行時(shí)間和姿態(tài)軌跡運(yùn)行時(shí)間相等，達(dá)到位姿同步的目的。

直線軌跡中還包含了姿態(tài)軌跡的變化，姿態(tài)的角位移、角速度及角加速度雖時(shí)間的變化關(guān)系如圖3所示。

圖3 姿態(tài)軌跡位移、速度及加速度變化

從圖3可知，姿態(tài)軌跡規(guī)劃算法可以實(shí)現(xiàn)啟停階段姿態(tài)角速度和角加速度變化的連續(xù)性，符合姿態(tài)軌跡規(guī)劃的預(yù)期目標(biāo)。同時(shí)從圖3中可以看出，姿態(tài)軌跡的運(yùn)行時(shí)間為2.85秒，與圖2中所示的位置軌跡運(yùn)行時(shí)間相等，驗(yàn)證了位姿同步處理算法的可行性。

3 結(jié)論

本文提出了基于正弦加減速控制算法的工業(yè)機(jī)器人位姿軌跡規(guī)劃算法，通過(guò)將正弦加減速控制策略應(yīng)用到機(jī)器人末端執(zhí)行器的位置軌跡和姿態(tài)軌跡的同步規(guī)劃中，使其在啟動(dòng)階段、勻速運(yùn)行階段及停止階段間平穩(wěn)光滑過(guò)渡。仿真結(jié)果表明，啟停階段末端執(zhí)行器的（角）速度和（角）加速度連續(xù)變化，整個(gè)運(yùn)行軌跡連續(xù)、平滑。