适用于高速旋转飞行体的半捷联式惯性测量与导航算法

摘要

本发明涉及惯性测量与导航算法，具体是一种适用于高速旋转飞行体的半捷联式惯性测量与导航算法。本发明解决了半捷联式惯性测量系统测得的运动信息无法准确反映高速旋转飞行体的运动信息的问题。适用于高速旋转飞行体的半捷联式惯性测量与导航算法，该算法是采用如下步骤实现的：1）实时测出三维比力；实时测出三维角速率；2）实时更新计算出系到n系的姿态矩阵、系相对n系的三维加速度、系相对n系的三维速度、系相对n系的三维位置、系相对n系的三维姿态角；3）求解出三维比力；4）求解出三维角速率；5）求解出三维加速度；6）求解出三维速度；7）求解出三维位置；8）求解出三维姿态角。本发明适用于测量高速旋转飞行体的运动信息。

主权项

一种适用于高速旋转飞行体的半捷联式惯性测量与导航算法，其特征在于：该算法是采用如下步骤实现的：1）假设高速旋转飞行体的发射坐标系为导航坐标系，简称为n系；假设高速旋转飞行体对应的坐标系为载体坐标系，简称为b系；假设半捷联式惯性测量系统对应的坐标系为测量坐标系，简称为系；假设在高速旋转飞行体的发射时刻，系与b系的对应轴向完全一致；当高速旋转飞行体开始运动后，b系随高速旋转飞行体同步变化，系则由于半捷联平台的隔转止旋作用而不随高速旋转飞行体同步变化，但b系和系的横滚轴方向始终一致，且b系和系的横滚角之差为通过半捷联式惯性测量系统中的三轴加速度计实时测出系相对n系的三维比力；通过半捷联式惯性测量系统中的三轴陀螺仪实时测出系相对n系的三维角速率、系与b系之间的横滚角之差；2）根据系相对n系的三维比力、系相对n系的三维角速率，实时更新计算出系到n系的姿态矩阵、系相对n系的三维加速度、系相对n系的三维速度、系相对n系的三维位置、系相对n系的三维姿态角；3）根据系相对n系的三维比力、系与b系之间的横滚角之差，求解出b系相对n系的三维比力；求解公式如下：$\begin{array}{c}\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \Delta {\gamma }_{b\tilde{b}}& \sin \Delta {\gamma }_{b\tilde{b}}\\ 0& -\sin \Delta {\gamma }_{b\tilde{b}}& \cos \Delta {\gamma }_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{f}_x^{\tilde{b}}\\ f_y^{\tilde{b}}\\ f_z^{\tilde{b}}\end{array}\right]\\ f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T\\ f^{\tilde{b}}={\left[\begin{array}{ccc}{f}_x^{\tilde{b}}& f_y^{\tilde{b}}& f_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(1\right);$式（1）中：f^b为b系相对n系的三维比力；为系相对n系的三维比力；为系与b系之间的横滚角之差；4）根据系相对n系的三维角速率、系与b系之间的横滚角之差，求解出b系相对n系的三维角速率；求解公式如下：$w_x^b=w_x^{\tilde{b}}+\stackrel{·}{\Delta }{\gamma }_{b\tilde{b}}$$\begin{array}{c}\left[\begin{array}{c}{w}_y^b\\ w_z^b\end{array}\right]=\left[\begin{array}{cc}\cos \Delta {\gamma }_{b\tilde{b}}& \sin \Delta {\gamma }_{b\tilde{b}}\\ -\sin \Delta {\gamma }_{b\tilde{b}}& \cos \Delta {\gamma }_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{w}_y^{\tilde{b}}\\ w_z^{\tilde{b}}\end{array}\right]\\ w^b={\left[\begin{array}{ccc}{w}_x^b& w_y^b& w_z^b\end{array}\right]}^T\\ w^{\tilde{b}}={\left[\begin{array}{ccc}{w}_x^{\tilde{b}}& w_y^{\tilde{b}}& w_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(2\right);$式（2）中：w^b为b系相对n系的三维角速率；为系相对n系的三维角速率；为系与b系之间的横滚角之差；5）根据系相对n系的三维加速度，求解出b系相对n系的三维加速度；求解公式如下：$\begin{array}{c}\left\{\begin{array}{c}{a}_{\mathrm{bx}}^n=a_{\tilde{b}x}^n\\ a_{\mathrm{by}}^n=a_{\tilde{b}y}^n\\ a_{\mathrm{bz}}^n=a_{\tilde{b}z}^n\end{array}\right.\\ a_b^n={\left[\begin{array}{ccc}{a}_{\mathrm{bx}}^n& a_{\mathrm{by}}^n& a_{\mathrm{bz}}^n\end{array}\right]}^T\\ a_{\tilde{b}}^n={\left[\begin{array}{ccc}{a}_{\tilde{b}x}^n& a_{\tilde{b}y}^n& a_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(3\right);$式（3）中：为b系相对n系的三维加速度；为系相对n系的三维加速度；6）根据系相对n系的三维速度，求解出b系相对n系的三维速度；求解公式如下：$\begin{array}{c}\left\{\begin{array}{c}{v}_{\mathrm{bx}}^n=v_{\tilde{b}x}^n\\ v_{\mathrm{by}}^n=v_{\tilde{b}y}^n\\ v_{\mathrm{bz}}^n=v_{\tilde{b}z}^n\end{array}\right.\\ v_b^n={\left[\begin{array}{ccc}{v}_{\mathrm{bx}}^n& v_{\mathrm{by}}^n& v_{\mathrm{bz}}^n\end{array}\right]}^T\\ v_{\tilde{b}}^n={\left[\begin{array}{ccc}{v}_{\tilde{b}x}^n& v_{\tilde{b}y}^n& v_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(4\right);$式（4）中：为b系相对n系的三维速度；为系相对n系的三维速度；7）根据系相对n系的三维位置，求解出b系相对n系的三维位置；求解公式如下：$\begin{array}{c}\left\{\begin{array}{c}{S}_{\mathrm{bx}}^n=S_{\tilde{b}x}^n\\ S_{\mathrm{by}}^n=S_{\tilde{b}y}^n\\ S_{\mathrm{bz}}^n=S_{\tilde{b}z}^n\end{array}\right.\\ S_b^n={\left[\begin{array}{ccc}{S}_{\mathrm{bx}}^n& S_{\mathrm{by}}^n& S_{\mathrm{bz}}^n\end{array}\right]}^T\\ S_{\tilde{b}}^n={\left[\begin{array}{ccc}{S}_{\tilde{b}x}^n& S_{\tilde{b}y}^n& S_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(5\right);$式（5）中：为b系相对n系的三维位置；为系相对n系的三维位置；8）根据系相对n系的三维姿态角、系与b系之间的横滚角之差，求解出b系相对n系的三维姿态角；求解公式如下：式（6）中：分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系与b系之间的横滚角之差。