基于人工势场法的车辆编队轨迹规划matlab仿真验证

给出了完整的MATLAB代码仿真；基于人工势场法编队的基本原理：通过构建车辆相对目标点的引力势场和斥力势场构建车辆所处地图下的整体势场，设置如图所示的势场图，

 图中圆心为我们参考的目标点，其可以提供引力方向；圆圈外引力为正，圆圈内引力为负。构建势场函数如式：

势场力选着势场函数的负梯度方向。

 上图为人工势场编队结果。

clear ;
close all;
clc;

%%初始化车辆与目标点
target=[0,0]';
car1(:,1)=[10 0];
car2(:,1)=[-10 10];
car3(:,1)=[-7 5];
car4(:,1)=[7 9];
Car1_a=0;
Car2_a=0;
Car3_a=0;
Car4_a=0;
Car1_v=0;
Car2_v=0;
Car3_v=0;
Car4_v=0;

a=0.01;
v=0.2;
R=5;
xite=0.2;
alfa=0.2;

Car1=car1;
Car2=car2;
Car3=car3;
Car4=car4;
i=0;
%%构建人工势场
while(1000-i)
    i=i+1;
 %距离   
Car1_l=norm(Car1-target);
Car2_l=norm(Car2-target);
Car3_l=norm(Car3-target);
Car4_l=norm(Car4-target);
%方位
Car1_o=Car1-target;
Car2_o=Car2-target;
Car3_o=Car3-target;
Car4_o=Car4-target;
%单位化方向向量
Car1_uo=Car1_o/Car1_l;
Car2_uo=Car2_o/Car2_l;
Car3_uo=Car3_o/Car3_l;
Car4_uo=Car4_o/Car4_l;
%引力
Car1_f=-xite*(Car1_l-R);
Car2_f=-xite*(Car2_l-R);
Car3_f=-xite*(Car3_l-R);
Car4_f=-xite*(Car4_l-R);
%合力
Car1_f=-xite*(Car1_l-R)-alfa*Car1_v;
Car2_f=-xite*(Car2_l-R)-alfa*Car2_v;
Car3_f=-xite*(Car3_l-R)-alfa*Car3_v;
Car4_f=-xite*(Car4_l-R)-alfa*Car4_v;
%车辆加速度更新
if Car1_f>a
    Car1_f=a;
elseif Car1_f<-a
    Car1_f=-a;
end
if Car2_f>a
    Car2_f=a;
elseif Car2_f<-a
    Car2_f=-a;
end
if Car3_f>a
    Car3_f=a;
elseif Car3_f<-a
    Car3_f=-a;
end
if Car4_f>a
    Car4_f=a;
elseif Car4_f<-a
    Car4_f=-a;
end
Car1_v=Car1_v+Car1_f;
Car2_v=Car2_v+Car2_f;
Car3_v=Car3_v+Car3_f;
Car4_v=Car4_v+Car4_f;
if Car1_v>v
    Car1_v=v;
elseif Car1_v<-v
    Car1_v=-v;
end
if Car2_v>v
    Car2_v=v;
elseif Car2_v<-v
    Car2_v=-v;
end
if Car3_v>v
    Car3_v=v;
elseif Car3_v<-v
    Car3_v=-v;
end
if Car4_v>v
    Car4_v=v;
elseif Car4_v<-v
    Car4_v=-v;
end
%车辆位置更新
Car1=Car1+Car1_v*Car1_uo;
Car2=Car2+Car2_v*Car2_uo;
Car3=Car3+Car3_v*Car3_uo;
Car4=Car4+Car4_v*Car4_uo;
%车辆轨迹保存
car1=[car1 Car1];
car2=[car2 Car2];
car3=[car3 Car3];
car4=[car4 Car4];


end
r = 5;
xc = 0;
yc = 0;

theta = linspace(0,2*pi);
x = r*cos(theta) + xc;
y = r*sin(theta) + yc;
%
for k=length(car1)
plot(x,y);
axis equal
    hold on
plot(car1(1,k),car1(2,k),'o',car2(1,k),car2(2,k),'o',car3(1,k),car3(2,k),'o',car4(1,k),car4(2,k),'o');
pause(1);

end
%}

上述程序中，设计思路

1：设置车辆初始状态信息；位置，速度，加速度。最大加速度、最大速度信息。

2:设置人工势场法中要用到的参数信息。

3：计算车辆所处的势场图；

4：根据势场力的大小与方向作为车辆的加速度信息。

5：车辆位置的更新。

思考：

人工势场法可以证明系统的稳定；在设置车辆加速度和速度时有一个上下界线，加入速度和加速度的限制不会影响系统的稳定性；因为，由系统的控制输出是向着系统稳定点去的，由于执行器的限制效果，使得下一系统的位置并不能到希望点；但是到达的这个点相当于系统处于的初始误差位置，继续根据算法进行控制依然是可以保证系统的稳定性的。