CN107479076B - 一种动基座下联合滤波初始对准方法 - Google Patents

一种动基座下联合滤波初始对准方法 Download PDF

Info

Publication number
CN107479076B
CN107479076B CN201710668483.6A CN201710668483A CN107479076B CN 107479076 B CN107479076 B CN 107479076B CN 201710668483 A CN201710668483 A CN 201710668483A CN 107479076 B CN107479076 B CN 107479076B
Authority
CN
China
Prior art keywords
matrix
error
ukf
misalignment angle
model
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710668483.6A
Other languages
English (en)
Other versions
CN107479076A (zh
Inventor
牛尊
朱柏承
谭思远
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Peking University
Original Assignee
Peking University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Peking University filed Critical Peking University
Priority to CN201710668483.6A priority Critical patent/CN107479076B/zh
Publication of CN107479076A publication Critical patent/CN107479076A/zh
Application granted granted Critical
Publication of CN107479076B publication Critical patent/CN107479076B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Abstract

本发明公开了一种动基座下联合滤波初始对准方法。该方法在大失准角带来的非线性环境下通过UKF模块完成粗对准任务，而当失准角减小到门限值之后切换到KF模块继续进行精对准。本发明创新性地设定了判决门限以实现UKF和KF的切换，整体算法既能够比较精确地处理大失准角带来的非线性问题，又能够比较快速平稳地完成初始对准。此外，本发明引入了一种基于测量值的反馈算法，这种反馈算法能够避免UKF的估计值波动带来的问题，提高了UKF的收敛速度。

Description

一种动基座下联合滤波初始对准方法
技术领域
本发明属于卫星导航领域，具体涉及一种动基座下联合滤波初始对准方法。
背景技术
近些年来，一些导航应用要求捷联惯导系统能够在动基座情况下完成初始对准以实现快速部署。对于基于MEMS技术的加速度计和陀螺仪组成的低成本捷联惯导系统而言，动基座情况下进行初始对准能够有效地克服器件噪声对对准过程的影响，缩短对准时间，还有一些特殊应用场景，如旋转炮弹的惯导系统需要在炮弹点火从炮管中飞出后才开始工作的场景，初始对准只能在动基座下进行。
由于动基座下载体运动状态复杂，初始失准角的三个分量往往都很大，在这种情况下，捷联惯导系统的失准角误差微分方程、速度误差微分方程的非线性程度均很高，采用扩展卡尔曼滤波法(EKF)对姿态失准角进行估计效果有限。而无迹卡尔曼滤波法(UKF)能够对非线性问题有很好的处理效果并且状态更新迭代过程与线性卡尔曼滤波算法类似。但是，相比于线性KF，UKF需要耗费更多的计算量，并且UKF收敛速度较慢。另一方面，UKF的稳定性目前还缺乏理论证明，长时间进行可能会出现发散情况。鉴于这种情况，有必要提出一种能够综合无迹卡尔曼滤波(UKF)和线性卡尔曼滤波(KF)两种算法优点的方法，使系统既能够比较精确的处理非线性较强的问题，又能够比较快速平稳的工作。
发明内容
为了解决上述问题，本发明提出了一种动基座下联合滤波初始对准方法。所述方法包括以下步骤：
步骤S1：结合卫星导航系统和捷联惯导系统，使两个系统相互独立工作以提供相应参数；
步骤S2：利用修正的UKF对准算法进行粗对准，将大的失准角缩小到较小的范围；
步骤S3：衡量状态量的可信度，决定是否利用测量量进行反馈；
步骤S4：判断失准角是否已经缩小到足够小的范围，如果未缩小到足够小范围，返回步骤S1，如果已经缩小到足够小的范围，切换至线性KF算法进行小失准角情况下的初始对准；
步骤S5：利用线性KF算法进行精对准。
优选的，步骤S1中所述卫星导航系统和捷联惯导系统结合，惯性导航系统(SINS)采用四子样解算算法，即解算频率是惯性器件采样频率的四分之一。惯性导航系统(SINS)以较高频率进行解算更新，卫星导航系统(GNSS)以较低频率进行输出更新，UKF/KF联合滤波结构的更新频率由惯性导航系统(SINS)的解算频率决定，当惯导解算更新而卫星导航系统(GNSS)不可用时，对前后时刻的卫星导航系统(GNSS)输出结果做线性插值。
优选的，所述步骤S2中，联合滤波结构的UKF部分负责实现粗对准功能，将大的失准角快速地缩小到较小的范围。在这一过程中失准角比较大，系统的非线性较强，为了加快滤波收敛速度，只选择失准角φ＝[φE,φN,φU]T、速度误差δVn＝[δVE,δVN,δVU]T和位置误差
Figure BDA0001372453320000021
作为UKF估计的状态变量，选取陀螺仪零偏
Figure BDA0001372453320000022
加速度计零偏
Figure BDA0001372453320000023
作为过程噪声。将卫星导航系统(GNSS)测量的速度和位置与捷联惯导系统(SINS)解算出来的速度和位置分别作差，将所得到的的速度差
Figure BDA0001372453320000024
与所得的位置差
Figure BDA0001372453320000025
作为观测量，将卫星导航系统(GNSS)的定速误差
Figure BDA0001372453320000026
和定位误差
Figure BDA0001372453320000027
作为观测噪声。
可选的，所述步骤S2中，为了加快收敛的速度，可以在状态变量中引入地磁辅助信息。借助于地磁传感器，通过测量地磁场矢量，可以实现对载体姿态的估计，从而为初始对准过程中姿态的确定提供参考信息。由于地磁计的测量值会受到来自器件自身固有的误差和铁性磁体的干扰，加上地磁模型计算出来的地磁场矢量也存在偏差，因此在将根据地磁计算模型WMM-2015计算出来的地磁矢量
Figure BDA0001372453320000028
添加到观测量的同时，要在过程噪声中添加地磁测量误差
Figure BDA0001372453320000029
和地磁模型计算误差
Figure BDA00013724533200000210
优选的，所述步骤S3中，采用了基于测量值的反馈调节算法，能够加速UKF的收敛速度。每次UKF更新都会对状态协方差矩阵Pk进行更新，Pk的对角元素反映了状态变量的估值的波动大小，其中前三个对角元素反映了失准角的估值的波动大小，在每一次UKF迭代估计和更新过程中，所提出的反馈算法都要把Pk前三个对角元素中的最大值与设定的稳定水准门限进行比较，如果该最大值大于稳定水准门限，说明此时状态变量的估值波动较大，UKF不将估计值进行反馈，直接进行下一周期的时间更新与观测更新；如果该最大值小于稳定水准门限，则说明此时状态变量的估值变化比较平稳，将估计值进行反馈，然后将估计值置零，继续下一周期的时间更新与观测更新。
可选的，所述步骤S3中，当满足反馈的条件时，可以进行部分反馈，将状态变量估计值与一个不大于1的系数相乘，得到待反馈估值，将待反馈估值进行反馈，然后从估计值中减去待反馈估值，继续下一周期的时间更新与观测更新。
优选的，所述步骤S4中，每次UKF完成一个更新周期后，都将三个方向的失准角的最大值与设定的门限值进行比较，如果最大值小于设定的失准角门限值，则计数器加一，开始计数过程。在计数过程中，一旦某一个更新周期完成之后，三个方向的失准角的最大值比设定的失准角门限值大，则计数器清零，计数过程终止。直到接下来的某一个更新周期完成之后，三个方向的失准角最大值又小于设定的失准角门限值，计数重新开始。当计数器数值大于设定的计数门限值后，UKF阶段结束，粗对准任务完成，系统切换至KF阶段开始精对准，在切换过程中，系统把UKF切换前最后时刻的滤波参数尤其是失准角估值及其协方差矩阵参数、速度误差估值及其协方差矩阵参数传递并初始化KF的对应参数。
优选的，所述步骤S5中，精对准阶段，系统的线性性质较强，对噪声更加敏感，将加速度计零偏和陀螺仪零偏列入KF的状态变量。此时位置误差与姿态失准角解耦，不再将位置误差列入KF的状态变量，KF状态变量为
Figure BDA0001372453320000031
选取加速度计随机游走
Figure BDA0001372453320000032
陀螺仪随机游走
Figure BDA0001372453320000033
作为过程噪声。以姿态失准角存在情况下的捷联惯导系统(SINS)解算的速度
Figure BDA0001372453320000034
同卫星导航系统(GNSS)提供的速度
Figure BDA0001372453320000035
之间的差值
Figure BDA0001372453320000036
作为观测量，选取卫星导航系统(GNSS)的定速误差δVGNSS作为观测噪声。
可选的，所述步骤S5中，为了加快收敛的速度，可以在状态变量中引入地磁辅助信息。
借助于地磁传感器，通过测量地磁场矢量，可以实现对载体姿态的估计，从而为初始对准过程中姿态的确定提供参考信息。由于地磁计的测量值会受到来自器件自身固有的误差和铁性磁体的干扰，加上地磁模型计算出来的地磁场矢量也存在偏差，因此在将根据地磁计算模型WMM-2015计算出来的地磁矢量
Figure BDA0001372453320000037
添加到观测量的同时，要在过程噪声中添加地磁测量误差
Figure BDA0001372453320000041
和地磁模型计算误差
Figure BDA0001372453320000042
附图说明
图1显示了本发明优选实施例的UKF/KF联合滤波算法整体结构图；
图2显示了本发明优选实施例的卡尔曼滤波算法结构图；
图3显示了本发明优选实施例的UKF/KF联合滤波反馈算法流程图；
图4显示了本发明优选实施例的UKF/KF联合滤波切换算法流程图；
图5显示了本发明优选实施例中载体运动的轨迹仿真示意图；
图6显示了本发明优选实施例中UKF采取不同反馈机制下收敛速度比较；
图7显示了本发明优选实施例的UKF/KF联合滤波结构中状态估计量的变化情况；
图8显示了本发明优选实施例的UKF/KF联合滤波中真实失准角的变化情况。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了一些公知技术的描述，以避免不必要地混淆本发明的概念。
图1显示了本发明优选实施例的UKF/KF联合滤波算法整体结构图。
如图1所示，本发明提供了一种动基座下的UKF/KF联合滤波算法，该算法主要依托于两个系统：卫星导航系统(GNSS)和捷联惯性导航系统(SINS)。该算法结构包括惯导器件模块(101)，地磁测量模块(102)，卫导输出模块(103)，惯导更新计算模块(104)，反馈模块(105)，卡尔曼滤波模块(106)。
惯导器件模块(101)包括陀螺仪和加速度计，与载体固连，采集到的加速度和角速度都是在载体坐标系(b系)中表示的。一方面，惯导器件模块(101)的输出接入卡尔曼滤波模块；另一方面惯导器件模块(101)的输出与反馈模块(105)的输出融合后作为惯导更新计算模块(104)的输入。惯导更新计算模块(104)起到一个数学平台的作用，能够将载体坐标系(b系)中的加速度与角速度转到导航坐标系(n系)当中。地磁测量模块(102)和卫导输出模块(103)的输出都接入卡尔曼滤波模块(106)，它们与惯导器件模块(101)一起为卡尔曼滤波的建模提供了必要的参数。卡尔曼滤波模块(106)完成数据融合，进行状态量估计，它的每一个周期的输出接到反馈模块(105)，卡尔曼滤波模块(106)的输出在反馈模块(105)中与预先设定好的门限值比较，决定是否对惯导更新计算模块(104)的输入与输出进行反馈。反馈模块(105)的输出还会接入到卡尔曼滤波模块(106)以更新状态变量。
图2显示了本发明优选实施例的卡尔曼滤波算法结构图。
如图2所示，卡尔曼滤波模块(106)包括UKF更新模块(201)、切换算法模块(202)以及KF更新模块(203)。惯导、卫导以及地磁信号传进卡尔曼滤波模块(106)，反馈模块(105)也与卡尔曼滤波模块(106)相连，算法起始这些输入到卡尔曼滤波模块(106)的信号都是连接到UKF更新模块(201)的，卡尔曼滤波模块(106)的输出信号也由UKF更新模块(201)提供，每一个更新周期后，UKF更新模块(201)的输出会在切换算法模块(202)中与预先设定好的门限值比较，决定是否切换到KF更新模块(203)，如果切换到KF更新模块(203)，则把惯导、卫导、地磁信号以及反馈模块(105)的输出这些信号输入KF更新模块(203)，把KF更新模块(203)的输出信号提供给反馈模块(105)作为反馈模块(105)的输入信号。
卡尔曼滤波模块(106)的UKF更新模块(201)是为了完成粗对准，负责将大的失准角快速地缩小到较小的范围。在这个模块中，我们选择失准角φ＝[φE,φN,φU]T、速度误差δVn＝[δVE,δVN,δVU]T和位置误差
Figure BDA0001372453320000051
作为UKF估计的状态变量，选取陀螺仪零偏
Figure BDA0001372453320000052
加速度计零偏
Figure BDA0001372453320000053
作为过程噪声。UKF处理的是非线性问题，要对姿态误差角及其相关的状态变量进行建模，这些误差模型包括：失准角误差模型，速度误差模型，位置误差模型。现将三个模型建立如下：
(1)失准角误差模型
Figure BDA0001372453320000054
在上述微分方程中，
Figure BDA0001372453320000055
是计算出来的姿态矩阵，它与真实姿态矩阵
Figure BDA0001372453320000056
的差异反映了失准角的大小，失准角的信息蕴含在矩阵
Figure BDA0001372453320000057
当中。εb代表陀螺仪零偏，因为陀螺仪固连在载体上，所以陀螺仪的测量量和误差量都表示在载体坐标系(b系)中。角速率矢量
Figure BDA0001372453320000058
的物理意义是导航坐标系(n系)中角速率的大小，
Figure BDA0001372453320000061
的物理意义是导航坐标系中角速率矢量的误差。这两者的具体计算式如下：
Figure BDA0001372453320000062
Figure BDA0001372453320000063
在上述两个表达式中，V＝[VE,VN,VU]T代表了导航坐标系中的速度，RM代表了地球子午圈曲率半径，RN代表了地球的卯酉圈曲率半径，h代表载体相对地球表面的高度，
Figure BDA0001372453320000064
代表载体所处纬度，δV＝[δVE,δVN,δVU]T代表导航坐标系(n系)中的速度误差。在一个更新周期内，纬度误差的影响因素极小，为了减少计算量，在
Figure BDA0001372453320000065
的表达式中忽略了纬度误差的影响。
(2)速度误差模型
Figure BDA0001372453320000066
在上述微分方程中，
Figure BDA0001372453320000067
是计算出来的姿态矩阵，它与真实姿态矩阵
Figure BDA0001372453320000068
的差异反映了失准角的大小，失准角的信息蕴含在矩阵
Figure BDA0001372453320000069
当中。fb表示比力加速度，物理意义是除去重力加速度之外的加速度，可由加速度计测量得到，
Figure BDA00013724533200000610
表示加速度计零偏。由于加速度计与载体固连，所以加速度计的测量量和误差量都表示在载体坐标系(b系)当中。
Figure BDA00013724533200000611
表示地球自转角速度在导航坐标系(n系)中的表示，
Figure BDA00013724533200000612
表示地球自转角速度的误差。
Figure BDA00013724533200000613
表示n系相对于e系的角速度矢量，
Figure BDA00013724533200000614
表示该角速度矢量的误差。这几个量的具体计算方式如下：
Figure BDA00013724533200000615
Figure BDA0001372453320000071
在上述表达式中出现RM，RN，h，V，δV等物理量含义均与失准角模型中所述的物理量含义相同。
(3)位置误差模型
Figure BDA0001372453320000072
上式中，δV，
Figure BDA0001372453320000073
的物理意义与上述失准角误差模型、速度误差模型中述及一致。位置误差模型反映了位置误差与姿态失准角解耦。
(4)惯性测量器件误差模型
加速度计和陀螺仪的测量误差成分比较复杂，本发明中所采用的误差模型包括零偏和随机游走两部分，这样通过陀螺仪测量的角速率
Figure BDA0001372453320000074
和加速度计测量的比力加速度
Figure BDA0001372453320000075
可以分别表示为：
Figure BDA0001372453320000076
Figure BDA0001372453320000077
其中随机噪声部分，
Figure BDA0001372453320000078
与
Figure BDA0001372453320000079
建模为白噪声。
根据我们建立的各种模型可以建立起UKF的状态方程：
Figure BDA0001372453320000081
其中状态更新方程f(x,u)是关于状态x以及输入变量u的非线性函数，w为惯性敏感测量器件所引起的过程噪声。
选择卫星导航系统(GNSS)测量的速度和位置与捷联惯性导航系统(SINS)解算出来的速度和位置分别作差，将所得的速度差和位置差作为观测量，所得到的UKF观测方程如下：
Figure BDA0001372453320000082
其中
Figure BDA0001372453320000083
为卫星导航系统的速度测量误差。各分量的大小一般小于0.2m/s，
Figure BDA0001372453320000084
为卫星导航系统定位误差，各个分量的大小一般小于10m。如果考虑地磁辅助信息，按照地磁计算模型WMM-2015计算出地磁矢量，将其作为一个观测量，根据地磁测量值与状态变量的关系添加到原有的测量方程，得到地磁信息辅助的测量方程：
Figure BDA0001372453320000085
有了状态方程和测量方程之后根据标准UKF的步骤分别对UKF进行状态估计和状态更新。在UKF更新的过程中UKF模块(201)会不断与反馈模块(105)交换数据，同时切换算法模块(202)会不断检测是否需要切换到KF模块(203)，当失准角角度足够小的时候会切换到KF模块(203)进行精对准。
精对准阶段滤波结构切换至KF，KF需要在UKF进行粗对准的基础上将姿态失准角进一步缩小，鉴于加速度计和陀螺仪噪声的影响，在精对准过程中，将加速度计零偏和陀螺仪零偏列入KF的状态变量，由于位置误差与姿态失准角解耦，考虑KF运行时的效率，位置误差不再列入KF的状态变量。同时，精对准阶段失准角已经为小角度，本发明中切换算法模块(202)的存在使得精对准阶段失准角的三个分量都不大于5°，这个时候失准角微分方程可以进行线性简化，简化后的失准角微分方程为：
Figure BDA0001372453320000091
根据这一简化的姿态角微分方程，结合速度误差微分方程，选定状态变量为：
Figure BDA0001372453320000092
可以建立KF的状态方程：
Figure BDA0001372453320000093
上式中，
Figure BDA0001372453320000094
Figure BDA0001372453320000095
Figure BDA0001372453320000101
Figure BDA0001372453320000102
这个时候可以看到状态方程A(u,,,X)与状态变量X之间成线性关系，由于状态方程是连续时间函数微分形式，为了便于离散化处理，KF状态方程离散化形式可以表示为：
Xk＝Φk,k-1Xk-1+Gk-1wk-1
其中，状态转移矩阵
Φk,k-1＝eAΔt≈I+AΔt
Gk-1＝G(t)Δt,wk-1＝w
Δt为KF更新的时间周期。
以姿态失准角存在情况下的捷联惯导解算的速度
Figure BDA0001372453320000103
同卫星导航系统提供的速度
Figure BDA0001372453320000104
之间的差值
Figure BDA0001372453320000105
作为观测量，对应的观测方程为：
Figure BDA0001372453320000106
其中，δVGNSS为卫星导航系统的定速误差，如果考虑地磁辅助信息，按照地磁计算模型WMM-2015计算出地磁矢量，将其作为一个观测量，根据地磁测量值与状态变量的关系，添加到原有的测量方程，可以得到地磁信息辅助的测量方程：
Figure BDA0001372453320000111
为了加快UKF的收敛速度，本发明采用了基于测量值的反馈调节算法模块(105)，在大失准角情况下UKF的估计值在开始估计的一段时间内剧烈抖动，如果直接反馈会加速UKF估值的振荡，造成UKF无法收敛，因此在UKF较为稳定的时候再进行反馈非常关键。本发明选择每次UKF更新后的状态协方差矩阵Pk的前三个对角元素作为度量UKF估值程度的参考量，这三个对角元素反映了失准角估值的波动程度，在反馈模块(105)设定稳定水平门限值为(π/180)2，只有三个对角元素中的最大值小于该稳定水平门限值时UKF才将所估计的失准角和速度误差反馈，否则，UKF将不进行反馈，直接进入下一时刻的状态估计和更新。每进行一次UKF更新，切换模块(202)都会将估计的状态值与设定的门限值(本发明中设置为5°)比较，如果失准角都小于5°，则切换模块(202)中的计数器开始计数。计数过程中如果出现某次UKF更新后某一失准角大于5°的情况，计数器清零，计数过程终止，直到下一次失准角都小于5°时，计数再次开始。当计数值大于设定的门限值时，卡尔曼滤波模块(105)从UKF模块(201)切换到KF模块(203)。
图3显示了本发明优选实施例的UKF/KF反馈调节算法流程图。
图4显示了本发明优选实施例的UKF/KF滤波切换算法流程图。
本发明具体实施例中，具体仿真分为以下几个步骤：
第一步，利用PSINS工具箱生成轨迹对应的惯性测量器件(加速度计与陀螺仪)和地磁计的理想测量输出，并根据器件的误差模型及其相关参数计算测量参数，二者叠加，得到惯性测量器件在进行按设计的轨迹运动的输出结果。很显然，误差模型的精度决定了输出结果对实际输出的逼真程度。
图5显示了载体运动的轨迹仿真示意图。
仿真过程中，分别对战术级精度的惯性测量器件和基于MEMS技术的低精度惯性测量器件进行了性能验证。
第二步，根据设置的器件参数和轨迹参数，利用PSINS工具箱生成仿真的器件测量参数以及参考的姿态信息，将这些数据作为UKF/KF初始对准算法的输入，经人为地设置捷联惯导初始条件后运行算法。
第三步，验证所提出的的UKF/KF联合滤波结构中的修正UKF在粗对准时的收敛性能，利用图5所示的轨迹对应的惯导器件输出结果，但在联合滤波结构中的UKF部分采取不同的反馈机制：1无反馈机制；2直接反馈；3基于测量值的反馈。以基于MEMS技术的低精度惯性器件为例，并利用地磁信息辅助进行仿真。仿真中，初始失准角设置为φ＝[30°,60°,160°]T，一旦连续45次UKF估计的失准角误差都小于5°，仿真提前终止，设置的总仿真步数为44501，以此来衡量3种不同方法的收敛性能，突显本发明提出的基于测量值进行反馈的方法的优越性。
图6显示了UKF采取不同反馈机制下收敛速度比较。
从图6可以看出，采用直接反馈机制的姿态误差在整个仿真阶段的误差都很大，相比而言没有反馈机制的UKF以及采用基于观测量的反馈调节算法都能够在小于500次迭代次数内将姿态误差收敛到±5°以内。其中，基于观测量的反馈调节UKF能够在小于150次迭代次数内将姿态误差收敛到±5°以内，所需要的时间较传统的无反馈UKF而言减少了60％以上。
第四步，验证UKF/KF联合滤波结构对准精度性能，以战术精度的惯性器件为例，不添加地磁辅助信息，初始条件如上文所述。
图7显示了联合滤波结构中状态估计量的变化情况。
图7的局部图着重显示了UKF阶段估计量反馈的情况，每次反馈都将状态估计量置零。
图8显示了真实失准角的变化情况。
图8局部图着重显示了每经过一次反馈，失准角都会有阶跃下降。
仿真结果表明，相较于传统的UKF算法，本发明采用的基于测量值反馈的UKF算法收敛时间能够缩短60％，精对准阶段，本仿真实例选取精度为战术级别捷联惯导器件，在借助地磁信息辅助下能够在25s内将姿态失准角收敛到0.1°以内。
应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制，因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界或者这种范围和边界的等同形式内的全部变化和修改例。

Claims (6)

1.一种动基座下联合滤波初始对准方法，其特征在于，所述方法包括以下步骤：
步骤S1：将卫星导航系统和捷联惯导系统结合，使两个系统相互独立工作，为联合滤波提供必要参数；
步骤S2：选择失准角φ、速度误差δVn以及位置误差δR作为UKF估计的状态变量，选择加速度计的零偏
Figure FDA0002685264220000011
和陀螺仪的零偏εb作为过程噪声；将卫星导航系统测量的速度
Figure FDA0002685264220000012
和位置
Figure FDA0002685264220000013
与捷联惯导系统解算出来的速度
Figure FDA0002685264220000014
和位置
Figure FDA0002685264220000015
分别作差，将所得的速度差和位置差作为观测量，选择卫星导航系统的定速误差
Figure FDA0002685264220000016
定位误差
Figure FDA0002685264220000017
作为观测噪声，若考虑地磁辅助信息，将根据地磁计算模型计算出的地磁矢量
Figure FDA0002685264220000018
添加到观测量当中，将对应的地磁测量误差
Figure FDA0002685264220000019
和地磁模型计算误差
Figure FDA00026852642200000110
添加到观测噪声中；利用失准角模型、速度误差模型、位置误差模型、地磁模型和惯性器件测量模型构建状态方程和观测方程：
Figure FDA00026852642200000111
Figure FDA00026852642200000112
上述公式中各个矩阵的含义为：矩阵
Figure FDA00026852642200000113
为状态变量的微分矩阵，f(x,u)是一个非线性函数，反映了状态转移过程，矩阵G(t)是过程噪声转移矩阵，矩阵w代表过程噪声，矩阵h(x)是观测矩阵，矩阵
Figure FDA00026852642200000114
是观测量矩阵，矩阵c(x)为观测噪声转移矩阵，矩阵v代表观测噪声；
利用UKF进行粗对准，将大失准角缩小到门限值以下；
步骤S3：在每次UKF迭代估计和更新过程中，将UKF更新后的状态协方差矩阵中失准角所对应的三个协方差与设定的稳定水平门限值进行比较，只有当失准角所对应的三个协方差都小于设定的稳定水平门限值时，UKF才将所估计的失准角和速度误差进行反馈，当满足反馈的条件时，进行部分反馈，将状态变量估计值与一个不大于1的系数相乘，得到待反馈估值，将待反馈估值进行反馈；
步骤S4：判断失准角是否已经小于设定的门限值，如果未缩小设定的门限值以下，返回步骤S1，如果已经缩小到设定的门限值以下，切换至KF进行小失准角情况下的初始对准；
步骤S5：利用线性KF算法进行精对准。
2.根据权利要求1所述的动基座下联合滤波初始对准方法，其特征在于：步骤S1中所述的卫星导航系统和捷联惯导系统结合，采用卫星导航系统测量的速度和位置与捷联惯导系统依据导航更新算法计算的速度与位置分别作差，并将所得到的速度和位置差值作为联合滤波结构的观测量，为了提高观测方程的观测度，加快收敛的速度和精度，联合滤波结构引入地磁计测量信息作为观测量。
3.根据权利要求1所述的动基座下联合滤波初始对准方法，其特征在于：所述步骤S2中，为了加快滤波收敛速度，选择失准角φ、速度误差δVn以及位置误差δR作为UKF估计的状态变量，选择加速度计的零偏
Figure FDA0002685264220000021
和陀螺仪的零偏εb作为过程噪声；将卫星导航系统测量的速度
Figure FDA0002685264220000022
和位置
Figure FDA0002685264220000023
与捷联惯导系统解算出来的速度
Figure FDA0002685264220000024
和位置
Figure FDA0002685264220000025
分别作差，将所得的速度差和位置差作为观测量，选择卫星导航系统的定速误差
Figure FDA0002685264220000026
定位误差
Figure FDA0002685264220000027
作为观测噪声，若考虑地磁辅助信息，将根据地磁计算模型计算出的地磁矢量
Figure FDA0002685264220000028
添加到观测量当中，将对应的地磁测量误差
Figure FDA0002685264220000029
和地磁模型计算误差
Figure FDA00026852642200000210
添加到观测噪声中；依据失准角模型、速度误差模型、位置误差模型、地磁模型和惯性测量器件误差模型构建UKF状态方程与观测方程；这五个模型分别为：
(1)失准角模型
Figure FDA00026852642200000211
在失准角模型中，
Figure FDA00026852642200000212
是计算出来的姿态矩阵，它与真实姿态矩阵
Figure FDA00026852642200000213
的差异反映了失准角的大小，矩阵
Figure FDA00026852642200000214
代表失准角姿态矩阵，角速率矢量
Figure FDA00026852642200000215
的物理意义是导航坐标系中角速率的大小，
Figure FDA00026852642200000216
的物理意义是导航坐标系中角速率矢量的误差；
(2)速度误差模型
Figure FDA0002685264220000031
在速度误差模型中，fb表示比力加速度，物理意义是除去重力加速度之外的加速度，
Figure FDA0002685264220000032
表示地球自转角速度在导航坐标系中的表示，
Figure FDA0002685264220000033
表示地球自转角速度的误差，
Figure FDA0002685264220000034
表示导航坐标系相对于e系的角速度矢量，
Figure FDA0002685264220000035
表示该角速度矢量的误差；
(3)位置误差模型
Figure FDA0002685264220000036
上式中，各物理量的物理意义与上述失准角模型、速度误差模型中述及一致，位置误差模型反映了位置误差与姿态失准角解耦；
(4)地磁模型
Figure FDA0002685264220000037
在地磁模型中，
Figure FDA0002685264220000038
代表地磁计测量量，
Figure FDA0002685264220000039
和
Figure FDA00026852642200000310
都是姿态变换矩阵；
(5)惯性测量器件误差模型
误差模型包括零偏和随机游走两部分，这样通过陀螺仪测量的角速率
Figure FDA00026852642200000311
和加速度计测量的比力加速度
Figure FDA00026852642200000312
分别表示为：
Figure FDA00026852642200000313
Figure FDA00026852642200000314
其中随机噪声部分，
Figure FDA0002685264220000041
与
Figure FDA0002685264220000042
建模为白噪声；
这一步骤中，最终建立的UKF状态方程和观测方程分别为：
Figure FDA0002685264220000043
Figure FDA0002685264220000044
如果考虑地磁辅助信息，则观测方程变为：
Figure FDA0002685264220000045
上述公式中各个矩阵的含义为：矩阵
Figure FDA0002685264220000046
为状态变量的微分矩阵，f(x,u)是一个非线性函数，反映了状态转移过程，矩阵G(t)是过程噪声转移矩阵，矩阵w代表过程噪声，矩阵h(x)是观测矩阵，矩阵
Figure FDA0002685264220000047
是观测量矩阵，矩阵c(x)为观测噪声转移矩阵，矩阵v代表观测噪声。
4.根据权利要求1所述的动基座下联合滤波初始对准方法，其特征在于：所述步骤S3中，由于在大失准角的情况下UKF的估计值在刚开始估计的一段时间内会剧烈波动，在这个时候将UKF所估计的值直接反馈会加速UKF估值的振荡，造成UKF无法收敛，因此选择UKF更新后的状态协方差矩阵中失准角所对应的三个协方差作为度量UKF估值可信度的参考量，在每次UKF迭代估计和更新过程中，所提出的反馈算法都要将这一参考量与设定的稳定水平门限值进行比较，只有当失准角所对应的三个协方差都小于设定的稳定水平门限值时，UKF才将所估计的失准角和速度误差进行反馈，否则不进行反馈，直接进入下一时刻的状态估计与更新，当满足反馈的条件时，进行部分反馈，将状态变量估计值与一个不大于1的系数相乘，得到待反馈估值，将待反馈估值进行反馈。
5.根据权利要求1所述的动基座下联合滤波初始对准方法，其特征在于：所述步骤S4中，切换算法决定联合结构是否从UKF切换到KF，并确保切换过程中联合滤波结构作为一个整体对捷联惯导系统失准角的估计保持平稳，本步骤依赖于一个满足一定条件下的UKF连续进行状态估计和更新次数的计数值，满足的条件是：对失准角的估计值都小于一个设定的门限值；若在计数过程中出现所估计的失准角大于设定的门限值的情况，则计数值清零，当计数值大于M次后，M的范围为10-50，滤波结构从UKF切换到KF，把切换前最后时刻的滤波参数用来初始化KF的对应参数。
6.根据权利要求1所述的动基座下联合滤波初始对准方法，其特征在于：所述步骤S5中，在UKF进行粗对准的基础上将失准角进一步缩小，选取失准角φ、速度误差δV、陀螺仪零偏εb和加速度计零偏
Figure FDA0002685264220000051
作为KF的状态变量，选取陀螺仪的随机游走
Figure FDA0002685264220000052
和加速度计的随机游走
Figure FDA0002685264220000053
作为过程噪声，将卫星导航系统测量的速度
Figure FDA0002685264220000054
与捷联惯导系统解算出来的速度
Figure FDA0002685264220000055
分别作差，将所得的速度差作为观测量，选择卫星导航系统的定速误差
Figure FDA0002685264220000056
作为观测噪声，若考虑地磁辅助信息，将根据地磁计算模型计算出的地磁矢量
Figure FDA0002685264220000057
添加到观测量当中，将对应的地磁测量误差
Figure FDA0002685264220000058
和地磁模型计算误差
Figure FDA0002685264220000059
添加到观测噪声中，依据简化的失准角模型和速度误差模型构建KF状态方程与观测方程，这两个模型分别为：
(1)简化的失准角模型
Figure FDA00026852642200000510
在失准角模型中，
Figure FDA00026852642200000511
是计算出来的姿态矩阵，它与真实姿态矩阵
Figure FDA00026852642200000512
的差异反映了失准角的大小，矩角速率矢量
Figure FDA00026852642200000513
的物理意义是导航坐标系中角速率的大小，
Figure FDA00026852642200000514
的物理意义是导航坐标系中角速率矢量的误差；
(2)速度误差模型
Figure FDA00026852642200000515
在速度误差模型中，fb表示比力加速度，物理意义是除去重力加速度之外的加速度，
Figure FDA0002685264220000061
表示地球自转角速度在导航坐标系中的表示，
Figure FDA0002685264220000062
表示地球自转角速度的误差，
Figure FDA0002685264220000063
表示导航坐标系相对于e系的角速度矢量，
Figure FDA0002685264220000064
表示该角速度矢量的误差；
这一步骤中，最终建立的KF状态方程和观测方程分别为：
Figure FDA0002685264220000065
Figure FDA0002685264220000066
如果考虑地磁辅助信息，则观测方程变为：
Figure FDA0002685264220000067
上述公式中各个矩阵的含义为：矩阵
Figure FDA0002685264220000068
为状态变量的微分矩阵，矩阵A(x,u)是状态更新矩阵，矩阵
Figure FDA0002685264220000069
是观测量矩阵，矩阵X为状态变量矩阵，矩阵G(t)是过程噪声转移矩阵，矩阵w代表过程噪声，矩阵H是观测矩阵，矩阵Θ是观测噪声,C代表过程噪声转移矩阵；
在状态方程中，4个矩阵块的具体表示为：
Figure FDA0002685264220000071
Figure FDA0002685264220000072
Figure FDA0002685264220000073
Figure FDA0002685264220000074
在上述4个矩阵块中，VE,VN,VU是载体在导航坐标系东北天三个方向的速度分量；RM代表了地球子午圈曲率半径，RN代表了地球的卯酉圈曲率半径，h代表载体相对地球表面的高度，
Figure FDA0002685264220000075
代表载体所处纬度，
Figure FDA0002685264220000076
是导航坐标系东北天三个方向的比力分量。
CN201710668483.6A 2017-08-08 2017-08-08 一种动基座下联合滤波初始对准方法 Active CN107479076B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710668483.6A CN107479076B (zh) 2017-08-08 2017-08-08 一种动基座下联合滤波初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710668483.6A CN107479076B (zh) 2017-08-08 2017-08-08 一种动基座下联合滤波初始对准方法

Publications (2)

Publication Number Publication Date
CN107479076A CN107479076A (zh) 2017-12-15
CN107479076B true CN107479076B (zh) 2020-11-03

Family

ID=60598937

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710668483.6A Active CN107479076B (zh) 2017-08-08 2017-08-08 一种动基座下联合滤波初始对准方法

Country Status (1)

Country Link
CN (1) CN107479076B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108645416B (zh) * 2018-03-30 2020-09-18 北京空间飞行器总体设计部 基于视觉测量系统的非合作目标相对导航仿真验证方法
CN109000682B (zh) * 2018-07-27 2020-03-17 中科宇达（北京）科技有限公司 基于直线航迹的空中粗对准的方法
CN108981751A (zh) * 2018-08-16 2018-12-11 昆山天地睿航智能科技有限公司 一种双轴旋转惯导系统的八位置在线自标定方法
CN111486865A (zh) * 2019-01-29 2020-08-04 北京理工大学 传递对准滤波器、传递对准方法及应用其的制导飞行器
CN110398257B (zh) * 2019-07-17 2022-08-02 哈尔滨工程大学 Gps辅助的sins系统快速动基座初始对准方法
CN110873577B (zh) * 2019-12-02 2022-05-17 中国人民解放军战略支援部队信息工程大学 一种水下快速动基座对准方法及装置
CN114396938B (zh) * 2021-12-07 2023-08-08 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种舰船捷联惯导系统的高精度初始对准方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519460B (zh) * 2011-12-09 2014-11-05 东南大学 一种捷联惯性导航系统非线性对准方法
CN103575298A (zh) * 2013-11-14 2014-02-12 哈尔滨工程大学 基于自调节的ukf失准角初始对准方法
CN104330092B (zh) * 2014-07-24 2017-08-04 南京理工大学 一种基于双模型切换的二次传递对准方法
CN104374401A (zh) * 2014-10-15 2015-02-25 哈尔滨工程大学 一种捷联惯导初始对准中重力扰动的补偿方法
CN106840194B (zh) * 2016-09-20 2019-09-27 南京喂啊游通信科技有限公司 一种大方位失准角线性对准方法

Also Published As

Publication number Publication date
CN107479076A (zh) 2017-12-15

Similar Documents

Publication Publication Date Title
CN107479076B (zh) 一种动基座下联合滤波初始对准方法
CN107690567B (zh) 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法
CN110398257A (zh) Gps辅助的sins系统快速动基座初始对准方法
CN101949710B (zh) Gnss辅助mems惯性传感器零偏的快速在线动态标定方法
CN105698792A (zh) 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统
CN108761512A (zh) 一种弹载bds/sins深组合自适应ckf滤波方法
CN107063245B (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
Ali et al. Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN104280022A (zh) 视觉辅助惯性测量单元的数字头盔显示设备跟踪系统
CN112857398B (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN104075713A (zh) 一种惯性/天文组合导航方法
CN110285815A (zh) 一种可在轨全程应用的微纳卫星多源信息姿态确定方法
CN108508463B (zh) 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法
CN105988129A (zh) 一种基于标量估计算法的ins/gnss组合导航方法
Sun et al. A robust indirect Kalman filter based on the gradient descent algorithm for attitude estimation during dynamic conditions
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
Lacambre et al. Limitations of the unscented Kalman filter for the attitude determination on an inertial navigation system
CN102221366B (zh) 一种基于模糊变地球自转角速度的快速精对准方法
CN112649022B (zh) 一种考虑挠曲变形和杆臂效应的大失准角传递对准方法
CN106092141B (zh) 一种改善相对位置传感器性能的方法及装置
Gul et al. GPS/SINS navigation data fusion using quaternion model and unscented Kalman filter
Girrbach et al. Towards in-field and online calibration of inertial navigation systems using moving horizon estimation
Elsabbagh et al. Precise Orientation Estimation Based on Nonlinear Modeling and Quaternion Transformations for Low Cost Navigation Systems
Yi et al. Nonlinear Bayesian filter: Alternative to the extended Kalman filter in the GPS/INS fusion systems

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant