机器人导航，人工势场法的地图导航规划

@[Matlab]人工势场法的导航规划示例

20*20二维地图，
随机给出障碍点，
能否到达目标点有提示，
除了地图底子以外其他参数都在第一个模块可以自己改，
代码有注释，斥力公式是改进人工势场的
作业来着,肝了挺久的吧，纯手打。

`主程序：main.m

%main.m
clf
clear;

%设置参数模块
x0(1)=0;    %起始点坐标
y0(1)=0;    
xg=20;      %目标点坐标
yg=20;      
Kat=10;     %引力增益		%引力增益不宜过大
Kre=1000;   %斥力增益
Q=1;        %安全距离		%安全距离Q不得小于步长h
h=0.01;     %前进步长		%步长可调，越小效果越好，时间会增加；
Hmax=10000; %步长容限		%步长过小则还需增大步长容限；		
Nobc=20;    %障碍点数量（这道题设为20）


%绘制地图模块
%障碍点位置使用随机数生成，所以可能有导航无解情况，不要慌
xob=[];
yob=[];     
for i=1:Nobc
    xob(i)=randperm(20,1);
    yob(i)=randperm(20,1);
    %不让障碍点重合于出发点和目标点
    while ((xob(i)==x0(1))&&(yob(i)==y0(1))||((xob(i)==xg)&&(yob(i)==yg)))      
        xob(i)=randperm(20,1);
        yob(i)=randperm(20,1);
    end
end
scatter(xob,yob,'k');hold on;
xlim([0,20]),ylim([0,20]);
xticks(0:1:20);
yticks(0:1:20);
grid;hold on;
plot(xg,yg,'*g');hold on;
xlabel('地图横坐标');
ylabel('地图纵坐标');
title('人工势场法20*20地图导航轨迹图');

%正式的步进搜索模块：
x0=[];
y0=[];
Frex=[;];
Frey=[;];
Fatx=[]';
Faty=[];
Fsumx=[];
Fsumy=[];
Cos=[];
Sin=[];
%步长搜索循环：
for j=1:Hmax
    x0(1)=0;
    y0(1)=0;
    Fsumx(j)=0;
    Fsumy(j)=0;
%若已到达目标点或者超界或者步进尺度用完则终止搜索
    if(abs(x0(j)-xg)<10^-1)&&(abs(y0(j)-yg)<10^-1) 
        fprintf('到达目的地，停止搜索\n');
        %成功到达目的点的话显示障碍物信息
        fprintf('障碍点:\n');                                                      
        for kdispx=1:20
            fprintf('%d\t',xob(kdispx));
        end
        fprintf('\n');
        for kdispy=1:20
            fprintf('%d\t',yob(kdispy));
        end
        fprintf('\nj=%d\n',j);
        break
    end
    if((x0(j)<0)||(x0(j)>20)||(y0(j)<0)||(y0(j)>20))                               
        %轨迹点超界条件，超界了就终止搜索
        fprintf('行车点超界，终止搜索\n');
        fprintf('j=%d\n',j);
        break
    end
    if(j==Hmax)
        fprintf('步进尺度已用完,运气不好再走一次\n');
        fprintf('x0(i)=%d,y0(j)=%d,j=%d\n',x0(j),y0(j),j);
    end  
%调用计算斥力模块
    for k=1:Nobc
        [Frex(j,k),Frey(j,k)]=compute_Repulso(x0(j),y0(j),xob(k),yob(k),Kre,Q);
    end
    Fsumx(j)=sum(Frex(j,1:Nobc));
    Fsumy(j)=sum(Frey(j,1:Nobc));
%调用计算引力模块
    [Fatx(j),Faty(j)]=compute_Attract(x0(j),y0(j),xg,yg,Kat);
    Fsumx(j)=Fsumx(j)+Fatx(j);
    Fsumy(j)=Fsumy(j)+Faty(j);    
%调用计算角度模块
    %求每一次所在点在步长半径内受到合力正交值指示角度
    [Cos(j),Sin(j)]=compute_Angle(Fsumx(j),Fsumy(j));                               
%作用力步进，计算下一个定位点位置
    %根据正交后x,y各方向要走的距离得到下一个点位置
    x0(j+1)=x0(j)+h*Cos(j);
    y0(j+1)=y0(j)+h*Sin(j);
%撞上障碍点警告
    for z=1:20
        if( abs(x0(j+1)-xob(z))<10^-1)&&(abs(x0(j+1)-xob(z))<10^-1)  
            fprintf('障碍点重合,j=%d,k=%d\n',j,k);
        end
    end    
end
plot(x0,y0,'r');

根据障碍物位置与当前位置，计算引力函数：compute_Attract.m:

%compute_Attract.m
%Kat:引力增益，(x0,y0):所在位置
function [fatx,faty]=compute_Attract(x,y,xg,yg,Kat)   
dgx=x-xg;
dgy=y-yg;
%Uat=1/2*K*dg2;Fat=Uat的各方向偏导
fatx=-Kat*dgx;
faty=-Kat*dgy;
end

根据障碍物位置与当前位置，计算斥力函数：compute_Repulso.m:

%compute_Repulso.m
%Kre:斥力增益，(x0,y0):所在位置
function [frex,frey]=compute_Repulso(x,y,xr,yr,Kre,Q)
drx=x-xr;
dry=y-yr;
dr2=drx^2+dry^2;
dr=sqrt(dr2);
if(dr>Q)
    frex=0;
    frey=0;
else
    %dr<=Q
    %原版人工势场法斥力公式：Ure=1/2*Kre*(1/dr-1/Q)^2;
    %之后发现求偏导用改进的人工势场法出来的斥力均衡正常一些
    frex=Kre*(1/dr-1/Q)^2*drx;
    frey=Kre*(1/dr-1/Q)^2*dry; 
end

计算下一步前进方向的角度函数：%compute_Angle.m

%compute_Angle.m
function [cos,sin]=compute_Angle(a,b)
c=sqrt(a^2+b^2);
cos=a/c;
sin=b/c;        %最基本的直角三角形求角度
end

附上程序打包：

https://download.csdn.net/download/LXT13592800607/86399866

载入Matlab添加至路径—>点击<运行>即可观察生成坐标地图上导航规划结果，命令行输出生成的障碍点横、纵坐标，可能存在有的随机地图生成使得导航陷入局部最优解而未跑至终点的情况，效果很直观。