[发明专利]一种基于惯性导航技术的姿态融合增强测量方法及系统

权利要求书

1.一种基于惯性导航技术的姿态融合增强测量方法，通过陀螺仪，加速度计和磁强计获取物体的姿态数据，对姿态数据进行修正，输出物体的互补滤波姿态数据，其特征在于：

步骤1，获取各传感器检测的姿态数据，进行数据初始化；

步骤2，对初始化后的姿态数据进行捷联解算；

步骤3，对加速度计数据和磁强计数据进行修正，其中，对磁强计数据进行修订，包括：

磁异常判断：当磁场强度变化过于剧烈，或强度与标定值相差过大，则不进行磁强计数据修正；

磁强计数据归一化：M＝[mx,my,mz]^T，M＝M/norm(M)；

计算磁场偏差：Em＝M×W，其中W为磁强计在体坐标系的投影；

将此体坐标系的磁场偏差Em＝[Emx，Emy，Emz]^T从体坐标系投影至导航系：t＝[0，Emx，Emy，Emz]；t＝q×t×q^-1；其中，q为四元数，用来描述物体的欧拉角；

为确保磁偏差修正不影响俯仰角和横滚角，将t的X、Y分量置为零：t(2:3)＝0，其中t(2:3)＝0表示将t的X、Y分量置为零；

将t重新投影回到导航系：t＝q^-1×t×q；Em＝t，

其中，基于磁强计数据计算体坐标系的磁场偏差Em，将所述体坐标系的磁场偏差Em投影为导航系的磁场偏差t，将所述导航系的磁场偏差t的X、Y分量置为零；其中，所述导航系的磁场偏差t为四元数，所述X、Y分量为四元数t的X、Y分量；

步骤4，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正陀螺零点，即对物体姿态数据e进行非线性互补滤波修正；其中所述磁强计数据包括修正后的所述体坐标系的磁场偏差Em；

e＝Em+Ea，e为物体姿态数据，Ea是加速度计偏差，Em为磁场偏差；

δ为陀螺零点偏移，

其中，公式中K_p、K_I、K_n为三个常数，需要根据陀螺仪、加速度计和磁强计的特性来确定，K_p，K_i是普通PID控制中的比例项和积分项，其中K_p与系统收敛速度有关，K_i确保系统稳态跟踪误差为零，K_n项加入了非线性调节方式；

步骤5，输出修正后的姿态数据。

2.根据权利要求1所述的基于惯性导航技术的姿态融合增强测量方法，其特征在于，所述数据初始化为四元数初始化，并初始化陀螺偏差。

3.根据权利要求1所述的基于惯性导航技术的姿态融合增强测量方法，其特征在于，所述捷联解算为四元数捷联解算。

4.根据权利要求1所述的基于惯性导航技术的姿态融合增强测量方法，其特征在于，修正各传感器对应的姿态数据包括加速度异常判断，磁异常判断。