一种基于局部搜索算法的机械臂自主避障路径生成方法与流程

1.本发明涉及工业机器人技术领域，特别涉及一种基于局部搜索算法的机械臂自主避障路径生成方法。


背景技术：

2.在现代机器人应用中，随着工作环境的复杂多样化，机械臂需要与环境中的物体和人类进行更多的交互，这对机械臂的自主避障能力提出了越来越高的要求。良好的自主避障能力不仅能够大幅增加了机械臂的工作效率，同时也增加了于人体交互的安全性。
3.目前已有的机械臂避障方案有如下几类：
4.1.手动调节法。该方法需要在障碍物处，手动更改机械臂的运动路径来避过障碍物。使用该方法往往需要经过多次尝试才能获得有效的机器人运动路径，调整过程繁琐低效，同时难以保证最终获取路径的性能。
5.2.人工势场法，神经网络法，随机树搜索法等自主规划方法。其中，人工势场法在障碍物,目标点和机械臂之间建立势场函数，通过目标点吸引势与障碍物的排斥势相结合，对机械臂的路径进行规划。然而，该方法存在容易陷入局部极小值，产生错误规划的问题。神经网络法需要大量额数据进行模型训练，引入过多的数据采集工作，同时难以保证算法的稳定性。随机树搜索法通过初始值向外随机搜索，不断扩大搜索范围，寻找可行的解。该算法需要浪费大量的算力在不正确的搜索方向上，算法效率不高。上述这些方法的另一个缺点是没有寻找到避开障碍物的最短路径，同时也未考虑所生成路径的曲率，不能保证机械臂最终的运行效率。


技术实现要素：

6.本发明的目的旨在至少解决所述技术缺陷之一。
7.为此，本发明的目的在于提出一种基于局部搜索算法的机械臂自主避障路径生成方法。
8.为了实现上述目的，本发明的实施例提供一种基于局部搜索算法的机械臂自主避障路径生成方法，包括如下步骤：
9.步骤s1，初始化障碍物相关信息，根据初始化后的信息生成障碍物的边界框；
10.步骤s2，将当前关节角度一个邻域内的关节点离散化；
11.步骤s3，从当前角度出发，依据预测的关节角度变化律获得下一步的搜索路径点，判断从当前关节角度出发能否获得可行路径点，如果能够获得可行路径点，则从可行路径点中筛选最优路径点，进而判断是否到达终点；如果不能获得可行路径点则进行路径回溯，返回前一个路径点重新进入s2向前搜索；
12.步骤s4，如果判断到达终点，则将所有最优路径点平滑连接起来，生成最终的平滑路径，如果判断没有到达终点，则前进一步返回步骤s2，搜索下一个路径点。
13.进一步，在所述步骤s1，所述初始化障碍物相关信息，包括：初始化路径起始点、终
点、角度极限、障碍物边界信息。
14.进一步，所述生成障碍物的边界框，包括：选择由圆形和矩形元素描述障碍物的边界框；其中圆形由球心oi(xoi,yoi,zoi)和半径roi组成，矩形由长方体的各个顶点(xci,yci,zci)表示，障碍物的边界框由上述元素组成的圆柱形，球形和多面体进行描述。
15.进一步，在所述步骤s2中，
16.首先，每个关节角度满足约束：
[0017][0018]
其中，分别表示第i个关节的关节角度，关节角度上限和下限；m表示关节总个数；
[0019]
式子(1)表示每个关节角度都必须处于其限定范围内，是机械臂处于任何姿态，均需要遵守的固有约束；
[0020]
依据机械臂的具体状态，对其关节角做出了进一步的约束：
[0021][0022][0023][0024]
其中，分别表示第i个关节第c步关节角的角度，角度上限和下限，c表示从起始开始的步数序号；
[0025]
分别表示第i个关节初始关节角度上限，第c-1步的关节角度上限，关节角极限变化范围；
[0026]
分别表示第i个关节初始关节角度下限，第c-1步的关节角度下限；
[0027]
确定离散关节点时的离散步长：
[0028][0029]
其中，ns表示离散关节点时划分的步数，dq
i,c
表示第i个关节角第c步的离散步长。
[0030]
进一步，在所述步骤s3中，
[0031]
步骤s31，每次搜索只选择距离当前点位一个离散步长的离散点位作为目标路径点，每次搜索时每个关节角度可能的结果为：
[0032][0033]
其中，表示第i个关节角第c步可能的搜索结果，q
i,c-1
表示第i个关节角第c-1步的最优路径点；
[0034]
步骤s32，对于上式中可能的搜索结果，确定搜索规则，寻找最优路径点。
[0035]
进一步，所述确定搜索规则，包括：
[0036]
式子(6)可以进一步地表示为：
[0037]
[0038]
其中，sn
i,c
为符号标志位，其可能的值为-1，0，1，其中i表示关节角序号，c表示步数序号；
[0039]
此时，每次路径点的搜索过程转化为sn
i,c
的遍历过程，采用基于预测关节角度变化律的方法确定sn
i,c
的遍历准则，使得能够优先遍历到最优路径点，加快搜索速度。
[0040]
进一步，所述遍历规则如下：
[0041]
第一步时，首先取：
[0042][0043]
其中，表示第i个关节角的目标角度，表示第i个关节角的初始角度，sgn为取符号函数，表示sn
i,1
的可能取值；
[0044]
依据式子(8)确定sn
i,1
遍历时的首值，剩余两值的遍历顺序依据随机法确定，进而确定sn
i,1
的遍历顺序；
[0045]
2)第二步，第三步时，首先取：
[0046][0047]
依据式子(9)确定sn
i,c
遍历时的首值，由式子(8)式子确定遍历时的第二个值，若(8)(9)两个式子给出同样的结果，则采用随机法确定遍历时的第二个值，确定遍历顺序；
[0048]
3)当步数大于三时，首先取：
[0049][0050]
依据式子(10)确定sn
i,c
遍历时的首值；若由式子(10)确定的首值不为0，则将遍历时的第二个值确定为0，进而确定遍历顺序；若由式子(10)确定的首值为0，则由式子(9)确定遍历时的第二个值，若式子(9)确定的值也为0，则采用式子(8)确定遍历时的第二个值，若式子(8)确定的值也为0，则采用随机法从剩余的两个可能值中确定遍历时的第二个值，最终确定遍历的顺序。
[0051]
进一步，确定离散路径点优劣的衡量规则对离散路径点进行筛选，采用消耗成本衡量每个离散路径点的优劣，具体规则如下：
[0052]
1)依据当前机械臂与障碍物的距离确定当前的状态：
[0053]fobst
(q
c-1
)＞sep1ꢀꢀ
(11)
[0054]
其中，q
c-1
表示前一步的最优路径点，当前为第c步，f
obst
表示在输入角度下，机械臂距离障碍物的最短距离，sep1表示机械臂状态阈值；
[0055]
当机械臂与障碍物间的距离小于设定的阈值时，则当前状态为环绕态，否则为非环绕态；
[0056]
确定搜索到路径点与障碍物的关系：
[0057][0058]
其中，表示当前搜索到的路径点，即当前路径点各个关节角度的集合，sep2表示机器人杆件允许距离障碍物的最小距离；
[0059]
若搜索到的路径点与障碍物间的最小距离大于设定阈值，则将当前路径点标记为可行路径点，进入后续的筛选，否则直接剔除该点，筛选下一个离散路径点；
[0060]
若当前状态为非环绕态，则每个可行路径点的消耗成本由最短距离原则确定：
[0061][0062]
其中，cost表示输入角度下的消耗成本；上式将搜索路径点与最终目标点的距离作为当前路径点的消耗成本。
[0063]
4)若当前状态为环绕态，则每个可行路径点的消耗成分由最短距离原则和最佳避障原则综合确定：
[0064][0065]
其中，k1表示最短路径准则的比重系数，k2表示障碍物距离原则的比重系数。
[0066]
进一步，所述路径回溯，包括如下步骤：
[0067]
当从qc向q
c+1
进行搜索时，若发现无法获得可行路径点，则将当前路径状态从qc回退至q
c-1
状态，排除上次规划找到的最优点，从剩余点中选择最优点，重现向qc搜索。
[0068]
进一步，在所述步骤s4中，
[0069]
采用五次样条曲线生成最终的路径：
[0070]
qi＝ail5+bil4+cil3+dil2+eil+fiꢀꢀ
(15)
[0071]
其中，ai,bi,ci,di,ei,fi表示描述第i个关节角的五次样条曲线的参数，l为形式参数；取第一个关节角q1作为形式参数，便于确定各个点位中，关节角度与形式参数的关系；此时，描述关节角q1的五次样条曲线退化为一维直线，其余关节角度为正常的五次样条曲线；至此，一条完整的机械臂避障路径曲线生成完毕
[0072]
根据本发明实施例的基于局部搜索算法的机械臂自主避障路径生成方法，具有以下有益效果：
[0073]
1、自动搜索可行的轨迹，省去的人工调整路径的繁琐；
[0074]
2、依据与障碍物的位置关系，动态调整搜索步长，降低了计算量，增加了轨迹的平滑度；
[0075]
3、通过预测调整每一步的搜索规则，加快了搜索速度，减少了计算量；
[0076]
4、将每一步的状态分为环绕态和非环绕态，保证了生成轨迹的柔顺度。
[0077]
本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0078]
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0079]
图1为根据本发明实施例的基于局部搜索算法的机械臂自主避障路径生成方法的流程图；
[0080]
图2为根据本发明实施例的基于局部搜索算法的机械臂自主避障路径生成方法的示意图。
具体实施方式
[0081]
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0082]
本发明实施例提供一种基于局部搜索算法的机械臂自主避障路径生成方法，采用局部搜索算法，同时考虑路径的长度与曲率，从机械臂的初始角度进行搜索，每一步从当前路径点搜索出下一步的最优路径点，进而将所有的最优路径点平滑连接，生成满足条件的最优路径。
[0083]
如图1和图2所示，本发明实施例的基于局部搜索算法的机械臂自主避障路径生成方法，包括如下步骤：
[0084]
步骤s1，初始化障碍物相关信息，根据初始化后的信息生成障碍物的边界框。
[0085]
初始化障碍物相关信息，包括：初始化路径起始点、终点、角度极限、障碍物边界信息。
[0086]
具体的，生成障碍物的边界框，包括：选择由圆形和矩形元素描述障碍物的边界框；其中圆形由球心oi(xoi,yoi,zoi)和半径roi组成，矩形由长方体的各个顶点(xci,yci,zci)表示，障碍物的边界框由上述元素组成的圆柱形，球形和多面体进行描述。
[0087]
步骤s2，将当前关节角度一个邻域内的关节点离散化。
[0088]
首先，每个关节角度满足约束：
[0089][0090]
其中，分别表示第i个关节的关节角度，关节角度上限和下限；m表示关节总个数。
[0091]
式子(1)表示每个关节角度都必须处于其限定范围内，是机械臂处于任何姿态，均需要遵守的固有约束。
[0092]
在本发明方法中，依据机械臂的具体状态，对其关节角做出了进一步的约束：
[0093][0094][0095][0096]
其中，分别表示第i个关节第c步关节角的角度，角度上限和下限，c表示从起始开始的步数序号；
[0097]
分别表示第i个关节初始关节角度上限，第c-1步的关节角度上限，关节角极限变化范围。
[0098]
分别表示第i个关节初始关节角度下限，第c-1步的关节角度下限。
[0099]
在此基础上，确定离散关节点时的离散步长：
[0100][0101]
其中，ns表示离散关节点时划分的步数，dq
i,c
表示第i个关节角第c步的离散步长。
[0102]
步骤s3，从当前角度出发，依据预测的关节角度变化律获得下一步的搜索路径点，判断从当前关节角度出发能否获得可行路径点，如果能够获得可行路径点，则从可行路径点中筛选最优路径点，进而判断是否到达终点；如果不能获得可行路径点则进行路径回溯，返回前一个路径点重新进入s2向前搜索。
[0103]
具体的，最优路径点搜索模块负责从初始角度出发，从离散点中选出符合条件的最优路径点，不断搜索前进，最终到达目标点。本发明方法中采用基于预测关节角度变化律的方法，对离散路径点进行快速搜索，搜索出最优路径点。
[0104]
首先，为了减小计算量，加快路径搜索速度，每次搜索只选择距离当前点位一个离散步长的离散点位作为目标路径点。所以，每次搜索时每个关节角度可能的结果为：
[0105][0106]
其中，表示第i个关节角第c步可能的搜索结果，q
i,c-1
表示第i个关节角第c-1步的最优路径点。
[0107]
其次，对于式子(6)所示的三个可能的搜索结果，需要确定搜索规则，便于最快找到最优路径点。
[0108]
搜索规则如下：
[0109]
式子(6)可以进一步地表示为：
[0110][0111]
其中，sn
i,c
为符号标志位，其可能的值为-1，0，1，其中i表示关节角序号，c表示步数序号。
[0112]
此时，每次路径点的搜索过程转化为了sn
i,c
的遍历过程。本发明方法中用采用基于预测关节角度变化律的方法确定sn
i,c
的遍历准则，使得能够优先遍历到最优路径点，加快了搜索速度。遍历规则具体如下：
[0113]
1)第一步时，首先取：
[0114][0115]
其中，表示第i个关节角的目标角度，表示第i个关节角的初始角度，sgn为取正负号函数，表示sn
i,1
的可能取值。
[0116]
依据式子(8)确定sn
i,1
遍历时的首值，剩余两值的遍历顺序依据随机法确定，进而确定sn
i,1
的遍历顺序。
[0117]
2)第二步，第三步时，首先取：
[0118][0119]
同样地，依据式子(9)确定sn
i,c
遍历时的首值，由式子(8)式子确定遍历时的第二个值，若(8)(9)两个式子给出同样的结果，则采用随机法确定遍历时的第二个值，确定遍历顺序。
[0120]
3)当步数大于三时，首先取：
[0121][0122]
同样地，依据式子(10)确定sn
i,c
遍历时的首值。若由式子(10)确定的首值不为0，
则将遍历时的第二个值确定为0，进而确定遍历顺序。若由式子(10)确定的首值为0，则由式子(9)确定遍历时的第二个值，若式子(9)确定的值也为0，则采用式子(8)确定遍历时的第二个值，若式子(8)确定的值也为0，则采用随机法从剩余的两个可能值中确定遍历时的第二个值，最终确定遍历的顺序。
[0123]
上述搜索规则预测了轨迹的变化方向，保证了以最快的速度搜索到最优路径点。
[0124]
在此基础上，确定离散路径点优劣的衡量规则便可对离散路径点进行筛选。
[0125]
本发明方法中采用消耗成本衡量每个离散路径点的优劣，具体规则如下：
[0126]
1)依据当前机械臂与障碍物的距离确定当前的状态：
[0127]fobst
(q
c-1
)＞sep1ꢀꢀ
(11)
[0128]
其中，q
c-1
表示前一步的最优路径点(当前为第c步)，f
obst
表示在输入角度下，机械臂距离障碍物的最短距离，sep1表示机械臂状态阈值。
[0129]
当机械臂与障碍物间的距离小于设定的阈值时，则当前状态为环绕态，否则为非环绕态。
[0130]
2)确定搜索到路径点与障碍物的关系：
[0131][0132]
其中，表示当前搜索到的路径点(即当前路径点各个关节角度的集合)，sep2表示机器人杆件允许距离障碍物的最小距离。
[0133]
若搜索到的路径点与障碍物间的最小距离大于设定阈值，则将当前路径点标记为可行路径点，进入后续的筛选，否则直接剔除该点，筛选下一个离散路径点。
[0134]
3)若当前状态为非环绕态，则每个可行路径点的消耗成本由最短距离原则确定：
[0135][0136]
其中，cost表示输入角度下的消耗成本。
[0137]
上式将搜索路径点与最终目标点的距离作为当前路径点的消耗成本。
[0138]
4)若当前状态为环绕态，则每个可行路径点的消耗成分由最短距离原则和最佳避障原则综合确定：
[0139][0140]
其中，k1表示最短路径准则的比重系数，
[0141]
k2表示障碍物距离原则的比重系数。
[0142]
该方法即不会因为搜寻最短路径，导致机械臂与障碍物距离过近，也不会因为搜寻最佳避障路径，导致路径过长。
[0143]
在上述步长的设定算法中，最短路径原则减小了路径的长度，提高了机械臂的工作效率。通过设置合理的环绕区阈值sep1和障碍物距离原则的比重系数k2能够保证在机器人绕过障碍物时，路径的曲率保持在较低的程度，提高了机械臂的运行速度。
[0144]
当采用前述算法进行搜索时，当满足收敛条件，或者在非环绕态直接判定当前点为最短路径点时，可将当前搜索点设为最终路径点。当难以判定最终路径点时，从所有符合条件的搜索点中选择成本最小的搜索点作为最终的路径点。
[0145]
cost(qc)＜＝sep
cost
ꢀꢀ
(15)
[0146]
其中，sep
cost
表示成本消耗阈值。
[0147]
为了避免搜索算法由于某个局部最优值的影响，未能找到全局最优值，采用路径回溯解决该问题。路径回溯过程，包括如下步骤：
[0148]
当从qc向q
c+1
进行搜索时，若发现无法获得可行路径点，则将当前路径状态从qc回退至q
c-1
状态，排除上次规划找到的最优点，从剩余点中选择最优点，重现向qc搜索。为了节省计算量，在最优路径点搜索模块中需要添加次优点寄存器，将最近两个节点搜索过程中遇到的次优点保存下来，以便路径回溯时直接使用。
[0149]
步骤s4，如果判断到达终点，则将所有最优路径点平滑连接起来，生成最终的平滑路径，如果判断没有到达终点，则前进一步返回步骤s2，搜索下一个路径点。
[0150]
本发明方法中采用五次样条曲线生成最终的路径：
[0151]
qi＝ail5+bil4+cil3+dil2+eil+fiꢀꢀ
(16)
[0152]
其中，ai,bi,ci,di,ei,fi表示描述第i个关节角的五次样条曲线的参数，l为形式参数。
[0153]
在这里取第一个关节角q1作为形式参数，便于确定各个点位中，关节角度与形式参数的关系。
[0154]
此时，描述关节角q1的五次样条曲线退化为一维直线，其余关节角度为正常的五次样条曲线。至此，一条完整的机械臂避障路径曲线生成完毕。
[0155]
根据本发明实施例的基于局部搜索算法的机械臂自主避障路径生成方法，具有以下有益效果：
[0156]
1、自动搜索可行的轨迹，省去的人工调整路径的繁琐；
[0157]
2、依据与障碍物的位置关系，动态调整搜索步长，降低了计算量，增加了轨迹的平滑度；
[0158]
3、通过预测调整每一步的搜索规则，加快了搜索速度，减少了计算量；
[0159]
4、将每一步的状态分为环绕态和非环绕态，保证了生成轨迹的柔顺度。
[0160]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0161]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求及其等同限定。