六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)算法

朱齊丹 王欣璐

（哈爾濱工程大學(xué)，哈爾濱，150001）

六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)算法

朱齊丹 王欣璐

（哈爾濱工程大學(xué)，哈爾濱，150001）

根據(jù)D-H參數(shù)法確定六自由度機(jī)械臂的運(yùn)動(dòng)學(xué)方程，結(jié)合平面幾何法和歐拉角變換法將機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解問(wèn)題分為兩部分，一通過(guò)平面幾何法確定機(jī)械臂腕部點(diǎn)的坐標(biāo)與前三個(gè)關(guān)節(jié)角的關(guān)系，二通過(guò)歐拉角變換法確定機(jī)械臂末端姿態(tài)與后三個(gè)關(guān)節(jié)角的關(guān)系，根據(jù)逆運(yùn)動(dòng)解的選取原則從八組解中選取最優(yōu)解；利用MATLAB中的Robotics Toolbox建立機(jī)械臂的正運(yùn)動(dòng)學(xué)模型，通過(guò)多組位姿下的正逆運(yùn)動(dòng)解對(duì)比驗(yàn)證逆運(yùn)動(dòng)學(xué)求解算法的準(zhǔn)確性；利用VC++中的QueryPerformanceCounter函數(shù)和MATLAB中tic-toc語(yǔ)句得到不同算法所消耗的平均時(shí)間，通過(guò)消耗時(shí)間的對(duì)比說(shuō)明該算法的快速性；利用VC++編程實(shí)現(xiàn)機(jī)械臂寫(xiě)字的過(guò)程，通過(guò)對(duì)比輸入字的形狀與機(jī)械臂末端的實(shí)際運(yùn)動(dòng)軌跡，進(jìn)一步驗(yàn)證該算法是一種快速而準(zhǔn)確的逆運(yùn)動(dòng)學(xué)求解算法。

機(jī)器人，六自由度，機(jī)械臂，逆運(yùn)動(dòng)解，平面幾何法，歐拉角變換法

0 引言

機(jī)械臂被廣泛應(yīng)用于機(jī)械制造、航空航天、醫(yī)療和原子能等領(lǐng)域，機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題是其軌跡規(guī)劃與控制的重要基礎(chǔ)，逆運(yùn)動(dòng)學(xué)求解是否快速準(zhǔn)確將直接影響到機(jī)械臂軌跡規(guī)劃與控制的精度，因此針對(duì)工業(yè)中常用的六自由度機(jī)械臂，設(shè)計(jì)一種快速準(zhǔn)確的逆運(yùn)動(dòng)學(xué)求解方法是十分重要的。

目前，機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解方法主要有：迭代法、解析法和幾何法。迭代法雖然在大多數(shù)情況下是可行的，但卻無(wú)法得到全部解；解析法計(jì)算較為復(fù)雜，但可以得到全部根；幾何法針對(duì)機(jī)械臂的某些特殊結(jié)構(gòu)進(jìn)行簡(jiǎn)化，再進(jìn)行求解，雖然對(duì)于一般機(jī)械臂不通用，但是其形式簡(jiǎn)單，求解所需的計(jì)算量遠(yuǎn)遠(yuǎn)小于迭代法和解析法。Paul等[1]于1981年提出的解析算法對(duì)后來(lái)的機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題研究有著指導(dǎo)性意義。Regnier[2]于1997年提出一種基于迭代法和分布式的算法，能夠求出多種結(jié)構(gòu)的六自由度機(jī)械臂的位置逆解，但相應(yīng)的計(jì)算時(shí)間也會(huì)變長(zhǎng)。Jun等[3]于2009年提出將工作區(qū)速度輸入的控制問(wèn)題轉(zhuǎn)化為求解機(jī)器人逆運(yùn)動(dòng)學(xué)問(wèn)題，使機(jī)械臂的雅克比矩陣可以通過(guò)逆運(yùn)動(dòng)解快速生成，提高了水下遠(yuǎn)程操作機(jī)器人系統(tǒng)的工作效率。Rolland等[4]于2009年針對(duì)并聯(lián)機(jī)械臂提出了基于遺傳算法的優(yōu)化方法，將非線性方程組求解問(wèn)題轉(zhuǎn)化為逐個(gè)優(yōu)化的過(guò)程，建立了逆運(yùn)動(dòng)學(xué)模型。國(guó)內(nèi)有很多學(xué)者在機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題上也做出了相應(yīng)的貢獻(xiàn)[5-13]。

對(duì)于機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解問(wèn)題，還有很多新興的方法，例如人工神經(jīng)網(wǎng)絡(luò)[14]、Groebner基法[15]等，這些方法雖然在理論上滿(mǎn)足了相應(yīng)的要求，但所消耗的計(jì)算時(shí)間一般較長(zhǎng)，不能滿(mǎn)足工業(yè)機(jī)械臂控制中對(duì)于快速性的要求，因此在實(shí)際的工業(yè)機(jī)械臂控制中，很少會(huì)用到這些方法。

本文在逆運(yùn)動(dòng)解的求取上選擇了將幾何法和歐拉角變換法相結(jié)合，將六自由度機(jī)械臂逆運(yùn)動(dòng)解的求取分為兩部分：第一部分通過(guò)已知的機(jī)械臂末端位姿求出機(jī)械臂腕部點(diǎn)的坐標(biāo)，利用幾何法和機(jī)械臂腕部點(diǎn)的坐標(biāo)求出前三個(gè)關(guān)節(jié)轉(zhuǎn)過(guò)的角度；第二部分通過(guò)已經(jīng)求得的前三個(gè)關(guān)節(jié)軸轉(zhuǎn)過(guò)的角度和機(jī)械臂末端的姿態(tài)，利用歐拉角變換法求出后三個(gè)關(guān)節(jié)軸轉(zhuǎn)過(guò)的角度。該方法的計(jì)算過(guò)程與單獨(dú)使用解析法或者幾何法相比更簡(jiǎn)便，實(shí)現(xiàn)該算法所需的代碼長(zhǎng)度也小于其他方法所需的代碼長(zhǎng)度，因此該算法所消耗的計(jì)算時(shí)間相對(duì)較短。本文最后通過(guò)VC6.0實(shí)現(xiàn)了六自由度機(jī)械臂在給定平面上寫(xiě)字的過(guò)程，從而驗(yàn)證了本文所提出的逆運(yùn)動(dòng)學(xué)求解算法的準(zhǔn)確性和快速性。

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

利用D-H方法對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)建模已成為機(jī)械臂運(yùn)動(dòng)學(xué)問(wèn)題研究的標(biāo)準(zhǔn)方法。其基本思想為：對(duì)每個(gè)關(guān)節(jié)軸指定一個(gè)參考坐標(biāo)系；確定任意兩個(gè)相鄰坐標(biāo)系的關(guān)系；得出由機(jī)械臂末端執(zhí)行器至基坐標(biāo)系的總變換矩陣。

1.1 機(jī)械臂關(guān)節(jié)坐標(biāo)系的建立

六自由度機(jī)械臂如圖1所示。根據(jù)機(jī)械臂的結(jié)構(gòu)特點(diǎn)，使用D-H表示法確定各個(gè)旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系，定義機(jī)械臂的初始位置，然后建立基坐標(biāo)系以及各個(gè)關(guān)節(jié)軸的連桿坐標(biāo)系。由D-H表示法建立的坐標(biāo)系如圖2所示。

圖1 機(jī)械臂結(jié)構(gòu)圖

1.2 機(jī)械臂關(guān)節(jié)變換矩陣與正運(yùn)動(dòng)學(xué)方程

建立連桿坐標(biāo)系之后，根據(jù)相鄰連桿坐標(biāo)系確定機(jī)械臂的連桿參數(shù)表，機(jī)械臂的連桿參數(shù)與運(yùn)動(dòng)范圍如表1所示。表中，表示相鄰連桿繞公共軸線旋轉(zhuǎn)的夾角，表示從公垂線與關(guān)節(jié)軸的交點(diǎn)的有向距離，表示關(guān)節(jié)軸和關(guān)節(jié)軸之間公垂線的長(zhǎng)度，表示相鄰關(guān)節(jié)坐標(biāo)系之間的扭轉(zhuǎn)角，各個(gè)關(guān)節(jié)角均以逆時(shí)針旋轉(zhuǎn)方向?yàn)檎?。表中和的單位均為毫米?/p>

表1 ：機(jī)械臂的連桿參數(shù)表

圖2 各個(gè)關(guān)節(jié)軸的連桿坐標(biāo)系示意圖

根據(jù)相鄰連桿關(guān)節(jié)坐標(biāo)系間的齊次變換矩陣，可求出機(jī)械臂末端坐標(biāo)系相對(duì)于基坐標(biāo)系的變換矩陣。正運(yùn)動(dòng)學(xué)公式表示為

其中：

2 逆運(yùn)動(dòng)解的求解過(guò)程

逆運(yùn)動(dòng)學(xué)求解是根據(jù)給定的機(jī)械臂末端位置和姿態(tài)，求出與該位姿對(duì)應(yīng)的六個(gè)關(guān)節(jié)軸轉(zhuǎn)過(guò)的角度。該機(jī)械臂的后三個(gè)關(guān)節(jié)軸相交于一點(diǎn)，滿(mǎn)足Pieper準(zhǔn)則[16]，因此存在封閉解。

可以將機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解分為兩部分：第一部分是根據(jù)機(jī)械臂腕部點(diǎn)的坐標(biāo)求解前三個(gè)關(guān)節(jié)角；第二部分通過(guò)已經(jīng)求出的前三個(gè)關(guān)節(jié)角和給定的機(jī)械臂末端姿態(tài)矩陣求出后三個(gè)關(guān)節(jié)角。前三個(gè)關(guān)節(jié)角的求取采用平面幾何法，后三個(gè)關(guān)節(jié)角的求取采用歐拉角變換法。

2.1 前三個(gè)關(guān)節(jié)角的求取

由圖1所示的機(jī)械臂可知，此類(lèi)機(jī)械臂在結(jié)構(gòu)上有特殊性，即第一個(gè)關(guān)節(jié)軸在水平面內(nèi)旋轉(zhuǎn)，而第二個(gè)和第三個(gè)關(guān)節(jié)軸都是在豎直平面內(nèi)旋轉(zhuǎn)，因此，可以將機(jī)械臂腕部點(diǎn)分別投影到水平面和豎直平面上，利用平面幾何的方法找出前三個(gè)關(guān)節(jié)角與機(jī)械臂腕部坐標(biāo)的關(guān)系。

將機(jī)械臂分別向垂直于第二、三個(gè)關(guān)節(jié)軸的豎直平面和水平面投影，垂直于第二、三個(gè)關(guān)節(jié)軸的豎直平面投影圖如圖3所示，水平面投影圖如圖4所示。

由此可得到如下關(guān)系：

由式（4）可得

圖3 機(jī)械臂的豎直平面投影圖

圖4 機(jī)械臂的水平面投影圖

將式（2）和（3）進(jìn)行平方和可得

由式（6）可得

其中

2.2 后三個(gè)關(guān)節(jié)角的求取

后三個(gè)關(guān)節(jié)角的求取選擇歐拉角變換的方法，由已經(jīng)求得的前三個(gè)關(guān)節(jié)角可以得到，機(jī)械臂末端姿態(tài)矩陣與其他旋轉(zhuǎn)矩陣有如下關(guān)系：

由式（8）可得

其逆矩陣為本身，由此可得

2.3 逆運(yùn)動(dòng)學(xué)驗(yàn)證

利用MATLAB中的Robotics Toolbox建立六自由度機(jī)械臂的正運(yùn)動(dòng)學(xué)模型。根據(jù)機(jī)械臂的D-H連桿參數(shù)建立機(jī)械臂的正運(yùn)動(dòng)學(xué)方程，并繪制出機(jī)械臂的三維示意圖，如圖5所示。

圖5 Robotics Toolbox中的機(jī)械臂三維示意圖

表2：機(jī)械臂的正逆運(yùn)動(dòng)解

圖5所示的機(jī)械臂位姿為六自由度機(jī)械臂的初始位姿，通過(guò)Robotics Toolbox建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型后，就可以驗(yàn)證逆運(yùn)動(dòng)解的正確性了。

任意選取幾組關(guān)節(jié)角，將每組關(guān)節(jié)角輸入到機(jī)械臂的正運(yùn)動(dòng)學(xué)方程中，可以得到相應(yīng)的機(jī)械臂位姿；將機(jī)械臂的位姿輸入到VC6.0環(huán)境下的逆運(yùn)動(dòng)學(xué)算法中，可以得到各個(gè)位姿所對(duì)應(yīng)的六個(gè)關(guān)節(jié)角；將這些關(guān)節(jié)角與輸入到機(jī)械臂正運(yùn)動(dòng)學(xué)方程的關(guān)節(jié)角進(jìn)行對(duì)比，即可驗(yàn)證逆運(yùn)動(dòng)解的正確性。

表2左側(cè)為多組關(guān)節(jié)角下機(jī)械臂的運(yùn)動(dòng)學(xué)正解，表2右側(cè)為相對(duì)應(yīng)的末端位置和姿態(tài)下機(jī)械臂的運(yùn)動(dòng)學(xué)逆解。表2中沒(méi)有給出各組關(guān)節(jié)角所對(duì)應(yīng)的機(jī)械臂末端姿態(tài)矩陣，但實(shí)際上運(yùn)動(dòng)學(xué)正解會(huì)得到機(jī)械臂末端的姿態(tài)矩陣，運(yùn)動(dòng)學(xué)逆解的求解也需要該姿態(tài)矩陣，在此沒(méi)有列出。

通過(guò)對(duì)比表2的左側(cè)和右側(cè)的關(guān)節(jié)角可得，在誤差允許范圍內(nèi)，機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解算法是可靠而準(zhǔn)確的。

3 機(jī)械臂寫(xiě)字過(guò)程的實(shí)現(xiàn)

機(jī)械臂寫(xiě)字的過(guò)程是通過(guò)VC6.0編程實(shí)現(xiàn)的，如圖6所示。程序定時(shí)讀取在書(shū)寫(xiě)過(guò)程中所經(jīng)過(guò)點(diǎn)的坐標(biāo)，并將這些坐標(biāo)數(shù)據(jù)存入到一個(gè)文本中；機(jī)械臂在運(yùn)動(dòng)時(shí)，對(duì)該文本中的數(shù)據(jù)進(jìn)行處理，使其作為機(jī)械臂末端軌跡中間點(diǎn)的坐標(biāo)；在這些中間點(diǎn)間進(jìn)行多項(xiàng)式插值形成各個(gè)關(guān)節(jié)角的期望曲線，將曲線上的點(diǎn)定時(shí)發(fā)送到機(jī)械臂的運(yùn)動(dòng)控制器中，使機(jī)械臂末端按照期望軌跡進(jìn)行運(yùn)動(dòng)。

圖6 寫(xiě)字界面

機(jī)械臂的運(yùn)動(dòng)控制界面如圖7所示。首先連接運(yùn)動(dòng)控制器；連接成功后，按下讀取數(shù)據(jù)按鈕，此時(shí)程序?qū)奈谋局凶x取屏幕上字體的坐標(biāo)，并經(jīng)過(guò)處理后作為機(jī)械臂末端軌跡的中間點(diǎn)。在運(yùn)動(dòng)前還要輸入速度和姿態(tài)矩陣，速度是指機(jī)械臂末端的矢量速度，姿態(tài)矩陣是指機(jī)械臂末端在運(yùn)動(dòng)過(guò)程中所要保持的姿態(tài)。輸入速度和姿態(tài)矩陣后按下軌跡運(yùn)動(dòng)按鈕，機(jī)械臂末端將在一個(gè)指定的平面上按照屏幕上的字體軌跡運(yùn)動(dòng)。

在整個(gè)運(yùn)動(dòng)過(guò)程中，機(jī)械臂各個(gè)關(guān)節(jié)角度、角速度和末端的直角坐標(biāo)將時(shí)時(shí)發(fā)生變化，程序?qū)崟r(shí)讀取機(jī)械臂各個(gè)關(guān)節(jié)的角度和角速度，機(jī)械臂末端的直角坐標(biāo)通過(guò)其正運(yùn)動(dòng)學(xué)方程得到。

圖7 機(jī)械臂的控制界面

在本文實(shí)驗(yàn)中，速度設(shè)置為0.2（機(jī)械臂末端的矢量速度），姿態(tài)矩陣選擇單位陣，即在整個(gè)寫(xiě)字過(guò)程中保持機(jī)械臂末端是豎直向下的。設(shè)置完成后，按下軌跡運(yùn)動(dòng)按鈕，機(jī)械臂在水平面上寫(xiě)出屏幕上的字，整個(gè)運(yùn)動(dòng)過(guò)程如圖8所示。

將該算法的計(jì)算過(guò)程與單獨(dú)應(yīng)用幾何法或代數(shù)法相對(duì)比，應(yīng)用幾何法的VC代碼長(zhǎng)度約為260行，而該算法所需的代碼長(zhǎng)度約為80行，通過(guò)QueryPerformanceCounter函數(shù)，獲得高分辨率性能計(jì)數(shù)器的當(dāng)前值；通過(guò)算法程序末端的計(jì)數(shù)器值與算法程序初始的計(jì)數(shù)器值相減，得到不同算法的計(jì)算過(guò)程所消耗的時(shí)間；測(cè)試多組位姿下不同算法所消耗的時(shí)間，最終得到：應(yīng)用幾何法所消耗的平均時(shí)間為0.49ms，應(yīng)用本文中的算法所消耗的平均時(shí)間為0.27ms。在MATLAB中，利用tic-toc語(yǔ)句得到應(yīng)用解析法所消耗的平均時(shí)間為0.33s。通過(guò)對(duì)比各個(gè)算法所消耗的平均時(shí)間，可以看出，本文提出的算法在快速性上要優(yōu)于其他算法。

通過(guò)對(duì)比輸入字體的形狀與機(jī)械臂的實(shí)際運(yùn)動(dòng)軌跡，說(shuō)明該逆運(yùn)動(dòng)學(xué)求解算法在準(zhǔn)確性和快速性上都滿(mǎn)足了相應(yīng)的要求，能夠應(yīng)用于工業(yè)機(jī)械臂的實(shí)時(shí)控制中。

4 結(jié)論

針對(duì)一類(lèi)具有典型結(jié)構(gòu)的六自由度機(jī)械臂，在已知機(jī)械臂結(jié)構(gòu)參數(shù)的基礎(chǔ)上，采用D-H方法建立機(jī)械臂的連桿坐標(biāo)系，得到六個(gè)關(guān)節(jié)的坐標(biāo)變換矩陣，通過(guò)坐標(biāo)變換矩陣連乘得到機(jī)械臂的正運(yùn)動(dòng)學(xué)方程；采用幾何法和歐拉角變換法相結(jié)合的方法對(duì)機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題進(jìn)行求解，通過(guò)逆解的選取原則選出最優(yōu)解；利用MATLAB中的RoboticsToolbox驗(yàn)證逆運(yùn)動(dòng)學(xué)求解算法的正確性；最后，通過(guò)VC6.0編寫(xiě)機(jī)械臂寫(xiě)字程序，實(shí)現(xiàn)機(jī)械臂寫(xiě)字的過(guò)程。

圖8 機(jī)械臂的運(yùn)動(dòng)軌跡

本文提出的逆運(yùn)動(dòng)學(xué)求解算法結(jié)合了幾何法和歐拉角變換法，相比于單獨(dú)使用幾何法或代數(shù)法，該方法能使逆運(yùn)動(dòng)學(xué)的求解過(guò)程得到簡(jiǎn)化，通過(guò)VC中的QueryPerformanceCounter函數(shù)和MATLAB中的tic-toc語(yǔ)句得到不同算法所消耗的平均時(shí)間，經(jīng)過(guò)對(duì)比所消耗的平均時(shí)間可以得出：該算法是一種較為快速簡(jiǎn)便的逆運(yùn)動(dòng)學(xué)求解算法，能夠應(yīng)用于工業(yè)機(jī)械臂的實(shí)時(shí)控制中。

[1] Paul R P, Shimano B E, Mayer G.Kinemaics control equations for simple manipulators [J]. IEEE Transactions on Systems, Man and Cybernetics, 1981, 11(6): 449-445.

[2] Regnier S, Ouezdou F B, Bidaud P. Distributed method for inveres kinematies of all serial manipulators [J]. Mechanism and Machine Theory, 1997, 32(7): 855- 867.

[3] Jun B H, Shim H W, Lee P M, et al. Workspace control system of underwater teleoperated manipulators on RO Vs [C]. Proceedings of OCEANS 2009 EUROPE. Piscataway, NJ: IEEE, 2009: 1-6.

[4] Rolland L, Chandra R. Forward kinematics of the 6-6 general parallel manipulator using real coded genetic algorithms [C]. Proceedings of IEEE/ ASME International Conference on Advanced Intelligent Mechatronics. Piscataway, NJ: IEEE, 2009: 1637-1642.

[5] 李憲華, 郭永存, 張軍, 等. 模塊化六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)解算與驗(yàn)證[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào), 2013, 44(4): 246-251.

[6] 姜宏超, 劉士榮, 張波濤. 六自由度模塊化機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析[J]. 浙江大學(xué)學(xué)報(bào), 2010, 44(7): 1348-1354.

[7] 付榮, 居鶴華. 高精度解耦六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)解法[J]. 計(jì)算機(jī)測(cè)量與控制, 2010, 18(7): 1637-1640.

[8] 劉金存, 魯守銀, 姜振廷,等. 一種液壓機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的新算法[J]. 制造業(yè)自動(dòng)化, 2013, 35(6): 39-42.

[9] 廖鋒. 六自由度機(jī)械臂逆運(yùn)動(dòng)控制方法[J]. 計(jì)算機(jī)工程與科學(xué), 2013, 35(6): 174-179.

[10] 謝志江, 李誠(chéng), 劉楠, 等. 6自由度裝校機(jī)械人逆解的確定[J]. 吉林大學(xué)學(xué)報(bào)（工學(xué)版）, 2012, 42(6): 1563-1568.

[11] 呂世增, 張大衛(wèi), 劉海年. 基于吳方法的6R機(jī)器人逆運(yùn)動(dòng)學(xué)旋量方程求解[J]. 機(jī)械工程學(xué)報(bào), 2010, 46(17): 35-41.

[12] 焦有宙, 丁攀, 趙大旭. 溫室3P3R機(jī)械臂系統(tǒng)動(dòng)力學(xué)建模與分析[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào), 2012, 43(5): 179-183.

[13] 劉松國(guó), 朱世強(qiáng), 王宣銀. 基于矩陣分解的一般6R機(jī)器人實(shí)時(shí)高精度逆運(yùn)動(dòng)學(xué)算法[J]. 機(jī)械工程學(xué)報(bào), 2008, 44(11): 304-309.

[14] Jovanovic V T, Kazerounian K. Using chaos to obtain global solutions in computational kinematics[J]. Journal of Mechanical, 1998, 120: 299-304.

[15] 杭魯濱, 王彥. 基于Groebner基法的一般串聯(lián)6R機(jī)器人機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)分析[J]. 上海交通大學(xué)學(xué)報(bào), 2004, 38(6): 853-856.

[16] Craig J J. Introduction to Robotics [M]. 北京:機(jī)械工業(yè)出版社, 2006.

本項(xiàng)目獲國(guó)家自然科學(xué)基金資助，項(xiàng)目編號(hào)：61175089