如何在轨迹规划或轨迹序列中画一个好看的车辆示意图(MATLAB 代码实现)

0. Motivation

       发过论文或者准备发论文的小伙伴或都遇见个这样的问题，如何将算法仿真的车辆轨迹序列可视化得好看，这对于论文是否accepted有一定的影响，常见于轨迹规划，轨迹跟踪控制等算法仿真场景中。 

       在可视化时，抛去车辆的外形，需要将车辆的重要状态信息表示出来: 车辆的位置坐标，航向角，方向盘转角(轨迹跟踪控制中常见的控制量)，速度信息。此外，如果车辆采用了避障算法，如三圆检测，轮廓膨胀等，需要实时将车辆的碰撞检测轮廓表示出来，效果会更加的直观。

1. Codes & Visuallization

codes overview

veh paras. setting

function Veh_CofigSetting(obj)
% set the veh_paras
obj.veh_cfg.Veh_L=4;
obj.veh_cfg.Veh_w=2;
obj.veh_cfg.rear_bumper_to_rear_axle = .7;   
obj.veh_cfg.front_bumper_to_front_axle = .7; 
obj.veh_cfg.wheel_base = 2.6;
obj.veh_cfg.half_width = 1;

% 轮胎参数(205/55R 16)
obj.veh_cfg.Tire_L=0.64;   % 轮胎直径(静止)
obj.veh_cfg.Tire_W=0.205;  % 轮胎宽度

% lf and lr are the front and rear
obj.veh_cfg.lf=1.4;
obj.veh_cfg.lr=1.2;
end

 veh vis.

function show_vehicle_rt(obj,todo_veh_sts,Fig_Num)

% Old style case
obj.plot_handle_list=[];

% veh pos shift
obj.vis_vehicle_sts=todo_veh_sts;

% veh_sts need to be row-vector
Veh_CofigSetting(obj);  set_color_matrix(obj)
Veh_Shape_Prototype(obj);
Veh_Shape_Shift(obj);

% preallo.
figure(Fig_Num)
for h=length(obj.shift_polyshape_list):-1:1
    if h==1  
        % plot the vehicle body
        cor_id=obj.veh_face_color_id; face_alpha=.0;    edge_alpha=1.0;   LineW=1.0;
    elseif h==9
        % veh part face
        cor_id=obj.veh_face_color_id; face_alpha=.3;    edge_alpha=0.0;   LineW=1.0;
    elseif h>=2 && h<=5
        cor_id=8; face_alpha=1.0;   edge_alpha=1.0;   LineW=2;
    elseif h>=6 && h<=8
        cor_id=8; face_alpha=0.0;   edge_alpha=1.0;   LineW=.4;
    end
    if obj.Vis_case_switch=="old_style" && h>=2; break; end
    cuur_veh_ele_handle=plot(obj.shift_polyshape_list(h,1), 'FaceColor', obj.color_List(cor_id,:),'FaceAlpha', face_alpha,'EdgeColor',obj.color_List(cor_id,:),'EdgeAlpha',edge_alpha,'LineWidth',LineW);
    % save the poly. handle
    obj.plot_handle_list=[obj.plot_handle_list;cuur_veh_ele_handle];
    hold on;
end 

if obj.Show_Colli_circle=="ON"; circle_handle_list=Plot_circle(obj,Fig_Num); obj.plot_handle_list=[obj.plot_handle_list;circle_handle_list];end

if obj.Basic_fig_infor_switch =="ON"
    axis equal;
    xlabel('Dir-X\it{/m}');ylabel('Dir-Y\it{/m}'); title("Vehicle visuallization demo. \it{\copyrightMMiL STU.}",'FontWeight','bold');set(gca,'FontName','times new roman','FontSize',12);
end

if obj.Veh_CoG_switch=="ON"; scatter_handle=scatter(todo_veh_sts(1,1),todo_veh_sts(1,2),60,'Marker','o','MarkerFaceColor',[1 0 0],'MarkerEdgeColor','none','MarkerFaceAlpha',1.0);hold on;
    obj.plot_handle_list=[obj.plot_handle_list;scatter_handle];
end

end

       这里构建了几个常见的可视化配色，见下：

      欢迎大家分享和讨论，一起进步。