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

425 阅读5分钟

1 算法介绍

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

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

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

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

A星算法

而在 8 连通图 中

A*8连通图

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

xy 坐标值(也是结点唯一性的标识) [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 dictionarypara.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 reconstructeddatapath = 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 maskmeas = 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 仿真结果

img

img

img

4 参考文献

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

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

博主简介:擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,相关matlab代码问题可私信交流。