基于ABB 機器人Rapid 編程語言的機器人軌跡規(guī)劃研究

徐源東， 朱 攀， 林 琳

（云南省機械研究設(shè)計院， 云南省機電一體化應(yīng)用技術(shù)重點實驗室， 云南 昆明 650031）

0 引言

機器人的根本任務(wù)， 就是要實現(xiàn)從一個開始位置到一個目標位置的運動， 這就需要通過對復(fù)雜組合體進行規(guī)劃來實現(xiàn)。 機器人的軌跡規(guī)劃（Trajectory Planning），就是要為機器人從初始位姿移動到目標位姿， 通過指定一系列參數(shù)，找出一條無碰撞且可行（連續(xù)平滑）的路徑，生成相關(guān)變量的時間律， 以描述期望的軌跡。 本文基于ABB 機器人Rapid 編程語言， 對機器人的運動學進行分析，進行了機器人軌跡規(guī)劃的研究與計算。

1 機器人位置與姿態(tài)

1.1 機器人坐標系

軌跡規(guī)劃的基本問題是將操作臂從初始位置移動到最終期望位置——即將工具坐標從當前值{Tinitial}移動到期望值{Tfinal}。 在工業(yè)機器人應(yīng)用中，用工具坐標系{T}相對于工件坐標系{S}的運動來描述。在ABB 機器人中表示為TCP（工具中心點：帶方向的點）相對于wobj（工件坐標系）的運動。

目前還沒有國際統(tǒng)一的機器人坐標系標準命名，本文以ABB 機器人的命名方式為例， 將機器人運動的相關(guān)坐標系分為5 類坐標系，用以描述機器人的相關(guān)運動。 分別為：大地坐標系（World coordinates）、基座標系（Base coordinates）、 工具坐標系 （Tool coordinates）、 用戶坐標系（User coordinates）和工件坐標系（Object coordinates）[1]。 各坐標系關(guān)系見圖1。

圖1 機器人運動相關(guān)坐標系

1.2 機器人目標點位數(shù)據(jù)與工具姿態(tài)

1.2.1 ABB 機器人目標位置定義

ABB 機器人RAPID 程序中，用兩種類型的數(shù)據(jù)描述機器人點位：

關(guān)節(jié)位置數(shù)據(jù)jointtarget 和TCP 位置數(shù)據(jù)robtarget。jointarget 記錄的是機器人各個軸的絕對偏移位置；robtarget 記錄的是機器人笛卡爾坐標系下的xyz 坐標及角度等數(shù)據(jù)。 兩種指令格式分別見圖2。

圖2 兩種類型的數(shù)據(jù)描述機器人點位

1.2.2 工具姿態(tài)描述

三維空間坐標系的姿態(tài)方向可通過基準坐標系的旋轉(zhuǎn)來表示，姿態(tài)的表示方法有很多種，如旋轉(zhuǎn)矩陣（標準正交陣）、轉(zhuǎn)角排列設(shè)定法（X-Y-Z 固定角、Z-Y-X 歐拉角、Z-Y-Z 歐拉角）、軸角（等效角度-軸線）、四元數(shù)（歐拉參數(shù)）等。 在ABB 機器人RAPID 編程語言中，以四元數(shù)（Quaternion）描述和儲存坐標系姿態(tài)。在實際應(yīng)用項目中，常通過將四元數(shù)轉(zhuǎn)化為歐拉角應(yīng)用， 如視覺定位抓取項目中，獲取產(chǎn)品的偏移角度后，通過獲取標定目標點的歐拉角Z，對相應(yīng)值進行計算后，對產(chǎn)品進行旋轉(zhuǎn)定位抓取。

（1）Z-Y-X 歐拉角：

坐標系{B}的歐拉角表示方法如下：

首先將坐標系{B}和一個已知參考坐標系{A}重合。先將{B}繞ZB^ 轉(zhuǎn)α 角，再繞Y^B轉(zhuǎn)β 角，最后繞X^B轉(zhuǎn)γ 角。這樣三個一組的旋轉(zhuǎn)被稱作歐拉角[2]。

圖3 Z-Y-X 歐拉角

（2）四元數(shù)：

在ABB 機器人RAPID 編程語言中，以四元數(shù)（Quaternion）描述和儲存坐標系姿態(tài)，姿態(tài)數(shù)據(jù)類型是Orient，格式為[q1，q2，q3，q4]，四元數(shù)是帶符號的常數(shù)，始終滿足平方和等于1：

式中的（x1，x2，x3）、（y1，y2，y3）、（z1，z2，z3）分別為旋轉(zhuǎn)坐標系X’，Y’，Z’軸單位向量在基準坐標系X、Y、Z 軸上的投影。

2 機器人運動學分析

2.1 機器人正運動學——D-H 表示法

D-H（Denavit-Hartenberg）表示法可用于表示任何機器人構(gòu)型，也可用于表示任何坐標系中的變換，以及任何可能的關(guān)節(jié)和連桿組合。 對于6 軸垂直串聯(lián)機器人的正運動學， 可以通過D-H 參數(shù)轉(zhuǎn)化為各個軸的位姿矩陣，將6 個位姿矩陣右乘即可得到當前機器人末端TRP 的笛卡爾坐標。

機器人的正運動學就是在已知機器人結(jié)構(gòu)參數(shù)的前提下，通過輸入各關(guān)節(jié)的角度（即jointtarget 數(shù)據(jù)），就能計算出當前法蘭盤（TRP）在固定坐標系（wobj）中的位置及姿態(tài)， 即通過關(guān)節(jié)數(shù)據(jù)jointtarget 求笛卡爾坐標數(shù)據(jù)robtarget。

圖4 六軸機器人控制模型與結(jié)構(gòu)參數(shù)

六軸機器人建模如上圖，建立Base 坐標系（X0Y0Z0），之后依次對應(yīng)1 軸到6 軸6 個坐標系（X1Y1Z1……X6Y6Z6），通過D-H 參數(shù)來表示這6 個動坐標系相對于前一個坐標系的關(guān)系（T1-T6）：

對于D-H 參數(shù)，旋轉(zhuǎn)和平移順序如下：先繞當前X軸旋轉(zhuǎn)α 度，再沿新的X 軸平移a，再繞新的Z 軸旋轉(zhuǎn)θ度，再沿新的Z 軸平移d。

按上式計算整理后，可以得到基于D-H 參數(shù)的位姿矩陣如下：

品牌工業(yè)機器人的D-H 參數(shù)一般機器人生產(chǎn)廠家在系統(tǒng)參數(shù)中已做設(shè)置。 在ABB 機器人中，相關(guān)D-H 參數(shù)一般在系統(tǒng)模塊（System moudle）中定義，在安裝系統(tǒng)時已自動加載，不會受用戶作業(yè)刪除操作影響，可在DHParameter 內(nèi)查看相關(guān)值。

TRP 是確定工具作業(yè)點、設(shè)定工具數(shù)據(jù)的基準位置。根據(jù)D-H 參數(shù)確定TRP 后，就可進一步設(shè)置工業(yè)機器人的工具作業(yè)點TCP（Tool Control Point），它是機器人關(guān)節(jié)、直線、圓弧插補等移動指令的控制對象，指令中的起點、終點就是TCP， 在指定坐標系上的位置值TCP 的位置與工具形狀、安裝方式密切相關(guān)。

在ABB 機器人的RAPID 編程語言中， 可直接使用CalcRobt 函數(shù)，將關(guān)節(jié)數(shù)據(jù)jointtarget 直接轉(zhuǎn)換為笛卡爾坐標系數(shù)據(jù)robtarget，示例如下：

CalcRobT（j_ temp，tool1WObj：=wobj0）

2.2 機器人逆運動學

逆運動學問題是由給定的末端執(zhí)行器位置和姿態(tài)，求解一系列滿足期望要求的關(guān)節(jié)角。 逆運動學可分解為兩部分：①進行坐標系變換求出相對于基座標系{B}的腕部坐標系{W}；②應(yīng)用逆運動學求關(guān)節(jié)角。

在ABB 機器人RAPID 編程語言中，即已知robtarget笛卡爾坐標系的偏移值tran.（x，y，z）， 和姿態(tài)rot.（q1，q2，q3，q4），根據(jù)已有D-H 參數(shù)，求解關(guān)節(jié)偏移jointtarget 的解。目的是將分配給末端執(zhí)行器在操作空間的運動，變換為相應(yīng)的關(guān)節(jié)空間的運動，使得期望的運動能執(zhí)行。

逆運動學相對于正運動學需研究以下問題：

（1）解的存在性：工作空間是機器人末端執(zhí)行器所能到達的范圍。 若解存在，則目標點必須在工作空間內(nèi)。 常規(guī)品牌機器人在主要參數(shù)中會標注機器人工作半徑，見圖5。

圖5 不同結(jié)構(gòu)類型的機器人的工作空間

（2）多重解問題：多重解的問題不僅取決于自由度的數(shù)量，還取決于非零D-H 參數(shù)的個數(shù)。 通常，連桿的非零參數(shù)越多，可行解的越多。機械的關(guān)節(jié)限制有可能會最終減少在實際結(jié)構(gòu)中多重可行解的數(shù)量[3]。

圖6 為ABB 公司IRB14000 機器人， 單臂有7 個自由度，對于同一個目標位置點Target_960，共有5 種關(guān)節(jié)變量組合可到達目標位置。

在ABB 機器人的RAPID 編程語言中， 可直接使用CalcJointT 函數(shù)，將笛卡爾坐標系數(shù)據(jù)robtarget 直接轉(zhuǎn)換為關(guān)節(jié)數(shù)據(jù)jointtarget，示例如下：

圖6 IRB14000 的右臂對于同一目標點Target_960 的不同軸配置

CalcJointT（Target_10，tool1WObj：=wobj0）

3 軌跡規(guī)劃

3.1 軌跡規(guī)劃綜述

路徑與軌跡規(guī)劃與受到控制的機器人從一個位置移動到另一個位置的方法有關(guān)。 用來指定并規(guī)劃路徑的方法有很多種。 任何在規(guī)定的時間里通過中間點的光滑函數(shù)都可以用來指定精確的路徑形狀。

路徑 （Path）表示在關(guān)節(jié)空間或操作空間中，機械手在執(zhí)行指定運動時必須跟隨的點的構(gòu)型序列，是純幾何描述。 軌跡（Trajectory）是一條指定了時間律的路徑[4]。

軌跡規(guī)劃算法的輸入包括路徑描述、 路徑約束以及由機械手動力學施加的約束， 其輸出是按時間順序給出的位置、 速度和加速度的值構(gòu)成的末端執(zhí)行器 （TCP）軌跡。 軌跡規(guī)劃算法生成一個描述末端執(zhí)行器位置和方向變量依照約束隨時間變化的時間序列。 在軌跡生成的運行時間內(nèi)需要計算位置、速度和加速度。

圖7 機器人在路徑上的依次運動

3.2 關(guān)節(jié)空間的規(guī)劃方法——MoveJ

當以關(guān)節(jié)轉(zhuǎn)角的函數(shù)來描述軌跡在空間和時間的軌跡生成時，通常將每個路徑點用工具坐標{T}相對于固定坐標系{S}的期望位置和姿態(tài)來進行描述。

3.2.1 高次多項式

在一定時間將工具從初始位置移動到目標位置，應(yīng)用逆運動學可以解出對應(yīng)目標位置和姿態(tài)的各個關(guān)節(jié)角。各關(guān)節(jié)初始位置已知，需要確定每個關(guān)節(jié)的運動函數(shù)，t0時刻為該關(guān)節(jié)的初始位置，tf時刻為該關(guān)節(jié)的期望目標位置，用高次多項式作為路徑曲線段，要確定在路徑曲線段的起始點和終止點的位置、速度和加速度，采用一個五次多項式進行插值：

這些約束條件確定了一個具有6 個方程和6 個未知數(shù)的線性方程組，其解為：

3.2.2 帶拋物線過渡的線性函數(shù)用于經(jīng)過中間點的路徑

在工業(yè)機器人實際應(yīng)用中， 從起始點到目標點常會設(shè)置過渡點，經(jīng)過過渡點時，通常使用拋物線進行過渡，保證運動的平滑，減少震動，降低磨損。 在工業(yè)機器人應(yīng)用中拋物線轉(zhuǎn)角半徑大小叫平滑度。

如圖8， 在關(guān)節(jié)空間中位某個關(guān)節(jié)θ 的運動指定了一組中間點，每兩個中間點之間使用線性函數(shù)相連，而各中間點附近使用拋物線過渡。

用j、k、l 表示三個相鄰的路徑點。位于路徑點k 處的過渡區(qū)段的時間間隔為t_k。 位于點j 和k 之間的直線部分的時間間隔為tjk。點j 和k 之間總的時間間隔為tdjk。直線部分的速度為θ·jk， 而在點j 處過渡區(qū)段的加速度為θ··j，如圖8 所示。

已知所有的路徑點θk、期望時間區(qū)間tdjk以及每個路徑點處加速度的模值|θ··k|，則可計算出過渡區(qū)段的時間間隔tk。 對于內(nèi)部路徑點，使用下列公式計算：

圖8 多段帶有過渡區(qū)段的直線路徑

對于第一個路徑段和最后一個路徑段， 整個過渡區(qū)的持續(xù)時間都必須計入路徑中， 所以對于第一個路徑段，使線性區(qū)段速度的兩個表達式相等來求解t1：

上式用于計算給定中間點及各個路徑段的持續(xù)時間，求出多段軌跡中各個過渡區(qū)段的時間和速度。系統(tǒng)使用各個關(guān)節(jié)默認加速度值。

3.3 笛卡爾空間的規(guī)劃方法——MoveL

讓工具在相隔較遠的中間點之間走直線運動， 這種執(zhí)行路徑的模式稱作笛卡爾直線運動。在笛卡爾運動中，可以使用笛卡爾變量關(guān)于時間的任意函數(shù)來定義路徑。

基于笛卡爾空間的運動規(guī)劃中， 組合成軌跡的函數(shù)都是描述笛卡爾變量的時間函數(shù)， 每個路徑點都是由工具坐標系相對于固定坐標系的期望位置和姿態(tài)來確定。路徑點是由{T}相對于{S}來描述的，在運行時以實時更新路徑的速度求出運動學逆解，在笛卡爾空間生成路徑后，通過求解逆運動學來計算出期望的關(guān)節(jié)角度。

3.4 路徑的實時生成

機器人在實時運行時，路徑生成器不斷產(chǎn)生用θ、θ·和θ··構(gòu)造的軌跡，并且將相關(guān)信息傳輸?shù)綑C器人控制系統(tǒng)。

3.4.1 關(guān)節(jié)空間路徑生成

在直線區(qū)段，各個關(guān)節(jié)的軌跡計算如下：

3.4.2 笛卡爾空間路徑生成

使用符號x 表示笛卡爾位姿矢量的分量， 重寫式（14）和式（15）。 則在曲線的直線區(qū)段，x 中的每個自由度按下式計算：

t 是第j 個中間點算起的時間，而x·jk是在軌跡規(guī)劃過程中由方程（9）求出。 在過渡區(qū)段中，每個自由度的軌跡計算如下：

4 結(jié)束語

本文基于ABB 機器人Rapid 編程語言，以ABB 機器人實際應(yīng)用中的程序為例， 對6 軸機器人進行了運動學分析，進而對機器人軌跡規(guī)劃進行了研究與計算，為機器人應(yīng)用的軌跡規(guī)劃提供了理論依據(jù)。