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

摘要

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

主权项

1.一种四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法，其特征在于包括如下步骤：步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；磁传感器信息ψ_m(k)；陀螺信息其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量；加计信息分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量；步骤二：执行故障检测滤波器及故障定位策略，判断z轴陀螺、扭矩模型、磁传感器的故障；步骤三：根据步骤二的故障定位结果，确定各个子滤波器的状态方程，进行扭矩模型/z轴陀螺子滤波器、扭矩模型/磁传感器子滤波器、扭矩模型/加速度计子滤波器的滤波融合估计；步骤四：根据故障检测结果，对扭矩模型/z轴陀螺子滤波器、扭矩模型/磁传感器子滤波器、扭矩模型/加速度计子滤波器进行故障隔离，对无故障的子滤波器进行全局融合；步骤五：根据步骤四的故障定位和隔离结果对各个子滤波器、故障检测滤波器状态量进行重置，并执行系统重置策略。