Обзор 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 обеспечивает плавное вращение с постоянной скоростью.

Фильтрация нижних частот с помощью SLERP

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

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

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;

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

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

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

dist функция возвращает измерение в радианах разности вращения, применяемой двумя кватернионами. Диапазон dist функция - это полуоткрытый интервал [0, pi).

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 может использоваться для создания как коротких траекторий между двумя ориентациями, так и для сглаживания или фильтрации нижних частот. Она нашла широкое применение в различных отраслях промышленности.

Ссылки