于權(quán)偉，李 光，肖 帆，楊加超，謝楚政

（湖南工業(yè)大學(xué) 機(jī)械工程學(xué)院，湖南 株洲 412007）

1 研究背景

機(jī)器人運(yùn)動(dòng)學(xué)包括正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)。正運(yùn)動(dòng)學(xué)是通過(guò)在工作空間內(nèi)選擇一組關(guān)節(jié)角來(lái)獲得機(jī)器人末端執(zhí)行器的姿態(tài)，對(duì)應(yīng)于一個(gè)唯一解。逆運(yùn)動(dòng)學(xué)是給定目標(biāo)位姿求解對(duì)應(yīng)的關(guān)節(jié)角，其解有多個(gè)。

目前，運(yùn)動(dòng)學(xué)建模主要采用D-H 坐標(biāo)法[1]和螺旋法[2]。D-H 坐標(biāo)法雖然應(yīng)用更廣泛，但也存在一些不足之處，主要體現(xiàn)在用于標(biāo)定D-H 模型的運(yùn)動(dòng)學(xué)參數(shù)不連續(xù)，當(dāng)機(jī)器人相鄰關(guān)節(jié)軸平行或接近平行時(shí)，會(huì)產(chǎn)生奇異性；而且D-H 坐標(biāo)法需要為每個(gè)連桿建立局部坐標(biāo)系，導(dǎo)致坐標(biāo)系不靈活。而螺旋理論是應(yīng)用李群知識(shí)和旋量理論提出的一種依據(jù)旋量指數(shù)積公式的運(yùn)動(dòng)學(xué)模型，從而推導(dǎo)出機(jī)器人運(yùn)動(dòng)方程的完整幾何表示，簡(jiǎn)化機(jī)器人機(jī)構(gòu)的分析，并提供串聯(lián)機(jī)器人的機(jī)構(gòu)參數(shù)化表示方法；利用螺旋理論建立機(jī)器人正向運(yùn)動(dòng)學(xué)模型，僅需要建立基礎(chǔ)坐標(biāo)系和工具坐標(biāo)系，便能從整體上描述機(jī)器人的運(yùn)動(dòng)。

逆運(yùn)動(dòng)學(xué)是已知末端執(zhí)行器的位姿，求解相應(yīng)關(guān)節(jié)變量的過(guò)程[3]。逆運(yùn)動(dòng)學(xué)求解的實(shí)質(zhì)是完成機(jī)器人工作空間到關(guān)節(jié)空間的映射。逆運(yùn)動(dòng)學(xué)方程組具有高維、非線性的特點(diǎn)，求解復(fù)雜且不易求出。在逆運(yùn)動(dòng)學(xué)中，多采用解析法、幾何法、代數(shù)法和矩陣?yán)碚撓嘟Y(jié)合[4]來(lái)求解6R 機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題，從而進(jìn)一步求得逆運(yùn)動(dòng)學(xué)解。

文獻(xiàn)[5]中采用的封閉解法，在通用性上不如數(shù)值法，但其函數(shù)構(gòu)造簡(jiǎn)便、計(jì)算方便快捷。其對(duì)滿足pieper 準(zhǔn)則的機(jī)器人具有通用性，利用矩陣求逆的方法生成12 組非線性方程，并利用代數(shù)法求解該方程，獲得機(jī)器人各關(guān)節(jié)角度變量的8 組解。文獻(xiàn)[6]中利用齊次坐標(biāo)變換矩陣推導(dǎo)出逆運(yùn)動(dòng)學(xué)問(wèn)題的解析解，其方法在計(jì)算逆向運(yùn)動(dòng)學(xué)的過(guò)程中需要進(jìn)行多次矩陣變換，導(dǎo)致求逆解速率較低。文獻(xiàn)[7]中采用幾何、代數(shù)方法和子問(wèn)題進(jìn)行逆運(yùn)動(dòng)學(xué)求解，在Matlab中利用代數(shù)消元法求解關(guān)節(jié)4和5中的奇異點(diǎn)；當(dāng)θ5=90°時(shí)，導(dǎo)致θ4無(wú)法進(jìn)行求解。文獻(xiàn)[8]中針對(duì)后3 個(gè)關(guān)節(jié)軸線相交于一點(diǎn)的6R 工業(yè)機(jī)器人，采用臂腕分離的方法求其逆解，即將其分為兩個(gè)3R 部分，雖然該方法可以避免矩陣求逆的運(yùn)算，但在求逆解中：若末端位置的x、y都為0，則θ1的變化不影響腕點(diǎn)的位置；若連桿坐標(biāo)系原點(diǎn)連線O2O3與O3O4共線，則θ2的兩個(gè)解相等；若θ5為零，則關(guān)節(jié)4 和6 的轉(zhuǎn)軸共線，導(dǎo)致θ4和θ6無(wú)法進(jìn)行求解。文獻(xiàn)[9]中將PUMA-560 機(jī)器人逆運(yùn)動(dòng)學(xué)求解分為位置求解和姿態(tài)求解兩個(gè)過(guò)程，首先使用D-H 坐標(biāo)法進(jìn)行位置求解得到關(guān)節(jié)角θ1、θ2和θ3，然后使用單位四元數(shù)的方法求解出θ4～θ6，但求逆解過(guò)程中表達(dá)式繁瑣，導(dǎo)致運(yùn)算速率較低。文獻(xiàn)[10]中將機(jī)器人末端執(zhí)行器位姿0Th逆解問(wèn)題轉(zhuǎn)化為末端腕部點(diǎn)位姿0T6的逆解問(wèn)題，排除0Th中含常量d6的多項(xiàng)式，簡(jiǎn)化求運(yùn)動(dòng)學(xué)逆解復(fù)雜度。通過(guò)反變換法求6R 機(jī)器人逆解，但求解關(guān)節(jié)4 中存在奇異點(diǎn)；當(dāng)s5=0 時(shí)，導(dǎo)致θ4無(wú)法進(jìn)行求解。

針對(duì)上述問(wèn)題，在文獻(xiàn)[5-10]的基礎(chǔ)下，本文提出一種利用分離-重構(gòu)技術(shù)求后3 個(gè)連續(xù)關(guān)節(jié)軸線相交于一點(diǎn)的6R 機(jī)器人逆解新方法。首先，采用螺旋理論的POE（exponential of product）法對(duì)PUMA-560 機(jī)器人進(jìn)行正向運(yùn)動(dòng)學(xué)建模，為后續(xù)求逆向運(yùn)動(dòng)學(xué)解提供必要準(zhǔn)備；然后通過(guò)公式變形，推導(dǎo)出機(jī)器人可分離的結(jié)論，并詳細(xì)介紹分離點(diǎn)的選取與重連的幾何約束條件；其次，證明了以子機(jī)器人重新結(jié)合為一個(gè)完整機(jī)器人的幾何約束條件，推導(dǎo)出關(guān)節(jié)角的求解公式，并對(duì)每個(gè)關(guān)節(jié)角進(jìn)行求解；最后，以仿真實(shí)驗(yàn)結(jié)果證明了本文所提方法的有效性。

2 機(jī)器人結(jié)構(gòu)分離

在研究機(jī)器人運(yùn)動(dòng)學(xué)中，主要采用D-H 坐標(biāo)法建立正向運(yùn)動(dòng)學(xué)模型，然后通過(guò)矩陣逆乘推導(dǎo)出求逆解的方程。事實(shí)上，這其中已體現(xiàn)將機(jī)器人結(jié)構(gòu)分離的思想，但由于D-H 坐標(biāo)法不具備直觀的幾何意義，從而導(dǎo)致該思想沒(méi)被提煉出來(lái)。雖然文獻(xiàn)[11]已經(jīng)提出將6R 機(jī)器人分離為兩個(gè)3R 子機(jī)器人的方法，但并未從理論上對(duì)其進(jìn)行推廣。腕部關(guān)節(jié)結(jié)構(gòu)簡(jiǎn)圖如圖1 所示。

圖1 腕部關(guān)節(jié)結(jié)構(gòu)簡(jiǎn)圖Fig.1 Schematic diagram of wrist joints

從圖1 可以看出，6R 機(jī)器人腕部關(guān)節(jié)結(jié)構(gòu)中，后3 個(gè)關(guān)節(jié)為軸線交于一點(diǎn)。本文使用幾何意義更明確的螺旋理論，首先證明n自由度機(jī)器人的可分離性，再詳細(xì)證明如何分離最后3 個(gè)關(guān)節(jié)為軸線交于一點(diǎn)的6R 機(jī)器人。

2.1 旋量和剛體運(yùn)動(dòng)

根據(jù)旋量理論，剛體空間運(yùn)動(dòng)可描述為由繞某一軸的旋轉(zhuǎn)和平移復(fù)合而成。設(shè)表示剛體旋轉(zhuǎn)軸的方向，r為軸上的一點(diǎn)，則剛體運(yùn)動(dòng)旋量可用ξ=[ω,ν]T表示，其中ν=q×ω，ξ同樣可以被稱為剛體的螺旋軸，則w的反對(duì)稱矩陣為

以反對(duì)稱矩陣為推廣，可將運(yùn)動(dòng)旋量ξ的矩陣形式記為

剛體運(yùn)動(dòng)指數(shù)坐標(biāo)如下：設(shè)ω(ω∈R3)表示旋轉(zhuǎn)軸方向的單位矢量，θ(θ∈R)為轉(zhuǎn)角。對(duì)于剛體每一個(gè)旋轉(zhuǎn)運(yùn)動(dòng)，都有一個(gè)旋轉(zhuǎn)矩陣R(R∈SO(3))與之對(duì)應(yīng)，具體關(guān)系由Rodrigues 公式[12]給出：

式中||ω||=1。

所以一般剛體運(yùn)動(dòng)指數(shù)坐標(biāo)可表示為

2.2 串聯(lián)機(jī)器人正向運(yùn)動(dòng)學(xué)的指數(shù)積公式

各關(guān)節(jié)運(yùn)動(dòng)由相關(guān)聯(lián)關(guān)節(jié)軸線的運(yùn)動(dòng)旋量產(chǎn)生，如果用ξ表示該關(guān)節(jié)軸線的單位運(yùn)動(dòng)旋量坐標(biāo)，則沿此軸線的剛體運(yùn)動(dòng)可表示為

定義機(jī)器人的初始位形為機(jī)器人當(dāng)θ=0 時(shí)的位形，并用表示機(jī)器人位于初始位形時(shí)的慣性坐標(biāo)系與工具坐標(biāo)系間的剛體變換。對(duì)于每個(gè)關(guān)節(jié)都可構(gòu)造一個(gè)單位運(yùn)動(dòng)旋量，這時(shí)除第i個(gè)關(guān)節(jié)的其他關(guān)節(jié)均固定于初始位形（θj=0），結(jié)合公式，可得到機(jī)器人正向運(yùn)動(dòng)學(xué)的指數(shù)積公式，為

2.3 n 自由度機(jī)器人可分離證明

式（5）可變形為

QL表示前i個(gè)關(guān)節(jié)的正向運(yùn)動(dòng)學(xué)，QR表示后n-i個(gè)關(guān)節(jié)的正向運(yùn)動(dòng)學(xué)。式中g(shù)i(0)是一個(gè)可逆的位姿矩陣，其原點(diǎn)位于第i+1 個(gè)關(guān)節(jié)軸軸線的原點(diǎn)，姿態(tài)與慣性坐標(biāo)系一致，其中，第i+1 個(gè)關(guān)節(jié)軸線的原點(diǎn)，定義為關(guān)節(jié)i與關(guān)節(jié)i+1 兩軸軸線的公垂線與關(guān)節(jié)i+1 軸線的交點(diǎn)。下文中所說(shuō)原點(diǎn)均以此定義。圖2 為子機(jī)器人R 基座標(biāo)系未變換的情況。

圖2 子機(jī)器人R 基座標(biāo)系未變換的情況Fig.2 R-based coordinate system of sub-robots without transformation

從圖2 可以看出，QL表示一個(gè)i自由度機(jī)器人正向運(yùn)動(dòng)學(xué)，其基座標(biāo)系與原n自由度機(jī)器人的慣性坐標(biāo)系{SL}重合，其工具坐標(biāo)系{Ti}則為gi(0)。QR則表示一個(gè)(n-i)自由度機(jī)器人正向運(yùn)動(dòng)學(xué)，其基坐標(biāo)系{SR}位于關(guān)節(jié)n的原點(diǎn)，姿態(tài)與{SL}一致，其工具坐標(biāo)系也為{Ti}，該機(jī)器人各關(guān)節(jié)的旋轉(zhuǎn)方向?yàn)樨?fù)方向。式（5）右端整體可理解為QR所表示機(jī)器人的基座標(biāo)系作變換后所得結(jié)果，如圖3 所示。

圖3 子機(jī)器人變換后的情況Fig.3 Sub-robots under the transformation

綜上所述，n自由度機(jī)器人可分離為兩個(gè)低自由度的子機(jī)器人，上述理論同樣適用含移動(dòng)副的機(jī)器人。

即對(duì)n自由度機(jī)器人逆運(yùn)動(dòng)學(xué)求解，可理解為尋找使得兩個(gè)子機(jī)器人重新結(jié)合為n自由度機(jī)器人的關(guān)節(jié)組合，可以重新結(jié)合的幾何約束條件為兩子機(jī)器人的工具坐標(biāo)系位姿重合。該約束條件也是機(jī)器人分離的通用準(zhǔn)則，尤其適用于分離點(diǎn)處的相鄰關(guān)節(jié)軸軸線為共線、平行或垂直關(guān)系的機(jī)器人。

3 6R 機(jī)器人的結(jié)構(gòu)分離

本節(jié)將以PUMA-560 為例，介紹6R 機(jī)器人的具體分離方法，為后面章節(jié)的逆運(yùn)動(dòng)學(xué)求解奠定基礎(chǔ)。為方便描述，下文將關(guān)節(jié)1 至分離點(diǎn)組成的部分記為子機(jī)器人L，將分離點(diǎn)至關(guān)節(jié)6 組成的部分記為子機(jī)器人R。

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

圖4 描述了PUMA-560 機(jī)器人在初始時(shí)各關(guān)節(jié)的螺旋軸。

圖4 PUMA-560 機(jī)器人的結(jié)構(gòu)簡(jiǎn)圖Fig.4 Structure schematic diagram of PUMA-560 robots

選擇{S}作為慣性坐標(biāo)系，{T}作為工具坐標(biāo)系，{T}的原點(diǎn)與關(guān)節(jié)6 的原點(diǎn)重合。則機(jī)器人的初始位形為

各關(guān)節(jié)螺旋軸如表1 所示。

表1 PUMA-560 機(jī)器人的各關(guān)節(jié)螺旋軸Table 1 Spiral axes of each joint of PUMA-560 robots

PUMA-560 機(jī)器人的正向運(yùn)動(dòng)學(xué)公式為：

3.2 結(jié)構(gòu)分離

已有文獻(xiàn)多采用臂腕分離法[13]求6R 機(jī)器人的逆運(yùn)動(dòng)學(xué)解，即分為兩個(gè)3R 部分，但在求逆解中θ5不能為0，否則會(huì)使關(guān)節(jié)4 和6 轉(zhuǎn)軸共線，導(dǎo)致θ4和θ6無(wú)法進(jìn)行求解。為避免奇異點(diǎn)，本文分離點(diǎn)選取關(guān)節(jié)4 的原點(diǎn)，將其分為4R 子機(jī)器人L 和2R 子機(jī)器人R。

由圖3 可知，機(jī)器人關(guān)節(jié)1 至關(guān)節(jié)4 組成部分與3R 仿人臂結(jié)構(gòu)類(lèi)似，該類(lèi)型機(jī)器人逆運(yùn)動(dòng)學(xué)已有完備的解法[14]。

由于此分離點(diǎn)相鄰關(guān)節(jié)不滿足圖1 所示，因此子機(jī)器人末端工具坐標(biāo)系需重新設(shè)定。圖5 中原6 自由度機(jī)器人的工具坐標(biāo)系變?yōu)樽訖C(jī)器人R的基座標(biāo)系，關(guān)節(jié)5 與關(guān)節(jié)6 軸線的單位向量不變，但旋轉(zhuǎn)變?yōu)樨?fù)方向，此時(shí)子機(jī)器人R 的基座標(biāo)系還未變換。

圖5 工具坐標(biāo)系間的幾何關(guān)系Fig.5 Geometric relations among tool coordinate systems

{TL}與{TR}分別表示兩個(gè)子機(jī)器人的工具坐標(biāo)系，二者的原點(diǎn)事實(shí)上是重合的，為便于觀察，將{TL}向右進(jìn)行平移。圖中yL和yR與關(guān)節(jié)4 軸線重合，其方向不受θ4影響，xL和xR與關(guān)節(jié)5 軸線重合，其方向不受θ5影響；并且xL與yR共面且垂直，重合。{TR}和{TL}兩個(gè)子機(jī)器人的工具坐標(biāo)系重合，即TR=TL。故兩個(gè)子機(jī)器人正向運(yùn)動(dòng)學(xué)可表示如下：

式（9）～（10）中：

將表1 中螺旋軸參數(shù)代入式（1）（2）（3）（9）（10），可得：

此時(shí)，兩個(gè)子機(jī)器人可重新結(jié)合為后三關(guān)節(jié)軸線交于一點(diǎn)的6R 機(jī)器人的幾何約束條件，變?yōu)閧TL}與{TR}重合，xL與yR垂直，

4 求逆運(yùn)動(dòng)學(xué)解

4.1 θ1、θ2、θ3 和θ6 的求解

子機(jī)器人L 和子機(jī)器人R 可重構(gòu)為6 自由度機(jī)器人應(yīng)滿足的第一個(gè)條件，是末端位置重合，故可令pL=pR，組成方程組：

式（13） 中：sij、cij、si、ci分 別 表 示sin(θi+θj)、cos(θi+θj)、sinθi、cosθi；i，j為關(guān)節(jié)的序號(hào)；下文與此相同。

首先求式（13）的平方和，得：

式中：K1=±1；

再將式（13）先平方后相加，得：

由上式得：

式中K2=±1。

由式（18）可得，當(dāng)C3≥0 時(shí)θ3才有解。

確定θ3后，可通過(guò)計(jì)算式（13）（14）組成方程組，得到θ2：

θ1可通過(guò)式（13）組成的方程組計(jì)算得出：

根據(jù)K1和K2的符號(hào)可知，θ?1、θ2和θ3可得到4組關(guān)于θ6的方程式。子機(jī)器人L 和子機(jī)器人R 可重構(gòu)為6 自由度機(jī)器人應(yīng)滿足的第二個(gè)條件是軸線yL與軸線xR的夾角保持垂直，即|xR|=|yL|=1，可得：

化簡(jiǎn)可得：

式中K3=±1。

由于向量yL與θ1、θ2和θ3有關(guān)，而xR只與θ6有關(guān)，故式（27）是一個(gè)僅與θ6有關(guān)的等式，根據(jù)θ1、θ2和θ3求解公式，可知θ6有8 個(gè)不同解。

4.2 θ4 和θ5 的求解

從圖4 可以看出，一旦θ1、θ2、θ3和θ6確定后，即前面兩個(gè)約束條件滿足后，只要分別旋轉(zhuǎn)θ4和θ5必然可以滿足=，但是直接用該約束條件求θ4和θ5并不方便。

觀察圖4 可知，由于θ1、θ2、θ3和θ6確定后，向量yL與xR將保持不變，而yL與yR之間應(yīng)該滿足＜yL，yR＞=0，且此時(shí)向量yL與yR只有θ5一個(gè)未知量，故可由yLyR=1 求得θ5：

式（31）中根號(hào)部分事實(shí)上為0，θ5只有唯一解。以下給出證明。各軸的位置關(guān)系如圖6 所示。

圖6 各軸的位置關(guān)系Fig.6 Position relation of each axis

如圖6 所示，θ5在旋轉(zhuǎn)過(guò)程中，只有使得向量yR始終位于xR和yL所確定的平面內(nèi)時(shí)才能滿足約束條件。因此θ5在最小正周期內(nèi)只有唯一解，式（31）可改寫(xiě)為

θ5求解后，再求θ4，使得＜xL,xR＞=0，同理可以求得θ4只有唯一解：

式（34）可改寫(xiě)為

5 實(shí)驗(yàn)和驗(yàn)證

計(jì)算PUMA-560 機(jī)器人的逆運(yùn)動(dòng)學(xué)解，以驗(yàn)證算法的正確性。PUMA-560 機(jī)器人的結(jié)構(gòu)參數(shù)如表2所示。

表2 PUMA-560 的D-H 參數(shù)Table 2 D-H parameters of PUMA-560

5.1 實(shí)驗(yàn)結(jié)果

在逆運(yùn)動(dòng)學(xué)算法的驗(yàn)證過(guò)程中，首先，在關(guān)節(jié)運(yùn)動(dòng)范圍內(nèi)給定一組初始的角度值；然后，根據(jù)POE公式計(jì)算目標(biāo)的位姿；接下來(lái)根據(jù)得到的目標(biāo)位姿和算法，確定每個(gè)關(guān)節(jié)的角度；在此之后，課題組再次使用獲得的角度集來(lái)求解目標(biāo)位姿，并且利用Matlab 對(duì)機(jī)器人的正、逆運(yùn)動(dòng)學(xué)進(jìn)行計(jì)算。具體的驗(yàn)證過(guò)程如下：

1）給定目標(biāo)位姿，PUMA-560 的桿長(zhǎng)、角度值、弧度值。

桿長(zhǎng)：d1=149.09 mm，d2=431.8 mm，d3=433.07 mm，d4=20.32 mm。

角度值：θ1=60°，θ2=50°，θ3=50°，θ4=60°，θ5=40°，θ6=-40°。

弧度值：θ1=1.047 19，θ2=0.872 78，θ3=0.872 66，θ4=1.047 19，θ5=0.698 13，θ6=-0.698 13。

2）求解正向運(yùn)動(dòng)學(xué)公式，得到目標(biāo)位姿。

3）根據(jù)所提出的目標(biāo)位姿和算法，求出所有逆運(yùn)動(dòng)學(xué)解，如表3 所示。

表3 目標(biāo)位姿對(duì)應(yīng)的逆解Table 3 Inverse solutions target pose correspondence

4）再次求正向運(yùn)動(dòng)學(xué)解，得到8 解的對(duì)應(yīng)位姿。

5） 令En為目標(biāo)位姿與實(shí)際位姿之間的誤差，為第1 組的實(shí)際位姿， 根據(jù)計(jì)算步驟（2）和步驟（4）的誤差，通過(guò)比較，可知第1 組解的誤差最大，第5 組解的誤差最小。

如式（37）和（38）所示，實(shí)際位姿與目標(biāo)位姿間最大誤差為10-13數(shù)量級(jí)，從而驗(yàn)證了算法的正確性和高精度性。

5.2 與D-H 坐標(biāo)法比較

采用文獻(xiàn)[6]中D-H坐標(biāo)法和本文提出的新算法，分別對(duì)關(guān)節(jié)角進(jìn)行1 000 次求逆運(yùn)算，每次求50 組逆解的平均值，等距抽取其中的10 組用來(lái)比較。算法的計(jì)算軟件為MatlabR2017b，PC 配置如下：處理器為Intel Core i7-8750H，CPU 速度為2.20 GHz，內(nèi)存為8.00 GB。D-H 法與新算法求逆解耗時(shí)的比較如圖7 所示。

從圖7 中可看出，新算法較D-H 法在求逆運(yùn)動(dòng)學(xué)解的過(guò)程中速度有顯著提高。D-H 法和新算法一次平均耗時(shí)分別為67.87,59.65 ms，新算法較D-H 法計(jì)算速度提高了12.1%。

圖7 D-H 法和新算法求逆解耗時(shí)比較Fig.7 Time consumption comparison between D-H method and the proposed algorithm

5.3 奇異性分析

文獻(xiàn)[9]中針對(duì)后3 個(gè)關(guān)節(jié)軸線相交于一點(diǎn)的6R 機(jī)器人，利用D-H 法對(duì)PUMA-560 機(jī)器人進(jìn)行正向運(yùn)動(dòng)學(xué)建模，采用臂腕分離法來(lái)求其逆解，即分為兩個(gè)3R 部分，雖然該方法可避免矩陣求逆的運(yùn)算，但在求逆解過(guò)程中，若θ5=0，則關(guān)節(jié)4 和6 轉(zhuǎn)軸共線，導(dǎo)致θ4和θ6無(wú)法進(jìn)行求解。利用本文的新算法，對(duì)上述情況進(jìn)行分析：

1）給定位姿角度值：θ1=60°，θ2=50°，θ3=50°，θ4=60°，θ5=0°，θ6=-40°，求正向運(yùn)動(dòng)學(xué)公式，得到目標(biāo)位姿：

2）根據(jù)新算法和目標(biāo)位姿，求出所有可能的關(guān)節(jié)角度。此時(shí)θ4、θ5、θ6會(huì)受到影響，而θ1、θ2、θ3不會(huì)受到任何影響，故可不詳細(xì)列出。8 組θ4、θ5、θ6的逆運(yùn)動(dòng)學(xué)解如表4 所示。

表4 8 組θ4、θ5、θ6 的逆運(yùn)動(dòng)學(xué)解Table 4 Solutions of eight groups of θ4,θ5 and θ6 inverse kinematics

由表4 可知，當(dāng)θ5=0 時(shí)，利用本文的新算法可避免奇異點(diǎn)的影響，并且可求得8 組θ4、θ5、θ6的解。

5.4 結(jié)果討論

在實(shí)驗(yàn)中確定了一組封閉解，即8 組解。因?yàn)镻UMA-560 為后3 個(gè)關(guān)節(jié)為軸線交于一點(diǎn)的機(jī)器人，由新算法求得其逆運(yùn)動(dòng)學(xué)解如表3 所示，可知第5 組的誤差最小。為驗(yàn)證新算法的正確性，將求得的8 組解與給定目標(biāo)位姿比較，發(fā)現(xiàn)第1 組解與其誤差最大，且實(shí)際位姿與目標(biāo)位姿誤差為10-13數(shù)量級(jí),驗(yàn)證了算法的正確性和高精度性。

與文獻(xiàn)[5]中封閉解法相比，新算法更具有理論推廣性，同時(shí)也推導(dǎo)出其他關(guān)節(jié)角的具體求解方法。文獻(xiàn)[6]中采用齊次坐標(biāo)變換矩陣推導(dǎo)出逆運(yùn)動(dòng)學(xué)問(wèn)題的解析解，其方法在計(jì)算逆向運(yùn)動(dòng)學(xué)的過(guò)程中需進(jìn)行多次矩陣變換，導(dǎo)致運(yùn)算速率較低。利用本文的新算法在求其逆解的過(guò)程，不僅避免奇異點(diǎn)，而且速度也有明顯提高。與文獻(xiàn)[9]的分離點(diǎn)相比，本文可避免奇異點(diǎn)，且計(jì)算過(guò)程中詳細(xì)介紹了可分離條件，并用數(shù)學(xué)表達(dá)式證明了機(jī)器人的可分離性。

6 結(jié)論

本文利用螺旋理論建立了正向運(yùn)動(dòng)學(xué)模型，并且通過(guò)對(duì)機(jī)器人正向運(yùn)動(dòng)學(xué)指數(shù)積表達(dá)式變形，證明了任意機(jī)器人可分離為兩個(gè)低自由度子機(jī)器人的可行性。提出后3 個(gè)關(guān)節(jié)軸線交于一點(diǎn)的6R 機(jī)器人分離點(diǎn)的選取及子機(jī)器人工具坐標(biāo)系設(shè)定的方法：分離點(diǎn)應(yīng)選取在可使子機(jī)器人具有完備逆解方法的位置；各子機(jī)器人工具坐標(biāo)系的某一軸與對(duì)應(yīng)子機(jī)器人末端關(guān)節(jié)軸線共線。以子機(jī)器人重新結(jié)合為6R 機(jī)器人的幾何約束關(guān)系，推導(dǎo)出各個(gè)關(guān)節(jié)角求解公式，最后求得機(jī)器人逆運(yùn)動(dòng)學(xué)解；最終對(duì)PUMA-560 機(jī)器人進(jìn)行驗(yàn)證，且實(shí)際位姿與目標(biāo)位姿誤差為10-13數(shù)量級(jí)，并在以下方面具有創(chuàng)新性：

與D-H 法求逆解速度相比，本文提出的方法對(duì)機(jī)器人結(jié)構(gòu)重新解構(gòu)，降低求解復(fù)雜度且?guī)缀我饬x明顯，計(jì)算速度提高了12.1%；

本文選擇的分離位置較傳統(tǒng)臂腕分離法可避免奇異點(diǎn)的影響，解決了傳統(tǒng)方法的弊端。

此外，本文提出的算法可被應(yīng)用到其他類(lèi)似構(gòu)型的6R 串聯(lián)機(jī)器人及其擴(kuò)展構(gòu)型逆解問(wèn)題求解中，具有一定通用性。