輕型仿生機(jī)器手的幾何求逆優(yōu)化及軌跡規(guī)劃

張明松，黃滔

（1. 三峽大學(xué) 機(jī)械與動(dòng)力學(xué)院，湖北 宜昌 443002；2. 水電機(jī)械設(shè)備設(shè)計(jì)與維護(hù)湖北省重點(diǎn)實(shí)驗(yàn)室，湖北 宜昌 443002）

輕型機(jī)械手廣泛應(yīng)用于科研教育以及康復(fù)醫(yī)療等多個(gè)領(lǐng)域，其設(shè)計(jì)非常輕巧便捷，內(nèi)置的控制器也比較方便用戶輕松控制機(jī)械臂，主要是全關(guān)節(jié)內(nèi)置的扭矩傳感器直接影響到了機(jī)械臂的性能[1]，使其擁有良好的安全性及人機(jī)交互性[2]。目前國(guó)內(nèi)有很多學(xué)者在機(jī)械臂運(yùn)動(dòng)學(xué)上以及機(jī)械臂軌跡規(guī)劃上有比較深入的研究。周霏等[3]基于四自由度機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)仿真，采用數(shù)值法進(jìn)行逆運(yùn)動(dòng)學(xué)的求解和分析；孫龍等[4]針對(duì)目前六自由度機(jī)械臂的運(yùn)動(dòng)過程中所產(chǎn)生軌跡偏差以及不平穩(wěn)的問題進(jìn)行大量模型的仿真研究完成正運(yùn)動(dòng)學(xué)分析，采用了數(shù)值法和多項(xiàng)式插值法，最終確定了機(jī)械臂的運(yùn)動(dòng)空間的預(yù)測(cè)和運(yùn)動(dòng)軌跡的規(guī)劃；劉壯壯等[5]針對(duì)七自由度冗余機(jī)器臂的奇異位型、運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)以及軌跡規(guī)劃問題進(jìn)行詳細(xì)的說明和分析，利用計(jì)算機(jī)軟件搭建了機(jī)械臂仿真環(huán)境，并且最終完成了算法的驗(yàn)證和證明；機(jī)械臂的運(yùn)動(dòng)學(xué)分析、軌跡規(guī)劃、動(dòng)力學(xué)仿真等，其中的各個(gè)板塊之間都有一定的關(guān)聯(lián)。運(yùn)動(dòng)學(xué)的理論公式和方程的推導(dǎo)以及最終求解，很大程度上影響了機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃，機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃又包含著各個(gè)關(guān)節(jié)的位移、速度、加速度的計(jì)算和求解，對(duì)于其中的任意值的變化都必定帶動(dòng)整個(gè)機(jī)械臂運(yùn)動(dòng)軌跡的變化，從而會(huì)導(dǎo)致之后的動(dòng)力學(xué)仿真分析產(chǎn)生不符合實(shí)際的運(yùn)動(dòng)。為了保證能夠更加符合實(shí)際，確保具有實(shí)際研究?jī)r(jià)值，對(duì)運(yùn)動(dòng)學(xué)計(jì)算和求解的研究是必不可少的。

本文以KINOVA jaco2 6DOF-S超輕量型機(jī)械臂為研究模型，基于一種結(jié)合位姿分離思想的幾何求逆優(yōu)化算法完成機(jī)械臂的運(yùn)動(dòng)分析，并使用數(shù)學(xué)計(jì)算軟件進(jìn)行計(jì)算和求解，最終完成對(duì)機(jī)械手臂的空間運(yùn)動(dòng)軌跡的分析，從而證明理論的準(zhǔn)確性和研究的合理性。本文中大部分使用數(shù)學(xué)計(jì)算軟件完成最終的計(jì)算和求解，從而保證求解的準(zhǔn)確性。為了保證理論的可靠性和計(jì)算的準(zhǔn)確性，使用MATLAB中的Robotics仿真模塊來建立機(jī)械臂數(shù)學(xué)模型，并將數(shù)學(xué)模型轉(zhuǎn)換為可直接觀察和分析的物理模型，完成對(duì)正運(yùn)動(dòng)學(xué)和改進(jìn)逆運(yùn)動(dòng)學(xué)算法進(jìn)行驗(yàn)證。最后基于正逆運(yùn)動(dòng)學(xué)的分析和求解，和五次多項(xiàng)式的計(jì)算和求解，最終完成對(duì)機(jī)械手臂進(jìn)行空間運(yùn)動(dòng)軌跡的規(guī)劃，為機(jī)械手臂控制和動(dòng)力學(xué)仿真的研究奠定理論基礎(chǔ)。

1 正運(yùn)動(dòng)學(xué)分析

1.1 連桿坐標(biāo)系的建立及D-H參數(shù)的構(gòu)建

連桿坐標(biāo)系的建立是為了研究機(jī)器人連桿之間的關(guān)系。本文中所研究對(duì)象是六自由度冗余機(jī)械手臂，因此對(duì)應(yīng)著會(huì)有六個(gè)關(guān)節(jié)軸，首先需要確定的是各個(gè)相鄰的軸線，設(shè)關(guān)節(jié)軸為i（i＝1～6）軸，其相鄰關(guān)節(jié)軸為i＋1軸，具體的坐標(biāo)系建立步驟如下：

步驟1：確定Zi軸，根據(jù)上述所確定的關(guān)節(jié)軸，可知相鄰的兩個(gè)關(guān)節(jié)軸分別為i軸和i＋1軸，最終根據(jù)兩個(gè)相鄰軸的關(guān)系來確定Zi軸，沿關(guān)節(jié)軸i的指向?yàn)閆i軸。

步驟2：確定機(jī)械臂空間坐標(biāo)系的原點(diǎn)位置，確定原點(diǎn)有以下兩種方法，第一種方法是關(guān)節(jié)軸i和i＋1不平行時(shí)，此時(shí)就需要找出關(guān)節(jié)軸i和i＋1之間的公垂線，將找出的關(guān)節(jié)軸i和i＋1的公垂線和關(guān)節(jié)軸i進(jìn)行相交，公垂線與關(guān)節(jié)軸相交的交點(diǎn)就是機(jī)械臂中連桿空間坐標(biāo)系{i}的原點(diǎn)。第二種方法是關(guān)節(jié)軸i和i＋1平行時(shí)，此時(shí)就只需找到關(guān)節(jié)軸i和i＋1的交點(diǎn)，將找到的關(guān)節(jié)軸i和i＋1的交點(diǎn)和關(guān)節(jié)軸i相交，關(guān)節(jié)軸i和i＋1的交點(diǎn)與關(guān)節(jié)軸相交的交點(diǎn)就是機(jī)械臂中連桿空間坐標(biāo)系{i}的原點(diǎn)。

步驟3：確定六自由度機(jī)械臂的Xi軸線。根據(jù)步驟1所確定的Zi軸來確定Xi軸，規(guī)定Xi軸垂直Zi軸，Xi軸的方向指向Zi＋1。確定Xi軸時(shí)也會(huì)出現(xiàn)關(guān)節(jié)軸i和i＋1相交的情況，這種情況下就認(rèn)為Xi軸與關(guān)節(jié)軸i和i＋1所在的平面垂直。

步驟4：確定Yi軸。根據(jù)步驟1～3可以知道機(jī)械臂空間坐標(biāo)系中的原點(diǎn)、Xi軸、Zi軸，進(jìn)而可以采用右手定來找出Yi軸。

根據(jù)KINOVA jaco2 6DOF-S超輕量型機(jī)械臂的結(jié)構(gòu)特點(diǎn)以及上述的坐標(biāo)系確定方法建立如圖1所示的連桿坐標(biāo)系。

圖1 機(jī)械臂連桿坐標(biāo)系

構(gòu)建D-H參數(shù)如表1所示。

表1 Jaco2 6DOF-S超輕量型機(jī)械臂D-H參數(shù)

1.2 正運(yùn)動(dòng)學(xué)計(jì)算

在定義機(jī)械臂的位姿時(shí)，其中坐標(biāo)系{i}和坐標(biāo)系{i-1}的齊次變換矩陣為，兩連桿之間的齊次變換矩陣為[2]：

式中：cθi表示cosθi；sθi表示sinθi；cαi-1表示cosαi-1；sαi-1表示sinαi-1。

已知D-H參數(shù)表和齊次變換矩陣，將

參數(shù)表中數(shù)據(jù)代入式（1）可計(jì)算出各相鄰兩個(gè)坐標(biāo)系之間的變換矩陣為：

將矩陣依次相乘可得正運(yùn)動(dòng)學(xué)公式為：

式中：

1.3 正運(yùn)動(dòng)學(xué)仿真

運(yùn)用MATLABRobotics Toolbox工具箱中Link函數(shù)和SerialLink函數(shù)搭建六自由度機(jī)械臂模型，生成六自由度機(jī)械手臂可視化模型，使用teach 函數(shù)生成可調(diào)節(jié)關(guān)節(jié)角的人機(jī)交互界面，為了能夠方便對(duì)六自由度機(jī)械手臂的研究，筆者將機(jī)械手臂的初始姿態(tài)設(shè)定為各個(gè)關(guān)節(jié)角均為零度時(shí)的運(yùn)動(dòng)姿態(tài)。

如圖2所示為機(jī)械臂的初始姿態(tài)，圖2（a）為機(jī)械臂關(guān)節(jié)角控制操作界面，拖動(dòng)界面上的滑塊就可以調(diào)節(jié)各個(gè)關(guān)節(jié)角度的大小，從而控制六自由度機(jī)械臂空間位置的姿態(tài)，實(shí)現(xiàn)手動(dòng)控制機(jī)械手臂的末端執(zhí)行器運(yùn)動(dòng)到所希望的位姿，圖2（b）為六自由度機(jī)械手臂的三維模型，此時(shí)圖中顯示的是六自由度機(jī)械手臂處于各個(gè)關(guān)節(jié)角為零度時(shí)的位姿。右側(cè)圖中的機(jī)械臂三維模型顯示的目的是為了保證手動(dòng)設(shè)置各個(gè)關(guān)節(jié)角時(shí)能夠?qū)崟r(shí)的觀察六自由度機(jī)械臂的位姿狀態(tài)，進(jìn)而可以更加準(zhǔn)確、快速地設(shè)定各個(gè)關(guān)節(jié)角度的大小。

圖2 機(jī)械臂初始姿態(tài)

另外，設(shè)置右側(cè)圖中的機(jī)械臂三維模型也方便了對(duì)正運(yùn)動(dòng)的計(jì)算結(jié)果進(jìn)行驗(yàn)證，本文所采用驗(yàn)證方法的具體步驟為：首先需要依據(jù)D-H參數(shù)表要求，任意選取六個(gè)關(guān)節(jié)角的角度大小，將其數(shù)據(jù)代入到本文中所列出的公式中，經(jīng)過理論計(jì)算后會(huì)得出一個(gè)機(jī)械手臂末端執(zhí)行器的位姿；之后，再通過運(yùn)用teach()命令進(jìn)行對(duì)比驗(yàn)證，將正運(yùn)動(dòng)學(xué)所求解的結(jié)果代入機(jī)器人工具箱中就能夠觀察機(jī)械臂的位置姿態(tài)。最后通過比較理論計(jì)算后的數(shù)值與使用機(jī)器人工具箱進(jìn)行仿真后的數(shù)值。本文使用了以上所提到的方法進(jìn)行了正運(yùn)動(dòng)學(xué)驗(yàn)算，結(jié)果顯示兩個(gè)數(shù)值保持一致，說明了本文理論計(jì)算是正確的，從而保證了后面的逆運(yùn)動(dòng)學(xué)和空間軌跡規(guī)劃的正確性和合理性。

2 幾何求逆優(yōu)化算法的設(shè)計(jì)

所謂逆運(yùn)動(dòng)學(xué)其實(shí)就是正運(yùn)動(dòng)學(xué)的逆算過程，在機(jī)械手臂實(shí)際的工作運(yùn)行中，往往都是給定機(jī)械手臂末端執(zhí)行器的位置，再通過計(jì)算機(jī)的運(yùn)算求解出每個(gè)關(guān)節(jié)角的角度大小，最后傳輸?shù)礁鱾€(gè)關(guān)節(jié)角的控制器中，從而控制著機(jī)械手臂實(shí)際的運(yùn)動(dòng)狀態(tài)。因此機(jī)器臂逆運(yùn)動(dòng)學(xué)計(jì)算就是在已知機(jī)械手臂末端執(zhí)行器的位姿后來進(jìn)行逆推導(dǎo)過程，通常逆推導(dǎo)后會(huì)計(jì)算出很多關(guān)節(jié)角的數(shù)值，所以需要根據(jù)機(jī)械手臂實(shí)際的工作要求選取那些符合工作要求的關(guān)節(jié)角再進(jìn)行之后的處理。

在逆推導(dǎo)的過程中有很多求解方法，本文采用的是幾何解法，將六自由度機(jī)械手臂的結(jié)構(gòu)用實(shí)線和虛線進(jìn)行表示，再通過幾何學(xué)進(jìn)行求解，但本文中所采用的幾何法是基于普通幾何法的求解進(jìn)行優(yōu)化處理，目的是可以更快的求解結(jié)果。本文逆推導(dǎo)的過程為：

式中：R為末端執(zhí)行器所在坐標(biāo)系相對(duì)基坐標(biāo)系的旋轉(zhuǎn)矩陣；P為笛卡爾空間中的位置矩陣[6]。

采用旋轉(zhuǎn)矩陣與位置矩陣分離法，使位置矩陣與幾何法結(jié)合來求解前三個(gè)關(guān)節(jié)角θ1、θ2、θ3隨后在此基礎(chǔ)上利用關(guān)節(jié)連桿4、5、6的旋轉(zhuǎn)矩陣來求解余下的關(guān)節(jié)角[7]。本文為了便于計(jì)算，基于θ2＝θ3的前提條件下進(jìn)行逆運(yùn)動(dòng)學(xué)求解，具體如下：

（1）步驟1：求關(guān)節(jié)角θ1、θ2、θ3。

根據(jù)D-H坐標(biāo)系和末端執(zhí)行器位姿矩陣，可得機(jī)械臂末端坐標(biāo)系原點(diǎn)的位置矩陣[Px Py Pz]T，將機(jī)械臂各關(guān)節(jié)連桿簡(jiǎn)化為剛體，其幾何位置關(guān)系如圖3所示[8]。

圖3 機(jī)械臂連桿幾何位置關(guān)系

由于本文所研究的機(jī)械臂關(guān)節(jié)6的運(yùn)動(dòng)對(duì)關(guān)節(jié)5的位置不產(chǎn)生影響。因此可以計(jì)算出關(guān)節(jié)5相對(duì)于基坐標(biāo)的位置矩陣，即：

可得：

由圖3可得機(jī)械臂末端執(zhí)行器的位置關(guān)系函數(shù)表達(dá)式為：

由式（6）、式（7）求解得：

將式（6）和式（8）聯(lián)立，平方后相加得：

（2）步驟2：求關(guān)節(jié)角θ4、θ5、θ6。

相鄰兩連桿間旋轉(zhuǎn)矩陣為：

由旋轉(zhuǎn)矩陣正交性質(zhì)可得：

求解得：

當(dāng)0＜θ6≤120°時(shí)：

當(dāng)-120°≤θ6≤0時(shí)：

由式（15）、式（16）可知θ6有兩種解，產(chǎn)生的原因是由于各關(guān)節(jié)角位移求解結(jié)果相互關(guān)聯(lián)。六自由度機(jī)械手臂在實(shí)際運(yùn)動(dòng)過程中會(huì)有一部分的解表示的關(guān)節(jié)角位移是機(jī)械手臂到達(dá)不了的位置，因此最后所求逆解的選取要由機(jī)械手臂的實(shí)際工作情況而定[9]。

3 關(guān)節(jié)空間軌跡規(guī)劃

關(guān)節(jié)空間軌跡規(guī)劃是用來規(guī)劃?rùn)C(jī)械手臂實(shí)際的運(yùn)動(dòng)軌跡，主要是利用每個(gè)關(guān)節(jié)的變量與時(shí)間之間的函數(shù)關(guān)系所形成的方程來表述機(jī)械手臂的運(yùn)動(dòng)軌跡。本文采用的是五次多項(xiàng)式插值算法，基于MatlabRoboticsToolbox平臺(tái)進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃，進(jìn)而得到各關(guān)節(jié)角度、速度和加速度與時(shí)間關(guān)系曲線[10]。

五次多項(xiàng)式中包含6個(gè)待定系數(shù)，其中的任意系數(shù)都可對(duì)起始點(diǎn)和目標(biāo)點(diǎn)的角度、角速度和角加速度進(jìn)行同時(shí)約束，設(shè)六自由度機(jī)械手臂關(guān)節(jié)角符合：

將一小段軌跡的頭尾兩個(gè)點(diǎn)看作運(yùn)動(dòng)軌跡的起點(diǎn)θ0和終點(diǎn)θf，并將起始速度約束為V0，終止速度約束為Vf，如下所示：

將約束條件代入函數(shù)式（18），可求得式（19），從而可得到插值法規(guī)劃的軌跡。

就機(jī)械手臂的空間軌跡規(guī)劃問題，通過五次多項(xiàng)式的插值算法，計(jì)算出滿足多約束條件下六自由度機(jī)械手臂運(yùn)動(dòng)的多項(xiàng)式方程，并最終通過數(shù)學(xué)計(jì)算工具M(jìn)ATLAB進(jìn)行運(yùn)算和仿真得到相應(yīng)的結(jié)果和仿真數(shù)據(jù)。對(duì)六自由度機(jī)械手臂進(jìn)行五次多項(xiàng)式插值，不僅可以得到機(jī)械手臂末端執(zhí)行器的空間軌跡，還可以得到每一個(gè)關(guān)節(jié)的角位移、角速度和角加速度[11]。

如圖4所示為機(jī)械臂末端關(guān)節(jié)的空間軌跡圖，空間軌跡圖的生成需要初始點(diǎn)和終止點(diǎn)兩個(gè)點(diǎn)作為起始和終止位置，筆者在做起始點(diǎn)和終止點(diǎn)設(shè)定時(shí)特意選定起始A點(diǎn)為機(jī)械臂各關(guān)節(jié)均為0度時(shí)機(jī)械臂末端關(guān)節(jié)的位置，而對(duì)空間軌跡終止B點(diǎn)則設(shè)定為關(guān)節(jié)角1為90°、關(guān)節(jié)角2為負(fù)30°、關(guān)節(jié)角3為30°、關(guān)節(jié)角4為0°、關(guān)節(jié)角5為30°、關(guān)節(jié)角5為0°時(shí)的機(jī)械臂末端關(guān)節(jié)位置。圖4（a）為機(jī)械手臂空間姿態(tài)圖，圖中所顯示的是機(jī)械手臂移動(dòng)到B點(diǎn)時(shí)的位姿。圖4（b）為空間軌跡圖，圖上所顯示的A、B兩點(diǎn)分別代表機(jī)械手臂的初始點(diǎn)和終止點(diǎn)，空間軌跡線表示的是機(jī)械手臂末端關(guān)節(jié)從初始A點(diǎn)移動(dòng)到終止B點(diǎn)所經(jīng)過的路徑。從圖4來看，空間軌跡為一條曲線型的軌跡，沒有突變也沒有斷口，且機(jī)械手臂能準(zhǔn)確地到達(dá)提前已設(shè)置好的的位置，進(jìn)而說明空間軌跡點(diǎn)設(shè)定沒有問題并且機(jī)械手臂控制具有合理性。

如圖5所示為機(jī)械手臂各個(gè)關(guān)節(jié)角位移曲線圖，從圖5中各關(guān)節(jié)隨時(shí)間變化的位移曲線圖可以看到圖中的關(guān)節(jié)角1、關(guān)節(jié)角2、關(guān)節(jié)角3、關(guān)節(jié)角5的位移曲線隨時(shí)間變化而變化的大致相同，只是零時(shí)刻弧度和達(dá)到穩(wěn)定點(diǎn)時(shí)刻不同，而圖中的關(guān)節(jié)角4、關(guān)節(jié)角6位移顯示為零，產(chǎn)生這樣的原因是機(jī)械手臂的起始點(diǎn)和終止點(diǎn)的設(shè)定不同，因此每個(gè)關(guān)節(jié)對(duì)應(yīng)的變化就有所不同。另外，通過對(duì)各個(gè)關(guān)節(jié)角的位移曲線圖的分析可以說明機(jī)械手臂是按照設(shè)定的路徑和程序進(jìn)行運(yùn)動(dòng)的，進(jìn)而證明的機(jī)械手臂計(jì)算和仿真的正確性。

圖6、圖7分別為六自由度機(jī)械手臂的關(guān)節(jié)角速度曲線圖和各關(guān)節(jié)角加速度曲線圖，圖中所顯示的均是各關(guān)節(jié)隨時(shí)間變化而變化的運(yùn)動(dòng)情況，關(guān)節(jié)角1、關(guān)節(jié)角2、關(guān)節(jié)角3、關(guān)節(jié)角5的角速度和角加速度隨時(shí)間變化的曲線大致相同，且角速度和角加速度均沒有突變，各關(guān)節(jié)角速度和角加速度變化的曲線是連續(xù)且緩和。通過圖4、圖5可知五次多項(xiàng)式機(jī)械臂軌跡規(guī)劃算法的有效性[11]，機(jī)械手臂的各關(guān)節(jié)運(yùn)動(dòng)平穩(wěn)，可達(dá)到空間軌跡規(guī)劃設(shè)計(jì)要求。

圖4 機(jī)械臂末端執(zhí)行器空間軌跡圖

圖5 各關(guān)節(jié)角位移曲線圖

圖6 各關(guān)節(jié)角速度曲線圖

圖7 各關(guān)節(jié)角加速度曲線圖

4 結(jié)論

針對(duì)輕型六自由度機(jī)械手，本文主要完成了對(duì)正逆運(yùn)動(dòng)學(xué)的推導(dǎo)計(jì)算，并就逆推導(dǎo)過程中提出一種結(jié)合位姿分離思想的幾何求逆優(yōu)化算法。本文有如下結(jié)論：

（1）六自由度機(jī)械手臂的正運(yùn)動(dòng)學(xué)重點(diǎn)在于計(jì)算和推導(dǎo)，其求解的結(jié)果與逆運(yùn)動(dòng)求解是相互關(guān)聯(lián)，并起到驗(yàn)證逆運(yùn)動(dòng)結(jié)果的作用。

（2）本文中六自由度機(jī)械手臂的逆運(yùn)動(dòng)學(xué)完成了各個(gè)關(guān)節(jié)角的求解，并根據(jù)本文所研究的機(jī)械手臂的結(jié)構(gòu)要求對(duì)幾何求逆算法進(jìn)行了優(yōu)化處理，提出一種位姿分離的思想，可以更好地解決多自由度求逆困難的問題。

（3）結(jié)合D-H坐標(biāo)系，本文建立了六自由度機(jī)械手臂的運(yùn)動(dòng)學(xué)模型，并推算出了六自由度機(jī)械手臂正逆運(yùn)動(dòng)學(xué)方程的數(shù)學(xué)函數(shù)表達(dá)式和各個(gè)關(guān)節(jié)運(yùn)算求解的過程，然后，通過MATLAB機(jī)器人工具箱分別驗(yàn)證和仿真了機(jī)械手臂的正、逆運(yùn)動(dòng)計(jì)算結(jié)果以及機(jī)械臂關(guān)節(jié)空間軌跡。本文完成了五次多項(xiàng)式的機(jī)械臂軌跡規(guī)劃并給出了各個(gè)關(guān)節(jié)角的角位移、角速度、角加速度的數(shù)據(jù)曲線。

本文通過公式推導(dǎo)、方程求解計(jì)算、驗(yàn)證仿真，最終結(jié)果表明，驗(yàn)算的結(jié)果與理論公式計(jì)算結(jié)果的數(shù)值幾乎保持一致，證明了本文中KINOVA jaco2 6DOF-S模型結(jié)構(gòu)和計(jì)算方法的一致性[7]，對(duì)六自由度機(jī)械臂空間控制的研究具有很大的借鑒和參考價(jià)值，為之后的機(jī)械臂動(dòng)力學(xué)等研究提供理論分析和可靠依據(jù)[9]。