肖志鍵，吳建華

1 引言

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

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

2.1 運(yùn)動(dòng)學(xué)建模

首先根據(jù)機(jī)器人的三維模型，如圖1所示。結(jié)合其幾何參數(shù)，建立起機(jī)器人的D-H連桿坐標(biāo)系模型，如圖2所示。并列出了各個(gè)D-H參數(shù)，如表1所示。

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

圖2 機(jī)器人的D-H坐標(biāo)系圖Fig.2 D-H Coordinate of Robot

表 1 D-H參數(shù)表Tab.1 D-H Parameters

其中，機(jī)器人連桿間的齊次坐標(biāo)變換矩陣為：

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

可以得到機(jī)器人基座到末端坐標(biāo)系的位姿表述為：

2.2 逆解的解析表達(dá)式

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

用 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：

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

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

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

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

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

3 逆解分析與選取

以機(jī)器人在笛卡爾空間下的軌跡規(guī)劃過程為例—首先給定末端工具坐標(biāo)系在若干關(guān)鍵路點(diǎn)上的位姿，接著構(gòu)造出連續(xù)的路徑通過這些關(guān)鍵路點(diǎn)，再對(duì)該路徑進(jìn)行插補(bǔ)進(jìn)而求得N個(gè)插補(bǔ)點(diǎn)的位姿。最后，將這N個(gè)插補(bǔ)點(diǎn)通過逆解映射到機(jī)器人的關(guān)節(jié)空間并獲得N個(gè)6維關(guān)節(jié)角矢量。

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

針對(duì)上述問題，首先對(duì)8組逆解的數(shù)學(xué)特性進(jìn)行分析，為后續(xù)引入快速選取算法提供理論支撐。

3.1 最優(yōu)解的唯一性

雖然逆解有多組，但是根據(jù)“最小行程法”的準(zhǔn)則，通常只有一組最優(yōu)的解。事實(shí)上，可以得到如下推論：

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

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

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

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

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

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

進(jìn)一步地，考慮1，2組解的J2關(guān)節(jié)角，有如下關(guān)系：

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

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

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

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

3.2 最優(yōu)解的“連續(xù)性”

根據(jù)機(jī)器人運(yùn)動(dòng)的連續(xù)性，可以進(jìn)一步得到如下推論：

在機(jī)器人末端沿預(yù)定路徑連續(xù)運(yùn)動(dòng)的過程中（不考慮經(jīng)過奇異位形附近以及工作空間邊界附近的特殊情況），若在任一時(shí)刻下，機(jī)器人末端所在的插補(bǔ)點(diǎn)所對(duì)應(yīng)的逆解索引為i，則該段路徑中所有插補(bǔ)點(diǎn)均可由索引為i的逆解映射求得其對(duì)應(yīng)關(guān)節(jié)角矢量。

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

符合上述連續(xù)性條件的f函數(shù)，即是所要尋找的最優(yōu)逆解映射，否則求解出的q（t）在時(shí)間上不連續(xù)，將導(dǎo)致速度的跳變。

接著利用機(jī)器人的微分運(yùn)動(dòng)特性：

其中，d=dxi+dyi+dzk；δ=δxi+δyi+δzk 分別對(duì)應(yīng)繞基坐標(biāo)系 f軸的微分旋轉(zhuǎn)和微分運(yùn)動(dòng)矢量。因此有δt→0時(shí)d T=ΔT→0。

記fi對(duì)應(yīng)索引為i的逆解映射函數(shù)。根據(jù)第二小節(jié)的結(jié)論，可以看出fi由6組初等函數(shù)所組成。因此根據(jù)初等函數(shù)的連續(xù)性，有fi∈C，即ΔT→0時(shí)fi（0T6+ΔT）-fi（0T6）→0。

假設(shè)在路徑的起始點(diǎn)有 q（t0）=f1（0 T6（t0）），即起始點(diǎn)對(duì)應(yīng)的逆解索引為1，則由上述公式可知是滿足條件的逆解映射函數(shù)，所以該段連續(xù)路徑上的所有點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角均可由求解，即逆解索引可以保持不變。

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

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

3.3 逆解算法

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

START：

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

index=1//最優(yōu)逆解索引

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）//將關(guān)節(jié)角映射到［-360°，360°］

END

上述算法的優(yōu)勢在于，一旦知道了起始點(diǎn)位姿對(duì)應(yīng)的逆解索引i，則后續(xù)插補(bǔ)點(diǎn)的求取無需更換索引，從而將整個(gè)逆解選取過程的時(shí)間復(fù)雜度降為O（1）。

盡管該算法是從某一特定構(gòu)型的6軸機(jī)器人所推導(dǎo)出來的，但對(duì)于那些滿足“Pieper”準(zhǔn)則并且由此能夠推倒出解析解表達(dá)式的機(jī)器人，該算法都適用，只是具體的表達(dá)式和中間計(jì)算結(jié)果會(huì)有所不同。

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

4 仿真驗(yàn)證

基于.NET和Unity平臺(tái)搭建了機(jī)器人虛擬樣機(jī)，開發(fā)出了6軸機(jī)器人的仿真環(huán)境軟件，如圖5所示。軟件具有關(guān)節(jié)空間虛擬示教和笛卡爾空間下的虛擬示教功能。

圖5 仿真環(huán)境Fig.5 Environment for Simulation

基于3.3節(jié)所介紹的逆解選取算法，令機(jī)器人末端沿一段預(yù)先設(shè)定的路徑運(yùn)動(dòng)，利用求解出的關(guān)節(jié)角序列控制機(jī)器人進(jìn)行運(yùn)動(dòng)，得到運(yùn)動(dòng)路徑和機(jī)器人位姿，如圖6所示。

圖6 運(yùn)動(dòng)路徑和終點(diǎn)位姿Fig.6 Motion Path and Terminal Position

可以看到機(jī)器人順利地按照求解出的關(guān)節(jié)角序列沿著預(yù)定的連續(xù)路徑進(jìn)行運(yùn)動(dòng)，從而驗(yàn)證了算法的可行性。

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

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

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

5 結(jié)論

借助機(jī)器人逆解的解析解可以快速實(shí)現(xiàn)位姿矩陣到關(guān)節(jié)角的映射，但逆解的選取一直以來是解析解所要面臨的問題?；谝豢?軸機(jī)器人的逆解解析解表達(dá)式，分析了逆解的數(shù)學(xué)特性，并得到了兩個(gè)關(guān)鍵推論，從而利用推論設(shè)計(jì)出了一種較為高效的逆解選取算法。最終通過搭建虛擬仿真平臺(tái)，對(duì)逆解的選取算法進(jìn)行了驗(yàn)證和性能分析，同時(shí)該算法對(duì)其他類似構(gòu)型的機(jī)器人也具有適用性。