基于預(yù)標(biāo)定基坐標(biāo)系及MIEKF算法的工業(yè)機(jī)器人標(biāo)定方法

林耿聰，肖曙紅，楊林，盧浩文，張建華

(1.廣東工業(yè)大學(xué)機(jī)電工程學(xué)院，廣東廣州 510006；2.佛山智能裝備技術(shù)研究院，廣東佛山 528000；3.佛山華數(shù)機(jī)器人有限公司，廣東佛山 528000)

0 前言

目前，工業(yè)機(jī)器人因其高靈活性被廣泛應(yīng)用于汽車行業(yè)、物流搬運(yùn)、航空航天和醫(yī)療手術(shù)等行業(yè)[1-2]，這些行業(yè)對機(jī)器人絕對定位精度的要求高。然而因核心部件制造精度和裝配精度等細(xì)微偏差的疊加影響，造成絕對定位精度基本都在毫米級上，難以滿足上述行業(yè)的高精度工作要求[3]。提高機(jī)器人絕對定位精度的手段有：提高零部件加工精度和裝配精度，以及通過運(yùn)動學(xué)標(biāo)定技術(shù)提高絕對定位精度。前者會使制造成本增加，且效果有限；后者操作簡便易實(shí)現(xiàn)，且能大幅度提升絕對定位精度。運(yùn)動學(xué)標(biāo)定方法的步驟為：建立誤差模型、點(diǎn)位采集、參數(shù)辨識及參數(shù)補(bǔ)償[4-5]。

目前大部分模型是基于DH(Denavit-Hartenberg)模型而改進(jìn)的MDH模型[6]。點(diǎn)位采集指采集機(jī)器人運(yùn)動空間中的點(diǎn)位數(shù)據(jù)，主要采用激光跟蹤儀、拉線測量裝置和球桿儀等采集儀器獲取數(shù)據(jù)[7-9]；參數(shù)誤差補(bǔ)償是指將算法辨識的幾何參數(shù)誤差補(bǔ)償?shù)綑C(jī)器人控制器上，從而提高精度；采用的辨識算法有最小二乘法、L-M算法、粒子群算法、擴(kuò)展卡爾曼濾波算法等。LI等[10]基于MDH模型對SCARA機(jī)器人建立距離誤差模型和位置誤差模型，用LS進(jìn)行參數(shù)辨識并補(bǔ)償；GONG等[11]先用最小二乘法辨識機(jī)器人的幾何誤差參數(shù)，再補(bǔ)償由自重產(chǎn)生的柔度誤差及熱誤差；GAO等[12]對辨識矩陣進(jìn)行奇異值分解后，去掉冗余參數(shù)和相關(guān)參數(shù)，再用改進(jìn)最小二乘法辨識機(jī)器人的幾何參數(shù)誤差；何曉煦等[13]先用L-M算法識別幾何參數(shù)誤差并補(bǔ)償，然后基于空間相似性對補(bǔ)償后的殘差進(jìn)行補(bǔ)償；JIANG等[14]利用EKF算法初步辨識幾何參數(shù)誤差，其結(jié)果作為PF算法的初值再進(jìn)行辨識，從而完成機(jī)器人的參數(shù)誤差補(bǔ)償，實(shí)現(xiàn)機(jī)器人定位精度提升；寇斌等人[15]基于粒子群算法收斂速度慢的缺點(diǎn)，提出一種動態(tài)粒子群算法，有效辨識出機(jī)器人幾何誤差參數(shù)。

綜上研究，本文作者將以HSR-JR630型工業(yè)機(jī)器人為研究對象，推導(dǎo)機(jī)器人的正運(yùn)動學(xué)模型，并提出位置數(shù)據(jù)在測量坐標(biāo)系和基坐標(biāo)系的快速轉(zhuǎn)換方法；考慮位置誤差模型完整最小連續(xù)性的建模方法，在模型建模時(shí)引入減速比及相關(guān)性分析去除冗余參數(shù)；最后用MIEKF(Modified Iterated Extended Kalman Filter)算法對去除冗余參數(shù)后的模型進(jìn)行參數(shù)辨識，并補(bǔ)償驗(yàn)證機(jī)器人的絕對定位精度。

1 建立運(yùn)動學(xué)模型

1.1 MDH模型

R(Xi,αi)·R(Yi,βi)=

(1)

其中：cθi表示cosθi；sθi表示sinθi；cαi表示cosαi；sαi表示sinαi。

1.2 機(jī)器人正運(yùn)動學(xué)

根據(jù)HSR-JR630型工業(yè)機(jī)器人的結(jié)構(gòu)參數(shù)確定其MDH參數(shù)，如表1所示。

表1 HSR-JR630型工業(yè)機(jī)器人的理論MDH參數(shù)

(2)

2 運(yùn)動學(xué)位置誤差模型

2.1 基坐標(biāo)系和工具坐標(biāo)系的預(yù)標(biāo)定

首先根據(jù)測量坐標(biāo)系、機(jī)器人基坐標(biāo)系和機(jī)器人工具坐標(biāo)系的位姿關(guān)系得到下式：

(3)

(4)

通過克羅內(nèi)克積?運(yùn)算和拉直算子vec()[16]，可以將式(4)變換得到：

(5)

其中：I3為三階單位矩陣。

文中的標(biāo)定測量了m組機(jī)器人末端位置，可將式(5)轉(zhuǎn)化為超定方程組：

Ewm=pinv(Hwm)·Pwm

(6)

其中：Hwm為3m×15的矩陣；Ewm為1×15的待求解列向量；Pwm為3m×1的列向量。

(7)

(8)

2.2 建立位置誤差模型

2.2.1 關(guān)節(jié)間的微分

(9)

(10)

則相鄰關(guān)節(jié)間相對于當(dāng)前坐標(biāo)系{i}的微分誤差矩陣為

(11)

2.2.2 位置誤差模型

(12)

由式(10)(12)得：

(13)

(14)

其中：ΔX=[ΔθΔdΔaΔαΔβΔrΔbΔt]T為40×1的各個(gè)參數(shù)誤差列向量；Δb=[dbxdbydbzδbxδbyδbz]T；Δt=[dtxdtydtz]T代表了構(gòu)造機(jī)器人基坐標(biāo)系誤差和工具坐標(biāo)系偏差；J為3×40的辨識雅克比矩陣。

2.3 基于相關(guān)系數(shù)及復(fù)共線性分析冗余參數(shù)

首先要確定冗余參數(shù)個(gè)數(shù)，對于測量了m組機(jī)器人運(yùn)動位姿，則有：

ΔPm=JmΔX

(15)

對式(15)的辨識雅克比矩陣Jm進(jìn)行奇異值分解，得：

(16)

其中：Σ=diag(σ1，σ2，…，σr)，r(r≤40)為Jm的秩，設(shè)置σr>10-5才是Jm的有效秩。則根據(jù)Σ得到r=33，即存在7個(gè)冗余參數(shù)。

其次，進(jìn)行相關(guān)性和復(fù)共線性分析確定冗余參數(shù)，用辨識雅克比矩陣Jm求取相關(guān)系數(shù)矩陣W來確定參數(shù)誤差的相關(guān)性，采用Z-Score變換對Jm進(jìn)行標(biāo)準(zhǔn)化處理，則標(biāo)準(zhǔn)化矩陣Z為

(17)

(18)

(19)

(20)

根據(jù)標(biāo)準(zhǔn)化矩陣Z求取相關(guān)系數(shù)矩陣，辨識雅克比矩陣的相關(guān)系數(shù)矩陣為

W=1/(3m-1)ZTZ

(21)

根據(jù)式(21)可知，dbz和Δd1、δbz和Δθ1、dtz和Δd6、Δd2和Δd3、dtx和Δa6之間的相關(guān)系數(shù)都為1，所以它們每兩者都存在相關(guān)性，因此選擇去除相關(guān)參數(shù)Δd1、Δθ1、Δd6、Δd2和Δa6。

另外，Δθ6、Δα6對應(yīng)的辨識雅克比矩陣列分別和工具坐標(biāo)系偏差dtx、dty、dtz對應(yīng)的辨識雅克比矩陣列存在復(fù)共線性，即：

Jm(Δθ6)=-tyJm(dtx)+txJm(dty)

(22)

Jm(Δα6)=-tzJm(dty)+txJm(dtz)

(23)

其中：tx、ty和tz分別為工具坐標(biāo)系位置的x、y、z分量，故Δθ6和Δα6也為冗余參數(shù)。

綜上，Δθ1、Δθ6、Δd1、Δd2、Δd6、Δa6、Δα6為冗余參數(shù)。剔除干擾模型的冗余參數(shù)后，待辨識的幾何誤差參數(shù)變?yōu)?3個(gè)，故Jm變?yōu)?m×33，ΔX為33×1的列向量，ΔPm為3m×1的列向量。

3 改進(jìn)迭代擴(kuò)展卡爾曼濾波算法

改進(jìn)迭代擴(kuò)展卡爾曼濾波算法是EKF算法[17]的改進(jìn)形式。算法原理是針對位置誤差模型的辨識雅克比矩陣參數(shù)量綱不同，對辨識雅克比矩陣進(jìn)行參數(shù)量綱一化，即通過對辨識雅克比矩陣進(jìn)行列縮放從而進(jìn)行參數(shù)歸一化，確保辨識雅克比矩陣列對各個(gè)參數(shù)誤差的影響度一致，提高辨識狀態(tài)參數(shù)的魯棒性。再提出了遺忘因子優(yōu)化協(xié)方差陣防止其病化，從而保證狀態(tài)參數(shù)能夠快速收斂。并且利用觀測值估計(jì)迭代，每次迭代更新都保持測量位置不變，只通過殘差矢量更新計(jì)算狀態(tài)參數(shù)，達(dá)成觀測方程的更新，找出最優(yōu)的狀態(tài)參數(shù)估計(jì)。

由m個(gè)標(biāo)定點(diǎn)組成的辨識雅克比矩陣Jm求解縮放系數(shù)的對角方差陣D[18]步驟如下：

(24)

D=diag(l1,l2,…,lm)

(25)

根據(jù)式(14)和式(25)可得每個(gè)標(biāo)定點(diǎn)的辨識雅克比矩陣和待辨識參數(shù)轉(zhuǎn)化為

Hk=JkD-1

(26)

ΔXk=DΔX

(27)

其中：ΔXk和Hk分別為系統(tǒng)狀態(tài)參數(shù)和系統(tǒng)觀測矩陣。則系統(tǒng)的狀態(tài)方程和觀測方程為

ΔXk|k-1=Ak-1ΔXk-1|k-1+wk-1

(28)

Zk=HkΔXk|k-1+Vk

(29)

其中：wk-1為系統(tǒng)隨機(jī)過程噪聲向量；Ak-1為狀態(tài)轉(zhuǎn)移矩陣，因幾何參數(shù)誤差不隨機(jī)器人空間位置變化而變化，則Ak-1=I；Zk為機(jī)器人末端位置誤差(ΔP)；Vk為測量噪聲。

改進(jìn)迭代擴(kuò)展卡爾曼濾波步驟如下：

幾何參數(shù)誤差的預(yù)測：

ΔXk|k-1=ΔXk-1|k-1

(30)

預(yù)測誤差協(xié)方差矩陣：

Pk|k-1=Pk-1|k-1+Qk-1

(31)

卡爾曼增益矩陣：

(32)

更新殘差矢量：

ΔZk=Zk-HkΔXk|k-1

(33)

更新幾何誤差狀態(tài)方程：

ΔXk|k=ΔXk|k-1+Kk|kΔZk

(34)

用遺忘因子γk計(jì)算更新修正的協(xié)方差矩陣：

Pk|k=γk(I-Kk|kHk)Pk|k-1(I-Kk|kHk)T+

(35)

式中的遺忘因子，由下式?jīng)Q定：

(36)

其中：tr()為矩陣的跡；I33×33為單位矩陣，Rk為測量噪聲的協(xié)方差矩陣，Rk=10-3I3×3，Q=10-5I33×33，P0|0=I33×33，初始化誤差ΔX0|0=0；k|k-1表示第k次迭代的先驗(yàn)值，k|k表示第k次迭代后估計(jì)值；當(dāng)k為m倍時(shí)，表示一輪迭代完畢，需要重新從第一個(gè)標(biāo)定點(diǎn)位開始迭代，迭代更新4輪后停止；然后將迭代辨識的結(jié)果ΔXk|k經(jīng)D轉(zhuǎn)換為實(shí)際的幾何參數(shù)誤差ΔX，并補(bǔ)償?shù)綑C(jī)器人控制器上。

ΔX=D-1ΔXk|k

(37)

4 實(shí)驗(yàn)結(jié)果

4.1 實(shí)驗(yàn)現(xiàn)場布局

將用HSR-JR630型工業(yè)機(jī)器人對上述所提出的標(biāo)定方法進(jìn)行標(biāo)定實(shí)驗(yàn)，實(shí)驗(yàn)現(xiàn)場布置如圖1所示。位置數(shù)據(jù)采集利用Leica AT960MR激光跟蹤儀及配套的激光反射靶球和點(diǎn)位采集軟件Spatial Analyer。

圖1 實(shí)驗(yàn)現(xiàn)場

4.2 實(shí)驗(yàn)結(jié)果分析

基于建立的位置誤差模型和預(yù)標(biāo)定得到的基坐標(biāo)系及工具坐標(biāo)系，用MIEKF算法辨識后的幾何參數(shù)誤差結(jié)果如圖2所示?？梢钥闯觯篗IEKF算法經(jīng)過觀測矩陣的歸一化、遺忘因子優(yōu)化協(xié)方差矩陣及利用觀測位置迭代，幾何參數(shù)誤差在兩輪迭代內(nèi)就可以快速收斂到穩(wěn)定值。

圖2 MIEKF算法辨識得到的參數(shù)誤差

根據(jù)辨識的結(jié)果對預(yù)標(biāo)定的基坐標(biāo)系和工具坐標(biāo)系進(jìn)行修正后的實(shí)際坐標(biāo)為

表2為MIEKF算法辨識得到HSR-JR630型機(jī)器人的各個(gè)關(guān)節(jié)實(shí)際幾何參數(shù)誤差。

使用MIEKF算法標(biāo)定后的精度再與EKF算法標(biāo)定的進(jìn)行比較，可以得到HSR-JR630型機(jī)器人的位置精度如表3和圖3所示。

表3 位置誤差的對比 單位：mm

從表3和圖3可知：未校正時(shí)，機(jī)器人的最大誤差和平均誤差分別為3.328、2.011 mm；用MIEKF標(biāo)定后的最大誤差和平均誤差分別為0.397、0.122 mm，精度提升了88.07%。并且實(shí)驗(yàn)結(jié)果表明，MIEKF算法比EKF辨識出機(jī)器人的幾何參數(shù)誤差精度更準(zhǔn)，提升的機(jī)器人的絕對定位精度效果更好。

5 結(jié)論

本文作者提出了基于預(yù)標(biāo)定基坐標(biāo)系及MIEKF算法的運(yùn)動學(xué)標(biāo)定方法，該方法考慮了激光跟蹤儀采集數(shù)據(jù)的效率，建立完整最小連續(xù)性的誤差模型以及高效率的辨識算法。利用該方法在HSR-JR630型工業(yè)機(jī)器人上進(jìn)行實(shí)驗(yàn)，結(jié)果表明：基坐標(biāo)系和工具坐標(biāo)系的預(yù)標(biāo)定有效減少了兩者擬合時(shí)間，提高標(biāo)定效率。應(yīng)用MIEKF算法在辨識機(jī)器人幾何參數(shù)誤差時(shí)克服了對初始參數(shù)條件敏感的問題，具有辨識精度高、收斂速度快和高魯棒性的特點(diǎn)。最大誤差和平均誤差分別從3.328、2.011 mm減小到0.397、0.122 mm，有效提高了HSR-JR630型機(jī)器人的絕對定位精度，為工業(yè)機(jī)器人運(yùn)動學(xué)標(biāo)定技術(shù)提供了新的方法和思路，具有很重要的實(shí)際應(yīng)用價(jià)值。