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

?

基于空間六自由度機械臂的逆運動學數(shù)值解法

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

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

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

空間機械臂；數(shù)值迭代；正逆運動學

0 引 言

機械臂正運動學和逆運動學的求解是機械臂實際控制中需要解決的基本問題。其中逆運動學的求解比正運動學的求解更加復(fù)雜，對于不同的機械臂構(gòu)型沒有標準的求解方法和步驟。一般機械臂逆運動學的求解方法主要有解析法、數(shù)值迭代法、幾何法等。

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

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

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

圖1 空間機器人

1 正運動學模型的求解

1.1 運動學模型的建立

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

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

b）連桿坐標系

圖2 機械臂運動學建模

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

1.2 正運動學求解

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

2 逆運動學求解

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

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

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

即

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

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

（5）

因此，可以利用式（6）求解六自由度空間機械臂的逆運動學。

3 仿真驗證

3.1 正運動學驗證

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

3.2 逆運動學驗證

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

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

圖3 逆運動學求解程序算法流程

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

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

由仿真結(jié)果可知，本文所用逆運動學求解方法能夠正確可靠地求解出六自由度空間機械臂的逆解，并且能夠有效地對腕部坐標系原點進行軌跡跟蹤。

4 結(jié)束語

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

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

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

[3] 張金濤, 徐文福, 孟得山. 基于臂型角參數(shù)化的冗余空間機械臂逆運動學求解方法[C]. 西安: 第32屆中國控制大會, 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] 王俊龍, 張國良, 敬斌, 徐君. 一種新的六自由度機械臂運動學反解方法研究[J]. 計算機工程與應(yīng)用, 2013, 49(22): 266-270.

[7] 姜宏超, 劉士榮, 張波濤. 六自由度模塊化機械臂的逆運動學分析[J]. 浙江大學學報, 2010, 44(7): 1348-1354.

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

[9] 尹釗, 賈尚暉. Moore-Penrose廣義逆矩陣與線性方程組的解[J]. 數(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-），女，助理工程師，主要研究方向為空間機器人