本发明涉及一种物体姿态测量，特别涉及一种基于惯性导航技术的姿态融合增强测量方法和系统，属于惯性导航技术领域。
背景技术
在实际生活中，运动物体的姿态测量是必不可少的。通过对姿态的测量，可以对运动物体进行控制，使其按照期望进行姿态运动；也可以通过姿态的测量，了解运动物体的姿态变化，进而得知其内在的运动规律。物体在空间运动存在六个自由度，分别是三个线自由度和三个角自由度。其中三个角自由度运动，即空间转动，可以用欧拉角来直观的进行表示，描述物体在三维空间的姿态，通常称为横滚角、俯仰角和航向角。这三个欧拉角也被称为姿态角，或者简称姿态，通常用四元数，即四维向量表示。欧拉角的变化，即姿态的变化。运动物体可以是飞机，汽车或者人的手臂等可以发生姿态变化的物体。
姿态测量在生产生活中无处不在。姿态测量装置一般采用陀螺仪，加速度计和磁强计。传统的高精度陀螺仪imu，使用的是高精度的传感器器件，价格昂贵，体积较大，重量较重，不适于精度要求较低的应用场合。因此，近年来，随着微机电mems器件的发展，出现了以mems陀螺仪，mems加速度计和mems磁传感器(也称作磁强计)为传感器的各种小型化的航姿参考系统ahrs。mems器件，成本低，质量轻，体积小。在精度满足使用需求的前提下，使用mems器件是不二选择。mems陀螺仪，利用科里奥利力测量运动物体的角速度，动态响应特性好(带宽较宽)，但计算姿态时采用积分算法，会产生累积误差；mems加速度计用于测量物体的运动加速度和重力加速度，易受物体振动的影响，低频特性好，静态精度高；mems磁传感器，测量物体所处当地的地磁场强度，易受外界电磁干扰，但是长期精度有界，不会发散。磁传感器和加速度计测量姿态没有累积误差，但动态响应差(带宽较窄)，且噪声相对较大。由于各个传感器本身固有的特点，导致使用单一种类传感器难以获得有效稳定的姿态信息。因此，考虑到mems陀螺仪、mems磁传感器和mems加速度计在频域上的特性互补，采用互补滤波方式融合三种传感器数据，使三种传感器优势互补，提炼有效的姿态信息，提高姿态测量精度，得到广泛应用。
现有的技术方案中，姿态互补滤波的主要缺点为：
(1)互补滤波通常采用pi反馈方式(kp和ki分别为反馈系数)，属于线性控制方式，很难同时保证算法的快速性和稳定性。
(2)mems磁传感器很容易受到外界干扰，在室内磁场环境更为恶劣，由于外界磁干扰的影响，常规互补滤波算法修正航向角时，经常会出现影响俯仰角和横滚角的情况。即：外界磁异常导致俯仰角和横滚角修正异常。
技术实现要素：
针对上述不足，考虑到采用传统的线性控制，只能达到指数稳定，即系统以指数方式趋近于零(需要无穷长时间达到零)。而引入非线性控制后，则能提高系统性能，达到有限时间稳定(在有限时间到达零点)。因此，提出一种基于惯性导航技术的非线性增强的互补滤波姿态测量方法和系统。
技术方案如下：
一种基于惯性导航技术的姿态融合增强测量方法。通过陀螺仪，加速度计和磁强计获取物体的姿态数据，对姿态数据进行修正，输出物体的互补滤波姿态数据，其特征在于：
步骤1，获取各传感器检测的姿态数据，进行数据初始化；
步骤2，对初始化后的姿态数据进行捷联解算；
步骤3，修正各传感器对应的姿态数据，其中，基于磁强计数据计算体坐标系的磁场偏差em，将所述磁场偏差em投影为导航系的磁场偏差t，将所述磁场偏差t的x、y分量置为零；其中，所述磁场偏差t为四元数，所述x、y分量为四元数t的x、y分量；
步骤4，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正陀螺零点；其中所述磁强计数据包括修正后的所述磁场偏差；
步骤5，输出修正后的姿态数据。
进一步，所述数据初始化为四元数初始化，并初始化陀螺偏差。
进一步，所述捷联解算为四元数捷联解算。
进一步，所述修正各传感器对应的姿态数据包括对加速度计数据进行修正，计算加速度计数据偏差e。
进一步，所述修正各传感器对应的姿态数据包括加速度异常判断，磁异常判断。
一种基于惯性导航技术的姿态融合增强测量系统，基于陀螺仪，加速度计和磁强计，获取物体的姿态数据，对姿态数据进行修正，输出物体的互补滤波姿态数据，其特征在于，包括：
初始化模块，对各传感器检测的姿态数据，进行数据初始化；
捷联更新模块，对初始化后的姿态数据进行捷联解算；
传感器数据修正模块，修正各传感器对应的姿态数据，其中，所述传感器数据修正模块包括磁强计数据修正模块，所述磁强计数据修正模块基于磁强计数据计算体坐标系的磁场偏差em，将所述磁场偏差em投影为导航系的磁场偏差t，将所述磁场偏差t的x、y分量置为零；其中，所述磁场偏差t为四元数，所述x、y分量为四元数t的x、y分量；
非线性互补滤波模块，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正陀螺零点；其中所述磁强计数据包括修正后的所述磁场偏差；
所述捷联更新模块输出修正后的姿态数据。
进一步，所述数据初始化为四元数初始化，并初始化陀螺偏差。
进一步，所述捷联解算为四元数捷联解算。
进一步，所述修正各传感器对应的姿态数据包括对加速度计数据进行修正，计算加速度计数据偏差e。
进一步，所述修正各传感器对应的姿态数据包括加速度异常判断，磁异常判断。
与现有技术相比，本发明的非线性增强的互补滤波姿态测量方法和系统具有如下有益效果：
(1)姿态解耦的磁航向修正，具有磁异常检测功能，且磁修正不影响俯仰角和横滚角修正；
(2)非线性互补方法，基于非线性理论，提供了更好的收敛速度和控制精度。
附图说明
图1：本发明非线性增强的互补滤波姿态测量系统结构框图；
图2：本发明比例线性反馈与非线性反馈对比图；
图3：本发明坐标系图；
具体实施方式
下面结合附图和具体实施例对本发明进行详细描述，但不作为对本发明的限定。
在一个实施例中，如图1所示，基于惯性导航技术的非线性增强的互补滤波姿态测量方法包括如下步骤：
1.获取速率陀螺仪数据，进行四元数初始化，并初始化陀螺偏差δ＝[δxδyδz]t＝[000]t
此陀螺偏差δ为零点偏移估计值，方法过程中，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正该陀螺偏差δ，即陀螺零点偏移估计值。陀螺的零点偏移是影响导航精度的主要因素。
2.姿态数据捷联解算，进行四元数捷联更新。
根据陀螺的三轴速率的测量值，按照下述公式得到三轴速率的估计值；
w＝w’+δ,
其中w＝[wxwywz]t为三轴速率的估计值，w’为三轴速率的测量值。
利用陀螺的三轴速率估计值w的三轴角速度wx，wy，wz，在每一个周期，根据下述公式进行空间三维积分，获得物体姿态的角度估计q。
其中，q＝[q0q1q2q3]t；q为四元组，用来描述物体的欧拉角，上述等式左边是
3.传感器数据融合修正
3.1加速度计数据的修正
由加速度计采集数据，并进行加速度修正：
3.1.1加速度异常判断：当三轴加速度计的数据模值sqrt(ax2+ay2+az2)与1g相差较大时，则不进行加速度计数据的修正；其中，ax,ay,az表示三轴向上的加速度测量值。
3.1.2加速度计数据归一化，获得a＝[ax，ay，az]t，a＝a/norm(a)；其中，norm代表归一化处理，即数组的平方和再开平方根。
3.1.3计算加速度计数据偏差ea：重力加速度在体坐标系中的分量v与加速度计输出值的叉积
ea＝a×v，其中v为重力加速度在体坐标系的分量。
v＝cbn[001]t；其中cbn是旋转矩阵，表示从导航系到体坐标系带旋转变换。
3.2磁强计数据修正
由磁强计采集数据，并进行磁强计数据修正：
3.2.1磁异常判断：当磁场强度变化过于剧烈，或强度与标定值相差过大，则不进行磁强计数据修正；
3.2.2磁强计数据归一化：m＝[mx,my,mz]t，m＝m/norm(m)；
3.2.3计算磁场偏差
em＝m×w，其中w为磁强计在体坐标系的投影；
3.2.4将此磁场偏差em＝[emx，emy，emz]t从体坐标系投影至导航系
t＝[0，emx，emy，emz]
t＝q×t×q-1；
3.2.5为确保磁偏差修正不影响俯仰角和横滚角，将t的x、y分量置为零
t(2:3)＝0；
3.2.6将t重新投影回到导航系
t＝q-1×t×q
em＝t
此时，磁场偏差em完成了磁强计数据解耦修正；
4.姿态数据四元数非线性互补滤波修正
根据修正后的加速度数据，磁强计数据，对物体姿态数据e进行非线性互补滤波修正：
e＝em+ea；
δ＝kp*e+ki∫edt+kn|e|1/2；
其中，ea是加速度计偏差，em为磁场偏差。由前述可知，这些数据都是三维向量。根据磁场和加速度计测量偏差估算陀螺零点偏移δ，进而完成零点偏移估算。公式中kp,ki,kn为三个常数，需要根据陀螺、加速度计和磁力计的特性来确定。kp,ki是普通pid控制(比例-积分-微分控制)中的比例项和积分项。其中kp与系统收敛速度有关，ki确保系统稳态跟踪误差为零。但普通pid控制的缺点之一是指数收敛，无法在有限时间收敛到零。kn项加入了非线性调节方式，能够获得比线性控制更好的效果，同时可以确保有限时间收敛到零。如图2的比例线性反馈与非线性反馈对比图所示，在陀螺零点附近，非线性反馈斜率远大于现行反馈，有更快的收敛速度。
根据加速度计和磁强计的偏差(即修正后的加速度数据，磁强计数据)，利用反馈控制的方式来修正速率陀螺零点，此种非线性控制方式的引入，提高系统响应速度和性能。
同时，在一个实施例中，实现上述方法的基于惯性导航技术的非线性增强的互补滤波姿态测量系统包括：
(1)初始化模块，获取各传感器数据，进行数据初始化；
(2)捷联更新模块，进行姿态数据的四元数捷联更新；输出姿态数据；
(3)传感器数据修正模块，对各传感器数据进行修正；具体包括：加速度计数据修正模块，对加速度计采集数据进行加速度修正；磁强计数据修正模块，对磁强计采集数据进行磁强计数据修正；非线性互补滤波模块，根据加速度计和磁强计的偏差，利用反馈控制的方式来修正速率陀螺零点。
上述各模块具体操作对应于上述基于惯性导航技术的非线性增强的互补滤波姿态测量的方法步骤。
本发明的具有如下有益效果：
(1)姿态解耦的磁航向修正，具有磁异常检测功能，且磁修正不影响俯仰角和横滚角修正；
(2)非线性互补方法，基于非线性理论，提供了更好的收敛速度和控制精度。
以上所述仅为本发明的较佳实施例，并不能因此而理解为对本发明范围的限制，应当指出，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。