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能夠找到一條路徑，則默認為 找到一條符合動力學特性的路徑。