3.1轨迹

 一条路径只是一个空间结构，及空间中由初始位姿过渡到最终位姿的图形，与路径不同，轨迹是具有特定时间属性的路径。例如，由A到B是路径，但如果规定了2m/s的速度，则变为A到B的轨迹。

3.1.1 平滑一维轨迹

 平滑一维轨迹常用时间的标量函数表示。这种函数的一个常见代表是时间多项式函数。该函数易计算，且可方便的提供连续性和边界条件。比较常见的是五次多项式:

Pasted image 20240221150803.png

 其中，t的范围为0到T。其一阶和二阶导数也是光滑的多项式：

Pasted image 20240221150952.png

 一般情况下速度和加速度边界条件为零。

将t=0，t=T带入上述3个方程，可得6个方程，写为矩阵形式为：

Pasted image 20240221151222.png

 该矩阵为方阵，可以使用线性代数方法求解（A,B,C,D,E,F）。

下面在MATLAB中计算

>> s=tpoly(0,1,50); 
%生成50×1列向量并打印
>> plot(s)

untitled.png

 如图所示，初始和终点速度加速度都为零。

也可设置为非零值：

>> s=tpoly(0,1,50,0.5,0);
>> plot(s)

1.png

 如图所示，初始速度为0.5，最终速度为0。

在实际应用中，为使机器人运动时间最短，应使其在最大速度上的时间尽量长。一种较理想的选择是采用混合曲线轨迹，即中间恒速，两侧加速和减速：

>> [s,sd,sdd]=lspb(0,1,50); 
%sd为速度，sdd为加速度
>> plot(s,sd,sdd)
>> plot(s);  
%打印图像
>> plot(sd);
>> plot(sdd);

 得到如下图像，

2.png

3.png

（速度）

4.png

（加速度）

 这种类型的轨迹也称为梯形轨迹。

（lspb函数也可通过第四个参数指定直线段速度。但是速度值不是任意的，过高或过低会导致函数返回错误。）

3.1.2多维的情况

 大多数机器人都有一个以上的运动轴或自由度，因此我们需要从初始位姿到最终位姿的多维平滑运动。

将标量轨迹扩展为向量是简单直接的，可使用工具箱中的mtraj函数完成：

%从(0,2)移动到(1,-1)
>> x=mtraj(@tpoly,[0 2],[1 -1],50);
>> x=mtraj(@lspb,[0 2],[1 -1],50);
>> plot(x)

 如图:

666 1.png

3.1.3多段轨迹

 在机器人应用中，经常要求机器人平滑地沿一条路径运动，并且不停顿的经过一个或多个中间节点。

为实现速度连续，我们只能舍弃让轨迹能达到每个中间点。

Pasted image 20240221160027.png

 汇接混合中的平均加速度为：

Pasted image 20240221160423.png

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

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

工具箱函数mstraj可以基于中间点矩阵生成一个多段多轴轨迹。

%4个中间点的两轴运动
>> via=[4,1;4,4;5,2;2,5];
>> q=mstraj(via,[2,1],[],[4,1],0.05,0);
>> plot(q)

untitled 1.png

从这个例子所给的参数表明，第一个轴的最大速度比第二个轴更大。然而在最后一个运动段，两轴是以相同的速度运动的，因为这段的运动时间是由最慢的轴决定的。

如果增加加速时间(mstraj函数最后一个参数):

>> q=mstraj(via,[2,1],[],[4,1],0.05,1);
>> plot(q)

1 1.png

轨迹会变得更圆滑，如图所示。这是因为增加加速时间意味着轨迹上可以有更多的时间来完成多项式曲线段的混合。

3.1.4三维空间姿态插值

 一个可行且常用的选择是尝试三角度表示法，如欧拉角或横滚-俯仰-偏航角,这样就可以使用线性插值法：

Pasted image 20240221162550.png

例如：

>> R0=rotz(-1)*roty(-1);
>> R1=rotz(1)*roty(1);
>> rpy0=tr2rpy(R0); rpy1=tr2rpy(R1);
>> rpy=mtraj(@tpoly,rpy0,rpy1,50);
>> tranimate(rpy2tr(rpy));

 由R0变化为R1。

54b04ace6c998840dd0d11626377b76c.gif

 由于姿态变化较大，可以看出坐标系的旋转轴线沿轨迹的变化。这个动作虽然平滑，但有时看起来也不太协调。

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

>>q0 = Quaternion(R0);
>>q1 = Quaternion(R1);
%插值
>>q = interp(q0,q1,[0:49]'/49);
>>about(q)
>>q[Quaternion] : 1x50(1656字节)

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

3.1.5笛卡尔运动

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

我们将初始位姿和最终位姿都表示为齐次变换矩阵：

>> T0=transl(0.4,0.2,0)*trotx(pi);
>> T1=transl(-0.4,-0.2,0.3)*troty(pi/2)*trotz(pi/2);
%插值
>> Ts=trinterp(T0,T1,[0:49]/49);
%Ts是一个三维矩阵：
>> about(Ts)
Ts [double] : 4x4x50 (6.4 kB)
>> tranimate(Ts)  
%动画

20240223_165627.gif

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

>> P=transl(Ts);
>> about(P)
P [double] : 50x3 (1.2 kB)

 将位置向量的变化画出：

>> plot(P)

9.png

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

>> Ts=trinterp(T0,T1,lspb(0,1,50));
>> P=transl(Ts);
>> plot(P)

ii.png

3.2时变坐标系

3.2.1旋转坐标系

 物体在三维空间中旋转时有一个角速度向量w=(w,w,,w₂)。这个向量的方向定义了瞬时转动轴，即在某个特定时间点坐标系旋转所绕的轴。力学中有一个众所周知的时变旋转矩阵微分表达式：

Pasted image 20240221174107.png

 S是一个斜对称矩阵，其三维情况下的具体形式如下：

Pasted image 20240221174221.png

 利用工具箱函数可得到：

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

S =

     0    -3     2
     3     0    -1
    -2     1     0

 工具箱函数vex有逆解功能，将斜对称矩阵转换为一个向量:

>> vex(S)'

ans =

     1     2     3

 R描述了标准正交旋转矩阵是如何作为一个角速度的函数变化的。