CN102519470B - 多级嵌入式组合导航***及导航方法 - Google Patents
多级嵌入式组合导航***及导航方法 Download PDFInfo
- Publication number
- CN102519470B CN102519470B CN201110407694.7A CN201110407694A CN102519470B CN 102519470 B CN102519470 B CN 102519470B CN 201110407694 A CN201110407694 A CN 201110407694A CN 102519470 B CN102519470 B CN 102519470B
- Authority
- CN
- China
- Prior art keywords
- state
- navigation
- inertia
- error
- satellites
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 17
- 238000005259 measurement Methods 0.000 claims abstract description 65
- 238000012937 correction Methods 0.000 claims abstract description 19
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 230000004927 fusion Effects 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 43
- 238000004364 calculation method Methods 0.000 claims description 36
- 238000005530 etching Methods 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000017105 transposition Effects 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims 2
- 230000002093 peripheral effect Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 238000013459 approach Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
本发明公布了一种多级嵌入式组合导航***及导航方法,属于组合导航技术领域。该方法包括以下步骤:首先建立了航空机载惯性导航***惯导误差状态量方程和惯性/卫星组合量测模型,随后定义了嵌入式惯性/卫星组合***的导航误差状态量并建立惯性/星敏感器组合量测模型;在此基础上,进行了嵌入式惯性/卫星组合***内部组合导航解算及闭环滤波修正,最后进行外部惯性/星敏感器组合导航解算及开环滤波修正,获得多信息融合导航结果。本方法能够在保留嵌入式惯性/卫星组合***完整独立集成结构的同时,实现惯性/卫星/天文异步多传感器信息的多级融合和对惯导的分步校正,有效提高综合导航性能,适合工程应用。
Description
技术领域
本发明公布了一种多级嵌入式组合导航***及导航方法,属于组合导航技术领域。
背景技术
近年来,由惯性导航***、卫星导航接收机和卡尔曼滤波计算单元构成的嵌入式惯性/卫星组合导航***工程化产品已日渐成熟,并在机载和弹载等环境下得到了广泛应用。惯性/卫星组合导航***根据卫星导航接收机的位置、速度测量信息,通过滤波算法对惯导误差进行在线估计和校正,能够获得较高的位置和速度测量精度;在缺乏对载体姿态直接测量的情况下,惯性/卫星组合导航***只能通过间接观测修正陀螺漂移,输出的姿态精度还较为有限。
随着新型飞行器的研制,对导航***性能要求的日益提高,特别是对于执行遥感、测绘及侦查任务等的飞行器,对其姿态测量精度均提出了较高的要求,需要进一步提高嵌入式惯性/卫星组合***的姿态测量精度。
在目前已有的姿态测量设备中,星敏感器在测量精度方面具有突出的优势。它通过探测天球上不同位置的恒星,提供飞行器的三轴姿态,目前星敏感器的指向精度和姿态测量精度最高可达到角秒的量级,同时能够克服惯性陀螺姿态测量精度随时间漂移的缺点。
因此,研究在既有的嵌入式惯性/卫星组合***基础上,基于星敏感器进行姿态量测的多级嵌入式组合导航***及导航方法,将能够有效地提高姿态测量精度,将具有突出的应用价值。
发明内容
技术问题:
嵌入式惯性/卫星组合***根据卫星导航的位置、速度量测信息,通过滤波算法对惯导误差进行在线估计和校正,能够获得较高的位置和速度测量精度;在缺乏对载体姿态直接测量的情况下,嵌入式惯性/卫星组合***只能通过间接观测修正平台误差角和陀螺漂移,输出的姿态精度还较为有限。在组合导航***中采用星敏感器进行姿态量测,将能够有效地提高姿态测量精度,但同时嵌入式惯性/卫星组合***内已存在独立工作的组合滤波和惯导闭环校正回路,需要维持其完整性。
本发明目的在于克服嵌入式惯性/卫星组合***姿态精度低的不足,提供了以嵌入式惯性/卫星组合***为基础,外加星敏感器实现多级嵌入式组合导航的方法。
技术方案:
本发明多级嵌入式组合导航***及方法,为达到上述发明目的,通过以下方案实现:
本发明所述多级嵌入式组合导航***,其特征在于由内部传感器部分、内部组合导航解算部分、外部传感器部分、外部组合导航解算部分构成;所述内部传感器部分由惯性测量单元和卫星导航接收机组成;所述内部组合导航解算部分由惯性导航解算模块、误差补偿A模块、状态预测A模块、状态估计A模块组成;所述外部传感器部分由星敏感器组成;所述外部组合导航解算部分由协方差转换模块、状态估计B模块、误差补偿B模块组成;惯性/卫星/天文多级嵌入式组合导航***中,惯性测量单元的输出端接误差补偿A模块的输入端,卫星导航接收机的输出端接状态估计A模块的输入端;误差补偿A模块的输出端接惯性导航解算模块的输入端,惯性导航解算模块的输出端接状态估计A模块、状态预测A模块、状态估计B模块和误差补偿B模块的输入端,状态预测A模块的输出端接状态估计A模块和协方差转化模块的输入端,状态估计A模块的输出端接误差补偿A模块和协方差转换模块的输入端;协方差转换模块的输出端接状态估计B模块的输入端、状态估计B模块的输出端接误差补偿B模块的输入端,误差补偿B模块的输出端输出导航结果。
本发明所述多级嵌入式组合导航方法,其特征在于包括以下步骤:
(1)选取东北天地理坐标系,定义航空机载惯性导航***惯导误差状态量为:
φE,φN,φU分别表示航空机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;δvE1,δvN1,δvU1分别表示航空机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量;δL1,δλ1,δh1分别表示航空机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量;εbx1,εby1,εbz1,εrx1,εry1,εrz1分别表示航空机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;分别表示航空机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏,T为转置;
建立航空机载惯性导航***惯导误差状态方程,如式(15)所示
X1(k)=Φ1(k,k-1)X1(k-1)+T1(k,k-1)W1(k-1) (1)
其中,Φ1(k,k-1)为tk-1时刻至tk时刻***的状态转移矩阵,W1(k-1)为tk时刻***的噪声矢量,Γ1(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵;X1(k-1)为tk-1时刻航空机载惯性导航***误差状态量,X1(k)为tk时刻航空机载惯性导航***误差状态量;
(2)采用地理系下位置和速度线性化观测原理,建立地理系下卫星导航位置速度观测量和被估计的步骤(1)所述的惯性导航***误差状态量之间的线性化量测方程,作为状态估计A模块的惯性/卫星组合量测模型;
状态估计A模块的量测值为惯导***输出的位置、速度和卫星导航接收机输出的位置、速度的差值,可以建立如下形式的状态估计A模块的量测方程
Z1(k)=H1(k)X1(k)+VG(k) (2)
其中 vI(k)为tk时刻惯导输出的飞行器速度矢量,pI(k)为tk时刻惯导输出的飞行器位置矢量,vG(k)为tk时刻卫星导航接收机输出的飞行器速度矢量,pG(k)为tk时刻卫星导航接收机输出的飞行器位置矢量;
H1(k)为tk时刻速度位置量测矩阵;
(3)选取东北天地理坐标系,定义内部组合导航解算部分的惯性/卫星组合导航误差状态量X2
X2=[φE2,φN2,φU2,εbx2,εby2,εbz2,εrx2,εry2,εrz2]T
φE2,φN2,φU2分别表示内部组合导航解算部分误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;εbx2,εby2,εbz2,εrx2,εry2,εrz2分别表示航空机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;
(4)采用地理系下姿态线性化观测原理,建立地理系下星敏感器姿态观测量和被估计的步骤(3)所述的内部组合导航解算部分误差状态量的之间的线性化量测方程,作为状态估计B模块惯性/星敏感器组合量测模型;
根据星敏感器姿态量测信息与惯性导航***误差状态量之间的关系,建立状态估计B模块的量测方程
Z2(k)=H2(k)X2(k)+Vs(k) (3)
其中Z2(k)=aI(k)-aS(k)
aI(k)为惯导输出的飞行器速度矢量,aS(k)为星敏感器输出的飞行器位置矢量,H2(k)为姿态量测矩阵,VS(k)为星敏感器的姿态量测噪声向量,X2(k)为tk时刻内部组合导航解算部分的惯性/卫星组合导航误差状态量;
(5)内部组合导航解算部分使用步骤(1)的惯性导航误差状态量方程和步骤(2)的惯性/卫星组合滤波模型进行惯性/卫星组合滤波,输出惯性/卫星组合导航结果及协方差信息到外部组合导航解算部分;
(6)外部组合导航解算部分接收步骤(5)的惯性/卫星组合导航结果及协方差信息,外部组合导航解算部分使用步骤(4)的惯性/星敏感器组合滤波模型进行惯性/星敏感器组合滤波,输出导航结果;
有益效果:
本发明能够在保留嵌入式惯性/卫星组合***完整独立集成结构的同时,实现外加星敏感器构成惯性/卫星/天文多级嵌入式组合导航***,和未采用本发明的嵌入式惯性/卫星组合***相比,采用本发明的姿态误差可以显著减小,本发明可以获得高精度的姿态、速度和位置信息,适合工程应用。
附图说明
图1为本发明的惯性/卫星/天文多级嵌入式组合导航***结构图;
图2为仿真飞行航迹图;
图3惯性/卫星组合导航和本发明的惯性/卫星/天文多级嵌入式组合导航横滚角误差曲线对比图;
图4惯性/卫星组合导航和本发明的惯性/卫星/天文多级嵌入式组合导航俯仰角误差曲线对比图;
图5惯性/卫星组合导航和本发明的惯性/卫星/天文多级嵌入式组合导航航向角误差曲线对比图;
图6为本发明的惯性/卫星/天文多级嵌入式组合导航方法的工作流程图。
具体实施方式
下面结合附图对发明的技术方案进行详细说明:
如图1所示,本发明所述惯性/卫星/天文多级嵌入式组合导航***,其特征在于由内部传感器部分、内部组合导航解算部分、外部传感器部分、外部组合导航解算部分构成。
所述内部传感器部分由惯性测量单元和卫星导航接收机组成;所述内部组合导航解算部分由惯性导航解算模块、误差补偿A模块、状态预测A模块、状态估计A模块组成;所述外部传感器部分由星敏感器组成;所述外部组合导航解算部分由协方差转换模块、状态估计B模块、误差补偿B模块组成;
惯性/卫星/天文多级嵌入式组合导航***中,惯性测量单元的输出端接误差补偿A模块的输入端,卫星导航接收机的输出端接状态估计A模块的输入端;误差补偿A模块的输出端接惯性导航解算模块的输入端,惯性导航解算模块的输出端接状态估计A模块、状态预测A模块、状态估计B模块和误差补偿B模块的输入端,状态预测A模块的输出端接状态估计A模块和协方差转化模块的输入端,状态估计A模块的输出端接误差补偿A模块和协方差转换模块的输入端;协方差转换模块的输出端接状态估计B模块的输入端、状态估计B模块的输出端接误差补偿B模块的输入端,误差补偿B模块的输出端输出导航结果。
本发明所述惯性/卫星/天文多级嵌入式组合导航方法的原理是:从机载地理系导航的角度入手,分析了嵌入式惯性/卫星组合***的工作流程和特点,在嵌入式惯性/卫星组合***为既有的集成***,其内部的内层卡尔曼滤波器独立完成惯导和卫星导航信息的组合,并在此基础上对惯导进行闭环反馈校正,获得嵌入式惯性/卫星组合***的导航输出。在此基础上,嵌入式惯性/卫星组合***的外部构建的外层滤波器与内层滤波器协同工作,进一步融合天文姿态敏感器的量测信息,并通过开环校正的方式修正嵌入式惯性/卫星组合***导航输出中的姿态误差,获得多信息融合导航结果,获得高精度的姿态、速度和位置信息。
以下结合附图具体叙述本发明实施方法的过程:
1.建立航空机载惯性导航***惯导误差状态量方程
选取东北天地理坐标系,定义航空机载惯性导航***惯导误差状态量为:
φE,φN,φU分别表示航空机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;δvE1,δvN1,δvU1分别表示航空机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量;δL1,δλ1,δh1分别表示航空机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量;εbx1,εby1,εbz1,εrx1,εry1,εrz1分别表示航空机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;分别表示航空机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏,T为转置;
建立航空机载惯性导航***惯导误差状态方程,如式(18)所示
X1(k)=Φ1(k,k-1)X1(k-1)+Γ1(k,k-1)W1(k-1) (4)
其中,Φ1(k,k-1)为tk-1时刻至tk时刻***的状态转移矩阵,W1(k-1)为tk时刻***的噪声矢量,Γ1(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵;X1(k-1)为tk-1时刻航空机载惯性导航***误差状态量,X1(k)为tk时刻航空机载惯性导航***误差状态量;
2.建立惯性/卫星组合量测模型
采用地理系下位置和速度线性化观测原理,建立地理系下卫星导航位置速度观测量和被估计惯性导航***误差状态量之间的线性化量测方程,作为状态估计A模块的惯性/卫星组合量测模型;
状态估计A模块的量测值为惯导***输出的位置、速度和卫星导航接收机输出的位置、速度的差值,可以建立如下形式的状态估计A模块的量测方程
Z1(k)=H1(k)X1(k)+VG(k) (5)
其中 vI(k)为tk时刻惯导输出的飞行器速度矢量,pI(k)为tk时刻惯导输出的飞行器位置矢量,vG(k)为tk时刻卫星导航接收机输出的飞行器速度矢量,pG(k)为tk时刻卫星导航接收机输出的飞行器位置矢量;
H1(k)为tk时刻速度位置量测矩阵;
3.定义嵌入式惯性/卫星组合***的导航误差状态量
根据图1给出的惯性/卫星/天文多级嵌入式组合导航***结构图,外部组合导航解算部分的状态估计,实质上是通过星敏感器的测量来估计嵌入式惯性/卫星组合***导航结果中的误差状态量。因此,基于惯性/星敏感器组合可观测性的分析,取式(20)中的平台误差角和陀螺误差状态作为嵌入式惯性/卫星组合***的导航误差状态量,定义内部组合导航解算部分的惯性/卫星组合导航误差状态量X2
X2=[φE2,φN2,φU2,εbx2,εby2,εbz2,εrx2,εry2,εrz2]T (6)
其中,φE2,φN2,φU2分别表示内部组合导航解算部分误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;εbx2,εby2,εbz2,εrx2,εry2,εrz2分别表示航空机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;
4.建立惯性/星敏感器组合量测模型
采用地理系下姿态线性化观测原理,建立地理系下星敏感器姿态观测量和被估计的内部组合导航解算部分误差状态量的之间的线性化量测方程,作为状态估计B模块惯性/星敏感器组合量测模型;
根据星敏感器姿态量测信息与惯性导航***误差状态量之间的关系,建立状态估计B模块的量测方程
Z2(k)=H2(k)X2(k)+VS(k) (7)
其中Z2(k)=aI(k)-aS(k)
aI(k)为惯导输出的飞行器速度矢量,aS(k)为星敏感器输出的飞行器位置矢量,H2(k)为姿态量测矩阵,VS(k)为星敏感器的姿态量测噪声向量,X2(k)为tk时刻内部组合导航解算部分的惯性/卫星组合导航误差状态量;
5.进行嵌入式惯性/卫星组合***内部组合导航解算及滤波修正
内部组合导航解算部分使用惯性导航误差状态量方程和惯性/卫星组合滤波模型进行惯性/卫星组合滤波,输出惯性/卫星组合导航结果及协方差信息到外部组合导航解算部分,具体按照以下子步骤进行:
5.1)惯导***进行解算,解出惯导导航结果;
5.2)状态预测A模块按照式错误!未找到引用源。进行惯性/卫星组合滤波中惯导误差状态量和协方差信息的时间更新;
P1(k,k-1)=Φ1(k,k-1)P1(k-1,k-1)Φ1(k,k-1)T (8)
+Γ1(k,k-1)Q1(k)Γ1(k,k-1)T
其中P1(k,k-1)为tk-1时刻至tk时刻惯性/卫星组合滤波一步预测协方差矩阵,P1(k-1,k-1)为tk-1时刻惯性/卫星组合滤波状态估计协方差矩阵,Q1(k)为tk时刻***的噪声协方差矩阵;
5.3)判断是否有新的卫星导航测量信息:如果是,则执行子步骤5.4);如果否,则按照式错误!未找到引用源。置协方差阵暂存值,并执行子步骤5.5);
M=P1(k,k-1) (9)
5.4)状态估计A模块接收新的卫星导航测量信息,按照式(24)、式(25)、式(26)对子步骤5.2)部分的惯导误差状态量和协方差信息的量测更新,误差补偿A模块用状态估计A模块估计出的惯性导航***的估计误差状态量X1(k,k)对惯导进行闭环校正;
K1(k)=P1(k,k-1)H1(k)T[H1(k)P1(k,k-1)H1(k)T+R1(k)]-1 (10)
X1(k)=K1(k)Z1(k) (11)
P1(k,k)=[I-K1(k)H1(k)]P1(k,k-1) (12)
置协方差阵暂存值
M=P1(k,k) (13)
其中K1(k)为tk时刻惯性/卫星组合滤波增益矩阵,H1(k)为tk时刻速度位置量测矩阵,R1(k)为tk时刻速度位置量测噪声协方差阵,P1(k,k)为tk时刻惯性/卫星组合滤波状态估计协方差矩阵,-1为求逆;
误差补偿A模块用估计出的惯性导航的估计误差状态量X1(k)对子步骤5.1)的惯性导航结果进行开环校正。
5.5)将惯导导航结果作为惯性/卫星组合***导航结果输出到外部组合导航解算部分,将协方差信息M输出到协方差转换模块,返回子步骤5.1);
信息M存储到协方差阵暂存单元,返回子步骤5.1)。
6.进行外部惯性/星敏感器组合导航解算及滤波修正
外部组合导航解算部分接收惯性/卫星组合导航结果及协方差信息,外部组合导航解算部分使用惯性/星敏感器组合滤波模型进行惯性/星敏感器组合滤波,输出导航结果,具体包括如下子步骤:
6.1)协方差转换模块接收惯性/卫星组合导航结果及协方差信息M,并将M按照维数关系分解为式(28)的形式
其中n×n代表矩阵的维数为n×n维;
协方差转换模块在式(14)的基础上,按照式(29)进行协方差转换,并输出到状态估计B模块
6.2)判断是否有新的星敏感器测量信息:如果是,则执行子步骤6.3);如果否,则执行子步骤6.4);
6.3)状态估计B模块接收新的星敏感器测量信息,按照式(30)、式(31)、式(32)对子步骤6.1)部分的协方差信息的量测更新;
协方差赋值
P2(k)=N (16)
计算外层滤波增益矩阵
K2(k)=P2(k)H2(k)T[H2(k)P2(k)H2(k)T+R2(k)]-1 (17)
计算惯性导航误差状态量估计值
X2(k)=K2(k)Z2(k) (18)
其中P2(k)为tk时刻惯性/星敏感器组合滤波状态估计协方差矩阵,K2(k)为tk时刻惯性/星敏感器组合滤波增益矩阵,H2(k)为tk时刻姿态量测矩阵,R2(k)为tk时刻姿态量测噪声协方差阵;
误差补偿B模块用估计出的惯性/卫星组合导航的估计误差状态量X2(k)对5.部分的惯性/卫星组合导航结果进行开环校正。
6.4)将步骤6.3)开环校正后的惯性/卫星组合导航结果作为多信息融合导航结果输出,返回子步骤6.1)。
为了验证发明所提出的惯性/卫星/天文多级嵌入式组合导航***和方法的性能,本文分别对未采用本发明方法的惯性/卫星组合***和本发明的***方法进行了对比,图2为验证时采用的飞行航迹,两种情况下姿态误差曲线分别如图3~图5所示。
通过图3~图5的仿真结果可以看出,本发明能够有效提高基于嵌入式惯性/卫星组合***的姿态精度。同时本发明在保留嵌入式惯性/卫星组合***完整独立集成结构和独立工作流程的同时,实现星敏感器对导航姿态的辅助校正,提高导航精度,具有有益的工程应用价值。
本发明的惯性/卫星/天文多级嵌入式组合导航***和方法工作流程如图6所示。
Claims (3)
1.一种多级嵌入式组合导航方法,其特征在于包括以下步骤:
(1)选取东北天地理坐标系,航空机载惯性导航***惯导误差状态量为:
分别表示航空机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;δvE1,δvN1,δvU1分别表示航空机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量;δL1,δλ1,δh1分别表示航空机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量;εbx1,εby1,εbz1,εrx1,εry1,εrz1分别表示航空机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;分别表示航空机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏,T为转置;
建立航空机载惯性导航***惯导误差状态方程,如式(1)所示
X1(k)=Φ1(k,k-1)X1(k-1)+Γ1(k,k-1)W1(k-1) (1)
其中,Φ1(k,k-1)为tk-1时刻至tk时刻***的状态转移矩阵,W1(k-1)为tk时刻***的噪声矢量,Γ1(k,k-1)为tk-1时刻至tk时刻***的噪声驱动矩阵;X1(k-1)为tk-1时刻航空机载惯性导航***误差状态量,X1(k)为tk时刻航空机载惯性导航***误差状态量;
(2)采用地理系下位置和速度线性化观测原理,建立地理系下卫星导航位置速度观测量和被估计的步骤(1)所述的惯性导航***误差状态量之间的线性化量测方程,作为状态估计A模块的惯性/卫星组合量测模型;
状态估计A模块的量测值为惯导***输出的位置、速度和卫星导航接收机输出的位置、速度的差值,建立如下形式的状态估计A模块的量测方程:
Z1(k)=H1(k)X1(k)+VG(k) (2)
其中 vI(k)为tk时刻惯导输出的飞行器速度矢量,pI(k)为tk时刻惯导输出的飞行器位置矢量,vG(k)为tk时刻卫星导航接收机输出的飞行器速度矢量,pG(k)为tk时刻卫星导航接收机输出的飞行器位置矢量;
H1(k)为tk时刻速度位置量测矩阵;
(3)选取东北天地理坐标系,定义内部组合导航解算部分的惯性/卫星组合导航误差状态量X2:
X2=[φE2,φN2,φU2,εbx2,εby2,εbz2,εrx2,εry2,εrz2]T
φE2,φN2,φU2分别表示内部组合导航解算部分误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量;εbx2,εby2,εbz2,εrx2,εry2,εrz2分别表示航空机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量;
(4)采用地理系下姿态线性化观测原理,建立地理系下星敏感器姿态观测量和被估计的步骤(3)所述的内部组合导航解算部分误差状态量的之间的线性化量测方程,作为状态估计B模块惯性/星敏感器组合量测模型;
根据星敏感器姿态量测信息与惯性导航***误差状态量之间的关系,建立状态估计B模块的量测方程:
Z2(k)=H2(k)X2(k)+VS(k) (3)
其中Z2(k)=aI(k)-aS(k)
aI(k)为惯导输出的飞行器速度矢量,aS(k)为星敏感器输出的飞行器位置矢量,H2(k)为姿态量测矩阵,VS(k)为星敏感器的姿态量测噪声向量,X2(k)为tk时刻内部组合导航解算部分的惯性/卫星组合导航误差状态量;
(5)内部组合导航解算部分使用步骤(1)的惯性导航误差状态量方程和步骤(2)的惯性/卫星组合滤波模型进行惯性/卫星组合滤波,输出惯性/卫星组合导航结果及协方差信息到外部组合导航解算部分;
(6)外部组合导航解算部分接收步骤(5)的惯性/卫星组合导航结果及协方差信息,外部组合导航解算部分使用步骤(4)的惯性/星敏感器组合滤波模型进行惯性/星敏感器组合滤波,输出导航结果。
2.根据权利要求1所述的多级嵌入式组合导航方法,其特征在于步骤(5)中内部组合导航解算部分的惯性/卫星组合滤波包含如下子步骤:
5.1)惯导***进行解算,解出惯导导航结果;
5.2)状态预测A模块按照式(4)进行惯性/卫星组合滤波中惯导误差状态量和协方差信息的时间更新;
其中P1(k,k-1)为tk-1时刻至tk时刻惯性/卫星组合滤波一步预测协方差矩阵,P1(k-1,k-1)为tk-1时刻惯性/卫星组合滤波状态估计协方差矩阵,Q1(k)为tk时刻***的噪声协方差矩阵;
5.3)判断是否有新的卫星导航测量信息:如果是,则执行子步骤5.4);如果否,则按照式(5)置协方差阵暂存值,并执行子步骤5.5);
M=P1(k,k-1) (5)
5.4)状态估计A模块接收新的卫星导航测量信息,按照式(6)、式(7)、式(8)对子步骤5.2)部分的惯导误差状态量和协方差信息的量测更新,误差补偿A模块用状态估计A模块估计出的惯性导航***的估计误差状态量X1(k,k)对惯导进行闭环校正;
K1(k)=P1(k,k-1)H1(k)T[H1(k)P1(k,k-1)H1(k)T+R1(k)]-1 (6)
X1(k)=K1(k)Z1(k) (7)
P1(k,k)=[I-K1(k)H1(k)]P1(k,k-1) (8)
置协方差阵暂存值
M=P1(k,k) (9)
其中Κ1(k)为tk时刻惯性/卫星组合滤波增益矩阵,H1(k)为tk时刻速度位置量测矩阵,R1(k)为tk时刻速度位置量测噪声协方差阵,P1(k,k)为tk时刻惯性/卫星组合滤波状态估计协方差矩阵,-1为求逆;
误差补偿A模块用估计出的惯性导航的估计误差状态量X1(k)对子步骤5.1)的惯性导航结果进行开环校正;
5.5)将惯导导航结果作为惯性/卫星组合***导航结果输出到外部组合导航解算部分,将协方差信息M输出到协方差转换模块,返回子步骤5.1)。
3.根据权利要求1所述的多级嵌入式组合导航方法,其特征在于步骤(6)中外部组合导航解算部分的惯性/星敏感器组合滤波包含如下子步骤:
6.1)协方差转换模块接收子骤(4)中输出的惯性/卫星组合导航结果及协方差信息M,并将M按照维数关系分解为式(10)的形式
其中n×n代表矩阵的维数为n×n维;
协方差转换模块在式(10)的基础上,按照式(11)进行协方差转换,并输出到状态估计B模块
6.2)判断是否有新的星敏感器测量信息:如果是,则执行子步骤6.3);如果否,则执行子步骤6.4);
6.3)状态估计B模块接收新的星敏感器测量信息,按照式(12)、式(13)、式(14)对子步骤6.1)部分的协方差信息的量测更新;
协方差赋值
P2(k)=N (12)
计算外层滤波增益矩阵
K2(k)=P2(k)H2(k)T[H2(k)P2(k)H2(k)T+R2(k)]-1 (13)
计算惯性导航误差状态量估计值
X2(k)=K2(k)Z2(k) (14)
其中P2(k)为tk时刻惯性/星敏感器组合滤波状态估计协方差矩阵,Κ2(k)为tk时刻惯性/星敏感器组合滤波增益矩阵,H2(k)为tk时刻姿态量测矩阵,R2(k)为tk时刻姿态量测噪声协方差阵;
误差补偿B模块用估计出的惯性/卫星组合导航的估计误差状态量X2(k)对子步骤6.1)的惯性/卫星组合导航结果进行开环校正;
6.4)将步骤6.3)开环校正后的惯性/卫星组合导航结果作为多信息融合导航结果输出,返回子步骤6.1)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201110407694.7A CN102519470B (zh) | 2011-12-09 | 2011-12-09 | 多级嵌入式组合导航***及导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201110407694.7A CN102519470B (zh) | 2011-12-09 | 2011-12-09 | 多级嵌入式组合导航***及导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102519470A CN102519470A (zh) | 2012-06-27 |
CN102519470B true CN102519470B (zh) | 2014-05-07 |
Family
ID=46290473
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201110407694.7A Active CN102519470B (zh) | 2011-12-09 | 2011-12-09 | 多级嵌入式组合导航***及导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102519470B (zh) |
Families Citing this family (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103047999B (zh) * | 2012-12-18 | 2015-09-30 | 东南大学 | 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 |
CN103063216B (zh) * | 2013-01-06 | 2015-08-12 | 南京航空航天大学 | 一种基于星像坐标建模的惯性与天文组合导航方法 |
CN103256928B (zh) * | 2013-04-28 | 2015-05-20 | 南京航空航天大学 | 一种分布式惯性导航***及其姿态传递对准方法 |
CN105737823B (zh) * | 2016-02-01 | 2018-09-21 | 东南大学 | 一种基于五阶ckf的gps/sins/cns组合导航方法 |
JP6677533B2 (ja) * | 2016-03-01 | 2020-04-08 | クラリオン株式会社 | 車載装置、及び、推定方法 |
CN108613674A (zh) * | 2018-03-25 | 2018-10-02 | 哈尔滨工程大学 | 一种基于自适应差分进化bp神经网络的姿态误差抑制方法 |
CN110132267B (zh) * | 2019-05-10 | 2021-06-08 | 上海航天控制技术研究所 | 空天地一体化飞行器的光纤惯导***及光纤惯导在轨对准方法 |
CN110285804B (zh) * | 2019-06-26 | 2022-06-17 | 南京航空航天大学 | 基于相对运动模型约束的车辆协同导航方法 |
CN110780326A (zh) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | 一种车载组合导航***和定位方法 |
CN111121766B (zh) * | 2019-12-17 | 2023-07-07 | 南京理工大学 | 一种基于星光矢量的天文与惯性组合导航方法 |
CN111625010B (zh) * | 2020-04-28 | 2023-04-14 | 北京控制工程研究所 | 一种基于组合滤波的航天器三超近零误差跟踪控制方法 |
CN111781626B (zh) * | 2020-08-09 | 2023-08-18 | 湖南星至导航科技有限公司 | 多功能导航***与方法 |
CN112269200B (zh) * | 2020-10-14 | 2024-05-17 | 北京航空航天大学 | 一种基于可观测度的惯性/卫星***自适应混合校正方法 |
CN112304309B (zh) * | 2020-10-21 | 2022-10-04 | 西北工业大学 | 一种基于心动阵列的高超飞行器组合导航信息解算方法 |
CN112648993B (zh) * | 2021-01-06 | 2022-07-15 | 北京自动化控制设备研究所 | 一种多源信息融合组合导航***及方法 |
CN116380148B (zh) * | 2023-04-06 | 2023-11-10 | 中国人民解放军93209部队 | 多传感器目标跟踪***的两级时空误差标校方法及装置 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20020008661A1 (en) * | 2000-07-20 | 2002-01-24 | Mccall Hiram | Micro integrated global positioning system/inertial measurement unit system |
US6408245B1 (en) * | 2000-08-03 | 2002-06-18 | American Gnc Corporation | Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit |
CN100356139C (zh) * | 2005-01-21 | 2007-12-19 | 清华大学 | 用于微小卫星的微型组合姿态测量*** |
CN101706281B (zh) * | 2009-11-13 | 2011-08-31 | 南京航空航天大学 | 惯性/天文/卫星高精度组合导航***及其导航方法 |
CN101788296B (zh) * | 2010-01-26 | 2011-11-16 | 北京航空航天大学 | 一种sins/cns深组合导航***及其实现方法 |
-
2011
- 2011-12-09 CN CN201110407694.7A patent/CN102519470B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN102519470A (zh) | 2012-06-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102519470B (zh) | 多级嵌入式组合导航***及导航方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN106289246B (zh) | 一种基于位置和姿态测量***的柔性杆臂测量方法 | |
CN102608596B (zh) | 一种用于机载惯性/多普勒雷达组合导航***的信息融合方法 | |
CN102809377B (zh) | 飞行器惯性/气动模型组合导航方法 | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN101788296B (zh) | 一种sins/cns深组合导航***及其实现方法 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN103235328B (zh) | 一种gnss与mems组合导航的方法 | |
CN110780326A (zh) | 一种车载组合导航***和定位方法 | |
CN102830414B (zh) | 一种基于sins/gps的组合导航方法 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN101858748A (zh) | 高空长航无人机的多传感器容错自主导航方法 | |
CN109781098B (zh) | 一种列车定位的方法和*** | |
CN104236586B (zh) | 基于量测失准角的动基座传递对准方法 | |
CN103076015A (zh) | 一种基于全面最优校正的sins/cns组合导航***及其导航方法 | |
CN102169184A (zh) | 组合导航***中测量双天线gps安装失准角的方法和装置 | |
CN111189442B (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN103364817B (zh) | 一种基于r-t-s平滑的pos***双捷联解算后处理方法 | |
Nguyen | Loosely coupled GPS/INS integration with Kalman filtering for land vehicle applications | |
CN114739425A (zh) | 基于rtk-gnss及全站仪的采煤机定位标定***及应用方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN112525204B (zh) | 一种航天器惯性和太阳多普勒速度组合导航方法 | |
CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |