CN114370870B - 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法 - Google Patents

适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法 Download PDF

Info

Publication number
CN114370870B
CN114370870B CN202210009783.4A CN202210009783A CN114370870B CN 114370870 B CN114370870 B CN 114370870B CN 202210009783 A CN202210009783 A CN 202210009783A CN 114370870 B CN114370870 B CN 114370870B
Authority
CN
China
Prior art keywords
imu
pose
measurement
external
relative
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
CN202210009783.4A
Other languages
English (en)
Other versions
CN114370870A (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.)
China North Computer Application Technology Research Institute
Original Assignee
China North Computer Application Technology Research Institute
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 China North Computer Application Technology Research Institute filed Critical China North Computer Application Technology Research Institute
Priority to CN202210009783.4A priority Critical patent/CN114370870B/zh
Publication of CN114370870A publication Critical patent/CN114370870A/zh
Application granted granted Critical
Publication of CN114370870B publication Critical patent/CN114370870B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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

Abstract

本发明涉及一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，包括：持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿观测结果；选取最新获取的和临近的两帧外部位姿观测结果，按照时间戳最接近再选取出两帧IMU解算结果；根据外部位姿观测结果和IMU解算结果求取相对偏差；相对偏差不超出偏差阈值范围，则将最新获取外部位姿观测结果作为外部观测量，进行卡尔曼滤波器状态向量的更新；利用更新的对应时刻的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。本发明提升位姿测量结果准确性，既保证有效的观测数据及时接入滤波器，又避免不合理的观测数据对滤波器的破坏。

Description

适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
技术领域
本发明涉及位姿测量和卡尔曼滤波技术领域，尤其涉及一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法。
背景技术
由于惯性器件的自身特性，纯惯性位姿测量会随着传感器漂移而产生累积误差。在惯性系中，通过由GNSS、重力与磁强计、视觉、雷达等外部位姿传感器提供位姿观测信息，通过设计卡尔曼滤波器，实时修正IMU零偏以及积分误差，得到准确的测姿结果。而外部位姿传感器提供的外部观测更新数据可能由于环境因素干扰存在异常输出，如果不加甄别直接引入滤波器中可能造成滤波器发散。同时，在恶劣应用环境下，可能无法保证准时获取观测信息，观测信息中断一段时间重新恢复时，需要对观测信息的准确性进行判定。基于上述应用条件，需要建立适当的约束条件以提升位姿测量结果的准确性，即能保证有效的观测数据能够及时接入滤波器，又能避免不合理的观测数据对滤波器的破坏。
发明内容
鉴于上述的分析，本发明旨在提供一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，提升位姿测量结果的准确性。
本发明提供的技术方案是：
本发明公开了一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，包括以下步骤：
在进行位姿测量的过程中，持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿传感器的外部位姿观测结果；
选取最新获取的和与之时间戳临近的共两帧外部位姿观测结果；并按照时间戳信息最接近要求，再选取出两帧对应的IMU解算结果；
根据两帧外部位姿观测结果和对应的两帧IMU解算结果求取外部位姿测量与惯性测量的相对偏差；
判断所述相对偏差是否超出偏差阈值范围；不超出，则将最新获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新；
利用更新的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。
进一步地，所述偏差阈值范围为[-2RMSDk,2RMSDk]；RMSDk为均方根偏差基准；
所述其中，Di，i＝k-n-1,…,k-1，为k时刻前一定时间内，n组有效的相对偏差；
当相对偏差Dk∈[-2RMSDk,2RMSDk]时，则将k时刻获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新，同时，所述Dk计入k时刻的均方根偏差基准的更新；否则，所述外部位姿观测结果作为无效数据丢弃不用，所述Dk不计入均方根偏差基准。
进一步地，所述相对偏差 为根据相邻两帧外部位姿观测结果计算的相对位姿变换矩阵；/>为与所述两帧相邻外部位姿观测结果时刻最接近的两帧IMU解算结果计算的相对位姿变换矩阵。
进一步地，所述位姿测量为基于双IMU差分测量的被测物体相对于运动载体的相对位姿；
所述被测物体和运动载体布置有各自的IMU，用于分别获取被测物体与运动载体的运动信息。
进一步地，卡尔曼滤波器的状态向量X＝[ph-p,vh-p,qh-p,bha,bpa,bhω,bpω]T；
X中ph-p、vh-p、qh-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；bhω、bha为被测物体IMU测量角速度与加速度的零偏；bpω、bpa为运动载体IMU测量角速度与加速度的零偏。
进一步地，建立的卡尔曼滤波的状态误差方程为：
FX为状态转移矩阵；FN为噪声转移矩阵；
为不含误差的状态向量，
ΔX为状态误差向量，
U为IMU测量向量，
U＝[ah,ωh,ap,ωp]T；
ah、ωh、ap、和ωp分别为被测物体IMU加速度与角速度输出、运动载体IMU的加速度与角速度输出；
N为状态噪声向量，
和/>分别为被测物体IMU加速度噪声、运动载体IMU加速度噪声、被测物体IMU角速度噪声、运动载体IMU角速度噪声，被测物体IMU加速度零偏噪声、运动载体IMU加速度零偏噪声、被测物体IMU角速度零偏噪声、运动载体IMU角速度零偏噪声。
进一步地，所述旋转四元数qh-p采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻被测物体与运动载体的相对角速度进行积分得到被测物体与运动载体的相对位姿。
进一步地，所述角速度差分法，包括：
获取被测物体坐标系到运动载体坐标系的相对旋转矩阵；
根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。
进一步地，建立的卡尔曼滤波的观测方程为
其中，误差向量Hp为位置量测矩阵；误差向量/> Hq为旋转量测矩阵；zp、zq为卡尔曼滤波的外部观测量的位置向量和角度向量；/>为卡尔曼滤波估计的位置向量和角度向量。
进一步地，所述外部位姿传感器包括视觉测姿、无线电测姿和电磁测姿的位姿传感器的其中一种或多种。
本发明的有益效果：
本发明的适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，提升位姿测量结果的准确性，既能保证有效的观测数据及时接入滤波器，又能避免不合理的观测数据对滤波器的破坏。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例中的滤波器更新信息筛选方法流程图；
图2为本发明实施例中的位姿变换矩阵获取的示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
本实施例所公开的一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，如图1所示，包括以下步骤：
步骤S1、在进行位姿测量的过程中，持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿传感器的外部位姿观测结果；
步骤S2、选取最新获取的和与之时间戳临近的共两帧外部位姿观测结果；并按照时间戳信息最接近要求，再选取出两帧对应的IMU解算结果；
步骤S3、根据两帧外部位姿观测结果和对应的两帧IMU解算结果求取外部位姿测量与惯性测量的相对偏差；
步骤S4、判断所述相对偏差是否超出偏差阈值范围；不超出，则将最新获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新；
步骤S5、利用更新的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。
具体的，所述步骤S1中，可以通过建立三个存储队列的方式分别存储获取的带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿传感器的外部位姿观测结果；
其中，IMU解算结果为IMU通过卡尔曼滤波的积分过程以较高采样频率获取的，包括解算的位置向量和/或姿态向量，采用矩阵形式Timu表示；
外部位姿观测结果为外部位姿传感器以较低速率获取的，包括与Timu相同维度的位置向量和/或姿态向量，采用矩阵形式Tcam表示。
其中，外部位姿传感器采用没有累积误差的包括视觉测姿、无线电测姿和电磁测姿的位姿传感器的其中一种或多种。外部位姿传感器的优点在于没有累积误差，但可能由于环境因素干扰存在异常输出，如果不加甄别直接引入滤波器中可能造成滤波器发散。
由于IMU解算结果以较高采样频率获取的，并且无需复杂处理，所以当外部位姿观测结果到来时，肯定存在临近于外部位姿观测结果时间戳的IMU解算结果。
这样，在步骤S2中，当最新获取外部位姿观测结果时，再选取与之最临近的一帧外部位姿观测结果，得到两帧相邻的外部位姿观测结果与/>并按照时间戳信息最接近要求，在存储的IMU解算结果选取出两帧对应的IMU解算结果/>与/>如图2所示为位姿变换矩阵获取的示意图。
具体的，在步骤S3中，求取外部位姿测量与惯性测量的相对偏差中，
根据计算相邻两帧外部位姿观测结果的相对位姿变换矩阵
计算与所述两帧相邻外部位姿观测结果时刻最接近的两帧IMU解算结果的相对位姿变换矩阵
在本实施例中，为求取相邻帧时间内视觉测量与惯性测量的相对偏差，定义在李代数/>上的对数映射的二范数Dk为外部位姿测量与惯性测量的相对偏差。
具体的，
在步骤S4中，确定的所述偏差阈值范围为[-2RMSDk,2RMSDk]；RMSDk为均方根偏差基准；
所述其中，Di，i＝k-n-1,…,k-1，为k时刻前一定时间内，n组有效的相对偏差；
当相对偏差Dk∈[-2RMSDk,2RMSDk]时，则将k时刻获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新，同时，所述Dk计入k时刻的均方根偏差基准的更新；否则，所述外部位姿观测结果作为无效数据丢弃不用，所述Dk不计入均方根偏差基准。
具体的，在步骤S4中，得到的外部位姿观测结果是约束判断，将其纳入卡尔曼滤波，作为外部观测量进行该时刻卡尔曼滤波器状态向量的更新，可消除在该外部位姿观测结果观测时刻前的卡尔曼滤波的累积误差。
基于此，得到的该时刻卡尔曼滤波器状态向量可作为后续滤波的初始值，重新对该时刻之后已存储的IMU测量数据进行捷联惯导解算，更新位姿测量结果，以提升位姿测量结果的准确性。从而实现，既能保证有效的观测数据能够及时接入滤波器，又能避免不合理的观测数据对滤波器的破坏。
在本实施例的一个具体方案中，在滤波器更新信息筛选的基础上，采用的位姿测量为基于双IMU差分测量的被测物体相对于运动载体的相对位姿；
所述被测物体和运动载体布置有各自的IMU，用于分别获取被测物体与运动载体的运动信息。
载体IMU与运动载体固连，可在惯性系自由运动，被测物体IMU固连在被测物体上，可以实现与载体的相对运动。
其中，适用于本方案的位姿测量卡尔曼滤波的滤波器的状态向量X＝[ph-p,vh-p,qh-p,bha,bpa,bhω,bpω]T；
X中ph-p、vh-p、qh-p为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；bhω、bha为被测物体IMU测量角速度与加速度的零偏；bpω、bpa为运动载体IMU测量角速度与加速度的零偏。
其中，载体坐标系下被测物体相对载体的位置ph-p、速度vh-p可采用现有的技术方法获得，在此就不赘述。
优选的，所述旋转四元数qh-p采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻被测物体与运动载体的相对角速度进行积分得到被测物体与运动载体的相对位姿。
进一步地，所述角速度差分法，包括：
1)获取被测物体坐标系到运动载体坐标系的相对旋转矩阵Rrel；
被测物体坐标系到运动载体坐标系的相对旋转矩阵Rrel可通过对被测物体和运动载体的标定获得。
2)根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
根据被测物体与运动载体的IMU获取角速度信息ωh(t)、ωp(t)与tk-1时刻相对旋转矩阵计算当前时刻t的相对角增量为：
3)使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。
用当前时刻相对角增量计算相对旋转四元数/>为
相对旋转四元数即为旋转四元数qh-p。
在本方案中，忽略杆臂效应时，真实情况下状态方程可以用下式表示：
式中，为相对旋转矩阵；ah、bha和nha为被测物体IMU加速度、加速度零偏和加速度噪声；ap、bpa、bpa为运动载体IMU加速度、加速度零偏和加速度噪声；ωh、bhω、nhω为被测物体IMU角速度、角速度零偏和角速度噪声，ωp、bpω、npω为运动载体IMU角速度、角速度零偏和角速度噪声；/>为被测物体IMU加速度零偏噪声和角速度零偏噪声；/>为运动载体IMU加速度零偏噪声和角速度零偏噪声。
为不含误差的状态向量；
定义为不含误差的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；/>为不含误差的被测物体IMU测量角速度与加速度的零偏；bpω、bpa为不含误差的运动载体IMU测量角速度与加速度的零偏；
则用于表示真实状态X与不含噪声状态的状态误差向量ΔX为：
其中各项的具体展开如下：
根据建立当前时刻误差状态方程Δx与下一时刻状态误差的递推关系
Δt为当前时刻到下一时刻的时间差。
得到，系统状态误差递推关系即状态误差方程为：
FX为状态转移矩阵；FN为噪声转移矩阵；
U＝[ah,ωh,ap,ωp]T；
ah、ωh、ap、和ωp分别为被测物体IMU加速度与角速度输出、运动载体IMU的加速度与角速度输出；
N为状态噪声向量，
其中，和/>分别为被测物体IMU加速度噪声、运动载体IMU加速度噪声、被测物体IMU角速度噪声、运动载体IMU角速度噪声，被测物体IMU加速度零偏噪声、运动载体IMU加速度零偏噪声、被测物体IMU角速度零偏噪声和运动载体IMU角速度零偏噪声的方差。
进一步地，
所述状态转移矩阵
其中，为不含误差的被测物体到运动载体相对旋转矩阵；
I为单位矩阵；
噪声转移矩阵
则，系统的状态协方差矩阵为
基于上述过程，所述更新卡尔曼滤波的误差状态协方差矩阵的过程，即滤波器的传播过程，包括：
1)获取被测物体、载体的IMU数据；
2)根据被测物体与载体运动模型更新状态向量
3)更新状态转移矩阵FX、更新协方差矩阵FNNFN T；
4)更新误差状态协方差矩阵
所述旋转四元数qh-p采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻被测物体与运动载体的相对角速度进行积分得到被测物体与运动载体的相对位姿。
适用于本方案的位姿测量卡尔曼滤波的滤波器的观测方程为：
其中，误差向量Hp为位置量测矩阵；误差向量/>Hq为旋转量测矩阵；zp、zq为卡尔曼滤波的外部观测量的位置向量和角度向量；/>为卡尔曼滤波估计的位置向量和角度向量。
根据本实施例中判断的新一帧外部位姿测量与惯性测量的相对偏差Dk∈[-2RMSDk,2RMSDk]时，则选择所述外部位姿测量，对卡尔曼滤波器的状态进行更新，其中，
1)列写更新部分位置测量模型zp；
其中，位置测量模型
式中，pc-t代表传感器观测的位移，由视觉测量经过内参变化后得到；ph-p、为被测物体相对载体的平移向量与旋转矩阵；pc-h为传感器相对被测物体的位置观测量，可通过标定得到；np为量测噪声。
则误差向量展开有：
展开后忽略二阶项，得到：
根据观测方程Δzp＝HpΔx，位置量测矩阵Hp写作如下：
式中，pc-h为位置观测量，[pc-h]×为对应叉乘运算矩阵。
2)列写更新部分角度测量模型zq；
其中，角度测量模型
则误差向量展开有
根据观测方程Δzq＝HqΔx，旋转量测矩阵Hq写作如下
本实施例的更新状态协方差矩阵与状态向量的过程包括：
1)计算观测残差
2)计算更新矩阵S＝HPHT+R；
3)计算卡尔曼增益K＝PHTS-1；
4)计算状态修正量
5)计算状态协方差矩阵的递推结果P←(Id-KH)P(Id-KH)T+KRKT。
6)将状态更新量与原本的状态向量/>叠加后得到更新后的状态向量。
综上所述，本实施例的适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，提升位姿测量结果的准确性，即能保证有效的观测数据能够及时接入滤波器，又能避免不合理的观测数据对滤波器的破坏。
特别是应用到双IMU差分的相对姿态测量中，载体IMU的位姿误差与零偏、相对姿态测量的位姿误差与零偏进行修正，得到高精度的被测物体与运动载体的相对位姿；修正惯性测姿误差，具有更高精度。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，其特征在于，包括以下步骤：
在进行位姿测量的过程中，持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿传感器的外部位姿观测结果；
选取最新获取的和与之时间戳临近的共两帧外部位姿观测结果；并按照时间戳信息最接近要求，再选取出两帧对应的IMU解算结果；
根据两帧外部位姿观测结果和对应的两帧IMU解算结果求取外部位姿测量与惯性测量的相对偏差；
判断所述相对偏差是否超出偏差阈值范围；不超出，则将最新获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新；
利用更新的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。
2.根据权利要求1所述的滤波器更新信息筛选方法，其特征在于，所述偏差阈值范围为[-2RMSDk，2RMSDk]；RMSDk为均方根偏差基准；
所述其中，Di，i＝k-n-1，…，k-1，为k时刻前一定时间内，n组有效的相对偏差；
当相对偏差Dk∈[-2RMSDk，2RMSDk]时，则将k时刻获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新，同时，所述Dk计入k时刻的均方根偏差基准的更新；否则，所述外部位姿观测结果作为无效数据丢弃不用，所述Dk不计入均方根偏差基准。
3.根据权利要求2所述的滤波器更新信息筛选方法，其特征在于，所述相对偏差 为根据相邻两帧外部位姿观测结果计算的相对位姿变换矩阵；/>为与所述两帧相邻外部位姿观测结果时刻最接近的两帧IMU解算结果计算的相对位姿变换矩阵。
4.根据权利要求1所述的滤波器更新信息筛选方法，其特征在于，所述位姿测量为基于双IMU差分测量的被测物体相对于运动载体的相对位姿；
所述被测物体和运动载体布置有各自的IMU，用于分别获取被测物体与运动载体的运动信息。
5.根据权利要求4所述的滤波器更新信息筛选方法，其特征在于，卡尔曼滤波器的状态向量X＝[ph-p，vh-p，qh-p，bha，bpa，bhω，bpω]T；
X中ph-p、vh-p、qh-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；bhω、bha为被测物体IMU测量角速度与加速度的零偏；bpω、bpa为运动载体IMU测量角速度与加速度的零偏。
6.根据权利要求5所述的滤波器更新信息筛选方法，其特征在于，建立的卡尔曼滤波的状态误差方程为：
FX为状态转移矩阵；FN为噪声转移矩阵；
为不含误差的状态向量，
ΔX为状态误差向量，
U为IMU测量向量，
U＝[ah，ωh，ap，ωp]T；
ah、ωh、ap、和ωp分别为被测物体IMU加速度与角速度输出、运动载体IMU的加速度与角速度输出；
N为状态噪声向量，
nha、npa、nhω、npω、和/>分别为被测物体IMU加速度噪声、运动载体IMU加速度噪声、被测物体IMU角速度噪声、运动载体IMU角速度噪声，被测物体IMU加速度零偏噪声、运动载体IMU加速度零偏噪声、被测物体IMU角速度零偏噪声、运动载体IMU角速度零偏噪声。
7.根据权利要求6所述的滤波器更新信息筛选方法，其特征在于，
所述旋转四元数qh-p采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻被测物体与运动载体的相对角速度进行积分得到被测物体与运动载体的相对位姿。
8.根据权利要求7所述的滤波器更新信息筛选方法，其特征在于，
所述角速度差分法，包括：
获取被测物体坐标系到运动载体坐标系的相对旋转矩阵；
根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。
9.根据权利要求6所述的滤波器更新信息筛选方法，其特征在于，建立的卡尔曼滤波的观测方程为
其中，误差向量Hp为位置量测矩阵；误差向量/> Hq为旋转量测矩阵；zp、zq为卡尔曼滤波的外部观测量的位置向量和角度向量；/>为卡尔曼滤波估计的位置向量和角度向量。
10.根据权利要求1-9任一项所述的滤波器更新信息筛选方法，其特征在于，
所述外部位姿传感器包括视觉测姿、无线电测姿和电磁测姿的位姿传感器的其中一种或多种。
CN202210009783.4A 2022-01-05 2022-01-05 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法 Active CN114370870B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210009783.4A CN114370870B (zh) 2022-01-05 2022-01-05 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210009783.4A CN114370870B (zh) 2022-01-05 2022-01-05 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法

Publications (2)

Publication Number Publication Date
CN114370870A CN114370870A (zh) 2022-04-19
CN114370870B true CN114370870B (zh) 2024-04-12

Family

ID=81142617

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210009783.4A Active CN114370870B (zh) 2022-01-05 2022-01-05 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法

Country Status (1)

Country Link
CN (1) CN114370870B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1987355A (zh) * 2006-12-22 2007-06-27 北京航空航天大学 一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法
JP2013185898A (ja) * 2012-03-07 2013-09-19 Yamaha Corp 状態推定装置
CN105783940A (zh) * 2016-01-07 2016-07-20 东南大学 基于信息预评判及补偿修正的sins/dvl/es组合导航方法
CN107289932A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
CN111189441A (zh) * 2020-01-10 2020-05-22 山东大学 一种多源自适应容错联邦滤波组合导航系统及导航方法
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN113188540A (zh) * 2021-03-31 2021-07-30 南京航空航天大学 基于恒星数目和构型的惯性/天文自适应滤波方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7193559B2 (en) * 2003-01-21 2007-03-20 Novatel, Inc. Inertial GPS navigation system with modified kalman filter
WO2018086133A1 (en) * 2016-11-14 2018-05-17 SZ DJI Technology Co., Ltd. Methods and systems for selective sensor fusion
CN110873888B (zh) * 2018-09-04 2022-05-06 腾讯大地通途（北京）科技有限公司 定位方法、定位装置、定位设备和计算机存储介质

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1987355A (zh) * 2006-12-22 2007-06-27 北京航空航天大学 一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法
JP2013185898A (ja) * 2012-03-07 2013-09-19 Yamaha Corp 状態推定装置
CN105783940A (zh) * 2016-01-07 2016-07-20 东南大学 基于信息预评判及补偿修正的sins/dvl/es组合导航方法
CN107289932A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
CN111189441A (zh) * 2020-01-10 2020-05-22 山东大学 一种多源自适应容错联邦滤波组合导航系统及导航方法
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN113188540A (zh) * 2021-03-31 2021-07-30 南京航空航天大学 基于恒星数目和构型的惯性/天文自适应滤波方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
舰船综合导航系统的联合卡尔曼滤波器设计;周哲, 洪文学, 庄良杰, 熊正南;船舶工程(第05期);47-50 *
车载移动测量中定位定姿系统误差校正与补偿研究;黎蕾蕾等;《武汉大学学报(信息科学版)》;第41卷(第9期);1245-1252 *

Also Published As

Publication number Publication date
CN114370870A (zh) 2022-04-19

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN109916395B (zh) 一种姿态自主冗余组合导航算法
CN111238535B (zh) 一种基于因子图的imu误差在线标定方法
CN111076722B (zh) 基于自适应的四元数的姿态估计方法及装置
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN112562077B (zh) 一种融合pdr和先验地图的行人室内定位方法
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN114485641A (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN114018254B (zh) 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN114383605A (zh) 基于mems传感器和稀疏地标点的室内定位及优化方法
CN114370870B (zh) 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
CN111998870B (zh) 一种相机惯导系统的标定方法和装置
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法
CN116380070A (zh) 一种基于时间戳优化的视觉惯性定位方法
CN116105772A (zh) 一种激光雷达与imu的标定方法、装置及存储介质
CN113267185B (zh) 抗磁干扰的定位方法及装置、系统、电子设备、存储介质
CN114323011B (zh) 一种适用于相对位姿测量的卡尔曼滤波方法
CN115239758A (zh) 时间戳校正方法、装置、设备、介质及计算机程序产品
CN111307176B (zh) 一种vr头戴显示设备中视觉惯性里程计的在线标定方法
CN113639766B (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