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