液壓驅動型水下掘進機器人臂架運動控制研究

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

（1.中交第二航務工程局有限公司 技術中心，湖北 武漢 430040； 2.長大橋梁建設施工技術交通行業重點實驗室，湖北 武漢 430040； 3.燕山大學 機械工程學院，河北 秦皇島 066004）

液壓驅動型機器人綜合機械、液壓、電氣、控制等多學科領域的先進技術，因其具有負載能力強、響應速度快、控制精度高及功率重量比大等優點被廣泛應用于工業制造、醫療服務、工程建造等技術領域［1-5］。串聯機械臂作為液壓驅動型機器人的重要分支，其運動控制因應用需求不同而成為該技術領域的重要研究方向。

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

上述學者基于各自工程領域的任務特點，對機器人液壓臂架運動控制的研究均取得了較好的應用效果。水下掘進機器人臂架主要應用于沉井施工中刃腳位置的取土作業，本文根據其作業特點對其進行運動控制研究。

1 運動學分析

1.1 總體結構與作業流程

水下掘進機器人總體結構如圖1 所示，伸縮機械臂內置伸縮油缸，可由擺幅油缸驅動圍繞其與擺幅基座的鉸接點進行回轉運動，同時擺幅基座由回轉驅動控制可繞中心軸線進行回轉運動。機器人臂架各關節驅動方式均為液壓驅動，為3 自由度機械臂。

圖1 水下掘進機器人結構Fig.1 Structure of underwater tunneling robot

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

圖2 作業流程Fig.2 Operation process chart

水下掘進機器人取土作業中，銑挖頭需按照預定軌跡進行運動，不僅需銑挖到位，還要避免碰觸刃腳，因此需要對銑挖頭進行精確、穩定的運動規劃和控制。

1.2 運動學模型

水下掘進機器人執行臂架第1 個關節繞回轉驅動軸線運動時，對取土作業平面內的運動無影響，因此可簡化為2 自由度機械臂，建立其D-H 坐標系，如圖3 所示。其中，基坐標系｛0｝Y0軸垂直于紙面向里，坐標系｛1｝Z1軸垂直紙面向外，坐標系｛2｝Y2軸垂直紙面向外，工具坐標系｛h｝與坐標系｛2｝重合；θ1為桿件轉角，a0、a1、d2為桿件尺寸，C為X1軸與Z2軸交點。

圖3 臂架D-H坐標系Fig.3 The D-H frame of the boom

根據連桿參數和坐標系可得D-H 參數，見表1。

表1 水下掘進機器人臂架D-H參數表Tab.1 D-H parameter table of boom

根據D-H 坐標系和D-H 參數表可得各連桿間齊次變換矩陣。其中，相鄰坐標系間齊次變換矩陣為

關節2坐標相對于基坐標系位姿矩陣為

代入表1參數，可得

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

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

式中：x、y、z為銑挖頭坐標系原點相對于基坐標系的位置坐標；?為銑挖頭姿態角。

由臂架運動學正解式（4）可得其反解為

水下掘進機器人執行臂架由擺幅油缸和伸縮油缸調整2 個關節的運動，因此，各關節運動可轉換為對應油缸的運動。設擺幅油缸初始長度為L1，伸縮量為x1，伸縮油缸伸縮量為x2。其中，AB=L1+x1，關節2為移動關節，d2=3 855+x2。根據幾何關系可得

進而可得擺幅油缸伸縮量與關節1轉換關系：

各幾何參數為L1=930.0 mm，AO1=1 200.5 mm，BO1=650.0 mm，∠BO1C=94°。

1.3 軌跡規劃

軌跡規劃是機械臂運動軌跡控制的基礎，主要包括在笛卡爾空間和關節空間內的軌跡規劃方法。其中，關節空間內的軌跡規劃方法只需知道各關節始、末位姿即可規劃出所需路徑，且具有計算量小、可避免機器人奇異性問題等優點［11］。結合水下掘進機器人臂架作業特點，采用關節空間內的軌跡規劃方法。

軌跡規劃算法的選取對臂架運動性能有著重要意義，其中，五次多項式算法能夠對關節運動始、末角度，角速度和角加速度進行約束，有利于臂架作業的平穩性和安全性。設臂架關節運動始、末時間分別為t0和tf，始、末位移分別為θ0和θf，則五次多項式插值函數可表示為

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

式中：α=θf-θ0。

擺幅油缸和伸縮油缸位移可根據與對應驅動關節之間的幾何關系轉換為五次多項式位移函數，因此，可將關節內的軌跡規劃轉換到驅動空間，從而控制液壓油缸進行軌跡跟蹤控制。

2 軌跡跟蹤控制系統

由運動學分析可知，作業過程中，水下掘進機器人銑挖頭的運動軌跡控制最終轉換為擺幅油缸和伸縮油缸的位移控制。臂架控制系統為電液比例位置控制系統，該系統是一種時變性、非線性系統，且存在著不確定的擾動因素，為實現較好的軌跡跟蹤效果，采用模糊PID控制。

2.1 電液比例控制系統線性模型

電液比例位置控制系統結構如圖4 所示，其中，液壓缸伸縮量通過位移傳感器反饋至比較器，與給定信號比較后，控制器通過比例閥對液壓缸進行糾偏控制，進而使液壓缸沿給定信號進行運動。銑挖頭在作業過程中對液壓缸造成的負載質量和受力變化可統一作為系統的外部干擾。

圖4 控制系統結構Fig.4 Structure of control system

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

表2 閥控缸系統參數Tab.2 Parameters of valve-controlled cylinder system

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

2.2 模糊PID控制器設計

模糊PID 控制是將模糊控制和PID 控制相結合的一種控制方法，基于一定模糊規則對PID 控制器的3 個參數進行實時優化，能夠適應存在時變性、非線性等模型多變的控制系統，因此其控制效果要優于常規PID 控制［13］。模糊PID 控制原理如圖6 所示，其中，e、ec分別為系統偏差和偏差變化率，經模糊化和模糊推理后得到修正參數ΔKP、ΔKI和ΔKD，從而實現對PID 控制器KP、KI和KD參數的實時調整。

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

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

2.3 仿真分析

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

圖7 階躍響應曲線Fig.7 Step response curve

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

圖8 斜坡響應曲線Fig.8 Slope response curve

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

3 臂架軌跡運動控制試驗

為驗證模糊PID 控制器的實際控制效果，在水下掘進機器人實驗平臺進行應用試驗，如圖9所示。

圖9 實驗平臺Fig.9 The experimental platform

分2 個階段分別對伸縮油缸和擺幅油缸進行試驗：第1 階段，伸縮臂伸長量由0.10 m 伸長至0.50 m，耗時50 s；第2 階段，擺幅油缸伸長量由0.64 m 縮回至0.34 m，耗時100 s。采用5次多項式算法對油缸運動進行軌跡規劃后，進行軌跡跟蹤控制，試驗結果如圖10所示。

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

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

4 結語

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