MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵
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) offset
SL1=Link([0 0 0 0 0 ],'modified');
SL2=Link([0 0.14909 0 -pi/2 0 ],'modified');
SL3=Link([0 0 0.4318 0 0 ],'modified');
SL4=Link([0 0.43307 0.02032 -pi/2 0 ],'modified');
SL5=Link([0 0 0 pi/2 0 ],'modified');
SL6=Link([0 0 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 0 1 0;
0 0 0 1];
T12 = [cos(theta2) -sin(theta2) 0 0;
0 0 1 d2;
-sin(theta2) -cos(theta2) 0 0;
0 0 0 1];
T23 = [cos(theta3) -sin(theta3) 0 a2;
sin(theta3) cos(theta3) 0 0;
0 0 1 0;
0 0 0 1];
T34 = [cos(theta4) -sin(theta4) 0 a3;
0 0 1 d4;
-sin(theta4) -cos(theta4) 0 0;
0 0 0 1];
T45 = [cos(theta5) -sin(theta5) 0 0;
0 0 -1 0;
sin(theta5) cos(theta5) 0 0;
0 0 0 1];
T56 = [cos(theta6) -sin(theta6) 0 0;
0 0 -1 0;
-sin(theta6) -cos(theta6) 0 0;
0 0 0 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.4331
1.0000 0 0.0000 0.4521
0 0 0 1.0000
T0_6 =
0 -1 0 0.1491
0 0 -1 -0.4331
1 0 0 0.4521
0 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系列向量
% 求解关节角1
theta1_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));
% 求解关节角3
m3_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));
% 求解关节角2
ms2_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;
% 求解关节角4
ms4_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);
% 求解关节角5
ms5_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);
% 求解关节角6
ms6_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 0
0.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.0000
0 1.0000 1.0000 0.0000 1.0000 0.0000
1.0000 0.0000 0.0000 0.7071 0.0000 -0.0000
jacobn =
-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 0
1.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
5. 参考资料
- 基于Matlab的PUMA 560运动学与轨迹规划仿真
- 博士论文《协作机器人零力控制与碰撞检测技术研究_陈赛旋》，有关于正逆运动学求解的详细推导过程
- 论文《六自由度模块化机械臂的逆运动学分析》