无人机基础训练之三维地图定义与散点拟合插值附matlab代码

✅作者简介：热爱科研的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('挠率图')

⛄ 运行结果

⛄ 参考文献

⛳️ 代码获取关注我

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

❤️ 关注我领取海量matlab电子书和数学建模资料