Industrial Robot Kinematics and Matlab Forward and Inverse Solution Algorithm Study Notes (Résumé d'une session plénière avec cœur) (3)

---

CSDN m'a rappelé qu'il y avait trop de mots et que je ne pouvais pas publier un article, j'ai donc dû diviser x2. . .

à propos

• Modèle standard DH
• Modèle DH amélioré
• cinématique robotique

Pour plus de détails, voir le premier article :

→→→ [Industrial Robot Kinematics and Matlab Forward and Inverse Solution Algorithm Study Notes (Résumé d'une séance plénière) (1)]

à propos

• Solution de cinématique inverse du robot ${je}_{}$、${je}_{}$、${je}_{}$

Voir le deuxième article pour plus de détails

→→→ [Industrial Robot Kinematics and Matlab Forward and Inverse Solution Algorithm Study Notes (Résumé d'une séance plénière) (2)]

---

❤ 2023.6.27 ❤

Cinématique inverse du robot

△ Solution algébrique de ${je}_{}$、${je}_{}$、${je}_{}$

○ Résolution pour ${je}_{}$

La description des deux côtés de la matrice à la fin de la formule est ${{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}$得：
$${{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}{}_6^{}J{=}_4^{}J\left({je}_{}\right){}_5^{}J\left({je}_{}\right){}_6^{}J\left({je}_{}\right)$$

Par exemple :
$${{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}=\left[\begin{array}{cccc}{c}_{}{c}_{}& s_{}{c}_{}& s_{}& -{un}_{}{c}_{}-d_{}{s}_{}-{un}_{}{c}_{}\\ -c{\_}_{}{s}_{}& -s_{}{s}_{}& c_{}& {un}_{}{s}_{}-d_{}{c}_{}+{un}_{}{s}_{}\\ s_{}& -c{\_}_{}& 0& 0\\ & & & \end{array}\right]$$
$${}_4^{}J\left({je}_{}\right){}_5^{}J\left({je}_{}\right){}_6^{}J\left({je}_{}\right)=\left[\begin{array}{cccc}{c}_{}{c}_{}{c}_{}-s_{}{s}_{}& -c{\_}_{}{s}_{}-c_{}{c}_{}{s}_{}& c_{}{s}_{}& {un}_{}\\ c_{}{s}_{}& -s_{}{s}_{}& -c{\_}_{}& -d_{}\\ c_{}{s}_{}+c_{}{c}_{}{s}_{}& c_{}{c}_{}-c_{}{s}_{}{s}_{}& s_{}{s}_{}& 0\\ & & & \end{array}\right]$$

---

• ※ processus de calcul

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)

résultat

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 est vraiment facile à utiliser. . . .

---

Soit les signes d'égalité des éléments (1,3) et (3,3) égaux pour obtenir :
$$\begin{gathered} {un}_{}{c}_{}{c}_{}+{un}_{}{s}_{}{c}_{}+{un}_{}{s}_{}=c_{}{s}_{} \\ {un}_{}{s}_{}-{un}_{}{c}_{}=s_{}{s}_{} \end{gathered}$$

Pour le processus de calcul, voir le recto pour trouver ${je}_{}$Le contenu de
→→→ passe au processus de calcul
La simplification est une simplification manuelle. . .

记
$$\begin{gathered} h_{}={un}_{}{c}_{}{c}_{}+{un}_{}{s}_{}{c}_{}+{un}_{}{s}_{} \\ {h}_{}={un}_{}{s}_{}-{un}_{}{c}_{} \end{gathered}$$

Prenez la somme des carrés des deux côtés de l'équation () pour obtenir
$$s_5^{}=h_1^{}+h_2^{}$$

Bien qu'ici par
$${je}_{}=\arcsin \left(\pm \sqrt{\left(h_1^{}+h_2^{}\right)}\right)$$
peut trouver${je}_{}$Mais selon la déclaration précédente, il est préférable d'utiliser la fonction atan2() pour résoudre, donc les résultats ici ne sont utilisés que pour juger ${je}_{}$Tend-il vers 0

D'après la vérification du programme, lorsque ${je}_{}$Lorsque la valeur est 0, en raison de l'influence des erreurs d'arrondi informatique, le ${je}_{}$La valeur de n'est pas 0, mais une très petite valeur.
Si vous jugez directement ${je}_{}$S'il est nul, cela invalidera non seulement le jugement, mais affectera également ${je}_{}$La valeur de , donc ici bien qu'en écrivant le jugement ${je}_{}$Si c'est 0, mais c'est jugé dans le programme $s_5^{}$est inférieur à une certaine valeur.

当${je}_{}\ne 0$时
$${je}_{}=Atan2\left(h_{},h_{}\right)$$
θ${je}_{}=0$ , le manipulateur est dans une configuration singulière, l'articulation 4 et l'articulation 6 se chevauchent en une ligne droite, et toutes les solutions à ce moment sont${je}_{}$Étant donné ${je}_{}$somme ou différence. Dans ce cas ${je}_{}$Vous pouvez prendre n'importe quelle valeur, mais choisissez généralement de conserver sa valeur actuelle.

---

• Implémentation du code 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

---

○ Résolution pour ${je}_{}$

Écrivez la matrice de description de la pose terminale sous la forme
$${{}_4^{}J}^{-1}{{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}{}_6^{}J{=}_6^{}J\left({je}_{}\right){}_5^{}J\left({je}_{}\right)$$

则
$$\begin{gathered} {{}_4^{}J}^{-1}{{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}= \\ \left[\begin{array}{cccc}{s}_{}{s}_{}+c_{}{c}_{}{c}_{}& s_{}{c}_{}{c}_{}-c_{}{s}_{}& c_{}{s}_{}& -{un}_{}{c}_{}{c}_{}-{un}_{}{c}_{}{c}_{}-{un}_{}{c}_{}-d_{}{c}_{}{s}_{}\\ s_{}{c}_{}-c_{}{s}_{}{c}_{}& -s_{}{s}_{}{c}_{}-c_{}{c}_{}& -s_{}{s}_{}& {un}_{}{s}_{}{c}_{}+{un}_{}{c}_{}{c}_{}+{un}_{}{s}_{}+d_{}{s}_{}{s}_{}\\ c_{}{s}_{}& s_{}{s}_{}& -c{\_}_{}& -{un}_{}{s}_{}-{un}_{}{s}_{}+d_{}{c}_{}-d_{}\\ & & & \end{array}\right] \end{gathered}$$
$${}_5^{}J\left({je}_{}\right){}_6^{}J\left({je}_{}\right)=\left[\begin{array}{cccc}{c}_{}{c}_{}& -c{\_}_{}{s}_{}& s_{}& 0\\ s_{}& c_{}& 0& 0\\ -s_{}{c}_{}& s_{}{s}_{}& c_{}& 0\\ & & & \end{array}\right]$$

---

• Le processus de calcul se réfère à ${je}_{}$le processus de

Le résultat est le suivant

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）等号左右相等得到：
$$\begin{gathered} {un}_{}\left(s_{}{s}_{}+c_{}{c}_{}{c}_{}\right)+{un}_{}\left(s_{}{c}_{}{c}_{}-c_{}{s}_{}\right)+{un}_{}\left(c_{}{s}_{}\right)=s_{} \\ {un}_{}\left(c_{}{s}_{}\right)+{un}_{}\left(s_{}{s}_{}\right)+{un}_{}\left(-c{\_}_{}\right)=c_{} \end{gathered}$$
令
$$\begin{gathered} p_{}={un}_{}\left(s_{}{s}_{}+c_{}{c}_{}{c}_{}\right)+{un}_{}\left(s_{}{c}_{}{c}_{}-c_{}{s}_{}\right)+{un}_{}\left(c_{}{s}_{}\right) \\ {p}_{}={un}_{}\left(c_{}{s}_{}\right)+{un}_{}\left(s_{}{s}_{}\right)+{un}_{}\left(-c{\_}_{}\right) \end{gathered}$$

则
$${je}_{}=Atan2\left(p_{},p_{}\right)$$

---

• Implémentation du code 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);

---

○ Résolution pour ${je}_{}$

Écrivez l'équation sous la forme
$${{}_5^{}J}^{-1}{{}_4^{}J}^{-1}{{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}{}_6^{}J{=}_6^{}J\left({je}_{}\right)$$
记
$${{}_5^{}J}^{-1}{{}_4^{}J}^{-1}{{}_3^{}J}^{-1}{{}_2^{}J}^{-1}{{}_1^{}J}^{-1}=\left[\begin{array}{cccc}{m}_{}& m_{}& m_{}& m_{}\\ m_{}& m_{}& m_{}& m_{}\\ m_{}& m_{}& m_{}& m_{}\\ & & & \end{array}\right]$$

dans

$$\begin{array}{rl}{m}_{}& =s_{}{s}_{}{c}_{}-c_{}{s}_{}{s}_{}+c_{}{c}_{}{c}_{}{c}_{}\\ m_{}& =-c{\_}_{}{s}_{}{c}_{}-s_{}{s}_{}{s}_{}+s_{}{c}_{}{c}_{}{c}_{}\\ m_{}& =s_{}{c}_{}+c_{}{c}_{}{s}_{}\\ m_{}& =s_{}{c}_{}-c_{}{s}_{}{c}_{}\\ m_{}& =-c{\_}_{}{c}_{}-s_{}{s}_{}{c}_{}\\ m_{}& =-s_{}{s}_{}\end{array}$$

---

• Le processus de calcul se réfère à ${je}_{}$le processus de

Le résultat est le suivant

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]

---

Amenez les résultats ci-dessus dans (), en observant la matrice des deux côtés de l'équation, rendez les éléments (1,1) et (3,1) égaux entre eux pour obtenir :

$$n_{}{m}_{}+n_{}{m}_{}+n_{}{m}_{}=c_{}{n}_{}{m}_{}+n_{}{m}_{}+n_{}{m}_{}=s_{}$$

Je ne laisserai pas le processus aller ici

Les solutions doivent

$${je}_{}=Atan2\left(n_{}{m}_{}+n_{}{m}_{}+n_{}{m}_{},n_{}{m}_{}+n_{}{m}_{}+n_{}{m}_{}\right)$$

---

• Implémentation du code 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));

---

△ Solution de Pieper d'intersection à trois axes

[En attente de suppléments. . . 】

Reportez-vous au contenu dans "Introduction à la robotique"

---

△ Jugement multi-solutions de la cinématique inverse du robot

[En attente de suppléments. . . 】

peut faire référence à

→→→ [Il existe plusieurs solutions dans la solution inverse du robot, comment filtrer et juger chaque solution]

---

Matrice jacobienne robotique

△ Matrice jacobienne de la vitesse du robot

En ce qui concerne le contenu de la matrice jacobienne, pour être honnête, j'ai été troublé par le manuel et j'ai senti que je pouvais avoir le syndrome de la dyslexie. . . . Ensuite, j'ai trouvé ce tutoriel à la station B, il semble très simple et pratique
→→→ 【4-1 matrice jacobienne de la vitesse du robot】

Voici mes notes d'étude

○ Concepts liés à la matrice jacobienne

• Concept de matrice jacobienne

La matrice jacobienne (Jacobian) est la relation de cartographie linéaire entre la vitesse spatiale de fonctionnement du robot et la vitesse spatiale articulaire, et elle peut également représenter la relation de transfert entre les deux espaces.

• Représentation de la pose finale du robot

matrice de transformation homogène

• Méthode de réglage de l'angle

La méthode de spécification de l'attitude en utilisant le système de coordonnées de référence relatif ou le système de coordonnées de mouvement relatif pour effectuer trois rotations consécutives

Normalement, l'angle de rotation autour du système de coordonnées est l'angle d'Euler, et la séquence doit être prise en compte, mais l'angle de rotation ici est considéré comme l'angle avec l'axe des coordonnées

Par conséquent, lors de l'utilisation de la méthode de réglage d'angle pour exprimer la pose terminale du robot, l'équation cinématique du robot peut être écrite comme

---

○ Prenons un robot planaire à deux liens comme exemple pour illustrer la matrice jacobienne

Robot à joint planaire à deux maillons comme indiqué sur la figure

La position finale du robot peut être exprimée comme

Différencier la formule modifiée pour obtenir

D'après la formule précédente, on obtient

alors

---