基于姿態角遞增算法的6軸機器人通用運動規劃方法

程 晗，王 內，壽曉波

（中國船舶重工集團公司第722研究所，武漢 430000）

0 引言

目前，針對機器人的運動學分析研究最多的是基于成熟的機器人產品如ABB IRB系列機器人等平面連桿構型的機器人[1～3]。對于更具有通用性的非平面連桿構型機器人系統卻研究較少，如前3關節軸線所在平面和后3關節所在平面有偏置的6軸機器人。

相較平面型機器人的正/逆運動學分析，本文建立一種更具通用性的6軸串聯式機器人正/逆運動學方程，結合機器人的結構參數，可以在機器人的運動控制中更加快速的進行運動學的求解。同時，針對這種平面偏置型6軸機器人，根據機器人末端運動軌跡的要求，建立一種基于等效轉軸和等效轉角的通用運動軌跡規劃方法。

1 通用6軸工業機器人的結構分析

常見的串聯式垂直多關節（一般為6關節）的機械臂的3D構型如圖1所示。

圖1 6軸串聯式機械臂3D構型

通用串聯構型的六軸機械臂的主要結構參數如圖2所示，相對于目前常見的機械臂構型如《論文1》中所述機器人，也即一般考慮機械臂的構型的簡化連桿模型是在同一個平面內。本文中在通用串聯構型的六軸機械臂引入了參數P7，使得平面連桿構型變為空間構型。

圖2 空間構型的通用機械臂主要結構參數

圖2中P1和P6用于確定世界坐標系和工具坐標系用，故下文中的正反解的解析表達式中沒有涉及。針對以上通用的6軸串聯機械臂構型，可以建立統一的運動學正反解標準方程。

2 通用6軸工業機器人的運動學分析

首先按照D-H連桿坐標系建立的一般規則，首先確定各個坐標系的X、Y、Z軸的方向和原點坐標，從而建立該通用6軸機器人的D-H坐標系。各關節的D-H坐標系的建立如圖1所示。

圖3 通用6軸機械臂的各連桿坐標系

假定基座標系和軸1的坐標系建立在同一點上的情況下，6軸機器人D-H坐標系下各個連桿和關節參數可以列出，如表1所示。

表1 通用6軸機器人的D-H坐標連桿參數表

2.1 正運動學分析

機器人的正解指的是通過機械臂的關節角求解出腕點的空間位置和姿態的過程，腕點的空間位姿（位置和姿態）我們使用D-H坐標來表示。

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

式中：

為校核所得結果的正確性，計算當θi取表1中的初始值{0,90°,0,0,0,0}時，計算結果為：

與機械臂結構簡圖即圖1中顯示的結果完全一致，說明這種通用機械臂運動學正解的解析表達式是準確可靠的。

2.2 逆運動學分析

根據機械臂運動學可得封閉解的Pieper準則，這種通用6旋轉關節的機械臂構型由于最后3軸交于一點，所以可用反變化法（也稱代數法）得封閉解如下：

將式子：

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

3 姿態角遞增規劃算法

不同于3軸或4軸工業機器人（如碼垛用機器人等），6軸機器人的運動軌跡規劃不僅需要控制工具末端TCP點的位置量即px、py、pz沿著規劃軌跡精準運動，同時也要保證工具的姿態按照預定義的運動姿態規劃進行實時變化。

例如以下的簡單直線插補過程，2個實線模塊表示的是工具起點和終點位姿，姿態角遞增規劃算法的目的是為了保證工具末端TCP點的位置和姿態能按照預定義的姿態規劃路徑逐漸精確地從起點位姿運動到終點位姿。

圖4 姿態角遞增的直線插補方式

鑒于齊次坐標的姿態矩陣即R3×3矩陣中共9個變量相對于3個姿態量過于冗余，故目前常見的姿態規劃方法主要是基于歐拉角、RPY角、四元數或者旋轉變換通式等，本文提到的運動控制算法主要采用的是基于旋轉變換通式的姿態角遞增規劃算法。

易得：

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

對于任意給定的旋轉矩陣：

依據以上2個式子，可以很容易求得任意給定的旋轉矩陣的等效轉軸k和等效轉角θ，插補過程中任意插補點的位姿矩陣為：

其中PΔ表示起點到終點的位置變化矢量，N表示依據運動步長規劃的插補步數。

姿態角遞增規劃算法的主要計算流程如圖5所示。

圖5 姿態角遞增規劃算法流程

4 仿真及結果分析

根據前述6軸機器人運動學算法和運動軌跡規劃算法，選定初末關節角1到6關節角度值分別為：{5°,26°,86°,12°,13°,30°}和{10°,51°,101°,33°,26°,47°}共兩點作為直線運動軌跡規劃的起點和終點的位姿。

運用上述方法規劃的TCP點直線運動軌跡如圖6所示。

圖6 規劃的TCP點末端位置

各關節的運動變化曲線如下圖所示，其中藍色細實線表示關節1（axis1），綠色細虛線表示關節2（axis2），紅色短劃線表示關節3（axis3），藍色粗實線表示關節4（axis4），綠色粗虛線表示關節5（axis5），紅色粗短劃線表示關節6（axis6）。由圖可知，經規劃算法之后關節1至關節6的起點和終點的角度值和規劃前的值一致，說明規劃算法能保證機械臂從起點運動到終點。

圖7 各關節角度變化序列

TCP點坐標系的三個軸變化如圖所示。其中綠色曲面為TCP點坐標系X軸分量的變化過程，紅色曲面表示TCP點坐標系Y軸分量的變化過程，藍色曲面表示TCP點坐標系Z軸分量的變化過程，黑色直線表示TCP點坐標系原點位置的變化。

圖8 TCP點姿態變化示意圖

由圖可知，TCP點坐標系的位置和姿態變量經過姿態角遞增算法規劃后能逐漸從初始位資連續運動到終點位姿，變化過程平滑、連續且穩定，證明了這種姿態角遞增算法在通用型6軸工業機器人運動位姿規劃過程中的可靠性。

5 結論

本文在通用6軸工業機器人運動學分析的基礎上，建立了基于姿態角遞增算法的位姿軌跡規劃方法。該方法不僅適用于目前常見的平面構型的機械臂，同時也能適用于前3關節和后3關節旋轉軸平面產生偏置的非平面構型的通用串聯式6軸機械臂，具備較高的普適性。正逆解過程和姿態角遞增算法均利用解析表達式描述，因而可以很方便地在機器人控制系統中實現編程控制，而且有助于提高控制系統的實時性。

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

[2] 蔡自興.機器人學[M].北京:清華大學出版社,2000.

[3] 孫迪生.王炎.機器人控制技術[M].北京:機械工業出版社,1998.

[4] 李磊.六自由度并聯平臺運動規律及控制方法研究[D].哈爾濱工業大學.2007.

[5] 門昌華,關學鋒,等.基于PMAC的六自由度機器人開放式控制系統開發[J].機電產品開發與創新.2008(7).

[6] 張蘭芳.基于VC++開發的并聯六自由度測控系統[D].浙江:浙江大學,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] 劉敏杰.并聯機器人動力學與自適應控制技術研究[D].上海:上海交通大學,2001.