CN115560774B - 一种面向动态环境的移动机器人路径规划方法 - Google Patents
一种面向动态环境的移动机器人路径规划方法 Download PDFInfo
- Publication number
- CN115560774B CN115560774B CN202211300546.XA CN202211300546A CN115560774B CN 115560774 B CN115560774 B CN 115560774B CN 202211300546 A CN202211300546 A CN 202211300546A CN 115560774 B CN115560774 B CN 115560774B
- Authority
- CN
- China
- Prior art keywords
- path
- mobile robot
- representing
- node
- pheromone
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 239000003016 pheromone Substances 0.000 claims abstract description 92
- 230000003068 static effect Effects 0.000 claims abstract description 60
- 241000257303 Hymenoptera Species 0.000 claims abstract description 56
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 35
- 238000012216 screening Methods 0.000 claims abstract description 17
- 230000004888 barrier function Effects 0.000 claims abstract description 13
- 230000007704 transition Effects 0.000 claims abstract description 13
- 238000012545 processing Methods 0.000 claims abstract description 7
- 238000011156 evaluation Methods 0.000 claims description 21
- 230000001133 acceleration Effects 0.000 claims description 17
- 230000008569 process Effects 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 10
- 238000012546 transfer Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000000670 limiting effect Effects 0.000 claims description 4
- 230000003247 decreasing effect Effects 0.000 claims description 3
- 230000001965 increasing effect Effects 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 2
- 230000008901 benefit Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 4
- 230000002829 reductive effect Effects 0.000 description 4
- 101100037618 Neurospora crassa (strain ATCC 24698 / 74-OR23-1A / CBS 708.71 / DSM 1257 / FGSC 987) ant-1 gene Proteins 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000002708 enhancing effect Effects 0.000 description 2
- 230000002147 killing effect Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000019637 foraging behavior Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002028 premature Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及移动机器人路径规划领域，具体涉及一种面向动态环境的移动机器人路径规划方法；包括建立静态栅格地图设置起始节点与目标节点；基于双层蚁群算法进行全局路径规划，在伪随机状态转移规则中引入下一节点到目标节点的欧氏距离的倒数；采用改进的信息素更新策略更新信息素浓度，并利用死锁处理策略惩罚陷入死锁的蚂蚁和使用回退策略的蚂蚁；直到迭代完成得到静态最优路径；采用关键点筛选策略对静态最优路径进行关键点筛选得到平滑静态最优路径；加入未知静态障碍物和动态障碍物，采用改进动态窗口法对平滑静态最优路径进行局部规划，最终输出动态最优路径；该方法有较快的收敛速度、较强的全局搜索能力以及实时避障能力。
Description
技术领域
本发明涉及移动机器人路径规划领域，具体涉及一种面向动态环境的移动机器人路径规划方法。
背景技术
路径规划是移动机器人领域的一个研究热点，其目的是在未知或已知的环境中，按照指定的优化规则(如距离最短、安全性最高、实时性强、时间最短等)搜索出一条从起始位置到目标位置最优或者接近最优的安全无碰撞路径。目前，局部路径规划算法主要包括人工势场法、动态窗口法，全局路径规划算法包括Dijkstra算法、A*算法、RRT(Rapidly-Exploring Random Trees)算法、蝙蝠算法、遗传算法、蚁群算法等。蚁群算法是模拟自然界中蚂蚁的觅食行为，其具有鲁棒性强、正反馈高、并行性高等优点，但是其搜索效率低、收敛速度慢、易陷入局部最优且无法避开未知障碍物。动态窗口法在进行路径规划时能够使移动机器人具有很好的避障能力，实时性高，但是容易陷入局部最优，其适合应用在局部路径规划上。文献[19]设置了不均匀初始信息素浓度，引入最优解与最差解更新信息素浓度，改进了状态转移规则，提高了算法的收敛速度以及全局搜索能力，但是在路径搜索过程中无法避开未知障碍物；文献[22]提出蚂蚁可以被访问的节点，并且机器人决策过程受起始节点与目标节点的影响，可以实现动态避障，但是该算法全局搜索能力一般。
发明内容
为解决现有技术中，移动机器人动态路径规划中的易碰撞、搜索能力差、收敛速度慢、容易陷入局部最优等问题，本发明结合双层蚁群算法(Two-layer Optimal Ant ColonyAlgorithm,TOACA)和改进动态窗口法(Dynamic window approach,DWA)提出动态路径规划方法(TOACA-DWA)，进而提供了一种面向动态环境的移动机器人路径规划方法，具体包括：
S1.建立静态栅格地图，在该静态栅格地图上设置移动机器人的起始节点与目标节点；
S2.基于双层蚁群算法在静态栅格地图中进行移动机器人全局路径规划，在双层蚁群算法的伪随机状态转移规则中引入下一节点到目标节点的欧氏距离的倒数；
S3.在全局路径规划过程中采用改进的信息素更新策略更新信息素浓度，并利用死锁处理策略惩罚陷入死锁的蚂蚁和使用回退策略的蚂蚁；直到迭代完成，得到静态最优路径；
S4.采用关键点筛选策略对静态最优路径进行关键点筛选，得到平滑静态最优路径；
S5.在静态栅格地图中加入未知静态障碍物和动态障碍物，采用改进动态窗口法对平滑静态最优路径进行局部规划，最终输出动态最优路径。
进一步的，在双层蚁群算法中采用伪随机状态转移规则选择下一节点，伪随机状态转移规则包括：
其中，j表示寻找的下一节点，τij(t)表示在第t次迭代中，路径＜i,j＞上的信息素浓度；ηij(t)表示在第t次迭代中，当前节点i到下一节点j的启发信息；表示下一节点j到目标节点E的欧氏距离的倒数，α表示信息素启发式因子，β表示期望启发式因子，χ表示目标点启发式因子，q表示在区间(0,1)内的随机数，q0表示根据迭代次数定义的选择因子，表示在第t次迭代中，第k只蚂蚁由当前节点i选择下一节点j的状态转移概率，allowedk表示第k只蚂蚁在当前节点i可选择的下一节点j的集合。
进一步的，选择因子q0的计算公式为：
其中，t∈Tmax表示当前的迭代次数，Tmax表示最大迭代次数。
进一步的，当所有蚂蚁完成一次路径搜索后，统计到达目标节点的蚂蚁所走过的成功路径，通过所有成功路径的长度总和计算平均路径长度，采用改进的信息素更新策略更新每一条成功路径的信息素浓度，具体包括：
若成功路径为最优路径，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)+λ1×ρ×Δτij(t+1)
若成功路径的长度大于最优路径，且小于平均路径长度，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)+λ2×ρ×Δτij(t+1)
若成功路径的长度大于平均路径长度，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)-λ3×ρ×Δτij(t+1)
其中，λ1、λ2、λ3为信息素增减比例系数，ρ表示信息素挥发因子，τij(t)表示在t次迭代后，路径＜i,j＞上的最终信息素浓度；τi′j(t+1)表示在t+1次迭代后，路径＜i,j＞上的更新信息素浓度；Δτij(t+1)表示在t+1次迭代后，路径＜i,j＞上的信息素浓度增量，其表达式如下：
式中Q为信息素强度，Lk表示第k只蚂蚁在本次迭代中所走过路径的总长度，表示第k只蚂蚁在本次迭代中所走过路径转折点个数，λ是一个常数，取值范围为(0,1)。
进一步的，限制路径的信息素浓度上下限，避免陷入局部最优，限制公式为：
其中，τmax表示信息素浓度最大值，τmin表示信息素浓度最小值。
进一步的，死锁处理策略包括：若蚂蚁进行路径搜索时陷入死锁，记录陷入死锁的位置并采用回退策略；若该蚂蚁的回退步数大于2，则将该蚂蚁抹杀并惩罚该蚂蚁陷入死锁的位置的信息素浓度；若该蚂蚁的回退步数小于等于2，则惩罚该蚂蚁陷入死锁的位置的信息素浓度。
进一步的，死锁处理策略中的惩罚公式为：
其中，τi′j(t+1)表示在t次迭代后，路径＜i,j＞上的更新信息素浓度；Ndeadlock表示在路径＜i,j＞上陷入死锁且回退步数大于2的蚂蚁数量；Nstepsback表示在路径＜i,j＞上陷入死锁且回退步数小于等于2的蚂蚁数量，τij(t+1)表示在t次迭代后，对路径＜i,j＞上的更新信息素浓度进行惩罚后的最终信息素浓度，λdl表示死锁惩罚因子，λsb表示回退惩罚因子，Nsteps表示回退步数。
进一步的，采用改进动态窗口法对平滑静态最优路径进行局部规划的过程为：
S51.初始化参数；假设移动机器人不能全向移动，即不能有横向的速度，只有前进的线速度和旋转角速度(v,w)；
S52.设置移动机器人的运动学模型，根据运动学模型确定移动机器人的速度范围Vs；结合线加速度和角加速度确定移动机器人实际的速度范围Vd以及移动机器人不与障碍物碰撞的速度范围Va；最终得到移动机器人的运动速度空间为Vr＝Vs∩Vd∩Va；最后设计评价函数G(v,w)；
S53.确定起始节点S(xs,ys)与第一子目标节点连接起始节点与第一子目标节点得到线段SK1，计算线段SK1与x轴正方向的夹角，将该夹角作为初始航向角；
S54.采用移动机器人的运动速度空间和运动学模型模拟多条轨迹，通过评价函数筛选最优轨迹到达第一子目标节点，继续选取下一子目标节点进行局部路径规划，直到到达目标节点。
进一步的，移动机器人的运动学模型表示为：
其中，xt表示t时刻移动机器人在x轴方向上的坐标，xt-1表示t-1时刻移动机器人在x轴方向上的坐标，yt表示t时刻移动机器人在y轴方向上的坐标，θt表示t时刻移动机器人运动方向与x轴的夹角，vt表示t时刻移动机器人的线速度，wt表示t时刻移动机器人的角速度，Δt表示的是采样间隔时间；
速度范围Vs表示为：
Vs＝{(v,w)|v∈[vmin,vmax],w∈[wmin,wmax]}
其中，vmin表示最小线速度，vmax表示最大线速度，wmin表示最小角速度，wmax表示最大角速度；
速度范围Vd表示为：
其中，vc表示采样时间间隔Δt内的线速度，表示最小线加速度，/>表示最大线加速度，wc表示采样时间间隔Δt内的角速度，/>表示最小角加速度，/>表示最大角加速度，
速度范围Va表示为：
其中，dist(v,w)表示速度(v,w)对应轨迹上离障碍物最近的距离，dist(v,w)主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率；
评价函数表示为：
G(v,w)＝σ(δ×heading(v,w)+ε×dist(v,w)+γ×vel(v,w))
其中，σ表示平滑系数，δ表示目标方向评价子函数权重系数，ε表示障碍物间隙评价子函数权重系数，γ表示速度评价子函数权重系数，heading(v,w)为方位角评价函数，表示移动机器人在当前设定的速度下，预测轨迹末端朝向与目标点之间的角度差距；vel(v,w)表示移动机器人在对应轨迹上的线速度。
本发明的有益效果：
在双层优化蚁群算法中，首先采用伪随机状态转移规则进行路径选择，引入下一节点到目标节点欧式距离的倒数到状态转移函数中，提高了算法的搜索效率以及全局搜索能力；其次提出一种奖励惩罚机制更新信息素，加强优质蚂蚁对后代蚂蚁的引导作用，减少劣质蚂蚁对后代蚂蚁的误导作用，限制信息素浓度的上下限，提高了算法的收敛速度且避免算法陷入局部最优；通过引入动态惩罚因子来减少死锁路径上的信息素浓度，防止蚂蚁陷入死锁，增加了路径搜索结果的多样性；最后采用关键节点筛选策略，删除全局路径上的冗余节点，在全局路径每两个相邻节点间采用改进动态窗口法进行局部规划，能够实现移动机器人在保证全局最优路径的基础上实时避障。该方法有较快的收敛速度、较强的全局搜索能力以及实时避障能力。
附图说明
图1为本发明的移动机器人路径规划方法步骤图；
图2为本发明的移动机器人路径规划方法完整流程图；
图3为本发明的关键节点筛选策略示意图；
图4为本发明的TOACA-DWA算法在动态环境下规划路径的示意图；
图5为本发明实施例的死锁状态情况图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供了一种面向动态环境的移动机器人路径规划方法，融合了双层蚁群算法(TOACA)与改进动态窗口法(DWA)，首先应用双层蚁群算法在静态栅格地图中找出一条全局最优路径，该全局路径已经通过关键节点筛选策略去除了冗余节点，其次在静态环境中加入未知静态障碍物和动态障碍物，形成一个动态环境，在静态全局最优路径每两个相邻节点间采用改进动态窗口法进行局部规划，能够实现移动机器人在保证全局最优路径的基础上实时避障。
具体地，如图1所示，包括以下步骤：
S1.建立静态栅格地图，在该静态栅格地图上设置移动机器人的起始节点与目标节点；
S2.基于双层蚁群算法在静态栅格地图中进行移动机器人全局路径规划，在伪随机状态转移规则中引入下一节点到目标节点的欧氏距离的倒数；
S3.采用改进的信息素更新策略更新信息素浓度，并利用死锁处理策略惩罚陷入死锁的蚂蚁和使用回退策略的蚂蚁；直到迭代完成，得到静态最优路径；
S4.采用关键点筛选策略对静态最优路径进行关键点筛选，得到平滑静态最优路径；
S5.在静态栅格地图中加入未知静态障碍物和动态障碍物，采用改进动态窗口法对平滑静态最优路径进行局部规划，最终输出动态最优路径。
在一实施例中，如图2所示，应用双层蚁群算法在静态栅格地图中进行全局路径规划，得到一条静态最优路径的过程包括：
S11.建立静态栅格地图，在该静态栅格地图中设置移动机器人的起始节点与目标节点，记录节点信息；
S12.参数初始化，包括蚂蚁数量M、最大迭代次数Tmax、信息素启发式因子α、期望启发式因子β、目标点启发式因子χ、信息素挥发因子ρ、信息素增减比例系数λ1、λ2、λ3；
S13.每只蚂蚁都有一个禁忌表，初始化所有蚂蚁的禁忌表以及路径信息素浓度，将M只蚂蚁放在起始节点；
S14.蚂蚁采用伪随机状态转移规则选择下一节点，并将自己走过的节点加入到自身的禁忌表中；
S15.判断是否所有蚂蚁都完成了一次路径搜索，若不是，则返回步骤S14让未完成路径搜索的蚂蚁继续探索，若是，则进入步骤S16；
S16.统计M只蚂蚁的状态，记录成功到达目标节点的蚂蚁所走过的成功路径；将未到达目标节点的蚂蚁“杀死”并记录各位置陷入死锁的蚂蚁数量；
S17.采用改进的信息素更新策略更新路径的信息素浓度，并且限制路径的信息素浓度的上下限；
S18.利用死锁处理策略惩罚陷入死锁的蚂蚁和使用回退策略的蚂蚁，进一步更新路径的信息素浓度；
S19.判断迭代次数是否达到最大值，若是，则输出静态最优路径，否则，返回步骤S13。
具体地，步骤S14中涉及的伪随机状态转移规则包括：
其中，j表示寻找的下一节点，τij(t)表示在第t次迭代中，路径＜i,j＞上的信息素浓度；ηij(t)表示在第t次迭代中，当前节点i到下一节点j的启发信息；表示下一节点j到目标节点E的欧氏距离的倒数，采用直角坐标系表示各节点的位置信息，(xj,yj)为下一节点的坐标，(xE,yE)为目标节点的坐标，α表示信息素启发式因子，β表示期望启发式因子，χ表示目标点启发式因子，/>表示在第t次迭代中，第k只蚂蚁由当前节点i选择下一节点j的状态转移概率，allowedk表示第k只蚂蚁在当前节点i可选择的下一节点j的集合，q表示在区间(0,1)内的随机数，q0表示根据迭代次数定义的选择因子，计算公式为：
其中，t∈Tmax表示当前的迭代次数，Tmax表示最大迭代次数，q0是一个单调递减的函数，可以提高算法的前期搜索效率，可以保障算法后期的全局搜索能力。
具体地，本实施例涉及的伪随机状态转移规则考虑的是当前节点到下一节点的距离以及下一节点到目标节点的距离，这样既考虑了全局路径的情况又考虑了局部路径的情况，基本算法只考虑了当前节点到下一节点的距离，缺乏全局性，引入下一节点到目标点的距离到状态转移函数中，在蚂蚁寻路中有一个全局的引导性，有利于蚂蚁寻得全局最优路径。
具体地，步骤S17提出的改进的信息素更新策略包括：
当所有蚂蚁完成一次路径搜索后，统计到达目标节点的蚂蚁所走过的成功路径，通过所有成功路径的长度总和计算平均路径长度，采用改进的信息素更新策略更新每一条成功路径的信息素浓度，其中：
若成功路径为最优路径，要对最优路径给予最高的信息素浓度奖励，加强最优路径上的信息素浓度，其信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)+λ1×ρ×Δτij(t+1)
若成功路径的长度大于最优路径，且小于平均路径长度，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)+λ2×ρ×Δτij(t+1)
若成功路径的长度大于平均路径长度，该成功路径较差，需要减弱该成功路径的信息素浓度，其信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)-λ3×ρ×Δτij(t+1)
其中，λ1、λ2、λ3为信息素增减比例系数，三者的取值范围均为(0.1,10)，通过仿真实验结果获取最优的一组信息素增减比例系数，ρ表示信息素挥发因子，τij(t)表示在t次迭代后，路径＜i,j＞上的最终信息素浓度；τi′j(t+1)表示在t+1次迭代后，路径＜i,j＞上的更新信息素浓度；Δτij(t+1)表示在t+1次迭代后，路径＜i,j＞上的信息素浓度增量，其计算公式为：
其中，Q为信息素强度，Lk表示第k只蚂蚁在本次迭代中所走过路径的总长度，表示第k只蚂蚁在本次迭代中所走过路径转折点个数，λ是一个常数，取值范围为(0,1)。信息素浓度增量公式不仅考虑了路径长度，还充分考虑了路径中的转折点个数，可以使得蚂蚁最终选择一条相对平滑的路径。
为了防止算法陷入局部最优以及早熟，限制信息素浓度上下限，具体公式为：
其中，τmax表示信息素浓度最大值，τmin表示信息素浓度最小值。
具体地，在蚂蚁的路径搜索过程中会出现陷入死锁的问题，即路径阻塞无法继续推进，本实施例中，当蚂蚁陷入死锁时，记录陷入死锁的位置，采用回退的办法，允许最大的回退步数为2；当回退步数大于2时，则“杀死”这只蚂蚁，且对该死锁路径上的信息素浓度进行严厉处罚，可以防止其他蚂蚁在此处陷入死锁；当回退步数小于等于2时，这只蚂蚁到达了目标节点，但客观认为这条路径质量不好，当一代蚂蚁搜索完毕之后，根据在这个位置陷入死锁的蚂蚁数量动态减少此路径上的信息素浓度。
步骤S18采用死锁处理策略惩罚陷入死锁以及使用回退策略的蚂蚁，尽可能地保障了蚂蚁种群的多样性，还可以有效防止其他蚂蚁在同一位置陷入死锁，死锁处理策略中的惩罚公式为：
其中，τi′j(t+1)表示在t次迭代后，路径＜i,j＞上的更新信息素浓度；Ndeadlock表示在路径＜i,j＞上陷入死锁且回退步数大于2的蚂蚁数量；Nstepsback表示在路径＜i,j＞上陷入死锁且回退步数小于等于2的蚂蚁数量，τij(t+1)表示在t次迭代后，对路径＜i,j＞上的更新信息素浓度进行惩罚后的最终信息素浓度，λdl表示死锁惩罚因子，λsb表示回退惩罚因子，Nsteps表示回退步数。
具体地，当某条路径上既有回退步数小于等于2的蚂蚁，又有回退步数大于2的蚂蚁时，按照回退步数大于2的情况进行惩罚。
具体地，如图5所示，图中黑色方块表示障碍物，白色方块为可行路径，实线为蚂蚁1走过的路径，虚线为蚂蚁2走过的路径，蚂蚁1在陷入死锁状态时采用回退策略，回退了两步过后有新的路径可以选择，成功摆脱死锁状态，朝着目标节点继续前进，这时不用杀死蚂蚁1，但要对陷入死锁的路径上的信息素进行轻微惩罚；而蚂蚁2在陷入死锁状态后同样采用回退策略，但是在回退两步后仍然没有新的路径可以选择，还是处于死锁状态，故“杀死”蚂蚁2，并对这条路径上的信息素进行严厉惩罚。
在一实施例中，经过双层蚁群算法的初次规划后，得到一条静态最优路径，记录其经过的栅格节点，将其表示为P＝{S,P1,P2,...,Pn,E}，S为起始节点，E为目标节点，Pn为静态最优路径中的第n个节点。
采用关键点筛选策略对静态最优路径进行关键点筛选，即进行二次规划时，蚂蚁下一节点选择的方向是朝着目标点的，对路径的二次规划是单向的。若不考虑节点间障碍物的存在情况，当蚂蚁位于节点Pi-1，选择下一节点时，蚂蚁可以选择的节点为P(i)＝{Pi,Pi+1,Pi+2,......,Pn-1,Pn,G}，其均为一次规划结果中节点Pi-1的后续节点。
一般来说栅格地图障碍物的类型与数量比较多，因此需要判断两个节点之间是否存在障碍物。假设现在有两个节点Pa,Pb，若Pa,Pb之间存在障碍物，则蚂蚁不能从Pa直接到达Pb，反之可以通过，具体判断式子如下
此时当蚂蚁位于Pi-1节点，选择下一节点时，将两个节点之间障碍物情况考虑进去，蚂蚁可以选择的节点如下
Pi o＝{P|P∈P(i)∩Pass(Pi-1,P)＝1}
选择距离目标节点最近的节点，依此寻找下一节点，直到到达目标节点。
具体地，图3所示，图中的黑色方块表示静态障碍物，白色方块为可通行路径，浅灰色线条为双层蚁群算法初次规划后的静态最优路径，黑色线条为采用关键点筛选策略进行二次规划后的平滑静态最优路径，从图中可以看出，平滑静态最优路径删除了冗余节点，转折点更少，整体更加平滑，距离更短。
在一实施例中，在静态栅格地图中加入未知静态障碍物和动态障碍物，构造动态环境，将平滑静态最优路径中的关键节点定义为子目标节点，采用改进动态窗口法对平滑静态最优路径进行局部规划，自适应改变机器人的朝向，最终输出动态最优路径。
具体过程如图2所示，包括：
S21.初始化参数，包括最大线速度vmax、最小线速度vmin、最大角速度wmax、最小角速度wmin、最大线加速度最小线加速度/>最大角加速度/>最小角加速度/>采样时间间隔Δt、目标方向评价子函数权重系数δ、障碍物间隙评价子函数权重系数ε、速度评价子函数权重系数γ、起始节点S、目标节点E、子目标节点Gi；
S22.设置移动机器人的运动学模型，根据运动学模型确定移动机器人的速度范围Vs；结合线加速度和角加速度确定移动机器人实际的速度范围Vd以及移动机器人不与障碍物碰撞的速度范围Va；最终得到移动机器人的运动速度空间为Vr＝Vs∩Vd∩Va；最后设计评价函数G(v,w)；
S23.确定起始节点S(xs,ys)与第一子目标节点连接起始节点与第一子目标节点得到线段SK1，计算线段SK1与x轴正方向的夹角，将该夹角作为初始航向角；
S24.采用移动机器人的运动速度空间和运动学模型模拟多条轨迹，通过评价函数筛选最优轨迹到达第一子目标节点，继续选取下一子目标节点进行局部路径规划，直到到达目标节点。
具体地，对移动机器人的速度进行采样约束，在速度空间中存在无穷多组(vt,wt)，但是在现实环境中，需要考虑机器人本身及环境的限制，对采样的速度约束在一定范围之内。
DWA的基本思想是结合移动机器人的运动特性，在路径规划过程中实时预测下一时间段Δt内的运动轨迹，并且假设在Δt内移动机器人的线速度与角速度恒定。速度空间的线速度V和角速度W反馈出的就是移动机器人的运动状态。本实施例的运动学模型表示为：
其中，xt表示t时刻移动机器人在x轴方向上的坐标，xt-1表示t-1时刻移动机器人在x轴方向上的坐标，yt表示t时刻移动机器人在y轴方向上的坐标，θt表示t时刻移动机器人运动方向与x轴的夹角，vt表示t时刻移动机器人的线速度，wt表示t时刻移动机器人的角速度，Δt表示的是采样间隔时间；
速度范围Vs表示为：
Vs＝{(v,w)|v∈[vmin,vmax],w∈[wmin,wmax]}
加速度受电机性能约束，在一个时间间隔Δt内，速度范围Vd表示为：
在机器人的运动时，当出现随机障碍物，为保障机器人不与障碍物发生碰撞，在最大减速度作用下，机器人必须在碰撞到障碍物之前停下来，则速度范围Va表示为：
评价函数表示为：
G(v,w)＝σ(δ×heading(v,w)+ε×dist(v,w)+γ×vel(v,w))
其中，dist(v,w)表示速度(v,w)对应轨迹上离障碍物最近的距离，主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率；heading(v,w)为方位角评价函数，表示移动机器人在当前设定的速度下，预测轨迹末端朝向与目标点之间的角度差距；vel(v,w)表示移动机器人在对应轨迹上的线速度，为了促进机器人快速到达目标。
具体地，如图4所示，虚线为TOACA规划出的平滑静态最优路径，在静态栅格地图中增加9个未知的静态障碍物和2个未知的动态障碍物，动态障碍物做匀速直线运动，且运动速度低于移动机器人，通过改进动态窗口法对平滑静态最优路径最局部路径规划，规避未知的静态障碍物和动态障碍物，得到全局最优动态路径。
在本发明中，除非另有明确的规定和限定，术语“安装”、“设置”、“连接”、“固定”、“旋转”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
Claims (4)
1.一种面向动态环境的移动机器人路径规划方法，其特征在于，包括以下步骤：
S1.建立静态栅格地图，在该静态栅格地图上设置移动机器人的起始节点与目标节点；
S2.基于双层蚁群算法在静态栅格地图中进行移动机器人全局路径规划，并在双层蚁群算法的伪随机状态转移规则中引入下一节点到目标节点的欧氏距离的倒数；
在双层蚁群算法中采用伪随机状态转移规则选择下一节点，伪随机状态转移规则包括：
其中，j表示寻找的下一节点，τij(t)表示在第t次迭代中，路径＜i,j＞上的信息素浓度；ηij(t)表示在第t次迭代中，当前节点i到下一节点j的启发信息；表示下一节点j到目标节点E的欧氏距离的倒数，α表示信息素启发式因子，β表示期望启发式因子，χ表示目标点启发式因子，q表示在区间(0,1)内的随机数，q0表示根据迭代次数定义的选择因子，表示在第t次迭代中，第k只蚂蚁由当前节点i选择下一节点j的状态转移概率，allowedk表示第k只蚂蚁在当前节点i可选择的下一节点j的集合；
选择因子q0的计算公式为：
其中，t∈Tmax表示当前的迭代次数，Tmax表示最大迭代次数；
S3.在全局路径规划过程中采用改进的信息素更新策略更新信息素浓度，并利用死锁处理策略惩罚陷入死锁的蚂蚁和使用回退策略的蚂蚁；直到迭代完成，得到静态最优路径；
当所有蚂蚁完成一次路径搜索后，统计到达目标节点的蚂蚁所走过的成功路径，通过所有成功路径的长度总和计算平均路径长度，采用改进的信息素更新策略更新每一条成功路径的信息素浓度，具体包括：
若成功路径为最优路径，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)+λ1×ρ×Δτij(t+1)
若成功路径的长度大于最优路径，且小于平均路径长度，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)+λ2×ρ×Δτij(t+1)
若成功路径的长度大于平均路径长度，信息素浓度更新公式为：
τi′j(t+1)＝(1-ρ)×τij(t)-λ3×ρ×Δτij(t+1)
其中，λ1、λ2、λ3为信息素增减比例系数，ρ表示信息素挥发因子，τij(t)表示在t次迭代后，路径＜i,j＞上的最终信息素浓度；τi′j(t+1)表示在t+1次迭代后，路径＜i,j＞上的更新信息素浓度；Δτij(t+1)表示在t+1次迭代后，路径＜i,j＞上的信息素浓度增量，其表达式如下：
式中Q为信息素强度，Lk表示第k只蚂蚁在本次迭代中所走过路径的总长度，表示第k只蚂蚁在本次迭代中所走过路径转折点个数，λ是一个常数，取值范围为(0,1)；
死锁处理策略包括：若蚂蚁进行路径搜索时陷入死锁，记录陷入死锁的位置并采用回退策略；若该蚂蚁的回退步数大于2，则将该蚂蚁抹杀并惩罚该蚂蚁陷入死锁的位置的信息素浓度；若该蚂蚁的回退步数小于等于2，则惩罚该蚂蚁陷入死锁的位置的信息素浓度；
死锁处理策略中的惩罚公式为：
其中，τi′j(t+1)表示在t次迭代后，路径＜i,j＞上的更新信息素浓度；Ndeadlock表示在路径＜i,j＞上陷入死锁且回退步数大于2的蚂蚁数量；Nstepsback表示在路径＜i,j＞上陷入死锁且回退步数小于等于2的蚂蚁数量，τij(t+1)表示在t次迭代后，对路径＜i,j＞上的更新信息素浓度进行惩罚后的最终信息素浓度，λdl表示死锁惩罚因子，λsb表示回退惩罚因子，Nsteps表示回退步数；
S4.采用关键点筛选策略对静态最优路径进行关键点筛选，得到平滑静态最优路径；
S5.在静态栅格地图中加入未知静态障碍物和动态障碍物，采用改进动态窗口法对平滑静态最优路径进行局部规划，最终输出动态最优路径。
2.根据权利要求1所述的一种面向动态环境的移动机器人路径规划方法，其特征在于，限制路径的信息素浓度上下限，避免陷入局部最优，限制公式为：
其中，τmax表示信息素浓度最大值，τmin表示信息素浓度最小值。
3.根据权利要求1所述的一种面向动态环境的移动机器人路径规划方法，其特征在于，采用改进动态窗口法对平滑静态最优路径进行局部规划的过程为：
S51.初始化参数；
S52.设置移动机器人的运动学模型，根据运动学模型确定移动机器人的速度范围Vs；结合线加速度和角加速度确定移动机器人实际的速度范围Vd以及移动机器人不与障碍物碰撞的速度范围Va；最终得到移动机器人的运动速度空间为Vr＝Vs∩Vd∩Va；最后设计评价函数G(v,w)；
S53.确定起始节点S(xs,ys)与第一子目标节点K1(xK1,yK1)，连接起始节点与第一子目标节点得到线段SK1，计算线段SK1与x轴正方向的夹角，将该夹角作为初始航向角；
S54.采用移动机器人的运动速度空间和运动学模型模拟多条轨迹，通过评价函数筛选最优轨迹到达第一子目标节点，继续选取下一子目标节点进行局部路径规划，直到到达目标节点。
4.根据权利要求3所述的一种面向动态环境的移动机器人路径规划方法，其特征在于，移动机器人的运动学模型表示为：
其中，xt表示t时刻移动机器人在x轴方向上的坐标，xt-1表示t-1时刻移动机器人在x轴方向上的坐标，yt表示t时刻移动机器人在y轴方向上的坐标，θt表示t时刻移动机器人运动方向与x轴的夹角，vt表示t时刻移动机器人的线速度，wt表示t时刻移动机器人的角速度，Δt表示的是采样间隔时间；
速度范围Vs表示为：
Vs＝{(v,w)|v∈[vmin,vmax],w∈[wmin,wmax]}
其中，vmin表示最小线速度，vmax表示最大线速度，wmin表示最小角速度，wmax表示最大角速度；
速度范围Vd表示为：
其中，vc表示采样时间间隔Δt内的线速度，表示最小线加速度，/>表示最大线加速度，wc表示采样时间间隔Δt内的角速度，/>表示最小角加速度，/>表示最大角加速度，
速度范围Va表示为：
其中，dist(v,w)表示速度(v,w)对应轨迹上离障碍物最近的距离；
评价函数表示为：
G(v,w)＝σ(δ×heading(v,w)+ε×dist(v,w)+γ×vel(v,w))
其中，σ表示平滑系数，δ表示目标方向评价子函数权重系数，ε表示障碍物间隙评价子函数权重系数，γ表示速度评价子函数权重系数，heading(v,w)为方位角评价函数，表示移动机器人在当前设定的速度下，预测轨迹末端朝向与目标点之间的角度差距，vel(v,w)表示移动机器人在对应轨迹上的线速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211300546.XA CN115560774B (zh) | 2022-10-24 | 2022-10-24 | 一种面向动态环境的移动机器人路径规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211300546.XA CN115560774B (zh) | 2022-10-24 | 2022-10-24 | 一种面向动态环境的移动机器人路径规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115560774A CN115560774A (zh) | 2023-01-03 |
| CN115560774B true CN115560774B (zh) | 2023-11-17 |
Family
ID=84746268
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211300546.XA Active CN115560774B (zh) | 2022-10-24 | 2022-10-24 | 一种面向动态环境的移动机器人路径规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115560774B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116048120B (zh) * | 2023-01-10 | 2024-04-16 | 中国建筑一局(集团)有限公司 | 一种未知动态环境下小型四旋翼无人机自主导航系统及方法 |
| CN116125995B (zh) * | 2023-04-04 | 2023-07-28 | 华东交通大学 | 一种高铁巡检机器人的路径规划方法及系统 |
| CN117055575A (zh) * | 2023-09-19 | 2023-11-14 | 天津开发区精诺瀚海数据科技有限公司 | 一种面向黑灯工厂的搬运车路径规划方法 |
| CN117091608B (zh) * | 2023-10-17 | 2023-12-29 | 山东怀思人工智能科技有限公司 | 一种教育机器人的路径规划方法、设备及介质 |
| CN118494468A (zh) * | 2024-07-17 | 2024-08-16 | 罗普特科技集团股份有限公司 | 基于人工智能的车辆控制方法及系统 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107145961A (zh) * | 2017-03-24 | 2017-09-08 | 南京邮电大学 | 一种基于改进遗传算法的旅游行程规划方法 |
| CN112631294A (zh) * | 2020-12-16 | 2021-04-09 | 上海应用技术大学 | 一种移动机器人智能路径规划方法 |
| CN112650242A (zh) * | 2020-12-22 | 2021-04-13 | 天津理工大学 | 一种基于混合算法的移动机器人路径规划方法 |
| WO2021189720A1 (zh) * | 2020-03-23 | 2021-09-30 | 南京理工大学 | 基于改进蚁群算法的泊车agv路径规划方法 |
| CN114756027A (zh) * | 2022-04-08 | 2022-07-15 | 重庆邮电大学 | 一种基于改进蚁群算法与贝塞尔曲线的移动机器人路径规划方法 |
-
2022
- 2022-10-24 CN CN202211300546.XA patent/CN115560774B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107145961A (zh) * | 2017-03-24 | 2017-09-08 | 南京邮电大学 | 一种基于改进遗传算法的旅游行程规划方法 |
| WO2021189720A1 (zh) * | 2020-03-23 | 2021-09-30 | 南京理工大学 | 基于改进蚁群算法的泊车agv路径规划方法 |
| CN112631294A (zh) * | 2020-12-16 | 2021-04-09 | 上海应用技术大学 | 一种移动机器人智能路径规划方法 |
| CN112650242A (zh) * | 2020-12-22 | 2021-04-13 | 天津理工大学 | 一种基于混合算法的移动机器人路径规划方法 |
| CN114756027A (zh) * | 2022-04-08 | 2022-07-15 | 重庆邮电大学 | 一种基于改进蚁群算法与贝塞尔曲线的移动机器人路径规划方法 |
Non-Patent Citations (2)
| Title |
|---|
| "基于自适应机制改进蚁群算法的面向动态环境的移动机器人全局路径规划";毛文平等;《控制与决策》;第1-9页 * |
| 基于自适应机制改进蚁群算法的面向动态环境的移动机器人全局路径规划;毛文平等,;《控制与决策》;第1-9页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115560774A (zh) | 2023-01-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115560774B (zh) | 一种面向动态环境的移动机器人路径规划方法 | |
| CN113093724B (zh) | 一种基于改进蚁群算法的agv路径规划方法 | |
| CN111930121B (zh) | 一种室内移动机器人的混合路径规划方法 | |
| CN114167865B (zh) | 一种基于对抗生成网络与蚁群算法的机器人路径规划方法 | |
| CN112269382B (zh) | 一种机器人多目标路径规划方法 | |
| CN110375761A (zh) | 基于增强蚁群优化算法的无人驾驶车辆路径规划方法 | |
| CN114895707B (zh) | 基于变频蝙蝠算法的农业无人机路径规划方法及系统 | |
| CN114756027A (zh) | 一种基于改进蚁群算法与贝塞尔曲线的移动机器人路径规划方法 | |
| CN110686695A (zh) | 基于目标评价因子的自适应蚁群a星混合算法 | |
| CN114967680B (zh) | 基于蚁群算法和卷积神经网络的移动机器人路径规划方法 | |
| CN112486185B (zh) | 在未知环境下基于蚁群和vo算法的路径规划方法 | |
| CN117522078A (zh) | 无人系统集群环境耦合下的可迁移任务规划方法及系统 | |
| CN114578845A (zh) | 一种基于改进蚁群算法的无人机航迹规划方法 | |
| Panda et al. | Autonomous mobile robot path planning using hybridization of particle swarm optimization and Tabu search | |
| Schmidt et al. | How to learn from risk: Explicit risk-utility reinforcement learning for efficient and safe driving strategies | |
| CN117506893A (zh) | 基于双重规划和自主学习的机械臂路径规划和控制方法 | |
| CN117387619A (zh) | 一种基于融合改进蚁群和遗传算法的路径规划方法 | |
| Wang et al. | Deep reinforcement learning by parallelizing reward and punishment using the maxpain architecture | |
| CN116360454A (zh) | 行人环境下基于深度强化学习的机器人路径避碰规划方法 | |
| CN116627175A (zh) | 一种基于改进蚁群算法的无人机路径规划方法 | |
| Peng et al. | Hybrid learning for multi-agent cooperation with sub-optimal demonstrations | |
| CN112330043B (zh) | 一种结合Q-learning与多蜂群算法的疏散路径规划方法及系统 | |
| Zhao et al. | Learning multi-agent communication with policy fingerprints for adaptive traffic signal control | |
| Zhou | Application of hybrid ant colony algorithm in urban traffic path planning | |
| Yu et al. | An intelligent robot motion planning method and application via lppo in unknown environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |