CN108592918A - 摇摆基座下mems imu的全姿态解算方法 - Google Patents

Abstract

本发明公开了一种摇摆基座下MEMS IMU的全姿态解算方法，其中，方法包括以下步骤：定义满足预设规则的坐标系；在定义的坐标系下，使得MEMSIMU按正反连续旋转方式进行旋转；采集旋转时MEMS惯性器件的数据；根据MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到MEMSIMU的全姿态解算结果。该方法不需要卫星或磁强计等辅助信息，就能够实现一定的全姿态解算精度，有效提高了解算的精确性，这在无卫星信号或磁场干扰环境中具有重要应用价值，MEMS IMU具有成本低廉、体积小、抗冲击等优点，应用广泛，本发明有效提高了其适用性。

Description

摇摆基座下MEMS IMU的全姿态解算方法

技术领域

本发明涉及惯性技术领域，特别涉及一种摇摆基座下MEMS IMU(Micro-Electro-Mechanical SystemInertial Measurement Unit，微机电惯性测量单元)的全姿态解算方法。

背景技术

随着惯性技术的迅速发展，MEMS IMU在惯性导航领域有着越来越多的应用。MEMSIMU具有成本低、体积小、重量轻、抗冲击的优点，在无人机、单兵导航、机器人、可穿戴设备、航姿参考***等领域有较为广泛地应用，因此研究MEMS IMU具有重要意义。

由于MEMS惯性器件具有较大的常值偏差，因此如果直接进行姿态解算会引起很大的误差。为了减小MEMS惯性器件常值偏差的影响，提高MEMS IMU的姿态解算精度，一方面可以提高工艺技术，从而提升器件精度，但是这种方式周期长，成本高；另一方面采用***级误差抑制技术，通过旋转调制减小MEMS惯性器件常值误差的影响，从而使得 MEMS IMU的姿态解算能够达到一定精度。

旋转调制技术已经在MEMS陀螺寻北仪中得到应用，但是MEMS陀螺寻北仪只能在静基座条件下实现寻北，当存在外界晃动的情况，误差极大，不能进行全姿态解算。而发动机引起振动或乘客上下车引起晃动的车辆、系泊状态的小船等都可以看成为摇摆基座，因此如何在摇摆基座上实现MEMS IMU的全姿态解算具有重要意义和应用需求。

相关技术中，通过单轴旋转调制实现静基座下的寻北，但是不能在摇摆基座上进行姿态解算；采用磁强计和卡尔曼滤波算法实现姿态解算，不足之处在于引入了磁强计，容易受到电磁设备的干扰，不能仅根据MEMS IMU的输出数据直接实现全姿态解算，限制了其应用范围。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的一个目的在于提出一种摇摆基座下MEMS IMU的全姿态解算方法，该方法应用广泛，可以有效提高MEMS IMU的适用性，有效提高其全姿态解算的精度。

为达到上述目的，本发明实施例提出了一种摇摆基座下MEMS IMU的全姿态解算方法，包括以下步骤：定义满足预设规则的坐标系；在定义的所述坐标系下，使得MEMS IMU 按正反连续旋转方式进行旋转；采集旋转时MEMS惯性器件的数据；根据所述MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到所述MEMS IMU的全姿态解算结果。

本发明实施例的摇摆基座下MEMS IMU的全姿态解算方法，不需要卫星或磁强计等辅助信息，就能够实现一定的全姿态解算精度，有效提高了解算的精确性，这在无卫星信号或磁场干扰环境中具有重要应用价值，MEMS IMU具有成本低廉、体积小、抗冲击等优点，应用广泛，本发明可以有效提高其适用性。

另外，根据本发明上述实施例的摇摆基座下MEMS IMU的全姿态解算方法还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述预设规则为右手定则，其中，所述坐标系包括导航坐标系n系、载体坐标系b系、IMU坐标系s系、地球坐标系e系、惯性坐标系 i系、基座惯性坐标系i_b0系。

进一步地，在本发明的一个实施例中，所述使得MEMS IMU按正反连续旋转方式进行旋转，进一步包括：在初始时刻，所述IMU坐标系s系与所述载体坐标系b系重合；使得所述MEMS IMU先绕z轴以角速度ω＝20°/s正向旋转360度，再反向以角速度ω＝20°/s旋转360度，并单轴正反交替连续旋转。

进一步地，在本发明的一个实施例中，所述根据所述MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到所述MEMS IMU的全姿态解算结果，进一步包括：根据所述惯性系对准算法确定不同坐标系之间的转换关系矩阵，根据所述各个坐标系之间的转换矩阵从所述微机电MEMS惯性器件的数据中提取得到俯仰角、横滚角、航向角；根据所述Mahony算法，将所述MEMS IMU中加速度计的数据获得的低频姿态估计进行低通滤波，并对陀螺仪输出直接积分获得的高频姿态估计进行高通滤波，并融合两者的姿态估计信息，以解算水平姿态角；根据所述Mahony算法修正所述俯仰角和所述横滚角，根据修正后的所述俯仰角、修正后的所述横滚角、所述航向角实现全姿态解算。

进一步地，在本发明的一个实施例中，其中，所述导航坐标系和所述惯性坐标系的转换矩阵

其中，ω_ie为地球自转角速率，L为载体所在位置的纬度，Δt为载体时间间隔；所述基座惯性坐标系与所述IMU坐标系之间的转换矩阵

其中为陀螺的输出数据；所述惯性坐标系和所述基座惯性坐标系之间的转换矩阵

其中，根据t_k1、t_k2时刻(t₀<t_k1<t_k2)的速度值V(t_k1)、V(t_k2)构建辅助矢量V(t_k1)×V(t_k2)，为所述基座惯性坐标系下的速度值，Vⁱ(t_k)为重力加速度在惯性系下的速度值。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点，从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明一个实施例的摇摆基座下MEMS IMU的全姿态解算方法的流程图；

图2为根据本发明一个具体实施例的摇摆基座下MEMS IMU的全姿态解算方法的流程图；

图3为根据本发明一个实施例的坐标系示意图；

图4为根据本发明一个实施例的采用的旋转方案的示意图；

图5为根据本发明一个实施例的惯性系对准算法示意图；

图6为根据本发明一个实施例的Mahony算法的示意图；

图7为根据本发明一个实施例的一次转台摇摆实验解算的俯仰角的变化图；

图8为根据本发明一个实施例的一次转台摇摆实验解算的横滚角的变化图；

图9为根据本发明一个实施例的一次转台摇摆实验解算的航向角的变化图；

图10为根据本发明一个实施例的摇摆基座下MEMS IMU的全姿态解算方法的各模块示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

下面参照附图描述根据本发明实施例提出的摇摆基座下MEMS IMU的全姿态解算方法。

图1是本发明一个实施例的摇摆基座下MEMS IMU的全姿态解算方法的流程图。

如图1所示，该摇摆基座下MEMS IMU的全姿态解算方法包括以下步骤：

在步骤S101中，定义满足预设规则的坐标系。

可以理解的是，如图2所示，首先，本发明实施例进行坐标系的定义。

在本发明的一个实施例中，如图3所示，预设规则为右手定则，其中，坐标系包括导航坐标系n系、载体坐标系b系、IMU坐标系s系、地球坐标系e系、惯性坐标系i系、基座惯性坐标系i_b0系。

在步骤S102中，在定义的坐标系下，使得MEMS IMU按正反连续旋转方式进行旋转。

可以理解的是，如图2所示，MEMS IMU按正反连续旋转的方案进行旋转。

进一步地，在本发明的一个实施例中，使得MEMS IMU按正反连续旋转方式进行旋转，进一步包括：在初始时刻，IMU坐标系s系与载体坐标系b系重合；使得MEMS IMU先绕z轴以角速度ω＝20°/s正向旋转360度，再反向以角速度ω＝20°/s旋转360度，并单轴正反交替连续旋转。

可以理解的是，如图4所示，初始时刻IMU坐标系s系与载体坐标系b系重合，然后IMU先绕z轴正向(逆时针)以角速度ω＝20°/s旋转360度，再反向(顺时针)以角速度ω＝20°/s旋转360度，单轴正反交替连续旋转。

在步骤S103中，采集旋转时MEMS惯性器件的数据。

可以理解的是，如图2所示，获取MEMS惯性器件的数据。在MEMS IMU采用旋转调制技术的情况下，获取MEMS惯性器件的数据，包括MEMS陀螺和MEMS加速度计的数据。

在步骤S104中，根据MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到MEMSIMU的全姿态解算结果。

可以理解的是，如图2所示，惯性系对准算法和Mahony算法的融合，实现 MEMSIMU的全姿态解算。

进一步地，在本发明的一个实施例中，根据MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到MEMS IMU的全姿态解算结果，进一步包括：根据惯性系对准算法确定不同坐标系之间的转换关系矩阵，根据各个坐标系之间的转换矩阵从 MEMS惯性器件的数据中提取得到俯仰角、横滚角、航向角；根据Mahony算法将MEMS IMU中加速度计的数据获得的低频姿态估计进行低通滤波，并对陀螺仪输出直接积分获得的高频姿态估计进行高通滤波，并融合两者的姿态估计信息，以解算水平姿态角；根据 Mahony算法修正俯仰角和横滚角，根据修正后的俯仰角、修正后的横滚角、航向角实现全姿态解算。

在本发明的一个实施例中，其中，导航坐标系和惯性坐标系的转换矩阵

其中，ω_ie为地球自转角速率，L为载体所在位置的纬度，Δt为载体时间间隔；基座惯性坐标系与IMU坐标系之间的转换矩阵

其中为陀螺的输出数据；惯性坐标系和基座惯性坐标系之间的转换矩阵

其中，根据t_k1、t_k2时刻(t₀<t_k1<t_k2)的速度值V(t_k1)、V(t_k2)构建辅助矢量V(t_k1)×V(t_k2)，为基座惯性坐标系下的速度值，Vⁱ(t_k)为重力加速度在惯性系下的速度值。

具体而言，(1)如图5所示，惯性系对准算法需要确定不同坐标系之间的转换关系矩阵。根据坐标系的相互位置关系，以及载体所在位置的纬度L及时间间隔Δt，确定导航坐标系和惯性坐标系的转换矩阵在摇摆基座环境中，干扰角速度远大于地球自转角速度，无法再用解析式对准算法。通过引入基座惯性坐标系i_b0系，根据地球自转角速度ω_ie和时间间隔Δt，结合重力加速度g在惯性空间中方向的变化，可以推算出地球的北向信息，这就是惯性系对准算法的基本原理。惯性系对准算法需要确定不同坐标系之间的转换关系矩阵。根据坐标系的相互位置关系，以及载体所在位置的纬度L及时间间隔Δt，确定导航坐标系和惯性坐标系的转换矩阵

其中ω_ie为地球自转角速率。

基座惯性坐标系与IMU坐标系之间的转换矩阵利用四元数法对方向余弦矩阵微分方程进行更新，其中为陀螺的输出数据。

确定惯性坐标系和基座惯性坐标系之间的转换矩阵

由于地球的转动，惯性坐标系下的重力加速度是一个随时间变化的量，可以表示为：

式中，Δt_k＝t_k-t₀是与初始时刻t₀的时间间隔。

对gⁱ进行积分，可以得到重力加速度在惯性系下的速度值，表示为：

基座惯性坐标系下的速度值可以表示为：

其中f^s为加速度计输出数据

则有：

利用t_k1、t_k2时刻(t₀<t_k1<t_k2)的速度值V(t_k1)、V(t_k2)构建辅助矢量V(t_k1)×V(t_k2)、V(t_k1)×V(t_k2)×V(t_k1)，可以得到求解方法：

根据上述步骤计算出的各个坐标系的转换矩阵，计算

又其中由测角元件测量的角度可以计算得出，从而得到的表达式，然后通过提取俯仰角、横滚角、航向角可以得到载体的姿态角，从而完成惯性系对准算法下的姿态解算。

(2)如图6所示，Mahony算法解算水平姿态角。由于在计算时，直接采用陀螺数据更新的方式，陀螺的常值偏差会影响姿态解算精度，尽管采用了旋转调制技术，但是在实际应用中，陀螺的常值漂移会缓慢变化，会降低旋转调制的效果。Mahony算法可以进一步消除MEMS惯性器件常值偏差的影响，通过将MEMS IMU中加速度计的数据获得的低频姿态估计进行低通滤波，并对陀螺仪输出直接积分获得的高频姿态估计进行高通滤波，并融合两者的姿态估计信息，从而提高水平姿态解算精度。

Mahony算法利用了测量的惯性方向和测得的角速率惯性方向来源于估计的最佳重力方向：

Mahony算法的计算过程：

δ＝k_p·e+k_I·∫e， (3)

式中，表示在s系中的重力向量；表示***姿态估计的四元数；δ是经过PI调节产生的新息；e表示测量的惯性方向和预测的向量方向之间的相对误差。k_p和k_I是比例积分环节的参数，分别用于控制加速度计和陀螺仪之间的交叉频率和校正陀螺仪误差。

重力向量在s系可以表示为：

令加速度计的读数为a＝(a_x,a_x,a_z)^T，在摇摆基座情况下，有则有：

此时，与叉乘可以得到误差值e，利用该误差值及公式3、公式4修正陀螺仪：

并通过求解微分方程更新四元数：

将四元数规范化：

将规范化四元数转化为俯仰角、横滚角、航向角。由于Mahony算法可以提高水平姿态精度，方位角没有实际含义，因此可以修正基于旋转调制技术的惯性系对准算法解算出的俯仰角和横滚角，而航向角仍为惯性系对准算法解算出的航向角，通过两种算法的融合提高了MEMS IMU的全姿态解算精度。

在本发明的一个具体实施例中，本发明实施例利用MEMS IMU进行转台实验，验证算法的实用性和可靠性，共进行10次实验。

每次实验的转台参数设置为：内框摇摆幅值6°，频率0.125Hz；中框摇摆幅值10°,频率 0.1Hz；外框正反交替旋转20°/s。陀螺的随机游走重复性10°/h，加速度计的零偏稳定性60μg，采样频率100Hz，采样时间5min。

为了验证对准结果的稳定性，每次实验转台都停在相同的位置，对应的实际姿态角为俯仰角0°,横滚角0°,航向角166°。因此根据本发明所计算出的全姿态角与实际姿态角进行对比，就可以验证本发明提出方法的可靠性。转台实验的全姿态解算结果统计如表1所示，其中，表1为转台实验的全姿态解算结果统计表。

表1

序号	俯仰角(°)	横滚角(°)	航向角(°)
				1	-0.0245	0.0360	164.85
2	-0.0241	0.0342	165.44
				3	-0.0332	0.0364	164.68
4	-0.0432	0.0406	166.61
				5	-0.0417	0.0311	165.83
6	-0.0505	0.0278	167.41
				7	-0.0258	0.0201	164.91
8	-0.0358	0.0371	164.99
				9	-0.0415	0.0402	165.19
10	-0.0368	0.0387	167.36

根据表1和图7、8和9，可以得到，本发明实施例的方法计算出的俯仰角和横滚角的平均值分别为-0.036°和0.034°，而如果仅仅采用惯性系对准算法，俯仰角和横滚角的均值分别为-0.125°和0.432°，可见本发明融合的Mahony算法，进一步提高了水平姿态解算精度；航向角的平均值为165.75°，标准差为1.04°。全姿态解算精度能够满足实际应用需求。

因此，本发明实施例的方法通过对MEMS IMU采用旋转调制技术、Mahony算法、惯性系对准算法的融合，能够实现一定的全姿态解算精度，而且没有依赖卫星、磁强计等外界信息，从而能够大大拓展MEMS IMU的应用范围。

根据本发明实施例提出的摇摆基座下MEMS IMU的全姿态解算方法，不需要外界辅助，通过旋转调制技术、惯性系对准算法、Mahony算法的融合，提高了MEMS IMU的全姿态解算精度，这在无卫星信号或有电磁设备干扰环境中(磁强计无法正常工作)具有重要应用价值；通过采用速度矢量求取而不是MEMS惯性器件的直接输出数据，这在一定程度上平滑了惯性器件的随机噪声，提高了计算精度；Mahony算法可以进一步减小陀螺常值漂移的影响，其算法思想融合了加速度计和陀螺的输出数据，计算量小，这在对水平精度要求高的场合具有重要的工程应用价值；融合算法在静基座或摇摆基座下均能实现全姿态解算，大大拓展了应用范围；MEMS IMU具有体积小、成本低、抗冲击等优点，在小型舰艇、无人机、行人导航、机器人、航姿参考***等领域具有更广泛的应用,因此本发明在这些领域具有重要应用价值。

其次参照附图描述根据本发明实施例提出的摇摆基座下MEMS IMU的全姿态解算方法各模块示意图。

图10是本发明一个实施例的摇摆基座下MEMS IMU的全姿态解算方法各模块示意图。

如图10所示，该摇摆基座下MEMS IMU的全姿态解算方法各模块10包括：定义模块100、旋转模块200、采集模块300和处理模块400。

其中，定义模块100用于定义满足预设规则的坐标系。旋转模块200用于在定义的坐标系下，使得MEMS IMU按正反连续旋转方式进行旋转。采集模块300用于采集旋转时MEMS惯性器件的数据。处理模块400用于根据MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到MEMS IMU的全姿态解算结果。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (5)

1.一种摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，包括以下步骤：

定义满足预设规则的坐标系；

在定义的所述坐标系下，使得MEMSIMU按正反连续旋转方式进行旋转；

采集旋转时MEMS惯性器件的数据；以及

根据所述MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到所述MEMSIMU的全姿态解算结果。

2.根据权利要求1所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，所述预设规则为右手定则，其中，所述坐标系包括导航坐标系n系、载体坐标系b系、IMU坐标系s系、地球坐标系e系、惯性坐标系i系、基座惯性坐标系i_b0系。

3.根据权利要求2所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，所述使得MEMSIMU按正反连续旋转方式进行旋转，进一步包括：

在初始时刻，所述IMU坐标系s系与所述载体坐标系b系重合；

使得所述MEMSIMU先绕z轴以角速度ω＝20°/s正向旋转360度，再反向以角速度ω＝20°/s旋转360度，并单轴正反交替连续旋转。

4.根据权利要求3所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，所述根据所述MEMS惯性器件的数据，通过惯性系对准算法和Mahony算法的融合，得到所述MEMSIMU的全姿态解算结果，进一步包括：

根据所述惯性系对准算法，确定不同坐标系之间的转换关系矩阵，根据所述各个坐标系之间的转换矩阵，从所述MEMS惯性器件的数据中提取得到俯仰角、横滚角、航向角；

根据所述Mahony算法，将所述MEMSIMU中加速度计的数据获得的低频姿态估计进行低通滤波，并对陀螺仪输出直接积分获得的高频姿态估计进行高通滤波，并融合两者的姿态估计信息，以解算水平姿态角；

根据所述Mahony算法修正所述俯仰角和所述横滚角，根据修正后的所述俯仰角、修正后的所述横滚角、所述航向角实现全姿态解算。

5.根据权利要求1-4任一项所述的摇摆基座下MEMS IMU的全姿态解算方法，其特征在于，其中，

所述导航坐标系和所述惯性坐标系的转换矩阵

其中，ω_ie为地球自转角速率，L为载体所在位置的纬度，Δt为载体时间间隔；

所述基座惯性坐标系与所述IMU坐标系之间的转换矩阵

其中为陀螺的输出数据；

所述惯性坐标系和所述基座惯性坐标系之间的转换矩阵

其中，根据t_k1、t_k2时刻(t₀<t_k1<t_k2)的速度值V(t_k1)、V(t_k2)构建辅助矢量V(t_k1)×V(t_k2)，为所述基座惯性坐标系下的速度值，Vⁱ(t_k)为重力加速度在惯性系下的速度值。