[发明专利]四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法

说明书

本发明公开一种四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法，步骤是：周期读取k时刻四旋翼飞行器机载传感器信息；执行故障检测滤波器及故障定位策略，判断z轴陀螺、扭矩模型、磁传感器的故障；确定各个子滤波器的状态方程，进行数据融合；根据故障检测结果，进行故障隔离；执行全局滤波器，对无故障的子滤波器进行数据融合，得到航向角信息；对各个子滤波器、故障检测滤波器状态量进行重置，并执行系统重置策略。此种方法通过四旋翼飞行器的扭矩模型，形成z轴陀螺的冗余，实现z轴陀螺、磁传感器、扭矩模型的故障检测及系统重置，在陀螺故障时仍能获得姿态角、航向角的准确估计。

技术领域

本发明属于组合导航及容错导航领域，具体涉及一种四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法。

背景技术

四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点，特别适合在近地面环境(如室内、城区和丛林等)中执行监视、侦察等任务，具有广阔的军事和民用前景。导航系统为四旋翼飞行器提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。

目前四旋翼飞行器常用的传感器包括惯性传感器、GNSS(卫星导航系统)、磁传感器、气压高度计，其中惯性传感器包括陀螺仪与加速度计。受成本、体积所限，四旋翼飞行器中选用的惯性传感器精度、可靠性较低，易受外界温度、振动干扰而产生性能下降，甚至失效。此时，会导致导航系统精度下降，影响飞行安全。目前，尚未有针对惯性传感器失效情况下的四旋翼飞行器导航方法。

发明内容

本发明的目的，在于提供一种四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法，通过四旋翼飞行器的扭矩模型，形成z轴陀螺的冗余，实现z轴陀螺、磁传感器、扭矩模型的故障检测及系统重置，在陀螺故障时仍能获得姿态角、航向角的准确估计

为了达成上述目的，本发明的解决方案是：

一种四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法，包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；磁传感器信息ψ_m(k)；陀螺信息其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量；加计信息分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量；

步骤二：执行故障检测滤波器及故障定位策略，判断z轴陀螺、扭矩模型、磁传感器的故障；

步骤三：根据步骤二的故障定位结果，确定各个子滤波器的状态方程，进行扭矩模型/z轴陀螺子滤波器、扭矩模型/磁传感器子滤波器、z轴陀螺/磁传感器子滤波器的数据融合；

步骤四：根据故障检测结果，对扭矩模型/z轴陀螺子滤波器、扭矩模型/磁传感器子滤波器、扭矩模型/加速度计子滤波器进行故障隔离；

步骤五：根据步骤四的故障定位和隔离结果，执行全局滤波器，对无故障的子滤波器进行数据融合，得到航向角信息：

步骤六：根据全局滤波器结果，对各个子滤波器、故障检测滤波器状态量进行重置，并执行系统重置策略。

采用上述方案后，本发明利用四旋翼飞行器的扭矩模型，与机载陀螺、加速度计、磁传感器相融合，可实现z轴陀螺故障情况下对航向角的准确估计。在该方法中，通过扭矩模型构建z轴陀螺的冗余信息，建立故障检测函数，实现对z轴陀螺故障、磁传感器故障以及扭矩模型故障的检测；同时，在z轴陀螺故障情况下，通过扭矩模型对其角速度信号进行重构，代替故障的陀螺进行导航解算，实现容错导航。该方法无需增加额外的惯性传感器，可实现对z轴陀螺的故障检测、隔离、信号重构，保障陀螺失效情况下航向角的估计精度，并且能够检测磁传感器故障。

附图说明

图1是本发明的流程图；