基于UKF的六維力傳感器重力補(bǔ)償算法研究

李非凡,羅楊宇,曹志勇,李 翔,程 光

(1.北京聯(lián)合大學(xué) 城市軌道交通與物流學(xué)院,北京 100101;2.中國(guó)科學(xué)院自動(dòng)化研究所,北京 100190;3.北京聯(lián)合大學(xué) 前沿智能技術(shù)研究院,北京 100101)

0 引言

隨著科學(xué)技術(shù)的發(fā)展,傳感器技術(shù)已經(jīng)廣泛應(yīng)用于機(jī)器人、航空航天、制造業(yè)、醫(yī)療設(shè)備等領(lǐng)域。在工業(yè)生產(chǎn)和科學(xué)研究中,力傳感器發(fā)揮著關(guān)鍵作用,特別是在人機(jī)交互領(lǐng)域中,精確測(cè)量物體的力和扭矩是確保人機(jī)交互系統(tǒng)精準(zhǔn)控制和操作的關(guān)鍵[1]。在力控制、力反饋中,精確的力測(cè)量可以確保人機(jī)交互系統(tǒng)的安全性和可靠性,并且可以提高人機(jī)交互系統(tǒng)的操作精度和效率[2]。在人機(jī)交互系統(tǒng)中,為了實(shí)時(shí)監(jiān)測(cè)機(jī)器人末端工具所受的力,通常在末端工具與機(jī)器人之間安裝六維力傳感器,可實(shí)時(shí)測(cè)量工具所受的三維力和力矩。隨著機(jī)器人的不斷運(yùn)動(dòng),位姿信息不斷改變,受末端工具的重力影響,傳感零位值不斷變化,導(dǎo)致傳感器測(cè)量上產(chǎn)生一定偏差,降低了測(cè)量精度,進(jìn)而影響機(jī)器人的操作。因此,有必要對(duì)機(jī)器人運(yùn)動(dòng)中的六維力傳感器進(jìn)行重力補(bǔ)償與校正,使傳感器在機(jī)器人運(yùn)動(dòng)過(guò)程中也能檢測(cè)出末端工具實(shí)際所受的外力大小,從而使系統(tǒng)能夠更加精密地完成操作任務(wù)。六維力傳感器具有高靈敏度和高精度,對(duì)環(huán)境中的干擾和噪聲非常敏感[3]。因此,在處理六維力傳感器數(shù)據(jù)時(shí),還需要采用一些有效的濾波和校準(zhǔn)技術(shù),以便最大限度地減小測(cè)量誤差和噪聲。

常用的六維力傳感器重力補(bǔ)償和校正算法包括線性回歸、最小二乘法、小波變換、神經(jīng)網(wǎng)絡(luò)和粒子群優(yōu)化等方法[4]。其中,利用線性回歸和最小二乘法計(jì)算傳感器的偏移量和靈敏度系數(shù)來(lái)進(jìn)行重力補(bǔ)償和校正的方法具有計(jì)算簡(jiǎn)單、易于實(shí)現(xiàn)等優(yōu)點(diǎn)。小波變換方法將傳感器測(cè)量值分解為多個(gè)頻段,并分別進(jìn)行重力補(bǔ)償和校正,從而有效地消除了高頻噪聲的影響[5]。神經(jīng)網(wǎng)絡(luò)和粒子群優(yōu)化方法可以根據(jù)傳感器的測(cè)量值和理論模型,通過(guò)自適應(yīng)學(xué)習(xí)和優(yōu)化來(lái)進(jìn)行重力補(bǔ)償和校正,因此,具有較好的準(zhǔn)確性和魯棒性。

這些算法在處理六維力傳感器的重力補(bǔ)償和校正時(shí)仍然存在不足之處。例如:算法的精度和準(zhǔn)確性容易受到環(huán)境和傳感器位置的影響[6];部分算法需要進(jìn)行多次試驗(yàn)和復(fù)雜的參數(shù)調(diào)節(jié),時(shí)間成本較高;部分算法對(duì)非線性和復(fù)雜的傳感器模型的處理能力有限,需要進(jìn)一步研究和優(yōu)化[7]。為解決上述問(wèn)題,本文使用 UKF 算法處理六維力傳感器數(shù)據(jù),以實(shí)現(xiàn)對(duì)六維力傳感器數(shù)據(jù)的高精度估計(jì)。

1 UKF 原理與算法設(shè)計(jì)

1.1 UKF 原理

UKF 是一種基于卡爾曼濾波器的無(wú)模型參數(shù)濾波器,可以用于估算非線性系統(tǒng)的狀態(tài)和參數(shù)。UKF算法是利用UT變換獲取Sigma點(diǎn)集合,這些Sigma點(diǎn)通過(guò)非線性系統(tǒng)進(jìn)行傳播,以獲得一組新的預(yù)測(cè)Sigma點(diǎn),再將預(yù)測(cè)的Sigma點(diǎn)通過(guò)觀測(cè)模型傳遞,以獲得一組預(yù)測(cè)測(cè)量。這組預(yù)測(cè)測(cè)量可以用來(lái)計(jì)算卡爾曼增益并更新狀態(tài)的均值和協(xié)方差矩陣,從而更新狀態(tài)估計(jì)[8]。通過(guò)非線性函數(shù)傳遞,將非線性函數(shù)線性化問(wèn)題轉(zhuǎn)換成系統(tǒng)狀態(tài)量概率密度分布的近似,然后基于Kalman算法框架進(jìn)行濾波。相較于傳統(tǒng)的卡爾曼濾波器,UKF 不需要對(duì)非線性系統(tǒng)進(jìn)行線性化處理,能夠更好地處理非線性系統(tǒng)和噪聲,具有更好的魯棒性和準(zhǔn)確性[9]。

1.2 UKF 算法設(shè)計(jì)流程

UKF 處理六維力傳感器數(shù)據(jù)的過(guò)程與一般的 UKF 處理過(guò)程類似,主要包括以下步驟:

1)狀態(tài)變量的選取:確定狀態(tài)向量,包括估計(jì)需要的物理量,即六維力數(shù)據(jù)。

2)建立狀態(tài)轉(zhuǎn)移函數(shù):建立狀態(tài)轉(zhuǎn)移函數(shù),將當(dāng)前狀態(tài)向量映射到下一時(shí)刻的狀態(tài)向量。

3)建立測(cè)量函數(shù):根據(jù)傳感器的測(cè)量原理,建立測(cè)量函數(shù),將狀態(tài)向量映射到測(cè)量向量。

4)噪聲模型:確定噪聲模型,包括過(guò)程噪聲和測(cè)量噪聲,通常假設(shè)為高斯分布。

5)初始狀態(tài)的設(shè)定:根據(jù)實(shí)際情況,設(shè)定初始狀態(tài)向量和協(xié)方差矩陣。

6)Sigma 點(diǎn)的生成:根據(jù)當(dāng)前狀態(tài)向量和協(xié)方差矩陣,生成Sigma點(diǎn)集合。Sigma點(diǎn)的數(shù)量通常為2n+1個(gè),其中n是狀態(tài)向量的維數(shù)。

7)Sigma 點(diǎn)的傳播:根據(jù)狀態(tài)轉(zhuǎn)移函數(shù),將 Sigma 點(diǎn)集合進(jìn)行狀態(tài)轉(zhuǎn)移,得到預(yù)測(cè)的Sigma點(diǎn)集合。這些點(diǎn)的傳播過(guò)程采用無(wú)跡變換,以保持Sigma點(diǎn)的高斯分布特性。

8)預(yù)測(cè)狀態(tài)向量和協(xié)方差矩陣:從預(yù)測(cè)Sigma點(diǎn)集合中,計(jì)算預(yù)測(cè)狀態(tài)向量和協(xié)方差矩陣。

9)預(yù)測(cè)測(cè)量向量和協(xié)方差矩陣:將預(yù)測(cè)狀態(tài)向量和協(xié)方差矩陣通過(guò)測(cè)量函數(shù)得到預(yù)測(cè)測(cè)量向量和協(xié)方差矩陣。

10)狀態(tài)預(yù)測(cè):使用當(dāng)前狀態(tài)的均值和協(xié)方差矩陣生成一組Sigma點(diǎn),通過(guò)非線性系統(tǒng)進(jìn)行傳播,得到一組新的預(yù)測(cè)Sigma點(diǎn),從預(yù)測(cè)的Sigma點(diǎn)計(jì)算出預(yù)測(cè)的狀態(tài)均值和協(xié)方差矩陣。

11)測(cè)量預(yù)測(cè):將預(yù)測(cè)的Sigma點(diǎn)通過(guò)觀測(cè)模型傳遞,得到一組預(yù)測(cè)測(cè)量。這組預(yù)測(cè)測(cè)量可以用來(lái)計(jì)算卡爾曼增益。

12)計(jì)算卡爾曼增益:卡爾曼增益將預(yù)測(cè)的狀態(tài)估計(jì)與實(shí)際觀測(cè)進(jìn)行比較。UKF使用基于預(yù)測(cè)狀態(tài)的均值和協(xié)方差矩陣、測(cè)量的均值和協(xié)方差矩陣以及預(yù)測(cè)測(cè)量的協(xié)方差矩陣計(jì)算卡爾曼增益。

13)更新狀態(tài)估計(jì):通過(guò)卡爾曼增益和測(cè)量值來(lái)更新狀態(tài)的均值和協(xié)方差矩陣,將產(chǎn)生一個(gè)新的狀態(tài)估計(jì)值,該值可用于下一次迭代的預(yù)測(cè)步驟[10]。

2 基于六維力傳感器的 UKF 算法設(shè)計(jì)

2.1 設(shè)置參數(shù)

2.1.1設(shè)置狀態(tài)向量x

狀態(tài)向量x包含了末端受力工具(機(jī)械手)在X、Y、Z3個(gè)方向上的位置、受力、力矩等信息,表示為

x=[fx,fy,fz,mx,my,mz]T。

(1)

其中,fx,fy,fz表示機(jī)器人在X、Y和Z軸上的受力,mx,my,mz表示機(jī)器人圍繞X、Y和Z軸的力矩。

在 UKF 算法中,狀態(tài)向量x的更新是通過(guò)對(duì)x進(jìn)行預(yù)測(cè)和校正來(lái)實(shí)現(xiàn)的。預(yù)測(cè)階段通過(guò)系統(tǒng)動(dòng)態(tài)方程對(duì)x進(jìn)行預(yù)測(cè),校正階段通過(guò)測(cè)量方程對(duì)預(yù)測(cè)值進(jìn)行校正。

2.1.2設(shè)置狀態(tài)噪聲Q

狀態(tài)噪聲Q表示系統(tǒng)動(dòng)態(tài)方程中的噪聲,它可以通過(guò)狀態(tài)轉(zhuǎn)移矩陣G和通常假設(shè)為零均值的高斯白噪聲QwGT計(jì)算得到:

Q=GQwGT。

(2)

其中,G是狀態(tài)轉(zhuǎn)移矩陣。Q是噪聲方差矩陣。QwGT通常假設(shè)為零均值的高斯白噪聲,即:

Qw～N(0,Qw)。

(3)

其中,Qw是噪聲方差。

2.1.3設(shè)置測(cè)量噪聲R

測(cè)量噪聲R表示測(cè)量方程中的噪聲,它可以通過(guò)測(cè)量轉(zhuǎn)移矩陣G和測(cè)量方差R0計(jì)算得到:

R=GR0GT。

(4)

其中,G是測(cè)量轉(zhuǎn)移矩陣,R0是測(cè)量方差矩陣。

2.1.4設(shè)置無(wú)跡變換參數(shù)α,β,κ

UKF 算法中的無(wú)跡變換(Unscented Transformation)將高斯分布的均值和協(xié)方差轉(zhuǎn)換為一組樣本點(diǎn),用于對(duì)非線性函數(shù)進(jìn)行逼近[11]。

無(wú)跡變換需要設(shè)置3個(gè)參數(shù):α,β,κ。其中,α控制樣本點(diǎn)離均值的距離,β用于處理高斯分布的不確定度,κ用于調(diào)整樣本點(diǎn)的權(quán)重。六維力傳感器UKF的算法取值為:α=0.001,β=2,κ=0。

2.2 預(yù)測(cè)階段

預(yù)測(cè)階段通過(guò)狀態(tài)轉(zhuǎn)移方程對(duì)狀態(tài)向量x進(jìn)行預(yù)測(cè),得到預(yù)測(cè)狀態(tài)向量xk|k-1和預(yù)測(cè)協(xié)方差矩陣Pk|k-1:

xk|k-1=f(xk|k-1),

(5)

(6)

其中,f是狀態(tài)轉(zhuǎn)移方程,Qk-1是狀態(tài)噪聲協(xié)方差矩陣。

2.3 校正階段

校正階段通過(guò)測(cè)量方程對(duì)預(yù)測(cè)狀態(tài)向量xk|k-1進(jìn)行校正,得到校正狀態(tài)向量xk|k和校正協(xié)方差矩陣Pk|k:

yk=h(xk-1|k-1),

(7)

xk|k=xk|k-1+Kk(yk-h(xk|k-1)),

Pk|k=(I-KkHk)Pk|k-1。

其中,yk是測(cè)量向量,h是測(cè)量方程,Sk是測(cè)量殘差的協(xié)方差,Rk是測(cè)量噪聲協(xié)方差矩陣,Hk是測(cè)量轉(zhuǎn)移矩陣。

在 UKF 算法中,根據(jù)預(yù)測(cè)狀態(tài)向量xk|k-1和預(yù)測(cè)協(xié)方差矩陣Pk|k-1,可以通過(guò)無(wú)跡變換計(jì)算一組樣本點(diǎn),然后根據(jù)這些樣本點(diǎn)和權(quán)重系數(shù)計(jì)算校正狀態(tài)向量xk|k和校正協(xié)方差矩陣Pk|k。

根據(jù)預(yù)測(cè)狀態(tài)向量xk|k-1和預(yù)測(cè)協(xié)方差矩陣Pk|k-1,計(jì)算一組無(wú)跡變換的樣本點(diǎn):

(8)

(9)

接下來(lái)計(jì)算預(yù)測(cè)測(cè)量向量yk|k-1和預(yù)測(cè)測(cè)量協(xié)方差矩陣Pyy,k|k-1:

(10)

(11)

最后,計(jì)算校正增益Kk、校正狀態(tài)向量xk|k-1和校正協(xié)方差矩陣Pk|k:

xk|k=xk|k-1+Kk(yk-yk|k-1),

(12)

(13)

3 實(shí)驗(yàn)過(guò)程

本實(shí)驗(yàn)采用六維力傳感器KWR 75,采樣頻率為 1 000 Hz,該傳感器具有高靈敏度和高穩(wěn)定性,能夠?qū)崟r(shí)采集目標(biāo)物體在6個(gè)自由度方向上的受力信息。實(shí)驗(yàn)裝置如圖 1 所示,機(jī)械臂采用睿爾曼RM-65六自由度機(jī)械臂,末端工具采用因時(shí)仿人機(jī)械手。六維力傳感器通過(guò)法蘭安裝在機(jī)械臂與機(jī)械手之間,通過(guò)傳感器測(cè)量機(jī)械手與外界物體接觸的力和力矩,利用 Visual Studio Python語(yǔ)言實(shí)時(shí)接收數(shù)據(jù)并代入U(xiǎn)KF算法中,導(dǎo)出實(shí)驗(yàn)數(shù)據(jù)并繪制數(shù)據(jù)曲線圖。實(shí)驗(yàn)通過(guò)采集末端機(jī)械手動(dòng)態(tài)空載下六維力傳感器的測(cè)量數(shù)據(jù),利用 UKF 算法對(duì)數(shù)據(jù)進(jìn)行處理,驗(yàn)證 UKF 算法的準(zhǔn)確性和魯棒性。

實(shí)驗(yàn)過(guò)程中,在末端工具不接觸外力與外界的環(huán)境下,對(duì)機(jī)械臂進(jìn)行加速度和角速度激勵(lì),記錄測(cè)量到的六維力的值[12]。圖 2～7 為力反饋的原始數(shù)據(jù),橫軸為時(shí)間,單位為s,縱軸為力或力矩,單位為 N 或 N·m 。其中,圖2表示物體在X方向上的力,圖 3 表示物體在Y方向上的力,圖 4 表示物體在Z方向上的力,圖 5 表示物體在X方向上的力矩,圖 6 表示物體在Y方向上的力矩,圖7表示物體在Z方向上的力矩。

在未對(duì)力反饋數(shù)據(jù)做任何處理時(shí),由于末端機(jī)械手重力的影響,在空載情況下,即在末端機(jī)械手不與外界物體接觸的情況下,六維力傳感器傳輸?shù)牧Ψ答仈?shù)據(jù)不為 0,誤差較大,誤差范圍在 0～6 N之間。其中,X方向上力的誤差范圍在0～3 N之間,Y方向上力的誤差范圍在 0～2 N之間,Z方向上力的誤差范圍在0～6 N之間,X方向上力矩的誤差范圍在 0～1 N·m之間,Y方向上力矩的誤差范圍在 0～1 N·m之間,Z方向上力矩的誤差范圍在 0～1 N·m之間。

在上述實(shí)驗(yàn)過(guò)程中,輸出同一時(shí)刻經(jīng)UKF算法補(bǔ)償后的六維力傳感器數(shù)據(jù),實(shí)驗(yàn)結(jié)果如圖8～13所示。其中,圖8表示傳感器在X方向上所受的力,圖9表示傳感器在Y方向上所受的力,圖10表示傳感器在Z方向上所受的力,圖11表示傳感器在X方向上的力矩,圖12表示傳感器在Y方向上的力矩,圖13表示傳感器在Z方向上的力矩。

圖1 實(shí)驗(yàn)裝置圖

圖2 X軸力

圖3 Y軸力

圖4 Z軸力

圖5 X軸力矩

圖6 Y軸力矩

圖7 Z軸力矩

圖8 X軸受力

圖9 Y軸受力

圖10 Z軸受力

圖11 X軸力矩

圖12 Y軸力矩

圖13 Z軸力矩

從圖 8～13 中可以看出,利用 UKF 算法補(bǔ)償處理后得到的力反饋數(shù)據(jù)的結(jié)果誤差更小,接近于 0,誤差范圍在 0～0.175 N 之間,其中X方向上力的誤差范圍在0～0.162 N 之間,Y方向上力的誤差范圍在0～0.160 N 之間,Z方向上力的誤差范圍在0～0.227 N 之間,X方向上力矩的誤差范圍在0～0.010 N·m之間,Y方向上力矩的誤差范圍在0～0.010 N·m 之間,Z方向上力矩的誤差范圍在0～0.039 N·m之間。

從圖 2～13 中可以看出:利用 UKF 算法處理后的數(shù)據(jù)結(jié)果偏差更小、更穩(wěn)定。這表明 UKF 算法在處理六維力傳感器數(shù)據(jù)時(shí)具有更高的精度和更好的魯棒性,可以有效提高數(shù)據(jù)的準(zhǔn)確性和可靠性。

4 結(jié)論

本文針對(duì)由機(jī)器人末端工具帶來(lái)的重力影響導(dǎo)致六維力傳感器數(shù)據(jù)不準(zhǔn)確的問(wèn)題,提出利用UKF算法對(duì)數(shù)據(jù)進(jìn)行補(bǔ)償處理。本文通過(guò)實(shí)驗(yàn)驗(yàn)證說(shuō)明使用 UKF 算法處理后的力反饋數(shù)據(jù)誤差更小、更平滑,偏差更小、更穩(wěn)定。這表明UKF算法在處理六維力傳感器數(shù)據(jù)時(shí),能夠有效提高數(shù)據(jù)的準(zhǔn)確性和可靠性。本文通過(guò)實(shí)驗(yàn)驗(yàn)證了 UKF 算法在處理六維力傳感器數(shù)據(jù)時(shí)的優(yōu)越性。在未來(lái)研究中,我們可以進(jìn)一步探索 UKF 算法在其他機(jī)器人中的應(yīng)用,例如,機(jī)器人的姿態(tài)估計(jì)、軌跡跟蹤等領(lǐng)域。同時(shí),我們也可以探索使用其他更為先進(jìn)的算法對(duì)六維力傳感器數(shù)據(jù)進(jìn)行處理和應(yīng)用,以期進(jìn)一步提高機(jī)器人系統(tǒng)的性能和精度。