結(jié)合PID與狀態(tài)觀測器的欠驅(qū)動(dòng)機(jī)械手末端控制

陳 靜，趙 晶

(長春電子科技學(xué)院機(jī)電工程學(xué)院，吉林長春130000；)

1 引言

近年來，機(jī)械手裝置逐漸應(yīng)用在軍事、醫(yī)療、食品、海洋等諸多行業(yè)。機(jī)械手包含了機(jī)械、控制和人工智能等多方面技術(shù)，是一種高科技自動(dòng)化生產(chǎn)設(shè)備[1-2]。由于械手末端夾的多樣性和多功能等特點(diǎn)，機(jī)械手末端表現(xiàn)出欠驅(qū)動(dòng)特征。大量學(xué)者通過構(gòu)建機(jī)械手的結(jié)構(gòu)模型，實(shí)現(xiàn)對機(jī)械手末端的控制。文獻(xiàn)[3]建立機(jī)械手的姿態(tài)空間模型，基于蜂群粒子群優(yōu)化算法求解出慣性權(quán)值和最優(yōu)粒子值，為了增強(qiáng)機(jī)械手的尋優(yōu)性能，分類中間值，并對慣性權(quán)重進(jìn)行權(quán)衡。實(shí)驗(yàn)結(jié)果表明，該算法可以使機(jī)械手具有較強(qiáng)的搜索能力，但算法的求解速度有待提高。文獻(xiàn)[4]采用改進(jìn)的神經(jīng)網(wǎng)絡(luò)PID控制器，構(gòu)建出機(jī)械手平面簡圖，并推導(dǎo)出機(jī)械手末端的運(yùn)動(dòng)狀態(tài)，得出了機(jī)械手的在線控制流程圖。實(shí)驗(yàn)結(jié)果表明，該算法可以提高對機(jī)械手末端執(zhí)行器軌跡跟蹤的精度，但推導(dǎo)過程較為繁瑣。文獻(xiàn)[5]提出基于改進(jìn)的模糊PID控制方法，結(jié)合機(jī)械手運(yùn)動(dòng)方程，構(gòu)建串聯(lián)機(jī)械手控制系統(tǒng)，控制機(jī)械手的角位移，實(shí)驗(yàn)結(jié)果表明，該方法適應(yīng)環(huán)境較強(qiáng)，能夠完成PID控制器參數(shù)的在線調(diào)解，然而搜索速度較慢，控制系統(tǒng)的輸出精度不高。

針對以上研究成果，本文提出結(jié)合PID與狀態(tài)觀測器的欠驅(qū)動(dòng)機(jī)械手末端控制方法。通過慣性權(quán)重獲取PID的最佳響應(yīng)參數(shù)，采用狀態(tài)觀測器完成對機(jī)械手狀態(tài)變量以及模型不確定因素的實(shí)時(shí)跟蹤。

2 PID控制器設(shè)計(jì)

PID廣泛應(yīng)用于工業(yè)控制系統(tǒng)中，由于在各種不同性能需求下，PID控制都可以滿足要求且容易實(shí)現(xiàn)，因此是一種非常普遍的控制算法。PID控制器通過對比例、積分、微分參數(shù)的調(diào)節(jié)，獲得最佳響應(yīng)值，完成控制系統(tǒng)的在線控制[6]，PID控制結(jié)構(gòu)如圖1所示。

圖1 PID控制結(jié)構(gòu)圖

PID控制微分方程[7]可表示為

(1)

其中：e(t)表示輸出誤差，且e(t)=r(t)-y(t)；Kpro表示比例系數(shù)；Kint表示積分系數(shù)；Kdif表示微分系數(shù)。通過誤差積分性能函數(shù)，提高PID控制對機(jī)械手的輸出精度，當(dāng)PID控制器的各個(gè)參數(shù)達(dá)到最優(yōu)解時(shí)，誤差積分函數(shù)的值最小，精度最高，誤差積分性能評價(jià)函數(shù)可表示為

(2)

為了方便調(diào)節(jié)PID各個(gè)參數(shù)更快地達(dá)到最優(yōu)狀態(tài)，本文引入慣性權(quán)重gval，通過粒子群的差異狀態(tài)分類選擇不同的慣性權(quán)重，公式表示為

(3)

(4)

3 機(jī)械手末端狀態(tài)觀測器設(shè)計(jì)

狀態(tài)觀測器是一種參數(shù)調(diào)節(jié)簡單、能夠抵制外部干擾的控制器核心組件[8]。狀態(tài)觀測器可以對狀態(tài)變量和模型的不確定因素進(jìn)行實(shí)時(shí)跟蹤。為了更好的模擬機(jī)械手系統(tǒng)，需構(gòu)建常量已知、擾動(dòng)變量未知的不確定非線性系統(tǒng)，公式表示為

+ζun(t)+a0u(t)

(5)

其中，F(xiàn)un[·]表示機(jī)械手系統(tǒng)中各個(gè)狀態(tài)的未知變化參數(shù)；ζun(t)表示機(jī)械手系統(tǒng)中的未知擾動(dòng)變量；u(t)表示機(jī)械手系統(tǒng)中的控制變量；a0表示常數(shù)。令

(6)

其中，A(t)表示未知函數(shù)。構(gòu)建非線性系統(tǒng)，公式表示為

(7)

其中，lnon_1(enon_1)，…，lnon_n+1(enon_1)表示非線性系統(tǒng)的連續(xù)函數(shù)，由式(6)和式(7)聯(lián)立可得

(8)

其中，enon_i(t)=znon_i(t)-xun_non_i(t)。當(dāng)非線性函數(shù)滿足enon_1·li(enon_i)>0時(shí)，對于在一定范圍內(nèi)任意變化的未知函數(shù)A(t)，系統(tǒng)都可以維持穩(wěn)定狀態(tài)。在此系統(tǒng)的基礎(chǔ)上，將機(jī)械手的系統(tǒng)函數(shù)更新為狀態(tài)變量形式，公式可表示為

(9)

(10)

其中，α1、α2、α3、α4表示合適的參數(shù)值，只要對這四個(gè)參數(shù)值進(jìn)行合適地選擇，系統(tǒng)便可以精確地估計(jì)出狀態(tài)變量，若系統(tǒng)存在擾動(dòng)，則可在實(shí)時(shí)作用量中引入擾動(dòng)量ξ(t)，其公式表示為

xun_non_i4=Fun[xun_non_i1(t)，xun_non_i2(t)，ξ(t)]

(11)

綜上所述，只要作用量xun_non_i4有界，并通過選擇合適的參數(shù)值，系統(tǒng)的狀態(tài)觀測器便可以很好地預(yù)測出機(jī)械手的狀態(tài)變量。

4 欠驅(qū)動(dòng)機(jī)械手力學(xué)分析

目前對于抓取工作的機(jī)械手而言，一般都采用全驅(qū)動(dòng)的設(shè)計(jì)方式，這樣不僅控制較為復(fù)雜，還造成資源浪費(fèi)。針對這一問題，本文提出了一種基于欠驅(qū)動(dòng)(驅(qū)動(dòng)單元的個(gè)數(shù)小于所對應(yīng)的自由度個(gè)數(shù))的機(jī)械手，雖然欠驅(qū)動(dòng)控制方式中的驅(qū)動(dòng)個(gè)數(shù)小于自由度個(gè)數(shù)，但是仍然可以在相應(yīng)約束條件下進(jìn)行正常工作，不僅滿足了結(jié)構(gòu)簡單的特點(diǎn)，還具有較好的適應(yīng)性。

欠驅(qū)動(dòng)機(jī)械手末端的控制過程比較復(fù)雜，欠驅(qū)動(dòng)機(jī)械手由兩組串聯(lián)的四連桿構(gòu)成，可完成對物體的抓取工作。然而精準(zhǔn)的抓取動(dòng)作主要由遠(yuǎn)指節(jié)控制實(shí)現(xiàn)，在不考慮摩擦力影響時(shí)，機(jī)械手末端的力矩平衡原理可簡化為

(12)

其中，F(xiàn)表示機(jī)械手末端與物體之間的接觸力；lnear表示機(jī)械手末端離物體較近的近指節(jié)長度；lfast表示機(jī)械手末端離物體較遠(yuǎn)的遠(yuǎn)指節(jié)長度；φ表示連桿的轉(zhuǎn)角；dfast_mid表示遠(yuǎn)指節(jié)和中指節(jié)關(guān)節(jié)點(diǎn)之間的距離；Tjoint表示機(jī)械手末端各個(gè)關(guān)節(jié)的力矩。機(jī)械手末端抓取力和驅(qū)動(dòng)力矩一一對應(yīng)，因此可以通過控制驅(qū)動(dòng)力矩實(shí)現(xiàn)對抓取力的控制。然而在欠驅(qū)動(dòng)機(jī)械手控制過程中存在靜力學(xué)問題，為了使力學(xué)系統(tǒng)保持靜平衡，本文引入虛功原理，公式為

(13)

(14)

(15)

其中，dF_i表示接觸力距離各個(gè)關(guān)節(jié)點(diǎn)間的距離。于是接觸力可以進(jìn)一步表示為

(16)

圖2 四連桿機(jī)構(gòu)

假設(shè)四連桿的各個(gè)長度分別為l1、l2、l3、l4，那么四連桿的分量形式可表示為

(17)

欠驅(qū)動(dòng)機(jī)械手的驅(qū)動(dòng)力矩Tdri是通過電動(dòng)機(jī)產(chǎn)生的轉(zhuǎn)矩傳遞給絲杠螺母，再通過連桿傳遞給機(jī)械手末端。絲杠螺母產(chǎn)生的力用Fscr表示；連接桿上傳遞的力用Ftra表示；機(jī)械手末端的旋轉(zhuǎn)點(diǎn)到連接桿的距離用drot_joi表示。由力矩平衡原理可得

Tdri=Ftra·drot_joi

(18)

由絲杠螺母的推力公式[9]有

(19)

其中，ηtra表示絲杠螺母的傳遞效率；Tout表示電動(dòng)機(jī)產(chǎn)生的力矩；dscr表示絲杠的螺距。對連接桿進(jìn)行受力分析，可以得出

Fscr=Ftracosθ

(20)

將以上公式聯(lián)立可以得出

(21)

綜上所述，在電動(dòng)機(jī)的輸出力矩已知的情況下，欠驅(qū)動(dòng)機(jī)械手末端進(jìn)行抓取工作時(shí)，可以通過公式精確地計(jì)算出各個(gè)關(guān)節(jié)的抓取力，完成對欠驅(qū)動(dòng)機(jī)械手末端的精準(zhǔn)控制。

5 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證PID與狀態(tài)觀測器相結(jié)合方法對機(jī)械手末端的控制情況，本文基于MATLAB仿真軟件進(jìn)行實(shí)驗(yàn)測試。首先通過對PID參數(shù)不斷仿真，得出理想的參數(shù)值，如表1所示。將目標(biāo)函數(shù)設(shè)置為cos2t、擾動(dòng)設(shè)置為100cost，仿真機(jī)械手末端在空間坐標(biāo)系中的跟蹤控制情況，并將本文算法與普通的PID控制方法進(jìn)行實(shí)驗(yàn)對比。仿真結(jié)果如圖3所示。

表1 PID理想?yún)?shù)表

圖3的a)～c)圖中，從上至下三條曲線依次為實(shí)際信號、本文跟蹤曲線和普通跟蹤曲線。對比可知，采用普通PID控制方法，機(jī)械手末端對三個(gè)坐標(biāo)軸的跟蹤效果不理想，相對來說x軸和y軸的跟蹤效果還可以，但是z軸方向基本跟蹤不上目標(biāo)函數(shù)，存在明顯的相位偏移和幅值偏差。采用本文方法的機(jī)械手末端能夠較好的跟蹤目標(biāo)函數(shù)，不僅x軸和y軸跟蹤效果良好，z軸也可以很好的跟蹤目標(biāo)函數(shù)，跟蹤誤差很小，相角和幅值誤差都得到顯著優(yōu)化。

圖3 控制跟蹤曲線對比

為了進(jìn)一步研究機(jī)械手末端抓取的運(yùn)動(dòng)規(guī)律，本文選擇邊長為30mm的方塊作為機(jī)械手的抓取對象。在不考慮各個(gè)關(guān)節(jié)間摩擦力的情況下，分析機(jī)械手的受力規(guī)律。圖4為遠(yuǎn)指節(jié)運(yùn)動(dòng)的速度仿真曲線。

圖4 遠(yuǎn)指節(jié)運(yùn)動(dòng)速度曲線

從圖中可以看出，當(dāng)機(jī)械手末端沒有觸碰到物體小方塊時(shí)，遠(yuǎn)指節(jié)的運(yùn)動(dòng)速度不斷增加，當(dāng)機(jī)械手末端觸碰到物體小方塊時(shí)，遠(yuǎn)指節(jié)的運(yùn)動(dòng)速度開始逐漸下降，當(dāng)時(shí)間為75ms時(shí)，機(jī)械手末端的遠(yuǎn)指節(jié)速度幾乎為0，表明機(jī)械手已達(dá)到平衡狀態(tài)。圖5為機(jī)械手末端抓取力仿真曲線。

從圖中可以看出，當(dāng)機(jī)械手末端與物體小方塊剛開始接觸時(shí)，由于碰撞的存在，抓取力有些抖動(dòng)，隨后在很短的時(shí)間內(nèi)隨著抓取運(yùn)動(dòng)的進(jìn)行，抓取力不斷增加，當(dāng)時(shí)間為90ms時(shí)，抓取力為最大值，機(jī)械手達(dá)到平衡狀態(tài)。由此可知，通過本文方法所設(shè)計(jì)的欠驅(qū)動(dòng)機(jī)械手可以完成對物體的精確抓取。

由于欠驅(qū)動(dòng)機(jī)械手在運(yùn)動(dòng)過程中連桿所受的力矩和機(jī)械手末端各關(guān)節(jié)所受的彈簧力都在不斷地發(fā)生變化，導(dǎo)致機(jī)械手末端的抓取力也不斷地發(fā)生變化，因此本文通過仿真分析了機(jī)械手末端各關(guān)節(jié)的抓取力變化情況。圖6為機(jī)械手末端各關(guān)節(jié)的角速度仿真曲線。

圖6 機(jī)械手末端各關(guān)節(jié)的角速度曲線

從圖中可以看出，機(jī)械手末端各關(guān)節(jié)在沒有觸碰到物體小方塊前，在驅(qū)動(dòng)力的作用下，近指節(jié)、中指節(jié)、遠(yuǎn)指節(jié)角速度不斷增加；當(dāng)近指節(jié)觸碰到物理小方塊后，角速度逐漸減??；隨之中指節(jié)、遠(yuǎn)指節(jié)先后觸碰物體小方塊，角速度也隨之減小。當(dāng)時(shí)間為2s時(shí)，各關(guān)節(jié)的角速度接近數(shù)值0，說明各關(guān)節(jié)已經(jīng)達(dá)到了抓取的平衡狀態(tài)。圖7為機(jī)械手末端各關(guān)節(jié)受力仿真曲線。

圖7 機(jī)械手末端各關(guān)節(jié)受力仿真曲線

從圖中可以看出，機(jī)械手末端各個(gè)關(guān)節(jié)的抓取力相差較小，而且各個(gè)關(guān)節(jié)的受力情況沒有發(fā)生突變，說明采用本文方法設(shè)計(jì)的欠驅(qū)動(dòng)機(jī)械手不僅可以滿足均勻受力的要求，還可以滿足穩(wěn)定抓取物體的要求。

6 結(jié)束語

本文針對機(jī)械手末端控制提出了一種結(jié)合PID與狀態(tài)觀測器的控制方法，同時(shí)采用欠驅(qū)動(dòng)方式對機(jī)械手進(jìn)行控制設(shè)計(jì)。通過對機(jī)械手末端抓取工作的力學(xué)理論和靜力學(xué)理論分析，計(jì)算出各個(gè)關(guān)節(jié)的抓取力，完成對欠驅(qū)動(dòng)機(jī)械手的精準(zhǔn)控制。通過MATLAB仿真，證明本文提出的方法可以對目標(biāo)函數(shù)進(jìn)行準(zhǔn)確跟蹤，跟蹤誤差維持在0.1mm之內(nèi)。通過對機(jī)械手末端抓取物體運(yùn)動(dòng)規(guī)律的實(shí)驗(yàn)，表明采用本文方法，機(jī)械手末端近指節(jié)、中指節(jié)、遠(yuǎn)指節(jié)的抓取力相差較小，沒有發(fā)生抓取力突變的情況，可以滿足穩(wěn)定抓取物體的要求，實(shí)現(xiàn)了對機(jī)械手末端的精準(zhǔn)控制。