模拟 6 轴关节机器人

• 下载 Robot_Simulator-noexe.zip - 2.2 MB
• 下载 Robot_Simulator.zip - 2.4 MB

• 引言
• 机器人类型
• 问题类型（重要，请阅读）
• 正向问题
• 逆向问题
• 模拟器

与机器人打交道很有趣，但并不容易。要进行详尽的解释，我应该写一本书。我尽量解释逻辑，但我必须提供许多链接供您深入研究。有很多计算要做。但我们可以使用计算机代数系统程序。我使用 Maxima： http://maxima.sourceforge.net/。因此，我们可以将精力集中在程序上，轻松进行各种尝试。当您从 Maxima 复制时，它已经是 C/C++ 格式。所以如果您给出正确的变量名，复制粘贴会非常容易。

机器人主要有两大类。并联机器人，也称为闭链机器人，http://en.wikipedia.org/wiki/Parallel_manipulator，它们操作空间小，但能达到非常高的速度和力。开链机器人或串联机器人http://en.wikipedia.org/wiki/Robotic_arm#Types，如笛卡尔机器人、SCARA 机器人，或我们的关节臂机器人。

并联机器人处理起来更重，因为我们无法泛化，必须逐次研究。而开链机器人，我们可以循着一条通用的路径。

这是我们的主角

首先你必须知道什么是旋转矩阵。这些是基本的旋转矩阵，http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations。我们的系统矩阵是通过这些矩阵的适当乘法得到的。

我们需要定义一个通用符号。常用的符号是 Denavit-Hartenberg 符号。http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters

这些符号定义了 Denavit-Hartenberg 参数。（这些是简化的定义，请参考链接以获取更深入的定义）

• z 轴沿关节轴线方向。重要，z 轴是电机旋转轴。z 轴上的平移称为 `dn`。
• x 轴指向下一个电机，如果 z 轴指向其他方向。x 轴上的平移称为 `an`。

实际上，我们选择坐标系，以便描述整个机器人时，我们只沿 z 或 x 轴移动。y 轴无关紧要。

Denavit-Hartenberg 矩阵是一个 4x4 矩阵。矩阵的 3x3 子矩阵是正常的旋转矩阵，第四列是平移矩阵，第四行填充零和一以获得一个方阵。

重要提示：平移列立即给出空间中的位置 (x,y,z)，无需进行任何其他计算。

所以我们开始

• 我们选择一个绝对坐标系。为方便起见，我们选择地面水平，z轴平行于第一个电机（更确切地说，是关节。实际上，由于齿轮或其他技术解决方案，电机通常不与旋转轴对齐。但为了解释，我们称之为电机）。
• 从地面开始，我们沿着 z 轴平移 `h` 毫米（或英寸）。矩阵 `T0`。
• 在 `h` 处有一个驱动旋转关节。我们将角度表示为变量 `t1`。矩阵 `RZ1`。
• 如果我们相乘：M1=T0*Rz1，我们得到变换矩阵，该矩阵给定角度后会返回“附着”在电机轴顶部的坐标系的位置和方向。
• 继续第二个关节，我们沿 z 轴平移 `d1` 毫米，沿 x 轴平移 `a1` 毫米。矩阵 `T1`。
• 我们必须旋转我们的坐标系，使我们的 z 轴平行于关节的旋转点，x 轴指向第三个关节。为此，只需围绕 x 轴旋转 90°（pi/2）度即可。这不是一个变量，而是一个常量，乘法后“消失”。矩阵 `Rpx1`。
• 现在我们可以引入电机角度。与第一个电机完全类似，但这次变量称为 `t2`。矩阵 `RZ2`。
• 矩阵 M2（附着在电机 2 轴的顶部）由第一个矩阵的乘积给出：M2=M1*T1*Rpx1*Rz2。
• ...现在继续直到最后一个手臂

如果你看第四列，它精确地给出了电机顶部在我们绝对坐标系中的位置。（第二个电机对位置还没有任何影响）

这是 Maxima 的一组指令。第一组指令是矩阵的定义（它们可以一次性运行），第二组是它们的乘法（逐臂运行，注释会弄乱语法，或者删除带注释的空行）。

 
 T0:matrix([1,0,0,0],[0,1,0,0],[0,0,1,H],[0,0,0,1]);
Rz1:matrix([cos(t1),-sin(t1),0,0],[sin(t1),cos(t1),0,0],[0,0,1,0],[0,0,0,1]);
T1x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T1:matrix([1,0,0,a1],[0,1,0,0],[0,0,1,d1],[0,0,0,1]);
Rpx1:matrix([1,0,0,0],[0,0,-1,0],[0,1,0,0],[0,0,0,1]);
Rz2:matrix([cos(t2),-sin(t2),0,0],[sin(t2),cos(t2),0,0],[0,0,1,0],[0,0,0,1]);
T2x:matrix([1,0,0, _x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T2:matrix([1,0,0,a2],[0,1,0,0],[0,0,1,0],[0,0,0,1]);
Rz3:matrix([cos(t3),-sin(t3),0,0],[sin(t3),cos(t3),0,0],[0,0,1,0],[0,0,0,1]);
T3x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T3:matrix([1,0,0,a3],[0,1,0,0],[0,0,1,0],[0,0,0,1]);
Rpy3:matrix([0,0,1,0],[0,1,0,0],[-1,0,0,0],[0,0,0,1]);
Rz4:matrix([cos(t4),-sin(t4),0,0],[sin(t4),cos(t4),0,0],[0,0,1,0],[0,0,0,1]);
T4x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T4:matrix([1,0,0,0],[0,1,0,0],[0,0,1,d4],[0,0,0,1]);
Rpy4:matrix([0,0,-1,0],[0,1,0,0],[1,0,0,0],[0,0,0,1]);
Rz5:matrix([cos(t5),-sin(t5),0,0],[sin(t5),cos(t5),0,0],[0,0,1,0],[0,0,0,1]);
T5x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0 ,0,0,1]);
T5:matrix([1,0,0,a5],[0,1,0,0],[0,0,1,0],[0 ,0,0,1]);
Rpy6:matrix([0,0,1,0],[0,1,0,0],[-1,0,0,0],[0,0,0,1]);
R6z:matrix([cos(t6),-sin(t6),0,0],[sin(t6),cos(t6),0,0],[0,0,1,0],[0,0,0,1]);
T6x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);

/*0-1*/
M1:T0.Rz1;/* position of coordinate system first motor*/

/*1-2*/
M1x:M1.T1x;/* Draws 1st  Arm*/
M1_1:M1.T1;/* moves to second joint*/
M1_2:M1_1.Rpx1; /*aligns the coordinate system to the next motor */
M2:M1_2.Rz2;/* coordinate system second motor*/

/*2-3*/
M2x:M2.T2x; /* Draws 2nd Arm*/
M2_1:M2.T2; /* translates to third joint*/
M3:M2_1.Rz3;/* coordinates system 3rd motor*/
M3:matrix([cos(t1)*cos(t2+t3), -cos(t1)*sin(t2+t3),sin(t1), a2*cos(t1)*cos(t2)+a1*cos(t1)],
[sin(t1)*cos(t2+t3),-sin(t1)*sin(t2+t3),-cos(t1),a2*sin(t1)*cos(t2)+a1*sin(t1)],[sin(t2+t3),cos(t2+t3),0,H+a2*sin(t2)+d1],[0,0,0,1]);

/*3-4*/
M3x:M3.T3x;/* Draws 3rd Arm*/
M3_1:M3.T3;/* translates to 4th joint*/
M3_2:M3_1.Rpy3;/*aligns the coordinate system to the next motor */
M4:M3_2.Rz4;/* coordinates system 4th motor*/


/*4-5*/
M4x:M4.T4x;/* draws  4th arm*/
M4_1:M4.T4;/* translates to 5th joint*/
M4_2:M4_1.Rpy4;/*aligns the coordinate system to the next motor*/
M5:M4_2.Rz5;/* coordinates system 5th motor*/

/*5-6*/
M5x:M5.T5x;/* draws  5th arm*/
M5_1:M5.T5;/* translates to 6th joint*/
M5_2:M5_1.Rpy6;/*aligns the coordinate system to the next joint*/
M6:M5_2.R6z;/* coordinate system 6th motor*/
/*end-effector*/
M6x:M6.T6x; 

如果你看，我在计算之后重新定义了矩阵 `M3`，以允许我们引入一些重要的简化，从而减少正弦和余弦的数量（最终会变得非常庞大）。

矩阵 `Tnx` 是带有泛型变量（`_x`、`_y`、`_z`）的矩阵，这些矩阵允许我们在电机的坐标系中绘制附着点。实际上，它允许我们绘制与电机连接的臂。因此，在乘法中：`Mnx:Mn.Tnx`；有注释 /*绘制第 n 个臂*/

 
Ry:matrix([cos(b),0,sin(b)],[0,1,0],[-sin(b),0,cos(b)]);
Rz:matrix([cos(a),-sin(a),0],[sin(a),cos(a),0],[0,0,1]);
Rxx:matrix([1,0,0],[0,cos(c),-sin(c)],[0,sin(c),cos(c)]);
Ryp_1:Ry.Rxx;
Ryp:Rz.Ryp_1; /* this is our Roll-Pitch-Yaw Matrix*/

 
ex:transpose(matrix([1,0,0]));
N:Ryp.ex;/* this is the vector of motor 5, what interests us is the x-component oriented to the top of end effector*/
Pw:(a5+d6)*N;/* now we multiply his value for the effective distance, gives as sum of the distance from motor 5 to motor 6 + distance from motor 6 to end effector*/

 

 
    pWx = x  - cos(a)*cos(b)*(d6 + a5);
    pWy = y - sin(a)*cos(b)*(d6 + a5);
    pWz = z + sin(b)*(d6 + a5);
 

 
    p->t1 = atan2(pWy, pWx);/*the rotation of the first arm, is given by the atan2 of the y and x coordinate of the wrist */
    
    /*we calculate the position of the joint 2, because this is a vertex of the triangle*/
    px = pWx -a1 * cos(p->t1);
    py = pWy - a1 * sin(p->t1);
    pz = pWz - d1 - h;
    
    /*we apply the theorem of cousins to define the anlge of the motor 3*/
    c3 = (px*px + py*py + pz*pz - pow(a3 + d4, 2) - a2*a2) / (2 * (a3 + d4) * a2);
    s3 =- sqrt(1 - c3*c3);
    p->t3 = atan2(s3, c3);

/*with similar consideration we find the angle of the 2nd motor*/    
 // http://www.rob.uni-luebeck.de/Lehre/2008w/Robotik/Vorlesung/Robotik1VL5_1_vers1.pdf  pag. 50
        xi = atan2(pz, sg*sqrt(px *px + py*py));// sg is my addition
        fi = atan2((a3 + d4) * s3, ( a2 + (a3 + d4) * c3));

        p->t2 = xi -  fi;