CN114407019B - 一种工业机器人关节空间的无碰撞轨迹规划方法 - Google Patents

Abstract

本发明公开了一种工业机器人关节空间的无碰撞轨迹规划方法，包括：步骤1、根据机器人的正运动学与逆运动学，建立用于计算所述工业机器人关节角度与位姿信息的运动学模型；步骤2、建立用于计算工业机器人运动能耗的动力学模型；步骤3、构建用于机器人识别规避的包络模型；步骤4、根据给定工业机器人的运动起始点位姿与终止点位姿，建立轨迹模型；步骤5、对轨迹模型进行碰撞判断，若无碰撞则向轨迹模型中添加约束并通过启发式优化算法进行优化计算，输出最终无碰撞轨迹参数集，利用所述无碰撞轨迹参数集可得到工业机器人的无碰撞运动轨迹。本发明针对单元制造系统的工业机器人，通过多个模型优化计算，实现高效率的无碰撞轨迹规划。

Description

一种工业机器人关节空间的无碰撞轨迹规划方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种工业机器人关节空间的无碰撞轨迹方法。

背景技术

近十几年工业机器人技术得到广泛应用，如汽车制造业中的焊接、喷涂等。但目前机器人的路径、轨迹规划大多基于人工示教，无法满足自动化生产线上快节拍、高效率的工作需求；此外，机器人的安全运行也是自动化生成的必然要求。

专利文献CN113246143A公开了一种机械臂动态规避障碍轨迹规划方法及装置，所述方法包括：基于机械臂所在的实际场景搭建对应统一的虚拟场景；确定所述机械臂在虚拟场景的位置，结合碰撞检测算法计算得到机械臂的候选运动轨迹；对该候选运动轨迹的路径进行分段并整合计算获得连续且执行时间最短的无碰撞可执行轨迹。

该方法未考虑到当连杆过长时，会出现中段与障碍物碰撞的，但是关节在模拟中为出现碰撞提示的问题。

专利文献CN108247637A公开了一种工业机器人手臂视觉防撞操作方法，包括：步骤1：构建机械手臂在所述工作空间的三维坐标系；步骤2：构建基于Elman网络的待抓取物体目标识别模型；步骤3：构建基于多核支持向量机的机械臂抓取最优路径模型；步骤5：基于物体类别确认物体重心，求得抓取终点坐标，获取机器臂抓取最优路径，驱动机器臂抓取物体。

该方法采用图像识别方法完成对待抓取物定位抓取，关于图像识别设备的前期额外投入较大，且对设备的运算能力有一定的要求。

专利文献CN107139171A一种基于力矩控制的工业机器人避障轨迹规划方法，包括通过识别各关节运动学以及惯性参数数据建立机器人动力学模型；以关节角度位移作为运动路径，采用一种正弦加加速度规划方法进行各关节的速度加速度规划，将速度和加速度带入动力学模型中即可求解运动路径中的控制力矩；采用一种检测关节投影干涉情况然后判断关键点距离的方法进行碰撞检测。

该方法采用控制力矩与路径优化相结合，从而提高了避障的反应速度，但是检测与控制力矩的设备成本过高，且实用性不强。

发明内容

为了解决上述问题，本发明提供了一种工业机器人关节空间的无碰撞轨迹规划方法，该方法针对单元制造系统的工业机器人关节空间信息，通过多个模型优化计算，实现低成本、高效率的无碰撞轨迹规划。

一种工业机器人关节空间的无碰撞轨迹规划方法，包括：

步骤1、根据机器人的正运动学与逆运动学，建立用于计算所述工业机器人关节角度与位姿信息的运动学模型；

步骤2、基于步骤1中的所述运动学模型，建立用于计算工业机器人运动能耗的动力学模型；

步骤3、根据工业机器人工作空间中的障碍物尺寸与位置，构建用于机器人识别规避的包络模型；

步骤4、根据给定工业机器人的运动起始点位姿与终止点位姿，建立轨迹模型，所述轨迹模型包括轨迹种类，以及与所述轨迹种类对应的轨迹参数集；

步骤5、对步骤4中的轨迹模型进行碰撞判断，若无碰撞则向轨迹模型中添加约束并通过启发式优化算法进行优化计算，输出最终无碰撞轨迹参数集，利用所述无碰撞轨迹参数集可得到工业机器人的无碰撞运动轨迹。

具体的，所述步骤3中的包络模型包括圆柱体、长方体以及椭圆体，本发明选用包络参数获取方便且同尺寸下包络空间最大的圆柱体。

具体的，所述步骤4中的轨迹种类包括梯形速度轨迹，S型轨迹，多项式轨迹以及样条曲线轨迹，本发明选用贴合实际的梯形速度轨迹。

具体的，所述步骤5中的启发式优化算法包括粒子群算法，遗传算法以及蚁群算法：本发明采用粒子群算法通过粒子速度v在t+1时刻的公式和粒子位置x在t+1时刻的公式进行优化。

优选的，所述步骤5中的碰撞判断，是基于运动学模型计算工业机器人与包络模型的空间坐标关系，所述空间坐标关系包括竖直高度与水平投影坐标。

具体的，所述基于运动学模型计算工业机器人与包络模型的空间坐标关系，判断为轨迹发生碰撞时，需要满足以下条件：

(z_robot,i,v<H)∧((x_robot,i,v,y_robot,i,v)∈∏)

其中，z_robot,i,v为机器人关节高度，H为包络模型的高度， (x_robot,i,v,y_robot,i,v)为机器人关节投影地面水平坐标，∏为包络模型的地面投影范围，i为机器人关节数，v为虚拟关节序号，当v≠0时代表该关节点为虚拟关节，反之则为真实关节，在公式中引入虚拟关节的概念，解决机器人连杆长度过长导致连杆两端真实关节无法判断是否碰撞的问题。

该方法相较于传统的欧氏距离、安全距离等碰撞判断方法，更加简便直观。

具体的，所述步骤5的具体过程为：

步骤5.1、确定当前轨迹参数集，给出初始轨迹；

步骤5.2、判断当前轨迹是否与工作空间中的障碍物发生碰撞，当轨迹与障碍物发生碰撞，则解除碰撞并重新生成新的轨迹，反之则进入下一步骤；

步骤5.3、判断轨迹是否满足约束的条件，当不满足约束时，则采用启发式优化算法对轨迹参数集进行优化；

步骤5.4、判断优化后的轨迹是否与工作空间中的障碍物发生碰撞，当轨迹与障碍物发生碰撞，则返回步骤5.2解除碰撞并重新生成新的轨迹规划；

步骤5.5、若满足约束的条件且不发生碰撞，则输出优化后的轨迹参数集。

优选的，所述步骤5.2中的解除碰撞，是通过竖直或水平投影方向上的最大碰撞距离重定位轨迹点实现，具体为：

(1)通过竖直方向上的最大碰撞距离来选择新的轨迹点；

(2)通过水平投影方向的最大碰撞距离来选择新的轨迹点；

通过上述方法配合使用可以有效解除碰撞，且运算压力小，反馈速度快。

具体的，所述步骤5中的约束，是将工业机器人的运动能耗作为约束条件，以最小运动时间为目标函数进行优化：

目标函数：

约束条件：无碰撞

其中，t为时间，q_s为机器人位于初始点A时的关节角，q_e为机器人位于终止点B时的关节角，为最大关节角速度，/>为最大关节角加速度，τ为机器人关节力矩，v为机器人的轨迹速度，Traj(·)代表轨迹规划种类，E为机器人的能耗，E_max为预设允许的机器人最大能耗。

与现有技术相比，本发明的有益效果为：

(1)针对单元制造系统的工业机器人，通过多个模型配合优化计算，实现低成本、高效率的无碰撞轨迹规划。

(2)在优化过程的碰撞判断，是通过判断机器人和障碍物在竖直和水平投影方向之间的关系而确定是否碰撞，从而降低了判断时的运算压力，提高了反馈速度。

(3)在碰撞判断中引入了虚拟关节的概念，解决机器人连杆长度过长导致连杆两端真实关节无法判断是否碰撞的问题。

附图说明

图1为实施例中机器人与障碍物的结构示意图；

图2为本发明提供的无碰撞轨迹规划方法的流程图；

图3为实施例中机器人的D-H运动学示意图；

图4为本实例中障碍物的包络模型的示意图；

图5为本发明提供的无碰撞轨迹规划方法中步骤5的流程图；

图6为本实例中机器人的碰撞示意图；

图7为实施例中竖向最大碰撞距离重定位轨迹的解除碰撞示意图；

图8为实施例中投影方向最大碰撞距离重定位轨迹的解除碰撞示意图；

图9为实施例中机器人的无碰撞轨迹示意图。

具体实施方式

如图1所示，本实施例中单元制造系统设有一台四关节串联机器人，在机器人的工作空间中还设有障碍物1与障碍物2，以四关节串联机器人和两个障碍物的位姿在全局坐标系{o}中描述，在全局坐标系{o}中的A点为四关节串联机器人的运动起始点，B点为四关节串联机器人的运动终止点。

为了实现该机器人在单元制造系统中能快速实现低成本、高效率的无碰撞轨迹规划的目标，提出了一种工业机器人关节空间的无碰撞轨迹规划方法，如图2所示，具体步骤如下：

步骤1、根据机器人的正运动学与逆运动学，建立用于计算所述工业机器人关节角度与位姿信息的运动学模型：

其中，如图3所示，正运动学采用D-H坐标系进行描述：在四关节串联机器人的四个转动部位依次建立坐标系用于后续描述机器人的运动学；

逆运动学则采用牛顿-拉夫逊迭代法进行迭代求解，具体数学理论及公式均为现有技术，即不再赘述。

步骤2、基于步骤1中的所述运动学模型，建立用于计算工业机器人运动能耗的动力学模型：

其中，逆动力学模型可简化为线性模式τ＝YΦ，τ为机器人关节力矩， Y为机器人观测矩阵，Ф为机器人动力学参数，机器人观测矩阵可通过机器人运动参数计算获得，机器人动力学参数可通过辨识动力学参数或三维模型得到。

步骤3、根据工业机器人工作空间中的障碍物尺寸与位置，构建用于机器人识别规避的包络模型：

其中常用的包络模型包括圆柱体、长方体以及椭圆体，本实施例中采用空间信息简单且包络面积较大的圆柱体包络模型，如图4所示，全局坐标系{o}中的障碍物1的包络参数为(H₁,r₁)，障碍物2的包络参数为(H₂,r₂)。

此外本发明是针对单元制造系统中的轨迹规划，上述参数均为固定参数或容易获取的参数。

步骤4、根据给定工业机器人的运动起始点位姿与终止点位姿，建立轨迹模型，该轨迹模型包括轨迹种类，以及与轨迹种类对应的轨迹参数集：

根据已建立的全局坐标系{o}，确定起始点A与终止点B的位姿矩阵 T_A与T_B，通过运动学模型求解获得对应的机器人关节角：

q_s＝IK(T_A),q_e＝IK(T_B)

其中，q_s为机器人位于初始点A时的关节角，q_e为机器人位于终止点B时的关节角，IK(·)为逆运动学函数；

轨迹种类包括梯形速度轨迹，S型轨迹，多项式轨迹以及样条曲线轨迹；

本实施例中采用梯形速度轨迹，该轨迹的表达式如下：

其中，a_max为最大角加速度，V_max为最大角速度，t为时间，0～t₁为加速阶段，t₁～t₂为匀速阶段，t₂～t₃为减速阶段；

而与梯形速度轨迹对应的轨迹参数集，具体表达式如下：

其中，S为轨迹参数集，q为关节角，为最大关节角速度，/>为最大关节角加速度。

步骤5、对步骤4中的轨迹模型进行碰撞判断，若无碰撞则向轨迹模型中添加约束并通过启发式优化算法进行优化计算，输出最终无碰撞轨迹参数集，利用所述无碰撞轨迹参数集可得到工业机器人的无碰撞运动轨迹：

如图5所示，碰撞判断与优化的具体过程，如下：

步骤5.1、确定当前轨迹参数集，给出初始轨迹；

步骤5.2、判断当前轨迹是否与工作空间中的障碍物发生碰撞，当轨迹与障碍物发生碰撞，则解除碰撞并重新生成新的轨迹，反之则进入下一步骤；

步骤5.3、判断轨迹是否满足约束的条件，当不满足约束时，则采用启发式优化算法对轨迹参数集进行优化；

步骤5.4、判断优化后的轨迹是否与工作空间中的障碍物发生碰撞，当轨迹与障碍物发生碰撞，则返回步骤5.2解除碰撞并重新生成新的轨迹规划；

步骤5.5、若满足约束的条件且不发生碰撞，则输出优化后的轨迹参数集，作为最终的轨迹规划方案。

其中，本实施例中的启发式优化算法选用粒子群算法，通过粒子速度 v在t+1时刻的公式和粒子位置x在t+1时刻的公式进行优化：

v(t+1)＝wv(t)+a₁b₁(g_best-x(t))+a₂b₂(p_best-x(t))

x(t+1)＝x(t)+v(t+1)

式中，w为粒子飞行的惯性权重，a₁和a₂是学习因子，b₁和b₂是0和 1之间的随机数，g_best是全局最优值，p_best是个体最优值。

如图6所示，为实施例中机器人的一种碰撞情况，图中机器人的拾取端已伸入包络模型空间中，即机器人关节J₄的高度z₄低于包络模型的高度 H，同时关节J₄的底面投影J′₄在包络模型的底面投影∏中，Δz_max为最大碰撞深度。

通过本发明中的碰撞判断，具体表示为：

(z_robot,i,v<H)∧((x_robot,i,v,y_robot,i,v)∈∏)

其中，z_robot,i,v为机器人关节高度，H为包络模型的高度， (x_robot,i,v,y_robot,i,v)为机器人关节投影地面水平坐标，∏为包络模型的地面投影范围，i为机器人关节数，v为虚拟关节序号，当v≠0时代表该关节点为虚拟关节，反之则为真实关节；

该表达式中还引入虚拟关节的概念，当机器人连杆长度过长时，可能会出现两端真实关节未满足碰撞判断，但是实际两关节之间的连杆却与包络模型碰撞情况，因此为连杆等间距设置虚拟关节可以有效解决上述问题。

其中，解除碰撞是基于竖直或水平投影方向上的最大碰撞距离重定位轨迹点实现，具体为：

确定新的R为旋转部分，表示姿态；O为平移部分，表示位置。

(1)通过竖直方向上的最大碰撞距离来选择新的轨迹点：如图7所示，在z轴方向上找到最大碰撞深度的轨迹点和对应的关节后，更新位姿矩阵第3行第4列的z坐标，具体表达式为：

T′_0i(3,4)＝T_0i(3,4)+c_zΔz_max

其中，T_0i为最大碰撞深度的轨迹点对应关节的位姿矩阵，T′_0i为新的位姿矩阵，Δz_max为最大碰撞深度，c_z为安全系数，c_z>1；

(2)通过水平投影方向的最大碰撞距离来选择新的轨迹点；如图8 所示，在水平方向(xoy平面)上找到最大碰撞长度的轨迹点和对应的关节后，更新位姿矩阵第1行第4列的x坐标和第2行第4列的y坐标，具体表达式为：

T′_0i(1,4)＝T_0i(1,4)+c_l cosβΔl_max

T′_0i(2,4)＝T_0i(2,4)+c_l sinβΔl_max

其中，T_0i为最大碰撞深度的轨迹点对应关节的位姿矩阵，T′_0i为新的位姿矩阵，Δl_max为最大碰撞长度，β为无碰撞点的方向角，c_l为安全系数， c_l>1。

对新的位姿矩阵通过运动模型求解，获得新的位姿矩阵对应的关节角度，进一步确定新的轨迹点，从而将原轨迹划分为两条新的子轨迹，解除碰撞。

在轨迹模型的优化过程中添加约束，是将工业机器人的运动能耗作为约束条件，以最小运动时间为目标函数进行优化，其表达式如下：

目标函数：

约束条件：无碰撞

其中，t为时间，q_s为机器人位于初始点A时的关节角，q_e为机器人位于终止点B时的关节角，为最大关节角速度，/>为最大关节角加速度，τ为机器人关节力矩，v为机器人的轨迹速度，Traj(·)代表轨迹规划种类，E为机器人的能耗，E_max为预设允许的机器人最大能耗。

经过上述步骤的优化后，本实施例的机器人优化后的无碰撞轨迹，如图9所示，其中，实线代表最优无碰撞轨迹路径，虚线代表在优化过程中出现的碰撞轨迹路线，A点为起始点，B点为终止点，C为碰撞点，D为优化后的无碰撞点。

Claims (5)

1.一种工业机器人关节空间的无碰撞轨迹规划方法，其特征在于，包括：

步骤1、根据机器人的正运动学与逆运动学，建立用于计算所述工业机器人关节角度与位姿信息的运动学模型；

步骤2、基于步骤1中所述的运动学模型，建立用于计算工业机器人运动能耗的动力学模型；

步骤3、根据工业机器人工作空间中的障碍物尺寸与位置，构建用于机器人识别规避的包络模型；

步骤4、根据给定工业机器人的运动起始点位姿与运动终止点位姿，建立轨迹模型，所述轨迹模型包括轨迹种类，以及与所述轨迹种类对应的轨迹参数集，所述轨迹种类包括梯形速度轨迹，S型轨迹，多项式轨迹以及样条曲线轨迹；

步骤5、基于运动学模型计算工业机器人与包络模型的空间坐标关系对步骤4中的轨迹模型进行碰撞判断，当满足以下条件：

(z_robot,i,v<H)∧((x_robot,i,v,y_robot,i,v)∈∏)

其中，z_robot,i,v为机器人关节高度，H为包络模型的高度，(x_robot,i,v,y_robot,i,v)为机器人关节投影地面水平坐标，∏为包络模型的地面投影范围，i为机器人关节数，v为虚拟关节序号，当v≠0时代表虚拟关节，反之则为真实关节，则判断为轨迹发生碰撞；

若无碰撞则向轨迹模型中将工业机器人的运动能耗作为约束条件并通过启发式优化算法以最小运动时间为目标函数进行优化计算，输出最终无碰撞轨迹参数集，利用所述无碰撞轨迹参数集可得到工业机器人的无碰撞运动轨迹；

所述目标函数：

所述约束条件：无碰撞

其中，t为时间，q_s为机器人位于初始点A时的关节角，q_e为机器人位于终止点B时的关节角，为最大关节角速度，/>为最大关节角加速度，τ为机器人关节力矩，v为机器人的轨迹速度，Traj(·)代表轨迹规划种类，E为机器人的能耗，E_max为预设允许的机器人最大能耗。

2.根据权利要求1所述的无碰撞轨迹规划方法，其特征在于，所述步骤3中的包络模型包括圆柱体、长方体以及椭圆体。

3.根据权利要求1所述的无碰撞轨迹规划方法，其特征在于，所述步骤5中的启发式优化算法包括粒子群算法，遗传算法以及蚁群算法。

4.根据权利要求1所述的无碰撞轨迹规划方法，其特征在于，所述步骤5的具体过程为：

步骤5.1、确定当前轨迹参数集，给出初始轨迹；

步骤5.2、判断当前轨迹是否与工作空间中的障碍物发生碰撞，当轨迹与障碍物发生碰撞，则解除碰撞并重新生成新的轨迹，反之则进入下一步骤；

步骤5.3、判断轨迹是否满足约束的条件，当不满足约束时，则采用启发式优化算法对轨迹参数集进行优化；

步骤5.4、判断优化后的轨迹是否与工作空间中的障碍物发生碰撞，当轨迹与障碍物发生碰撞，则返回步骤5.2解除碰撞并重新生成新的轨迹规划；

步骤5.5、若满足约束的条件且不发生碰撞，则输出优化后的轨迹参数集，作为最终的轨迹规划方案。

5.根据权利要求4所述的无碰撞轨迹规划方法，其特征在于，所述步骤5.2中的解除碰撞，是通过竖直或水平投影方向上的最大碰撞距离重定位轨迹点实现。