Обзор 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 или сглаживания. Это нашло широкое использование во множестве отраслей промышленности.

Ссылки