液壓驅(qū)動(dòng)型水下掘進(jìn)機(jī)器人臂架運(yùn)動(dòng)控制研究

劉修成，董奇峰，紀(jì)曉宇，范晨陽，孟林園

（1.中交第二航務(wù)工程局有限公司 技術(shù)中心，湖北 武漢 430040； 2.長(zhǎng)大橋梁建設(shè)施工技術(shù)交通行業(yè)重點(diǎn)實(shí)驗(yàn)室，湖北 武漢 430040； 3.燕山大學(xué) 機(jī)械工程學(xué)院，河北 秦皇島 066004）

液壓驅(qū)動(dòng)型機(jī)器人綜合機(jī)械、液壓、電氣、控制等多學(xué)科領(lǐng)域的先進(jìn)技術(shù)，因其具有負(fù)載能力強(qiáng)、響應(yīng)速度快、控制精度高及功率重量比大等優(yōu)點(diǎn)被廣泛應(yīng)用于工業(yè)制造、醫(yī)療服務(wù)、工程建造等技術(shù)領(lǐng)域［1-5］。串聯(lián)機(jī)械臂作為液壓驅(qū)動(dòng)型機(jī)器人的重要分支，其運(yùn)動(dòng)控制因應(yīng)用需求不同而成為該技術(shù)領(lǐng)域的重要研究方向。

在液壓機(jī)械臂運(yùn)動(dòng)控制中，主要目的是根據(jù)所規(guī)劃的運(yùn)動(dòng)軌跡執(zhí)行特定的任務(wù)。其中，軌跡規(guī)劃根據(jù)任務(wù)屬性而制定，軌跡跟蹤由液壓控制系統(tǒng)完成。覃艷明等［6-7］針對(duì)八自由度液壓鑿巖機(jī)器人任務(wù)需求，使用D-H 法求解機(jī)器人正、逆運(yùn)動(dòng)學(xué)方程，在關(guān)節(jié)空間內(nèi)采用五次多項(xiàng)式算法進(jìn)行軌跡規(guī)劃，并基于串級(jí)自抗擾控制器進(jìn)行軌跡跟蹤控制；王亞麗等［8-9］為實(shí)現(xiàn)五自由度光伏板清潔機(jī)器人液壓機(jī)械臂的精準(zhǔn)運(yùn)動(dòng)控制，基于D-H法和幾何法求解機(jī)器人運(yùn)動(dòng)學(xué)描述方程，使用比例-積分-微分（proportional integral derivative，PID）控制器對(duì)臂架電液比例控制系統(tǒng)進(jìn)行校正；黃振東等［10］根據(jù)3 自由度清污機(jī)器人液壓機(jī)械臂工作中路徑重復(fù)的特點(diǎn)，基于幾何法求解機(jī)器人正、逆運(yùn)動(dòng)學(xué)模型，使用位置反饋結(jié)合速度前饋的控制方法對(duì)電液控制系統(tǒng)進(jìn)行優(yōu)化。

上述學(xué)者基于各自工程領(lǐng)域的任務(wù)特點(diǎn)，對(duì)機(jī)器人液壓臂架運(yùn)動(dòng)控制的研究均取得了較好的應(yīng)用效果。水下掘進(jìn)機(jī)器人臂架主要應(yīng)用于沉井施工中刃腳位置的取土作業(yè)，本文根據(jù)其作業(yè)特點(diǎn)對(duì)其進(jìn)行運(yùn)動(dòng)控制研究。

1 運(yùn)動(dòng)學(xué)分析

1.1 總體結(jié)構(gòu)與作業(yè)流程

水下掘進(jìn)機(jī)器人總體結(jié)構(gòu)如圖1 所示，伸縮機(jī)械臂內(nèi)置伸縮油缸，可由擺幅油缸驅(qū)動(dòng)圍繞其與擺幅基座的鉸接點(diǎn)進(jìn)行回轉(zhuǎn)運(yùn)動(dòng)，同時(shí)擺幅基座由回轉(zhuǎn)驅(qū)動(dòng)控制可繞中心軸線進(jìn)行回轉(zhuǎn)運(yùn)動(dòng)。機(jī)器人臂架各關(guān)節(jié)驅(qū)動(dòng)方式均為液壓驅(qū)動(dòng)，為3 自由度機(jī)械臂。

圖1 水下掘進(jìn)機(jī)器人結(jié)構(gòu)Fig.1 Structure of underwater tunneling robot

為有效清理沉井刃腳位置的土層，水下掘進(jìn)機(jī)器人需控制銑挖頭深入刃腳下方進(jìn)行銑挖作業(yè)，主要作業(yè)流程如圖2 所示。首先為初始狀態(tài)：銑挖作業(yè)初始狀態(tài)如圖2（a）所示，此時(shí)擺幅油缸處于最大行程狀態(tài)，伸縮機(jī)械臂處于最小行程狀態(tài)；其次為下挖運(yùn)動(dòng)：擺幅油缸不動(dòng)作，伸縮機(jī)械臂伸長(zhǎng)指定距離，如圖2（b）所示；然后為擺幅運(yùn)動(dòng)：伸縮機(jī)械臂不動(dòng)作，擺幅油缸縮回指定距離，直至銑挖頭伸至刃腳下方，如圖2（c）所示；最后為重置運(yùn)動(dòng)：擺幅油缸不動(dòng)作，伸縮機(jī)械臂縮回至最小行程狀態(tài)，結(jié)束一輪銑挖作業(yè)，如圖2（d）所示。擺幅基座轉(zhuǎn)動(dòng)180°，重復(fù)上述流程可對(duì)另一側(cè)刃腳下方進(jìn)行銑挖作業(yè)。

圖2 作業(yè)流程Fig.2 Operation process chart

水下掘進(jìn)機(jī)器人取土作業(yè)中，銑挖頭需按照預(yù)定軌跡進(jìn)行運(yùn)動(dòng)，不僅需銑挖到位，還要避免碰觸刃腳，因此需要對(duì)銑挖頭進(jìn)行精確、穩(wěn)定的運(yùn)動(dòng)規(guī)劃和控制。

1.2 運(yùn)動(dòng)學(xué)模型

水下掘進(jìn)機(jī)器人執(zhí)行臂架第1 個(gè)關(guān)節(jié)繞回轉(zhuǎn)驅(qū)動(dòng)軸線運(yùn)動(dòng)時(shí)，對(duì)取土作業(yè)平面內(nèi)的運(yùn)動(dòng)無影響，因此可簡(jiǎn)化為2 自由度機(jī)械臂，建立其D-H 坐標(biāo)系，如圖3 所示。其中，基坐標(biāo)系｛0｝Y0軸垂直于紙面向里，坐標(biāo)系｛1｝Z1軸垂直紙面向外，坐標(biāo)系｛2｝Y2軸垂直紙面向外，工具坐標(biāo)系｛h｝與坐標(biāo)系｛2｝重合；θ1為桿件轉(zhuǎn)角，a0、a1、d2為桿件尺寸，C為X1軸與Z2軸交點(diǎn)。

圖3 臂架D-H坐標(biāo)系Fig.3 The D-H frame of the boom

根據(jù)連桿參數(shù)和坐標(biāo)系可得D-H 參數(shù)，見表1。

表1 水下掘進(jìn)機(jī)器人臂架D-H參數(shù)表Tab.1 D-H parameter table of boom

根據(jù)D-H 坐標(biāo)系和D-H 參數(shù)表可得各連桿間齊次變換矩陣。其中，相鄰坐標(biāo)系間齊次變換矩陣為

關(guān)節(jié)2坐標(biāo)相對(duì)于基坐標(biāo)系位姿矩陣為

代入表1參數(shù)，可得

式中：c1=cosθ1，s1=sinθ1。

由于工具坐標(biāo)系｛h｝與坐標(biāo)系｛2｝重合，則臂架末端銑挖頭坐標(biāo)相對(duì)于基坐標(biāo)系位姿矩陣為

式中：x、y、z為銑挖頭坐標(biāo)系原點(diǎn)相對(duì)于基坐標(biāo)系的位置坐標(biāo)；?為銑挖頭姿態(tài)角。

由臂架運(yùn)動(dòng)學(xué)正解式（4）可得其反解為

水下掘進(jìn)機(jī)器人執(zhí)行臂架由擺幅油缸和伸縮油缸調(diào)整2 個(gè)關(guān)節(jié)的運(yùn)動(dòng)，因此，各關(guān)節(jié)運(yùn)動(dòng)可轉(zhuǎn)換為對(duì)應(yīng)油缸的運(yùn)動(dòng)。設(shè)擺幅油缸初始長(zhǎng)度為L(zhǎng)1，伸縮量為x1，伸縮油缸伸縮量為x2。其中，AB=L1+x1，關(guān)節(jié)2為移動(dòng)關(guān)節(jié)，d2=3 855+x2。根據(jù)幾何關(guān)系可得

進(jìn)而可得擺幅油缸伸縮量與關(guān)節(jié)1轉(zhuǎn)換關(guān)系：

各幾何參數(shù)為L(zhǎng)1=930.0 mm，AO1=1 200.5 mm，BO1=650.0 mm，∠BO1C=94°。

1.3 軌跡規(guī)劃

軌跡規(guī)劃是機(jī)械臂運(yùn)動(dòng)軌跡控制的基礎(chǔ)，主要包括在笛卡爾空間和關(guān)節(jié)空間內(nèi)的軌跡規(guī)劃方法。其中，關(guān)節(jié)空間內(nèi)的軌跡規(guī)劃方法只需知道各關(guān)節(jié)始、末位姿即可規(guī)劃出所需路徑，且具有計(jì)算量小、可避免機(jī)器人奇異性問題等優(yōu)點(diǎn)［11］。結(jié)合水下掘進(jìn)機(jī)器人臂架作業(yè)特點(diǎn)，采用關(guān)節(jié)空間內(nèi)的軌跡規(guī)劃方法。

軌跡規(guī)劃算法的選取對(duì)臂架運(yùn)動(dòng)性能有著重要意義，其中，五次多項(xiàng)式算法能夠?qū)﹃P(guān)節(jié)運(yùn)動(dòng)始、末角度，角速度和角加速度進(jìn)行約束，有利于臂架作業(yè)的平穩(wěn)性和安全性。設(shè)臂架關(guān)節(jié)運(yùn)動(dòng)始、末時(shí)間分別為t0和tf，始、末位移分別為θ0和θf，則五次多項(xiàng)式插值函數(shù)可表示為

式中：分別為角度、角速度和角加速度約束函數(shù)，其中，θ（t0）=θ0，θ（tf）=θf。

式中：α=θf-θ0。

擺幅油缸和伸縮油缸位移可根據(jù)與對(duì)應(yīng)驅(qū)動(dòng)關(guān)節(jié)之間的幾何關(guān)系轉(zhuǎn)換為五次多項(xiàng)式位移函數(shù)，因此，可將關(guān)節(jié)內(nèi)的軌跡規(guī)劃轉(zhuǎn)換到驅(qū)動(dòng)空間，從而控制液壓油缸進(jìn)行軌跡跟蹤控制。

2 軌跡跟蹤控制系統(tǒng)

由運(yùn)動(dòng)學(xué)分析可知，作業(yè)過程中，水下掘進(jìn)機(jī)器人銑挖頭的運(yùn)動(dòng)軌跡控制最終轉(zhuǎn)換為擺幅油缸和伸縮油缸的位移控制。臂架控制系統(tǒng)為電液比例位置控制系統(tǒng)，該系統(tǒng)是一種時(shí)變性、非線性系統(tǒng)，且存在著不確定的擾動(dòng)因素，為實(shí)現(xiàn)較好的軌跡跟蹤效果，采用模糊PID控制。

2.1 電液比例控制系統(tǒng)線性模型

電液比例位置控制系統(tǒng)結(jié)構(gòu)如圖4 所示，其中，液壓缸伸縮量通過位移傳感器反饋至比較器，與給定信號(hào)比較后，控制器通過比例閥對(duì)液壓缸進(jìn)行糾偏控制，進(jìn)而使液壓缸沿給定信號(hào)進(jìn)行運(yùn)動(dòng)。銑挖頭在作業(yè)過程中對(duì)液壓缸造成的負(fù)載質(zhì)量和受力變化可統(tǒng)一作為系統(tǒng)的外部干擾。

圖4 控制系統(tǒng)結(jié)構(gòu)Fig.4 Structure of control system

電液比例閥控非對(duì)稱液壓缸位置控制系統(tǒng)模型如圖5所示［12］，圖中，Ka為比例放大器增益，Ksv為比例閥增益，ωsv為比例閥頻率，ξsv為比例閥阻尼比，Kq為比例閥流量增益，Ap為有效油缸面積，Kce為總流量壓力系數(shù)，V為液壓缸總?cè)莘e，βe為液壓油有效體積彈性模量，ωp為液壓固有頻率，ξp為液壓阻尼比系數(shù)，Kx為位移傳感器增益，各參數(shù)值見表2。

表2 閥控缸系統(tǒng)參數(shù)Tab.2 Parameters of valve-controlled cylinder system

圖5 電液比例位置控制系統(tǒng)模型Fig.5 Model of electro-hydraulic proportional position control system

2.2 模糊PID控制器設(shè)計(jì)

模糊PID 控制是將模糊控制和PID 控制相結(jié)合的一種控制方法，基于一定模糊規(guī)則對(duì)PID 控制器的3 個(gè)參數(shù)進(jìn)行實(shí)時(shí)優(yōu)化，能夠適應(yīng)存在時(shí)變性、非線性等模型多變的控制系統(tǒng)，因此其控制效果要優(yōu)于常規(guī)PID 控制［13］。模糊PID 控制原理如圖6 所示，其中，e、ec分別為系統(tǒng)偏差和偏差變化率，經(jīng)模糊化和模糊推理后得到修正參數(shù)ΔKP、ΔKI和ΔKD，從而實(shí)現(xiàn)對(duì)PID 控制器KP、KI和KD參數(shù)的實(shí)時(shí)調(diào)整。

圖6 模糊PID控制原理Fig.6 Schematic of fuzzy PID control

將變量e、ec、ΔKP、ΔKI和ΔKD的論域離散為{NB，NM，NS，ZO，PS，PM，PB}7 個(gè)子集，分別表示為負(fù)大、負(fù)中、負(fù)小、零、正小、正中和正大。各變量采用三角形隸屬度函數(shù)，取值范圍和控制器模糊規(guī)則見文獻(xiàn)［14］。

2.3 仿真分析

在Matlab/Simulink 環(huán)境下，搭建基于模糊PID 的閥控缸電液比例位置控制系統(tǒng)模型，檢驗(yàn)所設(shè)計(jì)控制器的控制效果。為驗(yàn)證系統(tǒng)的響應(yīng)速度，輸入幅值為0.1 m 的階躍信號(hào)，仿真結(jié)果如圖7 所示。由圖7 可知，經(jīng)模糊PID 校正后，擺幅油缸調(diào)整時(shí)間為0.42 s，相較于校正前響應(yīng)速度提高了96.7%且系統(tǒng)無超調(diào)；伸縮油缸調(diào)整時(shí)間為0.38 s，相較于校正前響應(yīng)速度提高了95.5%，且系統(tǒng)無超調(diào)。

圖7 階躍響應(yīng)曲線Fig.7 Step response curve

為驗(yàn)證系統(tǒng)的跟隨精度，輸入斜率為0.01 m/s的斜坡信號(hào)，仿真結(jié)果如圖8所示。由圖8可知：經(jīng)模糊PID 校正后，擺幅油缸最大跟隨誤差為1.7 mm，相較于校正前跟隨精度提高了95.9%；伸縮油缸最大跟隨誤差為1.8 mm，相較于校正前跟隨精度提高了93.0%。

圖8 斜坡響應(yīng)曲線Fig.8 Slope response curve

由仿真分析可知，系統(tǒng)加入模糊PID 控制器后，系統(tǒng)響應(yīng)速度和跟隨精度均得到了較大提升。

3 臂架軌跡運(yùn)動(dòng)控制試驗(yàn)

為驗(yàn)證模糊PID 控制器的實(shí)際控制效果，在水下掘進(jìn)機(jī)器人實(shí)驗(yàn)平臺(tái)進(jìn)行應(yīng)用試驗(yàn)，如圖9所示。

圖9 實(shí)驗(yàn)平臺(tái)Fig.9 The experimental platform

分2 個(gè)階段分別對(duì)伸縮油缸和擺幅油缸進(jìn)行試驗(yàn)：第1 階段，伸縮臂伸長(zhǎng)量由0.10 m 伸長(zhǎng)至0.50 m，耗時(shí)50 s；第2 階段，擺幅油缸伸長(zhǎng)量由0.64 m 縮回至0.34 m，耗時(shí)100 s。采用5次多項(xiàng)式算法對(duì)油缸運(yùn)動(dòng)進(jìn)行軌跡規(guī)劃后，進(jìn)行軌跡跟蹤控制，試驗(yàn)結(jié)果如圖10所示。

圖10 軌跡跟蹤曲線Fig.10 Trajectory tracking curve

由試驗(yàn)結(jié)果可知，在實(shí)際取土作業(yè)中，臂架軌跡控制精度雖然受到內(nèi)、外部非線性因素干擾的影響，伸縮油缸軌跡跟蹤誤差為7.3 mm，擺幅油缸軌跡跟蹤誤差為6.9 mm，但誤差均小于10.0 mm，符合使用要求，證明了軌跡規(guī)劃算法、軌跡跟蹤控制算法的有效性和實(shí)用性。

4 結(jié)語

本文根據(jù)水下掘進(jìn)機(jī)器人臂架作業(yè)運(yùn)動(dòng)特點(diǎn)，對(duì)其進(jìn)行軌跡規(guī)劃和軌跡跟蹤控制研究，通過理論分析、仿真和試驗(yàn)研究，得出如下結(jié)論：機(jī)器人臂架取土作業(yè)每個(gè)流程均為始、末關(guān)節(jié)角度已知的運(yùn)動(dòng)，且無須考慮中間點(diǎn)軌跡，考慮臂架運(yùn)動(dòng)的穩(wěn)定性，采用關(guān)節(jié)內(nèi)5 次多項(xiàng)式軌跡規(guī)劃算法對(duì)其進(jìn)行軌跡規(guī)劃；利用模糊PID 控制器對(duì)驅(qū)動(dòng)控制系統(tǒng)進(jìn)行校正，相較于未校正前，既提高了控制系統(tǒng)的響應(yīng)速度又提升了跟隨精度；取土作業(yè)試驗(yàn)結(jié)果表明，機(jī)器人臂架運(yùn)動(dòng)的軌跡規(guī)劃算法和軌跡跟蹤控制算法在實(shí)際應(yīng)用中能達(dá)到預(yù)期的效果，具有一定的可行性和有效性。