CN114812545A - 基于双激光多普勒测速仪和惯导***组合导航方法及装置 - Google Patents
基于双激光多普勒测速仪和惯导***组合导航方法及装置 Download PDFInfo
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000006073 displacement reaction Methods 0.000 claims abstract description 54
- 238000001914 filtration Methods 0.000 claims abstract description 52
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 239000013598 vector Substances 0.000 claims abstract description 33
- 238000009434 installation Methods 0.000 claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000004590 computer program Methods 0.000 claims description 15
- 230000009977 dual effect Effects 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000035945 sensitivity Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Operations Research (AREA)
- Probability & Statistics with Applications (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Computational Biology (AREA)
- Computing Systems (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Navigation (AREA)
Abstract
本发明提出了一种基于双激光多普勒测速仪和惯导***组合导航方法及装置,先构建两激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;将两激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为状态向量,构建滤波***的状态方程;将惯导***在载体坐标系中的位移增量与两激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为观测量,构建滤波***的观测方程;由滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。本发明能够提高组合导航的精度。
Description
技术领域
本发明属于组合导航技术领域,特别涉及一种基于双激光多普勒测速仪和惯导***组合导航方法及装置。
背景技术
组合导航***通过多个外部辅助传感器对所需信息进行测量,并通过卡尔曼滤波、因子图等方法将外部辅助传感器的测量值与捷联惯性导航***(SINS)的解算值进行融合,并对导航信息和传感器的误差进行估算和修正。组合导航充分发挥了各传感器的优势,使各传感器的优势互补,并通过反馈实时修正导航结果和传感器误差,极大的提高了***的导航精度和容错性。目前主流的组合导航***有SINS/全球定位***(GPS)、SINS/里程计(OD)、SINS/多普勒计程仪(DVL)、SINS/磁强计以及SINS/星敏感器等,但这些外部传感器在实际应用中都有着它们的局限性。
GPS的信号不稳定,抗干扰性较差且信号易被树木以及建筑物遮蔽,另外GPS信号在战时等紧急情况下存在被加密甚至人为篡改的可能性。OD的精度与载体的行驶状态以及车轮的状态密切相关,车轮温度、气压、磨损状况以及车辆行驶过程中出现的跳动、打滑都将使OD的测量精度降低。DVL时间延迟大、数据更新频率低、声波发散角大且不适用于陆上测量。星敏感器以及磁力计易受外界环境的干扰,因此大多只用于特定场合。
激光多普勒测速仪(LDV)是一种完全自主的速度传感器,有着测量精度高、空间分辨率好、动态响应快、测速范围广、方向灵敏度高以及非接触测量等优点,有望在组合导航领域得到广泛运用。但LDV的信号在积水路面或者凹陷路面会存在信号丢失的情况,这会影响组合导航的精度。在导航过程中,航向角误差是引起位置误差的主要原因,这也是低成本惯导***无法在长时间保持定位精度的原因。
因此,需要一种完全自主的高精度、高鲁棒性组合导航方案来实现在无卫星信号下陆用车辆的长时间、高精度自主导航。
发明内容
针对冗余配置光纤(激光)陀螺旋转惯导的联合定位问题,本发明提出了基于双激光多普勒测速仪和惯导***组合导航方法及装置。
为实现上述技术目的,本发明提出的技术方案为:
一方面,本发明提供一种基于双激光多普勒测速仪和惯导***组合导航方法,包括:
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
基于所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
进一步地,本发明中在组合导航开始前,对惯导***和两个激光多普勒测速仪进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏以及激光多普勒测速仪的刻度系数和安装误差角。
进一步地,本发明所述第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型,分别为:
其中为第一激光多普勒测速仪的输出速度在载体坐标系中的投影,I3为3×3单位矩阵,×表示求解斜对称矩阵,φm1=[φmx1 0 φmz1]T表示载体坐标系与第一激光多普勒测速仪所在坐标系的安装差角误差,φmx1为第一激光多普勒测速仪的俯仰安装角误差,φmz1为第一激光多普勒测速仪的航向安装角误差,表示从第一激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵,δK1表示第一激光多普勒测速仪的刻度系数误差,K1表示第一激光多普勒测速仪的刻度系数,表示第一激光多普勒测速仪的输出速度,表示载体坐标系中第一激光多普勒测速仪的速度误差。
进一步地,本发明所述第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型,分别为:
其中为第二激光多普勒测速仪的输出速度在载体坐标系中的投影,φm2=[φmx20φmz2]T表示载体坐标系与第二激光多普勒测速仪所在坐标系的安装差角误差,φmx2为第二激光多普勒测速仪的俯仰安装角误差,φmz2为第二激光多普勒测速仪的航向安装角误差,表示从第二激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵,δK2表示第二激光多普勒测速仪的刻度系数误差,K2表示第二激光多普勒测速仪的刻度系数,表示第二激光多普勒测速仪的输出速度,表示载体坐标系中第二激光多普勒测速仪的速度误差。
进一步地,本发明所述滤波***为卡尔曼滤波器,卡尔曼滤波器的状态方程为:
其中X(t)为组合导航的状态向量,F(t)为***状态转移矩阵,G(t)为***噪声矩阵,w(t)为***噪声向量。
其中H(t)为观测矩阵,V(t)为观测噪声向量,X(t)为组合导航的状态向量,为惯导***在载体坐标系中的输出速度,ψSINS为惯导***得到的航向角,ψLDV为由两个激光多普勒测速仪得到的航向角,为第一激光多普勒测速仪的输出速度在载体坐标系中的投影,为第二激光多普勒测速仪的输出速度在载体坐标系中的投影。
进一步地,由两个激光多普勒测速仪得到的航向角ψLDV,通过下式计算:
其中:d为两个激光多普勒测速仪之间的距离,D1和D2分别为在Δt时间内第一激光多普勒测速仪和第二激光多普勒测速仪的位移增量。
另一方面地,本发明提供一种基于双激光多普勒测速仪和惯导***组合导航装置,包括:
第一模块,用于考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
第二模块,用于将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
第三模块,用于将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
第四模块,利用所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
另一方面,本发明提供一种基于双激光多普勒测速仪和惯导***组合导航设备,包括至少两个激光多普勒测速仪和至少一套惯导***,任意两个激光多普勒测速仪和任意一套惯导***之间通过所述基于双激光多普勒测速仪和惯导***组合导航方法实现定位。
另一方面,本发明提供一种车辆,所述车辆上搭载有上述基于双激光多普勒测速仪和惯导***组合导航设备。所述惯导***是指捷联惯性导航***(SINS)。捷联惯性导航***(SINS)在使用时,将惯性测量的单元(IMU)安装在车辆后轮轴的中央,将两个激光多普勒测速仪平行安装在车辆的侧面或者尾部合适的位置,以便测速仪输出的光束平行且能够有效打在地面上,并实时获得有效的载体速度信息。
另一方面,本发明提供一种计算机设备,包括存储器和处理器,存储器存储有计算机程序,处理器执行计算机程序时实现以下步骤:
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
基于所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
再一方面,本发明还提供一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现以下步骤:
考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
基于所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
本发明提供了一种完全自主的高精度、高鲁棒性组合导航方案,能够实现在无卫星信号下陆用车辆的长时间、高精度自主导航。与现有技术相比,本发明的优点在于:
本发明通过使用两个激光多普勒测速仪与惯导***进行组合,不仅充分利用了激光多普勒测速仪测量精度高、空间分辨率好、动态响应快、测速范围广、方向灵敏度高以及非接触测量等优势;
本发明还提高了***的容错率,在一个激光多普勒测速仪存在信号丢失的情况下,另一个激光多普勒测速仪提供的信息能有效保证***的精度。
此外,本发明采用位移增量而不是传统的速度信息作为观测量能有效减弱激光多普勒测速仪和惯导***的噪声对滤波的影响。
最后,本发明通过两个激光多普勒测速仪获得的载体航向信息可以对导航过程中惯导的航向角误差进行约束,以提高组合导航的精度,尤其是低精度的惯导***。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图示出的结构获得其他的附图。
图1是本发明一实施例的流程图;
图2是本发明一实施例中两个激光多普勒测速仪以及惯导***中惯性测量单元(IMU)的安装关系以及双激光多普勒测速仪测量载体航向图;
图3为本发明一实施例的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚明白,下面将以附图及详细叙述来清楚说明本发明所揭示内容的精神,任何所属技术领域技术人员在了解本发明内容的实施例后,当可由本发明内容所教示的技术,加以改变及修饰,其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明,但并不作为对本发明的限定。
在一实施例中,参照图1,提供一种基于双激光多普勒测速仪和惯导***组合导航方法,包括:
(S1)分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
(S2)将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
(S3)将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
(S4)基于所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
其中的惯导***是指捷联惯性导航***(SINS)。在进行组合导航前,需要对捷联惯性导航***中惯性测量单元和两个激光多普勒测速仪进行标定,获得两个激光多普勒测速仪的刻度系数误差以及安装角误差,同时获得捷联惯性导航***中惯性测量单元的陀螺和加速度计的刻度系数误差、安装误差角、零偏。在进入正式导航过程前还需要完成***的初始化,包括获得同时搭载有所述捷联惯性导航***以及两个激光多普勒测速仪的载体的初始位置和速度信息以及完成初始对准。
在一实施例的(S1)中,考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型。
其中,所述第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型,分别为:
其中为第一激光多普勒测速仪的输出速度在载体坐标系中的投影,I3为3×3单位矩阵,×表示求解斜对称矩阵,φm1=[φmx1 0 φmz1]T表示载体坐标系与第一激光多普勒测速仪所在坐标系的安装差角误差,φmx1为第一激光多普勒测速仪的俯仰安装角误差,φmz1为第一激光多普勒测速仪的航向安装角误差,表示从第一激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵,δK1表示第一激光多普勒测速仪的刻度系数误差,K1表示第一激光多普勒测速仪的刻度系数,表示第一激光多普勒测速仪的输出速度,表示载体坐标系中第一激光多普勒测速仪的速度误差。
其中,所述第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型,分别为:
其中为第二激光多普勒测速仪的输出速度在载体坐标系中的投影,φm2=[φmx2 0 φmz2]T表示载体坐标系与第二激光多普勒测速仪所在坐标系的安装差角误差,φmx2为第二激光多普勒测速仪的俯仰安装角误差,φmz2为第二激光多普勒测速仪的航向安装角误差,表示从第二激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵,δK2表示第二激光多普勒测速仪的刻度系数误差,K2表示第二激光多普勒测速仪的刻度系数,表示第二激光多普勒测速仪的输出速度,表示载体坐标系中第二激光多普勒测速仪的速度误差。
在一实施例的(S2)中,将两个激光多普勒测速仪的刻度系数误差φm1、φm2和安装角误差δK1、δK2以及惯导***的惯导姿态误差速度误差δυn、位置误差δP、陀螺测量误差加速度计测量误差作为组合导航的状态向量X(t),有:
惯导***的误差模型为:
其中
υn=[υE υN υU]T为SINS在导航坐标系中的速度。L,λ,h分别表示纬度、经度和高度,其中位置误差δP=[δL δλ δh]T。RM和RN分别表示载体所在地的地球子午圈和卯酉圈曲率半径。ωie表示地球的自转角速度。fn表示加速度计输出比力在导航坐标系中的投影。
***状态转移矩阵F(t)为:
其中:
***噪声矩阵G(t)为:
***噪声向量w(t)为:
在本发明(S3)中,将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程。采用位移增量而不是传统的速度信息作为观测量能有效减弱激光多普勒测速仪和惯导***的噪声对滤波的影响。
其中H(t)为观测矩阵,V(t)为观测噪声向量,X(t)为组合导航的状态向量,为惯导***在载体坐标系中的输出速度,ψSINS为惯导***得到的航向角,ψLDV为由两个激光多普勒测速仪得到的航向角,为第一激光多普勒测速仪的输出速度在载体坐标系中的投影,为第二激光多普勒测速仪的输出速度在载体坐标系中的投影。
在一实施例中,由两个激光多普勒测速仪获得载体的航向角的具体过程如图2所示。当陆用载体在行驶过程存在航向变化时,两个激光多普勒测速仪的位移增量会不一样。两个激光多普勒测速仪之间的距离d很容易测得,在Δt时间内第一个激光多普勒测速仪和第二个激光多普勒测速仪的位移增量分别为D1和D2。那么通过两个激光多普勒测速仪得到的载体航向角可以由以下方程表示:
最后根据卡尔曼滤波得到的状态向量对惯导***和两个激光多普勒测速仪进行反馈校正,输出校正后的载体定位信息。
在一实施例中,提供一种基于双激光多普勒测速仪和惯导***组合导航装置,包括:
第一模块,用于考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
第二模块,用于将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
第三模块,用于将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
第四模块,利用所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
上述各模块功能的实现方法,可以采用前述各实施例中相同的方法实现,在此不再赘述。
在一实施例中,提供一种基于双激光多普勒测速仪和惯导***组合导航设备,包括至少两个激光多普勒测速仪和至少一套惯导***,任意两个激光多普勒测速仪和任意一套惯导***之间通过所述基于双激光多普勒测速仪和惯导***组合导航方法实现定位。
在一实施例中,提供一种基于双激光多普勒测速仪和惯导***组合导航设备,包括两个激光多普勒测速仪和一套惯导***,两个激光多普勒测速仪和惯导***之间通过所述基于双激光多普勒测速仪和惯导***组合导航方法实现定位。
上述各实施例中的基于双激光多普勒测速仪和惯导***组合导航设备可以搭载在任何有导航需求的车辆上,能够实现在无卫星信号下陆用车辆的长时间、高精度自主导航。本发明一实施例,提供一种车辆,所述车辆上搭载有上述基于双激光多普勒测速仪和惯导***组合导航设备。所述惯导***是指捷联惯性导航***(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所述的基于双激光多普勒测速仪和惯导***组合导航方法,其特征在于,第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型,分别为:
4.根据权利要求3所述的基于双激光多普勒测速仪和惯导***组合导航方法,其特征在于,第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型,分别为:
8.基于双激光多普勒测速仪和惯导***组合导航装置,其特征在于,包括:
第一模块,用于考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差,分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型;
第二模块,用于将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导***的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量,构建滤波***的状态方程;
第三模块,用于将惯导***在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导***在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导***得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量,构建滤波***的观测方程;
第四模块,利用所构建的滤波***进行滤波,利用滤波得到的状态向量对双激光多普勒测速仪和惯导***进行反馈校正,输出校正后的定位结果。
9.一种计算机设备,包括存储器和处理器,存储器存储有计算机程序,其特征在于:所述处理器执行计算机程序时实现如权利要求1所述的基于双激光多普勒测速仪和惯导***组合导航方法中的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于:所述计算机程序被处理器执行时实现如权利要求1所述的基于双激光多普勒测速仪和惯导***组合导航方法中的步骤。
11.一种基于双激光多普勒测速仪和惯导***组合导航设备,其特征在于:包括至少两个激光多普勒测速仪和至少一套惯导***,任意两个激光多普勒测速仪和任意一套惯导***之间通过如权利要求1所述的基于双激光多普勒测速仪和惯导***组合导航方法实现定位。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210410200.9A CN114812545B (zh) | 2022-04-19 | 2022-04-19 | 基于双激光多普勒测速仪和惯导***组合导航方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210410200.9A CN114812545B (zh) | 2022-04-19 | 2022-04-19 | 基于双激光多普勒测速仪和惯导***组合导航方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114812545A true CN114812545A (zh) | 2022-07-29 |
CN114812545B CN114812545B (zh) | 2024-07-09 |
Family
ID=82505647
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210410200.9A Active CN114812545B (zh) | 2022-04-19 | 2022-04-19 | 基于双激光多普勒测速仪和惯导***组合导航方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114812545B (zh) |
Cited By (4)
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组合的连续高程测量方法及*** |
CN116559966A (zh) * | 2023-03-06 | 2023-08-08 | 中国人民解放军国防科技大学 | 基于sins/ldv组合的重力测量方法及*** |
CN117249826A (zh) * | 2023-11-14 | 2023-12-19 | 中国人民解放军国防科技大学 | 基于二维ldv和惯导***的容错组合导航方法及装置 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB0111256D0 (en) * | 2001-05-09 | 2001-06-27 | Bae Systems Plc | A GPS based terrain referenced navigation system |
US9304198B1 (en) * | 2013-10-04 | 2016-04-05 | Sandia Corporation | Navigator alignment using radar scan |
CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
-
2022
- 2022-04-19 CN CN202210410200.9A patent/CN114812545B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB0111256D0 (en) * | 2001-05-09 | 2001-06-27 | Bae Systems Plc | A GPS based terrain referenced navigation system |
US9304198B1 (en) * | 2013-10-04 | 2016-04-05 | Sandia Corporation | Navigator alignment using radar scan |
CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
Non-Patent Citations (1)
Title |
---|
穆华 等: "基于步进式运动模型的单兵协同导航算法设计", 导航定位与守授时, 12 April 2021 (2021-04-12) * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116147577A (zh) * | 2023-03-06 | 2023-05-23 | 中国人民解放军国防科技大学 | 基于单轴rins/ldv组合的连续高程测量方法及*** |
CN116559966A (zh) * | 2023-03-06 | 2023-08-08 | 中国人民解放军国防科技大学 | 基于sins/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和惯导***的容错组合导航方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN114812545B (zh) | 2024-07-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114812545B (zh) | 基于双激光多普勒测速仪和惯导***组合导航方法及装置 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
US6859727B2 (en) | Attitude change kalman filter measurement apparatus and method | |
CN102169184B (zh) | 组合导航***中测量双天线gps安装失准角的方法和装置 | |
CN102830414B (zh) | 一种基于sins/gps的组合导航方法 | |
CN108387227B (zh) | 机载分布式pos的多节点信息融合方法及*** | |
US20100121601A1 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
CN102252677A (zh) | 一种基于时间序列分析的变比例自适应联邦滤波方法 | |
CN103217699B (zh) | 一种基于偏振信息的组合导航***递推优化初始对准方法 | |
CN111536972B (zh) | 一种基于里程计刻度系数修正的车载dr导航方法 | |
CN104697526A (zh) | 用于农业机械的捷联惯导***以及控制方法 | |
CN104344837A (zh) | 一种基于速度观测的冗余惯导***加速度计***级标定方法 | |
CN104344836A (zh) | 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法 | |
CN111141273A (zh) | 基于多传感器融合的组合导航方法及*** | |
CN112762961A (zh) | 一种车载惯性里程计组合导航在线标定方法 | |
KR20190003916A (ko) | 항법용 관성센서 캘리브레이션 방법 | |
CN114413895B (zh) | 光纤陀螺旋转惯导联合定位方法、装置、设备及介质 | |
CN108303063B (zh) | 一种高精度车载组合高程测量方法 | |
CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合***进行航向校正的方法 | |
CN115856922A (zh) | 一种松耦合陆地组合导航方法、装置、计算机设备和介质 | |
CN113092822A (zh) | 一种基于惯组的激光多普勒测速仪的在线标定方法和装置 | |
CN117053782A (zh) | 一种水陆两栖机器人组合导航方法 | |
CN112946681A (zh) | 融合组合导航信息的激光雷达定位方法 | |
CN113074757B (zh) | 车载惯导安装误差角的标定方法 | |
Vaknin et al. | Coarse leveling of gyro-free INS |
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 |