zhengjie1_1.rar_六自由度 正解_六自由度迭代_并联机构 正解_并联正解_牛顿六自由度

%%求偏导矩阵 牛顿迭代 正解 clc clear clf tic R0=1100; R1=820; Theta0 = 15.7/180*pi; Theta1 = 14/180*pi; %上 eta=[pi/3-Theta0/2;pi/3+Theta0/2;pi-Theta0/2;pi+Theta0/2;-pi/3-Theta0/2;-pi/3+Theta0/2]; %下铰点分布角 ksi=[Theta1/2;pi*2/3-Theta1/2;pi*2/3+Theta1/2;pi*4/3-Theta1/2;pi*4/3+Theta1/2;-Theta1/2]; %上铰点分布角 % eta=[Theta0/2;pi*2/3-Theta0/2;pi*2/3+Theta0/2;pi*4/3-Theta0/2;pi*4/3+Theta0/2;-Theta0/2]; %下铰点分布角 % ksi=[pi/3-Theta1/2;pi/3+Theta1/2;pi-Theta1/2;pi+Theta1/2;-pi/3-Theta1/2;-pi/3+Theta1/2]; %上铰点分布角 %下平台铰点 A1 = [R0*cos(eta(1)),R0*sin(eta(1)),0]'; A2 = [R0*cos(eta(2)),R0*sin(eta(2)),0]'; A3 = [R0*cos(eta(3)),R0*sin(eta(3)),0]'; A4 = [R0*cos(eta(4)),R0*sin(eta(4)),0]'; A5 = [R0*cos(eta(5)),R0*sin(eta(5)),0]'; A6 = [R0*cos(eta(6)),R0*sin(eta(6)),0]'; A=[A1,A2,A3,A4,A5,A6]; %上平台铰点 B10 = [R1*cos(ksi(1)),R1*sin(ksi(1)),0]'; B20 = [R1*cos(ksi(2)),R1*sin(ksi(2)),0]'; B30 = [R1*cos(ksi(3)),R1*sin(ksi(3)),0]'; B40 = [R1*cos(ksi(4)),R1*sin(ksi(4)),0]'; B50 = [R1*cos(ksi(5)),R1*sin(ksi(5)),0]'; B60 = [R1*cos(ksi(6)),R1*sin(ksi(6)),0]'; %初始杆长 lmin=1425 ;lmax=2090; l00=(lmin+lmax)/2; h=sqrt(l00^2-(R0*cos(eta(1))-R1*cos(ksi(1)))^2-(R0*sin(eta(1))-R1*sin(ksi(1)))^2); l0zong=zeros(2^6,6); for m=0:2^6-1 for n=1:6 l0zong(m+1,n)=bitshift(bitand(m,2^(n-1)),1-n)*(lmax-lmin)+lmin; end end % l0=l0zong(26,:); l0=[1425 2090 1425 1425 2090 1425]; % l10=2180;l20=1425;l30=1425;l40=1425;l50=1425;l60=2180; % l0=[l10,l20,l30,l40,l50,l60]; %初始位姿 x(1)=0;y(1)=0;z(1)=h;gama(1)=0/180*pi;beita(1)=0/180*pi;aerfa(1)=0/180*pi; weizi(1,:)=[x(1),y(1),z(1),gama(1),beita(1),aerfa(1)]; %开始迭代 fenshu2=1000; for i=1:fenshu2 weizhi=[x(i),y(i),z(i)]'; SS=[weizhi,weizhi,weizhi,weizhi,weizhi,weizhi]-A; %xyz杠 %变换矩阵 T1=[cos(aerfa(i)),-sin(aerfa(i)),0;sin(aerfa(i)),cos(aerfa(i)),0;0,0,1]; %偏转角 z T2=[cos(beita(i)),0,sin(beita(i));0,1,0;-sin(beita(i)),0,cos(beita(i))]; %俯仰角 y T3=[1,0,0;0,cos(gama(i)),-sin(gama(i));0,sin(gama(i)),cos(gama(i))]; %回转角 x T=T1*T2*T3; %上平台绞点坐标 uvw B1=T*B10; B2=T*B20; B3=T*B30; B4=T*B40; B5=T*B50; B6=T*B60; B=[B1,B2,B3,B4,B5,B6]; % uvw %6个驱动杆杆长 l1(i)=norm(B1+weizhi-A1); l2(i)=norm(B2+weizhi-A2); l3(i)=norm(B3+weizhi-A3); l4(i)=norm(B4+weizhi-A4); l5(i)=norm(B5+weizhi-A5); l6(i)=norm(B6+weizhi-A6); l(i,:)=[l1(i),l2(i),l3(i),l4(i),l5(i),l6(i)]; dertalmax(i)=max(abs(l(i,:)-l0)); dertalfang(i,:)=l(i,:).^2-l0.^2; if dertalmax(i)<0.01; break end %迭代矩阵，导数 ff123=2*(SS+B); ff14=2*(-SS(1,1)*B1(2)+SS(2,1)*B1(1)); %对aerfa求导，z轴 ff24=2*(-SS(1,2)*B2(2)+SS(2,2)*B2(1)); ff34=2*(-SS(1,3)*B3(2)+SS(2,3)*B3(1)); ff44=2*(-SS(1,4)*B4(2)+SS(2,4)*B4(1)); ff54=2*(-SS(1,5)*B5(2)+SS(2,5)*B5(1)); ff64=2*(-SS(1,6)*B6(2)+SS(2,6)*B6(1)); ff15=2*((-SS(1,1)*cos(aerfa(i))+SS(2,1)*sin(aerfa(i)))*B1(3)... -(A1(1)*cos(beita(i))+A1(2)*sin(beita(i))*sin(gama(i)))*SS(3,1)); ff25=2*((-SS(1,2)*cos(aerfa(i))+SS(2,2)*sin(aerfa(i)))*B2(3)... -(A2(1)*cos(beita(i))+A2(2)*sin(beita(i))*sin(gama(i)))*SS(3,2)); ff35=2*((-SS(1,3)*cos(aerfa(i))+SS(2,3)*sin(aerfa(i)))*B3(3)... -(A3(1)*cos(beita(i))+A3(2)*sin(beita(i))*sin(gama(i)))*SS(3,3)); ff45=2*((-SS(1,4)*cos(aerfa(i))+SS(2,4)*sin(aerfa(i)))*B4(3)... -(A4(1)*cos(beita(i))+A4(2)*sin(beita(i))*sin(gama(i)))*SS(3,4)); ff55=2*((-SS(1,5)*cos(aerfa(i))+SS(2,5)*sin(aerfa(i)))*B5(3)... -(A5(1)*cos(beita(i))+A5(2)*sin(beita(i))*sin(gama(i)))*SS(3,5)); ff65=2*((-SS(1,6)*cos(aerfa(i))+SS(2,6)*sin(aerfa(i)))*B6(3)... -(A6(1)*cos(beita(i))+A6(2)*sin(beita(i))*sin(gama(i)))*SS(3,6)); ff16=2*A1(2)*[T(1,3),T(2,3),T(3,3)]*SS(:,1) ; %对beita求导，x轴 ff26=2*A2(2)*[T(1,3),T(2,3),T(3,3)]*SS(:,2); ff36=2*A3(2)*[T(1,3),T(2,3),T(3,3)]*SS(:,3); ff46=2*A4(2)*[T(1,3),T(2,3),T(3,3)]*SS(:,4); ff56=2*A5(2)*[T(1,3),T(2,3),T(3,3)]*SS(:,5); ff66=2*A6(2)*[T(1,3),T(2,3),T(3,3)]*SS(:,6); ff=[ff123;ff16,ff26,ff36,ff46,ff56,ff66;ff15,ff25,ff35,ff45,ff55,ff65;... ff14,ff24,ff34,ff44,ff54,ff64]'; %迭代 weizi(i+1,:)=weizi(i,:)'-inv(ff)*dertalfang(i,:)'; x(i+1)=weizi(i+1,1); y(i+1)=weizi(i+1,2); z(i+1)=weizi(i+1,3); gama(i+1)=weizi(i+1,4); beita(i+1)=weizi(i+1,5); aerfa(i+1)=weizi(i+1,6); end A=[A1,A2,A3,A4,A5,A6,A1]; xa=A(1,:);ya=A(2,:);za=A(3,:); plot3(xa,ya,za,'b*',xa,ya,za,'linewidth',1.5,'color','y')%散点图 hold on B=[B1+weizhi,B2+weizhi,B3+weizhi,B4+weizhi,B5+weizhi,B6+weizhi,B1+weizhi]; xb=B(1,:);yb=B(2,:);zb=B(3,:); plot3(xb,yb,zb,'b*',xb,yb,zb,'linewidth',1.5,'color','b')%散点图 hold on line([A1(1),B1(1)+weizhi(1)],[A1(2),B1(2)+weizhi(2)],[A1(3),B1(3)+weizhi(3)],'linewidth',2,'color','g'); line([A2(1),B2(1)+weizhi(1)],[A2(2),B2(2)+weizhi(2)],[A2(3),B2(3)+weizhi(3)],'linewidth',2,'color','m'); line([A3(1),B3(1)+weizhi(1)],[A3(2),B3(2)+weizhi(2)],[A3(3),B3(3)+weizhi(3)],'linewidth',2,'color','r'); line([A4(1),B4(1)+weizhi(1)],[A4(2),B4(2)+weizhi(2)],[A4(3),B4(3)+weizhi(3)],'linewidth',2,'color','r'); line([A5(1),B5(1)+weizhi(1)],[A5(2),B5(2)+weizhi(2)],[A5(3),B5(3)+weizhi(3)],'linewidth',2,'color','r'); line([A6(1),B6(1)+weizhi(1)],[A6(2),B6(2)+weizhi(2)],[A6(3),B6(3)+weizhi(3)],'linewidth',2,'color','r'); dddd=dertalmax'; % % weizi1=[weizi(i,1),weizi(i,2),weizi(i,3),... % % weizi(i,4)/pi*180,weizi(i,5)/pi*180,weizi(i,6)/pi*180]; % % ganchang=l; % % l0(1,:)=[l10,l20,l30,l40,l50,l60]; % % dertalfang; % % dertalmax % % i % % toc % % % % % 改进的雅克比矩阵 % % % % jacobiV(1,:)=ff(1,:)/l1(i)/2; % % % % jacobiV(2,:)=ff(2,:)/l2(i)/2; % % % % jacobiV(3,:)=ff(3,:)/l3(i)/2; % % % % jacobiV(4,:)=ff(4,:)/l4(i)/2; % % % % jacobiV(5,:)=ff(5,:)/l5(i)/2; % % % % jacobiV(6,:)=ff(6,:)/l6(i)/2;