✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
三维路径规划所需的环境信息需要从地形模型中提取，良好的地形建模能有效提高路径规划效率；◆ 对于飞行环境中的较高的天然山体用指数函数来进行描述，数学模型可以表示为
⛄ 代码
clc
clear
close all
%% 初始化地形信息
mapRange = [100,100,100]; % 地图长、宽、高范围
N = 10; % 山峰个数
peaksInfo = struct; % 初始化山峰特征信息结构体
peaksInfo.center = []; % 山峰中心
peaksInfo.range = []; % 山峰区域
peaksInfo.height = []; % 山峰高度
peaksInfo = repmat(peaksInfo,N,1);
%% 随机生成N个山峰的特征参数
for i = 1:N
peaksInfo(i).center = [mapRange(1) * (rand*0.8+0.2), mapRange(2) * (rand*0.8+0.2)];
peaksInfo(i).height = mapRange(3) * (rand*0.7+0.3);
peaksInfo(i).range = mapRange*0.1*(rand*0.7+0.3);
end
%% 计算山峰曲面值
peaksData = [];
for x = 1:mapRange(1)
for y = 1:mapRange(2)
sum = 0;
for k = 1:N
h_i = peaksInfo(k).height;
x_i = peaksInfo(k).center(1);
y_i = peaksInfo(k).center(2);
x_si = peaksInfo(k).range(1);
y_si = peaksInfo(k).range(2);
sum = sum + h_i * exp(-((x-x_i)/x_si)^2 - ((y-y_i)/y_si)^2);
end
peaksData(x,y) = sum;
end
end
%% 构造曲面网格，用于后期MAP图插值判断三维路径是否与山峰交涉
% x列向量
x = [];
for i = 1:mapRange(1)
x = [x; ones(mapRange(2),1) * i];
end
% y列向量
y = (1:mapRange(2))';
y = repmat(y,length(peaksData(:))/length(y),1);
% peaksData列向量
peaksData = reshape(peaksData,length(peaksData(:)),1);
% 构造X/Y/Z网格数据
[X,Y,Z] = griddata(x,y,peaksData,...
linspace(min(x),max(x),100)',...
linspace(min(y),max(y),100));
%% 画山峰曲面
surf(X,Y,Z) % 画曲面图
shading flat % 各小曲面之间不要网格
clc
clear
close all
%% 根据散点获得拟合曲线三维路径
x_seq = [0,1,2,3,4];
y_seq = [5,2,3,4,1];
z_seq = [3,4,5,2,0];
% 利用spline函数进行拟合插值
k = length(x_seq);
t_seq = linspace(0,1,k);
T_seq = linspace(0,1,100);
X_seq = spline(t_seq,x_seq,T_seq);
Y_seq = spline(t_seq,y_seq,T_seq);
Z_seq = spline(t_seq,z_seq,T_seq);
% 画拟合曲线图
figure
hold on
scatter3(x_seq, y_seq, z_seq, 100, 'bs','MarkerFaceColor','g')
plot3(X_seq, Y_seq, Z_seq, 'r','LineWidth',2)
grid on
title('散点拟合曲线')
%% 计算曲线的曲率、挠率
% 计算三阶导数
f = [X_seq; Y_seq; Z_seq]; % 表示函数
delta = 1 / length(X_seq);
f1 = gradient(f)./delta; % 一阶导
f2 = gradient(f1)./delta; % 二阶导
f3 = gradient(f2)./delta; % 三阶导
f1 = f1';
f2 = f2';
f3 = f3';
% 曲率、挠率
v = cross(f1,f2,2); % 一阶导与二阶导做外积
e = dot(f3,v,2); %（r',r'',r'''）混合积
c = zeros(length(T_seq),1); % 定义矩阵c储存一阶导二阶导叉乘模长，d储存一阶导模长
d = c;
for i = 1:length(f)
c(i) = norm(v(i,:)); % 一阶导二阶导外积的模长
d(i) = norm(f1(i,:)); % 一阶导模长
end
k = c./(d.^3); % 曲率
torsion = e./c.^2; % 挠率
%% 画图
% 曲率图
figure
plot(k, 'r','LineWidth',2)
title('曲率图')
% 挠率图
figure
plot(torsion, 'r','LineWidth',2)
title('挠率图')
⛄ 运行结果
