CN108957508B - 车载pos离线组合估计方法和装置 - Google Patents

车载pos离线组合估计方法和装置 Download PDF

Info

Publication number
CN108957508B
CN108957508B CN201810156874.4A CN201810156874A CN108957508B CN 108957508 B CN108957508 B CN 108957508B CN 201810156874 A CN201810156874 A CN 201810156874A CN 108957508 B CN108957508 B CN 108957508B
Authority
CN
China
Prior art keywords
vehicle
equation
pos
estimation
mounted pos
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
CN201810156874.4A
Other languages
English (en)
Other versions
CN108957508A (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.)
EAST CHINA RESEARCH INSTITUTE OF ELECTRONIC ENGINEERING (NO 38 RESEARCH INSTITUTE OF CHINA ELECTRONICS TECHNOLOGY Group Corp.)
Beihang University
Original Assignee
East China Research Institute Of Electronic Engineering No 38 Research Institute Of China Electronics Technology Group Corp
Beihang University
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 East China Research Institute Of Electronic Engineering No 38 Research Institute Of China Electronics Technology Group Corp, Beihang University filed Critical East China Research Institute Of Electronic Engineering No 38 Research Institute Of China Electronics Technology Group Corp
Priority to CN201810156874.4A priority Critical patent/CN108957508B/zh
Publication of CN108957508A publication Critical patent/CN108957508A/zh
Application granted granted Critical
Publication of CN108957508B publication Critical patent/CN108957508B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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

Landscapes

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

Abstract

本公开涉及一种车载POS离线组合估计方法，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程。采用加权统计线性回归求得系统伪线性模型和线性化参数。采用基于二阶插值滤波的双滤波器平滑估计方法进行离线组合估计，前向滤波过程中采用二阶插值滤波获得1～N时刻的xf,k和Pf,k，后向滤波过程中采用信息滤波逆向求得k～N时刻的xb,k和Pb,k。将前向和后向滤波结果最优加权组合得到估计结果xm。该方法有效解决系统非线性误差导致精度下降问题，充分利用量测信息和模型信息获得高精度的车载POS位置、速度、姿态信息。本公开还涉及一种车载POS离线组合估计装置。

Description

车载POS离线组合估计方法和装置
技术领域
本公开涉及卫星导航技术领域，特别是涉及一种车载POS离线组合估计方法和装置。
背景技术
作为一种特殊的惯性/卫星组合导航系统，车载POS是车载高精度移动测绘中必不可少的关键通用设备。它由高精度惯性测量单元(Inertial Measurement Unit，IMU)、全球定位系统(Global Positioning System，GPS)、POS计算机(POS Computer System，PCS)、POS后处理软件四部分组成，为车载高精度移动测绘提供高频、高精度的时间、空间基准信息，是实现高精度车载测绘的关键。由IMU敏感载体的运动，将陀螺和加速度计的敏感信息转化为数字信息发送到PCS，PCS将接收陀螺和加速度计的敏感信息存储下来，通过事后离线方法对存储的IMU数据先进行捷联解算，同时将GPS位置和速度信息，与捷联解算结果进行实组合滤波。此外在组合滤波的基础上再进行平滑估计，最终得到高精度的位置、速度和姿态信息。但是，在车载POS离线组合估计过程中，存在系统非线性误差导致估计精度下降的问题，传统的离线组合估计方法的精度不能满足测绘载荷的需求，因此需要提出车载POS离线组合估计方法来提高离线组合估计的精度。
发明内容
基于此，有必要针对传统技术存在的问题，提供一种车载POS离线组合估计方法和装置，克服传统POS离线组合估计方法的估计精度不足的问题，该方法建立了基于捷联惯导误差方程和车辆运动约束条件建立非线性状态方程和量测方程，确定了解决非线性问题的组合估计方法，提高了车载POS离线组合估计的精度。
第一方面，本公开实施例提供了一种车载POS离线组合估计方法，所述方法包括：建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程；通过加权统计线性回归对所述车载POS非线性状态方程线性化，获取伪线性模型和线性化参数；对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息。
在其中一个实施例中，所述建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程，包括：根据捷联惯性导航系统误差方程建立所述车载POS非线性状态方程和基于车辆运动约束条件的所述量测方程。
在其中一个实施例中，所述量测方程中包括速度量测矩阵，其中，所述速度量测矩阵为：
Figure GDA0001821933690000021
Figure GDA0001821933690000022
式中
Figure GDA0001821933690000023
θ,γ分别表示航向角、俯仰角和横滚角。
在其中一个实施例中，所述伪线性模型为：
Figure GDA0001821933690000031
式中
Figure GDA0001821933690000032
和
Figure GDA0001821933690000033
为加权统计线性化参数；
Figure GDA0001821933690000034
和
Figure GDA0001821933690000035
为线性化误差项；Γk为系统噪声分布矩阵。
在其中一个实施例中，所述对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息，包括：
在前向滤波过程采用二阶插值滤波，对所述车载POS非线性状态方程中的非线性函数进行插值多项式展开，取二阶项作为函数的近似；
对其进行递推获得1～N时刻的状态估计xf,k和协方差阵Pf,k。
在其中一个实施例中，所述对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息，还包括：
在后向滤波过程中，基于获取的所述伪线性模型，采用信息滤波逆向求得k～N时刻的状态估计xb,k和协方差阵Pb,k；
将前向滤波和后向滤波的结果加权组合得到最终的估计结果xm。
第二方面，本公开实施例提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述第一方面的车载POS离线组合估计方法。
第三方面，本公开实施例提供了一种包含指令的计算机程序产品，当该计算机程序产品在计算机上运行时，使得计算机执行上述第一方面所述的方法。
第四方面，本公开实施例还提供了一种车载POS离线组合估计装置，所述装置包括：方程建立模块，用于建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程；获取模块，用于通过加权统计线性回归对所述车载POS非线性状态方程线性化，获取伪线性模型和线性化参数；估计模块，用于对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息。
在其中一个实施例中，所述方程建立模块，还用于根据捷联惯性导航系统误差方程建立所述车载POS非线性状态方程和基于车辆运动约束条件的所述量测方程。
本公开提供的一种车载POS离线组合估计方法和装置，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程。采用加权统计线性回归求得系统伪线性模型和线性化参数。采用基于二阶插值滤波的双滤波器平滑估计方法进行离线组合估计，前向滤波过程中采用二阶插值滤波获得1～N时刻的xf,k和Pf,k，后向滤波过程中采用信息滤波逆向求得k～N时刻的xb,k和Pb,k。将前向和后向滤波结果最优加权组合得到估计结果xm。该方法有效解决系统非线性误差导致精度下降问题，充分利用量测信息和模型信息获得高精度的车载POS位置、速度、姿态信息。
附图说明
图1为本公开一个实施例中的一种车载POS离线组合估计方法的步骤流程图；
图2为本公开另一个实施例中的一种车载POS离线组合估计方法的步骤流程图；
图3为本公开一个实施例中的一种车载POS离线组合估计装置的结构示意图。
具体实施方式
为了使本公开的目的、技术方案及优点更加清楚明白，以下通过实施例，并结合附图，对本公开一种车载POS离线组合估计方法和装置的具体实施方式进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本公开，并不用于限定本公开。
本公开涉及一种车载POS离线组合估计方法，可以应用于车载位置姿态测量系统(Position and Orientation System，POS)，也可以应用于惯性网络协同导航、INS(Inertial Navigation System，惯性导航系统)/GPS(Global Position System，全球定位系统)组合导航系统的可观测度分析及机动方式优化。
本公开的原理是：建立基于捷联惯导误差方程和车辆运动约束条件建立非线性状态方程和量测方程。根据捷联惯导误差方程和车辆运动约束条件建立系统非线性状态方程和量测方程。根据加权统计线性回归方法对非线性模型线性化求得伪线性模型和线性化参数，推导得到系统逆向模型。采用基于二阶插值滤波的双滤波器平滑方法对车载POS的数据进行离线组合估计获得高精度的车载POS位置、速度、姿态信息，提升车载POS离线组合估计的精度。
如图1所示，为一个实施例中的一种车载POS离线组合估计方法的步骤流程示意图。具体包括以下步骤：
步骤102，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程,。其中，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程，包括：根据捷联惯性导航系统误差方程建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程。
本实施例中，量测方程中包括速度量测矩阵，其中，速度量测矩阵为：
Figure GDA0001821933690000061
Figure GDA0001821933690000062
式中
Figure GDA0001821933690000063
θ,γ分别表示航向角、俯仰角和横滚角。
步骤104，通过加权统计线性回归对车载POS非线性状态方程线性化，获取伪线性模型和线性化参数。其中，伪线性模型为：
Figure GDA0001821933690000064
式中
Figure GDA0001821933690000065
和
Figure GDA0001821933690000066
为加权统计线性化参数；
Figure GDA0001821933690000067
和
Figure GDA0001821933690000068
为线性化误差项；Γk为系统噪声分布矩阵。
步骤106，对获取的伪线性模型和线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息。
本实施例中，对获取的伪线性模型和线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息，包括：在前向滤波过程采用二阶插值滤波，对所述车载POS非线性状态方程中的非线性函数进行插值多项式展开，取二阶项作为函数的近似；对其进行递推获得1～N时刻的状态估计xf,k和协方差阵Pf,k。
本实施例中，对获取的伪线性模型和线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息，还包括：在后向滤波过程中，基于获取的所述伪线性模型，采用信息滤波逆向求得k～N时刻的状态估计xb,k和协方差阵Pb,k；将前向滤波和后向滤波的结果加权组合得到最终的估计结果xm。
需要说明的是，基于二阶插值滤波的双滤波器平滑方法，有前向和后向双滤波器的结构。在前向滤波过程采用二阶插值滤波，对非线性状态方程中的非线性函数进行插值多项式展开，取二阶项作为函数的近似，然后对其进行递推获得1～N时刻的状态估计xf,k和协方差阵Pf,k。在后向滤波过程中，基于获取的伪线性模型，采用信息滤波逆向求得k～N时刻的状态估计xb,k和协方差阵Pb,k。最后，将前向滤波和后向滤波的结果加权组合得到最终的估计结果xm。
本公开提供的一种车载POS离线组合估计方法，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程。采用加权统计线性回归求得系统伪线性模型和线性化参数。采用基于二阶插值滤波的双滤波器平滑估计方法进行离线组合估计，前向滤波过程中采用二阶插值滤波获得1～N时刻的xf,k和Pf,k，后向滤波过程中采用信息滤波逆向求得k～N时刻的xb,k和Pb,k。将前向和后向滤波结果最优加权组合得到估计结果xm。该方法有效解决系统非线性误差导致精度下降问题，充分利用量测信息和模型信息获得高精度的车载POS位置、速度、姿态信息。
为了更清楚的理解与应用本公开提出的车载POS离线组合估计方法，进行以下示例。需要说明的是，本公开的保护范围不局限以下示例。
如图2所示，为另一个实施例中的一种车载POS离线组合估计方法的步骤流程示意图。具体包括以下步骤：
步骤201，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程。具体的，根据捷联惯性导航系统误差方程，建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程。其中，基于捷联惯性导航姿态角误差微分方程如下：
Figure GDA0001821933690000081
式中Φ＝[ΦE ΦN ΦU]T为姿态误差向量；
Figure GDA0001821933690000082
为地理坐标系相对于惯性坐标系在地理坐标系下的旋转角速率，
Figure GDA0001821933690000083
为
Figure GDA0001821933690000084
的误差角速率；εb＝[εx εy εz]T为载体坐标系下的陀螺常值漂移，且
Figure GDA0001821933690000085
Figure GDA0001821933690000086
为载体坐标系到导航坐标系之间的方向余弦矩阵，
Figure GDA0001821933690000087
为
Figure GDA0001821933690000088
的估计值；
Figure GDA0001821933690000089
为姿态误差矩阵。
进一步地，基于姿态角误差模型的速度误差微分方程如下：
Figure GDA00018219336900000810
式中Vn＝[VE VN VU]T为载体坐标系相对于地球坐标系在地理坐标系下的速度，δVn为Vn的误差；为加速度计所测比力在载体坐标系上的投影；
Figure GDA00018219336900000811
为地球坐标系相对于惯性坐标系在地理坐标系下的角度率误差，
Figure GDA00018219336900000812
为导航坐标系相对于地球坐标系在地理坐标系下的角度率误差；
Figure GDA00018219336900000813
为载体坐标系下的加速度计常值偏置，且
Figure GDA00018219336900000814
其中，非线性项为：
Figure GDA00018219336900000815
和
Figure GDA00018219336900000816
非线性矩阵
Figure GDA00018219336900000817
的表达式如下：
Figure GDA00018219336900000818
位置误差微分方程如下：
Figure GDA0001821933690000091
式中δL、δλ和δh分别表示纬度、经度和高度误差；RM和RN分别代表地球的沿子午圈和卯酉圈的主曲率半径。
基于上述捷联惯性导航系统误差方程,选取车载POS系统模型的15维状态变量：
Figure GDA0001821933690000092
式中：φ＝[φE φN φU]T为东、北、天向的系统失准角，δV＝[δVE δVN δVU]T为东、北、天向的系统速度误差，δP＝[δPE δPN δPU]T分别为东、北、天向的系统位置误差，ε＝[εxεy εz]T为陀螺常值漂移，
Figure GDA0001821933690000093
为加速度计偏置。选取SINS与GPS的速度之差、位置之差作为机载POS的系统量测量，量测向量可表示为：
Z＝[δV δP] (6)
式中
Figure GDA0001821933690000094
Figure GDA0001821933690000095
分别为导航坐标系(n系)下SINS、GPS的速度，
Figure GDA0001821933690000096
分别为n系下SINS、GPS的位置。
需要说明的是，在没有颠簸和侧滑的理想条件下，车辆的速度方向主要沿着车辆的行驶方向，车身横向和垂直方向的速度通常为零。实际情况则略有不同，而车辆实际运行中，侧滑现象不可避免，将这些速度误差视为扰动速度加入到量测方程中，量测方程如下：
Z＝HX+v (7)
式中v为量测噪声矩阵，H为量测矩阵可以表示为：
H＝[HV HP]T (8)
HP＝[03×6diag(RM+h,(RN+h)cosL,1)03×6] (9)
式中RM和RN分别代表地球的沿子午圈和卯酉圈的主曲率半径，v表示当地高度。
根据车辆运动约束条件推导得出速度量测矩阵可以表示为：
HV＝[03×3 Hv 03×9] (10)
Figure GDA0001821933690000101
式中
Figure GDA0001821933690000102
θ,γ分别表示航向角、俯仰角和横滚角。
步骤202，采用加权统计线性回归求得系统伪线性模型和线性化参数。
具体的，根据步骤201中建立的车载POS非线性状态方程和基于车辆运动约束条件的量测方程，采用加权统计线性回归对非线性模型线性化求得伪线性模型和线性化参数。其中得到的伪线性模型可以表示为：
Figure GDA0001821933690000103
式中
Figure GDA0001821933690000104
和
Figure GDA0001821933690000105
为加权统计线性化参数；
Figure GDA0001821933690000106
和
Figure GDA0001821933690000107
为线性化误差项；Γk为系统噪声分布矩阵。其中线性化参数可以表示为：
Figure GDA0001821933690000111
步骤203，前向滤波采用二阶插值滤波获得1-N时刻的状态估计和协方差阵。
步骤204，后向滤波采用信息滤波逆向求得k-N时刻的状态估计和协方差阵。
步骤205，前向和后向滤波结果加权组合得到最终估计结果。
具体的，根据步骤201中的车载POS非线性状态方程和基于车辆运动约束条件的量测方程和步骤202中的伪线性模型和线性化参数，采用基于二阶插值滤波的双滤波器平滑方法对车载POS的数据进行离线组合估计获得高精度的车载POS位置、速度、姿态信息。
基于二阶插值滤波的双滤波器平滑方法，有前向和后向双滤波器的结构。在前向滤波过程采用二阶插值滤波，对非线性状态方程中的非线性函数进行插值多项式展开，取二阶项作为函数的近似，然后对其进行递推获得1～N时刻的状态估计xf,k和协方差阵Pf,k。二阶插值滤波的步骤如下：
在插值滤波过程中采用Cholesky分解法引入4个平方根分解算子：
Figure GDA0001821933690000112
通常情况下，测量噪声协方差阵R与过程噪声协方差阵Q的平方根分解矩阵可以在滤波开始之前得到，而协方差阵的平方根分解矩阵
Figure GDA0001821933690000113
和
Figure GDA0001821933690000114
则需要在滤波过程中进行不断地更新，并不断地修正。
初始化：
Figure GDA0001821933690000121
式中
Figure GDA0001821933690000122
和
Figure GDA0001821933690000123
初始估计状态和误差协方差阵。
一步预测方程：
Figure GDA0001821933690000124
式中
Figure GDA0001821933690000125
为一步预测估计状态；nx为状态量的维数；nw过程噪声的维数。式中
Figure GDA0001821933690000126
为状态的一步估计；
Figure GDA0001821933690000127
是关于矩阵的协方差矩阵
Figure GDA0001821933690000128
分解的平方根的p列元素；nx是状态向量的维数；过程噪声阵的维数；h是给定的变量步长，其最优选择为h2＝3。
量测修正方程：
Figure GDA0001821933690000129
Figure GDA00018219336900001210
式中
Figure GDA00018219336900001211
表示状态估计；nv表示量测噪声阵的维数。增益阵Kk可以表示为：
Figure GDA00018219336900001212
Figure GDA00018219336900001213
交叉协方差阵
Figure GDA00018219336900001214
可以表示为：
Figure GDA00018219336900001215
一步预测估计误差的方差阵可以表示为：
Figure GDA00018219336900001216
其Cholesky分解因子为：
Figure GDA0001821933690000131
在后向滤波过程中，基于步骤(2)所述的伪线性模型，采用信息滤波逆向求得k～N时刻的状态估计xb,k和协方差阵Pb,k。
根据步骤202的伪线性模型和线性化参数，可以得到一个后向信息滤波器。由于统计线性化空间模型不同于常规的线性状态空间模型，必须从基本原理推导时间更新和量测更新方程。后向信息滤波算法步骤如下：
初始化：
Figure GDA0001821933690000132
Figure GDA0001821933690000133
信息矩阵时间更新：
Figure GDA0001821933690000134
Figure GDA0001821933690000135
令
Figure GDA0001821933690000136
为后向增益矩阵：
Figure GDA0001821933690000137
将上式代入公式(26)中，可以得到：
Figure GDA0001821933690000138
信息状态的时间更新
Figure GDA0001821933690000139
量测更新通过公式(23)～(29)，可以得到：
Figure GDA0001821933690000141
Figure GDA0001821933690000142
最后，将前向滤波和后向滤波的结果加权组合得到最终的估计结果xm。
Figure GDA0001821933690000143
Figure GDA0001821933690000144
本公开提出的一种车载POS离线组合估计方法，建立车载POS系统模型，推导逆向伪线性模型，采用基于二阶插值滤波的双滤波器平滑估计方法，利用所有的量测信息和模型信息，解决了传统方法中非线性误差导致精度下降的问题，提高了车载POS离线组合估计的精度。
基于同一公开构思，还提供了一种车载POS离线组合估计装置。由于此装置解决问题的原理与前述一种车载POS离线组合估计方法相似，因此，该装置的实施可以按照前述方法的具体步骤时限，重复之处不再赘述。
如图3所示，为一个实施例中的一种车载POS离线组合估计装置的结构示意图。该车载POS离线组合估计装置10包括：方程建立模块200、获取模块400和估计模块600。
其中，方程建立模块200用于建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程；获取模块400用于通过加权统计线性回归对车载POS非线性状态方程线性化，获取伪线性模型和线性化参数；估计模块600用于对获取的伪线性模型和线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息。
本实施例中，方程建立模块200还用于根据捷联惯性导航系统误差方程建立所述车载POS非线性状态方程和基于车辆运动约束条件的所述量测方程。
本公开提供的一种车载POS离线组合估计装置。方程建立模块200用于建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程；获取模块400用于通过加权统计线性回归对车载POS非线性状态方程线性化，获取伪线性模型和线性化参数；估计模块600用于对获取的伪线性模型和线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息。该装置有效解决系统非线性误差导致精度下降问题，充分利用量测信息和模型信息获得高精度的车载POS位置、速度、姿态信息。
本公开实施例还提供了一种计算机可读存储介质。该计算机可读存储介质上存储有计算机程序，该程序被图1或图2中处理器执行。
本公开实施例还提供了一种包含指令的计算机程序产品。当该计算机程序产品在计算机上运行时，使得计算机执行上述图1或2的方法。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random AccessMemory，RAM)等。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本公开的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本公开专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本公开构思的前提下，还可以做出若干变形和改进，这些都属于本公开的保护范围。因此，本公开专利的保护范围应以所附权利要求为准。

Claims (8)

1.一种车载POS离线组合估计方法，其特征在于，所述方法包括：
建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程；
通过加权统计线性回归对所述车载POS非线性状态方程线性化，获取伪线性模型和线性化参数；
对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息；
其中，所述伪线性模型为：
Figure FDA0003014139030000011
式中
Figure FDA0003014139030000012
和
Figure FDA0003014139030000013
为加权统计线性化参数；
Figure FDA0003014139030000014
和
Figure FDA0003014139030000015
为线性化误差项；Γk为系统噪声分布矩阵；
其中线性化参数可以表示为：
Figure FDA0003014139030000016
2.根据权利要求1所述的方法，其特征在于，所述建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程，包括：根据捷联惯性导航系统误差方程建立所述车载POS非线性状态方程和基于车辆运动约束条件的所述量测方程。
3.根据权利要求1或2所述的方法，其特征在于，所述量测方程中包括速度量测矩阵，其中，所述速度量测矩阵为：
Figure FDA0003014139030000021
Figure FDA0003014139030000022
式中
Figure FDA0003014139030000023
θ,γ分别表示航向角、俯仰角和横滚角。
4.根据权利要求1所述的方法，其特征在于，所述对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息，包括：
在前向滤波过程采用二阶插值滤波，对所述车载POS非线性状态方程中的非线性函数进行插值多项式展开，取二阶项作为函数的近似；
对其进行递推获得1～N时刻的状态估计xf,k和协方差阵Pf,k。
5.根据权利要求4所述的方法，其特征在于，所述对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息，还包括：
在后向滤波过程中，基于获取的所述伪线性模型，采用信息滤波逆向求得k～N时刻的状态估计xb,k和协方差阵Pb,k；
将前向滤波和后向滤波的结果加权组合得到最终的估计结果xm。
6.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1-5任一项所述的方法。
7.一种车载POS离线组合估计装置，其特征在于，所述装置包括：
方程建立模块，用于建立车载POS非线性状态方程和基于车辆运动约束条件的量测方程；
获取模块，用于通过加权统计线性回归对所述车载POS非线性状态方程线性化，获取伪线性模型和线性化参数；
估计模块，用于对获取的所述伪线性模型和所述线性化参数，采用基于二阶插值滤波的双滤波器平滑估计方法，对车载POS的数据进行离线组合估计，获取达到预设精度的车载POS位置、速度以及姿态信息；
其中，所述伪线性模型为：
Figure FDA0003014139030000031
式中
Figure FDA0003014139030000032
和
Figure FDA0003014139030000033
为加权统计线性化参数；
Figure FDA0003014139030000034
和
Figure FDA0003014139030000035
为线性化误差项；Γk为系统噪声分布矩阵；
其中线性化参数可以表示为：
Figure FDA0003014139030000036
8.根据权利要求7所述的装置，其特征在于，所述方程建立模块，还用于根据捷联惯性导航系统误差方程建立所述车载POS非线性状态方程和基于车辆运动约束条件的所述量测方程。
CN201810156874.4A 2018-02-24 2018-02-24 车载pos离线组合估计方法和装置 Active CN108957508B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810156874.4A CN108957508B (zh) 2018-02-24 2018-02-24 车载pos离线组合估计方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810156874.4A CN108957508B (zh) 2018-02-24 2018-02-24 车载pos离线组合估计方法和装置

Publications (2)

Publication Number Publication Date
CN108957508A CN108957508A (zh) 2018-12-07
CN108957508B true CN108957508B (zh) 2021-06-11

Family

ID=64495397

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810156874.4A Active CN108957508B (zh) 2018-02-24 2018-02-24 车载pos离线组合估计方法和装置

Country Status (1)

Country Link
CN (1) CN108957508B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111829555A (zh) * 2020-07-24 2020-10-27 中国人民解放军火箭军工程大学 一种基于运动约束的车载航姿系统误差补偿方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104655152A (zh) * 2015-02-11 2015-05-27 北京航空航天大学 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN106352876A (zh) * 2016-07-25 2017-01-25 北京航空航天大学 一种基于h∞和ckf混合滤波的机载分布式pos传递对准方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104655152A (zh) * 2015-02-11 2015-05-27 北京航空航天大学 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN106352876A (zh) * 2016-07-25 2017-01-25 北京航空航天大学 一种基于h∞和ckf混合滤波的机载分布式pos传递对准方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
"New developments in state estimation for nonlinear systems";Magnus Norgaard et al.;《Automatica》;20001231;第1627-1638页 *
"Second-order divided difference filter for vision-based relative navigation";WANG Xiao-gang et al.;《Journal of Harbin Institute of Technology》;20110331;第18卷(第3期);第16-20页 *
"固定区间平滑算法及其在组合导航系统中的应用";宫晓琳 等;《中国惯性技术学报》;20121231;第20卷(第6期);第687-693页 *
"高性能POS及其在移动测图系统中的应用";房建成 等;《红外与激光工程》;20070930;第36卷;第478-482页 *

Also Published As

Publication number Publication date
CN108957508A (zh) 2018-12-07

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
EP1585939B1 (en) Attitude change kalman filter measurement apparatus and method
CN109931957B (zh) 基于lgmkf的sins捷联惯性导航系统自对准方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN114076610B (zh) Gnss/mems车载组合导航系统的误差标定、导航方法及其装置
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN110346824B (zh) 一种车辆导航方法、系统、装置及可读存储介质
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
EP3388787B1 (en) Polar region operating attitude and heading reference system
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
US8229661B2 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN111121766A (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN111678514B (zh) 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
Bonnabel et al. Design and prototyping of a low-cost vehicle localization system with guaranteed convergence properties
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN111912427A (zh) 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN108957508B (zh) 车载pos离线组合估计方法和装置
CN113074757A (zh) 车载惯导安装误差角的标定方法
JP7206883B2 (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
TA01 Transfer of patent application right

Effective date of registration: 20210519

Address after: 100191 No. 37, Haidian District, Beijing, Xueyuan Road

Applicant after: BEIHANG University

Applicant after: EAST CHINA RESEARCH INSTITUTE OF ELECTRONIC ENGINEERING (NO 38 RESEARCH INSTITUTE OF CHINA ELECTRONICS TECHNOLOGY Group Corp.)

Address before: 100191 No. 37, Haidian District, Beijing, Xueyuan Road

Applicant before: BEIHANG University

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant