關(guān)節(jié)角參數(shù)化結(jié)合接近矢量可行方向的五自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解

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

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

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

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

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

1 機(jī)械臂建模及逆運(yùn)動(dòng)學(xué)分析

1.1 機(jī)械臂建模

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

圖1 五自由度機(jī)械臂Fig.1 5-DOF manipulator

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

表1 五自由度機(jī)械臂的D-H參數(shù)Table 1 D-H parameters of 5-DOF manipulator model

結(jié)合D-H參數(shù)，相鄰連桿的坐標(biāo)系的位姿變換矩陣為

式中，s代表正弦函數(shù)，c代表余弦函數(shù)。

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

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

坐標(biāo)系｛5｝與工具坐標(biāo)系｛T｝的變換矩陣為

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

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

式中，rij為機(jī)械臂末端執(zhí)行器相對(duì)于基坐標(biāo)系的第i個(gè)軸在第j個(gè)軸上的旋轉(zhuǎn)分量。

1.2 逆運(yùn)動(dòng)學(xué)分析

對(duì)式（4）中的10T21T32T43T54TT5T和旋轉(zhuǎn)矩陣乘以10T21T32T43T54TT5T的逆矩陣，由矩陣中對(duì)應(yīng)元素的值相等得到下列方程：

由于目標(biāo)位姿已知，根據(jù)上述方程可依次求出5個(gè)關(guān)節(jié)值，完成逆運(yùn)動(dòng)學(xué)求解。

然而，由于空間中剛體的運(yùn)動(dòng)描述需要6個(gè)自由度，對(duì)于五自由度機(jī)械臂，會(huì)出現(xiàn)給定的末端位姿不可達(dá)的情況。因此，如果采用傳統(tǒng)的位姿描述方式，將約束過多的自由度，在笛卡爾空間進(jìn)行軌跡規(guī)劃時(shí)就極易出現(xiàn)逆運(yùn)動(dòng)學(xué)無解的情況。

2 基于自由度約束的逆運(yùn)動(dòng)學(xué)求解

為了避免過約束，描述末端位姿的變量數(shù)必須與機(jī)械臂的自由度數(shù)相等，因此只能采用5個(gè)變量描述五自由度機(jī)械臂末端執(zhí)行器的位姿。首先，以工具坐標(biāo)系原點(diǎn)在基坐標(biāo)系中的位置作為目標(biāo)位置，這樣便限制了3個(gè)移動(dòng)的自由度。其次，拋棄以3個(gè)歐拉角描述工具坐標(biāo)系姿態(tài)相對(duì)基坐標(biāo)系姿態(tài)的方式，通過關(guān)節(jié)5的參數(shù)化和接近矢量的可行方向描述目標(biāo)姿態(tài)。

2.1 關(guān)節(jié)角參數(shù)化

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

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

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

圖2 機(jī)械臂幾何分析Fig.2 Geometric analysis of manipulator

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

基于上述分析，給定目標(biāo)位置T（工具坐標(biāo)系原點(diǎn)），即px、py、pz已知，將關(guān)節(jié)5 參數(shù)化，自定義關(guān)節(jié)5的值，即θ5確定，則可由投影關(guān)系得到

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

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

（2）當(dāng)r1

（3）當(dāng)r

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

結(jié)合機(jī)械臂模型，由幾何關(guān)系可得

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

3 仿真驗(yàn)證

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

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

圖4 路徑規(guī)劃結(jié)果Fig.4 Path planning results

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

4 結(jié)語

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