机器视觉与控制——时间与运动

前言

完成任何事情都需要时间，这就是时间存在的唯一理由。我们学习了如何在二维和三维空间中来描述物体的位姿。本章将在前一章的基础上对某些概念进行延展，研究位姿为时间函数的物体。对于机器人而言，我们希望创建一个让机器人能跟从的随时间变化的位姿，例如机器人末端执行器能沿着既定的位姿轨迹去抓住一个物体。本章会讨论如何生成一个位姿的时间序列，即一条轨迹，以便从初始位姿平滑地转变到最终位姿。并且会讨论位姿变化率的概念，即位姿的时间导数，以及它与力学中速度和角速度概念的相关性。有了这些知识，我们就能够在已知速度和角速度传感器的测量值的情况下，及时算出一个移动物体的预计位姿。这是所谓的位姿逆问题，也是惯性导航中的基本原理。

轨迹

一条路径只是一个空间结构——空间中从初始位姿过渡到最终位姿的一个图形。轨迹是具有特定时间属性的一条路径。轨迹的一个重要特征是要平滑——位置和姿态随时间流畅地变化。我们将从如何在一维空间产生平滑的轨迹开始，然后扩展到多维的情况，最后讨论如何生成分段线性轨迹，它能使机器人不间断地经过一系列中间点。

l 平滑一维轨迹

我们从时间的标量函数开始讨论。这种函数的重要特征是它的初始值和最终值是确定的，而且函数是光滑的。所谓光滑是指它的低阶时间导数是连续的。通常速度和加速度都必须是连续的，有时加速度的导数或加速度率也需要连续。这种函数的一个常见代表是时间多项式函数。多项式函数容易计算，而且可以方便地提供所需的连续性和边界条件。

可以发现，随着直线段速度的增大，该段占有的时间会减少,且最终会变为零。事实上，速度值是不能任意选择的，过高或过低的最大速度值都将产生一个不可行的轨迹，且函数返回错误。

l 多维的情况

大多数实用的机器人都有一个以上的运动轴或自由度。我们将其用向量形式表示为x∈R",M代表自由度的数目。轮式移动机器人由它的位置(x,y)或位姿(x,y,θ)来描述，而关节臂式机器人的末端工具则有位置(x,y,z),姿态(θ,,0,,θ,),或位姿(x,y,z,θ,,θ₂,θ,)。因此我们需要从初始位姿向量到最终位姿向量的多维平滑运动。

将平滑的标量轨迹扩展成向量情况是非常简单直接的，在工具箱中可以使用函数mtraj完成。例如，分50个时间步从(0,2)移动到(1,-1)可以表示为

>>x =mtraj(@tpoly,[02],[1 -1],50);

>>x =mtraj(@lspb,[02],[1 -1],50);

可得一个50×2的矩阵x,其中每一行对应一个时间步，每一列对应一个轴。输人给mtraj

的第一个参数是一个函数，即tpoly或lspb,它生成一个标量轨迹。以1spb为例，绘制其轨迹为

>>plot(x)

对于三维空间中的位姿问题，可以考虑先用以下方法将位姿齐次矩阵T转换为一个六维向量：

>>x=[transl(T);tr2rpy(T)']

然后再产生相应的轨迹。不过后面会看到，对于描述姿态的3个角度的插值会存在一定的限制。

l 多段轨迹

机器人应用中经常要求机器人平滑地沿一条路径运动，并不停顿地经过一个或多个中间节点。这样做或许是要在工作空间中避开障碍物，也可能是为了执行一项需要高精度连续轨迹的任务，如在制造业中要涂抹一圈密封胶。

为了使问题一般化，考虑该轨迹含有N个中间点x₄,k∈[1,N],因此有N-1个运动段。由上节可知，x₄εR”是位姿的一种向量表示。

机器人从静止状态x₁开始运动，到xv结束停止，但在通过(或接近)中间点时不能停顿。生成这个运动轨迹的最大问题是过约束。为了实现速度连续，我们只能舍弃让轨迹能达到每个中间点。在一维情况下这个问题更容易理解，如图3.4所示。该运动轨迹包括直线运动段与多项式曲线段的混合，如同1spb,但在这里我们选择了五次多项式，因为它们能够匹配位置、速度和加速度在起点和终点的边界条件。 

l 笛卡尔运动

另一个常见的需求是在SE(3)中生成两位姿之间的光滑路径，它同时涉及位置及姿态的变化。在机器人学中这通常被称作笛卡儿运动。 

时变坐标系

l 旋转坐标系

物体在三维空间中旋转时有一个角速度向量w=(w₄,w,,w₂)。这个向量的方向定义了瞬时转动轴，即在某个特定时间点坐标系旋转所绕的轴。通常情况下，这个轴是随时间变化而改变的。向量长度代表绕该轴的旋转率——这方面它类似于2.2.1.5节中旋转的角-轴表示法。 它描述了标准正交旋转矩阵是如何作为一个角速度的函数变化的。

l 增量运动

考虑一个坐标系经微小旋转从R₀变到R，该位姿变化(位移)由相对于世界坐标系的平移增量和旋转增量组成。给定这个位移量以及初始位姿，可求得最终位姿为 ，它非常接近于上面Tl给出的真实值，误差是由于位移量不是无穷小这一事实所致。

l 惯性导航系统

惯性导航系统是一个“黑匣子”,用来测算它相对于惯性参考系(宇宙)的速度、方向和位置。它无需外部信号输入，如来自卫星的无线电信号，这使得它非常适合用于潜艇、航天器和弹道导弹。惯性导航系统主要测量其加速度和角速度，并整合出它们的时间关系。 

我们将矩阵δ,S(w)R(t)与一个正交旋转矩阵相加，但这存在问题，因为其结果不是一个正交矩阵。然而如果被加项很小，那么结果会接近于正交矩阵，并且可以对它进行矫正。这一过程称为正交化，其强化了对正交矩阵中元素的限制。 

相比于正交旋转矩阵法，四元数法更常用于捷联式惯性导航系统的旋转更新方程。当然，两种方法的结果是相同的，只是四元数法的计算成本明显更少。

总结

本章从两个视角分别探讨了随时间变化的位姿。第一个是创建一系列机器人要跟从的位姿，即轨迹。轨迹的一个重要特点是要平滑——位置和姿态随时间连续变化。我们先从在一维情况下生成平滑轨迹的方法开始，然后扩展到多维情况，最后是要求经过一系列中间点的分段线性轨迹。第二个视角是研究正交旋转矩阵的时间导数，以及它们与力学中诸如速度和角速度这些概念的联系。这使我们可以求解位姿的逆问题，即先取得传感器测量值，再估算出一个移动物体的位姿——惯性导航原理。旋转插值和角速度积分都用到正交旋转矩阵法和四元数法。两种方法的结果是等价的，但四元数法的公式更简洁、计算更有效。