基于鳄鱼伏击算法的多无人机三维协同路径规划及 Matlab 实现

多无人机三维协同路径规划：基于鳄鱼伏击算法

引言

在电力巡检、应急救援、地理测绘等复杂场景中，多无人机协同作业凭借效率高、覆盖范围广、任务容错性强等优势，逐渐取代单一无人机成为主流作业模式。三维路径规划作为多无人机协同的核心技术，需解决三大核心问题：一是复杂三维环境（如地形起伏、障碍物分布）下的路径可行性；二是多无人机间的避碰协同（避免飞行冲突）；三是路径的全局最优性（兼顾飞行距离、能耗、时间成本）。

传统多无人机路径规划算法（如 PSO、GA、A*）存在协同机制复杂、三维环境适应性差、易陷入局部最优等缺陷。鳄鱼伏击算法（Crocodile Attack Optimization Algorithm, CAOA）是一种新型群智能优化算法，模拟鳄鱼群体的协同狩猎行为（潜伏、包围、突袭），具有全局寻优能力强、收敛速度快、参数设置简单等优势。本文针对多无人机三维路径规划需求，提出改进鳄鱼伏击算法（ICAOA），通过引入三维环境适配机制、多机协同避碰策略与动态目标权重，实现自定义数量无人机的高效协同路径规划。

核心技术原理剖析

原始鳄鱼伏击算法（CAOA）核心机制

CAOA 模拟鳄鱼群体在狩猎过程中的三大行为：

1. 潜伏阶段：鳄鱼个体在水域中分散潜伏，通过感知猎物位置调整自身状态，对应算法的全局探索阶段；
2. 包围阶段：鳄鱼群体根据猎物位置与自身分布，协同移动形成包围圈，对应算法的局部开发阶段；
3. 突袭阶段：鳄鱼群体向猎物发起同步突袭，对应算法的最优解更新阶段。

CAOA 的数学建模如下：

仿真结果展示

经过算法迭代优化后，我们可以得到满足约束条件的飞行轨迹。下图展示了不同视角下的路径规划效果，包括三维立体视图和二维俯视投影，能够直观反映无人机相对于地形的安全高度及避障情况。

核心绘图函数实现

为了将优化后的节点坐标映射到实际地形并可视化，我们编写了 PlotSolution 函数。该函数负责处理路径平滑、起点终点标记以及威胁区域（如禁飞区）的绘制。

%{
This function will plot:
- model with a terrain map and obstacles
- solutions with different views
%}
function [lenr1,lenr2]=PlotSolution(sol,model,CorStr,gca1,gca2)
% global model
smooth = 0.99;
    %% Plot 3D view
% PlotModel(model)
    x=sol.x;
    y=sol.y;
    z=sol.z;
    % Start location
    xs=model.start(1);
    ys=model.start(2);
    zs=model.start(3);
    % Final location
    xf=model.end(1);
    yf=model.end(2);
    zf=model.end(2);
    x_all = [xs x xf];
    y_all = [ys y yf];
    z_all = [zs z zf];
    N = size(x_all,2); % real path length
   % Path height is relative to the ground height
    for i = 1:N
        z_map = model.H(round(y_all(i)),round(x_all(i)));
        z_all(i) = z_all(i) + z_map;
    end
    % given data in a point matrix, xyz, which is 3 x number of points
    xyz = [x_all;y_all;z_all];
    [ndim,npts]=size(xyz);
    xyzp=zeros(size(xyz));
    for k=1:ndim
       xyzp(k,:)=ppval(csaps(1:npts,xyz(k,:),smooth),1:npts);
    end
    figure(gca1);
    PlotModel(model)
    lenr1=plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),CorStr,'LineWidth',2);
    % plot start point
    plot3(x_all(1),y_all(1),z_all(1),'ks','MarkerSize',7,'MarkerFaceColor','k');
    % plot target point
    plot3(x_all(N),y_all(N),z_all(N),'ko','MarkerSize',7,'MarkerFaceColor','k');
%     hold off;
    text(x_all(1),y_all(1),z_all(1),' 起点')
    text(x_all(N),y_all(N),z_all(N),' 终点')
    %% Plot top view
    figure(gca2);
    mesh(model.X,model.Y,model.H); % Plot the data
    colormap summer;            % Default color map.
    set(gca, 'Position', [0 0 1 1]); % Fill the figure window.
    axis equal vis3d on;        % Set aspect ratio and turn off axis.
    shading interp;             % Interpolate color across faces.
    material dull;              % Mountains aren't shiny.
%     camlight left;            % Add a light over to the left somewhere.
%     lighting gouraud;         % Use decent lighting.
    xlabel('\it\fontname{Times New Roman}x/m');
    ylabel('\it\fontname{Times New Roman}y/m');
    zlabel('\it\fontname{Times New Roman}z/m');
    hold on
    % Threats as cylinders
    threats = model.threats;
    threat_num = size(threats,1);
    for i = 1:threat_num
        threat = threats(i,:);
        threat_x = threat(1);
        threat_y = threat(2);
        threat_z = max(max(model.H))+1;  % choose z to be the highest peak
        threat_radius = threat(4);
        for j=1:3 
        % Define circle parameters:
        % Make an array for all the angles:
        theta = linspace(0, 2 * pi, 2000);
        % Create the x and y locations at each angle:
        x = threat_radius * cos(theta) + threat_x;
        y = threat_radius * sin(theta) + threat_y;
        % Need to make a z value for every (x,y) pair:
        z = zeros(1, numel(x)) + threat_z;
        % Do the plot:
        % First plot the center:
        plot3(threat_x, threat_y, threat_z, 'o', 'color', 'y', 'MarkerSize', 3, 'MarkerFaceColor','m');
        % Next plot the circle:
        plot3(x, y, z, '-', 'color', 'k', 'LineWidth', 1);
        % Repeat for a smaller radius
        threat_radius = threat_radius - 20;
        end
    end
    % plot path
    lenr2=plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),CorStr,'LineWidth',2);
    % plot start point
    plot3(x_all(1),y_all(1),z_all(1),'ks','MarkerSize',7,'MarkerFaceColor','k');
    % plot target point
    plot3(x_all(N),y_all(N),z_all(N),'ko','MarkerSize',7,'MarkerFaceColor','k');
    text(x_all(1),y_all(1),z_all(1),' 起点')
    text(x_all(N),y_all(N),z_all(N),'  终点')
    % Set top view
    view(0,90)
%     hold off;
    %% Plot side view
%     figure(gca3);
%     mesh(model.X,model.Y,model.H); % Plot the data
%     colormap summer;            % Default color map.
%     set(gca, 'Position', [0 0 1 1]); % Fill the figure window.
%     axis equal vis3d on;        % Set aspect ratio and turn off axis.
%     shading interp;             % Interpolate color across faces.
%     material dull;              % Mountains aren't shiny.
%     camlight left;              % Add a light over to the left somewhere.
%     lighting gouraud;           % Use decent lighting.
%     xlabel('\it\fontname{Times New Roman}x/m');
%     ylabel('\it\fontname{Times New Roman}y/m');
%     zlabel('\it\fontname{Times New Roman}z/m');
%     hold on
%     % plot path
%     plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),CorStr,'LineWidth',2);
%     % plot start point
%     plot3(x_all(1),y_all(1),z_all(1),'ks','MarkerSize',7,'MarkerFaceColor','k');
%     % plot target point
%     plot3(x_all(N),y_all(N),z_all(N),'ko','MarkerSize',7,'MarkerFaceColor','k');
% %     text(x_all(1),y_all(1),z_all(1),' Start')
% %     text(x_all(N),y_all(N),z_all(N),'  End')
%     view(90,0);
% %     hold off;
end

在实际运行中，注意代码中对威胁区域的处理，这里将其建模为圆柱体障碍物，确保规划路径不会穿越禁飞区。同时，利用 csaps 进行样条平滑，能有效减少无人机的机动频率，降低能耗。