说明：  通过研讨Stentz的Focussed D*文章，自己写的matlab代码，调试通过。用于机器人路径规划，仅供参考。
(Stentz, through discussions of the Focussed D* article, wrote it myself matlab code, debugging through. For robot path planning, for informational purposes only.)