CN116608853B - 载体动态姿态估计方法、设备、存储介质 - Google Patents

载体动态姿态估计方法、设备、存储介质 Download PDF

Info

Publication number
CN116608853B
CN116608853B CN202310902601.0A CN202310902601A CN116608853B CN 116608853 B CN116608853 B CN 116608853B CN 202310902601 A CN202310902601 A CN 202310902601A CN 116608853 B CN116608853 B CN 116608853B
Authority
CN
China
Prior art keywords
matrix
measurement
representing
value
kalman filter
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
CN202310902601.0A
Other languages
English (en)
Other versions
CN116608853A (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 CN202310902601.0A priority Critical patent/CN116608853B/zh
Publication of CN116608853A publication Critical patent/CN116608853A/zh
Application granted granted Critical
Publication of CN116608853B publication Critical patent/CN116608853B/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
    • 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

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

本发明公开载体动态姿态估计方法、设备、存储介质，本方法包括以下步骤：获取陀螺仪和加速度计的测量数据，构建测量误差模型；基于坐标转换矩阵构建惯性坐标系与传感器坐标系的转换关系；基于方向余弦矩阵以及转换关系、测量误差模型确定系统状态方程与量测方程；构建卡尔曼滤波器，确定卡尔曼滤波器初值；基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器；更新下一时刻的状态及测量信息，对更新的状态进行姿态解算，得到融合滤波后的俯仰角、横滚角。本发明通过自适应多重衰落因子对误差协方差的预测值进行修正，使卡尔曼滤波器始终保持良好的性能而不发散，同时提高解算精度。

Description

载体动态姿态估计方法、设备、存储介质
技术领域
本发明涉及导航定位技术领域，尤其涉及载体动态姿态估计方法、设备、存储介质。
背景技术
测量定位技术被广泛应用于机器人控制、航空航天、自动驾驶以及目标跟踪等领域，惯性测量单元（IMU）作为各种测量定位系统的基本组成单元被广泛研究，通常情况下IMU由三轴陀螺仪和三轴加速度计构成。陀螺仪可以通过测量的角速度信息对姿态进行解算，加速度计测量的加速度信号可解算出俯仰、横滚信息进而对姿态角进行修正，偏航角通常由外部传感器如磁力计、全球卫星导航系统进行修正。
但是传统IMU解算方法在动态环境下，系统的状态模型并不能真实反映实际物理过程，并且此时加速度计测量信息中还包含有害加速度的影响，导致状态模型与观测模型之间存在失配，容易使滤波器发散。为了稳定获取载体动态情况下的姿态信息，需要使用改进的卡尔曼滤波算法对陀螺仪和加速度计数据进行融合处理。
发明内容
根据本发明的提供了载体动态姿态估计方法、设备、存储介质，使滤波器始终保持良好的性能，同时提高姿态解算精度。
本发明提供了载体动态姿态估计方法，本方法包括以下步骤：
获取陀螺仪和加速度计的测量数据，构建测量误差模型；
基于坐标转换矩阵构建惯性坐标系与传感器坐标系的转换关系；
基于方向余弦矩阵以及转换关系、测量误差模型确定系统状态方程与量测方程；
构建卡尔曼滤波器，确定卡尔曼滤波器初值；
基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器；
更新下一时刻的状态及测量信息，对更新的状态进行姿态解算，得到融合滤波后的俯仰角、横滚角；
基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器，具体为，
从k时刻计算状态预测值，以及自适应多重衰落因子；
基于自适应多重衰落因子对误差协方差的预测值进行修正；
根据误差协方差矩阵的预测值及量测矩阵计算卡尔曼增益；
根据所述卡尔曼增益更新卡尔曼滤波器的系统状态以及误差协方差矩阵。
在一些实施方式中，还可以包括：基于坐标转换矩阵构建惯性坐标系与传感器坐标系的转换关系，具体为，
惯性坐标系系和传感器坐标系/>系之间的转换关系通过坐标变换矩阵/>表示如下：
其中，是传感器坐标系/>系相对于惯性坐标系/>系的坐标转换矩阵，惯性坐标系/>系相对于传感器坐标系/>系的坐标转换举证/>；/>分别表示横滚角、俯仰角以及偏航角，/>和/>分别代表余弦和正弦三角函数。
在一些实施方式中，还可以包括：所述测量误差模型为：
其中，分别表示陀螺仪和加速度计测量信号；/>为传感器坐标系下理想的角速度、外部加速度以及重力加速度，/>表示陀螺仪随机常值漂移，/>是/>的导数，/>表示陀螺仪测量信号中的白噪声，/>表示加速度计测量信号中的白噪声。
在一些实施方式中，还可以包括：基于方向余弦矩阵以及转换关系、测量误差模型确定系统状态方程与量测方程，具体为，
对方向余弦矩阵进行时间传播，并取一阶近似有：
其中是k时刻的方向余弦矩阵，/>是/>单位矩阵，T是采样时间，/>表示理想角速度的对角矩阵，并被定义如下：
其中、/>和/>分别为k时刻x、y和z轴上理想的角速度；
定义系统的状态向量为，其中/>表示坐标转换矩阵/>的第/>行第/>列，/>分别表示陀螺仪随机常值漂移在/>轴上的分量；
所述系统状态方程为：
其中，表示k时刻的系统状态向量，/>表示系统传递矩阵，/>、/>和/>分别为k时刻陀螺仪在x、y和z轴上测量的角速度，/>表示过程噪声，T为采样时间；
所述量测方程为
其中，是/>时刻的量测向量，/>为k时刻的量测矩阵，/>是k时刻加速度计的量测噪声，/>表示地球引力常数的绝对值。
在一些实施方式中，还可以包括：确定卡尔曼滤波器初值，所述初值包括状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值，具体为，
测量若干组IMU静置后所述加速度计和陀螺仪的输出数据，对加速度计在x、y和z轴上测量的加速度初值信息进行解算确定坐标转换矩阵的估计初值，取陀螺仪在x、y和z轴上测量的角速度以及所述输出数据的平均值作为状态零偏的估计初值，确定系统状态初值/>；
通过静态试验获取噪声协方差矩阵初值和/>以及误差协方差矩阵初值/>。
在一些实施方式中，还可以包括：基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器，具体为，
从时刻计算/>，/>表示k时刻系统状态估计值，；
基于自适应多重衰落因子对误差协方差的预测值/>进行修正有：
自适应多重衰落因子为：
其中表示求取a和b中的较大值，/>表示观测矩阵/>的第/>个参数，表示卡方检验的阈值；/>表示矩阵/>的第/>个对角参数，表示新息矩阵/>的第/>个参数，/>表示矩阵的第/>个对角参数；
计算卡尔曼增益：
更新系统状态和误差协方差矩阵/>:
。
在一些实施方式中，还可以包括：融合滤波后的俯仰角、横滚角，具体为，
。
第二方面，本发明还公开了载体动态姿态估计设备，包括
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如上任一项所述的载体动态姿态估计方法。
第三方面，本发明还公开了存储介质，其上存储有计算机程序，所述计算机程序被执行时实现如上任一项所述载体动态姿态估计方法。
与现有技术相比，本发明的有益效果在于：
本发明提供了载体动态姿态估计方法、设备、存储介质，针对载体动态环境下系统状态模型与量测模型之间会出现失配的问题，通过自适应多重衰落因子对误差协方差的预测值进行修正，使卡尔曼滤波器始终保持良好的性能而不发散，同时提高解算精度。
附图说明
图1为本发明所提供的载体动态姿态估计方法的流程示意图；
图2为本发明所提供的载体动态姿态估计方法的步骤S5的流程示意图；
图3为本发明所提供的载体动态姿态估计方法的机械臂摆动实验姿态实验解算效果对比图；
图4为本发明所提供的载体动态姿态估计方法的加速度较小的随机摆动实验姿态解算效果对比图；
图5为本发明所提供的载体动态姿态估计方法的加速度较大的随机摆动实验姿态解算效果对比图；
图6为本发明所提供的载体动态姿态估计装置的结构示意图。
具体实施方式
为了更好地理解和实施，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或模块的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或模块，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或模块。
如图1所示，本申请提供了载体动态姿态估计方法，通过自适应多重衰落因子对误差协方差的预测值进行修正，使滤波器始终保持良好的性能而不发散，同时提高解算精度。
具体的，本方法包括以下步骤：
步骤S1、获取陀螺仪和加速度计的测量数据，构建测量误差模型；
测量误差模型如下所示：
其中分别为传感器坐标系下理想的角速度、外部加速度以及重力加速度，/>表示陀螺仪随机常值漂移，是/>的导数，/>表示陀螺仪测量信号中的白噪声，/>表示加速度计测量信号中的白噪声。
步骤S2、基于坐标转换矩阵构建惯性坐标系与传感器坐标系的转换关系；
惯性坐标系与传感器坐标系之间的转换可以通过坐标转换矩阵实现，具体通过方向余弦矩阵DCM或者基于四元数向量建立坐标旋转矩阵。通过方向余弦矩阵表示惯性坐标系与传感器坐标系的转换关系为：
其中，是传感器坐标系/>系相对于惯性坐标系/>系的坐标转换矩阵，惯性坐标系/>系相对于传感器坐标系/>系的坐标转换举证/>；/>分别表示横滚角、俯仰角以及偏航角，/>和/>分别代表余弦和正弦三角函数。
步骤S3、基于方向余弦矩阵以及转换关系、测量误差模型确定系统状态方程与量测方程。
对于方向进行时间传播并取一阶近似有：
其中是k时刻的方向余弦矩阵，/>是/>单位矩阵，T是采样时间，/>表示理想角速度的对角矩阵，并被定义如下：
其中、/>和/>分别为k时刻x、y和z轴上理想的角速度。
由于加速度计测量的数据只与坐标转换矩阵的第三行元素有关，因此定义系统的状态向量为，其中/>表示坐标转换矩阵/>的第/>行第/>列，/>分别表示陀螺仪随机常值漂移在/>轴上的分量；
所述系统状态方程为：
其中，表示k时刻的系统状态向量，/>表示系统传递矩阵，/>、/>和/>分别为k时刻陀螺仪在x、y和z轴上测量的角速度，/>表示过程噪声，T为采样时间。
加速度计的测量信息可表示为：
其中表示地球引力常数的绝对值，本实例中/>，/>为加速度计的量测噪声。
对加速度计量测方程进行离散化，整理得到：
其中是/>时刻的量测向量，/>为k时刻的量测矩阵，/>是k时刻的量测噪声，量测噪声协方差矩阵/>。
步骤S4、构建卡尔曼滤波器，确定卡尔曼滤波器初值；
横滚角和俯仰角的初值可以由加速度计测量的信息获取，如下：
其中、/>和/>分别为加速度计在x、y和z轴上测量的加速度，本实例中取初值即可，通过横滚角、俯仰角的初值可以计算得到/>的初值。
将静置测量几组数据，对三轴陀螺仪测量输出数据取平均值作为陀螺仪随机常值漂移的初值。在本实例中，/>的初值分别设置为0.002、-0.002、-0.002，从而可以确定系统状态初值/>。同时根据静态实验选取噪声协方差矩阵初值/>和/>以及误差协方差矩阵初值/>。在本实例中/>以及/>可设置为：
其中表示对角矩阵。
步骤S5、基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器；
采用卡尔曼滤波器对加速度计与陀螺仪的输出数据进行融合。在该过程中，动态环境下系统状态模型与量测模型之间会出现失配的问题，因此，在本申请中引入一种自适应多重衰落因子，基于自适应多重衰落因子构建卡尔曼滤波器，对误差协方差矩阵的预测值进行修正。
如图2所示，具体包括以下步骤：
步骤S51、从k时刻计算状态预测值，以及自适应多重衰落因子；
从时刻计算/>，/>表示k时刻系统状态估计值，
自适应多重衰落因子通过以下公式计算：
其中表示求取a和b中的较大值，/>表示观测矩阵/>的第/>个参数，在本实施例中，/>表示第i个观测量对应的卡方检验阈值，由实验获取，根据定置信水平0.75的卡方分布表可得/>。/>表示矩阵/>的第/>个对角参数，/>表示新息矩阵/>的第/>个参数，/>表示矩阵的第/>个对角参数。
其中新息矩阵的方差/>，因此可以得到卡方分布/>，/>表示具有/>个自由度的卡方分布，/>是可以观察到的状态变量个数。卡方分布用来检验统计样本的实际观测值与理论推断值之间的偏离程度，卡方值越大，二者偏离程度越大。引入测试指标/>：
其中为卡方检验的阈值，可通过实验并结合卡方分布表获取。
步骤S52、基于自适应多重衰落因子对误差协方差的预测值/>进行修正：
步骤S53、根据误差协方差及量测矩阵计算卡尔曼增益；
计算卡尔曼增益：
。
步骤S54、根据所述卡尔曼增益更新卡尔曼滤波器的系统状态以及误差协方差矩阵。
更新系统状态和误差协方差矩阵/>:
。
步骤S6、更新下一时刻的状态及测量信息，对更新的状态进行姿态解算，得到融合滤波后的俯仰角、横滚角。
融合滤波后的俯仰角、横滚角解算如下：
如图3-5所示，为验证本方法的有效性，设置了三组实验进行对比，三组实验分别是：1）机械臂摆动实验；2）随机摆动实验（外部加速度较小的情况）；3）随机摆动实验（外部加速度较大的情况）。每组实验都采用两种姿态解算方法：传统卡尔曼滤波算法（KF），以及本文申请的基于自适应多重衰落因子的卡尔曼滤波算法（AKF），通过对比检验两种算法不同情况下的性能。
如图3所示，机械臂摆动实验中AKF的横滚角、俯仰角解算的均方根误差（RMSE）分别为1.0144°、0.7685°，而KF的姿态解算RMSE分别为1.0365°、0.9454°，AKF解算效果优于KF。
如图4所示，在实验2中AKF的横滚角、俯仰角解算的均方根误差分别为0.8136°、0.6112°，而KF的姿态解算RMSE分别为2.8592°、2.0040°，AKF解算效果优于KF。
如图5所示，在实验3中AKF的横滚角、俯仰角解算的均方根误差分别为2.7420°、2.1923°，而KF的姿态解算RMSE分别为13.2053°、4.8975°，AKF解算效果同样优于KF。以上实验能够有效说明本文申请的方法在各种动态场景下依旧拥有良好的姿态估计精度。
本申请提供了载体动态姿态估计方法，针对载体动态环境下系统状态模型与量测模型之间会出现失配的问题，通过自适应多重衰落因子对误差协方差的预测值进行修正，使卡尔曼滤波器始终保持良好的性能而不发散，同时提高解算精度。
基于相同的发明思想，本申请还提供一种设备，包括：存储有可执行程序代码的存储器61；
与存储器61耦合的处理器62；
用于与其他设备或通信网络通信，接收或者发送网络消息的收发器63；
用于连接存储器61、处理器62、收发器63进行内部通信的总线64。
收发器63接收网络上传输过来的消息，通过总线64传递给处理器62，处理器62通过总线64调用存储器61中存储的可执行程序代码进行处理，并将处理结果通过总线64传递给收发器63发送，从而实现本申请实施例提供的方法。
本申请实施例还提供一种非暂时性机器可读存储介质，所述非暂时性机器可读存储介质上存储有可执行程序，当所述可执行程序被处理器运行时，使所述处理器执行如上述实施例提供的处理方法。
本发明实施例公开了一种计算机可读存储介质，其存储用于电子数据交换的计算机程序，其中，该计算机程序使得计算机执行所描述的一种载体动态姿态估计方法。
本发明实施例公开了一种计算机程序产品，该计算机程序产品包括存储了计算机程序的非瞬时性计算机可读存储介质，且该计算机程序可操作来使计算机执行所描述的一种载体动态姿态估计方法。
以上所描述的实施例仅是示意性的，其中所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施例的具体描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，存储介质包括只读存储器（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 (6)

1.一种载体动态姿态估计方法，其特征在于，本方法包括以下步骤：
获取陀螺仪和加速度计的测量数据，构建测量误差模型；
基于坐标转换矩阵构建惯性坐标系与传感器坐标系的转换关系；
基于方向余弦矩阵以及转换关系、测量误差模型确定系统状态方程与量测方程；
构建卡尔曼滤波器，确定卡尔曼滤波器初值；
基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器；
更新下一时刻的状态及测量信息，对更新的状态进行姿态解算，得到融合滤波后的俯仰角、横滚角；
其中，基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器，具体为，
从k时刻计算状态预测值，以及自适应多重衰落因子；
基于自适应多重衰落因子对误差协方差的预测值进行修正；
根据误差协方差矩阵的预测值及量测矩阵计算卡尔曼增益；
根据所述卡尔曼增益更新卡尔曼滤波器的系统状态以及误差协方差矩阵；
基于坐标转换矩阵构建惯性坐标系与传感器坐标系的转换关系，具体为，
惯性坐标系系和传感器坐标系/>系之间的转换关系通过坐标变换矩阵/>表示如下：
其中，是传感器坐标系/>系相对于惯性坐标系/>系的坐标转换矩阵，惯性坐标系/>系相对于传感器坐标系/>系的坐标转换矩阵/>；/>分别表示横滚角、俯仰角以及偏航角，/>和/>分别代表余弦和正弦三角函数；
基于方向余弦矩阵以及转换关系、测量误差模型确定系统状态方程与量测方程，具体为，
对方向余弦矩阵进行时间传播，并取一阶近似有：
其中是k时刻的方向余弦矩阵，/>是/>单位矩阵，T是采样时间，/>表示理想角速度的对角矩阵，并被定义如下：
其中、/>和/>分别为k时刻x、y和z轴上理想的角速度；
定义系统的状态向量为，其中/>表示坐标转换矩阵/>的第/>行第/>列，/>分别表示陀螺仪随机常值漂移在/>轴上的分量；
所述系统状态方程为：
其中，表示k时刻的系统状态向量，/>表示系统传递矩阵，/>、/>和/>分别为k时刻陀螺仪在x、y和z轴上测量的角速度，/>表示过程噪声，T为采样时间；
所述量测方程为
其中，是/>时刻的量测向量，/>为k时刻的量测矩阵，/>是k时刻加速度计的量测噪声，/>表示地球引力常数的绝对值；
基于自适应多重衰落因子，在线调整误差协方差矩阵的预测值，更新所述卡尔曼滤波器，具体为，
从时刻计算/>，/>表示k时刻系统状态估计值，
基于自适应多重衰落因子对误差协方差的预测值/>进行修正有：
自适应多重衰落因子为：
其中，表示求取a和b中的较大值，/>表示观测矩阵/>的第/>个参数，表示卡方检验的阈值；/>表示矩阵/>的第/>个对角参数，表示新息矩阵/>的第/>个参数，/>表示矩阵的第/>个对角参数；
计算卡尔曼增益：
更新系统状态和误差协方差矩阵/>:
。
2.根据权利要求1所述的载体动态姿态估计方法，其特征在于，所述测量误差模型为：
其中，分别表示陀螺仪和加速度计测量信号；/>为传感器坐标系下理想的角速度、外部加速度以及重力加速度， />表示陀螺仪随机常值漂移， />是/>的导数，表示陀螺仪测量信号中的白噪声，/>表示加速度计测量信号中的白噪声。
3.根据权利要求1或2所述的载体动态姿态估计方法，其特征在于，确定卡尔曼滤波器初值，所述初值包括状态初值、噪声协方差矩阵初值以及误差协方差矩阵初值，具体为，
测量若干组IMU静置后所述加速度计和陀螺仪的输出数据，对加速度计在x、y和z轴上测量的加速度初值信息进行解算，确定坐标转换矩阵的估计初值，取陀螺仪在x、y和z轴上测量的角速度以及所述输出数据的平均值作为状态零偏的估计初值，确定系统状态初值/>；
通过静态试验获取噪声协方差矩阵初值和/>以及误差协方差矩阵初值/>。
4.根据权利要求3所述的载体动态姿态估计方法，其特征在于，融合滤波后的俯仰角、横滚角，具体为，
。
5.载体动态姿态估计设备，其特征在于，包括
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如权利要求1-4任一项所述的载体动态姿态估计方法。
6.存储介质，其特征在于，其上存储有计算机程序，所述计算机程序被执行时实现权利要求1-4任一项所述载体动态姿态估计方法。
CN202310902601.0A 2023-07-21 2023-07-21 载体动态姿态估计方法、设备、存储介质 Active CN116608853B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310902601.0A CN116608853B (zh) 2023-07-21 2023-07-21 载体动态姿态估计方法、设备、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310902601.0A CN116608853B (zh) 2023-07-21 2023-07-21 载体动态姿态估计方法、设备、存储介质

Publications (2)

Publication Number Publication Date
CN116608853A CN116608853A (zh) 2023-08-18
CN116608853B true CN116608853B (zh) 2023-09-29

Family

ID=87685772

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310902601.0A Active CN116608853B (zh) 2023-07-21 2023-07-21 载体动态姿态估计方法、设备、存储介质

Country Status (1)

Country Link
CN (1) CN116608853B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117312778B (zh) * 2023-11-28 2024-01-30 中国航空工业集团公司沈阳空气动力研究所 风洞气动热试验数据滤波处理方法、电子设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012068359A2 (en) * 2010-11-17 2012-05-24 Hillcrest Laboratories, Inc. Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic
WO2019242336A1 (zh) * 2018-06-22 2019-12-26 东南大学 一种水下滑翔器导航定位系统及上浮精度校正方法
CN111175795A (zh) * 2020-01-03 2020-05-19 暨南大学 Gnss/ins组合导航系统的两步抗差滤波方法及系统
CN116067370A (zh) * 2023-04-03 2023-05-05 广东智能无人系统研究院(南沙) 一种imu姿态解算方法及设备、存储介质
CN116147624A (zh) * 2022-12-21 2023-05-23 广东智能无人系统研究院(南沙) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN116164747A (zh) * 2022-12-15 2023-05-26 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012068359A2 (en) * 2010-11-17 2012-05-24 Hillcrest Laboratories, Inc. Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic
WO2019242336A1 (zh) * 2018-06-22 2019-12-26 东南大学 一种水下滑翔器导航定位系统及上浮精度校正方法
CN111175795A (zh) * 2020-01-03 2020-05-19 暨南大学 Gnss/ins组合导航系统的两步抗差滤波方法及系统
CN116164747A (zh) * 2022-12-15 2023-05-26 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统
CN116147624A (zh) * 2022-12-21 2023-05-23 广东智能无人系统研究院(南沙) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN116067370A (zh) * 2023-04-03 2023-05-05 广东智能无人系统研究院(南沙) 一种imu姿态解算方法及设备、存储介质

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An Adaptive Robust Cubature Kalman Filter Based on Sage-Husa Estimator for Improving Ship Heave Measurement Accuracy;Tao Zou等;IEEE;第23卷(第9期);10089-10099 *
一种用于机动目标跟踪的自适应渐消因子卡尔曼滤波;金杨等;长江信息通信(第5期);36-38 *
用于组合测姿的Kalman渐消因子自适应估计算法;苏鑫等;系统仿真学报;第24卷(第8期);1669-1673 *

Also Published As

Publication number Publication date
CN116608853A (zh) 2023-08-18

Similar Documents

Publication Publication Date Title
CN109163721B (zh) 姿态测量方法及终端设备
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN116067370B (zh) 一种imu姿态解算方法及设备、存储介质
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN116608853B (zh) 载体动态姿态估计方法、设备、存储介质
CN108731664B (zh) 机器人状态估计方法、装置、计算机设备和存储介质
CN107228674B (zh) 一种针对星敏感器和陀螺联合滤波的改进方法
CN108318027B (zh) 载体的姿态数据的确定方法和装置
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN113155129B (zh) 一种基于扩展卡尔曼滤波的云台姿态估计方法
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110986928A (zh) 光电吊舱三轴陀螺仪漂移实时修正方法
JP2019120587A (ja) 測位システム及び測位方法
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN116817896A (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN112985391B (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN110375740B (zh) 车辆导航方法、装置、设备和存储介质
CN113375669A (zh) 基于神经网络模型的姿态更新方法及装置
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN111207734A (zh) 一种基于ekf的无人机组合导航方法
CN113447018B (zh) 一种水下惯性导航系统的姿态实时估计方法
KR101140379B1 (ko) 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치
Xuan Application of real-time Kalman filter with magnetic calibration for MEMS sensor in attitude estimation

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