一种基于非线性滤波的电控罗经初始对准方法

专利名称：一种基于非线性滤波的电控罗经初始对准方法
技术领域：
本发明涉及电控罗经、平台罗经、惯性导航系统等设备(以下统称"陀 螺导航设备")中的初始对准等技术问题，具体是指一种基于非线性滤波 的电控罗经初始对准方法。
背景技术：
陀螺导航设备在启动进入正常工作前，首先处于准备和初始对准状
态。初始对准是建立导航装备所必须的初始条件，如初始位置、初始速度、
平台的初始姿态角误差等。舰艇的速度和位置是经过积分计算得到的，这 就要给出初始条件^、 &。、 &、 A。因为在码头启动，初始K。、 ^。为
零，％、 A。可以有已知测量点给出，或者用无线电定位仪等给出，但精度
要满足要求应在几十米之内。初始对准可以利用陀螺导航设备本身惯性元
件敏感失调角信号对系统进行校准，设计一个合理的初始对准方案，尽量
减小误差满足使用要求是十分必要的。对准精度和对准的时间是进行初始
对准时的两项重要技术指标。初始对准精度影响惯导系统的性能，初始对
准时间标志着快速反应能力，因此要求初始对准精度高、对准时间短，即
精而快。为了达到这一要求，要有符合实际系统的初始对准误差模型和相
应的初始对准滤波估计方法。
实际系统总是存在不同程度的非线性，有些系统可以近似看成线性系 统，但大多数系统则不能仅用线性微分方程描述，其中的非线性因数不能 忽略，或为了更好地分析综合结果，必须应用反映实际系统的非线性数学 模型，对随机非线性系统滤波问题应运而生。实际陀螺导航设备中不可避免地存在大量的随机噪声，频域内的高 通、低通、带通等传统滤波方法对此无能为力。后来学者提出了通过计算 功率谱来消除随机噪声的维纳滤波方法。虽然维纳滤波方法对随机白噪声
有效，但是该方法的计算量大，对稍复杂的系统无法实时适用。1960年， 卡尔曼提出了以状态空间法和矩阵运算为基础的卡尔曼滤波算法，它为时 域递推最优滤波提供了强有力的工具，它采用时域递推算法，不须存储时 间过程中的量测量，因而卡尔曼滤波在导航、通讯、遥感等各个领域得到 广泛应用。但是卡尔曼滤波只能处理线性系统。为解决工程实际中的非线 性问题，后来学者又提出了改进的卡尔曼滤波算法EKF(Extended Kalman Filter扩展卡尔曼滤波)，但还是存在不足。到目前为止，虽然对EKF 有众多的改进方法，如高阶截断EKF，迭代EKF等。但是，EKF的线性化 误差降低了模型的准确性，随时间的延长，精度难以保证，有时甚至发散， 这些缺陷仍然难以克服，所以各国导航界专家一直在努力研究新的更有效 的非线性降噪处理方法。为此，利用基于统计非线性变换的UKF方法，为 解决导航领域非线性问题开辟新的技术途径。

发明内容
本发明的目的是为了克服上述背景的不足，提出一种基于非线性滤波 的电控罗经初始对准方法，以达到提高初始对准精度和快速对准时间的实 际操作效果。
针对上述目的，本发明所提出的方法依据电控罗经中陀螺和加速度的 特性，建立电控罗经初始对准的UKF非线性误差模型，有效地得到系统随 机噪声的最优估计，保证补偿的精度，縮短启动时间，其具体方法是
1)电控罗经非线性初始对准模型的建立电控罗经采用固定指北式惯导机械编排。完整的误差模型，要考虑陀 螺的误差模型和加速度误差模型。陀螺误差包含有三个轴上的刻度因子误 差、三个轴上的未对准误差；同样加速度计误差中含有三个轴上的刻度因 子误差和三个失准误差。惯性导航系统误差一般有3个平台失准角 (a，P，Y); 3个速度误差(SVx, S Vy. S Vz);经度、纬度、高度3个位 置误差；考虑陀螺、加速度计诸误差模型因子可达21维状态变量。21维 误差状态对于电控罗经来说，在计算速度和复杂度上都不允许。发明人经 过降维处理，由21维降到15维，这样滤波计算时间可减小40%以上，而 精度性能却相当。所以，有必要对电控罗经的误差模型进行简化处理。因 电控罗经是船用设备，垂直向的加速度、速度、高度都可以忽略，只需计 算东向和北向加速度误差，三维的速度及位置也简化成二维平面。初始对 准时间不长，陀螺误差和东北向加速度都简化为随机的白噪声。据上述， 电控罗经的误差状态定义为一个5维的向量X4"p^《《f 。根据电控
罗经屮机械结构编排，可得到系统对准的《、 P、 7、 SK 、《^非线性 误差模型
d = —M^sin/cos^ + sin ^ - 5「r + sr +
》=(1 一 cos"h^cos^ - "vi^sin0 + 3已/《，++ 5
(1)
'= -(cosy - + ",in / - g(y cos, + a sin 7) + 2x^,气sin * + + #v = - sin m + (cosy -1)", + g(-/ sin;K + "cos" _ 2w J「x sin ^ + V义+ w脚
将上式改写为如下形式
X = g(X ) + Sco (2) 式中X=[a ",化W=[A ， w 1 M^f是陀螺和加速度计的 随机白噪声；5 = /,x5;其中A是地球半径； 是地球转动角速度；"、A y是平台误差角在导航坐标系三个轴上的投影；、 ^;分别是纬度和东北 向速度误差；& 、 、^是陀螺常值误差在当地地理坐标系o砂z中的投 影，▽-、 V^是水平加速度计常值偏置。
g(-)是非线性的，对其进行离散化，设采样间隔为A"将Z(, + A0在 I(O处进行泰勒展开，得到
K> + A/) = + g(jc)A ++... + 50(0 (3 )
5x: 2
令Z(f + A0:x^， l(0 = xt， 《(0 = %。根据精度要求，忽略展开级 数的二阶及二阶以上的小量，得到系统的离散方程为
、+1=、+水)& + ^ (4) 式(4)为电控罗经非线性初始对准模型。 2)电控罗经的UKF滤波初始对准 *状态方程
电控罗经的UKF滤波采用间接输出校正滤波器结构，其结构图如图4。
电控罗经中采用的UKF最优估计状态方程为前面提到的电控罗经的降阶 误差模型，见公式(4)。
误差观测方程
由于地磁场极易受到周围铁磁物对地磁场干扰，从而磁传感器会产生 较大误差，所以精度不高；而磁传感器的地磁观测是也是一种自主观测， 对外信息依赖少，同时也不需要进行对外进行任何通信交互，因而近年来 在导航系统的组合中也得到广泛应用。在所讨论的罗经内，我们加装了与 磁传感器测量装置，能观测到的量有航向和纵横摇角，磁传感器的体积小， 只要安装时尽可能与SINS靠近，从而磁传感器与S頂S间的臂杆效应很小，相对于磁传感器较大的观测误差，可以忽略。
设磁传感器输出的姿态分别为纵摇《.,,、横摇4、罗经输出 的姿态角为/^、.,/V.,/^、.。于是可近似认为得到姿态误差角的观测为
对式(5)进行变形，并令Z^[《,，Pt/, 有
(5)<formula>formula see original document page 10</formula>(6)
其中l,为磁传感器的误差观测噪声，对准的短时间内可认为是零均
c〃
值白噪声。将测量方程简记为
其中、为观测噪声。
*离散递推的UKF滤波算法 针对上面所建立的电控罗经的误差方禾i
定义的系统，改写为下式
<formula>formula see original document page 10</formula>(7)
噪声方程式(4)和(7)
(8)
<formula>formula see original document page 10</formula>在UKF滤波中，由于具有噪声项，需要对状态进行扩维处理,令 x"=[ ' wT VTf'，具体UKF滤波流程如下 状态初始条件为x0=￡(x0)7, ;jfMUf)t (;T)t UT)T]T,式(U)中的f取值表sigma点采 样方法，即UT变换的方法。其中加权系数『,的计算式为
『"二= /i /(丄+ ;i)
『义/(丄+ ;i) + (i — r2 + / )
『'",.=『，=1 /{2(丄+义)}
(14)
其中《=^7^万，r是一个小正数(通常取为l〉r 2leH)，决定了
l一 l一sigma点在i周围的散布。；<formula>formula see original document page 12</formula>是一个标度因子。A也是一个 标度因子，常取为0或3-丄，/7是对针状态向量x的分布所加的一个参量， / 取，(V ^),是矩阵平方根的第/列。 本发明的优越性在于
电控罗经初始对准试验结果表明，利用系统推导的误差模型和观测方 程进行UKF滤波初始对准后，在达到同样精度要求时，方位对准的时间由 原来的2879s縮短到200s以内；同时纵、横摇的失准角也在同样的精度 下初始对准时间分别上原来的2400s和2700s縮短到220s和180s;另外， 系统的速度随加速度随机误差的积累发散也大大改善，从图5可以看到在 1500s内，电控罗经自对准速度发散到14m/s,而采用UKF滤波初始对准后， 速度只发散到4m/s左右。
分析其初始对准测试试验的a、 A ^和速度(东北向速度矢量和)曲 线，可以得到以下结论
(1) 电控罗经系统推导的初始对准非线性误差模型及UKF组合初始对 准算法，能有效解决初始对准中大失准角存在的非线性，从而縮短初始对 准时间，使整个系统性能得到提高；
(2) 电控罗经采用UKF初始对准技术，能有效抑制常规静态下初始对 准的速度的发散现象；
电控罗经采用的初始对准非线性误差模型，UKF组合初始对准算法对 于其它系统中存在的大非线性问题及导航系统的导航状态误差的最优估 计等其它工程实际应用具有重要借鉴意义。


图1为基于UKF的电控罗经组合滤波对准结构框2为具体实施方式
中自初始对准与UKF组合初始对准的;K比较 图3为具体实施方式
中自初始对准与UKF组合初始对准的/ 比较 图4为具体实施方式
中自初始对准与UKF组合初始对准的a比较 图5为具体实施方式
中自初始对准与UKF组合初始对准的速度比较
具体实施例方式
下面结合附图详细说明本发明的实施情况，但它们并不构成对本发明 专利的限定，仅作举例而已。同时通过说明本发明的优点将变得更加清楚 和容易理解。
具体实施例方式
根据电控罗经的状态方程和观测方程，再据UKF的离散递推式(9) _ (12)，系统进行初始对准，其实现的方案流程见附图1。系统初始对准 试验中，初值选取/ ,《W/=[-U U -25， 0.2，0.2f ，角度单位为° ， 速度单位为:m/s;试验在重庆某工厂进行，其纬度为30°。由电控罗经的 误差统计特性，根据《卡尔曼滤波与组合导航原理》书中介绍的方法计算 尸。，再分别取各状态噪声方差强度为初值的5%。给定初值f。和尸。，电控 罗经利用(:/0++在控制软件中对前述误差模型和UKF进行实现，r取值 0.5， /7取2，滤波周期是O. ls，对电控罗经和磁传感器的采样率分别是 20Hz和10Hz。
电控罗经系统进行的静态初始对准试验中，系统的安装方位基准由经 讳仪光学对准保证对准北向。对系统进行独立自对准和UKF组合滤波对准 进行比较，可得到附图2、 3、 4、 5的结果。试验测试中，常值安装误差 由光学测量方法测出后被修正掉。系统的自对准方位失准角Z的测试结果 和电控罗经的UKF组合滤波初始对准中，方位失准角;K测试结果见图2。
权利要求
1. 一种基于非线性滤波的电控罗经初始对准方法，其特征在于包括以下步骤1)电控罗经非线性初始对准模型的建立；2)电控罗经的UKF滤波初始对准。
2. 根据权利要求1所述的一种基于非线性滤波的电控罗经初始对准 方法，其特征是所述的电控罗经非线性误差模型的建立方法如下电控罗经采用固定指北式惯导机械编排，得到包括陀螺误差、加速度 计误差、平台失准角、速度误差、位置误差诸因子的21维状态变量的误差模型，然后进行降维处理至15维，取"、-、 ^、 、 ^p;作为误差状态变量，得到系统对准的非线性误差模型如下& 二 —sin ycos^ + / h^ sin ^ -3& /乂' + ^ + h\ 》=(1 一 cos")m^ cos^ — an^ sin^ + / i t' ++ w少 ,=(-^sin 7 + " cos"m^' cos^ + tan0(5f^ / & + sz + wz (1) 〗3& = -(cos, — 1) + a少sin y — g(/ cos" + a sin" + 2m^(5F^ sin ^ + V_y + = — sin ;j/a^ + (cosy - + g(-y5sin ^ + "cos" 一 2n^(5f^ sin ^ + Vx +将上式改写为如下形式(2)式中X = [a - r < <5K/， W=[W, Wy w: Mfe 、/是陀螺和加速度计的 随机白噪声；3 = /5><5;其中^是地球半径；《,,是地球转动角速度；"、/ 、 y 是平台误差角在导航坐标系三个轴上的投影；、 ^;分别是纬度和东北 向速度误差；& 、 s、 ^是陀螺常值误差在当地地理坐标系o砂z中的投 影，VY、 Vy是水平加速度计常值偏置；g(-)是非线性的，对其进行离散化，设采样间隔为A"将义(/ + ^)在1(0 处进行泰勒展开，得到<formula>formula see original document page 3</formula>(3) 令1(^ + &) = ^+1， X(/) = ^， 0(0 = %。根据精度要求，忽略展开级 数的二阶及二阶以上的小量，得到系统的离散方程为式(4)为电控罗经的误差模型。
3、根据权利要求1或2所述的一种基于非线性滤波的电控罗经初始对准方法，其特征是所述的建立UKF非线性滤波状态方程和量测方程的步骤如下 1)建立UKF滤波状态方程电控罗经UKF滤波采用输出校正滤波器结构，状态方程为电控罗经的 非线性误差模型方程(4); 2)建立UKF滤波量测方程设磁传感器输出的姿态分别为纵摇/^.、横摇々、罗经输出的姿态角为WW,,PW.S.,//W、.。于是可近似认为得到姿态误差角的观测为<formula>formula see original document page 3</formula>对式(5)进行变形，并令2 = [^, &//,/，有(5)<formula>formula see original document page 3</formula>其中^,为磁传感器的误差观测噪声，对准的短时间内可认为是零均 值白噪声；将测量方程简记为^"(A) + 、 (7)其中、为观测噪声。
4.根据权利要求1或2或3所述的一种基于非线性滤波的电控罗经初始对准方法，其特征是所述的离散递推的UKF滤波算法步骤如下按电控罗经的误差模型方程和观测噪声方程式(4)和(7)定义的系统，改写为下式<formula>formula see original document page 4</formula>在UKF滤波中，由于具有噪声项，需要对状态进行扩维处理，令 x、[x' wT ,f'，具体UKF滤波流程如下状态初始条件为<formula>formula see original document page 4</formula>对于"{1,2,.,..oo}，计算重新构建的sigma点 UKF的预测方程是<formula>formula see original document page 5</formula>z岸一i =力(5C维-i, 5C単-i)<formula>formula see original document page 5</formula>UKF的更新方程为:<formula>formula see original document page 5</formula>(13)其中x、[x' wT vT]7, ;r=[(;r)T (r)T COT式(11)中的f取值表sigma点采 样方法，即UT变换的方法；其中加权系数『,的计算式为<formula>formula see original document page 5</formula>(14)其中《=^/^7万，r是一个小正数(通常取为l〉r 2le—"，决定了 sigma点在i周围的散布。/1 = (丄+ "-1是一个标度因子。zt也是一个 标度因子，常取为0或3-丄，T7是对针状态向量x的分布所加的一个参量，;/取2， (V ^),是矩阵平方根的第/列。
全文摘要
本发明提供了一种基于非线性滤波的电控罗经初始对准方法，本发明涉及电控罗经初始对准等技术问题，本发明所提出的方法依据电控罗经中陀螺和加速度的特性，建立电控罗经初始对准的UKF非线性误差模型，其具体方法是电控罗经非线性初始对准模型的建立；电控罗经的UKF滤波初始对准，本发明电控罗经的UKF滤波采用间接输出校正滤波器结构，电控罗经中采用的UKF最优估计状态方程为x_k+1＝x_k+g(x_k)Δt+Bω_k，观测噪声方程为z_k＝h(x_k)+v_k，本发明能有效解决初始对准中大失准角存在的非线性，从而缩短初始对准时间，使整个系统性能得到提高。
文档编号G01C21/00GK101545778SQ20081023687
公开日2009年9月30日 申请日期2008年12月17日 优先权日2008年12月17日
发明者强 刘, 徐金华, 许江宁 申请人:中国人民解放军海军工程大学