李 楊，魏兆勇，高國(guó)華，趙韋東，陳 平

(北京工業(yè)大學(xué)材料與制造學(xué)部，北京 100124)

近來(lái)，全球的災(zāi)難頻繁發(fā)生，人們的生命受到了非常大的危害.而災(zāi)害現(xiàn)場(chǎng)的復(fù)雜性和危險(xiǎn)性給救援人員的工作帶來(lái)了巨大的困難.隨著科學(xué)和社會(huì)的快速發(fā)展，國(guó)內(nèi)外的科學(xué)家都開始研究救援機(jī)器人.IRobot公司生產(chǎn)的Pack Bot機(jī)器人采用履帶輪行走，可適應(yīng)復(fù)雜的地形，采用連桿機(jī)械臂，可尋找幸存者[1]，負(fù)載可達(dá)到13.6 kg.八達(dá)重工公司設(shè)計(jì)出一種采用多自由度的雙臂機(jī)械臂，具有剪切等多種功能，可提起8 t的重物[2].寇彥蕓[3]設(shè)計(jì)了七自由度冗余機(jī)械臂，并采用液壓驅(qū)動(dòng)，考慮到救援的任務(wù)形式，采用雙臂作業(yè).

由于救援機(jī)械臂的自由度多，運(yùn)動(dòng)比較復(fù)雜，運(yùn)動(dòng)學(xué)模型的建立是機(jī)械臂軌跡規(guī)劃的基礎(chǔ)，決定了機(jī)械手的精度[4]，故需要對(duì)救援機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)分析，不同的機(jī)械臂構(gòu)型的逆運(yùn)動(dòng)學(xué)的解法也不相同.目前比較常用的運(yùn)動(dòng)學(xué)逆解的方法有解析法、迭代法、幾何法等傳統(tǒng)方法和遺傳算法、神經(jīng)算法等智能算法.張震等[5]提出了一種基于PSO-RBF神經(jīng)網(wǎng)絡(luò)的機(jī)械臂逆運(yùn)動(dòng)學(xué)算法，能夠很快得到精度高的關(guān)節(jié)角度，為機(jī)械臂的控制提供了基礎(chǔ).劉世平等[6]提出了建立BP神經(jīng)網(wǎng)絡(luò)模型來(lái)實(shí)現(xiàn)七自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解，并且進(jìn)行了軌跡跟蹤試驗(yàn)證明了模型的正確性.蔣燦燦等[7]利用旋量分析方法進(jìn)行了運(yùn)動(dòng)學(xué)分析,采用三次多項(xiàng)式軌跡規(guī)劃驗(yàn)證了方法的準(zhǔn)確性.傳統(tǒng)的方法求解比較復(fù)雜而且不能求出精確的唯一逆解，而智能算法往往不能保證求解的精度.

本文設(shè)計(jì)了一種高負(fù)載自重比的救援機(jī)械臂，通過(guò)底下移動(dòng)履帶車的行走實(shí)現(xiàn)對(duì)救援物資的搬運(yùn)工作.對(duì)救援機(jī)械臂正逆運(yùn)動(dòng)學(xué)方程進(jìn)行推導(dǎo)，利用機(jī)器人工具箱仿真驗(yàn)證運(yùn)動(dòng)學(xué)模型的正確性，運(yùn)用BP神經(jīng)網(wǎng)絡(luò)和解析法相結(jié)合的方式來(lái)求取逆解.通過(guò)BP神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)到的運(yùn)動(dòng)學(xué)結(jié)果來(lái)篩選解析法求出來(lái)的4組逆解，進(jìn)而可以得出唯一的精確解，為進(jìn)一步的軌跡規(guī)劃研究奠定了基礎(chǔ).

1 救援機(jī)械臂的總體方案

運(yùn)用三維畫圖軟件SolidWorks進(jìn)行機(jī)械臂模型的建立，五自由度機(jī)械臂可以使末端執(zhí)行器實(shí)現(xiàn)空間任意的位姿變換，機(jī)械臂的運(yùn)動(dòng)包括腰關(guān)節(jié)的旋轉(zhuǎn)、臂1關(guān)節(jié)及臂2關(guān)節(jié)的俯仰、臂3關(guān)節(jié)的旋轉(zhuǎn)和俯仰5個(gè)自由度.救援機(jī)械臂的整體模型如圖1所示.機(jī)械臂具有輕載和重載2種工作模式，通過(guò)結(jié)構(gòu)的鎖定和驅(qū)動(dòng)的轉(zhuǎn)換，實(shí)現(xiàn)輕載和重載模式的切換[8].

圖1 救援機(jī)械臂的整體模型

2 救援機(jī)械臂的運(yùn)動(dòng)學(xué)分析

2.1 救援機(jī)械臂的正運(yùn)動(dòng)學(xué)分析

2.1.1 坐標(biāo)系與參數(shù)的確立

Denavit-Hartenberg(D-H)參數(shù)法是Denavit和Hartenberg提出的，可以描述關(guān)節(jié)的變換，適合于連桿和關(guān)節(jié)的建模，通用性比較強(qiáng)[9].本文在每個(gè)關(guān)節(jié)處建立坐標(biāo)系如圖2所示.

圖2 救援機(jī)械臂坐標(biāo)系

通過(guò)D-H法則建立機(jī)械臂的連桿坐標(biāo)系來(lái)對(duì)救援機(jī)械臂連桿關(guān)系及參數(shù)進(jìn)行描述.從坐標(biāo)系xn-yn-zn到坐標(biāo)xn-1-yn-1-zn-1的變換矩陣為[10]

An+1=Rot(z,θn+1)×Trans(0,0,dn+1)×

Trans(an+1,0,0)×Rot(x,αn+1)=

(1)

2.1.2 機(jī)械臂D-H參數(shù)模型

在D-H關(guān)節(jié)坐標(biāo)系基礎(chǔ)上，建立D-H參數(shù)表，如表1所示.

表1 救援機(jī)械臂D-H參數(shù)表

2.1.3 救援機(jī)械臂正運(yùn)動(dòng)學(xué)建模

機(jī)械臂正運(yùn)動(dòng)學(xué)是指已知各關(guān)節(jié)的角度，求末端執(zhí)行器相對(duì)于基坐標(biāo)系的位姿.由式(1)可得機(jī)械臂末端坐標(biāo)系相對(duì)于基坐標(biāo)系的變換矩陣[11].正運(yùn)動(dòng)學(xué)公式為

(2)

nx=s1s5-s234c1c5

ny=-c1c5-s234c5s1

nz=c234c5

ox=c5s1+s234c1s5

oy=s234s1s5-c1c5

oz=-c234s5

ax=c234c1

ay=c234s1

az=s234

px=c1(a1+a3c23+a2c2+d5c234)

py=s1(a1+a3c23+a2c2+d5c234)

pz=d1+a3s23+a2s2+d5s234

式中：si表示sinθi；ci表示cosθi.

根據(jù)救援機(jī)械臂的工作范圍的設(shè)計(jì)指標(biāo)，本文中的參數(shù)設(shè)置為a1=-65 mm，d1=122 mm，a2=664 mm，a3=300 mm，d5=500 mm.將5個(gè)關(guān)節(jié)角的角度

帶入得

2.2 救援機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析

逆運(yùn)動(dòng)學(xué)求解是根據(jù)給定的機(jī)械臂末端的姿態(tài)和位置，求得所有關(guān)節(jié)的轉(zhuǎn)動(dòng)角度[12].求解過(guò)程如下：

1)求解θ1

(3)

化解，可以得到等式左邊為

等式右邊為

通過(guò)式(3)的(3，3)元素得

axs1-ayc1=0

可解得

θ1=atan2(ay,ax)，θ1=atan2(-ay,-ax)

2)求解θ5

通過(guò)式(3)的(3，1)和(3，2)元素得

nxs1-nyc1=s5

oxs1-oyc1=c5

(4)

可解得

θ5=atan2(nxs1-nyc1,oxs1-oyc1)

3)求解θ234

通過(guò)式(3)的(1，3)和(2，3)元素得

(5)

可解得

θ234=atan2(az,axc1+ays1)

4)求解θ3

根據(jù)式(3)的(1，4)和(2，4)元素有

(6)

重新排列式(6)，兩邊平方，然后相加得

(pxc1-a1+pys1-c234d5)2=(a3c23+a2c2)2

(pz-d1-d5s234)2=(a3s23+a2s2)2

所以

c3=

所以

θ3=atan2(s3,c3)

5)求解θ2

參考式(6)，得

pxc1-a1+pys1=a3(c2c3,s2s3)+a2c2+d5c234

pz-d1=a3(s2c3+c2s3)+a2s2+d5s234

所以

可解得

θ2=atan2(s2,c2)

6)求解θ4

θ4=θ234-θ2-θ3

2.3 運(yùn)動(dòng)學(xué)建模的仿真分析

2.3.1 正運(yùn)動(dòng)學(xué)模型的仿真分析

利用MATLAB機(jī)器人工具箱中的Link函數(shù)來(lái)建立機(jī)械臂的仿真對(duì)象[13]，得到仿真模型如圖3所示.

圖3 機(jī)械臂仿真模型

得到結(jié)果

可以看出,用MATLAB仿真的結(jié)果與正運(yùn)動(dòng)學(xué)方程推導(dǎo)的結(jié)果一致，故建立的模型是正確的.

2.3.2 逆運(yùn)動(dòng)學(xué)模型的驗(yàn)證

給定一個(gè)位姿x、y、z、R、P、Y，應(yīng)用逆運(yùn)動(dòng)學(xué)理論可以得到位姿所對(duì)應(yīng)的5個(gè)關(guān)節(jié)角.將得到的關(guān)節(jié)角度輸入到救援機(jī)械臂的正運(yùn)動(dòng)學(xué)模型中,與給定的位姿進(jìn)行比較，便可以驗(yàn)證逆運(yùn)動(dòng)學(xué)模型的正確性[14].給定位姿341.51、591.52、-188.40、-0.378、-4.981、171.318.通過(guò)計(jì)算，得到表2.

通過(guò)對(duì)比初始位姿x、y、z、R、P、Y和表2右側(cè)4組逆解對(duì)應(yīng)的位姿x′、y′、z′、R′、P′、Y′可以得出，機(jī)械臂逆運(yùn)動(dòng)學(xué)模型是準(zhǔn)確的.

表2 逆運(yùn)動(dòng)學(xué)驗(yàn)證

2.4 基于BP神經(jīng)網(wǎng)絡(luò)的運(yùn)動(dòng)學(xué)逆解

BP神經(jīng)網(wǎng)絡(luò)是一種按誤差逆?zhèn)鞑ニ惴ㄓ?xùn)練的多層前饋網(wǎng)絡(luò).BP神經(jīng)網(wǎng)絡(luò)由信息的正向傳播和誤差的反向傳播2個(gè)過(guò)程組成.BP神經(jīng)網(wǎng)絡(luò)系統(tǒng)拓?fù)浣Y(jié)構(gòu)包括輸入層、隱含層和輸出層.同一層神經(jīng)不存在相互連接，隱含層可以有1層或者多層，BP算法流程圖如圖4所示[15].

圖4 BP算法流程

2.4.1 BP網(wǎng)絡(luò)模型的設(shè)計(jì)

1)輸入輸出層的設(shè)計(jì)

輸入層節(jié)點(diǎn)數(shù)一般等于要訓(xùn)練的樣本維度數(shù)目，輸出層節(jié)點(diǎn)數(shù)根據(jù)輸出結(jié)果的空間維數(shù)確定[16].

本文中取輸入層節(jié)點(diǎn)數(shù)為10，分別為nx、ny、nz、ox、oy、ax、ay、px、py、pz，輸出節(jié)點(diǎn)數(shù)為5，為5個(gè)關(guān)節(jié)角.

2)隱含層的設(shè)計(jì)

Robert Hecht-Nielsen證明了對(duì)于任何閉區(qū)間內(nèi)的一個(gè)連續(xù)函數(shù)都可以用一個(gè)隱含層的BP神經(jīng)網(wǎng)絡(luò)來(lái)逼近.通過(guò)增加單層隱含層節(jié)點(diǎn)的數(shù)目，也可以保證網(wǎng)絡(luò)模型的預(yù)測(cè)精度，并且這種方式更加方便[17]，故本文選擇單隱含層.

隱含層節(jié)點(diǎn)數(shù)對(duì)神經(jīng)網(wǎng)絡(luò)的性能有一定影響，隱含層的節(jié)點(diǎn)數(shù)需要經(jīng)驗(yàn)和多次試驗(yàn)來(lái)確定.根據(jù)經(jīng)驗(yàn)可以參照

(7)

進(jìn)行設(shè)計(jì)[18].式中：l為隱含層節(jié)點(diǎn)數(shù)；n為輸入層節(jié)點(diǎn)數(shù)；m為輸出層節(jié)點(diǎn)數(shù)；a為1～10之間的常數(shù).

根據(jù)確定的輸入層節(jié)點(diǎn)數(shù)為10，輸出層節(jié)點(diǎn)數(shù)為5，由經(jīng)驗(yàn)公式(7)可求出隱含層節(jié)點(diǎn)數(shù)目的范圍是5～14.

3)傳遞、學(xué)習(xí)、及訓(xùn)練函數(shù)

傳遞函數(shù)必須是連續(xù)或可微的，常采用S型的對(duì)數(shù)或正切函數(shù)和線性函數(shù)[19].本文中隱含層傳遞函數(shù)使用tansig函數(shù)，輸出層傳遞函數(shù)使用purelin函數(shù).

學(xué)習(xí)函數(shù)有l(wèi)earngd函數(shù)和learngdm函數(shù)，本文建立的網(wǎng)絡(luò)模型使用learngdm學(xué)習(xí)函數(shù).

訓(xùn)練函數(shù)有trainbfg函數(shù)、traingdm函數(shù)、traingdx函數(shù)、Levenberg-Marquardt算法trainlm函數(shù).本文使用trainlm函數(shù)進(jìn)行訓(xùn)練,因?yàn)樵撍惴▽?duì)比一般的BP網(wǎng)絡(luò)算法,收斂速度較快，預(yù)測(cè)精度較高.

4)學(xué)習(xí)速率的選取

學(xué)習(xí)速率可以決定循環(huán)訓(xùn)練中的權(quán)值變化量，學(xué)習(xí)速率太大，網(wǎng)絡(luò)可能不穩(wěn)定，學(xué)習(xí)速率太小導(dǎo)致網(wǎng)絡(luò)訓(xùn)練的時(shí)間太長(zhǎng)，可能收斂太慢，一般來(lái)說(shuō)學(xué)習(xí)速率的選取范圍在0.01～0.80[20].

2.4.2 BP網(wǎng)絡(luò)運(yùn)行

1)數(shù)據(jù)的處理

利用正運(yùn)動(dòng)學(xué)模型，在MATLAB中隨機(jī)選取2 000組關(guān)節(jié)角度，得到運(yùn)動(dòng)學(xué)矩陣中的10個(gè)參數(shù)，其中1 600組數(shù)據(jù)作為訓(xùn)練數(shù)據(jù)，400組數(shù)據(jù)作為預(yù)測(cè)數(shù)據(jù).

BP神經(jīng)網(wǎng)絡(luò)的輸入和輸出參數(shù)的大小差距較大，會(huì)導(dǎo)致網(wǎng)絡(luò)模型最終計(jì)算結(jié)果的不準(zhǔn)確，所以需要進(jìn)行歸一化處理.本文的網(wǎng)絡(luò)模型使用mapminmax函數(shù)進(jìn)行數(shù)據(jù)歸一化[21]，將數(shù)據(jù)映射到[0,1]內(nèi)，它的變換式為

(8)

2)BP模型的建立

由于前文確認(rèn)BP網(wǎng)絡(luò)中隱含層的節(jié)點(diǎn)數(shù)為5～14，學(xué)習(xí)效率為0.01～0.80，故需設(shè)計(jì)模型來(lái)確定最優(yōu)的參數(shù).

通過(guò)編寫程序在MATLAB中運(yùn)行，其中隱含層節(jié)點(diǎn)數(shù)的采樣間隔是1，學(xué)習(xí)效率的采樣間隔是0.01.得到結(jié)果可知，當(dāng)隱含層節(jié)點(diǎn)數(shù)為7、學(xué)習(xí)速率為0.22時(shí)，訓(xùn)練的均方誤差最小.

確定好BP神經(jīng)網(wǎng)絡(luò)的參數(shù)后進(jìn)行訓(xùn)練，得到訓(xùn)練迭代過(guò)程如圖5所示.

圖5 訓(xùn)練迭代過(guò)程

對(duì)已知點(diǎn)的位姿，應(yīng)用逆運(yùn)動(dòng)學(xué)的解析法求出4組逆解，用上面訓(xùn)練好的BP神經(jīng)網(wǎng)絡(luò)求出預(yù)測(cè)的關(guān)節(jié)角，以均方根誤差為依據(jù)，篩選出唯一的逆解.

從400組預(yù)測(cè)數(shù)據(jù)中隨機(jī)選取一組關(guān)節(jié)角(120°，30°，-45°，-60°，30°)，其逆解的解析解及預(yù)測(cè)值如表3所示，從表3可以看出，最終確定的關(guān)節(jié)角為(120°，30°，-45°，-60°，30°)，與給定的一致.

表3 末端點(diǎn)的逆解與預(yù)測(cè)值

之后，隨機(jī)選取6個(gè)點(diǎn)進(jìn)行驗(yàn)證，結(jié)果如表4所示，經(jīng)過(guò)驗(yàn)證，采用BP神經(jīng)網(wǎng)絡(luò)得到的預(yù)測(cè)結(jié)果可以對(duì)用解析法得到的4組運(yùn)動(dòng)學(xué)逆解進(jìn)行篩選，從而得到唯一的逆解[22].

表4 BP篩選運(yùn)動(dòng)學(xué)逆解結(jié)果

3 結(jié)論

1)本文設(shè)計(jì)了一種多自由度救援機(jī)械臂，用于救援物資的搬運(yùn)工作，采用重載和輕載2種工作模式，并且建立了對(duì)應(yīng)的三維模型.

2)本文建立了救援機(jī)械臂的正逆運(yùn)動(dòng)學(xué)模型，通過(guò)MATLAB中Robotics Toolbox工具箱對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)進(jìn)行仿真，驗(yàn)證了運(yùn)動(dòng)學(xué)模型的正確性，采用BP神經(jīng)網(wǎng)絡(luò)對(duì)機(jī)械臂逆運(yùn)動(dòng)學(xué)逆解進(jìn)行預(yù)測(cè)，將預(yù)測(cè)結(jié)果作為依據(jù)，對(duì)用解析法得到的4組逆解進(jìn)行篩選，從而得到唯一的精確逆解，為后續(xù)的軌跡規(guī)劃的研究奠定了一定的基礎(chǔ).