CN101033973B - 微小型飞行器微惯性组合导航系统的姿态确定方法 - Google Patents

Abstract

一种微小型飞行器微惯性组合导航系统的姿态确定方法，属微小型飞行器姿态确定方法。本方法利用制导信息，控制卡尔曼滤波器噪声矩阵，实现随飞行状态的自适应调整，通过参数调整，实现微惯性组合导航系统在动态飞行条件下的姿态确定，具体步骤：通过GNC闭环回路制导算法，得到飞行器横滚角；通过传感器采集，获得飞行器角速度和比力；按捷联惯性导航算法解算出飞行器姿态、速度和位置信息；由加速度计和GPS数据计算飞行器的姿态和位置测量信息；实时计算卡尔曼滤波器的观测噪声矩阵中的水平姿态方差，通过卡尔曼滤波器估计捷联惯导系统的误差；对上述获得的姿态、速度和位置信息进行修正，并作为反馈信息，输入到控制系统。

Description

微小型飞行器微惯性组合导航系统的姿态确定方法

一、技术领域

本发明属于微小型飞行器的运动参数测量系统技术领域。

二、背景技术

微小型飞行器在近距离军事侦察、目标搜索，灾害监测等领域有特殊的使用价值和广阔的前景。微小型飞行器的导航、制导和控制系统组成的闭环回路的核心功能是对微型飞行器的状态进行测量与控制，使其能够达到自主飞行的目的。微小型飞行器姿态确定系统是微型导航系统的子系统，用于实时的测量微型飞行器的姿态，并将测定的姿态信息反馈到微型飞行器的控制器。微小型飞行器姿态确定系统为姿态稳定和飞行控制算法提供微小型飞行器的姿态信息，是微小型飞行器中的关键技术之一，是微小型飞行器自主飞行器的重要条件。

微小型飞行器惯性组合姿态确定系统是采用MEMS(MicroElectro-Mechanical System)惯性传感器(微陀螺、微加速度计)的微型捷联惯性导航系统。采用MEMS技术的硅微陀螺和加速度计体积小，重量轻，是目前唯一可满足微型飞行器重量与体积要求的惯性传感器。捷联惯性导航系统(Strapdown Inertial Navigation System)是一种基于牛顿运动定律以推算的方式实现导航功能的系统，其核心传感器是由陀螺仪(角运动传感器)和加速度计(线运动传感器)等两类惯性传感器构成的测量单元，由上述两类传感器构成的测量单元直接固联在运动载体上。捷联惯性导航系统中的处理器通过模拟/数字转换电路或者直接采用数字接口，读取上述测量单元内角运动和线运动传感器的数据，按惯性导航算法的原理流程，对原始的测量数据进行加工，解算出运动载体的姿态、速度和位置等参数。

MEMS惯性传感器测量误差大，基于MEMS惯性传感器的微惯性导航系统的姿态误差会快速发散，难以满足微小型飞行器自主飞行的需求。因此针对微小型飞行器的微型惯性导航系统中，需要引入误差不随时间增长的姿态信息，来抑制微型惯导系统姿态误差发散。对微型惯导系统的误差发散的抑制是微型惯性组合导航系统在微小型飞行器中应用的关键，国内外在此方面的提出的方法主要有利用加速度计作为倾角传感器采用加权的方式，与微惯导系统的姿态融合如：2004年8月，美国伯明翰扬大学的硕士学位论文：Design Of An Autopilot ForSmall Unmanned Aerial Vehicles，75-78；本专利发明人于2006年第6期应用科学学报论文“MEMS-INS微型飞行器姿态确定系统的实现研究”提出利用现代最优状态估计方法——卡尔曼滤波实现微惯性导航系统与加速度计获取水平姿态进行融合的方法等。

加速度计倾角传感器受载体动态的影响，已有文献的常规思路是对加速度计倾角传感器的使用进行约束，根据惯性测量单元IMU和航姿系统的信息对载体的动态过程进行判断，当载体动态过程超过一定范围后，不再利用加速度计倾角传感器进行姿态测量，由于微小型飞行器转动惯量小，机动灵活，受气流的影响严重，姿态变化范围和变化速率远大于一般无人机，这一方式不适用于微小型飞行器。

三、发明内容

本发明的主要目的是，改进已有文献中对动态条件下加速度计倾角传感器误差处理方法的不足，在导航、制导与控制(GNC)闭环条件下，从GNC回路中提取关键特征信息，探索适应于微小型飞行器特点的新途径，抑制微小型飞行器的动态飞行中由加速度计倾角传感器导致的姿态误差。

本发明的内容是微小型飞行器的MEMS微惯性组合导航系统利用其所在的微小型飞行器GNC闭环回路中的制导信息实现姿态确定方法，其特点在研通过以下步骤实现：

(8)微小型飞行器GNC闭环回路的制导算法解算步骤：根据微小型飞行器当前的位置和期望航路点的位置，计算出期望的飞行方向，将期望的飞行方向与当前飞行方向相减，所得的差乘以一系数k，得到期望的微小型飞行器横滚角，其中系数k可取范围是0.2到0.5；

(9)MEMS惯性测量单元信号采集步骤：采集MEMS惯性测量单元的输出信号，得到微小型飞行器的角速度和比力；

(10)捷联惯导算法解算步骤：将步骤(2)采集到的角速度和比力信号，按捷联惯性导航算法的流程，解算出微小型飞行器的姿态、速度和位置的导航信息；

(11)获取位置和姿态的测量信息步骤：读取GPS的位置、速度和航向信息，同时利用最近时刻采集到的比力信号，按加速度计倾角传感器的测量原理计算微小型飞行器的横滚角与俯仰角；

(12)卡尔曼滤波器观测矩阵的自适应调整步骤：以步骤(1)得到的期望的横滚角为自变量，按一分段函数，实时计算卡尔曼滤波器的观测噪声矩阵中水平姿态的方差，实时计算的卡尔曼滤波器的观测噪声矩阵中水平姿态的方差与其初值之间保持一倍数关系，这一倍数是分段函数的因变量，该分段函数的具体函数关系通过实际的飞行试验来确定；

(13)组合滤波及修正步骤：利用步骤(5)实时调整观测矩阵参数的卡尔曼滤波器，对步骤(4)获取的测量信息进行处理，估计步骤(2)捷联惯导解算出的姿态、速度和位置的误差，在步骤(2)获得的姿态、速度和位置中，扣除本步骤估计出的误差，并替换步骤(2)的姿态、速度和位置信息；

(14)姿态、位置信息的反馈步骤：步骤(6)获得的姿态信息输入到微小型飞行器的控制模块，控制模块通过微小型飞行器的舵面控制微小型飞行器的姿态变化；步骤(6)获得的位置和航向信息输入到制导模块，用于完成步骤(1)。

本发明从导航、制导于控制闭环系统的角度入手，基于卡尔曼滤波最有加强平均的本质，结合对微小型飞行器的飞行状态的判别，通过动态调整微惯性组合导航系统卡尔曼滤波器的观测噪声矩阵的方法，提高微惯性组合导航系统姿态的精度，减弱微小型飞行器动态飞行中倾角传感器对系统姿态的修正量，减弱动态飞行中导航、制导与控制回路的姿态修正等效干扰，提高微小型飞行器的飞行品质。

四、说明书附图

图1是微型飞行器导航、制导与控制系统原理框图。

图2是本发明的微型导航、制导与控制系统闭环回路框图。

图3是图2中MEMS微惯性组合导航系统组成框图。

图4现有技术中微小型飞行器自主飞行的横滚角曲线示意图。

图5是本发明微小型飞行器自主飞行的横滚角曲线示意图。

五、具体实施方式

本发明的原理是：

传统的GNC闭环回路中，导航系统提供导航参数反馈给制导与飞行控制算法，如图1中各模块和实线构成的原理框图所示。本发明则在传统GNC回路的基础上，增加从制导模块到导航子系统的前馈控制，如图1的虚线所示，利用制导信息，控制组合卡尔曼滤波器噪声矩阵，实现卡尔曼滤波器观测噪声阵随飞行状态的自适应调整，通过参数调整，改进微惯性组合导航系统对飞行状态的自适应性，实现已有的微小型飞行器微惯性组合导航系统在动态飞行条件下的姿态确定，提高动态飞行条件下的姿态精度和飞行的稳定性。

为了实现微小型飞行器微惯性组合导航系统的姿态确定，适应动态的飞行条件，需要完成工作：

(1)微小型飞行器GNC闭环回路的制导算法解算

微小型飞行器根据当前的位置和期望航路点的位置，计算出期望的航向角ψ_e，将期望的航向角与微导航系统当前实际测量得到的航向角ψ相减，所得的差乘以一系数k，得到期望的微小型飞行器横滚角，k为比例系数，取值范围是0.2到0.5。

以微小型飞行器起飞点为原点，以北向为x轴正方向，以东向为y轴正方向，建立当地大地水平坐标系，在该坐标系下，航向角以沿x轴正方向为0度，转向右侧则航向角增大，航向角的范围为0～360度，微小型飞行器的实时位置坐标为X、Y，其中X为横坐标、Y为纵坐标，当前期望飞到的航路点坐标为P_x、P_y，其中P_x为横坐标、P_y为纵坐标，则期望的飞行方向ψ_e按下式计算：

则期望的飞行方向按下式计算：

当Px＞X，Py≥Y， ${\mathrm{\ψ}}_e={\mathrm{tg}}^{-1}\frac{\mathrm{Py}-Y}{\mathrm{Px}-X};$

当Px＝X，Py＞Y，ψ_e＝π/2；

当Px＜X， ${\mathrm{\ψ}}_e={\mathrm{tg}}^{-1}\frac{\mathrm{Py}-Y}{\mathrm{Px}-X}+\mathrm{\π};$

当Px＝X，Py＜Y，ψ_e＝3π/2；

当Px＞X，Py＜Y ${\mathrm{\ψ}}_e={\mathrm{tg}}^{-1}\frac{\mathrm{Py}-Y}{\mathrm{Px}-X}+2\mathrm{\π};$

制导算法按下式生成期望的横滚角γ_e：γ_e＝k(ψ_e-ψ)，

其中ψ为微惯性组合导航系统实际测量得到的航向角。

(2)MEMS惯性测量单元信号采集步骤：采集MEMS惯性测量单元的输出信号，得到微小型飞行器的角速度和比力；

(3)捷联惯导算法解算

利用采集到的角速度和比力信号，按捷联惯性导航算法的流程，解算出微小型飞行器的姿态、速度和位置等导航信息。捷联惯导解算的初始姿态、速度和位置是由外部输入的。

机体坐标系相对于当地地理坐标系的角速度的在机体系的三个轴的分量依次为绕横滚轴的角速度ω_x，绕俯仰轴的角速度ω_y，绕方位轴的角速度ω_z，捷联惯导的计算周期为Δt，Δt时间内机体转过的角度Δθ的三个分量依次依次为横滚轴角增量Δθ_x，俯仰轴角增量Δθ_y，方位轴角增量Δθ_z，则转过的角度以矩阵方式表示为：

$\mathrm{\Δ\θ}=\left[\begin{array}{ccc}0& {-\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z& 0& {-\mathrm{\Δ\θ}}_x\\ -{\mathrm{\Δ\θ}}_y& {\mathrm{\Δ\θ}}_x& 0\end{array}\right]=\left[\begin{array}{ccc}0& {-\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& {-\mathrm{\ω}}_x\\ {-\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\mathrm{\Δt}$

t时刻的载体的姿态矩阵为

$C_n^b\left(t\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

那么t+Δt时刻姿态矩阵由从t时刻的姿态阵递推求得，将C_n ^b(t+Δt)简写为

则t+Δt时刻，捷联惯导的俯仰角θ、横滚角γ和航向角ψ等姿态信息由下式获得。

$\left.\begin{array}{c}\mathrm{\θ}={\sin }^{-1}\left(T_{23}\right)\\ \mathrm{\γ}={\mathrm{tg}}^{-1}\left(\frac{T_{13}}{T_{33}}\right)\\ \mathrm{\ψ}={\mathrm{tg}}^{-1}\left(\frac{T_{21}}{T_{32}}\right)\end{array}\right\}$

在姿态解算的基础上，根据当前的姿态和比力，获得当地地理坐标系中的运动加速度a_N，a_E，a_D，其中，a_N是北向的运动加速度，a_E东向的运动加速度，a_D地向的运动加速度。微小型飞行器的速度和位置由下式递推得到，V_N(t)表示t时刻微小型飞行器北向的速度，V_E(t)表示t时刻微小型飞行器东向的速度，V_D(t)表示t时刻微小型飞行器地向的速度，V_N(t+Δt)，V_E(t+Δt)，V_D(t+Δt)分别表示t+Δt时刻北向、东向和地向的速度，R表示地球半径，用符号纬度L_I，经度λ_I和高度h_I分别表示由捷联惯导算法得到的微小型飞行器的位置，则捷联惯导的速度和位置可计算如下。

$\left\{\begin{array}{c}{V}_N=(t+\mathrm{\Δt})=V_N\left(t\right)+a_N\mathrm{\Δt}\\ V_E(t+\mathrm{\Δt})=V_E\left(t\right)+a_E\mathrm{\Δt}\\ V_D(t+\mathrm{\Δt})=V_D\left(t\right)+a_D\mathrm{\Δt}\end{array}\right.$

$\left\{\begin{array}{c}{L}_I(t+\mathrm{\Δt})=L_I\left(t\right)+\frac{V_N\left(t\right)\mathrm{\Δt}}{R}\\ {\mathrm{\λ}}_I(t+\mathrm{\Δt})={\mathrm{\λ}}_I\left(t\right)+\frac{V_E\left(t\right)\mathrm{\Δt}}{R\cos \left(L_I\left(t\right)\right)}\\ h_I(t+\mathrm{\Δt})=h_I\left(t\right)-V_D\left(t\right)\mathrm{\Δt}\end{array}\right.$

(4)获取位置和姿态的测量信息

读取GPS的位置(纬度L_G、经度λ_G和高度h_G)和航向角ψ_G，同时利用最近时刻采集到的比力信号，按加速度计倾角传感器的测量原理计算微小型飞行器的横滚角γ_A与俯仰角θ_A。

(5)卡尔曼滤波器观测矩阵的自适应调整步骤

以步骤(1)得到的期望的横滚角γ_e为自变量，按一分段函数，t+1时刻的实时计算卡尔曼滤波器的观测噪声矩阵中水平姿态的方差R_t+1，实时计算的卡尔曼滤波器的观测噪声矩阵中水平姿态的方差与其初值之间保持一倍数关系，该倍数关系具有抛物线形式，这一倍数是分段函数的因变量，分段函数具有以下形式：

分段函数的具体函数关系通过实际的飞行试验来确定，其中，γ_e0表示横滚角的取值范围，R₀是水平姿态方差的初值，根据加速度计的测量噪声大小确定，k_R是不同状态下抛物线形式的倍数关系的公共成分，通过试验获得。

(6)组合滤波及修正步骤

利用步骤(5)实时调整观测矩阵参数的卡尔曼滤波器，对步骤(4)获取的测量信息进行处理，估计步骤(2)捷联惯导解算出的姿态、速度和位置的误差，在步骤(2)获得的姿态、速度和位置中，扣除本步骤估计出的误差，并替换步骤(2)的姿态、速度和位置信息。

该卡尔曼滤波器的特征在于：

滤波器的观测噪声矩阵是由步骤(5)实时确定的，状态变量X包括捷联惯导系统的北向平台误差角φ_N、东向平台误差角φ_E和地向平台误差角φ_D，北向速度误差δv_N、东向速度误差δv_E和地向速度误差δv_D，纬度误差δL，经度误差δλ、和高度误差δh，共9个，即X＝[φ_N φ_E φ_D δv_N δv_E δv_D δL δλ δh]^T，滤波器以平台误差角和位置误差为观测量，观测变量为6个。

对步骤(4)获取的测量信息进行处理如下：

设t+1为滤波时刻，此时由GPS航向角ψ_G，倾角传感器测得的横滚角γ_A和俯仰角θ_A，可得到姿态测量信息确定的姿态矩阵C_n″ ^b

$C_{n^{\′\′}}^b=\left[\begin{array}{ccc}{\cos \mathrm{\γ}}_A{\cos \mathrm{\ψ}}_G+{\sin \mathrm{\γ}}_A{\sin \mathrm{\θ}}_A{\sin \mathrm{\ψ}}_G& -{\cos \mathrm{\γ}}_A{\sin \mathrm{\ψ}}_G+{\sin \mathrm{\γ}}_A{\sin \mathrm{\θ}}_A{\cos \mathrm{\ψ}}_G& -\sin {\mathrm{\γ}}_A{\cos \mathrm{\θ}}_A\\ {\cos \mathrm{\θ}}_A{\sin \mathrm{\ψ}}_G& {\cos \mathrm{\θ}}_A{\cos \mathrm{\ψ}}_G& \sin {\mathrm{\θ}}_A\\ {\sin \mathrm{\γ}}_A{\cos \mathrm{\ψ}}_G-{\cos \mathrm{\γ}}_A{\sin \mathrm{\θ}}_A{\sin \mathrm{\ψ}}_G& {-\sin \mathrm{\γ}}_A{\sin \mathrm{\ψ}}_G-{\cos \mathrm{\γ}\sin \mathrm{\θ}}_A{\cos \mathrm{\ψ}}_G& {\cos \mathrm{\γ}}_A{\cos \mathrm{\θ}}_A\end{array}\right]$

由姿态矩阵C_n″ ^b确定的数学平台的三个平台误差角记为北向平台误差角φ_NA，东向平台误差角φ_EA，地向平台误差角φ_DG。

利用此时刻捷联惯导的姿态矩阵C_n ^b(t+1)的转置与C_n″ ^b相乘，得到捷联惯导系统的姿态阵对应的数学平台误差角[φ_N φ_E φ_D]^T与步骤(4)测量得到的姿态及姿态阵对应的数学平台误差角[φ_NA φ_EA φ_DG]^T之间的差，即

$C_n^b{(t+1)}^T{C}_{n^{\′\′}}^b=\left[\begin{array}{ccc}1& ({\mathrm{\φ}}_D-{\mathrm{\φ}}_{\mathrm{DG}})& -({\mathrm{\φ}}_E-{\mathrm{\φ}}_{\mathrm{EA}})\\ -({\mathrm{\φ}}_D-{\mathrm{\φ}}_{\mathrm{DG}})& 1& ({\mathrm{\φ}}_N-{\mathrm{\φ}}_{\mathrm{NA}})\\ ({\mathrm{\φ}}_E-{\mathrm{\φ}}_{\mathrm{EA}})& -({\mathrm{\φ}}_N-{\mathrm{\φ}}_{\mathrm{NA}})& 1\end{array}\right]$

卡尔曼滤波的平台误差角观测量为捷联惯导解算得到的微小型飞行器的位置与GPS得到的微小型飞行器的位置相减，得到位置的观测量

将平台误差角观测量和位置观测量合并为t+1时刻卡尔曼滤波器的总的观测量Z_t+1，则

定义测量矩阵

卡尔曼滤波器中，状态向量为9维，状态预测的方差阵P是9×9的矩阵，初始估值误差方差阵初值P₀，系统噪声方差阵初值Q₀观测噪声方差阵初值R₀等矩阵初值都由系统外部直接输入，状态转移φ_t+1，t是已有文献中已知矩阵，系统噪声矩阵Q_t由统噪声方差阵初值Q₀确定，观测噪声矩阵R_t+1由步骤(5)计算得到。

则各滤波时刻的状态估值的方差阵及状态预测的方差阵都可以递推得到。即在滤波时刻t的P_t基础上，按如下递推滤波方程，得到滤波时刻t+1的状态预测的方差阵P_t+1|t，滤波器增益矩阵K_t+1，状态估计值

及状态估值的方差阵P_t+1可由下式计算得到。

Q_t＝Q₀

$P_{t+1|t}={\mathrm{\φ}}_{t+1,t}{P}_t{\mathrm{\φ}}_{t+1,t}^T+Q_t$

K_t+1＝P_t+1|tH_t+1 ^T(H_t+1P_t+1|tH_t+1 ^T+R_t+1)^-1

${\hat{X}}_{t+1}=K_{t+1}{Z}_{t+1}$

P_t+1＝(I-K_t+1H_t+1)P_t+1|t

上述计算的状态估计值

是步骤(4)捷联惯导所得到得到的姿态、速度和位置的误差，在步骤(4)所得到的结果中，减去估计值

中的对应的量，即可提高微惯性组合导航系统的姿态的精度，达到确定微小型飞行器姿态的目的。

(7)姿态、位置信息的反馈步骤

步骤(6)获得的姿态信息输入到微小型飞行器的控制模块，控制模块通过微小型飞行器的舵面控制微小型飞行器的姿态变化；步骤(6)获得的位置和航向信息输入到制导模块，用于完成步骤(1)。

这样，通过动态调整微惯性组合导航系统卡尔曼滤波器的观测噪声矩阵的方法，提高微惯性组合导航系统姿态的精度，减弱微小型飞行器动态飞行中倾角传感器对系统姿态的修正量，减弱动态飞行中导航、制导与控制回路的姿态修正等效干扰，提高微小型飞行器的飞行品质。

图1中，各模块的及连接的实线箭头代表了导航、制导与控制闭环回路的基本的逻辑连接关系，从微小型飞行器制导算法模块和微小型飞行器飞行控制算法之间引出的指向微惯性组合导航系统的虚线箭头代表的是本发明的期望的横滚角的传递和利用关系。

图2是对图1的进一步细化，位置比较模块和航向控制模块构成图1中的微小型飞行器制导算法模块，期望俯仰模块、俯仰控制模块和横滚控制模块共同组成图1中的微小型飞行器飞行控制算法模块。虚线箭头与图1中相同，更清晰的表明了信息的来源和传递。

图3是图2中MEMS微惯性组合导航系统的进一步细化，表明了图2中虚线代表的期望的横滚角γ_e对微惯性组合导航系统卡尔曼滤波器观测噪声矩阵R_t+1的控制调整。

图4是采用现有技术情况下微型飞行器左盘旋飞行时的横滚角数据曲线，横轴代表时间，纵轴代表横滚角，315秒到330秒一直处于向左盘旋，横滚角的波动范围-2度到-32度，飞行中横向波动范围大，达到了30度。

图5是应用本发明后，微型飞行器重复左、右盘旋的数据曲线，曲线反映的飞行过程：80秒后，5次左盘旋，4次右盘旋，左、右盘旋相互交替，应用本发明后，左盘旋飞行中，横滚角的波动范围约在-18度到-30度，右盘旋飞行时横滚角的波动范围18度到30度。应用本发明后，波动范围明显减小，约12度，误差缩小了60％。

六、发明的效果

本发明从导航、制导与控制闭环系统的角度入手，基于卡尔曼滤波最优加权平均的本质，结合对微小型飞行器的飞行状态判别，通过动态调整微型组合导航系统卡尔曼滤波器的观测噪声R的方法，提高微惯性组合导航系统姿态的精度，来减弱微小型飞行器动态飞行中倾角传感器对系统姿态的观测修正量，减弱GNC回路的姿态修正等效干扰，提高飞行品质。

盘旋飞行试验效果：

对于微小型飞行器自主飞行中的微惯性组合导航系统，微小型飞行器盘旋转弯飞行是要经常经历的最恶劣的飞行状态。采用本发明前后的盘旋飞行试验中，两组横滚角对比曲线如图4和图5，从曲线上可以看出，采用本发明前，盘旋飞行中，微小型飞行器的姿态围绕着期望值的变化范围30度，采用本发明后，微小型飞行器自主飞行中的姿态围绕着期望值的波动范围小于12度，波动明显减小，飞行的曲线与期望值之间的偏差明显减小，误差缩小了60％。

本发明针对动态飞行中倾角传感器引入的测量误差，利用动态调整卡尔曼滤波器观测噪声的方法，改进微惯性组合导航系统的性能，减小测量误差，提高微小型飞行器的飞行平稳度。

大量试飞试验结果表明：本发明应用于微型飞行器，构成导航、制导与控制闭环系统，微型飞行器能够实现5-6米/秒的阵风环境下的自主姿态稳定和航路点的导航飞行，微小型飞行器的自主飞行性能达到了期望的水平。本发明具有很强的工程应用价值。

Claims (3)

1.一种微小型飞行器微惯性组合导航系统的姿态确定方法，其特点在于通过以下步骤实现：

(1)微小型飞行器GNC闭环回路的制导算法解算步骤：根据微小型飞行器当前的位置和期望航路点的位置，计算出期望的飞行方向，将期望的飞行方向与当前飞行方向相减，所得的差乘以一系数k，得到期望的微小型飞行器横滚角，其中系数k可取范围是0.2到0.5；

(2)MEMS惯性测量单元信号采集步骤：采集MEMS惯性测量单元的输出信号，得到微小型飞行器的角速度和比力；

(3)捷联惯导算法解算步骤：将步骤(2)采集到的角速度和比力信号，按捷联惯性导航算法的流程，解算出微小型飞行器的姿态、速度和位置的导航信息；

(4)获取位置和姿态的测量信息步骤：读取GPS的位置、速度和航向信息，同时利用最近时刻采集到的比力信号，按加速度计倾角传感器的测量原理计算微小型飞行器的横滚角与俯仰角；

(5)卡尔曼滤波器观测矩阵的自适应调整步骤：以步骤(1)得到的期望的横滚角为自变量，按一分段函数，实时计算卡尔曼滤波器的观测噪声矩阵中水平姿态的方差，实时计算的卡尔曼滤波器的观测噪声矩阵中水平姿态的方差与其初值之间保持一倍数关系，这一倍数是分段函数的因变量，该分段函数的具体函数关系通过实际的飞行试验来确定；

(6)组合滤波及修正步骤：利用步骤(5)实时调整观测矩阵参数的卡尔曼滤波器，对步骤(4)获取的测量信息进行处理，估计步骤(2)捷联惯导解算出的姿态、速度和位置的误差，在步骤(2)获得的姿态、速度和位置中，扣除本步骤估计出的误差，并替换步骤(2)的姿态、速度和位置信息；

(7)姿态、位置信息的反馈步骤：步骤(6)获得的姿态信息输入到微小型飞行器的控制模块，控制模块通过微小型飞行器的舵面控制微小型飞行器的姿态变化；步骤(6)获得的位置和航向信息输入到制导模块，用于完成步骤(1)。

2.如权利要求1所述的微小型飞行器微惯性组合导航系统的姿态确定方法，其特征在于期望的微小型飞行器横滚角γ_e的计算法是：

以微小型飞行器起飞点为原点，以北向为x轴正方向，以东向为y轴正方向，建立当地大地水平坐标系，在该坐标系下，航向角以沿x轴正方向为0度，转向右侧则航向角增大，航向角的范围为0～360度，微小型飞行器的实时位置坐标为X、Y，其中X为横坐标、Y为纵坐标，当前期望飞到的航路点坐标为P_x、P_y，其中P_x为横坐标、P_y为纵坐标，则期望的飞行方向ψ_e按下式计算：

当Px＞X，Py≥Y，

当Px＝X，Py＞Y，ψ_e＝π/2；

当Px＜X，

当Px＝X，Py＜Y，ψ_e＝3π/2；

当Px＞X，Py＜Y

制导算法按下式生成期望的横滚角γ_e：γ_e＝k(ψ_e-ψ)，

其中ψ为微惯性组合导航系统实际测量得到的航向角，k为比例系数，取值范围是0.2到0.5。

3.如权利要求1所述的微小型飞行器微惯性组合导航系统的姿态确定方法，其特征在于卡尔曼滤波观测矩阵自适应调整方法是：

以步骤(1)得到的期望的横滚角γ_e为自变量，按一分段函数，t+1时刻的实时计算卡尔曼滤波器的观测噪声矩阵中水平姿态的方差R_i+1，实时计算的卡尔曼滤波器的观测噪声矩阵中水平姿态的方差与其初值之间保持一倍数关系，该倍数关系具有抛物线形式，这一倍数是分段函数的因变量，分段函数具有以下形式：

分段函数的具体函数关系通过实际的飞行试验来确定，其中，γ_e0表示横滚角的取值范围，R₀是水平姿态方差的初值，根据加速度计的测量噪声大小确定，k_R是不同状态下抛物线形式的倍数关系的公共成分，通过试验获得。

Images