未知环境下基于改进蚁和动态窗口法的路径规划方法

1.本发明涉及智能路径规划技术领域，尤其是一种未知环境下基于改进蚁和动态窗口法的路径规划方法。

背景技术：

2.随着自动化、计算机等各种技术的不断发展和应用，智能体技术逐渐发展，结构日趋复杂，功能更加强大，智能体在工厂、物流、交通、智能电网等领域具有广泛应用。智能体在工作过程中由于外界环境变化原因，若不能及时规划新的路径并进行安全的跟踪，可能会发生碰撞，最终造成严重后果，移动智能体的路径规划技术逐渐成为研究热点问题。所谓移动智能体路径规划技术，就是智能体根据自身传感器对环境的感知，自行规划出一条安全的运行路线，同时高效完成作业任务。移动智能体路径规划主要解决3个问题：1)使得智能体可以从起始点到达目标点；2)使得智能体可以安全地避开环境中的障碍物，完成相应的作业任务；3)在完成前两项任务的前提下，优化生成的轨迹使其尽量平滑。
3.蚁算法属于仿生算法，模拟自然界中蚂蚁的觅食行为。最初由意大利学者m.dorigo等人于1991年提出(参见colorni a,dorigo m,maniezzo v.distributed optimization by ant colonies[c]//proceedings of the first european conference on artificial life.1991,142:134-142.)，他从蚂蚁的觅食行为中得到启发，将其思想应用于处理旅行商问题(tsp)，提出了蚁算法。此种算法有以下优点：稳定性较强、有较好的寻优能力、容易实现并行化。与此同时有以下缺点：算法收敛较慢，容易早熟、陷入局部最优，算法在某些情况下可能会出现停滞即死锁现象。为了解决死锁问题，相关学者们提出了一些改进方法，例如改变信息素更新的时间，可以提高蚁算法的收敛速度，但是其缺点是在算法初始阶段蚂蚁盲目地进行搜索(参见zhao j,cheng d,hao c.an improved ant colony algorithm for solving the path planning problem of the omnidirectional mobile vehicle[j].mathematical problems in engineering,2016,2016:1-10.)。文献(wang l.path planning for unmanned wheeled robot based on improved ant colony optimization[j].measurement and control,2020,53(5-6):1014-1021.)采用改进的传输策略降低蚂蚁因为随机死锁导致寻路失败的概率，提高了算法的收敛速度。文献(luo q,wang h b,zheng y,et al.research on path planning of mobile robot based on improved ant colony algorithm[j].neural computing and applications,2020,32(1):1555
–
1566.)在迭代开始之前，首先在特定的路径上不平均地分配了初始信息素，可以有效地避免算法在初始阶段，随机地进行盲目搜索，但是效率较低。还有部分学者采用了“早期死亡”的方法，禁止处于死锁状态的蚂蚁更新路径上的信息素，这种方法的优点是简单易实现，但是其缺点是对于后续的蚂蚁没有起到警戒或者说负反馈的作用，同时会导致丢弃大量的蚂蚁，减弱了算法搜索过程中的多样性。虽然这些改进算法使得蚁算法的性能在一定程度上有所改进，但是当地图环境复杂度增加时，算法陷入局部最优、遇到死锁情况的概率大大提升。

技术实现要素：

[0004]
本发明所要解决的技术问题在于，提供一种未知环境下基于改进蚁和动态窗口法的路径规划方法，能够实现在存在未知静态障碍物和动态障碍物的环境下进行实时的路径规划。
[0005]
为解决上述技术问题，本发明提供一种未知环境下基于改进蚁和动态窗口法的路径规划方法，包括如下步骤：
[0006]
步骤1、根据已知的地图信息，初始化地图矩阵map矩阵；其中，静态障碍物的区域值设为1，其他区域值设为0；初始化障碍物数组obs；
[0007]
步骤2、利用改进后的蚁算法计算初始路径pathlist；
[0008]
步骤3、若智能体可探测的范围内出现新的障碍物，则将其加入到障碍物集合obs中；
[0009]
步骤4、智能体为每个计算得出的路径点设置序号，存入内存，记录目前追踪的全局路径上的当前路径点，存入track_id变量，根据此变量设置目标追踪点；
[0010]
步骤5、根据改进后的动态窗口算法，出智能体下一时刻应该选取的最佳速度对即线速度、角速度，传递给控制层作为控制指令输入，智能体进行移动，转向步骤3。
[0011]
优选的，步骤2中，利用改进后的蚁算法计算初始路径pathlist具体包括如下步骤：
[0012]
步骤2-1、初始化起始位置与终点，设置蚁算法迭代总次数n
max
，已迭代次数nc，蚂蚁数量m，信息素挥发系数ρ参数信息，对地图进行栅格化处理；
[0013]
步骤2-2、遍历所有蚂蚁，开始搜索过程，假设蚂蚁k，根据地图大小构建禁忌表，用于记录蚂蚁k已走过的位置，将蚂蚁放置于起始点根据状态转移概率公式选择下一可行栅格点，在蚂蚁移动过程中记录蚂蚁走过的路径以及路径长度，直到蚂蚁没有下一可行栅格点或者已经到达终点，此时停止搜索，假如遇到了没有下一可行栅格点即死锁情况时，检查这条死锁路径与全局最优路径的最后相交栅格点，对死锁路径进行分割，对于后半部分进行信息素惩罚，对于下一只蚂蚁k+1，令其从相交栅格点开始搜索；
[0014]
步骤2-3、当所有蚂蚁完成搜索即单次迭代结束后，对蚂蚁走过的路径根据信息素计算公式计算信息素数值，进行信息素更新；
[0015]
步骤2-4、判断迭代次数是否已经达到上限，若没有达到上限，则跳转到步骤2-2，否则结束搜索，跳转到步骤2-5；
[0016]
步骤2-5、输出算法运行中保存的最优路径信息，算法结束。
[0017]
优选的，步骤3中，若智能体可探测的范围内出现新的障碍物，则将其加入到障碍物集合obs中包括如下步骤：
[0018]
步骤3-1、智能体利用感知设备探测附近范围信息；
[0019]
步骤3-2、根据探测信息与已知的地图信息，假如探测到未知的障碍物，则将其加入到障碍物集合obs中。
[0020]
优选的，步骤4中，智能体设置目标追踪点具体包括如下步骤：
[0021]
步骤4-1、若track_id与路径终点的序号相同，则跟踪结束，返回即可；
[0022]
步骤4-2、若路径点与智能体的距离小于阈值distance_min，则视其为已到达路径点，track_id递增，这一步的目的是使路径更加平滑；若上述距离大于阈值distance_min，
则出距离自身最近的路径点point，将其设置为追踪点；
[0023]
步骤4-3、根据track_id的大小设置追踪路径点target。
[0024]
优选的，步骤5中，智能体得到下一时刻应该选用的速度具体包括如下步骤：
[0025]
步骤5-1、初始化起始位置与终点；
[0026]
步骤5-2、计算可行的速度空间，即动态窗口vr；
[0027]
步骤5-3、对于步骤5-2中得到的动态窗口vr进行采样，根据预设的速度分辨率得到速度对集合；
[0028]
步骤5-4、对于上述得到的速度对集合，根据设定的模拟周期时间遍历计算速度对对应的智能体轨迹；
[0029]
步骤5-5、考虑到目标点的位置后，基于改进后的评价函数g(v,ω)选择当前最优的轨迹以及对应的速度对(v,ω)，智能体将此速度对传递给控制层进行移动，然后跳转到步骤3。
[0030]
本发明的有益效果为：本发明公开了未知环境下基于改进蚁与动态窗口法的路径规划方法，从而实现在存在未知静态障碍物和动态障碍物的环境下进行实时的路径规划；本发明利用改进后的蚁算法，出可靠路径，当智能体的探测范围内的环境发生变化时，修改障碍物集合，并且通过动态窗口法，对静态、动态障碍物进行规避，维护全局已知地图矩阵，保障其运动的安全性，此种搜索算法有效的减少了死锁现象的发生，提高了搜索效率，同时为了使路线更加平滑，减少智能体的时间、能量消耗，设计了独特的路径跟踪策略。
附图说明
[0031]
图1为本发明的方法流程示意图。
[0032]
图2为本发明的算法流程示意图。
[0033]
图3为本发明利用matlab仿真模拟的根据环境已知信息得到的路径规划图。
[0034]
图4为本发明利用python仿真模拟的无障碍物情况下的智能体运动轨迹图。
[0035]
图5为本发明利用python仿真模拟的当智能体发现未知动态障碍物时，智能体进行避碰的细节图。
[0036]
图6为本发明利用python仿真模拟的当智能体发现未知动态障碍物时，智能体完成避碰的运动轨迹图。
具体实施方式
[0037]
如图1和图2所示，一种未知环境下基于改进蚁和动态窗口法的路径规划方法，包括如下步骤：
[0038]
步骤1、根据已知的地图信息，初始化地图矩阵map矩阵；其中，静态障碍物的区域值设为1，其他区域值设为0；初始化障碍物数组obs。
[0039]
步骤2、利用改进后的蚁算法计算初始路径pathlist，具体包括以下步骤：
[0040]
步骤2-1、初始化起始位置与终点，设置蚁算法迭代总次数n
max
，已迭代次数nc，蚂蚁数量m，信息素挥发系数ρ等参数信息，对地图进行栅格化处理；
[0041]
步骤2-2、遍历所有蚂蚁，开始搜索过程。以蚂蚁k为例，根据地图大小构建禁忌表，用于记录蚂蚁k已走过的位置，将蚂蚁放置于起始点根据状态转移概率公式选择下一可行
栅格点，状态转移概率公式如下：
[0042][0043]
越大，代表蚂蚁选择节点j的概率越大，i代表当前节点，j代表下一个节点。allowedk表示对于蚂蚁k来说，在t时刻时根据禁忌表tabuk以及地图计算出的下一步可以选择的城市集合，即与当前节点连通的节点中，不存在于禁忌表中的可去节点。其中α，β都是参数，分别表示信息素和启发式函数对选择概率的影响程度，例如当β＝0的时候，表示蚂蚁只根据信息素决定下一步去哪里。
[0044]
在蚂蚁移动过程中记录蚂蚁走过的路径以及路径长度，直到蚂蚁没有下一可行栅格点或者已经到达终点，此时停止搜索，假如遇到了没有下一可行栅格点即死锁情况时，检查这条死锁路径与全局最优路径的最后相交栅格点p，对死锁路径进行分割，分别为前部分路径l1，后半部分路径l2，对于l2段进行信息素惩罚，对于下一只蚂蚁k+1，令其从相交栅格点开始搜索。
[0045][0046]
其中，len()函数获取l2集合中元素项的数量，权重λ∈(0,1)是一个正常数，其表示惩罚因子，λ越大，对死锁导致的路径惩罚程度越重。值得注意的是，上式中的信息素惩罚为局部更新，即在本次蚂蚁结束搜索后进行。根据上式可知，死锁的路径段l2包含的路径点越多即l2越长，那么对这条路径段的惩罚越严重，根据这样的接力搜索策略，可以大大地缩短蚂蚁死锁导致的时间消耗。
[0047]
步骤2-3、当所有蚂蚁完成搜索即单次迭代结束后，对蚂蚁走过的路径根据信息素计算公式计算信息素数值，进行信息素更新；
[0048][0049][0050]
τ
ij
(t+1)＝(1-ρ)
×
τ
ij
(t)+δτ
ij
(t)
[0051]
步骤2-4、判断迭代次数是否已经达到上限，若没有达到上限，则跳转到步骤2-2，否则结束搜索，跳转到步骤2-5；
[0052]
步骤2-5、输出算法运行中保存的最优路径信息，算法结束。
[0053]
步骤3、若智能体可探测的范围内出现新的障碍物，则将其加入到障碍物集合obs中，具体包括以下步骤：
[0054]
步骤3-1、智能体利用感知设备探测附近范围信息；
[0055]
步骤3-2、根据探测信息与已知的地图信息，假如探测到未知的障碍物，则将其加
入到障碍物集合obs中。
[0056]
步骤4、智能体为每个计算得出的路径点设置序号，存入内存，记录目前追踪的全局路径上的当前路径点，存入track_id变量，根据此变量设置目标追踪点，具体包括以下步骤：
[0057]
步骤4-1、若track_id与路径终点的序号相同，则跟踪结束，返回即可；
[0058]
步骤4-2、若路径点与智能体的距离小于阈值distance_min，则视其为已到达路径点，track_id递增，这一步的目的是使路径更加平滑；若上述距离大于阈值distance_min，则出距离自身最近的路径点point，将其设置为追踪点；
[0059]
步骤4-3、根据track_id的大小设置追踪路径点target。
[0060]
步骤5、根据改进后的动态窗口算法，出智能体下一时刻应该选取的最佳速度对即线速度、角速度，传递给控制层作为控制指令输入，智能体进行移动，转向步骤3，具体步骤如下：
[0061]
步骤5-1、初始化起始位置与终点；
[0062]
步骤5-2、计算可行的速度空间，即动态窗口vr；
[0063]vm
＝{(v,ω)|v∈[v
min
,v
max
]^ω∈[ω
min
,ω
max
]}
[0064][0065][0066]vr
＝vm∩va∩vd[0067]
其中，vc，ωc为智能体当前的线速度，角速度，为智能体线速度的最大减速度和最大加速度，为智能体角速度的最大减速度和最大加速度；
[0068]
步骤5-3、对于步骤5-2中得到的动态窗口vr进行采样，根据设置的速度分辨率得到速度对集合；
[0069]
步骤5-4、对于上述得到的速度对集合，根据如下的动力学模型，遍历计算速度对对应的智能体轨迹；
[0070]
δx＝v(t)
×
δt
×
cos(θ(t))
[0071]
δy＝v(t)
×
δt
×
sin(θ(t))
[0072]
x(t+1)＝x(t)+δx
[0073]
y(t+1)＝y(t)+δy
[0074]
θ(t+1)＝θ(t)+ω(t)
×
δt
[0075]
步骤5-5、考虑到目标点的位置后，根据评价函数对速度对模拟出的轨迹进行计算，选取评价值最大的轨迹对应的速度对，然后智能体采用此速度对进行移动，跳转到步骤3，评价函数如下：
[0076]
g(v,ω)＝σ(α
×
heading(v,ω)+β
×
dist(v,ω)+γ
×
vel(v,ω))
[0077]
heading(v,ω)＝180
°‑
θ
[0078][0079]
vel(v,ω)＝abs(v)。
[0080]
以下是本发明所设计的未知环境下基于改进蚁与动态窗口法的路径规划方法的仿真验证。
[0081]
为了证明未知环境下基于改进蚁与动态窗口法的路径规划方法的可行性和有效性，本发明通过matlab与python进行仿真实验。实验中所用的参数信息如表1所示，实验结果见图3～6。
[0082]
表1参数设置表
[0083][0084]
从图3～图6中可以看到，在存在未知静态障碍物和动态障碍物的环境中，基于改进蚁与动态窗口法的路径规划方法表现良好，可以有效地在部分未知环境中避碰障碍物的同时安全地进行路径规划。
[0085]
本发明的未知环境下基于改进蚁与动态窗口法的路径规划方法，通过改进死锁现象的蚁算法，出可靠路径，当智能体的探测范围内的环境发生变化时，修改障碍物集合，同时通过动态窗口法规避部分未知环境中突然出现的静态、动态障碍物；为了使路线更加平滑，减少智能体的时间、能量消耗，设计了独特的路径跟踪策略。实验结果表明，本发明提出的路径规划方法在部分未知环境下可以良好地避碰动态、静态障碍物。

技术特征：

1.未知环境下基于改进蚁和动态窗口法的路径规划方法，其特征在于，包括如下步骤：步骤1、根据已知的地图信息，初始化地图矩阵map矩阵；其中，静态障碍物的区域值设为1，其他区域值设为0；初始化障碍物数组obs；步骤2、利用改进后的蚁算法计算初始路径pathlist；步骤3、若智能体可探测的范围内出现新的障碍物，则将其加入到障碍物集合obs中；步骤4、智能体为每个计算得出的路径点设置序号，存入内存，记录目前追踪的全局路径上的当前路径点，存入track_id变量，根据此变量设置目标追踪点；步骤5、根据改进后的动态窗口算法，出智能体下一时刻应该选取的最佳速度对即线速度、角速度，传递给控制层作为控制指令输入，智能体进行移动，转向步骤3。2.如权利要求1所述的未知环境下基于改进蚁和动态窗口法的路径规划方法，其特征在于，步骤2中，利用改进后的蚁算法计算初始路径pathlist具体包括如下步骤：步骤2-1、初始化起始位置与终点，设置蚁算法迭代总次数n
max
，已迭代次数n
c
，蚂蚁数量m，信息素挥发系数ρ参数信息，对地图进行栅格化处理；步骤2-2、遍历所有蚂蚁，开始搜索过程，假设蚂蚁k，根据地图大小构建禁忌表，用于记录蚂蚁k已走过的位置，将蚂蚁放置于起始点根据状态转移概率公式选择下一可行栅格点，在蚂蚁移动过程中记录蚂蚁走过的路径以及路径长度，直到蚂蚁没有下一可行栅格点或者已经到达终点，此时停止搜索，假如遇到了没有下一可行栅格点即死锁情况时，检查这条死锁路径与全局最优路径的最后相交栅格点，对死锁路径进行分割，对于后半部分进行信息素惩罚，对于下一只蚂蚁k+1，令其从相交栅格点开始搜索；步骤2-3、当所有蚂蚁完成搜索即单次迭代结束后，对蚂蚁走过的路径根据信息素计算公式计算信息素数值，进行信息素更新；步骤2-4、判断迭代次数是否已经达到上限，若没有达到上限，则跳转到步骤2-2，否则结束搜索，跳转到步骤2-5；步骤2-5、输出算法运行中保存的最优路径信息，算法结束。3.如权利要求1所述的未知环境下基于改进蚁和动态窗口法的路径规划方法，其特征在于，步骤3中，若智能体可探测的范围内出现新的障碍物，则将其加入到障碍物集合obs中包括如下步骤：步骤3-1、智能体利用感知设备探测附近范围信息；步骤3-2、根据探测信息与已知的地图信息，假如探测到未知的障碍物，则将其加入到障碍物集合obs中。4.如权利要求1所述的未知环境下基于改进蚁和动态窗口法的路径规划方法，其特征在于，步骤4中，智能体设置目标追踪点具体包括如下步骤：步骤4-1、若track_id与路径终点的序号相同，则跟踪结束，返回即可；步骤4-2、若路径点与智能体的距离小于阈值distance_min，则视其为已到达路径点，track_id递增，这一步的目的是使路径更加平滑；若上述距离大于阈值distance_min，则出距离自身最近的路径点point，将其设置为追踪点；步骤4-3、根据track_id的大小设置追踪路径点target。5.如权利要求1所述的未知环境下基于改进蚁和动态窗口法的路径规划方法，其特
征在于，步骤5中，智能体得到下一时刻应该选用的速度具体包括如下步骤：步骤5-1、初始化起始位置与终点；步骤5-2、计算可行的速度空间，即动态窗口v
r
；步骤5-3、对于步骤5-2中得到的动态窗口v
r
进行采样，根据预设的速度分辨率得到速度对集合；步骤5-4、对于上述得到的速度对集合，根据设定的模拟周期时间遍历计算速度对对应的智能体轨迹；步骤5-5、考虑到目标点的位置后，基于改进后的评价函数g(v,ω)选择当前最优的轨迹以及对应的速度对(v,ω)，智能体将此速度对传递给控制层进行移动，然后跳转到步骤3。

技术总结

本发明公开了一种未知环境下基于改进蚁和动态窗口法的路径规划方法，首先根据已知的环境信息构建地图矩阵；然后使用改进蚁算法计算已知信息下的最短路径，得到全局安全路径；智能体追踪全局安全路径的同时利用感知设备探测周围环境，若发现未知的静态、动态障碍物，则将其加入到障碍物集合中，通过动态窗口算法，根据评价函数挑选出最佳的轨迹进而得到对应的速度对，将其作为控制指令传递给控制层，对障碍物进行规避，让智能体在存在静态和动态障碍物的部分未知环境下进行高效的路径规划并安全规避障碍。本发明能够实现在存在未知静态障碍物和动态障碍物的环境下进行实时的路径规划。的路径规划。的路径规划。