CN114234970A - 一种运动载体姿态实时测量方法、装置 - Google Patents

一种运动载体姿态实时测量方法、装置 Download PDF

Info

Publication number
CN114234970A
CN114234970A CN202111569311.6A CN202111569311A CN114234970A CN 114234970 A CN114234970 A CN 114234970A CN 202111569311 A CN202111569311 A CN 202111569311A CN 114234970 A CN114234970 A CN 114234970A
Authority
CN
China
Prior art keywords
attitude angle
acceleration
quaternion
value
attitude
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.)
Pending
Application number
CN202111569311.6A
Other languages
English (en)
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.)
South China Agricultural University
Original Assignee
South China Agricultural University
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 South China Agricultural University filed Critical South China Agricultural University
Priority to CN202111569311.6A priority Critical patent/CN114234970A/zh
Publication of CN114234970A publication Critical patent/CN114234970A/zh
Pending legal-status Critical Current

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

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

一种运动载体姿态实时测量方法、装置
技术领域
本申请涉及姿态测量技术领域，特别是涉及一种运动载体姿态实时计算方法、装置。
背景技术
运动载体包括飞行器、机器人、两轮平衡车等技术项目的研究取得了巨大的进步，这些项目能够完成侦察、监控以及危险环境下的搜救等任务。而在这些项目中，每一个都需要对载体或部件的姿态进行实时测量，这是实现姿态精确控制的基础。
传统测量运动载体姿态是采用两台高速摄像机进行测量，不仅设备价格昂贵，而且不能实现实时测量，只能进行后处理，而且操作繁琐，不易快速得到结果；或单独使用陀螺仪完成姿态角测量，误差会随时间积累，产生较大的误差，短时间结果可靠长时间不可靠；或单独使用加速度计完成姿态角测量在非平稳状态下(静止或匀速运动)结果有干扰，长时间结果可靠短时间不可靠。
总之，现有的运动载体姿态测量方式，不能准确的获得实时姿态角。
发明内容
基于此，有必要针对上述技术问题，提供一种能够提高运动载体实时姿态角计算的准确度的运动载体姿态角实时测量方法、装置。
一种运动载体姿态角实时测量方法，所述方法包括：
获取惯性传感器实时采集的运动载体的角速度和加速度；
根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；
分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；
将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
在其中一个实施例中，所述根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值，包括：
将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；
将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量；
将投影值与加速度向量相减，得到k次采样时刻测量误差；
根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值；
其中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件。
在其中一个实施例中，所述将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值，具体为：
采用四元数表示姿态角
Figure BDA0003422845110000021
用矩阵形式表达： qk＝[qk0 qk1 qk2 qk4]T；
其中，qk表示k次采样时刻的四元数，根据四元数qk与姿态矩阵
Figure BDA0003422845110000022
的关系得到姿态矩阵
Figure BDA0003422845110000023
Figure BDA0003422845110000024
设归一化重力加速度矢量为gn=[0 0 1]T，则归一化重力加速度矢量在 k次采样时刻载体坐标系下的投影值为：
Figure BDA0003422845110000031
其中，(gb)k为归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；
所述将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量，具体为；
第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理得到加速度向量
Figure BDA0003422845110000032
其中，
Figure BDA0003422845110000033
为在x轴上归一化了的加速度，
Figure BDA0003422845110000034
为在y轴上归一化了的加速度，
Figure BDA0003422845110000035
为在z轴上归一化了的加速度；
将投影值与加速度向量相减，得到k次采样时刻测量误差，具体为；
通过投影值(gb)k与加速度向量
Figure BDA0003422845110000036
相减，得到k次采样时刻测量误差ek：
Figure BDA0003422845110000037
根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值，具体为：
根据测量误差构建关于姿态角的误差函数
Figure BDA0003422845110000038
Figure BDA0003422845110000039
计算误差函数fg(qk)取最小值时的四元数qk的值，作为在误差函数最小值时的寻优估计值。
在其中一个实施例中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件，具体为：
根据共轭梯度法，更新迭代运动载体的姿态角的四元数：
(qk)h+1＝(qk)h+λhdh其中，qk表示k次采样时刻的四元数，采用四元数表示姿态角
Figure BDA0003422845110000041
用矩阵形式表达：
Figure BDA0003422845110000042
λh为最优步长，dh为搜索方向，(qk)h为迭代h次的四元数，最优步长和搜索方向的计算公式具体为：
Figure BDA0003422845110000043
dh＝-Gh+βhdh-1
定义，
Figure BDA0003422845110000044
其中，JT(qk)为测量误差ek的雅克比矩阵，Gh为测量误差ek在k次采样时刻的四元数qk处的梯度，
Figure BDA0003422845110000045
h为迭代次数，βh为方向调控参数，d1＝-G1，
Figure BDA0003422845110000046
||·||为欧式范数，每次采样运动载体的角速度和加速度后进行一次迭代，修改搜索方向的方向调控参数βh，使得共轭梯度法满足下降条件，修改后的方向调控参数βh为：
Figure BDA0003422845110000047
其中，
Figure BDA0003422845110000048
u为调控参数，u∈(0,1)，即满足
Figure BDA0003422845110000049
使得
Figure BDA00034228451100000410
其中，c为任意数，c的取值范围是(0,1)；其中，根据四元数与姿态角公式，得到的姿态角作为卡尔曼滤波的原始数据输入：
Figure BDA0003422845110000051
其中，φk、θk、ψk分别为k次采样时刻时运动载体的横滚角、俯仰角和航向角。
在其中一个实施例中，所述别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程，具体为：
设计卡尔曼滤波的系统模型的状态变量x，令x为姿态角，即x＝{φ，θ，ψ}T，将系统模型变化成差分方程，公式为：
Figure BDA0003422845110000052
将测量模型变化成差分方程，公式为：
Figure BDA0003422845110000053
其中，zk为k次采样时刻的测量变量，[φk θk ψk]分别表示k次采样时刻时运动载体的横滚角、俯仰角和航向角，[ωkx ωky ωkz]分别是k次采样时刻时陀螺仪固定在运动载体坐标系上采集的x轴、y轴、z轴方向上的角速度数据，ωk为k次采样时刻时引入系统并影响状态变量的噪声，vk为k次采样时刻时传感器测量的噪声，xk是k次采样时刻的状态变量，f(xk)为k次采样时刻的状态转移函数，h(xk)为值状态-测量函数。
在其中一个实施例中，上述运动载体姿态角实时测量方法，还包括：
选择适合当前均值和协方差的sigma点集和权重，根据sigma点集和权重构建样本，获得样本sigma点χi和权重Wi；其中，
Figure BDA0003422845110000061
Figure BDA0003422845110000062
Figure BDA0003422845110000063
Figure BDA0003422845110000064
Figure BDA0003422845110000065
Figure BDA0003422845110000066
其中，κ是任意常数，
Figure BDA0003422845110000067
是k次采样时刻的状态变量预测值，n为状态变量 x的维数，ui是来自于矩阵U的行向量，矩阵U与κ的关系为：
UTU＝(n+κ)Pk-1
其中，n＝3，κ＝0，Pk-1是k-1次采样时刻的误差协方差；
则，状态转移函数y＝f(x)的均值ym为：
Figure BDA0003422845110000068
其中，f(χi)为变量为sigma点的状态转移函数；
状态转移函数y＝f(x)的协方差Py为：
Figure BDA0003422845110000069
其中，ym为函数f(x)的均值，n为状态变量x的维数。
在其中一个实施例中，在寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角之前，包括：
计算残差ηk、残差协方差
Figure BDA0003422845110000074
和马氏距离M2；
判断马氏距离M2与卡方分布值
Figure BDA0003422845110000071
之间的大小；
如果
Figure BDA0003422845110000072
引入增强因子，重新计算残差协方差
Figure BDA0003422845110000075
与传统无迹卡尔曼滤波不同的卡尔曼增益Kk；
如果
Figure BDA0003422845110000073
计算传统无迹卡尔曼滤波的卡尔曼增益Kk。
一种运动载体姿态角实时测量装置，所述装置包括：
初始值获取模块，用于获取惯性传感器实时采集的运动载体的角速度和加速度；
寻优估计值计算模块，用于根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；
递推方程构建模块，用于分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；
姿态角计算模块，用于将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取惯性传感器实时采集的运动载体的角速度和加速度；
根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；
分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；
将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取惯性传感器实时采集的运动载体的角速度和加速度；
根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；
分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；
将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
上述运动载体姿态角实时测量方法、装置、计算机设备和存储介质，通过获得运动载体的角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得降噪后的姿态角；据角速度与姿态角、加速度与姿态角的建立系统模型和观测模型，通过系统模型和观测模型建立无迹卡尔曼滤波递推方程，将共轭梯度法得到的估计值导入无迹卡尔曼滤波算法中，进而获得准确的实时姿态角，本申请所述方法能够提高实时采集的姿态角的准确度。
附图说明
图1为一个实施例中运动载体姿态角实时测量方法的流程示意图；
图2为一个实施例中姿态角的示意图；
图3为一个具体实施例中运动载体姿态角实时测量方法的流程示意图；
图4为一个实施例中运动载体姿态角实时测量装置的结构框图；
图5为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种运动载体姿态角实时测量方法，包括以下步骤：
S110，获取惯性传感器实时采集的运动载体的角速度和加速度。
其中，惯性传感器是一种传感器，主要是检测和测量加速度、倾斜、冲击、振动、旋转和多自由度(DoF)运动。其中，运动载体可为平地机。惯性传感器连续的实时的采集运动载体的角速度和加速度，没采集一次进行下述步骤的处理。
S120，根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值。
S130，分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程。
S140，将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
上述运动载体姿态角实时测量方法中，通过获得运动载体的角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得降噪后的姿态角；据角速度与姿态角、加速度与姿态角的建立系统模型和观测模型，通过系统模型和观测模型建立无迹卡尔曼滤波递推方程，将共轭梯度法得到的估计值导入无迹卡尔曼滤波算法中，进而获得准确的实时姿态角，本申请所述方法能够提高实时采集的姿态角的准确度。
在其中一个实施例中，所述根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值，包括：将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量；将投影值与加速度向量相减，得到k次采样时刻测量误差；根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值；其中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件。
在其中一个实施例中，所述将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值，具体为：
采用四元数表示姿态角
Figure BDA0003422845110000091
用矩阵形式表达： qk＝[qk0 qk1 qk2 qk4]T；
其中，qk表示k次采样时刻的四元数，根据四元数qk与姿态矩阵
Figure BDA0003422845110000092
的关系得到姿态矩阵
Figure BDA0003422845110000093
Figure BDA0003422845110000101
设归一化重力加速度矢量为gn＝[0 0 1]T，则归一化重力加速度矢量在 k次采样时刻载体坐标系下的投影值为：
Figure BDA0003422845110000102
其中，(gb)k为归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；
所述将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量，具体为；
第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理得到加速度向量
Figure BDA0003422845110000103
其中，
Figure BDA0003422845110000104
为在x轴上归一化了的加速度，
Figure BDA0003422845110000105
为在y轴上归一化了的加速度，
Figure BDA0003422845110000106
为在z轴上归一化了的加速度；
将投影值与加速度向量相减，得到k次采样时刻测量误差，具体为；
通过投影值(gb)k与加速度向量
Figure BDA0003422845110000107
相减，得到k次采样时刻测量误差ek：
Figure BDA0003422845110000108
根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值，具体为：
根据测量误差构建关于姿态角的误差函数fg(qk)；
Figure BDA0003422845110000109
计算误差函数fg(qk)取最小值时的四元数qk的值，作为在误差函数最小值时的寻优估计值。
在其中一个实施例中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件，具体为：
根据共轭梯度法，更新迭代运动载体的姿态角的四元数：
(qk)h+1＝(qk)h+λhdh
其中，qk表示k次采样时刻的四元数，采用四元数表示姿态角
Figure BDA0003422845110000111
用矩阵形式表达：qk＝[qk0 qk1 qk2 qk4]T，λh为最优步长，dh为搜索方向，(qk)h为迭代h次的四元数，最优步长和搜索方向的计算公式具体为：
Figure BDA0003422845110000112
dh＝-Gh+βhdh-1
定义，
Figure BDA0003422845110000113
其中，JT(qk)为测量误差ek的雅克比矩阵，Gh为测量误差ek在k次采样时刻的四元数qk处的梯度，
Figure BDA0003422845110000114
Figure BDA0003422845110000115
与Gh是同一概念，h为迭代次数，βh为方向调控参数，d1＝-G1，
Figure BDA0003422845110000116
||·||为欧式范数，每次采样运动载体的角速度和加速度后进行一次迭代，修改搜索方向的方向调控参数βh，使得共轭梯度法满足下降条件，修改后的方向调控参数βh为：
Figure BDA0003422845110000117
其中，
Figure BDA0003422845110000118
此处
Figure BDA0003422845110000119
并无具体的实际含义，其用于表示一个式子的缩写，u为调控参数，u∈(0,1)，即满足
Figure BDA0003422845110000121
使得
Figure BDA0003422845110000122
其中，c为任意数，c的取值范围是(0,1)；其中，根据四元数与姿态角公式，得到的姿态角作为卡尔曼滤波的原始数据输入：
Figure BDA0003422845110000123
其中，φk、θk、ψk分别为k次采样时刻时运动载体的横滚角、俯仰角和航向角；例如，如图2所示，根据飞机的运动方向建立XYZ轴，围绕X轴的旋转角度是横滚角，围绕Y轴的旋转角度是俯仰角，围绕Z轴的旋转角度是航向角。
在其中一个实施例中，所述别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程，具体为：设计卡尔曼滤波的系统模型的状态变量x，令x为姿态角，即 x＝{φ，θ，ψ}T，将系统模型变化成差分方程，公式为：
Figure BDA0003422845110000124
将测量模型变化成差分方程，公式为：
Figure BDA0003422845110000125
其中，zk为k次采样时刻的测量变量，[φk θk ψk]分别表示k次采样时刻时运动载体的横滚角、俯仰角和航向角，[ωkx ωky ωkz]分别是k次采样时刻时陀螺仪固定在运动载体坐标系上采集的x轴、y轴、z轴方向上的角速度数据，ωk为k次采样时刻时引入系统并影响状态变量的噪声，vk为k次采样时刻时传感器测量的噪声，xk是k次采样时刻的状态变量，f(xk)为k次采样时刻的状态转移函数，h(xk)为值状态-测量函数。
其中，系统模型的差分方程为：
xk+1＝f(xk)+wk
测量模型的差分方程为：
zk＝h(xk)+vk
其中，将ωk用Qk表示，vk用Rk表示，Qk是k次采样时刻的wk的n阶协方差矩阵，Rk是k次采样时刻的vk的m阶协方差矩阵，将Qk和Rk设置为：
Figure BDA0003422845110000131
Figure BDA0003422845110000132
在其中一个实施例中，上述运动载体姿态角实时测量方法还包括：
选择适合当前均值和协方差的sigma点集和权重，根据sigma点集和权重构建样本，获得样本sigma点χi和权重Wi；其中，
Figure BDA0003422845110000133
Figure BDA0003422845110000134
Figure BDA0003422845110000135
Figure BDA0003422845110000136
Figure BDA0003422845110000137
Figure BDA0003422845110000138
其中，用sigma点和权重代表状态变量的统计特征，考虑一个状态变量x，它服从均值xm和协方差Px的正态分布，κ是任意常数，
Figure BDA0003422845110000141
是k次采样时刻的状态变量预测值，n为状态变量x的维数，ui是来自于矩阵U的行向量，矩阵U与κ的关系为：
UTU＝(n+κ)Pk-1
其中，矩阵U无具体实际含义，n＝3，κ＝0，Pk-1是k-1次采样时刻的误差协方差；其中，由共轭梯度法得到的估计值
Figure BDA0003422845110000142
表示为：
Figure BDA0003422845110000143
则，状态转移函数y＝f(x)的均值ym为：
Figure BDA0003422845110000144
其中，f(χi)为变量为sigma点的状态转移函数；
状态转移函数y＝f(x)的协方差Py为：
Figure BDA0003422845110000145
其中，ym为函数f(x)的均值，n为状态变量x的维数。
根据状态转移函数y＝f(x)的均值ym公式和状态转移函数y＝f(x)的协方差Py公式获得计算系统模型中f(xk)+wk的状态变量预测值
Figure BDA0003422845110000146
和误差协方差
Figure BDA0003422845110000147
以及测量模型中h(xk)+vk的测量变量预测值
Figure BDA0003422845110000148
和误差协方差Pkz。
具体的，系统模型中f(xk)+wk的状态变量预测值
Figure BDA0003422845110000149
计算公式为：
Figure BDA00034228451100001410
系统模型中f(xk)+wk的误差协方差
Figure BDA00034228451100001411
计算公式为：
Figure BDA0003422845110000151
测量模型中h(xk)+vk的测量变量预测值
Figure BDA0003422845110000152
计算公式为：
Figure BDA0003422845110000153
测量模型中h(xk)+vk的误差协方差Pkz，计算公式为：
Figure BDA0003422845110000154
在其中一个实施例中，在寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角之前，包括：计算残差ηk、残差协方差
Figure BDA00034228451100001511
和马氏距离M2；判断马氏距离M2与卡方分布值
Figure BDA0003422845110000155
之间的大小；如果
Figure BDA0003422845110000156
引入增强因子，重新计算残差协方差
Figure BDA00034228451100001512
与传统无迹卡尔曼滤波不同的卡尔曼增益Kk；如果
Figure BDA0003422845110000157
计算传统无迹卡尔曼滤波的卡尔曼增益Kk。
其中，传统的无迹卡尔曼滤波的卡尔曼增益Kk计算方式如下：
Figure BDA0003422845110000158
Figure BDA0003422845110000159
其中，Pkxz为互协协方差，Pkz为测量模型的误差协方差。
在其中一个实施例中，当运动载体存在怠速振动时，将会存在不良数据导致最后计算得到的姿态角存在很大误差，此处通过引入增强因子，将不良数据去除，具体为：
首先计算无不良值时的残差ηk与残差协方差
Figure BDA00034228451100001513
公式为：
Figure BDA00034228451100001510
Figure BDA0003422845110000161
其中，Rk为协方差差值；然后比较M2与
Figure BDA0003422845110000162
的大小，这里δ的值取0.05，则查表可知
Figure BDA0003422845110000163
若
Figure BDA0003422845110000164
时，系统出现不良值，
Figure BDA00034228451100001614
发生变化，不能用上式计算，此时引入增强因子αk，此时残差协方差
Figure BDA00034228451100001615
计算公式为：
Figure BDA00034228451100001616
卡尔曼增益Kk变为：
Kk＝Pkxz(Pkz+αkRk)-1
代替传统无迹卡尔曼滤波的Kk计算公式；其中，互协协方差Pkxz计算公式为：
Figure BDA0003422845110000165
增强因子αk计算公式为：
Figure BDA0003422845110000166
其中，tr(·)为求迹运算。
在残差ηk服从均值为0、协方差为
Figure BDA00034228451100001613
的高斯分布时，其马氏距离
Figure BDA0003422845110000167
服从卡方分布
Figure BDA0003422845110000168
然而，当系统存在测量不良值时，上述结论不再成立，因此，通过引入增强因子，去除不良数据。可以根据实际情况，设置用于卡方检验理论的一个阈值。设概率
Figure BDA0003422845110000169
如果
Figure BDA00034228451100001610
那么系统就出现了量测粗差，因此就是
Figure BDA00034228451100001611
系统的不良值判据。其中，δ为阈值，
Figure BDA00034228451100001612
为卡方分布值，可由表查到。由假设检验可知，只要引入的增强因子αk的值使得
Figure BDA0003422845110000171
成立，即意味着在δ检验水平下，输出残差M2服从卡方分布，保证了残差ηk的正交性，也就消除了测量不良值对估计结果的影响。
在其中一个实施例中，通过公式
Figure BDA0003422845110000172
和
Figure BDA0003422845110000173
得到测量系统的估计值
Figure BDA0003422845110000174
和误差协方差Pk，计算得到估计和误差协方差用于下一个时刻获得的角速度和加速度的计算。
在其中一个实施例中，惯性传感器与计算机中MATLAB进行通讯，将测得的角速度和加速度发送到MATLAB中，采用上述方法进行处理。
在其中一个实施例中，如图3所示，一种应用在平地机的运动载体姿态角实时测量方法，包括步骤：a1，在获得平地机的姿态角后，四元数qk与姿态矩阵
Figure BDA0003422845110000175
的关系得到姿态矩阵
Figure BDA0003422845110000176
a2，计算重力加速度在载体坐标系下投影和测量误差；a3，更新迭代平地机姿态四元数；a4，将四元数转换成姿态角，进入a10；a5，与前述步骤a1同步进行，计算残差ηk、残差协方差
Figure BDA00034228451100001722
和马氏距离M2；a6，判断马氏距离M2与卡方分布值
Figure BDA0003422845110000177
之间的大小，进入a7或a8；a7，如果
Figure BDA0003422845110000178
引入增强因子，重新计算残差协方差
Figure BDA00034228451100001723
与传统无迹卡尔曼滤波不同的卡尔曼增益Kk，进入a9；a8，如果
Figure BDA0003422845110000179
计算传统无迹卡尔曼滤波的卡尔曼增益Kk；a9，通过公式
Figure BDA00034228451100001710
和
Figure BDA00034228451100001711
得到测量系统的估计
Figure BDA00034228451100001712
和误差协方差Pk，计算得到估计和误差协方差用于下一个时刻获得的角速度和加速度的计算，进入a10；a10，根据sigma点集和权重构建样本，获得样本sigma点χi和权重Wi；a11，计算系统模型状态变量预测值
Figure BDA00034228451100001713
和误差协方差
Figure BDA00034228451100001714
a12，计算测量模型测量变量预测值
Figure BDA00034228451100001715
和误差协方差Pkz。
其中，卡尔曼增益Kk，是一个权重，目的是为了求出状态变量估计值(三个角度)和误差协方差，并用于下一个时刻获得的角速度和加速度；通过下述公式计算测量系统的估计值
Figure BDA00034228451100001716
和误差协方差Pk：
Figure BDA00034228451100001717
Figure BDA00034228451100001718
其中
Figure BDA00034228451100001719
Pk为测量系统的估计值、误差协方差，
Figure BDA00034228451100001720
为状态变量预测值、预测状态变量的误差协方差，zk为k次采样时刻的测量变量，
Figure BDA00034228451100001721
为k此采样时刻的测量变量预测值，Pkz为互协协方差，Kk卡尔曼增益。卡尔曼增益调节测量值与预测值的比例，影响状态变量估计值的大小。
上述运动载体姿态角实时测量方法，只需要惯性传感器、数据线与笔记本电脑就能实现对运动载体的实时测量，操作简单、成本低，本申请通过修改共轭梯度法的满足下降条件，姿态估计收敛速度较快；通过引入增强因子，将获得的不良数据去除，提高滤波性能；本申请实时对运动载体的姿态数据采集和处理，可以消除误差积累与短时间干扰的影响，显著改善姿态角测量的动态特性。
应该理解的是，虽然图1-3的流程图中的各个步骤按照箭头的指示依次显- 示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1-3中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图4所示，提供了一种运动载体姿态角实时测量装置，包括：初始值获取模块210、寻优估计值计算模块220、递推方程构建模块230 和姿态角计算模块240，其中：
初始值获取模块210，用于获取惯性传感器实时采集的运动载体的角速度和加速度。
寻优估计值计算模块220，用于根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值。
递推方程构建模块230，用于分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程。
姿态角计算模块240，用于将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
在其中一个实施例中，所述寻优估计值计算模块220，包括：投影值计算单元，用于将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；加速度向量计算单元，用于将第k次采样时刻的加速度计在x、 y、z轴上采集的加速度进行归一化处理，得到加速度向量；测量误差计算单元，用于将投影值与加速度向量相减，得到k次采样时刻测量误差；寻优估计值计算单元，用于根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值；其中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件。
在其中一个实施例中，所述将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值，具体为：
采用四元数表示姿态角
Figure BDA0003422845110000191
用矩阵形式表达： qk＝[qk0 qk1 qk2 qk4]T；
其中，qk表示k次采样时刻的四元数，根据四元数qk与姿态矩阵
Figure BDA0003422845110000192
的关系得到姿态矩阵
Figure BDA0003422845110000193
Figure BDA0003422845110000194
设归一化重力加速度矢量为gn＝[0 0 1]T，则归一化重力加速度矢量在 k次采样时刻载体坐标系下的投影值为：
Figure BDA0003422845110000195
其中，(gb)k为归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；
所述将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量，具体为；
第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理得到加速度向量
Figure BDA0003422845110000201
其中，
Figure BDA0003422845110000202
为在x轴上归一化了的加速度，
Figure BDA0003422845110000203
为在y轴上归一化了的加速度，
Figure BDA0003422845110000204
为在z轴上归一化了的加速度；
将投影值与加速度向量相减，得到k次采样时刻测量误差，具体为；
通过投影值(gb)k与加速度向量
Figure BDA0003422845110000205
相减，得到k次采样时刻测量误差ek：
Figure BDA0003422845110000206
根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值，具体为：
根据测量误差构建关于姿态角的误差函数fg(qk)；
Figure BDA0003422845110000207
计算误差函数fg(qk)取最小值时的四元数qk的值，作为在误差函数最小值时的寻优估计值。
在其中一个实施例中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件，具体为：
根据共轭梯度法，更新迭代运动载体的姿态角的四元数：
(qk)h+1＝(qk)h+λhdh
其中，qk表示k次采样时刻的四元数，采用四元数表示姿态角
Figure BDA0003422845110000208
用矩阵形式表达：qk＝[qk0 qk1 qk2 qk4]T，λh为最优步长，dh为搜索方向，(qk)h为迭代h次的四元数，最优步长和搜索方向的计算公式具体为：
Figure BDA0003422845110000211
dh＝-Gh+βhdh-1
定义，
Figure BDA0003422845110000212
其中，JT(qk)为测量误差ek的雅克比矩阵，Gh为测量误差ek在k次采样时刻的四元数qk处的梯度，
Figure BDA0003422845110000213
h为迭代次数，βh为方向调控参数，d1＝-G1，
Figure BDA0003422845110000214
||·||为欧式范数，每次采样运动载体的角速度和加速度后进行一次迭代，修改搜索方向的方向调控参数βh，使得共轭梯度法满足下降条件，修改后的方向调控参数βh为：
Figure BDA0003422845110000215
其中，
Figure BDA0003422845110000216
u为调控参数，u∈(0,1)，即满足
Figure BDA0003422845110000217
使得
Figure BDA0003422845110000218
其中，c为任意数，c的取值范围是(0,1)；其中，根据四元数与姿态角公式，得到的姿态角作为卡尔曼滤波的原始数据输入：
Figure BDA0003422845110000219
其中，φk、θk、ψk分别为k次采样时刻时运动载体的横滚角、俯仰角和航向角。
在其中一个实施例中，递推方程构建模块230，具体为：设计卡尔曼滤波的系统模型的状态变量x，令x为姿态角，即x＝{φ，θ，ψ}T，将系统模型变化成差分方程，公式为：
Figure BDA0003422845110000221
将测量模型变化成差分方程，公式为：
Figure BDA0003422845110000222
其中，zk为k次采样时刻的测量变量，[φk θk ψk]分别表示k次采样时刻时运动载体的横滚角、俯仰角和航向角，[ωkx ωky ωkz]分别是k次采样时刻时陀螺仪固定在运动载体坐标系上采集的x轴、y轴、z轴方向上的角速度数据，ωk为k次采样时刻时引入系统并影响状态变量的噪声，vk为k次采样时刻时传感器测量的噪声，xk是k次采样时刻的状态变量，f(xk)为k次采样时刻的状态转移函数，h(xk)为值状态-测量函数。
在其中一个实施例中，所述运动载体姿态角实时测量装置，还包括：点和权重计算模块，用于选择适合当前均值和协方差的sigma点集和权重，根据sigma 点集和权重构建样本，获得样本sigma点χi和权重Wi；其中，
Figure BDA0003422845110000223
Figure BDA0003422845110000224
Figure BDA0003422845110000225
Figure BDA0003422845110000226
Figure BDA0003422845110000231
Figure BDA0003422845110000232
其中，κ是任意常数，
Figure BDA0003422845110000233
是k次采样时刻的状态变量预测值，n为状态变量 x的维数，ui是来自于矩阵U的行向量，矩阵U与κ的关系为：
UTU＝(n+κ)Pk-1
其中，n＝3，κ＝0，Pk-1是k-1次采样时刻的误差协方差；
则，状态转移函数y＝f(x)的均值ym为：
Figure BDA0003422845110000234
其中，f(χi)为变量为sigma点的状态转移函数；
状态转移函数y＝f(x)的协方差Py为：
Figure BDA0003422845110000235
其中，ym为函数f(x)的均值，n为状态变量x的维数。
在其中一个实施例中，所述运动载体姿态角实时测量装置，还包括：卡尔曼增益计算模块，用于计算残差ηk、残差协方差
Figure BDA0003422845110000239
和马氏距离M2；判断马氏距离M2与卡方分布值
Figure BDA0003422845110000236
之间的大小；如果
Figure BDA0003422845110000237
引入增强因子，重新计算残差协方差
Figure BDA00034228451100002310
与传统无迹卡尔曼滤波不同的卡尔曼增益Kk；如果
Figure BDA0003422845110000238
计算传统无迹卡尔曼滤波的卡尔曼增益Kk。
关于运动载体姿态角实时测量装置的具体限定可以参见上文中对于运动载体姿态角实时测量方法的限定，在此不再赘述。上述运动载体姿态角实时测量装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、运营商网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种运动载体姿态角实时测量方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图5中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，还提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各方法实施例中的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-Only Memory，ROM)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(Random Access Memory，RAM)或外部高速缓冲存储器。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器(Static Random Access Memory， SRAM)或动态随机存取存储器(Dynamic Random Access Memory，DRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种运动载体姿态角实时测量方法，其特征在于，所述方法包括：
获取惯性传感器实时采集的运动载体的角速度和加速度；
根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；
分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；
将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
2.根据权利要求1所述的方法，其特征在于，所述根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值，包括：
将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；
将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量；
将投影值与加速度向量相减，得到k次采样时刻测量误差；
根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值；
其中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件。
3.根据权利要求2所述的方法，其特征在于，所述将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值，具体为：
采用四元数表示姿态角
Figure FDA0003422845100000011
用矩阵形式表达：qk＝[qk0qk1 qk2 qk4]T；
其中，qk表示k次采样时刻的四元数，根据四元数qk与姿态矩阵
Figure FDA0003422845100000012
的关系得到姿态矩阵
Figure FDA0003422845100000021
Figure FDA0003422845100000022
设归一化重力加速度矢量为gn＝[0 0 1]T，则归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值为：
Figure FDA0003422845100000023
其中，(gb)k为归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；
所述将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量，具体为；
第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理得到加速度向量
Figure FDA0003422845100000024
其中，
Figure FDA0003422845100000025
为在x轴上归一化了的加速度，
Figure FDA0003422845100000026
为在y轴上归一化了的加速度，
Figure FDA0003422845100000027
为在z轴上归一化了的加速度；
将投影值与加速度向量相减，得到k次采样时刻测量误差，具体为；
通过投影值(gb)k与加速度向量
Figure FDA0003422845100000028
相减，得到k次采样时刻测量误差ek：
Figure FDA0003422845100000029
根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值，具体为：
根据测量误差构建关于姿态角的误差函数fg(qk)；
Figure FDA00034228451000000210
计算误差函数fg(qk)取最小值时的四元数qk的值，作为在误差函数最小值时的寻优估计值。
4.根据权利要求2所述的方法，其特征在于，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件，具体为：
根据共轭梯度法，更新迭代运动载体的姿态角的四元数：
(qk)h+1＝(qk)h+λhdh
其中，qk表示k次采样时刻的四元数，采用四元数表示姿态角
Figure FDA0003422845100000031
用矩阵形式表达：qk＝[qk0 qk1 qk2 qk4]T，λh为最优步长，dh为搜索方向，(qk)h为迭代h次的四元数，最优步长和搜索方向的计算公式具体为：
Figure FDA0003422845100000032
dh＝-Gh+βhdh-1
定义，
Figure FDA0003422845100000033
其中，JT(qk)为测量误差ek的雅克比矩阵，Gh为测量误差ek在k次采样时刻的四元数qk处的梯度，
Figure FDA0003422845100000034
h为迭代次数，βh为方向调控参数，d1＝-G1，
Figure FDA0003422845100000035
||·||为欧式范数，每次采样运动载体的角速度和加速度后进行一次迭代，修改搜索方向的方向调控参数βh，使得共轭梯度法满足下降条件，修改后的方向调控参数βh为：
Figure FDA0003422845100000036
其中，
Figure FDA0003422845100000041
u为调控参数，u∈(0,1)，即满足
Figure FDA0003422845100000042
使得
Figure FDA0003422845100000043
其中，c为任意数，c的取值范围是(0,1)；其中，根据四元数与姿态角公式，得到的姿态角作为卡尔曼滤波的原始数据输入：
Figure FDA0003422845100000044
其中，φk、θk、ψk分别为k次采样时刻时运动载体的横滚角、俯仰角和航向角。
5.根据权利要求1所述的方法，其特征在于，所述别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程，具体为：
设计卡尔曼滤波的系统模型的状态变量x，令x为姿态角，即x＝{φ，θ，ψ}T，将系统模型变化成差分方程，公式为：
Figure FDA0003422845100000045
将测量模型变化成差分方程，公式为：
Figure FDA0003422845100000046
其中，zk为k次采样时刻的测量变量，[φk θk ψk]分别表示k次采样时刻时运动载体的横滚角、俯仰角和航向角，[ωkx ωky ωkz]分别是k次采样时刻时陀螺仪固定在运动载体坐标系上采集的x轴、y轴、z轴方向上的角速度数据，ωk为k次采样时刻时引入系统并影响状态变量的噪声，vk为k次采样时刻时传感器测量的噪声，xk是k次采样时刻的状态变量，f(xk)为k次采样时刻的状态转移函数，h(xk)为值状态-测量函数。
6.根据权利要求5所述的方法，其特征在于，还包括：
选择适合当前均值和协方差的sigma点集和权重，根据sigma点集和权重构建样本，获得样本sigma点χi和权重Wi；其中，
Figure FDA0003422845100000051
Figure FDA0003422845100000052
Figure FDA0003422845100000053
Figure FDA0003422845100000054
Figure FDA0003422845100000055
Figure FDA0003422845100000056
其中，κ是任意常数，
Figure FDA0003422845100000057
是k次采样时刻的状态变量预测值，n为状态变量x的维数，ui是来自于矩阵U的行向量，矩阵U与κ的关系为：
UTU＝(n+κ)Pk-1
其中，n＝3，κ＝0，Pk-1是k-1次采样时刻的误差协方差；
则，状态转移函数y＝f(x)的均值ym为：
Figure FDA0003422845100000058
其中，f(χi)为变量为sigma点的状态转移函数；
状态转移函数y＝f(x)的协方差Py为：
Figure FDA0003422845100000061
其中，ym为函数f(x)的均值，n为状态变量x的维数。
7.根据权利要求1所述的方法，其特征在于，在寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角之前，包括：
计算残差ηk、残差协方差
Figure FDA0003422845100000065
和马氏距离M2；
判断马氏距离M2与卡方分布值
Figure FDA0003422845100000064
之间的大小；
如果
Figure FDA0003422845100000062
引入增强因子，重新计算残差协方差
Figure FDA0003422845100000066
与传统无迹卡尔曼滤波不同的卡尔曼增益Kk；
如果
Figure FDA0003422845100000063
计算传统无迹卡尔曼滤波的卡尔曼增益Kk。
8.一种运动载体姿态角实时测量装置，其特征在于，所述装置包括：
初始值获取模块，用于获取惯性传感器实时采集的运动载体的角速度和加速度；
寻优估计值计算模块，用于根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；
递推方程构建模块，用于分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；
姿态角计算模块，用于将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202111569311.6A 2021-12-21 2021-12-21 一种运动载体姿态实时测量方法、装置 Pending CN114234970A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111569311.6A CN114234970A (zh) 2021-12-21 2021-12-21 一种运动载体姿态实时测量方法、装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111569311.6A CN114234970A (zh) 2021-12-21 2021-12-21 一种运动载体姿态实时测量方法、装置

Publications (1)

Publication Number Publication Date
CN114234970A true CN114234970A (zh) 2022-03-25

Family

ID=80760119

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111569311.6A Pending CN114234970A (zh) 2021-12-21 2021-12-21 一种运动载体姿态实时测量方法、装置

Country Status (1)

Country Link
CN (1) CN114234970A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115615428A (zh) * 2022-10-17 2023-01-17 中国电信股份有限公司 终端的惯性测量单元的定位方法、装置、设备和可读介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013061309A (ja) * 2011-09-15 2013-04-04 Yamaha Corp カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム
CN105890598A (zh) * 2016-04-08 2016-08-24 武汉科技大学 共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法
CN109443342A (zh) * 2018-09-05 2019-03-08 中原工学院 新型自适应卡尔曼无人机姿态解算方法
CN112683274A (zh) * 2020-12-22 2021-04-20 西安因诺航空科技有限公司 一种基于无迹卡尔曼滤波的无人机组合导航方法和系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013061309A (ja) * 2011-09-15 2013-04-04 Yamaha Corp カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム
CN105890598A (zh) * 2016-04-08 2016-08-24 武汉科技大学 共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法
CN109443342A (zh) * 2018-09-05 2019-03-08 中原工学院 新型自适应卡尔曼无人机姿态解算方法
CN112683274A (zh) * 2020-12-22 2021-04-20 西安因诺航空科技有限公司 一种基于无迹卡尔曼滤波的无人机组合导航方法和系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
付雷等: "基于动态权值共轭梯度的自适应互补滤波姿态估计算法", 《高技术通讯》, vol. 29, no. 10, pages 979 *
曲正伟等: "用于电力系统动态状态估计的改进鲁棒无迹卡尔曼滤波算法", 《电力系统自动化》, vol. 42, no. 10, pages 89 *
曾聪等: "基于共轭梯度的ＥＫＦ姿态估计算法", 《计算机工程与设计》, vol. 39, no. 10, pages 3119 - 3120 *
董一兵: "基于改进鲁棒无迹卡尔曼滤波的电力系统分区状态估计", 中国优秀硕士学位论文全文数据库工程科技II辑》, no. 5, pages 042 - 777 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115615428A (zh) * 2022-10-17 2023-01-17 中国电信股份有限公司 终端的惯性测量单元的定位方法、装置、设备和可读介质
CN115615428B (zh) * 2022-10-17 2024-02-02 中国电信股份有限公司 终端的惯性测量单元的定位方法、装置、设备和可读介质

Similar Documents

Publication Publication Date Title
CN109974714B (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
Qin et al. Accuracy improvement of GPS/MEMS-INS integrated navigation system during GPS signal outage for land vehicle navigation
CN112577527B (zh) 车载imu误差标定方法及装置
CN104819717B (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
CN111578928B (zh) 一种基于多源融合定位系统的定位方法及装置
CN111597647B (zh) 一种面向工业生产过程的弹簧阻尼系统滤波故障诊断方法
CN111121938A (zh) 一种实时监测车辆载重的方法、终端设备及计算机可读存储介质
CN112066984B (zh) 姿态角度解算方法、装置、处理设备和存储介质
CN114234970A (zh) 一种运动载体姿态实时测量方法、装置
CN116067370A (zh) 一种imu姿态解算方法及设备、存储介质
CN113218391A (zh) 一种基于ewt算法的姿态解算方法
Aligia et al. An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
CN111707294A (zh) 基于最优区间估计的行人导航零速区间检测方法和装置
CN113188505B (zh) 姿态角度的测量方法、装置、车辆及智能臂架
CN110941920A (zh) 一种用于无人机飞行载荷数据计算与后处理的方法
CN109506674B (zh) 一种加速度的校正方法及装置
CN112720450B (zh) 机器人关节角度检验方法、装置、设备及介质
CN113936044B (zh) 激光设备运动状态的检测方法、装置、计算机设备及介质
CN112729348A (zh) 一种用于imu系统的姿态自适应校正方法
CN115218927B (zh) 基于二次卡尔曼滤波的无人机imu传感器故障检测方法
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN110864684A (zh) 用户姿态测算方法
CN110672127A (zh) 阵列式mems磁传感器实时标定方法

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20220325