はじめに
姿勢推定したりするときに今までオイラー角を使ってきましたが、他のプラットフォームからオイラー角を取得するなど、環境が変わるとオイラー角の回転順番が変わってうっとおしいのでこれを機にクォータニオンに手を出しました。
MATLAB用クォータニオンクラス
クォータニオンについてはググると資料がたくさんあるので割愛します。
クォータニオンに関する関数(主にオイラー角とクォータニオンと回転行列の相互変換)を実装したMATLABクラスを作成しましたので参考にしてください。
classdef TF < handle
methods
function tf=TF()
end
%クォータニオンから回転行列
function R=quat2dcm(tf,q)
q2=q.^2;
R=[q2(1)-q2(2)-q2(3)+q2(4),2*(q(1)*q(2)+q(3)*q(4)),2*(q(1)*q(3)-q(2)*q(4));...
2*(q(1)*q(2)-q(3)*q(4)),-q2(1)+q2(2)-q2(3)+q2(4),2*(q(2)*q(3)+q(1)*q(4));...
2*(q(1)*q(3)+q(2)*q(4)),2*(q(2)*q(3)-q(1)*q(4)),-q2(1)-q2(2)+q2(3)+q2(4)];
end
%オイラー角から回転行列
function R=euler2dcm(tf,phi)
%3->2->1
ph=phi(3);
th=phi(2);
ps=phi(1);
R=[cos(ph)*cos(th) sin(ph)*cos(th) -sin(th);
cos(ph)*sin(th)*sin(ps)-sin(ph)*cos(ps) sin(ph)*sin(th)*sin(ps)+cos(ph)*cos(ps) cos(th)*sin(ps);
cos(ph)*sin(th)*cos(ps)+sin(ph)*sin(ps) sin(ph)*sin(th)*cos(ps)-cos(ph)*sin(ps) cos(th)*cos(ps)];
end
%回転行列からクォータニオン
function q=dcm2quat(tf,R)
q = [sqrt(1 + R(1,1) - R(2,2) - R(3,3))/2;...
sqrt(1 - R(1,1) + R(2,2) - R(3,3))/2;...
sqrt(1 - R(1,1) - R(2,2) + R(3,3))/2;...
sqrt(1 + R(1,1) + R(2,2) + R(3,3))/2];
[x,ix]=max(q);
switch ix
case 1
q([2 3 4])=0.25/q(1)*[R(1,2)+R(2,1);R(1,3)+R(3,1);R(2,3)-R(3,2)];
case 2
q([1 3 4])=0.25/q(2)*[R(1,2)+R(2,1);R(3,2)+R(2,3);R(3,1)-R(1,3)];
case 3
q([1 2 4])=0.25/q(3)*[R(3,1)+R(1,3);R(3,2)+R(2,3);R(1,2)-R(2,1)];
case 4
q([1 2 3])=0.25/q(4)*[R(2,3)-R(3,2);R(3,1)-R(1,3);R(1,2)-R(2,1)];
end
end
%回転行列からオイラー角
function phi=dcm2euler(tf,R)
%+-pi/2まで
phi=zeros(3,1);
phi(2)=atan2(-R(1,3),sqrt(R(2,3)^2+R(3,3)^2));
phi(1)=atan2(R(2,3),R(3,3));
phi(3)=atan2(R(1,2),R(1,1));
end
%クォータニオンからオイラー角
function phi=quat2euler(tf,q)
phi=tf.dcm2euler(tf.quat2dcm(q));
end
%オイラー角からクォータニオン
function q=euler2quat(tf,phi)
q=tf.dcm2quat(tf.euler2dcm(phi));
end
%クォータニオンの微分 omegaは角速度
function dq=quat_derivate(tf,q,omega)
p=[0,omega(3),-omega(2),omega(1);...
-omega(3),0,omega(1),omega(2);...
omega(2),-omega(1),0,omega(3);...
-omega(1),-omega(2),-omega(3),0];
dq=0.5*p*q;
end
end
end
変換しているオイラー角は3,2,1回転です。回転行列$R$とボディ座標$\Sigma_b$と慣性座標$\Sigma_o$の関係と回転行列とオイラー角$\Phi=[\phi,\theta,\psi]^T$の関係は以下のようになります。
$$R(\Phi)[\Sigma_b]=[\Sigma_o]\\
R(\Phi)=\begin{bmatrix}
C_\phi C_\theta& C_\phi S_\theta S_\psi-S_\phi C_\psi &C_\phi S_\theta C_\psi+S_\phi S_\psi \\
S_\phi C_\theta& S_\phi S_\theta S_\psi+C_\phi C_\psi &S_\phi S_\theta C_\psi-C_\phi S_\psi \\
-S_\theta& C_\theta\ S_\phi &C_\theta C_\psi
\end{bmatrix}
$$
また、オイラー角の範囲はatan2を使っているので±π radです。
おわりに
MATLABはRobotics toolboxを買わないとクォータニオンを扱う関数がなさそうだったので作成しました。これを使って姿勢推定アルゴリズムを実装して今のところ問題ないです。