当前位置:首页 >> 机械/仪表 >>

基于牛顿-拉夫逊迭代法的6自由度机器人逆解算法


1 16

传感器与微系统(Transducer

and Microsystem

Technologies)

2010年第29卷第lO期

彳\妒\pkp妒℃P吣

’计算与测试‰ 崎h《孔p毋≮p《s、甚。

基于牛顿一拉夫逊迭代法的

6自由度机器人逆解算法术


完,杨国梁,张方生,丁锋

(江南大学通信与控制工程学院。江苏无锡214122)

摘要:为解决一般6自由度旋转关节机器人逆运动学问题,提出了一种用牛顿一拉夫逊迭代法逐次逼近 目标位姿的逆解算法。根据正运动学方程建立雅克比矩阵,采用基于豪斯霍尔德的SVD分解求其伪逆来 避免雅克比矩阵的奇异性问题,通过建立迭代规则并逐次迭代找到最优的逆运动学单解,实际应用时无需 再建立多解取优策略。本算法具有较好的局部快速收敛性,能够达到较好的精度和速度,并在基于ARM9 的嵌入式系统上实现了此算法。相应的测试表明:算法实时性能够满足系统要求,可应用于机器人实时控 制系统。 关键词:机器人;运动控制;微分运动;6自由度旋转关节 中图分类号:TP242 文献标识码:B 文章编号:1000-9787(2010)10-0116-03

A n inverse Kinematics lnverse kinematics F O of the O the D t:eneral algorithm Renera’6 -DULY robot based
(College
of

on

Newton-Raphson iteration术

WANG Xian,YANG Guo—liang,ZHANG Fang—sheng,DING Feng
Communication and Control Engineering,Jiangnan University,Wuxi 214122,China)

Abstract:To resolve the inverse kinematics problem of the general 6-DOF rotary is proposed,using pseudo-inverse of iteration
to

joint

robot,all inverse algorithm

Jacobian matrix

to

solve differential

movement and using Newton-Raphson
to

approximate

target

position.Jacobian matrix is established according
to

forward kinematics,using SVD

decomposition of Householder and
can

avoid the singularity problem.This algorithm has good and fast local convergence, realized
can on an

achieve higher precision and the ideal speed.The algorithm is
tests

ARM9一based embedded
can

system.The corresponding applied
to

show that the algorithm is real—time which

meet system requirements.It

be

real—time robot control system;6 degree of

freedom(DOF)rotary joinot freedom(DOF)rotary joint

Key words:robot;motion control;differential motion;6 degree of

0引



误差,甚至一些机器人在设计时就不符合Pieper准则。针 对此时求解困难的问题,Lee


随着工业自动化程度的不断提高,机器人特别是6自 由度旋转关节机器人以其较少的冗余和较大的自由度得到 了广泛的应用。现代工业机器人多采用在线编程的方式, 要求在满足必需的精度的前提下达到较快的速度,这就使 得作为机器人控制基础的运动学问题受到越来越多的重 视。运动学分为正运动学和逆运动学,前者一般采用齐次 坐标变换法…,此种算法简单可靠,比较成熟。后者由于 机器人本体的多变性,至今没有成熟的算法1…。国内的研 究一般集中在Puma560或类Puma560等符合Pieper准则 的机器人上,如程永伦等人旧’4 o利用旋转子矩阵正交的特 性,通过矢量运算构造方程等方法提高了求逆解的速度。 由于机器人的生产、装配及使用磨损等不可避免的会带来

Y等人∞’61通过矢量乘法运

算找到14个线性无关的逆运动学方程,利用分离变量消元 法将逆运动学问题简化为关节变量半角正切的一元16次 方程求根问题,但此算法得到的运动学解不唯一,需要进一 步验证各解正确性并建立取优策略,增加了算法运算量;金 媛媛等人"1将局部搜索能力强的模拟退火算法与全局搜 索能力强的遗传算法相结合提出了一种逆运动学解法,虽 然能达到一定的精度,但程序需要占用较大的存储空间且 存在迭代次数多、运算量较大的问题,在嵌入式系统上难以 达到较高的实时性。本文以雅克比矩阵和微分运动为基 础,提出了一种用牛顿一拉夫逊迭代法来求逆运动学解的方 法,采用基于豪斯霍尔德的SVD分解求雅克比矩阵的伪

收稿日期:2010-04—12

t基金项目:国家自然科学基金资助项目(60574051)

万方数据

第10期

王宪,等:基于牛顿一拉夫逊迭代法的6自由度机器人逆解算法 合式(2)可得

117

逆来解决奇异性问题,通过迭代可以得到逆运动学唯一解, 此解等同于按能量最小原则确定的最终解。算法参数和所 需存储空间均较少,借助牛顿一拉夫逊迭代法的局部快速收 敛性,以较少的迭代次数即可得到所需精度的解。 1正运动学 正运动学是解决如何将机器人的关节坐标系转换到笛 卡尔坐标系的问题。本文采用齐次坐标变换法,建立机器 人Denavit Harten—berg(DH)坐标系,如图l,其中,蜀y0毛为 基坐标系,其余为关节坐标系,a表示关节问杆件长度。

睁㈤面dO=牌知,
即JdO=D. (3) 雅克比矩阵,中的每个元素可由对应的运动学方程对 其中一个关节变量0。的导数求得。对式(3)两边左乘t,+ 得

由于J可能是一个非满秩的方阵,所以,此处.,+为., 的伪逆。由如下方法计算,对.,进行基于豪斯霍尔德的 SVD分解得到J=UZV+(U为上三角矩阵,∑是一个对角 矩阵),变形后可得J+=rE+U 4(∑+是将∑转置,并将其 主对角线上每个非零元素求倒数得到的,U+为U的共轭转 置)。此时关节角增量

图1 Fig


DH坐标系 dO=y∑+U+D.
coordinate

(5)

DH

system

根据DH法则与定义的4个连杆参数[0理a d],可得

式(5)即为建立的牛顿一拉夫逊迭代法的修正方程。 下面进行微分运动向量D的求解。用微分算子乘以一 个坐标系将导致坐标系的变化,微分算子可由式(6)求得 △=Trans(d戈,dy,dz)xRot(南,dO)-I

从坐标系x。k一。z一。到坐标系瓦kz。的变换关系为
一1瓦=A。=Rot(z,0。)XTrans(O,0,d。)X Trans(a。,0,0)XRot(菇,O/。)

r,。。8臼”
sin0。

一8inpH。。8dn cosO。COS仅。 sinot。 0

8in巩8i““n
一cosO.sinoln

。n。。8以1


ansin0。l


|0



COS。d。 0

=。。oo

从基座开始到末端执行器的总齐次变换公式即为T=
AoAlA2A3A4A5。

2逆运动学 逆运动学是解决在已知机器人位姿的条件下如何求解 各关节角度的问题。牛顿一拉夫逊迭代法有收敛速度快和 自我校正的优点。81,上次迭代产生的误差不会传递到下次 迭代中,它是解非线性方程组的一个重要方法。为了解决 迭代法中雅克比矩阵奇异或病态时无法进行迭代的问题, 本文用伪逆代替雅克比矩阵的伪逆哺’9],使得算法的应用范 围更加广泛。 牛顿一拉夫逊迭代法需要用到微分运动和雅克比矩阵。 定义6自由度旋转关节机器人关节坐标系的广义速度为线 速度”和角速度60组成的6维列向量,微分运动则描述为 微分平移d和微分旋转6组成的6维列向量D,可由广义速 度对时间的导数求得,即有 疋nd

001一曩 ㈠]_[一曩
001

hl—㈡i叫l’、也妙出

一、 ● ● ● ● ● 、

(6)

微分运动可以表示为由当前位姿t。,运动到目标位姿

To。d=T。(,+△),即 △=r。-1咒。。-I. (7)

结合式(6)、式(7)可求得池,d,,,出及舭,研,&的值, 即得到微分运动向量D。 综上所述,采用牛顿一拉夫逊迭代法求一般6自由度旋 转关节机器人逆运动学解的步骤为: 1)根据当前机器人的关节角度运用正运动学求得当 前位姿咒。,(或直接用上次计算保留的当前位姿)。 2)以机器人末端坐标系作为微分运动的参考坐标系, 设目标位姿为r。。。,根据当前位姿计算微分运动矩阵:△=

盼l嘣im--I聘/挚.
万方数据



z=L州一J,根据式(6)的相应元素得到微分运动向量D。
3)将正运动学方程各式分别对各关节角求导,得到雅 克比矩阵.,,进一步建立牛顿一拉夫逊迭代法的迭代方程
JdO=D.

6自由度旋转关节机器人笛卡尔空间的广义速度和关 节空间的关节速度可通过6 x6雅克比矩阵.,相互转换,结

118

传感器与微系统

第29卷

4)对_,进行SVD分解并变形求得,的伪逆.,+,由 d0=_,+(O)D计算得到的。 5)计算Il曲Il,设定一个满足精度的充分小的数eps, 当lI曲II≤eps时,退出循环并输出当前位姿瓦。,;当循环 次数大于Ⅳ时,循环结束并输出失败信息,其中,Ⅳ为满足 预定循环次数要求的自定义充分大的整数;当lI曲II> eps,且循环次数小于Ⅳ时,令0。,=p。。,+d0,并计算瓦。重 复步骤(1)至(5),直至满足循环退出条件,此时p。。,即为满 足条件的机器人各关节角度。 由于采用的是逐渐逼近目标位姿的求解策略,所以,不 存在多解问题,迭代时会趋近于距离当前关节角最接近的 角度。 3验证 本文所使用的硬件为一台6自由度焊接机器人,具有 6个旋转关节,采用基于¥3C2440(主频400 MHz)的嵌入式 系统通工业以太网控制驱动器驱动6台电机,采用Linux (版本2.6.15)开源操作系统作为软件平台,应用程序使用 面向对象的C++语言编写,系统结构如图2。
Tab l Fig 基 皇

图3需要焊接的三角形圈
3 Diagram of triangle needed
to

weld


世 牡 耳{<

时间,s

图4各关节角的变化图
Fig


Diagram of various

joim

angle

changes

表1不同精度下的迭代次数
Number

of iterations under different precision
10一6 47 10—9 60

精度(rad)10-3

图2系统结构框图
Fig 2

迭代次数

23

System

structure

diagram

设定最大循环次数为N=100,精度eps=10一,初始位

3.3算法性能测试
算法所用时问的长短直接影响到机器人实际控制时的

f,1


0 1 o o

0 0 1 o

o、
0 o

实时性,本次测试在基于嵌入式系统的焊接机器人上进行, 逆运动学结果精确到小数点后6位,取1 000次计算并记录 算法所用时间,结果取平均值,如表2所示。
表2算法所用时间
Tab 2

姿Lr。I Lo



1j

I。

3.1算法正确性测试
对图3所示三角形的三条边进行焊接实验,设各顶点 别为A(-0.985023,--19.378845,32.918547),B(23.452 674,8.329251,14.181958),C(-2.186015,--24.791
14.181 642,

Time

of algorithm

spent

对雅克比矩阵进行SVD分解所用的时间占到了逆运 动学算法总时间的75%,可见雅克比矩阵分解算法对整个 逆运动学解法的执行时间有较大影响。本文采用的SVD 分解法虽然增加了算法的执行时间,但很好的解决了雅克 比矩阵奇异性问题,使得算法应用范围更广。在本嵌入式 系统平台上,对文献[3]所提算法进行试验,结果虽然比文 中算法所用时间略短,但文献[3]的算法所得为多解,需要 进一步确定最优解,再次增加了算法时间和复杂性。考虑 到在此嵌入式系统平台上控制机器人的伺服周期为微秒 级,文中所提算法完全能够达到所需要求,且比文献[3]算 法更具优势。 4结论 本文在嵌入式系统上实现了一种一般6自由度旋转关节 (下转第131页)

958)。为了使机器人末端执行器沿三条边精确运

动,每条边各取70个等分点,分别用本文逆运动学算法计 算对应的关节角度。试验时机器人从A点开始焊接,并沿 三条边依次经B点、c点后再返回到A点,此过程机器人 6个关节的角度变化情况如图4所示。将得到的关节角度 代入正运动学中得到末端执行器的坐标,此坐标和三角形 三条边上各等分点相吻合,表明算法能够正确计算逆运动 学解,并达到所需要的精度。

3.2算法迭代次数
迭代次数是衡量算法收敛性能好坏的重要指标,根据 不同的精度,取1000次计算分别记录逆运动学算法迭代次 数,结果取平均值,如表l。在操作机器人时可以由精度和 迭代次数的关系,根据实际需要设置。

万方数据

第lO期

吴立恒,等:电容式倾斜传感器在地壳形变测量中的应用
一1.48 —1.49

13l



e {l扛 脚

一 ¥ 搬 娶

一1.50 一1.51 -1.52 一1.53 —1.54 一1.55

倾斜角度/(3 圈3倾斜传感器标定曲线 Fig 3
Calibration
curve of

时I可/ll

图6南北向倾斜传感器记录到的印尼两次7级地震的同震响应
sensor

tilt

Fig



North-south tilt

sensor

recorded



magnitude 7 the same

level shock

数据。图4,图5,图6是倾斜仪在漳州台工作的数据节选。
1.70 1.65

earthquake

in

Indonesia twice to respond to

漳州实现安装,工作环境恒温又无大的振动干扰,较好地记 录到地球固体潮汐变化曲线,地震发生前、地壳的异常变化 和地震发生时的震波,为地震研究工作提供了丰富的数据 和依据。 参考文献:
[1]欧阳祖熙,张钧,陈征,等.地壳形变深井综合观测技术



1.60 1.55 1.50 1.45 1.40 1.35



图4东西向倾斜传感器观测曲线节选
Fig 4 Observation
18_38 18.36
curve

的新进展[J].国际地震动态,2009(11):1-13.
sensor

excerpt of

east?west

tilt

[2]何成平,欧阳祖熙.倾斜形变观测技术发展综述[J].地壳构 造与地壳虚力文集,2006(18):149--157. [3] 邱泽华,谢富仁,苏恺之,等.发展钻孔应变观测的战略构 想[J].国际地震动态2004,301(1):7--14.




蕤 基

18'34 18.32 18.30 18.28

[4]黄立,武立华.二维垂直摆倾斜仪对地倾斜的响应[J].哈
尔滨工程大学学报,2006,27(3):469--473.
时间/d

[5]马鸿钧,孟保成.一种测量地壳微形变的竖直摆倾斜传感 器[J].传感器技术,1998,17(6):36-38.
sensor

图5南北向倾斜传感器观测曲线节选
Fig 5 Observation
curve

excerpt north-south tilt

[6]

欧阳祖熙,张宗润,何成平.基于感应耦合比率臂的高精度位 移测量系统[J].电子技术应用,2004(4):41—43.

4结论 电容式倾斜传感器灵敏度高(2 X10_4”)、线性好、预调 节动态范围大,能够很好地满足深井安装的要求。将其集 成在深井地壳形变宽频带多分量的综合观测系统里。在福建

作者简介:
吴立恒(1980一),女,浙江宁波人,硕士,助理研究员,主要从 事地震前兆仪器传感器的研究。

pppppppppp、:p矿p矿妒p妒扩p妒扩妒p妒扩护pp≯∥p妒妒妒妒p扩妒p妒p妒护妒≯、3p
(上接第118页) 机器人运动学算法,并作了相应的测试,结果表明基于牛 顿一拉夫逊迭代法的逆运动学算法能够在满足实时性要求 的前提下达到较高的精度,具有较好的实用性。 参考文献:
[1] 王浩,谢存禧.基于回转变换张量的6R机器人运动学研 究[J].厦门大学学报,2005,9:640--644. [2] 于艳秋,廖启征.基于有理数运算的一般6R机器人位置逆解 算法[J].机械工程学报,2005,41(3):229--233. [3] 程永伦,朱世强,刘松国.基于旋转子矩阵正交的6R机器人 运动学逆解研究[J].机器人,2008,3:160一164. [9] [7]
rithm
for

the

inverse kinematics

of



general

serial

6R manipula-

tor[J].Mechanism and Machine Theory:2007,42(1):66—81. [6]Lee
tial
H Y,Liang



G.A

new vector theory

for the

analysis

of

spa.

mechanisms[J].Mechanism

and

Machine Theory,1988,

23(3):209--217.

金媛嫒,秦伟.机器人逆运动学的模拟退火自适应遗传算 法[J].机械与电子,2007(1):31--38.

[8]谭营,何振亚,邓超.一种机器人逆运动学求解的神经网 络方法[J].电子科技大学学报,1998(3):310--314. Yang
S X,Meng M.An

efficient
safety

neural

network

method for real—

time motion planning Autonomous

with

consideration[J].Robotics and

[4]

支目松国,朱世强,王宣银.基于矩阵分解的一般机器人实时高 精度逆运动学算法[J].机械工程学报,2008,11(11):304—
309.

System,2000,32:115--128.

作者简介:
王宪(1955一),男,江苏无锡人,学士,高级工程师。研究领

[5]

Manfred L

H,Martin P,Hans.Peter

S.A

new

and efficient

algo.

域为智能控制系统,现场总线。

万方数据

基于牛顿-拉夫逊迭代法的6自由度机器人逆解算法
作者: 作者单位: 刊名: 英文刊名: 年,卷(期): 被引用次数: 王宪, 杨国梁, 张方生, 丁锋, WANG Xian, YANG Guo-liang, ZHANG Fang-sheng, DING Feng 江南大学,通信与控制工程学院,江苏,无锡,214122 传感器与微系统 TRANSDUCER AND MICROSYSTEM TECHNOLOGIES 2010,29(10) 1次

参考文献(9条) 1.王浩;谢存禧 基于回转变换张量的6R机器人运动学研究 2005 2.于艳秋;廖启征 基于有理数运算的一般6R机器人位置逆解算法[期刊论文]-机械工程学报 2005(03) 3.程永伦;朱世强;刘松国 基于旋转子矩阵正交的6R机器人运动学逆解研究[期刊论文]-机器人 2008(2) 4.刘松国;朱世强;王宣银 基于矩阵分解的一般机器人实时高精度逆运动学算法[期刊论文]-机械工程学报 2008(11) 5.Manfred L H;Martin P;Hans-Peter S A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator[外文期刊] 2007(01) 6.Lee H Y;Liang C G A new vector theory for the analysis of spatial mechanisms 1988(03) 7.金媛媛;秦伟 机器人逆运动学的模拟退火自适应遗传算法[期刊论文]-机械与电子 2007(01) 8.谭营;何振亚;邓超 一种机器人逆运动学求解的神经网络方法 1998(03) 9.Yang S X;Meng M An efficient neural network method for realtime motion planning with safety consideration 2000

本文读者也读过(10条) 1. 李健.李剑锋.武桢.张启先 冗余度机器人多关节故障的运动学容错性及其优化[期刊论文]-机械工程学报 2002,38(7) 2. 王宪.张斌.朱建鸿.须文波 基于USS协议网络控制系统的采样周期与系统性能[期刊论文]-江南大学学报(自然科 学版)2002,1(2) 3. 张相胜.潘丰.ZHANG Xiang-sheng.PAN Feng 均质机复合温度控制系统的设计[期刊论文]-江南大学学报(自然 科学版)2010,09(6) 4. 王宪.曾建平.WANG Xian.ZENG Jian-ping 一种多电动机同步控制方法及其应用[期刊论文]-现代制造工程 2009(6) 5. 杜滨.赵京.DU Bin.ZHAO Jing 冗余机器人锁定故障关节的运动及力矩分配[期刊论文]-机械设计与研究 2010,26(4) 6. 段磊强.周军.张铎 基于多关节机器人运动学分析的故障检测方法[期刊论文]-机器人2003,25(z1) 7. 王克平.王宪.Wang Keping.Wang Xian 基于DDE技术的MATLAB与WinCC通信设计[期刊论文]-自动化仪表 2006,27(4) 8. 李小唐.魏世民.廖启征.王品.LI Xiao-tang.WEI Shi-min.LIAO Qi-zheng.WANG Pin 具有容错性能的6R机械手 位置反解[期刊论文]-机械设计2006,23(7) 9. 杨坤.王宪.YANG Kun.WANG Xian 基于模糊滑模算法的液压位置伺服系统[期刊论文]-计算机系统应用 2010,19(8) 10. 毕云峰.王宪.BI Yun-feng.WANG Xian 基于模糊控制的多电机同步控制[期刊论文]-机械与电子2008(1)

引证文献(1条) 1.韩雪.訾斌.孙辉辉 基于迭代和短直线逼近结合法的焊接机器人逆运动学分析及实例仿真[期刊论文]-制造业自动 化 2013(8)

本文链接:http://d.wanfangdata.com.cn/Periodical_cgqjs201010036.aspx


相关文章:
牛顿-拉夫逊法潮流计算
给出修正方程和迭代收敛条件; 2 武汉理工大学《电力系统分析》课程设计说明书 2.牛顿拉夫逊算法 2.1 牛顿算法数学原理:牛顿法 (Newton Method) :解非线性方程...
基于极坐标的牛顿-拉夫逊法潮流计算
基于极坐标的牛顿-拉夫逊法潮流计算_工学_高等教育...(6) 为调整计算、经济运行计算、短路计算和稳定计算...阻抗矩阵是满矩阵,高斯-赛德尔迭代法要求计算机储存...
基于MATLAB的牛顿拉夫逊迭代法计算潮流(附加短路计算)
暂无评价|0人阅读|0次下载|举报文档 基于MATLAB的牛顿拉夫逊迭代法计算潮流(附加短路计算)_工学_高等教育_教育专区。电力系统潮流计算 牛顿法 短路计算 Matlab 电力...
牛顿拉夫逊介绍(原理、计算方法、程序)
牛顿拉夫逊介绍(原理、计算方法、程序)_数学_自然科学...,一般迭代 4~5 次便可以收敛到一个非常精确的解...(6)求取节点电压的新值; (7)检查是否收敛,如不...
牛顿迭代法
无解,算法求出的近似根序列就不会收敛,迭代过程会...迭代法 (Newton method) 又称为牛顿 - 拉夫逊方法...N 0 1 2 3 4 5 6 7 牛顿法 1 1.41176470588...
牛顿拉夫逊潮流计算
毕业设计(论文)基于牛顿拉夫逊法的电力系统潮流计算专业年级 自动化 2007 级学姓号名 20072180 邹爱龙 邓立华 解大琴 指导教师 评阅人 2011 年 6 月 中国 ...
电力系统牛顿拉夫逊潮流计算
此外,在电力系统静态及稳态稳定计算时,要 利用潮流计算的结果作为其他计算的基础。 牛顿拉夫逊法是在电力系统潮流计算的常用算法之一,它收敛性好,迭代 次数少。 ...
基于牛顿拉夫逊法潮流计算的matlab实验报告
计算结果牛顿-拉夫逊法潮流计算结果 节点计算结果: n 节点 1 2 3 4 5 6 ...心得:本程序分模块进行,先是排序,再是求导纳阵,然后求雅阁比,再进行迭代运算,...
基于MATLAB牛顿拉夫逊法进行潮流计算
基于MATLAB牛顿拉夫逊法进行潮流计算_工学_高等教育_...(i,6)~=3 DP=P(i)-P1; DQ=Q(i)-Q1; ...%用高斯消去法解“w=-J*V” disp('迭代次数')...
牛顿迭代法文献综述
牛顿 迭代法又称为牛顿-拉夫逊方法,它是 牛顿在 ...数值解,经过 14 次迭代的误差达 到了,说明此算法...刘玉芹发表基于牛顿迭代法的移动机器人编队算 法,该...
更多相关标签:
牛顿拉夫逊迭代法 | 牛顿拉夫逊法潮流计算 | 牛顿拉夫逊法 | 牛顿拉夫逊 | 牛顿拉夫逊潮流计算 | 牛顿 拉夫逊方法 | 牛顿拉夫逊法 matlab | 牛顿拉夫逊法潮流例题 |