【路径规划】基于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.

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