CN116067370B - 一种imu姿态解算方法及设备、存储介质 - Google Patents

一种imu姿态解算方法及设备、存储介质 Download PDF

Info

Publication number
CN116067370B
CN116067370B CN202310339625.XA CN202310339625A CN116067370B CN 116067370 B CN116067370 B CN 116067370B CN 202310339625 A CN202310339625 A CN 202310339625A CN 116067370 B CN116067370 B CN 116067370B
Authority
CN
China
Prior art keywords
covariance matrix
error
measurement
gyroscope
state
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
CN202310339625.XA
Other languages
English (en)
Other versions
CN116067370A (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.)
Guangdong Intelligent Unmanned System Research Institute Nansha
Original Assignee
Guangdong Intelligent Unmanned System Research Institute Nansha
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 Guangdong Intelligent Unmanned System Research Institute Nansha filed Critical Guangdong Intelligent Unmanned System Research Institute Nansha
Priority to CN202310339625.XA priority Critical patent/CN116067370B/zh
Publication of CN116067370A publication Critical patent/CN116067370A/zh
Application granted granted Critical
Publication of CN116067370B publication Critical patent/CN116067370B/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
    • 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/20—Instruments for performing navigational calculations
    • 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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Automation & Control Theory (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种IMU姿态解算方法及设备、存储介质，包括以下步骤：构建陀螺仪与加速度计的误差模型；基于四元数向量表示导航坐标系与载体坐标系的转换关系，构建加速度输出模型；根据陀螺仪的输出信号与误差模型，构建滤波器的状态方程；基于加速度输出模型构建量测方程；设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值；基于容积卡尔曼滤波算法构建滤波器，引入Sage‑Husa噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子，对容积卡尔曼滤波器进行更新；对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息。本发明提高姿态解算精度，提高滤波器的鲁棒性。

Description

一种IMU姿态解算方法及设备、存储介质
技术领域
本发明涉及导航定位技术领域，尤其涉及一种IMU姿态解算方法及设备、存储介质。
背景技术
在机器人控制、航空航天、自动驾驶、目标跟踪等领域中，测量定位一直是举足轻重的研究方向。惯性测量单元（IMU）作为各种测量定位***的基本组成单元被广泛研究，IMU通常由三轴加速度计和三轴陀螺仪构成。通过陀螺仪测量的三轴角速度信息可以解算出IMU的横滚角、俯仰角、偏航角信息，通过加速度计测量的三轴加速度信息可以解算出IMU的横滚角、俯仰角信息。在许多情况下，并不需要估计航向状态（偏航角），因此，利用六轴IMU获取准确的横滚角、俯仰角信息是目前的研究重点。
由于陀螺仪测量的信号中除有效信息外还包含零偏、噪声等干扰，直接积分会使误差累积而导致姿态解算结果无效。此外，加速度计虽然也可以通过加速度的变换获取姿态信息，但是由于外部加速度的影响，加速度计解算的姿态角在高度动态的场景下并不可靠。为了获取准确的姿态信息，需要对陀螺仪和加速度计数据进行融合处理。
中国空气动力研究与发展中心低速空气动力研究所公开了一种基于MEMS惯性传感器的载体动态姿态估计方法（发明专利申请号：201911262998.1，发明名称：基于MEMS惯性传感器的载体动态姿态估计方法），该方法首先通过加速度计和陀螺仪的噪声模型对各自的数据进行处理，然后采用一种阈值方法对加速度计值和陀螺仪值进行判断，最后采用EKF作为融合算法进行数据融合。
以上专利所公开的方式采用拓展卡尔曼滤波（EKF）算法引入了高阶截断误差和雅可比矩阵运算，降低了姿态解算精度，同时缺少对噪声统计特性的实时估计，过程噪声协方差矩阵Q以及测量噪声协方差矩阵R的低估或者高估都可能导致滤波器从最优偏离到次优，此外，在运算过程中各种参数均存在偏差，某些情况下会影响滤波器的性能，传统卡尔曼滤波器的鲁棒性不够。
发明内容
本发明提供了一种IMU姿态解算方法及设备、存储介质，提高姿态解算精度，提高滤波器的鲁棒性。
为解决上述技术问题，本发明第一方面公开了一种IMU姿态解算方法，本方法包括以下步骤：
获取陀螺仪的误差参数与加速度计的误差参数，构建所述陀螺仪与加速度计的误差模型；
基于四元数向量表示导航坐标系与载体坐标系的转换关系，构建加速度输出模型；
根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程；基于所述加速度输出模型构建量测方程；
设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值；
基于容积卡尔曼滤波算法构建滤波器，引入Sage-Husa噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子修正卡尔曼增益，对所述容积卡尔曼滤波器进行更新；
更新下一时刻的状态及测量信息，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息。
在一些实施方式中，所述陀螺仪的误差模型为：
Figure SMS_1
Figure SMS_2
Figure SMS_3
其中，
Figure SMS_4
为陀螺仪的主要误差，/>
Figure SMS_5
为随机常值漂移，/>
Figure SMS_6
为陀螺仪测量信号中的白噪声，/>
Figure SMS_7
表示随机常值漂移/>
Figure SMS_8
的导数，/>
Figure SMS_9
为加速度计的主要误差，/>
Figure SMS_10
为加速度计测量信号中的白噪声。
在一些实施方式中，所述基于四元数向量表示导航坐标系与载体坐标系的转换关系，具体为，
定义单位四元数向量
Figure SMS_11
，所述导航坐标系/>
Figure SMS_12
系与载体坐标系
Figure SMS_13
系之间的坐标变换矩阵/>
Figure SMS_14
表示如下：
Figure SMS_15
=
Figure SMS_16
所述构建加速度输出模型，具体为，
加速度计在载体坐标系
Figure SMS_17
系测量的加速度信号/>
Figure SMS_18
可以写作：
Figure SMS_19
其中
Figure SMS_20
表示地球引力常数的绝对值，/>
Figure SMS_21
为加速度计的测量噪声，/>
Figure SMS_22
为坐标变换矩阵。
在一些实施方式中，根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程，具体为，
根据陀螺仪的输出角速度和四元数的微分方程：
Figure SMS_23
其中
Figure SMS_24
为载体坐标系/>
Figure SMS_25
系相对于导航坐标系/>
Figure SMS_26
系的角速度矩阵，表示如下：
Figure SMS_27
；
定义***状态向量
Figure SMS_28
，状态向量维数/>
Figure SMS_29
，从而获取***状态更新的微分方程如下：
Figure SMS_30
其中，陀螺仪角速度估计信号
Figure SMS_31
表示为陀螺仪实际测量信号/>
Figure SMS_32
和陀螺仪误差/>
Figure SMS_33
相减；卡尔曼滤波器通常采用离散状态空间模型，采用毕卡逐次逼近法并取一阶近似，同时引入采样时间间隔/>
Figure SMS_34
对***微分方程离散化有：
Figure SMS_35
其中
Figure SMS_36
表示k时刻的过程噪声，过程噪声协方差矩阵/>
Figure SMS_37
，其中/>
Figure SMS_38
为统计数学期望；误差协方差矩阵/>
Figure SMS_39
，/>
Figure SMS_40
表示协方差；
基于所述加速度输出模型构建量测方程，具体为，
根据加速度计的输出模型，得到量测方程
Figure SMS_41
如下：
Figure SMS_42
，
将量测方程离散化，得到
Figure SMS_43
；测量噪声协方差矩阵
Figure SMS_44
。
在一些实施方式中，设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值，具体为，
测量若干组IMU静置后所述陀螺仪的输出数据，取所述输出数据的平均值作为状态零偏的估计初值；
通过静态试验获取噪声协方差矩阵初值
Figure SMS_45
和/>
Figure SMS_46
以及误差协方差矩阵初值/>
Figure SMS_47
。
在一些实施方式中，对所述容积卡尔曼滤波器进行更新，包括状态更新、测量更新、误差协方差矩阵更新与噪声协方差矩阵更新；
所述状态更新，具体为，
从
Figure SMS_48
时刻开始计算/>
Figure SMS_49
的平方根/>
Figure SMS_50
以及容积点/>
Figure SMS_51
：
Figure SMS_52
Figure SMS_53
其中
Figure SMS_54
表示容积点集；对于函数/>
Figure SMS_55
，代入容积点，可得估计值/>
Figure SMS_56
；
计算状态预测值
Figure SMS_57
以及预测状态协方差/>
Figure SMS_58
:
Figure SMS_59
Figure SMS_60
其中
Figure SMS_61
，/>
Figure SMS_62
表示过程噪声协方差矩阵的修正值；
所述测量更新，具体为，
计算
Figure SMS_63
的平方根以及/>
Figure SMS_64
的容积点/>
Figure SMS_65
，
Figure SMS_66
Figure SMS_67
传播测量值的容积点
Figure SMS_68
，计算测量的估计值/>
Figure SMS_69
：
Figure SMS_70
Figure SMS_71
计算测量误差协方差和互协方差：
Figure SMS_72
Figure SMS_73
Figure SMS_74
表示测量噪声协方差矩阵的修正值；
同时，可获得测量方程的雅克比矩阵：
Figure SMS_75
。
在一些实施方式中，通过一种基于预测残差二段函数的自适应因子修正卡尔曼增益，具体为，
利用自适应因子
Figure SMS_76
修正卡尔曼滤波的增益矩阵：
Figure SMS_77
其中
Figure SMS_78
，/>
Figure SMS_79
为调节因子通常取/>
Figure SMS_80
；/>
Figure SMS_81
表示预测残差的学习统计量，/>
Figure SMS_82
=/>
Figure SMS_83
表示预测残差，
Figure SMS_84
表示预测残差的协方差矩阵，tr(.)表示矩阵的迹；
更新
Figure SMS_85
时刻的***状态以及误差协方差矩阵：
Figure SMS_86
Figure SMS_87
；
通过Sage-Husa噪声估计器自适应调整噪声协方差矩阵，具体为，
Figure SMS_88
Figure SMS_89
其中
Figure SMS_90
，/>
Figure SMS_91
为遗忘因子，取值范围为/>
Figure SMS_92
。
在一些实施方式中，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息，具体为，
Figure SMS_93
Figure SMS_94
。
根据本发明的第二方面，公开了所述设备包括：存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如上所述的一种IMU姿态解算方法。
根据本发明的第三方面，公开了所述存储介质包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如上任一项所述的一种IMU姿态解算方法。
与现有技术相比，本发明的有益效果在于：
（1）通过利用容积卡尔曼滤波作为基础数据融合算法，避免了传统计算方法中采用拓展卡尔曼滤波引入的高阶截断误差和雅可比矩阵运算，对于高维非线性***具有很好的滤波精度以及拓展性，并且更加稳定。
（2）针对大多数情况下噪声协方差矩阵的先验不可知，通过Sage-Husa噪声估计器自适应调整噪声协方差矩阵，保证了过程噪声协方差矩阵Q的半正定性以及测量噪声协方差矩阵R的正定性，也尽可能使噪声估计器进行无偏估计，保证了估计精度。
（3）针对复杂环境产生的干扰会降低传统卡尔曼滤波的姿态解算效率，引入基于预测残差二段函数的自适应因子对卡尔曼增益进行修正，提高了滤波器的鲁棒性，保证了部分场景下姿态解算的有效性。
附图说明
图1为本发明所提供的一种IMU姿态解算方法的流程示意图；
图2为本发明所提供的一种IMU姿态解算方法的桌面静态实验姿态解算效果对比图；
图3为本发明所提供的一种IMU姿态解算方法的微弱振动实验姿态解算效果对比图；
图4为本发明所提供的一种IMU姿态解算方法的机械臂随机摆动姿态解算效果对比图；
图5为本发明所提供的一种IMU姿态解算装置的结构示意图。
具体实施方式
为了更好地理解和实施，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或模块的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或模块，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或模块。
如图1所示，本申请提供了一种IMU姿态解算方法，通过利用容积卡尔曼滤波作为基础数据融合算法，避免了传统计算方法中采用拓展卡尔曼滤波引入的高阶截断误差和雅可比矩阵运算，对于高维非线性***具有很好的滤波精度以及拓展性，并且更加稳定。针对大多数情况下噪声协方差矩阵的先验不可知，通过Sage-Husa噪声估计器自适应调整噪声协方差矩阵，保证了过程噪声协方差矩阵Q的半正定性以及测量噪声协方差矩阵R的正定性，也尽可能使噪声估计器进行无偏估计，保证了估计精度。针对复杂环境产生的干扰会降低传统卡尔曼滤波的姿态解算效率，引入基于预测残差二段函数的自适应因子对卡尔曼增益进行修正，提高了滤波器的鲁棒性，保证了部分场景下姿态解算的有效性。
具体的，本方法包括以下步骤：
步骤S1、获取陀螺仪的误差参数与加速度计的误差参数，构建所述陀螺仪与加速度计的误差模型。
陀螺仪的误差参数包括随机常值漂移与测量信号中的白噪声，而加速度计的误差则包括加速度测量信号中的白噪声。基于上述多个误差参数，构建陀螺仪与加速度计的误差模型如下：
Figure SMS_95
Figure SMS_96
Figure SMS_97
其中，
Figure SMS_98
为陀螺仪的主要误差，/>
Figure SMS_99
为随机常值漂移，/>
Figure SMS_100
为陀螺仪测量信号中的白噪声，/>
Figure SMS_101
表示随机常值漂移/>
Figure SMS_102
的导数，/>
Figure SMS_103
为加速度计的主要误差，/>
Figure SMS_104
为加速度计测量信号中的白噪声。
步骤S2、基于四元数向量表示导航坐标系与载体坐标系的转换关系，构建加速度输出模型。
定义单位四元数向量为
Figure SMS_105
，则导航坐标系n系与载体坐标系b系之间的坐标转换关系可以通过四元数坐标变换矩阵/>
Figure SMS_106
表示如下：
Figure SMS_107
Figure SMS_108
其中，R为临时变量。
加速度在载体坐标系b系中的加速度信号
Figure SMS_109
可以写作：
Figure SMS_110
其中，
Figure SMS_111
表示地球引力常数的绝对值，在本实施例中/>
Figure SMS_112
，/>
Figure SMS_113
为加速度计的测量噪声。
步骤S3、根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程；基于所述加速度输出模型构建量测方程；
本申请中采用的卡尔曼滤波一般采用状态空间模型，根据陀螺仪输出信号以及误差模型，构建滤波器的状态方程，具体为：
获取陀螺仪的输出角速度与四元数的微分方程：
Figure SMS_114
其中
Figure SMS_115
为载体坐标系/>
Figure SMS_116
系相对于导航坐标系/>
Figure SMS_117
系的角速度矩阵，表示如下：
Figure SMS_118
定义***状态向量
Figure SMS_119
，状态向量维数/>
Figure SMS_120
，从而获取***状态更新的微分方程如下：
Figure SMS_121
其中，陀螺仪角速度估计信号
Figure SMS_122
可表示为陀螺仪实际测量信号
Figure SMS_123
和陀螺仪误差/>
Figure SMS_124
相减。卡尔曼滤波器通常采用离散状态空间模型，因此采用毕卡逐次逼近法并取一阶近似，同时引入采样时间间隔/>
Figure SMS_125
对***微分方程离散化有：
Figure SMS_126
其中
Figure SMS_127
表示k时刻的过程噪声，过程噪声协方差矩阵/>
Figure SMS_128
，其中
Figure SMS_129
为统计数学期望。
基于所述状态方程，得到误差协方差矩阵
Figure SMS_130
，/>
Figure SMS_131
表示协方差。
更进一步，基于加速度的输出模型，得到量测方程
Figure SMS_132
如下：
Figure SMS_133
；
将量测方程离散化有
Figure SMS_134
，进而得到测量噪声协方差矩阵
Figure SMS_135
。
步骤S4、设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值。
***状态初值包括四元数向量估计初值和陀螺仪零偏估计初值。陀螺仪零偏估计初值一般是将IMU静置测量多组陀螺仪的输出数据，也就是对三轴陀螺仪测量得到的数据取平均值，作为陀螺仪零偏估计初值。在本实施例中，***状态初值
Figure SMS_136
，同时根据静态实验选取噪声协方差矩阵初值/>
Figure SMS_137
和/>
Figure SMS_138
以及误差协方差矩阵初值/>
Figure SMS_139
。在本实例中，过程噪声协方差矩阵初值
Figure SMS_140
、测量噪声协方差矩阵初值/>
Figure SMS_141
以及误差协方差矩阵初值/>
Figure SMS_142
可设置为：
Figure SMS_143
Figure SMS_144
Figure SMS_145
其中
Figure SMS_146
表示对角矩阵。
步骤S5、基于容积卡尔曼滤波算法构建滤波器，引入Sage-Husa噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子修正卡尔曼增益，对所述容积卡尔曼滤波器进行更新；
本申请中采用容积卡尔曼滤波算法作为基础数据融合算法，避免扩展卡尔曼滤波（EFK）算法引入的高阶截断误差和雅可比矩阵运算，并且对于高维非线性***具有很好的滤波精度以及拓展性，并且更加稳定，也具有更高的姿态解算精度。为使滤波器在各种场景保持最优的滤波效果，引入改进的Sage-Husa噪声估计器自适应调整噪声协方差矩阵。为了提高卡尔曼滤波的鲁棒性，引入一种基于预测残差二段函数的自适应因子对卡尔曼增益进行修正。
容积卡尔曼滤波的更新包括状态更新、测量更新、误差协方差矩阵更新与噪声协方差矩阵更新；
（1）所述状态更新，具体为，
从
Figure SMS_147
时刻开始计算/>
Figure SMS_148
的平方根/>
Figure SMS_149
以及容积点/>
Figure SMS_150
：
Figure SMS_151
Figure SMS_152
其中
Figure SMS_153
表示容积点集；对于函数/>
Figure SMS_154
，代入容积点，可得估计值/>
Figure SMS_155
；
计算状态预测值
Figure SMS_156
以及预测状态协方差/>
Figure SMS_157
：
Figure SMS_158
Figure SMS_159
其中
Figure SMS_160
，/>
Figure SMS_161
表示过程噪声协方差矩阵的修正值。
（2）所述测量更新，具体为，
计算
Figure SMS_162
的平方根以及/>
Figure SMS_163
的容积点/>
Figure SMS_164
：
Figure SMS_165
Figure SMS_166
传播测量值的容积点
Figure SMS_167
，计算测量的估计值/>
Figure SMS_168
：
Figure SMS_169
Figure SMS_170
计算测量误差协方差和互协方差：
Figure SMS_171
/>
Figure SMS_172
Figure SMS_173
表示测量噪声协方差矩阵的修正值；
同时，可获得测量方程的雅克比矩阵：
Figure SMS_174
。
利用自适应因子
Figure SMS_175
修正卡尔曼滤波的增益矩阵：
Figure SMS_176
其中
Figure SMS_177
，/>
Figure SMS_178
为调节因子通常取/>
Figure SMS_179
，本实施例设置为1。
Figure SMS_180
表示预测残差的学习统计量，/>
Figure SMS_181
=/>
Figure SMS_182
表示预测残差，
Figure SMS_183
表示预测残差的协方差矩阵，tr(.)表示矩阵的迹。
更新
Figure SMS_184
时刻的***状态以及误差协方差矩阵：
Figure SMS_185
Figure SMS_186
。
本发明引入基于预测残差二段函数的自适应因子对卡尔曼增益进行修正，避免复杂环境产生的干扰会降低传统卡尔曼滤波的姿态解算效率，提高了滤波器的鲁棒性，保证了部分场景下姿态解算的有效性。
（3）噪声协方差矩阵更新，具体为：
Figure SMS_187
Figure SMS_188
其中
Figure SMS_189
，/>
Figure SMS_190
为遗忘因子通常取/>
Figure SMS_191
，本实例设置为0.99。
本发明对大多数情况下噪声协方差矩阵的先验不可知，引入改进的Sage-Husa噪声估计器，既保证了过程噪声协方差矩阵Q的半正定性以及测量噪声协方差矩阵R的正定性，也尽可能使噪声估计器进行无偏估计，保证了估计精度。
（4）规范化处理
进一步地，滤波过程中计算误差的存在会影响四元数向量的规范性，因此对每一次迭代后的四元数进行规范化处理有：
Figure SMS_192
。其中/>
Figure SMS_193
为四元数向量的2-范数，即对四元数向量/>
Figure SMS_194
取模。
步骤S6、更新下一时刻的状态及测量信息，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息。
具体来说，在经过卡尔曼滤波算法一次迭代以后，可以得到更新后的***状态，***状态中包含四元数向量，经过姿态转换可以获取融合滤波后的姿态角，即俯仰角、横滚角信息。
所述四元数向量的姿态解算如下：
Figure SMS_195
Figure SMS_196
。
为验证本申请的有效性，如图2、3、4所示，设置了三组对比实验，分别为桌面静态实验、微弱振动实验、机械臂随机摆动实验。每组对比实验均都采用三种姿态解算方法：传统的拓展卡尔曼滤波算法（EKF），传统的容积卡尔曼滤波算法（CKF），以及本文申请的自适应鲁棒容积卡尔曼滤波算法（ARCKF），通过对比检验三种算法不同情况下的性能。
由图2可见，在静态桌面实验中ARCKF俯仰角、横滚角解算精度最高，CKF在横滚角解算中误差较大，解算效果不稳定。经过误差计算，此时ARCKF解算的均方根误差（RMSE）分别为0.015°和0.0274°，解算精度均高于EKF算法和CKF算法。
由图3可见，在微弱振动实验中ARCKF解算效果表现最好，EKF解算效果表现较差。经过误差计算，此时ARCKF解算的RMSE分别为0.1038°和0.0979°，而EKF解算的RMSE分别为0.5383°和0.4678°，CKF滤波效果居中。
由图4可见，在机械臂随机摆动实验中ARCKF滤波效果同样表现最好。经过误差计算，此时的ARCKF解算的RMSE分别为0.4998°和0.7228°，能够有效说明本申请的方法在诸多应用场景具有很好姿态解算效果。
***的处理方法可参照上述方法的描述，在此不进行赘述。
如图5所示，该装置可以包括：存储有可执行程序代码的存储器51；
与存储器51耦合的处理器52；
用于与其他设备或通信网络通信，接收或者发送网络消息的收发器53；
用于连接存储器51、处理器52、收发器53进行内部通信的总线54。
收发器53接收网络上传输过来的消息，通过总线54传递给处理器52，处理器52通过总线54调用存储器51中存储的可执行程序代码进行处理，并将处理结果通过总线54传递给收发器53发送，从而实现本申请实施例提供的方法。
本申请实施例还提供一种非暂时性机器可读存储介质，所述非暂时性机器可读存储介质上存储有可执行程序，当所述可执行程序被处理器运行时，使所述处理器执行如上述实施例提供的处理方法。存储有可执行程序代码的存储器51；
与存储器51耦合的处理器52；
处理器52调用存储器51中存储的可执行程序代码，用于执行所描述的虚拟化核心网的时间敏感实现方法。本发明实施例公开了一种计算机可读存储介质，其存储用于电子数据交换的计算机程序，其中，该计算机程序使得计算机执行所描述的一种IMU姿态解算方法。
本发明实施例公开了一种计算机程序产品，该计算机程序产品包括存储了计算机程序的非瞬时性计算机可读存储介质，且该计算机程序可操作来使计算机执行所描述的一种IMU姿态解算方法。
以上所描述的实施例仅是示意性的，其中所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施例的具体描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，存储介质包括只读存储器（Read-Only Memory，ROM）、随机存储器（Random Access Memory，RAM）、可编程只读存储器（Programmable Read-only Memory，PROM）、可擦除可编程只读存储器（ErasableProgrammable Read Only Memory，EPROM）、一次可编程只读存储器（One-timeProgrammable Read-Only Memory，OTPROM）、电子抹除式可复写只读存储器（Electrically-Erasable Programmable Read-Only Memory，EEPROM）、只读光盘（CompactDisc Read-Only Memory，CD-ROM）或其他光盘存储器、磁盘存储器、磁带存储器，或者能够用于携带或存储数据的计算机可读的任何其他介质。
最后应说明的是：本发明实施例公开的一种虚拟化核心网的时间敏感实现方法及***所揭露的仅为本发明较佳实施例而已，仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解；其依然可以对前述各项实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应的技术方案的本质脱离本发明各项实施例技术方案的精神和范围。

Claims (10)

1.一种IMU姿态解算方法，其特征在于，本方法包括以下步骤：
获取陀螺仪的误差参数与加速度计的误差参数，构建陀螺仪与加速度计的误差模型；
基于四元数向量表示导航坐标系与载体坐标系的转换关系，构建加速度输出模型；
根据陀螺仪的输出信号与误差模型，构建滤波器的状态方程；基于加速度输出模型构建量测方程；
设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值；
基于容积卡尔曼滤波算法构建滤波器，引入Sage-Husa噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子修正卡尔曼增益，对容积卡尔曼滤波器进行更新；
更新下一时刻的状态及测量信息，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息；
其中，通过一种基于预测残差二段函数的自适应因子修正卡尔曼增益，具体为，
利用自适应因子
Figure QLYQS_1
修正卡尔曼滤波的增益矩阵：
Figure QLYQS_2
其中
Figure QLYQS_3
，/>
Figure QLYQS_4
为调节因子通常取/>
Figure QLYQS_5
；/>
Figure QLYQS_6
表示预测残差的学习统计量，/>
Figure QLYQS_7
=/>
Figure QLYQS_8
表示预测残差，
Figure QLYQS_9
表示预测残差的协方差矩阵，tr(.)表示矩阵的迹；
其中，
Figure QLYQS_10
为k+1时刻的互协方差，/>
Figure QLYQS_11
为k+1时刻的测量误差协方差，/>
Figure QLYQS_12
为k+1时刻的测量方程的雅克比矩阵，/>
Figure QLYQS_13
为k+1时刻的测量噪声协方差矩阵修正值，/>
Figure QLYQS_14
为k+1时刻的预测状态协方差。
2.根据权利要求1所述的一种IMU姿态解算方法，其特征在于，陀螺仪的误差模型为：
Figure QLYQS_15
Figure QLYQS_16
Figure QLYQS_17
其中，
Figure QLYQS_18
为陀螺仪的主要误差，/>
Figure QLYQS_19
为随机常值漂移，/>
Figure QLYQS_20
为陀螺仪测量信号中的白噪声，/>
Figure QLYQS_21
表示随机常值漂移/>
Figure QLYQS_22
的导数，/>
Figure QLYQS_23
为加速度计的主要误差，/>
Figure QLYQS_24
为加速度计测量信号中的白噪声。
3.根据权利要求2所述的一种IMU姿态解算方法，其特征在于，基于四元数向量表示导航坐标系与载体坐标系的转换关系，具体为，
定义单位四元数向量
Figure QLYQS_25
，导航坐标系/>
Figure QLYQS_26
系与载体坐标系/>
Figure QLYQS_27
系之间的坐标变换矩阵/>
Figure QLYQS_28
表示如下：
Figure QLYQS_29
=
Figure QLYQS_30
构建加速度输出模型，具体为，
加速度计在载体坐标系
Figure QLYQS_31
系测量的加速度信号/>
Figure QLYQS_32
可以写作：
Figure QLYQS_33
其中
Figure QLYQS_34
表示地球引力常数的绝对值，/>
Figure QLYQS_35
为加速度计的测量噪声，/>
Figure QLYQS_36
为坐标变换矩阵。
4.根据权利要求3所述的一种IMU姿态解算方法，其特征在于，根据陀螺仪的输出信号与误差模型，构建滤波器的状态方程，具体为，
根据陀螺仪的输出角速度和四元数的微分方程：
Figure QLYQS_37
其中
Figure QLYQS_38
为载体坐标系/>
Figure QLYQS_39
系相对于导航坐标系/>
Figure QLYQS_40
系的角速度矩阵，表示如下：
Figure QLYQS_41
；
定义***状态向量
Figure QLYQS_42
，状态向量维数
Figure QLYQS_43
，从而获取***状态更新的微分方程如下：
Figure QLYQS_44
其中，陀螺仪角速度估计信号
Figure QLYQS_45
表示为陀螺仪实际测量信号
Figure QLYQS_46
和陀螺仪误差/>
Figure QLYQS_47
相减；卡尔曼滤波器通常采用离散状态空间模型，采用毕卡逐次逼近法并取一阶近似，同时引入采样时间间隔/>
Figure QLYQS_48
对***微分方程离散化有：
Figure QLYQS_49
其中
Figure QLYQS_50
表示k时刻的过程噪声，过程噪声协方差矩阵/>
Figure QLYQS_51
，其中/>
Figure QLYQS_52
为统计数学期望；误差协方差矩阵/>
Figure QLYQS_53
，/>
Figure QLYQS_54
表示协方差；
基于加速度输出模型构建量测方程，具体为，
根据加速度计的输出模型，得到量测方程
Figure QLYQS_55
如下：
Figure QLYQS_56
，
将量测方程离散化，得到
Figure QLYQS_57
；测量噪声协方差矩阵
Figure QLYQS_58
。
5.根据权利要求4所述的一种IMU姿态解算方法，其特征在于，设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值，具体为，
测量若干组IMU静置后陀螺仪的输出数据，取输出数据的平均值作为状态零偏的估计初值；
通过静态试验获取噪声协方差矩阵初值
Figure QLYQS_59
和/>
Figure QLYQS_60
以及误差协方差矩阵初值/>
Figure QLYQS_61
。
6.根据权利要求5所述的一种IMU姿态解算方法，其特征在于，对容积卡尔曼滤波器进行更新，包括状态更新、测量更新、误差协方差矩阵更新与噪声协方差矩阵更新；
状态更新，具体为，
从
Figure QLYQS_62
时刻开始计算误差协方差矩阵/>
Figure QLYQS_63
的平方根/>
Figure QLYQS_64
以及容积点/>
Figure QLYQS_65
：
Figure QLYQS_66
Figure QLYQS_67
Figure QLYQS_68
其中
Figure QLYQS_69
表示容积点集；对于函数/>
Figure QLYQS_70
，代入容积点，可得估计值/>
Figure QLYQS_71
；
计算状态预测值
Figure QLYQS_72
以及预测状态协方差/>
Figure QLYQS_73
:
Figure QLYQS_74
Figure QLYQS_75
其中
Figure QLYQS_76
，/>
Figure QLYQS_77
表示过程噪声协方差矩阵的修正值；
测量更新，具体为，
计算
Figure QLYQS_78
的平方根以及/>
Figure QLYQS_79
的容积点/>
Figure QLYQS_80
，
Figure QLYQS_81
Figure QLYQS_82
传播测量值的容积点
Figure QLYQS_83
，计算测量的估计值/>
Figure QLYQS_84
：
Figure QLYQS_85
Figure QLYQS_86
计算测量误差协方差和互协方差：
Figure QLYQS_87
Figure QLYQS_88
Figure QLYQS_89
表示测量噪声协方差矩阵的修正值；
同时，可获得测量方程的雅克比矩阵：
Figure QLYQS_90
。
7.根据权利要求6所述的一种IMU姿态解算方法，其特征在于，更新
Figure QLYQS_91
时刻的***状态以及误差协方差矩阵：
Figure QLYQS_92
Figure QLYQS_93
；
Figure QLYQS_94
为k+1时刻的误差协方差矩阵，/>
Figure QLYQS_95
为k+1时刻的预测状态协方差，/>
Figure QLYQS_96
为k+1时刻的状态预测值；
通过Sage-Husa噪声估计器自适应调整噪声协方差矩阵，具体为，
Figure QLYQS_97
Figure QLYQS_98
其中
Figure QLYQS_99
，/>
Figure QLYQS_100
为遗忘因子，取值范围为/>
Figure QLYQS_101
。
8.根据权利要求7所述的一种IMU姿态解算方法，其特征在于，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息，具体为，
Figure QLYQS_102
Figure QLYQS_103
。
9.一种IMU姿态解算设备，其特征在于，包括
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如权利要求1-8任一项所述的一种IMU姿态解算方法。
10.一种存储介质，其特征在于，其上存储有计算机程序，所述计算机程序被执行时实现权利要求1~8任一所述一种IMU姿态解算方法。
CN202310339625.XA 2023-04-03 2023-04-03 一种imu姿态解算方法及设备、存储介质 Active CN116067370B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310339625.XA CN116067370B (zh) 2023-04-03 2023-04-03 一种imu姿态解算方法及设备、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310339625.XA CN116067370B (zh) 2023-04-03 2023-04-03 一种imu姿态解算方法及设备、存储介质

Publications (2)

Publication Number Publication Date
CN116067370A CN116067370A (zh) 2023-05-05
CN116067370B true CN116067370B (zh) 2023-06-27

Family

ID=86180522

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310339625.XA Active CN116067370B (zh) 2023-04-03 2023-04-03 一种imu姿态解算方法及设备、存储介质

Country Status (1)

Country Link
CN (1) CN116067370B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116382321B (zh) * 2023-06-05 2023-08-18 小神童创新科技(广州)有限公司 一种电动轮椅整机姿态控制***及其方法
CN116608853B (zh) * 2023-07-21 2023-09-29 广东智能无人***研究院(南沙) 载体动态姿态估计方法、设备、存储介质
CN117879540B (zh) * 2024-03-12 2024-06-18 西南应用磁学研究所(中国电子科技集团公司第九研究所) 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法
CN117972637B (zh) * 2024-03-28 2024-06-21 天津大学 角振动台的角速度数据融合方法和角速度数据融合装置

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109829938A (zh) * 2019-01-28 2019-05-31 杭州电子科技大学 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6859727B2 (en) * 2003-01-08 2005-02-22 Honeywell International, Inc. Attitude change kalman filter measurement apparatus and method
EP4194811A1 (en) * 2015-11-10 2023-06-14 Thales Defense & Security Inc. Robust vision-inertial pedestrian tracking with heading auto-alignment
CN105973238B (zh) * 2016-05-09 2018-08-17 郑州轻工业学院 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法
CN106291645B (zh) * 2016-07-19 2018-08-21 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
US11466990B2 (en) * 2016-07-22 2022-10-11 Regents Of The University Of Minnesota Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system
CN106767837B (zh) * 2017-02-23 2019-10-22 哈尔滨工业大学 基于容积四元数估计的航天器姿态估计方法
CN108761512A (zh) * 2018-07-28 2018-11-06 南京理工大学 一种弹载bds/sins深组合自适应ckf滤波方法
CN110455287A (zh) * 2019-07-24 2019-11-15 南京理工大学 自适应无迹卡尔曼粒子滤波方法
CN110567455B (zh) * 2019-09-25 2023-01-03 哈尔滨工程大学 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN111878064B (zh) * 2020-05-11 2024-04-05 中国科学院地质与地球物理研究所 一种姿态测量方法
CN112254718B (zh) * 2020-08-04 2024-04-09 东南大学 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109829938A (zh) * 2019-01-28 2019-05-31 杭州电子科技大学 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法

Also Published As

Publication number Publication date
CN116067370A (zh) 2023-05-05

Similar Documents

Publication Publication Date Title
CN116067370B (zh) 一种imu姿态解算方法及设备、存储介质
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考***算法
CN111551174A (zh) 基于多传感器惯性导航***的高动态车辆姿态计算方法及***
US11120562B2 (en) Posture estimation method, posture estimation apparatus and computer readable storage medium
JP6255924B2 (ja) センサー用ｉｃ、センサーデバイス、電子機器及び移動体
CN113155129B (zh) 一种基于扩展卡尔曼滤波的云台姿态估计方法
CN106813679B (zh) 运动物体的姿态估计的方法及装置
CN112798021B (zh) 基于激光多普勒测速仪的惯导***行进间初始对准方法
CN110887480A (zh) 基于mems传感器的飞行姿态估计方法及***
CN111721288A (zh) 一种mems器件零偏修正方法、装置及存储介质
CN110873563B (zh) 一种云台姿态估计方法及装置
CN116608853B (zh) 载体动态姿态估计方法、设备、存储介质
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN108450007B (zh) 使用廉价惯性传感器的冗余阵列的高性能惯性测量
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN110645976A (zh) 一种移动机器人的姿态估计方法及终端设备
JP2019082328A (ja) 位置推定装置
CN113009816A (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN110375773B (zh) Mems惯导***姿态初始化方法
CN106931965B (zh) 一种确定终端姿态的方法及装置
JP2015094631A (ja) 位置算出装置及び位置算出方法
CN114964214B (zh) 一种航姿参考***的扩展卡尔曼滤波姿态解算方法
CN115727871A (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