基于群智能的三维路径规划算法 1 —— 三维地图定义与散点拟合插值

1 重要函数

• repmat函数

B = repmat(A,m,n)，将矩阵 A 复制 m×n 块，即把 A 作为 B 的元素，B 由 m×n 个 A 平铺而成 B 的维数是 [size(A,1)*m, size(A,2)*n]

repmat函数说明

• reshape函数

reshape 是按照列取的，按照列的顺序进行转换的，也就是第一列读完，读第二列

reshape 用法
reshape 讲解

• griddata 函数：对二维或者三维散点数据插值

griddata函数拟合三维散点

官网解释

• linspace(start,stop,n)：n为步长，产生一个从start开始，最后一个是stop的等差数列，公差为(stop - start)/(n - 1)
• surf 函数

surf 函数绘制曲面

• spline 插值函数

三次样条插值spline函数

• (:)的含义：取矩阵的所有值构成列向量的操作（按列优先）

(:)的用法含义

• scatter3：绘制三维散点图

绘制三维散点图

2 三维地图定义

% 三维地图定义

clc
clear
close all

%% 初始化地形信息
mapRange = [100,100,100];           % 地图长、宽、高范围
N = 10;                             % 山峰个数
peaksInfo = struct;                 % 初始化山峰特征信息结构体(有3个字段)
peaksInfo.center = [];              % 山峰中心, 这里用(x,y)来表示
peaksInfo.range = [];               % 山峰区域, 区域大(小)坡度越缓(陡)
peaksInfo.height = [];              % 山峰高度
% B = repmat(A,m,n)，将矩阵 A 复制 m×n 块，即把 A 作为 B 的元素，B 由 m×n 个 A 平铺而成
% B 的维数是 [size(A,1)*m, size(A,2)*n] 
peaksInfo = repmat(peaksInfo,N,1);  % 扩展成10行1列，每一行结构体包括3个信息值

%% 随机生成N个山峰的特征参数
% rand 返回一个在区间 (0,1) 内均匀分布的随机数
for i = 1:N
    % mapRange的1和2表示地图长和宽，表示中心点(x,y)值, x和y的范围都是(100*0.2,100)
    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);% 3表示地图高度，高度范围是(100*0.3,100)
    peaksInfo(i).range = mapRange * 0.1 * (rand*0.7 + 0.3);% range的值以(x,y,z)形式表示出来，且3个值相同
end

%% 计算山峰曲面值
% 根据论文中公式计算，mapRange = [100,100,100] 地图长、宽、高范围
peaksData = [];                             % 100 * 100个z(x,y)值，本质就是z值
for x = 1:mapRange(1)                       % x 范围是1~100
    for y = 1:mapRange(2)                   % y 范围是1~100
        sum = 0;
        for k = 1:N                         % N = 10
            h_i = peaksInfo(k).height;
            x_i = peaksInfo(k).center(1);
            y_i = peaksInfo(k).center(2);
            x_si = peaksInfo(k).range(1);   % 沿 x 轴的衰减量
            y_si = peaksInfo(k).range(2);   % 沿 y 轴的衰减量
            sum = sum + h_i * exp(-((x-x_i)/x_si)^2 - ((y-y_i)/y_si)^2);
        end
        peaksData(x,y) = sum;               % 对于每一个(x,y)值都需要计算一次sum
    end
end

%% 构造曲面网格，用于后期MAP图插值判断三维路径是否与山峰交涉
% 每个点都是x,y,z构成，将x,y,z都转换成10000*1的矩阵

% x列向量
x = [];                                     
for i = 1:mapRange(1)
    % ;表示添加到列，每次添加100行的i值(i = 1:100), 添加100次, 1-100行为1, 101到200行为2
    x = [x; ones(mapRange(2),1) * i];       % ones(m,n)	构建m*n的矩阵，矩阵元素都是1
end

% y列向量(repmat函数)，结果是10000 * 1的列向量
y = (1:mapRange(2))';                           % 100 * 1的矩阵, '表示转置，1-100行分别是1:100
y = repmat(y,length(peaksData(:))/length(y),1); % y 由100行1列的 y 平铺而成，10000/100 = 100
% y = repmat(y, 100, 1);

% peaksData列向量, 即点(x,y)对应的z值。将原来的100*100的矩阵，重置为10000*1的列向量(reshape函数)
peaksData = reshape(peaksData,length(peaksData(:)),1);% 

% 构造X/Y/Z网格数据
% linspace(min(x),max(x),100)'是 100 * 1的矩阵, 值分别是1~100(插值100个数据，插值数量越大，网格越精细)
% linspace(min(y),max(y),100) 是 1 * 100的矩阵, 值分别是1~100 
[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     % 各小曲面之间不要网格

3 散点拟合

% 拟合散点

clc
clear
close all

%% 根据散点获得拟合曲线三维路径, 定义5个散点
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);                  % k = 5
t_seq = linspace(0,1,k);            % t = [0,0.25,0.50,0.75,1]
T_seq = linspace(0,1,100);          % 生成100个点

% X_seq是与T_seq对应的坐标值，计算的方法主要是采用三次多项式
X_seq = spline(t_seq,x_seq,T_seq);  % 根据t_seq和x_seq的5个点进行100次插值
Y_seq = spline(t_seq,y_seq,T_seq);
Z_seq = spline(t_seq,z_seq,T_seq);

%% 画拟合曲线图
figure
hold on
% 100 表示点的大小，'bs'中b表示blue,s表示square，蓝色方框，里面是绿色填充。绘制散点
scatter3(x_seq, y_seq, z_seq, 100, 'bs','MarkerFaceColor','g') % 对应最开始的5个散点
% plot3是绘制曲线的，r表示红色，线宽为2
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('挠率图')