MATLAB机器人工具箱【1】—— 机械臂建模+正逆运动学+雅克比矩阵
1. 二维空间位姿描述2. 三维空间位姿描述3. 建立机器人模型3.1 Link 类3.2 SerialLink 类3.3 建立机器人模型（展示）4. 正逆运动学+雅克比矩阵4.1 求解正运动学方程4.2 正运动学方程正确性验证4.3 求解逆运动学方程4.4 逆运动学方程正确性验证4.5 雅克比矩阵5. 参考资料本文在参考B站up主刘海涛大佬的视频分享基础上，结合自己学习的机器人学知识，利用MATLAB机器人工具箱加深理解和运用，机器人工具箱安装教程见Robotic toolbox 工具箱的安装和初步了解，安装了工具箱后就可以用来愉快的学习啦。
1. 二维空间位姿描述
2. 三维空间位姿描述
3. 建立机器人模型
根据自己的机器人模型实际参数采用标准DH法或者改进DH法建模，得到DH参数表，然后利用工具箱建模，下面是建模时必须要用到的两个类函数。
3.1 Link 类
3.2 SerialLink 类
3.3 建立机器人模型（展示）
关于puma560的DH参数表有很多版本，可以参考PUMA560机器人D-H参数和改进DH参数
这里以这一版本为例【和《机器人学》——蔡自兴版本保持一致】：
%%改进D-H模型% 关节角 连杆偏距 连杆长度 连杆转角 % theta(i) d(i) a(i-1) alpha(i-1) offsetSL1=Link([00 0 0 0],'modified');SL2=Link([00.149090 -pi/2 0],'modified');SL3=Link([00 0.4318 0 0],'modified');SL4=Link([00.433070.02032 -pi/2 0],'modified');SL5=Link([00 0 pi/2 0],'modified');SL6=Link([00 0 -pi/2 0],'modified');p560=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','puma560');%SerialLink 类函数%通过手动输入各个连杆转角，模型会自动运动到相应位置%方法一：% p560.teach([-pi/2 -pi/2 0 0 0 0]);%方法二：p560.plot([-pi/2 -pi/2 0 0 0 0]);
在MATLAB的机器人工具箱中提供了puma560机械臂模型，运行 mdl_puma560 命令，会发现自动生成四个1*6的关节角矩阵，他们分别代表以下状态：
4. 正逆运动学+雅克比矩阵
4.1 求解正运动学方程
【小技巧：相邻连杆变换矩阵第一列第三个元素如果是零的话，说明选择的是标准DH建模法，否则（就像上图所示）是改进DH法】
自己编写的正运动学函数：
function T06 = puma560_fk(theta1,theta2,theta3,theta4,theta5,theta6)a2 = 0.4318;a3 = 0.02032;d2 = 0.14909;d4 = 0.43307;T01 = [cos(theta1) -sin(theta1) 0 0;sin(theta1) cos(theta1)0 0;0 01 0;0 00 1];T12 = [cos(theta2) -sin(theta2) 0 0;0 01 d2;-sin(theta2) -cos(theta2) 0 0;0 00 1];T23 = [cos(theta3) -sin(theta3) 0 a2;sin(theta3) cos(theta3)0 0;0 01 0;0 00 1];T34 = [cos(theta4) -sin(theta4) 0 a3;0 01 d4;-sin(theta4) -cos(theta4) 0 0;0 00 1];T45 = [cos(theta5) -sin(theta5) 0 0;0 0-1 0;sin(theta5) cos(theta5)0 0;0 00 1];T56 = [cos(theta6) -sin(theta6) 0 0;0 0-1 0;-sin(theta6) -cos(theta6) 0 0;0 00 1];T06 = T01*T12*T23*T34*T45*T56;end
求解自己的正运动学公式：
%求正运动学公式T06 = puma560_fk(theta1,theta2,theta3,theta4,theta5,theta6)
结果：
T06 =[ sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), (2159*cos(theta1)*cos(theta2))/5000 - (14909*sin(theta1))/100000 - (127*cos(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta1)*cos(theta2)*cos(theta3))/6250 - (43307*cos(theta1)*cos(theta2)*sin(theta3))/100000 - (43307*cos(theta1)*cos(theta3)*sin(theta2))/100000][ - sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), sin(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) - sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), (14909*cos(theta1))/100000 + (2159*cos(theta2)*sin(theta1))/5000 - (43307*cos(theta2)*sin(theta1)*sin(theta3))/100000 - (43307*cos(theta3)*sin(theta1)*sin(theta2))/100000 - (127*sin(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta2)*cos(theta3)*sin(theta1))/6250][ sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))), sin(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))) + cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),(43307*sin(theta2)*sin(theta3))/100000 - (43307*cos(theta2)*cos(theta3))/100000 - (127*cos(theta2)*sin(theta3))/6250 - (127*cos(theta3)*sin(theta2))/6250 - (2159*sin(theta2))/5000][ 0, 0, 0, 1]
化简：
>> simplify(T06)ans =[ sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), (2159*cos(theta1)*cos(theta2))/5000 - (14909*sin(theta1))/100000 - (127*cos(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta1)*cos(theta2)*cos(theta3))/6250 - (43307*cos(theta1)*cos(theta2)*sin(theta3))/100000 - (43307*cos(theta1)*cos(theta3)*sin(theta2))/100000][ - sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), sin(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) - sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), (14909*cos(theta1))/100000 + (2159*cos(theta2)*sin(theta1))/5000 - (43307*cos(theta2)*sin(theta1)*sin(theta3))/100000 - (43307*cos(theta3)*sin(theta1)*sin(theta2))/100000 - (127*sin(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta2)*cos(theta3)*sin(theta1))/6250][ sin(theta2 + theta3)*sin(theta4)*sin(theta6) - cos(theta6)*(cos(theta2 + theta3)*sin(theta5) + sin(theta2 + theta3)*cos(theta4)*cos(theta5)), sin(theta6)*(cos(theta2 + theta3)*sin(theta5) + sin(theta2 + theta3)*cos(theta4)*cos(theta5)) + sin(theta2 + theta3)*cos(theta6)*sin(theta4), cos(theta2 + theta3)*cos(theta5) - sin(theta2 + theta3)*cos(theta4)*sin(theta5),- (2159*sin(theta2))/5000 - (127*116537^(1/2)*cos(theta2 + theta3 - atan(16/341)))/100000][ 0, 0, 0, 1]
正运动学方程一般写成这样：
简洁表达成这样：
4.2 正运动学方程正确性验证
验证自己编写的正运动学函数：
% 用自己编写的正运动学函数求解T06 = puma560_fk(-pi/2,-pi/2,0,0,0,0)% 用机器人工具箱提供的函数求解T0_6 = p560.fkine([-pi/2 -pi/2 0 0 0 0])
对比运算结果，正运动学无误：
T06 =0.0000 -1.0000 -0.0000 0.1491-0.0000 -0.0000 1.0000 -0.43311.0000 0 0.0000 0.45210 0 0 1.0000T0_6 = 0 -1 0 0.14910 0 -1 -0.43311 0 0 0.45210 0 0 1
4.3 求解逆运动学方程
求解按照《机器人学—蔡自兴》的步骤结合MATLAB就可以了，现在根据求解出来的各个关节角将逆运动学求解封装成函数：
function theta_Med=puma560_ik(T)% a--连杆长度，d--连杆偏移量a2=0.4318;a3=0.02032;d2=0.14909;d4=0.43307;nx=T(1,1); ny=T(2,1); nz=T(3,1); ox=T(1,2); oy=T(2,2); oz=T(3,2); ax=T(1,3); ay=T(2,3); az=T(3,3); px=T(1,4); py=T(2,4); pz=T(3,4);% 为方便计算，定义的m系列向量% 求解关节角1theta1_1 = atan2(py,px)-atan2(d2,sqrt(px^2+py^2-d2^2));theta1_2 = atan2(py,px)-atan2(d2,-sqrt(px^2+py^2-d2^2));% 求解关节角3m3_1 = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);theta3_1 = atan2(a3,d4)-atan2(m3_1,sqrt(a3^2+d4^2-m3_1^2));theta3_2 = atan2(a3,d4)-atan2(m3_1,-sqrt(a3^2+d4^2-m3_1^2));% 求解关节角2ms2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_1)-d4);mc2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_1)+a3);theta23_1 = atan2(ms2_1,mc2_1);theta2_1 = theta23_1 - theta3_1;ms2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_1)-d4);mc2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_1)+a3);theta23_2 = atan2(ms2_2,mc2_2);theta2_2 = theta23_2 - theta3_1;ms2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_2)-d4);mc2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_2)+a3);theta23_3 = atan2(ms2_3,mc2_3);theta2_3 = theta23_3 - theta3_2;ms2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_2)-d4);mc2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_2)+a3);theta23_4 = atan2(ms2_4,mc2_4);theta2_4 = theta23_4 - theta3_2; % 求解关节角4ms4_1=-ax*sin(theta1_1)+ay*cos(theta1_1);mc4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*cos(theta23_1)+az*sin(theta23_1);theta4_1=atan2(ms4_1,mc4_1);ms4_2=-ax*sin(theta1_2)+ay*cos(theta1_2);mc4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*cos(theta23_2)+az*sin(theta23_2);theta4_2=atan2(ms4_2,mc4_2);ms4_3=-ax*sin(theta1_1)+ay*cos(theta1_1);mc4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*cos(theta23_3)+az*sin(theta23_3);theta4_3=atan2(ms4_3,mc4_3);ms4_4=-ax*sin(theta1_2)+ay*cos(theta1_2);mc4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*cos(theta23_4)+az*sin(theta23_4);theta4_4=atan2(ms4_4,mc4_4);% 求解关节角5ms5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))-ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))+az*(sin(theta23_1)*cos(theta4_1));mc5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))+az*(-cos(theta23_1));theta5_1=atan2(ms5_1,mc5_1);ms5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))-ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))+az*(sin(theta23_2)*cos(theta4_2));mc5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))+az*(-cos(theta23_2));theta5_2=atan2(ms5_2,mc5_2);ms5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))-ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))+az*(sin(theta23_3)*cos(theta4_3));mc5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))+az*(-cos(theta23_3));theta5_3=atan2(ms5_3,mc5_3);ms5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))-ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))+az*(sin(theta23_4)*cos(theta4_4));mc5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))+az*(-cos(theta23_4));theta5_4=atan2(ms5_4,mc5_4);% 求解关节角6ms6_1=-nx*(cos(theta1_1)*cos(theta23_1)*sin(theta4_1)-sin(theta1_1)*cos(theta4_1))-ny*(sin(theta1_1)*cos(theta23_1)*sin(theta4_1)+cos(theta1_1)*cos(theta4_1))+nz*(sin(theta23_1)*sin(theta4_1));mc6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1));theta6_1=atan2(ms6_1,mc6_1);ms6_2=-nx*(cos(theta1_2)*cos(theta23_2)*sin(theta4_2)-sin(theta1_2)*cos(theta4_2))-ny*(sin(theta1_2)*cos(theta23_2)*sin(theta4_2)+cos(theta1_2)*cos(theta4_2))+nz*(sin(theta23_2)*sin(theta4_2));mc6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)+cos(theta23_2)*sin(theta5_2));theta6_2=atan2(ms6_2,mc6_2);ms6_3=-nx*(cos(theta1_1)*cos(theta23_3)*sin(theta4_3)-sin(theta1_1)*cos(theta4_3))-ny*(sin(theta1_1)*cos(theta23_3)*sin(theta4_3)+cos(theta1_1)*cos(theta4_3))+nz*(sin(theta23_3)*sin(theta4_3));mc6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)+cos(theta23_3)*sin(theta5_3));theta6_3=atan2(ms6_3,mc6_3);ms6_4=-nx*(cos(theta1_2)*cos(theta23_4)*sin(theta4_4)-sin(theta1_2)*cos(theta4_4))-ny*(sin(theta1_1)*cos(theta23_4)*sin(theta4_4)+cos(theta1_2)*cos(theta4_4))+nz*(sin(theta23_4)*sin(theta4_4));mc6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)+cos(theta23_4)*sin(theta5_4));theta6_4=atan2(ms6_4,mc6_4);% 整理得到4组运动学非奇异逆解theta_Med_1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2;theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3;theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4;];% 将操作关节‘翻转’可得到另外4组解theta_Med_2 = ...[ theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi;theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi;theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi;theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi;];theta_Med=[theta_Med_1;theta_Med_2]; end
4.4 逆运动学方程正确性验证
%用自己编写的逆运动学函数求解T06 = kinematics([-pi/2 -pi/2 0 0 0 0])theta_med = puma560_ik(T06)
8组逆运动学解
theta_med =-1.5708 -1.5708 -0.0000 0 0 0-4.0493 -3.0986 -0.0000 2.4780 1.6047 -0.0265-1.5708 -0.0430 -3.0478 0 1.5201 0-4.0493 4.7124 -3.0478 1.6901 0.6687 1.4192-1.5708 -1.5708 -0.0000 3.1416 0 3.1416-4.0493 -3.0986 -0.0000 5.6196 -1.6047 3.1151-1.5708 -0.0430 -3.0478 3.1416 -1.5201 3.1416-4.0493 4.7124 -3.0478 4.8317 -0.6687 4.5608
其中第一组解就是我想要的结果：
-1.5708 -1.5708 -0.0000 0 0 0
4.5 雅克比矩阵
机器人雅克比矩阵可以联系机器人末端操作空间速度和关节空间速度，已知关节空间速度，利用雅克比矩阵，可以得到操作空间的速度，再结合逆过程，就可以实现速度模式控制机器人，从而初步实现平滑运动。
公式中，v 和 w 是笛卡尔空间中的速度和角速度，并且 q(dot) 是一个关节速度向量。
%练习使用雅克比矩阵求解函数qn = [0 0.7854 3.1416 0 0.7854 0];%jacob0()求解的是将关节速度映射到世界坐标系中的末端执行器空间速度p560.jacob0(qn)%jaconb()求解的是将关节速度映射到工具坐标系中的末端执行器空间速度p560.jacobe(qn)
结果：
jacob0 =-0.1491 0.0153 0.3206 0 0 00.5972 0.0000 0.0000 0 0 0-0.0000 -0.5972 -0.2919 0 0 0-0.0000 0 0 0.7071 0 1.00000 1.0000 1.0000 0.0000 1.0000 0.00001.0000 0.0000 0.0000 0.7071 0.0000 -0.0000jacobn =-0.0000 -0.5972 -0.2919 0 0 0-0.5972 0.0000 0.0000 0 0 0-0.1491 0.0153 0.3206 0 0 01.0000 0 0 0.7071 0 0-0.0000 -1.0000 -1.0000 -0.0000 -1.0000 0-0.0000 0.0000 0.0000 0.7071 0.0000 1.0000
