主要内容

利用四元数SLERP定位低通滤波器

打开脚本

此示例显示如何使用球面线性插值（SLERP）来创建四元数和低通滤波器嘈杂轨迹的序列。SLERP是一种常用的计算机图形技术，用于创建旋转对象的动画。

SLERP概述

考虑一对四元数q_0美元和q_1美元．球面线性插值允许您创建一个四元数序列，平滑变化之间q_0美元和q_1美元以恒定的角速度。SLERP使用插值参数h这可以在0到1之间变化，并确定输出四元数所在的近距离q_0美元或者q_1美元．

四元数SLERP的原始公式是由Ken shomake给出的[1):

$$Slerp(q_0,q_1,h) = q_1(q_1^{-1} q_2)^h$$

正弦曲线的另一种形式(用于slerp函数实现):

$ $ Slerp (q_0 q_1, h) = \压裂{\罪((1小时)\θ)}{\罪\θ}q_0 + & # xA;\压裂{\罪(h \θ)}{\罪\θ}q_1 $ $

在哪里$ \ theta $是四元零件的点产品。注意$ \ theta = dist（q_0，q_1）/ 2 $．

SLERP与四元零件的线性插值

考虑下面的例子。从欧拉角构建两个四元数。

Q0 =四元数（[ -  80 10 0]，“eulerd”，“ZYX股票”，“帧”）;Q1 =四元数（[80 70 70]，“eulerd”，“ZYX股票”，“帧”）;

找到30％的四元数Q0来Q1,指定slerp参数为0.3。

P30 = SLERP（Q0，Q1,0.3）;

要查看插值四元数的欧拉角表示，使用eulerd函数。

eulerd（p30，“ZYX股票”，“帧”）
Ans = -56.6792 33.2464 -9.6740

在介于之间创建平滑的轨迹Q0和Q1,指定slerp插值参数作为0到1之间的均匀间隔数的向量。

dt = 0.01;h = (0: dt: 1)。';trajSlerped = slerp(q0, q1, h);

将SLERP算法的结果与轨迹相比Q0和Q1，利用简单线性插值(LERP)的每个四元数部分。

PartsLININTERP = INTERP1（[0; 1]，紧凑（[Q0; Q1]），H，“线性”）;

请注意，线性插值不给出单位四元数，因此必须归一化。

trajLerped =正常化(四元数(partsLinInterp));

计算每一种方法的角速度。

avslerp = helperquat2av（trajslerped，dt）;Avlerp = Helperquat2av（trajlered，dt）;

画出两组角速度。注意SLERP的角速度是恒定的，但它在线性插值中是不同的。

sp = allowerslerpplotting;sp.plotangularvelocities（avslerp，avlerp）;

SLERP以恒定速率产生平滑旋转。

低通滤波SLERP

SLERP还可以用于制作更复杂的功能。这里使用SLERP对噪声轨迹进行低通滤波。

通过从嘈杂的旋转向量形成四元线，可以构造旋转噪声。

rcurr = rng (1);σ= 1 e 1;noise = sigma .* (rand(numel(h)， 3) - 0.5);qnoise =四元数(noiserv,“rotvec”）;rng (rcurr);

破坏轨道trajslerped.在有噪声的情况下，用噪声向量增量地旋转轨迹qnoise．

trajnoised = trajSlerped .* qnoise;

你可以使用如下形式的单极滤波器平滑实值信号:

$$ y_k = y_{k-1} + \alpha(xk - y_{k-1}) $$

此公式基本上表示新的过滤状态求和$ k美元是否应该移向当前输入xk美元步长与当前输入和当前滤波器状态之间的距离成正比$ y_ {k-1} $．

这种方法的精神通知四元数序列如何是低通滤波。要做这个，都是dist和slerp使用功能。

的dist函数返回由两个四元数应用的旋转差的弧度度量值。的范围dist函数是半开区间[0,pi)。

的slerp功能用于将滤波器状态转向电流输入。当输入和电流滤波器状态之间的差异很大时，它会更朝向输入。dist，而当dist给出一个小价值。插值参数到slerp在闭区间[0,1]内，那么输出dist必须重新归一化到这个范围。但由于插补参数的全范围[0,1]性能较差，所以被限制在较小的范围内hrange以其为中心hbias．

hrange = 0.4;Hbias = 0.4;

限制低的和高的到区间[0,1]。

Low = max(min(hbias - (hrange./2)， 1)， 0);High = max(min(hbias + (hrange./2)， 1)， 0);hrangeLimited = high - low;

初始化过滤器并预分配输出。

y = trajnoisy（1）;过滤器初始状态%qout = zeros（尺寸（y），“喜欢”, y);%预分配过滤器输出qout（1）= y;

逐样本滤波噪声轨迹。

为II = 2：NUMER（Trajnoisy）x = trajnoisy（ii）;d = dist（y，x）;％将dist输出重新统称到范围[低，高]hlpf = (d / pi)。* hrangeLimited +低;y = slerp (y, x, hlpf);qout (ii) = y;结束f =图;trajNoisy sp.plotEulerd (f,'o'）;trajSlerped sp.plotEulerd (f,'k-。'，“线宽”2);qout sp.plotEulerd (f,“- - -”，“线宽”2);sp.addAnnotations (f hrange hbias);

结论

SLERP可用于创建两个方向之间的短轨迹和平滑或低通滤波。它已被广泛应用于各种行业。

参考

  1. Shoemake,肯。“用四元数曲线动画旋转。”ACM筝电脑图形19， no 3 (1985):245-54, doi:10.1145/325165.325242

导航工具箱文档

金宝app