一種水下電動機械臂運動學與奇異性分析

黃 忠，劉可安

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

0 引言

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

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

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

1 運動學模型

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

已知每個關節的位置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代表機械臂末端姿態，p=[px，py，pz]T代表末端位置。求解正運動學模型，需要使用到坐標系間通用的齊次變換矩陣［10］。

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

因此

1.2 逆運動學模型

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

由于關節2、關節3 和關節4 的旋轉軸相互平行，所以坐標系O4的單位x軸、y軸和z軸在坐標系O1的y軸上的投影恒為［0， 0， 1］T，對應于的第二行前三個元素恒為［0， 0， 1］T。坐標系O4的原點在坐標系O1的y坐標恒為0，如的第四列第二行元素所示。逆運動學的求解過程主要是利用了這些幾何特征。該機械臂逆運動學求解過程首先求解關節1、關節5和關節6的旋轉角，再求解關節2、關節3和關節4的旋轉角。

1.2.1 關節1、關節5和關節6的旋轉角

根據式（9）和上述的正運動學公式，可以得到：

式（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）左右兩邊第二行對應元素相等，可以得到：

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

因此可以得到關節1旋轉角θ1：

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

根據式（11）的第三項，可以得到關節5旋轉角θ5：

根據式（11）的第四項，可以得到關節6旋轉角θ6：

1.2.2 關節2、關節3和關節4的旋轉角

已經求解得到θ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)。

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

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

因此可以得到關節3旋轉角θ3：

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

因此，式（17）可以轉換成如下形式：

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

因此，式（19）可以轉換成如下形式：

根據式（20）可以得到關節2旋轉角θ2：

根據式（9）和式（16），可以得到：

因此，可以得到關節4旋轉角θ4：

2 雅可比矩陣

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

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

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

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

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

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

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

3 奇異性分析

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

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

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

2） 肘部奇異位置。根據式（21）可知，當ac+bd=0時，θ2無法求解，此時關節2、關節3和關節4的旋轉軸共面。

3） 腕部奇異位置。根據式（15）可知，當θ5=±90°時，θ6無法求解，導致機構失控。

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

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

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

4 仿真驗證

4.1 運動學模型驗證

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

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

4.1.1 測試1

關節旋轉角為［0， 0， 0， 0， 0， 0］。

4.1.1.1 正運動學

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

本文的運動學模型計算的末端位姿為

4.1.1.2 逆運動學

將計算得到的末端位姿作為輸入，機器人工具箱計算的關節旋轉角為［0， 0， 0， 0， 0， 0］。

本文的運動學模型計算的關節旋轉角為

4.1.2 測試2

關節旋轉角為［10，20，30，40，50，60］。

4.1.2.1 正運動學

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

本文的運動學模型計算的末端位姿為

4.1.2.2 逆運動學

機器人工具箱計算的關節旋轉角為［10，20，30，40，50，60］。

本文的運動學模型計算的關節旋轉角為［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

關節旋轉角為［10，-20，30，-40，50，-60］。

4.1.3.1 正運動學

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

本文的運動學模型計算的末端位姿為

4.1.3.2 逆運動學

機器人工具箱計算的關節旋轉角為

本文的運動學模型計算的關節旋轉角為［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 驗證結論

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

圖4 笛卡爾路徑規劃Fig.4 Cartesian path planning

4.2 奇異位置規避驗證

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

圖5 關節5 旋轉角與條件數的關系Fig.5 Relationship between the rotation angle of Joint 5 and the condition number

5 結束語

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

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