CN117249826A - 基于二维ldv和惯导系统的容错组合导航方法及装置 - Google Patents

基于二维ldv和惯导系统的容错组合导航方法及装置 Download PDF

Info

Publication number
CN117249826A
CN117249826A CN202311512395.9A CN202311512395A CN117249826A CN 117249826 A CN117249826 A CN 117249826A CN 202311512395 A CN202311512395 A CN 202311512395A CN 117249826 A CN117249826 A CN 117249826A
Authority
CN
China
Prior art keywords
dimensional
filter
sub
representing
speed
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.)
Granted
Application number
CN202311512395.9A
Other languages
English (en)
Other versions
CN117249826B (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202311512395.9A priority Critical patent/CN117249826B/zh
Publication of CN117249826A publication Critical patent/CN117249826A/zh
Application granted granted Critical
Publication of CN117249826B publication Critical patent/CN117249826B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Landscapes

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

Abstract

本申请涉及一种基于二维LDV和惯导系统的容错组合导航方法及装置。所述方法包括：通过将惯导系统与二维激光多普勒测速仪两个光束构建的二维测速系统以及两个光束各自构建的一维测速系统相结合来构建组合导航系统，并在进行组合导航时，使用自适应联邦卡尔曼滤波将惯导系统与三个测速系统进行融合，对三个子滤波器进行实时故障诊断与隔离，以避免有故障的测速系统输出影响整个滤波器，从而提高组合导航系统的总体性能。采用本方法能够在保证了组合导航系统精度的情况下拥有更高的容错能力，在二维激光多普勒测速仪单个光束长时间失效的情况下仍能实现较高精度的导航定位。

Description

基于二维LDV和惯导系统的容错组合导航方法及装置
技术领域
本申请涉及组合导航技术领域，特别是涉及一种基于二维LDV和惯导系统的容错组合导航方法及装置。
背景技术
激光多普勒测速仪(LDV)是一种基于激光散射的仪器，它利用运动粒子散射光的多普勒频移来确定粒子的速度，是一种完全自主的速度传感器，其具有测量精度高，非接触测量、空间分辨率好、动态响应快、测速范围广、方向灵敏度高等优点。与里程计相比，LDV的测量精度更高，且非接触测量这一优势使得LDV的输出不受载体轮胎状态的影响，且不存在单位转换所带来的延时。因此LDV在陆用自主导航领域有望得到大规模应用。
一维LDV与惯导系统组成的组合导航系统虽然有着很高的水平定位精度，但由于一维LDV无法提供载体的天向速度，因此一维LDV与惯导系统组成的组合导航系统在高度上估计精度不高。与一维LDV相比，二维LDV不仅可以提供车辆的前向速度还能提供车辆的天向速度，因此用二维LDV与惯导系统进行组合可以获得较为准确的高度信息。目前二维LDV与惯导系统的组合方式有松组合和紧组合两种，这两种组合方式都是基于二维LDV两个子光束所构建的二维测速系统，而忽略了二维LDV子光束自身所构建的一维测速系统，这使得二维LDV所带来的可用信息并未得到充分利用。为了将二维LDV所得到的一个二维测速系统与两个一维测速系统与惯导系统进行融合，卡尔曼滤波器是最为有效的方式之一，而集中式滤波是卡尔曼滤波器进行信息融合时最常用的处理方式。
集中式滤波将大系统中的所有子系统的状态变量包含在一起，构建成一个高维的卡尔曼滤波器。虽然集中式滤波在绝大多数时候都有着比分散式滤波更高的精度，但集中式滤波计算量大，且在子系统发生故障时不能很好对各个子系统进行隔离，避免有故障的子系统影响整个滤波器。因此在实际工程应用中，特别是不以精度为主要性能指标且各子系统拥有较多私有状态的系统可以牺牲些许精度来换取整个系统更高的容错能力。对于二维LDV与惯导系统构成的陆用组合导航系统，尤其是对二维LDV的所有信息进行充分利用时，提高整个系统的容错能力是十分必要的。
发明内容
基于此，有必要针对上述技术问题，提供一种基于二维LDV和惯导系统的容错组合导航方法及装置。
一种基于二维LDV和惯导系统的容错组合导航方法，所述方法包括：
构建包括二维激光多普勒测速仪和惯导系统的组合导航系统，根据二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度；
分别获取二维测速系统、两个一维测速系统以及惯导系统的误差项，根据二维测速系统的误差项将二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，根据一维测速系统的误差项将一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式；
根据惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，根据惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量；
根据惯导系统的误差项构建主滤波器，主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断，当判断子滤波器异常时，将异常的子滤波器进行隔离；当判断子滤波器正常时，主滤波器对所有正常子滤波器滤波得到的组合导航系统的局部状态估计进行融合，得到组合导航系统的全局状态估计，并根据信息共享因子将全局状态估计分配给所有正常子滤波器进行容错组合导航。
在其中一个实施例中，根据二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，包括：
根据二维激光多普勒测速仪的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，表示为，其中，/>表示二维测速系统在自身坐标系下的前向速度，/>表示二维测速系统在自身坐标系下的天向速度，/>表示沿二维激光多普勒测速仪的第一光束方向上的速度，/>表示沿二维激光多普勒测速仪的第二光束方向上的速度，/>表示两个光束夹角的二分之一，表示二维测速系统的自身坐标系，上标T表示矩阵转置。
在其中一个实施例中，根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度，包括：
根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度，表示为
；
；
其中，表示根据第一光束构建的一维测速系统在自身坐标系下的输出速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的输出速度，表示根据第一光束构建的一维测速系统在自身坐标系下的前向速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的前向速度，/>和/>表示两个比例因子，/>表示第一光束的出射倾角，/>表示第二光束的出射倾角，/>表示根据第一光束构建的一维测速系统的自身坐标系，/>表示根据第二光束构建的一维测速系统的自身坐标系。
在其中一个实施例中，二维测速系统的误差项包括二维测速系统的安装差角误差，以及的设计值与真实值之间的偏差/>，一维测速系统的误差项包括一维测速系统的安装差角误差，以及对应的两个比例因子误差/>、/>，惯导系统的误差项包括惯导系统的姿态误差/>、速度误差/>、位置误差/>、陀螺测量误差/>以及加速度计测量误差。
在其中一个实施例中，根据二维测速系统的误差项将二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，包括：
根据的设计值与真实值之间存在的偏差/>，获取二维测速系统在自身坐标系下的实际输出速度，表示为
；
其中，表示二维测速系统在自身坐标系下的实际前向速度，/>表示二维测速系统在自身坐标系下的实际天向速度，/>表示二维测速系统在自身坐标系下的输出速度；
根据与载体前向速度之间的倾角，将二维测速系统的自身坐标系/>进行旋转，得到旋转后的二维测速系统坐标系/>，并得到二维测速系统在/>系下的实际输出速度，表示为
；
其中，表示/>与载体前向速度之间的倾角，/>表示旋转矩阵，/>表示二维测速系统在/>系下的真实速度，/>为引起的速度偏差系数矩阵；
根据二维测速系统的安装差角误差将转换为导航坐标下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，表示为
；
其中，表示/>单位矩阵，/>表示惯导系统的姿态误差，/>表示真实的载体坐标系b到导航坐标系n的姿态转换矩阵，/>表示二维测速系统与惯导系统之间的安装差角误差，/>表示真实的m系到载体坐标系b的姿态转换矩阵。
在其中一个实施例中，根据一维测速系统的误差项将一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式，包括：
根据两个比例因子误差、/>，获取两个一维测速系统在自身坐标系下的实际输出速度，分别表示为
；
；
其中，表示根据第一光束构建的一维测速系统在自身坐标系下的实际输出速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的实际输出速度，表示根据第一光束构建的一维测速系统在自身坐标系下的实际前向速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的实际前向速度；
根据一维测速系统的安装差角误差将和/>转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式，分别表示为
；
；
其中，表示根据第一光束构建的一维测速系统在导航坐标系下的速度表达式，/>表示根据第二光束构建的一维测速系统在导航坐标系下的速度表达式，/>表示一维测速系统与惯导系统之间的安装差角误差，/>表示根据第一光束或第二光束构建的一维测速系统的自身坐标系，/>表示真实的/>系到载体坐标系b的姿态转换矩阵。
在其中一个实施例中，根据惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，包括：
将惯导系统的误差项和二维测速系统的误差项作为第一子滤波器状态向量，构建第一子滤波器的状态方程，表示为
；
其中，表示第一子滤波器，/>表示第一子滤波器的系统状态转移矩阵，表示第一子滤波器的系统噪声矩阵，/>表示第一子滤波器的系统噪声向量，/>表示第一子滤波器状态向量，/>为/>的微分；
将二维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第一子滤波器观测向量，构建第一子滤波器的量测方程，表示为
；
其中，表示第一子滤波器观测向量，/>表示第一子滤波器的量测矩阵，/>表示第一子滤波器的量测噪声向量，/>表示惯导系统在导航坐标系下的实际输出速度。
在其中一个实施例中，根据惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量，包括：
将惯导系统的误差项和根据第一光束构建的一维测速系统的误差项作为第二子滤波器状态向量，构建第二子滤波器的状态方程，表示为
；
其中，表示第二子滤波器，/>表示第二子滤波器系统状态转移矩阵，表示第二子滤波器系统噪声矩阵，/>表示第二子滤波器系统噪声向量，/>表示第二子滤波器状态向量，/>为/>的微分；
将根据第一光束构建的一维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第二子滤波器观测向量，构建第二子滤波器量测方程，表示为
；
其中，表示第二子滤波器观测向量，/>表示第二子滤波器的量测矩阵，/>表示第二子滤波器的量测噪声向量；
将惯导系统的误差项和根据第二光束构建的一维测速系统的误差项作为第三子滤波器状态向量，构建第三子滤波器的状态方程，表示为
；
其中，表示第三子滤波器，/>表示第三子滤波器的系统状态转移矩阵，表示第三子滤波器的系统噪声矩阵，/>表示第三子滤波器的系统噪声向量，表示第三子滤波器状态向量，/>为/>的微分；
将根据第二光束构建的一维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第三滤波器观测向量，构建第三子滤波器量测方程，表示为
；
其中，表示第三子滤波器观测向量，/>表示第三子滤波器的量测矩阵，/>表示第三子滤波器的量测噪声向量。
在其中一个实施例中，在根据惯导系统的误差项构建主滤波器之前，还包括：
根据子滤波器的新息矢量对各子滤波器的噪声协方差矩阵进行自适应处理，得到子滤波器中自适应处理后的噪声协方差矩阵，表示为
；
其中，表示第/>个子滤波器，/>表示/>时刻各子滤波器中自适应处理前的噪声协方差矩阵，/>表示自适应膨胀因子矩阵，/>表示/>中第/>个观测量所对应的膨胀因子，/>表示子滤波器离散化后/>时刻第/>个子滤波器的观测向量。
在其中一个实施例中，根据惯导系统的误差项构建主滤波器，包括：
将惯导系统的误差项作为主滤波器状态向量，构建主滤波器的状态方程，表示为
；
其中，表示主滤波器的系统状态转移矩阵，/>表示主滤波器的系统噪声矩阵，/>表示主滤波器的系统噪声向量，/>表示主滤波器状态向量，为/>的微分。
在其中一个实施例中，主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断的方式表示为
；
其中，表示/>时刻各子滤波器新息矢量的马氏距离，/>表示预设阈值，当时，判断子滤波器异常，将异常子滤波器进行隔离；当/>时，判断子滤波器正常，主滤波器对所有正常子滤波器滤波得到的组合导航系统的局部状态估计进行融合，得到组合导航系统的全局状态估计，并根据信息共享因子将全局状态估计分配给所有正常子滤波器进行容错组合导航。
在其中一个实施例中，信息共享因子根据子滤波器局部状态估计中的公共状态量以及主滤波器融合后的全局状态估计的马氏距离进行自适应更新，表示为
；
；
其中，表示子滤波器局部状态估计中的公共状态量以及主滤波器融合后的全局状态估计的马氏距离，当/>时，/>表示/>时刻子滤波器的信息共享因子，当时，/>表示/>时刻主滤波器的信息共享因子，/>表示/>时刻融合子滤波器信息的全局状态估计，/>表示/>时刻第/>个子滤波器的状态估计中包含公共状态量的部分，为/>与/>的协方差矩阵。
一种基于二维LDV和惯导系统的容错组合导航装置，所述装置包括：
测速模块，用于构建包括二维激光多普勒测速仪和惯导系统的组合导航系统，根据二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度；
坐标系转换模块，用于分别获取二维测速系统、两个一维测速系统以及惯导系统的误差项，根据二维测速系统的误差项将二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，根据一维测速系统的误差项将一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式；
子滤波器构建模块，用于根据惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，根据惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量；
组合导航模块，用于根据惯导系统的误差项构建主滤波器，主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断，当判断子滤波器异常时，将异常的子滤波器进行隔离；当判断子滤波器正常时，主滤波器对所有正常子滤波器滤波得到的组合导航系统的局部状态估计进行融合，得到组合导航系统的全局状态估计，并根据信息共享因子将全局状态估计分配给所有正常子滤波器进行容错组合导航。
上述基于二维LDV和惯导系统的容错组合导航方法及装置，通过将惯导系统与二维激光多普勒测速仪两个光束构建的二维测速系统以及两个光束各自构建的一维测速系统相结合来构建组合导航系统，充分利用了二维激光多普勒测速仪的两个光束的信息，并且在进行组合导航时，考虑到集中式滤波器具有计算量大以及不方便进行故障隔离等缺点，使用自适应联邦卡尔曼滤波将惯导系统与三个测速系统进行融合，对三个子滤波器进行实时故障诊断与隔离，以避免有故障的测速系统输出影响整个滤波器，从而提高组合导航系统的总体性能。不同于现有的二维激光多普勒测速仪与惯导系统松耦合和紧耦合的方法，本申请所提方法在保证了组合导航系统精度的情况下拥有更高的容错能力，在二维激光多普勒测速仪单个光束长时间失效的情况下仍能实现较高精度的导航定位。
附图说明
图1为一个实施例中基于二维LDV和惯导系统的容错组合导航方法的流程示意图；
图2为一个实施例中二维LDV在组合导航系统中的安装位置以及出射光路图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种基于二维LDV和惯导系统的容错组合导航方法，包括以下步骤：
步骤S1，构建包括二维激光多普勒测速仪和惯导系统的组合导航系统，根据二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度。
具体地，在构建测速系统之前，还需合理地安装惯导系统和二维激光多普勒测速仪，二维激光多普勒测速仪在组合导航系统中的安装位置以及出射光路如图2所示，本申请中所采用的惯导系统为惯性测量单元(IMU)，在使用时，将IMU安装在车辆后轮轴的中央，将二维LDV安装在惯导系统的正下方以尽量避免二维LDV与IMU之间的杆臂对定位结果造成影响。在导航开始前，还需对IMU进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏。在进入正式导航过程前还需要完成IMU的初始化，包括获得车辆的初始位置和速度信息并完成初始对准。
步骤S2，分别获取二维测速系统、两个一维测速系统以及惯导系统的误差项，根据二维测速系统的误差项将二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，根据一维测速系统的误差项将一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式。
可以理解，为了获得最佳的定位结果，由二维LDV所构建的二维测速系统和一维测速系统在使用前需要预先标定，但往往在实际使用过程中不会每次使用前都标定仪器，而之前的标定参数并不一定完全与当前时刻的标定参数一致，且车辆颠簸以及载体重心的变化也会导致标定参数出现细微变化。因此在组合导航过程中需要考虑标定参数的误差。
步骤S3，根据惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，根据惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量。
步骤S4，根据惯导系统的误差项构建主滤波器，主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断，当判断子滤波器异常时，将异常的子滤波器进行隔离；当判断子滤波器正常时，主滤波器对所有正常子滤波器滤波得到的组合导航系统的局部状态估计进行融合，得到组合导航系统的全局状态估计，并根据信息共享因子将全局状态估计分配给所有正常子滤波器进行容错组合导航。其中，新息是指滤波器的观测值减去预测值。
在其中一个实施例中，根据二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，包括：
根据二维激光多普勒测速仪的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，表示为，其中，表示二维测速系统在自身坐标系下的前向速度，/>表示二维测速系统在自身坐标系下的天向速度，/>表示沿二维激光多普勒测速仪的第一光束方向上的速度，/>表示沿二维激光多普勒测速仪的第二光束方向上的速度，/>表示两个光束夹角的二分之一，/>表示二维测速系统的自身坐标系，上标T表示矩阵转置，/>为二维LDV的光束波长，/>和/>为第一光束和第二光束所测量得到的多普勒频移。
在其中一个实施例中，根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度，包括：
根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度表示为
；
；
其中，表示根据第一光束构建的一维测速系统在自身坐标系下的输出速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的输出速度，表示根据第一光束构建的一维测速系统在自身坐标系下的前向速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的前向速度，/>和/>表示两个比例因子，/>表示第一光束的出射倾角，/>表示第二光束的出射倾角，/>表示根据第一光束构建的一维测速系统的自身坐标系，/>表示根据第二光束构建的一维测速系统的自身坐标系。
在其中一个实施例中，二维测速系统的误差项包括二维测速系统的安装差角误差，以及的设计值与真实值之间的偏差/>，一维测速系统的误差项包括一维测速系统的安装差角误差，以及对应的两个比例因子误差/>、/>，惯导系统的误差项包括惯导系统的姿态误差/>、速度误差/>、位置误差/>、陀螺测量误差/>以及加速度计测量误差。/>
在其中一个实施例中，根据二维测速系统的误差项将二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，包括：
二维测速系统在自身坐标系下的前向速度和天向速度/>与/>有关，由于/>的设计值与真实值之间存在偏差，因此二维测速系统在自身坐标系下的实际输出速度可以表示为
；
其中，表示二维测速系统在自身坐标系下的实际前向速度，/>表示二维测速系统在自身坐标系下的实际天向速度，/>表示二维测速系统在自身坐标系下的输出速度；
从图2可以看出，由于所构建的二维测速系统坐标系的y轴并不是指向车辆的前进方向，而是具有一定倾角，倾角的大小与二维激光多普勒测速仪两个光束的出射倾角密切相关，为了使得二维测速系统在自身坐标系下得到的前向速度的方向指向车辆前进方向，根据与载体前向速度之间的倾角，将二维测速系统的自身坐标系/>其x轴/>进行旋转，得到旋转后的二维测速系统坐标系/>，并得到二维测速系统在/>系下的实际输出速度，表示为
；
其中，表示/>与载体前向速度之间的倾角，/>表示二维测速系统在/>系下的真实速度，/>为/>引起的速度偏差系数矩阵，/>表示旋转矩阵，表示为/>
；
由于安装误差的存在，系与惯导系统所在的载体坐标系/>并不重合，二维测速系统与惯导系统之间存在安装差角，因此在进行组合导航前需要对二维测速系统与惯导系统之间的安装差角进行标定， 并根据二维测速系统的安装差角误差将/>转换为导航坐标下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，表示为
；
其中，表示/>单位矩阵，/>表示惯导系统的姿态误差，/>表示包含误差的载体坐标系b到导航坐标系n的姿态转换矩阵，/>表示真实的载体坐标系b到导航坐标系n的姿态转换矩阵，/>表示二维测速系统与惯导系统之间的安装差角误差，表示包含误差的m系到载体坐标系b的姿态转换矩阵，/>表示真实的m系到载体坐标系b的姿态转换矩阵。
在其中一个实施例中，根据一维测速系统的误差项将一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式，包括：
根据两个比例因子误差、/>，获取两个一维测速系统在自身坐标系下的实际输出速度，分别表示为
；
；
其中，表示根据第一光束构建的一维测速系统在自身坐标系下的实际输出速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的实际输出速度，表示根据第一光束构建的一维测速系统在自身坐标系下的实际前向速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的实际前向速度；
根据一维测速系统的安装差角误差将和/>转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式，分别表示为
；/>
；
其中，表示根据第一光束构建的一维测速系统在导航坐标系下的速度表达式，/>表示根据第二光束构建的一维测速系统在导航坐标系下的速度表达式，/>表示一维测速系统与惯导系统之间的安装差角误差，/>表示根据第一光束或第二光束构建的一维测速系统的自身坐标系，/>表示真实的/>系到载体坐标系b的姿态转换矩阵。可以理解，与二维测速系统一样，单个光束所构成的一维测速系统同样存在安装差角，且由于两个光束在同一平面上，因此两个一维测速系统具有相同的安装差角，即/>系和/>系重合，统一用/>系表示。
在其中一个实施例中，根据惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，包括：
将惯导系统的误差项和二维测速系统的误差项作为第一子滤波器状态向量，构建第一子滤波器的状态方程，表示为
；
其中，表示第一子滤波器，/>表示第一子滤波器的系统状态转移矩阵，表示第一子滤波器的系统噪声矩阵，/>表示第一子滤波器的系统噪声向量，/>表示第一子滤波器状态向量，/>为/>的微分，/>、/>、/>分别表示二维测速系统的俯仰安装差角误差，横滚安装差角误差以及航向安装差角误差，即/>；
将二维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第一子滤波器观测向量，构建第一子滤波器的量测方程，表示为
；
其中，表示第一子滤波器观测向量，/>表示第一子滤波器的量测矩阵，表示第一子滤波器的量测噪声向量，/>表示惯导系统在导航坐标系下的实际输出速度，/>表示车辆在/>系下的真实速度。
在其中一个实施例中，根据惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量，包括：
将惯导系统的误差项和根据第一光束构建的一维测速系统的误差项作为第二子滤波器状态向量，构建第二子滤波器的状态方程，表示为
；
其中，表示第二子滤波器，/>表示第二子滤波器系统状态转移矩阵，表示第二子滤波器系统噪声矩阵，/>表示第二子滤波器系统噪声向量，表示第二子滤波器状态向量，/>为/>的微分，/>和/>分别表示一维测速系统的俯仰安装差角误差和航向安装差角误差，即/>；
将根据第一光束构建的一维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第二子滤波器观测向量，构建第二子滤波器量测方程，表示为
；
其中，表示第二子滤波器观测向量，/>表示第二子滤波器的量测矩阵，/>表示第二子滤波器的量测噪声向量；
将惯导系统的误差项和根据第二光束构建的一维测速系统的误差项作为第三子滤波器状态向量，构建第三子滤波器的状态方程，表示为
；
其中，表示第三子滤波器，/>表示第三子滤波器的系统状态转移矩阵，表示第三子滤波器的系统噪声矩阵，/>表示第三子滤波器的系统噪声向量，/>表示第三子滤波器状态向量，/>为/>的微分；
将根据第二光束构建的一维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第三滤波器观测向量，构建第三子滤波器量测方程，表示为
；
其中，表示第三子滤波器观测向量，/>表示第三子滤波器的量测矩阵，/>表示第三子滤波器的量测噪声向量。
在其中一个实施例中，在根据惯导系统的误差项构建主滤波器之前，还包括：
根据子滤波器的新息矢量对各子滤波器的噪声协方差矩阵进行自适应处理，得到子滤波器中自适应处理后的噪声协方差矩阵，表示为
；
其中，表示第/>个子滤波器，/>表示/>时刻各子滤波器中自适应处理前的噪声协方差矩阵，/>表示自适应膨胀因子矩阵，/>表示/>中第j个观测量所对应的膨胀因子，/>表示子滤波器离散化后k时刻第i个子滤波器的观测向量。具体的，/>表示为/>，其中/>和/>分别表示/>和/>对角线上的第/>个元素，具体公式为：
表示由第/>个子滤波器新息的方差的实际值得到的测量噪声协方差矩阵；/>表示第/>个子滤波器新息的方差的实际值；/>表示第/>个子滤波器新息的方差的理论值；/>表示第/>个子滤波器的新息矢量。
其中，为渐消加权系数，/>，/>为渐消因子，/>为子滤波器离散化后/>时刻第/>个子滤波器的预测状态协方差矩阵，/>为子滤波器离散化后/>时刻第/>个子滤波器的量测矩阵，/>为子滤波器离散化后/>时刻第/>个子滤波器的一步预测状态向量。
可以理解，将常规卡尔曼滤波器用于组合导航系统会面临测量噪声协方差矩阵先验值要求过高的问题，且常规卡尔曼滤波器容易受到LDV不良数值的影响，因此对采用常规卡尔曼滤波器的子滤波器的噪声协方差矩阵进行自适应来提高子滤波器在实际应用中的自适应能力。
在其中一个实施例中，根据惯导系统的误差项构建主滤波器，包括：
将惯导系统的误差项作为主滤波器状态向量，由于主滤波器没有观测量，因此主滤波器只进行预测更新，根据主滤波器状态向量构建主滤波器的状态方程，表示为
；
其中，表示主滤波器的系统状态转移矩阵，/>表示主滤波器的系统噪声矩阵，/>表示主滤波器的系统噪声向量，/>表示主滤波器状态向量，/>为的微分。
在其中一个实施例中，主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断的方式表示为
；
；
；
其中，表示/>时刻各子滤波器新息矢量的马氏距离，/>表示预设阈值，表示自由度为n的卡方分布，当/>时，判断子滤波器异常，将异常子滤波器进行隔离；当/>时，判断子滤波器正常，主滤波器对所有正常子滤波器滤波得到的组合导航系统的局部状态估计进行融合，得到组合导航系统的全局状态估计，并根据信息共享因子将全局状态估计分配给所有正常子滤波器进行容错组合导航。
可以理解，当子滤波器的状态估计出现异常时，如果没有及时进行隔离，异常信息将进入主滤波器，最终导致主滤波器发散，因此有必要对子滤波器进行故障检测，并在子滤波器出现故障时将其隔离。
具体地，主滤波器进行信息融合过程如下：
；
；
其中，表示融合子滤波器信息得到的组合导航系统的全局状态估计，/>为/>对应的均方误差矩阵，式中的上标ci表示子滤波器中状态向量以及对应协方差矩阵中包含公共状态量的部分。
主滤波器根据信息共享因子进行全局状态估计分配地过程如下：
；
；/>
；
其中，为信息共享因子，且满足：/>，/>为主滤波器的信息共享因子。/>为分配给第/>个子滤波器的系统噪声矩阵，/>表示主滤波器融合子滤波器信息得到的系统噪声矩阵，/>为分配给第/>个子滤波器的公共状态量，/>为分配给第个子滤波器的状态均方差矩阵。
在其中一个实施例中，信息共享因子根据子滤波器局部状态估计中的公共状态量以及主滤波器融合后的全局状态估计的马氏距离进行自适应更新，表示为
；
；
其中，表示子滤波器局部状态估计中的公共状态量以及主滤波器融合后的全局状态估计的马氏距离，当/>时，/>表示k时刻子滤波器的信息共享因子，当时，/>表示k时刻主滤波器的信息共享因子，/>表示k时刻融合子滤波器信息的全局状态估计，/>表示/>时刻第/>个子滤波器的状态估计中包含公共状态量的部分，为/>与/>的协方差矩阵。可以理解，当/>越小，表示该子滤波器的状态估计与主滤波器融合后的全局状态估计越接近，因此在信息分配上应该分配更多的信息，即信息共享因子/>应该更大。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，提供了一种基于二维LDV和惯导系统的容错组合导航装置，包括：
测速模块，用于构建包括二维激光多普勒测速仪和惯导系统的组合导航系统，根据二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取二维测速系统在自身坐标系下的输出速度，根据二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取一维测速系统在自身坐标系下的输出速度；
坐标系转换模块，用于分别获取二维测速系统、两个一维测速系统以及惯导系统的误差项，根据二维测速系统的误差项将二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，根据一维测速系统的误差项将一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式；
子滤波器构建模块，用于根据惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，根据惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量；
组合导航模块，用于根据惯导系统的误差项构建主滤波器，主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断，当判断子滤波器异常时，将异常的子滤波器进行隔离；当判断子滤波器正常时，主滤波器对所有正常子滤波器滤波得到的组合导航系统的局部状态估计进行融合，得到组合导航系统的全局状态估计，并根据信息共享因子将全局状态估计分配给所有正常子滤波器进行容错组合导航。
关于基于二维LDV和惯导系统的容错组合导航装置的具体限定可以参见上文中对于基于二维LDV和惯导系统的容错组合导航方法的限定，在此不再赘述。上述基于二维LDV和惯导系统的容错组合导航装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。

Claims (13)

1.一种基于二维LDV和惯导系统的容错组合导航方法，其特征在于，所述方法包括：
构建包括二维激光多普勒测速仪和惯导系统的组合导航系统，根据所述二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取所述二维测速系统在自身坐标系下的输出速度，根据所述二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取所述一维测速系统在自身坐标系下的输出速度；
分别获取二维测速系统、两个一维测速系统以及惯导系统的误差项，根据所述二维测速系统的误差项将所述二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，根据所述一维测速系统的误差项将所述一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式；
根据所述惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将所述二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，根据所述惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量；
根据所述惯导系统的误差项构建主滤波器，所述主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断，当判断子滤波器异常时，将异常的子滤波器进行隔离；当判断子滤波器正常时，所述主滤波器对所有正常子滤波器滤波得到的所述组合导航系统的局部状态估计进行融合，得到所述组合导航系统的全局状态估计，并根据信息共享因子将所述全局状态估计分配给所有正常子滤波器进行容错组合导航。
2.根据权利要求1所述的方法，其特征在于，根据所述二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取所述二维测速系统在自身坐标系下的输出速度，包括：
根据二维激光多普勒测速仪的两个光束构建二维测速系统，并获取所述二维测速系统在自身坐标系下的输出速度，表示为，其中，/>表示所述二维测速系统在自身坐标系下的前向速度，/>表示所述二维测速系统在自身坐标系下的天向速度，/>表示沿二维激光多普勒测速仪的第一光束方向上的速度，/>表示沿二维激光多普勒测速仪的第二光束方向上的速度，/>表示两个光束夹角的二分之一，/>表示二维测速系统的自身坐标系，上标T表示矩阵转置。
3.根据权利要求2所述的方法，其特征在于，根据所述二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取所述一维测速系统在自身坐标系下的输出速度，包括：
根据所述二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取所述一维测速系统在自身坐标系下的输出速度，表示为
；
；
其中，表示根据第一光束构建的一维测速系统在自身坐标系下的输出速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的输出速度，表示根据第一光束构建的一维测速系统在自身坐标系下的前向速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的前向速度，/>和/>表示两个比例因子，/>表示第一光束的出射倾角，/>表示第二光束的出射倾角，/>表示根据第一光束构建的一维测速系统的自身坐标系，/>表示根据第二光束构建的一维测速系统的自身坐标系。
4.根据权利要求3所述的方法，其特征在于，所述二维测速系统的误差项包括二维测速系统的安装差角误差，以及的设计值与真实值之间的偏差/>，所述一维测速系统的误差项包括一维测速系统的安装差角误差，以及对应的两个比例因子误差/>、/>，所述惯导系统的误差项包括惯导系统的姿态误差/>、速度误差/>、位置误差/>、陀螺测量误差/>以及加速度计测量误差/>。
5.根据权利要求4所述的方法，其特征在于，根据所述二维测速系统的误差项将所述二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，包括：
根据的设计值与真实值之间存在的偏差/>，获取二维测速系统在自身坐标系下的实际输出速度，表示为
；
其中，表示二维测速系统在自身坐标系下的实际前向速度，/>表示二维测速系统在自身坐标系下的实际天向速度，/>表示二维测速系统在自身坐标系下的输出速度；
根据所述与载体前向速度之间的倾角，将所述二维测速系统的自身坐标系/>进行旋转，得到旋转后的二维测速系统坐标系/>，并得到二维测速系统在/>系下的实际输出速度，表示为
；
其中，表示所述/>与载体前向速度之间的倾角，/>表示旋转矩阵， />表示二维测速系统在/>系下的真实速度，/>为引起的速度偏差系数矩阵；
根据二维测速系统的安装差角误差将所述转换为导航坐标下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，表示为
；
其中，表示/>单位矩阵，/>表示惯导系统的姿态误差，/>表示真实的载体坐标系b到导航坐标系n的姿态转换矩阵，/>表示二维测速系统与惯导系统之间的安装差角误差，表示真实的m系到载体坐标系b的姿态转换矩阵。
6.根据权利要求5所述的方法，其特征在于，根据所述一维测速系统的误差项将所述一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式，包括：
根据两个比例因子误差、/>，获取两个一维测速系统在自身坐标系下的实际输出速度，分别表示为
；
；
其中，表示根据第一光束构建的一维测速系统在自身坐标系下的实际输出速度，表示根据第二光束构建的一维测速系统在自身坐标系下的实际输出速度，表示根据第一光束构建的一维测速系统在自身坐标系下的实际前向速度，/>表示根据第二光束构建的一维测速系统在自身坐标系下的实际前向速度；
根据一维测速系统的安装差角误差将所述和/>转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式，分别表示为
；
；
其中，表示根据第一光束构建的一维测速系统在导航坐标系下的速度表达式，表示根据第二光束构建的一维测速系统在导航坐标系下的速度表达式，/>表示一维测速系统与惯导系统之间的安装差角误差，/>表示根据第一光束或第二光束构建的一维测速系统的自身坐标系，/>表示真实的/>系到载体坐标系b的姿态转换矩阵。
7.根据权利要求6所述的方法，其特征在于，根据所述惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将所述二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，包括：
将所述惯导系统的误差项和二维测速系统的误差项作为第一子滤波器状态向量，构建第一子滤波器的状态方程，表示为
；
其中，表示第一子滤波器，/>表示第一子滤波器的系统状态转移矩阵，表示第一子滤波器的系统噪声矩阵，/>表示第一子滤波器的系统噪声向量，/>表示第一子滤波器状态向量，/>为/>的微分；
将二维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第一子滤波器观测向量，构建第一子滤波器的量测方程，表示为
；
其中，表示第一子滤波器观测向量，/>表示第一子滤波器的量测矩阵，表示第一子滤波器的量测噪声向量，/>表示惯导系统在导航坐标系下的实际输出速度。
8.根据权利要求7所述的方法，其特征在于，根据所述惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量，包括：
将惯导系统的误差项和根据第一光束构建的一维测速系统的误差项作为第二子滤波器状态向量，构建第二子滤波器的状态方程，表示为
；
其中，表示第二子滤波器，/>表示第二子滤波器系统状态转移矩阵，/>表示第二子滤波器系统噪声矩阵，/>表示第二子滤波器系统噪声向量，/>表示第二子滤波器状态向量，/>为/>的微分；
将根据第一光束构建的一维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第二子滤波器观测向量，构建第二子滤波器量测方程，表示为
；
其中，表示第二子滤波器观测向量，/>表示第二子滤波器的量测矩阵，表示第二子滤波器的量测噪声向量；
将惯导系统的误差项和根据第二光束构建的一维测速系统的误差项作为第三子滤波器状态向量，构建第三子滤波器的状态方程，表示为
；
其中，表示第三子滤波器，/>表示第三子滤波器的系统状态转移矩阵，/>表示第三子滤波器的系统噪声矩阵，/>表示第三子滤波器的系统噪声向量，/>表示第三子滤波器状态向量，/>为/>的微分；
将根据第二光束构建的一维测速系统在导航坐标系下的速度表达式与惯导系统在导航坐标系下的速度之差作为第三滤波器观测向量，构建第三子滤波器量测方程，表示为
；
其中，表示第三子滤波器观测向量，/>表示第三子滤波器的量测矩阵，表示第三子滤波器的量测噪声向量。
9.根据权利要求8所述的方法，其特征在于，在根据所述惯导系统的误差项构建主滤波器之前，还包括：
根据子滤波器的新息矢量对各子滤波器的噪声协方差矩阵进行自适应处理，得到子滤波器中自适应处理后的噪声协方差矩阵，表示为
；
其中，表示第/>个子滤波器，/>表示/>时刻各子滤波器中自适应处理前的噪声协方差矩阵，/>表示自适应膨胀因子矩阵，/>表示/>中第/>个观测量所对应的膨胀因子，/>表示子滤波器离散化后/>时刻第/>个子滤波器的观测向量。
10.根据权利要求9所述的方法，其特征在于，根据所述惯导系统的误差项构建主滤波器，包括：
将惯导系统的误差项作为主滤波器状态向量，构建主滤波器的状态方程，表示为
；
其中，表示主滤波器的系统状态转移矩阵，/>表示主滤波器的系统噪声矩阵，/>表示主滤波器的系统噪声向量，/>表示主滤波器状态向量，/>为/>的微分。
11.根据权利要求10所述的方法，其特征在于，所述主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断的方式表示为
；
其中，表示/>时刻各子滤波器新息矢量的马氏距离，/>表示预设阈值，当时，判断子滤波器异常，将异常子滤波器进行隔离；当/>时，判断子滤波器正常，所述主滤波器对所有正常子滤波器滤波得到的所述组合导航系统的局部状态估计进行融合，得到所述组合导航系统的全局状态估计，并根据信息共享因子将所述全局状态估计分配给所有正常子滤波器进行容错组合导航。
12.根据权利要求11所述的方法，其特征在于，所述信息共享因子根据子滤波器局部状态估计中的公共状态量以及主滤波器融合后的全局状态估计的马氏距离进行自适应更新，表示为
；
；
其中，表示子滤波器局部状态估计中的公共状态量以及主滤波器融合后的全局状态估计的马氏距离，当/>时，/>表示k时刻子滤波器的信息共享因子，当时，/>表示k时刻主滤波器的信息共享因子，/>表示k时刻融合子滤波器信息的全局状态估计，/>表示k时刻第/>个子滤波器的状态估计中包含公共状态量的部分，为/>与/>的协方差矩阵。
13.一种基于二维LDV和惯导系统的容错组合导航装置，其特征在于，所述装置包括：
测速模块，用于构建包括二维激光多普勒测速仪和惯导系统的组合导航系统，根据所述二维激光多普勒测速仪中的两个光束构建二维测速系统，并获取所述二维测速系统在自身坐标系下的输出速度，根据所述二维激光多普勒测速仪的单个光束构建两个一维测速系统，并获取所述一维测速系统在自身坐标系下的输出速度；
坐标系转换模块，用于分别获取二维测速系统、两个一维测速系统以及惯导系统的误差项，根据所述二维测速系统的误差项将所述二维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到二维测速系统在导航坐标系下的速度表达式，根据所述一维测速系统的误差项将所述一维测速系统在自身坐标系下的输出速度转换为导航坐标系下的速度表达式，得到一维测速系统在导航坐标系下的速度表达式；
子滤波器构建模块，用于根据所述惯导系统的误差项和二维测速系统的误差项构建第一子滤波器，并将所述二维测速系统与惯导系统在导航坐标系下的速度差作为第一子滤波器观测向量，根据所述惯导系统的误差项和任意一个一维测速系统的误差项构建第二子滤波器以及第三子滤波器，并将对应的一维测速系统与惯导系统在导航坐标系下的速度差作为第二子滤波器观测向量和第三子滤波器观测向量；
组合导航模块，用于根据所述惯导系统的误差项构建主滤波器，所述主滤波器根据子滤波器新息矢量的马氏距离对各个子滤波器进行故障判断，当判断子滤波器异常时，将异常的子滤波器进行隔离；当判断子滤波器正常时，所述主滤波器对所有正常子滤波器滤波得到的所述组合导航系统的局部状态估计进行融合，得到所述组合导航系统的全局状态估计，并根据信息共享因子将所述全局状态估计分配给所有正常子滤波器进行容错组合导航。
CN202311512395.9A 2023-11-14 2023-11-14 基于二维ldv和惯导系统的容错组合导航方法及装置 Active CN117249826B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311512395.9A CN117249826B (zh) 2023-11-14 2023-11-14 基于二维ldv和惯导系统的容错组合导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311512395.9A CN117249826B (zh) 2023-11-14 2023-11-14 基于二维ldv和惯导系统的容错组合导航方法及装置

Publications (2)

Publication Number Publication Date
CN117249826A true CN117249826A (zh) 2023-12-19
CN117249826B CN117249826B (zh) 2024-01-30

Family

ID=89131628

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311512395.9A Active CN117249826B (zh) 2023-11-14 2023-11-14 基于二维ldv和惯导系统的容错组合导航方法及装置

Country Status (1)

Country Link
CN (1) CN117249826B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015113329A1 (zh) * 2014-01-28 2015-08-06 北京融智利达科技有限公司 一种基于mems惯导的组合车载导航系统
JP2022016762A (ja) * 2020-07-13 2022-01-25 アクト電子株式会社 レーザドップラ速度計測装置
CN114812545A (zh) * 2022-04-19 2022-07-29 中国人民解放军国防科技大学 基于双激光多普勒测速仪和惯导系统组合导航方法及装置
CN115856922A (zh) * 2022-12-30 2023-03-28 中国人民解放军国防科技大学 一种松耦合陆地组合导航方法、装置、计算机设备和介质
CN115900705A (zh) * 2022-12-30 2023-04-04 中国人民解放军国防科技大学 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN115950450A (zh) * 2022-12-30 2023-04-11 中国人民解放军国防科技大学 二维多普勒测速仪的标定方法、装置、计算机设备和介质
CN116337000A (zh) * 2023-03-06 2023-06-27 中国人民解放军国防科技大学 基于sins/ldv组合的连续高程测量方法及系统
CN116594000A (zh) * 2023-05-23 2023-08-15 中国人民解放军国防科技大学 基于位置观测的激光多普勒测速仪在线标定方法和装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015113329A1 (zh) * 2014-01-28 2015-08-06 北京融智利达科技有限公司 一种基于mems惯导的组合车载导航系统
JP2022016762A (ja) * 2020-07-13 2022-01-25 アクト電子株式会社 レーザドップラ速度計測装置
CN114812545A (zh) * 2022-04-19 2022-07-29 中国人民解放军国防科技大学 基于双激光多普勒测速仪和惯导系统组合导航方法及装置
CN115856922A (zh) * 2022-12-30 2023-03-28 中国人民解放军国防科技大学 一种松耦合陆地组合导航方法、装置、计算机设备和介质
CN115900705A (zh) * 2022-12-30 2023-04-04 中国人民解放军国防科技大学 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN115950450A (zh) * 2022-12-30 2023-04-11 中国人民解放军国防科技大学 二维多普勒测速仪的标定方法、装置、计算机设备和介质
CN116337000A (zh) * 2023-03-06 2023-06-27 中国人民解放军国防科技大学 基于sins/ldv组合的连续高程测量方法及系统
CN116594000A (zh) * 2023-05-23 2023-08-15 中国人民解放军国防科技大学 基于位置观测的激光多普勒测速仪在线标定方法和装置

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
XIANG, ZHIYI ET AL.: "A SINS/GNSS/2D-LDV integrated navigation scheme for unmanned ground vehicles", 《MEASUREMENT SCIENCE AND TECHNOLOGY》, vol. 34, no. 12, pages 1 - 13, XP020477201, DOI: 10.1088/1361-6501/acf2b4 *
ZHOU, JIAN ET AL.: "A novel laser Doppler velocimeter and its integrated navigation system with strapdown inertial navigation", 《OPTICS AND LASER TECHNOLOGY》, vol. 64, pages 319 - 323, XP029038323, DOI: 10.1016/j.optlastec.2014.06.001 *
厉文涛等: "基于二维激光多普勒测速仪建立新组合导航系统的方法", 《中国激光》, vol. 47, no. 3, pages 1 - 6 *
王琦;高春峰;聂晓明;周健;魏国;龙兴武;: "陆用组合导航中二维激光多普勒测速仪的标定方法", 中国激光, no. 08, pages 151 - 157 *
陈红江等: "基于二维激光多普勒测速仪的车载组合导航系统", 《红外与激光工程》, vol. 47, no. 12, pages 1 - 7 *

Also Published As

Publication number Publication date
CN117249826B (zh) 2024-01-30

Similar Documents

Publication Publication Date Title
US11827166B2 (en) Systems and methods for impact detection with noise attenuation of a sensor signal
JP5258362B2 (ja) パリティロジックを使用する慣性測定装置の故障検出、分離、再構成
JP5270184B2 (ja) 衛星航法／推測航法統合測位装置
EP1760431B1 (en) Inertial navigation system with a plurality of Kalman filters and vehicle equipped with such a system
US10955261B2 (en) Air data attitude reference system
US7421343B2 (en) Systems and methods for reducing vibration-induced errors in inertial sensors
CN107449443A (zh) 雷达高度计的完整性监视
CN110030999A (zh) 一种基于惯性导航的定位方法、装置、系统和车辆
US8898013B2 (en) Navigation device and process integrating several hybrid inertial navigation systems
US20150153460A1 (en) Sequential Estimation in a Real-Time Positioning or Navigation System Using Historical States
KR20060048581A (ko) 네비게이션 중복성을 위한 시스템
CN111044051B (zh) 一种复合翼无人机容错组合导航方法
US11451457B2 (en) Sensor delay time estimation device, sensor delay time estimation method and recording medium
Jafari et al. Inertial navigation accuracy increasing using redundant sensors
KR20210102262A (ko) 무결성 범위 결정 방법
KR101096113B1 (ko) 신뢰도가 향상된 항법 시스템 및 그 제공방법
CN117249826B (zh) 基于二维ldv和惯导系统的容错组合导航方法及装置
CN112105961A (zh) 基于多数据融合的定位方法、可移动平台及存储介质
KR101160630B1 (ko) 주행 모드 판별법, 상기 주행 모드 판별을 이용한 항법 시스템 및 그 제공방법
KR102093743B1 (ko) 센서 융합을 이용한 차선 레벨 자동차 위치 정보 측위 시스템
CN116594000A (zh) 基于位置观测的激光多普勒测速仪在线标定方法和装置
US11209457B2 (en) Abnormality detection device, abnormality detection method, and non-transitory tangible computer readable medium
JP7491065B2 (ja) 状態推定装置、及び状態推定方法、状態推定プログラム
Karimi et al. Localization of an autonomous underwater vehicle using a decentralized fusion architecture
JP7406570B2 (ja) 挙動予測による誤差及びインテグリティ評価

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