CN112461236B - 一种车载高精度容错组合导航方法及系统 - Google Patents

一种车载高精度容错组合导航方法及系统 Download PDF

Info

Publication number
CN112461236B
CN112461236B CN202011319293.1A CN202011319293A CN112461236B CN 112461236 B CN112461236 B CN 112461236B CN 202011319293 A CN202011319293 A CN 202011319293A CN 112461236 B CN112461236 B CN 112461236B
Authority
CN
China
Prior art keywords
error
equation
fault
strapdown inertial
vehicle
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
CN202011319293.1A
Other languages
English (en)
Other versions
CN112461236A (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.)
Rocket Force University of Engineering of PLA
Original Assignee
Rocket Force University of Engineering of PLA
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 Rocket Force University of Engineering of PLA filed Critical Rocket Force University of Engineering of PLA
Priority to CN202011319293.1A priority Critical patent/CN112461236B/zh
Publication of CN112461236A publication Critical patent/CN112461236A/zh
Application granted granted Critical
Publication of CN112461236B publication Critical patent/CN112461236B/zh
Active 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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种车载高精度容错组合导航方法及系统。该方法包括：基于捷联惯导系统的姿态输出与里程计的路程增量输出、多普勒雷达的速度输出，分别设计地理坐标系下载车位置解算算法和速度解算算法；研究里程计与多普勒雷达的误差模型，分别建立前述位置解算和速度解算的误差模型；采用间接法滤波进行组合导航设计，以捷联惯导系统误差、里程计误差、多普勒雷达误差等作为系统状态，将捷联惯导输出的位置、速度与前述位置解算、速度解算获得的对应信息分别相减作为量测，采用卡尔曼滤波设计高精度容错组合导航的滤波算法。本发明能够实现车辆精确导航，在里程计或多普勒雷达某一设备出现故障时仍保持较高的导航精度。

Description

一种车载高精度容错组合导航方法及系统
技术领域
本发明涉及车载导航定位技术领域，特别是涉及一种车载高精度容错组合导航方法及系统。
背景技术
现代军用及民用特殊车辆通常需要具备高精度、高可靠性的导航定位能力，而且在一些复杂恶劣环境下能够具有较高的自主性和较强的抗干扰能力。
目前，国内外用于中高精度车载导航的系统通常是基于惯性导航系统。众所周知，惯性导航系统具有绝对的自主性和抗干扰能力，非常适用于各种复杂恶劣环境下进行导航定位。但是，惯性导航系统的导航误差随时间不断积累，无法完成长时间高精度导航。目前，在很多车载导航系统中，采用卫星导航系统与惯性导航系统构成车载惯性/卫星组合导航系统，虽然实现了高精度导航，但由于卫星信号易被干扰或屏蔽，因此惯性/卫星组合导航的可靠性不高、抗干扰能力较弱。此外，部分车载导航系统中采用了惯性/里程计组合导航模式，这种方式精度高、自主性强、抗干扰性好，但是里程计一旦出现故障而无法正常输出，此时仅靠惯性导航系统本身根本无法完成长时间高精度导航。
发明内容
本发明的目的是为现代军用及民用特殊车辆提供一种车载高精度容错组合导航方法及系统，以解决现有车载导航系统在复杂恶劣环境下抗干扰能力不强或可靠性不高的问题。
为实现上述目的，本发明提供了如下方案：
一种车载高精度容错组合导航方法，包括：
根据捷联惯导系统输出的载车姿态矩阵以及里程计输出的行驶路程增量，解算地理坐标系下的载车位置；
根据捷联惯导系统输出的载车姿态矩阵与多普勒雷达输出的载车行驶速度，解算地理坐标系下的载车速度；
分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程；所述误差源包括捷联惯导姿态误差、里程计刻度系数误差、多普勒雷达刻度系数误差和安装误差；
当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程；
当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程；
当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程；
根据所述高精度组合导航的状态方程、量测方程，所述容错组合导航的第一状态方程、第一量测方程，所述容错组合导航的第二状态方程、第二量测方程，采用卡尔曼滤波确定所述捷联惯导系统的误差实时估计值；
根据所述捷联惯导系统的误差实时估计值，对所述捷联惯导系统实时进行误差校正，并将经过误差校正的捷联惯导系统的输出作为组合导航系统的输出。
可选的，所述分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程，具体包括：
所述位置解算误差方程为：
Figure BDA0002792317020000021
Figure BDA0002792317020000022
Figure BDA0002792317020000023
其中，δLO为纬度误差；δλO为经度误差、δhO为高度误差；φE,φN,φU分别为捷联惯导系统的数学平台沿东、北、天向的姿态误差；vE、vN、vU分别为载车沿东、北、天向的速度；L、h分别为载车所在位置的纬度、高度；RM、RN分别为当地子午圈的主曲率半径、卯酉圈的主曲率半径；δKO为里程计刻度系数误差；
所述速度解算误差方程为：
δvDE＝-vUφN+vNφU+vEδKD+T13VδAx-T11VδAz
δvDN＝-vEφU+vUφE+vNδKD+T23VδAx-T21VδAz
δvDU＝-vNφE+vEφN+vUδKD+T33VδAx-T31VδAz
其中，δvDE、δvDN、δvDU分别为速度解算沿东、北、天向的误差；Tij(i,j＝1,2,3)为载车姿态矩阵的第i行、第j列元素。
可选的，所述当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程，具体包括：
当所有导航设备均正常工作时，根据所述捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；
当所有导航设备均正常工作时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差，以及所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立高精度组合导航的量测方程。
可选的，所述当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程，具体包括：
当多普勒雷达出现故障时，根据所述捷联惯导系统的误差模型以及所述位置解算误差方程，建立容错组合导航的第一状态方程；
当多普勒雷达出现故障时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差作为量测，建立容错组合导航的第一量测方程。
可选的，所述当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程，具体包括：
当里程计出现故障时，根据所述捷联惯导系统的误差模型以及所述速度解算误差方程，建立容错组合导航的第二状态方程；
当里程计出现故障时，以所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立容错组合导航的第二量测方程。
一种车载高精度容错组合导航系统，包括：
载车位置解算模块，用于根据捷联惯导系统输出的载车姿态矩阵以及里程计输出的行驶路程增量，解算地理坐标系下的载车位置；
载车速度解算模块，用于根据捷联惯导系统输出的载车姿态矩阵与多普勒雷达输出的载车行驶速度，解算地理坐标系下的载车速度；
位置与速度解算误差模型建立模块，用于分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程；所述误差源包括捷联惯导姿态误差、里程计刻度系数误差、多普勒雷达刻度系数误差和安装误差；
高精度组合导航状态方程与量测方程建立模块，用于当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程；
容错组合导航第一状态方程与第一量测方程建立模块，用于当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程；
容错组合导航第二状态方程与第二量测方程建立模块，用于当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程；
误差实时估计值确定模块，用于根据所述高精度组合导航的状态方程、量测方程，所述容错组合导航的第一状态方程、第一量测方程，所述容错组合导航的第二状态方程、第二量测方程，采用卡尔曼滤波确定所述捷联惯导系统的误差实时估计值；
误差在线校正模块，用于根据所述捷联惯导系统的误差实时估计值，对所述捷联惯导系统实时进行误差校正，并将经过误差校正的捷联惯导系统的输出作为组合导航系统的输出。
可选的，所述位置与速度解算误差模型建立模块，具体包括：
所述位置解算误差方程为：
Figure BDA0002792317020000051
Figure BDA0002792317020000052
Figure BDA0002792317020000053
其中，δLO为纬度误差；δλO为经度误差、δhO为高度误差；φE,φN,φU分别为捷联惯导系统的数学平台沿东、北、天向的姿态误差；vE、vN、vU分别为载车沿东、北、天向的速度；L、h分别为载车所在位置的纬度、高度；RM、RN分别为当地子午圈的主曲率半径、卯酉圈的主曲率半径；δKO为里程计刻度系数误差；
所述速度解算误差方程为：
δvDE＝-vUφN+vNφU+vEδKD+T13VδAx-T11VδAz
δvDN＝-vEφU+vUφE+vNδKD+T23VδAx-T21VδAz
δvDU＝-vNφE+vEφN+vUδKD+T33VδAx-T31VδAz
其中，δvDE、δvDN、δvDU分别为速度解算沿东、北、天向的误差；Tij(i,j＝1,2,3)为载车姿态矩阵的第i行、第j列元素。
可选的，所述高精度组合导航状态方程与量测方程建立模块，具体包括：
高精度组合导航状态方程建立单元，用于当所有导航设备均正常工作时，根据所述捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；
高精度组合导航量测方程建立单元，用于当所有导航设备均正常工作时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差，以及所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立高精度组合导航的量测方程。
可选的，所述容错组合导航第一状态方程与第一量测方程建立模块，具体包括：
容错组合导航第一状态方程建立单元，用于当多普勒雷达出现故障时，根据所述捷联惯导系统的误差模型以及所述位置解算误差方程，建立容错组合导航的第一状态方程；
容错组合导航第一量测方程建立单元，用于当多普勒雷达出现故障时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差作为量测，建立容错组合导航的第一量测方程。
可选的，所述容错组合导航第二状态方程与第二量测方程建立模块，具体包括：
容错组合导航第二状态方程建立单元，用于当里程计出现故障时，根据所述捷联惯导系统的误差模型以及所述速度解算误差方程，建立容错组合导航的第二状态方程；
容错组合导航第二量测方程建立单元，用于当里程计出现故障时，以所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立容错组合导航的第二量测方程。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
为了实现车载条件下高精度、高可靠性组合导航，本发明提供了一种利用捷联惯导系统与里程计、多普勒雷达进行车载高精度容错组合导航的方法及系统，首先基于捷联惯导系统的姿态输出与里程计的路程增量输出、多普勒雷达的速度输出，分别设计地理坐标系下载车位置解算算法和速度解算算法；然后，研究里程计与多普勒雷达的误差模型，分别建立前述位置解算和速度解算的误差模型；采用间接法滤波进行组合导航设计，以捷联惯导系统误差、里程计误差、多普勒雷达误差等作为系统状态，将捷联惯导输出的位置、速度与前述位置解算、速度解算获得的对应信息分别相减作为量测，采用卡尔曼滤波设计高精度容错组合导航的滤波算法。从而，不仅实现车辆精确导航，而且能够在里程计或多普勒雷达某一设备出现故障时，仍能保持较高的导航精度，因此具有高精度、高可靠性、高自主性和强抗干扰能力，本发明工程易于实现，使用性能可靠，非常适用于各种特殊应用领域特别是军事领域的车辆导航。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明车载高精度容错组合导航技术的原理框图；
图2为本发明车载高精度容错组合导航方法的流程图；
图3为本发明车载高精度容错组合导航方法的各个算法设计流程图；
图4为本发明车载高精度容错组合导航系统结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是为现代军用及民用特殊车辆提供一种车载高精度容错组合导航方法及系统，而且能够在一些复杂恶劣环境下具有较高的自主性和较强的抗干扰能力。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明涉及车载导航定位技术领域，对于现代军用及民用特殊车辆，如发射车、装甲车、指挥车、测绘车等，通常需要具备高精度、高可靠性的导航定位能力，而且要求在一些复杂恶劣环境下能够具有较高的自主性和较强的抗干扰能力。目前，国内外较为普遍使用的车载惯性/卫星组合导航系统，虽然实现了高精度导航，但由于卫星信号易被干扰或屏蔽，因此惯性/卫星组合导航的可靠性不高、抗干扰能力较弱。此外，部分车载导航系统中采用了惯性/里程计组合导航模式，这种方式精度高、自主性强、抗干扰性好，但是由于惯性导航系统存在导航误差随时间积累发散的固有缺点，一旦里程计出现故障而无法正常输出，此时仅靠惯性导航系统本身根本无法完成长时间高精度导航。
车载多普勒雷达利用雷达发射的电磁波与回波之间的频率差异来获得载车速度，具有精度高、输出连续、抗干扰性好、自主性强等突出优点，特别是能够有效避免因车辆打滑或滑行带来的测量误差；而里程计虽然同样精度高、自主性强、抗干扰性好，但往往存在因车辆打滑或滑行而带来较大测量误差的问题。为此，本发明提出了一种利用捷联惯导系统与里程计、多普勒雷达进行车载高精度容错组合导航的方法及系统，其中里程计与多普勒雷达互相作为备份以提高系统的容错性，重点研究组合导航相关算法，从而不仅实现车辆精确导航，而且实现高可靠性、高自主性和强抗干扰能力，工程易于实现、使用性能可靠，非常适用于各种特殊应用领域特别是军事领域的车辆导航。图1为本发明车载高精度容错组合导航技术的原理框图，图2为本发明车载高精度容错组合导航方法的流程图，如图2所示。
图3为本发明车载高精度容错组合导航方法的各个算法设计流程图。如图3所示，一种车载高精度容错组合导航方法包括：
步骤301：根据捷联惯导系统输出的载车姿态矩阵以及里程计输出的行驶路程增量，解算地理坐标系下的载车位置。
所述步骤301具体包括：
选取导航坐标系(n系)为东-北-天地理坐标系，车体坐标系(b系)为右前上坐标系。利用捷联惯导系统输出的载车姿态矩阵
Figure BDA0002792317020000081
可以根据里程计输出的行驶路程增量实时解算获得载车的位置信息。
设里程计输出的载车在[tj-1,tj]时间段内行驶路程增量为ΔSj，则根据tj时刻捷联惯导系统输出的载车姿态矩阵
Figure BDA0002792317020000082
可得载车行驶路程增量在导航坐标系的投影
Figure BDA0002792317020000083
为
Figure BDA0002792317020000084
设tj-1时刻载车所在位置的纬、经、高度分别为Lj-1、λj-1、hj-1，tj时刻所在位置为Lj、λj、hj，则载车位置的更新解算公式为
Figure BDA0002792317020000091
Figure BDA0002792317020000092
hj＝hj-1+ΔSUj
其中，ΔSEj、ΔSNj、ΔSUj分别为载车在[tj-1,tj]时间段内行驶路程增量在导航坐标系三个轴上的投影分量，即
Figure BDA0002792317020000093
步骤302：根据捷联惯导系统输出的载车姿态矩阵与多普勒雷达输出的载车行驶速度，解算地理坐标系下的载车速度。
所述步骤302具体包括：
设tj时刻多普勒雷达输出的载车行驶速度为
Figure BDA0002792317020000094
则载车行驶速度在车体坐标系下的投影
Figure BDA0002792317020000095
为
Figure BDA0002792317020000096
利用tj时刻捷联惯导系统输出的载车姿态矩阵
Figure BDA0002792317020000097
可以得到载车行驶速度在导航坐标系下的投影
Figure BDA0002792317020000098
为
Figure BDA0002792317020000099
上式即为地理坐标系下载车速度的更新解算公式。
步骤303：分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程；所述误差源包括捷联惯导姿态误差、里程计刻度系数误差、多普勒雷达刻度系数误差和安装误差。
所述步骤303具体包括：
在载车位置解算过程中，除了初始条件误差，捷联惯导姿态误差和里程计误差是最主要的误差源。对于里程计的误差，经过标定补偿后，主要还剩下刻度系数误差，通常可考虑为随机常值，即
Figure BDA00027923170200000910
多普勒雷达的测速误差主要由刻度系数误差和安装误差所引起，其中刻度系数误差是最重要的误差源，其可考虑用一阶马尔科夫过程来描述，即
Figure BDA0002792317020000101
其中，δKD为多普勒雷达的刻度系数误差，τk为相关时间，wk为白噪声误差。
在工程实际中，通常将多普勒雷达固联在安装框体上，由于安装轴线难以完全一致，因此必然存在一定的安装误差，通常可以将多普勒雷达的安装误差考虑为随机常值，即
Figure BDA0002792317020000102
其中，δAi(i＝x,y,z)为多普勒雷达沿车体坐标系x、y、z轴上的安装误差。
设位置解算的纬度、经度、高度误差分别为δLO、δλO、δhO，则根据地理坐标系下位置解算微分方程，考虑里程计刻度系数误差δKO的影响，并忽略关于误差的二阶小量，可推导获得位置解算的误差方程为
Figure BDA0002792317020000103
Figure BDA0002792317020000104
Figure BDA0002792317020000105
其中，φE,φN,φU分别为捷联惯导系统的数学平台沿东、北、天向的姿态误差，vE、vN、vU分别为载车沿东、北、天向的速度，L、h分别为载车所在位置的纬度、高度，RM、RN分别为当地子午圈的主曲率半径、卯酉圈的主曲率半径。
设速度解算沿东、北、天向的误差分别为δvDE、δvDN、δvDU，根据载车速度的测量值和真实值在车体坐标系下投影之间的转换关系，考虑多普勒雷达安装误差的影响，并忽略关于误差的二阶小量，可推导获得速度解算的误差方程为
δvDE＝-vUφN+vNφU+vEδKD+T13VδAx-T11VδAz
δvDN＝-vEφU+vUφE+vNδKD+T23VδAx-T21VδAz
δvDU＝-vNφE+vEφN+vUδKD+T33VδAx-T31VδAz
其中，Tij(i,j＝1,2,3)是载车姿态矩阵
Figure BDA0002792317020000111
的第i行、第j列元素。
步骤304：当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程。
所述步骤304具体包括：
采用间接法滤波设计组合导航算法，即选取系统误差作为滤波的系统状态。其中，捷联惯导系统的误差模型在很多文献中已有研究，里程计与多谱勒雷达的误差模型、位置解算的误差模型如本发明所述，而速度解算的误差与捷联惯导数学平台姿态误差、多普勒雷达刻度系数误差和安装误差之间存在线性关系，则不再将速度解算误差列入组合导航滤波器的状态。
因此，选取捷联惯导系统的数学平台姿态误差、速度误差、位置误差、惯性器件误差，位置解算误差、里程计刻度系数误差，以及多普勒雷达的安装误差和刻度系数误差作为高精度组合导航的系统状态，包括：捷联惯导系统数学平台沿东、北、天向的姿态误差φE,φN,φU，沿东、北、天向的速度误差δvE,δvN,δvU，纬、经、高度误差δL,δλ,δh，x、y、z轴上陀螺的常值漂移εbx,εby,εbz，x、y、z轴上加速度计的常值偏置
Figure BDA0002792317020000112
以及位置解算的纬、经、高度误差δLO,δλO,δhO，里程计刻度系数误差δKO，多普勒雷达安装误差δAx,δAy,δAz，多普勒雷达刻度系数误差δKD，即高精度组合导航的系统状态X为
Figure BDA0002792317020000113
于是，根据捷联惯导系统的误差模型，以及前述位置解算与速度解算的误差模型，并结合系统状态X，可以获得高精度组合导航的状态方程，其形式可表示为：
Figure BDA0002792317020000114
其中，F为系统状态矩阵，G为系统噪声驱动阵，W为系统白噪声，其噪声方差阵为q。
考虑到捷联惯导系统既能输出载车的位置信息，又能输出载车的速度信息，因此可以利用前述位置解算、速度解算分别获得的载车位置、速度信息，与捷联惯导系统的对应输出构造组合导航的量测。也就是说，将捷联惯导系统输出的载车位置、速度信息与前述位置解算、速度解算分别输出的结果对应相减作为量测，即高精度组合导航的量测Z为
Z＝[LS-LO,λS-λO,hS-hO,vSE-vDE,vSN-vDN,vSU-vDU]T
这里，LS,λS,hS分别为捷联惯导输出的纬、经、高度；LO,λO,hO分别为载车位置解算获得的纬、经、高度；vSE,vSN,vSU分别为捷联惯导输出的东、北、天向速度；vDE,vDN,vDU分别为载车速度解算获得的东、北、天向速度。
对于所述高精度组合导航的量测Z，当考虑捷联惯导系统的位置、速度误差，以及所述位置解算误差、速度解算误差，那么量测Z则可表示为
Z＝[δL-δLO,δλ-δλO,δh-δhO,δvE-δvDE,δvN-δvDN,δvU-δvDU]T
从而，根据所述系统状态X、量测Z的表达式，结合所述速度解算的误差方程，可以获得高精度组合导航的量测方程，其形式可表示为
Z＝HX+V
其中，量测矩阵
Figure BDA0002792317020000121
V为量测白噪声，这里
Figure BDA0002792317020000122
Figure BDA0002792317020000123
步骤305：当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程。
所述步骤305具体包括：
在组合导航过程中，如果多普勒雷达出现故障而无法正常输出的时候，此时可采用捷联惯导与里程计进行组合导航。相应的，可基于捷联惯导系统的误差模型、前述位置解算的误差模型，建立容错组合导航的第一状态方程。因此，选取捷联惯导系统的数学平台姿态误差、速度误差、位置误差、惯性器件误差，以及位置解算误差、里程计刻度系数误差作为组合导航的系统状态，包括：捷联惯导系统数学平台沿东、北、天向的姿态误差φE,φN,φU，沿东、北、天向的速度误差δvE,δvN,δvU，纬、经、高度误差δL,δλ,δh，x、y、z轴上陀螺的常值漂移εbx,εby,εbz，x、y、z轴上加速度计的常值偏置
Figure BDA0002792317020000131
以及位置解算的纬、经、高度误差δLO,δλO,δhO，里程计刻度系数误差δKO，即容错组合导航的第一系统状态XO为
Figure BDA0002792317020000132
于是，根据捷联惯导系统的误差模型、前述位置解算的误差模型，并结合第一系统状态XO，可以获得容错组合导航的第一状态方程，其形式可表示为：
Figure BDA0002792317020000133
其中，FO为系统状态矩阵，GO为系统噪声驱动阵，WO为系统白噪声，其噪声方差阵为qO。
由于多普勒雷达出现故障而无法输出，此时则无法利用多普勒雷达的输出进行载车速度解算，但仍然可以利用里程计的输出进行载车位置解算。因此，将捷联惯导系统输出的位置与载车位置解算获得的位置对应相减作为量测，即容错组合导航的第一量测ZO为
ZO＝[LS-LO,λS-λO,hS-hO]T
这里，LS,λS,hS分别为捷联惯导系统输出的纬、经、高度；LO,λO,hO分别为载车位置解算获得的纬、经、高度。
对于所述容错组合导航的第一量测ZO，当考虑捷联惯导系统的位置误差、位置解算误差，那么第一量测ZO则可表示为
ZO＝[δL-δLO,δλ-δλO,δh-δhO]T
从而，根据所述第一系统状态XO、第一量测ZO的表达式，可以获得容错组合导航的第一量测方程，其形式可表示为
ZO＝HOXO+VO
其中，量测矩阵HO＝[O3×6 H1 0]，VO为量测白噪声。
步骤306：当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程。
所述步骤306具体包括：
在组合导航过程中，如果里程计出现故障而无法正常输出的时候，此时可采用捷联惯导与多普勒雷达进行组合导航。相应的，可基于捷联惯导系统的误差模型、前述速度解算的误差模型，建立容错组合导航的第二状态方程。因此，选取捷联惯导系统的数学平台姿态误差、速度误差、位置误差、惯性器件误差，以及多普勒雷达的安装误差和刻度系数误差作为容错组合导航的第二系统状态，包括：捷联惯导系统数学平台沿东、北、天向的姿态误差φE,φN,φU，沿东、北、天向的速度误差δvE,δvN,δvU，纬、经、高度误差δL,δλ,δh，x、y、z轴上陀螺的常值漂移εbx,εby,εbz，x、y、z轴上加速度计的常值偏置
Figure BDA0002792317020000141
以及多普勒雷达安装误差δAx,δAy,δAz，多普勒雷达刻度系数误差δKD，即容错组合导航的第二系统状态XD为
Figure BDA0002792317020000142
于是，根据捷联惯导系统的误差模型、前述速度解算的误差模型，并结合第二系统状态XD，可以获得容错组合导航的第二状态方程，其形式可表示为：
Figure BDA0002792317020000143
其中，FD为系统状态矩阵，GD为系统噪声驱动阵，WD为系统白噪声，其噪声方差阵为qD。
由于里程计出现故障而无法输出，此时则无法利用里程计的输出进行载车位置解算，但仍然可以利用多普勒雷达的输出进行载车速度解算。因此，将捷联惯导系统输出的速度与载车速度解算获得的速度对应相减作为量测，即容错组合导航的第二量测ZD为
ZD＝[vSE-vDE,vSN-vDN,vSU-vDU]T
这里，vSE,vSN,vSU分别为捷联惯导输出的东、北、天向速度；vDE,vDN,vDU分别为载车速度解算获得的东、北、天向速度。
对于所述容错组合导航的第二量测ZD，当考虑捷联惯导系统的速度误差、速度解算误差，那么第二量测ZD则可表示为
ZD＝[δvE-δvDE,δvN-δvDN,δvU-δvDU]T
从而，根据所述第二系统状态XD、第二量测ZD的表达式，可以获得容错组合导航的第二量测方程，其形式可表示为
ZD＝HDXD+VD
其中，量测矩阵HD＝[H2 O3×8 H3]，VD为量测白噪声。
步骤307：根据所述高精度组合导航的状态方程、量测方程，所述容错组合导航的第一状态方程、第一量测方程，所述容错组合导航的第二状态方程、第二量测方程，采用卡尔曼滤波确定所述捷联惯导系统的误差实时估计值。
所述步骤307具体包括：
获得所述组合导航的状态方程与量测方程以后，可以利用离散化公式与卡尔曼滤波算法进行组合导航滤波计算。这里，需要说明的是，捷联惯导系统由于采取冗余设计与故障检测技术，一般认为其是绝对可靠的。由此可见，在组合导航过程中，如果里程计或多普勒雷达因发生故障而无法正常输出时，利用前述步骤305、306，组合导航的状态方程与量测方程仍然可以正常建立，那么组合导航滤波就可以正常进行，从而实现了车载组合导航系统的容错性。
为了在组合导航计算机上实现滤波计算，需对所述组合导航的状态方程和量测方程进行离散化处理。由于所述量测方程本身已经是离散的，而所述状态方程则是连续的，因此只需对连续的状态方程进行离散化，离散化成如下形式：
Xk+1＝Φk+1,kXk+Wk
式中，Φk+1,k为一步转移矩阵；Wk满足E[Wk]＝0且
Figure BDA0002792317020000151
其中Qk为等效离散系统噪声方差阵。其中，矩阵Φk+1,k和Qk的具体算法如下所述：
设卡尔曼滤波周期为T(T＝tk+1-tk)，记F(tk)＝Fk。当卡尔曼滤波周期T较短时(一般取1秒甚至更短)，一步转移阵Φk+1,k的实时计算公式为：
Figure BDA0002792317020000161
记G(tk)＝Gk，则等效离散系统噪声方差阵Qk的实时计算公式如下
Figure BDA0002792317020000162
Figure BDA0002792317020000163
Mi+1＝FkMi+(FkMi)T
Figure BDA0002792317020000164
从而，完成所述连续的系统状态方程的离散化处理。这时，设定系统状态的初值
Figure BDA0002792317020000165
及其估计均方误差P0之后，并结合k时刻的量测Zk，就可利用下列离散型卡尔曼滤波基本方程进行滤波计算：
Figure BDA0002792317020000166
Figure BDA0002792317020000167
Figure BDA0002792317020000168
Figure BDA0002792317020000169
Figure BDA00027923170200001610
从而，通过滤波计算，可以递推计算获得k时刻的状态估计
Figure BDA00027923170200001611
即意味着经过滤波计算可以获得捷联惯导系统各种误差的实时估计值。
步骤308：根据所述捷联惯导系统的误差实时估计值，对所述捷联惯导系统实时进行误差校正，并将经过误差校正的捷联惯导系统的输出作为组合导航系统的输出。
获得捷联惯导系统各种误差的实时估计值以后，需要利用该估计值对捷联惯导系统实时进行误差校正，最后将经过误差校正的捷联惯导系统的输出作为车载高精度容错组合导航系统的输出。
对于捷联惯导系统的位置误差、速度误差校正，可以从捷联惯导系统输出的位置、速度信息中减去对应的位置误差估计值、速度误差估计值即可。
对于捷联惯导系统的姿态误差校正，则需要计算姿态校正矩阵方能实现。设滤波计算获得捷联惯导系统数学平台姿态误差的估计值分别为
Figure BDA0002792317020000171
则捷联惯导系统的姿态校正矩阵
Figure BDA0002792317020000172
为：
Figure BDA0002792317020000173
利用上述姿态校正矩阵可以对捷联惯导系统实际得到的姿态矩阵
Figure BDA0002792317020000174
进行误差校正，即：
Figure BDA0002792317020000175
这里，
Figure BDA0002792317020000176
即为经过误差校正后的载车姿态矩阵。
从而，利用误差校正后的载车姿态矩阵
Figure BDA0002792317020000177
可以根据下式与表实时计算出载车的航向姿态角，不妨记航向角为ψ，俯仰角为θ，横滚角为γ，则：
Figure BDA0002792317020000178
这里，ψ主、θ主、γ主分别为航向角、俯仰角、横滚角的主值。
表1航向姿态角真值表
Figure BDA0002792317020000179
图4为本发明车载高精度容错组合导航系统结构图。如图4所示，一种车载高精度容错组合导航系统包括：
载车位置解算模块401，用于根据捷联惯导系统输出的载车姿态矩阵以及里程计输出的行驶路程增量，解算地理坐标系下的载车位置。
载车速度解算模块402，用于根据捷联惯导系统输出的载车姿态矩阵与多普勒雷达输出的载车行驶速度，解算地理坐标系下的载车速度。
位置与速度解算误差模型建立模块403，用于分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程；所述误差源包括捷联惯导姿态误差、里程计刻度系数误差、多普勒雷达刻度系数误差和安装误差。
所述位置与速度解算误差模型建立模块403，具体包括：
所述位置解算误差方程为：
Figure BDA0002792317020000181
Figure BDA0002792317020000182
Figure BDA0002792317020000183
其中，δLO为纬度误差；δλO为经度误差、δhO为高度误差；φE,φN,φU分别为捷联惯导系统的数学平台沿东、北、天向的姿态误差；vE、vN、vU分别为载车沿东、北、天向的速度；L、h分别为载车所在位置的纬度、高度；RM、RN分别为当地子午圈的主曲率半径、卯酉圈的主曲率半径；δKO为里程计刻度系数误差；
所述速度解算误差方程为：
δvDE＝-vUφN+vNφU+vEδKD+T13VδAx-T11VδAz
δvDN＝-vEφU+vUφE+vNδKD+T23VδAx-T21VδAz
δvDU＝-vNφE+vEφN+vUδKD+T33VδAx-T31VδAz
其中，δvDE、δvDN、δvDU分别为速度解算沿东、北、天向的误差；Tij(i,j＝1,2,3)为载车姿态矩阵的第i行、第j列元素。
高精度组合导航状态方程与量测方程建立模块404，用于当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程。
所述高精度组合导航状态方程与量测方程建立模块404，具体包括：
高精度组合导航状态方程建立单元，用于当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；
高精度组合导航量测方程建立单元，用于当所有导航设备均正常工作时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差，以及所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立高精度组合导航的量测方程。
容错组合导航第一状态方程与第一量测方程建立模块405，用于当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程。
所述容错组合导航第一状态方程与第一量测方程建立模块405具体包括：
容错组合导航第一状态方程建立单元，用于当多普勒雷达出现故障时，根据所述捷联惯导系统的误差模型以及所述位置解算误差方程，建立容错组合导航的第一状态方程；
容错组合导航第一量测方程建立单元，用于当多普勒雷达出现故障时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差作为量测，建立容错组合导航的第一量测方程。
容错组合导航第二状态方程与第二量测方程建立模块406，用于当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程。
所述容错组合导航第二状态方程与第二量测方程建立模块406具体包括：
容错组合导航第二状态方程建立单元，用于当里程计出现故障时，根据所述捷联惯导系统的误差模型以及所述速度解算误差方程，建立容错组合导航的第二状态方程；
容错组合导航第二量测方程建立单元，用于当里程计出现故障时，以所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立容错组合导航的第二量测方程。
误差实时估计值确定模块407，用于根据所述高精度组合导航的状态方程、量测方程，所述容错组合导航的第一状态方程、第一量测方程，所述容错组合导航的第二状态方程、第二量测方程，采用卡尔曼滤波确定所述捷联惯导系统的误差实时估计值。
误差在线校正模块408，用于根据所述捷联惯导系统的误差实时估计值，对所述捷联惯导系统实时进行误差校正，并将经过误差校正的捷联惯导系统的输出作为组合导航系统的输出。
本发明与现有技术相比，具有下列优势：
1、本发明提供的车载高精度容错组合导航方法，为车载导航定位技术领域增添了一种工程可行、效果显著的新方法，该方法具有高精度、高可靠性、高自主性和强抗干扰能力等突出优点，非常适用于各种特殊应用领域特别是军事领域的车辆导航。
2、本发明不仅有效克服了捷联惯导系统的误差随时间不断发散缺点，而且也摆脱了对外部信息或设备的依赖，特别是不易受到外部复杂电磁环境的干扰，极大地提高了车载导航系统的绝对自主性、抗干扰性和快速机动性。
3、本发明利用里程计与多普勒雷达共同辅助捷联惯导系统进行车载导航，不仅显著提高了系统导航精度，而且当里程计或多普勒雷达中某一设备发生故障而无法正常输出时，采用本发明中所设计的方法仍可以保证组合导航滤波正常进行，并依旧保持较高的导航精度，从而实现了车载组合导航系统的容错性。
4、本发明设计了地理坐标系下载车位置解算算法与速度解算算法，通过利用捷联惯导系统的姿态输出、里程计的路程增量输出以及多普勒雷达的速度输出，不仅实现了载车位置和速度的实时解算，而且为高精度容错组合导航提供了有效的量测信息，从而为组合导航的实现奠定了基础。
5、本发明通过分析位置解算和速度解算的误差源，建立了误差源数学模型，推导获得了位置解算与速度解算的误差方程，这不仅揭示了本发明中载车位置解算与速度解算误差的传播特性，而且为高精度容错组合导航状态方程的建立奠定了基础。
6、本发明基于所建立的组合导航状态方程与量测方程，借助卡尔曼滤波这一最优估计手段，经过滤波计算能够实时获得捷联惯导系统误差的最优估计值，利用该估计值才得以对捷联惯导系统进行误差在线校正，从而最终实现车载高精度导航。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种车载高精度容错组合导航方法，其特征在于，包括：
根据捷联惯导系统输出的载车姿态矩阵以及里程计输出的行驶路程增量，解算地理坐标系下的载车位置；
根据捷联惯导系统输出的载车姿态矩阵与多普勒雷达输出的载车行驶速度，解算地理坐标系下的载车速度；
分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程；所述误差源包括捷联惯导姿态误差、里程计刻度系数误差、多普勒雷达刻度系数误差和安装误差，具体包括：
所述位置解算误差方程为：
Figure FDA0003788638790000011
Figure FDA0003788638790000012
Figure FDA0003788638790000013
其中，δLO为纬度误差；δλO为经度误差、δhO为高度误差；φE,φN,φU分别为捷联惯导系统的数学平台沿东、北、天向的姿态误差；vE、vN、vU分别为载车沿东、北、天向的速度；L、h分别为载车所在位置的纬度、高度；RM、RN分别为当地子午圈的主曲率半径、卯酉圈的主曲率半径；δKO为里程计刻度系数误差；
所述速度解算误差方程为：
δvDE＝-vUφN+vNφU+vEδKD+T13VδAx-T11VδAz
δvDN＝-vEφU+vUφE+vNδKD+T23VδAx-T21VδAz
δvDU＝-vNφE+vEφN+vUδKD+T33VδAx-T31VδAz
其中，δvDE、δvDN、δvDU分别为速度解算沿东、北、天向的误差；V为量测白噪声；δAx,δAy,δAz为x轴、y轴、z轴上的多普勒雷达安装误差；δKD为多普勒雷达刻度系数误差；Tij为载车姿态矩阵的第i行、第j列元素，i,j＝1,2,3；
当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程；
当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程；
当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程；
根据所述高精度组合导航的状态方程、量测方程，所述容错组合导航的第一状态方程、第一量测方程，所述容错组合导航的第二状态方程、第二量测方程，采用卡尔曼滤波确定所述捷联惯导系统的误差实时估计值；
根据所述捷联惯导系统的误差实时估计值，对所述捷联惯导系统实时进行误差校正，并将经过误差校正的捷联惯导系统的输出作为组合导航系统的输出。
2.根据权利要求1所述的一种车载高精度容错组合导航方法，其特征在于，所述当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程，具体包括：
当所有导航设备均正常工作时，根据所述捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；
当所有导航设备均正常工作时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差，以及所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立高精度组合导航的量测方程。
3.根据权利要求1所述的一种车载高精度容错组合导航方法，其特征在于，所述当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程，具体包括：
当多普勒雷达出现故障时，根据所述捷联惯导系统的误差模型以及所述位置解算误差方程，建立容错组合导航的第一状态方程；
当多普勒雷达出现故障时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差作为量测，建立容错组合导航的第一量测方程。
4.根据权利要求1所述的一种车载高精度容错组合导航方法，其特征在于，所述当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程，具体包括：
当里程计出现故障时，根据所述捷联惯导系统的误差模型以及所述速度解算误差方程，建立容错组合导航的第二状态方程；
当里程计出现故障时，以所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立容错组合导航的第二量测方程。
5.一种车载高精度容错组合导航系统，其特征在于，包括：
载车位置解算模块，用于根据捷联惯导系统输出的载车姿态矩阵以及里程计输出的行驶路程增量，解算地理坐标系下的载车位置；
载车速度解算模块，用于根据捷联惯导系统输出的载车姿态矩阵与多普勒雷达输出的载车行驶速度，解算地理坐标系下的载车速度；
位置与速度解算误差模型建立模块，用于分析载车位置解算和速度解算的误差源，并根据所述误差源确定位置解算误差方程以及速度解算误差方程；所述误差源包括捷联惯导姿态误差、里程计刻度系数误差、多普勒雷达刻度系数误差和安装误差；
所述位置与速度解算误差模型建立模块，具体包括：
所述位置解算误差方程为：
Figure FDA0003788638790000051
Figure FDA0003788638790000052
Figure FDA0003788638790000053
其中，δLO为纬度误差；δλO为经度误差、δhO为高度误差；φE,φN,φU分别为捷联惯导系统的数学平台沿东、北、天向的姿态误差；vE、vN、vU分别为载车沿东、北、天向的速度；L、h分别为载车所在位置的纬度、高度；RM、RN分别为当地子午圈的主曲率半径、卯酉圈的主曲率半径；δKO为里程计刻度系数误差；
所述速度解算误差方程为：
δvDE＝-vUφN+vNφU+vEδKD+T13VδAx-T11VδAz
δvDN＝-vEφU+vUφE+vNδKD+T23VδAx-T21VδAz
δvDU＝-vNφE+vEφN+vUδKD+T33VδAx-T31VδAz
其中，δvDE、δvDN、δvDU分别为速度解算沿东、北、天向的误差；V为量测白噪声；δAx,δAy,δAz为x轴、y轴、z轴上的多普勒雷达安装误差；δKD为多普勒雷达刻度系数误差；Tij为载车姿态矩阵的第i行、第j列元素，i,j＝1,2,3；
高精度组合导航状态方程与量测方程建立模块，用于当所有导航设备均正常工作时，根据捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；并根据所述捷联惯导系统输出的位置及速度、所述地理坐标系下的载车位置、所述地理坐标系下的载车速度，建立高精度组合导航的量测方程；
容错组合导航第一状态方程与第一量测方程建立模块，用于当多普勒雷达出现故障时，根据捷联惯导系统的误差模型、所述位置解算误差方程，建立容错组合导航的第一状态方程；根据所述捷联惯导系统输出的位置、所述地理坐标系下的载车位置，建立容错组合导航的第一量测方程；
容错组合导航第二状态方程与第二量测方程建立模块，用于当里程计出现故障时，根据捷联惯导系统的误差模型、所述速度解算误差方程，建立容错组合导航的第二状态方程；根据所述捷联惯导系统输出的速度、所述地理坐标系下的载车速度，建立容错组合导航的第二量测方程；
误差实时估计值确定模块，用于根据所述高精度组合导航的状态方程、量测方程，所述容错组合导航的第一状态方程、第一量测方程，所述容错组合导航的第二状态方程、第二量测方程，采用卡尔曼滤波确定所述捷联惯导系统的误差实时估计值；
误差在线校正模块，用于根据所述捷联惯导系统的误差实时估计值，对所述捷联惯导系统实时进行误差校正，并将经过误差校正的捷联惯导系统的输出作为组合导航系统的输出。
6.根据权利要求5所述的一种车载高精度容错组合导航系统，其特征在于，所述高精度组合导航状态方程与量测方程建立模块，具体包括：
高精度组合导航状态方程建立单元，用于当所有导航设备均正常工作时，根据所述捷联惯导系统的误差模型、所述位置解算误差方程以及所述速度解算误差方程，建立高精度组合导航的状态方程；
高精度组合导航量测方程建立单元，用于当所有导航设备均正常工作时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差，以及所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立高精度组合导航的量测方程。
7.根据权利要求5所述的一种车载高精度容错组合导航系统，其特征在于，所述容错组合导航第一状态方程与第一量测方程建立模块，具体包括：
容错组合导航第一状态方程建立单元，用于当多普勒雷达出现故障时，根据所述捷联惯导系统的误差模型以及所述位置解算误差方程，建立容错组合导航的第一状态方程；
容错组合导航第一量测方程建立单元，用于当多普勒雷达出现故障时，以所述捷联惯导系统输出的位置与所述地理坐标系下的载车位置之差作为量测，建立容错组合导航的第一量测方程。
8.根据权利要求5所述的一种车载高精度容错组合导航系统，其特征在于，所述容错组合导航第二状态方程与第二量测方程建立模块，具体包括：
容错组合导航第二状态方程建立单元，用于当里程计出现故障时，根据所述捷联惯导系统的误差模型以及所述速度解算误差方程，建立容错组合导航的第二状态方程；
容错组合导航第二量测方程建立单元，用于当里程计出现故障时，以所述捷联惯导系统输出的速度与所述地理坐标系下的载车速度之差作为量测，建立容错组合导航的第二量测方程。
CN202011319293.1A 2020-11-23 2020-11-23 一种车载高精度容错组合导航方法及系统 Active CN112461236B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011319293.1A CN112461236B (zh) 2020-11-23 2020-11-23 一种车载高精度容错组合导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011319293.1A CN112461236B (zh) 2020-11-23 2020-11-23 一种车载高精度容错组合导航方法及系统

Publications (2)

Publication Number Publication Date
CN112461236A CN112461236A (zh) 2021-03-09
CN112461236B true CN112461236B (zh) 2022-10-04

Family

ID=74799239

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011319293.1A Active CN112461236B (zh) 2020-11-23 2020-11-23 一种车载高精度容错组合导航方法及系统

Country Status (1)

Country Link
CN (1) CN112461236B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114383610B (zh) * 2021-12-24 2024-07-16 郑州恒达智控科技股份有限公司 一种基于移动三维扫描技术的抖动检测分段滤波方法
CN118225077A (zh) * 2023-11-23 2024-06-21 徐州信智科技有限公司 一种煤矿井下无轨胶轮车多源信息融合定位系统及方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111426320A (zh) * 2020-05-18 2020-07-17 中南大学 一种基于图像匹配/惯导/里程计的车辆自主导航方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6477465B1 (en) * 1999-11-29 2002-11-05 American Gnc Corporation Vehicle self-carried positioning method and system thereof
US20060287824A1 (en) * 2005-01-29 2006-12-21 American Gnc Corporation Interruption free navigator
CN105628026B (zh) * 2016-03-04 2018-09-14 深圳大学 一种移动物体的定位定姿方法和系统
DE102016212919A1 (de) * 2016-07-14 2018-01-18 Continental Automotive Gmbh Verfahren zum Bestimmen einer Position, Steuerungsmodul und Speichermedium
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法
CN110221333B (zh) * 2019-04-11 2023-02-10 同济大学 一种车载ins/od组合导航系统的量测误差补偿方法
CN111912427B (zh) * 2019-05-10 2022-03-01 中国人民解放军火箭军工程大学 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN111220151B (zh) * 2019-12-20 2021-08-03 湖北航天技术研究院总体设计所 载体系下考虑温度模型的惯性和里程计组合导航方法
CN111649762B (zh) * 2019-12-28 2022-05-10 中国船舶重工集团公司第七一七研究所 一种惯性多普勒全参量高精度标定方法及装置
CN111536972B (zh) * 2020-06-19 2020-10-30 立得空间信息技术股份有限公司 一种基于里程计刻度系数修正的车载dr导航方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111426320A (zh) * 2020-05-18 2020-07-17 中南大学 一种基于图像匹配/惯导/里程计的车辆自主导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Hongyang Bai ; Xiaozhong Xue.Simulation Research on FOG-SINS/Doppler Radar/Baro-altimeter Integrated Navigation for Helicopters.《2010 Second International Conference on Computer Modeling and Simulation》.2010,全文. *
SINS/OD组合导航系统转弯误差补偿;王志伟，秦俊奇，石志勇，等;《火力与指挥控制》;20180131;第43卷(第1期);全文 *
基于激光多普勒测速仪的车载组合导航;魏国等;《强激光与粒子束》;20110131;第23卷(第01期);全文 *

Also Published As

Publication number Publication date
CN112461236A (zh) 2021-03-09

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN101476894B (zh) 车载sins/gps组合导航系统性能增强方法
CN110926468B (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN112505737B (zh) 一种gnss/ins组合导航方法
CN109945860A (zh) 一种基于卫星紧组合的ins和dr惯性导航方法与系统
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN112762961B (zh) 一种车载惯性里程计组合导航在线标定方法
CN112461236B (zh) 一种车载高精度容错组合导航方法及系统
CN106405592B (zh) 车载北斗载波相位周跳检测与修复方法及系统
CN114545472B (zh) 一种gnss/ins组合系统的导航方法和装置
CN113074757B (zh) 车载惯导安装误差角的标定方法
CN108303120B (zh) 一种机载分布式pos的实时传递对准的方法及装置
CN110887472A (zh) 一种偏振-地磁信息深度融合全自主姿态解算方法
CN111912427B (zh) 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN116184471A (zh) 一种gnss与惯性导航组合系统在复杂环境下的定位方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN109945857B (zh) 一种面向不动产实地测量的车载惯性定位方法及其装置
CN114264304B (zh) 复杂动态环境高精度水平姿态测量方法与系统
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
CN115164881A (zh) 一种用于轨道检测的时栅辅助惯性测量系统及方法
CN109596139B (zh) 基于mems的车载导航方法
Shien et al. Integrated navigation accuracy improvement algorithm based on multi-sensor fusion
Fang et al. Integrating SINS sensors with odometer measurements for land vehicle navigation system

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