开源七轴myArm协作机械臂正逆运动学技术讲解

引言：
在本文中，我们将深入探讨机器人学的两个核心概念：正运动学和逆运动学。这两个概念是理解和控制机械臂运动的基础。通过一个具体的7轴机械臂实例，我们将详细介绍如何计算机械臂的正运动学和逆运动学。我们首先会解释正运动学和逆运动学的基本概念和数学原理，然后我们将展示如何应用这些原理来计算7轴机械臂的运动。我们的目标是让读者对机械臂的运动控制有一个深入的理解，并了解如何在实践中应用这些知识。

myArm 300
我们先来简要介绍我们待会会使用到的机械臂myArm 300.

myArm 300 Pi是一块全新的7自由度的中心对称构型协作机械臂，myArm搭载着7个高精度的伺服电机，使得最大工作半径300mm，末端最大负载成都能够达到200g，重复定位精度最高达到了±0.5mm。

myArm 300 Pi的控制主板采用了树莓派4B 4G，这是一种广受欢迎的微型计算机，因其强大的性能和灵活的编程环境而备受赞誉。这意味着用户可以使用多种编程语言来控制myArm 300 Pi，包括Python、C++、Java等，极大地提高了使用者的便利性和灵活性,它还支持ROS1/ROS2 在ROS操作系统中提供了许多功能，例如路径规划，避障，三维感知等。

基本概念
我们要想控制机械臂，就得有一套关于机械臂运动控制的算法。正逆运动学是其中的重要组成部分。在介绍正逆运动学之前，我们想先介绍DH模型，这个模型使得正逆运动学的计算变得更加直接和简单。

DH模型
DH参数模型是机器人学中一种常用的描述机器人运动学的方法。它通过一组参数来描述机器人的关节连杆，从而方便地计算机器人末端执行器地位置和姿态。

当使用DH模型用来描述机械臂时，我们将机械臂分解为一系列链接和关节，每个链接和关节都与一个坐标系相关联。我们选择一个基准坐标系，通常是机械臂的基座（底座）坐标系，然后，为每个链接和关节定义一个坐标系，我们定义两个坐标系：前一个链接的坐标系和当前链接的坐标系。这两个坐标系之间的变换由四个参数描述：

连杆长度（a）：它表示相邻链接之间的距离，沿着前一个链接的法线测量。它通常指的是链接的长度或者链接轴线的长度。

连杆的旋转角度（α）：它表示相邻链接之间的旋转角度，围绕前一个链接的法线旋转。它通常指的是链接轴线之间的旋转。

关节的长度（d）：它表示链接的长度或者关节的长度，沿着当前链接的法线测量。它通常指的是关节轴线的长度。

关节的旋转角度（θ）：它表示关节的旋转角度，围绕当前链接的法线旋转。它通常指的是关节的角度或者关节的转动。

通过组合这些参数，可以构建一个四维的变换矩阵，这个变换矩阵可以表示机械臂末端执行器的位置和姿态。

以myArm为例子，下图是myArm 300 的DH模型图以及SDH参数表。当我们有了一个机械臂的DH模型图我们具体能干些什么呢：

正向运动学：通过给定关节角度，可以使用DH模型图计算机械臂末端执行器的位置和姿态，从而进行路径规划和运动控制

逆向运动学：反过来，也可以用DH模型图来解决逆向运动学问题，即给定目位置和姿态，计算出关节角度的变化，从而实现平滑的运动轨迹。

路径规划：给定一个开始位置和一个结束位置，可以使用DH模型来规划机械臂的运动路径。这可能涉及到解决一系列的逆运动学问题。

除此之外还有很多，比如说碰撞检测和臂章，动画和可视化机械臂，仿真机械臂等等。

正运动学
正运动学是用来描述机械臂从基座到末端执行器的位置和方向的计算方法。给定每个关节的角度，我们可以计算出机械臂末端的位置和方向。这是一个基于几何和三角学的过程。

下面举一个简单的例子，在一个平面坐标系当中，已知连杆的感觉长度为l1和l2，并且已知两个关节的转动角度分别为θ1和θ2，那么其末端执行器的位置(x,y)可以根据三角函数求出。

x=l1cos(θ1)+l2cos(θ1+θ2)

y=l1sin(θ1)+l2sin(θ1+θ2)

就可以求出x，y的坐标了。

在实际应用中，我们通常会直接在三维空间中直接进行计算，我们可以使用齐次坐标来进行转换，在齐次坐标中，一个二维点(x, y)可以表示为一个三维点(x, y, 1)，一个三维点(x, y, z)可以表示为一个四维点(x, y, z, 1)。通过这种方式，我们可以使用相同的矩阵乘法操作来描述平移和旋转。

例如，一个二维的平移变换可以表示为以下形式的3x3矩阵：

[[1, 0, dx], [0, 1, dy], [0, 0, 1]]

其中，dx和dy表示在x轴和y轴方向上的平移距离。同样，一个二维的旋转变换可以表示为以下形式的3x3矩阵：

[[cos(theta), -sin(theta), 0], [sin(theta), cos(theta), 0], [0, 0, 1]]

其中，theta表示旋转角度，也就是DH参数中的θ。

通过将所有关节的变换矩阵相乘，我们就可以得到从机器人基座到末端执行器的总变换。这就是正运动学的基本计算过程。这个过程可以用以下的数学形式来表示：

T = A1 * A2 * A3 * ... * An

其中 T 是总变换矩阵，Ai 是第 i 个关节的变换矩阵，n 是关节的数量。注意，矩阵乘法不满足交换律，所以乘法的顺序很重要。最终得到的T总变化矩阵算出来的结果就是机械臂末端相对于基座的坐标。提前了解DH模型，对理解后面的矩阵变换很有帮助。

逆运动学
逆向运动学是指根据机械臂末端执行器的位置和姿态，计算机械臂各个关节的角度。刚好跟正运动学反过来，是求取各个关节的角度。逆运动学的计算通常要比正运动学复杂得多，这是一个从效果反推原因的过程，通常需要解决非线性方程组，而且解可能不唯一，或者根本不存在。我们还是举例说明，以2自由度的平面机械臂来做一个了解。我们已知（x，y）的值，要求出θ1和θ2的角度。

首先我们可以x,y到原点的位置的距离d，这可以勾股定理得到（sqrt代表平方根）

d = sqrt(x^2 + y^2)

然后，我们可以使用余弦定理来求解第二个关节的角度θ2。余弦定理可以描述三个边长已知的三角形中，任何一个角的余弦值。在这里，我们可以将d，L1和L2看作是三个边长，然后求解θ2：

cos(theta2) = (L1^2 + L2^2 - d^2) / (2 * L1 * L2)

因为θ2可能有两个解（顺时针和逆时针），所以我们需要根据实际情况来选择合适的解。

最后，我们可以使用正弦定理或余弦定理来求解第一个关节的角度θ1。正弦定理可以描述三个边长和对应的角的正弦值之间的关系。在这里，我们可以将d，L1和θ1看作是三个已知的值，然后求解θ1：

theta1 = atan2(y, x) - atan2(L2 * sin(theta2), L1 + L2 * cos(theta2))

求解完之后就可以得知各个关节的角度了，这是在二维的空间中的求解方法，在三维空间中的计算方法也是一样的道理，在计算的时候得考虑机械臂关节的限制，以及其他的因素来决定最终的角度。

7轴机械臂实例
接下会用到python对myArm进行编程，用到的是pymycobot库。

正运动学关节控制
正运动学算法通常有一个确定的结果，正运动学的目标是根据给定的关节角度，计算机械臂执行器的位置和姿态，只存在唯一的结果。下面是使用角度控制的代码：

from pymycobot import Myarm import time # create myarm object ma = Myarm('/dev/ttyAMA0',115200) ma.send_angles([degree_list],speed) # send_angles function def send_angles(self, degrees, speed): """Send the degrees of all joints to robot arm. Args: degrees: a list of degree values(List[float]).n for mycobot: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0].n for mypalletizer: [0.0, 0.0, 0.0, 0.0] for mypalletizer 340: [0.0, 0.0, 0.0] for myArm: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0].n speed : (int) 1 ~ 100 """ # self.calibration_parameters(degrees=degrees, speed=speed) degrees = [self._angle2int(degree) for degree in degrees] return self._mesg(ProtocolCode.SEND_ANGLES, degrees, speed)

这就是最直观的正运动学算法的控制，关节角度控制机械臂运动。

逆运动学算法控制，坐标控制
前面也有提及到，你运动学算法相对来说复杂很多，可能存在多个解的情况，也可能存在无解的情况。逆运动学算法的控制是通过控制机械臂末端坐标的变化，让机械臂前往下一个坐标位置。我们用python编写控制坐标运动的代码:

from pymycobot import Myarm import time # create myarm object ma = Myarm('/dev/ttyAMA0',115200) ma.send_coords([coordinates_list],speed,mode) # mode: 0:linear motion,1:nonlinear motion def send_coords(self, coords, speed, mode=None): """Send all coords to robot arm. Args: coords: a list of coords value(List[float]). for mycobot :[x(mm), y, z, rx(angle), ry, rz]n for mypalletizer: [x, y, z, θ] for mypalletizer 340: [x, y, z] speed : (int) 0 ~ 100 mode : (int) 0 - angluar, 1 - linear (mypalletizer 340 does not require this parameter) """ # self.calibration_parameters(coords=coords, speed=speed) coord_list = [] for idx in range(3): coord_list.append(self._coord2int(coords[idx])) for angle in coords[3:]: coord_list.append(self._angle2int(angle)) # print(coord_list) if mode is not None: return self._mesg(ProtocolCode.SEND_COORDS, coord_list, speed, mode) else: return self._mesg(ProtocolCode.SEND_COORDS, coord_list, speed)

7轴机械臂又被称之为冗余机器人，多的一个轴通常用于提供更多的灵活性和运动自由度，所以就出现了这么一个现象，机械臂的末端坐标值保持不变的时候，会有许多不同的姿态。

这就是开头说到的逆运动学的特点，同一个坐标下有许多不同的解。在坐标控制中还有一个关键的信息就是线性运动和非线性运动，主要是指机械臂末端的运动路径的类型。

线性运动：在这种模式下，机器人末端执行器在两个点之间直线移动。这就意味着，无论机器人的关节如何移动，末端执行器都会沿着直线路径从一点移动到另一点。这种类型的运动通常在需要精确位置控制的应用中使用，例如在装配线上的装配任务。
非线性运动：在这种模式下，机器人末端执行器的路径不是直线，而是一条曲线。这种类型的运动可以使机器人在复杂的环境中更灵活地移动，例如在需要避开障碍物或在特定的路径上移动的情况下。
总结
在我们探讨了正逆运动学的基本概念以及数学原理后，我们可以看到这两个概念在机械臂中的控制是特别重要的。然而，正运动学和逆运动学只是机械臂控制的一部分。在实际应用中，我们还需要考虑到动力学、控制理论、传感器反馈、以及实际硬件的限制。

随着技术的发展，机械臂将在很多领域发挥越来越大的作用，包括制造业、医疗保健、家庭服务、搜索和救援等。通过深入理解这些知识，我们可以更加的了解什么是机械臂，让它在各种应用中发挥更大的作用。

如果你喜欢这篇文章的话欢迎在下方留言或者点赞，你的支持是我们更新的动力！

审核编辑 黄宇