合力和斥力的计算方法符合高中物理矢量相加法则
为了解决局部最优化和目标不可达,每个障碍物的斥力分为两个:一种是沿障碍物与车辆的连线指向车辆,另一个是 沿目标与车辆连线 指向目标。
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
通过效果图看出来,路径规划还存在很多问题需要优化。