放射源處置機(jī)器人電液行走控制系統(tǒng)仿真與實(shí)驗(yàn)

閆九祥，孫 潔，肖永飛，張艷芳，朱 琳

(齊魯工業(yè)大學(xué)(山東省科學(xué)院)，山東省科學(xué)院自動(dòng)化研究所，山東省機(jī)器人與制造自動(dòng)化技術(shù)重點(diǎn)實(shí)驗(yàn)室，山東 濟(jì)南 250014)

引言

放射源處置機(jī)器人適用于核電站事故時(shí)放射源裝置的無(wú)人處置作業(yè)，也可用于放射源丟失時(shí)，在未知空間內(nèi)對(duì)放射源部件的搜索與回收。通過(guò)機(jī)器人實(shí)現(xiàn)無(wú)人作業(yè)，提高搜索與處置效率，避免放射源對(duì)人員的輻射傷害。

國(guó)內(nèi)外對(duì)放射源探測(cè)處置機(jī)器人運(yùn)動(dòng)控制及雙馬達(dá)電液控制技術(shù)進(jìn)行了大量研究，姜明明等[1]設(shè)計(jì)了一種六輪懸掛移動(dòng)機(jī)器人，該機(jī)器人使用三角定位法，對(duì)伽馬放射源進(jìn)行搜尋處置。魏曉朝等[2]針對(duì)車(chē)輛在復(fù)雜工作環(huán)境下系統(tǒng)負(fù)載干擾問(wèn)題，提出了一種基于液壓調(diào)速系統(tǒng)的遺傳算法優(yōu)化前饋-反饋+PID轉(zhuǎn)速?gòu)?fù)合控制方法。張華等[3-4]針對(duì)農(nóng)藥噴霧劑液壓四驅(qū)行走系統(tǒng)，提出直控式復(fù)合控制策略及自抗擾控制策略，改善了液壓行走機(jī)構(gòu)速度控制性能。丁海港等[5]設(shè)計(jì)了閥-泵并聯(lián)變模式液壓調(diào)速系統(tǒng)，提出了一種并聯(lián)變模式控制策略，使閥控與泵控協(xié)調(diào)工作，提高了液壓調(diào)速系統(tǒng)的靈活性和適應(yīng)性，豐富了目前液壓系統(tǒng)的調(diào)速方式。楊前明等[6]、魯植雄等[7]、郝小星等[8]針對(duì)閥控雙馬達(dá)調(diào)速系統(tǒng)，分別提出了模糊變結(jié)構(gòu)控制策略、主從式速度同步控制策略及滑膜自適應(yīng)控制策略等。在研究了相關(guān)電液速度控制技術(shù)的基礎(chǔ)上，結(jié)合具體應(yīng)用需求，重點(diǎn)研究負(fù)載時(shí)變工況下，雙通道電液速度控制問(wèn)題，以放射源應(yīng)急處置機(jī)器人電液比例閥控馬達(dá)行走控制系統(tǒng)作為研究對(duì)象，建立閥控馬達(dá)速度控制系統(tǒng)數(shù)學(xué)模型，提出雙通道模糊PID閥控馬達(dá)轉(zhuǎn)速控制策略，對(duì)系統(tǒng)進(jìn)行仿真與實(shí)驗(yàn)，分析驗(yàn)證所設(shè)計(jì)的電液比例閥控馬達(dá)行走控制系統(tǒng)的正確性。

1 電液行走控制系統(tǒng)概述

圖1為放射源處置機(jī)器人結(jié)構(gòu)示意圖，機(jī)器人由6部分組成。放射源探測(cè)器可探測(cè)一定空間范圍內(nèi)的放射源信號(hào)，同時(shí)輸出放射源相對(duì)機(jī)器人的三維空間坐標(biāo)信息，行走機(jī)構(gòu)將放射源坐標(biāo)位置信息作為尋源目標(biāo)，進(jìn)行自動(dòng)或遙操尋源作業(yè)。放射源應(yīng)急處置機(jī)器人設(shè)計(jì)有柔性機(jī)械臂、液壓機(jī)械臂進(jìn)行放射源的處置作業(yè)。液壓機(jī)械臂可配置不同類(lèi)型的執(zhí)行機(jī)構(gòu)進(jìn)行障礙物破拆與重物搬移，柔性機(jī)械臂采用直流伺服電機(jī)驅(qū)動(dòng)，可進(jìn)行高精度微操作業(yè)。在放射源處置作業(yè)時(shí)，機(jī)器人通過(guò)監(jiān)控系統(tǒng)為手動(dòng)遙操作業(yè)回傳實(shí)時(shí)現(xiàn)場(chǎng)畫(huà)面。

1.行走機(jī)構(gòu) 2.機(jī)身本體 3.監(jiān)控系統(tǒng) 4.柔性機(jī)械臂5.液壓機(jī)械臂 6.放射源探測(cè)器圖1 放射源處置機(jī)器人結(jié)構(gòu)示意圖

圖2為放射源應(yīng)急處置機(jī)器人電液行走控制系統(tǒng)原理圖，電液行走控制系統(tǒng)主要由控制器、液壓泵、比例換向閥、液壓馬達(dá)、轉(zhuǎn)速傳感器等組成。給定控制器初始電信號(hào)值，控制器對(duì)給定電信號(hào)進(jìn)行計(jì)算處理并輸出PWM電流信號(hào)驅(qū)動(dòng)電液比例換向閥，電液比例換向閥閥芯位移根據(jù)驅(qū)動(dòng)電流信號(hào)變化，從而改變進(jìn)入液壓馬達(dá)的液壓油流量，控制液壓馬達(dá)的轉(zhuǎn)速。在液壓馬達(dá)處加裝速度編碼器，采集馬達(dá)轉(zhuǎn)速并與給定的電信號(hào)進(jìn)行比較處理，所得的信號(hào)差值作為二次控制調(diào)節(jié)信號(hào)，對(duì)液壓馬達(dá)速度進(jìn)行調(diào)節(jié)。

圖2 機(jī)器人電液行走控制系統(tǒng)原理圖

2 電液控制系統(tǒng)數(shù)學(xué)建模

2.1 系統(tǒng)模型數(shù)學(xué)方程

換向閥的線(xiàn)性化流量方程為[8]：

(1)

式中，QL1,QL2—— 左右滑閥負(fù)載流量

Kq1，Kq2—— 左右滑閥流量增益

Xv1，Xv2—— 左右滑閥位移

Kc1，Kc2—— 左右滑閥流量壓力系數(shù)

pL1，pL2—— 左右液壓負(fù)載壓力

假定本系統(tǒng)中各閥與馬達(dá)連接油管短而粗，忽略管路的內(nèi)摩擦力，且系統(tǒng)無(wú)飽和及氣穴現(xiàn)象，液壓油溫度及體積彈性模量恒定，則馬達(dá)腔流量連續(xù)方程為[9]：

(2)

式中，Qm1，Qm2—— 左右馬達(dá)負(fù)載流量

Dm1，Dm2—— 左右馬達(dá)弧度排量

θ1，θ2—— 左右馬達(dá)角位移

Cm1，Cm2—— 左右馬達(dá)泄漏系數(shù)

Vm1，Vm2—— 左右馬達(dá)有效容積

βe—— 馬達(dá)體積彈性模量

在不考慮液壓油質(zhì)量的前提下，根據(jù)牛頓力學(xué)定律，液壓馬達(dá)的力矩平衡方程為[10]：

(3)

式中,J1，J2—— 左右馬達(dá)負(fù)載等效轉(zhuǎn)動(dòng)慣量

Bm1，Bm2—— 左右馬達(dá)負(fù)載阻尼系數(shù)

K1，K2—— 左右馬達(dá)負(fù)載彈簧剛度

T1，T2—— 左右馬達(dá)等效總力矩

2.2 系統(tǒng)傳遞函數(shù)

圖3為機(jī)器人電液行走控制系統(tǒng)傳遞函數(shù)框圖。G11(S),G12(S),G13(S),G14(S)分別為左行走機(jī)構(gòu)控制器模塊、PWM電流模塊、比例換向閥、液壓馬達(dá)的傳遞函數(shù)，G21(S),G22(S),G23(S),G24(S)分別為右行走機(jī)構(gòu)控制器模塊、PWM電流模塊、比例換向閥、液壓馬達(dá)的傳遞函數(shù)。滑閥固有頻率與液壓固有頻率接近，行走機(jī)構(gòu)外力負(fù)載為履帶與地面摩擦力，對(duì)式(1)～式(3)進(jìn)行拉氏變換，得到電液比例閥及液壓馬達(dá)與負(fù)載的傳遞函數(shù)，控制器、速度傳感器的傳遞函數(shù)參數(shù)由元件出廠(chǎng)設(shè)定參數(shù)確定。

圖3 機(jī)器人電液行走控制系統(tǒng)傳遞函數(shù)框圖

系統(tǒng)中各元件所對(duì)應(yīng)的傳遞函數(shù)串聯(lián)組成機(jī)器人電液比例行走控制系統(tǒng)的傳遞函數(shù)，系統(tǒng)各環(huán)節(jié)傳遞函數(shù)為：

(4)

(5)

(6)

(7)

式中，I1(s)，I2(s) —— 左右通道輸出電流

X1(s)，X2(s) —— 系統(tǒng)左右通道輸入值

Ka1，Ka2—— 左右通道比例環(huán)節(jié)增益

Q1(s)，Q2(s) —— 左右通道比例閥空載流量

ωsv1，ωsv2—— 左右通道比例閥固有頻率

ξ1，ξ2—— 比例閥阻尼比

ωh1，ωh2—— 左右通道液壓馬達(dá)固有頻率

N1(s)，N2(s) —— 左右通道速度傳感器脈沖數(shù)

V1(s)，V2(s) —— 左右通道馬達(dá)輸出軸轉(zhuǎn)速

Kv1，Kv2—— 左右通道速度傳感器增益[11]

2.3 控制策略及系統(tǒng)模型

放射源應(yīng)急處置機(jī)器人尋源行走過(guò)程中，控制器根據(jù)接收到的放射源位置信息及空間位置導(dǎo)航信息計(jì)算雙馬達(dá)輸出速度，同時(shí)監(jiān)測(cè)雙馬達(dá)轉(zhuǎn)速反饋值，對(duì)輸出轉(zhuǎn)速進(jìn)行實(shí)時(shí)調(diào)整，實(shí)現(xiàn)機(jī)器人差速轉(zhuǎn)向、直線(xiàn)行駛。由于作業(yè)空間及行駛環(huán)境復(fù)雜多變，系統(tǒng)須具備自適應(yīng)調(diào)整功能，同時(shí)對(duì)輸入信號(hào)具備高速、高精度響應(yīng)。

在典型時(shí)域線(xiàn)性控制系統(tǒng)采用的經(jīng)典PID控制器，通過(guò)對(duì)控制器比例、積分、微分參數(shù)的整定，使控制系統(tǒng)響應(yīng)速度提高，動(dòng)態(tài)性能改善，消除穩(wěn)態(tài)誤差[12-13]。放射源應(yīng)急處置機(jī)器人電液行走控制系統(tǒng)在實(shí)際工作環(huán)境中，行駛負(fù)載存在時(shí)變性，負(fù)載的變化會(huì)產(chǎn)生功率損耗，經(jīng)典PID控制策略無(wú)法滿(mǎn)足系統(tǒng)的時(shí)變控制[14-15]。為實(shí)現(xiàn)系統(tǒng)控制目標(biāo)，提出雙通道模糊PID控制策略，機(jī)器人電液行走模糊PID控制系統(tǒng)模型如圖4所示。在不同負(fù)載條件下測(cè)試液壓馬達(dá)的速度與加速度，對(duì)PID控制器參數(shù)進(jìn)行整定，得到不同負(fù)載條件下最優(yōu)參數(shù)集，模糊規(guī)律如表1所示，表中a，b，c分別代表參量ΔKp，ΔKi，ΔKd。

表1 ΔKp，ΔKi，ΔKd模糊規(guī)律

圖4 機(jī)器人電液行走控制系統(tǒng)模型

3 系統(tǒng)仿真與實(shí)驗(yàn)

3.1 建立系統(tǒng)Simulink仿真模型

放射源應(yīng)急處置機(jī)器人電液行走控制系統(tǒng)的電液比例換向閥及液壓馬達(dá)參數(shù)如表2所示。根據(jù)機(jī)器人電液行走控制系統(tǒng)閉環(huán)傳遞函數(shù)方框圖，在MATLAB仿真軟件中建立系統(tǒng)仿真模型，將表2所示液壓系統(tǒng)參數(shù)帶入控制系統(tǒng)閉環(huán)傳遞函數(shù)模型中，在控制模型中引入模糊PID控制器，在MATLAB仿真軟件中對(duì)系統(tǒng)仿真模型輸入幅值為1的階躍信號(hào)，仿真運(yùn)行4 s，仿真系統(tǒng)函數(shù)對(duì)階躍信號(hào)的時(shí)域響應(yīng)，將仿真結(jié)果輸出到圖形顯示元件中。

表2 液壓系統(tǒng)參數(shù)

3.2 系統(tǒng)仿真分析

將元件參數(shù)帶入放射源應(yīng)急處置機(jī)器人雙馬達(dá)電液比例閥控馬達(dá)行走控制系統(tǒng)數(shù)學(xué)模型中，對(duì)系統(tǒng)模型進(jìn)行仿真，分析系統(tǒng)動(dòng)態(tài)特性，驗(yàn)證所設(shè)計(jì)的行走驅(qū)動(dòng)控制系統(tǒng)的正確性。

對(duì)無(wú)PID矯正系統(tǒng)輸入幅值為1的階躍響應(yīng)信號(hào)ω，仿真時(shí)間4 s，在系統(tǒng)模型中引入模糊PID控制器，仿真結(jié)果如圖5所示。系統(tǒng)要求機(jī)器人行走系統(tǒng)對(duì)行進(jìn)與制動(dòng)2個(gè)工況的行駛響應(yīng)都具備優(yōu)良的動(dòng)態(tài)響應(yīng)特性，對(duì)系統(tǒng)輸入周期性脈沖跟隨方波進(jìn)行仿真，驗(yàn)證系統(tǒng)行進(jìn)與制動(dòng)運(yùn)動(dòng)時(shí)控制系統(tǒng)輸出性能，系統(tǒng)響應(yīng)輸出如圖6所示。

圖5 系統(tǒng)階躍響應(yīng)時(shí)域曲線(xiàn)

圖6 系統(tǒng)階躍響應(yīng)跟隨曲線(xiàn)

對(duì)比無(wú)矯正系統(tǒng)和模糊PID控制器矯正系統(tǒng)的仿真曲線(xiàn)圖5及系統(tǒng)階躍響應(yīng)跟隨曲線(xiàn)圖6，可以看出：

(1) 無(wú)矯正系統(tǒng)在階躍信號(hào)的觸發(fā)下，時(shí)域輸出曲線(xiàn)在3 s后達(dá)到穩(wěn)態(tài)輸出，無(wú)超調(diào)。雙側(cè)行走馬達(dá)在1.5 s前最大速度響應(yīng)差值為輸入信號(hào)幅值的15%；

(2) 模糊PID矯正后系統(tǒng)響應(yīng)速度提高，在1.3 s時(shí)達(dá)到穩(wěn)態(tài)輸出，雙側(cè)行走馬達(dá)在0.3 s前最大速度響應(yīng)差值為輸入信號(hào)幅值的3%；

(3) PID矯正后系在周期階躍信號(hào)的觸發(fā)下，雙側(cè)行走馬達(dá)加減速響應(yīng)輸出最大速度響應(yīng)差值均低于輸入信號(hào)幅值的3%。

仿真結(jié)果表明，無(wú)矯正系統(tǒng)時(shí)域輸出曲線(xiàn)可達(dá)到穩(wěn)態(tài)輸出，穩(wěn)態(tài)誤差較小，無(wú)超調(diào)，證明了所推導(dǎo)的數(shù)學(xué)模型的正確性。

系統(tǒng)無(wú)矯正前雙側(cè)行走馬達(dá)在1.5 s前最大速度響應(yīng)差值為輸入信號(hào)幅值的15%，無(wú)法滿(mǎn)足高速直線(xiàn)行走及快速轉(zhuǎn)向的要求。模糊PID矯正后系統(tǒng)穩(wěn)態(tài)響應(yīng)時(shí)間提高到1.3 s，雙側(cè)行走馬達(dá)在0.3 s前最大速度響應(yīng)最大差值降低為輸入信號(hào)幅值的3%，改善了系統(tǒng)動(dòng)態(tài)響應(yīng)性能。

雙側(cè)行走馬達(dá)加減速響應(yīng)時(shí)間均明顯提高，達(dá)到穩(wěn)態(tài)輸出前雙側(cè)馬達(dá)速度差值均低于輸入信號(hào)幅值的3%，PID矯正后系統(tǒng)做行進(jìn)與制動(dòng)運(yùn)動(dòng)時(shí)，均具有較好的動(dòng)態(tài)輸出特性。

3.3 系統(tǒng)實(shí)驗(yàn)分析

進(jìn)行實(shí)驗(yàn)的機(jī)器人樣機(jī)如圖7所示，電液行走控制系統(tǒng)由液壓泵、液壓馬達(dá)、電液比例換向閥、控制器等組成，液壓馬達(dá)輸出軸安裝有SICK編碼器，液壓泵由直流伺服電機(jī)驅(qū)動(dòng)。液壓泵采用A1V18定量泵，液壓馬達(dá)選用JT02VN，電液比例閥為4WRAE系列，所需輸入電流范圍為0～1 A；控制器采用EPEC3036，該控制器可編程輸出PWM電流范圍為0～1.03 A，分辨率為1 mA;控制器通過(guò)CAN總線(xiàn)接收速度傳感器數(shù)據(jù)。

圖7 實(shí)驗(yàn)機(jī)器人樣機(jī)實(shí)物圖

如圖8所示，選用實(shí)驗(yàn)場(chǎng)地為不平整砂石路面，在這種路面條件下，機(jī)器人在行走時(shí)所克服的行走阻力隨路面起伏變化。為驗(yàn)證電液行走控制系統(tǒng)動(dòng)態(tài)性能，在控制器中對(duì)系統(tǒng)加載勻加速、勻速、勻減速三段式速度運(yùn)動(dòng)曲線(xiàn)，驗(yàn)證系統(tǒng)在不平整砂石路面條件下對(duì)不同運(yùn)動(dòng)條件的輸出響應(yīng)。

圖8 實(shí)驗(yàn)現(xiàn)場(chǎng)

三段式速度運(yùn)動(dòng)曲線(xiàn)如圖9所示，勻加速階段加速度為1.5 rad/s2，勻速階段速度為3 rad/s，勻減速階段加速度為0.75 rad/s2。通過(guò)采集雙側(cè)編碼器速度反饋值繪制雙馬達(dá)轉(zhuǎn)速響應(yīng)曲線(xiàn)，如圖10所示。

圖9 三段式速度輸入曲線(xiàn)

圖10 馬達(dá)轉(zhuǎn)速實(shí)驗(yàn)結(jié)果擬合曲線(xiàn)

實(shí)驗(yàn)結(jié)果表明，在不平整砂石路面情況下，雙側(cè)馬達(dá)轉(zhuǎn)在加減速階段最大轉(zhuǎn)速穩(wěn)態(tài)誤差低于幅值2%，在勻速行走階段，最大轉(zhuǎn)速穩(wěn)態(tài)誤差低于幅值3%，試驗(yàn)結(jié)果與仿真結(jié)果吻合，驗(yàn)證了控制模型的正確性。所采集的馬達(dá)速度響應(yīng)值對(duì)輸入曲線(xiàn)的擬合優(yōu)度為0.96，試驗(yàn)結(jié)果證明了系統(tǒng)在加速、勻速及減速度階段具有良好的響應(yīng)特性。

4 結(jié)論

利用MATLAB仿真工具對(duì)所設(shè)計(jì)的模糊PID控制系統(tǒng)模型進(jìn)行仿真驗(yàn)證，仿真結(jié)果表明，系統(tǒng)響應(yīng)速度提高至1.3 s，速度差值低于輸入幅值3%，穩(wěn)定精度得到改善。通過(guò)樣機(jī)在負(fù)載砂石路面的三段速度行走實(shí)驗(yàn)，證明了建立的閥控馬達(dá)速度控制系統(tǒng)數(shù)學(xué)模型及雙通道模糊PID電液比例速度控制策略的正確性及準(zhǔn)確性。仿真及實(shí)驗(yàn)結(jié)果證明所設(shè)計(jì)的全液壓履帶式電液比例閥控雙液壓馬達(dá)行走控制系統(tǒng)方案可以滿(mǎn)足放射源處置機(jī)器人對(duì)電液行走機(jī)構(gòu)的速度性能要求。