机器人的轨迹规划是运动学正、逆解的实际应用, 是对机器人实施控制的基础, 对机械手的运行效率、平稳性、作业精度和能量消耗具有重要的意义^[1].轨迹规划是根据作业任务要求, 讨论关节的位移、速度和加速度随时间变化的规律^[2].轨迹规划算法的输入是工作空间离散点序列, 输出是一条通过这些离散点的空间轨迹.根据离散点序列给定方式的不同, 分为关节空间的轨迹规划和笛卡尔空间的轨迹规划^[3].

在关节空间进行规划, 需要给定机器人工作序列点的位形, 通过运动学逆解求得相应的关节变量序列.对每一个关节变量的相邻点进行插值, 可得相应关节的运动轨迹方程.由于机器人的被控量为关节变量, 该方法便于实时控制.

在笛卡尔空间进行规划, 工作路径点由笛卡尔坐标给出, 概念直观, 易于理解, 但涉及笛卡尔坐标到关节变量的转换, 计算量大, 且易陷于奇异点^[4].根据末端执行器运动轨迹的形状, 一般分为空间直线插补法和圆弧插补法^[5].

轨迹规划一直是机器人领域研究的热点, 国内外学者作了大量的工作, 常见的规划方法包括：多项式插值法、抛物线过渡的线性插值法、样条曲线法、摆线法、B样条曲线法.如凌家良等^[6-9]采用五次多项式插值法, 获得理想平滑的运动轨迹；Zhang等^[10-12]利用摆线运动在端点处零速度和零加速度的特点, 应用于黄瓜采摘机器人的轨迹规划, 该方法计算简单, 实时性较好；董辉等^[13]在五次非均匀样条曲线插值的基础上, 采用改性的粒子群算法得到关于时间最优的运动轨迹；司艳伟等^[14]对比研究七次样条曲线和三次B样条曲线, 发现三次B样条不仅能够提高运算效率, 而且能够避免出现龙格现象；Guan等^[15]结合三次样条插值计算量小和四次样条插值满足加速度连续, 提出采用4-3-4复合插值方法, 并进行轨迹优化.

本文的收获机械手主要有2个运动阶段, 包括从复位点到采摘点的运动和采摘完成到回收舱的运动, 无复杂的作业序列点, 因此适用于点到点(PTP)的运动规划.在求解机械手正、逆运动学的基础上, 采用现有的插值计算方法, 分别从位移、速度和加速度层面讨论这些方法的优劣, 寻求适用于该对象的轨迹规划方法.

1 番茄采摘机械手机构

研究对象是具有2个平移关节和5个转动关节7自由度的冗余机械手, 其中关节1、2是平移关节, 关节3~7为转动关节, 机构简图如图 1所示^[16-18].具体的三维结构图如图 2所示.

图 1中, l₁~l₇分别为机械手各连杆长度, d₁、d₂、θ₃ ~θ₇分别为关节1~7的关节变量.

2 番茄采摘机械手运动学 2.1 正运动学

为了研究机械手各连杆之间的位置和姿态关系, 采用D-H前置法^[19-20]对番茄收获机械手建立坐标系, 求得的D-H参数如表 1所示.表中, i为关节序号, α_i-1为连杆扭角, a_i-1为连杆长度, d_i为连杆偏距, θ_i为关节角.

根据表 1的D-H参数, 可以得到相邻连杆i-1和连杆i之间的变换矩阵^i-1T_i, 进而得到末端连杆相对于基坐标系的变换矩阵⁰T₇.

$ \begin{array}{l} ^0{\mathit{\boldsymbol{T}}_7}{ = ^0}{\mathit{\boldsymbol{T}}_1}^1{\mathit{\boldsymbol{T}}_2}^2{\mathit{\boldsymbol{T}}_3}^3{\mathit{\boldsymbol{T}}_4}^4{\mathit{\boldsymbol{T}}_5}^5{\mathit{\boldsymbol{T}}_6}^6{\mathit{\boldsymbol{T}}_7} = \\ \left[ {\begin{array}{*{20}{c}} \mathit{\boldsymbol{R}}&\mathit{\boldsymbol{P}}\\ 0&1 \end{array}} \right] = \left[ {\begin{array}{*{20}{l}} {{n_x}}&{{o_x}}&{{a_x}}&{{p_x}}\\ {{n_y}}&{{o_y}}&{{a_y}}&{{p_y}}\\ {{n_z}}&{{o_z}}&{{a_z}}&{{p_z}}\\ 0&0&0&1 \end{array}} \right]. \end{array} $

(1)

式中：R为机械手末端执行器相对于基座的旋转矩阵, P为机械手末端执行器相对于极坐标系的位置向量.位置向量各分量如下：

$ \left. \begin{array}{l} {p_x} = {C_3}({l_3} + {l_4}{C_4} + {l_5}{C_{45}} + ({l_6} + h){C_{456}}), \\ {p_y} = {d_2} + {l_2} + {l_4}{S_4} + {l_5}{S_{45}} + ({l_6} + h){S_{456}}, \\ {p_z} = {S_3}({l_3} + {l_4}{C_4} + {l_5}{C_{45}} + ({l_6} + h){C_{456}}) + {d_1}. \end{array} \right\} $

(2)

式中：S_i=sin θ_i, S_i…(i+n)=sin (θ_i+…+θ_i+n), C_i=cos θ_i, C_i…(i+n)=cos (θ_i+…+θ_i+n).

逆运动学通过代数和几何相结合的方法^[21]求解.给定末端执行器相对于基座的位姿矩阵, 能够得到对应的关节向量.

2.2 雅克比矩阵

雅克比矩阵描述机械手的操作空间与关节空间速度间的线性映射关系, 也可以看成是关节空间的微分运动向操作空间的微分运动之间的转换矩阵^[22].求解方法主要有矢量积法和微分变换法^[23], 本文采用矢量积法.

关节i的雅克比矩阵列向量如下：

$ {\mathit{\boldsymbol{J}}_i} = \left\{ {\begin{array}{*{20}{c}} {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{z}}_i}}\\ 0 \end{array}} \right], }&{平移关节, i = 1, 2, \cdots , 7;}\\ {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{z}}_i}{ \times ^i}\mathit{\boldsymbol{p}}_n^0}\\ {{\mathit{\boldsymbol{z}}_i}} \end{array}} \right], }&{转动关节, i = 1, 2, \cdots , 7.} \end{array}} \right. $

(3)

式中：z_i为坐标系{i}的z轴单位向量在基坐标系{0}中的表示；ⁱp_n⁰为末端执行器坐标原点相对于坐标系{i}的位置向量在基坐标系{0}中的表示, 即ⁱp_n⁰=_i⁰Rⁱp_n.

3 机械手关节空间轨迹规划方法

关节空间的轨迹规划方法需要将机械手末端所经过各路径点的位姿, 通过机械手逆运动学变换, 求出各关节变量值；继而通过插值计算, 将关节变量值表示成时间的函数, 使机械手末端依次通过起始点、中间各路径, 最后到达终止点.轨迹规划流程图如图 3所示.

采用多项式插值法、抛物线过渡的线性插值、摆线法和B样条曲线法进行PTP轨迹规划.这些方法需要考虑各关节的速度、加速度等约束条件, 运动学参数要在允许的变化范围内, 还要保证各关节位移、速度、加速度等变化的连续性, 使机械手的各关节运行平稳, 减小振动和冲击, 延长机械手使用寿命.方法优劣的评价指标^[24]如下.

1) 轨迹函数连续, 且一阶、二阶导数在端点处的值为零, 保证机械手启停的平稳性.

2) 最大速度和最大加速度应尽可能小, 保证系统紧急停车时的机构安全.

3) 规划算法的实时性.

3.1 多项式插值法轨迹规划

在已知某关节初始位移和期望位移的情况下, 运用三次多项式插值法求取关节的运动轨迹θ_i(t)(i=1, …, 7), 规划时间为t_f.由于三次多项式有4个未知数, 除路径点(初始点和终止点)约束外, 还需要添加2个约束条件.一般为了满足关节运动速度的连续性要求, 必须保证在路径点的关节速度为0.即已知：

$ \left. {\begin{array}{*{20}{c}} {{\theta _i}\left( 0 \right) = {\theta _{i0}}, }&{{{\dot \theta }_i}\left( 0 \right) = 0;}\\ {{\theta _i}({t_{\rm{f}}}) = {\theta _{i1}}, }&{{{\dot \theta }_i}({t_{\rm{f}}}) = 0.} \end{array}} \right\} $

(4)

可以求得关节θ_i的运动轨迹：

$ {\theta _i}\left( t \right) = {a_0} + {a_1}t + {a_2}{t^2} + {a_3}{t^3}. $

(5)

为了同时满足速度和加速度边界条件, 需要更高阶的多项式插值.考虑五次多项式插值法, 关节运动轨迹有6个未知系数, 因此需要6个约束条件, 添加路径点加速度为0的约束, 即

$ \left. \begin{array}{l} {{\ddot \theta }_i}\left( 0 \right) = 0, \\ {{\ddot \theta }_i}({t_{\rm{f}}}) = 0. \end{array} \right\} $

(6)

可以求得关节θ_i的运动轨迹：

$ {\theta _i}\left( t \right) = {a_0} + {a_1}t + {a_2}{t^2} + {a_3}{t^3} + {a_4}{t^4} + {a_5}{t^5}. $

(7)

3.2 抛物线过渡法轨迹规划

单纯的直线插值轨迹规划在中间路径点处的速度不连续, 加速度无限大, 严重危害电机, 因此考虑在线性插值端点邻域内设置一段抛物线过渡, 如图 4所示.两段抛物线段的持续时间相同, 加速度大小相同, 符号相反, 这种方法有效保证了整段轨迹的位移和速度的连续性.

带有抛物线过渡的线性插值的轨迹函数设成分段函数θ_{i, j}(t), 表示关节i的第j段轨迹函数, 其中i={1, 2, …, 7}, j=1, 2, 3.通过路径点位移与速度约束和2个过渡点的位移与速度连续性的要求, 求得关节轨迹函数.路径点位置、速度约束为

$ \left. {\begin{array}{*{20}{c}} {{\theta _{i1}}\left( 0 \right) = {\theta _{i0}}, }&{{{\dot \theta }_{i0}}\left( 0 \right) = {{\dot \theta }_{i0}};}\\ {{\theta _{i3}}({t_{\rm{f}}}) = {\theta _{i1}}, }&{{{\dot \theta }_{i3}}({t_{\rm{f}}}) = {{\dot \theta }_{i1}}.} \end{array}} \right\} $

(8)

过渡点位置、速度约束为

$ \left. {\begin{array}{*{20}{c}} {{\theta _{i1}}({t_1}) = {\theta _{i2}}({t_1}), }&{{{\dot \theta }_{i1}}({t_1}) = {{\dot \theta }_{i2}}({t_1});}\\ {{\theta _{i2}}({t_2}) = {\theta _{i3}}({t_2}), }&{{{\dot \theta }_{i2}}({t_2}) = {{\dot \theta }_{i3}}({t_2}).} \end{array}} \right\} $

(9)

关节轨迹函数为

$ {\theta _i}\left( t \right) = \left\{ {\begin{array}{*{20}{c}} {{c_1} + {c_2}t + {c_3}{t^2}, }&{0 \le t \le {t_1};}\\ {{c_4} + {c_5}t, }&{{t_1} \le t \le {t_2};}\\ {{c_6} + {c_7}t + {c_8}{t^2}, }&{{t_2} \le t \le {t_{\rm{f}}}.} \end{array}} \right. $

(10)

3.3 摆线法轨迹规划

摆线运动曲线平滑, 速度和加速度满足连续性要求, 且在定义域端点处的值为零, 因此适合点到点的关节空间轨迹规划.摆线运动正则形式下的运动方程为

$ s\left( \tau \right) = \tau - \frac{1}{{2{\rm{ \mathsf{ π} }}}}\sin \left( {2{\rm{ \mathsf{ π} }}\tau } \right);\tau \in \left[ {0, 1} \right]. $

(11)

式中：τ为归一化时间, τ=t/t_f.

摆线运动和它的前2阶导数在正则区间[-1, 1]的曲线如图 5所示.

运用摆线曲线函数进行关节轨迹规划, 将所有关节的规划时间设为一致, 从而生成许多组关节变量, 这些随时间变化的关节变量序列组构成了机械手关节空间的运动轨迹.某关节的位移、速度和加速度轨迹函数如下：

$ \left. \begin{array}{l} {\theta _i}\left( t \right) = {\theta _{i0}} + \left( {{\theta _{i1}} - {\theta _{i0}}} \right)s\left( \tau \right), \\ {{\dot \theta }_i}\left( t \right) = \frac{{{\theta _{i1}} - {\theta _{i0}}}}{{{t_{\rm{f}}}}}s'\left( \tau \right), \\ {{\ddot \theta }_i}\left( t \right) = \frac{{{\theta _{i1}} - {\theta _{i0}}}}{{{t_{\rm{f}}}}}s''\left( \tau \right). \end{array} \right\} $

(12)

3.4 B样条曲线法

B样条曲线是一种广泛应用于工程拟合的曲线, 具有局部支撑性、导数连续性等特性, 常用于路径点较多的轨迹规划中.若机械手工作过程中有m个工作序列点, 则关节i对应有m个关节角θ_i0, θ_i1, …θ_{i, m-1}.由连续性条件和边界条件可以求出m+n-1个控制点, 每相邻关节角之间能够得到1条n阶B样条曲线, 整条轨迹由m-1段曲线组成.采用三次B样条曲线进行PTP(m=2)的轨迹规划, 根据文献[25], 得到关节位移函数：

$ {\theta _i}\left( t \right) = \frac{1}{6}\left[ {{t^3}, {t^2}, t, 1} \right]\left[ {\begin{array}{*{20}{c}} { - 1}&3&{ - 3}&1\\ 3&{ - 6}&3&0\\ { - 3}&0&3&0\\ 1&4&1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{b_0}}\\ {{b_1}}\\ {{b_2}}\\ {{b_3}} \end{array}} \right]. $

(13)

由路径点条件和速度、加速度边界条件, 求得控制点为

$ \mathit{\boldsymbol{B}} = \left[ {\begin{array}{*{20}{c}} {{b_0}}\\ {{b_1}}\\ {{b_2}}\\ {{b_3}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} { - 1}&2\\ 2&{ - 1}\\ { - 1}&2\\ 2&{ - 1} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\theta _{i0}}}\\ {{\theta _{i1}}} \end{array}} \right]. $

(14)

4 仿真实验与分析

为了验证上述不同的规划算法, 在运动学与雅克比矩阵研究的基础上, 对机械手从复位点到采摘点的关节运动进行规划.根据以下特性指标, 选取最适合本机械手的规划方法.具体的实现过程如下.

1) 假设已知采摘点的位姿, 通过运动学逆解求得各关节的初始角和终止角.

2) 将各关节的初始角和终止角代入相应规划算法的方程, 得到位移、速度和加速度的函数.

3) 指定运动时间, 通过Matlab仿真, 得到各关节的运动曲线.

4) 将各关节的位移、速度序列点代入正运动学方程, 得到末端执行器的运动轨迹和各速度分量.

已知复位点和采摘点位姿为

$ \begin{array}{l} {\mathit{\boldsymbol{T}}_0} = \left[ {\begin{array}{*{20}{c}} 0&0&1&{1.055}\\ 1&0&0&{0.4}\\ 0&1&0&0\\ 0&0&0&1 \end{array}} \right], \\ {\mathit{\boldsymbol{T}}_1} = \left[ {\begin{array}{*{20}{c}} 1&0&0&0\\ 0&1&0&{1.15}\\ 0&0&1&{0.085}\\ 0&0&0&1 \end{array}} \right]. \end{array} $

根据逆运动学求取位置层面逆解, 得到机械手各关节的初始角和终止角为

$ \begin{array}{l} \;\;\;\;\;\;\;\;\;{\mathit{\boldsymbol{q}}_0} = {\left[ {0, \;\;0, \;\;0, \;\;0, \;\;0, \;\;0} \right]^{\rm{T}}}, \\ {\mathit{\boldsymbol{q}}_1} = {\left[ {0.3, \;\;0.2, \;\; - \frac{{\rm{ \mathsf{ π} }}}{2}, \;\;\frac{{\rm{ \mathsf{ π} }}}{2}, \;\;0, \;\; - \frac{{\rm{ \mathsf{ π} }}}{2}, \;\; - \frac{{\rm{ \mathsf{ π} }}}{2}} \right]^{\rm{T}}}. \end{array} $

设定从起始点到终止点的PTP规划的运行时间为1 s, 根据各轨迹规划函数的相应约束条件, 代入初始角和终止角, 求得各关节轨迹函数的系数, 最终得到相应的轨迹函数.考虑到关节1~7的曲线类似, 4种规划算法对应的关节1轨迹曲线如图 6所示.图中，v为速度，a为加速度.

由图 6(a)可知, 4种方法均满足路径点位置边界条件, 且曲线平滑满足连续性要求.由图 6(b)可知, 4种方法均满足路径点速度边界条件和连续性要求, 最大速度由大到小为摆线法、五次多项式法、三次B样条曲线法和抛物线过渡法.由图 6(c)可知, 五次多项式法和摆线法满足路径点加速度边界条件, 且曲线平滑满足连续性要求；三次B样条曲线法加速度呈线性变化；抛物线过渡法加速度在过渡点处产生突变, 将会引起较大的惯性冲击.

三次B样条曲线法只满足位置与速度边界条件；抛物线过渡法虽然运算简单, 但容易造成系统不稳定；五次多项式法和摆线法既满足路径点边界条件, 又满足连续性要求.

通过4种方法规划的关节位移函数, 能够得到关节向量时间序列.将对应时间下的关节向量代入正运动学方程, 能够得到该时间点的末端执行器笛卡尔坐标, 从而得到直观的末端运动轨迹.五次多项式法对应的末端轨迹如图 7所示.

由图 7可知, 五次多项式插值法对应的末端执行器的运动轨迹平滑, 无跳变点.通过雅克比矩阵求得对应于关节运动的末端速度, 各方向的速度分量如图 8所示.图中，v_x、v_y、v_z分别为x、y、z轴正方形线速度，ω_x、ω_y、ω_z分别为x、y、z轴正方向角速度.

由图 8可知, 五次多项式插值法和摆线法对应的末端最大速度较小.五次多项式和摆线运动法对应的各关节最大速度如表 2所示.可知, 五次多项式法与摆线法相比, 速度变化平均降低6.26%.

综上所述, 五次多项式法在满足路径点边界条件的同时, 关节运动速度的变化幅度小, 能耗较低且安全系数更高.本文选用基于五次多项式插值的关节轨迹规划方法.

5 结论

(1) 采用现存的关节轨迹规划方法, 对七自由度番茄收获机械手从复位点到采摘点的运动进行规划.结果表明：五次多项式法的计算量适中, 满足路径点的位移、速度和加速度边界条件, 规划的曲线连续平滑, 动态性能较好, 为今后实施控制奠定了基础.

(2) 结合关节空间规划方法的易于可控性和笛卡尔空间规划的直观性, 在用关节空间规划出轨迹函数后, 利用位置和速度层面的正运动学方程, 得到直观的末端执行器的轨迹与各方向的速度分量.

(3) 该机械手工作在非结构化的环境中, 末端执行器在工作过程中不可避免地会存在障碍物.今后的研究重点是障碍条件下的轨迹规划与轨迹优化.