matlab roty,求助大神，关于求解非线性方程函数fsolve的问题

运行程序结果如下：

001.png (13.72 KB, 下载次数: 0)

2017-1-11 20:26 上传

程序如下：

syms x y z a b c

M0=[0;0.85;1.2;1];

L0=[-0.93*sin(5*pi/180);0.93*cos(5*pi/180);0;1];

P0=[0.93*sin(5*pi/180);0.93*cos(5*pi/180);0;1];

V0=[0;0.85-0.93*cos(5*pi/180);1.2;0];

K0=[0.93*sin(5*pi/180);0.85-0.93*cos(5*pi/180);1.2;0];

U0=[-0.93*sin(5*pi/180);0.85-0.93*cos(5*pi/180);1.2;0];

for j=0:2

M1=rotz(j*2*pi/3)*M0;

L1=rotz(j*2*pi/3)*L0;

P1=rotz(j*2*pi/3)*P0;

V1=rotz(j*2*pi/3)*V0;

K1=rotz(j*2*pi/3)*K0;

U1=rotz(j*2*pi/3)*U0;

tew=trans(a,b,c)*rotz(z)*roty(y)*rotx(x);

K1=K1/norm(K1);

M1c=tew*M1;

V1c=tew*V1;

V1c=V1c/norm(V1c);

vr1=reflt(V1c(1),V1c(2),V1c(3))*K1;

dians=jiaodian(L1,K1,M1c,V1c);

dianp=jiaodian(dians,vr1,P1,U1);

%主镜坐标到PSD坐标变换矩阵

TWP1=trans(-0.93*sin(5*pi/180+j*2*pi/3),-0.93*cos(5*pi/180+j*2*pi/3),0)*rotz(0)*roty(0)*rotx(0);

PSD=TWP1*dianp;

PSDP(2*j+1)=PSD(1);

PSDP(2*j+2)=PSD(2);

end

eq1=PSDP(1);

eq2=PSDP(2);

eq3=PSDP(3);

eq4=PSDP(4);

eq5=PSDP(5);

eq6=PSDP(6);

eq1 = matlabFunction(eq1);

eq2 = matlabFunction(eq2);

eq3 = matlabFunction(eq3);

eq4 = matlabFunction(eq4);

eq5 = matlabFunction(eq5);

eq6 = matlabFunction(eq6);

%options = optimoptions(@fsolve,'MaxIter',1500,'MaxFunEvals',1500)

eq=@(x)[eq1(x(1),x(2),x(3),x(4),x(5),x(6));eq2(x(1),x(2),x(3),x(4),x(5),x(6));...

eq3(x(1),x(2),x(3),x(4),x(5),x(6));eq4(x(1),x(2),x(3),x(4),x(5),x(6));...

eq5(x(1),x(2),x(3),x(4),x(5),x(6));eq6(x(1),x(2),x(3),x(4),x(5),x(6));];

[sol,fval] = fsolve(eq,[0.01,0.011,0.011,0.01,0.05,0.03])

下面是定义的几个函数：

%旋转变化矩阵

function y=trans(xtr,ytr,ztr);

y=[1 0 0 xtr

01 0 ytr

00 1 ztr

00 0 1];

function y=rotx(xrt);

y=[1 0 0 0

0cos(xrt) -sin(xrt) 0

0sin(xrt) cos(xrt) 0

00 0 1];

function y=roty(xrt);

y=[cos(xrt) 0 sin(xrt) 0

0  1 0 0

-sin(xrt) 0  cos(xrt) 0

00 0 1];

function y=rotz(xrt);

y=[cos(xrt) -sin(xrt) 0 0

sin(xrt) cos(xrt) 0 0

00 1 0

00 0 1];

function y=reflt(l,m,n);

y=[1-2*l*l -2*l*m -2*l*n 0

-2*l*m 1-2*m*m -2*n*m 0

-2*l*n -2*n*m 1-2*n*n 0

00 0 1];

%求线与面的交点坐标，其中L为直线上一点坐标，K为直线方向矢量；M为面上一点坐标，V为面法向量

function y=jiaodian(L,K,M,V);

xl=L(1);

yl=L(2);

zl=L(3);

xk=K(1);

yk=K(2);

zk=K(3);

xt=M(1);

yt=M(2);

zt=M(3);

l=V(1);

m=V(2);

n=V(3);

B=l*xk+m*yk+n*zk;

G=l*xt+m*yt+n*zt;

xdian=(xk*G+m*(xl*yk-xk*yl)+n*(xl*zk-xk*zl))/B;

ydian=(yk*G+l*(yl*xk-yk*xl)+n*(yl*zk-yk*zl))/B;

zdian=(zk*G+l*(zl*xk-zk*xl)+m*(zl*yk-zk*yl))/B;

y=[xdian;ydian;zdian;1];

哪位路过的大神，帮忙看一下，小弟在此先谢过了