以人工驾驶与自动车混行为环境做的三车道智能车轨迹优化

一、整体思路：

（1）下层车辆仿真：普通车：1本身普通车目标车道=初始车道，全程不换道；2普通车也需要换道，但是不编码，只能按照运行条件，可换道时换道，如果中途没有换道，则在阈值处强制换道。

（2）内部算法：智能车：在换道时间一定要换到目标车道，如果换道时间点不能换，则强制换道，有前后车加减速规则；（目标车道都是初始条件给定）

（3）算法：实数编码结合上下限算出具体间隔，竞标赛法选择，交叉方式为多点交叉和算术交叉，变异为多点变异

二、问题现状：

1、结果种群x均一样,车辆所在车道y无变换：

2、车辆均会以最大速度行驶：3、附上主函数代码段及算法：

%% 2020.7.24 自动驾驶与自动驾驶与人为驾驶车辆混行下的轨迹优化人为驾驶车辆混行下的轨迹优化
% 单次仿真
clc;
clear;
close all;
rng('default')
rng(1)
%% 参数设置 
%% 注：所有参数无单位（统一量纲），
%% 可统一添加 例如 车距2，最大速度6，统一长度单位为0.5，时间单位2s，那么它的实际车距1m,最大速度6*0.5米/2s=1.5m/s
data.s1_IDM=5; %参数
data.s0_IDM=5; %拥堵时期望最小间距
data.b_n_IDM=2; %期望减速度
data.sigma_IDM=2; %期望减速度
data.d_safe_bound=[1,5]; %最小安全间距
data.d_safe=3;
data.v_max=10; %车辆最大速度
data.a_max=2;%最大加速度
data.T_headway_bound=[1,10]; %安全车头时距
data.T_headway=5;
data.T_shortGap=1;
data.sigama=0.5; %加速系数
data.theta=4; %IDM模型中速度幂系数
data.num_Vehicle=[20,20]; %车辆数 智能30 自主70
data.num_Lane=3; %三车道
data.len_Road=1000; %在前1000个元宝生成车辆
data.len_Vehicle=10; %车身长度5
data.maxT=500; %仿真时长

data.changeAction=[1,2;
    2,1;
    2,3;
    3,2]; %换道行为     
data.y0=1000; %800米处强制换道
data.maxChange=2; %本路段最多换道次数
data.t0_bound=[0,0]; %换道间隔
data.t0=data.t0_bound(2);
data.mint0=1; %最小换道间隔
%% 随机生成车辆分布
data.Road=zeros(data.len_Road,data.num_Lane);
data.Vehicle=zeros(sum(data.num_Vehicle),20);
data.numChange=zeros(sum(data.num_Vehicle),1);
no=1;
for i=1:length(data.num_Vehicle)
    for j=1:data.num_Vehicle(i)
        type=i;
        y=randi(data.num_Lane);
        y0=randi(data.num_Lane); %目标出口
        data.numChange(no)=abs(y-y0);
        x=randi(data.len_Road-data.len_Vehicle-data.d_safe-1);
        t0=0;% 换道计时
        while sum(data.Road(x:x+data.len_Vehicle-1+data.d_safe,y))~=0
            y=randi(data.num_Lane);
            x=randi(data.len_Road-data.len_Vehicle);
        end
        v=randi(data.v_max);
        a=data.a_max;
        temp=[no,type,x,y,v,a,y0,t0,0];
        data.Vehicle(no,1:length(temp))=temp;
        data.Road(x:x+data.len_Vehicle-1,y)=1;
        no=no+1;
    end
end
data.index=find(data.Vehicle(:,2)==1 & data.Vehicle(:,4)~=data.Vehicle(:,7));
%%
data.len_Road=1500; %道路长度
%%
lb=0;
ub=1;
fobj=@aimFcn_1;
option.lb=lb;
option.ub=ub;
option.dim=sum(data.numChange(data.index));
if length(option.lb)==1
    option.lb=ones(1,option.dim)*option.lb;
    option.ub=ones(1,option.dim)*option.ub;
end
option.fobj=fobj;
option.showIter=0;

%% 算法参数设置
% 基本参数
option.numAgent=4;        %种群个体数
option.maxIteration=10;    %最大迭代次数
% 遗传算法
option.p1_GA=0.75;
option.p2_GA=0.2;
% 粒子群群算法
option.c1_pso=1;
option.c2_pso=1;
option.w_pso=0.1;
str_legend=[{'GA'},{'PSO'}];
%% 初始化种群个体
x=ones(option.numAgent,option.dim);
y=ones(option.numAgent,1);
for i=1:option.numAgent
    x(i,:)=rand(size(option.lb)).*(option.ub-option.lb)+option.lb;
    [y(i),~,x(i,:)]=option.fobj(x(i,:),option,data);
end
%% 使用算法求解
rng(1)
[bestY(1,:),bestX(1,:),recording(1)]=GA(x,y,option,data);
rng(1)
[bestY(2,:),bestX(2,:),recording(2)]=PSO(x,y,option,data);
%%
figure
hold on
for i=1:length(recording)
   plot((recording(i).bestFit),'LineWidth',2)
end
legend(str_legend)
title('适应度函数曲线')
%%


figure
hold on
plot((recording(1).bestFit),'LineWidth',2)
plot((recording(1).meanFit),'LineWidth',2)
legend('最优','平均')
title('遗传算法适应度函数曲线')
figure
hold on
plot((recording(2).bestFit),'LineWidth',2)
plot((recording(2).meanFit),'LineWidth',2)
legend('最优','平均')
title('粒子群算法适应度函数曲线')

%% 输出结果
str='遗传算法'
[~,result1]=aimFcn_2(bestX(1,:),option,data,str);
drawPC1(result1,option,data)
str='粒子群算法'
[~,result2]=aimFcn_2(bestX(2,:),option,data,str);
drawPC1(result2,option,data)

function [bestY,bestX,recording]=GA(x,y,option,data)
    %% 遗传算法
    %% 初始化
    recording.bestFit=zeros(option.maxIteration+1,1);
    recording.meanFit=zeros(option.maxIteration+1,1);
    %% 更新记录
    [y_g,position]=min(y);
    x_g=x(position(1),:);
    y_p=y;
    x_p=x;
    recording.bestFit=y_g;
    recording.meanFit=mean(y_p);
    %% 开始更新
    for iter=1:option.maxIteration
        disp(['GA,iter:',num2str(iter),',minFit:',num2str(y_g)])
        if iter==1
            newX=x*0;
            newY=y;
        end
        %% 竞标赛法选择个体
        for i=1:option.numAgent*2
            maxContestants=ceil(randi(option.numAgent)*option.p1_GA);
            index=randperm(option.numAgent,maxContestants);
            [~,position]=min(y(index));
            parent(i)=index(position(1));
        end
        p=y/sum(y);
        for i=1:option.numAgent*2
            tempR=randi(option.numAgent);
            p0=0;
            while 1
                if rand<p0+p(tempR)
                    parent(i)=tempR;
                    break;
                else
                    p0=p0+p(tempR);
                end
            end
        end
        newX=x*0;
        newY=y*0;
        %% 交叉(多点交叉)
        for i=1:option.numAgent
            if rand<option.p1_GA
                tempR=rand(size(x(i,:)));
                if rand<0.5
                    newX(i,:)=x(parent(i),:);
                    newX(i,tempR>0.5)=x(parent(i+option.numAgent),tempR>0.5);
                else
                    newX(i,:)=tempR.*x(parent(i),:)+(1-tempR).*x(parent(i+option.numAgent),:);
                end
            else
                newX(i,:)=x(parent(i),:);
            end
        end
        %% 变异
        for i=1:option.numAgent*option.p2_GA
            index=randi(option.numAgent);
            tempR=rand(size(x(i,:)));
            temp=rand(size(option.lb)).*(option.ub-option.lb)+option.lb;
            newX(index,tempR>0.5)=temp(tempR>0.5);
        end
        %% 重新计算适应度值
        for i=1:option.numAgent
            newX(i,:)=checkX(newX(i,:),option,data);
            [newY(i),~,newX(i,:)]=option.fobj(newX(i,:),option,data);
            if newY(i)<y_p(i)
                y_p(i)=newY(i);
                x_p(i,:)=newX(i,:);
                if y_p(i)<y_g
                    y_g=y_p(i);
                    x_g=x_p(i,:);
                end
            end
        end
        x=x_p;
        %% 更新记录
        recording.bestFit(1+iter)=y_g;
        recording.meanFit(1+iter)=mean(y_p);
    end
    bestY=y_g;
    bestX=x_g;
end

以上为部分内容，不知道是问题的模型原因还是算法设计错误，希望能有相关领域大佬给予指点问题所在原因，可私聊有酬