CN115358681A - 一种静态障碍物下室内多任务点路径规划方法 - Google Patents

Abstract

本发明属于物流机器人领域，尤其涉及一种静态障碍物下室内多任务点路径规划方法，包括以下步骤：S1、采集静态障碍地图，标注生成兴趣点集合M；S2、采用Floyd算法，生成兴趣点拓扑连接序列表；S3、输入兴趣点拓扑连接序列表；S4、输入任务点集合F；S5、查询兴趣点集合X；S6、采用模拟退火算法求解兴趣点集合X中的兴趣点，得到兴趣点的有效排列Q(X)；S7、组合求解出的兴趣点连接关系得到多任务最优组合序列。本发明有效解决了静态障碍地图复杂度对规划效率的影响，提高了规划效率以及多任务最优路径稳定性，从而提高了仓库搬运机器人的运送效率。

Description

一种静态障碍物下室内多任务点路径规划方法

技术领域

本发明属于物流机器人领域，尤其涉及一种静态障碍物下室内多任务点路径规划方法。

背景技术

静态障碍物下室内机器人多任务路径规划方法是指：机器人从等待区领取多个任务点，机器人对所要走的路径进行选择，该路径必须能够经过领取到的所有的任务点并在任务点执行任务，同时避开静态环境下的障碍物，最后在完成所有任务后回到等待区。路径选择的目标是要使求得的路径路程为所有路径之中的最小值并且避开静态障碍物。

目前机器人多任务规划方法主要有如下三种：

（1）人工神经网络：将多任务问题理解为组合优化问题，通过构造能量函数，建立神经元，与任务节点数据集G建立联系，经过神经网络演化、迭代，逐渐趋于稳定状态，输出途经多任务点的路径；

（2）动态规划法+可视图算法：使用环境地图模型，在所有任务点的点集之中，两两任务点间使用可视图算法，在环境地图模型中寻找到两点间最短路径，最短路径路程用任务点之间的代价矩阵表示，假设从顶点任务节点出发，逐级将多任务路径最优问题分解成多个子问题，通过先求解子问题，再由这些子问题的解，最终得到全局多任务路径最优解。

（3）遗传算法+Dijkstra算法（狄克斯特拉算法）：使用栅格法构建障碍地图模型，通过遗传算法获取到任务点序列，通过Dijkstra算法在栅格障碍地图模型中计算任务序列个体中任务点之间在栅格地图中避障最短路径，通过迭代多次的选优、交叉、变异，得到途经多个任务点并且避开静态障碍物的最短路径组合，输出最优路径。

上述三种方法中存在的不足分别是：

（1）人工神经网络目前存在的问题：一是获得的多任务避障路径不是很理想，训练时间及次数对结果的质量有很大的影响，加大训练时间及次数会影响效率；二是存在不稳定性，在算法运算过程中会得到无效路径。

（2）动态规划法+可视图算法目前存在的问题：一是动态规划法能够稳定输出最短任务序列，但是耗费时间较大；二是动态规划法可能会陷入局部最优解；三是可视图法算法的复杂度与障碍物的数量以及障碍物形状成正比，影响规划难度。

（3）遗传算法+Dijkstra算法存在的弊端：一是遗传算法中交叉算法的设计极易影响结果，导致相同任务输出结果不一致；二是遗传算法选择时，当较优个体数值过大时，会导致后期慢，陷入局部最优，整体时间过长；三是输出结果仅是相对最优而非全局最优；四是Dijkstra算法中栅格地图模型地图可规划区域较大时，容易导致Dijkstra算法搜索范围增大从而影响效率。

发明内容

根据以上现有技术中的不足，发明提供了一种提高了规划效率以及多任务最优路径稳定性，从而提高了仓库搬运机器人的运送效率的静态障碍物下室内多任务点路径规划方法。

本发明所述的静态障碍物下室内多任务点路径规划方法，包括以下步骤：

S1、采集静态障碍地图，标注生成兴趣点集合M={1，2，……，m}，编辑兴趣点集合M中兴趣点之间的连接关系；

S2、采用Floyd算法，利用多源最短路径算法计算兴趣点集合M中某一兴趣点i到其余兴趣点集合K中任一兴趣点可联通的路径距离最优以及次优解，生成兴趣点拓扑连接序列表，以id序列和序列表路径距离组成，其中，兴趣点集合K是指除兴趣点i以外的兴趣点的集合；

S3、输入兴趣点拓扑连接序列表；

S4、输入任务点集合F={f0，f1，f2，f3……fn}，其中机器人位置为起点f0；

S5、查询兴趣点集合X={x0，x1，x2，x3……xn}，其中兴趣点集合X为兴趣点集合M中与任务点集合F中的任务点一一对应的兴趣点的集合；

S6、采用模拟退火算法求解兴趣点集合X中的兴趣点，得到兴趣点的有效排列Q(X)={v1，v2……vn}；

S7、组合求解出的兴趣点连接关系得到多任务最优组合序列。

其中，Floyd算法又称为插点法，是一种利用动态规划的思想寻找给定的加权图中多源点之间最短路径的算法，也称为多源最短路径算法。

所述的S2中，Floyd算法生成兴趣点拓扑连接序列表的具体步骤为：

S21、通过拓扑地图表，获取兴趣点的相邻连接点关系，建立邻接矩阵dist储存路径，对相连两点之间的距离进行标记，如果两点没有直接相连则标记为无穷大；

S22、将第1个兴趣点到第n个兴趣点依次加入拓扑地图表中，在每个兴趣点加入时试探是否有路径长度被更改；

S23、遍历拓扑地图表中每一个兴趣点，用i，j表示其中两点，判断i，j两点之间距离是否因为新加入的点z而发生最小距离变化，如果状态发生改变，则更新i，j两点之间距离；

S24、重复上述步骤S21-S23直到最后加入兴趣点试探完成；

S25、输出最短路径和次优路径的兴趣点联通序列，同时最终状态代表点与点之间的最短路径，建立兴趣点拓扑连接序列表。

其中S23中，状态发生改变的更新方法的方程为：

dp[i][j]=min(dp[i][j]，dp[i][k]+dp[k][j])，其中dp[i][j]为i到j的最短路径，dp[i][k]为i到k的最短路径，dp[k][j]为k到j的最短路径。

所述的S6中，模拟退火算法包括以下步骤：

S31、设置的主要控制参数包括：快速降温速率q1，快速降温高温阈值G，二次精准降温速率q2、初始温度T0、当前温度为T，结束温度Td、精度值K、计数值count以及链长L；

S32、查询任务点对应的兴趣点序列，随机生成序列初始解Y1，计数值count设置为0，精度值K设置为1；

S33、通过对当前解Y1进行变异，对构成新解的元素进行交换、位移、置换、倒置的多重方式变异，并用二零域变换法产生新的序列组合新解Y2；

S34、建立Metropolis准则，若路径长度函数为f(Y)，则当前解的路径长度为f(Y1)，新解的路径长度为f(Y2)，路径差为df=f(Y2)-f(Y1)，则接受新解路径序列概率准则为：

；

S35、精度值K加1，判断K是否大于L，是则进行降温，否则返回S33继续迭代；

S36、根据当前温度T是否大于快速降温高温阈值G，判断是否需要快速低精度降温，是则采用快速降温速率q1进行降温，即T=q1*T，继续迭代执行二次精准降温；否则采用二次精准降温速率q2进行降温，即T=q2*T，同时计数值count加1；

S37、若T小于结束温度Td，则停止迭代输出当前状态，否则返回S32继续迭代；

S38、得到有效排列Q(X)={v1，v2……vn}。

所述的S7中的最优组合序列的具体步骤为：通过离线兴趣点连接拓扑表查询获取N个任务点之间的距离，计算有效排列Q(X)={v1，v2……vn}并回到起点，使得：

取得最小值，即全局多任务组合路径最短，其中

表示兴趣点i到兴趣点i+1的路径距离，

表示兴趣点vn到兴趣点v1的路径距离。

本发明所具有的有益效果是：本发明设置了兴趣点拓扑连接序列表，通过兴趣点拓扑宏观描述障碍地图中有效通行区域，有效解决了静态障碍地图复杂度对规划效率的影响，同时也能满足静态障碍物地图中避障问题；使用模拟退火法利用拓扑关系求得多点规划最优解，提高了规划效率以及多任务最优路径稳定性，从而提高了仓库搬运机器人的运送效率。

附图说明

图1为本发明的步骤流程图；

图2为实施例1的静态障碍地图；

图3为本发明的模拟退火算法设计流程图；

图4为实施例2的兴趣点拓扑连接关系图；

图5为实施例2的相邻连接点关系初始化状态表；

图6为实施例2的兴趣点拓扑连接序列表。

具体实施方式

下面结合附图对本发明的实施例做进一步描述：

实施例1：

如图1-图3所示，一种静态障碍物下室内多任务点路径规划方法，包括以下步骤：

S1、采集静态障碍地图，标注生成兴趣点集合M={1，2，……，18}，编辑兴趣点集合M中兴趣点之间的连接关系，其中五边形为搬运机器人，圆形为兴趣点1-18，六边形为任务点F1-F4，如图2所示；

S2、采用Floyd算法，利用多源最短路径算法计算兴趣点集合M中某一兴趣点i到其余兴趣点集合K中任一兴趣点可联通的路径距离最优以及次优解，生成兴趣点拓扑连接序列表，以id序列和序列表路径距离组成，其中，兴趣点集合K是指除兴趣点i以外的兴趣点的集合；

S3、输入兴趣点拓扑连接序列表；

S4、输入任务点集合F={f0,f1,f2,f3,f4}，其中机器人位置为起点f0；

S5、查询兴趣点集合X={1，3，16，6，7}，对应F={f0,f1,f2,f3,f4}；

S6、采用模拟退火算法求解兴趣点集合X={1，3，16，6，7}，得到兴趣点的有效排列Q(X)={1，3，6，7，16}；

S7、组合求解出的兴趣点连接关系得到多任务最优组合序列：

R={f0,2,3,f1,4,6,f3,7,f4,7,6,15,16,f2,17,18,11,10,5,1,f0}。

R即为搬运机器人的最优路径，沿R中的兴趣点依次行走即可完成所有任务并回到起点。

S2中，Floyd算法生成兴趣点拓扑连接序列表的具体步骤为：

S21、通过拓扑地图表，获取兴趣点的相邻连接点关系，建立邻接矩阵dist储存路径，对相连两点之间的距离进行标记，如果两点没有直接相连则标记为无穷大；

S22、将第1个兴趣点到第n个兴趣点依次加入拓扑地图表中，在每个兴趣点加入时试探是否有路径长度被更改；

S23、遍历拓扑地图表中每一个兴趣点，用i，j表示其中两点，判断i，j两点之间距离是否因为新加入的点z而发生最小距离变化，如果状态发生改变，则更新i，j两点之间距离，状态发生改变的更新方法的方程为：

dp[i][j]=min(dp[i][j]，dp[i][z]+dp[z][j])，其中dp[i][j]为i到j的最短路径，dp[i][z]为i到z的最短路径，dp[z][j]为z到j的最短路径；

S24、重复上述步骤S21-S23直到最后加入兴趣点试探完成；

S25、输出最短路径和次优路径的兴趣点联通序列，同时最终状态代表点与点之间的最短路径，建立兴趣点拓扑连接序列表。

S6中，模拟退火算法包括以下步骤：

S31、设置的主要控制参数包括：快速降温速率q1，快速降温高温阈值G，二次精准降温速率q2、初始温度T0、当前温度为T，结束温度Td、精度值K、计数值count以及链长L；

S32、查询任务点对应的兴趣点序列，随机生成序列初始解Y1，计数值count设置为0，精度值K设置为1；

S33、通过对当前解Y1进行变异，对构成新解的元素进行交换、位移、置换、倒置的多重方式变异，并用二零域变换法产生新的序列组合新解Y2；

S34、建立Metropolis准则，若路径长度函数为f(Y)，则当前解的路径长度为f(Y1)，新解的路径长度为f(Y2)，路径差为df=f(Y2)-f(Y1)，则接受新解路径序列概率准则为：

；

S35、精度值K加1，判断K是否大于L，是则进行降温，否则返回S33继续迭代；

S36、根据当前温度T是否大于快速降温高温阈值G，判断是否需要快速低精度降温，是则采用快速降温速率q1进行降温，即T=q1*T，继续迭代执行二次精准降温；否则采用二次精准降温速率q2进行降温，即T=q2*T，同时计数值count加1；

S37、若T小于结束温度Td，则停止迭代输出当前状态，否则返回S32继续迭代；

S38、得到有效排列Q(X)，该序列可使得：

取得最小值，即全局多任务组合路径最短，其中

表示兴趣点i到兴趣点i+1的路径距离，

表示兴趣点vn到兴趣点v1的路径距离。

模拟退火算法因为其大量的迭代次数以及概率突跳特性，拥有强大的全局搜索能力。但同时也需要大量的时间来完成算法的搜索过程。为了提升算法的效率，目前对模拟退火算法的改进方案大多是通过增加算子来达到提高算法最优解精度的目的，而对于时间效率的考虑较少。本申请对模拟退火算法的过程进行了探索，针对提高收敛速度的要求，提出了以下改进措施。

（1）快速低精度降温

针对退火策略提出快速低精度降温，每一个温度都有能量值。在某温度下的若干次迭代中，迭代成功的次数越多，则该温度的能量值越大。给定一个能量标准值，当能量值超过设定的能量标准值，则进行幅度较大的降温，将温度快速降低到低精度范围内。在算法使用快速低精度降温后，相比没有使用快速低精度降温的算法，数据迭代处理的次数大大减少。

（2）二次精准退火

快速低精度降温可以实现快速大幅度降温效果，由于降温幅度较大只能得到一个低精度范围内的解；在大幅度降温的过程中步长较大可能会将期望的最小临近温度跳过，从而遗漏掉最优解。为了尽量消除这种情况带来的影响，算法采用二次精准退火：

1）利用第一次快速低精度降温退火产生一个较优的解，然后把温度初始化；

2）开始进行第二次精准退火，以第一次退火结果作为第二次退火的初始解，重新回到高温状态下去迭代；

3）由于初始解的精确度已经收敛至接近高精确度的邻域范围，因此退火过程中整体的迭代成功率下降，更容易搜寻到最优解，产生一个最终解。算法使用跳跃式降温与二重退火的改进后，在保证了结果的精确性的基础上又减少了数据迭代处理次数。

（3）多重路径变异法

路径变异方式的多样性影响到全局解是否会陷入到局部最优。在产生新解的方式，如果只用一种路径的变异方式进行组合，会使模拟退火算法陷入局部最优，而不是全局最优解。采用以一定概率接受较差解，同时多增加交换、移位、倒置多种路径的变异方式，跳出局部最优解，新路径的产生就能从多个方向去进行，更大的可能去逼近全局最优解。

实施例2：

如图4所示，以兴趣点集合M有5个兴趣点{1，2，3，4，5}为例，说明Floyd算法的具体步骤。

兴趣点元素包含地图位置坐标、兴趣点id、关联id和点属性，拓扑地图如表1。

表1 拓扑地图表

Id	A(m)	B(m)	连接id	连接点距离(m)	点属性
						1	A1	B1	2,3,5	5,6,8	1
2	A2	B2	1,3	5,7	0
						3	A3	B3	1,2,5	6,7,10	0
4	A4	B4	5	5	0
						5	A5	B5	1,3,4	8,10,5	0

其中，A为横轴坐标，B为纵轴坐标，点属性1为起点，0为非起点。

图4中，箭头线上的数字即为相连接的两点之间的距离。

S21、通过拓扑地图表，获取兴趣点的相邻连接点关系，建立邻接矩阵dist储存路径，对相连两点之间的距离进行标记，如果两点没有直接相连则标记为无穷大；

具体如图5所示。其中，A1B2格的数字5代表点1和点2之间的距离为5，而A1B3的无穷大符号则表示点1与点3没有直接相连，以此类推。

S22、将第1个兴趣点到第n个兴趣点依次加入拓扑地图表中，在每个兴趣点加入时试探是否有路径长度被更改；

S23、遍历拓扑地图表中每一个兴趣点，用i，j表示其中两点，判断i，j两点之间距离是否因为新加入的点z而发生最小距离变化，如果状态发生改变，则更新i，j两点之间距离；

S24、重复上述步骤S21-S23直到最后加入兴趣点试探完成；

例如，初始时只有兴趣点1；加入兴趣点2后，两点之间距离为5；加入兴趣点3后，i、j分别代表点1、点2，兴趣点3为z，z的加入并没有改变i、j之间的距离，因此i、j两点之间距离无需更新。

再加入兴趣点4后，i、j则可以是兴趣点1、2、3中任意两点的组合，即{1,2}、{1,3}和{2,3}，z为兴趣点4，此时再分别对i、j之间的距离进行判断。以此类推。

S25、输出最短路径和次优路径的兴趣点联通序列，同时最终状态代表点与点之间的最短路径，建立兴趣点拓扑连接序列表。最终输出的兴趣点拓扑连接序列表如图6。

图6中，对应的坐标数值即为两点之间的最短路径距离。

输出的兴趣点联通序列为：

[{“id”：1“A”:a1“B”:b1“group”:

[

{sg：1,1 list:1,1 Dis:dis1},

{sg：1,2 list:1,2 Dis:dis2},

{sg：1,3 list:1,2,3 Dis:dis3},

{sg：1,4 list:1,5,4 Dis:dis4},

{sg：1,5 list:1,5 Dis:dis5}

]

}]

其中，以{sg：1,4 list:1,5,4 Dis:dis4}行为例，1,4即为兴趣点1和兴趣点4，list:1,5,4即为两点之间的最短路径为点1→点5→点4。

Claims (5)

1.一种静态障碍物下室内多任务点路径规划方法，其特征在于包括以下步骤：

S1、采集静态障碍地图，标注生成兴趣点集合M={1，2，……，m}，编辑兴趣点集合M中兴趣点之间的连接关系；

S2、采用Floyd算法，利用多源最短路径算法计算兴趣点集合M中某一兴趣点i到其余兴趣点集合K中任一兴趣点可联通的路径距离最优以及次优解，生成兴趣点拓扑连接序列表，以id序列和序列表路径距离组成，其中，兴趣点集合K是指除兴趣点i以外的兴趣点的集合；

S3、输入兴趣点拓扑连接序列表；

S4、输入任务点集合F={f0，f1，f2，f3……fn}，其中机器人位置为起点f0；

S5、查询兴趣点集合X={x0，x1，x2，x3……xn}，其中兴趣点集合X为兴趣点集合M中与任务点集合F中的任务点一一对应的兴趣点的集合；

S6、采用模拟退火算法求解兴趣点集合X中的兴趣点，得到兴趣点的有效排列Q(X)={v1，v2……vn}；

S7、组合求解出的兴趣点连接关系得到多任务最优组合序列。

2.根据权利要求1所述的一种静态障碍物下室内多任务点路径规划方法，其特征在于：所述的S2中，Floyd算法生成兴趣点拓扑连接序列表的具体步骤为：

S21、通过拓扑地图表，获取兴趣点的相邻连接点关系，建立邻接矩阵dist储存路径，对相连两点之间的距离进行标记，如果两点没有直接相连则标记为无穷大；

S22、将第1个兴趣点到第n个兴趣点依次加入拓扑地图表中，在每个兴趣点加入时试探是否有路径长度被更改；

S23、遍历拓扑地图表中每一个兴趣点，用i，j表示其中两点，判断i，j两点之间距离是否因为新加入的点z而发生最小距离变化，如果状态发生改变，则更新i，j两点之间距离；

S24、重复上述步骤S21-S23直到最后加入兴趣点试探完成；

S25、输出最短路径和次优路径的兴趣点联通序列，同时最终状态代表点与点之间的最短路径，建立兴趣点拓扑连接序列表。

3.根据权利要求2所述的一种静态障碍物下室内多任务点路径规划方法，其特征在于：所述的S23中，状态发生改变的更新方法的方程为：

dp[i][j]=min(dp[i][j]，dp[i][z]+dp[z][j])，其中dp[i][j]为i到j的最短路径，dp[i][z]为i到z的最短路径，dp[z][j]为z到j的最短路径。

4.根据权利要求1所述的一种静态障碍物下室内多任务点路径规划方法，其特征在于：所述的S6中，模拟退火算法包括以下步骤：

S31、设置的主要控制参数包括：快速降温速率q1，快速降温高温阈值G，二次精准降温速率q2、初始温度T0、当前温度为T，结束温度Td、精度值K、计数值count以及链长L；

S32、查询任务点对应的兴趣点序列，随机生成序列初始解Y1，计数值count设置为0，精度值K设置为1；

S33、通过对当前解Y1进行变异，对构成新解的元素进行交换、位移、置换、倒置的多重方式变异，并用二零域变换法产生新的序列组合新解Y2；

S34、建立Metropolis准则，若路径长度函数为f(Y)，则当前解的路径长度为f(Y1)，新解的路径长度为f(Y2)，路径差为df=f(Y2)-f(Y1)，则接受新解路径序列概率准则为：

；

S35、精度值K加1，判断K是否大于L，是则进行降温，否则返回S33继续迭代；

S36、根据当前温度T是否大于快速降温高温阈值G，判断是否需要快速低精度降温，是则采用快速降温速率q1进行降温，即T=q1*T，继续迭代执行二次精准降温；否则采用二次精准降温速率q2进行降温，即T=q2*T，同时计数值count加1；

S37、若T小于结束温度Td，则停止迭代输出当前状态，否则返回S32继续迭代；

S38、得到有效排列Q(X)={v1，v2……vn}。

5.根据权利要求1所述的一种静态障碍物下室内多任务点路径规划方法，其特征在于：所述的S7中的最优组合序列的具体步骤为：通过离线兴趣点连接拓扑表查询获取N个任务点之间的距离，计算有效排列Q(X)={v1，v2……vn}并回到起点，使得：

取得最小值，即全局多任务组合路径最短，其中

表示兴趣点i到兴趣点i+1的路径距离，

表示兴趣点vn到兴趣点v1的路径距离。

Images