FMM 快速行进法（一）

地图构建

快速行进法用于到目标点之间的路径规划，上图描绘了一个模拟环境，其中位于通道之间的两个小岛，其中起点和目标点分别用红色阴影圆圈和红色星星表示，障碍区域用黑色表示。

地图环境

伪代码逻辑

第一步：该算法首先读取珊格地图计算其速度矩阵V(当设定地图为各向同性的时候，可以设定速度矩阵为全1矩阵) ，它与规划空间具有相同的大小，并定义了规划空间中每个点的接口传播速度

第二步：执行FMM计算到达时间矩阵T，从目标点运行FMM快速行进算法 界面无法在不可行区域进行传播，即界面在不可行区域的传播速度为 0，到达时间趋近于无穷大，界面传播的终止条件确切来说是覆盖所有的可行区域

第三步
在时间矩阵T上，通过梯度下降法得到规划路径

第四步：生成如上图的可视化合成图

matlab实现

时间矩阵更新公式

时间矩阵的更新参考上图中红色框内最终结果

FMM运行效果

最终通过matlab完成的FMM路径搜索效果如上图所示。

存在问题：

1. 从终点向起点的波纹传递不是以圆形波纹在传递，而是以正方形在扩展，不太符合FMM的初心意义。
2. 最终得到的曲线离障碍物太近，在实际中很容易发生碰撞。
   上述问题将在之后引入其他原理加以解决

基础FMM的matlab实现
先运行get_map.m程序获取地图
再运行main.m进行路径规划