asteprgktresolve.m

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

% 变步长四阶龙格-库塔法对一阶微分方程组积分一步。耗时间最多
function AStepRGKTResolve 

% 声明全局变量
GlobalVariables

p=0;
hh=h;
n1=1;
pp=1+toleps;
x=t;
p(1:rotFreeNum)=0;
% 记录初始值c1-速度大小,cv-平动大小，cm-转动方向余弦矩阵，zhzh--转轴的方向
c1=wch;     cv=rch1;    cm=rch2;

while pp>=toleps
	a(1)=hh/2.0;	a(2)=a(1);	a(3)=hh;	a(4)=hh;
	g1=wch;	  % 上以次的迭代结果，用来和下以次的结果作比较
	wch=c1;   % 恢复初始值
	rch1=cv;
    rch2=cm;

    MotionStation; %恢复系统的状态
    
	dt=h/n1;
	t=x;
 	d1(1:allFreeNum)=0;  
	jj=0;
	for j=1:n1
		FormAcceEquations;
        % 记录下一次微分方程的初值  % 新的状态值
		e1=wch;     b1=wch;     
        ev=rch1;    bv=rch1;
        em=rch2;    bm=rch2;
        ii0=0;      iv=0;       im=0;       ip=0;
		p0(1:rotFreeNum)=0;           
		for i=1:nBodies
			if joint(i).trfree==1
				im=im+1;
				ii0=ii0+1;                
				bv(im).a(1)=bv(im).a(1)+hh*wch(ii0);
				rch10(im).a(1)=rch1(im).a(1)+hh*wch(ii0)/2.0;
			elseif joint(i).trfree==3
				im=im+1;
				bv(im).a(1:3)=bv(im).a(1:3)+hh*wch(ii0+(1:3));
				rch10(im).a(1:3)=rch1(im).a(1:3)+hh*wch(ii0+(1:3))/2.0;
				ii0=ii0+3;
			end
			if joint(i).rofree~=0
				p0(ip+(1:joint(i).rofree))=p0(ip+(1:joint(i).rofree))+hh*wch(ii0+(1:joint(i).rofree));
				p(ip+(1:joint(i).rofree))=p(ip+(1:joint(i).rofree))+hh*wch(ii0+(1:joint(i).rofree))/2.0;
				ii0=ii0+joint(i).rofree;
				ip=ip+joint(i).rofree;
			end
		end		
		for k=1:3
			ii0=0;      iv=0;       im=0;       iz=0;       ip=0;
			if k==1         % k等于一开始
				wch=e1(1:allFreeNum)+hh*d1(1:allFreeNum)/2.0; % 为求微分方程作准备!下一状态的累计
				b1=b1(1:allFreeNum)+hh*d1(1:allFreeNum)/6.0;  
				for i=1:nBodies
					if 	joint(i).trfree==1
						ii0=ii0+1;
						iv=iv+1;
						rch1(iv).a(1)=rch10(iv).a(1);
						rch10(iv).a(1)=rch10(iv).a(1)+hh*hh*d1(ii0)/4.0;
						bv(iv).a(1)=bv(iv).a(1)+hh*hh*d1(ii0)/6.0;
					elseif joint(i).trfree==3
						iv=iv+1;
						rch1(iv).a(1:3)=rch10(iv).a(1:3);
						rch10(iv).a(1:3)=rch10(iv).a(1:3)+hh*hh*d1(ii0+(1:3))/4.0;
						bv(iv).a(1:3)=bv(iv).a(1:3)+hh*hh*d1(ii0+(1:3))/6.0;
						ii0=ii0+3;
					end
					if joint(i).rofree~=0
						im=im+1;
						joint(i).wi(1:joint(i).rofree)=p(ip+(1:joint(i).rofree));
						p(ip+(1:joint(i).rofree))=p(ip+(1:joint(i).rofree))+hh*hh*d1(ii0+(1:joint(i).rofree))/4.0;
						p0(ip+(1:joint(i).rofree))=p0(ip+(1:joint(i).rofree))+hh*hh*d1(ii0+(1:joint(i).rofree))/6.0;
						ii0=ii0+joint(i).rofree;
						ip=ip+joint(i).rofree;
						[a1,p1,p2,p3,p4]=RotateC(joint(i).m(1),joint(i).m(2),joint(i).m(3),joint(i).wi(1),joint(i).wi(2),joint(i).wi(3));
						if joint(i).rofree==1
							iz=iz+1;
							zhzh(iz).a=p1*em(im).a';
						elseif joint(i).rofree==2
							zhzh(iz+1).a=p1*em(im).a';
							zhzh(iz+2).a=p2*em(im).a';   
							iz=iz+2;
						elseif joint(i).rofree==3
							zhzh(iz+1).a=p1*em(im).a';
							zhzh(iz+2).a=p2*em(im).a';   
							zhzh(iz+3).a=p3*em(im).a';           
							iz=iz+3;
						end 
						rch2(im).a=em(im).a*a1;
					end
				end
				MotionStation;
				FormAcceEquations;
			end             %       k等于一结束

			if k==2			%       k等于二开始	              
				wch=e1(1:allFreeNum)+hh*d1(1:allFreeNum); 
				b1=b1(1:allFreeNum)+hh*d1(1:allFreeNum)/3.0;
				for i=1:nBodies
					if 	joint(i).trfree==1
						ii0=ii0+1;
						iv=iv+1;
						rch1(iv).a(1)=rch10(iv).a(1);
						rch10(iv).a(1)=ev(iv).a(1)+hh*hh*d1(ii0)/2.0+hh*e1(ii0);
						bv(iv).a(1)=bv(iv).a(1)+hh*hh*d1(ii0)/6.0;
					elseif joint(i).trfree==3
						iv=iv+1;
						rch1(iv).a(1:3)=rch10(iv).a(1:3);
						rch10(iv).a(1:3)=ev(iv).a(1:3)+hh*hh*d1(ii0+(1:3))/2.0+hh*e1(ii0+(1:3));
						bv(iv).a(1:3)=bv(iv).a(1:3)+hh*hh*d1(ii0+(1:3))/6.0;
						ii0=ii0+3;
					end
                 	if joint(i).rofree~=0
						im=im+1;
						joint(i).wi(1:joint(i).rofree)=p(ip+(1:joint(i).rofree));
						p(ip+(1:joint(i).rofree))=hh*hh*d1(ii0+(1:joint(i).rofree))/2.0+hh*e1(ii0+(1:joint(i).rofree));
						p0(ip+(1:joint(i).rofree))=p0(ip+(1:joint(i).rofree))+hh*hh*d1(ii0+(1:joint(i).rofree))/6.0;
						ii0=ii0+joint(i).rofree;
						ip=ip+joint(i).rofree;
						[a1,p1,p2,p3,p4]=RotateC(joint(i).m(1),joint(i).m(2),joint(i).m(3),joint(i).wi(1),joint(i).wi(2),joint(i).wi(3));
						if joint(i).rofree==1
							iz=iz+1;
							zhzh(iz).a=p1*em(im).a';
						elseif joint(i).rofree==2
							zhzh(iz+1).a=p1*em(im).a';
							zhzh(iz+2).a=p2*em(im).a';    
							iz=iz+2;
                        elseif joint(i).rofree==3
							zhzh(iz+1).a=p1*em(im).a';
							zhzh(iz+2).a=p2*em(im).a';    
							zhzh(iz+3).a=p3*em(im).a';            
							iz=iz+3;
						end 
						rch2(im).a=em(im).a*a1;
					end                    
				end	
				MotionStation;
				FormAcceEquations;
			end             %      k等于二结束

			if k==3         %      k等于三开始 
                wch=e1(1:allFreeNum)+hh*d1(1:allFreeNum);  
				b1=b1(1:allFreeNum)+hh*d1(1:allFreeNum)/3.0; 

				for i=1:nBodies
					if 	joint(i).trfree==1
						ii0=ii0+1;
						iv=iv+1;
						rch1(iv).a(1)=rch10(iv).a(1);
						bv(iv).a(1)=bv(iv).a(1)+hh*hh*d1(ii0)/6.0;
					elseif joint(i).trfree==3
						iv=iv+1;
						rch1(iv).a(1:3)=rch10(iv).a(1:3);
						bv(iv).a(1:3)=bv(iv).a(1:3)+hh*hh*d1(ii0+(1:3))/6.0;
						ii0=ii0+3;
					end
					if joint(i).rofree~=0
						im=im+1;
						joint(i).wi(1:joint(i).rofree)=p(ip+(1:joint(i).rofree));
						p0(ip+(1:joint(i).rofree))=p0(ip+(1:joint(i).rofree))+hh*hh*d1(ii0+(1:joint(i).rofree))/6.0;
						ii0=ii0+joint(i).rofree;
						ip=ip+joint(i).rofree;
						[a1,p1,p2,p3,p4]=RotateC(joint(i).m(1),joint(i).m(2),joint(i).m(3),joint(i).wi(1),joint(i).wi(2),joint(i).wi(3));
						if joint(i).rofree==1
							iz=iz+1;
							zhzh(iz).a=p1*em(im).a';
						elseif joint(i).rofree==2
							zhzh(iz+1).a=p1*em(im).a';
							zhzh(iz+2).a=p2*em(im).a';   
							iz=iz+2;
						elseif joint(i).rofree==3
							zhzh(iz+1).a=p1*em(im).a';
							zhzh(iz+2).a=p2*em(im).a';   
							zhzh(iz+3).a=p3*em(im).a';            
							iz=iz+3;
						end 
						rch2(im).a=em(im).a*a1;
					end
				end
				MotionStation;
				FormAcceEquations;
			end             %      k等于三结束
		end	
		tt=t+a(k);
		wch=b1(1:allFreeNum)+hh*d1(1:allFreeNum)/6.0;
     	rch1=bv;
		im=0;   ip=0;   iz=0;   ii0=0;
		for i=1:nBodies
			if joint(i).rofree~=0
				im=im+1;
				joint(i).wi(1:joint(i).rofree)=p0(ip+(1:joint(i).rofree));
				ip=ip+joint(i).rofree;
				[a1,p1,p2,p3,p4]=RotateC(joint(i).m(1),joint(i).m(2),joint(i).m(3),joint(i).wi(1),joint(i).wi(2),joint(i).wi(3));
				if joint(i).rofree==1
					iz=iz+1;
					zhzh(iz).a=p1*em(im).a';
                elseif joint(i).rofree==2
					iz=iz+2;                    
					zhzh(iz+1).a=p2*em(im).a';
					zhzh(iz+2).a=p2*em(im).a';    
                elseif joint(i).rofree==3
					zhzh(iz+1).a=p1*em(im).a';
					zhzh(iz+2).a=p2*em(im).a';    
					zhzh(iz+3).a=p3*em(im).a';            
					iz=iz+3;
				end 
				rch2(im).a=em(im).a*a1;
			end
		end
		MotionStation;
		t=t+dt;
	end
	pp=0.0;
	for i=1:allFreeNum
		q1=abs(wch(i)-g1(i));
		if q1>=pp
            pp=q1;
		end
	end
	hh=hh/2.0;
	n1=n1+n1;
end
t=x;