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

下圖內容為求導過程?

為了解決局部最優化和目標不可達，每個障礙物的斥力分為兩個：一種是沿障礙物與車輛的連線指向車輛，另一個是?沿目標與車輛連線 指向目標。

?

?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)>1if i>max_iterbreak;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,:)>=border0Fre(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];endFre=[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

?通過效果圖看出來，路徑規劃還存在很多問題需要優化。

總結

以上是生活随笔為你收集整理的人工势场法--路径规划--原理--matlab代码的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。