Ориентация фильтра lowpass Используя кватернион SLERP

В этом примере показано, как использовать сферическую линейную интерполяцию (SLERP), чтобы создать последовательности кватернионов, и lowpass фильтруют шумные траектории. SLERP является обычно используемым методом компьютерной графики для создания анимаций вращающегося объекта.

Обзор SLERP

Рассмотрите пару кватернионов и. Сферическая линейная интерполяция позволяет вам создавать последовательность кватернионов, которые варьируются гладко между и с постоянной угловой скоростью. SLERP использует параметр интерполяции h это может находиться в интервале между 0 и 1 и определяет, как близко выходной кватернион или к или к.

Исходная формулировка кватерниона SLERP была дана Кеном Шоемэйком [1] как:

Альтернативная формулировка с синусоидами (используемый в slerp реализация функции):

где скалярное произведение частей кватерниона. Обратите внимание на то, что.

SLERP по сравнению с линейной интерполяцией частей кватерниона

Рассмотрите следующий пример. Создайте два кватерниона из Углов Эйлера.

q0 = quaternion([-80 10 0], 'eulerd', 'ZYX', 'frame');
q1 = quaternion([80 70 70], 'eulerd', 'ZYX', 'frame');

Найти кватернион 30 процентами пути от q0 к q1, задайте slerp параметр как 0,3.

p30 = slerp(q0, q1, 0.3);

Чтобы просмотреть представление Угла Эйлера интерполированного кватерниона, используйте eulerd функция.

eulerd(p30, 'ZYX', 'frame')
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], compact([q0;q1]), h, 'linear');

Обратите внимание на то, что линейная интерполяция не дает модульные кватернионы, таким образом, они должны быть нормированы.

trajLerped = normalize(quaternion(partsLinInterp));

Вычислите скорости вращения из каждого подхода.

avSlerp = helperQuat2AV(trajSlerped, dt);
avLerp = helperQuat2AV(trajLerped, dt);

Постройте оба набора скоростей вращения. Заметьте, что скорость вращения для SLERP является постоянной, но это варьируется для линейной интерполяции.

sp = HelperSlerpPlotting;
sp.plotAngularVelocities(avSlerp, avLerp);

SLERP производит сглаженное вращение на постоянном уровне.

Фильтрация lowpass с SLERP

SLERP может также использоваться, чтобы сделать более комплексные функции. Здесь, SLERP используется к фильтру lowpass шумная траектория.

Вращательный шум может быть создан путем формирования кватерниона из шумного вектора вращения.

rcurr = rng(1);
sigma = 1e-1;
noiserv = sigma .* ( rand(numel(h), 3) - 0.5);
qnoise = quaternion(noiserv, 'rotvec');
rng(rcurr);

Повредить траекторию trajSlerped с шумом инкрементно вращайте траекторию с шумовым векторным qnoise.

trajNoisy = trajSlerped .* qnoise;

Можно сглаживать сигналы с действительным знаком с помощью однополюсного фильтра формы:

Эта формула по существу говорит, что новое состояние фильтра должно быть перемещено к текущему входу размером шага, который пропорционален расстоянию между текущим входом и текущим состоянием фильтра.

Дух этого подхода сообщает, как последовательностью кватерниона может быть отфильтрованный lowpass. Для этого, оба dist и slerp функции используются.

dist функция возвращает измерение в радианах различия, попеременно примененного двумя кватернионами. Область значений dist функция является полуоткрытым интервалом [0, пи).

slerp функция используется, чтобы регулировать состояние фильтра к текущему входу. Это управляется больше к входу, когда различие между входом и текущим состоянием фильтра имеет большой dist, и меньше к входу, когда dist дает маленькое значение. Параметр интерполяции к slerp находится в закрытом интервале [0,1], таким образом, выход dist должен быть повторно нормирован к этой области значений. Однако полный спектр [0,1] для параметра интерполяции дает низкую производительность, таким образом, это ограничивается меньшей областью значений hrange сосредоточенный в hbias.

hrange = 0.4;
hbias = 0.4;

Ограничьте low и high к интервалу [0, 1].

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

Инициализируйте фильтр и предварительно выделите выходные параметры.

y = trajNoisy(1); % initial filter state
qout = zeros(size(y), 'like', y); % preallocate filter output
qout(1) = y;

Отфильтруйте шумную траекторию, выборку выборкой.

for ii=2:numel(trajNoisy)
    x = trajNoisy(ii);
    d = dist(y, x);

    % Renormalize dist output to the range [low, high]
    hlpf = (d./pi).*hrangeLimited + low;
    y = slerp(y,x,hlpf);
    qout(ii) = y;
end

f = figure;
sp.plotEulerd(f, trajNoisy, 'o');
sp.plotEulerd(f, trajSlerped, 'k-.', 'LineWidth', 2);
sp.plotEulerd(f, qout, '-', 'LineWidth', 2);
sp.addAnnotations(f, hrange, hbias);

Заключение

SLERP может использоваться в создании и короткие траектории между двумя ориентациями и для фильтрации lowpass или сглаживания. Это нашло широкое использование во множестве отраслей промышленности.

Ссылки

  1. Shoemake, Кен. "Анимируя Вращение с Кривыми Кватерниона". Компьютерная графика ACM SIGRAPH 19, № 3 (1985):245-54, doi:10.1145/325165.325242