自適應卡爾曼濾波與PSO-GA-BP算法的機器人誤差補償

李光保 高 棟 路 勇 平 昊 周愿愿

1.哈爾濱工業大學機電工程學院,哈爾濱,1500002.上海航天精密機械研究所,上海,201600

0 引言

某型號發射筒(箱)口框位置的切割開孔,傳統模式是采用人工劃線、手持切割鋸進行切割,存在質量穩定性差、效率低、勞動強度大等問題。為了滿足常態化高強密度發射形式下導彈快速、批量研制的需求,考慮到激光切割(以中薄板為主)的切割速度快、加工精度高、切縫狹窄等一系列優點[1-2]以及待切割對象形狀各異的特點,采用七軸機器人夾持激光切割頭的方式對發射筒(箱)進行切割,配合工件裝夾平臺,發射筒(箱)可完成360°回轉,從而將可切割范圍覆蓋整個發射筒(箱)。但是工業機器人在安裝和加工過程中存在幾何誤差[3-5],另外,其末端在負載和各關節自重的影響下結構發生受力變形,相對應的工業機器人位置也發生偏差,而且運動過程中控制指令發出的脈沖信號容易受到環境干擾,多種因素共同作用使得工業機器人的軌跡精度和定位精度無法得到保證[6-7]。

目前工業機器人誤差補償主要通過激光跟蹤儀或多傳感器融合采集工業機器人位姿信息作為機器人真實位姿信息。由于一般未考慮測量不確定度問題[8],實際提出的補償量誤差較大。本文針對七軸激光切割機器人,基于Sage-Husa自適應卡爾曼濾波方法建立理想機器人模型[9-10],求解真實的七軸機器人位姿參數;同時采用粒子群優化-遺傳算法-BP神經網絡(PSO-GA-BP)聯合算法建立七軸機器人關節補償預測模型,并與遺傳算法-BP神經網絡(GA-BP)算法及粒子群優化-BP神經網絡(PSO-BP)算法的預測結果進行對比,最后通過Workvisual 5.0軟件對工業機器人各坐標點誤差補償,然后進行加工。

1 七軸激光切割機器人工作原理

七軸激光切割機器人主要由庫卡機器人、外部第七軸、發射筒(箱)主軸箱、激光單元、控制系統、水冷機、設備操作臺、機器人導軌平臺、激光安全防護房等部分組成,如圖1所示。機器人選擇庫卡機器人KR50 R2500結合Y向外部軸電機構成七軸機器人,另外在七軸機器人的末端安裝激光切割頭,搭建相應的七軸KRC4高性能機器人控制器,附帶相應的手持編程器?？刂破鬟x配Beckhoff EK1100 EtherCAT通信耦合器與主控制器內部通信,通過EL6631 Profinet通信端子模塊進行Profinet通信網路組態與PLC通信,確?？刂葡到y數據信息的實時性能。通過對主控制器進行編程,將激光單元、主軸箱電機、水冷機、輔氣系統的運行與機器人本體及第七軸動作結合,合理設計邏輯時序,實現發射筒(箱)激光切割工藝流程。

圖1 發射筒七軸激光切割機器人

2 七軸機器人模型的建立

為實現對七軸機器人的誤差補償,運用D-H建模方法建立七軸機器人模型,通過正逆運動學數值算法對建立的七軸機器人模型進行驗證,證明模型的正確性,為誤差補償預測模型的建立提供機器人理論位姿數據。

2.1 庫卡七軸機器人

發射筒(箱)激光切割采用的機器人結構形式如圖2所示,由六根可活動的、相互連接到一起的軸組成,同時在六軸機器人底座位置增加Y軸方向滑動的第七軸,用于機器人Y軸方向的移動。因此,采用的七軸激光切割機器人各連桿的空間坐標系如圖2所示。

圖2 庫卡KR50 R2500結構形式

2.2 D-H建模

表1 D-H參數

根據表1中的機器人D-H參數,運用機器人工具箱中的Link函數分別建立6個旋轉桿件模型和1個移動桿件模型,采用qlim函數定義A1～A7的關節驅動范圍,使用SerialLink類函數將Link函數建立的連桿連成一個整體,生成一個串聯機械臂模型。運行程序后七軸機器人模型如圖3所示。此時,θ1=0.897 43 m,θ2=-1.4694 rad,θ3=-1.8459 rad,θ4=-3.1416 rad,θ5=0.241 79 rad,θ6=-0.436 66 rad,θ7=1.7907 rad,x=1.651 m,y=0.275 m,z=0。

圖3 七軸庫卡機器人模型

2.3 正逆運動學驗證

根據D-H算法確定的參數,用旋轉和位移來表示相鄰關節坐標系間齊次變換關系[13]:

(1)

式中,(nx,ny,nz)T、(ox,oy,oz)T、(ax,ay,az)T均為激光切割器在基坐標系下的旋轉姿態參數;(px,py,pz)T為激光切割器在基坐標系下的位置參數。

七軸機器人末端激光切割器在機器人基坐標系下的位姿可定義為(x,y,z,α,β,γ)T,其中:

(3)

(4)

(5)

式中,(x,y,z)為激光切割器的位置坐標;(α,β,γ)為激光切割器坐標系與基座坐標系三坐標軸的夾角。

隨機選取七軸機器人運行范圍內的各關節參數進行驗證,如圖4所示。此時,θ1=1.092 m,θ2=0.132 93 rad,θ3=0.181 37 rad,θ4=-0.156 43 rad,θ5=0.1885 rad,θ6=-3.1166 rad,θ7=-1.8221 rad,x=1.642 m,y=1.312 m,z=0.106 m。

圖4 七軸機器人正運動學求解

根據表2隨機產生的七軸關節角度值,運用正運動學數值法建立機器人模型,求得的位姿誤差最大值為0.0003 m,驗證了本文中建立的七軸機器人模型具有較高的正運動學計算精度。

表2 機器人正運動學計算末端姿態值

運用代數求解法完成逆運動學求解,逐個分離變量求解,得到各關節數值[14],即用矩陣逐次左乘簡化方程,可得如下方程:

(6)

(7)

計算結果如表3所示。

表3 逆運動學求解的各關節角度值

根據隨機選擇的機器人位姿運用代數求解法和運用機器人工具箱建立機器人模型,求得各關節角度值誤差最大值為0.0003 rad,驗證了本文中建立的七軸機器人模型具有較高的逆運動學計算精度。

由上述計算結果可證明建立的七軸庫卡機器人仿真模型具有較高的數據可信度,可作為機器人位姿理論數據進行處理。

3 Sage-Husa自適應卡爾曼濾波

3.1 卡爾曼濾波

卡爾曼濾波是根據上一時刻的最優估計值和此時刻的測量值來進行連續的加權計算,對測量環境中存在的各種混合噪聲干擾進行濾除[15],從而得到當前時刻的最優估計值。結合本文研究的問題,以某一時刻七軸機器人理論模型顯示的位姿為卡爾曼濾波算法中的預測值,激光跟蹤儀測量的七軸機器人位姿為卡爾曼濾波算法中的測量值,建立的狀態方程和測量方程如下:

狀態方程

Xk+1=φkXk+Bkuk+ωk

(8)

式中,Xk+1為k+1時刻6維的系統狀態變量,包括(x,y,z,r,p,l)機器人位姿參數;φk、Bk為系統參數;uk為機器人控制系統的輸入量;ωk為機器人控制系統噪聲偏差。

觀測方程

Yk+1=Ck+1Xk+1+γk+1

(9)

式中,Yk+1為k+1時刻的機器人位姿測量值;Ck+1為k+1時刻的測量系統參數;γk+1為激光跟蹤儀測量噪聲偏差。

卡爾曼濾波隨著時間的更新而不停地更新預測值和校正值,其中:

(10)

(11)

(12)

(13)

(14)

式(10)、式(11)、式(12)為預測方程,式(13)、式(14)為校正方程。式(10)～式(14)為卡爾曼濾波算法的迭代公式,根據分析發現卡爾曼濾波主要依賴系統噪聲和測量噪聲模型的準確性。

3.2 基于Sage -Husa的自適應卡爾曼濾波器

針對以上情況,本文采用基于Sage-Husa的自適應卡爾曼濾波來解決系統噪聲和測量噪聲模型不準確的問題,實時更新估計突變噪聲的均值、協方差,從而對模型參數和噪聲特性進行實時修正,以減小與實際情況的誤差[16]?；赟age-Husa的自適應卡爾曼濾波算法是在卡爾曼濾波每次迭代的同時對噪聲模型進行一次更新計算,使得系統噪聲和測量噪聲均值、協方差的值保持在最優估計值的狀態,系統噪聲和測量噪聲均值、協方差更新如下所示:

(15)

(16)

(17)

dk=(1-b)/(1-bk+1)

(18)

3.3 結果對比

將采集的數據分別使用卡爾曼濾波與基于Sage-Husa的自適應卡爾曼濾波進行濾波降噪。根據工程實踐的經驗法,采集系統中卡爾曼濾波器的系統噪聲協方差設為0.001,測量噪聲協方差R設為0.0025,本文采用七軸庫卡機器人中Y軸坐標信息進行實驗,卡爾曼濾波與基于Sage-Husa的自適應卡爾曼濾波的數據處理結果如圖5所示,誤差如圖6所示。

圖5 數據分析

圖6 誤差分析

4 基于PSO-GA-BP的神經網絡誤差模型的建立

針對傳統機器人誤差模型預測方法精度低、需求量大的問題,提出誤差反傳BP神經網絡誤差預測模型[17],并利用遺傳算法GA模型優化BP神經網絡結構模型,再運用粒子群算法PSO優化GA-BP神經網絡模型的權值和閾值,從而提高模型的收斂速度以及預測精度。PSO-GA-BP的神經網絡誤差模型算法流程如圖7所示。數據集分為訓練數據集和測試數據集。

圖7 PSO-GA-BP神經網絡算法流程

4.1 BP神經網絡模型

BP神經網絡是一種按誤差反向傳播算法訓練的前向神經網絡,一般由輸入層、隱含層和輸出層組成[18],其數學模型如下所示:

Iout1=f1(w1Iin+b1)

(19)

Iout2=f2(w2Iout1+b2)

(20)

(21)

式中,Iin、f1、w1、Iout1和b1分別為神經網絡輸入層的輸入、隱含層的傳遞函數、輸入層和隱含層之間的連接權值、隱含層的輸出、輸入層和隱含層之間的連接閾值;Iout2、f2、w2、和b2分別為輸出層的輸出、輸出層的傳遞函數、隱含層與輸出層的連接閾值、隱含層與輸出層的連接閾值;h為隱含層節點數;m為輸入層節點數;n為輸出層節點數;ae為常數,ae∈[1,10]。

理論證明3層的神經網絡能逼近任意的非線性映射關系,因此本文采用3層神經網絡結構。

根據BP神經網絡模型,首先選取七軸庫卡機器人的理想位姿參數作為BP神經網絡模型輸入,包括坐標位置信息以及沿X、Y、Z方向的轉動角度。理論位姿計算出的各軸關節角度值與經過Sage-Husa自適應濾波后的各軸關節角度值之差作為BP神經網絡模型的輸出。輸入為6個參數,輸出為7個參數,隱含層的節點數根據經驗式(21)確定。

基于BP神經網絡的七軸機器人誤差預測模型如圖8所示。

圖8 BP神經網絡誤差預測模型

根據經驗公式,隱含層的節點數h區間為[4,13]。選擇的隱含層節點數越多,則非凸的目標函數越復雜,局部最小值點成倍增長,利用有限的數據集訓練的BP神經網絡模型性能較差,而且結構參數也會呈幾何級增長;若訓練數據較少,則容易出現過擬合現象。過少的隱含層節點又不能完整地反映輸入和輸出的函數關系。針對此類問題,本文運用GA算法求解最適用的隱含層節點數。

4.2 GA-BP算法選取最優隱含層節點數

對GA算法首先進行種群初始化,包括種群規模、迭代次數,設置各參數的取值范圍。根據BP神經網絡的初始網絡結構,通過GA算法對BP神經網絡進行優化,以BP神經網絡模型預測值和實際值之間的誤差平方和為適應度函數。

(1)選擇操作。本文選用基于適應度比例的輪盤賭法選擇策略,每個個體i的選擇概率Pi為

式中,Fi為個體i的適應度值,由于適應度越小越好,所以在個體選擇前對適應度值取倒數;ke為系數;N為種群個數。

(2)交叉操作。由于在本文中個體采用實數編碼,所以交叉操作采用實數交叉法,第k個染色體ak和第i個染色體ai在j位的交叉操作如下:

akj=akj(1-be)+aijbe

aij=aij(1-be)+akjbe

式中,be為[0,1]之間的隨機數。

(3)變異操作。本文采用單點變異方法,即選取第i個個體的第j個基因進行變異:

f(g)=r2(1-g/Gmax)2

式中,amax、amin為基因aij的上界和下界;r2為一個隨機數;g為當前迭代次數;Gmax為最大進化次數;r為[0,1]之間的隨機數。

適應度函數和適用隱含層節點數如圖9、圖10所示,根據仿真分析可知,隱含層節點數為10時均方差較小,故本文中的BP神經網絡結構選取10為隱含層節點數。

圖9 適應度曲線

圖10 隱含層節點數對應均方差

4.3 基于PSO-GA-BP算法的誤差預測模型

運用PSO 算法對BP神經網絡進行全局搜索,防止BP神經網絡陷入局部最優[19],通過更新粒子速度和位置尋找全局最優粒子,對 BP 神經網絡的權值和閾值進行優化,實現對七軸庫卡機器人誤差預測模型的構建。PSO算法參數設置為:慣性權重we,學習因子ce1、ce2,粒子最大速度和最大迭代次數。本文選用自適應調整的線性遞減慣性權重,慣性權重的典型權值為wemax=0.9,wemin=0.4,取初始權重we為0.9。學習因子滿足ce1+ce2=4,且ce1為2時迭代次數較小,ce1在兩端點0和4時迭代次數較大,出現對稱性;在兩端點出現搜索失敗率較大。取ce1=ce2=2,根據需要的當前位置與最好位置之間的分辨率(精度)設置粒子最大速度為0.5。為防止連續迭代達不到收斂狀態和觀測的本文粒子群算法迭代收斂次數,確定算法最大迭代次數為200。參數設置完成后對數據進行訓練和測試,得到各關節誤差補償值的預測與實際輸出的變化,如圖11～圖17所示。

圖11 關節1誤差補償值預測效果

圖12 關節2誤差補償值預測效果

圖13 關節3誤差補償值預測效果

圖14 關節4誤差補償值預測效果

圖15 關節5誤差補償值預測效果

圖16 關節6誤差補償值預測效果

圖17 關節7誤差補償值預測效果

4.4 GA-BP、PSO-BP、PSO-GA-BP算法預測數據的誤差對比

分別建立GA-BP和PSO-BP算法的誤差預測模型,經過數據集訓練和測試得到GA-BP算法、PSO-BP算法與PSO-GA-BP算法的預測值和實際值之間的各關節絕對誤差,如圖18～圖24所示。對比可知,PSO-GA-BP模型預測能力較高,預測值和實際值之間的誤差較小,為七軸庫卡機器人的誤差補償提供了較好的誤差補償模型。

圖18 不同算法對應的關節1輸出誤差絕對值

圖19 不同算法對應的關節2輸出誤差絕對值

圖20 不同算法對應的關節3輸出誤差絕對值

圖21 不同算法對應的關節4輸出誤差絕對值

圖22 不同算法對應的關節5輸出誤差絕對值

圖23 不同算法對應的關節6輸出誤差絕對值

圖24 不同算法對應的關節7輸出誤差絕對值

4.5 誤差補償與加工效果

本文通過上述分析得到機器人各關節誤差補償的預測輸出,然后通過庫卡機器人Workvisual 5.0軟件對各位姿坐標進行誤差補償,減小機器人幾何誤差、系統誤差及測量誤差對機器人軌跡的影響,從而提高機器人的定位精度和軌跡精度。對機器人各位姿坐標誤差補償完成后,再次運用攜帶激光器的七軸庫卡機器人對發射筒(箱)進行加工。誤差補償前后的機器人軌跡如圖25所示,補償前后的加工效果如圖26、圖27所示。

圖25 機器人軌跡

(a)補償前加工試驗件正面圖 (b)補償后加工試驗件正面圖

(a)補償前加工試驗件側面圖

5 結論

(1)本文采用機器人正逆運動學數值計算方法來驗證D-H機器人建模方式建立的七軸機器人模型,其計算結果和仿真結果對比發現,機器人位姿參數及各關節角度值誤差最大分別為0.0003 m和0.0003 rad,證明了模型具有較高的正確性。

(2)本文采用Sage-Husa自適應卡爾曼濾波來解決系統噪聲和測量噪聲的模型不準確問題,實時更新估計突變噪聲的均值、協方差,從而對模型參數和噪聲特性進行實時修正,以減小誤差,獲得更高的估計精度。

(3)對GA-BP、PSO-BP、PSO-GA-BP算法模型的預測誤差進行對比,結果顯示PSO-GA-BP算法模型具有較高的預測精度,各關節的預測輸出與實際輸出的絕對誤差最大為0.1 cm和0.1°。

(4)通過采用機器人軟件對各關節的誤差進行補償,得到的機器人仿真軌跡誤差減小約72%,切割實驗表明所得到的加工件滿足加工精度要求。