CN101929862A - 基于卡尔曼滤波的惯性导航系统初始姿态确定方法 - Google Patents

Abstract

本发明涉及一种惯性导航系统初始姿态的确定方法，特别涉及一种基于卡尔曼滤波的惯性导航系统初始姿态确定方法，根据陀螺仪输出与地球自转角速率的关系和根据加速度传感器输出与重力加速度的关系，计算出航向角、俯仰角和横滚角，并代入卡尔曼滤波器最后确定初始姿态。与现有技术相比，本发明改善惯性导航系统的可观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和横滚角。

Description

基于卡尔曼滤波的惯性导航系统初始姿态确定方法

技术领域

本发明涉及一种惯性导航系统初始姿态的确定方法，特别涉及一种基于卡尔曼滤波的惯性导航系统初始姿态确定方法。

背景技术

无人机是一种无须机上驾驶员的飞机。他们可以是被操作者远程操控，也可以通过预先设定好的轨迹进行自我控制。而这种被用于进行自我控制的无人机在军事界被广为应用，在军事中深入使用自控无人机不仅可用于救援工作，侦察工作，甚至被用于远程导弹的控制中，而这一切的前提是不断进步的无人机技术。

惯性导航系统便是无人机控制中广为应用的一种。它的使用前提便是通过加速度和姿态角速度的运算按照设定轨迹进行自我控制。而惯性制导系统很自然的需要一种非常精确的控制系统能够保持飞行的平衡和不偏离轨道。要达到上述效果，首先要对惯性导航系统初始姿态进行确认。

目前对飞行器进行修正时，系统必须提供精确的姿态角的估算。姿态角估算的精确与否直接关系到了飞行器的稳定性和可靠性。技术人员通过对所完成的硬件电路板进行运动，将加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转角收集到的数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理。结果前者噪音很厉害，且常有高频波干扰，运算简单且不可靠；后者噪音不明显，但是12秒采样4500次的情况下，陀螺仪的角度偏移30度，这样使得该结果无法被使用。

发明内容

本发明要解决的技术问题是提供一种基于卡尔曼滤波的惯性导航系统初始姿态确定方法，使得初始姿态数据分析和处理的结果噪音不明显，数据无偏移。

为解决上述技术问题，所述的基于卡尔曼滤波的惯性导航系统初始姿态确定方法，其特征在于包括以下步骤：

(1)导航系统预热完毕，保持在初始位置，称为第一位置，采用加速度传感器对X轴和Z轴进行采样；采用陀螺仪对Y轴进行采样；

(2)将上述加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转角收集到的数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理；

(3)将上述经过处理的数据经过陀螺仪0修正后积分得出角度，并根据陀螺仪输出与地球自转角速率的关系，转换成第一位置的横滚角γ₁；

(4)在正式的卡尔曼滤波之前，根据陀螺仪的数据手册对上述数据进行高通滤波；

(5)将步骤(2)经过处理的数据经过Atan(z/x)，并根据加速度传感器输出与重力加速度的关系，转换成第一位置的航向角φ₁和俯仰角θ₁；

(6)在正式的卡尔曼滤波之前，对上述数据进行低通滤波；

(7)将步骤(4)和步骤(6)经过处理的数据利用卡尔曼滤波进行处理，计算出第二位置的航向角φ₂、俯仰角θ₂和横滚角γ₂，将其作为本导航系统的初始姿态。

进一步说，所述卡尔曼滤波是一种叠代

alphak＝alpha k-1+(_u_k-bias)*dt

其中(_u_k)代表状态k时的角速率输出量；

两种输入是由陀螺仪和加速度的数值组成，卡尔曼滤波模型的是按照以下的方式进行的：

x_k+1＝A·x_k+B·u_k

${\left(\begin{array}{c}\mathrm{alpha}\\ \mathrm{bias}\end{array}\right)}_{k+1}=\left(\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right){\left(\begin{array}{c}\mathrm{alpha}\\ \mathrm{bias}\end{array}\right)}_k+\left(\begin{array}{c}\mathrm{dt}\\ 0\end{array}\right)u_k$

第一个式子是线形模型的普通方式，A和B矩阵需要被填入并且选择一个状态x，变量u代表着输入量，在这里u就是陀螺仪输入量；

然后使用以下提到过的公式：

alpha k＝alpha k-1+(_u_k-bias)*dt

而卡尔曼滤波就会靠将计算值和加速度计的第二输入量的值进行比较，从而在每一次叠代中纠正偏差。

与现有技术相比，本发明有如下优点：

1、本发明利用两个位置上陀螺仪和加速度计输出的数据，改善惯性导航系统的可观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和横滚角。

2、本发明能够在初始姿态计算的同时完成测漂和定标任务，不但提高了系统初始姿态计算的精度，而且定标精度也得到了提高，在SINS导航状态给予补偿，可有效地提高导航定位精度。

附图说明

图1为本发明基于卡尔曼滤波的惯性导航系统初始姿态确定方法的流程图。

具体实施方式

下面结合附图对本实用新型作进一步说明：

如图1所示，基于卡尔曼滤波的惯性导航系统初始姿态确定方法，其特征在于包括以下步骤：

在步骤101和步骤102，导航系统预热完毕，保持在初始位置，称为第一位置，采用加速度传感器对X轴和Z轴进行采样；采用陀螺仪对Y轴进行采样；

然后在步骤103，将上述加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转角收集到的数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理；

在步骤105、步骤107和步骤109，将上述经过处理的数据经过陀螺仪0修正后积分得出角度，并根据陀螺仪输出与地球自转角速率的关系，转换成第一位置的横滚角γ₁；

接着在步骤110，在正式的卡尔曼滤波之前，根据陀螺仪的数据手册对上述数据进行高通滤波；

在步骤104和步骤106，将步骤103经过处理的数据经过Atan(z/x)，并根据加速度传感器输出与重力加速度的关系，转换成第一位置的航向角φ₁和俯仰角θ₁；

接着在步骤108，在正式的卡尔曼滤波之前，对上述数据进行低通滤波；

最后在步骤111，将步骤110和步骤108经过处理的数据利用卡尔曼滤波进行处理，计算出第二位置的航向角φ₂、俯仰角θ₂和横滚角γ₂，将其作为本导航系统的初始姿态。

进一步说，所述卡尔曼滤波是一种叠代

alpha k＝alpha k-1+(_u_k-bias)*dt

其中(_u_k)代表状态k时的角速率输出量；

两种输入是由陀螺仪和加速度的数值组成，卡尔曼滤波模型的是按照以下的方式进行的：

x_k+1＝A·x_k+B·u_k

${\left(\begin{array}{c}\mathrm{alpha}\\ \mathrm{bias}\end{array}\right)}_{k+1}=\left(\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right){\left(\begin{array}{c}\mathrm{alpha}\\ \mathrm{bias}\end{array}\right)}_k+\left(\begin{array}{c}\mathrm{dt}\\ 0\end{array}\right)u_k$

第一个式子是线形模型的普通方式，A和B矩阵需要被填入并且选择一个状态x，变量u代表着输入量，在这里u就是陀螺仪输入量；

然后使用以下提到过的公式：

alpha k＝alpha k-1+(_u_k-bias)*dt

而卡尔曼滤波就会靠将计算值和加速度计的第二输入量的值进行比较，从而在每一次叠代中纠正偏差。

与现有技术相比，本发明有如下优点：

1、本发明利用两个位置上陀螺仪和加速度计输出的数据，改善惯性导航系统的可观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和横滚角。

2、本发明能够在初始姿态计算的同时完成测漂和定标任务，不但提高了系统初始姿态计算的精度，而且定标精度也得到了提高，在SINS导航状态给予补偿，可有效地提高导航定位精度。

Claims (2)

1.一种基于卡尔曼滤波的惯性导航系统初始姿态确定方法，其特征在于包括以下步骤：

(1)导航系统预热完毕，保持在初始位置，称为第一位置，采用加速度传感器对X轴和Z轴进行采样；采用陀螺仪对Y轴进行采样；

(2)将上述加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转角收集到的数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理；

(3)将上述经过处理的数据经过陀螺仪0修正后积分得出角度，并根据陀螺仪输出与地球自转角速率的关系，转换成第一位置的横滚角γ₁；

(4)在正式的卡尔曼滤波之前，根据陀螺仪的数据手册对上述数据进行高通滤波；

(5)将步骤(2)经过处理的数据经过Atan(z/x)，并根据加速度传感器输出与重力加速度的关系，转换成第一位置的航向角φ₁和俯仰角θ₁；

(6)在正式的卡尔曼滤波之前，对上述数据进行低通滤波；

(7)将步骤(4)和步骤(6)经过处理的数据利用卡尔曼滤波进行处理，计算出第二位置的航向角φ₂、俯仰角θ₂和横滚角γ₂，将其作为本导航系统的初始姿态。

2.根据权利要求1所述的基于卡尔曼滤波的惯性导航系统初始姿态确定方法，其特征在于：所述卡尔曼滤波是一种叠代

alpha k＝alpha k-1+(_u_k-bias)*dt

其中(_u_k)代表状态k时的角速率输出量；

两种输入是由陀螺仪和加速度的数值组成，卡尔曼滤波模型的是按照以下的方式进行的：

x_k+1＝A·x_k+B·u_k

${\left(\begin{array}{c}\mathrm{alpha}\\ \mathrm{bias}\end{array}\right)}_{k+1}=\left(\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right){\left(\begin{array}{c}\mathrm{alpha}\\ \mathrm{bias}\end{array}\right)}_k+\left(\begin{array}{c}\mathrm{dt}\\ 0\end{array}\right)u_k$

第一个式子是线形模型的普通方式，A和B矩阵需要被填入并且选择一个状态x，变量u代表着输入量，在这里u就是陀螺仪输入量；

然后使用以下提到过的公式：

alpha k＝alpha k-1+(_u_k-bias)*dt

而卡尔曼滤波就会靠将计算值和加速度计的第二输入量的值进行比较，从而在每一次叠代中纠正偏差。

Images