工具：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)