clear %清除工作区变量
clc %清除命令行历史窗口
close all
%尺寸说明见《8字组国赛报告》的附录
%输出参数的尺寸单位都为m
%参数输入
n=400; %高度的细分程度,
%重物下降的高度h
h=linspace(0,0.1,n);%这里把高度0.1m分成n份
%驱动轴传动比ii
% ii=18/68;%原传动比,分子和分母分别对应主动动轮的齿数,模为0.5mm
% ii=26/60;
ii=42/130;
%后轮半径
% R=0.150/2;
R=0.110/2;
%驱动轮A与转向轮横向偏距a1
a1=0.0465;
%驱动轮B与转向轮横向偏距a2
a2=0.0465;
%曲柄半径r1,
% r1=0.03;%原曲柄半径
r1=0.0255;
%绳轮半径r2
r2=0.004;
%驱动轴与转向轮的距离d
d=0.113;
%转向杆的长c
c=0.0358;
%连杆长l
% l=0.1107;
l=0.071;
%转向轮的初始偏角p0
p0=-9.43/180*pi;
%驱动轴转过的角度sd1
sd1=h/r2;
%空间曲柄连杆机构的模型,输入的角度为sd1,输出的角度为af,
%原理见论文《8字组国赛报告》
x0=sqrt(l^2-r1^2);
C=-(r1^2-l^2+2*c^2+x0^2-2*x0*r1*cos(sd1));
A=2*r1*c.*cos(sd1)-2*x0*c;
B=2*c^2;
%转向轮转过的角度af
af=asin(C./sqrt(A.^2+B^2))-acos(A./sqrt(A.^2+B^2))+pi+p0;
%计算三个轮子的轨迹,原理见《8字组国赛报告》
format long %改变数值显示的位数
%轨迹曲率半径rou
rou=d./(tan(af))+a1;
%驱动轮转过的角度sd2
sd2=sd1/ii;
%小车行驶的路程s
s=sd2*R;
ds=s(2)-s(1); %小车的速度ds,即路程的差分,相当于连续轨迹的微分
dbd=ds./(rou); %小车行驶过程中的方位角的变化率dbd
bd=cumsum(dbd);%小车方位角bd,对方位角的变化dbd进行累加,即积分
dx=-ds*sin(bd);%小车x方向上的位移dx
dy=ds*cos(bd); %小车y方向上的位移y
x=cumsum(dx); %小车x方向的位移x
y=cumsum(dy); %小车y方向的位移y
a=-pi/90; %小车初始方位角a,即小车发车时的角度
%小车最终x方向的位移x
x=x*cos(a)-y*sin(a)+0.313;
%小车最终y方向的位移y
y=x*sin(a)+y*cos(a)+0.005;
%左轮的轨迹
xb=x-(a1+a2).*cos(bd);
yb=y-(a1+a2).*sin(bd);
%右轮的轨迹
% xc=x-(a1-0.01)*cos(bd)-(d-0.1)*sin(bd);
% yc=y-(a1-0.01)*sin(bd)+(d-0.1)*cos(bd);
xc=x-(a1)*cos(bd)-(d)*sin(bd);
yc=y-(a1)*sin(bd)+(d)*cos(bd);
%画出三个轮的轨迹
% plot(x*1000,y*1000,'k',xb*1000,yb*1000,'k',xc*1000,yc*1000,'k');%这里的单位为mm
% grid on %给图加上网格线
% set(gca,'DataAspectRatio',[1 1 1])%令x,y坐标轴的比例为一比一
% set(gca,'xtick',-200:100:400);%x轴网格线的具体范围
% set(gca,'ytick',-700:100:740);%y轴网格线的具体范围
x=x*1000;
y=y*1000;
xb=xb*1000;
yb=yb*1000;
xc=xc*1000;
yc=yc*1000;
figure
set(gcf,'Unit','normalized','Position',[0,0,1,0.9])
%展示动态曲线
h2 = plot(0,0);
hold all
h = plot(0,0);
h1 = plot(0,0);
grid on
axis equal
xlabel('X坐标/mm');
ylabel('Y坐标/mm');
title('小车轨迹动态曲线');
% axis([-60000 60000 -60000 60000 -60000 60000])
for m = 1:numel(x)
set(h2,'color','blue');
set(h,'color','black');
set(h1,'color','black');
% set(h,'XData',x(1:m),'XData',xb(1:m),'YData',y(1:m),'YData',yb(1:m));
set(h2,'XData',yc(1:m),'YData',xc(1:m));
set(h,'XData',y(1:m),'YData',x(1:m));
set(h1,'XData',yb(1:m),'YData',xb(1:m));
drawnow;%刷新屏幕
pause(0.02);
end
legend('前轮','左后轮','右后轮');
MATLAB无碳小车轨迹代码.rar
需积分: 45 153 浏览量
2020-05-05
09:30:14
上传
评论 41
收藏 4KB RAR 举报
求知小菜鸟
- 粉丝: 1805
- 资源: 28