基于四元數(shù)的6R串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)逆解應(yīng)用

陳 愛，馮 桑，何 春，陸 曉

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

0 前言

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

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

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

1.1 四元數(shù)

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

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

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

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

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

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

其共軛

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

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

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

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

那么兩直線的交點(diǎn)P 可以用以下方程求得

1.3 普呂克坐標(biāo)系下剛體運(yùn)動(dòng)的描述

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

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

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

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

2.1 末端位置求解

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

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

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

6R 機(jī)器人的末端姿態(tài)通常用歐拉角表示，機(jī)器人工具坐標(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)系，其對(duì)應(yīng)的四元數(shù)為qz，qy，qx，則[8]：

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

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

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

3 6R 串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)逆解（Inverse Kinematic）

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

3.1 求解機(jī)器人第6軸方向

帶下標(biāo)_ik 的符號(hào)表示運(yùn)動(dòng)學(xué)逆解中的變量，下同。

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

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

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

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

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

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

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

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

圖3 機(jī)器人模型圖

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

至此6R串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)逆解全部求得。

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

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

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

4.1 正解

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

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

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

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

4.2 機(jī)器人運(yùn)動(dòng)學(xué)逆解

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

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

5 計(jì)算仿真

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

圖4 串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)Simulink仿真模型

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

并利用Matlab/robot toolbox建立的機(jī)器人運(yùn)動(dòng)學(xué)模型對(duì)該8 組運(yùn)動(dòng)學(xué)逆解進(jìn)行姿態(tài)仿真，得到圖5中的8個(gè)機(jī)器人姿態(tài)。

表2 6R機(jī)器人8組運(yùn)動(dòng)學(xué)逆解

6 結(jié)論

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

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

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

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

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

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

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

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

圖5 6R機(jī)器人運(yùn)動(dòng)學(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型機(jī)器人運(yùn)動(dòng)學(xué)逆解研究［J］.機(jī)械設(shè)計(jì)與制造，2008（10）：174-176.

［3］于艷秋，王品，廖啟征.一般6R機(jī)器人的位置反解與運(yùn) 動(dòng) 仿 真［J］.中 國 機(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 機(jī)器人位置反解的對(duì)偶四元數(shù)法［J］.機(jī)械工程學(xué)報(bào)，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）.