雙機器人不同約束下的協(xié)調運動軌跡規(guī)劃方法的研究＊

蘇赫朋 苗鴻賓② 李夢虔 申光鵬 余 浪

（①中北大學機械工程學院，山西 太原 030051；②山西省深孔加工工程技術研究中心，山西 太原 030199）

在工業(yè)自動化程度不斷提高的今天，在復雜的生產(chǎn)環(huán)境下，單機器人難以滿足現(xiàn)實的需求，如大型搬運、孔軸部件的裝配、復雜表面的焊接等。與單機器人相比，雙機器人不僅要考慮工作空間和外部約束，還要考慮二者的運動協(xié)調關系，它們在進行配合作業(yè)時就建立了約束關系。

Saridis G N[1]提出了面向復雜環(huán)境的高層決策模式，這種模式以智能與精度為核心，將系統(tǒng)劃分為3 個層次，最低控制是低智能、高精度的，最高控制是高智能、低精度的。Kasagami F 等[2]提出了雙機協(xié)同焊接的控制方法，兩個機器人分別夾持工件和進行焊接，對夾持工件的機器人示教，焊接機器人根據(jù)運動學約束自動尋找并跟蹤焊接位置。該方法避免了多次示教引起誤差，縮短了準備時間。李洋等[3]構建了基于RRT 的雙機協(xié)作平臺，并對其進行了軌跡規(guī)劃。王雷[4]通過集中控制系統(tǒng)，使兩臺機器人在時間和空間上保持一致，并提出了兩種解決方案：點對點規(guī)劃和路徑規(guī)劃。孟石等[5]在Ke-Scope 平臺上，對兩個機器人的疊加和耦合運動模式進行了分析，并對兩個機器人的協(xié)同工作進行了程序設計和仿真。

本文以JLRB8-600 型機器人為研究對象，以協(xié)調搬運和協(xié)調寫字為例，分別研究緊約束和松約束運動，分析其運動學關系及空間約束關系，使用Matlab 進行實驗驗證。

1 機器人運動學建模

歐德吉JLRB8-600 型六自由度串聯(lián)機器人具有結構精巧、占地少以及重量輕的特點，其主要技術指標為：臂展長不大于1 000 mm，臂重42 kg，最大荷重8 kg，重復定位精度0.05 mm。機器人及其運動范圍如圖1 所示。

圖1 機器人及其運動范圍

機器人的運動學模型需滿足有完備性、參數(shù)極少化和連續(xù)性的條件[6]。所以，D-H 參數(shù)法被用于運動學模型的建立，只需要 θi（關節(jié)轉角）、di（連桿偏移）、ai-1（連桿長度）、 αi-1（連桿扭轉）4 個運動參量就能描述機械臂關節(jié)，其D-H 參數(shù)表見表1。

表1 D-H 參數(shù)表

2 雙機器人緊約束下的協(xié)調運動軌跡規(guī)劃

兩個機械臂末端執(zhí)行器在工作過程中位姿保持不變的情況稱為緊約束。兩個開鏈機器人通過緊約束形成了一個閉環(huán)運動鏈[7]。由底座、關節(jié)、末端、末端執(zhí)行器和工件組成。在緊約束中，兩個機器人不同坐標系之間的關系如圖2 所示，對機器人進行編號i，定義世界坐標系為主機器人i1的基坐標系，{Ri}為機器人i的基坐標系，{Ei}為機器人i的末端坐標系， {Ti}為機器人i的工具坐標系， {C}為工件的質心坐標系，為末端坐標系到基坐標系的變換矩陣，為末端坐標系到工具坐標系的變換矩陣，為工具坐標系到工件質心坐標系的變換矩陣。

圖2 緊約束中各坐標系之間的關系

聯(lián)合搬運的緊約束運動形成了閉環(huán)運動鏈：

分別右乘對應矩陣得到：

機器人i1的基坐標系到末端坐標系的變換矩陣可由等號右邊求得，逆運動學分析可獲得各關節(jié)的角位移。已知和，通過示教得出工件在機器人i1基坐標下的運動軌跡為

式（3）中都是常數(shù)矩陣，結合式（1）有：

左乘對應矩陣得：

機器人i1與i2的是隨時間變化的函數(shù)矩陣，式（5）中可根據(jù)解得，這樣就可以實現(xiàn)機器人i1與i2在時間上的同步。緊約束運動學原理如圖3 所示。

圖3 緊約束運動學原理

3 雙機器人松約束下的協(xié)調運動軌跡規(guī)劃

松約束運動中機器人末端的相對位姿因任務的不同而發(fā)生變化[8]。設世界坐標系與機器人i1基坐標系重合，即{R}={R1} ， {U}代表工件坐標系，將其定義在被i2末端執(zhí)行器夾持固定的工件上，是工具坐標系到工件坐標系的變換矩陣。松約束中各坐標系關系如圖4 所示。

圖4 松約束中各坐標系之間的關系

雙機器人松約束運動中，仍有一條閉環(huán)運動鏈，這條運動鏈從世界坐標系，即主機器人的基本坐標系出發(fā)，再將兩個機器人的身體分割成兩個部分，最后匯合在一起，形成一個閉環(huán)[9]。因此，可以推導出主從機之間的關系：

式（6）和式（7）是從基礎坐標到工具坐標的主、從機器人的運動鏈，可以得到以下關系：

4 仿真實驗分析

4.1 緊約束下的Matlab 雙機器人軌跡驗證

假定工件C 的尺寸為Cx×Cy×Cz（單位為mm），質心與工件坐標系 {C}原點重合，位于中心位置，Z軸沿機器人i1基坐標系X軸方向，X軸豎直向上，Y軸由右手法則確定，如圖5 所示。

圖5 工件尺寸和質心坐標系位置

圖6 顯示了在兩個機械手的緊約束中不同部分的坐標系信息。將任務目標設置為：機器人在兩邊搬運工件C，定義起點A，終點B，總長L，i1在左、i2在右，二者沿世界坐標系軸x相對立，搬運過程中，二者相對速度為零。得到工件C與世界坐標系的變換矩陣為

圖6 緊約束下各坐標系相對位置

在緊約束下，機器人i1與i2夾持工具相對于機器人末端的變換矩陣為

在搬運過程中，工件相對機器人i1與i2工具坐標系變換矩陣為

設置從機器人基坐標系相對于世界坐標系的位置為

根據(jù)式（2）可以得出i1末端相對于基坐標系的變換矩陣：

由于兩機器人末端的位姿不發(fā)生變化，由式（3）得：

根據(jù)（5）可得機器人i2末端到基坐標系的變換矩陣：

由式（14）和式（16），可得機器人i1與i2末端相對基坐標系的變換矩陣，再通過逆運動學求出各關節(jié)的角位移，從而實現(xiàn)機械臂的動作。

在 Matlab 的基礎上，將上述雙機器人協(xié)調搬運參數(shù)設定為：A=[400,250,200]、B=[400,-250,200]，工件C輪廓尺寸100 mm×100 mm×200 mm，工件質心與末端坐標系原點保持水平，有f2=f3=100 mm，起點為A，終點為B，i1與i2的工具有效長度為f1=f4=50 mm，設置f=1 000 mm。上面的數(shù)值被替換到式（14）～式（16）中仿真。

Matlab 因其良好的運算性能而被廣泛應用[10]。利用 Matlab 中的Robotic Toolbox 對兩個機器人在動畫層面上的動作進行了模擬和分析。該工具箱是澳洲學者Peter Corke 發(fā)明的，用來研究和模擬關節(jié)型和移動型的機器人，里面包含了一些關于機器人的基礎算法的函數(shù)，有一些比較成熟的代碼，可以很直觀地顯示出編寫的程序。

圖7 所示為用 Matlab 軟件實現(xiàn)的兩臺機械臂協(xié)同操作的模擬原理圖。

圖7 雙機器人緊約束運動仿真

紅線表示工件C 的軌跡，基座在世界坐標系沿軸線X相對放置，由此可以看出，兩臺機器人的末端X方向是相同的，軸線Y與軸線Z方向相反。

圖8 所示為主機器人的軌跡，此時動作已經(jīng)結束，末端執(zhí)行器的軌跡為直線，運動過程中末端位姿不發(fā)生變化，移動距離為500 mm 。圖9 所示為主機器人各關節(jié)的角位移。

圖8 末端運動軌跡

圖9 主機器人各關節(jié)角位移變化

對于機器人i2，基坐標原點位于[1000,0,0]，X軸與世界坐標系X軸方向相反，運動軌跡總長500 mm，i2的末端軌跡如圖10 所示，i2的各關節(jié)的角位移如圖11 所示。

圖10 末端運動軌跡

圖11 從機器人各關節(jié)角位移變化

仿真結果與預定參數(shù)一致，各關節(jié)運動相對平緩，無明顯拐點，機器人的動作平穩(wěn)順暢，驗證了該方法的正確性和有效性。

4.2 松約束下的Matlab 雙機器人軌跡驗證

在協(xié)同寫字過程中，使機器人i1夾持書寫板，機器人i2夾持書寫筆；寫字板變速移動，寫字筆書寫“123”字體，并隨著寫字板移動，如圖12 所示；定義工件坐標系 {U}的原點在寫字板中心，i2工具坐標系{T2}原點在寫字筆的頂端，圖13 和圖14 分別為各坐標系相對位置關系，仍按圖6 設置i1與i2的基坐標系。

圖12 書寫內容

圖13 主機器人末端、工具及工件坐標系之間的關系

圖14 從機器人末端、工具及工件坐標系之間的關系

i1的末端、工具和工件坐標系如圖13 所示。

i2的末端、工具和工件坐標系如圖14 所示。

設i1基坐標系與世界坐標系重合，在主基坐標系中，起點為A[Ax,Ay,Az]，終點為B[Bx,By,Bz]，總長l1。i1的運動路徑確定后，即可推導出i2的運動路徑及相關參數(shù)。

寫字的過程中，規(guī)定從左到右，從上到下，一筆寫完一個字，寫完后抬筆移到下一個位置。每個字的起始位置為左上角，寫字筆的理想路徑如圖15 所示。設抬筆高度為d，計算得寫字總長度l2為

圖15 寫字筆寫字順序

以式（8）和式（9）為基礎，得到i1與i2末端相對基坐標的關系等式。從圖4 中可以看出，在松約束下中，i1工具坐標系、基坐標系分別與工件坐標系之間的變換矩陣：

?ai(i=x,y,z)是寫字板移動過程中與寫字筆所需的距離，該數(shù)值有正負之分。

i1末端與工具坐標系的關系矩陣：

由式（8）、式（18）、式（19）和式（20）可得i1末端相對基坐標系的變換矩陣，逆運動學求出各關節(jié)角位移，使機器人到達相應位置：

i2末端與寫字筆工具坐標系之間的關系：

寫字筆工具坐標系與寫字板工件坐標系的關系為

?l為寫字筆在軸上的移動距離，[pi,qi,wi]是工件坐標系相對工具坐標在某一時刻的位置，與書寫內容和順序有關。由圖15 可以看出，完成這一任務共需18 筆，而“3”中只有1 次重復。因此可以得出筆的書寫過程是由18 個點來控制的結論，也就是說i=2,3,4,···,i≥18，i=1時是起始位置，是i2工具坐標系中書寫開始點。

假設世界坐標系與i1基坐標系重合，那么i2與世界坐標系的關系就可以確定：

據(jù)式（8）、式（19）、式（22）、式（23）、式（24），i2末端相對基坐標系的變換矩陣為

假設各參數(shù)為

在Matlab Robotic Toolbox 中建立機器人模型進行仿真，結果如圖16 所示。

圖16 松約束運動仿真結果

圖16 所示為兩個機器人的初始位置，i1在左側，基坐標系與世界坐標系一致，i2在右側，基坐標系在世界坐標系中[1 000,0,0]。寫字板的軌跡如圖17 所示。

圖17 主機器人的運動軌跡

寫字板作直線運動，從[500,500,600] 到[500,-500,600]，運動過程中，i1各關節(jié)的角位移變化如圖18 所示。

圖18 主機器人各關節(jié)角位移變化

從圖中可以看到，主機器人寫字板是直線運動，各關節(jié)角的變化較小。i2工具坐標系原點在筆尖，仿真結果如圖19 所示。當i1運動時，寫字筆會寫下任務要求的文字，運動軌跡如圖20 所示。粗線是書寫軌跡，細線是起筆、落筆、移動的軌跡，寫字筆沿著世界坐標系y軸正向移動。“2”和“3”中缺少的“—”，可由寫字板沿著y軸正方向移動寫出對。寫出的標準字跡如圖21 所示，能夠看到，書寫過程非常順暢，每一次抬筆落筆都能精確地移動到相應位置，i2的各個關節(jié)的角位移如圖22 所示。

圖19 從機器人的運動軌跡

圖20 世界坐標系下筆的軌跡

圖21 寫字板上的筆記

圖22 從機器人各關節(jié)角位移變化

由圖18 可知，i1各關節(jié)角位移光滑平坦，這是由于i1的運動只有y軸上的平移，不需要關節(jié)角發(fā)生快速改變。由圖22 可知，i2各關節(jié)角的變化比較復雜，這是因為為了書寫所需的筆跡，機器人需要在空間中頻繁移動，但曲線上整體沒有突出拐點，關節(jié)的變化相對平穩(wěn)。兩機器人的運動都是可以實現(xiàn)的，在運動過程中每個關節(jié)的變形都是平緩的。通過以上實驗，證明了該方法的正確性及有效性。

5 結語

雙機器人之間的協(xié)同運動，最重要的就是它們的軌跡是否符合任務要求。本文的研究內容分為緊約束搬運任務和松約束書寫任務，對運動軌跡規(guī)劃的理論方法進行了分析設計，用 Matlab 測試了兩種方法的正確性及穩(wěn)定性。實驗證明，兩臺機械臂均能很好地完成提前設定的任務，并且各關節(jié)角位移變化平緩、沒有突兀的拐點，這對于研究兩臺機械臂的協(xié)同控制有一定的理論價值。