基於元胞自動機NaSch模型的多車道手動-自動混合駕駛仿真模型的Matlab實現

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

花了一天半的時間用新學會的matlab實現了一下。

 

───────────────────────────────────────────────────────────────────────────────────────

2018-1-31更新：

居然真的有人會看我的博文誒，那我不能就這么不負責任的直接甩代碼跑路了，稍微評價一下我自己的代碼吧……

 

1、首先是優點，就是能用（囧），調調參數，幾個車道，多少元胞，多少輛車，自動駕駛車手動駕駛車比例，基本都能改。

2、缺點一：各種規則十分不清晰明確，正確性有待商榷……

　　特別是變道規則，又臭又長，我自己寫完之后都不想看第二遍，而且我懷疑跟論文里寫的規則有出入（雖然我說實話，論文里的變道規則也說的不是很清晰）

3、缺點二：速度賊慢，慢的跟爬一樣。因為這個代碼是為了數學建模敲的，所以也多考慮優化速度什么的，直接就簡單粗暴地寫了。

　　這導致了，我在出結果的時候，車道數不敢設大於3，元胞數不敢設大於200，循環時常不敢超過300,；

　　即使這樣，初始放入車輛的時候，元胞占用率大於0.3的時候，程序跑的就肉眼可見的慢，如果想跑個幾十上百組的數據求平均的話，估計可以打局王者榮耀再來看看結果……

4、穩定性賊差，我算結果的時候，幾百組幾百組求平均（元胞占用率0.0幾的情況下，跑的還是挺快的），結果都穩定不下來……導致我們敏感性分析基本等於瞎扯。

 

感覺自己是真雞兒的菜，寫了這么久的代碼，敲個稍微大點的程序，還是敲得跟坨shit一樣

───────────────────────────────────────────────────────────────────────────────────────

2018-5-7更新：

emmm發現居然少貼了一個函數的代碼，補上了calc_Vave()函數；

然后關於這個模型怎么用……

最簡單的用法是：

更改global變量的值（車輛數、車道數、自動駕駛車輛占比等等等等），更改模擬時長T，然后運行NaSch()這個函數，返回的ans答案是整個時長T內所有車輛的平均速度

───────────────────────────────────────────────────────────────────────────────────────

 

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
    [d,d_safe,d_l_for,d_l_bac,d_r_for,d_r_bac] = calcDist(cells,cars);
    
    %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 Ret = calc_Vave(cars)
global CarsNum;

sum = 0;
for i = 1:1:CarsNum
    sum = sum+cars(i).V;
end

Ret = sum./CarsNum;
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

 

源代碼直接下載鏈接