Обзор 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 используются функции.

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

The 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. Оно нашло широкое применение в различных отраслях промышленности.

Ссылки