機器人逆運動學解析解的選取算法

肖志鍵，吳建華

1 引言

逆運動學求解問題在機器人的運動控制和軌跡規劃過程中扮演著重要角色，求解方法也不唯一。相比于數值解法，解析解的計算速度和精度有較大提升[1-3]，因此對于那些實時性要求較高且滿足“Pieper準則”[4]的機器人而言是較好的選擇。但是解析解只適用于特定構型的機器人，且需要面臨從多組逆解中選取出最合適的那一組解的問題。常見的選取方式是“最短行程”法，即通過與上一時刻的關節角進行比較選取出變化最小的那一組解作為最優解[5-6]。此外，也可以利用機器人的幾何姿態對逆解結果進行劃分從而方便選取[7]，進一步還可借助姿態判別因子進行選取[1，8]。通過在“最小行程法”的基礎上進行改進，還可以不需要求解出所有可能解，而是通過分支法相對較快地找到合適的那組解[9]。雖然這些方法能夠解決多解選取問題，但是選取的過程都需要引入額外的計算量，如果每次逆解計算都采用這些算法進行選取，則消耗的計算資源不容小覷。針對上述問題，首先選取了某種構型的6軸機器人，建立其運動學模型，得到了相應的8組解析解表達式。接著從機器人運動的連續性出發，分析了解析解的2個數學特性，在此基礎之上推導出一種快速逆解選取算法，進而簡化了多解選取過程。最后，借助Unity開發環境搭建了虛擬樣機仿真平臺，對快速選取算法進行了仿真和驗證。經過該算法優化的逆解選取過程在需要大量逆解計算情況下的時間復雜度降為O（1），且適用于類似構型的機器人。

2 運動學逆解

2.1 運動學建模

首先根據機器人的三維模型，如圖1所示。結合其幾何參數，建立起機器人的D-H連桿坐標系模型，如圖2所示。并列出了各個D-H參數，如表1所示。

圖1 6軸機器人的三維模型Fig.1 3D Model of 6-Axis Robot

圖2 機器人的D-H坐標系圖Fig.2 D-H Coordinate of Robot

表 1 D-H參數表Tab.1 D-H Parameters

其中，機器人連桿間的齊次坐標變換矩陣為：

其中，cθi=cos（θi）；cαi=cos（αi）；cθi=sin（θi）；sαi=sin（αi）。

可以得到機器人基座到末端坐標系的位姿表述為：

2.2 逆解的解析表達式

機器人的2、3、4軸相互平行，如圖2所示。因此其滿足Pieper準則，可以得到其逆解的解析表達式。根據：

用 LHSi，j，RHSi，j表示左右矩陣位置（i，j）上的元素，首先由LHS2，4=RHS2，4可得 θ1的兩組可能解：

其中，R2=para2x+para2y；parax=px-d6ax；paray=py-d6ay。

同樣由 LHS2，2=RHS2，2和 LHS2，1=RHS2，1可得 θ5，θ6：

同樣根據 LHS1，2=RHS1，2以及 LHS2，2=RHS2，2可得：

接著根據 LHS1，4=RHS1，4以及 LHS2，4=RHS2，4，可得：

其中，A=-2a2N；B=2a2N；C=M2+N2+a22-a23；

結合之前的求解結果，可得θ3，θ4。最終可得整個逆解的求取過程，如圖3所示（索引用于區分8組逆解）。

圖3 8組解析解表達式的求解過程Fig.3 Solution Procedure of 8 Group Analytical Solution Expressions

3 逆解分析與選取

以機器人在笛卡爾空間下的軌跡規劃過程為例—首先給定末端工具坐標系在若干關鍵路點上的位姿，接著構造出連續的路徑通過這些關鍵路點，再對該路徑進行插補進而求得N個插補點的位姿。最后，將這N個插補點通過逆解映射到機器人的關節空間并獲得N個6維關節角矢量。

由于逆解的計算次數與插補點數成正比，因此普通逆解選取算法的時間復雜度為O（N）。此外還需要將8組解映射到，進一步加大了計算量。

針對上述問題，首先對8組逆解的數學特性進行分析，為后續引入快速選取算法提供理論支撐。

3.1 最優解的唯一性

雖然逆解有多組，但是根據“最小行程法”的準則，通常只有一組最優的解。事實上，可以得到如下推論：

在機器人末端沿預定路徑連續運動的過程中（這里不考慮經過奇異位形附近以及工作空間邊界附近的特殊情況），在任一時刻下假設機器人的位姿矢量為 p0=［x0，y0，z0，r0，p0，y0］，其所對應的關節角矢量為 θ0=［θ1，θ2，θ3，θ4，θ5，θ6］，若下一時刻其所要經過的插補點的位姿為p1，則?δ＞0，使得8組解中最多只有1組解 θ1=［θ11，θ12，θ13，θ14，θ15，θ16］滿足下述“鄰近關系”：

上式意味著該組解的位姿矢量位于當前位姿矢量的鄰域內（后續都將式（6）稱為“鄰近關系”）。δ為一個小量，因此上式實際上就代表著“最小行程”。和一般的“最小行程法”的不同之處在于，假設已經知道了δ的值，則根據上述推論，只要找到一組滿足“鄰近條件”的解，則該解即是距離上一關節角矢量“最近”的那組最優解。

這里給出該推論的簡單證明過程和可行的δ值。假設8組解中第 j組解滿足（θ1i）j-θi＜δ，i∈［1，6］（不妨假設 j=1），若能找到 δ＞0，使得對于剩下的 j∈［2，8］，都不滿足（θ1i）j-θi＜δ，i∈［1，6］，即可驗證該推論。

首先，對于J1關節，通過計算可得式（2）中參數R是坐標系5的原點到基坐標系原點的連線在基坐標系XY平面上的投影距離。且R2max=（a22+a23+d25）-d24=109mm，此時機器人末端恰好位于工作空間的外邊界，且由式（2）可得：

根據已知條件 θ11-θ1≤δ，如果 θ12-θ1≤δ，則有 θ11-θ12≤2δ。因此，當取 0＜δ＜=8.18°時，有＞δ，即在該 δ值下第5-8組解都不滿足“鄰近關系”。

進一步考慮1-4組解的J5關節角，有 θ51-θ52=2θ51，且θ51=0時機器人位于奇異位形。由于路徑不經過奇異位形附近，這里令 θ51≥5°。此時只要取 0＜δ＜5°，則必有 θ52-θ5＞δ，因此第3-8組解都不滿足鄰近關系。

進一步地，考慮1，2組解的J2關節角，有如下關系：

事實上M，N分別是坐標系4和坐標系1的原點連線在坐標系1的X，Z平面上的投影距離，由圖2并結合幾何意義可得≤a2+a3=663，且上、下界分別對應機械臂2、3、4軸共面的兩種奇異位形。最后借助Matlab可以繪制出θ21-θ22的范圍，如圖4所示。

圖 4 θ21-θ22的范圍Fig.4 Range of θ21-θ22

當 θ21-θ22不位于上下限時，不妨取44≤≤662，此時 θ21-θ22≥10°。因此，當取 0＜δ＜5°時，2-8 組解均不滿足鄰近關系，1組解是唯一一組滿足條件的解。

綜上，任取0＜δ＜5°，推論均成立。事實上，利用推論可以直接簡化逆解選取過程。

3.2 最優解的“連續性”

根據機器人運動的連續性，可以進一步得到如下推論：

在機器人末端沿預定路徑連續運動的過程中（不考慮經過奇異位形附近以及工作空間邊界附近的特殊情況），若在任一時刻下，機器人末端所在的插補點所對應的逆解索引為i，則該段路徑中所有插補點均可由索引為i的逆解映射求得其對應關節角矢量。

這里給出簡單的推論證明過程：記連續運動過程中機器人各個關節組成的6維矢量為q（t）∈C。若已知在一段連續路徑中的末端位姿矩陣0T6（t），求其對應的逆解映射，等同于找到一個映射函數f（0 T6（t）），使得通過映射得到的q（t）=f（0 T6（t））至少滿足：

符合上述連續性條件的f函數，即是所要尋找的最優逆解映射，否則求解出的q（t）在時間上不連續，將導致速度的跳變。

接著利用機器人的微分運動特性：

其中，d=dxi+dyi+dzk；δ=δxi+δyi+δzk 分別對應繞基坐標系 f軸的微分旋轉和微分運動矢量。因此有δt→0時d T=ΔT→0。

記fi對應索引為i的逆解映射函數。根據第二小節的結論，可以看出fi由6組初等函數所組成。因此根據初等函數的連續性，有fi∈C，即ΔT→0時fi（0T6+ΔT）-fi（0T6）→0。

假設在路徑的起始點有 q（t0）=f1（0 T6（t0）），即起始點對應的逆解索引為1，則由上述公式可知是滿足條件的逆解映射函數，所以該段連續路徑上的所有點對應的關節角均可由求解，即逆解索引可以保持不變。

至此只證明了f1滿足逆解映射，但未證明f1是滿足逆解映射的唯一一種映射，即未必是“最優的”。事實上，根據3.1節的推論“k組解中最多只有1組解滿足鄰近關系”，該段連續路徑上的所有點對應的關節角只能由f1求解。倘若在tn時刻有q（tn）=fk（0T6（t0）），k≠1 成立，則根據 3.1 節推論，可得 q（tn）-q（tn+Δt）＞δ，ΔT→0，因此不滿足連續性條件。

因此f1是滿足逆解映射的唯一一個映射函數，證明了上述推論。

3.3 逆解算法

根據3.1與3.2節的推論，可以得到一種快速算法來代替一般的逆解選取算法：

START：

getanalytical fi（），i=1，2，…，8//獲得八組逆解映射函數

index=1//最優逆解索引

for（j=1；j＜9；++j）

if fj（0T6）==θ0then

index=j；break；

for（j=1；j＜=（numof rodapoints）；++j）

θj=findex（（0T6）j）

θj=Convert（θj）//將關節角映射到［-360°，360°］

END

上述算法的優勢在于，一旦知道了起始點位姿對應的逆解索引i，則后續插補點的求取無需更換索引，從而將整個逆解選取過程的時間復雜度降為O（1）。

盡管該算法是從某一特定構型的6軸機器人所推導出來的，但對于那些滿足“Pieper”準則并且由此能夠推倒出解析解表達式的機器人，該算法都適用，只是具體的表達式和中間計算結果會有所不同。

此外，盡管兩個推論均是以不經過奇異點附近和工作空間邊界附近為前提假設，而實際機器人完全有可能經過奇異點及工作空間附近，但這并不意味著把這些特殊情況納入考慮范圍后該算法就失效了。事實上，可以在該算法中加入奇異位形的判別以改進該算法，使該算法更具魯棒性。

4 仿真驗證

基于.NET和Unity平臺搭建了機器人虛擬樣機，開發出了6軸機器人的仿真環境軟件，如圖5所示。軟件具有關節空間虛擬示教和笛卡爾空間下的虛擬示教功能。

圖5 仿真環境Fig.5 Environment for Simulation

基于3.3節所介紹的逆解選取算法，令機器人末端沿一段預先設定的路徑運動，利用求解出的關節角序列控制機器人進行運動，得到運動路徑和機器人位姿，如圖6所示。

圖6 運動路徑和終點位姿Fig.6 Motion Path and Terminal Position

可以看到機器人順利地按照求解出的關節角序列沿著預定的連續路徑進行運動，從而驗證了算法的可行性。

圖7 算法時間性能分析Fig.7 Time Performance Analysis of Algorithms

進一步對其時間性能進行分析。首先選取一段實驗路徑，將其劃分為10000個路點，并記錄使用快速逆解算法前后所分別消耗的時間（實驗計算機為i5-4210UCPU芯片），共計進行20次實驗并獲得其平均值，如圖7所示。

根據實驗結果，在上述實驗中采用快速選取算法的逆解相比采用普通“最小行程”法（計算出全部8組解并選取最優解）的逆解其計算速度提升了4.6倍，節省了82%的逆解計算時間。

5 結論

借助機器人逆解的解析解可以快速實現位姿矩陣到關節角的映射，但逆解的選取一直以來是解析解所要面臨的問題。基于一款6軸機器人的逆解解析解表達式，分析了逆解的數學特性，并得到了兩個關鍵推論，從而利用推論設計出了一種較為高效的逆解選取算法。最終通過搭建虛擬仿真平臺，對逆解的選取算法進行了驗證和性能分析，同時該算法對其他類似構型的機器人也具有適用性。