多机器人多目标实时动态避障方法、系统及存储介质与流程

1.本发明涉及一种多机器人多目标实时动态避障方法、系统及存储介质，属于移动机器人技术领域。


背景技术：

2.当前移动机器人路径规划技术迅速发展，在移动机器人领域占据重要位置。目前移动机器人在路径规划中，使用dwa(dwa动态窗口法)动态窗口法在距离目标点距离很近的情况下，由于周围距离障碍物或者与其他机器人的距离较近、机器人加速度和旋转加速度的限制使得评价出来的路径难以到达终点，在终点周围徘徊。另外，一些移动机器人系统很难达到多机器人多目标实时动态避障的要求。


技术实现要素：

3.本发明所要解决的技术问题是：在多机器人条件下，机器人如何实时检测障碍物，并避开障碍物。
4.为解决上述技术问题，本发明提供一种多机器人多目标实时动态避障方法，包括以下步骤：
5.一种多机器人多目标实时动态避障方法，包括以下步骤：
6.步骤1：建立移动机器人运动学模型；
7.步骤2：进行目标终点的最优分配；
8.步骤3：规划每个移动机器人的最优路径；
9.步骤4：根据最优路径给出的移动机器人速度、角速度控制量，控制机器人行走。
10.一种多机器人多目标实时动态避障系统，包括以下程序模块：
11.模型模块：建立移动机器人运动学模型；
12.分配模块：进行目标终点的最优分配；
13.最优路径模块：规划每个移动机器人的最优路径；
14.控制模块：根据最优路径给出的移动机器人速度、角速度控制量，控制机器人行走。
15.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现如上述一种多机器人多目标实时动态避障方法。
16.本发明所达到的有益效果：本发明方法的核心是为多个机器人分配最优目标，然后在动态窗口法中评价函数添加一项终点距离评价函数，距离终点的距离乘以距离因子，当机器人距离目标在一定的范围内则改变该距离因子，再根据改进的动态窗口法实时评价出局部的路径，同时计算每个机器人到障碍物的距离和各个机器人的距离，实现多机器人多目标实时动态避障。
附图说明
17.图1a为单机器人单目标算法改进前效果图；
18.图1b为单机器人单目标算法改进后效果图；
19.图2a为多机器人多目标算法改进前效果图；
20.图2b为多机器人多目标算法改进后效果图。
具体实施方式
21.下面结合附图对本明作进一步的详细说明。
22.实施例1
23.本发明的一种多机器人多目标实时动态避障方法，包括以下步骤：
24.输入：移动机器人提供的角速度w和速度v；
25.输出：每个机器人选择自己的目标终点和经过改进的动态窗口法规划的实时局部路径。
26.步骤1：建立移动机器人运动学模型：
27.首先移动机器人只能前进和旋转，运动学模型如下：
28.x＝x'+v*δt*cos(θ
t
)
29.y＝y'+v*δt*sin(θ
t
)
30.θ
t
＝θ
t
'+w*δt
31.其中，x'和y'为上一时θ
t
'刻坐标，为上一时刻车身角度，x和y为当前时刻坐标，θ
t
为当前时刻车身角度，δt为相邻时刻之间的时间差，θ
t
为当前时刻车身角度，w和v分别为移动机器人角速度和速度；
32.步骤2：进行目标终点的最优分配，具体包括以下步骤：
33.21)计算每个移动机器人到各个目标终点的距离，即索引目标距离；
34.22)将得到的所有索引目标距离由小到大进行排序；
35.23)锁定索引目标距离最短的前n个机器人及对应的目标终点；
36.设置n*n二维矩阵:
[0037][0038]
行表示机器人，列表示目标终点，按以下步骤操作：
[0039]
a)初始化n*n二维矩阵,a
ij
＝0,i表示第个i机器人，j表示第j个目标终点；
[0040][0041]
b)矩阵中包含n2个目标距离，第i个机器人到第j个目标距离最短，则a
ij
所在位置
设为1，a
ij
所在行与列所在位置均设为2；
[0042]
c)然后再选择除a
ij
所在行与列以外，其余目标距离最短的，同样其所在位置设为1，其所在行与列所在位置均设为2；
[0043]
d)再次重复步骤c)n
‑
1次，剩下一个机器人；
[0044]
e)最后一个机器人对应目标终点没有选择，所在位置设为0。
[0045]
下面以4个机器人为例进行说明：
[0046]
a)初始化4*4零矩阵
[0047][0048]
b)每个机器人到每个目标有16个目标距离，然后第二个机器人到第三个目标距离最短，其所在位置为1，所在行列所在位置均设置为2；
[0049][0050]
c)然后再选择其余9个值中目标距离最短的，是第三个机器人到第一个目标距离，其所在位置为1，所在行列所在位置均设置为2；
[0051][0052]
d)然后再选择其余4个值中目标距离最短的，第一个机器人到第二个目标距离，其所在位置为1，所在行列都设置为2；
[0053][0054]
e)最后一个机器人对应目标终点没有选择，将其所在位置设为0。
[0055]
24)对锁定好的目标终点根据机器人序号排序；
[0056]
25)交换目标终点：检查当前索引目标距离最长的机器人到目标终点是否为最长距离，如果不是，则所述机器人与锁定的最长索引目标距离的机器人交换目标终点；
[0057]
步骤3：规划每个移动机器人的最优路径，具体包括以下步骤：
[0058]
31)设定速度限制条件：
[0059]
a)移动机器人受自身最大速度和最小速度的限制：
[0060]
v
m
＝{v∈[v
min
,v
max
],w∈[w
min
,w
max
]}
[0061]
其中，v
m
为移动机器人速度，v
max
为移动机器人最大速度，w
max
为移动机器人最小速度，w
max
为移动机器人最大角速度，w
min
为移动机器人最小角速度；
[0062]
b)移动机器人受电机性能的限制，存在最大的加减速限制：v
d
最大的加减速限制，为电机最大加速度，为最大减速度，为最大角加速度，为最大角减速度；
[0063][0064]
其中v
c
，w
c
是机器人的当前速度和当前角速度；∧表示逻辑与；
[0065]
c)因为移动机器人在碰到障碍物之前应该停止，如果满足当前速度在最大减速度的条件下停止，所行走的距离必须小于当前移动机器人到障碍物的距离，表示为：
[0066]
当v
a
＞0，则s
d
＝s
d
+v
a
*δt，
[0067]
其中，v
a
为当前速度，为最大减速度，s
d
为停止下来的距离；
[0068]
移动机器人角速度和速度能否在碰到障碍物之前停止，并不是在采样一开始就得到的，需要模拟出来机器人轨迹以后，找到障碍物位置，计算出机器人到障碍物之间的距离，然后判断当前采样的移动机器人角速度、速度能否在碰到障碍物之前停止，如果能够停止，那么移动机器人角速度、速度是可接受的，如果不能停止，移动机器人角速度、速度需要舍弃。
[0069]
32)设定评价函数为：
[0070][0071]
其中，方位角评价函数heading(v,w)：用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和与目标之间的角度差距(角度差距为机器人正方向与机器人对准目标之间的角度)；δ，β，γ，分别为方位角评价函数系数，距离评价函数系数，速度评价函数系数，终点距离评价函数系数；
[0072]
距离评价函数dist(v,w)：移动机器人在当前轨迹上与最近的障碍物之间的距离；如果在当前轨迹轨迹上没有障碍物，则将距离评价函数设定为一个常数；
[0073]
速度评价函数velocity(v,w)：评价当前轨迹的速度大小；
[0074]
终点距离评价函数goaldist(v,w)：评价机器人距离目标终点的距离大小；终点距离评价限定一个最大值，如果不设定，到终点距离将太占比重如果机器人到终点的距离大于某个设定值(具体某个值根据实际情况调整)，则设置为该值。
[0075]
33)将各评价函数归一化处理；
[0076]
即对每个评价函数求出的结果除以各项评价函数值的和；
[0077]
34)计算所有评价函数值，选择评价函数值为最高值的路径作为最优路径；
[0078]
确定各个评价函数的系数，计算得出最终评价函数值，选择数值最高对应的路径为最优路径。
[0079]
在matlab软件中，单机器人单目标算法改进前、后效果如图1a、图1b所示，其中，圆形或椭圆形为障碍物，扫帚形状物为机器人所在位置模拟局部路径，到达终点，路径明显变短，曲线为机器人走过的路径，可以看到改进后，距离终点一定距离时，机器人更快到达终点；
[0080]
步骤4：根据最优路径给出的移动机器人速度、角速度控制量，控制机器人行走，则多个机器人都能够顺利避障快速到达终点。
[0081]
在matlab软件中，多机器人多目标算法改进前、后效果如图2a和图2b所示。如图2a改进前，四个机器人受到彼此之间距离影响较大，难以到达终点；如图2b改进后，四个机器人受到机器人到目标点距离的影响，很容易就到达终点。
[0082]
以上是本发明的一个具体实施方案，使用任何符合运动学模型的移动机器人均可使用上述方法实现多机器人多目标实时动态避障。
[0083]
一种多机器人多目标实时动态避障系统，包括以下程序模块：
[0084]
模型模块：建立移动机器人运动学模型；
[0085]
分配模块：进行目标终点的最优分配；
[0086]
最优路径模块：规划每个移动机器人的最优路径；
[0087]
控制模块：根据最优路径给出的移动机器人速度、角速度控制量，控制机器人行走。
[0088]
一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现如上述一种多机器人多目标实时动态避障方法。
[0089]
需要明确的是，本发明并不局限于上文所描述并在图中示出的特定配置和处理。为了简明起见，这里省略了对已知方法的详细描述。在上述实施例中，描述和示出了若干具体的步骤作为示例。但是，本发明的方法过程并不限于所描述和示出的具体步骤，本领域的技术人员可以在领会本发明的精神后，作出各种改变、修改和添加，或者改变步骤之间的顺序。
[0090]
以上所述的结构框图中所示的功能块可以实现为硬件、软件、固件或者它们的组合。当以硬件方式实现时，其可以例如是电子电路、专用集成电路(asic)、适当的固件、插件、功能卡等等。当以软件方式实现时，本发明的元素是被用于执行所需任务的程序或者代码段。程序或者代码段可以存储在机器可读介质中，或者通过载波中携带的数据信号在传输介质或者通信链路上传送。“机器可读介质”可以包括能够存储或传输信息的任何介质。机器可读介质的例子包括电子电路、半导体存储器设备、rom、闪存、可擦除rom(erom)、软盘、cd
‑
rom、光盘、硬盘、光纤介质、射频(rf)链路，等等。代码段可以经由诸如因特网、内联网等的计算机网络被下载。
[0091]
还需要说明的是，本发明中提及的示例性实施例，基于一系列的步骤或者装置描述一些方法或系统。但是，本发明不局限于上述步骤的顺序，也就是说，可以按照实施例中提及的顺序执行步骤，也可以不同于实施例中的顺序，或者若干步骤同时执行。
[0092]
需要说明的是，本发明中提及的示例性实施例，基于一系列的步骤或者装置描述一些方法或系统。但是，本发明不局限于上述步骤的顺序，也就是说，可以按照实施例中提及的顺序执行步骤，也可以不同于实施例中的顺序，或者若干步骤同时执行。在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。