CN111076722B - 基于自适应的四元数的姿态估计方法及装置 - Google Patents

基于自适应的四元数的姿态估计方法及装置 Download PDF

Info

Publication number
CN111076722B
CN111076722B CN201911129159.2A CN201911129159A CN111076722B CN 111076722 B CN111076722 B CN 111076722B CN 201911129159 A CN201911129159 A CN 201911129159A CN 111076722 B CN111076722 B CN 111076722B
Authority
CN
China
Prior art keywords
quaternion
noise
observation
attitude estimation
accelerometer
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
CN201911129159.2A
Other languages
English (en)
Other versions
CN111076722A (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.)
South Surveying & Mapping Technology Co ltd
Original Assignee
South GNSS Navigation Co Ltd
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 GNSS Navigation Co Ltd filed Critical South GNSS Navigation Co Ltd
Priority to CN201911129159.2A priority Critical patent/CN111076722B/zh
Publication of CN111076722A publication Critical patent/CN111076722A/zh
Application granted granted Critical
Publication of CN111076722B publication Critical patent/CN111076722B/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
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

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

基于自适应的四元数的姿态估计方法及装置
技术领域
本发明涉及姿态检测技术领域，尤其涉及一种基于自适应的四元数的姿态估计方法及装置。
背景技术
目前，多种传感器已经大规模综合应用于具体的载体，对于单一传感器，融合多种创拿起进行载体姿态估计，对提高姿态估计结果的精度、准度和鲁棒性均有重要意义。
1、融合多种传感器进行姿态估计需求背景
得益于微机电系统技术的发展(Micro-Electro-Mechanical System,MEMS)，越来越多的传感器变得微型化，且成本低廉，容易获得，如智能手机就集成了陀螺仪、加速度计、磁强计、红外传感器、摄像头等。又随着嵌入式平台算力不断提升，使得在嵌入平台的板载CPU上融合这些传感器的观测数据成为可能。
2、全姿态估计需求背景
在全姿态估计的场景中，如可无人机，穿戴设备，手持设备等的工况，如果载体的姿态以欧拉角的方式估计，载体的俯仰角是有可能出现90°的情况的，此时欧拉角的有效参数将会从三个退化为两个，无法确定载体此时的姿态。
3、复杂多变环境下的定向问题
在无遮挡环境下，定向问题可以由动态的GNSS接收机，或两台静态的 GNSS组成基线解决。在部分遮挡或全遮挡的环境下实现定向，较多的方法是使用地磁定向。但是地磁容易受到干扰。同时，载体的线性加速度也会干扰加速度计观测载体的姿态。因此，设计一种能够更为准确测量姿态的方法成为本领域技术人员亟待解决的技术问题。
发明内容
为了克服现有技术的不足，本发明的目的之一在于提供一种基于四元数的姿态估计方法，其能提升姿态估计的精准性。
本发明的目的之二在于提供一种电子设备，其能提升姿态估计的精准性。
本发明的目的之三在于提供一种计算机可读存储介质，其能提升姿态估计的精准性。
本发明的目的之一采用如下技术方案实现：
一种基于四元数的姿态估计方法，包括如下步骤：
模型构建步骤：根据四元数进行卡尔曼滤波器构建，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；所述卡尔曼滤波器中采用陀螺仪的角度随机游走系数ARW对四元数的过程噪声进行建模，且对陀螺仪的零偏噪声进行建模；
数据获取步骤：获取磁强计和加速度计的观测量；
更新步骤：判断加速度计的观测量是否超限，如果加速度计的观测量未超限，则对卡尔曼滤波器进行加速度计的观测值更新；如果磁强计的观测量未超限，则对卡尔曼滤波器进行磁强计的观测值更新以得更新后的四元数；
结果输出步骤：将更新后的四元数进行归一化以及协方差阵对称化处理后作为最终的姿态信息，并根据最终姿态信息输出姿态估计的观测值。
进一步地，所述卡尔曼滤波器通过如下步骤构建得到：
步骤1：对于任意四 维向量q，有运算符Ξ(q)：q＝[q0 q1 q2 q3]T
Figure GDA0003570236880000021
步骤2：四元数卡尔曼滤波的状态为四元数qk和陀螺零偏εk组合，也即是
Figure GDA0003570236880000031
且四元数关于角增量的状态转移矩阵为：
Figure GDA0003570236880000032
Figure GDA0003570236880000033
Figure GDA0003570236880000034
Figure GDA0003570236880000035
Figure GDA0003570236880000036
步骤3：定义四元数卡尔曼滤波的状态协方差更新方程以及观测噪声矩阵
Figure GDA0003570236880000037
分别为：
Figure GDA0003570236880000038
Figure GDA0003570236880000039
其中四元数卡尔曼滤波的过程噪声
Figure GDA00035702368800000310
为：
Figure GDA00035702368800000311
Figure GDA00035702368800000312
Figure GDA00035702368800000313
Figure GDA00035702368800000314
且
Figure GDA00035702368800000315
其中ARW为陀螺仪的角度随机游走系数，σ为陀螺仪的测量噪声标准差，τ为陀螺仪的测量噪声相关时间；Rk是与传感器直接相关的观测噪声矩阵，
Figure GDA00035702368800000316
是这两个矩阵的Kronecker积；在步骤3中，由于
Figure GDA0003570236880000041
Figure GDA0003570236880000042
τx＝τy＝τz则
Figure GDA0003570236880000043
简化计算方法为：
Figure GDA0003570236880000044
其中，
Figure GDA0003570236880000045
且
Figure GDA0003570236880000046
为
Figure GDA0003570236880000047
的四元数协方差阵；
步骤4：记传感器三轴单位化的观测矢量为
Figure GDA0003570236880000048
对应的在参考矢量在导航系的投影为
Figure GDA0003570236880000049
可以得到四元数卡尔曼滤波的乘性观测矩阵：
Figure GDA00035702368800000410
最终得到四元数卡尔曼滤波的量测更新如下：
Figure GDA00035702368800000411
Figure GDA00035702368800000412
Figure GDA00035702368800000413
Figure GDA00035702368800000414
进一步地，在步骤3中观测噪声
Figure GDA00035702368800000415
的简化计算方法为：
Figure GDA00035702368800000416
表示这个观测噪声矩阵是针对单位化观测量的。
进一步地，所述更新步骤具体为：
如果加速度计未超限，对卡尔曼滤波器进行加速度计观测值
Figure GDA00035702368800000417
Figure GDA00035702368800000418
更新；
如果磁强计未超限，对滤波器进行磁强计观测值
Figure GDA00035702368800000419
Figure GDA00035702368800000420
更新，进而从
Figure GDA00035702368800000421
中得到
Figure GDA00035702368800000422
根据公式：
Figure GDA0003570236880000051
计算得到
Figure GDA0003570236880000052
其中*表示四元数乘法，
Figure GDA0003570236880000053
是
Figure GDA0003570236880000054
的共轭四元数，
Figure GDA0003570236880000055
包括横滚角和俯仰角信息；
计算TRIAD算法，并忽略时间下标k，
Figure GDA0003570236880000056
Figure GDA0003570236880000057
Figure GDA0003570236880000058
将得到的结果进行量测更新以得到
Figure GDA0003570236880000059
和
Figure GDA00035702368800000510
进一步地，在所述更新步骤中，若GNSS接收机地速大于设定阈值，采用给出的航向角ψ作为观测值代替不稳定的磁场观测量，具体实现过程如下：
Figure GDA00035702368800000511
br＝[vE vN 0]T同样的将br代入TRIAD 算法中进行量测更新。
进一步地，还包括自适应步骤，所述自适应步骤具体如下：
根据新息计算公式计算四元数卡尔曼滤波器的新息；所述新息计算公式为：
Figure GDA00035702368800000512
若陀螺仪稳定性低，将不含有角度随机游走系数的过程噪声项投影到观测空间得到L1，且
Figure GDA00035702368800000513
根据如下计算公式计算得到自适应系数ηk；
Figure GDA00035702368800000514
Figure GDA00035702368800000515
Figure GDA00035702368800000516
β＝0.9→0.99
Figure GDA00035702368800000517
对自适应系数ηk做下界约束得到如下公式：
Figure GDA0003570236880000061
Figure GDA0003570236880000062
其中λ是调节自适应效果的比例因子；
若陀螺仪稳定性较高，计算得到不包括传感器观测噪声系数ρ的观测噪声协方差阵L1，
Figure GDA0003570236880000063
根据如下计算公式计算得到自适应系数ηk；
Figure GDA0003570236880000064
Figure GDA0003570236880000065
Figure GDA0003570236880000066
β＝0.9→0.99
Figure GDA0003570236880000067
需要对自适应系数ηk做下界约束：
Figure GDA0003570236880000068
Figure GDA0003570236880000069
其中λ是调节自适应效果的比例因子。
进一步地，所述数据获取步骤之后还包括传感器补偿步骤：采用在线卡尔曼滤波并结合误差模型实现对观测矢量和参考矢量的校准。
本发明的目的之二采用如下技术方案实现：
一种电子设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现本发明目之一中任意一项所述的一种基于四元数的姿态估计方法。
本发明的目的之三采用如下技术方案实现：
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如本发明目的之一中任意一项所述的一种基于四元数的姿态估计方法。
相比现有技术，本发明的有益效果在于：
本发明的基于四元数的姿态估计方法，可以进行全姿态估计，不必对四元数进行线性化，且对载体初始姿态不敏感；且本发明不单融合了多种传感器的观测结果，而且考虑了陀螺工况时存在的零偏，并对零偏的噪声进行建模，提高了姿态估计精度、准度和鲁棒性。
附图说明
图1为实施例一的基于四元数的姿态估计方法的流程图；
图2为实施例一的基于四元数的姿态估计方法的具体实施原理图。
具体实施方式
下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。
实施例一
如图1和图2所示，本实施例提供了一种基于四元数的姿态估计方法，其特征在于，包括如下步骤：
S101：根据四元数进行卡尔曼滤波器构建，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；所述卡尔曼滤波器中采用陀螺仪的角度随机游走系数ARW对四元数的过程噪声进行建模，且对陀螺仪的零偏噪声进行建模；采用一阶高斯马尔科夫过程对陀螺零偏的过程噪声进行建模。
四元数描述姿态有自身的缺点，其四个参数物理意义不明确，与欧拉角的三个参数并非一一对应关系，无法独立估计各个欧拉角。利用四元数进行姿态估计的常见做法是使用四元数扩展卡尔曼滤波(QuaternionExtensionKalmanFilter, QEKF)，QEKF是将四元数与欧拉角的关系进行线性化，得到近似的一一对应关系，再用四元数扩展卡尔曼滤波进行估计，但这存在一些问题，如初始条件敏感、非线性状态下误差较大、若观测量类型较多，还要求解繁琐高维雅可比矩阵等问题。故而在本实施例中采用基于Clifford代数的四元数卡尔曼滤波 (Quaternion Kalman Filter,QKF)，可以不必求解四元数线性化的雅可比矩阵，且对初始条件不敏感；从而使得避免出现线性化问题。而现有技术中会采用非线性的反三角函数构造观测量，由于卡尔曼滤波是最优线性估计，只能接受线性化的方法,相比于使用非线性方法的线性化近似,使用直接的线性方法建模是更优的。
所述卡尔曼滤波器通过如下步骤构建得到：
步骤1：对于任意四 维向量q，有运算符Ξ(q)：q＝[q0 q1 q2 q3]T
Figure GDA0003570236880000081
步骤2：四元数卡尔曼滤波的状态为四元数qk和陀螺零偏εk组合，也即是
Figure GDA0003570236880000082
且四元数关于角增量的状态转移矩阵为：
Figure GDA0003570236880000091
Figure GDA0003570236880000092
Figure GDA0003570236880000093
Figure GDA0003570236880000094
Figure GDA0003570236880000095
步骤3：定义四元数卡尔曼滤波的状态协方差更新方程以及观测噪声矩阵
Figure GDA0003570236880000096
分别为：
Figure GDA0003570236880000097
Figure GDA0003570236880000098
其中四元数卡尔曼滤波的过程噪声
Figure GDA0003570236880000099
为：
Figure GDA00035702368800000910
Figure GDA00035702368800000911
Figure GDA00035702368800000912
Figure GDA00035702368800000913
且
Figure GDA00035702368800000914
其中ARW为陀螺仪的角度随机游走系数，σ为陀螺仪的测量噪声标准差，τ为陀螺仪的测量噪声相关时间；Rk是与传感器直接相关的观测噪声矩阵，
Figure GDA00035702368800000915
是这两个矩阵的Kronecker积；在步骤3中，经验上来说，消费级陀螺一般取1000s，战术级陀螺一般取3000s 以上，一般认为三个轴在这三个参数上的差别可以忽略。由于
Figure GDA0003570236880000101
Figure GDA0003570236880000102
τx＝τy＝τz则
Figure GDA0003570236880000103
简化计算方法为：
Figure GDA0003570236880000104
其中，
Figure GDA0003570236880000105
且
Figure GDA0003570236880000106
为
Figure GDA0003570236880000107
的四元数协方差阵。
在步骤3中观测噪声
Figure GDA0003570236880000108
的简化计算方法为：
Figure GDA0003570236880000109
表示这个观测噪声矩阵是针对单位化观测量的。
步骤4：记传感器三轴单位化的观测矢量为
Figure GDA00035702368800001010
对应的在参考矢量在导航系的投影为
Figure GDA00035702368800001011
可以得到四元数卡尔曼滤波的乘性观测矩阵：
Figure GDA00035702368800001012
最终得到四元数卡尔曼滤波的量测更新如下：
Figure GDA00035702368800001013
Figure GDA00035702368800001014
Figure GDA00035702368800001015
Figure GDA00035702368800001016
S102：获取磁强计和加速度计的观测量；有关传感器原始观测量的改正问题，需要依照传感器误差模型对b，r进行改正，才可以构建
Figure GDA00035702368800001017
Figure GDA00035702368800001018
用作四元数卡尔曼滤波的估计，改正参数由在线卡尔曼滤波估计给出。
加速度计观测矢量与参考矢量，记以下矢量的单位矢量为
Figure GDA00035702368800001019
Figure GDA00035702368800001020
Figure GDA0003570236880000111
磁强计观测矢量与参考矢量，记以下矢量的单位矢量为
Figure GDA0003570236880000112
Figure GDA0003570236880000113
Figure GDA0003570236880000114
在步骤S102之后包括传感器补偿步骤：采用在线卡尔曼滤波并结合误差模型实现对观测矢量和参考矢量的校准。实现了在线补偿外界线性加速度干扰，实现在线校正磁强计附近的软磁干扰，实现在线补偿磁强计附近硬磁干扰。
在加速度计，磁强计实际定姿过程中，载体自身的运动状态，所处环境难免会干扰它们运行，使之输出的观测值含有较大误差。本实施例实现了一个能够在线估计这些干扰的滤波器，可以实时估计出外部环境的干扰，使得算法对来自环境有较强的鲁棒性。
为了将各项潜在误差考虑进来,需要对传感器进行校准,以及动态地补偿干扰.改正观测量的误差分为三部分工作:第一、
Figure GDA0003570236880000115
Figure GDA0003570236880000116
改正:使用Sk-1,hk-1,rm,k-1, ra,k-1,结合误差模型对bk,rk进行改正,单位化后得到
Figure GDA0003570236880000117
Figure GDA0003570236880000118
此步骤在观测值预处理时完成；第二、qk改正:各项被考虑的误差会引起一个失准角矢量φ,使用φ改正qk,此步骤在整合滤波内完成；第三、Sk-1,hk-1,rm,k-1,ra,k-1改正:使用在线补偿卡尔曼滤波估计δSk,δhk,δrm,k,δra,k,然后迭代改正,此步骤在在线补偿卡尔曼滤波内完成。
S103：判断加速度计的观测量是否超限，如果加速度计的观测量未超限，则对卡尔曼滤波器进行加速度计的观测值更新；如果磁强计的观测量未超限，则对卡尔曼滤波器进行磁强计的观测值更新以得更新后的四元数；
所述步骤S103具体为：
如果加速度计未超限，对卡尔曼滤波器进行加速度计观测值
Figure GDA0003570236880000121
Figure GDA0003570236880000122
更新；
如果磁强计未超限，对滤波器进行磁强计观测值
Figure GDA0003570236880000123
Figure GDA0003570236880000124
更新，进而从
Figure GDA0003570236880000125
中得到
Figure GDA0003570236880000126
根据公式：
Figure GDA0003570236880000127
计算得到
Figure GDA0003570236880000128
其中*表示四元数乘法，
Figure GDA0003570236880000129
是
Figure GDA00035702368800001210
的共轭四元数，
Figure GDA00035702368800001211
包括横滚角和俯仰角信息；
计算TRIAD算法，并忽略时间下标k，
Figure GDA00035702368800001212
Figure GDA00035702368800001213
Figure GDA00035702368800001214
将得到的结果进行量测更新以得到
Figure GDA00035702368800001215
和
Figure GDA00035702368800001216
若GNSS接收机地速大于设定阈值，采用给出的航向角ψ作为观测值代替不稳定的磁场观测量，具体实现过程如下：
Figure GDA00035702368800001217
br＝[vE vN 0]T；同样的将br代入 TRIAD算法中进行量测更新。
如果磁强计被充分校准，磁场干扰得到良好补偿，可以直接使用磁场原始观测量，如果载体处于低速或者静止状态，为了提高横滚角和俯仰角的估计精度，有必要实施TRIAD算法对观测量进行重新构建。
在使用多个传感器进行姿态估计时，我们希望有条件地组合来自不同传感器的观测量，如在静止或低速条件下，加速度计对横滚角和俯仰角的测量准度，要比无磁场干扰下的磁强计高，若此时引入磁强计观测量会对横滚角和俯仰角的估计结果造成偏差，即使对磁强计观测量进行降权可以提升估计结果精度，这种偏差仍然存在。另外，过度降低磁强计观测量的权，会造成航向角估计精度下降，使得算法整体稳定性下降。此时我们只想利用磁强计得到的航向角。
与此同时，输入给QKF的观测量不是以欧拉角形式，而是参考矢量在传感器上的投影矢量，因此不能简单地将磁强计得到航向角输入QKF。这使得我们需要额外构建一种算法，能够将磁强计得到矢量中所包含的横滚角和俯仰角信息，替换成QKF一步预测结果所包含的横滚角和俯仰角信息，再输入QKF。本实施例使用TRIAD算法完成这一过程。
S104：将更新后的四元数进行归一化以及协方差阵对称化处理后作为最终的姿态信息，并根据最终姿态信息输出姿态估计的观测值。
在本实施例中还包括自适应步骤，所述自适应步骤具体如下：
根据新息计算公式计算四元数卡尔曼滤波器的新息；所述新息计算公式为：
Figure GDA0003570236880000131
若陀螺仪稳定性低，将不含有角度随机游走系数的过程噪声项投影到观测空间得到L1，且
Figure GDA0003570236880000132
根据如下计算公式计算得到自适应系数ηk；
Figure GDA0003570236880000133
Figure GDA0003570236880000134
Figure GDA0003570236880000135
β＝0.9→0.99
Figure GDA0003570236880000136
对自适应系数ηk做下界约束得到如下公式：
Figure GDA0003570236880000141
Figure GDA0003570236880000142
其中λ是调节自适应效果的比例因子；
若陀螺仪稳定性较高，计算得到不包括传感器观测噪声系数ρ的观测噪声协方差阵L1，
Figure GDA0003570236880000143
根据如下计算公式计算得到自适应系数ηk；
Figure GDA0003570236880000144
Figure GDA0003570236880000145
Figure GDA0003570236880000146
β＝0.9→0.99
Figure GDA0003570236880000147
需要对自适应系数ηk做下界约束：
Figure GDA0003570236880000148
Figure GDA0003570236880000149
其中λ是调节自适应效果的比例因子。
基于新息-协方差最小化准则的观测噪声自适应算法，应对观测噪声统计特性可能发生的变化；传感器在工作时，环境变化是随机的，如器件温度升高会导致传感器的热噪声变大；载体发生震动时，加速度计的观测噪声也会变大等。为了应对这样的环境变化，我们需要自适应地调整观测噪声，以获得更好的估计结果，提高了算法的鲁棒性。
在线补偿卡尔曼滤波，变量定义，vec(m)运算符，对矩阵m进行阵列展开：
Figure GDA0003570236880000151
在线补偿卡尔曼滤波的状态为：
Xδ＝[φ vec(δS) δh δrm δra]T
分别是：φ,1×3:状态中所考虑的误差项引起的失准角
οvec(δS),1×9:按照列向量展开的S,3×3矩阵,此矩阵综合描述了磁强计安装角误差,线性比例因子,交轴偏差变化
οδh,1×3:磁强计零偏变化
οδrm,1×3:参考磁场可能发生的变化(载体附近的磁场干扰)
οδra,1×3:参考重力可能发生的变化(载体线性加速度)。
且在线卡尔曼滤波的时间更新步骤：
Figure GDA0003570236880000152
量测更新，设传感器观测量和参考了的差异为δ，则有加速度的差异为δa和磁强计的差异δm，将这两个量考虑为在线卡尔曼滤波的观测量，且接下来考虑的均不为单位化向量，而是传感器给出的观测量。
记当前一步预测四元数对应的投影矩阵：
Figure GDA0003570236880000161
Figure GDA0003570236880000162
Figure GDA0003570236880000163
Figure GDA0003570236880000164
在线补偿卡尔曼滤波的观测量是传感器补偿后的观测矢量减去参考矢量：
Figure GDA0003570236880000165
Figure GDA0003570236880000166
利用四元数卡尔曼滤波观测噪声的自适应系数构造在线卡尔曼滤波的观测噪声：
Figure GDA0003570236880000167
Figure GDA0003570236880000168
Figure GDA0003570236880000169
其中k0，k1要使得以下等式成立：
Figure GDA00035702368800001610
Figure GDA00035702368800001611
和
Figure GDA00035702368800001612
的设定，分别是滤波器设计的磁强计停止更新阈值Tm的加速度计停止更新阈值Ta有关，一般设3倍的阈值T即是标准差σδ；
最终得到量测更新实施：
Figure GDA0003570236880000171
Figure GDA0003570236880000172
Figure GDA0003570236880000173
Figure GDA0003570236880000174
反馈后的改正量Sk,hk,rm,k,ra,k将在k+1时刻改正自来传感器的原始观测量。
本实施例基于四元数的姿态估计方法有如下几个亮点：1、基于四元数卡尔曼滤波，可以进行全姿态估计，不必对四元数进行线性化，对载体初始姿态不敏感；2、融合了多种传感器的观测结果，提高了姿态估计精度、准度和鲁棒性； 3、分离了磁强计的航向角观测量，使之不会对横滚角和俯仰角的估计造成影响4、在线自适应观测噪声的变化，可以应对传感器工况变化造成的观测噪声变化，提升了算法的鲁棒性。
实施例二
实施例二公开了一种电子设备，该电子设备包括处理器、存储器以及程序，其中处理器和存储器均可采用一个或多个，程序被存储在存储器中，并且被配置成由处理器执行，处理器执行该程序时，实现实施例一的一种基于四元数的姿态估计方法。该电子设备可以是手机、电脑、平板电脑等等一系列的电子设备。
实施例三
实施例三公开了一种计算机可读存储介质，该存储介质用于存储程序，并且该程序被处理器执行时，实现实施例一的一种基于四元数的姿态估计方法。
当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的方法操作，还可以执行本发明任意实施例所提供的方法中的相关操作。
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器 (Read-Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台电子设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
值得注意的是，上述基于内容更新通知装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。
上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

Claims (8)

1.一种基于四元数的姿态估计方法，其特征在于，包括如下步骤：
模型构建步骤：根据四元数进行卡尔曼滤波器构建，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；所述卡尔曼滤波器中采用陀螺仪的角度随机游走系数ARW对四元数的过程噪声进行建模，且对陀螺仪的零偏噪声进行建模；
数据获取步骤：获取磁强计和加速度计的观测量；
更新步骤：判断加速度计的观测量是否超限，如果加速度计的观测量未超限，则对卡尔曼滤波器进行加速度计的观测值更新；如果磁强计的观测量未超限，则对卡尔曼滤波器进行磁强计的观测值更新以得更新后的四元数；
结果输出步骤：将更新后的四元数进行归一化以及协方差阵对称化处理后作为最终的姿态信息，并根据最终姿态信息输出姿态估计的观测值；
所述卡尔曼滤波器通过如下步骤构建得到：
步骤1：对于任意四维 向量q，有运算符Ξ(q)：q＝[q0 q1 q2 q3]T
Figure FDA0003570236870000011
步骤2：四元数卡尔曼滤波的状态为四元数qk和陀螺零偏εk组合，也即是
Figure FDA0003570236870000012
且四元数关于角增量的状态转移矩阵为：
Figure FDA0003570236870000021
Figure FDA0003570236870000022
Figure FDA0003570236870000023
Figure FDA0003570236870000024
Figure FDA0003570236870000025
步骤3：定义四元数卡尔曼滤波的状态协方差更新方程以及观测噪声矩阵
Figure FDA0003570236870000026
分别为：
Figure FDA0003570236870000027
Figure FDA0003570236870000028
其中四元数卡尔曼滤波的过程噪声
Figure FDA0003570236870000029
为：
Figure FDA00035702368700000210
Figure FDA00035702368700000211
Figure FDA00035702368700000212
Figure FDA00035702368700000213
且
Figure FDA00035702368700000214
其中ARW为陀螺仪的角度随机游走系数，σ为陀螺仪的测量噪声标准差，τ为陀螺仪的测量噪声相关时间；Rk是与传感器直接相关的观测噪声矩阵，
Figure FDA00035702368700000215
是这两个矩阵的Kronecker积；在步骤3中，由于
Figure FDA0003570236870000031
则
Figure FDA0003570236870000032
简化计算方法为：
Figure FDA0003570236870000033
其中，
Figure FDA0003570236870000034
且
Figure FDA0003570236870000035
为
Figure FDA0003570236870000036
的四元数协方差阵
步骤4：记传感器三轴单位化的观测矢量为
Figure FDA0003570236870000037
对应的在参考矢量在导航系的投影为
Figure FDA0003570236870000038
可以得到四元数卡尔曼滤波的乘性观测矩阵：
Figure FDA0003570236870000039
最终得到四元数卡尔曼滤波的量测更新如下：
Figure FDA00035702368700000310
Figure FDA00035702368700000311
Figure FDA00035702368700000312
Figure FDA00035702368700000313
2.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，在步骤3中观测噪声
Figure FDA00035702368700000314
的简化计算方法为：
Figure FDA00035702368700000315
中
Figure FDA00035702368700000316
表示这个观测噪声矩阵是针对单位化观测量的。
3.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，所述更新步骤具体为：
如果加速度计未超限，对卡尔曼滤波器进行加速度计观测值
Figure FDA0003570236870000041
更新；
如果磁强计未超限，对滤波器进行磁强计观测值
Figure FDA0003570236870000042
更新，进而从
Figure FDA0003570236870000043
中得到
Figure FDA0003570236870000044
根据公式：
Figure FDA0003570236870000045
计算得到
Figure FDA0003570236870000046
其中*表示四元数乘法，
Figure FDA0003570236870000047
是
Figure FDA0003570236870000048
的共轭四元数，
Figure FDA0003570236870000049
包括横滚角和俯仰角信息；
计算TRIAD算法，并忽略时间下标k，
Figure FDA00035702368700000410
Figure FDA00035702368700000411
Figure FDA00035702368700000412
将得到的结果进行量测更新以得到
Figure FDA00035702368700000413
和
Figure FDA00035702368700000414
4.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，在所述更新步骤中，若GNSS接收机地速大于设定阈值，采用给出的航向角ψ作为观测值代替不稳定的磁场观测量，具体实现过程如下：
Figure FDA00035702368700000415
br＝[vE vN 0]T同样的将br代入TRIAD算法中进行量测更新。
5.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，还包括自适应步骤，所述自适应步骤具体如下：
根据新息计算公式计算四元数卡尔曼滤波器的新息；所述新息计算公式为：
Figure FDA00035702368700000416
若陀螺仪稳定性低，将不含有角度随机游走系数的过程噪声项投影到观测空间得到L1，且
Figure FDA00035702368700000417
根据如下计算公式计算得到自适应系数ηk；
Figure FDA0003570236870000051
Figure FDA0003570236870000052
Figure FDA0003570236870000053
β＝0.9→0.99
Figure FDA0003570236870000054
对自适应系数ηk做下界约束得到如下公式：
Figure FDA0003570236870000055
Figure FDA0003570236870000056
其中λ是调节自适应效果的比例因子；
若陀螺仪稳定性较高，计算得到不包括传感器观测噪声系数ρ的观测噪声协方差阵L1，
Figure FDA0003570236870000057
根据如下计算公式计算得到自适应系数ηk；
Figure FDA0003570236870000058
Figure FDA0003570236870000059
Figure FDA00035702368700000510
β＝0.9→0.99
Figure FDA00035702368700000511
需要对自适应系数ηk做下界约束：
Figure FDA0003570236870000061
Figure FDA0003570236870000062
其中λ是调节自适应效果的比例因子。
6.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，所述数据获取步骤之后还包括传感器补偿步骤：采用在线卡尔曼滤波并结合误差模型实现对观测矢量和参考矢量的校准。
7.一种电子设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-6中任意一项所述的一种基于四元数的姿态估计方法。
8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1-6任意一项所述的一种基于四元数的姿态估计方法。
CN201911129159.2A 2019-11-18 2019-11-18 基于自适应的四元数的姿态估计方法及装置 Active CN111076722B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911129159.2A CN111076722B (zh) 2019-11-18 2019-11-18 基于自适应的四元数的姿态估计方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911129159.2A CN111076722B (zh) 2019-11-18 2019-11-18 基于自适应的四元数的姿态估计方法及装置

Publications (2)

Publication Number Publication Date
CN111076722A CN111076722A (zh) 2020-04-28
CN111076722B true CN111076722B (zh) 2022-07-19

Family

ID=70311234

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911129159.2A Active CN111076722B (zh) 2019-11-18 2019-11-18 基于自适应的四元数的姿态估计方法及装置

Country Status (1)

Country Link
CN (1) CN111076722B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112683267B (zh) * 2020-11-30 2022-06-03 电子科技大学 一种附有gnss速度矢量辅助的车载姿态估计方法
CN112683269B (zh) * 2020-12-07 2022-05-03 电子科技大学 一种附有运动加速度补偿的marg姿态计算方法
CN113029127B (zh) * 2021-03-10 2023-05-09 中国人民解放军海军航空大学 基于分布式循环结构的飞行器自主姿态估计方法
CN113237478B (zh) * 2021-05-27 2022-10-14 哈尔滨工业大学 一种无人机的姿态和位置估计方法及无人机
CN114459466A (zh) * 2021-12-29 2022-05-10 宜昌测试技术研究所 一种基于模糊控制的mems多传感器数据融合处理方法
CN117879540B (zh) * 2024-03-12 2024-06-18 西南应用磁学研究所(中国电子科技集团公司第九研究所) 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿系统惯性/地磁组合方法
CN103941274A (zh) * 2014-04-15 2014-07-23 北京北斗星通导航技术股份有限公司 一种导航方法及导航终端
CN107402007A (zh) * 2016-05-19 2017-11-28 成都天禄科技有限公司 一种提高微型ahrs模块精度的方法和微型ahrs模块
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN108759870A (zh) * 2018-07-03 2018-11-06 哈尔滨工业大学 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN110457858A (zh) * 2019-08-22 2019-11-15 广州大学 基于双轴实测加速度的高层建筑模态振动主轴的确定方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿系统惯性/地磁组合方法
CN103941274A (zh) * 2014-04-15 2014-07-23 北京北斗星通导航技术股份有限公司 一种导航方法及导航终端
CN107402007A (zh) * 2016-05-19 2017-11-28 成都天禄科技有限公司 一种提高微型ahrs模块精度的方法和微型ahrs模块
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN108759870A (zh) * 2018-07-03 2018-11-06 哈尔滨工业大学 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
CN110457858A (zh) * 2019-08-22 2019-11-15 广州大学 基于双轴实测加速度的高层建筑模态振动主轴的确定方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Magnetic, Acceleration Fields and Gyroscope Quaternion (MAGYQ)-Based Attitude Estimation with Smartphone Sensors for Indoor Pedestrian Navigation;Valerie Renaudin等;《SENSORS》;20141231(第14期);22863-22890 *
基于分解四元数的自适应姿态四元数卡尔曼滤波;张德先等;《控制理论与应用》;20180331;第35卷(第3期);367-374 *
基于四元数和Kalman滤波器的多传感器数据融合算法;姜晓旭等;《计量技术》;20171231(第5期);31-33 *

Also Published As

Publication number Publication date
CN111076722A (zh) 2020-04-28

Similar Documents

Publication Publication Date Title
CN111076722B (zh) 基于自适应的四元数的姿态估计方法及装置
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考系统算法
US9417091B2 (en) System and method for determining and correcting field sensors errors
Nazarahari et al. 40 years of sensor fusion for orientation tracking via magnetic and inertial measurement units: Methods, lessons learned, and future challenges
CN109163721A (zh) 姿态测量方法及终端设备
US9068843B1 (en) Inertial sensor fusion orientation correction
Pylvänäinen Automatic and adaptive calibration of 3D field sensors
US7860651B2 (en) Enhanced inertial system performance
EP1489381B1 (en) Method and apparatus for compensating for acceleration errors and inertial navigation system employing the same
CN108398128B (zh) 一种姿态角的融合解算方法和装置
CN112577521B (zh) 一种组合导航误差校准方法及电子设备
US10274318B1 (en) Nine-axis quaternion sensor fusion using modified kalman filter
KR101922700B1 (ko) 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치
CN103017763A (zh) 状态估计设备和偏移更新方法
US10197396B2 (en) Always on compass calibration system and methods
US11898874B2 (en) Gyroscope bias estimation
CN105606096A (zh) 一种载体运动状态信息辅助的姿态和航向计算方法和系统
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114485641A (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
CN108562290A (zh) 导航数据的滤波方法、装置、计算机设备及存储介质
Aligia et al. An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer
CN106931965B (zh) 一种确定终端姿态的方法及装置
US20200049504A1 (en) Machine learning zero-rate level calibration
CN116718153B (zh) 一种基于gnss和ins的形变监测方法及系统
US20200408527A1 (en) Method for estimating the movement of an object moving in a magnetic field

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20240621

Address after: 510000 Si Cheng Road, Tianhe District, Guangzhou, Guangdong Province, No. 39

Patentee after: SOUTH SURVEYING & MAPPING TECHNOLOGY CO.,LTD.

Country or region after: China

Address before: 510665 area a, 4 / F, area a, 5 / F, area a, 6 / F, 39 Sicheng Road, Tianhe District, Guangzhou City, Guangdong Province

Patentee before: GUANGZHOU SOUTH SATELLITE NAVIGATION INSTRUMENT Co.,Ltd.

Country or region before: China