基于Delaunay三角劃分網(wǎng)格計(jì)算多連桿機(jī)器人工作空間

吳 虎， 李鑫寧， 楊先海， 薛 鵬， 譚 帥

(1. 山東理工大學(xué)機(jī)械工程學(xué)院， 山東淄博 255000； 2.國家采油裝備工程技術(shù)研究中心， 山東東營 257091)

多連桿機(jī)器人是由多個活動關(guān)節(jié)將剛性桿件組成的連桿機(jī)構(gòu)，具有工作空間范圍大、靈活性好、機(jī)械結(jié)構(gòu)簡單、避障性能好、可避免機(jī)構(gòu)奇異性等優(yōu)點(diǎn)，因此廣泛應(yīng)用于工業(yè)生產(chǎn)、工程實(shí)踐、醫(yī)療手術(shù)等領(lǐng)域[1-2]。多連桿機(jī)器人為機(jī)械裝置的主要執(zhí)行部件，其靈活程度對于整個機(jī)構(gòu)系統(tǒng)的流暢運(yùn)行起著至關(guān)重要的作用。

工作空間的基本概念[3]及靈活性[4]、 邊界的確定[5]、靈活角的分析[6]、 工作空間的解析關(guān)系[7]等研究領(lǐng)域的探索， 為多連桿機(jī)器人工作空間的研究提供了理論基礎(chǔ)。 目前， 多連桿機(jī)器人工作空間的求解方法比較多， 主要可以歸納為圖解法(幾何法)、 解析法和數(shù)值積分法三大類。 圖解法[8]利用幾何繪圖的原理求得工作空間的邊界， 該方法直觀性強(qiáng)。 García-Murillo等[9]提出一種用于分析2(3-RRPS, R為轉(zhuǎn)動副, P為移動副, S為球副)并聯(lián)機(jī)器人工作空間的幾何方法， 通過計(jì)算機(jī)輔助設(shè)計(jì)(CAD)軟件進(jìn)行實(shí)例分析計(jì)算。 劉懷周等[10]利用圖解法分析深水作業(yè)串聯(lián)機(jī)械臂的工作空間邊界曲線， 運(yùn)用遺傳算法求得工作空間最優(yōu)解。 解析法[11]通過多次求解邊界曲面(線)的包絡(luò)問題， 確定工作空間的邊界方程。 Chen等[12]運(yùn)用解析法確定混合刻板繪圖儀的工作空間邊界， 并采用優(yōu)化算法計(jì)算了工作空間的體積。 于紅英等[13]利用解析法建立了并聯(lián)機(jī)構(gòu)各支鏈工作空間的邊界方程。 何世凱等[14]提出矩陣零空間的方法求解力旋量工作空間邊界。 利用數(shù)值積分法[15]計(jì)算多連桿機(jī)器人的工作空間時， 通過正運(yùn)動學(xué)方程計(jì)算多連桿機(jī)器人末端執(zhí)行機(jī)構(gòu)端點(diǎn)的坐標(biāo)， 從而形成工作空間邊界曲線或曲面。 隨著計(jì)算機(jī)軟、 硬件技術(shù)的不斷發(fā)展， 數(shù)值積分法可以分析任意形式多連桿機(jī)器人的工作空間， 因此得到了越來越廣泛的應(yīng)用， 其中比較典型的是基于蒙特卡羅法的工作空間求解。 與圖解法與解析法相比， 蒙特卡羅法省略了復(fù)雜的數(shù)學(xué)推導(dǎo)過程， 可編程性強(qiáng)， 容易實(shí)現(xiàn)可視化， 被廣泛地應(yīng)用在各種領(lǐng)域機(jī)器人的工作空間求解中， 但是傳統(tǒng)的蒙特卡羅法計(jì)算工作空間不夠精確。 He等[16]提出了一種基于增加邊界點(diǎn)密度的蒙特卡羅法計(jì)算工作空間。 Zhao等[17]利用傳統(tǒng)的蒙特卡羅法生成種子工作空間， 根據(jù)正態(tài)分布擴(kuò)展種子工作空間， 設(shè)置精度閾值動態(tài)調(diào)整標(biāo)準(zhǔn)差， 提出了一種體素算法來求出工作空間的體積， 通過不斷細(xì)化邊界部分來減小結(jié)果誤差。 Bader等[18]混合計(jì)算自運(yùn)動流形范圍估計(jì)工作空間包絡(luò)和蒙特卡羅積分估計(jì)方向體積的算法， 針對高冗余機(jī)器人提出了一種容錯工作空間的計(jì)算方法。 Hamida等[4]將遺傳算法和蒙特卡羅法相結(jié)合， 針對機(jī)器人結(jié)構(gòu)緊湊和靈巧性最大化進(jìn)行優(yōu)化。 樊桂菊等[19]建立了以可達(dá)工作空間性能和結(jié)構(gòu)緊湊為指標(biāo)的優(yōu)化模型， 利用遺傳算法求最優(yōu)參數(shù)， 采用網(wǎng)格劃分法求解工作空間體積， 達(dá)到較高的精度。 雖然上述方法在計(jì)算精度、 邊界確定等方面取得了很大的進(jìn)步， 但同時也存在直觀性不強(qiáng)、 表達(dá)式繁瑣、 對于多自由度的機(jī)器人運(yùn)算量過大等問題。

為了更準(zhǔn)確地描述多連桿機(jī)器人的工作空間，在工作空間密度函數(shù)[20]和自適應(yīng)非結(jié)構(gòu)化網(wǎng)格生成理論[21-22]的基礎(chǔ)上，本文中提出基于Delaunay三角劃分網(wǎng)格的多連桿機(jī)器人工作空間計(jì)算方法(簡稱本文方法)，通過與數(shù)值積分法、 網(wǎng)格劃分法進(jìn)行對比， 驗(yàn)證本文方法的準(zhǔn)確性。

1 靈活工作空間與工作空間

可達(dá)工作空間是在參考坐標(biāo)系中末端執(zhí)行機(jī)構(gòu)所能達(dá)到的所有位置坐標(biāo)的集合，記作W；靈活工作空間是在總的可達(dá)工作空間內(nèi)，末端執(zhí)行機(jī)構(gòu)可以以任意姿態(tài)達(dá)到的坐標(biāo)點(diǎn)所構(gòu)成的工作空間，記作W1；次工作空間是工作空間中去除靈活工作空間所剩余的空間，記作W2。三者的關(guān)系為

W=W1+W2

。

(1)

圖1所示為平面3R機(jī)器人的運(yùn)動模型。 該機(jī)構(gòu)由長度分別為l1、l2、l3的連桿組成， 令l1>l2+l3，l2>l3， 機(jī)械臂驅(qū)動轉(zhuǎn)角θi∈[-π,π](i=1, 2, 3)，以此例來說明靈活工作空間與工作空間的關(guān)系。

li(i=1,2,3)—連桿長度； θi—機(jī)械臂驅(qū)動轉(zhuǎn)角； xoy—平面直角坐標(biāo)系。圖1 平面3R(R為轉(zhuǎn)動副)機(jī)器人的運(yùn)動模型

多連桿機(jī)器人的工作空間與靈活工作空間如圖2所示。圓C1半徑R1=l1-l2-l3，圓C2半徑R2=l1-(l2-l3)=l1-l2+l3，圓C3半徑R3=l1+(l2-l3)=l1+l2-l3，圓C4半徑R4=l1+l2+l3。顯然，工作空間W是圓C1和C4圍成的圓環(huán)區(qū)域，靈活工作空間W1是圓C2和C3圍成的圓環(huán)區(qū)域，其他剩余的工作空間為W2。

li(i=1,2,3)—連桿長度； Ch(h=1,2,3,4)—圓心為坐標(biāo)原點(diǎn)o的不同半徑的圓。圖2 多連桿機(jī)器人的工作空間與靈活工作空間

2 工作空間密度函數(shù)

將機(jī)械機(jī)構(gòu)的工作空間均勻地劃分成等尺寸的單元，每個單元內(nèi)末端執(zhí)行機(jī)構(gòu)可以到達(dá)的點(diǎn)數(shù)與整個工作空間內(nèi)到達(dá)點(diǎn)數(shù)比值除以單元的面積或體積即為工作空間密度[23]。工作密度函數(shù)值越大，證明機(jī)械機(jī)構(gòu)的靈活性越大， 同時可以更加直觀地表達(dá)多連桿機(jī)器人的工作空間。 蒙特卡羅法屬于實(shí)驗(yàn)數(shù)學(xué)的一個分支， 利用隨機(jī)數(shù)進(jìn)行統(tǒng)計(jì)實(shí)驗(yàn)， 將均值、 概率等作為待解決問題的數(shù)值解， 也稱為蒙特卡羅模擬。 通過研究多連桿機(jī)器人的工作空間密度， 采用蒙特卡羅法，從概率統(tǒng)計(jì)的角度借助計(jì)算機(jī)技術(shù)進(jìn)行仿真， 可以直觀地描述工作空間的形狀、 特征及性質(zhì)。

2.1 蒙特卡羅法計(jì)算步驟

工作空間解算的蒙特卡羅算法原理[6]是

W={M(k)∶k∈G}

，

(2)

式中：k為廣義關(guān)節(jié)變量；M(k)為正運(yùn)動學(xué)參數(shù)；G為關(guān)節(jié)空間集合。

采用蒙特卡羅法求解多連桿機(jī)器人工作空間的主要思想如下：通過計(jì)算機(jī)仿真隨機(jī)產(chǎn)生大量的關(guān)節(jié)空間點(diǎn)，映射至工作空間，得到一定數(shù)量的末端位置隨機(jī)點(diǎn)，進(jìn)而統(tǒng)計(jì)解算工作空間的邊界。蒙特卡羅算法的步驟如下：

1)根據(jù)靈活工作空間的定義，對各個關(guān)節(jié)轉(zhuǎn)角的樣本進(jìn)行劃分取點(diǎn)，構(gòu)建樣本空間G；

2)依據(jù)正運(yùn)動學(xué)參數(shù)M(k)解算機(jī)械臂末端執(zhí)行機(jī)構(gòu)所能到達(dá)的各個位置；

3)輸出機(jī)械臂末端執(zhí)行機(jī)構(gòu)的位置，構(gòu)建靈活工作空間W，樣本容量越大，所得到的幾何輪廓越清晰，靈活工作空間W越精確。

平面3R機(jī)器人正運(yùn)動計(jì)算模型如圖3所示。根據(jù)圖中多連桿機(jī)器人末端執(zhí)行機(jī)構(gòu)位置與連桿長度及關(guān)節(jié)轉(zhuǎn)角的幾何關(guān)系，用Denavit-Hartenberg(D-H)法列出其正運(yùn)動學(xué)方程。

li(i=1,2,3)—連桿長度； θi—機(jī)械臂驅(qū)動角； P—末端執(zhí)行機(jī)構(gòu)位置點(diǎn)； o、 A、 B—關(guān)節(jié)中心點(diǎn)； x0oy0、 x1oy1、 x2Ay2、 x2By2—平面直角坐標(biāo)系。圖3 平面3R(R為轉(zhuǎn)動副)機(jī)器人正運(yùn)動計(jì)算模型

假設(shè)坐標(biāo)軸z0、z1、z2垂直于紙面向外，從空間中的點(diǎn)(x0,y0,z0)到(x1,y1,z1)的齊次旋轉(zhuǎn)變換矩陣為

(3)

從點(diǎn)(x1,y1,z1)到(x2,y2,z2)的齊次旋轉(zhuǎn)變換矩陣為

(4)

從點(diǎn)(x2,y2,z2)到(x3,y3,z3)的齊次旋轉(zhuǎn)變換矩陣為

(5)

從點(diǎn)(x3,y3,z3)到(x0,y0,z0)的齊次旋轉(zhuǎn)變換矩陣為

(6)

可得末端執(zhí)行機(jī)構(gòu)P點(diǎn)的位置矢量為

(7)

式中(xP,yP,zP)為末端執(zhí)行機(jī)構(gòu)位置點(diǎn)P點(diǎn)的坐標(biāo)。

基于以上公式推導(dǎo)，可以得到兩桿機(jī)器人的坐標(biāo)如下：

(8)

(9)

式中l(wèi)i為第i個連桿的長度。

以上運(yùn)動學(xué)方程是在直角坐標(biāo)系下構(gòu)建的，要得到極坐標(biāo)系下對應(yīng)的表達(dá)式，可利用直角坐標(biāo)與極坐標(biāo)的關(guān)系，即

(10)

(11)

式中：r為極徑；φ為極角。

2.2 密度函數(shù)的構(gòu)建

蒙特卡羅法計(jì)算多連桿機(jī)器人工作空間采用的是統(tǒng)計(jì)學(xué)的方法。 Sigmoid函數(shù)是統(tǒng)計(jì)學(xué)中常用的數(shù)學(xué)模型。 根據(jù)工作空間密度函數(shù)符合隨機(jī)密度函數(shù)[20]的特性， 通過建立數(shù)學(xué)模型來描述工作空間密度。

設(shè)N連桿機(jī)械臂桿件的密度函數(shù)為fN(p)，其中p∈G，N連桿機(jī)械臂的工作空間密度函數(shù)為

f1,2,…,N(p)=(f1*f2*…*fN)(p)

,

(12)

式中*為在G上的卷積運(yùn)算符。

ρ1,2,…,N(x)=(ρ1⊙ρ2⊙…⊙ρN)(x)

,

(13)

工作空間是連續(xù)的空間，其特點(diǎn)是隨機(jī)變量可以連續(xù)地取值，所有的連續(xù)隨機(jī)變量都有概率密度函數(shù)。u為多維向量，μ為均值，σ2為方差，多元正態(tài)分布密度函數(shù)的形式為

(14)

對式(14)進(jìn)行嚴(yán)格泛化，得

(15)

式中：Σ為N×N型對稱正定矩陣； |·|為矩陣的行列式。μ、Σ的求解如下：

(16)

(17)

(18)

連續(xù)的概率密度函數(shù)不局限于1，因此在工作空間的研究中，所有連續(xù)隨機(jī)變量都是可測的，所有連續(xù)分布都有密度。對于N連桿機(jī)械臂，當(dāng)每個連桿轉(zhuǎn)角有M個狀態(tài)時，連桿機(jī)械臂工作空間內(nèi)的樣本個數(shù)為K=MN，此樣本空間的方差S1,2,…,N(K)為

(19)

式中XK為空間樣本方差的矩陣。

當(dāng)M足夠大時，實(shí)際工作空間密度函數(shù)的方差為

(20)

式中In為n×n型單位矩陣。

根據(jù)卷積迭代及極值定理，Sigmoid函數(shù)構(gòu)建的工作空間密度函數(shù)為

exp[-(u-μ)T(u-μ)Σ-1] 。

(21)

設(shè)桿長l1=1.8，l2=2.4，l3=3.1，在轉(zhuǎn)角θ1、θ2、θ3不受限制的情況下，取樣本點(diǎn)個數(shù)M=8 000，根據(jù)式(9)運(yùn)算并繪制出工作空間密度圖，如圖4所示。

2.3 計(jì)算仿真

根據(jù)構(gòu)建的密度函數(shù)，建立五自由度多連桿機(jī)器人模型，如圖5所示，其中l(wèi)1=0.42，l2=0.375，l3=0.42，l1、l2、l3與底座轉(zhuǎn)軸不在一個平面內(nèi)(l1、l2、l3與底座轉(zhuǎn)軸沿轉(zhuǎn)軸同方向偏移0.1、 0.2、 0.3)，轉(zhuǎn)角θ1、θ2、θ3以及底座轉(zhuǎn)角θ4、 執(zhí)行機(jī)構(gòu)轉(zhuǎn)角θ5不受限制， 所取的樣本點(diǎn)個數(shù)M=20 000。 圖6所示為五自由度多連桿機(jī)器人工作空間仿真視圖，4個視圖全面描述了工作空間密度。

xy—坐標(biāo)平面。圖4 3R(R為轉(zhuǎn)動副)機(jī)器人工作空間密度云圖

xyz—空間直角坐標(biāo)系。圖5 五自由度多連桿機(jī)器人模型

(a)工作空間三維視圖

(b)xy坐標(biāo)平面

(c)xz坐標(biāo)平面

(d)yz坐標(biāo)平面圖6 五自由度多連桿機(jī)器人工作空間仿真視圖

六自由度多連桿機(jī)器人模型如圖7所示，其中l(wèi)1=0.42，l2=0.375，l3=0.42，l4=0.42， 桿l1、l2、l3、l4與底座轉(zhuǎn)軸均在一個平面內(nèi)，轉(zhuǎn)角θ1、θ2、θ3、θ4以及底座轉(zhuǎn)角θ5， 執(zhí)行機(jī)構(gòu)轉(zhuǎn)角θ6不受限制，所取的樣本點(diǎn)個數(shù)M=20 000。六自由度多連桿機(jī)器人工作空間仿真視圖如圖8所示。

圖7 六自由度多連桿機(jī)器人模型

3 工作空間計(jì)算

根據(jù)連桿機(jī)器人運(yùn)動學(xué)及工作空間密度研究，提出基于Delaunay三角劃分網(wǎng)格計(jì)算多連桿機(jī)器人工作空間的方法， 原理如圖9所示。 本文方法是一種非結(jié)構(gòu)化網(wǎng)格生成方法，首先建立一個包含所有點(diǎn)的初始三角網(wǎng)，然后將剩余的點(diǎn)逐點(diǎn)插入，并且確保初始三角網(wǎng)成為Delaunay三角網(wǎng)，同時，每個Delaunay三角形的外接圓不包含其他結(jié)點(diǎn)；若出現(xiàn)外接圓內(nèi)有其他結(jié)點(diǎn)，則需修改局部網(wǎng)格，直至滿足每個Delaunay三角形的外接圓不包含其他結(jié)點(diǎn)這一條件。

(a)工作空間三維視圖

(b)xy坐標(biāo)平面

(c)xz坐標(biāo)平面

(d)yz坐標(biāo)平面圖8 六自由度多連桿機(jī)器人工作空間仿真視圖

圖9 基于Delaunay三角劃分網(wǎng)格的 多連桿機(jī)器人工作空間計(jì)算方法原理

1)建立初始網(wǎng)格。首先找到包含該密度空間點(diǎn)集的外接矩形，通常保證外接矩形的邊長是點(diǎn)集最大邊界長度的3倍。然后任意連接矩形的對角線，形成2個三角形，作為初始Delaunay三角網(wǎng)格。

2)點(diǎn)的逐次插入。在保證計(jì)算區(qū)域內(nèi)工作空間密度函數(shù)均勻的情況下，插入點(diǎn)必在三角形內(nèi)或三角形邊上，3種插入點(diǎn)的情況如圖10所示。插入點(diǎn)P′與三角形的3個頂點(diǎn)U、V、W相連，三角形及其外接圓形成Delaunay腔，然后連接插入點(diǎn)與空腔的每個頂點(diǎn)，每次插入一個新點(diǎn)之后，重復(fù)連接插入點(diǎn)與空腔的每個頂點(diǎn)操作。

U、 V、 W—三角形的3個頂點(diǎn)； V1—三角形的鄰近頂點(diǎn)； P′—插入點(diǎn)。圖10 點(diǎn)的逐次插入中插入點(diǎn)的3種情況

3)構(gòu)造外接圓。首先對U、V、W這3個點(diǎn)構(gòu)造外接圓T1，把所有結(jié)點(diǎn)包含進(jìn)去，之后每次只引入一個結(jié)點(diǎn)，生成相應(yīng)的圓T2，T3，……，直至所有結(jié)點(diǎn)都進(jìn)入Delaunay三角劃分網(wǎng)格。

4)刪除最近邊。將三角形中離新結(jié)點(diǎn)最近的一條邊刪除，對網(wǎng)格進(jìn)行重生成，于是新結(jié)點(diǎn)與老結(jié)點(diǎn)形成新的三角形，如圖11所示。

圖11 通過刪除最近邊對網(wǎng)格進(jìn)行重生成所得新三角形

6)工作空間面積的求和。工作空間面積S的求解公式為

(22)

式中：n1為內(nèi)部空間子網(wǎng)格個數(shù)；n2為外部邊界空間子網(wǎng)格個數(shù)。

以圖3為模型，機(jī)器人連桿參數(shù)如表1所示。

表1 平面3R(R為轉(zhuǎn)動副)機(jī)器人連桿長度與機(jī)械臂驅(qū)動轉(zhuǎn)角

用數(shù)值積分法、 網(wǎng)格劃分法和本文方法分別計(jì)算工作空間面積， 并與理論計(jì)算值進(jìn)行比較， 如表2所示。 從表中可以看出， 本文方法的計(jì)算結(jié)果較準(zhǔn)確， 與其他2種方法相比， 誤差率分別降低了0.99%和0.50%， 提高了工作空間的計(jì)算精度。

表2 不同方法計(jì)算的工作空間面積比較

4 結(jié)論

為了更準(zhǔn)確地描述多連桿機(jī)器人的工作空間，在對多連桿機(jī)器人進(jìn)行運(yùn)動學(xué)分析的基礎(chǔ)上，本文中提出基于Delaunay三角劃分網(wǎng)格計(jì)算連桿機(jī)器人工作空間，得出如下結(jié)論：

1)應(yīng)用構(gòu)建的Sigmoid工作空間密度函數(shù)，不僅可以直觀地表達(dá)工作空間的工作范圍，而且可以定量地得出工作空間體積的數(shù)值，避免了繁瑣的表達(dá)式及過量的運(yùn)算，為工程中的實(shí)際應(yīng)用提供了有效的計(jì)算方法，同時為工作空間后續(xù)的深入研究提供了思路。

2)通過本文方法與數(shù)值積分法、 網(wǎng)格劃分發(fā)法等其他計(jì)算方法的對比，驗(yàn)證了本文方法的準(zhǔn)確性。

3)在工作空間的研究中，正運(yùn)動學(xué)的D-H參數(shù)算法隨著連桿機(jī)器人自由度的增加，需要在每個關(guān)節(jié)增加坐標(biāo)系，計(jì)算量隨之增大。如何減少計(jì)算量、 提高計(jì)算效率是需要進(jìn)一步研究的問題。