MATLAB画小车

这里参考了robotarium库的代码

具体实现如下：

my_gritsbot_patch函数，完成小车的图形绘制，可修改相关参数完成小车样式改变。

function [ patch_data ] = my_gritsbot_patch()
%GRITSBOT_PATCH This is a helper function to generate patches for the
%simulated GRITSbots.  YOU SHOULD NEVER HAVE TO USE THIS FUNCTION.
%
% PATCH_DATA = GRITSBOT_PATCH() generates a struct containing patch data
% for a robot patch.

    % Make it facing 0 rads
    robot_width = 1;
    robot_height = 2; 
    wheel_width = 0.2; 
    wheel_height = 0.4; 
    led_size = 0.1; 
    
    % Helper functions to generate vertex coordinates for a centered
    % rectangle and a helper function to shift a rectangle.
    rectangle = @(w, h) [w/2 h/2 1; -w/2 h/2 1; -w/2 -h/2 1; w/2 -h/2 1];
    shift = @(r, x, y) r + repmat([x, y, 0], size(r, 1), 1);
    
    % Create vertices for body, wheel, and led.
    body = rectangle(robot_width, robot_height);
    wheel = rectangle(wheel_width, wheel_height);
    led = rectangle(led_size, led_size);
    
    % Use pre-generated vertices and shift them around to create a robot
    left_wheel_1 = shift(wheel, -(robot_width + wheel_width)/2, -robot_height/3);
    right_wheel_1 = shift(wheel, (robot_width + wheel_width)/2, -robot_height/3);
    left_wheel_2 = shift(wheel, -(robot_width + wheel_width)/2, robot_height/3);
    right_wheel_2 = shift(wheel, (robot_width + wheel_width)/2, robot_height/3);
    left_led = shift(led,  robot_width/4, robot_height/2 - 2*led_size);
    right_led = shift(led,  -robot_width/4, robot_height/2 - 2*led_size);
    
    % Putting all the robot vertices together
    vertices = [
     body ; 
     left_wheel_1; 
     left_wheel_2;
     right_wheel_1;
     right_wheel_2;
     left_led;
     right_led
    ];

    % Only color the body of the robot.  Everything else is black.
    colors = [
     [255, 0, 0]/255; 
     0 0 0;
     0 0 0;
     0 0 0;
     0 0 0;
     1 1 1;
     1 1 1
    ];

    % This seems weird, but it basically tells the patch function which
    % vertices to connect.
    faces = repmat([1 2 3 4 1], 7, 1);
    
    for i = 2:7
       faces(i, :) = faces(i, :) + (i-1)*4;
    end
    
   patch_data = []; 
   patch_data.vertices = vertices;
   patch_data.colors = colors;
   patch_data.faces = faces;
end

主函数：

axis([-1,1,-1,1]);
data = my_gritsbot_patch;
this.robot_body = data.vertices;
x  = 0;
y  = 0;
th = 0;
rotation_matrix = [
    cos(th) -sin(th) x;
    sin(th)  cos(th) y;
    0 0 1];
transformed = this.robot_body*rotation_matrix';
this.robot_handle{1} = patch(...
    'Vertices', transformed(:, 1:2), ...
    'Faces', data.faces, ...
    'FaceColor', 'flat', ...
    'FaceVertexCData', data.colors, ...
    'EdgeColor','none');

轨迹跟踪代码：

clear all;clc;

%% define constant
l=0.2;
R=1;
K=0.1;
delta_T=0.5;

%% define initialization
Pos=[0,12,3/2*pi]; %X,Y,Theta
T=linspace(0,2*pi,200);
desire_Pos=[16*power(sin(T),3);13*cos(T)-5*cos(2*T)-2*cos(3*T)-cos(4*T)];

%%  Graphics
f3=figure;
xlabel('x (m)')
ylabel('y (m)')
grid on

%%  robot dimensions
data = my_gritsbot_patch;
this.robot_body = data.vertices;
x  = Pos(1);
y  = Pos(2);
th = Pos(3)-pi/2;
rotation_matrix = [
    cos(th) -sin(th) x;
    sin(th)  cos(th) y;
    0 0 1];
transformed = this.robot_body*rotation_matrix';
this.robot_handle{1} = patch(...
'Vertices', transformed(:, 1:2), ...
'Faces', data.faces, ...
'FaceColor', 'flat', ...
'FaceVertexCData', data.colors, ...
'EdgeColor','none');

%% define controller


%% draw picture
h= animatedline('color','r','LineStyle','--');
h_car= animatedline('color','b');
h_car_model = animatedline('Marker','o','MaximumNumPoints',1);     %只显示1个新的点
axis([-20 20 -20 15])

for k = 1:length(T)
    addpoints(h_car,Pos(1),Pos(2));
    addpoints(h_car_model,Pos(1),Pos(2));
    
    e_x=Pos(1)-desire_Pos(1,k);
    e_y=Pos(2)-desire_Pos(2,k);
    u_x=-K*e_x;
    u_y=-K*e_y;
    A_mat=[cos(Pos(3)) -l*sin(Pos(3));sin(Pos(3)) l*cos(Pos(3))];
    P=[cos(Pos(3)) 0;sin(Pos(3)) 0;0 1];
    U_mat=[u_x,u_y]';
    v_w_mat=A_mat\U_mat;
    Pos(1)=Pos(1)+v_w_mat(1)*cos(Pos(3))-v_w_mat(2)*l*sin(Pos(3));
    Pos(2)=Pos(2)+v_w_mat(1)*sin(Pos(3))+v_w_mat(2)*l*cos(Pos(3));
    
    Pos(3)=Pos(3)+v_w_mat(2)*delta_T;
   
    x  = Pos(1);
    y  = Pos(2);
    th = Pos(3)-pi/2;
    rotation_matrix = [
        cos(th) -sin(th) x;
        sin(th)  cos(th) y;
        0 0 1];
    transformed = this.robot_body*rotation_matrix';
    set(this.robot_handle{1}, 'Vertices', transformed(:, 1:2));
    

    addpoints(h,desire_Pos(1,k),desire_Pos(2,k));
    

    frame=getframe(gcf);
    im = frame2im(frame); 
    [imind,cm] = rgb2ind(im,256);
    if k==1
         imwrite(imind,cm,'experiment.gif','gif', 'LoopCount',inf,'DelayTime',0.000001);
    end
    if rem(k,2)==0
         imwrite(imind,cm,'experiment.gif','gif','WriteMode','append','DelayTime',0.000001);
    end
    drawnow
end