meanrot

四元数の平均回転

R2020a 以降

構文

quatAverage = meanrot(quat)

quatAverage = meanrot(quat,dim)

quatAverage = meanrot(___,nanflag)

説明

quatAverage = meanrot(quat) は、サイズが 1 に等しくない最初の次元に沿って quat の要素の平均回転を返します。

• 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") ではこれらが無視されます。

例

四元数の平均回転

3 つのオイラー角のセットに対応する四元数の行列を作成します。

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 を使用して加法性ノイズを平均化します。

quaternion(1,0,0,0) からの距離が正規分布であると関数 dist で定義された 1e6 の quaternion のベクトルを作成します。ノイズを含む quaternion ベクトルに対応するオイラー角をプロットします。

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 を使用して、与えられた quaternion のベクトルの平均四元数を求めます。オイラー角に変換し、結果をプロットします。

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 は、回転行列間の差のフロベニウス ノルムの二乗が最小になる四元数を出力します。2 つの四元数について考えます。

• q0 は回転がないことを表します。

• q90 は "x" 軸を中心とする 90 度の回転を表します。

q0 = quaternion([0 0 0],"eulerd","ZYX","frame");
q90 = quaternion([0 0 90],"eulerd","ZYX","frame");

"x" 軸を中心とする 0 ～ 180 度の回転を表す四元数のスイープ qSweep を作成します。

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") として定義します。q0 と q90 を指定して meanrot を呼び出し、同じ結果になることを確認します。

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 = 49

通常、四元数の平均は互いに近い回転について計算されるため、この例で示すエッジ ケースを実環境に応用することはほとんどありません。大きく離れた 2 つの四元数の平均を求めるには、関数 slerp を使用します。slerp を使用して実験を繰り返し、大きな距離について返される四元数の平均がより直感的になることを確認します。

qMean = slerp(q0,q180,0.5);
q0_q180 = eulerd(qMean,"ZYX","frame")

q0_q180 = 1×3

         0         0   90.0000

入力引数

quat — 四元数

quaternion オブジェクト | quaternion オブジェクトの配列

平均を計算する四元数。quaternion オブジェクト、または任意の次元の quaternion オブジェクトの配列として指定します。

dim — 演算の対象の次元

正の整数スカラー

演算の対象の次元。正の整数スカラーとして指定します。値を指定しない場合、既定値はサイズが 1 に等しくない最初の次元です。

次元 dim は、長さが 1 に縮小した次元を示します。size(quatAverage,dim) は 1 ですが、他のすべての次元のサイズは変化しません。

データ型: double | single

nanflag — NaN の条件

"includenan" (既定値) | "omitnan"

NaN の条件。次の値のいずれかとして指定します。

• "includenan" –– 平均回転の計算に NaN 値を含めます。結果は NaN になります。

• "omitnan" –– 入力に含まれるすべての NaN 値を無視します。

データ型: char | string

出力引数

quatAverage — 四元数の平均回転

quaternion オブジェクト | quaternion オブジェクトの配列

四元数の平均回転。quaternion オブジェクト、または quaternion オブジェクトの配列として返されます。

アルゴリズム

meanrot は、四元数の平均 $$\overline{q}$$ を[1]に従って決定します。$$\overline{q}$$ は、回転行列間の差のフロベニウス ノルムの二乗が最小になる四元数です。

$$\overline{q}=\arg \begin{array}{c}\min \\ q\in {\text{S}}^3\end{array}{\displaystyle \sum _{i=1}^n{\Vert A\left(q\right)-A\left(q_i\right)\Vert }_F^2}$$

参照

[1] Markley, F. Landis, Yang Chen, John Lucas Crassidis, and Yaakov Oshman. "Average Quaternions." Journal of Guidance, Control, and Dynamics. Vol. 30, Issue 4, 2007, pp. 1193-1197.

拡張機能

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2020a で導入

参考

関数

• dist | slerp

オブジェクト

• quaternion

トピック

• Rotations, Orientations, and Quaternions for Automated Driving