A star算法-CSDN博客

1. 算法的理解

1.2 a 星算法的基本的原理

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

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