杜正聰，黎安慶 ，李建雄，牟圓圓 ，羅佳源

（西昌學(xué)院 a.信息技術(shù)學(xué)院； b.機(jī)械與電氣工程學(xué)院，四川 西昌 615000）

0 引言

電機(jī)在運(yùn)行過程中，如果速度出現(xiàn)突變，則會(huì)產(chǎn)生沖擊、震蕩等現(xiàn)象。柔性加減速算法可使速度曲線趨于平滑［1-2］。常見的柔性加減速算法有梯形加減速算法、s形加減速算法和指數(shù)加減速算法［3-5］。后二者由于參數(shù)多，計(jì)算量大，在需要快速響應(yīng)的機(jī)器人應(yīng)用場合不適用。梯形加減速算法存在加速、勻速、減速3個(gè)階段，該算法參數(shù)少、運(yùn)算快，有大量學(xué)者對(duì)此進(jìn)行了研究和應(yīng)用。Chen等［6］提出了一種基于3參數(shù)Lame曲線的梯形軌跡優(yōu)化規(guī)劃方法，可優(yōu)化工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡平滑度；吳鵬飛等［7］提出了一種改進(jìn)型變權(quán)重梯形速度優(yōu)化算法，可減小接觸時(shí)的碰撞；Yoon等［8］提出一種根據(jù)物體固有頻率調(diào)整梯形加減速參數(shù)來減小機(jī)構(gòu)震動(dòng)的方法；Heo等［9］提出了一種新的梯形速度剖面生成方法，并用位置指令實(shí)現(xiàn)了上述算法；Kim等［10］在分揀系統(tǒng)中應(yīng)用梯形算法使分揀效能提高了40%。但上述梯形算法僅僅在機(jī)床、液壓等可提前將所有運(yùn)動(dòng)規(guī)劃完的設(shè)備中應(yīng)用。

服務(wù)類機(jī)器人在異物留存、生物體闖入運(yùn)動(dòng)范圍、目標(biāo)運(yùn)動(dòng)規(guī)律變化等情況時(shí)，需中斷當(dāng)前運(yùn)動(dòng)規(guī)劃，但重新進(jìn)行運(yùn)動(dòng)規(guī)劃時(shí)初速度不為零，現(xiàn)有梯形加減速算法不能解決上述問題。本文在梯形加減速算法的基礎(chǔ)上提出了類梯形加減速算法，并進(jìn)行了理論推導(dǎo)。為驗(yàn)證該算法，設(shè)計(jì)了伺服轉(zhuǎn)臺(tái)，并編寫了伺服控制軟件，最后通過實(shí)驗(yàn)驗(yàn)證了該算法。

1 梯形加減速算法

梯形加減速算法其速度時(shí)間曲線如圖1所示。加減速階段的加速度恒為amax，勻速階段速度為vmax，t1為加速段結(jié)束時(shí)刻，t2為勻速段結(jié)束時(shí)刻，t3為減速段結(jié)束時(shí)刻，目標(biāo)位置為Sa。3個(gè)階段運(yùn)行起止時(shí)刻的公式［11-13］如式（1）～（3）所示。

圖1 梯形加減速和三角形加減速運(yùn)動(dòng)圖

若Sa減小，首先應(yīng)縮短勻速段運(yùn)行時(shí)間，直至為零；若Sa進(jìn)一步減小，則應(yīng)同時(shí)縮短加、減速階段運(yùn)行時(shí)間。此時(shí)加速段尚未結(jié)束，運(yùn)動(dòng)直接進(jìn)入減速段，原來3階段的梯形加減速變?yōu)?階段的“三角形”加減速運(yùn)動(dòng)。判定該情況是否出現(xiàn)的條件如式（4）所示，運(yùn)動(dòng)規(guī)劃如式（5）～（7）所示。

以上是初末速度均為零的梯形加減速算法，該算法適用于機(jī)床、液壓設(shè)備、繪圖儀等無須中斷運(yùn)動(dòng)規(guī)劃的設(shè)備［14-15］。

2 類梯形加減速算法

對(duì)于服務(wù)類機(jī)器人，當(dāng)運(yùn)動(dòng)出現(xiàn)中斷時(shí)，假設(shè)位置為S0，速度為V0，其余參數(shù)amax、vmax、t1、t2、t3的含義均與上文的一致，新的目標(biāo)位置為Sb。設(shè)S′b=Sb-S0為下一步運(yùn)動(dòng)規(guī)劃需要運(yùn)動(dòng)距離，對(duì)S′b不同，有不同運(yùn)動(dòng)策略。

當(dāng)S′b滿足條件時(shí)，運(yùn)動(dòng)規(guī)劃為加速、勻速、減速3段，各段的起始時(shí)刻分別如式（8）～（10）所示，其運(yùn)動(dòng)曲線如圖2a所示。

圖2 類梯形加減速算法速度-時(shí)間曲線圖

當(dāng)S′b減小時(shí)，縮減勻速段時(shí)間。當(dāng)S′b滿足條件時(shí)，勻速段運(yùn)行時(shí)間縮減為0，S′b再減小則應(yīng)縮減加、減速段運(yùn)行的時(shí)間，最大速度為v'max（v'max＜vmax）。加、減速段的截止時(shí)刻表示如式（11）～（13）所示，其運(yùn)動(dòng)曲線如圖2b所示。

S'b繼續(xù)減小，當(dāng)滿足條件-S2=時(shí)，運(yùn)動(dòng)規(guī)劃為開始就勻減速，速度減為零后反向加速運(yùn)動(dòng)，速度達(dá)到-v'max（-v'max＜-vmax），再勻減速至停止。減、加速段的時(shí)刻計(jì)算分別如式（14）～（16）所示，其運(yùn)動(dòng)曲線如圖2c所示。

S'b進(jìn)一步減小，當(dāng)滿足條件時(shí)。開始即勻減速，速度減為零后反向加速運(yùn)動(dòng)，直到反向運(yùn)動(dòng)速度達(dá)到-vmax，并保持勻速運(yùn)動(dòng)后才勻減速至停止。減速、勻速、加速段的時(shí)刻計(jì)算分別如式（17）～（19）所示，其運(yùn)動(dòng)曲線如圖2d所示。

3 伺服試驗(yàn)臺(tái)硬件設(shè)計(jì)

為驗(yàn)證上述算法，本文設(shè)計(jì)了伺服試驗(yàn)臺(tái)。該試驗(yàn)臺(tái)如圖3所示，主體框架呈立方體結(jié)構(gòu)，使用20 mm規(guī)格方鋼管切割焊接，其余部件安裝在框架上?？蚣茼敳堪惭b回轉(zhuǎn)軸承以及承臺(tái)，在承臺(tái)上可固定用于目視粗略直接測量角度的360°標(biāo)記板；框架左側(cè)安裝一根指示轉(zhuǎn)臺(tái)位置的指示桿。承臺(tái)與電機(jī)之間使用同步輪系傳動(dòng)，同步輪系減速比為5∶1，由17齒小輪85齒大輪及同步帶組成。

圖3 伺服轉(zhuǎn)臺(tái)結(jié)構(gòu)圖

控制電路（圖4）由STM32F103單片機(jī)作為運(yùn)算控制模塊，該單片機(jī)可提供32位、72 Mhz的算力。經(jīng)驗(yàn)證，在3閉環(huán)結(jié)構(gòu)下該模塊可實(shí)現(xiàn)500 Hz以上的控制頻率，滿足本設(shè)計(jì)要求。驅(qū)動(dòng)單元主芯片為BTN7971，最大提供45 V、50 A負(fù)載，具備電流測量功能。根據(jù)電機(jī)參數(shù)，將其工作電壓設(shè)置為24 V，最大通過5 A電流。電機(jī)選用80∶1減速比的直流行星減速電機(jī)，該電機(jī)尾部裝配了AS5048磁編碼器作為位置反饋傳感器，其分辨率達(dá)到0.02°。整機(jī)供電使用開關(guān)電源模塊，將220 V轉(zhuǎn)換為24 V。DC-DC轉(zhuǎn)換器將24 V轉(zhuǎn)換為5 V后提為控制系統(tǒng)供電。系統(tǒng)運(yùn)行時(shí)，單片機(jī)通過驅(qū)動(dòng)單元驅(qū)動(dòng)電機(jī)運(yùn)行，電機(jī)角度通過磁編碼器反饋到單片機(jī)當(dāng)中形成閉環(huán)控制。電機(jī)運(yùn)動(dòng)通過行星減速器和同步輪系傳遞到承臺(tái)上。在調(diào)試時(shí)，電機(jī)還可通過串口與上位機(jī)通信，顯示速度、位置等信息。

圖4 控制系統(tǒng)結(jié)構(gòu)框圖

4 伺服試驗(yàn)臺(tái)軟件設(shè)計(jì)

本文針對(duì)硬件平臺(tái)進(jìn)行了軟件設(shè)計(jì)，選擇在MDK上使用C語言開發(fā)程序。程序流程圖如圖5所示，程序起始后首先接收運(yùn)動(dòng)參數(shù)：Sb、S0、v0、amax、vmax。計(jì)算得出各段運(yùn)動(dòng)的起止時(shí)刻，之后進(jìn)入運(yùn)動(dòng)控制循環(huán)。循環(huán)第1步根據(jù)當(dāng)前運(yùn)行時(shí)間計(jì)算下一步運(yùn)行的位置Snext，將該位置指令送入伺服程序運(yùn)算得到PWM信號(hào)，最后經(jīng)驅(qū)動(dòng)單元放大功率，驅(qū)動(dòng)電機(jī)。如果運(yùn)動(dòng)控制循環(huán)運(yùn)行過程中出現(xiàn)運(yùn)動(dòng)中斷指令，則程序記錄當(dāng)前位置為S0、速度為v0，并根據(jù)上述數(shù)據(jù)重新進(jìn)行運(yùn)動(dòng)規(guī)劃。

圖5 運(yùn)動(dòng)規(guī)劃及伺服環(huán)路軟件流程

5 實(shí)驗(yàn)與驗(yàn)證

在每次伺服環(huán)路運(yùn)行過程中，程序?qū)?dāng)前測試數(shù)據(jù)通過串口發(fā)送給PC機(jī)。PC機(jī)對(duì)接收到的數(shù)據(jù)進(jìn)行處理并顯示。

首先驗(yàn)證運(yùn)動(dòng)規(guī)劃的作用。設(shè)置初始位置為0（°），當(dāng)給定位置指令S= 3×106（°）后，伺服轉(zhuǎn)臺(tái)在沒有運(yùn)動(dòng)規(guī)劃的情況下，以機(jī)電系統(tǒng)提供的最大加速度向目標(biāo)運(yùn)動(dòng)，并且在目標(biāo)點(diǎn)附近停止。電機(jī)運(yùn)行速度在0和3.5 s時(shí)均出現(xiàn)階躍性突變，在3.5 s時(shí)甚至出現(xiàn)較大過沖，產(chǎn)生機(jī)械沖擊（圖6）。

圖6 有無使用運(yùn)動(dòng)規(guī)劃的速度測試曲線

同樣的測試條件，在使用梯形加減速算法情況下，速度指令以及速度響應(yīng)均緩慢變化，加速度相比較之前小，機(jī)械沖擊更小。

為驗(yàn)證不同目標(biāo)位置下算法能否滿足設(shè)計(jì)要求，設(shè)定目標(biāo)位置值逐步減小。具體值根據(jù)上述類梯形加減速算法的4種位置而設(shè)置：S1= 1×107（°），S2=3×106（°），S3= -3×106（°），S2= -8×106（°）。設(shè)置初始位置為0 （°），初始速度為400 （°）/s。

伺服轉(zhuǎn)臺(tái)的測試曲線及理想曲線如圖7所示。所有曲線均從400起始，表明算法具備非零初速度規(guī)劃能力。所有曲線的速度均未出現(xiàn)突變，加速度沒有出現(xiàn)陡增現(xiàn)象。由于目標(biāo)位置為正，S1、S2的曲線全部都在正向，而S3、S4由于目標(biāo)位置為負(fù)，曲線的大部分為反向速度。S2、S3曲線由于運(yùn)動(dòng)目標(biāo)位置距離初始位置較近，所以沒有出現(xiàn)勻速段運(yùn)動(dòng)。

圖7 不同運(yùn)動(dòng)目標(biāo)的速度測試曲線

為驗(yàn)證類梯形算法在上一次運(yùn)動(dòng)規(guī)劃未完成，有殘余末速度的情況下，多次重新規(guī)劃目標(biāo)的能力，設(shè)計(jì)了4段運(yùn)動(dòng)。后3段運(yùn)動(dòng)均在上一段運(yùn)動(dòng)未執(zhí)行完畢的情況下，重新做運(yùn)動(dòng)規(guī)劃。表1記錄了每次運(yùn)動(dòng)規(guī)劃的目標(biāo)位置，以及重規(guī)劃時(shí)的運(yùn)動(dòng)狀態(tài)。該實(shí)驗(yàn)的理想曲線和實(shí)測曲線如圖8所示。曲線中S2、S3、S4處分別進(jìn)行了速度重規(guī)劃，3處初速度均不為零，但理想速度和實(shí)測速度曲線均連續(xù)，表明算法實(shí)現(xiàn)了任意運(yùn)動(dòng)階段中斷現(xiàn)有運(yùn)動(dòng)規(guī)劃，且重規(guī)劃運(yùn)動(dòng)時(shí)運(yùn)動(dòng)速度沒有突變。

表1 多次重規(guī)劃運(yùn)動(dòng)的目標(biāo)位置及狀態(tài)

圖8 中斷運(yùn)動(dòng)規(guī)劃后重規(guī)劃運(yùn)動(dòng)圖

6 結(jié)論

本文針對(duì)服務(wù)類機(jī)器人運(yùn)動(dòng)規(guī)劃需要被中斷的實(shí)際要求，在梯形加減速算法的基礎(chǔ)上提出了類梯形算法。并從機(jī)械、電氣、控制方面進(jìn)行設(shè)計(jì)，搭建了伺服轉(zhuǎn)臺(tái)。通過實(shí)驗(yàn)表明，該算法可根據(jù)目標(biāo)點(diǎn)位置不同而采取最佳運(yùn)動(dòng)策略，達(dá)到速度不突變的要求。該算法可以根據(jù)當(dāng)前運(yùn)動(dòng)狀態(tài)和目標(biāo)位置隨時(shí)重規(guī)劃運(yùn)動(dòng)，以滿足服務(wù)機(jī)器人運(yùn)動(dòng)規(guī)劃中初速度不為零的要求。