CN108303120B - 一种机载分布式pos的实时传递对准的方法及装置 - Google Patents

一种机载分布式pos的实时传递对准的方法及装置 Download PDF

Info

Publication number
CN108303120B
CN108303120B CN201810153926.2A CN201810153926A CN108303120B CN 108303120 B CN108303120 B CN 108303120B CN 201810153926 A CN201810153926 A CN 201810153926A CN 108303120 B CN108303120 B CN 108303120B
Authority
CN
China
Prior art keywords
subsystem
error
equation
angle
time
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.)
Expired - Fee Related
Application number
CN201810153926.2A
Other languages
English (en)
Other versions
CN108303120A (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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN201810153926.2A priority Critical patent/CN108303120B/zh
Publication of CN108303120A publication Critical patent/CN108303120A/zh
Application granted granted Critical
Publication of CN108303120B publication Critical patent/CN108303120B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明实施例提供了一种机载分布式POS实时传递对准的方法和装置，建立了大失准角条件的传递对准数学模型和误差模型，利用KF和UKF分别进行线性状态变量和非线性状态变量的时间更新和量测更新，然后利用线性状态变量估计值和非线性状态变量估计值对子系统的速度、位置和姿态进行修正，由于考虑了位置误差、非线性滤波时线性状态变量估计值作为参数以及线性滤波时非线性状态变量估计值作为参数存在的误差，并分别在计算非线性状态变量一步预测协方差矩阵和线性状态变量一步预测协方差矩阵时予以补偿，因而提高了传递对准的精度。

Description

一种机载分布式POS的实时传递对准的方法及装置
技术领域
本发明涉及导航系统领域，具体涉及一种机载分布式POS的实时传递对准的方法及装置。
背景技术
多任务遥感载荷是目前机载对地观测的重要发展方向之一，如集成高分辨率测绘相机、成像光谱仪、大视场红外扫描仪、合成孔径雷达(Synthetic Aperture Radar，SAR)于同一载机的多任务载荷，机载分布式阵列天线SAR等。对于装备多任务遥感载荷的综合航空遥感系统，需要对各载荷分布点的运动参数进行高精度测量。
分布式位置姿态测量系统(Position and Orientation System，POS)是目前获取载机多点位置、速度、姿态等运动参数的有效手段。分布式POS的组成主要包括一个高精度主位置姿态测量系统(主系统)、多个子惯性测量单元(Inertial Measurement Unit，IMU)、一个导航计算机和一套后处理软件。其中主系统由高精度主IMU和全球导航卫星系统(Global Navigation Satellite System，GNSS)组成，主IMU一般安装在机舱内或机腹部，子系统一般分布在载机两侧的机翼上，依靠主系统的高精度位置、速度、姿态等运动参数对其进行传递对准以实现所在处运动信息的精确测量。受成本以及载机对IMU体积、重量的限制，如分布式阵列天线SAR，单侧机翼上的SAR天线可多达几十部，每个天线处的空间和承重能力都非常有限，因此急需低成本、小型化的分布式POS以实现各载荷运动参数的获取。
机载对地观测成像分实时成像和离线成像两种工作模式中，实时成像要求分布式POS进行实时传递对准。由于低成本分布式POS系统惯性器件精度低，在实际应用中分布式POS是一个非线性系统，如果直接采用线性滤波方法，如代表性的卡尔曼滤波(KzlmanFilter，KF)，进行传递对准则无法满足精度要求，而现有的高精度非线性滤波方法的计算量较大，如代表性的方法：无迹卡尔曼滤波(Unscent Kalman Filter，UKF),需要对状态变量进行采样，其计算量与系统状态变量的维数成正比，UKF的运算复杂度约为
Figure BDA0001580642430000021
其中，p为系统模型的状态变量维数，q为观测量维数。由于实际飞行中机体存在弹性变形，且主、子系统之间存在安装误差角，需要将弹性变形角和安装误差角均扩充为状态变量进行估计，再加上子系统的误差(包括速度误差、姿态误差、位置误差和惯性仪表误差)，这样完整的传递对准状态变量维数将高达24维甚至更高，使得高精度非线性滤波方法难以满足传递对准的实时性要求。
目前基于KF+UKF混合滤波的对准方法(郭泽,缪玲娟.基于KF/UKF组合滤波的SINS大方位失准角初始对准[J].宇航学报,2014,35(2):163-170.；Zhao G,Yang Q,ZhangZ.Initial Alignment of Large Azimuth Misalignment Angle in SINS Based on UKF-KF[C]中国卫星导航学术年会.2014.)仅用于SINS静基座自对准和GNSS/SINS组合初始对准中，存在以下不足：(1)假设位置精确已知，且仅考虑方位失准角为大角度，没有考虑三维大失准角的情况，简化了误差方程，但损失了精度；(2)初始对准仅采用速度误差作为量测量，因而系统量测方程为线性，不适用于传递对准系统量测方程为非线性的情况；(3)在进行UKF滤波时，没有考虑线性状态变量估计值的误差，直接将线性状态变量的估计值作为非线性状态方程的参数，引入了模型误差，导致非线性状态变量的估计结果出现误差；(4)先估计非线性状态变量再估计线性状态变量的串行方式，不但增加了计算时间，而且使得非线性状态变量进行tk时刻的量测更新时，系统量测方程的参数仍为tk-1时刻的线性状态变量估计值，而tk-1时刻的线性状态变量估计值的精度必然差于tk时刻线性状态变量估计值的精度，因此将tk-1时刻的线性状态变量估计值作为非线性状态方程的参数给非线性状态的估计值带来误差。目前，有文献(尹建君,张建秋,林青.Unscented卡尔曼滤波-卡尔曼滤波算法[J].系统工程与电子技术,2008(04):617-620.)假设非线性状态的系统状态方程与线性状态无关，考虑了问题(2)中提到的系统量测方程为非线性的情况，提出用蒙特卡洛法对线性状态进行采样，匹配非线性状态的样本点来进行UKF的量测更新，以减小线性状态的估计误差对滤波结果的影响。但是蒙特卡洛法仍然需要对状态变量进行采样，额外增加了计算量。另外，蒙特卡洛法还存在采样点少时精度低的问题。
发明内容
本发明实施例提供一种机载分布式POS的实时传递对准的方法及装置，以期克服传统线性滤波方法无法满足精度要求，及非线性滤波方法计算复杂的的问题。
第一方面，本发明实施例提供一种机载分布式POS的实时传递对准的方法，包括：建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；
利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新，并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新；
将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure BDA0001580642430000031
将所述
Figure BDA0001580642430000032
作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure BDA0001580642430000033
根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值。
其中，述非线性状态方程由与姿态状态变量有关的微分方程组成，所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成。
所述建立大失准角条件下的机载分布式POS传递对准的误差模型，包括：建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型，所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程，所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型第二方面，本发明实施例还提供一种机载分布式POS的实时传递对准的装置，包括：
建立模块，用于建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；
第一更新模块，用于利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新，并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新；
第二更新模块，用于所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure BDA0001580642430000041
以及将所述
Figure BDA0001580642430000042
作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure BDA0001580642430000043
修正模块，用于根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值。本发明实施例提供的机载分布式POS的实时传递对准的方法和装置，解决了传统方法中线性滤波方法无法满足精度要求，及非线性滤波方法计算复杂的的的问题，该方法在保证三维大失准角条件下传递精度的同时，较少计算量、提高运算速度，从而满足机载多任务遥感载荷对分布式POS精度和实时性的要求。
附图说明
图1是本发明实施例提供的一种机载分布式POS的实时传递对准的方法一实施例的流程示意图；
图2是本发明实施例提供的一种机载分布式POS的实时传递对准的装置的一实施例的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清查、完整的描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
以下分别进行详细说明。
请参阅图1，图1为本发明实施例提供的方法的一个实施例流程示意图，本发明实施例提供的机载分布式POS的实施传递对准的方法，包括：
S101、建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型。
其中，误差模型包括子系统的惯导误差模型，主系统和子系统间的角度误差模型；
数学模型包括系统状态方程和系统量测方程，系统状态方程包括线性状态方程和非线性状态方程。
在本发明实施例中，线性状态方程可以由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成；非线性状态方程可以由与姿态等状态变量有关的微分方程组成。
S102、利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新，并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新。
需要说明的是，KF是针对线性系统的最优估计方法，而UKF是一种典型的非线性滤波方法。UKF通过确定采样点来近似状态的分布，然后进行UT变换得到系统状态均值和方差的近似值，从而完成非线性状态变量和误差协方差阵的更新。UKF不需要对系统方程和量测方程做线性化处理，且精度至少达到二阶近似。目前UKF已经得到了广泛应用。由于低成本分布式POS传递对准误差模型的非线性是由子系统载体坐标系与真实导航坐标系之间的大失准角引起的，其他状态变量仍为线性，因此可将状态空间分解成线性和非线性状态变量两部分，其中线性状态变量由KF进行估计，非线性状态变量由UKF进行估计，即将线性滤波方法KF和非线性滤波方法UKF有机结合，形成KF+UKF混合滤波进行传递对准，可达到提高估计精度、降低计算量的目的。
本发明实施例基于改进的KF+UKF混合滤波的机载分布式POS传递对准滤波估计；首先采用复杂加性噪声条件下的UKF方法对时刻tk-1(k＝1,2，…,g，g为总时间)时刻非线性状态方程中的非线性状态变量进行时间更新，同时采用KF对tk-1时刻线性状态方程中的线性状态变量进行时间更新。
S103、将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure BDA0001580642430000061
将所述
Figure BDA0001580642430000062
作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure BDA0001580642430000063
在本发明实施例中，进行时间更新后，再进行量测更新，需要说明的是，时间更新和量测更新是一个循环的过程，量测更新后再进行时间更新。对于量测更新，针对非线性状态变量，将KF时间更新后的线性状态变量作为非线性状态方程的参数，用UKF对非线性状态变量进行量测更新从而得到tk时刻的姿态误差估计值，同时，针对线性状态变量，将UKF时间更新后的非线性状态变量作为线性状态方程的参数，用KF对线性状态变量进行量测更新从而得到tk时刻的速度误差和位置误差估计值。
S104、根据线性状态变量估计值
Figure BDA0001580642430000064
和非线性状态变量估计值
Figure BDA0001580642430000065
对子系统的捷联解算结果进行修正，分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值。
在本发明实施例中，利用得到的姿态误差估计值、速度误差估计值和位置误差估计值修正子系统的捷联解算结果，从而得到tk时刻子系统的位置、速度和姿态信息。
在本发明中，机载分布式POS的传递对准是指机载分布式POS系统的子系统传递对准。
所述步骤S101中，建立大失准角条件下的机载分布式POS传递对准误差模型的具体步骤包括：
1)建立的大失准角条件下的子系统的惯导误差模型
在本发明实施例中相关参考坐标系的定义包括：记i为地心惯性坐标系，在本放实施例中也记为惯性坐标系；e为地球坐标系；导航坐标系为东北天地理坐标系，n表示真实导航坐标系，n1表示子系统计算导航坐标系；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系；用a和b分别代表主系统载体坐标系和子系统载体坐标系。
大失准角条件下子IMU的惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程、惯性仪表误差微分方程，根据上述定义，
①姿态微分方程：
Figure BDA0001580642430000071
其中，
Figure BDA0001580642430000072
为子系统的姿态失准角，φE、φN和φU分别为东向、北向、天向失准角，下标E、N和U分别表示东向、北向和天向；
Figure BDA0001580642430000073
为子系统的真实导航坐标系相对惯性坐标系的角速度；
Figure BDA0001580642430000074
为
Figure BDA0001580642430000075
的误差角速度；
Figure BDA0001580642430000076
为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵；I3×3为3行、3列的单位矩阵，εb为子系统陀螺仪误差，εb＝εc+wε，其中，εc为子系统陀螺仪常值漂移，
Figure BDA0001580642430000077
wε为子系统陀螺仪随机误差，
Figure BDA0001580642430000078
Figure BDA0001580642430000079
和
Figure BDA00015806424300000710
分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移，
Figure BDA00015806424300000711
和
Figure BDA00015806424300000712
分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差；
Figure BDA00015806424300000713
为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵。
Figure BDA0001580642430000081
和
Figure BDA0001580642430000082
的表达式分别为：
Figure BDA0001580642430000083
Figure BDA0001580642430000084
②速度误差微分方程为：
Figure BDA0001580642430000085
其中，Vn＝[VE VN VU]T和δVn＝[δVE δVN δVU]T分别为子系统的速度和速度误差，其中VE、VN和VU分别为东向、北向和天向速度，δVE、δVN和δVU分别为东向、北向和天向速度误差；fb＝[fx fy fz]T是子系统的比力，其中fx、fy和fz分别为载体坐标系x轴方向、y轴方向和z轴方向比力；
Figure BDA0001580642430000086
和
Figure BDA0001580642430000087
分别为地球坐标系相对惯性坐标系的角速度ωie及其误差δωie在子系统真实导航坐标系下的表示；
Figure BDA0001580642430000088
和
Figure BDA0001580642430000089
分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差；
Figure BDA00015806424300000810
为子系统加速度计误差，
Figure BDA00015806424300000811
其中，
Figure BDA00015806424300000812
为子系统加速度计常值偏置，
Figure BDA00015806424300000813
Figure BDA00015806424300000814
为系统加速度计随机误差，
Figure BDA00015806424300000815
Figure BDA00015806424300000816
和
Figure BDA00015806424300000817
分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置，
Figure BDA0001580642430000091
和
Figure BDA0001580642430000092
分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差。
③位置误差微分方程为：
Figure BDA0001580642430000093
其中，L、λ、h分别为子系统纬度、经度、高度，δL、δλ、δh分别为纬度误差、经度误差、高度误差；
Figure BDA0001580642430000094
为纬度的一阶导数，
Figure BDA0001580642430000095
Figure BDA0001580642430000096
为经度的一阶导数，
Figure BDA0001580642430000097
Figure BDA0001580642430000098
为高度的一阶导数；RM和RN分别为沿子午圈和卯酉圈的主曲率半径。
④惯性仪表误差微分方程为：
Figure BDA0001580642430000099
其中，εc为子系统陀螺仪常值漂移，
Figure BDA00015806424300000910
为子系统加速度计常值偏置。
2)建立主系统和子系统间的角误差模型，具体包括：
①建立子系统固定安装误差角ρ的微分方程
Figure BDA00015806424300000911
其中，ρ＝[ρx ρy ρz]T，ρx、ρy和ρz分别为子系统在载体坐标系x轴、y轴和z轴的安装误差角；
②建立弹性变形角的微分方程
Figure BDA00015806424300000912
其中，θj为子系统载体坐标系第j轴上的弹性变形角，j＝x,y,z，θ＝[θx θy θz]T为弹性变形角；βj＝2.146/τj，τj为二阶马尔科夫过程相关时间；ηj为零均值白噪声，其方差
Figure BDA0001580642430000101
满足：
Figure BDA0001580642430000102
其中，σj 2为弹性变形角θj的方差，βj和
Figure BDA0001580642430000103
为描述弹性变形角θ的二阶马尔科夫过程的参数。
所述步骤S101中，建立大失准角条件下的机载分布式POS传递对准数学模型的具体步骤包括：
1)建立子系统状态方程
系统状态变量包括非线性状态变量和线性状态变量两部分，分别定义如下：
非线性状态变量xNL＝[x1]T；
线性状态变量xL＝[x2 x3]T。
其中，
x1＝[φE φN φU],
Figure BDA0001580642430000104
Figure BDA0001580642430000105
系统状态方程包括非线性状态方程和线性状态方法，其中非线性状态方程为：
Figure BDA0001580642430000106
线性状态方程为：
Figure BDA0001580642430000107
其中，
Figure BDA0001580642430000108
和
Figure BDA0001580642430000109
分别为tk时刻的非线性状态变量和线性状态变量，
Figure BDA00015806424300001010
和
Figure BDA00015806424300001011
分别为tk-1时刻的非线性状态变量和线性状态变量，非线性状态方程的系统噪声为
Figure BDA00015806424300001012
线性状态方程的系统噪声为
Figure BDA00015806424300001013
其中
Figure BDA00015806424300001014
分别为自系统载体坐标系x轴、y轴、z轴陀螺仪的随机误差，
Figure BDA00015806424300001015
分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差；非线性状态方程的系统噪声wNL和线性状态方程的wL均为零均值高斯白噪声，wNL的方差阵QNL由陀螺仪随机漂移确定，wL的方差阵QL由加速度计随机偏置和二阶马尔科夫过程参数
Figure BDA0001580642430000111
决定；
系统状态方程中各系数矩阵的具体表达式如下：
Figure BDA0001580642430000112
Figure BDA0001580642430000113
Figure BDA0001580642430000114
Figure BDA0001580642430000115
Figure BDA0001580642430000121
Figure BDA0001580642430000122
Figure BDA0001580642430000123
Figure BDA0001580642430000124
2)建立系统量测方程
系统量测变量定义为：
z＝[δψ δθ δγ δVE′ δVN′ δVU′ δL′ δλ′ δh′]T；
列写系统量测方程如下：
Figure BDA0001580642430000125
其中，zk为tk时刻的量测量，δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差，δVE′、δVN′、δVU′分别为子系统与主系统东向、北向、天向速度之差，δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差；量测噪声
Figure BDA0001580642430000131
其中vδψ、vδθ、vδγ分别为主系统航向角、俯仰角、横滚角的量测噪声，
Figure BDA0001580642430000132
分别为主系统东向、北向、天向速度的量测噪声，vδL、vδλ、vδh分别为主系统纬度、经度、高度的量测噪声；v为量测噪声，取为零均值高斯白噪声，vk为v在tk时刻的值，其方差阵R由主系统的姿态精度、速度精度和位置精度决定。
系统量测方程各系数矩阵分别为：
Figure BDA0001580642430000133
令主系统姿态的方向余弦矩阵
Figure BDA0001580642430000134
失准角的方向余弦矩阵
Figure BDA0001580642430000135
记
Figure BDA0001580642430000136
为矩阵Ta的第l行、第m列的元素，N(lm)为矩阵N的第l行、第m列的元素，l＝1,2,3，m＝1,2,3，则上式中H1和H2的表达式分别为：
Figure BDA0001580642430000137
Figure BDA0001580642430000138
其中，
Figure BDA0001580642430000139
Figure BDA00015806424300001310
Figure BDA0001580642430000141
Figure BDA0001580642430000142
Figure BDA0001580642430000143
Figure BDA0001580642430000144
Figure BDA0001580642430000145
Figure BDA0001580642430000146
本发明实施例汇总主要采用改进的KF+UKF混合滤波进行传递对准滤波估计，具体包括步骤S102的时间更新和步骤S103的量测更新。
1)时间更新
①采用复杂加性噪声条件下的UKF计算tk时刻非线性状态变量的一步预测值
Figure BDA0001580642430000147
这里所述的一步预测值是值非线性状态变量进行时间更新后所得的结果。
设nNL为非线性状态变量的维数，计算tk-1时刻的2nNL+1个样本点
Figure BDA0001580642430000148
Figure BDA0001580642430000149
Figure BDA00015806424300001410
Figure BDA00015806424300001411
其中，
Figure BDA00015806424300001416
10-4≤α≤1，κ＝3-nNL，
Figure BDA00015806424300001412
和
Figure BDA00015806424300001413
分别为tk-1时刻非线性状态变量
Figure BDA00015806424300001414
的估计值和估计协方差矩阵；
Figure BDA00015806424300001415
表示矩阵
Figure BDA0001580642430000151
的下三角分解平方根的第i列；
考虑到线性状态变量
Figure BDA0001580642430000152
在tk-1时刻的估计值
Figure BDA0001580642430000153
与真实值
Figure BDA0001580642430000154
必然存在误差
Figure BDA0001580642430000155
即
Figure BDA0001580642430000156
这里将
Figure BDA0001580642430000157
视为协方差矩阵为
Figure BDA0001580642430000158
的零均值高斯白噪声，
Figure BDA0001580642430000159
为线性状态变量
Figure BDA00015806424300001510
在tk-1时刻的估计协方差矩阵；由于非线性状态方程的系统噪声为复杂加性噪声，因此不需要对状态变量进行扩维，系统维数降低了15维，大大减少了采样数目，减小了计算量。
计算tk时刻的一步预测值
Figure BDA00015806424300001511
及其协方差矩阵
Figure BDA00015806424300001512
如下：
Figure BDA00015806424300001513
Figure BDA00015806424300001514
Figure BDA00015806424300001515
式中，
Figure BDA00015806424300001516
为
Figure BDA00015806424300001517
的一步预测模型值，
Figure BDA00015806424300001518
为补偿项；Wi为权值，计算方法如下：
Figure BDA00015806424300001519
Figure BDA00015806424300001520
Figure BDA00015806424300001521
其中，β与状态变量的分布形式有关，对于正态分布，β＝2为最优值；
②采用KF计算tk时刻线性状态变量的一步预测值
Figure BDA00015806424300001522
考虑到G(xNL)与非线性状态变量xNL有关，而xNL的估计值
Figure BDA00015806424300001523
存在误差δxNL，即
Figure BDA00015806424300001524
可将G(xNL)在
Figure BDA00015806424300001525
处进行泰勒展开，保留一阶线性项，将δxNL视为协方差矩阵为
Figure BDA00015806424300001526
的零均值高斯白噪声并在计算
Figure BDA00015806424300001527
时予以补偿；
时间更新过程如下：
Figure BDA00015806424300001528
Figure BDA00015806424300001529
其中，
Figure BDA0001580642430000161
Figure BDA0001580642430000162
为补偿项；
2)量测更新
①采用复杂加性噪声条件下的UKF计算tk时刻的非线性状态变量估计值
Figure BDA0001580642430000163
根据得到的非线性状态变量一步预测值
Figure BDA0001580642430000164
计算非线性状态变量的一步预测样本点
Figure BDA0001580642430000165
Figure BDA0001580642430000166
Figure BDA0001580642430000167
Figure BDA0001580642430000168
考虑到
Figure BDA0001580642430000169
存在误差，即
Figure BDA00015806424300001610
Figure BDA00015806424300001611
为
Figure BDA00015806424300001612
的误差，将
Figure BDA00015806424300001613
视为协方差矩阵为
Figure BDA00015806424300001614
的零均值高斯白噪声，
Figure BDA00015806424300001615
为tk时刻量测变量一步预测值的自协方差矩阵，
Figure BDA00015806424300001616
为tk时刻非线性状态变量一步预测值与量测变量一步预测值的互协方差矩阵。由于系统量测方程的噪声为加性噪声，
Figure BDA00015806424300001617
和
Figure BDA00015806424300001618
的计算过程如下：
Figure BDA00015806424300001619
Figure BDA00015806424300001620
其中，
Figure BDA00015806424300001621
Figure BDA00015806424300001622
为补偿项；
计算非线性状态变量的增益矩阵
Figure BDA00015806424300001623
Figure BDA00015806424300001624
计算滤波估计值及其协方差矩阵：
Figure BDA0001580642430000171
Figure BDA0001580642430000172
②采用KF计算tk时刻线性状态变量的估计值
Figure BDA0001580642430000173
Figure BDA0001580642430000174
Figure BDA0001580642430000175
Figure BDA0001580642430000176
其中，
Figure BDA0001580642430000177
为线性状态变量的增益矩阵，Y′为
Figure BDA0001580642430000178
的雅可比矩阵，即
Figure BDA0001580642430000179
Figure BDA00015806424300001710
为补偿项；I21×21为21行21列的单位矩阵；
所述步骤S104对子系统运动参数的修正具体包括：
利用上述步骤得到的tk时刻的失准角φE、φN、φU，速度误差δVE、δVN、δVU和位置误差δL、δλ、δh，修正子系统的捷联解算结果，得到tk时刻更加准确的子系统位置、速度和姿态；
①速度修正
Figure BDA00015806424300001711
Figure BDA00015806424300001712
Figure BDA00015806424300001713
其中，
Figure BDA00015806424300001714
和
Figure BDA00015806424300001715
分别为子系统修正后的东向、北向和天向速度；
Figure BDA00015806424300001716
和
Figure BDA00015806424300001717
分别为子系统捷联解算得到的东向、北向和天向速度；δVE、δVN和δVU分别为tk时刻KF估计出的子系统捷联解算东向、北向和天向速度误差；
②位置修正
Lnew＝Lold-δL
λnew＝λold-δλ
hnew＝hold-δh
其中，Lold、λold和hold分别为子系统捷联解算得到的纬度、经度和高度；Lnew、λnew和hnew分别为子系统修正后的纬度、经度和高度；δL、δλ和δh分别为tk时刻KF估计出的子系统捷联解算纬度、经度和高度误差；
③姿态修正
计算tk时刻子系统地理坐标系n与计算地理坐标系n1间的方向余弦矩阵
Figure BDA0001580642430000181
Figure BDA0001580642430000182
计算tk时刻子子系统载体坐标系b与真实地理坐标系n之间的方向余弦矩阵
Figure BDA0001580642430000183
Figure BDA0001580642430000184
其中
Figure BDA0001580642430000185
为tk时刻子系统捷联解算得到的方向余弦矩阵；
由被更新后的子系统的姿态阵
Figure BDA0001580642430000186
计算tk时刻子系统的航向角ψs、俯仰角θs和横滚角γs，将
Figure BDA0001580642430000187
记为：
Figure BDA0001580642430000188
其中，Tlm为矩阵
Figure BDA0001580642430000189
中第l行、第m列的元素，则子IMU航向角ψs、俯仰角θs和横滚角γs的主值，即ψs主、θs主和γs主分别为：
Figure BDA00015806424300001810
θs主＝arcsin(T32)
Figure BDA00015806424300001811
由于航向角ψs、俯仰角θs和横滚角γs的取值范围分别定义为[0，2π]、
Figure BDA00015806424300001812
Figure BDA00015806424300001813
[-π，+π]；那么，ψs、θs和γs的取值可由下式确定：
Figure BDA00015806424300001814
θs＝θs主，
Figure BDA0001580642430000191
通过对子系统的速度、位置和姿态进行修正，能够得到更加准确的子系统安装点的速度、位置和姿态信息，完成传递对准。
本发明针对低成本分布式POS实时传递对准中，因系统的非线性，存在线性滤波方法精度低、非线性滤波方法不能满足实时性要求的问题，对现有线性滤波方法KF和非线性滤波方法UKF的混合滤波进行改进，用改进的KF+UKF混合滤波进行实时传递对准，提高传递对准精度的同时减少计算量。与现有技术相比，本发明：(1)建立了大失准角条件下的传递对准数学模型，能够适用于计算导航坐标系与真实导航坐标系的东向夹角、北向夹角和天向夹角均为大失准角的情况；(2)适用于系统状态方程和系统量测方程均为非线性的情况；(3)考虑了位置误差、非线性滤波时线性状态变量估计值作为参数以及线性滤波时非线性状态变量估计值作为参数存在的误差，并分别在计算非线性状态变量一步预测协方差矩阵和线性状态变量一步预测协方差矩阵时予以补偿，因而提高了传递对准的精度；(4)采用KF和UKF并行滤波的结构，克服了现有方法先估计非线性状态变量再估计线性状态变量的串行方式带来的非线性滤波时间更新滞后的问题，压缩了计算时间，从而满足机载分布式POS实时传递对准的精度和实时性要求。
请参阅图2，图2为本发明实施例提供的机载分布式POS的实时传递对准装置的一个实施例结构示意图，本发明实施例提供的机载分布式POS的实时传递对准的装置，主要包括：
建立模块201，用于建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；
第一更新模块202，用于利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新，并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新；
第二更新模块203，用于所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure BDA0001580642430000201
以及将所述
Figure BDA0001580642430000202
作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure BDA0001580642430000203
修正模块204，用于根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值。
其中，非线性状态方程由与姿态状态变量有关的微分方程组成，线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成。
进一步的，建立模块201具体用于建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型，所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程，所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型。
需要说明的是，本发明实施例提供的机载分布式POS的实时传递对准装置为用于实现机载分布式POS的实时传递对准的方法的装置。因此，方法权利要求中具有的特征也可用于实现该装置，同样属于本发明装置的保护范围，因此在本实施例中并未一一列明。
由上可见，本发明实施例提供的机载分布式POS的实时传递对准装置建立了大失准角条件下的传递对准数学模型，适用于系统状态方程和系统量测方程均为非线性的情况，考虑了位置误差、非线性滤波时线性状态变量估计值作为参数以及线性滤波时非线性状态变量估计值作为参数存在的误差，并分别在计算非线性状态变量一步预测协方差矩阵和线性状态变量一步预测协方差矩阵时予以补偿，因而提高了传递对准的精度，解决了现有方法先估计非线性状态变量再估计线性状态变量的串行方式带来的非线性滤波时间更新滞后的问题，压缩了计算时间，从而满足机载分布式POS实时传递对准的精度和实时性要求。
同时，在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
以上对本发明实施例所提供的机载分布式POS的实时传递对准的方法和装置进行了详细介绍，本文中应用了具体个例对交互的本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (2)

1.一种机载分布式POS的实时传递对准的方法，其特征在于，包括：
建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；所述非线性状态方程由与姿态状态变量有关的微分方程组成，所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成；
所述建立大失准角条件下的机载分布式POS传递对准的误差模型，包括：建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型，所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程，所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型；
所述姿态误差微分方程为：
Figure FDA0002290621860000011
其中，
Figure FDA0002290621860000012
为子系统姿态失准角，φE、φN和φU分别为东向、北向、天向失准角，
Figure FDA0002290621860000013
为子系统真实导航坐标系相对惯性坐标系的角速度；
Figure FDA0002290621860000014
为
Figure FDA0002290621860000015
的误差角速度；
Figure FDA0002290621860000016
为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵；I3×3为3行、3列的单位矩阵，εb为子系统陀螺仪误差，εb＝εc+wε，其中，εc为子系统陀螺仪常值漂移，
Figure FDA0002290621860000017
wε为子系统陀螺仪随机误差，
Figure FDA0002290621860000018
Figure FDA0002290621860000019
和
Figure FDA00022906218600000110
分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移，
Figure FDA00022906218600000111
和
Figure FDA00022906218600000112
分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差；
Figure FDA00022906218600000113
为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵；其中，
Figure FDA00022906218600000114
Figure FDA0002290621860000021
所述速度误差微分方程为：
Figure FDA0002290621860000022
其中，Vn＝[VE VN VU]T和δVn＝[δVE δVN δVU]T分别为子系统速度和速度误差，其中VE、VN和VU分别为东向、北向和天向速度，δVE、δVN和δVU分别为东向、北向和天向速度误差；fb＝[fxfy fz]T是子系统的比力，其中fx、fy和fz分别为载体坐标系x方向、y方向和z方向比力；
Figure FDA0002290621860000023
和
Figure FDA0002290621860000024
分别为地球坐标系相对惯性坐标系的角速度ωie及其误差δωie在子系统真实导航坐标系下的表示；
Figure FDA0002290621860000025
和
Figure FDA0002290621860000026
分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差；
Figure FDA0002290621860000027
为子系统加速度计误差，
Figure FDA0002290621860000028
其中，
Figure FDA0002290621860000029
为子系统加速度计常值偏置，
Figure FDA00022906218600000210
Figure FDA00022906218600000211
为系统加速度计随机误差，
Figure FDA00022906218600000212
Figure FDA00022906218600000213
和
Figure FDA00022906218600000214
分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置，
Figure FDA00022906218600000215
和
Figure FDA00022906218600000216
分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差；
所述位置误差微分方程为：
Figure FDA00022906218600000217
其中，L、λ、h分别为子系统的纬度、经度、高度，δL、δλ、δh分别为纬度误差、经度误差、高度误差；
Figure FDA00022906218600000218
为纬度的一阶导数，
Figure FDA00022906218600000220
为经度的一阶导数，
Figure FDA00022906218600000221
Figure FDA00022906218600000222
为高度的一阶导数；RM和RN分别为沿子午圈和卯酉圈的主曲率半径；
所述惯性仪表误差微分方程为：
Figure FDA00022906218600000223
其中，εc为子系统陀螺仪常值漂移，
Figure FDA0002290621860000031
为子系统加速度计常值偏置；
所述建立主系统和子系统间的角误差模型，包括：
建立子系统固定安装误差角ρ的微分方程：
Figure FDA0002290621860000032
其中，ρ＝[ρx ρy ρz]T为子系统固定安装误差角，ρx、ρy和ρz分别为子系统在载体坐标系x轴、y轴和z轴的安装误差角；
建立子系统弹性变形角θ的微分方程：
Figure FDA0002290621860000033
其中，θj为子系统载体坐标系第j轴上的弹性变形角，j＝x,y,z，θ＝[θx θy θz]T为弹性变形角；βj＝2.146/τj，τj为二阶马尔科夫过程相关时间；ηj为零均值白噪声，其方差
Figure FDA0002290621860000036
满足：
Figure FDA0002290621860000037
其中，σj 2为弹性变形角θj的方差，βj和
Figure FDA0002290621860000038
为描述弹性变形角θ的二阶马尔科夫过程的参数；
利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新，并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新；
所述非线性状态变量xNL和线性状态变量xL分别定义为：
xNL＝[x1]T
xL＝[x2 x3]T
其中，
x1＝[φE φN φU],
Figure FDA0002290621860000034
Figure FDA0002290621860000035
所述非线性状态方程为：
Figure FDA0002290621860000041
所述线性状态方程为：
Figure FDA0002290621860000042
其中，
Figure FDA0002290621860000043
和
Figure FDA0002290621860000044
分别为tk时刻的非线性状态变量和线性状态变量，
Figure FDA0002290621860000045
和
Figure FDA0002290621860000046
分别为tk-1时刻的非线性状态变量和线性状态变量，非线性状态方程的系统噪声为
Figure FDA0002290621860000047
线性状态方程的系统噪声为
Figure FDA0002290621860000048
其中
Figure FDA0002290621860000049
分别为子系统载体坐标系x轴、y轴、z轴陀螺仪的随机误差，
Figure FDA00022906218600000410
分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差；非线性状态方程的系统噪声wNL和线性状态方程的wL均为零均值高斯白噪声，wNL的方差阵QNL由陀螺仪随机漂移确定，wL的方差阵QL由加速度计随机偏置和二阶马尔科夫过程参数
Figure FDA00022906218600000411
确定；系统状态方程中各系数矩阵的表达式如下：
Figure FDA00022906218600000412
Figure FDA00022906218600000413
Figure FDA00022906218600000414
Figure FDA0002290621860000051
Figure FDA0002290621860000052
Figure FDA0002290621860000053
Figure FDA0002290621860000054
Figure FDA0002290621860000055
所述建立大失准角条件下的机载分布式POS传递对准的数学模型包括：建立大失准角条件下的机载分布式POS传递对准的系统量测模型；
所述建立大失准角条件下的机载分布式POS传递对准的系统量测模型，包括：
系统量测量定义为：
z＝[δψ δθ δγ δV′E δV′N δV′U δL′ δλ′ δh′]T
建立系统量测方程：
Figure FDA0002290621860000061
其中，zk为tk时刻的量测量，δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差，δV′E、δV′N、δV′U分别为子系统与主系统东向、北向、天向速度之差，δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差；量测噪声
Figure FDA0002290621860000062
其中vδψ、vδθ、vδγ分别为主系统航向角、俯仰角、横滚角的量测噪声，
Figure FDA0002290621860000063
分别为主系统东向、北向、天向速度的量测噪声，vδL、vδλ、vδh分别为主系统纬度、经度、高度的量测噪声；v为量测噪声，取为零均值高斯白噪声，vk为v在tk时刻的值；
所述利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量进行时间更新，包括：
计算tk-1时刻的2nNL+1个样本点
Figure FDA0002290621860000064
其中，nNL为非线性状态变量的维数：
Figure FDA0002290621860000065
其中，
Figure FDA00022906218600000611
10-4≤α≤1，κ＝3-nNL，
Figure FDA0002290621860000066
和
Figure FDA0002290621860000067
分别为tk-1时刻非线性状态变量
Figure FDA0002290621860000068
的估计值和估计协方差矩阵；
Figure FDA0002290621860000069
表示矩阵
Figure FDA00022906218600000610
的下三角分解平方根的第i列；
利用UKF计算tk时刻非线性状态变量的一步预测值
Figure FDA0002290621860000071
Figure FDA0002290621860000072
Figure FDA0002290621860000073
Figure FDA0002290621860000074
其中，
Figure FDA0002290621860000075
为
Figure FDA0002290621860000076
的一步预测模型值，
Figure FDA0002290621860000077
为补偿项；Wi为权值；
所述利用KF对tk-1时刻所述线性状态方程中的线性状态变量进行时间更新，包括：
Figure FDA0002290621860000078
其中，
Figure FDA0002290621860000079
Figure FDA00022906218600000710
为补偿项；
将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure FDA00022906218600000711
将所述
Figure FDA00022906218600000712
作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure FDA00022906218600000713
所述将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure FDA00022906218600000714
包括：
根据所述非线性状态变量一步预测值
Figure FDA00022906218600000715
计算非线性状态变量的一步预测样本点
Figure FDA00022906218600000716
Figure FDA0002290621860000081
其中，
Figure FDA0002290621860000082
Figure FDA0002290621860000083
为
Figure FDA0002290621860000084
的误差，
Figure FDA0002290621860000085
为协方差矩阵为
Figure FDA0002290621860000086
的零均值高斯白噪声，
Figure FDA0002290621860000087
为tk时刻量测变量一步预测值的自协方差矩阵，
Figure FDA0002290621860000088
为tk时刻非线性状态变量一步预测值与量测量一步预测值的互协方差矩阵，
Figure FDA0002290621860000089
和
Figure FDA00022906218600000810
的计算过程如下：
Figure FDA00022906218600000811
其中，
Figure FDA00022906218600000812
Figure FDA00022906218600000813
为补偿项；
计算非线性状态变量的增益矩阵
Figure FDA00022906218600000814
Figure FDA00022906218600000815
计算滤波估计值及其协方差矩阵：
Figure FDA00022906218600000816
Figure FDA00022906218600000817
将所述UKF更新后的tk-1时刻的非线性状态作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure FDA00022906218600000818
包括：
Figure FDA0002290621860000091
Figure FDA0002290621860000092
Figure FDA0002290621860000093
其中，
Figure FDA0002290621860000094
为线性状态变量的增益矩阵，Y′为
Figure FDA0002290621860000095
的雅可比矩阵，
Figure FDA0002290621860000096
Figure FDA0002290621860000097
为补偿项；I21×21为21行21列的单位矩阵；
根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值；
所述根据所述线性状态变量估计值对所述子系统的捷联解算结果进行修正，得到修正后的tk时刻的子系统的线性状态变量值，包括：
Figure FDA0002290621860000098
Figure FDA0002290621860000099
Figure FDA00022906218600000910
其中，
Figure FDA00022906218600000911
和
Figure FDA00022906218600000912
分别为子IMU修正后的东向、北向和天向速度；
Figure FDA00022906218600000913
和
Figure FDA00022906218600000914
分别为子系统捷联解算得到的东向、北向和天向速度；δVE、δVN和δVU分别为tk时刻KF估计出的子系统捷联解算东向、北向和天向速度误差；
Lnew＝Lold-δL
λnew＝λold-δλ
hnew＝hold-δh
其中，Lold、λold和hold分别为子IMU捷联解算得到的纬度、经度和高度；Lnew、λnew和hnew分别为子IMU修正后的纬度、经度和高度；δL、δλ和δh分别为tk时刻KF估计出的子IMU捷联解算纬度、经度和高度误差；
所述根据所述非线性状态变量估计值对所述子系统的捷联解算结果进行修正，得到修正后的tk时刻的子系统的非线性状态变量值，包括：
计算tk时刻子系统真实导航坐标系n与计算导航坐标系n1间的方向余弦矩阵
Figure FDA0002290621860000101
Figure FDA0002290621860000102
计算tk时刻子系统载体坐标系b与真实导航坐标系n之间的方向余弦矩阵
Figure FDA0002290621860000103
Figure FDA0002290621860000104
其中
Figure FDA0002290621860000105
为tk时刻子系统捷联解算得到的方向余弦矩阵；
由被更新后的
Figure FDA0002290621860000106
计算tk时刻子系统的航向角ψs、俯仰角θs和横滚角γs，将
Figure FDA0002290621860000107
记为：
Figure FDA0002290621860000108
其中，Tlm为矩阵
Figure FDA0002290621860000109
中第l行、第m列的元素，则子IMU航向角ψs、俯仰角θs和横滚角γs的主值，即ψs主、θs主和γs主分别为：
Figure FDA00022906218600001010
θs主＝arcsin(T32)
Figure FDA00022906218600001011
Figure FDA00022906218600001013
Figure FDA00022906218600001012
θs＝θs主，
Figure FDA0002290621860000111
2.一种机载分布式POS的实时传递对准的装置，其特征在于，包括：
建立模块，用于建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；所述非线性状态方程由与姿态状态变量有关的微分方程组成，所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成；
所述建立模块具体用于建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型，所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程，所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型；
其中，所述姿态误差微分方程为：
Figure FDA0002290621860000112
其中，
Figure FDA0002290621860000113
为子系统姿态失准角，φE、φN和φU分别为东向、北向、天向失准角，
Figure FDA0002290621860000114
为子系统真实导航坐标系相对惯性坐标系的角速度；
Figure FDA0002290621860000115
为
Figure FDA0002290621860000116
的误差角速度；
Figure FDA0002290621860000117
为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵；I3×3为3行、3列的单位矩阵，εb为子系统陀螺仪误差，εb＝εc+wε，其中，εc为子系统陀螺仪常值漂移，
Figure FDA0002290621860000118
wε为子系统陀螺仪随机误差，
Figure FDA0002290621860000119
Figure FDA00022906218600001110
和
Figure FDA00022906218600001111
分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移，
Figure FDA00022906218600001112
和
Figure FDA00022906218600001113
分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差；
Figure FDA00022906218600001114
为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵；其中，
Figure FDA0002290621860000121
Figure FDA0002290621860000122
所述速度误差微分方程为：
Figure FDA0002290621860000123
其中，Vn＝[VE VN VU]T和δVn＝[δVE δVN δVU]T分别为子系统速度和速度误差，其中VE、VN和VU分别为东向、北向和天向速度，δVE、δVN和δVU分别为东向、北向和天向速度误差；fb＝[fxfy fz]T是子系统的比力，其中fx、fy和fz分别为载体坐标系x方向、y方向和z方向比力；
Figure FDA0002290621860000124
和
Figure FDA0002290621860000125
分别为地球坐标系相对惯性坐标系的角速度ωie及其误差δωie在子系统真实导航坐标系下的表示；
Figure FDA0002290621860000126
和
Figure FDA0002290621860000127
分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差；
Figure FDA0002290621860000128
为子系统加速度计误差，
Figure FDA0002290621860000129
其中，
Figure FDA00022906218600001210
为子系统加速度计常值偏置，
Figure FDA00022906218600001211
Figure FDA00022906218600001212
为系统加速度计随机误差，
Figure FDA00022906218600001213
Figure FDA00022906218600001214
和
Figure FDA00022906218600001215
分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置，
Figure FDA00022906218600001216
和
Figure FDA00022906218600001217
分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差；
所述位置误差微分方程为：
Figure FDA00022906218600001218
其中，L、λ、h分别为子系统的纬度、经度、高度，δL、δλ、δh分别为纬度误差、经度误差、高度误差；
Figure FDA00022906218600001219
为纬度的一阶导数，
Figure FDA00022906218600001220
Figure FDA00022906218600001221
为经度的一阶导数，
Figure FDA00022906218600001222
Figure FDA00022906218600001223
为高度的一阶导数；RM和RN分别为沿子午圈和卯酉圈的主曲率半径；
所述惯性仪表误差微分方程为：
Figure FDA0002290621860000131
其中，εc为子系统陀螺仪常值漂移，
Figure FDA0002290621860000132
为子系统加速度计常值偏置；
所述建立主系统和子系统间的角误差模型，包括：
建立子系统固定安装误差角ρ的微分方程：
Figure FDA0002290621860000133
其中，ρ＝[ρx ρy ρz]T为子系统固定安装误差角，ρx、ρy和ρz分别为子系统在载体坐标系x轴、y轴和z轴的安装误差角；
建立子系统弹性变形角θ的微分方程：
Figure FDA0002290621860000134
其中，θj为子系统载体坐标系第j轴上的弹性变形角，j＝x,y,z，θ＝[θx θy θz]T为弹性变形角；βj＝2.146/τj，τj为二阶马尔科夫过程相关时间；ηj为零均值白噪声，其方差
Figure FDA0002290621860000136
满足：
Figure FDA0002290621860000135
其中，σj 2为弹性变形角θj的方差，βj和
Figure FDA0002290621860000137
为描述弹性变形角θ的二阶马尔科夫过程的参数；
第一更新模块，用于利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新，并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新；
所述非线性状态变量xNL和线性状态变量xL分别定义为：
xNL＝[x1]T
xL＝[x2 x3]T
其中，
x1＝[φE φN φU],
Figure FDA0002290621860000141
Figure FDA0002290621860000142
所述非线性状态方程为：
Figure FDA0002290621860000143
所述线性状态方程为：
Figure FDA0002290621860000144
其中，
Figure FDA0002290621860000145
和
Figure FDA0002290621860000146
分别为tk时刻的非线性状态变量和线性状态变量，
Figure FDA0002290621860000147
和
Figure FDA0002290621860000148
分别为tk-1时刻的非线性状态变量和线性状态变量，非线性状态方程的系统噪声为
Figure FDA0002290621860000149
线性状态方程的系统噪声为
Figure FDA00022906218600001410
其中
Figure FDA00022906218600001411
分别为子系统载体坐标系x轴、y轴、z轴陀螺仪的随机误差，
Figure FDA00022906218600001412
分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差；非线性状态方程的系统噪声wNL和线性状态方程的wL均为零均值高斯白噪声，wNL的方差阵QNL由陀螺仪随机漂移确定，wL的方差阵QL由加速度计随机偏置和二阶马尔科夫过程参数
Figure FDA00022906218600001415
确定；系统状态方程中各系数矩阵的表达式如下：
Figure FDA00022906218600001413
Figure FDA00022906218600001414
Figure FDA0002290621860000151
Figure FDA0002290621860000152
Figure FDA0002290621860000153
Figure FDA0002290621860000154
Figure FDA0002290621860000155
Figure FDA0002290621860000156
所述建立模块具体用于建立大失准角条件下的机载分布式POS传递对准的系统量测模型；
所述建立大失准角条件下的机载分布式POS传递对准的系统量测模型，包括：
系统量测量定义为：
z＝[δψ δθ δγ δV′E δV′N δV′U δL′ δλ′ δh′]T
建立系统量测方程：
Figure FDA0002290621860000161
其中，zk为tk时刻的量测量，δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差，δV′E、δV′N、δV′U分别为子系统与主系统东向、北向、天向速度之差，δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差；量测噪声
Figure FDA0002290621860000162
其中vδψ、vδθ、vδγ分别为主系统航向角、俯仰角、横滚角的量测噪声，
Figure FDA0002290621860000163
分别为主系统东向、北向、天向速度的量测噪声，vδL、vδλ、vδh分别为主系统纬度、经度、高度的量测噪声；v为量测噪声，取为零均值高斯白噪声，vk为v在tk时刻的值；
所述第一更新模块具体用于，计算tk-1时刻的2nNL+1个样本点
Figure FDA0002290621860000164
其中，nNL为非线性状态变量的维数：
Figure FDA0002290621860000165
其中，
Figure FDA00022906218600001610
10-4≤α≤1，κ＝3-nNL，
Figure FDA0002290621860000166
和
Figure FDA0002290621860000167
分别为tk-1时刻非线性状态变量
Figure FDA0002290621860000168
的估计值和估计协方差矩阵；
Figure FDA0002290621860000169
表示矩阵
Figure FDA0002290621860000171
的下三角分解平方根的第i列；
利用UKF计算tk时刻非线性状态变量的一步预测值
Figure FDA0002290621860000172
Figure FDA0002290621860000173
Figure FDA0002290621860000174
Figure FDA0002290621860000175
其中，
Figure FDA0002290621860000176
为
Figure FDA0002290621860000177
的一步预测模型值，
Figure FDA0002290621860000178
为补偿项；Wi为权值；
所述第一更新模块还具体用于根据以下公式利用KF对tk-1时刻所述线性状态方程中的线性状态变量进行时间更新：
Figure FDA0002290621860000179
其中，
Figure FDA00022906218600001710
Figure FDA00022906218600001711
为补偿项；
第二更新模块，用于将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到tk时刻的非线性状态变量估计值
Figure FDA00022906218600001712
以及将所述
Figure FDA00022906218600001713
作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到tk时刻的线性状态变量估计值
Figure FDA00022906218600001714
所述第二更新模块具体用于，根据所述非线性状态变量一步预测值
Figure FDA00022906218600001715
计算非线性状态变量的一步预测样本点
Figure FDA00022906218600001716
Figure FDA0002290621860000181
其中，
Figure FDA0002290621860000182
Figure FDA0002290621860000183
为
Figure FDA0002290621860000184
的误差，
Figure FDA0002290621860000185
为协方差矩阵为
Figure FDA0002290621860000186
的零均值高斯白噪声，
Figure FDA0002290621860000187
为tk时刻量测变量一步预测值的自协方差矩阵，
Figure FDA0002290621860000188
为tk时刻非线性状态变量一步预测值与量测量一步预测值的互协方差矩阵，
Figure FDA0002290621860000189
和
Figure FDA00022906218600001810
的计算过程如下：
Figure FDA00022906218600001811
其中，
Figure FDA00022906218600001812
Figure FDA00022906218600001813
为补偿项；
计算非线性状态变量的增益矩阵
Figure FDA00022906218600001814
Figure FDA00022906218600001815
计算滤波估计值及其协方差矩阵：
Figure FDA00022906218600001816
Figure FDA00022906218600001817
以及，所述第二更新模块具体用于根据以下获得时刻的tk时刻的非线性状态变量估计值
Figure FDA00022906218600001818
Figure FDA00022906218600001819
Figure FDA00022906218600001820
Figure FDA00022906218600001821
其中，
Figure FDA0002290621860000191
为线性状态变量的增益矩阵，Y′为
Figure FDA0002290621860000192
的雅可比矩阵，
Figure FDA0002290621860000193
Figure FDA0002290621860000194
为补偿项；I21×21为21行21列的单位矩阵；
修正模块，用于根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值；
所述修正模块具体用于，根据以下公式对所述子系统的捷联解算结果进行修正得到修正后tk时刻的子系统的线性状态变量值：
Figure FDA0002290621860000195
Figure FDA0002290621860000196
Figure FDA0002290621860000197
其中，
Figure FDA0002290621860000198
和
Figure FDA0002290621860000199
分别为子IMU修正后的东向、北向和天向速度；
Figure FDA00022906218600001910
和
Figure FDA00022906218600001911
分别为子系统捷联解算得到的东向、北向和天向速度；δVE、δVN和δVU分别为tk时刻KF估计出的子系统捷联解算东向、北向和天向速度误差；
Lnew＝Lold-δL
λnew＝λold-δλ
hnew＝hold-δh
其中，Lold、λold和hold分别为子IMU捷联解算得到的纬度、经度和高度；Lnew、λnew和hnew分别为子IMU修正后的纬度、经度和高度；δL、δλ和δh分别为tk时刻KF估计出的子IMU捷联解算纬度、经度和高度误差；
所述修正模块还具体用于，根据以下对所述子系统的捷联解算结果进行修正得到修正后tk时刻的子系统的非线性状态变量值：
计算tk时刻子系统真实导航坐标系n与计算导航坐标系n1间的方向余弦矩阵
Figure FDA00022906218600001912
Figure FDA0002290621860000201
计算tk时刻子系统载体坐标系b与真实导航坐标系n之间的方向余弦矩阵
Figure FDA0002290621860000202
Figure FDA0002290621860000203
其中
Figure FDA0002290621860000204
为tk时刻子系统捷联解算得到的方向余弦矩阵；
由被更新后的
Figure FDA0002290621860000205
计算tk时刻子系统的航向角ψs、俯仰角θs和横滚角γs，将
Figure FDA0002290621860000206
记为：
Figure FDA0002290621860000207
其中，Tlm为矩阵
Figure FDA0002290621860000208
中第l行、第m列的元素，则子IMU航向角ψs、俯仰角θs和横滚角γs的主值，即ψs主、θs主和γs主分别为：
Figure FDA0002290621860000209
θs主＝arcsin(T32)
Figure FDA00022906218600002010
Figure FDA00022906218600002011
Figure FDA00022906218600002012
θs＝θs主，
Figure FDA0002290621860000211
CN201810153926.2A 2018-02-22 2018-02-22 一种机载分布式pos的实时传递对准的方法及装置 Expired - Fee Related CN108303120B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810153926.2A CN108303120B (zh) 2018-02-22 2018-02-22 一种机载分布式pos的实时传递对准的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810153926.2A CN108303120B (zh) 2018-02-22 2018-02-22 一种机载分布式pos的实时传递对准的方法及装置

Publications (2)

Publication Number Publication Date
CN108303120A CN108303120A (zh) 2018-07-20
CN108303120B true CN108303120B (zh) 2020-03-24

Family

ID=62848609

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810153926.2A Expired - Fee Related CN108303120B (zh) 2018-02-22 2018-02-22 一种机载分布式pos的实时传递对准的方法及装置

Country Status (1)

Country Link
CN (1) CN108303120B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110044385B (zh) * 2019-05-09 2020-12-08 北京壹氢科技有限公司 一种大失准角情况下的快速传递对准方法
CN111239718B (zh) * 2020-01-17 2022-11-15 电子科技大学 一种基于单星辐射源的多基站目标定位方法
CN112525191B (zh) * 2021-02-08 2021-06-08 北京航空航天大学 一种基于相对捷联解算的机载分布式pos传递对准方法
CN113074753A (zh) * 2021-03-19 2021-07-06 南京天巡遥感技术研究院有限公司 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103913181A (zh) * 2014-04-24 2014-07-09 北京航空航天大学 一种基于参数辨识的机载分布式pos传递对准方法
CN104655152A (zh) * 2015-02-11 2015-05-27 北京航空航天大学 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN106352876A (zh) * 2016-07-25 2017-01-25 北京航空航天大学 一种基于h∞和ckf混合滤波的机载分布式pos传递对准方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102865881B (zh) * 2012-03-06 2014-12-31 武汉大学 一种惯性测量单元的快速标定方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103913181A (zh) * 2014-04-24 2014-07-09 北京航空航天大学 一种基于参数辨识的机载分布式pos传递对准方法
CN104655152A (zh) * 2015-02-11 2015-05-27 北京航空航天大学 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN106352876A (zh) * 2016-07-25 2017-01-25 北京航空航天大学 一种基于h∞和ckf混合滤波的机载分布式pos传递对准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
机载分布式POS传递对准建模与仿真;房建成等;《中国惯性技术学报》;20120815(第04期);379-385 *

Also Published As

Publication number Publication date
CN108303120A (zh) 2018-07-20

Similar Documents

Publication Publication Date Title
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN106289246B (zh) 一种基于位置和姿态测量系统的柔性杆臂测量方法
Fang et al. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation
CN108387227B (zh) 机载分布式pos的多节点信息融合方法及系统
CN103913181B (zh) 一种基于参数辨识的机载分布式pos传递对准方法
CN110780326A (zh) 一种车载组合导航系统和定位方法
CN108303120B (zh) 一种机载分布式pos的实时传递对准的方法及装置
US6859727B2 (en) Attitude change kalman filter measurement apparatus and method
CN110926468B (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN102353378B (zh) 一种矢量形式信息分配系数的组合导航系统自适应联邦滤波方法
CN102538792A (zh) 一种位置姿态系统的滤波方法
Ahn et al. Fast alignment using rotation vector and adaptive Kalman filter
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN102621565A (zh) 一种机载分布式pos的传递对准方法
CN104655152A (zh) 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN104698486A (zh) 一种分布式pos用数据处理计算机系统实时导航方法
CN105091907A (zh) Sins/dvl组合中dvl方位安装误差估计方法
US9243914B2 (en) Correction of navigation position estimate based on the geometry of passively measured and estimated bearings to near earth objects (NEOS)
CN112325886B (zh) 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿系统
CN104977004A (zh) 一种激光惯组与里程计组合导航方法及系统
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20200324