专利名称：基于卡尔曼滤波的惯性导航系统初始姿态确定方法
技术领域：
本发明涉及一种惯性导航系统初始姿态的确定方法，特别涉及一种基于卡尔曼滤 波的惯性导航系统初始姿态确定方法。
背景技术：
无人机是一种无须机上驾驶员的飞机。他们可以是被操作者远程操控，也可以通 过预先设定好的轨迹进行自我控制。而这种被用于进行自我控制的无人机在军事界被广为 应用，在军事中深入使用自控无人机不仅可用于救援工作，侦察工作，甚至被用于远程导弹 的控制中，而这一切的前提是不断进步的无人机技术。惯性导航系统便是无人机控制中广为应用的一种。它的使用前提便是通过加速度 和姿态角速度的运算按照设定轨迹进行自我控制。而惯性制导系统很自然的需要一种非常 精确的控制系统能够保持飞行的平衡和不偏离轨道。要达到上述效果，首先要对惯性导航 系统初始姿态进行确认。目前对飞行器进行修正时，系统必须提供精确的姿态角的估算。姿态角估算的精 确与否直接关系到了飞行器的稳定性和可靠性。技术人员通过对所完成的硬件电路板进行 运动，将加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转角收集到的 数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理。结果前者噪音很厉害，且 常有高频波干扰，运算简单且不可靠；后者噪音不明显，但是12秒采样4500次的情况下，陀 螺仪的角度偏移30度，这样使得该结果无法被使用。
发明内容
本发明要解决的技术问题是提供一种基于卡尔曼滤波的惯性导航系统初始姿态 确定方法，使得初始姿态数据分析和处理的结果噪音不明显，数据无偏移。为解决上述技术问题，所述的基于卡尔曼滤波的惯性导航系统初始姿态确定方 法，其特征在于包括以下步骤(1)导航系统预热完毕，保持在初始位置，称为第一位置，采用加速度传感器对X 轴和Z轴进行采样；采用陀螺仪对Y轴进行采样；(2)将上述加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转 角收集到的数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理；(3)将上述经过处理的数据经过陀螺仪0修正后积分得出角度，并根据陀螺仪输 出与地球自转角速率的关系，转换成第一位置的横滚角Y1;(4)在正式的卡尔曼滤波之前，根据陀螺仪的数据手册对上述数据进行高通滤 波；(5)将步骤(2)经过处理的数据经过Atan (ζ/χ)，并根据加速度传感器输出与重力 加速度的关系，转换成第一位置的航向角小工和俯仰角Q1 ；(6)在正式的卡尔曼滤波之前，对上述数据进行低通滤波；
(7)将步骤(4)和步骤(6)经过处理的数据利用卡尔曼滤波进行处理，计算出第二 位置的航向角Φ2、俯仰角92和横滚角Y2,将其作为本导航系统的初始姿态。进一步说，所述卡尔曼滤波是一种叠代alphak = alpha k_l+(_u_k_bias)氺dt其中(_u_k)代表状态k时的角速率输出量；两种输入是由陀螺仪和加速度的数值组成，卡尔曼滤波模型的是按照以下的方式 进行的xk+1 = A · xk+B · Uk 第一个式子是线形模型的普通方式，A和B矩阵需要被填入并且选择一个状态X， 变量u代表着输入量，在这里u就是陀螺仪输入量；然后使用以下提到过的公式alpha k = alpha k_l+(_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修正后积 分得出角度，并根据陀螺仪输出与地球自转角速率的关系，转换成第一位置的横滚角Y1 ；接着在步骤110，在正式的卡尔曼滤波之前，根据陀螺仪的数据手册对上述数据进行高通滤波；在步骤104和步骤106，将步骤103经过处理的数据经过Atan (ζ/χ)，并根据加速 度传感器输出与重力加速度的关系，转换成第一位置的航向角小工和俯仰角Q1 ；接着在步骤108，在正式的卡尔曼滤波之前，对上述数据进行低通滤波；最后在步骤111，将步骤110和步骤108经过处理的数据利用卡尔曼滤波进行处 理，计算出第二位置的航向角Φ2、俯仰角92和横滚角Y2，将其作为本导航系统的初始姿 态。进一步说，所述卡尔曼滤波是一种叠代alpha k = alpha k_l+(_u_k_bias)氺dt其中(_u_k)代表状态k时的角速率输出量；两种输入是由陀螺仪和加速度的数值组成，卡尔曼滤波模型的是按照以下的方式 进行的 第一个式子是线形模型的普通方式，A和B矩阵需要被填入并且选择一个状态X， 变量u代表着输入量，在这里u就是陀螺仪输入量；然后使用以下提到过的公式alpha k = alpha k_l+(_u_k_bias)氺dt而卡尔曼滤波就会靠将计算值和加速度计的第二输入量的值进行比较，从而在每 一次叠代中纠正偏差。与现有技术相比，本发明有如下优点1、本发明利用两个位置上陀螺仪和加速度计输出的数据，改善惯性导航系统的可 观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计 常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和 横滚角。2、本发明能够在初始姿态计算的同时完成测漂和定标任务，不但提高了系统初始 姿态计算的精度，而且定标精度也得到了提高，在SINS导航状态给予补偿，可有效地提高 导航定位精度。
权利要求
一种基于卡尔曼滤波的惯性导航系统初始姿态确定方法，其特征在于包括以下步骤(1)导航系统预热完毕，保持在初始位置，称为第一位置，采用加速度传感器对X轴和Z轴进行采样；采用陀螺仪对Y轴进行采样；(2)将上述加速度传感器的X轴和Z轴采样，以及陀螺仪的Y轴采样分别估测滚转角收集到的数据通过RS232传到PC电脑的MATLAB中进行数据分析和处理；(3)将上述经过处理的数据经过陀螺仪0修正后积分得出角度，并根据陀螺仪输出与地球自转角速率的关系，转换成第一位置的横滚角γ1；(4)在正式的卡尔曼滤波之前，根据陀螺仪的数据手册对上述数据进行高通滤波；(5)将步骤(2)经过处理的数据经过Atan(z/x)，并根据加速度传感器输出与重力加速度的关系，转换成第一位置的航向角φ1和俯仰角θ1；(6)在正式的卡尔曼滤波之前，对上述数据进行低通滤波；(7)将步骤(4)和步骤(6)经过处理的数据利用卡尔曼滤波进行处理，计算出第二位置的航向角φ2、俯仰角θ2和横滚角γ2，将其作为本导航系统的初始姿态。
2.根据权利要求1所述的基于卡尔曼滤波的惯性导航系统初始姿态确定方法，其特征 在于所述卡尔曼滤波是一种叠代alpha k = alpha k_l+(―u_k_bias)氺dt 其中(_u_k)代表状态k时的角速率输出量；两种输入是由陀螺仪和加速度的数值组成，卡尔曼滤波模型的是按照以下的方式进行的xk+1 = A · xk+B · ukι alpim \ =il -dt \ ( Ol^a \ + ( dt \ ^ItoU0 1八—人+1 ◎广第一个式子是线形模型的普通方式，A和B矩阵需要被填入并且选择一个状态X，变量 u代表着输入量，在这里u就是陀螺仪输入量； 然后使用以下提到过的公式 alpha k = alpha k_l+(―u_k_bias)氺dt而卡尔曼滤波就会靠将计算值和加速度计的第二输入量的值进行比较，从而在每一次 叠代中纠正偏差。
全文摘要
本发明涉及一种惯性导航系统初始姿态的确定方法，特别涉及一种基于卡尔曼滤波的惯性导航系统初始姿态确定方法，根据陀螺仪输出与地球自转角速率的关系和根据加速度传感器输出与重力加速度的关系，计算出航向角、俯仰角和横滚角，并代入卡尔曼滤波器最后确定初始姿态。与现有技术相比，本发明改善惯性导航系统的可观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和横滚角。
文档编号G01C21/16GK101929862SQ20091005396
公开日2010年12月29日 申请日期2009年6月26日 优先权日2009年6月26日
发明者杨新毅, 王萧 申请人:上海市上海中学