差分小车模型、单轨模型-matlab画图实现

记得点赞哈。

一、差分小车模型（非完整运动学约束）

1.1 椭圆外部轮子

 代码1

robot_draw(2,6,1,3.1415926/3,'b')
axis equal;

function robot_draw(x,y,r,theta,color)
% x y圆心坐标，r圆半径，theta小车行驶角度，color小车颜色
% 调用示例： robot_draw(4,7,1,3.1415926/5,'b')

%     figure('visible','off');
    rectangle('Position',[x-r,y-r,2*r,2*r],'Curvature',[1,1],'LineWidth',1.5,'EdgeColor',color);
    hold on;
    x1 = x+1.2*r*cos(theta);
    y1 = y+1.2*r*sin(theta);
    line([x,x1],[y,y1],'Color',color,'LineWidth',1.5)
    hold on;
    ecc = axes2ecc(r/2,r/8);
    wheel_left = [x+9*r/8*cos(theta+3.1415926/2), y+9*r/8*sin(theta+3.1415926/2)];
    [elat,elon]=ellipse1(wheel_left(1),wheel_left(2),[r/2 ecc],rad2deg(theta));
    line(elat,elon,'Color',color,'LineWidth',1.2);
    hold on;
    wheel_right = [x+9*r/8*cos(theta-3.1415926/2), y+9*r/8*sin(theta-3.1415926/2)];
    [elat,elon]=ellipse1(wheel_right(1),wheel_right(2),[r/2 ecc],rad2deg(theta));
    line(elat,elon,'Color',color,'LineWidth',1.2);
end

 1.2 长方形内部轮子

 代码2

robot_draw(2,6,1,3.1415926/3,'b')
axis equal;

function robot_draw(x,y,r,theta,color)
% x y圆心坐标，r圆半径，theta小车行驶角度，color小车颜色
% 调用示例： robot_draw(4,7,1,3.1415926/5,'b')
    rectangle('Position',[x-r,y-r,2*r,2*r],'Curvature',[1,1],'LineWidth',1.5,'EdgeColor',color);
    hold on;
    x1 = x+1.2*r*cos(theta);
    y1 = y+1.2*r*sin(theta);
    line([x,x1],[y,y1],'Color','m','LineWidth',1.5)
    hold on;
    L_num = 3*r/5;
    W_num = r/6;
    L_wheel = r-2*W_num;
    %左轮
    angle_left = theta-3.1415926/2;
    rot_mat_left = [cos(-angle_left), sin(-angle_left); -sin(-angle_left), cos(-angle_left)];
    left_up  = rot_mat_left*[-W_num/2-L_wheel;L_num*0.5] + [x;y];
    left_dw  = rot_mat_left*[-W_num/2-L_wheel;-L_num*0.5] + [x;y];
    right_up = rot_mat_left*[W_num/2-L_wheel;L_num*0.5] + [x;y];
    right_dw = rot_mat_left*[W_num/2-L_wheel;-L_num*0.5] + [x;y];
    hold on;
    line([left_up(1),right_up(1),right_dw(1),left_dw(1),left_up(1)],[left_up(2),right_up(2),right_dw(2),left_dw(2),left_up(2)],'Color',color,'LineWidth',1.5);
    hold on;
    %右轮
    angle_righ = theta+3.1415926/2;
    rot_mat_righ = [cos(-angle_righ), sin(-angle_righ); -sin(-angle_righ), cos(-angle_righ)];
    left_up  = rot_mat_righ*[-W_num/2-L_wheel;L_num*0.5] + [x;y];
    left_dw  = rot_mat_righ*[-W_num/2-L_wheel;-L_num*0.5] + [x;y];
    right_up = rot_mat_righ*[W_num/2-L_wheel;L_num*0.5] + [x;y];
    right_dw = rot_mat_righ*[W_num/2-L_wheel;-L_num*0.5] + [x;y];
    hold on;
    line([left_up(1),right_up(1),right_dw(1),left_dw(1),left_up(1)],[left_up(2),right_up(2),right_dw(2),left_dw(2),left_up(2)],'Color',color,'LineWidth',1.5);
end

 二、单轨运动学模型

 代码

robot_draw(4,7,1,3.1415926/6,-3.1415926/5,'b')
axis equal;


function h=robot_draw(x,y,r,theta,w,color)
% x y圆心坐标，r圆半径，theta小车行驶角度，w方向盘角度,color小车颜色
% 调用示例： robot_draw(4,7,1,3.1415926/5,'b')
    theta_n = theta+w;
    
    l_car = 0.8*r;
    rot_mat1 = [cos(-theta), sin(-theta); -sin(-theta), cos(-theta)];
    p1 = rot_mat1*[l_car; 0] + [x;y];
    p2 = rot_mat1*[-l_car;0] + [x;y];
    hold on;
    h1 = line([p1(1),p2(1)],[p1(2),p2(2)],'Color',color,'LineWidth',1.5);
    
    %车前把手
    angle = theta_n-3.1415926/2;
    w_car = 3*r/8;
    rot_mat2 = [cos(-angle), sin(-angle); -sin(-angle), cos(-angle)];
    p3 = rot_mat2*[-w_car; 0] + p1;
    p4 = rot_mat2*[ w_car; 0] + p1;
    hold on;
    h2 = line([p3(1),p4(1)],[p3(2),p4(2)],'Color',color,'LineWidth',1.5);
    
    %后轮
    L_num = 3*r/5;
    W_num = r/6;
    L_wheel = r-2*W_num;
    angle_left = theta-3.1415926/2;
    rot_mat_left = [cos(-angle_left), sin(-angle_left); -sin(-angle_left), cos(-angle_left)];
    left_up  = rot_mat_left*[-W_num/2; L_num*0.5-L_wheel] + [x;y];
    left_dw  = rot_mat_left*[-W_num/2;-L_num*0.5-L_wheel] + [x;y];
    right_up = rot_mat_left*[ W_num/2; L_num*0.5-L_wheel] + [x;y];
    right_dw = rot_mat_left*[ W_num/2;-L_num*0.5-L_wheel] + [x;y];
    hold on;
    h3 = line([left_up(1),right_up(1),right_dw(1),left_dw(1),left_up(1)],[left_up(2),right_up(2),right_dw(2),left_dw(2),left_up(2)],'Color',color,'LineWidth',1.5);
    
    %前轮
    L_num = 2*r/5;
    angle_righ = theta_n+3.1415926/2;
    rot_mat_righ = [cos(-angle_righ), sin(-angle_righ); -sin(-angle_righ), cos(-angle_righ)];
    left_up  = rot_mat_righ*[-W_num/2; L_num*0.5] + p1;
    left_dw  = rot_mat_righ*[-W_num/2;-L_num*0.5] + p1;
    right_up = rot_mat_righ*[ W_num/2; L_num*0.5] + p1;
    right_dw = rot_mat_righ*[ W_num/2;-L_num*0.5] + p1;
    hold on;
    h4 = line([left_up(1),right_up(1),right_dw(1),left_dw(1),left_up(1)],[left_up(2),right_up(2),right_dw(2),left_dw(2),left_up(2)],'Color',color,'LineWidth',1.5);
    h = [h1,h2,h3,h4];
end

 理论学习参考 【汽车】【控制】01.01 车辆运动学模型到动力学模型_哔哩哔哩_bilibili