楊鐵濱，茅爍，王海濱，于宗辰，張育銘

（東北林業(yè)大學(xué)工程技術(shù)學(xué)院，黑龍江 哈爾濱 150040）

隨著人們對(duì)城市綠化的重視，以及避免道路兩側(cè)枯腐樹枝斷裂掉落而導(dǎo)致安全事故發(fā)生，需要大量使用高空作業(yè)車來對(duì)高處樹枝樹干進(jìn)行修剪工作?，F(xiàn)在高空修枝的作業(yè)方式都是工作人員站在高空作業(yè)車平臺(tái)上進(jìn)行操作，工作過程中地存在安全隱患，工作效率低。在我國(guó)，每年都會(huì)因?yàn)楫a(chǎn)品設(shè)計(jì)本身存在的問題或機(jī)械操作的不規(guī)范造成一些安全事故。所以，要從根本上減少或杜絕人員傷亡事故，提高修枝工作效率，避免城市交通阻塞，就要用機(jī)械代替人力進(jìn)行操作，從而實(shí)現(xiàn)高安全性、高效率的高空作業(yè)［1］。

路徑規(guī)劃一直是空間機(jī)械臂的重點(diǎn)研究方向之一，它旨在機(jī)械臂工作空間中找到一條從初始點(diǎn)到目標(biāo)點(diǎn)且不與障礙物發(fā)生碰撞的最短路徑［2］。目前，國(guó)內(nèi)外學(xué)者對(duì)路徑規(guī)劃問題已獲得了一些研究成果。路徑規(guī)劃方法大致可以分為2種：①是傳統(tǒng)的路徑規(guī)劃方法，如人工勢(shì)場(chǎng)法、A*算法、RRT算法等［3］；②是與智能算法相結(jié)合的路徑規(guī)劃方法，如遺傳算法、蟻群算法（ant clony optimization，ACO）、基于視覺的智能算法等［4］。在上述算法中，ACO具有很強(qiáng)的局部尋優(yōu)能力，本文選擇ACO對(duì)機(jī)械臂進(jìn)行路徑進(jìn)行規(guī)劃研究［5］。

CoppeliaSim能夠方便地實(shí)現(xiàn)多剛體系統(tǒng)的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)仿真，Matlab具有強(qiáng)大的計(jì)算功能，并可以方便地構(gòu)建控制系統(tǒng)［6］。利用這2個(gè)軟件建立機(jī)械臂的聯(lián)合仿真系統(tǒng)，既可以對(duì)機(jī)械臂的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)進(jìn)行仿真分析，又可以搭建機(jī)械臂的控制系統(tǒng)，通過交互式聯(lián)合仿真，可以有效地提高機(jī)械臂的性能，為實(shí)際物理樣機(jī)的研制提供技術(shù)依據(jù)［7］。

1 基于ACO的三維路徑規(guī)劃算法

三維路徑規(guī)劃指在已知三維地圖中，規(guī)劃出一條從出發(fā)點(diǎn)到目標(biāo)點(diǎn)滿足某項(xiàng)指標(biāo)最優(yōu)，并且避開了所有三維障礙物的三維最優(yōu)路徑［8］。一般三維路徑規(guī)劃算法存在計(jì)算過程復(fù)雜、信息存儲(chǔ)量大、難以直接進(jìn)行全局規(guī)劃等問題。ACO具有分布計(jì)算、群體智能等特點(diǎn)，在路徑規(guī)劃算法中有較大優(yōu)勢(shì)。

1.1 算法流程與Matlab程序

基于ACO的三維路徑規(guī)劃算法流程如圖1所示?；贏CO的三維路徑規(guī)劃算法主要包含4個(gè)函數(shù)：①啟發(fā)值函數(shù)，該函數(shù)主要用于計(jì)算可視區(qū)域內(nèi)各點(diǎn)的啟發(fā)值；②適應(yīng)度計(jì)算函數(shù)，該函數(shù)主要用于計(jì)算每條路徑上的適應(yīng)度值；③路徑搜索函數(shù)，該函數(shù)采用ACO根據(jù)信息素和啟發(fā)值，搜索出從出發(fā)點(diǎn)到終點(diǎn)的三維路徑；④主函數(shù)，該函數(shù)主要用于ACO的全局尋優(yōu)，通過迭代尋找全局最優(yōu)解。

圖1 三維路徑規(guī)劃算法Fig.1 Three dimensional path planning algorithm

1.2 仿真結(jié)果

采用ACO進(jìn)行三維路徑規(guī)劃，為展示算法的精度，便于宏觀判斷，規(guī)劃范圍為200 m×200 m×200 m的城市街區(qū)，其中X、Y、Z軸方向每個(gè)節(jié)點(diǎn)的間距為50 m。路徑起點(diǎn)在規(guī)劃空間的序號(hào)為［11050］，終點(diǎn)在規(guī)劃空間的序號(hào)為［214150］。算法的基本設(shè)置為種群規(guī)模為100只，算法迭代200次，路徑規(guī)劃結(jié)果如圖2所示。

圖2 仿真結(jié)果Fig.2 Path planning results

2 虛擬樣機(jī)機(jī)械系統(tǒng)

2.1 機(jī)械臂三維模型的建立

CoppeliaSim具有很強(qiáng)的機(jī)械臂運(yùn)動(dòng)學(xué)仿真能力，但很難精確建立復(fù)雜的三維實(shí)體模型［9］。因此，利用SolidWorks建立了高空修枝工作裝置的實(shí)體模型。設(shè)計(jì)的機(jī)械臂具有4個(gè)關(guān)節(jié)，能實(shí)現(xiàn)4自由度的回轉(zhuǎn)運(yùn)動(dòng)。機(jī)械臂三維模型如圖3所示。

圖3 機(jī)械臂三維模型Fig.3 Three dimensional model of manipulator

2.2 機(jī)械臂運(yùn)動(dòng)仿真系統(tǒng)的建立

機(jī)械臂運(yùn)動(dòng)學(xué)仿真需要在CoppeliaSim中進(jìn)行，因此需要將SolidWorks中建立的三維模型導(dǎo)入到CoppeliaSim中。其方法是將SolidWorks中的文件保存為STL格式，然后導(dǎo)入到CoppeliaSim中［10］。在CoppeliaSim中，對(duì)導(dǎo)入的每個(gè)零部件進(jìn)行編輯，定義其材料、質(zhì)量、轉(zhuǎn)動(dòng)慣量等相關(guān)屬性，從而使虛擬樣機(jī)與實(shí)際物理樣機(jī)具有相同或者相近的物理特性，以便更好地模擬實(shí)際系統(tǒng)。然后對(duì)機(jī)械臂添加轉(zhuǎn)動(dòng)副約束，定義約束后的機(jī)械臂模型如圖4所示。

圖4 定義約束后的機(jī)械臂模型Fig.4 Manipulator model after defining constraints

3 聯(lián)合仿真系統(tǒng)

聯(lián)合仿真的核心是輸入變量與輸出變量數(shù)據(jù)的實(shí)時(shí)交換。使用CoppeliaSim的逆運(yùn)動(dòng)學(xué)求解器（IK）模塊，可以根據(jù)機(jī)械臂末端執(zhí)行器的位置，自動(dòng)逆向求解出機(jī)械臂各關(guān)節(jié)的絕對(duì)位置和絕對(duì)角度，進(jìn)行機(jī)械臂在笛卡爾空間的運(yùn)動(dòng)控制。在Matlab中利用Simulink仿真模塊搭建聯(lián)合仿真控制系統(tǒng)，將ACO三維路徑規(guī)劃輸出的路徑點(diǎn)和機(jī)械臂的初始點(diǎn)作為輸入變量，機(jī)械臂的絕對(duì)位置和各關(guān)節(jié)的絕對(duì)角度作為輸出變量，進(jìn)行機(jī)械臂的控制，從而實(shí)現(xiàn)機(jī)械系統(tǒng)與控制系統(tǒng)的聯(lián)合仿真。

3.1 CoppeliaSim模型

在CoppeliaSim中進(jìn)行聯(lián)合仿真時(shí)輸入變量的設(shè)置，如圖5所示。

圖5 仿真場(chǎng)景模型Fig.5 Simulation scene model

圖中，如A區(qū)所示，在CoppeliaSim中建立機(jī)械臂連接關(guān)系模型樹；如B區(qū)所示，在CoppeliaSim中通過使用dummy點(diǎn)來進(jìn)行笛卡爾空間的輔助定位，進(jìn)而在控制機(jī)械臂時(shí)確定修枝和放置枝椏時(shí)的絕對(duì)位置；如C區(qū)所示，通過設(shè)置IK Group To Simulink，獲取CoppeliaSim自帶的逆運(yùn)動(dòng)學(xué)計(jì)算工具，方便在Simulink中進(jìn)行控制；如D區(qū)所示，在CoppeliaSim中搭建仿真所需的街道環(huán)境，使仿真更接近于真實(shí)場(chǎng)景；如E區(qū)所示，設(shè)置CoppeliaSim中的IK計(jì)算模塊，使其應(yīng)用于所需機(jī)械臂的計(jì)算，并依次設(shè)置機(jī)械臂的每個(gè)關(guān)節(jié)，通過Simulink控制機(jī)械臂的關(guān)節(jié)位置。

3.2 Simulink模型

在Simulink中搭建聯(lián)合仿真時(shí)的接口、輸入、輸出及控制模塊，如圖6所示。

圖6 Simulink模型Fig.6 Simulink model

圖中，A區(qū)為Simulink與CoppeliaSim聯(lián)合仿真的接口模塊；B區(qū)為輸入變量，獲取機(jī)械臂運(yùn)動(dòng)過程中末端執(zhí)行器的絕對(duì)位置和絕對(duì)姿態(tài)；D區(qū)為輸出變量，輸出機(jī)械臂運(yùn)動(dòng)所需的絕對(duì)位置和絕對(duì)角度；C區(qū)為Stateflow模塊，通過編寫的流程動(dòng)態(tài)，實(shí)現(xiàn)聯(lián)合仿真時(shí)從輸入變量到輸出變量的轉(zhuǎn)變。

3.3 聯(lián)合仿真控制系統(tǒng)

聯(lián)合仿真控制系統(tǒng)主要通過Simulink模型中的Stateflow模塊實(shí)現(xiàn)。通過編寫的流程動(dòng)態(tài)代碼，實(shí)現(xiàn)聯(lián)合仿真時(shí)從輸入變量到輸出變量的轉(zhuǎn)變。流程動(dòng)態(tài)如圖7所示。

圖7 Stateflow流程動(dòng)態(tài)Fig.7 Stateflow process dynamic diagram

4 仿真結(jié)果討論

為了對(duì)機(jī)械臂聯(lián)合仿真系統(tǒng)的性能進(jìn)行驗(yàn)證，以末端執(zhí)行器為例，分別測(cè)試了末端執(zhí)行器運(yùn)動(dòng)到待修剪枝椏處X、Y、Z坐標(biāo)位置變化，如圖8～圖10所示。

圖8 末端執(zhí)行器在X方向上的坐標(biāo)位置Fig.8 Coordinate position of end effector in X direction

圖9 末端執(zhí)行器在Y方向上的坐標(biāo)位置Fig.9 Coordinate position of end effector in Y direction

通過聯(lián)合仿真分析可知，末端執(zhí)行器在較短的時(shí)間內(nèi)，可以由初始位置上升，并穩(wěn)定地達(dá)到目標(biāo)位置。CoppeliaSim的仿真過程證明，機(jī)械臂末端執(zhí)行器由初始位置迅速到達(dá)指令給定位置，并在該位置保持不變，具有較好的軌跡跟蹤性能。

在末端執(zhí)行器X、Y、Z坐標(biāo)位置變化曲線中，仿真實(shí)驗(yàn)在剛開始時(shí)，會(huì)有較大的波動(dòng)，其主要原因是現(xiàn)有機(jī)械臂控制系統(tǒng)受自身重力影響起動(dòng)惰性較大，但幅度對(duì)路徑規(guī)劃影響不大，系統(tǒng)很快達(dá)到收斂。

5 結(jié)語

本文開發(fā)的虛擬仿真設(shè)計(jì)方法，在避免推導(dǎo)繁瑣的運(yùn)動(dòng)學(xué)方程的情況下，實(shí)現(xiàn)高空作業(yè)車機(jī)械臂的路徑規(guī)劃仿真設(shè)計(jì)，降低了機(jī)械臂開發(fā)成本的同時(shí)，節(jié)省了迭代設(shè)計(jì)時(shí)間。通過應(yīng)用Solidworks、Matlab、CoppeliaSim建立了高空作業(yè)車機(jī)械臂模型，并通過基于ACO的三維路徑規(guī)劃算法計(jì)算出機(jī)械臂末端執(zhí)行器運(yùn)動(dòng)軌跡，建立了聯(lián)合仿真控制系統(tǒng)。通過仿真實(shí)驗(yàn)驗(yàn)證了機(jī)械臂系統(tǒng)的可行性。所采用的設(shè)計(jì)分析方法，為機(jī)械臂路徑規(guī)劃研究提供了一種新的途徑。本文提出的虛擬設(shè)計(jì)和仿真技術(shù)還可以為高空作業(yè)車操作員提供操作和培訓(xùn)指導(dǎo)，在降低操作難度的同時(shí)，更能大大提高生產(chǎn)作業(yè)效率。