?作者簡介：熱愛科研的Matlab仿真開發者，修心和技術同步精進，matlab項目合作可私信。

🍎個人主頁：Matlab科研工作室

🍊個人信條：格物致知。

更多Matlab仿真內容點擊👇

智能優化算法 ?神經網絡預測 雷達通信? 無線傳感器

信號處理 圖像處理 路徑規劃 元胞自動機 無人機

? 內容介紹

飛行器的測量精度，航跡路徑的合理規劃，飛行器工作時的穩定性、安全性等這些變化對飛行器的綜合控制系統要求越來越高。無人機航路規劃是為了保證無人機完成特定的飛行任務，并且能夠在完成任務的過程中躲避各種障礙、威脅區域而設計出最優航跡路線的問題。常見的航跡規劃算法如圖1所示。

圖1 常見路徑規劃算法?

文中主要對無人機巡航階段的航跡規劃進行研究，假設無人機在飛行中維持高度與速度不變，那么航跡規劃成為一個二維平面的規劃問題。在航跡規劃算法中,A*算法計算簡單，容易實現。在改進A*算法基礎上，提出一種新的、易于理解的改進A*算法的無人機航跡規劃方法。傳統A*算法將規劃區域柵格化，節點擴展只限于柵格線的交叉點，在柵格線的交叉點與交叉點之間往往存在一定角度的兩個運動方向。將存在角度的兩段路徑無限放大、細化，然后分別用兩段上的相應路徑規劃點作為切點，找到相對應的組成內切圓的圓心，然后作弧，并求出相對應的兩切點之間的弧所對應的圓心角，根據下式計算出弧線的長度

式中：R———內切圓的半徑；

α———切點之間弧線對應的圓心角。

**1 A*算法概述**

A*算法是在Dijstar算法的基礎上引入的啟發式函數，通過定義的代價函數來評估代價大小，從而確定最優路徑。A*算法的代價函數

式中：f(x,y）———初始狀態X0(x0,y0）到達目標狀態X1(x1,y1）的代價估計；

g(x,y）———狀態空間中從初始狀態X0(x0,y0）到狀態N(x1,y1）的實際代價；

h(x,y）———從狀態N(x1,y1）到目標狀態X1(x1,y1）最佳路徑的估計代價。

要找到最短路徑的實質是找到f(x,y）的最小值，其中在式（2）中尋找最短路徑的關鍵在于求估計代價h (x,y）值。設系數λ表示狀態N(x1,y1）到X1(x1,y1）最優距離，如果λ<h(x,y），搜索范圍小，不能保證得到最優解；λ>h(x,y），搜索范圍大，費時，但能找到最優解；λ=h(x,y），搜索到最短路徑。其中h(x,y）一般用歐幾里德距離（式（3））或者絕對值距離（式（4））計算。

A*算法是以起始點為中心，周圍8個柵格的中心為下一步預選，并不斷地計算預選位置的f(x,y）值，其中f(x,y）值最小的作為當前位置，依次逐層比較，直到當前位置的臨近點出現目標點為止，其最小單元如圖2所示。

圖2 最小單元

A*算法的流程如下：

1）創建開始節點START，目標節點TARGET、OPEN列表、CLOSE列表、CLOSE列表初始為空；

2）將START加入到OPEN列表；

3）檢查OPEN列表中的節點，若列表為空，則無可行路徑；若不為空，選擇使f(x,y）值最小的節點k;

4）將節點k從OPEN中去除，并將其添加到CLOSE中，判斷節點k是否目標節點TARGET，若是，則說明找到路徑；若不是，則繼續擴展節點k，生成k節點的子節點集，設q為k的子節點集，對所有節點q計算相應的f(x,y）值，并選擇f(x,y）值最小的節點，將該節點放入CLOSE列表中；

5）跳到3），直到算法獲得可行路徑或無解退出。

? 部分代碼

function? MakeData()

%%%%%%%%Make Data of Map%%%%%%%%

load ('TerrainData.mat');

%%%%%%Define The 2-D Map Array%%%%%

MAX_X = 100;

MAX_Y = 100;

MAX_Z = 50;

Cut_Data = Final_Data(301:400,101:200);

mesh(double(Cut_Data));

MAX_Final_Data = max(max(Cut_Data));

MIN_Final_Data = min(min(Cut_Data));

for i=1:100

? ? for j=1:100

? ? ? ? New_Data(i,j) = ceil((Cut_Data(i,j)-MIN_Final_Data)/100);

? ? ? ? Display_Data(i,j) = (Cut_Data(i,j)-MIN_Final_Data)/100;

? ? end

end

%%%%%%Map Matrix Initialization%%%%%%

MAP=2*(ones(MAX_X,MAX_Y,MAX_Z));

% Obtain Obstacle, Target and Robot Position

% Initialize the MAP with input values

% Obstacle=-1,Target = 0,Robot=1,Space=2

%%%%%%%Make Random Terrain Data%%%%%%%

for i=1:MAX_X

? ? for j=1:MAX_Y

? ? ? ? Z_UpData = New_Data(i,j);

? ? ? ? for z = 1:Z_UpData

? ? ? ? ? ? MAP(i,j,z) = -1;

? ? ? ? end

? ? end

end

CLOSED=[];

%Put all obstacles on the Closed list

k=1;%Dummy counter

for i=1:MAX_X

? ? for j=1:MAX_Y

? ? ? ? Z_UpData = New_Data(i,j);

? ? ? ? for z = 1:Z_UpData

? ? ? ? ? ? CLOSED(k,1)=i;?

? ? ? ? ? ? CLOSED(k,2)=j;

? ? ? ? ? ? CLOSED(k,3)=z;

? ? ? ? ? ? k=k+1;

? ? ? ? end? ? ? ??

? ? end

end

%%%%%%%%%輸入禁飛區信息

c2=size(CLOSED,1);

for i_z=1:20

? ? for i_x=1:100

? ? ? ? for i_y=1:100

? ? ? ? ? ? flag = 1;

? ? ? ? ? ? Length = (i_x-30)^2 + (i_y-30)^2;? ? ? ? ? ??

? ? ? ? ? ? for c1=1:c2

? ? ? ? ? ? ? ? if (i_x == CLOSED(c1,1) && i_y == CLOSED(c1,2) && i_z == CLOSED(c1,3))

? ? ? ? ? ? ? ? ? ? flag = 0;

? ? ? ? ? ? ? ? end

? ? ? ? ? ? end

? ? ? ? ? ? if Length <= 25 & flag == 1

? ? ? ? ? ? ? ? CLOSED(c2+1,1)=i_x;

? ? ? ? ? ? ? ? CLOSED(c2+1,2)=i_y;

? ? ? ? ? ? ? ? CLOSED(c2+1,3)=i_z;

? ? ? ? ? ? ? ? c2 = c2+1;

? ? ? ? ? ? end

? ? ? ? end

? ? end

end

%%%%%%%%%輸入異常氣象區域信息

% k = 1;

% c3 = size(CLOSED,1);

% for i_z=1:10

%? ? ?for i_x=1:100

%? ? ? ? ?for i_y=1:100

%? ? ? ? ? ? ?flag = 1;

%? ? ? ? ? ? ?Length = (i_x-60)^2 + (i_y-30)^2;? ? ? ? ? ??

%? ? ? ? ? ? ?for c1=1:c3

%? ? ? ? ? ? ? ? ?if (i_x == CLOSED(c1,1) && i_y == CLOSED(c1,2) && i_z == CLOSED(c1,3))

%? ? ? ? ? ? ? ? ? ? ?flag = 0;

%? ? ? ? ? ? ? ? ?end

%? ? ? ? ? ? ?end

%? ? ? ? ? ? ?if Length <= 56.25 & flag == 1

%? ? ? ? ? ? ? ? ?Threaten_Weather(k,1)=i_x;

%? ? ? ? ? ? ? ? ?Threaten_Weather(k,2)=i_y;

%? ? ? ? ? ? ? ? ?Threaten_Weather(k,3)=i_z;

%? ? ? ? ? ? ? ? ?k = k+1;

%? ? ? ? ? ? ?end

%? ? ? ? ?end

%? ? ?end

% end

save MapData MAX_X MAX_Y MAX_Z MAP CLOSED Final_Data Display_Data

? 運行結果

? 參考文獻

?[1]趙德群, 段建英, 陳鵬宇,等. 基于A*算法的三維地圖最優路徑規劃[J]. 計算機系統應用, 2017, 26(7):7.

?? 關注我領取海量matlab電子書和數學建模資料

??部分理論引用網絡文獻，若有侵權聯系博主刪除

總結

以上是生活随笔為你收集整理的基于Astar算法实现飞行轨迹的三维规划附Matlab代码的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。