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');
小车运动请参考:MATLAB 轮式机器人轨迹跟踪仿真
轨迹跟踪代码:
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