雙臂并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正逆解及其奇異位形存在性預(yù)測(cè)

王 恒，余 盛，陸 勇，朱芳甫，蔣科堅(jiān)

（1浙江理工大學(xué) 信息科學(xué)與工程學(xué)院，杭州 310018；2歐姆龍杭州分公司，杭州 310030）

0 引 言

20世紀(jì)50年代末，世界第一臺(tái)工業(yè)機(jī)器人的研制成功開(kāi)啟了工業(yè)生產(chǎn)進(jìn)入無(wú)人化的序幕。人們可以控制機(jī)器人從事危險(xiǎn)、精細(xì)和重復(fù)性工作，工業(yè)機(jī)器人的控制研究越來(lái)越受到關(guān)注。工業(yè)機(jī)器人的控制是以運(yùn)動(dòng)學(xué)正逆解為基礎(chǔ)的，因此，在機(jī)器人投入工作之前需先求得機(jī)器人的運(yùn)動(dòng)學(xué)正逆解，但是機(jī)器人在動(dòng)作過(guò)程中可能會(huì)處于一個(gè)特殊的位置或形態(tài)，此種情況下機(jī)器人將無(wú)法正常工作，稱為奇異位形。本文將針對(duì)一種工業(yè)機(jī)器人—雙臂并聯(lián)機(jī)器人，對(duì)其運(yùn)動(dòng)學(xué)正逆解和奇異位形預(yù)測(cè)展開(kāi)研究。

對(duì)機(jī)器人正逆解和奇異性問(wèn)題的研究旨在實(shí)現(xiàn)對(duì)機(jī)器人精確有效的控制。Gosselin等人提出將并聯(lián)機(jī)器人的奇異類(lèi)型劃分成3種，并利用雅克比矩陣的行列式分別進(jìn)行判定，利用該方法對(duì)5種并聯(lián)機(jī)器人奇異位形進(jìn)行分析，并從速度的輸入和輸出的角度給出相應(yīng)的物理意義。Shao等人采用映射法，以六階方程的形式給出了三自由度空間并聯(lián)機(jī)械手的正逆解，并基于幾何約束條件對(duì)該并聯(lián)機(jī)械手的奇異性進(jìn)行分析，但沒(méi)有給出位于奇異位形時(shí)的物理意義和失控分析。Nag等人針對(duì)一種六自由度并聯(lián)機(jī)構(gòu)，提出了一種用于識(shí)別無(wú)奇異空間的方法，并求出了該機(jī)器人不含奇異位形的工作空間。Baron等人針對(duì)冗余并聯(lián)機(jī)器人提出了一種避免奇異位形的幾何方法，首先通過(guò)確定執(zhí)行器和奇異點(diǎn)接近程度，然后優(yōu)化運(yùn)動(dòng)學(xué)冗余的自由度，再進(jìn)行奇異點(diǎn)的確定，該方法在一定程度上可以預(yù)測(cè)奇異位形，避免機(jī)器人工作異常。Hijazi等人針對(duì)一種平面并聯(lián)機(jī)器人，提出了一種奇異軌跡和奇異位置的識(shí)別方法。呂志忠等人將爬壁機(jī)器人視為并聯(lián)機(jī)器人，分別利用牛頓法和解析法得出該機(jī)器人運(yùn)動(dòng)學(xué)正逆解，并利用雅克比矩陣和Grassmann線幾何理論分析了機(jī)器人的正逆運(yùn)動(dòng)學(xué)奇異位形，并對(duì)正運(yùn)動(dòng)學(xué)奇異位形進(jìn)行了驗(yàn)證。潘英等人建立了新型五自由度3D打印并聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)模型，計(jì)算了該機(jī)器人的運(yùn)動(dòng)學(xué)逆解，并利用螺旋理論法求出雅克比矩陣，對(duì)奇異性進(jìn)行了分析。李保坤等人利用逆速度雅克比矩陣和力雅可比矩陣對(duì)六自由度Stewart并聯(lián)機(jī)器人的奇異位形進(jìn)行了分析，并提出冗余驅(qū)動(dòng)法消除奇異位形。常定勇等人設(shè)計(jì)出了一個(gè)兩輸出的并聯(lián)機(jī)器人，采用閉環(huán)矢量法求得運(yùn)動(dòng)學(xué)逆解，并利用機(jī)構(gòu)的自由度對(duì)奇異位形進(jìn)行了分析。朱偉等人針對(duì)弱耦合三平移并聯(lián)機(jī)器人，利用機(jī)構(gòu)原理圖推出位置的正逆解，并通過(guò)對(duì)速度的分析求得機(jī)構(gòu)雅克比矩陣，進(jìn)一步得出該機(jī)器人正逆奇異位形。馬廣英等人針對(duì)3UPR并聯(lián)機(jī)構(gòu)的4足機(jī)器人的運(yùn)動(dòng)學(xué)問(wèn)題，通過(guò)矩陣變換法求得了機(jī)器人的位置逆解，并進(jìn)一步利用速度模型對(duì)奇異性進(jìn)行分析。周少瑞等人提出一種新型的對(duì)稱3-CRCR／RPU并聯(lián)機(jī)器人，利用旋轉(zhuǎn)矩陣法以解析式的形式給出了該機(jī)器人正逆解，并利用機(jī)構(gòu)雅克比矩陣證明了該機(jī)器人不存在奇異位形。葉鵬達(dá)等人針對(duì)并聯(lián)機(jī)器人奇異性分析問(wèn)題提出了代數(shù)法、幾何法、運(yùn)動(dòng)學(xué)法和現(xiàn)代數(shù)學(xué)法，并分別進(jìn)行了詳細(xì)的分析。

在以往文獻(xiàn)中，對(duì)機(jī)器人的奇異性分析往往從數(shù)學(xué)計(jì)算和矩陣特征的角度給出定義和判斷，而對(duì)在奇異位形實(shí)際可能發(fā)生機(jī)器人臂失控現(xiàn)象沒(méi)有對(duì)應(yīng)的描述分析。本文針對(duì)雙臂并聯(lián)機(jī)器人的運(yùn)動(dòng)控制，提出了一套包括正逆解、工作空間確定、奇異位形預(yù)測(cè)的完整方法，并驗(yàn)證正逆解和奇異位形預(yù)測(cè)的正確性，對(duì)該機(jī)器人處于奇異位形時(shí)的失控情形進(jìn)行分析。

1 雙臂并聯(lián)機(jī)器人結(jié)構(gòu)

雙臂并聯(lián)機(jī)器人是一個(gè)閉環(huán)結(jié)構(gòu)，其機(jī)械結(jié)構(gòu)如圖1所示。主要由機(jī)架、伺服電機(jī)、驅(qū)動(dòng)關(guān)節(jié)、主動(dòng)臂、從動(dòng)臂、從動(dòng)關(guān)節(jié)和末端組成。其中，驅(qū)動(dòng)關(guān)節(jié)和伺服電機(jī)相連，通過(guò)控制驅(qū)動(dòng)關(guān)節(jié)的角度，使機(jī)器人臂末端按目標(biāo)軌跡和目標(biāo)速度移動(dòng)。

圖1 雙臂并聯(lián)機(jī)器人結(jié)構(gòu)圖Fig.1 Structure diagram of the two-armed parallel robot

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

雙臂并聯(lián)機(jī)器人參數(shù)化數(shù)學(xué)模型如圖2所示。圖2中，L是雙臂并聯(lián)機(jī)器人的主動(dòng)臂長(zhǎng)度，L是從動(dòng)臂長(zhǎng)度，是驅(qū)動(dòng)關(guān)節(jié)之間的距離，是末端關(guān)節(jié)之間的距離，左右驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)角分別為、，、是左右驅(qū)動(dòng)關(guān)節(jié)，、是左右從動(dòng)關(guān)節(jié)，、是左右末端關(guān)節(jié)，為末端中心，坐標(biāo)為（，）。

在圖2結(jié)構(gòu)中建立了2個(gè)坐標(biāo)系，分述如下：

圖2 雙臂并聯(lián)機(jī)器人參數(shù)化數(shù)學(xué)模型Fig.2 Parametric mathematical model of a two-armed parallel robot

（1）末端二維笛卡爾坐標(biāo)系（，）：以2個(gè)驅(qū)動(dòng)關(guān)節(jié)的中點(diǎn)為原點(diǎn)，即的中點(diǎn)。

（2）驅(qū)動(dòng)坐標(biāo)系（，）：為2個(gè)驅(qū)動(dòng)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度。當(dāng)2個(gè)主動(dòng)臂平展時(shí)，為零位（0，0），左主動(dòng)臂逆時(shí)針?lè)较驗(yàn)檎?，右主?dòng)臂順時(shí)針?lè)较驗(yàn)檎?/p>

則模型結(jié)構(gòu)關(guān)鍵點(diǎn)對(duì)應(yīng)的坐標(biāo)為：

2.1 雙臂并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正解

運(yùn)動(dòng)學(xué)正解就是已知機(jī)器人的2個(gè)驅(qū)動(dòng)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度（，），求解末端中心的對(duì)應(yīng)位置（，）。根據(jù)圖2幾何關(guān)系可建立2個(gè)二元二次方程（1）和（2），即：

為簡(jiǎn)化解的形式，設(shè)：

解得雙臂并聯(lián)機(jī)器人正解為：

因?yàn)闄C(jī)器人臂只在末端笛卡爾坐標(biāo)系的下半部分運(yùn)行，因此，正解中的正負(fù)號(hào)只取負(fù)號(hào)。

2.2 雙臂并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)逆解

運(yùn)動(dòng)學(xué)逆解是已知末端中心所處的位置（，），求解驅(qū)動(dòng)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度（，）。

令：

對(duì)式（1）進(jìn)行整理，得：

其中，

同理，令：

對(duì)式（2）整理可得：

其中，

3 雙臂并聯(lián)機(jī)器人的工作空間

機(jī)器人的工作空間是指末端中心在末端坐標(biāo)系平面中所能到達(dá)點(diǎn)的集合，即機(jī)器人臂的有效工作范圍。本文所述雙臂并聯(lián)機(jī)器人的實(shí)驗(yàn)平臺(tái)如圖3所示，其主要機(jī)械結(jié)構(gòu)和對(duì)應(yīng)實(shí)際參數(shù)，見(jiàn)表1。

圖3 雙臂并聯(lián)機(jī)器人Fig.3 Two-armed parallel robot

表1 雙臂并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)表Tab.1 Structural parameters of the two-armed parallel robot

在實(shí)際操作中，機(jī)器人臂的工作空間是根據(jù)驅(qū)動(dòng)關(guān)節(jié)旋轉(zhuǎn)角的實(shí)際可活動(dòng)范圍確定的，為防止機(jī)器人臂撞到機(jī)架，本文實(shí)驗(yàn)臺(tái)的驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)角范圍為［-30°，90°］。由此可得機(jī)器人臂工作空間，如圖4所示。

圖4 機(jī)器人工作空間Fig.4 Robot work space

4 雙臂并聯(lián)機(jī)器人的奇異性分析與預(yù)測(cè)方法

研究機(jī)器人運(yùn)動(dòng)發(fā)現(xiàn)，當(dāng)機(jī)器人臂處在某一特定位形時(shí)，可能會(huì)導(dǎo)致該機(jī)器人處于不可控的狀態(tài)。譬如，會(huì)出現(xiàn)末端線速度和驅(qū)動(dòng)關(guān)節(jié)角速度無(wú)法對(duì)應(yīng)，并且發(fā)生末端自由度數(shù)增加或減少的現(xiàn)象，稱為機(jī)器人的奇異性。因此，對(duì)給定機(jī)器人對(duì)象，判斷和預(yù)測(cè)該機(jī)器人結(jié)構(gòu)是否存在奇異位形是非常重要的環(huán)節(jié)。本文借鑒雅克比矩陣的行列式判定方法，針對(duì)雙臂并聯(lián)機(jī)器人結(jié)構(gòu)，從理論上對(duì)其處于奇異位形時(shí)的情形進(jìn)行分析，并提出奇異位形的預(yù)測(cè)方法。

4.1 雙臂并聯(lián)機(jī)器人的雅克比矩陣

將運(yùn)動(dòng)學(xué)方程（1）和（2）兩邊同時(shí)對(duì)時(shí)間求導(dǎo)，得式（3）和式（4）：

其中， 、 、 分別是方程（1）中、、對(duì)時(shí)間求導(dǎo)項(xiàng)的系數(shù)， 、 、 分別是方程（2）中、、對(duì)時(shí)間求導(dǎo)項(xiàng)的系數(shù)，具體為：

將方程（3）和（4）轉(zhuǎn)化為矩陣的形式，見(jiàn)式（5）：

令：

方程（5）就可寫(xiě)為式（6）：

其中，J和J都為22矩陣，稱為雙臂并聯(lián)機(jī)器人的雅克比矩陣。

通過(guò)式（6）還可以進(jìn)一步得出式（7）和式（8）：

式（7）是在已知驅(qū)動(dòng)關(guān)節(jié)角速度的情況下求出末端的線速度。式（8）是在已知末端線速度的情況下求出驅(qū)動(dòng)關(guān)節(jié)的角速度。

4.2 雙臂并聯(lián)機(jī)器人的奇異性理論分析

并聯(lián)機(jī)器人研究認(rèn)為，奇異性有3種類(lèi)型。對(duì)此擬展開(kāi)論述如下。

4.2.1 正運(yùn)動(dòng)學(xué)奇異性

根據(jù)雅克比矩陣行列式判定條件，當(dāng)發(fā)生正運(yùn)動(dòng)學(xué)奇異性時(shí)，會(huì)表現(xiàn)出如式（9）所示的特征：

式（9）可表述為當(dāng)雅克比矩陣J對(duì)應(yīng)的行列式為零，而J對(duì)應(yīng)行列式不為零同時(shí)發(fā)生時(shí)，機(jī)器人就處于正運(yùn)動(dòng)學(xué)奇異位形。

4.2.2 逆運(yùn)動(dòng)學(xué)奇異性

參考式（6）和雅克比矩陣行列式判定條件，發(fā)生逆運(yùn)動(dòng)學(xué)奇異性時(shí)，需滿足式（10）：

4.2.3 混合奇異性

當(dāng)雅克比矩陣J和J同時(shí)不滿秩，即滿足式（11）時(shí)，雙臂并聯(lián)機(jī)器人將發(fā)生混合奇異性：

當(dāng)雙臂并聯(lián)機(jī)器人處于此種奇異位形時(shí)，會(huì)同時(shí)出現(xiàn)正逆運(yùn)動(dòng)學(xué)奇異位形發(fā)生時(shí)的現(xiàn)象。

4.3 雙臂并聯(lián)機(jī)器人的奇異位形預(yù)測(cè)方法

為了能夠?qū)崿F(xiàn)對(duì)雙臂并聯(lián)機(jī)器人奇異位形的預(yù)測(cè)，根據(jù)式（9）、式（10），可得出雙臂并聯(lián)機(jī)器人處于正逆兩種奇異位形下的判定條件。

機(jī)器人處在正運(yùn)動(dòng)學(xué)奇異位形的判定條件，推得的數(shù)學(xué)公式具體如下：

其中，x、x、x、x是圖2中、、、在末端坐標(biāo)系中的橫坐標(biāo)，y、y、y、y是、、、的縱坐標(biāo)。

機(jī)器人處在逆運(yùn)動(dòng)學(xué)奇異位形的判定條件是，以下2個(gè)條件至少滿足一個(gè)：

5 實(shí)驗(yàn)和分析

5.1 實(shí)驗(yàn)平臺(tái)

為驗(yàn)證雙臂并聯(lián)機(jī)器人正逆解和奇異位形預(yù)測(cè)方法的正確性，在雙臂并聯(lián)機(jī)器人操作平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證，實(shí)現(xiàn)控制硬件平臺(tái)為Omron NJ控制器，軟件環(huán)境Sysmac Studio。

5.2 正逆解實(shí)驗(yàn)驗(yàn)證

本次實(shí)驗(yàn)分為2個(gè)部分：關(guān)鍵坐標(biāo)點(diǎn)數(shù)值驗(yàn)證和實(shí)際軌跡驗(yàn)證。

在驗(yàn)證機(jī)器人實(shí)際軌跡前，先進(jìn)行關(guān)鍵坐標(biāo)點(diǎn)數(shù)值的驗(yàn)證，并與實(shí)際末端中心位置對(duì)比，見(jiàn)表2。

表2 正逆解數(shù)值驗(yàn)證表Tab.2 Numerical verification table of positive and negative solutions

在關(guān)鍵坐標(biāo)點(diǎn)正逆解驗(yàn)證正確后，以一個(gè)實(shí)際工程使用的工作軌跡，驗(yàn)證整個(gè)機(jī)器人臂控制完整過(guò)程的正確性。該軌跡為一個(gè)食品生產(chǎn)流水線的包裝盒開(kāi)箱動(dòng)作，如圖5所示。實(shí)驗(yàn)證明，本文提出的正逆解方法是正確的，能夠通過(guò)驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)角的控制，正確地控制機(jī)器人末端運(yùn)動(dòng)軌跡。

圖5 機(jī)器人臂實(shí)際運(yùn)行軌跡圖Fig.5 Actual trajectory of robot arms

5.3 奇異位形的預(yù)測(cè)和實(shí)驗(yàn)分析驗(yàn)證

5.3.1 正運(yùn)動(dòng)學(xué)奇異位形的預(yù)測(cè)與失控分析

根據(jù)式（12）可對(duì)正運(yùn)動(dòng)學(xué)奇異位形進(jìn)行預(yù)測(cè)。當(dāng)、、、位于同一水平線，或者、、、形成一個(gè)矩形或平行四邊形時(shí)，滿足正運(yùn)動(dòng)學(xué)奇異位形的條件，得到的3種情況如圖6所示。

圖6 雙臂并聯(lián)機(jī)器人理論正奇異位形Fig.6 Theoretical positive singular dislocations of the two-armed parallel robot

在圖6（a）位形，2個(gè)從動(dòng)臂和末端完全展平時(shí)，驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)角（，）將無(wú)法控制末端中心坐標(biāo)（，）的移動(dòng)；在圖6（b）、（c）中的虛線位形，2個(gè)從動(dòng)臂形成了平行四邊形，可以自由移動(dòng)，相當(dāng)于多了一個(gè)不受控制的自由度，此時(shí)即使驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)角（，）確定情況下，末端中心坐標(biāo)（，）無(wú)法確定。所以這3種位形屬于正運(yùn)動(dòng)學(xué)奇異位形。

將實(shí)驗(yàn)并聯(lián)機(jī)器人的臂長(zhǎng)參數(shù)代入式（12），可得到驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)角（，）和其行列式值J的關(guān)系，三維曲面與行列式值det（J）0平面相交為一條曲線，如圖7所示。因此，可以判斷，正奇異位型不止一個(gè)點(diǎn)，而是相交曲線上的所有點(diǎn)，都為發(fā)生正奇異位形的點(diǎn)。

圖7 驅(qū)動(dòng)關(guān)節(jié)角度（θ1，θ2）和其行列式值Jx的關(guān)系Fig.7 Relationship between the driving joint angle（θ1，θ2）and its determinant value Jx

在實(shí)際的機(jī)器人實(shí)驗(yàn)臺(tái)上，可以很容易地?cái)[出正運(yùn)動(dòng)學(xué)奇異位形的形態(tài)，如圖8所示。正奇異位形不止一個(gè)位置，只要平行于，都是奇異位形。另外，實(shí)驗(yàn)機(jī)器人由于從動(dòng)臂長(zhǎng)度大于主動(dòng)臂長(zhǎng)度，可知、、、位于同一水平線的位形不存在；相應(yīng)地，在圖7中相交的曲線也未反映出此奇異位形的存在。說(shuō)明本文方法可以正確預(yù)測(cè)正運(yùn)動(dòng)學(xué)奇異位形的存在和姿態(tài)。

圖8 實(shí)驗(yàn)機(jī)器人存在的正奇異位形Fig.8 Positive singular dislocations in the presence of experimental robots

5.3.2 逆運(yùn)動(dòng)學(xué)奇異位形的預(yù)測(cè)與失控分析

根據(jù)雙臂并聯(lián)機(jī)器人逆奇異位形的判定條件，可以判斷當(dāng)、、位于同一條直線或者、、位于同一條直線，兩者至少有一個(gè)成立時(shí)，滿足逆運(yùn)動(dòng)學(xué)奇異位形的條件，即只要存在主動(dòng)臂和從動(dòng)臂處于伸直狀態(tài)，機(jī)器人就處于逆運(yùn)動(dòng)學(xué)奇異位形，如圖9所示。

對(duì)圖9中的逆奇異位形做相應(yīng)分析，當(dāng)主動(dòng)臂和從動(dòng)臂接近伸直狀態(tài)（呈180°），但還沒(méi)有完全伸直時(shí)，即如圖9（b）中的虛線部分，此時(shí)，機(jī)器人末端已經(jīng)接近工作空間的邊緣。但此時(shí)若要求機(jī)器人末端以的速度向外移動(dòng)到工作空間邊緣，分析可知，只要在工作空間內(nèi)，機(jī)器人末端肯定可以通過(guò)正逆解計(jì)算到達(dá)工作空間邊緣。然而，當(dāng)要求機(jī)器人末端以一個(gè)有限大的速度運(yùn)動(dòng)到工作空間邊緣時(shí)，驅(qū)動(dòng)關(guān)節(jié)需要執(zhí)行一個(gè)無(wú)窮大的角速度，顯然超過(guò)了電機(jī)驅(qū)動(dòng)的實(shí)際能力。即在此處逆奇異位形，機(jī)器人末端在方向的自由度消失了，原因是主動(dòng)臂角速度的切線方向與垂直，無(wú)法提供方向的運(yùn)動(dòng)。

圖9 雙臂并聯(lián)機(jī)器人逆奇異位形Fig.9 Two-armed parallel robots with inverse singular dislocations

6 結(jié)束語(yǔ)

本文針對(duì)雙臂并聯(lián)機(jī)器人機(jī)械結(jié)構(gòu)，提出了用于求取運(yùn)動(dòng)學(xué)正逆解數(shù)學(xué)模型的方法，實(shí)現(xiàn)通過(guò)驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)角度（，）控制機(jī)器人末端中心坐標(biāo)（，）的運(yùn)動(dòng)軌跡，并確定機(jī)器人臂的有效工作范圍；提出了雙臂并聯(lián)機(jī)器人正逆運(yùn)動(dòng)學(xué)奇異位形的預(yù)測(cè)方法，給出了明確的判定條件。實(shí)驗(yàn)證明了奇異位形存在性預(yù)測(cè)的正確性，分析了奇異位型所處的物理姿態(tài)和失控狀況。