李憲華　張雷剛　張軍　于清波

摘 要：針對(duì)川崎機(jī)械臂FS03N的構(gòu)型特點(diǎn)，提出了一種逆運(yùn)動(dòng)學(xué)的求解方法。采用DH法建立了機(jī)械臂的連桿坐標(biāo)系，得到正運(yùn)動(dòng)學(xué)方程，通過變量分離將機(jī)械臂姿態(tài)采用歐拉角表示，得到了機(jī)械臂位姿的一組廣義坐標(biāo)。通過對(duì)FS03N的構(gòu)型分析，采用幾何法與反變換法相結(jié)合的方法，以解的組合關(guān)系為基礎(chǔ)，得到了機(jī)械臂的8組封閉解。建立了基于Matlab的機(jī)械臂算法驗(yàn)證與3D仿真運(yùn)動(dòng)平臺(tái)，驗(yàn)證了逆運(yùn)動(dòng)學(xué)解算的正確性，為機(jī)械臂的軌跡與路徑規(guī)劃提供了前提條件。

關(guān)鍵詞：機(jī)械臂；逆運(yùn)動(dòng)學(xué)；組合關(guān)系；封閉解；仿真驗(yàn)證

中圖分類號(hào)：TD241；TD391 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：1672-1098（2014）03-0039-06

機(jī)械臂運(yùn)動(dòng)學(xué)是機(jī)器人研究領(lǐng)域的基礎(chǔ)課題，包括正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)。正運(yùn)動(dòng)是指在已知機(jī)械臂各關(guān)節(jié)角度的情況下，求取末端執(zhí)行器位于基礎(chǔ)坐標(biāo)系下的位姿；而逆運(yùn)動(dòng)學(xué)則是指在已知末端執(zhí)行器位于基坐系下的位姿情況下，求取機(jī)械臂的各關(guān)節(jié)角度值。對(duì)于具有串聯(lián)結(jié)構(gòu)的機(jī)械臂，正運(yùn)學(xué)求解相對(duì)容易，而對(duì)于逆運(yùn)動(dòng)學(xué)求解，由于機(jī)械臂的結(jié)構(gòu)不同，其求解的復(fù)雜程度也不相同。從工程的應(yīng)用角度出發(fā)，逆運(yùn)動(dòng)學(xué)求解更為重要。

上世紀(jì)80年代，Paul等采用的解析法對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)進(jìn)行求解，對(duì)于后來的運(yùn)動(dòng)學(xué)逆解問題具有指導(dǎo)性意義[1]。Primrose首次證明一般6自由度機(jī)械臂最多具有16組逆解[2]。Regnier等采于迭代法，能夠計(jì)算出多種結(jié)構(gòu)六自由度手臂的逆解[3]。于艷秋等采用有理數(shù)的方法求解了一般6自由度機(jī)器人手臂逆運(yùn)動(dòng)學(xué)問題，雖然保證了解的精度，但是卻難以解決實(shí)時(shí)實(shí)現(xiàn)問題[4]。朱世強(qiáng)課題組從算法的實(shí)時(shí)性角度出發(fā)，采用矩陣分解和向量?jī)?nèi)積的方法，對(duì)課題組研制的錢江一號(hào)機(jī)械臂逆解進(jìn)行了研究[5-6]。錢東海等基于旋量理論建立機(jī)械臂運(yùn)動(dòng)學(xué)模型，利用消元理論和Paden-Kahan子問題相結(jié)合的方法，提出了一種機(jī)械臂的逆運(yùn)動(dòng)學(xué)算法[7]。呂世增等將吳方法引入機(jī)械臂逆運(yùn)動(dòng)學(xué)求解中，通過特征列思想和旋量法相結(jié)合，并利用數(shù)學(xué)計(jì)算軟件實(shí)現(xiàn)了逆運(yùn)動(dòng)學(xué)的求解[8]。

在機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解過程中，解析法可以找到全部根，但是計(jì)算較為復(fù)雜；幾何法雖然對(duì)一般機(jī)械臂不具備通用，但是形式簡(jiǎn)單易于理解；迭代法受初值選取問題約束，往往無法得到全部解；遺傳算法[9-10]、神經(jīng)網(wǎng)絡(luò)[11]與Groebner基法[12]在理論上是可行的，但實(shí)時(shí)性不強(qiáng)，且存在解的精度與穩(wěn)定性問題，這與要求實(shí)時(shí)性控制高的機(jī)械臂而言，具有很大的局限性，很少用于實(shí)際機(jī)械臂控制之中。

本文首先采用DH法建立該種機(jī)械臂的連桿坐標(biāo)系，得到正運(yùn)動(dòng)學(xué)方程，并通過將姿態(tài)矩陣分離變量，得到機(jī)械臂包含六個(gè)參數(shù)的一組廣義坐標(biāo)。對(duì)于逆運(yùn)動(dòng)學(xué)求解，前3個(gè)關(guān)節(jié)采用幾何法，后3個(gè)關(guān)節(jié)采用反變換法，通過給出解的組合關(guān)系的方法，計(jì)算得到該機(jī)械臂的8組解。開發(fā)基于Matlab的機(jī)械臂運(yùn)動(dòng)仿真平臺(tái)，驗(yàn)證運(yùn)動(dòng)學(xué)逆解計(jì)算的正確性。

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

1.1 坐標(biāo)系建立

該工業(yè)機(jī)械臂共有6個(gè)旋轉(zhuǎn)關(guān)節(jié)，如圖1所示。根據(jù)機(jī)械臂的構(gòu)型特點(diǎn)，采用DH法確定其各關(guān)節(jié)處的連桿坐標(biāo)系，首先確定其基坐標(biāo)系o0-x0y0z0，原點(diǎn)位于關(guān)節(jié)1軸線與關(guān)節(jié)2軸線所在水平平面的交點(diǎn)上，然后依次建立位于關(guān)節(jié)2-6處的坐標(biāo)系，o6-x6y6z6為工具坐標(biāo)系，完整的坐標(biāo)系如圖2所示。完成坐標(biāo)系建立后，根據(jù)相鄰桿件間坐標(biāo)系的關(guān)系，確定其DH參數(shù)，如表1所示，其中θi為第i關(guān)節(jié)角度值，di為相鄰關(guān)節(jié)間的桿件長(zhǎng)度，ai為相鄰關(guān)節(jié)間的桿件偏移量，αi為相鄰坐標(biāo)系間的扭轉(zhuǎn)角。

1.2 正運(yùn)動(dòng)學(xué)方程

相鄰桿件間的坐標(biāo)變換矩陣為

將表1中的各參數(shù)分別代入式（1），可得6個(gè)矩陣T1、T2、T3、T4、T5、T6，將此6個(gè)矩陣依次相乘，便可得到機(jī)械臂從基坐標(biāo)系至工具坐標(biāo)系的坐標(biāo)變換矩陣如式2所示。

式中包含了機(jī)械臂的位置與姿態(tài)信息，其中[n s a]為姿態(tài)矩陣共有9個(gè)元素，[px py pz]′為機(jī)械臂工具坐標(biāo)系的位置。而工業(yè)機(jī)械臂一般采用一組完備的廣義坐標(biāo)（X Y Z O A T）進(jìn)行表示，因此將式（2）中包含的姿態(tài)矩陣進(jìn)行變換，采用歐拉角進(jìn)行表示。其計(jì)算思想為，令手臂的姿態(tài)矩陣與歐拉角的轉(zhuǎn)動(dòng)矩陣相等如式（3）所示，通過對(duì)應(yīng)元素相等，可以先求得第一個(gè)轉(zhuǎn)動(dòng)角度φ，在此基礎(chǔ)上，對(duì)式（3）變量分離，進(jìn)而求得θ與ψ，求解過程如下

最后可以得到機(jī)械臂的一組完備廣義坐標(biāo)（px， py， pz， φ，θ，ψ）。圖1所示機(jī)械臂各關(guān)節(jié)角度為機(jī)械零，但是此時(shí)通過DH法建立的坐標(biāo)系關(guān)節(jié)坐標(biāo)卻為（90，-90，90，0，0，0），因此在具體求解過程中，要進(jìn)行加權(quán)處理。

2 逆運(yùn)動(dòng)學(xué)

逆運(yùn)動(dòng)學(xué)就是在已知機(jī)械臂末端執(zhí)行器位姿的情況下，求解機(jī)械臂的各關(guān)節(jié)角度值，是正運(yùn)動(dòng)學(xué)的反過程。串聯(lián)機(jī)械臂正運(yùn)動(dòng)學(xué)的解具有唯一性特點(diǎn)，而逆運(yùn)動(dòng)學(xué)的求解相對(duì)較為復(fù)雜，且存在多解與無解的可能性。該機(jī)械臂滿足Pieper準(zhǔn)則，可以得到最多8組逆運(yùn)動(dòng)學(xué)封閉解。在以下逆運(yùn)動(dòng)學(xué)求解過程中，各關(guān)節(jié)角度以機(jī)械臂的機(jī)械零為基準(zhǔn)進(jìn)行。

2.1 構(gòu)型分析

結(jié)合圖1與圖2， 可以看出該機(jī)械臂的肩部始終位于o0-x0y0平面上； 大臂與小臂長(zhǎng)度相同a2=d4，即肩、肘與腕三點(diǎn)構(gòu)成一等腰三角形，從可操作度的角度出發(fā)，當(dāng)大臂與小臂的之和為常數(shù)時(shí)，大臂與小臂的長(zhǎng)度相同，使機(jī)械臂的可操作度達(dá)到最大值。無論機(jī)械臂的各關(guān)節(jié)如何運(yùn)動(dòng)，其肩、肘與腕構(gòu)成的等腰三角形始終位于同一平面，該平面與水平面相互垂直。

2.2 求解θ1、θ2和θ3關(guān)節(jié)角

機(jī)械臂末端執(zhí)行器位于基坐標(biāo)系下的位姿為（px， py， pz， φ，θ，ψ），通過式（3）可以求得其姿態(tài)矩陣[n s a]，而后便可確定機(jī)械臂腕部位置坐標(biāo)（xW， yW， zW）有式（8）和圖3、圖4。

圖4 關(guān)節(jié)1角度與坐標(biāo)系關(guān)系結(jié)合該機(jī)械臂的構(gòu)型特點(diǎn)分析與圖3，可以看出點(diǎn)S、E、W與W′四點(diǎn)構(gòu)成的平面始終與o0-x0y0平面垂直，以機(jī)械臂的機(jī)械零位為基準(zhǔn)并結(jié)合關(guān)節(jié)1角度的定義（見圖4～圖6），可以得出第一關(guān)節(jié)的兩組解為

圖6 角度關(guān)系圖由于S、E、W三點(diǎn)位于與o0-x0y0垂直的平面上，為了便于求解，將此空間中的三個(gè)點(diǎn)轉(zhuǎn)換為平面問題進(jìn)行求解。W點(diǎn)位于o0-x0y0z0坐標(biāo)系下的坐標(biāo)值已知，將該坐標(biāo)值轉(zhuǎn)化為如圖5所示的S-x′ 0 y′ 0 z′ 0，從S-x′ 0 y′ 0 z′ 0至o0-x0y0 z0可以看作由一個(gè)平移與一個(gè)旋轉(zhuǎn)組成，S-x′ 0 y′ 0 z′ 0先沿其y′ 0 軸平移-a1個(gè)單位，然后繞z′ 0 旋轉(zhuǎn)-θ1角度，如圖5所示，得到其旋轉(zhuǎn)矩陣為

求取關(guān)節(jié)1角度值后，再進(jìn)行關(guān)節(jié)3的求取，圖6為坐標(biāo)系轉(zhuǎn)換后的關(guān)節(jié)角度關(guān)系圖，其中E′為機(jī)械臂肘部可能的另一位置。結(jié)合圖5與圖6，求取關(guān)節(jié)3角度值有

2.3 求解θ4、θ5和θ6 關(guān)節(jié)角

2.4 解的對(duì)應(yīng)關(guān)系

通過以上可以計(jì)算得到該機(jī)械臂的各關(guān)節(jié)角度值，共有可能的8組解。對(duì)于給定的一組位姿值（px， py，pz，φ，θ，ψ），通過式（9）與（10）可以求得第1關(guān)節(jié)的兩組解，通過式（14）可以求得第3關(guān)節(jié)的兩組解，通過式（17）可以求得第2關(guān)節(jié)的兩組解，第2關(guān)節(jié)的兩組解都與第1關(guān)節(jié)的解相關(guān)聯(lián)，因此對(duì)于第1關(guān)節(jié)的兩組解，第2關(guān)節(jié)共有四組解，且其取值與第3關(guān)節(jié)角度值的正負(fù)號(hào)相關(guān)聯(lián)，因此前三個(gè)關(guān)節(jié)解的組合共有四種組合。對(duì)于后3個(gè)關(guān)節(jié)解的組合，共有兩組，直接與前3個(gè)關(guān)節(jié)解相關(guān)聯(lián)，因此對(duì)應(yīng)每一組的前3個(gè)關(guān)節(jié)解，有兩組后3個(gè)關(guān)節(jié)解的組合，因此得出了該機(jī)械臂的8組解組合的對(duì)應(yīng)關(guān)系。為了形象地說明此8組解的組合關(guān)系，采用樹形的形式加以表述，該機(jī)械臂的逆解樹如圖7所示。

為了驗(yàn)證FS03N機(jī)械臂運(yùn)動(dòng)學(xué)模型建立的正確性，開發(fā)了基于Matlab的機(jī)械臂運(yùn)動(dòng)仿真平臺(tái)，圖8為軟件設(shè)計(jì)流程圖，圖9為構(gòu)建的仿真平臺(tái)。本仿真平臺(tái)包含了正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)驗(yàn)證，并且可以實(shí)時(shí)地觀察到機(jī)械臂的運(yùn)動(dòng)情況。

逆運(yùn)動(dòng)學(xué)求解正確性的驗(yàn)證如下：先隨機(jī)取機(jī)械臂的一組關(guān)節(jié)角度（30， 20， -70， -40， -30，-60），通過正運(yùn)動(dòng)學(xué)計(jì)算公式可以得到此時(shí)的機(jī)械臂位姿為（7.120 86，-63.756 69，416.680 73，-79.459 80，74.736 51，89.308 97），然后將此組位姿值作為機(jī)械臂逆運(yùn)動(dòng)學(xué)求解計(jì)算的輸入值，可以計(jì)算得到該機(jī)械臂的8組解，而后將這8組解逐組輸入正運(yùn)動(dòng)學(xué)求解的文本框內(nèi)，每組得到一組與之對(duì)應(yīng)的位姿值，每一組位姿與原輸入的角度值得到的位姿進(jìn)行比較，結(jié)果相同，從而驗(yàn)證了逆運(yùn)動(dòng)學(xué)算法的正確性，并且此8組解中包含原隨機(jī)輸入的一組關(guān)節(jié)角度值，如表2～表3所示。

4 結(jié)束語

本文針對(duì)川崎FS03N機(jī)械臂的運(yùn)動(dòng)學(xué)問題，采用DH方法建立了機(jī)械臂的關(guān)節(jié)坐標(biāo)系，得到了正運(yùn)動(dòng)學(xué)方程，而后采用變量分離的辦法，對(duì)位姿采用6個(gè)參數(shù)加以表示。對(duì)于逆運(yùn)動(dòng)學(xué)求解，前3個(gè)關(guān)節(jié)采用幾何法求解，后3個(gè)關(guān)節(jié)采用反變換法求解，通過給出解的幾何關(guān)系，得到了該種構(gòu)型機(jī)械臂的8組封閉解。為了驗(yàn)證算法了正確性，開發(fā)了基于Matlab軟件的運(yùn)動(dòng)仿真平臺(tái)，不僅驗(yàn)證了算法的正確性更能直觀地觀察到機(jī)械臂的運(yùn)動(dòng)情況。本文采用的計(jì)算方法，可以為機(jī)械臂的運(yùn)動(dòng)控制提供相應(yīng)的理論依據(jù)，開發(fā)的運(yùn)動(dòng)仿真平臺(tái)為機(jī)械臂的軌跡規(guī)劃與解決避碰問題提供了可視化的條件。

參考文獻(xiàn)：

[1] PAUL R P， SHIMANO B E， MAYER G. Kinematics control equations for simple manipulators[J]. IEEE Transactions on System， Man and Cybernetics， 1981， 11（6）：449-455.

[2] E J F PRIMROSE.On the input-output equation of the general 7R mechanism[J]. Mechanism and Machine Theory， 1986， 21（6）：509-510.

[3] REGNIER S， OUEZDOU F B， BIDAUD P. Distributed method for inverse kinematics of all serial manipulators[J]. Mechanism and Machine Theory， 1997， 32（7）：855-867.

[4] 于艷秋， 廖啟征. 基于有理數(shù)運(yùn)算的一般6R機(jī)器人位置逆解算法[J]. 機(jī)械工程學(xué)報(bào)， 2005， 41（3）：229-233.

[5] 劉松國， 朱世強(qiáng)， 王宣銀. 基于矩陣分解的一般6R機(jī)器人實(shí)時(shí)高精度逆運(yùn)動(dòng)學(xué)算法[J]. 機(jī)械工程學(xué)報(bào)， 2008， 44（11）：304-309.

[6] 劉華山， 朱世強(qiáng)， 吳劍波. 基于向量積的機(jī)器人實(shí)時(shí)性逆解算法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2009， 40（6）：212-216.

[7] 錢東海， 王新峰， 趙偉， 等. 基于旋量理論和Paden-Kahan子問題的6自由度機(jī)器人逆解算法[J]. 機(jī)械工程學(xué)報(bào)， 2009， 45（9）：72-76.

[8] 呂世增， 張大衛(wèi)， 劉海年. 基于吳方法的6R機(jī)器人逆運(yùn)動(dòng)學(xué)旋量方程求解[J]. 機(jī)械工程學(xué)報(bào)， 2010， 46（17）：35-41.

[9] HUANG WUXIN， TAN SHILI. Inverse kinematics of compliant manipulator based on the immune genetic algorithm[C]// International conference on natural comutation. Ji'nan， China， 2008：390-394.

[10] 張智， 朱齊丹， 吳自新. 基于免疫遺傳算法的機(jī)械手避碰逆解[J].系統(tǒng)仿真學(xué)報(bào)， 2007， 19（3）：514-518.

[11] V T JOVANOVIC， K KAZEROUNIAN.Using chaos to obtain global solutions in computational kinematics[J]. Journal of Mechanical， 1998， 120：299-304.

[12] 杭魯濱， 王彥. 基于Groebner基法的一般串聯(lián)6R機(jī)器人機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)分析[J]. 上海交通大學(xué)學(xué)報(bào)， 2004， 38（6）：853-856.

（責(zé)任編輯：李 麗）