黃 忠，劉可安

（株洲中車時(shí)代電氣股份有限公司， 湖南 株洲 412001）

0 引言

水下機(jī)器人是開(kāi)發(fā)海洋的重要裝備，其中作業(yè)型水下機(jī)器人常搭載水下機(jī)械臂進(jìn)行水下打撈、援潛救生、海底設(shè)施維護(hù)與裝置回收、海底生物及巖石標(biāo)本的采樣等方面的作業(yè)［1］。目前，廣泛使用的是液壓驅(qū)動(dòng)水下機(jī)械臂，但是水下液壓機(jī)械臂存在漏油、重量重、控制精度低、響應(yīng)速度慢等缺點(diǎn)［2］。水下電動(dòng)機(jī)械臂每個(gè)關(guān)節(jié)采用大轉(zhuǎn)矩伺服電機(jī)控制，可以完全克服水下液壓機(jī)械臂的缺點(diǎn)，并且水下電動(dòng)機(jī)械臂易于向智能化方向擴(kuò)展，因此水下電動(dòng)機(jī)械臂是未來(lái)的發(fā)展趨勢(shì)。

運(yùn)動(dòng)學(xué)模型是機(jī)械臂姿態(tài)控制的基礎(chǔ)，分為正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)。正運(yùn)動(dòng)學(xué)根據(jù)每個(gè)關(guān)節(jié)的當(dāng)前位置，計(jì)算機(jī)械臂末端位姿。逆運(yùn)動(dòng)學(xué)顧名思義即正運(yùn)動(dòng)學(xué)的逆過(guò)程，根據(jù)機(jī)械臂末端位姿，求解滿足該位姿條件下每個(gè)關(guān)節(jié)的位置［3］。建立機(jī)械臂運(yùn)動(dòng)學(xué)模型的方法有解析法和數(shù)值法。解析法直接推導(dǎo)求解正逆運(yùn)動(dòng)學(xué)方程，優(yōu)點(diǎn)是可以得到全部逆解，且求解速度快，缺點(diǎn)是運(yùn)動(dòng)學(xué)方程推導(dǎo)難度較大，不具有通用性。數(shù)值法通過(guò)迭代的方式求逆解，優(yōu)點(diǎn)是方法簡(jiǎn)單，通用性好，缺點(diǎn)是無(wú)法求得全部逆解，求解速度慢［4］。國(guó)內(nèi)外學(xué)者對(duì)機(jī)械臂的運(yùn)動(dòng)學(xué)已經(jīng)開(kāi)展了很多研究。Hawkins等人［5］采用解析法推導(dǎo)了UR5和UR10工業(yè)機(jī)械臂的運(yùn)動(dòng)學(xué)方程。羅貴成等人［6］基于改進(jìn)的D-H參數(shù)法建立了一種六自由度機(jī)械臂的運(yùn)動(dòng)學(xué)模型，并提出了一種逆解的選取方法。姜濤等人［7］將機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題轉(zhuǎn)化為目標(biāo)優(yōu)化問(wèn)題，基于改進(jìn)的粒子群算法搜索機(jī)械臂逆解。芮宏斌等人［8］提出了基于BP神經(jīng)網(wǎng)絡(luò)求解機(jī)械臂逆運(yùn)動(dòng)學(xué)的方法。裴香麗［9］對(duì)一種水下機(jī)械臂建立了運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型。

機(jī)械臂工作空間中存在一些奇異位置［10］，當(dāng)機(jī)械臂位于奇異位置時(shí)，逆運(yùn)動(dòng)學(xué)方程無(wú)解，即無(wú)法通過(guò)逆運(yùn)動(dòng)學(xué)模型計(jì)算關(guān)節(jié)位置，并且此時(shí)機(jī)械臂末端的微小移動(dòng)可能導(dǎo)致關(guān)節(jié)的大幅運(yùn)動(dòng)，從而導(dǎo)致機(jī)械臂失控，甚至機(jī)毀人亡的事故［11］。因此必須對(duì)機(jī)械臂的奇異位置進(jìn)行分析，并且在機(jī)械臂的運(yùn)動(dòng)過(guò)程中對(duì)奇異位置進(jìn)行規(guī)避，從而保證機(jī)械臂運(yùn)動(dòng)安全性。張倩玉等人［12］對(duì)桌面型607串聯(lián)機(jī)械臂進(jìn)行了工作空間和奇異位形分析。唐家豪［13］基于雅可比矩陣分析了一種七自由度機(jī)械臂的奇異位置。本文采用解析法分析水下電動(dòng)機(jī)械臂的運(yùn)動(dòng)學(xué)模型，并詳細(xì)分析了水下電動(dòng)機(jī)械臂的奇異性，提出了規(guī)避奇異位置的方法。

1 運(yùn)動(dòng)學(xué)模型

本文研究的水下電動(dòng)機(jī)械臂的三維模型如圖1所示，其包含6個(gè)關(guān)節(jié)，所有關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)，每個(gè)關(guān)節(jié)通過(guò)一個(gè)伺服電機(jī)驅(qū)動(dòng)。建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型首先需要建立坐標(biāo)系。圖2顯示了整個(gè)機(jī)械臂的坐標(biāo)系，坐標(biāo)系的建立方法服從改進(jìn)的D-H（Denavit-Hartenberg）參數(shù)［10］對(duì)坐標(biāo)系的要求，O代表坐標(biāo)原點(diǎn)。根據(jù)坐標(biāo)系獲得該水下電動(dòng)機(jī)械臂的改進(jìn)D-H參數(shù)，如表1所示。表中i為關(guān)節(jié)索引，αi-1，ai-1，di，θi分別為連桿扭角、連桿長(zhǎng)度、偏距和關(guān)節(jié)旋轉(zhuǎn)角。a1～a4，d6，θ22，θ33，θ44的定義如圖2所示，a1為O1和O2之間的距離，a2為O2和O3之間的距離，a3為O3和O4之間的距離，a4為O4和O5之間的距離，d6為O5和O6之間的距離，θ22為線段O2O3與豎直線之間的銳角，θ33為線段O2O3與線段O3O4之間的鈍角，θ44為線段O3O4與豎直線之間的鈍角。

圖1 水下電動(dòng)機(jī)械臂三維模型Fig.1 Three-dimensional model of the underwater electric manipulator

圖2 機(jī)械臂坐標(biāo)系Fig.2 Coordinate system of the manipulator

1.1 正運(yùn)動(dòng)學(xué)模型

已知每個(gè)關(guān)節(jié)的位置q=[θ1，θ2，θ3，θ4，θ5，θ6]T，求解機(jī)械臂末端相對(duì)于基坐標(biāo)系O0的位姿代表末端坐標(biāo)系O6相對(duì)于基坐標(biāo)系O0的齊次變換矩陣。矩陣中的n=[nx，ny，nz]T，o=[ox，oy，oz]T，a=[ax，ay，az]T代表機(jī)械臂末端姿態(tài)，p=[px，py，pz]T代表末端位置。求解正運(yùn)動(dòng)學(xué)模型，需要使用到坐標(biāo)系間通用的齊次變換矩陣［10］。

基于式（1）和D-H參數(shù)，可以計(jì)算得到：

因此

1.2 逆運(yùn)動(dòng)學(xué)模型

從圖2中可以看出該機(jī)械臂的關(guān)節(jié)2、關(guān)節(jié)3和關(guān)節(jié)4 的旋轉(zhuǎn)軸相互平行，因此該機(jī)械臂滿足pieper 準(zhǔn)則［14］，具有逆運(yùn)動(dòng)學(xué)解析解。對(duì)于逆運(yùn)動(dòng)學(xué)問(wèn)題，機(jī)械臂末端相對(duì)于基坐標(biāo)的位姿是已知的。

由于關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)4 的旋轉(zhuǎn)軸相互平行，所以坐標(biāo)系O4的單位x軸、y軸和z軸在坐標(biāo)系O1的y軸上的投影恒為［0， 0， 1］T，對(duì)應(yīng)于的第二行前三個(gè)元素恒為［0， 0， 1］T。坐標(biāo)系O4的原點(diǎn)在坐標(biāo)系O1的y坐標(biāo)恒為0，如的第四列第二行元素所示。逆運(yùn)動(dòng)學(xué)的求解過(guò)程主要是利用了這些幾何特征。該機(jī)械臂逆運(yùn)動(dòng)學(xué)求解過(guò)程首先求解關(guān)節(jié)1、關(guān)節(jié)5和關(guān)節(jié)6的旋轉(zhuǎn)角，再求解關(guān)節(jié)2、關(guān)節(jié)3和關(guān)節(jié)4的旋轉(zhuǎn)角。

1.2.1 關(guān)節(jié)1、關(guān)節(jié)5和關(guān)節(jié)6的旋轉(zhuǎn)角

根據(jù)式（9）和上述的正運(yùn)動(dòng)學(xué)公式，可以得到：

式（10）的左邊為

其中，σ3=θ2+θ3+θ4+θ22-θ33-θ44。

式（10）的右邊為

其 中 ，σ4=oycos(θ1)-oxsin(θ1)；σ5=oxcos(θ1)+oysin(θ1)；σ6=nycos(θ1)-nxsin(θ1)；σ7=nxcos(θ1)+nysin(θ1)；σ8=aycos(θ1)-axsin(θ1)；σ9=axcos(θ1)+aysin(θ1)。

式（10）左右兩邊第二行對(duì)應(yīng)元素相等，可以得到：

根據(jù)式（11）的第一個(gè)等式，可以得到：

因此可以得到關(guān)節(jié)1旋轉(zhuǎn)角θ1：

其中atan2是正切逆函數(shù)，根據(jù)輸入的兩個(gè)參數(shù)的符號(hào)確定輸出角度的符號(hào)，θ1有兩個(gè)解。

根據(jù)式（11）的第三項(xiàng)，可以得到關(guān)節(jié)5旋轉(zhuǎn)角θ5：

根據(jù)式（11）的第四項(xiàng)，可以得到關(guān)節(jié)6旋轉(zhuǎn)角θ6：

1.2.2 關(guān)節(jié)2、關(guān)節(jié)3和關(guān)節(jié)4的旋轉(zhuǎn)角

已經(jīng)求解得到θ1，θ5和θ6，在式（10）的左右兩邊同時(shí)右乘()-1：

因?yàn)棣?，θ5，θ6已知，所以式（16）右邊的所有元素都已知。令代表的第一行第四列的元素。x，y的具體計(jì)算如下：

其 中 ，σ10=nxcos(θ1) cos(θ6)+nycos(θ6) sin(θ1)-oxcos(θ1) sin(θ6)-oysin(θ1) sin(θ6)；σ11=axcos(θ1)+aysin(θ1)。

根據(jù)式（9）和式（16）右邊矩陣第四列第二行和第三行元素相等，可以得到：

將上面兩個(gè)方程平方后相加得到：

因此可以得到關(guān)節(jié)3旋轉(zhuǎn)角θ3：

根據(jù)式（17），令：s2=sin(θ2+θ22)；s3=sin(θ3-θ33)；c2=cos(θ2+θ22)；c3=cos(θ3-θ33)。

因此，式（17）可以轉(zhuǎn)換成如下形式：

令：a=a2+a3c3；b=a3s3；c=-y；d=a1-x。

因此，式（19）可以轉(zhuǎn)換成如下形式：

根據(jù)式（20）可以得到關(guān)節(jié)2旋轉(zhuǎn)角θ2：

根據(jù)式（9）和式（16），可以得到：

因此，可以得到關(guān)節(jié)4旋轉(zhuǎn)角θ4：

2 雅可比矩陣

機(jī)械臂的雅克比矩陣可將機(jī)械臂工作空間運(yùn)動(dòng)映射到關(guān)節(jié)空間運(yùn)動(dòng)［10］，如下式所示。

式中：?——機(jī)械臂工作空間各個(gè)維度的運(yùn)動(dòng)速度，其一般是一個(gè)6維列向量，前3個(gè)元素代表移動(dòng)速度，后3個(gè)元素代表旋轉(zhuǎn)角速度? ——機(jī)械臂每個(gè)關(guān)節(jié)的速度；J——雅克比矩陣，其行數(shù)等于機(jī)械臂工作空間運(yùn)動(dòng)維度，一般等于6，列數(shù)等于機(jī)械臂關(guān)節(jié)數(shù)量。

本文中的水下電動(dòng)機(jī)械臂有6 個(gè)關(guān)節(jié)，因此其雅克比矩陣是一個(gè)6×6的方陣。

當(dāng)機(jī)械臂位于奇異位置時(shí)，雅克比矩陣的行列式等于零，因此雅克比矩陣可以用來(lái)判斷機(jī)械臂是否位于奇異位置，并可用于奇異位置規(guī)避。下面將推導(dǎo)出該水下電動(dòng)機(jī)械臂的雅克比矩陣，為機(jī)械臂的奇異性分析提供理論基礎(chǔ)。

推導(dǎo)機(jī)械臂的雅克比矩陣需要采用兩個(gè)遞推公式［10］：

式中：——坐標(biāo)系Oi的角速度；——坐標(biāo)系Oi到坐標(biāo)系Oi+1的旋轉(zhuǎn)矩陣，其可以從式（2）～式（7）獲得；——第i+1 個(gè)關(guān)節(jié)的角速度?——坐標(biāo)系Oi+1的z軸的單位向量［0， 0， 1］T——坐標(biāo)系Oi的線速度——坐標(biāo)系Oi+1的原點(diǎn)在坐標(biāo)系Oi上的位置向量。

在已知?的情況下，可以很容易寫出雅克比矩陣J。注意，此時(shí)的雅克比矩陣是相對(duì)于機(jī)械臂的末端坐標(biāo)系的?？梢酝ㄟ^(guò)矩陣變換將雅克比矩陣轉(zhuǎn)換到相對(duì)于基坐標(biāo)系的。不過(guò)對(duì)于奇異位置分析，相對(duì)于末端坐標(biāo)系和相對(duì)于基坐標(biāo)系的雅克比矩陣得出的結(jié)果是一樣的。

3 奇異性分析

機(jī)械臂在奇異位置時(shí)，逆運(yùn)動(dòng)學(xué)方程無(wú)解，并且機(jī)械臂工作空間內(nèi)的一點(diǎn)微小變化可能會(huì)引起關(guān)節(jié)角度的劇烈變化，因此需要獲得奇異位置的直觀表達(dá)，避免機(jī)器人失控。

從該水下電動(dòng)機(jī)械臂的逆運(yùn)動(dòng)學(xué)方程中，可以分析出該機(jī)械臂存在3個(gè)奇異位置：

1） 肩部奇異位置。根據(jù)式（12）、式（13）可知，當(dāng)px-d6ax=0 時(shí)，θ1無(wú)法求解，所以該位置是一個(gè)奇異位置。

2） 肘部奇異位置。根據(jù)式（21）可知，當(dāng)ac+bd=0時(shí)，θ2無(wú)法求解，此時(shí)關(guān)節(jié)2、關(guān)節(jié)3和關(guān)節(jié)4的旋轉(zhuǎn)軸共面。

3） 腕部奇異位置。根據(jù)式（15）可知，當(dāng)θ5=±90°時(shí)，θ6無(wú)法求解，導(dǎo)致機(jī)構(gòu)失控。

可以對(duì)機(jī)械臂的雅克比矩陣進(jìn)行奇異值分解［15］：

式中：U∈Rm×m，V∈Rn×n，Σ∈Rm×n，U和V為正交矩陣，n為關(guān)節(jié)數(shù)，m為機(jī)械臂工作空間運(yùn)動(dòng)維度；λi——雅克比矩陣第i個(gè)特征值，且λ1≥λ2≥…≥λm≥0。

可以采用條件數(shù)K來(lái)衡量機(jī)械臂的靈活性［16］，條件數(shù)越大，說(shuō)明機(jī)械臂可操作性越弱，越接近奇異位置。條件數(shù)可采用下式計(jì)算：

4 仿真驗(yàn)證

4.1 運(yùn)動(dòng)學(xué)模型驗(yàn)證

目前已經(jīng)獲得水下電動(dòng)機(jī)械臂的正、逆運(yùn)動(dòng)學(xué)解析解，為了驗(yàn)證所建立的運(yùn)動(dòng)學(xué)模型的準(zhǔn)確性，本文通過(guò)文獻(xiàn)［17］開(kāi)發(fā)的Robotics Toolbox 建立該水下電動(dòng)機(jī)械臂的仿真模型，如圖3所示。Robotics Toolbox是一個(gè)免費(fèi)開(kāi)源的機(jī)器人工具箱?？梢圆捎闷渲械膄kine函數(shù)和ikine函數(shù)求解機(jī)械臂的正逆運(yùn)動(dòng)學(xué)參數(shù)。本文將通過(guò)幾個(gè)測(cè)試?yán)觼?lái)對(duì)比本文構(gòu)建的運(yùn)動(dòng)學(xué)模型計(jì)算結(jié)果與機(jī)器人工具箱的運(yùn)動(dòng)學(xué)計(jì)算結(jié)果，從而驗(yàn)證本文的運(yùn)動(dòng)學(xué)模型的準(zhǔn)確性。

圖3 水下電動(dòng)機(jī)械臂仿真模型Fig.3 Simulation model of the underwater electric manipulator

4.1.1 測(cè)試1

關(guān)節(jié)旋轉(zhuǎn)角為［0， 0， 0， 0， 0， 0］。

4.1.1.1 正運(yùn)動(dòng)學(xué)

機(jī)器人工具箱計(jì)算的末端位姿為

本文的運(yùn)動(dòng)學(xué)模型計(jì)算的末端位姿為

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

將計(jì)算得到的末端位姿作為輸入，機(jī)器人工具箱計(jì)算的關(guān)節(jié)旋轉(zhuǎn)角為［0， 0， 0， 0， 0， 0］。

本文的運(yùn)動(dòng)學(xué)模型計(jì)算的關(guān)節(jié)旋轉(zhuǎn)角為

4.1.2 測(cè)試2

關(guān)節(jié)旋轉(zhuǎn)角為［10，20，30，40，50，60］。

4.1.2.1 正運(yùn)動(dòng)學(xué)

機(jī)器人工具箱計(jì)算的末端位姿為

本文的運(yùn)動(dòng)學(xué)模型計(jì)算的末端位姿為

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

機(jī)器人工具箱計(jì)算的關(guān)節(jié)旋轉(zhuǎn)角為［10，20，30，40，50，60］。

本文的運(yùn)動(dòng)學(xué)模型計(jì)算的關(guān)節(jié)旋轉(zhuǎn)角為［10，20，30，40，50，60］，［10.000 0， -31.749 8， 148.768 9，-27.019 0， 50.000 0， 60.000 0］，［-170.000 0， 25.604 4，38.229 1， -153.833 5， -50.000 0， -120.000 0］，［-170.000 0， -19.139 6， 140.539 8， 148.599 8，-50.000 0， -120.000 0］。

4.1.3 測(cè)試3

關(guān)節(jié)旋轉(zhuǎn)角為［10，-20，30，-40，50，-60］。

4.1.3.1 正運(yùn)動(dòng)學(xué)

機(jī)器人工具箱計(jì)算的末端位姿為

本文的運(yùn)動(dòng)學(xué)模型計(jì)算的末端位姿為

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

機(jī)器人工具箱計(jì)算的關(guān)節(jié)旋轉(zhuǎn)角為

本文的運(yùn)動(dòng)學(xué)模型計(jì)算的關(guān)節(jié)旋轉(zhuǎn)角為［10.000 0，-20.000 0， 30.000 0， -40.000 0， 50.000 0， -60.000 0］，［10.000 0， -71.749 8， 148.768 9， -107.019 0， 50.000 0，-60.000 0］。

4.1.4 驗(yàn)證結(jié)論

通過(guò)對(duì)比可以看出，本文構(gòu)建的水下機(jī)械臂運(yùn)動(dòng)學(xué)模型是準(zhǔn)確的，并且逆運(yùn)動(dòng)學(xué)方程可以獲得所有滿足末端位姿的關(guān)節(jié)旋轉(zhuǎn)角。圖4 顯示了基于所構(gòu)建的運(yùn)動(dòng)學(xué)模型進(jìn)行水下機(jī)械臂笛卡爾空間的軌跡規(guī)劃，可以控制水下機(jī)械臂末端沿著圖中的紅色軌跡運(yùn)動(dòng)。

圖4 笛卡爾路徑規(guī)劃Fig.4 Cartesian path planning

4.2 奇異位置規(guī)避驗(yàn)證

根據(jù)奇異位置分析可知，當(dāng)關(guān)節(jié)5 旋轉(zhuǎn)角為±90°時(shí)，機(jī)械臂處于奇異位置。圖5顯示了關(guān)節(jié)5旋轉(zhuǎn)角從-80°變化到80°的范圍內(nèi)，機(jī)械臂條件數(shù)的變化情況，此時(shí)其他關(guān)節(jié)位置保持為0°。因?yàn)楫?dāng)關(guān)節(jié)5 旋轉(zhuǎn)角等于±90°時(shí)，雅克比矩陣的最小特征值等于零，條件數(shù)為無(wú)窮大，所以沒(méi)有顯示在圖5中。從圖5可以看出關(guān)節(jié)5旋轉(zhuǎn)角等于0°時(shí)，條件數(shù)最小，此時(shí)機(jī)械臂的可操作性最強(qiáng)。隨著關(guān)節(jié)5旋轉(zhuǎn)角增大或減小，條件數(shù)呈指數(shù)級(jí)的增長(zhǎng)。這說(shuō)明機(jī)械臂的可操作性隨著關(guān)節(jié)5旋轉(zhuǎn)角的增大或減少而迅速減弱，且隨著關(guān)節(jié)5旋轉(zhuǎn)角的增大或減小而迅速接近奇異位置。因此在實(shí)際控制機(jī)械臂運(yùn)動(dòng)過(guò)程中，可以采用條件數(shù)來(lái)判斷機(jī)械臂當(dāng)前關(guān)節(jié)狀態(tài)是否接近奇異位置，從而有效規(guī)避奇異位置，保證機(jī)械臂操控的安全性。

圖5 關(guān)節(jié)5 旋轉(zhuǎn)角與條件數(shù)的關(guān)系Fig.5 Relationship between the rotation angle of Joint 5 and the condition number

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

針對(duì)水下電動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)模型與結(jié)構(gòu)強(qiáng)相關(guān)的問(wèn)題以及奇異位置規(guī)避問(wèn)題，本文采用改進(jìn)的D-H參數(shù)法建立了一種水下六自由度電動(dòng)機(jī)械臂的運(yùn)動(dòng)學(xué)模型，推導(dǎo)了機(jī)械臂的正、逆運(yùn)動(dòng)學(xué)方程；求得了機(jī)械臂的雅可比矩陣，并基于雅可比矩陣詳細(xì)分析了水下電動(dòng)機(jī)械臂的奇異位置，采用條件數(shù)判斷機(jī)械臂當(dāng)前狀態(tài)是否接近奇異位置。仿真實(shí)驗(yàn)結(jié)果表明本文建立的運(yùn)動(dòng)學(xué)模型可以根據(jù)關(guān)節(jié)位置精確計(jì)算機(jī)械臂末端位姿，也可以根據(jù)末端位姿反解機(jī)械臂關(guān)節(jié)位置；且當(dāng)機(jī)械臂接近奇異位置時(shí)，條件數(shù)會(huì)呈指數(shù)級(jí)增加，因此可以采用條件數(shù)進(jìn)行奇異位置規(guī)避。

目前本文只通過(guò)仿真的方法來(lái)驗(yàn)證所建立的水下電動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)模型和奇異位置規(guī)避方法，后續(xù)將在真實(shí)的水下電動(dòng)機(jī)械臂上進(jìn)行更充分全面的驗(yàn)證，從而保障水下電動(dòng)機(jī)械臂作業(yè)的安全性。