基于 PID/LQR 鑿巖機器人機械臂控制算法

王 輝，盛 榮，郭劍東

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

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

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

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

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

1 鑿巖機器人機械臂簡介

1.1 單三角結構鉆臂

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

圖1 單三角結構鉆臂Fig.1 Single triangular structure drilling arm

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

1.2 鉆臂支臂缸運動學解算

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

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

圖2 鑿巖機器人三角鉆臂結構Fig.2 Triangular drilling arm structure of rock-drilling robot

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

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

式中：E0為初始狀態的E點。

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

1.3 鉆臂支臂缸的 PID 控制

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

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

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

圖4 雙閉環控制框圖Fig.4 Block diagram of double closed-loop control

2 機械臂動力學建模

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

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

根據現代控制理論，定義狀態變量和機械臂的動力學模型的輸入

式 (7) 可簡化為

寫成矩陣形式為[12]

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

假設各個臂為質量均勻分布的連桿機構，

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

機械臂參數如表1 所列。

表1 機械臂參數Tab.1 Parameters of robotic arm

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

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

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

3 LQR 控制器設計

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

LQR 方法給定了決定性能的目標函數

式中：Q為半正定的狀態加權矩陣；R為正定的控制加權矩陣；x∈R12·12；u∈R12·1。

系統的期望性能由Q、R矩陣決定，針對不同工況選擇不同的權重系數，求解出符合各自工況下的最優控制器。

設狀態反饋控制率

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

Riccati 方程為[15]

式中的A、B、Q、R都是已知的，可以求得P，進而求得閉環狀態方程

得到本系統的最優控制矢量

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

3.1 單回路 LQR 控制器設計

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

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

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

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

3.2 多回路 LQR 控制器設計

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

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

4 試驗仿真分析

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

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

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

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

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

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

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

對于大臂擺動和俯仰的控制，經過解耦之后控制左右 2 個液壓缸的長度。圖10 所示為控制左液壓缸長度的曲線，PID 控制存在較多的峰值，在作業過程中會造成大臂的抖動，進而影響作業精度，而 LQR控制在相應過程中只存在 1 個峰值，隨后誤差會逐漸趨近 0，說明經過 LQR 控制的系統自動調節平衡能力要強于 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

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

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

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

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

圖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 結語

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