余 凌，劉 飄，羅金滿，王湘女

（廣東電網(wǎng)有限責(zé)任公司東莞供電局，廣東 東莞 523008）

0 引言

帶作業(yè)機(jī)械臂的電力機(jī)器人可以減輕人工勞動(dòng)強(qiáng)度，解決現(xiàn)有巡檢設(shè)備只有檢測功能沒有操作功能的問題，實(shí)現(xiàn)電力機(jī)器人代替人工來安全地完成作業(yè)，某種程度上解決了人工作業(yè)效率低和高風(fēng)險(xiǎn)等問題［1-5］。電力機(jī)器人作業(yè)時(shí)是依靠機(jī)械臂來完成任務(wù)的，機(jī)械臂的末端裝有末端執(zhí)行器來操作不同的任務(wù)。為了高效安全地實(shí)行機(jī)械臂自主執(zhí)行任務(wù)，如帶電導(dǎo)線撿拾、螺栓緊固等，需要對機(jī)械臂的關(guān)節(jié)空間進(jìn)行實(shí)時(shí)的運(yùn)動(dòng)軌跡規(guī)劃。如日本公司所研發(fā)的帶電作業(yè)機(jī)器人Robot-PhaseⅢ可實(shí)現(xiàn)半自動(dòng)化作業(yè)，能自主識別目標(biāo)位置并進(jìn)行運(yùn)動(dòng)規(guī)劃。

目前大部分對機(jī)械臂的運(yùn)動(dòng)規(guī)劃研究集中在運(yùn)動(dòng)規(guī)劃方法上。欒慶磊提出一種改進(jìn)型漸進(jìn)最優(yōu)快速隨機(jī)擴(kuò)展樹（RRT*）算法用于路徑規(guī)劃，解決機(jī)械臂在運(yùn)動(dòng)路徑規(guī)劃上搜索隨機(jī)性強(qiáng)、成功率低的問題［6］；陶志遠(yuǎn)針對斷路器帶電檢修機(jī)器人的機(jī)械臂作了路徑規(guī)劃關(guān)鍵技術(shù)研究［7］；邾根豪提出了一種針對帶電作業(yè)機(jī)器人基于時(shí)間最優(yōu)的軌跡規(guī)劃算法和控制策略研究［8］。韓波等對機(jī)械臂的運(yùn)動(dòng)學(xué)算法進(jìn)行了建模和設(shè)計(jì)［9-14］。以上研究在不同方向上對機(jī)械臂的運(yùn)動(dòng)規(guī)劃方法優(yōu)化等做出了貢獻(xiàn)，但較少涉及其規(guī)劃算法后對機(jī)械臂的三維動(dòng)態(tài)仿真內(nèi)容。在實(shí)際的機(jī)械臂作業(yè)過程中，單憑操作人員的想像很難預(yù)測機(jī)械臂的運(yùn)動(dòng)規(guī)劃后的軌跡，一旦規(guī)劃出現(xiàn)問題，機(jī)械臂的運(yùn)動(dòng)狀況將無法預(yù)測?；诖耍杂脩粼O(shè)計(jì)為中心，本文設(shè)計(jì)一個(gè)基于四自由度電力巡檢機(jī)器人機(jī)械臂的運(yùn)動(dòng)仿真系統(tǒng)來模擬機(jī)械臂的運(yùn)動(dòng)過程，實(shí)現(xiàn)機(jī)械臂的常規(guī)運(yùn)動(dòng)仿真和軌跡規(guī)劃仿真。

1 作業(yè)機(jī)械臂的建模

電力巡檢機(jī)器人的作業(yè)環(huán)境通常是非規(guī)則化的，當(dāng)需要機(jī)械臂進(jìn)行操作時(shí)，如需要快速將帶電導(dǎo)線與被保護(hù)物分離時(shí)，操作導(dǎo)線的周邊物品會不可避免地成為障礙物，給作業(yè)帶來不便。為此，面向普適計(jì)算方法，對操作機(jī)械臂先進(jìn)行運(yùn)動(dòng)建模研究。機(jī)械臂的運(yùn)動(dòng)學(xué)分析是運(yùn)動(dòng)規(guī)劃的前提基礎(chǔ)，可分為機(jī)械臂的正運(yùn)動(dòng)分析與逆運(yùn)動(dòng)分析兩個(gè)方面［15-16］。本文采用四自由度的操作機(jī)械臂作為研究對象，只對主要運(yùn)動(dòng)部件進(jìn)行建模型，分別為基座、旋轉(zhuǎn)平臺、大臂和小臂以及末端機(jī)構(gòu)，四個(gè)自由度均為旋轉(zhuǎn)型關(guān)節(jié)，如圖1所示。

圖1 四自由度操作機(jī)械臂Fig.1 Manipulator arm with four degrees of freedom

1.1 機(jī)械臂正運(yùn)動(dòng)分析

機(jī)械臂正運(yùn)動(dòng)分析是指根據(jù)已知機(jī)械臂連桿參數(shù)與機(jī)械臂的運(yùn)動(dòng)坐標(biāo)系，建立一數(shù)學(xué)模型。該模型是以機(jī)械臂各關(guān)節(jié)量(θ)作為輸入變量，以機(jī)械臂末端執(zhí)行器坐標(biāo)值為輸出變量。然后利用該模型可根據(jù)已知的機(jī)械臂各關(guān)節(jié)量得出機(jī)械臂末端執(zhí)行器在空間坐標(biāo)系的坐標(biāo)值。首先根據(jù)機(jī)械臂關(guān)節(jié)坐標(biāo)系和廣義連桿的4 個(gè)Denavit-Hartenberg（D-H）特征參數(shù)(d，k，a，θ)定義，繪制出機(jī)械臂處于初始狀態(tài)時(shí)的運(yùn)動(dòng)學(xué)坐標(biāo)系，具體如圖2 所示。(d，k，a，θ)4 個(gè)參數(shù)含義分別是d為連桿偏距，k為連桿長度，a為連桿轉(zhuǎn)角，θ為關(guān)節(jié)角；然后根據(jù)機(jī)器人矩陣學(xué)知識求出各機(jī)械臂連桿的相對變換矩陣T（其中i表示機(jī)械臂連桿的編號，i-1 即表示上一連桿的編號，i≥1）；最后將各機(jī)械臂連桿的變換矩陣相乘得出末端執(zhí)行器相對于基座標(biāo)的變換矩陣05T。05T變換矩陣能夠根據(jù)機(jī)械臂各連桿關(guān)節(jié)變量(θ)的具體值，通過計(jì)算后得出末端執(zhí)行器在基坐標(biāo)系的位置和姿態(tài)。05T的得出如公式（1）所示。

圖2 機(jī)械臂運(yùn)動(dòng)學(xué)坐標(biāo)系Fig.2 Kinematic coordinate system of manipulator

式（1）中，用n，o，a，p分別表示矩陣中的元素。nx表示矩陣中第一行第一列的元素；ox表示矩陣第一行第二列的元素；ax表示矩陣第一行第三列的元素；px表示第一行第四列的元素；ny表示矩陣中第二行第一列的元素，其余類推；nz表示第三行第一列的元素，其余類推。

1.2 機(jī)械臂逆運(yùn)動(dòng)分析

機(jī)械臂逆運(yùn)動(dòng)分析是正運(yùn)動(dòng)分析的逆過程，是指在已知各連桿幾何參數(shù)的情況下，給定末端執(zhí)行器在其坐標(biāo)系中的期望坐標(biāo)值，求出要使末端執(zhí)行器運(yùn)動(dòng)到期望位置時(shí)每個(gè)關(guān)節(jié)所應(yīng)該運(yùn)動(dòng)的數(shù)值量。利用變換方程式（1）求出各連桿關(guān)節(jié)變量(θ)。機(jī)械臂逆運(yùn)動(dòng)分析的最終目標(biāo)是求出各關(guān)節(jié)變量(θ)的解析表達(dá)式，常用的方法有代數(shù)解法與幾何解法。根據(jù)實(shí)際的操作機(jī)械臂結(jié)構(gòu)與對計(jì)算量的考慮，本文采用幾何解法與代數(shù)法相結(jié)合解法。先用代數(shù)解法求出底座旋轉(zhuǎn)關(guān)節(jié)的表達(dá)式，然后繪制出機(jī)械臂各連桿（大臂旋轉(zhuǎn)a1，中臂旋轉(zhuǎn)a2，小臂旋轉(zhuǎn)a3）的姿態(tài)位置圖，如圖3 為機(jī)械臂運(yùn)動(dòng)到某一時(shí)刻時(shí)的姿態(tài)位置簡圖，最終得出其他各變量的解析表達(dá)式。

圖3 機(jī)械臂連桿位置簡圖Fig.3 Mechanical arm link position diagram

2 改進(jìn)A*算法的機(jī)械臂避障路徑規(guī)劃

由于C 空間具有直觀、易于實(shí)現(xiàn)以及在運(yùn)用合適搜索方法的情況下可找到最優(yōu)運(yùn)動(dòng)路徑，且機(jī)械臂常用的路徑規(guī)劃方法為C 空間法，因此本文采用基于C空間的路徑規(guī)劃方法［17］。根據(jù)C空間的特點(diǎn)建立機(jī)械臂的C 空間并進(jìn)行了C 空間障礙的柵格化表示后，采用A?搜索算法，搜索一條以起始位置點(diǎn)為起點(diǎn)，目標(biāo)位置點(diǎn)為終點(diǎn)的無碰撞路徑［18-28］。

A?搜索算法在搜索過程利用一個(gè)估價(jià)函數(shù)作為搜索指標(biāo)，該函數(shù)表示為f?(x) =g?(x) +h?(x)，其中f?(x)表示從起始節(jié)點(diǎn)到節(jié)點(diǎn)x的一條最佳路徑的實(shí)際代價(jià)加上從節(jié)點(diǎn)x到目標(biāo)節(jié)點(diǎn)的一條最佳路徑的代價(jià)之和；g?(x)表示從起始節(jié)點(diǎn)到節(jié)點(diǎn)x之間最小代價(jià)路徑的實(shí)際代價(jià)；h?(x)表示從x節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的最小代價(jià)路徑上的代價(jià)。然而實(shí)際上f?(x)是無法預(yù)先知道的，解決辦法是用函數(shù)f(x) =g(x) +h(x)來近似代替以上估計(jì)函數(shù)，其中必須滿足條件g?(x) ≤g(x)，h?(x) ≥h(x)。對于搜索算法在二維C-空間柵格平面上進(jìn)行搜索時(shí)，每次搜索的方向有8 個(gè)，如圖4所示。

圖4 二維平面的8個(gè)搜索方向Fig.4 Eight search directions in a two-dimensional plane

由于機(jī)械臂的運(yùn)動(dòng)空間屬于三維空間，使用搜索算法搜索時(shí)每次搜索的方向就變?yōu)?6個(gè)，每次擴(kuò)展的節(jié)點(diǎn)也變?yōu)?6個(gè)，此時(shí)搜索算法每次擴(kuò)展的節(jié)點(diǎn)一下子變成了二維平面的3 倍之多，這樣不僅會增加節(jié)點(diǎn)存儲空間的使用，而且會嚴(yán)重影響搜索的速度。為了讓A?搜索算法能夠適用于三維柵格C-空間，對A?搜索算法的實(shí)現(xiàn)進(jìn)行改進(jìn)，具體為：對三維C空間上的任意一點(diǎn)，可以看成是三維空間的xy平面、yz平面以及xz平面這3個(gè)平面上的一點(diǎn)，因此，對于起始點(diǎn)與目標(biāo)點(diǎn)不在同一個(gè)平面的情況，可先分別對起始點(diǎn)與目標(biāo)點(diǎn)設(shè)定不同的從屬平面，如起始點(diǎn)從屬于R 平面而目標(biāo)點(diǎn)從屬于r0平面，如圖5所示。

圖5 改進(jìn)的A?搜索算法原理圖Fig.5 Improved A? search algorithm schematic

然后在這兩個(gè)從屬平面的相交處找出一個(gè)中間點(diǎn)，此時(shí)三維路徑搜索問題可轉(zhuǎn)換成兩個(gè)二維平面的路徑搜索問題的合成，這樣便可利用A?搜索算法分別在兩個(gè)從屬平面搜索出最優(yōu)路徑，然后再將這兩條路徑組合成最終的搜索路徑。具體的操作步驟如圖6所示。改進(jìn)后的A?搜索算法分別從兩個(gè)平面上搜索出兩條路徑，然后組合起來得出最終三維C 空間的運(yùn)動(dòng)路徑，其相對于直接使用A?搜索算法在三維空間中搜索，不僅節(jié)省了搜索過程節(jié)點(diǎn)的存儲空間，而且還加快了搜索的速度。

圖6 改進(jìn)的A?搜索算法流程圖Fig.6 Improved A? search algorithm flow chart

3 操作機(jī)械臂的運(yùn)動(dòng)規(guī)劃仿真系統(tǒng)

3.1 機(jī)械臂運(yùn)動(dòng)仿真系統(tǒng)設(shè)計(jì)

本次仿真采用Visual Studio 為編程工具，使用Eon Studio 作為仿真程序接口，實(shí)現(xiàn)了單一障礙物的操作機(jī)械臂避障運(yùn)動(dòng)仿真。為了便于研究，用經(jīng)過簡化處理的四自由度運(yùn)動(dòng)模型實(shí)體代表操作機(jī)械臂，以長圓柱體模型作為障礙物以及用小圓球代表運(yùn)動(dòng)目標(biāo)點(diǎn)。為了配合路徑規(guī)劃，將機(jī)械臂簡化為二自由度，只實(shí)現(xiàn)兩連桿的運(yùn)動(dòng)，仿真的目標(biāo)是讓連桿2 的端點(diǎn)運(yùn)動(dòng)到指定的目標(biāo)點(diǎn)。首先，利用Visual Studio 編寫主體程序框架，在程序里實(shí)現(xiàn)運(yùn)動(dòng)反解程序，路徑搜索程序以及軌跡規(guī)劃程序；其次，把操作機(jī)械臂運(yùn)動(dòng)模型導(dǎo)入Eon Studio 中進(jìn)行虛擬仿真程序的設(shè)計(jì)；最后，Eon 虛擬仿真程序以Eon Studio控件形式嵌入到Visual Studio主程序框架中，利用Eon Studio 控件提供的接口函數(shù)，Visual Studio 可對仿真程序進(jìn)行控制以及對仿真程序事件進(jìn)行響應(yīng)，仿真流程如圖7所示。

圖7 操作機(jī)械臂運(yùn)動(dòng)仿真流程圖Fig.7 Movement simulation flow chart of manipulator

3.2 機(jī)械臂運(yùn)動(dòng)仿真系統(tǒng)測試驗(yàn)證

機(jī)械臂運(yùn)動(dòng)仿真系統(tǒng)的目的是在設(shè)定的具體條件下驗(yàn)證規(guī)劃后的運(yùn)動(dòng)軌跡光滑平穩(wěn)性。所設(shè)置的機(jī)械臂包括連桿1和連桿2，并設(shè)置抓取的對象為目標(biāo)小球和設(shè)定障礙物。具體步驟為：

1）設(shè)定仿真條件，設(shè)連桿1與連桿2長度同為500，兩連桿關(guān)節(jié)初始值同為00。假設(shè)目標(biāo)點(diǎn)在基座標(biāo)系的位置為（600，0，300），障礙物的位置為（600，0，200）。

2）運(yùn)行運(yùn)動(dòng)學(xué)反解程序，路徑搜索程序以及軌跡規(guī)劃程序，得出兩關(guān)節(jié)運(yùn)動(dòng)路徑點(diǎn)和平滑后的運(yùn)動(dòng)曲線，如圖8和圖9所示。

圖8 連桿1關(guān)節(jié)位移時(shí)間運(yùn)動(dòng)曲線Fig.8 Joint displacement time motion curve of Connecting Rod 1

圖9 連桿2關(guān)節(jié)位移時(shí)間運(yùn)動(dòng)曲線圖Fig.9 Joint displacement time motion curve of Connecting Rod 2

3）在Visual Studio主程序框架中根據(jù)設(shè)定的路徑曲線進(jìn)行運(yùn)動(dòng)仿真。為了讓機(jī)械臂的運(yùn)動(dòng)更逼近兩關(guān)節(jié)的運(yùn)動(dòng)曲線，在主框架程序中設(shè)定仿真運(yùn)動(dòng)每隔0.1 s 更新一次關(guān)節(jié)的位置，如圖10 所示。圖10（a）表示機(jī)械臂的初始狀態(tài)，此時(shí)兩連桿關(guān)節(jié)的值為00；圖10（b）表示機(jī)械臂運(yùn)動(dòng)仿真過程的某一狀態(tài)，由于運(yùn)動(dòng)位置點(diǎn)經(jīng)過路徑規(guī)劃以及軌跡規(guī)劃，因此連桿的運(yùn)動(dòng)不但可避開障礙物，而且還可實(shí)現(xiàn)平穩(wěn)過渡；圖10（c）表示機(jī)械臂運(yùn)動(dòng)到目標(biāo)位置點(diǎn)。算例驗(yàn)證了所搭建的電力機(jī)器人機(jī)械臂運(yùn)動(dòng)的有效性。

圖10 機(jī)械臂避障運(yùn)動(dòng)仿真Fig.10 Simulation of obstacle avoidance motion of mechanical arm

4 結(jié)語

應(yīng)用虛擬現(xiàn)實(shí)技術(shù)、普適任務(wù)計(jì)算的思想，把機(jī)器人機(jī)械臂的任務(wù)分解，從操作目標(biāo)、運(yùn)動(dòng)規(guī)劃和計(jì)劃計(jì)算出發(fā)，構(gòu)建了機(jī)器人運(yùn)動(dòng)規(guī)劃數(shù)學(xué)模型，在虛擬環(huán)境下進(jìn)行了三維運(yùn)動(dòng)仿真。利用Visual Studio 編程工具以及Eon Studio 仿真軟件對以上研究結(jié)果進(jìn)行仿真實(shí)驗(yàn)，為實(shí)際操作機(jī)械臂運(yùn)動(dòng)控制提供參考依據(jù)，也為虛擬人體運(yùn)動(dòng)和機(jī)器人運(yùn)動(dòng)提供了一種計(jì)算方法。