六自由度机器人D-H法正逆运动学分析matlab代码

%--------------------- %定义初值 %--------------------- %syms x1 x2 x3 x4 x5 x6 %x1=sym(0.53/180*pi);x2=sym(-pi/2+19.85/180*pi);x3=sym(0.09/180*pi);x4=sym(19.95/180*pi);x5=sym(pi+90.39/180*pi);x6=sym(0.05/180*pi); x1=sym(20/180*pi);x2=sym(50/180*pi);x3=sym(20/180*pi);x4=sym(10/180*pi);x5=sym(40/180*pi);x6=sym(10/180*pi); d1=280;d2=0;d3=0;d4=210;d5=0;d6=0; %d1=125;d2=0;d3=0;d4=210;d5=0;d6=0; a1=0;a2=210;a3=75;a4=0;a5=0;a6=0; y1=-sym(pi/2);y2=0;y3=-sym(pi/2);y4=sym(pi/2);y5=sym(pi/2);y6=0; %-------------------- %转换矩阵通用函数定义 %-------------------- F=@(x,d,a,y) [cos(x) -sin(x)*cos(y) sin(x)*sin(y) a*cos(x);sin(x) cos(x)*cos(y) -cos(x)*sin(y) a*sin(x); 0 sin(y) cos(y) d;0 0 0 1]; %----------------------- %各转换矩阵及运动学方程 %----------------------- T01=F(x1,d1,a1,y1); T12=F(x2,d2,a2,y2); T23=F(x3,d3,a3,y3); T34=F(x4,d4,a4,y4); T45=F(x5,d5,a5,y5); T56=F(x6,d6,a6,y6); T06=T01*T12*T23*T34*T45*T56; T06=vpa(T06,10) %------------------- %含变量的运动学方程 %T06 = %[ cos(x6)*(cos(x5)*(sin(x1)*sin(x4) - cos(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))) - sin(x5)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2))) - sin(x6)*(cos(x4)*sin(x1) - sin(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))), - cos(x6)*(cos(x4)*sin(x1) - sin(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))) - sin(x6)*(cos(x5)*(sin(x1)*sin(x4) - cos(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))) - sin(x5)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2))), cos(x5)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2)) - sin(x5)*(sin(x1)*sin(x4) - cos(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))), 210*cos(x1)*cos(x2) - 70*sin(x5)*(sin(x1)*sin(x4) - cos(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3))) + 70*cos(x5)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2)) - 210*cos(x1)*cos(x2)*sin(x3) + 210*cos(x1)*cos(x3)*sin(x2) - 75*cos(x1)*sin(x2)*sin(x3) + 75*cos(x1)*cos(x2)*cos(x3)] %[ cos(x6)*(cos(x5)*(cos(x1)*sin(x4) + cos(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))) - sin(x5)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2))) - sin(x6)*(cos(x1)*cos(x4) + sin(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))), - cos(x6)*(cos(x1)*cos(x4) + sin(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))) - sin(x6)*(cos(x5)*(cos(x1)*sin(x4) + cos(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))) - sin(x5)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2))), cos(x5)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2)) - sin(x5)*(cos(x1)*sin(x4) + cos(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))), 210*cos(x2)*sin(x1) - 70*sin(x5)*(cos(x1)*sin(x4) + cos(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))) + 70*cos(x5)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2)) + 75*cos(x2)*cos(x3)*sin(x1) - 210*cos(x2)*sin(x1)*sin(x3) + 210*cos(x3)*sin(x1)*sin(x2) - 75*sin(x1)*sin(x2)*sin(x3)] %[ cos(x6)*(sin(x5)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)) - cos(x4)*cos(x5)*(cos(x2)*sin(x3) + cos(x3)*sin(x2))) + sin(x4)*sin(x6)*(cos(x2)*sin(x3) + cos(x3)*sin(x2)), cos(x6)*sin(x4)*(cos(x2)*sin(x3) + cos(x3)*sin(x2)) - sin(x6)*(sin(x5)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)) - cos(x4)*cos(x5)*(cos(x2)*sin(x3) + cos(x3)*sin(x2))), cos(x4)*sin(x5)*(cos(x2)*sin(x3) + cos(x3)*sin(x2)) - cos(x5)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)), 210*cos(x2)*cos(x3) - 210*sin(x2) - 75*cos(x2)*sin(x3) - 75*cos(x3)*sin(x2) + 210*sin(x2)*sin(x3) - 70*cos(x5)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)) + 70*cos(x4)*sin(x5)*(cos(x2)*sin(x3) + cos(x3)*sin(x2)) + 280] %[ 0, 0, 0, 1]