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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128

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
%}