function [s,exitflag_s,dt,d,exitflag_d,AES_END_Flag,PlanFlag,t_1,s2]= PathOptimization(PathSwitch,ModeSelection,PlanFlag,AES_END_Flag,T,s0,T_lc,V_set, V_ego,a_ego,y0,y_delta,d_obs,k0,k1,V_lim, w,L_w,a_ymax,TTC,kappa_max,period,t_1, s_delay,d_delay,s1) coder.extrinsic('quadprog') %% 五次多项式的轨迹优化 % T_lc为换道的时距,s0为换道时的距离常数 % T为运动规划的时间窗宽度,V_set为驾驶员设定的期望车速 % w1、w2、w3、w4是多目标优化的权重系数 % V_ego为本车车速,a_ego为本车加速度 % y0为车道线横向偏差 % y_delta为避障模式下参考线为车道中心线向左或者向右偏离一定的量,即障碍物在本车道右边时向左偏移y_delta,障碍物在本车道左边时向右偏移y_delta %d_obs为障碍物的横向距离,delta_min为障碍物的横向安全距离 % delta_d为车辆偏离车道中心线允许的偏差 % delta_CA0为车道内转向避障的横向安全阈值 % delta_safe为横向安全阈值 % k0为航向角,k1为曲率,V_lim为车辆安全车速 % L_w为车道宽度,TTC为碰撞时间 % a_ymax为侧向加速度阈值,j_ymax为侧向冲击度阈值 % kappa_max为转向系统决定的最大曲率 % x_s=[a0,a1,a2,a3,a4,a5]'; % x_d=[b0,b1,b2,b3,b4,b5]'; if PathSwitch==1&&AES_END_Flag==0 if PlanFlag==1 s_T=s0+T_lc*V_set; a_max=20; j_max=200; w1=w(1,1); w2=w(2,1); w3=w(3,1); w4=w(4,1); %% s方向,纵向 H_s1=[zeros(3,6);0, 0, 0, 2*T, 3/2*T^2, 2/3*T^3; 0, 0, 0, 3/2*T^2, 4/3*T^3, 5/8*T^4;0, 0, 0, 2/3*T^3, 5/8*T^4, 3/10*T^5]; H_s2=[zeros(2,6);0, 0, T, 1/2*T^2, 1/6*T^3, 1/24*T^4; 0, 0, 1/2*T^2, 1/3*T^3, 1/8*T^4, 1/30*T^5; 0, 0, 1/6*T^3, 1/8*T^4, 1/20*T^5, 1/72*T^6; 0, 0, 1/24*T^4, 1/30*T^5,1/72*T^6, 1/252*T^7]; H_s3=[zeros(1,6);0, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5; 0, 1/2*T^2, 1/3*T^3, 1/8*T^4, 1/30*T^5, 1/144*T^6; 0, 1/6*T^3, 1/8*T^4, 1/20*T^5, 1/72*T^6, 1/336*T^7; 0, 1/24*T^4, 1/30*T^5, 1/72*T^6, 1/252*T^7, 1/1152*T^8; 0, 1/120*T^5, 1/144*T^6, 1/336*T^7, 1/1152*T^8, 1/5184*T^9]; H_s4=[T, T^2, 1/2*T^3, 1/6*T^4, 1/24*T^5, 1/120*T^6; T^2, T^3, 1/2*T^4, 1/6*T^5, 1/24*T^6, 1/120*T^7; 1/2*T^3, 1/2*T^4, 1/4*T^5, 1/12*T^6, 1/48*T^7, 1/240*T^8; 1/6*T^4, 1/6*T^5, 1/12*T^6, 1/36*T^7, 1/144*T^8, 1/720*T^9; 1/24*T^5, 1/24*T^6, 1/48*T^7, 1/144*T^8, 1/576*T^9, 1/2880*T^10; 1/120*T^6, 1/120*T^7, 1/240*T^8, 1/720*T^9, 1/2880*T^10, 1/14400*T^11]; f_s=-(2*w3*V_set+2*w4*T*s_T)*[1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5]; H_s=w1*H_s1+w2*H_s2+w3*H_s3+w4*H_s4; J_s0=w4*T*s_T^2+w3*T*V_set^2; %J_s=x_s'*H_s*x_s+f_s*x_s+J_s0; %%%%%%% 边界约束 %%%%%% Aeq_s=[1, 0, 0, 0, 0, 0;0, 1, 0, 0, 0, 0;0, 0, 1, 0, 0, 0; 0, 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4;0, 0, 1, T, 1/2*T^2, 1/6*T^3]; Beq_s=[0; V_ego; a_ego; V_set; 0]; %%%%%%% 安全性约束、舒适性约束、非完整性约束 %%%%%%% A_s=[0, -1, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, -1, 0, 0, 0; 0, 0, 1, 0, 0, 0; 0, 0, 0, -1, 0, 0; 0, 0, 0, 1, 0, 0; % t=0时 0, -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3, -1/24*(0.1*T)^4; 0, 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3, 1/24*(0.1*T)^4; 0, 0, -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3; 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3; 0, 0, 0, -1, -0.1*T, -1/2*(0.1*T)^2; 0, 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2;% t=0.1T时 0, -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3, -1/24*(0.2*T)^4; 0, 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3, 1/24*(0.2*T)^4; 0, 0, -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3; 0, 0, 0, -1, -0.2*T, -1/2*(0.2*T)^2; 0, 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2; % t=0.2T时 0, -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3, -1/24*(0.3*T)^4; 0, 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3, 1/24*(0.3*T)^4; 0, 0, -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3; 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3; 0, 0, 0, -1, -0.3*T, -1/2*(0.3*T)^2; 0, 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2;% t=0.3T时 0, -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3, -1/24*(0.4*T)^4; 0, 1, 0.4*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3, 1/24*(0.4*T)^4; 0, 0, -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3; 0, 0, 0, -1, -0.4*T, -1/2*(0.4*T)^2; 0, 0, 0, 1, 0.4*T, 1/2*(0.4*T)^2;% t=0.4T时 0, -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3, -1/24*(0.5*T)^4; 0, 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3, 1/24*(0.5*T)^4; 0, 0, -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3; 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3; 0, 0, 0, -1, -0.5*T, -1/2*(0.5*T)^2; 0, 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2;% t=0.5T时 0, -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3, -1/24*(0.6*T)^4; 0, 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3, 1/24*(0.6*T)^4; 0, 0, -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3; 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3; 0, 0, 0, -1, -0.6*T, -1/2*(0.6*T)^2; 0, 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2;% t=0.6T时 0, -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3, -1/24*(0.7*T)^4; 0, 1, 0.7*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3, 1/24*(0.7*T)^4; 0, 0, -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3; 0, 0, 1, 0.7*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3; 0, 0, 0, -1, -0.7*T, -1/2*(0.7*T)^2; 0, 0, 0, 1, 0.7*T, 1/2*(0.7*T)^2;% t=0.7T时 0, -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3, -1/24*(0.8*T)^4; 0, 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3, 1/24*(0.8*T)^4; 0, 0, -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3; 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3; 0, 0, 0, -1, -0.8*T, -1/2*(0.8*T)^2; 0, 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2;% t=0.8T时 0, -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3, -1/24*(0.9*T)^4; 0, 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3, 1/24*(0.9*T)^4; 0, 0, -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3; 0, 0, 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3; 0, 0, 0, -1, -0.9*T, -1/2*(0.9*T)^2; 0, 0, 0, 1, 0.2*T, 1/2*(0.9*T)^2;% t=0.9T时 0, -1, -T, -1/2*T^2, -1/6*T^3, -1/24*T^4; 0, 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4; 0, 0, -1, -T, -1/2*T^2, -1/6*T^3; 0, 0, 1, T, 1/2*T^2, 1/6*T^3; 0, 0, 0, -1, -T, -1/2*T^2; 0, 0, 0, 1, T, 1/2*T^2];% t=T时 B_s=[0; V_lim; a_max; a_max; j_max; j_max; % t=0时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.1T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.2T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.3T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.4T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.5T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.6T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.7T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.8T时 0; V_lim; a_max; a_max; j_max; j_max; % t=0.9T时 0; V_lim; a_max; a_max; j_max; j_max]; % t=T时 [s0,~,exitflag_S]=quadprog(H_s,f_s,A_s,B_s,Aeq_s,Beq_s); % 二次规划函数 %% d方向,横向 H_d1=H_s1; H_d2=[zeros(2,6);0, 0, 2*T, 3/2*T^2, 2/3*T^3, 5/24*T^4; 0, 0, 3/2*T^2, 4/3*T^3, 5/8*T^4, 1/5*T^5; 0, 0, 2/3*T^3, 5/8*T^4, 3/10*T^5, 7/72*T^6; 0, 0, 5/24*T^4, 1/5*T^5, 7/72*T^6, 2/63*T^7]; H_d3=H_s3; H_d4=[T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5, 1/720*T^6; 1/2*T^2, 1/3*T^3, 1/8*T^4, 1/30*T^5, 1/144*T^6, 1/840*T^7; 1/6*T^3, 1/8*T^4, 1/20*T^5, 1/72*T^6, 1/336*T^7, 1/1920*T^8; 1/24*T^4, 1/30*T^5, 1/72*T^6, 1/252*T^7, 1/1152*T^8, 1/6480*T^9; 1/120*T^5, 1/144*T^6, 1/336*T^7, 1/1152*T^8, 1/5184*T^9, 1/28800*T^10; 1/720*T^6, 1/840*T^7, 1/1920*T^8, 1/6480*T^9, 1/28800*T^10, 1/158400*T^11]; H_d=w1*H_d1+w2*H_d2+w3*H_d3+w4*H_d4; % J_d=x_d'*H_d*x_d; %%%%% 参数设置 %%%%% if ModeSelection==0 || ModeSelection==2%ModeSelection=0跟随模式,ModeSelection=1避障模式,ModeSelection=2换道模式 b_d2=0;delta_d=0.15*L_w; b_d1=delta_d-y0/2+y0/2*sign(y0);b_d3=delta_d+y0/2+y0/2*sign(y0); else y_0=y_delta+y0;b_d2=y_0;delta_d=0.3*L_w; b_d1=delta_d-y_0/2+y_0/2*sign(y_0); b_d3=delta_d+y_0/2+y_0/2*sign(y_0); end if ModeSelection==1 || ModeSelection==2 eta=1; %换道模式和避障模式 else eta=0; %跟随模式 end j_ymax=200;delta_safe=2;delta_safe0=2; %%%%% 边界约束 %%%%%% Aeq_d=[1, 0, 0, 0, 0, 0;0, 1, 0, 0, 0, 0;0, 0, 1, 0, 0, 0; 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5;0, 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4]; Beq_d=[y0; V_ego*k0; a_ego*k0+(V_ego^2)*k1; b_d2; 0]; %%%%%%% 安全性约束、舒适性约束、非完整性约束 %%%%%%% A_d=[sign(y0)*eta, TTC*sign(y0)*eta, 1/2*(TTC^2)*sign(y0)*eta, 1/6*(TTC^3)*sign(y0)*eta, 1/24*(TTC^4)*sign(y0)*eta, 1/120*(TTC^5)*sign(y0)*eta; %sign为取y0的正负号,y0为正数时等于1,y0为负数时等于-1,y0为0是等于0 -1, 0, 0, 0, 0, 0; 1, 0, 0, 0, 0, 0; 0, 0, -1, 0, 0, 0; 0, 0, 1, 0, 0, 0; 0, 0, 0, -1, 0, 0; 0, 0, 0, 1, 0, 0; % t=0时 -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3, -1/24*(0.1*T)^4, -1/120*(0.1*T)^5; 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3, 1/24*(0.1*T)^4, 1/120*(0.1*T)^5; 0, 0, -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3; 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3; 0, 0, 0, -1, -0.1*T, -1/2*(0.1*T)^2; 0, 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2;% t=0.1T时 -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3, -3/24*(0.2*T)^4, -1/120*(0.2*T)^5 ; 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3, 1/24*(0.2*T)^4, 1/120*(0.2*T)^5; 0, 0, -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3; 0, 0, 0, -1, -0.2*T, -1/2*(0.2*T)^2; 0, 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2;% t=0.2T时 -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3, -1/24*(0.3*T)^4, -1/120*(0.3*T)^5; 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3, 1/24*(0.3*T)^4, 1/120*(0.3*T)^5; 0, 0, -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3; 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3; 0, 0, 0, -1, -0.3*T, -1/2*(0.3*T)^2; 0, 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2;% t=0.3T时 -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3, -1/24*(0.4*T)^4, -1/120*(0.4*T)^5; 1, 0.4*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3, 1/24*(0.4*T)^4, 1/120*(0.4*T)^5; 0, 0, -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3; 0, 0, 1, 0.4*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3; 0, 0, 0, -1, -0.4*T, -1/2*(0.4*T)^2; 0, 0, 0, 1, 0.4*T, 1/2*(0.4*T)^2;% t=0.4T时 -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3, -1/24*(0.5*T)^4, -1/120*(0.5*T)^5; 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3, 1/24*(0.5*T)^4, 1/120*(0.5*T)^5; 0, 0, -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3; 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3; 0, 0, 0, -1, -0.5*T, -1/2*(0.5*T)^2; 0, 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2;% t=0.5T时 -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3, -1/24*(0.6*T)^4, -1/120*(0.6*T)^5; 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3, 1/24*(0.6*T)^4, 1/120*(0.6*T)^5; 0, 0, -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3; 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3; 0, 0, 0, -1, -0.6*T, -1/2*(0.6*T)^2; 0, 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2;% t=0.6T时 -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3, -1/24*(0.7*T)^4, -1/120*(0.7*T)^5; 1, 0.7*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3, 1/24*(0.7*T)^4, 1/120*(0.7*T)^5; 0, 0, -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3; 0, 0, 0, -1, -0.7*T, -1/2*(0.7*T)^2; 0, 0, 0, 1, 0.7*T, 1/2*(0.7*T)^2;% t=0.7T时 -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3, -1/24*(0.8*T)^4, -1/120*(0.8*T)^5; 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3, 1/24*(0.8*T)^4, 1/120*(0.8*T)^5; 0, 0, -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3; 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3; 0, 0, 0, -1, -0.8*T, -1/2*(0.8*T)^2; 0, 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2;% t=0.8T时 -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3, -1/24*(0.9*T)^4, -1/120*(0.9*T)^5; 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3, 1/24*(0.9*T)^4, 1/120*(0.9*T)^5; 0, 0, -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3; 0, 0, 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3; 0, 0, 0, -1, -0.9*T, -1/2*(0.9*T)^2; 0, 0, 0, 1, 0.9*T, 1/2*(0.9*T)^2;% t=0.9T时 -1, -T, -1/2*T^2, -1/6*T^3, -1/24*T^4, -1/120*T^5; 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5; 0, 0, -1, -T, -1/2*T^2, -1/6*T^3; 0, 0, 1, 0.2*T, 1/2*T^2, 1/6*T^3; 0, 0, 0, -1, -T, -1/2*T^2; 0, 0, 0, 1, T, 1/2*T^2;% t=T时 0, a_ego, -V_ego, 0, 0, 0;% t=0时 0, -a_ego, V_ego, 0, 0, 0;0, a_ego, a_ego*0.1*T-V_ego, 1/2*a_ego*(0.1*T)^2-V_ego*(0.1*T), 1/6*a_ego*(0.1*T)^3-1/2*V_ego*(0.1*T)^2, 1/24*a_ego*(0.1*T)^4-1/6*V_ego*(0.1*T)^3;% t=0.1T时 0, -a_ego, -(a_ego*0.1*T-V_ego), -(1/2*a_ego*(0.1*T)^2-V_ego*(0.1*T)), -(1/6*a_ego*(0.1*T)^3-1/2*V_ego*(0.1*T)^2), -(1/24*a_ego*(0.1*T)^4-1/6*V_ego*(0.1*T)^3); 0, a_ego, a_ego*0.2*T-V_ego, 1/2*a_ego*(0.2*T)^2-V_ego*(0.2*T), 1/6*a_ego*(0.2*T)^3-1/2*V_ego*(0.2*T)^2, 1/24*a_ego*(0.2*T)^4-1/6*V_ego*(0.2*T)^3;% t=0.2T时 0, -a_ego,-( a_ego*0.2*T-V_ego), -(1/2*a_ego*(0.2*T)^2-V_ego*(0.2*T)), -(1/6*a_ego*(0.2*T)^3-1/2*V_ego*(0.2*T)^2), -(1/24*a_ego*(0.2*T)^4-1/6*V_ego*(0.2*T)^3); 0, a_ego, a_ego*0.3*T-V_ego, 1/2*a_ego*(0.3*T)^2-V_ego*(0.3*T), 1/6*a_ego*(0.3*T)^3-1/2*V_ego*(0.3*T)^2, 1/24*a_ego*(0.3*T)^4-1/6*V_ego*(0.3*T)^3;% t=0.3T时 0, -a_ego, -(a_ego*0.3*T-V_ego), -(1/2*a_ego*(0.3*T)^2-V_ego*(0.3*T)), -(1/6*a_ego*(0.3*T)^3-1/2*V_ego*(0.3*T)^2), -(1/24*a_ego*(0.3*T)^4-1/6*V_ego*(0.3*T)^3); 0, a_ego, a_ego*0.4*T-V_ego, 1/2*a_ego*(0.4*T)^2-V_ego*(0.4*T), 1/6*a_ego*(0.4*T)^3-1/2*V_ego*(0.4*T)^2, 1/24*a_ego*(0.4*T)^4-1/6*V_ego*(0.4*T)^3;% t=0.4T时 0, -a_ego, -(a_ego*0.4*T-V_ego), -(1/2*a_ego*(0.4*T)^2-V_ego*(0.4*T)), -(1/6*a_ego*(0.4*T)^3-1/2*V_ego*(0.4*T)^2), -(1/24*a_ego*(0.4*T)^4-1/6*V_ego*(0.4*T)^3); 0, a_ego, a_ego*0.5*T-V_ego, 1/2*a_ego*(0.5*T)^2-V_ego*(0.5*T), 1/6*a_ego*(0.5*T)^3-1/2*V_ego*(0.5*T)^2, 1/24*a_ego*(0.5*T)^4-1/6*V_ego*(0.5*T)^3;% t=0.5T时 0, -a_ego, -(a_ego*0.5*T-V_ego), -(1/2*a_ego*(0.5*T)^2-V_ego*(0.5*T)), -(1/6*a_ego*(0.5*T)^3-1/2*V_ego*(0.5*T)^2), -(1/24*a_ego*(0.5*T)^4-1/6*V_ego*(0.5*T)^3); 0, a_ego, a_ego*0.6*T-V_ego, 1/2*a_ego*(0.6*T)^2-V_ego*(0.6*T), 1/6*a_ego*(0.6*T)^3-1/2*V_ego*(0.6*T)^2, 1/24*a_ego*(0.6*T)^4-1/6*V_ego*(0.6*T)^3;% t=0.6T时 0, -a_ego, -(a_ego*0.6*T-V_ego), -(1/2*a_ego*(0.6*T)^2-V_ego*(0.6*T)), -(1/6*a_ego*(0.6*T)^3-1/2*V_ego*(0.6*T)^2), -(1/24*a_ego*(0.6*T)^4-1/6*V_ego*(0.6*T)^3); 0, a_ego, a_ego*0.7*T-V_ego, 1/2*a_ego*(0.7*T)^2-V_ego*(0.7*T), 1/6*a_ego*(0.7*T)^3-1/2*V_ego*(0.7*T)^2, 1/24*a_ego*(0.7*T)^4-1/6*V_ego*(0.7*T)^3;% t=0.7T时 0, -a_ego, -(a_ego*0.7*T-V_ego), -(1/2*a_ego*(0.7*T)^2-V_ego*(0.7*T)), -(1/6*a_ego*(0.7*T)^3-1/2*V_ego*(0.7*T)^2), -(1/24*a_ego*(0.7*T)^4-1/6*V_ego*(0.7*T)^3); 0, a_ego, a_ego*0.8*T-V_ego, 1/2*a_ego*(0.8*T)^2-V_ego*(0.8*T), 1/6*a_ego*(0.8*T)^3-1/2*V_ego*(0.8*T)^2, 1/24*a_ego*(0.8*T)^4-1/6*V_ego*(0.8*T)^3;% t=0.8T时 0, -a_ego, -(a_ego*0.8*T-V_ego), -(1/2*a_ego*(0.8*T)^2-V_ego*(0.8*T)), -(1/6*a_ego*(0.8*T)^3-1/2*V_ego*(0.8*T)^2), -(1/24*a_ego*(0.8*T)^4-1/6*V_ego*(0.8*T)^3); 0, a_ego, a_ego*0.9*T-V_ego, 1/2*a_ego*(0.9*T)^2-V_ego*(0.9*T), 1/6*a_ego*(0.9*T)^3-1/2*V_ego*(0.9*T)^2, 1/24*a_ego*(0.9*T)^4-1/6*V_ego*(0.9*T)^3;% t=0.9T时 0, -a_ego, -(a_ego*0.9*T-V_ego), -(1/2*a_ego*(0.9*T)^2-V_ego*(0.9*T)), -(1/6*a_ego*(0.9*T)^3-1/2*V_ego*(0.9*T)^2), -(1/24*a_ego*(0.9*T)^4-1/6*V_ego*(0.9*T)^3); 0, a_ego, a_ego*T-V_ego, 1/2*a_ego*T^2-V_ego*T, 1/6*a_ego*T^3-1/2*V_ego*T^2, 1/24*a_ego*T^4-1/6*V_ego*T^3;% t=T时 0, -a_ego, -(a_ego*T-V_ego), -(1/2*a_ego*T^2-V_ego*T), -(1/6*a_ego*T^3-1/2*V_ego*T^2), -(1/24*a_ego*T^4-1/6*V_ego*T^3)]; B_d=[eta*d_obs*sign(y0)-eta*delta_safe;b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3; kappa_max*V_ego^3]; [d0,~,exitflag_D]=quadprog(H_d,zeros(6,1),A_d,B_d,Aeq_d,Beq_d); %二次规划函数 %% 验证二次规划函数是否有解 s5=zeros(6,1); exitflag_s5=0;exitflag_s5= exitflag_S; %定义数据类型,避免mxArrays数据类型 d5=zeros(6,1); exitflag_d5=0;exitflag_d5= exitflag_D; if exitflag_s5==1 && exitflag_d5==1 exitflag_s=1;s5=s0;s=s5;%求解s方向的多项式系数 exitflag_d=1;d5=d0;d=d5;%求解d方向的多项式系数 PlanFlag=0;%规划成功之后,不再对多项式系数进行求解 else exitflag_s=0;%路径规划失败 s=s5;exitflag_d=0;d=d5; PlanFlag=1;%规划失败之后,继续对多项式系数进行求解 end else s=s_delay;exitflag_s=1;%规划成功之后,系数保持不变,继续维持上一时刻 d=d_delay;exitflag_d=1; end %%%%根据避障过程中车辆的行驶里程,计算时间T s2=s1+V_ego*period+0.5*a_ego*(period^2);t_1=t_1+period; dt=[1,t_1,1/2*(t_1^2),1/6*(t_1^3),1/24*(t_1^4),1/120*(t_1^5)]*d; if t_1>=T-0.5 AES_END_Flag=1; %避障结束标志位 else AES_END_Flag=0; end else s=zeros(6,1);exitflag_s=-1;d=zeros(6,1);exitflag_d=-1; t_1=0;dt=0;AES_END_Flag=0;s2=0; end |