（2-3）A*算法：在机器人导航系统中的应用案例

A*算法在机器人导航系统中发挥着关键作用。首先，机器人通过传感器获取环境地图信息，包括障碍物、通行区域等。然后，利用A算法进行路径规划，考虑地形、障碍物分布等因素，找到一条最优路径，以最小的代价实现从起点到目标点的导航。接着，机器人根据A*算法规划的路径进行智能导航，实时避开障碍物，完成自主移动。这样的路径规划方法使机器人能够高效、灵活地在复杂环境中导航，提高了自主机器人的工作效率和安全性。最后，系统可根据实时感知更新地图和路径，适应环境变化，保持导航的实时性和准确性，使机器人在各种应用场景中取得卓越的导航性能。

本项目是一个基于A*（A星）路径规划算法的ROS（机器人操作系统）应用，展示了A算法在ROS中的实现，通过订阅地图信息，启动节点，接收起点和目标点，执行路径规划，并通过ROS话题发布路径信息。本项目的功能特点如下所示：

1. A*路径规划算法实现：项目核心是A*算法，通过有效的启发式搜索，在给定地图上找到最佳路径。
2. ROS节点应用：项目以ROS节点形式存在，接收地图信息、起点和目标点，执行路径规划，并发布路径信息。
3. 参数配置：用户可以通过ROS参数配置启发式计算方式、图像二值化阈值、膨胀半径等参数，以满足不同场景的需求。
4. 实时可视化：通过ROS话题发布路径信息，用户可以在RViz等可视化工具中实时查看A*算法计算得到的路径。

通过这个项目，可以帮助大家深入了解路径规划算法在机器人领域的应用，为进一步研究和开发提供基础。

实例2-1：基于ROS的机器人路径规划系统（codes/2/Astar/）

本项目是一个使用A*算法实现的ROS节点，为机器人导航提供高效路径规划。该系统支持动态地图输入，灵活调整参数，通过可视化界面展示规划路径，为机器人在复杂环境中的自主导航提供可靠支持。

1. 实现A*算法

（1）编写文件Astar\include\Astar.h，这是一个C++实现的A*路径规划算法的头文件。它定义了Astar类，提供了初始化地图、路径规划、路径可视化等接口函数。其中，节点结构Node表示地图上的点，AstarConfig结构包含A算法的配置参数，Astar类封装了A*算法的实现，使用优先队列管理开放列表和哈希表加速查找，最终实现了地图上两点之间的路径规划。

#ifndef ASTAR_H
#define ASTAR_H

#include <iostream>
#include <queue>
#include <unordered_map>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

namespace pathplanning {

// 节点类型
enum NodeType {
    obstacle = 0,   // 障碍物
    free,           // 可通行
    inOpenList,     // 在开放列表中
    inCloseList     // 在关闭列表中
};

// 节点结构
struct Node {
    Point point;     // 节点坐标
    int F, G, H;     // 代价
    Node* parent;    // 父节点

    Node(Point _point = Point(0, 0)) : point(_point), F(0), G(0), H(0), parent(NULL) {
    }
};

// 用于优先队列比较的结构
struct cmp {
    bool operator() (pair<int, Point> a, pair<int, Point> b) {
        return a.first > b.first;  // 最小堆
    }
};

// A*算法配置
struct AstarConfig {
    bool Euclidean;         // 使用欧氏距离或曼哈顿距离
    int OccupyThresh;       // 地图二值化阈值
    int InflateRadius;      // 膨胀半径

    AstarConfig(bool _Euclidean = true, int _OccupyThresh = -1, int _InflateRadius = -1)
        : Euclidean(_Euclidean), OccupyThresh(_OccupyThresh), InflateRadius(_InflateRadius) {
    }
};

class Astar {

public:
    // 接口函数
    void InitAstar(Mat& _Map, AstarConfig _config = AstarConfig());
    void InitAstar(Mat& _Map, Mat& Mask, AstarConfig _config = AstarConfig());
    void PathPlanning(Point _startPoint, Point _targetPoint, vector<Point>& path);
    void DrawPath(Mat& _Map, vector<Point>& path, InputArray Mask = noArray(),
                  Scalar color = Scalar(0, 0, 255), int thickness = 1, Scalar maskcolor = Scalar(255, 255, 255));

    // 内联函数，将二维坐标转换为一维索引
    inline int point2index(Point point) {
        return point.y * Map.cols + point.x;
    }

    // 内联函数，将一维索引转换为二维坐标
    inline Point index2point(int index) {
        return Point(int(index / Map.cols), index % Map.cols);
    }

private:
    void MapProcess(Mat& Mask);      // 地图处理
    Node* FindPath();                // 寻找路径
    void GetPath(Node* TailNode, vector<Point>& path);  // 获取路径

private:
    // 成员变量
    Mat Map;                // 地图
    Point startPoint, targetPoint;  // 起始点和目标点
    Mat neighbor;           // 邻居矩阵

    Mat LabelMap;           // 标签地图
    AstarConfig config;     // A*算法配置

    // 优先队列及哈希表
    priority_queue<pair<int, Point>, vector<pair<int, Point>>, cmp> OpenList;
    unordered_map<int, Node*> OpenDict;
    vector<Node*> PathList;
};

}  // namespace pathplanning

#endif // ASTAR_H

（2）编写文件Astar\src\Astar.cpp，功能是实现了A*算法，用于在给定的地图中进行路径规划。通过初始化地图、配置参数，以及调用路径规划函数，可以获取起点到目标点的最优路径。另外，此文件还提供了绘制路径和地图预处理的功能，以及将地图坐标转换为图像坐标的工具函数。

#include "Astar.h"

namespace pathplanning{

// 初始化A*算法，如果没有遮罩图，则默认初始化为空
void Astar::InitAstar(Mat& _Map, AstarConfig _config)
{
    Mat Mask;
    InitAstar(_Map, Mask, _config);
}

// 初始化A*算法，包括地图、遮罩图和配置参数
void Astar::InitAstar(Mat& _Map, Mat& Mask, AstarConfig _config)
{
    char neighbor8[8][2] = {
            {-1, -1}, {-1, 0}, {-1, 1},
            {0, -1},            {0, 1},
            {1, -1},   {1, 0},  {1, 1}
    };

    Map = _Map;
    config = _config;
    neighbor = Mat(8, 2, CV_8S, neighbor8).clone();

    MapProcess(Mask);
}

// 路径规划入口函数，给定起点和目标点，返回路径
void Astar::PathPlanning(Point _startPoint, Point _targetPoint, vector<Point>& path)
{
    // 获取起点和目标点
    startPoint = _startPoint;
    targetPoint = _targetPoint;

    // 调用A*算法，返回尾节点
    Node* TailNode = FindPath();
    // 根据尾节点获取路径
    GetPath(TailNode, path);
}

// 在地图上绘制路径，可以传入遮罩图
void Astar::DrawPath(Mat& _Map, vector<Point>& path, InputArray Mask, Scalar color,
        int thickness, Scalar maskcolor)
{
    if(path.empty())
    {
        cout << "Path is empty!" << endl;
        return;
    }
    _Map.setTo(maskcolor, Mask);
    for(auto it:path)
    {
        rectangle(_Map, it, it, color, thickness);
    }
}

// 地图预处理，包括二值化、膨胀等
void Astar::MapProcess(Mat& Mask)
{
    int width = Map.cols;
    int height = Map.rows;
    Mat _Map = Map.clone();

    // 将RGB图像转为灰度图像
    if(_Map.channels() == 3)
    {
        cvtColor(_Map.clone(), _Map, cv::COLOR_BGR2GRAY);
    }

    // 二值化处理
    if(config.OccupyThresh < 0)
    {
        threshold(_Map.clone(), _Map, 0, 255, cv::THRESH_OTSU);
    } else
    {
        threshold(_Map.clone(), _Map, config.OccupyThresh, 255, cv::THRESH_BINARY);
    }

    // 膨胀处理
    Mat src = _Map.clone();
    if(config.InflateRadius > 0)
    {
        Mat se = getStructuringElement(MORPH_ELLIPSE, Size(2 * config.InflateRadius, 2 * config.InflateRadius));
        erode(src, _Map, se);
    }

    // 获取遮罩图
    bitwise_xor(src, _Map, Mask);

    // 初始化LabelMap
    LabelMap = Mat::zeros(height, width, CV_8UC1);
    for(int y=0;y<height;y++)
    {
        for(int x=0;x<width;x++)
        {
            if(_Map.at<uchar>(y, x) == 0)
            {
                LabelMap.at<uchar>(y, x) = obstacle;
            }
            else
            {
                LabelMap.at<uchar>(y, x) = free;
            }
        }
    }
}

// A*算法的核心实现，返回路径的尾节点
Node* Astar::FindPath()
{
    int width = Map.cols;
    int height = Map.rows;
    Mat _LabelMap = LabelMap.clone();

    // 将起点加入OpenList
    Node* startPointNode = new Node(startPoint);
    OpenList.push(pair<int, Point>(startPointNode->F, startPointNode->point));
    int index = point2index(startPointNode->point);
    OpenDict[index] = startPointNode;
    _LabelMap.at<uchar>(startPoint.y, startPoint.x) = inOpenList;

    while(!OpenList.empty())
    {
        // 找到F值最小的节点
        Point CurPoint = OpenList.top().second;
        OpenList.pop();
        int index = point2index(CurPoint);
        Node* CurNode = OpenDict[index];
        OpenDict.erase(index);

        int curX = CurPoint.x;
        int curY = CurPoint.y;
        _LabelMap.at<uchar>(curY, curX) = inCloseList;

        // 判断是否到达目标点
        if(curX == targetPoint.x && curY == targetPoint.y)
        {
            return CurNode; // 找到有效路径
        }

        // 遍历周围的节点
        for(int k = 0;k < neighbor.rows;k++)
        {
            int y = curY + neighbor.at<char>(k, 0);
            int x = curX + neighbor.at<char>(k, 1);
            if(x < 0 || x >= width || y < 0 || y >= height)
            {
                continue;
            }
            if(_LabelMap.at<uchar>(y, x) == free || _LabelMap.at<uchar>(y, x) == inOpenList)
            {
                // 判断是否可以通过对角线
                int dist1 = abs(neighbor.at<char>(k, 0)) + abs(neighbor.at<char>(k, 1));
                if(dist1 == 2 && _LabelMap.at<uchar>(y, curX) == obstacle && _LabelMap.at<uchar>(curY, x) == obstacle)
                    continue;

                // 计算G、H、F值
                int addG, G, H, F;
                if(dist1 == 2)
                {
                    addG = 14;
                }
                else
                {
                    addG = 10;
                }
                G = CurNode->G + addG;
                if(config.Euclidean)
                {
                    int dist2 = (x - targetPoint.x) * (x - targetPoint.x) + (y - targetPoint.y) * (y - targetPoint.y);
                    H = round(10 * sqrt(dist2));
                }
                else
                {
                    H = 10 * (abs(x - targetPoint.x) + abs(y - targetPoint.y));
                }
                F = G + H;

                // 更新节点的G、H、F值
                if(_LabelMap.at<uchar>(y, x) == free)
                {
                    Node* node = new Node();
                    node->point = Point(x, y);
                    node->parent = CurNode;
                    node->G = G;
                    node->H = H;
                    node->F = F;
                    OpenList.push(pair<int, Point>(node->F, node->point));
                    int index = point2index(node->point);
                    OpenDict[index] = node;
                    _LabelMap.at<uchar>(y, x) = inOpenList;
                }
                else // _LabelMap.at<uchar>(y, x) == inOpenList
                {
                    // 找到节点
                    int index = point2index(Point(x, y));
                    Node* node = OpenDict[index];
                    if(G < node->G)
                    {
                        node->G = G;
                        node->F = F;
                        node->parent = CurNode;
                    }
                }
            }
        }
    }

    return NULL; // 未找到有效路径
}

// 根据尾节点获取路径
void Astar::GetPath(Node* TailNode, vector<Point>& path)
{
    PathList.clear();
    path.clear();

    // 将路径保存到PathList
    Node* CurNode = TailNode;
    while(CurNode != NULL)
    {
        PathList.push_back(CurNode);
        CurNode = CurNode->parent;
    }

    // 将路径保存到vector<Point>
    int length = PathList.size();
    for(int i = 0;i < length;i++)
    {
        path.push_back(PathList.back()->point);
        PathList.pop_back();
    }

    // 释放内存
    while(OpenList.size()) {
        Point CurPoint = OpenList.top().second;
        OpenList.pop();
        int index = point2index(CurPoint);
        Node* CurNode = OpenDict[index];
        delete CurNode;
    }
    OpenDict.clear();
}

} 

2. 占据栅格地图的坐标转换

占据栅格地图的坐标转换指的是在占据栅格地图中，通过转换坐标系统，实现图像坐标和地图坐标之间的相互转换。在机器人导航和路径规划中，占据栅格地图是一种常见的地图表示方式，其中每个栅格代表地图上的一个小区域，栅格的状态表示该区域是否被占据（occupied）。这样的坐标转换功能对于在机器人导航系统中进行传感器数据和地图的配准以及路径规划等任务非常重要。

（1）编写文件Astar\include\OccMapTransform.h，定义了一个名为 OccupancyGridParam 的类，用于处理占据栅格地图的参数及坐标变换。该类包含了获取地图参数、图像坐标到地图坐标的转换以及地图坐标到图像坐标的转换等接口。通过这些功能，可以方便地进行占据栅格地图的信息处理和坐标转换。

#include "OccMapTransform.h"

// 获取占据栅格地图参数
void OccupancyGridParam::GetOccupancyGridParam(nav_msgs::OccupancyGrid OccGrid) {
    // 获取参数
    resolution = OccGrid.info.resolution;
    height = OccGrid.info.height;
    width = OccGrid.info.width;
    x = OccGrid.info.origin.position.x;
    y = OccGrid.info.origin.position.y;

    double roll, pitch, yaw;
    geometry_msgs::Quaternion q = OccGrid.info.origin.orientation;
    tf::Quaternion quat(q.x, q.y, q.z, q.w);  // x, y, z, w
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
    theta = yaw;

    // 计算R, t
    R = Mat::zeros(2, 2, CV_64FC1);
    R.at<double>(0, 0) = resolution * cos(theta);
    R.at<double>(0, 1) = resolution * sin(-theta);
    R.at<double>(1, 0) = resolution * sin(theta);
    R.at<double>(1, 1) = resolution * cos(theta);
    t = Mat(Vec2d(x, y), CV_64FC1);
}

// 图像到地图坐标的变换
void OccupancyGridParam::Image2MapTransform(Point& src_point, Point2d& dst_point) {
    // 上下翻转
    Mat P_src = Mat(Vec2d(src_point.x, height - 1 - src_point.y), CV_64FC1);
    // 旋转和平移
    Mat P_dst = R * P_src + t;

    dst_point.x = P_dst.at<double>(0, 0);
    dst_point.y = P_dst.at<double>(1, 0);
}

// 地图到图像坐标的变换
void OccupancyGridParam::Map2ImageTransform(Point2d& src_point, Point& dst_point) {
    Mat P_src = Mat(Vec2d(src_point.x, src_point.y), CV_64FC1);
    // 旋转和平移
    Mat P_dst = R.inv() * (P_src - t);
    // 上下翻转
    dst_point.x = round(P_dst.at<double>(0, 0));
    dst_point.y = height - 1 - round(P_dst.at<double>(1, 0));
}

3. 准备地图

文件Astar\maps\gmapping-005.pgm是激光SLAM（Simultaneous Localization and Mapping）算法中使用的激光雷达建图阶段生成的栅格地图，这种地图通常以 PGM（Portable Graymap） 格式保存，其中每个像素的灰度值表示相应位置的占据概率。在本项目中，该地图文件用于测试 A* 路径规划算法的输入地图。如图2-8所示。

图2-8  地图文件

4. 主程序

编写文件Astar\src\main.cpp，这是一个ROS节点，使用A*算法进行路径规划。它订阅了地图("map")，起始点("initialpose")和目标点("move_base_simple/goal")的话题，通过初始化A*算法类，处理地图，计算路径，并发布规划好的路径("nav_path")和处理后的地图("mask")。另外，这个ROS节点还根据参数进行路径规划和地图处理，可以配置使用欧氏距离或曼哈顿距离，设置占据阈值、膨胀半径等参数。

#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <opencv2/opencv.hpp>
#include "Astar.h"
#include "OccMapTransform.h"

using namespace cv;
using namespace std;

//-------------------------------- 全局变量 ---------------------------------//
// 订阅者
ros::Subscriber map_sub;
ros::Subscriber startPoint_sub;
ros::Subscriber targetPoint_sub;
// 发布者
ros::Publisher mask_pub;
ros::Publisher path_pub;

// 对象
nav_msgs::OccupancyGrid OccGridMask;
nav_msgs::Path path;
pathplanning::AstarConfig config;
pathplanning::Astar astar;
OccupancyGridParam OccGridParam;
Point startPoint, targetPoint;

// 参数
double InflateRadius;
bool map_flag;
bool startpoint_flag;
bool targetpoint_flag;
bool start_flag;
int rate;

//-------------------------------- 回调函数 ---------------------------------//
void MapCallback(const nav_msgs::OccupancyGrid& msg)
{
    // 获取参数
    OccGridParam.GetOccupancyGridParam(msg);

    // 获取地图
    int height = OccGridParam.height;
    int width = OccGridParam.width;
    int OccProb;
    Mat Map(height, width, CV_8UC1);
    for(int i=0; i<height; i++)
    {
        for(int j=0; j<width; j++)
        {
            OccProb = msg.data[i * width + j];
            OccProb = (OccProb < 0) ? 100 : OccProb; // 将Unknown设置为0
            // OccGrid的原点在地图的左下角
            Map.at<uchar>(height-i-1, j) = 255 - round(OccProb * 255.0 / 100.0);
        }
    }

    // 初始化Astar
    Mat Mask;
    config.InflateRadius = round(InflateRadius / OccGridParam.resolution);
    astar.InitAstar(Map, Mask, config);

    // 发布Mask
    OccGridMask.header.stamp = ros::Time::now();
    OccGridMask.header.frame_id = "map";
    OccGridMask.info = msg.info;
    OccGridMask.data.clear();
    for(int i=0; i<height; i++)
    {
        for(int j=0; j<width; j++)
        {
            OccProb = Mask.at<uchar>(height-i-1, j) * 255;
            OccGridMask.data.push_back(OccProb);
        }
    }

    // 设置标志
    map_flag = true;
    startpoint_flag = false;
    targetpoint_flag = false;
}

void StartPointCallback(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
    Point2d src_point = Point2d(msg.pose.pose.position.x, msg.pose.pose.position.y);
    OccGridParam.Map2ImageTransform(src_point, startPoint);

    // 设置标志
    startpoint_flag = true;
    if(map_flag && startpoint_flag && targetpoint_flag)
    {
        start_flag = true;
    }

//    ROS_INFO("startPoint: %f %f %d %d", msg.pose.pose.position.x, msg.pose.pose.position.y,
//             startPoint.x, startPoint.y);
}

void TargetPointtCallback(const geometry_msgs::PoseStamped& msg)
{
    Point2d src_point = Point2d(msg.pose.position.x, msg.pose.position.y);
    OccGridParam.Map2ImageTransform(src_point, targetPoint);

    // 设置标志
    targetpoint_flag = true;
    if(map_flag && startpoint_flag && targetpoint_flag)
    {
        start_flag = true;
    }

//    ROS_INFO("targetPoint: %f %f %d %d", msg.pose.position.x, msg.pose.position.y,
//             targetPoint.x, targetPoint.y);
}

//-------------------------------- 主函数 ---------------------------------//
int main(int argc, char * argv[])
{
    // 初始化节点
    ros::init(argc, argv, "astar");
    ros::NodeHandle nh;
    ros::NodeHandle nh_priv("~");
    ROS_INFO("Start astar node!\n");

    // 初始化变量
    map_flag = false;
    startpoint_flag = false;
    targetpoint_flag = false;
    start_flag = false;

    // 参数
    nh_priv.param<bool>("Euclidean", config.Euclidean, true);
    nh_priv.param<int>("OccupyThresh", config.OccupyThresh, -1);
    nh_priv.param<double>("InflateRadius", InflateRadius, -1);
    nh_priv.param<int>("rate", rate, 10);

    // 订阅话题
    map_sub = nh.subscribe("map", 10, MapCallback);
    startPoint_sub = nh.subscribe("initialpose", 10, StartPointCallback);
    targetPoint_sub = nh.subscribe("move_base_simple/goal", 10, TargetPointtCallback);

    // 广播话题
    mask_pub = nh.advertise<nav_msgs::OccupancyGrid>("mask", 1);
    path_pub = nh.advertise<nav_msgs::Path>("nav_path", 10);

    // 循环等待回调
    ros::Rate loop_rate(rate);
    while(ros::ok())
    {
        if(start_flag)
        {
            double start_time = ros::Time::now().toSec();
            // 开始规划路径
            vector<Point> PathList;
            astar.PathPlanning(startPoint, targetPoint, PathList);
            if(!PathList.empty())
            {
                path.header.stamp = ros::Time::now();
                path.header.frame_id = "map";
                path.poses.clear();
                for(int i=0; i<PathList.size(); i++)
                {
                    Point2d dst_point;
                    OccGridParam.Image2MapTransform(PathList[i], dst_point);

                    geometry_msgs::PoseStamped pose_stamped;
                    pose_stamped.header.stamp = ros::Time::now();
                    pose_stamped.header.frame_id = "map";
                    pose_stamped.pose.position.x = dst_point.x;
                    pose_stamped.pose.position.y = dst_point.y;
                    pose_stamped.pose.position.z = 0;
                    path.poses.push_back(pose_stamped);
                }
                path_pub.publish(path);
                double end_time = ros::Time::now().toSec();

                ROS_INFO("Find a valid path successfully! Use %f s", end_time - start_time);
            }
            else
            {
                ROS_ERROR("Can not find a valid path");
            }

            // 设置标志
            start_flag = false;
        }

        if(map_flag)
        {
            mask_pub.publish(OccGridMask);
        }

        loop_rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

5. 调试运行

（1）启动ROS节点：使用如下roslaunch命令启动A*算法的ROS节点。

roslaunch astar astar.launch

（2）订阅路径信息：使用如下rostopic命令订阅nav_path话题。

rostopic echo /nav_path

这将显示路径信息，其中包括一系列包含(x, y)坐标的ROS消息。

（3）使用RViz进行可视化：使用RViz（ROS Visualization）工具创建一个Path显示器，将路径信息进行可视化。在RViz中添加一个Path显示器，并订阅/nav_path话题。效果如图2-8所示。

图2-8  路径在地图中的可视化效果

本专题已结束：

（2-1）A*算法：A*算法（A-star Algorithm）简介-CSDN博客

（2-2）A*算法：A*算法的核心思想-CSDN博客