一種機械臂運動學研究及3D仿真平臺構建

李憲華　張雷剛　張軍　于清波

摘 要：針對川崎機械臂FS03N的構型特點，提出了一種逆運動學的求解方法。采用DH法建立了機械臂的連桿坐標系，得到正運動學方程，通過變量分離將機械臂姿態采用歐拉角表示，得到了機械臂位姿的一組廣義坐標。通過對FS03N的構型分析，采用幾何法與反變換法相結合的方法，以解的組合關系為基礎，得到了機械臂的8組封閉解。建立了基于Matlab的機械臂算法驗證與3D仿真運動平臺，驗證了逆運動學解算的正確性，為機械臂的軌跡與路徑規劃提供了前提條件。

關鍵詞：機械臂；逆運動學；組合關系；封閉解；仿真驗證

中圖分類號：TD241；TD391 文獻標志碼：A 文章編號：1672-1098（2014）03-0039-06

機械臂運動學是機器人研究領域的基礎課題，包括正運動學與逆運動學。正運動是指在已知機械臂各關節角度的情況下，求取末端執行器位于基礎坐標系下的位姿；而逆運動學則是指在已知末端執行器位于基坐系下的位姿情況下，求取機械臂的各關節角度值。對于具有串聯結構的機械臂，正運學求解相對容易，而對于逆運動學求解，由于機械臂的結構不同，其求解的復雜程度也不相同。從工程的應用角度出發，逆運動學求解更為重要。

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

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

本文首先采用DH法建立該種機械臂的連桿坐標系，得到正運動學方程，并通過將姿態矩陣分離變量，得到機械臂包含六個參數的一組廣義坐標。對于逆運動學求解，前3個關節采用幾何法，后3個關節采用反變換法，通過給出解的組合關系的方法，計算得到該機械臂的8組解。開發基于Matlab的機械臂運動仿真平臺，驗證運動學逆解計算的正確性。

1 正運動學

1.1 坐標系建立

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

1.2 正運動學方程

相鄰桿件間的坐標變換矩陣為

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

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

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

2 逆運動學

逆運動學就是在已知機械臂末端執行器位姿的情況下，求解機械臂的各關節角度值，是正運動學的反過程。串聯機械臂正運動學的解具有唯一性特點，而逆運動學的求解相對較為復雜，且存在多解與無解的可能性。該機械臂滿足Pieper準則，可以得到最多8組逆運動學封閉解。在以下逆運動學求解過程中，各關節角度以機械臂的機械零為基準進行。

2.1 構型分析

結合圖1與圖2， 可以看出該機械臂的肩部始終位于o0-x0y0平面上； 大臂與小臂長度相同a2=d4，即肩、肘與腕三點構成一等腰三角形，從可操作度的角度出發，當大臂與小臂的之和為常數時，大臂與小臂的長度相同，使機械臂的可操作度達到最大值。無論機械臂的各關節如何運動，其肩、肘與腕構成的等腰三角形始終位于同一平面，該平面與水平面相互垂直。

2.2 求解θ1、θ2和θ3關節角

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

圖4 關節1角度與坐標系關系結合該機械臂的構型特點分析與圖3，可以看出點S、E、W與W′四點構成的平面始終與o0-x0y0平面垂直，以機械臂的機械零位為基準并結合關節1角度的定義（見圖4～圖6），可以得出第一關節的兩組解為

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

求取關節1角度值后，再進行關節3的求取，圖6為坐標系轉換后的關節角度關系圖，其中E′為機械臂肘部可能的另一位置。結合圖5與圖6，求取關節3角度值有

2.3 求解θ4、θ5和θ6 關節角

2.4 解的對應關系

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

為了驗證FS03N機械臂運動學模型建立的正確性，開發了基于Matlab的機械臂運動仿真平臺，圖8為軟件設計流程圖，圖9為構建的仿真平臺。本仿真平臺包含了正運動學與逆運動學驗證，并且可以實時地觀察到機械臂的運動情況。

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

4 結束語

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

參考文獻：

[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] 于艷秋， 廖啟征. 基于有理數運算的一般6R機器人位置逆解算法[J]. 機械工程學報， 2005， 41（3）：229-233.

[5] 劉松國， 朱世強， 王宣銀. 基于矩陣分解的一般6R機器人實時高精度逆運動學算法[J]. 機械工程學報， 2008， 44（11）：304-309.

[6] 劉華山， 朱世強， 吳劍波. 基于向量積的機器人實時性逆解算法[J]. 農業機械學報， 2009， 40（6）：212-216.

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

[8] 呂世增， 張大衛， 劉海年. 基于吳方法的6R機器人逆運動學旋量方程求解[J]. 機械工程學報， 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].系統仿真學報， 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基法的一般串聯6R機器人機構逆運動學分析[J]. 上海交通大學學報， 2004， 38（6）：853-856.

（責任編輯：李 麗）