【飞行器】基于matlab蚁群算法飞行器巡检路径【含Matlab源码 268期】

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【飞行器】基于matlab蚁群算法飞行器巡检路径【含Matlab源码 268期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：
订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、四旋翼飞行器简介

0 引 言
四旋翼飞行器由于具有可垂直起降、机动性强、操作方便等诸多优点,在军事和民用场合得到广泛应用,从而成为众多学者的研究热点。四旋翼飞行器是具有四输入、六输出的欠驱动、非线性、强耦合系统。其姿态控制精度和抗干扰问题一直是研究重点。目前国内较普遍的飞行器控制算法主要包括:反步法、自适应控制、H控制、滑模控制、自抗扰控制等,对实现四旋翼飞行器的姿态控制具有重要的理论和实践意义。文献提出应用反步算法为飞行器上下、前后、左右、偏航4个子系统配置控制律,实现了四旋翼飞行器对设定轨迹的精确跟踪。该算法在构造Lyapunov函数的过程中,是以其导数小于零为前提,因此应用受到局限。文献针对传统的离散线性滑模应用于四旋翼飞行器控制具有跟踪误差大、响应速度慢、不能有限时间收敛等问题,提出了干扰观测器补偿的自适应离散终端滑模控制,使响应时间更快、跟踪效果更理想、鲁棒性更强。文献利用线性扩张状态观测器对四旋翼飞行器内部不确定干扰和外部干扰进行实时估计,进而采取线性状态反馈控制对扰动的估计值进行在线补偿,以实现四旋翼飞行器的姿态控制。

1 四旋翼飞行器动力学模型的建立
1. 1 四旋翼飞行器受力分析
对于飞行器的每个旋翼,剖面呈非对称,一旦旋翼旋转,由于上表面空气流速比下表面快,故上表面受到的空气压力小于下表面,上下表面受到的压差形成升力,如图1所示。旋翼1、3逆时针旋转,旋翼2、4顺时针旋转。由叶素动量理论可知,每个旋翼产生的升力Fi与电机转速ωi的平方成正比,即Fi=kFω2i(i=1,2,3,4),其中kF为升力系数。

图1 四旋翼飞行器受力分析图
当旋翼旋转时,空气阻力会阻碍其旋转。这种阻力形成施加在机体上的反扭转力矩,当4个旋翼转速相等时,旋翼产生的反扭矩作用相互抵消。四旋翼飞行器通过改变2对正反螺旋桨的转速实现对其运动控制。在4个旋翼转速相等的情况下,同时增加或者减少4个旋翼转速,可以实现飞行器上升或者下降。如果4个旋翼产生的升力之和等于机体重力,可以实现飞行器空中悬停。保持旋翼2、4的转速不变,改变旋翼1或者旋翼3的转速,飞行器在力矩l(F3-F1)或l(F1-F3)的作用下(其中l为电机轴线到飞行器中心距离),可实现俯仰运动。保持旋翼1和旋翼3的转速不变,改变旋翼2或者旋翼4的转速,飞行器在力矩l(F4-F2)或l(F2-F4)的作用下,可实现横滚运动。如果同时改变旋翼1和旋翼3的转速,或者同时改变旋翼2和旋翼4的转速,并保持飞行器总升力与机体重力相等,飞行器会在反扭矩的作用下实现偏航运动。由此可见,实现飞行器垂直运动的升力,以及实现俯仰、横滚、偏航运动的旋转力矩可以表示为

式中: F——飞行器垂直运动的升力;
Mx、My、Mz——四旋翼俯仰、横滚和偏航运动的旋转力矩;
kF——升力系数;
kM——旋转力矩系数。

1. 2 动力学模型建立
为了描述飞行器的姿态和运动状态,需要引入地理坐标系n(X,Y,Z)与载体坐标系b(x,y,z)。地理坐标系又称为东北天坐标系,载体坐标系与飞行器固连,原点为飞行器中心。将地理坐标系与载体坐标系原点重合,并将地理坐标系分别绕X、Y、Z轴旋转3次之后可得到载体坐标系,地理坐标系到载体坐标系的转换矩阵可表示为


由式(5)和式(2)可求得地理坐标系n中飞行器在X、Y、Z轴方向所受旋翼升力向量:


在低速飞行过程中,角速度矢量较小,式(8)中左边第二项可近似认为为零,则式(8)可化简为

将式(10)转换可得:

由式(7)和式(11)可得四旋翼飞行器在低速飞行情况下的非线性动力学模型:

由式(12)可知,四旋翼飞行器线运动不影响角运动,但是角运动会影响线运动。以u1、u2、u3、u4为系统输入,通过改变这4个输入变量的值,可以改变飞行器的3个线位移和3个角位移,从而实现对飞行器的运动控制。

2 四旋翼飞行器的控制系统构建与仿真
经典PID算法结构简单,基于偏差设计反馈律,不依赖受控对象的具体数学模型,在很多过程控制中均有良好表现。尽管各种新的控制算法不断涌现,但是并没有改变PID控制算法在工业控制中的主导地位。本文根据四旋翼在飞行过程中经常会遇到不确定外界干扰等情况,设计了基于小扰动的PID控制器,如图2所示。

图2 PID控制器结构图

三、蚁群算法简介

1 蚁群算法(ant colony algorithm,ACA)起源和发展历程
Marco Dorigo等人在研究新型算法的过程中，发现蚁群在寻找食物时，通过分泌一种称为信息素的生物激素交流觅食信息从而能快速的找到目标，于是在1991年在其博士论文中首次系统地提出一种基于蚂蚁种群的新型智能优化算法“蚂蚁系统（Ant system,简称AS）”，后来，提出者及许多研究者对该算法作了各种改进，将其应用于更为广泛的领域，如图着色问题、二次分配问题、工件排序问题、车辆路径问题、车间作业调度问题、网络路由问题、大规模集成电路设计等。近些年来，M.Dorigo等人把蚂蚁算法进一步发展成一种通用的优化技术“蚁群优化（Ant Colony Optimization,简称ACO）”，并将所有符合ACO框架的算法称为“蚁群优化算法（ACO algorithm）”。

具体来说，各个蚂蚁在没有事先告知食物在什么地方的前提下开始寻找食物。当一只找到食物以后，它会向环境释放一种挥发性分泌物pheromone (称为信息素,该物质随着时间的推移会逐渐挥发消失，信息素浓度的大小表征路径的远近)信息素能够让其他蚂蚁感知从而起到一个引导的作用。通常多个路径上均有信息素时，蚂蚁会优先选择信息素浓度高的路径，从而使浓度高的路径信息素浓度更高，形成一个正反馈。有些蚂蚁并没有像其它蚂蚁一样总重复同样的路，他们会另辟蹊径，如果另开辟的道路比原来的其他道路更短，那么，渐渐地，更多的蚂蚁被吸引到这条较短的路上来。最后，经过一段时间运行，可能会出现一条最短的路径被大多数蚂蚁重复着。最终，信息素浓度最高的路径即是最终被蚂蚁选中的最优路径。
与其他算法相比，蚁群算法是一种比较年轻的算法，具有分布式计算、无中心控制、个体之间异步间接通信等特点，并且易于与其他优化算法相结合，经过不少仁人志士的不断探索，到今天已经发展出了各式各样的改进蚁群算法，不过蚁群算法的原理仍是主干。

2 蚁群算法的求解原理
基于上述对蚁群觅食行为的描述，该算法主要对觅食行为进行以下几个方面模拟：
（1）模拟的图场景中包含了两种信息素，一种表示家，一种表示食物的地点，并且这两种信息素都在以一定的速率进行挥发。
（2）每个蚂蚁只能感知它周围的小部分地方的信息。蚂蚁在寻找食物的时候，如果在感知范围内，就可以直接过去，如果不在感知范围内，就要朝着信息素多的地方走，蚂蚁可以有一个小概率不往信息素多的地方走，而另辟蹊径，这个小概率事件很重要，代表了一种找路的创新，对于找到更优的解很重要。
（3）蚂蚁回窝的规则与找食物的规则相同。
（4）蚂蚁在移动时候首先会根据信息素的指引，如果没有信息素的指引，会按照自己的移动方向惯性走下去，但也有一定的机率改变方向，蚂蚁还可以记住已经走过的路，避免重复走一个地方。
（5）蚂蚁在找到食物时留下的信息素最多，然后距离食物越远的地方留下的信息素越少。找到窝的信息素留下的量的规则跟食物相同。蚁群算法有以下几个特点:正反馈算法、并发性算法、较强的鲁棒性、概率型全局搜索、不依赖严格的数学性质、搜索时间长，易出现停止现象。
蚂蚁转移概率公式：

公式中：是蚂蚁k从城市i转移到j的概率；α，β分别为信息素和启发式因子的相对重要程度；为边（i，j）上的信息素量；为启发式因子；为蚂蚁k下步允许选择的城市。上述公式即为蚂蚁系统中的信息素更新公式，是边（i,j)上的信息素量；ρ是信息素蒸发系数，0<ρ<1;为第k只蚂蚁在本次迭代中留在边（i,j）上的信息素量；Q为一正常系数；为第k只蚂蚁在本次周游中的路径长度。
在蚂蚁系统中，信息素更新公式为：

3 蚁群算法的求解步骤：
（1）初始化参数在计算之初，需要对相关参数进行初始化，如蚁群规模（蚂蚁数量）m、信息素重要程度因子α、启发函数重要程度因子β、信息素会发银子ρ、信息素释放总量Q、最大迭代次数iter_max、迭代次数初值iter=1。
（2）构建解空间将各个蚂蚁随机地置于不同的出发点，对每个蚂蚁k（k=1,2,3…m），按照（2-1）计算其下一个待访问城市，直到所有蚂蚁访问完所有城市。
（3）更新信息苏计算每个蚂蚁经过路径长度Lk(k=1,2,…，m），记录当前迭代次数中的最优解（最短路径）。同时，根据式（2-2）和（2-3）对各个城市连接路径上信息素浓度进行更新。
（4） 判断是否终止若iter<iter_max，则令iter=iter+1,清空蚂蚁经过路径的记录表，并返回步骤2；否则，终止计算，输出最优解。
（5）判断是否终止若iter<iter_max，则令iter=iter+1,清空蚂蚁经过路径的记录表，并返回步骤2；否则，终止计算，输出最优解。3. 判断是否终止若iter<iter_max，则令iter=iter+1,清空蚂蚁经过路径的记录表，并返回步骤2；否则，终止计算，输出最优解。

四、部分源代码

%% Test Swarm Trajectory in 3-D   在三维图中测试群轨迹

% Date   : 2/17/2021
% Info    : This is a script developed to show the swarm navigation
% algorithms in 3-D. This script also allows for the user to specify
% various waypoints so that they can maneuver around a space
% 这是一个用来显示3-d的群导航算法的脚本。该脚本还允许用户指定各种路标，以便他们可以在空间中操作

clear all;
warning('off');   % 消除警告
false = 0;
true = 1;

%% (1)Setup the movie stuff 设置视频信息
make_movie = true;
% make_pot = false;    % 这个地方暂时可以理解为可以省略,与下程序相互对应;
% make video showing potential field | Takes a long time to make, so usually set to false
% 设置视频的字段需要很长时间，所以通常设置为false
writerObj = [];
% Define the plane of points to evaluate potential function, if you are making video of potential function
% 如果你制作势能函数的视频，定义点的平面来评估势能函数
x = linspace(0, 50, 100);    % 0-50中平均分100个数,组成1*100double矩阵
y = linspace(0, 50, 100);    % 0-50中平均分100个数,组成1*100double矩阵 
z = 5;                       % 这个地方的5代表的是什么？
[X,Y] = meshgrid(x,y);       % X为100*100double,Y为100*100double
% meshgrid用于从数组x和y产生网格.生成的网格矩阵X和Y大小是相同的.它也可以是更高维的.
[rx, cx] = size(X);          % rx = 100;cx = 100
Z = zeros(rx, cx);           % Z为100*100double

% Setup the movie file and frame rate 设置视频文件和帧速率
if( make_movie )
    writerObj = VideoWriter('3D_test3.avi');  % 定义一个视频文件用来存动画
    writerObj.FrameRate = 60;                 % 将帧写入视频FrameRate：帧速率(每秒钟60张图)
    open(writerObj);                          % 打开该视频文件
end
% initial perspective in video  视频的初始化角度
az = -60;
el = 20;
% final perspective in video  视频的最后角度
azf = -60;
elf = 20;


%%  (2)Parameter specifications for drones    设置无人机的参数
Nd =2;                            % number of drones in swarm                (无人机种群数量)
ind_c = -1;                       % index for center/lead drone              (中心/领头无人机的指数) 这条语句的作用是什么？
radius =1.5;                      % radius for drones and possibly obstacles (无人机和可能障碍物的半径)
dims = [0, 50; 0, 50; 0, 50];     % first row is lower and upper x bounds    (设置三维坐标的界限)
                                  % second row is lower and upper y bounds
                                  % third row is lower and upper z bounds
xc = [40,40,40]';               % initial location for central/lead drone  (中央/领头无人机的初始位置)
end_loc = [5, 5, 5]';             % Desired end location                     (预期结束位置)

%% (3)Setup the waypoint object 设置路径对象
dist_thresh = 2;                  % 这个函数的设置似乎没有什么影响              
% The distance threshold(距离阈值) the swarm must be within of the waypoint(距离阈值群必须在航路点内)
                                  % to make the waypoint change to the new one
% 群体的距离阈值必须在航点内，以使航点变为新的航点
% 如何连接航路点？
wypt = Waypoints( dist_thresh );
wypt.addPoint( [ 15; 50; 20 ] );  % first waypoint   (第一个航点，就是蓝色的点)
wypt.addPoint( [ 0; 50; 0 ] );    % second waypoint  (第二个航点)
wypt.addPoint( end_loc );         % last waypoint    (最后一个航点)

% 蓝色的点
wypt2 = Waypoints( dist_thresh ); % wypt和wypt2的区别在哪里？为什么优先考虑wypt2的改变值     
wypt2.addPoint( [ 15; 50; 20 ] ); % first waypoint
wypt2.addPoint( [ 0; 50; 0 ] );   % second waypoint
wypt2.addPoint( end_loc );        % last waypoint


%% (4)Parameter specifications for the obstacles  设置障碍物参数
No =2;                                % 障碍物数量
i = 1;
obst = [];                            % 障碍物初始化  
dims_o = [0, 50; 0, 50; 0, 50];       % first row is lower and upper x bounds   限定障碍物的设定范围
                                      % second row is lower and upper y bounds
%% 获得障碍物的参数                                      
while( i <= No )
    % randomly generate position of obstacle within bounds  (随机在界内产生障碍物位置)
    pos = rand(3,1).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,1);   % pos是一个3*1的矩阵
    % Add new obstacle to obstacle array obst               (在障碍物数组中添加新的障碍物)
    obst = [obst, Obstacle(pos,radius)];
    i = i + 1;
end

%% (5)Create drones and obstacle arrays 创建无人机和障碍阵列
i = 1;
drones = [];
while( i <= Nd )                                                % 无人机种群数量
    if( i == ind_c )
        % Add lead drone to drone array   在无人机列阵中添加领头的无人机
        drones = [drones, Drone(radius, xc , 2)];               % 领头无人机的初始位置
    else
        % Add a follower drone to drone array  在无人机阵列中添加一个追随者无人机
        DEL = [xc - 1, xc + 1];    % make swarm initialize around lead drone randomly 让集群围绕着领头机随机初始化 控制领航机和群机之间的距离
        pos = rand(3,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1);
        drones = [drones, Drone(radius, pos, 1)];
    end
    i = i + 1;
end

%% (6)Setup figure traits for the movie  视频设置图像特征
close all;
h = figure('Position', [10, 10, 1000, 800]);
set(gca,'nextplot','replacechildren'); % set（gca,...）改变坐标轴的外观，如字号，字体，颜色等。replacechildren：在当前设置下清楚所有子对象。
set(gcf,'Renderer','zbuffer');
dims2 = dims';                         % dims这个地方指的是障碍物的范围（转置）

%% Do the initial drawing 建立最初的图像
i = 1;
figure(1)
N = 1;

% 这边暂时理解为可以省略，与前程序make_pot = false; 相互对应;
% if( make_pot )
%     N = 2;
% end

%% 无人机模型建立
subplot(1,N,1)     % 将生成的图像分别1*N的格子，将图形放在第一个格子上
while( i <=Nd )    % loop through drones  遍历无人机
    drawObject(drones(i));
    if( i == 1 )
        hold on    % 当前轴及图像保持而不被刷新，准备接受此后将绘制的图形，多图共存
    end
    i = i + 1;
end

%% 障碍物模型建立
i = 1;
while( i <= No )   % loop through obstacles 遍历障碍物
    drawObject(obst(i));
    i = i + 1;
end

%% 作图
hold off        % 使当前轴及图像不再具备被刷新的性质，新图出现时，取消原图，关闭图形保持功能
grid on         % 在画图时添加网格线
view(az, el)    % 作用为通过方位角AZ和俯视角EL设置观察图形的视点
axis(dims2(:))  % axis控制仿真图坐标的范围
xpos = [];

%% (8)Do iterations of the drone moving to final location 将无人机不断迭代并移动到终点位置
it = 1;      % 这个地方的it应该是迭代的意思（iterations）
count = 0;   % 计数
done = 0;    % 完成
while( done == 0 )
    i = 1;
    % Compute new locations     计算新的起点
    pt = wypt.getWaypoint( drones(1).pos );
    while( i <= Nd )
        drones(i).pos = drones(i).pos + GradientDescentUpdate(drones(i).pos, i, drones, obst, pt ); % GradientDescentUpdate 梯度下降法
        if( count > 20 || it >= 150 )  % 逻辑或
            done = 1;
        end
        i = i + 1;
    end
    
    % Draw drones in new position 在新的位置绘制无人机
    i = 1;    
    figure(1)
    subplot(1,N,1)
    
    while( i <= Nd )          % loop through drones  遍历无人机
        drawObject(drones(i));
        if( i == 1 )
            hold on
        end
        i = i + 1;
    end
   
    drawWaypoints( wypt2 );
    i = 1;
    while( i <= No )            % loop through obstacles 遍历障碍物
        drawObject(obst(i));
        i = i + 1;
    end
    hold off
    coef = it/100;      % 这个地方指的是什么意思？
    
    %zoom(2)
    grid on
    axis(dims2(:))
    view(az + coef*(azf - az) , el + coef*(elf - el) )
    %rotate(h, [0, 0, 1], 1)
    pause(.01)
    
%% Add frame to movie, if you are wanting to make the movie
    % 如果你想录制视频，可以添加帧
    if( make_movie && it > 3 )  % 逻辑与
        ind = 1;
        if( N > 1 )
            subplot(1, N, 2)
            ind = 1;
            for ii = 1:rx
                for jj = 1:cx
                    Z(ii, jj) = getTotalPotential( [X(ii,jj); Y(ii, jj); z(1)], ind, drones, obst, end_loc );
                end
            end
            surf(X,Y,Z)
            axis( [x(1), x(end), y(1), y(end)] )
        end     
        frame = getframe(gcf);            % 获取整个窗口内容的图像
        writeVideo(writerObj,frame);
    end
    it = it + 1;
end
% Close movie file, if you are recording a movie   如果正在录制视频，关闭视频文件
if( make_movie )
    close(writerObj);
end


  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
  
106
  
107
  
108
  
109
  
110
  
111
  
112
  
113
  
114
  
115
  
116
  
117
  
118
  
119
  
120
  
121
  
122
  
123
  
124
  
125
  
126
  
127
  
128
  
129
  
130
  
131
  
132
  
133
  
134
  
135
  
136
  
137
  
138
  
139
  
140
  
141
  
142
  
143
  
144
  
145
  
146
  
147
  
148
  
149
  
150
  
151
  
152
  
153
  
154
  
155
  
156
  
157
  
158
  
159
  
160
  
161
  
162
  
163
  
164
  
165
  
166
  
167
  
168
  
169
  
170
  
171
  
172
  
173
  
174
  
175
  
176
  
177
  
178
  
179
  
180
  
181
  
182
  
183
  
184
  
185
  
186
  
187
  
188
  
189
  
190
  
191
  
192
  
193
  
194
  
195
  
196
  
197
  
198
  
199
  
200
  
201
  
202
  
203
  
204
  
205
  
206
  
207
  
208
  
209
  
210
  
211
  
212
  
213
 

五、运行结果

六、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]张萍.四旋翼飞行器姿态控制建模与仿真[J].电机与控制应用. 2019,46(12)
[4]刘岩,杨牧.四旋翼飞行器飞行控制系统研究与设计[J].山东工业技术. 2019,(07)

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/113775326