輕型仿生機器手的幾何求逆優化及軌跡規劃

張明松，黃滔

（1. 三峽大學 機械與動力學院，湖北 宜昌 443002；2. 水電機械設備設計與維護湖北省重點實驗室，湖北 宜昌 443002）

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

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

1 正運動學分析

1.1 連桿坐標系的建立及D-H參數的構建

連桿坐標系的建立是為了研究機器人連桿之間的關系。本文中所研究對象是六自由度冗余機械手臂，因此對應著會有六個關節軸，首先需要確定的是各個相鄰的軸線，設關節軸為i（i＝1～6）軸，其相鄰關節軸為i＋1軸，具體的坐標系建立步驟如下：

步驟1：確定Zi軸，根據上述所確定的關節軸，可知相鄰的兩個關節軸分別為i軸和i＋1軸，最終根據兩個相鄰軸的關系來確定Zi軸，沿關節軸i的指向為Zi軸。

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

步驟3：確定六自由度機械臂的Xi軸線。根據步驟1所確定的Zi軸來確定Xi軸，規定Xi軸垂直Zi軸，Xi軸的方向指向Zi＋1。確定Xi軸時也會出現關節軸i和i＋1相交的情況，這種情況下就認為Xi軸與關節軸i和i＋1所在的平面垂直。

步驟4：確定Yi軸。根據步驟1～3可以知道機械臂空間坐標系中的原點、Xi軸、Zi軸，進而可以采用右手定來找出Yi軸。

根據KINOVA jaco2 6DOF-S超輕量型機械臂的結構特點以及上述的坐標系確定方法建立如圖1所示的連桿坐標系。

圖1 機械臂連桿坐標系

構建D-H參數如表1所示。

表1 Jaco2 6DOF-S超輕量型機械臂D-H參數

1.2 正運動學計算

在定義機械臂的位姿時，其中坐標系{i}和坐標系{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參數表和齊次變換矩陣，將

參數表中數據代入式（1）可計算出各相鄰兩個坐標系之間的變換矩陣為：

將矩陣依次相乘可得正運動學公式為：

式中：

1.3 正運動學仿真

運用MATLABRobotics Toolbox工具箱中Link函數和SerialLink函數搭建六自由度機械臂模型，生成六自由度機械手臂可視化模型，使用teach 函數生成可調節關節角的人機交互界面，為了能夠方便對六自由度機械手臂的研究，筆者將機械手臂的初始姿態設定為各個關節角均為零度時的運動姿態。

如圖2所示為機械臂的初始姿態，圖2（a）為機械臂關節角控制操作界面，拖動界面上的滑塊就可以調節各個關節角度的大小，從而控制六自由度機械臂空間位置的姿態，實現手動控制機械手臂的末端執行器運動到所希望的位姿，圖2（b）為六自由度機械手臂的三維模型，此時圖中顯示的是六自由度機械手臂處于各個關節角為零度時的位姿。右側圖中的機械臂三維模型顯示的目的是為了保證手動設置各個關節角時能夠實時的觀察六自由度機械臂的位姿狀態，進而可以更加準確、快速地設定各個關節角度的大小。

圖2 機械臂初始姿態

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

2 幾何求逆優化算法的設計

所謂逆運動學其實就是正運動學的逆算過程，在機械手臂實際的工作運行中，往往都是給定機械手臂末端執行器的位置，再通過計算機的運算求解出每個關節角的角度大小，最后傳輸到各個關節角的控制器中，從而控制著機械手臂實際的運動狀態。因此機器臂逆運動學計算就是在已知機械手臂末端執行器的位姿后來進行逆推導過程，通常逆推導后會計算出很多關節角的數值，所以需要根據機械手臂實際的工作要求選取那些符合工作要求的關節角再進行之后的處理。

在逆推導的過程中有很多求解方法，本文采用的是幾何解法，將六自由度機械手臂的結構用實線和虛線進行表示，再通過幾何學進行求解，但本文中所采用的幾何法是基于普通幾何法的求解進行優化處理，目的是可以更快的求解結果。本文逆推導的過程為：

式中：R為末端執行器所在坐標系相對基坐標系的旋轉矩陣；P為笛卡爾空間中的位置矩陣[6]。

采用旋轉矩陣與位置矩陣分離法，使位置矩陣與幾何法結合來求解前三個關節角θ1、θ2、θ3隨后在此基礎上利用關節連桿4、5、6的旋轉矩陣來求解余下的關節角[7]。本文為了便于計算，基于θ2＝θ3的前提條件下進行逆運動學求解，具體如下：

（1）步驟1：求關節角θ1、θ2、θ3。

根據D-H坐標系和末端執行器位姿矩陣，可得機械臂末端坐標系原點的位置矩陣[Px Py Pz]T，將機械臂各關節連桿簡化為剛體，其幾何位置關系如圖3所示[8]。

圖3 機械臂連桿幾何位置關系

由于本文所研究的機械臂關節6的運動對關節5的位置不產生影響。因此可以計算出關節5相對于基坐標的位置矩陣，即：

可得：

由圖3可得機械臂末端執行器的位置關系函數表達式為：

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

將式（6）和式（8）聯立，平方后相加得：

（2）步驟2：求關節角θ4、θ5、θ6。

相鄰兩連桿間旋轉矩陣為：

由旋轉矩陣正交性質可得：

求解得：

當0＜θ6≤120°時：

當-120°≤θ6≤0時：

由式（15）、式（16）可知θ6有兩種解，產生的原因是由于各關節角位移求解結果相互關聯。六自由度機械手臂在實際運動過程中會有一部分的解表示的關節角位移是機械手臂到達不了的位置，因此最后所求逆解的選取要由機械手臂的實際工作情況而定[9]。

3 關節空間軌跡規劃

關節空間軌跡規劃是用來規劃機械手臂實際的運動軌跡，主要是利用每個關節的變量與時間之間的函數關系所形成的方程來表述機械手臂的運動軌跡。本文采用的是五次多項式插值算法，基于MatlabRoboticsToolbox平臺進行關節空間軌跡規劃，進而得到各關節角度、速度和加速度與時間關系曲線[10]。

五次多項式中包含6個待定系數，其中的任意系數都可對起始點和目標點的角度、角速度和角加速度進行同時約束，設六自由度機械手臂關節角符合：

將一小段軌跡的頭尾兩個點看作運動軌跡的起點θ0和終點θf，并將起始速度約束為V0，終止速度約束為Vf，如下所示：

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

就機械手臂的空間軌跡規劃問題，通過五次多項式的插值算法，計算出滿足多約束條件下六自由度機械手臂運動的多項式方程，并最終通過數學計算工具MATLAB進行運算和仿真得到相應的結果和仿真數據。對六自由度機械手臂進行五次多項式插值，不僅可以得到機械手臂末端執行器的空間軌跡，還可以得到每一個關節的角位移、角速度和角加速度[11]。

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

如圖5所示為機械手臂各個關節角位移曲線圖，從圖5中各關節隨時間變化的位移曲線圖可以看到圖中的關節角1、關節角2、關節角3、關節角5的位移曲線隨時間變化而變化的大致相同，只是零時刻弧度和達到穩定點時刻不同，而圖中的關節角4、關節角6位移顯示為零，產生這樣的原因是機械手臂的起始點和終止點的設定不同，因此每個關節對應的變化就有所不同。另外，通過對各個關節角的位移曲線圖的分析可以說明機械手臂是按照設定的路徑和程序進行運動的，進而證明的機械手臂計算和仿真的正確性。

圖6、圖7分別為六自由度機械手臂的關節角速度曲線圖和各關節角加速度曲線圖，圖中所顯示的均是各關節隨時間變化而變化的運動情況，關節角1、關節角2、關節角3、關節角5的角速度和角加速度隨時間變化的曲線大致相同，且角速度和角加速度均沒有突變，各關節角速度和角加速度變化的曲線是連續且緩和。通過圖4、圖5可知五次多項式機械臂軌跡規劃算法的有效性[11]，機械手臂的各關節運動平穩，可達到空間軌跡規劃設計要求。

圖4 機械臂末端執行器空間軌跡圖

圖5 各關節角位移曲線圖

圖6 各關節角速度曲線圖

圖7 各關節角加速度曲線圖

4 結論

針對輕型六自由度機械手，本文主要完成了對正逆運動學的推導計算，并就逆推導過程中提出一種結合位姿分離思想的幾何求逆優化算法。本文有如下結論：

（1）六自由度機械手臂的正運動學重點在于計算和推導，其求解的結果與逆運動求解是相互關聯，并起到驗證逆運動結果的作用。

（2）本文中六自由度機械手臂的逆運動學完成了各個關節角的求解，并根據本文所研究的機械手臂的結構要求對幾何求逆算法進行了優化處理，提出一種位姿分離的思想，可以更好地解決多自由度求逆困難的問題。

（3）結合D-H坐標系，本文建立了六自由度機械手臂的運動學模型，并推算出了六自由度機械手臂正逆運動學方程的數學函數表達式和各個關節運算求解的過程，然后，通過MATLAB機器人工具箱分別驗證和仿真了機械手臂的正、逆運動計算結果以及機械臂關節空間軌跡。本文完成了五次多項式的機械臂軌跡規劃并給出了各個關節角的角位移、角速度、角加速度的數據曲線。

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