Matlab航迹规划仿真——A*算法

ゝ一世哀愁。 2022-03-18 03:01 274阅读 0赞

### 文章目录 ###

*  1. 初始化参数
 *  2. 构建地图
 *  3. A\*算法搜索路径
 *  4. 路径优化
 *  5. 效果图
 *  6. 下载链接

--------------------

基本的A\*算法在这里不再讲述，想要了解的朋友可以自己在CSDN搜索，在此主要解释下代码。

# 1. 初始化参数 #

主要参数:

*  地图大小
 *  起始点和目标点坐标

clc
    clear all
    m = 30;n = 30;
    Spoint = [3 3];	  %起始点坐标
    Epoint = [29 22];	%目标点坐标

# 2. 构建地图 #

`-inf`表示不可到达的障碍物点

%%构建地图
    for i = 1:m+2
        if i == 1
            for j = 1:n+2
                Matrix(i,j) = -inf;
            end
        elseif i == m+2
            for j = 1:n+2
                Matrix(i,j) = -inf;
            end
        else
            for j = 1:n+2
                if ((j == 1)|(j == n+2))
                    Matrix(i,j) = -inf;
                else
                    Matrix(i,j) = inf;
                end
            end
        end
    end
    %%障碍
    for j=2:10
        Matrix(5,j)=-inf;
    for j=2:15
         Matrix(24,j)=-inf;
    for j=9:24
    %for j=6:24
         Matrix(10,j)=-inf;
    for j=20:31
        Matrix(15,j)=-inf;
    for j=5:20
        Matrix(20,j)=-inf;
    for j=18:27
        Matrix(28,j)=-inf;
    for i=2:6  
        Matrix(i,18)=-inf;
    for i=17:20
          Matrix(i,5)=-inf;
    for i=23:25 
         Matrix(i,20)=-inf;
    for i=13:17
        Matrix(i,13)=-inf;
    end
    end
    end
    end
    end
    end
    end
    end
    end
    end
    %end
    
    % 显示地图
    %subplot(2,2,1);
    h1 = plot(Spoint(1),Spoint(2),'gO');
    hold on
    h2 = plot(Epoint(1),Epoint(2),'rO');

# 3. A\*算法搜索路径 #

%%寻路
    Matrix(Spoint(1),Spoint(2))=0;
    Matrix(Epoint(1),Epoint(2))=inf;
    G=Matrix;
    F=Matrix;
    openlist=Matrix;
    closelist=Matrix;
    parentx=Matrix;
    parenty=Matrix;
    openlist(Spoint(1),Spoint(2)) =0;
    %closelist(Epoint(1),Epoint(2))=inf;
    
    for i = 1:n+2
        for j = 1:m+2
            k = Matrix(i,j);
            if(k == -inf)
                %subplot(2,2,1);
                h3 = plot(i,j,'k.');
    %         elseif(k == inf)  % show green feasible point
    %             %subplot(2,2,1);
    %             plot(i,j,'gh');
    %         else
    %             %subplot(2,2,1);
    %             plot(i,j,'gh');
            end
            hold on
        end
    end
    axis([0 m+3 0 n+3]);
    %subplot(2,2,1);
    plot(Epoint(1),Epoint(2),'b+');
    %subplot(2,2,1);
    plot(Spoint(1),Spoint(2),'b+');
    while(1)
        num=inf;
        for p=1:m+2
            for q=1:n+2
                if(openlist(p,q)==0&&closelist(p,q)~=1)
                    Outpoint=[p,q];
                    if(F(p,q)>=0&&num>F(p,q))
                        num=F(p,q);
                        Nextpoint=[p,q];
                    end
                end
            end
        end
        closelist(Nextpoint(1),Nextpoint(2))=1;
        for i = 1:3
            for j = 1:3
                k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
                if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1)
                    continue;
                elseif (k == -inf)
                    G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
                    closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1;
                elseif (k == inf)
                    distance=((i-2)^2+(j-2)^2)^0.5;
                    G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance;
                    openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0;
                   % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数
                    H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
                    H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
                    H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal);
    
    
                    % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
                    
                    F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;
                    parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
                    parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
                else distance=((i-2)^2+(j-2)^2)^0.5;
                    if(k>(distance+G(Nextpoint(1),Nextpoint(2))))
                    k=distance+G(Nextpoint(1),Nextpoint(2));
                   % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;  %欧几里德距离启发函数
                    H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
                    H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
                    H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal);
    
    
                     % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
                    
                    F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H;
                    parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
                    parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
                    end
                end
                if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
                     parentx(Epoint(1),Epoint(2))=Nextpoint(1);
                    parenty(Epoint(1),Epoint(2))=Nextpoint(2);
                    break;
                end
            end
            if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
                 parentx(Epoint(1),Epoint(2))=Nextpoint(1);
                    parenty(Epoint(1),Epoint(2))=Nextpoint(2);
                break;
            end
        end
        if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
             parentx(Epoint(1),Epoint(2))=Nextpoint(1);
                    parenty(Epoint(1),Epoint(2))=Nextpoint(2);
            break;
        end
    end
        P=[];
        s=1;
    while(1)
        if(num==inf)
            break;
        end
        %subplot(2,2,1);
        h4 = plot(Epoint(1),Epoint(2),'b+');
    
        P(s,:)=Epoint;
        s=s+1;
    %      pause(1);
        xx=Epoint(1);
        Epoint(1)=parentx(Epoint(1),Epoint(2));
        Epoint(2)=parenty(xx,Epoint(2));
        if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2))
            %subplot(2,2,1);
            plot(Epoint(1),Epoint(2),'b+');
            P(s,:)=Epoint;
            break;
        end
    end
    P(s+1,:)=Spoint;
    legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点');
    
    count=0;
    for i=2:12
        for j=2:12
            if(G(i,j)~=inf&&G(i,j)~=-inf)
                count=count+1;
            end
        end
    end
    count

# 4. 路径优化 #

%将得到的折现曲线拟合成光滑的曲线
    P=P';
    a=[];
    b=[];
    a=P(1,:);
    b=P(2,:);
    figure
    %subplot(2,2,3);
    plot(a,b);
    axis([0,n+3,0,n+3]);
    
    values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);
    figure
    %subplot(2,2,4);
    plot(values(1,:),values(2,:),'r');
    axis([0,m+3,0,m+3]);

# 5. 效果图 #

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70][]

**A\*路径**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1][]

**优化后路径**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 2][]

# 6. 下载链接 #

直接复制到matlab即可使用，或者也可以[点击下载][Link 1]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]: /images/20220318/c34a6a5d64e74902b595220ab102daa0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]: /images/20220318/083ee2fdce7142158fa80e5d9cdcf95d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 2]: /images/20220318/571b68dfdc56431f95b1092898eac8e1.png
[Link 1]: https://download.csdn.net/download/qq_16775293/10963781