www.gusucode.com > 基于路径跟踪原理求解6_SPS并联机构位置正解 > gaijinnew7.m

    %三阶牛顿法，预估两次计算一次误差   
%%%%%%zn=xn-f(xn)/f(xn)'%%%%%%%%%%%    0.039
%%%%%%%xn+1=xn-2f(xn)/(f(xn)+f(zn)')%%%%%%%%%

tic
clear all
clc
vtr=1.e-3;
s=1;
%z=[202.8391 290.3898 604.7034 714.3585 366.8086 225.5108]';
l=[160 150 167 158 165 157];%杆长的目标位置

x0 =[0 0 121 0 0 0];%平台的初始位姿

rp=6006.25;%p平台的外接圆半径平方
rb=6006.25;%b平台的外接圆半径平方

pa=[-10.12 -76.84 0];
pb=[10.12 -76.84 0];
pc=[71.60 29.66 0];
pd=[61.48 47.18 0];
pe=[-61.48 47.18 0];
pf=[-71.60 29.66 0];%在p坐标系，六点坐标
		
ba=[-61.48 -47.18 0];
bb=[61.48 -47.18 0];
bc=[71.60 -29.66 0];
bd=[10.12 76.84 0];
be=[-10.12 76.84 0];
bf=[-71.60 -29.66 0];%在b坐标系，六点坐标

r11=cos(x0(4))*cos(x0(5));
r12=(cos(x0(4))*sin(x0(5))*sin(x0(3)))-(sin(x0(4))*cos(x0(6)));
r13=(cos(x0(4))*sin(x0(5))*cos(x0(3)))+(sin(x0(4))*sin(x0(6)));
r21=sin(x0(4))*cos(x0(5));
r22=(sin(x0(4))*sin(x0(5)))*sin(x0(3))+(cos(x0(4))*cos(x0(6)));
r23=(sin(x0(4))*sin(x0(5)))*cos(x0(3))-(cos(x0(4))*sin(x0(6)));
r31=-sin(x0(5));
r32=cos(x0(5))*sin(x0(6));
r33=cos(x0(5))*cos(x0(6));
R=[r11 r12 r13;r21 r22 r23;r31 r32 r33];%坐标P相对于坐标B方向的转换矩阵，r-X，b-Y,a-Z轴

while (s>vtr)     %整体循环，直至路径终点
    %将滑块的终点坐标z和动平台的起点位置x0带入到轨迹曲线F中
F0=[
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pa(1))*(ba(1)*r11+ba(2)*r12+ba(3)*r13)+2*(y-pa(2))*(ba(1)*r21+ba(2)*r22+ba(3)*r23)+2*(z-pa(3))*(ba(1)*r31+ba(2)*r32+ba(3)*r33)-2*(x*pa(1)+y*ba(3))-l(1));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pb(1))*(bb(1)*r11+bb(2)*r12+bb(3)*r13)+2*(y-pb(2))*(bb(1)*r21+bb(2)*r22+bb(3)*r23)+2*(z-pb(3))*(bb(1)*r31+bb(2)*r32+bb(3)*r33)-2*(x*pb(1)+y*bb(3))-l(2));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pc(1))*(bc(1)*r11+bc(2)*r12+bc(3)*r13)+2*(y-pc(2))*(bc(1)*r21+bc(2)*r22+bc(3)*r23)+2*(z-pc(3))*(bc(1)*r31+bc(2)*r32+bc(3)*r33)-2*(x*pc(1)+y*bc(3))-l(3));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pd(1))*(bd(1)*r11+bd(2)*r12+bd(3)*r13)+2*(y-pd(2))*(bd(1)*r21+bd(2)*r22+bd(3)*r23)+2*(z-pd(3))*(bd(1)*r31+bd(2)*r32+bd(3)*r33)-2*(x*pd(1)+y*bd(3))-l(4));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pe(1))*(be(1)*r11+be(2)*r12+be(3)*r13)+2*(y-pe(2))*(be(1)*r21+be(2)*r22+be(3)*r23)+2*(z-pe(3))*(be(1)*r31+be(2)*r32+be(3)*r33)-2*(x*pe(1)+y*be(3))-l(5));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pf(1))*(bf(1)*r11+bf(2)*r12+bf(3)*r13)+2*(y-pf(2))*(bf(1)*r21+bf(2)*r22+bf(3)*r23)+2*(z-pf(3))*(bf(1)*r31+bf(2)*r32+bf(3)*r33)-2*(x*pf(1)+y*bf(3))-l(6));
    ]
F0_x=[diff(F0(1),x);diff(F0(2),y);diff(F0(3),z); diff(F0(4),a); diff(F0(5),b); diff(F0(6),r);] 

    %牛顿迭代公式
x=x0-inv(F0_x)*F0;

r11=cos(x(4))*cos(x(5));
r12=(cos(x(4))*sin(x(5))*sin(x(3)))-(sin(x(4))*cos(x(6)));
r13=(cos(x(4))*sin(x(5))*cos(x(3)))+(sin(x(4))*sin(x(6)));
r21=sin(x(4))*cos(x(5));
r22=(sin(x(4))*sin(x(5)))*sin(x(3))+(cos(x(4))*cos(x(6)));
r23=(sin(x(4))*sin(x(5)))*cos(x(3))-(cos(x(4))*sin(x(6)));
r31=-sin(x(5));
r32=cos(x(5))*sin(x(6));
r33=cos(x(5))*cos(x(6));
R=[r11 r12 r13;r21 r22 r23;r31 r32 r33];

F_x=[diff(F0(1),x);diff(F0(2),y);diff(F0(3),z); diff(F0(4),a); diff(F0(5),b); diff(F0(6),r);]        

   %三阶的牛顿公式
xn=x0-2.*inv(F0_x+F_x)*F0;

x0=xn;
r11=cos(x0(4))*cos(x0(5));
r12=(cos(x0(4))*sin(x0(5))*sin(x0(3)))-(sin(x0(4))*cos(x0(6)));
r13=(cos(x0(4))*sin(x0(5))*cos(x0(3)))+(sin(x0(4))*sin(x0(6)));
r21=sin(x0(4))*cos(x0(5));
r22=(sin(x0(4))*sin(x0(5)))*sin(x0(3))+(cos(x0(4))*cos(x0(6)));
r23=(sin(x0(4))*sin(x0(5)))*cos(x0(3))-(cos(x0(4))*sin(x0(6)));
r31=-sin(x0(5));
r32=cos(x0(5))*sin(x0(6));
r33=cos(x0(5))*cos(x0(6));
R=[r11 r12 r13;r21 r22 r23;r31 r32 r33];
F0=[
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pa(1))*(ba(1)*r11+ba(2)*r12+ba(3)*r13)+2*(y-pa(2))*(ba(1)*r21+ba(2)*r22+ba(3)*r23)+2*(z-pa(3))*(ba(1)*r31+ba(2)*r32+ba(3)*r33)-2*(x*pa(1)+y*ba(3))-l(1));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pb(1))*(bb(1)*r11+bb(2)*r12+bb(3)*r13)+2*(y-pb(2))*(bb(1)*r21+bb(2)*r22+bb(3)*r23)+2*(z-pb(3))*(bb(1)*r31+bb(2)*r32+bb(3)*r33)-2*(x*pb(1)+y*bb(3))-l(2));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pc(1))*(bc(1)*r11+bc(2)*r12+bc(3)*r13)+2*(y-pc(2))*(bc(1)*r21+bc(2)*r22+bc(3)*r23)+2*(z-pc(3))*(bc(1)*r31+bc(2)*r32+bc(3)*r33)-2*(x*pc(1)+y*bc(3))-l(3));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pd(1))*(bd(1)*r11+bd(2)*r12+bd(3)*r13)+2*(y-pd(2))*(bd(1)*r21+bd(2)*r22+bd(3)*r23)+2*(z-pd(3))*(bd(1)*r31+bd(2)*r32+bd(3)*r33)-2*(x*pd(1)+y*bd(3))-l(4));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pe(1))*(be(1)*r11+be(2)*r12+be(3)*r13)+2*(y-pe(2))*(be(1)*r21+be(2)*r22+be(3)*r23)+2*(z-pe(3))*(be(1)*r31+be(2)*r32+be(3)*r33)-2*(x*pe(1)+y*be(3))-l(5));
      (6006.25+6006.25+x^2+y^2+z^2+2*(x-pf(1))*(bf(1)*r11+bf(2)*r12+bf(3)*r13)+2*(y-pf(2))*(bf(1)*r21+bf(2)*r22+bf(3)*r23)+2*(z-pf(3))*(bf(1)*r31+bf(2)*r32+bf(3)*r33)-2*(x*pf(1)+y*bf(3))-l(6));
    ]
    %动平台的精度曲线F对动平台的位置坐标求导，代入x0点坐标，代入滑块的终点坐标值z
F0_x=[diff(F0(1),x);diff(F0(2),y);diff(F0(3),z); diff(F0(4),a); diff(F0(5),b); diff(F0(6),r);] 
    %牛顿迭代公式
x=x0-inv(F0_x)*F0;

r11=cos(x(4))*cos(x(5));
r12=(cos(x(4))*sin(x(5))*sin(x(3)))-(sin(x(4))*cos(x(6)));
r13=(cos(x(4))*sin(x(5))*cos(x(3)))+(sin(x(4))*sin(x(6)));
r21=sin(x(4))*cos(x(5));
r22=(sin(x(4))*sin(x(5)))*sin(x(3))+(cos(x(4))*cos(x(6)));
r23=(sin(x(4))*sin(x(5)))*cos(x(3))-(cos(x(4))*sin(x(6)));
r31=-sin(x(5));
r32=cos(x(5))*sin(x(6));
r33=cos(x(5))*cos(x(6));
R=[r11 r12 r13;r21 r22 r23;r31 r32 r33];
F_x=[diff(F0(0),x);diff(F0(1),y);diff(F0(2),z); diff(F0(3),a); diff(F0(4),b); diff(F0(5),r);] 
xn=x0-2.*inv(F0_x+F_x)*F0;

%计算迭代结果的精度矩阵s1和精度矩阵值s


s1==[diff(F0(1),x);diff(F0(2),y);diff(F0(3),z); diff(F0(4),a); diff(F0(5),b); diff(F0(6),r);] 
 
s=sqrt(s1(1)^2+s1(2)^2+s1(3)^2+s1(4)^2+s1(5)^2+s1(6)^2)
%如果精度s不满足要求，则将牛顿迭代值保存为下一次迭代的起始点
x0=x;
F0=s1;
 end        %end for if (double(s)<vtr) 
% % end         %end for while (t<=1)
Px=vpa(xn(1));
Py=vpa(xn(2));
Pz=vpa(xn(3));
w=vpa(xn(4)*180/pi);
v=vpa(xn(5)*180/pi);
u=vpa(xn(6)*180/pi);
% w=vpa(x(4));
% v=vpa(x(5));
% u=vpa(x(6));
xn=[Px,Py,Pz;w,v,u]

t=toc