轨迹规划五次多项式子-对换道时间进行优化

只考虑两个目标然后对代码进行优化

%j为为目标函数
%包含两个约束条件分别是换道时间和换道加速度，通过舒适性和效率进行约束
clc
clear all
k1=0.5
k2=0.5
%j=(k1/4)*abs(ay)+(k2/6)*tf
d=3.75
t_all=10%总的时间周期
tf=0:0.01:(t_all);%从总的时间周期中取出换道时间tf
len=length(tf);
j=zeros(1,len);%每个换道时间计算的j值
for i=1:len
    t=0:0.01:tf(i);
    len2=length(t);
    ay=zeros(1,len2);
    for m=1:len2
    ay(m)=(120*d*t(m)^3)/(tf(i)^5)-(180*d*t(m)^2)/(tf(i)^4)+(60*d*t(m))/(tf(i)^3);
    end
    j(i)=(k1/4)*(max(ay))+(k2/6)*tf(i)
end
x=tf(:,101:1000)
y=j(:,101:1000)
plot(x,y)
axis([0,11,0,3])
index=find(y==min(y))
x(:,index)
xlabel('换道时间/s')
ylabel('目标函数J')
hold

#绘制加速度和时间的变化曲线

%根据换道时间求解侧向加速度
%侧向加速度公式，本文假定车道宽为3.5m
%tf为换道时间，t为换道时间内车辆侧向加速度的变化
%tf的取值为0-6，ay取值为-5-5
d=3.75
tf=4.02
t=0:0.01:(tf);
len=length(t);
ay=zeros(1,len);
for i=1:len
ay(i)=(120*d*t(i)^3)/(tf^5)-(180*d*t(i)^2)/(tf^4)+(60*d*t(i))/(tf^3);
end
plot(t,ay)
xlabel('侧向加速度')
ylabel('时间/s')
axis([0,6,-6,6])

三个目标然后进行优化

%j为为目标函数
%对约束条件中进行增加了东西，增加了不确定因素分别是前后左右车的加速度和减速度
%当前车道前车
%a_3=2*(v-v_3-s_3/(0.5*t)+6.15688/(0.5*t))/(0.5*t)
%本车车速
%v=60/3.6
%v_3=55/3.6
%s_3=30
%目标车道前车
%a_2=2*(v-v_2-s_2/t+6.0942/t)/t
%目标车道后车
%a_1=2*(v-v_1+s_1/t-6.0314/t)/t
clc
clear all
k1=1/5
k2=2/5
k3=2/5
%j=(k1/4)*abs(ay)+(k2/6)*tf
d=3.75
t_all=10%总的时间周期
tf=0:0.01:(t_all);%从总的时间周期中取出换道时间tf
len=length(tf);
j=zeros(1,len);%每个换道时间计算的j值
for i=1:len
    t=0:0.01:tf(i);
    len2=length(t);
    ay=zeros(1,len2);
    for m=1:len2
    ay(m)=(120*d*t(m)^3)/(tf(i)^5)-(180*d*t(m)^2)/(tf(i)^4)+(60*d*t(m))/(tf(i)^3);
    end
    v=60/3.6%%%本车车速
    v_3=55/3.6%当前车道前车车速
    s_3=30%与当前车道前车的距离
    v_2=65/3.6%目标车道前车的速度
    s_2=50%目标车道前车的距离
    v_1=60/3.6%目标车道后车的速度
    s_1=30%目标车道后车的距离
    a_1=2*(v-v_1+s_1/tf(i)-6.0314/tf(i))/tf(i)%目标车道后车允许的加速度
    a_2=2*(v-v_2-s_2/tf(i)+6.0942/tf(i))/tf(i)%目标车道前车允许的加速度
    a_3=2*(v-v_3-s_3/(0.5*tf(i))+6.15688/(0.5*tf(i)))/(0.5*tf(i))%当前车道前车的加速度
    %j(i)=(k1/4)*(max(ay))+(k2/6)*tf(i)+((min(abs(a_1),2)/2+min(abs(a_2),2)/2+min(abs(a_3),2)/2)/3)
    j(i)=(k1/4)*(max(ay))+(k2/6)*tf(i)+((min(abs(a_1),2)/2+min(abs(a_2),2)/2+min(abs(a_3),2)/2)/3)*k3
end
x=tf(:,101:1001)
y=j(:,101:1001)
plot(x,y)
axis([0,11,0,3])
index=find(y==min(y))
x(:,index)
xlabel('换道时间/s')
ylabel('目标函数J')
hold