[发明专利]基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法

摘要

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

主权项

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