CN114459478B - 一种基于姿态运动学模型的惯性测量单元数据融合方法 - Google Patents

一种基于姿态运动学模型的惯性测量单元数据融合方法 Download PDF

Info

Publication number
CN114459478B
CN114459478B CN202210060818.7A CN202210060818A CN114459478B CN 114459478 B CN114459478 B CN 114459478B CN 202210060818 A CN202210060818 A CN 202210060818A CN 114459478 B CN114459478 B CN 114459478B
Authority
CN
China
Prior art keywords
time
attitude
value
quaternion
gyroscope
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
CN202210060818.7A
Other languages
English (en)
Other versions
CN114459478A (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN202210060818.7A priority Critical patent/CN114459478B/zh
Publication of CN114459478A publication Critical patent/CN114459478A/zh
Application granted granted Critical
Publication of CN114459478B publication Critical patent/CN114459478B/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
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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

一种基于姿态运动学模型的惯性测量单元数据融合方法
技术领域
本发明涉及姿态测量传感器数据融合技术领域，更具体地说，它涉及一种基于姿态运动学模型的惯性测量单元数据融合方法。
背景技术
姿态测量传感器包括陀螺仪，磁力计和加速度计。多个姿态测量传感器组成的测量系统也被称为惯性测量单元，常见的惯性测量单元一般由三轴陀螺仪、三轴加速度计和三轴磁力计组成。单个姿态测量传感器可以用来获取载体的部分的姿态信息。具体来说，加速度计通过重力矢量在特定方向的夹角测量相关的姿态来获得运动载体的加速度信息，通过相应的旋转矩阵计算可以获得运动载体的姿态信息，其测量值不受时间的影响，动态性能较好，但由于加速度计z轴与重力加速度方向平行，这就导致无法获得准确的z轴姿态数据。磁力计与加速度计类似，在x轴和y轴方向的测量数据是不准确的，且容易受外部环境的干扰，短时间的测量值容易产生误差。陀螺仪通过陀螺效应测量角速度的，通过内部陀螺与初始方向的偏移计算角速度和旋转方向，再通过进一步积分得到姿态信息，但是由于计算姿态信息过程涉及到积分环节，陀螺仪测量的姿态数据一直会带有累计误差。
在进行姿态测量的时候，靠单一的姿态测量传感器获得的测量数据解算出来的姿态信息精度是很低的。例如，仅靠加速度计或磁力计获得的姿态信息，只能测某一方位的姿态值，不具备参考价值。而仅靠陀螺仪获得的姿态信息，会在长时间的计算中产生累计误差，同样不具备参考价值。
在现有技术中，针对多姿态测量传感器的数据融合获得更高精度的姿态数据，一般采用对姿态传感器的误差特性进行分析补偿的方法，例如卡尔曼滤波[刘颖,杨媛,徐金涛.融合互补滤波和卡尔曼滤波的姿态解算算法[J].西安邮电大学学报,2021,26(04):98-104.]，通过将测量传感器的噪声建模为高斯噪声，估计测量传感器的内部偏差，融合惯性测量单元进一步获得姿态数据。但是，卡尔曼滤波并没有考虑非线性姿态运动学模型，并且无法处理非高斯噪声，会制约融合精度的提升。
因此，本发明旨在设计提供一种基于姿态运动学模型的惯性测量单元数据融合方法，以解决上述问题。
发明内容
本发明的目的是为了解决上述问题，提供一种基于姿态运动学模型的惯性测量单元数据融合方法，本发明以四元数作为姿态表示方式，基于刚体的姿态运动学模型，设计姿态观测器融合惯性测量单元的测量数据，进行融合误差分析得到高精度的姿态融合数据和角速度估计数据，提高载体在姿态变化时候惯性测量单元解算姿态的精度，为载体进行稳定高效的姿态控制提供保障。
本发明的上述技术目的是通过以下技术方案得以实现的：一种基于姿态运动学模型的惯性测量单元数据融合方法，包括以下步骤：
S1、获取当前时刻载体惯性测量单元的测量数据，并进行归一化处理；
S2、基于陀螺仪误差模型对惯性测量单元的陀螺仪测量数据进行实时校正；
S3、基于惯性测量单元的量测模型，将加速度计和磁力计的测量数据分层解算成实时参考四元数姿态；
S4、针对刚体姿态运动学设计融合观测器，将步骤S2中所得的校正陀螺仪测量数据和步骤S3中所得的参考四元数姿态输入融合观测器，得到实时姿态融合估计数据；
S5、对步骤S4所得的实时姿态融合估计数据进行姿态运动学误差评估，更新下一时刻姿态融合值和角速度估计值，并返回步骤S1。
进一步地，步骤S1的具体方法为：
S11、归一化三轴加速度计实时测量数据：
载体姿态运动的k时刻，加速度计的测量数据为ag(k)＝[ax(k),ay(k),az(k)]T，通过下式对其进行归一化得到实时标准化加速度计数据：
其中，vng(k)＝[v1(k),v2(k),v3(k)]T为k时刻标准加速度计测量数据；
S12、归一化三轴磁力计实时测量数据：
磁力计在k时刻的测量数据为mg(k)＝[mx(k),my(k),mz(k)]T，通过下式对其进行归一化得到实时标准化磁力计数据：
其中，mng(k)＝[m1(k),m2(k),m3(k)]T为k时刻标准磁力计测量数据；
S13、归一化三轴陀螺仪实时测量数据：
陀螺仪在k时刻的测量数据为wg(k)＝[wx(k),wy(k),wz(k)]T，通过下式对其进行归一化得到实时标准陀螺仪测量数据：
其中，ωng(k)＝[ωg1(k),ωg2(k),ωg3(k)]T为k时刻标准陀螺仪测量数据。
进一步地，步骤S2的具体方法为：
S21、计算陀螺仪的误差矩阵：
将惯性测量单元单独放置水平面上，将其内部陀螺仪每个轴都相应的转动n圈，通过下式计算陀螺仪的误差矩阵：
其中，E代表陀螺仪的误差矩阵；BO为陀螺仪输出旋转矩阵；Bo-1为BO的逆矩阵；
S22、校正标准陀螺仪的实时测量数据：
利用陀螺仪误差矩阵、比例因子和零点漂移偏差将步骤S13得到的标准陀螺仪实时测量数据进行校正，具体如下：
其中，ω(k)＝[ω1(k),ω2(k),ω3(k)]T校正后的k时刻陀螺仪测量数据；hx,hy,hz分别为陀螺仪x,y,z轴的输出比例因子；bx,by,bz为陀螺仪x,y,z轴的零点漂移偏差；陀螺仪零点漂移偏差和输出比例因子可以由惯性测量单元的生产厂家给出。
进一步地，步骤S3的具体方法为：
S31、解算参考俯仰角和参考偏航角：
将步骤S11中得到的标准加速度计实时测量数据，通过下式转换为载体的参考实时俯仰角和实时参考滚转角：
其中，θ(k)为k时刻载体参考俯仰角；φ(k)为k时刻载体参考滚转角；
S32、解算参考偏航角：
将步骤S12中得到的标准磁力计实时测量数据和步骤S31中得到的俯仰角和偏航角数据，通过下式转换为载体的实时参考偏航角：
其中，ψ(k)为载体偏航角；
S33、转换载体参考欧拉角为实时参考四元数姿态：
将步骤S31中得到的参考俯仰角和参考偏航角以及步骤S32中得到的参考偏航角，通过下式转换为实时参考四元数：
其中，q(k)＝[q0(k),q1(k),q2(k),q3(k)]T为k时刻载体参考四元数姿态。
进一步地，步骤S4的具体方法为：
S41、计算姿态估计值和参考四元数姿态的相对误差：
通过下式计算姿态估计值和参考四元数姿态的实时相对误差：
其中，为k时刻姿态估计值，该值由观测器迭代得到，选取初始迭代值为/>qe(k)是为k时刻姿态估计值和参考四元数姿态的相对误差；qe0(k)为四维向量qe(k)的标量部分；qev(k)＝[qe1(k),qe2(k),qe3(k)]T为四维向量qe(k)的矢量部分；/>为qev(k)的转置；
其中，符号代表如下运算：
其中，qv(k)＝[q1(k),q2(k),q3(k)]T；为qv(k)的转置；/>I3为三阶单位矩阵；
矩阵S(qv(k))的具体形式如下：
S42、设计观测器，迭代得到角速度估计值：
下一时刻角速度估计值可以用下面的观测器迭代得到：
为k+1时刻角速度估计值，该值由k时刻角速度估计值/>迭代得到，初始时刻估计角速度/>可以人为给定；k2为常数，k2＞0；T为采样时间；E(qe(k))为姿态矩阵，具体形式如下：
其中，矩阵S(qev(k))的具体形式如下：
S43、设计融合观测器，迭代得到姿态估计值：
下一时刻姿态估计值采用下面的融合观测器迭代得到：
k1为常数，k1＞0；矩阵为如下形式：
矩阵R(qe(k))为旋转矩阵，其具体形式如下：
其中：
进一步地，步骤S5的具体方法为：
S51、通过刚体姿态运动学迭代参考四元数姿态：
通过刚体姿态运动学迭代下一时刻的参考四元数姿态，具体如下：
其中，q(k+1)是k+1时刻的参考四元数姿态；
姿态矩阵E(q(k))，表示如下：
S52、迭代得到相对四元数误差：
迭代下一时刻的四元数相对误差，具体如下：
qe(k+1)为k+1时刻的四元数相对误差；ET(qe(k))为姿态矩阵E(qe(k))的转置；
S53、评估步骤S43得到的估计四元数：
计算下一时刻估计四元数与参考四元数的误差，如下：
eq(k+1)为k+1时刻估计四元数与参考四元数的误差；
根据离散系统理论，若eq(k+1)和qe(k+1)的2-范数满足下式：
||eq(k+1)-q(k+1)||≤1×10-2；
则认定四元数误差qe(k+1)会收敛于极小的误差区域，即k+1时刻的姿态估计值与k+1时刻实际姿态值无限接近；
S54、更新姿态融合值：
根据步骤S53可知，若eq(k+1)和qe(k+1)满足：
||eq(k+1)-q(k+1)||≤1×10-2；
则下一时刻估计角速度值和姿态融合值为：
其中，为k+1的融合四元数姿态值；
若eq(k+1)和qe(k+1)满足：
eq(k+1)-q(k+1)||＞1×10-2；
则下一时刻估计角速度值和姿态融合值为：
最后，输出k+1时刻的估计角速度值和姿态融合值/>并令k＝k+1；并返回S1。
综上所述，本发明具有以下有益效果：本发明的方法以四元数作为姿态表示方式，基于刚体的姿态运动学模型，设计姿态观测器融合惯性测量单元的测量数据，进行融合误差分析得到高精度的姿态融合数据和角速度估计数据，能够提高载体在姿态变化时候惯性测量单元解算姿态的精度，从而便于为载体进行稳定高效的姿态控制提供保障。
附图说明
图1为本发明的一种基于姿态运动学模型的惯性测量单元数据融合方法的步骤示意图。
图2为本发明的一种基于姿态运动学模型的惯性测量单元数据融合方法的流程图。
图3为本实施例采用卡尔曼滤波进行姿态融合的姿态响应曲线。
图4为本实施例采用本发明方法进行姿态融合的姿态响应曲线。
图5为本实施例采用本发明方法进行姿态融合的角速度估计曲线。
具体实施方式
下面将结合本发明的实施例及附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例：一种基于姿态运动学模型的惯性测量单元数据融合方法，由图1所示，包括以下具体步骤：
S1、获取当前时刻载体惯性测量单元的测量数据并进行归一化处理；
S11、归一化三轴加速度计实时测量数据；
S12、归一化三轴磁力计实时测量数据；
S13、归一化三轴陀螺仪实时测量数据；
S2、基于陀螺仪误差模型对惯性测量单元的陀螺仪测量数据实时校正；
S21、计算陀螺仪的误差矩阵；
S22、校正标准陀螺仪的实时测量数据；
S3、基于惯性测量单元的量测模型将加速度计和磁力计的测量数据分层解算成实时参考四元数姿态；
S31、解算参考俯仰角和参考偏航角；
S32、解算参考偏航角；
S33、转换载体参考欧拉角为实时参考四元数姿态；
S4、针对刚体姿态运动学设计融合观测器，将步骤S2所得的校正陀螺仪测量数据和步骤S3所得的参考四元数姿态输入融合观测器，得到实时姿态估计数据；
S41、计算姿态估计值和参考四元数姿态的相对误差；
S42、设计观测器，迭代得到角速度估计值；
S43、设计融合观测器，迭代得到姿态估计值；
S5、对步骤S4中所得的姿态融合估计值进行姿态运动学误差评估，更新下一时刻姿态融合值和角速度估计值，并返回步骤S1；
S51、通过刚体姿态运动学迭代参考四元数姿态；
S52、迭代得到相对四元数误差；
S53、评估步骤S43得到的估计四元数；
S54、更新姿态融合值；
最后，输出时刻的估计角速度值和姿态融合值，并令；并返回步骤S1。
本发明的该实施例中，采用MATLAB 2019b作为仿真计算软件，模拟搭载三轴加速度计、三轴磁力计和三轴陀螺仪的四旋翼的三维姿态运动，采用本发明的基于姿态运动学模型的惯性测量单元数据融合方法与采用卡尔曼滤波数据融合进行效果对比，输出两种方法在姿态运动中的姿态融合值。
此外，本发明的本实施例采用传统PID算法对四旋翼姿态进行控制，其控制参数为kp＝5.14，kI＝3.21,kD＝1.2，控制四旋翼从初始姿态(四元数值：q＝[0.768，0.371,0.384,0.353]T)到达指定稳定姿态值(四元数值：q＝[1,0,0,0]T)。取仿真的四旋翼的转动惯量矩阵为：
仿真环境下，选择观测器的增益为：k1＝12，k2＝3.4。选择观测器初始迭代的估计四元数值为选择观测器初始迭代的估计角速度值为进行对比的卡尔曼滤波融合方法的初始迭代四元数值为/>
图3给出了应用卡尔曼滤波的四旋翼姿态响应曲线，黑色点线为真实姿态响应曲线，黑色实线为卡尔曼滤波输出的估计姿态值。考虑当估计值与真实值误差在±1×10-2区间内认为估计到真实值，则从图3中可以看出，卡尔曼滤波的姿态四元数q0在6.73s估计到真实值，姿态四元数q1在7.32s估计到真实值，姿态四元数q2在7.17s估计到真实值，姿态四元数q3在8.02s估计到真实值。
图4给出了应用本发明基于姿态运动学模型的惯性测量单元数据融合方法的四旋翼姿态响应曲线，黑色点线为真实姿态响应曲线，黑色实线为本发明的融合姿态值。仍然考虑估计值与真实值误差在±1×10-2rad/s区间内认为估计到真实值，则从图4可以看出，本发明的姿态融合的姿态四元数q0在2.13s内估计到真实值，姿态四元数q1在2.11s估计到真实值，姿态四元数q2在3.01s估计到真实值，姿态四元数q3在2.02s估计到真实值。
图5给出了应用本发明基于姿态运动学模型的惯性测量单元数据融合方法的四旋翼角速度估计曲线和四旋翼真实响应曲线。考虑估计角速度与真实角速度误差在±1×10-2区间内认为估计到真实值，则从图5中可以看出，本发明的姿态融合算法输出的角速度估计值ω1在7.21s内估计到真实值，角速度估计值ω2在7.36s内估计到真实值，ω3在7.47s内估计到真实值。
从上述的仿真结果可以明显看出，本发明的基于姿态运动学模型的惯性测量单元数据融合方法可以快速、准确的估计出姿态，其精度和估计速度明显优于卡尔曼滤波估计姿态。
在本发明的上述实施例中，本发明的一种基于姿态运动学模型的惯性测量单元数据融合方法，其精度高、普适性强，能够实时准确的输出载体的姿态信息。
本具体实施例仅仅是对本发明的解释，其并不是对本发明的限制，本领域技术人员在阅读完本说明书后可以根据需要对本实施例做出没有创造性贡献的修改，但只要在本发明的权利要求范围内都受到专利法的保护。

Claims (3)

1.一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：包括以下步骤：
S1、获取当前时刻载体惯性测量单元的测量数据，并将陀螺仪、加速度计、磁力计进行归一化处理；
S2、基于陀螺仪误差模型对惯性测量单元的陀螺仪测量数据进行实时校正；步骤S2的具体方法为：
S21、计算陀螺仪的误差矩阵：
将惯性测量单元单独放置水平面上，将其内部陀螺仪每个轴都相应的转动n圈，通过下式计算陀螺仪的误差矩阵：
其中，E代表陀螺仪的误差矩阵；BO为陀螺仪输出旋转矩阵；为BO的逆矩阵；
S22、校正标准陀螺仪的实时测量数据：
利用陀螺仪误差矩阵、比例因子和零点漂移偏差将步骤S13得到的标准陀螺仪实时测量数据进行校正，具体如下：
其中，ω(k)＝[ω1(k),ω2(k),ω3(k)]T校正后的k时刻陀螺仪测量数据；hx,hy,hz分别为陀螺仪x,y,z轴的输出比例因子；bx,by,bz为陀螺仪x,y,z轴的零点漂移偏差；
S3、基于惯性测量单元的量测模型，将加速度计和磁力计的测量数据分层解算成实时参考四元数姿态；步骤S3的具体方法为：
S31、解算参考俯仰角和参考偏航角：
将步骤S11中得到的标准加速度计实时测量数据，通过下式转换为载体的参考实时俯仰角和实时参考滚转角：
其中，θ(k)为k时刻载体参考俯仰角；φ(k)为k时刻载体参考滚转角；
S32、解算参考偏航角：
将步骤S12中得到的标准磁力计实时测量数据和步骤S31中得到的俯仰角和偏航角数据，通过下式转换为载体的实时参考偏航角：
其中，ψ(k)为载体偏航角；
S33、转换载体参考欧拉角为实时参考四元数姿态：
将步骤S31中得到的参考俯仰角和参考偏航角以及步骤S32中得到的参考偏航角，通过下式转换为实时参考四元数：
其中，q(k)＝[q0(k),q1(k),q2(k),q3(k)]T为k时刻载体参考四元数姿态；
S4、针对刚体姿态运动学设计融合观测器，将步骤S2中所得的校正陀螺仪测量数据和步骤S3中所得的参考四元数姿态输入融合观测器，得到实时姿态融合估计数据；
S5、对步骤S4所得的实时姿态融合估计数据进行姿态运动学误差评估，更新下一时刻姿态融合值和角速度估计值，并返回步骤S1；
步骤S5的具体方法为：
S51、通过刚体姿态运动学迭代参考四元数姿态：
通过刚体姿态运动学迭代下一时刻的参考四元数姿态，具体如下：
其中，q(k+1)是k+1时刻的参考四元数姿态；
姿态矩阵E(q(k))，表示如下：
S52、迭代得到相对四元数误差：
迭代下一时刻的四元数相对误差，具体如下：
qe(k+1)为k+1时刻的四元数相对误差；ET(qe(k))为姿态矩阵E(qe(k))的转置；
S53、评估步骤S43得到的估计四元数：
计算下一时刻估计四元数与参考四元数的误差，如下：
eq(k+1)为k+1时刻估计四元数与参考四元数的误差；
根据离散系统理论，若eq(k+1)和qe(k+1)的2-范数满足下式：
||eq(k+1)-q(k+1)||≤1×10-2；
则认定四元数误差qe(k+1)会收敛于极小的误差区域，即k+1时刻的姿态估计值与k+1时刻实际姿态值无限接近；
S54、更新姿态融合值：
根据步骤S53可知，若eq(k+1)和qe(k+1)满足：
||eq(k+1)-q(k+1)||≤1×10-2；
则下一时刻估计角速度值和姿态融合值为：
其中，为k+1的融合四元数姿态值；
若eq(k+1)和qe(k+1)满足：
||eq(k+1)-q(k+1)||＞1×10-2；
则下一时刻估计角速度值和姿态融合值为：
最后，输出k+1时刻的估计角速度值和姿态融合值/>并令k＝k+1；并返回S1。
2.根据权利要求1所述的一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：步骤S1的具体方法为：
S11、归一化三轴加速度计实时测量数据：
载体姿态运动的k时刻，加速度计的测量数据为ag(k)＝[ax(k),ay(k),az(k)]T，通过下式对其进行归一化得到实时标准化加速度计数据：
其中，vng(k)＝[v1(k),v2(k),v3(k)]T为k时刻标准加速度计测量数据；
S12、归一化三轴磁力计实时测量数据：
磁力计在k时刻的测量数据为mg(k)＝[mx(k),my(k),mz(k)]T，通过下式对其进行归一化得到实时标准化磁力计数据：
其中，mng(k)＝[m1(k),m2(k),m3(k)]T为k时刻标准磁力计测量数据；
S13、归一化三轴陀螺仪实时测量数据：
陀螺仪在k时刻的测量数据为wg(k)＝[wx(k),wy(k),wz(k)]T，通过下式对其进行归一化得到实时标准陀螺仪测量数据：
其中，ωng(k)＝[ωg1(k),ωg2(k),ωg3(k)]T为k时刻标准陀螺仪测量数据。
3.根据权利要求1所述的一种基于姿态运动学模型的惯性测量单元数据融合方法，其特征是：步骤S4的具体方法为：
S41、计算姿态估计值和参考四元数姿态的相对误差：
通过下式计算姿态估计值和参考四元数姿态的实时相对误差：
其中，为k时刻姿态估计值，该值由观测器迭代得到，选取初始迭代值为/>qe(k)是为k时刻姿态估计值和参考四元数姿态的相对误差；qe0(k)为四维向量qe(k)的标量部分；qev(k)＝[qe1(k),qe2(k),qe3(k)]T为四维向量qe(k)的矢量部分；/>为qev(k)的转置；
其中，符号代表如下运算：
其中，qv(k)＝[q1(k),q2(k),q3(k)]T；为qv(k)的转置；/>I3为三阶单位矩阵；
矩阵S(qv(k))的具体形式如下：
S42、设计观测器，迭代得到角速度估计值：
下一时刻角速度估计值可以用下面的观测器迭代得到：
为k+1时刻角速度估计值，该值由k时刻角速度估计值/>迭代得到，初始时刻估计角速度/>可以人为给定；k2为常数，k2＞0；T为采样时间；E(qe(k))为姿态矩阵，具体形式如下：
其中，矩阵S(qev(k))的具体形式如下：
S43、设计融合观测器，迭代得到姿态估计值：
下一时刻姿态估计值采用下面的融合观测器迭代得到：
k1为常数，k1＞0；矩阵为如下形式：
矩阵R(qe(k))为旋转矩阵，其具体形式如下：
其中：
CN202210060818.7A 2022-01-19 2022-01-19 一种基于姿态运动学模型的惯性测量单元数据融合方法 Active CN114459478B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210060818.7A CN114459478B (zh) 2022-01-19 2022-01-19 一种基于姿态运动学模型的惯性测量单元数据融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210060818.7A CN114459478B (zh) 2022-01-19 2022-01-19 一种基于姿态运动学模型的惯性测量单元数据融合方法

Publications (2)

Publication Number Publication Date
CN114459478A CN114459478A (zh) 2022-05-10
CN114459478B true CN114459478B (zh) 2024-03-19

Family

ID=81410334

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210060818.7A Active CN114459478B (zh) 2022-01-19 2022-01-19 一种基于姿态运动学模型的惯性测量单元数据融合方法

Country Status (1)

Country Link
CN (1) CN114459478B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115855072B (zh) * 2023-03-03 2023-05-09 北京千种幻影科技有限公司 驾驶模拟平台的姿态估算方法、装置、设备及存储介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7289906B2 (en) * 2004-04-05 2007-10-30 Oregon Health & Science University Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于重力四元数的陀螺漂移估计与补偿;杨金显;杨闯;;传感技术学报(第08期);全文 *
多导弹协同作战中的分布式协调控制问题;温广辉 等;《指挥与控制学报》;全文 *
车载GPS/DR组合导航系统的信息融合新方案;寇艳红, 张其善, 李先亮;遥测遥控(第01期);全文 *

Also Published As

Publication number Publication date
CN114459478A (zh) 2022-05-10

Similar Documents

Publication Publication Date Title
Ren et al. A novel self-calibration method for MIMU
CN107024674A (zh) 一种基于递推最小二乘法的磁强计现场快速标定方法
WO2022160391A1 (zh) 磁力计信息辅助的mems陀螺仪标定方法及标定系统
CN114216456B (zh) 一种基于imu与机器人本体参数融合的姿态测量方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN110174123B (zh) 一种磁传感器实时标定方法
JP2012173190A (ja) 測位システム、測位方法
CN108592943B (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN112129322B (zh) 一种捷联惯组与三轴转台的安装误差检测方法及校正方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN114459478B (zh) 一种基于姿态运动学模型的惯性测量单元数据融合方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN112461262A (zh) 一种校正三轴磁强计误差的装置和方法
CN107860382B (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
Aligia et al. An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer
CN106595669B (zh) 一种旋转体姿态解算方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110954081A (zh) 一种磁罗盘快速校准装置及方法
CN110954080A (zh) 一种消除载体磁干扰的磁罗盘校准方法
CN113156167B (zh) 一种三轴加速度计的标定方法及装置
CN112284388B (zh) 一种无人机多源信息融合导航方法
Xu et al. All-attitude motion tracking estimation based on Euler angles using MARG sensors
Sabir et al. Simplification of calibration of low-cost MARG sensors without high-precision laboratory equipment
Yi et al. A Decoupled-DCM Orientation Estimator for Eliminating Influence of Magnetic Interference on Roll Angle and Pitch Angle

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