基于模型補(bǔ)償?shù)臋C(jī)械臂位置跟蹤控制研究

鐘 輝，袁邦頤，，丁度坤，3，辛曼玉，鄧建新，黃秋林，

（1.廣東匯興精工智造股份有限公司，廣東 東莞523819;2.廣西大學(xué) 廣西制造系統(tǒng)與先進(jìn)制造技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣西 南寧 530003；3.東莞職業(yè)技術(shù)學(xué)院 電子信息學(xué)院，廣東 東莞 523808）

0 引言

當(dāng)前機(jī)器人在各行各業(yè)中均大放異彩，在工業(yè)生產(chǎn)過(guò)程中，為了在整個(gè)過(guò)程中實(shí)現(xiàn)不同任務(wù)下的高效率自動(dòng)化生產(chǎn)，不同自由度的機(jī)械臂在不同生產(chǎn)場(chǎng)景下均有其應(yīng)用。二關(guān)節(jié)機(jī)械臂以其結(jié)構(gòu)簡(jiǎn)單、成本低廉、控制方便等優(yōu)點(diǎn)被廣泛用于搬運(yùn)、抓取等作業(yè)中[1]，在第二桿后增加一個(gè)旋轉(zhuǎn)和平移關(guān)節(jié)后就形成了常見的Scara機(jī)器人，可實(shí)現(xiàn)抓取、分揀等功能，如圖1所示。

圖1 分揀機(jī)器人

隨著工作任務(wù)需求的不斷提高，如精密裝配、打磨加工等場(chǎng)景，對(duì)機(jī)器人位置和路徑跟蹤精度要求也越來(lái)越高?？刂破鞯母櫺阅苁怯绊憴C(jī)械臂位置跟蹤精度的主要因素，所以，如何設(shè)計(jì)一個(gè)反應(yīng)迅速和跟蹤精確的控制器顯得尤為重要。文獻(xiàn)[2]針對(duì)二連桿機(jī)械臂的運(yùn)動(dòng)控制問題提出了一種基于深度強(qiáng)化學(xué)習(xí)的控制方法。文獻(xiàn)[3]針對(duì)用于三自由度機(jī)械臂各關(guān)節(jié)末端軌跡跟蹤控制的非線性系統(tǒng)在擾動(dòng)存在的情況下跟蹤效率較低的問題，提出一種變?cè)鲆娴鷮W(xué)習(xí)控制律。文獻(xiàn)[4]提出了一種自適應(yīng)模糊滑模控制算法。采用自適應(yīng)單輸入單輸出模糊系統(tǒng)來(lái)計(jì)算控制增益。雖然以上方法對(duì)不確定模型下的機(jī)械臂位置跟蹤有較好的效果，但均存在收斂慢、跟蹤性能較一般的問題。本研究意在針對(duì)模型已知的情況下設(shè)計(jì)一種控制器，使機(jī)械臂能快速準(zhǔn)確地跟蹤目標(biāo)軌跡。

1 機(jī)械臂控制方法

1.1 PID控制

1936年Callender和Stevenson等給出了比例積分微分（Proportion Integration Differentiation，PID）調(diào)節(jié)器的算法。此后不斷有新的控制算法誕生，但仍無(wú)法撼動(dòng)PID算法在控制領(lǐng)域的地位。當(dāng)前其已經(jīng)應(yīng)用于90%～95%的控制電路中，它具有簡(jiǎn)單、實(shí)用、魯棒性強(qiáng)等優(yōu)點(diǎn)[5-7]。在實(shí)際應(yīng)用中，一個(gè)簡(jiǎn)單的線性解耦的PID反饋控制器可以在不引起任何穩(wěn)態(tài)誤差的情況下得到期望的位置。

基本的連續(xù)PID控制公式如下[6]：

其中Kp為比例系數(shù)，Ti為積分時(shí)間常數(shù)，Td為微分時(shí)間常數(shù)，e（t）為目標(biāo)值宇實(shí)際值的偏差，t為時(shí)間。

其傳遞函數(shù)為：

對(duì)于PID策略，用戶也可取消部分環(huán)節(jié)如（積分或微分環(huán)節(jié)）來(lái)針對(duì)不同系統(tǒng)進(jìn)行控制，由此衍生出PD、PI等控制算法，各有其優(yōu)點(diǎn)，不同的控制策略適用于不同的控制系統(tǒng)，調(diào)節(jié)偏差快速變化時(shí)使調(diào)解量在最短的時(shí)間內(nèi)得到強(qiáng)化，有調(diào)節(jié)靜差，適合調(diào)節(jié)滯后環(huán)節(jié)。例如機(jī)械臂的位置跟蹤，由于積分環(huán)節(jié)會(huì)在末端收斂時(shí)產(chǎn)生震蕩現(xiàn)象，導(dǎo)致收斂慢，為達(dá)快速收斂的效果，適合使用PD控制器。

1.2 模型偏差補(bǔ)償與滑?？刂芐MC簡(jiǎn)介

對(duì)于許多控制系統(tǒng)，因?yàn)閺?fù)雜性或技術(shù)問題，常常無(wú)法建立精確的模型，但是這會(huì)導(dǎo)致控制效果與預(yù)期有較大差距，因此對(duì)不確定的模型進(jìn)行適當(dāng)?shù)难a(bǔ)償具有很大的實(shí)際意義[9]，模型偏差補(bǔ)償是一個(gè)思想，通過(guò)對(duì)誤差構(gòu)成的分析，可以得到一類時(shí)變非線性系統(tǒng)的連續(xù)軌跡跟蹤問題的模型偏差補(bǔ)償控制方案，可以從仿真的角度得到最好的跟蹤效果。

可變結(jié)構(gòu)控制（Variable Structure Control，VSC）與滑模控制（Sliding Mode Control，SMC）已廣泛應(yīng)用于具有不確定性和或輸入耦合的系統(tǒng)。其設(shè)計(jì)理念是獲得一個(gè)頻繁切換控制律，將系統(tǒng)驅(qū)動(dòng)到一個(gè)指定的和用戶選擇的表面上，稱為滑動(dòng)表面。當(dāng)系統(tǒng)處于滑動(dòng)模式時(shí)，其動(dòng)力學(xué)嚴(yán)格由滑動(dòng)表面的動(dòng)力學(xué)決定，因此對(duì)參數(shù)變化和系統(tǒng)擾動(dòng)不敏感，魯棒性強(qiáng)，滑?？刂屏硪粋€(gè)優(yōu)點(diǎn)就是對(duì)非線性系統(tǒng)具有良好的控制效果。

機(jī)器人作為一個(gè)復(fù)雜的、時(shí)變的、非線性的、具有很多建模不確定性項(xiàng)的系統(tǒng)，單一地PID控已經(jīng)無(wú)法滿足其路徑跟蹤需求，為了提升PID控制的魯棒性，本研究將傳統(tǒng)的比例微分（Proportion Differentiation，PD）控制與模型偏差補(bǔ)償相結(jié)合，設(shè)計(jì)了基于模型補(bǔ)償?shù)臋C(jī)械臂PD型控制器。

圖2 二關(guān)節(jié)機(jī)械臂模型

2 機(jī)械臂模型

忽略關(guān)節(jié)間的摩擦力和其他干擾力，n關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)方程為：

其中q為n×1的關(guān)節(jié)位置向量，M（q）為n×n的機(jī)械臂慣性矩陣，C（q，）為n×n的離心力與科氏力結(jié)合矩陣，G（q）為n×1的重力矩陣，τ為各關(guān)節(jié)電機(jī)輸出的力矩。

機(jī)械臂系統(tǒng)動(dòng)力學(xué)有以下特性：（q）-2C（q，）是一個(gè)斜對(duì)稱矩陣，xT[（q）-2C（q，）]x= 0；慣性矩陣M（q）是對(duì)稱正定矩陣[11]。

由于機(jī)械臂相鄰關(guān)節(jié)間存在耦合，無(wú)法用一般方法對(duì)其進(jìn)行動(dòng)力學(xué)建模，當(dāng)前常用的二關(guān)節(jié)機(jī)械臂動(dòng)力學(xué)模型的構(gòu)建方法主要有：拉格朗日法（Lagrange）、牛頓-歐拉法（Newton-Euler）、凱恩法（Kane）等方法。其中拉格朗日法利用系統(tǒng)能量的微分求出力和力矩，適用于較為復(fù)雜條件下的系統(tǒng)建模。

根據(jù)拉格朗日法可獲取機(jī)器人動(dòng)力學(xué)參數(shù)，表示為：

表1 二關(guān)節(jié)機(jī)械臂物理參數(shù)表

其中m1為關(guān)節(jié)1質(zhì)量，l1為關(guān)節(jié)一的長(zhǎng)度，lc1為關(guān)節(jié)1質(zhì)量中心距第一關(guān)節(jié)長(zhǎng)度，I1為關(guān)節(jié)一的轉(zhuǎn)動(dòng)慣量，me為質(zhì)量，lce為質(zhì)量中心距第二關(guān)節(jié)距離，δe為質(zhì)量中心與第二桿之間的夾角。

3 基于模型補(bǔ)償?shù)腜D機(jī)械手控制器設(shè)計(jì)

S函數(shù)（S-Function）是一種由用戶自定義功能的Simulink模塊。利用S-function也可以定義自己的微分方程、差分方程、離散系統(tǒng)方程。當(dāng)Simulink自帶模塊不足以滿足需求時(shí)，我們可以利用S函數(shù)來(lái)編寫所需的功能模塊，從而擴(kuò)展Simulink的功能。

S函數(shù)相當(dāng)于在Matlab中提供了一個(gè)模板程序，用戶通過(guò)修改模板程序，根據(jù)需求在各個(gè)子函數(shù)中編寫對(duì)應(yīng)程序，即可自定義S函數(shù)模塊的功能[10]。

S函數(shù)通過(guò)flag標(biāo)志來(lái)控制運(yùn)行順序，共包含6個(gè)子程序，分別是：

①mdlInitializeSizes初始化；

②mdlDerivatives（t，x，u）計(jì)算連續(xù)狀態(tài)的微分；

③mdlUpdate（t，x，u）更新離散狀態(tài)、采樣時(shí)間與最大步長(zhǎng)；

④mdlOutputs（t，x，u）計(jì)算S函數(shù)輸出；

⑤mdlGetTimeOfNextVarHit（t，x，u）計(jì)算下一個(gè)采樣時(shí)間；

⑥mdlTerminate（t，x，u）。

根據(jù)Lyapunov第二方法來(lái)設(shè)計(jì)控制器，可以定義機(jī)械臂位置誤差為，其中q=[q1q2]T，q=[q1q2]T。Λ為一個(gè)2×2的正的常數(shù)比例矩陣，-

設(shè)計(jì)基于模型補(bǔ)償?shù)腜D型控制律為：

其中KD為正定的微分矩陣。

構(gòu)造Lyapunov函數(shù)：

對(duì)其求導(dǎo)得：

將 式（3）（4）代入上式可得：

由前文可知：

xT[]x，KD為正定的微分矩陣，則有：

所以V˙（t）為半負(fù)定的，當(dāng)且僅當(dāng)s=0時(shí)，V˙（t）=0，根據(jù)LaSalle[11]定理可知系統(tǒng)收斂。

4 基于Matlab/Simulink的仿真

為驗(yàn)證以上控制模型，在Matlab2020b中搭建了控制系統(tǒng)的仿真模型，并同時(shí)搭建僅基于PD控制的二關(guān)節(jié)機(jī)械臂位置控制仿真模型作為對(duì)照。使二關(guān)節(jié)機(jī)械臂按照期望位置qd=[sin（2πt）sin（2πt）]T進(jìn)行運(yùn)動(dòng)。

經(jīng)過(guò)參數(shù)整定后，設(shè)計(jì)用于對(duì)照的基于PD控制的二關(guān)節(jié)機(jī)械臂位置控制器參數(shù)為，

能達(dá)到較好的控制效果。

圖3 關(guān)節(jié)1的位置和速度跟蹤

圖5 關(guān)節(jié)力矩輸出

圖4 關(guān)節(jié)2的位置和速度跟蹤

從以上仿真結(jié)果可以看出，基于模型補(bǔ)償?shù)腜D型控制方法位置跟蹤收斂時(shí)間在0.4 s，而傳統(tǒng)PD控制則是1.9 s左右，且穩(wěn)態(tài)跟蹤誤差可以精確到控制到1%以內(nèi)，而傳統(tǒng)PD控制關(guān)節(jié)1位置跟蹤則存在滯后性，關(guān)節(jié)2位置跟蹤誤差則達(dá)6%。無(wú)論是調(diào)節(jié)時(shí)間還是穩(wěn)態(tài)誤差，都是所提方法跟蹤性能更好。綜上，所設(shè)計(jì)的控制器對(duì)兩個(gè)關(guān)節(jié)的位置和速度跟蹤性能都是令人滿意的，這表明控制器能夠達(dá)到控制要求。

5 結(jié)語(yǔ)

通過(guò)PD控制與滑?？刂葡嘟Y(jié)合設(shè)計(jì)的基于模型補(bǔ)償?shù)臋C(jī)械手PD控制器，并在Matlab/simulink中建立仿真模型，采用S函數(shù)編寫各個(gè)模塊進(jìn)行仿真，同時(shí)建立傳統(tǒng)的機(jī)械臂PD位置控制模型，與所設(shè)計(jì)的控制器進(jìn)行對(duì)比，仿真結(jié)果表，該控制器跟蹤性能優(yōu)于傳統(tǒng)的PD控制器，能夠很好地跟蹤預(yù)定的關(guān)節(jié)軌跡。