自由飘浮空间机器人笛卡尔避奇异运动规划

第37卷 第11期2009年 11月 华 中 科 技 大 学 学 报(自然科学版)

J.H uazhong U niv.o f Sci.&T ech.(N atural Science Edition)V ol.37N o.11 No v. 2009

收稿日期:2009-03-19.

作者简介:吴剑威(1981-),男,博士研究生,E -ma il:wujianw ei0823@https://www.360docs.net/doc/f112641687.html,.基金项目:国家高技术研究发展计划资助项目(2006A A04Z228).

自由飘浮空间机器人笛卡尔避奇异运动规划

吴剑威 史士财 刘 宏

(哈尔滨工业大学机器人技术与系统国家重点实验室,黑龙江哈尔滨150001)

摘要:为了解决自由飘浮空间机器人笛卡尔运动规划过程中由动力学奇异而导致的关节速度无限大或无法计算的问题,提出了一种基于奇异值分解的高精度避奇异运动规划算法.在传统阻尼最小方差的基础上,利用奇异值分解将阻尼系数从固定量转化为随广义雅可比矩阵动态自适应变化的变量,算法保证了空间机器人通过奇异点时各关节的角速度变化曲线连续且平稳.算例验证了算法的有效性,结果表明该算法在非奇异位置不会引入跟踪误差,相比传统阻尼最小二乘法累计跟踪误差大约降低15%.

关 键 词:自由飘浮空间机器人;运动规划;动力学奇异;阻尼最小方差;奇异值分解中图分类号:T P24 文献标识码:A 文章编号:1671-4512(2009)11-0005-04

Cartesian singularity -avoiding path planning

for free-floating space robot

Wu J ianw ei S hi S hicai L iu H ong

(State Key Labor ator y o f Robotics and Sy stem,H arbin Inst itute of T echno lo gy ,H arbin 150001,China)

Abstract :Ow ing to the dynam ic sing ular ity,the joint velocity co uld be infinite or could be not calcu -late,w hen the planned Cartesian path of a fr ee -floating space robot.A singularity avoid path planning algorithm based on the singular value deco mposition w as proposed.The algorithm changes the factor fro m constant to a dynamic variable damping factor changed w ith the deter minant of generalized Jaco -bian matrix.T he continuity and stableness o f the jo int v elo city cur ve w as guar anteed w hile the space manipulator passing by the system sing ularity point.And an ex ample w as presented for ver ifying the validity o f the proposed alg orithm.T he results show that this algo rithm does not include the erro rs at no nsing ular area,and the inherited error is decreased about 15%co mpared to the method o f tradition -al damped least -square.Key words :free -floating space robot;path planning;dy namic singularity ;damped least square

(DLS);singular value decomposition (SVD)

在空间站的组装、空间目标的捕获及在轨维护等空间活动中,空间机器人起着举足轻重的作用[1].与地面固定基座机器人不同,自由飘浮空间机器人末端执行器在运动过程中会对作为其基座的卫星产生反作用力和力矩,导致卫星的姿态和位置发生变化.通常位置级运动学方程不能用于规划关节的运动,因此当利用速度级逆运动学方程进行规划时,不可避免地会遇到雅可比矩阵奇

异的情况.此时若仍采用基于雅可比矩阵逆的运

动学求解方法,则在奇异点附近关节速度和加速度会变得非常大,使得相应的规划和控制算法失效.因此必须进行广义雅可比矩阵的避奇异处理.

空间机器人的运动学方程与动力学方程存在耦合,其广义雅可比矩阵[2]不仅包含运动学参数,也包含动力学参数,文献[3~10]探讨了有关概念和规划方法.本研究利用基于奇异值分解的阻尼

最小方差法实现了空间机器人奇异鲁棒性逆运动学方程求解,有效回避了空间机器人运动控制中动力学奇异问题.相比传统阻尼最小方差法,该算法实现了更高的机械臂末端跟踪精度.

1 自由飘浮空间机器人运动学模型

空间机器人一般运动学方程为

v e e =J s

v0

+J m ,

式中:v e和 e分别为机械臂末端线速度和角速度;v0和 0分别为基座质心处的线速度和角速度;

为机械臂关节角速度;J s为与基座运动相关的雅可比矩阵;J m为与机械臂运动相关的雅可比矩阵,J m与地面机器人的雅可比矩阵相同.

为了节省能源,空间机器人通常处于自由漂浮状态下工作,作为基座的航天器位置和姿态均不受控.此时系统的线动量和角动量守恒,满足如下两个非完整约束

n

i=0

m i r i=0;(1) n

i=0

(I i i+r i m i r i)=0,(2)式中:m i为机械臂第i个连杆的质量;r i为第i个连杆的位置矢量;I i为第i个连杆相对质心的惯性矩阵; i为第i个连杆的角速度;n为机械臂的自由度.根据式(1)和(2)可以解得:

v0=-(J Tw/ - r0g H-1s H m) ;(3)

0=-H-1s H m ,(4)式中各参数具体含义可参见文献[2].将式(3)和(4)代入式(1),得到自由漂浮状态下的空间机器人运动学方程

[v e, e]T=J g ,

式中J g为空间机器人的广义雅可比矩阵,它与基座的姿态、机械臂的关节角以及系统的质量和惯量有关.

2 笛卡尔连续轨迹规划

自由漂浮空间机器人是典型的非完整系统,机械臂末端位姿不但与当前关节角有关,而且与关节运动历史有关,无法像地面固定基座机器人那样通过解析的位置级逆运动学求解关节角[11].因此通常利用空间机器人的速度级逆运动学方程进行数值方法求解.

空间机器人笛卡尔空间连续轨迹规划的计算流程如下:

a.给出期望的机械臂末端连续位置轨迹p e(t)与姿态轨迹 e(t),决定末端的线速度v e(t)与角速度 e(t):

v e(t)= p e(t); e(t)=r (t),

机械臂末端绕矢量r从初始姿态旋转到末端姿态.

b.根据初始条件计算广义雅可比矩阵J g,并利用相应的奇异回避算法,求得广义雅可比矩阵的鲁棒逆J+g.

c.根据速度级逆运动学,规划机械臂关节的角速度

(t)=J+g[v e(t), e(t)]T.

d.根据角速度规划出当前关节角

= + t.

e.判断计算是否结束,若没结束则返回到步骤b;若结束则输出规划的关节角.

3 奇异鲁棒性逆运动学方程

利用广义雅可比矩阵逆的空间机器人笛卡尔轨迹规划算法在计算过程中很可能会遇到雅可比矩阵奇异的情况.奇异鲁棒性逆运动学就是要解决在广义雅可比矩阵出现奇异时如何求得其逆解的问题.

3.1 传统阻尼最小方差法

阻尼最小方差(DLS)法[12]的思路是在跟踪精度和关节速度之间作一个折衷,极小化目标

J - x + 2 ,(5)式中:

x为末端速度矢量[v e(t), e(t)]T; 2是阻尼因子,可理解为 J

-

x 项与 的相对权重.通过解相应的法方程

[J, I]T =[ x,0]T,

可以得到使式(5)极小化的惟一解

=J T(JJ T+ 2I)-1 x,

即

=J+g x.(6) 3.2 基于奇异值分解的阻尼最小方差法

通过奇异值分解(SVD)来进一步揭示阻尼最小方差的原理.利用Givens变换法[13]将J g进行奇异值分解,得

J g= 1u1v T1+ 2u2v T2+ + r u r v T r,(7)式中: i为J g的奇异值;u i和v i为J g的奇异矢量.于是

J T g J g+ 2I= r i=1( 2i+ 2)u i v T i,(8)

6

华 中 科 技 大 学 学 报(自然科学版) 第37卷

将式(8)代入式(6),可以得到

( )=(J T g J g+ 2I)-1J T g[v e(t), e(t)]T=

r i=1

i

2i+ 2v i u

T

i

v e(

t)

e(t)

.(9)

将机械臂末端速度向量[v e(t), e(t)]T表示为输出矢量u i的线性组合

v e(t)

e(t)

= r i=1 x i u i,

考虑到u i与u j(i j)相互正交,则

x i=u T i[v e(t), e(t)]T.

将上式代入式(9)中,得到

( )= r i=1 i 2i+ 2v i x i.

令 ( )i=[ i/( 2i+ 2)]v i x i,则 ( )= r

i=1

( )i,显然

( )i各参数之间相互正交,所以有

( ) 2= r

i=1 ( )i 2= r

i=1

i

2i+ 2

2

x2i,

容易得到追踪误差

[v e, e]-J g ( ) 2=

r i=1 x2i 2

2i+ 2

2

+ m i=r+1 x2i,(10)

式中r和m分别为广义雅可比矩阵降秩和满秩时矩阵的秩.式(10)中误差由两部分组成,其中

m i=r+1 x2i对应于奇异值为零输出速度也为零引起的

误差,即不论关节速度多大,在机械臂末端u i(r<

i

r

i=1

x2i[ 2/( 2i+ 2)]2可以通过调整阻尼系数 来

减小误差.

综合考虑末端速度的连续性以及尽量减小跟

踪误差,引入一个动态变化的 ,即

i=

max(1+cos( i/ ))/2 ( i< );

0 ( i ),

(11)

式中: max为最大阻尼系数; 为空间机器人广义

雅可比矩阵接近奇异、且需要引入阻尼时的 i

值.该方法与传统阻尼最小方差法不同的是,并不

是对每一个 i都引入 ,只是对小到一定程度的

奇异值引入阻尼 . max和 的选取原则是尽量使

关节速度平滑过渡且跟踪误差尽量小.一般需要

反复调整得到比较合适的值,通常 max和 在同

一个数量级上.

4 仿真分析

这里以带有飞行基座的6自由度空间机器人

作为研究对象,其连杆运动学和动力学参数如表

1所示,表中:a0为惯性系到基座质心的位置矢量

在惯性坐标系中的表示;b0为基座质心到关节1

位置矢量在惯性坐标系中的表示;a i(1 i 6)为

表1 空间机器人运动学和动力学参数

参数基座连杆1连杆2连杆3连杆4连杆5连杆6质量/kg450.01.59.61.59.01.510.5 00-492.50292.00-136.0

a i/mm(0 i 6)0054.0-121.0-150.0121.00

0124.000000

500.00-492.5124.0349.0-124.0-164.0 b i/mm(0 i 6)0121.0-54.0029.000

751.0000000

转动惯量/ (kg m2)I x x

I y y

I zz

I x y

I x z

I y z

200.00

200.00

200.00

3.31 10-3

3.31 10-3

3.31 10-3

3.10 10-2

1.50

1.48

3.31 10-3

3.31 10-3

3.31 10-3

0.73

0.60

0.63

-0.01

3.31 10-3

3.31 10-3

3.31 10-3

0.10

0.10

0.09

关节i到杆件i质心的位置矢量在坐标系{i}中的表示;b i(1 i 5)为杆件i质心到关节i+1的位置矢量在坐标系{i}中的表示;b6为杆件6的质心到工具坐标系原点的位置矢量在坐标系{6}中的表示.

仿真的任务是使空间机器人末端执行器在给定的两点间跟踪一条直线路径.以基座质心为惯性系,令机械臂末端初始位姿为(1.37,-0.08, 0.64,-3.14,0,-2.97),目标位姿为(1.50,0, -2,-3.16,-0.26,-3.14),其中位姿向量前三位是机械臂末端在惯性系的x,y,z方向的位置,后三位是末端工具坐标系x,y,z方向相对于惯性系的欧拉角.机械臂关节角的初始位置为(175 ,-60 ,-60 ,0 ,210 ,5 ).考虑到起始点和

7

第11期 吴剑威等:自由飘浮空间机器人笛卡尔避奇异运动规划

终止点轨迹的平滑性,机械臂末端线速度与角速度采用梯形规划.

机器人运动规划过程中会通过动力学奇异点,系统广义雅可比矩阵的行列式的值D 随时间的变化如图1所示,在7.52s 处广义雅可比行列式的值到达零

.

图1 广义雅可比矩阵行列式值D 的变化曲线

如果此时仍采用直接求逆的运动学算法计算,关节的速度会变得非常大.这种方法在实际控制中是不可接受的,因此需要进行避奇异处理.图2是应用本文方法所规划的各关节角速度随时间变化曲线.仿真过程中取参数 max = =0.08,可以看出在引入阻尼之后,关节的速度在广义雅可比矩阵奇异区域很明显地受到了限制.各关节角速度曲线比较平滑,变化范围在6 /s 以内,因此这种奇异回避算法可以应用于实际

.

图2 基于SVD 的奇异鲁棒算法规划的关节角速度机械臂末端位置和姿态在3个方向上的跟踪误差如图3所示,其中: p 为位置误差; 为姿

态误差.本文方法在机械臂末端通过奇异点之前是没有跟踪误差的,只是在奇异区域内产生了误差.而且跟踪误差随着奇异程度发生变化,在奇异最严重的地方引入了最大的跟踪误差.由奇异位

置所产生的非奇异区域的累积误差可以通过闭环

控制系统进行补偿.然而,DLS 法在之前非奇异区域也引入了误差.根据图3的结果可得出本文算法相比DLS 法累计跟踪误差大约降低15%.

图3 本文方法与传统DL S 法末端跟踪误差比较

算法中动态变化阻尼因子的参数需要合理进行选择,一般需要反复调整.遗传算法、粒子群算法等可以用于参数的选择.

参

考

文

献

[1]K ing D.Space servicing :past,present and futur e

[C] P ro ceedings of the 6th Internatio nal Sympos-i um on A rtif ical Intellig ence and Robotics &A utoma -tion in Space.M ontrea:Canadian Space A gency,2001:18-22.

[2]U metani Y,Y oshida K.R eso lv ed motio n r ate co ntr ol

o f space manipulator s w ith generalized Jaco bian ma -tr ix [J ].IEEE T ransact ions on Ro bo tics and A uto -matio n,1989,5(3):303-314.

[3]Pa padopoulo s E,Dubow sky S.Dynamic sing ularities

in the contr ol o f free -floating space manipulat ors[J].A SM E Jour nal of Dynamic Systems,M easurement and Co nt rol,1993,115(1):44-52.

[4]Papadopoulos E.P ath planning fo r space manipula -to rs exhibit ing nonholo no mic behav ior [C] Pr oceed -ing s o f the IEEE Inter national Co nfer ence o n Intell-i g ent Ro bo ts and Sy st ems.Raleigh:IEEE,1992:669-675.

[5]L ampar iello R ,Deutr ich K.Simplified path planning

fo r fr ee -floating robots,DL R Internal Repor t 515-99-04[R].M unich:DL R,1999.

[6]Pandey S,A gr awal S.Path planning of free floating

pr ismatic -jo inted manipulator s[J].M ultibody System D ynamics,1997,1(1):127-140.

[7]Xi F,Fenton R G.On t he inverse kinematics of space

manipulato rs for avo iding

(下转第17页)

8 华 中 科 技 大 学 学 报(自然科学版) 第37卷

参考文献

[1]U sui A kir a1,M atsumoto K atsuto ki.Geo log ical study

of co balt-rich fer romanganese cr usts using a camer a-monito red drill machine in t he M arshall Island ar ea

[C] P roceedings o f the ISO PE O cean M ining Sym-

po sium.T sukuba:Internatio nal Societ y o f Offshor e and P olar Eng ineers,2003:12-15.

[2]冯 仪,陈柏金.车载雷达机电式自动调平控制系统

[J].华中科技大学学报:自然科学版,2004,32(6): 66-68.

[3]张艳兵,姚舜才,任作新.升高低点调平技术研究

[J].华北工学院学报,2004(3):201-203.

[4]王海淼,万彦辉,孟卫锋.自适应模糊PI D控制在惯

性平台数字调平系统中的应用[J].弹箭与制导学报,2005,25(3):145-147.

[5]潘宏侠,高 强,何 臻.自适应模糊面开关阀控液

压自动调平方法:中国,CN101303567A[P].2008-11-12.

[6]吴 锋,杨俊义,雷 龙,等.某车载高炮液压自动调

平控制系统[J].火炮发射与控制学报,2007,3(1): 67-72.

[7]Xu Jianx in,X u Jing.A new fuzzy lo gic learning con-

tro l scheme for repetitive t rajecto ry tr acking pro blem [J].Fuzzy Set s and Systems,2003,133(1):57-75.

[8]任雁胜,兰 凯.模糊控制算法在海底遥控取样钻机

液压压力控制系统中的应用[J].机床与液压,2008, 36(8):252-254.

[9]Co mpany O,P ier rot F.M o delling and design issues

of a3-ax is para llel machine-too l[J].M echanism and M achine T heo ry,2002,37(11):1325-1345. [10]盛 英,仇原鹰.6腿支撑液压式平台自动调平算

法[J].西安电子科技大学学报,2002,10(5):593-597.

(上接第8页)dynamic singular ities[C] Pr oceeding s o f the I EEE Int er national Conference o n Ro bo tics and Automat ion.San Dieg o:IEEE,1994:3460-3465.

[8]徐文福,梁 斌,李 成,等.空间3R机器人工作空

间分析[J].宇航学报,2007,28(5):1389-1394. [9]顾晓勤,刘延柱.空间机械臂动力学奇点与回避[J].

宇航学报,1998,19(4):32-36.

[10]丁希伦,战 强,解玉文.自由飘浮的空间机器人系

统的动力学奇异特性分析及其运动规划[J].航空学报,2001,22(5):474-477.

[11]Nakamura Y,M ukherjee R.N onholonomic path

planning of space r obots[C] Pro ceedings of the

I EEE Internatio nal Conference on Ro bo tics and Au-

to mation.Piscataw ay:I EEE,1989:1050-1055. [12]W ampler C W.M anipulat or inver se kinematic solu-

tions based on v ector fo rmulations and damped least-squar es methods[J].I EEE T r ansactio ns on Sys-tems,1986,16(1):93-101.

[13]M aciejew ski A A,Reag in J M.A parallel alg or ithm

and architecture for the co nt rol of kinematica lly re-dundant manipulator s[J].IEEE T r ansactio ns on R obotics and A utomation,1994,10(4):405-414.

17

第11期 王利恒等:海底钻机平台调平控制算法研究