【路径规划】基于FMM快速行进法实现船舶路径规划附matlab代码

第一步

描绘了一个模拟环境，其中位于通道之间的两个小岛，其中起点和目标点分别用红色阴影圆圈和红色星星表示，障碍区域用黑色表示，其中到目标点（或障碍）的距离越大，电势就越高。

论文中描述为：该算法首先读取珊格地图 计算其速度矩阵v    它与规划空间具有相同的大小，并定义了规划空间中每个点的接口传播速度

执行FMM计算到达时间矩阵T，在时间矩阵T上，最终应用梯度下降法搜索最优路径

第二步

从目标点运行FM快速行进算法  界面无法在不可行区域进行传播，即界面在不可行区域的传播速度为 0，到达时间趋近于无穷大。

界面传播的终止条件确切来说是覆盖所有的可行区域

第三步

通过快速行进法计算得到到达时间地图，每个点的势值表示接口的局部到达时间

与此图相类似

第四步 

通过梯度下降法得到规划路径

第五步、生成类似合成图

与此图类似  图中的颜色分布代表时间地图  忽略绿色路线 红色路线为生成的路径

function T = get_T(pose,mat_T,mat_V)

%计算pose处的T

%左

pose_left=[pose(1)-1,pose(2)];

     if isnan(mat_T(pose_left(1),pose_left(2)))||pose_left(1)200

         T1=mat_T(pose(1)-1,pose(2));

     else

         T1=min(mat_T(pose(1)-1,pose(2)),mat_T(pose(1)+1,pose(2)));

     end

%上

pose_up=[pose(1),pose(2)-1];

     if isnan(mat_T(pose_up(1),pose_up(2)))||pose_up(2)200

         T2=mat_T(pose(1),pose(2)-1);

     else

         T2=min(mat_T(pose(1),pose(2)-1),mat_T(pose(1),pose(2)+1));

     end

% 计算T(pose)

    if mat_T(pose(1),pose(2))>=T1 && mat_T(pose(1),pose(2))=T2 && mat_T(pose(1),pose(2))