肖帆，曾克儉

（1.湖南工業(yè)大學(xué) 機械工程學(xué)院，湖南株洲 412007；2.湖南工業(yè)大學(xué) 科技學(xué)院，湖南株洲 412007）

機器人逆運動學(xué)求解作為機器人離線編程、軌跡規(guī)劃、控制算法設(shè)計等其他課題研究的基礎(chǔ)[1]，一直是機器人學(xué)中的一個經(jīng)典問題，同樣也是研究熱點。逆運動學(xué)求解的實質(zhì)是完成機器人工作空間到關(guān)節(jié)空間的映射，逆運動學(xué)方程組具有高維、非線性的特點，求解復(fù)雜且不易求出。

當機器人的結(jié)構(gòu)滿足PIEPER[2]準則，即最后3 個關(guān)節(jié)為軸線交于一點的球型腕部設(shè)計時，可以得到解析解。然而該結(jié)構(gòu)的手腕無法實現(xiàn)360°翻轉(zhuǎn)，難以滿足在受限空間作業(yè)的要求，同時腕部承受能力不強。因而在焊接、噴涂和材料處理等工業(yè)中，工業(yè)機器人被設(shè)計為非球型手腕結(jié)構(gòu)，以使其具有更高的負載能力，更遠的水平抵達距離和靈活性[3]，但這也導(dǎo)致該類機器人難以得到逆運動學(xué)解析解，并使得機器人的逆運動學(xué)非線性方程組變得更復(fù)雜，耦合度更高。此時可以利用一般6R 機器人的位姿逆解方法求腕部偏置型6R 機器人的逆運動學(xué)解，這些方法主要利用關(guān)節(jié)的半角正切，將運動學(xué)方程轉(zhuǎn)化為1 元16 次多項式進行求解，但是這些方法的公式推導(dǎo)過程十分繁瑣耗時[4～6]。

近些年隨著計算技術(shù)的發(fā)展，遺傳算法[7]、神經(jīng)網(wǎng)絡(luò)算法[8～11]等基于元啟發(fā)的搜索算法被用于6R 機器人的反解中，但是這類算法仍難以滿足實時性控制的要求。肖帆等[12]提出的M-CMA-ES 算法可以滿足機械臂的實時求解，但是難以有效地得到非冗余機器人的逆運動學(xué)多解，而多解對于機器人避障具有十分重要的意義。李光等[13]提出基于唯一域的方法求非冗余機器人逆運動學(xué)多解，該方法需要利用機器人的雅可比矩陣行列式等于零時得到的多項式劃分關(guān)節(jié)空間，而對于許多非球腕結(jié)構(gòu)的機械臂而言，它們的雅可比矩陣行列式通常難以得到簡化的相乘的多項式。

劉志忠等[14]、韓磊等[15]均采用球型手腕的逆解作為初始值的數(shù)值迭代方法求得了偏置型手腕的逆解，但是該方法難以得到完備的逆解。Wu 等[16]通過兩次矩陣逆乘變換，得到了只含θ6的非線性封閉方程，并將該方程的求解轉(zhuǎn)換為最小化問題，由于初始迭代點為隨機生成，無法保證算法穩(wěn)定的計算效率。Trinh 等[17]提出一種幾何分析的方法，用于求兩款腕部偏置型機器人的逆運動學(xué)解，該方法可以得到一個只含未知數(shù)θ6的非線性封閉方程。Pashkevich等[18]提出將含6 個未知變量的高斯方程組，轉(zhuǎn)換為用θ6或θ4表示其余5 個關(guān)節(jié)變量的非線性封閉方程，然后用腕部無偏置型6R 機器人的逆解作為初始值，進行一維迭代求解。Kucuk 等[19]研究了含移動副和轉(zhuǎn)動副的腕部偏置型6R 機器人，利用θ1表示其余5 個關(guān)節(jié)變量，采用與文獻[15]類似的方法求得逆解。Husty 等[20]提出將6R 串聯(lián)機器人分解為兩個3R 子鏈后用微分流形來求解機器人逆運動學(xué)的方法。卜王輝等[21]在文獻[18-20]的基礎(chǔ)上，對腕部前端偏置型機器人進行了研究，給出了機器人分解為兩部分時選擇切斷點的建議，但其分析方法具有局限性，且變換方程的推導(dǎo)過程過于復(fù)雜。

在文獻[17-21]研究的基礎(chǔ)上，針對幾款常見的6R 非球腕工業(yè)機器人，本文提出多切斷點分解—重構(gòu)技術(shù)推導(dǎo)其逆運動學(xué)公式?；谛坷碚摰闹笖?shù)積公式建立機器人的正向運動學(xué)公式，得出機器人可分解的結(jié)論；給出切斷點的選擇和重連的幾何約束條件；仿真實驗證明了本文所提方法的有效性。

1 n 自由度機器人的分解

研究機器人運動學(xué)的學(xué)者中，大多使用DH 方法[22]建立正向運動學(xué)公式，然后通過矩陣的逆乘找到可以求逆解的方程。事實上，這其中已經(jīng)包含了將機器人分解的思想，由于DH 法不具備直觀地幾何意義，從而導(dǎo)致該思想沒有被提煉出來。旋量法中的Paden-Kahan 子問題[1,23]也包含了機器人分解的思想，但是該方法在做公式變形時，只考慮了分解點的位置，而忽略了分解點的姿態(tài)信息。

1.1 n 自由度機器人運動學(xué)的旋量表示

旋量理論[24]將剛體在空間的運動描述為由繞某一定軸的旋轉(zhuǎn)和平移復(fù)合而成。設(shè)ω=[ωx ωy ωz]T∈R3表示剛體旋轉(zhuǎn)軸的方向，r為軸上一點的位置，則剛體的運動旋量可用ξ=[ωv]表示，其中v=r×ω，ξ同樣可以被稱為剛體的螺旋軸。ω的反對稱矩陣記為

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

剛體的矩陣指數(shù)形式為

式中eθω=I+ωsinθ+ω2(1-cosθ)。

圖1 為n自由度機器人。

圖1 n 自由度機器人

根據(jù)旋量理論，圖1 中n自由度機器人的正向運動學(xué)公式的指數(shù)積表達形式為

式中：gST(θ)為經(jīng)剛體變換后機器人末端工具坐標系{T}為相對于慣性坐標系{S}的位姿gST(θ)；gST(0)為機器人位于初始位形時工具坐標系{T}相對于慣性坐標{S}的位姿。

各關(guān)節(jié)軸旋轉(zhuǎn)的正方向規(guī)定為右手拇指指向關(guān)節(jié)軸線時，四指彎曲的方向。

1.2 n 自由度機器人可分解說明

式（4）可變形為

式中g(shù)i+1(0)是一個位姿矩陣，由于位姿矩陣始終可逆，故等式兩邊同時約去它亦可保持相等。若gi+1(0)的位置位于連接關(guān)節(jié)i至關(guān)節(jié)i+1 的連桿之上，則等號兩邊的式子將具有直觀的幾何意義，表示原機器人分為兩部分子鏈。

圖2 中{Ti+1}為建立在關(guān)節(jié){i+1}原點上的坐標系，其相對于原世界坐標系的位姿矩陣為gi+1(0)。由式（4）的形式可知，式（5）等號左邊的式子表示關(guān)節(jié)Link1左側(cè)端點至Linki端點組成的子鏈，圖2 上部，需要注意的是，關(guān)節(jié)1 此時與Link1已斷開；式（5）等號右邊的式子，則表示關(guān)節(jié)1 與關(guān)節(jié)n至關(guān)節(jié)i+1 所組成的子鏈，其中關(guān)節(jié)1 與Linkn末端由虛擬連桿連接，該虛擬連桿由gST(θ)gST-1(0)的值決定。

圖2 機器人分解圖

因此式（5）等號兩邊的式子表示在關(guān)節(jié)1 和關(guān)節(jié)i+1 的原點分別進行了切斷，并重新連接組成了兩個新的子鏈，為方便理解，記關(guān)節(jié)1 處為切斷點1，關(guān)節(jié)i+1 處為切斷點2，不含虛擬連桿的子鏈記為子鏈L，含虛擬連桿的子鏈記為子鏈R。值得注意的是，子鏈R中關(guān)節(jié)角前面加負號，表示與原機器人對應(yīng)關(guān)節(jié)角的旋轉(zhuǎn)方向相反。

子鏈的工具坐標系{i+1}可以任意設(shè)定。為了簡化后續(xù)逆運動學(xué)公式的推導(dǎo)，給出以下原則：

1）切斷點優(yōu)先選擇在關(guān)節(jié)的原點；

2）位于切斷點1 處的兩子鏈的基座標系，盡量與原機器人慣性坐標系一致；

4）工具坐標系{i+1}應(yīng)有一條軸線與所在切斷點關(guān)節(jié)的軸線共線或平行；

5）分離得到的子鏈中，決定末端位置的關(guān)節(jié)數(shù)和決定條件4）中軸線方位的關(guān)節(jié)數(shù)應(yīng)同時最少。

此時，兩個子鏈重新結(jié)合為原機器人的通用幾何約束為末端執(zhí)行器的位姿重合。

2 機器人的分解及逆運動學(xué)求解

本節(jié)將以文獻[21]中的MOTOMAN-EA140型6R 機器人為例求逆解，該機器人手腕存在30 mm的前端偏置。

2.1 正向運動學(xué)

圖3 中選擇{S}作為慣性坐標系，{T}作為工具坐標系，{T}與螺旋軸的參數(shù)均以{S}為參考描述，且{T}的原點與關(guān)節(jié)6 的原點重合。

機器人零位時工具坐標系的位姿為

實驗的設(shè)計對象是水杯產(chǎn)品。水杯較常見，其復(fù)雜程度低、易于表現(xiàn)、設(shè)計周期短，但是創(chuàng)意空間較大。水杯的評價指標以單一的感性因素為主，避免了多目標評價的復(fù)雜因素影響。

各關(guān)節(jié)螺旋軸參數(shù)列于表1 中。

表1 各關(guān)節(jié)螺旋軸參數(shù)

MOTOMAN-EA140 機器人的正向運動學(xué)公式為

2.2 分解

根據(jù)2.1 的分離原則，可將式（7）變形為

其中

從圖4 可以明顯看出，子鏈L的末端位置以及工具坐標系的xL軸只由關(guān)節(jié)2、3 決定；子鏈R的末端位置以及工具坐標系的yR軸只由關(guān)節(jié)1、6 決定。而該幾何結(jié)果，同樣可從式（8）和式（9）的結(jié)果中反應(yīng)，將表1 中螺旋軸參數(shù)代入式（11）和式（12）中，得

圖4 MOTOMAN-EA140 機器人分解圖

式中：si=sinθi；ci=cosθi；sij=sin(θi+θj)；cij=cos(θi+θj)；i、j=1,···,6，且i≠j。

2.3 逆運動學(xué)求解

由子鏈重新結(jié)合的通用約束條件可知pL=pR。

首先由pL2=pR2可直接得到θ1關(guān)于θ6的表達式為

其中

將pL1=pR1、pL3=pR3聯(lián)立為方程組，并將等號右邊的常數(shù)項均移至等號左邊，然后平方求和，得

將已求得的θ1和θ3代入pL1=pR1、pL3=pR3組成的方程組中，可得

其中

至此可以得到θ1、θ2和θ3關(guān)于θ6的表達式，由式（14）和式（17）可知θ1、θ2和θ3將有四組解。

由2.2 節(jié)可知xL與yR分別由θ2、θ3和θ1、θ6決定，而兩個子鏈末端工具坐標系重合是必要的約束條件，故xL⊥yR，即xL·yR=0，因此可得關(guān)于θ6的封閉方程為

從前面θ1、θ2和θ3解的組數(shù)可知，方程（22）也將有4 個。一旦求出θ6，即可反代回式（14）～ 式（21）求出θ1、θ2和θ3，然后利用子鏈姿態(tài)矩陣相等，可以求出θ4、θ5。

其中

其中

2.4 二分法求解

方程（22）是關(guān)于θ6的非線性方程，直接求解很困難，本文使用二分法[25]對其搜索求解。將式（20）改寫為關(guān)于θ6的函數(shù)

此時只需求出式（26）中的零點，因求解過程中均為數(shù)值解，故只需求使得|f(θ6)|≤ε的θ6即可，ε為設(shè)定的算法終止閾值。求解方法如下：

1）初始化θ6。θ6的迭代步長h，閾值ε；

2）確定區(qū)間。將θ6迭代的數(shù)值代入式（14）～ 式（21）和式（26），若θ6k=θ6k-1+h，則確定二分區(qū)間為[a,b]，a=θ6k-1，b=θ6k，否則繼續(xù)迭代，直至θ6k≥2π 停止。在此搜索過程中，若|f(θ6k)|≤ε，同樣停止迭代，輸出θ6k。

3）求區(qū)間[a，b]中點c。（1）計算f(c)，若|f(c)|≤ε則停止；（2）若f(a)f(c)＜0，則令b=c；（3）若f(c)f(b)＜0，則令a=c；（4）若達到閾值ε，則輸出a或b，否則繼續(xù)重復(fù)1）～3）。

3 仿真算例

算法的計算軟件為MATLAB2014b，PC 配置如下：處理器為Intel(R) Core(TM) i7-9750H，CPU 速度為2.60 GHz，安裝內(nèi)存為16.00 GB。算法文件格式采用.m 函數(shù)編寫，再使用MATLAB Coder 轉(zhuǎn)換為mex_fun 格式。

圖3 中MOTOMAN-EA140 機器人的連桿參數(shù)分別為l1=450 mm，l2=150 mm，l3=570 mm，l4=200 mm，l5=640 mm，l6=30 mm，l7=20 0mm。二分法參數(shù)設(shè)置為：θ6(0)=0，ε=10-14，h=π/180。

設(shè)定目標位姿為

式（26）與其對應(yīng)的圖形如圖5 所示。圖中4 條曲線分別對應(yīng)式（26）的4 組函數(shù)，圖中明顯可見，黃色“--”與藍色“-.”曲線分別有4 個零點，其它兩條曲線各有2 個兩點，因此位姿T共有12 組解（見表2，表中e為逆解代入正向運動學(xué)得到的位姿與T的誤差二范數(shù)）。在MATLAB 中采用mex_fun 函數(shù)編寫二分法，得到圖中所有解的時間約為2 ms。其中有4 個零點的曲線比只有2 個零點的曲線約多花費1/3 的求解時間。

表2 目標位姿T 的12 組逆解

圖5 位姿T 對應(yīng)的f(θ6)曲線

本文所使用二分法的總體時間與步長h的取值有關(guān)，步長越大則消耗的時間越少，但如果f(θ6)函數(shù)的零點距離很近，則可能會遺漏掉部分解。同時，零點個數(shù)的多少也對求解時間有影響，零點越多，消耗的時間也越多。

因本文此次研究的重點在于逆運動學(xué)公式的推導(dǎo)，故在仿真中沒有與其它算法的求解效率作比較，僅提供了一個解決式（26）的選擇方案。

值得注意的一點，將機器人的手腕偏置設(shè)為0，則本文所羅列的公式即為球腕機器人的解析解公式。

4 結(jié)論

1）通過對機器人正向運動學(xué)的指數(shù)積表達式變形，說明了任意機器人可分解為兩個低自由度子鏈的可行性，并描述了各式子的幾何意義。給出了機器人切斷點的通用原則，以及子鏈重新結(jié)合的通用約束條件。

2）在手腕具有前端偏置的工業(yè)機器人的關(guān)節(jié)1 與關(guān)節(jié)5 原點處切斷，得到了2 個子鏈，其中子鏈L 的工具坐標系只具有平面運動效果，兩個子鏈的正向運動學(xué)式子更為簡潔。其它非球腕工業(yè)機器人，也可借鑒該切斷方法推導(dǎo)逆解公式。

3）根據(jù)子鏈重新結(jié)合的幾何約束條件，簡化了各關(guān)節(jié)逆運動學(xué)推導(dǎo)的過程，并得到了關(guān)于θ6的一元非線性封閉方程，該推導(dǎo)過程具有明確的幾何意義。

4）為求解一元非線性方程提供了一種方案，二分法得到目標位姿所有逆解的時間約為2 ms，計算位姿誤差為10-12mm。