机器人学——时间与运动

轨迹

路径：空间中从初始位姿到最终位姿的图形（空间结构）
轨迹：具有特定时间属性的一条路径，即包含了完成路径所需的时间或速度。例如从A到B是一条路径，但如果规定了10s的时间或2m/s的速度，就变成了轨迹。
轨迹的一个重要特征是要平滑，即位姿对时间的导数是连续的。

一维轨迹

从时间的标量函数开始讨论，这种函数的重要特征是初始值和终止值都是确定的，并且函数是平滑的，平滑是指它的低阶时间导数是连续的，有时加速度率也是连续的。
通常用时间的五次多项式函数描述位置：

初末状态若已知，得六个等式，六个未知数（选取五次多项式的原因）

MATLAB工具箱生成一个五次多项式：

[s,sd,sdd] = tpoly(0,1,50,0,0);%返回的参数为路径，速度，加速度

一个真正的机器人关节都有一个额定的最大速度，而且为了使关节运动时间最短，应使其运行在最大速度上的时间尽可能长。因此我们希望速度曲线的顶部是一条直线。
混合曲线轨迹：延长最大速度运行时间，使得速度曲线顶部为一平直线，两侧为加减速段。

[s,sd,ssd] = lspb(0,1,50);%lspb第四个参数为直线段速度指定

多维情况

大多数机器人都有一个以上的运动轴或自由度。我们将其用向量形式表示为$x\in {\mathbb{R}}^M$，M代表自由度。轮式机器人由它的位置(x,y)或位姿$(x,y,\varphi )$表示 ，机械臂末端工具则由位置(x,y,z)，姿态$({\varphi }_r,{\varphi }_p,{\varphi }_y)$或位姿$(x,y,z,{\varphi }_r,{\varphi }_p,{\varphi }_y)$表示。因此需要从初始位姿向量到最终位姿向量的多维平滑运动。

x = mtraj(@tpoly,[0,2],[1,-1],50);%分50个时间步从(0,2)运动到(1,-1)
plot(x);
xlabel('x1')
ylabel('x2')

x = mtraj(@lspb,[0,2],[1,-1],50);%分50个时间步从(0,2)运动到(1,-1)
plot(x);
xlabel('x1')
ylabel('x2')

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

x = [transl(T);tr2rpy(T)]%平移量+旋转角

多段轨迹

机器人应用中经常需要机器人平滑地沿一条路径运动，并不停顿地经过一个或多个中间节点。
为了实现速度连续，舍弃中间点位置，采用直线和曲线拟合，就像：

对于多轴的情况，很可能在某个特定运动段其中一些轴要比其他轴需要移动更多距离，如果各个关节有不同的速度限制时，将会使轨迹生成变得更加复杂。为了解决上述问题，第一步需要确定哪个轴将是完成最慢的，依据每个轴对应的这个运动距离以及轴的最大可达速度。
工具箱mstraj可以基于中间点矩阵生成一个多段多轴轨迹

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

mstraj第一个参数是中间点矩阵，每点对应矩阵的一行；第二个参数是每轴的最大速度向量；第三个参数是每段的运动时间向量；第四个参数是起点各轴坐标；第五个向量是采样时间间隔；最后一个参数是加速时间，加速时间越长，轨迹会更圆滑。

三维空间姿态插值

在机器人学中，我们经常需要对姿态进行插值。例如，我们需要机器人的末端执行器平滑地从姿态${\xi }_0$到${\xi }_1$。利用某个函数$\xi (s)=\sigma ({\zeta }_0,{\zeta }_1,s)$，其中$s\in [0,1]$，函数的边界条件为$\sigma ({\zeta }_0,{\zeta }_1,0)={\zeta }_0$，$\sigma ({\zeta }_0,{\zeta }_1,1)={\zeta }_1$，而且$\xi (s)=\sigma ({\zeta }_0,{\zeta }_1,s)$要能平滑地经过s内定义的一系列中间位姿。
利用三角度表示法是可行的，如欧拉角或横滚-俯仰-偏航角，$\zeta \sim \Gamma \in {\mathbb{R}}^3$，这样就可以使用线性插值法：

%定义两个姿态
R0 = rotz(-1/pi*180)*roty(-1/pi*180);
R1 = rotz(1/pi*180)*roty(1/pi*180);
%得到等价横滚-俯仰-偏航角
rpy0 = tr2rpy(R0);
rpy1 = tr2rpy(R1);
%分50个时间步在它们之间生成一条轨迹
rpy = mtraj(@tpoly,rpy0,rpy1,50);

笛卡尔运动

笛卡尔运动是两位姿（位置及姿态）之间光滑路径的生成。

%定义齐次变换矩阵
T0 = transl(0.4,0.2,0)*trotx(180);
T1 = transl(-0.4,-0.2,0.3)*troty(90)*trotz(-90);

工具箱trinterp提供了沿路径单位化距离$s\in [0,1]$中的位姿插值，其中平移部分是线性插值法，旋转部分是四元数插值法。

Ts = trinterp(T0,T1,[0:49]/49);%生成每个时间步的齐次变换矩阵
%about(Ts);
%动画展示
tranimate(Ts)
%平移轨迹部分获得
P = transl(Ts);
figure(1)
plot(P);
%横滚-俯仰-偏航角轨迹获得
rpy = tr2rpy(Ts);
figure(2)
plot(rpy);

从图中可以看出，位置坐标随时间线性变化，而姿态坐标随时间变化不规律且不平滑。并且，平移运动在第一个点和最后一个点的速度和加速度也是不连续的，起点和终点的速度与加速度都出现的有限值的跳跃。为此，我们可以用前面讨论过的标量函数tpoly和lspb来创建一个时间上平滑的s，这样沿路径的运动就平滑了。

Ts = trinterp(T0,T1,lspb(0,1,50));

这样做对于轨迹来说是不变的，但这时坐标系会沿路径逐渐加速到一个恒定速度，然后结束是再减速。
工具箱还提供了一个速记函数ctraj来进行轨迹插值。

Ts = ctraj(T0,T1,50)