CN112697142B - 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 - Google Patents

一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 Download PDF

Info

Publication number
CN112697142B
CN112697142B CN202011517160.5A CN202011517160A CN112697142B CN 112697142 B CN112697142 B CN 112697142B CN 202011517160 A CN202011517160 A CN 202011517160A CN 112697142 B CN112697142 B CN 112697142B
Authority
CN
China
Prior art keywords
wheel speed
time
matrix
moment
speed odometer
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
CN202011517160.5A
Other languages
English (en)
Other versions
CN112697142A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202011517160.5A priority Critical patent/CN112697142B/zh
Publication of CN112697142A publication Critical patent/CN112697142A/zh
Application granted granted Critical
Publication of CN112697142B publication Critical patent/CN112697142B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Abstract

本发明公开了一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法，包括以下步骤：步骤1：周期读取t时刻陀螺输出、加速度计输出和轮速里程计输出；步骤2：计算惯性/轮速里程计预积分量测、雅克比矩阵和协方差矩阵：步骤3：基于协方差矩阵，构建惯性/轮速里程计融合的目标函数，利用图优化算法对载体位置、速度、姿态、陀螺零偏、加速度计零偏、轮速里程计标度因数、轮速里程计与惯性传感器外参进行估计，得到优化结果：步骤4：利用优化结果和雅克比矩阵，对步骤2计算得到的惯性/轮速里程计预积分量测进行校正。

Description

一种基于预积分理论的惯性/轮速里程计融合定位与参数优 化方法
技术领域
本发明属于自主导航技术领域，具体涉及一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法。
背景技术
惯性/轮速里程计融合是一种常见的组合导航方式，其都是利用递推的方式对载体的导航信息进行解算。惯性/轮速里程计组合导航具有自主性强，不受外接干扰的优势，在车载导航领域具有广泛的应用。
传统的惯性/轮速里程计组合导航普遍是基于卡尔曼滤波器的方式，对载体的导航信息以及传感器参数进行解算。但是卡尔曼滤波器无法有效利用历史信息，同时其对非线性量测的处理也会引入误差，进而影响解算精度。尽管基于图优化的方法能够解决上述问题，但是历史状态的优化会导致轮速里程计的量测约束需要进行重复积分，从而影响计算效率。
发明内容
发明目的：为提高解算精度和计算效率，本发明提出了一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法，通过引入预积分理论与图优化算法，提高惯性/轮速里程计的融合估计精度以及计算效率。
技术方案：一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法，包括以下步骤：
步骤1：周期读取t时刻陀螺输出、加速度计输出和轮速里程计输出；
步骤2：计算惯性/轮速里程计预积分量测、雅克比矩阵和协方差矩阵：
步骤3：基于协方差矩阵，构建惯性/轮速里程计融合的目标函数，利用图优化算法对载体位置、速度、姿态、陀螺零偏、加速度计零偏、轮速里程计标度因数、轮速里程计与惯性传感器外参进行估计，得到优化结果：
步骤4：利用优化结果和雅克比矩阵，对步骤2计算得到的惯性/轮速里程计预积分量测进行校正。
进一步的，所述步骤2具体包括：
S210：假设惯性坐标系与机体系重合，建立参考系为世界系的状态传播方程；
S220：将状态传播方程的参考系由世界系转换为tk时刻的机体系，记为bk系；
S230：采用递推方法计算tk+1时刻的机体相对于bk系的惯性/轮速里程计预积分量测；
S240：构建tk时刻与tk+1时刻之间的惯性/轮速里程计预积分量测误差状态传播方程；
S250：基于惯性/轮速里程计预积分量测误差状态传播方程构建雅克比矩阵和协方差矩阵的递推方程；
S260：基于雅克比矩阵和协方差矩阵的递推方程，计算得到tk+1时刻的雅克比矩阵和协方差矩阵；
其中，所述惯性/轮速里程计预积分量测包括利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量
Figure BDA0002847744660000021
利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量
Figure BDA0002847744660000022
利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量
Figure BDA0002847744660000023
和利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量
Figure BDA0002847744660000024
进一步的，所述的参考系为世界系的状态传播方程形式如下：
Figure BDA0002847744660000025
Figure BDA0002847744660000026
其中，假设惯性坐标系与机体系重合，记为b；世界系记为w，
Figure BDA0002847744660000027
和
Figure BDA0002847744660000028
分别为tk与tk+1时刻机体在世界系下的位置向量，
Figure BDA0002847744660000029
和
Figure BDA00028477446600000210
分别为tk与tk+1时刻机体在世界系下的速度向量，
Figure BDA00028477446600000211
和
Figure BDA00028477446600000212
分别为tk与tk+1时刻机体系相对于世界系的四元数，Δtk为[tk,tk+1]之间的时间间隔，
Figure BDA00028477446600000213
为t时刻从机体系到世界系的旋转矩阵，gw为重力向量；
Figure BDA00028477446600000214
和
Figure BDA00028477446600000215
分别为t时刻加速度计与陀螺的零偏，na和nw分别为加速度计和陀螺的噪声，
Figure BDA0002847744660000031
和
Figure BDA0002847744660000032
为tk与tk+1时刻轮速里程计在机体系下的位置向量，
Figure BDA0002847744660000033
为t时刻轮速里程计系到机体系的旋转矩阵，
Figure BDA0002847744660000034
为t时刻轮速里程计的标度因数，ns为轮速里程计的量测噪声；
Figure BDA0002847744660000035
为陀螺输出，
Figure BDA0002847744660000036
为加速度计输出，
Figure BDA0002847744660000037
为轮速里程计输出。
进一步的，所述的将状态传播的参考系由世界系转换为tk时刻的机体系，记为bk，形式如下：
Figure BDA0002847744660000038
其中，
Figure BDA0002847744660000039
其中，
Figure BDA00028477446600000310
为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量，
Figure BDA00028477446600000311
为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量，
Figure BDA00028477446600000312
为利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量，
Figure BDA00028477446600000313
为利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量，
Figure BDA00028477446600000314
为t时刻的机体相对于bk系的旋转矩阵，
Figure BDA00028477446600000315
为时刻的机体相对于bk系的四元数变化量。
进一步的，采用预积分量测递推方程计算tk+1时刻的机体相对于bk系的惯性/轮速里程计预积分量测；
所述预积分量测递推方程形式如下：
Figure BDA0002847744660000041
其中，
Figure BDA0002847744660000042
和
Figure BDA0002847744660000043
分别为ti以及ti+1时刻的机体相对于bk系的标称位置变化量，
Figure BDA0002847744660000044
和
Figure BDA0002847744660000045
分别为ti以及ti+1时刻的机体相对于bk系的标称速度变化量，
Figure BDA0002847744660000046
和
Figure BDA0002847744660000047
分别为ti以及ti+1时刻的机体相对于bk系的标称四元数变化量，
Figure BDA0002847744660000048
和
Figure BDA0002847744660000049
分别为ti以及ti+1时刻的机体相对于bk系的位置变化量；
Figure BDA00028477446600000410
和
Figure BDA00028477446600000411
分别为ti以及ti+1时刻的加速度计标称零偏，
Figure BDA00028477446600000412
和
Figure BDA00028477446600000413
分别为ti以及ti+1时刻的陀螺标称零偏，
Figure BDA00028477446600000414
和
Figure BDA00028477446600000415
分别为ti以及ti+1时刻的轮速里程计标称标度因数，
Figure BDA00028477446600000416
和
Figure BDA00028477446600000417
分别为ti以及ti+1时刻的机体相对于bk系的标称旋转矩阵，
Figure BDA00028477446600000418
和
Figure BDA00028477446600000419
分别为ti以及ti+1时刻轮速里程计系到机体系的标称旋转矩阵，
Figure BDA00028477446600000420
和
Figure BDA00028477446600000421
分别为ti以及ti+1时刻轮速里程计在机体系下的位置向量，
Figure BDA00028477446600000422
和
Figure BDA00028477446600000423
分别为ti以及ti+1时刻加速度计输出，
Figure BDA00028477446600000424
和
Figure BDA00028477446600000425
分别为ti以及ti+1时刻陀螺输出，
Figure BDA00028477446600000426
和
Figure BDA00028477446600000427
分别为ti以及ti+1时刻轮速里程计输出，δt为[ti,ti+1]之间的时间间隔。
进一步的，所述误差状态传播方程形式如下：
δzi+1＝Fδzi+VN (6)
Figure BDA0002847744660000051
其中，δzi与δzi+1分别为ti与ti+1时刻的预积分量测误差，F为误差的状态转移矩阵，V为测量噪声系数矩阵，N为量测噪声向量；
Figure BDA0002847744660000052
和
Figure BDA0002847744660000053
分别为ti与ti+1时刻的位置变化量误差，
Figure BDA0002847744660000054
和
Figure BDA0002847744660000055
分别为ti与ti+1时刻的旋转误差，
Figure BDA0002847744660000056
和
Figure BDA0002847744660000057
分别为ti与ti+1时刻的速度变化量误差，
Figure BDA0002847744660000058
和
Figure BDA0002847744660000059
分别为ti与ti+1时刻的加速度计零偏误差，
Figure BDA00028477446600000510
和
Figure BDA00028477446600000511
分别为ti与ti+1时刻的陀螺零偏误差，
Figure BDA00028477446600000512
和
Figure BDA00028477446600000513
分别为ti与ti+1时刻位置变化量误差，
Figure BDA00028477446600000514
和
Figure BDA00028477446600000515
分别为ti与ti+1时刻的轮速里程计标度因数误差，
Figure BDA00028477446600000516
和
Figure BDA00028477446600000517
分别为ti与ti+1时刻轮速里程计在机体系下的位置误差，
Figure BDA00028477446600000518
和
Figure BDA00028477446600000519
分别为ti与ti+1时刻轮速里程计系与机体系的旋转误差；
误差的状态转移矩阵、测量噪声系数矩阵和量测噪声向量采用如下公式：
Figure BDA00028477446600000520
Figure BDA0002847744660000061
Figure BDA0002847744660000062
其中，状态转移矩阵与测量噪声系数矩阵中的元素采用如下所示：
Figure BDA0002847744660000063
Figure BDA0002847744660000064
Figure BDA0002847744660000065
Figure BDA0002847744660000066
Figure BDA0002847744660000067
Figure BDA0002847744660000071
Figure BDA0002847744660000072
Figure BDA0002847744660000073
Figure BDA0002847744660000074
Figure BDA0002847744660000075
Figure BDA0002847744660000076
Figure BDA0002847744660000077
Figure BDA0002847744660000078
Figure BDA0002847744660000079
Figure BDA00028477446600000710
其中，
Figure BDA00028477446600000711
和
Figure BDA00028477446600000712
分别为ti以及ti+1时刻加速度计的量测噪声，
Figure BDA00028477446600000713
和
Figure BDA00028477446600000714
分别为ti以及ti+1时刻陀螺的量测噪声，
Figure BDA00028477446600000715
和
Figure BDA00028477446600000723
分别为加速度计与陀螺的随机游走噪声，
Figure BDA00028477446600000716
和
Figure BDA00028477446600000717
分别为ti以及ti+1时刻轮速里程计的量测噪声，
Figure BDA00028477446600000718
和
Figure BDA00028477446600000719
分别为轮速里程计标度因数以及安装参数的随机游走噪声。
进一步的，所述雅克比矩阵和协方差矩阵的递推方程，表示为：
Figure BDA00028477446600000720
对应时刻的系统状态的雅克比矩阵和协方差矩阵为：
Figure BDA00028477446600000721
雅克比矩阵和协方差矩阵的递推方程为：
Figure BDA00028477446600000722
其中，Ji和Ji+1分别为ti以及ti+1时刻的雅克比矩阵，Pi和Pi+1分别为ti以及ti+1时刻的协方差矩阵，Q为噪声信号N的协方差矩阵，表示为：
Figure BDA0002847744660000081
进一步的，步骤3中的目标函数采用如下形式：
Figure BDA0002847744660000082
其中，
Figure BDA0002847744660000083
为惯性/里程计测量残差，表示为：
Figure BDA0002847744660000091
Figure BDA0002847744660000092
其中，
Figure BDA0002847744660000093
分别为tk时刻机体系相对于世界系的位置、速度与四元数，
Figure BDA0002847744660000094
和
Figure BDA0002847744660000095
为tk时刻加速度计与陀螺的零偏，
Figure BDA0002847744660000096
分别为tk时刻轮速里程计的标度因数、轮速里程计系相对于机体系的位置、四元数；
待优化变量采用如下形式：
Figure BDA0002847744660000097
Figure BDA0002847744660000098
对
Figure BDA0002847744660000099
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000101
其中，L(·)和R(·)分别代表四元数的左乘与右乘，对于四元数q＝[x y z s]＝[w s]：
Figure BDA0002847744660000102
Figure BDA0002847744660000103
Figure BDA0002847744660000104
对
Figure BDA0002847744660000105
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000111
Figure BDA0002847744660000112
对
Figure BDA0002847744660000113
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000114
Figure BDA0002847744660000115
对
Figure BDA0002847744660000116
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000121
Figure BDA0002847744660000122
对
Figure BDA0002847744660000123
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000124
Figure BDA0002847744660000125
对
Figure BDA0002847744660000126
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000131
其中，
Figure BDA0002847744660000132
和
Figure BDA0002847744660000133
分别为
Figure BDA0002847744660000134
对应
Figure BDA0002847744660000135
和
Figure BDA0002847744660000136
的雅克比矩阵，
Figure BDA0002847744660000137
和
Figure BDA0002847744660000138
分别为
Figure BDA0002847744660000139
对应
Figure BDA00028477446600001310
和
Figure BDA00028477446600001311
的雅克比矩阵，
Figure BDA00028477446600001312
为
Figure BDA00028477446600001313
对应
Figure BDA00028477446600001314
的雅克比矩阵，
Figure BDA00028477446600001315
Figure BDA00028477446600001316
和
Figure BDA00028477446600001317
分别为
Figure BDA00028477446600001318
对应
Figure BDA00028477446600001319
和
Figure BDA00028477446600001320
的雅克比矩阵；上述雅克比矩阵均对应
Figure BDA00028477446600001321
的子块矩阵，可以从
Figure BDA00028477446600001322
对应位置中获取。
进一步的，所述步骤4具体包括：
Figure BDA00028477446600001323
Figure BDA00028477446600001324
Figure BDA00028477446600001325
Figure BDA00028477446600001326
其中，
Figure BDA00028477446600001327
和
Figure BDA00028477446600001328
分别为递推得到的预积分量测，
Figure BDA00028477446600001329
和
Figure BDA00028477446600001330
分别为校正后的预积分量测。
有益效果：通过本发明，可以提高惯性/里程计导航与参数估计精度，并解决优化过程中对里程计量测信息的重复积分问题，提高计算效率。
附图说明
图1为本发明方法的流程示意图；
图2为跑车测试轨迹；
图3为采用传统方法、本发明方法时轮速里程计标度因数的估计结果；
图4为采用传统方法、本发明方法时轮速里程计与机体安装角度的估计结果；
图5为采用传统方法、本发明方法时轮速里程计与机体安装横向位置的估计结果
图6为采用传统方法、本发明方法时轮速里程计与机体安装纵向位置的估计结果；
图7为采用传统方法、本发明方法估计结果进行补偿后的轮速里程计水平位置误差。
具体实施方式
现通过参考示范性实施例，本发明的目的和功能以及用于实现这些目的和功能的方法将得以阐明。然而，本发明并不受限于以下所公开的示范性实施例；可以通过不同形式来对其加以实现。说明书的实质仅仅是帮助相关领域技术人员综合理解本发明的具体细节。
结合图1，本发明的优化方法包括以下步骤：
步骤1：周期读取t时刻陀螺输出、加速度计输出、轮速里程计输出；
步骤2：计算惯性/轮速里程计预积分量测、雅克比矩阵及其协方差矩阵：现对该步骤做进一步说明。
S210：状态传播方程采用如下公式：
Figure BDA0002847744660000141
Figure BDA0002847744660000142
其中，假设惯性坐标系与机体系重合，记为b；世界坐标系采用东北天坐标系，记为w，
Figure BDA0002847744660000151
和
Figure BDA0002847744660000152
分别为tk与tk+1时刻机体在世界系下的位置向量，
Figure BDA0002847744660000153
和
Figure BDA0002847744660000154
分别为tk与tk+1时刻机体在世界系下的速度向量，
Figure BDA0002847744660000155
和
Figure BDA0002847744660000156
分别为tk与tk+1时刻机体系相对于世界系的四元数，Δtk为[tk,tk+1]之间的时间间隔，
Figure BDA0002847744660000157
为t时刻从机体系到世界系的旋转矩阵，gw为重力向量。
Figure BDA0002847744660000158
和
Figure BDA0002847744660000159
分别为t时刻加速度计与陀螺的零偏，na和nw分别为加速度计和陀螺的噪声，
Figure BDA00028477446600001510
和
Figure BDA00028477446600001511
为tk与tk+1时刻轮速里程计在机体系下的位置向量，
Figure BDA00028477446600001512
为t时刻轮速里程计系到机体系的旋转矩阵，
Figure BDA00028477446600001513
为t时刻轮速里程计的标度因数，ns为轮速里程计的量测噪声，
Figure BDA00028477446600001514
为陀螺输出、
Figure BDA00028477446600001515
为加速度计输出、
Figure BDA00028477446600001516
为轮速里程计输出。
S220：将状态传播的参考系由世界系转换为tk时刻的机体系，记为bk，采用如下公式：
Figure BDA00028477446600001517
其中，
Figure BDA00028477446600001518
其中，
Figure BDA00028477446600001519
为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量，
Figure BDA00028477446600001520
为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量，
Figure BDA00028477446600001521
为利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量，
Figure BDA00028477446600001522
为利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量，
Figure BDA00028477446600001523
为t时刻的机体相对于bk系的旋转矩阵，
Figure BDA0002847744660000161
为时刻的机体相对于bk系的四元数变化量
S230：预积分量测递推方程采用如下公式：
Figure BDA0002847744660000162
其中，
Figure BDA0002847744660000163
和
Figure BDA0002847744660000164
分别为ti以及ti+1时刻的机体相对于bk系的标称位置变化量，
Figure BDA0002847744660000165
和
Figure BDA0002847744660000166
分别为ti以及ti+1时刻的机体相对于bk系的标称速度变化量，
Figure BDA0002847744660000167
和
Figure BDA0002847744660000168
分别为ti以及ti+1时刻的机体相对于bk系的标称四元数变化量，
Figure BDA0002847744660000169
和
Figure BDA00028477446600001610
分别为ti以及ti+1时刻的机体相对于bk系的位置变化量。
Figure BDA00028477446600001611
和
Figure BDA00028477446600001612
分别为ti以及ti+1时刻的加速度计标称零偏，
Figure BDA00028477446600001613
和
Figure BDA00028477446600001614
分别为ti以及ti+1时刻的陀螺标称零偏，
Figure BDA00028477446600001615
和
Figure BDA00028477446600001616
分别为ti以及ti+1时刻的轮速里程计标称标度因数，
Figure BDA00028477446600001617
和
Figure BDA00028477446600001618
分别为ti以及ti+1时刻的机体相对于bk系的标称旋转矩阵，
Figure BDA00028477446600001619
和
Figure BDA00028477446600001620
分别为ti以及ti+1时刻轮速里程计系到机体系的标称旋转矩阵，
Figure BDA00028477446600001621
和
Figure BDA00028477446600001622
分别为ti以及ti+1时刻轮速里程计在机体系下的位置向量，
Figure BDA00028477446600001623
和
Figure BDA00028477446600001624
分别为ti以及ti+1时刻加速度计输出，
Figure BDA00028477446600001625
和
Figure BDA00028477446600001626
分别为ti以及ti+1时刻陀螺输出，
Figure BDA00028477446600001627
和
Figure BDA00028477446600001628
分别为ti以及ti+1时刻轮速里程计输出，δt为[ti,ti+1]之间的时间间隔。
S240：误差状态传播方程采用如下公式：
δzi+1＝Fδzi+VN (6)
Figure BDA0002847744660000171
其中，δzi与δzi+1分别为ti与ti+1时刻的预积分量测误差，F为误差的状态转移矩阵，V为测量噪声系数矩阵，N为量测噪声向量。
Figure BDA0002847744660000172
和
Figure BDA0002847744660000173
分别为ti与ti+1时刻的位置变化量误差，
Figure BDA0002847744660000174
和
Figure BDA0002847744660000175
分别为ti与ti+1时刻的旋转误差，
Figure BDA0002847744660000176
和
Figure BDA0002847744660000177
分别为ti与ti+1时刻的速度变化量误差，
Figure BDA0002847744660000178
和
Figure BDA0002847744660000179
分别为ti与ti+1时刻的加速度计零偏误差，
Figure BDA00028477446600001710
和
Figure BDA00028477446600001711
分别为ti与ti+1时刻的陀螺零偏误差，
Figure BDA00028477446600001712
和
Figure BDA00028477446600001713
分别为ti与ti+1时刻位置变化量误差，
Figure BDA00028477446600001714
和
Figure BDA00028477446600001715
分别为ti与ti+1时刻的轮速里程计标度因数误差，
Figure BDA00028477446600001716
和
Figure BDA00028477446600001717
分别为ti与ti+1时刻轮速里程计在机体系下的位置误差，
Figure BDA00028477446600001718
和
Figure BDA00028477446600001719
分别为ti与ti+1时刻轮速里程计系与机体系的旋转误差。
ti以及ti+1都大于tk且小于tk+1。这里描述的是连续两个惯性量测时刻ti以及ti+1的递推关系，而tk与tk+1之间分布着n个惯性量测，因此需要递推n次才能得到tk与tk+1之间的预积分量测。
误差状态的状态转移矩阵与噪声系数矩阵采用如下公式：
Figure BDA00028477446600001720
Figure BDA0002847744660000181
Figure BDA0002847744660000182
其中，状态转移矩阵与噪声系数阵的元素采用如下所示：
Figure BDA0002847744660000183
Figure BDA0002847744660000184
Figure BDA0002847744660000185
Figure BDA0002847744660000186
Figure BDA0002847744660000187
Figure BDA0002847744660000191
Figure BDA0002847744660000192
Figure BDA0002847744660000193
Figure BDA0002847744660000194
Figure BDA0002847744660000195
Figure BDA0002847744660000196
Figure BDA0002847744660000197
Figure BDA0002847744660000198
Figure BDA0002847744660000199
Figure BDA00028477446600001910
其中，
Figure BDA00028477446600001911
和
Figure BDA00028477446600001912
分别为ti以及ti+1时刻加速度计的量测噪声，
Figure BDA00028477446600001913
Figure BDA00028477446600001914
Figure BDA00028477446600001915
分别为ti以及ti+1时刻陀螺的量测噪声，
Figure BDA00028477446600001916
和
Figure BDA00028477446600001917
分别为加速度计与陀螺的随机游走噪声，
Figure BDA00028477446600001918
和
Figure BDA00028477446600001919
分别为ti以及ti+1时刻轮速里程计的量测噪声，
Figure BDA00028477446600001920
和
Figure BDA00028477446600001921
分别为轮速里程计标度因数以及安装参数的随机游走噪声。
S240：雅克比矩阵和协方差矩阵的递推方程：
初始时刻(即
Figure BDA00028477446600001922
对应的时刻)系统状态的雅克比矩阵和协方差矩阵为：
Figure BDA00028477446600001923
雅克比矩阵和协方差矩阵的递推方程为：
Figure BDA00028477446600001924
其中，Ji和Ji+1分别为ti以及ti+1时刻的雅克比矩阵，Pi和Pi+1分别为ti以及ti+1时刻的协方差矩阵，Q为噪声信号N的协方差矩阵，表示为：
Figure BDA0002847744660000201
步骤3：构建惯性/轮速里程计融合的目标函数，利用图优化算法对载体位置、速度、姿态、陀螺零偏、加速度计零偏、轮速里程计标度因数、轮速里程计与惯性传感器外参进行估计：现对该步骤作出具体说明。
S310：状态向量采用如下形式：
Figure BDA0002847744660000202
其中，
Figure BDA0002847744660000203
分别为tk时刻机体系相对于世界系的位置、速度与四元数，
Figure BDA0002847744660000204
和
Figure BDA0002847744660000205
为tk时刻加速度计与陀螺的零偏，
Figure BDA0002847744660000206
分别为tk时刻轮速里程计的标度因数、轮速里程计系相对于机体系的位置、四元数。
S320：目标函数采用如下形式：
Figure BDA0002847744660000207
其中，
Figure BDA0002847744660000208
为惯性/里程计测量残差，可以表示为：
Figure BDA0002847744660000211
S330：待优化变量采用如下形式：
Figure BDA0002847744660000212
S340：雅克比矩阵采用如下形式：
Figure BDA0002847744660000213
对
Figure BDA0002847744660000214
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000221
其中，L(·)和R(·)分别代表四元数的左乘与右乘，对于四元数q＝[x y z s]＝[w s]：
Figure BDA0002847744660000222
Figure BDA0002847744660000223
Figure BDA0002847744660000224
对
Figure BDA0002847744660000225
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000231
Figure BDA0002847744660000232
对
Figure BDA0002847744660000233
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000236
Figure BDA0002847744660000234
对
Figure BDA0002847744660000235
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000241
Figure BDA0002847744660000242
对
Figure BDA0002847744660000243
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000244
Figure BDA0002847744660000245
对
Figure BDA0002847744660000246
进行求导，可得雅克比矩阵如下：
Figure BDA0002847744660000251
其中，
Figure BDA0002847744660000252
和
Figure BDA0002847744660000253
分别为
Figure BDA0002847744660000254
对应
Figure BDA0002847744660000255
和
Figure BDA0002847744660000256
的雅克比矩阵，
Figure BDA0002847744660000257
和
Figure BDA0002847744660000258
分别为
Figure BDA0002847744660000259
对应
Figure BDA00028477446600002510
和
Figure BDA00028477446600002511
的雅克比矩阵，
Figure BDA00028477446600002512
为
Figure BDA00028477446600002513
对应
Figure BDA00028477446600002514
的雅克比矩阵，
Figure BDA00028477446600002515
Figure BDA00028477446600002516
和
Figure BDA00028477446600002517
分别为
Figure BDA00028477446600002518
对应
Figure BDA00028477446600002519
和
Figure BDA00028477446600002520
的雅克比矩阵。上述雅克比矩阵均对应
Figure BDA00028477446600002521
的子块矩阵，可以从
Figure BDA00028477446600002522
对应位置中获取。
以上是对步骤3目标函数进行求导所获得的雅克比矩阵，解算之后才能得到优化结果
Figure BDA00028477446600002523
和
Figure BDA00028477446600002524
步骤4：利用优化结果对预积分量测进行校正。
Figure BDA00028477446600002525
Figure BDA00028477446600002526
Figure BDA00028477446600002527
Figure BDA00028477446600002528
其中，系数
Figure BDA0002847744660000261
和
Figure BDA0002847744660000262
是误差状态转移矩阵
Figure BDA0002847744660000263
的子块，
Figure BDA0002847744660000264
和
Figure BDA0002847744660000265
分别为递推得到的预积分量测，
Figure BDA0002847744660000266
Figure BDA0002847744660000267
和
Figure BDA0002847744660000268
分别为校正后的预积分量测。
现采用实际跑车数据测试的形式，对使用本发明方法后的轮速里程计参数估计精度进行验证，其中惯性传感器精度如下：陀螺零偏稳定性为10deg/h，加速度计零偏稳定性为40μg，轮速里程计测速精度为0.01m/s，卫星定位精度为厘米级，用于提供轨迹参考。惯性传感器与轮速里程计的采样频率均为50Hz，卫星采样频率为1Hz。
图2为跑车测试轨迹，图3为采用传统方法、本发明方法时轮速里程计标度因数的估计结果，由图3可知，传统方法与本发明方法的标定结果不同；图4为采用传统方法、本发明方法时轮速里程计与机体安装角度的估计结果，由图4可知，传统方法与本发明方法的标定结果不同；图5～图6为采用传统方法、本发明方法时轮速里程计与机体安装位置的估计结果，由图5～图6可知，传统方法与本发明方法的标定结果不同；图7为采用传统方法、本发明方法估计结果进行补偿后的轮速里程计水平位置误差，由图7可知，采用本发明方法补偿后的水平位置误差小于传统方法的位置误差，可以看出本发明方法的标定结果优于传统方法。

Claims (1)

1.一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法，其特征在于：包括以下步骤：
步骤1：周期读取t时刻陀螺输出、加速度计输出和轮速里程计输出；
步骤2：计算惯性/轮速里程计预积分量测、雅克比矩阵和协方差矩阵：
步骤3：基于协方差矩阵，构建惯性/轮速里程计融合的目标函数，利用图优化算法对载体位置、速度、姿态、陀螺零偏、加速度计零偏、轮速里程计标度因数、轮速里程计与惯性传感器外参进行估计，得到优化结果：
步骤4：利用优化结果和雅克比矩阵，对步骤2计算得到的惯性/轮速里程计预积分量测进行校正；
所述步骤2具体包括：
S210：假设惯性坐标系与机体系重合，建立参考系为世界系的状态传播方程；
S220：将状态传播方程的参考系由世界系转换为tk时刻的机体系，记为bk系；
S230：采用递推方法计算tk+1时刻的机体相对于bk系的惯性/轮速里程计预积分量测；
S240：构建tk时刻与tk+1时刻之间的惯性/轮速里程计预积分量测误差状态传播方程；
S250：基于惯性/轮速里程计预积分量测误差状态传播方程构建雅克比矩阵和协方差矩阵的递推方程；
S260：基于雅克比矩阵和协方差矩阵的递推方程，计算得到tk+1时刻的雅克比矩阵和协方差矩阵；
其中，所述惯性/轮速里程计预积分量测包括利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量
Figure FDA0003906478740000011
利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量
Figure FDA0003906478740000012
利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量
Figure FDA0003906478740000013
和利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量
Figure FDA0003906478740000034
；
所述的参考系为世界系的状态传播方程形式如下：
Figure FDA0003906478740000021
Figure FDA0003906478740000022
其中，假设惯性坐标系与机体系重合，记为b；世界系记为w，
Figure FDA0003906478740000023
和
Figure FDA0003906478740000024
分别为tk与tk+1时刻机体在世界系下的位置向量，
Figure FDA0003906478740000025
和
Figure FDA0003906478740000026
分别为tk与tk+1时刻机体在世界系下的速度向量，
Figure FDA0003906478740000027
和
Figure FDA0003906478740000028
分别为tk与tk+1时刻机体系相对于世界系的四元数，Δtk为[tk,tk+1]之间的时间间隔，
Figure FDA0003906478740000029
为t时刻从机体系到世界系的旋转矩阵，gw为重力向量；
Figure FDA00039064787400000210
和
Figure FDA00039064787400000211
分别为t时刻加速度计与陀螺的零偏，na和nw分别为加速度计和陀螺的噪声，
Figure FDA00039064787400000212
和
Figure FDA00039064787400000213
为tk与tk+1时刻轮速里程计在机体系下的位置向量，
Figure FDA00039064787400000214
为t时刻轮速里程计系到机体系的旋转矩阵，
Figure FDA00039064787400000215
为t时刻轮速里程计的标度因数，ns为轮速里程计的量测噪声；
Figure FDA00039064787400000216
为陀螺输出，
Figure FDA00039064787400000217
为加速度计输出，
Figure FDA00039064787400000218
为轮速里程计输出；
所述的将状态传播的参考系由世界系转换为tk时刻的机体系，记为bk，形式如下：
Figure FDA00039064787400000219
其中，
Figure FDA00039064787400000220
其中，
Figure FDA0003906478740000031
为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量，
Figure FDA0003906478740000032
为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量，
Figure FDA0003906478740000033
为利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量，
Figure FDA0003906478740000034
为利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量，
Figure FDA0003906478740000035
为t时刻的机体相对于bk系的旋转矩阵，
Figure FDA0003906478740000036
为时刻的机体相对于bk系的四元数变化量；
采用预积分量测递推方程计算tk+1时刻的机体相对于bk系的惯性/轮速里程计预积分量测；
所述预积分量测递推方程形式如下：
Figure FDA0003906478740000037
其中，
Figure FDA0003906478740000038
和
Figure FDA0003906478740000039
分别为ti以及ti+1时刻的机体相对于bk系的标称位置变化量，
Figure FDA00039064787400000310
和
Figure FDA00039064787400000311
分别为ti以及ti+1时刻的机体相对于bk系的标称速度变化量，
Figure FDA00039064787400000312
和
Figure FDA00039064787400000313
分别为ti以及ti+1时刻的机体相对于bk系的标称四元数变化量，
Figure FDA00039064787400000314
和
Figure FDA00039064787400000315
分别为ti以及ti+1时刻的机体相对于bk系的位置变化量；
Figure FDA00039064787400000316
和
Figure FDA00039064787400000317
分别为ti以及ti+1时刻的加速度计标称零偏，
Figure FDA00039064787400000318
和
Figure FDA00039064787400000319
分别为ti以及ti+1时刻的陀螺标称零偏，
Figure FDA00039064787400000320
和
Figure FDA00039064787400000321
分别为ti以及ti+1时刻的轮速里程计标称标度因数，
Figure FDA00039064787400000322
和
Figure FDA00039064787400000323
分别为ti以及ti+1时刻的机体相对于bk系的标称旋转矩阵，
Figure FDA0003906478740000041
和
Figure FDA0003906478740000042
分别为ti以及ti+1时刻轮速里程计系到机体系的标称旋转矩阵，
Figure FDA0003906478740000043
和
Figure FDA0003906478740000044
分别为ti以及ti+1时刻轮速里程计在机体系下的位置向量，
Figure FDA0003906478740000045
和
Figure FDA0003906478740000046
分别为ti以及ti+1时刻加速度计输出，
Figure FDA0003906478740000047
和
Figure FDA0003906478740000048
分别为ti以及ti+1时刻陀螺输出，
Figure FDA0003906478740000049
和
Figure FDA00039064787400000410
分别为ti以及ti+1时刻轮速里程计输出，δt为[ti,ti+1]之间的时间间隔；
所述惯性/轮速里程计预积分量测误差状态传播方程形式如下：
δzi+1＝Fδzi+VN (6)
Figure FDA00039064787400000411
其中，δzi与δzi+1分别为ti与ti+1时刻的预积分量测误差，F为误差的状态转移矩阵，V为测量噪声系数矩阵，N为量测噪声向量；
Figure FDA00039064787400000412
和
Figure FDA00039064787400000413
分别为ti与ti+1时刻的位置变化量误差，
Figure FDA00039064787400000414
和
Figure FDA00039064787400000415
分别为ti与ti+1时刻的旋转误差，
Figure FDA00039064787400000416
和
Figure FDA00039064787400000417
分别为ti与ti+1时刻的速度变化量误差，
Figure FDA00039064787400000418
和
Figure FDA00039064787400000419
分别为ti与ti+1时刻的加速度计零偏误差，
Figure FDA00039064787400000420
和
Figure FDA00039064787400000421
分别为ti与ti+1时刻的陀螺零偏误差，
Figure FDA00039064787400000422
和
Figure FDA00039064787400000423
分别为ti与ti+1时刻位置变化量误差，
Figure FDA00039064787400000424
和
Figure FDA00039064787400000425
分别为ti与ti+1时刻的轮速里程计标度因数误差，
Figure FDA00039064787400000426
和
Figure FDA00039064787400000427
分别为ti与ti+1时刻轮速里程计在机体系下的位置误差，
Figure FDA00039064787400000428
和
Figure FDA00039064787400000429
分别为ti与ti+1时刻轮速里程计系与机体系的旋转误差；
误差的状态转移矩阵、测量噪声系数矩阵和量测噪声向量采用如下公式：
Figure FDA0003906478740000051
Figure FDA0003906478740000052
Figure FDA0003906478740000053
其中，状态转移矩阵与测量噪声系数矩阵中的元素采用如下所示：
Figure FDA0003906478740000061
Figure FDA0003906478740000062
Figure FDA0003906478740000063
Figure FDA0003906478740000064
Figure FDA0003906478740000065
Figure FDA0003906478740000066
Figure FDA0003906478740000067
Figure FDA0003906478740000068
Figure FDA0003906478740000069
Figure FDA00039064787400000610
Figure FDA00039064787400000611
Figure FDA00039064787400000612
Figure FDA00039064787400000613
Figure FDA00039064787400000614
Figure FDA00039064787400000615
其中，
Figure FDA00039064787400000616
和
Figure FDA00039064787400000617
分别为ti以及ti+1时刻加速度计的量测噪声，
Figure FDA00039064787400000618
和
Figure FDA00039064787400000619
分别为ti以及ti+1时刻陀螺的量测噪声，
Figure FDA00039064787400000620
和
Figure FDA00039064787400000621
分别为加速度计与陀螺的随机游走噪声，
Figure FDA00039064787400000622
和
Figure FDA00039064787400000623
分别为ti以及ti+1时刻轮速里程计的量测噪声，
Figure FDA00039064787400000624
和
Figure FDA00039064787400000625
分别为轮速里程计标度因数以及安装参数的随机游走噪声；
所述雅克比矩阵和协方差矩阵的递推方程，表示为：
Figure FDA0003906478740000071
对应时刻的系统状态的雅克比矩阵和协方差矩阵为：
Figure FDA0003906478740000072
雅克比矩阵和协方差矩阵的递推方程为：
Figure FDA0003906478740000073
其中，Ji和Ji+1分别为ti以及ti+1时刻的雅克比矩阵，Pi和Pi+1分别为ti以及ti+1时刻的协方差矩阵，Q为噪声信号N的协方差矩阵，表示为：
Figure DEST_PATH_IMAGE002
；
步骤3中的目标函数采用如下形式：
Figure FDA0003906478740000075
其中，
Figure FDA0003906478740000076
为惯性/里程计测量残差，表示为：
Figure FDA0003906478740000081
Figure FDA0003906478740000082
其中，
Figure FDA0003906478740000083
分别为tk时刻机体系相对于世界系的位置、速度与四元数，
Figure FDA0003906478740000084
和
Figure FDA0003906478740000085
为tk时刻加速度计与陀螺的零偏，
Figure FDA0003906478740000086
分别为tk时刻轮速里程计的标度因数、轮速里程计系相对于机体系的位置、四元数；
待优化变量采用如下形式：
Figure FDA0003906478740000087
Figure FDA0003906478740000088
对
Figure FDA0003906478740000089
进行求导，可得雅克比矩阵如下：
Figure FDA0003906478740000091
其中，L(·)和R(·)分别代表四元数的左乘与右乘，对于四元数q＝[x y z s]＝[ws]：
Figure FDA0003906478740000092
Figure FDA0003906478740000093
Figure FDA0003906478740000094
对
Figure FDA0003906478740000095
进行求导，可得雅克比矩阵如下：
Figure FDA0003906478740000101
Figure FDA0003906478740000102
对
Figure FDA0003906478740000103
进行求导，可得雅克比矩阵如下：
Figure FDA0003906478740000104
Figure FDA0003906478740000105
对
Figure FDA0003906478740000106
进行求导，可得雅克比矩阵如下：
Figure FDA0003906478740000111
Figure FDA0003906478740000112
对
Figure FDA0003906478740000113
进行求导，可得雅克比矩阵如下：
Figure FDA0003906478740000114
Figure FDA0003906478740000115
对
Figure FDA0003906478740000116
进行求导，可得雅克比矩阵如下：
Figure FDA0003906478740000121
其中，
Figure FDA0003906478740000122
和
Figure FDA0003906478740000123
分别为
Figure FDA0003906478740000124
对应
Figure FDA0003906478740000125
和
Figure FDA0003906478740000126
的雅克比矩阵，
Figure FDA0003906478740000127
和
Figure FDA0003906478740000128
分别为
Figure FDA0003906478740000129
对应
Figure FDA00039064787400001210
和
Figure FDA00039064787400001211
的雅克比矩阵，
Figure FDA00039064787400001212
为
Figure FDA00039064787400001213
对应
Figure FDA00039064787400001214
的雅克比矩阵，
Figure FDA00039064787400001215
Figure FDA00039064787400001216
和
Figure FDA00039064787400001217
分别为
Figure FDA00039064787400001218
对应
Figure FDA00039064787400001219
和
Figure FDA00039064787400001220
的雅克比矩阵；上述雅克比矩阵均对应
Figure FDA00039064787400001221
的子块矩阵，从
Figure FDA00039064787400001222
对应位置中获取；
所述优化结果包括
Figure FDA00039064787400001223
和
Figure DEST_PATH_IMAGE004
；
Figure FDA00039064787400001225
Figure FDA00039064787400001226
Figure FDA00039064787400001227
Figure FDA00039064787400001228
其中，
Figure FDA00039064787400001229
和
Figure FDA00039064787400001230
分别为递推得到的预积分量测，
Figure FDA00039064787400001231
和
Figure FDA00039064787400001232
分别为校正后的预积分量测。
CN202011517160.5A 2020-12-21 2020-12-21 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 Active CN112697142B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011517160.5A CN112697142B (zh) 2020-12-21 2020-12-21 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011517160.5A CN112697142B (zh) 2020-12-21 2020-12-21 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法

Publications (2)

Publication Number Publication Date
CN112697142A CN112697142A (zh) 2021-04-23
CN112697142B true CN112697142B (zh) 2023-03-10

Family

ID=75509230

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011517160.5A Active CN112697142B (zh) 2020-12-21 2020-12-21 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法

Country Status (1)

Country Link
CN (1) CN112697142B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113203428A (zh) * 2021-05-28 2021-08-03 拉扎斯网络科技（上海）有限公司 里程统计装置、基于里程统计装置的数据统计方法及接口

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111780754A (zh) * 2020-06-23 2020-10-16 南京航空航天大学 基于稀疏直接法的视觉惯性里程计位姿估计方法
CN111811506A (zh) * 2020-09-15 2020-10-23 中国人民解放军国防科技大学 视觉/惯性里程计组合导航方法、电子设备及存储介质

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法
CN111780754A (zh) * 2020-06-23 2020-10-16 南京航空航天大学 基于稀疏直接法的视觉惯性里程计位姿估计方法
CN111811506A (zh) * 2020-09-15 2020-10-23 中国人民解放军国防科技大学 视觉/惯性里程计组合导航方法、电子设备及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
GNSS/IMU/ODO/LiDAR-SLAM Integrated Navigation System Using IMU/ODO Pre-Integration;Le Chang等;《sensors》;20200820;第20卷(第4702期);第1-17页 *
基于 IMU/ODO 预积分的多传感器即插即用因子图融合方法;白师宇等;《中国惯性技术学报》;20201031;第28卷(第5期);第624-637页 *

Also Published As

Publication number Publication date
CN112697142A (zh) 2021-04-23

Similar Documents

Publication Publication Date Title
CN105300379B (zh) 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109343095B (zh) 一种车载导航车辆组合定位装置及其组合定位方法
CN109870173A (zh) 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN111238535B (zh) 一种基于因子图的imu误差在线标定方法
CN107063245B (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN111323050A (zh) 一种捷联惯导和多普勒组合系统标定方法
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN110700029B (zh) 一种轨道平顺性测试方法及系统
CN112014599B (zh) 一种集装箱卡车的轮速计速度修正方法及装置
CN112697142B (zh) 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法
CN110133694A (zh) 基于双天线gnss航向和轮速辅助的车辆定位方法及系统
CN113175933A (zh) 一种基于高精度惯性预积分的因子图组合导航方法
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN111912427B (zh) 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN106403999A (zh) 基于gnss的惯性导航加速度计漂移实时补偿方法
CN106918828B (zh) 一种飞行器自主导航方法及系统
CN113514064A (zh) 一种鲁棒因子图多源容错导航方法
CN111220151B (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
GR01 Patent grant
GR01 Patent grant