[发明专利]摇摆基座下MEMS IMU的全姿态解算方法

权利要求书

1.一种摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，包括以下步骤：

定义满足预设规则的坐标系；

在定义的所述坐标系下，使得MEMSIMU按正反连续旋转方式进行旋转；

采集旋转时MEMS惯性器件的数据；以及

根据所述MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到所述MEMSIMU的全姿态解算结果。

2.根据权利要求1所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，所述预设规则为右手定则，其中，所述坐标系包括导航坐标系n系、载体坐标系b系、IMU坐标系s系、地球坐标系e系、惯性坐标系i系、基座惯性坐标系i_b0系。

3.根据权利要求2所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，所述使得MEMSIMU按正反连续旋转方式进行旋转，进一步包括：

在初始时刻，所述IMU坐标系s系与所述载体坐标系b系重合；

使得所述MEMSIMU先绕z轴以角速度ω＝20°/s正向旋转360度，再反向以角速度ω＝20°/s旋转360度，并单轴正反交替连续旋转。

4.根据权利要求3所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，所述根据所述MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到所述MEMSIMU的全姿态解算结果，进一步包括：

根据所述惯性系对准算法，确定不同坐标系之间的转换关系矩阵，根据所述各个坐标系之间的转换矩阵，从所述MEMS惯性器件的数据中提取得到俯仰角、横滚角、航向角；

根据所述Mahony算法，将所述MEMSIMU中加速度计的数据获得的低频姿态估计进行低通滤波，并对陀螺仪输出直接积分获得的高频姿态估计进行高通滤波，并融合两者的姿态估计信息，以解算水平姿态角；

根据所述Mahony算法修正所述俯仰角和所述横滚角，根据修正后的所述俯仰角、修正后的所述横滚角、所述航向角实现全姿态解算。

5.根据权利要求1-4任一项所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，其中，

所述导航坐标系和所述惯性坐标系的转换矩阵

其中，ω_ie为地球自转角速率，L为载体所在位置的纬度，Δt为载体时间间隔；

所述基座惯性坐标系与所述IMU坐标系之间的转换矩阵

其中为陀螺的输出数据；

所述惯性坐标系和所述基座惯性坐标系之间的转换矩阵

其中，根据t_k1、t_k2时刻(t₀<t_k1<t_k2)的速度值V(t_k1)、V(t_k2)构建辅助矢量V(t_k1)×V(t_k2)，为所述基座惯性坐标系下的速度值，Vⁱ(t_k)为重力加速度在惯性系下的速度值。