路径规划算法初探

前言：真实世界中人类的路径规划是对记忆信息和实时感知信息综合分析的过程，在虚拟技术中属于行为控制层级的技术。

一，机器人路径规划分类：

      1.全局路径规划（环境完全已知）

      2.局部路径规划（环境未知或部分未知，通过感知实时获取环境信息）

      另外环境又分静态与动态，所以任何路径规划问题均可细分为如下四类之一：

      1)全局静态环境路径规划：构型空间法，自由空间法，栅格法（区别在于环境建模的方法不同）

构型空间法——将机器人作为质点，根据机器人的大小与姿态，将障碍物的形状向外扩展形成构型空间，然后质点在构型空间中进行搜索

自由空间法——采用预定义的基本形体，构造自由空间，并将自由空间表示为图结构，进行图搜索，构造空间图的时间复杂度很大

栅格法——顾名思义，环境被离散为一系列网格单元，场景的复杂度由网格表示，按照网格内是否有障碍物，标记为自由区和障碍区
      2)全局动态环境路径规划： 
      3)局部静态环境路径规划：人工势场法，基于模糊逻辑的路径规划，基于遗传算法的路径规划

人工势场法——目标地对机器人有引力，障碍物对机器人又斥力，环境中任何一点势力为引力与斥力的叠加，机器人从初始位置开始，沿着势场下降最快的方向达到目标点。特点：（优：便于实时控制；缺：容易陷入局部最优，导致运动死锁发生，解决办法是广义势场法，逃脱算法）

基于模糊逻辑的路径规划——顾名思义，基于多传感器信息的模糊逻辑，来模拟人驾驶规则，特点：（优：鲁棒性，适合未知，动态环境的路径规划；缺：障碍物增多，计算量很大）深入理解需查阅相关文献

基于遗传算法的路径规划——遗传算法有利于寻找全局最优点而非陷入局部最优，但是速度慢，解决办法是遗传模拟退火算法，抑制早熟

      4)局部动态环境路径规划

        机器人路径规划模型分类：1.传统模型，基于功能的要求，按照自顶向下的方式建立的。其规划过程按照“感知－建模－规划－行动”的顺序进行，是一种典型的慎思结构，稳定但缺乏应变能力；2.基于行为的路径规划模型，该模型将路径规划分解为独立模块如，避障，跟踪等，这些行为模块通过彼此的协调和竞争，共同完成导航任务优缺点与1刚好相反。

二，虚拟人路径规划

      虚拟人全局路径规划可以使用人工势场法，可以直接使用A*算法，但是更好的方法是全局+局部双层模型：

      虚拟人路径规划应该建立在全局记忆和局部感知的混合信息基础上，其规划过程是全局规划和局部规划的有机结合。

1.虚拟人路径规划模型:利用合成视觉模拟感知系统，来获取规划所需的环境信息,并采用 A*算法进行全局路径搜索——双层算法：局部感知+全局寻优

       路径规划行为的整体结构是基于功能实现的，按照“感知－建模－规划－执行”的过程依次进行。但在路径规划模型内部，全局和局部两个规划器之间是按照基于行为的结构组织的，每个规划器都可以对虚拟人的执行系统进行控制，两者通过竞争和协作实现路径规划和躲避障碍的功能。这种复合的结构使得虚拟人在运动过程中既具有基于功能控制的理性特点，又具有基于行为控制的快速反应能力。
                              

路径规划模型从感知系统中获取的感知和记忆信息可分为两类：一类是经记忆模型淘汰后保存下的长期信息，这些记忆是虚拟人经过观察对环境形成的较为静态的信息，反映了环境中静态障碍的分布。另一类是感知到的短期信息，反映了环境中的动态障碍和未知障碍。
 
当虚拟人开始一次漫游时，首先全局规划器根据已有的长期信息进行全局静态规划，确定虚拟人应该经过的最优化路线。然后全局规划器控制执行系统按照该路径运动。在运动过程中，感知系统会持续对周围环境进行感知。当发现动态的物体或未知障碍时，局部规划器根据这些感知到的局部信息，确定短期內的运动。当避障行为的优先级高于沿原路径前进时，局部规划器就能够通过竞争获得执行系统的控制权，使得虚拟人按照局部规划结果运动。完成对当前感知障碍的规避行为后，全局规划器再次取得执行系统的控制权，使得虚拟人重新回到全局规划路径上，继续向目标点运动。
 
从以上对过程的分析可见，事实上局部规划器是采用局部规划技术实现规避动态或未知障碍的功能。尽管大部分研究都简单地通过改变运动路线来规避障碍，但这并不是唯一的手段。通过改变运动状态（如加速、减速）同样可以实现
避障行为。因此，局部规划器并不仅仅实现路径的规划，而是对包括路径在内的运动状态的规划。
 

2.环境建模方法:

         1）.切线图表示二维空间，八叉树表示三维空间（通过A*算法进行空间路径搜索，全局）

         2）.虚拟人的漫游行为一般是平面运动，我们将已知障碍的包围盒投影到二维平面上，采用离散网格的方法表示空间，以加快规划速度，首先采用膨胀法建立位姿空间，然后用四叉树的存储形式保存离散化的位姿空间。

        虚拟人路径规划策略就转化为在四叉树中寻找两个叶子结点间最短路径的方法，

3.通过A*算法进行空间路径搜索，全局，关于A*算法可以参考博客：堪称最好的A*算法

A*关键之处1为代价函数-启发式函数，2为程序实现中的open与closed集的数据结构，3为算法的改进

f = g+h,其中g为是起始点到点n的实际花费，h只是一个估计值，估计从n点到终点的最优路径的花费，当h收敛到一个极值时，A*算法有最优值，（h是启发函数）h->h*,当启发函数为h*时，得到的路径正好为最优路径

引用“堪称最好的A*算法”

（

• 一种极端情况，如果h(n)是0，则只有g(n)起作用，此时A*演变成Dijkstra算法，这保证能找到最短路径。
• 如果h(n)经常都比从n移动到目标的实际代价小（或者相等），则A*保证能找到一条最短路径。h(n)越小，A*扩展的结点越多，运行就得越慢。
• 如果h(n)精确地等于从n移动到目标的代价，则A*将会仅仅寻找最佳路径而不扩展别的任何结点，这会运行得非常快。尽管这不可能在所有情况下发生，你仍可以在一些特殊情况下让它们精确地相等（译者：指让h(n)精确地等于实际值）。只要提供完美的信息，A*会运行得很完美，认识这一点很好。
• 如果h(n)有时比从n移动到目标的实际代价高，则A*不能保证找到一条最短路径，但它运行得更快。
• 另一种极端情况，如果h(n)比g(n)大很多，则只有h(n)起作用，A*演变成BFS算法。
• 关键是我们的目的是什么是速度还是准确度

）

A*算法核心思路（从百度百科看到的）

While(OPEN!=NULL)

{

    从OPEN表中取估价值f最小的节点n;

   if(n节点==目标节点) break;

else

{

   if(X in OPEN) 比较两个X的估价值f //注意是同一个节点的两个不同路径的估价值

   if( X的估价值小于OPEN表的估价值 )

    更新OPEN表中的估价值; //取最小路径的估价值

   if(X in CLOSE) 比较两个X的估价值 //注意是同一个节点的两个不同路径的估价值

   if( X的估价值小于CLOSE表的估价值 )

    更新CLOSE表中的估价值; 把X节点放入OPEN //取最小路径的估价值

   if(X not in both)

    求X的估价值;

    并将X插入OPEN表中; //还没有排序

}

4.局部路径规划算法——有两个重要的概念点：

其一：碰撞检测，几何思路求交点，在实际应用中，碰撞预测工作由世界模型进行，然后将预测碰撞时间作为结果发送给两个对象，以降低整体计算量。

其二：碰撞避免方法——关键在于运动代价函数的选取（存在着一种潜在的优化规则，即如果沿某个方向运动，所需要付出的“代价”最小。）

        对于静态障碍，虚拟人只能通过改变运动方向来避免碰撞（代价函数为Eo）。

        对于运动障碍，则通过改变方向和运动速度(代价函数为Ev)都可能实现规避。最简单的选择策略是采用随机的方式。而更为合理的方式则是通过比较两者的代价函数最小值，选择其中代价较小的方法进行规避。通过调节Eo与Ev中的权重值，虚拟人会表现出不同的选择倾向。还可以加入视域的概念来进行碰撞检测。

当运动障碍是另一个虚拟人时，如果两者同时采取规避措施，可能会造成规避路线的振荡。一般的解决方法是采用随机选择或优先级的方式，保证只有其中一个进行规避；更为高级的思路是：1.每个虚拟人的感知范围是不同
的，因此两者不会同时预测到碰撞的发生，最先发现对方的虚拟人可能会先行规避。2.通过评价函数中各个权值的个性化，虚拟人会表现出不同的行为特征，赋予不同的碰撞代价函数权值，两者之中具有较小碰撞代价函数权值的虚拟人会更为谨慎，在较远的位置采取规避。