建立DH模型
→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（一）——DH模型与正运动学】
机器人正运动学
→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（一）——DH模型与正运动学】
机器人逆运动学
△ 求θ 1、θ 2 、θ 3
→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（二）——逆运动学P1】
△ 代数解求θ 4、θ 5 、θ 6
○ 求解θ 4
其中：
※ 计算过程
syms Q1 Q2 Q3 Q4 Q5 Q6 d1 d4 dt a1 a2 a3 nx ny nz ox oy oz ax ay az px py pz
%ZK-500连杆间齐次变换矩阵
T_01 =[ cos(Q1), -sin(Q1), 0, 0
sin(Q1), cos(Q1), 0, 0
0, 0, 1, d1
0, 0, 0, 1];
T_12 =[ cos(Q2), -sin(Q2), 0, a1
0, 0, -1, 0
sin(Q2), cos(Q2), 0, 0
0, 0, 0, 1];
T_23 =[ cos(Q3), -sin(Q3), 0, a2
sin(Q3), cos(Q3), 0, 0
0, 0, 1, 0
0, 0, 0, 1];
T_34 =[ cos(Q4), -sin(Q4), 0, a3
0, 0, -1, -d4
sin(Q4), cos(Q4), 0, 0
0, 0, 0, 1];
T_45 =[ cos(Q5), -sin(Q5), 0, 0
0, 0, 1, 0
-sin(Q5), -cos(Q5), 0, 0
0, 0, 0, 1];
T_56 =[ cos(Q6), -sin(Q6), 0, 0
0, 0, -1, 0
sin(Q6), cos(Q6), 0, 0
0, 0, 0, 1];
T_6t=[ 1 0 0 0
0 1 0 0
0 0 1 dt
0 0 0 1];
% 计算T_06和T_16的逆矩阵
T_06=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1];
T_03=T_01*T_12*T_23;
T_36=T_34*T_45*T_56;
% 计算T_01的逆矩阵
T_03_inv = inv(T_03);
T_03_inv=simplify(T_03_inv)
T_36=simplify(T_36)
结果
T_03_inv =
[ cos(Q2 + Q3)*cos(Q1), cos(Q2 + Q3)*sin(Q1), sin(Q2 + Q3), - a1*cos(Q2 + Q3) - d1*sin(Q2 + Q3) - a2*cos(Q3)]
[-sin(Q2 + Q3)*cos(Q1), -sin(Q2 + Q3)*sin(Q1), cos(Q2 + Q3), a1*sin(Q2 + Q3) - d1*cos(Q2 + Q3) + a2*sin(Q3)]
[ sin(Q1), -cos(Q1), 0, 0]
[ 0, 0, 0, 1]
T_36 =
[cos(Q4)*cos(Q5)*cos(Q6) - sin(Q4)*sin(Q6), - cos(Q6)*sin(Q4) - cos(Q4)*cos(Q5)*sin(Q6), cos(Q4)*sin(Q5), a3]
[ cos(Q6)*sin(Q5), -sin(Q5)*sin(Q6), -cos(Q5), -d4]
[cos(Q4)*sin(Q6) + cos(Q5)*cos(Q6)*sin(Q4), cos(Q4)*cos(Q6) - cos(Q5)*sin(Q4)*sin(Q6), sin(Q4)*sin(Q5), 0]
[ 0, 0, 0, 1]
matlab真好用。。。。
令元素（1,3）和（3,3）等号左右相等得到：
计算过程见前面求θ 2的内容
→→→跳转到 计算过程
化简是手动化简的。。。
记
将式（）等号两边取平方和可得
虽然这里通过
当θ 5 = 0时，机械臂处于奇异位形，关节4和关节6重合成一条直线，此时所有解都是θ 4与θ 6的和或差。在这种情况下θ 4可以任意取值，但一般选择保持其当前值。
matlab代码实现
%% theta4求解
h1=ax*cos(theta1)*cos(theta2+theta3)+ay*sin(theta1)*cos(theta2+theta3)+az*sin(theta2+theta3);
h2=ax*sin(theta1)-ay*cos(theta1);
s5sq=h1^2+h2^2; % 用sin(theta5)的平方作为判断机械臂是否处于奇异位形
% 需要判断theta5是否为0
% 这里判断theta5是否为0其实有点问题，因为当theta5接近0的时候就会出问题，所以这里选择判断sin(theta5)的平方是否小于某一特定值，若小于则认为theta5为0
% 当theta5=0时，theta4保持不变
% 当theta5≠0时，根据theta4可能取到两组值
if s5sq<0.0000001
theta4=q_r(4); %q_r(4)是上一步theta4的值
else
theta4_1=atan2(h2,h1);
theta4_2=atan2(-h2,-h1);
end
○ 求解θ 5
将末端位姿描述矩阵写成
则
计算过程参考θ 4的过程
结果如下
T_04_inv =
[
sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3),
cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) - cos(Q1)*sin(Q4) - cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3),
sin(Q2 + Q3)*cos(Q4),
-cos(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3))
]
[
cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4),
sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4) - cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - cos(Q1)*cos(Q4),
-sin(Q2 + Q3)*sin(Q4),
sin(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3))
]
[
sin(Q2 + Q3)*cos(Q1),
sin(Q2 + Q3)*sin(Q1),
-cos(Q2 + Q3),
d1*cos(Q2 + Q3) - d4 - a1*sin(Q2 + Q3) - a2*sin(Q3)]
[0,0,0,1]
T_left =
[
nx*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - ny*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + nz*sin(Q2 + Q3)*cos(Q4),
ox*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - oy*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + oz*sin(Q2 + Q3)*cos(Q4),
ax*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - ay*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + az*sin(Q2 + Q3)*cos(Q4),
px*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - cos(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3)) - py*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + pz*sin(Q2 + Q3)*cos(Q4)
]
[
nx*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - ny*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - nz*sin(Q2 + Q3)*sin(Q4),
ox*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - oy*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - oz*sin(Q2 + Q3)*sin(Q4),
ax*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - ay*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - az*sin(Q2 + Q3)*sin(Q4),
px*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) + sin(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3)) - py*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - pz*sin(Q2 + Q3)*sin(Q4)
]
[
ny*sin(Q2 + Q3)*sin(Q1) - nz*cos(Q2 + Q3) + nx*sin(Q2 + Q3)*cos(Q1),
oy*sin(Q2 + Q3)*sin(Q1) - oz*cos(Q2 + Q3) + ox*sin(Q2 + Q3)*cos(Q1),
ax*sin(Q2 + Q3)*cos(Q1) - az*cos(Q2 + Q3) + ay*sin(Q2 + Q3)*sin(Q1),
d1*cos(Q2 + Q3) - d4 - pz*cos(Q2 + Q3) - a1*sin(Q2 + Q3) - a2*sin(Q3) + py*sin(Q2 + Q3)*sin(Q1) + px*sin(Q2 + Q3)*cos(Q1)
]
[0,0,0,1]
T_46 =
[ cos(Q5)*cos(Q6), -cos(Q5)*sin(Q6), sin(Q5), 0]
[ sin(Q6), cos(Q6), 0, 0]
[-cos(Q6)*sin(Q5), sin(Q5)*sin(Q6), cos(Q5), 0]
[ 0, 0, 0, 1]
令元素（1,3）和（3,3）等号左右相等得到：
令
则
matlab代码实现
p1=ax*(sin(theta1)*sin(theta4)+cos(theta1)*cos(theta4)*cos(theta2+theta3))+...
ay*(sin(theta1)*cos(theta4)*cos(theta2+theta3)-cos(theta1)*sin(theta4))+...
az*cos(theta4)*sin(theta2+theta3);
p2=ax*cos(theta1)*sin(theta2+theta3)+ay*sin(theta1)*sin(theta2+theta3)-az*cos(theta2+theta3);
theta5_1=atan2(p1,p2);
theta5_2=atan2(-p1,-p2);
○ 求解θ 6
将等式写成
记
其中
计算过程参考θ 4的过程
结果如下
T_05_inv =
[
cos(Q5)*sin(Q1)*sin(Q4) - cos(Q1)*cos(Q2)*sin(Q3)*sin(Q5) - cos(Q1)*cos(Q3)*sin(Q2)*sin(Q5) - cos(Q1)*cos(Q4)*cos(Q5)*sin(Q2)*sin(Q3) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4)*cos(Q5),
%=c5s1s4-c1c2s3s5-c1c3s2s5-c1c4c5s2s3+c1c2c3c4c5
%=c5s1s4-c1s5(c2s3+s2c3)+c1c4c5(c2c3-s2s3)
%=c5s1s4-c1s5s23+c1c4c5c23
cos(Q2)*cos(Q3)*cos(Q4)*cos(Q5)*sin(Q1) - cos(Q2)*sin(Q1)*sin(Q3)*sin(Q5) - cos(Q3)*sin(Q1)*sin(Q2)*sin(Q5) - cos(Q4)*cos(Q5)*sin(Q1)*sin(Q2)*sin(Q3) - cos(Q1)*cos(Q5)*sin(Q4),
%=c2c3c4c5s1-c2s1s3s5-c3s1s2s5-c4c5s1s2s3-c1c5s4
%=s1c4c5(c2c3-s2s3)-s1s5(s2c3+c2s3)-c1c5s4
%=s1c4c5c23-s1s5s23-c1c5s4
cos(Q2)*cos(Q3)*sin(Q5) - sin(Q2)*sin(Q3)*sin(Q5) + cos(Q2)*cos(Q4)*cos(Q5)*sin(Q3) + cos(Q3)*cos(Q4)*cos(Q5)*sin(Q2),
%=c2c3s5-s2s3s5+c2c4c5s3+c3c4c5s2
%=s5(c2c3-s2s3)+c4c5(c2s3+s2c3)
%=s5c23+c4c5s23
d4*sin(Q5) - a3*cos(Q4)*cos(Q5) + a2*sin(Q3)*sin(Q5) + d1*sin(Q2)*sin(Q3)*sin(Q5) - a2*cos(Q3)*cos(Q4)*cos(Q5) - d1*cos(Q2)*cos(Q3)*sin(Q5) + a1*cos(Q2)*sin(Q3)*sin(Q5) + a1*cos(Q3)*sin(Q2)*sin(Q5) - a1*cos(Q2)*cos(Q3)*cos(Q4)*cos(Q5) - d1*cos(Q2)*cos(Q4)*cos(Q5)*sin(Q3) - d1*cos(Q3)*cos(Q4)*cos(Q5)*sin(Q2) + a1*cos(Q4)*cos(Q5)*sin(Q2)*sin(Q3)
]
[
cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)*sin(Q5) - cos(Q1)*cos(Q2)*cos(Q5)*sin(Q3) - cos(Q1)*cos(Q3)*cos(Q5)*sin(Q2) - sin(Q1)*sin(Q4)*sin(Q5) - cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4)*sin(Q5),
cos(Q1)*sin(Q4)*sin(Q5) - cos(Q2)*cos(Q5)*sin(Q1)*sin(Q3) - cos(Q3)*cos(Q5)*sin(Q1)*sin(Q2) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1)*sin(Q5) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)*sin(Q5),
cos(Q2)*cos(Q3)*cos(Q5) - cos(Q5)*sin(Q2)*sin(Q3) - cos(Q2)*cos(Q4)*sin(Q3)*sin(Q5) - cos(Q3)*cos(Q4)*sin(Q2)*sin(Q5),
d4*cos(Q5) + a2*cos(Q5)*sin(Q3) + a3*cos(Q4)*sin(Q5) + d1*cos(Q5)*sin(Q2)*sin(Q3) - d1*cos(Q2)*cos(Q3)*cos(Q5) + a1*cos(Q2)*cos(Q5)*sin(Q3) + a1*cos(Q3)*cos(Q5)*sin(Q2) + a2*cos(Q3)*cos(Q4)*sin(Q5) + a1*cos(Q2)*cos(Q3)*cos(Q4)*sin(Q5) + d1*cos(Q2)*cos(Q4)*sin(Q3)*sin(Q5) + d1*cos(Q3)*cos(Q4)*sin(Q2)*sin(Q5) - a1*cos(Q4)*sin(Q2)*sin(Q3)*sin(Q5)
]
[
cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4),
%=c4s1-c1c2c3s4+c1s2s3s4
%=s1c4-c1s4(c2c3-s2s3)
sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4) - cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - cos(Q1)*cos(Q4),
%=s1s2s3s4-c2c3s1s4-c1c4
%=-c1c4-s1s4c23
-sin(Q2 + Q3)*sin(Q4),
sin(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3))
]
[0,0,0,1]
将以上结果带入（）中，通过观察等式两边的矩阵，令元素（1,1）和（3,1）等号左右相等得到：
这里就不放过程了
解得
matlab代码实现
%% theta6求解
m11=sin(theta1)*sin(theta4)*cos(theta5)-cos(theta1)*sin(theta5)*sin(theta2+theta3)+cos(theta1)*cos(theta4)*cos(theta5)*cos(theta2+theta3);
m12=-cos(theta1)*sin(theta4)*cos(theta5)-sin(theta1)*sin(theta5)*sin(theta2+theta3)+sin(theta1)*cos(theta4)*cos(theta5)*cos(theta2+theta3);
m13=sin(theta5)*cos(theta2+theta3)+cos(theta4)*cos(theta5)*sin(theta2+theta3);
m31=sin(theta1)*cos(theta4)-cos(theta1)*sin(theta4)*cos(theta2+theta3);
m32=-cos(theta1)*cos(theta4)-sin(theta1)*sin(theta4)*cos(theta2+theta3);
m33=-sin(theta4)*sin(theta2+theta3);
theta6_1=atan2(nx*m31+ny*m32+nz*m33,nx*m11+ny*m12+nz*m13);
theta6_2=atan2(-(nx*m31+ny*m32+nz*m33),-(nx*m11+ny*m12+nz*m13));
△ 三轴相交的Pieper解法
【等待补充。。。】
参考《机器人学导论》中内容
△ 机器人逆运动学多解的判断
【等待补充。。。】
可以参考
→→→【机器人逆解中存在多个解，怎么对每个解进行筛选和判断】
机器人雅可比矩阵
雅可比的内容有点长，单独开了一篇
评论（0）
您还未登录，请登录后发表或查看评论