基于卡爾曼濾波的恒力裝置恒力跟隨方法*

吳凌峰,易建業(yè),謝 暉,2

(1.季華實(shí)驗(yàn)室,佛山 528000;2.大捷智能科技(廣東)有限公司,佛山 528000)

0 引言

機(jī)器人打磨作業(yè)是取代人工打磨和提高打磨效率的重要手段,而在打磨作業(yè)過程中,機(jī)器人的打磨工具與加工件有接觸力要求,需要實(shí)現(xiàn)機(jī)器人的柔順控制。打磨機(jī)器人主動(dòng)柔順控制一般會(huì)采在機(jī)器人控制器中加入強(qiáng)化學(xué)習(xí)、阻抗控制、力/位混合控制等算法[1-4],雖然控制效果和魯棒性優(yōu)良,但普遍存在對(duì)機(jī)器人本體要求高、控制算法復(fù)雜不容易實(shí)現(xiàn)等問題。由于力控制由機(jī)器人末端的恒力裝置實(shí)現(xiàn),“機(jī)器人+恒力裝置”的機(jī)器人打磨系統(tǒng)無需在工業(yè)機(jī)器人封閉式的控制器中加入力傳感器和主動(dòng)柔順?biāo)惴?具有力位解耦、實(shí)現(xiàn)簡(jiǎn)單、可移植性高、應(yīng)用更廣泛的優(yōu)點(diǎn)。

恒力裝置作為“機(jī)器人+恒力裝置”中機(jī)器人被動(dòng)柔順核心部件,國(guó)內(nèi)外學(xué)者研發(fā)出了各種形式的恒力裝置,該裝置從驅(qū)動(dòng)方式方式上可分類彈簧式、電機(jī)式、氣動(dòng)式3種:AHN、HUANG、LIU等[5-7]都研究了基于彈簧等阻尼器的機(jī)械式恒力裝置,該類裝置可以實(shí)現(xiàn)恒力被動(dòng)柔順,但力值不可主動(dòng)調(diào)整;趙亞平、MOHAMMAD等[8-9]都提出了基于電機(jī)作為力輸出源的恒力裝置,該類恒力裝置通過控制電機(jī)控制恒力的輸出,具有主動(dòng)柔順、調(diào)節(jié)響應(yīng)迅速的特點(diǎn),也存在力矩小、發(fā)熱大的缺點(diǎn);JIN等[10]研究了基于氣囊的氣動(dòng)式恒力輸出裝置,穩(wěn)態(tài)誤差可達(dá)到2 N,但存在無法安裝電主軸和更換打磨工具的問題,LIANG等[11]研究了基于氣缸和柔性機(jī)構(gòu)的恒力裝置,實(shí)現(xiàn)恒力在軸向和徑向的輸出。

氣缸式恒力裝置具有控制簡(jiǎn)單、柔性好的優(yōu)點(diǎn),工程中打磨機(jī)器人也較多使用奧地利生產(chǎn)的ACF自適應(yīng)法蘭,該裝置內(nèi)置力傳感器,結(jié)構(gòu)和控制復(fù)雜,且價(jià)格昂貴、技術(shù)保密。馬文超、袁樂天等[12-13]研究院校都對(duì)氣缸式的打磨機(jī)器人恒力裝置進(jìn)行了研究,但該恒力裝置缺乏對(duì)外部負(fù)載質(zhì)量、速度和加速度的感知,存在輸出力無法適應(yīng)外部負(fù)載變化的情況。

對(duì)此,針對(duì)內(nèi)部無力傳感器的氣缸式恒力裝置存在的因外部負(fù)載變化而精度難以保證的問題,本文基于內(nèi)部無力傳感器的氣缸式恒力裝置,研究其恒力跟隨方法,以提高其在負(fù)載質(zhì)量變化和傾角變化時(shí)的恒力跟隨精度。

1 無力傳感的恒力裝置

恒力裝置主要由電氣比例閥、電磁換向閥、氣缸、位移傳感器、傾角傳感器、以及上法蘭、下法蘭、花鍵副組成,如圖1所示為恒力裝置恒力輸出原理。

圖1 恒力裝置恒力輸出工作原理

恒力裝置工作時(shí),一般會(huì)下掛一定負(fù)載的打磨工具(簡(jiǎn)稱外部負(fù)載,后文都用外部負(fù)載),其接收外部給定的輸出力目標(biāo)值,輸出相應(yīng)的正壓力(簡(jiǎn)稱為輸出力,后文都用輸出力)如圖2所示,其工作流程如下:控制器接收來自外部的恒力值,從位移傳感器、傾角傳感器、電氣比例閥采集氣壓、位置、傾角信息,通過數(shù)據(jù)處理及算法運(yùn)算得到氣缸所需的氣壓值和氣缸進(jìn)氣方向,并將氣壓值和進(jìn)氣方向發(fā)送給電氣比例閥和電磁換向閥,使得氣缸產(chǎn)生相應(yīng)的推拉力,繼而實(shí)現(xiàn)目標(biāo)力的輸出。

圖2 恒力裝置受力情況

2 基于卡爾曼濾波器的恒力跟隨方法

2.1 恒力裝置恒力跟隨模型

當(dāng)恒力裝置被用于機(jī)器人打磨作業(yè)時(shí),恒力裝置上法蘭連接機(jī)器人,下法蘭安裝打磨工具,對(duì)此時(shí)的恒力裝置進(jìn)行受力分析。如圖2所示為恒力裝置受力情況,由于氣缸活塞、活塞桿、打磨工具為固定連接,將該三者視為一個(gè)彈簧阻尼系統(tǒng),其受力平衡方程式為:

(1)

式中:Fn為打磨工具對(duì)工件施加的法向力(與目標(biāo)輸出力為一對(duì)反作用力),P1、P2為氣缸的有桿腔氣壓和無桿腔氣壓,S1、S2為氣缸的活塞有桿側(cè)面積和無桿側(cè)面積,m1為恒力裝置的活塞和下法蘭的質(zhì)量(為定值),m2為打磨工具的質(zhì)量,α為打磨工具與重力方向的傾角(由傾角儀測(cè)得),f為活塞受到的氣缸內(nèi)壁的摩擦力,u為阻尼系數(shù),x為氣缸活塞的壓縮距離(由位移傳感器測(cè)得)。

2.2 基于卡爾曼濾波器的速度加速度獲取

卡爾曼濾波算法是一種除去噪聲還原真實(shí)數(shù)據(jù)的一種數(shù)據(jù)處理技術(shù),在測(cè)量協(xié)方差已知時(shí),通過輸入觀測(cè)數(shù)據(jù),可得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。針對(duì)線性系統(tǒng)模型,卡爾曼濾波器可基于當(dāng)前狀態(tài)測(cè)量數(shù)據(jù)和和系統(tǒng)模型對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)[14-15]。因此,本文引入卡爾曼濾波器獲取恒力裝置恒力跟隨模型中的位移、速度及加速度。

本系統(tǒng)中,卡爾曼濾波器中的系統(tǒng)狀態(tài)的預(yù)測(cè)過程可表示為:

xk+1=Akxk+Bkuk+wk

(2)

式中:xk+1為第k+1時(shí)刻系統(tǒng)狀態(tài)變量的預(yù)測(cè)值,xk為第k時(shí)刻的系統(tǒng)狀態(tài)變量的最優(yōu)估計(jì)值,Ak為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,Bk為控制矩陣,uk為控制向量,wk為系統(tǒng)中的噪聲誤差。

卡爾曼濾波器中的系統(tǒng)狀態(tài)的觀測(cè)過程可表示為:

zk=Hkxk+vk

(3)

式中:zk+1為第k+1次觀測(cè)的測(cè)量變量,Hk為觀測(cè)矩陣,vk+1為第k+1次的協(xié)方差為Rk的觀測(cè)誤差。

基于預(yù)測(cè)過程和觀測(cè)過程,卡爾曼濾波器的計(jì)算步驟主要由初始化濾波器、狀態(tài)預(yù)估、狀態(tài)觀測(cè)和狀態(tài)矯正4個(gè)步驟組成。

在初始化濾波器階段中,基于卡爾曼濾波算法原理對(duì)系統(tǒng)的預(yù)測(cè)過程和觀測(cè)過程進(jìn)行初始化,主要包括系統(tǒng)的初始狀態(tài)、觀測(cè)矩陣Hk、狀態(tài)轉(zhuǎn)移矩陣Ak、觀測(cè)誤差協(xié)方差矩陣Rk、系統(tǒng)誤差協(xié)方差矩陣Qk進(jìn)行賦值。

在狀態(tài)預(yù)估階段,包括對(duì)狀態(tài)變量的預(yù)測(cè)和誤差協(xié)方差變量的預(yù)測(cè),分別可通過式(4)和式(5)進(jìn)行預(yù)測(cè):

xk+1=Akxk+Bkuk

(4)

(5)

式中:Pk+1為第k+1次預(yù)測(cè)的系統(tǒng)誤差協(xié)方差,Qk為初始的系統(tǒng)誤差協(xié)方差。

在觀測(cè)階段,可通過式(3)得到系統(tǒng)的觀測(cè)變量的值;在狀態(tài)更新階段,主要包括對(duì)系統(tǒng)狀態(tài)的最優(yōu)估計(jì)和系統(tǒng)誤差協(xié)方差矩陣的更新,系統(tǒng)狀態(tài)的最優(yōu)估計(jì)和系統(tǒng)誤差協(xié)方差矩陣的更新可通過式(6)～式(8)進(jìn)行計(jì)算:

(6)

Pk+1=(I-Kk+1Hk+1)Pk+1

(7)

(8)

基于上述的卡爾曼濾波器原理,如式(9)～式(10)所示,本系統(tǒng)以位移、速度、加速度為系統(tǒng)的狀態(tài)變量,位移為系統(tǒng)的觀測(cè)變量,帶入卡爾曼濾波器計(jì)算,即可得到恒力裝置的氣缸的位移、速度和加速度。

x=[pva]T

(9)

z=[p0 0]T

(10)

式中:p為恒力裝置的氣缸伸縮量,v為恒力裝置的氣缸伸縮過程中的速度,a為恒力裝置的氣缸伸縮過程中的加速度。

2.3 外部負(fù)載大小標(biāo)定

由于恒力裝置未裝有力傳感器測(cè)量外部負(fù)載的大小(即打磨工具的質(zhì)量m2),為保證恒力輸出的精度,對(duì)此提出了一種獲取恒力跟隨模型中的恒力裝置外部負(fù)載大小的方法。

對(duì)式(1)進(jìn)行變形處理,可得到外部負(fù)載大小m2滿足如下關(guān)系式:

(11)

(12)

考慮到恒力裝置采用的氣缸為低摩擦氣缸,摩擦力很小可忽略不計(jì),式(12)可簡(jiǎn)化為:

(13)

式中:P1為有桿腔氣壓,P2為無桿腔氣壓(其值為大氣壓值,為已知)。

由于恒力裝置可測(cè)得氣壓P1和負(fù)載傾角α,將外部負(fù)載處于靜止平衡狀態(tài)時(shí)的氣壓P1和傾角α帶入式(13)中,即可求解得到外部負(fù)載m2值。

2.4 氣缸進(jìn)氣方向及氣壓求解

在恒力跟隨模型中其他參數(shù)確定后,恒力裝置的輸出力取決于氣缸進(jìn)氣方向和氣缸氣壓大小。而當(dāng)外部負(fù)載大小和傾角發(fā)生變化時(shí),為保證恒力輸出精度,需相應(yīng)確定氣缸進(jìn)氣方向和氣壓大小。對(duì)此,對(duì)式(1)變形處理,并忽略可不計(jì)摩擦力,得到氣缸氣壓與恒力的關(guān)系式:

(14)

記Fa為氣缸所需產(chǎn)生的氣壓力,Fa滿足:

(15)

由于有桿腔和無桿腔兩者有一個(gè)與大氣聯(lián)通,因此有桿腔氣壓P1和無桿腔氣壓P2滿足如下關(guān)系:

(16)

式中:Pa為大氣氣壓值。

當(dāng)氣缸進(jìn)氣腔不同時(shí),聯(lián)立式(14)～式(15)氣缸產(chǎn)生的氣壓力滿足如下關(guān)系:

(17)

根據(jù)式(16)可知,可得到如下恒力跟隨方法:

(1)當(dāng)外部負(fù)載的質(zhì)量、傾角、速度、加速度變化時(shí),控制器按照式(15)實(shí)時(shí)計(jì)算Fa的大小;

(2)當(dāng)Fa≥Pa(S1-S2)成立時(shí),恒力裝置選擇進(jìn)氣腔為有桿腔;當(dāng)Fa≤Pa(S1-S2)成立時(shí),恒力裝置選擇進(jìn)氣腔為無桿腔;

(3)按照式(17)計(jì)算達(dá)到目標(biāo)輸出力所需的進(jìn)氣腔氣壓大小。

3 恒力跟隨實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)

為驗(yàn)證所提出無力傳感的恒力裝置的恒力跟隨方法,搭建了如下平臺(tái)并進(jìn)行恒力輸出驗(yàn)證實(shí)驗(yàn)。如圖3和圖4所示,實(shí)驗(yàn)平臺(tái)主要由恒力裝置、實(shí)驗(yàn)臺(tái)架、力傳感器測(cè)量傳輸裝置、氣源4部分組成。

圖3 恒力裝置實(shí)物 圖4 力裝置實(shí)驗(yàn)臺(tái)架

如圖3所示為恒力裝置實(shí)物,為滿足實(shí)驗(yàn)平臺(tái)條件了,搭建了如圖4所示的恒力裝置實(shí)驗(yàn)臺(tái)架。該實(shí)驗(yàn)臺(tái)架由可轉(zhuǎn)動(dòng)臺(tái)架、5 kg外部負(fù)載、稱重式力傳感器、直線模組及其驅(qū)動(dòng)器組成。其中,稱重式力傳感器可直接測(cè)量恒力裝置輸出的力值,可轉(zhuǎn)動(dòng)臺(tái)架可以調(diào)節(jié)恒力裝置的傾角,直線模組可改變恒力裝置的力傳感器的位置以保證與負(fù)載末端接觸,滿足不同負(fù)載時(shí)的恒力裝置輸出力的測(cè)量需求。

3.2 恒力跟隨實(shí)驗(yàn)

為驗(yàn)證所提出的恒力裝置及其他恒力跟隨方法,基于上述的實(shí)驗(yàn)平臺(tái)進(jìn)行了外部負(fù)載質(zhì)量標(biāo)定實(shí)驗(yàn)和不同負(fù)載傾角下恒力裝置恒力輸出精度的驗(yàn)證實(shí)驗(yàn)。

如圖5所示,為對(duì)恒力裝置進(jìn)行外部負(fù)載質(zhì)量標(biāo)定實(shí)驗(yàn),恒力裝置下端掛一定配重,通過緩慢調(diào)節(jié)氣缸氣壓實(shí)現(xiàn)外部負(fù)載的靜態(tài)平臺(tái),控制器通過2.3節(jié)所用方法計(jì)算得到外部負(fù)載質(zhì)量大小,并自動(dòng)保存用于恒力跟隨方法中。

圖5 外部負(fù)載質(zhì)量標(biāo)定實(shí)驗(yàn)

如圖6所示,在對(duì)外部負(fù)載標(biāo)定后,將恒力裝置分別傾斜至3.8°、22°、45°,進(jìn)行動(dòng)態(tài)循環(huán)實(shí)驗(yàn),上位機(jī)軟件采集恒力裝置的位移、速度、傾角和力值信息,以驗(yàn)證所研制的恒力裝置恒力輸出精度。

(a) 3.8° (b) 22° (c) 45°圖6 不同角度動(dòng)態(tài)循環(huán)

如圖7～圖10所示,為上位機(jī)軟件采集到的3.8°、22°、45°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)時(shí)恒力裝置數(shù)據(jù),其中,Ft為輸出力目標(biāo)值,N;Fs為力傳感器測(cè)得的輸出力實(shí)際值,N;P為氣缸氣壓(0.1 MPa,即1個(gè)大氣壓,當(dāng)P為正時(shí),有桿腔進(jìn)氣;當(dāng)P為負(fù)時(shí),無桿腔進(jìn)氣);α為負(fù)載傾角,°;x為負(fù)載位置,mm;v為負(fù)載速度,mm/s;a為負(fù)載加速度,mm/s2。

圖7 3.8°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)時(shí)負(fù)載運(yùn)動(dòng)狀態(tài) 圖8 3.8°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)時(shí)輸出力情況

圖9 22°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)時(shí)輸出力情況 圖10 45°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)時(shí)輸出力情況

由于3.8°、22°、45°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)僅角度不同,實(shí)驗(yàn)過程類似,因此僅較為詳細(xì)地?cái)⑹?.8°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)的過程和輸出力情況。如圖7所示在3.8°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)中,負(fù)載在直線模組的推動(dòng)下以3 mm/s的速度做往復(fù)運(yùn)動(dòng)。每當(dāng)負(fù)載做完往復(fù)運(yùn)動(dòng)時(shí),上位機(jī)軟件則發(fā)送一個(gè)新的輸出力的目標(biāo)值Ft給恒力裝置(Ft給定順序?yàn)?0,25,…,100,95,…,20),恒力裝置根據(jù)Ft和自身狀態(tài)執(zhí)行恒力輸出算法并相應(yīng)控制氣缸動(dòng)作,實(shí)現(xiàn)對(duì)輸出力的目標(biāo)值Ft的響應(yīng),并由力傳感實(shí)時(shí)采集恒力裝置輸出力的實(shí)際值Fs。

對(duì)比圖7中的x、v、a曲線可以發(fā)現(xiàn),由卡爾曼濾波后獲得的x、v都較為平滑,a由于直線模組加速和減速而有一些周期性凸點(diǎn)。由x、v、a曲線可以看出,卡爾曼濾波可以較為準(zhǔn)確地獲得負(fù)載的位置、速度和加速度,為恒力跟隨方法獲得較為準(zhǔn)確負(fù)載運(yùn)動(dòng)參數(shù)。

由圖7～圖10可知,當(dāng)輸出力的目標(biāo)值Ft變化時(shí),恒力裝置會(huì)根據(jù)Ft的變化實(shí)時(shí)調(diào)節(jié)氣缸氣壓P以使得輸出力的實(shí)際值Fs跟隨Ft變化;當(dāng)負(fù)載傾角不同和負(fù)載處在直線往復(fù)運(yùn)動(dòng)時(shí),恒力裝置的輸出力仍能與輸出力的目標(biāo)值Ft保持較小誤差跟隨;證明了本文所提出的恒力跟隨方法可以在負(fù)載傾角變化和負(fù)載處在慢速運(yùn)動(dòng)時(shí)實(shí)現(xiàn)實(shí)際輸出力跟隨目標(biāo)輸出力。

為獲得恒力裝置在3.8°、22°、45°動(dòng)態(tài)循環(huán)實(shí)驗(yàn)中的實(shí)際輸出力與目標(biāo)輸出力的跟隨誤差,對(duì)該3個(gè)實(shí)驗(yàn)中的恒力裝置的輸出力誤差進(jìn)行統(tǒng)計(jì),并繪制了如圖11所示的輸出力誤差情況圖,為量化誤差情況,統(tǒng)計(jì)了如表1所示的跟蹤誤差統(tǒng)計(jì)表。

圖11 恒力裝置輸出力誤差情況

表1 恒力裝置的跟蹤誤差統(tǒng)計(jì)

由表1和圖11可知,在3種傾角下,該恒力裝置的跟蹤誤差小于2 N的時(shí)刻點(diǎn)占比為0.883 1、0.905 8、0.887 8,跟蹤誤差小于3 N的時(shí)刻點(diǎn)占比為0.946 1、0.946 7、0.938 9,驗(yàn)證了該恒力裝置及恒力跟隨方法的可行性。由于恒力裝置會(huì)存在剛接收到輸出力目標(biāo)值而沒時(shí)間響應(yīng)就反饋實(shí)際輸出力的情況,少數(shù)時(shí)刻誤差大于4 N的。

4 結(jié)論

無力傳感的恒力裝置是打磨機(jī)器人被動(dòng)柔順的關(guān)鍵部件,為解決無力傳感的恒力裝置缺乏對(duì)外部負(fù)載質(zhì)量、速度和加速度的感知和存在輸出力無法適應(yīng)外部負(fù)載變化的問題,本文提出了基于卡爾曼濾波的恒力跟隨方法,最終得到結(jié)論如下:

(1)在恒力輸出模型的基礎(chǔ)上,利用卡爾曼濾波器和標(biāo)定方法得到對(duì)外部負(fù)載的質(zhì)量、速度加速度,實(shí)現(xiàn)了無力傳感的恒力裝置對(duì)外部負(fù)載的感知。

(2)在感知外部負(fù)載后,通過實(shí)時(shí)求解恒力裝置的氣缸氣壓大小和進(jìn)氣方向,可實(shí)現(xiàn)輸出力對(duì)目標(biāo)力的跟隨,其跟隨誤差精度為3 N,基本滿足機(jī)器人打磨作業(yè)的恒力精度要求,證明了所提方法的可行性。