CN108731676B - 一种基于惯性导航技术的姿态融合增强测量方法及*** - Google Patents

Abstract

本发明提供一种基于惯性导航技术的姿态融合增强测量方法和***，方法包括步骤1，获取各传感器检测的姿态数据，进行数据初始化；步骤2，对初始化后的姿态数据进行捷联解算；步骤3，修正各传感器对应的姿态数据；步骤4，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正陀螺零点；步骤5，输出修正后的姿态数据。本发明具有磁异常检测功能，且磁修正不影响俯仰角和横滚角修正；基于非线性理论，提供了更好的收敛速度和控制精度。

Description

一种基于惯性导航技术的姿态融合增强测量方法及***

技术领域

本发明涉及一种物体姿态测量，特别涉及一种基于惯性导航技 术的姿态融合增强测量方法和***，属于惯性导航技术领域。

背景技术

在实际生活中，运动物体的姿态测量是必不可少的。通过对姿 态的测量，可以对运动物体进行控制，使其按照期望进行姿态运动； 也可以通过姿态的测量，了解运动物体的姿态变化，进而得知其内 在的运动规律。物体在空间运动存在六个自由度，分别是三个线自 由度和三个角自由度。其中三个角自由度运动，即空间转动，可以 用欧拉角来直观的进行表示，描述物体在三维空间的姿态，通常称 为横滚角、俯仰角和航向角。这三个欧拉角也被称为姿态角，或者 简称姿态，通常用四元数，即四维向量表示。欧拉角的变化，即姿 态的变化。运动物体可以是飞机，汽车或者人的手臂等可以发生姿 态变化的物体。

姿态测量在生产生活中无处不在。姿态测量装置一般采用陀螺 仪，加速度计和磁强计。传统的高精度陀螺仪IMU，使用的是高精 度的传感器器件，价格昂贵，体积较大，重量较重，不适于精度要 求较低的应用场合。因此，近年来，随着微机电MEMS器件的发展， 出现了以MEMS陀螺仪，MEMS加速度计和MEMS磁传感器(也称作磁 强计)为传感器的各种小型化的航姿参考***AHRS。MEMS器件，成 本低，质量轻，体积小。在精度满足使用需求的前提下，使用MEMS 器件是不二选择。MEMS陀螺仪，利用科里奥利力测量运动物体的角 速度，动态响应特性好(带宽较宽)，但计算姿态时采用积分算法， 会产生累积误差；MEMS加速度计用于测量物体的运动加速度和重力 加速度，易受物体振动的影响，低频特性好，静态精度高；MEMS磁 传感器，测量物体所处当地的地磁场强度，易受外界电磁干扰，但 是长期精度有界，不会发散。磁传感器和加速度计测量姿态没有累 积误差，但动态响应差(带宽较窄)，且噪声相对较大。由于各个 传感器本身固有的特点，导致使用单一种类传感器难以获得有效稳 定的姿态信息。因此，考虑到MEMS陀螺仪、MEMS磁传感器和 MEMS加速度计在频域上的特性互补，采用互补滤波方式融合三种 传感器数据，使三种传感器优势互补，提炼有效的姿态信息，提高 姿态测量精度，得到广泛应用。

现有的技术方案中，姿态互补滤波的主要缺点为：

(1)互补滤波通常采用PI反馈方式(k_p和k_I分别为反馈系数)， 属于线性控制方式，很难同时保证算法的快速性和稳定性。

(2)MEMS磁传感器很容易受到外界干扰，在室内磁场环境更为 恶劣，由于外界磁干扰的影响，常规互补滤波算法修正航向角时， 经常会出现影响俯仰角和横滚角的情况。即：外界磁异常导致俯仰 角和横滚角修正异常。

发明内容

针对上述不足，考虑到采用传统的线性控制，只能达到指数稳 定，即***以指数方式趋近于零(需要无穷长时间达到零)。而引 入非线性控制后，则能提高***性能，达到有限时间稳定(在有限 时间到达零点)。因此，提出一种基于惯性导航技术的非线性增强的互补滤波姿态测量方法和***。

技术方案如下：

一种基于惯性导航技术的姿态融合增强测量方法。通过陀螺仪， 加速度计和磁强计获取物体的姿态数据，对姿态数据进行修正，输 出物体的互补滤波姿态数据，其特征在于：

步骤1，获取各传感器检测的姿态数据，进行数据初始化；

步骤2，对初始化后的姿态数据进行捷联解算；

步骤3，修正各传感器对应的姿态数据，其中，基于磁强计数 据计算体坐标系的磁场偏差Em，将所述磁场偏差Em投影为导航系 的磁场偏差t，将所述磁场偏差t的X、Y分量置为零；其中，所述 磁场偏差t为四元数，所述X、Y分量为四元数t的X、Y分量；

步骤4，根据修正后的加速度数据、磁强计数据，利用反馈控 制的方式来修正陀螺零点；其中所述磁强计数据包括修正后的所述 磁场偏差；

步骤5，输出修正后的姿态数据。

进一步，所述数据初始化为四元数初始化，并初始化陀螺偏差。

进一步，所述捷联解算为四元数捷联解算。

进一步，所述修正各传感器对应的姿态数据包括对加速度计数 据进行修正，计算加速度计数据偏差e。

进一步，所述修正各传感器对应的姿态数据包括加速度异常判 断，磁异常判断。

一种基于惯性导航技术的姿态融合增强测量***，基于陀螺仪， 加速度计和磁强计，获取物体的姿态数据，对姿态数据进行修正， 输出物体的互补滤波姿态数据，其特征在于，包括：

初始化模块，对各传感器检测的姿态数据，进行数据初始化；

捷联更新模块，对初始化后的姿态数据进行捷联解算；

传感器数据修正模块，修正各传感器对应的姿态数据，其中， 所述传感器数据修正模块包括磁强计数据修正模块，所述磁强计数 据修正模块基于磁强计数据计算体坐标系的磁场偏差Em，将所述磁 场偏差Em投影为导航系的磁场偏差t，将所述磁场偏差t的X、Y分量置为零；其中，所述磁场偏差t为四元数，所述X、Y分量为四 元数t的X、Y分量；

非线性互补滤波模块，根据修正后的加速度数据、磁强计数据， 利用反馈控制的方式来修正陀螺零点；其中所述磁强计数据包括修 正后的所述磁场偏差；

所述捷联更新模块输出修正后的姿态数据。

进一步，所述数据初始化为四元数初始化，并初始化陀螺偏差。

进一步，所述捷联解算为四元数捷联解算。

进一步，所述修正各传感器对应的姿态数据包括对加速度计数 据进行修正，计算加速度计数据偏差e。

进一步，所述修正各传感器对应的姿态数据包括加速度异常判 断，磁异常判断。

与现有技术相比，本发明的非线性增强的互补滤波姿态测量方 法和***具有如下有益效果：

(1)姿态解耦的磁航向修正，具有磁异常检测功能，且磁修正 不影响俯仰角和横滚角修正；

(2)非线性互补方法，基于非线性理论，提供了更好的收敛速 度和控制精度。

附图说明

图1：本发明非线性增强的互补滤波姿态测量***结构框图；

图2：本发明比例线性反馈与非线性反馈对比图；

图3：本发明坐标系图；

具体实施方式

下面结合附图和具体实施例对本发明进行详细描述，但不作为 对本发明的限定。

在一个实施例中，如图1所示，基于惯性导航技术的非线性增 强的互补滤波姿态测量方法包括如下步骤：

1.获取速率陀螺仪数据，进行四元数初始化，并初始化陀螺偏差 δ＝[δ_x δ_y δ_z]^T＝[0 0 0]^T

此陀螺偏差δ为零点偏移估计值，方法过程中，根据修正后的 加速度数据、磁强计数据，利用反馈控制的方式来修正该陀螺偏差 δ，即陀螺零点偏移估计值。陀螺的零点偏移是影响导航精度的主 要因素。

2.姿态数据捷联解算，进行四元数捷联更新。

根据陀螺的三轴速率的测量值，按照下述公式得到三轴速率的估 计值；

w＝w’+δ,

其中w＝[wx wy wz]^T为三轴速率的估计值，w’为三轴速率的测 量值。

利用陀螺的三轴速率估计值w的三轴角速度wx，wy，wz，在每 一个周期，根据下述公式进行空间三维积分，获得物体姿态的角度 估计q。

其中，q＝[q₀ q₁ q₂ q₃]^T；q为四元组，用来描述物体的欧拉角， 上述等式左边是

为q的导数。

3.传感器数据融合修正

3.1加速度计数据的修正

由加速度计采集数据，并进行加速度修正：

3.1.1加速度异常判断：当三轴加速度计的数据模值 sqrt(ax²+ay²+az²)与1g相差较大时，则不进行加速度计数据的修正； 其中，ax,ay,az表示三轴向上的加速度测量值。

3.1.2加速度计数据归一化，获得A＝[ax，ay，az]^T， A＝A/norm(A)；其中，norm代表归一化处理，即数组的平方和再开 平方根。

3.1.3计算加速度计数据偏差e_a：重力加速度在体坐标系中的分 量V与加速度计输出值的叉积

Ea＝A×V，其中V为重力加速度在体坐标系的分量。

V＝C^b _n[0 0 1]^T；其中C^b _n是旋转矩阵，表示从导航系到体坐 标系带旋转变换。

3.2磁强计数据修正

由磁强计采集数据，并进行磁强计数据修正：

3.2.1磁异常判断：当磁场强度变化过于剧烈，或强度与标定值 相差过大，则不进行磁强计数据修正；

3.2.2磁强计数据归一化：M＝[mx,my,mz]^T，M＝M/norm(M)；

3.2.3计算磁场偏差

Em＝M×W，其中W为磁强计在体坐标系的投影；

3.2.4将此磁场偏差Em＝[Emx，Emy，Emz]^T从体坐标系投影至导 航系

t＝[0，Emx，Emy，Emz]

t＝q×t×q^-1；

3.2.5为确保磁偏差修正不影响俯仰角和横滚角，将t的X、Y分 量置为零

t(2:3)＝0；

3.2.6将t重新投影回到导航系

t＝q^-1×t×q

Em＝t

此时，磁场偏差Em完成了磁强计数据解耦修正；

4.姿态数据四元数非线性互补滤波修正

根据修正后的加速度数据，磁强计数据，对物体姿态数据e进 行非线性互补滤波修正：

e＝Em+Ea；

δ＝K_P*e+K_I∫edt+K_n|e|^1/2；

其中，Ea是加速度计偏差，Em为磁场偏差。由前述可知，这些 数据都是三维向量。根据磁场和加速度计测量偏差估算陀螺零点偏 移δ，进而完成零点偏移估算。公式中Kp,Ki,Kn为三个常数，需要 根据陀螺、加速度计和磁力计的特性来确定。Kp,Ki是普通PID控制(比例-积分-微分控制)中的比例项和积分项。其中Kp与***收 敛速度有关，Ki确保***稳态跟踪误差为零。但普通PID控制的缺 点之一是指数收敛，无法在有限时间收敛到零。Kn项加入了非线性 调节方式，能够获得比线性控制更好的效果，同时可以确保有限时 间收敛到零。如图2的比例线性反馈与非线性反馈对比图所示，在 陀螺零点附近，非线性反馈斜率远大于现行反馈，有更快的收敛速 度。

根据加速度计和磁强计的偏差(即修正后的加速度数据，磁强 计数据)，利用反馈控制的方式来修正速率陀螺零点，此种非线性 控制方式的引入，提高***响应速度和性能。

同时，在一个实施例中，实现上述方法的基于惯性导航技术的 非线性增强的互补滤波姿态测量***包括：

(1)初始化模块，获取各传感器数据，进行数据初始化；

(2)捷联更新模块，进行姿态数据的四元数捷联更新；输出姿 态数据；

(3)传感器数据修正模块，对各传感器数据进行修正；具体包 括：加速度计数据修正模块，对加速度计采集数据进行加速度修正； 磁强计数据修正模块，对磁强计采集数据进行磁强计数据修正；非 线性互补滤波模块，根据加速度计和磁强计的偏差，利用反馈控制 的方式来修正速率陀螺零点。

上述各模块具体操作对应于上述基于惯性导航技术的非线性增 强的互补滤波姿态测量的方法步骤。

本发明的具有如下有益效果：

(1)姿态解耦的磁航向修正，具有磁异常检测功能，且磁修正 不影响俯仰角和横滚角修正；

(2)非线性互补方法，基于非线性理论，提供了更好的收敛速 度和控制精度。

以上所述仅为本发明的较佳实施例，并不能因此而理解为对本 发明范围的限制，应当指出，对本领域的普通技术人员来说，在不 脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属 于本发明的保护范围。

Claims (8)

1.一种基于惯性导航技术的姿态融合增强测量方法，通过陀螺仪，加速度计和磁强计获取物体的姿态数据，对姿态数据进行修正，输出物体的互补滤波姿态数据，其特征在于：

步骤1，获取各传感器检测的姿态数据，进行数据初始化；

步骤2，对初始化后的姿态数据进行捷联解算；

步骤3，对加速度计数据和磁强计数据进行修正，其中，对磁强计数据进行修订，包括：

磁异常判断：当磁场强度变化过于剧烈，或强度与标定值相差过大，则不进行磁强计数据修正；

磁强计数据归一化：M＝[mx,my,mz]^T，M＝M/norm(M)；

计算磁场偏差：Em＝M×W，其中W为磁强计在体坐标系的投影；

将此体坐标系的磁场偏差Em＝[Emx，Emy，Emz]^T从体坐标系投影至导航系：t＝[0，Emx，Emy，Emz]；t＝q×t×q^-1；其中，q为四元数，用来描述物体的欧拉角；

为确保磁偏差修正不影响俯仰角和横滚角，将t的X、Y分量置为零：t(2:3)＝0，其中t(2:3)＝0表示将t的X、Y分量置为零；

将t重新投影回到导航系：t＝q^-1×t×q；Em＝t，

其中，基于磁强计数据计算体坐标系的磁场偏差Em，将所述体坐标系的磁场偏差Em投影为导航系的磁场偏差t，将所述导航系的磁场偏差t的X、Y分量置为零；其中，所述导航系的磁场偏差t为四元数，所述X、Y分量为四元数t的X、Y分量；

步骤4，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正陀螺零点，即对物体姿态数据e进行非线性互补滤波修正；其中所述磁强计数据包括修正后的所述体坐标系的磁场偏差Em；

e＝Em+Ea，e为物体姿态数据，Ea是加速度计偏差，Em为磁场偏差；

δ为陀螺零点偏移，

其中，公式中K_p、K_I、K_n为三个常数，需要根据陀螺仪、加速度计和磁强计的特性来确定，K_p，K_i是普通PID控制中的比例项和积分项，其中K_p与***收敛速度有关，K_i确保***稳态跟踪误差为零，K_n项加入了非线性调节方式；

步骤5，输出修正后的姿态数据。

2.根据权利要求1所述的基于惯性导航技术的姿态融合增强测量方法，其特征在于，所述数据初始化为四元数初始化，并初始化陀螺偏差。

3.根据权利要求1所述的基于惯性导航技术的姿态融合增强测量方法，其特征在于，所述捷联解算为四元数捷联解算。

4.根据权利要求1所述的基于惯性导航技术的姿态融合增强测量方法，其特征在于，修正各传感器对应的姿态数据包括加速度异常判断，磁异常判断。

5.一种基于惯性导航技术的姿态融合增强测量***，基于陀螺仪，加速度计和磁强计，获取物体的姿态数据，对姿态数据进行修正，输出物体的互补滤波姿态数据，其特征在于，包括：

初始化模块，对各传感器检测的姿态数据，进行数据初始化；

捷联更新模块，对初始化后的姿态数据进行捷联解算；

传感器数据修正模块，对加速度计数据和磁强计数据进行修正，其中，对磁强计数据进行修订，包括：

磁异常判断：当磁场强度变化过于剧烈，或强度与标定值相差过大，则不进行磁强计数据修正；

磁强计数据归一化：M＝[mx,my,mz]^T，M＝M/norm(M)；

计算磁场偏差：Em＝M×W，其中W为磁强计在体坐标系的投影；

将此体坐标系的磁场偏差Em＝[Emx，Emy，Emz]^T从体坐标系投影至导航系：t＝[0，Emx，Emy，Emz]；t＝q×t×q^-1；其中，q为四元数，用来描述物体的欧拉角；

为确保磁偏差修正不影响俯仰角和横滚角，将t的X、Y分量置为零：t(2:3)＝0，其中t(2:3)＝0表示将t的X、Y分量置为零；

将t重新投影回到导航系：t＝q^-1×t×q；Em＝t，

其中，所述传感器数据修正模块包括磁强计数据修正模块，所述磁强计数据修正模块基于磁强计数据计算体坐标系的磁场偏差Em，将所述体坐标系的磁场偏差Em投影为导航系的磁场偏差t，将所述导航系的磁场偏差t的X、Y分量置为零；其中，所述导航系的磁场偏差t为四元数，所述X、Y分量为四元数t的X、Y分量；

非线性互补滤波模块，根据修正后的加速度数据、磁强计数据，利用反馈控制的方式来修正陀螺零点，即对物体姿态数据e进行非线性互补滤波修正；其中所述磁强计数据包括修正后的所述体坐标系的磁场偏差Em；

e＝Em+Ea，e为物体姿态数据；

δ为陀螺零点偏移，

其中，公式中K_p、K_I、K_n为三个常数，需要根据陀螺仪、加速度计和磁强计的特性来确定，K_p，K_i是普通PID控制中的比例项和积分项，其中K_p与***收敛速度有关，K_i确保***稳态跟踪误差为零，K_n项加入了非线性调节方式；

所述捷联更新模块输出修正后的姿态数据。

6.根据权利要求5所述的基于惯性导航技术的姿态融合增强测量***，其特征在于，所述数据初始化为四元数初始化，并初始化陀螺偏差。

7.根据权利要求5所述的基于惯性导航技术的姿态融合增强测量***，其特征在于，所述捷联解算为四元数捷联解算。

8.根据权利要求5所述的基于惯性导航技术的姿态融合增强测量***，其特征在于，

修正各传感器对应的姿态数据包括加速度异常判断，磁异常判断。

Images