具有运动时间约束的机械手最优平滑轨迹规划.doc

具有运动时间约束的机械手最优平滑轨迹规划摘要：为提高机械手在指定时间内完成运动任务的平滑性，提出一种最优平滑轨迹规划方法。以7次B样条曲线插值关节位置序列，保证关节加加速度连续且关节启停速度、加速度和加加速度可设定。将运动学约束转化为B样条曲线控制顶点约束，以加加速度平方积分的最小值作为平滑性能指标，采用遗传算法对平滑性能指标和相应的时间节点向量全局寻优，进而规划出最优平滑轨迹，改进了现有平滑轨迹规划方法没有考虑加加速度累积效果和运动学、运动时间约束的不足。实验结果表明，该方法规划的轨迹具有加加速度平滑和加加速度累积最小的特点，比现有的多项式曲线轨迹具有更好的可跟踪性能，为机械手执行具有运动时间约束的任务提供了理想的轨迹。关键词：机械手；轨迹规划；最优平滑；B样条；遗传算法A Smoothness-optimal Trajectory Planning Method with Constraint on Traveling Time for ManipulatorsAbstract: In order to improve running smoothness of joints while manipulators execute a traveling task in definite time, a smoothness-optimal trajectory planning method is proposed. Seven order B-spline is adopted to interpolate joint position sequences, so continuous jerk is ensured and start-stop joint velocity, acceleration and jerk can be configured. Constraints on manipulators kinematics are transferred to constraints on control points of B-splines, and genetic algorithm is exploited to globally minimize the integral of squared jerk and seek the according optimal time nodes, then problems lie in the existing methods that taking no attention to jerk accumulation and constraints on kinematics or traveling time are solved, base on which the smoothness-optimal trajectories are generated. Experimental results show that, trajectories generated by the proposed planner have the advantages of continuous jerk, minimum accumulative jerk and providing better tracking performance comparing to existing trajectories constructed with polynomial curves, which make them optimal ones for manipulators to execute tasks with constraint on traveling time. Key words: manipulator; trajectory planning; smoothness-optimal; B-spline; genetic algorithm机械手轨迹规划是轨迹控制的基础，对机械手的运行效率、平稳性、作业精度和能量消耗具有重要意义。目前机械手任务空间和关节空间基本的轨迹规划方法已经成熟，但针对一定作业任务的轨迹构造方法和针对一定轨迹性能指标的轨迹优化方法还不断被提出1-7。由于不连续或过大的机械手关节加加速度会增大轨迹跟踪误差、引起振动和缩短使用寿命1，且研究表明人类手臂运动也符合加加速度最小原则2，因此，基于加加速度优化的平滑轨迹规划成为新的研究重点。PIAZZI 等3采用三次样条曲线插值方法构造关节轨迹，并以极小化极大方法对各关节的加加速度进行优化。MACFARLANE等4考虑了加加速度有界的机械手时间最优轨迹规划问题，采用五次多项式插值方法在线规划出平滑且时间近似最优的轨迹。PETRINEC等5研究了四次和五次多项式结合方法构造的关节轨迹相对于三次和四次多项式结合方法构造的关节轨迹的优点，得到加加速度连续的轨迹。GASPARETTO等6综合考虑执行时间和加加速度累积效果，采用三次样条曲线插值关节位置序列，生成满足运动学约束的时间-加加速度综合最优,且关节速度和加速度连续的轨迹。但以上研究没有考虑机械手的关节运动速度和加速度约束1,3-5，或者没有考虑关节加加速度累积效应1-5和突变问题1,3,6，因此得到的并非最优平滑轨迹。本文综合考虑机械手运动学约束和作业时间的严格要求，采用连续的7次B样条曲线插值关节位置序列，将各关节运动学约束转化为B样条曲线控制顶点约束，并利用遗传算法8, 9对关节加加速度平方的积分进行全局最小寻优，得到满足运动学约束、加加速度连续、加加速度平方积分最小且关节启停速度、加速度和加加速度可设定的关节轨迹，从而为机械手伺服控制器提供了理想的最优平滑轨迹。1 最优平滑轨迹问题描述根据机械手任务空间轨迹跟踪精度的要求，将一定几何和时间特性的作业轨迹离散化，得到位姿矩阵序列，通过逆运动学计算求出对应的关节位置序列。轨迹规划和轨迹优化方法首先求出与关节位置对应的时间，构成关节位置-时间节点序列机械手必须经过的关节位置节点数为个。采用多项式曲线或者样条曲线插值关节位置-时间节点序列，可以得到关节轨迹曲线，并满足： (1)式中，为机械手关节数目，、和分别为关节的运动速度、加速度和加加速度约束。理想的平滑轨迹不仅要求关节速度和加速度连续，而且要求关节加加速度连续。关节加加速度累积越小，轨迹平滑性越好，对关节驱动器而言，驱动力矩变化累积越小，关节运动越平稳。现有的加加速度最优轨迹规划方法采用三次到五次多项式构造关节轨迹，并采用极小化极大方法优化关节加加速度： (2)式中，为机械手遍历的总时间，一般由机械手工作单元的时序要求确定。这种优化方法会遇到关节加加速度不连续或者关节加加速度半无穷约束问题，更没有考虑加加速度的累积效果。为解决这些问题，本文采用次B样条曲线构造关节轨迹，并将关节加加速度平方积分作为优化指标构造最优平滑轨迹规划问题： (3)式中，为根据关节保护确定的权重系数，相对较大的可以保护关节，为加加速度累积指数，为时间节点向量，与最优平滑轨迹对应的时间节点向量和全局最小加加速度累积指数分别为和。由于次B样条曲线具有连续的特性，为满足式(1)中的约束条件且保证关节加加速度连续，须有。2 加加速度连续轨迹构造机械手各关节轨迹采用次非均匀有理B样条开曲线统一描述为： 式中，为B样条轨迹曲线的控制顶点向量，为关节轨迹的控制顶点数，为归一化时间变量，为次规范B样条基函数，为时刻处的关节位置向量。关节轨迹继承了B样条曲线的局部支承和连续特性，其阶导数可由德布尔递推公式求出： (4)由式(4)可求出B样条关节轨迹在时刻处的关节速度、加速度和加加速度： (5)B样条曲线严格处于其控制顶点形成的凸包并集内。为使B样条关节轨迹依次经过中的个关节位置-时间节点，必须反求B样条轨迹的控制顶点。以累计时长参数化方法将时间节点归一化，得到次B样条轨迹的定义域向量，且有：进而列出个关节位置-时间节点方程： (6)式中，。根据B样条轨迹定义域向量的元素个数与其控制顶点个数之间的关系，即：可知，需反求的控制顶点数为个。为使轨迹的启停速度、加速度和加加速度可配置，需6个辅助方程。因此，取=7，得到含有个未知控制顶点向量的个方程： 式中，、和分别为关节速度、加速度和加加速度曲线。、和分别为轨迹起始速度、加速度和加加速度向量，、和分别为轨迹终止速度、加速度和加加速度向量，均可根据关节轨迹启停特性要求设定。以矩阵形式将关节的个轨迹控制顶点反求方程表示为： (7)式中为系数矩阵。令表示矩阵的第行第列个元素，则有式中，。中的其它各元素根据轨迹起始和终止位置、速度、加速度和加加速度确定，且有： (8)式中，、和可由递推公式(4)求出。解矩阵方程(7)，可得各关节的B样条轨迹控制顶点向量： (9)根据控制顶点向量和归一化时间节点向量，即可规划出各关节在时刻经过位置的速度、加速度和加加速度均连续的关节轨迹，且各关节轨迹具有统一的描述形式，不受关节数目的限制。3 运动学约束简化采用多项式曲线构造满足运动学约束的关节轨迹，将遇到半无穷约束检验问题。由于B样条曲线具有凸包性质，即曲线上的点处于控制顶点构成的凸包并集内，且曲线的阶次越高凸包性质越强，因此使各关节轨迹满足式(1)描述的运动学约束，仅需7次B样条轨迹曲线的控制顶点满足：式中，、和分别为第个关节的B样条速度、加速度和加加速度轨迹曲线的第个控制顶点，可由德布尔递推公式(4)求出： (10)可见，采用B样条曲线构造关节轨迹可以简洁地将机械手关节速度、加速度和加加速度约束转化为曲线控制顶点约束，有利于约束优化问题的求解。结合式(1)和式(3)，将机械手最优平滑轨迹规划描述为非线性约束优化问题： (11)式中，为根据机械手作业时序要求指定的轨迹执行时间，为考虑7次B样条曲线的强凸包性质而采用的运动学约束放大倍数。必须大于或等于式(12)描述的最小执行时间。 (12)实际应用中，只需在求解式(12)时，通过23次轨迹规划和采样检验确定，最小执行时间问题仅需在规划最优平滑轨迹之前求解一次，此后即可用于检验的有效性。4 遗传算法寻优采用具有并行计算和全局寻优性能的遗传算法求解式(11)描述的最优平滑轨迹规划问题和式(12)描述的最小执行时间问题。传统的遗传算法对非可行个体采用较大惩罚或者舍弃非可行个体，从而缩小搜索空间，其后果是很难搜索到全局最优解。因此，构造新的非可行个体评价方法。设时间节点向量解的可行域为，将突破约束的最大值定义为与的距离：式中，。对于式(12)描述的最小执行时间问题，可令。表示，时，越大表明越远离可行域。采用模糊逻辑定义时间节点向量的可行度：式中，且越大表明属于可行域的程度越高，当=1时，完全属于可行域，当=0时，完全不属于可行域。根据和定义新的适应度函数：(13)式中，采用式(13)构造的适应度函数，使不可行时间节点向量具有不为零的较小的适应度值，从而在遗传操作中以较小的概率将优良基因遗传到下一代个体。为提高全局搜索的效率，根据时间节点向量的下界和初始值产生初始种群。设的下界为式中，。令式中，对应的分别为、和。根据、和确定时间节点向量的初始值为：根据新的适应度函数和合理的初始种群，采用遗传算法可快速搜索到全局最优加加速度累积指数和对应的时间节点向量。5 仿真试验结果以自主研制的钱江I号焊接机械手作为实验平台，验证提出的最优平滑轨迹规划方法的有效性。机械手系统如图1，其D-H参数如表1，各关节的运动学约束如表2，各关节轨迹须经过的关节位置序列如表3。设定关节启停速度、加速度和加加速度均为零，所有关节的保护权重系数取为1。在规划最优平滑轨迹之前，采用遗传算法求解式(12)描述的轨迹最小执行时间求解问题，得到。根据和采用浮点数编码和均匀分布随机数发生器产生初始种群，通过计算个体的适应度值，利用精英保留策略保证遗传算法的全局收敛性，精英保留率为0.2，遗传操作中采用轮盘选择算子，交叉概率为，变异概率为。通过3次调整，将运动学约束放大倍数合理的设定为1.9。求解出的轨迹最小执行时间为=15.18s，对应的时间节点向量为=4.71, 3.40, 0.92, 0.70, 0.94, 0.57, 3.94。而TAN等7采用相同的运动学约束和关节位置序列，规划出的最小执行时间为18.91s，这是由于本文采用的7次B样条轨迹的平滑性能优于TAN等采用的二次多项式加余弦函数轨迹。由第2节讨论的B样条轨迹构造方法，即可得到执行时间最小的关节轨迹曲线，如图2，各关节必须经过的关节位置序列用圆点标出。然后，设定最优平滑轨迹执行时间，采用与最小执行时间求解过程相同的遗传算法策略和参数设置，即可由式(11)得到最优平滑轨迹的最小加加速度累积指数和对应的时间节点向量。设定=20s，得到最优平滑轨迹的时间节点向量为=6.04, 4.36, 1.71, 1.18, 1.34, 0.85, 4.52，关节位置、速度、加速度和加加速度轨迹曲线如图3。对比最优平滑轨迹和最小执行时间轨迹，可知两者都具有连续的关节位置、速度、加速度和加加速度，且满足运动学约束。但最优平滑轨迹的加加速度累积指数为44.24 o/s2，最小执行时间轨迹的加加速度累积指数为111.54 o/s2，即前者的执行时间延长了31.75%，而加加速度累积指数降低了60.34%。表1 钱江I号焊接机器人D-H参数Tab.1 D-H parameters of QJ-No.1 arc welding manipulator关节/ o/ mm/ mm/ o1250150-90.020550030160-90.04594090.050090.06000图1 钱江I号焊接机械手Fig.1 QJ-No.1 arc welding manipulator表2 运动学约束Tab.2 kinematics constraints约束项各关节约束值123456/ (o/s)10095100150130110/ ( o/s2)454075709080/ ( o/s3)606055707570表3 关节位置序列Tab.3 joint position sequences 节点各关节位置 / o12345643.357.37130.57039.06-46.6643.33-18.00152.09045.9048.0250.04-41.85170.66051.19-32.3562.67-53.76179.41053.59-4.9978.04-57.32182.70054.6233.0494.40-52.73178.45053.3875.94104.13-42.46173.54050.3394.76111.916.79132.80040.41112.16v / (o/s)J4J2J5J6J1J3a / (o/s2)j / (o/s3)p / ot / s图2 最小执行时间轨迹Fig.2 Trajectories with minimum executing timeJ4v / (o/s)p / oa / (o/s2)j / (o/s3)-J2J6J5J1J3t / s 图3 最优平滑轨迹Fig.3 Smoothness-optimal trajectories将最优平滑轨迹按照机械手关节伺服控制周期离散化，送入伺服控制器做轨迹跟踪控制实验。关节伺服控制周期为400us。同时，根据关节位置序列和最优平滑轨迹的时间节点向量，采用成熟的三次多项式曲线插值方法规划关节轨迹，并进行轨迹跟踪实验对比。在相同的伺服控制器参数及相关实验条件下，两种轨迹跟踪实验得到的位置跟踪误差绝对值对比如图4，可见，本文的7次B样条最优平滑轨迹的位置跟踪误差明显小于现有的三次多项式轨迹的位置跟踪误差。t / sabs(e) / o三次样条7次B样条J1J6J5J4J3J2图4 关节位置跟踪误差对比Fig.4 Comparison of joint position tracking errors6 结语提出一种基于7次B样条的最优平滑轨迹规划方法，具有以下三个特点：(1) 保证关节轨迹起始和终止速度、加速度和加加速度的可配置性，以及速度、加速度和加加速度的连续性，并使运动学约束简洁地描述为B样条曲线控制顶点约束，降低了优化问题的复杂性。(2) 将加加速度平方积分最小作为平滑轨迹优化目标，考虑了极小化极大加加速度优化方法没有考虑的加加速度累积效果。(3) 针对运动学约束和轨迹执行时间约束设计新的适应度函数，采用遗传算法对加加速度累积指数和对应的时间节点向量寻优，具有并行运算和全局最优的特点。在常规的轨迹规划中，机械手经过的关节位置序列根据作业精度指定，时间节点向量根据运行速度直接给出，此时不存在轨迹优化问题，但7次B样条轨迹构造方法依然适用。提出的轨迹构造方法和约束优化策略也可用于在线避障和能量最优等轨迹的规划。参考文献(References)：1 K. J. KYRIAKOPOULOS, G. N. SARIDIS. Minimum jerk path generation. IEEE International Conference on Robotics and Automation, Philadelphia, 1988, 1: 364-3692 J. E. BOBROW, B. MARTIN, G. SOHL, et al. Optimal robot motions for physical criteria. Journal of Robotic Systems, 2001, 18(12): 785-7953 A. PIAZZI, A. VISIOLI. Global minimum-jerk trajectory planning of robot manipulators. IEEE Transaction on Industrial Electronics, 2000, 47(1): 140-1494 S. MACFARLANE, E. A. CROFT. Jerk-bounded manipulator trajectory planning: design for real-time applications. IEEE Transactions on Robotics and Automation, 200