基于扩展视野自适应蚁群算法的移动机器人路径规划方法

1.本发明属于机器人技术领域，具体涉及一种移动机器人路径规划方法。


背景技术：

2.移动机器人是实现智能制造的重要驱动力，已经成为学术界和企业界关注的焦点。其中，路径规划是移动机器人的核心组成部分，其目标是找到从起始点到目标点的无碰撞最优或者近似最优路径。快速、高效、安全的路径规划算法是寻找到最优路径的关键，同时也是实现移动机器人成功应用的理论基础。
3.文献“miao c,chen g,yan c.path planning optimization of indoor mobile robot based on adaptive ant colony algorithm[j].computers&industrial engineering,2021:107230.”提供了一种改进的自适应蚁群算法来克服现有室内移动机器人路径规划算法存在的部分缺点，如路径规划时间长、最优路径收敛速度慢和易陷入局部最优。但是该方法由于搜索范围局限于围绕当前位置的八个备选栅格、自适应信息素后期变化不明显，从而存在蚂蚁短视和信息素挥发系数后期调节乏力的问题，导致算法搜索效率降低，收敛速度慢。


技术实现要素：

[0004]
为了克服现有技术的不足，本发明提供了一种基于扩展视野自适应蚁群算法的移动机器人路径规划方法，该方法通过将蚂蚁当前视野从四个方向进行扩展，扩展后的蚂蚁视野更加广阔，可加快蚂蚁移动速度。同时在计算状态转移概率时，兼顾转角和安全距离两个因素，多因素共同控制算法中的状态转移概率。在轮盘赌选择策略中，采用参数自适应伪随机转移策略，在迭代过程中通过在不同阶段改变概率参数的值可使算法在前期提高收敛速度，在后期提高全局搜索能力。自适应信息素挥发系数在前期值较小可加快算法的全局搜索范围，随着寻找到的路径越来越短，信息素挥发系数变大，可使路径上浓度差变大，增强了蚁群的引导功能，提高了蚁群的搜索速度，使算法快速收敛。
[0005]
本发明解决其技术问题所采用的技术方案包括如下步骤：
[0006]
步骤1：环境建模；
[0007]
构建二维工作空间w，其中障碍物集合所占据的空间为o
obs
＝{o1,o2,o3....ok}，满足1《r《r，w表示障碍物所在的工作空间，r表示工作空间中的障碍物数量；移动机器人的自由空间且给定移动机器人的起始位置s＝(xs,ys)，目标点位置e＝(xe,ye)；设移动机器人的路径工作点p由工作空间中的点序列p
path
＝(p1,p2,...p
l
)组成，其中p
l
＝(x
l
,y
l
)，p
l
表示栅格中每一个栅格的索引值，x
l
,y
l
表示栅格p
l
的中心点在栅格坐标系的坐标值，l＝1,2,...l，p1＝s，p
l
＝e，l表示移动机器人在工作空间路径工作点的总点数；
[0008]
移动机器人在工作空间中两点间的距离为
[0009][0010]
其中，x
l 1
、y
l 1
表示栅格p
l 1
的中心点在栅格坐标系的坐标值；
[0011]
在地图中对每个栅格编序号，取栅格中心位置作为栅格在坐标系中的坐标，第w个栅格在坐标系中的坐标如下：
[0012][0013]
其中，mod为取余运算符，ceil表示向后取整符，nw是对应栅格的标号，n表示每一列的栅格数量；栅格的坐标值均采用栅格中心位置的坐标表示，称为栅格的坐标；
[0014]
步骤2：初始化算法参数；
[0015]
定义最大迭代次数nc
max
，蚂蚁数量m
max
；
[0016]
信息素启发因子α、目标距离启发因子β、转角控制启发因子γ和障碍物安全距离启发因子δ分别表示节点路径信息素含量τ
ij
(t)、目标距离启发信息η
ij
(t)、转角控制启发信息g
ij
(t)和障碍物安全距离启发信息l
ij
(t)对当前蚂蚁选择下一路径点的影响程度，且均为常数；
[0017]
起始点设为s，目标点设为e，初始化信息素矩阵tau，当代最短路径长度min k l＝∞，当前全局最短路径长度min l＝∞，每只蚂蚁的路径长度lengthm＝0，蚂蚁携带信息素总量q＝1；
[0018]
在算法进行初始迭代时，取迭代次数nc＝1，蚂蚁个数m＝1，依次开始迭代；在初始时刻，蚂蚁位于起始点s开始进行路径寻优，当前点位置i＝s，并将起始点s加入禁忌表t aboom，禁忌表中的点不允许在该只蚂蚁后续选择路径点中出现；
[0019]
步骤3：扩展视野；
[0020]
以第i个栅格作为当前位置，以当前位置为中心，从上下左右四个方向开始扩展，直到四个方向都首次与障碍物或者边界接触时停止，形成十字形；扩展出来的十字形上边界、下边界、左边界、右边界距离当前位置的距离分别表示为：d1,d2,d3,d4；令a＝d1 d2,b＝d3 d4，从而得到横向和纵向扩展后的总宽度；
[0021]
(a)若a《b，纵向宽度小，则以纵向宽度为基准进行扩展；当以纵向为基准时，纵向宽度以边界为准保持不变，横向左边界和右边界之间的所有栅格分别向左右两个方向进行扩展，当左右两边分别有任一个栅格扩展后首次与障碍物相接触或者到达边界时停止各自的扩展；扩展完成后，左右两边以扩展到达的栅格条作为边界，上下两边以十字形的上边界和下边界作为边界，形成矩形框，称为视野栅格；
[0022]
(b)若a≥b，横向宽度较小，则以横向宽度为基准进行扩展；当以横向宽度为基准时，横向宽度以边界为准保持不变，纵向上边界和下边界的所有栅格分别向上下两个方向进行扩展，当上下两边分别有任一个栅格扩展后首次与障碍物相接触或者到达边界时停止各自的扩展；扩展完成后，上下两边以扩展到达的栅格条作为边界，左右两边以十字形的左边界和右边界作为边界，形成矩形框，称为视野栅格；
[0023]
步骤3-1：删除视野栅格中含有的障碍物栅格，剩余栅格作为备选栅格；将删除的障碍物后的视野栅格集合定义为d
obsi
；
[0024]
步骤3.2，筛选栅格；
[0025]
将当前位置与每个备选栅格分别连接，如果连线段经过障碍物栅格，则删掉此备选栅格，得到最终蚂蚁在栅格i处进行状态转移的备选栅格集合di；
[0026]
步骤3.3，条件判断，若在栅格i处的可选择栅格allowedi＝d
i-taboom为则转到步骤6；反之，顺序执行后续步骤。
[0027]
步骤4：状态转移参数自适应调整；
[0028]
当前栅格在栅格坐标系中的坐标为(xi,yi)，各备选栅格坐标为(xj,yj),j∈allowedi；由式(1)计算当前位置到每一个备选节点的距离d
ij
以及备选点到目标点的距离d
je
：
[0029][0030]
由此得到当前备选点j的目标距离启发信息η
ij
为：
[0031][0032]
步骤4-1：计算从当前点到各备选点的旋转角度；
[0033]
起始点与目标点的距离d
se
为：
[0034][0035]
以当前点与备选点连线为以起始点与目标点连线为根据余弦定理依次计算每一个向量与之间的夹角θ
ijse
：
[0036][0037]
步骤4-2：计算转角控制启发信息；
[0038]
根据步骤4-1计算得到的旋转角度θ
ijse
，得到转角控制启发信息表达式：
[0039]
a)当θ
ijse
≠180
°
时，转角控制启发信息表达式为：
[0040][0041]
b)当θ
ijse
＝180
°
时，转角控制启发信息表达式为：
[0042]gij
(t)＝0.01
[0043]
步骤4-3：计算备选栅格的障碍物安全距离启发信息
[0044]
步骤4-3-1：根据当前点i的坐标(xi,yi)与备选点j的坐标(xj,yj)得到当前点与备选点连线的直线表达式：
[0045]
(y
i-yj)x (x
j-xi)y xiy
j-xiyj＝0
[0046]
步骤4-3-2：分别求出障碍物栅格集合d
obsi
内各障碍物栅格中心点c对应的坐标值(x
obsi
,y
obsi
)；
[0047]
步骤4-3-3：根据点到直线的距离求出各障碍物栅格中心点k距离直线
ij
的最短直
线距离：
[0048][0049]
步骤4-3-4,：备选栅格的障碍物安全距离启发信息为l
ijobs
(t)＝min(l
ijobs
(c))；
[0050]
步骤4-4：计算状态转移概率；
[0051]
计算蚂蚁m从当前位置i向j点转移的概率为：
[0052][0053]
步骤4-5：根据迭代次数动态调整轮盘赌参数q0的值：
[0054][0055]
式中，δ0为常数；
[0056]
步骤4-6：根据轮盘赌方式选择节点；
[0057]
在得到每个备选节点j的状态转移概率后，根据轮盘赌策略进行节点选择：
[0058][0059]
当随机数q≤q0时，蚂蚁选择备选节点中最大的点，反之，则随机选择一个备选节点，并且移动到下一节点j，节点路径长度路径总长度lengthm←
lengthm length
ij
；
[0060]
步骤4-7：修改禁忌表t aboom；
[0061]
蚂蚁每访问一个节点j就将其加入禁忌表taboom中，禁忌表中的点不允许在该只蚂蚁后续选择路径点中出现；
[0062]
步骤4-8：计算路径占用栅格；
[0063]
计算当前点与备选点连线的斜率
[0064]
(a)若k
ij
＝0，占用栅格坐标求解公式为：
[0065][0066]
(b)若k
ij
不存在时，占用栅格坐标求解公式为：
[0067][0068]
(c)若k
ij
＝1时，占用栅格坐标求解公式为：
[0069][0070]
(d)若1《k
ij
《∞，按照当前点到备选点连线的顺序，计算出连线与各水平栅格的交点，连线占用栅格为与交点相接的两个栅格，若交点为栅格顶点则忽略；若0《k
ij
《1，则计算出连线与各垂直栅格的交点，连线占用栅格为与交点相接的两个栅格，若交点为栅格顶点则忽略；
[0071]
步骤4-9，坐标转换；
[0072]
在得到占用栅格的坐标点后，根据式(2)转换成对应的栅格地图索引值；
[0073]
步骤5：判断蚂蚁是否移动到目标点e：
[0074]
a)若当前蚂蚁未到达目标点e，则重复步骤3～步骤5；
[0075]
b)若当前位置栅格为目标点e，若lengthm《min k l,令min k l＝lengthm，并顺序执行后续步骤；
[0076]
步骤6：判断当前蚂蚁序号m是否为最大蚂蚁数量；若m《m
max
，令m
←
m 1，将禁忌表taboom清零，重复步骤3～步骤6；若m＝m
max
，则顺序执行步骤7；
[0077]
步骤7：调整信息素挥发系数ρ；
[0078]
根据当前迭代次数和当代最佳路径长度调节信息素挥发系数。
[0079][0080]
式中，ρ表示路径上信息素的挥发度，κ是常数；
[0081]
步骤7-1：更新全局信息素；
[0082]
经过h时刻后，整个蚁群完成一次迭代，各条路径上的信息素按照式(3)进行更新：
[0083]
τ
ij
(t h)＝(1-ρ)
·
τ
ij
(t) δτ
ij
(t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0084]
式中，δτ
ij
(t)表示m只蚂蚁在此次循环中残留在各条路径上的信息素量的总和，计算式为每只蚂蚁留在路径上每个占用栅格的信息素量的计算式为：
[0085][0086]
步骤7-2：如果当代最优路径min k l《min l，令全局最优路径min l＝min k l；
[0087]
步骤8：判断当前迭代次数是否达到最大迭代次数；
[0088]
若nc《nc
max
，令迭代次数nc加1，重复步骤3～步骤8；若迭代次数nc＝nc
max
，则算法达到最大允许迭代次数，输出全局最优路径min l，算法终止。
[0089]
优选地，所述最大迭代次数nc
max
＝100，蚂蚁数量m
max
＝50，信息素启发因子α＝2，目标距离启发因子β＝3，转角控制启发因子γ＝1，障碍物安全距离启发因子δ＝2。
[0090]
本发明的有益效果如下：
[0091]
本发明通过将蚁群算法蚂蚁视野进行扩展，扩大每步的搜索范围，从而加快蚂蚁的移动速度，进而提高算法搜索效率；同时在算法中兼顾蚂蚁状态转移时的转角，减少蚂蚁在状态转移时需要旋转的角度，减少了实际应用于机器人时不必要的转角造成的能量损耗。在移动过程中，考虑路线距离最近障碍物的距离，增加移动过程中的安全性。在轮盘赌选择策略中，采用参数自适应伪随机转移策略，在迭代过程中通过在不同阶段改变概率参数的值可使算法在前期提高收敛速度，在后期提高全局搜索能力。同样，自适应信息素挥发系数在前期值较小可加快算法的全局搜索范围，随着寻找到的路径越来越短，信息素挥发系数变大，可使路径上浓度差变大，增强了蚁群的引导功能，提高了蚁群的搜索速度，使算法快速收敛。
附图说明
[0092]
图1为本发明方法总流程图。
[0093]
图2为本发明的m
×
n栅格地图索引方式及坐标表示形式示意图。
[0094]
图3为本发明的扩展视野方式流程图。
[0095]
图4为本发明的转角控制启发因子计算方式流程图。
[0096]
图5为本发明的横向扩展视野方式示意图。
[0097]
图6为本发明的纵向扩展视野方式示意图。
[0098]
图7为本发明的转角控制启发因子示意图。
[0099]
图8为本发明的改进后的bresenham法的占用栅格示意图(1)。
[0100]
图9为本发明的改进后的bresenham法的占用栅格示意图(2)。
具体实施方式
[0101]
下面结合附图和实施例对本发明进一步说明。
[0102]
本文定义机器人的移动准则为:
[0103]
1)机器人只允许在白色方格表示的自由空间移动，不允许穿越黑色方格表示的障碍物，但允许机器人与障碍物方格切角而行。
[0104]
2)运动过程中不允许机器人在同一栅格中重复出现。
[0105]
如图1所示，一种基于扩展视野的移动机器人路径规划方法，包括如下步骤：
[0106]
1、环境建模。
[0107]
如图2所示，本实例中，取栅格地图尺寸为20
×
20。在地图中对每个栅格编序号，取栅格中心位置作为栅格在坐标系中的坐标，不同序号的栅格在坐标系中的坐标如下：
[0108]
[0109]
2、初始化算法参数：取最大迭代次数nc
max
＝100，蚂蚁数量m
max
＝50，信息素启发因子α＝2，目标距离启发因子β＝3，转角控制启发因子γ＝1，障碍物安全距离启发因子δ＝2，起始点s＝1，目标点e＝400，初始化信息素矩阵当代最短路径代数minkl＝∞，当前全局最短路径长度minl＝∞，每只蚂蚁的路径长度lengthm＝0，蚂蚁携带信息素总量q＝1。
[0110]
在算法进行初始迭代时，取迭代次数nc＝1，蚂蚁个数m＝1，依次开始迭代。在初始时刻，蚂蚁为与起始点s开始进行路径寻优，当前点位置i＝s，并将起始点s加入禁忌表taboom，禁忌表中的点不允许在该只蚂蚁后续选择路径点中出现。
[0111]
3、扩展视野。如图3、图5、图6所示，在i点位置进行视野扩展，以当前位置为中心，从上下左右四个方向开始扩展，直到四个方向都首次与障碍物或者边界接触时停止，形成十字形。这样扩展出来的边界从上到下(纵向)从左到右(横向)距离当前位置的距离分别表示为:d1,d2,d3,d4。令a＝d1 d2,b＝d3 d4，从而得到横向和纵向扩展后的总宽度。
[0112]
(a)若a《b，纵向宽度小，则以纵向宽度为基准进行扩展；当以纵向为基准时，纵向宽度以边界为准保持不变，横向左边界和右边界之间的所有栅格分别向左右两个方向进行扩展，当左右两边分别有任一个栅格扩展后首次与障碍物相接触或者到达边界时停止各自的扩展；扩展完成后，左右两边以扩展到达的栅格条作为边界，上下两边以十字形的上边界和下边界作为边界，形成矩形框，称为视野栅格；
[0113]
(b)若a≥b，横向宽度较小，则以横向宽度为基准进行扩展；当以横向宽度为基准时，横向宽度以边界为准保持不变，纵向上边界和下边界的所有栅格分别向上下两个方向进行扩展，当上下两边分别有任一个栅格扩展后首次与障碍物相接触或者到达边界时停止各自的扩展；扩展完成后，上下两边以扩展到达的栅格条作为边界，左右两边以十字形的左边界和右边界作为边界，形成矩形框，称为视野栅格；
[0114]
3.1、扩展后的视野栅格。扩展后的视野栅格由四条边界栅格条围成的矩形框组成，视野栅格中包含的障碍物栅格集合为d
obsi
，删除视野栅格中的障碍物栅格，得到备选栅格。
[0115]
3.2、筛选栅格。将当前位置与得到的备选栅格上的每个点依次连接，如果连线段经过障碍物栅格，则删掉此备选栅格，得到最终的蚂蚁在栅格i进行状态转移的备选栅格集合di。
[0116]
3.3，条件判断，若在栅格i处的可选择栅格allowedi＝d
i-taboom为则转到步骤6；反之，顺序执行后续步骤。
[0117]
4、状态转移参数自适应调整。如图4和图7所示，首先计算每个备选栅格的目标距离启发信息η
ij
(t)。根据步骤3获得蚂蚁在当前位置的备选节点后，得到当前节点在栅格坐标系中的坐标(xi,yi)，备选节点坐标为(xj,yj),j∈allowedi。由式(1)计算当前位置到每一个备选节点的距离d
ij
以及备选点到目标点的距离d
je
：
[0118][0119]
由此得到当前备选点j的目标距离启发信息为
[0120][0121]
4.1、计算从当前点到各备选点的旋转角度。根据第4步计算得到的当前点i与各备选点j的距离d
ij
，起始点与目标点的距离d
se
：
[0122][0123]
以当前点与备选点连线为以起始点与目标点连线为根据余弦定理依次计算每一个向量与之间的夹角θ
ijse
：
[0124][0125]
4.2、计算转角控制启发信息。根据步骤4.1计算得到的旋转角度θ
ijse
，得到转角控制信息表达式：
[0126]
a)当θ
ijse
≠180
°
时，转角控制启发信息表达式为:
[0127][0128]
b)当θ
ijse
＝180
°
时，蚂蚁若选择此备选点会背离目标点方向，为防止出现在角度为180
°
时转角控制信息为0从而使后续计算状态转移概率时出现分母为0的情况，此时转角控制启发信息表达式为：
[0129]gij
(t)＝0.01
[0130]
4.3、计算备选栅格的障碍物安全距离启发信息l
ijobs
(t)。
[0131]
4.3.1、根据当前点i的坐标(xi,yi)与各备选点j的坐标(xj,yj)得到当前点与备选点连线的直线表达式：
[0132]
(y
i-yj)x (x
j-xi)y xiy
j-xjyi＝0
[0133]
4.3.2、由步骤3.1得到的当前点扩展视野后边界中包含的障碍物栅格集合d
obsi
，分别求出集合内障碍物栅格中心点c对应的坐标值(x
obsi
,y
obsi
)；
[0134]
4.3.3、根据点到直线的距离求出各障碍物栅格中心点k距离直线ij的最短距离：
[0135][0136]
4.3.4、备选栅格的障碍物安全距离启发信息为l
ijobs
(t)＝min(l
ijobs
(c))；
[0137]
4.4、计算状态转移概率。根据步骤4～步骤4.3计算得到的目标距离启发信息η
ij
(t)，转角控制启发信息g
ij
(t)，障碍物安全距离启发信息l
ijobs
(t)，可得到蚂蚁m从当前位置i向j点转移的概率为
[0138][0139]
式中，τ
ij
(t)表示在节点j处路径上的信息素含量，allowedi表示蚂蚁m在位置i处进行视野扩展后的可行备选节点，即t 1时刻蚂蚁m可以访问的位置，即有allowedi＝d
i-tabook。
[0140]
4.5、根据迭代次数来动态的调整参数q0的值：
[0141][0142]
式中，nc为当前迭代次数，nc
max
为总迭代次数，q0的值随着迭代次数的变化而变化，δ0为常数值。
[0143]
4.6、根据轮盘赌的方式来选择节点。在得到每个备选节点j的状态转移概率后，根据轮盘赌策略进行节点选择。
[0144][0145]
当随机数q≤q0时，蚂蚁选择备选节点中最大的点，反之，则随机选择一个备选节点，并且移动到下一节点j，节点路径长度路径总长度lengthm＝lengthm length
ij
。
[0146]
4.7、修改禁忌表taboom。蚂蚁每访问一个节点j就将其加入禁忌表taboom中，禁忌表中的点不允许在该只蚂蚁后续选择路径点中出现。
[0147]
4.8、计算路径占用栅格。计算出当前点与备选点连线的斜率
[0148]
(a)判断斜率k
ij
的值。若k
ij
＝0，占用栅格坐标求解公式为：
[0149][0150]
(b)若k
ij
不存在时，占用栅格坐标求解公式为：
[0151][0152]
(c)若k
ij
＝1时，占用栅格坐标求解公式为：
[0153][0154]
(d)若1《k
ij
《∞，按照当前点到备选点连线的顺序，计算出连线与各平行网格线的交点，连线占用栅格为与交点相接的两个栅格，若交点为栅格顶点则忽略；若0《k
ij
《1，则计算出连线与各垂直网格线的交点，连线占用栅格为与交点相接的两个栅格，若交点为栅格顶点则忽略。
[0155]
4.9、坐标转换。在得到占用栅格的坐标点后，根据式(2)转换成对应的栅格地图索引值。
[0156]
5、判断蚂蚁是否移动到目标点e：
[0157]
a)若当前蚂蚁未到达目标点e，则重复步骤3～步骤5；
[0158]
b)若当前位置栅格为目标点e，若lengthm《minkl,令minkl＝lengthm，并顺序执行后续步骤。
[0159]
6、判断当前蚂蚁数量m是否达到最大蚂蚁数量。若m《m
max
，令m＝m 1，,将禁忌表taboom清零，重复步骤3～步骤6；若m＝m
max
，则顺序执行步骤7。
[0160]
7、如图8和图9，调整信息素挥发系数ρ。根据当前迭代次数和当代最佳路径长度调节信息素挥发系数。
[0161][0162]
式中，ρ表示路径上信息素的挥发度，κ是常数，nc
max
为最大迭代次数，nc为当前迭代次数，d
se
为起始点s到目标点e的直线距离，minkl为当代蚂蚁的最佳路径长度。
[0163]
7.1、更新全局信息素。经过h时刻后，整个蚁群完成一次迭代，各条路径上的信息素按照式(3)进行更新，
[0164]
τ
ij
(t h)＝(1-ρ)
·
τ
ij
(t) δτ
ij
(t)
ꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0165]
式中，δτ
ij
(t)表示m只蚂蚁在此次循环中残留在各条路径上的信息素量的总和，计算公式为每只蚂蚁留在路径上每个占用栅格的信息素量的计算式为
[0166][0167]
其中，q表示信息素强度的常数，lengthm为蚂蚁m走过的路径的总长度。
[0168]
7.2、如果当代最优路径minkl《minl，令全局最优路径minl＝minkl。
[0169]
7.3、判断当前迭代次数是否达到最大迭代次数。若nc《nc
max
，令迭代次数nc＝nc 1，重复步骤3～步骤7.3；若迭代次数nc＝nc
max
，则算法达到最大允许迭代次数，输出全局最优路径minl，算法终止。