読者です 読者をやめる 読者になる 読者になる

Butterfly_Effect( )

僕はとにかく地面と別れる方法を探しているんだ

Eigenで力学してみる(1)

Dynamics Computer Program

C++向けの行列計算ライブラリEigenを使って姿勢運動を扱う話.
初回はウォーミングアップということで,Quaternionクラスの使い方の練習をする.

はじめに

僕はC++をメインで使ってる.特に行列計算ライブラリのEigenは手放せない.Matlab/Octave的な行列演算機能と,C言語の持つ高速性を両立できるからだ.
ところでEigenにはQuaternion(4元数)クラスがあるのだけど,これの使い方を解説してる記事は多くない.
Quaternion*1によるキネマティクス処理は強力なので,これを使わないのはとても勿体ない.

Quaternionの時間発展を追う

詳しい解説は省くけれど,Quaternionを使うとベクトルの回転を容易に行える.注意しなくてはいけないのは,

  1. 座標系を固定し,ベクトルを回転させる
  2. ベクトルを固定し,座標系を回転させる

のどちらを計算しているのか意識することだ.ここでは前者を行っている.

#include "Eigen/Dense"
#include "Eigen/Geometry"
#include <iostream>

using namespace std;
using namespace Eigen;

int main(){
    Quaterniond q;
    Vector3d vec;
    Matrix3d DCM;
    
    // z軸の単位ベクトル
    Vector3d Z = Vector3d::UnitZ();
    cout << "Z = " << Z.transpose() << endl;
    
    // x軸周りに+π/4回転させるクォータニオンを生成
    q = AngleAxisd(M_PI/4, Vector3d::UnitX());
    cout << "q = " << q.coeffs().transpose() << endl;
    
    // ベクトルZをクォータニオンによって回転させる
    vec = q * Z;
    cout << "vec = " << vec.transpose() << endl;
    
    // クォータニオンからDCMを生成する
    DCM = q.toRotationMatrix();
    cout << "DCM = " << endl << DCM << endl;
    
    // クォータニオンを作用させることと,DCMを作用させることは等価
    vec = DCM * Z;
    cout << "vec = " << vec.transpose() << endl;

    return EXIT_SUCCESS;
}

なお,ベクトルを固定して座標系を回転させる場合は,共役クォータニオンを作用させればいい.

//vec = q * Z;    <-------この行を修正する
vec = q.conjugate() * Z;

ただし,この時クォータニオンが規格化されていることが前提条件.クォータニオンの規格化は次のように行う.

q.normalize();

Modeling and Simulation of Aerospace Vehicle Dynamics (Aiaa Education Series)

Modeling and Simulation of Aerospace Vehicle Dynamics (Aiaa Education Series)

Spacecraft Attitude Dynamics (Dover Books on Aeronautical Engineering)

Spacecraft Attitude Dynamics (Dover Books on Aeronautical Engineering)

人工衛星の力学と制御ハンドブック―基礎理論から応用技術まで

人工衛星の力学と制御ハンドブック―基礎理論から応用技術まで

*1:正確には,ハミルトンの4元数表記を使わない場合はQuaternionとは呼ばない.姿勢力学に応用されるときは行列表記が使われ,そのときはオイラーパラメータと呼ばれる.