閆九祥,孫 潔,肖永飛,張艷芳,朱 琳
(齊魯工業(yè)大學(山東省科學院),山東省科學院自動化研究所,山東省機器人與制造自動化技術(shù)重點實驗室,山東 濟南 250014)
放射源處置機器人適用于核電站事故時放射源裝置的無人處置作業(yè),也可用于放射源丟失時,在未知空間內(nèi)對放射源部件的搜索與回收。通過機器人實現(xiàn)無人作業(yè),提高搜索與處置效率,避免放射源對人員的輻射傷害。
國內(nèi)外對放射源探測處置機器人運動控制及雙馬達電液控制技術(shù)進行了大量研究,姜明明等[1]設(shè)計了一種六輪懸掛移動機器人,該機器人使用三角定位法,對伽馬放射源進行搜尋處置。魏曉朝等[2]針對車輛在復雜工作環(huán)境下系統(tǒng)負載干擾問題,提出了一種基于液壓調(diào)速系統(tǒng)的遺傳算法優(yōu)化前饋-反饋+PID轉(zhuǎn)速復合控制方法。張華等[3-4]針對農(nóng)藥噴霧劑液壓四驅(qū)行走系統(tǒng),提出直控式復合控制策略及自抗擾控制策略,改善了液壓行走機構(gòu)速度控制性能。丁海港等[5]設(shè)計了閥-泵并聯(lián)變模式液壓調(diào)速系統(tǒng),提出了一種并聯(lián)變模式控制策略,使閥控與泵控協(xié)調(diào)工作,提高了液壓調(diào)速系統(tǒng)的靈活性和適應(yīng)性,豐富了目前液壓系統(tǒng)的調(diào)速方式。楊前明等[6]、魯植雄等[7]、郝小星等[8]針對閥控雙馬達調(diào)速系統(tǒng),分別提出了模糊變結(jié)構(gòu)控制策略、主從式速度同步控制策略及滑膜自適應(yīng)控制策略等。在研究了相關(guān)電液速度控制技術(shù)的基礎(chǔ)上,結(jié)合具體應(yīng)用需求,重點研究負載時變工況下,雙通道電液速度控制問題,以放射源應(yīng)急處置機器人電液比例閥控馬達行走控制系統(tǒng)作為研究對象,建立閥控馬達速度控制系統(tǒng)數(shù)學模型,提出雙通道模糊PID閥控馬達轉(zhuǎn)速控制策略,對系統(tǒng)進行仿真與實驗,分析驗證所設(shè)計的電液比例閥控馬達行走控制系統(tǒng)的正確性。
圖1為放射源處置機器人結(jié)構(gòu)示意圖,機器人由6部分組成。放射源探測器可探測一定空間范圍內(nèi)的放射源信號,同時輸出放射源相對機器人的三維空間坐標信息,行走機構(gòu)將放射源坐標位置信息作為尋源目標,進行自動或遙操尋源作業(yè)。放射源應(yīng)急處置機器人設(shè)計有柔性機械臂、液壓機械臂進行放射源的處置作業(yè)。液壓機械臂可配置不同類型的執(zhí)行機構(gòu)進行障礙物破拆與重物搬移,柔性機械臂采用直流伺服電機驅(qū)動,可進行高精度微操作業(yè)。在放射源處置作業(yè)時,機器人通過監(jiān)控系統(tǒng)為手動遙操作業(yè)回傳實時現(xiàn)場畫面。
1.行走機構(gòu) 2.機身本體 3.監(jiān)控系統(tǒng) 4.柔性機械臂5.液壓機械臂 6.放射源探測器圖1 放射源處置機器人結(jié)構(gòu)示意圖
圖2為放射源應(yīng)急處置機器人電液行走控制系統(tǒng)原理圖,電液行走控制系統(tǒng)主要由控制器、液壓泵、比例換向閥、液壓馬達、轉(zhuǎn)速傳感器等組成。給定控制器初始電信號值,控制器對給定電信號進行計算處理并輸出PWM電流信號驅(qū)動電液比例換向閥,電液比例換向閥閥芯位移根據(jù)驅(qū)動電流信號變化,從而改變進入液壓馬達的液壓油流量,控制液壓馬達的轉(zhuǎn)速。在液壓馬達處加裝速度編碼器,采集馬達轉(zhuǎn)速并與給定的電信號進行比較處理,所得的信號差值作為二次控制調(diào)節(jié)信號,對液壓馬達速度進行調(diào)節(jié)。
圖2 機器人電液行走控制系統(tǒng)原理圖
換向閥的線性化流量方程為[8]:
(1)
式中,QL1,QL2—— 左右滑閥負載流量
Kq1,Kq2—— 左右滑閥流量增益
Xv1,Xv2—— 左右滑閥位移
Kc1,Kc2—— 左右滑閥流量壓力系數(shù)
pL1,pL2—— 左右液壓負載壓力
假定本系統(tǒng)中各閥與馬達連接油管短而粗,忽略管路的內(nèi)摩擦力,且系統(tǒng)無飽和及氣穴現(xiàn)象,液壓油溫度及體積彈性模量恒定,則馬達腔流量連續(xù)方程為[9]:
(2)
式中,Qm1,Qm2—— 左右馬達負載流量
Dm1,Dm2—— 左右馬達弧度排量
θ1,θ2—— 左右馬達角位移
Cm1,Cm2—— 左右馬達泄漏系數(shù)
Vm1,Vm2—— 左右馬達有效容積
βe—— 馬達體積彈性模量
在不考慮液壓油質(zhì)量的前提下,根據(jù)牛頓力學定律,液壓馬達的力矩平衡方程為[10]:
(3)
式中,J1,J2—— 左右馬達負載等效轉(zhuǎn)動慣量
Bm1,Bm2—— 左右馬達負載阻尼系數(shù)
K1,K2—— 左右馬達負載彈簧剛度
T1,T2—— 左右馬達等效總力矩
圖3為機器人電液行走控制系統(tǒng)傳遞函數(shù)框圖。G11(S),G12(S),G13(S),G14(S)分別為左行走機構(gòu)控制器模塊、PWM電流模塊、比例換向閥、液壓馬達的傳遞函數(shù),G21(S),G22(S),G23(S),G24(S)分別為右行走機構(gòu)控制器模塊、PWM電流模塊、比例換向閥、液壓馬達的傳遞函數(shù)?;y固有頻率與液壓固有頻率接近,行走機構(gòu)外力負載為履帶與地面摩擦力,對式(1)~式(3)進行拉氏變換,得到電液比例閥及液壓馬達與負載的傳遞函數(shù),控制器、速度傳感器的傳遞函數(shù)參數(shù)由元件出廠設(shè)定參數(shù)確定。
圖3 機器人電液行走控制系統(tǒng)傳遞函數(shù)框圖
系統(tǒng)中各元件所對應(yīng)的傳遞函數(shù)串聯(lián)組成機器人電液比例行走控制系統(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—— 左右通道液壓馬達固有頻率
N1(s),N2(s) —— 左右通道速度傳感器脈沖數(shù)
V1(s),V2(s) —— 左右通道馬達輸出軸轉(zhuǎn)速
Kv1,Kv2—— 左右通道速度傳感器增益[11]
放射源應(yīng)急處置機器人尋源行走過程中,控制器根據(jù)接收到的放射源位置信息及空間位置導航信息計算雙馬達輸出速度,同時監(jiān)測雙馬達轉(zhuǎn)速反饋值,對輸出轉(zhuǎn)速進行實時調(diào)整,實現(xiàn)機器人差速轉(zhuǎn)向、直線行駛。由于作業(yè)空間及行駛環(huán)境復雜多變,系統(tǒng)須具備自適應(yīng)調(diào)整功能,同時對輸入信號具備高速、高精度響應(yīng)。
在典型時域線性控制系統(tǒng)采用的經(jīng)典PID控制器,通過對控制器比例、積分、微分參數(shù)的整定,使控制系統(tǒng)響應(yīng)速度提高,動態(tài)性能改善,消除穩(wěn)態(tài)誤差[12-13]。放射源應(yīng)急處置機器人電液行走控制系統(tǒng)在實際工作環(huán)境中,行駛負載存在時變性,負載的變化會產(chǎn)生功率損耗,經(jīng)典PID控制策略無法滿足系統(tǒng)的時變控制[14-15]。為實現(xiàn)系統(tǒng)控制目標,提出雙通道模糊PID控制策略,機器人電液行走模糊PID控制系統(tǒng)模型如圖4所示。在不同負載條件下測試液壓馬達的速度與加速度,對PID控制器參數(shù)進行整定,得到不同負載條件下最優(yōu)參數(shù)集,模糊規(guī)律如表1所示,表中a,b,c分別代表參量ΔKp,ΔKi,ΔKd。
表1 ΔKp,ΔKi,ΔKd模糊規(guī)律
圖4 機器人電液行走控制系統(tǒng)模型
放射源應(yīng)急處置機器人電液行走控制系統(tǒng)的電液比例換向閥及液壓馬達參數(shù)如表2所示。根據(jù)機器人電液行走控制系統(tǒng)閉環(huán)傳遞函數(shù)方框圖,在MATLAB仿真軟件中建立系統(tǒng)仿真模型,將表2所示液壓系統(tǒng)參數(shù)帶入控制系統(tǒng)閉環(huán)傳遞函數(shù)模型中,在控制模型中引入模糊PID控制器,在MATLAB仿真軟件中對系統(tǒng)仿真模型輸入幅值為1的階躍信號,仿真運行4 s,仿真系統(tǒng)函數(shù)對階躍信號的時域響應(yīng),將仿真結(jié)果輸出到圖形顯示元件中。
表2 液壓系統(tǒng)參數(shù)
將元件參數(shù)帶入放射源應(yīng)急處置機器人雙馬達電液比例閥控馬達行走控制系統(tǒng)數(shù)學模型中,對系統(tǒng)模型進行仿真,分析系統(tǒng)動態(tài)特性,驗證所設(shè)計的行走驅(qū)動控制系統(tǒng)的正確性。
對無PID矯正系統(tǒng)輸入幅值為1的階躍響應(yīng)信號ω,仿真時間4 s,在系統(tǒng)模型中引入模糊PID控制器,仿真結(jié)果如圖5所示。系統(tǒng)要求機器人行走系統(tǒng)對行進與制動2個工況的行駛響應(yīng)都具備優(yōu)良的動態(tài)響應(yīng)特性,對系統(tǒng)輸入周期性脈沖跟隨方波進行仿真,驗證系統(tǒng)行進與制動運動時控制系統(tǒng)輸出性能,系統(tǒng)響應(yīng)輸出如圖6所示。
圖5 系統(tǒng)階躍響應(yīng)時域曲線
圖6 系統(tǒng)階躍響應(yīng)跟隨曲線
對比無矯正系統(tǒng)和模糊PID控制器矯正系統(tǒng)的仿真曲線圖5及系統(tǒng)階躍響應(yīng)跟隨曲線圖6,可以看出:
(1) 無矯正系統(tǒng)在階躍信號的觸發(fā)下,時域輸出曲線在3 s后達到穩(wěn)態(tài)輸出,無超調(diào)。雙側(cè)行走馬達在1.5 s前最大速度響應(yīng)差值為輸入信號幅值的15%;
(2) 模糊PID矯正后系統(tǒng)響應(yīng)速度提高,在1.3 s時達到穩(wěn)態(tài)輸出,雙側(cè)行走馬達在0.3 s前最大速度響應(yīng)差值為輸入信號幅值的3%;
(3) PID矯正后系在周期階躍信號的觸發(fā)下,雙側(cè)行走馬達加減速響應(yīng)輸出最大速度響應(yīng)差值均低于輸入信號幅值的3%。
仿真結(jié)果表明,無矯正系統(tǒng)時域輸出曲線可達到穩(wěn)態(tài)輸出,穩(wěn)態(tài)誤差較小,無超調(diào),證明了所推導的數(shù)學模型的正確性。
系統(tǒng)無矯正前雙側(cè)行走馬達在1.5 s前最大速度響應(yīng)差值為輸入信號幅值的15%,無法滿足高速直線行走及快速轉(zhuǎn)向的要求。模糊PID矯正后系統(tǒng)穩(wěn)態(tài)響應(yīng)時間提高到1.3 s,雙側(cè)行走馬達在0.3 s前最大速度響應(yīng)最大差值降低為輸入信號幅值的3%,改善了系統(tǒng)動態(tài)響應(yīng)性能。
雙側(cè)行走馬達加減速響應(yīng)時間均明顯提高,達到穩(wěn)態(tài)輸出前雙側(cè)馬達速度差值均低于輸入信號幅值的3%,PID矯正后系統(tǒng)做行進與制動運動時,均具有較好的動態(tài)輸出特性。
進行實驗的機器人樣機如圖7所示,電液行走控制系統(tǒng)由液壓泵、液壓馬達、電液比例換向閥、控制器等組成,液壓馬達輸出軸安裝有SICK編碼器,液壓泵由直流伺服電機驅(qū)動。液壓泵采用A1V18定量泵,液壓馬達選用JT02VN,電液比例閥為4WRAE系列,所需輸入電流范圍為0~1 A;控制器采用EPEC3036,該控制器可編程輸出PWM電流范圍為0~1.03 A,分辨率為1 mA;控制器通過CAN總線接收速度傳感器數(shù)據(jù)。
圖7 實驗機器人樣機實物圖
如圖8所示,選用實驗場地為不平整砂石路面,在這種路面條件下,機器人在行走時所克服的行走阻力隨路面起伏變化。為驗證電液行走控制系統(tǒng)動態(tài)性能,在控制器中對系統(tǒng)加載勻加速、勻速、勻減速三段式速度運動曲線,驗證系統(tǒng)在不平整砂石路面條件下對不同運動條件的輸出響應(yīng)。
圖8 實驗現(xiàn)場
三段式速度運動曲線如圖9所示,勻加速階段加速度為1.5 rad/s2,勻速階段速度為3 rad/s,勻減速階段加速度為0.75 rad/s2。通過采集雙側(cè)編碼器速度反饋值繪制雙馬達轉(zhuǎn)速響應(yīng)曲線,如圖10所示。
圖9 三段式速度輸入曲線
圖10 馬達轉(zhuǎn)速實驗結(jié)果擬合曲線
實驗結(jié)果表明,在不平整砂石路面情況下,雙側(cè)馬達轉(zhuǎn)在加減速階段最大轉(zhuǎn)速穩(wěn)態(tài)誤差低于幅值2%,在勻速行走階段,最大轉(zhuǎn)速穩(wěn)態(tài)誤差低于幅值3%,試驗結(jié)果與仿真結(jié)果吻合,驗證了控制模型的正確性。所采集的馬達速度響應(yīng)值對輸入曲線的擬合優(yōu)度為0.96,試驗結(jié)果證明了系統(tǒng)在加速、勻速及減速度階段具有良好的響應(yīng)特性。
利用MATLAB仿真工具對所設(shè)計的模糊PID控制系統(tǒng)模型進行仿真驗證,仿真結(jié)果表明,系統(tǒng)響應(yīng)速度提高至1.3 s,速度差值低于輸入幅值3%,穩(wěn)定精度得到改善。通過樣機在負載砂石路面的三段速度行走實驗,證明了建立的閥控馬達速度控制系統(tǒng)數(shù)學模型及雙通道模糊PID電液比例速度控制策略的正確性及準確性。仿真及實驗結(jié)果證明所設(shè)計的全液壓履帶式電液比例閥控雙液壓馬達行走控制系統(tǒng)方案可以滿足放射源處置機器人對電液行走機構(gòu)的速度性能要求。