adamsdiffequation.asv

用于计算多体系统动力学的程序,可用于多体系统动力学的计算和仿镇,也可用于机器人的设计和计算

% ADAMS求解一阶微分方城组
function AdamsDiffEquation

% 声明全局变量
Global Variables

a(1:3,1:3)=0;   
a1=t;
ik=1;
for i=1:allFreeNum 
    zw(i,1)=wch(i); 
end
for i=1:transJointsNum 
    zrv(i,1).a=rch1(i).a; 
end
for i=1:rotJointsNum 
    zrm(i,1).a=rch2(i).a; 
end

MotionStation;
ll0=0;
FormAcceEquations;

% 记录输出位置和速度
for ii=1:nBodies
    Bd(ii).Rs(str2num(int2str(t/h))+1).aD=[t,body(ii).r,joint(ii).v,joint(ii).w];  
end

b1(1,1:allFreeNum)=d1(1:allFreeNum);	% 速度微分
b2(1,1:allFreeNum)=wch(1:allFreeNum);   % 位置微分

% 求第二时刻到第四时刻的状态
for i=2:4   
	if i<=nSteps 
		t=a1+(i-1)*h;
        % RGKT法求解积分4次        
 		AStepRGKTResolve;             
    	ll0=1;
		for k=1:allFreeNum
			zw(k,i)=wch(k);
		end
		kv=0;km=0;
		for k=1:nBodies
			if joint(k).trfree~=0
				kv=kv+1;
				zrv(kv,i).a=rch1(kv).a;
			end
			if joint(k).rofree~=0
				km=km+1;
				zrm(km,i).a=rch2(km).a;
			end
		end
		% 存入输出数据
		for ii=1:nBodies
            Bd(ii).Rs(str2num(int2str(t/h))+1).aD=[t,body(ii).r,joint(ii).v,joint(ii).w];  
		end
        
		MotionStation;
		FormAcceEquations; 
        b1(i,1:allFreeNum)=d1(1:allFreeNum);	% 速度微分
        b2(i,1:allFreeNum)=wch(1:allFreeNum);   % 位置微分
    end
end

% 求第五时刻到第nSteps时刻的状态	
for i=5:nSteps    
	jj0=0;jm=0;jv=0;jz=0;
    % 预估
	for j=1:nBodies  
        % 平动速度和平动位移 
		if joint(j).trfree~=0
			jv=jv+1;
			rch1(jv).a=0;           
			for jj1=1:joint(j).trfree    
				wch(jj0+jj1)=zw(jj0+jj1,i-1)+h*(55*b1(4,jj0+jj1)-59*b1(3,jj0+jj1)+37*b1(2,jj0+jj1)-9*b1(1,jj0+jj1))/24;
				rch1(jv).a(jj1)=zrv(jv,i-1).a(jj1)+h*(55*b2(4,jj0+jj1)-59*b2(3,jj0+jj1)+37*b2(2,jj0+jj1)-9*b2(1,jj0+jj1))/24;
            end
			jj0=jj0+joint(j).trfree;
		end  
        % 转动速度和方向余弦矩阵和转轴方向
		if joint(j).rofree~=0
			jm=jm+1;
			joint(j).wi(1:3)=0;
			for jj1=1:joint(j).rofree
				wch(jj0+jj1)=zw(jj0+jj1,i-1)+h*(55*b1(4,jj0+jj1)-59*b1(3,jj0+jj1)+37*b1(2,jj0+jj1)-9*b1(1,jj0+jj1))/24;
				joint(j).wi(jj1)=h*(55*b2(4,jj0+jj1)-59*b2(3,jj0+jj1)+37*b2(2,jj0+jj1)-9*b2(1,jj0+jj1))/24;
            end
			[a,p1,p2,p3,p4]=RotateC(joint(j).m(1),joint(j).m(2),joint(j).m(3),joint(j).wi(1),joint(j).wi(2),joint(j).wi(3));
			if joint(j).rofree==1
				jz=jz+1;
				zhzh(jz).a=p1*zrm(jm,i-1).a';
			elseif joint(j).rofree==2
				jz=jz+2;                
				zhzh(jz+1).a=p1*zrm(jm,i-1).a';
				zhzh(jz+2).a=p2*zrm(jm,i-1).a';   
			elseif joint(j).rofree==3
				jz=jz+3;                
				zhzh(jz+1).a=p1*zrm(jm,i-1).a';
				zhzh(jz+2).a=p2*zrm(jm,i-1).a';    
				zhzh(jz+3).a=p3*zrm(jm,i-1).a';            
			end 
			rch2(jm).a=zrm(jm,i-1).a*a;
			jj0=jj0+joint(j).rofree;
		end
	end 
	t=a1+(i-1)*h;
	b1(1,1:allFreeNum)=b1(2,1:allFreeNum);	b1(2,1:allFreeNum)=b1(3,1:allFreeNum);	b1(3,1:allFreeNum)=b1(4,1:allFreeNum);
    b2(1,1:allFreeNum)=b2(2,1:allFreeNum);	b2(2,1:allFreeNum)=b2(3,1:allFreeNum);	b2(3,1:allFreeNum)=b2(4,1:allFreeNum);

	MotionStation;
	FormAcceEquations;
	% 调用FormAcceEquations求微分,做运动学和动力学分析时,只需要相对前一速度和位置的变化,
	% 否则无法产生FormAcceEquations,它不是时间的统一函数,必须保留前一次的变量,如平动位移,
	% 转动方位变化.这样在调FormAcceEquations只需知道和变化时有关的增量.

    b1(4,1:allFreeNum)=d1(1:allFreeNum);
	b2(4,1:allFreeNum)=wch(1:allFreeNum);

	% 校正
	jj0=0;jv=0;jm=0;jz=0;
	for j=1:nBodies
		if joint(j).trfree~=0
			jv=jv+1;
			for jj=1:joint(j).trfree
				zw(jj0+jj,i)=zw(jj0+jj,i-1)+h*(9*b1(4,jj+jj0)+19*b1(3,jj+jj0)-5*b1(2,jj+jj0)+b1(1,jj+jj0))/24;
				zrv(jv,i).a(jj)=zrv(jv,i-1).a(jj)+h*(9*b2(4,jj+jj0)+19*b2(3,jj+jj0)-5*b2(2,jj+jj0)+b2(1,jj+jj0))/24;                
				rch1(jv).a(jj)=zrv(jv,i).a(jj);
				wch(jj0+jj)=zw(jj+jj0,i);
			end
			jj0=jj0+joint(j).trfree;
		end 
		if joint(j).rofree~=0
			jm=jm+1;
            joint(j).wi(1:3)=0;
			for jj=1:joint(j).rofree
				zw(jj0+jj,i)=zw(jj0+jj,i-1)+h*(9*b1(4,jj+jj0)+19*b1(3,jj+jj0)-5*b1(2,jj+jj0)+b1(1,jj+jj0))/24;
				joint(j).wi(jj)=h*(9*b2(4,jj+jj0)+19*b2(3,jj+jj0)-5*b2(2,jj+jj0)+b2(1,jj+jj0))/24;
				wch(jj0+jj)=zw(jj+jj0,i);                
			end
			[a,p1,p2,p3,p4]=RotateC(joint(j).m(1),joint(j).m(2),joint(j).m(3),joint(j).wi(1),joint(j).wi(2),joint(j).wi(3));
			if joint(j).rofree==1
				jz=jz+1;
				zhzh(jz).a=p1*zrm(jm,i-1).a';
			elseif joint(j).rofree==2
				jz=jz+2;                
				zhzh(jz+1).a=p1*zrm(jm,i-1).a';
				zhzh(jz+2).a=p2*zrm(jm,i-1).a';   
            elseif joint(j).rofree==3
				jz=jz+3;                
				zhzh(jz+1).a=p1*zrm(jm,i-1).a';
				zhzh(jz+2).a=p2*zrm(jm,i-1).a';    
				zhzh(jz+3).a=p3*zrm(jm,i-1).a';            
			end 
			jj0=jj0+joint(j).rofree;
			zrm(jm,i).a=zrm(jm,i-1).a*a;
			rch2(jm).a=zrm(jm,i).a;
		end         
	end
	% 存入输出数据
	for ii=1:nBodies
        Bd(ii).Rs(str2num(int2str(t/h))+1).aD=[t,body(ii).r,joint(ii).v,joint(ii).w];  
	end
    
    MotionStation;
	FormAcceEquations; 
	b1(4,1:allFreeNum)=d1(1:allFreeNum);
	b2(4,1:allFreeNum)=wch(1:allFreeNum);   
end
t=a1;