Ориентация фильтра 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

Для просмотра документации необходимо авторизоваться на сайте