工业机器人轨迹规划算法是保证工业机器人实现稳定运动的核心技术.对于弧焊等机器人来说，通常要求末端执行器尽量以给定速度沿着指定示教路径前进，并保证在相邻路径之间能够自动平滑过渡.为解决路径平滑过渡这一难题，最早采用的是基于样条曲线插补等方式的关节空间轨迹规划算法^[1-3]，此类算法具有约束条件少和计算速度快等优势，但同时存在空间轨迹不直观和轨迹形状会随速度不同而改变等缺点，因此逐渐被机器人厂商弃用.近年来，国内外学者大多转向了操作空间轨迹规划算法的研究.常见的工作空间轨迹规划算法主要有3类：第1类是卷积类算法，该方法通常是给定混叠时间参数，对路径进行均匀离散化后采用卷积计算来实现加减速规划，但此类算法存在着很多与关节空间轨迹规划算法类似的缺点^[4].第2类则是混叠类算法，包括速度混叠算法^[5]和位置混叠算法^[6-7]，此类算法一般需要给定加减速时间，然后通过线性、多项式或者摆线等函数形式的同伦变形获得速度曲线或者位置曲线，缺点是加速度约束涉及时间变量，需要复杂的处理流程，其中文献^[7]是对ABB公司TrueMove技术的一次不完全尝试.最后一类算法是规划类算法，包括速度规划算法^[8]和位置规划算法^[9-10]，这一类算法通过设计过渡曲线的数学表达式来实现.其中速度规划算法计算流程复杂，且难以保证轨迹不随速度参数的更改而变化.而位置规划算法的研究中，Siciliano等将关节空间的位置规划算法扩展到笛卡尔空间，获得了抛物线形状的过渡轨迹，使得加速度约束不再取决于时间常数，但是仍存在加速度矢量方向突变的问题^[9].林仕高等提出五次多项式位置规划过渡算法，但是因加速度约束是涉及时间变量的非线性函数而未能找到加速度约束处理方案^[10].此外，还有一些研究通过先选取过渡路径，然后将其细分为微小路径段，最后采用前瞻算法实现速度平滑和轨迹修形，然而此类算法计算复杂，难以保证在高速运动条件下的实时性^{[4, 11]}.

为此，提出一种计算效率高的基于有限项正弦级数的新型位置规划算法来解决上述问题.

1 节点配置

操作空间中典型的连续轨迹包括直线段与直线段、直线段与圆弧段以及圆弧段与圆弧段间的衔接.如图 1所示，为简化论述，取平面直线段Path1与直线段Path2间的衔接为代表进行本文算法的论述，而空间连续轨迹可以很轻松地从平面算法中导出.

其中：S₀为Path1的起始点，S₁为Path1和Path2的衔接点，S₂为Path2的结束点；r₁为S₁处的过渡半径；P₁和P₂分别为以S₁为圆心、r₁为半径的圆与Path1和Path2相交而得到的过渡节点；v₁和v₂则分别为沿Path1和Path2的巡行速度；k₁为过渡节点处待优化的速度比例系数，且一般要求0 ＜k₁≤1.

首先设t为完成过渡所需要的时间，点P₁和P₂的位置矢量分别用p₀和p_T表示.

为保证在平移坐标系中轨迹不变，将位置矢量p₀和p_T以相对位置矢量$\tilde{p}$₀和$\tilde{p}$_T来表示.其中，

$\tilde{p}$₀=p₀-p₀=0，$\tilde{p}$_T=p_T-p₀.

采用各轴分别进行平滑过渡的算法，故将相对位置、速度和加速度矢量分别向坐标轴投影以获得分量.该算法假设所有过渡节点的切向加速度均为0，从而使后续推导大为简化.过渡节点P₁处的相对位置、速度和加速度的坐标轴分量配置如下：

p_0j=$\tilde{p}$₀·e_j，$\tilde{v}$_0j=k_10j，a_0j=k₁²$\tilde{a}$_0j，

其中，${\tilde{v}}$_0j=v₁·e₀^τ·e_j，${\tilde{a}}$_0j=a₁·e₀ⁿ·e_j，a₁=v₁²R₁^-1，e₀^τ和e₀ⁿ分别为P₁处的切向单位向量和法向单位向量，e_j为坐标轴j的单位向量，R₁为P₁处的曲率半径(若为直线，R₁→∞，a₁=0)，j=x，y.

同理，可得过渡节点P₂处的相对位置、速度和加速度的坐标轴分量配置为

p_Tj=${\tilde{p}}$_T·e_j，${\tilde{p}}$_Tj=k_1Tj，a_Tj=k₁²${\tilde{a}}$_Tj.

由上述可知，在确定k₁之后，过渡节点的实际配置将随之确定.对于非过渡区域的轨迹只需采用经典的起止点的速度和加速度为给定值的直线段或圆弧段的轨迹规划算法^{[9-10, 12-17]}即可.由此可见本文过渡算法的关键在于确定k₁.以下将具体阐述算法的原理和计算步骤.

2 算法原理

对于每一个坐标轴方向，引入基于有限项正弦级数的位置规划函数：

${{p}_{j}}\left( t \right)=\sum\limits_{i={{n}_{s}}}^{{{n}_{e}}}{{{A}_{ij}}}sin(\frac{\pi }{{{2}^{i-1}}}\frac{t}{T}+{{\varphi }_{ij}}),0\le t\le T.$

(1)

则过渡节点的运动学约束方程组如下：

$\begin{matrix} {{p}_{0j}}={{p}_{j}}\left( 0 \right),{{p}_{Tj}}={{p}_{j}}\left( T \right), \\ {{v}_{0j}}{{=}_{j}}\left( 0 \right),{{v}_{Tj}}{{=}_{j}}\left( T \right), \\ {{a}_{0j}}{{=}_{j}}\left( 0 \right),{{a}_{Tj}}{{=}_{j}}\left( T \right). \\ \end{matrix}$

(2)

为保证上述方程组解的确定性，方程数量与待定系数数量需相同，选取n_s=3，n_e=5.

考察待优化的速度比例系数k₁，易知其与过渡总时间T成反相关，定义为

$T={{k}^{-1}}_{1}s,$

(3)

式中s=4r₁(v₁+v₂)^-1.将式(3)代入式(2)的约束方程组，借助正弦函数的特点可消去k₁，得到

$Z=CV,$

(4)

式中：

Z=p_0j，[p_Tj，${\tilde{v}}$_0j，${\tilde{v}}$_Tj，${\tilde{v}}$_0j，${\tilde{v}}$_Tj]^T，

C=[α₃，α₄，α₅，β₃，β₄，β₅，]

α_i=[o_i，0，q_i，0，0，-p_i，1，-o_i，2，-q_i，2]^T，

β_i=[0，p_i，0，o_i，1，q_i，1，0，-p_i，2]^T，

o_i，n=2^-n(i-1)uⁿ，u=πs^-1，

p_i，n=o_i，nsin 2^-(i-1)π，q_i，n=o_i，ncos 2^-(i-1)π，

V=[v_3j，v_4j，v_5j，w_3j，w_4j，w_5j]^T，

v_ij=A_ijsin φ_ij，w_ij=A_ijcos φ_ij.

故由V=C^-1Z可解得向量V.而由向量V可得

$\begin{matrix} {{\varphi }_{ij}}=arctan~({{v}_{ij}}/{{w}_{ij}}), \\ {{A}_{ij}}={{v}_{ij}}/sin~{{\varphi }_{ij}}, \\ i=3,4,5. \\ \end{matrix}$

(5)

下面进一步考虑加速度性能约束.

将式(1)进行二次求导可得加速度表达式为

${{a}_{j}}\left( t \right)=-\sum\limits_{i=3}^{5}{{{(\frac{{{k}_{1}}u}{{{2}^{i-1}}})}^{2}}}{{A}_{ij}}sin~(\frac{{{k}_{1}}u}{{{2}^{i-1}}}t+{{\varphi }_{ij}}).$

取ø=k₁ut/16，有ø∈[0，π/16]，可得

$|{{a}_{j}}\left( \phi \right){{|}_{max}}={{(\frac{1}{16}{{k}_{1}}u)}^{2}}\centerdot |{{\rho }_{j}}\left( \phi \right){{|}_{max}},$

(6)

其中：

$\begin{align} & {{\rho }_{j}}\left( \phi \right)=\sum\limits_{i=3}^{5}{({{E}_{ij}}s}in~{{\phi }_{i}}+{{F}_{ij}}cos~{{\phi }_{i}}){{\phi }_{i}}={{2}^{-(i-5)}}\phi ,{{E}_{ij}}={{2}^{-2(i-5)}}{{A}_{ij}}cos~{{\varphi }_{ij}}, \\ & {{F}_{ij}}={{2}^{-2(i-5)}}{{A}_{ij}}sin~{{\varphi }_{ij}}. \\ \end{align}$

可以看出，在给定|a_j(ø)|_max的允许值a_jmax后，对于每一个坐标轴j，分别存在一个k₁满足式(6)，将其记为k_1j，则对应有

${{k}_{1j}}=\frac{16\sqrt{{{a}_{jmax}}}}{u\sqrt{|{{\rho }_{j}}\left( \phi \right){{|}_{max}}}}.$

(7)

为获得k_1j，并避免繁杂的数值判断，可采用近似计算方法计算|ρ_j(ø)|_max.将ρ_j(ø)表达式中的正弦和余弦函数用下面的幂级数前3项近似：

$sin~{{\phi }_{i}}={{\phi }_{i}}-\frac{{{\phi }^{3}}_{i}}{3!}+\frac{{{\phi }^{5}}_{i}}{5!},cos~{{\phi }_{i}}=1-\frac{{{\phi }^{2}}_{i}}{2!}+\frac{{{\phi }^{4}}_{i}}{4!},$

可得近似表达式为

${{\rho }_{j}}\left( \phi \right)=\sum\limits_{l=0}^{5}{{{b}_{jl}}{{\phi }^{l}}},\phi \in \left[ 0,\pi /16 \right].$

(8)

上式为五次多项式，其系数b_jl为常量，求导可得四次多项式.可以通过解析法得到式(8)的极值，考虑边界情况，进而可获得ρ_j(ø)的近似最大值和近似最小值，分别记为ρ_jmax和ρ_jmin.为确认上述近似方法的有效性，需评估幂级数截断所产生的近似误差，定义为

$\begin{matrix} {{e}_{sin~}}({{\phi }_{i}})=|1-{{(sin~{{\phi }_{i}})}^{-1}}({{\phi }_{i}}-\frac{{{\phi }^{3}}_{i}}{3!}+\frac{{{\phi }^{5}}_{i}}{5!})|, \\ {{e}_{cos~}}({{\phi }_{i}})=|1-{{(cos~{{\phi }_{i}})}^{-1}}(1-\frac{{{\phi }^{2}}_{i}}{2!}+\frac{{{\phi }^{4}}_{i}}{4!})|. \\ \end{matrix}$

(9)

可得近似误差如图 2所示.

由图 2可知，在ø_i∈0，π/4时，近似方法所得的|ρ_j(ø)|_max误差不超过0.05%，而在此基础上可采用全局优化方法计算获得精确解.此处为简化表述，取

$|{{\rho }_{j}}\left( \phi \right){{|}_{max}}\approx \frac{max\left\{ |{{\rho }_{jmax}}\left| , \right|{{\rho }_{jmin}}| \right\}}{1-0.05%},$

(10)

由此解得

${{k}_{1j}}\approx \frac{64{{r}_{1}}\sqrt{{{a}_{jmax}}}}{\pi ({{v}_{1}}+{{v}_{2}})\sqrt{\frac{max\{|{{\rho }_{jmax}}\left| , \right|{{\rho }_{jmin}}|\}}{1-0.05%}}},$

(11)

则k₁值由下式取得：

${{k}_{1}}=min\left\{ {{k}_{1j}} \right\}.$

(12)

3 算法步骤

基于上述理论可以建立过渡区域的过渡算法，步骤如下：

1) 根据式(4)和式(5)求解φ_ij和A_ij.

2) 根据式(8)求解ρ_jmax和ρ_jmin.

3) 根据式(11)和式(12)分别求解k_1j和k₁.

4) 计算基本过渡曲线为

${{p}_{j}}\left( t \right)=\sum\limits_{i=3}^{5}{{{A}_{ij}}}sin~(\frac{{{k}_{1}}u}{{{2}^{i-1}}}t+{{\varphi }_{ij}}),0\le t\le T.$

(13)

由式(11)可知，若对v₁和v₂进行同比变化，则k₁与速度成反比，使得过渡节点配置不变，进而可知幅值系数与相位系数均不变，因此过渡轨迹的路径和速度剖面形状等均不改变.

5) 如果(v₁+v₂)过小或者r₁过大，可能造成k₁＞1.若k₁＞1，则可对式(13)进行时间缩放处理：

${{p}_{j}}\left( t \right)=\sum\limits_{i=3}^{5}{{{A}_{ij}}sin}~(\frac{u}{{{2}^{i-1}}}t+{{\varphi }_{ij}}),0\le t\le {{k}_{1}}T.$

(14)

即：将过渡时间扩大k₁倍，使速度缩小k₁倍,而过渡轨迹的路径形状不发生改变.

6) 将所得相对位置矢量变换成绝对位置矢量即可得到最终过渡曲线.

但需注意，如果k₁＞＞1，则上述方法会使得过渡速度最小值过小，不利于保持运动过程中速度的相对稳定.因此在实际机器人控制器中仍需提供类似于文献^{[4, 8, 11]}的分段细化和速度前瞻算法以处理(v₁+v₂)过小的情况.而对于r₁过大的情况，通常意味着示教程序质量不高，需要进行一些局部调整，例如插入圆弧或增加示教点等.

4 仿真与实验

采用辰星(天津)自动化设备有限公司的D2-350两自由度高速并联机械手^[18-19]为工业机器人模型，并以门式轨迹(b_w=305 mm，h_w=25 mm)^[19]为典型轨迹在MATLAB软件上进行算法的仿真测试.

D2-350两自由度高速并联机械手如图 3所示^[19]，其操作空间为水平和铅锤方向所形成的平面，具体参数见表 1^[19].

给定门式轨迹如图 4所示，其基本参数见表 2，参考D2-350机械手设定过渡参数,见表 3.

由于该算法主要用于过渡节点的选取以及过渡曲线的生成，为更直观地分析该算法的特性，以下主要对P₁点处过渡区域进行详细分析.

经过计算可得k₁=0.36，则P₁点处过渡区域的相对位置、速度、加速度和过渡轨迹的曲线分别如图 5、图 6、图 7和图 8所示.

从P₁过渡区域加速度曲线可以看出，用该算法求取的过渡曲线的加速度完全满足所施加的物理约束，说明该算法可以达到预期效果.

为验证仿真结果，进行实验测试.本测试采用摆线加减速方式实现非过渡区域轨迹规划，同时，由于高速运动需要很高的采样频率，故采用编码器反馈的关节转角进行运动学正解计算得到实际过渡轨迹.实际过渡轨迹、速度和加速度曲线分别如图 9、图 10和图 11所示，采样时间为1 ms.

由图 9可知实际过渡轨迹与计算过渡轨迹基本吻合，而由图 11可知其加速度满足机械手物理约束条件，故该算法得到验证.

同时，用户可以通过调整过渡半径r₁，得到不同的过渡轨迹，图 12所示过渡轨迹簇中的过渡轨迹分别为r₁=2，3，4，5 mm时所生成的过渡轨迹.

如图 13所示：过渡轨迹1为v₁=v₂=200 mm/s时的过渡轨迹，而过渡轨迹2为v₁=v₂=100 mm/s时的过渡轨迹，两者完全重合，证明在速度等比例变化时，所得过渡轨迹不变.

5 结 论

本文给出了一种工业机器人操作空间连续轨迹规划的过渡算法，该算法的主要目的是在保证连续轨迹过渡精度和工业机器人物理约束的条件下充分发挥工业机器人的能力，尽量提高过渡区域的速度，减少过渡所需时间，同时尽可能地使计算过程简便，以便用于在线规划，提高算法的通用性.通过MATLAB仿真和两自由度高速并联机械手的实验验证，证实了该算法能够在保证连续轨迹平滑过渡的同时，保证其物理约束条件.此外，该算法具备在速度等比例变化的条件下过渡轨迹不变的特性，这种特性使其能够更广泛地应用于机器人应用领域.此外，该算法是位置规划方法，容易计入位置约束条件，因此其可扩展性良好.后续研究将对姿态配置进行类似处理，进而实现包含空间姿态的轨迹过渡，推动位置规划类连续轨迹规划方法的创新和发展.