3D旋转矩阵与变换矩阵

有必要搞清楚3D旋转矩阵与坐标变换矩阵的定义和两者之间的关系。

机体坐标系(b)中的矢量和位置坐标转换到导航坐标系(n)中，必须依赖姿态转换矩阵(也称方向余弦矩阵)，反之亦然。 矢量$\bf{A}$在导航坐标系中表示为$\bf{A}^n$，转换到机体坐标系中表示为形式$\bf{A}^b$，利用方向余弦矩阵$\bf{C}^b_n$可完成此转换，表示为 $\bf{A}^b=\bf{C}^b_n\bf{A}^n$ 其中， $\bf{C}^b_n=\left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&{\cos \left( \phi \right)}&{\sin \left( \phi \right)}\\ 0&{ - \sin \left( \phi \right)}&{\cos \left( \phi \right)} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\cos \left( \theta \right)}&0&{ - \sin \left( \theta \right)}\\ 0&1&0\\ {\sin \left( \theta \right)}&0&{\cos \left( \theta \right)} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\cos \left( \psi \right)}&{\sin \left( \psi \right)}&0\\ { - \sin \left( \psi \right)}&{\cos \left( \psi \right)}&0\\ 0&0&1 \end{array}} \right]$

计算得到， $\bf{C}^b_n=\left[ {\begin{array}{*{20}{c}} {\cos \left( \theta \right)\cos \left( \psi \right)}&{\cos \left( \theta \right)\sin \left( \psi \right)}&{ - \sin \left( \theta \right)}\\ {\sin \left( \phi \right)\sin \left( \theta \right)\cos \left( \psi \right) - \cos \left( \phi \right)\sin \left( \psi \right)}&{\sin \left( \phi \right)\sin \left( \theta \right)\sin \left( \psi \right) + \cos \left( \phi \right)\cos \left( \psi \right)}&{\sin \left( \phi \right)\cos \left( \theta \right)}\\ {\cos \left( \phi \right)\sin \left( \theta \right)\cos \left( \psi \right) + \sin \left( \phi \right)\sin \left( \psi \right)}&{\cos \left( \phi \right)\sin \left( \theta \right)\sin \left( \psi \right) - \sin \left( \phi \right)\cos \left( \psi \right)}&{\cos \left( \phi \right)\cos \left( \theta \right)} \end{array}} \right]$

Matlab Code: euler2rotMat(phi, theta, psi)

function R = euler2rotMat(phi, theta, psi)
%EULER2ROTMAT Converts a ZYX Euler angle orientation to a rotation matrix

%

%   q = euler2rotMat(axis, angle)

%

%   Converts ZYX Euler angle orientation to a rotation matrix where phi is

%   a rotation around X, theta around Y and psi around Z.

%

%   For more information see:

%   http://www.x-io.co.uk/node/8#quaternions

%

%	Date          Author          Notes

%	27/09/2011    SOH Madgwick    Initial release


R(1,1,:) = cos(psi).*cos(theta);
R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi);
R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi);

R(2,1,:) = sin(psi).*cos(theta);
R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi);
R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi);

R(3,1,:) = -sin(theta);
R(3,2,:) = cos(theta).*sin(phi);
R(3,3,:) = cos(theta).*cos(phi);

end