?一、螢火蟲算法無人機(jī)避障三維航跡規(guī)劃簡介

1 無人機(jī)航跡規(guī)劃問題的數(shù)學(xué)模型
建立三維航跡規(guī)劃問題的數(shù)學(xué)模型時, 不但考慮無人機(jī)基本約束, 還考慮復(fù)雜的飛行環(huán)境, 包括山體地形和雷暴威脅區(qū)。

1.1 無人機(jī)基本約束
規(guī)劃的無人機(jī)三維航跡, 通常需要滿足一些基本約束, 包括最大轉(zhuǎn)彎角、最大爬升角或下滑角、最小航跡段長度、最低和最高飛行高度, 以及最大航跡長度等約束。其中, 最大轉(zhuǎn)彎角約束, 是指無人機(jī)只能在水平面內(nèi)小于或等于指定的最大轉(zhuǎn)彎角內(nèi)轉(zhuǎn)彎;最大爬升角或下滑角約束, 是指無人機(jī)只能在垂直平面內(nèi)小于或等于指定的最大爬升角或下滑角內(nèi)爬升或下滑;最小航跡段長度約束, 要求無人機(jī)改變飛行姿態(tài)之前, 按目前的航跡方向飛行的最短航程;最低和最高飛行高度約束, 要求無人機(jī)在指定的飛行高度區(qū)間飛行;最大航跡長度約束, 是指無人機(jī)的航跡長度小于或等于指定的閾值。

記q (x, y, z, θ, ψ) 為無人機(jī)的飛行位置與姿態(tài), 其中, (x, y, z) 為無人機(jī)的位置, θ為無人機(jī)的水平轉(zhuǎn)彎角, ψ為無人機(jī)的豎直爬升角或下滑角, 進(jìn)而建立上述基本約束的數(shù)學(xué)表達(dá)式。

1.2 飛行環(huán)境障礙物和威脅區(qū)建模
在飛行環(huán)境中, 高聳的山體近似采用圓錐體等效表示, 用以e為底的自然指數(shù)圖形生成, 那么, 山體地形可以通過多個位置不同的圓錐體疊加而成。若將參考海拔基準(zhǔn)高度設(shè)置為xOy平面, 記 (x, y, z) 為山體地形中的點, 那么

式中:N為山體個數(shù);xk0和yk0為第k座山體中心對稱軸的橫坐標(biāo)和縱坐標(biāo);hk為第k座山體的最大高度;xki和yki為第k座山體的橫向斜度和縱向斜度。

在飛行環(huán)境中, 山體附近通常存在雷暴等極端氣象, 本文視為飛行威脅區(qū), 并通過球體近似等效表示, 且記第k座山體附近飛行威脅區(qū)的球心坐標(biāo)為 (xks0, yks0, zks0) , 半徑為rk。

1.3 目標(biāo)函數(shù)及航跡表示
在本文中, 執(zhí)行任務(wù)的某型無人機(jī), 其航跡規(guī)劃的目標(biāo)函數(shù)是生成一條由起始點到目標(biāo)點的無碰撞可行航跡。采用q (x, y, z, θ, ψ) 表示無人機(jī)在飛行空域中某特定位置的特定姿態(tài), 那么 (x, y, z) 則表示無人機(jī)所在航跡點, θ表示無人機(jī)的水平轉(zhuǎn)彎角, ψ表示無人機(jī)的豎直爬升角或下滑角。采用r (q) 表示由起始點qinitial到目標(biāo)點qgoal的無碰撞可行航跡, 那么航跡規(guī)劃的過程可以寫成如下形式:

2 螢火蟲算法
螢火蟲算法（Firefly Algorithm，F(xiàn)A）是由Yang于2010年提出的一種群智能優(yōu)化算法，在自然界中，螢火蟲之間通過自身發(fā)光來吸引異性前來交配以及吸引獵物進(jìn)行捕獵，而該算法主要仿照自然界中螢火蟲之間受彼此亮度而相互吸引的行為來進(jìn)行目標(biāo)范圍內(nèi)的尋優(yōu)過程。
在螢火蟲算法的基本思想中，主要涉及到的兩個概念有兩個，分別為吸引力與個體間的移動，下面將分別從這兩個方面進(jìn)行介紹。

2.1 吸引力
在螢火蟲算法中，每個螢火蟲的位置代表了一個待求問題的可行解，而螢火蟲的亮度表示該螢火蟲位置的適應(yīng)度值，亮度越高的螢火蟲個體在解空間內(nèi)的位置越好，在解空間內(nèi)，每個螢火蟲會向著亮度比自己高的螢火蟲飛行來搜尋更優(yōu)的位置，亮度越大對其他的螢火蟲的吸引度越大。同時，螢火蟲之間光的傳播介質(zhì)會吸收光，降低光的亮度，影響光的傳播，所以螢火蟲之間的吸引度會隨著空間距離成反比，即兩只螢火蟲之間的吸引度會隨著這兩只螢火蟲之間距離的增大而減小?；诖?，使用相互吸引度公式對螢火蟲之間的吸引力進(jìn)行計算。

2.2 個體間的位置移動
受到吸引力的影響，種群中的每一只螢火蟲均會被亮度比其大的螢火蟲所吸引，進(jìn)而向其所在的方向進(jìn)行移動，具體的位置更新公式如下，而依次向所有亮度更大的個體移動完之后的位置才是螢火蟲的最終確定位置。

2.3 算法步驟
使用螢火蟲算法對優(yōu)化問題進(jìn)行求解時的具體步驟可以歸納如下：
（1）設(shè)置種群的個體數(shù)量、初始吸引度值β0、傳播介質(zhì)對光的吸收系數(shù)γ以及步長的擾動因子α；
（2）以螢火蟲個體的位置信息作為待優(yōu)化問題的解，以螢火蟲個體的亮度作為解對應(yīng)的適應(yīng)度值，根據(jù)待優(yōu)化問題的解的范圍，隨機(jī)初始化種群所有個體的位置信息；
（3）根據(jù)待求解問題，計算種群中每個個體的適應(yīng)度值，之后對種群個體的適應(yīng)度值進(jìn)行比較，將最高適應(yīng)度值作為種群的歷史最優(yōu)適應(yīng)度值，將其對應(yīng)的位置信息作為種群的歷史最優(yōu)位置信息；
（4）依次將每個個體與其它個體進(jìn)行適應(yīng)度值的比較，同時計算個體之間的空間距離，按照適應(yīng)度值低的個體被適應(yīng)度高的個體吸引的原則，確定每個個體分別被種群內(nèi)哪些個體吸引并根據(jù)空間距離計算出每個個體受到的所有吸引力；
（5）每個個體依次向所有適應(yīng)度值比它高的個體位置方向進(jìn)行移動，按照相關(guān)公式進(jìn)行位置更新；
（6）計算每個個體更新后的適應(yīng)度值，對種群的歷史最優(yōu)適應(yīng)度值、歷史最優(yōu)位置信息進(jìn)行更新；
（7）根據(jù)預(yù)設(shè)的迭代次數(shù)重復(fù)步驟4到步驟6，當(dāng)達(dá)到最大迭代次數(shù)時停止迭代過程，輸出種群的歷史最優(yōu)位置信息，此位置信息即為算法優(yōu)化后獲得的問題最優(yōu)解。

?二、部分源代碼

%% 三維地圖-無人機(jī)尋路
% 3D map - aircraft pathfinding
%% 這是使用原始算法的直接求解結(jié)果，添加專用于本問題的更新方式可以進(jìn)一步提高精度
% This is the direct result of using the original algorithm,
% adding some specific update methods to this problem can further improve the accuracy
clc;
clear;
close all;
warning off

%% 載入數(shù)據(jù)
data.S=[1,950,12]; %起點位置
data.E=[950,1,1]; %終點點位置
data.Obstacle=xlsread(‘data.xls’);
data.numObstacles=length(data.Obstacle(:,1));
data.mapSize=[1000,1000,20]; %10m 地圖尺寸
data.unit=[50,50,1]; %地圖精度
data.S0=ceil(data.S./data.unit);
data.E0=ceil(data.E./data.unit);
data.mapSize0=data.mapSize./data.unit;
data.map=zeros(data.mapSize0);
figure
plot3(data.S(:,1),data.S(:,2),data.S(:,3),‘o’,‘LineWidth’,1,…
‘MarkerEdgeColor’,‘g’,…
‘MarkerFaceColor’,‘g’,…
‘MarkerSize’,8)
hold on
plot3(data.E(:,1),data.E(:,2),data.E(:,3),‘h’,‘LineWidth’,1,…
‘MarkerEdgeColor’,‘g’,…
‘MarkerFaceColor’,‘g’,…
‘MarkerSize’,8)
for i=1:data.numObstacles
x=1+data.Obstacle(i,1);
y=1+data.Obstacle(i,2);
z=1+data.Obstacle(i,3);
long=data.Obstacle(i,4);
wide=data.Obstacle(i,5);
pretty=data.Obstacle(i,6);
[V,F] = DrawCuboid(long, wide, pretty, x,y,z);
x0=ceil(x/data.unit(1));
y0=ceil(y/data.unit(2));
z0=ceil(z/data.unit(3));
long0=ceil(long/data.unit(1));
wide0=ceil(wide/data.unit(2));
pretty0=ceil(pretty/data.unit(3));
data.map(x0:x0+long0,y0:y0+wide0,z0:z0+pretty0)=1;
end
legend(‘起點’,‘終點’)
title(‘三維地形地圖’)
grid on
axis equal
%%
% index=find(data.map==1);
% [p1,p2,p3] = ind2sub(size(data.map), index);
% figure
% plot3(data.S0(:,1),data.S0(:,2),data.S0(:,3),‘o’,‘LineWidth’,1,…
% ‘MarkerEdgeColor’,‘g’,…
% ‘MarkerFaceColor’,‘g’,…
% ‘MarkerSize’,8)
% hold on
% plot3(data.E0(:,1),data.E0(:,2),data.E0(:,3),‘h’,‘LineWidth’,1,…
% ‘MarkerEdgeColor’,‘g’,…
% ‘MarkerFaceColor’,‘g’,…
% ‘MarkerSize’,8)
% plot3(p1,p2,p3,‘.’,‘LineWidth’,1,…
% ‘MarkerEdgeColor’,‘k’,…
% ‘MarkerFaceColor’,‘g’,…
% ‘MarkerSize’,10)
% legend(‘起點’,‘終點’)
% title(‘三維地形地圖’)
% grid on
% axis equal
% xlabel(‘x（km）’)
% ylabel(‘y（km）’)
% zlabel(‘z（km）’)
%% 生成可移動方向
temp=[1,0,-1];
direction=[];
for i=1:3
for j=1:3
for k=1:3
direction=[direction;temp(i),temp(j),temp(k)];
end
end
end
position=find(direction(:,1)==0 & direction(:,2)==0 & direction(:,3)==0);
direction(position,:)=[];
data.direction=direction;

%% 算法參數(shù)設(shè)置 Parameters
% 基本參數(shù)
numAgent=20; %種群個體數(shù) size of population，可自行修改
Max_iter=20; %最大迭代次數(shù) maximum number of interation，可自行修改
lb=0;%下限，可自行修改
ub=1;%上限，可自行修改
dim=prod(data.mapSize0); % 優(yōu)化變量個數(shù)
fobj=@(x) aimFcn(x,data);%目標(biāo)函數(shù)，用以優(yōu)化

%% 使用優(yōu)化算法求解

Optimal_results{2,index}=recording;%迭代曲線
Optimal_results{3,index}=bestY;%最佳函數(shù)值
Optimal_results{4,index}=bestX; %最佳變量值
Optimal_results{5,index}=result; %優(yōu)化結(jié)果
Optimal_results{6,index}=toc; %運(yùn)行時間
index = index +1;

?三、運(yùn)行結(jié)果

?四、matlab版本及參考文獻(xiàn)

1 matlab版本
2014a

2 參考文獻(xiàn)
[1]田疆,李二超.用于無人機(jī)三維航跡規(guī)劃改進(jìn)連接型快速擴(kuò)展隨機(jī)樹算法[J].航空工程進(jìn)展. 2018,9(04)

3 備注
簡介此部分摘自互聯(lián)網(wǎng)，僅供參考，若侵權(quán)，聯(lián)系刪除文章來源地址http://www.zghlxwxcb.cn/news/detail-764278.html

到了這里，關(guān)于【FA三維路徑規(guī)劃】螢火蟲算法無人機(jī)避障三維航跡規(guī)劃【含Matlab源碼 3661期】的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！