Paden-Kahan第二子問(wèn)題擴(kuò)展問(wèn)題求機(jī)械臂逆運(yùn)動(dòng)學(xué)研究

張樹(shù)剛，呂北軒，李 勇，隋麗君

（呼倫貝爾職業(yè)技術(shù)學(xué)院機(jī)電工程系，內(nèi)蒙古 呼倫貝爾 021000）

1 國(guó)內(nèi)外相關(guān)研究概述

自2020年初新型冠狀病毒肺炎疫情爆發(fā)以來(lái)，在疫情的沖擊下，制造業(yè)企業(yè)的發(fā)展面臨著具備一定操作技能的操作者短缺的“用工荒”及在崗員工感染新冠病毒的雙重風(fēng)險(xiǎn)的掣肘。當(dāng)前，中國(guó)所取得的抗疫階段性勝利的經(jīng)驗(yàn)表明了，最有效地控制疫情的方法就是隔離以及保持社交距離?，F(xiàn)如今疫情等客觀(guān)條件勢(shì)必將倒逼制造業(yè)企業(yè)向數(shù)字化、智能化生產(chǎn)的轉(zhuǎn)型，推進(jìn)少人化生產(chǎn)和柔性制造，即使企業(yè)提升自身抗風(fēng)險(xiǎn)能力的同時(shí)，也有利于提高生產(chǎn)效率。在抗疫常態(tài)化、制造業(yè)少人化的背景下［1-2］，由一些系列關(guān)節(jié)串接而成的多關(guān)節(jié)工業(yè)機(jī)械臂，作為智能化生產(chǎn)的數(shù)字化車(chē)間設(shè)備拓?fù)鋵由系年P(guān)鍵物理組元［3-4］，顯然已經(jīng)成為了這場(chǎng)“減人不減效”的抗疫戰(zhàn)爭(zhēng)中的關(guān)鍵設(shè)備。運(yùn)動(dòng)學(xué)逆解是機(jī)械臂進(jìn)行工作軌跡規(guī)劃及優(yōu)化工作效率的理論前提［5］，高效率求運(yùn)動(dòng)學(xué)逆解的方法將顯著提高機(jī)械臂的工作效率，進(jìn)而有利于提升制造業(yè)企業(yè)復(fù)工復(fù)產(chǎn)的進(jìn)度。當(dāng)前學(xué)界業(yè)內(nèi)的相關(guān)研究主要集中在對(duì)機(jī)械臂逆解的具體的算法與存在性上。對(duì)于逆解的具體的運(yùn)算上，當(dāng)前主要采用兩種方法構(gòu)建機(jī)械臂運(yùn)動(dòng)學(xué)模型，即Denavit-Hartenberg 法與旋量理論法，并且大量的學(xué)者已進(jìn)行了相關(guān)的研究，并已做出大量的應(yīng)用。這里根據(jù)文獻(xiàn)［5-7］，總結(jié)出Denavit-Hartenberg法與螺旋理論的特性，具體如表1所示。

表1 Denavit-Hartenberg法與螺旋理論的特性Tab.1 Characteristics of Denavit-Hartenberg Methods and Screw Theory

由上表可知螺旋理論相比Denavit-Hartenberg法而言，其具備著較為明確的幾何意義與直觀(guān)性，并減少了繁瑣的坐標(biāo)系構(gòu)建與關(guān)節(jié)坐標(biāo)變換過(guò)程，求解的運(yùn)算效率較高。當(dāng)前，國(guó)內(nèi)外諸多學(xué)者已采用了旋量指數(shù)積（POE，Product of Exponentials）對(duì)串聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)進(jìn)行了大量的分析與研究。文獻(xiàn)［8］運(yùn)用旋量指數(shù)積，對(duì)數(shù)字化腦立體定向儀，這一串聯(lián)結(jié)構(gòu)構(gòu)建了運(yùn)動(dòng)學(xué)方程，并避免了各個(gè)構(gòu)建繁瑣的坐標(biāo)系構(gòu)建與坐標(biāo)變換。文獻(xiàn)［9］成功地運(yùn)用螺旋理論對(duì)安川MH6 型機(jī)械臂構(gòu)建了運(yùn)動(dòng)學(xué)模型，并用八組逆解驗(yàn)證了旋量理論逆運(yùn)動(dòng)學(xué)求解的有效性。文獻(xiàn)［10］針對(duì)后三關(guān)節(jié)軸線(xiàn)交于一點(diǎn)的6R串聯(lián)機(jī)械臂，將Paden-Kahan子問(wèn)題結(jié)合消元法，提出了一種逆解算法，并通過(guò)實(shí)例得到了驗(yàn)證。文獻(xiàn)［11］在旋量理論的基礎(chǔ)上結(jié)合數(shù)學(xué)吳方法對(duì)機(jī)器人逆解問(wèn)題進(jìn)行了改進(jìn)，實(shí)際表明該方法具備較強(qiáng)的準(zhǔn)確性與效率?？梢?jiàn)，旋量理論已在學(xué)界業(yè)內(nèi)得到了廣泛的應(yīng)用。但旋量理論的逆解亦具備一定局限性，如在文獻(xiàn)［12］中，指出該團(tuán)隊(duì)所研制的一種具備多軸線(xiàn)平行的新型機(jī)械臂難以運(yùn)用經(jīng)典的Paden-Kahan 子問(wèn)題直接進(jìn)行解決。簡(jiǎn)而言之，旋量理論機(jī)械臂逆解的基礎(chǔ)Paden-Kahan 子問(wèn)題，只能解決特定的自由度較低串聯(lián)機(jī)構(gòu)的逆解問(wèn)題［10］。

這里運(yùn)用旋量運(yùn)動(dòng)理論，將串聯(lián)機(jī)械臂各個(gè)運(yùn)動(dòng)副與連桿的運(yùn)動(dòng)，轉(zhuǎn)換為旋量指數(shù)積構(gòu)建出了串聯(lián)機(jī)械臂的正逆運(yùn)動(dòng)學(xué)。并在Paden-Kahan第二子問(wèn)題的基礎(chǔ)上，提出了其第二子問(wèn)題的一種新的擴(kuò)展問(wèn)題，實(shí)現(xiàn)了對(duì)其經(jīng)典問(wèn)題進(jìn)行補(bǔ)充。最后以ABB IRB2600型機(jī)器臂作為驗(yàn)證對(duì)象，通過(guò)第二子問(wèn)題的一種新的擴(kuò)展問(wèn)題對(duì)其進(jìn)行求逆運(yùn)算，在經(jīng)過(guò)對(duì)6組解驗(yàn)證后，驗(yàn)證了這里提出的Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題在串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方面應(yīng)用的有效性。

2 機(jī)械臂旋量系坐標(biāo)建立及其運(yùn)動(dòng)學(xué)構(gòu)建

設(shè)空間中的一物體發(fā)生旋轉(zhuǎn)后，其固聯(lián)的動(dòng)坐標(biāo)系相對(duì)慣性坐標(biāo)系所發(fā)生的旋轉(zhuǎn)可以用旋轉(zhuǎn)矩陣R進(jìn)行表征，且其為（3×3）特殊正交矩陣，即R∈SO（3）=｛R∈?3×3：RR T=E，detR=1｝，式中：E—單位矩陣。由Euler旋轉(zhuǎn)定理可知，對(duì)于任意位姿矩陣R∈SO（3），可等效于繞固定軸ω=｛ωx，ωy，ωz｝T∈R3旋轉(zhuǎn)一定角度θ，其中，||ω||=1，θ∈［0，2π）。進(jìn)而，物體在空間的旋轉(zhuǎn)可以用Rodrigues旋轉(zhuǎn)公式的旋量指數(shù)積形式，將R表征為ω與θ的函數(shù)：

在剛體旋轉(zhuǎn)運(yùn)動(dòng)的基礎(chǔ)上，進(jìn)一步考慮剛體一般運(yùn)動(dòng)。由Chasles定理可知，三維空間中任s意的剛體運(yùn)動(dòng)均可視作為該剛體繞某一軸線(xiàn)的旋轉(zhuǎn)與沿該軸的平移的合成的有限螺旋運(yùn)動(dòng)，具體表現(xiàn)［13］，如圖1所示。其中，A為慣性系，B為動(dòng)系統(tǒng)，而B(niǎo)的位形空間為R3與SO（3）的乘積空間，記為SE（3）=｛（p，R）：p∈R3，R∈SO（3）｝=R3×SO（3）。其中，p為A原點(diǎn)到B原點(diǎn)的位置矢量，B相對(duì)于A(yíng)的旋轉(zhuǎn)矩陣為R。旋量理論中運(yùn)用SE（3）中的一元素來(lái)表示某一特定的剛體運(yùn)動(dòng)，即有限螺旋運(yùn)動(dòng)。

圖1 一般剛體運(yùn)動(dòng)示意圖Fig.1 General Rigid Motion

廣義的有限螺旋運(yùn)動(dòng)在空間中的運(yùn)動(dòng)，如圖2所示。根據(jù)運(yùn)動(dòng)合成原理可以視作為，先由點(diǎn)1旋轉(zhuǎn)至點(diǎn)2，后由點(diǎn)2移動(dòng)到點(diǎn)3。即廣義的旋量運(yùn)動(dòng)為繞軸線(xiàn)ω所做的旋轉(zhuǎn)與沿軸線(xiàn)ω移動(dòng)的合成，其具體可用旋量坐標(biāo)ξ來(lái)表示旋量運(yùn)動(dòng)，即可由一對(duì)三維向量構(gòu)成的六維向量表示，具體為ξ=［ν，ω］T∈R6，其中，ν=-ω×q。其反對(duì)稱(chēng)陣旋量可表示為：

圖2 廣義有限螺旋運(yùn)動(dòng)示意圖Fig.2 Generalized Finite Screw Motion

并且為了衡量旋量運(yùn)動(dòng)的平移運(yùn)動(dòng)與轉(zhuǎn)動(dòng)的比值，引入了節(jié)距h，具體為：

且文獻(xiàn)［14］指出，運(yùn)動(dòng)旋量與θ乘積的矩陣指數(shù)可以表征任意剛體變換，即，具體可以表示為：

2.1 運(yùn)用旋量理論的機(jī)械臂正運(yùn)動(dòng)學(xué)表示

機(jī)構(gòu)的運(yùn)動(dòng)副所蘊(yùn)含的受約束的運(yùn)動(dòng)，是使機(jī)構(gòu)能夠形成特定運(yùn)動(dòng)的基本條件［15］。對(duì)于由n個(gè)R副構(gòu)成的開(kāi)式串聯(lián)機(jī)械臂，其是由剛性連桿間通過(guò)R副耦合串接而成，而每個(gè)R副又起到將相鄰的各個(gè)連桿之間的運(yùn)動(dòng)限制于SE（3）的子群之中。對(duì)于機(jī)械臂的每一關(guān)節(jié)臂在三維空間中的運(yùn)動(dòng)均可用特殊群SE（3）來(lái)表征。從本質(zhì)而言，機(jī)械臂的運(yùn)動(dòng)是由多個(gè)R副所在的關(guān)節(jié)軸線(xiàn)的運(yùn)動(dòng)旋量耦合而成。對(duì)于某一關(guān)節(jié)上的運(yùn)動(dòng)旋量，則沿此運(yùn)動(dòng)旋量軸線(xiàn)連桿的剛體運(yùn)動(dòng)可以表示為：

式中：gab（0）—運(yùn)動(dòng)過(guò)程中連桿的初始位形；

gab（Θ）—運(yùn)動(dòng)過(guò)程中連桿的最終位形；

Θ—關(guān)節(jié)空間，即：

文獻(xiàn)［14］指出，剛體空間運(yùn)動(dòng)SE（3）可以通過(guò)se（3）的指數(shù)變換來(lái)表征，即機(jī)械臂中的各個(gè)連桿之間的剛體變換可寫(xiě)作為某個(gè)運(yùn)動(dòng)旋量的指數(shù)形式，即gst：Q→SE（3）。因此由Euler 定理及Chasles定理可知，空間中機(jī)械臂的空間運(yùn)動(dòng)可以視作為為各個(gè)關(guān)節(jié)旋轉(zhuǎn)群的正則坐標(biāo)的耦合，具體的開(kāi)鏈機(jī)構(gòu)的總運(yùn)動(dòng)方程可以表示為：

上式即機(jī)械臂正運(yùn)動(dòng)學(xué)的指數(shù)積公式。

2.2 運(yùn)用旋量理論的機(jī)械臂逆運(yùn)動(dòng)學(xué)表示

2.2.1 Paden-Kahan子問(wèn)題的描述

在運(yùn)用旋量理論對(duì)機(jī)械臂的逆運(yùn)動(dòng)進(jìn)行求解時(shí)，常將運(yùn)動(dòng)旋量視作為幾何體，并在旋量指數(shù)積所構(gòu)建出的正解基礎(chǔ)上，運(yùn)用幾何方法將原先較為復(fù)雜的逆解問(wèn)題分解為多個(gè)連續(xù)的子問(wèn)題進(jìn)行求逆。學(xué)者Paden 與學(xué)者Kahan 一同總結(jié)出了三種典型的運(yùn)用幾何方法對(duì)運(yùn)動(dòng)旋量指數(shù)積求逆解的子問(wèn)題，而這些具備著明確幾何意義且可用旋量指數(shù)積能夠明確表示的求逆問(wèn)題統(tǒng)稱(chēng)為Paden-Kahan 子問(wèn)題。這里列出了兩位學(xué)者所總結(jié)出的這三種經(jīng)典的Paden-Kahan 子問(wèn)題的具體問(wèn)題描述及幾何表示［14］，如表2所示。

相關(guān)文獻(xiàn)表明，Paden-Kahan第二子問(wèn)題為解決以多R副構(gòu)成串聯(lián)機(jī)械臂的關(guān)鍵［10-12］。故此，對(duì)于子問(wèn)題1與子問(wèn)題3，對(duì)這里暫且不進(jìn)行詳細(xì)討論。

2.2.2 Paden-Kahan第二子問(wèn)題求解

Paden-Kahan第二子問(wèn)題，指對(duì)剛體繞兩個(gè)已確定了幾何關(guān)系的定軸依次進(jìn)行有序的旋轉(zhuǎn)后的運(yùn)動(dòng)進(jìn)行逆運(yùn)動(dòng)學(xué)求解，其具體的幾何描述，如表2所示。文獻(xiàn)［5］已給出了Paden-Kahan的第二經(jīng)典子問(wèn)題具體的解法。當(dāng)ω1×ω2≠o時(shí)，即兩定軸軸線(xiàn)不平行時(shí)，設(shè)有如下矢量：

表2 Paden-Kahan子問(wèn)題Tab.2 Paden-Kahan Subproblems

進(jìn)一步求得θ1與θ2有：

值得注意的是，當(dāng)表示旋量的螺旋的兩個(gè)圓之間有兩個(gè)交點(diǎn)時(shí)，式（11）有2組解；兩圓有一個(gè)交點(diǎn)時(shí)，式（11）有1組解；當(dāng)兩圓不相交時(shí)，無(wú)解。而當(dāng)ω1×ω2=o時(shí)，即兩軸線(xiàn)平行時(shí)，則有：

并設(shè)有：

則有：

2.2.3 Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題

Paden-Kahan第二子問(wèn)題解決了剛體繞兩個(gè)已確定了幾何關(guān)系的定軸進(jìn)行有序的旋轉(zhuǎn)的逆解。但對(duì)多R副構(gòu)成串聯(lián)機(jī)械臂進(jìn)行求逆時(shí)，即對(duì)多個(gè)連續(xù)的第二子問(wèn)題進(jìn)行求逆時(shí)，易因R副軸線(xiàn)的幾何關(guān)系不確定，即Paden-Kahan第二子問(wèn)題所繞定軸間的幾何關(guān)系不確定，而產(chǎn)生求逆解的困難。這里在第二子問(wèn)題的基礎(chǔ)上進(jìn)行了擴(kuò)展，旨在簡(jiǎn)化第二子問(wèn)題求機(jī)械臂逆解的過(guò)程。其問(wèn)題可描述為繞若干個(gè)無(wú)確定幾何關(guān)系的定軸依次進(jìn)行有序的旋轉(zhuǎn)的逆解問(wèn)題，其幾何描述，如圖3所示。

下面將對(duì)算例進(jìn)行求解說(shuō)明，算例具體情形，如圖3 所示。顯然，圖示問(wèn)題為Paden-Kahan 第二子問(wèn)題的衍化，其中ξ2與ξ3的軸線(xiàn)相互平行，即ω3×ω2=o；而ξ1則與其余兩旋量呈現(xiàn)非平行的任意角度，且此問(wèn)題可由下式進(jìn)行表征：

圖3 Paden-Kahan第二子問(wèn)題擴(kuò)展問(wèn)題圖示Fig.3 The Extension Problem of the Second Paden-Kahan Problem

q點(diǎn)在旋量ξ1上的投影s為：

式中：r—旋量ξ1軸線(xiàn)上的任意一點(diǎn)。

幾何上直觀(guān)，逆運(yùn)動(dòng)學(xué)是否有逆解取決于各個(gè)表征旋量的螺旋的圓之間是否有交點(diǎn)，而表征旋量的圓的半徑可由下式表征：

而是否有交點(diǎn)qc則可由幾何方法進(jìn)行確定，具體的判別式D為：

點(diǎn)qc的取值與D有關(guān)，即：

通過(guò)點(diǎn)qc的確定，即θ1可通過(guò)子問(wèn)題1進(jìn)行求解，即有如下：

θ2與θ3則可按第二子問(wèn)題兩旋轉(zhuǎn)軸平行進(jìn)行處理，即：

根據(jù)式（12）～式（14）進(jìn)行求解。特別注意的是，當(dāng)D＞0時(shí)，qc存在2個(gè)解，需分別代入上式進(jìn)行兩次求解，并根據(jù)具體的旋量大小進(jìn)行取舍。

3 ABB IRB2600型機(jī)器臂逆運(yùn)動(dòng)學(xué)構(gòu)建

為驗(yàn)證這里所提出的Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題求解的正確性，這里以ABB公司的IRB2600型機(jī)械臂作為驗(yàn)證對(duì)象，下面將以旋量指數(shù)積原理并結(jié)合Paden-Kahan第二子問(wèn)題及其擴(kuò)展問(wèn)題，對(duì)機(jī)械臂的正逆運(yùn)動(dòng)學(xué)進(jìn)行求解。IRB2600型機(jī)械臂為典型的6R機(jī)械臂，即可通過(guò)6個(gè)旋轉(zhuǎn)副（R副）實(shí)現(xiàn)空間運(yùn)動(dòng)。該型機(jī)械臂的拓?fù)浣Y(jié)構(gòu)可表示為SOC｛-R1⊥R2∥R3⊥R4⊥R5⊥R6-｝，其中⊥與∥分別表示2個(gè)旋轉(zhuǎn)關(guān)節(jié)軸線(xiàn)的垂直與平行，具體的各個(gè)關(guān)節(jié)的軸線(xiàn)的空間位置關(guān)系，如圖4（a）所示。

圖4 ABB IRB2600型機(jī)械臂及其各個(gè)關(guān)節(jié)運(yùn)動(dòng)軸線(xiàn)Fig.4 ABB IRB2600 Robot and its All Ankle Axes

構(gòu)造出機(jī)械臂各個(gè)R副所在關(guān)節(jié)的運(yùn)動(dòng)旋量，具體為：

并考慮機(jī)械臂的幾何參數(shù)，得到各個(gè)關(guān)節(jié)所在的旋量為：

由上文所述，6R機(jī)械臂的正解公式為：

由于末端執(zhí)行器所在點(diǎn)q6，位于4、5與6的軸線(xiàn)交接點(diǎn)上，設(shè)（4×4）方陣T為機(jī)械臂末端執(zhí)行器的姿態(tài)矩陣，并考慮到式（10）與gab（θ）=T后則有：

θ4與θ5可由Paden-Kahan第二子問(wèn)題通過(guò)下式進(jìn)行求解：

θ6則根據(jù)Paden-Kahan第一子問(wèn)題通過(guò)下式進(jìn)行求解：

以上θ1、θ2、θ3、θ4、θ5與θ6均得到求解。

4 實(shí)驗(yàn)與分析

根據(jù)ABB 公司的IRB2600產(chǎn)品說(shuō)明書(shū)，得到其具體的D-H參數(shù)，如表3所示。

表3 這里所用ABB IRB2600型機(jī)器臂D-H 參數(shù)Tab.3 D-H Parameters of ABB IRB2600 Robot

并取Θ的各個(gè)元素的數(shù)值為：

并由正運(yùn)動(dòng)學(xué)計(jì)算出機(jī)械臂末端執(zhí)行器的位姿矩陣T為：

并以所計(jì)算出的位姿矩陣T作為逆運(yùn)動(dòng)學(xué)的求解目標(biāo)，運(yùn)用旋量理論并考慮了Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題，構(gòu)建出了ABB IRB2600型逆解公式后，運(yùn)用Matlab 軟件求得六組逆解值，如表4所示。在得到以上六組逆解后，再分別重新代入正運(yùn)動(dòng)學(xué)公式進(jìn)行求解。分析結(jié)果可知，所有的逆解均未出現(xiàn)無(wú)解情況，且所有解均在機(jī)械臂的有效工作空間之中；并且所有的六組逆解代回正解公式后，均近似到達(dá)了式（25）所列出的位姿，其中方向偏差最大為（6.8×10-12），最大位置偏差為（1.2×10-10）mm。由此驗(yàn)證了這里提出的Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題在串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方面應(yīng)用的有效性。

表4 六組運(yùn)用Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題進(jìn)行求解后的逆解值Tab.4 Six Inverse Solutions by the Second Paden-Kahan Subproblems

5 結(jié)論

（1）運(yùn)用旋量理論實(shí)現(xiàn)了機(jī)械臂正運(yùn)動(dòng)學(xué)的指數(shù)積公式的構(gòu)建；（2）提出了Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題，即繞若干個(gè)無(wú)確定幾何關(guān)系的定軸依次進(jìn)行有序的旋轉(zhuǎn)的逆解問(wèn)題的求解方法；（3）以ABB IRB2600型機(jī)械臂作為驗(yàn)證對(duì)象，通過(guò)第二子問(wèn)題的一種新的擴(kuò)展問(wèn)題對(duì)其進(jìn)行求逆運(yùn)算，在經(jīng)過(guò)對(duì)6組解驗(yàn)證后，驗(yàn)證了這里提出的Paden-Kahan第二子問(wèn)題的擴(kuò)展問(wèn)題在串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方面應(yīng)用的有效性。