collision avoidance---matlab

%%%%%%This file generate smooth polynomial connected Dstar points taking the points near the obstacles as way-points%%%%%%%%%%%%%

%%%%%Author and Copyright: Yi Guo 2003%%%%%%%%%%%%

clear all;
figure

l=0.8;rho=0.2;R=1;
t0=0;Ts=10;T=40;

Ts=[0 20 40];

path_points_dstar=[ 10 5;9    6;8    6; 7    6; 6    7; 5    7; 4    8;3    9;3   10;...
        3   11;3   12; 3   13;4   14;5   14;6   14;7   14;8   14;9   14;
    10   15;11   16;12   17;13   18;14   19;15   19;16   19;17   19; ...
        18   19;19   19;20   19;21   19;22   19;23   19;24   19;25   19;26   19;27   19;
    28   19;29   19;30   19;31   19;32   19;33   19;34   19;35   19;36   19;37   18;...
        38   18;39   18;40   18;41   18;42   18;43   18;44   18;
45   17;46   16;47   15;48   15;49   15;50   15;5015];

path_points=[path_points_dstar（1:）;path_points_dstar（8:）;path_points_dstar（12:）;...
        path_points_dstar（25:）;path_points_dstar（42:）;path_points_dstar（60:）;path_points_dstar（60:）];

size_p=size（path_points）;
length_p=size_p（1）;

thetaf=0;
phif=0;

Length=0;

%%%%%%%%%%%%%%%%%%%For loop%%%%%%%%%%%%%%%%%%%%%%%%%

for j=1:length_p-2

x0=path_points（j1）;
y0=path_points（j2）;

xf=path_points（j+11）;
yf=path_points（j+12）;

%%%%%%%%%%%%%%%%If xf~=x0
while xf~=x0
    
theta0=thetaf;
phi0=phif;

thetaf=phif;
   
z10=x0-l*cos（theta0）/2;
z1f=xf-l*cos（thetaf）/2;

z20=tan（phi0）/（l*（cos（theta0）^3））; %non-zero phi
%z20=0; %zero phi
z30=tan（theta0）;
z40=y0-l*sin（theta0）/2;

a6min=0;
a6max=0;

z1=z10;
z2=z20;
z3=z30;
z4=z40;
z4_old=z40;

% Initial states

z1（1）=z10;
z2（1）=z20; 
z3（1）=z30;
z4（1）=z40;

x（1）=x0;
y（1）=y0;
theta（1）=theta0;
phi（1）=phi0;

z1f=xf-l/2*cos（thetaf）; 
z2f=1/l*tan（phif）/（cos（thetaf））^3;
z3f=tan（thetaf）;
z4f=yf-l/2*sin（thetaf）;

C=（z1f-z10）/40;
T1=0.01;
tt=0:T1:T;

xo1（1）=23;
yo1（1）=15;

r=0.5;
a61max=0;
a61min=0;

xo2（1）=50;
yo2（1）=20;
vo2x=[-0.15 0.15]; %0.2
vo2y=[-0.1 -0.29]; %-0.3

a62max=0;
a62min=0;

k1=1;

%if j==4
%    k1=2;
%end
if j==5
    k1=2;
end

for k=1:k1    
    % the index of changing the initial conditions
    index=Ts（k）/T1+1;
        
    % the matrix B Y A in the boundary conditions
    B=[ 1 z1（index） （z1（index））^2 （z1（index））^3 （z1（index））^4 （z1（index））^5; ...
        0 1 2*z1（index） 3*（z1（index））^2 4*（z1（index））^3 5*（z1（index））^4;...
        0 0 2 6*z1（index） 12*（z1（index））^2 20*（z1（index））^3;...
        1 z1f z1f^2 z1f^3 z1f^4 z1f^5; ...
        0 1 2*z1f 3*z1f^2 4*z1f^3 5*z1f^4;...
        0 0 2 6*z1f 12*z1f^2 20*z1f^3];
    
    Y=[z4（index）; z3（index）; z2（index）; z4f; z3f; 0];
    %Y=[z4（index）; z3（index）; z2（index）; z4f; z3f; tan（phif）/（l*（cos（thetaf）^3））];

    A=[（z1（index））^6; 6*（z1（index））^5; 30*（z1（index））^4; z1f^6; 6*z1f^5; 30*z1f^4];
       
    a6（k）=0;
    
    if j==4
        vo1x=0.1; % [0 0.5 0.2]; %0.2
        vo1y=0.2; %