基于激光引导的SCARA 的轨迹规划与优化*

王 锴，关胜晓

(中国科学技术大学 信息科学技术学院，安徽 合肥230026)

0 引言

随着工业化进程的推进，采用机器代替人工已是大势所趋。目前生产线上的机器人大多遵循固定的工作模式，对于没有视觉信息的机械臂，其运动路径一般由程序给定，如果要改变其路径，则需通过手动示教或修改源程序的方式，而频繁的手动控制无益于实现装置的自动化。对于引入视觉信息的机械臂，可通过图像或视频算法识别环境信息，进而在线规划运动方案，提高了系统的适应能力。 但在环境复杂的场合，如特殊天气、光线昏暗、目标物特征难以提取等条件下，识别检测算法的复杂度将大幅提升，导致系统成本增加，不利于实际项目的运用。 工业生产线上，SCARA 因其结构相对简单、运行稳定高效的优点得到了广泛应用。 在SCARA 的设计工作上，学者们已经取得了丰硕的成果，1984 年，Adept 公司设计制造出第一台SCARA 机器人，从此奠定了后世SCARA 结构的雏形[1]。如今的研究工作多集中于结构的优化，如张红等采用有限元分析和拓扑优化方法实现了小臂的轻量化[2]。 为结合视觉信息，Hill 和Park 于1979 年提出了视觉伺服的概念，经过近五十年的发展，手眼视觉伺服、场景视觉伺服系统等已经广泛应用于实际生产[3]。 轨迹规划中，多项式插值是主流方法，张秀林利用B 样条曲线对关节轨迹进行逼近，基于非均匀变异，使用改进的遗传算法求得最优时间，完成了最优轨迹规划[4]。

为拓宽机械臂的应用场景，降低系统成本，在定位精度有一定忍耐限度的前提下，提出了一种基于激光引导的SCARA 方案。通过DH 法建立运动学模型，分析得到工作空间，并根据点到点和多路径点的应用需求，分别使用五次多项式插值和三次B样条曲线插值法进行轨迹规划，为使首末时刻关节加速度为零，引入了修正因子，可以大幅降低机械臂受到的冲击。 结合约束条件设计罚函数，对遗传算法种群初始化、选择和变异方向进行改进，可以加速种群进化，并取得时间上的近似最优解。 结果表明，相较于传统的遗传算法，改进的遗传算法能更快地收敛，得到的最优解比未优化前有明显的改观。

1 激光引导SCARA 的系统组成原理

针对无视觉系统适应性差和智能视觉系统复杂度高的问题， 引入激光作为视觉识别的目标信息，用激光投影制造的特征代替复杂环境中的不确定特征，大大降低识别和定位难度，同时结合视觉信息，可识别视场中的障碍物，估计深度信息，为定位提供视觉反馈，使系统同时具备无视觉系统和智能视觉系统二者的优点。

如图1 所示，系统主要由任务调度器、图像质量优化算法、结合激光的视觉伺服系统和监控平台组成。

为确保系统的稳定运行，上位机嵌入了任务调度策略。 自动运行部分包含激光头投射动作序列、SCARA 动作序列、故障事件的处理动作及系统状态量的监测。 手动控制部分包含控制命令解析与处理、系统状态监测。 保证装置状态在任何时刻都是可控和稳定的。

图1 激光引导SCARA 的系统组成图

为配合视觉伺服系统，激光头安装在设计的托盘上，位于SCARA 大臂下侧，可以防止机械臂运行时造成的遮挡。 其投射方向由安装在托盘上的两个直流电机控制，可根据要求改变激光头水平方向的偏移角和竖直方向的俯仰角。 通过编写预置程序的方式控制激光投射轨迹，也可用遥操作的方式，在控制端实时发送指令，控制投射点的移动。 使用激光制造特征简易有效，系统能在光线不足的情况下正常运行，面对特殊天气状况时(雨、雾等)，加入去雨和去雾算法，提升图像质量。 安装在SARA 机械臂末端的单目相机以固定频率采集平面图像，提取投射点特征，计算靶心坐标，经过坐标系变换，得到世界坐标系下的表示，最后再通过运动学逆解计算各关节的关节角。 对于起点到目标点的路径，系统提供了多种方案。 如果路径形状无限制，可采用常用的五次多项式插值，在关节空间完成平滑轨迹的规划。 如果要求经过某些路径点，可采用基于三次B 样条的规划方案，根据给定的型值点计算控制点，利用约束的运行时间，可得到对应的角度、角速度、角加速度方程，表达系统运动状态的变化。结合各运动学变量的约束，以最短时间为目标函数，使用遗传算法求解最短时间的近似解，可完成时间最优轨迹的规划。 如果对轨迹形状有严格要求，则需在笛卡尔空间进行规划。 轨迹跟踪算法引入滑模控制方法，其不需要精确的系统模型，具备优良的抗干扰性能，设计的快速终端滑模面能加速被控量的收敛， 并有效降低系统状态切入滑膜面产生的抖振问题。

监控平台作为人机交互的接口，是自动化设备中不可缺少的部分。 通过有线串口或无线通信的方式收集传感器信息，实时显示在软件界面中，相机回传的图像可以帮助操作人员掌握机械臂的工作状态。 采集的数据存放在本地数据库中，利于后续查阅，也可同步至云平台，实现远程监控。提供运行模式切换按钮、机械臂动作按钮、紧急动作按钮，通过向下位机发送对应指令，进而控制电机动作。 手动模式支持路径点记录，激光打样后，控制器计算和记录这些点的世界坐标，根据用户给定的目标点进行轨迹规划，并检验轨迹合理性，如果轨迹不可达，系统将发出提示信息，用户可自行切换至自动运行或重新给定路径点。

2 建立SCARA 机械臂的运动学模型

运动学模型分为正向运动学和逆向运动学模型。 二者的求解方向完全相反。 已知各关节的关节角，求解末端的目标位姿，称为正向运动学建模；由目标位姿反求各关节的关节角，称为反向运动学建模[5]。 实际情况中，一般已知目标位姿，需控制电机驱动机械臂末端到达指定位置，所以需要进行逆向运动学建模。

建立运动学模型前，必须建立合适的参考系，并明确各坐标系间的转换关系。 本文以桃子TZ-500 SCARA 机器人作为实验平台，使用DH 坐标系法在四个关节建立坐标系，其中，关节1、2、4 为旋转关节，关节3 为移动关节，表1 为DH 参数表，坐标系建立如图2 所示。 以{O0-X0Y0Z0}为世界坐标系，其位置是绝对的，不随机械臂运动，轨迹规划时需要将末端位姿的坐标转为世界坐标系下的表示。

表1 SCARA 机械臂DH 参数

矩阵Ti表示坐标系{Oi-XiYiZi}到坐标系{Oi-1-Xi-1Yi-1Zi-1}的转换矩阵，1≤i≤4。

图2 TZ-500 SCARA 机械臂DH 坐标系

将四个矩阵相乘，可得到机械臂末端相对于世界坐标系的转换关系：

其 中，θ12-4表 示θ1+θ2-θ4，θ12表 示θ1+θ2。 式(5)为末端位姿矩阵，(px，py，pz，1)为终点的齐次坐标，一般情况下，这一信息是已知量。 结合式(5)、式(6) ， 进 行 逆 运 动 学 求 解， 得 到 各 关 节 的 关节角：

根据机械臂产品手册， 获得TZ-500 SCARA 的第一段臂长L1=250 mm，第二段臂长L2=257 mm，基座高度d1=200 mm，末端安装长度d3=74.5 mm，前两个关节的限位角为[-140°，140°]。 利用已知参数，编写MATLAB 程序，建立TZ500 的仿真模型，绘出其工作平面，如图3 所示。

图3 TZ500 SCARA 机械臂的工作空间俯视图

3 SCARA 机械臂的轨迹规划策略

合理的轨迹规划方案可以保证机械臂能快速、稳定、安全地抵达目标位置，为后续的轨迹跟踪提供路径指导。 轨迹规划分为笛卡尔空间的规划和关节空间的规划。 由于前者需要通过运动学逆解在笛卡尔坐标和关节坐标之间频繁转换，因此计算量很大。关节空间的规划容易满足运动学和动力学约束[6]，因而是目前广泛采用的方式。

3.1 点到点的五次多项式插值规划

多项式插值函数广泛应用于轨迹规划中，但函数阶次越高，其凸包性越差，不利于轨迹的优化[7]，为了保证加速度的连续平滑，阶次必须大于4，故两点间的轨迹可采用五次多项式进行规划。 由运动学模型可知， 前两个关节决定了机械臂末端在平面上的位置，假设在世界坐标系中，末端起始点的XOY 平面坐标为(507，0)，激光投射点的平面坐标为(300，200)，关节角度q 是时间的五次函数，可写为如下形式：

两边对时间求解一阶导数和二阶导数，得到：

其 中，q0、qd分别为起始点和终点的关节角，v0、vd分别为起始点和终点的关节角速度，α0、αd分别为起始点和终点的关节角加速度，t0、td分别为运动的初始时刻和到达终点的时刻。 通过逆运动学求解，可得关节1 的qd=-0.205 4 rad， 关节2 的qd=1.559 5 rad，在电机提供的力矩足够的前提下(后文同)，规定起点和终点的速度，加速度为0，运行时间为3 s，带入数据计算，可得五次多项式的系数，整理得到两个关节轨迹的五次方程：

其中，qi(t)代表第i 个关节角度随时间的变化关系。 对式(10)做微分运算，可得到角速度、角加速度的数学描述，两个关节的角度、角速度、角加速度的曲线如图4、图5 所示。

可以看到，五次多项式的规划方案，可以保证角度、角速度、角加速度变化的连续和平滑，关节所受冲击也是连续的。 在轨迹形状没有严格要求时，采用五次多项式插值，可以获得良好的运动学特性。

3.2 多路径点的三次B 样条曲线规划

实际工作中，受到环境的约束，用户可能会要求SCARA 机械臂必须经过指定的路径点。 此时点到点的插值无法满足控制要求。 由于笛卡尔空间规划的计算量偏大，在路径点之间轨迹形状没有严格要求的情况下，可采用B 样条曲线规划。 B 样条曲线可以拥有任意的形状，其具备连续性和局部支撑性[8]，各段轨迹能平滑连接，每段轨迹的形状由若干个控制点控制，如果该段轨迹违反了约束条件，通过调整控制点，便可以对轨迹进行修正，同时不影响其他符合要求的轨迹，从而简化了设计。 手动控制激光投射，通过视觉检测点位置，控制器保存目标点作为型值点，利用目标点信息计算出B 样条曲线控制点，再结合约束条件得到对应的轨迹，可保证工作空间的路径经过指定的点。 考虑到计算复杂度，装置采用了基于三次多项式的B 样条曲线插值。 均匀B 样条基函数的定义为：

图4 关节1 角度、角速度、角加速度时间曲线

图5 关节2 角度、角速度、角加速度时间曲线

这是著名的Cox-deBoor 递推公式。 其中Ni，k表示第i 段k 阶B 样条基函数，集合{u0，u1，…，um}是m+1 个非递减数组成的节点向量，0≤i≤m-1，k≥1，m+1 为节点数量，[ui，ui+1]为第i 个节 点 闭 区 间，一 般 情 况 下，0 ≤u ≤1。 则B 样 条曲线方程可表示为：

其中，p 为实际曲线上对应的点，d 为控制点，N为基函数，n+1 为控制点数量。对于有n+1 个控制点的k 次B 样条曲线，其节点数为n+k+2。当k=3，即采用基于三次多项式的基函数时，每段轨迹的控制点有4 个，利用式(11)，可求出各阶B 样条的基函数，解得三次B 样条基函数：

只要确定4 个控制点，就能计算出每段的实际轨迹：

式中，i=1，2，…，m-1，m 为关节空间的节点数。{Vi-1，Vi，Vi+1，Vi+2}是 一组控 制 点，θi(u)为 第i 段u 处对应的关节角。 利用关节角变化连续的条件，可得：

其中，Pi为 型值 点，i=1，2， …，m，θ0和θm分别 代 表起点和终点。 可以发现，式(15)一共有m 个方程，但控制点数量有m+2 个，如果要求出所有控制点，需要再补充两个条件[9]。 对式(14)求解一阶导数和二阶导数，得出角速度和角加速度的表达式：

规定初始速度和终止速度为0，则ω1(0)=0，ωm-1(1)=0，代入式(16)，得到V0=V2，Vm+1=Vm-1。如果令初始加速度和终止加速度为0，则可求得V0=2V1-V2，Vm+1=2Vm-Vm-1，在世界坐标系下，取四个XOY平面的点p1(507，0)，p2(400，100)，p3(300，200)，p4(150，300)，单位为mm。 经运动学逆解，转化为关节空间的型值点P1，P2，P3，P4，以弧度表示，由式(17)反求出控制点：

6 个控制点可形成三段轨迹，由式(14)，规定每段轨迹的运行时间为3 s，可得关节1 三段轨迹的表达式如式(18)所示：

同样地，可以得到关节2 的三段轨迹表达式。限于篇幅，本文仅以关节1 为例，绘出各运动量随时间变化的曲线，如图6～图8 所示。

图6 关节1 的关节角曲线

图7 关节1 的关节角速度曲线

图8 关节1 的关节角加速度曲线

图中，AB、BC、CD 分别代表了三段轨迹对应的运动学特征。 可以发现，角度和角速度在全过程都是平滑连续的，但角加速度在初始时刻和终止时刻发生突变，因而此处机械臂所受冲击较大，表现为明显的抖振，这对机械臂控制精度的提高和使用寿命都是不利的。

为了解决初始时刻加速度不为0 的问题，引入了修正因子。 由式(16)注意到，a1(0)=V0-2V1+V2，现在在式(14)的基础上添加修正因子M(u)，它是一个五次函数，以0，1 两个边界点作为零点，第一段轨迹对应修正因子M1(u)，第三段轨迹对应修正因子M3(u)：

将u 转化为时间t 表示的形式，此时角加速度在起始时刻和终止时刻的值均为0。 角速度和角加速度曲线如图9、图10 所示。

图9 加入修正因子的关节1 角速度曲线

图10 加入修正因子的关节1 角加速度曲线

可见，初始时刻和终止时刻的加速度已经为0，意味着冲击大大减小。 在轨迹过渡段虽然连续，但不平滑，这是采用三次多项式的缺陷，本文采用基于三次多项式B 样条曲线规划，是对降低计算复杂度和提升运动性能所做的平衡。

3.3 基于改进遗传算法的最优时间轨迹规划

最优时间的轨迹规划，对于提高机械臂的运行效率至关重要。 研制自动化基坑开挖系统的初衷，就是在缩减人力成本的前提下，进一步提高施工效率。旋挖土方时，需要将挖出的土方转移至堆积区，再返回继续作业。 这段轨迹一般有较远的距离，且转移土方的动作序列重复度高，如果机械臂的轨迹是时间最优的，将会显著地提高开挖效率。 遗传算法是解决优化问题的优秀方案，其原理是模拟自然界的种群繁衍，通过选择初始种群，不断地进行交叉、选择、变异，最终获得性能优良的个体[10]。 而选择的标准，则是定义的适应度函数。 算法的主体包括初始种群的选取和初始化，种群编码，适应度计算，交叉，选择，变异，不断迭代，直到满足算法终止条件。本文基于前文三次B 样条曲线的结果进行优化。

(1)种群选择与初始化

算法的优化目标是时间最优解，这里设置种群区间为[0.1，3](取0 会导致后续计算中表达式分母为0)，设种群数量为100，将此区间均分为100 份，在每个区间内产生一个个体，这样可以保证个体均匀分布在解空间内，同时保证了随机性。

(2)编码

采用二进制编码，方便后续的交叉与变异操作。规定可行解的精度为0.001，则二进制编码长度为12 位。

(3)定义适应度函数

结合角速度大小约束、角加速度大小约束以及不同段轨迹切换时的连续性，得到式(21)的限定条件：

其中，0≤u≤1；i 为关节 编号，i=1，2；j 为第j 段轨迹，j=1，2，3。 式(21)从上到下代表的含义分别为：所有关节在任意段轨迹的最大角速度不超过60°/s，关节1 在任意段轨迹的最大角加速度不超过45°/s2，关节2 在任意段轨迹的最大角加速度不超过60°/s2，所有关节在轨迹切换时的角速度、角加速度是连续的。 基于罚函数设计适应度函数，当所有变量满足约束条件时，惩罚项很小，反之，惩罚项越大，适应度函数的值越小，达到筛选子代个体的作用：

其中，T为轨迹运行总时间，P1、P2、P3为惩罚项，τi为第i个关节的力矩，τi，max为第i个关节可提供的最大力矩，fitness 为适应度函数。

(4)选择与交叉

运用轮盘赌算法，根据个体的适应度值计算个体在子代中出现的概率， 适应度值越高的个体，其被选择的概率越大。 按照适应度从大到小排序，保留较大的一半作为下一代的一部分，并记录每代的最优个体。 选择两个个体作为父亲和母亲，并进行单点交叉。原则上，交叉操作不能破坏优良的个体，因而，对适应度高的个体，交叉概率应较小，对于适应度适中的个体， 其产生优良个体的概率较大，应提高其交叉变异的概率。 另外，后期交叉变异的概率相对于前期应该减小，因为后期更注重于局部搜索，过大的概率参数将使收敛时间增加。 结合以上分析，设计自适应的交叉率pc：

其中，f 为所选个体的适应度，sumf为种群适应度总和，p 即为f 所占比例，G 为总迭代次数，g 为当前迭代次数，size 为种群大小。 p 越接近平均适应度，交叉概率越大。

(5)变异

合理的变异操作可以减小后期陷入局部极值的可能，增加局部搜索能力[11]。前期变异概率太小，将无法发挥变异增加种群多样性的效果，后期变异概率太大，产生不良个体的概率也会变大，延长优化时间，因而采用固定的变异概率是不合适的。 这里根据迭代的代数，逐渐减小变异概率：

目标函数可定义为：

前文的B 样条曲线轨迹，自定义运行时间为每段3 s，取T1=T2=T3，绘出[0.1，3]定义域内的目标函数曲线，并生成包含300 个个体的种群，观察它们的分布情况。 具体分布见图11。

图11 种群个体分布图

可以发现，最优解很靠近图中的曲线，说明最优解中三段轨迹的运行时间应当是接近的。 初始交叉率为0.8，变异率为0.2，迭代300 次，可获得最优解的近似，分别采用未改进的遗传算法和改进后的遗传算法展开计算，获得迭代次数和最优解(轨迹总运行时间)的趋势图，如图12、图13 所示。图中每个“*”号代表着每一代的最优个体，在传统遗传算法中，大约在60 代后趋于稳定，但能发现，所求的值明显还可以继续优化，属于局部最优。 采用固定的交叉变异率，可能破坏了一些已有的优良个体，使得个体远离最优解,且存在最优解求解稳定性差的问题。 改进后的遗传算法迭代了30 代后就开始趋于稳定了，且后期没有产生差异巨大的不良个体。求得的最优解由圆圈标出，见图12(b)、图13(b)。 最优解为T1=2.230 4 s，T2=2.230 4 s，T3=2.230 4 s。

图12 传统遗传算法的优化过程

图13 改进遗传算法的优化过程

根据求得的最优解，与前文定义的3 s 运行轨迹的方案做对比，以关节1 为例，得到角度、角速度、角加速度曲线，如图14 所示。

可以看到，在各参数满足约束条件的前提下，优化时间达到了6.7 s 左右，已经有了较为明显的提升。

4 结论

作为智能开孔机器人项目的重要部分，基于激光引导的SCARA 装置旨在以低成本且有效的方式利用视觉信息，提升系统对不同环境的适应性。 装置设计并涵盖了最优轨迹规划在内的多种规划方案，并配备成熟的滑模变结构控制方法。 系统任务调度器指导执行机构的动作，保证装置稳定运行。支持遥操作等多种方式的人机交互，提高管理简便性的同时，降低了工人的劳动强度。 通过DH 坐标系建立了TZ500 机械臂的正逆运动学模型，并根据实际参数绘制了工作空间。 针对点到点简单轨迹和多限定点的复杂路径，分别采用五次多项式插值和三次B 样条插值进行规划，并获得了符合要求的轨迹。 对初始时刻和终止时刻加速度不为0 的问题，引入了修正因子，有效地降低了首末两个时刻机械臂受到的冲击。 基于三次B 样条曲线规划，利用改进的遗传算法，再结合约束条件，设计了带惩罚项的适应度函数，求解时间最优解，实验证明，改进的遗传算法收敛速度更快，优化后的运行时间比优化前有较为明显的缩短，且求解精度更高。 针对加速度不平滑的问题，后续可考虑使用基于五次多项式的B 样条曲线规划，为了提高运行速度，可做多线程程序编写的尝试，通过提高并发度充分利用计算机资源，从而削减计算复杂度提升带来的负面影响。

图14 优化前后的角度、角速度、角加速度对比