CN112729348A - 一种用于imu系统的姿态自适应校正方法 - Google Patents
一种用于imu系统的姿态自适应校正方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000005259 measurement Methods 0.000 claims abstract description 119
- 238000005070 sampling Methods 0.000 claims abstract description 71
- 230000003044 adaptive effect Effects 0.000 claims abstract description 49
- 238000004364 calculation method Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 50
- 239000013598 vector Substances 0.000 claims description 25
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 230000005358 geomagnetic field Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 2
- 238000001914 filtration Methods 0.000 description 9
- 230000008569 process Effects 0.000 description 3
- 239000003245 coal Substances 0.000 description 1
- 238000005553 drilling Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- ONUFESLQCSAYKA-UHFFFAOYSA-N iprodione Chemical compound O=C1N(C(=O)NC(C)C)CC(=O)N1C1=CC(Cl)=CC(Cl)=C1 ONUFESLQCSAYKA-UHFFFAOYSA-N 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
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
Description
技术领域
本发明涉及姿态参数估计领域,特别是涉及一种用于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、根据加速度计量测预测残差和磁强计量测预测残差,获取加速度计自适应校正因子和磁强计自适应校正因子其中,表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子,表示从第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是本发明所述一种用于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、根据加速度计量测预测残差和磁强计量测预测残差,获取加速度计自适应校正因子和磁强计自适应校正因子其中,表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子,表示从第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、根据系统的陀螺仪输出角速度计算陀螺仪的姿态四元数,将得到的陀螺仪姿态四元数作为系统状态,建立系统的状态方程:
其中,表示第k时刻根据陀螺仪输出得到的姿态四元数向量,表示陀螺仪姿态四元数向量分量;这里,表示姿态参数中的旋转角度,分别表示姿态参数中x轴、y轴和z轴的轴向量分量;ΔT为陀螺仪数据的采样周期,I是4*4单位矩阵,为第k-1时刻陀螺仪输出的3*1角速度向量,是第k-1陀螺仪输出角速度组成的4*4矩阵;Ak-1为第k-1时刻的状态转移矩阵,且大小为4*4矩阵;Wk-1为第k-1时刻的系统噪声向量,且大小为4*1向量。
步骤b、根据加速度计输出加速度、磁强计输出磁场,建立系统加速度计量测方程和系统磁强计量测方程,分别如下:
其中,表示第k时刻加速度计量测值,表示第k时刻磁强计量测值,为第k时刻导航坐标系到载体坐标系的四元数转换矩阵,为第k时刻加速度计量测噪声,为第k时刻磁强计量测噪声,an为导航坐标系下重力加速度的参考值,hn为导航坐标系下的磁场值,且hn=[cosβ,0,-sinβ]T·B;这里,磁倾角β为地磁场与水平面之间的夹角,B为地磁场强度。
本发明方法步骤2中,所述UT形式采样具体为:
其中,xi表示第i个陀螺仪采样点状态,为系统状态,S表示系统状态误差协方差阵,n为系统状态向量维数,参数λ=α2(n+K)-n;这里,参数α取值介于0~1之间,K为比例因子;按照UT形式采样,获取陀螺仪采样点向量xi,i表示陀螺仪采样点序数,i=0,1,2,...,2n+1,且i为自然数。
步骤2中,所述超球体单行采样规则具体步骤为:
其中,参数wm取值均介于0~1之间,参数b为常数。
其中,j表示加速度计或磁强计的采样点序数,j=1,...,n+1,且j为自然数。
步骤c、超球体采样的n维迭代形式为:
本发明方法步骤3中,所述第k时刻系统状态预测值和第k时刻系统状态预测误差协方差阵具体为:
qi,k=Ak-1·xi,k-1+Wk-1,其中i=0,...2n;
其中,为第k时刻系统状态预测值,qi,k为第k时刻的第i个系统状态,为第k时刻系统状态预测误差协方差阵,qr{}表示正交三角分解,cholupdate{}表示矩阵乔莱斯基分解,Qk-1为第k-1时刻系统的噪声协方差阵,表示UT形式采样初始点均值权重,表示UT形式采样初始点协方差权重,表示UT形式其余采样点均值权重,表示UT形式其余采样点协方差权重。
本发明方法步骤4中,所述加速度计量测预测残差和磁强计量测预测残差具体为:
根据加速度计量测预测值和磁强计量测预测值按照加速度计在第k时刻输出值磁强计在第k时刻输出值其中,ax,k,ay,k,az,k分别对应加速度计x轴、y轴和z轴第k时刻的输出,hx,k,hy,k,hz,k分别对应磁强计x轴、y轴和z轴第k时刻的输出;计算加速度计第k时刻量测预测残差和磁强计第k时刻量测预测残差为:
本发明方法步骤5中,所述加速度计自适应校正因子和磁强计自适应校正因子的计算,包括如下具体步骤:
步骤b、在加速度计量测残差自适应校正因子前乘上第k和第k-1两个时刻加速度计输出值差值ΔAa的绝对值,得到加速度计自适应校正因子 在磁强计量测残差自适应校正因子前乘上第k和第k-1两个时刻磁强计输出值差值ΔHh的绝对值,得到磁强计自适应校正因子ΔAa、ΔHh分别如下:
其中,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中,所述判断加速度计自适应校正因子和磁强计自适应校正因子的大小,执行姿态的自适应校正具体为:
本发明方法步骤7中,所述姿态校正方法,包括如下具体步骤:
本发明方法步骤8中,所述姿态校正方法,包括如下具体步骤:
总之,本发明所述一种用于IMU系统的姿态自适应校正方法首先获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程;获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点,减少了算法的输入,提高了滤波效率;其次,对获取的采样点输入系统状态方程,加速度计量测方程和磁强计量测方程,进行状态预测;根据状态预测值获取量测预测残差;再次,根据量测预测残差获取加速度计自适应校正因子和磁强计自适应校正因子对系统姿态进行自适应校正,以此类推,不断自适应校正系统的姿态。因此,本发明所述顺序校正的超球体采样自适应无迹卡尔曼滤波方法在计算效率和精确度都有较大的提升。
实际应用中,由于本发明所述一种用于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、根据加速度计量测预测残差和磁强计量测预测残差,获取加速度计自适应校正因子和磁强计自适应校正因子其中,表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子,表示从第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、根据系统的陀螺仪输出角速度计算陀螺仪的姿态四元数,将得到的陀螺仪姿态四元数作为系统状态,建立系统的状态方程:
其中,表示第k时刻根据陀螺仪输出得到的姿态四元数向量,表示陀螺仪姿态四元数向量分量;这里,表示姿态参数中的旋转角度,分别表示姿态参数中x轴、y轴和z轴的轴向量分量;ΔT为陀螺仪数据的采样周期,I是4*4单位矩阵,为第k-1时刻陀螺仪输出的3*1角速度向量,是第k-1陀螺仪输出角速度组成的4*4矩阵,Ak-1为第k-1时刻的状态转移矩阵,且大小为4*4矩阵;Wk-1为第k-1时刻的系统噪声向量,且大小为4*1向量;
步骤b、根据加速度计输出加速度、磁强计输出磁场,建立系统加速度计量测方程和系统磁强计量测方程,分别如下:
3.根据权利要求1或2所述的IMU系统的姿态自适应校正方法,其特征在于,步骤2中,所述UT形式采样为:
其中,xi表示第i个陀螺仪采样点状态,为系统状态,S表示系统状态误差协方差阵,n为系统状态向量维数,参数λ=α2(n+K)-n;这里,参数α取值介于0~1之间,K为比例因子;按照UT形式采样,获取陀螺仪采样点状态向量的集合{xi},i表示陀螺仪采样点序数,i=0,1,2,...,2n,且i为自然数;
步骤2中,所述的超球体单行采样规则,包括如下具体步骤:
其中,参数wm取值均介于0~1之间,参数b为常数;
步骤c、超球体采样的n维迭代形式为:
其中,j表示加速度计或磁强计的采样点序数,j=1,...,n+1,且j为自然数。
4.根据权利要求3所述的IMU系统的姿态自适应校正方法,其特征在于,步骤3中,所述第k时刻系统状态预测值和第k时刻系统状态预测误差协方差阵,分别如下:
qi,k=Ak-1·xi,k-1+Wk-1,其中i=0,...2n
其中,为第k时刻系统状态预测值,qi,k为第k时刻第i个系统状态,为第k时刻系统状态预测误差协方差阵,qr{ }表示正交三角分解,cholupdate{ }表示矩阵乔莱斯基分解,Qk-1为第k-1时刻系统的噪声协方差阵,表示UT形式采样初始点均值权重,表示UT形式采样初始点协方差权重,表示UT形式其余采样点均值权重,表示UT形式其余采样点协方差权重;
步骤3中、所述加速度计的量测预测值和磁强计的量测预测值,是将超球体采样得到的加速度计采样点状态向量和磁强计采样点状态向量分别输入加速度计量测方程和磁强计量测方程进行预测,获得第k时刻加速度计量测预测值第k时刻磁强计量测预测值
6.根据权利要求5所述的IMU系统的姿态自适应校正方法,其特征在于,步骤5中,所述加速度计自适应校正因子和磁强计自适应校正因子的计算,包括如下具体步骤:
步骤b、在加速度计量测残差自适应校正因子前乘上第k和第k-1两个时刻加速度计输出值差值ΔAa的绝对值,得到加速度计自适应校正因子 在磁强计量测残差自适应校正因子前乘上第k和第k-1两个时刻磁强计输出值差值ΔHh的绝对值,得到磁强计自适应校正因子ΔAa、ΔHh分别如下:
其中,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时刻的输出差值。
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)
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)
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 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
-
2021
- 2021-01-10 CN CN202110073075.2A patent/CN112729348B/zh active Active
Patent Citations (8)
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)
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 |