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

徐源東， 朱 攀， 林 琳

（云南省機械研究設計院， 云南省機電一體化應用技術重點實驗室， 云南 昆明 650031）

0 引言

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

1 機器人位置與姿態

1.1 機器人坐標系

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

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

圖1 機器人運動相關坐標系

1.2 機器人目標點位數據與工具姿態

1.2.1 ABB 機器人目標位置定義

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

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

圖2 兩種類型的數據描述機器人點位

1.2.2 工具姿態描述

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

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

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

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

圖3 Z-Y-X 歐拉角

（2）四元數：

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

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

2 機器人運動學分析

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

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

機器人的正運動學就是在已知機器人結構參數的前提下，通過輸入各關節的角度（即jointtarget 數據），就能計算出當前法蘭盤（TRP）在固定坐標系（wobj）中的位置及姿態， 即通過關節數據jointtarget 求笛卡爾坐標數據robtarget。

圖4 六軸機器人控制模型與結構參數

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

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

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

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

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

在ABB 機器人的RAPID 編程語言中， 可直接使用CalcRobt 函數，將關節數據jointtarget 直接轉換為笛卡爾坐標系數據robtarget，示例如下：

CalcRobT（j_ temp，tool1WObj：=wobj0）

2.2 機器人逆運動學

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

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

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

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

圖5 不同結構類型的機器人的工作空間

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

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

在ABB 機器人的RAPID 編程語言中， 可直接使用CalcJointT 函數，將笛卡爾坐標系數據robtarget 直接轉換為關節數據jointtarget，示例如下：

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

CalcJointT（Target_10，tool1WObj：=wobj0）

3 軌跡規劃

3.1 軌跡規劃綜述

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

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

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

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

3.2 關節空間的規劃方法——MoveJ

當以關節轉角的函數來描述軌跡在空間和時間的軌跡生成時，通常將每個路徑點用工具坐標{T}相對于固定坐標系{S}的期望位置和姿態來進行描述。

3.2.1 高次多項式

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

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

3.2.2 帶拋物線過渡的線性函數用于經過中間點的路徑

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

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

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

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

圖8 多段帶有過渡區段的直線路徑

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

上式用于計算給定中間點及各個路徑段的持續時間，求出多段軌跡中各個過渡區段的時間和速度。系統使用各個關節默認加速度值。

3.3 笛卡爾空間的規劃方法——MoveL

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

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

3.4 路徑的實時生成

機器人在實時運行時，路徑生成器不斷產生用θ、θ·和θ··構造的軌跡，并且將相關信息傳輸到機器人控制系統。

3.4.1 關節空間路徑生成

在直線區段，各個關節的軌跡計算如下：

3.4.2 笛卡爾空間路徑生成

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

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

4 結束語

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