本发明实施例涉及导航领域，尤其涉及一种低成本惯性传感器在机动环境下的高精度导航方法。
背景技术
惯性传感器imu被设置在载体平台上，受到平台震动和抖动的影响，会产生大量的噪声，产生的噪声会反映在传感器的数据中，导致惯性传感器imu的性能下降，不能满足高精度的导航需求。目前，克服上述噪声影响的方式通常有两种：一种是软件优化的方式，一种是对载体平台优化的方式。对于大多数低成本惯性传感器来说，受制于处理单元运算能力较差，无法执行复杂的算法，从而多采用对载体平台优化的方式。但是，平台、设备的多样性，以及需要面对多种复杂的运动环境，都对优化载体平台带来了挑战，往往需要设计复杂的避震装置，并且通用性和扩展性都较差。平台设计的复杂性，加之算法层面的简单、鲁棒性差，这些综合因素的影响使得低成本惯性导航方案在短期内产生较大的误差。对于现有技术的上述缺点，目前还没有一种完善的解决方案，急需一种针对低成本惯性传感器的、能够克服机动环境下的噪声干扰的高精度、低运算量的导航方法。
技术实现要素：
有鉴于此，本发明实施例提供了一种低成本惯性传感器在机动环境下的高精度导航方法，以解决现有技术中优化载体平台复杂度高、通用性差以及软件优化算法计算量大、移植性差的问题，本发明所述方法可以克服机动环境下震动、抖动的影响，提高导航算法的精度，同时降低计算量。
本发明实施例提供了一种低成本惯性传感器在机动环境下的高精度导航方法，所述方法包含以下步骤：
步骤一：信号获取，获取惯性传感器的输出信号，包括加速度和角速度；
步骤二：数据预处理，采用快速傅立叶变换对所述输出信号进行低通滤波；
步骤三：信号补偿，基于上一周期惯性传感器的输出信号对当前信号进行补偿，所述信号补偿包括圆锥补偿和旋转划桨补偿；
步骤四：获取载体电机转速和陀螺仪角速度，确定载体当前运动状态，根据所述运动状态判断是否进行姿态校正；
步骤五：解算所述载体当前的速度和位置。
优选地，所述步骤三中，通过圆锥补偿可获得经过补偿之后的旋转矢量，所述旋转矢量经转换后可得载体的姿态角，分别为滚转角、俯仰角和航向角，分别记作φ,θ,ψ，具体计算方式为：
其中β(t)为经过补偿之后的旋转矢量，δα为当前的角增量采样，δα'为前一周期的角增量采样，ω(t)为角速度，t为预设周期。
优选地，所述步骤三中，通过旋转划桨补偿可以获得经过补偿后的速度增量dv(t)，通过所述dv(t)可以求解得到三轴加速度，具体计算方式为：
其中，δv为当前的速度增量采样，δv'为前一周期的速度增量采样，其中δr为速度旋转误差，
优选地，当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置。
优选地，当所述载体包含4个驱动电机时，所述获取载体电机转速，具体包括：获取当前载体4个驱动电机的转速，分别为m1,m2,m3,m4；
设置电机的最小稳定值为m_eps；
如果前方两个电机转速同后方两电机转速差大于电机的最小稳定值，或者左侧两个电机转速同右侧两电机转速差大于电机的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
优选地，获取当前三轴角速度分别为wn，we，wd；
角速度的最小稳定值为w_eps；
如果当前三轴的角速度的绝对值大于角速度的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
优选地，所述确定载体当前运动状态，还包括：获取当前失准角；
设置失准角的阈值为att_eps；
如果当前失准角大于所述阈值，则确定载体当前运动状态为非平稳运动状态。
优选地，采用基于失准角的卡尔曼滤波确定当前失准角；所述基于失准角的卡尔曼滤波采用东向水平失准角
所述陀螺漂移ωbx,ωby和水平加速度fx,fy基于载体坐标系，所述载体坐标系三轴表示为xyz，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；，所述东向水平失准角
设滤波器系统的状态方程为：
其中
根据失准角方程，上式可表示为
其中，
简化转换矩阵为以下形式
对f(x)和h(x)其求解雅克比矩阵得到矩阵f和h，得到线性化后的状态方程：
其中，向量x为上一时刻系统状态向量，向量
优选地，所述基于失准角的卡尔曼滤波包括时间更新和量测校正，所述进行姿态校正即为进行量测校正，以更新卡尔曼滤波的增益矩阵和量测噪声序列方差阵，消除累积误差。
优选地，所述时间更新的具体步骤包括：
其中，向量
优选地，所述量测更新的具体步骤包括：
其中，向量
优选地，所述解算所载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为vk-1，
则当前时刻的速度vk，位置pk的计算方式为：
vk＝vk-1+ak-1×δt
pk＝pk-1+vk×δt。
本发明实施例提供了一种低成本惯性传感器在机动环境下的高精度导航方法，考虑载体运动、陀螺仪角速度和失准角三方面因素，确定是否进行姿态校正，能够有效地降低累积误差，提高导航的精确性，降低算法的复杂度；并且，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量。
附图说明
图1是本发明实施例一提供的一种低成本惯性传感器在机动环境下的高精度导航方法的流程图；
图2是本发明实施例二提供的一种基于低成本惯性传感器在机动环境下的位置速度更新方法的流程图；
图3是本发明实施例三提供的一种基于失准角的卡尔曼滤波姿态校正方法的流程图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。
另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。
实施例一
本发明实施例一具体可以应用在需要高精度导航的产品，如无人机定位，自动驾驶，数字城市，机器人导航等。图1为本发明实施例一提供的一种低成本惯性传感器在机动环境下的高精度导航方法的流程图。本实施例的方法具体包括：
110、步骤一：信号获取，获取惯性传感器的输出信号，包括加速度和角速度。
120、数据预处理，采用快速傅立叶变换对所述输出信号进行低通滤波。
130、信号补偿，基于上一周期惯性传感器的输出信号对当前信号进行补偿，所述信号补偿包括圆锥补偿和旋转划桨补偿。
在本实施例中，通过圆锥补偿可获得经过补偿之后的旋转矢量，所述旋转矢量经转换后可得载体的姿态角，分别为滚转角、俯仰角和航向角，分别记作φ,θ,ψ，具体计算方式为：
其中，β(t)为经过补偿之后的旋转矢量，δα为当前的角增量采样，δα'为前一周期的角增量采样，ω(t)为角速度，t为预设周期。
通过旋转划桨补偿可以获得经过补偿后的速度增量dv(t)，通过所述dv(t)可以求解得到三轴加速度，具体计算方式为：
其中，δv为当前的速度增量采样，δv'为前一周期的速度增量采样，其中δr为速度旋转误差，
140、确定载体运动状态，获取载体电机转速和陀螺仪角速度，确定载体当前运动状态，根据所述运动状态判断是否进行姿态校正。
150、解算所述载体当前的速度和位置。
在本实施例中，当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置。
所述解算所述载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为vk-1，
则当前时刻的速度vk，位置pk的计算方式为：
vk＝vk-1+ak-1×δt
pk＝pk-1+vk×δt。
本发明实施例一提供了一种低成本惯性传感器在机动环境下的高精度导航方法，通过对圆锥补偿和旋转划桨补偿进行适应性地改进，不对信号进行多周期的补偿处理，只运用上一周期的信号量并结合当前信号进行补偿，能够降低运算复杂度，同时提高精度；另外，基于载体电机转速、陀螺仪角速度进行运动状态判定，根据当前运动状态确定是否进行姿态校正，进一步地降低了运算量，提高了导航的精度。
实施例二
图2是本发明实施例二提供的一种基于低成本惯性传感器在机动环境下的位置速度更新方法的流程图。本实施例以上述实施例为基础进行优化，本实施例的方法具体包括：
210、获取信息，确定载体当前运动状态。
在本实施例中，获取载体电机转速，获取陀螺仪三轴角速度和失准角。
220、判断载体是否处于平稳状态。
在本实施例中，当所述载体包含4个驱动电机时，所述获取载体电机转速，具体包括：获取当前载体4个驱动电机的转速，分别为m1,m2,m3,m4；
设置电机的最小稳定值为m_eps；
如果前方两个电机转速同后方两电机转速差大于电机的最小稳定值，或者左侧两个电机转速同右侧两电机转速差大于电机的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
获取当前三轴角速度分别为wn，we，wd；
角速度的最小稳定值为w_eps；
如果当前三轴的角速度的绝对值大于角速度的最小稳定值，则确定当前载体运动状态为非平稳运动状态。
所述确定载体当前运动状态，还包括：获取当前失准角；
设置失准角的阈值为att_eps；
如果当前失准角大于所述阈值，则确定载体当前运动状态为非平稳运动状态。
采用基于失准角的卡尔曼滤波确定当前失准角；所述基于失准角的卡尔曼滤波采用东向水平失准角
所述陀螺漂移ωbx,ωby和水平加速度fx,fy基于载体坐标系，所述载体坐标系三轴表示为xyz，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；，所述东向水平失准角
设滤波器系统的状态方程为：
其中
根据失准角方程，上式可表示为
其中，
简化转换矩阵为以下形式
对f(x)和h(x)其求解雅克比矩阵得到矩阵f和h，得到线性化后的状态方程：
其中，向量x为上一时刻系统状态向量，向量
230、姿态校正。
在本实施例中，当所述载体的运动状态为平稳运动状态时，则先进行姿态校正，再解算所述载体的速度和位置；当所述载体的运动状态为非平稳运动状态时，则不进行姿态校正，直接解算所述载体的速度和位置。
其中，基于失准角的卡尔曼滤波包括时间更新和量测校正，所述进行姿态校正即为进行量测校正，以更新卡尔曼滤波的增益矩阵和量测噪声序列方差阵，消除累积误差。
所述时间更新的具体步骤包括：
其中，向量
所述量测校正的具体步骤包括：
其中，向量
240、解算速度位置。
在本实施例中，所述解算所载体当前的速度和位置的具体步骤包括：
上一时刻的速度记为vk-1，
则当前时刻的速度vk，位置pk的计算方式为：
vk＝vk-1+ak-1×δt
pk＝pk-1+vk×δt。
本发明实施例二提供了一种基于低成本惯性传感器在机动环境下的位置速度更新方法，综合考虑载体电机转速、陀螺仪角速度和失准角三方面因素，确定是否进行姿态校正，能够有效地降低累积误差，提高导航的精确性，降低算法的复杂度；并且，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量。
实施例三
图3是本发明实施例三提供的一种基于失准角的卡尔曼滤波姿态校正方法的流程图。本实施例的方法具体包括：
310、获取惯性传感器的三轴加速度计的测量值。
320、判断载体是否处于加速状态。
在本实施例中，判断载体是否处于加速状态的步骤为：
步骤1：计算所述惯性传感器的三轴加速度计的矢量和
步骤2：计算载体低机动状态下的三轴加速度；具体步骤为：
在载体低机动的状态下有
其中，重力向量为gn＝[00g]t，g为当地重力加速度，f1为低机动状态下三轴加速度计输出的加速度向量，记作f1＝[fx1,fy1,fz1]t；
由式(2)可计算得出理想加速度fx1,fy1,fz1：
步骤3：定义一偏差数值为eps，分别计算
在载体处于非加速状态时，进行时间更新和量测修正，在载体处于加速状态时，仅进行时间更新
330、进行时间更新。
在本实施例中，所述时间更新的具体步骤包括：
其中，向量
340、进行时间更新和量测修正。
在本实施例中，所述量测修正的具体步骤包括：
其中，向量
350、确定载体姿态。
在本实施例中，确定载体的姿态包括确定载体的失准角。
本领域技术人员可以理解，本实施例中限定的进行量测校正的条件可以与实施例一或实施例二中限定的进行量测校正的条件相互组合，既可以仅满足任一条件即进行量测校正，也可以同时满足两者的条件才进行量测校正。对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。
本发明实施例三提供了一种基于失准角的卡尔曼滤波姿态更新方法，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量；并且，采用上述参数作为状态量和量测量，可以自适应地选择时间更新或量测修正，即使用当前加速度矢量和以及加速度计数值与理想值的差进行自适应卡尔曼滤波，进一步降低了运算量。
在此请注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。