林耿聰,肖曙紅,楊林,盧浩文,張建華
(1.廣東工業(yè)大學(xué)機(jī)電工程學(xué)院,廣東廣州 510006;2.佛山智能裝備技術(shù)研究院,廣東佛山 528000;3.佛山華數(shù)機(jī)器人有限公司,廣東佛山 528000)
目前,工業(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ī)器人的絕對定位精度。
R(Xi,αi)·R(Yi,βi)=
(1)
其中:cθi表示cosθi;sθi表示sinθi;cαi表示cosαi;sαi表示sinαi。
根據(jù)HSR-JR630型工業(yè)機(jī)器人的結(jié)構(gòu)參數(shù)確定其MDH參數(shù),如表1所示。
表1 HSR-JR630型工業(yè)機(jī)器人的理論MDH參數(shù)
(2)
首先根據(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.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的辨識雅克比矩陣。
首先要確定冗余參數(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的列向量。
改進(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)
將用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)場
基于建立的位置誤差模型和預(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ī)器人的絕對定位精度效果更好。
本文作者提出了基于預(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à)值。