基于自適應(yīng)阻抗模型的雙機器人協(xié)調(diào)搬運內(nèi)力控制

王耀武，張得禮，胡雪剛

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

0 引言

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

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

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

1 運動學(xué)分析和內(nèi)力建模

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

圖1 雙機器人協(xié)調(diào)運動學(xué)和靜力學(xué)關(guān)系

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

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

(1)

(2)

(3)

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

(4)

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

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

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

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

(5)

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

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

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

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

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

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

3 仿真和實驗

3.1 仿真和實驗?zāi)Ｐ偷慕?/h3>

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

圖4 雙機器人協(xié)調(diào)搬運示意圖

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

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

3.2 仿真驗證

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

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

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

3.3 實驗驗證

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

圖6 雙機器人協(xié)調(diào)搬運實驗

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

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

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

4 結(jié)語

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