示教是工业机器人在喷涂应用中的一个重要环节。机器人的示教方法可以分为两大类：一种是直接面向机器人本体进行示教，称为直接示教；另一种是远离机器人本体通过计算机技术对机器人进行示教，称为离线示教。直接示教方法主要有示教盒示教和手把手示教两种。手把手示教是由操作人员直接拖拽机器人末端工具的方式实现复杂轨迹的示教。与示教盒示教和离线编程相比，手把手示教喷涂效果所见即所得，无需编程，简单易学，效率高，只需简短培训就可使原喷涂工人直接成为机器人的操作工人，解决了中小涂装企业技术升级后机器人应用人才短缺问题，并且避免了大量喷涂工下岗带来的社会问题。

在手把手示教过程中，主要存在两方面的问题。一方面，受到机器人拖拽灵活性和操作人员自身等因素的影响，示教路径和速度难以光顺，喷涂时速度和加速度会有突变，产生振动、冲击，影响喷涂质量。另一方面，由于机器人拖拽灵活性的影响，示教时难以达到或者难以迅速达到机器人期望的工作速度，导致机器人再现喷涂时只能同时提高整体的速度。因为这两方面问题的存在，使手把手示教机器人的现实应用受到一定的诟病。为解决上述问题，本文提出了一种对手把手示教喷涂采样数据进行运动规划的方法。

机器人关节角空间运动规划主要是对关节速度、加速度进行规划，以时间最优和平滑过渡为目标进行优化^[1-3]。由于从关节坐标到机器人末端工具轴位姿映射过程存在大量的三角函数等非线性运算，直接对关节坐标优化后会造成机器人末端机构位姿误差难以控制，使再现位姿与采样位姿产生较大误差。本文提出在笛卡尔空间中运用样条曲线对机器人手把手示教轨迹进行光顺处理，对再现运动速度进行重新规划，使得机器人再现运动时能够以较高的速度平稳运行，解决手把手示教存在的上述问题。

1 示教轨迹的位姿拟合

在手把手示教过程中，工业机器人控制器对6个关节伺服电机的绝对位置编码器数值进行时间分割采样，得到6关节坐标时间分割采样数据 $\{ {[\begin{array}{*{20}{c}} {{A_1}}, \cdots,{{A_6}} \end{array}]_i}\} _{i = {\rm{0}}}^n$ 构成示教轨迹。对采样得到的n个6关节坐标数据进行6关节机器人正运算，得到示教轨迹的矩阵变换表示，记为 ${\rm{\{ }}{{{M}}_i}{\rm{\} }}_{i = {\rm{0}}}^n$ ，如式(1)所示。

${{{M}}_i} = \left[ {\begin{array}{*{20}{c}} {{{{R}}_i}}&{{{{P}}_i}} \\ 0&1 \end{array}} \right]$

(1)

式中， ${{{R}}_i} \in {\mathbb{R}^{3 \times 3}}$ 表示第i个示教点对机器人基坐标系的旋转变换矩阵，包含喷枪的姿态信息； ${{{P}}_i} \in {\mathbb{R}^{3 \times 1}}$ 表示第i个示教点对机器人基坐标系的平移变换矩阵，包含喷枪的空间位置信息。喷枪空间位置集合记为 $\{ {{{P}}_i}\} _{i = 0}^n$ ，旋转变换集合记为 $\{ {{{R}}_i}\} _{i = 0}^n$ 。

对手把手示教轨迹进行优化，可以转化为对给定轨迹 ${\rm{\{ }}{{{M}}_i}{\rm{\} }}_{i = {\rm{0}}}^n$ 的运动规划问题。其整体的运动优化流程如图1所示。主要流程是对示教轨迹进行位姿拟合，使轨迹更加光滑。拟合完成后，再对其重新离散化，同时对其速度进行规划，使速度曲线平滑、加速度曲线无突变，最后进行喷枪姿态的插值，保证姿态变化平滑，机器人再现运动平稳。

1.1 轨迹预处理

轨迹的预处理主要目的是减少后续位姿拟合的计算量，适当去除一些示教点，用较少的示教点来表示轨迹而不丢失轨迹自身的特征。如图2所示，计算空间矢量 ${ {{S}} _i}$ ， ${ {{S}} _{i + 1}}$ 之间的夹角 ${\alpha _{i + 1}}$ ，并与给定的阈值 ${\alpha _0}$ 作比较。假如 ${\alpha _{i + 1}}$ 小于等于阈值 ${\alpha _0}$ ，则认为3点处于同一空间直线上，此时舍去中间点 ${{{P}}_{i{\rm{ + }}1}}$ 。假如 ${\alpha _{i + 1}}$ 大于阈值 ${\alpha _0}$ ，则保留中间点 ${{{P}}_{i{\rm{ + }}1}}$ 。

运用以上方法，对空间位置集合 $\{ {{{P}}_i}\} _{i = 0}^n$ 进行处理，可以得到空间位置特征点点集合 $\{ {{{P}}_i}\} _{i = 0}^m$ ， $m < n$ 。根据 ${\rm{\{ }}{{{P}}_i}{\rm{\} }}_{i = 0}^m$ 取对应示教轨迹点，得到示教轨迹特征点序列 ${\rm{\{ }}{{{M}}_i}{\rm{\} }}_{i = {\rm{0}}}^m$ 。

旋转矩阵R描述轨迹点的姿态较为复杂而且难以量化计算，本文引入单位四元数的方法描述机器人末端执行器的姿态轨迹曲线^[4-5]。空间某点在绕轴 $ {{n}} $ 旋转 $\theta $ 角度时，其单位四元数可以表示为 ${{q}} = (\cos ({\theta / 2})$ ， $\sin ({\theta / 2}) {{n}} )$ ，其中 $ {{n}} $ 为三维单位向量。根据旋转矩阵和四元数转换公式^[6]，将 $\{ {{{R}}_i}\} _{i = 0}^m$ 转换成单位四元数表示 $\{ {{{q}}_i}\} _{i = 0}^m$ 。因此，示教轨迹特征点序列 ${\rm{\{ }}{{{M}}_i}{\rm{\} }}_{i = {\rm{0}}}^m$ 可以表示为式(2)所示。

${{{M}}_i} = (\begin{array}{*{20}{c}} {{{{q}}_i}}&{{{{P}}_{{i}}}} \end{array})$

(2)

1.2 位姿轨迹拟合

为获得平滑连续的空间位置曲线和姿态曲线，对示教空间位置序列 $\{ {{{P}}_i}\} _{i = 0}^m$ 和姿态序列 $\{ {{{q}}_i}\} _{i = 0}^m$ 构造2阶连续的插值函数。

对于序列 $\{ {{{P}}_i}\} _{i = 0}^m$ ，包含的是机器人末端执行机构的空间位置轨迹信息，可以使用3次非均匀B样条曲线去描述位置曲线 ${{C}}(u)$ ，如式(3)所示。

${{C}}(u) = \mathop \sum \limits_{i = 0}^n {{{d}}_i}{N_{i,3}}(u)$

(3)

式中， $u \in [0,1]$ ， ${{{d}}_i}$ 表示B样条曲线的控制顶点， ${N_{i,3}}(u)$ 是定义在节点矢量U上的B样条曲线基函数。想要明确曲线 ${{C}}(u)$ 的表达式，需要求取计算节点矢量 ${{U}}[0,0,0,{u_3}, \cdots ,{u_{m + }}_3,1,1,1]$ 以及控制顶点 ${{{d}}_i}$ ，相关的求取计算方法可参考文献[7]。

由单位四元数 ${{q}} = (\cos ({\theta / 2}),\sin ({\theta / 2}) {{n}} )$ 的自然对数运算见式(4)^[8]。

$\left\{\begin{array}{l} {{p}} = {\rm{ln}}({{q}}) = \displaystyle\frac{\theta }{2} {{n}} \\ {{q}} = \exp ({{p}}) = \exp \left(\displaystyle\frac{\theta }{2} {{n}} \right) \end{array}\right.$

(4)

令函数 ${{p}}(t) = \ln ({{q}}(t))$ ，根据文献[9]可知，构造2阶连续的单位四元数插值函数 ${{q}}(t)$ 可以转化为构造2阶连续的函数 ${{p}}(t)$ 。

根据式(4)，可以将姿态序列 $\{ {{{q}}_i}\} _{i = 0}^m$ 转换成一个三维向量序列集合 $\{ {{{p}}_i}\} _{i = 0}^m$ 。同理，对 $\{ {{{p}}_i}\} _{i = 0}^m$ 采用3次B样条曲线进行描述，如式(5)所示。

${{p}}(t) = \mathop \sum \limits_{i = 0}^n {{{\varphi }}_i}{N_{i,3}}(t)$

(5)

根据同样的方法可以计算得到曲线 ${{p}}(t)$ 的节点矢量 ${{\varGamma}} [0,0,0,{t_3}, \cdots ,{t_{m + }}_3,1,1,1]$ 和控制顶点 ${{{\varphi }}_i}$ 。得到姿态曲线 ${{q}}(t)$ ，如式(6)所示。

${{q}}(t) = \exp ({{p}}(t))$

(6)

2 机器人位置曲线的插补

为获得新的离散示教路径，需要分别对位置曲线 ${{C}}(u)$ 和姿态曲线 ${{q}}(t)$ 进行插补离散。

2.1 B样条曲线泰勒展开插补算法

曲线参数化插补，即由轨迹空间的进给步长∆L映射到参数空间从而求取参数 ${u_{i + 1}} = {u_i} + \Delta {u_i}$ 。本文采用一阶泰勒展开进行空间位置曲线 ${{C}}(u)$ 的插补计算，其计算公式如式(7)所示^[10-11]。

${u_{i + 1}} = {u_i} + {u'}({t_i}){T_c} + o({T_c})$

(7)

式中， ${T_c}$ 为机器人运动插补周期， $o({T_c})$ 为泰勒展开余项。

对机器人末端位置拟合曲线 ${{C}}(u)$ ，在 $ t $ 时刻末端的瞬时速度可以表示为式(8)所示。

$v(t) = \left\| {\frac{{{\rm{d}}{{C}}(u)}}{{{\rm{d}}t}}} \right\| = \left\| {\frac{{{\rm{d}}{{C}}(u)}}{{{\rm{d}}u}}} \right\|\frac{{{\rm{d}}u}}{{{\rm{d}}t}}$

(8)

由式(8)，有式(9)

${u'}\left( t \right) = \frac{{{\rm{d}}u}}{{{\rm{d}}t}} = \frac{{v(t)}}{{\left\| {{{{C}}'}(u)} \right\|}}$

(9)

忽略泰勒余项，式(7)可以写为式(10)所示。

${u_{i + 1}} = {u_i} + \frac{{v({t_i})}}{{{\sigma _i}}}{T_c}$

(10)

式中， ${\sigma _i} = \left\| {{{{C}}'}(u)} \right\|$ ， $v({t_i})$ 是机器人末端在 ${t_i}$ 时刻的速度。

反向插补计算时，有式(11)。

${u_i} = {u_{i + 1}} - \frac{{v\left( {{t_i}} \right)}}{{{\sigma _{i + 1}}}}{T_c}$

(11)

2.2 位置曲线插补过程

工人示教速度不能满足机器人作业速度，不能很好地适用于产品加工，机器人再现速度需要提升。为了使机器人作业时速度更加平稳，插补时使用区域连续S型速度曲线对速度进行规划^[12-13]。

位置曲线 ${{C}}(u)$ 往往是比较复杂的，机器人末端执行机构需要在适当的位置提前减速，以较低的安全速度通过一些高曲率的点，避免速度变化给机器人本体带来冲击。这就需要在插补前对曲线 ${{C}}(u)$ 进行速度前瞻处理，提前找出速度突变点，并对突变点进行速度限制。速度突变点可以根据系统最大加速度的约束来寻找，其约束条件见式(12)所示。

$\left\{ {\begin{array}{*{20}{l}} {\kappa ({u_i}) > \kappa ({u_{i - 1}})}\\ {\kappa ({u_i}) > \kappa ({u_{i + 1}})}\\ {\kappa ({u_i}) > {{{A_{\rm{max}}}} / {v_{f}^2}}} \end{array}} \right.$

(12)

式(12)中， $\kappa ({u_i})$ 是曲线 ${{C}}(u)$ 在参数 ${u_i}$ 处的曲率， ${A_{\rm{max}}}$ 是系统限定的最大加速度， ${v_f}$ 是机器人作业速度。

突变点对应的限制速度可以通过系统法向加速度限制和系统弓高误差限制进行确定^[14-15]。在找到 ${{C}}(u)$ 中所有的突变点后，相当于将曲线分成了若干速度规划段，插补时根据各个速度突变点对应的限制速度对每段曲线进行区域连续S型曲线速度规划，这样就能确保机器人末端执行机构可以按限制速度通过突变点，并保证速度的平滑。

对曲线 ${{C}}(u)$ 进行区域连续S型曲线速度规划需要提前找到速度突变点的减速点，这就要求精确知道规划路径的总长度，但是B样条曲线不能精确计算得到规划路径长度。

针对这一问题，采用双向插补离散的方式寻找减速点。如图3所示，点 ${{C}}({u_n})$ 和 ${{C}}({u_m})$ 是两相邻突变点，对应参数为 ${u_n}$ 和 ${u_m}$ ，对应限制速度为 ${v_n}$ 和 ${v_m}$ 。双向插补是从点 ${{C}}({u_n})$ 正方向和点 ${{C}}({u_m})$ 反方向同时进行S型速度曲线加速段的插补，正方向进行插补计算1次，反方向也跟着插补计算1次，并且反向插补计算完成后才能进行下一次正向插补。根据文献[11]中的速度规划可知，对于给定的最大速度 ${v_{\rm{max}}}$ 、系统最大加速度 ${a_{\rm{max}}}$ 、起始速度 ${v_s}$ ，S型速度曲线加速段的表达式如式(13)所示。

${v_a}(t) = - \frac{{4{a_{\rm{max}}}}}{{3T_1^2}}{t^3} + \frac{{2{a_{\rm{max}}}}}{{3{T_1}}}{t^2} + {v_s}$

(13)

其中， ${T_1}$ 为从初始速度加速到最大速度所需要的时间，且 ${T_1} = {{1.5({v_{\rm{max}}} - {v_s})} / {{a_{\rm{max}}}}}$ 。

寻找减速点 ${{C}}({u_d})$ 的过程为

步骤1：假设曲线段 $|{{C}}({u_n}){{C}}({u_m})|$ 所能达到的最大速度 ${v_{\rm{max}}} = {v_f}$ ，给定最大加速为系统限定最大加速度 ${A_{\rm{max}}}$ 、插补周期 ${T_c}$ ，由式(13)得到由速度 ${v_n}$ 和速度 ${v_m}$ 加速至作业速度 ${v_f}$ 的速度表达式 ${v_{an}}(t)$ 和 ${v_{am}}(t)$ 。

步骤2：初始化正向插参数 $u_i^n = {u_n}$ 和反向插补参数 $u_{i + 1}^m = {u_m}$ ，参数u的上标表示插补方向。

步骤3：设置插补次数N，并计算正向插补速度 $v_i^n = {v_{an}}(N{T_c})$ 和反向插补速度 $v_i^m = {v_{am}}(N{T_c})$ 。插补次数指当前正在进行第N次插补，其初始值设置为1。

步骤4：计算正、反向插补参数 $u_{i + 1}^n$ 、 $u_i^m$ 。 $u_{i + 1}^n$ 根据式(10)和速度 $v_i^n$ 计算， $u_i^m$ 根据式(11)和速度 $v_i^m$ 计算。

步骤5：判断 $u_{i + 1}^n$ 和 $u_i^m$ 的大小。(1) 如果 $u_{i + 1}^n < u_i^m$ ，进入步骤6；(2) 如果 $u_{i + 1}^n = u_i^m$ ，进入步骤7；(3) 如果 $u_{i + 1}^n > u_i^m$ ，进入步骤8。

步骤6：判断插补速度 $v_i^n$ 和 $v_i^m$ 是否达到作业速度 ${v_f}$ 。(1) 若 $v_i^n \leqslant {v_f}$ 且 $v_i^m = {v_f}$ ，则减速点 ${{C}}({u_d}) = {{C}}(u_i^m)$ ，进入步骤9；(2) 若 $v_i^n = {v_f}$ 且 $v_i^m < {v_f}$ ，则回步骤3，N=N+1，后续步骤只计算反向插补，正向插补数据保持不变。(3) 若 $v_i^n < {v_f}$ 且 $v_i^m < {v_f}$ ，回步骤3，N=N+1。

步骤7：判断 $v_i^n$ 和 $v_i^m$ 是否相等。(1) 如果 $v_i^n = v_i^m$ ，则减速点 ${{C}}({u_d}) = {{C}}(u_i^m)$ ，进入步骤9；(2) 如果 $v_i^n \ne v_i^m$ ，进入步骤8。

步骤8：令 ${v_{\rm{max}}} = \alpha {v_{\rm{max}}}$ ，一般有 $0.5 \leqslant \alpha < 1$ ，回步骤1更新，重新进行查找计算。

步骤9：结束查找流程。

上述流程在查找减速点 ${{C}}({u_d})$ 的时候，已经完成加速段和减速段的插补离散化，可以保留其插补结果，避免重复计算。

3 姿态与位置的同步

在对位置曲线 ${{C}}(u)$ 进行插补离散化后得到机器人末端执行机构的空间位置路径离散序列 $\{ {{{P}}_i}\} _{i = 0}^k$ ，为了生成示教路径，还需要对姿态曲线 ${{q}}(t)$ 进行离散化得到姿态路径序列 $\{ {{{q}}_i}\} _{i = 0}^k$ 。

姿态曲线离散化时应该尽可能地保持与原姿态同步，也就是机器人末端机构运行通过新的示教路径上某一空间点时，姿态应当与原来的姿态尽可能保持一致。因此，需要建立位置曲线 ${{C}}(u)$ 参数u与姿态曲线 ${{q}}(t)$ 参数t直接的映射关系，如式(14)所示。

$t = f(u)$

(14)

因为位置曲线 ${{C}}(u)$ 是根据示教轨迹特征点序列 $\{ {{{M}}_i}\} _{i = 0}^m$ 中的空间位置信息进行拟合计算得到的，姿态曲线 ${{q}}(t)$ 则是根据 $\{ {{{M}}_i}\} _{i = 0}^m$ 中的姿态信息进行拟合计算得到的，并且 ${{C}}(u)$ 和 ${{q}}(t)$ 的节点矢量都是通过规范积累弦长化方法构造的。所以曲线 ${{C}}(u)$ 的节点矢量U与曲线 ${{q}}(t)$ 的节点矢量 ${{\varGamma}} $ 具有一一对应的关系，利用这一关系可以给出参数u对和参数t简单的线性映射关系。

首先判断参数u所在的节点矢量区间 $[{u_j},{u_{j + 1}}]$ ，通过节点矢量U和节点矢量 ${{\varGamma}} $ 的对应关系，得到参数t所在区间 $[{t_j},{t_{j + 1}}]$ 。然后建立参数u、t简单的线性关系，如式(15)所示。

$t = f(u) = {t_j} + \frac{{u - {u_j}}}{{{u_{j + 1}} - {u_j}}}({t_{j + 1}} - {t_j})$

(15)

将位置曲线 ${{C}}(u)$ 插补离散得到的参数序列 $\{ {u_i}\} _{i = 0}^k$ 映射到姿态曲线 ${{q}}(t)$ 的参数空间，可以得到姿态曲线 ${{q}}(t)$ 参数离散序列 $\{ {t_i}\} _{i = 0}^k$ 。把序列 $\{ {t_i}\} _{i = 0}^k$ 代入姿态曲线表达式，得到机器人末端执行机构姿态路径四元数序列 $\{ {{{q}}_i}\} _{i = 0}^k$ ，再根据旋转矩阵和四元数转换公式^[6]，可以得到旋转矩阵序列 $\{ {{{R}}_i}\} _{i = 0}^k$ 。结合空间路径序列 $\{ {{{P}}_i}\} _{i = 0}^k$ ，可以得到新的示教路径序列集合 $\{ {{M}}_i^*\} _{i = 0}^k$ ，如式(16)所示。

${{M}}_i^* = \left[ {\begin{array}{*{20}{c}} {{{R}}(t)}&{{{P}}(t)}\\ 0&1 \end{array}} \right]$

(16)

这样就实现了位置和姿态的同步。对示教路径序列集合 $\{ {{M}}_i^*\} _{i = 0}^k$ 进行机器人逆运动学计算可以得到6关节坐标数据，形成优化后的示教文件。

4 实验与仿真验证

本文以佛山某机器人公司自行研制的手把手示教喷涂机器人为实验平台，对提出的运动轨迹优化算法进行验证。实验截取广州某卫浴公司实际浴缸生产中玻纤−树脂复合材料喷涂的一段手把手示教路径，进行实验分析。该段示教轨迹由870个示教点组成，将这些示教点进行转化得到对应的机器人6个关节角角度，再通过机器人正运算，得到笛卡尔坐标系下的喷枪位姿。经过拟合优化处理，可以得到相应的位姿拟合曲线。给定喷涂速度 ${v_f}$ 为500 mm/s，最大加速度 ${A_{\rm{max}}}$ 为1 000 mm/s²，插补时间 ${T_c}$ 为10 ms，对拟合曲线进行离散化，可以得到优化后的示教路径。机器人喷枪优化前、后运动轨迹如图4(a)、(b)所示，空间轨迹上的短线条代表机器人喷枪的姿态向量。从图4中可以看出，优化后该段示教轨迹更加光滑，姿态变化连续、与原示教轨迹姿态同步。优化后路径点数目为350个，相比于原始数据减少了一半，再现喷涂整体速度得到提高。

为了进一步分析其拟合效果，进行位置曲线拟合误差计算，可以得到最大拟合误差为1.095 mm，平均拟合误差为0.073 mm，标准差为0.144 mm，拟合误差能满足机器人的喷涂精度。拟合误差分布如图5所示。

对优化效果进行验证，分别对原始轨迹和优化轨迹的机器人喷枪速度、加速度进行对比分析。图6是优化前、后喷枪速度、加速度对比图。从图6可以明显观察到原始轨迹速度不稳定，波动较大而且整体速度较低。原始轨迹加速度存在不规则波动，而且在一些点处有很大的突变。经过优化处理后的机器人喷枪轨迹运动速度曲线呈现出所规划的S型，整体较为平滑，整体运动速度得到提高，提高了机器人的工作效率。优化轨迹的加速度虽然仍然存在突变，但是比较优化前减小了一个数量级，加速度曲线得到改善。将优化轨迹数据转换成示教再现数据格式，在机器人上进行再现实验，实验结果显示，有效地减小了再现喷涂过程中的振动，喷涂效果明显改善，同时喷涂效率也得到了提高。

5 总结

本文提出的手把手示教机器人再现运动优化方法得到实验验证，结果表明此优化方法能够在保持原示教喷涂轨迹位姿精度的前提下，提高了机器人再现喷涂的平稳性，喷涂效果明显改善，同时也改善了工人示教速度与预期喷涂速度不匹配的问题，对提高手把手示教机器人在涂装行业上的使用具有一定的应用价值。