CN108871323B - 一种低成本惯性传感器在机动环境下的高精度导航方法 - Google Patents

一种低成本惯性传感器在机动环境下的高精度导航方法 Download PDF

Info

Publication number
CN108871323B
CN108871323B CN201810380903.5A CN201810380903A CN108871323B CN 108871323 B CN108871323 B CN 108871323B CN 201810380903 A CN201810380903 A CN 201810380903A CN 108871323 B CN108871323 B CN 108871323B
Authority
CN
China
Prior art keywords
carrier
matrix
current
vector
state
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
CN201810380903.5A
Other languages
English (en)
Other versions
CN108871323A (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.)
Allwinner Technology Co Ltd
Original Assignee
Allwinner Technology 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 Allwinner Technology Co Ltd filed Critical Allwinner Technology Co Ltd
Priority to CN201810380903.5A priority Critical patent/CN108871323B/zh
Publication of CN108871323A publication Critical patent/CN108871323A/zh
Application granted granted Critical
Publication of CN108871323B publication Critical patent/CN108871323B/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/165—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 combined with non-inertial navigation instruments

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

本发明实施例公开了一种低成本惯性传感器在机动环境下的高精度导航方法。所述方法包括：获取惯性传感器IMU输出信号；使用快速傅立叶变换对惯性传感器IMU输出信号进行低通滤波；结合上一周期IMU的信号量进行圆锥补偿和旋转划桨补偿；基于载体电机转速和陀螺仪角速度对载体运动状态进行判断，确定是否进行姿态校正；最终，进行速度和位置的解算，确定载体当前的速度和位置。本发明实施例的技术方案充分考虑载体运动状态，能够克服载体震动和抖动对IMU造成的影响，使得姿态解算更为准确，鲁棒性强，并具有较高的通用性和扩展性。

Description

一种低成本惯性传感器在机动环境下的高精度导航方法
技术领域
本发明实施例涉及导航领域，尤其涉及一种低成本惯性传感器在机动环境下的高精度导航方法。
背景技术
惯性传感器IMU被设置在载体平台上，受到平台震动和抖动的影响，会产生大量的噪声，产生的噪声会反映在传感器的数据中，导致惯性传感器IMU的性能下降，不能满足高精度的导航需求。目前，克服上述噪声影响的方式通常有两种：一种是软件优化的方式，一种是对载体平台优化的方式。对于大多数低成本惯性传感器来说，受制于处理单元运算能力较差，无法执行复杂的算法，从而多采用对载体平台优化的方式。但是，平台、设备的多样性，以及需要面对多种复杂的运动环境，都对优化载体平台带来了挑战，往往需要设计复杂的避震装置，并且通用性和扩展性都较差。平台设计的复杂性，加之算法层面的简单、鲁棒性差，这些综合因素的影响使得低成本惯性导航方案在短期内产生较大的误差。对于现有技术的上述缺点，目前还没有一种完善的解决方案，急需一种针对低成本惯性传感器的、能够克服机动环境下的噪声干扰的高精度、低运算量的导航方法。
发明内容
有鉴于此，本发明实施例提供了一种低成本惯性传感器在机动环境下的高精度导航方法，以解决现有技术中优化载体平台复杂度高、通用性差以及软件优化算法计算量大、移植性差的问题，本发明所述方法可以克服机动环境下震动、抖动的影响，提高导航算法的精度，同时降低计算量。
本发明实施例提供了一种低成本惯性传感器在机动环境下的高精度导航方法，所述方法包含以下步骤：
步骤一：信号获取，获取惯性传感器的输出信号，包括加速度和角速度；
步骤二：数据预处理，采用快速傅立叶变换对所述输出信号进行低通滤波；
步骤三：信号补偿，基于上一周期惯性传感器的输出信号对当前信号进行补偿，所述信号补偿包括圆锥补偿和旋转划桨补偿；
步骤四：获取载体电机转速和陀螺仪角速度，确定载体当前运动状态，根据所述运动状态判断是否进行姿态校正；
步骤五：解算所述载体当前的速度和位置。
优选地，所述步骤三中，通过圆锥补偿可获得经过补偿之后的旋转矢量，所述旋转矢量经转换后可得载体的姿态角，分别为滚转角、俯仰角和航向角，分别记作φ,θ,ψ，具体计算方式为：
Figure GDA0001719069740000021
Figure GDA0001719069740000022
其中β(T)为经过补偿之后的旋转矢量，Δα为当前的角增量采样，Δα'为前一周期的角增量采样，ω(t)为角速度，T为预设周期。
优选地，所述步骤三中，通过旋转划桨补偿可以获得经过补偿后的速度增量dv(T)，通过所述dv(T)可以求解得到三轴加速度，具体计算方式为：
Figure GDA0001719069740000023
Figure GDA0001719069740000024
Figure GDA0001719069740000025
其中，Δv为当前的速度增量采样，Δv'为前一周期的速度增量采样，其中Δr为速度旋转误差，
Figure GDA0001719069740000031
为速度划桨误差。
优选地，当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置。
优选地，当所述载体包含4个驱动电机时，所述获取载体电机转速，具体包括：获取当前载体4个驱动电机的转速，分别为m1,m2,m3,m4；
设置电机的最小稳定值为m_eps；
如果前方两个电机转速同后方两电机转速差大于电机的最小稳定值，或者左侧两个电机转速同右侧两电机转速差大于电机的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
优选地，获取当前三轴角速度分别为wN，wE，wD；
角速度的最小稳定值为w_eps；
如果当前三轴的角速度的绝对值大于角速度的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
优选地，所述确定载体当前运动状态，还包括：获取当前失准角；
设置失准角的阈值为att_eps；
如果当前失准角大于所述阈值，则确定载体当前运动状态为非平稳运动状态。
优选地，采用基于失准角的卡尔曼滤波确定当前失准角；所述基于失准角的卡尔曼滤波采用东向水平失准角
Figure GDA0001719069740000032
北向水平失准角
Figure GDA0001719069740000033
和陀螺漂移ωbx,ωby作为系统状态，记作
Figure GDA0001719069740000034
以水平加速度fx,fy作为量测状态，记作Z＝[fy fx]T；
所述陀螺漂移ωbx,ωby和水平加速度fx,fy基于载体坐标系，所述载体坐标系三轴表示为xyz，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；，所述东向水平失准角
Figure GDA0001719069740000041
北向水平失准角
Figure GDA0001719069740000042
基于NED导航坐标系；
设滤波器系统的状态方程为：
Figure GDA0001719069740000043
其中
Figure GDA0001719069740000044
为系统状态的导数，f(x)为系统状态的函数，w为系统噪声，Z为系统的量测状态，h(x)为系统的量测状态函数，v为量测噪声；记失准角为
Figure GDA0001719069740000045
陀螺漂移为ωb＝[ωbx ωby 0]T，当前时刻的载体姿态为att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；
根据失准角方程，上式可表示为
Figure GDA0001719069740000046
其中，
Figure GDA0001719069740000047
为载体坐标系向导航坐标系的转换矩阵，记作
Figure GDA0001719069740000048
简化转换矩阵为以下形式
Figure GDA0001719069740000049
对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，得到线性化后的状态方程：
Figure GDA00017190697400000410
其中，向量X为上一时刻系统状态向量，向量
Figure GDA0001719069740000051
为系统状态向量的一步预测值，矩阵F为系统传递矩阵，向量W,V为系统噪声向量，向量
Figure GDA0001719069740000052
为系统量测向量，矩阵H为量测矩阵，其中
Figure GDA0001719069740000053
Figure GDA0001719069740000054
优选地，所述基于失准角的卡尔曼滤波包括时间更新和量测校正，所述进行姿态校正即为进行量测校正，以更新卡尔曼滤波的增益矩阵和量测噪声序列方差阵，消除累积误差。
优选地，所述时间更新的具体步骤包括：
Figure GDA0001719069740000055
Figure GDA0001719069740000056
其中，向量
Figure GDA0001719069740000057
为系统状态向量的一步预测值，向量
Figure GDA0001719069740000058
为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φk/k-1为一步转移矩阵，矩阵Pk/k-1为状态向量的一步预测方差矩阵，矩阵Pk/k-1为状态向量的上一时刻方差矩阵，矩阵Qk为系统噪声方差阵。
优选地，所述量测更新的具体步骤包括：
Figure GDA0001719069740000059
Figure GDA00017190697400000510
Pk＝(I-KkHk)Pk/k-1 (17)
其中，向量
Figure GDA00017190697400000511
为系统状态向量的当前最优估算值，矩阵Kk为增益矩阵，矩阵Hk为量测矩阵，矩阵Rk为量测噪声序列方差阵，矩阵I为单位矩阵。
优选地，所述解算所载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为Vk-1，
Figure GDA0001719069740000061
上一时刻加速度记为Ak-1，
Figure GDA0001719069740000062
上一时刻的位置记为Pk-1，
Figure GDA0001719069740000063
解算时间为Δt；
则当前时刻的速度Vk，位置Pk的计算方式为：
Vk＝Vk-1+Ak-1×Δt (18)
Pk＝Pk-1+Vk×Δt (19)。
本发明实施例提供了一种低成本惯性传感器在机动环境下的高精度导航方法，考虑载体运动、陀螺仪角速度和失准角三方面因素，确定是否进行姿态校正，能够有效地降低累积误差，提高导航的精确性，降低算法的复杂度；并且，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量。
附图说明
图1是本发明实施例一提供的一种低成本惯性传感器在机动环境下的高精度导航方法的流程图；
图2是本发明实施例二提供的一种基于低成本惯性传感器在机动环境下的位置速度更新方法的流程图；
图3是本发明实施例三提供的一种基于失准角的卡尔曼滤波姿态校正方法的流程图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。
另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。
实施例一
本发明实施例一具体可以应用在需要高精度导航的产品，如无人机定位，自动驾驶，数字城市，机器人导航等。图1为本发明实施例一提供的一种低成本惯性传感器在机动环境下的高精度导航方法的流程图。本实施例的方法具体包括：
110、步骤一：信号获取，获取惯性传感器的输出信号，包括加速度和角速度。
120、数据预处理，采用快速傅立叶变换对所述输出信号进行低通滤波。
130、信号补偿，基于上一周期惯性传感器的输出信号对当前信号进行补偿，所述信号补偿包括圆锥补偿和旋转划桨补偿。
在本实施例中，通过圆锥补偿可获得经过补偿之后的旋转矢量，所述旋转矢量经转换后可得载体的姿态角，分别为滚转角、俯仰角和航向角，分别记作φ,θ,ψ，具体计算方式为：
Figure GDA0001719069740000081
Figure GDA0001719069740000082
其中，β(T)为经过补偿之后的旋转矢量，Δα为当前的角增量采样，Δα'为前一周期的角增量采样，ω(t)为角速度，T为预设周期。
通过旋转划桨补偿可以获得经过补偿后的速度增量dv(T)，通过所述dv(T)可以求解得到三轴加速度，具体计算方式为：
Figure GDA0001719069740000083
Figure GDA0001719069740000084
Figure GDA0001719069740000085
其中，Δv为当前的速度增量采样，Δv'为前一周期的速度增量采样，其中Δr为速度旋转误差，
Figure GDA0001719069740000086
为速度划桨误差。
140、确定载体运动状态，获取载体电机转速和陀螺仪角速度，确定载体当前运动状态，根据所述运动状态判断是否进行姿态校正。
150、解算所述载体当前的速度和位置。
在本实施例中，当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置。
所述解算所述载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为Vk-1，
Figure GDA0001719069740000087
上一时刻加速度记为Ak-1，
Figure GDA0001719069740000088
上一时刻的位置记为Pk-1，
Figure GDA0001719069740000089
解算时间为Δt；
则当前时刻的速度Vk，位置Pk的计算方式为：
Vk＝Vk-1+Ak-1×Δt (1-6)
Pk＝Pk-1+Vk×Δt (1-7)。
本发明实施例一提供了一种低成本惯性传感器在机动环境下的高精度导航方法，通过对圆锥补偿和旋转划桨补偿进行适应性地改进，不对信号进行多周期的补偿处理，只运用上一周期的信号量并结合当前信号进行补偿，能够降低运算复杂度，同时提高精度；另外，基于载体电机转速、陀螺仪角速度进行运动状态判定，根据当前运动状态确定是否进行姿态校正，进一步地降低了运算量，提高了导航的精度。
实施例二
图2是本发明实施例二提供的一种基于低成本惯性传感器在机动环境下的位置速度更新方法的流程图。本实施例以上述实施例为基础进行优化，本实施例的方法具体包括：
210、获取信息，确定载体当前运动状态。
在本实施例中，获取载体电机转速，获取陀螺仪三轴角速度和失准角。
220、判断载体是否处于平稳状态。
在本实施例中，当所述载体包含4个驱动电机时，所述获取载体电机转速，具体包括：获取当前载体4个驱动电机的转速，分别为m1,m2,m3,m4；
设置电机的最小稳定值为m_eps；
如果前方两个电机转速同后方两电机转速差大于电机的最小稳定值，或者左侧两个电机转速同右侧两电机转速差大于电机的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
获取当前三轴角速度分别为wN，wE，wD；
角速度的最小稳定值为w_eps；
如果当前三轴的角速度的绝对值大于角速度的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
所述确定载体当前运动状态，还包括：获取当前失准角；
设置失准角的阈值为att_eps；
如果当前失准角大于所述阈值，则确定载体当前运动状态为非平稳运动状态。
采用基于失准角的卡尔曼滤波确定当前失准角；所述基于失准角的卡尔曼滤波采用东向水平失准角
Figure GDA0001719069740000101
北向水平失准角
Figure GDA0001719069740000102
和陀螺漂移
Figure GDA0001719069740000103
作为系统状态，记作
Figure GDA0001719069740000104
以水平加速度fx,fy作为量测状态，记作Z＝[fy fx]T；
所述陀螺漂移ωbx,ωby和水平加速度fx,fy基于载体坐标系，所述载体坐标系三轴表示为xyz，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；，所述东向水平失准角
Figure GDA0001719069740000105
北向水平失准角
Figure GDA0001719069740000106
基于NED导航坐标系；
设滤波器系统的状态方程为：
Figure GDA0001719069740000107
其中
Figure GDA0001719069740000108
为系统状态的导数，f(x)为系统状态的函数，w为系统噪声，Z为系统的量测状态，h(x)为系统的量测状态函数，v为量测噪声；记失准角为
Figure GDA0001719069740000109
陀螺漂移为ωb＝[ωbx ωby 0]T，当前时刻的载体姿态为att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；
根据失准角方程，上式可表示为
Figure GDA0001719069740000111
其中，
Figure GDA0001719069740000112
为载体坐标系向导航坐标系的转换矩阵，记作
Figure GDA0001719069740000113
简化转换矩阵为以下形式
Figure GDA0001719069740000114
对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，得到线性化后的状态方程：
Figure GDA0001719069740000115
其中，向量X为上一时刻系统状态向量，向量
Figure GDA0001719069740000118
为系统状态向量的一步预测值，矩阵F为系统传递矩阵，向量W,V为系统噪声向量，向量
Figure GDA0001719069740000119
为系统量测向量，矩阵H为量测矩阵，其中
Figure GDA0001719069740000116
Figure GDA0001719069740000117
230、姿态校正。
在本实施例中，当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置。
其中，基于失准角的卡尔曼滤波包括时间更新和量测校正，所述进行姿态校正即为进行量测校正，以更新卡尔曼滤波的增益矩阵和量测噪声序列方差阵，消除累积误差。
所述时间更新的具体步骤包括：
Figure GDA0001719069740000121
Figure GDA0001719069740000122
其中，向量
Figure GDA0001719069740000123
为系统状态向量的一步预测值，向量
Figure GDA0001719069740000124
为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φk/k-1为一步转移矩阵，矩阵Pk/k-1为状态向量的一步预测方差矩阵，矩阵Pk/k-1为状态向量的上一时刻方差矩阵，矩阵Qk为系统噪声方差阵。
所述量测校正的具体步骤包括：
Figure GDA0001719069740000125
Figure GDA0001719069740000126
Pk＝(I-KkHk)Pk/k-1 (2-12)
其中，向量
Figure GDA0001719069740000127
为系统状态向量的当前最优估算值，矩阵Kk为增益矩阵，矩阵Hk为量测矩阵，矩阵Rk为量测噪声序列方差阵，矩阵I为单位矩阵。
240、解算速度位置。
在本实施例中，所述解算所载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为Vk-1，
Figure GDA0001719069740000128
上一时刻加速度记为Ak-1，
Figure GDA0001719069740000129
上一时刻的位置记为Pk-1，
Figure GDA00017190697400001210
解算时间为Δt；
则当前时刻的速度Vk，位置Pk的计算方式为：
Vk＝Vk-1+Ak-1×Δt (2-13)
Pk＝Pk-1+Vk×Δt (2-14)。
本发明实施例二提供了一种基于低成本惯性传感器在机动环境下的位置速度更新方法，综合考虑载体电机转速、陀螺仪角速度和失准角三方面因素，确定是否进行姿态校正，能够有效地降低累积误差，提高导航的精确性，降低算法的复杂度；并且，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量。
实施例三
图3是本发明实施例三提供的一种基于失准角的卡尔曼滤波姿态校正方法的流程图。本实施例的方法具体包括：
310、获取惯性传感器的三轴加速度计的测量值。
320、判断载体是否处于加速状态。
在本实施例中，判断载体是否处于加速状态的步骤为：
步骤1：计算所述惯性传感器的三轴加速度计的矢量和
Figure GDA0001719069740000131
记为
Figure GDA0001719069740000132
其中fx,fy,fz分别为三轴加速度值；
步骤2：计算载体低机动状态下的三轴加速度；具体步骤为：
在载体低机动的状态下有
Figure GDA0001719069740000133
其中，重力向量为gn＝[0 0 g]T，g为当地重力加速度，f1为低机动状态下三轴加速度计输出的加速度向量，记作f1＝[fx1,fy1,fz1]T；
由式(2)可计算得出理想加速度fx1,fy1,fz1：
Figure GDA0001719069740000141
步骤3：定义一偏差数值为eps，分别计算
Figure GDA0001719069740000142
|fx1-fx|、|fy1-fy|、|fz1-fz|，若上述任一结果大于偏差数值eps，则载体处于加速状态；否则，载体处于非加速状态。
在载体处于非加速状态时，进行时间更新和量测修正，在载体处于加速状态时，仅进行时间更新
330、进行时间更新。
在本实施例中，所述时间更新的具体步骤包括：
Figure GDA0001719069740000143
Figure GDA0001719069740000144
其中，向量
Figure GDA0001719069740000145
为系统状态向量的一步预测值，向量
Figure GDA0001719069740000146
为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φk/k-1为一步转移矩阵，矩阵Pk/k-1为状态向量的一步预测方差矩阵，矩阵Pk/k-1为状态向量的上一时刻方差矩阵，矩阵Qk为系统噪声方差阵。
340、进行时间更新和量测修正。
在本实施例中，所述量测修正的具体步骤包括：
Figure GDA0001719069740000147
Figure GDA0001719069740000148
Pk＝(I-KkHk)Pk/k-1 (3-7)
其中，向量
Figure GDA0001719069740000149
为系统状态向量的当前最优估算值，矩阵Kk为增益矩阵，矩阵Hk为量测矩阵，矩阵Rk为量测噪声序列方差阵，矩阵I为单位矩阵。
350、确定载体姿态。
在本实施例中，确定载体的姿态包括确定载体的失准角。
本领域技术人员可以理解，本实施例中限定的进行量测校正的条件可以与实施例一或实施例二中限定的进行量测校正的条件相互组合，既可以仅满足任一条件即进行量测校正，也可以同时满足两者的条件才进行量测校正。对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。
本发明实施例三提供了一种基于失准角的卡尔曼滤波姿态更新方法，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量；并且，采用上述参数作为状态量和量测量，可以自适应地选择时间更新或量测修正，即使用当前加速度矢量和以及加速度计数值与理想值的差进行自适应卡尔曼滤波，进一步降低了运算量。
在此请注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (9)

1.一种低成本惯性传感器在机动环境下的高精度导航方法，其特征在于，所述方法包含以下步骤：
步骤一：信号获取，获取惯性传感器的输出信号，包括加速度和角速度；
步骤二：数据预处理，采用快速傅立叶变换对所述输出信号进行低通滤波；
步骤三：信号补偿，基于上一周期惯性传感器的输出信号对当前信号进行补偿，所述信号补偿包括圆锥补偿和旋转划桨补偿；
步骤四：获取载体电机转速、陀螺仪角速度和当前失准角，确定载体当前运动状态，根据所述运动状态判断是否进行姿态校正；
步骤五：解算所述载体当前的速度和位置；
当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置；
所述确定载体当前运动状态包括，如果当前失准角大于阈值，则确定载体当前运动状态为非平稳运动状态，设置失准角的阈值为att_eps；
获取载体当前失准角包括，采用基于失准角的卡尔曼滤波确定当前失准角；所述基于失准角的卡尔曼滤波采用东向水平失准角
Figure FDA0003106794860000011
北向水平失准角
Figure FDA0003106794860000012
和陀螺漂移ωbx,ωby作为系统状态，记作
Figure FDA0003106794860000013
以水平加速度fx,fy作为系统的量测状态，记作Z＝[fy fx]T；
所述陀螺漂移ωbx,ωby和水平加速度fx,fy基于载体坐标系，所述载体坐标系三轴表示为xyz，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；所述东向水平失准角
Figure FDA0003106794860000021
北向水平失准角
Figure FDA0003106794860000022
基于NED导航坐标系；
设滤波器系统的状态方程为：
Figure FDA0003106794860000023
其中
Figure FDA00031067948600000211
为系统状态的导数，f(X)为系统状态的函数，w为系统噪声，h(X)为系统的量测状态函数，v为量测噪声；记失准角为
Figure FDA0003106794860000024
陀螺漂移为ωb＝[ωbx ωby 0]T，当前时刻的载体姿态为att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；
根据失准角方程，上式表示为
Figure FDA0003106794860000025
其中，
Figure FDA0003106794860000026
为载体坐标系向导航坐标系的转换矩阵，记作
Figure FDA0003106794860000027
简化转换矩阵为以下形式
Figure FDA0003106794860000028
对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，得到线性化后的状态方程：
Figure FDA0003106794860000029
其中，向量X为系统状态向量，向量
Figure FDA00031067948600000210
为系统状态向量的导数，矩阵F为系统传递矩阵，向量W,V为系统噪声向量，向量Z为系统量测向量，矩阵H为量测矩阵，其中
Figure FDA0003106794860000031
Figure FDA0003106794860000032
2.根据权利要求1所述的方法，其特征在于，所述步骤三中，通过圆锥补偿获得经过补偿之后的旋转矢量，所述旋转矢量经转换后可得载体的姿态角，分别为滚转角、俯仰角和航向角，分别记作φ,θ,ψ，具体计算方式为：
Figure FDA0003106794860000033
Figure FDA0003106794860000034
其中β(T)为经过补偿之后的旋转矢量，Δα为当前的角增量采样，Δα'为前一周期的角增量采样，ω(t)为角速度，T为预设周期。
3.根据权利要求2所述的方法，其特征在于，所述步骤三中，通过旋转划桨补偿获得经过补偿后的速度增量dv(T)，通过所述dv(T)求解得到三轴加速度，具体计算方式为：
Figure FDA0003106794860000035
Figure FDA0003106794860000036
Figure FDA0003106794860000037
其中，Δv为当前的速度增量采样，Δv'为前一周期的速度增量采样，其中Δr为速度旋转误差，
Figure FDA0003106794860000038
为速度划桨误差。
4.根据权利要求1所述的方法，其特征在于，当所述载体包含4个驱动电机时，所述获取载体电机转速，具体包括：获取当前载体4个驱动电机的转速，分别为m1,m2,m3,m4；
设置电机的最小稳定值为m_eps；
如果前方两个电机转速同后方两电机转速差大于电机的最小稳定值，或者左侧两个电机转速同右侧两电机转速差大于电机的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
5.根据权利要求1所述的方法，其特征在于，获取当前三轴角速度分别为wN，wE，wD；
角速度的最小稳定值为w_eps；
如果当前三轴的角速度的绝对值大于角速度的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
6.根据权利要求1所述的方法，其特征在于，所述基于失准角的卡尔曼滤波包括时间更新和量测校正，所述进行姿态校正即为进行量测校正，以更新卡尔曼滤波的增益矩阵和量测噪声序列方差阵，消除累积误差。
7.根据权利要求6所述的方法，其特征在于，所述时间更新的具体步骤包括：
Figure FDA0003106794860000041
Figure FDA0003106794860000042
其中，向量
Figure FDA0003106794860000043
为系统状态向量的一步预测值，向量
Figure FDA0003106794860000044
为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φk/k-1为一步转移矩阵，矩阵Pk/k-1为状态向量的一步预测方差矩阵，矩阵Pk-1为状态向量的上一时刻方差矩阵，矩阵Qk-1为系统噪声方差阵。
8.根据权利要求7所述的方法，其特征在于，所述量测校正的具体步骤包括：
Figure FDA0003106794860000045
Figure FDA0003106794860000051
Pk＝(I-KkHk)Pk/k-1 (17)
其中，向量
Figure FDA0003106794860000052
为系统状态向量的当前最优估算值，矩阵Kk为增益矩阵，矩阵Hk为量测矩阵，矩阵Rk为量测噪声序列方差阵，矩阵I为单位矩阵。
9.根据权利要求1所述的方法，其特征在于，所述解算载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为Vk-1，
Figure FDA0003106794860000053
上一时刻加速度记为Ak-1，
Figure FDA0003106794860000054
上一时刻的位置记为Pk-1，
Figure FDA0003106794860000055
解算时间为Δt；
则当前时刻的速度Vk，位置Pk的计算方式为：
Vk＝Vk-1+Ak-1×Δt (18)
Pk＝Pk-1+Vk×Δt (19)。
CN201810380903.5A 2018-04-25 2018-04-25 一种低成本惯性传感器在机动环境下的高精度导航方法 Active CN108871323B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810380903.5A CN108871323B (zh) 2018-04-25 2018-04-25 一种低成本惯性传感器在机动环境下的高精度导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810380903.5A CN108871323B (zh) 2018-04-25 2018-04-25 一种低成本惯性传感器在机动环境下的高精度导航方法

Publications (2)

Publication Number Publication Date
CN108871323A CN108871323A (zh) 2018-11-23
CN108871323B true CN108871323B (zh) 2021-08-24

Family

ID=64327314

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810380903.5A Active CN108871323B (zh) 2018-04-25 2018-04-25 一种低成本惯性传感器在机动环境下的高精度导航方法

Country Status (1)

Country Link
CN (1) CN108871323B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111238530B (zh) * 2019-11-27 2021-11-23 南京航空航天大学 一种捷联惯性导航系统空中动基座初始对准方法
CN111307114B (zh) * 2019-11-29 2022-09-27 哈尔滨工程大学 基于运动参考单元的水面舰船水平姿态测量方法
CN111878064B (zh) * 2020-05-11 2024-04-05 中国科学院地质与地球物理研究所 一种姿态测量方法
CN113442171B (zh) * 2021-07-01 2022-06-24 南京蔚蓝智能科技有限公司 一种机器人动态稳定判别方法和动态自适应姿态控制方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102128624A (zh) * 2010-12-28 2011-07-20 浙江大学 一种高动态捷联惯性导航并行计算装置
CN102519458A (zh) * 2011-12-16 2012-06-27 浙江大学 一种光纤陀螺捷联惯导的划桨运动补偿方法
CN104655136A (zh) * 2015-02-17 2015-05-27 西安航天精密机电研究所 一种适用于激光捷联惯性导航系统的多凹点fir滤波方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8005635B2 (en) * 2007-08-14 2011-08-23 Ching-Fang Lin Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102128624A (zh) * 2010-12-28 2011-07-20 浙江大学 一种高动态捷联惯性导航并行计算装置
CN102519458A (zh) * 2011-12-16 2012-06-27 浙江大学 一种光纤陀螺捷联惯导的划桨运动补偿方法
CN104655136A (zh) * 2015-02-17 2015-05-27 西安航天精密机电研究所 一种适用于激光捷联惯性导航系统的多凹点fir滤波方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于对偶法捷联惯导圆锥与划桨算法一般结果;秦永元等;《中国惯性技术学报》;20080229;第16卷(第1期);全文 *
新的捷联惯性导航划桨误差补偿算法;张泽等;《吉林大学学报（工学版）》;20100930;第40卷(第5期);正文第1-3节 *
机抖式激光惯组高动态下导航误差分析;尤太华等;《航天控制》;20151231;第33卷(第6期);正文第1-3节 *

Also Published As

Publication number Publication date
CN108871323A (zh) 2018-11-23

Similar Documents

Publication Publication Date Title
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
CN108051866B (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN106814753B (zh) 一种目标位置矫正方法、装置及系统
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
JP6922641B2 (ja) 角速度導出装置および角速度導出方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN108592917B (zh) 一种基于失准角的卡尔曼滤波姿态估计方法
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN114061619B (zh) 一种基于在线标定的惯导系统姿态补偿方法
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112683269A (zh) 一种附有运动加速度补偿的marg姿态计算方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
CN104121930B (zh) 一种基于加表耦合的mems陀螺漂移误差的补偿方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN109506674B (zh) 一种加速度的校正方法及装置
CN111141285B (zh) 一种航空重力测量装置
JP4527171B2 (ja) 単一ｇｐｓと慣性データ（加速度，角速度）によるビークル姿勢角の計測方法
CN110375773B (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
GR01 Patent grant
GR01 Patent grant