基于OROCOS的工業(yè)機器人控制軟件設(shè)計

曾 玥，杜澤強，田會方，吳迎峰

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

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

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

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

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

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

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

圖1 總體控制原理示意圖

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

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

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

2.1 軟件總體設(shè)計

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

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

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

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

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

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

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

圖3 機器人坐標系

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

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

namespace KDL

{

Chain myrobot;//實例化對象

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);//正運動學(xué)求解器

}

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

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

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

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

KDL :: Path_RoundedComposite路徑，由帶有圓角的路徑點組成。

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)主要分為計算軌跡和運行軌跡。

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

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

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

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

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

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

圖5 UML類圖

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

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

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

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

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

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

圖6 主界面

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

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

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

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

表2 運動指令表

4 軌跡測試

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

圖7 路徑示意圖

末端執(zhí)行器路徑點配置如下，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個插值路徑點，12個路徑段。

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

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

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

對笛卡爾空間中的每一個路徑點求逆解，即可得到關(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ā)送運動控制指令到伺服控制器。

5 結(jié)論

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