Hybrid A*——ROS实现带有车辆运动学约束的路径规划算法

本文旨在帮助读者完成2D栅格地图上的RS-A*路径规划算法，便于进行车辆导航。

废话不多说，直接开始！

一、2D栅格地图准备

2D栅格地图有两种途径：

1. 2D激光SLAM算法直接获取；

2. 3D激光SLAM算法获取点云地图后投影获取：3D激光SLAM点云地图pcd转导航可用的2D栅格地图_pcd2pgm-CSDN博客

二、Hybrid A*算法

安装Hybrid A*：

#创建工作空间
mkdir -p ~/hy_astar/src
cd ~/hy_astar/src
catkin_init_workspace
 
#克隆代码
git clone https://github.com/zm0612/Hybrid_A_Star.git

将所得的2D栅格地图pgm文件复制到Hybrid_A_Star/maps下，并替换Hybrid_A_Star/maps下的yaml文件。

一般情况下我们的栅格地图的分辨率均为0.05，将Hybrid_A_Star/src/hybrid_a_star_flow.cpp中的：

        const double map_resolution = 0.2;
        kinodynamic_astar_searcher_ptr_->Init(
                current_costmap_ptr_->info.origin.position.x,
                1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,
                current_costmap_ptr_->info.origin.position.y,
                1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,
                current_costmap_ptr_->info.resolution,
                map_resolution
        );

        unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);
        unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);
        for (unsigned int w = 0; w < map_w; ++w) {
            for (unsigned int h = 0; h < map_h; ++h) {
                auto x = static_cast<unsigned int> ((w + 0.5) * map_resolution
                                                    / current_costmap_ptr_->info.resolution);
                auto y = static_cast<unsigned int> ((h + 0.5) * map_resolution
                                                    / current_costmap_ptr_->info.resolution);

                if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {
                    kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
                }
            }
        }

修改为：

        const double map_resolution = 0.05;
        kinodynamic_astar_searcher_ptr_->Init(
                current_costmap_ptr_->info.origin.position.x,
                1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,
                current_costmap_ptr_->info.origin.position.y,
                1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,
                current_costmap_ptr_->info.resolution,
                map_resolution);
        unsigned int map_w = std::floor(current_costmap_ptr_->info.width);
        unsigned int map_h = std::floor(current_costmap_ptr_->info.height);

        for (unsigned int w = 0; w < map_w; ++w)
        {
            for (unsigned int h = 0; h < map_h; ++h)
            {
                if (current_costmap_ptr_->data[h * current_costmap_ptr_->info.width + w] > 0)
                {                   
                    kinodynamic_astar_searcher_ptr_->SetObstacle(w, h); 
                }
            }
        }   

注意这里的map_resolution应与yaml中一致，修改之后完成编译：

cd ~/hy_astar
catkin_make

三、运行

编译完成后运行代码：

cd ~/hy_astar
source devel/setup.bash
roslaunch hybrid_a_star run_hybrid_a_star.launch

运行成功打开rviz界面，在地图的可行区域内用rviz中的2D Pose Estimate设置规划起点，用2D Nav Goal设置终点，短暂等待后规划出一条基于RS曲线的符合车辆运动学约束的路径：

至此，便完成了符合车辆运动学约束的路径规划！

如需适配到其他模块，例如自动生成轨迹，替换由手动给定起始末端位姿方式，则需修改Hybrid_A_Star/src/hybrid_a_star_flow.cpp中订阅的/initialpose和/move_base_simple/goal话题！

另外以上代码在碰撞检测时，将车辆轮廓限定为矩形，矩形大小通过Hybrid_A_Star/src/hybrid_a_star.cpp的SetVehicleShape函数设定，适配自己的车型需要修改其函数参数。