【无人机】基于A星算法实现三维栅格地图路径规划matlab代码

1 算法介绍

A*搜寻算法俗称A星算法。这是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。常用于游戏中的NPC的移动计算，或线上游戏的BOT的移动计算上。（拷自百度百科）是常用搜索算法中，能够以最短时间来求得最短路径的一种启发式搜索算法。兼具广搜和深搜的优点，那么比较起广搜和深搜这些盲目搜索，A*有什么特点呢？A*搜索的效率就在于：以深搜的形式来扩展已知结点中的最优结点，而最优结点的定位又类似于广搜。 

估计大家现在也不知道我在乱七八糟说些什么。下面正式开始介绍A*算法。

 在构建A*搜索函数主体之前，我们需要一个估价函数（ f ），来评估通过当前到目标状态的代价（大家可以理解为，通过这条路走到目的地有多长的路程）

其中  f（估价函数值） = actual（已消耗的实际代价）+ estimate（还可能再消耗的预估代价）actual 无需多说，便是从搜索起始结点到当前扩展结点的代价（距离）总和； estimate 的确定，需要用到曼哈顿距离（Manhattan Distance），   在简单的二维地图中，我们可以认为，上下左右4个方向上的代价为10，而斜向的4个方向的代价为14（√(200) 的近似值 [1] ）

而在 8 连通图 中

在A*搜索所使用到的结点的结构：

       x、y              坐标值（也是结点唯一性的标识）[2]
        actual          上面提到的“真实值”
        estimate      预估值
        father          父结点地址（也可以是 父结点的坐标值 或 被拓展的方向 ）

在此补充一下结点状态
        活结点：尚未扩展的结点
        死结点：已扩展完的结点
        扩展结点：当前正在扩展的结点

下面给出A*算法的主步骤

（1）将 targetNode（目标结点）放入 open 表，actual = 0; estimate = Esitimate( &startNode, &targetNode )。[3]

（2）进入循环，重复下列过程；如果 open 表为空，则搜索失败，既：target 与 Start 之间不存在通路。

（3）遍历 open 表，将最小的 f 值（actual + Esitimate）选为 bestNode，并从 open 表中删除，放入 close 表。

（4）判断 bestNode 是否为 startNode ，若是，则成功搜索到了最短路径。

（5）若不是，开始遍历 8个方向，扩展 bestNode。

            （a）根据方向和 bestNode 的坐标计算子结点坐标，子结点的 actual = bestNode 的 actual + 10（上向左右）或 14（斜向），
                    estimate = Estimate( &子结点, &startNode )，father = bestNode。
            （b）判断 bestNode 的子结点 是否在 close 表中，若是，loop （5）。
            （c）若不是，判断子结点是否在 open 表中，若是，开始（i），否则执行（d）；

                        （i） 再判断 open 表中记录的结点的 actual 值是否大于 当前子结点的 actual 值（新旧路径代价比较）；
                        （ii）若是，令 open 表中记录的结点的 actual = 当前子结点的 actual，令 open 表中记录的结点的 father 指向bestNode。loop （5）。
            （d）否则（子结点既不在 open 表中，又不在 close 表中），将子结点记入 open 表，loop（5）。 

（6）loop （2）

2 部分代码

% 'test_DeSCI.m' tests 'decompress snapshot compressive imaging (DeSCI)' algorithm for video reconstruction in
% 'coded aperture compressive temporal imaging (CACTI)'
 
% Reference
%   [1] M. Qiao, Z. Meng, J. Ma, X. Yuan, Deep learning for video compressive
%       sensing, APL Photonics 5, 030801 (2020).
%   [2] Y. Liu, X. Yuan, J. Suo, D.J. Brady, and Q. Dai, Rank Minimization 
%       for Snapshot Compressive Imaging, IEEE Trans. Pattern Anal. Mach. 
%       Intell. (TPAMI), DOI:10.1109/TPAMI.2018.2873587, 2018.
 
 
% Contact
%   Xin Yuan, Bell Labs, xyuan@bell-labs.com
%   Mu Qiao, New Jersey Institute of Technology, muqiao@njit.edu
%   Update Mar 13, 2020.
 
 
%% [0] environment configuration
clear; 
clc;
close all
 
addpath(genpath('./DeSCI_algorithm')); % algorithms
datasetdir = './dataset';                % dataset dictionary
 
para.dataname = 'meas_waterBalloon_cr_10';    % selected 2D measurement
para.cr = str2double(para.dataname(end-1:end));    % compression ratio of the selected measurement, i.e., number of video frames to be recovered from each single 2D measurement
para.numRec = 1;                              % number of measurement frames to be reconstructed
 
datapath = sprintf('%s/%s.mat',datasetdir,para.dataname);  % path of the selected 2D measurement
 
%% [1] load dataset
 
load(datapath);               % load measurement
load('./dataset/mask.mat');   % load mask
 
meas = meas(:,:,1:para.numRec);
meas = 1850*meas./max(meas(:));
mask  = double(mask(:,:,1:para.cr));
 
[nrow,ncol,~] = size(meas);
 
%% [2] ADMM-WNNM-TV
para.nframe =   para.numRec; 
para.MAXB   = 255;
MAXB = para.MAXB;
 
para.Mfunc  = @(z) A_xy(z,mask);
para.Mtfunc = @(z) At_xy_nonorm(z,mask);
para.Phisum = sum(mask.^2,3);
para.Phisum(para.Phisum==0) = 1;
 
para.flag_iqa = false; % disable image quality assessments in iterations
para.acc = 1; % enable acceleration
para.flag_iqa = false; % disable image quality assessments in iterations
para.projmeth = 'admm_res'; % projection method
%  (GAP for noiseless or ADMM for noisy)
para.gamma  =    1; % regularization factor for noise suppression
para.denoiser = 'wnnm'; % WNNM denoising
para.wnnm_int_fwise = true; % enable GAP-WNNM integrated (with frame-wise denoising)
para.blockmatch_period = 20; % period of block matching
para.sigma   = [50 45 40]/MAXB; % noise deviation (to be estimated and adapted)
para.vrange   = 1; % range of the signal
para.maxiter = [50 50 50]; % first trial
para.patchsize = 24; % patch size
para.iternum = 1; % iteration number in WNNM
para.enparfor = true; % enable parfor
para.numworkers = 12;
if para.enparfor % if parfor is enabled, start parpool in advance
    mycluster = parcluster('local');
    delete(gcp('nocreate')); % delete current parpool
    ord = 0;
    while para.cr/2^ord > mycluster.NumWorkers
        ord = ord+1;
    end
    poolobj = parpool(mycluster,min(max(floor(para.cr/2^ord),1),para.numworkers));
end
 
[vadmmwnnmtv,~,~,tadmmwnnm] = ...
    admmdenoise_cacti(mask,meas,[],[],para);
 
%% [3] show results in figure
 
% [3.0] rotate and crop
recon = vadmmwnnmtv;
recon_rotate = zeros(725,725,para.numRec*para.cr);
 
for np=1:para.numRec*para.cr
    recon_rotate(:,:,np) = imrotate(recon(:,:,np),-135);
end
 
recon_rotate = recon_rotate(182:182+363,182:182+363,:);
recon_rotate = recon_rotate/max(recon_rotate(:));
 
% [3.1] show results in figure
figure;
for i=1:para.numRec*para.cr
    imshow(recon_rotate(:,:,i));
    pause(0.2);
end
 

3 仿真结果

4 参考文献

[1]王云常,戴朱祥,李涛.基于A星算法与人工势场法的无人机路径规划[J].扬州大学学报(自然科学版),2019,22(03):36-38+49.

5 代码下载