Industrial Robot Kinematics and Matlab Forward and Inverse Solution Algorithm Study Notes (Summary of a plenary session with heart) (2)

---

CSDN reminds me that there are too many words, and I can’t post one article, so I have to split it up

about

• Standard DH model
• Improved DH model
• robot kinematics

For details, see the first article:

→→→ [Industrial Robot Kinematics and Matlab Forward and Inverse Solution Algorithm Study Notes (Summary of a Plenary Session) (1)]

❤ 2023.6.19 ❤

Robot Inverse Kinematics

※ Meaning and connection of algebraic solution, geometric solution, analytical solution (closed solution), numerical solution

Speaking of which, I was quite dizzy when I first encountered these concepts. After communicating with others, I found that I was not the only one who was dizzy. People often confuse the concepts and make mistakes.

Next, please ChatGPT to answer first

Then I summarize

Both algebraic solution and geometric solution belong to analytical solution (closed solution) . Their characteristic is that specific equations can be listed and the obtained results are accurate. The difference is that the algebraic solution is based on the characteristics of the matrix, and the unknown is solved by establishing a system of equations through the method of equal specific elements; while the geometric solution is to establish a system of equations through the geometric relationship of the robot to solve the unknown.

Only robots with specific configurations have analytical solutions, but fortunately, most of the robots we usually use are of specific configurations.

The numerical solution corresponds to the analytical solution, and the characteristic is that the calculation process relies on iteration, and the obtained result is an approximate solution. It is applicable to all robots, including special configurations and general configurations, and is mostly used on robots that do not have an analytical solution.

---

For the ZK-500 robot, because it belongs to a special configuration in which the rear three axes intersect at one point, the analytical method is used here to find the inverse solution of the robot.

First, the pose description matrix is ​​written as follows

$${}_6^{0}T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]={}_1^{0}T\left(i_1\right){}_2^{1}T\left(i_2\right){}_3^{2}T\left(i_3\right){}_4^{3}T\left(i_4\right){}_5^{4}T\left(i_5\right){}_6^{5}T\left(i_6\right)$$

△ Algebraic solution for $i_1$、$i_2$、$i_3$

※Reference materials

→→→ [MD-H model kinematics forward solution, inverse solution and verification of attitude angle]
This is really a treasure author

→→→Introduction to Robotics (John J Craig)

---

※ About why use atan2() function to solve

$Atan2\left(\right)$ , this function is the same as$Atan\left(\right)$ is the same as arctangent, but its value range is$(-\pi ,\pi )$ , you can get the quadrant corresponding to the angle, which is the enhanced version of$Atan\left(\right)$。

For details, please refer to
→→→ atan2()
→→→ atan2 vs atan

As for why this function is used, I was also very confused at this point. After calculating the sin value or cos value of some corners, theoretically, the arcsine/arccosine function can be used to solve the corresponding angle. Why bother? Find sin and cos at the same time, and then divide and use atan2() to solve it?

After searching for information, I saw this article

→→→ [Why is it better to use the double-variable arctangent function atan2 instead of the arc/cosine function for the inverse solution of robot kinematics? 】

Let me sum it up, the general meaning is that it is not impossible to use the arcsine/arccosine function directly, but their range of values ​​is relatively small, and the joint range of the robot can reach ( − π , π ) (-\pi,\pi $(-\pi ,\pi )$ , if you use the arcsine/arccosine function to solve it, you need to fully consider the value range of different poses and then make a judgment. And the value range of the atan2() function is$(-\pi ,\pi )$ , in many cases the extra judgment process is omitted.

And the arcsin()/arccos() function lacks definition at some special points, which will lead to the error amplification of the calculation result, but the atan2() function does not have this problem.

atan2() function definition

• Problems needing attention when using atan2() function

But according to the article [Why is it best to use the two-variable arctangent function atan2 instead of the arc/cosine function for the inverse solution of robot kinematics? 】 As mentioned, it is not foolproof after using atan2(), and some problems need to be paid attention to:
1. The definition when x=0, y=0
2. The jump of the value of the function when x or y passes through 0

See the above article for specific solutions

---

○ Solving for $i_1$

First, include $i_1$part of the shift to the left side of the equation

$${\left[{}_1^{0}T\left(i_1\right)\right]}^{-1}{}_6^{0}T={}_2^{1}T\left(i_2\right){}_3^{2}T\left(i_3\right){}_4^{3}T\left(i_4\right){}_5^{4}T\left(i_5\right){}_6^{5}T\left(i_6\right)$$

1 0 T {_1^0} T${}_1^{0}To\; find\; the\; inverse\; of\; T$ , the equation is written as follows

$$\left[\begin{array}{cccc}{c}_1& s_1& 0& 0\\ -s_1& c_1& 0& 0\\ 0& 0& 1& -d_1\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]={}_6^{1}T$$

Solve for ${}_6^{1}After\; T$ , by observing the matrix on both sides of the equation, make the elements (2,4) equal to get

$$-s_1{p}_x+c_1{p}_y=0$$

---

• ※ 【How to get this equation】

There is a lot of content in this solution process, so let me use matlab to calculate it. . .

• Matrix on the left side of the equation (each square bracket represents a row)

[nx*cos(Q1) + ny*sin(Q1), 
 ox*cos(Q1) + oy*sin(Q1), 
 ax*cos(Q1) + ay*sin(Q1), 
 px*cos(Q1) + py*sin(Q1)]
 
[ny*cos(Q1) - nx*sin(Q1), 
 oy*cos(Q1) - ox*sin(Q1), 
 ay*cos(Q1) - ax*sin(Q1), 
 py*cos(Q1) - px*sin(Q1)]
 
[nz,oz,az,pz - d1]

[0,0,0,1]

• Matrix on the right side of the equation (each square bracket represents a row)

[- cos(Q6)*(sin(Q2 + Q3)*sin(Q5) - cos(Q2 + Q3)*cos(Q4)*cos(Q5)) - cos(Q2 + Q3)*sin(Q4)*sin(Q6),   
 sin(Q6)*(sin(Q2 + Q3)*sin(Q5) - cos(Q2 + Q3)*cos(Q4)*cos(Q5)) - cos(Q2 + Q3)*cos(Q6)*sin(Q4), 
 sin(Q2 + Q3)*cos(Q5) + cos(Q2 + Q3)*cos(Q4)*sin(Q5), 
 a1 + a3*cos(Q2 + Q3) + d4*sin(Q2 + Q3) + a2*cos(Q2)]
 
[- cos(Q4)*sin(Q6) - cos(Q5)*cos(Q6)*sin(Q4),
 cos(Q5)*sin(Q4)*sin(Q6) - cos(Q4)*cos(Q6),
 sin(Q4)*sin(Q5),
 0]
 
[cos(Q6)*(cos(Q2 + Q3)*sin(Q5) + sin(Q2 + Q3)*cos(Q4)*cos(Q5)) - sin(Q2 + Q3)*sin(Q4)*sin(Q6), 
 - sin(Q6)*(cos(Q2 + Q3)*sin(Q5) + sin(Q2 + Q3)*cos(Q4)*cos(Q5)) - sin(Q2 + Q3)*cos(Q6)*sin(Q4), 
 sin(Q2 + Q3)*cos(Q4)*sin(Q5) - cos(Q2 + Q3)*cos(Q5),      
 a3*sin(Q2 + Q3) - d4*cos(Q2 + Q3) + a2*sin(Q2)]
 
[0,0,0,1]
 

Sure enough, it contains $i_1$(here $Q_1$) among the elements (2,4) is the best calculation. . .

---

In the formula, $p_x$、$p_y$are known, then $i_1$Can be expressed as:

$$i_1=Atan2\left(p_y,p_x\right)$$

It should be noted that the input variable is first $p_y$then $p_x$For tan ⁡ θ = pypx \tan{\theta}=\frac{p_y}{p_x
}$\tan i=\frac{p_y}{p_x}$, that is, enter the numerator first, and then the denominator.

There are two sets of solutions here:
$$i_1=Atan2\left(p_y,p_x\right)$$
或$$i_1^{\prime }=Atan2\left(-p_y,-p_x\right)$$

---

• Matlab code implementation

%% theta1求解
%theta1可能的解有两组
theta1_1=atan2(py,px);
theta1_2=atan2(-py,-px);

---

○ Solving for $i_3$

Let the matrix elements (1,4) and (3,4) in the above equation be equal to:

$$\begin{gathered} c_1{p}_x+s_1{p}_y=a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23} \\ {p}_z-d_1=a_2{s}_2+a_3{s}_{23}-d_4{c}_{23} \end{gathered}$$

The sum of the squares around the equal sign is
$$m^2+n^2=a_2^2+a_3^2+d_4^2+2a{\_}_2\left(a_3{c}_3+d_4{s}_3\right)$$
among them:
$$\begin{gathered} m=c_1{p}_x+s_1{p}_y-a_1 \\ n=p_z-d_1 \end{gathered}$$

---

About the calculation process of this step:

I demonstrate with this code:

syms a1 a2 a3 d4 theta2 theta3

m=a2*cos(theta2)+a3*cos(theta2+theta3)+d4*sin(theta2+theta3);
n=a2*sin(theta2)+a3*sin(theta2+theta3)-d4*cos(theta2+theta3);

k=m^2 + n^2

% 展开表达式为多项式形式
K1 = expand(m^2 + n^2) 
%K = collect(K, [a1 a2 a3 d4]);  

% 化简多项式
K2=simplify(K1)

% 整理多项式的项
k3=collect(K2)

result:

k =
(a3*cos(theta2 + theta3) + d4*sin(theta2 + theta3) + a2*cos(theta2))^2 + (a3*sin(theta2 + theta3) - d4*cos(theta2 + theta3) + a2*sin(theta2))^2
 
K1 =
a2^2*cos(theta2)^2 + a2^2*sin(theta2)^2 + a3^2*cos(theta2)^2*cos(theta3)^2 + d4^2*cos(theta2)^2*cos(theta3)^2 + a3^2*cos(theta2)^2*sin(theta3)^2 + a3^2*cos(theta3)^2*sin(theta2)^2 + d4^2*cos(theta2)^2*sin(theta3)^2 + d4^2*cos(theta3)^2*sin(theta2)^2 + a3^2*sin(theta2)^2*sin(theta3)^2 + d4^2*sin(theta2)^2*sin(theta3)^2 + 2*a2*a3*cos(theta2)^2*cos(theta3) + 2*a2*a3*cos(theta3)*sin(theta2)^2 + 2*a2*d4*cos(theta2)^2*sin(theta3) + 2*a2*d4*sin(theta2)^2*sin(theta3)
 
K2 =
a2^2 + 2*cos(theta3)*a2*a3 + 2*sin(theta3)*a2*d4 + a3^2 + d4^2
 
k3 =
a2^2 + 2*cos(theta3)*a2*a3 + 2*sin(theta3)*a2*d4 + a3^2 + d4^2

---

令$H=\left(m^2+n^2-a_2^2-a_3^2-d_4^2\right)/{2a}_2$
Then the equation can be expressed as:
$$a_3{c}_3+d_4{s}_3=H$$

可得：
$$i_3=Atan2\left(H,\pm \sqrt{a_3^2+d_4^2-H^2}\right)-Atan2\left(a_3,d_4\right)$$

Due to the existence of positive and negative signs, $i_3$There are two sets of solutions.

---

• ※ 【About how this step came about】

I didn't find the specific content of converting to arctangent, but this derivation process can refer to

→→→ [acosx+bsinx = c; how to calculate x]

---

• Matlab code implementation

%% theta3求解
%theta1的取值已经确定，但是因为theta1有两组可能得解，所以这里实际上m有两组取值，这里只给出一组
m=cos(theta1)*px+sin(theta1)*py-a1; 
n=pz-d1;
H=(m^2+n^2-a2^2-a3^2-d4^2)/(2*a2);
theta3_1=atan2(H,sqrt(a3^2+d4^2-H^2))-atan2(a3,d4);
theta3_2=atan2(H,-sqrt(a3^2+d4^2-H^2))-atan2(a3,d4);

---

○ Solving for $i_2$

Regarding the solution of \theta_2$, I tried several methods.

· Methods of Introduction to Robotics (failed attempts)

First, I tried the method documented in the textbook Introduction to Robotics (John J Craig).

As we all know, PUMA560 is used as a case in "Introduction to Robotics", and the structure of PUMA560 is different from the configuration of industrial robots commonly used today (mainly different from the robots I study), so it cannot be used directly. . Here is the calculation with reference to its steps.

The first step is to organize the calculation formula of the end pose description matrix of the robot as follows

$$[{}_3^{0}T\left(i_2\right){]}^{-1}{}_6^{0}T={}_4^{3}T\left(i_4\right){}_5^{4}T\left(i_5\right){}_6^{5}T\left(i_6\right)$$

Given the range of the voltages (1.4) and (2.4), the quantity
$$\begin{gathered} (p_z-d_1)\sin \left(i_{23}\right)-a_1\cos \left(i_{23}\right)-a_2\cos (i_3)+... \\ {p}_x\cos \left(i_1\right)\cos \left(i_{23}\right)+p_y\sin \left(i_1\right)\cos \left(i_{23}\right)=a_3 \\ (p_z-d_1)\cos \left(i_{23}\right)+a_1\sin \left(i_{23}\right)+a_2\sin \left(i_3\right)-... \\ {p}_y\cos \left(i_1\right)\cos \left(i_{23}\right)+p_x\sin \left(i_1\right)\cos \left(i_{23}\right)=-d_4 \end{gathered}$$

---

• The calculation process is as follows

calculation process

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_left=T_03_inv*T_06;
disp("等式左边矩阵：")
%T_left=simplify(T_left)
T_left
disp("等式右边矩阵：")
simplify(T_36)

The result is as follows

T_left =
 
[nz*cos(Q2)*sin(Q3) + nz*cos(Q3)*sin(Q2) - nx*cos(Q1)*sin(Q2)*sin(Q3) - ny*sin(Q1)*sin(Q2)*sin(Q3) + nx*cos(Q1)*cos(Q2)*cos(Q3) + ny*cos(Q2)*cos(Q3)*sin(Q1),

 oz*cos(Q2)*sin(Q3) + oz*cos(Q3)*sin(Q2) - ox*cos(Q1)*sin(Q2)*sin(Q3) - oy*sin(Q1)*sin(Q2)*sin(Q3) + ox*cos(Q1)*cos(Q2)*cos(Q3) + oy*cos(Q2)*cos(Q3)*sin(Q1), 

az*cos(Q2)*sin(Q3) + az*cos(Q3)*sin(Q2) - ay*sin(Q1)*sin(Q2)*sin(Q3) + ax*cos(Q1)*cos(Q2)*cos(Q3) + ay*cos(Q2)*cos(Q3)*sin(Q1) - ax*cos(Q1)*sin(Q2)*sin(Q3), 

pz*sin(Q2 + Q3) - d1*sin(Q2 + Q3) - a1*cos(Q2 + Q3) - a2*cos(Q3) + (px*cos(Q1 + Q2 + Q3))/2 + (py*sin(Q1 + Q2 + Q3))/2 + (px*cos(Q2 - Q1 + Q3))/2 - (py*sin(Q2 - Q1 + Q3))/2]

[nz*cos(Q2)*cos(Q3) - nz*sin(Q2)*sin(Q3) - ny*cos(Q2)*sin(Q1)*sin(Q3) - ny*cos(Q3)*sin(Q1)*sin(Q2) - nx*cos(Q1)*cos(Q2)*sin(Q3) - nx*cos(Q1)*cos(Q3)*sin(Q2),

 oz*cos(Q2)*cos(Q3) - oz*sin(Q2)*sin(Q3) - oy*cos(Q2)*sin(Q1)*sin(Q3) - oy*cos(Q3)*sin(Q1)*sin(Q2) - ox*cos(Q1)*cos(Q2)*sin(Q3) - ox*cos(Q1)*cos(Q3)*sin(Q2),

 az*cos(Q2)*cos(Q3) - az*sin(Q2)*sin(Q3) - ax*cos(Q1)*cos(Q2)*sin(Q3) - ax*cos(Q1)*cos(Q3)*sin(Q2) - ay*cos(Q2)*sin(Q1)*sin(Q3) - ay*cos(Q3)*sin(Q1)*sin(Q2), 

pz*cos(Q2 + Q3) - d1*cos(Q2 + Q3) + a1*sin(Q2 + Q3) + a2*sin(Q3) + (py*cos(Q1 + Q2 + Q3))/2 - (px*sin(Q1 + Q2 + Q3))/2 - (py*cos(Q2 - Q1 + Q3))/2 - (px*sin(Q2 - Q1 + Q3))/2]

[
nx*sin(Q1) - ny*cos(Q1),
ox*sin(Q1) - oy*cos(Q1),
ax*sin(Q1) - ay*cos(Q1),
px*sin(Q1) - py*cos(Q1)
]

[0,0,0, 1]
 
等式右边矩阵：
[
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]

Then let ChatGPT simplify it for me

The end result is this

(pz - d1)*sin(Q23) - a1*cos(Q23) - a2*cos(Q3) + px*cos(Q1)*cos(Q23) + py*sin(Q1)*cos(Q23)=a3;
(pz - d1)*cos(Q23) + a1*sin(Q23) + a2*sin(Q3) - py*cos(Q1)*cos(Q23) + px*sin(Q1)*cos(Q23)=-d4;

---

At this time, only s 23 s_{23} is unknown in the equation$s_{23}$and $c_{23}$, set it as unknown, (theoretically) you can solve $i_2+i_3$

设
$$\begin{gathered} m_1=c_1{p}_x+s_1{p}_y-a_1 \\ {m}_2=p_z-d_1 \\ {m}_3=a_3+a_2{c}_3 \end{gathered}$$

$$\begin{gathered} n_1=-c_1{p}_y-s_1{p}_x-d_1 \\ {n}_2=a_1 \\ {n}_3=-d_4-a_2{s}_3 \end{gathered}$$

Organize the equation system as
$$\begin{gathered} m_1{c}_{23}+m_2{s}_{23}=m_3 \\ {n}_1{c}_{23}+n_2{s}_{23}=n_3 \end{gathered}$$

Solutions have to

$$\begin{gathered} x=s_{23}=(m_1{n}_3-m_3{n}_1)/(m_1{n}_2-m_2{n}_1) \\ y=c_{23}=-(m_2{n}_3-m_3{n}_2)/m_1{n}_2-m_2{n}_1) \end{gathered}$$

Level 23 = For 2 (y, x) \theta_{23}=\mathrm{For2}(y,x )
$$i_{23}=Atan2(y,x)$$

Then according to $i_3$The value of $i_2$

[! ! ? ? 】It looks fine, but the calculated result is different from the preset value. I don’t know where the problem is. If someone can see it, I hope to give some pointers.

---

• Matlab code implementation

m1=cos(theta1)*px+sin(theta1)*py-a1;
m2=pz-d1;
m3=a3+a2*cos(theta3);

n1=-cos(theta1)*py-sin(theta1)*px+pz-d1;
n2=a1;
n3=-d4-a2*sin(theta1);

x = (m1*n3 - m3*n1)/(m1*n2 - m2*n1);
y = -(m2*n3 - m3*n2)/(m1*n2 - m2*n1);

theta23=atan2(y,x)-pi/2;%这里减pi/2因为在DH参数中theta2补偿了pi/2

---

· Methods in referenced articles (failed attempts)

That is the article mentioned earlier:

→→→ [MD-H model kinematics forward solution, inverse solution and verification of attitude angle calculation]

In this article, the author did not give the specific derivation process, but only gave the matlab code, the code is as follows

%theta2
c3=cosd(Theta3);s3=sind(Theta3);
g1=f2-d6*f1;
g2=f3-d6*az;
g3=a4*c3-d4*s3+a3;
Theta2=(atan2(g3,sqrt(g1^2+g2^2-g3^2))-atan2(g2,g1))*180/pi;
————————————————
版权声明：本文为CSDN博主「Vittore-Li」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/Vittore_Li/article/details/123185721

[! 】Note that the definition of the DH parameters of the author of this article is different from mine. My a1=his a2, my a2=his a3, my a3=his a4, and the connection of his DH model The Z axis of the rod is opposite to my direction, I don't know what effect it will have on the solution.

I looked at it for a long time and didn't understand how this came about, and the result of running it directly was also incorrect (running after making corresponding modifications to my DH model).

If anyone knows what method the author uses, I hope you can enlighten me! (clasping fists)

---

· The method of a master's thesis (the method that can get the correct result)

Yes, it is the thesis of the boss I mentioned earlier (previous article). In fact, my previous process is basically (or all) written according to the steps of the boss, but I added my understanding and verification process.

The boss is different, and the method I use is something I have never seen before. . .

The end position of the robot (the origin of the coordinate system 6) is as follows:

$$\begin{gathered} p_x=a_1{c}_1+a_3{c}_1{c}_{23}+d_4{c}_1{s}_{23}+a_2{c}_1{c}_2 \\ {p}_y=a_1{s}_1+a_3{s}_1{c}_{23}+d_4{s}_1{s}_{23}+a_2{c}_2{s}_1 \\ {p}_z=d_1+a_2{s}_2+a_3{s}_{23}-d_4{c}_{23} \end{gathered}$$

Organized to get:

$$\begin{gathered} \frac{p_x}{c_1}-a_1=a_3{c}_{23}+d_4{s}_{23}+a_2{c}_2 \\ \frac{p_y}{s_1}-a_1=a_3{c}_{23}+d_4{s}_{23}+a_2{c}_2 \\ {p}_z-d_1=a_2{s}_2+a_3{s}_{23}-d_4{c}_{23} \end{gathered}$$

remember

$$k_1=\frac{p_x}{c_1}-a_1{k_2=p}_z-d_1$$

get:

$$\begin{gathered} k_1-a_2{c}_2=a_3{c}_{23}+d_4{s}_{23} \\ {k}_2-a_2{s}_2=a_3{s}_{23}-d_4{c}_{23} \end{gathered}$$

Find the sum of squares:

$$a_3^2+d_4^2=k_1^2+k_2^2+a_2^2-2k_1{a}_2{c}_2-2k_2{a}_2{s}_2$$

remember

$$k_3=\left(k_1^2+k_2^2+a_2^2-a_3^2-d_4^2\right)/{2a}_2$$

but:

$$k_1{c}_2+k_2{s}_2=k_3$$
Solution:
$$i_2=Atan2\left(k_3,\pm \sqrt{k_1^2+k_2^2-k_3^2}\right)-Atan2\left(k_1,k_2\right)$$

---

• Matlab code implementation

k1=px/cos(theta1)-a1;
k2=pz-d1;
k3=(k1^2+k2^2+a2^2-a3^2-d4^2)/(2*a2);

theta2_1=atan2(k3,sqrt(k1^2+k2^2-k3^2))-atan2(k1,k2);
theta2_2=atan2(k3,-sqrt(k1^2+k2^2-k3^2))-atan2(k1,k2);

Remember to subtract $i_2$an offset of pi/2

---

❤ 2023.6.21 ❤

The above is to use the method of algebraic solution to find the angles of the first three axes. According to the general process, the last four axes should be found at this time, but as a summary method article, I am not satisfied with only recording one implementation method of.

After learning the method of the younger brother, I think it should be easier to use the set method to solve the first three axes, at least it is easier to understand than the algebraic method that is the same as the brain teaser.

△ Geometry solution for $i_1$、$i_2$、$i_3$

○ Solving for $i_1$

It can be seen from the schematic diagram of the robot that for this configuration of the robot, the mechanism composed of rod 1, rod 2 and rod 3 is in the same vertical plane, and the end of the robot is the common origin of the coordinate system of rod 4, rod 5 and rod 6. It can be seen from this that the end coordinates of the known robot $\left(p_x,p_y,p_z\right)$ to obtain the rotation angle θ 1 \theta_1of axis 1$i_1$

$$i_1=Atan2\left(p_y,p_x\right)$$

Don't even consider the case of multiple solutions

---

○ Solving for $i_3$

The schematic diagram of the robot structure is as follows

As shown in the figure, point C' coordinates $\left(p_x,p_y,p_z\right)$ ,A ratio$\left(a_1\cos i_1,a_1\sin i_1,d_1\right)$

$$\begin{gathered} L_{B^{\prime }{C}^{\prime }}=L_{BC}=\sqrt{B{D}^2+C{D}^2}=\sqrt{a_3^2+d_4^2} \\ {L}_{AC}=\sqrt{A{D}^2-C{D}^2}=\sqrt{{\left(a_2+a_3\right)}^2-d_4^2} \\ {L}_{A{B}^{\prime }}=L_{AB}=a_2 \end{gathered}$$

Because points O, A, C' are coplanar, so
$$L_{A{C}^{\prime }}=\sqrt{{\left(\sqrt{p_x^2+p_y^2}-a_1\right)}^2+{\left(p_z-d_1\right)}^2}$$

According to the law of cosines:
$$c^2=a^2+b^2-2ab\cos \gamma$$
得，在$\Delta A{\mathrm{B}}^{{}^{\prime }}{\mathrm{C}}^{{}^{\prime }}$中：
$$\cos i_3^{\prime }=\frac{L_{A{B}^{\prime }}^2+L_{B^{\prime }{C}^{\prime }}^2-L_{A{C}^{\prime }}^2}{2L_{A{B}^{\prime }}{L}_{B^{\prime }{C}^{\prime }}}$$

Also obtained from the law of cosines, in $\Delta ABC$中：
$$\cos i_B=\frac{L_{AB}^2+L_{BC}^2-L_{AC}^2}{2L_{AB}{L}_{BC}}$$
According to the geometric relationship:
$$i_3=\arccos i_3^{\prime }-\arccos i_B$$
Note that according to the properties of the law of cosines, the obtained $i_3^{\prime }$The value of is always less than 180°, and $i_3+i_B$There are cases greater than 180°, so $i_3$There are two groups of values.

---

○ Solving for $i_2$

According to $i_2$Given $i_3$Different values, $i_2$The solution has the following four situations:

$$\begin{gathered} i_2=\angle \mathrm{BA}{\mathrm{C}}^{{}^{\prime }}-\angle {\mathrm{B}}^{{}^{\prime }}{\mathrm{AC}}^{{}^{\prime }} \\ {i}_2=\angle BA{C}^{{}^{\prime }}+\angle B^{{}^{\prime }}{AC}^{{}^{\prime }} \\ {i}_2=-\angle \mathrm{BA}{\mathrm{C}}^{{}^{\prime }}-\angle B^{{}^{\prime }}{AC}^{{}^{\prime }} \\ {i}_2=-\angle BA{C}^{{}^{\prime }}+\angle B^{{}^{\prime }}{AC}^{{}^{\prime }} \end{gathered}$$

According to the figure, the coordinates of point B $\left(a_1\cos i_1,a_1\sin i_1,d_1+a_2\right)$ .
Since the points O, A, B, and C' are coplanar, so

$$L_{B{C}^{\prime }}=\sqrt{{\left(\sqrt{p_x^2+p_y^2}-a_1\right)}^2+{\left(p_z-d_1-a_2\right)}^2}$$

任$\angle \mathrm{BA}{\mathrm{C}}^{{}^{\prime }}=\beta$，$\angle {\mathrm{B}}^{{}^{\prime }}{\mathrm{AC}}^{{}^{\prime }}=c$

According to the law of cosines, in $ABC{\_}^{{}^{\text{'}}}$ in:

$$\cos b=\frac{L_{AB}^2+L_{A{C}^{\prime }}^2-L_{B{C}^{\prime }}^2}{2L_{AB}{L}_{A{C}^{\prime }}}$$

在$\Delta {\mathrm{AB}}^{{}^{\prime }}{\mathrm{C}}^{{}^{\prime }}$中：
$$\cos c=\frac{L_{A{B}^{\prime }}^2+L_{A{C}^{\prime }}^2-L_{B^{\prime }{C}^{\prime }}^2}{2L_{A{B}^{\prime }}{L}_{A{C}^{\prime }}}$$

Ratio:
$$i_2=\pm \arccos b\pm \arccos \gamma$$
$i_2$Four sets of solutions can be obtained.

---

○ Summary of geometric methods

After sorting out the method of calculating the angles of the first three axes by the geometric method, I feel that the geometric method is not so simple, but the intuition is affirmative, but the geometric solution does not improve the situation of multiple algebraic solutions, and this multiple solution needs to be based on Judging by geometric relations is not necessarily simpler than algebraic solutions.

Because the calculation process is not complicated, the code of matlab is not given here (mainly I did not type).

If there is any problem in the process or there is a better method, please let me know! (clasping fists)

---

△ Algebraic solution to $i_4$、$i_5$、$i_6$

CSDN also reminded me that there are too many words, and I can’t post one article, so I had to disassemble it and solve it with inverse kinematics $i_4$、$i_5$、$i_6$, see Jacobi related content

→→→ [Industrial Robot Kinematics and Matlab Forward and Inverse Algorithm Study Notes (Summary of a Plenary Session) (3)]