CN113296407A - 一种基于5次非均匀有理b样条的多机协同轨迹优化方法 - Google Patents

Abstract

本发明公开了一种基于5次非均匀有理B样条的多机协同轨迹优化方法，首先在确定机器人各自的运动轨迹后，对各机器人的加工路径进行离散化处理，将离散点通过机器人逆解得到关节空间中的一系列关节位置序列，利用五次非均匀有理B样条曲线数学模型对关节序列进行插值，构造端点运动参数可指定的高阶连续的关节轨迹。采用带精英策略的非支配排序遗传算法(NSGA2)以运行时间、轨迹平滑性为目标，以时间间隔为决策变量对机械臂运动轨迹进行优化，获得高阶连续的优化轨迹。本发明克服多项式插值会随着次数增高而出现的Runge现象，解决了单目标优化技术不能满足先进制造背景下实际工程对机器人综合性能的要求。

Description

一种基于5次非均匀有理B样条的多机协同轨迹优化方法

技术领域

本发明涉及智能制造领域中的航天制造方向，尤其涉及一种基于5次非均匀有理B样条的多机协同轨迹优化方法。

背景技术

随着高新技术的发展和需求的推动，航天器微小化已成为发展趋势。但在微纳卫星研制生产方面，我国与美国等国家相比一直处于跟跑阶段。目前，卫星创新研究院微纳卫星研究所在产品齐套的基础上完成1颗微纳卫星的组装、集成测试和试验时间是60天左右。这远远无法满足微纳卫星批量化生产的需求。而利用工业机器人搭建的增材制造系统，具有高效性，灵活性，为突破卫星规模化、批量化生产瓶颈，并大幅提高研制生产能力提供了全新的解决方法。

工业机器人的刚度只有机床的1/50，在加工过程中极易受到外力影响而发生变形，而为了满足更快速、高精度的运动，制造商往往会降低机器人系统的惯性和质量，从而导致机器人结构刚度的再次降低、振动现象的增加。在机器人的轨迹规划中，冲击(加速度的导数)是至关重要的，降低冲击可以提高跟踪精度，达到更高的任务执行速度，减少机器人的关节抖动。在工业应用中，机器人末端在笛卡尔空间中运动，为了满足笛卡尔空间的动态约束(扭矩)和运动约束(速度和加速度等)需要对机器人进行复杂的数学建模以及繁重的计算以实时为机器人控制器提供末端执行器的位置和方向。因此，必须将任务空间轨迹转换到关节空间，在关节空间在进行轨迹规划，以得到满足关节驱动器和传动器物理性能的关节轨迹，因此机器人的轨迹一般都是在关节空间中进行的。针对工业机器人本体弱刚性，加工作业易发生关节抖动，无法满足高打印精度的问题，提出了一种基于5次非均匀有理B样条的多机协同轨迹优化方法，旨在提高机器人运动稳定性，降低机器人运行过程中的冲击，延长整体结构的平滑性和使用寿命，保证整体增材制造微纳卫星的精度需求。

发明内容

本发明提供一种基于5次非均匀有理B样条的多机协同轨迹优化方法，旨在解决现有技术存在的机器人关节运动不平稳，关节振动导致末端运动无法满足高精度协同运动的问题，提出了在实际的应用中轨迹规划不仅需要考虑制作工艺，而且需要优化时间、能量、平稳性等。

为实现上述目的，本发明采用以下技术方案：一种基于5次非均匀有理B样条的多机协同轨迹优化方法，包括以下步骤：

S1、根据机器人轨迹跟踪精度的要求，将任务空间几何形状进行轨迹离散化处理，得到位姿矩阵序列T_i和相应的时间节点序列t_i，通过机器人逆运动学计算求出T_i对应的关节位置序列p_i，进而构成关节位置-时间序列S＝(p_i,T_i)，其中p_i＝(p_1ip_2i...p_Ni)^T,p_Ni表示关节N在路径点i处的关节角度值；

S2、采用5次非均匀B样条对机器人关节位置序列进行插值，构造端点运动参数可指定的高阶连续的关节轨迹，根据机器人各关节的运动轨迹求出任意时刻的关节位置；

S3、根据实际作业需求，以机器人运行时间最短、轨迹最平滑为目标对关节轨迹进行优化，完成多机器人协同轨迹优化。

为优化上述技术方案，采取的具体措施还包括：

进一步地，步骤S2中的对机器人关节位置序列插值，构造插值数学模型的方法为：

对插值关节曲线统一描述为

式中，d_j为控制顶点非均匀B样条次数，N_j,k(u)为k次规范B样条基函数，且

式中：k表示非均匀B样条次数，j表示序号；

由于轨迹曲线需要严格满足位置-时间序列S中的每个时间位置-时间约束，因此需要根据该约束反求B样条插值轨迹曲线的控制顶点；以累计弦长参数化的方法对时间节点t_i归一化，得到k次B样条轨迹曲线的定义域节点矢量u＝(u₀u₁...u_n+2k)，有

可列出与位置-时间序列S中的位置-时间约束对应的n+1个方程：

对于k次非均匀B样条开曲线还需增加k-1个通常由边界条件给定的附加方程，附加条件通过切矢边界条件得到，通过对曲线开始和结束的速度加速度和冲击进行约束；B样条轨迹曲线的r阶导数为

所以

V(t₀),V(t_n),A(t₀),A(t_n),J(t₀),J(t_n)分别代表初始和结束的速度、加速度、和加加速度，p′(u)、p″(u)、p″′(u)分别为关节速度、加速度和加加速度轨迹曲线；

以矩阵方程形式描述轨迹曲线控制顶点反求方程

p＝C_Nd (18)

式中C_N∈R^{(n×5)×(n×5)}为系数矩阵，且

根据控制点和归一化时间节点矢量即可求出关节在时刻t_i经过位置p_i的关节轨迹曲线。

进一步地，步骤S3中以机器人作业效率最高、轨迹最平滑为目标将构建的轨迹进行优化，构造优化约束条件的具体方法如下：

对关节运动进行约束转化，

式中

B样条曲线具有凸包性质，因此，使关节m的轨迹曲线满足运动学约束，仅需B样条轨迹曲线的控制顶点满足

式中

和

分别为第m个关节的B样条速度、加速度和脉动曲线的第j个控制顶点。

进一步地，步骤S3中构建关节轨迹优化模型的具体流程如下：

多目标优化模型为：

式中：各目标函数S_i(x)(i＝1,2,...,f)及约束函数g_i(x)≤0和h_k(x)＝0都是决策变量x＝(x₁,x₂,...,x_n)函数。

进一步地，定义如下优化目标：

式中：S₁为运动时间，是路径点之间的时间间隔之和，衡量机器人的运动效率；S₂为关节平均加加速度，衡量轨迹的平滑度。T为机器人完成指定任务轨迹从初始位姿运动到终止位姿的时间；j_i为关节加加速度。

进一步地，利用NSGA2算法以各路径点之间的时间间隔为决策变量进行机械臂的多目标轨迹优化求解，步骤如下：

1)根据给定的机械臂运动路径点之间的时间间隔的范围随机产生种群规模为N的初始父代种群H₀，并通过筛选、杂交、变异等进化方式生成大小同为N的子代种群I₀；

2)将父代种群和子代种群合并和排序，生成种群规模为2N的种群R_t，对R_t进行非支配排序，得到等级F₁、F₂、F₃…；再依据每一非支配层中个体的拥挤度筛选个体构成新的父代种群；

3)根据非支配排序、拥挤度距离和约束违背值计算的排序结果选取N个个体作为新的父代种群H_i+1；

4)通过遗传算子产生新的子代种群I_i+1；

5)重复2)至4)步，直到达到算法设置的最大迭代次数；

将优化选取的时间变量重新代入关节位置序列，利用五次非均匀B样条插值，构造优化后端点运动参数可指定的高阶连续的关节轨迹，完成针对此条加工路径的关节轨迹优化。

本发明的有益效果是：本发明利用五次非均匀B样条插值机器人关节轨迹，得到有可修改性的启停速度、加速度均可指定的高阶连续运动轨迹。克服多项式插值会随着次数增高而出现的Runge现象。本发明还利用NSGA2算法以时间-平滑性为优化目标，以关节速度、加速度、加加速度为约束进行优化，获得具有良好收敛性和多样性的Pareto最优解。解决了单目标优化技术不能满足先进制造背景下实际工程对机器人综合性能的要求。

附图说明

图1为本发明的机器人轨迹规划流程图；

图2为本发明的加工轨迹离散图；

图3为本发明的NSGA2新群体构成示意图；

图4为本发明的基于NSGA2的优化流程图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

本发明实施例提供一种基于5次非均匀有理B样条的多机协同轨迹优化方法，如图1所示，本发明的机器人轨迹规划流程图完成了该发明的基本实施过程，将各个运动路径点利用机器人运动学逆解转变成对应的关节路径点，其次利用五次非均匀有理B样条曲线数学模型对关节关键点进行插值，构造端点运动参数可指定的高阶连续的关节轨迹。采用带精英策略的非支配排序遗传算法(NSGA2)以运行时间、轨迹平滑性为目标对机械臂运动轨迹进行优化，获得高阶连续的优化轨迹。步骤如下：

S1、根据机器人轨迹跟踪精度的要求，将任务空间几何形状进行轨迹离散化处理，得到位姿矩阵序列T_i和相应的时间节点序列t_i，通过机器人逆运动学计算求出T_i对应的关节位置序列p_i，进而构成关节位置-时间序列S＝(p_i,T_i)，其中p_i＝(p_1ip_2i...p_Ni)^T,p_Ni表示关节N在路径点i处的关节角度值；

S2、采用5次非均匀B样条对机器人关节位置序列进行插值，构造端点运动参数可指定的高阶连续的关节轨迹，根据机器人各关节的运动轨迹可求出任意时刻的关节位置；

S3、根据实际作业需求，以机器人运行时间最短、轨迹最平滑为目标对关节轨迹进行优化，完成多机器人协同轨迹优化。

步骤S2中的对机器人关节位置序列插值，构造插值数学模型的方法为：

对插值关节曲线统一描述为

式中，d_j为控制顶点非均匀B样条次数，N_j,k(u)为k次规范B样条基函数，且

式中：k表示非均匀B样条次数，j表示序号。由于轨迹曲线需要严格满足位置-时间序列S中的每个时间位置-时间约束，因此需要根据该约束反求B样条插值轨迹曲线的控制顶点。以累计弦长参数化的方法对时间节点t_i归一化，得到k次B样条轨迹曲线的定义域节点矢量u＝(u₀u₁...u_n+2k)，有

可列出与位置-时间序列S中的位置-时间约束对应的n+1个方程：

对于k次非均匀B样条开曲线还需增加k-1个通常由边界条件给定的附加方程，附加条件通过切矢边界条件得到，通过对曲线开始和结束的速度加速度和冲击进行约束。B样条轨迹曲线的r阶导数为

所以

V(t₀),V(t_n),A(t₀),A(t_n),J(t₀),J(t_n)分别代表初始和结束的速度、加速度、和加加速度，p′(u)、p″(u)、p″′(u)分别为关节速度、加速度和加加速度轨迹曲线。

以矩阵方程形式描述轨迹曲线控制顶点反求方程

p＝C_Nd (29)

式中C_N∈R^{(n×5)×(n×5)}为系数矩阵，且

根据控制点和归一化时间节点矢量即可求出关节在时刻t_i经过位置p_i的关节轨迹曲线。

上述步骤S3中以机器人作业效率最高、轨迹最平滑为目标优化关节轨迹，构造优化的约束条件具体方法如下：

对关节运动进行约束转化，

式中

B样条曲线具有凸包性质，因此，使关节m的轨迹曲线满足运动学约束，仅需B样条轨迹曲线的控制顶点满足

式中

和

分别为第m个关节的B样条速度、加速度和脉动曲线的第j个控制顶点，通过将关节运动学约束转化为B样条轨迹曲线的控制顶点约束，避免对轨迹曲线进行采样检验的半无穷约束问题。

上述步骤S3中以机器人作业效率最高、轨迹最平滑为目标优化关节轨迹，构建关节轨迹优化模型的具体流程如下：

多目标优化模型为：

式中：各目标函数S_i(x)(i＝1,2,...,f)及约束函数g_i(x)≤0和h_k(x)＝0都是决策变量x＝(x₁,x₂,...,x_n)函数。为了兼顾机器人运行时间和轨迹平滑性定义如下优化目标：

式中：S₁为运动时间，是路径点之间的时间间隔之和，衡量机器人的运动效率；S₂为关节平均加加速度，衡量轨迹的平滑度。T为机器人完成指定任务轨迹从初始位姿运动到终止位姿的时间；j_i为关节加加速度。这两个目标相互矛盾，运行时间的减少会导致更大的冲击。而减小冲击会导致更长的执行时间和更低的执行效率。在机器人进行轨迹规划的过程中不可避免的会遇到这两者之间的权衡，对于多目标优化问题，优化结果通常是一个解集，且这些解在就针对优化目标函数而言是无法比较优劣的，其特点是无法在改进任何目标函数的同时不削弱至少一个其他目标函数，这些解称作Pareto最优解或者非支配解。NSGA2算法即基于Pareto最优概念的非支配排序遗传算法，是求解多目标优化问题的一种有效算法并得到了广泛有效的应用。利用该算法以各路径点之间的时间间隔为决策变量进行机械臂的多目标轨迹优化求解。

步骤如下：

1)根据给定的机械臂运动路径点之间的时间间隔的范围随机产生种群规模为N的初始父代种群H₀，并通过筛选、杂交、变异等进化方式生成大小同为N的子代种群I₀；

2)将父代种群和子代种群合并和排序，生成种群规模为2N的种群R_t，对R_t进行非支配排序，得到等级F₁、F₂、F₃…；再依据每一非支配层中个体的拥挤度筛选个体构成新的父代种群；

3)根据非支配排序、拥挤度距离和约束违背值计算的排序结果选取N个个体作为新的父代种群H_i+1；

4)通过遗传算子产生新的子代种群I_i+1；

5)重复2)至4)步，直到达到算法设置的最大迭代次数；

将优化选取的时间变量重新代入关节位置序列，利用五次非均匀B样条插值，构造优化后端点运动参数可指定的高阶连续的关节轨迹，完成针对此条加工路径的关节轨迹优化。

综上可知，本发明在关节空间中进行机器人的轨迹规划，但为了使其在运动过程中平稳、无振动，使用五次B样条对关节位置序列进行插值，保证各关节加加速度连续，获得高阶光滑连续且启停速度、加速度均可指定的运动轨迹，NSGA2算法可很好的实现时间-平滑性的多目标优化，优化的轨迹满足运动学约束。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (6)

1.一种基于5次非均匀有理B样条的多机协同轨迹优化方法，其特征在于，包括以下步骤：

S1、根据机器人轨迹跟踪精度的要求，将任务空间几何形状进行轨迹离散化处理，得到位姿矩阵序列T_i和相应的时间节点序列t_i，通过机器人逆运动学计算求出T_i对应的关节位置序列p_i，进而构成关节位置-时间序列S＝(p_i,T_i)，其中p_i＝(p_1ip_2i...p_Ni)^T,p_Ni表示关节N在路径点i处的关节角度值；

S2、采用5次非均匀B样条对机器人关节位置序列进行插值，构造端点运动参数可指定的高阶连续的关节轨迹，根据机器人各关节的运动轨迹求出任意时刻的关节位置；

S3、根据实际作业需求，以机器人运行时间最短、轨迹最平滑为目标对关节轨迹进行优化，完成多机器人协同轨迹优化。

2.根据权利要求1所述的多机协同轨迹优化方法，其特征在于，步骤S2中的对机器人关节位置序列插值，构造插值数学模型的方法为：

对插值关节曲线统一描述为

式中，d_j为控制顶点非均匀B样条次数，N_j,k(u)为k次规范B样条基函数，且

式中：k表示非均匀B样条次数，j表示序号；

由于轨迹曲线需要严格满足位置-时间序列S中的每个时间位置-时间约束，因此需要根据该约束反求B样条插值轨迹曲线的控制顶点；以累计弦长参数化的方法对时间节点t_i归一化，得到k次B样条轨迹曲线的定义域节点矢量u＝(u₀u₁...u_n+2k)，有

可列出与位置-时间序列S中的位置-时间约束对应的n+1个方程：

对于k次非均匀B样条开曲线还需增加k-1个通常由边界条件给定的附加方程，附加条件通过切矢边界条件得到，通过对曲线开始和结束的速度加速度和冲击进行约束；B样条轨迹曲线的r阶导数为

所以

V(t₀),V(t_n),A(t₀),A(t_n),J(t₀),J(t_n)分别代表初始和结束的速度、加速度、和加加速度，p′(u)、p″(u)、p″′(u)分别为关节速度、加速度和加加速度轨迹曲线；

以矩阵方程形式描述轨迹曲线控制顶点反求方程

p＝C_Nd (7)

式中C_N∈R^{(n×5)×(n×5)}为系数矩阵，且

根据控制点和归一化时间节点矢量即可求出关节在时刻t_i经过位置p_i的关节轨迹曲线。

3.根据权利要求2所述的多机协同轨迹优化方法，其特征在于，步骤S3中以机器人作业效率最高、轨迹最平滑为目标将构建的轨迹进行优化，构造优化约束条件的具体方法如下：

对关节运动进行约束转化，

式中

B样条曲线具有凸包性质，因此，使关节m的轨迹曲线满足运动学约束，仅需B样条轨迹曲线的控制顶点满足

式中

和

分别为第m个关节的B样条速度、加速度和脉动曲线的第j个控制顶点。

4.根据权利要求3所述的多机协同轨迹优化方法，其特征在于，步骤S3中构建关节轨迹优化模型的具体流程如下：

多目标优化模型为：

式中：各目标函数S_i(x)(i＝1,2,...,f)及约束函数g_i(x)≤0和h_k(x)＝0都是决策变量x＝(x₁,x₂,...,x_n)函数。

5.根据权利要求4所述的多机协同轨迹优化方法，其特征在于，定义如下优化目标：

式中：S₁为运动时间，是路径点之间的时间间隔之和，衡量机器人的运动效率；S₂为关节平均加加速度，衡量轨迹的平滑度。T为机器人完成指定任务轨迹从初始位姿运动到终止位姿的时间；j_i为关节加加速度。

6.根据权利要求5所述的多机协同轨迹优化方法，其特征在于，利用NSGA2算法以各路径点之间的时间间隔为决策变量进行机械臂的多目标轨迹优化求解，步骤如下：

1)根据给定的机械臂运动路径点之间的时间间隔的范围随机产生种群规模为N的初始父代种群H₀，并通过筛选、杂交、变异等进化方式生成大小同为N的子代种群I₀；

2)将父代种群和子代种群合并和排序，生成种群规模为2N的种群R_t，对R_t进行非支配排序，得到等级F₁、F₂、F₃…；再依据每一非支配层中个体的拥挤度筛选个体构成新的父代种群；

3)根据非支配排序、拥挤度距离和约束违背值计算的排序结果选取N个个体作为新的父代种群H_i+1；

4)通过遗传算子产生新的子代种群I_i+1；

5)重复2)至4)步，直到达到算法设置的最大迭代次数；

将优化选取的时间变量重新代入关节位置序列，利用五次非均匀B样条插值，构造优化后端点运动参数可指定的高阶连续的关节轨迹，完成针对此条加工路径的关节轨迹优化。

Images