本發(fā)明涉及一種基于力矩觀測和摩擦辨識(shí)的機(jī)器人接觸力檢測方法，利用機(jī)器人動(dòng)力學(xué)方程和廣義動(dòng)量構(gòu)造力矩觀測器來檢測機(jī)器人末端接觸力。

背景技術(shù)：

隨著制造業(yè)從“自動(dòng)化”、“無人化”向“智能化”、“協(xié)作化”的轉(zhuǎn)變，越來越多的生產(chǎn)領(lǐng)域要求工人和機(jī)器人共享工作區(qū)協(xié)同進(jìn)行工作。在人機(jī)共融生產(chǎn)模式下，機(jī)器人越來越多的要和周圍環(huán)境接觸，需要對(duì)接觸力進(jìn)行嚴(yán)格的控制，因此首先需要檢測機(jī)器人運(yùn)行過程中末端執(zhí)行器受到的接觸力。傳統(tǒng)的接觸力檢測主要是基于外部力傳感器來實(shí)現(xiàn)的，不但增加了成本，還使得機(jī)器人控制系統(tǒng)更加復(fù)雜。目前比較實(shí)用的方法是基于機(jī)器人自身傳感器的接觸力檢測方法。這種方法不需要任何外部傳感器，大大降低了生產(chǎn)成本，可以直接應(yīng)用于機(jī)器人上。

中國專利CN104050355A公開了一種基于H_∞濾波的機(jī)械臂外力估計(jì)方法，該專利采用模擬低通濾波器對(duì)動(dòng)力學(xué)方程進(jìn)行中信號(hào)進(jìn)行濾波，推導(dǎo)出不顯含加速度的動(dòng)力學(xué)模型；將外力看作是狀態(tài)空間方程的狀態(tài)變量，基于H_∞濾波估計(jì)外力；再利用雙線性變換的方法設(shè)計(jì)數(shù)字濾波器，從而實(shí)現(xiàn)在實(shí)際機(jī)械臂系統(tǒng)中的應(yīng)用。該專利提出的方法不能對(duì)機(jī)器人關(guān)節(jié)摩擦誤差和系統(tǒng)模型誤差做出補(bǔ)償，理論上可行，于實(shí)際應(yīng)用還有待改進(jìn)。

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

針對(duì)現(xiàn)有技術(shù)存在的缺陷，本發(fā)明的目的是提出一種基于力矩觀測和摩擦辨識(shí)的機(jī)器人接觸力檢測方法，基于機(jī)器人動(dòng)力學(xué)方程和廣義動(dòng)量構(gòu)造關(guān)節(jié)接觸力矩觀測量，利用構(gòu)造的關(guān)節(jié)摩擦力矩觀測量來辨識(shí)機(jī)器人關(guān)節(jié)摩擦參數(shù)，將理論運(yùn)行狀態(tài)下的觀測量視為系統(tǒng)模型誤差，在剔除摩擦和系統(tǒng)模型誤差的影響后得到了準(zhǔn)確的接觸力檢測值。

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

一種基于力矩觀測和摩擦辨識(shí)的機(jī)器人接觸力檢測方法，具體步驟如下：

步驟一：由機(jī)器人動(dòng)力學(xué)方程和廣義動(dòng)量構(gòu)造關(guān)節(jié)接觸力矩觀測量，無需采集機(jī)器人關(guān)節(jié)加速度信息，通過構(gòu)造觀測量來非直接檢測關(guān)節(jié)接觸力矩，具體為：

機(jī)器人動(dòng)力學(xué)方程的一般形式如下：

其中q,是機(jī)器人關(guān)節(jié)的角度矢量，角速度矢量和角加速度矢量，M(q)是機(jī)器人的慣性矩陣，是機(jī)器人的哥氏力和離心力矢量，G(q)是重力矢量，τ_j是機(jī)器人關(guān)節(jié)驅(qū)動(dòng)力矩；

當(dāng)機(jī)器人和周圍環(huán)境發(fā)生接觸時(shí)，接觸力的作用會(huì)等效到機(jī)器人的關(guān)節(jié)處，產(chǎn)生關(guān)節(jié)接觸力矩，通過觀測機(jī)器人關(guān)節(jié)處的接觸力矩來檢測末端接觸力，機(jī)器人與周圍環(huán)境發(fā)生接觸時(shí)的動(dòng)力學(xué)方程為：

其中τ_e是末端接觸力等效到關(guān)節(jié)處的關(guān)節(jié)接觸力矩；

通過式(2)得出關(guān)節(jié)接觸力矩為：

式(3)中，機(jī)器人幾何參數(shù)和動(dòng)力學(xué)參數(shù)通過三維模型得到，關(guān)節(jié)角度和關(guān)節(jié)角速度通過機(jī)器人關(guān)節(jié)電機(jī)編碼器獲取，機(jī)器人關(guān)節(jié)電機(jī)驅(qū)動(dòng)力矩通過關(guān)節(jié)驅(qū)動(dòng)電流得到，在忽略機(jī)器人模型誤差和其他擾動(dòng)的情況下，機(jī)器人關(guān)節(jié)驅(qū)動(dòng)力矩和關(guān)節(jié)電機(jī)驅(qū)動(dòng)力矩的關(guān)系為：

τ_j＝λτ_m-τ_f (4)

其中λ為機(jī)器人關(guān)節(jié)的減速比，τ_f為關(guān)節(jié)摩擦力矩，關(guān)節(jié)驅(qū)動(dòng)力矩通過關(guān)節(jié)電機(jī)驅(qū)動(dòng)力矩和關(guān)節(jié)摩擦力矩求得；為了計(jì)算關(guān)節(jié)接觸力矩，需要解決關(guān)節(jié)角加速度和關(guān)節(jié)摩擦力矩不能直接獲取這個(gè)問題，為了解決機(jī)器人關(guān)節(jié)角加速度不能直接獲取的問題，引入機(jī)器人的廣義動(dòng)量p：

式(5)兩邊微分得到：

因?yàn)镸(q)是正定對(duì)稱矩陣，是斜對(duì)稱矩陣，所以有：

將式(2)和式(7)代入式(6)得到：

由式(8)看出，動(dòng)量微分與接觸產(chǎn)生的關(guān)節(jié)接觸力矩具有解耦性，并且與關(guān)節(jié)角加速度無關(guān)，基于機(jī)器人廣義動(dòng)量來構(gòu)造關(guān)節(jié)接觸力矩觀測量：

其中r為機(jī)器人關(guān)節(jié)接觸力矩τ_e的觀測量，K₁，K₂為系數(shù)矩陣；

步驟二：在機(jī)器人未與周圍環(huán)境接觸時(shí)將關(guān)節(jié)摩擦力據(jù)視為外部擾動(dòng)構(gòu)造摩擦力據(jù)觀測量，采用指數(shù)摩擦模型和正余弦函數(shù)組合的摩擦模型來辨識(shí)摩擦參數(shù)，具體為：

機(jī)器人未發(fā)生接觸時(shí)考慮摩擦作用的機(jī)器人動(dòng)力學(xué)模型為：

基于接觸力矩觀測的思想構(gòu)造摩擦力矩觀測量為：

其中N為系數(shù)矩陣，f為機(jī)器人關(guān)節(jié)摩擦力矩觀測量；

將式(11)微分后做拉式變換得到：

由式(12)看出，當(dāng)N取足夠大的值時(shí)，認(rèn)為摩擦力矩觀測量在誤差允許的范圍內(nèi)能夠表示實(shí)際的摩擦力矩；

τ_f＝f (13)

機(jī)器人關(guān)節(jié)摩擦模型用指數(shù)摩擦模型來表示：

其中τ_c為庫倫摩擦力，τ_s為靜摩擦力，為Stribeck速度，ζ為經(jīng)驗(yàn)參數(shù)，σ為粘性摩擦參數(shù)；由于機(jī)器人關(guān)節(jié)存在諧波齒輪傳動(dòng)系統(tǒng)，其摩擦不但包含與速度有關(guān)的庫倫摩擦與粘性摩擦，還存在與關(guān)節(jié)角度有關(guān)的周期性波動(dòng)，采用正余弦函數(shù)組合的形式來描敘：

τ_f(q)＝K₁sin(q)+K₂cos(q)+K₃sin(2q)+K₄cos(2q) (15)

有諧波齒輪傳動(dòng)的關(guān)節(jié)型機(jī)器人的關(guān)節(jié)摩擦模型用下式表示：

其中K_f等于0表示關(guān)節(jié)處沒有諧波減速器，K_f等于1表示關(guān)節(jié)處存在諧波傳動(dòng)；

利用式(13)關(guān)節(jié)摩擦力矩觀測量來辨識(shí)摩擦參數(shù)，將摩擦力矩觀測量τ_f與式(16)計(jì)算的摩擦力矩的差值定義為目標(biāo)函數(shù)，對(duì)摩擦參數(shù)尋優(yōu)使目標(biāo)函數(shù)最?。?/p>

其中n為總的采樣點(diǎn)數(shù)，這是一個(gè)非線性優(yōu)化問題，采用遺傳算法來求解得到關(guān)節(jié)摩擦參數(shù)；

利用辨識(shí)的摩擦模型參數(shù)計(jì)算關(guān)節(jié)摩擦力矩，考慮摩擦作用后的接觸力矩觀測量為：

步驟三：將理論運(yùn)行狀態(tài)下的觀測值視為系統(tǒng)模型誤差，通過消除系統(tǒng)模型誤差來減小接觸力矩觀測值誤差，根據(jù)關(guān)節(jié)接觸力矩和機(jī)器人力雅克比矩陣實(shí)時(shí)計(jì)算末端接觸力，具體為：

系統(tǒng)模型誤差記為r_d(t)，表示r(t)中的角度、角速度變量都是軌跡規(guī)劃得到的理論值，考慮系統(tǒng)模型誤差后的接觸力矩觀測量為：

ξ(t)＝r(t)-r_d(t) (19)

得到機(jī)器人關(guān)節(jié)接觸力矩觀測量后由式(20)得到末端接觸力為：

F_e＝(J^T(q))^-1ξ(t) (20)

其中F_e為末端接觸力，J^T(q)為機(jī)器人雅克比矩陣的轉(zhuǎn)置矩陣。

本發(fā)明與現(xiàn)有技術(shù)相比較，具有如下突出的實(shí)質(zhì)性特點(diǎn)和顯著的優(yōu)點(diǎn)：

本發(fā)明方法不需要在機(jī)器人上添加額外的傳感器，僅需要采集機(jī)器人關(guān)節(jié)電機(jī)電流、關(guān)節(jié)角度信息即可檢測接觸力；對(duì)于機(jī)器人關(guān)節(jié)復(fù)雜的摩擦模型采用觀測值辨識(shí)摩擦參數(shù)，無需直接測量摩擦力矩；將理論運(yùn)行狀態(tài)下的觀測值視為系統(tǒng)模型誤差并剔除其影響，大大提高了檢測精度。

附圖說明

圖1是二自由度機(jī)器人末端接觸力檢測示意圖。

圖2為MTALAB和ADAMS聯(lián)合仿真系統(tǒng)示意圖。

圖3是機(jī)器人關(guān)節(jié)摩擦力矩觀測值與實(shí)際值的對(duì)比圖。

圖4是機(jī)器人末端接觸力檢測值與實(shí)際值的對(duì)比圖。

具體實(shí)施方式

以下將結(jié)合附圖和實(shí)施例對(duì)本發(fā)明的具體實(shí)施作進(jìn)一步說明。

如圖1所示為本發(fā)明所提出的二自由度機(jī)器人末端接觸力檢測示意圖，機(jī)器人控制器根據(jù)軌跡規(guī)劃得到的期望軌跡計(jì)算出關(guān)節(jié)驅(qū)動(dòng)力矩驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)，關(guān)節(jié)接觸力矩觀測器根據(jù)反饋回來的機(jī)器人關(guān)節(jié)角度和角速度以及關(guān)節(jié)驅(qū)動(dòng)力矩來觀測關(guān)節(jié)接觸力矩，最后通過雅克比矩陣計(jì)算得到末端接觸力。

一種基于力矩觀測和摩擦辨識(shí)的機(jī)器人接觸力檢測方法，具體步驟如下：

步驟一：由機(jī)器人動(dòng)力學(xué)方程和廣義動(dòng)量構(gòu)造關(guān)節(jié)接觸力矩觀測量，無需采集機(jī)器人關(guān)節(jié)加速度信息，通過構(gòu)造觀測量來非直接檢測關(guān)節(jié)接觸力矩，具體為：

機(jī)器人動(dòng)力學(xué)方程的一般形式如下：

其中q,是機(jī)器人關(guān)節(jié)的角度矢量，角速度矢量和角加速度矢量，M(q)是機(jī)器人的慣性矩陣，是機(jī)器人的哥氏力和離心力矢量，G(q)是重力矢量，τ_j是機(jī)器人關(guān)節(jié)驅(qū)動(dòng)力矩；

當(dāng)機(jī)器人和周圍環(huán)境發(fā)生接觸時(shí)，接觸力的作用會(huì)等效到機(jī)器人的關(guān)節(jié)處，產(chǎn)生關(guān)節(jié)接觸力矩，通過觀測機(jī)器人關(guān)節(jié)處的接觸力矩來檢測末端接觸力，機(jī)器人與周圍環(huán)境發(fā)生接觸時(shí)的動(dòng)力學(xué)方程為：

其中τ_e是末端接觸力等效到關(guān)節(jié)處的關(guān)節(jié)接觸力矩；

通過式(2)得出關(guān)節(jié)接觸力矩為：

式(3)中，機(jī)器人幾何參數(shù)和動(dòng)力學(xué)參數(shù)通過三維模型得到，關(guān)節(jié)角度和關(guān)節(jié)角速度通過機(jī)器人關(guān)節(jié)電機(jī)編碼器獲取，機(jī)器人關(guān)節(jié)電機(jī)驅(qū)動(dòng)力矩通過關(guān)節(jié)驅(qū)動(dòng)電流得到，在忽略機(jī)器人模型誤差和其他擾動(dòng)的情況下，機(jī)器人關(guān)節(jié)驅(qū)動(dòng)力矩和關(guān)節(jié)電機(jī)驅(qū)動(dòng)力矩的關(guān)系為：

τ_j＝λτ_m-τ_f (4)

其中λ為機(jī)器人關(guān)節(jié)的減速比，τ_f為關(guān)節(jié)摩擦力矩，關(guān)節(jié)驅(qū)動(dòng)力矩通過關(guān)節(jié)電機(jī)驅(qū)動(dòng)力矩和關(guān)節(jié)摩擦力矩求得；為了計(jì)算關(guān)節(jié)接觸力矩，需要解決關(guān)節(jié)角加速度和關(guān)節(jié)摩擦力矩不能直接獲取這個(gè)問題，為了解決機(jī)器人關(guān)節(jié)角加速度不能直接獲取的問題，引入機(jī)器人的廣義動(dòng)量p：

式(5)兩邊微分得到：

因?yàn)镸(q)是正定對(duì)稱矩陣，是斜對(duì)稱矩陣，所以有：

將式(2)和式(7)代入式(6)得到：

由式(8)看出，動(dòng)量微分與接觸產(chǎn)生的關(guān)節(jié)接觸力矩具有解耦性，并且與關(guān)節(jié)角加速度無關(guān)，基于機(jī)器人廣義動(dòng)量來構(gòu)造關(guān)節(jié)接觸力矩觀測量：

其中r為機(jī)器人關(guān)節(jié)接觸力矩τ_e的觀測量，K₁，K₂為系數(shù)矩陣；

步驟二：在機(jī)器人未與周圍環(huán)境接觸時(shí)將關(guān)節(jié)摩擦力據(jù)視為外部擾動(dòng)構(gòu)造摩擦力據(jù)觀測量，采用指數(shù)摩擦模型和正余弦函數(shù)組合的摩擦模型來辨識(shí)摩擦參數(shù)，具體為：

機(jī)器人未發(fā)生接觸時(shí)考慮摩擦作用的機(jī)器人動(dòng)力學(xué)模型為：

基于接觸力矩觀測的思想構(gòu)造摩擦力矩觀測量為：

其中N為系數(shù)矩陣，f為機(jī)器人關(guān)節(jié)摩擦力矩觀測量；

將式(11)微分后做拉式變換得到：

由式(12)看出，當(dāng)N取足夠大的值時(shí)，認(rèn)為摩擦力矩觀測量在誤差允許的范圍內(nèi)能夠表示實(shí)際的摩擦力矩；

τ_f＝f (13)

機(jī)器人關(guān)節(jié)摩擦模型用指數(shù)摩擦模型來表示：

其中τ_c為庫倫摩擦力，τ_s為靜摩擦力，為Stribeck速度，ζ為經(jīng)驗(yàn)參數(shù)，σ為粘性摩擦參數(shù)；由于機(jī)器人關(guān)節(jié)存在諧波齒輪傳動(dòng)系統(tǒng)，其摩擦不但包含與速度有關(guān)的庫倫摩擦與粘性摩擦，還存在與關(guān)節(jié)角度有關(guān)的周期性波動(dòng)，采用正余弦函數(shù)組合的形式來描敘：

τ_f(q)＝K₁sin(q)+K₂cos(q)+K₃sin(2q)+K₄cos(2q) (15)

有諧波齒輪傳動(dòng)的關(guān)節(jié)型機(jī)器人的關(guān)節(jié)摩擦模型用下式表示：

其中K_f等于0表示關(guān)節(jié)處沒有諧波減速器，K_f等于1表示關(guān)節(jié)處存在諧波傳動(dòng)；

利用式(13)關(guān)節(jié)摩擦力矩觀測量來辨識(shí)摩擦參數(shù)，將摩擦力矩觀測量τ_f與式(16)計(jì)算的摩擦力矩的差值定義為目標(biāo)函數(shù)，對(duì)摩擦參數(shù)尋優(yōu)使目標(biāo)函數(shù)最?。?/p>

其中n為總的采樣點(diǎn)數(shù)，這是一個(gè)非線性優(yōu)化問題，采用遺傳算法來求解得到關(guān)節(jié)摩擦參數(shù)；

利用辨識(shí)的摩擦模型參數(shù)計(jì)算關(guān)節(jié)摩擦力矩，考慮摩擦作用后的接觸力矩觀測量為：

步驟三：將理論運(yùn)行狀態(tài)下的觀測值視為系統(tǒng)模型誤差，通過消除系統(tǒng)模型誤差來減小接觸力矩觀測值誤差，根據(jù)關(guān)節(jié)接觸力矩和機(jī)器人力雅克比矩陣實(shí)時(shí)計(jì)算末端接觸力，具體為：

系統(tǒng)模型誤差記為r_d(t)，表示r(t)中的角度、角速度變量都是軌跡規(guī)劃得到的理論值，考慮系統(tǒng)模型誤差后的接觸力矩觀測量為：

ξ(t)＝r(t)-r_d(t) (19)

得到機(jī)器人關(guān)節(jié)接觸力矩觀測量后由式(20)得到末端接觸力為：

F_e＝(J^T(q))^-1ξ(t) (20)

其中F_e為末端接觸力，J^T(q)為機(jī)器人雅克比矩陣的轉(zhuǎn)置矩陣。

如圖2所示，為根據(jù)構(gòu)造的接觸力矩觀測量搭建的MATLAB和ADAMS聯(lián)合仿真系統(tǒng)，設(shè)整個(gè)控制系統(tǒng)的控制周期為0.005s。連桿1的長度l₁＝1m,質(zhì)量m₁＝26.5kg，轉(zhuǎn)動(dòng)慣量I_c1＝2.5kgm²,連桿2的長度l₂＝0.8m,質(zhì)量m₂＝21.5kgm²，轉(zhuǎn)動(dòng)慣量I_c2＝1.3kgm²,重力加速度g＝9.81m/s²。在MATLAB中利用樣條曲線規(guī)劃機(jī)器人運(yùn)動(dòng)軌跡，根據(jù)計(jì)算力矩控制算法控制ADAMS中的機(jī)器人運(yùn)動(dòng)。

推導(dǎo)式(1)中二自由度機(jī)械臂動(dòng)力學(xué)方程的一般形式，在MATLAB中編程計(jì)算關(guān)節(jié)驅(qū)動(dòng)力矩，然后根據(jù)式(9)構(gòu)造關(guān)節(jié)接觸力矩觀測量，其中K₁＝2500，K₂＝0.04。

圖3是機(jī)器人關(guān)節(jié)摩擦力矩觀測值與實(shí)際值的對(duì)比圖。在機(jī)器人關(guān)節(jié)處添加摩擦力矩作用，采用本發(fā)明中摩擦力矩觀測方法觀測摩擦力矩，其中實(shí)線表示實(shí)際添加的摩擦力矩，虛線表示觀測的摩擦力矩，從圖中看出實(shí)線和虛線吻合很好，說明本發(fā)明中提出的摩擦力矩觀測方法可行。

圖4是機(jī)器人末端接觸力檢測值與實(shí)際值的對(duì)比圖。在機(jī)器人末端添加接觸力作用，如圖中實(shí)線所示，采用本發(fā)明提出的接觸力檢測方法得到的接觸力如圖中虛線所示?？梢钥闯觯^測的接觸力和實(shí)際加載的接觸力誤差較小，說明本發(fā)明提出的接觸力檢測方法可行。