“手动”操纵器

有一次，在大学的第二年，我被赋予任务：做些“某种”烹饪，以确认“未来机器人技术工程师”的高水平。 出于这种悲哀，我进入了意识的深层去寻找想法。 短暂旅行后，有关该设备的想法浮出水面，我想与公众分享该设备的创建理论。

发展的本质是这样。 考虑常用的5轴机械手（运动结构图如下图所示）。 它在太空中移动时的功能十分广泛：它可以在5个坐标中进行移动，并且同时可以 沿三个XYZ轴3个坐标，并绕其中两个旋转。 在这种情况下，操纵器的工作元件能够占据工作空间中的任何位置，并同时保持工作体所需的方向（不计算工作元件绕其自身轴线的旋转）。



该设计涉及拟人类型的操纵器，因此当然能够大致再现人的手的运动。 对这个论点进行争论时，提出了一个想法，那就是使操纵器实时远程复制我的手的动作会很有趣。 这样我就在手上放了一个传感器来读取手的运动（线性和角度的运动），这种机械性的动作会在我之后重复。 由于机械手可以执行移动和定向工作身体的功能，因此在手的帮助下，人可以移动和定向手掌，因此这两个过程相似并且可以表示为一个常见过程-机械手实时捕获手的运动。


这样就完成了问题的陈述。 现在，让我们分析任务（该语言不会变成“技术任务”）。 有必要开发两个结构上独立的设备：机械手本身和安装在手臂上的运动跟踪设备。 接下来是创建数学描述，编译算法和编写软件的耗时过程。 我们将项目分为三个部分：

1.机械手的发展
2.开发用于跟踪手臂运动的设备
3.其他一切

简要介绍每个阶段：

1. 在这里，一切都是微不足道的。在CAD中，我们设计结构，执行零件图纸并制造，然后组装。 然后，我们计算运动学并编写我们自己的软件，该软件将隐藏对驱动器的低级调用。
2. 在这一阶段，我们确定传感器的类型，设计电路图，制作印刷电路板，开发确定空间位置的算法，编写软件。
3. 实际上，对于先前的阶段，要使机械手服从所有者。 在这里，我开发了一种用于将坐标转换为机械手工作区域的算法。

1.机械手的发展

如上所述，在这个项目中，我们使用5个自由度的操纵器。 这样的自由度就足够了，因为 它们包括三个线性位移和两个旋转位移。 对于工作体的定向，最后两个是必需的，因为空间中PO的方向可以以矢量的形式指定，并且可以肯定地从两个旋转角度（不考虑长度）恢复矢量。 在这种情况下，我们需要五个广义坐标，它们等于轴数。

机械手的连杆通过第五类旋转运动学对连接。 机械手的设计是其类型的标准配置（在本文开头介绍了运动学方案）。 它有一个固定床，一个基本座标系统与之相连；一个旋转架，其余的可动连杆与该旋转架成链状连接。 工件连接到最终链节（法兰），在这种情况下，其模仿为箭头形指针（请参见下图）。




3D模型是在CAD“ Compass 3D”中构建的。 零件材料-4毫米胶合板和PLA用于塑料零件。 胶合板制成的零件在激光机上切出，塑料制成的零件在3D打印机上打印。 Dynamixel AX-12数字伺服器用作轴驱动器。 机械手的外观类似于一只邪恶的昆虫，他因此获得了绰号“ Komar”。

运动学的数学描述

在此阶段，有必要计算机械手的正向和反向运动学。 直接的任务是使用广义坐标的已知值（在这种情况下，是连杆的旋转角度）来确定连杆相对于某个基本坐标系的位置。 与之相反的任务称为“逆运动学”或“逆运动学”，它在于确定广义坐标的参数，以实现机械手工作体的所需位置和方向。

让我们从计算逆运动学开始。 让我们描述操纵器的几何方案，从中可以清楚地了解到我们感兴趣的几何关系。



所需位置由半径矢量表示 $$$\ vec {r}$ 。 值得解释为什么向量 $$$\ vec {r}$ 绘制到法兰点，而不是RO的终点。 由于工作体的方向是我们预先知道的（我本人会根据某些要求确定），因此必须将法兰点放在正确的位置，用矢量表示 $$$\ vec {r}$ 。 该向量是通过从半径向量中减去而获得的 $$$\ vec {R}$ 绘制到PO的终点，其方向向量 $$$\ vec {v_0}$ ，相对于BSK，即：

$$

$$\ vec {r} = \ vec {R}-\ vec {v_0}$$

考虑法兰点到所需位置的过渡。 它是通过铰链A ， B和C中的链节a和b （我以矢量的形式描绘）的旋转来实现的。 基本坐标系（BSC）的原点放置在铰链B的点上。 铰链A的旋转轴沿Z轴指向，轴B和C垂直于Z轴指向。

当所有手续都完成后，让我们说清楚。 为了解决逆运动学问题，由于机械手设计的简单性，我使用了几何方法。 从几何学上可以看出向量 $$$\ vec {r}$ 等于链接向量的总和 $$$\ vec {a}$ 和 $$$\ vec {b}$ 。 角度 $$$\ theta$ ， $$$\ gamma_1$ ， $$$\ gamma_2$ -连杆A ， B和C的旋转角度分别。
考虑一个以向量为界的三角形 $$$\ vec {r}$ ， $$$\ vec {a}$ 和 $$$\ vec {b}$ 。 从这个三角形，通过余弦定理，我们找到角度 $$$\ alpha$ 和 $$$\ beta$ 。 令向量的长度等于：

$$

$$| \ vec {r} | = r \ quad | \ vec {a} | = a \ quad | \ vec {b} | = b$$

我们编写关于所需角度的余弦定理：

$$

$$b ^ 2 = a ^ 2 + r ^ 2-2ar \ cos（\ alpha）\\ r ^ 2 = a ^ 2 + b ^ 2-2ab \ cos（\ beta）$$

表达角度 $$$\ alpha$ 和 $$$\ beta$ ：

$$

$$\ alpha = \ arccos \左（\ frac {a ^ 2 + r ^ 2-b ^ 2} {2ar} \右）\ quad \ beta = \ arccos \左（\ frac {a ^ 2 + b ^ 2 -r ^ 2} {2ab} \ right）$$

从几何图中可以看出：

$$

$$\ theta = \ arctan = \左（\ frac {r_y} {r_x} \右）\ quad \ omega = \ arcsin = \左（\ frac {r_z} {r} \右）$$

然后：

$$

$$\ gamma_1 = \ omega + \ alpha \ quad \ gamma_2 = 180 ^ \ circ- \ beta$$

最后，通过以下公式可以实现从线性坐标到链接的旋转角度的转换：

$$

$$\ theta = arctan \左（\ frac {r_y} {r_x} \右）\\ \ gamma_1 = arcsin \左（\ frac {r_z} {r} \右）+ arccos \左（\ frac {a ^ 2 + r ^ 2-b ^ 2} {2ab} \右）\\ \ gamma_2 = 180 ^ \圆弧\左（\ frac {a ^ 2 + b ^ 2-r ^ 2} {2ar} \右）$$

现在您已经将法兰点移动到所需位置，您需要正确地调整工作体的方向。 为此，您需要知道向量的坐标 $$$\ vec {v}$ 相对于法兰点，即 以局部坐标系（LSC）为基础的坐标- $$${E'}$ 的起点位于操纵器的法兰点。

$$

$$E'= \左[\ vec {x}'\ \ vec {y}'\ \ vec {z}'\右]$$

向量 $$$\ vec {x}'$ 由链接b指示，矢量 $$$\ vec {y}'$ -沿转轴f1的轴。
寻找基础 $$$E'$ 我们从BSK定义转移矩阵 $$$E_0$ ）和LSK。 该矩阵是通过铰链A ， B和C中的旋转组合获得的：

$$

$$E'= CE_0$$

在哪里

$$

$$C = R_CR_BR_A$$

因为 矩阵 $$$E_0$ 是单身，则：

$$

$$E'= C = R_CR_BR_A$$

矩阵 $$$C$ 它是通过将绕轴的旋转矩阵乘以一个角度来计算的 。
我们定义一个计算此矩阵的函数：

$$

$$f = axisAngle2rotMat（轴，角度）$$

该轴以矢量的形式作为通过先前旋转获得的矩阵的一列，并且已经在上面计算了角度。

$$

$$C_A = axisAngle2rotMat（E_0 ^ {\ langle3 \ rangle}，\ theta）\\ C_ {BA} = R_BC_A = axisAngle2rotMat（C_A ^ {\ langle2 \ rangle}，\ gamma_1）C_A \\ C_ {CBA} = R_CC_ { BA} = axisAngle2rotMat（C_ {BA} ^ {\ langle2 \ rangle}，\ gamma_1）C_ {BA}$$

结果，我们得到：

$$

$$C = C_ {CBA} \ quad E'= C$$

让BSK中的工作体向量 $$$\ vec {v_0}$ 。 那么相等是成立的：

$$

$$\ vec {v_0} = E'\ vec {v}$$

从这里我们表达 $$$\ vec {v}$ -工作单位的基础 $$$E'$ ，即 相对于法兰点：

$$

$$\ vec {v} = E'^ T \ vec {v_0}$$

现在，知道了PO向量，就可以计算出定向轴f1和f2的旋转角度


要将PO移动到给定位置，需要在拐角处旋转 $$$\ varphi_1$ 和 $$$\ varphi_2$ 。 该图显示

$$

$$\ varphi_1 = arctan \左（\ frac {v_y} {v_z} \右）\ quad \ varphi_2 = arcsin \左（\ frac {\ sqrt {v_y ^ 2 + v_z ^ 2}} {V} \ right）$$

在哪里 $$$V = | \ vec {v} | = \ sqrt {v_x ^ 2 + v_y ^ 2 + v_z ^ 2}$ -工作体的长度，
$$$v_x，v_y，v_z$ -矢量坐标 $$$\ vec {v}$ 。

机械手的软件分为上下两个级别。 上层是以库和各种命令方法的形式在Matlab中编写的，下层是在Atmega328微控制器上编写的。 高层的任务是制定发送到微控制器的命令。 他的任务是组建团队，并按照它的要求将驱动器安装在正确的位置。 以这种形式，控制系统被证明是可靠的（到目前为止……）并且易于使用。


命令的制定和从上层发送如下：用户在命令窗口中Matlab从库中调用一个函数（例如，简单地移动到一个点的函数），指示必要的参数（通常这些是PO的坐标和方向）。 经过上述数学考虑，记录在库中后，将计算轴的旋转角度以及必要时的旋转速度。 通过简单的协议后，我们将命令汇编并通过串行COM端口将其发送到较低级别的微控制器。

关于书面图书馆的几句话。 它具有以下功能：

1. 串行连接功能
2. 移动功能

• 所有驱动器的速度保持恒定
• 到一个点，所有驱动器最终停止（驱动器的速度与其自身最大的旋转角度成比例地选择。因此，在接近该点时，所有驱动器停止移动）
• 到一定程度，每个驱动器都平稳运动（在这种情况下，速度图看起来像是具有加速，恒定速度和制动区域的梯形）
  
  
  图表

  

  
  
  
  
• 根据手动指定的轴旋转角度和旋转速度

功能列表当然可以扩展，但是此功能足以直接解决机械手所设计的问题。

该视频演示了机械手的操作。

2.开发用于跟踪手臂运动的设备

转到下一步。 在这里，您将需要设计一种特殊的设备，该设备可以实时跟踪手掌中的位置。 另外，该过程称为“运动捕捉”。 为了实施该计划，我决定使用基于三个传感器的陀螺仪惯性导航系统（SINS）：陀螺仪，加速度计和磁力计。 这些传感器中的一堆被称为IMU-一个传感器（换句话说，这就是同一个SINS）。

首先，我设计了具有所需功能的印刷电路板，并在其上放置了这三个传感器。 作为处理控制器，我选择了（或更确切地说选择了）Atmega2560微控制器。 传感器在中国市场上常用且便宜。 这些是成对的传感器三轴陀螺仪，加速度计MPU6050和磁力计HMC5883l。



计算空间位置的算法非常简单：首先，我们以局部坐标系（LSC）为基础来确定方向，该局部坐标系与传感器相连，最后与手掌相连。



确定相对于初始坐标系（NSC）的方向，该坐标系实质上是上电时设备的初始位置。 NSC和LSC的碱基由其正交的列向量组成：

$$

$$E_0 = \左[\ vec {X} \ \ vec {Y} \ \ vec {Z} \ right] = \左[\开始{array} {cccc} 1＆0＆0 \\ 0＆1＆0 \\ 0＆0＆1 \ end {array} \ right] \\ E'= \ left [\ vec {x}'\ \ vec {y}'\ \ vec {z}'\ right]$$

它们与比率相关：

$$

$$E'= CE_0$$

哪里 $$$C$ 是过渡矩阵，也是空间中的旋转矩阵。 我们将NSC的基础视为身份矩阵。 因此

$$

$$E'= C$$

。
之后，加速度矢量 $$$\ vec {A}$ 关于LSC已知的，必须在NSC中确定。 这是通过将加速度列向量乘以LSC来完成的：

$$

$$\ vec {A} = E'\ vec {A}'$$

然后我们将获得的加速度矢量随时间积分两次并获得距离，即 坐标：

$$

$$\ vec {r} = \ iint \ vec {A} dt$$

实现该算法的首次尝试非常幼稚。天真地是，我认为只有通过陀螺仪使用离散积分才能获得绕轴的旋转矩阵。 在这种情况下，可以将轴视为角速度的伪向量，其分量（在轴上的投影）是陀螺仪传感器的输出。 而角度又是通过积分向量模块获得的。 当我看到一个巨大的尺寸（所谓的“零漂移”）时，最初的热情和热情消失了，这是由于离散积分的误差和传感器的固有噪声所引起的累积误差所致。 即使这样，我仍然认为，根据加速度计，它在静止时显示矢量，即重力加速度矢量的倒数（以下称为重力矢量），您可以对矩阵进行校正，但是这里存在与校正可行相关的不确定性如果有可能以其他方式恢复旋转矩阵，即，仅在达到围绕Z轴的角度旋转之前，校正将完成。 不使用陀螺仪数据


然后，有必要使用另一个因素，通过该因素可以对情况进行全面调整。 这就是地球的磁场，它与地球上单个点的重力一起不会随时间变化（当然是在短期内）。 使用上述HMC5883l传感器进行测量。 在这种情况下，我们有两个向量，足以确定空间相对于它们的位置。

现在出现了一个逻辑问题-“该怎么做？” 在尝试回答这个问题失败之后，我自己爬上了互联网，在那儿找到了所需的信息。 根据三种测量（角速度，重力矢量和磁场矢量）确定空间方向的任务也出现在业余飞机（例如，直升机）的设计中，因此已经通过各种方法反复解决了这个问题。 一种流行的方法是所谓的Majwick过滤器（Sebastian OH Madgwick）。 在阅读了原始文章并且不懂英语之后，我转向了精彩的翻译 （感谢作者所做的工作）。 在研究本文的研究过程中，我越来越多地尝试编写自己的位置确定滤波器算法，因为到那时我的知识水平已大大提高。 为了兴趣，“发明一辆自行车”！ 然后我“发明”了它。 下面我给出我的推理。

该算法使用所有三个传感器的读数。 让我提醒您，在给定的加速度计和磁力计对的前提下，算法的任务是计算物体的方向并补偿陀螺仪的零漂移。 使用四元数作为描述位置的数学工具，因为 与矩阵不同，它在构造和优化算法方面很方便，并且需要较少的数学运算来计算它。 空间自转四元数如下：

$$

$$q = \左[q_1 \ q_2 \ q_3 \ q_4 \右]$$

知道归一化向量描述的旋转轴 $$$\ vec {v} = \左[v_x \ v_y \ v_z \右]$ 和角度 $$$\ varphi$ （它们来自上方），您可以计算四元数：

$$

$$q_ {v，\ varphi} = \左[\ cos \左（\ frac {\ varphi} {2} \右）\ \ v_x \ sin \左（\ frac {\ varphi} {2} \右）\ \ v_y \ sin \ left（\ frac {\ varphi} {2} \ right）\ \ v_z \ sin \ left（\ frac {\ varphi} {2} \ right）\ right] \ quad（*）$$

然后，仅使用陀螺仪，在循环的每次迭代中，我们将根据以下表达式计算四元数的当前值：

$$

$$q_n ^ G = q_ {n-1} q_ {v，d \ varphi}$$

在这里 $$$q_n ^ G$ -在给定时间的四元数。 顶部的索引G表示此值是指相对于陀螺仪读数测得的值，即 角速度 $$$q_ {n-1}$ -前一时刻四元数的值； $$$q_ {v，d \ varphi}$ –在一个测量步骤中更改位置，我们可以说位置离散变化了一个角度 $$$d \ varphi$ 为 $$$dt$ 采样周期由旋转四元数（*）表示。

计算的下一步是根据来自一对加速度计和磁力计传感器的数据找到旋转矩阵。 具体来说，我们考虑了地球磁场的重力矢量和感应矢量，如上所述，它们相对于地球和相关的NSC都是静态的。 这里的论文是这样的： 在LSC和NSC的基础上知道重力和感应的值，我们可以计算出从NSC到LSC的过渡矩阵（旋转）和空间旋转四元数 。

表示为 $$$\ vec {G}$ 归一化的重力初始向量。 可以认为如下：

$$

$$\ vec {G} = {\左[0 \ 0 \ 1 \右]} ^ T$$

通过 $$$\ vec {H}$ 表示归一化的初始磁感应矢量：

$$

$$\ vec {H} = {\左[H_x \ H_y \ H_z \右]} ^ T$$

还需要第三个向量。 $$$\ vec {K}$ 与这两个有关。 通过向量乘法获得 $$$\ vec {H}$ 在 $$$\ vec {G}$ ：

$$

$$\ vec {K} = {\左[K_x \ K_y \ K_z \右]} ^ T = \ vec {H} \次\ vec {G} = {\左[H_y \ -H_x \ 0 \右]} ^ T$$

现在形成矩阵 $$$M_0$ 它代表了初始位置，即 NSC，因为在最初时间，LSC与NSC一致。 矩阵是从向量的分量中获得的 $$$\ vec {H}$ ， $$$\ vec {G}$ 和 $$$\ vec {K}$ ：

$$

$$M_0 = \左[\ vec {H} \ \ vec {G} \ \ vec {K} \ right] = \左[\开始{array} {cccc} H_x＆0＆H_y \\ H_y＆0＆-H_x \\ H_z＆1＆0 \ end {数组} \对]$$

同样，但在循环的每次迭代中，都会创建一个相似的矩阵 $$$M'= \左[\ vec {h} \ \ vec {g} \ \ vec {k} \右]$ 在哪里 $$$\ vec {h}$ 是当前归一化的磁感应矢量， $$$\ vec {g}$ 是当前归一化的重力矢量， $$$\ vec {k} = \ vec {h} \ times \ vec {g}$ 。 该矩阵描述了LSC的位置，因为 这些向量在传感器的坐标系中是已知的，这与LSC一致。 然后，在知道两个矩阵的情况下，我们可以编写以下方程式：

$$

$$E'M'= E_0M_0$$

它链接在不同情况下找到的两个矩阵。 给定一个表达式 $$$E'= CE_0$ 以及什么依据 $$$E_0$ 是单位矩阵，我们得到：

$$

$$CM'= M_0$$

从这里我们表示过渡矩阵 $$$C$ ：

$$

$$C = {\左（M_0 ^ {-1} \右）} ^ TM'^ T$$

正如您所了解的那样，第一个因子在算法刚开始时仅计算一次，并且不需要在过程中重新计算。 在循环的每次迭代中，很容易而不是强制地形成第二个因素。

之后，找到转换矩阵后，我们将其转换为四元数 $$$q_n ^ A$ （从矩阵转换为四元数的算法是公共的事情，在此不再赘述）。

结果，我们知道两个相互独立的四元数。 为了合并结果，我应用了一个系数为 $$$F$ ，取值范围为0到1：

$$

$$q_n = Fq_n ^ G + \左（1-F \右）q_n ^ A$$

结果，我们得到一个四元数，该四元数包含有关具有SINS模块的设备空间位置的最新信息。

自行车走了。算法起作用了，但是在某些情况下，它的表现有些不适当，原则上并没有干扰，但没有给出积极思考的理由。 我满足了我的运动兴趣，现在您可以更好地寻求现成的解决方案。 然后，我再次回到Sebastian Majwik的文章，并决定使用他的算法，幸运的是，这个奇妙的人在项目网站上发布了所有源代码。 该档案库的源代码使用多种语言，包括Matlab语言。 这个事实影响了我决定放弃在微控制器上进行所有计算（尤其是因为它是8位）并在Matlab环境中已经在计算机上编写程序的想法。 该微控制器仅用于询问传感器并将数据发送到PC（与机械手类似的情况）。 在这种情况下，要进行更改和调试的主程序代码是基于Matlab环境的，这在处理过程中非常方便。

好吧，我们得到的四元数给出的方向。 根据计划下一步是什么？ 然后根据加速度计跟踪对象的线性坐标。 好吧，这里最好只声明不可能使用加速度计以必要的精度确定坐标。 至少至少找到一个足够的位置变化！ 当然，曾尝试解决此问题，但无法实时找到坐标。 怎么了 因为嘈杂的传感器读数的双重离散积分仅导致以疯狂的速度飞入平流层，但没有取得积极的结果。 因此，我截断了段落开头指示的算法，直到找到方向为止，然后我停在那里。

3.其他一切

因此，我已经完成了设计的最后阶段，已经充分使用了操纵器并包装了IMU传感器。 在执行前两个步骤时，我已经清楚地知道了一切将如何工作以及采用哪种算法。 因此，我足够快地处理了这一阶段。 由于只知道手掌的方向，因此我们直接与工作体一起工作，而这需要相应地确定方向。 如果RO不违反工作区域（旋转所有360无效），则RO可以采取任何方向。

在机械手开发项目中，手动设置PO的矢量，然后找到驱动器的旋转角度。 因此，必须基于手掌的方向来确定此向量。 方向可以表示为基础 $$$E'$ 通过将四元数转换为矩阵来找到它。 该基础由其正交的列向量组成；因此，我们获得了三个向量 $$$\ vec {x}'$ ， $$$\ vec {y}'$ 和 $$$\ vec {z}'$ 。 以向量为方向会很方便 $$$\ vec {x}'$ ，因为我假设了操纵器的初始位置，所以当所有链接和PO都位于基本坐标系的XZ平面中时，PO在那一刻是水平的，即 向量 $$$\ vec {v_0}$ 首先，它与BSK和NSC的X轴对齐（我们将SINS模块的初始位置定位为使NSC与机械手的BSK一致）。 在这种情况下，矩阵的第一列 $$$E'$ 等价向量 $$$\ vec {v_0}$ 。 然后，沿着PO的长度对其进行归一化，并获得相对于操纵器基本坐标系而言所需的向量。 然后我们遵循从p.1开始的广义坐标的计算顺序。

值得注意的是，机械手在更改RO的方向时能够以两种模式移动：第一种-当法兰点固定时​​，第二种-当PO的端点固定时，但观察到其方向。 第二种模式更有趣，因为 即使没有线性运动，在遵守PO方向的同时，所有旋转轴仍将用于移动法兰点。 但是，从第一种模式到第二种模式的过渡只需要加上一个减去向量的操作即可找到向量 $$$\ vec {r}$ （请参见第1段）。

上述所有操作也都在Matlab环境中进行了编程。 一般而言，程序的结构如下：

1. 启动时进行校准。 我们将整个服务器场设置为原始位置，并且不敢触摸，请为SINS模块校准以从中获取一定数量的原始数据。 校准是必要的，因为您需要消除滤波器产生的初始偏移量（我不明白为什么，而且效果很好）。
2. 脚本的开头。 我们初始化所有库和变量，设置法兰的初始位置或PO的终点。
3. 循环开始。 在每次迭代中，我们询问传感器，找到四元数，将其转换为矩阵，这是基础 $$$E'$ ，这是RO所在的基础。 第一列作为PO的方向，我们使用第1节中的数学考虑找到所有广义坐标。
4. 知道最后时刻和现在的广义坐标，我们就可以计算出轴旋转的角速度。
5. 我们将数据包发送到机械手的微控制器。

结果看起来很笨拙，但是如果仔细研究，您会看到这项研究的积极结果。 当然，这项工作不会假装完成，并且需要大量的工作来完善，但前提是可以使用。 但是我还没有找到特定的应用程序。有人可以告诉我吗？