CN107402005A - 一种基于惯性/里程计/rfid的高精度组合导航方法 - Google Patents
一种基于惯性/里程计/rfid的高精度组合导航方法 Download PDFInfo
- Publication number
- CN107402005A CN107402005A CN201610339762.3A CN201610339762A CN107402005A CN 107402005 A CN107402005 A CN 107402005A CN 201610339762 A CN201610339762 A CN 201610339762A CN 107402005 A CN107402005 A CN 107402005A
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- msub
- mtr
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明属于组合导航技术领域，具体涉及一种基于惯性/里程计/RFID的高精度组合导航方法。本发明的方法包括以下步骤：步骤1、使用码里程计测量获得载体车辆的北向速度Vn_D、天向速度Vu_D和东向速度Ve_D；步骤2、使用RFID定位系统获得载体车辆的纬度LR和经度λR；步骤3、使用卡尔曼滤波算法计算系统误差，实现组合导航；步骤4、根据卡尔曼滤波输出的各误差量，对里程计、FRID定位系统和惯导系统的相应数据进行修正。本发明解决了纯惯性导航系统的导航误差随时间而积累，因而不能完全满足实际应用需要的问题，提出的方法在里程计、RFID的辅助下完成高精度速度、位置测量，并实现对组合导航系统误差的估计，消除了误差的积累，满足了实际应用的需要。
Description
技术领域
本发明属于组合导航技术领域，具体涉及一种基于惯性/里程计/RFID的高精度组合导航方法。
背景技术
随着载体长时间、远程航行的能力不断得到增强，对导航系统的精度和自主性要求也在不断提高。纯惯性导航系统由于其固有的导航误差随时间而积累的缺点，因而不能完全满足实际应用的需要。
发明内容
本发明需要解决的技术问题为：纯惯性导航系统由于其固有的导航误差随时间而积累，因而不能完全满足实际应用的需要。
本发明的技术方案如下所述：
一种基于惯性/里程计/RFID的高精度组合导航方法，包括以下步骤：
步骤1、里程计测速，使用码里程计测量获得载体车辆的北向速度Vn_D、天向速度Vu_D和东向速度Ve_D；
步骤2、RFID定位，使用RFID定位系统获得载体车辆的纬度LR和经度λR；
步骤3、组合导航，使用卡尔曼滤波算法计算系统误差，实现组合导航；
步骤4、误差修正，根据卡尔曼滤波输出的各误差量，对里程计、FRID定位系统和惯导系统的相应数据进行修正。
优选的，所述步骤1中里程计的速度输出在载体坐标系上表示为
式中，SD为里程计采样周期内载体行程，tD为里程计采样周期，进而计算出 车辆的北向速度Vn_D、天向速度Vu_D和东向速度Ve_D。
优选的，所述步骤3中，
共选取20个系统状态量：δVn，δVu，δVe，δL，δh，δλ，φn，φu，φe， εx，εy，εz，δVn_D，δVu_D，δVe_D，δLR，δλR；
其中：
δVn,δVu,δVe分别表示惯导北向、天向、东向的速度误差；
δL,δh,δλ分别表示惯导的纬度误差、高度误差、经度误差；
φn,φu,φe分别表示惯导导航坐标系内北、天、东三个方向的失准角；
分别表示惯导载体坐标系内X、Y、Z三个方向的加速度计零偏；
εx,εy,εz分别表示惯导载体坐标系内X、Y、Z三个方向的陀螺漂移；
δVn_D，δVu_D，δVe_D分别为里程计在惯导载体坐标系内北、天、东三个方向上的速度误差；
δLR，δλR分别为RFID定位系统的纬度和经度误差；
系统状态方程为：
式中：X(t)为上述20个状态量所构成的状态向量，
W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取；
滤波器量测方程形式如下：
Z＝HX+V
量测量Z为惯导系统和里程计、RFID分别给出的速度、位置的差值，实际上是两者误差的差值：
式中，V为量测噪声，考虑为白噪声；Vn_imu为惯导系统输出的北向速度，Vu_imu为惯导系统输出的天向速度，Ve_imu为惯导系统输出的东向速度，LR为惯导系统输出的纬度和λR为惯导系统输出的经度；
量测矩阵H＝[A5×6 05×9 -I5×5]，其中，I5×5为单位矩阵，
建立上述误差模型后，选用卡尔曼滤波方法作为参数辨识方法，具体公式如下：
状态一步预测
状态估计
滤波增益矩阵
一步预测误差方差阵
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
本发明的有益效果为：
本发明提出了一种惯性/里程计/RFID的高精度组合导航方法，该方法在里程计、RFID的辅助下完成高精度速度、毫米级别的位置测量，并实现对组合导航系统误差的估计，消除了惯性导航系统导航误差随时间而积累的缺点，最终能在实现毫米级别的导航，满足了实际应用的需要。
附图说明
图1为RFID定位的原理示意图。
具体实施方式
下面结合附图和实施例对本发明的一种基于惯性/里程计/RFID的高精度组合导航方法进行详细说明。
本发明的方法，首先利用里程计测量载体的前向速度，并对里程计进行误差建模。接着，利用RFID测量载体的位置信息，并对RFID位置测量系统进行误差建模。最后，将惯导系统输出的速度、位置信息与里程计、RFID输出的速度、位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对惯性误差、里程计误差、RFID误差进行估计，输出对应误差估计量，对惯导系统、里程计、RFID位置测量系统进行误差修正，最终实现高精度的组合导航。具体步骤如下：
步骤1、里程计测速
里程计借助于车轮的转动直接测量载体在地面行驶的路程，而测速仪的测速原理是基于多普勒效应，通常测速仪都存在“死区”问题，即当载体低速行驶时将不产生速度输出，因此里程计比测速仪更适合于高精度的组合导航应用。
假设载体行驶时，车轮无侧滑和弹跳，紧贴路面行驶，里程计测量的是沿车体正前方向上的行程，则里程计的速度输出可以在载体坐标系上表示为
式中，SD为里程计采样周期内载体行程，tD为里程计采样周期，进而计算出 车辆的北向速度Vn_D、天向速度Vu_D和东向速度Ve_D。
步骤2、RFID定位
RFID定位系统主要由两部分组成：RFID接收天线，简称天线，与RFID标签，简称标签。天线安装于载体下方，标签需要事先铺设于地面且顶部与地面平行。以某型RFID接收天线为例，天线的感应面积为1500×600mm，当天线借助载体运动于标签上方时，标签将激励天线产生能量同时将自己的编码信息传递给天线，天线内的信息处理电路对产生的能量进行分析，当产生的能量持续一段时间后认为已经定位，同时得到标签相对于接收天线感应区域中心位置的偏移量，将偏移量与标签编码等有效信息通过串口或CAN总线发送给用户，完成一次RFID定位的工作。
如图1所示，虚线为期望的载体运动轨迹，一般将RFID标签安装于期望的轨迹上。当安装于载体下方的接收天线经过标签T105时，即可计算出标签相对于接收天线的感应区域中心点的偏移，假设为5mm，并且获得标签的编码为T105，而每个标签准确的地理位置为已知，通过偏移量与标签的地理位置，即可推算出接收天线感应区域的位置，获得载体车辆的纬度LR和经度λR，实现了标签定位的功能。
步骤3、组合导航
惯性/里程计/RFID组合导航，使用卡尔曼滤波器，将惯性速度与里程计速度之差、惯性位置与RFID位置之差作为滤波器的观测量，通过滤波计算估计出系统误差，进行误差修正后可以提高惯导系统的定位精度。
惯性/里程计/RFID组合导航的算法如下：
卡尔曼滤波实质上是一种递推线性最小方差滤波方法，它不要求储存过去的量测值，只要根据当时的量测值和前一时刻的估计，就可以实时地计算出所需信号的估计。
1)误差模型
本发明所设计的惯性/里程计/RFID组合导航方法误差主要包括4个方面：一是惯导的导航参数解算误差；二是惯导的惯性器件自身误差和其受环境影响而引起的误差；三是里程计的相关误差；四是RFID的相关误差。采用间接法滤波，系统状态方程就是各误差方程，共选取20个系统状态量：δVn，δVu，δVe，δL，δh，δλ，φn，φu，φe，εx，εy，εz，δVn_D，δVu_D，δVe_D，δLR，δλR。
其中：
δVn,δVu,δVe分别表示惯导北向、天向、东向的速度误差；
δL,δh,δλ分别表示惯导的纬度误差、高度误差、经度误差；
φn,φu,φe分别表示惯导导航坐标系内北、天、东三个方向的失准角；
分别表示惯导载体坐标系内X、Y、Z三个方向的加速度计零偏；
εx,εy,εz分别表示惯导载体坐标系内X、Y、Z三个方向的陀螺漂移；
δVn_D，δVu_D，δVe_D分别为里程计在惯导载体坐标系内北、天、东三个方向上的速度误差；
δLR，δλR分别为RFID定位系统的纬度和经度误差。
系统状态方程为：
式中：X(t)为上述20个状态量所构成的状态向量，
W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取。
滤波器量测方程形式如下：
Z＝HX+V
量测量Z为惯导系统和里程计、RFID分别给出的速度、位置的差值，实际上是两者误差的差值：
式中，V为量测噪声，考虑为白噪声。Vn_imu为惯导系统输出的北向速度，Vu_imu为惯导系统输出的天向速度，Ve_imu为惯导系统输出的东向速度，LR为惯导系统输出的纬度和λR为惯导系统输出的经度。
量测矩阵H＝[A5×6 05×9 -I5×5]，其中，I5×5为单位矩阵，
2)卡尔曼滤波模型
建立上述误差模型后，选用卡尔曼滤波方法作为参数辨识方法，卡尔曼滤波方程采用文献《卡尔曼滤波和组合导航原理》(第一版，秦永元等编著)中的形式，具体公式如下：
状态一步预测
状态估计
滤波增益矩阵
一步预测误差方差阵
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1 (5)
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
步骤4、误差修正
根据卡尔曼滤波输出的各误差量，对里程计、FRID定位系统和惯导系统的相应数据进行修正。
Claims (3)
1.一种基于惯性/里程计/RFID的高精度组合导航方法，其特征在于：包括以下步骤：
步骤1、里程计测速，使用码里程计测量获得载体车辆的北向速度Vn_D、天向速度Vu_D和东向速度Ve_D；
步骤2、RFID定位，使用RFID定位系统获得载体车辆的纬度LR和经度λR；
步骤3、组合导航，使用卡尔曼滤波算法计算系统误差，实现组合导航；
步骤4、误差修正，根据卡尔曼滤波输出的各误差量，对里程计、FRID定位系统和惯导系统的相应数据进行修正。
2.如权利要求1所述的基于惯性/里程计/RFID的高精度组合导航方法，其特征在于：所述步骤1中里程计的速度输出在载体坐标系上表示为
<mrow>
<msubsup>
<mi>v</mi>
<mi>D</mi>
<mi>b</mi>
</msubsup>
<mo>=</mo>
<msup>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>S</mi>
<mi>D</mi>
</msub>
<mo>/</mo>
<msub>
<mi>t</mi>
<mi>D</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
式中，SD为里程计采样周期内载体行程，tD为里程计采样周期，进而计算出车辆的北向速度Vn_D、天向速度Vu_D和东向速度Ve_D。
3.如权利要求1所述的基于惯性/里程计/RFID的高精度组合导航方法，其特征在于：所述步骤3中，
共选取20个系统状态量：δVn，δVu，δVe，δL，δh，δλ，φn，φu，φe，▽x，▽y，▽z，εx，εy，εz，δVn_D，δVu_D，δVe_D，δLR，δλR；
其中：
δVn,δVu,δVe分别表示惯导北向、天向、东向的速度误差；
δL,δh,δλ分别表示惯导的纬度误差、高度误差、经度误差；
φn,φu,φe分别表示惯导导航坐标系内北、天、东三个方向的失准角；
▽x,▽y,▽z分别表示惯导载体坐标系内X、Y、Z三个方向的加速度计零偏；
εx,εy,εz分别表示惯导载体坐标系内X、Y、Z三个方向的陀螺漂移；
δVn_D，δVu_D，δVe_D分别为里程计在惯导载体坐标系内北、天、东三个方向上的速度误差；
δLR，δλR分别为RFID定位系统的纬度和经度误差；
系统状态方程为：
<mrow>
<mover>
<mi>X</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>F</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mi>X</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>G</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mi>W</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
式中：X(t)为上述20个状态量所构成的状态向量，
X(t)＝[δVn,δVu,δVe,δL,δh,δλ,φn,φu,φe,▽x,▽y,▽z,εx,εy,εz,δVn_D,δVu_D,δVe_D,δLR,δλR]；W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取；
滤波器量测方程形式如下：
Z＝HX+V
量测量Z为惯导系统和里程计、RFID分别给出的速度、位置的差值，实际上是两者误差的差值：
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>V</mi>
<mrow>
<mi>n</mi>
<mo>_</mo>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>V</mi>
<mrow>
<mi>n</mi>
<mo>_</mo>
<mi>D</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>V</mi>
<mrow>
<mi>u</mi>
<mo>_</mo>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>V</mi>
<mrow>
<mi>u</mi>
<mo>_</mo>
<mi>D</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>V</mi>
<mrow>
<mi>e</mi>
<mo>_</mo>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>V</mi>
<mrow>
<mi>e</mi>
<mo>_</mo>
<mi>D</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>L</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>L</mi>
<mi>R</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&lambda;</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mi>R</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;V</mi>
<mrow>
<mi>n</mi>
<mo>_</mo>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&delta;V</mi>
<mrow>
<mi>n</mi>
<mo>_</mo>
<mi>D</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;V</mi>
<mrow>
<mi>u</mi>
<mo>_</mo>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&delta;V</mi>
<mrow>
<mi>u</mi>
<mo>_</mo>
<mi>D</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;V</mi>
<mrow>
<mi>e</mi>
<mo>_</mo>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&delta;V</mi>
<mrow>
<mi>e</mi>
<mo>_</mo>
<mi>D</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;L</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&delta;L</mi>
<mi>R</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;&lambda;</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&delta;&lambda;</mi>
<mi>R</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mi>V</mi>
</mrow>
1
式中，V为量测噪声，考虑为白噪声；Vn_imu为惯导系统输出的北向速度，Vu_imu为惯导系统输出的天向速度，Ve_imu为惯导系统输出的东向速度，LR为惯导系统输出的纬度和λR为惯导系统输出的经度；
量测矩阵H＝[A5×6 05×9 -I5×5]，其中，I5×5为单位矩阵，
<mrow>
<mi>A</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
建立上述误差模型后，选用卡尔曼滤波方法作为参数辨识方法，具体公式如下：
状态一步预测
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
状态估计
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>&lsqb;</mo>
<msub>
<mi>Z</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
滤波增益矩阵
<mrow>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msup>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mrow>
一步预测误差方差阵
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610339762.3A CN107402005A (zh) | 2016-05-20 | 2016-05-20 | 一种基于惯性/里程计/rfid的高精度组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610339762.3A CN107402005A (zh) | 2016-05-20 | 2016-05-20 | 一种基于惯性/里程计/rfid的高精度组合导航方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107402005A true CN107402005A (zh) | 2017-11-28 |
Family
ID=60389136
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610339762.3A Pending CN107402005A (zh) | 2016-05-20 | 2016-05-20 | 一种基于惯性/里程计/rfid的高精度组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107402005A (zh) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109443350A (zh) * | 2018-12-27 | 2019-03-08 | 西安中科光电精密工程有限公司 | 基于神经网络的蓝牙/光电/ins组合导航装置及方法 |
| CN109827572A (zh) * | 2019-03-12 | 2019-05-31 | 北京星网宇达科技股份有限公司 | 一种检测车位置预测的方法及装置 |
| CN110631574A (zh) * | 2018-06-22 | 2019-12-31 | 北京自动化控制设备研究所 | 一种惯性/里程计/rtk多信息融合方法 |
| CN110657788A (zh) * | 2018-06-29 | 2020-01-07 | 北京自动化控制设备研究所 | 一种起重机轨道平顺性动态检测方法 |
| CN110763228A (zh) * | 2019-10-08 | 2020-02-07 | 哈尔滨工程大学 | 基于海底油气管节点位置的组合导航系统误差修正方法 |
| CN111256708A (zh) * | 2020-02-17 | 2020-06-09 | 青岛黄海学院 | 一种基于射频识别的车载组合导航方法 |
| CN112362052A (zh) * | 2020-10-27 | 2021-02-12 | 中国科学院计算技术研究所 | 一种融合定位方法及系统 |
| CN112558087A (zh) * | 2020-11-20 | 2021-03-26 | 东风汽车集团有限公司 | 一种自动驾驶车辆的定位系统及方法 |
| CN114279462A (zh) * | 2021-11-24 | 2022-04-05 | 中国船舶重工集团公司第七0七研究所 | 一种惯导周期性误差自主估计与预测方法 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
-
2016
- 2016-05-20 CN CN201610339762.3A patent/CN107402005A/zh active Pending
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
Non-Patent Citations (1)
| Title |
|---|
| 王其等: "RFID辅助捷联定位系统的地铁列车组合定位系统建模与仿真", 《系统仿真技术》 * |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110631574B (zh) * | 2018-06-22 | 2021-05-11 | 北京自动化控制设备研究所 | 一种惯性/里程计/rtk多信息融合方法 |
| CN110631574A (zh) * | 2018-06-22 | 2019-12-31 | 北京自动化控制设备研究所 | 一种惯性/里程计/rtk多信息融合方法 |
| CN110657788A (zh) * | 2018-06-29 | 2020-01-07 | 北京自动化控制设备研究所 | 一种起重机轨道平顺性动态检测方法 |
| CN109443350A (zh) * | 2018-12-27 | 2019-03-08 | 西安中科光电精密工程有限公司 | 基于神经网络的蓝牙/光电/ins组合导航装置及方法 |
| CN109443350B (zh) * | 2018-12-27 | 2023-09-01 | 仝人智能科技(江苏)有限公司 | 基于神经网络的蓝牙/光电/ins组合导航装置及方法 |
| CN109827572A (zh) * | 2019-03-12 | 2019-05-31 | 北京星网宇达科技股份有限公司 | 一种检测车位置预测的方法及装置 |
| CN110763228A (zh) * | 2019-10-08 | 2020-02-07 | 哈尔滨工程大学 | 基于海底油气管节点位置的组合导航系统误差修正方法 |
| CN111256708A (zh) * | 2020-02-17 | 2020-06-09 | 青岛黄海学院 | 一种基于射频识别的车载组合导航方法 |
| CN112362052A (zh) * | 2020-10-27 | 2021-02-12 | 中国科学院计算技术研究所 | 一种融合定位方法及系统 |
| CN112362052B (zh) * | 2020-10-27 | 2022-09-16 | 中国科学院计算技术研究所 | 一种融合定位方法及系统 |
| CN112558087B (zh) * | 2020-11-20 | 2023-06-23 | 东风汽车集团有限公司 | 一种自动驾驶车辆的定位系统及方法 |
| CN112558087A (zh) * | 2020-11-20 | 2021-03-26 | 东风汽车集团有限公司 | 一种自动驾驶车辆的定位系统及方法 |
| CN114279462A (zh) * | 2021-11-24 | 2022-04-05 | 中国船舶重工集团公司第七0七研究所 | 一种惯导周期性误差自主估计与预测方法 |
| CN114279462B (zh) * | 2021-11-24 | 2023-06-20 | 中国船舶重工集团公司第七0七研究所 | 一种惯导周期性误差自主估计与预测方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107402005A (zh) | 一种基于惯性/里程计/rfid的高精度组合导航方法 | |
| CN101476894B (zh) | 车载sins/gps组合导航系统性能增强方法 | |
| Chen et al. | Estimate the pitch and heading mounting angles of the IMU for land vehicular GNSS/INS integrated system | |
| CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
| CN105698822B (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
| Fu et al. | High-accuracy SINS/LDV integration for long-distance land navigation | |
| CN106595652B (zh) | 车辆运动学约束辅助的回溯式行进间对准方法 | |
| CN103777220B (zh) | 基于光纤陀螺、速度传感器和gps的实时精确位姿估计方法 | |
| CN105509738B (zh) | 基于惯导/多普勒雷达组合的车载定位定向方法 | |
| CN104165641B (zh) | 一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法 | |
| CN104515527B (zh) | 一种无gps信号环境下的抗粗差组合导航方法 | |
| CN103727941B (zh) | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 | |
| CN102589552B (zh) | 低成本组合导航系统的数据融合方法和装置 | |
| CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
| CN104819716A (zh) | 一种基于mems的ins/gps组合的室内外个人导航算法 | |
| CN107860399A (zh) | 一种基于地图匹配的车载捷联惯导行进间精对准方法 | |
| CN101201255A (zh) | 基于智能导航算法的车辆组合导航系统 | |
| CN109870173A (zh) | 一种基于校验点的海底管道惯性导航系统的轨迹修正方法 | |
| Jung et al. | Monocular visual-inertial-wheel odometry using low-grade IMU in urban areas | |
| CN105318876A (zh) | 一种惯性里程计组合高精度姿态测量方法 | |
| CN109945860A (zh) | 一种基于卫星紧组合的ins和dr惯性导航方法与系统 | |
| CN109343095A (zh) | 一种车载导航车辆组合定位装置及其组合定位方法 | |
| CN103759733B (zh) | 基于联邦滤波的ins/vkm/vdm车载导航系统 | |
| CN106568449A (zh) | 一种基于mems的车辆模型辅助和约束的gnss/ins组合导航方法 | |
| CN108180923A (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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20171128 |