双矢量定姿问题

参考 武汉大学-牛小骥《惯性导航原理与方法》课程

原理

姿态矩阵求解原理

静置在地面上的IMU，其加速度计测量值的是重力加速度向量在b系中的分量，陀螺输出为地球自转角速度向量在b系中的投影。

根据向量在不同坐标系(n系、b系)下的转换关系，有

\[g^b = C_n^b g^n，\omega_{ie}^b = C_{ie}^b \omega_{ie}^n \]

定义向量

\[v = g \times \omega_{ie} \]

写成矩阵：

\[[g^b \quad \omega_{ie}^b \quad v^b] = C_n^b [g^n \quad \omega_{ie}^n \quad v^n] \]

求解姿态矩阵

\[C_b^n = \begin{bmatrix} (g^n)^T \\ (\omega_{ie}^n)^T \\ (v^n)^T \end{bmatrix}^{-1} \begin{bmatrix} (g^b)^T \\ (\omega_{ie}^b)^T \\ (v^b)^T \end{bmatrix} \]

\(C_b^n\) 为从b系到n系的坐标变换矩阵，是正交矩阵

\[C_b^n = (C_n^b)^{-1} = (C_n^b)^T \]

姿态角计算公式

根据姿态矩阵计算姿态角，方向余弦矩阵 \(\mathbf{C}_b^n\) 的第 \(i\) 行第 \(j\) 列的元素记作 \(c_{ij}, 1 \leq i,j \leq 3\)
计算俯仰角
$$
\theta = \tan^{-1} \frac{-c_{31}}{\sqrt{c_{32}^2 + c_{33}^2}}, \ |\theta| \leq \frac{\pi}{2}
$$
计算横滚角
$$
\phi = \tan^{-1} \frac{c_{32}}{c_{33}}, \ |\phi| \leq \pi
$$
计算航向角
$$
\psi = \tan^{-1} \frac{c_{21}}{c_{11}}, \ |\psi| \leq \pi
$$

保证数值稳定度

数值稳定性：先对参与解算的矢量做单位化和正交化处理

定义向量

\[\boldsymbol{v}_g = \frac{\boldsymbol{g}^n}{|\boldsymbol{g}^n|}, \quad \boldsymbol{v}_\omega = \frac{\boldsymbol{g}^n \times \boldsymbol{\omega}_{ie}^n}{|\boldsymbol{g}^n \times \boldsymbol{\omega}_{ie}^n|}, \quad \boldsymbol{v}_{g\omega} = \frac{\boldsymbol{g}^n \times \boldsymbol{\omega}_{ie}^n \times \boldsymbol{g}^n}{|\boldsymbol{g}^n \times \boldsymbol{\omega}_{ie}^n \times \boldsymbol{g}^n|} \]

定义向量

\[\boldsymbol{w}_g = \frac{\boldsymbol{g}^b}{|\boldsymbol{g}^b|}, \quad \boldsymbol{w}_\omega = \frac{\boldsymbol{g}^b \times \boldsymbol{\omega}_{ie}^b}{|\boldsymbol{g}^b \times \boldsymbol{\omega}_{ie}^b|}, \quad \boldsymbol{w}_{g\omega} = \frac{\boldsymbol{g}^b \times \boldsymbol{\omega}_{ie}^b \times \boldsymbol{g}^b}{|\boldsymbol{g}^b \times \boldsymbol{\omega}_{ie}^b \times \boldsymbol{g}^b|} \]

求解姿态矩阵

\[\mathbf{C}_b^n = \begin{bmatrix} \boldsymbol{v}_g^T \\ \boldsymbol{v}_\omega^T \\ \boldsymbol{v}_{g\omega}^T \end{bmatrix}^{-1} \begin{bmatrix} \boldsymbol{w}_g^T \\ \boldsymbol{w}_\omega^T \\ \boldsymbol{w}_{g\omega}^T \end{bmatrix} = \begin{bmatrix} \boldsymbol{v}_g & \boldsymbol{v}_\omega & \boldsymbol{v}_{g\omega} \end{bmatrix} \begin{bmatrix} \boldsymbol{w}_g^T \\ \boldsymbol{w}_\omega^T \\ \boldsymbol{w}_{g\omega}^T \end{bmatrix} \]

如此得到的姿态矩阵必然为单位正交矩阵，再根据所得姿态矩阵求解相应的姿态角。

惯导的具体算法实例

双矢量定姿问题，要求两个矢量在两个参考坐标系下的投影均为已知，在惯导实际问题中，通常两个坐标系分别为参考坐标系（地心地固坐标系）和载体坐标系（以车为例，就是车本身的质心坐标系）；两个矢量通常为当地重力和地球自转，这两个矢量在参考坐标系中显然已知，在载体坐标系中，以车为例，是通过加速度计和陀螺仪测量出来的，也是已知的。

下面是具体的一些计算公式（推导过程略）：

\[g^n = \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} \quad \boldsymbol{\omega}_{ie}^n = \begin{bmatrix} \omega_e \cos \varphi \\ 0 \\ -\omega_e \sin \varphi \end{bmatrix} \]

\[\boldsymbol{\omega}_{ie}^b = \boldsymbol{\omega}_{ib}^b = \begin{bmatrix} \omega_{ib,x} \\ \omega_{ib,y} \\ \omega_{ib,z} \end{bmatrix} \quad g^b = -f^b = \begin{bmatrix} -f_x \\ -f_y \\ -f_z \end{bmatrix} \]

\[\begin{bmatrix} (g^n)^T \\ (\boldsymbol{\omega}_{ie}^n)^T \\ (v^n)^T \end{bmatrix}^{-1} = \begin{bmatrix} \frac{\tan \varphi}{g} & \frac{1}{\omega_e \cos \varphi} & 0 \\ 0 & 0 & \frac{1}{g \omega_e \cos \varphi} \\ \frac{1}{g} & 0 & 0 \end{bmatrix} \]

\(\varphi\)是纬度。
只有当 \(g^n, \boldsymbol{\omega}_{ie}^n\) 两个向量不共线时，上述逆矩阵才存在。
解析粗对准方法不能用于地球两极地区；测量误差的存在使得姿态矩阵不是正交矩阵。