关节空间和工作空间的混合轨迹规划算法研究

宁学涛，黄 伟，郑天江，杨亚威

（1.中国科学院宁波材料技术与工程研究所，宁波 315201；2.中北大学，太原 030000）

关节空间和工作空间的混合轨迹规划算法研究

宁学涛1,2，黄 伟1,2，郑天江1，杨亚威1

（1.中国科学院宁波材料技术与工程研究所，宁波 315201；2.中北大学，太原 030000）

0 引言

轨迹规划对于提高机器人的使用效率，减小工作过程中的振动具有重要意义。合理的轨迹规划能够使驱动电机力矩和功率得到的充分的利用，并从软件控制上提高机器人的整体性能。

华南理工大学的陈伟华等人[1]，对机器人的连续路径进行了工作空间内的轨迹规划，在路径的拐角处采用关节空间轨迹规划进行过渡，但是，并没有进行相应的动力学优化。同济大学的李万莉等人[2]，江南大学的凌家良等人[3]，进行了纯关节空间内的轨迹规划，也没有进行相应的动力学优化。

对于每个运行阶段，机器人都有不同的运动学和动力学要求，例如，对于本文中所述的Delta两自由度高速并联工业机器人，在竖直方向物体抓取和释放阶段，要求机器人末端执行器应尽量减小水平方向的抖动，同时应尽量减小关节空间所需驱动力矩和功率，并且减小水平方向的抖动应作为这一阶段的主要矛盾加以解决，即拥有良好的工作空间性能；在水平转运阶段和中间过渡阶段，要求减小关节空间驱动力矩和功率，同时应尽量减小关节空间的输入角速度，即拥有良好的关节空间性能。由于纯工作空间和纯关节空间轨迹规划得到的拟合曲线一般具有良好的工作空间或关节空间性能，为了从理论上提高机器人的性能，本文将对机器人的竖直物体抓取和释放阶段采用工作空间五次样条函数轨迹规划方法，对水平转运阶段和中间过渡阶段采用关节空间五次样条函数轨迹规划方法，最后通过实验验证方法的合理性。

1 工作空间关键点的选取

综合，工作空间和关节空间轨迹规划中工作空间关键点的需求，在工作空间选取了9个工作空间关键点，如图1所示，其中曲线01、78段为工作空间轨迹规划段，该阶段是前面所述的物体抓取和释放曲线段，应具有良好的工作空间性能，即应尽量减小水平方向的抖动。其余曲线段为关节空间轨迹规划阶段，该阶段要求机器人具有良好的关节空间驱动力矩和功率，由于该阶段的关键点对称布置，得到的工作空间拟合曲线没有较大的抖动。

图1 工作空间关键点的选取

关键点2、6为工作空间拐弯半径控制点，对该两点的位置进行调节不仅可以改变拐弯半径，还可以调节23、34、45、56曲线段竖直方向的抖动；关键点1、7为工作空间轨迹规划和关节空间轨迹规划的衔接点，在该两点处应该选择合理的工作空间速度和加速度数值，以求得到较好的工作空间和关节空间拟合曲线。

现只对其中一个关键点数值选取进行陈述，另一关键点数值选取与该关键点类似。当速度选择不合理时，该关键点（时间为0.2s，蓝色曲线和红色曲线衔接处）成为速度拟合曲线的尖点（即从曲线上明显突出的点），并且该关键点附近的加速度急剧变化、数值较大，如图2、图3所示，分别为关键点处速度选择较小、较大时的关节空间左驱动电机拟合曲线。

图2 关键点处速度较小

图3 关键点处速度较大

当加速度选择不合理时，该关键点成为速度曲线的曲率变化关键点，并且该关键点的加速度出现尖点，如图4、图5所示，分别为关键点处加速度选择较小、较大时的关节空间左驱动电机拟合曲线。

图4 关键点处加速度较小

图5 关键点处加速度较大

当关键点处的速度、加速度选择不合理时，还会出现工作空间位移拟合曲线过冲，速度拟合曲线抖动较大，加速度拟合曲线峰值较大，所需驱动电机力矩峰值和功率峰值急剧增加的现象，这里不再赘述。

2 混合轨迹规划五次样条函数模型

混合轨迹规划五次样条函数模型是工作空间轨迹规划和关节空间轨迹规划相结合的数学模型，工作空间轨迹规划曲线段01、78段使用工作空间轨迹规划数学模型，工作空间的五次样条函数轨迹规划数学模型如式(1)所示。

通过计算得到五次样条函数系数如式(3)所示：

为了使每段关节空间内关节角位移、速度、加速度、加加速度拟合曲线连接处连续可导，需要建立合理的边界条件。Delta两自由度高速并联工业机器人有两个关节输入量，现只对关节角1的边界条件进行阐述，关节角2的边界条件与关节角1相同，不同之处在于工作空间内7个关键点经过运动学逆解得到的关节空间内的7关键点的数值不同。根据已经给定的关节角1的已知量和每段拟合曲线的边界条件建立如下方程式。

过境货物分为直接过境和间接过境，而其中间接过境的货物和准备在国内销售的货物不一样，属于尚未进口的货物，根据我国《海关知识产权保护条例》的规定，我们只对进口货物有执法权，但在自贸区内，国内海关对于进口的货物却无执法权，因为政策规定进入海关的货物免于办理申报手续，国内海关不可以采取知识产权保护措施的问题便相继出现。鉴于此，重庆自贸区应该在其以全方位开放为主、货物和监管更加便利等特殊性的条件下，建立一个区别于传统自贸区的独特模式。因此自贸区内的过境货物应视为进口货物，统一过境货物执法措施。

拟合曲线1独有方程式：

拟合曲线1、2共有方程式：

拟合曲线2独有方程式：

拟合曲线n独有方程式：

将得到关节空间内从关键点1到关键点7的7个点的含有36个未知量的36个关于时间的线性方程，对其进行整理得到矩阵：B=KA。

其中：

是36×36的时间矩阵；

A为待求的方程组的系数；

系数矩阵A=K-1B，通过计算得到了关节角1的拟合曲线的系数矩阵A，即得到了关节角1从关键点1到关键点7的拟合曲线，同理可得到关节角1从关键点7到关键点1的拟合曲线，关节角2的拟合方法与关节角1类似，不再赘述。

3 动力学轨迹规划优化模型

混合轨迹规划的动力学优化模型，结合了工作空间轨迹规划的动力学优化模型和关节空间轨迹规划的动力学优化模型，在工作空间轨迹规划01、78曲线段使用工作空间动力学轨迹优化数学模型进行动力学优化；其余关节空间轨迹规划曲线段使用关节空间的动力学轨迹优化数学模型进行动力学优化。

图6 混合轨迹规划流程图

Delta机器人混合空间轨迹规划流程如图6所示，其中判断1为选取的工作空间关键点处速度、加速度是否合理，即关节空间中拟合曲线，速度是否为尖点，并且加速度是否急剧变化、数值较大；工作空间中拟合曲线，位移是否过冲，速度拟合曲线抖动是否较大，加速度拟合曲线峰值是否较大，以及所需驱动电机力矩峰值和功率峰值是否急剧增加、数值较大。判断2为得到的工作空间内末端执行器的速度是否小于等于速度所要求的最大峰值，为了增加机器人的末端执行器的运行速度，应尽量使末端执行器的速度维持在峰值；判断3为得到的工作空间内的末端执行器的加速度是否小于等于加速度所要求的最大峰值。判断4为得到的关节空间内的拟合曲线的关节角位移、速度是否没有过冲，速度、加速度、加加速度拟合曲线峰值是否相差较小；判断5为得到的关节驱动力矩峰值和功率峰值是否相差较小，为了充分利用驱动电机的性能，应尽量使每条拟合曲线段内的关节驱动力矩或驱动功率维持在峰值附近。以上判别量都是时间的函数，修改每段拟合曲线的时间即可对运动学和动力学的拟合曲线进行整体优化。

4 拟合曲线分析

根据Delta机器人工作空间和关节空间的混合轨迹规划五次样条函数模型及其动力学优化模型，编写机器人的Python语言混合空间轨迹规划程序，得到的拟合曲线如图7～图9所示，其中，蓝色、黑色曲线为工作空间轨迹规划得到的拟合曲线，红色、绿色曲线为关节空间轨迹规划得到的拟合曲线。

图7 左右驱动关节运动学拟合曲线

图7为混和轨迹规划法得到的关节空间内左右驱动关节运动学拟合曲线，由上至下分别表示驱动关节角位移、速度、加速度。由图可以看出，关节空间内的位移、速度拟合曲线均连续可导，加速度拟合曲线连续但不可导。左右驱动关节的速度大小均小于8rad/s，左驱动关节加速度峰值小于200rad/s2，右驱动关节加速度峰值小于150rad/s2。

图8 末端执行器拟合曲线

图8为混合轨迹规划法得到的工作空间内末端执行器x轴方向和y轴方向拟合曲线，由上至下分别表示末端执行器的位移、速度、加速度拟合曲线。由图可以看出，利用以上混合轨迹规划五次样条函数模型及其动力学优化模型，得到的工作空间内末端执行器的位移、速度拟合曲线均连续可导，加速度拟合曲线连续但不可导，拟合曲线x轴方向和y轴方向速度峰值大小约为3m/s，加速度峰值大小为60m/s2，得到的工作空间内的x、y轴方向速度、加速度拟合曲线的峰值相差较小。

图9 左右驱动关节动力学拟合曲线

图9为混合轨迹规划法动力学优化后得到的关节空间内驱动电机力矩和功率拟合曲线。由图可知，左右驱动关节力矩拟合曲线和功率拟合曲线均连续但不可导，左驱动关节力矩拟合曲线峰值大小小于80N.m，右驱动关节力矩拟合曲线峰值大小小于等于90N.m，左右关节驱动力矩峰值大小相差较小；左右驱动电机的功率拟合曲线为取绝对值后的拟合曲线，左驱动关节功率拟合曲线峰值大小小于400w，右驱动关节功率拟合曲线峰值大小小于550w，左右关节驱动功率峰值大小相差较大。

由Delta机器人的混合轨迹规划五次样条函数模型及其动力学优化模型得到的拟合曲线分析可知，在末端执行器竖直抓取和释放物体01、78曲线段，将末端执行器水平x方向是否抖动作为主要衡量指标，将末端执行器y方向速度极值大小，关节空间速度、加速度大小，驱动电机力矩极值大小和功率极值大小等作为次要衡量指标，可知混合轨迹规划法得到的工作空间内末端执行器竖直运行阶段拟合曲线明显好于关节空间轨迹规划法得到的拟合曲线，这将十分有利于提高抓取和释放物体时末端执行器的稳定性；在末端执行器的水平转运和中间过渡阶段，将末端执行器x方向速度极值大小，关节空间速度、加速度大小，驱动电机力矩极值大小和功率极值大小等作为主要衡量指标，将末端执行器y方向是否抖动作为次要衡量指标，可知混合轨迹规划法得到的工作空间内末端执行器水平转运和中间过渡阶段拟合曲线明显好于工作空间轨迹规划法得到的拟合曲线，这将十分有利于提高Delta机器人的实际控制性能，同时降低了所需驱动电机的力矩和功率，具有很强的实用价值。

5 实验结果分析

为了对混合轨迹规划程序结果的实际应用价值进行验证，从Copley交流伺服电机驱动器中读取了驱动电机的实际运行参数，图10、图11中紫红色曲线分别为左右驱动电机的位移曲线。从图中可以看出，利用混合空间轨迹规划法得到的驱动电机的实际位移曲线均连续可导，与理论规划结果图7对比可知，实际的位移曲线对时间轴做了翻转，但两曲线的趋势相同。

图10 左边驱动电机位移图

图11 右边驱动电机位移图

图12 左边驱动电机速度图

图13 右边驱动电机速度图

图12、图13为左右驱动电机速度图，绿色曲线为轨迹规划指令曲线，紫红色曲线为驱动电机的实际速度曲线，与理论规划结果图7对比可知，测得的速度曲线对时间轴做了翻转，实际速度曲线稍有抖动，相对于纯工作空间或纯关节空间轨迹规划得到的拟合曲线，工作空间和关节空间轨迹规划得到的拟合曲线更有利于提高机器人末端执行器和机器人的实际控制性能。

6 结束语

本文以提高工作空间末端执行器的物体抓取和释放曲线段工作空间拟合曲线与工作空间水平转运和过渡阶段的关节空间拟合曲线性能为目标，旨在于利用工作空间和关节空间轨迹规划的优点进行混合空间的轨迹规划，以此提高机器人的性能，并由得到的拟合曲线和实验证明了该方法的合理性。

[1]陈伟华,张铁,崔敏其.基于五次多项式过渡的机器人轨迹规划的研究[J].煤矿机械,2011,32(12):49-50.

[2]李万莉,陈熙巍,茹兰.基于SimMechanics的4自由度机器人的轨迹规划和仿真系统设计[J].中国工程机械学报,2008,6(2):144-148.

[3]凌家良,施荣华,王国才.工业机器人关节空间的差值轨迹规划[J].惠州学院学报,2009,29(3):52-57.

[4]Liu, I. H., Tsai,M. C.,Cheng, M. Y.,& Su, K. H. Planning and implementation of motion trajectory based on C 2 PH spline[A].IEEE Industrial Electronics Society[C].2011:246-251.

[5]Gasparetto A, Zanotto V. Optimal trajectory planning for industrial robots[J].Advances in Engineering Software, 2010,41(4):548-556.

[6]G.B. Zhou, B.R. Song, J.L. Xie. Numerical calculation[M].Beijing. Higher Education Press,2008.

Mixed joint space and work space trajectory planning algorithm

NING Xue-tao1,2, HUANG Wei1,2, ZHENG Tian-jiang1, YANG Ya-wei1

为了实现机器人的高速、高精度控制，必须对机器人进行合理的轨迹规划，同时，轨迹规划也

宁学涛（1989 -），男，河北唐山人，硕士研究生，研究方向为高速并联工业机器人。

是实现驱动电机力矩和功率得到充分利用的核心内容。将编写Python语言的轨迹规划程序，并使用关节空间和工作空间的五次样条函数混合轨迹规划法对Delta两自由度机器人进行轨迹规划，再利用动力学对轨迹规划结果进行相应的优化。生成的拟合曲线表明，机器人的混合轨迹规划法充分利用了关节空间和工作空间轨迹规划法的优点，并且动力学优化后的结果，对提高驱动电机的使用效率具有很好的指导性意义，最后通过实验验证了结果的正确性。

机器人；运动学；动力学；混合空间；轨迹规划

TP242

A

1009-0134(2015)07(下)-0020-05

10.3969/j.issn.1009-0134.2015.07(下).06

2015-04-05

宁波市国际合作项目（2014D10008）；数控一代机械产品创新应用示范工程（2014BAZ04784）；精密驱动控制技术创新团队（2012B82005）；宁波市自然科学基金（2014A610084）