绕三轴旋转公式

最新推荐文章于 2024-10-14 05:57:07 发布

jinzhu1911

最新推荐文章于 2024-10-14 05:57:07 发布

阅读量7k

点赞数 1

分类专栏：坐标转换

本文链接：https://blog.csdn.net/jinzhu1911/article/details/98528828

版权

坐标转换专栏收录该内容

1 篇文章

订阅专栏

本文中定义旋转正方向是指沿轴从正往负看，逆时针为正。
NED坐标系到机体坐标系的变换

1.绕轴旋转公式

1.1 绕x轴旋转 $\phi$ 角度

$R_x(\phi)=\left[ \begin{matrix}x'\\ y'\\ z'\end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\phi & sin\phi \\ 0 & -sin\phi & cos\phi \end{matrix} \right] \left[ \begin{matrix} x\\ y\\ z \end{matrix} \right]\tag{1}$
其中， $\begin{matrix}x,y,z\end{matrix}]$ 表示旋转 $\theta$ 之前坐标的坐标，而 $\begin{matrix}x',y',z'\end{matrix}]$ 表示旋转之后的坐标，同理可以得到其他两轴的旋转公式。

1.2 绕y轴旋转 $\theta$

$R_y(\theta)=\left[ \begin{matrix} x'\\ y'\\ z' \end{matrix} \right]= \left[ \begin{matrix} cos\theta& 0& -sin\theta \\ 0& 1& 0 \\ sin\theta& 0& cos\theta \end{matrix} \right] \left[ \begin{matrix} x\\ y\\ z \end{matrix} \right]\tag{2}$

1.3 绕z轴旋转 $\psi$

$R_z(\psi)\left[ \begin{matrix} x'\\ y'\\ z' \end{matrix} \right]= \left[ \begin{matrix} cos\psi& sin\psi& 0 \\ -sin\psi& cos\psi& 0 \\ 0& 0& 1 \end{matrix} \right] \left[ \begin{matrix} x\\ y\\ z \end{matrix} \right]\tag{3}$

1.4 北东地坐标系转换到机体转换矩阵

$R_{NED2b}=R_x(\phi)R_y(\theta)R_z(\psi)=$ $\left[ \begin{matrix} cos(\theta)cos(\psi) & cos(\theta)sin(\psi) & -sin(\theta)\\ cos(\psi)sin(\theta)sin(phi)-sin(\psi)cos(\phi) & sin(\psi)sin(\theta)sin(\phi)+cos(\psi)cos(\phi) & cos(\theta)sin(\phi)\\ sin(\psi)sin(\phi)+cos(psi)sin(\theta)cos(\phi) & sin(\psi)sin(\theta)cos(\phi)-cos(\psi)sin(\phi) & cos(\theta)cos(\phi) \end{matrix} \right]\tag{4}$
$\left[ \begin{matrix} X_{b}\\Y_{b}\\Z_{b} \end{matrix} \right]=R_{NED2b} \left[ \begin{matrix} X_{NED}\\Y_{NED}\\Z_{NED} \end{matrix} \right]\tag{5}$

2.角速度转换公式

根据北东地坐标系旋转到机体坐标系的顺序可知
$\left[ \begin{matrix} p\\q\\r \end{matrix} \right]= \left[ \begin{matrix} 1\\0\\0 \end{matrix} \right] \dot{\phi}+ R_x(\phi)\left[\begin{matrix}0\\1\\0 \end{matrix} \right]\dot{\theta}+ R_x(\phi)R_y(\theta) \left[\begin{matrix}0\\0\\1 \end{matrix} \right]\dot{\psi} = \left[ \begin{matrix} 1 & 0 & -sin\theta \\ 0 & cos\phi & cos\theta sin\phi \\ 0 & -sin\phi & cos\theta cos\phi \end{matrix} \right] \left[ \begin{matrix} \dot{\phi}\\\dot{\theta}\\\dot{\psi} \end{matrix} \right]\tag{5}$
根据矩阵转置可求得
$\left[ \begin{matrix} \dot{\phi}\\\dot{\theta}\\\dot{\psi} \end{matrix} \right] = \left[ \begin{matrix} 1 & tan\theta sin\phi & tan\theta cos\phi \\ 0 & cos\phi & -sin\phi \\ 0 & \frac{-sin\phi}{cos\theta} & \frac{cos\phi}{cos\theta} \end{matrix} \right] \left[ \begin{matrix} p\\q\\r \end{matrix} \right]$