徐源東, 朱 攀, 林 琳
(云南省機(jī)械研究設(shè)計(jì)院, 云南省機(jī)電一體化應(yīng)用技術(shù)重點(diǎn)實(shí)驗(yàn)室, 云南 昆明 650031)
機(jī)器人的根本任務(wù), 就是要實(shí)現(xiàn)從一個(gè)開始位置到一個(gè)目標(biāo)位置的運(yùn)動, 這就需要通過對復(fù)雜組合體進(jìn)行規(guī)劃來實(shí)現(xiàn)。 機(jī)器人的軌跡規(guī)劃(Trajectory Planning),就是要為機(jī)器人從初始位姿移動到目標(biāo)位姿, 通過指定一系列參數(shù),找出一條無碰撞且可行(連續(xù)平滑)的路徑,生成相關(guān)變量的時(shí)間律, 以描述期望的軌跡。 本文基于ABB 機(jī)器人Rapid 編程語言, 對機(jī)器人的運(yùn)動學(xué)進(jìn)行分析,進(jìn)行了機(jī)器人軌跡規(guī)劃的研究與計(jì)算。
軌跡規(guī)劃的基本問題是將操作臂從初始位置移動到最終期望位置——即將工具坐標(biāo)從當(dāng)前值{Tinitial}移動到期望值{Tfinal}。 在工業(yè)機(jī)器人應(yīng)用中,用工具坐標(biāo)系{T}相對于工件坐標(biāo)系{S}的運(yùn)動來描述。在ABB 機(jī)器人中表示為TCP(工具中心點(diǎn):帶方向的點(diǎn))相對于wobj(工件坐標(biāo)系)的運(yùn)動。
目前還沒有國際統(tǒng)一的機(jī)器人坐標(biāo)系標(biāo)準(zhǔn)命名,本文以ABB 機(jī)器人的命名方式為例, 將機(jī)器人運(yùn)動的相關(guān)坐標(biāo)系分為5 類坐標(biāo)系,用以描述機(jī)器人的相關(guān)運(yùn)動。 分別為:大地坐標(biāo)系(World coordinates)、基座標(biāo)系(Base coordinates)、 工具坐標(biāo)系 (Tool coordinates)、 用戶坐標(biāo)系(User coordinates)和工件坐標(biāo)系(Object coordinates)[1]。 各坐標(biāo)系關(guān)系見圖1。
圖1 機(jī)器人運(yùn)動相關(guān)坐標(biāo)系
1.2.1 ABB 機(jī)器人目標(biāo)位置定義
ABB 機(jī)器人RAPID 程序中,用兩種類型的數(shù)據(jù)描述機(jī)器人點(diǎn)位:
關(guān)節(jié)位置數(shù)據(jù)jointtarget 和TCP 位置數(shù)據(jù)robtarget。jointarget 記錄的是機(jī)器人各個(gè)軸的絕對偏移位置;robtarget 記錄的是機(jī)器人笛卡爾坐標(biāo)系下的xyz 坐標(biāo)及角度等數(shù)據(jù)。 兩種指令格式分別見圖2。
圖2 兩種類型的數(shù)據(jù)描述機(jī)器人點(diǎn)位
1.2.2 工具姿態(tài)描述
三維空間坐標(biāo)系的姿態(tài)方向可通過基準(zhǔn)坐標(biāo)系的旋轉(zhuǎn)來表示,姿態(tài)的表示方法有很多種,如旋轉(zhuǎn)矩陣(標(biāo)準(zhǔn)正交陣)、轉(zhuǎn)角排列設(shè)定法(X-Y-Z 固定角、Z-Y-X 歐拉角、Z-Y-Z 歐拉角)、軸角(等效角度-軸線)、四元數(shù)(歐拉參數(shù))等。 在ABB 機(jī)器人RAPID 編程語言中,以四元數(shù)(Quaternion)描述和儲存坐標(biāo)系姿態(tài)。在實(shí)際應(yīng)用項(xiàng)目中,常通過將四元數(shù)轉(zhuǎn)化為歐拉角應(yīng)用, 如視覺定位抓取項(xiàng)目中,獲取產(chǎn)品的偏移角度后,通過獲取標(biāo)定目標(biāo)點(diǎn)的歐拉角Z,對相應(yīng)值進(jìn)行計(jì)算后,對產(chǎn)品進(jìn)行旋轉(zhuǎn)定位抓取。
(1)Z-Y-X 歐拉角:
坐標(biāo)系{B}的歐拉角表示方法如下:
首先將坐標(biāo)系{B}和一個(gè)已知參考坐標(biāo)系{A}重合。先將{B}繞ZB^ 轉(zhuǎn)α 角,再繞Y^B轉(zhuǎn)β 角,最后繞X^B轉(zhuǎn)γ 角。這樣三個(gè)一組的旋轉(zhuǎn)被稱作歐拉角[2]。
圖3 Z-Y-X 歐拉角
(2)四元數(shù):
在ABB 機(jī)器人RAPID 編程語言中,以四元數(shù)(Quaternion)描述和儲存坐標(biāo)系姿態(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)坐標(biāo)系X’,Y’,Z’軸單位向量在基準(zhǔn)坐標(biāo)系X、Y、Z 軸上的投影。
D-H(Denavit-Hartenberg)表示法可用于表示任何機(jī)器人構(gòu)型,也可用于表示任何坐標(biāo)系中的變換,以及任何可能的關(guān)節(jié)和連桿組合。 對于6 軸垂直串聯(lián)機(jī)器人的正運(yùn)動學(xué), 可以通過D-H 參數(shù)轉(zhuǎn)化為各個(gè)軸的位姿矩陣,將6 個(gè)位姿矩陣右乘即可得到當(dāng)前機(jī)器人末端TRP 的笛卡爾坐標(biāo)。
機(jī)器人的正運(yùn)動學(xué)就是在已知機(jī)器人結(jié)構(gòu)參數(shù)的前提下,通過輸入各關(guān)節(jié)的角度(即jointtarget 數(shù)據(jù)),就能計(jì)算出當(dāng)前法蘭盤(TRP)在固定坐標(biāo)系(wobj)中的位置及姿態(tài), 即通過關(guān)節(jié)數(shù)據(jù)jointtarget 求笛卡爾坐標(biāo)數(shù)據(jù)robtarget。
圖4 六軸機(jī)器人控制模型與結(jié)構(gòu)參數(shù)
六軸機(jī)器人建模如上圖,建立Base 坐標(biāo)系(X0Y0Z0),之后依次對應(yīng)1 軸到6 軸6 個(gè)坐標(biāo)系(X1Y1Z1……X6Y6Z6),通過D-H 參數(shù)來表示這6 個(gè)動坐標(biāo)系相對于前一個(gè)坐標(biāo)系的關(guān)系(T1-T6):
對于D-H 參數(shù),旋轉(zhuǎn)和平移順序如下:先繞當(dāng)前X軸旋轉(zhuǎn)α 度,再沿新的X 軸平移a,再繞新的Z 軸旋轉(zhuǎn)θ度,再沿新的Z 軸平移d。
按上式計(jì)算整理后,可以得到基于D-H 參數(shù)的位姿矩陣如下:
品牌工業(yè)機(jī)器人的D-H 參數(shù)一般機(jī)器人生產(chǎn)廠家在系統(tǒng)參數(shù)中已做設(shè)置。 在ABB 機(jī)器人中,相關(guān)D-H 參數(shù)一般在系統(tǒng)模塊(System moudle)中定義,在安裝系統(tǒng)時(shí)已自動加載,不會受用戶作業(yè)刪除操作影響,可在DHParameter 內(nèi)查看相關(guān)值。
TRP 是確定工具作業(yè)點(diǎn)、設(shè)定工具數(shù)據(jù)的基準(zhǔn)位置。根據(jù)D-H 參數(shù)確定TRP 后,就可進(jìn)一步設(shè)置工業(yè)機(jī)器人的工具作業(yè)點(diǎn)TCP(Tool Control Point),它是機(jī)器人關(guān)節(jié)、直線、圓弧插補(bǔ)等移動指令的控制對象,指令中的起點(diǎn)、終點(diǎn)就是TCP, 在指定坐標(biāo)系上的位置值TCP 的位置與工具形狀、安裝方式密切相關(guān)。
在ABB 機(jī)器人的RAPID 編程語言中, 可直接使用CalcRobt 函數(shù),將關(guān)節(jié)數(shù)據(jù)jointtarget 直接轉(zhuǎn)換為笛卡爾坐標(biāo)系數(shù)據(jù)robtarget,示例如下:
CalcRobT(j_ temp,tool1WObj:=wobj0)
逆運(yùn)動學(xué)問題是由給定的末端執(zhí)行器位置和姿態(tài),求解一系列滿足期望要求的關(guān)節(jié)角。 逆運(yùn)動學(xué)可分解為兩部分:①進(jìn)行坐標(biāo)系變換求出相對于基座標(biāo)系{B}的腕部坐標(biāo)系{W};②應(yīng)用逆運(yùn)動學(xué)求關(guān)節(jié)角。
在ABB 機(jī)器人RAPID 編程語言中,即已知robtarget笛卡爾坐標(biāo)系的偏移值tran.(x,y,z), 和姿態(tài)rot.(q1,q2,q3,q4),根據(jù)已有D-H 參數(shù),求解關(guān)節(jié)偏移jointtarget 的解。目的是將分配給末端執(zhí)行器在操作空間的運(yùn)動,變換為相應(yīng)的關(guān)節(jié)空間的運(yùn)動,使得期望的運(yùn)動能執(zhí)行。
逆運(yùn)動學(xué)相對于正運(yùn)動學(xué)需研究以下問題:
(1)解的存在性:工作空間是機(jī)器人末端執(zhí)行器所能到達(dá)的范圍。 若解存在,則目標(biāo)點(diǎn)必須在工作空間內(nèi)。 常規(guī)品牌機(jī)器人在主要參數(shù)中會標(biāo)注機(jī)器人工作半徑,見圖5。
圖5 不同結(jié)構(gòu)類型的機(jī)器人的工作空間
(2)多重解問題:多重解的問題不僅取決于自由度的數(shù)量,還取決于非零D-H 參數(shù)的個(gè)數(shù)。 通常,連桿的非零參數(shù)越多,可行解的越多。機(jī)械的關(guān)節(jié)限制有可能會最終減少在實(shí)際結(jié)構(gòu)中多重可行解的數(shù)量[3]。
圖6 為ABB 公司IRB14000 機(jī)器人, 單臂有7 個(gè)自由度,對于同一個(gè)目標(biāo)位置點(diǎn)Target_960,共有5 種關(guān)節(jié)變量組合可到達(dá)目標(biāo)位置。
在ABB 機(jī)器人的RAPID 編程語言中, 可直接使用CalcJointT 函數(shù),將笛卡爾坐標(biāo)系數(shù)據(jù)robtarget 直接轉(zhuǎn)換為關(guān)節(jié)數(shù)據(jù)jointtarget,示例如下:
圖6 IRB14000 的右臂對于同一目標(biāo)點(diǎn)Target_960 的不同軸配置
CalcJointT(Target_10,tool1WObj:=wobj0)
路徑與軌跡規(guī)劃與受到控制的機(jī)器人從一個(gè)位置移動到另一個(gè)位置的方法有關(guān)。 用來指定并規(guī)劃路徑的方法有很多種。 任何在規(guī)定的時(shí)間里通過中間點(diǎn)的光滑函數(shù)都可以用來指定精確的路徑形狀。
路徑 (Path)表示在關(guān)節(jié)空間或操作空間中,機(jī)械手在執(zhí)行指定運(yùn)動時(shí)必須跟隨的點(diǎn)的構(gòu)型序列,是純幾何描述。 軌跡(Trajectory)是一條指定了時(shí)間律的路徑[4]。
軌跡規(guī)劃算法的輸入包括路徑描述、 路徑約束以及由機(jī)械手動力學(xué)施加的約束, 其輸出是按時(shí)間順序給出的位置、 速度和加速度的值構(gòu)成的末端執(zhí)行器 (TCP)軌跡。 軌跡規(guī)劃算法生成一個(gè)描述末端執(zhí)行器位置和方向變量依照約束隨時(shí)間變化的時(shí)間序列。 在軌跡生成的運(yùn)行時(shí)間內(nèi)需要計(jì)算位置、速度和加速度。
圖7 機(jī)器人在路徑上的依次運(yùn)動
當(dāng)以關(guān)節(jié)轉(zhuǎn)角的函數(shù)來描述軌跡在空間和時(shí)間的軌跡生成時(shí),通常將每個(gè)路徑點(diǎn)用工具坐標(biāo){T}相對于固定坐標(biāo)系{S}的期望位置和姿態(tài)來進(jìn)行描述。
3.2.1 高次多項(xiàng)式
在一定時(shí)間將工具從初始位置移動到目標(biāo)位置,應(yīng)用逆運(yùn)動學(xué)可以解出對應(yīng)目標(biāo)位置和姿態(tài)的各個(gè)關(guān)節(jié)角。各關(guān)節(jié)初始位置已知,需要確定每個(gè)關(guān)節(jié)的運(yùn)動函數(shù),t0時(shí)刻為該關(guān)節(jié)的初始位置,tf時(shí)刻為該關(guān)節(jié)的期望目標(biāo)位置,用高次多項(xiàng)式作為路徑曲線段,要確定在路徑曲線段的起始點(diǎn)和終止點(diǎn)的位置、速度和加速度,采用一個(gè)五次多項(xiàng)式進(jìn)行插值:
這些約束條件確定了一個(gè)具有6 個(gè)方程和6 個(gè)未知數(shù)的線性方程組,其解為:
3.2.2 帶拋物線過渡的線性函數(shù)用于經(jīng)過中間點(diǎn)的路徑
在工業(yè)機(jī)器人實(shí)際應(yīng)用中, 從起始點(diǎn)到目標(biāo)點(diǎn)常會設(shè)置過渡點(diǎn),經(jīng)過過渡點(diǎn)時(shí),通常使用拋物線進(jìn)行過渡,保證運(yùn)動的平滑,減少震動,降低磨損。 在工業(yè)機(jī)器人應(yīng)用中拋物線轉(zhuǎn)角半徑大小叫平滑度。
如圖8, 在關(guān)節(jié)空間中位某個(gè)關(guān)節(jié)θ 的運(yùn)動指定了一組中間點(diǎn),每兩個(gè)中間點(diǎn)之間使用線性函數(shù)相連,而各中間點(diǎn)附近使用拋物線過渡。
用j、k、l 表示三個(gè)相鄰的路徑點(diǎn)。位于路徑點(diǎn)k 處的過渡區(qū)段的時(shí)間間隔為t_k。 位于點(diǎn)j 和k 之間的直線部分的時(shí)間間隔為tjk。點(diǎn)j 和k 之間總的時(shí)間間隔為tdjk。直線部分的速度為θ·jk, 而在點(diǎn)j 處過渡區(qū)段的加速度為θ··j,如圖8 所示。
已知所有的路徑點(diǎn)θk、期望時(shí)間區(qū)間tdjk以及每個(gè)路徑點(diǎn)處加速度的模值|θ··k|,則可計(jì)算出過渡區(qū)段的時(shí)間間隔tk。 對于內(nèi)部路徑點(diǎn),使用下列公式計(jì)算:
圖8 多段帶有過渡區(qū)段的直線路徑
對于第一個(gè)路徑段和最后一個(gè)路徑段, 整個(gè)過渡區(qū)的持續(xù)時(shí)間都必須計(jì)入路徑中, 所以對于第一個(gè)路徑段,使線性區(qū)段速度的兩個(gè)表達(dá)式相等來求解t1:
上式用于計(jì)算給定中間點(diǎn)及各個(gè)路徑段的持續(xù)時(shí)間,求出多段軌跡中各個(gè)過渡區(qū)段的時(shí)間和速度。系統(tǒng)使用各個(gè)關(guān)節(jié)默認(rèn)加速度值。
讓工具在相隔較遠(yuǎn)的中間點(diǎn)之間走直線運(yùn)動, 這種執(zhí)行路徑的模式稱作笛卡爾直線運(yùn)動。在笛卡爾運(yùn)動中,可以使用笛卡爾變量關(guān)于時(shí)間的任意函數(shù)來定義路徑。
基于笛卡爾空間的運(yùn)動規(guī)劃中, 組合成軌跡的函數(shù)都是描述笛卡爾變量的時(shí)間函數(shù), 每個(gè)路徑點(diǎn)都是由工具坐標(biāo)系相對于固定坐標(biāo)系的期望位置和姿態(tài)來確定。路徑點(diǎn)是由{T}相對于{S}來描述的,在運(yùn)行時(shí)以實(shí)時(shí)更新路徑的速度求出運(yùn)動學(xué)逆解,在笛卡爾空間生成路徑后,通過求解逆運(yùn)動學(xué)來計(jì)算出期望的關(guān)節(jié)角度。
機(jī)器人在實(shí)時(shí)運(yùn)行時(shí),路徑生成器不斷產(chǎn)生用θ、θ·和θ··構(gòu)造的軌跡,并且將相關(guān)信息傳輸?shù)綑C(jī)器人控制系統(tǒng)。
3.4.1 關(guān)節(jié)空間路徑生成
在直線區(qū)段,各個(gè)關(guān)節(jié)的軌跡計(jì)算如下:
3.4.2 笛卡爾空間路徑生成
使用符號x 表示笛卡爾位姿矢量的分量, 重寫式(14)和式(15)。 則在曲線的直線區(qū)段,x 中的每個(gè)自由度按下式計(jì)算:
t 是第j 個(gè)中間點(diǎn)算起的時(shí)間,而x·jk是在軌跡規(guī)劃過程中由方程(9)求出。 在過渡區(qū)段中,每個(gè)自由度的軌跡計(jì)算如下:
本文基于ABB 機(jī)器人Rapid 編程語言,以ABB 機(jī)器人實(shí)際應(yīng)用中的程序?yàn)槔?對6 軸機(jī)器人進(jìn)行了運(yùn)動學(xué)分析,進(jìn)而對機(jī)器人軌跡規(guī)劃進(jìn)行了研究與計(jì)算,為機(jī)器人應(yīng)用的軌跡規(guī)劃提供了理論依據(jù)。