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

1 内容介绍

第一步

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

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

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

第二步

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

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

第三步

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

与此图相类似

第四步 

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

第五步、生成类似合成图

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

2 部分代码

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)<1

         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_right=[pose(1)+1,pose(2)];

     if isnan(mat_T(pose_right(1),pose_right(2)))||pose_right(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)<1

         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

%下  

pose_down=[pose(1),pose(2)+1];

     if isnan(mat_T(pose_down(1),pose_down(2)))||pose_down(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

        T=T1+1/mat_V(pose(1),pose(2));

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

        T=T2+1/mat_V(pose(1),pose(2));

    else

%         disp('error')

        T=min(T1,T2)+1/mat_V(pose(1),pose(2));

    end

%     if T1<=T2

%         T=T1+1/mat_V(pose(1),pose(2));

%     else

%         T=T2+1/mat_V(pose(1),pose(2));

%     end

end

3 运行结果

4 参考文献

[1]王跃午. 基于快速行进法的无人艇编队路径规划技术研究[D]. 哈尔滨工程大学.

[2]刘蔚, 谈果戈, 邹劲,等. 基于快速行进平方法的水面无人船路径规划[J]. 信息与控制, 2021, 50(3):13.​

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机、雷达通信、无线传感器等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。