suanhenbi02.m

汽车悬架运动学仿真/双横臂悬架/可以自由下载

clear;
clc;
m1=400;      %下臂长  参数
m2=600;      %上臂长  参数
b1=0;        %下臂夹角  参数
b2=0;        %上臂夹角  参数
fc=120;      %主销长度
cx=0;        %c点x坐标
fx=0;        %f点x坐标
px=0;        %p点x坐标
gx=0;        %g点x坐标
e=1;         %fp/pc
aa1=30 ;     %gp长度
aa2=(aa1^2+0.25*fc^2)^0.5;      %gc长度
aa3=aa2 ;     %gf长度
rr=300;      %轮胎半径
%%%%%%%%%%%%%%%%%%%%%%%%计算h点初始坐标
cy0=m1*cos(b1);
cz0=m1*sin(b1);
fy0=m1*cos(b1);
fz0=m1*sin(b1)+fc;
py0=cy0/(1+e)+e*fy0/(1+e);
pz0=cz0/(1+e)+e*fz0/(1+e);
% k10=(py0^2-cy0^2+pz0^2-cz0^2-aa1^2+aa2^2)/(2*(pz0-cz0));
% k20=(cy0-py0)/(pz0-cz0);
% ka0=1+k20^2;
% kb0=2*k10*k20-2*cy0-2*k20*cz0;
% kc0=k10^2+cy0^2+cz0^2-2*k10*cz0-aa2^2;
% gy0=(-kb0+(kb0^2-4*ka0*kc0)^0.5)/(2*ka0);
% gz0=k10+k20*gy0;
   k10=(fy0^2-cy0^2+fz0^2-cz0^2)/(2*(fz0-cz0));   
   k20=(cy0-fy0)/(fz0-cz0);
   aa0=1+k20^2;
   bb0=2*(-cy0+(k10-cz0)*k20);
   cc0=cy0^2+(k10-cz0)^2-aa2^2;
   gy0=(-bb0+(bb0^2-4*aa0*cc0)^0.5)/(2*aa0);
   gz0=k10+k20*gy0; 
% e20=gy0-py0;
% e30=gz0-pz0;
% e40=((1-e30^2)/(e20^2+e30^2))^0.5;
% hy0=gy0+e20*e30*e40*rr/(e20^2); 
% hz0=gz0-e40*rr;
   e10=(gz0-pz0)/(gy0-py0);
   hz0=gz0-rr/(1+e10^2)^0.5;
   hy0=gy0-e10*(hz0-gz0); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算横臂转角输入输出
%  hz00=m1*sin(b1)-(rr-0.5*fc);      %轮胎接地点初始z
%  hy00=m1*cos(b1)+aa1;      %轮胎接地点初始y
a1=zeros(500);
a2=zeros(500);
a3=zeros(500);
a4=zeros(500);
i=[1,0,0;0,1,0;0,0,1];
m=[0;m1*cos(b1)-m2*cos(b2);m1*sin(b1)-m2*sin(b2)+fc];  %车架矢量
a0=[0;m1*cos(b1);m1*sin(b1)];                          %下臂初始位置
b0=[0;m2*cos(b2);m2*sin(b2)];                          %上臂初始位置
p=[1;0;0];
q=[1;0;0];
q1=[0,0,0;0,0,-1;0,1,0];
k=0;
   for c1=-pi/4:pi/1800:pi/4;     %c1  下臂转角 
   zp=[1,0,0;0,cos(c1),-sin(c1);0,sin(c1),cos(c1)];
   ic1=(m-zp*a0)'*(i-q*q')*b0;
   jc1=(m-zp*a0)'*q1*b0;
   kc1=ic1+(m+b0)'*(zp-i)*a0;
   hc1=ic1^2+jc1^2-kc1^2;        
   c2=2*atan((jc1-hc1^0.5)/(ic1+kc1));    %c2 上臂转角
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算主要点的坐标
   cy=m1*cos(c1+b1);
   cz=m1*sin(c1+b1);
   fy=m1*cos(b1)-m2*cos(b2)+m2*cos(b2+c2);
   fz=m1*sin(b1)-m2*sin(b2)+m2*sin(b2+c2)+fc;
   py=cy/(1+e)+e*fy/(1+e);
   pz=cz/(1+e)+e*fz/(1+e);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%g点坐标
 %%  k1=(py^2-cy^2+pz^2-cz^2-aa1^2+aa2^2)/(2*(pz-cz));
%    k2=(cy-py)/(pz-cz);
%    ka=1+k2^2;
%    kb=2*k1*k2-2*cy-2*k2*cz;
%    kc=k1^2+cy^2+cz^2-2*k1*cz-aa2^2;
%    gy=(-kb+(kb^2-4*ka*kc)^0.5)/(2*ka);
%    gz=k1+k2*gy;
   k1=(fy^2-cy^2+fz^2-cz^2)/(2*(fz-cz));   
   k2=(cy-fy)/(fz-cz);
   aa=1+k2^2;
   bb=2*(-cy+(k1-cz)*k2);
   cc=cy^2+(k1-cz)^2-aa2^2;
   gy=(-bb+(bb^2-4*aa*cc)^0.5)/(2*aa);
   gz=k1+k2*gy; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%h点坐标
%    e2=gy-py;
%    e3=gz-pz;
%    e4=((1-e3^2)/(e2^2+e3^2))^0.5;
%    hy=gy+e2*e3*e4*rr/(e2^2);
%    hz=gz-e4*rr;
   e1=(gz-pz)/(gy-py);
   hz=gz-rr/(1+e1^2)^0.5;
   hy=gy-e1*(hz-gz); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%目标函数角度
   w=atan((gy-hy)/(gz-hz));   %目标函数角度,轮胎摆角
         if abs(hz-hz0)<=50;
            k=k+1;   
           a1(k)=hz-hz0;
           a2(k)=hy-hy0;
           a3(k)=w*180/pi;
           a4(k)=c1*180/pi;
         end
   end
plot(a2(1:k),a1(1:k),'r-',a3(1:k),a1(1:k),'r*')