差分小车模型、单轨模型-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