姚 明，周 敏，王 婧

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

地電波檢測(cè)是變電設(shè)備日常巡檢的一種重要且有效的手段。本文將地電波檢測(cè)機(jī)械臂裝置與移動(dòng)電力巡檢機(jī)器人結(jié)合起來(lái)，能夠?qū)崿F(xiàn)地電波自動(dòng)巡檢功能。地電波機(jī)械臂涉及結(jié)構(gòu)設(shè)計(jì)、運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、控制器設(shè)計(jì)等很多方面，本文主要研究地電波機(jī)械臂的動(dòng)力學(xué)問題，使用牛頓-歐拉法建立地電波機(jī)械臂的動(dòng)力學(xué)模型[1-5]。在MATLAB中對(duì)地電波檢測(cè)機(jī)械臂的動(dòng)力學(xué)模型進(jìn)行仿真。

1 地電波檢測(cè)機(jī)械臂介紹

1.1 機(jī)械臂結(jié)構(gòu)

機(jī)械臂包含五個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)和一個(gè)兩手指的末端夾持器。它可以到達(dá)655 mm的高度，質(zhì)量為6.3 kg，設(shè)計(jì)能夠夾持質(zhì)量0.5 kg的物體。在機(jī)械臂保持豎直姿態(tài)的情況下，關(guān)節(jié)1和關(guān)節(jié)5可以繞著相互平行的轉(zhuǎn)動(dòng)軸進(jìn)行轉(zhuǎn)動(dòng)。

1.2 機(jī)械臂操作系統(tǒng)

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

2 機(jī)械臂動(dòng)力學(xué)建模

2.1 動(dòng)力學(xué)建模方法概述

地電波檢測(cè)機(jī)械臂的動(dòng)力學(xué)建模問題，即研究關(guān)節(jié)角度、關(guān)節(jié)速度、關(guān)節(jié)加速度等關(guān)節(jié)運(yùn)動(dòng)狀態(tài)與各關(guān)節(jié)驅(qū)動(dòng)力矩之間的關(guān)系。建立地電波檢測(cè)機(jī)械臂的精確動(dòng)力學(xué)模型并深入研究其動(dòng)力學(xué)特性，為接下來(lái)設(shè)計(jì)機(jī)械臂動(dòng)力學(xué)控制器實(shí)現(xiàn)精確實(shí)時(shí)運(yùn)動(dòng)控制打下基礎(chǔ)。

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

2.2 牛頓歐拉動(dòng)力學(xué)建模

2.2.1 慣性張量

對(duì)于軸向轉(zhuǎn)動(dòng)的機(jī)械臂，采用慣性張量描述機(jī)械臂連桿的質(zhì)量分布，可用3×3矩陣表示如下：

(1)

矩陣中的各元素：

(2)

(3)

(4)

(5)

(6)

(7)

其中，機(jī)械臂連桿由單元體dv組成，其關(guān)節(jié)連桿的密度為ρ。Ixx，Iyy，Izz稱為慣量矩，Ixy，Iyz，Iyz稱為慣量積，這六個(gè)獨(dú)立的變量取決于所在連桿坐標(biāo)系的位姿。隨著連桿質(zhì)心坐標(biāo)系的旋轉(zhuǎn)，當(dāng)連桿慣性張量中的慣量積為零時(shí)，此時(shí)連桿坐標(biāo)系的坐標(biāo)軸成為連桿主軸，慣量矩稱為主慣性矩。根據(jù)慣性張量的特點(diǎn)，可以根據(jù)地電波檢測(cè)機(jī)械臂各個(gè)連桿的質(zhì)心坐標(biāo)系位置和慣性張量確定該機(jī)械臂的質(zhì)量分布。

2.2.2 牛頓—?dú)W拉動(dòng)力學(xué)方程

牛頓—?dú)W拉動(dòng)力學(xué)方程描述了關(guān)節(jié)力矩和關(guān)節(jié)運(yùn)動(dòng)狀態(tài)之間的數(shù)學(xué)關(guān)系。根據(jù)其定義方法，對(duì)于可動(dòng)關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)的機(jī)械臂來(lái)說，該算法的運(yùn)算過程如下：

外推：

(8)

(9)

(10)

(11)

(12)

(13)

內(nèi)推：

(14)

(15)

(16)

2.3 地電波檢測(cè)機(jī)械臂動(dòng)力學(xué)建模

將地電波檢測(cè)機(jī)械臂的慣性張量，連桿質(zhì)量，質(zhì)心矢量，旋轉(zhuǎn)矩陣，關(guān)節(jié)角度、速度、加速度等參數(shù)帶入方程中便可計(jì)算出關(guān)節(jié)力矩。

連桿質(zhì)心位置(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

連桿質(zhì)量(kg):

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

采用牛頓—?dú)W拉方程，可以將得到的動(dòng)力學(xué)結(jié)果可以寫成如下狀態(tài)空間的形式：

(17)

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

(18)

(19)

3 基于MATLAB的動(dòng)力學(xué)仿真

在MATLAB中對(duì)地電波檢測(cè)機(jī)械臂的動(dòng)力學(xué)模型進(jìn)行仿真。根據(jù)地電波檢測(cè)機(jī)械臂的關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度等運(yùn)動(dòng)狀態(tài)可以通過計(jì)算得到關(guān)節(jié)驅(qū)動(dòng)力矩，仿真結(jié)果如圖1所示。

圖1 動(dòng)力學(xué)仿真結(jié)果

4 動(dòng)力學(xué)控制器設(shè)計(jì)

基于地電波檢測(cè)機(jī)械臂動(dòng)力學(xué)模型設(shè)計(jì)控制器，并在Simulink與Gazebo環(huán)境下進(jìn)行仿真，以達(dá)到了良好地控制效果，為接下來(lái)更先進(jìn)的控制器設(shè)計(jì)打下基礎(chǔ)。

4.1 控制器設(shè)計(jì)

地電波檢測(cè)機(jī)械臂是一個(gè)非線性耦合動(dòng)力系統(tǒng)，在運(yùn)動(dòng)控制過程中必然存在各種不確定因素增大了其控制難度，因此對(duì)其控制器設(shè)計(jì)與分析具有重要意義?？刂齐y點(diǎn)具體如下。

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

(2)外干擾：如電機(jī)驅(qū)動(dòng)飽和、傳感器測(cè)量誤差、采樣延時(shí)等。

(3)強(qiáng)非線性：傳動(dòng)摩擦力、振動(dòng)誤差、響應(yīng)滯后等。

4.2 控制器設(shè)計(jì)

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

F=αF′+β

(20)

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

(21)

對(duì)于常見的機(jī)械臂動(dòng)力學(xué)方程：

(22)

因此，可以利用控制器分解法設(shè)計(jì)如下控制器：

τ=ατ′+β

(23)

(24)

其中：

α=M(θ)

(25)

(26)

該控制系統(tǒng)模型如圖2所示。

圖2 基于模型的地電波檢測(cè)機(jī)械臂控制

利用式(22)至式(26)，可以得到系統(tǒng)閉環(huán)誤差方程：

(27)

由于Kv和Kp為對(duì)角矩陣，因此各關(guān)節(jié)是解耦的。

4.3 Simulink仿真

利用地電波檢測(cè)機(jī)械臂動(dòng)力學(xué)模型設(shè)計(jì)了該控制器，并在MATLAB的Simulink中搭建了仿真環(huán)境(見圖3)，設(shè)計(jì)了地電波檢測(cè)機(jī)械臂的動(dòng)力學(xué)仿真模塊，并測(cè)試了地電波檢測(cè)機(jī)械臂各關(guān)節(jié)的階躍響應(yīng)。各關(guān)節(jié)的階躍響應(yīng)曲線見圖4。

圖3 Simulink仿真環(huán)境搭建

圖4 各關(guān)節(jié)的階躍響應(yīng)曲線

4.4 Gazebo仿真

Gazebo可以實(shí)現(xiàn)地電波檢測(cè)機(jī)械臂的物理仿真，通過在ROS與MATLAB聯(lián)合控制在Gazebo仿真環(huán)境下的地電波檢測(cè)機(jī)械臂運(yùn)動(dòng)來(lái)實(shí)現(xiàn)控制效果。為了方便控制人機(jī)交互，采用MATLAB編寫了上位機(jī)軟件，其界面見圖5。

圖5 地電波檢測(cè)機(jī)械臂的上位機(jī)控制界面

5 結(jié)語(yǔ)

針對(duì)動(dòng)力學(xué)建模問題，采用牛頓-歐拉法得到了其逆動(dòng)力學(xué)方程?；诳刂坡煞纸夥椒ㄔO(shè)計(jì)了機(jī)械臂的運(yùn)動(dòng)控制器，并搭建了Simulink仿真模型，仿真結(jié)果顯示了該系統(tǒng)良好的階躍響應(yīng)、軌跡跟蹤性能。