CN115213898A - 一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，包括步骤1至步骤6，还包括逆解多目标优化和最优解选取约束设定过程。本发明提出了逆解多目标优化方法，该方法综合考虑焊接机器人的结构尺寸和工作状况，优于传统的运动插补并利用matlab进行仿真；本发明的轨迹规划方法提高了逆解优化原则的全面性，并在一定程度上提高焊接机器人的运行刚度，同时将其和S型加减速曲线位置插补、基于单位四元数球面线性方法的姿态插补相结合，保证了焊接机机器人在完成预期轨迹的同时，末端轨迹、速度、加速度曲线、姿态曲线以及各关节位移变化曲线连续平滑，无突变，从而提高焊接机器人工作时候的平稳性。

Description

一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划 方法

技术领域

本发明属于机器人笛卡尔空间轨迹规划技术领域，具体涉及一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法。

背景技术

现代工业技术的快速发展，工业生产线逐渐趋向智能化。大多数生产线，例如汽车、船舶制造领域，进行焊接作业时，其工作环境恶劣，为了降低生产成本、提高生产效率、保证焊接精度，机器人便成为其生产线焊接作业中不可或缺的存在。机器人在进行工作尤其是从事高精度工作时，需要预先对机器人进行运动控制、轨迹规划等方面的验证，若直接进行实际验证，则成本大、安全性低。对于焊接机器人，其运行轨迹是确定的，焊接件的焊缝一般为直线和圆弧。为了保证焊接机器人能够沿焊缝准确完成焊接任务，有必要进行笛卡尔空间轨迹规划，以保证末端运动轨迹的稳定性，从而提高焊接精度和效率。然而，逆解的最优选择是轨迹规划的基础，因此需要选择合适的关节变量，与轨迹规划方法相结合，使得焊接机器人按照预期轨迹运行，平稳地完成焊接任务。

专利CN112757306A(公开号)中，根据当前的机械臂位姿进行正-逆解或者逆-正解转换，取转换结果与当前机械臂位姿的差的范数最小的逆解作为最合适的一组解；利用七次多项式轨迹规划方法进行轨迹规划。

专利CN111113431A(公开号)中，利用改进的D-H参数建立机器人运动学模型；根据机器人末端在轨迹点的位姿，利用封闭解法求解机器人6个关节角，得到每个轨迹点的多种关节角逆解；根据机器人各个关节角的工作范围，去除每个轨迹点的部分关节角逆解；基于“整体轨迹最短行程”和“多移动小关节，少移动大关节”的原则建立机器人逆解优化数学模型；将每个轨迹点筛选后的逆解转化为一个个节点,轨迹点的先后关系连接相应的节点，并引入起始节点S和终止节点T，建立有向图G；运用Dijkstra算法求解S节点到T节点最短路径。

对于现有的逆解优化原则，只是单纯考虑了“关节位移最小”原则，或者将“关节位移最小原则”和“多移动小关节，少移动大关”原则相结合，进行逆解优化，却忽视机器人的结构和工作状况,从而使得逆解最优并不全面；常规逆解优化方法主要考虑“行程最短”原则、“多移动小关节，少移动大关节”原则，挑选出关节变量。但是，该方法并不全面，未考虑机器人的结构尺寸以及机器人实际工况。

为此我们提出一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法来解决现有技术中存在的问题。

发明内容

本发明的目的在于提供一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，以解决上述背景技术中提出现有技术中的问题。

为实现上述目的，本发明采用了如下技术方案：

一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，包括：

步骤1、根据改进D-H参数法建立焊接机器人的运动学模型；

步骤2、机器人末端轨迹是根据将焊缝轨迹离散化得到的一系列轨迹点，利用插值算法得到的，因此将某端轨迹等价为有限个轨迹点考虑，由于焊接机器人为6自由串联的机器人，且后轴交于一点，利用解析法，进行逆解计算得出8组关节变量；

步骤3、约束其8组关节变量应在各个关节的角度极限范围之内；

步骤4、针对以往逆解优化原则不全面，未考虑机器人的结构尺寸以及机器人实际工况；结合机器人结构尺寸和工作情况，设定运行刚度指标k_sg以及改进的关节转动引起后续连杆移动空间范围性能指标S_gj，将两者与“关节位移最小”原则结合，设定逆解多目标优化指标为ω：

步骤5、基于S型加减速曲线的圆弧插补；

步骤6、通过matlab进行仿真，得出相关数据，主要分析圆弧轨迹，可知保证了焊接机器人末端轨迹、速度、加速度、姿态曲线的平稳性。

优选的，步骤4的

中；

其中p为两个影响指标的权重大小，θ_j为当前关节角度，θ_i,j为该轨迹点第i组第j个关节变量。

优选的，步骤5包括如下步骤：

步骤S1、通过设定T_k＝t_k-t_k-1(k＝1,...,7)，表示各个阶段时间，此外引入变量τ，设τ_k＝t-t_k-1，作为以各个时间段起始时间为零点的相对时间，同时设定J为加加速、减减速阶段的加加速度；设定a_max为最大加速度，v_max为最大速度，v_s为初始速度，v₁为初始速度；同时T₁＝T₃＝T₅＝T₇、T₂＝T₆，通过对每个过程进行分析，可以得到各个时间段的加速度函数、速度函数、位移函数；

确定每个阶段的运行时间，需设定S型曲线的位移、起止速度、最大速度、最大加速度、加加速度，每个阶段的运行时间如下公式：

归一化时间l(t)算子在插补运动中起到了调整步长的作用，归一化时间算子

其中S(t)为S型加减速曲线的随时间变化的位移，L为起止点的距离；

步骤S2、焊接机器人常规的焊缝形状，在笛卡尔空间下对焊接机器人进行圆弧轨迹规划；

提取直线焊缝，确定末端轨迹起始点p₀(x₀，y₀，z₀)和终止点p_n(x_n，y_n，z_n)的坐标；将基于S型加减速曲线的归一化时间算子作为插补步长，则第i个插值点的坐标p_i(x_i，y_i，z_i)则为：

提取圆弧焊缝，设定圆弧起止点坐标分别为p₁(x₁,y₁,z₁)、p₃(x₃,y₃,z₃),中间点p₂(x₂,y₂,z₂),根据空间圆弧的构建方法，确定坐标系{C}和坐标系{B}之间的转换矩阵

离散出圆弧焊缝的各点坐标：

x_ic＝r·cos(l(i)·dir·θ₁₃)

y_ic＝r·sin(l(i)·dir·θ₁₃)

z_ic＝0

设定焊缝起止点的姿态矩阵，求出其对应的四元数q₁、q₂，根据球面线性插补SLERP方法和归一化时间算子l(t)，进型姿态的插补如下公式所示：

优选的，步骤S1中各个时间段的加速度函数、速度函数、位移函数的公式分别如下：

优选的，还包括逆解多目标优化和最优解选取约束设定过程：

步骤A1、约束其8组关节变量应在各个关节的角度极限范围之内；

步骤A2、结合机器人结构尺寸和工作情况，进行逆解多目标优化：

对于焊接机器人，在实施焊接任务时，主要考虑关节转动引起后续连杆移动空间以及机械臂在实际工作所存在的刚度问题，因此将其作为影响因子，则刚度性能指标如下所示：

其中C_tt为笛卡尔柔度矩阵C的平移子矩阵，其C的表达式为：

公式中，J(q)为焊接机器人的雅可比矩阵，可由改进的D-H参数所确定，随机械臂姿态变化而变化，K_θ为关节刚度矩阵且是对角矩阵，其指标越大刚度性能越好，在行程最短原则基础上考虑了功率最小化，未考虑焊机节机器人机构参数，如下公式所示：

其中l_i为前三个关节的连杆长度；

关节转动引起后续连杆移动空间范围性能指标S_j，要比常规表达式更为全面，包含了机器人的结构参数，可得基于标准型D-H参数的表达式：

但考虑焊接机器人后三轴交与一点，将两公式相结合，基于改进型D-H参数，前三个关节的性能指标S_gj如下公式所示，后三个关节的表达形式不变；

将性能指标S_gj与“关节位移最小”原则相结合，得出关节角行程优化模型γ_S为：

统一数量级，令将刚度指标设为k_sg＝k_s·10^-μ，将刚度指标和关节角行程优化模型相结合，形成逆解多目标优化指标ω，如下公式所示：

优选的，步骤A2中，逆解最优选取的代码流程包括：

Step1、输入当前各个关节角变量θ_j，j＝1～6；

Step2、利用逆解表达式，求出下一位姿的关节变量，挑取符合关节角极限的关节变量θ_i,j，i＝1～n，j＝1～6；

Step3、将上述数据，带入逆解多目标优化指标中，求出每组关节角所对应的优化指标

Step4、进行对比，

挑选出ω_i最小的一组关节角度，为逆解最优解；

Step5、将挑选出的最优解作为θ_j，进行下一轮的逆解最优解选取。

本发明的技术效果和优点：本发明提出的一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，与现有技术相比，具有以下优点：

1、本发明提出了逆解多目标优化方法，该方法综合考虑焊接机器人的结构尺寸和工作状况，构建刚度性能评价指标、优化关节转动引起后续连杆移动面积性能指标，并于“关节位移最小”原则相结合，进行逆解最优选取。和以往优化方法进行对比，更具有全面性，并在一定程度上，改善了关节角度的变化幅度，以及机器人不同轨迹点处的刚度；并和轨迹规划相结合，在此基础上，将S型加减速曲线与焊接机器人的笛卡尔空间直线插补运动和圆弧插补运动相结合，对焊接机器人的位置插补；

2、本发明优于传统的运动插补，姿态插补采用基于S型加减速曲线的单位四元数球面线性插补。利用matlab进行仿真，得出相关数据，逆解优化方法和插值方法相结合，保证了焊接机器人末端轨迹、速度、加速度、姿态曲线以及关节变量曲线连续平滑，从而保证了焊接质量。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书以及附图中所指出的结构来实现和获得。

附图说明

图1为本发明实施例中S型加减速曲线的示意图；

图2为本发明实施例中常规焊缝的示意图；

图3为本发明实施例中通过matlab进行仿真，得出相关数据的流程图；

图4为本发明实施例中焊接机器人末端位移、速度、加速度的示意图；

图5为本发明实施例中焊接机器人末端圆弧轨迹的示意图；

图6为本发明实施例中焊接机器人各个关节变量的示意图；

图7为本发明实施例中焊接机器人末端姿态变化的示意图；

图8为本发明实施例中焊接机器人的优化方法对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供了如图1-8所示的实施例中：

一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，包括如下步骤：

1、根据改进D-H参数法建立焊接机器人的运动学模型。

2、机器人末端轨迹是根据将焊缝轨迹离散化得到的一系列轨迹点，利用插值算法得到的，因此将某端轨迹等价为有限个轨迹点考虑，由于焊接机器人为6自由串联的机器人，且后轴交于一点，利用解析法，进行逆解计算得出8组关节变量

3、约束其8组关节变量应在各个关节的角度极限范围之内。

4、针对以往逆解优化原则不全面，未考虑机器人的结构尺寸以及机器人实际工况。结合机器人结构尺寸和工作情况，设定运行刚度指标k_sg以及改进的关节转动引起后续连杆移动空间范围性能指标S_gj，将两者与“关节位移最小”原则结合，设定逆解多目标优化指标为ω。

其中p为两个影响指标的权重大小，θ_j为当前关节角度，θ_i,j为该轨迹点第i组第j个关节变量。

5、基于S型加减速曲线的圆弧插补

(1)通过图1设定T_k＝t_k-t_k-1(k＝1,...,7)，表示各个阶段时间。此外引入变量τ，设τ_k＝t-t_k-1，作为以各个时间段起始时间为零点的相对时间，同时设定J为加加速、减减速阶段的加加速度；设定a_max为最大加速度，v_max为最大速度，v_s为初始速度，v₁为初始速度；同时T₁＝T₃＝T₅＝T₇、T₂＝T₆。通过对每个过程进

行分析，可以得到各个时间段的加速度函数、速度函数、位移函数如公式(2)～(4)所示：

确定每个阶段的运行时间，需设定S型曲线的位移、起止速度、最大速度、最大加速度、加加速度。每个阶段的运行时间如公式(5)～(7)：

归一化时间l(t)算子在插补运动中起到了调整步长的作用，根据文献，归一化时间算子

其中S(t)为S型加减速曲线的随时间变化的位移，L为起止点的距离。

(2)焊接机器人常规的焊缝形状如图2所示，在笛卡尔空间下对焊接机器人进行圆弧轨迹规划3。

提取直线焊缝，确定末端轨迹起始点p₀(x₀，y₀，z₀)和终止点p_n(x_n，y_n，z_n)的坐标。将基于S型加减速曲线的归一化时间算子作为插补步长，则第i个插值点的坐标p_i(x_i，y_i，z_i)则为：

提取圆弧焊缝，设定圆弧起止点坐标分别为p₁(x₁,y₁,z₁)、p₃(x₃,y₃,z₃),中间点p₂(x₂,y₂,z₂),根据空间圆弧的构建方法，确定坐标系{C}和坐标系{B}之间的转换矩阵

离散出圆弧焊缝的各点坐标：

x_ic＝r·cos(l(i)·dir·θ₁₃) (9)

y_ic＝r·sin(l(i)·dir·θ₁₃) (10)

z_ic＝0 (11)

设定焊缝起止点的姿态矩阵，求出其对应的四元数q₁、q₂，根据球面线性插补SLERP方法和归一化时间算子l(t)，进型姿态的插补如公式(13)所示：

6、通过matlab进行仿真，得出相关数据，流程如图3，主要分析圆弧轨迹，可知保证了焊接机器人末端轨迹、速度、加速度、姿态曲线的平稳性，如图4～图7。

本发明技术方案中的关键点：

逆解多目标优化，最优解选取约束设定如下：

(1)约束其8组关节变量应在各个关节的角度极限范围之内；

(2)结合机器人结构尺寸和工作情况，进行逆解多目标优化：

对于焊接机器人，在实施焊接任务时，主要考虑关节转动引起后续连杆移动空间以及机械臂在实际工作所存在的刚度问题，因此将其作为影响因子，则刚度性能指标如下所示：

其中C_tt为笛卡尔柔度矩阵C的平移子矩阵，其C的表达式为：

公式中，J(q)为焊接机器人的雅可比矩阵，可由改进的D-H参数所确定，随机械臂姿态变化而变化，K_θ为关节刚度矩阵且是对角矩阵，其指标越大刚度性能越好。对于“多移动小关节，少动大关节”原则，在行程最短原则基础上考虑了功率最小化，未考虑焊机节机器人机构参数，如公式(3)所示：

其中l_i为前三个关节的连杆长度。

关节转动引起后续连杆移动空间范围性能指标S_j，要比常规表达式更为全面，包含了机器人的结构参数，可得基于标准型D-H参数的表达式：

但考虑焊接机器人后三轴交与一点，将公式(4)，(5)相结合，基于改进型D-H参数，前三个关节的性能指标S_gj如公式(6)所示，后三个关节的表达形式不变。

将性能指标S_gj与“关节位移最小”原则相结合，得出关节角行程优化模型γ_S为：

统一数量级，令将刚度指标设为k_sg＝k_s·10^-μ，将刚度指标和关节角行程优化模型相结合，形成逆解多目标优化指标ω，如公式(8)所示：

逆解最优选取的代码思路如下：

Step1：输入当前各个关节角变量θ_j，j＝1～6；

Step2：利用逆解表达式，求出下一位姿的关节变量，挑取符合关节角极限的关节变量θ_i,j，i＝1～n，j＝1～6；

Step3：将上述数据，带入逆解多目标优化指标中，求出每组关节角所对应的优化指标

Step4：进行对比，

挑选出ω_i最小的一组关节角度，为逆解最优解。

Step5：将挑选出的最优解作为θ_j，进行下一轮的逆解最优解选取。

表1权值和机器人影响因子分析

多目标优化指标更为全面，同时与关节转动引起后续连杆移动空间范围性能指标S_j进行对比，可知对于后三轴改善了其关节变量的变化幅度，在一定程度上改善了其刚度，如图7与图8所示；

综上所述，本发明的轨迹规划方法提高了逆解优化原则的全面性，并在一定程度上提高焊接机器人的运行刚度，同时将其和S型加减速曲线位置插补、基于单位四元数球面线性方法的姿态插补相结合，保证了焊接机机器人在完成预期轨迹的同时，末端轨迹、速度、加速度曲线、姿态曲线以及各关节位移变化曲线连续平滑，无突变，从而提高焊接机器人工作时候的平稳性；

本发明提出了逆解多目标优化方法，该方法综合考虑焊接机器人的结构尺寸和工作状况，构建刚度性能评价指标、优化关节转动引起后续连杆移动面积性能指标，并于“关节位移最小”原则相结合，进行逆解最优选取。和以往优化方法进行对比，更具有全面性，并在一定程度上，改善了关节角度的变化幅度，以及机器人不同轨迹点处的刚度；并和轨迹规划相结合，在此基础上，将S型加减速曲线与焊接机器人的笛卡尔空间直线插补运动和圆弧插补运动相结合，对焊接机器人的位置插补；

本发明优于传统的运动插补，姿态插补采用基于S型加减速曲线的单位四元数球面线性插补。利用matlab进行仿真，得出相关数据，逆解优化方法和插值方法相结合，保证了焊接机器人末端轨迹、速度、加速度、姿态曲线以及关节变量曲线连续平滑，从而保证了焊接质量。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，其特征在于，包括：

步骤1、根据改进D-H参数法建立焊接机器人的运动学模型；

步骤2、机器人末端轨迹是根据将焊缝轨迹离散化得到的一系列轨迹点，利用插值算法得到的，因此将某端轨迹等价为有限个轨迹点考虑，由于焊接机器人为6自由串联的机器人，且后轴交于一点，利用解析法，进行逆解计算得出8组关节变量；

步骤3、约束其8组关节变量应在各个关节的角度极限范围之内；

步骤4、针对以往逆解优化原则不全面，未考虑机器人的结构尺寸以及机器人实际工况；结合机器人结构尺寸和工作情况，设定运行刚度指标k_sg以及改进的关节转动引起后续连杆移动空间范围性能指标S_gj，将两者与“关节位移最小”原则结合，设定逆解多目标优化指标为ω：

步骤5、基于S型加减速曲线的圆弧插补；

步骤6、通过matlab进行仿真，得出相关数据，主要分析圆弧轨迹，可知保证了焊接机器人末端轨迹、速度、加速度、姿态曲线的平稳性。

2.根据权利要求1所述的一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，其特征在于：

步骤4的

中；

其中p为两个影响指标的权重大小，θ_j为当前关节角度，θ_i,j为该轨迹点第i组第j个关节变量。

3.根据权利要求1所述的一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，其特征在于：

步骤5包括如下步骤：

步骤S1、通过设定T_k＝t_k-t_k-1(k＝1,...,7)，表示各个阶段时间，此外引入变量τ，设τ_k＝t-t_k-1，作为以各个时间段起始时间为零点的相对时间，同时设定J为加加速、减减速阶段的加加速度；设定a_max为最大加速度，v_max为最大速度，v_s为初始速度，v₁为初始速度；同时T₁＝T₃＝T₅＝T₇、T₂＝T₆，通过对每个过程进行分析，可以得到各个时间段的加速度函数、速度函数、位移函数；

确定每个阶段的运行时间，需设定S型曲线的位移、起止速度、最大速度、最大加速度、加加速度，每个阶段的运行时间如下公式：

归一化时间l(t)算子在插补运动中起到了调整步长的作用，归一化时间算子

其中S(t)为S型加减速曲线的随时间变化的位移，L为起止点的距离；

步骤S2、焊接机器人常规的焊缝形状，在笛卡尔空间下对焊接机器人进行圆弧轨迹规划；

提取直线焊缝，确定末端轨迹起始点p₀(x₀，y₀，z₀)和终止点p_n(x_n，y_n，z_n)的坐标；将基于S型加减速曲线的归一化时间算子作为插补步长，则第i个插值点的坐标p_i(x_i，y_i，z_i)则为：

提取圆弧焊缝，设定圆弧起止点坐标分别为p₁(x₁,y₁,z₁)、p₃(x₃,y₃,z₃),中间点p₂(x₂,y₂,z₂),根据空间圆弧的构建方法，确定坐标系{C}和坐标系{B}之间的转换矩阵

离散出圆弧焊缝的各点坐标：

x_ic＝r·cos(l(i)·dir·θ₁₃)

y_ic＝r·sin(l(i)·dir·θ₁₃)

z_ic＝0

设定焊缝起止点的姿态矩阵，求出其对应的四元数q₁、q₂，根据球面线性插补SLERP方法和归一化时间算子l(t)，进型姿态的插补如下公式所示：

4.根据权利要求3所述的一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，其特征在于：步骤S1中各个时间段的加速度函数、速度函数、位移函数的公式分别如下：

5.根据权利要求1所述的一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，其特征在于：

还包括逆解多目标优化和最优解选取约束设定过程：

步骤A1、约束其8组关节变量应在各个关节的角度极限范围之内；

步骤A2、结合机器人结构尺寸和工作情况，进行逆解多目标优化：

对于焊接机器人，在实施焊接任务时，主要考虑关节转动引起后续连杆移动空间以及机械臂在实际工作所存在的刚度问题，因此将其作为影响因子，则刚度性能指标如下所示：

其中C_tt为笛卡尔柔度矩阵C的平移子矩阵，其C的表达式为：

公式中，J(q)为焊接机器人的雅可比矩阵，可由改进的D-H参数所确定，随机械臂姿态变化而变化，K_θ为关节刚度矩阵且是对角矩阵，其指标越大刚度性能越好，在行程最短原则基础上考虑了功率最小化，未考虑焊机节机器人机构参数，如下公式所示：

其中l_i为前三个关节的连杆长度；

关节转动引起后续连杆移动空间范围性能指标S_j，要比常规表达式更为全面，包含了机器人的结构参数，可得基于标准型D-H参数的表达式：

但考虑焊接机器人后三轴交与一点，将两公式相结合，基于改进型D-H参数，前三个关节的性能指标S_gj如下公式所示，后三个关节的表达形式不变；

将性能指标S_gj与“关节位移最小”原则相结合，得出关节角行程优化模型γ_S为：

统一数量级，令将刚度指标设为k_sg＝k_s·10^-μ，将刚度指标和关节角行程优化模型相结合，形成逆解多目标优化指标ω，如下公式所示：

6.根据权利要求5所述的一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法，其特征在于：

步骤A2中，逆解最优选取的代码流程包括：

Step1、输入当前各个关节角变量θ_j，j＝1～6；

Step2、利用逆解表达式，求出下一位姿的关节变量，挑取符合关节角极限的关节变量θ_i,j，i＝1～n，j＝1～6；

Step3、将上述数据，带入逆解多目标优化指标中，求出每组关节角所对应的优化指标

Step4、进行对比，

挑选出ω_i最小的一组关节角度，为逆解最优解；

Step5、将挑选出的最优解作为θ_j，进行下一轮的逆解最优解选取。

Images