一种基于运动状态监测的自适应水平姿态测量方法技术

本发明专利技术公开了一种基于运动状态监测的自适应水平姿态测量方法，基于新建立的状态空间模型，将水平姿态角作为状态变量，补偿随机常值零偏的角速度增量Δω

【技术实现步骤摘要】
一种基于运动状态监测的自适应水平姿态测量方法
本专利技术涉及一种基于运动状态监测的自适应水平姿态测量方法，属于惯性

技术介绍
随着微机电系统技术的发展，低成本MEMS-IMU在导航领域有着越来越多的应用，通过利用基于微机电系统的惯性传感器进行运动参数测量，可以检测船舶在海中复杂的运动状态，实现用户对水面舰船的运动数据采集，这要求MEMS-IMU能够实时准确地输出载体的角运动参数和线运动参数。微机电陀螺仪具有随机漂移特性，其积分误差随时间累积，加速度计不存在累积误差，但是易受到载体震动影响。常用的将二者数据融合的算法是卡尔曼滤波和互补滤波，例如在专利申请号为201811070907.X，名称为“基于机动状态判断的MEMS惯性导航系统水平姿态自修正方法”的专利文件中，通过比较加速度计输出和当地重力加速度幅值，将载体运动分为低、中、高动态。在低和中动态时，实时调整量测噪声矩阵，在高动态时只进行时间更新。但是若载体较长时间处于高动态下，则姿态误差会越来越大。又如在专利申请号为202011092956.0，名称为“基于自适应EKF算法的小型无人机MARG航姿估计方法”的专利文件中，针对外部加速度的自适应滤波算法，通过对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。传统自适应调整的方法大多通过对载体加速度计输出的模值与当地重力加速度进行比较，实现运载体机动状态的判断，未考虑载体角运动对加速度测量的干扰，这对于一些复杂环境来说可能是不充分的。专利
技术实现思路
针对上述现有技术，本专利技术要解决的技术问题是提供一种基于运动状态监测的自适应水平姿态测量方法，可以提高运载体在机动场景下的水平姿态测量精度，为运载体提供准确的水平姿态信息。为解决上述技术问题，本专利技术的一种基于运动状态监测的自适应水平姿态测量方法，包括以下步骤：步骤1：对捷联惯性导航系统进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角θ0和俯仰角φ0，然后进入导航工作模式；步骤2：初始化卡尔曼滤波器，将步骤1中得到的初始水平姿态角θ0和φ0作为卡尔曼滤波状态量X0＝[θ0φ0]T的初始值，初始均方误差为P0；步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力和角速度步骤4：利用第k时刻的角速度增量作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测，其中，T为解算周期；步骤5：利用步骤3中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节，其中N为数据窗的大小；步骤6：第k时刻比力为选取量测向量Zk＝[fx,kfy,k]T进行量测更新，实现状态量的校正，其中fx,k、fy,k和fz,k分别为比力在载体系x轴、y轴和z轴方向的分量；步骤7：将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。本专利技术还包括：1.步骤4中卡尔曼滤波一步预测具体为：式中，为k-1时刻状态最优估计，Pk-1为k-1时刻状态估计的均方误差阵，为k时刻状态一步预测，Φk/k-1＝I2为k时刻状态一步转移矩阵，I2为二阶单位阵，为k时刻输入系数矩阵，φk-1、θk-1为k-1时刻水平姿态最优估计，uk-1为第k时刻角速度增量Pk/k-1为k时刻状态一部预测均方误差阵，Qk为系统噪声方差阵。2.步骤5所述利用步骤3中获得的比力fb和角速度ωb对运载体的机动状态进行判别具体为：利用步骤3中获得的k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行计算，求取机动向量Tk，实现机动判断，Tk满足：式中，和分别为对步骤3中第k-N+1时刻到k时刻惯性测量单元输出且补偿随机常值零偏的比力fb和角速度ωb进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σf和σω分别为权重系数；3.步骤5中对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节具体为：式中，Tk,i为Tk的第i个元素，为加速度计噪声对应方差。4.步骤6中量测更新的更新方程为：式中，为k时刻量测矩阵，g为当地重力加速度，φk、θk为k时刻水平姿态一步预测值，Kk为k时刻滤波增益，为k时刻状态估计，Pk为k时刻状态估计均方误差阵。本专利技术的有益效果：本专利技术涉及以微机电惯性测量单元(MEMS-IMU)为核心器件的姿态测量单元，本专利技术建立新的状态空间模型，将水平姿态角作为状态变量，MEMS-IMU输出的角速度增量Δωb作为系统方程的控制输入，MEMS-IMU输出的比力fb作为量测量，同时对载体机动判断条件进行改善，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息的判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对对平姿态信息的影响。该方法对载体机动状态无特殊要求，能够在无外界信息辅助的情况下，保证系统在不同运动状态下具有较高的姿态测量精度，有一定的工程应用价值。附图说明图1为本专利技术的实现流程图；图2为本专利技术的实现算法流程图；图3为本专利技术提出算法解算误差。具体实施方式下面结合附图和具体实施方式对本专利技术做进一步说明。本专利技术是这样实现的：对捷联惯性导航系统的惯性测量元件进行充分预热，并完成器件随机常值零偏计算和初始水平姿态角的计算，之后可进入导航工作状态；将水平姿态角作为状态变量，MEMS-IMU输出的角速度增量Δωb作为系统方程的控制输入，MEMS-IMU输出的比力fb作为量测量，建立卡尔曼滤波方程，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息进行判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对水平姿态解算的影响。具体步骤如下：步骤1、对捷联惯性导航系统的惯性测量元件进行充分预热，并完成器件随机常值零偏计算和初始水平姿态角(横滚角θ0和俯仰角φ0)的计算，使之进入导航工作状态；步骤2、将步骤1中得到的初始水平姿态角θ0和φ0作为卡尔曼滤波状态量X0＝[θ0φ0]T的初始值，初始均方误差为P0，初始化卡尔曼滤波器；步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力和角速度步骤4、利用第k时刻的角速度增量作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测，其中，T为解算周期；；步骤5、利用步骤2中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节，其中N为数据窗的大小；步骤6、利用第k时刻的比力作为量测向量Zk进行量测更新，实现状态量的校正；步骤7、将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。步骤4中卡尔曼滤波一步预测本文档来自技高网...

【技术保护点】
1.一种基于运动状态监测的自适应水平姿态测量方法，其特征在于，包括以下步骤：/n步骤1：对捷联惯性导航系统进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角θ

【技术特征摘要】
1.一种基于运动状态监测的自适应水平姿态测量方法，其特征在于，包括以下步骤：
步骤1：对捷联惯性导航系统进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角θ0和俯仰角φ0，然后进入导航工作模式；
步骤2：初始化卡尔曼滤波器，将步骤1中得到的初始水平姿态角θ0和φ0作为卡尔曼滤波状态量X0＝[θ0φ0]T的初始值，初始均方误差为P0；
步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力和角速度
步骤4：利用第k时刻的角速度增量作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测，其中，T为解算周期；
步骤5：利用步骤3中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节，其中N为数据窗的大小；
步骤6：第k时刻比力为选取量测向量Zk＝[fx,kfy,k]T进行量测更新，实现状态量的校正，其中fx,k、fy,k和fz,k分别为比力在载体系x轴、y轴和z轴方向的分量；
步骤7：将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。


2.根据权利要求1所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤4所述卡尔曼滤波一步预测具体为：



式中，为k-1时刻状态最优估计，Pk-1为k-1时刻状态估计的均方误差阵，为k时刻状态一步预测，Φk/...

【专利技术属性】
技术研发人员：赵玉新，李帅阳，奔粤阳，吴磊，李倩，周广涛，臧新乐，魏廷枭，王健成，周一帆，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23