陳 愛，馮 桑，何 春，陸 曉

（廣東工業(yè)大學(xué)，廣東廣州 510006）

0 前言

空間6R機械手的運動學(xué)逆解是空間機構(gòu)學(xué)中最難的問題之一，曾被Freudenstein喻為機構(gòu)運動學(xué)中的“珠穆朗瑪峰”[1]。傳統(tǒng)的逆運動學(xué)求解方法具有一定的局限性，比如D-H 法的矩陣運算中，需要對進行大量的“0”元素以及矩陣求逆運算，計算冗余繁雜；文獻[2]利用特定機器人幾何構(gòu)型特點求得機器人運動學(xué)逆解的解析式，但其運算過程需要大量人工參與，不具通用性，求解困難，而且涉及多解和奇異性等問題，不易實現(xiàn)實時在線控制[2]。

四元數(shù)法是基于旋量理論（Screw Theory）的一種算法，可以在一個全局坐標(biāo)系下建立各個剛體位置以及剛體運動的描述[2]。四元數(shù)法的物理意義在于其可以表征剛體變換時瞬時歐拉軸和所需的轉(zhuǎn)角。四元數(shù)法與歐拉方程相比，具有計算量小、精度高[2]，且不會出現(xiàn)奇異現(xiàn)象等特點[2-3]。

1 四元數(shù)（quaternion）及普呂克坐標(biāo)系（Plücker coordinate）

1.1 四元數(shù)

四元數(shù)由一個實數(shù)單位和三個虛數(shù)單位組成，可以看作是拓展后的復(fù)數(shù)，通常寫成

式（1）中，q0，q1，q2，q3均為實數(shù)，i，j，k 服從以下運算規(guī)律：

單位四元數(shù)（unit quaternion）是模為1 的四元數(shù)，可以將其寫成：

（1）單位四元數(shù)的共軛

其中：*為四元數(shù)乘法。

（2）單位對偶四元數(shù)

其共軛

1.2 普呂克坐標(biāo)系

三維空間直線在普呂克坐標(biāo)系下描述為：

其中：d 為直線的單位方向向量；m=p×d ，p 為直線上任意一點的坐標(biāo)向量。

采用普呂克坐標(biāo)系表示直線后，直線由其方向向量d 和直線上任意一點p 構(gòu)成，并且直線方程與該直線上的點的具體位置無關(guān)。因此在普呂克坐標(biāo)系下表示直線，有利于簡化6R機器人各連桿的結(jié)構(gòu)。

那么兩直線的交點P 可以用以下方程求得

1.3 普呂克坐標(biāo)系下剛體運動的描述

對于給定型號的機器人，其初始位置時各軸心線的方向矢量均為已知。軸心線線上的點可以通過幾何關(guān)系獲得。普呂克坐標(biāo)系下的直線Lpi可以看作是一個特殊的對偶四元數(shù)，其旋轉(zhuǎn)部分和向量部分的是不均為零的純旋轉(zhuǎn)為了滿足四元數(shù)運算，將普呂克坐標(biāo)下的直線擴展為8 維表示，即6R工業(yè)機器人各關(guān)節(jié)軸線：

i=1～6，為6R機器人第i 個旋轉(zhuǎn)軸。

2 單位四元數(shù)在6R 串聯(lián)機器人運動學(xué)正解中的應(yīng)用

本文采用對偶四元數(shù)法，利用6R 串聯(lián)機器人的末端三軸交于一點的幾何特點，以及四元數(shù)的物理意義，容易求得6R 串聯(lián)機器人運動學(xué)正解。

2.1 末端位置求解

由于6R機器人末端三軸心線交于一點（稱為腕關(guān)節(jié)點，wrist）才能有封閉解（如圖1）。為了簡化計算，筆者求第4、5軸的交點。因此，腕關(guān)節(jié)點坐標(biāo)Pw為：

圖1 6R串聯(lián)機器人結(jié)構(gòu)示意圖

2.2 末端姿態(tài)求解

6R 機器人的末端姿態(tài)通常用歐拉角表示，機器人工具坐標(biāo)系在各關(guān)節(jié)旋轉(zhuǎn)后與旋轉(zhuǎn)前變換過程中所產(chǎn)生的x、y和z軸的旋轉(zhuǎn)變換。

根據(jù)幾何變換關(guān)系可得：

設(shè)坐標(biāo)系通過繞z，y，x 軸旋轉(zhuǎn)角度γ，β，α 到坐標(biāo)系，其對應(yīng)的四元數(shù)為qz，qy，qx，則[8]：

其中qx，qy，qz分別表示繞x，y，z 旋轉(zhuǎn)的四元數(shù)。

反之同理可以得到用四元數(shù)N0，N1，N2，N3來表示歐拉角，則可以得到：

6R 機器人運動學(xué)正解由末端的位置和姿態(tài)構(gòu)成，由上述公式可以求得機器人運動學(xué)正解。

3 6R 串聯(lián)機器人運動學(xué)逆解（Inverse Kinematic）

6R 串聯(lián)機器人運動學(xué)逆解的求解是一個難題。與運動學(xué)正解相反，運動學(xué)逆解是根據(jù)機器人末端姿態(tài)求解機器人關(guān)節(jié)角的問題。TSAI 等[1]利用高維逼近分析逆運動學(xué)，推出了不同結(jié)構(gòu)的各種6R 機器人最多只有16 解的結(jié)論。然而一般6R 串聯(lián)機器人的逆解解析的表達式往往很復(fù)雜，這給快速精確計算運動學(xué)逆解帶來了困難。在笛卡爾坐標(biāo)系下，倪振松[1]等，將剛體運動用雙四元數(shù)描述，但其構(gòu)造Dixon結(jié)式進行消元求解，得到一元十六次方程，但其求解方程過程較為復(fù)雜，對計算機資源配置要求非常高。而Paden-Kahan法由于其自身局限性，因此通常將串聯(lián)機器人的運動學(xué)逆解分解為多個Paden-Kahan 子問題進行求解。但在其空間幾何關(guān)系轉(zhuǎn)換過程中，計算量龐大，增加了整個計算過程的計算量。

3.1 求解機器人第6軸方向

帶下標(biāo)_ik 的符號表示運動學(xué)逆解中的變量，下同。

從而得到了機器人關(guān)節(jié)點的坐標(biāo)值pw_ik。

3.2 求解串聯(lián)機器人各個關(guān)節(jié)角

前三個關(guān)節(jié)角可以通過幾何關(guān)系求得：

圖2 平面二桿機構(gòu)示意圖

其中：a，b 分別表示平面二桿機構(gòu)的桿長，θi為關(guān)節(jié)i的轉(zhuǎn)角角度，當(dāng)桿長已知的情況下，通過上述方程組可以求得方程組的解為：

根據(jù)幾何關(guān)系，d5始終和d6、d4垂直，即：

該等式等號左邊是一個常數(shù)值；等號右邊的物理意義是：第5關(guān)節(jié)的軸心線繞第4關(guān)節(jié)軸心線旋轉(zhuǎn)θ4后得到的結(jié)果。等式中唯一的變量是θ4_ik，根據(jù)等式很容易求得θ4_ik的值，等式中唯一的變量是θ4_ik，根據(jù)等式很容易求得θ4_ik的值。

同理：由于q16已知，通過以下等式

圖3 機器人模型圖

分別可以求得θ5_ik，θ6_ik。

至此6R串聯(lián)機器人運動學(xué)逆解全部求得。

4 四元數(shù)法求解6R串聯(lián)機器人運動學(xué)實例

本文針對如圖3 所示的機器人，進行四元數(shù)法的6R串聯(lián)機器人的運動學(xué)仿真。該機器人幾何尺寸如表1。

表1 6R串聯(lián)機器人基本參數(shù)

4.1 正解

根據(jù)表格中的機器人的基本參數(shù)，由公式（3），可以得到各軸心線旋轉(zhuǎn)的單位四元數(shù)描述：

再將公式（25）代入到公式（5）可以得到繞各軸心線變換的單位對偶四元數(shù)描述：

然后將表格中數(shù)據(jù)帶入到公式（8），可以得到各個軸心線在普呂克坐標(biāo)系下的直線方程：

將（26）（27）代入到公式（10）、（11）和公式（14），可以得到6R 串聯(lián)機器人的運動學(xué)正解，即機器人末端位置和姿態(tài)。

4.2 機器人運動學(xué)逆解

在已知機器人末端點位置PE_ik以及歐拉角αik，βik，γik的情況下，由公式（13）可以得到：

可以得到串聯(lián)機器人的6 個關(guān)節(jié)角θi（i=1，2，3，4，5，6）。

5 計算仿真

本文利用Matlab/robot toolbox工具箱建立機器人運動學(xué)模型，再對基于四元數(shù)法的6R串聯(lián)機器人的運動學(xué)逆解進行了計算仿真。任意給定6 個關(guān)節(jié)的旋轉(zhuǎn)角度θi后，利用Matlab/Simulink 模型對運動學(xué)正解進行仿真，得到機器人末端的位置坐標(biāo)（Position）及末端的歐拉角（EulerAngle），再將其輸入到運動學(xué)逆解模塊（Inverse Kinematic），得到8個運動學(xué)逆解結(jié)果。過程及結(jié)果如圖4所示。

圖4 串聯(lián)機器人運動學(xué)Simulink仿真模型

將經(jīng)過四元數(shù)正解后得到的機器人末端執(zhí)行器的姿態(tài)代入模型進行運動學(xué)逆解模型進行求解，得到8 組運動學(xué)逆解（如表2）。從表中數(shù)據(jù)可以看出，在誤差允許的范圍內(nèi)，第1 組數(shù)據(jù)和輸入的數(shù)據(jù)完全一樣。在Matlab 中計算，該組數(shù)據(jù)的計算精度完全滿足工程應(yīng)用的需要。

并利用Matlab/robot toolbox建立的機器人運動學(xué)模型對該8 組運動學(xué)逆解進行姿態(tài)仿真，得到圖5中的8個機器人姿態(tài)。

表2 6R機器人8組運動學(xué)逆解

6 結(jié)論

將超復(fù)數(shù)形式的四元數(shù)以及對偶四元數(shù)應(yīng)用于6R 串聯(lián)機構(gòu)的運動學(xué)正逆解分析的數(shù)學(xué)建模，用基于普呂克坐標(biāo)系的直線描述串聯(lián)機構(gòu)的連桿，避免機構(gòu)繁瑣的D-H法描述，物理意義簡潔明了。與D-H法相比，四元數(shù)中每一個元素都有很強的物理意義和計算意義，避免無效計算，占用計算機資源，影響著計算速度。

將運動學(xué)正解中點的旋轉(zhuǎn)變換轉(zhuǎn)化成變換后的直線的交點，脫離了具體的各連桿的具體位置的計算。

在運動學(xué)逆解中，與傳統(tǒng)D-H 法通過求解矩陣的逆，再進行矩陣乘法計算，最后解方程。四元素求共軛，只需將虛部正負(fù)號改變即刻，計算復(fù)雜度極其小。

D-H 法中，兩個4×4 矩陣A 和B 相乘得到矩陣C，一般共需要進行7×16=112 次加法或乘法運算；而四元數(shù)法中，兩個四元數(shù)相乘運算，一般共需要進行7×4=28 次加法或乘法運算。從理論上，四元數(shù)的計算量也明顯小于矩陣的計算量。

四元數(shù)法直接得到各個關(guān)節(jié)旋轉(zhuǎn)角度的一元一次方程或者一元二次方程組，其計算復(fù)雜度為O（1），理論上計算復(fù)雜度大大降低。

四元數(shù)法大大簡化了計算過程，提高了計算效率和精度，便于機器人控制的實時性，在實際工程應(yīng)用中具有很強的實際意義。

采用四元數(shù)法，可以統(tǒng)一機器人的運動學(xué)中的位置和姿態(tài)[1]，即在同一個坐標(biāo)系就可以完全描述6R機器人的運動學(xué)的正逆解。

四元數(shù)法除了可以描述轉(zhuǎn)動副外，還可以描述移動或者螺旋運動。因此本文研究的四元數(shù)法還可以推廣到螺旋副或者移動副的運動機構(gòu)中，比如四元數(shù)用在第4 軸為滾珠絲杠花鍵軸的SCARA機器人的運動學(xué)的數(shù)學(xué)建模中。

圖5 6R機器人運動學(xué)8組逆解位姿

［1］Freudenstein F.Kinematics：past，present and future［J］.Mechanism and Machine Theory，1973，8（2）：151-161.

［2］李發(fā)忠，趙德安，馬從國.FANUC_M-6iB型機器人運動學(xué)逆解研究［J］.機械設(shè)計與制造，2008（10）：174-176.

［3］于艷秋，王品，廖啟征.一般6R機器人的位置反解與運 動 仿 真［J］.中 國 機 械 工 程，2003（24）：2130-2132.

［4］Emre Sariyildiz，Eray Cakiray and Hakan Temeltas.A comparative study of three inverse kinematic methods of serial industrial robot manipulators in the screw theory framework ［J］.International Journal of Advanced Robotic Systems，2011（8）：9-24.

［5］Yurong Nan，Panfeng Wu.A New Method for the Inverse Kinematics of 6-DOF Manipulator Based on Dual Quaternion［J］.Advances in Mechanical and Electronic Engineering Lecture Notes in Electrical Engineering，2012（176）：295-302.

［6］AHN Jin-su，CHUNG Won-jee，JUNG Chang-doo.Realization of orientation interpolation of 6-axis articulated robot using quaternion［J］.J.Cent.South Univ，2012（19）：3407-3414.

［7］程國采.四元數(shù)法及其應(yīng)用［M］.長沙：國防科技大學(xué)出版社，1991.

［8］E.Sariyildiz，H.Temeltas，Solution of Inverse Kinematic Problem for Serial Robot Using Dual Quaternions and Plücker Coordinates［A］.IEEE/ASME International Conference on Advanced Intelligent Mechatronics［C］.Suntec Convention and Exhibition Center Singapore，July 14-17，2009.

［9］TSAI L W，MORGAN A.Solving the kinematics of the most general six and five-degree-of-freedom manipulators by continuation methods［J］.Mechanism and Machine Theory，1985，107（2）：189-200.

［10］倪振松，廖啟征，魏世民，等.空間6R 機器人位置反解的對偶四元數(shù)法［J］.機械工程學(xué)報，2009（11）：25-29.

［11］Li Jing，Wang Huinan，Liu Haiying.Dual Quaternion Curve Interpolation Algorithm for Formation Satellites［J］.Transactions of Nanjing University of Aeronautics&Astronautics，2012，29（1）.