Scilabによる運動学プログラミング

回転行列からロール・ピッチ・ヨー角を求める

// 出力される角度はDegree で得られる
function [ roll, pitch, yaw ] = RollPitchYaw( rotationMatrix )

  roll_R = atan( rotationMatrix( 3,2 ),rotationMatrix( 3,3 ) );
  pitch_R = atan( -rotationMatrix( 3,1 ), sqrt( rotationMatrix( 1,1 )^2 + rotationMatrix( 2,1 )^2 ) );
  yaw_R = atan( rotationMatrix( 2,1 ), rotationMatrix( 1,1 ) );

  // Radian をDegree に変換する
  roll = roll_R * ( 180.0 / %pi )
  pitch = pitch_R * ( 180.0 / %pi )
  yaw = yaw_R * ( 180.0 / %pi )

ロール・ピッチ・ヨー角から回転行列を求める

// 引数の角度はDegree で与える
function [ rotationMatrix ] = RotationMatrix( roll, pitch, yaw )

  // Degree を Radian に変換する
  roll_R = roll * ( %pi / 180.0 );
  pitch_R = pitch * ( %pi / 180.0 );
  yaw_R = yaw * ( %pi / 180.0 );


  Rx = [
    1 0 0
    0 cos( roll_R ) -sin( roll_R )
    0 sin( roll_R ) cos( roll_R )
  ];

  Ry = [
    cos( pitch_R ) 0 sin( pitch_R )
    0 1 0
    -sin( pitch_R ) 0 cos( pitch_R )
  ];

  Rz = [
    cos( yaw_R ) -sin( yaw_R ) 0
    sin( yaw_R ) cos( yaw_R ) 0
    0 0 1
  ];

  rotationMatrix = Rz * Ry * Rx

ベクトルからひずみ対称行列 (交代行列) を求める

function[ result ] = skew( v )

  result = [   0  -v(3)  v(2)
             v(3)    0  -v(1)
            -v(2)  v(1)    0 ];

角速度ベクトルによって生じる回転行列を求める (ロドリゲスの公式)

function [ result ] = Rodrigues( axis, degree )

  // Degree を Radian に変換する
  radian = degree * ( %pi / 180.0 );

  // ひずみ対称行列を求める
  a = skew( axis );

  // 項に分割して計算する
  exp1 = eye( 3, 3 );
  exp2 = a * sin( radian );
  exp3 = a * a * ( 1.0 - cos( radian ) );

  result = exp1  + exp2 + exp3;

関連ページ