​✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍一、研究背景与问题建模1. 工程场景低空突防无人机 / 敌方制导导弹作为威胁目标我方防空拦截无人机需要完成攻防协同路径规划核心双重需求拦截任务快速逼近敌方目标缩短交会时间防空规避规避敌方雷达探测区、火力杀伤区、地形障碍物建筑、山体约束无人机最大速度、最大转弯角、航程上限、禁飞区硬约束。传统单一算法缺陷A*静态栅格最优但动态多威胁、多机协同、全局最优寻优能力弱多目标权衡差标准遗传算法 GA全局搜索强但局部路径平滑性差、局部寻优慢易出现折线、碰撞风险混合方案GA 负责全局路径节点寻优A * 负责节点间局部平滑安全路径插值兼顾全局多目标最优与局部避障平滑。⛳️ 运行结果 部分代码function [path,n_points]a_star_3D(K,E3d_safe,x0_old,y0_old,z0_old,xend_old,yend_old,zend_old,sizeE)​%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Cost weightskg K(1);kh K(2);​%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Algorithm​% Round start / end to integer grid indices (use floor/ceil as original)x0 floor(x0_old);y0 floor(y0_old);z0 floor(z0_old);xend ceil(xend_old);yend ceil(yend_old);zend ceil(zend_old);​% Sizey_size sizeE(1);x_size sizeE(2);z_size sizeE(3);​% Guard: if start or end are blocked in E3d_safe, abort earlyif E3d_safe(y0, x0, z0)warning(a_star_3D: start voxel is blocked in E3d_safe. Returning empty path.);path [];n_points 0;return;endif E3d_safe(yend, xend, zend)warning(a_star_3D: end voxel is blocked in E3d_safe. Returning empty path.);path [];n_points 0;return;end​% Node from which the good neighbour is reachedcame_fromx zeros(sizeE);came_fromy zeros(sizeE);came_fromz zeros(sizeE);came_fromx(y0, x0, z0) x0;came_fromy(y0, x0, z0) y0;came_fromz(y0, x0, z0) z0;​open_list sub2ind(sizeE, y0, x0, z0); % linear indices of open nodesis_open false(sizeE); is_open(y0,x0,z0) true;is_closed false(sizeE);​% G and F arrays (initialize to Inf)G Inf(sizeE);F Inf(sizeE);G(y0, x0, z0) 0;F(y0, x0, z0) sqrt((xend-x0)^2 (yend-y0)^2 (zend-z0)^2);​% Initializeexit_path 0;​% Main loop: while open not empty and path not foundwhile ~isempty(open_list) exit_path0​% vectorized selection of best node in open_list (fast)[~, idx_min] min( F(open_list) ); % vectorized, no loopcurrent_lin open_list(idx_min);[ycurrent, xcurrent, zcurrent] ind2sub(sizeE, current_lin);​% Check arrivalif xcurrentxend ycurrentyend zcurrentzendexit_path 1;break;end​% move current from open to closed (O(1) with masks and list swap)is_open(current_lin) false;is_closed(current_lin) true;% remove from open_list by swapping with last element for O(1) removalif idx_min numel(open_list)open_list(idx_min) open_list(end);endopen_list(end) [];% now open_list updated​% Explore neighbours (26-neighborhood)for di -1:1for dj -1:1for dk -1:1​% skip selfif di0 dj0 dk0continue;end​xn xcurrent di;yn ycurrent dj;zn zcurrent dk;​if xn 1 || yn 1 || zn 1 || xn x_size || yn y_size || zn z_sizecontinue;end​neigh_lin sub2ind(sizeE, yn, xn, zn);​% skip if blockedif E3d_safe(yn, xn, zn)continue;end​% If neighbour already closed, skipif is_closed(neigh_lin)continue;end​% compute tentative gcost norm([xn-xcurrent, yn-ycurrent, zn-zcurrent]);g_try G(ycurrent, xcurrent, zcurrent) cost;​if ~is_open(neigh_lin)% first time discovered: add to openis_open(neigh_lin) true;open_list(end1) neigh_lin;% set came_from nowcame_fromy(yn, xn, zn) ycurrent;came_fromx(yn, xn, zn) xcurrent;came_fromz(yn, xn, zn) zcurrent;% set scoresG(yn, xn, zn) g_try;H sqrt((xend - xn)^2 (yend - yn)^2 (zend - zn)^2);F(yn, xn, zn) kg * G(yn, xn, zn) kh * H;else% already in open: check for improvementif g_try G(yn, xn, zn)came_fromy(yn, xn, zn) ycurrent;came_fromx(yn, xn, zn) xcurrent;came_fromz(yn, xn, zn) zcurrent;G(yn, xn, zn) g_try;H sqrt((xend - xn)^2 (yend - yn)^2 (zend - zn)^2);F(yn, xn, zn) kg * G(yn, xn, zn) kh * H;endend​endendend​end​% If no path found, return emptyif exit_path 0path [];n_points 0;return;end​% Reconstruct path backwards from current (which is the goal)path_backwards [ycurrent, xcurrent, zcurrent];i 2;while xcurrent ~ x0 || ycurrent ~ y0 || zcurrent ~ z0py came_fromy(ycurrent, xcurrent, zcurrent);px came_fromx(ycurrent, xcurrent, zcurrent);pz came_fromz(ycurrent, xcurrent, zcurrent);​% Safety: if came_from is zero (no parent), abort and return empty with warningif py 0 || px 0 || pz 0warning(a_star_3D: reconstruction failed, missing came_from for a node. Returning empty path.);path [];n_points 0;return;end​path_backwards(i, :) [py, px, pz];ycurrent py;xcurrent px;zcurrent pz;i i 1;end​% Number of waypointsn_points size(path_backwards, 1);​% Reverse path sequence to be from start to goalpath path_backwards(n_points 1 - (1:n_points), :);​% Reassign original non-integer start/end coordinatespath(1, :) [y0_old, x0_old, z0_old];path(n_points, :) [yend_old, xend_old, zend_old];​end​ 参考文献[1]陈浩,李军,唐宇,等.基于动态罚函数遗传算法的电磁探测卫星多星规划方法[J].国防科技大学学报, 2009, 31(2):7.DOI:10.3969/j.issn.1001-2486.2009.02.010.往期回顾扫扫下方二维码Matlab科研助手推荐搜索论文复现