基于正弦加减速控制算法的工业机器人位姿轨迹规划研究

黄忠明，陈爱文，黄凤良

（1.南京师范大学 电气与自动化工程学院，南京 210042；2.扬州职业大学 电子工程学院，扬州 225009）

0 引言

位姿轨迹规划是通过给出有限个示教点规划得到笛卡尔空间下的目标轨迹，是实现机器人高效、准确、平稳地执行作业任务、减少繁重的机器人示教工作的重要手段。位姿轨迹规划包括位置轨迹规划和姿态轨迹规划，二种轨迹规划一般分开进行，也有有一些学者试图将二者同时规划，但不管如何规划，只有位置轨迹与姿态轨迹同时光滑连续且同步运行，才能保证机器人末端执行器的任务轨迹连续光滑。位姿轨迹在启停阶段会存在速度阶跃的情况，这会对机器人机械本体造成冲击，关节上的驱动器无法补偿如此巨大的力而导致电流过载，同时，机器人本体的振动和冲击会影响工作质量、造成本体磨损影响机器人寿命[1]。杜亮、张铁[2]采用位姿分离的方式，先插补机器人的位置点，再在各位置点上以欧拉角的方式插补机器人姿态，这种方法简单方便，实现了位姿轨迹同步规划，提高了机器人控制的实时性，但无法实现启停阶段位置轨迹和姿态轨迹的（角）速度与（角）加速度的连续性。宋金华[3]采用7段S曲线加减速算法与三次样条插值算法结合的方法，根据速度变化将轨迹分段，每段轨迹均采用三次样条插值，并以此作为插值曲线来规划末端执行器的直线轨迹和圆弧轨迹，但7段S曲线加减速算法速度分段较多、算法复杂，且未提及位置规划过程中的姿态处理方法。陈伟华、张铁[4]研究了三次均匀有理B样条在自由曲线轨迹中的应用，曲线插补过程中同样进行了加减速控制，但其采用的线性加减速控制并不能使运动加速度连续。

本文采用正弦加减速控制算法，以笛卡尔空间下的直线轨迹为目标位置轨迹，单位四元数表示姿态，分别规划位置轨迹和姿态轨迹，并对两种轨迹进行位姿同步规划处理，获得位置和姿态轨迹均连续光滑且同步变化的位姿轨迹，以解决启停阶段（角）速度和（角）加速度不连续的问题。

1 位姿轨迹规划算法研究

1.1 位置轨迹规划

设机器人末端执行器从Ps点沿直线运动到Pe点，起始点Ps坐标为（xs，ys，zs），终点Pe坐标为（xe，ye，ze），采用定时插补的方式，设插补周期为Δt，运行速度为v，从起点到第i个插补点的位移可表示为si=iΔs=ivΔt。将直线PsPe表示为关于是时间t的参数方程的形式，则第i个插补点的X，Y，Z坐标分别为：

式中，Δx=xe-xs，Δy=ye-ys，Δz=ze-zs，。（xi，yi，zi）即为第i个插补周期t=iΔt时刻末端执行器的三维坐标。

本文采用正弦加减速控制算法对运动过程进行加减速分段，原理如下：

1）加速阶段：加速度a随时间按正弦规律变化，从0变化到最大值，再变化为0。整个加速阶段，加速度的值始终位于y坐标轴的正半轴，而速度也会在此阶段从0达到最大值，即匀速阶段速度cm。

2）匀速阶段：以恒定速度vm运行至减速阶段，该阶段也是机器人末端执行器运行的主要阶段。

3）减速阶段：加速度a随时间按正弦规律变化，从0变化到最小值，再变化为0。整个加速阶段，加速度的值始终位于y坐标轴的负半轴，而速度也会在匀速阶段速度vm减小到0，此时机器人末端执行器走完预设轨迹，到达目标位置，运行结束。

给定若干个示教点，设定最大速度vm，最大加速度am，则整个过程加速度的表达式为：

由式(2)积分得到速度表达式为：

由式(3)积分得到位移表达式为：

在不考虑关节空间下对笛卡尔空间预设轨迹进一步细分的情况下，关节驱动器驱动各关节运动的进给量就是一个插补周期内机器人末端执行器沿X，Y，Z三个方向上的变化量对应的关节角变化量，由式(1)及si=iΔs可知，的大小与运行完第i个插补周期t的位移si相关，因此，直线段的加减速划分考察的是直线沿X，Y，Z三个方向的位移s随运行时间t和运行速度v的变化情况。设机器人运行完第i个插补周期t的位移为si，匀速段速度为vc，最大加速度为am，则由式(4)得加速、匀速和减速三个阶段对应的位移si分别为：

加速阶段（0＜t≤t1）：

匀速阶段（t1＜t≤t2）：

减速阶段（t2＜t≤Tpos）：

各参数确定后，需校验匀速段速度vc是否“可达”，即在给定的加速度下机器人末端执行器可以经历加速、匀速和减速的完整过程，当整个运动过程处于临界状态时（末端执行器加速到速度最大值时直接进入减速阶段），t1=Tpos-t1=t2。已知加速度段的位移为：

当vc≤vcmax时，匀速段速度vc“可达”，即运动过程有匀速段，否则需要重新设定最大运行速度vc或者最大加速度am。

1.2 姿态轨迹规划

四元数表示的两姿态间的轨迹规划通常采用单位四元数球面线性插值的方法，设末端执行器的起始Ps和结束点Pe对应姿态的单位四元数分别表示为Q0和Q1，二者间的角位移为，总运动时间为Tort，插补周期为Δtt，则球面插值曲线为：

式中，Slerp函数的返回值Qu表示末端执行器在起始姿态和结束姿态间的一个中间姿态，0≤u≤1，u的取值为，其中n=Tort/Δt。

四元数球面线性插值虽然可以实现两姿态间的轨迹插值，但在启停阶段存在姿态角速度不连续的情况，因此论文同样采用正弦加减速控制算法，对姿态角的变化过程进行加减速控制。设定最大角速度wc，最大角加速度am，参照正弦加减速控制算法在位置轨迹规划中的应用，得不同阶段两姿态间角位移θ随时间的变化关系为：

速度规划的本质是控制角位移的变化，即对球面线性插值公式slerp函数中的u的控制。设两姿态间的角位移为，使代入slerp，得到随插补周期t变化的四元数序列，即起始点姿态向结束点姿态变化过程中的姿态点序列，其余参数的确定方法与位置轨迹中的确定方法相同。最后将四元数序列转化为姿态矩阵序列[5,6]。

1.3 位姿同步规划

机器人运行过程中末端执行器的位置和姿态一般是同步变化的的，以保证末端执行器平稳运行，因此需要对位置轨迹和姿态轨迹进行同步规划处理。设位置轨迹运行总时间和姿态轨迹运行总时间分别为Tpos和Tort，当Tpos≠Tort时，通过调整位置轨迹或姿态轨迹中一方的最大速度或最大角速度值，重新规划位置或姿态轨迹，使两个轨迹的运行时间相等。根据Tpos和Tort的大小关系分三种情况进行同步处理：

1）Tpos=Tort：位置轨迹与姿态轨迹运行时间相等，满足位姿同步的要求。

2）Tpos＞Tort：为之轨迹运行时间大于姿态轨迹运行时间，此时增加Tort，使Tort=Tpos，计算相应的匀速段角速度wc，重新规划姿态轨迹。

3）Tpos＜Tort：位置轨迹运行时间小于姿态轨迹运行时间，此时增加Tpos，使Tpos=Tort，计算相应的匀速段速度vc或角速度wc，重新规划位置轨迹。

2 仿真结果与分析

以Cyton Gamma300七自由度机器人为实验对象，取末端执行器的两个示教点分别作为起始点和终止点，对应的起始点位姿矩阵Ts和终止点位姿矩阵Te分别为：

在MATALB环境下验证上述基于正弦加减速控制算法的位姿轨迹规划算法。

位姿矩阵中位置坐标的单位为毫米（mm），姿态的单位为弧度（rad），以起始点为例，机器人末端执行器的位置和姿态在矩阵中分别对应为：

设定机器人末端执行的最大运行速度为300mm/s，最大运行加速度为500mm/s2，最大姿态角速度为0.3rad/s，最大姿态角加速度为0.5rad/s2，插补周期为2ms，经本文的位姿轨迹规划算法得到机器人的不同视角下的位姿轨迹如图1所示。

轨迹中位移、速度及加速度随时间变化的关系如图2所示。

图1 直线位姿轨迹

图2 直线位置轨迹位移、速度及加速度变化

由图2可知，末端执行器在直线运动启停加减速阶段、匀速阶段的变化均符合足正弦加减速控制策略的控制规律，整个过程位移、速度及加速度连续均连续变化，符合位置规划算法的预期目标。从图2中可以看出，直线位置轨迹最大运行速度并不是设定的0.3m/s，这是因为在进行位姿同步处理时姿态轨迹运行时间大于位置轨迹运行时间，此时同步处理算法通过改变最大运行段速度重新对位置轨迹进行插补，从而使位置轨迹运行时间和姿态轨迹运行时间相等，达到位姿同步的目的。

直线轨迹中还包含了姿态轨迹的变化，姿态的角位移、角速度及角加速度虽时间的变化关系如图3所示。

图3 姿态轨迹位移、速度及加速度变化

从图3可知，姿态轨迹规划算法可以实现启停阶段姿态角速度和角加速度变化的连续性，符合姿态轨迹规划的预期目标。同时从图3中可以看出，姿态轨迹的运行时间为2.85秒，与图2中所示的位置轨迹运行时间相等，验证了位姿同步处理算法的可行性。

3 结论

本文提出了基于正弦加减速控制算法的工业机器人位姿轨迹规划算法，通过将正弦加减速控制策略应用到机器人末端执行器的位置轨迹和姿态轨迹的同步规划中，使其在启动阶段、匀速运行阶段及停止阶段间平稳光滑过渡。仿真结果表明，启停阶段末端执行器的（角）速度和（角）加速度连续变化，整个运行轨迹连续、平滑。