Среднее вращение кватерниона
quatAverage = meanrot(quat)quat вдоль первого размера массива, размер которого не равен 1.
Если quat является вектором, meanrot(quat) возвращает среднее значение поворота элементов.
Если quat является матрицей, meanrot(quat) возвращает вектор строки, содержащий среднее значение поворота каждого столбца.
Если quat является многомерным массивом, то mearot(quat) работает вдоль первого размера массива, размер которого не равен 1, рассматривая элементы как векторы. Этот размер становится равным 1, в то время как размеры всех остальных размеров остаются прежними.
meanrot функция нормализует входные кватернионы, quat, перед расчетом среднего.
quatAverage = meanrot(quat,dim)dim. Например, если quat является матрицей, то meanrot(quat,2) - вектор столбца, содержащий среднее значение каждой строки.
quatAverage = meanrot(___,nanflag)NaN значения из вычисления для любого из предыдущих синтаксисов. meanrot(quat,'includenan') включает все NaN значения в вычислении при mean(quat,'omitnan') игнорирует их.
Создайте матрицу кватернионов, соответствующую трем наборам углов Эйлера.
eulerAngles = [40 20 10; ... 50 10 5; ... 45 70 1]; quat = quaternion(eulerAngles,'eulerd','ZYX','frame');
Определите среднее вращение, представленное кватернионами. Преобразование среднего угла поворота в углы Эйлера в градусах для удобства чтения.
quatAverage = meanrot(quat)
quatAverage = quaternion
0.88863 - 0.062598i + 0.27822j + 0.35918k
eulerAverage = eulerd(quatAverage,'ZYX','frame')
eulerAverage = 1×3
45.7876 32.6452 6.0407
Использовать meanrot по последовательности кватернионов для усреднения аддитивного шума.
Создайте вектор из 1е6 кватернионов, расстояние которого определяется dist функция от кватерниона (1,0,0,0) обычно распределена. Постройте график углов Эйлера, соответствующих вектору шумного кватерниона.
nrows = 1e6; ax = 2*rand(nrows,3) - 1; ax = ax./sqrt(sum(ax.^2,2)); ang = 0.5*randn(size(ax,1),1); q = quaternion(ax.*ang ,'rotvec'); noisyEulerAngles = eulerd(q,'ZYX','frame'); figure(1) subplot(3,1,1) plot(noisyEulerAngles(:,1)) title('Z-Axis') ylabel('Rotation (degrees)') hold on subplot(3,1,2) plot(noisyEulerAngles(:,2)) title('Y-Axis') ylabel('Rotation (degrees)') hold on subplot(3,1,3) plot(noisyEulerAngles(:,3)) title('X-Axis') ylabel('Rotation (degrees)') hold on
Использовать meanrot для определения среднего кватерниона, заданного вектором кватернионов. Преобразуйте в углы Эйлера и постройте график результатов.
qAverage = meanrot(q); qAverageInEulerAngles = eulerd(qAverage,'ZYX','frame'); figure(1) subplot(3,1,1) plot(ones(nrows,1)*qAverageInEulerAngles(:,1)) title('Z-Axis') subplot(3,1,2) plot(ones(nrows,1)*qAverageInEulerAngles(:,2)) title('Y-Axis') subplot(3,1,3) plot(ones(nrows,1)*qAverageInEulerAngles(:,3)) title('X-Axis')
meanrot Алгоритм и ограничения meanrot Алгоритм
meanrot функция выводит кватернион, который минимизирует квадратичную норму Фробениуса разности между матрицами вращения. Рассмотрим два кватерниона:
q0 представляет отсутствие вращения.
q90 представляет поворот на 90 градусов вокруг оси X.
q0 = quaternion([0 0 0],'eulerd','ZYX','frame'); q90 = quaternion([0 0 90],'eulerd','ZYX','frame');
Создание кватернионного сдвига, qSweep, который представляет повороты от 0 до 180 градусов вокруг оси X.
eulerSweep = (0:1:180)'; qSweep = quaternion([zeros(numel(eulerSweep),2),eulerSweep], ... 'eulerd','ZYX','frame');
Новообращенный q0, q90, и qSweep к матрицам вращения. В цикле вычислите метрику для минимизации для каждого элемента сдвига кватерниона. Постройте график результатов и верните значение сдвига Эйлера, соответствующее минимуму метрики.
r0 = rotmat(q0,'frame'); r90 = rotmat(q90,'frame'); rSweep = rotmat(qSweep,'frame'); metricToMinimize = zeros(size(rSweep,3),1); for i = 1:numel(qSweep) metricToMinimize(i) = norm((rSweep(:,:,i) - r0),'fro').^2 + ... norm((rSweep(:,:,i) - r90),'fro').^2; end plot(eulerSweep,metricToMinimize) xlabel('Euler Sweep (degrees)') ylabel('Metric to Minimize')
[~,eulerIndex] = min(metricToMinimize); eulerSweep(eulerIndex)
ans = 45
Минимум метрики соответствует повороту Эйлера на 45 градусов. То есть meanrot определяет среднее значение между quaterion([0 0 0],'ZYX','frame') и quaternion([0 0 90],'ZYX','frame') как quaternion([0 0 45],'ZYX','frame'). Звонить meanrot с q0 и q90 для проверки одного и того же результата.
eulerd(meanrot([q0,q90]),'ZYX','frame')
ans = 1×3
0 0 45.0000
Ограничения
Метрика, которая meanrot использование для определения среднего вращения не является уникальным для кватернионов, значительно удаленных друг от друга. Повторите приведенный выше эксперимент для кватернионов, разделенных 180 градусами.
q180 = quaternion([0 0 180],'eulerd','ZYX','frame'); r180 = rotmat(q180,'frame'); for i = 1:numel(qSweep) metricToMinimize(i) = norm((rSweep(:,:,i) - r0),'fro').^2 + ... norm((rSweep(:,:,i) - r180),'fro').^2; end plot(eulerSweep,metricToMinimize) xlabel('Euler Sweep (degrees)') ylabel('Metric to Minimize')
[~,eulerIndex] = min(metricToMinimize); eulerSweep(eulerIndex)
ans = 159
Средства кватерниона обычно вычисляются для вращений, которые близки друг к другу, что делает краевой корпус, показанный в этом примере, маловероятным в реальных приложениях. Чтобы усреднить два кватерниона, которые значительно удалены, используйте slerp функция. Повторите эксперимент с помощью slerp и убедитесь, что возвращаемое значение кватерниона является более интуитивно понятным для больших расстояний.
qMean = slerp(q0,q180,0.5); q0_q180 = eulerd(qMean,'ZYX','frame')
q0_q180 = 1×3
0 0 90.0000
meanrot определяет среднее значение кватерниона, согласно [1]. - кватернион, минимизирующий квадратичную норму Фробениуса разности между матрицами вращения:
) ‖ F2
[1] Маркли, Ф. Лэндис, Ян Чен, Джон Лукас Крассидис и Яаков Ошман. «Средние кватернионы». Журнал руководства, управления и динамики. Том 30, выпуск 4, 2007, стр. 1193-1197.