dijkstra

open_set是当前正在计算的节点；

每次从当前open_set集合中找出cost最小的节点进行计算更新：从open_set中去除该节点，保存到close_set中；

运动更新可以根据运动模型选择合适的节点运动方式；

【代价的计算方式是：在原节点基础之上，增加运动模型产生的代价；】

从当前节点通过运动更新的节点：

如果超出地图或遇到障碍物，则忽略该节点；

如果已经在close_set集合中，则忽略这个节点；

如果不在open_set中，添加到open_set集合中；

如果已经在open_set中，并且代价更低，则替换掉；

如果更新的当前节点是目标节点，则结束循环，因为每个节点都保存了他的父节点，从目标节点反向搜索到起始节点，得到最近的路径。

=================================================

a-star的启发式代价计算方式：新增当前节点到目标点的距离代价。