轨迹规划是机器人运动控制的核心^[1]，旨在确定机器人末端执行器在其工作空间的轨迹^[2]。轨迹规划分为关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划算法具有约束条件少和计算速度快等优势，但存在空间轨迹不直观和轨迹形状会随速度不同而改变等缺点^[3-5]。笛卡尔空间轨迹规划描述路径准确，可以直观地观测到机械手末端的运动轨迹。笛卡尔空间轨迹规划包括位姿规划和速度规划，即通过设计机器人末端位置和姿态的数学表达式，并结合加减速控制模型加以实现^[6]。

目前机器人系统中常用的速度规划算法有梯型速度规划算法和S型速度规划算法。后者因其速度曲线形如S状而得名，广泛应用于高速、高精度的加工系统^[7]。Hu等^[8]通过梯型速度规划方法控制机床，验证加速度的不连续变化会引起振动和冲击。史步海等^[9]提出一种等价梯型的新型S型速度规划算法，算法简单，一定程度上降低机械冲击，但实质上仍属于梯型速度规划。Jahanpour等^[10]针对加速度的不连续变化，研究一种基于S型的速度曲线的轨迹规划方法，但未考虑路径约束下会对速度规划参数的影响。

本文以机械手笛卡尔空间的轨迹规划为目标，建立空间直线和圆弧插补运动的位姿模型。采用S型加减速控制模型，并根据路径长度和始末速度等约束条件，将S型速度规划分4种类型。将算法集成于自主研发的六自由度机械手实时控制平台中，验证了本文算法的有效性和合理性。

1 S型速度规划

常用的七段式S型速度曲线由加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段组成。本文根据路径长度和始末速度等约束条件将S型速度曲线分4种类型，如图1所示。

图1中， ${t_1} \sim {t_2}$ 表示分段时刻，用 ${T_i}$ 表示分段用时。设定插补最大速度 ${v_{\max }}$ ，最大加速度 ${a_{\max }}$ ，加加速度 $J$ ，根据速度曲线图知：

${s_1} = \frac{{Jv_{\max }^2 + v_{\max }^{}a_{\max }^2}}{{2Ja_{\max }^{}}}$

(1)

${s_2} = \frac{{a_{\max }^3}}{{{J^2}}}$

(2)

式中： ${s_1}$ 表示经过加加速阶段、匀加速阶段、减加速阶段达到最大速度完成的位移； ${s_2}$ 表示仅经过加加速段和减加速阶段达到最大速度完成的位移。

类型一　七段式。若 $s > 2{s_1}$ ，则在给定距离 $s$ 范围内可以达到最大速度 ${v_{\max }}$ 和最大加速度 ${a_{\max }}$ ，速度规划为七段式，如图1(a)所示。

七段式分段用时分别为

${T_1} = {T_3} = {T_5} = {T_7} = \frac{{{a_{\max }}}}{J}$

(3)

$ {T_2} = {T_6} = \frac{{{v_{\max }}}}{{{a_{\max }}}} - \frac{{{a_{\max }}}}{J} $

(4)

${T_4} = \frac{{s - 2{s_1}}}{{{v_{\max }}}}$

(5)

七段式速度函数为

$v(t) = \left\{ \begin{array}{l} \displaystyle\frac{1}{2}J{t^2}, \qquad t < {T_1}\\ \displaystyle\frac{1}{2}JT_1^2 + {a_{\max }}(t - {T_1}), \\ \qquad{T_1} < t < \displaystyle\sum\limits_{i = 1}^2 {{T_i}} \\ {v_{\max }} - \displaystyle\frac{1}{2}J{\left(\displaystyle\sum\limits_{i = 1}^3 {{T_i}} - t\right)^2},\\ \qquad\displaystyle\sum\limits_{i = 1}^2 {{T_i}} < t < \sum\limits_{i = 1}^3 {{T_i}} \\ {v_{\max }}, \qquad \displaystyle\sum\limits_{i = 1}^3 {{T_i}} < t < \displaystyle\sum\limits_{i = 1}^4 {{T_i}} \\ {v_{\max }} - \displaystyle\frac{1}{2}J{\left(t - \displaystyle\sum\limits_{i = 1}^4 {{T_i}} \right)^2}, \\ \qquad \displaystyle\sum\limits_{i = 1}^4 {{T_i}} < t < \displaystyle\sum\limits_{i = 1}^5 {{T_i}} \\ {v_{\max }} - \displaystyle\frac{1}{2}JT_5^2 - {a_{\max }}\left(t - \sum\limits_{i = 1}^5 {{T_i}} \right), \\ \qquad \displaystyle\sum\limits_{i = 1}^5 {{T_i}} < t < \sum\limits_{i = 1}^6 {{T_i}} \\ \displaystyle\frac{1}{2}J{\left(\sum\limits_{i = 1}^7 {{T_i}} - t\right)^2}, \\ \qquad \displaystyle\sum\limits_{i = 1}^6 {{T_i}} < t < \sum\limits_{i = 1}^7 {{T_i}} \end{array} \right.$

(6)

七段式位移函数为

$ d(t) = \left\{ \begin{aligned} & \frac{{J{t^3}}}{6},\quad t < {T_1}\\ & {d_1} + \frac{J}{2}T_1^2(t - {T_1}) + \frac{{a_{\max }^{}}}{2}{(t - {T_1})^2},\quad{T_1} < t < \sum\limits_{i = 1}^2 {{T_i}} \\ & {d_2} + {v_{\max }}\left(t - \sum\limits_{i = 1}^2 {{T_i}} \right) + \frac{J}{6}{\left(\sum\limits_{i = 1}^3 {{T_i}} - t\right)^3} - \frac{{J}}{6}{T_3}^3,\\ & \qquad \sum\limits_{i = 1}^2 {{T_i}} < t < \sum\limits_{i = 1}^3 {{T_i}} \\ & {d_3} + {v_{\max }}\left(t - \sum\limits_{i = 1}^3 {{T_i}}\right),\quad\sum\limits_{i = 1}^3 {{T_i}} < t < \sum\limits_{i = 1}^4 {{T_i}} \\ & {d_4} + {v_{\max }}\left(t - \sum\limits_{i = 1}^4 {{T_i}} \right) - \frac{1}{6}J{\left(t - \sum\limits_{i = 1}^4 {{T_i}} \right)^3},\\ &\qquad \sum\limits_{i = 1}^4 {{T_i}} < t < \sum\limits_{i = 1}^5 {{T_i}} \\ & {d_5} + \left({v_{\max }} - \frac{1}{2}JT_5^2\right)\left(t - \sum\limits_{i = 1}^5 {{T_i}} \right) - \frac{{a_{\max }^{}}}{6}{\left(t - \sum\limits_{i = 1}^5 {{T_i}} \right)^2},\\ & \qquad \sum\limits_{i = 1}^5 {{T_i}} < t < \sum\limits_{i = 1}^6 {{T_i}} \\ & s - \frac{1}{6}J{\left(\sum\limits_{i = 1}^7 {{T_i}} - t\right)^3},\quad\sum\limits_{i = 1}^6 {{T_i}} < t < \sum\limits_{i = 1}^7 {{T_i}} \end{aligned} \right. $

(7)

其中，

$\left\{ \begin{split} & {d_1} = \frac{{a_{\max }^3}}{{6{J^2}}}\\ & {d_2} = {d_1} + \frac{J}{2}T_1^2{T_2} + \frac{{{a_{\max }}T_2^2}}{2}\\ & {d_3} = {d_2} + \frac{{{V_{\max }}{a_{\max }}}}{J} - \frac{{a_{\max }^3}}{{6{J^2}}} = {s_1}\\ & {d_4} = s - {d_3}\\ & {d_5} = s - {d_2}\\ & {d_6} = s - {d_1} \end{split} \right.$

式中 ${d_1} \sim {d_6}$ 表示分段运行距离。

类型二　六段式。若 $ \begin{array}{l} s \geqslant 2{s_2}\,\, \text{且}\,\, s < 2{s_1} \end{array} $ ，则在给定距离范围内可以达到最大加速度 $a_{\max }^{}$ ，但达不到最大速度 ${v_{\max }}$ ，速度规划为六段式，如图1(b)所示。

最大速度值修改为

${v_{\max }} = \frac{{ - \displaystyle\frac{{a_{\max }^2}}{J} + \sqrt {{{(\displaystyle\frac{{a_{\max }^2}}{J})^2}} + 4{a_{\max }}s} }}{2}$

(8)

六段式需要调整的时间段为

${T_4} = 0$

(9)

类型三　五段式。若 $ \begin{array}{l} {v_{\max }} \leqslant \displaystyle\frac{{a_{\max }^2}}{J} \end{array} $ 且 $ \begin{array}{l} s > 2{v_{\max }}\sqrt {\displaystyle\frac{{{v_{\max }}}}{J}} \end{array} $ ，则在给定距离 $s$ 范围内可以达到最大速度 ${v_{\max }}$ ，但不能达到最大加速度 $a_{\max }^{}$ ，速度规划为五段式，如图1(c)所示。

最大加速度值修改为

${a_{\max }} = \sqrt {{v_{\max }}J} $

(10)

五段式需要调整的时间段为

${T_2} = {T_6} = 0$

(11)

${T_4} = \frac{{s - 2{v_{\max }}\sqrt {\displaystyle\frac{{v_{\max }^{}}}{J}} }}{{{v_{\max }}}} = \frac{s}{{{v_{\max }}}} - 2\sqrt {\frac{{v_{\max }^{}}}{J}} $

(12)

类型四　四段式。如果 $s < 2{s_2}$ ，则在给定距离 $s$ 范围内既达不到最大加速度也达不到最大速度，速度规划为四段式，如图1(d)所示。

最大加速度值修改为

${a_{\max }} = \sqrt[3]{{\frac{{s{J^2}}}{2}}}$

(13)

最大速度值修改为

${v_{\max }} = Jt_1^2$

(14)

四段式需要调整的时间段为

${T_2} = {T_4} = {T_6} = 0$

(15)

2 笛卡尔空间轨迹规划

本文研究的笛卡尔空间的位姿规划主要包括直线插补运动和圆弧插补运动的位置规划和姿态规划。

2.1 位置规划

1)直线插补。

给定空间两点 ${P_1}({x_1},{y_1},{z_1})$ ， ${P_2}({x_2},{y_2},{z_2})$ ，则各插补点的表达式为

$s = \sqrt {{{({x_2} - {x_1})}^2} + {{({y_2} - {y_1})}^2} + {{({z_2} - {z_1})}^2}} $

(16)

$\left\{ \begin{array}{l} {x_k} = {x_1} + {\lambda _k}({x_2} - {x_1})\\ {y_k} = {y_1} + {\lambda _k}({y_2} - {y_1})\\ {z_k} = {z_1} + {\lambda _k}({z_2} - {z_1}) \end{array} \right.$

(17)

式中： ${\lambda _k}$ 为归一化因子， ${\lambda _k} \in [0,1]$ ， $s$ 为直线段距离。

2)圆弧插补。

设三维空间中任意不共线的三点 ${P_1}({x_1},{y_1},{z_1})$ 、 ${P_2}({x_2},{y_2},{z_2})$ 、 ${P_3}({x_3},{y_3},{z_3})$ ，对其确定的圆弧进行轨迹规划：

①确定空间圆弧的圆心坐标 ${P_0}\left( {{x_0},{y_0},{z_0}} \right)$ 及半径 $r$ ， ${P_1}$ 、 ${P_2}$ 、 ${P_3}$ 三点确定的空间平面的方程为： $Ax + By + Cz + D = 0$ ，具体方法可参照文献[11]，这里不再赘述。

②在局部坐标系 $o - xyz$ 下建立空间圆弧所在平面的新坐标系 $o' - x'y'z'$ 。以圆心 ${P_0}$ 为新坐标系原点 $o'$ ，令 $x'$ 轴的方向为 ${P_0}{P_1} $ 如图2所示。则 $x'$ 轴在局部坐标系内的方向余弦为

${{n}} = {\left[ {\frac{{{x_1} - {x_0}}}{r}{\rm{ }}\;\frac{{{y_1} - {y_0}}}{r}{\rm{ }}\;\frac{{{z_1} - {z_0}}}{r}} \right]^{\rm{T}}}$

(18)

令 $z'$ 轴方向为 $ {{{{P_1}}}{{{P_2}}}} \times {{{{P_2}}}{{{P_3}}}} $ ，则 $z'$ 轴在局部坐标内的方向余弦为

${{a}} = {\left[ {\frac{A}{k}{\rm{ }}\;\frac{B}{k}{\rm{ }}\;\frac{C}{k}} \right]^{\rm{T}}}$

(19)

式中 $k = \sqrt {{A^2} + {B^2} + {C^2}} $ 。

$y'$ 轴方向可由右手法则确定，其方向为 ${{o}} = {{a}} \times {{n}}$ 。令 ${{p}} = {\left[ {{x_0}\;{y_0}\;{z_0}} \right]^{\rm{T}}}$ ，由齐次变换原理^[12]，可求得坐标系 $o' - x'y'z'$ 到局部坐标系 $o - xyz$ 的齐次变换矩阵 ${{T}}$ ：

${{T}} = \left[ {\begin{array}{*{20}{c}} {{n}}&{{o}}&{{a}}&{{p}}\\ 0&0&0&0 \end{array}} \right]$

(20)

令局部坐标系内的任意一点 $P$ 的齐次坐标为 ${\left[ x\;y\;z\;1 \right]^{\rm{T}}}$ ，则 $P$ 点在坐标系 $o' - x'y'z'$ 的坐标为 ${\left[ x'\;y'\;z'\;1 \right]^{\rm{T}}}$ ，有

${\left[ {x'\ y'\ z'\ 1} \right]^{\rm{T}}} = {{{T}}^{ - 1}}{\left[ {x'\ y'\ z'\ 1} \right]^{\rm{T}}}$

(21)

3)求圆弧的夹角 $\theta $ 。由式(21)计算出空间三点 ${P_1}$ 、 ${P_2}$ 、 ${P_3}$ 在坐标系 $o' - x'y'z'$ 下相应坐标为： ${P'_1}({x'_1},{y'_1},0)$ 、 ${P'_2}({x'_2},{y'_2},0)$ 、 ${P'_3}({x'_3},{y'_3},0)$ 。

整个圆弧轨迹插补过程的角度为 $\theta $ 。得到：

$\theta = \left\{ \begin{array}{l} \arctan 2({y_3}^\prime ,{x_3}^\prime ) + 2{\text{π}},\quad{\rm{ }}{y_3}^\prime < 0;\\ \arctan 2({y_3}^\prime ,{x_3}^\prime ),\quad{y_3}^\prime \geqslant 0.{\rm{ }} \end{array} \right.$

(22)

4)在圆弧所在平面上 ${\varPi _1}$ 进行平面规划。基于齐次变换原理，可得

$\left\{ \begin{array}{l} {\theta _k} = {\lambda _k} \cdot \theta \\ {{x'}_i} = r \cdot \cos {\theta _k}\\ {{y'}_i} = r \cdot \sin {\theta _k}\\ {{z'}_i} = 0 \end{array} \right.$

(23)

对 ${{\textit{λ}} _k}$ 进行S型速度规划便可完成圆弧运动的插补计算，根据弧长和速度等约束条件可分为七段、六段、五段、四段4种情况，可参考直线部分。

2.2 基于四元数的姿态规划

四元数具有无数据冗余、不存在万向节锁死等优点^[13]。设 $q = \left[ {s,\left( {a,b,c} \right)} \right]$ ，由机器人正运动学齐次矩阵可求对应的两个互补的单位四元数：

$\left\{ \begin{array}{l} s = \pm \sqrt {{n_x} + {o_y} + {a_z} + {1 / 2}} \\ a = {{({o_z} - {a_y})} / {4s}}\\ b = {{({a_x} - {n_z})} / {4s}}\\ c = {{({n_y} - {o_x})} / {4s}} \end{array} \right.$

(24)

设工业机器人的初始姿态和目标姿态分别为R₁和R₂，根据式(24)可求出其对应的四元数q₁和q₂。由于每个姿态矩阵对应两个互补的四元数，考虑到时间最优，选择与前一姿态夹角较小的一组四元数。定义两个四元数夹角为 $\varOmega $ ，则：

$\cos \;\varOmega = {q_1} \cdot {q_2}$

(25)

对于两点间的姿态可由Slerp公式^[13]求出：

${\rm{Slerp}}({q_1},{q_2},{\lambda _k}) = \frac{{{q_1}\sin (1 - {\lambda _k})\varOmega + {q_2}\sin ({q_2}\varOmega )}}{{\sin \;\varOmega }}$

(26)

2.3 轨迹规划流程

轨迹规划需通过插补算法来实现。本文采用弧长增量法的插补技术，即在每个插补周期内，按照进给速度用微小线段去逼近目标轮廓线^[14]。本文设计的插补算法程序流程如图3所示。

设采样时刻为 ${t_k}$ ，代入式(7)可得：

${\lambda _k} = \frac{{d({t_k})}}{{{d_i}}}{\rm{ }}\;k = 1,2, \cdots, N$

(27)

将式(27)代入空间直线或圆弧的位姿表达式中，求解便可得到位置插补点。

3 仿真分析及实验

自主开发的六自由度工业机器人控制系统由嵌入式PC(x86架构)、Linux/RTAI实时操作系统和伺服驱动等模块组成。本文在嵌入式PC上利用C++编程实现规划算法。控制器与伺服驱动器之间采用EtherCAT工业实时以太网进行通信^[14]。

3.1 直线插补实验

选取系统允许的最大速度和最大加速度分别为3 600 mm/s和2 800 mm/s²，J=7 500 mm/s³，插补周期为2 ms。选取起始点坐标为(369.7，−127.1，402.8，0.011 81，0.728 92，0.684 36，−0.013 89)，目标进给速度为108 mm/s。进行了4组实验，实验参数如表1所示。实验得到的位移、速度、加速度曲线如图4和图5所示。实验得到的直线运动喷绘效果如图6所示。

根据4组实验参数可分别得到四段、五段、六段和七段速度规划曲线，验证轨迹规划算法的有效性。此外，由图5可知，S型速度规划可保证加速度连续，减小机械冲击。

3.2 圆弧插补实验

与直线插补运算类似，机器人系统运动参数与直线部分相同。选取圆弧起点坐标分别为P₀(369.7，−127.1，402.8，0.011 81，0.728 92，0.684 36，−0.013 89)，P₁(473.6，−23.45，502.8，0.011 81，0.728 92，0.684 36，−0.013 89)，P₂(369.7，80.2，402.8，0.011 81，0.728 92，0.684 36，−0.013 89)。分别进行加速度为600 mm/s²和2 800 mm/s²两组实验。实验得到的位移、速度、加速度曲线如图7和图8所示。实验得到的圆弧运动喷绘效果如图9所示。

通过对比在加速度为600 mm/s²和2 800 mm/s²两种情况，可分别得到七段式速度规划和五段式速度规划。同理，可通过调整圆弧弧长、速度和加速度等参数获得四段式和六段式速度规划。与直线部分类似，S型速度规划可保证加速度连续。

4 结论

1)采用S型速度规划，基于四元数的直线与圆弧运动的位姿模型，可实现笛卡尔空间规划。

2)根据路径长度约束条件，能将速度规划类型分为4种情况，可实现速度和加速度的自动调整。

3)本文设计的S型速度规划算法可保证加速度连续，减小机械冲击。

在下一步工作中，将增加误差等约束条件，提高控制精度；进一步优化机器人控制系统软硬件平台，提高系统实时性。