混合A*算法研究

目的

　　笔者看了一些介绍“混合A星”的博客，写的比较业余和肤浅。本文从专业的角度探讨一下无人车运动规划中著名的混合A星方法底层原理和代码实现。混合A星的应用包括泊车、取车（例如特斯拉的Smart Summon，如下右图），也可以用于低速清扫车的运动规划。

1　开源代码

　　据我所知，网上的开源混合A星算法代码有下面几个（还有一些matlab、mathematica、python版本笔者未包含）。通过分析他们的代码我们可以取长补短，也欢迎大家补充。
　　1　百度Apollo无人驾驶项目中的开放场地轨迹规划模块open_space；
　　2　瑞典老外的硕士论文项目：karl kurzer，本文也以它为例进行代码解剖；
　　3　ROS Navigation2的插件SmacPlannerHybrid，这个知名度比较低，但是较新，代码进行了优化；
　　4　Habrador以特斯拉为例用C#写的：Self-driving-vehicle；

2　基本思想

　　混合A星方法（Hybrid A*）由斯坦福大学的Dmitri Dolgov, Sebastian Thrun等人在2008年的会议论文《Practical Search Techniques in Path Planning for Autonomous Driving》中首次提出，后来又在此基础上补充了一些细节发表在机器人领域的顶级期刊 International Journal of Robotics Reaserch 上。狭义的混合A星只包含搜索的过程，广义的混合A星把后处理也加上了。所以混合A星可以看成是由几个模块组合起来的。混合A星适用于有运动约束的机器人，最典型的例子就是各种轮式移动机器人，比如汽车。
　　单词“Hybrid”有“杂交、混血”之意。顾名思义，混合A星可以看成是探索树方法和A星算法的混血儿。这两种方法在机器人领域都是家喻户晓的大明星，所以他们结个婚生个孩子也很自然，毕竟组合创新是最容易的创新之一。拙劣的创新是生硬的模仿，但是混合A星方法组合的就比较巧妙，把爸妈的优点都继承了。什么优点呢，笔者后面会仔细分析。总之，它们的孩子也出名了，甚至最新版的Matlab居然都内置了混合A星函数：plannerHybridAStar，想体验的读者可以玩玩，但是Matlab使用了占据栅格地图进行碰撞检测。
　　混合A星算法的思想比较简单，为了让入门者更容易理解，先介绍它的明星父母：探索树方法和A星算法。探索树方法也是一个比较通用的方法，有好多变种，最有名的就是RRT（快速探索随机树）方法。
　　A星算法也是非常有名，它其实也是生长一颗树，从起点开始不断向外探索，直到找到目标点停止生长。这么描述听起来好像跟Dijkstra方法一样，只不过A星方法更“聪明”。Dijkstra方法比较笨，它只知道傻傻地一个个生长最外面的所有节点。A星就知道哪些节点离目标更近，它会优先生长更近的节点，所以A星比Dijkstra方法快。

3　启发信息

　　启发信息（heuristics）非常重要，所以笔者不免多说几句。我们知道，A星算法是Dijkstra算法的升级版，那么A星算法比Dijkstra算法厉害在哪里呢？它厉害就厉害在使用了启发信息上。启发信息对于新手来说可能又是一个难理解的抽象概念。想象一下，人生中迷茫不知所措的时候别人给你的建议或者指点，这就是一种启发，让你找到出路。笔者大胆改编一下培根的名言：信息就是力量。当然，它既然叫启发那就意味着它提供的信息是有帮助的，如果有人给你指了一条错误的路，那就不叫启发了。所以说启发信息这个东西也不是随随便便就能设计出来的，多少有点技巧，这也是为什么常用的启发信息就那么几个。
　　启发信息对于混合A星算法同样至关重要，所以也是作者着重讨论的。混合A星算法使用了两种启发信息的组合，这两种启发一种“考虑机器人运动约束不考虑环境障碍物”，一种刚好相反“不考虑运动约束考虑障碍物”，如下图所示。这个“考虑运动约束不考虑障碍物”的启发信息就是大名鼎鼎的Reeds-Shepp曲线的长度，“不考虑运动约束考虑障碍物”的启发信息就是传统A星算法搜索到目标的路径的长度。混合A星算法选择两者的最大值作为最终的启发信息。为什么要使用这两种启发信息？作者是这样解释的，使用第一种（“考虑约束不考虑障碍物”）是为了防止机器人从错误的方向到达目标，作者说实际试验中使用这种启发比不使用的效率提高了一个数量级，看来是必不可少了。使用第二种（“考虑障碍物不考虑约束”）是为了防止在断头路或者U型障碍物里浪费时间。有没有同时既考虑运动约束也考虑障碍物的启发信息呢？目前还没有。

　　启发信息唯一的作用是给搜索算法提供指引、让算法更“聪明”，从而优先搜索有可能产生解的区域，不要在没必要的区域浪费时间。因为搜索的范围越多计算量越大效率越低，所以启发信息提供的指导越准确，最终算法的效率就越高。启发信息既然只是提供指导，那么它的计算量就不能太大，不能喧宾夺主，要是计算量太大就没有意义了，还是用人生迷茫的例子解释，别人提供建议太复杂了。启发信息最好是很快就能算出来的，例如A星使用的“到目标的欧式距离”，混合A星使用的Reeds-Shepp曲线的长度和A星得到的最优解不是非常快（跟欧式距离比），但是还是可以接受的。作者也发现了，Reeds-Shepp曲线和A星得到的最优解这两个启发信息既然不依赖障碍物，那就可以提前把所有离散节点的启发信息一次性都算好存起来，然后在搜索过程中直接查询，这种用空间换时间的策略也是一种解决办法。启发信息必须满足一些要求，最重要的一个要求是，不能高估，可以低估。低估的意思是启发信息的大小比实际解的值要小，即便你不知道实际解的值（比如最短距离），但是你可以估计它的下限。这里采用的两个启发信息都是低估，所以是合理的，从两个低估里取最大值当然还是低估，所以仍然是合理的。
　　作者还表示，这么定义的启发信息跟混合A星其实没什么关系（不依赖混合A星的状态）。也就是说，你可以用在其它搜索算法上（随便用不要钱），当然你要是找到更牛逼的启发信息也可以换成你自己的。所以，启发信息是个可以替换的模块，跟混合A星算法不是绑定的，你觉得好就用，有更好的也可以换，当然你要是彻底不用那混合A星算法就发挥不出它的威力了。

4　栅格

　　栅格就是对空间的一种划分，当然不是唯一的划分方式。栅格在混合A星方法中起到什么作用呢，或者说为什么要使用栅格？仅仅是为了继承A星的特点吗。如果是这样的话，那么栅格并不是必须的。为什么呢？因为每次的扩展都会产生有限的子节点，同理子节点的子节点也是有限个的，所以直接对子节点应用A星搜索的代价计算方法和选择方法就可以了。所以混合A星使用栅格真正的目的是限制节点对空间的覆盖。

4.1　共占栅格问题

　　在扩展子节点的时候，如果移动的距离比栅格的范围小，那就有可能与自己的父节点位于同一个栅格，下图展示了这种情况。

　　在这种情况下，子节点$x_{succ}$的代价值永远大于父节点$x$的代价值，由于$A^{\ast }$算法总是优先挑选代价值小的节点，所以后果就是子节点$x_{succ}$永远不会被选中。为什么会这样呢？还是看上图，假设父节点$x$的已用代价是 g ( x ) g(x)