基于姿態(tài)角遞增算法的6軸機(jī)器人通用運(yùn)動(dòng)規(guī)劃方法

程 晗，王 內(nèi)，壽曉波

（中國(guó)船舶重工集團(tuán)公司第722研究所，武漢 430000）

0 引言

目前，針對(duì)機(jī)器人的運(yùn)動(dòng)學(xué)分析研究最多的是基于成熟的機(jī)器人產(chǎn)品如ABB IRB系列機(jī)器人等平面連桿構(gòu)型的機(jī)器人[1～3]。對(duì)于更具有通用性的非平面連桿構(gòu)型機(jī)器人系統(tǒng)卻研究較少，如前3關(guān)節(jié)軸線所在平面和后3關(guān)節(jié)所在平面有偏置的6軸機(jī)器人。

相較平面型機(jī)器人的正/逆運(yùn)動(dòng)學(xué)分析，本文建立一種更具通用性的6軸串聯(lián)式機(jī)器人正/逆運(yùn)動(dòng)學(xué)方程，結(jié)合機(jī)器人的結(jié)構(gòu)參數(shù)，可以在機(jī)器人的運(yùn)動(dòng)控制中更加快速的進(jìn)行運(yùn)動(dòng)學(xué)的求解。同時(shí)，針對(duì)這種平面偏置型6軸機(jī)器人，根據(jù)機(jī)器人末端運(yùn)動(dòng)軌跡的要求，建立一種基于等效轉(zhuǎn)軸和等效轉(zhuǎn)角的通用運(yùn)動(dòng)軌跡規(guī)劃方法。

1 通用6軸工業(yè)機(jī)器人的結(jié)構(gòu)分析

常見(jiàn)的串聯(lián)式垂直多關(guān)節(jié)（一般為6關(guān)節(jié)）的機(jī)械臂的3D構(gòu)型如圖1所示。

圖1 6軸串聯(lián)式機(jī)械臂3D構(gòu)型

通用串聯(lián)構(gòu)型的六軸機(jī)械臂的主要結(jié)構(gòu)參數(shù)如圖2所示，相對(duì)于目前常見(jiàn)的機(jī)械臂構(gòu)型如《論文1》中所述機(jī)器人，也即一般考慮機(jī)械臂的構(gòu)型的簡(jiǎn)化連桿模型是在同一個(gè)平面內(nèi)。本文中在通用串聯(lián)構(gòu)型的六軸機(jī)械臂引入了參數(shù)P7，使得平面連桿構(gòu)型變?yōu)榭臻g構(gòu)型。

圖2 空間構(gòu)型的通用機(jī)械臂主要結(jié)構(gòu)參數(shù)

圖2中P1和P6用于確定世界坐標(biāo)系和工具坐標(biāo)系用，故下文中的正反解的解析表達(dá)式中沒(méi)有涉及。針對(duì)以上通用的6軸串聯(lián)機(jī)械臂構(gòu)型，可以建立統(tǒng)一的運(yùn)動(dòng)學(xué)正反解標(biāo)準(zhǔn)方程。

2 通用6軸工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)分析

首先按照D-H連桿坐標(biāo)系建立的一般規(guī)則，首先確定各個(gè)坐標(biāo)系的X、Y、Z軸的方向和原點(diǎn)坐標(biāo)，從而建立該通用6軸機(jī)器人的D-H坐標(biāo)系。各關(guān)節(jié)的D-H坐標(biāo)系的建立如圖1所示。

圖3 通用6軸機(jī)械臂的各連桿坐標(biāo)系

假定基座標(biāo)系和軸1的坐標(biāo)系建立在同一點(diǎn)上的情況下，6軸機(jī)器人D-H坐標(biāo)系下各個(gè)連桿和關(guān)節(jié)參數(shù)可以列出，如表1所示。

表1 通用6軸機(jī)器人的D-H坐標(biāo)連桿參數(shù)表

2.1 正運(yùn)動(dòng)學(xué)分析

機(jī)器人的正解指的是通過(guò)機(jī)械臂的關(guān)節(jié)角求解出腕點(diǎn)的空間位置和姿態(tài)的過(guò)程，腕點(diǎn)的空間位姿（位置和姿態(tài)）我們使用D-H坐標(biāo)來(lái)表示。

利用表1即機(jī)械臂D-H參數(shù)表中的數(shù)據(jù)很容易求得機(jī)器人末端也即軸6的坐標(biāo)系{6}相對(duì)于基座標(biāo)系{0}的位姿矩陣為：

式中：

為校核所得結(jié)果的正確性，計(jì)算當(dāng)θi取表1中的初始值{0,90°,0,0,0,0}時(shí)，計(jì)算結(jié)果為：

與機(jī)械臂結(jié)構(gòu)簡(jiǎn)圖即圖1中顯示的結(jié)果完全一致，說(shuō)明這種通用機(jī)械臂運(yùn)動(dòng)學(xué)正解的解析表達(dá)式是準(zhǔn)確可靠的。

2.2 逆運(yùn)動(dòng)學(xué)分析

根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)可得封閉解的Pieper準(zhǔn)則，這種通用6旋轉(zhuǎn)關(guān)節(jié)的機(jī)械臂構(gòu)型由于最后3軸交于一點(diǎn)，所以可用反變化法（也稱(chēng)代數(shù)法）得封閉解如下：

將式子：

同時(shí)有，令兩邊矩陣的（1，3）和（3，3）元素分別相等，得到：

3 姿態(tài)角遞增規(guī)劃算法

不同于3軸或4軸工業(yè)機(jī)器人（如碼垛用機(jī)器人等），6軸機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃不僅需要控制工具末端TCP點(diǎn)的位置量即px、py、pz沿著規(guī)劃軌跡精準(zhǔn)運(yùn)動(dòng)，同時(shí)也要保證工具的姿態(tài)按照預(yù)定義的運(yùn)動(dòng)姿態(tài)規(guī)劃進(jìn)行實(shí)時(shí)變化。

例如以下的簡(jiǎn)單直線插補(bǔ)過(guò)程，2個(gè)實(shí)線模塊表示的是工具起點(diǎn)和終點(diǎn)位姿，姿態(tài)角遞增規(guī)劃算法的目的是為了保證工具末端TCP點(diǎn)的位置和姿態(tài)能按照預(yù)定義的姿態(tài)規(guī)劃路徑逐漸精確地從起點(diǎn)位姿運(yùn)動(dòng)到終點(diǎn)位姿。

圖4 姿態(tài)角遞增的直線插補(bǔ)方式

鑒于齊次坐標(biāo)的姿態(tài)矩陣即R3×3矩陣中共9個(gè)變量相對(duì)于3個(gè)姿態(tài)量過(guò)于冗余，故目前常見(jiàn)的姿態(tài)規(guī)劃方法主要是基于歐拉角、RPY角、四元數(shù)或者旋轉(zhuǎn)變換通式等，本文提到的運(yùn)動(dòng)控制算法主要采用的是基于旋轉(zhuǎn)變換通式的姿態(tài)角遞增規(guī)劃算法。

易得：

上述式子即為旋轉(zhuǎn)變換通式的建立過(guò)程，反向問(wèn)題則是依靠旋轉(zhuǎn)變換矩陣求解等效轉(zhuǎn)軸k和等效轉(zhuǎn)角θ。

對(duì)于任意給定的旋轉(zhuǎn)矩陣：

依據(jù)以上2個(gè)式子，可以很容易求得任意給定的旋轉(zhuǎn)矩陣的等效轉(zhuǎn)軸k和等效轉(zhuǎn)角θ，插補(bǔ)過(guò)程中任意插補(bǔ)點(diǎn)的位姿矩陣為：

其中PΔ表示起點(diǎn)到終點(diǎn)的位置變化矢量，N表示依據(jù)運(yùn)動(dòng)步長(zhǎng)規(guī)劃的插補(bǔ)步數(shù)。

姿態(tài)角遞增規(guī)劃算法的主要計(jì)算流程如圖5所示。

圖5 姿態(tài)角遞增規(guī)劃算法流程

4 仿真及結(jié)果分析

根據(jù)前述6軸機(jī)器人運(yùn)動(dòng)學(xué)算法和運(yùn)動(dòng)軌跡規(guī)劃算法，選定初末關(guān)節(jié)角1到6關(guān)節(jié)角度值分別為：{5°,26°,86°,12°,13°,30°}和{10°,51°,101°,33°,26°,47°}共兩點(diǎn)作為直線運(yùn)動(dòng)軌跡規(guī)劃的起點(diǎn)和終點(diǎn)的位姿。

運(yùn)用上述方法規(guī)劃的TCP點(diǎn)直線運(yùn)動(dòng)軌跡如圖6所示。

圖6 規(guī)劃的TCP點(diǎn)末端位置

各關(guān)節(jié)的運(yùn)動(dòng)變化曲線如下圖所示，其中藍(lán)色細(xì)實(shí)線表示關(guān)節(jié)1（axis1），綠色細(xì)虛線表示關(guān)節(jié)2（axis2），紅色短劃線表示關(guān)節(jié)3（axis3），藍(lán)色粗實(shí)線表示關(guān)節(jié)4（axis4），綠色粗虛線表示關(guān)節(jié)5（axis5），紅色粗短劃線表示關(guān)節(jié)6（axis6）。由圖可知，經(jīng)規(guī)劃算法之后關(guān)節(jié)1至關(guān)節(jié)6的起點(diǎn)和終點(diǎn)的角度值和規(guī)劃前的值一致，說(shuō)明規(guī)劃算法能保證機(jī)械臂從起點(diǎn)運(yùn)動(dòng)到終點(diǎn)。

圖7 各關(guān)節(jié)角度變化序列

TCP點(diǎn)坐標(biāo)系的三個(gè)軸變化如圖所示。其中綠色曲面為T(mén)CP點(diǎn)坐標(biāo)系X軸分量的變化過(guò)程，紅色曲面表示TCP點(diǎn)坐標(biāo)系Y軸分量的變化過(guò)程，藍(lán)色曲面表示TCP點(diǎn)坐標(biāo)系Z軸分量的變化過(guò)程，黑色直線表示TCP點(diǎn)坐標(biāo)系原點(diǎn)位置的變化。

圖8 TCP點(diǎn)姿態(tài)變化示意圖

由圖可知，TCP點(diǎn)坐標(biāo)系的位置和姿態(tài)變量經(jīng)過(guò)姿態(tài)角遞增算法規(guī)劃后能逐漸從初始位資連續(xù)運(yùn)動(dòng)到終點(diǎn)位姿，變化過(guò)程平滑、連續(xù)且穩(wěn)定，證明了這種姿態(tài)角遞增算法在通用型6軸工業(yè)機(jī)器人運(yùn)動(dòng)位姿規(guī)劃過(guò)程中的可靠性。

5 結(jié)論

本文在通用6軸工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上，建立了基于姿態(tài)角遞增算法的位姿軌跡規(guī)劃方法。該方法不僅適用于目前常見(jiàn)的平面構(gòu)型的機(jī)械臂，同時(shí)也能適用于前3關(guān)節(jié)和后3關(guān)節(jié)旋轉(zhuǎn)軸平面產(chǎn)生偏置的非平面構(gòu)型的通用串聯(lián)式6軸機(jī)械臂，具備較高的普適性。正逆解過(guò)程和姿態(tài)角遞增算法均利用解析表達(dá)式描述，因而可以很方便地在機(jī)器人控制系統(tǒng)中實(shí)現(xiàn)編程控制，而且有助于提高控制系統(tǒng)的實(shí)時(shí)性。

[1] Paden B,Panja R.1998.Globally stable‘PD+’,controller for robot manipulators[J].International of Control,47:1697-1712.

[2] 蔡自興.機(jī)器人學(xué)[M].北京:清華大學(xué)出版社,2000.

[3] 孫迪生.王炎.機(jī)器人控制技術(shù)[M].北京:機(jī)械工業(yè)出版社,1998.

[4] 李磊.六自由度并聯(lián)平臺(tái)運(yùn)動(dòng)規(guī)律及控制方法研究[D].哈爾濱工業(yè)大學(xué).2007.

[5] 門(mén)昌華,關(guān)學(xué)鋒,等.基于PMAC的六自由度機(jī)器人開(kāi)放式控制系統(tǒng)開(kāi)發(fā)[J].機(jī)電產(chǎn)品開(kāi)發(fā)與創(chuàng)新.2008(7).

[6] 張?zhí)m芳.基于VC++開(kāi)發(fā)的并聯(lián)六自由度測(cè)控系統(tǒng)[D].浙江:浙江大學(xué),2002.

[7] L.S.Farhy.Modeling of oscillations of endocrine networks with feedback[J]·Methods Enzymol.2004(384):54-81.

[8] Delta Tau Data system Inc.PMAcAccEssoRY 8s[M].1997.

[9] 劉敏杰.并聯(lián)機(jī)器人動(dòng)力學(xué)與自適應(yīng)控制技術(shù)研究[D].上海:上海交通大學(xué),2001.