matlab 机器数,机器人条件数

女爷i 2023-01-14 04:53 156阅读 0赞

for x2=-0.5\*pi:pi/3:0.65\*pi

for x3=-0.5\*pi:pi/3:0.65\*pi

for x4=-0.5\*pi:pi/3:0.65\*pi

for x5=-0.5\*pi:pi/3:0.65\*pi

nx=-sin(x4)\*cos(x5)\*cos(x6)-cos(x4)\*sin(x6);

ny=sin(x2+x3)\*(cos(x4)\*cos(x5)\*cos(x6)-sin(x4)\*sin(x6))+sin(x4)\*cos(x5)\*cos(x6);

nz=cos(x2+x3)\*cos(x4)\*cos(x5)\*cos(x6)-sin(x2+x3)\*sin(x4)\*sin(x6)-sin(x2+x3)\*sin(x5)\*cos(x6);

ox=sin(x4)\*cos(x5)\*sin(x6)-cos(x4)\*cos(x6) ;

oy=-cos(x2+x3)\*(cos(x4)\*cos(x5)\*sin(x6)+sin(x4)\*cos(x6))-cos(x2+x3)\*sin(x5)\*sin(x6);

oz=-cos(x2+x3)\*(cos(x4)\*cos(x5)\*sin(x6)+sin(x4)\*cos(x6))+sin(x2+x3)\*sin(x5)\*sin(x6);

ax=-cos(x4)\*sin(x4)\*sin(x5);

ay=sin(x2+x3)\*cos(x4)\*sin(x5)-cos(x2+x3)\*sin(x5);

az=cos(x2+x3)\*cos(x4)\*sin(x5)+sin(x2+x3)\*cos(x5);

px=0;

py=sin(x2+x3)\*a3-cos(x2+x3)\*d4+sin(x2)\*a2;

pz=cos(x2+x3)\*a3+sin(x2+x3)\*d4+cos(x2)\*a2+d1;

R=\[nx,ox,nx,0,0,0;

ny,oy,ay,0,0,0;

nz,oz,az,0,0,0;

0,0,0,nx,ox,ax;

0,0,0,ny,oy,ay;

0,0,0,nz,oz,az\];%机器人正运动学方程一部分的组合

J1=\[(cos(x4)\*sin(x6)+sin(x4)\*cos(x5)\*cos(x6))\*(sin(x2+x3)\*a3-cos(x2+x3)\*d4+sin(x2)\*a2),(a2+a3\*cos(x3)+d4\*sin(x3))\*(cos(x6)\*(cos(x3)\*sin(x5)+cos(x4)\*cos(x5)\*sin(x3))-sin(x3)\*sin(x4)\*sin(x6))-(a3\*sin(x3)-d4\*cos(x3))\*(- cos(x6)\*(sin(x3)\*sin(x5)-cos(x3)\*cos(x4)\*cos(x5))-cos(x3)\*sin(x4)\*sin(x6)),-sin(x5)\*cos(x6)\*a3+(cos(x4)\*cos(x5)\*cos(x6)-sin(x4)\*sin(x6))\*d4,0,0,0;

\-(sin(x2+x3)\*a3-cos(x2+x3)\*d4+sin(x2)\*a2)\*(cos(x5)\*sin(x4)\*sin(x6)-cos(x4)\*cos(x6)) ,-(a3\*sin(x3)-d4\*cos(x3))\*( sin(x6)\*(sin(x3)\*sin(x5)-cos(x3)\*cos(x4)\*cos(x5))-cos(x3)\*cos(x6)\*sin(x4))+(a2+a3\*cos(x3)+d4\*sin(x3))\*(- sin(x6)\*(cos(x3)\*sin(x5)+cos(x4)\*cos(x5)\*sin(x3))-cos(x6)\*sin(x3)\*sin(x4)),sin(x5)\*sin(x6)\*a3-(cos(x4)\*cos(x5)\*sin(x6)+sin(x4)\*cos(x6))\*d4,0,0,0;

(sin(x2+x3)\*a3-cos(x2+x3)\*d4+sin(x2)\*a2)\*sin(x4)\*sin(x5),(a2+cos(x3)\*a3+sin(x3)\*d4)\*(sin(x3)\*cos(x4)\*sin(x5)-cos(x3)\*cos(x5))-(cos(x3)\*cos(x4)\*sin(x5)+sin(x3)\*cos(x5))\*(a3\*sin(x3)-cos(x3)\*d4),cos(x5)\*a3-cos(x4)\*sin(x5)\*d4,0,0,0;

cos(x2+x3)\*cos(x4)\*cos(x5)\*cos(x6)-sin(x2+x3)\*sin(x4)\*cos(x6)-sin(x2+x3)\*sin(x5)\*cos(x6),sin(x4)\*cos(x5)\*cos(x6)+cos(x4)\*sin(x6),sin(x4)\*cos(x5)\*cos(x6)+cos(x4)\*sin(x6),-sin(x5)\*cos(x6),sin(x6),0;

\-cos(x2+x3)\*(cos(x4)\*cos(x5)\*sin(x6)+sin(x4)\*cos(x6))+sin(x2+x3)\*sin(x5)\*sin(x6),-sin(x4)\*cos(x5)\*sin(x6)+cos(x4)\*cos(x6),-sin(x4)\*cos(x5)\*sin(x6)+cos(x4)\*cos(x6),sin(x5)\*sin(x6),cos(x6),0;

cos(x2+x3)\*cos(x4)\*sin(x5)+sin(x2+x3)\*cos(x5),sin(x4)\*sin(x5),sin(x4)\*sin(x5),cos(x5),0,1\];%相对雅克比矩阵

J=R\*J1;%雅克比矩阵

JJ=subs(J,\{x1,d1,a2,a3,d4,x6\},\{0,140,210,70,210,0\});

disp(JJ);

z=cond(JJ,2);%雅克比矩阵条件数

x=sin(x2+x3)\*a3-cos(x2+x3)\*d4+sin(x2)\*a2;

y=cos(x2+x3)\*a3+sin(x2+x3)\*d4+cos(x2)\*a2+d1;

plot3(x,y,z)%机器人条件数在工作空间的分布

hold on;

grid on;

end