浙江大学学报(工学版), 2022, 56(12): 2321-2329 doi: 10.3785/j.issn.1008-973X.2022.12.001

机械工程

基于多种群竞争松鼠搜索算法的机械臂时间最优轨迹规划

赵业和,, 刘达新, 刘振宇,, 谭建荣

1. 浙江大学 计算机辅助设计与图形学国家重点实验室,浙江 杭州 310027

2. 设计工程及数字孪生浙江省工程研究中心,浙江 杭州 310027

Time-optimal trajectory planning of manipulator based on multi-group competition squirrel search algorithm

ZHAO Ye-he,, LIU Da-xin, LIU Zhen-yu,, TAN Jian-rong

1. State Key Laboratory of CAD & CG, Zhejiang University, Hangzhou 310027, China

2. Engineering Research Center for Design Engineering and Digital Twin of Zhejiang Province, Hangzhou 310027, China

通讯作者: 刘振宇,男,教授. orcid.org/ 0000-0003-2463-4553. E-mail: liuzy@zju.edu.cn

收稿日期: 2022-01-19  

基金资助: 国家重点研发计划资助项目(2019YFB1312600);浙江省重点研发计划资助项目(2021C01008)

Received: 2022-01-19  

Fund supported: 国家重点研发计划资助项目(2019YFB1312600);浙江省重点研发计划资助项目(2021C01008)

作者简介 About authors

赵业和(1998—),男,硕士生,从事机器人与智能制造研究.orcid.org/0000-0001-9793-9716.E-mail:zhaoyehe@zju.edu.cn , E-mail:zhaoyehe@zju.edu.cn

摘要

针对传统智能优化算法在机械臂关节空间进行时间最优轨迹规划应用中存在的寻优效率低、优化结果全局性和稳定性差的问题,提出新的机械臂时间最优轨迹规划方法. 在建立机械臂关节空间内的时间最优轨迹规划模型时考虑位置约束,根据输入的关节点列,使用S形曲线估算时间的取值区间,对生成算法的所有个体进行多种群竞争迭代,得出机械臂关节空间轨迹规划的时间最优解. 与不同算法的仿真对比试验结果表明,所提方法较传统的优化算法具有更高的寻优效率和更好的优化全局性;所提方法的稳定性好,其多次优化结果的方差相较单种群算法低3个数量级.

关键词: 关节空间 ; 轨迹规划 ; 松鼠搜索算法 ; 多项式插值 ; 机械臂

Abstract

Aiming at the problems of low optimization efficiency, poor global and stability of optimization results in the application of traditional intelligent optimization algorithms to time-optimal trajectory planning of manipulator in the joint space, a new time-optimal trajectory planning method for manipulator was proposed. The position constraints were considered when the time-optimal trajectory planning model in the joint space of the manipulator was established. According to the input joint point sequence, an S-shaped curve was used to estimate the time interval, and all the individuals in the algorithm were generated to perform multi-group competition iterations. After that, the time-optimal solution of trajectory planning of manipulator in the joint space was obtained. Results of simulation comparison experiments with different algorithms show that the proposed method has higher optimization efficiency and better global optimization than the traditional optimization algorithms. Also, the proposed method has good stability. The variance of its multiple optimization results can be 3 orders of magnitude lower than that of the single population algorithm.

Keywords: joint space ; trajectory planning ; squirrel search algorithm ; polynomial interpolation ; manipulator

PDF (1645KB) 元数据 多维度评价 相关文章 导出 EndNote| Ris| Bibtex  收藏本文

本文引用格式

赵业和, 刘达新, 刘振宇, 谭建荣. 基于多种群竞争松鼠搜索算法的机械臂时间最优轨迹规划. 浙江大学学报(工学版)[J], 2022, 56(12): 2321-2329 doi:10.3785/j.issn.1008-973X.2022.12.001

ZHAO Ye-he, LIU Da-xin, LIU Zhen-yu, TAN Jian-rong. Time-optimal trajectory planning of manipulator based on multi-group competition squirrel search algorithm. Journal of Zhejiang University(Engineering Science)[J], 2022, 56(12): 2321-2329 doi:10.3785/j.issn.1008-973X.2022.12.001

机械臂轨迹规划是机器人作业规划的重要内容. 机械臂的轨迹规划方法分为笛卡尔空间的轨迹规划和关节空间的轨迹规划,2种方法都要求规划的轨迹加速度连续[1]. 笛卡尔空间的轨迹规划存在须规避奇异点、起始点和终止点有不同解、大量的矩阵运算等问题,因此在对末端轨迹没有特殊要求的情况下,使用关节空间的规划效率更高. 关节空间的轨迹规划通常使用多项式进行插值. 在关节空间对多个路径点进行轨迹规划时,不仅相邻2个路径点间的插值时间难以确定,而且须考虑机械臂存在运动约束的影响. 为了在满足速度、加速度的约束前提下,充分发挥机械臂的性能,须优化机械臂运动的性能指标.

时间最优是常被采用的关节空间轨迹优化指标,但基于多项式插值的轨迹表达式具有阶次高、没有凸包性质的特点,因此该指标难以使用传统方法优化. 利用智能优化方法优化轨迹规划的插值时间,能够让多项式插值应用到更高要求的轨迹规划中[2]. Liu等[3-4]使用粒子群算法对带约束的关节空间进行时间最优轨迹规划. Gasparetto等[5]通过三次多项式对关节空间轨迹进行插值,并用序列二次规划法,通过权重因子平衡选择时间最优或者加加速度最小的轨迹规划. 朱世强等[6]采用7次B样条曲线插值关节位置序列,并利用序列二次规划方法求解非线性约束优化问题. 殷凤健等[7]通过设计交叉概率和变异概率自适应调整的遗传算法,对关节空间进行时间最优轨迹规划. 汪婷等[8]针对时间最优的轨迹规划问题提出遗传算法、差分进化算法和序列二次规划混合的算法,为解决该问题提供了新的思路. Zhao等[9]针对时间最优的轨迹规划问题,提出改进鲸鱼算法和粒子群算法的混合算法. Zhang 等[10]提出高效、稳定性优良的自适应布谷鸟搜索算法,该算法在严格的动态约束下使机械臂在关节空间中的总运动时间最小. Xidas等[11]提出基于标准多种群遗传算法,并将该算法用于超冗余机械臂的最优时间轨迹规划. Yu等[12]采用遗传算法同时优化机械臂的路径点和运动时间,得到机械臂的时间最优运动路径. Zhang等[13-14]基于机械臂动力学模型和动力学约束对机械臂进行时间最优轨迹规划. Palleschi等[15]研究人机共享环境中的协作机器人性能和安全权衡的问题:在保证协作机器人安全性的同时,使运动时间最短. 上述算法在对关节空间内的时间最优轨迹规划问题进行数学建模时都忽略了位置约束,而在使用多项式进行轨迹插值的过程中关节位置可能会超出约束. 此外,已有研究较少涉及优化变量搜索区间的确定方法.

本研究采用基于4-3-4型连续多段多项式对关节空间进行插值[16],根据关节空间的约束建立数学模型. 通过S形加减速[17]估算优化变量的搜索空间,从而提高优化的效率和收敛的速度. 为了进一步提高优化的效率,采用自然启发式的智能优化算法,即松鼠搜索算法(squirrel search algorithm,SSA). 该算法启发自松鼠觅食的自然过程,具有收敛速度快、寻优效果好的特点[18]. 本研究还将针对SSA算法存在的缺点,提出多种群竞争策略改进SSA,通过仿真试验验证算法的有效性.

1. 增加位置约束的关节空间时间最优轨迹规划数学建模

机械臂在进行关节空间的任务规划时,须通过对在笛卡尔空间下选取的一系列路径点进行逆运动学求解得到关节空间的点列. 机械臂运动到各路径点时的关节角度 ${{\boldsymbol{p}}_i} = \left[ {p_i^1, \cdots ,p_i^j, \cdots ,p_i^m} \right]$i=0,1,···,n;其中i为路径点序号,n为末位路径点序号, $p_i^j$为机械臂运动到第i个路径点时第j个关节的角度,m为机械臂的关节总数. 为了保证所有关节运动的一致性,须使每个关节在连续2个路径点间的运动时间间隔一致. 关节空间轨迹规划要求角加速度连续,即要求各关节位置关于时间t的插值函数 ${\theta ^j}(t)$j=1,···,m)至少 ${C^2}$连续. 要满足 ${\theta ^j}({t_i}) = p_i^j$,即要满足生成的插值函数须经过每个路径点对应的关节角度位置,其中 ${t_i}$为机械臂运动到第i个路径点的时刻. 为了保证插值函数 ${C^2}$连续,对关节空间轨迹的首尾段采用四次多项式、中间段采用三次多项式(4-3-4型连续多段多项式)进行轨迹插值.

以关节j的插值函数 $ {\theta ^j}(t) $为例,阐述如何在每段轨迹时间确定的前提下,求解插值函数的表达式. 因为有n+1个路径点,所以 ${\theta ^j}(t)$包含n段轨迹多项式,用 ${\theta _u}(t)$表示 ${\theta ^j}(t)$在时间区间 $ [{t_{u - 1}},{t_u}] $的表达式,其中u=1,2,···,n. 相邻段的多项式间连续,且其一阶导数和二阶导数也连续. 首尾段的四次多项式表达式分别为

$ {\theta _1}(t) = {a_1}_0+{a_1}_1t+{a_1}_2{t^2}+{a_1}_3{t^3}+{a_1}_4{t^4}, $

$ {\theta _n}(t) = {a_n}_0+{a_{n1}}t+{a_{n2}}{t^2}+{a_{n3}}{t^3}+{a_{n4}}{t^4}. $

中间段的三次多项式表达式为

$ {\theta }_{u}(t)={a}_{u0}+{a}_{u1}t+{a}_{u2}{t}^{2}+{a}_{u3}{t}^{3}\text{;}u=2,\cdots ,n-1. $

求解多项式的表达式即求解每段表达式的各个系数,共4n+2个系数. 关节j在每个路径点i的角度 $p_i^j$i=0,1,···,n)是已知的,起始路径点和终止路径点的角速度 $\dot p_0^j$$\dot p_n^j$,以及角加速度 $\ddot p_0^j$$\ddot p_n^j$也是已知的. 由于首尾路径点的位置、速度和加速度已知,可以得到6个等式. 对于序号为1至n−1的中间路径点,任意点i的位置已知,可以得到2个等式: $ {\theta }_{i}({t}_{i})={p}_{i}^{j}、{\theta }_{i+1}({t}_{i})={p}_{i}^{j} $,根据路径点i处的速度和加速度未知但连续,可以得到2个等式: ${\dot \theta _i}({t_i}) - {\dot \theta _{i+1}}({t_i}) = 0$${\ddot \theta _i}({t_i}) - {\ddot \theta _{i+1}}({t_i}) = 0$,因此n−1个中间点可以得到4(n−1)个等式. 综上,根据所有n+1个路径点信息,可以得到线性不相关的等式共4n+2个. 将求解系数的方程组写成矩阵形式 $ {\boldsymbol{C}} {\boldsymbol{A}} = {\boldsymbol{B}} $,其中C$(4n+2) \times (4n+2)$维矩阵,AB均为 $(4n+2) \times 1$维矩阵,由式(1)~(3)得到

$ {\boldsymbol{C}} = \left[ {\begin{array}{*{20}{c}} 1& {{t_0}}& {t_0^2}& {t_0^3}& {t_0^4}& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& {2{t_0}}& {3t_0^2}& {4t_0^3}& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 2& {6{t_0}}& {12t_0^2}& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 1& {{t_1}}& {t_1^2}& {t_1^3}& {t_1^4}& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& {{t_1}}& {t_1^2}& {t_1^3}& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& {2{t_1}}& {3t_1^2}& {4t_1^3}& 0& { - 1}& { - 2{t_1}}& { - 3t_1^2}& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 2& {6{t_1}}& {12t_1^2}& 0& 0& { - 2}& { - 6{t_1}}& \cdots & 0& 0& 0& 0& 0& 0& 0& 0& 0\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 1& {{t_{n - 1}}}& {t_{n - 1}^2}& {t_{n - 1}^3}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 1& {{t_{n - 1}}}& {t_{n - 1}^2}& {t_{n - 1}^3}& {t_{n - 1}^4}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 0& 1& {2{t_{n - 1}}}& {3t_{n - 1}^2}& 0& { - 1}& { - 2{t_{n - 1}}}& { - 3t_{n - 1}^2}& { - 4t_{n - 1}^3}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 0& 0& 2& {6{t_{n - 1}}}& 0& 0& { - 2}& { - 6{t_{n - 1}}}& { - 12t_{n - 1}^2}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 1& {{t_n}}& {t_n^2}& {t_n^3}& {t_n^4}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 0& 1& {2{t_n}}& {3t_n^2}& {4t_n^3}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \cdots & 0& 0& 0& 0& 0& 0& 2& {6{t_n}}& {12t_n^2} \end{array}} \right]. $

$ {\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} {{a_{10}}} \\ {{a_{11}}} \\ {{a_{12}}} \\ \vdots \\ {{a_{n2}}} \\ {{a_{n3}}} \\ {{a_{n4}}} \end{array}} \right],{\boldsymbol{B}} = \left[ {\begin{array}{*{20}{c}} {p_0^j} \\ {\dot p_0^j} \\ {\ddot p_0^j} \\ \vdots \\ {p_n^j} \\ {\dot p_n^j} \\ {\ddot p_n^j} \end{array}} \right]. $

解得系数矩阵 $ {\boldsymbol{A}} = {{\boldsymbol{C}}^{ - 1}} {\boldsymbol{B}} $,求出关节插值函数 ${\theta ^j}(t)$.

建立数学模型,把关节空间时间最优的轨迹规划问题转化为数学优化问题. 设第u段轨迹的时间间隔为 ${x_u}$,优化变量定义为 ${\boldsymbol{X}} = \left[ {{x_1}, \cdots ,{x_u}, \cdots ,{x_n}} \right]$Xn维向量. 基于时间最优的目标,将目标函数设定为各段轨迹时间之和T最小,由此建立数学优化模型为

$ \min T = \sum\limits_{u = 1}^n {{x_u}}; $

$ \begin{split} {\rm{s}}.{\rm{t}}. \; & {\max \left| {{\theta ^j}(t)} \right| \leqslant \theta _{\max }^j} , {\max \left| {{{\dot \theta }^j}(t)} \right| \leqslant v_{\max }^j}, \\ & {\max \left| {{{\ddot \theta }^j}(t)} \right| \leqslant a_{\max }^j}, {\max \left| {{{\dddot \theta }^j}(t)} \right| \leqslant J_{\max }^j} . \end{split}$

式中: $ {\theta }_{\mathrm{max}}^{j}、{v}_{\mathrm{max}}^{j}、{a}_{\mathrm{max}}^{j}、{J}_{\mathrm{max}}^{j} $分别为关节j的角度位置最大值约束、速度最大值约束、加速度最大值约束、加加速度最大值约束. 约束条件须求解每个关节位置、速度、加速度、加加速度绝对值的最大值,即求解插值函数的零阶、一阶、二阶、三阶导数的最大值,求解步骤如下. 1)对于单个关节,求解关节插值函数在每段函数表达式各阶导数的极值和端点值,得到这段多项式上各阶导数的绝对值最大值. 2)比较单个关节每段多项式各阶导数的绝对值最大值,得到单个关节全过程中各阶导数的绝对值最大值. 3)比较所有关节的插值多段多项式各阶导数的绝对值最大值,取其中的最大值,求得全局位置、速度、加速度、加加速度绝对值最大值. 上述求解最大值的过程,最多只需求解一元三次方程,求解效率高,求解精度高,保证了所求最大值的准确性.

2. 基于多种群竞争松鼠搜索算法的时间最优问题求解

优化的数学模型阶次高、结构复杂,传统的优化方法难以应用,适合采用智能优化方法求解. 针对传统智能优化算法寻优效率低、全局性差的缺点,采用能够求解复杂系统优化问题的SSA进行求解. 单种群SSA的算法流程如图1所示. 本研究先设计基于单种群SSA的方法,再基于单种群SSA拓展到多种群竞争松鼠搜索算法(multi-group competition squirrel search algorithm, MSSA).

图 1

图 1   松鼠搜索算法的流程图

Fig.1   Flow chart of squirrel search algorithm


2.1. 基于S形加减速估计优化变量搜索区间

SSA是随机搜索算法,当优化变量的维度很大时,如果搜索区间也很大或没有合适的搜索区间,可能会导致算法搜索结果失败. 因此,确定好优化变量的搜索区间不仅能确保算法的收敛性,也能加快搜索的效率. 记 $s_u^j$为关节j在第u段的位移量.

${x_u}$左边界的确定. 关节j在第u段的时间左边界 $l_u^j = {k_l}{{s_u^j}}/{{v_{\max }^j}}$,其中 ${k_l}$为估算系数. 根据实验测得当 ${k_l} = 1.25$时,算法的效率较高. 求出所有关节在第u段的左边界,得到 ${x_u}$的左边界 ${l_u} = \max (l_u^j)$.

${x_u}$右边界的估计. 关节j在第u段的时间右边界没有确定的计算方法. S形加减速曲线的速度在加减速段上和采用的4-3-4型插值函数在除首尾段上的速度都是关于时间的二次函数,速度曲线相似. 因此采用首末速度为0的7段S形加减速曲线估算关节j在第u段的时间. 取最大速度、最大加速度、最大加加速度分别为 $ {v}_{\mathrm{max}}={k}_{v}{v}_{\mathrm{max}}^{j}、 {a}_{\mathrm{max}}={k}_{a}{a}_{\mathrm{max}}^{j}、{J}_{\mathrm{max}}={k}_{J}{J}_{\mathrm{max}}^{j} $. 根据实验测得当 $ {k}_{v}=0.6、 {k}_{a}=0.2、{k}_{J}=0.2 $ 时,算法的效率较高.

为了方便表述,用s简化表示 $s_u^j$. 插值时间 ${t_s}$根据文献[17]可以推导得到以下5种情况. 当 ${v_{\max }}{J_{\max }} \leqslant a_{\max }^2$,7段S形加减速一定不存在匀加速段和匀减速段. 1)当 $s \leqslant 2{v_{\max }} ({{{{v_{\max }}}}/{{{J_{\max }}}})^{1/2}}$,不存在匀速段, $t_s=4 ({{0.5s}/{ J_{\max }}})^{1/3}$. 2)当 $s > 2{v_{\max }}({{{{v_{\max }}}}/{{{J_{\max }}}})^{1/2}}$,存在匀速段, ${t_s} = {s}/{{{v_{\max }}}}+2({{{{v_{\max }}}}/{{{J_{\max }}}})^{1/2}}$.${v_{\max }}{J_{\max }} > a_{\max }^2$,7段S形加减速可能存在匀加速段和匀减速段. 3)当 $s \leqslant {{2a_{\max }^3}}/{{J_{\max }^2}}$,不存在匀加速、匀减速和匀速段, $t_s=4 ({{0.5s}/{ J_{\max }}})^{1/3}$. 4)当 ${{2a_{\max }^3}}/ {{J_{\max }^2}} < s \leqslant {v_{\max }}\left({{{a_{\max }}}}/ {{{J_{\max }}}} + {{{v_{\max }}}}/{{{a_{\max }}}}\right)$, 存在匀加速和匀减速段,不存在匀速段, ${t_s} = ({{{a_{\max }}{J_{\max }}}} ) ^{-1} \cdot( {{a_{\max }^2 +}} ( a_{\max }^4 + 4{a_{\max }}J_{\max }^2s )^{1 /2})$. 5) 当 $s > {v_{\max }}({{{a_{\max }}}}/ {{{J_{\max }}}} + {{{v_{\max }}}}/ {{{a_{\max }}}} )$,存在匀加速、匀减速和匀速段, ${t_s} = {s}/{{{v_{\max }}}}+{{{v_{\max }}}}/{{{a_{\max }}}}$. 取关节j在第u段的时间右边界 $r_u^j = {k_r}{t_s}$. 其中 ${k_r}$为右边界估算系数,根据实验测得当 ${k_r} = 1.2$时,算法的效率较高. 求出所有关节在第u段的右边界,得到 ${x_u}$的右边界 ${r_u} = \max (r_u^j)$.

综上,优化变量X的左边界 ${\boldsymbol{L}} = [{l_1},{l_2}, \cdots {l_n}]$,右边界 ${\boldsymbol{R}} = \left[ {{r_1},{r_2}, \cdots ,{r_n}} \right]$.

2.2. 种群的初始化

单种群个体的数量N=50,即森林中有50棵树,其中1棵为山核桃树 ${{\boldsymbol{S}}_{\text{h}}}$、3棵为橡树 ${{\boldsymbol{S}}_{\text{a}}}$,剩下的为普通树 ${{\boldsymbol{S}}_{\text{n}}}$,按照50个松鼠的位置计算适应度,并按降序排序. 山核桃树将被分配给最优秀的松鼠,3棵橡树分配给后面连续的3个优秀的松鼠,剩下的依次分配在普通树上. 用 ${{\boldsymbol{S}}_k}(k = 1, \cdots N)$表示第k个松鼠的位置向量,其值对应优化变量X各元素,

$ {{\boldsymbol{S}}_k} = U({{\boldsymbol{S}}_{\text{L}}},{{\boldsymbol{S}}_{\text{U}}}). $

式中: $U({{\boldsymbol{S}}_{\text{L}}},{{\boldsymbol{S}}_{\text{U}}})$${{\boldsymbol{S}}_{\text{L}}}$${{\boldsymbol{S}}_{\text{U}}}$的均匀随机分布; $ {{\boldsymbol{S}}_{\text{L}}} $${{\boldsymbol{S}}_{\text{U}}}$分别是松鼠搜索空间的下界和上界,分别对应优化变量的左边界L和右边界R.

2.3. 适应度评价

适应度函数的设计须区分满足约束的个体和不满足约束的个体. 对于满足约束的个体,T越小,适应度越大. 对于不满足约束的个体,时间越短插值的多项式就越陡峭,导致多项式各阶导数的绝对值最大值变大. 因此,对于不满足约束的个体来说,T越大,适应度越大. 这样设计的适应度函数能够保证种群的多样性. 适应度函数 $f$设计为

$\left.\begin{split} & f=10 \mathrm{exp}\left( {10} \beta /{T}\right)\text{;}\\ & \beta=\left\{\begin{array}{ll} 1, & 个体满足约束;\\ -1, & 个体不满足约束.\end{array} \right. \end{split}\right\} $

T根据松鼠位置向量各元素并结合式(6)计算得到.

2.4. 生成新种群

生成新种群是群体智能算法的关键,SSA生成新种群的过程受松鼠动态觅食过程的启发. 生成新种群时,引入捕食者出现概率 ${P_{\text{d}}}$对应松鼠在觅食过程中可能出现天敌的情况,取 ${P_{\text{d}}} = 0.1$. 生成新种群的过程可能出现3种情况:1)在橡树上的松鼠可能会向山核桃树移动,获得松鼠新位置的方式为

$ {\boldsymbol{S}}_{\text{a}}^{\alpha +1} = \left\{ {\begin{array}{*{20}{l}} {{\boldsymbol{S}}_{\text{a}}^\alpha +{d_{\text{g}}} {G_{\text{c}}} ({\boldsymbol{S}}_{\text{h}}^\alpha - {\boldsymbol{S}}_{\text{a}}^\alpha )},&{{R_1} \geqslant {P_{\text{d}}}} ;\\ {U({{\boldsymbol{S}}_{\text{L}}},{{\boldsymbol{S}}_{\text{U}}})}, &{{R_1} < {P_{\text{d}}}} . \end{array}} \right. $

2) 在普通树上的松鼠可能会向橡树移动,获得松鼠新位置的方式为

$ {\boldsymbol{S}}_{\text{n}}^{\alpha +1} = \left\{ {\begin{array}{*{20}{l}} {{\boldsymbol{S}}_{\text{n}}^\alpha +{d_{\text{g}}} {G_{\text{c}}} ({\boldsymbol{S}}_{\text{a}}^\alpha - {\boldsymbol{S}}_{\text{n}}^\alpha )}, &{{R_2} \geqslant {P_{\text{d}}}} ;\\ {U({{\boldsymbol{S}}_{\text{L}}},{{\boldsymbol{S}}_{\text{U}}})}, &{{R_2} < {P_{\text{d}}}} . \end{array}} \right. $

3) 在普通树上的松鼠可能会向山核桃树移动,获得松鼠新位置的方式为

$ {\boldsymbol{S}}_{\text{n}}^{\alpha +1} = \left\{ {\begin{array}{*{20}{l}} {{\boldsymbol{S}}_{\text{n}}^\alpha +{d_{\text{g}}} {G_{\text{c}}} ({\boldsymbol{S}}_{\text{h}}^\alpha - {\boldsymbol{S}}_{\text{n}}^\alpha )}, &{{R_3} \geqslant {P_{\text{d}}}}; \\ {U({{\boldsymbol{S}}_{\text{L}}},{{\boldsymbol{S}}_{\text{U}}})}, &{{R_3} < {P_{\text{d}}}} . \end{array}} \right. $

式中: $\alpha $为当前迭代的代数, ${d_{\text{g}}}$为松鼠的随机滑翔距离, $ {R}_{1}、{R}_{2}、{R}_{3} $为[0, 1.0]的随机数. ${d_{\text{g}}}$由滑翔空气动力学公式求解得到,结合滑翔常数 ${G_{\text{c}}}$实现探索和开发的平衡, 通常取 ${G_{\text{c}}}$=1.9. 根据算法的来源,可以得到

$ {d_{\text{g}}} {G_{\text{c}}} = {M_{\text{v}}} {C_{\text{L}}}. $

式中: ${C_{\text{L}}}$为[0.675, 1.500]的随机变量,由随机变量模拟下滑长度的变化; ${M_{\text{v}}}$为松鼠移动常数,由松鼠的动力学公式、滑翔常数得到, ${M_{\text{v}}}$=1.407 4.

2.5. 季节变化产生的随机搬迁

为了增加算法的随机全局搜索能力,避免算法陷入局部最优解,模拟季节变化对松鼠觅食的影响,引入季节监测条件. 1) 计算季节变化条件. 季节常量 ${{{S}}_{\rm{c}}}^{ \alpha} = ({\sum\nolimits_{u = 1}^n {{{({{{{S}}}}_{{{{\rm{a}},{\rm{u}}}}}^\alpha} - {{{S}}}}_{{{{\rm{h}},{\rm{u}}}}}^\alpha}})^{1/2}$,其中 ${{{{S}}}}_{{{{\rm{a}},{\rm{u}}}}}^\alpha$${{{{S}}}}_{{{{\rm{h}},{\rm{u}}}}}^\alpha$分别为 ${{S}}_{\text{a}}^\alpha $${{S}}_{\text{h}}^\alpha $的第u个分量. 监测季节变化条件 ${S_{ {\rm{c}}}}^\alpha < {S_{\min }}$${S_{\min }}$为季节性常数计算的最小值,

$ {S_{\min }} = \frac{{10{^{ - 6}}}}{{{{(365)}^{2.5\alpha /{\alpha _{\text{m}}}}}}}. $

式中: ${\alpha _{\text{m}}}$为最大迭代值. ${S_{\min }}$较大有利于全局搜索,较小有利于局部搜索. 2) 冬季末随机搬迁. 基于列维飞行[18]的位置更新公式,重新定位满足季节监测条件的普通树上的松鼠位置为

$ {\boldsymbol{S}}_{\text{n}}^{{\text{new}}} = {{\boldsymbol{S}}_{\text{L}}}+{\text{Levy(}}{r_{\text{a}}},{r_{\text{b}}}{\text{)}} \times {\text{(}}{{\boldsymbol{S}}_{\text{U}}} - {{\boldsymbol{S}}_{\text{L}}}). $

式中: $ {\text{Levy(}}{r_{\text{a}}},{r_{\text{b}}}{\text{)}} $为列维飞行用于确定搜索路径的系数,是随机改变符合列维分布步长的方法. 列维飞行通过随机改变步长,偶尔寻找远离当前位置的新位置,能够有效提高元启发式算法的全局搜索能力. 根据文献[18]得到求解列维飞行的表达式为

$ {\text{Levy}}({r_{\text{a}}},{r_{\text{b}}}) = 0.007 \times \frac{{{r_{\text{a}}}}}{{{{\left| {{r_{\text{b}}}} \right|}^{\frac{2}{3}}}}}. $

式中: ${r_{\text{a}}},{r_{\text{b}}}$均为[0,1.0]上正态分布的随机数. 引入季节变化产生的随机搬迁,可以有效增加算法的全局搜索能力.

2.6. 加入多种群竞争策略改进松鼠搜索算法

松鼠搜索算法是随机搜索算法,单种群松鼠搜索算法存在2个问题: 1)搜索结果不一定是全局最优,2)每次优化的结果均有差异. 为此,提出多种群竞争策略来改进松鼠搜索算法.

图2所示,多种群竞争策略1)生成多个种群,每个种群独立迭代,到达一定迭代次数后,从每个种群中选择最优秀的一半个体;2)将选出来的所有个体随机分配给新的多种群,依次进行4轮竞争淘汰,得到最优个体. 图中, ${P_{v,w}}$为第v轮第w个种群. 每轮结束,种群数减少一半,首轮种群个数是8,末轮种群个数是1,最后迭代出最优个体. 每轮的迭代次数相同,每个种群的种群数量相同,取N=50.

图 2

图 2   多种群竞争策略示意图

Fig.2   Schematic diagram of multi group competition strategy


在使用C++开发算法的过程中,多种群的实现基于多线程库Thread实现. 算法仿真试验所用的计算机CPU为8核,最多可以实现8个真正意义上的多线程,因此多种群竞争策略首轮选择8个种群数. 每轮的迭代结束后,将竞争得到的优秀个体随机分配到各个新种群,此时须将上轮得到的个体打乱,本研究使用C++中的std::shuffle函数将所有个体打乱.

2.7. 应用多种群竞争松鼠搜索算法解决时间最优轨迹规划问题模型

机械臂关节空间轨迹规划的时间最优问题模型的输入是关节空间的点列,输出是所有关节的插值函数. 1)根据输入的关节点列,通过S形加减速估算出优化变量的搜索区间. 2)使用随机数引擎生成满足在搜索区间均匀分布的 $N \times M$个个体,其中M为多种群的数量,初始值为8. 3)将个体平均分配到M=8个单种群中,每个单种群每次独立迭代G次. 4)每轮迭代完成后,从每个单种群中取出最优秀的一半个体,打乱后重新平均分配给 $M/2$个单种群,重复4轮迭代. 5)第4轮只有1个单种群迭代,得到最优的个体,最优个体的位置向量即为优化变量最优解 ${{\boldsymbol{X}}^*}$. 6)根据 ${{\boldsymbol{X}}^*}$求出所有关节的插值函数的表达式,完成机械臂的时间最优轨迹规划. MSSA解决时间最优轨迹规划问题的流程图如图3所示.

图 3

图 3   多种群竞争松鼠搜索算法解决时间最优轨迹规划问题流程图

Fig.3   Flow chart of solving time-optimal trajectory planning problem based on multi-group competition squirrel search algorithm


3. 仿真与试验结果

3.1. 机械臂建模

MSSA和SSA具有通用性,能够应用于不同关节数目的串联机械臂和不同数量路径点的场景. 以7轴协作机械臂为研究对象,在机械臂工作空间选择路径点,通过逆运动学求解关节空间的角度位置. 机械臂的实物图如图4所示. 机械臂采用改进D-H法进行运动学建模,D-H参数如表1所示. 表中,i为关节编号, ${\alpha _{i - 1}}$为机械臂连杆扭转角, ${a_{i - 1}}$为连杆长度, ${d_i}$为连杆偏距, ${\theta _i}$为关节转角. 7轴机械臂的每个关节都有位置、速度、加速度和加加速度的约束,本研究选用的机械臂的所有关节约束是相同的,机械臂的运动学约束如下:关节角θ∈[−180°,180°]、关节角速度v∈[0, 100] (°)/s、关节角加速度a∈[0, 1 000] (°)/s2、关节角加加速度J∈[0, 1 000] (°)/s3.

图 4

图 4   机械臂实物图

Fig.4   Picture of robotic arm


表 1   机械臂的D-H参数

Tab.1  D-H parameters of robotic arm

i αi−1/(°) ai−1/mm di/mm θi/(°)
1 0 0 249 0
2 90 0 0 0
3 90 0 264 0
4 −90 0 0 0
5 90 0 243 0
6 −90 0 0 −90
7 −90 90 80 −90

新窗口打开| 下载CSV


3.2. 增加位置约束的必要性试验

机械臂关节空间的轨迹规划过程中的位置约束不可忽略,否则规划出来的结果可能会超过机械臂的位置限制. 对单个关节分别使用不增加位置约束的SSA和增加位置约束的SSA进行时间最优轨迹规划,单个关节的路径点0~5对应的关节角度位置分别为30°、−20°、20°、178°、10°、60°. 分别对轨迹进行优化后得到的位置变化情况对比如图5所示. 可以看出,机械臂关节在运动到点3附近时,无位置约束得到的规划结果超出了位置限制. 因此不增加位置约束在某些情况下的规划结果可能会超出机械臂本身的关节位置限制,增加位置约束后可以避免这种情况的发生.

图 5

图 5   有无位置约束的轨迹优化结果对比

Fig.5   Comparison of trajectory optimization results with and without position constraints


3.3. 算法试验结果

为了验证算法的效果,根据机械臂在抓取物体时实际工作空间中示教的路径点,取6个路径点的情况来规划关节空间的轨迹. 通过机械臂的逆运动学求解,得到各个路径点的关节位置序列如表2所示. 表中,I为机械臂关节角度.

表 2   机械臂的关节位置序列

Tab.2  Joint position sequence of robotic arm

点列 I/(°)
关节1 关节2 关节3 关节4 关节5 关节6 关节7
0 0 70 0 70 −20 −40 0
1 40 −5 −90 60 40 20 60
2 160 65 −10 20 5 −60 120
3 50 −10 −70 90 −60 20 −30
4 −25 50 −25 40 150 −70 60
5 −120 −40 −75 90 −5 −130 −50

新窗口打开| 下载CSV


3.3.1. 使用单种群松鼠搜索算法优化轨迹

根据给定的关节序列,确定优化变量的维度为5,确定各分量的搜索区间. 输入关节约束,取终止迭代次数 $G = 200$,用单种群SSA优化每段时间,得到机械臂的总运行时间. 使用Matlab进行仿真,种群最优个体适应度的变化曲线如图6所示. 适应度的变化结果表明,SSA的收敛快、搜索效率很高,在100次迭代后,曲线趋近收敛. 优化得到的总时间为11.158 s,每段轨迹的多项式插值时间序列为[1.523, 1.500, 1.890, 3.285, 2.960],单位均为s,下同.

图 6

图 6   松鼠搜索算法的适应度变化曲线

Fig.6   Fitness curve of squirrel search algorithm


为了对比SSA的算法效率,使用遗传算法(genetic algorithm, GA)优化关节空间的轨迹. 取GA的种群数量、最大迭代次数、适应度函数与SSA的相同. GA优化得到的总时间为11.677 s,时间序列为[1.528, 1.715, 2.012, 3.499, 2.923],GA得到的适应度变化曲线如图7所示. SSA优化得到的总时间比GA优化结果缩短4.44%. 图67的对比结果表明,SSA的迭代效率更高,种群适应度提升频率更高,得到的结果更优,比传统优化算法GA的效率更高. SSA、GA都存在优化结果不稳定的问题,增加种群数量是增加算法优化结果稳定性的有效途径.

图 7

图 7   遗传算法的适应度变化曲线

Fig.7   Fitness curve of genetic algorithm


3.3.2. 使用多种群竞争松鼠搜索算法优化轨迹

设定各个种群的迭代次数G=50,4次竞争淘汰的总迭代次数则为200,与单种群SSA的迭代次数相同. MSSA得到的总时间为11.046 s,每段多项式插值的时间为[1.523, 1.500, 1.886, 3.520, 2.617],MSSA的优化结果比GA的优化结果提升5.40%,比SSA的提升1.0%. 在优化过程中,MSSA每轮中各个种群的最优适应度变化情况如图8所示. 各关节的位置、速度、加速度、加加速度变化图像如图9所示.

图 8

图 8   多种群竞争松鼠搜索算法的适应度变化曲线

Fig.8   Fitness curve of multi-group competition squirrel search algorithm


图 9

图 9   机械臂的关节运动轨迹图像

Fig.9   Joint trajectory image of  robotic arm


3.4. 试验结果分析

试验表明SSA、MSSA在机械臂关节空间时间最优轨迹规划中应用的有效性,2种算法均具有收敛速度快、寻优效果好、优化结果全局性好的优点. 对不同数量的路径点分别使用GA、SSA和MSSA进行优化,得到优化的结果如表3所示. 表中, ${t_{{\text{GA}}}}$${t_{{\text{SSA}}}}$${t_{{\text{MSSA}}}}$分别为GA、SSA和MSSA算法在不同路径点数下计算得到的最终插值时间, ${I_{{\text{SSA}}}}$${I_{{\text{MSSA}}}}$分别为SSA和MSSA相对GA算法计算得到的插值时间的提升百分比. SSA比GA的优化效果平均提升8.55%,MSSA对比GA的优化效果平均提升11.76%. 由表看出,MSSA的优化效果更好,得到的结果全局性更好;此外,点数越大,MSSA比SSA、GA的优化结果提升越明显.

表 3   不同路径点数下各算法得到的插值时间对比

Tab.3  Comparison of interpolation time calculated by various algorithms under different number of path points

点数 $ {t_{{\text{GA}}}} $/s ${t_{{\text{SSA}}}}$/s ${t_{{\text{MSSA}}}}$/s ${I_{{\text{SSA}}}}$/% ${I_{{\text{MSSA}}}}$/%
7 13.432 6 12.569 5 12.522 8 6.425 4 6.773 1
8 15.252 4 13.771 7 13.646 7 9.708 0 10.527 5
9 16.566 1 15.763 8 15.049 4 4.843 0 9.155 4
10 18.014 9 16.847 9 16.171 5 6.478 0 10.232 6
11 19.629 1 17.713 9 17.505 1 9.756 9 10.820 7
12 21.862 3 19.590 9 18.928 4 10.389 6 13.419 9
13 23.129 3 20.932 9 19.766 9 9.496 2 14.537 4
14 24.193 2 22.209 0 21.256 7 8.201 5 12.137 7
15 26.475 5 23.676 0 22.580 1 10.573 9 14.713 2
16 30.319 4 27.810 1 25.683 9 8.276 2 15.288 9
17 33.922 3 30.674 3 29.208 6 9.574 8 13.895 6
18 34.506 4 31.016 3 30.525 7 10.114 4 11.536 1
19 36.061 2 33.230 9 32.802 3 7.848 6 9.037 1
20 39.790 4 36.577 9 34.787 0 8.073 6 12.574 4

新窗口打开| 下载CSV


为了更显著地表现多种群竞争策略的优点,对6个路径点在关节空间的轨迹规划分别使用GA、SSA和MSSA进行多次寻优,取寻优次数为1 000次. 3种算法在优化结果稳定性方面的差异如表4. 表中, $\overline T$为算法多次计算得到的插值时间的平均值, ${S^2}$为方差. 对比3种算法各自得到的机械臂运行总时间的平均值和方差,可以得出结论:SSA、MSSA具有比传统的智能优化算法更好的全局寻优性能,而MSSA除了具有比SSA更好的全局寻优性能外,还具有更加稳定的寻优效果.

表 4   不同算法优化结果的稳定性对比

Tab.4  Stability comparison of algorithm optimization results

算法 $\overline T$/s ${S^2}$/s2
GA 11.671 2 0.019 6
SSA 11.119 6 0.018 0
MSSA 11.049 4 8.5×10−5

新窗口打开| 下载CSV


4. 结 语

本研究在用4-3-4型连续多段多项式建立机械臂关节空间轨迹插值函数的基础上,通过多种群竞争策略松鼠搜索算法优化插值时间. 区别于常见的关节空间时间最优轨迹规划方法:1)在建立优化数学模型时,增加关节位置约束;2)在利用算法进行优化前,通过估算优化变量的搜索空间,提高算法的收敛速度和效率. 通过仿真试验,对比MSSA、SSA、GA得到的结果表明,MSSA算法具有更好的寻优效果和更加稳定的寻优特性,算法的优化结果提高了机械臂在关节空间的轨迹规划效率. 由于MSSA、SSA算法的迭代性质以及算法运行中涉及的大量矩阵运算,导致算法运行时间较长,不适于在线实时的关节空间轨迹规划,而适合离线阶段对指定路径点和关节轨迹的寻优. 后续计划进一步研究如何提升算法的运算性能,以满足更多应用场景的需要.

参考文献

约翰 J. 克雷格. 机器人学导论(原书第4版)[M]. 贠超, 王伟, 译. 北京: 机械工业出版社, 2018: 144-161.

[本文引用: 1]

居鹤华, 付荣

基于GA的时间最优机械臂轨迹规划算法

[J]. 控制工程, 2012, 19 (3): 472- 477

DOI:10.3969/j.issn.1671-7848.2012.03.026      [本文引用: 1]

JU He-hua, FU Rong

Time-optimal trajectory planning algorithm based on GA for manipulator

[J]. Control Engineering of China, 2012, 19 (3): 472- 477

DOI:10.3969/j.issn.1671-7848.2012.03.026      [本文引用: 1]

LIU C, CAO G H, QU Y Y, et al

An improved PSO algorithm for time-optimal trajectory planning of Delta robot in intelligent packaging

[J]. The International Journal of Advanced Manufacturing Technology, 2020, 107: 1091- 1099

DOI:10.1007/s00170-019-04421-7      [本文引用: 1]

HUANG P F, XU Y S. PSO-based time-optimal trajectory planning for space robot with dynamic constraints [C]// 2006 IEEE International Conference on Robotics and Biomimetics. Kunming: IEEE, 2006: 1402-1407.

[本文引用: 1]

GASPARETTO A, ZANOTTO V

Optimal trajectory planning for industrial robots

[J]. Advances in Engineering Software, 2010, 41 (4): 548- 556

DOI:10.1016/j.advengsoft.2009.11.001      [本文引用: 1]

朱世强, 刘松国, 王宣银, 等

机械手时间最优脉动连续轨迹规划算法

[J]. 机械工程学报, 2010, 46 (3): 47- 52

DOI:10.3901/JME.2010.03.047      [本文引用: 1]

ZHU Shi-qiang, LIU Song-guo, WANG Xuan-yin, et al

Time-optimal and jerk-continuous trajectory planning algorithm for manipulators

[J]. Journal of Mechanical Engineering, 2010, 46 (3): 47- 52

DOI:10.3901/JME.2010.03.047      [本文引用: 1]

殷凤健, 梁庆华, 程旭, 等

基于时间最优的机械臂关节空间轨迹规划算法

[J]. 机械设计与研究, 2017, 33 (5): 12- 15

DOI:10.13952/j.cnki.jofmdr.2017.0183      [本文引用: 1]

YIN Feng-jian, LIANG Qing-hua, CHENG Xu, et al

Research on mechanical arm joint space trajectory planning algorithm based on optimal time

[J]. Machine Design and Research, 2017, 33 (5): 12- 15

DOI:10.13952/j.cnki.jofmdr.2017.0183      [本文引用: 1]

汪婷, 罗欣

一种六轴机械臂时间最优轨迹规划方法

[J]. 工业控制计算机, 2021, 34 (4): 66- 69

DOI:10.3969/j.issn.1001-182X.2021.04.027      [本文引用: 1]

WANG Ting, LUO Xin

A time-optimal trajectory planning method for manipulators

[J]. Industrial Control Computer, 2021, 34 (4): 66- 69

DOI:10.3969/j.issn.1001-182X.2021.04.027      [本文引用: 1]

ZHAO J, ZHU X J, SONG T J

Serial manipulator time-jerk optimal trajectory planning based on hybrid IWOA-PSO algorithm

[J]. IEEE Access, 2022, 10: 6592- 6604

DOI:10.1109/ACCESS.2022.3141448      [本文引用: 1]

ZHANG L H, WANG Y, ZHAO X Y, et al

Time-optimal trajectory planning of serial manipulator based on adaptive cuckoo search algorithm

[J]. Journal of Mechanical Science and Technology, 2021, 35: 3171- 3181

DOI:10.1007/s12206-021-0638-5      [本文引用: 1]

XIDIAS E K

Time-optimal trajectory planning for hyper-redundant manipulators in 3D workspaces

[J]. Robotics and Computer-Integrated Manufacturing, 2018, 50: 286- 298

DOI:10.1016/j.rcim.2017.10.005      [本文引用: 1]

YU X L, DONG M S, YIN W M

Time-optimal trajectory planning of manipulator with simultaneously searching the optimal path

[J]. Computer Communications, 2022, 181: 446- 453

DOI:10.1016/j.comcom.2021.10.005      [本文引用: 1]

ZHANG T, ZHANG M H, ZOU Y B

Time-optimal and smooth trajectory planning for robot manipulators

[J]. International Journal of Control, Automation and Systems, 2021, 19: 521- 531

DOI:10.1007/s12555-019-0703-3      [本文引用: 1]

MA J W, GAO S, YAN H T, et al

A new approach to time-optimal trajectory planning with torque and jerk limits for robot

[J]. Robotics and Autonomous Systems, 2021, 140: 103744

DOI:10.1016/j.robot.2021.103744      [本文引用: 1]

PALLESCHI A, HAMAD M, ABDOLSHAH S, et al

Fast and safe trajectory planning: solving the Cobot performance/safety trade-off in human-robot shared environments

[J]. IEEE Robotics and Automation Letters, 2021, 6 (3): 5445- 5452

DOI:10.1109/LRA.2021.3076968      [本文引用: 1]

GAO M Y, DING P, YANG Y X. Time-optimal trajectory planning of industrial robots based on particle swarm optimization [C]// 2015 Fifth International Conference on Instrumentation and Measurement, Computer, Communication and Control (IMCCC). Qinhuangdao: IEEE, 2015: 1934-1939.

[本文引用: 1]

石川, 赵彤, 叶佩青, 等

数控系统S曲线加减速规划研究

[J]. 中国机械工程, 2007, (12): 1421- 1425

DOI:10.3321/j.issn:1004-132X.2007.12.009      [本文引用: 2]

SHI Chuan, ZHAO Tong, YE Pei-qing, et al

Study on S-shape curve acceleration and deceleration control on NC system

[J]. China Mechanical Engineering, 2007, (12): 1421- 1425

DOI:10.3321/j.issn:1004-132X.2007.12.009      [本文引用: 2]

JAIN M, SINGH V, RANI A

A novel nature-inspired algorithm for optimization: squirrel search algorithm

[J]. Swarm and Evolutionary Computation, 2019, 44: 148- 175

DOI:10.1016/j.swevo.2018.02.013      [本文引用: 3]

/