基于Simulink的地電波檢測機械臂>運動控制器及動力學研究

姚 明，周 敏，王 婧

(1.國網上海市電力公司市北供電公司，上海 200072；2.上海服澤能源科技有限公司,上海 200025)

地電波檢測是變電設備日常巡檢的一種重要且有效的手段。本文將地電波檢測機械臂裝置與移動電力巡檢機器人結合起來，能夠實現地電波自動巡檢功能。地電波機械臂涉及結構設計、運動學、動力學、控制器設計等很多方面，本文主要研究地電波機械臂的動力學問題，使用牛頓-歐拉法建立地電波機械臂的動力學模型[1-5]。在MATLAB中對地電波檢測機械臂的動力學模型進行仿真。

1 地電波檢測機械臂介紹

1.1 機械臂結構

機械臂包含五個轉動關節和一個兩手指的末端夾持器。它可以到達655 mm的高度，質量為6.3 kg，設計能夠夾持質量0.5 kg的物體。在機械臂保持豎直姿態的情況下，關節1和關節5可以繞著相互平行的轉動軸進行轉動。

1.2 機械臂操作系統

機器人操作系統(Robot Operating System，簡稱ROS)是機器人控制領域流行的應用開發環境，包括提供硬件抽象、設備驅動、可視化器、消息傳遞等的庫和工具，可以創建ROS節點與節點管理器連接。每個連接到節點管理器的節點都可以通過訂閱相應的主題來收聽其他節點提供的所有消息[6-10]。除了消息之外，參數和服務可以以相同方式用于連接到節點管理器的所有節點。用戶通過編寫節點發布消息或話題、服務來與其他節點通信。 KUKA-youBot有一個現成的ROS組件，它為ROS和直接訪問硬件的youBot驅動程序之間的通信建立了一個接口。這使得可以通過youBot組件發布消息形成反饋控制來控制關節位置或速度。

2 機械臂動力學建模

2.1 動力學建模方法概述

地電波檢測機械臂的動力學建模問題，即研究關節角度、關節速度、關節加速度等關節運動狀態與各關節驅動力矩之間的關系。建立地電波檢測機械臂的精確動力學模型并深入研究其動力學特性，為接下來設計機械臂動力學控制器實現精確實時運動控制打下基礎。

對于如何建立動力學模型的方法，主要有牛頓-歐拉法、拉格朗日法等[1-5]。牛頓-歐拉法推導過程簡單便于編程且計算比較快，對機械臂的每個構件分別運用牛頓方程和歐拉方程，適用于工業機械臂的動力學計算。但是其考慮不做功的內力、內力矩，并且方程為非封閉形式，消耗不必要的計算機時間。拉格朗日法基于總能量的標量函數推導其動力學方程。隨著機械臂自由度數目的逐漸增加，計算次數顯著增多。最近幾年興起的智能化動力學建模出現較大發展，比如模糊動力學模型、智能自適應模型等。智能化建模方法不依賴具體機械臂的動力學特性，方便對機械臂實施更高層次的控制。針對地電波檢測機械臂的結構特點，本文采用牛頓-歐拉法為地電波檢測機械臂建立逆動力學模型。

2.2 牛頓歐拉動力學建模

2.2.1 慣性張量

對于軸向轉動的機械臂，采用慣性張量描述機械臂連桿的質量分布，可用3×3矩陣表示如下：

(1)

矩陣中的各元素：

(2)

(3)

(4)

(5)

(6)

(7)

其中，機械臂連桿由單元體dv組成，其關節連桿的密度為ρ。Ixx，Iyy，Izz稱為慣量矩，Ixy，Iyz，Iyz稱為慣量積，這六個獨立的變量取決于所在連桿坐標系的位姿。隨著連桿質心坐標系的旋轉，當連桿慣性張量中的慣量積為零時，此時連桿坐標系的坐標軸成為連桿主軸，慣量矩稱為主慣性矩。根據慣性張量的特點，可以根據地電波檢測機械臂各個連桿的質心坐標系位置和慣性張量確定該機械臂的質量分布。

2.2.2 牛頓—歐拉動力學方程

牛頓—歐拉動力學方程描述了關節力矩和關節運動狀態之間的數學關系。根據其定義方法，對于可動關節均為轉動關節的機械臂來說，該算法的運算過程如下：

外推：

(8)

(9)

(10)

(11)

(12)

(13)

內推：

(14)

(15)

(16)

2.3 地電波檢測機械臂動力學建模

將地電波檢測機械臂的慣性張量，連桿質量，質心矢量，旋轉矩陣，關節角度、速度、加速度等參數帶入方程中便可計算出關節力矩。

連桿質心位置(mm)：

1Pc1=(15.16 -3.59 -31.05)

2Pc2=(113.97 -15 19.03)

3Pc3=(104.41 0.13 -20.22)

4Pc4=(-0.15 53.53 24.64)

5Pc5=(0 1.2 -16.48)

連桿主慣性矩(kg·m2)：

l1xx=0.002 952 5;l1yy=0.006 009 1;l1zz=0.002 882 1

l2xx=0.003 114 5;l2yy=0.000 584 3;l2zz=0.003 163 1

l3xx=0.001 727 67;l3yy=0.000 419 67;l3zz=0.001 846 8

l4xx=0.000 676 4;l4yy=0.001 057 3;l4zz=0.000 661 0

l5xx=0.000 193 4;l5yy=0.000 160 2;l5zz=0.000 068 9

連桿質量(kg):

m1=1.390,m2=1.318,m3=0.821,m4=0.769,m5=0.687

采用牛頓—歐拉方程，可以將得到的動力學結果可以寫成如下狀態空間的形式：

(17)

因式(17)中的矢量取決于位置和速度，也稱為狀態空間方程：

(18)

(19)

3 基于MATLAB的動力學仿真

在MATLAB中對地電波檢測機械臂的動力學模型進行仿真。根據地電波檢測機械臂的關節角度、關節角速度和關節角加速度等運動狀態可以通過計算得到關節驅動力矩，仿真結果如圖1所示。

圖1 動力學仿真結果

4 動力學控制器設計

基于地電波檢測機械臂動力學模型設計控制器，并在Simulink與Gazebo環境下進行仿真，以達到了良好地控制效果，為接下來更先進的控制器設計打下基礎。

4.1 控制器設計

地電波檢測機械臂是一個非線性耦合動力系統，在運動控制過程中必然存在各種不確定因素增大了其控制難度，因此對其控制器設計與分析具有重要意義。控制難點具體如下。

(1)參數不確定：如機械臂的負載等參數的變化。

(2)外干擾：如電機驅動飽和、傳感器測量誤差、采樣延時等。

(3)強非線性：傳動摩擦力、振動誤差、響應滯后等。

4.2 控制器設計

地電波檢測機械臂的控制問題是一個多輸入、多輸出問題，可以把控制部分分解成基于模型的控制部分和伺服控制部分。

F=αF′+β

(20)

式(20)為基于模型的控制部分，對于5自由度的地電波檢測機械臂來說，α代表5×5的解耦矩陣，起解耦作用。

(21)

對于常見的機械臂動力學方程：

(22)

因此，可以利用控制器分解法設計如下控制器：

τ=ατ′+β

(23)

(24)

其中：

α=M(θ)

(25)

(26)

該控制系統模型如圖2所示。

圖2 基于模型的地電波檢測機械臂控制

利用式(22)至式(26)，可以得到系統閉環誤差方程：

(27)

由于Kv和Kp為對角矩陣，因此各關節是解耦的。

4.3 Simulink仿真

利用地電波檢測機械臂動力學模型設計了該控制器，并在MATLAB的Simulink中搭建了仿真環境(見圖3)，設計了地電波檢測機械臂的動力學仿真模塊，并測試了地電波檢測機械臂各關節的階躍響應。各關節的階躍響應曲線見圖4。

圖3 Simulink仿真環境搭建

圖4 各關節的階躍響應曲線

4.4 Gazebo仿真

Gazebo可以實現地電波檢測機械臂的物理仿真，通過在ROS與MATLAB聯合控制在Gazebo仿真環境下的地電波檢測機械臂運動來實現控制效果。為了方便控制人機交互，采用MATLAB編寫了上位機軟件，其界面見圖5。

圖5 地電波檢測機械臂的上位機控制界面

5 結語

針對動力學建模問題，采用牛頓-歐拉法得到了其逆動力學方程。基于控制律分解方法設計了機械臂的運動控制器，并搭建了Simulink仿真模型，仿真結果顯示了該系統良好的階躍響應、軌跡跟蹤性能。