利用matlab实现NUBRS曲线曲面插补仿真

clear; U=input('input U:') X=input('please input X:') Y=input('please input Y:') Z=input('please input Z:') W=input('please input W:') Uz=U;%读取NURBS参数:节点矢量，控制顶点，权因子 Xz=X; Yz=Y; Zz=Z; Wz=W; Uf=fliplr(1-U);%读取NURBS参数:节点矢量，控制顶点，权因子（反向） Xf=fliplr(X); Yf=fliplr(Y); Zf=fliplr(Z); Wf=fliplr(W); T=0.001;hm=0.001;am=4900;F=18*1000/60;A=2400;tag=1; J=48000;fs=0;T1=A/J;T3=T1;f1=0.5*J*T1*T1;T2=(F-fs-2*f1)/A;f2=f1+A*T2;all=T1/T+T2/T+T3/T+1;e=0; for n=1:T1/T %加减速相关参数预处理、反向插补预测减速点 L0(n)=1/6*J*((n*T)^3-((n-1)*T)^3) end for n=1:T2/T L0(n+T1/T)=f1*T+0.5*A*((n*T)^2-((n-1)*T)^2) end for n=1:T3/T L0(n+T1/T+T2/T)=f2*T+0.5*A*((n*T)^2-((n-1)*T)^2)-1/6*J*((n*T)^3-((n-1)*T)^3) end L0(all)=0.3 for k=1:2 if k==1 U=Uf;X=Xf;Y=Yf;Z=Zf;W=Wf; else U=Uz;X=Xz;Y=Yz;Z=Zz;W=Wz; end for i=4:length(W)%插补预处理 M{i-3}(4,4)=0;M{i-3}(3,4)=0;M{i-3}(2,4)=0; M{i-3}(1,1)=-(U(i+1)-U(i)).^2./(U(i+1)-U(i-1))./(U(i+1)-U(i-2)); M{i-3}(1,4)=(U(i+1)-U(i)).^2./(U(i+2)-U(i))./(U(i+3)-U(i)); M{i-3}(2,3)=3.*(U(i+1)-U(i)).^2./(U(i+1)-U(i-1))./(U(i+2)-U(i-1)); M{i-3}(3,3)=3.*(U(i+1)-U(i)).*(U(i)-U(i-1))./(U(i+1)-U(i-1))./(U(i+2)-U(i-1)); M{i-3}(4,3)=(U(i)-U(i-1)).^2./(U(i+1)-U(i-1))./(U(i+2)-U(i-1)); M{i-3}(1,3)=-M{i-3}(2,3)./3-M{i-3}(1,4)-(U(i+1)-U(i)).^2./(U(i+2)-U(i))./(U(i+2)-U(i-1)); M{i-3}(1,2)=-M{i-3}(1,1)-M{i-3}(1,3)-M{i-3}(1,4); M{i-3}(2,1)=-3.*M{i-3}(1,1); M{i-3}(2,2)=3.*M{i-3}(1,1)-M{i-3}(2,3); M{i-3}(3,1)=3.*M{i-3}(1,1); M{i-3}(3,2)=-3.*M{i-3}(1,1)-M{i-3}(3,3); M{i-3}(4,1)=-M{i-3}(1,1); M{i-3}(4,2)=1+M{i-3}(1,1)-M{i-3}(4,3); end syms a t t=(a-U(4))./(U(5)-U(4)); bx=[t.^3,t.^2,t.^1,t.^0]*M{1}*[W(1).*X(1);W(2).*X(2);W(3).*X(3);W(4).*X(4)]./([t.^3,t.^2,t.^1,t.^0]*M{1}*[W(1);W(2);W(3);W(4)]); by=[t.^3,t.^2,t.^1,t.^0]*M{1}*[W(1).*Y(1);W(2).*Y(2);W(3).*Y(3);W(4).*Y(4)]./([t.^3,t.^2,t.^1,t.^0]*M{1}*[W(1);W(2);W(3);W(4)]); bz=[t.^3,t.^2,t.^1,t.^0]*M{1}*[W(1).*Z(1);W(2).*Z(2);W(3).*Z(3);W(4).*Z(4)]./([t.^3,t.^2,t.^1,t.^0]*M{1}*[W(1);W(2);W(3);W(4)]); dx=diff(bx,'a'); dy=diff(by,'a'); dz=diff(bz,'a'); u(1)=0; for j=1:Inf if j<=2 %初始化参数 a=u(j); cx=eval(dx); cy=eval(dy); cz=eval(dz); u(j+1)=u(j)+L0(j)./sqrt((cx^2)+(cy^2)+(cz^2)); end if j>=4 u(j)=0.25.*(9.*u(j-1)-6.*u(j-2)+u(j-3))%阿当姆斯微分方程算法快速参数递推式预估插补参数值 end for s=1:Inf for i=4:length(W) %预估插补点坐标值 if u(j)>=U(i)&u(j)<U(i+1) t=(u(j)-U(i))./(U(i+1)-U(i)); x(j)=[t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3).*X(i-3);W(i-2).*X(i-2);W(i-1).*X(i-1);W(i).*X(i)]./([t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3);W(i-2);W(i-1);W(i)]); y(j)=[t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3).*Y(i-3);W(i-2).*Y(i-2);W(i-1).*Y(i-1);W(i).*Y(i)]./([t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3);W(i-2);W(i-1);W(i)]); z(j)=[t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3).*Z(i-3);W(i-2).*Z(i-2);W(i-1).*Z(i-1);W(i).*Z(i)]./([t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3);W(i-2);W(i-1);W(i)]); break end end if u(j)>=1 u(j)=1 x(j)=X(length(W)); y(j)=Y(length(W)); z(j)=Z(length(W)); break end if j==1 break end if j>=2 ub(j-1)=0.5.*(u(j)+u(j-1)) for i=4:length(W) if ub(j-1)>=U(i)&ub(j-1)<U(i+1) t=(ub(j-1)-U(i))./(U(i+1)-U(i)); xb(j-1)=[t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3).*X(i-3);W(i-2).*X(i-2);W(i-1).*X(i-1);W(i).*X(i)]./([t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3);W(i-2);W(i-1);W(i)]); yb(j-1)=[t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3).*Y(i-3);W(i-2).*Y(i-2);W(i-1).*Y(i-1);W(i).*Y(i)]./([t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3);W(i-2);W(i-1);W(i)]); zb(j-1)=[t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3).*Z(i-3);W(i-2).*Z(i-2);W(i-1).*Z(i-1);W(i).*Z(i)]./([t.^3,t.^2,t.^1,t.^0]*M{i-3}*[W(i-3);W(i-2);W(i-1);W(i)]); break end end h(j-1)=sqrt((xb(j-1)-0.5*x(j-1)-0.5*x(j))^2+(yb(j-1)-0.5*y(j-1)-0.5*y(j))^2+(zb(j-1)-0.5*z(j-1)-0.5*z(j))^2)%弓高误差 Lp(j-1)=sqrt((x(j)-x(j-1))^2+(y(j)-y(j-1))^2+(z(j)-z(j-1))^2)%预估步长 Lh(j-1)=sqrt(hm/h(j-1))*Lp(j-1)%由弓高误差确定的约束步长 La(j-1)=T/2*sqrt(0.5*am/hm)*Lh(j-1)%进给加速度确定的约束步长 if j<=all L(j-1)=min([L0(j-1),Lh(j-1),La(j-1)])%加速阶段期望步长 elseif u(j)<udec L(j-1)=min([0.3,Lh(j-1),La(j-1)])%匀速阶段期望步长 else L(j-1)=min([L0(all-e),Lh(j-1),La(j-1)])%减速阶段期望步长 end r(j-1)=abs(L(j-1)-Lp(j-1))/L(j-1);%插补步长的相对偏差 tag=tag+1; if k==2 plot(tag,j) end if r(j-1)<=0.001 if k==2&u(j)>=udec e=e+1; end %if k==2 %plot(j,L(j-1)) %end break else u(j)=u(j-1)+L(j-1)/Lp(j-1)*(u(j)-u(j-1))%参数的修正 %if k==2 %plot(j,L(j-1)) %end end end end %---------------->>73 if k==1&j==all%反向插补结束 udec=1-max(u) break end if k==2&j>=2 %plot3([x(j-1) x(j)],[y(j-1) y(j)],[z(j-1) z(j)],'r')%动态插补仿真演示 %axis([0 200 0 200 0 80]) %plot([x(j-1) x(j)],[y(j-1) y(j)],'r') hold on drawnow end if u(j)==1%终点判别 break end end%插补结束---------------------------->62 end