A star算法

1. 算法的理解

1.2 a 星算法的基本的原理

a 星 是一种启发式搜索算法， 用于在地图中的两个目标点之间寻找最短的路径，它结合了最优先搜索和Dijkstra算法的特点，通过考虑从起点到当前点的距离（或者代价 g（n) ) 和估算的从当前点到目标点的最短距离（启发式估计h（n） ）来进行，算法为图中每一个节点维护一个值 f(n) = g(n) + h(n），它代表了从起点经过节点n 到达目标点的估计成本 ，在搜索过程中，a 星算法会优先选择扩展f（n） 值最小的点，这有助于它高效的找到最短路径 。

1.2 a 星算法如何在效率和准确性之间权衡？

主要取决于额启发式函数h(n） ， 在cost ：f(n) = g(n) + h(n） 如果对

• 启发式估计h(n）总是低估从任意节点到目标节点的实际成本，那么可以保证找到最短路径
• 启发式估计h(n）很大，能更快的找到目标， 但路径可能不是最优的
• 启发式估计h(n）过小或者小于0 ，那么a 星会退化成DijKstra算法，效率低但是可以找到确保找到最短路径。

因此启发式函数的选择需要在搜索效率和路径优化度之间做出权衡 。

1.3 a 星算法中常用的启发式函数

• 曼哈顿距离(Manhattan Distance) : 其中移动仅限于水平和垂直方向，启发式计算的是两点在各轴上的差值的绝对之和
• 欧几里得距离（Euclidean Distance）：启发式是两点之间的直线距离
• 对角线距离（Diagonal Distance）： 移动可以是水平垂直以及对角线方向
• 切比雪夫距离（Chebyshev distance）：计算的是在任何方向上移动所需最大步数

    switch (distance_norm)
    {
    case Euclidean:
        {
        double dx = abs((double)(start_index(0) - end_index(0)));
        double dy = abs((double)(start_index(1) - end_index(1)));
        double dz = abs((double)(start_index(2) - end_index(2)));
        h = std::sqrt((std::pow(dx,2.0) + std::pow(dy,2.0)+std::pow(dz,2.0)));
        break;}
    case Manhattan:
        {
        double dx = abs((double)(start_index(0) - end_index(0)));
        double dy = abs((double)(start_index(1) - end_index(1)));
        double dz = abs((double)(start_index(2) - end_index(2)));
        h = dx + dy + dz;
        break;}
    case L_infty:
        {
        double dx = abs((double)(start_index(0) - end_index(0)));
        double dy = abs((double)(start_index(1) - end_index(1)));
        double dz = abs((double)(start_index(2) - end_index(2)));
        h = std::max({dx,dy,dz});}
        break;
    case Diagonal:
        {
        double distance[3];
        distance[0] = abs((double)(start_index(0) - end_index(0)));
        distance[1] = abs((double)(start_index(1) - end_index(1)));
        distance[2] = abs((double)(start_index(2) - end_index(2)));
        std::sort(distance,distance+3);
        h = distance[0] + distance[1] + distance[2] +(std::sqrt(3.0)-3) * distance[0] + (std::sqrt(2.0)-2)*distance[1];
        break;}
    
    default:
        break;
    }

1.4 实现a星的数据结构

#ifdef _Node_H_ 
#define _Node_H_ 

#include<iostream>
#include<ros/ros.h>
#include<Eigen/Eigen>
#include<Memory>


#define inf 1>>20 ; 
struct GridNode;
typedef std::shared_ptr<GridNode> GridNodePtr ; 

struct GridNode{
    int id_ ; 
    Eigen::Vector3d coord_ ; 
    Eigen::Vectros3i dir_ ; 
    Eigen::Vector3i index_ ; 

    double gScore_ ; 
    double fScore_ ; 

    GridNodePtr cameFrome_ ; 

    std::multimap<double , GridNodePtr> ::iterator nodeMapIt ;  

    GridNode(Eigen::Vector3i index , Eigen::Vector3d coord)
    {
        id_  = 0 ; 
        coord_ = coord  ; 
        index_ = index  ; 

         gScore = inf ; 
         fScore  = inf ; 

    cameFrome_  = nullptr ; 
    }

    ~GridNode() ;
    GridNode() ;

}
#endif