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

背景技術(shù)：

手動牽引示教機器人操作是人機協(xié)作中不可或缺的一個環(huán)節(jié)，是指示教人員在笛卡爾空間中或是在關節(jié)空間中拖動機器人運動到示教點的操作。牽引示教時，機器人末端或關節(jié)需實時并精確地跟蹤人拖動的意圖方向，并使得機器人能夠做出期望的運動狀態(tài)。傳統(tǒng)牽引示教機器人的兩種直觀的解決方案：基于現(xiàn)有的控制器二次開發(fā)接口做導納控制或是基于力矩模式做無力矩傳感器的零力拖動示教。然而，這兩種方案存在以下缺點：①由于接口開發(fā)程度低，只有部分的接口函數(shù)可用，例如點到點的運動指令(MovJ或是MoveL)，無法對機器人運動過程中的速度和加速度進行控制；②基于現(xiàn)有的控制器二次開發(fā)接口，只能建立力偏差F與位移量x的關系，因此在每個力控周期內(nèi)機器人末端都會經(jīng)歷“加速-勻速-減速”或是“加速-減速”的過程，因此會發(fā)現(xiàn)在牽引時機器人末端的運動不平滑，牽引效果表現(xiàn)出實時牽引效果差和牽引精度低。③基于零力控制的無力矩傳感器的拖動示教而言牽引精度低，需要依賴于精確的動力學模型，況且在低速拖動時，有很多非線性的因素影響了拖動的精度，在無力矩傳感器的情況下很難做到精確的牽引示教，因此也很難用在實際的工業(yè)現(xiàn)場。

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

發(fā)明目的：本發(fā)明的目的是提供一種能夠解決現(xiàn)有技術(shù)中存在的缺陷的基于阻抗模型的高精度牽引示教機器人的速度控制方法。

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

本發(fā)明所述的基于阻抗模型的高精度牽引示教機器人的速度控制方法，包括以下步驟：

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

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

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

S4：根據(jù)逆運動學進而求得關節(jié)空間中的關節(jié)角度函數(shù)；

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

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

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

式(1)中，F(xiàn)_zmax表示能夠承受的最大六維力，V_max表示允許的機器人末端最大的速度，F(xiàn)_d表示期望跟蹤的六維力，F(xiàn)為力的偏差量，f為單調(diào)函數(shù)，S表示對角線函數(shù)為0或1的6*6的對角矩陣。

進一步，所述步驟S3包括以下步驟：

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

式(2)中，θ_i為初始位置，v_i為初始速度，v_i+1為期望的終點速度，t為歸一化的時間，T為機器人控制器與伺服驅(qū)動器的通訊周期；

S3.2：判斷期望的終點速度v_i+1與初始速度v_i之差的絕對值是否超過V_max：如果超過V_max，則進行步驟S3.3；否則，結(jié)束；

S3.3：判斷v_i是否大于v_i+1：如果是，則根據(jù)式(3)重新計算位置函數(shù)θ(t)、速度函數(shù)v(t)和加速度函數(shù)a(t)；如果不是，則根據(jù)式(4)重新計算位置函數(shù)θ(t)、速度函數(shù)v(t)和加速度函數(shù)a(t)；

進一步，所述步驟S5中，控制器的總線為EtherCAT總線。

進一步，所述機器人控制器從上到下分為六個層次，即用戶層、六維力信號采集和處理層、阻抗控制層、速度控制層、關節(jié)插補層和EtherCAT總線通訊層；其中，用戶層、六維力信號采集和處理層、阻抗控制層對外開放，速度控制層、關節(jié)插補層和EtherCAT總線通訊層不對外開發(fā)。

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

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

1)改善牽引的實時跟隨效果；相比于傳統(tǒng)的基于導納控制的牽引而言，本發(fā)明可以實現(xiàn)實時的跟蹤效果，且機器人跟隨時運動平滑；

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

3)提高了牽引的精度；相比于傳統(tǒng)的基于零力控制的無力矩傳感器的拖動示教而言牽引精度更高，無力矩傳感器拖動示教的問題是力矩雖然經(jīng)過濾波后值是相對穩(wěn)定的，但是從伺服驅(qū)動器中讀取的力矩值精度不高，因為沒有力矩傳感器做基準，因此只能達到拖動示教的趨勢但是不能達到高精度的牽引示教。

附圖說明

圖1為本發(fā)明具體實施方式的牽引示教機器人的速度控制架構(gòu)圖；

圖2為本發(fā)明具體實施方式的手動牽引示教下單位力控周期的流程圖；

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

圖4為本發(fā)明具體實施方式的基于阻抗模型的F與v的對應關系圖；

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

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

具體實施方式

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

本具體實施方式公開了一種基于阻抗模型的高精度牽引示教機器人的速度控制方法，包括以下步驟：

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

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

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

S4：根據(jù)逆運動學進而求得關節(jié)空間中的關節(jié)角度函數(shù)；

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

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

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

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

本具體實施方式基于現(xiàn)有的自主研發(fā)的開放式控制器，分析了手動牽引示教的本質(zhì)特性，包括人牽引機器人時的行為特性和機器人在跟隨過程中需滿足的特點。圖2為手動牽引示教下單位力控周期的流程圖。

人的行為具有以下特征：

1)行為的離散性：牽引行為是離散且不連續(xù)的，可能隨時牽引，也可能隨時放開；

2)力的時變性：牽引力是時變的，可能某一時刻力大，某一時刻力小。

結(jié)合力牽引時機器人的物理特性和人的行為特征，為達到好的跟隨效果，機器人跟隨時需滿足如下特點：

1)連續(xù)性：機器人的運動應該連續(xù)和平滑，防止速度突變或是加速度突變；

2)精確性：機器人時刻跟隨到人所賦予的牽引力，精確性主要體現(xiàn)在松開的一瞬間，機器人需以最小的距離停止；

3)實時性：能夠?qū)崟r的根據(jù)力反饋量做出響應，例如當力大時，以相對較大的速度進行跟隨，當力小時，以相對較小的速度進行跟隨；

4)安全性：機器人的最大運動速度和加速度應滿足人機協(xié)作時的國際安全規(guī)范。

根據(jù)上述的分析結(jié)果，分析出如下約束條件：力偏差F變大時，機器人的運動速度v隨之變大；力偏差F變小時，機器人的運動速度v隨之變??；當力偏差F為0時，機器人應該靜止，即速度v等于0，對應的約束表達式如下所示：

式(1)中，F(xiàn)_zmax表示能夠承受的最大六維力，V_max表示允許的機器人末端最大的速度，F(xiàn)_d表示期望跟蹤的六維力，v_i為初始速度。

根據(jù)上述的分析可知，需要建立了力偏差F與機器人運動速度v的變換關系，以單軸為例，具體的關系式如下所示：

此模型根據(jù)用戶需求進行設定，如說明附圖4所示，但是一定要滿足單調(diào)函數(shù)關系。將單軸擴展為六軸，對應的表達式如下所示：

式(3)中，V_i為機器人末端在笛卡爾空間中移動的速度和繞軸旋轉(zhuǎn)的角速度組成的矩陣，V_i為6*1的矩陣，矩陣V_i中前三行是機器人末端在笛卡爾空間中移動的速度，后三行是機器人末端在笛卡爾空間中繞軸旋轉(zhuǎn)的角速度；f為單調(diào)函數(shù)，S表示對角線函數(shù)為0或1的6*6的對角矩陣。V_i、V_max、F、F_d、F_zmax和S均為六維度的矩陣，S為一對角矩陣，指定對角線元素只能為0或是1。元素設置為0表示該方向上的位置偏移量不受力偏差信號的影響，即該方向不被牽引；元素設置為1表示該方向力牽引有效。用戶可根據(jù)實際需求進行設定，比如設定S的對角線為[0 0 1 0 0 0]，表示只有z軸方向受到力的約束，即只有z方向的牽引，若設定其對角線為[1 1 1 1 1 1]，表示六個維度均受到力的約束，即可以實現(xiàn)六個維度的牽引。

目前控制器與驅(qū)動器之間的通訊周期為1ms或是4ms，以1ms或是4ms為基準控制周期定義合適的時間片來實現(xiàn)一個力控周期的控制，定義單位時間片時需考慮以下三點：①與力傳感器的傳輸頻率匹配；②機器人加速段或是減速段平滑；③能夠在指定的時間和電機允許的加速度范圍內(nèi)加速到指定的速度。綜合上述三點，定義單位時間片為20個控制周期，即20ms或是80ms為一個時間片。在控制器底層保證了力控周期足夠短，從而可以實現(xiàn)實時的牽引。

已知初始位置θ_i、初始速度v_i、初始加速度(a_i＝0)、期望的終點速度v_i+1和期望的終點加速度(a_i+1＝0)建立如下的四次多項式方程：

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

為實現(xiàn)平滑的牽引效果，需要對速度進行四次多項式的S型插補規(guī)劃，具體如說明附圖5所示，根據(jù)運動過程中的速度連續(xù)、初始位置(θ_i)、初始速度(v_i)、初始加速度(a_i＝0)、期望的終點速度(v_i+1)和期望的終點加速度(a_i+1＝0)等約束條件，確定四次多項式的系數(shù)，并推導出對應的位置、速度和加速度的函數(shù)，具體表達式如下所示：

式(6)中，θ_i為初始位置，v_i為初始速度，v_i+1為期望的終點速度，t為歸一化的時間，T為機器人控制器與伺服驅(qū)動器的通訊周期。

由說明書附圖中可知，曲線①～⑤為(i+1)時刻5種典型的速度曲線圖，但是⑥和⑦屬于兩種特殊情況：

對于曲線⑥而言，意味著力F反向，此時需要對初始速度與期望的終點速度進行比較，保證最大的速度變化量不超過Vmax，若超過Vmax，此時變化速度變化量需要按Vmax重新計算，對應的位置函數(shù)、速度函數(shù)和加速度函數(shù)如下所示。

對于曲線⑦而言，在時刻i機器人末端的運動速度v_i達到最大速度v_max，此時人手突然放開，下一個時刻i+1機器人末端的期望運動速度為0，此時機器人以最大的加速度減速，其對應的移動位置量即為手動牽引示教機器人時的最大誤差，當人手并非突然放開而是平滑放開時，手動牽引示教機器人時的誤差可以達到0。

此外，本具體實施方式還在機器人末端安裝六維力傳感器，因為在無外界測量儀器的前提下很難找到基準值，導致結(jié)果是牽引精度低，因此在無力矩傳感器的情況下零力拖動并不適用在實際工業(yè)應用中。

在實際的手動牽引過程時，需考慮人機協(xié)作安全過程中的最大速度、最大加速度等參數(shù)。結(jié)合國內(nèi)外眾多人機協(xié)作安全和危險測試的研究結(jié)果，機器人的運動最大速度應該結(jié)合工業(yè)機器人的功率和負載而定，建議5Kg負載以下的機器人最大速度為0.25m/s，5Kg-10Kg負載的機器人最大的運行速度為0.2m/s，10Kg-20Kg負載的工業(yè)機器人的最大運行速度為0.1m/s，20Kg負載以上的工業(yè)機器人不建議采用牽引的方式進行示教。

本發(fā)明以ESTUN ER16工業(yè)機器人為實驗對象，其負載為16Kg，因此限定其最大的運行速度為0.1m/s，下面給出其對應的最大的加速度和單位時間內(nèi)的最大位移結(jié)果，如圖6所示。由圖6可知，最大的加速度為0.15m/s²，單位時間內(nèi)的最大位移為0.05m。