混合A*算法研究

目的　
　　看了一些介绍“混合A星”的博客，写的都太业余了。本文从专业的角度探讨一下无人车运动规划中著名的混合A星（Hybrid A Star）方法，首先我们最关心的两个问题是：
　　● 混合A星适用于什么场景？
　　存在障碍物的环境，低速、有运动约束的机器人或无人车。在无人驾驶上具体可以用于停车场的自动泊车的路径规划。
　　这里的关键词是“约束”，如果你看到有人把混合A星用在差速机器人或者全向机器人上，那你就可以得出判断：使用者不懂混合A星。因为，如果机器人没有运动约束，那么完全没有必要用混合A星。
　　● 混合A星的缺点是什么？
　　知己知彼才能用好，混合A星一个比较大的问题是，它输出的路径质量一般比较差，这里差的意思是指路径包含一些不必要的拐弯或者倒车动作，这就是为什么一般在混合A星之后再优化一下，或者把混合A星当成一个给优化算法提供初始路径的子模块，而不是单独使用。
　　还有，混合A星也是一种依赖分辨率的方法，受制于分辨率，它要么有可能找不到解，要么花的时间太多，实时性不好。
　　
1　混合A星的思想

　　混合A星算法是2008年斯坦福的Dmitri Dolgov, Sebastian Thrun等人在论文《Practical Search Techniques in Path Planning for Autonomous Driving》中首次提出的，后来又补充了一些细节发表在顶级期刊International Journal of Robotics Reaserch上。狭义的混合A星只包含搜索的过程，广义的混合A星把后处理也加上了。所以混合A星可以看成是由几个模块组合起来的。路径规划的三大门派：图搜索法、随机采样法、势场法全部在这篇文章中现身了。
　　混合A星可以看成是探索树方法和A星算法的混血，这两种方法在机器人领域都是大名鼎鼎，所以结个婚生个孩子也很正常，毕竟组合创新是最容易的创新之一。就连Matlab居然都内置了混合A星算法：plannerHybridAStar，可见这个算法也出名了，但是Matlab使用了占据栅格地图进行碰撞检测。混合A星适合有运动约束的机器人，最典型的就是汽车。
　　混合A星算法的思想比较简单，为了让入门者更容易理解，我先介绍它的爸爸妈妈：探索树方法和A星算法。探索树方法也是一个比较通用的方法，有好多变种，最有名的就是RRT（快速探索随机树）方法，如下面的左图所示。
　　A星算法也是非常有名，它其实也是生长一颗树，从起点开始不断向外探索，直到找到目标点停止生长。这么描述听起来好像跟Dijkstra方法一样，只不过A星方法更“聪明”。Dijkstra方法比较笨，它只知道傻傻地一个个生长最外面的所有节点。A星就知道哪些节点离目标更近，它会优先生长更近的节点，所以A星比Dijkstra方法快。

1.1　启发信息

　　启发信息（heuristics）对于混合A星算法至关重要，也是作者着重讨论的。混合A星算法使用了两种启发信息的组合，这两种启发一种“考虑运动约束不考虑障碍物”，一种刚好相反“不考虑运动约束考虑障碍物”，如下图所示。这个“考虑约束不考虑障碍物”的启发就是Reeds-Shepp曲线的长度，“不考虑约束考虑障碍物”的启发就是传统A星算法搜索到目标的路径的长度。混合A星算法选择两者的最大值作为最终的启发信息。为什么要使用两种启发信息？作者是这样解释的，使用第一种（“考虑约束不考虑障碍物”）是为了防止机器人从错误的方向到达目标，作者说实际试验中使用这种启发比不使用的效率提高了一个数量级，看来是必不可少了。使用第二种（“考虑障碍物不考虑约束”）是为了防止在断头路或者U型障碍物里浪费时间。

　　启发信息唯一的作用是给搜索算法提供指引、让算法更“聪明”，从而优先探索有可能产生解的区域，不要在没必要的区域浪费时间。因为我们的探索过程计算量可能比较大，所以启发信息提供的指导越准确，最终算法的效率就越高。启发信息既然只是提供指导，那么它的计算量就不能太大，最好是很快就能算出来的，例如A星使用的到目标的欧式距离，混合A星使用的Reeds-Shepp曲线的长度和A星的结果不是非常快（跟欧式距离比），但是还是可以接受的。作者也发现了，Reeds-Shepp曲线这一启发信息既然不依赖障碍物，那就可以提前把所有离散节点的长度一次性都算出了，然后用的时候直接查询，用空间换时间的方法显然就快多了。对启发信息的一个要求是，不能高估，可以低估。这里采用的两个启发信息都是低估，所以是合理的，最终取最大值当然也是低估。
　　作者还表示，这么定义的启发信息跟混合A星算法其实没什么关系（不依赖混合A星的状态）。也就是说，你可以用在其它搜索算法上（随便用不要钱），当然你要是有更牛逼的启发信息也可以把它换成你自己的。所以，启发信息是个可以替换的模块，跟混合A星算法不是绑定的，你觉得好就用，有更好的也可以换，当然你要是彻底不用那混合A星算法就发挥不出它的威力了。

 3　混合A星算法的缺点

　　需要设置的参数有：
　　机器人控制量采样值
　　空间离散的分辨率。空间离散的分辨率如何选择呢？机器人每次运动的距离如何设置呢，显然与空间分辨率有关，如果分辨率太小，计算量就太大；如果分辨率太大，机器人运动距离太小，那么每次生成的节点都在一个栅格里，探索树就没办法扩展。
　　启发函数如何选取？
　　论文中讨论了几种启发函数，一种是考虑约束但不考虑障碍物的RS曲线的长度，一种是不考虑约束但是考虑障碍物的普通A星算法搜索出来的路径长度。

3　混合A星算法的改进

　　1 双向探索，即不仅在起点向终点扩展，也从终点向起点扩展。这在终点障碍物密集时比较有效。
　　2 变分辨率，不使用均匀一致的分辨率，在障碍物附近使用较高的分辨率，原理障碍物使用低分辨率，减少不必要的计算。
　　3 轨迹片段仿真使用连续变化的控制量。

4　混合A星算法实现

　　混合A星算法实现采用的是Github上 karlkurzer 的版本，下载和编译过程如下。新建并运行脚本即可实现傻瓜式一键安装，非常简单。

#!/bin/bash
mkdir -p ~/HybridAStar/src
cd ~/HybridAStar/src
git clone https://github.com/karlkurzer/path_planner.git
cd .. 
catkin_make
source devel/setup.bash
rospack profile
roslaunch hybrid_astar manual.launch

4.1　碰撞检测

　　碰撞检测是离散的方法，将机器人的外形（长方形）分成一个个小方格，每个方格依次判断是否在障碍物中，如果有一个在那就意味着发生了碰撞。
　　具体的碰撞检测包含以下几步：
　　首先，如何从机器人的外形得到它的离散方格？作者使用的是论文《A Fast Voxel Traversal Algorithm for Ray Tracing》中的方法，代码是https://github.com/francisengelmann/fast_voxel_traversal/blob/master/main.cpp。该方法比Bresenham方法更好。一个展示动画在这里：点击我（A Fast Voxel Traversal Algorithm (shadertoy.com)）。

 
2　代码优化

 　　源程序有几个严重的缺点，首先分辨率定义混乱。A* 搜索有自己的离散分辨率，而碰撞检测采用占据栅格法，栅格也有自己的分辨率，一般这两个是不一样的。而源程序定义为一样的，这显然不合理。其次，连接目标使用的是Dubins曲线，这对无人车来说显然是不合理的。修改后的运行效果如下图所示，其中黑色物体表示障碍物，红色箭头表示起始的无人车位姿，绿色的则表示目标位姿。可见，大多数情况能得到比较好的路径，但是有时得到的路径质量也很差，有时甚至失败（侧方位停车的时候）。例子中使用的地图分辨率是10cm，A* 搜索分辨率是1m，角度离散为144个（2.5°）。

　　原文件使用的全局坐标系名称是path，我们改成更常用的map。在path.h文件里修改，如下： 

path.header.frame_id = "map";

　　发布的路径话题名称是/path，路径在rviz中显示没有问题，但是如果打印出来就会发现消息头多了一个，如下图所示。原因是 path.cpp 文件中的Path::clear()函数里的path.poses.clear()语句导致，把它放到下面的Path::updatePath函数中即可。

3　前进后退

 混合A星算法输出的是一条路径，该路径由一连串离散的坐标点组成。如果你想在路径基础上添加速度构成一条轨迹，可以这么做：前进时设置速度为正值，后退时设置速度为负值，在前进和后退相接的点设置速度为0。这样做需要判断什么时候是前进，什么时候是后退。判断方法是计算机器人航向角θ 与相邻路径点构成的向量θ的夹角Δ θ。涉及到角度要小心，需要保证角度的范围。为此定义正则函数modpipi，其功能是将角度转换为( − π , π )的范围内。计算相邻路径点向量与x xx轴的角度需要利用atan2函数，使用这个函数也要小心。注意，atan2(0,0)=0。原程序有一个小BUG，即可能存在两个坐标完全一样的点，原因是在做Dubins曲线拼接时没有考虑重叠。为此，先对路径预处理，剔除相邻路径点x y xyxy坐标完全相同的其中一个点。这也是
4　话题发布path.cpp

　　规划出来的路径通过path.cpp这个文件发布出来。这个文件中的命名雷同，看起来非常累，比如文件名叫path，类的名称叫Path，话题名也叫path，坐标系也叫path，这是一种混乱的命名方式。
　　这里我们最感兴趣的是发布路径的消息：

nav_msgs::Path path;

　　负责向里填充数据的是addSegment函数

void Path::addSegment(const Node3D& node)

　　planner.h里定义了碰撞检测

CollisionDetection configurationSpace

然后把加载的地图赋值给它，

configurationSpace.updateGrid(map);

————————————————
版权声明：本文为CSDN博主「robinvista」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/robinvista/article/details/106279968