劉玉梅，曹曉寧，王秀剛，王帆，徐振，盧海隔

(1.吉林大學交通學院，吉林長春130022;2.長春軌道客車股份有限公司，吉林長春130062)

與串聯(lián)機構相比，并聯(lián)機構的逆解求解容易，正解求解困難［1-2］.針對正解問題，學者們根據(jù)桿長約束條件建立以位姿為變量的非線性方程組，利用同倫連續(xù)法、牛頓迭代、消元法、神經(jīng)網(wǎng)絡、遺傳算法、粒子群法等計算方法來求解［3］.對于基于齊次變換矩陣或普通變換矩陣的并聯(lián)機構運動學正解牛頓迭代求解算法來說，由于該算法涉及到求解含有三角函數(shù)的非線性方程組問題而不易解決，并且每迭代一次，以三角函數(shù)形式表示的雅克比矩陣需更新一次，計算量龐大且復雜，嚴重影響了計算速度，不利于基于位姿反解的運動位姿控制進行實時監(jiān)控及修正.

為進一步提高位姿正解求解速度和收斂性，本文結合齊次變換矩陣元素正交關系及桿長約束條件，構建以齊次變換矩陣元素為變量的位姿正解模型，利用牛頓拉夫遜法逐次逼近齊次變換矩陣最優(yōu)數(shù)值解，再根據(jù)齊次變換矩陣元素與位姿變量對應函數(shù)關系求解對應位姿.

1 數(shù)學模型

6自由度并聯(lián)機構是并聯(lián)機器人機構中的一大類，是學者研究得最多的并聯(lián)機構［4］，廣泛應用在運動模擬器、六維力與力矩傳感器和并聯(lián)機床、微操作機器人等領域［5］.從完全并聯(lián)的角度出發(fā)，這類機構必須具有6個或6個以上的運動鏈，如具有6個作動器的Stewart并聯(lián)機構，吉林大學研制的7個作動器的6自由度并聯(lián)機構，以及哈爾濱工業(yè)大學研制的8作動器6自由度并聯(lián)機構［6］，且隨著并聯(lián)機構的日益發(fā)展及承載能力的要求，將來可能會出現(xiàn)具有更多支鏈的6自由度并聯(lián)機構.雖然在現(xiàn)有的并聯(lián)機構中也存在擁有3個運動鏈的6自由度并聯(lián)機構，但本文只研究具有6個或6個以上，12個以下的運動鏈的6自由度并聯(lián)機構.

結合不同形式的6自由度并聯(lián)機構結構特點，分別在6自由度并聯(lián)機構的上平臺和下平臺上建立體坐標系o-xyz及靜坐標系O-XYZ，作動器上鉸鏈點Ai(i=1，2…，n，n表示作動器個數(shù)，6≤n≤12)在體坐標系中的坐標用向量形式表示為

作動器下鉸鏈點Bi(i=1，2…n)在靜坐標系中的坐標用向量形式表示為

式中:Aix、Aiy、Aiz分別為第i個作動器上鉸鏈點在體坐標系沿x、y、z軸的坐標值;Bix、Biy、Biz分別為第i個作動器下鉸鏈點在靜坐標系沿X、Y、Z軸的坐標值.

當上平臺運動后，任一上鉸鏈點在體坐標系中的向量可以通過坐標變換方法變換到該點在靜坐標系中的向量，進而上下鉸鏈點向量可表示為

式中:T為齊次變換矩陣，由于有限轉(zhuǎn)動合成與轉(zhuǎn)動次序有關，基于歐拉角的空間變換矩陣形式T可表述為12種形式，包括 TXYX、TYXY、TZXY、TXYZ、TYXZ、TZXZ、TXZX、TYZX、TZYX、TXZY、TYZY及 TZYZ形式.假設平臺的位姿用體坐標系相對于靜坐標系的廣義坐標 q=(q1，q2，q3，q4，q5，q6)來描述. 則ZYX空間齊次變換矩陣表示為［7］

式中:qi(i=1，2，3)為體坐標系相對于靜坐標系的3個繞x、y、z軸的姿態(tài)角，qi(i=4，5，6)為體坐標原點相對于靜坐標系沿x、y、z3個坐標軸的平移量.

n個作動器的長度可由上、下鉸鏈點間的距離來確定，因此由桿長約束條件得

式中:Li為各個作動器的長度，i=1，2，…，n.

將式(5)進行整理得

上式可以表示為

式中:Tij表示空間齊次變換矩陣中第i行、第j列元素，i=1，2，3，4，j=1，2，3，4.

空間齊次變換矩陣12種形式的前3階順序主子矩陣的每個元素都是qi(i=1，2，3)的三角函數(shù)，且元素之間呈現(xiàn)正交性［8-9］，用數(shù)學語言表達如下:

式(7)中有6個等式，分別為行或列的元素的平方和等于1.

結合含有6個自由度、n(6≤n≤12)個作動器的并聯(lián)機構幾何參數(shù)及空間運動學原理，由桿長約束條件得到以矩陣元素為未知量的n個方程，并選取前3階順序主子矩陣元素關系式的任意12-n個聯(lián)立組成 12 個變元 ［T11，T12，T13，T14，T21，…，T33，T34］，12個多項式方程的方程組，從而將以位姿［q1，q2，q3，q4，q5，q6］為變量的正解問題隨之轉(zhuǎn)化為齊次矩陣元素為變量的求解問題.

為了使模型格式統(tǒng)一，添加虛擬作動器桿長ln+1，ln+2，…，l12，并將虛擬桿桿長均設為 0，則由桿長約束條件及任意選取的12-n個旋轉(zhuǎn)矩陣關系式聯(lián)立進一步表示為

式中:

式(8)可進一步抽象為

經(jīng)分析得知:式(8)實質(zhì)上是含［T11，T12，T13，T14，T21，…，T33，T34］12 個未知數(shù)，12 個桿長的 12 個二次非線性方程組，其將上平臺實時位姿q=(q1，q2，q3，q4，q5，q6)的求解問題轉(zhuǎn)化為 ［T11，T12，T13，T14，T21，T22，T23，T24，T31，T32，T33，T34］的求解問題，雖然未知數(shù)個數(shù)由6個變?yōu)?2個，但同時避免了以姿態(tài)qi(i=1，2，3)為變量的三角函數(shù)的反復運算，從而簡化了求解過程.求得變換矩陣元素數(shù)值解后，利用12種空間齊次變換矩陣與位姿變量之間的函數(shù)映射公式即可得到給定液壓缸的長度下對應的位姿q=(q1，q2，q3，q4，q5，q6).以 ZYX 旋轉(zhuǎn)空間齊次變換矩陣為例，空間齊次變換矩陣與位姿變量之間的映射關系如下:

考慮到6自由度并聯(lián)機構工作空間在-π/2，π/2(

)范圍內(nèi)，在此范圍內(nèi)正弦及正切都呈現(xiàn)單調(diào)遞增性，所以二者反函數(shù)取值具有單一性.

2 迭代求解

2.1 雅克比矩陣的確定

確定雅克比矩陣是利用牛頓迭代方法求得位姿正解數(shù)值解的關鍵［10］.經(jīng)研究證明，以齊次變換矩陣元素為變量的雅克比矩陣解析表達式只與聯(lián)立組成的非線性方程組有關，與齊次變換矩陣形式無關.對于具有相同數(shù)目作動器的并聯(lián)機構而言，如果位姿正解非線性方程組選取一致，則雅克比矩陣解析表達式一致，故可廣泛應用.由位姿正解方程組Fi(T，Li)=0(i=1，2，…，12)中各元素 Fi1，F(xiàn)i2，F(xiàn)i3對各變量 Tj求導，得到雅克比矩陣解析總表達式如下式所示.根據(jù)實際并聯(lián)機構桿長約束條件建立6自由位姿正解模型后，選取前3階順序主子矩陣正交性方程，進而從雅克比矩陣解析總表達式查找對應的Ji(i=1，2，…，12，表示行數(shù))構成所求的雅克比矩陣解析表達式:

2.2 牛頓迭代算法

將式(9)作多元泰勒展開，取其線性部分，可得［11］

式中:k為迭代次數(shù);ΔT(k)j=T(k+1)j-T(k)j.

則上式經(jīng)整理表示為

式(14)可以看作ΔT(k)j為未知數(shù)，J為系數(shù)矩陣的線性方程組.本文利用QR算法求解此線性方程組，得出方程組的解ΔT(k)j.計算步驟［12］如下:

1)選取位姿初值 q0及液壓缸伸縮量.將 q0代入到齊次變換矩陣公式得到對應的T0.

2)確定以齊次變換矩陣元素為未知量的6自由度平臺位姿正解模型，并根據(jù)模型查閱雅克比矩陣總表，構成雅克比矩陣J解析式，將變換矩陣初值T0及液壓缸伸縮量代入到式(9)和 J解析式中確定Fi(T(0)，Li)及 J(0)的取值.

3)利用QR算法求解下列線性方程組，得出方程組的解ΔT(k)j，

按照上述步驟反復迭代，每迭代一次根據(jù)最新得到的 T(k)對 J及 Fi(T(k))更 新 一 次.將max(ΔT1，ΔT2，…，ΔT12)≤ ε(ε 為求解精度)作為算法結束運行條件，按照下面公式完成迭代過程:

值得注意的是:由于齊次變換矩陣公式多樣性，即使在同一并聯(lián)機構位姿初始值相同條件下，對應的齊次變換矩陣元素初值也不同.

3 仿真模型及實例解算

利用Simulink搭建的位姿正解仿真模型，仿真模型如圖1所示，包括位姿初值輸入模塊、桿長輸入模塊、位姿q與T轉(zhuǎn)化模塊、牛頓迭代模塊、T與位姿轉(zhuǎn)化模塊、輸出與顯示模塊.該模型在識別作動器位移傳感器輸出的作動器實時位移后，自動實現(xiàn)位姿與齊次坐標矩陣之間相互轉(zhuǎn)化、雅克比矩陣及位姿反解與實際桿長之差的迭代更新，利用牛頓迭代法逐次逼近直至求得6自由度并聯(lián)機構平臺位姿的最優(yōu)數(shù)值解，并將計算結果保存.該仿真模型可用于不同形式的、不同作動器數(shù)目的6自由度并聯(lián)機構位姿正解解算，具有廣泛的實用性，同時也為其他自由度并聯(lián)機構位姿正解提供了借鑒作用.

以吉林大學汽車運輸研究所設計并開發(fā)的具有7作動器的3個軸向6自由度運動平臺為例，利用本文提出的基于齊次變換矩陣數(shù)值解的6自由度平臺位姿正解解算方法實現(xiàn)了位姿正解解算.取位姿初始值 q=(0，0，0，0，0，3 910).7 個液壓缸對應長度用向量 表 示 為 L=(2 940，2 940，2 920，2 920，2 730，2 730，2 920)，設置誤差精度為 1×10-5，運行位姿正解仿真模型完成迭代過程，迭代4次后就已經(jīng)滿足誤差精度要求，結果為迭代過程耗時為0.0141 s，以位姿為變量的傳統(tǒng)牛頓迭代計算方法耗時為0.022 46 s，相比可節(jié)省0.005 s時間，2種方法結果均為 q=(0.025，0，0，0.015 8，0.178 2，3 920)，進而驗證本文提出的解算方法的高效性以及實用性.

圖1 位姿正解仿真模型Fig.1 The forward kinematics simulation model

圖2 牛頓迭代模塊Fig.2 The New ton iteration module

4 結論

1)本文研究的以齊次變換矩陣元素為變量的位姿正解解算方法比以位姿為變量的傳統(tǒng)計算方法耗時少.2)該方法可服務于不同形式、不同作動器數(shù)目、不同齊次變換矩陣形式的6自由度并聯(lián)機構位姿正解，實用性強.3)由于篇幅有限，文中列舉的實例較少，今后研究中應選取不同作動器數(shù)目、不同齊次變換矩陣形式的6自由度并聯(lián)機構位姿正解進行研究.

［1］高洪，趙韓.并聯(lián)機器人機構學理論研究綜述［J］.安徽工程科技學院學報:自然科學版，2006，21(1):73-79.GAO Hong，ZHAO Han A review about theory of mechanism for parallel robotics［J］.Journal of Anhui University of Technology and Science，2006 ，21(1):73-79.

［2］ZHOU Hui，CAO Yi，ZHANG Qiuju.The research on direct kinematic problem of a special class of the Stewart-Gough manipulators［C］//Proceedings of 2008 IEEE International Conference on Mechatronics and Automation.Takamatsu，Japan:Inst of Elec and Elec Eng Computer Society，2008:971-976.

［3］陳長憶，車林仙.應用粒子群算法的3-RPS并聯(lián)機器人機構位置正解［J］.現(xiàn)代制造工程，2006，(5):77-79.CHEN Changyi，CHE Linxian.Forward position analysis of 3-RPS in-parallel manipulators based on particle swarm optimization［J］.Modern Manufacturing Engineering，2006，(5):77-79.

［4］李立，張劍，陳永，等.同倫迭代法的研究及其應用于機構學問題的求解［J］.西南交通大學學報:自然科學版，2000，35(1):57-60.LILi，ZHANG Jian，CHEN Yong，et al.On the homogony iteration method and its application in the mechanism problems［J］.Journal of Southwest Jiaotong University，2000，35(1):57-60.

［5］BOUDREAU R，TURKKAN N.Solving the forward kinematics of parallel manipulators with a genetic algorithm［J］.Journal of Robotic Systems，1996，13(2):111-125.

［6］關廣豐.液壓驅(qū)動六自由度振動試驗系統(tǒng)控制策略研究［D］.哈爾濱:哈爾濱工業(yè)大學，2007:29.GUAN Guangfeng.Control strategy of hydraulically driven 6-DOF vibration test system［D］.Harbin:Harbin Institute of Technology，2007:29.

［7］GALLARDO J，LESSO R，RICO JM，et al.The kinematics of modular spatial hyper-redundant manipulators formed from RPS-type limbs［J］.Robotics and Autonomous Systems，2011，58(1):12-21.

［8］程世利，吳洪濤，王超群，等.基于正交補的6-3 Stewart并聯(lián)機構運動學正解［J］.中國機械工程，2011，22(5):505-508.CHENG Shili，WU Hongtao，WANG Chao qun，et al.Forward kinematics analysis of 6-3 Stewart parallel mechanisms based on orthogonal complement method［J］.Journal of Mechanical Engineering，2011，22(5):505-508.

［9］程永倫，朱世強，劉松國，等.基于旋轉(zhuǎn)子矩陣正交的6R機器人運動學逆解研究［J］.機器人，2008，30(2):160-164.CHENG Yonglun，ZHU Shiqiang，LIU Songguo，et al.Inverse kinematics of 6R robots based on the orthogonal character of rotation sub-matrix［J］.Robot，2008，30(2):160-164.

［10］賈軍艷.機構運動分析與求解雅可比矩陣的計算機模擬法及應用［D］.秦皇島:燕山大學，2006:15-18.JIA Junyan.Computer simulation approach for solving Jacobian and kinematic of mechanisms and application［D］.Qin huangdao:Yanshan University，2006:15-18.

［11］YANG Chifu，HUANG Qitao，OGBOBE P O，et al.Forward kinematics analysis of parallel robots using global Newton-Raphson method［C］∥Proceedings of 2009 2nd International Conference on Intelligent Computing Technology and Automation.Hunan，China:IEEE Computer Society，2009:407-410.

［12］TAROKH M，KEERTHIK，LEE M.Classification and characterization of inverse kinematics solutions for anthropomorphic manipulators［J］.Robotics and Autonomous Systems，2010，58(1):115-120.