人工势场法--路径规划--原理--matlab代码

合力和斥力的计算方法符合高中物理矢量相加法则

下图内容为求导过程 

为了解决局部最优化和目标不可达，每个障碍物的斥力分为两个：一种是沿障碍物与车辆的连线指向车辆，另一个是 沿目标与车辆连线 指向目标。

 

 matlab代码如下：

clc;
clear;
close all;
%% 基本信息、常数等设置
eta_ob=25;          %计算障碍物斥力的权益系数
eta_goal=10;        %计算障目标引力的权益系数
eta_border=25;      %车道边界斥力权益系数
n=1;                 %计算障碍物斥力的常数
border0=20;          %斥力作用边界
max_iter=1000;        %最大迭代次数 
step=0.3;            %步长

car_width=1.8;         %车宽
car_length=3.5;      %车长
road_width=3.6;      %道路宽
road_length=100;      %道路长

%% 起点、障碍物、目标点的坐标、速度信息

P0=[3 1.3 1 1];               %横坐标 纵坐标 x方向分速度 y方向分速度
Pg=[road_length-4 5.4 0 0];
Pob=[15 1.8;
     30 5.4;
     46 1.6;
     65 5.0;
     84 2.7;]

 %% 未达目标附近前不断循环
 Pi=P0;
 i=1;
 while sqrt((Pi(1)-Pg(1))^2+(Pi(2)-Pg(2))^2)>1
     if i>max_iter
         break;
     end
     %计算每个障碍物与当前车辆位置的向量(斥力)、距离、单位向量
     for j=1:size(Pob,1)
         vector(j,:)=Pi(1,1:2)-Pob(j,1:2);
         dist(j,:)=norm(vector(j,:));
         unit_vector(j,:)=[vector(j,1)/dist(j,:) vector(j,2)/dist(j,:)];
     end

     %计算目标与当前车辆位置的向量(引力)、距离、单位向量
     max=j+1;
     vector(max,:)=Pg(1,1:2)-Pi(1,1:2);
     dist(max,:)=norm(vector(max,:));
     unit_vector(max,:)=[vector(max,1)/dist(max,:) vector(max,2)/dist(max,:)];
     
     %计算每个障碍物的斥力
     for j=1:size(Pob,1)
        if dist(j,:)>=border0
            Fre(j,:)=[0,0];
        else
            %障碍物斥力指向物体
            Fre_abs_ob=eta_ob*(1/dist(j,:)-1/border0)*(dist(max)^n/dist(j,:)^2);
            Fre_ob=[Fre_abs_ob*unit_vector(j,1) Fre_abs_ob*unit_vector(j,2)];
            %障碍物斥力 指向目标
            Fre_abs_g=n/2*eta_ob*(1/dist(j,:)-1/border0)^2*dist(max)^(n-1);
            Fre_g=[Fre_abs_g*unit_vector(max,1) Fre_abs_g*unit_vector(max,2)];
            Fre(j,:)=Fre_ob+Fre_g;
        end 
     end  

     if Pi(2)>=(road_width-car_width)/2 && Pi(2)< road_width/2   %下绿色下区域
         Fre_edge=[0,eta_border*norm(Pi(1,3:4))*(exp(road_width/2)-Pi(2))];
     elseif Pi(2)>= road_width/2 &&  Pi(2)<= (road_width+car_width)/2   %下绿色上区域
         Fre_edge=[0,-1/3*eta_border*Pi(2)^2];
     elseif Pi(2)>=(3*road_width-car_width)/2 && Pi(2)<= 3*road_width/2   %上绿色下区域
         Fre_edge=[0,1/3*eta_border*(3*road_width/2-Pi(2))^2];
     elseif Pi(2)>=3*road_width/2 && Pi(2)<= (3*road_width+car_width)/2    %上绿色上区域
         Fre_edge=[0,eta_border*norm(Pi(1,3:4))*(exp(Pi(2)-3*road_width/2))];
     else 
         Fre_edge=[0 0];
     end
     
     Fre=[sum(Fre(:,1))+Fre_edge(1) sum(Fre(:,2))+Fre_edge(2)]; 
     %计算引力
     Fat=[eta_goal*dist(max,1)*unit_vector(max,1) eta_goal*dist(max,1)*unit_vector(max,2)];
     
     F_sum=[Fre(1)+Fat(1),Fre(2)+Fat(2)];
     unit_vector_sum(i,:)=F_sum/norm(F_sum);
     
     Pi(1,1:2)= Pi(1,1:2)+step*unit_vector_sum(i,:);
     
     path(i,:)= Pi(1,1:2);
     i=i+1;
 end
 %% 画图
figure(1)
%下车道下红色区域
fill([0,road_length,road_length,0],[0,0,(road_width-car_width)/2,(road_width-car_width)/2],[1,0,0]);
hold on
%下车道上红色区域，上车道下红色区域
fill([0,road_length,road_length,0],[(road_width+car_width)/2,(road_width+car_width)/2,...
     (3*road_width-car_width)/2,(3*road_width-car_width)/2],[1,0,0]);
%下车道绿色区域
fill([0,road_length,road_length,0],[(road_width-car_width)/2,(road_width-car_width)/2,...
     (road_width+car_width)/2,(road_width+car_width)/2],[0,1,0]);

%上车道绿色区域
fill([0,road_length,road_length,0],[(3*road_width-car_width)/2,(3*road_width-car_width)/2,...
     (3*road_width+car_width)/2,(3*road_width+car_width)/2],[0,1,0]);
%上车道上红色区域
fill([0,road_length,road_length,0],[ (3*road_width+car_width)/2,(3*road_width+car_width)/2,2*road_width,2*road_width],[1,0,0]);
%路面中心线、边界线
plot([0,road_length],[road_width,road_width],'w--','linewidth',2);
plot([0,road_length],[2*road_width,2*road_width],'w','linewidth',2);
plot([0,road_length],[0,0],'w','linewidth',2);

%障碍物、目标
plot(Pob(:,1),Pob(:,2),'ko');
plot(Pg(1),Pg(2),'mp');
%车
fill([P0(1)-car_length/2,P0(1)+car_length/2,P0(1)+car_length/2,P0(1)-car_length/2],...
     [P0(2)-car_width/2,P0(2)-car_width/2,P0(2)+car_width/2,P0(2)+car_width/2],[0,0,1]);

 plot(path(:,1),path(:,2),'w.');
 
axis equal
set(gca,'XLim',[0 road_length])
set(gca,'YLim',[0 2*road_width])

运行代码，效果如下：

蓝色为车 ★为目标点 黑色圈为障碍物 白色点为规划的路径。

下图是更改参数后的不同效果：

 

 原理学习来自于 B站小黎的Ally

 通过效果图看出来，路径规划还存在很多问题需要优化。