%%  考虑驾驶员特性和前车速度的单向3车道模型
clc;
clear;
%%  参数设置
lane_length = 100; %车道长度
car_rate = 0.1;  %车辆占有率
v_max = 5; %最大车速
time_max = 1000; %仿真步长
time_span = 0.1; %仿真图片输出间隔
p_slowdown = 0.3; %随机慢化概率
p_changelane = 0.8; %满足换道条件换道概率%%  随机生成初始车辆信息
[space,car,car_number] = initialize(car_rate,lane_length,v_max);%%  显示初始仿真图
figure('doublebuffer','on');%开启双缓存
space = -1*space;
H = imshow(space,[]);
title('考虑驾驶员特性及前车速度的单向3车道模型','color','red');
space = -1*space;%%  开始仿真
for time=1:time_max%%  换道阶段[car,space] = change_lane(space,car,car_number,p_changelane,lane_length,v_max);%%  跟驰阶段[space,car] = follow(space,car,v_max,lane_length,p_slowdown,car_number);%%  显示仿真图space = -1*space;set(H,'CData',space);pause(time_span);space = -1*space;
end

function [space,car,car_number] = initialize(car_rate,lane_length,v_max)
car_number = fix(1+(3*lane_length-1)*car_rate); %按车辆占有率算出的车辆数
%%  创建空间
space = zeros(3,lane_length);%元胞空间
car = struct('v',zeros(1,car_number),'m',zeros(1,car_number),'n',zeros(1,car_number),'r',zeros(1,car_number),'a',zeros(1,car_number));
% 车辆信息结构体从左到右为速度，车道，列，反应时间，驾驶员对冒险驾驶行为的偏好
%%  随机生成初始车辆信息
for id=1:car_number%%  位置信息初始化if  id<=fix(car_number/3)%%  最左边车道随机投放车辆car.m(id) = 1;car.n(id) = fix( 1+rand(1)*(lane_length-1) );while space(car.m(id),car.n(id))==1car.n(id) = fix( 1+rand(1)*(lane_length-1) );endspace( 1,car.n(id) ) = 1;elseif  id<fix( (car_number*2)/3 )%%  中间车道随机投放车辆car.m(id) = 2;car.n(id) = fix( 1+rand(1)*(lane_length-1) );while space(car.m(id),car.n(id))==1car.n(id) = fix( 1+rand(1)*(lane_length-1) );endspace( 2,car.n(id) ) = 1;else%%  最右边车道投放车辆   car.m(id) = 3;car.n(id) = fix( 1+rand(1)*(lane_length-1) );while space(car.m(id),car.n(id))==1car.n(id) = fix( 1+rand(1)*(lane_length-1) );endspace( 3,car.n(id) ) = 1;end%%  车速及驾驶员特性初始化car.v(id) = fix( 1+rand(1)*(v_max-1) ); %速度初始化car.r(id) = rand(1); %随机生成驾驶员反应时间和对冒险驾驶行为的偏好参数，均为0到1间数字car.a(id) = 1-car.r(id);end
end

function [car,space] = change_lane(space,car,car_number,p_changelane,lane_length,v_max)
%换道函数
for id=1:car_number[cycle,empty]=get_empty_front(car.m(id),car.n(id),lane_length,space);%获取前方车距if cycle  %头车不换道v_front = car.v( logical( (car.n==(car.n(id) +empty+1)) .* (car.m==car.m(id)) ) );%获取前车速度if car.m(id) == 1 %左侧车道换道[cycle_right,empty_right_front] = get_empty_front(car.m(id)+1,car.n(id),lane_length,space);%获取右侧车道车距if cycle_rightv_front_right = car.v( logical( (car.n==(car.n(id) +empty_right_front+1)) .* (car.m==car.m(id)+1) ));%获取右前车速度elsev_front_right = v_max;end%%  换道决策if   empty+car.a(id)*v_front < car.v(id)*(1+car.r(id))%%  前车阻碍车辆对速度的追求if  (v_front_right*car.a(id)+empty_right_front) > (empty+car.a(id)*v_front) && space( car.m(id)+1,car.n(id) )==0  %旁边车道距离更大且旁边无车%%  满足安全条件换道if  rand(1)<p_changelane %满足换道条件以概率换道space(car.m(id),car.n(id)) =0;car.m(id) = car.m(id)+1;space(car.m(id),car.n(id)) =1;endendendelseif car.m(id) == 2 %中间车道换道[cycle_right,empty_right_front] = get_empty_front(car.m(id)+1,car.n(id),lane_length,space);%获取右侧车道车距[cycle_left,empty_left_front] = get_empty_front(car.m(id)-1,car.n(id),lane_length,space);%获取左侧车道车距%获取右前车速度if cycle_rightv_front_right = car.v( logical( (car.n==(car.n(id) +empty_right_front+1)) .* (car.m==car.m(id)+1) ));elsev_front_right = v_max;end%获取左前车速度if cycle_leftv_front_left = car.v( logical( (car.n==(car.n(id) +empty_left_front+1)) .* (car.m==car.m(id)-1) ));elsev_front_left = v_max;end%%  换道决策if  empty+car.a(id)*v_front < car.v(id)*(1+car.r(id))%%  前车阻碍车辆对速度的追求%%  优先左换道if  (v_front_left*car.a(id)+empty_left_front) > (empty+car.a(id)*v_front) && space( car.m(id)-1,car.n(id) )==0  %旁边车道距离更大且旁边无车%%  满足安全条件换道if  rand(1)<p_changelane %满足换道条件以概率换道space(car.m(id),car.n(id)) =0;car.m(id) = car.m(id)-1;space(car.m(id),car.n(id)) =1;endelse%左换道条件不满足则右换道if  (v_front_right*car.a(id)+empty_right_front) > (empty+car.a(id)*v_front) && space( car.m(id)+1,car.n(id) )==0  %旁边车道距离更大且旁边无车%%  满足安全条件换道if  rand(1)<p_changelane %满足换道条件以概率换道space(car.m(id),car.n(id)) =0;car.m(id) = car.m(id)+1;space(car.m(id),car.n(id)) =1;endendendendelse  %右侧车道换道[cycle_left,empty_left_front] = get_empty_front(car.m(id)-1,car.n(id),lane_length,space);%获取左侧车道车距if cycle_leftv_front_left = car.v( logical( (car.n==(car.n(id) +empty_left_front+1)) .* (car.m==car.m(id)-1) ));%获取左前车速度elsev_front_left = v_max;end%%  换道决策if  empty+car.a(id)*v_front < car.v(id)*(1+car.r(id))%%  前车阻碍车辆对速度的追求if  (v_front_left*car.a(id)+empty_left_front) > (empty+car.a(id)*v_front) && space( car.m(id)-1,car.n(id) )==0  %旁边车道距离更大且旁边无车%%  满足安全条件换道if  rand(1)<p_changelane %满足换道条件以概率换道space(car.m(id),car.n(id)) =0;car.m(id) = car.m(id)-1;space(car.m(id),car.n(id)) =1;endendendendend
end

function [space,car] = follow(space,car,v_max,lane_length,p_slowdown,car_number)
%跟驰函数
cycle = zeros(1,car_number);%储存车辆是否为头车
for id = 1:car_number%%  加速car.v(id) = min(car.v(id)+1,v_max);%%  获取车辆前空元胞数以及是否符合满足周期循环[cycle(id),empty]=get_empty_front(car.m(id),car.n(id),lane_length,space);%%  减速if cycle  %头车不受前车影响而减速if cycle(id) %获取前车速度v_front = car.v( logical( (car.n==(car.n(id) +empty+1)) .* (car.m==car.m(id)) ) );%获取前车速度elsev_front = v_max;endcar.v(id) = min (car.v(id) , fix(empty+car.a(id)*v_front-car.v(id)*car.r(id)) );%车速不能超过驾驶员预估空间end%%  概率慢化if  rand(1) <= p_slowdowncar.v(id) = max( car.v(id)-1,0 );end
end%%  位置更新e = 2;f = 2;g = 2;for id = 1:car_numberif  car.n(id)+v_max >= lane_length %会驶出车道%%  周期边界条件将头车以原速度道路最左边if cycle(id)==0 %头车if  car.n(id)+car.v(id)>lane_lengthspace ( car.m(id),car.n(id) ) = 0;car.n(id) = 1;space ( car.m(id),car.n(id) ) = 1;endelse %非头车if  car.n(id)+car.v(id)>lane_lengthspace ( car.m(id),car.n(id) ) = 0;switch car.m(id)case 1car.n(id) = e;e = e+1;case 2car.n(id) = f;f = f+1;case 3car.n(id) = g;g = g+1;endspace ( car.m(id),car.n(id) ) = 1;end  endspace ( car.m(id),car.n(id) ) = 0;car.n(id) = car.n(id) +car.v(id);while  space ( car.m(id),car.n(id) ) == 1car.n(id) = car.n(id) -1;endspace ( car.m(id),car.n(id) ) = 1;else %不可能驶出车道space ( car.m(id),car.n(id) ) = 0;                car.n(id) = car.n(id) +car.v(id);while  space ( car.m(id),car.n(id) ) == 1car.n(id) = car.n(id) -1;endspace ( car.m(id),car.n(id) ) = 1;endend
end

%用于求m车道n列的前车距以及判断是否为头车，cycle值为0是头车
function [cycle,empty_front] = get_empty_front(m,n,lane_length,space)
empty_front = 1;
cycle=1;
if  n+1 < lane_length%求前车距while space(m,n+empty_front) == 0if n+empty_front<lane_lengthempty_front = empty_front+1;elsecycle = 0;empty_front = empty_front+1;break;endend   empty_front = empty_front-1;
elseempty_front = 0;cycle = 0;
end
end