【路径规划】基于人工势场法实现车辆路径规划附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：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])

⛄ 运行结果

⛄ 参考文献

[1] 杨一波, 王朝立. 基于改进的人工势场法的机器人避障控制及其MATLAB实现[J]. 上海理工大学学报, 2013, 35(5):5.

[2] 王树凤, 张钧鑫, 刘宗锋. 基于改进人工势场法的智能车辆超车路径规划研究[J]. 汽车技术, 2018(3):5.

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料