基于四元数SLERP的低通滤波器定位
这个例子展示了如何使用球面线性插值(SLERP)来创建四元数序列和低通滤波器噪声轨迹。SLERP是一种常用的计算机图形技术,用于创建旋转物体的动画。
SLERP概述
考虑一对四元数而且.球面线性插值允许您创建一个四元数序列,在四元数之间平滑地变化而且角速度恒定。SLERP使用插值参数h
这可以在0和1之间变化,并决定输出四元数与这两者的接近程度或.
四元数SLERP的原始公式是由Ken Shoemake [1):
正弦曲线的另一种形式(用于slerp
函数实现)为:
在哪里是四元数部分的点积。请注意,.
SLERP与四元数部分的线性插值
考虑下面的例子。从欧拉角建立两个四元数。
Q0 =四元数([-80 10 0],“eulerd”,“ZYX股票”,“帧”);Q1 =四元数([80 70 70],“eulerd”,“ZYX股票”,“帧”);
求四元数的30%距离q0处
来第一季度
,指定slerp
参数为0.3。
P30 = slerp(q0, q1, 0.3);
要查看插值四元数的欧拉角表示,请使用eulerd
函数。
eulerd (e,“ZYX股票”,“帧”)
Ans = -56.6792 33.2464 -9.6740
创造一个顺畅的轨迹q0处
而且第一季度
,指定slerp
插值参数作为0到1之间的均匀间隔数的向量。
Dt = 0.01;H = (0:dt:1).';trajSlerped = slerp(q0, q1, h);
比较SLERP算法与轨迹之间的结果q0处
而且第一季度
,对每个四元数部分采用简单线性插值(LERP)。
partsLinInterp = interp1([0;1], compact([q0;q1]), h,“线性”);
注意,线性插值不会给出单位四元数,所以它们必须被标准化。
trajLerped = normalize(四元数(partsLinInterp));
计算每种方法的角速度。
avSlerp = helperQuat2AV(trajSlerped, dt);avLerp = helperQuat2AV(trajLerped, dt);
画出两组角速度。注意,SLERP的角速度是恒定的,但线性插值的角速度是不同的。
sp = helperslerpplots;sp.plotAngularVelocities (avSlerp avLerp);
SLERP以恒定的速率产生平稳旋转。
SLERP低通滤波
SLERP还可以用于制作更复杂的函数。在此,采用SLERP对有噪声的弹道进行低通滤波。
旋转噪声可以通过由噪声旋转向量形成四元数来构造。
Rcurr = rng(1);Sigma = 1e-1;噪声v = sigma .* (rand(数字(h), 3) - 0.5);Qnoise =四元数(noiserv,“rotvec”);rng (rcurr);
破坏轨道trajSlerped
有噪声时,用噪声矢量增量地旋转轨迹qnoise
.
trajnoise = trajSlerped .* qnoise;
你可以平滑实值信号使用单极滤波器的形式:
这个公式本质上是说新的过滤器状态应该向当前输入移动吗通过与当前输入和当前滤波器状态之间的距离成正比的步长.
这种方法的精髓在于如何对四元数序列进行低通滤波。要做到这一点,两者经销
而且slerp
使用函数。
的经销
函数返回由两个四元数应用的旋转差的弧度测量值。的范围经销
函数是半开区间[0,pi)。
的slerp
函数用于将滤波器状态引导到当前输入。当输入状态与当前滤波器状态相差较大时,它更倾向于输入经销
,而较少朝向输入时经销
给出一个小值。的插值参数。slerp
是在闭区间[0,1],那么输出的经销
必须重新标准化到此范围。但是,插值参数的全范围[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 =高-低;
初始化筛选器并预分配输出。
y = trajnoise (1);初始滤波器状态%Qout = 0 (size(y),“喜欢”, y);%预分配过滤器输出Qout (1) = y;
逐样本滤波噪声轨迹。
为ii=2:数字(trajnoise) x = trajnoise (ii);D = dist(y, x);将区域输出重新正规化到范围[低,高]HLPF = (d./pi)。*hrangeLimited + low;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可用于创建两个方向之间的短轨迹和平滑或低通滤波。它已被广泛应用于各种行业。
参考文献
Shoemake,肯。四元数曲线旋转动画ACMSIGRAPH电脑图形19, no 3 (1985):245-54, doi:10.1145/325165.325242