基于终身规划A*和速度障碍法的路径规划方法与流程

基于终身规划a*和速度障碍法的路径规划方法
技术领域：
1.本发明属于智能路径规划技术领域，尤其涉及基于终身规划a*和速度障碍法的路径规划方法。


背景技术：

2.随着自动化、计算机等各种技术的不断发展和应用，智能体技术逐渐发展，结构日趋复杂，功能更加强大，智能体在工厂、物流、交通、智能电网等领域具有广泛应用。智能体在工作过程中由于外界环境变化原因，若不能及时规划新的路径并进行安全的跟踪，可能会发生碰撞，最终造成严重后果，移动智能体的路径规划技术逐渐成为研究热点问题。所谓移动智能体路径规划技术，就是智能体根据自身传感器对环境的感知，自行规划出一条安全的运行路线，同时高效完成作业任务。移动智能体路径规划主要解决3个问题:1)使得智能体可以从起始点到达目标点；2)使得智能体可以安全地避开环境中的障碍物，完成相应的作业任务；3)在完成前两项任务的前提下，优化生成的轨迹使其尽量平滑。智能体路径规划技术是移动智能体研究的核心内容之一，路径规划技术最早可以追溯到1959年dijkstra提出的基于网格地图的路径规划法，迄今为止，己有大量的研究成果报道。文献(戴博,肖晓明,蔡自兴.移动机器人路径规划技术的研究现状与展望[j].控制工程,2005(03):198-202.)将移动智能体路径规划方法，根据智能体对环境感知的角度，分为3种类型:基于环境模型的规划方法、基于事例学习的规划方法和基于行为的路径规划方法；从智能体路径规划的目标范围看，又可分为全局路径规划和局部路径规划；从规划环境是否随时间变化方面看，还可分为静态路径规划和动态路径规划。
[0003]
传统的图搜索方法对于频繁发生变化的地图环境难以做到快速求解新的路径，需要重新考虑所有节点从起点进行规划，增量式搜索算法避免了上述方法的局限性且具有良好的可拓展性，因而得到广泛研究。现有的技术中，智能体通常沿着一条计算出的路径进行移动，当障碍物出现在路径上时候，智能体需要实时地改变移动路线，实现安全移动。终身规划a*由sven koenig和maxim likhachev在2004年提出，采用了启发式集中搜索从而在动态环境中找到从开始节点到目标节点的最短路径。此外，终身规划a*利用增量技术，重新利用以前的搜索得来的信息来寻找新的路径，而不需要重新运行算法，减少了需要重新计算的节点的数量。但终身规划a*算法的搜索效率对障碍物的速度很敏感，在处理复杂的安全路径规划问题上具有局限性。


技术实现要素：

[0004]
本发明为了克服在部分未知环境下智能体的安全路径规划问题，提供基于终身规划a*和速度障碍法的路径规划方法，以期能安全可靠地在存在未知静态障碍物和动态障碍物的环境下进行实时的路径规划。本发明利用终身规划a*算法，以增量式搜索的方式重用之前的路径状态，找出可靠路径，当智能体的探测范围内的环境发生变化时，修改受影响的栅格节点状态，减少运算开销，并且通过速度障碍法，对静态、动态障碍物进行规避，维护全
局已知地图矩阵，保障其运动的安全性。此种搜索算法通过使用先前搜索的经验信息来加速搜索，在目标节点没有改变的情况下，可以提高搜索效率。同时为了使路线更加平滑，减少智能体的时间、能量消耗，设计了独特的路径跟踪策略。
[0005]
基于终身规划a*和速度障碍法的路径规划方法，包括以下步骤：
[0006]
步骤1.根据部分已知的地图信息进行栅格化，初始化环境栅格图map二维数组；其中，静态障碍物的区域值设为1，其他区域值设为0；
[0007]
步骤2.利用终身规划a*算法计算初始路径path；
[0008]
步骤3.智能体根据路径追踪策略进行追踪；每个计算得出的路径点设置序号被存入内存，将目前追踪的全局路径上的当前路径点存入track_id变量，同时根据此变量设置追踪路径点；
[0009]
步骤4.根据速度障碍算法，找出智能体下一时刻的运动速度，规避动态障碍物、静态障碍物；
[0010]
步骤5.根据步骤4计算的运动速度，智能体进行移动；
[0011]
步骤6.若探测范围内没有出现新的静态障碍物，则转向步骤3；若出现新的静态障碍物，则更新二维数组map，根据步骤2中终身规划a*算法的更新算法，调整受到影响的所有路径点，重新计算全局路径。
[0012]
进一步地，步骤2中利用终身规划a*算法计算初始路径path具体构建方法为：
[0013]
步骤2-1.初始化，设置地图大小，距离类型；
[0014]
步骤2-2.设置起始节点的rhs值、g值，设其rhs初始值为0，g值为无穷大。设置其他节点的rhs值、g值均为无穷大。构建优先队列u，将起始节点其加入优先队列；
[0015]
步骤2-3.从优先队列中获取节点，当优先队列u中优先级最小的节点小于目标点的优先级或者目标点的rhs值与g值不等，即局部不一致时，弹出最优节点x；否则转向步骤2-7；
[0016]
步骤2-4.如果当前节点x的g值大于rhs，即局部过一致，则令当前节点的g(x)＝rhs(x)；否则，g(x)为无穷大：
[0017][0018]
并且更新节点x，更新方法：根据节点x的前驱节点计算x节点的rhs值；
[0019]
rhs(x)＝mins′
∈pred(x)
(g(s
′
) c(s
′
，x))
[0020]
其中，pred(x)是x的前驱节点集合。如果节点x在优先队列u中，从u中删除x；如果节点x的rhs值和g值不等，计算x的优先级，同时将其加入u中；
[0021]
步骤2-5.对于节点x的所有后继节点集合successors中的节点，利用步骤2-4中的更新方法进行更新；
[0022]
步骤2-6.从终点开始计算，从与当前节点之间没有障碍物的邻接节点集合中，找到g值最小的节点，将其加入路径中，迭代之后，得到全局路径；
[0023]
进一步地，步骤3中，智能体进行轨迹跟踪的方法如下：
[0024]
步骤3-1.若track_id与路径终点的序号相同，则跟踪结束，返回即可；
[0025]
步骤3-2.若路径点与智能体的距离小于阈值distance_min，则视其为已到达路径点，track_id递增，这一步的目的是使路径更加平滑；若上述距离大于阈值distance_max，
则找出距离自身最近的路径点point，将其设置为追踪点。
[0026]
步骤3-3.根据track_id的大小设置追踪路径点target；
[0027]
进一步地，步骤4中，找出下一时刻计算智能体的运动速度的步骤具体为：
[0028]
步骤4-1.设置安全参数为μ，新建一个候选速度集合v_set，在约束的速度范围内，随机产生v
cand_num
项速度并且加入此集合。具体的，将速度设置为两个属性：方向与速度大小，利用服从于均匀分布的random.randint函数随机产生方向与速度大小，如下式所示：
[0029]
v＝(v，θ)，v
min
＜v＜v
max
，θ
min
＜θ＜θ
max
[0030]
其中，v
min
，v
max
分别为最小、最大速率，θ
min
，θ
max
分别为最小、最大航向角。
[0031]
步骤4-2.根据上述步骤中的速度集合，将其排序，从最小值顺序选取速度，根据此速度计算所有的动态障碍物、静态障碍物与智能体的预测碰撞时间，若智能体与其均不会发生碰撞，则碰撞时间设置为∞，否则记下最小预测碰撞时间t；最后根据下式计算惩罚项以及理想速度：
[0032][0033][0034]
其中，μ为安全参数，v
pre
为期望速度，v
cand
为候选速度，tar为追踪路径点位置，position为智能体位置，||v
max
||为智能体最大速度；
[0035]
步骤4-3.当速度候选集合已经全部遍历完成时，下一时刻智能体的速度被设置为使得惩罚项最小的速度；若还未完成，则跳转到步骤4。
[0036]
相对于现有技术，本发明的优点如下：
[0037]
(1)本发明公开了基于终身规划a*和速度障碍法的路径规划方法，从而实现在存在未知静态障碍物和动态障碍物的环境下进行实时的路径规划；
[0038]
(2)本发明利用终身规划a*算法，以增量式搜索的方式重用之前的路径状态，找出可靠路径，当智能体的探测范围内的环境发生变化时，修改受影响的栅格节点状态，减少运算开销，并且通过速度障碍法，对静态、动态障碍物进行规避，维护全局已知地图矩阵，保障其运动的安全性。此种搜索算法通过使用先前搜索的经验信息来加速搜索，在目标节点没有改变的情况下，可以提高搜索效率。同时为了使路线更加平滑，减少智能体的时间、能量消耗，设计了独特的路径跟踪策略。
附图说明
[0039]
图1是本发明基于终身规划a*和速度障碍法的路径规划方法的算法步骤图；
[0040]
图2是本发明的算法流程图；
[0041]
图3是本发明利用python仿真模拟的已知环境下的路径规划图；
[0042]
图4是本发明利用python仿真模拟的部分未知环境存在障碍物情况下的路线规划图；
[0043]
图5是环境发生变化，障碍物减少情况下的路线规划图；
[0044]
图6是地形改变后，新环境下的路径规划图；
[0045]
图7是地形改变后且增加障碍物后的路径规划图；
[0046]
图8是图7情况下，障碍物再次变化后的路径规划图；
[0047]
具体实施方法
[0048]
实施例1：下面将结合附图就本发明的发明目的、技术方案、发明优点作进一步详细说明。
[0049]
本发明首先使用终身规划a*算法，找出可靠路径，当环境发生变化时，修改已知地图矩阵，利用终身规划a*算法中的更新方法，对受影响的栅格点进行更新，重用之前规划信息从而得到新的路径，降低运算消耗。通过速度障碍法，规避出现的障碍物；同时通过路径跟踪策略使得移动路线更加平滑，减少智能体的时间、能量消耗。
[0050]
如图1-2所示，本发明在部分未知环境下基于终身规划a*和速度障碍法的路径规划方法，其方法具体执行方法如下：
[0051]
步骤1.根据已知的地图信息进行栅格化，初始化环境栅格图map二维数组；其中，静态障碍物的区域值设为1，其他区域值设为0；
[0052]
步骤2.利用终身规划a*算法计算初始路径path，具体包括以下步骤：
[0053]
步骤2-1.初始化，设置地图大小，距离类型；
[0054]
步骤2-2.设置起始节点的rhs值、g值，设其rhs初始值为0，g值为1e10。设置其他节点的rhs值、g值均为无穷大，取1e10。构建优先队列u，将起始节点其加入优先队列；
[0055]
步骤2-3.从优先队列中获取节点，当优先队列u中优先级最小的节点小于目标点的优先级或者目标点的rhs值与g值不等，即局部不一致时，弹出k值最小的节点即最优节点x；否则转向步骤2-7；
[0056]
步骤2-4.如果当前节点x的g值大于rhs，即局部过一致，则令当前节点的g(x)＝rhs(x)；否则，g(x)为1e10:
[0057][0058]
并且更新节点x，更新方法：根据节点x的前驱节点计算x节点的rhs值；
[0059]
rhs(x)＝min
s'∈pred(x)
(g(s') c(s',x))
[0060]
其中，pred(x)是x的前驱节点集合。如果节点x在优先队列u中，从u中删除x；如果节点x的rhs值和g值不等，计算x的优先级，同时将其加入u中；
[0061]
步骤2-5.对于节点x的所有后继节点集合successors中的节点，利用步骤2-4中的更新方法进行更新；
[0062]
步骤2-6.从终点开始计算，从与当前节点之间没有障碍物的邻接节点集合中，找到g值最小的节点，将其加入路径中，迭代之后，得到全局路径；
[0063]
步骤3.智能体根据路径追踪策略进行追踪；每个计算得出的路径点设置序号后被存入内存，将目前追踪的全局路径上的当前路径点存入track_id变量；其中智能体根据路径追踪策略进行追踪具体包括以下步骤：
[0064]
步骤3-1.若track_id与路径终点的序号相同，则跟踪结束，返回即可；
[0065]
步骤3-2.若路径点与智能体的距离小于阈值distance_min，则视其为已到达路径点，track_id递增，这一步的目的是使路径更加平滑；若上述距离大于阈值distance_max，则找出距离自身最近的路径点point，将其设置为追踪点。
[0066]
步骤3-3.根据track_id的大小设置追踪路径点target；
[0067]
步骤4.根据速度障碍算法，找出下一时刻智能体的运动速度，具体包括以下步骤：
[0068]
步骤4-1.设置安全参数为μ，新建一个候选速度集合v_set，在约束的速度范围内，随机产生v
cand_num
项速度并且加入此集合。具体的，将速度设置为两个属性：方向与速度大小，利用服从于均匀分布的random.randint函数随机产生方向与速度大小，如下式所示：
[0069]
v＝(v，θ)，v
min
＜v＜v
max
，θ
min
＜θ＜θ
max
[0070]
其中，v
min
，v
max
分别为最小、最大速率，θ
min
，θ
max
分别为最小、最大航向角。
[0071]
步骤4-2.根据上述步骤中的速度集合，将其排序，从最小值顺序选取速度，根据此速度计算所有的动态障碍物、静态障碍物与智能体的预测碰撞时间，若智能体与其均不会发生碰撞，则碰撞时间设置为1e10，否则记下最小预测碰撞时间t；最后根据下式计算惩罚项以及理想速度：
[0072][0073][0074]
其中，μ为安全参数，v
pre
为期望速度，v
cand
为候选速度，tar为追踪路径点位置，position为智能体位置，具体实现中用二维数组存储，||v
max
||为智能体的最大速度；
[0075]
步骤4-3.若速度候选集合已经全部遍历完成，则从中选取惩罚项最小的速度作为下一时刻智能体的速度；若还未完成，则跳转到步骤4；
[0076]
步骤5.根据步骤4所算出来的速度，智能体进行移动；
[0077]
步骤6.若智能体可探测的范围内没有出现新的静态障碍物，则转向步骤3；若可探测的范围内出现新的障碍物，则更新二维数组map，根据步骤2中终身规划a*算法的更新算法，调整受到影响的所有栅格点，根据新的起始节点计算全局路径。
[0078]
以下是本发明所设计的基于终身规划a*和速度障碍法的路径规划方法仿真验证。
[0079]
为了证明基于终身规划a*和速度障碍法的路径规划方法的可行性和有效性，本发明通过python进行仿真实验。实验中所用的参数信息如表1所示，实验结果见图1～5。
[0080][0081]
表1参数设置
[0082]
如图3-8所示，实心圆形表示起始点，五角星表示终点。可以看出，基于终身规划a*和速度障碍法的路径规划方法表现良好，可以有效地在部分未知环境中避碰障碍物的同时安全地进行路径规划。
[0083]
本发明的基于终身规划a*和速度障碍法的路径规划方法，通过增量式搜索算法，重用之前的路径状态，找出可靠路径，当智能体的探测范围内的环境发生变化时，修改地图矩阵中受影响的栅格节点状态，此种搜索算法通过使用先前搜索的经验信息来加速搜索，在目标节点没有改变的情况下，搜索效率更高。同时通过速度障碍法，规避部分未知环境中突然出现的动态障碍物；为了使路线更加平滑，减少智能体的时间、能量消耗，设计了独特的路径跟踪策略。实验结果表明，本发明提出的路径规划方法在部分未知环境下可以良好地避碰动态、静态障碍物。
[0084]
以上仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明创造的保护范围之内。