最初编写此代码是为了完成课程作业。随后参考了多篇文章，决定以Matlab格式编写路径规划算法，一方面作为个人笔记，另一方面供他人参考。在整个编写过程中，尽量详细列出了参考的思想和代码，如有遗漏，请在评论中指出。希望大家能够谅解其中不足之处。