Matlab机器人工具箱（二）

时间与运动

上一章主要介绍了在二维和三维空间中如何描述位姿。这一章则将上章拓展到对象，其位姿是时间的函数。
分两个部分，第一个部分主要讨论如何产生一系列短暂的位姿序列，一条足够光滑的从起始位姿到终止位姿的轨迹；
第二部分主要讨论位姿改变速率的概念，即其导数，以及如何将从机械学角度比如速度和角速度等方面将这些概念
联系起来。

轨迹

注意路径（path）和轨迹（trajectory）的区别，路径是指从A到B的空间概念，而轨迹是有特定时间点的一条路径。
轨迹的一个重要特征就是光滑（包括位置和姿态都关于时间是光滑的）。
接下来讨论如何在一维空间中产生光滑的轨迹，然后扩展到多维空间，然后扩展到分线段的轨迹。

• 一维光滑轨迹

  我们从一个时间函数开始讨论。这个函数的特点是其初始值和终止值是给定的，并且它是光滑的。光滑在这里意味着其一阶微分是连续的。
  多项式函数就是这样一个函数。我们经常使用五次多项式，其加速度是光滑的三次曲线，加加速度是光滑的抛物线。

  Matlab机器人工具箱提供了函数tpoly来产生 一个五次多项式轨迹：

  s=tpoly(0,1,50);

  产生一个0到1之间的五次轨迹，时间间隔是50次。

  我们也可以得到其速度和加速度曲线：

  [s,sd,sdd]=tpoly(0,1,50);

  其中sd，sdd就分别是速度和加速度。可以用plot(s),plot(sd)等将这些曲线绘制出来

  也可以在tpoly函数中给出初始值和终止值：
  s=tpoly(0,1,50,0.5,0);

  其中0.5就是速度初始值，0是速度终止值。

  但是这样产生的轨迹其速度分配不合理，平均速度只有峰值的百分之52，这就说明，对于大多数时候，速度都远比峰值少。
  实际的机器人的速度都会被很好的定义，以让其尽可能多地在最大速度下运行，我们希望速度曲线在顶部的时候尽量平坦，而不是出现一些山峰。

  一个广泛使用的是组合轨迹：其速度是由一个常数和多项式组成的加速和减速阶段。我们可以在matlab中用lspb()函数来产生这种轨迹：

  s=lspb(0,1,50);

  它的参数的意义和tpoly一样，其轨迹是一段线性直线段和抛物线组合起来的，和tpoly一样，也可以得到其速度和加速度曲线：

  [s,sd,sdd]=lspb(0,1,50);

  lspb函数有第四个参数，是给定速度峰值的：

  s=lspb(0,1,50,0.025)；
  s=lspb(0,1,50,0.035);

  最后绘制的轨迹及其速度，加速度曲线如下图所示：

• 多维情况

  许多机器人都是有几个轴的运动或多个自由度，我们通常都用一个向量来表示这些自由度。轮式机器人用它的位置（x,y）或位姿（x,y,theta）来表征。机器人的工具有位置(x,y,z)，姿态（r,p,y）或者位姿（x,y,z,r,p,y）。我们因此需要一个从起始位姿到终止位姿的多维空间的运动。

  在机器人工具箱中，使用函数mtraj()来实现。例如分50步从(0,2)到（1,-1）：

• 多维分割轨迹

  在机器人应用中通常需要经过几个中间点沿着一条光滑轨迹运动。但有时候为了得到连续的速度，我们通常都会一些中间点，不会精确地到达这些中间点，而是会和之前一样，由一些多项式组合而成。

机器人工具箱提供了函数mstraj()来产生一条基于中间点的多段多轴的轨迹。例如2轴经过4个中间点的轨迹就
可以这样产生：

其中via就是4个中间点的坐标，mstraj的第一个参数就是要通过的中间点，第二个参数就是每个轴的最大的速度
所组成的向量，第三个参数是每个段的间隔所组成的向量，第四个参数是初始的轴坐标，第六个参数是取样周期，
最后一个参数是加速时间。mstraj函数会返回一个矩阵，行表示时间间隔，列表示坐标值。

• 三维姿态的插补

  在机器人学中，我们通常需要插补姿态，例如，要求机器人末端从一个姿态平滑地运动到另一个姿态。

  一般使用欧拉角或RPY来做姿态插补，而不用正交矩阵，例如，我们定义两个姿态：

可以找到对应的RPY：

然后可以在这两个姿态之间以50个步长创建一条轨迹：

可以以动画的形式来演示：

也可以用单位四元素法来做插补，例如上面的例子中，我们先找到其单位四元素：

然后进行插补：

用tranimate（q）就可以动态显示了。

笛卡尔运动

上面只讨论了从一个姿态变换到另一个姿态的运动，而没有平移运动，然后在实际的机器人系统中，一般都是两
种运动都有的，所以当设计到从一个位姿运动到另一个位姿的时候，我们叫做笛卡尔运动。

我们用齐次变换来表示起始和终止位姿：

机器人工具箱中提供函数trinterp()提供这种插补，例如：

在这里平移部分用的是线性插补，姿态部分用的方式还是四元素插补所用的方法：interp。
所以一条在两个位姿之间的步长50的轨迹就可以产生了：

其中Ts是一个三维矩阵，前二维表示齐次变换矩阵，后一维表示时间步长：

我们可以看到路径上第一个位姿：

我们也可以tranimate(Ts)看到它的变换过程：

我们可以得到这个轨迹的平移部分：
>>p=transl(Ts)；

可以得到一个50x3的矩阵，每一行就是轨迹点的位置，我们可以绘制出来：
>>plot(P)

我们也可以得到其姿态矩阵：
>>rpy=tr2rpy(Ts);
>>plot(rpy)

从上图中可以看到平移运动在开始和结束的地方有速度和加速度的不连续。我们可以用tpoly和lspb函数来使其
连续。让上述两个函数作为trinterp函数的第三个参数。
>>Ts=trinterp(T0,T1,lspb(0,1,50));

从图中可以看出其速度和加速度都是连续的，但轨迹是没有改变的。机器人工具箱提供了另外一个比较方便的函数：
>>Ts=ctraj(T0,T1,50);

随时间变化的坐标系

前面部分讨论的是如何产生有平移和旋转速度分量的坐标系运动。平移部分比较简单，就是位置的变换速率，而姿
态的速度就有点复杂。
我们已经注意到旋转的不可互换性，但是对于无穷小的角度改变姿态的乘法运算却是可以互换的。我们可以用数值
来验证一下：

从上面可以看到当旋转的角度很小的时候，是可以相互交换顺序的。我们也可以通过函数vex得到角度的变化：

那么对于两个相对变化很小的两个姿态，我们可以用工具箱函数tr2delta来得到其之间的变化差值：

上面的d就是两个姿态T0和T1之间的姿态差值，我们也可以用工具箱函数delta2tr来用这个差值得到T0经过这个差值变换之后的结果：

从上面可以看出这样得到的结果和T1其实是很接近的，之所以会有差距是因为这个差值并不是足够小，所以会有一
些差距。