基于空間六自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)數(shù)值解法

張栩曼，張中哲，王燕波，楊 濤，鄧 濤

?

基于空間六自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)數(shù)值解法

張栩曼，張中哲，王燕波，楊 濤，鄧 濤

（北京精密機(jī)電控制設(shè)備研究所，北京，100076）

針對(duì)解析法只能用于特殊構(gòu)型的機(jī)械臂且求解過(guò)程復(fù)雜、計(jì)算量大的問(wèn)題，提出一種適用于大多數(shù)串聯(lián)型機(jī)械臂的數(shù)值迭代逆運(yùn)動(dòng)學(xué)求解方法。利用該方法，求解自主研發(fā)的空間六自由度機(jī)械臂的運(yùn)動(dòng)學(xué)逆解，并通過(guò)仿真證明該方法的可靠性。

空間機(jī)械臂；數(shù)值迭代；正逆運(yùn)動(dòng)學(xué)

0 引 言

機(jī)械臂正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)的求解是機(jī)械臂實(shí)際控制中需要解決的基本問(wèn)題。其中逆運(yùn)動(dòng)學(xué)的求解比正運(yùn)動(dòng)學(xué)的求解更加復(fù)雜，對(duì)于不同的機(jī)械臂構(gòu)型沒有標(biāo)準(zhǔn)的求解方法和步驟。一般機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解方法主要有解析法、數(shù)值迭代法、幾何法等。

解析法的優(yōu)勢(shì)在于使用該方法可以求解出所有的根，還可以證明根不存在的情況；缺點(diǎn)在于這種方法一般都非常的繁瑣，需要大量的計(jì)算。數(shù)值迭代法的優(yōu)勢(shì)在于使用這種方法計(jì)算相對(duì)簡(jiǎn)單，可以求解出大部分機(jī)械臂的逆解[1]，但每次迭代在給定預(yù)期初值的情況下，最終只能獲得一組解，如果解不存在，數(shù)值迭代將無(wú)法收斂。幾何法可以對(duì)某些特殊的臂型進(jìn)行簡(jiǎn)化，并利用幾何關(guān)系求解，但是它一般需要和解析法配合使用[1~3]。

Pieper證明了有相鄰的3根關(guān)節(jié)軸交于一點(diǎn)的六自由度機(jī)械臂存在封閉解[4]。Duffy證明了有相鄰3根關(guān)節(jié)軸平行的六自由度機(jī)械臂存在封閉解[5]。現(xiàn)在市場(chǎng)上的工業(yè)機(jī)器人幾乎都有3根相交軸，一般都可以利用解析法求得所有的封閉解，再采用其他方法從中選取一組較優(yōu)的可行解[6]。姜宏超等[7]針對(duì)六自由度模塊化機(jī)械臂利用解析法求解出了全部逆解。李憲華[1]、劉金存[2]、張金濤[3]都是利用解析法和幾何法結(jié)合的方法求解出了六自由度機(jī)械臂的全部逆解。但是對(duì)于一些不是此種構(gòu)型的、難以用解析法求其逆解的機(jī)械臂，需要一種比較通用的求解方法。

本文所研究的空間機(jī)械臂固連在飛行器本體上組成空間機(jī)器人，用于實(shí)現(xiàn)空間碎片抓捕、在軌服務(wù)等任務(wù)。空間機(jī)械臂工作在失重的環(huán)境中，關(guān)節(jié)轉(zhuǎn)動(dòng)慣量不會(huì)由前級(jí)向后級(jí)累加，因此設(shè)計(jì)成由6個(gè)相同的模塊化關(guān)節(jié)和臂桿連接組成的模塊化六自由度空間機(jī)械臂。由于本文空間機(jī)械臂安裝范圍、重量指標(biāo)的限制以及工作范圍的要求，機(jī)械臂無(wú)法設(shè)計(jì)成有3根相交軸的結(jié)構(gòu)，因此采用圖1所示的特殊構(gòu)型。該構(gòu)型在逆運(yùn)動(dòng)學(xué)的求解問(wèn)題上存在一定的難度和特殊性，使用解析法計(jì)算難以找到一組獨(dú)立不相關(guān)的變量。因此本文使用一種數(shù)值迭代的方法求其運(yùn)動(dòng)學(xué)逆解。

圖1 空間機(jī)器人

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

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

要研究機(jī)械臂的運(yùn)動(dòng)，就必須建立它的運(yùn)動(dòng)學(xué)模型，再通過(guò)運(yùn)動(dòng)學(xué)模型求解正、逆運(yùn)動(dòng)學(xué)。一般通過(guò)建立關(guān)節(jié)坐標(biāo)系，用數(shù)學(xué)方法抽象機(jī)械臂實(shí)體，描述機(jī)械臂相對(duì)基坐標(biāo)系的位置和姿態(tài)。本文使用通用的D-H參數(shù)運(yùn)動(dòng)學(xué)建模法建模。空間六自由度機(jī)械臂構(gòu)型如圖2a所示，連桿坐標(biāo)系的建立如圖2b所示，其中基坐標(biāo)系{0}與坐標(biāo)系{1}重合，{6}為空間機(jī)械臂腕部坐標(biāo)系。D-H參數(shù)表如表1所示。

a）機(jī)械臂構(gòu)型

b）連桿坐標(biāo)系

圖2 機(jī)械臂運(yùn)動(dòng)學(xué)建模

表1 六自由度空間機(jī)械臂D-H參數(shù)表

1.2 正運(yùn)動(dòng)學(xué)求解

空間六自由度機(jī)械臂正運(yùn)動(dòng)學(xué)的求解問(wèn)題就是已知機(jī)械臂各關(guān)節(jié)角度值{,,,,,}，求解機(jī)械臂腕部坐標(biāo)系相對(duì)基座標(biāo)系的位姿。表1定義了空間六自由度機(jī)械臂的所有連桿參數(shù){,,,}，其中只有參數(shù)是變量，其余均為常量。由連桿參數(shù)可以確定連桿坐標(biāo)系{}相對(duì)于坐標(biāo)系{-1}的齊次變換矩陣。

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

空間六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的問(wèn)題即已知腕部坐標(biāo)系{6}相對(duì)基座標(biāo)系{0}的期望位姿，即已知，求解達(dá)到這一位姿時(shí)6個(gè)關(guān)節(jié)轉(zhuǎn)角{,,,,,}的值。逆解可能不存在，也可能有多重解或者只有一個(gè)解。

針對(duì)本文六自由度空間機(jī)械臂，利用牛頓迭代法[8]求解其運(yùn)動(dòng)學(xué)逆解。首先需要構(gòu)建非線性方程組：，，

已知最終所需腕部坐標(biāo)系{6}相對(duì)于基坐標(biāo)系{0}的齊次變換矩陣，對(duì)于每一輪迭代也可得到一個(gè)齊次變換矩陣，為迭代次數(shù)，。將兩矩陣中的元素一一相減得方程組：

即

由式（3）可以確定此方程組的雅克比矩陣為

求解方程組的牛頓迭代公式為

（5）

因此，可以利用式（6）求解六自由度空間機(jī)械臂的逆運(yùn)動(dòng)學(xué)。

3 仿真驗(yàn)證

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

空間六自由度機(jī)械臂臂型2=3=5=164.5 mm，4=444.5 mm，2=610 mm。為了驗(yàn)證上述正運(yùn)動(dòng)學(xué)模型的正確性，按照式（2）在Matlab中編寫程序。程序輸入條件為六自由度空間機(jī)械臂各關(guān)節(jié)角~，輸出條件為腕部坐標(biāo)系相對(duì)于基坐標(biāo)系的齊次變換矩陣。根據(jù)圖2b所示坐標(biāo)系間的幾何關(guān)系，可確定空間機(jī)械臂處于折疊狀態(tài)時(shí)腕部坐標(biāo)系相對(duì)于基坐標(biāo)系的齊次變換矩陣為，空間機(jī)械臂處于折疊狀態(tài)時(shí)各關(guān)節(jié)角度值為。將此組角度值輸入正運(yùn)動(dòng)學(xué)求解程序，可得。證明上文所得正運(yùn)動(dòng)學(xué)模型的正確性。

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

為了驗(yàn)證本文所述逆運(yùn)動(dòng)學(xué)求解方法的正確性和可靠性。在Matlab中編寫逆運(yùn)動(dòng)學(xué)求解程序和運(yùn)動(dòng)規(guī)劃程序，對(duì)一條指定直線進(jìn)行軌跡跟蹤，并在三維空間中作出規(guī)劃軌跡和仿真軌跡，對(duì)規(guī)劃軌跡位置點(diǎn)和仿真軌跡位置點(diǎn)的位置進(jìn)行對(duì)比。

按照式（6）在Matlab中編寫逆運(yùn)動(dòng)學(xué)求解程序，程序輸入條件為初始角和期望達(dá)到位姿的齊次變換矩陣，輸出為達(dá)到期望位姿時(shí)各關(guān)節(jié)角的值以及利用正運(yùn)動(dòng)學(xué)解求得的齊次變換矩陣。算法流程如圖3所示。

圖3 逆運(yùn)動(dòng)學(xué)求解程序算法流程

假設(shè)空間機(jī)械臂運(yùn)動(dòng)控制器每20 ms給關(guān)節(jié)控制器發(fā)送一組關(guān)節(jié)角度值，即每20 ms運(yùn)動(dòng)控制器要根據(jù)運(yùn)動(dòng)規(guī)劃進(jìn)行一次逆運(yùn)動(dòng)學(xué)解算。假設(shè)機(jī)械臂末端速度為0.05 m/s，則每20 ms的時(shí)間間隔內(nèi)空間機(jī)械臂末端的運(yùn)動(dòng)距離只有1 mm。因此，為了方便逆運(yùn)動(dòng)學(xué)解算的進(jìn)行，以逆運(yùn)動(dòng)學(xué)求解算法為基礎(chǔ)編寫運(yùn)動(dòng)規(guī)劃程序，將前一次迭代所得和中的旋轉(zhuǎn)矩陣作為后一次迭代的初始輸入。

位置追蹤結(jié)果如圖4所示，求解出的各關(guān)節(jié)角度值如圖5所示。

由仿真結(jié)果可知，本文所用逆運(yùn)動(dòng)學(xué)求解方法能夠正確可靠地求解出六自由度空間機(jī)械臂的逆解，并且能夠有效地對(duì)腕部坐標(biāo)系原點(diǎn)進(jìn)行軌跡跟蹤。

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

由于本文自主研發(fā)空間六自由度機(jī)械臂的特殊性，利用解析法求解機(jī)械臂逆運(yùn)動(dòng)學(xué)非常復(fù)雜，難以找到一組完全獨(dú)立的變量，因此使用牛頓數(shù)值迭代法求解六自由度機(jī)械臂逆解。利用D-H運(yùn)動(dòng)學(xué)建模法建立六自由度機(jī)械臂正運(yùn)動(dòng)學(xué)模型。利用機(jī)械臂腕部坐標(biāo)系{6}相對(duì)于基坐標(biāo)系{0}的齊次變換矩陣構(gòu)建非線性方程組，通過(guò)牛頓迭代，使每次迭代所得齊次變換矩陣無(wú)限接近期望位姿的齊次變換矩陣，最終迭代收斂得到期望關(guān)節(jié)角。并通過(guò)仿真實(shí)驗(yàn)證明正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)求解方法正確可靠。本文提出的機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值迭代解法對(duì)于解析法難以求解運(yùn)動(dòng)學(xué)逆解的多自由度串聯(lián)型機(jī)械臂均適用，可以有效地求解出機(jī)械臂運(yùn)動(dòng)學(xué)逆解，也為機(jī)械臂軌跡規(guī)劃和控制提供了理論依據(jù)。

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

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

[3] 張金濤, 徐文福, 孟得山. 基于臂型角參數(shù)化的冗余空間機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方法[C]. 西安: 第32屆中國(guó)控制大會(huì), 2013.

[4] Pieper D L. The kinematics of manipulators under computercontrol[D]. California: Stanford University, 1968.

[5] Duffy J. Analysis of mechanisms and robot manipulators[M]. London: Edward Arnold, 1980.

[6] 王俊龍, 張國(guó)良, 敬斌, 徐君. 一種新的六自由度機(jī)械臂運(yùn)動(dòng)學(xué)反解方法研究[J]. 計(jì)算機(jī)工程與應(yīng)用, 2013, 49(22): 266-270.

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

[8] 朱靜芬, 韓丹夫. “牛頓類”迭代的收斂性和誤差估計(jì)[J]. 浙江大學(xué)學(xué)報(bào)(理學(xué)版), 2005, 32(6): 623-626.

[9] 尹釗, 賈尚暉. Moore-Penrose廣義逆矩陣與線性方程組的解[J]. 數(shù)學(xué)的實(shí)踐與認(rèn)識(shí), 2009, 39(9): 239-244.

[10] 王莎莎. 廣義逆矩陣的性質(zhì)及應(yīng)用[J]. 才智, 2014(19): 149.

Inverse Kinematical Numerical Method Based on6-DOF Space Manipulator

Zhang Xu-man, Zhang Zhong-zhe, Wang Yan-bo, Yang Tao, Deng Tao

(Beijing Institute of Precision Mechanical and Electrical Control Equipment, Beijing, 100076)

Analytical method is only applicable to the manipulators with special structure and the solving process is complicated with a large amount of calculation. Aiming at this problem, a kind of inverse kinematical iterative method is proposed for most serial manipulators. This method is used to obtain the inverse kinematics solution of a 6-DOF space manipulator which is independently researched and developed. Then the reliability of this method is proved through the simulation.

Space manipulator; Numeric iteration; Forward kinematics and inverse kinematics

1004-7182(2016)03-0081-04

10.7654/j.issn.1004-7182.20160319

V476

A

2015-03-15；

2015-05-10

張栩曼（1990-），女，助理工程師，主要研究方向?yàn)榭臻g機(jī)器人