模拟器 6 轴关节机器人

最新推荐文章于 2024-08-12 08:33:59 发布

wouderw

最新推荐文章于 2024-08-12 08:33:59 发布

阅读量249

点赞数

文章标签： 1024程序员节

机器人类型

存在两大机器人家族。Parallel 也称为近距离电影http://en.wikipedia.org/wiki/Parallel_manipulator，操作空间小但可以达到很高的速度和力量。打开电影或连续剧http://en.wikipedia.org/wiki/Robotic_arm#Types，例如笛卡尔、斯卡拉或我们的文章手臂。

机器人运动学中的问题有两个：

直接运动学
逆运动学

考虑到电机的约束，直接的问题是检索末端执行器的位置（最后一个手臂，使用您的工具保持、焊接、油漆等）。

逆问题是检索电机的约束，知道空间中的位置和方向。我想，这是我们工作时的正常情况。“棋子在那儿，我们必须把机器人带到那里”，但也很难解决。从分析上讲，这永远不可能。有一些特殊的配置可以让我们解决它，非常适合小型微控制器，计算能力很小。我们的配置是一个“拟人化手臂”，允许我们进行分析求解。

直接问题

对于直接问题，我们可以只使用矩阵。看起来很难，但理解逻辑后，也不是不可能。它只是一种学习算法。

首先你必须知道什么是旋转矩阵。这些是基本的旋转矩阵，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，是一个正常的旋转矩阵，第4列是平移矩阵，第4行用0和1填充得到方阵。

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

所以我们开始：

我们选择一个绝对坐标系。为方便起见，我们选择地平面，z轴平行于第一个电机（最好是关节。实际上有齿轮，或其他技术方案，电机通常不与旋转轴对齐。但为了解释我们说电机）
从地面出发，我们沿 z 轴平移，单位为h毫米（或英寸）。矩阵T0
那里有h一个从动旋转接头。我们将角度表示为变量t1。矩阵RZ1
如果我们相乘：M1=T0*Rz1，我们得到变换矩阵，给定角度，我们可以返回“连接”到电机轴顶部的坐标系的位置和方向。
向前移动第二个关节，我们d1沿 z 平移 mm， a1沿 x 平移 mm。矩阵T1。
我们必须旋转我们的坐标系，使我们的 z 轴平行于关节的旋转点，并且 x 轴指向第三个关节。为此，绕 x 轴旋转 90°(pi/2) 度就足够了。在乘法“消失”之后，这不是一个变量，而是一个常数。矩阵Rpx1
现在我们可以介绍电机的角度。与第一个电机完全相似，但这次变量称为t2MatrixRZ2
矩阵 M2（附在电机 2 轴的顶部）由第一个矩阵M2=M1*T1*Rpx1*Rz2 相乘得出
...现在继续直到最后一个手臂

如果您查看第 4 列，它会准确地为您提供电机顶部在我们的绝对坐标系中的位置。（第二个电机对位置仍然没有影响）

这是 Maxima 的一组说明。第一组指令是矩阵的定义（它们可以一次运行），第二组指令是它们的乘法（运行 arm for arm，注释会弄乱语法，或者删除带有注释的 void 行）：

C++

收缩▲

<span style="color:#000000"><span style="background-color:#fbedbb"> T0:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,H],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rz1:matrix([cos(t1),-sin(t1),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[sin(t1),cos(t1),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T1x:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,_x],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,_y],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,_z],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T1:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,a1],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,d1],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rpx1:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,-<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rz2:matrix([cos(t2),-sin(t2),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[sin(t2),cos(t2),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T2x:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>, _x],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,_y],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,_z],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T2:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,a2],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rz3:matrix([cos(t3),-sin(t3),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[sin(t3),cos(t3),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T3x:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,_x],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,_y],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,_z],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T3:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,a3],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rpy3:matrix([<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[-<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rz4:matrix([cos(t4),-sin(t4),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[sin(t4),cos(t4),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T4x:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,_x],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,_y],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,_z],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T4:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,d4],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rpy4:matrix([<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,-<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rz5:matrix([cos(t5),-sin(t5),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[sin(t5),cos(t5),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T5x:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,_x],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,_y],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,_z],[<span style="color:#000080">0</span> ,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T5:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,a5],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span> ,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rpy6:matrix([<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[-<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
R6z:matrix([cos(t6),-sin(t6),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[sin(t6),cos(t6),<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
T6x:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,_x],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,_y],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,_z],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);

<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>0-1*/</em></span>
M1:T0.Rz1;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> position of coordinate system first motor*/</em></span>

<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>1-2*/</em></span>
M1x:M1.T1x;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> Draws 1st  Arm*/</em></span>
M1_1:M1.T1;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> moves to second joint*/</em></span>
M1_2:M1_1.Rpx1; <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>aligns the coordinate system to the next motor */</em></span>
M2:M1_2.Rz2;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> coordinate system second motor*/</em></span>

<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>2-3*/</em></span>
M2x:M2.T2x; <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> Draws 2nd Arm*/</em></span>
M2_1:M2.T2; <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> translates to third joint*/</em></span>
M3:M2_1.Rz3;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> coordinates system 3rd motor*/</em></span>
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),<span style="color:#000080">0</span>,H+a2*sin(t2)+d1],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);

<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>3-4*/</em></span>
M3x:M3.T3x;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> Draws 3rd Arm*/</em></span>
M3_1:M3.T3;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> translates to 4th joint*/</em></span>
M3_2:M3_1.Rpy3;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>aligns the coordinate system to the next motor */</em></span>
M4:M3_2.Rz4;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> coordinates system 4th motor*/</em></span>


<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>4-5*/</em></span>
M4x:M4.T4x;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> draws  4th arm*/</em></span>
M4_1:M4.T4;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> translates to 5th joint*/</em></span>
M4_2:M4_1.Rpy4;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>aligns the coordinate system to the next motor*/</em></span>
M5:M4_2.Rz5;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> coordinates system 5th motor*/</em></span>

<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>5-6*/</em></span>
M5x:M5.T5x;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> draws  5th arm*/</em></span>
M5_1:M5.T5;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> translates to 6th joint*/</em></span>
M5_2:M5_1.Rpy6;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>aligns the coordinate system to the next joint*/</em></span>
M6:M5_2.R6z;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> coordinate system 6th motor*/</em></span>
<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>end-effector*/</em></span>
M6x:M6.T6x; </span></span>

如果你看，我M3在他的计算之后重新定义了矩阵，让我们引入一些重要的简化，让我们减少窦和余窦的数量（最终变得巨大）

矩阵Tnx, 是具有通用变量 ( _x, _y, _z) 的矩阵，这些矩阵允许我们绘制附加在电机坐标系中的点。实际上让我们拉动手臂，在电机上联合起来。因此在乘法中：Mnx:Mn.Tnx ; 有评论/*画第n个手臂*/

逆问题

这是最容易的部分。我知道，它看起来很难，但只是一种算法。在您了解它的工作原理后，它会自动出现。

逆问题，你不能用算法解决，但你必须考虑找到一个解析解决方案。永远不可能。有时您必须使用数值分析（例如牛顿）来求解，这取决于您的硬件配置。

你想去哪里？

这是一个6自由度机器人。你必须给他6个参数。前三个很简单，是 ( x, y, z)。其他三个是方向。但是这三个数字是什么意思？

有几种方法可以定义空间中的方向：

滚动俯仰偏航。来自航空科学。http://en.wikipedia.org/wiki/Aircraft_principal_axes
欧拉角 ZYZ：http ://en.wikipedia.org/wiki/Euler_angles 。
四元数http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
矩阵的三个旋转的每个组合未对齐...

最常用于此类应用的是欧拉角（需要引用）。因为遵循手腕的实际配置（最后三个手臂，称为手腕）

我在这个应用程序中使用了 Roll-Pitch-Yaw。有两个原因：首先它是用户友好的。更直接的是，这样的飞机，你有三个数字，你大概了解飞机的资产。其次尝试不同的东西。欧拉角逆运动学网上满了，快速研究一下你会发现可能也是家常便饭。

准备好

这种几何，允许我们把问题分成两部分。前三个手臂，负责手腕的定位：

我们知道手腕必须在哪里。因为是我们想要的目标，我们可以做一些几何考虑：第 4 个电机，不提供平移贡献，它与 arm3 是一个整体，它只能提供旋转贡献。电机 6 也是如此。因此，如果我们将平行于电机 5 的 x 轴的向量乘以我们的 RPY 矩阵，我们就可以检索到手腕的绝对位置。

C++

<span style="color:#111111"><span style="background-color:#ffffff"><span style="color:#000000"><span style="background-color:#fbedbb">Ry:matrix([cos(b),<span style="color:#000080">0</span>,sin(b)],[<span style="color:#000080">0</span>,<span style="color:#000080">1</span>,<span style="color:#000080">0</span>],[-sin(b),<span style="color:#000080">0</span>,cos(b)]);
Rz:matrix([cos(a),-sin(a),<span style="color:#000080">0</span>],[sin(a),cos(a),<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,<span style="color:#000080">0</span>,<span style="color:#000080">1</span>]);
Rxx:matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>],[<span style="color:#000080">0</span>,cos(c),-sin(c)],[<span style="color:#000080">0</span>,sin(c),cos(c)]);
Ryp_1:Ry.Rxx;
Ryp:Rz.Ryp_1; <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> this is our Roll-Pitch-Yaw Matrix*/</em></span>

 
ex:transpose(matrix([<span style="color:#000080">1</span>,<span style="color:#000080">0</span>,<span style="color:#000080">0</span>]));
N:Ryp.ex;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> this is the vector of motor 5, what interests us is the x-component oriented to the top of end effector*/</em></span>
Pw:(a5+d6)*N;<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em> 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*/</em></span></span></span></span></span>

此操作为我们提供了跟随手腕位置 ( pWx, pWy, pWz)

C++

<span style="color:#111111"><span style="background-color:#ffffff"><span style="color:#000000"><span style="background-color:#fbedbb">pWx = x  - cos(a)*cos(b)*(d6 + a5);
pWy = y - sin(a)*cos(b)*(d6 + a5);
pWz = z + sin(b)*(d6 + a5);
</span></span></span></span>

x,y,z,a,b我们想要的目标点在哪里。

由于我们机器人的几何形状，我们可以通过正常的三角考虑立即确定前三个角度：

C++

<span style="color:#111111"><span style="background-color:#ffffff"><span style="color:#000000"><span style="background-color:#fbedbb">    p-<span style="color:#0000ff">></span>t1 = atan2(pWy, pWx);<span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>the rotation of the first arm, is given by the atan2 of the y and x coordinate of the wrist */</em></span>
    
    <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>we calculate the position of the joint 2, because this is a vertex of the triangle*/</em></span>
    px = pWx -a1 * cos(p-<span style="color:#0000ff">></span>t1);
    py = pWy - a1 * sin(p-<span style="color:#0000ff">></span>t1);
    pz = pWz - d1 - h;
    
    <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>we apply the theorem of cousins to define the anlge of the motor 3*/</em></span>
    c3 = (px*px + py*py + pz*pz - pow(a3 + d4, <span style="color:#000080">2</span>) - a2*a2) / (<span style="color:#000080">2</span> * (a3 + d4) * a2);
    s3 =- sqrt(<span style="color:#000080">1</span> - c3*c3);
    p-<span style="color:#0000ff">></span>t3 = atan2(s3, c3);

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

        p-<span style="color:#0000ff">></span>t2 = xi -  fi;</span></span></span></span>

函数 atan2，已经包含在 C++ 中，（我也将它包含在我的代码中，但引用了）它是一个 atan，在整个区间 2*pi 中定义，http ://en.wikipedia.org/wiki/Atan2 ：

最后三个角的解。必须用矩阵来完成：

前三个电机的旋转，也会对最终的方向产生不可避免的影响，因此我们必须找到手腕的实际方向，然后从这个位置设置最后三个电机的角度。关于名称的以下符号的小注释Ri-j，我表示从i到j的旋转矩阵。未知数是R3-6，我们必须找到这个矩阵和一些已知项之间的关系。矩阵R0-6, 是已知的。是我们的目标。我们想要的点。但是通过我们的Roll-Pitch-Yawn矩阵以矩阵形式表示。而且R0-3，众所周知，我们已经计算了这个角度。

将左两个成员与R0-3的逆相乘（旋转矩阵是正交矩阵，因此 inv(m)=transpose(M)）并重新排序我们有： R3-6=transpose(R0-3)* R0-6=transpose (R0-3)* Rrpy

对于马克西姆

C++

<span style="color:#111111"><span style="background-color:#ffffff"><span style="color:#000000"><span style="background-color:#fbedbb">R03:minor(M3,<span style="color:#000080">4</span>,<span style="color:#000080">4</span>);
R03t:transpose(R03);
R36:R03t.Ryp; <span style="color:#008000"><em>/*</em></span><span style="color:#008000"><em>where Ryp is our Roll Pitch Yawn matrix. known because is our target
</em></span></span></span></span></span>

在此矩阵的每个末端给定一个名称：

我们可以使用以下等式直接检索电机角度：

C++

<span style="color:#111111"><span style="background-color:#ffffff"><span style="color:#000000"><span style="background-color:#fbedbb">p-<span style="color:#0000ff">></span>t5 = atan2(sqrt(nz *nz + ny *ny), (nx));
p-<span style="color:#0000ff">></span>t4 = atan2((-nz), (-ny));
p-<span style="color:#0000ff">></span>t6 = atan2(-sx, -ax);
</span></span></span></span>

重要这是一个非常简单的解决方案，在现实中，它需要对标志进行非常仔细的研究。找到每个奇点和不连续点（函数 atan，不连续），这会导致位置非常快速的变化，并且电机的加速度非常大。我几乎找到了每个奇异点，如果有人发现不对劲，请与我交流。如果解决它，他会让我很开心。