本发明涉及传感器，具体为一种基于mems惯性传感器的载体姿态估计方法
背景技术：
1、mems，即微机电系统(micro-electro-mechanical systems)，是指集微型传感器、执行器以及信号处理和控制电路、接口电路、通信和电源于一体的微型机电系统。其内部结构一般在微米甚至纳米量级，是一个独立的智能系统，mems技术建立在微米/纳米技术(即超精密技术)基础之上的，以超精密机械加工技术为前提，融合多种现代高科技手段制造出的微型传感器、执行器以及微机械结构。它的发展集结了多种现代科学技术发展的结晶，包括精密机械制造、电子、化学、材料、物理学、光学、生物医学等多种学科，并追求系统小型化、集成化、智能化的发展目标，由于其体积小、重量轻、功耗低、性能稳定、可靠性高、成本低、便于批量化生产及集成化等特性，mems被广泛应用于汽车、医疗、工业、消费电子和军事等领域。具体来说，mems传感器可以测量许多物理量，如加速度、角速度、压力、位移、流量、温度、湿度、光照强度等，从而为各种应用场景提供关键的感知数据。
2、目前mems惯性传感器的惯性系统误差标定涉及比例因子、非正交性、零偏及安装误差角等主要误差参数。当前标定过程均采用三轴转台进行全参数误差标定，这一过程通常需要几个小时才能完成，而由于mems惯性系统误差参数稳定性较差，会在较长的时间内出现误差参数变化的特性，所以不能快速的标定误差参数，因此我们提出了一种基于mems惯性传感器的载体姿态估计方法来解决上述问题。
技术实现思路
1、(一)解决的技术问题
2、针对现有技术的不足，本发明提供了一种基于mems惯性传感器的载体姿态估计方法，解决了上述背景技术中所提出的问题。
3、(二)技术方案
4、一种方向余弦分解原理的姿态估计方法，包括以下几个步骤：
5、步骤1：获取mems惯性传感器数据；
6、由mems加速度计量测模型可知：
7、
8、式中，表示加速度计测量比力；fb表示真实比力；表示加速度计零偏；表示加速度计量测噪声；
9、由mems陀螺仪测量模型可知：
10、
11、式中，表示陀螺仪测量角速度；表示真实角速度；εb表示陀螺仪零偏；表示陀螺仪量测噪声；
12、步骤2：建立方向余弦分解姿态估计模型；
13、由方向余弦矩阵可知：
14、
15、式中，表示载体系b到导航系n的方向余弦矩阵；θc＝cos(θ)表示对俯仰角θ求余弦；θs＝sin(θ)表示对俯仰角θ求正弦；φc＝cos(φ)表示对横滚角φ求余弦；φs＝sin(θ)表示对俯仰角φ求正弦；ψc＝cos(ψ)表示对航向角ψ求余弦；ψs＝sin(ψ)表示对俯仰角ψ求正弦；
16、且方向余弦微分可以表示为：
17、
18、式中，表示方向余弦矩阵的微分；表示载体系b到导航系n的方向余弦矩阵；[ωb×]表示角速度ωb叉乘计算，可以采用下式计算：
19、
20、式中，ωb表示旋转角速度；表示旋转角速度x分量；表示旋转角速度y分量；表示旋转角速度z分量；
21、对上面方向余弦微分进行提取计算可得：
22、
23、式中，表示对方向余弦矩阵的第三行求微分；c31表示方向余弦矩阵的第三行第一列元素；c32表示方向余弦矩阵的第三行第二列元素；c32表示方向余弦矩阵的第三行第二列元素；c33表示方向余弦矩阵的第三行第三列元素；表示旋转角速度x分量；表示旋转角速度y分量；表示旋转角速度z分量；
24、由加速度计测量值与重力加速度关系可知：
25、
26、式中，fb表示真实比力；表示比力x分量；表示比力y分量；表示比力z分量；表示载体系b到导航系n的方向余弦矩阵；(.)t表示对矩阵求转置；g表示重力加速度；c31表示方向余弦矩阵的第三行第一列元素；c32表示方向余弦矩阵的第三行第二列元素；c32表示方向余弦矩阵的第三行第二列元素；c33表示方向余弦矩阵的第三行第三列元素；
27、步骤3：建立自适应扩展卡尔曼滤波器实现姿态估计；
28、利用上述计算，可以构造状态：
29、
30、式中，x表示状态量；方向余弦矩阵的第三行；εb表示陀螺仪零偏；c31表示方向余弦矩阵的第三行第一列元素；c32表示方向余弦矩阵的第三行第二列元素；c32表示方向余弦矩阵的第三行第二列元素；c33表示方向余弦矩阵的第三行第三列元素；表示陀螺零偏εb的x分量；表示陀螺零偏εb的y分量；表示陀螺零偏εb的z分量；
31、构造线性系统模型：
32、
33、式中，xk+1表示k+1时刻状态量；xk表示k时刻状态量；fk(xk，uk)表示非线性状态转移函数；γk表示采样间隔时间；wk表示过程噪声；yk表示观测矢量；h表示观测矩阵；vk表示观测噪声；
34、上式中，非线性转移函数可以表示为：
35、
36、式中，fk(xk，uk)表示非线性状态转移函数；方向余弦矩阵的第三行；i3表示三维单位矩阵；xk表示k时刻状态量；δt表示采样时间；uk表示输入量；
37、观测矩阵可以表示为：
38、h＝[gi3 03×3]
39、式中，h表示观测矩阵；g表示重力加速度；i3表示三维单位矩阵；
40、为了抑制状态相关噪声影响，采用自适应过程噪声协方差：
41、
42、式中，qk表示过程噪声协方差；表示状态预测方差；表示零偏方差；i3表示三维单位矩阵；γ表示采样间隔时间；
43、观测噪声同样采用自适应调节方程：
44、
45、式中，rk表示量测噪声方差阵；表示加速度量测去掉重力加速度之后的加速度；i3表示三维单位矩阵；表示加速度计量测噪声方差；
46、步骤4：姿态估计时间长度为m，若k≥m，则输出姿态估计与零偏估计结果，完成估计过程，若k<m，表示估计未完成，则重复上述步骤一至步骤三，直至姿态估计结束。
47、进一步地，步骤1中，陀螺仪量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200hz，加速度计量测常值漂移误差为加速度计量测随机游走误差为输出频率为200hz。
48、进一步地，步骤4中的m＝600。
49、(三)有益效果
50、与现有技术相比，本发明提供了一种基于mems惯性传感器的载体姿态估计方法，具备以下有益效果：本发明采用简化mems惯性系统误差模型，具有计算简便的优点；本发明设计标量域参数标定过程，结合迭代优化算法实现参数估计，具有高效简便的优点；本发明采用三轴旋转及矩阵分解算法，实现陀螺仪误差参数估计，具有计算简便、精确的优点，从而在进行mems惯性系统误差标定时，能够快速简化标定过程，实现参数标定，同时减小对外部设备的依赖，提高系统便携特性。
1.一种基于mems惯性传感器的载体姿态估计方法，其特征在于：包括以下几个步骤：
2.根据权利要求1所述的一种基于mems惯性传感器的载体姿态估计方法，其特征在于：步骤1中陀螺仪量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200hz，加速度计量测常值漂移误差为加速度计量测随机游走误差为输出频率为200hz。
3.根据权利要求1所述的一种基于mems惯性传感器的载体姿态估计方法，其特征在于：步骤4中的m＝600。
