【机器人学、机器视觉与控制】（二）时间与运动_综合

文章目录

时间与运动
- 1 轨迹
- - 1.1 平滑一维轨迹
  - 1.2 多维的情况
  - 1.3 多段轨迹
  - 1.4 三维空间姿态插值
  - 1.5 笛卡尔运动
- 2 时变坐标系
- - 2.1 旋转坐标系
  - 2.2 增量运动

时间与运动

1 轨迹

一条路径只是一个空间结构——空间中从初始位姿过渡到最终位姿的一个图形。轨迹是具有特定时间属性的一条路径。例如，从A到B是路径，但如果规定了10秒的时间或 $2 ms^{-1}$ 的速度，则变成从A到B的轨迹。

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

1.1 平滑一维轨迹

我们从时间的标量函数开始讨论。这种函数的重要特征是它的初始值和最终值是确定的，而且函数是光滑的。所谓光滑是指它的低阶时间导数是连续的。通常速度和加速度都必须是连续的，有时加速度的导数或加速度率也需要连续。

这种函数的一个常见代表是时间多项式函数。多项式函数容易计算，而且可以方便地提供所需的连续性和边界条件。比较常用的是五次多项式:
$S(t)=At^5+Bt^4+Ct^3+Dt^2+Et+F$

其中时间 $t∈[0,T]t\in [0,T]$ 。其一阶和二阶导数也是光滑的多项式：
$S˙(t)=5t4+4Bt3+3Ct2+2Dt+E\dot{S}(t)=5t^4+4Bt^3+3Ct^2+2Dt+E$ $S¨(t)=20At3+12Bt2+6Ct+2D\ddot{S}(t)=20At^3+12Bt^2+6Ct+2D$

轨迹定义了位置、速度和加速度的边界条件，一般情况下速度和加速度的边界条件均为零。

在 $t = 0$ 和 $t = T$ 时分别将边界条件代入以上几个方程，得到6个方程，我们可以写成以下矩阵形式：
在这里插入图片描述
由于该矩阵是方阵，我们可以使用标准的线性代数方法来求解系数向量 $A, B, C, D, E, F)$ ，

使用poly生成一个五次多项式轨迹：

s=tpoly(0,1,50)

返回一个 $50×150\times1$ 的列向量，其值在0到1范围内分50个时间步平滑变化。我们可以绘制出该轨迹曲线：
在这里插入图片描述

相应的速度和加速度可以通过增加输出选项参数返回：

[s,sd,sdd]=tpoly(0,1,50);
subplot(1,2,1)
plot(sd)
title('速度')
subplot(1,2,2)
plot(sdd)
title('加速度')

在这里插入图片描述
可以看出初始和终点的速度和加速度都是零——函数默认值。初始速度和终点速度也可以被设置为非零值：

[s,sd,sdd]=tpoly(0,1,50,0.5,0);
subplot(3,1,1)
plot(s)
title('轨迹曲线')
subplot(3,1,2)
plot(sd)
title('速度')
subplot(3,1,3)
plot(sdd)
title('加速度')

在这里插入图片描述
在这种情况下，初始速度为0.5，最终速度为0。

结果也体现出项式轨迹的一个问题。非零的初始速度导致多项式轨迹的中间值超过终点值——轨迹中的峰值达到5，超出0到1的范围。

多项式轨迹另一个很实际的问题是它的速度
在这里插入图片描述

从图中可以看到，其速度在 $t = 25$ 时达到最大值，这意味着大多数时间的速度都远小于这个最大值。计算其平均速度为：

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

只有最大值的 $52$ ，一个真正的的机器人关节都有一个额定的最大速度，而且为了使关节运动时间最短，应使其运行在最大速度上的时间尽可能长。因此，我们希望速度曲线的顶部时一条平直线。

一种公认的较理想的选择的选择是采用混合曲线轨迹，即由中间的恒速段平直线加上两侧的加速段和减速段多项式曲线构成的轨迹，混合轨迹函数实现：

[s,sd,sdd]=lspb(0,1,50);  % 函数的参数与`tplot`的含义相同。
subplot(3,1,1)
plot(s)
title('轨迹曲线')
subplot(3,1,2)
plot(sd)
title('速度')
subplot(3,1,3)
plot(sdd)
title('加速度')

在这里插入图片描述
轨迹由一条线段（匀速）与两条抛物线混合而成，因此得名lspb，术语“混合“（blend)常用来指轨迹加入线性部分。

这种类型的轨迹由于其速度-时间曲线的形状也被称作梯形轨迹，并普遍应用于工业马达驱动中。

可以通过第四个输入参数为直线段指定一个速度：

[s,sd,sdd]=lspb(0,1,50,0.025);
subplot(3,1,1)
plot(s)
title('轨迹曲线')
subplot(3,1,2)
plot(sd)
title('速度')
subplot(3,1,3)
plot(sdd)
title('加速度')

在这里插入图片描述

1.2 多维的情况

大多数实用的机器人都有一个以上的运动轴或自由度。我们将其用向量形式表示为 $x∈R^M$ ， $M$ 代表自由度的数目。轮式移动机器人由它的位置 $(x ， y)$ 或位姿 $(x，y，θ)(x，y，\theta)$ 来描述，而关节臂式机器人的末端工具则有位置 $(x ， y ， z)$ ，姿态 $(θr，θp，θy)(\theta_r，\theta_p，\theta_y)$ ，或位姿 $(x，y，z，θr，θp，θy)(x，y，z，\theta_r，\theta_p，\theta_y)$ ，因此我们需要从初始位姿向量到最终位姿向量的多维平滑运动。将平滑的标量轨迹扩展成向量情况是非常简单直接的，在工具箱中可以使用函数mtraj完成。例如，分50个时间步从 $(0 ， 2)$ 移动到 $(1 ， ? 1)$ 可以表示为：

[s,sd,sdd]=mtraj(@tpoly,[0,2],[1,-1],50)
[s,sd,sdd]=mtraj(@lspb,[0,2],[1,-1],50)

可得到一个 $50×250\times 2$ 的矩阵 $x$ ，其中每一行对应一个时间步，每一列对应一个轴。输入给mtraj的第一个参数是一个函数，即tpoly或lspb，它生成一个标量轨迹。以lspb为例，绘制其轨迹为：

[s,sd,sdd]=mtraj(@lspb,[0,2],[1,-1],50)
subplot(3,1,1)
plot(s)
legend('x1','x2','location','BestOutside')
title('轨迹曲线')
subplot(3,1,2)
plot(sd)
legend('x1','x2','location','BestOutside')
title('速度')
subplot(3,1,3)
plot(sdd)
legend('x1','x2','location','BestOutside')
title('加速度')

在这里插入图片描述

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

x=[transl(T)',tr2rpy(T)']

如：

T=transl(1,0,0)*trotx(pi/2)*transl(0,1,0);
b=[transl(T)',tr2rpy(T)]
a=[0,0,0,0,0,0]
[s,sd,sdd]=mtraj(@tpoly,a,b,50)
subplot(3,1,1)
plot(s)
legend('x1','x2','x3','x4','x5','x6','location','BestOutside')
title('轨迹曲线')
subplot(3,1,2)
plot(sd)
legend('x1','x2','x3','x4','x5','x6','location','BestOutside')
title('速度')
subplot(3,1,3)
plot(sdd)
legend('x1','x2','x3','x4','x5','x6','location','BestOutside')
title('加速度')

在这里插入图片描述

1.3 多段轨迹

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

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

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

在这里插入图片描述

轨迹的第一段从速度为零的初始位姿 $x_1$ 开始加速，然后汇接入指向第二个位姿 $x_2$ 的直线段。汇接时间被设为常数 $t_{acc}$ ，在到达 $x_2$ 之前的 $t_{acc}/2$ 时间，直线轨迹又汇接上一段多项式曲线，持续 $t_{acc}$ 时间后再次汇接到从 $x_2$ 到 $x_3$ 的直线段，然后就一直重复这一过程。可以给每个直线段设定一个恒速值 $x˙k\dot{x}_k$ 。因此，汇接混合中的平均加速度为：
在这里插入图片描述

如果该轴的最大加速能力已知，那么就可以计算出最小混合时间。

对于多轴的情况，很可能在某个特定运动段其中一些轴要比其他轴需要移动更多距离，而如果各个关节有不同的速度限制时，将会使轨迹生成变得复杂。为解决上述问题，第一步需要确定哪个轴将是完成最慢的，依据是每个轴对应这个运动段的运动距离以及该轴最大可达速度。这样可以算出该段的运动持续时间和各轴实际所需的速度。这确保了所有轴在同一时间达到下一个目标 $x_k$ 。

工具箱函数mstraj可以基于中间点矩阵生成一个多段多轴轨迹。例如，有4个中间点的两轴运动可以用以下方法生成：

via=[4,1;4,4;5,2;2,5];
q=mstraj(via,[2,1],[],[4,1],0.05,0);
plot(q,'o')

在这里插入图片描述

函数mstraj的第一个参数是中间点矩阵，每点对应矩阵的一行。剩余的参数分别是：每轴的最大速度向量，每段的运动时间向量，起点各轴坐标，采样时间间隔，以及加速时间。函数mstraj返回一个矩阵，行对应的是时间每步，列对应的是各个轴。在没有提供输出参数时mstraj将绘制出轨迹曲线。从这个例子所给的参数表明，第一个轴的最大速度比第二个轴更大。然而在最后一个运动段，两轴是以相同的速度运动的，因为这段的运动时间是由最慢的轴决定的。

如果增加加速时间：

via=[4,1;4,4;5,2;2,5];
q=mstraj(via,[2,1],[],[4,1],0.05,1);
plot(q,'o')

在这里插入图片描述
轨迹会变得更圆滑。轨迹会变得更圆滑。这是因为增加加速时间意味着轨迹上可以有更多的时间来完成多项式曲线段的混合。该函数也接受可选的起点和终点的速度参数，而且 $t_{acc}$ 可以是一个向量，给出每次混合的加速度时间。

记住，这个函数只是简单地对向量代表的位姿进行插值。在这个例子中，向量被假定为笛卡儿坐标向量，但这个函数也适用于欧拉角或横滚–俯仰-偏航角，然而这个函数不是对坐标系旋转进行插值的理想方式。

1.4 三维空间姿态插值

在机器人学中，我们经常需要对姿态进行插值。例如，我们需要机器人的末端执行器平滑地从姿态 $ξ0\xi_0$ 改变到 $ξ1\xi_1$ 。即我们需要某个函数 $ξ(s)=σ(ξ1,ξ,s)\xi(s)=\sigma(\xi_1,\xi,s)$ ，其中 $s∈[0,1]s\in[0,1]$ ，函数的边界条件为 $σ(ξ0,ξ1,0)=ξ0\sigma(\xi_0,\xi_1,0)=\xi_0$ , $σ(ξ0,ξ1,1)=ξ1\sigma(\xi_0,\xi_1,1)=\xi_1$ ，而且 $σ(ξ0,ξ1,s)\sigma(\xi_0,\xi_1,s)$ 要能平滑地经过 $s$ 内定义的一系列中间位姿。

如果位姿由正交旋转矩阵表示， $ξ?R∈SO(3)\xi\sim R\in SO(3)$ ，可以考虑用简单的线性插值 $σ(R0,R1,s)=(1?s)R0+sR1\sigma(R_0,R_1,s)=(1-s)R_0+sR_1$ 。但这是不可行的，这样插值后得到的通常不再是有效的正交矩阵，因为正交矩阵必须满足列向量范数为1以及列向量之间正交的条件。

一个可行且常用的选择是尝试三角度表示法，入欧拉角和横滚-俯仰-偏航角， $ξ?Γ∈S3\xi\sim \Gamma\in S^3$ ，这样就可以使用线性插值法： $σ(Γ0,Γ1,s)=(1?s)Γ0+sΓ1\sigma(\Gamma_0,\Gamma_1,s)=(1-s)\Gamma_0+s\Gamma_1$

例如，我们定义了两个姿态：

R0=rotz(-1)*roty(-1);
R1=rotz(1)*roty(1);

得到等价的横滚-俯仰-偏航角

rpy0=tr2rpy(R0);
rpy1=tr2rpy(R1);

然后分50个时间步在它们之间生成一条轨迹：

rpy=mtraj(@tpoly,rpy0,rpy1,50);

通过动画演示：

tranimate(rpy2tr(rpy));

在这里插入图片描述
由于姿态变化较大，可以看出坐标系的旋转轴线沿轨迹的变化。这个动作虽然平滑，但有时看起来也不太协调。另外，如果在所采用的三角度系统中有或之一接近奇异点，也会出现问题。

采用单位四元数的插值法只比三角度向量法复杂一点点，它在空间中产生一个绕固定轴旋转的姿态变化。首先，我们利用工具箱函数找到与这两个位姿等价的四元数:

R0=rotz(-1)*roty(-1);
R1=rotz(1)*roty(1);
q0=Quaternion(R0);
q1=Quaternion(R1);

然后对它们进行插值：

q=interp(q0,q1,[0:49]'/49);
about(q)

在这里插入图片描述
得到一个50个四元数对象的向量。我们也可以用动画演示轨迹变化：

四元数插值法是通过使用球型线性插值(slerp)来实现的，其中单位四元数要在一个四维超球面上沿一个大圆路径运动。而在三维空间中的结果就是绕定轴转动。

1.5 笛卡尔运动

另一个常见的需求是在 $S E (3)$ 中生成两位姿之间的光滑路径，它同时涉及位置及姿态的变化。在机器人学中这通常被称作笛卡尔运动。
我们将初始和最终位姿都表示成齐次变换矩阵：

T0=transl(0.4,0.2,0)*trotx(pi);
T1=transl(-0.4,-0.2,0.3)*troty(pi/2)*trotz(-pi/2);

工具箱函数trinterp提供了沿路径单位话距离 $s∈[0,1]s\in[0,1]$ 中的位姿插值。例如，其中间的位姿为：

trinterp(T0,T1,0.5)

在这里插入图片描述
其中平移部分是用线性插值，旋转部分是用四元数插值法interp进行球面插值。
两个位姿之间50个分布的轨迹可用以下方法生成：

Ts=trinterp(T0,T1,[0:49]/49);

动画：

tranimate(Ts)

在这里插入图片描述
动画展示了坐标系从位姿 $T 1$ 平移、旋转到位姿 $T 2$ 的变化过程。

该轨迹的平移部分由以下方式获得：

P=transl(Ts)

每一行对应每个时间分步上的位置向量。
用plot函数画出：

plot(P)
legend('x','y','z')

在这里插入图片描述

以横滚-俯仰-偏摆角格式画的姿态变化曲线：

rpy=tr2rpy(Ts);
plot(rpy)
legend('横滚','俯仰','偏摆')

在这里插入图片描述
从图中看到，位置坐标随时间的变化既平滑又呈线性，而姿态随时间的变化也平滑，但不是线性的。

然而，平移运动在第一个和最后一个点上的速度和加速度是不连续的。问题在于虽然轨迹在空间中是平滑的，但沿轨迹的步距s在时间上并不平滑。路径起点上的速度值从零跳跃到有限值，然后在终点又突降至零——没有相应的起始加速和结束减速。我们可以用前面讨论过的标量函数tpoly和lspb来创建一个时间上平滑的s，这样沿路径的运动也平滑了。我们只需将传递给trinterp的第三个参数更改为一个沿路径的单位化距离向量即可:

Ts=trinterp(T0,T1,tpoly(0,1,50))

P=transl(Ts)
plot(P)
legend('x','y','z')

在这里插入图片描述

rpy=tr2rpy(Ts);
plot(rpy)
legend('横滚','俯仰','偏摆')

在这里插入图片描述

工具箱还提供了一个ctraj来进行轨迹插值。

Ts=ctraj(T0,T1,50)

2 时变坐标系

上面讨论了如何产生坐标系的运动，其中包含平移速度和旋转速度两部分。平移速度代表了坐标系原点位置的变化率，而旋转速度则要更复杂一些。

2.1 旋转坐标系

物体在三维空间中旋转时有一个角速度向量 $w=(w_x,w_y,w_z)$ 。这个向量的方向定义了瞬时转动轴，即在某个特定时间点坐标系旋转所绕的轴。通常情况下，这个轴是随时间变化而改变的。向量长度代表绕该轴的旋转率。
力学中有一个总所周知的时变旋转矩阵微分表达式：
$R˙(t)=S(w)R(t)\dot{R}(t)=S(w)R(t)$
其中， $R˙∈SO(2)\dot{R}\in SO(2)$ 或 $S O (3)$ ， $S (.)$ 是一个斜对称矩阵，其三维情况下的具体形式为：
在这里插入图片描述

利用工具箱，可以得到：

S=skew([1,2,3])

在这里插入图片描述
工具箱函数vex具有逆解的功能：将斜对称矩阵转换成一个向量：

vex(S)

$R˙\dot{R}$ 是什么呢：

在这里插入图片描述

2.2 增量运动

现在考虑一个坐标系经微小旋转从 $R_0$ 变到 $R_1$ ，这时可以将方程写为：
在这里插入图片描述

对于无穷小角度变化的乘法来说，旋转变化的顺序是可交换的：
在这里插入图片描述

获得上面矩阵对应的微小旋转角 $δθ\delta_{\theta}$

vex(Rdelta-eye(3,3))

Reference：
《机器人学+机器视觉与控制+MATLAB算法基础》 Peter Corke 著刘荣等译