基于卡爾曼濾波的機(jī)器人力控制

王憲倫，孫旭祥，丁文壯，段奕林

(青島科技大學(xué) 機(jī)電工程學(xué)院, 山東 青島 266061)

0 引言

隨著社會(huì)需求的不斷提升，機(jī)器人已經(jīng)不再局限于在特定環(huán)境下進(jìn)行簡(jiǎn)單作業(yè)。機(jī)器人應(yīng)用領(lǐng)域也在不斷擴(kuò)展，像公共服務(wù)領(lǐng)域的服務(wù)機(jī)器人，醫(yī)療領(lǐng)域的手術(shù)機(jī)器人，康復(fù)機(jī)器人等。這些領(lǐng)域都要求機(jī)器人能夠?qū)?fù)雜的工作環(huán)境做出正確快速的響應(yīng)，所以機(jī)器人與環(huán)境之間的交互能力就顯得尤為重要。僅從機(jī)器人已經(jīng)廣泛應(yīng)用的工業(yè)領(lǐng)域來講，其作業(yè)形式也日趨多樣化，像打磨、拋光、裝配等接觸式作業(yè)除了有位置精度要求外，還需要對(duì)機(jī)器人與工件的接觸力進(jìn)行控制。因此，設(shè)計(jì)出能夠適應(yīng)復(fù)雜且未知環(huán)境的機(jī)器人力控制方法也就受到了越來越多的重視。

文獻(xiàn)[1]結(jié)合機(jī)器人位姿、力和力矩混合控制和Kalman狀態(tài)觀測(cè)器的優(yōu)點(diǎn)，提出了對(duì)系統(tǒng)干擾具有較強(qiáng)抑制能力的力/位混合控制方法。文獻(xiàn)[2]在室內(nèi)復(fù)雜環(huán)境下，采用擴(kuò)展卡爾曼濾波器(EKF)對(duì)機(jī)器人姿態(tài)進(jìn)行實(shí)時(shí)估計(jì)，實(shí)現(xiàn)慣性測(cè)量單元(IMU)姿態(tài)信息與RF2O數(shù)據(jù)的融合,達(dá)到降低系統(tǒng)誤差的目的。文中主要研究了機(jī)器人末端與復(fù)雜環(huán)境存在實(shí)時(shí)接觸力情況下，針對(duì)現(xiàn)有的阻抗控制在力信號(hào)采集過程中存在反饋偏差較大的問題，通過卡爾曼濾波器對(duì)返回的力信號(hào)進(jìn)行處理，提高系統(tǒng)的精確性與魯棒性。

1 機(jī)器人與環(huán)境接觸動(dòng)力學(xué)模型

機(jī)器人動(dòng)力學(xué)是機(jī)器人進(jìn)行跟蹤控制研究的基礎(chǔ)，合適的動(dòng)力學(xué)模型能夠幫助機(jī)器人更加準(zhǔn)確、快速地完成作業(yè)任務(wù)。當(dāng)機(jī)器人末端與外部環(huán)境接觸時(shí)，機(jī)器人的關(guān)節(jié)空間的動(dòng)力學(xué)表達(dá)式為[3]

(1)

其中：qRn為n自由度機(jī)器人的關(guān)節(jié)位移；為機(jī)器人的關(guān)節(jié)角速度；M(q)為n×n維的慣性矩陣；表示機(jī)器人的離心力、哥氏力矩陣；G(q)為機(jī)器人的關(guān)節(jié)連桿重力向量；τ為機(jī)器人的各軸輸出力矩；τe為機(jī)器人末端與環(huán)境的接觸力/力矩。

關(guān)節(jié)角速度與機(jī)器人末端速度關(guān)系可表示為

(2)

其中Jq(q)為機(jī)器人的雅可比矩陣，表示機(jī)器人末端位移與各關(guān)節(jié)位移之間的關(guān)系式。機(jī)器人末端位移的加速度為

(3)

則機(jī)器人末端速度、加速度與機(jī)器人雅可比矩陣的關(guān)系為：

(4)

(5)

由上述推導(dǎo)過程式(1)、式(4)、式(5)可以得到機(jī)器人與環(huán)境接觸時(shí)的動(dòng)力學(xué)方程為

(6)

2 卡爾曼濾波器模型

針對(duì)機(jī)器人傳感器在返回?cái)?shù)據(jù)時(shí)產(chǎn)生的噪聲干擾，文中采用卡爾曼濾波器估計(jì)控制過程中機(jī)器人狀態(tài)，預(yù)測(cè)下一時(shí)刻狀態(tài)，使運(yùn)動(dòng)過程中的估計(jì)均方差最小，且卡爾曼濾波器在軌跡預(yù)測(cè)方向有較好的應(yīng)用[4-9]，不需要對(duì)機(jī)器人各種誤差進(jìn)行精確的數(shù)學(xué)建模便可估計(jì)期望動(dòng)作。

卡爾曼濾波器的第一個(gè)公式為狀態(tài)預(yù)測(cè)公式

(7)

(8)

第三個(gè)公式為

Zt=H·xt+v

(9)

其中：H為觀測(cè)矩陣；v是觀測(cè)誤差；Zt為當(dāng)前時(shí)刻的觀測(cè)值。

系統(tǒng)狀態(tài)更新的公式為

(10)

(11)

最佳估計(jì)值的噪聲分布為

(12)

圖1 基于卡爾曼濾波的力控制

(13)

控制模型內(nèi)的狀態(tài)方程為

(14)

其中：y(t)為系統(tǒng)輸出，是機(jī)器人末端與外部相互作用力；wr、wo為系統(tǒng)噪聲，和觀測(cè)噪聲均認(rèn)為是符合期望為u=0,方差σ2分別為Q、R正態(tài)分布；x(t)=[x1(t)x2(t)]T；A=[0Kd]T；B=[0 1； 0 -Kv]T；系統(tǒng)分量x1(t)、x2(t)為機(jī)器人的受力情況與力相對(duì)于時(shí)間的一階導(dǎo)數(shù)；系統(tǒng)的輸入是u(t-Td)，為系統(tǒng)速度。

3 仿真實(shí)驗(yàn)

3.1 力控制模塊

通過MATLAB軟件中的Simulink來搭建機(jī)器人力控制模塊[10-12]，控制模型如圖2所示，給定期望接觸力為Fd=16.75N，環(huán)境剛度ke=3000N/m，Md=20,Bd=600,系統(tǒng)響應(yīng)時(shí)間Td=0.001s，觀測(cè)狀態(tài)協(xié)方差矩陣協(xié)方差Rk=0.001 5，系統(tǒng)測(cè)量噪聲矩陣型方差Qk=0.002，采樣時(shí)間T=10Td=0.01s，假設(shè)力傳感器的測(cè)量噪聲符合隨機(jī)。

圖2 機(jī)器人力控制模塊

得到經(jīng)典阻抗控制與基于卡爾曼濾波的力控制仿真的力跟蹤結(jié)果如圖3所示。由圖3可以得知基于卡爾曼濾波的力控制可以有效地降低噪聲，將力控制在期望值附近，與經(jīng)典阻抗力控制相比，卡爾曼濾波器有較強(qiáng)的魯棒性可實(shí)現(xiàn)預(yù)測(cè)和抑制噪聲。

圖3 力控制結(jié)果對(duì)比

3.2 可視化仿真

采用某公司KR60系列L45 HA 機(jī)器人作為仿真對(duì)象，結(jié)合MATLAB中的Simscape Multibody工具箱與Simulink中的控制模塊可以實(shí)現(xiàn)機(jī)械系統(tǒng)的可視化仿真，利用基于卡爾曼濾波的力控制模型對(duì)目標(biāo)工件進(jìn)行外輪廓軌跡跟蹤仿真，并與經(jīng)典阻抗控制模型進(jìn)行比對(duì)，機(jī)器人末端力采集位置為力傳感器的形心，如圖4所示。圖中機(jī)械臂末端坐標(biāo)系即為力采集位置的坐標(biāo)系。

圖4 力采集位置

1)平面軌跡跟蹤實(shí)驗(yàn)

設(shè)定目標(biāo)工件的剛度ke=500N/m，期望力0N，采樣時(shí)間T=0.01s，卡爾曼濾波的阻抗控制中各參數(shù)觀測(cè)狀態(tài)協(xié)方差矩陣協(xié)方差Rk=0.001 5，系統(tǒng)測(cè)量噪聲矩陣型方差Qk=0.002，力采集的噪聲在區(qū)間(-10N,20N)內(nèi)均勻分布，在力采集坐標(biāo)系中z軸兩組仿真結(jié)果如圖5、圖6所示。

圖5 經(jīng)典阻抗仿真結(jié)果

圖6 卡爾曼濾波力控制仿真結(jié)果

圖5為經(jīng)典阻抗控制模型的力采集結(jié)果，圖6為基于卡爾曼濾波的力控制采集結(jié)果。從實(shí)際的仿真結(jié)果中可以看出基于卡爾曼濾波的力控制能夠有效降低噪聲對(duì)力控制的影響且能盡量減小機(jī)器人末端與工件的接觸力并且能保持位置精確。

2)曲面軌跡跟蹤實(shí)驗(yàn)

用同樣的方法驗(yàn)證機(jī)器人的力控制策略在曲面上軌跡跟蹤的效果，設(shè)定目標(biāo)工件的剛度ke=600N/m，期望力為F=15N，機(jī)器人工具與目標(biāo)工件的動(dòng)摩擦因數(shù)μ=0.07，采樣時(shí)間T=0.04s，卡爾曼濾波的阻抗控制中各參數(shù)觀測(cè)狀態(tài)協(xié)方差矩陣協(xié)方差Rk=0.003 5，系統(tǒng)測(cè)量噪聲矩陣型方差Qk=0.01，力采集的噪聲在區(qū)間(10N,20N)內(nèi)均勻分布，并在目標(biāo)工件表面添加凹坑、凸起等表面缺陷。仿真過程如圖7所示。

圖7 曲面上的軌跡跟蹤

在力采集坐標(biāo)系中z軸兩組仿真結(jié)果如圖8、圖9所示。

圖8 經(jīng)典阻抗曲面仿真結(jié)果

圖8為經(jīng)典阻抗控制模型的力采集結(jié)果，圖9為基于卡爾曼濾波的力控制采集結(jié)果。從兩組結(jié)果中可以看出基于卡爾曼濾波的力控制在曲面上同樣可以降低噪聲對(duì)力控制的影響，保持末端接觸力的變化較小。

圖9 卡爾曼濾波力控制曲面仿真結(jié)果

4 結(jié)語

考慮到機(jī)器人執(zhí)行復(fù)雜作業(yè)任務(wù)時(shí)與環(huán)境之間的交互問題，本文采用基于卡爾曼濾波的機(jī)器人力控制方法改善了經(jīng)典阻抗控制力信號(hào)產(chǎn)生噪聲較大的問題，通過分別搭建平面軌跡跟蹤平臺(tái)和曲面軌跡跟蹤平臺(tái)仿真驗(yàn)證了所用力控制方法的有效性，有效提高了機(jī)器人與環(huán)境交互時(shí)機(jī)器人力控制的柔順性。