王耀武，張得禮，胡雪剛

(南京航空航天大學(xué) 機(jī)電學(xué)院，江蘇 南京 210016)

0 引言

雙機(jī)器人運(yùn)動(dòng)學(xué)協(xié)調(diào)分為相對(duì)協(xié)調(diào)和同步協(xié)調(diào)[1]。在相對(duì)協(xié)調(diào)任務(wù)中，雙機(jī)器人末端工具的相對(duì)位姿發(fā)生變化，比如雙機(jī)器人協(xié)調(diào)完成工件的噴涂任務(wù)；在同步協(xié)調(diào)任務(wù)中，雙機(jī)器人末端工具的相對(duì)位姿不發(fā)生變化，比如雙機(jī)器人協(xié)調(diào)搬運(yùn)。在同步協(xié)調(diào)任務(wù)中，典型的如搬運(yùn)，由于機(jī)器人自身的定位精度誤差、雙機(jī)器人之間坐標(biāo)系標(biāo)定的誤差、工件的尺寸誤差等，若雙機(jī)器人是在位置控制模式下進(jìn)行任務(wù)，則不可避免地會(huì)對(duì)物體施加很大的內(nèi)力，嚴(yán)重時(shí)會(huì)對(duì)物體和機(jī)器人造成損壞[2]。因此需要對(duì)物體上的內(nèi)力進(jìn)行控制。

目前雙機(jī)器人協(xié)調(diào)內(nèi)力控制方法主要如下：文獻(xiàn)[2]針對(duì)雙機(jī)器人搬運(yùn)作業(yè)中的內(nèi)力控制問(wèn)題，提出基于遺傳算法的阻尼比例微分控制方法，但是參數(shù)調(diào)整和擇優(yōu)過(guò)程繁瑣。文獻(xiàn)[3]采用變阻抗模型對(duì)兩個(gè)機(jī)器人對(duì)物體施加的力進(jìn)行調(diào)整，同時(shí)克服外界的擾動(dòng)力實(shí)現(xiàn)機(jī)器人對(duì)外界的柔順。文獻(xiàn)[4]基于雙機(jī)器人對(duì)等架構(gòu)采用阻抗控制同時(shí)調(diào)整雙機(jī)器人和物體之間的接觸力。文獻(xiàn)[5]采用位置/力混合控制，同時(shí)控制兩個(gè)機(jī)器人和物體之間的接觸力。上述研究都是同時(shí)調(diào)整兩個(gè)機(jī)器人對(duì)物體的接觸力，可能會(huì)產(chǎn)生新的內(nèi)力，而且文獻(xiàn)[5]中位置力/混合控制不適用大部分只開(kāi)放位置控制權(quán)限的機(jī)器人。文獻(xiàn)[6]通過(guò)模型參考自適應(yīng)算法來(lái)調(diào)整從機(jī)器人的運(yùn)動(dòng)軌跡進(jìn)而控制內(nèi)力，但是模型數(shù)學(xué)運(yùn)算不利于實(shí)現(xiàn)。文獻(xiàn)[7]提出了面向被操作對(duì)象的協(xié)調(diào)策略，并對(duì)作用于被操作對(duì)象的合力進(jìn)行力分解。文獻(xiàn)[8]在此基礎(chǔ)上提出了面向被操作對(duì)象的內(nèi)外阻抗控制策略。雖然上述研究考慮了雙臂對(duì)物體的內(nèi)力和外力控制，但是都采用定阻抗的控制策略，魯棒性不強(qiáng)。除此之外，其他的智能控制策略如文獻(xiàn)[9-10]也用到了協(xié)調(diào)控制中，這種模型有過(guò)于復(fù)雜、強(qiáng)耦合、非線性等缺點(diǎn)，并未在現(xiàn)有的控制系統(tǒng)中得以應(yīng)用。

針對(duì)上述雙機(jī)器人協(xié)調(diào)內(nèi)力控制方法存在的問(wèn)題，本文在雙機(jī)器人主從協(xié)調(diào)架構(gòu)和所介紹的內(nèi)力計(jì)算模型的基礎(chǔ)上，提出了基于自適應(yīng)阻抗模型的從機(jī)器人單端位置調(diào)節(jié)內(nèi)力控制策略。由于雙機(jī)器人作用于物體上的內(nèi)力是一對(duì)相互作用力，在搬運(yùn)過(guò)程中，規(guī)劃的主機(jī)器人的離線軌跡不變，從機(jī)器人根據(jù)其對(duì)物體的內(nèi)力大小來(lái)調(diào)整自己的位置?；谏鲜霾呗杂行У亟鉀Q了雙機(jī)器人協(xié)調(diào)搬運(yùn)過(guò)程中的內(nèi)力控制問(wèn)題。

1 運(yùn)動(dòng)學(xué)分析和內(nèi)力建模

如圖1所示，Tw表示雙機(jī)器人系統(tǒng)的世界坐標(biāo)系；Tb1表示主機(jī)器人的基坐標(biāo)系，Tw和Tb1重合；Tb2表示從機(jī)器人的基坐標(biāo)系；Te1表示主機(jī)器人和物體接觸點(diǎn)處的坐標(biāo)系，Te2表示從機(jī)器人和物體接觸點(diǎn)處的坐標(biāo)系；o表示物體的質(zhì)心；To表示物體質(zhì)心處的坐標(biāo)系；b1Te1表示Te1相對(duì)Tb1的表達(dá)，b2Te2表示Te2相對(duì)Tb2的表達(dá)；b1Tb2表示Tb2相對(duì)Tb1的表達(dá)；e2Te1表示Te1相對(duì)Te2的表達(dá)；l1表示主機(jī)器人和物體的接觸點(diǎn)到物體質(zhì)心的矢量，l2表示從機(jī)器人和物體的接觸點(diǎn)到物體質(zhì)心的矢量；Fe1、Ne1表示主機(jī)器人在接觸點(diǎn)處對(duì)物體施加的力在Te1中的表達(dá)；Fe2、Ne2表示從機(jī)器人在接觸點(diǎn)處對(duì)物體施加的力在Te2中的表達(dá)；Fo1、No1表示主機(jī)器人在接觸點(diǎn)處對(duì)物體施加的力在To中的表達(dá)；Fo2、No2表示從機(jī)器人在接觸點(diǎn)處對(duì)物體施加的力在To中的表達(dá)。

圖1 雙機(jī)器人協(xié)調(diào)運(yùn)動(dòng)學(xué)和靜力學(xué)關(guān)系

由圖1中雙機(jī)器人主從協(xié)調(diào)搬運(yùn)的運(yùn)動(dòng)學(xué)閉環(huán)鏈[11],可得公式(1)。由公式(1)可知，只要知道b1Tb2、b1Te1、e2Te1，就可以知道b2Te2，即主從協(xié)調(diào)是從機(jī)器人跟隨主機(jī)器人的運(yùn)動(dòng)而運(yùn)動(dòng)。

b2Te2=(b1Tb2)-1b1Te1(e2Te1)-1

(1)

(2)

(3)

式中：oRe1表示Te1相對(duì)于To的旋轉(zhuǎn)矩陣；S(l1)表示對(duì)l1的叉乘運(yùn)算，其結(jié)果為3×3的矩陣；oRe2表示Te2相對(duì)于To的旋轉(zhuǎn)矩陣；S(l2)表示對(duì)l2的叉乘運(yùn)算，其結(jié)果為3×3 的矩陣。物體所受的內(nèi)力[12]如公式(4)所示。

(4)

式中：fi表示物體所受的內(nèi)力，其是一個(gè)12維的矢量；fi1表示主機(jī)器人對(duì)物體的內(nèi)力，fi2表示從機(jī)器人對(duì)物體的內(nèi)力，都是6維的矢量。fi1和fi2是一對(duì)相互作用力，它們都是相對(duì)于To的。

2 內(nèi)力控制策略

2.1 自適應(yīng)阻抗模型

以某一維度力控制為例，自適應(yīng)阻抗模型[13]的公式表達(dá)式如式(5)所示。

(5)

自適應(yīng)阻抗模型的控制框圖如圖2所示，除相關(guān)字符含義上已說(shuō)明外，Ke表示環(huán)境剛度，x表示機(jī)器人實(shí)際跟蹤到的位置，一般認(rèn)為xc=x。

圖2 自適應(yīng)阻抗阻抗模型控制框圖

2.2 從機(jī)器人單端調(diào)節(jié)內(nèi)力控制策略

內(nèi)力控制策略如圖3所示。xd1表示主機(jī)器人規(guī)劃的期望軌跡，xd2表示從機(jī)器人規(guī)劃的期望軌跡；x1表示主機(jī)器人的實(shí)際軌跡，x2表示從機(jī)器人的實(shí)際軌跡；xc2表示修改后的從機(jī)器人的實(shí)際軌跡；Fe1表示主機(jī)器人和物體的接觸力，F(xiàn)e2表示從機(jī)器人和物體的接觸力；oFi2表示從機(jī)器人對(duì)物體的內(nèi)力在物體質(zhì)心坐標(biāo)系To中的表達(dá)；oΔx2表示從機(jī)器人的位置修正量在To中的表達(dá)，Δx2表示oΔx2經(jīng)坐標(biāo)系變換到從機(jī)器人末端的位置修正量；oFd表示從機(jī)器人對(duì)物體的期望內(nèi)力在物體質(zhì)心坐標(biāo)系To中的表達(dá)。

圖3 內(nèi)力控制策略框圖

由上文內(nèi)力模型可知，雙機(jī)器人作用于物體的內(nèi)力是一對(duì)相互作用力，因此，控制一個(gè)機(jī)器人對(duì)物體的作用力即可。在雙機(jī)器人搬運(yùn)過(guò)程中，根據(jù)雙機(jī)器人對(duì)物體的接觸力可實(shí)時(shí)計(jì)算出從機(jī)器人對(duì)物體的內(nèi)力，根據(jù)期望內(nèi)力oFd和上文的自適應(yīng)阻抗模型可實(shí)時(shí)計(jì)算出從機(jī)器人的位置修正量Δx2來(lái)實(shí)時(shí)修改從機(jī)器人的期望軌跡,從而使控制內(nèi)力控制在期望水平上。

3 仿真和實(shí)驗(yàn)

3.1 仿真和實(shí)驗(yàn)?zāi)Ｐ偷慕?/h3>

如圖4所示，雙機(jī)器人末端安裝有法蘭，從機(jī)器人法蘭安裝力傳感器，所搬運(yùn)物體為鋁質(zhì)薄壁箱體，長(zhǎng)、寬、高分別為280 mm、220 mm、120 mm。在箱體長(zhǎng)度方向人為加入環(huán)境阻抗，即厚度都為10 mm的橡膠墊，橡膠墊和箱體總質(zhì)量約為2.1 kg，雙機(jī)器人通過(guò)和橡膠墊接觸產(chǎn)生一定壓力來(lái)完成搬運(yùn)任務(wù)。箱體質(zhì)地均勻，To取在箱體幾何中心，Tb1、Tb2之間的位姿關(guān)系由四點(diǎn)握手標(biāo)定法[14]得到，傳感器坐標(biāo)系(圖中未標(biāo)出)的z軸和Te2的z軸共線。

圖4 雙機(jī)器人協(xié)調(diào)搬運(yùn)示意圖

圖4是搬運(yùn)前的初始狀態(tài)，雙機(jī)器人到達(dá)指定位置和橡膠墊接觸產(chǎn)生期望內(nèi)力oFd和-oFd，To、Te1、Te2各坐標(biāo)軸平行。啟動(dòng)搬運(yùn)后，Te1的x、y、z軸和Tb1的z、y、x軸保持平行，只有相對(duì)位置發(fā)生變化。主機(jī)器人先在Tb1的z軸正向移動(dòng)40 mm，然后在Tb1的y軸移動(dòng)400 mm，最后在Tb1的z軸負(fù)向移動(dòng)40 mm，速度為20 mm/s。整個(gè)搬運(yùn)過(guò)程從機(jī)器人進(jìn)行跟隨運(yùn)動(dòng)，即雙機(jī)器人搬運(yùn)箱體完成抬起、移動(dòng)、放下的搬運(yùn)任務(wù)。

分析圖4，由上文介紹的內(nèi)力計(jì)算模型可以知道，從機(jī)器人對(duì)物體的內(nèi)力就是傳感器z軸檢測(cè)到的力的反力，oFd和-oFd就是物體z軸受到的內(nèi)力(壓力)，控制過(guò)程就是根據(jù)oFd和搬運(yùn)過(guò)程產(chǎn)生的內(nèi)力經(jīng)過(guò)自適應(yīng)阻抗模型產(chǎn)生從機(jī)器人末端的位置修正量，從機(jī)器人不斷修正位置從而把內(nèi)力控制在oFd。

3.2 仿真驗(yàn)證

經(jīng)多次橡膠墊擠壓實(shí)驗(yàn)，測(cè)定橡膠墊和機(jī)器人之間的接觸剛度約為50 N/mm。根據(jù)箱體和橡膠墊質(zhì)量以及機(jī)器人和橡膠墊之間的摩擦系數(shù)初步估計(jì)搬運(yùn)箱體所需最小期望內(nèi)力oFd=50 N，這個(gè)內(nèi)力是使物體在搬運(yùn)過(guò)程中不掉落的最小內(nèi)力。為了模擬實(shí)際搬運(yùn)過(guò)程中從機(jī)器人末端在z軸方向相對(duì)于主機(jī)器人末端的位置變化，給從機(jī)器人末端一周期為搬運(yùn)總時(shí)間，即24 s、幅值為0.5 mm的位置誤差信號(hào)。在Matlab/Simulink中搭建仿真模型，通過(guò)仿真得到結(jié)果。

如圖5所示，未進(jìn)行力控制之前，雙機(jī)器人對(duì)物體的壓力，即內(nèi)力，在25 N～75 N之間變化，開(kāi)啟內(nèi)力控制之后，通過(guò)調(diào)整公式(5)中的阻抗參數(shù)和更新率，內(nèi)力被穩(wěn)定控制在(50±0.5)N。

圖5 內(nèi)力控制仿真結(jié)果

3.3 實(shí)驗(yàn)驗(yàn)證

根據(jù)3.1節(jié)介紹的仿真模型搭建實(shí)驗(yàn)平臺(tái)，如圖6所示。實(shí)驗(yàn)使用的機(jī)器人是埃斯頓六關(guān)節(jié)通用機(jī)器人ER30和ER20，分別作為主機(jī)器人和從機(jī)器人，傳感器使用某公司的M3714A搭配M8128B1采集盒。傳感器安裝在從機(jī)器人末端，采集到的數(shù)據(jù)經(jīng)采集盒處理后上傳到上位機(jī)，供編程處理使用。這里需說(shuō)明，所有的軌跡規(guī)劃程序都是作者在上位機(jī)上編程實(shí)現(xiàn)，用到的只是機(jī)器人控制器的伺服功能，上位機(jī)和機(jī)器人控制器的通信周期為10 ms。

圖6 雙機(jī)器人協(xié)調(diào)搬運(yùn)實(shí)驗(yàn)

經(jīng)過(guò)多次運(yùn)動(dòng)實(shí)驗(yàn)，確定oFd=60 N，此內(nèi)力剛好可以完成搬運(yùn)任務(wù)且不會(huì)產(chǎn)生多余的內(nèi)力。為了編寫力控制程序，公式(5)經(jīng)過(guò)后向差分離散化[15-16]之后在上位機(jī)上編程實(shí)現(xiàn)，通過(guò)多次調(diào)整公式(5)中的阻抗參數(shù)和更新率，得到最終的實(shí)驗(yàn)結(jié)果。

圖7所示是傳感器采集到的控制前后的內(nèi)力變化曲線。在進(jìn)行內(nèi)力控制之前，只進(jìn)行運(yùn)動(dòng)控制完成搬運(yùn)軌跡，搬運(yùn)總時(shí)長(zhǎng)24 s，其中抬起2 s，放下2 s，移動(dòng)20 s。在抬起和移動(dòng)、移動(dòng)和放下的轉(zhuǎn)折點(diǎn)處由于相對(duì)位置誤差突然變大導(dǎo)致內(nèi)力發(fā)生突變，搬運(yùn)過(guò)程中內(nèi)力最大為120 N。由于接觸剛度為50 N/mm，雙機(jī)器人間最大約有1.2 mm的相對(duì)位置誤差。在開(kāi)啟內(nèi)力控制之后，不僅在轉(zhuǎn)折點(diǎn)處把內(nèi)力及時(shí)控制在60 N左右，在移動(dòng)過(guò)程中內(nèi)力也被穩(wěn)定控制在(60±2)N。

圖7 內(nèi)力控制實(shí)驗(yàn)結(jié)果

4 結(jié)語(yǔ)

本文所提出的基于自適應(yīng)阻抗模型的從機(jī)器人單端位置調(diào)節(jié)內(nèi)力控制策略有良好的效果，在雙機(jī)器人搬運(yùn)箱體完成抬起、搬運(yùn)、放下且速度為20 mm/s的運(yùn)動(dòng)軌跡過(guò)程中，給定物體z軸期望內(nèi)力60 N的任務(wù)要求，最終內(nèi)力不僅在移動(dòng)過(guò)程中被控制在(60±2)N，在抬起和移動(dòng)以及移動(dòng)和放下的轉(zhuǎn)折點(diǎn)處，物體內(nèi)力有突變的情況下，仍然被及時(shí)地控制在期望水平。