基于凸优化的车辆运动规划
工具：matlab、cvx（凸优化工具箱）
初始可行解
然后出现障碍物后
这是根据（时间差）每次求出的路径
最终得出的新的轨迹。
对于多个车辆（n大于等于2）时
将避障条件变为
即每辆车之间要满足这个条件：
这是两个车初始路径。
最后优化轨迹：
第一辆车从（2,5）到（8,5），第一辆车从（5.2）到（5.8），依次对应每个点。
本文优化目标只用了距离，同时可以加时间模型和角度变化等模型，加权起来求最优。约束只加了位置和速度约束，同时可以加上加速度的约束，以及地形约束等。
对于凸型障碍物则采用“大M法”规划。
出现部分冲突则是因为步长原因。
第一部分实验
T =20;
d = 2;
E = 0.001;
N = 2;
v_max = 0.8;
x_i = [2;5]; y_i = [5;2];
v_x_i = [0;0]; v_y_i = [0;0];
x_f = [8;5]; y_f = [5;8];
for n=1:N
p_final{n} = repmat([x_f(n) y_f(n)],T, 1);
end
P=[];
V=[];
for n=1:N
cvx_begin
variables p(T,2) v(T,2)
minimize( norm( p - p_final{n}, 1 ))
subject to
p(1,:) == [x_i(n) y_i(n)]
v(1,:) == [v_x_i(n) v_y_i(n)]
for k = 1:T-1
p(k+1,:) == p(k,:) + v(k,:)
abs(v(k,1)) <= v_max
abs(v(k,2)) <= v_max
end
cvx_end
P{n} = p;
V{n} = v;
end
figure (1)
axis([0 10 0 10])
grid on
hold on
for n=1:N
plot( P{1}(:,1),P{1}(:,2),'o-','Color','k')
plot( P{2}(:,1),P{2}(:,2),'o-','Color','b')
hold on
end
% th=0:pi/24:2*pi;
% X=2*cos(th)+5;
% Y=2*sin(th)+5;
% scatter(X,Y,'.k');
% hold on
figure(2)
axis([0 10 0 10])
grid on
hold on
for a = 2:N
for n = 1:a-1
I = 0;
for k = 1:T-1
if( norm( P{a}(k,:)-P{n}(k,:)) < d)
I = k ;
break
end
end
while I ~= 0
cvx_begin
variables p_new(T,2) v_new(T,2)
minimize( norm(p_new-p_final{a},1 )+ norm( p_new - P{a},1 ))
subject to
p_new(1,:) == [x_i(a) y_i(a)]
v_new(1,:) == [v_x_i(a) v_y_i(a)]
for k = 1:T-1
p_new(k+1,:) == p_new(k,:) + v_new(k,:)*(k+1-k)
abs(v_new(k,1)) <= v_max
abs(v_new(k,2)) <= v_max
end
((p_new(I,:) - P{n}(I,:))*((P{a}(I,:)- P{n}(I,:))' / (norm(P{a}(I,:)-P{n}(I,:))))) >= d
cvx_end
P{a} = p_new;
V{a} = v_new;
I=0;
for k = 1: T-1
if( norm( P{a}(k,:)-P{n}(k,:)) < d)
I = k;
break
end
end
hold on
plot( p_new(:,1),p_new(:,2),'o-', 'color', 'r')
clear p_new v_new f_new
end
end
end
% plot( p_new(:,1),p_new(:,2),'o-', 'color','r')
figure(3)
axis([0 10 0 10])
grid on
hold on
for n=1:N
plot( P{n}(:,1),P{n}(:,2),'o-','Color',[0.0 (n/N) 1.0])
hold on
end第二部分实验
clc
clear
T =50;
N = 1;
v_max = 0.2;
x_i = [10]; y_i = [5];
v_x_i = [0]; v_y_i = [0];
x_f = [5]; y_f = [7];
for n=1:N
p_final{n} = repmat([x_f(n) y_f(n)],T, 1);
end
P=[];
V=[];
ob=[7,8,3,8];
M=10000;
cvx_begin quiet
variables p(T,2) v(T,2)
variable o(T-1,4) binary
minimize( norm( p - p_final{n}, 1 ))
subject to
p(1,:) == [x_i(n) y_i(n)]
v(1,:) == [v_x_i(n) v_y_i(n)]
for k = 1:T-1
p(k+1,:) == p(k,:) + v(k,:)
abs(v(k,1)) <= v_max
abs(v(k,2)) <= v_max
sum(o(k,:))<=3
p(k+1,1)<=ob(1)+M*o(k,1)
-p(k+1,1)<=-ob(2)+M*o(k,2)
p(k+1,2)<=ob(3)+M*o(k,3)
-p(k+1,2)<=-ob(4)+M*o(k,4)
end
cvx_end
P{1} = p;
axis([4 10 2 10])
grid on
hold on
for n=1:N
plot( P{1}(:,1),P{1}(:,2),'o-','Color','k')
hold on
end
ax=7;
ay = 3;
l = 1;
w = 5;
x = [ax,ax+l,ax+l,ax,ax];
y = [ay,ay,ay+w,ay+w,ay];
line(x,y)