基于Matlab的配網(wǎng)帶電作業(yè)機(jī)器人正逆運(yùn)動(dòng)學(xué)仿真

文/李帥 張金祿 么軍 李威 何玉龍

1 引言

電力工業(yè)是國(guó)民經(jīng)濟(jì)的基礎(chǔ)，配電網(wǎng)又是其關(guān)鍵環(huán)節(jié)。當(dāng)線路發(fā)生突發(fā)故障時(shí)，停電檢修對(duì)社會(huì)經(jīng)濟(jì)影響巨大，嚴(yán)重影響百姓日常生活；另外，停電以及檢修完畢之后的上電都會(huì)對(duì)系統(tǒng)內(nèi)的設(shè)備造成沖擊傷害，影響設(shè)備的使用壽命。傳統(tǒng)意義上的帶電作業(yè)，作業(yè)人員通過(guò)絕緣作業(yè)工具與帶電設(shè)備接觸，檢修作業(yè)強(qiáng)度大，危險(xiǎn)性高，人員受傷情況難以避免。

因此，通過(guò)研發(fā)一種配網(wǎng)帶電作業(yè)機(jī)器人，不僅可減少作業(yè)時(shí)間即停電時(shí)間，同時(shí)還可以提高作業(yè)的可靠性，保護(hù)帶電作業(yè)人員，就顯得尤為重要。

2 機(jī)器人D-H法

Denavit-Hartenbery法簡(jiǎn)稱(chēng)D-H法。該方法于1956年由上述兩位學(xué)者提出，絕大多數(shù)機(jī)械臂的數(shù)學(xué)模型可由該方法構(gòu)建。絕大部分的關(guān)節(jié)型機(jī)械手臂，按順序?qū)⑥D(zhuǎn)動(dòng)副和移動(dòng)副固定在連桿上，連桿不在同一平面，并且旋轉(zhuǎn)軸彼此不平行。連桿坐標(biāo)建立方式如圖1所示。

Ci-1、Ci、Ci+1三個(gè)不同連桿對(duì)應(yīng)各自關(guān)節(jié)i-1，i，i+1，Ji、Ji+1為關(guān)節(jié)i和i+1的軸線，作其公垂線與對(duì)應(yīng)坐標(biāo)系OiXiYiZi中的Ji+1交點(diǎn)為坐標(biāo)原點(diǎn)，Zi軸與Ji+1軸平行且向上，Xi軸平行于公垂線，遠(yuǎn)離Ji軸為正方向。Ji對(duì)應(yīng)坐標(biāo)系Oi-1Xi-1Yi-1Zi-1。

參數(shù)aiαiθidi各自代表不同含義，在單個(gè)連桿關(guān)節(jié)中包括兩個(gè)參數(shù)連桿長(zhǎng)度ai表示單連桿長(zhǎng)度，即公垂線上Ji、Ji+1間距離。αi表示Zi與Zi+1之間的偏轉(zhuǎn)角度，逆時(shí)針為正。Di表示相鄰兩個(gè)連桿公垂線間距離。θi表示相鄰兩個(gè)連桿之間的扭轉(zhuǎn)角，逆時(shí)針為正。

在連桿坐標(biāo)系中，相鄰兩連桿可通過(guò)平移與旋轉(zhuǎn)互相轉(zhuǎn)換，分為以下四步：

表1：機(jī)械臂連桿D-H參數(shù)

圖1：連桿坐標(biāo)建立方式

圖2：配網(wǎng)帶電作業(yè)機(jī)器人機(jī)械臂模型

（1）將初始連桿繞坐標(biāo)系Z軸旋轉(zhuǎn)，旋轉(zhuǎn)角度為θ，使初始連桿與目標(biāo)連桿的X軸共面；

（2）將初始連桿繞坐標(biāo)系Z軸平移，平移距離為d，使初始連桿與目標(biāo)連桿的X軸共線；

（3）將初始連桿繞坐標(biāo)系X軸平移，平移角度為a，使得初始連桿與目標(biāo)連桿坐標(biāo)原點(diǎn)重合；

（4）將初始連桿繞坐標(biāo)系X軸轉(zhuǎn)動(dòng)，轉(zhuǎn)動(dòng)角度為α，使得初始連桿與目標(biāo)連桿Z軸共線；

將連桿坐標(biāo)轉(zhuǎn)換通過(guò)矩陣定義如下：

坐標(biāo)系變換：

機(jī)器人運(yùn)動(dòng)求解過(guò)程中，機(jī)械臂末端的位姿可以通過(guò)連桿的坐標(biāo)系變換得到。機(jī)械臂數(shù)學(xué)模型的建立關(guān)鍵在于找到局部坐標(biāo)系在全局坐標(biāo)系中的映射關(guān)系。發(fā)生平移時(shí)，其變換矩陣為T(mén)r(dx, dy, dz)：

旋轉(zhuǎn)角度為θ的坐標(biāo)系3個(gè)方向的變換矩陣分別為：

令PU表示點(diǎn)P相對(duì)于坐標(biāo)系U的描述；TUR表示坐標(biāo)系R與U之間的變換矩陣；即點(diǎn)P與坐標(biāo)系R之間的轉(zhuǎn)換關(guān)系如下PR：

圖3：配電機(jī)器人機(jī)械臂起始狀態(tài)

圖4：機(jī)器人運(yùn)動(dòng)學(xué)仿真界面

圖5：運(yùn)動(dòng)學(xué)正解

PR=PU×TUR

3 機(jī)器人數(shù)學(xué)模型建立

3.1 機(jī)械臂數(shù)學(xué)模型

為了完成機(jī)器人的仿真，第一步是創(chuàng)建其數(shù)學(xué)模型。Robotics工具箱的Link可用于構(gòu)建其模型。

其各個(gè)參數(shù)定義如下：

（1）a為連桿長(zhǎng)度，Z軸在X軸上的偏移；

（2）α為連桿扭角，Z軸繞X軸轉(zhuǎn)角，逆時(shí)針為正；

（3）d為連桿偏置，X軸在Z軸上的偏移；

（4）θ為關(guān)節(jié)角度，X軸繞Z軸轉(zhuǎn)角，逆時(shí)針為正；

（5）σ代表關(guān)節(jié)類(lèi)型：0表示旋轉(zhuǎn)關(guān)節(jié)，1表示移動(dòng)關(guān)節(jié)；

（6）φ代表移動(dòng)關(guān)節(jié)偏移量。

本文所用機(jī)械臂如圖2所示。該機(jī)械手由六個(gè)旋轉(zhuǎn)關(guān)節(jié)組成。各桿件的D-H參數(shù)如表1所示。

3.2 機(jī)器人正運(yùn)動(dòng)求解

正運(yùn)動(dòng)求解，即把關(guān)節(jié)變量、機(jī)器人末端初始位姿、各關(guān)節(jié)變化角度當(dāng)作已知，把機(jī)器人末端的最終位姿當(dāng)作未知的求解過(guò)程。表示為連桿i相對(duì)于連桿i-1的坐標(biāo)位置。的變換通式為：

本文機(jī)器人機(jī)械臂末端位姿可表示為：

3.3 機(jī)器人逆運(yùn)動(dòng)求解

逆運(yùn)動(dòng)求解，即把關(guān)節(jié)變量、機(jī)器人末端初始位姿、最終位姿當(dāng)作已知，把關(guān)節(jié)變化角度當(dāng)作未知并求解的過(guò)程。逆運(yùn)動(dòng)問(wèn)題實(shí)際上是運(yùn)動(dòng)方程的解。

給定末端執(zhí)行器的位姿，即已知n,o,a,p的坐標(biāo)。對(duì)于每個(gè)變量，只需將所對(duì)應(yīng)的連桿進(jìn)行逆變換，即可求解θ1, θ2, θ3, θ4, θ5, θ6。

變量公式分別如下：

3.4 初始狀態(tài)仿真

在本文作業(yè)任務(wù)中，需兩條機(jī)械臂相互協(xié)調(diào)配合，方可完成接引流線任務(wù)，兩機(jī)械臂初始狀態(tài)角θ分別為[90° 100° -50° 30° -90° 0]和[90° 70° 30°-20° -90° 0]。結(jié)果如圖3所示。

3.5 仿真界面設(shè)計(jì)

對(duì)雙臂數(shù)學(xué)模型的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)仿真，其實(shí)質(zhì)為每一條壁單獨(dú)仿真，出于模擬工況較多的考慮，需進(jìn)行大量重復(fù)編寫(xiě)或更改代碼，每一次修改都在源代碼里修改，容易出現(xiàn)疏漏，不夠直觀，且造成模擬效率的下降，將程序模塊化，整合為如圖4所示界面。

仿真界面左側(cè)為模型建立部分，首先輸入連桿的各個(gè)參數(shù)，包含關(guān)節(jié)角度θ，連桿扭角α，連桿偏置d，連桿長(zhǎng)度a，連桿的初始長(zhǎng)度和變化范圍。為簡(jiǎn)化輸入步驟，設(shè)置了默認(rèn)參數(shù)一鍵輸入功能。待各連桿參數(shù)確定后，點(diǎn)擊建立模型，后臺(tái)程序即可生成數(shù)學(xué)模型，并可通過(guò)繪圖和動(dòng)態(tài)演示可視化模型狀態(tài)。

仿真界面右側(cè)為運(yùn)動(dòng)學(xué)分析部分，可輸入各連桿的初始位置、正求解位置和逆求解位置。為簡(jiǎn)化輸入，設(shè)置了默認(rèn)參數(shù)一鍵輸入功能。待各連桿初始和終止位置確定后，點(diǎn)擊正求解，即可在下方正解位置矩陣處顯示結(jié)果，并可通過(guò)繪制軌跡曲線可視化模型位置。反求解環(huán)節(jié)亦同上。

4 機(jī)械手臂運(yùn)動(dòng)學(xué)仿真

正運(yùn)動(dòng)學(xué)問(wèn)題（DKP）和逆運(yùn)動(dòng)學(xué)問(wèn)題（IKP）是機(jī)械臂運(yùn)動(dòng)學(xué)仿真中的主要問(wèn)題。

4.1 正運(yùn)動(dòng)學(xué)問(wèn)題仿真

調(diào)用MATLAB中Robotics工具箱對(duì)機(jī)械臂數(shù)學(xué)模型正運(yùn)動(dòng)求解過(guò)程如下：

T=fkine(robot,q);

Robot為所建立的機(jī)械臂數(shù)學(xué)模型，q為關(guān)節(jié)變化角度，T為多得正運(yùn)動(dòng)求解。選取左側(cè)機(jī)械臂為例，數(shù)學(xué)模型的各關(guān)節(jié)初始狀態(tài)設(shè)為q0，各坐標(biāo)變化量為qz，其正運(yùn)動(dòng)學(xué)求解過(guò)程為：

運(yùn)行后，由MATLAB計(jì)算得到的運(yùn)動(dòng)學(xué)正解如圖5，其中最后一列為機(jī)械臂末端位置坐標(biāo)（-164.0000 -631.4900 26.9651），正解模型如圖6。用jtraj函數(shù)仿真其1秒運(yùn)動(dòng)軌跡，如圖7所示。

4.2 逆運(yùn)動(dòng)學(xué)問(wèn)題仿真

調(diào)用MATLAB中Robotics工具箱對(duì)機(jī)械臂數(shù)學(xué)模型逆運(yùn)動(dòng)求解過(guò)程如下：

q=ikine(robot, T, q0, m)

其中，q是逆運(yùn)動(dòng)學(xué)問(wèn)題的解，即每個(gè)關(guān)節(jié)的變化，robot是機(jī)械手的數(shù)學(xué)模型，q0是關(guān)節(jié)初始角，T為反求解的變換矩陣，m是4×4矩陣。

T1是正運(yùn)動(dòng)解的結(jié)果。逆運(yùn)動(dòng)求解過(guò)程如下：

qz2=ikine(bot, T1, q0, m)

運(yùn)行后，由MATLAB計(jì)算得到的運(yùn)動(dòng)學(xué)逆解為：

qz2=[1.0749-2.2721 -1.7441 -0.5394 1.6551 -0.4807]

與qz1不同，將逆解qz2帶入正求解方程，得末端位置坐標(biāo)[-164.0000 -631.4901 26.9652]與正問(wèn)題求解工程中給定的末端位置坐標(biāo)相同，即證明相同位置坐標(biāo)可通過(guò)不同坐標(biāo)變換得到，同時(shí)也證明逆運(yùn)動(dòng)求解的不唯一性。

5 結(jié)論

使用D-H方法，建立了用于配電網(wǎng)帶電作業(yè)的機(jī)械臂的結(jié)構(gòu)參數(shù)，并在MATLAB Robotics工具箱中建立了其模型，完成了機(jī)械臂正逆問(wèn)題的求解和仿真，并完成其軌跡規(guī)劃，

為日后作業(yè)過(guò)程中可能遇到不同復(fù)雜工況下的軌跡規(guī)劃、離線編程和動(dòng)態(tài)設(shè)計(jì)等研究奠定了基礎(chǔ)。

圖6：運(yùn)動(dòng)學(xué)正解模型

圖7：正求解各關(guān)節(jié)運(yùn)動(dòng)過(guò)程