CSDN提示我字数太多,一篇发不下,只好拆分开x2。。。
❤ 2023.6.27 ❤
→→→【工业机器人运动学与Matlab正逆解算法学习笔记(用心总结一文全会)(一)——DH模型与正运动学】
→→→【工业机器人运动学与Matlab正逆解算法学习笔记(用心总结一文全会)(一)——DH模型与正运动学】
→→→【工业机器人运动学与Matlab正逆解算法学习笔记(用心总结一文全会)(二)——逆运动学P1】
将式末端位姿描述矩阵两边同时左 3 2 T − 1 2 1 T − 1 1 0 T − 1 {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1} 32T−1 21T−1 10T−1得:
3 2 T − 1 2 1 T − 1 1 0 T − 1 6 0 T = 4 3 T ( θ 4 ) 5 4 T ( θ 5 ) 6 5 T ( θ 6 ) \ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}\ _6^0T=_4^3T\left(\theta_4\right)\ _5^4T\left(\theta_5\right)\ _6^5T\left(\theta_6\right) 32T−1 21T−1 10T−1 60T=43T(θ4) 54T(θ5) 65T(θ6)
其中:
3 2 T − 1 2 1 T − 1 1 0 T − 1 = [ c 1 c 23 s 1 c 23 s 23 − a 1 c 23 − d 1 s 23 − a 2 c 3 − c 1 s 23 − s 1 s 23 c 23 a 1 s 23 − d 1 c 23 + a 2 s 3 s 1 − c 1 0 0 0 0 0 1 ] \ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}=\left[\begin{matrix}c_1c_{23}&s_1c_{23}&s_{23}&-a_1c_{23}-d_1s_{23}-a_2c_3\\-c_1s_{23}&-s_1s_{23}&c_{23}&a_1s_{23}-d_1c_{23}+a_2s_3\\s_1&-c_1&0&0\\0&0&0&1\\\end{matrix}\right] 32T−1 21T−1 10T−1= c1c23−c1s23s10s1c23−s1s23−c10s23c2300−a1c23−d1s23−a2c3a1s23−d1c23+a2s301
4 3 T ( θ 4 ) 5 4 T ( θ 5 ) 6 5 T ( θ 6 ) = [ c 4 c 5 c 6 − s 4 s 6 − c 6 s 4 − c 4 c 5 s 6 c 4 s 5 a 3 c 6 s 5 − s 5 s 6 − c 5 − d 4 c 4 s 6 + c 5 c 6 s 4 c 4 c 6 − c 5 s 4 s 6 s 4 s 5 0 0 0 0 1 ] {_4^3}T\left(\theta_4\right)\ _5^4T\left(\theta_5\right)\ _6^5T\left(\theta_6\right)=\left[\begin{matrix}c_4c_5c_6-s_4s_6&-c_6s_4-c_4c_5s_6&c_4s_5&a_3\\c_6s_5&-s_5s_6&-c_5&-d_4\\c_4s_6+c_5c_6s_4&c_4c_6-c_5s_4s_6&s_4s_5&0\\0&0&0&1\\\end{matrix}\right] 43T(θ4) 54T(θ5) 65T(θ6)= c4c5c6−s4s6c6s5c4s6+c5c6s40−c6s4−c4c5s6−s5s6c4c6−c5s4s60c4s5−c5s4s50a3−d401
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)等号左右相等得到:
a x c 1 c 23 + a y s 1 c 23 + a z s 23 = c 4 s 5 a x s 1 − a y c 1 = s 4 s 5 a_xc_1c_{23}+a_ys_1c_{23}+a_zs_{23}=c_4s_5\\ a_xs_1-a_yc_1=s_4s_5 axc1c23+ays1c23+azs23=c4s5axs1−ayc1=s4s5
计算过程见前面求 θ 2 \theta_2 θ2的内容
→→→跳转到 计算过程
化简是手动化简的。。。
记
h 1 = a x c 1 c 23 + a y s 1 c 23 + a z s 23 h 2 = a x s 1 − a y c 1 h_1=a_xc_1c_{23}+a_ys_1c_{23}+a_zs_{23}\\ h_2=a_xs_1-a_yc_1 h1=axc1c23+ays1c23+azs23h2=axs1−ayc1
将式()等号两边取平方和可得
s 5 2 = h 1 2 + h 2 2 s_5^2=h_1^2+h_2^2 s52=h12+h22
虽然这里通过
θ 5 = arcsin ( ± ( h 1 2 + h 2 2 ) ) \theta_5=\arcsin{\left(\pm\sqrt{\left(h_1^2+h_2^2\right)}\right)} θ5=arcsin(±(h12+h22))
就能求出 θ 5 \theta_5 θ5但是根据前面的说法,最好用atan2()函数来求解,所以这里的结果只用来判断 θ 5 \theta_5 θ5是不是趋向于0
根据程序验证,当 θ 5 \theta_5 θ5取值为0时,由于电脑的舍入误差的影响,经过计算的 θ 5 \theta_5 θ5的数值并不为0,而是一个极小的值。
如果直接判断 θ 5 \theta_5 θ5是否为零,则不但会使判断失效,还会影响 θ 4 \theta_4 θ4的取值,因此这里虽然写作判断 θ 5 \theta_5 θ5是否为0,但在程序中是判断 s 5 2 s_5^2 s52的是否小于某一特定值。
当 θ 5 ≠ 0 \theta_5\neq0 θ5=0时
θ 4 = A t a n 2 ( h 2 , h 1 ) \theta_4=\mathrm{Atan}2\left(h_2,h_1\right) θ4=Atan2(h2,h1)
当 θ 5 = 0 \theta_5=0 θ5=0时,机械臂处于奇异位形,关节4和关节6重合成一条直线,此时所有解都是 θ 4 \theta_4 θ4与 θ 6 \theta_6 θ6的和或差。在这种情况下 θ 4 \theta_4 θ4可以任意取值,但一般选择保持其当前值。
%% 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
将末端位姿描述矩阵写成
4 3 T − 1 3 2 T − 1 2 1 T − 1 1 0 T − 1 6 0 T = 6 5 T ( θ 6 ) 5 4 T ( θ 6 ) \ \ {{_4^3}T}^{-1}\ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}\ _6^0T=_6^5T\left(\theta_6\right)\ {_5^4}T\left(\theta_6\right) 43T−1 32T−1 21T−1 10T−1 60T=65T(θ6) 54T(θ6)
则
4 3 T − 1 3 2 T − 1 2 1 T − 1 1 0 T − 1 = [ s 1 s 4 + c 1 c 4 c 23 s 1 c 4 c 23 − c 1 s 4 c 4 s 23 − a 1 c 4 c 23 − a 2 c 3 c 4 − a 3 c 4 − d 1 c 4 s 23 s 1 c 4 − c 1 s 4 c 23 − s 1 s 4 c 23 − c 1 c 4 − s 4 s 23 a 1 s 4 c 23 + a 2 c 3 c 4 + a 3 s 4 + d 1 s 4 s 23 c 1 s 23 s 1 s 23 − c 23 − a 1 s 23 − a 2 s 3 + d 1 c 23 − d 4 0 0 0 1 ] \ {{_4^3}T}^{-1}\ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}=\\\left[\begin{matrix}s_1s_4+c_1c_4c_{23}&s_1c_4c_{23}-c_1s_4&c_4s_{23}&-a_1c_4c_{23}-a_2c_3c_4-a_3c_4-d_1c_4s_{23}\\s_1c_4-c_1s_4c_{23}&-s_1s_4c_{23}-c_1c_4&-s_4s_{23}&a_1s_4c_{23}+a_2c_3c_4+a_3s_4+d_1s_4s_{23}\\c_1s_{23}&s_1s_{23}&-c_{23}&-a_1s_{23}-a_2s_3+d_1c_{23}-d_4\\0&0&0&1\\\end{matrix}\right] 43T−1 32T−1 21T−1 10T−1= s1s4+c1c4c23s1c4−c1s4c23c1s230s1c4c23−c1s4−s1s4c23−c1c4s1s230c4s23−s4s23−c230−a1c4c23−a2c3c4−a3c4−d1c4s23a1s4c23+a2c3c4+a3s4+d1s4s23−a1s23−a2s3+d1c23−d41
5 4 T ( θ 5 ) 6 5 T ( θ 6 ) = [ c 5 c 6 − c 5 s 6 s 5 0 s 6 c 6 0 0 − s 5 c 6 s 5 s 6 c 5 0 0 0 0 1 ] {_5^4}T\left(\theta_5\right)\ {_6^5}T\left(\theta_6\right)=\left[\begin{matrix}c_5c_6&-c_5s_6&s_5&0\\s_6&c_6&0&0\\-s_5c_6&s_5s_6&c_5&0\\0&0&0&1\\\end{matrix}\right] 54T(θ5) 65T(θ6)= c5c6s6−s5c60−c5s6c6s5s60s50c500001
结果如下
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)等号左右相等得到:
a x ( s 1 s 4 + c 1 c 4 c 23 ) + a y ( s 1 c 4 c 23 − c 1 s 4 ) + a z ( c 4 s 23 ) = s 5 a x ( c 1 s 23 ) + a y ( s 1 s 23 ) + a z ( − c 23 ) = c 5 a_x\left(s_1s_4+c_1c_4c_{23}\right)+a_y\left(s_1c_4c_{23}-c_1s_4\right)+a_z\left(c_4s_{23}\right)=s_5\\ a_x\left(c_1s_{23}\right)+a_y\left(s_1s_{23}\right)+a_z\left(-c_{23}\right)=c_5 ax(s1s4+c1c4c23)+ay(s1c4c23−c1s4)+az(c4s23)=s5ax(c1s23)+ay(s1s23)+az(−c23)=c5
令
p 1 = a x ( s 1 s 4 + c 1 c 4 c 23 ) + a y ( s 1 c 4 c 23 − c 1 s 4 ) + a z ( c 4 s 23 ) p 2 = a x ( c 1 s 23 ) + a y ( s 1 s 23 ) + a z ( − c 23 ) p_1=a_x\left(s_1s_4+c_1c_4c_{23}\right)+a_y\left(s_1c_4c_{23}-c_1s_4\right)+a_z\left(c_4s_{23}\right)\\ p_2=a_x\left(c_1s_{23}\right)+a_y\left(s_1s_{23}\right)+a_z\left(-c_{23}\right) p1=ax(s1s4+c1c4c23)+ay(s1c4c23−c1s4)+az(c4s23)p2=ax(c1s23)+ay(s1s23)+az(−c23)
则
θ 5 = A t a n 2 ( p 1 , p 2 ) \theta_5=\mathrm{Atan}2\left(p_1,p_2\right) θ5=Atan2(p1,p2)
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);
将等式写成
5 4 T − 1 4 3 T − 1 3 2 T − 1 2 1 T − 1 1 0 T − 1 6 0 T = 6 5 T ( θ 6 ) \ {{_5^4}T}^{-1}\ {{_4^3}T}^{-1}\ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}\ _6^0T=_6^5T\left(\theta_6\right) 54T−1 43T−1 32T−1 21T−1 10T−1 60T=65T(θ6)
记
5 4 T − 1 4 3 T − 1 3 2 T − 1 2 1 T − 1 1 0 T − 1 = [ m 11 m 12 m 13 m 14 m 21 m 22 m 23 m 24 m 31 m 32 m 33 m 34 0 0 0 1 ] \ {{_5^4}T}^{-1}\ {{_4^3}T}^{-1}\ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}=\left[\begin{matrix}m_{11}&m_{12}&m_{13}&m_{14}\\m_{21}&m_{22}&m_{23}&m_{24}\\m_{31}&m_{32}&m_{33}&m_{34}\\0&0&0&1\\\end{matrix}\right] 54T−1 43T−1 32T−1 21T−1 10T−1= m11m21m310m12m22m320m13m23m330m14m24m341
其中
m 11 = s 1 s 4 c 5 − c 1 s 5 s 23 + c 1 c 4 c 5 c 23 m 12 = − c 1 s 4 c 5 − s 1 s 5 s 23 + s 1 c 4 c 5 c 23 m 13 = s 5 c 23 + c 4 c 5 s 23 m 31 = s 1 c 4 − c 1 s 4 c 23 m 32 = − c 1 c 4 − s 1 s 4 c 23 m 33 = − s 4 s 23 \begin{aligned} m_{11}&=s_1s_4c_5-c_1s_5s_{23}+c_1c_4c_5c_{23}\\ m_{12}&=-c_1s_4c_5-s_1s_5s_{23}+s_1c_4c_5c_{23}\\ m_{13}&=s_5c_{23}+c_4c_5s_{23}\\ m_{31}&=s_1c_4-c_1s_4c_{23}\\ m_{32}&=-c_1c_4-s_1s_4c_{23}\\ m_{33}&=-s_4s_{23}\\ \end{aligned} m11m12m13m31m32m33=s1s4c5−c1s5s23+c1c4c5c23=−c1s4c5−s1s5s23+s1c4c5c23=s5c23+c4c5s23=s1c4−c1s4c23=−c1c4−s1s4c23=−s4s23
结果如下
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)等号左右相等得到:
n x m 11 + n y m 12 + n z m 13 = c 6 n x m 31 + n y m 32 + n z m 33 = s 6 n_xm_{11}+n_ym_{12}+n_zm_{13}=c_6 n_xm_{31}+n_ym_{32}+n_zm_{33}=s_6 nxm11+nym12+nzm13=c6nxm31+nym32+nzm33=s6
这里就不放过程了
解得
θ 6 = A t a n 2 ( n x m 31 + n y m 32 + n z m 33 , n x m 11 + n y m 12 + n z m 13 ) \theta_6=\mathrm{Atan}2\left(n_xm_{31}+n_ym_{32}+n_zm_{33},n_xm_{11}+n_ym_{12}+n_zm_{13}\right) θ6=Atan2(nxm31+nym32+nzm33,nxm11+nym12+nzm13)
%% 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));
【等待补充。。。】
参考《机器人学导论》中内容
【等待补充。。。】
可以参考
→→→【机器人逆解中存在多个解,怎么对每个解进行筛选和判断】
雅可比的内容有点长,单独开了一篇
→→→【工业机器人运动学与Matlab正逆解算法学习笔记(用心总结一文全会)(四)——雅可比矩阵】