前提

座標系が角速度 $\omega = (r,p,y)^\top$ で回転しているとする。

微小回転行列

本来三次元回転SO3は非可換であるが，微小回転を考えればこれを可換にすることができる。1

先程の角速度で微小回転行列というものが考えられており， $\omega \Delta t = (\alpha,\beta,\gamma)^\top$ とすれば以下のような回転行列で表せる。

f:id:ossyaritoori:20190719013841p:plain

これは以下に示すcross product表記を用いれば， $R= \omega\Delta t_{ \times }+eye$ と表すことができる。

f:id:ossyaritoori:20190719014129p:plain — wikipediaより引用：歪対称行列

クォータニオンによる角速度の積分

クォータニオン便利計算ノートによればクォータニオンによる積分は以下の通り。

f:id:ossyaritoori:20190719014911p:plain — 一番下の式には1/2が入っていないと思う。見落としがなければ。

先程のcross productを用いて表すなら，

$$ \dot{q}=\frac{1}{2} \begin{pmatrix} 0 & -\omega^\top\\ \omega & \omega_{\times} \end{pmatrix} q $$

と書いたほうがスッキリするだろう。

MATLABで比較プログラム

P.I.corke先生のRobotics Tool boxを使用。課金できる方は自前でToolboxを買うと良い。なお，2019年以降に東大に在籍している学生は無料で全ライセンス使えるのでぜひ活用するとよい。

petercorke.com

昔は全機能使えたと思ったのですが今入れたらいくつかの機能が削除されていたような？？？

問題設定①

最初の角度をオール0とする。同次変換行列を作っておく。

orig_eul = [0,0,0]'
orig_tr = rpy2tr(orig_eul')

omegaを適当に定め，10ms積分するとする。

omega = [0.1,0.2,0.3];
dt = 0.01;

微小回転行列の作成

cross productを計算するskewという関数を用いて簡単に作成可能。

R1=skew(omega*dt)+eye(3)

TR1 =r2t(R1)*orig_tr

TR1 =

    1.0000   -0.0030    0.0020         0
    0.0030    1.0000   -0.0010         0
   -0.0020    0.0010    1.0000         0
         0         0         0    1.0000

クォータニオンの計算

回転操作などをクォータニオンに変換すればよいが，新しく落としたToolbox：10.3.1はクォータニオンにまつわる変換がいくつか取り去られていた。しょうがないので自分で書いた。

orig_q = eul2quat_self(orig_eul)
P2q = integratequat(orig_q,omega*dt)
TR2 = rpy2tr(quat2eul_self(P2q))

TR2 =

    1.0000   -0.0030    0.0020         0
    0.0030    1.0000   -0.0010         0
   -0.0020    0.0010    1.0000         0
         0         0         0    1.0000

問題設定②

最初の角度を適当に決める。ロールピッチヨーのオイラー角を適当に決める。うん。後でやります。

orig_eul = rand(3,1)

orig_eul =

    0.9134
    0.6324
    0.0975

TR1 =

    0.8014    0.4054    0.4398         0
    0.0816    0.6543   -0.7518         0
   -0.5926    0.6384    0.4913         0
         0         0         0    1.0000

TR2 =

    0.8014    0.4054    0.4398         0
    0.0816    0.6543   -0.7518         0
   -0.5926    0.6384    0.4913         0
         0         0         0    1.0000

結果がほぼ同じになるのは違和感がある。前者は1次近似で後者は変換部を除けば理想的な計算結果になると思っていたので。誤差は1e-5のオーダ。

 TR1-TR2
 =

   1.0e-05 *

    0.7539   -0.0609    0.1604         0
    0.1528    0.0783   -0.5804         0
   -0.0849    0.0234    0.3760         0
         0         0         0         0