机械臂工作空间求解

q1min=-pi/4;q1max=pi/4;q2min=-2*pi/3;q2max=pi/18;q3min=-pi/2;q3max=pi/2;q4min=-4*pi/3;q4max=pi/3;q5min=-pi/4;q5max=pi/4;
a1=20;a2=320;a3=310;
n=20000;
q1=q1min+（q1max-q1min）.*rand（n1）;
q2=q2min+（q2max-q2min）.*rand（n1）;
q3=q1min+（q3max-q3min）.*rand（n1）;
q4=q1min+（q4max-q4min）.*rand（n1）;
q5=q1min+（q5max-q5min）.*rand（n1）;
 
 px=a1.*cos（q1） + a3.*（cos（q1）.*cos（q2）.*cos（q3） - cos（q1）.*sin（q2）.*sin（q3）） + a2.*cos（q1）.*cos（q2）
 py=a1.*sin（q1） + a3.*（cos（q2）.*cos（q3）.*sin（q1） - sin（q1）.*sin（q2）.*sin（q3）） + a2.*cos（q2）.*sin（q1）
 pz=- a2.*sin（q2） -