0. Intro.
<文笔匮乏望大家担待>。为了提高CAVs在复杂条件下的通过安全性和通行效率，在特定条件下进行多车协同与决策控制，特别是在城市情况下，具有很重要的研究意义和较大的挑战。对于城市场景，无信号的环形交叉路口(Fig-1)通常被认为比十字路口更复杂、更具挑战性。 本文主要尝试利用Level-k动态博弈算法对CAVs的决策策略进行建模。
Level-k 博弈理论的主要思想是认为人类驾驶员在决策过程中会有不同层次的推理水平(Reasoning level)，通过将环境特定交互范围内的所有博弈对象依据一定的规则进行等级划分(Level-k,k =0,1,2,...)，并且规定最低等级玩家会在不考虑其他玩家决策的情况下做出决策，而对于k不为0的玩家，通过假设其他玩家的等级都为(k-1)，然后根据这个假设来预测他们的行为，并依据预测结果做出最佳决策。
关于Level-k理论的具体介绍可见文献：Game-Theoretic Modeling of Multi-Vehicle Interactions at Uncontrolled Intersections.
1. Roundabout Scenario (RAS) Modeling
基于MATLAB编程(OOP)可轻松实现环岛场景的定义，亦可以通过常见的地图定义方式如ESDF，OGM(Occupancy Grid Map)等,这里MMiL采用前者, 建立地图的目的是便于车辆获取地图的位置信息, 所以在代码中地图以位置序列的形式输入决策层。
这里参照已有文献研究的无信号灯环岛场景进行场景搭建如下图：
1.1 道路基础信息
分析道路中的基础组成要素，可以先完成大的框架，如直线, 圆的定位以及绘制如 Fig-1.，很很明显这个图不太nice, 进一步观察一下可以适当完善空白区域以及添加适当的修饰文字比如定义车辆交互冲突区域，画上道路方位标志,车道标志etc. 完善后的如图 Fig-2.
 |
 |
从刚刚的道路建模的过程来看，整个过程还是非常简单的, 是吧。可能稍微复杂一点的是绿色道路区域需要的是不规则填充，这里可以建议用patch函数。然后保存相关的x-y坐标序列，便于决策算法进行碰撞检测等索引。
Part-codes
function Road_modeling(obj)
Road_struct_ini(obj);
% ----* Roundabout Scenario modeling
% H-B1
H_B1_L=[(-40:.5:-obj.Road_EX)',repmat(4,size(-40:.5:-obj.Road_EX,2),1)];
H_B1_R=[(obj.Road_EX:.5:40)', repmat(4,size(obj.Road_EX:.5:40,2),1)];
obj.UnsInter_Road.H_B1={[],[]};
obj.UnsInter_Road.H_B1(end,:)={'H-B1-Left','H-B1-Right'}; obj.UnsInter_Road.H_B1(end+1,:)={H_B1_L,H_B1_R};
% H-L2
H_L2_L=[(-40:.5:-obj.Road_EX)',zeros(size(-40:.5:-obj.Road_EX,2),1)];
H_L2_R=[(obj.Road_EX:.5:40)', zeros(size(obj.Road_EX:.5:40,2),1)];
obj.UnsInter_Road.H_L2(end,:)={'H-L2-Left','H-L2-Right'}; obj.UnsInter_Road.H_L2(end+1,:)={H_L2_L,H_L2_R};
% H-B2
H_B2_L=[(-40:.5:-obj.Road_EX)',repmat(-4,size(-40:.5:-obj.Road_EX,2),1)];
H_B2_R=[(obj.Road_EX:.5:40)', repmat(-4,size(obj.Road_EX:.5:40,2),1)];
obj.UnsInter_Road.H_B2(end,:)={'H-B2-Left','H-B2-Right'}; obj.UnsInter_Road.H_B2(end+1,:)={H_B2_L,H_B2_R};
% V-B1
V_B1_U=[repmat(-4,size(obj.Road_EX:.5:40,2),1),(obj.Road_EX:.5:40)']; % upper
V_B1_L=[repmat(-4,size(-40:.5:-obj.Road_EX,2),1),(-40:.5:-obj.Road_EX)']; % lower
obj.UnsInter_Road.V_B1(end,:)={'V-B1-Upper','V-B1-Lower'}; obj.UnsInter_Road.V_B1(end+1,:)={V_B1_U,V_B1_L};
% V_L2
V_L2_U=[zeros(size(obj.Road_EX:.5:40,2),1),(obj.Road_EX:.5:40)']; % upper
V_L2_L=[zeros(size(-40:.5:-obj.Road_EX,2),1),(-40:.5:-obj.Road_EX)']; % lower
obj.UnsInter_Road.V_L2(end,:)={'V-L2-Upper','V-L2-Lower'}; obj.UnsInter_Road.V_L2(end+1,:)={V_L2_U,V_L2_L};
% V-B2
V_B2_U=[repmat(4,size(obj.Road_EX:.5:40,2),1),(obj.Road_EX:.5:40)']; % upper
V_B2_L=[repmat(4,size(-40:.5:-obj.Road_EX,2),1),(-40:.5:-obj.Road_EX)']; % lower
obj.UnsInter_Road.V_B2(end,:)={'V-B2-Upper','V-B2-Lower'}; obj.UnsInter_Road.V_B2(end+1,:)={V_B2_U,V_B2_L};
obj.UnsInter_Road.Transi_Lane(end,:)={'transi_lane/L1-L4, Str_end_Node','Vis_Colli_CoG'};
% str_node--> end_node
% transi_L1
obj.UnsInter_Road.Transi_Lane(end+1,:)={[-obj.Road_W,obj.Road_EX;-obj.Road_EX,obj.Road_W],[-obj.Road_EX,obj.Road_EX]};
% transi_L2
obj.UnsInter_Road.Transi_Lane(end+1,:)={[obj.Road_EX,obj.Road_W,;obj.Road_W,obj.Road_EX],[obj.Road_EX,obj.Road_EX]};
% transi_L3
obj.UnsInter_Road.Transi_Lane(end+1,:)={[obj.Road_W,-obj.Road_EX;obj.Road_EX,-obj.Road_W],[obj.Road_EX,-obj.Road_EX]};
% transi_L4
obj.UnsInter_Road.Transi_Lane(end+1,:)={[-obj.Road_EX,-obj.Road_W,;-obj.Road_W,-obj.Road_EX],[-obj.Road_EX,-obj.Road_EX]};
% C1
obj.UnsInter_Road.Road_Curve(end,:)={'Curve_list,C1-C4','Circle_CoG','Radi'};
C1_radi =4; CoG_c1_x = -8; CoG_c1_y = 8; theta_C1 = 0:-pi/50:-0.5* pi;
C1_x = (CoG_c1_x+C1_radi*cos(theta_C1))'; C1_y = (CoG_c1_y+C1_radi*sin(theta_C1))';
obj.UnsInter_Road.Road_Curve(end+1,:)={[C1_x,C1_y],[-8,8],C1_radi};
% C2
C2_radi =4; CoG_c2_x = 8; CoG_c2_y = 8; theta_C2 = -0.5* pi:-pi/50:-pi;
C2_x = (CoG_c2_x+C2_radi*cos(theta_C2))'; C2_y = (CoG_c2_y+C2_radi*sin(theta_C2))';
obj.UnsInter_Road.Road_Curve(end+1,:)={[C2_x,C2_y],[8,8],C2_radi};
% C3
C3_radi =4; CoG_c3_x = 8; CoG_c3_y = -8; theta_C3 = 0.5* pi:pi/50:pi;
C3_x = (CoG_c3_x+C3_radi*cos(theta_C3))'; C3_y = (CoG_c3_y+C3_radi*sin(theta_C3))';
obj.UnsInter_Road.Road_Curve(end+1,:)={[C3_x,C3_y],[8,-8],C3_radi};
% C4
C4_radi =4; CoG_c4_x = -8; CoG_c4_y = -8; theta_C4 = 0:pi/50:0.5*pi;
C4_x = (CoG_c4_x+C4_radi*cos(theta_C4))'; C4_y = (CoG_c4_y+C4_radi*sin(theta_C4))';
obj.UnsInter_Road.Road_Curve(end+1,:)={[C4_x,C4_y],[-8,-8],C4_radi};
%-----*
% roundabout circle def
obj.UnsInter_Road.RoundA_Circle(end,:)={'RA_Curve X-Y','RA_Transi_Curve X-Y','RA-Radi'};
roundA_x =0; roundA_y = 0; theta_roundA = 0:pi/50:2* pi;
RoundA_x_list = (roundA_x+obj.RoundA_radii*cos(theta_roundA))'; RoundA_y_list = (roundA_y+obj.RoundA_radii*sin(theta_roundA))';
% calc RA_Transi_Curve
RoundA_transi_x_list = (roundA_x+obj.RA_transi_radii*cos(theta_roundA))'; RoundA_transi_y_list = (roundA_y+obj.RA_transi_radii*sin(theta_roundA))';
obj.UnsInter_Road.RoundA_Circle(end+1,:)={[RoundA_x_list,RoundA_y_list],[RoundA_transi_x_list,RoundA_transi_y_list],[obj.RoundA_radii,obj.RA_transi_radii]};
fprintf('
Road modeling completed.
');
end
1.2 全局参考线建模
对于level-k算法应用在环岛路口协同决策中，需要建立一条参考路径(reference line), 也可以理解为输入给决策层的粗糙全局导航路径。这里根据Fig-0.，建立了一条相对应的两车全局路径，如图Fig-3.。虽说这里只需要一条粗糙的参考路径，但Fig-0.所建立的路径也未免过于粗糙，所以这里MMiL引入路径平滑算法sEMA(对称指数移动平均滤波算法)进行适当的优化，看着效果还不错, hhhhh, 如果需要一条精细的路径，还需要满足CAVs最小转弯半径的需求，reeds-sheep曲线可满足要求。
Part-codes
% global refer.line modeling
function set_rerfence_line(obj)
fprintf("
Unsignalized roundabout scenario reference line modeling.
");
% ----------* calc ego
curr_road_ctrEqs=2; curr_road_ctrEqs_Flag='x'; % x=2
Transi_xy_list=get_RA_ref_part_line(obj,curr_road_ctrEqs_Flag,curr_road_ctrEqs); % seg_2 // Min-->Max（row-ind）
% seg-1
obj.ego_ref=[repmat(2,size(-40:.2:Transi_xy_list(1,2),2),1),(-40:.2:Transi_xy_list(1,2))'];
obj.ego_ref=[obj.ego_ref;Transi_xy_list];
% seg-3
seg3_tem=[repmat(2,size(Transi_xy_list(end,2):.2:80,2),1),(Transi_xy_list(end,2):.2:80)'];
obj.ego_ref=[obj.ego_ref;seg3_tem];
% ----------* calc obj
curr_road_ctrEqs_obj=-2; curr_road_ctrEqs_Flag_obj='y'; % y=2
Transi_xy_list_obj=get_RA_ref_part_line(obj,curr_road_ctrEqs_Flag_obj,curr_road_ctrEqs_obj); % seg_2 // Min-->Max（row-ind）
% seg-1
obj.obj_ref=[(-40:.2:Transi_xy_list_obj(1,1))',repmat(-2,size(-40:.2:Transi_xy_list_obj(1,1),2),1)];
obj.obj_ref=[obj.obj_ref;Transi_xy_list_obj];
% seg-3
seg3_tem=[(Transi_xy_list_obj(end,1):.2:80)',repmat(-2,size(Transi_xy_list_obj(end,1):.2:80,2),1)];
obj.obj_ref=[obj.obj_ref;seg3_tem];
% uni_smoothing by sEMA filter
Smooth_width=10;
obj.ego_ref=sEMA_Smoother(obj,obj.ego_ref,Smooth_width);
obj.obj_ref=sEMA_Smoother(obj,obj.obj_ref,Smooth_width);
end
2. Level-k algo.
------------------------------------ 暂且搁置..........较忙................ ---------------------------------
3. Simulation
仿真结果见Fig-4.
