[发明专利]高精度载体动态姿态测量方法及系统

权利要求书

1.高精度载体动态姿态测量方法，其特征在于，包括：

(1)在载体上设置第一微机电惯性测量单元和第二微机电惯性测量单元，其中第一微机电惯性测量单元通过转台安装在载体上且能够在转台的带动下周期性往复旋转，第二微机电惯性测量单元直接固定连接在载体上；

(2)对第一微机电惯性测量单元和第二微机电惯性测量单元分别进行标定；

（3）对标定后的第一微机电惯性测量单元和第二微机电惯性测量单元分别进行粗对准，粗对准时第一微机电惯性测量单元和第二微机电惯性测量单元输出的数据作为首次惯导解算得到的导航参数；

(4)根据前一次惯导解算得到的导航参数进行当前时刻的惯导解算，若当前时刻第一微机电惯性测量单元处于旋转状态，则对第二微机电惯性测量单元应用状态变换卡尔曼滤波算法进行滤波估计，得到当前时刻载体姿态值，而第一微机电惯性测量单元只对其进行预测而不进行滤波；若当前时刻第一微机电惯性测量单元处于静止状态，则对第一微机电惯性测量单元和第二微机电惯性测量单元同时应用状态变换卡尔曼滤波算法进行滤波估计，得到当前时刻载体姿态值；其中状态变换卡尔曼滤波算法中选择30维状态量即

(1)

其中分别为第二微机电惯性测量单元的三个姿态误差，分别为航向角误差、滚动角误差及俯仰角误差，分别为第二微机电惯性测量单元的三个速度误差，分别为北向速度误差、东向速度误差及地向速度误差，分别为第二微机电惯性测量单元的三个位置误差，分别为经度误差、纬度误差及高度误差，分别为第二微机电惯性测量单元的三个陀螺误差，分别为x轴陀螺零偏、y轴陀螺零偏及z轴陀螺零偏，分别为第二微机电惯性测量单元的三个加速度计误差，分别为x轴加速度计零偏、y轴加速度计零偏及z轴加速度计零偏；而分别为第一微机电惯性测量单元的航向角误差、滚动角误差及俯仰角误差，分别为第一微机电惯性测量单元的北向速度误差、东向速度误差及地向速度误差，分别为第一微机电惯性测量单元的经度误差、纬度误差及高度误差，分别为第一微机电惯性测量单元的x轴陀螺零偏、y轴陀螺零偏及z轴陀螺零偏，分别为第一微机电惯性测量单元的x轴加速度计零偏、y轴加速度计零偏及z轴加速度计零偏；

状态变换卡尔曼滤波算法中选取15维观测量即

（2）

其中为第二微机电惯性测量单元的北向、东向、地向的三个速度值，为第一微机电惯性测量单元的北向、东向、地向的三个速度值，为载体的北向、东向、地向的三个速度值，分别为第一微机电惯性测量单元和第二微机电惯性测量单元三个姿态值的差值，分别为第一微机电惯性测量单元和第二微机电惯性测量单元三个速度值的差值，分别为第一微机电惯性测量单元和第二微机电惯性测量单元三个位置值的差值；

状态变换卡尔曼滤波算法中的状态方程和观测方程如下：

（3）

（4）

其中为选取的状态量的导数；F(t)为状态转移矩阵；x(t)为选取的状态量；G(t)为过程噪声分布矩阵；w(t)为过程噪声；z(t)为观测量；H(t)为观测矩阵；v(t)为观测噪声；

(5)重复步骤（4），直至结束。

2.根据权利要求1所述的高精度载体动态姿态测量方法，其特征在于，第一微机电惯性测量单元的旋转规则为双位置往复旋转，并在每个位置停留预设时间。

3.根据权利要求2所述的高精度载体动态姿态测量方法，其特征在于，预设时间为3到8分钟。