基于OROCOS的工業機器人控制軟件設計

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

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

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

OROCOS適用于纏繞生產線機器人主要有兩大優點 ：

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

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

1 總體控制原理設計

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

圖1 總體控制原理示意圖

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

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

2 OROCOS算法中間件設計

2.1 軟件總體設計

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

圖2 主要組件和關系圖

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

在運行時，通過以太網互聯網協議從人機界面程序接收消息。這些消息被中繼到消息處理組件，該組件處理這些消息以標識人機界面正在發出的請求。如果消息是請求信息，則消息處理程序將收集請求信息并將其發送到人機界面。如果消息是要執行的指令，則消息處理程序將在應用程序的其他組件中啟動相應的命令或方法，并回復人機界面以指示指令的成功或失敗。例如，笛卡爾點對點運動命令將被中繼到軌跡生成組件。然后，軌跡生成器將開始順序輸出位于連接初始位置和目標位置的直線上的設定點，在每個更新周期向目標移動一步。在工作空間限制器檢查以確保設置點不位于標記為“禁止”的區域之后，這些笛卡爾設置點將通過反向運動學組件，該組件將它們轉換為關節空間表示。然后，所需的關節配置通過關節限制器，該限制器確保每個設定點不違反任何關節行程限制。然后，所允許的配置進入機器人組件，該組件指示控制硬件執行運動。人機界面在關節空間中發送的命令將繞過該系統的早期階段，關節空間軌跡生成組件直接將其反饋到關節限制器中。

2.2 正逆運動學組件實現

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

根據纏繞生產線機器人的機械結構，建立坐標系，如圖3所示，得出D-H參數如表1所示。

圖3 機器人坐標系

表1 機器人D-H參數

正逆運動學的D-H參數配置在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);//正運動學求解器

}

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

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

2.3 軌跡生成組件實現

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

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

KDL :: VelocityProfile_Trap梯形速度曲線。

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

KDL :: Trajectory_Composite由基礎軌跡組成的復合軌跡，調用添加函數以添加軌跡。

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

圖4 軌跡規劃流程圖

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

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

3 人機交互界面軟件設計

3.1 軟件架構設計

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

圖5 UML類圖

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

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

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

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

3.2 基本手動功能設計

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

圖6 主界面

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

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

3.3 自動控制功能設計

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

表2 運動指令表

4 軌跡測試

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

圖7 路徑示意圖

末端執行器路徑點配置如下，Vector表示位置矢量，單位為毫米，EulerZYZ表示Z-Y-Z形式的歐拉角姿態矢量，單位為弧度。

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)));// ⑧

笛卡爾路徑測試結果如圖8所示，得到1 559個插值路徑點，12個路徑段。

圖8 笛卡爾路徑規劃圖

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

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

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

圖10 各關節角-時間圖

圖11 各關節角速度-時間圖

經過以上求解，軟件會自動生成G代碼，保存在文件中。導入到Qt界面，逐行讀取G代碼，發送運動控制指令到伺服控制器。

5 結論

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