(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111425854.0 (22)申请日 2021.11.27 (71)申请人 中国舰船研究设计中心 地址 201108 上海市闵行区华宁路2 931号 (72)发明人 刘萍　刘士忠　成冬冬　余秀　 王肖建　 (74)专利代理 机构 上海航天局专利中心 31 107 代理人 唐敏 (51)Int.Cl. G01C 25/00(2006.01) G06F 30/20(2020.01) (54)发明名称 一种冗余式惯性测量单元故障补位陀螺仪 标定方法 (57)摘要 本发明提供一种冗余式惯性测量单元故障 补位陀螺仪标定方法， 其特征在于， 包括如下步 骤： 步骤一： 确定待标定的补位陀螺仪的数量及 其序号； 步骤二： 构建故障补位陀螺仪标定模型； 步骤三： 构建状态空间模 型， 确定初始参数； 步骤 四： 建立序贯自适应扩展卡尔曼滤波算法， 估计 待标定参数； 步骤五： 将估计得到的参数根据补 偿模型标定陀螺仪， 得到精确的角速度输出。 本 发明具有估计精度高、 通用性强、 自主性强等优 点， 无需外界参考即可实现系统量测噪声发生改 变的条件下， 冗余式惯性测量单元 故障补位陀螺 仪精确标定 。 权利要求书2页 说明书8页 附图8页 CN 114235006 A 2022.03.25 CN 114235006 A 1.一种冗余式惯性测量单 元故障补位陀螺 仪标定方法， 其特 征在于， 包括如下步骤： 步骤一： 确定待标定的补位陀螺 仪的数量及其序号； 步骤二： 构建故障补位陀螺 仪标定模型； 步骤三： 构建状态空间模型， 确定初始参数； 步骤四： 建立序贯自适应扩展卡尔曼 滤波算法， 估计待标定参数； 步骤五： 将估计得到的参数根据补偿模型 标定陀螺 仪， 得到精确的角速度输出。 2.如权利要求1所述的冗余式惯性测量单元故障补位陀螺仪标定方法， 其特征在于， 步 骤四中的所述待标定参数包括 零偏、 标度因数误差和安装误差 。 3.如权利要求1所述的冗余式惯性测量单元故障补位陀螺仪标定方法， 其特征在于， 步 骤二中所述故障部位陀螺 仪标定模型为： W(HTFH)‑1HTFωi‑ωm＝(KW+θyU+θxV‑KθyU‑KθxV)(HTFH)‑1HTFωi+b+ ε 其中， H为冗余式惯性测量单元的冗余配置矩阵， U＝[u1 u2…um]T， V＝[v1 v2…vm]T， W＝ [w1 w2…wm]T为坐标转换矩阵， ui、 vi和wi与冗余式惯性测量单元的配置方式相关， b＝[b1  b2…bm]T为待标定补位陀螺仪零偏， K＝diag([k1 k2…km])为补位陀螺仪标度因数误差矩 阵， θx＝diag([ θx1 θx2…θxm])， θy＝diag([ θy1 θy2…θym])为补位陀螺仪安装误差矩阵， n为冗 余式惯性测量单元 中陀螺仪的总数量， m为待标定故障补 位陀螺仪数量， 且m≤n ‑3， ωi为无 故障陀螺仪输出， F为故障矩阵， 在第i个传感器发生 故障时， 故障矩阵的第i行和第i列元素 为零， ε为陀螺 仪噪声矩阵， ωm＝[ω1 ω2…ωm]T为补位陀螺 仪输出的角速度。 4.如权利要求1所述的冗余式惯性测量单元故障补位陀螺仪标定方法， 其特征在于， 步 骤四中所述状态空间模型包 含状态方程和量测方程， 所述状态方程 为： xk+1＝xk 其中， x＝[b  k θx θy]T， b为所有待标定补位陀螺仪的零偏， k为所有待标定补位陀螺仪 的标度因数误差， θx和 θy为所有待标定补位陀螺 仪的安装误差； 所述量测方程 为： zk+1＝(KW+θyU+θxV‑KθyU‑KθxV)·(HTFH)‑1HTFωi+b+ ε 其中， zk+1＝W(HTFH)‑1HTFωi‑ωm为量测量。 5.如权利要求1所述的状态 空间模型， 其特征在于， 步骤四中所述序贯自适应扩展卡尔 曼滤波算法包 含如下步骤： 第一、 状态预测 第二、 估计误差协方差预测 Pk/k‑1＝Pk‑1+Qk‑1 第三、 量测噪声协方差Rk估计： 其中， Jk为所述量测方程的雅克比矩阵， 权　利　要　求　书 1/2 页 2 CN 114235006 A 2第四、 计算扩展卡尔曼 滤波增益 第五、 更新状态估计 第六、 更新估计误差协方差： Pk＝(In‑KkJk)Pk/k‑1。 6.如权利要求1所述的状态空间模型， 其特 征在于， 步骤五中所述补偿模型为： 其中， (‑θxV‑θyU+W)*为‑θxV‑θyU+W的广义逆矩阵， 为补偿零偏、 标度因数误差和安装 误差后的补位陀螺 仪输出。 7.如权利要求1所述的状态 空间模型， 其特征在于， 所述冗余式惯性测量单元中惯性器 件数量大于或等于4个。 8.如权利要求3所述的状态 空间模型， 其特征在于， 所述故障矩阵F确定方法为： 系统无 故障时故障矩阵为单位对角阵， 在第i个传感器发生 故障时， 故障矩阵的第i行和 第i列元素 为零。权　利　要　求　书 2/2 页 3 CN 114235006 A 3