AEB算法可视化（基于m脚本）

本文采用Matlab自动驾驶工具箱与m脚本结合进行可视化，对感知层不采用scenarioReader模块进行融合处理。

m脚本管理数据：

clc
clear
% 速度控制器数据字典
SpeedController.v_set=5.5;%设定的速度
SpeedController.Kp=1.1;%PID控制器参数
SpeedController.Ki=0.1;%PID控制器参数
SpeedController.Amax=3;%速度控制器限幅
SpeedController.Amin=-3;%速度控制器限幅
% AEB主算法数据字典
AEB.PB1_decel=3.8;%AEB轻制动的制动减速度
AEB.PB2_decel=5.3;%AEB重制动的制动减速度
AEB.FB_decel=9.8;%AEB全制动的制动减速度
AEB.timeMargin=0;%AEB剩余碰撞时间时间裕值
AEB.headwayOffset=1.7;%相对距离安全裕值
%FCW功能数据字典
FCW.driver_decel=4;
FCW.timeToReact=1.2;
%障碍物初始化数据
ObjInfo.IniposX=6.4194;
ObjInfo.IniposY=25.8677;
ObjInfo.VelX=1;
ObjInfo.VelY=0;
ObjInfo.accX=0;
ObjInfo.accY=0;

相对距离的计算如下：由于自动驾驶工具箱中的坐标定义和plot中坐标定义不同，故作如下修改：

relative函数定义如下：

function [rel_d, rel_v] =relative(egoposX,egoposY,ObjposX,ObjposY,egoVelX,egoVelY,ObjVelX,objVelY)
delta_X=egoposX-ObjposX;
delta_Y=egoposY-ObjposY;
rel_d=sqrt(delta_X^2+delta_Y^2);
Vel_X=egoVelX-ObjVelX;
Vel_Y=egoVelY-objVelY;
rel_v=sqrt(Vel_X^2+Vel_Y^2);
end

采用enable模块进行可视化：

绘图脚本如下：

function plotVelcheAndObj(velche_X,velche_Y, Obj_X,Obj_Y)
clear;
%主车参数
egoHeading=0.037*pi/180;
ego_L=4.7;
ego_W=1.8;
%障碍物参数
ObjHeading=89*pi/180;
Obj_L=0.24;
Obj_W=0.45;
[ veh_x, veh_y ] = PlotVehicle( velche_X, velche_Y, egoHeading, ego_L,ego_W);
[ obj_x, obj_y ] = PlotVehicle( Obj_X, Obj_Y,ObjHeading, Obj_L, Obj_W);
plot(veh_y, veh_x,'linewidth',2,'Color','r');
hold on;
set(gca,'XDir','reverse');
plot(obj_x, obj_y,'linewidth',2,'Color','b');
axis equal;
hold off;
end

plotvelche脚本如下：

function [ vehandObj_x, vehandObj_y ] = PlotVehicle( x_Bas, y_Bas, Heading, Width, Long )
HalfWidth = Width/2;
LF_x = (Long*cos(Heading) + Width/2*sin(Heading))+x_Bas;
LF_y = (-Width/2*sin(Heading) - Width/2*cos(Heading))+y_Bas;
LR_x = (0*cos(Heading) + Width/2*sin(Heading))+x_Bas;
LR_y = (-Width/2*sin(Heading) - Width/2*cos(Heading))+y_Bas;
RF_x = (Long*cos(Heading) -Width/2*sin(Heading))+x_Bas;
RF_y = (Long*sin(Heading) + Width/2*cos(Heading))+y_Bas;
RR_x = (0*cos(Heading) -Width/2*sin(Heading))+x_Bas;
RR_y = (Width/2*sin(Heading) +Width/2*cos(Heading))+y_Bas;
vehandObj_x = [LF_x, LR_x, RR_x, RF_x, LF_x];
vehandObj_y = [LF_y, LR_y, RR_y, RF_y, LF_y];
end

最终绘图效果如下：