黃 忠，劉可安

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

0 引言

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

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

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

1 運動學(xué)模型

本文研究的水下電動機械臂的三維模型如圖1所示，其包含6個關(guān)節(jié)，所有關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)，每個關(guān)節(jié)通過一個伺服電機驅(qū)動。建立機械臂的運動學(xué)模型首先需要建立坐標系。圖2顯示了整個機械臂的坐標系，坐標系的建立方法服從改進的D-H（Denavit-Hartenberg）參數(shù)［10］對坐標系的要求，O代表坐標原點。根據(jù)坐標系獲得該水下電動機械臂的改進D-H參數(shù)，如表1所示。表中i為關(guān)節(jié)索引，αi-1，ai-1，di，θi分別為連桿扭角、連桿長度、偏距和關(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 水下電動機械臂三維模型Fig.1 Three-dimensional model of the underwater electric manipulator

圖2 機械臂坐標系Fig.2 Coordinate system of the manipulator

1.1 正運動學(xué)模型

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

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

因此

1.2 逆運動學(xué)模型

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

由于關(guān)節(jié)2、關(guān)節(jié)3 和關(guān)節(jié)4 的旋轉(zhuǎn)軸相互平行，所以坐標系O4的單位x軸、y軸和z軸在坐標系O1的y軸上的投影恒為［0， 0， 1］T，對應(yīng)于的第二行前三個元素恒為［0， 0， 1］T。坐標系O4的原點在坐標系O1的y坐標恒為0，如的第四列第二行元素所示。逆運動學(xué)的求解過程主要是利用了這些幾何特征。該機械臂逆運動學(xué)求解過程首先求解關(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）和上述的正運動學(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）左右兩邊第二行對應(yīng)元素相等，可以得到：

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

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

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

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

根據(jù)式（11）的第四項，可以得到關(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）的左右兩邊同時右乘()-1：

因為θ1，θ5，θ6已知，所以式（16）右邊的所有元素都已知。令代表的第一行第四列的元素。x，y的具體計算如下：

其 中 ，σ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）右邊矩陣第四列第二行和第三行元素相等，可以得到：

將上面兩個方程平方后相加得到：

因此可以得到關(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 雅可比矩陣

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

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

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

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

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

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

在已知?的情況下，可以很容易寫出雅克比矩陣J。注意，此時的雅克比矩陣是相對于機械臂的末端坐標系的。可以通過矩陣變換將雅克比矩陣轉(zhuǎn)換到相對于基坐標系的。不過對于奇異位置分析，相對于末端坐標系和相對于基坐標系的雅克比矩陣得出的結(jié)果是一樣的。

3 奇異性分析

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

從該水下電動機械臂的逆運動學(xué)方程中，可以分析出該機械臂存在3個奇異位置：

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

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

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

可以對機械臂的雅克比矩陣進行奇異值分解［15］：

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

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

4 仿真驗證

4.1 運動學(xué)模型驗證

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

圖3 水下電動機械臂仿真模型Fig.3 Simulation model of the underwater electric manipulator

4.1.1 測試1

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

4.1.1.1 正運動學(xué)

機器人工具箱計算的末端位姿為

本文的運動學(xué)模型計算的末端位姿為

4.1.1.2 逆運動學(xué)

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

本文的運動學(xué)模型計算的關(guān)節(jié)旋轉(zhuǎn)角為

4.1.2 測試2

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

4.1.2.1 正運動學(xué)

機器人工具箱計算的末端位姿為

本文的運動學(xué)模型計算的末端位姿為

4.1.2.2 逆運動學(xué)

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

本文的運動學(xué)模型計算的關(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 測試3

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

4.1.3.1 正運動學(xué)

機器人工具箱計算的末端位姿為

本文的運動學(xué)模型計算的末端位姿為

4.1.3.2 逆運動學(xué)

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

本文的運動學(xué)模型計算的關(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 驗證結(jié)論

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

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

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

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

圖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é)束語

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

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