某排爆機(jī)械手關(guān)節(jié)控制系統(tǒng)研究

沙毅剛，王 強(qiáng)，何曉暉，楊 菲，朱曉基

(陸軍工程大學(xué) 野戰(zhàn)工程學(xué)院機(jī)電教研中心， 南京 210007)

排爆機(jī)械手是用來排除未爆彈藥的主要作業(yè)工具，主要由機(jī)械臂和手爪組成，大型排爆機(jī)械手的控制由于其本身作業(yè)環(huán)境特殊、結(jié)構(gòu)重量較大，導(dǎo)致控制精度較低，調(diào)整時(shí)間較長(zhǎng)，因此，合理優(yōu)化排爆機(jī)械手的控制策略一直是研究的重點(diǎn)。排爆機(jī)械手的驅(qū)動(dòng)方式有液壓驅(qū)動(dòng)、電機(jī)驅(qū)動(dòng)和氣壓?jiǎn)?dòng)，作業(yè)時(shí)，控制器輸出信號(hào)控制驅(qū)動(dòng)裝置，驅(qū)動(dòng)裝置直接帶動(dòng)各關(guān)節(jié)到達(dá)相應(yīng)位置，通過各關(guān)節(jié)聯(lián)合運(yùn)動(dòng)，使機(jī)械手末端達(dá)到指定位置，實(shí)現(xiàn)排爆過程。由于排爆機(jī)械手本身重量較大，使其在作業(yè)過程中會(huì)產(chǎn)生較大的滯后性和慣性，并且關(guān)節(jié)驅(qū)動(dòng)需要的力矩較大，因此PID成為目前排爆機(jī)械手控制的主流算法，然而，PID控制時(shí)存在初始參數(shù)確定困難、超調(diào)量較大、響應(yīng)時(shí)間較長(zhǎng)等問題，因此，開發(fā)新的控制方法，或在原有PID算法上加以改善，具有非常重要的研究?jī)r(jià)值和應(yīng)用前景。

為提高排爆機(jī)械手等大型機(jī)械手的控制精度，國(guó)內(nèi)外學(xué)者為此進(jìn)行了廣泛的研究。寧煜[1]設(shè)計(jì)了一種單神經(jīng)元來優(yōu)化機(jī)械臂PID控制方法，使關(guān)節(jié)誤差減小，魯棒性更強(qiáng)，但微分跟蹤器參數(shù)難以確定； Indrawanto[2]考慮到機(jī)械臂動(dòng)力學(xué)具有的強(qiáng)非線性等特點(diǎn)，因此采用具有強(qiáng)魯棒性特點(diǎn)的滑?？刂七M(jìn)行關(guān)節(jié)控制內(nèi)的運(yùn)動(dòng)控制。Aly[3]結(jié)合模糊和PID控制策略，推出了一個(gè)新型的模糊PID算法，用以大型電液機(jī)械臂軌跡控制，與傳統(tǒng)PID方法對(duì)比，此方法可實(shí)現(xiàn)更快的收斂以及具有更優(yōu)的魯棒性能。沈玲[4]研究了大型送料機(jī)械手夾緊模糊PID控制系統(tǒng),采用二維模糊控制器,根據(jù)模糊規(guī)則建立模糊推理系統(tǒng),搭建模糊PID控制系統(tǒng)仿真模型,縮短了機(jī)械手反應(yīng)時(shí)間。倪驍驊等[5]研究了機(jī)械手自適應(yīng)滑模控制系統(tǒng)，建立機(jī)械手運(yùn)動(dòng)數(shù)學(xué)模型，設(shè)計(jì)了滑模控制系統(tǒng)，對(duì)控制系統(tǒng)的穩(wěn)定性進(jìn)行證明，通過仿真驗(yàn)證滑模控制效果，降低了機(jī)械手角速度跟蹤誤差。

本文基于排爆機(jī)械手關(guān)節(jié)PID控制參數(shù)初始值確定困難、超調(diào)量較大等問題，設(shè)計(jì)一種排爆機(jī)械手關(guān)節(jié)BP+PID伺服電機(jī)控制系統(tǒng)，運(yùn)用BP神經(jīng)網(wǎng)絡(luò)優(yōu)秀的學(xué)習(xí)能力和非線性求解能力對(duì)其參數(shù)進(jìn)行整定，加快PID控制的收斂速度，提升控制精度。通過建立直流伺服電機(jī)模型、BP+PID算法模型，并與傳統(tǒng)PID控制進(jìn)行對(duì)比，驗(yàn)證了該控制系統(tǒng)的實(shí)用性與可行性。

1 排爆機(jī)械手關(guān)節(jié)控制系統(tǒng)分析

1.1 排爆機(jī)械手系統(tǒng)參數(shù)

某排爆機(jī)械手采用六自由度的結(jié)構(gòu)，其最大臂展為2 m，最大夾持力為20 kg，機(jī)械手本身重189 kg，采用伺服電機(jī)直接帶動(dòng)關(guān)節(jié)進(jìn)行運(yùn)動(dòng)，其結(jié)構(gòu)簡(jiǎn)化模型如圖1所示。其機(jī)械參數(shù)如表1所示。

圖1 某排爆機(jī)械手結(jié)構(gòu)示意圖

表1 排爆機(jī)械手機(jī)械參數(shù)

某排爆機(jī)械手關(guān)節(jié)控制系統(tǒng)原理如圖2所示。

圖2 排爆機(jī)械手關(guān)節(jié)伺服電機(jī)控制系統(tǒng)原理框圖

系統(tǒng)采用位置直流伺服電機(jī)直接驅(qū)動(dòng)機(jī)械手關(guān)節(jié)運(yùn)動(dòng)，并通過角度傳感器采集機(jī)械手關(guān)節(jié)運(yùn)動(dòng)的角度反饋給電機(jī)進(jìn)行實(shí)時(shí)調(diào)整。

1.2 關(guān)節(jié)控制系統(tǒng)數(shù)學(xué)模型

直流伺服電機(jī)工作時(shí)，其工作轉(zhuǎn)矩等于負(fù)載轉(zhuǎn)矩與負(fù)載慣性系統(tǒng)加、減轉(zhuǎn)矩之和，即：

(1)

式(1)中，T(t)為電動(dòng)機(jī)輸出轉(zhuǎn)矩(N·m)；TL(t)為負(fù)載轉(zhuǎn)矩(N·m)；w(t)為電動(dòng)機(jī)角速度(rad/s)；Ja為電動(dòng)機(jī)電樞轉(zhuǎn)動(dòng)慣量，Ja=2.2×10-3kg·m2；J1為負(fù)載的轉(zhuǎn)動(dòng)慣量，需將機(jī)械手關(guān)節(jié)的慣性轉(zhuǎn)換到電機(jī)軸上，取5×10-3kg·m2;h為絲杠螺距;mz為工作臺(tái)質(zhì)量。

當(dāng)電機(jī)電路處于動(dòng)態(tài)過程中時(shí)，電機(jī)內(nèi)部線圈施加的電源電壓Ua(t)和電樞線圈內(nèi)通過的電流Ia(t)的關(guān)系為：

(2)

式(2)中，Ra為電機(jī)電樞線圈內(nèi)阻，Ra=20 Ω；La為電機(jī)電樞線圈的電感，La=2H；eb(t)為電機(jī)電樞線圈在釘子磁場(chǎng)中運(yùn)動(dòng)時(shí)產(chǎn)生的反電動(dòng)勢(shì)。

電機(jī)輸出轉(zhuǎn)矩M(t)應(yīng)與通過電樞線圈的電流大小成正比，則M(t)=kTIa(t)[6]。其中，kT為電機(jī)輸出扭矩常數(shù)，kT=9.2 N·m·A-1。電機(jī)電樞線圈產(chǎn)生的反電動(dòng)勢(shì)eb(t)與電樞的工作角速度w(t)成正比[7]，故有：

eb(t)=kbw(t)

(3)

式(3)中，kb為電機(jī)電樞反電動(dòng)勢(shì)系數(shù)，kb=0.024 4 V·rad-1。

對(duì)上述式子進(jìn)行拉普拉斯變換，并令初始條件為0，則有：

M(s)=ML(s)+(Ja+J1)s·Ω(s)

(4)

Ua(s)=(Rs+sLa)Ia(s)+Eb(s)

(5)

M(s)=kTIa(s)

(6)

Eb(s)=kbΩ(s)

(7)

對(duì)上式進(jìn)行綜合，得：

(8)

在不考慮外部干擾負(fù)載ML(s)時(shí)，直流伺服電機(jī)的傳遞函數(shù)為：

(9)

式(9)中，Tm為直流電機(jī)的時(shí)間常數(shù)。

假設(shè)kφ為關(guān)節(jié)角度對(duì)應(yīng)電壓轉(zhuǎn)換系數(shù)，此處取kφ=0.07 V·rad-1。

直流伺服電機(jī)的閉環(huán)控制系統(tǒng)如圖3所示。

圖3 直流伺服電機(jī)閉環(huán)控制系統(tǒng)框圖

1.3 關(guān)節(jié)控制系統(tǒng)穩(wěn)定性分析

系統(tǒng)穩(wěn)定性是指控制系統(tǒng)在使它偏離平衡狀態(tài)的擾動(dòng)消失后返回原來平衡狀態(tài)的能力[8]。在設(shè)計(jì)控制系統(tǒng)時(shí)，首先要求系統(tǒng)是穩(wěn)定的。對(duì)于本排爆機(jī)械手關(guān)節(jié)電機(jī)伺服控制系統(tǒng)，依據(jù)以上系統(tǒng)傳遞函數(shù)模型推導(dǎo)出系統(tǒng)的開環(huán)傳遞函數(shù)為：

(10)

系統(tǒng)零-極點(diǎn)分布圖如圖4，系統(tǒng)零-極點(diǎn)主要在s右半平面內(nèi)，因此該系統(tǒng)為最小相位系統(tǒng)。系統(tǒng)奈奎斯特圖如圖5，系統(tǒng)是穩(wěn)定的。系統(tǒng)響應(yīng)曲線如圖6，

圖4 電機(jī)伺服控制系統(tǒng)零-極點(diǎn)分布圖

圖5 電機(jī)伺服控制系統(tǒng)奈奎斯特圖

圖6 電機(jī)伺服控制系統(tǒng)響應(yīng)曲線

可以看出，系統(tǒng)受到?jīng)_擊時(shí)響應(yīng)時(shí)間大約為0.5 s，調(diào)節(jié)時(shí)間大約為2.7 s，超調(diào)量大約為0.2 rad。該伺服電機(jī)控制系統(tǒng)雖能趨于穩(wěn)定但系統(tǒng)調(diào)整時(shí)間過長(zhǎng)、超調(diào)量過大。由此，本文提出的BP+PID控制來優(yōu)化電機(jī)伺服控制系統(tǒng)，以提高機(jī)械手關(guān)節(jié)控制的響應(yīng)速度和調(diào)整精度。

2 BP神經(jīng)網(wǎng)絡(luò)PID控制設(shè)計(jì)

經(jīng)典增量式數(shù)字PID的控制計(jì)算公式為：

u(k)=u(k-1)+Kp[e(k)-e(k-1)]+Kie(k) +

Kd[e(k)-2e(k-1)+e(k-2)]

(11)

式(11)中，Kp、Ki、Kd分別為比例、積分、微分系數(shù)；e(k)為當(dāng)前采樣時(shí)刻的期望輸出與實(shí)際輸出之差；u(k)為當(dāng)前采樣時(shí)刻的控制量[9]。

將Kp、Ki、Kd視為以來于系統(tǒng)運(yùn)行狀態(tài)的可調(diào)參數(shù)時(shí)，可將上式描述為：

u(k)=f(u(k-1),Kp,Ki,Kd,e(k),e(k-1),e(k-2))

(12)

式(12)中，f(·)是與Kp、Ki、Kd、u(k-1)、u(k)等有關(guān)的非線性函數(shù)。

通過BP神經(jīng)網(wǎng)絡(luò)訓(xùn)練和學(xué)習(xí)來找到此函數(shù)最佳的一個(gè)控制規(guī)律。BP神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)如圖7所示。

圖7 BP網(wǎng)絡(luò)結(jié)構(gòu)示意圖

BP神經(jīng)網(wǎng)絡(luò)輸出層的3個(gè)輸出分別對(duì)應(yīng)PID控制器的3個(gè)可調(diào)參數(shù)Kp、Ki、Kd。由于Kp、Ki、Kd不能為負(fù)，所以輸出層神經(jīng)元的激活函數(shù)取非負(fù)的Sigmoid函數(shù)，而隱含層神經(jīng)元的激活函數(shù)可取正負(fù)對(duì)稱的tanh函數(shù)[10-12]。BP神經(jīng)網(wǎng)絡(luò)的輸入為(M為輸入變量的個(gè)數(shù)):

(13)

隱含層的輸入輸出為：

(14)

(15)

(16)

網(wǎng)絡(luò)輸出層的輸入輸出為：

(17)

(18)

則有：

(19)

采用輸出誤差二次方為網(wǎng)絡(luò)的損失函數(shù)：

(20)

按照梯度下降法修正網(wǎng)絡(luò)的加權(quán)系數(shù)，并附加一個(gè)使搜索快速收斂全局極小的慣性項(xiàng)，則有BP神經(jīng)網(wǎng)絡(luò)輸出層的加權(quán)系數(shù)修正計(jì)算公式為：

(21)

(22)

同理可得隱含層加權(quán)系數(shù)的修正計(jì)算公式為：

(23)

(24)

式(22)中，g′[x]=g(x)[1-g(x)]，式(24)中，f′[x]=[1-f2(x)]/2。

其系統(tǒng)結(jié)構(gòu)如圖8所示。

圖8 BP+PID控制系統(tǒng)結(jié)構(gòu)框圖

BP神經(jīng)網(wǎng)絡(luò)PID控制算法流程可總結(jié)為圖9。

圖9 BP+PID程序控制算法流程框圖

3 系統(tǒng)仿真實(shí)驗(yàn)及結(jié)果分析

根據(jù)以上數(shù)學(xué)模型在Matlab/Simulink中進(jìn)行建模與仿真，Simulink模型如圖10所示。在建立的排爆機(jī)械手關(guān)節(jié)控制系統(tǒng)進(jìn)行模擬仿真，假定在某一作業(yè)過程中，機(jī)械手視覺系統(tǒng)定位了某一目標(biāo)，中心處理器根據(jù)逆運(yùn)動(dòng)學(xué)計(jì)算，排爆機(jī)械手大臂關(guān)節(jié)的期望角度為1 rad，因此在系統(tǒng)的輸入接口輸入信號(hào)為1 m/s的關(guān)節(jié)信號(hào)進(jìn)行仿真，查看該關(guān)節(jié)運(yùn)動(dòng)情況[15-16]。當(dāng)控制系統(tǒng)輸入1 m/s的關(guān)節(jié)信號(hào)時(shí)，關(guān)節(jié)角度運(yùn)動(dòng)如圖11所示。

圖10 排爆機(jī)械手關(guān)節(jié)BP+PID伺服電機(jī)控制系統(tǒng)Simulink仿真模型示意圖

圖11 排爆機(jī)械手關(guān)節(jié)運(yùn)動(dòng)效果圖

由以上仿真結(jié)果可以看出，該排爆機(jī)械手關(guān)節(jié)控制方案可行。為證明該控制方案與傳統(tǒng)PID機(jī)械手控制相比具有一定的先進(jìn)性與實(shí)用性，在Simulink中建立相同系統(tǒng)采用PID控制進(jìn)行仿真，PID排爆機(jī)械手關(guān)節(jié)伺服電機(jī)控制Simulink模型如圖12所示，其中，PID控制參數(shù)Kp=0.94、Ki=0.04、Kd=0.40,并輸入相同關(guān)節(jié)信號(hào)進(jìn)行仿真。將兩次仿真結(jié)果進(jìn)行對(duì)比，兩個(gè)系統(tǒng)控制參數(shù)Kp、Ki、Kd參數(shù)變化如圖13所示，得Kp=0.222、Ki=0.54、Kd=0.245關(guān)節(jié)角響應(yīng)如圖14所示，誤差如圖15所示。

圖12 排爆機(jī)械手關(guān)節(jié)傳統(tǒng)PID伺服電機(jī)控制系統(tǒng)Simulink仿真模型示意圖

圖13 Kp、Ki、Kd參數(shù)曲線

根據(jù)圖14、圖15，BP+PID伺服控制系統(tǒng)響應(yīng)時(shí)間約為1.2 s，系統(tǒng)調(diào)整時(shí)間約為2 s，系統(tǒng)誤差約為0.01 rad，系統(tǒng)穩(wěn)定上升至目標(biāo)角度，無超調(diào)量；PID伺服控制系統(tǒng)響應(yīng)時(shí)間約為0.7 s，系統(tǒng)調(diào)整時(shí)間約為2.5 s，系統(tǒng)誤差約為0.04 rad，系統(tǒng)初期存在波動(dòng)，波動(dòng)角度約為0.1 rad；原伺服電機(jī)控制系統(tǒng)響應(yīng)時(shí)間約為0.6 s，調(diào)整時(shí)間約為2.9 s，超調(diào)量約為0.2 rad，誤差約為0.03 s，系統(tǒng)波動(dòng)較大。

圖14 排爆機(jī)械手關(guān)節(jié)角度曲線

圖15 關(guān)節(jié)角度誤差曲線

由此可知，1)原系統(tǒng)由于加入了控制環(huán)節(jié)導(dǎo)致系統(tǒng)響應(yīng)時(shí)間相比較原系統(tǒng)稍慢，但系統(tǒng)調(diào)整時(shí)間和誤差變小，超調(diào)量變小，因此在原系統(tǒng)中加入控制環(huán)節(jié)具有一定的必要性；2) 相比較傳統(tǒng)PID控制，由于自身參數(shù)學(xué)習(xí)過程導(dǎo)致BP+PID伺服電機(jī)控制系統(tǒng)的響應(yīng)時(shí)間稍慢，但系統(tǒng)整體超調(diào)量變小，抗干擾能力更強(qiáng)，能夠根據(jù)系統(tǒng)誤差和期望迅速調(diào)整Kp、Ki、Kd參數(shù)。3) BP+PID伺服電機(jī)控制系統(tǒng)可以根據(jù)系統(tǒng)整體性能得出最優(yōu)Kp、Ki、Kd參數(shù)，解決了傳統(tǒng)PID系統(tǒng)控制參數(shù)初始化難以確定的問題，且調(diào)整時(shí)間相比較其他兩個(gè)系統(tǒng)較短。由此驗(yàn)證了本文提出的排爆機(jī)械手關(guān)節(jié)控制系統(tǒng)的有效性和實(shí)用性。

5 結(jié)論

1) 引入PID進(jìn)行機(jī)械手關(guān)節(jié)控制優(yōu)化，并且通過應(yīng)用BP神經(jīng)網(wǎng)絡(luò)調(diào)整傳統(tǒng)PID的控制參數(shù)Kp、Ki、Kd。

2) 本文提出的排爆機(jī)械手關(guān)節(jié)BP+PID伺服控制系統(tǒng)與傳統(tǒng)PID系統(tǒng)相比控制精度更高，且抗干擾能力較強(qiáng)、魯棒性較強(qiáng)。

3) 本文提出的排爆機(jī)械手關(guān)節(jié)BP+PID伺服電機(jī)控制系統(tǒng)仿真，給排爆機(jī)械手控制提供了新的算法，可為更加精確的控制研究提供參考。