基于優(yōu)化ADRC的單臂機(jī)器人軌跡跟蹤研究

陳 壯，王憲倫，陳 閃

(1.芯恩(青島)集成電路有限公司，山東 青島 266000;2.青島科技大學(xué)機(jī)電工程學(xué)院，山東 青島 266061;3.青島濱海學(xué)院機(jī)電工程學(xué)院，山東 青島 266555)

0 引言

機(jī)器人抓取物體的工作看似簡(jiǎn)單，實(shí)際上是多耦合的非線性軌跡解算過(guò)程[1]。對(duì)于機(jī)器人末端執(zhí)行器的軌跡規(guī)劃，一般將末端執(zhí)行器的笛卡爾坐標(biāo)系由運(yùn)動(dòng)學(xué)算法映射到關(guān)節(jié)空間中，解算并擬合得到理想的抓取角位移軌跡和角加速度，將關(guān)節(jié)變量轉(zhuǎn)換成脈沖值發(fā)送給伺服電機(jī)完成各軸的協(xié)同動(dòng)作。但是實(shí)際上機(jī)器人常處于微振動(dòng)的環(huán)境中，不可避免地受到自身共振和地面振動(dòng)的不確定撓動(dòng)，導(dǎo)致抓取軌跡與擬合的軌跡偏離。為了使機(jī)器人高精度跟蹤抓取軌跡，需要在已知機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型的前提下構(gòu)造軌跡跟蹤控制系統(tǒng)。

Yu等[2]為了減輕內(nèi)外部的干擾耦合對(duì)無(wú)人直升機(jī)螺旋槳操控的影響，建立了ADRC自抗撓模型，對(duì)比PID控制算法，提高了系統(tǒng)的動(dòng)態(tài)穩(wěn)定性。Wang等[3]將自抗撓模型應(yīng)用于交流電，估計(jì)逆變器的周期性負(fù)載擾動(dòng)。廖薇等[4]構(gòu)造了ADRC模型使得海洋絞車適用于不同線纜直徑和重量的工況，即使當(dāng)環(huán)境變化時(shí)，控制器也能保持良好的動(dòng)態(tài)性能，對(duì)比了PID控制器，證明了ADRC有較快的響應(yīng)速度和抗干擾能力。

自抗擾控制(ADRC)算法是在PID控制理論基礎(chǔ)上延伸發(fā)展而來(lái)的[5]，能自主抵抗機(jī)器人內(nèi)外部的各類干擾，具備高精度的軌跡跟蹤能力，并且其關(guān)鍵參數(shù)也可以根據(jù)實(shí)際跟蹤能力進(jìn)一步優(yōu)化。

1 關(guān)節(jié)空間動(dòng)力學(xué)模型

實(shí)驗(yàn)用的是加拿大Kinova公司研發(fā)的MICO2仿人六軸串聯(lián)機(jī)器人，在關(guān)節(jié)空間下對(duì)應(yīng)位置和時(shí)間的拉格朗日動(dòng)力學(xué)方程的一般形式為

(1)

(2)

(3)

2 建立ADRC模型

標(biāo)準(zhǔn)二階ADRC由微分跟蹤器(TD)、非線性狀態(tài)誤差反饋(NLSEF)和擴(kuò)張狀態(tài)觀測(cè)器(ESO)獨(dú)立的3部分組成，如圖1所示。變量q為機(jī)器人各關(guān)節(jié)軸輸出的角位移，設(shè)定變量qc為機(jī)器人關(guān)節(jié)擬合軌跡的任意位置，變量qc1為輸入軌跡qc的跟蹤信號(hào)，qc2為輸入軌跡qc微分后速度的跟蹤信號(hào)，它們由ESO實(shí)時(shí)提取跟蹤值qc1和qc2，從而確保跟蹤值的穩(wěn)定性。κ為系統(tǒng)內(nèi)外的不確定干擾；τ為考慮內(nèi)外干擾引入的系統(tǒng)補(bǔ)償量。

圖1 二階ADRC框架模型

跟蹤值qc1和qc2的二階非線性跟蹤微分器TD函數(shù)形式為

(4)

k=fh(qc1-q，qc2,r0,μ),r為跟蹤速度控制因子，一般|k|≤r；r0為r的整數(shù)倍，一般按實(shí)際控制需求選擇合適的倍數(shù)；h為軌跡采樣周期，μ為噪聲濾波控制因子，一般為h的20倍；fh為控制反饋跟蹤函數(shù)。

ESO是自抗擾控制器的核心，用于跟蹤變量，估計(jì)并補(bǔ)償不確定撓動(dòng)以消除系統(tǒng)噪聲，這也是PID沒(méi)有的功能。設(shè)變量z1為qc1的估計(jì)值，變量z2為qc2的估計(jì)值，變量z3為擴(kuò)張變態(tài)變量。β1、β2、β3為ESO內(nèi)部參數(shù)，構(gòu)建ESO算法為

(5)

至此，已經(jīng)通過(guò)ESO估計(jì)了機(jī)器人內(nèi)外不確定撓動(dòng)，為了使撓動(dòng)的抵消變得可控，引入非線性狀態(tài)誤差反饋(NLSEF)。NLSEF是自抗擾控制器特有的組成部分，流程如圖2所示，用于調(diào)整增益限制系統(tǒng)撓動(dòng)，調(diào)節(jié)速度在較小的范圍內(nèi)波動(dòng)從而避免超調(diào)。已知TD跟蹤得到的角位移qc1和速度qc2，以及ESO得到軌跡估計(jì)值z(mì)1和速度估計(jì)值z(mì)2。對(duì)應(yīng)數(shù)值做差法運(yùn)算得到跟蹤誤差e1和e2。結(jié)合控制反饋跟蹤函數(shù)fh，得到NLSEF表達(dá)式為

(6)

c為阻尼系數(shù)。

圖2 NLSEF計(jì)算流程

3 軌跡跟蹤仿真

外部干擾對(duì)軌跡的影響，主要體現(xiàn)在末端執(zhí)行器角位移變量的瞬時(shí)幅值存在噪聲且角加速度響應(yīng)延遲[6-7]。選擇Kinova MICO2的第3關(guān)節(jié)軸跟蹤已規(guī)劃的關(guān)節(jié)角位移變量q為例，驗(yàn)證二階ADRC設(shè)計(jì)的可行性。ADRC跟蹤結(jié)果如圖3所示。由圖3可知，當(dāng)存在干擾時(shí)，第3關(guān)節(jié)軸反饋的實(shí)際關(guān)節(jié)角位移受噪聲干擾的影響較明顯，明顯偏離理想的規(guī)劃軌跡，而經(jīng)過(guò)ADRC估計(jì)并補(bǔ)償擾動(dòng)后使得跟蹤控制的角位移能夠近似向規(guī)劃的軌跡趨近。

圖3 第3關(guān)節(jié)關(guān)節(jié)角位移軌跡

4 ADRC參數(shù)優(yōu)化

ADRC算法在運(yùn)算效率和跟蹤干擾等方面都優(yōu)于PID算法，甚至不依賴于數(shù)學(xué)模型。各部分參數(shù)的優(yōu)化起著至關(guān)重要的作用?？紤]到閉環(huán)系統(tǒng)的動(dòng)態(tài)特性，為了避免因數(shù)據(jù)量較大而造成誤差信號(hào)時(shí)滯現(xiàn)象，需要根據(jù)誤差補(bǔ)償值動(dòng)態(tài)微調(diào)ESO的內(nèi)部參數(shù)β1、β2、β3以及NLSEF的參數(shù)kp。參數(shù)kp的選取影響著目標(biāo)值跟蹤精度，過(guò)大會(huì)出現(xiàn)超調(diào)現(xiàn)象，因此需要從非線性控制率層面抑制系統(tǒng)超調(diào)，減小振蕩。

進(jìn)一步解算kp的最優(yōu)解，設(shè)計(jì)了一種基于遺傳算法優(yōu)化徑向基網(wǎng)絡(luò)算法(GA-RBF)實(shí)時(shí)優(yōu)化ADRC。RBF神經(jīng)網(wǎng)絡(luò)能精準(zhǔn)快速逼近設(shè)定值，同時(shí)由遺傳算法(GA)的強(qiáng)魯棒性并行搜索優(yōu)化徑向基函數(shù)(RBF)的網(wǎng)絡(luò)權(quán)值并避免局部極小值，盡可能完善RBF神經(jīng)網(wǎng)絡(luò)[8-9]。RBF高斯逼近激活函數(shù)為

(7)

隱含層激活函數(shù)φ(‖x-ui‖)一般為

φ(‖x-ui‖)=exp(-‖x-ui‖2/σ2)

(8)

‖x-ui‖為基函數(shù)與輸入矩陣的差向量；g矩陣為權(quán)值；σ為函數(shù)寬度閾值參數(shù)，影響RBF徑向范圍；p為神經(jīng)元數(shù)量；ui為第i個(gè)節(jié)點(diǎn)的聚類中心。

通常采用GA并行全局搜索RBF的最優(yōu)解。GA優(yōu)化RBF網(wǎng)絡(luò)的權(quán)值流程如圖4所示。設(shè)定二進(jìn)制編碼，迭代種群規(guī)模為N，借鑒生物繁殖理念，選擇過(guò)程則以評(píng)價(jià)染色體的方式擇優(yōu)交叉組合產(chǎn)生新的后代，交叉概率和變異概率則跟隨適應(yīng)度改變。當(dāng)滿足迭代閾值時(shí)，停止迭代并輸出此時(shí)種群中最大適應(yīng)度的個(gè)體作為最優(yōu)權(quán)值。網(wǎng)絡(luò)權(quán)值預(yù)測(cè)數(shù)值變化情況如圖5所示。

圖4 GA優(yōu)化RBF網(wǎng)絡(luò)的權(quán)值流程

圖5 網(wǎng)絡(luò)權(quán)值預(yù)測(cè)數(shù)值變化

由圖5可知，在迭代初期，RBF的網(wǎng)絡(luò)權(quán)值g有較大的波動(dòng)，使得預(yù)測(cè)值不可靠，隨著迭代進(jìn)化次數(shù)的增加使得權(quán)值預(yù)測(cè)值逐步趨于穩(wěn)定。這也意味著GA預(yù)測(cè)的最小誤差也跟隨迭代進(jìn)化次數(shù)的增加逐步收斂至0。使得在當(dāng)前ADRC跟蹤模型下，預(yù)測(cè)了合適的RBF網(wǎng)絡(luò)權(quán)值g，通過(guò)RBF網(wǎng)絡(luò)優(yōu)化了NLSEF，微調(diào)參數(shù)kp，最終實(shí)現(xiàn)更高的逼近精度。

5 實(shí)驗(yàn)誤差分析

第3關(guān)節(jié)角位移變量q由GA-RBF優(yōu)化后的ADRC跟蹤結(jié)果與規(guī)劃軌跡作差，得到絕對(duì)誤差偏移量δ，如圖6所示。通過(guò)偏移幅值清楚地看出該關(guān)節(jié)的各離散點(diǎn)的跟蹤誤差臨界波動(dòng)較小，角位移絕對(duì)誤差偏移量δ基本上能控制在-0.25～0.25 rad范圍內(nèi)，且誤差可控。從而將算法映射到機(jī)器人的6個(gè)關(guān)節(jié)軸，各軸協(xié)同工作使得機(jī)器人末端執(zhí)行器實(shí)時(shí)跟隨已規(guī)劃的軌跡工作。

圖6 ADRC跟蹤誤差

6 結(jié)束語(yǔ)

機(jī)器人普遍存在來(lái)自內(nèi)外界的不確定性干擾，為了使得機(jī)器人更好地遵循規(guī)劃好的軌跡運(yùn)行。本文以Kinova MICO2為例，在關(guān)節(jié)空間下建立了ADRC關(guān)節(jié)角位移跟蹤模型。為了確保ADRC適用于各類復(fù)雜環(huán)境，保證其閉環(huán)誤差跟蹤穩(wěn)定性。建立GA-RBF網(wǎng)絡(luò)進(jìn)一步優(yōu)化了NLSEF參數(shù)kp，控制角位移跟蹤誤差在一定時(shí)間內(nèi)穩(wěn)定在可接受的范圍內(nèi)，表明GA-RBF優(yōu)化的ADRC使機(jī)器人實(shí)際工作軌跡向著規(guī)劃軌跡收斂。