類梯形加減速算法在服務(wù)機器人中的應(yīng)用研究

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

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

0 引言

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

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

1 梯形加減速算法

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

圖1 梯形加減速和三角形加減速運動圖

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

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

2 類梯形加減速算法

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

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

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

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

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

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

3 伺服試驗臺硬件設(shè)計

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

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

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

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

4 伺服試驗臺軟件設(shè)計

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

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

5 實驗與驗證

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

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

圖6 有無使用運動規(guī)劃的速度測試曲線

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

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

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

圖7 不同運動目標的速度測試曲線

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

表1 多次重規(guī)劃運動的目標位置及狀態(tài)

圖8 中斷運動規(guī)劃后重規(guī)劃運動圖

6 結(jié)論

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