CN112729348A - 一种用于imu系统的姿态自适应校正方法 - Google Patents

一种用于imu系统的姿态自适应校正方法 Download PDF

Info

Publication number
CN112729348A
CN112729348A CN202110073075.2A CN202110073075A CN112729348A CN 112729348 A CN112729348 A CN 112729348A CN 202110073075 A CN202110073075 A CN 202110073075A CN 112729348 A CN112729348 A CN 112729348A
Authority
CN
China
Prior art keywords
accelerometer
magnetometer
measurement
moment
adaptive correction
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
Application number
CN202110073075.2A
Other languages
English (en)
Other versions
CN112729348B (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.)
Henan University of Technology
Original Assignee
Henan University of Technology
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 Henan University of Technology filed Critical Henan University of Technology
Priority to CN202110073075.2A priority Critical patent/CN112729348B/zh
Publication of CN112729348A publication Critical patent/CN112729348A/zh
Application granted granted Critical
Publication of CN112729348B publication Critical patent/CN112729348B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种用于IMU系统的姿态自适应校正方法，包括：获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点；将获取的采样点对应输入系统的状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；根据量测预测残差获取加速度计自适应校正因子
Figure DSA0000230787980000011
和磁强计自适应校正因子
Figure DSA0000230787980000012
判断自适应校正因子之间的大小对系统姿态进行自适应校正。本发明所述的用于IMU系统的姿态自适应校正方法具有计算效率高，精确等特点，可广泛应用于姿态参数估计领域。

Description

一种用于IMU系统的姿态自适应校正方法
技术领域
本发明涉及姿态参数估计领域，特别是涉及一种用于IMU(Inertial MeasurementUnit，惯性测量单元)系统的姿态自适应校正方法。
背景技术
传统的姿态校正方法是通过卡尔曼滤波的方法利用加速度计或磁强计对陀螺仪输出的系统姿态进行校正，适用于线性系统，但在煤矿井下定向钻进场合大多都为非线性系统，传统的方法并不适用。对于非线性系统，扩展卡尔曼滤波(EKF)则是对将非线性系统线性化，再进行卡尔曼滤波。但是在进行线性化的过程中忽略了其余高阶项，会引入较大误差，并且处理非线性较强的动态系统时，可能会导致滤波发散。无迹卡尔曼滤波的提出利用概率分布来逼近非线性，可达到EKF的二阶精度，被广泛的应用于航天和无人驾驶等姿态参数估计领域。然而在许多环境下，由于UKF需要的采样数据繁多，会导致算法效率较低。同时，在数值计算过程中状态误差协方差也可能会出现负定情况而无法抑制算法发散。
由此可见，在现有技术中，无迹卡尔曼滤波还存在计算效率低和算法发散等问题。
发明内容
有鉴于此，本发明的主要目的在于提供一种用于IMU系统的姿态自适应校正方法。
为了达到上述目的，本发明提出的技术方案为：
一种用于IMU系统的姿态自适应校正方法，包括如下步骤：
步骤1、根据IMU(Inertial Measurement Unit，惯性测量单元)系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程。
步骤2、根据UT(Unscented Transformation，无迹变换)形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数。
步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将第k时刻系统状态预测值、加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻。
步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差。
步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子
Figure BSA0000230788000000021
和磁强计自适应校正因子
Figure BSA0000230788000000022
其中，
Figure BSA0000230788000000023
表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，
Figure BSA0000230788000000024
表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔAa为第k和第k-1时刻加速度计输出值差值，ΔHh为第k和第k-1时刻磁强计输出值差值。
步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8。
步骤7、根据系统第k时刻加速度计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻磁强计量测预测值和预测残差，对经过加速度计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。
步骤8、根据系统第k时刻磁强计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻加速度计量测预测值和预测残差，对经过磁强计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。
综上所述，本发明所述一种用于IMU系统的姿态自适应校正方法首先获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；根据步骤2获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点，减少了算法的输入，提高了滤波效率；其次，对获取的采样点输入系统状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；再次，根据量测预测残差获取加速度计自适应校正因子
Figure BSA0000230788000000031
Figure BSA0000230788000000032
和磁强计自适应校正因子
Figure BSA0000230788000000033
对系统姿态进行自适应校正，以此类推，不断自适应校正系统的姿态。因此，本发明所述顺序校正的超球体采样自适应无迹卡尔曼滤波方法在计算效率和精确度都有较大的提升。
附图说明
图1是本发明所述一种用于IMU(Inertial Measurement Unit，惯性测量单元)系统的姿态自适应校正方法的总体流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图及具体实施例对本发明作进一步地详细描述。
图1是本发明所述一种用于IMU(Inertial Measurement Unit，惯性测量单元)系统的姿态自适应校正方法的总体流程图。如图1所示，本发明所述用于IMU系统的姿态自适应校正方法，包括如下步骤：
步骤1、根据IMU系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程。
步骤2、根据UT(Unscented Transformation，无迹变换)形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数。
步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将第k时刻系统状态预测值、加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻。
步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差。
步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子
Figure BSA0000230788000000051
和磁强计自适应校正因子
Figure BSA0000230788000000052
其中，
Figure BSA0000230788000000053
表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，
Figure BSA0000230788000000054
表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔAa为第k和第k-1时刻加速度计输出值差值，ΔHh为第k和第k-1时刻磁强计输出值差值。
步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8。
步骤7、根据系统第k时刻加速度计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻磁强计量测预测值和预测残差，对经过加速度计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。
步骤8、根据系统第k时刻磁强计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻加速度计量测预测值和预测残差，对经过磁强计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。
本发明方法中，当加速度计和磁强计量测传感器任意量测具有不良数据而影响系统状态(姿态四元数)时，可以通过自适应因子来调节加速度计量测和磁强计量测的校正顺序，削弱不良数据的影响；当加速度计量测具有不良数据时，通过自适应顺序校正因子判断，使用加速度计量测对系统状态进行校正，加速度计对系统状态预测值和状态预测误差协方差对量测噪声协方差、状态四元数和状态估计协方差进行更新；然后使用磁强计对经过加速度计校正后的系统状态进行校正，更新加速度计校正后的系统状态估计值和状态估计协方差。当磁强计量测具有不良数据时，通过自适应顺序校正因子判断，使用磁强计量测对系统状态进行校正，磁强计对系统状态预测值和状态预测误差协方差对量测噪声协方差、状态四元数和状态估计协方差进行更新；然后使用加速度计对经过磁强计校正后的系统状态进行校正，更新磁强计校正后的系统状态估计值和状态估计协方差。
本发明步骤1中，所述系统状态方程、加速度计量测方程和磁强计量测方程具体为：
步骤a、根据系统的陀螺仪输出角速度计算陀螺仪的姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统的状态方程：
Figure BSA0000230788000000061
其中，
Figure BSA0000230788000000062
表示第k时刻根据陀螺仪输出得到的姿态四元数向量，
Figure BSA0000230788000000063
表示陀螺仪姿态四元数向量分量；这里，
Figure BSA0000230788000000064
表示姿态参数中的旋转角度，
Figure BSA0000230788000000065
分别表示姿态参数中x轴、y轴和z轴的轴向量分量；ΔT为陀螺仪数据的采样周期，I是4*4单位矩阵，
Figure BSA0000230788000000066
为第k-1时刻陀螺仪输出的3*1角速度向量，
Figure BSA0000230788000000067
是第k-1陀螺仪输出角速度组成的4*4矩阵；Ak-1为第k-1时刻的状态转移矩阵，且大小为4*4矩阵；Wk-1为第k-1时刻的系统噪声向量，且大小为4*1向量。
步骤b、根据加速度计输出加速度、磁强计输出磁场，建立系统加速度计量测方程和系统磁强计量测方程，分别如下：
Figure BSA0000230788000000071
Figure BSA0000230788000000072
其中，
Figure BSA0000230788000000073
表示第k时刻加速度计量测值，
Figure BSA0000230788000000074
表示第k时刻磁强计量测值，
Figure BSA0000230788000000075
为第k时刻导航坐标系到载体坐标系的四元数转换矩阵，
Figure BSA0000230788000000076
为第k时刻加速度计量测噪声，
Figure BSA0000230788000000077
为第k时刻磁强计量测噪声，an为导航坐标系下重力加速度的参考值，hn为导航坐标系下的磁场值，且hn＝[cosβ，0，-sinβ]T·B；这里，磁倾角β为地磁场与水平面之间的夹角，B为地磁场强度。
本发明方法步骤2中，所述UT形式采样具体为：
Figure BSA0000230788000000078
当i＝0时；
Figure BSA0000230788000000079
当i＝1，2，...，n时；
Figure BSA00002307880000000710
当i＝n+1，...，2n时；
其中，xi表示第i个陀螺仪采样点状态，
Figure BSA00002307880000000711
为系统状态，S表示系统状态误差协方差阵，n为系统状态向量维数，参数λ＝α2(n+K)-n；这里，参数α取值介于0～1之间，K为比例因子；按照UT形式采样，获取陀螺仪采样点向量xi，i表示陀螺仪采样点序数，i＝0，1，2，...，2n+1，且i为自然数。
步骤2中，所述超球体单行采样规则具体步骤为：
步骤a、计算超球体单行采样初始点
Figure BSA00002307880000000712
的均值权重：
Figure BSA00002307880000000713
Figure BSA00002307880000000714
其中，参数wm取值均介于0～1之间，参数b为常数。
步骤b、计算其余n+1个超球体采样点
Figure BSA00002307880000000715
的均值权重
Figure BSA00002307880000000716
和协方差权重
Figure BSA0000230788000000081
Figure BSA0000230788000000082
Figure BSA0000230788000000083
其中，j表示加速度计或磁强计的采样点序数，j＝1，...，n+1，且j为自然数。
步骤c、超球体采样的n维迭代形式为：
Figure BSA0000230788000000084
步骤d、根据步骤c，从超球体采样的n维迭代形式，分别获取加速度计采样点状态向量
Figure BSA0000230788000000085
磁强计采样点状态向量
Figure BSA0000230788000000086
Figure BSA0000230788000000087
Figure BSA0000230788000000088
本发明方法步骤3中，所述第k时刻系统状态预测值和第k时刻系统状态预测误差协方差阵具体为：
Figure BSA0000230788000000089
Figure BSA00002307880000000810
qi，k＝Ak-1·xi，k-1+Wk-1，其中i＝0，...2n；
Figure BSA00002307880000000811
其中i＝1，...2n；
Figure BSA00002307880000000812
其中i＝1，...2n；
Figure BSA00002307880000000813
Figure BSA00002307880000000814
其中，
Figure BSA00002307880000000815
为第k时刻系统状态预测值，qi，k为第k时刻的第i个系统状态，
Figure BSA0000230788000000091
为第k时刻系统状态预测误差协方差阵，qr{}表示正交三角分解，cholupdate{}表示矩阵乔莱斯基分解，Qk-1为第k-1时刻系统的噪声协方差阵，
Figure BSA0000230788000000092
表示UT形式采样初始点均值权重，
Figure BSA0000230788000000093
表示UT形式采样初始点协方差权重，
Figure BSA0000230788000000094
表示UT形式其余采样点均值权重，
Figure BSA0000230788000000095
表示UT形式其余采样点协方差权重。
本发明方法步骤4中，所述加速度计量测预测残差和磁强计量测预测残差具体为：
根据加速度计量测预测值
Figure BSA0000230788000000096
和磁强计量测预测值
Figure BSA0000230788000000097
按照加速度计在第k时刻输出值
Figure BSA0000230788000000098
磁强计在第k时刻输出值
Figure BSA0000230788000000099
其中，ax，k，ay，k，az，k分别对应加速度计x轴、y轴和z轴第k时刻的输出，hx，k，hy，k，hz，k分别对应磁强计x轴、y轴和z轴第k时刻的输出；计算加速度计第k时刻量测预测残差和磁强计第k时刻量测预测残差为：
Figure BSA00002307880000000910
Figure BSA00002307880000000911
其中，
Figure BSA00002307880000000912
表示第k时刻加速度计量测预测残差，
Figure BSA00002307880000000913
表示第k时刻磁强计量测预测残差。
本发明方法步骤5中，所述加速度计自适应校正因子和磁强计自适应校正因子的计算，包括如下具体步骤：
步骤a、计算加速度计量测残差自适应校正因子
Figure BSA00002307880000000914
和磁强计量测残差自适应校正因子
Figure BSA00002307880000000915
其中，tr(·)为矩阵的迹，|·|表示绝对值符号。
步骤b、在加速度计量测残差自适应校正因子前乘上第k和第k-1两个时刻加速度计输出值差值ΔAa的绝对值，得到加速度计自适应校正因子
Figure BSA0000230788000000101
Figure BSA0000230788000000102
在磁强计量测残差自适应校正因子前乘上第k和第k-1两个时刻磁强计输出值差值ΔHh的绝对值，得到磁强计自适应校正因子
Figure BSA0000230788000000103
ΔAa、ΔHh分别如下：
Figure BSA0000230788000000104
Figure BSA0000230788000000105
其中，ax，k-1，ay，k-1，az，k-1分别表示加速度计x轴、y轴和z轴第k-1时刻的输出加速度分量，hx，k-1，hy，k-1，hz，k-1分别表示磁强计x轴、y轴和z轴第k-1时刻的输出磁场分量，Δax，k，Δay，k，Δaz，k为加速度计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值，Δhx，k，Δhy，k，Δhz，k为磁强计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值。
本发明方法步骤6中，所述判断加速度计自适应校正因子和磁强计自适应校正因子的大小，执行姿态的自适应校正具体为：
当加速度计自适应校正因子
Figure BSA0000230788000000106
大于磁强计自适应校正因子
Figure BSA0000230788000000107
时，加速度计量测输出值和预测值之间的残差
Figure BSA0000230788000000108
变大，加速度计的量测输出受到量测噪声的影响，执行步骤7。
当磁强计自适应校正因子
Figure BSA0000230788000000109
大于加速度计自适应校正因子
Figure BSA00002307880000001010
时：磁强计量测输出值和预测值之间的残差
Figure BSA00002307880000001011
变大，磁强计的量测输出受到量测噪声的影响，执行步骤8。
本发明方法步骤7中，所述姿态校正方法，包括如下具体步骤：
步骤a、根据加速度计量测预测值和预测残差，对k时刻系统状态的预测值
Figure BSA0000230788000000111
和k时刻系统状态的预测协方差阵
Figure BSA0000230788000000112
进行校正，得到k时刻系统状态的估计值
Figure BSA0000230788000000113
和k时刻系统状态的误差协方差阵
Figure BSA0000230788000000114
步骤b、根据磁强计的量测预测值和预测残差，对经过加速度计第校正后的系统状态估计值
Figure BSA0000230788000000115
和状态误差协方差阵
Figure BSA0000230788000000116
进行校正，得到k时刻系统状态的估计值
Figure BSA0000230788000000117
和k时刻系统状态的误差协方差阵
Figure BSA0000230788000000118
本发明方法步骤8中，所述姿态校正方法，包括如下具体步骤：
步骤a、根据磁强计量测预测值和预测残差，对k时刻系统状态的预测值
Figure BSA0000230788000000119
和k时刻系统状态的预测协方差阵
Figure BSA00002307880000001110
进行校正，得到k时刻系统状态的估计值
Figure BSA00002307880000001111
和k时刻系统状态的误差协方差阵
Figure BSA00002307880000001112
步骤b、根据加速度计的量测预测值和预测残差，对经过磁强计校正后的系统状态估计值
Figure BSA00002307880000001113
和状态误差协方差阵
Figure BSA00002307880000001114
进行校正，得到k时刻系统状态的估计值
Figure BSA00002307880000001115
和k时刻系统状态的误差协方差阵
Figure BSA00002307880000001116
总之，本发明所述一种用于IMU系统的姿态自适应校正方法首先获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点，减少了算法的输入，提高了滤波效率；其次，对获取的采样点输入系统状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；再次，根据量测预测残差获取加速度计自适应校正因子
Figure BSA00002307880000001117
和磁强计自适应校正因子
Figure BSA00002307880000001118
对系统姿态进行自适应校正，以此类推，不断自适应校正系统的姿态。因此，本发明所述顺序校正的超球体采样自适应无迹卡尔曼滤波方法在计算效率和精确度都有较大的提升。
实际应用中，由于本发明所述一种用于IMU系统的姿态自适应校正方法不仅在获取系统采样点、加速度计量测和磁强计量测采样点时考虑到算法的计算效率性，还在校正过程中考虑了量测残差，进一步的提高了算法的收敛性原则，故本发明所述用于IMU系统的姿态自适应校正方法的计算效率和精确度都得到了进一步提升。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种用于IMU系统的姿态自适应校正方法，其特征在于，所述校正方法包括如下步骤：
步骤1、根据IMU系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程；
步骤2、根据UT形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数；
步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻；
步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差；
步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子
Figure FSA0000230787990000011
和磁强计自适应校正因子
Figure FSA0000230787990000012
其中，
Figure FSA0000230787990000013
表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，
Figure FSA0000230787990000014
表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔAa为第k和第k-1时刻加速度计输出值差值，ΔHh为第k和第k-1时刻磁强计输出值差值；
步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8；
步骤7、根据系统第k时刻加速度计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻磁强计量测预测值和预测残差，对经过加速度计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正；
步骤8、根据系统第k时刻磁强计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻加速度计量测预测值和预测残差，对经过磁强计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。
2.根据权利要求1所述的IMU系统的姿态自适应校正方法，其特征在于，步骤1包括如下具体步骤：
步骤a、根据系统的陀螺仪输出角速度计算陀螺仪的姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统的状态方程：
Figure FSA0000230787990000021
其中，
Figure FSA0000230787990000022
表示第k时刻根据陀螺仪输出得到的姿态四元数向量，
Figure FSA0000230787990000023
表示陀螺仪姿态四元数向量分量；这里，
Figure FSA0000230787990000024
表示姿态参数中的旋转角度，
Figure FSA0000230787990000025
分别表示姿态参数中x轴、y轴和z轴的轴向量分量；ΔT为陀螺仪数据的采样周期，I是4*4单位矩阵，
Figure FSA0000230787990000031
为第k-1时刻陀螺仪输出的3*1角速度向量，
Figure FSA0000230787990000032
是第k-1陀螺仪输出角速度组成的4*4矩阵，Ak-1为第k-1时刻的状态转移矩阵，且大小为4*4矩阵；Wk-1为第k-1时刻的系统噪声向量，且大小为4*1向量；
步骤b、根据加速度计输出加速度、磁强计输出磁场，建立系统加速度计量测方程和系统磁强计量测方程，分别如下：
Figure FSA0000230787990000033
Figure FSA0000230787990000034
其中，
Figure FSA0000230787990000035
表示第k时刻加速度计量测值，
Figure FSA0000230787990000036
表示第k时刻磁强计量测值，
Figure FSA0000230787990000037
为第k时刻导航坐标系到载体坐标系的四元数转换矩阵，
Figure FSA0000230787990000038
为第k时刻加速度计量测噪声，
Figure FSA0000230787990000039
为第k时刻磁强计量测噪声，an为导航坐标系下重力加速度的参考值，hn为导航坐标系下的磁场值，且hn＝[cosβ，0，-sinβ]T·B；这里，磁倾角β为地磁场与水平面之间的夹角，B为地磁场强度。
3.根据权利要求1或2所述的IMU系统的姿态自适应校正方法，其特征在于，步骤2中，所述UT形式采样为：
Figure FSA00002307879900000310
当i＝0时；
Figure FSA00002307879900000311
当i＝1，2，...，n时；
Figure FSA00002307879900000312
当i＝n+1，...，2n时；
其中，xi表示第i个陀螺仪采样点状态，
Figure FSA00002307879900000313
为系统状态，S表示系统状态误差协方差阵，n为系统状态向量维数，参数λ＝α2(n+K)-n；这里，参数α取值介于0～1之间，K为比例因子；按照UT形式采样，获取陀螺仪采样点状态向量的集合{xi}，i表示陀螺仪采样点序数，i＝0，1，2，...，2n，且i为自然数；
步骤2中，所述的超球体单行采样规则，包括如下具体步骤：
步骤a、计算超球体单行采样初始点
Figure FSA00002307879900000314
的均值权重：
Figure FSA0000230787990000041
其中，参数wm取值均介于0～1之间，参数b为常数；
步骤b、计算其余n+1个超球体采样点
Figure FSA0000230787990000042
的均值权重
Figure FSA0000230787990000043
和协方差权重
Figure FSA0000230787990000044
Figure FSA0000230787990000045
Figure FSA0000230787990000046
步骤c、超球体采样的n维迭代形式为：
Figure FSA0000230787990000047
步骤d、根据步骤c，从超球体采样的n维迭代形式，分别获取n+2个加速度计采样点状态向量
Figure FSA0000230787990000048
n+2个磁强计采样点状态向量
Figure FSA0000230787990000049
Figure FSA00002307879900000410
Figure FSA00002307879900000411
其中，j表示加速度计或磁强计的采样点序数，j＝1，...，n+1，且j为自然数。
4.根据权利要求3所述的IMU系统的姿态自适应校正方法，其特征在于，步骤3中，所述第k时刻系统状态预测值和第k时刻系统状态预测误差协方差阵，分别如下：
Figure FSA00002307879900000412
Figure FSA00002307879900000413
qi，k＝Ak-1·xi，k-1+Wk-1，其中i＝0，...2n
Figure FSA0000230787990000051
其中i＝1，...2n
Figure FSA0000230787990000052
其中i＝1，...2n
Figure FSA0000230787990000053
Figure FSA0000230787990000054
其中，
Figure FSA0000230787990000055
为第k时刻系统状态预测值，qi，k为第k时刻第i个系统状态，
Figure FSA0000230787990000056
为第k时刻系统状态预测误差协方差阵，qr{ }表示正交三角分解，cholupdate{ }表示矩阵乔莱斯基分解，Qk-1为第k-1时刻系统的噪声协方差阵，
Figure FSA0000230787990000057
表示UT形式采样初始点均值权重，
Figure FSA0000230787990000058
表示UT形式采样初始点协方差权重，
Figure FSA0000230787990000059
表示UT形式其余采样点均值权重，
Figure FSA00002307879900000510
表示UT形式其余采样点协方差权重；
步骤3中、所述加速度计的量测预测值和磁强计的量测预测值，是将超球体采样得到的加速度计采样点状态向量
Figure FSA00002307879900000511
和磁强计采样点状态向量
Figure FSA00002307879900000512
分别输入加速度计量测方程和磁强计量测方程进行预测，获得第k时刻加速度计量测预测值
Figure FSA00002307879900000513
第k时刻磁强计量测预测值
Figure FSA00002307879900000514
Figure FSA00002307879900000515
当j＝0，1，...n+1时；
其中，
Figure FSA00002307879900000516
5.根据权利要求4所述的IMU系统的姿态自适应校正方法，其特征在于，步骤4中，所述加速度计量测预测残差和磁强计量测预测残差具体为：
根据加速度计量测预测值
Figure FSA00002307879900000517
和磁强计量测预测值
Figure FSA00002307879900000518
按照加速度计在第k时刻输出值
Figure FSA00002307879900000519
磁强计在第k时刻输出值
Figure FSA00002307879900000520
其中，ax，k，ay，k，az，k分别对应加速度计x轴、y轴和z轴第k时刻的输出，hx，k，hy，k，hz，k分别对应磁强计x轴、y轴和z轴第k时刻的输出；计算加速度计第k时刻量测预测残差和磁强计第k时刻量测预测残差为：
Figure FSA0000230787990000061
Figure FSA0000230787990000062
其中，
Figure FSA0000230787990000063
表示第k时刻加速度计量测预测残差，
Figure FSA0000230787990000064
表示第k时刻磁强计量测预测残差。
6.根据权利要求5所述的IMU系统的姿态自适应校正方法，其特征在于，步骤5中，所述加速度计自适应校正因子和磁强计自适应校正因子的计算，包括如下具体步骤：
步骤a、计算加速度计量测残差自适应校正因子
Figure FSA0000230787990000065
和磁强计量测残差自适应校正因子
Figure FSA0000230787990000066
其中，tr(·)为矩阵的迹，|·|表示绝对值符号；
步骤b、在加速度计量测残差自适应校正因子前乘上第k和第k-1两个时刻加速度计输出值差值ΔAa的绝对值，得到加速度计自适应校正因子
Figure FSA0000230787990000067
Figure FSA0000230787990000068
在磁强计量测残差自适应校正因子前乘上第k和第k-1两个时刻磁强计输出值差值ΔHh的绝对值，得到磁强计自适应校正因子
Figure FSA0000230787990000069
ΔAa、ΔHh分别如下：
Figure FSA00002307879900000610
Figure FSA00002307879900000611
其中，ax，k-1，ay，k-1，az，k-1分别表示加速度计x轴、y轴和z轴第k-1时刻的输出加速度分量，hx，k-1，hy，k-1，hz，k-1分别表示磁强计x轴、y轴和z轴第k-1时刻的输出磁场分量，Δax，k，Δay，k，Δaz，k为加速度计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值，Δhx，k，Δhy，k，Δhz，k为磁强计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值。
CN202110073075.2A 2021-01-10 2021-01-10 一种用于imu系统的姿态自适应校正方法 Active CN112729348B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110073075.2A CN112729348B (zh) 2021-01-10 2021-01-10 一种用于imu系统的姿态自适应校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110073075.2A CN112729348B (zh) 2021-01-10 2021-01-10 一种用于imu系统的姿态自适应校正方法

Publications (2)

Publication Number Publication Date
CN112729348A true CN112729348A (zh) 2021-04-30
CN112729348B CN112729348B (zh) 2023-11-28

Family

ID=75592589

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110073075.2A Active CN112729348B (zh) 2021-01-10 2021-01-10 一种用于imu系统的姿态自适应校正方法

Country Status (1)

Country Link
CN (1) CN112729348B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114184211A (zh) * 2021-12-27 2022-03-15 北京计算机技术及应用研究所 一种惯导可靠性试验中性能变化机理一致性判定方法
CN117027764A (zh) * 2022-05-20 2023-11-10 中国石油天然气集团有限公司 钻井定位装置、方法和系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN110440756A (zh) * 2019-06-28 2019-11-12 中国船舶重工集团公司第七0七研究所 一种惯导系统姿态估计方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111649747A (zh) * 2020-06-15 2020-09-11 中国计量科学研究院 一种基于imu的自适应ekf姿态测量改进方法
CN112013836A (zh) * 2020-08-14 2020-12-01 北京航空航天大学 一种基于改进自适应卡尔曼滤波的航姿参考系统算法
WO2020253854A1 (zh) * 2019-06-21 2020-12-24 台州知通科技有限公司 移动机器人姿态角解算方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
WO2020253854A1 (zh) * 2019-06-21 2020-12-24 台州知通科技有限公司 移动机器人姿态角解算方法
CN110440756A (zh) * 2019-06-28 2019-11-12 中国船舶重工集团公司第七0七研究所 一种惯导系统姿态估计方法
CN111649747A (zh) * 2020-06-15 2020-09-11 中国计量科学研究院 一种基于imu的自适应ekf姿态测量改进方法
CN112013836A (zh) * 2020-08-14 2020-12-01 北京航空航天大学 一种基于改进自适应卡尔曼滤波的航姿参考系统算法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114184211A (zh) * 2021-12-27 2022-03-15 北京计算机技术及应用研究所 一种惯导可靠性试验中性能变化机理一致性判定方法
CN117027764A (zh) * 2022-05-20 2023-11-10 中国石油天然气集团有限公司 钻井定位装置、方法和系统
CN117027764B (zh) * 2022-05-20 2024-02-09 中国石油天然气集团有限公司 钻井定位装置、方法和系统

Also Published As

Publication number Publication date
CN112729348B (zh) 2023-11-28

Similar Documents

Publication Publication Date Title
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考系统算法
Tong et al. Adaptive EKF based on HMM recognizer for attitude estimation using MEMS MARG sensors
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN109974714A (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110174121A (zh) 一种基于地磁场自适应修正的航姿系统姿态解算方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN107063254B (zh) 一种陀螺地磁组合的姿态解算方法
CN112729348B (zh) 一种用于imu系统的姿态自适应校正方法
Troni et al. Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular Rate Sensors: Theory and Comparative Experimental Evaluation.
CN110231029A (zh) 一种水下机器人多传感器融合数据处理方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN109000639B (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
CN111413651A (zh) 一种磁场总场的补偿方法、装置、系统及存储介质
Suh Simple-structured quaternion estimator separating inertial and magnetic sensor effects
CN104819717B (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
Guan et al. Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN106595669A (zh) 一种旋转体姿态解算方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
Vertzberger et al. Attitude and heading adaptive estimation using a data driven approach
CN115839726A (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