【路径规划】基于蚁群算法和匈牙利算法的二维多无人机路径规划matlab源码

匈牙利算法是由匈牙利数学家Edmonds于1965年提出，因而得名。匈牙利算法是基于Hall定理中充分性证明的思想，它是部图匹配最常见的算法，该算法的核心就是寻找增广路径，它是一种用增广路径求二分图最大匹配的算法。

维基：

设G=(V,E)是一个无向图。如顶点集V可分区为两个互不相交的子集V1,V2之并，并且图中每条边依附的两个顶点都分属于这两个不同的子集。则称图G为二分图。二分图也可记为G=(V1,V2,E)。

给定一个二分图G，在G的一个子图M中，M的边集{E}中的任意两条边都不依附于同一个顶点，则称M是一个匹配。 选择这样的子集中边数最大的子集称为图的最大匹配问题(maximal matching problem)

如果图的所有顶点都与某匹配中的一条边相关联，则称此匹配为完全匹配，也称作完备，完美匹配。

　求最大匹配的一种显而易见的算法是：先找出全部匹配，然后保留匹配数最多的。但是这个算法的时间复杂度为边数的指数级函数。因此，需要寻求一种更加高效的算法。下面介绍用增广路求最大匹配的方法(称作匈牙利算法，由数学家Harold Kuhn于1955年提出)。

　增广路的定义(也称增广轨或交错轨)：

　　若P是图G中一条连通两个未匹配顶点的路径，并且属于M的边和不属于M的边(即已匹配和待匹配的边)在P上交替出现，则称P为相对于M的一条增广路径。（M为一个匹配）

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。
1. 构建解空间将各个蚂蚁随机地置于不同的出发点，对每个蚂蚁k（k=1,2,3…m），按照（2-1）计算其下一个待访问城市，直到所有蚂蚁访问完所有城市。
2. 更新信息苏计算每个蚂蚁经过路径长度Lk(k=1,2,…，m），记录当前迭代次数中的最优解（最短路径）。同时，根据式（2-2）和（2-3）对各个城市连接路径上信息素浓度进行更新。
3. 判断是否终止若iter<iter_max，则令iter=iter+1,清空蚂蚁经过路径的记录表，并返回步骤2；否则，终止计算，输出最优解。
4. 判断是否终止若iter<iter_max，则令iter=iter+1,清空蚂蚁经过路径的记录表，并返回步骤2；否则，终止计算，输出最优解。3. 判断是否终止若iter<iter_max，则令iter=iter+1,清空蚂蚁经过路径的记录表，并返回步骤2；否则，终止计算，输出最优解。

4简单案例–依据蚁群算法的步骤求解二元函数最小值优化二元函数:
（4-1）优化思路：首先，通过对约束条件求解, 我们可以得到满足约束条件的可行解所构成的搜索空间。这是一个二维的凸空间,如果把各个分量的范围搜索等分成离散的点,那么空间整个的大小可以由分量xi的划分粗细来确定。假设,xi被划分成了Ni份,那么整个搜索空间就有2*Ni个。而且为了求得精确解将分量 xi 的区间划分得很细, 那么搜索空间将变得十分巨大, 这也正是问题求解的困难所在,当然这种划分区间的方法也是许多优化算法所一般采用的方法。)将每个分量的取值区间等分成N个点,那么xi就有N种选择二维决策量x就有N^2种选择组合。并将每个分量的N个取值点看作N个城市City。最开始将m个蚂蚁Ant随机地放到x1的N中的m个City上。搜索开始后,Ant按照转移概率进行城市选择。在第一次选择时，用rand函数随机选择，之后可按照选择概率转移。
本案例的具体步骤为:(1)求解约束条件得出各分量的区间，由题意知[-1,1];(2)将分量区间细化,建立搜索空间,本案例细分成73份,xi=[-1,-1+2/73,…1];(3)判断精度是否满足要求,若满足,则输出最优解并结束, 否则继续(4);(4)每个路径上设置信息量初始值;(5)随机选择路径求解;(6)每个蚂蚁按照选择概率来选择路径求解；
(7)若每个蚂蚁都找到解,则继续(8),否则转(4);(8)将当前最优解的分量再细化, 转(2)。

clc;

clear;

%% initialization


% Define the position of the robots

% % Robot_position=[1,2;1,4;1,6;1,8;1,10];

% Robot_position=[5,7;3,2;7,13;6,9;5,5];

% % Define the target positions

% Target_position=[3,6;5,4;5,6;5,8;8,6];


% Random position

UAV_number=2; % The number of UAVs

task_number=5; % The number of Target positions

SizeofMap = [1 100];

size_UAV = 0;

size_task = 0;


while (size_UAV<UAV_number && size_task < task_number)

UAV_position = randi(SizeofMap,UAV_number,2);

Target_position = randi(SizeofMap,task_number,2);

% UAV_position = unique(UAV_position,'rows');

% Target_position = unique(Target_position,'rows');

size_UAV = size(unique(UAV_position,'rows'),1);

size_task = size(unique(Target_position,'rows'),1);

end


% Initial the speed of UAVs

UAV_speed=ones(UAV_number,1)*50;



% UAV_position = randi([1,15],10,2);

% Target_position = randi([1,15],10,2);


% UAV_number=5; % The number of UAVs

% task_number=5; % The number of Target positions

maxT=10; % The maximum tasks can be done by a single worker

task_fixed_number=1; % The number of workers are required for a single task

% unfinishtask_number=task_number;


ant_num_TA=50; % The numbder of ants in Task Allocation

ant_num_PP=30; % The numbder of ants in Path Planning

iteratornum_TA=30; % The iteration times in Task Allocation

iteratornum_PP=10; % The iteration times in Path Planning



% pheromoneMatrix = [];% The matrix to store the pheromone

% maxPheromoneMatrix = []; % The assigned workers based on the pheromone matrix

% criticalPointMatrix = [];% This matrix is used to decide allocation by

% % using pheromone matrix or randomly

%

% % Initialize the last 3 matrix

% % The pheromone are initialized as all '1'

% pheromoneMatrix = ones(task_number,UAV_number);

% for i=1:task_number

% % From beginning, assign the workers randomly

% maxPheromoneMatrix(i) =unidrnd(UAV_number) ;

% % Once the value less or equal to 1, the allocation is based on the

% % pheromone matrix; otherwise, it is allocated randomly. For

% % initialization, the tasks are allocated randomly.

% criticalPointMatrix(i)=ceil(ant_num/task_number );

% end


%% The implementation of Ant Colony Algorithm



%task_number = task_number_new;

tic;

best_ant_path = AntColonyTaskAllocation(UAV_position,Target_position,UAV_number,UAV_speed,task_number,...

ant_num_TA, iteratornum_TA, maxT,task_fixed_number, ant_num_PP, iteratornum_PP);

toc;


[row,col]=find(best_ant_path==1);


for i = 1: UAV_number

mid = find(col==i);

possibility(i) =length(mid);

end

%

% task_number_imme = task_number;

Best_Strategy_entire = zeros( max(possibility),UAV_number);

for i = 1: UAV_number

mid = find(col==i);

for j = 1: length(mid)

Best_Strategy_entire(j,i)=row(mid(j));

end

end




%% Display the task allcocation strategy


count = ones(1,UAV_number); % count which task the UAV is handling

while (isempty (Target_position) == 0)


for i = 1: UAV_number

Best_Strategy(1,i) = Best_Strategy_entire(count(i),i);

end


for j = 1:size(Best_Strategy,2)

if(Best_Strategy(1,j) == 0)

%Best_Strategy_updated(i,j)=0;

%Target_position(Best_Strategy(1,j),:) = UAV_position( j, :);

UAV_step(j,:)=[0, 0];

%task_number=task_number+1;

else

% end


% for j =1: size(Best_Strategy,2)

difference(j,:)=Target_position(Best_Strategy(j),:)-UAV_position(j,:);

if (difference(j,1)==0)

angle(j)= pi/2;

else

angle(j)= atan(difference(j,2)/difference(j,1));

end

% end

%end


% for i= 1: size(Best_Strategy,1)

% for j= 1: size(Best_Strategy,2)

if (difference(j,1) > 0)

UAV_step(j,1)= UAV_speed(j)*cos(angle(j));

UAV_step(j,2)= UAV_speed(j)*sin(angle(j));

elseif(difference(j,1) < 0)

UAV_step(j,1)= -UAV_speed(j)*cos(angle(j));

UAV_step(j,2)= -UAV_speed(j)*sin(angle(j));

elseif ((difference(j,1) == 0)&& (difference(j,2) > 0))

UAV_step(j,1)= UAV_speed(j)*cos(angle(j));

UAV_step(j,2)= UAV_speed(j)*sin(angle(j));

elseif ((difference(j,1) == 0)&& (difference(j,2) < 0))

UAV_step(j,1)= - UAV_speed(j)*cos(angle(j));

UAV_step(j,2)= - UAV_speed(j)*sin(angle(j));

end

end

end


[UAV_position_new,Target_position_new,task_number, count] = Draw_Strategy(UAV_position,Target_position,...

Best_Strategy, SizeofMap, UAV_step, UAV_speed, task_number, count);

if (task_number == 0)

break;

标签：蚂蚁,路径,信息,UAV,算法,源码,蚁群
来源： https://blog.51cto.com/u_15287693/3025133