CN111551175B - 一种航姿参考系统的互补滤波姿态解算方法 - Google Patents

Abstract

本发明涉及一种航姿参考系统的互补滤波姿态解算方法，其中，包括：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵利用第一组磁强计数据进行方位对准，计算初始航向角采集陀螺仪输出、加速度输出和磁强计输出数据；利用加速度和磁强计输出数据计算陀螺仪误差计算当地导航系磁场强度参考值计算加速度输出在导航系的投影值fⁿ，计算加速度叉乘结果f_X，计算磁矢量叉乘结果计算误差计算陀螺仪数据误差修正值利用陀螺仪的输出修正后进行姿态更新；根据更新后的姿态信息计算姿态角。

Description

一种航姿参考系统的互补滤波姿态解算方法

技术领域

本发明涉及导航技术，特别涉及一种航姿参考系统的互补滤波姿态解算方法。

背景技术

随着智能制造和尖端芯片的发展，无人机在军用和民用领域都得到广泛使用。为了得到稳定的无人机飞行控制，导航系统成为不可或缺的机载设备之一。一般情况下，导航系统由惯性导航和卫星导航配合磁强计等传感器组成。正常情况下，导航系统能不间断提供位置、速度和姿态角(姿态角包括俯仰角、滚转角和航向角)。但由于卫星导航系统易受干扰造成信号中断，为了保证飞机的安全，需要采用航姿参考系统作为飞机姿态传感器。另外，对于一些消费级无人机或视距内的小型无人机，如果不需要获得位置信息，可以采用航姿参考系统作为姿态传感器，减少卫星导航带来的系统成本。

航姿参考系统由陀螺仪、加速度计、磁强计、相关电路、结构和线缆等组成，利用互补滤波算法，实现载体航向角、俯仰角和滚转角的解算。算法的原理是：没有位置和速度量测信息时，飞机会保持匀速直线运动或盘旋运动，线加速度变化较小，此时加速度计测量的信息为重力加速度和干扰加速度的叠加，利用陀螺仪输出的角速度积分可以得到系统的姿态角，利用磁强计输出计算导航坐标系磁场分量。忽略重力加速度在不同位置的变化和干扰加速度，利用导航系到本体系的姿态矩阵将导航系下重力加速度和磁场分量投影到本体坐标系上，与加速度计和磁强计实际输出作叉乘，得到的结果作为陀螺仪漂移修正本体系陀螺仪角速度输出，并进一步实现姿态更新。传统互补滤波姿态解算方法框图如图1所示。

其中，导航系的定义为：x轴和y轴在当地水平面，x轴指向正东，y轴指向正北，z轴垂直于当地水平面向上。本体系的定义为：y轴沿着载体纵轴指向前，x轴沿着载体横轴指向右，z轴符合右手定则。

目前，传统航姿参考系统的互补滤波姿态解算方法有两处缺陷。第一，根据磁场分布特性，东向磁场分量为0，传统互补滤波姿态解算方法将姿态角全部初始化为0，考虑到叉乘计算方法，当航向角为180°时，磁强计失去了对航向角的修正作用，存在修正盲点。特别是初始方位为180°时，航姿参考系统解算航向角将始终维持在0°，姿态角解算结果错误。第二，磁强计叉乘结果直接反馈到本体系的三个轴，在本体系下同时修正三个轴的陀螺仪，磁强计的测量误差耦合进俯仰角和滚转角的计算，造成俯仰角和滚转角的计算误差。

目前，现有的航姿参考系统的互补滤波姿态解算方法有两处缺陷。第一，根据磁场分布特性，东向磁场分量为0，传统互补滤波姿态解算方法将姿态角全部初始化为0，考虑到叉乘计算方法，当航向角为180°时，磁强计失去了对航向角的修正作用，存在修正盲点。特别是初始方位为180°时，航姿参考系统解算航向角将始终维持在0°，姿态角解算结果错误。第二，磁强计叉乘结果直接反馈到本体系的三个轴，在本体系下同时修正三个轴的陀螺仪，磁强计的测量误差耦合进俯仰角和滚转角的计算，造成俯仰角和滚转角的计算误差。

发明内容

本发明的目的在于提供一种航姿参考系统的互补滤波姿态解算方法，用于解决上述现有技术的问题。

本发明一种航姿参考系统的互补滤波姿态解算方法，其中，包括：步骤一：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；步骤二：利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵第三步：利用第一组磁强计数据进行方位对准，计算初始航向角/>并初始化四元数Q_k＝[Q_k,1 Q_k,2 Q_k,3Q_k,4]^T和本体系到导航系的姿态转换矩阵/>第四步：采集陀螺仪输出、加速度输出和磁强计输出数据；第五步：利用加速度和磁强计输出数据计算陀螺仪误差/>计算当地导航系磁场强度参考值/>计算加速度输出在导航系的投影值fⁿ，计算加速度叉乘结果f_X，计算磁矢量叉乘结果/>计算误差/>第六步：计算陀螺仪数据误差修正值/>第七步：利用陀螺仪的输出修正后进行姿态更新；第八步：根据更新后的姿态信息计算姿态角；第九步：重复第四步到第八步，直到采样时刻结束。

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第一步的计算过程具体包括：陀螺仪输出的角度增量为：加速度计输出的速度增量为：/>磁强计输出为：/>归一化导航系重力加速度gⁿ＝[0 0 1]^T；

对采集的数据进行归一化为：

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵包括：

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第三步具体包括：

定义水平系为：导航系绕z轴转过航向角后得到的坐标系，记磁强计在水平系的分量为

计算航向角

初始化四元数Q_k：

计算姿态转换矩阵和/>

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第五步：利用加速度和磁强计输出数据计算陀螺仪误差计算当地导航系磁场强度参考值/>

计算加速度输出在导航系的投影值fⁿ：

计算加速度叉乘结果f_X：

计算磁矢量叉乘结果

其中，符号×表示叉乘，即对于a＝[a_x a_y a_z]^T，b＝[b_x b_y b_z]^T

取加速度叉乘f_X前两项和磁叉乘第三项，作为误差/>

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第六步具体包括：设定比例系数k_p＝10，积分系数k_i＝0.01，初始采样时刻积分误差修正值计算比例误差修正值/>

计算积分误差修正值

计算一个采样周期dT内陀螺仪误差修正值

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第七步具体包括：

一个采样周期dT内角度增量：

等效转过的角度：

采用三阶毕卡逼近更新当前采用时刻的四元数：Q_k+1＝[Q_k+1,1 Q_k+1,2 Q_k+1,3 Q_k+1,4]^T，并进行归一化；

其中：

根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第八步：计算姿态角包括：

根据更新的四元数Q_k+1更新转换矩阵和/>

根据姿态矩阵计算姿态角：

其中：为/>的第i行第j列。

本发明针对目前航姿参考系统传统互补滤波姿态解算方法存在修正盲点和误差耦合的技术缺陷，设计了一种改进的互补滤波姿态解算方法，可实现任意姿态角下加速度计和磁强计对陀螺仪的角速度修正，同时在导航系下计算陀螺仪修正值，分离俯仰角、滚转角和航向角通道，避免了磁强计误差对俯仰角和滚转角的耦合误差。

附图说明

图1为现有的互补滤波姿态解算框图；

图2本发明的互补滤波姿态解算框图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

图2本发明的互补滤波姿态解算框图，如图2所示，航姿参考系统的互补滤波姿态解算方法包括：

第一步：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化：

陀螺仪输出的角度增量为：加速度计输出的速度增量为：磁强计输出为：/>归一化导航系重力加速度gⁿ＝[0 0 1]^T。

对采集的数据进行归一化：

第二步：利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵

第三步：利用第一组磁强计数据进行方位对准，计算初始航向角并初始化四元数Q_k＝[Q_k,1 Q_k,2 Q_k,3 Q_k,4]^T和本体系到导航系的姿态转换矩阵/>

定义水平系为：导航系绕z轴转过航向角后得到的坐标系。记磁强计在水平系的分量为

计算航向角

初始化四元数Q_k：

计算姿态转换矩阵和/>

第四步：采集陀螺仪输出、加速度输出和磁强计输出数据。

第五步：利用加速度和磁强计输出数据计算陀螺仪误差计算当地导航系磁场强度参考值/>

计算加速度输出在导航系的投影值fⁿ：

计算加速度叉乘结果f_X：

计算磁矢量叉乘结果

其中，符号×表示叉乘，即对于a＝[a_x a_y a_z]^T，b＝[b_x b_y b_z]^T

取加速度叉乘f_X前两项和磁叉乘第三项，作为角速度误差/>

第六步：计算陀螺仪数据误差修正值

设定比例系数k_p＝10，积分系数k_i＝0.01，初始采样时刻积分误差修正值

计算比例误差修正值

计算积分误差修正值

计算一个采样周期dT内陀螺仪误差修正值

第七步：利用陀螺仪的输出修正后进行姿态更新

一个采样周期dT内角度增量：

等效转过的角度：

采用三阶毕卡逼近更新当前采用时刻的四元数：Q_k+1＝[Q_k+1,1 Q_k+1,2 Q_k+1,3 Q_k+1,4]^T，并进行归一化。

Q_k+1＝Q_k+1/||Q_k+1||

其中：

第八步：计算姿态角

根据更新的四元数Q_k+1更新转换矩阵和/>

根据姿态矩阵计算姿态角：

其中：为/>的第i行第j列。

第九步：重复第四步到第八步，直到采样时刻结束。

此时即完成了改进的互补滤波姿态解算。

本发明的关键点，利用初始对准实现初值姿态测量，避免了航向角的修正盲点；同时在导航系下计算陀螺仪修正值，避免了俯仰角、滚转角与航向角的交叉耦合，从而提高了姿态角的解算精度。

本发明设计了一种改进的互补滤波方法，该方法优点为：传统互补滤波姿态解算存在航向角修正盲点，同时，磁强计误差被耦合进俯仰角和滚转角的解算，造成了交叉耦合。本方法通过初始对准提供初始姿态角和初始四元数，可实现任意姿态角下陀螺仪的修正，保证了姿态角的正确性。同时，陀螺仪修正值计算更换到导航坐标系进行，方便分离加速度计通道和磁强计通道的耦合，提高了俯仰角和滚转角的测量精度。

本发明针对目前航姿参考系统传统互补滤波姿态解算方法，存在修正盲点和误差耦合的技术缺陷，设计了一种改进的互补滤波姿态解算方法，可实现任意姿态角下加速度计和磁强计对陀螺仪的角速度修正，同时在导航系下计算陀螺仪修正值，分离俯仰角、滚转角和航向角通道，避免了磁强计误差对俯仰角和滚转角的耦合误差。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (1)

1.一种航姿参考系统的互补滤波姿态解算方法，其特征在于，包括：

步骤一：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；

步骤二：利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵

第三步：利用第一组磁强计数据进行方位对准，计算初始航向角并初始化四元数Q_k＝[Q_k,1 Q_k,2 Q_k,3 Q_k,4]^T和本体系到导航系的姿态转换矩阵/>

第四步：采集陀螺仪输出、加速度输出和磁强计输出数据；

第五步：利用加速度和磁强计输出数据计算陀螺仪误差计算当地导航系磁场强度参考值/>计算加速度输出在导航系的投影值fⁿ，计算加速度叉乘结果f_X，计算磁矢量叉乘结果/>计算误差/>

第六步：计算陀螺仪数据误差修正值

第七步：利用陀螺仪的输出修正后进行姿态更新；

第八步：根据更新后的姿态信息计算姿态角；

第九步：重复第四步到第八步，直到采样时刻结束；

第一步的计算过程具体包括：

陀螺仪输出的角度增量为：加速度计输出的速度增量为：磁强计输出为：/>归一化导航系重力加速度gⁿ＝[0 0 1]^T；

对采集的数据进行归一化为：

利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵包括：

第三步具体包括：

定义水平系为：导航系绕z轴转过航向角后得到的坐标系，记磁强计在水平系的分量为

计算航向角

初始化四元数Q_k：

计算姿态转换矩阵和/>

第五步：利用加速度和磁强计输出数据计算陀螺仪误差计算当地导航系磁场强度参考值/>

计算加速度输出在导航系的投影值fⁿ：

计算加速度叉乘结果f_X：

计算磁矢量叉乘结果

其中，符号×表示叉乘，即对于a＝[a_x a_y a_z]^T，b＝[b_x b_y b_z]^T

取加速度叉乘f_X前两项和磁叉乘第三项，作为误差/>

第六步具体包括：

设定比例系数k_p＝10，积分系数k_i＝0.01，初始采样时刻积分误差修正值

计算比例误差修正值

计算积分误差修正值

计算一个采样周期dT内陀螺仪误差修正值

第七步具体包括：

一个采样周期dT内角度增量：

等效转过的角度：

采用三阶毕卡逼近更新当前采用时刻的四元数：Q_k+1＝[Q_k+1,1 Q_k+1,2 Q_k+1,3 Q_k+1,4]^T，并进行归一化；

其中：

第八步：计算姿态角包括：

根据更新的四元数Q_k+1更新转换矩阵和/>

根据姿态矩阵计算姿态角：

其中：为/>的第i行第j列。