液壓驅(qū)動(dòng)機(jī)器人速度控制的研究

劉廣亮，肖永飛，趙永國，孫 潔

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

移動(dòng)機(jī)器人驅(qū)動(dòng)方式有多種形式，主要包括電機(jī)驅(qū)動(dòng)，液壓驅(qū)動(dòng)和氣壓驅(qū)動(dòng)等。 其中液壓驅(qū)動(dòng)方式具有動(dòng)力大、力與慣量比大、快速響應(yīng)性高、易于實(shí)現(xiàn)直接驅(qū)動(dòng)等特點(diǎn)，適用于承載能力大、高功率密度比、抗沖擊以及防爆工作環(huán)境。 與電機(jī)驅(qū)動(dòng)和氣壓驅(qū)動(dòng)相比， 液壓驅(qū)動(dòng)具有更高的功率密度，能夠滿足機(jī)器人野外運(yùn)輸所需的高負(fù)重及快速運(yùn)動(dòng)需求，同時(shí)液壓驅(qū)動(dòng)系統(tǒng)可以采用內(nèi)燃機(jī)提供能源動(dòng)力，通過添加燃料進(jìn)行快速補(bǔ)給，具有長時(shí)間的續(xù)航能力， 比電能源機(jī)器人更適合從事野外作業(yè)。目前廣泛應(yīng)用于四足、六足、多足等腿足式機(jī)器人以及煤礦井下機(jī)器人[1-4]。

由于液壓系統(tǒng)具有時(shí)滯性、強(qiáng)非線性、時(shí)變負(fù)載性的特點(diǎn)，所以液壓機(jī)器人的速度控制精度一直是難點(diǎn)問題，在此方面研究人員開展了很多研究工作。文獻(xiàn)[5]提出基于RBF 神經(jīng)網(wǎng)絡(luò)的自適應(yīng)最優(yōu)控制系統(tǒng)。 文獻(xiàn)[6]提出采用非線性特性分段校正方法來提高機(jī)器人控制性能的方法。 文獻(xiàn)[7]研究了液壓負(fù)載補(bǔ)償與跟蹤誤差補(bǔ)償結(jié)合的力控制方法，將閥控系統(tǒng)與泵控系統(tǒng)結(jié)合成新型的泵閥復(fù)合驅(qū)動(dòng)系統(tǒng)，既降低系統(tǒng)的能量損耗，又保證了液壓驅(qū)動(dòng)單元響應(yīng)速度和響應(yīng)精度。

本文主要針對(duì)移動(dòng)機(jī)器人的液壓驅(qū)動(dòng)啟動(dòng)死區(qū)、各個(gè)比例閥不均衡等非線性與時(shí)變負(fù)載問題，對(duì)液壓驅(qū)動(dòng)的機(jī)器人速度精確控制進(jìn)行研究。 采用分段補(bǔ)償和分段線性校正方法，設(shè)計(jì)了兩輪速度同步補(bǔ)償算法，實(shí)現(xiàn)移動(dòng)機(jī)器人兩輪液壓馬達(dá)的同步控制，經(jīng)驗(yàn)證機(jī)器人精度控制性能得到很大改善，速度控制精度和控制響應(yīng)提升明顯。

1 機(jī)器人獨(dú)立兩輪差速運(yùn)動(dòng)學(xué)模型

獨(dú)立兩輪差速運(yùn)動(dòng)學(xué)模型如圖1 所示，全局坐標(biāo)系｛XOY｝，局部坐標(biāo)系｛X′O′Y′｝。 局部坐標(biāo)系Y′軸與兩輪的中軸線重合，方向從右輪指向左輪。 X′軸方向?yàn)闄C(jī)器人的正前方。 假設(shè)機(jī)器人質(zhì)心C 位于兩驅(qū)動(dòng)輪軸線的中點(diǎn)，與坐標(biāo)系原點(diǎn)O′重合。 局部坐標(biāo)系相對(duì)于全局坐標(biāo)系的旋轉(zhuǎn)角為θ， 為機(jī)器人的方向角。

圖1 兩輪差速運(yùn)動(dòng)學(xué)模型Fig.1 Kinematic model of two-wheel differential drive

局部坐標(biāo)系到全局坐標(biāo)系的映射正交旋轉(zhuǎn)矩陣為

假定兩驅(qū)動(dòng)輪軸線的長度為l， 左右驅(qū)動(dòng)輪半徑都為r，機(jī)器人的位姿向量為p=（x，y，θ）T。 兩輪差速驅(qū)動(dòng)移動(dòng)機(jī)器人的正運(yùn)動(dòng)學(xué)方程[8]為

式中：v 為機(jī)器人質(zhì)心C 處的線速度；w 為機(jī)器人的轉(zhuǎn)向角速度；vr，vl分別為機(jī)器人的左右驅(qū)動(dòng)輪線速度。

在全局坐標(biāo)系下，機(jī)器人的代表點(diǎn)C 的位置和姿態(tài)可以通過式（2）中的速度關(guān)系式積分求得：

在航跡演算中根據(jù)式（3）計(jì)算機(jī)器人姿態(tài)軌跡。

假設(shè)相鄰時(shí)間段內(nèi)機(jī)器人曲線航跡近似為直線，如圖2 所示[9]。 同時(shí)假設(shè)機(jī)器人不是全向移動(dòng)，只是前進(jìn)和旋轉(zhuǎn)，不能縱向移動(dòng)。 機(jī)器人兩個(gè)相鄰運(yùn)動(dòng)時(shí)刻在采樣周期內(nèi)運(yùn)動(dòng)距離短，可以將軌跡看作勻速直線運(yùn)動(dòng)，也就是說機(jī)器人在Δt 時(shí)間內(nèi)沿局部坐標(biāo)系X′軸移動(dòng)了vΔt， 在全局坐標(biāo)系統(tǒng)中移動(dòng)距離為

圖2 相鄰點(diǎn)軌跡的直線近似Fig.2 Linear approximation of adjacent trajectory points

那么，t+1 時(shí)刻機(jī)器人姿態(tài)為

根據(jù)式（1）可知機(jī)器人中心點(diǎn)線速度v 為

角速度w 為

若vr=vl，機(jī)器人軌跡為直線段；若vr≠vl，機(jī)器人軌跡為圓弧。

機(jī)器人的逆運(yùn)動(dòng)學(xué)可以根據(jù)正運(yùn)動(dòng)學(xué)式（1）、式（2）及式（6）、式（7）求得。

即：

通常給定機(jī)器人中心點(diǎn)的線速度和角速度，然后根據(jù)式（8）、式（9）推出左右兩個(gè)驅(qū)動(dòng)輪的線速度，也是機(jī)器人的驅(qū)動(dòng)電機(jī)線速度vr，vl與機(jī)器人線速度v 及和角速度w 間的關(guān)系。

在控制系統(tǒng)驅(qū)動(dòng)電機(jī)時(shí)，需要將驅(qū)動(dòng)電機(jī)線速度轉(zhuǎn)換為電機(jī)的轉(zhuǎn)速值。設(shè)電機(jī)的轉(zhuǎn)速為n，單位為r/min，線速度的單位為m/s，由于：

則有：

式中：nr，nl分別為差速運(yùn)動(dòng)時(shí)左右輪轉(zhuǎn)速值，單位為r/min。

2 最大速度與加減速限制

為了實(shí)現(xiàn)液壓機(jī)器人平穩(wěn)精準(zhǔn)的運(yùn)動(dòng)，需要對(duì)機(jī)器人速度和加速度進(jìn)行規(guī)劃[10]，確保機(jī)器人在運(yùn)動(dòng)過程中速度不突變，平穩(wěn)過渡，從而避免出現(xiàn)機(jī)器人機(jī)械沖擊和失步震蕩。 同時(shí)由于機(jī)器人本體機(jī)械性能限定了機(jī)器人的速度范圍，超出速度限制的指令將是無效的，因此需要通過速度最大值限制方式對(duì)機(jī)器人速度、加速度、加加速度進(jìn)行加減速控制[11]。

2.1 最大速度

差速運(yùn)動(dòng)控制系統(tǒng)中（如ROS 的差速運(yùn)動(dòng)控制器[12]）需要配置速度的最大最小值vmax，vmin，最終速度值為

式中的vmax，vmin在實(shí)際應(yīng)用中分別包括左右輪各兩個(gè)速度：機(jī)器人線速度和角速度。 從式（8）可知線速度和角速度要轉(zhuǎn)化為兩輪的轉(zhuǎn)速n。 而對(duì)每個(gè)特定的驅(qū)動(dòng)電機(jī)轉(zhuǎn)速是有限制的（假設(shè)為最大轉(zhuǎn)速為nmax），即線速度和角速度合并后的兩輪轉(zhuǎn)速n 不能大于兩輪的最大轉(zhuǎn)速nmax，所以在設(shè)定機(jī)器人本體的最大線速度和角速度的時(shí)候需要考慮與驅(qū)動(dòng)電機(jī)最大轉(zhuǎn)速之間的匹配。給定一個(gè)機(jī)器人線速度v，那么在驅(qū)動(dòng)輪最大轉(zhuǎn)速的限制下就會(huì)有一個(gè)wmax，如果角速度指令超出這個(gè)wmax值，機(jī)器人也只能按wmax執(zhí)行轉(zhuǎn)向，結(jié)果會(huì)造成機(jī)器人轉(zhuǎn)向不足。

假設(shè)驅(qū)動(dòng)輪的最大轉(zhuǎn)速為nmax，那么根據(jù)式（11）得到兩輪最大線速度：

再根據(jù)式（6）、式（7）可知機(jī)器人的線速度和角速度最大為

但是這兩個(gè)最大值不可能同時(shí)達(dá)到，當(dāng)轉(zhuǎn)向速度最大達(dá)到wmax時(shí)：

根據(jù)式（6），此時(shí)機(jī)器人線速度v=0。 而當(dāng)線速度最大達(dá)到vmax時(shí)：

再根據(jù)式（7），此時(shí)機(jī)器人角速度w=0。

假設(shè)給定機(jī)器人額定線速度v~， 如果其中一個(gè)驅(qū)動(dòng)輪（如右輪）達(dá)到最大轉(zhuǎn)速，則此時(shí)另一個(gè)驅(qū)動(dòng)輪（左輪）速度為

此時(shí)機(jī)器人角速度w 為

因此在機(jī)器人驅(qū)動(dòng)電機(jī)最大轉(zhuǎn)速限制下，機(jī)器人角速度與線速度存在制約關(guān)系，如圖3 所示，需要根據(jù)實(shí)際需要分別設(shè)置機(jī)器人移動(dòng)的線速度和角速度，并保證線速度和角速度合并后不會(huì)超過驅(qū)動(dòng)電機(jī)最大轉(zhuǎn)速限制。

圖3 角速度與線速度之間的制約關(guān)系Fig.3 Constraint relation between angular and linear velocity

2.2 加速度控制

在機(jī)器人加減速控制中可以根據(jù)S 曲線加減速法[13]進(jìn)行配置。 假設(shè)差速運(yùn)動(dòng)控制器中配置加速度的最大最小值分比為amax，amin，vk，vk+1，時(shí)間差為Δt，那么Δt 時(shí)間內(nèi)對(duì)應(yīng)最大最小速度變化值分別為dvmax，dvmin，那么：

最后速度：

2.3 加速度控制

同理，假設(shè)差速運(yùn)動(dòng)控制器中配置加速度的最大最小值分別為jmax，jmin，vk，vk+1時(shí)間差為Δt，那么Δt 時(shí)間內(nèi)對(duì)應(yīng)最大最小加速度變化值分別為damax，damin，那么：

最后速度：

3 液壓驅(qū)動(dòng)機(jī)器人速度控制

3.1 液壓驅(qū)動(dòng)速度控制存在的問題

由于液壓系統(tǒng)的時(shí)滯性和時(shí)變負(fù)載性，造成液壓驅(qū)動(dòng)與伺服電機(jī)驅(qū)動(dòng)有很大不同，首先液壓馬達(dá)具有啟動(dòng)閾值（死區(qū)），輸入值低于啟動(dòng)閾值馬達(dá)不運(yùn)轉(zhuǎn)，其次移動(dòng)機(jī)器人的兩個(gè)液壓馬達(dá)，存在流量分配不均勻的問題，即給兩個(gè)液壓馬達(dá)同樣的輸入指令值，左右液壓馬達(dá)運(yùn)行速度快慢不一樣，并且在前進(jìn)方向和后退方向也存在輸出比例不一致的問題。

如圖4 所示，對(duì)機(jī)器人左右輪前進(jìn)與后退方向進(jìn)行速度的測試，從得到測試數(shù)據(jù)分析可以看到左右輪輸入啟動(dòng)閾值不同，前進(jìn)方向左輪啟動(dòng)閾值約385，右輪啟動(dòng)閾值約357（注：液壓控制器輸入最大值1000）。輸入輸出比也不一致，同樣的輸入左輪慢右輪快。 并且左右輪在前進(jìn)方向和后退方向，啟動(dòng)閾值也不同，右輪在前進(jìn)方向啟動(dòng)閾值為357，后退方向啟動(dòng)閾值都是385。 圖4 充分顯示了液壓系統(tǒng)的輸入輸出的非線性特點(diǎn)。

圖4 左右輪原始速度數(shù)據(jù)分析Fig.4 Analysis of two wheel original velocity data

3.2 速度控制模型

在機(jī)器人自主控制時(shí)，必須保證驅(qū)動(dòng)電機(jī)的線性輸出，即同樣輸入指令機(jī)器人的左右輪以及前進(jìn)和后退都應(yīng)該是同樣的輸出，這樣才能保證液壓機(jī)器人平穩(wěn)準(zhǔn)確運(yùn)行，為此構(gòu)建基于分段線性擬合的液壓機(jī)器人輸入輸出模型如下：

式中：c 為啟動(dòng)閾值；vin為輸入值；k 為系數(shù)；下標(biāo)r，l表示右輪和左輪； 下標(biāo)b，f 表示前進(jìn)和后退。 如kfr表示前進(jìn)方向右輪系數(shù)。

將整個(gè)液壓系統(tǒng)劃分為左右輪和前進(jìn)后退方向4 種情況分別進(jìn)行分段數(shù)據(jù)線性擬合和分段線性校正，從而獲得擬合參數(shù)，見表1。

表1 速度控制模型擬合參數(shù)表Tab.1 Parameters of velocity control model fit

然后在首次擬合的基礎(chǔ)上，進(jìn)行多組機(jī)器人樣機(jī)試驗(yàn)驗(yàn)證分析，在同時(shí)兼顧左右輪和前后方向匹配，獲取最終的優(yōu)化參數(shù)，見表2。

表2 速度控制模型優(yōu)化參數(shù)表Tab.2 Optimized parameters of velocity control model

4 實(shí)驗(yàn)驗(yàn)證

4.1 液壓機(jī)器人樣機(jī)

液壓機(jī)器人樣機(jī)如圖5所示，為履帶式內(nèi)燃機(jī)驅(qū)動(dòng)機(jī)器人。

圖5 液壓驅(qū)動(dòng)機(jī)器人Fig.5 Hydraulically driven robot

液壓驅(qū)動(dòng)部分采用EPEC 液壓控制器控制比例閥，完成對(duì)液壓馬達(dá)的轉(zhuǎn)速控制。 內(nèi)燃機(jī)的發(fā)動(dòng)機(jī)為洋馬2TNV70-PYU，發(fā)動(dòng)機(jī)功率7.3 kW，轉(zhuǎn)速2400 r·min-1，液壓系統(tǒng)采用力士樂，液壓流量20 L/min，液壓系統(tǒng)壓力25 MPa。 機(jī)器人驅(qū)動(dòng)輪距0.8 m，驅(qū)動(dòng)輪直徑0.285 m。

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

對(duì)采用優(yōu)化參數(shù)的液壓機(jī)器人樣機(jī)對(duì)進(jìn)行了測試，結(jié)果如圖6 所示，實(shí)驗(yàn)結(jié)果表明在相同輸入下左右輪、前進(jìn)方向、后退方向速度輸出基本匹配一致，具有線性關(guān)系。

圖6 左右輪速度對(duì)比Fig.6 Comparison of left and right wheel velocity

5 結(jié)語

液壓驅(qū)動(dòng)機(jī)器人具有動(dòng)力性能好，越野能力強(qiáng)的特點(diǎn)，特別適用于特種作業(yè)環(huán)境，但是液壓驅(qū)動(dòng)控制模型復(fù)雜，精確控制難度大。 本文在分析了機(jī)器人差速運(yùn)動(dòng)控制理論模型和加減速控制基礎(chǔ)上，詳細(xì)研究了液壓驅(qū)動(dòng)速度控制模型，采用分段數(shù)據(jù)線性擬合和校正的方法，對(duì)機(jī)器人速度控制參數(shù)進(jìn)行了優(yōu)化，并應(yīng)用于內(nèi)燃機(jī)驅(qū)動(dòng)液壓機(jī)器人，經(jīng)測試驗(yàn)證機(jī)器人運(yùn)行平穩(wěn)可靠。