码迷，mamicode.com
首页 > 其他好文 > 详细

基于元胞自动机NaSch模型的多车道手动-自动混合驾驶仿真模型的Matlab实现

时间：2018-01-27 13:44:33      阅读：245      评论：0      收藏：0      [点我收藏+]

标签：ack   sim   star   for   man   loop   osi   bsp   shuffle   

模型的建立基本来自于：http://www.doc88.com/p-2078634086043.html

花了一天半的时间用新学会的matlab实现了一下。

 

NaSch模型主函数：

function [ret_Vave] = NaSch()
rng(‘shuffle‘);

global CellsNum; CellsNum = 200; %The number of cells in each lane
global LanesNum; LanesNum = 3; %The number of Lanes
global D; D = 0.02; %The cell occupancy rate
global L; L = 5; %The length of one cell
global CarsNum; CarsNum = round( LanesNum .* CellsNum .* D ); %The number of cars
global SelfRatio; SelfRatio = 0.5; %Self-driving vehicle rate
global Vmin; Vmin = 1; %Minimum speed limit
global Vmax; Vmax = 6; %Maximum speed limit

%Meet the lane changing conditions...
global ChangeRightP; ChangeRightP = 0.5; %The probability of changing into the right lane
global ChangeLeftP; ChangeLeftP = 0.8; %The probability of changing into the left lane

[z,cars] = RoadInit();
cells = z;
cars = CarInit(cars);

T = 1000; %The duration of simulation
Vave = zeros(1,T); %Average velocity per unit time

global loopCars; loopCars = [];
%Start the simulation
for t = 1:1:T
    
    [cells,cars] = EntryControl(cells,cars);
    
    %Calculate each distance
    for i = 1:1:CarsNum
        [d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac] = calcDist(cells,cars);
    end
    
    %Get the next state
    for i = 1:1:CarsNum
        if(cars(i).M==1) %Leftmost lane
            [cells,cars] = L_NextState(i,cells,cars,d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac);
        elseif(cars(i).M==LanesNum) %Rightmost lane
            [cells,cars] = R_NextState(i,cells,cars,d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac);
        else %Middle lane
            [cells,cars] = M_NextState(i,cells,cars,d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac);
        end
    end
    
    cars = RandSlow(cars);
    [cells,cars] = Update(cells,cars);
    cells = cells(1:LanesNum,1:CellsNum); %Control the size of Cells Matrix
    Vave(t) = calc_Vave(cars); %Calculate current average velocity
end

ret_Vave = mean(Vave); %Calculate average velocity
end

 

初始化函数：

function [cell_mat,car_mat] = RoadInit()
global LanesNum;
global CellsNum;
global CarsNum;

mat = zeros(LanesNum,CellsNum);

Car(1,CarsNum) = struct(‘ID‘,[],‘V‘,[],‘M‘,[],‘N‘,[],‘Self‘,[]);
%Vehicle Information: ID, Speed, Position, Whether self driving or not

for i = 1:1:CarsNum
    m=round( 1 + ( LanesNum - 1 ) * rand(1) );
    n=round( 1 + ( CellsNum - 1 ) * rand(1) );
    while( mat(m,n) ~= 0 )
        m = round( 1 + ( LanesNum - 1 ) * rand(1) );
        n = round( 1 + ( CellsNum - 1 ) * rand(1) );
    end
    mat(m,n) = i;
    
    Car(i).ID = i;
    Car(i).M = m;
    Car(i).N = n;
end

cell_mat = mat;
car_mat = Car;
end

function car_mat = CarInit(Car)
global CarsNum;
global Vmin;
global Vmax;
global SelfRatio;

for i = 1:1:CarsNum
    Car(i).V = round( Vmin + ( Vmax - Vmin ) * rand(1) );
    if(rand(1)<=SelfRatio) Car(i).Self = 1;
    else Car(i).Self = 0;
    end
end

car_mat = Car;
end

 

入口控制：

function [new_cells,new_cars] = EntryControl(cells,cars)
global loopCars;
global LanesNum;
global Vmin;

while(StackSize(loopCars) > 0)
    
    if( all( cells(:,1) ~= 0 ) == 1 )
        break
    end
    
    for i = 1:1:LanesNum
        if(cells(i,1) == 0)
            id = topStack(loopCars); popStack(loopCars);
            cells(i,1) = id;
            cars(id).V = Vmin;
            cars(id).M = i;
            cars(id).N = 1;
            break
        end
    end
    
end

new_cells = cells;
new_cars = cars;
end

 

计算各种车距：

function [Ret_d,Ret_d_safe,Ret_d_l_for,Ret_d_l_bac,Ret_d_r_for,Ret_d_r_bac] = calcDist(cells,cars)
global LanesNum;
global CellsNum;
global CarsNum;

d = linspace(-1,-1,CarsNum);
d_safe = linspace(-1,-1,CarsNum);
d_l_for = linspace(-1,-1,CarsNum);
d_l_bac = linspace(-1,-1,CarsNum);
d_r_for = linspace(-1,-1,CarsNum);
d_r_bac = linspace(-1,-1,CarsNum);

for i = 1:1:CarsNum
    
    m = cars(i).M; %The m_th lane
    n = cars(i).N; %The n_th cell
    
    %Calculate the distance to front car
    d(i) = inf;
    for f = n+1:1:CellsNum
        if(cells(m,f) ~= 0)
            d(i) = f - n;
            break
        end
    end
    
    %If the left lane exist
    if( m-1 >= 1 )
        %Forward
        d_l_for(i) = inf;
        for p = n:1:CellsNum
            if( cells( m-1 , p ) ~= 0 )
                d_l_for(i) = p - n;
                break
            end
        end
        %Backward
        for p = n:-1:1
            d_l_bac(i) = inf;
            if( cells( m-1 , p ) ~= 0 )
                d_l_bac(i) = n - p;
                break
            end
        end
    end
    
    %If the right lane exist
    if( m+1 <= LanesNum )
        %Forward
        d_r_for(i) = inf;
        for p = n:1:CellsNum
            if( cells( m+1 , p ) ~= 0 )
                d_r_for(i) = p - n;
                break
            end
        end
        %Backward
        d_r_bac(i) = inf;
        for p = n:-1:1
            if( cells( m+1 , p ) ~= 0 )
                d_r_bac(i) = n - p;
                break
            end
        end
    end
    
    %The d_safe of two kinds of cars is different
    if(cars(i).Self == 1)
        d_safe(i) = cars(i).V;
    else
        d_safe(i) = 2 .* cars(i).V;
    end
    
end

Ret_d = d;
Ret_d_safe = d_safe;
Ret_d_l_for = d_l_for;
Ret_d_l_bac = d_l_bac;
Ret_d_r_for = d_r_for;
Ret_d_r_bac = d_r_bac;
end

 

加减速规则实现：

function new_cars = Accelerate(id,cars,d,d_Safe)
global Vmax;
global Pa;
Pa = 0.8; %Acceleration probability

if( d(id) > d_Safe(id) && cars(id).V < Vmax )
    if(rand(1) <= Pa)
        cars(id).V = cars(id).V + 1;
    end
end

new_cars = cars;
end

function new_cars = Decelerate(id,cars,d,d_safe)
global Vmax;
global Vmin;

if( d(id) < d_safe(id) && d(id) >= Vmax - Vmin )
    cars(id).V = max( cars(id).V - 1 , Vmin );
elseif( d(id) < Vmax - Vmin )
    cars(id).V = Vmin;
end

new_cars = cars;
end

 

关于变道的状态更新：

function [new_cells,new_cars] = L_NextState(i,cells,cars,d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac)
global Vmax;
global LanesNum;
global loopCars;

if( d_r_for(i) > d_safe(i) ) %与右前方车辆距离大于安全距离
    ok1 = 1;
else
    ok1 = 0;
end

if( d_r_bac(i) > 1 + Vmax - min(cars(i).V+1,Vmax) ) %与右后方车辆安全
    ok2 = 1;
else
    ok2 = 0;
end

if( cars(i).M+2 <= LanesNum && cells(cars(i).M+2,cars(i).N) ~= 0 ) %右边相隔一个车道平行位置有车
    id = cells(cars(i).M+2,cars(i).N); %获取该车编号
    if( min( d_l_for(id) , d_l_bac(id) ) < d_safe(id) ) %判断该车不能向左变道
        ok3 = 1;
    else
        ok3 = 0;
    end
else
    ok3 = 1;
end

ChangeLaneOK = ok1 && ok2 && ok3; %得到是否有变道条件

if( d(i) < d_safe(i) ) %与前车距离小于安全距离，能变道则必须变道
    if(ChangeLaneOK)
        
        cells(cars(i).M,cars(i).N) = 0;
        cars(i).M = cars(i).M + 1;
        if( cells( cars(i).M , cars(i).N ) ~= 0 ) %撞车控制
            loopCars = pushStack(loopCars,cars(i).ID);
        else
            cells(cars(i).M,cars(i).N) = cars(i).ID; %变道
        end
        
        cars = Accelerate(i,cars,d,d_safe); %加速
    else
        cars = Decelerate(i,cars,d,d_safe); %减速
    end
else %与前车距离大于等于安全距离
    ChangeLaneDesire = getCLD(i,cars,‘R‘); %计算变道欲望
    if(ChangeLaneOK && ChangeLaneDesire)
        
        cells(cars(i).M,cars(i).N) = 0;
        cars(i).M = cars(i).M + 1;
        if( cells( cars(i).M , cars(i).N ) ~= 0 ) %撞车控制
            loopCars = pushStack(loopCars,cars(i).ID);
        else
            cells(cars(i).M,cars(i).N) = cars(i).ID; %变道
        end
        
        cars = Accelerate(i,cars,d,d_safe); %加速
    else
        cars = Accelerate(i,cars,d,d_safe); %加速
        cars = Decelerate(i,cars,d,d_safe); %减速
    end
end

new_cells = cells;
new_cars = cars;
end

function [new_cells,new_cars] = M_NextState(i,cells,cars,d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac)
global Vmax;
global LanesNum;
global loopCars;
%计算中间车道上车辆的下一时刻状态


if( d_l_for(i) > d_safe(i) ) %与左前方车辆距离大于安全距离
    ok1 = 1;
else
    ok1 = 0;
end

if( d_l_bac(i) > 1 + Vmax - min(cars(i).V+1,Vmax) ) %与左后方车辆安全
    ok2 = 1;
else
    ok2 = 0;
end

if( cars(i).M-2 >= 1 && cells(cars(i).M-2,cars(i).N) ~= 0 ) %左边相隔一个车道平行位置有车
    id = cells(cars(i).M-2,cars(i).N); %获取该车编号
    if( min( d_r_for(id) , d_r_bac(id) ) < d_safe(id) ) %判断该车不能向右变道
        ok3 = 1;
    else
        ok3 = 0;
    end
else
    ok3 = 1;
end

ChangeLeftLaneOK = ok1 && ok2 && ok3; %得到是否有向左变道条件


if( d_r_for(i) > d_safe(i) ) %与右前方车辆距离大于安全距离
    ok1 = 1;
else
    ok1 = 0;
end

if( d_r_bac(i) ~= inf && d_r_bac(i) ~= -1 && cells( cars(i).M + 1 , cars(i).N - d_r_bac(i) ) > 0 ) %右后方有车
    id = cells( cars(i).M + 1 , cars(i).N - d_r_bac(i) ); %得到该车的编号
    if( d_r_bac(i) > d_safe(id) ) %与右后方车辆安全
        ok2 = 1;
    else
        ok2 = 0;
    end
else
    ok2 = 1;
end

if( cars(i).M+2 <= LanesNum && cells(cars(i).M+2,cars(i).N) ~= 0 ) %右边相隔一个车道平行位置有车
    id = cells(cars(i).M+2,cars(i).N); %获取该车编号
    if( min( d_l_for(id) , d_l_bac(id) ) < d_safe(id) ) %判断该车不能向左变道
        ok3 = 1;
    else
        ok3 = 0;
    end
else
    ok3 = 1;
end

ChangeRightLaneOK = ok1 && ok2 && ok3; %得到是否有向右变道条件


if( d(i) < d_safe(i) ) %与前车距离小于安全距离
    
    if(ChangeRightLaneOK) %能够向右变道则向右变道
        
        cells(cars(i).M,cars(i).N) = 0;
        cars(i).M = cars(i).M + 1;
        if( cells( cars(i).M , cars(i).N ) ~= 0 ) %撞车控制
            loopCars = pushStack(loopCars,cars(i).ID);
        else
            cells(cars(i).M,cars(i).N) = cars(i).ID; %变道
        end
        
        cars = Accelerate(i,cars,d,d_safe); %加速
        
    else %不能向右变道则尝试向左变道
        
        if(ChangeLeftLaneOK == 1) %能向左变道则向左变道
            
            cells(cars(i).M,cars(i).N) = 0;
            cars(i).M = cars(i).M - 1;
            if( cells( cars(i).M , cars(i).N ) ~= 0 ) %撞车控制
                loopCars = pushStack(loopCars,cars(i).ID);
            else
                cells(cars(i).M,cars(i).N) = cars(i).ID; %变道
            end
            
        else %两边都不允许变道
            cars = Decelerate(i,cars,d,d_safe); %减速
        end
        
    end
    
else %与前车距离大于等于安全距离
    
    ChangeLaneDesire = getCLD(i,cars,‘R‘); %计算变道欲望
    
    if(ChangeRightLaneOK && ChangeLaneDesire) %能够向右变道，且有变道欲望
        
        cells(cars(i).M,cars(i).N) = 0;
        cars(i).M = cars(i).M + 1;
        if( cells( cars(i).M , cars(i).N ) ~= 0 ) %撞车控制
            loopCars = pushStack(loopCars,cars(i).ID);
        else
            cells(cars(i).M,cars(i).N) = cars(i).ID; %变道
        end
        
        cars = Accelerate(i,cars,d,d_safe); %加速
    else
        cars = Accelerate(i,cars,d,d_safe); %加速
        cars = Decelerate(i,cars,d,d_safe); %减速
    end
    
end

new_cells = cells;
new_cars = cars;
end

function [new_cells,new_cars] = R_NextState(i,cells,cars,d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac)
global Vmax;
global loopCars;
%计算最右侧车道上车辆的下一时刻状态

if( d_l_for(i) > d_safe(i) ) %与左前方车辆距离大于安全距离
    ok1 = 1;
else
    ok1 = 0;
end

if( d_l_bac(i) > 1 + Vmax - min(cars(i).V+1,Vmax) ) %与左后方车辆安全
    ok2 = 1;
else
    ok2 = 0;
end

if( cars(i).M-2 >= 1 && cells(cars(i).M-2,cars(i).N) ~= 0 ) %左边相隔一个车道平行位置有车
    id = cells(cars(i).M-2,cars(i).N); %获取该车编号
    if( min( d_r_for(id) , d_r_bac(id) ) < d_safe(id) ) %判断该车不能向右变道
        ok3 = 1;
    else
        ok3 = 0;
    end
else
    ok3 = 1;
end

ChangeLaneOK = ok1 && ok2 && ok3; %得到是否有变道条件

if( d(i) < d_safe(i) ) %与前车距离小于安全距离
    ChangeLaneDesire = getCLD(i,cars,‘L‘); %计算变道欲望
    if(ChangeLaneOK && ChangeLaneDesire)
        cells(cars(i).M,cars(i).N) = 0;
        cars(i).M = cars(i).M - 1;
        if( cells( cars(i).M , cars(i).N ) ~= 0 ) %撞车控制
            loopCars = pushStack(loopCars,cars(i).ID);
        else
            cells(cars(i).M,cars(i).N) = cars(i).ID; %变道
        end
    else
        cars = Decelerate(i,cars,d,d_safe); %减速
    end
else %与前车距离大于等于安全距离
    cars = Accelerate(i,cars,d,d_safe); %加速
    cars = Decelerate(i,cars,d,d_safe); %减速
end

new_cells = cells;
new_cars = cars;
end

 

计算变道欲望：

function ret = getCLD(id,cars,Type)
%Calculate the desire to change the lane
global ChangeRightP;
global ChangeLeftP;

if(Type == ‘R‘) %To right
    if(cars(id).Self == 1) %Self driving
        ret = 1;
    else %Manual driving
        if(rand(1) <= ChangeRightP)
            ret = 1;
        else
            ret = 0;
        end
    end;
else %To left
    if(cars(id).Self == 1) %Self driving
        ret = 1;
    else %Manual driving
        if(rand(1) <= ChangeLeftP)
            ret = 1;
        else
            ret = 0;
        end
    end;
end

end

 

随机慢化：

function new_cars = RandSlow(cars)
global CarsNum;
global Vmin;

p=0.1; %手动车慢化概率
q=0.03; %自动驾驶车慢化概率

for i = 1:1:CarsNum
    if(cars(i).Self==1)
        if(rand(1) <= q)
            cars(i).V = max( cars(i).V - 1 , Vmin );
        end
    else
        if(rand(1) <= p)
            cars(i).V = max( cars(i).V - 1 , Vmin );
        end
    end
end

new_cars = cars;
end

 

状态更新：

function [new_cells,new_cars] = Update(cells,cars)
global CarsNum;
global CellsNum;
global loopCars;

for i = 1:1:CarsNum
    cells( cars(i).M , cars(i).N ) = 0;
    if( cars(i).N + cars(i).V > CellsNum ) %Drive out of the road
        loopCars = pushStack(loopCars,cars(i).ID);
    else
        cells( cars(i).M , cars(i).N ) = cars(i).ID;
    end;
end

new_cells = cells;
new_cars = cars;
end

 

栈模拟：

function newStack = pushStack(Stack,val)
newStack = [val,Stack];
end
function newStack = popStack(Stack)
newStack = Stack(2:end);
end

function ret = StackSize(Stack)
ret = size(Stack,2);
end

function Val = topStack(Stack)
Val = Stack(1);
end

 

源代码直接下载链接

基于元胞自动机NaSch模型的多车道手动-自动混合驾驶仿真模型的Matlab实现

标签：ack   sim   star   for   man   loop   osi   bsp   shuffle   

原文地址：https://www.cnblogs.com/dilthey/p/8365141.html
踩
(0)
赞
(0)
   
举报
评论 一句话评论（0）
登录后才能评论！
分享档案
更多>
2021年07月29日 (22)
2021年07月28日 (40)
2021年07月27日 (32)
2021年07月26日 (79)
2021年07月23日 (29)
2021年07月22日 (30)
2021年07月21日 (42)
2021年07月20日 (16)
2021年07月19日 (90)
2021年07月16日 (35)
周排行
mamicode.com排行更多图片
更多
友情链接
兰亭集智   国之画   百度统计   站长统计   阿里云   chrome插件   新版天听网
关于我们 - 联系我们 - 留言反馈
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码！