基于阻抗控制的雙臂機(jī)器人不對稱PLC任務(wù)系統(tǒng)研究

毛睿欣

摘 要：針對單臂機(jī)器人難以應(yīng)對復(fù)雜的任務(wù)問題，以雙臂協(xié)作進(jìn)行書寫任務(wù)為切入點(diǎn)，介紹了雙臂機(jī)器人控制系統(tǒng)框架，并以TwinCAT軟件的PLC為上層控制核心設(shè)計了雙臂機(jī)器人實(shí)時控制系統(tǒng)，通過引入相對雅可比對機(jī)器人的動力學(xué)方程和阻抗方程進(jìn)行了改進(jìn)，為控制系統(tǒng)的實(shí)現(xiàn)提供了理論基礎(chǔ);并通過不同期望阻抗下的書寫任務(wù)實(shí)驗，驗證所提出的控制系統(tǒng)設(shè)計方案的有效性。結(jié)果表明，所設(shè)計的控制系統(tǒng)能夠滿足實(shí)驗任務(wù)的需求，相對阻抗控制行為與期望可保持高度的一致性。

關(guān)鍵詞：雙臂機(jī)器人;阻抗控制;可編程邏輯控制器;控制系統(tǒng)

中圖分類號：TP242.3 文獻(xiàn)標(biāo)識碼：A 文章編號：1001-5922（2021）12-0134-06

Study on Asymmetric PLC Task System of Dual-arm Robot Based On Impedance Control

Mao Ruixin

（Guangdong University of Technology， Guangzhou 510000， China）

Abstract：Single arm robot is difficult to deal with complex task. With the two-armed collaborative writing task as the entry point， the dual-arm robot control system framework is introduced， and taking the PLC of TwinCAT software as the upper control core to design real time control system for dual-arm robot. In addition， the relative Jacobian is introduced to improve the dynamics equation and impedance equation of the robot， which provides a theoretical basis for the realization of the control system. And The effectiveness of the proposed control system is verified by writing task experiments under different expected impedance. The results show that the designed control system can meet the requirements of experimental tasks， and the relative impedance control behavior can maintain a high degree of consistency with the expectation.

Key words：Dual-arm robot;Impedance control;PLC;Control system

0 引言

隨著機(jī)器人技術(shù)的不斷發(fā)展，單臂機(jī)器人在分揀、裝配等領(lǐng)域的應(yīng)用均獲得了較高的認(rèn)可度。然而，就如人類擁有一對手臂一樣，單臂機(jī)器人往往難以應(yīng)付復(fù)雜的任務(wù)。雙臂機(jī)器人通常設(shè)計為具有相互作用的形式，并且在某些情況下，與其工作環(huán)境相互作用。因而，雙臂機(jī)器人可為實(shí)現(xiàn)擬人化的直觀任務(wù)提供平臺，并能在執(zhí)行某些工作時直接可取代人類進(jìn)行獨(dú)立作業(yè)[1-3]。其雙手任務(wù)可分為對稱或非對稱。在非對稱雙手任務(wù)中，每個機(jī)械臂擔(dān)當(dāng)不同的角色，但每個機(jī)械臂都有所需完成的必要任務(wù)，例如交易刷卡、記錄筆記、協(xié)同裝配以及開啟瓶蓋等[4]。這些任務(wù)涉及大量復(fù)雜的技術(shù)，但雙臂機(jī)器人所遇到的最大挑戰(zhàn)是其任務(wù)阻抗控制的復(fù)雜性[5]。

以此為出發(fā)點(diǎn)，本文對雙臂協(xié)作進(jìn)行協(xié)同工作任務(wù)進(jìn)行了研究，提出了一種在雙臂系統(tǒng)上實(shí)現(xiàn)阻抗控制的方法，同時在控制系統(tǒng)的軟件設(shè)計方面，通過TwinCAT軟件實(shí)現(xiàn)PLC高實(shí)時性的總線控制系統(tǒng)，用以取代落后的運(yùn)動控制卡+伺服驅(qū)動模式，來保障機(jī)器人控制算法的高速、實(shí)時運(yùn)行。采用時間延遲估計技術(shù)，通過雙臂的不精確動態(tài)信息估計機(jī)器人動力學(xué)使得控制得到了進(jìn)一步簡化。從控制角度來看，該算法的相對雅可比是通過組合每個臂的各個雅可比得出的，它創(chuàng)建了一個具有運(yùn)動冗余的單臂鏈，從而有助于簡化控制策略。另外，該方法將雙臂作為單個機(jī)械手進(jìn)行控制，更容易協(xié)調(diào)兩個機(jī)械臂之間的相互作用。

1 雙臂機(jī)器人控制系統(tǒng)架構(gòu)概述

通常情況下機(jī)器人采用由上而下的循環(huán)控制模式，即運(yùn)動控制卡控制上層運(yùn)動規(guī)劃，各種控制指令采用CAN或者485總線被發(fā)送到驅(qū)動裝置，通過驅(qū)動裝置來有效控制伺服電機(jī)的輸出[6]。由此可見，在機(jī)器人由上而下的循環(huán)控制模式中，運(yùn)動控制卡在機(jī)器人運(yùn)動學(xué)以及運(yùn)動軌跡規(guī)劃等任務(wù)中發(fā)揮著至關(guān)重要的作用。

1.1 機(jī)器人控制系統(tǒng)軟件架構(gòu)

將德國倍福公司研發(fā)的TwinCAT軟件作為雙臂機(jī)器人上層控制的心臟，構(gòu)建雙臂機(jī)器人控制系統(tǒng)的軟件架構(gòu)。圖1為基于TwinCAT PLC的控制系統(tǒng)軟件架構(gòu)，該架構(gòu)在操作系統(tǒng)下構(gòu)造了實(shí)時環(huán)境，使得其具有了PLC的功能。與傳統(tǒng)的PLC相比，基于TwinCAT PLC的控制系統(tǒng)軟件架構(gòu)具有十分明顯的優(yōu)勢。雙臂機(jī)器人采用阻抗控制時必須對機(jī)器人的動力學(xué)數(shù)據(jù)、雅克比數(shù)據(jù)進(jìn)行計算與存儲，這使得TwinCAT軟件的優(yōu)勢得到了充分體現(xiàn)。采用TwinCAT軟件能夠控制的最大電機(jī)軸數(shù)多達(dá)255個，即同時控制兩臺6軸工業(yè)機(jī)器人形成雙臂系統(tǒng)綽綽有余。

在基于計算機(jī)的機(jī)器人控制系統(tǒng)中，控制時鐘周期為毫秒級。一般而言，時鐘周期與機(jī)器人運(yùn)動控制優(yōu)化空間之間成反比，其周期越長，運(yùn)動控制優(yōu)化空間越小;周期越短，運(yùn)動控制優(yōu)化空間越大。雙臂機(jī)器人只有具有更大的運(yùn)動控制優(yōu)化空間，才能夠獲得良好的性能和更加平穩(wěn)的運(yùn)動效果。CAN總線控制周期為8～20 ms，基本能夠滿足基本插補(bǔ)位置模式，但這種控制方式?jīng)]有機(jī)器人動力學(xué)反饋，且在宏觀上對控制效果的影響并不明顯[7]。而對于融入了機(jī)器人動力學(xué)的高級算法來說，周期比較長會導(dǎo)致速度、加速度等規(guī)劃運(yùn)動量的計算受到影響，同時外加控制系統(tǒng)的滯后性使得雙臂機(jī)器人跟隨運(yùn)動指令的穩(wěn)態(tài)誤差會比較大，從而導(dǎo)致雙臂機(jī)器人無法按照預(yù)先設(shè)定的軌跡運(yùn)動。

1.2 阻抗控制中的高速總線

對于本文所涉及的雙臂協(xié)作任務(wù)，需要通過阻抗控制來解決機(jī)器人的柔順控制問題。阻抗控制下構(gòu)建控制系統(tǒng)的微分方程，其微分方程所反映的是每一個周期控制指令的周期性變化。對于EtherCAT總線而言，其最高能夠支持1 ms的控制周期，即和CAN總線相比能夠控制的調(diào)整時間縮短了8倍，這對雙臂機(jī)器人末端位置控制、機(jī)器人各關(guān)節(jié)速度與加速度的控制更加精準(zhǔn)化。機(jī)器人受到阻抗單元控制時，其宏觀體現(xiàn)在末端位置按照末端受力的一個收斂過程，更短的控制周期使得機(jī)器人的各項運(yùn)動參數(shù)更加平滑[8]。

2 雙臂機(jī)器人不對稱任務(wù)

圖2為雙臂機(jī)器人執(zhí)行不對稱任務(wù)示意圖，其中，機(jī)器人A為參考機(jī)器人;機(jī)器人B為目標(biāo)機(jī)器人。坐標(biāo)系定義如下：目標(biāo)坐標(biāo)系T 位于機(jī)器人B末端執(zhí)行器;相對參考系R位于機(jī)器人A的末端執(zhí)行器;機(jī)器人A的基坐標(biāo)系為A;機(jī)器人B的基坐標(biāo)系為B;世界坐標(biāo)系為world。定義T相對于R的相對位置為向量xR∈RnR，nR表示任務(wù)空間的自由度。圖2中顯示了機(jī)器人A末端執(zhí)行器的位姿xA，以及機(jī)器人B末端執(zhí)行器的位姿xB;qA為機(jī)器人A的關(guān)節(jié)角度向量;qB為機(jī)器人B的關(guān)節(jié)角度向量。

2.1 相對雅可比的計算

在運(yùn)動學(xué)領(lǐng)域中，相對速度向量x'R∈RnR是通過相對雅可比 JR∈RnR×nT與關(guān)節(jié)空間速度向量q'∈RnT=[ q'TA? ? q'TB ]T 的作用得出的。

x'R=JRq'

q'A∈RnA和q'B∈RnB分別表示機(jī)器人A和機(jī)器人B的關(guān)節(jié)空間速度，且nT=nA+nB，根據(jù)各自的物理含義可得：

x'R=x'R-x'A

將公式（2）代入公式（1）可得：

而? ? ? ? ? ? ? ? ? ? ? ? ?是相對于慣性參考系的相對雅可比矩陣。

2.2 相對阻抗控制

任務(wù)空間中的相對加速度向量xR'' 可表示為xR''= JRq''+J'Rq'，式中：J'R為JR的導(dǎo)數(shù);q''表示關(guān)節(jié)加速度。其逆運(yùn)動學(xué)方程為：

式中，I∈RnT×nT為單位陣;v∈RnT為關(guān)節(jié)空間加速度向量。此時，設(shè)定v=0，可實(shí)現(xiàn)最小范數(shù)加速。

整個雙臂系統(tǒng)的完整動力學(xué)為機(jī)器人A和機(jī)器人B的組合動力學(xué)，即

其中為關(guān)節(jié)力矩向量;M（q）為慣性矩陣，其為機(jī)器人 A與機(jī)器人B組合而成的對角矩陣;c（q，q' ）表示科里奧利力和離心力產(chǎn)生的組合扭矩;g（q）表示重力產(chǎn)生的組合扭矩; f （q，q' ）表示庫侖和粘性摩擦組合扭矩;τd表示由于干擾引起的扭矩;τe表示由于環(huán)境接觸力引起的扭矩。

此種狀態(tài)下，任務(wù)的物理系統(tǒng)目標(biāo)動力學(xué)，可通過所需的阻抗方程實(shí)現(xiàn)[9]。具體可通過修改所需的阻抗方程使其能夠適應(yīng)雙臂的兩個末端執(zhí)行器之間的相對運(yùn)動。它們之間的內(nèi)部阻抗以及末端執(zhí)行器的外部阻抗之間的關(guān)系對應(yīng)于等效單個機(jī)器人。在參考系R中，fR∈RnR表示兩個末端執(zhí)行器之間的相對接觸力，從虛功原理中可得τe= JRT fR;MRd ， BRd ， KRd∈RnR×nR，它們分別表示所需的質(zhì)量、阻尼和剛度矩陣;xRd ， x'Rd ， x''Rd∈RnR分別表示所需的位姿、速度和加速度向量。當(dāng)在雙臂系統(tǒng)中使用兩個6自由度機(jī)器人時，不是管理12個組件來對兩個臂執(zhí)行阻抗控制，而是僅使用6個組件等效單個機(jī)械臂，因此更容易實(shí)現(xiàn)。此外，可根據(jù)末端執(zhí)行器相對于任務(wù)空間之間的相對運(yùn)動來指定期望的運(yùn)動[10-11]。

2.3 時延估計和理想速度反饋

通常情況下，阻抗控制器中有兩項關(guān)鍵技術(shù)，即時延估計及理想速度的反饋。時延估計是一種無需模型即可估算機(jī)器人動力學(xué)的技術(shù)。理想速度反饋源于自然導(dǎo)納控制，用于消除不連續(xù)物質(zhì)的非線性，如庫侖摩擦、靜摩擦力和慣性力等不確定性。前者負(fù)責(zé)連續(xù)非線性，而后者負(fù)責(zé)非連續(xù)非線性。機(jī)器人動力學(xué)方程可據(jù)此寫為如下形式：

式中，M 為常對角矩陣;h（q ， q' ， q'' ）包含所有連續(xù)和不連續(xù)的非線性，為了對其進(jìn)行計算，假設(shè)在給定的時間 t 和足夠小的采樣時間，可得

用（q ， q' ， q'' ）表示h（q ， q' ， q'' ）的估計值，可得（q ， q' ， q'' ）=h（q ， q' ， q'' ）（t-L），結(jié)合式（7）可得：

可得到操作空間動力學(xué)方程：

將v設(shè)為0，τ設(shè)為τu：

3 控制系統(tǒng)的實(shí)現(xiàn)

本文通過3個組實(shí)驗驗證所提出的相對阻抗控制在雙臂系統(tǒng)上執(zhí)行非對稱任務(wù)，包括接觸、非接觸和過渡3種狀態(tài)。據(jù)此確定給定的任務(wù)所需的軌跡和阻抗。

3.1 實(shí)驗設(shè)置

雙臂實(shí)時控制平臺的配置如圖3所示。

本文所采用的雙臂機(jī)器人為三星公司制造的兩個6自由度PUMA型工業(yè)機(jī)器人，F(xiàn)araman-AC2作為參考機(jī)器人（左），F(xiàn)araman-AT2作為目標(biāo)機(jī)器人（右）。在參考機(jī)器人的末端執(zhí)行器上安裝有6維力/力矩傳感器，在其上面固定丙烯酸板。通過該傳感器測量動力學(xué)方程中的 fR，即相對于R的相對接觸力。盤子上覆蓋著一張白紙，使書寫更加醒目。工具機(jī)器人的末端執(zhí)行器上連接一個帶有紅色墨水的鋼球。

控制器使用帶有i7四核CPU的工控機(jī)，力/力矩傳感器通過以太網(wǎng)與TwinCAT進(jìn)行連接，采樣周期設(shè)定為2 ms。

3.2 實(shí)驗描述

以兩個6自由度的工業(yè)機(jī)器人作為實(shí)驗對象，通過兩個機(jī)器人的協(xié)調(diào)運(yùn)動來表示接觸、非接觸、過渡書寫3種不對稱任務(wù)，將工業(yè)機(jī)器人連接到參考機(jī)器人的末端執(zhí)行器所在的板上寫3個字母。

（1）相對位姿的期望軌跡。在xR-yR平面寫入字母，設(shè)定zR軸方向位置超過書寫板所在的位置，從而確保在寫入的過程中始終保持接觸。相對位置軌跡由27個直線段或者曲線段所組成，每一段由5階多項式擬合而成，圖4給出了期望軌跡與書寫板位置隨時間變化的關(guān)系。執(zhí)行t =0～66 s整個任務(wù)，所需的姿態(tài)軌跡由與機(jī)器人左臂末端執(zhí)行器來完成，其在R中的滾動俯仰-偏航角為[αR ， βR ， γR]T=[π ， 0 ， 0]T。因此，僅僅需要XR的期望值就可以滿足要求，而不用機(jī)器人每一個末端執(zhí)行器的期望值。

（2）期望的相對阻抗。為抵抗書寫板剛性，在接觸與非接觸之間執(zhí)行10次過渡，同時在zR約束方向設(shè)定高阻尼和低剛度的柔順行為。為了驗證沿著zR約束方向能夠準(zhǔn)確、平穩(wěn)地實(shí)現(xiàn)期望的相對阻抗，對沿著zR約束方向的3種過阻尼阻抗?fàn)顟B(tài)進(jìn)行測試，設(shè)定阻尼比ζZR=4，5，6。其余的自由度，當(dāng)臨界阻尼阻抗ζ=1時，必須采取底阻尼、高剛度的精準(zhǔn)寫入。對每一個過阻尼ζZR，設(shè)置對應(yīng)的相對質(zhì)量MRd，其阻尼矩陣和剛度矩陣分別為BRd和KRd。

3.3 控制器實(shí)現(xiàn)

為了實(shí)現(xiàn)期望的相對軌跡和相對阻抗行為，將式（12）中所提出的控制器模型用于實(shí)驗。的選擇對控制性能具有顯著影響，原則上， 的最佳選擇是M （ q ），即實(shí)際慣性矩陣。然而，由于M （ q ）難以準(zhǔn)確估計并且計算有著實(shí)時性要求，因此 通常通過反復(fù)試驗來調(diào)整。它的對角元素從穩(wěn)定區(qū)域內(nèi)的最小正值逐漸增加，而不使用系統(tǒng)參數(shù)。理想速度反饋增益被設(shè)置為沒有發(fā)生振動的最高值。

值得注意的是，所提出的控制需要時延估計的關(guān)節(jié)加速度向量q''（t-L）。在實(shí)際實(shí)驗中，編碼器信號通常易被噪聲干擾，且由于通過數(shù)值微分計算關(guān)節(jié)加速度，噪聲的影響易被放大。通過降低 可以減弱噪聲，而無需明確使用額外的低通濾波器。使用本文提出的相對阻抗方程（6），使得期望的任務(wù)等同于單個機(jī)器人的情況。

4 實(shí)驗結(jié)果分析

4.1 相對阻抗組1#

期望的相對阻抗組1#，使用期望阻抗組1#參數(shù)進(jìn)行雙臂書寫任務(wù)。xR， yR， zR軸的相對位置和姿態(tài)軌跡如圖5所示。

除zR方向外，其余方向均實(shí)現(xiàn)了良好的跟蹤精度。沿zR方向的位置誤差因保持接觸并獲得所需的相對阻抗造成，為必要誤差。期望的相對阻抗行為實(shí)現(xiàn)效果良好。

4.2 相對阻抗組2#和3#

為實(shí)現(xiàn)各種所需阻抗時驗證所提出的控制器的性能，在兩個附加條件下執(zhí)行相同的書寫任務(wù)。本文對所提出的阻尼比的變化相對于阻尼系數(shù)BRd和勁度系數(shù)KRd進(jìn)行了修改，也可通過改變慣性系數(shù)MRd 將阻尼比改變?yōu)槠淦谕怠?/p>

3個實(shí)驗組的沿約束方向zR的相對位置響應(yīng)如圖6所示。觀察到根據(jù)相對阻抗的響應(yīng)變化，所需相對阻抗的阻尼比越小，建立時間越快，過沖越小。

在3個實(shí)驗的書寫任務(wù)期間沿著約束方向zR的相對阻抗誤差范圍如圖7所示，2#和3#組的RMS誤差分別為0.419和0.441。

由上述結(jié)果可知，所提出的控制器在不同的阻抗值下可以很好地實(shí)現(xiàn)所需的相對阻抗。

5 結(jié)語

本文基于TwinCAT PLC進(jìn)行雙臂機(jī)器人實(shí)時控制，實(shí)現(xiàn)了雙臂機(jī)器人不對稱PLC任務(wù)系統(tǒng)的阻抗控制，確保機(jī)器人控制算法的效率大大提升。采用時間延遲估計技術(shù)，通過雙臂的不精確動態(tài)信息估計機(jī)器人動力學(xué)使得控制得到了進(jìn)一步簡化。

控制器可以將兩個臂的關(guān)節(jié)速度映射到它們的末端執(zhí)行器之間的相對運(yùn)動。此外本文引入相對雅可比用來改進(jìn)動力學(xué)和阻抗方程，以實(shí)現(xiàn)將雙臂映射到一個機(jī)器人用以執(zhí)行非對稱雙手書寫任務(wù)。通過使用相對雅可比行列式，阻抗控制被簡化為用于兩個臂的單個控制器，使得兩個末端執(zhí)行器之間的相對運(yùn)動表征執(zhí)行期望任務(wù)。這使得在指定期望阻抗和期望軌跡時所提出的阻抗控制更簡單且更直觀。對于書寫任務(wù)的實(shí)施，在不同的阻尼比下進(jìn)行了3個組實(shí)驗，相對剛度假設(shè)最大穩(wěn)態(tài)誤差為1.35%;時延估計的使用使得所提出的控制器更容易實(shí)現(xiàn);對所提出的控制器進(jìn)行了穩(wěn)定性分析，結(jié)果表明誤差最終是有界的。

參考文獻(xiàn)

[1]李開霞， 丁 玲， 趙江海，等. 雙臂機(jī)器人末端一體化關(guān)節(jié)控制與驅(qū)動單元設(shè)計[J]. 機(jī)電工程， 2017（9）：1 019-1 023.

[2]郭 鵬， 房靈申， 趙明揚(yáng)，等. 基于擴(kuò)展任務(wù)的雙臂機(jī)器人位姿優(yōu)化[J]. 組合機(jī)床與自動化加工技術(shù)， 2018， 527（1）：8-12.

[3]王美玲， 駱敏舟. 一種冗余自由度雙臂機(jī)器人協(xié)調(diào)控制器設(shè)計[J]. 系統(tǒng)仿真學(xué)報， 2016， 28（8）：1 783-1 789.

[4]孟令威. 七自由度手臂運(yùn)動控制與軌跡插值[J]. 機(jī)械設(shè)計與制造， 2017（05）：263-266.

[5]趙金剛， 戈新生. 動態(tài)規(guī)劃求解空間雙臂機(jī)器人非完整運(yùn)動最優(yōu)控制問題[J]. 力學(xué)季刊， 2016（2）：225-233.

[6]MORRIS，A. S，MADANI. Quadratic Optimal Control of a Two-flexible-link Robot Manipulator[J]. Quadratic Optimal Control of a Two-flexible-link Robot Manipulator， 1998， 1（16）： 97-108.

[7]PILTAN F，RASHIDIAN MR，SHAMSODINI M， et al. Effect of Rule Base on the Fuzzy-based Tuning Fuzzy Sliding Mode Controller： Applied to 2 Nd Order Nonlinear System[J]. International Journal of Advanced Science & Technology， 2012， 60（3）： 222-231.

[8]黃俊銘， 陳 浩. 深海海底車行星輪式行走機(jī)構(gòu)建模分析[J]. 機(jī)械設(shè)計與制造， 2017（6）：5-9.

[9]劉 闊， 林劍峰， 馬曉波， 等. 阻抗控制在挖掘機(jī)器人中的應(yīng)用與研究[J]. 智能制造， 2010（5）：48-49.

[10]龔蘭芳， 曹成濤， 許倫輝. 肢康復(fù)訓(xùn)練機(jī)器人協(xié)作控制與S型規(guī)劃算法研究[J]. 中國工程機(jī)械學(xué)報，2019（06）： 482-487.