機(jī)器人軌跡規(guī)劃控制策略研究

張 霞，孫 強(qiáng)，蔡順燕，劉 張

(1.成都師范學(xué)院 物理與工程技術(shù)學(xué)院,成都 611130；2.中國科學(xué)院沈陽自動化研究所,沈陽 110016 )

0 引 言

軌跡規(guī)劃是指機(jī)器人在作業(yè)空間內(nèi)高效、快速、平穩(wěn)的完成既定任務(wù)，即有效實(shí)現(xiàn)末端執(zhí)行部件的位姿變化和調(diào)整，在空域、時(shí)域上路徑、速度及加速度的規(guī)劃?；谲壽E規(guī)劃線性函數(shù)，規(guī)劃機(jī)器人在運(yùn)動學(xué)和動力學(xué)約束條件下平穩(wěn)、光滑運(yùn)動軌跡，對實(shí)現(xiàn)機(jī)器人高精度、平穩(wěn)、高效和低功耗作業(yè)具有重要意義[1]。目前已有部分學(xué)者對上述問題獲得一系列的研究成果，針對完成“任務(wù)”形式多樣性，涌現(xiàn)出示教-再現(xiàn)運(yùn)動、關(guān)節(jié)空間運(yùn)動及空間直線曲線運(yùn)動等許多軌跡規(guī)劃及其優(yōu)化的方法。為了滿足工業(yè)機(jī)器人高效、快速響應(yīng)要求，時(shí)間最優(yōu)軌跡規(guī)劃成為熱門軌跡優(yōu)化問題并展開大量研究[2-3]。實(shí)際上，能量消耗、振動、脈動的累積等也是機(jī)器人軌跡規(guī)劃的重要性能指標(biāo)。在文獻(xiàn)[4]中，將激振力作為性能指標(biāo)，利用B樣條曲線在運(yùn)動學(xué)和動力學(xué)約束條件下進(jìn)行軌跡優(yōu)化。叢明，徐曉飛等以時(shí)間-脈動最優(yōu)為目標(biāo)，同時(shí)考慮了運(yùn)動學(xué)約束，利用三次樣條函數(shù)進(jìn)行軌跡規(guī)劃獲得了比較平滑的軌跡[5]。利用能量消耗最優(yōu)軌跡規(guī)劃算法產(chǎn)生的光滑軌跡更容易進(jìn)行跟蹤且能夠減小執(zhí)行器和機(jī)器人結(jié)構(gòu)的應(yīng)力，并且在一些能源有限的場合，可以節(jié)約能源[6]。

考慮動作時(shí)間和能量消耗最優(yōu)的路徑規(guī)劃，通常通過位移、時(shí)間樣條函數(shù)或者三次、五次多項(xiàng)式函數(shù)解算光滑軌跡，并在優(yōu)化過程中考慮速度，加速度，脈動，力矩等約束條件。文獻(xiàn)[7]中利用多目標(biāo)函數(shù)法(功效系數(shù)法)，采用直線法、折現(xiàn)法和指數(shù)法評價(jià)動作時(shí)間和能量消耗最優(yōu)解，基于機(jī)器人運(yùn)動學(xué)和動力學(xué)仿真模型，利用三次多項(xiàng)式函數(shù)生成光滑軌跡并逐步迭代優(yōu)化。文獻(xiàn)[8]同樣利用三次多項(xiàng)式函數(shù)插值規(guī)劃軌跡，因軌跡不夠平滑產(chǎn)生脈動，不連續(xù)性脈動造成空間內(nèi)多關(guān)節(jié)點(diǎn)加速度陡然增大、軌跡跟蹤誤差增大并引起關(guān)節(jié)高頻振動，脈動優(yōu)化成為平穩(wěn)完成“任務(wù)”的研究重點(diǎn)[9]。為了滿足機(jī)器人空間內(nèi)作業(yè)效率最優(yōu)、平穩(wěn)性高的特性，文獻(xiàn)[10]提出通過逆運(yùn)動學(xué)解算出與機(jī)器人多關(guān)節(jié)空間位置序列，采用7次B樣條函數(shù)插值構(gòu)造各個(gè)關(guān)節(jié)初始、終止時(shí)的運(yùn)動參數(shù)，機(jī)器人在完成啟動和停止運(yùn)動時(shí)其參數(shù)可控制，并且運(yùn)動函數(shù)指標(biāo)(速度、加速度和脈動)均連續(xù)的關(guān)節(jié)軌跡，但其優(yōu)化目標(biāo)只包括運(yùn)動總時(shí)間。

本文采用C4連續(xù)的5次均勻B樣條曲線插值關(guān)節(jié)位置序列，獲得軌跡脈動連續(xù)、關(guān)節(jié)運(yùn)動參數(shù)(速度、加速度)可控的軌跡曲線。為了解決軌跡規(guī)劃函數(shù)中存在的非線性約束問題，并且能夠獲得全局與局部最優(yōu)解，本文采用自適應(yīng)混合遺傳算法(Adaptive Hybrid Genetic Algorithm，AHGA)，綜合考慮動作時(shí)間和能量消耗，在運(yùn)動學(xué)和動力學(xué)約束下規(guī)劃出時(shí)間與能量綜合最優(yōu)且脈動連續(xù)的軌跡。

1 空間軌跡優(yōu)化

考慮機(jī)器人運(yùn)動過程需保持較高穩(wěn)定性，該文所述機(jī)器人本體采用同步帶傳動方案。該機(jī)構(gòu)形式存在剛性不足特點(diǎn)，在較大速度和加速度工作時(shí)產(chǎn)生振動，造成空間關(guān)節(jié)速度、加速度的突變[5]?；谏鲜銮闆r，本文運(yùn)用逆運(yùn)動學(xué)方程將已規(guī)劃路徑轉(zhuǎn)化為關(guān)節(jié)空間節(jié)點(diǎn)，基于五次B樣條函數(shù)構(gòu)造軌跡，實(shí)現(xiàn)空間節(jié)點(diǎn)平滑的連接，根據(jù)實(shí)際需求優(yōu)化并提高運(yùn)動快速性和功耗。由于空間關(guān)節(jié)運(yùn)動時(shí)間一致，軌跡規(guī)劃流程圖如圖1所示。

圖1 軌跡規(guī)劃流程圖

圖中，Δt為伺服控制采樣周期。

五次B樣條曲線方程為

(1)

式中，di+j-1為控制點(diǎn)，i=1,2,…,n-1，u∈(0,1)，Qi(u)為第i段B樣條曲線上的關(guān)節(jié)結(jié)點(diǎn)，Nj,5(u)為5次規(guī)范B樣條基函數(shù)。

寫成矩陣形式：

(2)

di-1,di,di+1,di+1,di+3,di+4為組控制點(diǎn)；Qi(u)為曲線上的點(diǎn)，u為參數(shù)，0≤u≤1，其上點(diǎn)的橫、縱坐標(biāo)分別用t(u)、q(u)表示.

B樣條軌跡曲線Qi(u)(其中i=1,2,…,n-1)對時(shí)間t進(jìn)行三階求導(dǎo)，獲得機(jī)構(gòu)本體的軌跡速度、加速度和脈動，函數(shù)如下：

(3)

已構(gòu)造五次均勻B樣條軌跡并不是最優(yōu)運(yùn)動軌跡，在實(shí)際應(yīng)用中，需要對其進(jìn)行優(yōu)化，使軌跡更光滑。以時(shí)間最短和能耗最小為優(yōu)化目標(biāo)，采用改進(jìn)優(yōu)化的線性加權(quán)法進(jìn)行軌跡優(yōu)化，函數(shù)如下：

綜合考慮時(shí)間和能量對軌跡的影響，采用線性加權(quán)法對已構(gòu)造樣條軌跡進(jìn)行優(yōu)化，定義目標(biāo)函數(shù)如下：

(4)

機(jī)器人關(guān)節(jié)速度、加速度、脈動和力矩極限值分別為vcj、acj、jcj和Tcj，作為約束條件對機(jī)器人運(yùn)動學(xué)和動力學(xué)指標(biāo)進(jìn)行優(yōu)化。

(1)關(guān)節(jié)速度值

(5)

j=1,2,3，i=1,2,…,n-1

(2)關(guān)節(jié)加速度值

(6)

j=1,2,3，i=1,2,…,n-1

(3)關(guān)節(jié)脈動值

(7)

j=1,2,3,i=1,2,…,n-1

(4)關(guān)節(jié)力或者力矩

(8)

j=1,2,3,i=1,2,…,n-1

2 自適應(yīng)混合遺傳算法

遺傳算法計(jì)算是基于自然界生物適者生存的規(guī)律提出的一種隨機(jī)優(yōu)化算法。計(jì)算過程簡單、高效，但在使用過程中提前收斂、無法獲取局部最優(yōu)解缺陷[6]。

優(yōu)化方法主要有微粒群算法(PSO)、差分進(jìn)化算法(DE)、基于假設(shè)檢驗(yàn)的模擬退火算法(SA)等[7]，上述方法局部搜索能力優(yōu)，但考慮無法獲取局部最優(yōu)解缺陷，將局部搜索能力優(yōu)的算法與遺傳算法相結(jié)合，提出一種混合遺傳算法，達(dá)到搜索能力提高的目的[8]。

該混合遺傳算法從隨機(jī)生成的初始群體出發(fā)，有效空間中搜索全局最優(yōu)解，算法流程如圖2所示。

圖2 算法流程圖

3 仿真結(jié)果

仿真驗(yàn)證算法參數(shù)為如下：群體規(guī)模M取50，迭代次數(shù)為200，轉(zhuǎn)化系數(shù)α=1，Pc1=0.9，Pc2=0.6，Pm1=0.1,Pm2=0.001。機(jī)器人完成既定路徑規(guī)劃并且各關(guān)節(jié)可無碰撞平穩(wěn)運(yùn)行，利用空間坐標(biāo)系變換得到的逆運(yùn)動學(xué)方程求解機(jī)器人各個(gè)關(guān)節(jié)的位置序列，如表1所示。根據(jù)第2節(jié)已確定邊界約束參數(shù)和選型的電機(jī)額定扭矩，計(jì)算出機(jī)構(gòu)本體動力學(xué)參數(shù)，如表2所示。

表1 機(jī)器人關(guān)節(jié)位置序列

表2 機(jī)器人約束

表3 SGA最優(yōu)軌跡規(guī)劃結(jié)果

表4 AHGA最優(yōu)軌跡規(guī)劃結(jié)果

經(jīng)過對表3和表4的對比可知，所述優(yōu)化后的混合遺傳算法同自適應(yīng)遺傳算法比較，局部搜索收斂能力更強(qiáng)。利用AHGA方法進(jìn)行軌跡優(yōu)化，在ω1=ω2=0.5情況下獲得三個(gè)關(guān)節(jié)的位置p、速度v、加速度a、脈動j的優(yōu)化解如圖3～5所示。圖中，位置為p，速度為v，加速度為a，脈動j。

圖3所示，仿真曲線表明在0～18s時(shí)域完成既定運(yùn)動軌跡，機(jī)器人3個(gè)關(guān)節(jié)在起始、末端速度和加速度均為零，縱坐標(biāo)所示關(guān)節(jié)速度、加速度與脈動參數(shù)曲線連續(xù)、平滑，機(jī)器人各關(guān)節(jié)軌跡跟蹤的誤差效果最優(yōu)。

圖3 優(yōu)化軌跡

4 試驗(yàn)驗(yàn)證

以硅片傳輸機(jī)器人為試驗(yàn)平臺，基于虛擬樣機(jī)驗(yàn)證軌跡規(guī)劃策略和遺傳優(yōu)化算法有效性。機(jī)器人具有三個(gè)自由度，1軸伸縮，2軸的回轉(zhuǎn)與3軸的升降，具體結(jié)構(gòu)如圖4所示。

圖4 ADAMS仿真模型

對采用五次B樣條和三次B樣條規(guī)劃的關(guān)節(jié)軌跡性能進(jìn)行試驗(yàn)對比研究。在關(guān)節(jié)伺服控制器結(jié)構(gòu)參數(shù)不變的情況下，將最優(yōu)平滑軌跡按照機(jī)器人關(guān)節(jié)伺服控制周期離散化，送入伺服控制器做軌跡跟蹤控制實(shí)驗(yàn)，關(guān)節(jié)伺服控制周期為1.28 ms。

4.1 機(jī)器人系統(tǒng)搭建

機(jī)器人系統(tǒng)主要包括機(jī)器人本體、控制系統(tǒng)、電源模塊和軟件模塊組成。

機(jī)器人運(yùn)動控制系統(tǒng)采用主從式結(jié)構(gòu)，包括上位機(jī)和下位機(jī)。上位機(jī)完成機(jī)器人運(yùn)動軌跡參數(shù)設(shè)定、上電初始化和運(yùn)動參數(shù)實(shí)時(shí)反饋。下位機(jī)進(jìn)行控制，包括硬件層面的PLC控制、軟件層面的軌跡規(guī)劃以及數(shù)據(jù)采集等功能，控制系統(tǒng)原理如圖5所示。

圖5 控制系統(tǒng)原理

機(jī)器人控制系統(tǒng)采用位置環(huán)閉環(huán)控制，編碼器實(shí)現(xiàn)位置信息反饋，通過調(diào)整微分、積分、比例環(huán)節(jié)等參數(shù)，使其滿足運(yùn)動性能。

4.2 軟件實(shí)現(xiàn)

控制系統(tǒng)軟件主要是上位機(jī)軟件的設(shè)計(jì)，本文軟件采用VC++編寫，并調(diào)用PMAC運(yùn)動控制卡帶有的運(yùn)動控制庫函數(shù)實(shí)現(xiàn)機(jī)器人的運(yùn)動控制，本文軟件具有離線優(yōu)化和在線控制的功能。

軟件的離線功能主要是用于機(jī)器人軌跡的優(yōu)化計(jì)算，在線功能是用于實(shí)時(shí)生成運(yùn)動軌跡實(shí)現(xiàn)機(jī)器人運(yùn)動控制。

人機(jī)交互界面如圖6所示，輸入?yún)^(qū)為軌跡優(yōu)化算法設(shè)計(jì)變量，型值點(diǎn)表示機(jī)器人3個(gè)關(guān)節(jié)運(yùn)動路徑的結(jié)點(diǎn)，顯示區(qū)實(shí)時(shí)顯示機(jī)構(gòu)本體3個(gè)軸的位置與速度參數(shù)，運(yùn)動程序可以完成三次和五次B樣條兩種軌跡規(guī)劃的運(yùn)動。

圖6 人機(jī)界面

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

機(jī)構(gòu)本體控制流程圖如圖7所示，根據(jù)接收的3個(gè)關(guān)節(jié)空間坐標(biāo)信息轉(zhuǎn)換成機(jī)器人的運(yùn)動軌跡，根據(jù)伺服周期將軌跡劃分為很多段運(yùn)動控制信息，并發(fā)送至下位機(jī)PMAC卡中，PMAC板將信號流發(fā)送到各關(guān)節(jié)驅(qū)動實(shí)現(xiàn)馬達(dá)按照既定要求運(yùn)動，各個(gè)關(guān)節(jié)采用閉環(huán)控制，傳動鏈末端通過編碼器完成位置信息反饋。

圖7 系統(tǒng)控制策略

仿真與實(shí)物樣機(jī)實(shí)驗(yàn)對比，三個(gè)關(guān)節(jié)的位置和角速度曲線如圖8～10所示，表明自適應(yīng)混合遺傳算法優(yōu)化的軌跡平滑性較好，具有較好的軌跡跟蹤能力。

圖8 1軸位置和角速度曲線

圖9 2軸位置和角速度曲線

圖10 3軸位置和角速度曲線

保證其它參數(shù)一致，驗(yàn)證不同插值函數(shù)計(jì)算得到的軌跡性能，本文采集軌跡跟蹤誤差并利用Matlab處理數(shù)據(jù)，3個(gè)關(guān)節(jié)軸位置跟蹤誤差絕對值曲線，如圖11所示。

error1-三次B樣條跟蹤誤差，error2-五次B樣條跟蹤誤差

通過對各個(gè)關(guān)節(jié)軌跡的觀察發(fā)現(xiàn)，最優(yōu)解可滿足既定約束參數(shù)；各關(guān)節(jié)始末速度、加速度為零，且關(guān)節(jié)速度、加速度與脈動曲線均連續(xù)、平滑，機(jī)器人關(guān)節(jié)軌跡跟蹤誤差降低到最小程度。

3個(gè)關(guān)節(jié)五次B樣條軌跡所示的位置跟蹤最大誤差分別為 ；誤差均方根分別為 ；3個(gè)關(guān)節(jié)的三次B樣條軌跡位置跟蹤最大誤差分別為 ；誤差均方根分別為 。試驗(yàn)表明軌跡規(guī)劃方法具備可有效降低軌跡跟蹤誤差優(yōu)點(diǎn)。

6 結(jié) 語

本文根據(jù)機(jī)器人工作特點(diǎn)，在考慮機(jī)器人運(yùn)動學(xué)約束條件的同時(shí)，對機(jī)器人的運(yùn)動軌跡進(jìn)行優(yōu)化，并提出一種關(guān)節(jié)空間中機(jī)器人時(shí)間與能量綜合最優(yōu)軌跡規(guī)劃方法；針對機(jī)器人關(guān)節(jié)運(yùn)動軌跡非線性約束全局與局部最優(yōu)解要求，提出一種優(yōu)化的自適應(yīng)混合遺傳算法，利用閥函數(shù)處理非線性約束問題。同時(shí)融合隨機(jī)方向搜索的方法，提高了局部搜索能力和解的質(zhì)。仿真計(jì)算和試驗(yàn)表明本文數(shù)值優(yōu)化算法可獲得性能良好運(yùn)動軌跡解算，所述軌跡規(guī)劃方法可達(dá)到提高機(jī)器人軌跡跟蹤精度的優(yōu)點(diǎn)。