具有运动时间约束的机械手最优平滑轨迹规划_刘松国 2009.pdf

《具有运动时间约束的机械手最优平滑轨迹规划_刘松国 2009.pdf》由会员分享，可在线阅读，更多相关《具有运动时间约束的机械手最优平滑轨迹规划_刘松国 2009.pdf（6页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、第!卷#第$期%&年!月#电#机#与#控#制#学#报()(*+,-*#./*0-1(2#/13#*41+,4)#5678!168$169:%&#具有运动时间约束的机械手最优平滑轨迹规划刘松国，#朱世强，#吴文祥（浙江大学 流体传动及控制国家重点实验室，浙江 杭州!&%;）摘#要：为了提高机械手在指定时间内完成运动任务的平滑性，提出一种最优平滑轨迹规划方法。采用七次 样条曲线插值关节位置序列，保证关节加加速度连续且关节启停速度、加速度和加加速度可设定。将运动学约束转化为 样条曲线控制顶点约束，以加加速度平方积分的最小值作为平滑性能指标，采用遗传算法对平滑性能指标和相应的时间节点向量全局寻优，进而

2、规划出最优平滑轨迹，解决了现有平滑轨迹规划方法没有考虑加加速度累积效果和运动学、运动时间约束的问题。实验结果表明，该方法规划的轨迹具有加加速度平滑和加加速度累积最小的特点，与现有的三次样条轨迹相比，可有效降低轨迹跟踪误差。关键词：机械手；轨迹规划；最优平滑；%:%文献标志码：/文章编号：!&;?（%&）&$?&A;?&$!#$%&()#*$+,-$.,/0$#.1*-,&+&2$%#3 4+$%0#&($.,+&$#&$.,5-+&2$+6#.,&+*7-,$#.()-B 26CDEDF6，#G0B 2HIEJIKCD，#LB LMCENIKCD（2OKOM PMQ)KR 6S T7FIU=6

3、VMW+WKCXYIXXI6C KCU*6COW67，GHMZIKCD BCI9MWXIOQ，0KCDH6F!&%;，*HICK）89($.,0$：-C 6WUMW O6 IYW69M WFCCICD XY66OHCMXX 6S Z6ICOX VHI7M YKCIF7KO6WX MNMFOM K OWK9M7ICD OKX ICUMSICIOM OIYM，K XY66OHCMXXE6OIYK7 OWKZMO6WQ 7KCCICD YMOH6U IX W66XMU:2M9MC 6WUMW EX7ICM VKX KEU6OMU O6 ICOMW67KOM Z6ICO 6XIOI6C XMJFMCM

4、X，X6 6COICF6FX ZMW VKX MCXFWMU KCU XOKWOEXO6 Z6ICO 9M76IOQ，KM7MWKOI6C KCU ZMW KC RM 6CSIDFWMU:*6CXOWKICOX 6C YKCIF7KO6WXICMYKOIX VMWM OWKCXSMWWMU O6 6CEXOWKICOX 6C 6COW67 6ICOX 6S EX7ICMX，KCU DMCMOI K7D6WIOHY VKX MN76IOMU O6 D76RK77Q YICIYIM OHM ICOMDWK76S XJFKWMU ZMW KCU XMM OHM K6WUICD 6OIYK7 OIYM

5、 C6UMX，OHMC XY66OHCMXXE6OIYK7 OWKZMO6WIMX VMWM DMCEMWKOMU，KCU W6R7MYX 7IMU IC OHM MNIXOICD YMOH6UX IC7FUICD OKICD C6 KOOMCOI6C O6 ZMW KFYF7KOI6C KCU6CXOWKICOX 6C ICMYKOIX 6W OWK9M7ICD OIYM VMWM X679MU:(NMWIYMCOK7 WMXF7OX XH6V OHKO，OWKZMO6WIMX DMCMWKEOMU RQ OHM W66XMU 7KCCMW HK9M OHM KU9KCOKDMX 6S 6C

6、OICF6FX KCU YICIYFY KFYF7KOI9M ZMW，KCU KCUMWMKXM OWKZMO6WQ OWKICD MWW6WX MSSMOI9M7Q 6YKWICD O6 OHM MNIXOICD FRI X7ICM OWKZMO6WIMX:1 4#.3(：YKCIF7KO6W；OWKZMO6WQ 7KCCICD；XY66OHCMXXE6OIYK7；EX7ICM；DMCMOI K7D6WIOHY收稿日期：%&A?!%?!A基金项目：浙江省科技面上项目（%&A*%!&$）作者简介：刘松国（!A&），男，博士，研究方向为计量测试技术、智能机器人；朱世强（!$），男，教授，博士生导

7、师，研究方向为智能机器人、机电液控制系统；吴文祥（!A_），男，博士研究生，研究方向为智能机器人。;引 言机械手轨迹规划是轨迹控制的基础，对机械手的运行效率、平稳性、作业精确度和能量消耗具有重要意义。机械手任务空间和关节空间的基本轨迹规划方法已经成熟，但针对特定作业任务和性能指标的轨迹构造和优化方法还不断涌现!#。由于不连续的关节加加速度会增大轨迹跟踪误差、引起关节振动和缩短使用寿命!，基于加加速度优化的轨迹规划成为研究重点。$%&%等采用三次样条插值曲线构造关节轨迹，并以极小化极大方法对关节加加速度进行优化(。)&*+&,-&./等针对加加速度有界的时间最优轨迹规划问题，采用五次多项式曲线规

8、划出时间近似最优的平滑轨迹0。$/1,%./*等研究了四次和五次多项式轨迹相对于三次和四次多项式轨迹的优点，并规划出加加速度连续的轨迹2。3&4$&5,/116 等采用三次样条曲线插值关节位置序列，规划出满足运动学约束的时间 加加速度综合最优，且关节速度和加速度连续的轨迹7。现有研究没有考虑机械手关节运动速度和加速度约束!，(2，或者没有考虑关节加加速度累积效应!2和突变问题!，(，7，因此得到的轨迹并非最优平滑。本文综合考虑运动学和作业时间约束，采用*7连续的七次 8 样条曲线插值关节位置序列，将运动学约束转化为 8 样条曲线控制顶点约束，利用遗传算法9:对关节加加速度平方积分的最小值进行全

9、局寻优，从而规划出具有运动时间约束的最优平滑轨迹。!最优平滑轨迹问题描述根据机械手任务空间轨迹跟踪精度的要求，将具有一定几何和时间特性的作业轨迹离散化，得到位姿矩阵!，通过逆运动学计算求出对应的关节位置!。轨迹规划和优化方法首先求出与关节位置!对应的时间!，构成关节位置时间节点序列#;（!，!）(&（）;)&（），*&（）;+&（），,&（）;!&（），?A%&!;&（!），(&（）%-(&，*&（）%-*&，,&（）%-,&。（!）式中：&;!，B，.，.为机械手关节数目；-(&、-*&和-,&分别为关节&的运动速度、加速度和加加速度约束。理想的平滑轨迹要求关节速度、加速度和加加速度连续，且

10、关节加加速度累积越小，轨迹平滑性越好。对关节驱动器而言，关节加加速度累积越小表示驱动力矩变化累积越小，关节运动越平稳。现有的加加速度最优轨迹规划方法采用三次到五次多项式曲线构造关节轨迹，并采用极小化极大方法优化关节加加速度：CDE CFG,&（!），?A#$/!0=（!1!/!）0 2HIJ。（B）式中：!#;!=，!，!$!1(#$K!；!;!L!；2HIJ为机械手遍历位置时间节点序列#的总时间，一般由机械手工作单元的时序要求确定。这种优化方法会遇到关节加加速度不连续或关节加加速度半无穷约束问题，且没有考虑加加速度累积效果。为解决上述问题，采用 3 次 8 样条曲线构造关节轨迹，并将关节加加

11、速度平方积分作为优化指标，构造最优平滑轨迹规划问题为4（!#）0 CDE#.&0!2HIJ=3&,B&（，!#）H!，?A#$/!0=（!1!/!）0 2HIJ。（(）式中：3&为关节保护权重系数，相对较大的 3&可以保护关节&；4（!#）为加加速度累积指数；!#为时间节点向量，与最优平滑轨迹对应的时间节点向量和全局最小加加速度累积指数分别表示为!#+和4（!#+）。由于 3 次 8 样条曲线具有 53!连续的特性，而关节位置曲线至少为 5(连续时，才能保证关节位置曲线的三阶导数仍然连续，即关节速度、加速度和加加速度均连续，因此要求 3)0。$加加速度连续轨迹构造机械手各关节轨迹采用 3 次非

12、均匀有理 8 样条开曲线统一描述为（6）0#7,0=$,.,，3（6）0#!,0!/3$,.,，3（6）。式中：$,(#（7 L!）K!为 8 样条轨迹曲线的控制顶点向量，7 L!为关节轨迹的控制顶点数；6(6!，6!L!863，67 L!9 6=，67 L 3 L!为归一化时间变量；.,，3（6）为 3 次规范 8 样条基函数；（6）(#.K!为时刻 6 处的关节位置向量。8 样条曲线具有局部支承和 53!连续特性，其 8 阶导数可由德布尔递推公式求出，即8（6）0#!,0!/318$8,.,，3/8（6），$9,0$,，9 0=，（3 1!/9）$9/!,/$9/!,/!，6,131!/9

13、/6,9 0!，B，8，,0!/3 1 9，!。（0）由式（0）可求出时刻 6(6!，6!L!处的关节速度、加速度和加加速度，即9:9电 机 与 控 制 学 报 第!(卷!（!）#（$）#%&%()!#!&*&，(!（$），$（!）+（$）#%&%()#&*&，(（$），%（!）!（$）#%&%()#&*&，(#（$）。（$）%样条曲线严格处于其控制顶点形成的凸包并集内。为使关节轨迹依次经过,中的-&!个关节位置时间节点，必须反求%样条轨迹的控制顶点。以累计时长参数化方法将时间节点!%归一化，得到(次%样条轨迹的定义域向量&$(，$!，$-&(，且有$($!$(，$-&($-&(&!$-&(!

14、，$%$%!)!%(!#-!&(!&.，%()!，()，-)(!，进而可以列出-&!个关节位置时间节点方程（$%)(）#%)(&%#&*&，(（$%)(）%，%(，!，-。（)）为使关节启停速度、加速度和加加速度可配置，取(*，得到含有-&*个控制顶点向量的-&*个方程，即（$%)*）#%)*&%#&*&，*（$%)*）%，+%(，!，-，#（$）$*!,，+#（$）$-&*!-，+（$）$*$,，+（$）$-&*$-，!（$）$*%,，+!（$）$-&*%-。式中，#（$）、+（$）和!（$）分别为关节速度、加速度和加加速度曲线。!,、$,和%,分别为轨迹起始速度、加速度和加加速度向量，/-

15、、0-和&-分别为轨迹终止速度、加速度和加加速度向量，均可根据关节轨迹启停特性要求设定。以矩阵形式将关节 1 的-&*个轨迹控制顶点反求方程表示为1#1 1。（*）式中：121(，21!，21-，/,1，/-1，0,1，0-1，&,1，&-1.；#1 31(，31!，31（-&)）.；1(!（-&*）/（-&*）为系数矩阵。令 1（4，5）表示矩阵 1的第 4 行第 5列个元素，4(，!，-&)，5(，!，-&)，则有1（-0，-1）*-1，*（$-0&*）。式中：-0!，-2!；-1-0，-0&!，-0&)。1中的其他各元素根据轨迹起始和终止位置、速度、加速度和加加速度确定，且有#(，#-&

16、)-，#!,，#!-&)!-，#$,，#-&)$-，#%,，#-&)%-。（3）式中，#!、#、#、#!-&)、#-&)和#-&)可由式（4）求出。解方程（*），可得各关节的 6 样条轨迹控制顶点向量#1 2!11。（5）根据控制顶点向量#1和归一化时间节点向量&，即可规划出在时刻!%经过位置%的速度、加速度和加加速度均连续的关节轨迹，且各关节轨迹具有统一的描述形式，不受关节数目*的限制。#运动学约束简化采用多项式曲线构造满足运动学约束的关节轨迹，将遇到半无穷约束检验问题。由于%样条曲线具有凸包性质，且曲线的阶次越高凸包性质越强，因此使各关节轨迹满足式（!）描述的运动学约束，仅需七次%样条轨迹

17、曲线的控制顶点满足678 3!1&%5/1，&!，-&)，678 31&%501，&，#，-&)，678 3#1&%5&1，&#，4，-&)。式中，3!1&、31&和 3#1&分别为第 1 个关节的%样条速度、加速度和加加速度轨迹的第&个控制顶点，可由式（!(）求出，即#!&*#&2#&2!$&*2$&，#&4$&)2$&#&2#&2!$&*2$&2#&2!2#&2$&)2$&()2!，#&!(（$&$2$&）（$&)2$&）#&2#&2!$&*2$(&2+#&2!2#&2$&)2$&2!2#&2!2#&2$&)2$&2!&#&22#&2#$&$2$&)2。（!(）采用%样条曲线构造关节轨迹可

18、以简洁地将机械手关节速度、加速度和加加速度约束转化为曲线控制顶点约束，有利于约束优化问题的求解。结合式（!）和式（#），将机械手最优平滑轨迹规划描述为非线性约束优化问题，即7（!(）69:#*1!8;-=5(（!(）#-!%(（!%)!%）8;-(，51（!(）678&!，-&)3!1&（!(）2(15/1%(，5*&1（!(）678&，#，-&)31&（!(）2(1501%(，5*&1（!(）678&#，4，-&)3#1&（!(）2(15&1%(。（!）553第)期具有运动时间约束的机械手最优平滑轨迹规划式中：!#为根据机械手作业时序要求确定的轨迹执行时间；$为运动学约束放大倍数，以充分利用

19、七次%样条曲线的强凸包性质。实际应用中，$只需通过二、三次轨迹规划和采样检验确定。!#必须大于或等于由下式确定的最小执行时间!&(，即)!&(#&(#$%*&#+!&，,-.-()（!）/&01*/*，2，$34+*)*（!）5$(,)%+，(-3)（!）/&01*/2，6，$34+2)*（!）5$(.)%+，(2-3)（!）/&01*/6，7，$34+6)*（!）5$(*)%+。（*2）最小执行时间问题仅需在最优平滑轨迹规划之前求解一次，此后!&(即可用于检验!#的有效性。!遗传算法寻优采用具有并行计算和全局寻优性能的遗传算法求解式（*）描述的最优平滑轨迹规划问题和式（*2）描述的最小执行时

20、间问题。传统的遗传算法对非可行个体采用较大惩罚或者舍弃非可行个体，从而缩小搜索空间，其后果是很难搜索到全局最优解。因此，构造新的非可行个体评价方法。设时间节点向量解的可行域为/!，将!突破约束的最大值定义为!与/!的距离，即+（!）/&01（+，&01（(&（!），(01（!）。式中，&/*，2，6-。(01（!）为等式约束，对于式（*2）描 述 的 最 小 执 行 时 间 问 题，可 令(01（!）/+。+（!）/+表示!(/!，+（!）8+时，+（!）越大表明!越远离可行域/!。采用模糊逻辑定义时间节点向量!的可行度，有2!（!）#!（!）3#6-*#*（!）*3 6-。式中：!（!）/*

21、，(01（!）/+，+，(01（!）,+；*（!）/*，(*（!）%+，*5(*（!）&01（(*（!），其他。2!（!）(+，*，且 2!（!）越大表明!属于可行域/!的程度越高，当 2!（!）/*时，!完全属于可行域/!；当 2!（!）/+时，!完全不属于可行域/!。根据+（!）和 2!（!）定义新的适应度函数0,.4（!），即0,.4（!）/5（!）*3+（!）3*2!（!()）6，5（!）)+，5（!）9*3+（!）3*2!)!6，5（!）:+。（*6）式中：6)*；5（!）#*#-)#*!#+)*2)（，!）!。采用式（*6）构造的适应度函数，使不可行时间节点向量具有不为零的较小的适

22、应度值，从而在遗传操作中以较小的概率将优良基因遗传到下一代个体。为提高全局搜索的效率，根据时间节点向量的下界和初始值产生初始种群。设!的下界为!;/!;+，!;*，!;（$5*）?参数如表*，各关节的运动学约束如表 2，各关节轨迹须经过的关节位置序列如表 6。图$钱江%号焊接机械手&()$*+,-.)%/01 23456(7/6894/:.0+A电)机)与)控)制)学)报)第*6 卷)表!钱江#号焊接机械手$%&参数()*+!$%&,(-(.+/+-0 12 34%516!(-7 8+*9:;运动学约束()*+?:;+.(/:70 71;0/-(:;/0约束项各关节约束值$%+-&,#$（%）

23、$.）$)&$&$+$（%）$.%）-&-/&/)0&$（%）$.+）,&/&/表 关节位置序列()*+41:;/,10:/:1;0+A=+;7+0节点各关节位置$（%）$%+-&,-+*+&/*+/$+*&/+)*,(-,*,$-+*+($0*$&%*)-&*)-0*%&*-(-$*0&$/*,&$*$)(+%*+&+,%*,/(&+*/,$/)*-$&+*&)(-*)-/0*-(&/*+%$0%*/&-*,%+*-&)-*-(&%*/+$/0*-&+*+0/&*)-,$-*$+(-%*-,$/+*&-&*+)-*/,/$*)$,*/)$+%*0-*-$%*$,1 1 设定关节启停速度、加速

24、度和加加速度均为，所有关节保护权重系数()取为$。在规划最优平滑轨迹之前，采用遗传算法求解式（$%）描述的轨迹最小执行时间问题，得到*#23。根据!和!4采用浮点数编码和均匀分布随机数发生器产生初始种群，通过+#,（!）计算个体的适应度值，利用精英保留策略保证遗传算法的全局收敛性，精英保留率为*%，遗传操作中采用轮盘选择算子，交叉概率为56*0，变异概率为#6*$。通过三次调整，将运动学约束放大倍数(5合理的设定为$*)。求解出的轨迹最小执行时间为*#236$&*$0-，对应的时间节点向量为!+6-*/$，+*-，*)%，*/，*)-，*&/，+*)-。而789 等采用相同的运动学约束和关节位

25、置序列，规划出的最小执行时间为$0*)$./，这是由于本文采用的七次.样条轨迹的平滑性能优于 789 等采用的二次多项式加余弦函数轨迹。由第%节讨论的:样条轨迹构造方法，可得到最小执行时间关节轨迹曲线，如图%，各关节必须经过的关节位置序列用圆点标出。图 最小执行时间轨迹B:-(C+7/1-:+0 8:/D.:;:.=.+E+7=/:;/:.+然后，设定最优平滑轨迹执行时间*;=，采用与最小执行时间求解过程相同的遗传算法策略和参数设置，即可由式（$）得到最优平滑轨迹的最小加加速度累积指数和对应的时间节点向量。设定*;?的伺服控制器做轨迹跟踪控制实验，关节伺服控制周期为-#.。同时，根据关节位置序

26、列和最优平滑轨迹的时间节点向量，采用三次多项式曲线插值方法规划关节轨迹，并进行轨迹跟踪实验对比。在相同的伺服控制器参数及相关实验条件下，两种轨迹跟踪实验得到的位置跟踪绝对误差对比如图-，可见七次:样条最优平滑轨迹的位置跟踪误差明显小于三次多项式轨迹的$)第,期具有运动时间约束的机械手最优平滑轨迹规划位置跟踪误差。!结 语本文提出了一种基于七次!样条的最优平滑轨迹规划方法，具有以下特点：）保证关节轨迹起始和终止速度、加速度和加加速度可配置，且速度、加速度和加加速度连续；#）将运动学约束描述为!样条曲线控制顶点约束，降低了优化问题的复杂性。将加加速度平方积分的最小值作为平滑轨迹优化目标，考虑了极小

27、化极大加加速度优化方法没有考虑的加加速度累积效果；$）针对运动学约束和轨迹执行时间约束设计新的适应度函数，采用遗传算法对加加速度累积指数和对应的时间节点向量寻优，具有并行运算和全局最优的特点；%）提出的轨迹构造和优化方法也可用于在线避障轨迹规划和能量最优轨迹规划。参 考 文 献：&()*+,-,./,0 1，0+)*2*0 3 45 67879:9;?AB C8D=A7E8 F G G!#$%&#$()#*+)#,%&%#-%)#.)/)$(-0#1 23$)45$()#，+?=7H#%I#J，JKK，-B7HLH?B7，.05 JKK，：$M%I$MJ5#&!,!),N 1 O，6+)P*4

28、!，0,Q/3，%$*5,?A79H=EREA 9EDA7E8S TE=?BUS7VH V=7A=7 1 5 6)3&#*),.)/)$(-780$%50，#WW，K（#）：XKY IXJY5$&-*+ZZ*+，*0*,/*+5 3HERH 97879:9D;A=;VAE=U?H8878CET=EREA 987?:HAE=S 1 5!9�-$()#0)#!#130$&(*%-4$&)#(-0，#WWW，%X（）：%W I%J5%&6+F+)/+4O 0，F),P O+5 1DRE:8LL 987?:HAE=A=;DVAE=U?H8878C：LS7C8 TE=HDA79?H7VA7E8S 1

29、 5!9�-$()#0)#.)/)$(-0#1 23$)5$()#，#WW$，J（）：%#IY#5Y&-OP)*4OF，,+F*F Z5 P=;VAE=U?H8878C HCE=7AB9 RSLE8 AB VE8A78:7AU ET;F G G:%1($%&#%#+)#,%&%#-%)#+)#$&)*#1 23$)5$()#，1:8#X I#J，#WWX，+AB8S，3=V5#WWX：IY5M&3+0-+)OPP,+，Z+4,PP,5+AVB87:TE=A79D;E?A7D9H?H8878C ET=EREA A=;VAE=7S 1 5.)/)$(-0#1+)5;3$%&4!#$%4&$%1

30、:#3,-$3&(#，#WWK，#%：%Y I%#M5X&P+4 3 Z，N+43(F5 PBE=A7VH 8L?=798AH=S=VB E8A79DE?A79H A=;VAE=U?H8878C 8L VE8A=EH ET 78L:SA=7H=EREAS1 5+=(#%0%6)3&#*),+)#$&)*9=%)&8#1 2;*(-$()#0，#WW$，#W（#）：KY IJ#5K&P+43 1，N+43 25+BUR=7L C8A7V HCE=7AB9 TE=AU?ET 8E8DH78=?=EC=9978C?=ERH:$=%5$(-0?($=2;*(-$()#0，JJK，$M（Y）：I#5J&Z+FQ+)*+-PQ，+0-)+3+PQ,0 4+5,?A79H=EREA AS SVBL:DH78C RSL E8 C8A7V HCE=7AB9 1 5.)/)$(-0#1+)5;3$%&4!#$%&$%1:#3,-$3&(#，#WWY，#：MX IXJ5（编辑：刘素菊）#WJ电&机&与&控&制&学&报&第$卷&