1.设置一个具有优先级的队列去存储所有扩展过的节点
2.设置启发式函数
3.设置起始点的状态state xs
4.设置起始点的G值为0，其他节点为无穷大。
循环：
从队列中取出f = g +h 的最小值
标记这个节点
如果是目标节点，则返回
对于这个节点所有未扩展过的邻居
if g(m ) = infinite
g(m) = g(n) +Cnm
Push node m into the queue.
记录的是节点的状态
if g(m) > g(n) + Cnm
g(m) = g(n) + Cnm
更新m的状态
end
end
1.启发式函数不同的选择 欧式距离需要遍历的节点较多。
2.不考虑障碍物只考虑动力学特性采用obvp来解决这个问题。遍历的节点相对较少。
3.不考虑障碍物只考虑动力学特性，但是地图环境较为复杂，因此遍历的节点会更多 。
其他改进的技巧：
可以前向积分10次，或者20次时进行一次obvp，如果此时利用obvp能够找到一条路径，则默认为 找到一条符合动力学特性的路径。