曾 玥，杜澤強(qiáng)，田會方，吳迎峰

(1.武漢理工大學(xué) 機(jī)電工程學(xué)院，湖北 武漢 430070；2.西安向陽航天材料股份有限公司，陜西 西安 710065)

工業(yè)機(jī)器人行業(yè)已有大半個世紀(jì)的歷史了，一直熱衷于適應(yīng)電子、機(jī)械、材料和通信等方面的標(biāo)準(zhǔn)的革新[1-2]。但是，該行業(yè)當(dāng)前的軟件實(shí)現(xiàn)情況與20年前的計算機(jī)行業(yè)非常相似：每個制造商都有自己的專有軟件、算法、數(shù)據(jù)結(jié)構(gòu)以及編程語言。定義開放數(shù)據(jù)格式標(biāo)準(zhǔn)或編程語言的努力僅取得了很小的成功，因此軟件交換的可能性實(shí)際上非常低[3-4]。工業(yè)機(jī)器人行業(yè)缺乏互操作性和開放性，這導(dǎo)致了新手會面臨巨大的投入門檻和較低的創(chuàng)新交叉利用率。開放式機(jī)器人軟件(open robot control software,OROCOS)解決了這一難點(diǎn)。

OROCOS適用于纏繞生產(chǎn)線機(jī)器人主要有兩大優(yōu)點(diǎn) ：

(1)相對于常見的機(jī)器人操作系統(tǒng)(robot operating system,ROS)，OROCOS的最大優(yōu)勢是能夠提供現(xiàn)成的硬實(shí)時操作。生產(chǎn)線上各個工位的執(zhí)行時間一般情況下是確定的，也會有突發(fā)事件，因此對于機(jī)器人控制系統(tǒng)的實(shí)時性有要求。OROCOS實(shí)時工具包(real time transmit,RTT)可以讓機(jī)器人在確定的時間內(nèi)執(zhí)行任務(wù)或響應(yīng)關(guān)鍵事件[5]。

(2)如果纏繞的對象改變，生產(chǎn)線各個裝置的位置分布和執(zhí)行流程也會變化，因此期望機(jī)器人控制系統(tǒng)只需修改盡量少的程序，就能適應(yīng)這些調(diào)整。OROCOS使用基于組件的軟件開發(fā)(component-based software development,CBSD)技術(shù)能很好地滿足這一點(diǎn)。OROCOS使用模塊化控制結(jié)構(gòu)可大大提高代碼的可重用性，易于跟蹤流程執(zhí)行，減少編程錯誤[6]。

1 總體控制原理設(shè)計

纏繞生產(chǎn)線機(jī)器人的控制原理主要包括Qt人機(jī)交互界面和ORCOS機(jī)器人算法中間件，如圖1所示。

圖1 總體控制原理示意圖

Qt人機(jī)交互界面基于Linux操作系統(tǒng)，主要包括機(jī)器人參數(shù)設(shè)置、手動控制、自動控制和動畫仿真功能。操作者可以用手動控制進(jìn)行調(diào)試，進(jìn)而使用自動控制編寫運(yùn)動程序。

手動控制產(chǎn)生的指令或自動控制程序經(jīng)命令解釋器產(chǎn)生的指令信號傳遞給OROCOS機(jī)器人算法中間件。中間件將進(jìn)行各種機(jī)器人算法分析，主要包括運(yùn)動學(xué)分析和軌跡規(guī)劃，由各個組件程序進(jìn)行互聯(lián)實(shí)現(xiàn)[7]。

2 OROCOS算法中間件設(shè)計

2.1 軟件總體設(shè)計

圖2顯示了構(gòu)成應(yīng)用的組件。命令和消息處理組件在一個線程中運(yùn)行，負(fù)責(zé)處理與人機(jī)界面程序的交互以及需要記錄的任何信息。運(yùn)動學(xué)和軌跡組件在一個單獨(dú)的線程中運(yùn)行，負(fù)責(zé)生成要發(fā)送給機(jī)器人的關(guān)節(jié)級命令。機(jī)器人組件負(fù)責(zé)通過以太網(wǎng)與機(jī)器人硬件進(jìn)行交互。此組件在其自己的線程中以高于任何其他組件的頻率運(yùn)行，以便以所需的速率中繼命令和更新信息。

圖2 主要組件和關(guān)系圖

CMake跨平臺構(gòu)建自動化系統(tǒng)用于管理應(yīng)用程序中軟件組件的編譯。此系統(tǒng)允許使用獨(dú)立于編譯代碼的系統(tǒng)生成配置文件。然后，該系統(tǒng)自動生成makefile，以便與程序中使用的Ubuntu Linux操作系統(tǒng)中的GNU編譯器集合一起使用。

在運(yùn)行時，通過以太網(wǎng)互聯(lián)網(wǎng)協(xié)議從人機(jī)界面程序接收消息。這些消息被中繼到消息處理組件，該組件處理這些消息以標(biāo)識人機(jī)界面正在發(fā)出的請求。如果消息是請求信息，則消息處理程序?qū)⑹占埱笮畔⒉⑵浒l(fā)送到人機(jī)界面。如果消息是要執(zhí)行的指令，則消息處理程序?qū)⒃趹?yīng)用程序的其他組件中啟動相應(yīng)的命令或方法，并回復(fù)人機(jī)界面以指示指令的成功或失敗。例如，笛卡爾點(diǎn)對點(diǎn)運(yùn)動命令將被中繼到軌跡生成組件。然后，軌跡生成器將開始順序輸出位于連接初始位置和目標(biāo)位置的直線上的設(shè)定點(diǎn)，在每個更新周期向目標(biāo)移動一步。在工作空間限制器檢查以確保設(shè)置點(diǎn)不位于標(biāo)記為“禁止”的區(qū)域之后，這些笛卡爾設(shè)置點(diǎn)將通過反向運(yùn)動學(xué)組件，該組件將它們轉(zhuǎn)換為關(guān)節(jié)空間表示。然后，所需的關(guān)節(jié)配置通過關(guān)節(jié)限制器，該限制器確保每個設(shè)定點(diǎn)不違反任何關(guān)節(jié)行程限制。然后，所允許的配置進(jìn)入機(jī)器人組件，該組件指示控制硬件執(zhí)行運(yùn)動。人機(jī)界面在關(guān)節(jié)空間中發(fā)送的命令將繞過該系統(tǒng)的早期階段，關(guān)節(jié)空間軌跡生成組件直接將其反饋到關(guān)節(jié)限制器中。

2.2 正逆運(yùn)動學(xué)組件實(shí)現(xiàn)

所有通過機(jī)器人組件發(fā)送給機(jī)器人硬件的命令都用關(guān)節(jié)空間表示，但是系統(tǒng)的上層命令是用笛卡爾空間表示。因此，需要一個或多個運(yùn)動學(xué)組件來表示之間的轉(zhuǎn)換量。正運(yùn)動學(xué)將關(guān)節(jié)角度轉(zhuǎn)換為笛卡爾位置，逆運(yùn)動學(xué)將笛卡爾運(yùn)動轉(zhuǎn)換為相應(yīng)的關(guān)節(jié)角度或速率[8]。OROCOS運(yùn)動學(xué)庫(kinematics and dynamics library,KDL)提供了涉及機(jī)器人串聯(lián)運(yùn)動鏈的運(yùn)動學(xué)計算功能。它定義了基本幾何的類，包括向量和旋轉(zhuǎn)矩陣。三維變換由Frame表示，它包含一個表示位移距離的向量和一個表示相對坐標(biāo)軸的旋轉(zhuǎn)矩陣。提供了組合變換和反變換計算的功能。還定義了Twist類表示速度和角速度的6x1矩陣，Wrench類表示力和力矩，并且可以通過一個框架從一個坐標(biāo)系轉(zhuǎn)換到另一個坐標(biāo)系。

根據(jù)纏繞生產(chǎn)線機(jī)器人的機(jī)械結(jié)構(gòu)，建立坐標(biāo)系，如圖3所示，得出D-H參數(shù)如表1所示。

圖3 機(jī)器人坐標(biāo)系

表1 機(jī)器人D-H參數(shù)

正逆運(yùn)動學(xué)的D-H參數(shù)配置在configureHook()中完成，具體配置代碼如下：

namespace KDL

{

Chain myrobot;//實(shí)例化對象

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0,0,0,0)));//軸1

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0.18,-M_PI/2,0,-M_PI×3/4)));//軸2

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0.6650,0,0,M_PI/4)));//軸3

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0.17,-M_PI/2,0.6750,0)));//軸4

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0,M_PI/2,0,0)));//軸5

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0,-M_PI/2,0,0)));//軸6

ChainFkSolverPos_recursive fwdkin(myrobot);//正運(yùn)動學(xué)求解器

}

上述代碼中Chain類用于構(gòu)建運(yùn)動學(xué)鏈，它封裝了串行運(yùn)動學(xué)互連結(jié)構(gòu)，本組件實(shí)例化了一個運(yùn)動學(xué)鏈對象armChain。addSegment()函數(shù)為運(yùn)動學(xué)鏈添加段(Segment)，纏繞生產(chǎn)線機(jī)器人為6自由度機(jī)器人，即需添加6個段。每個段的D-H參數(shù)包含在Frame中。

在運(yùn)行更新(updateHook)期間，大部分工作是在KDL提供的運(yùn)動學(xué)求解器中完成的。正向運(yùn)動學(xué)求解器KDL :: ChainFkSolverPos_recursive使用當(dāng)前關(guān)節(jié)角度來計算末端執(zhí)行器位置，從第一個Frame開始，然后沿著運(yùn)動學(xué)鏈連續(xù)右乘變換矩陣。逆位置運(yùn)動學(xué)由KDL :: ChainIkSolverPos_NR_JL提供，它使用牛頓迭代法以及逆速度運(yùn)動學(xué)求解器。逆速度運(yùn)動學(xué)的實(shí)現(xiàn)采用雅可比偽逆的KDL求解器。

2.3 軌跡生成組件實(shí)現(xiàn)

從人機(jī)交互界面接收到的運(yùn)動指令僅僅是一個目標(biāo)位置或一串離散的路徑點(diǎn)。但是，機(jī)器人執(zhí)行組件需要不斷更新所需位置。因此，必須實(shí)現(xiàn)軌跡生成功能，以產(chǎn)生更密的路徑點(diǎn)序列，使機(jī)器人從起始位置平穩(wěn)地移動到目標(biāo)位置。KDL為軌跡生成功能提供了一些相關(guān)的類，本設(shè)計中需要用到的幾個關(guān)鍵的類分別為：

KDL :: Path_RoundedComposite路徑，由帶有圓角的路徑點(diǎn)組成。

KDL :: VelocityProfile_Trap梯形速度曲線。

KDL::Trajectory_Segment由VelocityProfile_Trap和Path_RoundedComposite組合成的一條基礎(chǔ)軌跡。

KDL :: Trajectory_Composite由基礎(chǔ)軌跡組成的復(fù)合軌跡，調(diào)用添加函數(shù)以添加軌跡。

軌跡規(guī)劃的流程如圖4所示，其代碼編寫在updateHook()函數(shù)中，它是以一個周期不斷循環(huán)的，每次循環(huán)主要分為計算軌跡和運(yùn)行軌跡。

圖4 軌跡規(guī)劃流程圖

(1)計算軌跡。首先，遍歷每個路徑點(diǎn)位的位姿，將其添加到路徑中，創(chuàng)建帶有圓角的路徑。然后，設(shè)置速度和加速度上限，創(chuàng)建一條從起始位置到終止位置的梯形速度曲線。最后將路徑和速度曲線合成一條基礎(chǔ)軌跡，添加到復(fù)合軌跡中。

(2)運(yùn)行軌跡。首先判斷軌跡是否創(chuàng)建成功，再判斷當(dāng)前時間是否在運(yùn)行設(shè)置的軌跡運(yùn)行時長內(nèi)。然后從創(chuàng)建的軌跡中獲取當(dāng)前的期望狀態(tài)，包括位置、速度和加速度信息。再給當(dāng)前時間增加一個周期，這個周期是軌跡規(guī)劃組件的循環(huán)周期。最后輸出調(diào)整狀態(tài)的命令，命令的參數(shù)同樣包括位置、速度和加速度信息。如果當(dāng)前時間超過設(shè)置運(yùn)行時長，則軌跡規(guī)劃完成。

3 人機(jī)交互界面軟件設(shè)計

3.1 軟件架構(gòu)設(shè)計

統(tǒng)一建模語言(unified modeling language，UML)是一種編制軟件藍(lán)圖的標(biāo)準(zhǔn)化語言，它的目標(biāo)之一就是為開發(fā)團(tuán)隊提供標(biāo)準(zhǔn)通用的設(shè)計語言來開發(fā)和構(gòu)建計算機(jī)應(yīng)用[9]。如圖5所示的UML圖是使用ProcessOn軟件制作，項(xiàng)目主要用到的類有Base、Component、EndEfector、Handler等，以下分別詳細(xì)介紹這些類。

圖5 UML類圖

(1)Base類負(fù)責(zé)數(shù)據(jù)管理和與圖形用戶界面的通信，整個項(xiàng)目中的Base只實(shí)例化一次，并且將其匯總到所有與GUI(graphical user interface)相關(guān)的所有類中。與項(xiàng)目中的許多其他類一樣，該類繼承自屬于所使用框架的QObject庫。

(2)Component類是負(fù)責(zé)保存與其實(shí)例化各個段的所有信息的類。其構(gòu)造函數(shù)接收在Base中創(chuàng)建的機(jī)器人實(shí)體的rootEntity作為參數(shù)。此類的參數(shù)與.obj模型的零件渲染密切相關(guān)。Component類還可以獲取和設(shè)置定義零件配置所必需的點(diǎn)、軸和角度。Component類提供了動畫配置，例如基于設(shè)置速度的動畫持續(xù)時間。每個元素都會添加一個Controller類對象。

(3)EndEfector類是一個從Component繼承并與機(jī)器人的末端執(zhí)行器相關(guān)聯(lián)的類。其中包含用于控制要執(zhí)行的任務(wù)及其持續(xù)時間的方法和屬性。末段執(zhí)行器是用于夾持氣瓶芯模，因此它的狀態(tài)有夾緊、釋放和保持。

(4)Controller類實(shí)例化每個Component的控制器。這些控制器的目標(biāo)是每個Component的QTransform屬性 ，即它們處理機(jī)器人每個段的空間變換。變換操作是在動畫的每一幀中給出配置值。因此，存在與相應(yīng)Component類鏈接的方法，例如零件點(diǎn)、軸和角度的獲取和設(shè)置。Controller類中最重要的功能是更新Component變換特性的4×4矩陣，包含零件執(zhí)行的平移和旋轉(zhuǎn)信息。

3.2 基本手動功能設(shè)計

如圖6所示的主界面包括一個主窗口(Widget類 )，以及用于啟動和停止機(jī)器人的按鈕。啟動隨著一系列的歸位運(yùn)動自動進(jìn)行。停止動作會將機(jī)器人制動在當(dāng)前位置，使其處于未激活狀態(tài)。該主窗口還具有一個文本框，該文本框指示機(jī)器人的整體狀態(tài)(啟動或停止)，另外兩個文本框提供有關(guān)每個關(guān)節(jié)的角度、相對速度和末端執(zhí)行器配置的信息。按鈕“設(shè)置初始特征”，“加載命令文件”和“選擇模式”將打開其他各個窗口。

圖6 主界面

在主窗口中，如果按下“設(shè)置初始特征”按鈕，則將打開“屬性”窗口。在此窗口中，用戶可以輸入機(jī)器人的所需參數(shù)：關(guān)節(jié)角度及其各自的速度。輸入這些參數(shù)后，當(dāng)用戶按下“確定”按鈕時，將加載這些數(shù)據(jù)，如果它們與先前的數(shù)據(jù)不同，則機(jī)器人將繼續(xù)執(zhí)行相應(yīng)的運(yùn)動。

如果用戶從主窗口按下“選擇模式”按鈕，則會打開操作窗口。用戶可以從該窗口完全控制機(jī)器人，通過窗口中相應(yīng)的滑塊控制每個關(guān)節(jié)的角度和速度來移動機(jī)器人。

3.3 自動控制功能設(shè)計

實(shí)際生產(chǎn)中，除非生產(chǎn)線的其他裝置改變，機(jī)器人的運(yùn)動應(yīng)該是重復(fù)的。這需要設(shè)計一個自動控制功能，用戶能將手動控制的指令記錄下來，并且保存起來，使機(jī)器人能按照指令執(zhí)行一連串動作。因此設(shè)計了一套類似G代碼的運(yùn)動指令，如表2所示。在操作窗口中，用戶還可以通過在相應(yīng)的文本框中逐行輸入運(yùn)動指令來使機(jī)器人運(yùn)動。

表2 運(yùn)動指令表

4 軌跡測試

軌跡路徑如圖7所示，選取8個末段執(zhí)行器的路徑點(diǎn)：①初始位置；②水平旋轉(zhuǎn)工作臺附近；③④在水平工作臺掛鉤上；⑤再次退到水平工作臺附近；⑥在纖維纏繞機(jī)處；⑦在三抓卡盤中；⑧在纖維纏繞機(jī)附近。

圖7 路徑示意圖

末端執(zhí)行器路徑點(diǎn)配置如下，Vector表示位置矢量，單位為毫米，EulerZYZ表示Z-Y-Z形式的歐拉角姿態(tài)矢量，單位為弧度。

path->Add(Frame(Rotation::EulerZYZ(M_PI/2,-M_PI/2,0),

Vector(0,-384.8,850.2)));// ①

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1037,0,735)));// ②

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1080,0,735)));// ③

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1080,0,745)));// ④

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1037,0,745)));// ⑤

path->Add(Frame(Rotation::EulerZYZ(-M_PI/2,-M_PI/2,-M_PI/2),Vector(-350,1172,1000)));// ⑥

path->Add(Frame(Rotation::EulerZYZ(-M_PI/2,-M_PI/2,-M_PI/2),Vector(-410,1172,1000)));// ⑦

path->Add(Frame(Rotation::EulerZYZ(-M_PI/2,-M_PI/2,-M_PI/2),Vector(-410,1000,1000)));// ⑧

笛卡爾路徑測試結(jié)果如圖8所示，得到1 559個插值路徑點(diǎn)，12個路徑段。

圖8 笛卡爾路徑規(guī)劃圖

如圖9所示笛卡爾空間x、y、z方向的位置隨時間變化，總體比較平緩，沒有劇烈變化。速度隨時間變化曲線為梯型，速度和加速度都在控制范圍以內(nèi)。

圖9 笛卡爾各方向位置-時間和路徑速度-時間變化圖

對笛卡爾空間中的每一個路徑點(diǎn)求逆解，即可得到關(guān)節(jié)空間中每個關(guān)節(jié)角隨時間變化的曲線，如圖10所示。對關(guān)節(jié)值求差分，可以得到角速度隨時間變化的曲線,如圖11所示。

圖10 各關(guān)節(jié)角-時間圖

圖11 各關(guān)節(jié)角速度-時間圖

經(jīng)過以上求解，軟件會自動生成G代碼，保存在文件中。導(dǎo)入到Qt界面，逐行讀取G代碼，發(fā)送運(yùn)動控制指令到伺服控制器。

5 結(jié)論

針對水處理纏繞罐自動纏繞及固化生產(chǎn)線中對工業(yè)機(jī)器人控制的需求，對機(jī)器人控制軟件進(jìn)行整體方案設(shè)計。在Linux和OROCOS系統(tǒng)下，用C++編寫消息處理組件、正向運(yùn)動學(xué)組件、工作空間限制器等組件并將其連接。用Qt設(shè)計人機(jī)交互界面，手動控制、自動控制等功能。進(jìn)行3D動畫仿真,機(jī)器人各個關(guān)節(jié)均可按照指令轉(zhuǎn)動，滿足設(shè)計預(yù)期要求。