[Matlab six-degree-of-freedom robot] kinematics positive solution (with complete code of MATLAB robot positive solution)

Past review

【Summary】

related resources:

$\boxed{[Matlabsixdegrees\; of\; freedomrobot]series\; ofarticles}$

【Main line】

1. Define standard and improved DH parameters, and establish a robot model.
2. Correct solution of kinematics
3. Construction of Robot Workspace Based on Monte Carlo Method

【Supplementary explanation】

1. Understanding of Flexible Workspaces and Accessible Workspaces
2. Detailed establishment steps of modified DH parameters (modified Denavit-Hartenberg)
3. Questions about parameterization of rotation (Euler angles, attitude angles, quaternions)
4. The Explanation of Double Variable Function atan2(x,y)
5. Some Problems Concerning Inverse Solution of Robot Kinematics

foreword

This article introduces the problems related to the positive solution of robot kinematics, how to understand the forward kinematics, and use the DH parameters to solve the positive solution of robot kinematics.

---

The following is the text of this article, including the understanding of the meaning of the positive solution and the step-by-step analysis of the code.

text

1. Correct solution of kinematics

Definition: Knowing the motion parameters of each joint, find the pose of the end effector relative to the reference coordinate system.

Solving steps:

• The connecting rods are connected end to end;
• Determine the homogeneous transformation matrix between the connecting rods;
• Get the final total transformation matrix.

And the unknowns in the total transformation matrix are only theRotation angle, so the end Cartesian space coordinates of the six-DOF robot can be obtained by obtaining the rotation angle.

1. Homogeneous transformation matrix

In the previous article Matlab established a six-degree-of-freedom robot model, it explained in detail how to define the DH parameters of the robot and how to set $z_i$Axis, etc., only part of the content is described here.
Sign convention for DH parameters

DH Convention Parameters	notation convention
$i$	joint angle 关节转角
$d$	link offset 连杆偏移
$a$	link length 连杆长度
$a\left(alpha\right)$	link twist 连杆扭角
Rules of procedure for formulating DH parameters

• parameter $a$ is axis$z_0$和轴$z_1$Between along the axis $x_1$the measured distance;
• Angle $\alpha$ is perpendicular to$x_1$The axis z measured in the plane $z_0$sum $z_1$angle between. Angle $The\; positive\; value\; of\; \alpha$ is defined as from$z_0$to $z_1$, determined by the right-hand rule;
• parameter $d$ is from the origin$o_0$to axis $x_1$give $z_0$the distance between intersection points along $z_0$The axis is measured;
• $\theta$ is perpendicular to$z_0$Measured from $x_0$to $x_1$Angle.

After understanding the above symbol conventions and program rules, on this basis, each homogeneous transformation matrix $T_i$can be expressed as a product of fundamental matrices,

• 对于标准型D-H参数
  Thatproduct order如下：
  ${}^{i-1}{T}_i=Rot(z_{,}{i}_i)\times Trans(z，d_i)\times Trans(x_{，}{a}_i)\times Rot(x，{\alpha }_i)$
  Universal homogeneous transformation matrixAs follows:
  ${}^{i-1}{T}_i=$$\left[\begin{array}{cccc}cos{\theta }_i& -sin{\theta }_{i}cos{\alpha }_i& sin{\theta }_{i}In\alpha \_{}_i& a_{i}cos{\theta }_i\\ sin{\theta }_i& cos{\theta }_{i}cos{\alpha }_i& -cos{\theta }_{i}In\alpha \_{}_i& a_{i}sin{\theta }_i\\ 0& In\alpha \_{}_i& cos{\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right]$
• 对于改进型D-H参数
  Thatproduct order如下：
  ${}^{i-1}{T}_i=Rot(x_i,a_{i-1})\times Trans(x_i，a_{i-1})\times Rot(z_i,i_i)\times Trans(z_i，d_i)$
  Universal homogeneous transformation matrixAs follows:
  ${}^{i-1}{T}_i=$$\left[\begin{array}{cccc}cos{\theta }_i& -sin{\theta }_i& 0& a_i\\ sin{\theta }_{i}cos{\alpha }_i& cos{\theta }_{i}cos{\alpha }_i& -sin{\alpha }_i& -sin{\alpha }_i{d}_i\\ sin{\theta }_{i}In\alpha \_{}_i& cos{\theta }_{i}In\alpha \_{}_i& cos{\alpha }_i& cos{\alpha }_i{d}_i\\ 0& 0& 0& 1\end{array}\right]$

2. Total transformation

OKDH parameter establishment methodAfter constructing the DH parameters of each joint, they are substituted into their respective general homogeneous transformation matrices to obtain ${}^0{T}_1$、${}^1{T}_2$、${}^{2T}{\_}_3$、${}^{3T}{\_}_4$、${}^4{T}_5$、${}^5{T}_6$There are six matrices in total. Here the author chooses theImproved DH parameters, so the respective matrices are as follows:
${}^0{T}_1=$$\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& a_1\\ cos{\alpha }_{1}sin{\theta }_1& cos{\alpha }_{1}cos{\theta }_1& -sin{\alpha }_1& -d_{1}In\alpha \_{}_1\\ In\alpha \_{}_{1}sin{\theta }_1& In\alpha \_{}_{1}cos{\theta }_1& cos{\alpha }_1& d_{1}cos{\alpha }_1\\ 0& 0& 0& 1\end{array}\right]$
${}^1{T}_2=$$\left[\begin{array}{cccc}cos{\theta }_2& -sin{\theta }_2& 0& a_2\\ cos{\alpha }_{2}sin{\theta }_2& cos{\alpha }_{2}cos{\theta }_2& -sin{\alpha }_2& -d_{2}In\alpha \_{}_2\\ In\alpha \_{}_{2}sin{\theta }_2& In\alpha \_{}_{2}cos{\theta }_2& cos{\alpha }_2& d_{2}cos{\alpha }_2\\ 0& 0& 0& 1\end{array}\right]$
${}^{2T}{\_}_3=$$\left[\begin{array}{cccc}cos{\theta }_3& -sin{\theta }_3& 0& a_3\\ cos{\alpha }_{3}sin{\theta }_3& cos{\alpha }_{3}cos{\theta }_3& -sin{\alpha }_3& -d_{3}In\alpha \_{}_3\\ In\alpha \_{}_{3}sin{\theta }_3& In\alpha \_{}_{3}cos{\theta }_3& cos{\alpha }_3& d_{3}cos{\alpha }_3\\ 0& 0& 0& 1\end{array}\right]$
${}^{3T}{\_}_4=$$\left[\begin{array}{cccc}cos{\theta }_4& -sin{\theta }_4& 0& a_4\\ cos{\alpha }_{4}sin{\theta }_4& cos{\alpha }_{4}cos{\theta }_4& -sin{\alpha }_4& -d_{4}In\alpha \_{}_4\\ In\alpha \_{}_{4}sin{\theta }_4& In\alpha \_{}_{4}cos{\theta }_4& cos{\alpha }_4& d_{4}cos{\alpha }_4\\ 0& 0& 0& 1\end{array}\right]$
${}^4{T}_5=$$\left[\begin{array}{cccc}cos{\theta }_5& -sin{\theta }_5& 0& a_5\\ cos{\alpha }_{5}sin{\theta }_5& cos{\alpha }_{5}cos{\theta }_5& -sin{\alpha }_5& -d_{5}In\alpha \_{}_5\\ In\alpha \_{}_{5}sin{\theta }_5& In\alpha \_{}_{5}cos{\theta }_5& cos{\alpha }_5& d_{5}cos{\alpha }_5\\ 0& 0& 0& 1\end{array}\right]$
${}^5{T}_6=$$\left[\begin{array}{cccc}cos{\theta }_6& -sin{\theta }_6& 0& a_6\\ cos{\alpha }_{6}sin{\theta }_6& cos{\alpha }_{6}cos{\theta }_6& -sin{\alpha }_6& -d_{6}In\alpha \_{}_6\\ In\alpha \_{}_{6}sin{\theta }_6& In\alpha \_{}_{6}cos{\theta }_6& cos{\alpha }_6& d_{6}cos{\alpha }_6\\ 0& 0& 0& 1\end{array}\right]$

Multiply the six homogeneous transformation matrices in order to get the six-degree-of-freedom robotTotal transform:
${}^0{T}_6=$${}^0{T}_1\times$${}^1{T}_2\times$${}^{2T}{\_}_3\times$${}^{3T}{\_}_4\times$${}^4{T}_5\times$${}^5{T}_6=$$\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]$

2. Code implementation

1. Define the parameters of each connecting rod

The definition of symbolic variables for connecting rod parameters istwo ways,code show as below:

the first wayis to define all parameters as unknowns

syms   theta1   d1    a1     alpha1;
syms   theta2   d2    a2     alpha2;
syms   theta3   d3    a3     alpha3;
syms   theta4   d4    a4     alpha4;
syms   theta5   d5    a5     alpha5;
syms   theta6   d6    a6     alpha6;

But because setting too many unknowns will lead to the result of the total transformation is too lengthy, so set herethe second wayTo avoid complicated results, please refer to Matlab in the previous article to establish a six-degree-of-freedom robot model for the following parameters

%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
%由于我们需要分析各轴θi所对应的机器人末端位置
%因此theta1 theta2 theta3 theta4 theta5 theta6仍设为未知量
syms theta1 theta2 theta3 theta4 theta5 theta6

With the above parameter settings, the next parameterhomogeneous transformation matrixsetting.

2. Homogeneous transformation matrix and total transformation

Firstly, each parameter is summarized into a unified matrix, which is convenient for the subsequent parameter analysis.quote

% 参数矩阵取名为MDH
MDH = [theta1          d1       a1       alpha1;
       theta2+pi/2     d2       a2       alpha2;    
       theta3          d3       a3       alpha3;
       theta4          d4       a4       alpha4;
       theta5          d5       a5       alpha5;
       theta6          d6       a6       alpha6];

Notice! ! !Since the author's 6DOF robot has aJoint Variable Offset, the offset is the key that causes theta2the need to add in MDH pi/2.

Next, refer to the value of the parameter matrix in the homogeneous transformation matrix, and define the homogeneous transformation matrix of each joint. The code is as follows:

T01=[cos(MDH(1,1))               -sin(MDH(1,1))                 0              MDH(1,3);
     sin(MDH(1,1))*cos(MDH(1,4))  cos(MDH(1,1))*cos(MDH(1,4))  -sin(MDH(1,4)) -sin(MDH(1,4))*MDH(1,2);
     sin(MDH(1,1))*sin(MDH(1,4))  cos(MDH(1,1))*sin(MDH(1,4))   cos(MDH(1,4))  cos(MDH(1,4))*MDH(1,2);
      0                           0                             0              1];
T12=[cos(MDH(2,1))               -sin(MDH(2,1))                 0              MDH(2,3);
     sin(MDH(2,1))*cos(MDH(2,4))  cos(MDH(2,1))*cos(MDH(2,4))  -sin(MDH(2,4)) -sin(MDH(2,4))*MDH(2,2);
     sin(MDH(2,1))*sin(MDH(2,4))  cos(MDH(2,1))*sin(MDH(2,4))   cos(MDH(2,4))  cos(MDH(2,4))*MDH(2,2);
      0                           0                             0              1];
T23=[cos(MDH(3,1))               -sin(MDH(3,1))                 0              MDH(3,3);
     sin(MDH(3,1))*cos(MDH(3,4))  cos(MDH(3,1))*cos(MDH(3,4))  -sin(MDH(3,4)) -sin(MDH(3,4))*MDH(3,2);
     sin(MDH(3,1))*sin(MDH(3,4))  cos(MDH(3,1))*sin(MDH(3,4))   cos(MDH(3,4))  cos(MDH(3,4))*MDH(3,2);
      0                           0                             0              1];
T34=[cos(MDH(4,1))               -sin(MDH(4,1))                 0              MDH(4,3);
     sin(MDH(4,1))*cos(MDH(4,4))  cos(MDH(4,1))*cos(MDH(4,4))  -sin(MDH(4,4)) -sin(MDH(4,4))*MDH(4,2);
     sin(MDH(4,1))*sin(MDH(4,4))  cos(MDH(4,1))*sin(MDH(4,4))   cos(MDH(4,4))  cos(MDH(4,4))*MDH(4,2);
      0                           0                             0              1];
T45=[cos(MDH(5,1))               -sin(MDH(5,1))                 0              MDH(5,3);
     sin(MDH(5,1))*cos(MDH(5,4))  cos(MDH(5,1))*cos(MDH(5,4))  -sin(MDH(5,4)) -sin(MDH(5,4))*MDH(5,2);
     sin(MDH(5,1))*sin(MDH(5,4))  cos(MDH(5,1))*sin(MDH(5,4))   cos(MDH(5,4))  cos(MDH(5,4))*MDH(5,2);
      0                           0                             0              1];
T56=[cos(MDH(6,1))               -sin(MDH(6,1))                 0              MDH(6,3);
     sin(MDH(6,1))*cos(MDH(6,4))  cos(MDH(6,1))*cos(MDH(6,4))  -sin(MDH(6,4)) -sin(MDH(6,4))*MDH(6,2);
     sin(MDH(6,1))*sin(MDH(6,4))  cos(MDH(6,1))*sin(MDH(6,4))   cos(MDH(6,4))  cos(MDH(6,4))*MDH(6,2);
      0                           0                             0              1];

Through the above, we can see that the total transformation code is as follows:

T06 = T01*T12*T23*T34*T45*T56;

3. Code running results

In the code example, we will refer to $i_i$Proceed as followsassignment

theta1 = pi/3;
theta2 = pi/4;
theta3 = pi/5;
theta4 = pi/3;
theta5 = pi/4;
theta6 = pi/5;

After running the program, as shown in the figure below After the simulation

of the previous article , the result is shown in the figure belowrobot.teach()

---

Summarize

The above is the content of forward kinematics. This article introduces in detail how to understand forward kinematics and the implementation of code. The robot toolbox provides functions and methods on how to deal with the length of the connecting rod and the twist of the connecting rod.

references

1. Matlab Robot Toolbox (1) - Establishment, Drawing and Forward and Inverse Kinematics of Robots
2. Establishing the Kinematics Positive Solution of Robot by DH Parameter Method
3. Six-axis robot matlab write kinematics positive solution function (DH model)
   
4. [Robotics] Planar 2R Robot (1) - Positive Kinematics