王 輝，盛 榮，郭劍東

1南京航空航天大學自動化學院 江蘇南京 210016

2徐州威卡電子控制技術(shù)有限公司 江蘇徐州 221122

3南京航空航天大學中小型無人機先進技術(shù)工信部重點實驗室 江蘇南京 210016

鑿 巖機器人控制的關鍵點是鉆臂移動及定位控制，其臂架多數(shù)為采用三角結(jié)構(gòu)且具有冗余自由度的機械臂，是一種多輸入多輸出、交叉耦合的非線性系統(tǒng)控制。在小負載機器人領域，Jiang 等人[1]實時測量跟蹤機械臂的運動，利用在線測量數(shù)據(jù)進行不斷迭代補償從而提高精度。陳綿鵬等人[2]利用Levenberg-Marquardt 算法辨識出誤差模型中的誤差參數(shù)并進行補償，最終提高機械臂末端定位精度。Zhang 等人[3]利用曲線擬合方法識別機械臂末端運動軌跡特征，實現(xiàn)機械臂 D-H 模型中的參數(shù)校準。周宏兵[4]著眼于整個鉆臂的運動分析，建立了鉆臂運動學方程，但是沒有對三角鉆臂進行運動分析。周永行等人[5]研究了 2 個驅(qū)動變量 (驅(qū)動液壓缸) 和 2 個目標參數(shù) (機械臂的空間位置) 之間的耦合關系，借助空間幾何的方法得到了近似的線性關系，實現(xiàn)了鉆臂的運動控制，但是推導中對三角支撐的驅(qū)動機構(gòu)進行了簡化，會帶來控制誤差。鑿巖機器人通常在隧道中進行工作，小負載機器人領域中的方法難以滿足其惡劣工況。鑿巖機器人機械臂屬于復雜的非線性系統(tǒng)，機械臂受重力影響柔性變形較大，所以曲線擬合和空間插值方法在鑿巖機器人誤差補償應用中具有一定的局限性。

筆者首先構(gòu)建單三角鉆臂運動學模型，對鉆臂支臂缸進行運動學解算，從理論上對三角鉆臂進行運動學解耦，得出驅(qū)動液壓缸變量和機械臂空間位置之間的關系，并使用 PID 算法[6]對機械臂進行控制，而后對鑿巖機械臂進行動力學建模，提出一種線性二次型調(diào)節(jié)器 LQR (Linear Quadratic Regulator)[7-8]，并對控制器進行仿真和試驗比對控制效果，驗證了控制器的有效性。

1 鑿巖機器人機械臂簡介

1.1 單三角結(jié)構(gòu)鉆臂

單三角鑿巖機器人的鉆臂結(jié)構(gòu)如圖1 所示，阿特拉斯 Boomer-XE3C 以及新一代的鑿巖機器人均使用該類型臂架結(jié)構(gòu)。

圖1 單三角結(jié)構(gòu)鉆臂Fig.1 Single triangular structure drilling arm

鉆臂左支臂缸 Cb1 和鉆臂右支臂缸 Cb2 形成三角臂，控制大臂的擺動和俯仰，伸縮液壓缸 Cz 用于實現(xiàn)鉆臂的長度變化，推進器翻轉(zhuǎn)液壓缸 Cr 用于實現(xiàn)推進器的軸向旋轉(zhuǎn)，推進器補償液壓缸 Ce 用于補償推進器的前后位移，推進器俯仰液壓缸 Cl 實現(xiàn)推進器的俯仰變化，推進器擺動液壓缸 Cg 實現(xiàn)推進器的左右擺動。鉆臂左右 2 個支臂缸配合動作實現(xiàn)大臂的俯仰和擺動，為達到精準定位目的，需要精準控制2 個支臂缸的伸縮?？刂?2 個支臂缸的難度要高于其他液壓缸的控制器，因此有必要對三角定位機構(gòu)的控制進行研究。

1.2 鉆臂支臂缸運動學解算

左右支臂缸同時控制大臂擺動和俯仰，控制大臂擺動角度同時會影響到大臂俯仰角度，因此這是一個典型的 MIMO 控制系統(tǒng)，必須對支臂缸進行運動學分析，推導出從大臂空間角度到驅(qū)動缸長度的運動學公式，為實現(xiàn)精確運動控制奠定基礎。

如圖2 所示，以左右鉆臂液壓缸軸套與車體的連接點B、C的中點O為坐標系原點，鉆臂與車體的連接軸A為坐標系的z軸，O點沿大臂方向水平向前為x軸，y軸由右手定則確定，R點為大臂后鉸點與軸套的鉸接點，G點為右液壓缸后鉸點與軸套的鉸接點，E點為左液壓缸后鉸點與軸套的鉸接點，N、M點為大臂和液壓缸的鉸接點，△JMN為垂直于臂的截面，H點為J點在水平面的投影，OH與x軸的夾角為大臂擺動角度q1，RJ與OH之間的夾角為大臂俯仰角度q2。鉆臂運動學解算轉(zhuǎn)化為：在已知大臂擺動角度q1和大臂俯仰角度q2的前提下，求解左右 2 個液壓缸長度，即圖2 中的EN和GM的長度LEN和LGM。

圖2 鑿巖機器人三角鉆臂結(jié)構(gòu)Fig.2 Triangular drilling arm structure of rock-drilling robot

J點位于大臂坐標系，可得J點坐標，

已知J點可計算出M點坐標，

式中：E0為初始狀態(tài)的E點。

在△GIM中，可以求得IM的長度和GI的長度，最終可以求得GM的長度及EN的長度

1.3 鉆臂支臂缸的 PID 控制

由式 (3) 可以根據(jù)關節(jié)變量q1、q2得到目標支臂缸長度。對目標支臂缸長度做閉環(huán)控制，可以實現(xiàn)支臂缸位置控制，支臂缸控制框圖如圖3 所示。

圖3 鉆臂支臂缸控制框圖Fig.3 Block diagram of load arm cylinder control for drilling arm

考慮到液壓系統(tǒng)具有較大的慣性質(zhì)量，響應速度慢，流量會隨負載分配，控制中采用速度、位置雙閉環(huán)的控制方法[9]，控制框圖如圖4 所示。其中液壓缸線速度可以通過對式 (3) 兩邊求導得到，如式 (5) 所示。內(nèi)環(huán)是對液壓缸線速度進行 PID 控制，目標線速度可以通過三次、五次多項式差值等方法得到；外環(huán)是對液壓缸位置進行 PID 控制[10]。

圖4 雙閉環(huán)控制框圖Fig.4 Block diagram of double closed-loop control

2 機械臂動力學建模

該機械臂有 6 個自由度：大臂擺動角度q1、大臂俯仰角度q2、大臂伸縮長度q3、推進梁旋轉(zhuǎn)角度q4、推進梁俯仰角度q5、推進梁伸縮角度q6，其運動需要使用q1～q6共 6 個參數(shù)來確定，設機械臂的角度矢量為

根據(jù)上節(jié)的運動學解算結(jié)果，利用拉格朗日方法[11]可以求出動力學模型：

根據(jù)現(xiàn)代控制理論，定義狀態(tài)變量和機械臂的動力學模型的輸入

式 (7) 可簡化為

寫成矩陣形式為[12]

式中：I∈R n·n為單位矩陣。

假設各個臂為質(zhì)量均勻分布的連桿機構(gòu)，

式中：M(q) 為各個臂的轉(zhuǎn)動慣量與關節(jié)角度組成的矩陣，平移關節(jié)此項為 0；N(q,) 為各個臂的質(zhì)量、關節(jié)角度和角速度組成的矩陣；G(q) 為各個臂的重力分量組成的矩陣。

機械臂參數(shù)如表1 所列。

表1 機械臂參數(shù)Tab.1 Parameters of robotic arm

將表1 參數(shù)帶入式 (13) 中可以求得各個系數(shù)。

在 Matlab 中使用機器人工具箱[13]建立所示機械臂模型，如圖5 所示，可以得到 12×12 的矩陣A，12×6 的矩陣B，6×12 的矩陣C，6×1 的矩陣G。給機械臂的動力學參數(shù)賦予表1 中的數(shù)值，假設機械臂以角速度 5°/s 經(jīng)過關節(jié)變量的零點，伸縮量維持 0 mm 不變，可得

圖5 鉆臂模型Fig.5 Drilling arm model

3 LQR 控制器設計

LQR[14]是線性二次型調(diào)節(jié)器的縮寫，是一種反饋最優(yōu)控制器。設計過程中要解決的問題是如何選取使代價函數(shù)值最小的狀態(tài)量和控制量權(quán)重。筆者選取LQR 控制器主要是對機械臂平順性進行研究，定義性能指標為表示機械臂位移、速度的加權(quán)平方和在時域的積分值。

LQR 方法給定了決定性能的目標函數(shù)

式中：Q為半正定的狀態(tài)加權(quán)矩陣；R為正定的控制加權(quán)矩陣；x∈R12·12；u∈R12·1。

系統(tǒng)的期望性能由Q、R矩陣決定，針對不同工況選擇不同的權(quán)重系數(shù)，求解出符合各自工況下的最優(yōu)控制器。

設狀態(tài)反饋控制率

式中：Ki為狀態(tài)反饋增益矩陣；G為式 (7) 的重力分量，選取合適的數(shù)值把G抵消。

Riccati 方程為[15]

式中的A、B、Q、R都是已知的，可以求得P，進而求得閉環(huán)狀態(tài)方程

得到本系統(tǒng)的最優(yōu)控制矢量

最優(yōu)控制矢量由模型參數(shù)和加權(quán)系數(shù)決定，而加權(quán)系主要靠設計者整定獲得，這就使控制器性能存在較大的主觀性。

3.1 單回路 LQR 控制器設計

采用 LQR 控制方法對鑿巖機器人機械臂進行控制，根據(jù)其狀態(tài)空間表達式利用 MATLAB 中的 lqr()函數(shù)計算出K1矩陣[16]。對于大臂伸縮、推進梁旋轉(zhuǎn)、推進梁俯仰、推進梁伸縮的控制，由于他們之間不存在耦合關系，以推進梁俯仰為例，其控制框圖如圖6 所示。

圖6 推進梁俯仰控制框圖Fig.6 Block diagram of pitching control for advancing beam

在 MATLAB Simulink 中搭建模型，如圖7 所示。得到反饋 LQR 控制器的最優(yōu)參數(shù)為K=[1.15 -2.51]。

圖7 推進梁俯仰 Simulink 仿真Fig.7 Simulink simulation of advancing beam pitching

3.2 多回路 LQR 控制器設計

由于單回路 LQR 控制器依靠一個單一的狀態(tài)反饋矩陣對系統(tǒng)進行控制，對于耦合系統(tǒng)控制效果不佳，考慮將大臂擺動和俯仰解耦分開控制，實現(xiàn)兩回路 LQR 控制[17-18]，兩回路 LQR 控制結(jié)構(gòu)如圖8 所示。得到反饋 LQR 控制器的最優(yōu)參數(shù)為K=[2.35-1.50 0.02 0.1]。

圖8 三角鉆臂運動控制框圖Fig.8 Block diagram of motion control for triangular drilling arm

4 試驗仿真分析

對于大臂伸縮、推進梁旋轉(zhuǎn)、推進梁俯仰、推進梁伸縮的控制，將筆者設計的 LQR 控制器和 PID 控制進行對比，系統(tǒng)階躍響應對比結(jié)果如圖9 所示。

圖9 推進梁俯仰 LQR 控制器和 PID 控制對比Fig.9 Comparison of LQR controller and PID control for advancing beam pitching

對于大臂擺動和俯仰的控制，將設計的 LQR 控制器和 1.3 節(jié)的 PID 控制進行對比，以左液壓缸為例，系統(tǒng)的階躍響應對比結(jié)果如圖10 所示。

圖10 臂架擺動 LQR 控制器和 PID 控制對比Fig.10 Comparison of LQR controller and PID control for arm support swing

將階躍響應的結(jié)果進行整理，如表2 所列。

表2 機械臂響應結(jié)果Tab.2 Response results of robotic arm

由表2 可以看出，推進梁旋轉(zhuǎn)角度 PID 控制器到達穩(wěn)態(tài)的時間為 4.6 s，使用 LQR 控制器后到達穩(wěn)態(tài)的時間僅為 2.0 s，效率提高 1 倍，柔性機械臂的定位速度大幅提升。

對于大臂擺動和俯仰的控制，經(jīng)過解耦之后控制左右 2 個液壓缸的長度。圖10 所示為控制左液壓缸長度的曲線，PID 控制存在較多的峰值，在作業(yè)過程中會造成大臂的抖動，進而影響作業(yè)精度，而 LQR控制在相應過程中只存在 1 個峰值，隨后誤差會逐漸趨近 0，說明經(jīng)過 LQR 控制的系統(tǒng)自動調(diào)節(jié)平衡能力要強于 PID 控制，這對于機械臂起到了較好的保護作用。

單回路 LQR 控制的機械臂的跟蹤曲線如圖11 所示。圖11 中目標角度變化過程中，控制誤差基本能夠維持在 0°，跟蹤過程中角速度的誤差＜0.01°/s。

圖11 推進梁俯仰 LQR 跟蹤控制Fig.11 LQR tracking control of advancing beam pitching

雙回路 LQR 控制的機械臂的跟蹤曲線如圖12所示。圖12 中目標角度變化過程中，控制誤差基本＜0.5°，跟蹤過程中角速度的誤差＜0.5°/s。

圖12 大臂擺動 LQR 跟蹤控制Fig.12 LQR tracking control of main arm swing

5 實車驗證

采用徐工某型號鑿巖機器人對文中的算法進行了驗證，如圖13 所示。

圖13 某型號鑿巖機器人Fig.13 A certain type of rock-drilling robot

給定機械臂大臂擺動、俯仰、伸縮，推進梁俯仰、擺動關節(jié)初始值為 [0° 0° 0 mm 0° 0°]，目標值為 [18° -32.4° 100 mm 14.4° -36°]，利用梯形軌跡公式進行關節(jié)空間軌跡計算后得到各部分角度/長度變化數(shù)據(jù)，如表3 所列，角/線速度變化數(shù)據(jù)如表4 所列。

表3 各關節(jié)角度/長度變化Tab.3 Changes in joint angles/lengths

表4 各關節(jié)角速度/線速度變化Tab.4 Changes in angular velocity/linear velocity of each joint

使用 PEAK CAN 記錄控制過程中大臂俯仰、推進梁擺動角速度變化曲線，如圖14、15 所示，關節(jié)角度基本能與理論角度對應上。

圖14 大臂俯仰角速度變化曲線Fig.14 Change curve of pitching angular velocity of main arm

圖15 推進梁擺動角速度變化曲線Fig.15 Change curve of swing angular velocity of advancing beam

6 結(jié)語

對鑿巖機器人機械臂運動學和動力學進行了研究，通過運動學分析三角鉆臂，解耦了大臂擺動和大臂俯仰運動，并對三角機構(gòu)使用 PID 進行控制。通過動力學分析，得到機械臂各關節(jié)的動力學方程，進而建立機械臂的狀態(tài)空間方程。根據(jù)機械臂運動特點提出一種基于線性二次最優(yōu)控制 (LQR) 的控制方法，并與 PID 控制進行仿真對比，結(jié)果表明，該方法能夠提高機械臂的定位精度，減小機械臂移動過程中的沖擊、抖動，并將該方法應用到徐工某型號鑿巖機器人機械臂控制中，取得了良好的效果。研究結(jié)果可為鑿巖機器人控制提供有效幫助，為隧道施工提供更多的支持。