基于六維線性插值的六自由度機(jī)械臂逆運(yùn)動學(xué)方程求解方法

周　鋒，林　楠，陳小平

(中國科學(xué)技術(shù)大學(xué) 多智能體系統(tǒng)實(shí)驗(yàn)室，合肥 230026)(*通信作者電子郵箱zf062@mail.ustc.edu.cn)

0　引言

六自由度(Degrees Of Freedom, DOF)機(jī)械臂逆運(yùn)動學(xué)求解問題一直是機(jī)器人領(lǐng)域研究的熱點(diǎn)。對于特殊結(jié)構(gòu)的六自由度機(jī)械臂，連續(xù)三個關(guān)節(jié)平行或三個關(guān)節(jié)軸線相交于一點(diǎn)，則必有封閉解，可以使用代數(shù)法如幾何法[1]、指數(shù)積方法[2]求解運(yùn)動學(xué)反問題。可對于一般幾何結(jié)構(gòu)的六自由度串聯(lián)機(jī)械臂，以上方法求解計算復(fù)雜度很高或不可解，主要求解方法以數(shù)值解為主，如雙四元數(shù)[3]、李代數(shù)[4]、迭代法(如Newton-Raphson迭代算法[5])等，但理論復(fù)雜，且迭代法還需求解雅克比矩陣，運(yùn)算復(fù)雜、用時久，因而研究方向轉(zhuǎn)向啟發(fā)式算法。Kalra等[6]將遺傳算法應(yīng)用到六自由度機(jī)械臂逆運(yùn)動學(xué)問題的求解中；Chiddarwar等[7]則將神經(jīng)網(wǎng)絡(luò)應(yīng)用到六自由度機(jī)械臂逆運(yùn)動學(xué)問題的求解中。神經(jīng)網(wǎng)絡(luò)是模擬函數(shù)關(guān)系，對于逆運(yùn)動中一個位姿對應(yīng)多個關(guān)節(jié)角的非函數(shù)映射，只能在局部模擬處理，當(dāng)輸入數(shù)據(jù)較少或較多時預(yù)測效果都會有所下降，不易控制。

一維插值主要有牛頓插值、拉格朗日插值、Hermite插值和三次樣條插值。一維插值在機(jī)器人領(lǐng)域一般應(yīng)用在關(guān)節(jié)空間軌跡規(guī)劃，常用的有三次多項(xiàng)式、五次多項(xiàng)式[8]。二維插值主要有反距離加權(quán)插值法、克里金插值法、最小曲率法、改進(jìn)謝別德法、自然鄰近插值法、最近鄰近插值法、多元回歸法、徑向基函數(shù)法、線性插值三角網(wǎng)法、移動平均法和局部多項(xiàng)式法。二維插值一般應(yīng)用在降水量分析[9]、海洋環(huán)境數(shù)據(jù)處理[10]等應(yīng)用中。在插值領(lǐng)域，分段線性插值能夠穩(wěn)定、有效地逼近目標(biāo)函數(shù)，但對于本文要研究的六自由度機(jī)械臂逆運(yùn)動學(xué)求解問題，當(dāng)前的插值方法維度較低。

針對一般結(jié)構(gòu)的六自由度機(jī)械臂逆運(yùn)動學(xué)方程求解困難的問題，本文提出六維線性插值及反插值理論，利用當(dāng)前關(guān)節(jié)角附近的7個相鄰的關(guān)節(jié)角點(diǎn)發(fā)現(xiàn)運(yùn)動規(guī)律，逼近目標(biāo)位姿。與徑向基網(wǎng)絡(luò)(Radical Basis Function Network, RBFN)相比，反插值法預(yù)測的關(guān)節(jié)角對應(yīng)的位姿可迅速逼近目標(biāo)位姿；與Newton-Raphson迭代算法相比，反插值法不需要求解雅克比矩陣，只需知道機(jī)械臂的運(yùn)動數(shù)據(jù)，利用差商代替微商求解逆運(yùn)動學(xué)解，計算復(fù)雜度較低。

1　六自由度機(jī)械臂結(jié)構(gòu)

由于D-H坐標(biāo)系法的關(guān)節(jié)參數(shù)的下標(biāo)與關(guān)節(jié)軸不對應(yīng)，所以本文采用Khalil等提出的約定[11]建立參考坐標(biāo)系，本文設(shè)計的六自由度機(jī)械臂的參考坐標(biāo)系如圖1所示，每個圓柱體代表一個電機(jī)，每個箭頭所指方向是電機(jī)軸的朝向，0坐標(biāo)系為基坐標(biāo)系，7坐標(biāo)系為末端執(zhí)行器坐標(biāo)系，在初始狀態(tài)下，7坐標(biāo)系與0坐標(biāo)系各坐標(biāo)軸朝向相同。各關(guān)節(jié)參數(shù)如表1所示。

圖1　六自由度機(jī)械臂坐標(biāo)系Fig. 1　Coordinate system of 6-DOF manipulator

表1　各關(guān)節(jié)幾何參數(shù)Tab. 1　Geometric parameters of every joint

在Khalil等提出的約定[11]下，通過繞xi-1軸旋轉(zhuǎn)角度αi，沿xi-1軸平移αi，繞zi軸旋轉(zhuǎn)角度θi，沿zi軸平移di，可實(shí)現(xiàn)參考坐標(biāo)系i相對于參考坐標(biāo)系i-1的定位。

繞xi-1軸旋轉(zhuǎn)角度αi的變換矩陣為：

(1)

沿xi-1軸平移ai的變換矩陣為：

(2)

繞zi軸旋轉(zhuǎn)角度θi的變換矩陣為：

(3)

沿zi軸平移di的變換矩陣為：

(4)

通過這些獨(dú)立變換的串聯(lián)：

Rot(xi-1,αi)Trans(xi-1,ai)Rot(zi,θi)Trans(zi,di)

(5)

得到參考坐標(biāo)系i相對于參考坐標(biāo)系i-1的等價齊次變換矩陣為：

i-1Ti=

(6)

(7)

2　末端執(zhí)行器位姿描述

本文使用ZYX型歐拉角描述姿態(tài)，定義如圖2所示，首先繞z軸將x軸旋轉(zhuǎn)到x′軸在xoy平面內(nèi)的投影x″，然后繞y(y″)軸旋轉(zhuǎn)使x(x″)軸與x′軸重合，然后再繞x(x′)軸使y(y″)軸與y′軸重合。經(jīng)以上三個步驟，便從xyz坐標(biāo)系得到了x′y′z′坐標(biāo)系，在變換過程中形成的三個角度分別為α、β、γ，為x′y′z′坐標(biāo)系相對xyz坐標(biāo)系的歐拉角參數(shù)。

圖2　改進(jìn)后的歐拉角定義Fig. 2　Definition of improved Euler angle

末端執(zhí)行器坐標(biāo)系相對基坐標(biāo)系的位置參數(shù)為：

x=0T7(1,4)

(8)

y=0T7(2,4)

(9)

z=0T7(3,4)

(10)

其中：0T7是0坐標(biāo)系(基坐標(biāo)系)到7坐標(biāo)系(末端執(zhí)行器坐標(biāo)系)的等價齊次變換矩陣；0T7(1,4)表示矩陣0T7的第一行第四列的那個元素。

末端執(zhí)行器坐標(biāo)系相對基坐標(biāo)系的姿態(tài)參數(shù)為：

α=arctan2(0T7(2,1),0T7(1,1))

(11)

(12)

(13)

3　徑向基網(wǎng)絡(luò)

徑向基網(wǎng)絡(luò)(RBFN)是使用徑向基函數(shù)的線性組合來模擬任一連續(xù)函數(shù)的，常用的徑向基函數(shù)有高斯函數(shù)、反演S型函數(shù)和擬多二次函數(shù)。一個徑向基函數(shù)(Radial Basis Function, RBF)神經(jīng)網(wǎng)絡(luò)的計算過程如下：

步驟1從訓(xùn)練數(shù)據(jù)中選取基函數(shù)的中心，常用的方法是K-means聚類。

步驟2在得到聚類中心后，徑向基函數(shù)的擴(kuò)展常數(shù)便可以通過計算各聚類中心之間的距離來得到。

步驟3計算輸出權(quán)值即徑向基函數(shù)的組合系數(shù)。

徑向基網(wǎng)絡(luò)在機(jī)械臂逆運(yùn)動學(xué)中常用的方法是將位姿作為輸入，關(guān)節(jié)角作為輸出，網(wǎng)絡(luò)結(jié)構(gòu)如圖3所示。

圖3　RBFN結(jié)構(gòu)Fig. 3　Structure of RBFN

RBF神經(jīng)網(wǎng)絡(luò)在正運(yùn)動學(xué)方面的應(yīng)用是比較有效的，因?yàn)闄C(jī)械臂的正運(yùn)動學(xué)方程是函數(shù)關(guān)系,即一個自變量對應(yīng)一個因變量；但在逆運(yùn)動學(xué)方面，由于一個位姿對應(yīng)多組關(guān)節(jié)角，不是函數(shù)關(guān)系，RBF神經(jīng)網(wǎng)絡(luò)的應(yīng)用情況就會比較糟糕?？梢酝ㄟ^篩選數(shù)據(jù)的方式，使在局部范圍內(nèi)呈現(xiàn)函數(shù)關(guān)系。具體算法流程如圖4所示。

4　六維線性插值及反插值理論

徑向基網(wǎng)絡(luò)在局部模擬目標(biāo)函數(shù)，當(dāng)輸入數(shù)據(jù)較少或較多時預(yù)測效果都會有所下降，不易控制，而分段線性插值可以穩(wěn)定、有效地逼近目標(biāo)函數(shù)。一維線性插值是在兩個相鄰節(jié)點(diǎn)(x1,y1)、(x2,y2)間建立一元一次函數(shù)y=a*x+b，可以對以這兩個節(jié)點(diǎn)的區(qū)間內(nèi)的值作預(yù)測，如給出x,求出對應(yīng)的y，同時也能作反插值運(yùn)算，即給出y求對應(yīng)的x。二維線性插值的最小預(yù)測單元是由三個相鄰的節(jié)點(diǎn)組成的三角面，其對應(yīng)的插值函數(shù)為z=a*x+b*y+c。以此類推，六維線性插值的最小預(yù)測單元由7個相鄰的非線性相關(guān)的節(jié)點(diǎn)組成，其對應(yīng)的插值函數(shù)為：

x=a1*θ1+b1*θ2+c1*θ3+d1*θ4+e1*θ5+f1*θ6+g1

(14)

y=a2*θ1+b2*θ2+c2*θ3+d2*θ4+e2*θ5+f2*θ6+g2

(15)

z=a3*θ1+b3*θ2+c3*θ3+d3*θ4+e3*θ5+f3*θ6+g3

(16)

α=a4*θ1+b4*θ2+c4*θ3+d4*θ4+e4*θ5+f4*θ6+g4

(17)

β=a5*θ1+b5*θ2+c5*θ3+d5*θ4+e5*θ5+f5*θ6+g5

(18)

γ=a6*θ1+b6*θ2+c6*θ3+d6*θ4+e6*θ5+f6*θ6+g6

(19)

圖4　RBF預(yù)測算法Fig. 4　RBF prediction algorithm

設(shè)第i個相鄰節(jié)點(diǎn)的6個關(guān)節(jié)角度為：

其中θij代表第i個相鄰節(jié)點(diǎn)的第j個關(guān)節(jié)的角度。

設(shè)第i個相鄰節(jié)點(diǎn)的空間位姿為：

其中：xi、yi、zi為末端執(zhí)行器坐標(biāo)系的位置參數(shù)；αi、βi、γi為末端執(zhí)行器坐標(biāo)系的姿態(tài)參數(shù)。

則7個相鄰的非線性相關(guān)的節(jié)點(diǎn)數(shù)據(jù)代入插值函數(shù)公式(14)，有如下方程組：

(20)

(21)

(22)

如果預(yù)測的關(guān)節(jié)角對應(yīng)的位姿與目標(biāo)位姿的距離大于給定精度，新預(yù)測的數(shù)據(jù)可以作為相鄰的節(jié)點(diǎn)迭代預(yù)測，直至滿足給定精度或超出最大迭代次數(shù)。

正向插值流程如圖5所示。

圖5　正向插值流程Fig. 5　Flow chart of forward interpolation

反向插值流程如圖6所示。

其中，關(guān)節(jié)角的距離計算公式為：

dis=‖θ-θgoal‖

(23)

位姿的距離計算公式為：

(24)

其中位置參數(shù)以mm為單位，姿態(tài)參數(shù)以度為單位。

5　仿真驗(yàn)證實(shí)驗(yàn)

實(shí)驗(yàn)硬件環(huán)境：一臺計算機(jī)(CPU為Intel i5- 4460 3.2 GHz,內(nèi)存為8 GB)。

實(shí)驗(yàn)軟件環(huán)境：Matlab R2015b。

首先使用Matlab按照正運(yùn)動學(xué)方程在限定每個關(guān)節(jié)活動角度為30°的范圍內(nèi)隨機(jī)產(chǎn)生100萬組經(jīng)驗(yàn)數(shù)據(jù)，同時隨機(jī)產(chǎn)生1萬組測試數(shù)據(jù)。

從測試數(shù)據(jù)中隨機(jī)選取30組待預(yù)測關(guān)節(jié)角，按照正向插值算法預(yù)測關(guān)節(jié)角對應(yīng)的位姿，其中X坐標(biāo)預(yù)測結(jié)果如圖7所示。由圖7可以看出，正向插值預(yù)測的位姿與實(shí)際位姿一致。

圖6　反插值流程Fig. 6　Flow chart of inverse interpolation

圖7　正向插值結(jié)果Fig. 7　Forward interpolation results

從測試數(shù)據(jù)中隨機(jī)選取一個目標(biāo)位姿：

依據(jù)徑向基網(wǎng)絡(luò)(RBFN)求解逆運(yùn)動學(xué)解算法，設(shè)定均方誤差(Mean Squared Error, MSE)為0.1，最大神經(jīng)元個數(shù)為1 000，誤差為0，最大迭代次數(shù)為10，預(yù)測角度逼近目標(biāo)位置的情況如表2所示。其中，角度和方差(Sum of Squares due to Error, SSE)為6個關(guān)節(jié)角參量和目標(biāo)關(guān)節(jié)角參量的誤差平方和，位姿SSE為6個位姿參量和目標(biāo)位姿的誤差平方和。由表2可以看出，隨著迭代次數(shù)的增加，徑向基網(wǎng)絡(luò)預(yù)測的關(guān)節(jié)角對應(yīng)的位姿慢慢逼近目標(biāo)位姿，但是逼近速度緩慢。

按照反向插值步驟設(shè)置誤差為0，最大迭代次數(shù)為10，實(shí)驗(yàn)結(jié)果如表3所示。由表3可以看出，在第三次反向插值時，位姿SSE(6個位姿參量和目標(biāo)位姿的誤差平方和)為0.002 2<1,說明位姿中三個位置參量的誤差小于1 mm，三個姿態(tài)參量誤差小于1°；第10次插值時，相鄰的7個點(diǎn)相距很近，超出Matlab運(yùn)算精度，矩陣奇異退出。六維線性反插值理論在收斂的情況下理論上可以求出給定位姿的任意精度的逆運(yùn)動學(xué)解。

表2　RBF實(shí)驗(yàn)結(jié)果Tab. 2　Experimental results of RBF

表3　反插值實(shí)驗(yàn)結(jié)果Tab. 3　Experimental results of reverse interpolation

隨機(jī)選取30組位姿，設(shè)定誤差為0，最大迭代次數(shù)為10，六維線性反插值和徑向基網(wǎng)絡(luò)預(yù)測的關(guān)節(jié)角對應(yīng)的位姿的X坐標(biāo)誤差如圖8所示。從圖8中可以看出，徑向基網(wǎng)絡(luò)比較穩(wěn)定，六維線性反插值法在一個點(diǎn)預(yù)測誤差較大，但整體預(yù)測精度較高。徑向基網(wǎng)絡(luò)預(yù)測耗時2 034 s，插值法耗時127 s，插值法能夠更快地作出預(yù)測。

圖8　反插值與RBF預(yù)測誤差對比Fig. 8　Comparison of prediction error between inverse interpolation and RBF

隨機(jī)選取10 000組測試數(shù)據(jù)，設(shè)定要求精度：位姿SSE<1,實(shí)驗(yàn)結(jié)果顯示8 940組測試數(shù)據(jù)得到滿足要求精度的逆運(yùn)動學(xué)解，逆運(yùn)動學(xué)求解率89.4%，求解率較高，能夠滿足機(jī)器人日常抓取的需求。

6　結(jié)語

針對一般結(jié)構(gòu)的六自由度機(jī)械臂逆運(yùn)動學(xué)方程求解困難的問題，本文借鑒徑向基網(wǎng)絡(luò)基于數(shù)據(jù)的求解思想，結(jié)合一維、二維線性插值理論，提出六維線性插值及反插值理論。實(shí)驗(yàn)結(jié)果表明本文提出的反插值法能夠通過較少的迭代次數(shù)求出給定精度的逆運(yùn)動學(xué)解，且逆運(yùn)動學(xué)解的求解率較高。本文的方法雖然能夠求解任意精度的逆運(yùn)動學(xué)解，但求解率只達(dá)到了89.4%，未達(dá)到100%，下一步將研究提高求解率的方法；以及如何優(yōu)化存儲結(jié)構(gòu)，在大批量的經(jīng)驗(yàn)數(shù)據(jù)中快速找出7個相鄰的非線性相關(guān)的節(jié)點(diǎn)。

參考文獻(xiàn)(References)

[1]王英石.冗余機(jī)器人的運(yùn)動學(xué)及軌跡規(guī)劃研究[D].天津：南開大學(xué)，2014：15-32. (WANG Y S. Research on the kinematics and trajectory planning of redundant robots [D]. Tianjin: Nankai University, 2014: 15-32.)

[2]劉亞軍，黃田.6R操作臂逆運(yùn)動學(xué)分析與軌跡規(guī)劃[J].機(jī)械工程學(xué)報，2012,48(3)：9-15. (LIU Y J, HUANG T. Inverse kinematics and trajectory planning of 6R serial manipulators [J]. Journal of Mechanical Engineering, 2012, 48(3): 9-15.)

[3]QIAO S, LIAO Q, WEI S, et al. Inverse kinematic analysis of the general 6R robot made of RGRR-I [J]. Chinese Journal of Mechanical Engineering, 2010, 46(1): 30-36.

[4]ROCCO S D, EKLUND D, SOMMESE A J, et al. Algebraic C*-actions and the inverse kinematics of a general 6R manipulator [J]. Applied Mathematics and Computation, 2010, 216(9): 2512-2524.

[5]沈瓏斌.冗余靈巧臂的示教學(xué)習(xí)[D].杭州：浙江大學(xué)，2014：16-17. (SHEN L B. Learning from demonstration for redundant manipulator [D]. Hangzhou: Zhejiang University, 2014: 16-17.)

[6]KALRA P, MAHAPATRA P B, AGGARWAL D K. An evolutionary approach for solving the multimodal inverse kinematics problem of industrial robots [J]. Mechanism and Machine Theory, 2006, 41(10): 1213-1229.

[7]CHIDDARWAR S S, BABU N R. Comparison of RBF and MLP neural networks to solve inverse kinematic problem for 6R serial robot by a fusion approach [J]. Engineering Applications of Artificial Intelligence, 2010, 23(7): 1083-1092.

[8]凌家良，施榮華，王國才.工業(yè)機(jī)器人關(guān)節(jié)空間的插值軌跡規(guī)劃[J].惠州學(xué)院學(xué)報，2009,29(3)：52-57. (LING J L, SHI R H, WANG G C. The industrial robot trajectory planning in joint space [J]. Journal of Huizhou University, 2009, 29(3): 52-57.)

[9]李艷，朱軍，胡亞，等.不同插值方法模擬四川省逐月降水量的對比分析[J].水土保持研究，2017,24(1)：151-160. (LI Y, ZHU J, HU Y, et al. Comparison analysis on different spatial interpolation methods to simulate monthly precipitation in Sichuan province [J]. Research of Soil and Water Conservation, 2017, 24(1): 151-160.)

[10]李峋，仵彥卿，范海梅.高維空間插值在海洋環(huán)境數(shù)據(jù)預(yù)處理中的應(yīng)用[J].海洋環(huán)境科學(xué)，2009,28(6)：729-733. (LI X, WU Y Q, FAN H M. Application of high-dimension interpolation method in pretreatment of marine environmental data [J]. Marine Environmental Science, 2009, 28(6): 729-733.)

[11]SICILIANO B, KHATIB O. Handbook of Robotics [M]. Berlin: Springer, 2007: 24-26.

[12]張欣.智能乒乓球發(fā)球機(jī)器人[D].廈門：廈門大學(xué)，2014. (ZHANG X. The intelligent table tennis serve robot [D]. Xiamen: Xiamen University, 2014.)