差分

このページの2つのバージョン間の差分を表示します。

--- articles:quaternion [2021/06/15 16:35] – [回転を表す四元数と直交行列] Takashi Suehiro
+++ articles:quaternion [2022/03/04 08:59] (現在) – [プログラムと練習問題] Takashi Suehiro
@@ 行 108: / 行 108: @@
 \boldsymbol{z}' = \left( \begin{array}{c}
-(q_x q_y + q_w q_y) \\
+(q_x q_z + q_w q_y) \\
 (q_y q_z - q_w q_x) \\
 q_w^2 + q_z^2 - q_x^2  - q_y^2
@@ 行 117: / 行 117: @@
 $$
 \left( \begin{array}{cc}
-q_w^2 + q_x^2 - q_y^2  - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_y + q_w q_y) \\
+q_w^2 + q_x^2 - q_y^2  - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\
 (q_y q_x + q_w q_z) & q_w^2 + q_y^2 - q_z^2  - q_x^2 & 2(q_y q_z - q_w q_x) \\
 (q_z q_x - q_w q_y) & 2(q_z q_y + q_w q_x) & q_w^2 + q_z^2 - q_x^2  - q_y^2
@@ 行 137: / 行 137: @@
 $$
 a_{xx} + a_{yy} + a_{zz} = 3 q_w^2 - (q_x^2 + q_y^2  + q_z^2 )
-= 3 q_w^2 - (1 - q_w^2)
-= 4 q_w^2 - 1
 $$
-よって,
+ここで
 $$
-q_w = {1 \over 2} \sqrt{a_{xx} + a_{yy} + a_{zz} + 1}
+q_x^2 + q_y^2  + q_z^2 = \mathrm{sin}^2{\theta \over 2} = 1 - \mathrm{cos}^2{\theta \over 2} = 1 - q_w^2 \tag{4}
 $$
-また
+よって，
 $$
-a_{yz} - a_{zy} = 2(q_z q_y + q_w q_x) - 2(q_y q_z - q_w q_x) = 4 q_w q_x
+a_{xx} + a_{yy} + a_{zz} = 4 q_w^2 - 1
 $$
-よって$ q_w \ne 0 $の場合，
+$q_w=\mathrm{cos}{\theta \over 2} \ge 0$を考慮して，
 $$
-q_x = {{a_{yz} - a_{zy}} \over {4 q_w}} = {{a_{yz} - a_{zy}} \over {2 \sqrt{a_{xx} + a_{yy} + a_{zz} + 1}}}
+q_w = {1 \over 2} \sqrt{a_{xx} + a_{yy} + a_{zz} + 1}   \tag{5}
 $$
-同様に，
+また
 $$
-q_y = {{a_{zx} - a_{xz}} \over {4 q_w}} = {{a_{zx} - a_{xz}} \over {2 \sqrt{a_{xx} + a_{yy} + a_{zz} + 1}}}
+a_{yz} - a_{zy} = 2(q_z q_y + q_w q_x) - 2(q_y q_z - q_w q_x) = 4 q_w q_x
 $$
+よって$ q_w \ne 0 $の場合，
 $$
-q_z = {{a_{xy} - a_{yx}} \over {4 q_w}} = {{a_{xy} - a_{yx}} \over {2 \sqrt{a_{xx} + a_{yy} + a_{zz} + 1}}}
+q_x = {{a_{yz} - a_{zy}} \over {4 q_w}} \tag{6}
 $$
-$ q_w = 0 $の場合，式(2)に代入して整理すると
+しかし，これでは$q_w$が小さいときに誤差が大きくなるので以下のように解くことにする．
 $$
-\left( \begin{array}{cc}
+a_{xx} - a_{yy} - a_{zz} = - q_w^2 + 3q_x^2 - q_y^2  - q_z^2
- q_x^2 - 1 & 2 q_x q_y  & 2 q_x q_y  \\
-q_y q_x &  2 q_y^2 - 1 & 2 q_y q_z \\
-q_z q_x & 2 q_z q_y & 2 q_z^2 - 1
-\end{array} \right)　\tag{4}
 $$
-係数を比較すると
+ここで，式(4)を使い$q_w$を消すと$q_y$，$q_z$も消えて，
 $$
--1 + 2 q_x^2 = a_{xx}
+a_{xx} - a_{yy} - a_{zz} = 4q_x^2 - 1
 $$
+すなわち
 $$
--1 + 2 q_y^2 = a_{yy}
+q_x^2 = {1 \over 4} (a_{xx} - a_{yy} - a_{zz} + 1)
 $$
+この平方根を求めれば良いのだが，正負の選択が問題になる．これは式(6)を利用することで決定できる．
+したがって，
 $$
--1 + 2 q_z^2 = a_{zz}
+q_x = \mathrm{sign}(a_{yz} - a_{zy}){1 \over 2} \sqrt{a_{xx} - a_{yy} - a_{zz} + 1} \tag{7}
 $$
+同様に，
 $$
-q_x q_y = a_{xy} = a_{yx}
+q_y = \mathrm{sign}(a_{zx} - a_{xz}){1 \over 2} \sqrt{- a_{xx} + a_{yy} - a_{zz} + 1}
 $$
 $$
-q_y q_z = a_{yz} = a_{zy}
+q_z = \mathrm{sign}(a_{xy} - a_{yx}){1 \over 2} \sqrt{- a_{xx} - a_{yy} + a_{zz} + 1}
 $$
-$$
-q_z q_x = a_{zx} = a_{xz}
-$$
-よって
-$$
-q_x = \pm \sqrt{ {a_{xx} + 1} \over 2}
-$$
-$ q_x \ne 0 $の場合，$\pi$回転はどちらに回しても同じなので正負はどちらを選んでもよい．
-他の成分はこれと整合を取るため以下のように計算する．
-$$
-q_y = a_{xy}/ 2 q_x
-$$
-$$
-q_z = a_{zx}/ 2 q_x
-$$
-$ q_x = 0 $の場合は他の成分から解けば良い．
-ここの説明では$ q_x $から解いたが実際には上３つの式を解いて
-一番絶対値の大きい成分から残りを計算する精度的にも良い値が得られる．
+この計算方法はゼロ割が発生しないので安定して計算ができるが，
+実際のプログラムでは計算誤差の影響で平方根の中が極めて小さいときに負になることがあるという点に注意を払う必要がある．
 ===== 四元数による回転の連鎖と座標系の姿勢表現 =====
@@ 行 255: / 行 238: @@
   * パラメタが4つで冗長パラメタが少ないので正規化しやすい　\\ 直交行列はパラメタが9で直交性を維持するのが難しい
   * ジンバルロックを起こさない（オイラー角と比較して）
-  * 回転や座標系の連鎖（積の計算）が軽い（積16和12. 直交行列だと積27和8）
+  * 回転や座標系の連鎖（積の計算）が軽い（積16和12. 直交行列だと積27和18）
 欠点は
   * 座標系の表現と捉えた場合，座標軸がどうなっているか計算しないと分からない
   * 人間にとって分かりにくい（オイラー角と比較して）
-  * ベクトルの回転の計算が重い（積28和20．直交行列だと積9和6）
+  * ベクトルの回転の計算が重い（積28和20((ベクトルの実部の計算を除いている．普通に四元数の積でやると積32,和24))．直交行列だと積9和6）
 四元数，直交行列，それぞれ使いどころによると思うが，
@@ 行 266: / 行 249: @@
 私にはあまり四元数のメリットが見えない．
+四元数の一番のメリットは計算誤差の蓄積を防ぐ正規化が容易なことかもしれない．
+回転行列の正規直交化は少しならず工夫が必要である．
+  *  x => y => z などの順に正規直交化を行う．ただし軸の平等性が崩れる．
+  *  ロドリゲス，四元数へ変換した後，正規化を行い，回転行列に戻す．ただし直交行列以外の変換には正当性がない．
+  *  特異値分解を行い，対角成分をすべて1((鏡像変換になるのを避ける必要あり))にして戻す．おそらく一番良さそうだが手間がかかる((参考文献：３次元回転，金谷健一．4.5節 回転行列の最適補正))．
+いずれにしても少々手間がかかるので都度やるのは面倒である．
+===== プログラムと練習問題 =====
+[[articles:geo_basic#QUATERNION|四元数のプログラム]]
+[[articles:geo_manual#四元数|練習問題]]