6R模塊化機器人逆解算法與誤差分析

劉廣瑞，孟少飛，王 釗，高建設(shè)

（鄭州大學(xué)機械與動力工程學(xué)院，河南 鄭州 450001）

1 引言

6R機器人是一個集成度很高的機電一體化產(chǎn)品，運動學(xué)是其研究的首要前提，包括正逆運動學(xué)，而逆運動學(xué)求解的精度、泛化能力、實時性等是機器人進行軌跡規(guī)劃和離線編程的基礎(chǔ)，機器人的逆運動學(xué)模型是一個多輸入多輸出系統(tǒng)，其逆運動學(xué)方程具有高維、非線性的特點，求解復(fù)雜且不易求出。

許多學(xué)者對6R 串聯(lián)機械臂的逆運動學(xué)研究做了很大的貢獻[1-3]，例如解析法，幾何法和數(shù)值迭代法等。解析法雖然可以得到全部解，但計算過程較復(fù)雜；幾何法只能簡化某些特殊的結(jié)構(gòu)，通用性較差；迭代法具有通用性，但卻無法得到全部的解，且存在迭代時間長和迭代不收斂的情況，進行實時控制時可靠性低。

神經(jīng)網(wǎng)絡(luò)作為一種智能算法，能在一個有限的集合內(nèi)和任意精度下逼近任何非線性函數(shù)，影響網(wǎng)絡(luò)的收斂精度主要有兩個因素，訓(xùn)練樣本的質(zhì)量、網(wǎng)絡(luò)的結(jié)構(gòu)參數(shù)。

不少學(xué)者運用神經(jīng)網(wǎng)絡(luò)對機器人逆運動學(xué)求解方面進行了研究工作，訓(xùn)練樣本采集主要為運動學(xué)正解和實驗獲取兩種途徑。

文獻[4]采用RBF 網(wǎng)絡(luò)研究了SCARA 機器人逆解問題，訓(xùn)練樣本通過對關(guān)節(jié)角取等間隔獲得，雖取得了滿意結(jié)果，但精度和泛化能力仍需提高。

文獻[5-6]分別采用RBF網(wǎng)絡(luò)和MFBP網(wǎng)絡(luò)研究了機器人逆解問題，雖然逆解角度取得了不錯的效果，但對樣本篩選處理的算法過程過于復(fù)雜，由于逆解多解的存在，剔除矛盾樣本時人為干擾因素較多，增加了逆解求解的難度和時間周期。

文獻[7]采用BP網(wǎng)絡(luò)研究了自主研發(fā)的6R機器人逆解問題，取得了滿意的結(jié)果，但訓(xùn)練樣本通過實驗獲取，浪費大量精力，且樣本容量較小，訓(xùn)練的網(wǎng)絡(luò)泛化能力較差。

不難看出，單獨的用解析法、幾何法、數(shù)值迭代法進行機器人逆運動學(xué)求解具有一定的局限性，雖然神經(jīng)網(wǎng)絡(luò)在機器人逆運動學(xué)中取得了一定的成就，但由于機器人逆解多解和奇異性問題使得在神經(jīng)網(wǎng)絡(luò)求解的精度、泛化能力、篩選訓(xùn)練樣本算法的問題上仍有進一步的提升空間。

因此為了提高求解的精度和縮短逆解求解的時間周期，這里采用解析法與BP神經(jīng)網(wǎng)絡(luò)相結(jié)合的方法求解6R模塊化機器人逆運動學(xué)，解析法求解前幾個關(guān)節(jié)逆解，其余使用BP神經(jīng)網(wǎng)絡(luò)預(yù)測求逆解，即（q解析+BP，q=0,1…6）通過使用不同的組合模式，仿真對比發(fā)現(xiàn)，3解析+BP組合模型求解精度比其它組合模式具有更高的精度，較強的泛化能力，且縮短了求逆解的時間周期，該求逆解模型為6R機器人求逆解提供了新的思路和方法。

2 6R模塊化機器人正運動學(xué)分析

2.1 結(jié)構(gòu)參數(shù)建模

機械臂正運動學(xué)求解是已知機械臂各關(guān)節(jié)角度，求取機械臂末端手抓在基坐標(biāo)系下的期望位姿，6R機器人的前三個關(guān)節(jié)角度確定手腕參考點的位置，后三個關(guān)節(jié)角度確定手腕的姿態(tài)，根據(jù)機器人的結(jié)構(gòu)參數(shù)和外形尺寸，運用D-H法建立各個關(guān)節(jié)的坐標(biāo)系，其結(jié)構(gòu)示意圖和各連桿變換坐標(biāo)系，如圖1所示。

相鄰兩連桿坐標(biāo)系的齊次變換矩陣為；

式中：ai-1—連桿的長度；αi-1—扭角；di—偏置；θi—關(guān)節(jié)角。其中：ci=cos(θi)，si=sin(θi)。

6R模塊化機器人的D-H參數(shù)表，如表1所示。

表1 6R模塊化機器人D-H參數(shù)Tab.1 D-H Parameters of 6-R Modular Robot

2.2 正運動學(xué)解算

根據(jù)相鄰連桿坐標(biāo)的齊次變化矩陣，機器人末端相對于坐標(biāo)系0的位姿矩陣為：

式中：En—機器人末端姿態(tài)；Pn—機器人末端位置。

式中：sij=sin(θi+θj)；cij=cos(θi+θj)。

機器人作業(yè)時是已知手抓工具末端的位姿矩陣，而求逆解需知機器人末端的位姿矩陣即：

上式對機器人直接求逆是非常困難的，需要對12個非線性方程進行求解，由于神經(jīng)網(wǎng)絡(luò)可以預(yù)測非線性系統(tǒng)，因此采用解析+BP神經(jīng)網(wǎng)絡(luò)相結(jié)合的方式求逆解。

網(wǎng)絡(luò)訓(xùn)練時為了減小網(wǎng)絡(luò)結(jié)構(gòu)負擔(dān)，把機器人末端姿態(tài)變量En轉(zhuǎn)化為繞固定軸x-y-z變化的RPY角，根據(jù)坐標(biāo)轉(zhuǎn)換原理可得末端姿態(tài)En的RPY角為：

根據(jù)文獻[8]可知，采用解析法求解時，后一個關(guān)節(jié)角的求解需在前一個關(guān)節(jié)角的求解基礎(chǔ)之上，隨著解析解求關(guān)節(jié)角度個數(shù)的增加，求解算法越來越復(fù)雜，前三個關(guān)節(jié)確定機器人位置，后三個確定姿態(tài)，求機器人前三個關(guān)節(jié)角度比后三個關(guān)節(jié)角度簡單許多，所以qmax=3。

針對大部分6R機器人，如PUMA560、IRB120等，多解一般產(chǎn)生在前三個關(guān)節(jié)，當(dāng)q=3時，對神經(jīng)網(wǎng)絡(luò)訓(xùn)練樣本進行篩選，簡化了逆解多解的問題，減少了矛盾樣本的容量，從而縮短了篩選樣本的時間周期，因此當(dāng)q=3時，該逆解組合求解模型較優(yōu)，計算周期最短。以下主要討論3解析+BP組合模型的求解過程。

3 逆運動學(xué)求解

該機器人最后三個關(guān)節(jié)軸線交于一點，滿足Pieper準(zhǔn)則，存在封閉解。由于直接求解12個非線性方程組較為復(fù)雜，所以根據(jù)機器人末端位姿用解析法求前三個關(guān)節(jié)角度，從而得到的位姿，根據(jù)齊次矩陣變換原理得到手爪末端相對于第三個關(guān)節(jié)坐標(biāo)的位姿變化矩陣為：

接下來用BP神經(jīng)網(wǎng)絡(luò)對后三個關(guān)節(jié)角度進行預(yù)測。

3.1 求解θ1、θ2、θ3

令式（22）兩邊的（1，4）、（2，4）元素分別對應(yīng)相等有：

因此求出關(guān)節(jié)2、3的角度和：

當(dāng)選中關(guān)節(jié)3的一組解后，關(guān)節(jié)2的角度也隨之確定，即：

3.2 求解θ4、θ5、θ6

BP 神經(jīng)網(wǎng)絡(luò)是一種按誤差逆?zhèn)鞑ニ惴ㄓ?xùn)練的多層前饋網(wǎng)絡(luò)，學(xué)習(xí)規(guī)則使用最速下降法，通過反向傳播不斷調(diào)整網(wǎng)絡(luò)的權(quán)值和閾值，使網(wǎng)絡(luò)的誤差平方和最小。網(wǎng)絡(luò)模型拓撲結(jié)構(gòu)包括輸入層，隱含層，輸出層，其結(jié)構(gòu)，如圖2所示。

網(wǎng)絡(luò)的輸入為機器人手抓末端位姿變量α、β、γ、Px、Py和Pz，網(wǎng)絡(luò)的輸出為機器人各個關(guān)節(jié)角，網(wǎng)絡(luò)的輸入和輸出的關(guān)系為：

式中：—輸入層第j個神經(jīng)元在樣本p作用下的輸入(j，p=1，2，3…M)；Wij、θi—輸入層到隱含層的權(quán)值和隱含層的閾值(i=1，2，3…，q)；g1—隱含層的激活函數(shù)；Wki、θj—隱含層到輸出層的權(quán)值和輸出層的閾值。(k=1，2，3…，L)；g2—輸出層的激活函數(shù)—輸出層第k個神經(jīng)元在樣本p作用下的輸出。對于每一個樣本P的輸入模式對的二次型誤差函數(shù)為：

BP網(wǎng)絡(luò)的實質(zhì)就是根據(jù)誤差Jp函數(shù)梯度變換的反方向不斷的調(diào)整網(wǎng)絡(luò)的權(quán)值和閾值，當(dāng)網(wǎng)絡(luò)的全局誤差或迭代次數(shù)滿足設(shè)定的條件時，網(wǎng)絡(luò)停止訓(xùn)練。

訓(xùn)練數(shù)據(jù)的準(zhǔn)備工作是網(wǎng)絡(luò)設(shè)計與訓(xùn)練的基礎(chǔ)，數(shù)據(jù)選擇的科學(xué)合理性不但影響網(wǎng)絡(luò)的學(xué)習(xí)和泛化能力，而且影響網(wǎng)絡(luò)的精度。

因前三個關(guān)節(jié)角度由解析法求出，所以訓(xùn)練樣本只針對后三個關(guān)節(jié)角取值，假定選取各個關(guān)節(jié)角度的間隔為θ，單位為度，可得總的樣本容量N為：

選取訓(xùn)練樣本時，若取各關(guān)節(jié)的角度間隔θ=2時，可得N=757666。雖然樣本覆蓋范圍較廣，但樣本之間可能存在冗余和矛盾樣本，因此要對樣本進行優(yōu)選。

許多學(xué)者完成了用模糊C均值聚類算法（FCM）對樣本數(shù)據(jù)進行聚類，但該算法存在的嚴(yán)重缺陷是對初始聚類中心敏感且不易收斂于全局最優(yōu)，因此，這里采用基于核的改進的模糊C均值聚類算法（KFCM）[9-10]對訓(xùn)練樣本進行優(yōu)選。它通過核函數(shù)將訓(xùn)練樣本映射到特征空間中，求取聚類中心，使得目標(biāo)函數(shù)最小，目標(biāo)函數(shù)為：

式中：K(xj，vi)—高斯徑向基函數(shù)。得：

利用拉格朗日的極值必要條件，得出：

式中：uij—第j個樣本屬于第i個聚類中心的隸屬度；

vi—第i個聚類中心；

m—模糊指數(shù)；

δ—徑向基函數(shù)的參數(shù)；

c—聚類個數(shù)；

采用上式可得到訓(xùn)練樣本數(shù)據(jù)的聚類中心，接下來用隨機抽取算法優(yōu)選訓(xùn)練樣本。算法步驟如下；

（1）確定各個關(guān)節(jié)角度取值間隔，通過機器人正運動學(xué)得出機器人手抓末端工具相對于第三個關(guān)節(jié)的位姿矩陣樣本數(shù)據(jù)。

（2）對樣本數(shù)據(jù)進行預(yù)處理，剔除矛盾樣本和冗余的樣本，以減少樣本容量。

（3）設(shè)迭代次數(shù)k=0，算法最大迭代次數(shù)為maxiter，收斂精度ε，用FCM算法初始化中心矩陣V(0)。

（4）用式（31）、式（32）計算Uk+1，Vk+1，令k=k+1。

（5）重復(fù)步驟④，直到滿足如下終止條件；k≥maxiter或或存在i(1 ≤i≤c)使得。

（6）采用隨機抽取算法對聚類后的樣本進行選取，得到優(yōu)選的網(wǎng)絡(luò)訓(xùn)練樣本。

由于網(wǎng)絡(luò)的輸入數(shù)據(jù)常常具有不同的物理意義和量綱，為防止因凈輸入的絕對值過大而使神經(jīng)元輸出飽和，需對訓(xùn)練樣本進行歸一化處理，歸一化后的樣本參數(shù)范圍在(0，1)，歸一化的公式為：

式中：xi-輸入/輸出數(shù)據(jù)；xmin-數(shù)據(jù)范圍的最小值；xmax-數(shù)據(jù)范圍的最大值。

根據(jù)以上算法步驟，在機器人的工作空間范圍內(nèi)約選出2000個訓(xùn)練樣本，對手抓末端位姿經(jīng)式（16）變換，然后經(jīng)式（33）進行歸一化處理，參考文獻［11］中方法，對BP神經(jīng)網(wǎng)絡(luò)進行訓(xùn)練。

經(jīng)過訓(xùn)練后的關(guān)節(jié)角度是（0～1）之間的數(shù)，需要進行反歸一化處理，即：

4 6R機器人軌跡仿真

4.1 運動軌跡

為了驗證該逆解模型方法的正確性，根據(jù)該6R模塊化機器人的結(jié)構(gòu)參數(shù)，用Matlab軟件的Robotics工具箱建立機器人連桿模型，并使機器人手抓末端仿真一段圓弧曲線，如圖3所示。

圖3 機器人作業(yè)軌跡圖Fig.3 Robot Operation Trajectory Diagram

隨機取20個點作為預(yù)測點，并確立這些點的末端姿態(tài)，由式（15）、式（16）得到20組α、β、γ、Px、Py和Pz，經(jīng)歸一化后作為BP神經(jīng)網(wǎng)絡(luò)的測試數(shù)據(jù)，得到20組網(wǎng)絡(luò)預(yù)測輸出值，并由式（34）反歸一化處理。

4.2 逆解模型效果評定

為了驗證該逆解模型方法的有效性，采用以下兩個指標(biāo)絕對誤差和均方根誤差進行評價，即：

式中：θyci—第i個關(guān)節(jié)的逆解角度；θi—第i個關(guān)節(jié)角度的理論值；N—測試樣本數(shù)，N=20。

由于采用解析解+BP神經(jīng)網(wǎng)絡(luò)求逆解，逆解存在多解，為了更好評定網(wǎng)絡(luò)預(yù)測的效果，逆解只選取與20個位置點關(guān)節(jié)角的理論值相對應(yīng)的角度。20個作業(yè)位置的關(guān)節(jié)角度理論值、逆運動學(xué)求解值、關(guān)節(jié)角預(yù)測誤差值，如表2所示。求得后三個關(guān)節(jié)角在20個作業(yè)位置的絕對誤差值，如圖4所示。

表2 6R機器人20個作業(yè)位置的逆解Tab.2 Inverse Solution of 20 Working Positions of 6R Robot

圖4 后三個關(guān)節(jié)角絕對誤差值Fig.4 Absolute Error of the Angle of the Last Three Joints

從表2和圖4中看出，前三個關(guān)節(jié)角的絕對誤差和均方根誤差均為0，證明了該正運動學(xué)模型建立和解析法求解的正確性。后三個關(guān)節(jié)角求解的絕對誤差最小值為6.8212e-8，絕對誤差最大值為2.0922e-4，均方根誤差最小值為1.1812e-5，均方根誤差最大值為1.2218e-4。通過以上數(shù)據(jù)可以看出，基于前三個關(guān)節(jié)解析法+BP神經(jīng)網(wǎng)絡(luò)的逆解預(yù)測模型具有很高的求解精度。

為了客觀的評價該算法的求解精度和可行性，用和該算法類似的求解步驟分別建立不同組合的數(shù)學(xué)模型(qmax=3)；0 解析+BP、1解析+BP、2解析+BP。

根據(jù)表1中6R機器人關(guān)節(jié)轉(zhuǎn)角限制范圍任取兩組關(guān)節(jié)角，對以上建立的不同組合模型進行逆解求解精度的對比，結(jié)果，如表3所示。

通過表3 對任取兩組關(guān)節(jié)角逆解求解的結(jié)果得出，0解析+BP求解的絕對誤差最小值為0.0474，絕對誤差最大值為0.2972，均方根誤差為0.1623，1 解析+BP 求解的絕對誤差最小值為0.0000，絕對誤差最大值為0.1344，均方根誤差為0.0653，2解析+BP求解的絕對誤差最小值為0.0000，絕對誤差最大值為0.0928，均方根誤差為0.0402，3 解析+BP 求解的絕對誤差最小值為0.0000，絕對誤差最大值為0.0001，均方根誤差為6.4550e-05。通過對比以上數(shù)據(jù)的絕對誤差值、均方根誤差值，3解析+BP組合模式的誤差均小于其它的求解組合模式，說明該求逆解組合方式不但模型更優(yōu)、計算周期短，而且具有較高的求解精度、泛化能力和魯棒性。

表3 6R模塊化機器人不同組合模式的逆解Tab.3 Inverse Solution of Different Combination Modes of 6R Modular Robot

5 結(jié)論

（1）提出了一種解析法+BP神經(jīng)網(wǎng)絡(luò)相結(jié)合求解6R機器人逆運動學(xué)的方法，并以6R模塊化機器人為例進行逆運動求取，通過運動學(xué)軌跡仿真驗證了該方法的有效性與可行性。

（2）通過對q解析法+BP神經(jīng)網(wǎng)絡(luò)相結(jié)合求逆解的4種組合模式進行仿真誤差精度對比，結(jié)果顯示隨著q值的增大逆解模型的求解精度逐漸提高，且進行神經(jīng)網(wǎng)絡(luò)訓(xùn)練時矛盾樣本的容量逐漸減少，縮短了樣本篩選的時間周期，即當(dāng)q=3時，該逆解模型最優(yōu)。

（3）3解析法+BR神經(jīng)網(wǎng)絡(luò)建立的求逆解模型可廣泛用于其他一般的6R機器人，獲得較高的逆解精度、較短的計算時間周期并能準(zhǔn)確的進行軌跡規(guī)劃。