机器人控制系统运动学方程

控制机器人系统的运动通常要求了解终端受动物体的方位（通常指工具提示或者受控参照系或者受控坐标系统）与物理控制的用于操作终端受动物体的执行器或者电机方位（通常指受控参照系或者受控坐标系统）之间的关系。这些知识可以大体表征机器人系统的运动学结构，通常用运动学方程表示。一些高性能运动控制器具有整理这些方程并得到相对运动轨迹的能力，确保机器人系统的实时位置控制。本文中，将介绍2种特定机器人系统的运动学方程。

为什么使用运动学？3自由度起重机

3自由度（DOF）起重机是一个相对简单的机器人系统，用户可以控制起重距离、起重机回转角度和起重机的倾角（见图1）。通常一台起重机还有第四个自由度——起重物体的悬挂高度，这里不考虑此自由度的作用，并不会影响分析结果的普适性。

图1 一台如图所示的三自由度起重机，允许用户控制起重距离、回转角和倾斜角，但是不允许起重机的工作端作直线运动。资料来源：www.rki-us.com

通常，起重机的起重臂旋转路径就像一个圆弧，而且，如果起重臂倾斜角增加，终端受动物体（起重机末端的起重物）就会沿着圆弧路径向上。这些自然形成的轨迹对于某些环境是可以接受的，然而，一旦用户希望起重物体的运动路径是由很多线段组成的话，该怎么办？或者是在任意三个方向上的随意形状，又该如何？例如起重物体是某些测量设备或者图像采集系统。简单的例子是将起重物体沿着方形轨迹移动，这对于一些系统是很有用的。

那些学习过坐标几何学的人应该对起重机的受控坐标系统很熟悉——通常就是球坐标系。空间中的一个点可以用球坐标系中的三个参数来表征：距离原点的距离、x-y平面上距离x轴的方位角θ（0 – 2π区间）和与z周的夹角θ（0 – π区间）。球坐标系的图例如图2所示。

图2 那些学习过坐标几何学的人应该对起重机的受控坐标系统很熟悉，通常就是球坐标系。

沿着线段作移动，我们希望在标准3D笛卡尔坐标系中工作。笛卡尔坐标系中的一个点由(x, y, z)表征，从直观上说，这个坐标系更便于进行线段位移控制。例如，方形的运动轨迹由4条线段组成，线段运动就是笛卡尔坐标系中最基本的运动模式。问题转化为：我们如何在这两种坐标系统进行转换。答案是运动方程。运动方程可以将笛卡尔坐标系(x, y, z)与起重机球坐标系(r, θ, Φ)联系在一起。

在进一步探讨之前，让我们快速地判断一下，为什么这些方程是必要的。如果用户想要在笛卡尔坐标系下控制运动路径，他/她就需要确定一条由一系列(x, y, z) 坐标位置组成的轨迹。当使用运动控制器时，对于很多种类的运动，明确地指明运动轨迹是没有必要的。运动控制通常产生一个运动轮廓（一系列(x, y, z)坐标位置）用于控制运动，例如点到点运动就意味着笛卡尔坐标系下的直线运动。如果我们知道受动物体的目标(x, y, z)位置，然后就可以反推运动方程，运动控制器就可以计算出如何控制实际的起重机（包括起重臂长度、倾斜角和回转角——(r, θ, Φ)）

前向运动方程更多地用于初次校准。他们可以用于测量反馈位置，并将 (r, θ, Φ) 结果转换为用户更加关心的(x, y, z)坐标。这个过程也可以用于确定安装位置，和用于将任意位置的起重机坐标初始化为相对的(x, y, z)坐标。

由此可见运动方程的必要性，现在就该讨论如何解运动方程了。先从反推运动学方程开始，我们希望得到起重机的(r, θ, Φ)坐标：



实际上依靠对球坐标系/笛卡尔坐标系的观察就可以解这个等式，使用一些三角公式，可以得到如下等式：



观察上面第三个等式，Φ是由关于r的等式表述的，而r又可由第一个等式中的(x, y, z)解出。前向运动方程的形式类似：



通过观察，这几个方程同样可以轻松解出：



更复杂的例子——6自由度Stewart六脚平台

Stewart六脚平台在很多场合都有应用，包括自动检测、机器人手术、人造卫星和望远镜定位以及机械仿真等等。六脚包括6个独立的受控执行器（长度），在一端汇聚到一个固定的基座，另一端与一平面平台连接，允许6个自由度，(α (roll), (pitch), γ (yaw), x, y, z)。几何学实例如图3所示。



图3 Stewart六轴平台在很多场合都有应用，包括自动检测、机器人手术、人造卫星和望远镜定位以及机械仿真等等。来源: ACS Motion Control

对于此系统，反推运动学方程可以告诉我们：对于给定的(α, , γ, x, y, z)，可以知道执行器的长度(l1, l2, l3, l4, l5, l6)是多少，还可以知道姿态（P）。前向方程用于计算姿态P，用执行器的脚长度I表示。前向运动方程是封闭的方程组，传统计算方法是不可解的。但是，可以通过使用牛顿迭代法来解此前向方程，下文将作讨论。

为了解此系统的反向运动方程，必须确定平台种类以及执行器汇聚点的位置，因为脚长就是点与点之间的距离。平台执行器所处位置用基点坐标系表示如下：



上述等式的下标表明了向量的参考坐标系。这里，点的位置实际上是齐次坐标，以(x, y, z, w) 或者 (x/w, y/w, z/w)的形式表述，为了简化讨论，这里的w我们可以令其等于1。R是变换矩阵，可以将平台点（Ppi）也就是平台系数转换成(Bpi)，也就是基座系数。R是3×4的矩阵，包括3×3的旋转矩阵和3×1的平移矩阵。



在R等式中，“s”代表正弦函数；“c”代表余弦函数。上述等式中的旋转矩阵是单位矩阵，用来变换roll、pitch和yaw三个向量的方向。平移矩阵就是一个简单的向量。由于（Bbi）的值是已知的，所以一旦知道了(Bpi)的值，就可以通过计算两点的距离得到脚的长度。



上面的等式实际上很简单，但是由于引入了矩阵理论，所以有很多项。下面是最终的反向运动方程（对于脚“i”）。



此系统的前向运动学方程相对复杂一些，由于处理上的要求，前向方程也不容易解。这里，比直接解方程更好的方法是使用迭代法，初始估计值代入方程、更新，然后重复，直到估计值的误差小于某一限定值。具体的计算方法在此就不再赘述，此方法对于以下的过程都可以通用。此方法是前文提及的牛顿迭代法的推广。关于此方法有很多相关文章，本文在此着重讨论前向运动方程的应用。第一步是估计初始姿态K，或者换种说法，估计(α , , γ, x, y, z)的值。对于一个运动控制器，初始估计值通常是(α , , γ, x, y, z)的受控位置。从此估计值，反推运动学方程，可以计算出执行器的长度，称之为 (g1, g2, g3, g4, g5, g6) ，或者以向量的形式写成g.，数学表达式如下：g = I(k)。

然后，基于估计值算出的长度与来自反馈设备的实际长度I相比较，得到“估值误差”e，可以写成e = g – l。

如果估值误差小于某一个限定值，那么此过程就此结束。如果估值误差不小于这个限定值，那么，就需要一个更好的估计值。这个过程一直重复，直到估计值足够完美（此时，这个估计值就被作为方程的解！）为了理解如何从数学上确定一个“更好的估计值”，首先考虑下面这个简单的微积分学例子。假设我们有一个通用函数y = f(x)，f是非线性的。如果我们要计算由x的变化所导致的y的变化，下面的等式有效：



现在，如果我们考察一小段f(x)的变化区间，我们可以将这个区间近似成线性关系，有如下等式：



上面的等式在基础微积分学通常被称为斜率局部逼近，可以写成下面的形式：



如果前面的表达式再重写一次，我们可以得到以x变化量为参数的等式：



现在，让我们将同样的思想，应用到这个等式上。考虑到我们有一个非线性函数g=I(k)，用于确定基于估计方位（k）和 估计脚长度（g）。从设备反馈，我们也知道脚的实际长度（I）。我们的估值误差就是e=g-l,，目标是如果估值误差不符合要求就为方位（k*）从新确定一个估计值。然后重复使用上面的方法：



上面的等式看起来很熟悉，它基本上就是我们在之前的简单微积分例子中展示的方程。我们可以简单地再一次重写这个方程，以得到方位 (k*)的新估值表达式：



矩阵dI(k)/dk就是我们所说的Jacobian矩阵。在将Jacobian矩阵应用于我们的上述等式之前必须先将其翻转，对于六阶矩阵就需要6×6求逆矩阵，这是一个复杂的数学推导。一旦姿态的新估计值计算出来，这个过程就可以再次重复，直到误差(e = g – l) 小于某个可以接受的级别。

如果反向运动方程已知，如下就是通用前向运动方程的推导步骤：
1.估计平台的初始姿态（k）。对于运动控制器，这通常就是初始受控姿态。
2.基于估计值，计算脚长度，使用反向运动方程(g=I(k))。
3.通过将此脚长度与从编码器获得的当前实际脚长度进行比较得到（e = g – l），如果误差大小小于某个限制，此算法就将当前值收敛于k，并且跳到第6步，如果误差超过此限制，继续第4步。
4.使用反向Jacobian矩阵产生新的估计值 (k*=k-inv(dI(k)/dk)*e)。
5.用第4步的结果更新估计值。
6.从第2步重新开始迭代。这个算法并不仅仅适用于六自由度系统，只要反向运动方程已知，它就可以用于推导任何系统的前向方程（有收敛解的系统）。

总结

一个强壮的控制机器人系统的方法的关键就是运动方程，这些方程不仅仅描述了系统的几何结构，他们也使得具有足够处理能力和速度的现代运动控制器，能够完成必要的计算，以提供对系统的平滑运动控制。运动方程通常在实时领域可以直接求解，然而，对于一些复杂系统，直接求解无法实现，可以采用一些算法，进行求解。高性能的现代控制器提供了特定的结构，可以在控制系统内固化这些等式，为很多领域提供了开发机器人系统的可能性。