CN116067370A - 一种imu姿态解算方法及设备、存储介质 - Google Patents
一种imu姿态解算方法及设备、存储介质 Download PDFInfo
- Publication number
- CN116067370A CN116067370A CN202310339625.XA CN202310339625A CN116067370A CN 116067370 A CN116067370 A CN 116067370A CN 202310339625 A CN202310339625 A CN 202310339625A CN 116067370 A CN116067370 A CN 116067370A
- Authority
- CN
- China
- Prior art keywords
- covariance matrix
- error
- measurement
- gyroscope
- noise
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 239000011159 matrix material Substances 0.000 claims abstract description 89
- 238000005259 measurement Methods 0.000 claims abstract description 52
- 238000001914 filtration Methods 0.000 claims abstract description 29
- 239000013598 vector Substances 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 23
- 230000001133 acceleration Effects 0.000 claims abstract description 19
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 230000003044 adaptive effect Effects 0.000 claims abstract description 13
- 230000004927 fusion Effects 0.000 claims abstract description 12
- 238000006243 chemical reaction Methods 0.000 claims abstract description 8
- 230000006870 function Effects 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 238000012360 testing method Methods 0.000 claims description 2
- 230000000694 effects Effects 0.000 description 10
- 238000002474 experimental method Methods 0.000 description 7
- 238000010606 normalization Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 208000000044 Amnesia Diseases 0.000 description 1
- 208000031091 Amnestic disease Diseases 0.000 description 1
- 230000006986 amnesia Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012612 static experiment Methods 0.000 description 1
- 238000006467 substitution reaction 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
-
- 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获取准确的横滚角、俯仰角信息是目前的研究重点。
由于陀螺仪测量的信号中除有效信息外还包含零偏、噪声等干扰，直接积分会使误差累积而导致姿态解算结果无效。此外，加速度计虽然也可以通过加速度的变换获取姿态信息，但是由于外部加速度的影响，加速度计解算的姿态角在高度动态的场景下并不可靠。为了获取准确的姿态信息，需要对陀螺仪和加速度计数据进行融合处理。
中国空气动力研究与发展中心低速空气动力研究所公开了一种基于MEMS惯性传感器的载体动态姿态估计方法（发明专利申请号：201911262998.1，发明名称：基于MEMS惯性传感器的载体动态姿态估计方法），该方法首先通过加速度计和陀螺仪的噪声模型对各自的数据进行处理，然后采用一种阈值方法对加速度计值和陀螺仪值进行判断，最后采用EKF作为融合算法进行数据融合。
以上专利所公开的方式采用拓展卡尔曼滤波（EKF）算法引入了高阶截断误差和雅可比矩阵运算，降低了姿态解算精度，同时缺少对噪声统计特性的实时估计，过程噪声协方差矩阵Q以及测量噪声协方差矩阵R的低估或者高估都可能导致滤波器从最优偏离到次优，此外，在运算过程中各种参数均存在偏差，某些情况下会影响滤波器的性能，传统卡尔曼滤波器的鲁棒性不够。
发明内容
本发明提供了一种IMU姿态解算方法及设备、存储介质，提高姿态解算精度，提高滤波器的鲁棒性。
为解决上述技术问题，本发明第一方面公开了一种IMU姿态解算方法，本方法包括以下步骤：
获取陀螺仪的误差参数与加速度计的误差参数，构建所述陀螺仪与加速度计的误差模型；
基于四元数向量表示导航坐标系与载体坐标系的转换关系，构建加速度输出模型；
根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程；基于所述加速度输出模型构建量测方程；
设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值；
基于容积卡尔曼滤波算法构建滤波器，引入Sage-Husa噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子修正卡尔曼增益，对所述容积卡尔曼滤波器进行更新；
更新下一时刻的状态及测量信息，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息。
在一些实施方式中，所述陀螺仪的误差模型为：
其中，为陀螺仪的主要误差，为随机常值漂移，为陀螺仪测量信号中的白噪声，表示随机常值漂移的导数，为加速度计的主要误差，为加速度计测量信号中的白噪声。
在一些实施方式中，所述基于四元数向量表示导航坐标系与载体坐标系的转换关系，具体为，
定义单位四元数向量，所述导航坐标系系与载体坐标系系之间的坐标变换矩阵表示如下：
=
所述构建加速度输出模型，具体为，
加速度计在载体坐标系系测量的加速度信号可以写作：
其中表示地球引力常数的绝对值，为加速度计的测量噪声，为坐标变换矩阵。
在一些实施方式中，根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程，具体为，
根据陀螺仪的输出角速度和四元数的微分方程：
其中为载体坐标系系相对于导航坐标系系的角速度矩阵，表示如下：
；
定义***状态向量，状态向量维数，从而获取***状态更新的微分方程如下：
其中，陀螺仪角速度估计信号表示为陀螺仪实际测量信号和陀螺仪误差相减；卡尔曼滤波器通常采用离散状态空间模型，采用毕卡逐次逼近法并取一阶近似，同时引入采样时间间隔对***微分方程离散化有：
其中表示k时刻的过程噪声，过程噪声协方差矩阵，其中为统计数学期望；误差协方差矩阵，表示协方差；
基于所述加速度输出模型构建量测方程，具体为，
根据加速度计的输出模型，得到量测方程如下：
，
将量测方程离散化，得到；测量噪声协方差矩阵。
在一些实施方式中，设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值，具体为，
测量若干组IMU静置后所述陀螺仪的输出数据，取所述输出数据的平均值作为状态零偏的估计初值；
通过静态试验获取噪声协方差矩阵初值和以及误差协方差矩阵初值。
在一些实施方式中，对所述容积卡尔曼滤波器进行更新，包括状态更新、测量更新、误差协方差矩阵更新与噪声协方差矩阵更新；
所述状态更新，具体为，
从时刻开始计算的平方根以及容积点：
其中表示容积点集；对于函数，代入容积点，可得估计值；
计算状态预测值以及预测状态协方差:
其中，表示过程噪声协方差矩阵的修正值；
所述测量更新，具体为，
计算的平方根以及的容积点，
传播测量值的容积点，计算测量的估计值：
计算测量误差协方差和互协方差：
表示测量噪声协方差矩阵的修正值；
同时，可获得测量方程的雅克比矩阵：
。
在一些实施方式中，通过一种基于预测残差二段函数的自适应因子修正卡尔曼增益，具体为，
利用自适应因子修正卡尔曼滤波的增益矩阵：
其中，为调节因子通常取；表示预测残差的学习统计量，=表示预测残差，表示预测残差的协方差矩阵，tr(.)表示矩阵的迹；
更新时刻的***状态以及误差协方差矩阵：
；
通过Sage-Husa噪声估计器自适应调整噪声协方差矩阵，具体为，
其中，为遗忘因子，取值范围为。
在一些实施方式中，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息，具体为，
。
根据本发明的第二方面，公开了所述设备包括：存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如上所述的一种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、获取陀螺仪的误差参数与加速度计的误差参数，构建所述陀螺仪与加速度计的误差模型。
陀螺仪的误差参数包括随机常值漂移与测量信号中的白噪声，而加速度计的误差则包括加速度测量信号中的白噪声。基于上述多个误差参数，构建陀螺仪与加速度计的误差模型如下：
其中，为陀螺仪的主要误差，为随机常值漂移，为陀螺仪测量信号中的白噪声，表示随机常值漂移的导数，为加速度计的主要误差，为加速度计测量信号中的白噪声。
步骤S2、基于四元数向量表示导航坐标系与载体坐标系的转换关系，构建加速度输出模型。
定义单位四元数向量为，则导航坐标系n系与载体坐标系b系之间的坐标转换关系可以通过四元数坐标变换矩阵表示如下：
其中，R为临时变量。
加速度在载体坐标系b系中的加速度信号可以写作：
其中，表示地球引力常数的绝对值，在本实施例中，为加速度计的测量噪声。
步骤S3、根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程；基于所述加速度输出模型构建量测方程；
本申请中采用的卡尔曼滤波一般采用状态空间模型，根据陀螺仪输出信号以及误差模型，构建滤波器的状态方程，具体为：
获取陀螺仪的输出角速度与四元数的微分方程：
其中为载体坐标系系相对于导航坐标系系的角速度矩阵，表示如下：
定义***状态向量，状态向量维数，从而获取***状态更新的微分方程如下：
其中，陀螺仪角速度估计信号可表示为陀螺仪实际测量信号和陀螺仪误差相减。卡尔曼滤波器通常采用离散状态空间模型，因此采用毕卡逐次逼近法并取一阶近似，同时引入采样时间间隔对***微分方程离散化有：
其中表示k时刻的过程噪声，过程噪声协方差矩阵，其中为统计数学期望。
基于所述状态方程，得到误差协方差矩阵，表示协方差。
更进一步，基于加速度的输出模型，得到量测方程如下：
；
将量测方程离散化有，进而得到测量噪声协方差矩阵。
步骤S4、设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值。
***状态初值包括四元数向量估计初值和陀螺仪零偏估计初值。陀螺仪零偏估计初值一般是将IMU静置测量多组陀螺仪的输出数据，也就是对三轴陀螺仪测量得到的数据取平均值，作为陀螺仪零偏估计初值。在本实施例中，***状态初值，同时根据静态实验选取噪声协方差矩阵初值和以及误差协方差矩阵初值。在本实例中，过程噪声协方差矩阵初值、测量噪声协方差矩阵初值以及误差协方差矩阵初值可设置为：
其中表示对角矩阵。
步骤S5、基于容积卡尔曼滤波算法构建滤波器，引入Sage-Husa噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子修正卡尔曼增益，对所述容积卡尔曼滤波器进行更新；
本申请中采用容积卡尔曼滤波算法作为基础数据融合算法，避免扩展卡尔曼滤波（EFK）算法引入的高阶截断误差和雅可比矩阵运算，并且对于高维非线性***具有很好的滤波精度以及拓展性，并且更加稳定，也具有更高的姿态解算精度。为使滤波器在各种场景保持最优的滤波效果，引入改进的Sage-Husa噪声估计器自适应调整噪声协方差矩阵。为了提高卡尔曼滤波的鲁棒性，引入一种基于预测残差二段函数的自适应因子对卡尔曼增益进行修正。
容积卡尔曼滤波的更新包括状态更新、测量更新、误差协方差矩阵更新与噪声协方差矩阵更新；
（1）所述状态更新，具体为，
从时刻开始计算的平方根以及容积点：
其中表示容积点集；对于函数，代入容积点，可得估计值；
计算状态预测值以及预测状态协方差：
其中，表示过程噪声协方差矩阵的修正值。
（2）所述测量更新，具体为，
计算的平方根以及的容积点：
传播测量值的容积点，计算测量的估计值：
计算测量误差协方差和互协方差：
表示测量噪声协方差矩阵的修正值；
同时，可获得测量方程的雅克比矩阵：
。
利用自适应因子修正卡尔曼滤波的增益矩阵：
其中，为调节因子通常取，本实施例设置为1。表示预测残差的学习统计量，=表示预测残差，表示预测残差的协方差矩阵，tr(.)表示矩阵的迹。
更新时刻的***状态以及误差协方差矩阵：
。
本发明引入基于预测残差二段函数的自适应因子对卡尔曼增益进行修正，避免复杂环境产生的干扰会降低传统卡尔曼滤波的姿态解算效率，提高了滤波器的鲁棒性，保证了部分场景下姿态解算的有效性。
（3）噪声协方差矩阵更新，具体为：
其中，为遗忘因子通常取，本实例设置为0.99。
本发明对大多数情况下噪声协方差矩阵的先验不可知，引入改进的Sage-Husa噪声估计器，既保证了过程噪声协方差矩阵Q的半正定性以及测量噪声协方差矩阵R的正定性，也尽可能使噪声估计器进行无偏估计，保证了估计精度。
（4）规范化处理
进一步地，滤波过程中计算误差的存在会影响四元数向量的规范性，因此对每一次迭代后的四元数进行规范化处理有：。其中为四元数向量的2-范数，即对四元数向量取模。
步骤S6、更新下一时刻的状态及测量信息，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息。
具体来说，在经过卡尔曼滤波算法一次迭代以后，可以得到更新后的***状态，***状态中包含四元数向量，经过姿态转换可以获取融合滤波后的姿态角，即俯仰角、横滚角信息。
所述四元数向量的姿态解算如下：
。
为验证本申请的有效性，如图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噪声估计器自适应调整噪声协方差矩阵，以及一种基于预测残差二段函数的自适应因子修正卡尔曼增益，对所述容积卡尔曼滤波器进行更新；
更新下一时刻的状态及测量信息，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息。
2.根据权利要求1所述的一种IMU姿态解算方法，其特征在于，所述陀螺仪的误差模型为：
其中，为陀螺仪的主要误差，为随机常值漂移，为陀螺仪测量信号中的白噪声，表示随机常值漂移的导数，为加速度计的主要误差，为加速度计测量信号中的白噪声。
3.根据权利要求2所述的一种IMU姿态解算方法，其特征在于，所述基于四元数向量表示导航坐标系与载体坐标系的转换关系，具体为，
定义单位四元数向量，所述导航坐标系系与载体坐标系系之间的坐标变换矩阵表示如下：
=
所述构建加速度输出模型，具体为，
加速度计在载体坐标系系测量的加速度信号可以写作：
其中表示地球引力常数的绝对值，为加速度计的测量噪声，为坐标变换矩阵。
4.根据权利要求3所述的一种IMU姿态解算方法，其特征在于，根据所述陀螺仪的输出信号与所述误差模型，构建滤波器的状态方程，具体为，
根据陀螺仪的输出角速度和四元数的微分方程：
其中为载体坐标系系相对于导航坐标系系的角速度矩阵，表示如下：
；
定义***状态向量，状态向量维数，从而获取***状态更新的微分方程如下：
其中，陀螺仪角速度估计信号表示为陀螺仪实际测量信号和陀螺仪误差相减；卡尔曼滤波器通常采用离散状态空间模型，采用毕卡逐次逼近法并取一阶近似，同时引入采样时间间隔对***微分方程离散化有：
其中表示k时刻的过程噪声，过程噪声协方差矩阵，其中为统计数学期望；误差协方差矩阵，表示协方差；
基于所述加速度输出模型构建量测方程，具体为，
根据加速度计的输出模型，得到量测方程如下：
，
将量测方程离散化，得到；测量噪声协方差矩阵。
5.根据权利要求1-4任一所述的一种IMU姿态解算方法，其特征在于，设置***状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值，具体为，
测量若干组IMU静置后所述陀螺仪的输出数据，取所述输出数据的平均值作为状态零偏的估计初值；
通过静态试验获取噪声协方差矩阵初值和以及误差协方差矩阵初值。
6.根据权利要求5所述的一种IMU姿态解算方法，其特征在于，对所述容积卡尔曼滤波器进行更新，包括状态更新、测量更新、误差协方差矩阵更新与噪声协方差矩阵更新；
所述状态更新，具体为，
从时刻开始计算的平方根以及容积点：
其中表示容积点集；对于函数，代入容积点，可得估计值；
计算状态预测值以及预测状态协方差:
其中，表示过程噪声协方差矩阵的修正值；
所述测量更新，具体为，
计算的平方根以及的容积点，
传播测量值的容积点，计算测量的估计值：
计算测量误差协方差和互协方差：
表示测量噪声协方差矩阵的修正值；
同时，可获得测量方程的雅克比矩阵：
。
7.根据权利要求6所述的一种IMU姿态解算方法，其特征在于，通过一种基于预测残差二段函数的自适应因子修正卡尔曼增益，具体为，
利用自适应因子修正卡尔曼滤波的增益矩阵：
其中，为调节因子通常取；表示预测残差的学习统计量，=表示预测残差，表示预测残差的协方差矩阵，tr(.)表示矩阵的迹；
更新时刻的***状态以及误差协方差矩阵：
；
通过Sage-Husa噪声估计器自适应调整噪声协方差矩阵，具体为，
其中，为遗忘因子，取值范围为。
8.根据权利要求7所述的一种IMU姿态解算方法，其特征在于，对更新的四元数向量进行姿态解算，得到融合滤波后的俯仰角、横滚角信息，具体为，
。
9.一种IMU姿态解算设备，其特征在于，包括
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如权利要求1-8任一项所述的一种IMU姿态解算方法。
10.一种存储介质，其特征在于，其上存储有计算机程序，所述计算机程序被执行时实现权利要求1~8任一所述一种IMU姿态解算方法。
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 true CN116067370A (zh) | 2023-05-05 |
| CN116067370B 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) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116382321A (zh) * | 2023-06-05 | 2023-07-04 | 小神童创新科技(广州)有限公司 | 一种电动轮椅整机姿态控制***及其方法 |
| CN116608853A (zh) * | 2023-07-21 | 2023-08-18 | 广东智能无人***研究院(南沙) | 载体动态姿态估计方法、设备、存储介质 |
| CN117879540A (zh) * | 2024-03-12 | 2024-04-12 | 西南应用磁学研究所(中国电子科技集团公司第九研究所) | 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法 |
| CN117972637A (zh) * | 2024-03-28 | 2024-05-03 | 天津大学 | 角振动台的角速度数据融合方法和角速度数据融合装置 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040133346A1 (en) * | 2003-01-08 | 2004-07-08 | Bye Charles T. | Attitude change kalman filter measurement apparatus and method |
| CN105973238A (zh) * | 2016-05-09 | 2016-09-28 | 郑州轻工业学院 | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 |
| WO2017083420A1 (en) * | 2015-11-10 | 2017-05-18 | Thales Visionix, Inc. | Robust vision-inertial pedestrian tracking with heading auto-alignment |
| CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
| CN108761512A (zh) * | 2018-07-28 | 2018-11-06 | 南京理工大学 | 一种弹载bds/sins深组合自适应ckf滤波方法 |
| US20190129044A1 (en) * | 2016-07-19 | 2019-05-02 | Southeast University | Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling |
| US20190154449A1 (en) * | 2016-07-22 | 2019-05-23 | Regents Of The University Of Minnesota | Square-root multi-state constraint kalman filter for vision-aided inertial navigation system |
| CN109829938A (zh) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
| CN110455287A (zh) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
| CN110567455A (zh) * | 2019-09-25 | 2019-12-13 | 哈尔滨工程大学 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
| WO2021227012A1 (zh) * | 2020-05-11 | 2021-11-18 | 中国科学院地质与地球物理研究所 | 一种姿态测量方法 |
| US20220404152A1 (en) * | 2020-08-04 | 2022-12-22 | Southeast University | Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering |
-
2023
- 2023-04-03 CN CN202310339625.XA patent/CN116067370B/zh active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040133346A1 (en) * | 2003-01-08 | 2004-07-08 | Bye Charles T. | Attitude change kalman filter measurement apparatus and method |
| WO2017083420A1 (en) * | 2015-11-10 | 2017-05-18 | Thales Visionix, Inc. | Robust vision-inertial pedestrian tracking with heading auto-alignment |
| US20180299271A1 (en) * | 2015-11-10 | 2018-10-18 | Thales Defense & Security Inc. | Robust vision-inertial pedestrian tracking with heading auto-aligment |
| CN105973238A (zh) * | 2016-05-09 | 2016-09-28 | 郑州轻工业学院 | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 |
| US20190129044A1 (en) * | 2016-07-19 | 2019-05-02 | Southeast University | Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling |
| US20190154449A1 (en) * | 2016-07-22 | 2019-05-23 | Regents Of The University Of Minnesota | Square-root multi-state constraint kalman filter for vision-aided inertial navigation system |
| CN106767837A (zh) * | 2017-02-23 | 2017-05-31 | 哈尔滨工业大学 | 基于容积四元数估计的航天器姿态估计方法 |
| CN108761512A (zh) * | 2018-07-28 | 2018-11-06 | 南京理工大学 | 一种弹载bds/sins深组合自适应ckf滤波方法 |
| CN109829938A (zh) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
| CN110455287A (zh) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
| CN110567455A (zh) * | 2019-09-25 | 2019-12-13 | 哈尔滨工程大学 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
| WO2021227012A1 (zh) * | 2020-05-11 | 2021-11-18 | 中国科学院地质与地球物理研究所 | 一种姿态测量方法 |
| US20220404152A1 (en) * | 2020-08-04 | 2022-12-22 | Southeast University | Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116382321A (zh) * | 2023-06-05 | 2023-07-04 | 小神童创新科技(广州)有限公司 | 一种电动轮椅整机姿态控制***及其方法 |
| CN116382321B (zh) * | 2023-06-05 | 2023-08-18 | 小神童创新科技(广州)有限公司 | 一种电动轮椅整机姿态控制***及其方法 |
| CN116608853A (zh) * | 2023-07-21 | 2023-08-18 | 广东智能无人***研究院(南沙) | 载体动态姿态估计方法、设备、存储介质 |
| CN116608853B (zh) * | 2023-07-21 | 2023-09-29 | 广东智能无人***研究院(南沙) | 载体动态姿态估计方法、设备、存储介质 |
| CN117879540A (zh) * | 2024-03-12 | 2024-04-12 | 西南应用磁学研究所(中国电子科技集团公司第九研究所) | 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法 |
| CN117972637A (zh) * | 2024-03-28 | 2024-05-03 | 天津大学 | 角振动台的角速度数据融合方法和角速度数据融合装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116067370B (zh) | 2023-06-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116067370B (zh) | 一种imu姿态解算方法及设备、存储介质 | |
| CN112013836B (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考***算法 | |
| CN104884902B (zh) | 用于三轴磁力计和三轴加速度计的数据融合的方法和装置 | |
| CN111551174A (zh) | 基于多传感器惯性导航***的高动态车辆姿态计算方法及*** | |
| JP6255924B2 (ja) | センサー用ｉｃ、センサーデバイス、電子機器及び移動体 | |
| CN113155129B (zh) | 一种基于扩展卡尔曼滤波的云台姿态估计方法 | |
| CN112798021B (zh) | 基于激光多普勒测速仪的惯导***行进间初始对准方法 | |
| CN106813679B (zh) | 运动物体的姿态估计的方法及装置 | |
| CN110887480A (zh) | 基于mems传感器的飞行姿态估计方法及*** | |
| CN111721288A (zh) | 一种mems器件零偏修正方法、装置及存储介质 | |
| CN110873563B (zh) | 一种云台姿态估计方法及装置 | |
| CN110645976B (zh) | 一种移动机器人的姿态估计方法及终端设备 | |
| CN116608853B (zh) | 载体动态姿态估计方法、设备、存储介质 | |
| CN115855048A (zh) | 一种多传感器融合位姿估计方法 | |
| CN108450007B (zh) | 使用廉价惯性传感器的冗余阵列的高性能惯性测量 | |
| CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
| CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
| CN110595434A (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
| CN106931965B (zh) | 一种确定终端姿态的方法及装置 | |
| CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
| JP2015094631A (ja) | 位置算出装置及び位置算出方法 | |
| CN114636418A (zh) | 一种基于mems测量姿态相对变化的方法、装置及电子设备 | |
| CN113959430B (zh) | 飞行汽车的航姿确定方法、装置、车载终端及存储介质 | |
| CN113137975B (zh) | 天文惯性组合导航的惯性校正方法、装置及电子设备 | |
| CN112859138B (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 |