路径规划-astar实现和改进,路径缩短和修剪

astar(优化为最短路径，可见性树)

 

astar改进，路径张紧拉直，搜索最短路径

 

 

 

 

 

 

 

 上图中，红色路径为astar路径；靛蓝色路径为拉直后的路径，拉直可以避免因为启发值导致的直接朝向目标点的路径段；拉直后的路径近似为最短路径；