基于分层搜索树实现工件自动布置的变位机路径规划方法与流程

1.本发明属于机器人工程技术领域，具体涉及基于分层搜索树实现工件自动布置的变位机路径规划方法。


背景技术：

2.在焊接场景中，通常需要变位机来调整工件的位置，使焊接机器人更加方便地进行焊接。由于工件通常是固接在变位机上的，因此变位机在焊接过程中的运动将会影响工件焊缝的位置，而焊缝的位置决定着最终的焊接质量。因此，变位机的运动路径影响最终的焊缝质量，对变位机进行路径规划有重大意义。
3.焊接过程中，焊缝位置一般有平焊、横焊、立焊和仰焊。平焊中的俯仰角和倾斜角都需要控制在一定的范围内，以保证熔化金属能够稳定成型，避免偏焊。文献《kinematic aspects of a robot
‑
positioner system in an arc weldingapplication》给出机器人变位机系统的运动学原理，并给出焊缝俯仰角与倾斜角的描述，该文献给出焊缝位置的约束函数与代价函数，可作为变位机路径规划的约束和评价指标。然而，在明确约束与代价函数后，若以传统的优化方法，或用局部启发式的智能搜索方法，则会因路径点数太多而导致变量太多，路径求解困难且算法的优化效果较差。


技术实现要素：

4.本发明的主要目的在于克服现有技术的缺点与不足，提出一种基于分层搜索树实现工件自动布置的变位机路径规划方法，通过分层搜索树的逐层建立过程，能够给出一定分辨率下的变位机的所有满足约束的路径组合，从组合中根据代价函数来选取最优路径，该方法能够满足概率完备性，当问题无解时，方法能够较快终止。
5.为了达到上述目的，本发明采用以下技术方案：
6.基于分层搜索树实现工件自动布置的变位机路径规划方法，包括以下步骤：
7.s1、初始化分层搜索树，确定分层搜索树的根和结束条件；
8.s2、计算规划空间并采样，计算工件焊缝位置约束函数，根据焊缝位置约束和避障约束从搜索树第二层开始计算变位机的规划空间，并从规划空间中采样合法位形；
9.s3、选择扩展起始点并计算扩展终止点，根据工件焊缝位置代价函数，从搜索树中选择到采样点代价最小的多个节点作为候选扩展点；
10.s4、生成局部路径，使用关节插补方式生成局部路径，对局部路径每一个离散点进行约束判定，若满足约束则将扩展终止点加入到搜索树；
11.s5、判断结束条件，判断分层搜索树当前层采样是否结束以及算法是否结束。
12.进一步的，步骤s1中，确定分层搜索树的根具体为：
13.搜索树的根为用户定义或从第一个路径点对应的规划空间进行采样，规划空间需要使得变位机满足避障以及使工件处于船型焊，即为最适合焊接的位置；
14.确定分层搜索树的结束条件具体为：
15.分层搜索树的结束条件为分层搜索树扩展到最后一个路径点。
16.进一步的，步骤s2中，计算规划空间并采样具体为：
17.s21、离散变位机的规划空间，对变位机的所有位形进行穷举，其所有位形的集合为：
[0018][0019]
其中，q
min
[k]和q
max
[k]为第k个关节运动范围的下界和上界，为第k个关节的离散数量，m
k
为第k个关节采样的分辨率，f
i
为插值函数；
[0020]
s22、确定焊缝位置约束，焊缝位置约束保证焊缝成型过程质量，使焊缝处于水平状态且使相邻母材的法向量之和垂直指向上，以防止母材下倾导致融化金属往下流，具体约束如下：
[0021][0022]
其中，ε1为接近于零的极小值，ε2取10
°
，θ1为焊缝的旋转角，θ2为焊缝俯仰角，θ3为焊缝相邻母材夹角的1/2，θ1的约束用于保证母材平面法向量始终朝上，防止母材下倾，θ2的约束用于减少熔化金属往倾斜角方向流动；
[0023]
s23、采样合法位形，根据s21中得到的离散规划空间，从中均匀采样并作碰撞约束与焊缝位置约束的判定，若满足约束，则得到随机位形q
rand
，否则重新采样。
[0024]
进一步的，步骤s3中，选择扩展起始点并计算扩展终止点具体包为：
[0025]
s31、确定扩展起始点，扩展起始点q
near
根据代价函数从搜索树的上一层进行选取，代价函数为：
[0026]
g(q
rand
)＝g(q
near
) min{f
l
(lp)}
[0027]
其中，g(q)为从根节点q
root
到当前节点q的累积最小代价，f
l
(lp)为局部路径lp的代价函数，代价函数包括焊缝位置代价函数和路径长度代价函数，使用加权求和函数来表示：
[0028]
f
l
(lp)＝w1f
pobj
(lp) w2f
dobj
(lp)
[0029]
其中，w1和w2为两个函数对应的权值，焊缝位置代价函数用表示，即控制焊缝位置的俯仰角与旋转角越小越好，路径长度代价函数用表示，即用前后两路径点的关节角变化值来表示；
[0030]
s32、确定扩展终止点，扩展终止点到扩展起始点的距离小于分层搜索树的最大扩展步长，可选择随机位形q
rand
作为扩展终止点q
new
。
[0031]
进一步的，步骤s4中，采用关节插补的方式生成局部路径，并对局部路径的每一个插补点进行避障约束和焊缝位置约束的判定，若局部路径中的每一个离散点均满足避障约束和焊缝位置约束，则将扩展终止点加入到搜索树中并指向扩展起始点，否则返回到步骤s2。
[0032]
进一步的，步骤s5中，判断结束条件具体为：
[0033]
当搜索树扩展到最后一个路径点对应的层数时，搜索结束；从最后一层中选择累积代价最小的节点往前搜索到根，即为最终得到的最优路径。
[0034]
本发明与现有技术相比，具有如下优点和有益效果：
[0035]
1、本发明适用但不限于满足平焊位置的工件自动布置，仅需改变约束函数与目标函数即可满足其他焊缝摆放位置的工件自动布置，算法通用性强。
[0036]
2、本发明中涉及到的分层搜索树能够较快地计算当前路径点对应的规划空间，当规划空间中不存在可行位形时问题无解，算法在问题无解时能够及时终止。
[0037]
3、本发明基于采样的方式来扩展分层搜索树，当采样次数迫近规划空间位形的数量时，若问题有解，则找到解的概率趋近于1，算法满足概率完备性。
附图说明
[0038]
图1是本发明方法的流程图；
[0039]
图2是本发明的分层搜索树示意图；
[0040]
图3是本发明的焊缝位置约束示意图。
具体实施方式
[0041]
下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0042]
实施例
[0043]
如图1所示，本发明，基于分层搜索树实现工件自动布置的变位机路径规划方法，包括以下步骤：
[0044]
s1、初始化分层搜索树，确定分层搜索树的根和结束条件，求解第一个路径点对应的规划空间，从规划空间中取多个可行位形作为搜索树的根；
[0045]
本实施例中，步骤s1中初始化分层搜索树，确定分层搜索树的根，使其满足避障约束与初始路径点对应的焊缝位置约束，从第一个路径点位置对应的规划空间中选择最合适的一个位形。分层搜索树的结束条件为分层搜索树扩展到最后一个路径点。s2、计算规划空间并采样，计算工件焊缝位置约束函数，从搜索树第二层开始计算变位机的规划空间，并从规划空间中采样合法位形；
[0046]
在本实施例中，步骤s2中计算规划空间并采样的具体步骤如下：
[0047]
s21、离散变位机的规划空间，变位机的自由度数较低，关节组合较少，可对变位机的所有位形进行穷举，其所有位形的集合为：
[0048][0049]
其中，q
min
[k]和q
max
[k]为第k个关节运动范围的下界和上届，
为第k个关节的离散数量，m
k
为第k个关节采样的分辨率，f
i
为插值函数；
[0050]
s22、确定焊缝位置约束，焊缝位置约束保证焊缝成型过程质量，使焊缝处于水平状态且使相邻母材的法向量之和垂直指向上，以防止母材下倾导致融化金属往下流；
[0051]
在本实施例中，如图3所示，为焊缝位置约束示意图，p1和p2为母材，向量v为焊缝在当前焊接点的切线方向，其与水平面xoy形成的夹角θ2为焊缝当前位置的俯仰角，向量n与焊缝相邻面在当前焊缝点的角平分线方向重合，其与任一母材的夹角为θ3，将世界坐标系z轴往焊缝点坐标系的xoz平面投影，投影向量与n的夹角θ1为焊缝的旋转角，焊缝位置约束表示为：
[0052][0053]
其中，ε1为接近于零的极小值，ε2取10度左右，θ1的约束是为了保证母材平面法向量始终朝上，防止母材下倾，θ2的约束是为了减少熔化金属往倾斜角方向流动；
[0054]
s23、采样合法位形，根据步骤s21中得到的离散规划空间，从中均匀采样并作碰撞约束与焊缝位置约束的判定，若满足约束，则得到随机位形q
rand
，否则重新采样。
[0055]
s3、选择扩展起始点并计算扩展终止点，根据工件焊缝位置代价函数，从搜索树中选择到采样点代价最小的n个节点作为候选扩展点；
[0056]
本实施例中，步骤s3中选择扩展起始点并计算扩展终止点的具体步骤如下：
[0057]
s31、确定扩展起始点，扩展起始点q
near
根据代价函数从搜索树的上一层进行选取，代价函数为：
[0058]
g(q
rand
)＝g(q
near
) min{f
l
(lp)}
[0059]
其中，g(q)为从根节点q
root
到当前节点q的累积最小代价，f
l
(lp)为局部路径lp的代价函数，代价函数包括焊缝位置代价函数和路径长度代价函数，使用加权求和函数来表示：
[0060]
f
l
(lp)＝w1f
pobj
(lp) w2f
dobj
(lp)
[0061]
其中，w1和w2为两个函数对应的权值，焊缝位置代价函数用表示，即控制焊缝位置的俯仰角与旋转角越小越好，路径长度代价函数用表示，即用前后两路径点的关节角变化值来表示；
[0062]
s32、确定扩展终止点，扩展终止点到扩展起始点的距离小于分层搜索树的最大扩展步长，可选择随机位形q
rand
作为扩展终止点q
new
。
[0063]
s4、生成局部路径，使用关节插补方式生成局部路径，对局部路径每一个离散点进行约束判定，若满足约束则将扩展终止点加入到搜索树；
[0064]
本实施例中，步骤s4生成局部路径，使用关节插补的方式生成局部路径，并对局部路径的每一个插补点进行避障约束和焊缝位置约束判定，若局部路径中的每一个离散点均满足避障约束和焊缝位置约束，则将扩展终止点加入到搜索树中，并指向扩展起始点，否则回到步骤s2中。
[0065]
s5、判断结束条件，判断分层搜索树当前层采样是否结束以及算法是否结束。
[0066]
在本实施例中，步骤s5判断结束条件，当搜索树扩展到最后一个路径点对应的层数时，搜索结束，从最后一层中选择累积代价最小的节点往前搜索到根，即为最终得到的最优路径。
[0067]
如图2所示，为算法过程中维护的分层搜索树，p
i
(i＝0,1...n)为焊缝经过离散后的路径点位置，分别对应着搜索树t的第r
i
(i＝0,1...n)层的规划空间。
[0068]
还需要说明的是，在本说明书中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0069]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。