基于地磁辅助惯性导航的状态反馈系统研究

唐志勇，艾文娟，刘岩

（1.长春理工大学　光电工程学院，长春　130022；2.长春理工大学　机电工程学院，长春　130022）

TANG Zhiyong1，AI Wenjuan2，LIU Yan2（1.School of Optoelectronic Engineering，Changchun University of Science and Technology，Changchun 130022；2.School of Mechatronical Engineering，Changchun University of Science and Technology，Changchun 130022）

基于地磁辅助惯性导航的状态反馈系统研究

唐志勇1，艾文娟2，刘岩2

（1.长春理工大学光电工程学院，长春130022；2.长春理工大学机电工程学院，长春130022）

针对MEMS技术的惯性传感器低精度的特点，提出一种高精度的组合导航算法，采用地磁辅助导航进行闭环校正，并对地磁的自差和航向角进行观测，以达到较高的惯性导航精度和更高的动态性能。实验证明，状态反馈的惯性导航系统能很好地克服惯导的误差累积，大幅度提高了地磁导航的抗干扰能力，得到较好的导航精度。

地磁导航；MEMS技术；组合滤波；自差校正

吊舱等侦察设备经常在敌后纵深、战场或水下深潜等条件下工作，工作环境比较复杂，要求具有成本低廉、体积小、抗冲击力强、可靠性高的稳定精确导航系统，微机械（MEMS）导航元器件［1］在小型飞行器如无人机侦查设备上有广阔的应用前景，但是微机械惯性导航元件存在导航精度低，时间漂移大等缺点，微机械磁力计检测的信号微弱，更容易受到载体磁化磁场或强磁的影响，从而大大降低导航的精度。

1　微机械导航元器件的误差模型

ADIS16405是基于MEMS技术的导航元器件，集成三轴陀螺仪、三轴加速计、三轴磁力计为一体，微机械元件的主要误差来源包括零偏误差［2］、随机漂移误差和噪声等，根据误差来源和相关误差的特性，可以建立误差模型。在本文的研究中，将误差等效成随机常数误差补偿、一阶马尔科夫过程和高斯白噪声。

微机械陀螺仪的误差模型如下：

其中，δω为陀螺仪的测量误差，nb为陀螺的随机误差常数，nr为一阶马尔科夫过程，nw为高斯白噪声，Tg为陀螺仪漂移的相关时间，wr为陀螺仪的漂移噪声。

微机械加速计的误差模型如下：

其中，∇为加速计测量误差，∇b为加速计随机误差常数，∇r为一阶马尔科夫过程，na为高斯白噪声，Ta为加速计漂移的相关时间，wa为加速计的漂移噪声。

微机械磁传感器的误差模型如下：

其中，δH为磁传感器［3］的测量误差，εb为磁力计随机误差常数，εr为一阶马尔科夫过程，εw为高斯白噪声，Th为磁传感器漂移的相关时间，wh为磁传感器的漂移噪声。

磁力计又称为磁罗经，广泛应用于船舶、航空航天等的导航系统中。现代的机体多采用钢铁架构，在受到地磁磁化后具有磁场，由于磁罗经早期应用于船舶的导航，所以地磁磁化产生的磁场称为船磁［4］。在船磁的作用下，罗经偏离磁北Nm所指的方向为罗北Nc，磁北和罗北之间的夹角称为磁罗经自差δ。自差的示意图如图1所示。

图1　磁罗经自差示意图

2　地磁辅助惯性组合导航的数学模型

2.1地磁辅助惯性导航的状态方程

状态方程的标准形式如下：

在当地地理坐标系下的导航方程如下：

其中：

可以根据导航方程推导出误差模型如下：

式中，Φ=［ψ，θ，φ］T分别表示俯仰角、横滚角、航向角［6］。εn为陀螺仪的漂移角速度误差，∇fb为加速计的测量误差。

其中，σ和θ分别是惯导元件和磁强计的白噪声漂移误差的均方差。

2.2地磁辅助惯性导航的观测方程

位置观测方程如下：

式中，LINS、λINS、hINS为惯性导航系统的输出位置信息，LGNS、λGNS、hGNS为地磁导航系统的输出位置信息，Hp为位置观测方程的观测矩阵，ΔVp为位置观测噪声。

由于地磁导航的磁罗经自差不能通过观测信息的组合导航滤波对ΔVp进行消除，所以组合导航的观测方程应包括地磁导航的磁罗经自差［7］δ，地磁导航的状态模型如下：

式中，X′、Y′、Z′为带误差的磁场强度在载体坐标系上的投影，X、Y、Z为真实磁场强度在载体坐标系上的投影，a、b、c、d、e、f、g、h、k分别为软铁系数，w为系统白噪声。

自差观测方程如下：式中，HB为地磁自差观测方程的观测矩阵，ΔVB为地磁自差观测噪声。

将X′，Y′，Z′旋转到水平坐标系，即东北天坐标系得：

其中θ是飞行器的横滚角，φ是飞行器的俯仰角。则系统的测量磁航向角φ可由下式计算：

航向角观测方程如下：

其中φINS，φGNS分别是惯性导航的航向角输出信息和地磁导航的航向角输出信息，Hφ是航向角观测方程的观测参数，ΔVφ是航向角观测噪声。

整理成标准观测模型的形式如下：

其中，H（t）是7×12维的状态观测矩阵，HP（t）是3× 12维的状态观测矩阵，HB（t）是3×12维的状态观测矩阵，Hφ（t）是1×12维的状态观测矩阵。观测噪声v（t）是随机噪声向量，它服从均值为零的高斯正态分布，可写作v（t）～N（0，R），R表示随机向量的方差。

3　组合导航模型的性能分析及优化算法

3.1组合导航的控制算法研究

经典卡尔曼滤波算法是根据观测向量、观测模型和系统模型来获取状态向量的最优估计，是一种被广泛应用的工程误差估计算法。但是经典卡尔曼滤波要求观测量是与时间无关的噪声，也就是白噪声［8］，而本文的自差和姿态等都是与时间相关的噪声，卡尔曼滤波器会把这类噪声变量归因于状态的改变，可能会影响误差的传递过程，破坏状态的估计。状态反馈可以配置极点、实现系统解耦控制、构成最优调节器、改善跟踪系统的稳态特性等特点，但是状态变量一般都是不易直接测量的，而且状态反馈虽然不会改变系统的可控性，但可能会影响系统的可观测性，所以可以采用状态观测器［9］，对不可观测的状态进行状态重构，并采用重构的状态代替系统的真实状态，进行有效的状态估计。同时考虑不到组合导航过程中存在很多干扰和噪声，因此提出了如下的消除误差和干扰的方法，即利用系统输出来估计系统状态，以实现状态反馈控制，带有观测器的组合导航反馈系统如图2所示。其中，∑0表示给定的系统状态空间，∑͂表示新的状态空间表达式，X̂表示状态空间的估计状态向量。设状态反馈的状态空间表达式为：

通过反馈控制器K可以使得系统内部稳定，而且达到一定的稳态精度，能够提高系统的鲁棒性［13］。

状态观测器采用卡尔曼滤波进行状态估计，采用降维观测器只是对位置、地磁和航向角进行状态估计。

图2　状态反馈示意图

首先研究系统的状态观测器是否存在，由于状态观测器存在的条件是系统状态完全可观，所以进行可观测性验证。线性时变系统的能控性判据为：

如果矩阵F（t）、H（t）满足n-1次连续可微的条件，在时间区间［t0，t1］内，又有：

则系统是完全能观测的。其中分块矩阵为：

由系统的状态方程和观测方程整理化简得F（t）、H（t）如下：

HP（t）可以参考文献［6］和文献［8］，HB（t）可由上文求解，Hφ（t）计算如下：，当用坐标转换矩阵和欧拉角描述一个小角度的扰动时，考虑到小角度近似：

计算可得到N0（t），N1（t），…，N11（t）线性无关，其秩为满秩，满足状态完全可观测，可以建立状态观测器，对相关状态进行估计。

3.2组合导航模型的反馈校正

采用组合导航滤波得到的误差估计，对惯导系统进行闭环校正［10］，也就是前面所说的带有状态观测器的反馈系统，当状态变量不便直接测量得到时，可以采用状态观测器获得状态向量X（t）的估计值（t），并用̂（t）代替X（t）完成惯导系统的闭环反馈任务。采用地磁惯性组合导航的闭环反馈系统结构图3所示。

图3　闭环反馈结构图

4　组合导航的性能仿真验证

4.1基于Matlab的组合导航仿真实验

组合导航的性能还需要进行验证，才能够判断算法的可行性和算法特性，所以该节进行仿真实验，通过标准传感器对ADIS16405元件的误差进行标定如表1所示。

表1　导航元件的标定误差

其它仿真参数：系统状态初值X0、状态估计均方误差矩阵初值P0、系统噪声均方误差初始矩阵Q0、系统观测噪声均方误差初始矩阵R0，设置如下：

进行仿真的条件如下：设置载体的初始位置为东经125°，北纬43°，并以140km/h的速度匀速航行，仿真的频率为20Hz。进行状态估计的仿真如图4至图7所示，分别为经典卡尔曼滤波和带状态反馈的优化卡尔曼滤波的仿真结果。

图4　组合导航的经度误差仿真

图5　组合导航的纬度误差仿真

图6　组合导航的高度误差仿真

图7　组合导航的航向角误差仿真

4.2优化校正后的组合导航性能验证

通过卡尔曼滤波对组合导航的误差值进行状态估计，比较初始状态和滤波后的状态，可以看出卡尔曼滤波可以很好的消除随机干扰，但是系统的响应速度较慢，稳定性较差，而且建立的数学模型常常采用简化的模型，存在较大的模型误差［11，12］，由仿真图像可以看出系统的跟随性能存在缺陷，位置和地磁的导航精度较高，但是航向角仍然存在较大的误差，经过前面的分析，可以采用H∞滤波进行系统的优化，得到更好的动态特性、鲁棒镇定和干扰抑制能力。

5　结论

由组合导航的状态观测反馈，通过对地磁惯性组合的状态向量进行状态估计，设计状态观测器，并采用估计状态代替实际的状态变量进行反馈［13］，能够有效的提高组合导航的精度，避免了直接采用卡尔曼滤波的各种复杂要求，能更加简单有效地实现组合导航的噪声和干扰的滤除，为导航提供了更加方便的控制方法。但是采用状态估计反馈，增加了状态的阶次，也就是提高了系统的维数，而且并不能改善系统的动态特性，反而，带观测器的状态反馈系统在鲁棒性上，要比真实状态的反馈系统更差。所以还有待进一步优化算法，或者进行系统状态反馈的鲁棒性镇定问题进行更深入的研究。

［1］ 张嗣灜，高立群.现代控制理论［M］.北京：清华大学出版社，2006，27-189.

［2］ 战德军，戴东凯，张忠华.单轴旋转INS/GPS组合导航中重力垂线偏差引起的姿态误差分析［J］.中国惯性技术学报，2014，2（11）：301-305.

［3］ 姚一飞，李增科，高井祥.GPS/INS紧耦合导航中多路径效应改正算法应用［J］.中国惯性技术学报，2014，5 （8）：782-787.

［4］ 王鹏，张迎春.一种小卫星高精度自主定轨/定姿一体化新方法［J］.中国惯性技术学报，2014，12（3）：741-747.

［5］ 杨晓东，王炜.地磁导航原理［M］.北京：国防工业出版社，2009.

［6］ 段本印.地磁辅助惯性导航系统技术研究［D］.哈尔滨：哈尔滨工业大学，2012.

［7］ 晏登洋.惯性地磁组合导航原理［D］.西安：西北工业大学，2007.

［8］ 伊程毅.基于地磁和微惯性器件组合姿态测量系统研究［D］.哈尔滨：哈尔滨工业大学，2013.

［9］ 丁君.基于微惯性传感器的姿态算法研究［D］.上海：上海交通大学，2013.

［10］ 董超，钱峰.微型姿态解算系统的抗磁干扰算法研究［J］.电子测量系统，2011，12（5）：59-62.

［11］ 杨宇彬，汤国建，包卫国.地磁姿态探测器的实时标定算法研究［J］.飞行力学，2014，19（7）：93-96.

［12］ 龙达峰，李杰，刘俊.地磁传感器误差参数估计与补偿方法［J］.传感技术学报，2014，9（10）：1649-1653.

［13］ 刘晓光，胡静涛，王鹤.基于自适应滤波的组合导航系统研究［J］.仪器仪表学报，2014，12（7）：1013-1020.

Based on the Geomagnetic-aided Inertial Navigation System of State Feedback

According to the characteristics of low precision of the inertial sensor of MEMS technology，a high precision of integrated navigation algorithm is put forward，adopt geomagnetic aided navigation with closed-loop correction，and the deviation of the magnetic and heading Angle of the observation is in order to achieve high precision of inertial navigation and a higher dynamic performance.Experiments show that state feedback of inertial navigation system can overcome the error accumulation of inertial navigation，greatly improve the anti-interference ability of the geomagnetic navigation，get better navigation accuracy.

geomagnetic navigation；MEMS technology；composite filtering；deviation correction

TP273

A

1672-9870（2015）06-0127-06

TANG Zhiyong1，AI Wenjuan2，LIU Yan2
（1.School of Optoelectronic Engineering，Changchun University of Science and Technology，Changchun 130022；2.School of Mechatronical Engineering，Changchun University of Science and Technology，Changchun 130022）

2015-09-07

唐志勇（1976-），男，硕士，讲师，E-mail：3551086@qq.com