基于执行时间约束的机器人关节空间轨迹优化

王 琨，骆敏舟，曹 毅，李 可，张秋菊

（1.江南大学 江苏省食品先进制造装备技术重点实验室，江苏 无锡214122；2.江南大学机械工程学院，江苏 无锡214122；3.中国科学技术大学 信息科学技术学院，安徽 合肥230027）

0 引 言

机器人在完成操作任务时，运动位置、速度、加速度的瞬间改变会造成机械冲击，激起系统振荡，导致运动不稳定，降低机械臂的运动精度［1，2］。快速精确的轨迹规划算法是保证运动系统的精度和性能的前提条件［3］，对关节运动轨迹的合理规划与优化可以提高机械臂的抗干扰性能，改善机械臂的定位精度以及轨迹跟踪精度，提高机械臂的工作效率［4－6］。因此，为了满足机器人高精度的运动性能需求，对关节的运动轨迹进行规划和优化。近年来，研究人员针对机器人各关节的位置、速度和加速度的运动轨迹约束条件，展开了各种轨迹执行时间的优化算法研究。如张斌等通过对轨迹的执行时间的优化处理，获取同时满足关节速度约束和驱动力约束条件的运动轨迹［7，8］；徐海黎等通过优化机器人的运动时间，使运动轨迹满足给定的约束条件［9］；Saravanan等采用NURBS曲线实现机器人运动轨迹的规划，通过轨迹执行时间的优化使轨迹满足多种约束条件［10］。

本文提出一种基于轨迹执行时间归一化处理的关节空间运动轨迹优化算法，通过分析关节位置、速度、加速度运动轨迹与归一化时间的约束关系，调节轨迹的执行时间以获取机器人关节的最优运动位置轨迹。该方法将轨迹执行时间转化成无量纲变量，避免复杂的计算过程，能够简单有效地应用于机器人的轨迹优化控制。在Matlab环境下模拟不同轨迹执行时间对运动轨迹的影响，验证该优化算法的可行性及可靠性，其能有效提高机器人的运动效率。

1 关节空间相对τ的轨迹规划

为了保证机器人在关节空间内平滑地完成操作任务，机器人任意关节在运动过程中均要满足关节的位置、速度、加速度或加速度变化的约束条件。将关节的轨迹位置函数表示为θ（t）。若关节在运动的初始时刻ti和终止时刻tf的位置、速度和加速度等约束条件分别为

轨迹插值函数需要满足上述6个约束条件，在众多的轨迹拟合算法中［11－13］，采用五次多项式实现关节的轨迹函数的插值算法可以有效简化轨迹的表达形式。因此关节的轨迹函数可表示如下

令T 为轨迹的执行时间，有：T＝tf－ti，。

由方程式 （2）可知，关节的轨迹插值函数是关于时间t的函数，而t为轨迹的开始时刻与终止时刻中的任意时间变量，为了简化轨迹函数与时间的数学关系，对轨迹执行时间T 进行归一化处理。

已知关节位置函数θ（t）在初始时刻和终止时刻的约束值，因此关节位置的轨迹函数可表示如下

式 （3）中，s为比例函数。τ表示归一化时间，有

τ和s （τ）满足以下条件

2 基于归一化时间的轨迹描述

根据式 （3）对关节位置求解一阶时间导数和二阶时间导数，可以得到关节速度和关节加速度关于归一化时间τ的函数

式中：s（τ）和¨s（τ）分别表示s（τ）的一阶时间导数和二阶时间导数。

上述的关于归一化时间τ的关节位置、速度和加速度的表达式仍需要满足表达式 （1）所示的起始时刻和终止时刻的约束条件，因此，有

求解上述等式，可以求出s（τ）需要满足的6 个约束条件

采用五次多项式拟合s函数。令

将s（τ）函数在τ＝0及τ＝1时需要满足的6个条件表示成五次多项式的系数的函数，并采用矩阵形式表示

将方程组 （10）～ （15）中的约束条件代入表达式（17），可得到五次多项式的6个系数的表达式

将计算出的五次多项式的系数代入s 函数表达式（16），再将s函数代入关节的运动轨迹的函数 （3），可以得到关节旋转角度的轨迹的具体表达式

对表达式 （19）分别求解一阶时间导数和二阶时间导数，可得到关节运动的速度轨迹和加速度轨迹的表达式，分别如方程式 （20）和式 （21）所示

上述方程式 （19）～式 （21）分别表达了关节运动的位置、速度和加速度相对归一化时间τ的变化函数，若需将其转化成相对时间t的函数，将τ＝ （t－ti）/ （tf－ti）代入表达式 （19）～式 （21）中，即可计算关节位置、速度和加速度相对时间变化的运动轨迹。

3 基于位置约束的轨迹执行时间最优处理

轨迹规划给出的约束条件仅指定了关节在初始时刻和终止时刻的运动信息，实际工程中，由于机械臂的机械设计因素或者驱动电机功率的因素，关节的任意时刻的位置、速度和加速度均受一定的约束条件的影响。

假设由于关节的驱动器的驱出力矩要求，关节运动有最大的旋转加速度和最小旋转加速度 （令顺时针旋转方向为正）的限制条件，同理，关节的速度轨迹也需限制在关节的驱动电机的最小输出的旋转速率和最大输出旋转速率之间。此外，根据机械臂的设计参数可知，每个关节都有对应的旋转角度范围。因此，关节的位置轨迹也需要限制在关节的最小旋转角度和最大旋转角度范围之间。由于关节的运动加速度和速度限制主要与关节的驱动电机的参数有关，因此可以通过选择合适参数的电机来避免关节运动参数的限制。这里主要分析关节的运动位置的限制条件。

采用五次多项式对关节的运动轨迹进行插值时，关节运动的初始位置和终止位置均设定在关节的运动角度范围之内。因此若保证关节的旋转角度从初始时刻至终止时刻始终保持单调递增或单调递减的运动状态，即可保证关节的运动角度在执行时间内始终处于关节的运动范围之内。

从关节运动轨迹的表达式可以看出，当给定初始时刻和终止时刻的约束条件，轨迹曲线与轨迹执行时间T 有关。因此可以通过优化轨迹的执行时间T 调节关节的运动轨迹，使其在轨迹执行时间T 内保持单调递增或单调递减。根据微积分原理可知，位置轨迹的单调性与其对时间一阶导数的正负有关。若速度轨迹在执行时间T 内均大于零，则关节的位置轨迹单调递增；若速度轨迹在执行时间T 内均小于零，则关节的位置轨迹随时间单调递减。将采用五次多项式规划的关节速度轨迹表示如下

因此，执行时间T 的选择需要满足以下两个条件之一

由于关节的初始位置θi，终止位置θf以及轨迹执行时间T 均为已知量，且有

可将式 （23）中的约束条件转化成以下形式

比较表达式 （7）和式 （20），则s（τ）可表示为

采用Matlab软件对方程式 （26）进行运算，得到以下表达形式

其中，P 的具体表达式如下式所示

将关节初始时刻和终止时刻的运动参数的约束值分别代入表达式 （27）和式 （28）中，s（τ）即可简化为关于τ的一元四次方程。在Matlab里画出s（τ）相对于变量τ变化的曲线。则曲线s（τ）与τ轴交点即为方程的4个解。根据实际情况，令其任意相邻的两个解处于 ［0，1］的取值范围之外，即可保证s（τ）在τ∈ ［0，1］的取值范围内始终大于零或小于零。这样即可保证关节的位置曲线s（τ）在τ∈ ［0，1］的范围内始终单调递增或单调递减。

4 仿真与分析

随机给定一组轨迹规划任务，验证不同的轨迹执行时间T 的选择对关节运动轨迹的影响。假设运动关节在初始时刻的位置、速度和加速度值分别为θi＝1.5rad，θi＝2.88rad/s，¨θi＝－1.5rad/s2，在目标点的位置、速度和加速度分别设定为θf＝2.14rad，θf＝0，¨θf＝0。令关节运动的执行时间为T＝1 s。将表达式τ＝t/T 代入关于τ的运动轨迹表达式 （19）～式 （21），可在Matlab里描绘关节位置、速度和加速度相对时间的运动轨迹，如图1所示。图1（a）显示的关节位置曲线在执行时间内出现超调量，关节在0.3 s附近时刻已经到达目标位置，但由于执行时间选择较大，导致关节偏离目标位置一段时间之后再回到目标位置。很明显，该运动过程极大地降低关节的运动效率，而且有可能造成关节位置超出其运动范围，不利于机械臂的优化控制。

通过调整执行时间T 对五次多项式插值函数进行优化。首先将各约束条件的具体数值代入关节速度的表达式（27），并在Maple软件中调用factor函数，将关节的速度函数表示成4个因式连乘的形式

由于关节位置的初始值为1.5rad，终止角度为2.14rad。有θf＞θi，因此令θi为单调递增函数。期望关节位置在执行时间内始终变化单调递增，即要求关节的运动速度始终大于或等于零。即

式（29）中，（τ－1）2除了在终止时刻外，始终大于零。又有：θf－θi＞0。因此，若希望表达式 （29）满足s（τ）≥0的条件，则执行时间T 需要满足以下几个条件之一：

图1 关节运动位置、速度和加速度的轨迹曲线

在Maple软件中对条件一、条件二和条件三的几个方程组分别进行求解，结果是条件三的方程组无解，条件一和条件二求解得出执行时间T 应满足以下条件

将上述的约束条件进行整理，取其并集，可得到使得满足约束条件下五次多项式插值函数在关节运动时间内始终不产生超调量，执行时间T 应满足的约束条件

依据式 （32）给出的执行时间范围选择合适的变量T，令T＝0.4，代入五次多项式插值的位置、速度和加速度方程。描绘运动轨迹的曲线，如图2所示。

图2 轨迹执行时间约束后的关节运动曲线

将图2所表示的运动轨迹与图1作比较，可得出此下结论，当轨迹执行时间T 满足优化后的时间范围的限制条件时，关节在运动过程中位置始终处于初始位置与终止位置的范围之内，此时关节的位置曲线不再出现超调量。因此，对轨迹的执行时间T 进行优化可有效调整五次多项式插值的关节位置轨迹曲线，提高关节的运动效率，并使关节避开不能到达的运动范围。

5 结束语

本文在满足机器人运动空间的各种约束条件下对机器人的轨迹执行时间进行优化，实现了减少机器人运动时间，提高其工作效率的目标。为了方便轨迹的执行时间的提取计算，将轨迹的执行时间进行归一化处理，转化成无量纲的变量。并在此基础上根据给定的运动约束条件采用五次多项式拟合机器人在关节空间内的位置、速度和加速度的插值函数。为了避免关节的运动位置轨迹出现超调量，提高机器人的运动效率，分析关节速度轨迹与执行时间的数值关系，并计算执行时间的约束条件，从而实现关节运动轨迹的优化。在详细介绍该算法的理论推导之后，采用Matlab对关节空间的运动进行仿真验证，分别描绘优化前后关节的运动轨迹曲线，从而验证该优化算法的可行性及可靠性。仿真结果表明，通过调节轨迹的执行时间，可有效优化五次多项式插值的关节位置轨迹曲线，提高关节的运动效率，并使关节避开不能到达的运动范围。

本文重点考虑机器人的关节空间的位置轨迹的约束条件，下一步工作将详细考虑机器人关节空间的速度以及加速度的轨迹约束条件，研究机器人其它的运动性能指标优化处理算法，从而更有效地指导机器人在实际工程中的轨迹规划作业。

［1］SUN Liang，MA Jiang，RUAN Xiaogang.Trajectory planning and simulation of 6－DOF manipulator［J］.Control Engineering of China，2010，17 （3）：388－392 （in Chinese）.［孙亮，马江，阮晓钢.六自由度机械臂轨迹规划与仿真研究［J］.控制工程，2010，17 （3）：388－392.］

［2］MU Haihua，ZHOU Yunfei，YAN Sijie，et al.A third－order trajectory planning algorithm for ultra－precision point－to－point motion ［J］.Mechanical Science and Technology for Aerospace Engineering，2008，27 （2）：234－240 （in Chinese）. ［穆海华，周云飞，严思杰，等.超精密点对点运动3阶轨迹规划算法研究 ［J］.机械科学与技术，2008，27 （2）：234－240.］

［3］JIA Songtao，ZHU Yu，YIN Wensheng，et al.Trajectory planning for ultra－precision stages ［J］.Design ＆ Research，2007 （10）：14－17 （in Chinese）. ［贾松涛，朱煜，尹文生，等.超精密工作台运动轨迹规划 ［J］.设计与研究，2007（10）：14－17.］

［4］Gosselin Clement，Foucault Simon.Dynamic point－to－point trajectory planning of a two－dof cable－suspended parallel robot ［J］.IEEE Transactions on Robotics，2014，30 （3）：728－736.

［5］Berman Sigal，Liebermann Dario G，McIntyre Joseph.Constrained motion control on a hemispherical surface：Path planning ［J］.Journal of Neurophysiology，2014，111 （5）：954－968.

［6］Liu Huashan，Lai Xiaobo，Wu Wenxiang.Time－optimal and jerk－continuous trajectory planning for robot manipulators with kinematic constraints ［J］.Robotics and Computer－Integrated Manufacturing，2013，29 （2）：309－317.

［7］ZHANG Bin，FANG Qiang，KE Yinglin.Optimal time trajectory planning method for a kind of posture aligning system of large rigid bodies ［J］.Chinese Journal of Mechanical Engineering，2008，44 （8）：248－252 （in Chinese）. ［张斌，方强，柯映林.大型刚体调姿系统最优时间轨迹规划 ［J］.机械工程学报，2008，44 （8）：248－252.］

［8］ZHANG Bin.Joint－space trajectory planning for robots under multiple constraints［J］.Chinese Journal of Mechanical Engineering，2011，47 （21）：1－6 （in Chinese）. ［张斌.基于多约束的机器人关节空间轨迹规划 ［J］.机械工程学报，2011，47 （21）：1－6.］

［9］XU Haili，XIE Xiangrong，ZHUANG Jian，et a1.Global time－energy optimal planning of industrial robot trajectories［J］.Journal of Mechanical Engineering，2010，46 （9）：19－25 （in Chinese）.［徐海黎，解祥荣，庄键，等.工业机器人的最优时间与最优能量轨迹规划 ［J］.机械工程学报，2010，46 （9）：19－25.］

［10］Saravanan R，Ramabalab S，Balamurugan C.Multi－objective trajectory planner for industrial robots with payload constraints［J］.Robotica，2008，26：753－765.

［11］LI Dongjie，QIU Jiangyan，YOU Bo.Optimal algorithm for trajectory planning for the robot［J］.Electric Machines and Control，2009，13 （1）：123－127 （in Chinese）. ［李东洁，邱江艳，尤波.一种机器人轨迹规划的优化算法 ［J］.电机与控制学报，2009，13 （1）：123－127.］

［12］HAN Dapeng，WEI Qing，YANG Leping，et al.A twistbased method for real－time trajectory planning in task space［J］.Robot，2008，30 （4）：304－310 （in Chinese）. ［韩大鹏，韦庆，杨乐平，等.任务空间实时轨迹规划的旋量方法［J］.机器人，2008，30 （4）：304－310.］

［13］Haddad M，Khalil W，Lehtihet HE.Trajectory planning of unicycle mobile robots with a trapezoidal－velocity constraint ［J］.IEEE Transactions on Robotics，2010，26 （5）：954－962.