本發(fā)明涉及工業(yè)機(jī)器人領(lǐng)域，特別是涉及剛性條件下基于阻抗模型的高精度牽引示教機(jī)器人的速度控制方法。

背景技術(shù)：

多機(jī)器人協(xié)作系統(tǒng)相比于單機(jī)器人而言，在協(xié)同作業(yè)能力、適應(yīng)復(fù)雜任務(wù)和適應(yīng)復(fù)雜環(huán)境等方面體現(xiàn)出優(yōu)勢(shì)，并且也是共融機(jī)器人中的一個(gè)非常重要的環(huán)節(jié)，是智能群體實(shí)現(xiàn)的基礎(chǔ)。以多機(jī)器人協(xié)作焊接為例，多機(jī)器人協(xié)作焊接中較為核心的關(guān)鍵科學(xué)問(wèn)題是：雙臂(兩臺(tái)搬運(yùn)機(jī)器人)間的位置力協(xié)調(diào)問(wèn)題，目前的難點(diǎn)體現(xiàn)在雙臂間的內(nèi)力難控、現(xiàn)有控制算法實(shí)現(xiàn)難度大等。傳統(tǒng)的解決雙臂間位置力協(xié)調(diào)的三種直觀解決方案是：1)建立雙機(jī)器人閉鏈下的動(dòng)力學(xué)模型：建立雙臂及被操作對(duì)象的耦合動(dòng)力學(xué)模型，并嘗試從理論上解決系統(tǒng)建模誤差和外界干擾等問(wèn)題；2)混合位置/力控制：對(duì)機(jī)器人末端的接觸力和位置進(jìn)行正交控制，在某一方向進(jìn)行力控制，其他方向進(jìn)行位置控制。3)建立機(jī)械臂與被操作物體之間的內(nèi)阻抗，解決機(jī)器人間的內(nèi)力問(wèn)題；建立被操作物體與環(huán)境之間的外阻抗，解決物體與環(huán)境的外力問(wèn)題；內(nèi)阻抗與外阻抗融合控制。然而，這三種方案存在以下缺點(diǎn)：①針對(duì)方案1和方案3，目前大部分研究停留在理論分析階段，實(shí)驗(yàn)也停留在簡(jiǎn)單地雙臂協(xié)作夾持工件進(jìn)行簡(jiǎn)單的運(yùn)動(dòng)。②針對(duì)方案2，混合位置/力控制只適用于非嚴(yán)格緊協(xié)調(diào)下某個(gè)方向受到力約束的情況，如協(xié)作搬運(yùn)、抓取和裝配等任務(wù)，并不適合嚴(yán)格緊協(xié)調(diào)下六維度均受到力約束的工業(yè)應(yīng)用場(chǎng)合，如多機(jī)器人協(xié)作焊接工件。③針對(duì)方案3，未解決實(shí)際控制中嚴(yán)格緊協(xié)調(diào)下雙臂實(shí)時(shí)性較差和內(nèi)力難控的問(wèn)題。

技術(shù)實(shí)現(xiàn)要素：

發(fā)明目的：本發(fā)明的目的是提供一種能夠解決現(xiàn)有技術(shù)中存在的缺陷的剛性條件下基于阻抗模型的雙臂協(xié)調(diào)的加速度控制方法。

技術(shù)方案：為達(dá)到此目的，本發(fā)明采用以下技術(shù)方案：

本發(fā)明所述的剛性條件下基于阻抗模型的雙臂協(xié)調(diào)的加速度控制方法，包括以下步驟：

S1：機(jī)器人控制器采集六維力傳感器的信息，首先對(duì)采集到的信息進(jìn)行濾波處理，然后進(jìn)行重力補(bǔ)償，最后得到與期望力或者期望力矩值的偏差量數(shù)據(jù)；

S2：根據(jù)阻抗模型將力的偏差量數(shù)據(jù)或者力矩值的偏差量數(shù)據(jù)轉(zhuǎn)變?yōu)闄C(jī)器人末端在笛卡爾空間中移動(dòng)的加速度和繞軸旋轉(zhuǎn)的角加速度；

S3：根據(jù)變形的S型速度控制曲線對(duì)運(yùn)動(dòng)進(jìn)行平滑插補(bǔ)，求得對(duì)應(yīng)的位置函數(shù)、速度函數(shù)、加速度函數(shù)和加加速度函數(shù)；

S4：根據(jù)逆運(yùn)動(dòng)學(xué)進(jìn)而求得關(guān)節(jié)空間中的關(guān)節(jié)角度函數(shù)；

S5：將關(guān)節(jié)角度函數(shù)進(jìn)行關(guān)節(jié)空間的等時(shí)同步插補(bǔ)后通過(guò)控制器的總線發(fā)送至伺服驅(qū)動(dòng)器，進(jìn)而控制機(jī)器人的動(dòng)作。

進(jìn)一步，所述步驟S1中，機(jī)器人控制器采集六維力傳感器的信息是在指定力控周期通過(guò)UDP通訊口讀取六維力傳感器的力和力矩信息。

進(jìn)一步，所述步驟S2中，機(jī)器人末端在笛卡爾空間中移動(dòng)的加速度和繞軸旋轉(zhuǎn)的角加速度組成的矩陣用a_i表示，a_i為6*1的矩陣，矩陣a_i中前三行是機(jī)器人末端在笛卡爾空間中移動(dòng)的加速度，后三行是機(jī)器人末端在笛卡爾空間中繞軸旋轉(zhuǎn)的角加速度，a_i為：

式(1)中，F(xiàn)_zmax表示能夠承受的最大六維力和力矩信息，a_max表示機(jī)器人末端允許的最大加速度，F(xiàn)_d表示期望跟蹤的六維力，f為一個(gè)單調(diào)函數(shù)，S表示對(duì)角線函數(shù)為0或1的對(duì)角矩陣。

進(jìn)一步，所述步驟S3包括以下步驟：

S3.1：根據(jù)式(2)求得位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)：

式(2)中，θ_i為初始位置，v_i為初始速度，a_i為初始加速度，a_i+1為期望的終點(diǎn)速度，t為歸一化的時(shí)間，T為機(jī)器人控制器與伺服驅(qū)動(dòng)器的通訊周期；

S3.2：判斷a_i+1與a_i之差的絕對(duì)值是否超過(guò)a_max，v_i的絕對(duì)值是否超過(guò)V_max，a_max表示允許的機(jī)器人末端最大的加速度，V_max表示允許的機(jī)器人末端最大的速度：如果a_i+1與a_i之差的絕對(duì)值超過(guò)a_max，v_i的絕對(duì)值沒(méi)有超過(guò)V_max，則進(jìn)行步驟S3.3；如果a_i+1與a_i之差的絕對(duì)值超過(guò)a_max，v_i的絕對(duì)值也超過(guò)V_max，則進(jìn)行步驟S3.4；

S3.3：判斷a_i+1與a_i的大?。喝绻鸻_i＞a_i+1，則根據(jù)式(3)重新計(jì)算位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；如果a_i＜a_i+1，則根據(jù)式(4)重新計(jì)算位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；

S3.4：根據(jù)式(5)重新計(jì)算位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；

進(jìn)一步，所述步驟S5中，控制器的總線為EtherCAT總線。

進(jìn)一步，所述機(jī)器人控制器從上到下分為六個(gè)層次，即用戶(hù)層、六維力信號(hào)采集和處理層、阻抗控制層、加速度控制層、關(guān)節(jié)插補(bǔ)層和EtherCAT總線通訊層；其中，用戶(hù)層、六維力信號(hào)采集和處理層、阻抗控制層對(duì)外開(kāi)放，加速度控制層、關(guān)節(jié)插補(bǔ)層和EtherCAT總線通訊層不對(duì)外開(kāi)發(fā)。

進(jìn)一步，所述用戶(hù)層提供用戶(hù)二次開(kāi)發(fā)的用戶(hù)交互接口，六維力信號(hào)采集和處理層提供用戶(hù)二次開(kāi)發(fā)的六維力采集和處理接口，阻抗控制層提供用戶(hù)二次開(kāi)發(fā)的阻抗控制接口。

有益效果：與現(xiàn)有技術(shù)相比，本發(fā)明具有如下的有益效果：

1)解決實(shí)際工業(yè)問(wèn)題：剖析現(xiàn)在實(shí)際工業(yè)現(xiàn)場(chǎng)存在的問(wèn)題，分析其本質(zhì)特點(diǎn)，提出對(duì)應(yīng)的解決方法，并非只停留在理論分析階段和簡(jiǎn)單的雙臂搬運(yùn)應(yīng)用；

2)方法通用性：相比于傳統(tǒng)的混合位置力協(xié)調(diào)方法只適用于非嚴(yán)格緊協(xié)調(diào)(僅某個(gè)方向或某幾個(gè)方向受到力的約束)任務(wù)，所提的方案更具有通用性，同時(shí)為非嚴(yán)格緊協(xié)調(diào)任務(wù)和嚴(yán)格緊協(xié)調(diào)任務(wù)(六個(gè)維度均受到力約束)兩種情況提供解決方案；

3)改善協(xié)調(diào)的實(shí)時(shí)跟隨效果和解決內(nèi)力不可控問(wèn)題。相比于傳統(tǒng)的力控方案而言，本發(fā)明可以實(shí)現(xiàn)實(shí)時(shí)的力跟蹤效果，且機(jī)器人跟隨時(shí)運(yùn)動(dòng)平滑，并有效地解決雙臂間的內(nèi)力難控問(wèn)題；

4)提供開(kāi)放式的二次開(kāi)發(fā)接口。為用戶(hù)開(kāi)放的接口包括：任務(wù)層的用戶(hù)交互接口、阻抗層的模型接口，用戶(hù)可根據(jù)需求自己修改柔性參數(shù)和修改阻抗模型。

附圖說(shuō)明

圖1為本發(fā)明具體實(shí)施方式的方法針對(duì)的雙臂協(xié)調(diào)控制系統(tǒng)的示意圖；

圖2為本發(fā)明具體實(shí)施方式的加速度控制架構(gòu)圖；

圖3為本發(fā)明具體實(shí)施方式的剛性條件下雙臂協(xié)調(diào)的單位力控周期的流程圖；

圖4為本發(fā)明具體實(shí)施方式的阻抗控制的框圖；

圖5為本發(fā)明具體實(shí)施方式的基于阻抗模型的F與a的對(duì)應(yīng)關(guān)系圖；

圖6為本發(fā)明具體實(shí)施方式的基于變形S型曲線的速度控制示意圖；

圖7為本發(fā)明具體實(shí)施方式的基于速度控制曲線的位置、速度、加速度和加加速度的示意圖。

具體實(shí)施方式

下面結(jié)合具體實(shí)施方式對(duì)本發(fā)明的技術(shù)方案作進(jìn)一步的介紹。

本具體實(shí)施方式公開(kāi)了一種剛性條件下基于阻抗模型的雙臂協(xié)調(diào)的加速度控制方法，包括以下步驟：

S1：機(jī)器人控制器采集六維力傳感器的信息，首先對(duì)采集到的信息進(jìn)行濾波處理，然后進(jìn)行重力補(bǔ)償，最后得到與期望力或者期望力矩值的偏差量數(shù)據(jù)；

S2：根據(jù)阻抗模型將力的偏差量數(shù)據(jù)或者力矩值的偏差量數(shù)據(jù)轉(zhuǎn)變?yōu)闄C(jī)器人末端在笛卡爾空間中移動(dòng)的加速度和繞軸旋轉(zhuǎn)的角加速度；

S3：根據(jù)變形的S型速度控制曲線對(duì)運(yùn)動(dòng)進(jìn)行平滑插補(bǔ)，求得對(duì)應(yīng)的位置函數(shù)、速度函數(shù)、加速度函數(shù)和加加速度函數(shù)；

S4：根據(jù)逆運(yùn)動(dòng)學(xué)進(jìn)而求得關(guān)節(jié)空間中的關(guān)節(jié)角度函數(shù)；

S5：將關(guān)節(jié)角度函數(shù)進(jìn)行關(guān)節(jié)空間的等時(shí)同步插補(bǔ)后通過(guò)控制器的總線發(fā)送至伺服驅(qū)動(dòng)器，進(jìn)而控制機(jī)器人的動(dòng)作。

圖1為雙臂協(xié)調(diào)控制系統(tǒng)的結(jié)構(gòu)示意圖。定義其中一臺(tái)機(jī)器人為主導(dǎo)機(jī)器人，夾持被操作對(duì)象的一部分，并主導(dǎo)運(yùn)動(dòng)的軌跡；另外一臺(tái)機(jī)器人為輔助機(jī)器人，夾持被操作對(duì)象的另一部分，在輔助機(jī)器人末端安裝有六維力傳感器，為防止由于位姿誤差導(dǎo)致內(nèi)力過(guò)大損壞力傳感器，在機(jī)器人末端與六維力傳感器之間安裝有保護(hù)裝置，輔助機(jī)器人根據(jù)末端六維力傳感器的反饋信息跟蹤主導(dǎo)機(jī)器人的運(yùn)動(dòng)軌跡。

機(jī)器人控制器從上到下分為六個(gè)層次，如圖2所示，即用戶(hù)層、六維力信號(hào)采集和處理層、阻抗控制層、加速度控制層、關(guān)節(jié)插補(bǔ)層和EtherCAT總線通訊層；其中，1)用戶(hù)層主要用于與用戶(hù)接口進(jìn)行交互，例如與示教盒交互，該層的作用是根據(jù)用戶(hù)需求設(shè)定相關(guān)的參數(shù)，如輔助機(jī)器人在協(xié)調(diào)時(shí)需表現(xiàn)出的柔性參數(shù)等。2)六維力信號(hào)采集和處理層主要用于六維力信號(hào)的采集、濾波、重力補(bǔ)償?shù)取?)阻抗控制層主要建立六維力偏差值與輔助機(jī)器人末端運(yùn)動(dòng)的關(guān)系，本發(fā)明中建立了力偏差值F與輔助機(jī)器人末端在笛卡爾空間中運(yùn)動(dòng)加速度a的變換關(guān)系，具體的關(guān)系式的示意圖如圖4所示，采用的模型為阻抗控制模型。4)加速度控制層主要是根據(jù)圖4所建立的模型關(guān)系在笛卡爾空間中實(shí)現(xiàn)基于加速度曲線的平滑控制，是輔助機(jī)器人在主導(dǎo)機(jī)器人牽引下末端在笛卡爾空間中基于阻抗模型的最終行為表現(xiàn)：實(shí)時(shí)跟蹤主導(dǎo)機(jī)器人的運(yùn)動(dòng)軌跡。5)關(guān)節(jié)插補(bǔ)層主要是在關(guān)節(jié)空間中對(duì)各軸進(jìn)行等時(shí)插補(bǔ)同步規(guī)劃。6)EtherCAT總線通訊層主要完成關(guān)節(jié)角度值向脈沖量的轉(zhuǎn)換，并通過(guò)EtherCAT主站將該脈沖轉(zhuǎn)換量發(fā)送給伺服從站，同時(shí)接收來(lái)自伺服從站的狀態(tài)信息，包括狀態(tài)值、當(dāng)前位置值、速度值、加速度值等。

機(jī)器人控制器中最核心的層為：力信號(hào)采集和處理層、阻抗層和加速度控制層，此三層通稱(chēng)為雙臂協(xié)調(diào)力控包。1)力信號(hào)采集和處理結(jié)果影響了牽引的精度，力信號(hào)一般通過(guò)濾波的方法去除抖動(dòng)信號(hào)，然后對(duì)輔助機(jī)器人在各種位姿下進(jìn)行重力補(bǔ)償。2)阻抗層的核心為阻抗模型，用戶(hù)可根據(jù)需求自己設(shè)定的線性函數(shù)變換或是二次多項(xiàng)式函數(shù)變換，但是需滿(mǎn)足單調(diào)函數(shù)的要求。3)加速度控制層是五次多項(xiàng)式的S型變形加速度規(guī)劃，在指定的單位力控周期內(nèi)對(duì)加速度進(jìn)行平滑插補(bǔ)，使輔助機(jī)器人末端運(yùn)動(dòng)光滑且保證實(shí)時(shí)跟隨主導(dǎo)機(jī)器人。

在圖2中，開(kāi)放式控制器中的“開(kāi)放式”體現(xiàn)在：給用戶(hù)提供二次開(kāi)發(fā)的接口，包括三個(gè)開(kāi)放的接口：用戶(hù)交互接口(User Interface，UI)、六維力采集和處理接口(Force/Torque Acquire Interface，F(xiàn)TAI)和阻抗控制接口(Impedance Control Interface，ICI)，其它的層(速度控制層、關(guān)節(jié)插補(bǔ)層和通訊層)涉及具體的控制實(shí)現(xiàn)，因此不對(duì)外開(kāi)放。

圖3為剛性條件下雙臂協(xié)調(diào)的單位力控周期流程圖。用戶(hù)可根據(jù)用戶(hù)接口(例如示教盒)啟動(dòng)輔助機(jī)器人協(xié)調(diào)跟蹤模式，并設(shè)置相關(guān)的機(jī)器人柔性跟蹤參數(shù)，例如機(jī)器人期望表現(xiàn)出的慣量(M)、阻尼(B)和剛度(K)系數(shù)等。機(jī)器人控制器實(shí)時(shí)對(duì)六維力信息進(jìn)行采集、濾波和處理，然后進(jìn)行傳感器的重力補(bǔ)償后得到當(dāng)前六維力偏差量，進(jìn)而根據(jù)基于阻抗模型的加速度控制算法求得位姿偏差量，最終控制輔助機(jī)器人按照位姿偏移量跟蹤主導(dǎo)機(jī)器人的軌跡。

圖4為阻抗控制的結(jié)構(gòu)圖，是對(duì)圖3流程圖的一個(gè)結(jié)構(gòu)介紹。

步驟S2中，在步驟S1的基礎(chǔ)上，根據(jù)用戶(hù)設(shè)定的機(jī)器人柔性參數(shù)和可選擇的阻抗模型類(lèi)型初始化阻抗層的參數(shù)，柔性參數(shù)可以設(shè)定機(jī)器人期望表現(xiàn)的慣量、阻尼和剛度，同時(shí)可以根據(jù)阻抗模型類(lèi)型設(shè)定如說(shuō)明書(shū)附圖5中的函數(shù)關(guān)系，一般情況下，根據(jù)用戶(hù)的柔性參數(shù)會(huì)自動(dòng)選擇對(duì)應(yīng)的函數(shù)關(guān)系(圖5中的某一種函數(shù)關(guān)系)，通用的函數(shù)表達(dá)式如下：

式(1)中，a_i為機(jī)器人末端在笛卡爾空間中移動(dòng)的加速度和繞軸旋轉(zhuǎn)的角加速度組成的矩陣，a_i為6*1的矩陣，矩陣a_i中前三行是機(jī)器人末端在笛卡爾空間中移動(dòng)的加速度，后三行是機(jī)器人末端在笛卡爾空間中繞軸旋轉(zhuǎn)的角加速度；F_zmax表示能夠承受的最大六維力和力矩信息，F(xiàn)_zmax是一個(gè)由三維力和三維力矩組成的矩陣，a_max表示機(jī)器人末端允許的最大加速度，F(xiàn)_d表示期望跟蹤的六維力，f表示圖5中所示的函數(shù)，S表示對(duì)角線函數(shù)為0或1的對(duì)角矩陣。

本文選取曲線③為例，即線性函數(shù)關(guān)系，則對(duì)應(yīng)的函數(shù)表達(dá)式如下所示：

上式中的S均為六維度的矩陣，S為一對(duì)角矩陣，指定對(duì)角線元素只能為0或是1。元素設(shè)置為0表示該方向上的位置偏移量不受力偏差信號(hào)的影響，即該方向不受力約束；元素設(shè)置為1表示該方向受到力約束，此處根據(jù)實(shí)際需求設(shè)定。

步驟S3包括以下步驟：

S3.1：在步驟S2的基礎(chǔ)上，首先定義力控周期，定義的原則是：目前控制器與驅(qū)動(dòng)器之間的通訊周期為1ms或是4ms，以1ms或是4ms為基準(zhǔn)控制周期定義合適的時(shí)間片來(lái)實(shí)現(xiàn)一個(gè)力控周期的控制，定義單位時(shí)間片為20個(gè)控制周期，即20ms或是80ms為一個(gè)時(shí)間片，在控制器底層保證了力控周期足夠短。

已知初始位置θ_i、初始速度v_i、初始加速度a_i、初始加加速度j_i＝0、期望的終點(diǎn)速度v_i+1、期望的終點(diǎn)加速度a_i+1和期望的終點(diǎn)加加速度j_i+1＝0，建立如下的五次多項(xiàng)式方程：

其中，w₀,w₁,w₂,w₃,w₄,w₅為五次多項(xiàng)式的系數(shù)，根據(jù)“某一時(shí)刻末的加速度與下一時(shí)刻初的加速度連續(xù)”的約束條件得到對(duì)應(yīng)如下所示的約束方程。

根據(jù)上述約束條件可以計(jì)算得到五次多項(xiàng)式的系數(shù)，對(duì)應(yīng)的加速度控制曲線如說(shuō)明附圖6所示。進(jìn)而推導(dǎo)出對(duì)應(yīng)的位置、加速度和加加速度的函數(shù)，具體表達(dá)式如下所示：

式(5)中，θ_i為初始位置，v_i為初始速度，a_i為初始加速度，a_i+1為期望的終點(diǎn)速度，t為歸一化的時(shí)間，T為機(jī)器人控制器與伺服驅(qū)動(dòng)器的通訊周期；根據(jù)式(5)求得位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；

S3.2：判斷a_i+1與a_i之差的絕對(duì)值是否超過(guò)a_max，v_i的絕對(duì)值是否超過(guò)V_max，a_max表示允許的機(jī)器人末端最大的加速度，V_max表示允許的機(jī)器人末端最大的速度：如果a_i+1與a_i之差的絕對(duì)值超過(guò)a_max，v_i的絕對(duì)值沒(méi)有超過(guò)V_max，則進(jìn)行步驟S3.3；如果a_i+1與a_i之差的絕對(duì)值超過(guò)a_max，v_i的絕對(duì)值也超過(guò)V_max，則進(jìn)行步驟S3.4；

S3.3：判斷a_i+1與a_i的大?。喝绻鸻_i＞a_i+1，則根據(jù)式(6)重新計(jì)算位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；如果a_i＜a_i+1，則根據(jù)式(7)重新計(jì)算位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；

S3.4：根據(jù)式(8)重新計(jì)算位置函數(shù)θ(t)、速度函數(shù)v(t)、加速度函數(shù)a(t)和加加速度函數(shù)j(t)；

本具體實(shí)施方式以?xún)膳_(tái)ESTUN ER16工業(yè)機(jī)器人為實(shí)驗(yàn)對(duì)象，其負(fù)載均為16Kg，一臺(tái)作為主導(dǎo)機(jī)器人，一臺(tái)作為輔助機(jī)器人?，F(xiàn)只考慮輔助機(jī)器人的情況，假定初始位置θ_i＝0、初始速度v_i＝0、初始加速度a_i＝0、初始加加速度j_i＝0、期望的終點(diǎn)加速度a_i+1＝500mm/s²和期望的終點(diǎn)加加速度(j_i+1＝0)，對(duì)應(yīng)的位置、速度、加速度和加加速度在單位時(shí)間片內(nèi)的曲線圖如圖7所示。

將上述按照說(shuō)明書(shū)附圖1搭建雙臂系統(tǒng)，按說(shuō)明書(shū)附圖2中的層次將上述算法在控制器中予以實(shí)現(xiàn)，即可機(jī)器人牽引機(jī)器人的效果。