加权A*算法

1. 初始化open_set和close_set；

2. 将起点加入open_set中，并设置优先级为0 (优先级最高)；

3. if open_set不为空，then从open_set中选取优先级最高的节点n：

4. if节点n为终点，then：

5. 从终点开始逐步追踪parent节点，一直达到起点；

6. 返回找到的结果路径，算法结束；

7. if如果节点n不是终点，then：

8. 将节点n从open_set中删除，并加入close_set中；

9. 遍历节点n所有的邻近节点：

10. if邻近节点m在close_set中，then：

11. 跳过，选取下一个邻近节点；

12. if邻近节点m也不在open_set中，then：

13. 设置节点m的parent为节点n；

14. 计算节点m的优先级：

$g\left(s\right)=g\left(u\right)+\text{cost}(s_i,s_{i+1})$ ； $h\left(s\right)=\text{distance}()$ ； $f\left(s\right)=valu{e}_1\ast g\left(s\right)+valu{e}_2\ast h(s)$ ；

15. 将节点m加入open_set中。