CN113741416B - 基于改进捕食者猎物模型和dmpc的多机器人全覆盖路径规划方法 - Google Patents

Abstract

一种基于改进捕食者猎物模型和DMPC的多机器人全覆盖路径规划方法，输入地图空间，机器人位置和运动优先级等信息；对地图空间栅格化，并赋予栅格状态属性；随机生成初始参数种群和种群数量；根据周围栅格状态，确定机器人运动模型；进入死区时，利用A*回溯脱离；未进入死区，根据激励函数找到周围激励最大的栅格并移动；引入DMPC方法，根据运动模型，预测机器人的移动序列；利用WOA对参数优化求解寻找最佳的移动序列；通过不断的滚动决策，实现地图的全覆盖。本发明能够得到路径重复率低，总路径长度短的全覆盖路径。

Description

基于改进捕食者猎物模型和DMPC的多机器人全覆盖路径规划 方法

技术领域

本发明属于多机器人全覆盖领域，特别涉及一种基于改进捕食者猎物模型和分布式模型预测控制(Distributed Model Predictive Control，以下简称DMPC)的多机器人全覆盖路径规划方法。

背景技术

机器人的全覆盖路径规划(Coverage Path planning，CPP)已经广泛的应用于机器人的作业中，如变电站检测的巡检机器人，锅炉探伤的爬壁机器人，地面清洁的扫地机器人等。机器人的全覆盖路径规划指的是机器人避开障碍物的前提下，规划出一条遍历所有工作空间的路径。单机器人由于自身性能的约束，无法快速覆盖大范围区域，难以处理复杂危险环境，系统的鲁棒性差。多机器人可以通过机器人之间的相互协作比单机器人更快的完成对地图空间的全覆盖，同时多机器人具有内置的冗余性，系统的鲁棒性好，因此研究多机器人的全覆盖方法是有必要的。

由于多机器人协同覆盖同一区域，所以在规划机器人覆盖路径的过程中除了避开障碍物还要避免机器人之间发生碰撞，所以对于多机器人全覆盖路径规划是有必要的。

在实际机器人使用中，大部分情况下机器人往往只能拥有部分或无法拥有环境的先验知识，所以对于未知地图的全覆盖路径规划是有必要的。现有未知地图的全覆盖方法有基于分区的全覆盖方法，基于生物神经网络的全覆盖方法，基于模板和回溯的全覆盖方法等，然而现有方法存在着在规划后期陷入局部最优，回溯区域重复而导致机器人覆盖区域重叠等问题。

发明内容

为了克服现有技术的在规划后期陷入局部最优，回溯区域重复而导致机器人覆盖区域重叠的不足，为了得到路径重复率低，总路径长度短的全覆盖路径，本发明提供一种基于改进捕食者猎物模型和DMPC的多机器人全覆盖路径规划方法。

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

一种基于改进捕食者猎物模型和DMPC的多机器人全覆盖路径规划方法，包括下列步骤：

步骤1：输入地图空间，多机器人的初始位置，机器人的探测半径和机器人的运动速度，机器人运动的优先级；

步骤2：对地图空间栅格化，使得机器人能刚好覆盖一个栅格，并对每个栅格赋予不同的状态属性，将多机器人系统划分为多个子系统，栅格的状态属性I(x,y)和机器人覆盖的任务空间WS分别为：

WS＝{(x,y)|I(x,y)＝1,0＜x＜x_l,0＜y＜y_l} (2)

式中x_l，y_l分别为地图空间离散后x轴的长度和y轴的长度；

步骤3：随机生成初始参数种群种群数量为pop；

步骤4：机器人根据传感器检测到的信息，确定周围栅格的状态属性，从而判断机器人是否陷入死区，确定机器人的运动模型。若陷入死区跳转至步骤5，若为陷入死区跳转至步骤7；

步骤5：从回溯列表中剔除其他回溯点半径r内的栅格坐标，得到搜索列表L_search为：

L_search＝L_return/L_back,r (3)

式中L_return代表回溯列表，L_back,r代表回溯点半径r内的点的集合；

步骤6：从搜索列表中找到距离当前机器人最近的自由栅格坐标，使用A*算法脱离死区，得到机器人的移动序列后跳转至步骤10，机器人的移动序列A_path为：

A_path＝{A₁,A₂,…A_i,…P_return} (4)

式中A_i为A*算法规划出的路径点坐标，P_return为回溯点坐标；

步骤7：根据改进后的捕食者猎物模型计算机器人临近点未覆盖栅格的各激励值大小，计算避开竞争者激励E_compete为：

式中D代表两栅格之间的欧式距离，O_kt,j代表机器人k在t时刻的临近点j的位置，H代表机器人k通讯范围H_max内其他机器人的个数。R_k,i，R_k,m分别代表通讯范围内的第i，m个机器人位置；

步骤8：计算移动方向激励E_direction为：

∠O_k,t-1O_k,tO_kt,j＝|atan2(y_k,j-y_k,t,x_k,j-x_k,t)-atan2(y_k,t-y_k,t-1,x_k,t-x_k,t-1)| (7)

式中∠O_k,t-1O_k,tO_kt,j为机器人当前移动方向于下一步移动方向之间的夹角，(x_k,t-1,y_k,t-1)，(x_k,t,y_k,t)，(x_k,j,y_k,j)分别为机器人k前一时刻，当前时刻和下一时刻的坐标位置；

步骤9：计算边界激励E_b为：

n^N(O_kt,j)代表在机器人k的临近点j处未覆盖的栅格数量，代表允许的临近点最大个数；

步骤10：计算栅格总激励值E为：

E(O_kt,j)＝ω_cE_compete(O_kt,j)+ω_sE_direction(O_kt,j)+E_b(O_kt,j) (9)

式中ω_c，ω_s为权重因子；

步骤11：机器人根据最大激励或者脱离死区序列决定机器人下一步移动坐标，当多个机器人在相同时刻选择同一栅格时，优先级高的机器人优先运动，机器人下一步移动坐标为step_k,t*：

步骤12：引入DMPC方法，根据步骤4-11预测机器人T步路径序列，以覆盖自由栅格数和路径长度为指标，设立评价函数，预测序列y_k(t)和评价函数分别J^(T)为：

y_k(t)＝[y_k(t+1|t),y_k(t+2|t),…y_k(t+T|t)] (11)

式中N_cover代表机器人覆盖的栅格数，L代表路径长度，α，β表示权重系数；

步骤13：通过WOA优化ω_s，迭代次数WOA_iter，以机器人T步的评价函数作为适应度函数，得到机器人未来T步的最佳移动序列L_T：

L_T＝{Step_k,t+1,…Step_k,t+i,…Step_k,t+T} (13)

其中Step_k,t+i代表机器人k在t+i时刻的位置，i＝1,2…T；

步骤14：机器人选择移动序列中最靠前的序列并移动，同时更新栅格地图状态和机器人位置，并将未覆盖的栅格信息加入回溯列表中，删除已覆盖的栅格点；

步骤15：当前迭代次数iter达到最大迭代次数iter_max或时，执行步骤16；否则，iter＝iter+1，返回步骤3；

步骤16：输出路径，结束。

本发明的有益效果主要表现在以下几个方面：

1.以改进后的捕食者猎物作为运动模型，有效的将机器人的回溯点分离，避免了机器人覆盖范围的重叠。覆盖过程中通过边界激励的引导，使得机器人能有效的覆盖边界区域。

2.基于DMPC框架下的滚动决策和WOA优化求解，使得本方法有效的平衡了短期收益和长期收益，避免了局部最优。

3.本发明可以实现对未知地图的全覆盖，在避免障碍物和其他机器人的同时，覆盖率可达100％，并且拥有较低的重复率。

附图说明

图1是机器人运动模型流程图。

图2是多机器人的全覆盖路径规划方法流程图。

图3是地图空间栅格示意图。

图4是仿真实验中多机器人全覆盖路径

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图4，一种基于改进捕食者猎物模型和DMPC的多机器人全覆盖路径规划方法，包括下列步骤：

步骤1：输入地图空间30*30，机器人数量为4，多机器人的初始位置，机器人的探测半径和机器人的运动速度，机器人运动的优先级；

步骤2：对地图空间栅格化，使得机器人能刚好覆盖一个栅格，并对每个栅格赋予不同的状态属性，将多机器人系统划分为多个子系统，栅格的状态属性和机器人覆盖的任务空间分别为：

WS＝{(x,y)|I(x,y)＝1,0＜x＜x_l,0＜y＜y_l} (2)

式中x_l，y_l分别为地图空间离散后x轴的长度和y轴的长度，x_l＝30，y_l＝30；

步骤3：随机生成初始参数种群种群数量pop＝50；

步骤4：机器人根据激光雷达检测到周围环境的信息，确定周围栅格的状态属性，从而判断机器人是否陷入死区，确定机器人的运动模型，若陷入死区跳转至步骤4，若为陷入死区跳转至步骤5；

步骤5：从回溯列表中剔除其他回溯点半径r内的栅格坐标，得到搜索列表L_search：

L_search＝L_return/L_back,r (3)

式中L_return代表回溯列表，L_back,r代表回溯点半径r内的点的集合，其中r＝3；

步骤6：从搜索列表中找到距离当前机器人最近的自由栅格坐标，使用A*算法脱离死区，得到机器人的移动序列后跳转至步骤7，机器人的移动序列A_path为：

A_path＝{A₁,A₂,…A_i,…P_return} (4)

式中A_i为A*算法规划出的路径点坐标；

步骤7：根据改进后的捕食者猎物模型计算机器人临近点未覆盖栅格的各激励值大小，计算避开竞争者激励E_compete为：

式中D代表两栅格之间的欧式距离，O_kt,j代表机器人k在t时刻的临近点j的位置，H代表机器人k通讯范围H_max内其他机器人的个数，H_max＝3。R_k,i，R_k,m分别代表通讯范围内的第i，m个机器人位置；

步骤8：计算移动方向激励E_direction为：

∠O_k,t-1O_k,tO_kt,j＝|a tan 2(y_k,j-y_k,t,x_k,j-x_k,t)-a tan 2(y_k,t-y_k,t-1,x_k,t-x_k,t-1)|(7)

式中∠O_k,t-1O_k,tO_kt,j为机器人当前移动方向于下一步移动方向之间的夹角，(x_k,t-1,y_k,t-1)，(x_k,t,y_k,t)，(x_k,j,y_k,j)分别为机器人k前一时刻，当前时刻和下一时刻的坐标位置；

步骤9：计算边界激励E_b为：

n^N(O_kt,j)代表在机器人k的临近点j处自由栅格数量，代表机器人允许的临近点最大个数，/>

步骤10：计算栅格总激励值E为：

E(O_kt,j)＝ω_cE_compete(O_kt,j)+ω_sE_direction(O_kt,j)+E_b(O_kt,j) (9)

式中ω_c，ω_s为权重因子ω_c＝0.01，ω_s∈[0,1]；

步骤11：机器人根据最大激励或者脱离死区序列决定机器人下一步移动坐标，当多个机器人在相同时刻选择同一栅格时，优先级高的机器人优先运动，移动方式为：

其中A_path,0代表脱离死区路径规划中最靠前的路径点坐标，O_k,j∈N^u(R_k)代表机器人k临近点的集合；

步骤12：引入DMPC方法，根据步骤4-11预测机器人T步路径序列，以覆盖自由栅格数和路径长度为指标，设立评价函数，预测序列y_k(t)和评价函数分别J^(T)为：

y_k(t)＝[y_k(t+1|t),y_k(t+2|t),…y_k(t+T|t)] (11)

式中N_cover代表机器人覆盖的栅格数，L代表路径长度，α，β表示权重系数，α＝1，β＝0.01，T＝3；

步骤13：通过WOA优化初始种群以机器人T步的评价函数为适应度函数，迭代次数为WOA_iter＝20，得到机器人未来T步的最佳移动序列L_T：

L_T＝{Step_k,t,Step_k,t+1,…Step_k,t+T-1} (13)

其中Step_k,t代表机器人k在t时刻的位置；

步骤14：机器人选择移动序列中最靠前的序列并移动，同时更新栅格地图状态和机器人位置，并将自由栅格信息加入回溯列表中，删除已覆盖的栅格点；

步骤15：当前迭代次数iter达到最大迭代次数iter_max或时，执行步骤16，iter_max＝350；否则，iter＝iter+1，返回步骤3；

步骤16：输出各个机器人的路径，结束。

以上详细地描述了本发明的具体实例，但本发明不限于以上实例。本领域的普通技术人员依据本发明现有的构思上通过逻辑推理、分析和其他实验可以得到的所有变形，均应认为是本发明的保护范围。

Claims (1)

1.一种基于改进捕食者猎物模型和DMPC的多机器人全覆盖路径规划方法，其特征在于，所述方法包括下列步骤：

步骤1：输入地图空间，多机器人的初始位置，机器人的探测半径和机器人的运动速度，机器人运动的优先级；

步骤2：对地图空间栅格化，使得机器人能刚好覆盖一个栅格，并对每个栅格赋予不同的状态属性，将多机器人系统划分为多个子系统，栅格的状态属性I(x,y)和机器人覆盖的任务空间WS分别为：

WS＝{(x,y)|I(x,y)＝1,0＜x＜x_l,0＜y＜y_l} (2)

式中x_l，y_l分别为地图空间离散后x轴的长度和y轴的长度；

步骤3：随机生成初始参数种群种群数量为pop；

步骤4：机器人根据传感器检测到的信息，确定周围栅格的状态属性，从而判断机器人是否陷入死区，确定机器人的运动模型，若陷入死区跳转至步骤5，若为陷入死区跳转至步骤7；

步骤5：从回溯列表中剔除其他回溯点半径r内的栅格坐标，得到搜索列表L_search为：

L_search＝L_return/L_back,r (3)

式中L_return代表回溯列表，L_back,r代表回溯点半径r内的点的集合；

步骤6：从搜索列表中找到距离当前机器人最近的自由栅格坐标，使用A*算法脱离死区，得到机器人的移动序列后跳转至步骤10，机器人的移动序列A_path为：

A_path＝{A₁,A₂,…A_i,…P_return} (4)

式中A_i为A*算法规划出的路径点坐标，P_return为回溯点坐标；

步骤7：根据改进后的捕食者猎物模型计算机器人临近点未覆盖栅格的各激励值大小，计算避开竞争者激励E_compete为：

式中D代表两栅格之间的欧式距离，O_kt,j代表机器人k在t时刻的临近点j的位置，H代表机器人k通讯范围H_max内其他机器人的个数，R_k,i，R_k,m分别代表通讯范围内的第i，m个机器人位置；

步骤8：计算移动方向激励E_direction为：

∠O_k,t-1O_k,tO_kt,j＝|a tan 2(y_k,j-y_k,t,x_k,j-x_k,t)-a tan 2(y_k,t-y_k,t-1,x_k,t-x_k,t-1)| (7)

式中∠O_k,t-1O_k,tO_kt,j为机器人当前移动方向于下一步移动方向之间的夹角，(x_k,t-1,y_k,t-1)，(x_k,t,y_k,t)，(x_k,j,y_k,j)分别为机器人k前一时刻，当前时刻和下一时刻的坐标位置；

步骤9：计算边界激励E_b为：

n^N(O_kt,j)代表在机器人k的临近点j处未覆盖的栅格数量，代表允许的临近点最大个数；

步骤10：计算栅格总激励值E为：

E(O_kt,j)＝ω_cE_compete(O_kt,j)+ω_sE_direction(O_kt,j)+E_b(O_kt,j) (9)

式中ω_c，ω_s为权重因子；

步骤11：机器人根据最大激励或者脱离死区序列决定机器人下一步移动坐标，当多个机器人在相同时刻选择同一栅格时，优先级高的机器人优先运动，机器人下一步移动坐标为step_k,t*：

步骤12：引入DMPC方法，根据步骤4-11预测机器人T步路径序列，以覆盖自由栅格数和路径长度为指标，设立评价函数，预测序列y_k(t)和评价函数分别J^(T)为：

y_k(t)＝[y_k(t+1|t),y_k(t+2|t),…y_k(t+T|t)] (11)

式中N_cover代表机器人覆盖的栅格数，L代表路径长度，α，β表示权重系数；

步骤13：通过WOA优化ω_s，迭代次数WOA_iter，以机器人T步的评价函数作为适应度函数，得到机器人未来T步的最佳移动序列L_T：

L_T＝{Step_k,t+1,…Step_k,t+i,…Step_k,t+T} (13)

其中Step_k,t+i代表机器人k在t+i时刻的位置，i＝1,2…T；

步骤14：机器人选择移动序列中最靠前的序列并移动，同时更新栅格地图状态和机器人位置，并将未覆盖的栅格信息加入回溯列表中，删除已覆盖的栅格点；

步骤15：当前迭代次数iter达到最大迭代次数iter_max或时，执行步骤16；否则，iter＝iter+1，返回步骤3；

步骤16：输出路径，结束。