關節角參數化結合接近矢量可行方向的五自由度機械臂逆運動學求解

萬珍平 羅釗 陸龍生 張端康 趙明華 呂曉能

（1. 華南理工大學 機械與汽車工程學院，廣東 廣州 510640；2. 廣東南牧機械設備有限公司，廣東 云浮 527300）

近年來，在中國制造向“中國智造”升級的過程中，機器人的應用領域不斷拓展［1-6］。對于清潔、噴涂、焊接等對末端執行器姿態無嚴格要求的場合，6 個及6 個以上自由度的機械臂并不總是任務所需，綜合考慮工作任務和整機性能，常采用五自由度的機械臂［7-8］。然而，五自由度的機械臂無法達到任意位姿，采用傳統的位姿描述方式進行逆運動學求解時，極易出現無解的情況［9］。

針對這一問題，通常采取兩種解決方法：一是減少運動學約束方程的數量［10］，但求解得到的關節值須滿足減少的運動學約束方程；二是增加虛擬關節［11］，即將缺省的關節值設置為常量，但這對機械臂的結構有特殊要求。因此，這兩種方法都有一定的局限性。為此，一些學者［12-13］要求用戶給出末端執行器的可到達位置和方向，然而，可到達的位置和方向并不容易找到。為解決此問題，Shimizu［14］引入單位無約束矢量對相鄰三軸相交的五自由度機械臂進行逆運動學求解，但無約束矢量的選取沒有結合模型做進一步的分析，存在盲目性。朱曉龍等［15］基于自由度約束描述末端位姿，通過幾何法對五自由度機械臂進行逆運動學求解，但沒有對接近矢量的范圍進行約束，接近矢量的選取亦存在盲目性。李萬莉等［16］結合工況特點，運用幾何法，通過先求解第5 關節的位置再求解前4 個關節的位置，來完成逆運動學求解，該方法應用場景非常特殊，不具有代表性。

有鑒于此，文中針對一種適用于清潔、噴涂、焊接等作業的五自由度機械臂，基于自由度約束的末端位姿描述，通過關節角參數化結合接近矢量可行方向進行逆運動學求解，并在笛卡爾空間進行路徑規劃仿真分析，驗證該方法的可行性和準確性。

1 機械臂建模及逆運動學分析

1.1 機械臂建模

文中以適用于噴涂、清潔、焊接等場合的五自由度機械臂為研究對象，其三維模型如圖1（a）所示。該機械臂由5 個關節組成，包含4 個旋轉關節、1 個移動關節。機械臂的整體結構較為特殊，但其運動模型具有一定的代表性。根據其臂型結構和幾何參數，使用D-H 參數描述法，由基坐標系開始依次建立固連于連桿的坐標系，如圖1（b）所示。

圖1 五自由度機械臂Fig.1 5-DOF manipulator

依據相鄰連桿的幾何參數確定D-H參數，如表1所示，其中a3為144 mm，d3初始長度為1 144 mm，關節3最大伸出長度為800 mm，d4為148 mm，d5為1 180 mm，關節1、關節4和關節5的取值范圍均為0~360°，關節2的取值范圍為-135°~-45°。

表1 五自由度機械臂的D-H參數Table 1 D-H parameters of 5-DOF manipulator model

結合D-H參數，相鄰連桿的坐標系的位姿變換矩陣為

式中，s代表正弦函數，c代表余弦函數。

在求解過程中，通常使用位姿矩陣來描述空間中剛體的位姿，但在實際使用中，輸入4×4的旋轉矩陣確定姿態極為不便，且不直觀，因此常采用歐拉角的姿態描述方式。歐拉角與姿態矩陣的變換關系為

式中，α、β、γ為Z-Y-Z歐拉角的參數。

坐標系｛5｝與工具坐標系｛T｝的變換矩陣為

式中，x0為坐標系｛5｝沿著它的x軸平移的距離，取80 mm。

將表1 中的D-H 參數代入連桿變換矩陣（式（1））中，以工具坐標系為研究對象，其位姿矩陣為已知量，由姿態矩陣和位置矢量組成（其中：n、o、a為單位向量，表示工具坐標系的姿態；px、py、pz表示工具坐標系的位置），將變換矩陣依次連乘，可以得到工具坐標系相對于基坐標系的變換矩陣如下：

式中，rij為機械臂末端執行器相對于基坐標系的第i個軸在第j個軸上的旋轉分量。

1.2 逆運動學分析

對式（4）中的10T21T32T43T54TT5T和旋轉矩陣乘以10T21T32T43T54TT5T的逆矩陣，由矩陣中對應元素的值相等得到下列方程：

由于目標位姿已知，根據上述方程可依次求出5個關節值，完成逆運動學求解。

然而，由于空間中剛體的運動描述需要6個自由度，對于五自由度機械臂，會出現給定的末端位姿不可達的情況。因此，如果采用傳統的位姿描述方式，將約束過多的自由度，在笛卡爾空間進行軌跡規劃時就極易出現逆運動學無解的情況。

2 基于自由度約束的逆運動學求解

為了避免過約束，描述末端位姿的變量數必須與機械臂的自由度數相等，因此只能采用5個變量描述五自由度機械臂末端執行器的位姿。首先，以工具坐標系原點在基坐標系中的位置作為目標位置，這樣便限制了3個移動的自由度。其次，拋棄以3個歐拉角描述工具坐標系姿態相對基坐標系姿態的方式，通過關節5的參數化和接近矢量的可行方向描述目標姿態。

2.1 關節角參數化

關節角參數化可根據工作任務靈活地自定義關節5的值，這不僅將末端執行器的運動空間由三維降為二維，同時可直接求得關節5的值，還可通過接近矢量Z5的可行方向完全約束末端執行器的姿態。通過分析機械臂模型的幾何關系和連桿變換矩陣，將關節5 的值代入式（6），可求得關節1 的值，而活動的第2、3、4關節值則可根據接近矢量Z5的可行方向確定。

2.2 接近矢量可行方向的求解

關節5 參數化后，機械臂的第2、3、4 關節的活動空間由三維空間變為平面，接近矢量Z5則活動于另一平面。為了求得接近矢量Z5的可行方向，可通過圖2（a）所示幾何投影關系，將工具坐標系投影至末端臂軸線所在的豎直平面。投影后的機械臂機構簡圖如圖2（b）所示，圖中T′為投影之后工具坐標系的原點（x，y，z），δ為OT′與Z0軸的夾角，d′5為投影之后末端臂的長度，ε為投影前的末端臂與投影后的虛末端臂的夾角，θ為虛末端臂與T′O所成的夾角，τ（τ=δ+θ）為接近矢量Z′5與Z0軸的夾角，r為T′到基坐標系原點O的距離，q2為關節3伸出臂的伸出長度，q1為基臂長度，a3為表1中的連桿長度。

圖2 機械臂幾何分析Fig.2 Geometric analysis of manipulator

由機械臂模型初始位置可得到夾角τ與關節2、關節4的值存在θ2+4=τ- 2π +ε的數學關系，因此，求出接近矢量Z5的可行方向，再從可行域中取值，然后通過式（9）—（11）求解d3，再通過式（12）—（14）求解θ2，最后通過式（15）求解θ4，即可完成逆運動學求解過程。

基于上述分析，給定目標位置T（工具坐標系原點），即px、py、pz已知，將關節5 參數化，自定義關節5的值，即θ5確定，則可由投影關系得到

τ的大小與工具坐標系｛T′｝的位置、伸出臂的運動范圍（q2∈ ［0，800］ mm）及各桿長度有關，其中只有T′的位置是未知的。當工具坐標系｛T′｝的位置由遠及近變化時，根據機械臂模型及各連桿參數，可按以下3 種情況討論T′在不同位置時τ的大小，完成接近矢量Z5的可行方向求解。

（1）當r>r3時，如圖3（a）所示，T′在離基坐標系原點較遠處（即遠端時），伸出臂可往上或往下自由伸縮，而不受基臂長度的影響，伸出臂伸到最長時，θ最大，即τ的取值范圍達到最大，此時τ∈ ［δ-θmax，δ+θmax］，文中僅取τ∈ ［δ，δ+θmax］。

（2）當r1

（3）當r

圖3 接近矢量的演變Fig.3 Evolution of proximity vectors

結合機械臂模型，由幾何關系可得

至此，完成了關節角參數化結合接近矢量可行方向的逆運動學求解。由式（5）和式（14）共得到4組解，根據運動連續性和各關節運動范圍從中篩選最優解。求解過程中輸入的位姿為（px，py，pz，θ5，λτ），其中，px、py、pz的單位為“mm”，θ5的單位為“°”，λτ為接近矢量Z5可行方向的比例系數，且

3 仿真驗證

為了驗證上述逆運動學求解方法的可行性和準確性，以工具坐標系原點為目標點，對直線、圓弧路徑進行跟蹤驗證。首先在Matlab中編寫程序進行運動規劃并對插補點進行逆運動學求解，再將求解得到的關節值代入正運動學模型中，最終通過圖像的方式對實際運動路徑與規劃路徑進行對比分析。初始條件設置如下：線段起始點A的位姿為（-700，500，-1 000，0.0，0.1），終止點B的位姿為（700，2 500，0，90.0，0.1）；圓弧起點C的位姿為（-500，2 050，-400，0.0，0.1），中間點D的位姿為（-100，2 100，-600，16.6，0.1），終止點E的位姿為（500，2 500，-1 000，90.0，0.1）。對機械臂運動路徑中的各位姿參數進行線性插值，再通過等距取路徑點進行位姿求解。由于位姿求解時得到4組解，因此需要通過運動連續性公式［17］結合各關節的運動范圍尋求最優解，即式（29）中Δ取最小值時對應的解即為最優解。

i= 1，2，3，4，ki為關節i的加權因子。由于工作半徑較大，因此，靠近原點的關節權重較大，對應的加權因子取值分別為1.0、1.0、1.0、0.5。路徑規劃過程中直線、圓弧的位置追蹤結果分別如圖4（a）和4（c）所示，相應的各關節值的變化分別如圖4（b）和4（d）所示。

圖4 路徑規劃結果Fig.4 Path planning results

從圖4（a）和4（c）可以看出，實際路徑與規劃路徑吻合，兩者之間幾乎沒有誤差，證明了該算法的可行性和準確性。由圖4（b）和4（d）可以發現，機械臂分別沿直線、圓弧運動時，除關節3的運動存在轉折外，其余關節運動平穩。關節3運動的轉折主要受機械臂臂型及逆運動學求解方法的影響。關節5 參數化后，關節3 的值由接近矢量Z5的方向決定，其中接近矢量可行方向的比例系數采用固定因子。通過接近矢量的方向變化結合圖3分析可知，當機械臂由近及遠運動時，關節3的值先減小再增大，其中關節3 運動的轉折點為圖3 遠端與中間端的臨界點。針對該轉折點，可對關節3的速度和加速度加以限制，以保障機械臂安全、可靠地執行作業任務。

4 結語

文中針對傳統位姿描述方法對五自由度機械臂過約束導致的逆運動學易出現無解的問題，以一種適用于清潔、噴涂、焊接等作業的五自由度機械臂為例，基于描述位姿的變量數與機械臂的自由度數相等，通過關節角參數化結合接近矢量可行方向確保了運動學逆解的存在性。文中方法計算復雜度低，求解過程較為簡便，路徑規劃仿真分析也證明了該方法的可行性和準確性。文中選取的五自由度機械臂模型具有一定的代表性，其求解思路可為欠自由度機械臂的逆運動學求解提供借鑒。