基于分離-重構技術的6R機器人逆解新方法

于權偉，李 光，肖 帆，楊加超，謝楚政

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

1 研究背景

機器人運動學包括正運動學和逆運動學。正運動學是通過在工作空間內選擇一組關節(jié)角來獲得機器人末端執(zhí)行器的姿態(tài)，對應于一個唯一解。逆運動學是給定目標位姿求解對應的關節(jié)角，其解有多個。

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

逆運動學是已知末端執(zhí)行器的位姿，求解相應關節(jié)變量的過程[3]。逆運動學求解的實質是完成機器人工作空間到關節(jié)空間的映射。逆運動學方程組具有高維、非線性的特點，求解復雜且不易求出。在逆運動學中，多采用解析法、幾何法、代數(shù)法和矩陣理論相結合[4]來求解6R 機器人的逆運動學問題，從而進一步求得逆運動學解。

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

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

2 機器人結構分離

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

圖1 腕部關節(jié)結構簡圖Fig.1 Schematic diagram of wrist joints

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

2.1 旋量和剛體運動

根據旋量理論，剛體空間運動可描述為由繞某一軸的旋轉和平移復合而成。設表示剛體旋轉軸的方向，r為軸上的一點，則剛體運動旋量可用ξ=[ω,ν]T表示，其中ν=q×ω，ξ同樣可以被稱為剛體的螺旋軸，則w的反對稱矩陣為

以反對稱矩陣為推廣，可將運動旋量ξ的矩陣形式記為

剛體運動指數(shù)坐標如下：設ω(ω∈R3)表示旋轉軸方向的單位矢量，θ(θ∈R)為轉角。對于剛體每一個旋轉運動，都有一個旋轉矩陣R(R∈SO(3))與之對應，具體關系由Rodrigues 公式[12]給出：

式中||ω||=1。

所以一般剛體運動指數(shù)坐標可表示為

2.2 串聯(lián)機器人正向運動學的指數(shù)積公式

各關節(jié)運動由相關聯(lián)關節(jié)軸線的運動旋量產生，如果用ξ表示該關節(jié)軸線的單位運動旋量坐標，則沿此軸線的剛體運動可表示為

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

2.3 n 自由度機器人可分離證明

式（5）可變形為

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

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

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

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

綜上所述，n自由度機器人可分離為兩個低自由度的子機器人，上述理論同樣適用含移動副的機器人。

即對n自由度機器人逆運動學求解，可理解為尋找使得兩個子機器人重新結合為n自由度機器人的關節(jié)組合，可以重新結合的幾何約束條件為兩子機器人的工具坐標系位姿重合。該約束條件也是機器人分離的通用準則，尤其適用于分離點處的相鄰關節(jié)軸軸線為共線、平行或垂直關系的機器人。

3 6R 機器人的結構分離

本節(jié)將以PUMA-560 為例，介紹6R 機器人的具體分離方法，為后面章節(jié)的逆運動學求解奠定基礎。為方便描述，下文將關節(jié)1 至分離點組成的部分記為子機器人L，將分離點至關節(jié)6 組成的部分記為子機器人R。

3.1 正向運動學

圖4 描述了PUMA-560 機器人在初始時各關節(jié)的螺旋軸。

圖4 PUMA-560 機器人的結構簡圖Fig.4 Structure schematic diagram of PUMA-560 robots

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

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

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

PUMA-560 機器人的正向運動學公式為：

3.2 結構分離

已有文獻多采用臂腕分離法[13]求6R 機器人的逆運動學解，即分為兩個3R 部分，但在求逆解中θ5不能為0，否則會使關節(jié)4 和6 轉軸共線，導致θ4和θ6無法進行求解。為避免奇異點，本文分離點選取關節(jié)4 的原點，將其分為4R 子機器人L 和2R 子機器人R。

由圖3 可知，機器人關節(jié)1 至關節(jié)4 組成部分與3R 仿人臂結構類似，該類型機器人逆運動學已有完備的解法[14]。

由于此分離點相鄰關節(jié)不滿足圖1 所示，因此子機器人末端工具坐標系需重新設定。圖5 中原6 自由度機器人的工具坐標系變?yōu)樽訖C器人R的基座標系，關節(jié)5 與關節(jié)6 軸線的單位向量不變，但旋轉變?yōu)樨摲较颍藭r子機器人R 的基座標系還未變換。

圖5 工具坐標系間的幾何關系Fig.5 Geometric relations among tool coordinate systems

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

式（9）～（10）中：

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

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

4 求逆運動學解

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

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

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

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

式中：K1=±1；

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

由上式得：

式中K2=±1。

由式（18）可得，當C3≥0 時θ3才有解。

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

θ1可通過式（13）組成的方程組計算得出：

根據K1和K2的符號可知，θ?1、θ2和θ3可得到4組關于θ6的方程式。子機器人L 和子機器人R 可重構為6 自由度機器人應滿足的第二個條件是軸線yL與軸線xR的夾角保持垂直，即|xR|=|yL|=1，可得：

化簡可得：

式中K3=±1。

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

4.2 θ4 和θ5 的求解

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

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

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

圖6 各軸的位置關系Fig.6 Position relation of each axis

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

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

式（34）可改寫為

5 實驗和驗證

計算PUMA-560 機器人的逆運動學解，以驗證算法的正確性。PUMA-560 機器人的結構參數(shù)如表2所示。

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

5.1 實驗結果

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

1）給定目標位姿，PUMA-560 的桿長、角度值、弧度值。

桿長：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）求解正向運動學公式，得到目標位姿。

3）根據所提出的目標位姿和算法，求出所有逆運動學解，如表3 所示。

表3 目標位姿對應的逆解Table 3 Inverse solutions target pose correspondence

4）再次求正向運動學解，得到8 解的對應位姿。

5） 令En為目標位姿與實際位姿之間的誤差，為第1 組的實際位姿， 根據計算步驟（2）和步驟（4）的誤差，通過比較，可知第1 組解的誤差最大，第5 組解的誤差最小。

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

5.2 與D-H 坐標法比較

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

從圖7 中可看出，新算法較D-H 法在求逆運動學解的過程中速度有顯著提高。D-H 法和新算法一次平均耗時分別為67.87,59.65 ms，新算法較D-H 法計算速度提高了12.1%。

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

5.3 奇異性分析

文獻[9]中針對后3 個關節(jié)軸線相交于一點的6R 機器人，利用D-H 法對PUMA-560 機器人進行正向運動學建模，采用臂腕分離法來求其逆解，即分為兩個3R 部分，雖然該方法可避免矩陣求逆的運算，但在求逆解過程中，若θ5=0，則關節(jié)4 和6 轉軸共線，導致θ4和θ6無法進行求解。利用本文的新算法，對上述情況進行分析：

1）給定位姿角度值：θ1=60°，θ2=50°，θ3=50°，θ4=60°，θ5=0°，θ6=-40°，求正向運動學公式，得到目標位姿：

2）根據新算法和目標位姿，求出所有可能的關節(jié)角度。此時θ4、θ5、θ6會受到影響，而θ1、θ2、θ3不會受到任何影響，故可不詳細列出。8 組θ4、θ5、θ6的逆運動學解如表4 所示。

表4 8 組θ4、θ5、θ6 的逆運動學解Table 4 Solutions of eight groups of θ4,θ5 and θ6 inverse kinematics

由表4 可知，當θ5=0 時，利用本文的新算法可避免奇異點的影響，并且可求得8 組θ4、θ5、θ6的解。

5.4 結果討論

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

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

6 結論

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

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

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

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