[経路計画] 3次元経路計画問題を解決するためのA *アルゴリズム[Matlab035]

1.はじめに

Aアルゴリズム
Aアルゴリズムは広く二点間の最短経路を見つけるために、ゲームのマップと現実の世界で使用されるダイクストラアルゴリズムに基づいて、典型的なヒューリスティック検索アルゴリズム、です。Aアルゴリズムの主な目的は、式（1）に示すように、ヒューリスティックな評価関数を維持することです。
f（n）= g（n）+ h（n）（1）
この中で、f（n）は、アルゴリズムが各ノードを検索するときに対応するヒューリスティック関数です。これは2つの部分で構成され、最初の部分g（n）は開始ノードから現在のノードまでの実際の移動コストであり、2番目の部分h（n）は現在のノードから終了までの移動コストの推定値です。ポイント。アルゴリズムが拡張されるたびに、f（n）値が最小のノードが、最適パス上の次のノードとして選択されます。
実際のアプリケーションでは、最短距離を最適化の目標とすると、h（n）は、現在のポイントから終点までのユークリッド距離またはマンハッタン距離と見なされることがよくあります。h（n）= 0の場合、現在のノードとエンドポイントに関する情報が使用されておらず、Aアルゴリズムが非ヒューリスティックダイクストラアルゴリズムに縮退していることを意味します。アルゴリズムの検索スペースが大きくなり、検索時間が長くなります。
A *アルゴリズムの手順は次のとおりです。アルゴリズムは、PリストとQリストの2つのセットを維持します。Pテーブルには、検索されたが最適パスツリーに追加されていないノードが格納されます。Qテーブルには、最適パスツリーに追加されたノードが保持されます。
（1）PテーブルとQテーブルを空に設定し、開始点SをPテーブルに追加し、そのg値を0に設定し、親ノードを空にし、道路網内の他のノードのg値を無限大に設定します。。
（2）Pリストが空の場合、アルゴリズムは失敗します。それ以外の場合は、Pリストでf値が最小のノードを選択し、BTとして記録して、Qリストに追加します。BTがエンドポイントTであるかどうかを判断し、そうである場合は手順（3）に進みます。そうでない場合は、道路網のトポロジ属性と交通ルールに従ってBTの隣接ノードNTを見つけ、次の手順を実行します。

①NTのヒューリスティック値を計算する
f（NT）= g（NT）+ h（NT）（2）
g（NT）= g（BT）+ cost（BT、NT）（3）
ここで、cost（BT、NT）It BTをNTに渡すコストです。
②NTがPテーブルにあり、式（3）で計算したg値が元のNTのg値よりも小さい場合は、NTのg値を式（3）の結果に更新し、の親ノードを設定します。 BTとしてのNT。
③NTがQテーブルにあり、式（3）で計算したg値が元のNTのg値よりも小さい場合は、NTのg値を式（3）の結果に更新し、NTの親ノードをBT、およびNTをPリストに移動します。
④NTがPリストにもQリストにも含まれていない場合は、NTの親ノードをBTに設定し、NTをPリストに移動します。
⑤手順（2）に進み、実行を継続します。
（3）終点Tからバックトラックし、親ノードを順番に見つけ、始点Sまで最適化されたパスに追加すると、最適化されたパスを取得できます。

第二に、ソースコード

clc;
clear all;

[datax,datay,dataz]=loadmap('map.xyz');
startpoint=[datax(1,30),datay(30,1),dataz(30,30)+0.1];%起点
endpoint=[datax(1,390),datay(390,1),dataz(390,390)+0.1];%终点
q=endpoint-startpoint;
q=q/norm(q);
direction=q;
position=startpoint;
limit=[pi/6,pi/6];

dataz=dataz.*2.5;
% r_posi=[300,320];
% R=3;
% dataz=radar_pos(r_posi,R,datax,datay,dataz);

%  r_posi1=[100,120];R1=3;
r_posi2=[200,150];R2=4;
r_posi3=[300,320];R3=5;
r_posi4=[150,320];R4=3.5;
r_posi5=[200,80];R5=4.2;

%  dataz=radar_pos(r_posi1,R1,datax,datay,dataz);
dataz=radar_pos(r_posi2,R2,datax,datay,dataz);
dataz=radar_pos(r_posi3,R3,datax,datay,dataz);
dataz=radar_pos(r_posi4,R5,datax,datay,dataz);
dataz=radar_pos(r_posi5,R5,datax,datay,dataz);

lest=sqrt(sum((position-endpoint).^2));
h=0.5;
k=0;

while lest>h    
    k=k+1    
    route(k,:)=position;    
    [open_list,dir_list]=openlist(position,direction,h,limit);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    fate=zeros(25,3);    
    for i=1:25        
        xipt=open_list(i,1);
        yipt=open_list(i,2);
        [zopt,xind,yind]= mappoint(xipt,yipt,datax,datay,dataz);
        if(zopt>open_list(i,3))
            fate(i,:)=[1,1,1];
            open_list(i,:)=[0,0,0];
        end        
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    G=k*h;  
    
    H=H_func(open_list,endpoint);    
    [mini,ind]=min(H);    
    lest=sqrt(sum((position-endpoint).^2));   
    
    F=G+mini;
        position=open_list(ind,:);
        direction=dir_list(ind,:);      

end

x=route(:,1);
y=route(:,2);
z=route(:,3)+21;
figure(1)
mesh(datax,datay,dataz+20);
hold on;
% view(0,10)
plot3(x,y,z,'r--*');
axis equal;

3.備考