下面是M文件
function [q2,d1,q50,q31,q32,q3,q41,q42,q4]=funrobota(n1,n2,n3,o1,o2,o3,m1,m2,m3,a1,p1,p2,p3,d0,d2,d3,d4)
%function [q2,d1,q50,q31,q32,q3]=funrobota(n1,n2,n3,o1,o2,o3,m1,m2,m3,a1,p1,p2,p3,d0,d2,d3,d4)
%求解角度q2
[q21]=solve('m1+sin(q21)=0');
q21=eval(q21);
if -pi/2<q21<pi/2 & abs(m3+cos(q21))<0.05   %设定精度
    q2=q21;
end

%求解距离d1
if abs(q2-pi/2)<0.05
    d11=p3+d4;
    elseif abs(q2+pi/2)<0.05
    d11=p3-d4;
    else
d11=p3-tan(q2)*(a1-p1);
end
if d11<d0
    d1=d11;%判断移动距离是否超过有效行程
end
%为求解角度q3先求解角度q5
[q51]=solve('cos(q2)*o1-sin(q2)*o3+sin(q51)=0','q51');
q51=eval(q51);
q52=pi-q51;
%q53=q51+2*pi;
%q54=q52-2*pi;
if -3*pi/2<q51<5*pi/2 & abs(o2+cos(q51))<0.05
    q50=q51;
    %q501=q53;
  elseif -3*pi/2<q52<5*pi/2 & abs(o2+cos(q52))<0.05
    q50=q52;
    %q501=q54;
else
    return
end
%求解角度q3
%[q31]=solve('p2+d2+sin(q50)*d4+sin(q31)*d3=0');
%q31=eval(q31);
q31=-asin((p2+d2+sin(q50)*d4)/d3);
q32=pi-q31;
if -pi/2<q31<3*pi/2 & abs(cos(q2)*cos(q50)*d4+cos(q2)*cos(q31)*d3+a1-p1)<20 %距离值，误差可以相对较大
    q3=q31;
elseif -pi/2<q32<1.5*pi & abs(cos(q2)*cos(q50)*d4+cos(q2)*cos(q32)*d3+a1-p1)<20
    q3=q32;
else
    return
end
%q3=q3*180/pi;
%q3=90-q3;
%求解角度q4
q41=acos(cos(q3)*(cos(q2)*n1-sin(q2)*n3)-sin(q3)*n2);
q42=-q41;
%if -pi<q41<pi & abs(cos(q3)*(cos(q2)*p1-sin(q2)*(p3-d1)-cos(q2)*a1)-sin(q3)*(p2+d2)-cos(q41)*d4-d3)<5
if -pi<q41<pi & abs(sin(q41)+sin(q3)*(cos(q2)*n1-sin(q2)*n3)+cos(q3)*n2)<0.05
    q4=q41;
%elseif -pi<q41<pi & abs(cos(q3)*(cos(q2)*p1-sin(q2)*(p3-d1)-cos(q2)*a1)-sin(q3)*(p2+d2)-cos(q41)*d4-d3)<5
elseif -pi<q42<pi & abs(sin(q42)+sin(q3)*(cos(q2)*n1-sin(q2)*n3)+cos(q3)*n2)<0.05
    q4=q42;
else
    return
end
q2=(q2*180/pi)+90;
q3=90-(q3*180/pi);
q4=-q4*180/pi;
这个是求反解的程序，因为不是六自由度的，所以不能用自带的“ikine"函数直接求解。但是在把已知的参数替换后”[q2,d1,q50,q31,q32,q3,q41,q42,q4]=funrobota(0.866,-0.5,0,0.5,0.866,0,0,0,1,145,10,20,30,450,374.5,260,250)“
结果出现错误：“??? Undefined function or variable "q2".

Error in ==> funrobota at 11
if abs(q2-pi/2)<0.05”
急求哪位大侠帮我看一下，该怎么改呢