基于OpenHRP的多機(jī)器人協(xié)作運(yùn)動(dòng)規(guī)劃與仿真

萬(wàn)鸞飛, 李中望, 陳慧蓉

(蕪湖職業(yè)技術(shù)學(xué)院 電氣工程學(xué)院，安徽 蕪湖 241006)

多機(jī)器人系統(tǒng)的“協(xié)作”分為程序協(xié)作和運(yùn)動(dòng)協(xié)作兩大類(lèi)別.程序協(xié)作僅要求某段軌跡開(kāi)始的時(shí)刻相同；運(yùn)動(dòng)協(xié)作要求運(yùn)動(dòng)某段軌跡的時(shí)間相同，即同時(shí)開(kāi)始和同時(shí)結(jié)束運(yùn)動(dòng).

目前多機(jī)器人系統(tǒng)基本都采用了主從式的控制方式，將參與協(xié)作的機(jī)器人分為主機(jī)器人和從機(jī)器人兩類(lèi).主機(jī)器人的運(yùn)動(dòng)獨(dú)立，從機(jī)器人的運(yùn)動(dòng)由主控制器控制，在運(yùn)動(dòng)時(shí)間、末端執(zhí)行器位姿與主機(jī)器人保持對(duì)應(yīng)關(guān)系.比如在多機(jī)器人焊接系統(tǒng)中，通常選取搬運(yùn)機(jī)器人為主機(jī)器人，焊接機(jī)器人為從機(jī)器人[1-2]，焊接機(jī)器人的運(yùn)動(dòng)要配合搬運(yùn)機(jī)器人的運(yùn)動(dòng)以保證滿足焊接的要求.

1 多機(jī)器人協(xié)作運(yùn)動(dòng)學(xué)約束關(guān)系

1.1 并發(fā)協(xié)作的運(yùn)動(dòng)學(xué)約束關(guān)系[3]

并發(fā)協(xié)作過(guò)程中只要求各機(jī)器人的運(yùn)動(dòng)開(kāi)始時(shí)刻相同，機(jī)器人末端執(zhí)行器沒(méi)有約束關(guān)系.因此，并發(fā)協(xié)作過(guò)程中機(jī)器人末端執(zhí)行器約束關(guān)系不存在.

圖1 雙機(jī)器人協(xié)作搬運(yùn)系統(tǒng)示意圖

1.2 耦合運(yùn)動(dòng)的運(yùn)動(dòng)學(xué)約束關(guān)系

耦合協(xié)作運(yùn)動(dòng)過(guò)程要求從機(jī)器人與主機(jī)器人具有相同形式的運(yùn)動(dòng)，主從機(jī)器人的末端執(zhí)行器在整個(gè)協(xié)作過(guò)程中保持固定不變.多機(jī)器人耦合協(xié)作運(yùn)動(dòng)的示例如圖1所示，該類(lèi)協(xié)作運(yùn)動(dòng)主要應(yīng)用于負(fù)荷分擔(dān)的任務(wù)，如協(xié)作搬運(yùn)等.

考慮到耦合協(xié)作過(guò)程中，主從機(jī)器人的末端執(zhí)行器的位姿保持不變，可用一個(gè)常量變換矩陣來(lái)表示.

假設(shè)齊次矩陣mbPm(t)∈R4×4是主機(jī)器人末端在主機(jī)器人基坐標(biāo)系下t時(shí)刻的齊次變換矩陣，sbPs(t)∈R4×4是從機(jī)器人末端在從機(jī)器人基坐標(biāo)系下t時(shí)刻的齊次變換矩陣.假定從機(jī)器人末端在主機(jī)器人基坐標(biāo)系下的齊次變換矩陣為mbPs(t)∈R4×4，則該矩陣可表示為式(1).

mbPs(t)=mbHsb·sbPs(t)

(1)

其中,mbHsb∈R4×4是從機(jī)器人基坐標(biāo)系到主機(jī)器人基坐標(biāo)系的齊次變換矩陣.此變換矩陣在當(dāng)兩個(gè)機(jī)器人被安裝之后位置固定，因此mbHsb在整個(gè)過(guò)程中是一個(gè)恒定的齊次變換矩陣.設(shè)定t0作為mbPm(t)和sbPs(t)共同的起始時(shí)間，并設(shè)定mHs∈R4×4為在主機(jī)器人基坐標(biāo)系下由從機(jī)器人末端到主機(jī)器人末端在t0時(shí)刻的變換矩陣，即得

mbPm(0)=mHs·mbPs(0)

(2)

將式(1)代入式(2)在t=t0得到

mbPm(0)=mHs·mbHsb·sbPs(0)

(3)

由式(3)可得

(4)

根據(jù)耦合運(yùn)動(dòng)的定義得知mHs在整個(gè)耦合運(yùn)動(dòng)中是一個(gè)常量矩陣，由此可得

(5)

由式(5)可得

(6)

式(6)表示在耦合協(xié)作運(yùn)動(dòng)過(guò)程中主從機(jī)器人末端位姿之間的約束關(guān)系，根據(jù)式(6)可知當(dāng)兩個(gè)機(jī)器人的起始位姿都確定后，耦合運(yùn)動(dòng)過(guò)程中從機(jī)器人的末端位姿sbPs(t)完全取決于主機(jī)器人的末端位姿mbPm(t)，式(6)為耦合運(yùn)動(dòng)軌跡規(guī)劃建立了理論基礎(chǔ).

2 多機(jī)器人協(xié)作規(guī)劃算法

2.1 主從機(jī)器人單獨(dú)軌跡規(guī)劃算法

2.1.1 主機(jī)器人軌跡規(guī)劃

主機(jī)器人軌跡規(guī)劃包括直線和圓弧規(guī)劃，直線規(guī)劃通過(guò)LineInterpolatePoints函數(shù)實(shí)現(xiàn)，圓弧規(guī)劃通過(guò)ArcInterpolatePoints函數(shù)實(shí)現(xiàn)，主機(jī)器人的直線和圓弧的測(cè)試程序如圖2所示.

程序中o表示搬運(yùn)機(jī)器人的初始位姿，圓弧運(yùn)動(dòng)的起點(diǎn)、中點(diǎn)和終點(diǎn)分別是a、b、c.機(jī)器人運(yùn)動(dòng)軌跡為一段直線、一段圓弧，最后從圓弧的終點(diǎn)回到初始位置.整個(gè)運(yùn)動(dòng)過(guò)程中，機(jī)器人的姿態(tài)不發(fā)生變化.

2.1.2 從機(jī)器人軌跡規(guī)劃

從機(jī)器人軌跡規(guī)劃包括直線和圓弧規(guī)劃，實(shí)現(xiàn)函數(shù)與上述一致，測(cè)試程序如圖3所示.

圖2 主機(jī)器人直線-圓弧測(cè)試程序

圖3 從機(jī)器人直線-圓弧測(cè)試程序

2.2 耦合運(yùn)動(dòng)算法

2.2.1 直線耦合運(yùn)動(dòng)

直線耦合運(yùn)動(dòng)是指具有協(xié)作關(guān)系的機(jī)器人在某一時(shí)刻同時(shí)開(kāi)始直線形式的運(yùn)動(dòng)，運(yùn)動(dòng)過(guò)程中保持各機(jī)器人末端位姿關(guān)系不變[4-5]，依靠直線耦合運(yùn)動(dòng)函數(shù)CoupleMotionL[6]實(shí)現(xiàn)，測(cè)試程序如圖4所示.

圖4 直線耦合運(yùn)動(dòng)測(cè)試程序

以雙機(jī)器人進(jìn)行直線耦合為例，已知主機(jī)器人基座標(biāo)系與從機(jī)器人的相對(duì)位姿，主機(jī)器人的起點(diǎn)和終點(diǎn)，程序根據(jù)直線耦合運(yùn)動(dòng)原理從機(jī)器人的起點(diǎn)可計(jì)算出從機(jī)器人相對(duì)于自身基座標(biāo)系的運(yùn)動(dòng)軌跡.

2.2.2 圓弧耦合運(yùn)動(dòng)

圓弧耦合運(yùn)動(dòng)是指具有協(xié)作關(guān)系的機(jī)器人在某一時(shí)刻同時(shí)開(kāi)始圓弧形式的運(yùn)動(dòng)[7-8]，通過(guò)圓弧耦合運(yùn)動(dòng)函數(shù)CoupleMotionC實(shí)現(xiàn)，測(cè)試程序如圖5所示.

圖5 圓弧耦合運(yùn)動(dòng)測(cè)試程序

規(guī)劃圓弧耦合運(yùn)動(dòng)時(shí)，給出主機(jī)器人與從機(jī)器人的相對(duì)位姿，主機(jī)器人圓弧的起點(diǎn)、中點(diǎn)、終點(diǎn)和速度，只需給出從機(jī)器人圓弧運(yùn)動(dòng)的起點(diǎn)，從機(jī)器人具體的運(yùn)動(dòng)軌跡和速度均可通過(guò)耦合關(guān)系計(jì)算得出.

3 多機(jī)器人協(xié)作仿真功能驗(yàn)證

3.1 直線耦合運(yùn)動(dòng)三維仿真

在規(guī)劃直線耦合運(yùn)動(dòng)的仿真環(huán)境中，主機(jī)器人末端安裝抓手工具—從機(jī)器人手持焊槍.機(jī)器人初始位置行走至直線耦合運(yùn)動(dòng)起點(diǎn)，協(xié)作機(jī)器人耦合運(yùn)動(dòng)一段直線，直線耦合運(yùn)動(dòng)結(jié)束后，機(jī)器人回到各自的初始位置.仿真圖如圖6所示.

圖6 直線耦合運(yùn)動(dòng)三維仿真圖

3.2 圓弧耦合運(yùn)動(dòng)三維仿真

在規(guī)劃圓弧耦合運(yùn)動(dòng)的仿真環(huán)境中，主機(jī)器人末端安裝抓手工具—從機(jī)器人手持焊槍.機(jī)器人均從初始位置行走至圓弧耦合運(yùn)動(dòng)起點(diǎn)，協(xié)作機(jī)器人耦合運(yùn)動(dòng)一段圓弧，圓弧耦合運(yùn)動(dòng)結(jié)束后，機(jī)器人回到各自的初始位置.具體的仿真圖如圖7所示.

圖7 圓弧耦合運(yùn)動(dòng)三維仿真圖

4 總結(jié)

本文是以主從機(jī)器人單獨(dú)軌跡規(guī)劃算法和耦合運(yùn)動(dòng)算法為依據(jù)，進(jìn)行主機(jī)器人和從機(jī)器人直線軌跡規(guī)劃和圓弧軌跡規(guī)劃.以雙機(jī)器人協(xié)作耦合展開(kāi)實(shí)驗(yàn)，通過(guò)仿真實(shí)驗(yàn)，得出多機(jī)器人協(xié)作直線耦合和圓弧耦合運(yùn)動(dòng)三維仿真實(shí)例，確保了雙機(jī)器人協(xié)作加工路徑規(guī)劃的科學(xué)性.

[1] 甘亞輝.柔性化焊接系統(tǒng)中的多機(jī)器人協(xié)作控制研究[D].南京：東南大學(xué)，2014.

[2] 孟石,戴先中,甘亞輝.多機(jī)器人協(xié)作系統(tǒng)軌跡約束關(guān)系分析及示教方法[J].機(jī)器人,2012,34(5)：546-552，565.

[3] 梅樂(lè).基于多智能體的多機(jī)器人跟蹤控制研究[D].合肥：合肥工業(yè)大學(xué),2016.

[4] 阮志虎.兩輪差速驅(qū)動(dòng)移動(dòng)機(jī)器人的運(yùn)動(dòng)模型與控制研究[D].重慶：重慶大學(xué),2015.

[5] 張穎.多移動(dòng)機(jī)器人編隊(duì)導(dǎo)航的雙邊遙操作方法研究[D].南京：東南大學(xué),2016.

[6] 陳宇航,趙鐵石,宋曉鑫,等.變結(jié)構(gòu)并聯(lián)多維振動(dòng)臺(tái)自由度特性與運(yùn)動(dòng)學(xué)分析[J].機(jī)器人,2016,38(2)：135-143.

[7] 高煥兵.帶電搶修作業(yè)機(jī)器人運(yùn)動(dòng)分析與控制方法研究[D].濟(jì)南：山東大學(xué),2015.

[8] 馬勇.多移動(dòng)機(jī)器人路徑規(guī)劃研究[D].武漢：華中科技大學(xué),2012.