使用 pchip在路径点之间插值以生成平滑的轨迹

在路径点之间插值以生成平滑的轨迹。使用 pchip 避免过冲，这可能会违反机器人的关节限制。

framerate = 15; % 设置帧率
r = rateControl(framerate); % 控制帧率


tFinal = 10; % 轨迹的最终时间
tWaypoints = [0,linspace(tFinal/2,tFinal,size(qWaypoints,1)-1)]; % 时间点设置
numFrames = tFinal*framerate; % 总帧数
qInterp = pchip(tWaypoints,qWaypoints',linspace(0,tFinal,numFrames))'; % 插值生成平滑轨迹

在使用 pchip 函数时传入参数中使用转置符号（'）的原因是为了确保输入数据的维度和格式符合函数的要求。让我们仔细看一下相关的参数和转置的原因：

1. pchip 函数的基本用法

pchip 函数的基本用法如下：

vq = pchip(x, v, xq)

• x: 原始数据点的自变量（通常是时间或位置），要求是一个向量。
• v: 原始数据点的函数值，通常是一个与 x 相对应的向量或矩阵。
• xq: 需要进行插值的自变量点，要求是一个向量。
• vq: 返回的插值结果。

2. 数据的维度和排列方式

假设我们有以下数据：

• qWaypoints: 一个 M x D 矩阵，M 是路径点的数量，D 是机器人关节的数量。每一行代表某个时间点机器人的关节位置。
• tWaypoints: 一个 1 x M 向量，表示每个路径点对应的时间戳。

根据 pchip 函数的要求：

• tWaypoints 作为自变量 x，应该是一个行向量。
• qWaypoints 作为函数值 v，通常要求是一个与 x 兼容的矩阵。

3. 转置的原因

• 默认情况下，qWaypoints 是 M x D 的矩阵，其中每一行代表一个路径点处的关节位置（M 是路径点的数量，D 是关节的数量）。然而，在插值时，pchip 期望 v 的每一列对应一个 x 的值（即 tWaypoints 的一个时间点）。

• 为满足这一要求，我们需要对 qWaypoints 进行转置，使得它变为 D x M 的矩阵。这样，pchip 就可以正确地将每一列数据与 tWaypoints 的相应时间点对齐。

qInterp = pchip(tWaypoints, qWaypoints', linspace(0, tFinal, numFrames))';

• qWaypoints': 通过转置，原来的 M x D 矩阵变成 D x M，每一列现在对应一个时间点的所有关节位置。
• pchip 函数完成插值后，返回的插值结果是一个 D x numFrames 的矩阵。
• 最后再次转置 qInterp，将结果恢复为 numFrames x D，其中每一行对应插值后的关节位置。

总结

通过对 qWaypoints 使用转置符号 '，可以确保 pchip 函数在插值时正确匹配时间戳和关节位置数据的维度。第一次转置是为了使每个关节的插值计算可以与时间对应，插值后的矩阵再转置回来，以便与 qInterp 的预期输出格式（numFrames x D）匹配。