路径规划算法代码的重构以及反复测试结果