written 22 months ago by |
Solution:
In order to solve the Inverse Kinematics of a robot we need to first solve the Direct Kinematics of a three-axis robot.
Using steps 0 to 7 of the D-H algorithm we obtain the following link-coordinate diagram for a three-axis articulated robot:
Then using steps 8 to 12 of the D-H algorithm we obtain the following Kinematic Parameters as shown below:
$ \begin{array}{lllllc} \hline \text { Axis } & \theta & d & a & \alpha & \text { Home } \\ \hline 1 & q_1 & 0 & a_1 & 0 & \pi / 3 \\ 2 & q_2 & 0 & a_2 & 0 & -\pi / 3 \\ 3 & q_3 & d_3 & 0 & 0 & 0 \\ \hline \end{array}\\ $
To solve the inverse kinematics, we use the arm matrix equation.
$ \begin{aligned}\\ T_{\text {base }}^{\text {too b }} &=T_0^1 T_1^2 T_2^3 \\ &=\left[\begin{array}{cccc} \mathrm{C}_1 & -\mathrm{S}_1 & 0 & a_1 \mathrm{C}_1 \\ \mathrm{~S}_1 & \mathrm{C}_1 & 0 & a_1 \mathrm{~S}_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} \mathrm{C}_2 & -\mathrm{S}_2 & 0 & a_2 \mathrm{C}_2 \\ \mathrm{~S}_2 & \mathrm{C}_2 & 0 & a_2 \mathrm{~S}_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} \mathrm{C}_3 & -\mathrm{S}_3 & 0 & 0 \\ \mathrm{~S}_3 & \mathrm{C}_3 & 0 & 0 \\ 0 & 0 & 1 & d_3 \\ 0 & 0 & 0 & 1 \end{array}\right] \\ &=\left[\begin{array}{cccc} \mathrm{C}_{12} & -\mathrm{S}_{12} & 0 & a_1 \mathrm{C}_1+a_2 \mathrm{C}_{12} \\ \mathrm{~S}_{12} & \mathrm{C}_{12} & 0 & a_1 \mathrm{~S}_1+a_2 \mathrm{~S}_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} \mathrm{C}_3 & -\mathrm{S}_3 & 0 & 0 \\ \mathrm{~S}_3 & \mathrm{C}_3 & 0 & 0 \\ 0 & 0 & 1 & d_3 \\ 0 & 0 & 0 & 1 \end{array}\right] \\ &=\left[\begin{array}{cccc} \mathrm{C}_{123} & -\mathrm{S}_{123} & 0 & a_1 \mathrm{C}_1+a_2 \mathrm{C}_{12} \\ \mathrm{~S}_{123} & \mathrm{C}_{123} & 0 & a_1 \mathrm{~S}_1+a_2 \mathrm{~S}_{12} \\ 0 & 0 & 1 & d_3 \\ 0 & 0 & 0 & 1 \end{array}\right] \end{aligned}\\ $
$ W(q)=\left[\begin{array}{c} a_1 C_1+a_2 C_{12} \\ a_1 S_1+a_2 S_{12} \\ 0 \\ 0 \\ \exp \left(q_3 / \pi\right) \end{array}\right]\\ $
$ \begin{aligned} w_1^2+w_2^2 &=\left(a_1 \mathrm{C}_1+a_2 \mathrm{C}_{12}\right)^2+\left(a_1 \mathrm{~S}_1+a_2 \mathrm{~S}_{12}\right)^2 \\\\ &=a_1^2 \mathrm{C}_1^2+2 a_1 a_2 \mathrm{C}_1 \mathrm{C}_{12}+a_2^2 \mathrm{C}_{12}^2+a_1^2 \mathrm{~S}_1^2+2 a_1 a_2 \mathrm{~S}_1 \mathrm{~S}_{12}+a_2^2 \mathrm{~S}_{12}^2 \\\\ &=a_1^2+2 a_1 a_2\left(\mathrm{C}_{12} \mathrm{C}_1+\mathrm{S}_{12} \mathrm{~S}_1\right)+a_2^2 \\\\ &=a_1^2+2 a_1 a_2 \mathrm{C}_2+a_2^2 \end{aligned}\\ $
$ q_2=\pm \arccos \frac{w_1^2+w_2^2-a_1^2-a_2^2}{2 a_1 c_2}\\ $
$ \begin{aligned}\\ &\left(a_1+a_2 C_2\right) C_1-\left(a_2 S_2\right) S_1=w_1 \\ &\left(a_2 S_2\right) C_1+\left(a_1+a_2 C_2\right) S_1=w_2 \end{aligned}\\ $
$ \begin{aligned} &\mathrm{C}_1=\frac{\left(a_1+a_2 \mathrm{C}_2\right) w_1+a_2 \mathrm{~S}_2 w_2}{\left(a_1+a_2 \mathrm{C}_2\right)^2+\left(a_2 \mathrm{~S}_2\right)^2} \\\\ &\mathrm{~S}_1=\frac{\left(a_1+a_2 \mathrm{C}_2\right) w_2-a_2 \mathrm{~S}_2 w_1}{\left(a_1+a_2 \mathrm{C}_2\right)^2+\left(a_2 \mathrm{~S}_2\right)^2} \\ \end{aligned}\\ $
$ q_1=\operatorname{atan} 2\left[\left(a_1+a_2 C_2\right) w_2-a_2 S_2 w_1,\left(a_1+a_2 C_2\right) w_1+a_2 S_2 w_2\right]\\ $
$ q_3=\pi \ln w_6\\ $
Complete Solution:
The following is the entire algorithm for the 3-axis planar articulated robot: