CN114812545A - 基于双激光多普勒测速仪和惯导系统组合导航方法及装置 - Google Patents

基于双激光多普勒测速仪和惯导系统组合导航方法及装置 Download PDF

Info

Publication number
CN114812545A
CN114812545A CN202210410200.9A CN202210410200A CN114812545A CN 114812545 A CN114812545 A CN 114812545A CN 202210410200 A CN202210410200 A CN 202210410200A CN 114812545 A CN114812545 A CN 114812545A
Authority
CN
China
Prior art keywords
laser doppler
doppler velocimeter
inertial navigation
coordinate system
error
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.)
Pending
Application number
CN202210410200.9A
Other languages
English (en)
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 CN202210410200.9A priority Critical patent/CN114812545A/zh
Publication of CN114812545A publication Critical patent/CN114812545A/zh
Pending legal-status Critical Current

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/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/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Operations Research (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computing Systems (AREA)
  • Navigation (AREA)

Abstract

本发明提出了一种基于双激光多普勒测速仪和惯导系统组合导航方法及装置，先构建两激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；将两激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为状态向量，构建滤波系统的状态方程；将惯导系统在载体坐标系中的位移增量与两激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为观测量，构建滤波系统的观测方程；由滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。本发明能够提高组合导航的精度。

Description

基于双激光多普勒测速仪和惯导系统组合导航方法及装置
技术领域
本发明属于组合导航技术领域，特别涉及一种基于双激光多普勒测速仪和惯导系统组合导航方法及装置。
背景技术
组合导航系统通过多个外部辅助传感器对所需信息进行测量，并通过卡尔曼滤波、因子图等方法将外部辅助传感器的测量值与捷联惯性导航系统(SINS)的解算值进行融合，并对导航信息和传感器的误差进行估算和修正。组合导航充分发挥了各传感器的优势，使各传感器的优势互补，并通过反馈实时修正导航结果和传感器误差，极大的提高了系统的导航精度和容错性。目前主流的组合导航系统有SINS/全球定位系统(GPS)、SINS/里程计(OD)、SINS/多普勒计程仪(DVL)、SINS/磁强计以及SINS/星敏感器等，但这些外部传感器在实际应用中都有着它们的局限性。
GPS的信号不稳定，抗干扰性较差且信号易被树木以及建筑物遮蔽，另外GPS信号在战时等紧急情况下存在被加密甚至人为篡改的可能性。OD的精度与载体的行驶状态以及车轮的状态密切相关，车轮温度、气压、磨损状况以及车辆行驶过程中出现的跳动、打滑都将使OD的测量精度降低。DVL时间延迟大、数据更新频率低、声波发散角大且不适用于陆上测量。星敏感器以及磁力计易受外界环境的干扰，因此大多只用于特定场合。
激光多普勒测速仪(LDV)是一种完全自主的速度传感器，有着测量精度高、空间分辨率好、动态响应快、测速范围广、方向灵敏度高以及非接触测量等优点，有望在组合导航领域得到广泛运用。但LDV的信号在积水路面或者凹陷路面会存在信号丢失的情况，这会影响组合导航的精度。在导航过程中，航向角误差是引起位置误差的主要原因，这也是低成本惯导系统无法在长时间保持定位精度的原因。
因此，需要一种完全自主的高精度、高鲁棒性组合导航方案来实现在无卫星信号下陆用车辆的长时间、高精度自主导航。
发明内容
针对冗余配置光纤(激光)陀螺旋转惯导的联合定位问题，本发明提出了基于双激光多普勒测速仪和惯导系统组合导航方法及装置。
为实现上述技术目的，本发明提出的技术方案为：
一方面，本发明提供一种基于双激光多普勒测速仪和惯导系统组合导航方法，包括：
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
基于所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
进一步地，本发明中在组合导航开始前，对惯导系统和两个激光多普勒测速仪进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏以及激光多普勒测速仪的刻度系数和安装误差角。
进一步地，本发明所述第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：
Figure BDA0003603855770000031
Figure BDA0003603855770000032
其中
Figure BDA0003603855770000033
为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，I3为3×3单位矩阵，×表示求解斜对称矩阵，φm1＝[φmx1 0 φmz1]T表示载体坐标系与第一激光多普勒测速仪所在坐标系的安装差角误差，φmx1为第一激光多普勒测速仪的俯仰安装角误差，φmz1为第一激光多普勒测速仪的航向安装角误差，
Figure BDA0003603855770000034
表示从第一激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK1表示第一激光多普勒测速仪的刻度系数误差，K1表示第一激光多普勒测速仪的刻度系数，
Figure BDA0003603855770000035
表示第一激光多普勒测速仪的输出速度，
Figure BDA0003603855770000036
表示载体坐标系中第一激光多普勒测速仪的速度误差。
进一步地，本发明所述第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：
Figure BDA0003603855770000037
Figure BDA0003603855770000038
其中
Figure BDA0003603855770000039
为第二激光多普勒测速仪的输出速度在载体坐标系中的投影，φm2＝[φmx20φmz2]T表示载体坐标系与第二激光多普勒测速仪所在坐标系的安装差角误差，φmx2为第二激光多普勒测速仪的俯仰安装角误差，φmz2为第二激光多普勒测速仪的航向安装角误差，
Figure BDA00036038557700000310
表示从第二激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK2表示第二激光多普勒测速仪的刻度系数误差，K2表示第二激光多普勒测速仪的刻度系数，
Figure BDA00036038557700000311
表示第二激光多普勒测速仪的输出速度，
Figure BDA00036038557700000312
表示载体坐标系中第二激光多普勒测速仪的速度误差。
进一步地，本发明所述滤波系统为卡尔曼滤波器，卡尔曼滤波器的状态方程为：
Figure BDA0003603855770000041
其中X(t)为组合导航的状态向量，F(t)为系统状态转移矩阵，G(t)为系统噪声矩阵，w(t)为系统噪声向量。
进一步地，本发明中，在Δt时间内，惯导系统在载体坐标系中的位移增量为
Figure BDA0003603855770000042
第一激光多普勒测速仪在载体坐标系下的位移增量为
Figure BDA0003603855770000043
第二激光多普勒测速仪在载体坐标系下的位移增量为
Figure BDA0003603855770000044
卡尔曼滤波器的观测方程为：
Figure BDA0003603855770000045
其中H(t)为观测矩阵，V(t)为观测噪声向量，X(t)为组合导航的状态向量，
Figure BDA0003603855770000046
为惯导系统在载体坐标系中的输出速度，ψSINS为惯导系统得到的航向角，ψLDV为由两个激光多普勒测速仪得到的航向角，
Figure BDA0003603855770000047
为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，
Figure BDA0003603855770000048
为第二激光多普勒测速仪的输出速度在载体坐标系中的投影。
进一步地，由两个激光多普勒测速仪得到的航向角ψLDV，通过下式计算：
Figure BDA0003603855770000049
其中：d为两个激光多普勒测速仪之间的距离，D1和D2分别为在Δt时间内第一激光多普勒测速仪和第二激光多普勒测速仪的位移增量。
另一方面地，本发明提供一种基于双激光多普勒测速仪和惯导系统组合导航装置，包括：
第一模块，用于考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
第二模块，用于将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
第三模块，用于将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
第四模块，利用所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
另一方面，本发明提供一种基于双激光多普勒测速仪和惯导系统组合导航设备，包括至少两个激光多普勒测速仪和至少一套惯导系统，任意两个激光多普勒测速仪和任意一套惯导系统之间通过所述基于双激光多普勒测速仪和惯导系统组合导航方法实现定位。
另一方面，本发明提供一种车辆，所述车辆上搭载有上述基于双激光多普勒测速仪和惯导系统组合导航设备。所述惯导系统是指捷联惯性导航系统(SINS)。捷联惯性导航系统(SINS)在使用时，将惯性测量的单元(IMU)安装在车辆后轮轴的中央，将两个激光多普勒测速仪平行安装在车辆的侧面或者尾部合适的位置，以便测速仪输出的光束平行且能够有效打在地面上，并实时获得有效的载体速度信息。
另一方面，本发明提供一种计算机设备，包括存储器和处理器，存储器存储有计算机程序，处理器执行计算机程序时实现以下步骤：
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
基于所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
再一方面，本发明还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
基于所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
本发明提供了一种完全自主的高精度、高鲁棒性组合导航方案，能够实现在无卫星信号下陆用车辆的长时间、高精度自主导航。与现有技术相比，本发明的优点在于：
本发明通过使用两个激光多普勒测速仪与惯导系统进行组合，不仅充分利用了激光多普勒测速仪测量精度高、空间分辨率好、动态响应快、测速范围广、方向灵敏度高以及非接触测量等优势；
本发明还提高了系统的容错率，在一个激光多普勒测速仪存在信号丢失的情况下，另一个激光多普勒测速仪提供的信息能有效保证系统的精度。
此外，本发明采用位移增量而不是传统的速度信息作为观测量能有效减弱激光多普勒测速仪和惯导系统的噪声对滤波的影响。
最后，本发明通过两个激光多普勒测速仪获得的载体航向信息可以对导航过程中惯导的航向角误差进行约束，以提高组合导航的精度，尤其是低精度的惯导系统。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1是本发明一实施例的流程图；
图2是本发明一实施例中两个激光多普勒测速仪以及惯导系统中惯性测量单元(IMU)的安装关系以及双激光多普勒测速仪测量载体航向图；
图3为本发明一实施例的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述来清楚说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。
在一实施例中，参照图1，提供一种基于双激光多普勒测速仪和惯导系统组合导航方法，包括：
(S1)分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
(S2)将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
(S3)将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
(S4)基于所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
其中的惯导系统是指捷联惯性导航系统(SINS)。在进行组合导航前，需要对捷联惯性导航系统中惯性测量单元和两个激光多普勒测速仪进行标定，获得两个激光多普勒测速仪的刻度系数误差以及安装角误差，同时获得捷联惯性导航系统中惯性测量单元的陀螺和加速度计的刻度系数误差、安装误差角、零偏。在进入正式导航过程前还需要完成系统的初始化，包括获得同时搭载有所述捷联惯性导航系统以及两个激光多普勒测速仪的载体的初始位置和速度信息以及完成初始对准。
在一实施例的(S1)中，考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型。
其中，所述第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：
Figure BDA0003603855770000091
Figure BDA0003603855770000092
其中
Figure BDA0003603855770000093
为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，I3为3×3单位矩阵，×表示求解斜对称矩阵，φm1＝[φmx1 0 φmz1]T表示载体坐标系与第一激光多普勒测速仪所在坐标系的安装差角误差，φmx1为第一激光多普勒测速仪的俯仰安装角误差，φmz1为第一激光多普勒测速仪的航向安装角误差，
Figure BDA0003603855770000094
表示从第一激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK1表示第一激光多普勒测速仪的刻度系数误差，K1表示第一激光多普勒测速仪的刻度系数，
Figure BDA0003603855770000095
表示第一激光多普勒测速仪的输出速度，
Figure BDA0003603855770000096
表示载体坐标系中第一激光多普勒测速仪的速度误差。
其中，所述第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：
Figure BDA0003603855770000097
Figure BDA0003603855770000098
其中
Figure BDA0003603855770000099
为第二激光多普勒测速仪的输出速度在载体坐标系中的投影，φm2＝[φmx2 0 φmz2]T表示载体坐标系与第二激光多普勒测速仪所在坐标系的安装差角误差，φmx2为第二激光多普勒测速仪的俯仰安装角误差，φmz2为第二激光多普勒测速仪的航向安装角误差，
Figure BDA00036038557700000910
表示从第二激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK2表示第二激光多普勒测速仪的刻度系数误差，K2表示第二激光多普勒测速仪的刻度系数，
Figure BDA00036038557700000911
表示第二激光多普勒测速仪的输出速度，
Figure BDA00036038557700000912
表示载体坐标系中第二激光多普勒测速仪的速度误差。
在一实施例的(S2)中，将两个激光多普勒测速仪的刻度系数误差φm1、φm2和安装角误差δK1、δK2以及惯导系统的惯导姿态误差
Figure BDA00036038557700000913
速度误差δυn、位置误差δP、陀螺测量误差
Figure BDA00036038557700000914
加速度计测量误差
Figure BDA00036038557700000915
作为组合导航的状态向量X(t)，有：
Figure BDA0003603855770000101
惯导系统的误差模型为：
Figure BDA0003603855770000102
Figure BDA0003603855770000103
Figure BDA0003603855770000104
Figure BDA0003603855770000105
Figure BDA0003603855770000106
Figure BDA0003603855770000107
Figure BDA0003603855770000108
其中
Figure BDA0003603855770000109
Figure BDA00036038557700001010
Figure BDA00036038557700001011
υn＝[υE υN υU]T为SINS在导航坐标系中的速度。L，λ，h分别表示纬度、经度和高度，其中位置误差δP＝[δL δλ δh]T。RM和RN分别表示载体所在地的地球子午圈和卯酉圈曲率半径。ωie表示地球的自转角速度。fn表示加速度计输出比力在导航坐标系中的投影。
根据上述误差模型以及状态向量X(t)，使用卡尔曼滤波来进行状态估计，滤波系统的状态方程为：
Figure BDA00036038557700001012
其中F(t)为系统状态转移矩阵，G(t)为系统噪声矩阵，w(t)为系统噪声向量。
系统状态转移矩阵F(t)为：
Figure BDA0003603855770000111
其中：
Figure BDA0003603855770000112
Figure BDA0003603855770000113
Figure BDA0003603855770000114
Figure BDA0003603855770000115
Figure BDA0003603855770000116
Figure BDA0003603855770000121
系统噪声矩阵G(t)为：
Figure BDA0003603855770000122
系统噪声向量w(t)为：
Figure BDA0003603855770000123
εwi和
Figure BDA0003603855770000124
分别表示陀螺和加速度计的噪声。
在本发明(S3)中，将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程。采用位移增量而不是传统的速度信息作为观测量能有效减弱激光多普勒测速仪和惯导系统的噪声对滤波的影响。
具体地，在一实施例中，设在Δt时间内，惯导系统在载体坐标系中的位移增量为
Figure BDA0003603855770000125
第一激光多普勒测速仪在载体坐标系下的位移增量为
Figure BDA0003603855770000126
第二激光多普勒测速仪在载体坐标系下的位移增量为
Figure BDA0003603855770000127
卡尔曼滤波器的观测方程为：
Figure BDA0003603855770000128
其中H(t)为观测矩阵，V(t)为观测噪声向量，X(t)为组合导航的状态向量，
Figure BDA0003603855770000129
为惯导系统在载体坐标系中的输出速度，ψSINS为惯导系统得到的航向角，ψLDV为由两个激光多普勒测速仪得到的航向角，
Figure BDA0003603855770000131
为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，
Figure BDA0003603855770000132
为第二激光多普勒测速仪的输出速度在载体坐标系中的投影。
Figure BDA0003603855770000133
Figure BDA0003603855770000134
Figure BDA0003603855770000135
以及
Figure BDA0003603855770000136
是第一个激光多普勒测速仪速度在载体坐标系下的东向、北向以及天向分量。
Figure BDA0003603855770000137
以及
Figure BDA0003603855770000138
是第二个激光多普勒测速仪速度在载体坐标系下的东向、北向以及天向分量。
在一实施例中，由两个激光多普勒测速仪获得载体的航向角的具体过程如图2所示。当陆用载体在行驶过程存在航向变化时，两个激光多普勒测速仪的位移增量会不一样。两个激光多普勒测速仪之间的距离d很容易测得，在Δt时间内第一个激光多普勒测速仪和第二个激光多普勒测速仪的位移增量分别为D1和D2。那么通过两个激光多普勒测速仪得到的载体航向角可以由以下方程表示：
Figure BDA0003603855770000139
最后根据卡尔曼滤波得到的状态向量对惯导系统和两个激光多普勒测速仪进行反馈校正，输出校正后的载体定位信息。
在一实施例中，提供一种基于双激光多普勒测速仪和惯导系统组合导航装置，包括：
第一模块，用于考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
第二模块，用于将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
第三模块，用于将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
第四模块，利用所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
上述各模块功能的实现方法，可以采用前述各实施例中相同的方法实现，在此不再赘述。
在一实施例中，提供一种基于双激光多普勒测速仪和惯导系统组合导航设备，包括至少两个激光多普勒测速仪和至少一套惯导系统，任意两个激光多普勒测速仪和任意一套惯导系统之间通过所述基于双激光多普勒测速仪和惯导系统组合导航方法实现定位。
在一实施例中，提供一种基于双激光多普勒测速仪和惯导系统组合导航设备，包括两个激光多普勒测速仪和一套惯导系统，两个激光多普勒测速仪和惯导系统之间通过所述基于双激光多普勒测速仪和惯导系统组合导航方法实现定位。
上述各实施例中的基于双激光多普勒测速仪和惯导系统组合导航设备可以搭载在任何有导航需求的车辆上，能够实现在无卫星信号下陆用车辆的长时间、高精度自主导航。本发明一实施例，提供一种车辆，所述车辆上搭载有上述基于双激光多普勒测速仪和惯导系统组合导航设备。所述惯导系统是指捷联惯性导航系统(SINS)。捷联惯性导航系统(SINS)在使用时，将惯性测量的单元(IMU)安装在车辆后轮轴的中央，将两个激光多普勒测速仪平行安装在车辆的侧面或者尾部合适的位置，以便测速仪输出的光束平行且能够有效打在地面上，并实时获得有效的载体速度信息。
在本实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图3所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储样本数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现上述实施例中基于双激光多普勒测速仪和惯导系统组合导航方法的步骤。
本领域技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中基于双激光多普勒测速仪和惯导系统组合导航方法的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中基于双激光多普勒测速仪和惯导系统组合导航方法的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (11)

1.基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，包括：
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
基于所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
2.根据权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，还包括：在组合导航开始前，对惯导系统和两个激光多普勒测速仪进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏以及激光多普勒测速仪的刻度系数和安装误差角。
3.根据权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：
Figure FDA0003603855760000011
Figure FDA0003603855760000012
其中
Figure FDA0003603855760000013
为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，I3为3×3单位矩阵，×表示求解斜对称矩阵，φm1＝[φmx1 0 φmz1]T表示载体坐标系与第一激光多普勒测速仪所在坐标系的安装差角误差，φmx1为第一激光多普勒测速仪的俯仰安装角误差，φmz1为第一激光多普勒测速仪的航向安装角误差，
Figure FDA0003603855760000021
表示从第一激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK1表示第一激光多普勒测速仪的刻度系数误差，K1表示第一激光多普勒测速仪的刻度系数，
Figure FDA0003603855760000022
表示第一激光多普勒测速仪的输出速度，
Figure FDA0003603855760000023
表示载体坐标系中第一激光多普勒测速仪的速度误差。
4.根据权利要求3所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：
Figure FDA0003603855760000024
Figure FDA0003603855760000025
其中
Figure FDA0003603855760000026
为第二激光多普勒测速仪的输出速度在载体坐标系中的投影，φm2＝[φmx2 0φmz2]T表示载体坐标系与第二激光多普勒测速仪所在坐标系的安装差角误差，φmx2为第二激光多普勒测速仪的俯仰安装角误差，φmz2为第二激光多普勒测速仪的航向安装角误差，
Figure FDA0003603855760000027
表示从第二激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK2表示第二激光多普勒测速仪的刻度系数误差，K2表示第二激光多普勒测速仪的刻度系数，
Figure FDA0003603855760000028
表示第二激光多普勒测速仪的输出速度，
Figure FDA0003603855760000029
表示载体坐标系中第二激光多普勒测速仪的速度误差。
5.根据权利要求1或2或3或4所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，所述滤波系统为卡尔曼滤波器，卡尔曼滤波器的状态方程为：
Figure FDA00036038557600000210
其中F(t)为系统状态转移矩阵，X(t)为组合导航的状态向量，G(t)为系统噪声矩阵，w(t)为系统噪声向量。
6.根据权利要求5所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，在Δt时间内，惯导系统在载体坐标系中的位移增量为
Figure FDA0003603855760000031
第一激光多普勒测速仪在载体坐标系下的位移增量为
Figure FDA0003603855760000032
第二激光多普勒测速仪在载体坐标系下的位移增量为
Figure FDA0003603855760000033
卡尔曼滤波器的观测方程为：
Figure FDA0003603855760000034
其中H(t)为观测矩阵，V(t)为观测噪声向量，X(t)为组合导航的状态向量，
Figure FDA0003603855760000035
为惯导系统在载体坐标系中的输出速度，ψSINS为惯导系统得到的航向角，ψLDV为由两个激光多普勒测速仪得到的航向角，
Figure FDA0003603855760000036
为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，
Figure FDA0003603855760000037
为第二激光多普勒测速仪的输出速度在载体坐标系中的投影。
7.根据权利要求6所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，由两个激光多普勒测速仪得到的航向角ψLDV，通过下式计算：
Figure FDA0003603855760000038
其中：d为两个激光多普勒测速仪之间的距离，D1和D2分别为在Δt时间内第一激光多普勒测速仪和第二激光多普勒测速仪的位移增量。
8.基于双激光多普勒测速仪和惯导系统组合导航装置，其特征在于，包括：
第一模块，用于考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；
第二模块，用于将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；
第三模块，用于将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；
第四模块，利用所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。
9.一种计算机设备，包括存储器和处理器，存储器存储有计算机程序，其特征在于：所述处理器执行计算机程序时实现如权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法中的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法中的步骤。
11.一种基于双激光多普勒测速仪和惯导系统组合导航设备，其特征在于：包括至少两个激光多普勒测速仪和至少一套惯导系统，任意两个激光多普勒测速仪和任意一套惯导系统之间通过如权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法实现定位。
CN202210410200.9A 2022-04-19 2022-04-19 基于双激光多普勒测速仪和惯导系统组合导航方法及装置 Pending CN114812545A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210410200.9A CN114812545A (zh) 2022-04-19 2022-04-19 基于双激光多普勒测速仪和惯导系统组合导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210410200.9A CN114812545A (zh) 2022-04-19 2022-04-19 基于双激光多普勒测速仪和惯导系统组合导航方法及装置

Publications (1)

Publication Number Publication Date
CN114812545A true CN114812545A (zh) 2022-07-29

Family

ID=82505647

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210410200.9A Pending CN114812545A (zh) 2022-04-19 2022-04-19 基于双激光多普勒测速仪和惯导系统组合导航方法及装置

Country Status (1)

Country Link
CN (1) CN114812545A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115979263A (zh) * 2023-03-21 2023-04-18 中国人民解放军国防科技大学 一种低空飞行载体导航方法及系统
CN116147577A (zh) * 2023-03-06 2023-05-23 中国人民解放军国防科技大学 基于单轴rins/ldv组合的连续高程测量方法及系统
CN117249826A (zh) * 2023-11-14 2023-12-19 中国人民解放军国防科技大学 基于二维ldv和惯导系统的容错组合导航方法及装置

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116147577A (zh) * 2023-03-06 2023-05-23 中国人民解放军国防科技大学 基于单轴rins/ldv组合的连续高程测量方法及系统
CN116147577B (zh) * 2023-03-06 2024-05-03 中国人民解放军国防科技大学 基于单轴rins/ldv组合的连续高程测量方法及系统
CN115979263A (zh) * 2023-03-21 2023-04-18 中国人民解放军国防科技大学 一种低空飞行载体导航方法及系统
CN117249826A (zh) * 2023-11-14 2023-12-19 中国人民解放军国防科技大学 基于二维ldv和惯导系统的容错组合导航方法及装置
CN117249826B (zh) * 2023-11-14 2024-01-30 中国人民解放军国防科技大学 基于二维ldv和惯导系统的容错组合导航方法及装置

Similar Documents

Publication Publication Date Title
CN114812545A (zh) 基于双激光多普勒测速仪和惯导系统组合导航方法及装置
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
US6859727B2 (en) Attitude change kalman filter measurement apparatus and method
CN102169184B (zh) 组合导航系统中测量双天线gps安装失准角的方法和装置
CN110780326A (zh) 一种车载组合导航系统和定位方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN108387227B (zh) 机载分布式pos的多节点信息融合方法及系统
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
CN103217699B (zh) 一种基于偏振信息的组合导航系统递推优化初始对准方法
CN104344836A (zh) 一种基于姿态观测的冗余惯导系统光纤陀螺系统级标定方法
CN104344837A (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
CN111141273A (zh) 基于多传感器融合的组合导航方法及系统
CN104764467A (zh) 空天飞行器惯性传感器误差在线自适应标定方法
KR20190003916A (ko) 항법용 관성센서 캘리브레이션 방법
CN114413895B (zh) 光纤陀螺旋转惯导联合定位方法、装置、设备及介质
CN112762961A (zh) 一种车载惯性里程计组合导航在线标定方法
CN108303063B (zh) 一种高精度车载组合高程测量方法
CN108303120B (zh) 一种机载分布式pos的实时传递对准的方法及装置
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN113074757B (zh) 车载惯导安装误差角的标定方法
Vaknin et al. Coarse leveling of gyro-free INS
CN113092822A (zh) 一种基于惯组的激光多普勒测速仪的在线标定方法和装置
CN115900705A (zh) 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN115856922A (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