【路径规划】基于蚁群算法的三维路径动态仿真

一、简介

1 蚁群算法的提出
蚁群算法(ant colony optimization, ACO)，又称蚂蚁算法，是一种用来寻找优化路径的机率型算法。它由Marco Dorigo于1992年在他的博士论文中提出，其灵感来源于蚂蚁在寻找食物过程中发现路径的行为。遗传算法在模式识别、神经网络、机器学习、工业优化控制、自适应控制、生物科学、社会科学等方面都得到应用。
2 算法的基本原理
在这里插入图片描述

二、源代码

function [sys,x0,str,ts] = quadrotor_plot(t,x,u,flag,s,plot,enable,vehicle)
 
    ts = [-1 0];
    
    if ~isfield(vehicle, 'nrotors')
        vehicle.nrotors = 4;    % sensible default for quadrotor function
    end
    
    switch flag,
        case 0
            [sys,x0,str,ts] = mdlInitializeSizes(ts,plot,enable); % Initialization
        case 3
            sys = mdlOutputs(t,u,s,plot,enable, vehicle); % Calculate outputs
        case {1,2, 4, 9} % Unused flags
            sys = [];
        otherwise
            error(['unhandled flag = ',num2str(flag)]); % Error handling
    end
    
    
    % Initialize
function [sys,x0,str,ts] = mdlInitializeSizes(ts,plot,enable)
    % Call simsizes for a sizes structure, fill it in, and convert it
    % to a sizes array.
    sizes = simsizes;
    sizes.NumContStates  = 0;
    sizes.NumDiscStates  = 0;
    sizes.NumOutputs     = 0;
    sizes.NumInputs      = 6;
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    x0  = [];
    str = [];          % Set str to an empty matrix.
    ts = [0.05 0];
    
    if enable == 1
        figure(plot);
        clf;
        %colordef(1,'none');
    end
    % End of mdlInitializeSizes.
    
    
function sys = mdlOutputs(t,u,s, plot, enable, quad)
    global a1s b1s
    
    % not quite sure what this is about -- PIC
    if numel(a1s) == [0];
        a1s = zeros(1, quad.nrotors);
        b1s = zeros(1, quad.nrotors);
    end
    
    % vehicle dimensons
    d = quad.d; %Hub displacement from COG
    r = quad.r; %Rotor radius

    for i = 1:quad.nrotors
        theta = (i-1)/quad.nrotors*2*pi;
        %   Di      Rotor hub displacements (1x3)
        D(:,i) = [ d*cos(theta); d*sin(theta); 0];
        scal = s(1)/4;
        %Attitude center displacements
        C(:,i) = [ scal*cos(theta); scal*sin(theta); 0];
    end