【路径规划】基于前向动态规划算法在地形上找到最佳路径（Matlab代码实现）

news2024/9/29 18:06:53

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

汽车必须尽可能靠近目标坐标。旅行时间应尽可能短。汽车一次只能在八个方向上移动一步：左、右、上、下、左上、左下、右上、右下。

成本函数定义为高度差，它始终为正数。在行驶过程中，汽车必须避免爬升轨迹。但是，下降轨迹对汽车也没有好处。因此，下降轨迹并不比平坦轨迹更可取。

【路径规划】基于前向动态规划算法在地形上找到最佳路径，并用Matlab代码实现。

📚2 运行结果

部分代码：

function visualizePath(T, optimalPath)

% Visualize the path on given terrain T.
%

% -------------------------------------------------------------------------

[m, n] = size(T);
l = length(optimalPath);

% Convert back the node cardinal number to the corresponding xyz coordinate.
x = zeros(1, l);
y = zeros(1, l);
z = zeros(1, l);
for i = 1 : l
x(i) = mod(optimalPath(i) - 1, n) + 1;
y(i) = abs((optimalPath(i) - 1 - mod(optimalPath(i) - 1, n)) / n) + 1;
z(i) = 1.0 + T(y(i), x(i));
end

% Draw the optimal path as line.
plot3(x, y, z, 'r', 'LineWidth', 2)

% Draw asterisk symbol (*) at destination nodes.
plot3(x(l), y(l), z(l), '*m', 'LineWidth', 4)

function visualizeTerrain(T)

% Visualize the terrain.
%
% Don't forget to hold the figure when you want to visualize the optimal
% path using visualizePath function.

% -------------------------------------------------------------------------

[m, n] = size(T);

[X,Y] = meshgrid(1 : n, 1 : m);
surf(X, Y, T(1 : m, 1 : n));

end