自动驾驶技术 ROS混合A*算法路径规划插件（非完整约束模型，阿克曼转向模型小车）

混合A*路径规划插件

       传统的机器人导航算法（如京东亚马逊使用的物流仓储机器人），大多数是将地图栅格化之后，使用A*算法或Dijkstra搜索算法进行路径规划，其大多数是面向完整约束的动力学模型或差速模型，因此生成的导航路径有可能是横平竖直的。很显然这样的路径对于传统的四轮车辆（使用阿克曼转向机构的车辆）仍然是难以实现的。

"它的路径还未优化，但它很有天赋！"

---

ROS 路径规划

       在继续深入研究之前，我们有必要了解ROS体系下的Navigation导航包，并且需要熟悉一下导航程序包是如何工作的。
       直接阅读查看ROS官方文档，然后源代码下到本地研究一番。当然单纯的阅读代码也许是干涩无味的，并且其中使用的多线程语句也会让我们的代码阅读与理解工作雪上加霜。这时不如我们然后将代码编译一下，跑起来看看。 适时给自己一点点鼓励，一点点小的进展也能让你信心倍增！同时我们也可以参考一下这篇帖子的内容，结合着源代码梳理一下导航程序包的工作流程。
       move_base中的代码内容对有C语言编程基础的小伙伴其实不难。只是需要预先熟悉C++多线程、C++STL容器与智能指针的一些内容。这里推荐一本书《Cpp_Concurrency_In_Action-master》，可以深入了解C++多线程与智能指针的内容，这对接下来的代码的深入理解将会有很大的帮助。


接下来这里简单描述一下move_base导航程序的工作流程：

       于是乎，我们看到，如果要在ROS上使用并测试我们自己的路径规划器, 并不需要从头到尾全部自己编写一整套完整程序，我们只需要针对类插件下手即可。然后在启动文件中，向参数服务器声明我们希望使用的规划器的名字即可。只要类插件编写的符合接口要求，那么插件就能正常的嵌入到规划程序中去。

---

非完整约束条件

       在实际的路径规划过程中，需要根据测量的运动学模型计算路径的损失函数，并且还需要确保给规划的路径匹配机器人的约束。因此在研究一般性的车辆自动驾驶问题时，首先要了解一般的车辆的运动学模型。
       本文为简化难度，将车辆的模型简化，不考虑车轮的倾角、前束角等参数，并且忽略车辆在垂直方向上的运动，仅考虑在二维平面的速度与转向角度。车辆处于低速运动状态，此时车轮与地面做滚动运动，不产生滑动，车辆运行时处于可控状态。

1. $x$、$y$为车辆后轴中心位置在当前参照坐标系的位置
2. $\phi$ 为转向角度，是方向盘直接控制的变量
3. $a$ 为车辆轴距
4. $\nu$为小车速度
5. M 为位于车辆坐标系y轴上的任意一点，在车辆坐标系上的坐标为(0,m)
   $$\omega =\frac{\nu \tan \left(\phi \right)}{a}$$
   $$v_M=\omega \sqrt{{\left(\frac{a}{\tan \left(\phi \right)}\right)}^2+m^2}$$
   $${\phi }_M={\tan }^{-1}\left(\frac{m\tan \left(\phi \right)}{a}\right)$$
          因此可知，当M点刚好与车辆后轴中心位置重合时，此时的$v_m=v$，同时${\phi }_m=0$。当M点位于其他点位时，其速度，速度方向都与车辆的转向角度$\phi$有关，而$\phi$又被方向盘控制，因此控制方向盘就能控制车身。
   $${\upsilon }_{\perp }=\frac{\dot{x}}{\cos \left(\theta -\frac{\pi }{2}\right)}$$
   $${\upsilon }_{\perp }=\frac{-\dot{y}}{\sin \left(\theta -\frac{\pi }{2}\right)}$$
   $$\frac{\dot{x}}{\cos \left(\theta -\frac{\pi }{2}\right)}=\frac{-\dot{y}}{\sin \left(\theta -\frac{\pi }{2}\right)}$$
   $$\dot{x}\sin \left(\theta -\frac{\pi }{2}\right)+\dot{y}\cos \left(\theta -\frac{\pi }{2}\right)=0$$
   由此得出非完整约束：
   $$\dot{x}\cos \left(\theta \right)-\dot{y}\sin \left(\theta \right)=0$$
          可以看出，在车辆行驶时，车辆后轴与车轴平行方向速度始终等于0。表示为$V\perp =0$。
   而在除后轴这一条轴上，由于方向盘控制的转向变量 因此存在着垂直于车辆坐标系y轴的速度分量。
   虽然非完整约束加少了汽车速度空间的维数：汽车能前进后退，或围绕其转向极限内的某一圆心转动，并且不能自由的横向平移或自由旋转。但非完整约束并不会减少汽车的位形空间，汽车仍然可以到达二维平面的任意位置和方向。
          如何理解这个约束条件所描述的场景？就拿最常见的侧方停车举例：你可以逐步改变位置和速度的方向到达目标位置，但你不能直接把车推进车位中。
   
   
   

“你不能将车辆直接推进车库中，但你可以将汽车倒进车库”

---

ROS导航包面对非完整约束模型存在的问题

       在上文中介绍了ROS体系下的Navigation导航包与非完整约束模型。那么这里我们将要讨论一下导航包内置的A*导航算法为什么在非完整约束条件车辆上不太适用。
       我们知道A*算法是基于栅格化地图的图搜索，那么我们在栅格化地图时就不可避免的将每个地图栅格离散化。

       很显然这样的路径，对采用麦克纳姆轮、差速模型之类的小车还算简单，但对于正常转向布局的车辆是无法执行的。
       虽然Navigation导航包的A*路径规划中采取一些轨迹平滑化处理，但是在真正算法层面并未考虑非完整约束车辆模型的性质，因此在避障、转向、倒车等场景下，明显能观察到车辆运行十分不顺畅。

---

混合A*算法

       既然存在着问题，那么一定有人已经想到了解决办法。斯坦福大学2010年，首次提出一种满足车辆运动学的算法 即Hybrid A*,并在(DARPA)的城市挑战赛中得以运用。
       混合A*算法加上了车辆的约束条件并将离散的拓展点修改为连续的拓展点，通过这样就能得到一条连续的正常车辆可以执行的路径。

ROS插件

       要在move_base导航包中使用自己编写的第三方规划算法。如果要重新编写整个导航包，工作量似乎过大。好在ROS在设计之初就充分考虑了模块化的需求，为我们提供了不改动源码，就能实现程序功能拓展的解决方案：ROS类插件。ROS的类插件机制(ROS Plugin)是以C++的多态为核心。使用类插件时，程序只需调用声明了接口函数的基类模板头文件。程序在编译、链接时不需要将类插件真正的实现代码编译。我们可以单独编写、编译类插件的实现代码，在单独编写类插件时，子类通过继承基类模板（主程序时调用的声明了接口函数的基类模板）以虚函数的方式实现多态。

       对于我们要编写的全局导航包，通过阅读move_base源码，知道了base_navagation就是全局导航中使用的基类模板，基类模板留给我们两个接口：初始化接口与调用接口。我们需要将导航算法插件继承基类模板，按照其接口要求编写插件，然后通过ROS类插件管理系统注册插件，这样我们就能在不改变move_base包的情况下，使用新的算法。

       需要在编写插件时，加上对插件的声明：

// A code block
PLUGINLIB_EXPORT_CLASS(hybrid_astar_planner::HybridAStarPlanner, 
						nav_core::BaseGlobalPlanner)//注册为类插件的声明

然后在cmake文件中设置一下编译、安装选项

//cmakefile
add_library(${PROJECT_NAME}
****.cpp
****.cpp
)
target_link_libraries(${PROJECT_NAME} ${OMPL_LIBRARIES})

install(FILES bgp_plugin.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

添加plugin.xml插件描述文件
而后在carrot_planner包中加入 **plugin.xml文件.

//xml file
<library path="lib/libhybrid_astar_planner">
  <class name="hybrid_astar_planner/HybridAStarPlanner" type="hybrid_astar_planner::HybridAStarPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A implementation of a based planner using Hybrid A*
    </description>
  </class>
</library>

最修改**_package.xml文件在**_package.xml文件，在最后加上如下

//xml file
  <export>
      <nav_core plugin="${prefix}/hybrid_astar_planner_plugin.xml" />
  </export>

编译成功之后，我们就将插件成功安装进了ROS中

代码实现

       由于混合A*相对于A*来说，搜索环境从离散的环境中变为连续，因此拓展节点需要的开销也变得很大，需要拓展的节点也变得很大。因此我们需要设计一个性能优秀的启发函数对搜索算法进行优化。

       起初使用A*算法对节点进行优化，但是每一个拓展节点都对其进行A*拓展得到启发值，开销同样不小，也让算法复杂度大大提高，因此经过尝试，使用人工势场法为核心设计启发函数。

       又由于人工势场法会导致搜索算法陷入局部最小点，因此在本设计中对人工势场法进行了改进，在构建人工势场的时候，将目标点设为人工势场的起点。使用Dijkstar搜索算法遍历所有节点。并将势场函数带入障碍物密度、距离中，得出一张全局地图势场。这样得益于Dijkstar算法的特性，地图中所有的点的梯度方向都将最终指向目标点。使得不会产生局部最小点。

       同时为优化搜索性能采用二项堆作为混合A*中的优先队列，使用哈希表对开集与闭集搜索进行优化。
       虽然基本完成了设计内容，但是仍存在着不足。还存在着如规划算法收敛速度慢、车辆参数不匹配、程序鲁棒性不足、系统抖动等问题，在路径规划时，虽然采用了很多优化的方法，但产生人工势场将花费90%的规划时间，未来也将针对性做出优化。
完整代码请前往项目github下载，当然也可以去gitee下载