基于卡爾曼濾波的機器人力控制

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

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

0 引言

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

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

1 機器人與環(huán)境接觸動力學模型

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

(1)

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

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

(2)

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

(3)

則機器人末端速度、加速度與機器人雅可比矩陣的關系為：

(4)

(5)

由上述推導過程式(1)、式(4)、式(5)可以得到機器人與環(huán)境接觸時的動力學方程為

(6)

2 卡爾曼濾波器模型

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

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

(7)

(8)

第三個公式為

Zt=H·xt+v

(9)

其中：H為觀測矩陣；v是觀測誤差；Zt為當前時刻的觀測值。

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

(10)

(11)

最佳估計值的噪聲分布為

(12)

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

(13)

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

(14)

其中：y(t)為系統(tǒng)輸出，是機器人末端與外部相互作用力；wr、wo為系統(tǒng)噪聲，和觀測噪聲均認為是符合期望為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)為機器人的受力情況與力相對于時間的一階導數(shù)；系統(tǒng)的輸入是u(t-Td)，為系統(tǒng)速度。

3 仿真實驗

3.1 力控制模塊

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

圖2 機器人力控制模塊

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

圖3 力控制結果對比

3.2 可視化仿真

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

圖4 力采集位置

1)平面軌跡跟蹤實驗

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

圖5 經典阻抗仿真結果

圖6 卡爾曼濾波力控制仿真結果

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

2)曲面軌跡跟蹤實驗

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

圖7 曲面上的軌跡跟蹤

在力采集坐標系中z軸兩組仿真結果如圖8、圖9所示。

圖8 經典阻抗曲面仿真結果

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

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

4 結語

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