CN112697142B - 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 - Google Patents
一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000005457 optimization Methods 0.000 title claims abstract description 21
- 230000010354 integration Effects 0.000 title claims abstract description 16
- 230000004927 fusion Effects 0.000 title claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims abstract description 122
- 238000005259 measurement Methods 0.000 claims abstract description 56
- 238000004364 calculation method Methods 0.000 claims abstract description 5
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 4
- 230000008859 change Effects 0.000 claims description 26
- 238000009795 derivation Methods 0.000 claims description 19
- 230000007704 transition Effects 0.000 claims description 10
- 238000009434 installation Methods 0.000 claims description 6
- 238000005295 random walk Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 238000007796 conventional method Methods 0.000 description 14
- 238000010586 diagram Methods 0.000 description 5
- 238000012360 testing method Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012545 processing Methods 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/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
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
本发明公开了一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法,包括以下步骤:步骤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系的位置变化量利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量和利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量
进一步的,所述的参考系为世界系的状态传播方程形式如下:
其中,假设惯性坐标系与机体系重合,记为b;世界系记为w,和分别为tk与tk+1时刻机体在世界系下的位置向量,和分别为tk与tk+1时刻机体在世界系下的速度向量,和分别为tk与tk+1时刻机体系相对于世界系的四元数,Δtk为[tk,tk+1]之间的时间间隔,为t时刻从机体系到世界系的旋转矩阵,gw为重力向量;和分别为t时刻加速度计与陀螺的零偏,na和nw分别为加速度计和陀螺的噪声,和为tk与tk+1时刻轮速里程计在机体系下的位置向量,为t时刻轮速里程计系到机体系的旋转矩阵,为t时刻轮速里程计的标度因数,ns为轮速里程计的量测噪声;为陀螺输出,为加速度计输出,为轮速里程计输出。
进一步的,所述的将状态传播的参考系由世界系转换为tk时刻的机体系,记为bk,形式如下:
其中,
其中,为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量,为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量,为利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量,为利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量,为t时刻的机体相对于bk系的旋转矩阵,为时刻的机体相对于bk系的四元数变化量。
进一步的,采用预积分量测递推方程计算tk+1时刻的机体相对于bk系的惯性/轮速里程计预积分量测;
所述预积分量测递推方程形式如下:
其中,和分别为ti以及ti+1时刻的机体相对于bk系的标称位置变化量,和分别为ti以及ti+1时刻的机体相对于bk系的标称速度变化量,和分别为ti以及ti+1时刻的机体相对于bk系的标称四元数变化量,和分别为ti以及ti+1时刻的机体相对于bk系的位置变化量;和分别为ti以及ti+1时刻的加速度计标称零偏,和分别为ti以及ti+1时刻的陀螺标称零偏,和分别为ti以及ti+1时刻的轮速里程计标称标度因数,和分别为ti以及ti+1时刻的机体相对于bk系的标称旋转矩阵,和分别为ti以及ti+1时刻轮速里程计系到机体系的标称旋转矩阵,和分别为ti以及ti+1时刻轮速里程计在机体系下的位置向量,和分别为ti以及ti+1时刻加速度计输出,和分别为ti以及ti+1时刻陀螺输出,和分别为ti以及ti+1时刻轮速里程计输出,δt为[ti,ti+1]之间的时间间隔。
进一步的,所述误差状态传播方程形式如下:
δzi+1=Fδzi+VN (6)
其中,δzi与δzi+1分别为ti与ti+1时刻的预积分量测误差,F为误差的状态转移矩阵,V为测量噪声系数矩阵,N为量测噪声向量;
和分别为ti与ti+1时刻的位置变化量误差,和分别为ti与ti+1时刻的旋转误差,和分别为ti与ti+1时刻的速度变化量误差,和分别为ti与ti+1时刻的加速度计零偏误差,和分别为ti与ti+1时刻的陀螺零偏误差,和分别为ti与ti+1时刻位置变化量误差,和分别为ti与ti+1时刻的轮速里程计标度因数误差,和分别为ti与ti+1时刻轮速里程计在机体系下的位置误差,和分别为ti与ti+1时刻轮速里程计系与机体系的旋转误差;
误差的状态转移矩阵、测量噪声系数矩阵和量测噪声向量采用如下公式:
其中,状态转移矩阵与测量噪声系数矩阵中的元素采用如下所示:
其中,和分别为ti以及ti+1时刻加速度计的量测噪声,和分别为ti以及ti+1时刻陀螺的量测噪声,和分别为加速度计与陀螺的随机游走噪声,和分别为ti以及ti+1时刻轮速里程计的量测噪声,和分别为轮速里程计标度因数以及安装参数的随机游走噪声。
进一步的,所述雅克比矩阵和协方差矩阵的递推方程,表示为:
雅克比矩阵和协方差矩阵的递推方程为:
其中,Ji和Ji+1分别为ti以及ti+1时刻的雅克比矩阵,Pi和Pi+1分别为ti以及ti+1时刻的协方差矩阵,Q为噪声信号N的协方差矩阵,表示为:
进一步的,步骤3中的目标函数采用如下形式:
待优化变量采用如下形式:
其中,L(·)和R(·)分别代表四元数的左乘与右乘,对于四元数q=[x y z s]=[w s]:
进一步的,所述步骤4具体包括:
有益效果:通过本发明,可以提高惯性/里程计导航与参数估计精度,并解决优化过程中对里程计量测信息的重复积分问题,提高计算效率。
附图说明
图1为本发明方法的流程示意图;
图2为跑车测试轨迹;
图3为采用传统方法、本发明方法时轮速里程计标度因数的估计结果;
图4为采用传统方法、本发明方法时轮速里程计与机体安装角度的估计结果;
图5为采用传统方法、本发明方法时轮速里程计与机体安装横向位置的估计结果
图6为采用传统方法、本发明方法时轮速里程计与机体安装纵向位置的估计结果;
图7为采用传统方法、本发明方法估计结果进行补偿后的轮速里程计水平位置误差。
具体实施方式
现通过参考示范性实施例,本发明的目的和功能以及用于实现这些目的和功能的方法将得以阐明。然而,本发明并不受限于以下所公开的示范性实施例;可以通过不同形式来对其加以实现。说明书的实质仅仅是帮助相关领域技术人员综合理解本发明的具体细节。
结合图1,本发明的优化方法包括以下步骤:
步骤1:周期读取t时刻陀螺输出、加速度计输出、轮速里程计输出;
步骤2:计算惯性/轮速里程计预积分量测、雅克比矩阵及其协方差矩阵:现对该步骤做进一步说明。
S210:状态传播方程采用如下公式:
其中,假设惯性坐标系与机体系重合,记为b;世界坐标系采用东北天坐标系,记为w,和分别为tk与tk+1时刻机体在世界系下的位置向量,和分别为tk与tk+1时刻机体在世界系下的速度向量,和分别为tk与tk+1时刻机体系相对于世界系的四元数,Δtk为[tk,tk+1]之间的时间间隔,为t时刻从机体系到世界系的旋转矩阵,gw为重力向量。和分别为t时刻加速度计与陀螺的零偏,na和nw分别为加速度计和陀螺的噪声,和为tk与tk+1时刻轮速里程计在机体系下的位置向量,为t时刻轮速里程计系到机体系的旋转矩阵,为t时刻轮速里程计的标度因数,ns为轮速里程计的量测噪声,为陀螺输出、为加速度计输出、为轮速里程计输出。
S220:将状态传播的参考系由世界系转换为tk时刻的机体系,记为bk,采用如下公式:
其中,
其中,为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量,为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量,为利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量,为利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量,为t时刻的机体相对于bk系的旋转矩阵,为时刻的机体相对于bk系的四元数变化量
S230:预积分量测递推方程采用如下公式:
其中,和分别为ti以及ti+1时刻的机体相对于bk系的标称位置变化量,和分别为ti以及ti+1时刻的机体相对于bk系的标称速度变化量,和分别为ti以及ti+1时刻的机体相对于bk系的标称四元数变化量,和分别为ti以及ti+1时刻的机体相对于bk系的位置变化量。和分别为ti以及ti+1时刻的加速度计标称零偏,和分别为ti以及ti+1时刻的陀螺标称零偏,和分别为ti以及ti+1时刻的轮速里程计标称标度因数,和分别为ti以及ti+1时刻的机体相对于bk系的标称旋转矩阵,和分别为ti以及ti+1时刻轮速里程计系到机体系的标称旋转矩阵,和分别为ti以及ti+1时刻轮速里程计在机体系下的位置向量,和分别为ti以及ti+1时刻加速度计输出,和分别为ti以及ti+1时刻陀螺输出,和分别为ti以及ti+1时刻轮速里程计输出,δt为[ti,ti+1]之间的时间间隔。
S240:误差状态传播方程采用如下公式:
δzi+1=Fδzi+VN (6)
其中,δzi与δzi+1分别为ti与ti+1时刻的预积分量测误差,F为误差的状态转移矩阵,V为测量噪声系数矩阵,N为量测噪声向量。和分别为ti与ti+1时刻的位置变化量误差,和分别为ti与ti+1时刻的旋转误差,和分别为ti与ti+1时刻的速度变化量误差,和分别为ti与ti+1时刻的加速度计零偏误差,和分别为ti与ti+1时刻的陀螺零偏误差,和分别为ti与ti+1时刻位置变化量误差,和分别为ti与ti+1时刻的轮速里程计标度因数误差,和分别为ti与ti+1时刻轮速里程计在机体系下的位置误差,和分别为ti与ti+1时刻轮速里程计系与机体系的旋转误差。
ti以及ti+1都大于tk且小于tk+1。这里描述的是连续两个惯性量测时刻ti以及ti+1的递推关系,而tk与tk+1之间分布着n个惯性量测,因此需要递推n次才能得到tk与tk+1之间的预积分量测。
误差状态的状态转移矩阵与噪声系数矩阵采用如下公式:
其中,状态转移矩阵与噪声系数阵的元素采用如下所示:
其中,和分别为ti以及ti+1时刻加速度计的量测噪声, 分别为ti以及ti+1时刻陀螺的量测噪声,和分别为加速度计与陀螺的随机游走噪声,和分别为ti以及ti+1时刻轮速里程计的量测噪声,和分别为轮速里程计标度因数以及安装参数的随机游走噪声。
S240:雅克比矩阵和协方差矩阵的递推方程:
雅克比矩阵和协方差矩阵的递推方程为:
其中,Ji和Ji+1分别为ti以及ti+1时刻的雅克比矩阵,Pi和Pi+1分别为ti以及ti+1时刻的协方差矩阵,Q为噪声信号N的协方差矩阵,表示为:
步骤3:构建惯性/轮速里程计融合的目标函数,利用图优化算法对载***置、速度、姿态、陀螺零偏、加速度计零偏、轮速里程计标度因数、轮速里程计与惯性传感器外参进行估计:现对该步骤作出具体说明。
S310:状态向量采用如下形式:
S320:目标函数采用如下形式:
S330:待优化变量采用如下形式:
S340:雅克比矩阵采用如下形式:
其中,L(·)和R(·)分别代表四元数的左乘与右乘,对于四元数q=[x y z s]=[w s]:
步骤4:利用优化结果对预积分量测进行校正。
现采用实际跑车数据测试的形式,对使用本发明方法后的轮速里程计参数估计精度进行验证,其中惯性传感器精度如下:陀螺零偏稳定性为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系的位置变化量利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量和利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量 ;
所述的参考系为世界系的状态传播方程形式如下:
其中,假设惯性坐标系与机体系重合,记为b;世界系记为w,和分别为tk与tk+1时刻机体在世界系下的位置向量,和分别为tk与tk+1时刻机体在世界系下的速度向量,和分别为tk与tk+1时刻机体系相对于世界系的四元数,Δtk为[tk,tk+1]之间的时间间隔,为t时刻从机体系到世界系的旋转矩阵,gw为重力向量;和分别为t时刻加速度计与陀螺的零偏,na和nw分别为加速度计和陀螺的噪声,和为tk与tk+1时刻轮速里程计在机体系下的位置向量,为t时刻轮速里程计系到机体系的旋转矩阵,为t时刻轮速里程计的标度因数,ns为轮速里程计的量测噪声;为陀螺输出,为加速度计输出,为轮速里程计输出;
所述的将状态传播的参考系由世界系转换为tk时刻的机体系,记为bk,形式如下:
其中,
其中,为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的位置变化量,为利用陀螺与加速度计量测得到的tk+1时刻机体相对于bk系的速度变化量,为利用陀螺量测得到的tk+1时刻机体相对于bk系的四元数变化量,为利用陀螺与轮速里程计量测得到的tk+1时刻机体相对于bk系的位置变化量,为t时刻的机体相对于bk系的旋转矩阵,为时刻的机体相对于bk系的四元数变化量;
采用预积分量测递推方程计算tk+1时刻的机体相对于bk系的惯性/轮速里程计预积分量测;
所述预积分量测递推方程形式如下:
其中,和分别为ti以及ti+1时刻的机体相对于bk系的标称位置变化量,和分别为ti以及ti+1时刻的机体相对于bk系的标称速度变化量,和分别为ti以及ti+1时刻的机体相对于bk系的标称四元数变化量,和分别为ti以及ti+1时刻的机体相对于bk系的位置变化量;和分别为ti以及ti+1时刻的加速度计标称零偏,和分别为ti以及ti+1时刻的陀螺标称零偏,和分别为ti以及ti+1时刻的轮速里程计标称标度因数,和分别为ti以及ti+1时刻的机体相对于bk系的标称旋转矩阵,和分别为ti以及ti+1时刻轮速里程计系到机体系的标称旋转矩阵,和分别为ti以及ti+1时刻轮速里程计在机体系下的位置向量,和分别为ti以及ti+1时刻加速度计输出,和分别为ti以及ti+1时刻陀螺输出,和分别为ti以及ti+1时刻轮速里程计输出,δt为[ti,ti+1]之间的时间间隔;
所述惯性/轮速里程计预积分量测误差状态传播方程形式如下:
δzi+1=Fδzi+VN (6)
其中,δzi与δzi+1分别为ti与ti+1时刻的预积分量测误差,F为误差的状态转移矩阵,V为测量噪声系数矩阵,N为量测噪声向量;
和分别为ti与ti+1时刻的位置变化量误差,和分别为ti与ti+1时刻的旋转误差,和分别为ti与ti+1时刻的速度变化量误差,和分别为ti与ti+1时刻的加速度计零偏误差,和分别为ti与ti+1时刻的陀螺零偏误差,和分别为ti与ti+1时刻位置变化量误差,和分别为ti与ti+1时刻的轮速里程计标度因数误差,和分别为ti与ti+1时刻轮速里程计在机体系下的位置误差,和分别为ti与ti+1时刻轮速里程计系与机体系的旋转误差;
误差的状态转移矩阵、测量噪声系数矩阵和量测噪声向量采用如下公式:
其中,状态转移矩阵与测量噪声系数矩阵中的元素采用如下所示:
其中,和分别为ti以及ti+1时刻加速度计的量测噪声,和分别为ti以及ti+1时刻陀螺的量测噪声,和分别为加速度计与陀螺的随机游走噪声,和分别为ti以及ti+1时刻轮速里程计的量测噪声,和分别为轮速里程计标度因数以及安装参数的随机游走噪声;
所述雅克比矩阵和协方差矩阵的递推方程,表示为:
雅克比矩阵和协方差矩阵的递推方程为:
其中,Ji和Ji+1分别为ti以及ti+1时刻的雅克比矩阵,Pi和Pi+1分别为ti以及ti+1时刻的协方差矩阵,Q为噪声信号N的协方差矩阵,表示为:
步骤3中的目标函数采用如下形式:
待优化变量采用如下形式:
其中,L(·)和R(·)分别代表四元数的左乘与右乘,对于四元数q=[x y z s]=[ws]:
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)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113203428A (zh) * | 2021-05-28 | 2021-08-03 | 拉扎斯网络科技(上海)有限公司 | 里程统计装置、基于里程统计装置的数据统计方法及接口 |
Citations (4)
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 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
-
2020
- 2020-12-21 CN CN202011517160.5A patent/CN112697142B/zh active Active
Patent Citations (4)
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)
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 |
---|---|---|
CN103235328B (zh) | 一种gnss与mems组合导航的方法 | |
CN107941217B (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
CN103743395B (zh) | 一种惯性重力匹配组合导航***中时间延迟的补偿方法 | |
CN111238535B (zh) | 一种基于因子图的imu误差在线标定方法 | |
CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
CN110567454B (zh) | 一种复杂环境下sins/dvl紧组合导航方法 | |
CN109870173A (zh) | 一种基于校验点的海底管道惯性导航***的轨迹修正方法 | |
CN109343095B (zh) | 一种车载导航车辆组合定位装置及其组合定位方法 | |
CN107063245B (zh) | 一种基于5阶ssrckf的sins/dvl组合导航滤波方法 | |
CN111323050A (zh) | 一种捷联惯导和多普勒组合***标定方法 | |
CN109612460B (zh) | 一种基于静止修正的垂线偏差测量方法 | |
CN111024074B (zh) | 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 | |
CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及*** | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN110700029B (zh) | 一种轨道平顺性测试方法及*** | |
CN112014599B (zh) | 一种集装箱卡车的轮速计速度修正方法及装置 | |
CN112697142B (zh) | 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 | |
CN110133694A (zh) | 基于双天线gnss航向和轮速辅助的车辆定位方法及*** | |
CN113175933A (zh) | 一种基于高精度惯性预积分的因子图组合导航方法 | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN111912427B (zh) | 一种多普勒雷达辅助捷联惯导运动基座对准方法及*** | |
CN109084755B (zh) | 一种基于重力视速度与参数辨识的加速度计零偏估计方法 | |
CN106918828B (zh) | 一种飞行器自主导航方法及*** | |
CN113008229A (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 |