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

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

1 引言

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

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

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

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

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

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

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

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

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

2 6R模塊化機(jī)器人正運(yùn)動(dòng)學(xué)分析

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

機(jī)械臂正運(yùn)動(dòng)學(xué)求解是已知機(jī)械臂各關(guān)節(jié)角度，求取機(jī)械臂末端手抓在基坐標(biāo)系下的期望位姿，6R機(jī)器人的前三個(gè)關(guān)節(jié)角度確定手腕參考點(diǎn)的位置，后三個(gè)關(guān)節(jié)角度確定手腕的姿態(tài)，根據(jù)機(jī)器人的結(jié)構(gòu)參數(shù)和外形尺寸，運(yùn)用D-H法建立各個(gè)關(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模塊化機(jī)器人的D-H參數(shù)表，如表1所示。

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

2.2 正運(yùn)動(dòng)學(xué)解算

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

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

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

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

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

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

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

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

3 逆運(yùn)動(dòng)學(xué)求解

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

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

3.1 求解θ1、θ2、θ3

令式（22）兩邊的（1，4）、（2，4）元素分別對(duì)應(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ò)模型拓?fù)浣Y(jié)構(gòu)包括輸入層，隱含層，輸出層，其結(jié)構(gòu)，如圖2所示。

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

式中：—輸入層第j個(gè)神經(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個(gè)神經(jīng)元在樣本p作用下的輸出。對(duì)于每一個(gè)樣本P的輸入模式對(duì)的二次型誤差函數(shù)為：

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

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

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

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

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

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

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

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

vi—第i個(gè)聚類中心；

m—模糊指數(shù)；

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

c—聚類個(gè)數(shù)；

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

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

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

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

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

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

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

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

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

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

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

4 6R機(jī)器人軌跡仿真

4.1 運(yùn)動(dòng)軌跡

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

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

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

4.2 逆解模型效果評(píng)定

為了驗(yàn)證該逆解模型方法的有效性，采用以下兩個(gè)指標(biāo)絕對(duì)誤差和均方根誤差進(jìn)行評(píng)價(jià)，即：

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

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

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