面向聚變堆維護(hù)的重載機(jī)械臂CMOR運(yùn)動學(xué)算法設(shè)計(jì)及仿真

趙中旭，潘洪濤，程 勇，樊玉琦

(1.合肥工業(yè)大學(xué) 計(jì)算機(jī)與信息學(xué)院，安徽 合肥 230009; 2.中國科學(xué)院等離子體物理研究所，安徽 合肥 230009)

0 引 言

中國聚變工程實(shí)驗(yàn)堆(Chinese Fusion Engineering Testing Reactor，縮寫CFETR)是中國自主設(shè)計(jì)和研制并聯(lián)國際合作的重大科學(xué)工程[1]。CFETR內(nèi)部屬于極端環(huán)境，具有強(qiáng)輻射、高溫、強(qiáng)磁場以及高真空等特點(diǎn)。此外，真空室內(nèi)部設(shè)備眾多，各種管道錯綜復(fù)雜，工作空間狹小，再加上艙內(nèi)的某些部件會受到放射性和有毒物質(zhì)的污染，即使在維護(hù)期間維護(hù)人員也不能直接對相關(guān)部件進(jìn)行操作，需要借助于遙操縱手段在艙外通過機(jī)器人完成相應(yīng)的操作。

由于維護(hù)空間較大，總設(shè)計(jì)總臂長較長；但受內(nèi)壁腔體尺寸限制，單關(guān)節(jié)過長，自由度小的機(jī)械臂設(shè)計(jì)無法滿足幾何限制，故需要設(shè)計(jì)具有多個長度適合的關(guān)節(jié)以滿足可達(dá)空間的需求[2]。結(jié)合以上設(shè)計(jì)要求，設(shè)計(jì)了一種基于底盤的9自由度冗余機(jī)械臂CMOR(CFETR Multipurpose Overload Robot)。該機(jī)械臂末端負(fù)載2.5 t、懸臂9 m、末端定位精度±10 mm，搭載著末端執(zhí)行其并結(jié)合遙操作工具完成偏濾器第一壁的搬運(yùn)、真空室清理以及管路維護(hù)等操作，如圖1所示。

圖1 CMOR系統(tǒng)在CFETR真空室內(nèi)部圖

CMOR機(jī)械臂共有8個關(guān)節(jié)，關(guān)節(jié)與關(guān)節(jié)之間按照要求有不同形式的關(guān)節(jié)驅(qū)動(見圖2)。其中關(guān)節(jié)2、3為水平方向的偏航運(yùn)動，關(guān)節(jié)4、6、8為軸線的旋轉(zhuǎn)運(yùn)動，關(guān)節(jié)5、7、9為豎直方向的俯仰運(yùn)動。它主要用于真空室內(nèi)部的維護(hù)工作，并通過底部軌道從中窗口送入真空室內(nèi)部。中窗口為矩形截面，高為2 740 mm，寬為2 330 mm。真空室內(nèi)部的維護(hù)空間是由高場側(cè)包層、低場側(cè)包層以及偏濾器表面組成的環(huán)形空間內(nèi)。重載機(jī)械臂進(jìn)入中窗口預(yù)設(shè)位置后，可通過底部軌道與真空室固定，機(jī)械臂本體可以用真空室內(nèi)部空間實(shí)現(xiàn)從折疊狀態(tài)到工作狀態(tài)。CMOR機(jī)械臂在不同工作空間內(nèi)所需達(dá)到的真空室扇區(qū)范圍也不相同，分別為：在真空室包層及偏濾器輪廓組成的空間內(nèi)需要能夠達(dá)到±45°，在真空室內(nèi)表面的空間內(nèi)需要能夠達(dá)到±11.25°。

1 CMOR機(jī)械臂運(yùn)動學(xué)分析

1.1 正運(yùn)動學(xué)求解

CMOR機(jī)械臂是一個九自由度機(jī)器人，包含一個移動關(guān)節(jié)和8個轉(zhuǎn)動關(guān)節(jié)，負(fù)責(zé)將前端執(zhí)行機(jī)器人送到接近維護(hù)點(diǎn)的位置?？紤]機(jī)械臂的復(fù)雜程度和順序結(jié)構(gòu)，采用D-H法[3]建立九自由度機(jī)械臂各關(guān)節(jié)，及末端坐標(biāo)系，如圖3為機(jī)械臂的數(shù)學(xué)模型。根據(jù)該模型，可得到表1所列的機(jī)械臂各連桿的D-H參數(shù)。

圖3 CMOR機(jī)械臂的DH模型

表1 CMOR機(jī)械臂模型的D-H參數(shù)表

根據(jù)空間坐標(biāo)變化公式可得相鄰兩坐標(biāo)系之間關(guān)系：

將連桿參數(shù)一次帶入式子中，就可以得到機(jī)器人的其次變換矩陣：

機(jī)械臂的末端執(zhí)行器的坐標(biāo)系相對于基座標(biāo)系的位姿變換矩陣可表示為：

對上式求解可得(僅展示末端位置P(px,py,pz))：

px=a2c2-d4(c2s3+c3s2)-d6(c5(c2s3+c3s2)+c4s5(c2c3-s2s3))-d8(c7(c5(c2s3+c3s2)+c4s5(c2c3-s2s3))-s7(c6(s5(c2s3+c3s2) -c4c5(c2c3-s2s3))+s4s6(c2c3-s2s3)))+a4c4(c2c3-s2s3)

py=a2s2+d4(c2c3-s2s3)+d6(c5(c2c3-s2s3)-c4s5(c2s3+c3s2))+d8(c7(c5(c2c3-s2s3)-c4s5(c2s3+c3s2))-s7(c6(s5(c2c3-s2s3)+c4c5(c2s3+c3s2))-s4s6(c2s3+c3s2)))+a4c4(c2s3+c3s2)

pz=d1-a4s4+d8(s7(c4s6+c5c6s4)+c7s4s5)+d6s4s5

1.2 逆運(yùn)動學(xué)求解

CMOR機(jī)械臂是一個構(gòu)型稍復(fù)雜的多自由度機(jī)械臂，其無法通過解析解或封閉解方法進(jìn)行求解，需要利用數(shù)值法求解[4-6]；文中使用基于雅可比逆矩陣的求解方法，其基本原理是給定機(jī)械臂末端的位姿，選取一組盡量合適的關(guān)節(jié)值為初始值，運(yùn)用正運(yùn)動學(xué)計(jì)算當(dāng)前位置，當(dāng)與目標(biāo)位姿有誤差時，采用雅可比偽逆來確定迭代方向，并通過迭代的方法使關(guān)節(jié)值收斂至目標(biāo)位置。其算法如下：

(1) 已知機(jī)械臂末端目標(biāo)位姿(Rref,Pref)；

(2) 選擇一組盡量接近目標(biāo)的關(guān)節(jié)值q=(d1,θ2,θ3,θ4,θ5,θ6,θ6,θ7,θ8,θ9)，由正運(yùn)動學(xué)求出當(dāng)前位姿(R,P)；

(3) 計(jì)算位姿誤差ΔP=Pref-P，ΔR=RTRref；

(4) 若誤差小于預(yù)設(shè)誤差閾值時，則當(dāng)前q值為機(jī)械臂解；

(5) 若誤差小大于預(yù)設(shè)誤差閾值時，求解關(guān)節(jié)角度修正量dq，使q=q+dq，返回(2)。

關(guān)節(jié)速度和末端位置速度的線性關(guān)系：

dx=J·dq?dq=J+·dx

式中：J表示雅可比矩陣，但對于7自由度機(jī)械臂而言，由于其雅可比矩陣是個長方形，無法計(jì)算它的逆，用其偽逆J+代替(J+∈R6×9,J+=JT(JJT)-1)，由上式得到dq=J+·dx，即為關(guān)節(jié)角度修正量。

1.3 CMOR機(jī)械臂運(yùn)動學(xué)驗(yàn)證

首先，在MATLAB Robotics toolbox工具箱中通過編寫程序輸入機(jī)械臂參數(shù)建立一個CMOR機(jī)械臂模型。定義一組關(guān)節(jié)值A(chǔ)=[5.000,1.571,1.571,1.047,0.524,1.047,0.524,1.047,0.524],帶入式(1)中可得：

使用函數(shù)fkine(A)同樣可得到位姿：

比較可得T=Ta，驗(yàn)證了正運(yùn)動學(xué)模型的正確性。以T作為已知位姿，通過逆運(yùn)動學(xué)求解可得到一組關(guān)節(jié)值B=[-0.003,1.532,1.433,0.152 5,0.268,-0.883,-0.701,-2.467,0.281]，將B帶入正運(yùn)動學(xué)方程可得：

比較可得Ta=Tb，驗(yàn)證了逆運(yùn)動學(xué)求解方法的正確性。

2 工作空間求解

機(jī)械臂的工作空間是指在機(jī)械臂正常運(yùn)行時，末端執(zhí)行器的坐標(biāo)原點(diǎn)能在空間活動的最大范圍，或者說該原點(diǎn)所有可達(dá)點(diǎn)的體積空間，這一空間又稱為可達(dá)空間或總工作空間。文中采用蒙特卡羅法求解機(jī)械臂的工作空間[7-8]，求解步驟如下：

(1) 根據(jù)求解的正運(yùn)動學(xué)可得到末端執(zhí)行器原點(diǎn)在基座標(biāo)系下的坐標(biāo)P(px,py,pz)。

(2) 根據(jù)各個關(guān)節(jié)角度的轉(zhuǎn)動范圍，在Matlab中調(diào)用隨機(jī)函數(shù)rand()生成每個關(guān)節(jié)變量的隨機(jī)角度，跟據(jù)式(1)求出關(guān)節(jié)i的隨機(jī)角度值：

θi=θi,min+(θi,max-θi,min)·rand(N,1)

(1)

式中：θi表示第i各關(guān)節(jié)轉(zhuǎn)動角的最小值；θi,max-θi,min表示第i各關(guān)節(jié)轉(zhuǎn)角的范圍值；N為生成隨機(jī)點(diǎn)的數(shù)量，N=10 000。

(3) 將產(chǎn)生的各關(guān)節(jié)隨機(jī)角度值帶入坐標(biāo)(px,py,pz)中，可以得到機(jī)械臂的工作空間。使用的隨機(jī)點(diǎn)的數(shù)量越多，則生成的工作空間與實(shí)際的操作空間越接近。

用Matlab生成的CMOR機(jī)械臂工作空間如圖3所示。

從圖3可以得出用蒙特卡羅法得到的CMOR機(jī)械臂末端的活動空間，范圍是沿X(-800 0，800 0)，Y(-420 0，440 0)，Z(-600 0，800 0)三個坐標(biāo)方向圍成的一個橢圓體的區(qū)域。結(jié)合真空室的尺寸很容易看到，在xoz和yoz平面的末端點(diǎn)能夠覆蓋整個工作環(huán)境，由圖5可得再xoy平面可得CMOR的末端可滿足達(dá)到真空室內(nèi)±45°的扇形范圍。

圖4 冗余重載機(jī)械臂的工作空間

圖5 CFETER與冗余機(jī)械臂工作空間分析圖

3 軌跡規(guī)劃

3.1 五次多項(xiàng)式軌跡規(guī)劃

機(jī)械臂的軌跡規(guī)劃分為笛卡爾空間和關(guān)節(jié)空間。在關(guān)節(jié)空間內(nèi)進(jìn)行軌跡規(guī)劃有以下優(yōu)點(diǎn)：在線計(jì)算量小，即無需進(jìn)行機(jī)械臂的逆解或正解解算、不受關(guān)節(jié)奇異構(gòu)型影響。但前期對應(yīng)的操作空間的軌跡無法預(yù)測，增加了機(jī)械臂與環(huán)境碰撞的可能[9]。采用五次多項(xiàng)式曲線插值算法[10]進(jìn)行CMOR機(jī)械臂的軌跡規(guī)劃。以關(guān)節(jié)角度、角速度以及角加速度作為邊界條件，以確保各關(guān)節(jié)變量的連續(xù)無突變。公式如下：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

同時我們要限制路徑起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角度、角速度、角加速度：

由上式可知，該方程組由6個方程和6個未知數(shù)。求解方程可得：

將系數(shù)帶入軌跡方程中，可以得到機(jī)械臂的五次多項(xiàng)式插值曲線的軌跡方程。

3.2 軌跡規(guī)劃的仿真

為近一步驗(yàn)證建立的運(yùn)動學(xué)方程的準(zhǔn)確性還需進(jìn)一步的在虛擬樣機(jī)進(jìn)行驗(yàn)證。

圖6 CMOR機(jī)械臂的運(yùn)動軌跡圖

首先，在Matlab中設(shè)定好機(jī)械臂末端位置的初始位姿A和需要移動的到的目標(biāo)位姿B，采用五次多項(xiàng)式插值算法完成軌跡規(guī)劃。在Matlab中編寫程序模擬仿真過程，設(shè)置步長為40，生成機(jī)械臂末端位置在在空間中的運(yùn)動曲線。仿真結(jié)束后生成機(jī)械臂的9個關(guān)節(jié)位置、速度、加速度隨時間變化曲線圖。分析圖像可以得到機(jī)械臂從初始位置到目標(biāo)位置的整個運(yùn)動過程平滑無突變。

圖7 各關(guān)節(jié)參數(shù)變化曲線圖

各關(guān)節(jié)運(yùn)動參數(shù)隨時間變換曲線如圖5所示，從上到下分別為關(guān)節(jié)1、關(guān)節(jié)4、關(guān)節(jié)6、關(guān)節(jié)3、關(guān)節(jié)8、關(guān)節(jié)9、關(guān)節(jié)7、關(guān)節(jié)5和關(guān)節(jié)2，9條關(guān)節(jié)都相對平滑無沖擊，并未出然增加或減少的突變現(xiàn)象，能完成點(diǎn)到點(diǎn)的高速運(yùn)動，證明了機(jī)械臂結(jié)構(gòu)的正確性以及運(yùn)動學(xué)方程的正確性。

4 結(jié) 語

以設(shè)計(jì)的9自由度CMOR機(jī)械臂為研究對象，采用D-H參數(shù)法、雅可比偽逆法建立了CMOR機(jī)械臂的運(yùn)動學(xué)模型，基于Matlab建立了機(jī)械人系統(tǒng)的仿真模型，采用蒙特卡羅法計(jì)算了機(jī)械臂的工作空間。同時開展了關(guān)節(jié)空間機(jī)械臂軌跡規(guī)劃的研究，研究了五次多項(xiàng)式插值法點(diǎn)到點(diǎn)軌跡規(guī)劃，并機(jī)型仿真驗(yàn)證，仿真結(jié)果表明建立的機(jī)械臂模型和運(yùn)動學(xué)方程的正確性，為該機(jī)械臂的后續(xù)工作提供了理論依據(jù)。