四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法与流程

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

背景技术

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

附图说明

图1是本发明的流程图；

图2是故障检测结构图；

图3是z轴陀螺故障时的故障检测结果；

图4是z轴陀螺故障时的z轴角速度估计结果；

图5是z轴陀螺故障时的航向角估计结果；

图6是扭矩模型故障时的故障检测结果；

图7是磁传感器故障时的故障检测结果。

具体实施方式

以下将结合附图，对本发明的技术方案及有益效果进行详细说明。

如图1所示，本发明提供一种四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法，其故障检测结构如图2所示，具体是采用半物理仿真的形式，四旋翼无人机做带有z轴角加速度的航向转动，采集机载传感器实验数据，其包含惯性传感器实验数据、电子调速器数据、gps数据、磁传感器数据。所述方法包括如下步骤：

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

步骤二：执行故障检测滤波器及故障定位策略，判断z轴陀螺、扭矩模型、磁传感器的故障，故障检测滤波器由扭矩模型/z轴陀螺检测滤波器、扭矩模型/磁传感器检测滤波器、z轴陀螺/磁传感器检测滤波器3个检测滤波器组成，上述3个检测滤波器的状态更新、故障定位步骤如下：

步骤21，扭矩模型/z轴陀螺检测滤波器故障检测过程步骤如下：

步骤211，计算k时刻扭矩模型/z轴陀螺检测滤波器的状态估计值及估计均方误差

kz0(k)＝kz0(k-1)

kz1(k)＝kz1(k-1)

kz2(k)＝kz2(k-1)

式中，kz0(k)、kz1(k)、kz2(k)为k时刻扭矩模型系数；kz0(k-1)、kz1(k-1)、kz2(k-1)为k-1时刻扭矩模型系数；ωmz(k-1)为通过扭矩模型计算得到的k-1时刻的机体系相对于导航系的角速度在机体系z轴上的分量；ωmz(k)为通过扭矩模型计算得到的k时刻的机体系相对于导航系的角速度在机体系z轴上的分量；ωi(k)为第i个旋翼转速，i＝1,2,3,4；δt为离散采样周期；上标t表示转置；为雅可比矩阵，i3×3为3×3的单位矩阵，03×1为3×1的零矩阵；为k时刻估计均方误差；gd1(k-1)＝[i4×4]，为系统噪声系数矩阵，i4×4为4×4单位矩阵，；wd1(k-1)＝[wkz0(k-1)wkz1(k-1)wkz2(k-1)wmz(k-1)]^t，为系统噪声，wkz0(k-1)、wkz1(k-1)、wkz2(k-1)为k-1时刻扭矩模型系数的白噪声；wmz(k-1)为k-1时刻扭矩模型白噪声；

步骤212，计算k时刻扭矩模型/z轴陀螺检测滤波器的故障统计参数

式中，为k时刻扭矩模型/z轴陀螺检测滤波器的统计参数，为k时刻的残差，为机体系相对于导航系的角速度在机体系z轴上的分量，通过陀螺输出获得，为k时刻的残差方差，hd1(k)＝[0001]^t，rd1(k)＝diag([wgz(k)]²)，wgz(k)为k时刻的机体系z轴陀螺的白噪声，diag表示对角化，-1表示求逆；

步骤213，根据故障统计参数结果，计算k时刻扭矩模型/z轴陀螺检测滤波器的检测函数

式中，t1是阈值，当j1(k)＝1时，扭矩模型或z轴陀螺故障；当j1(k)＝0时，扭矩模型和z轴陀螺均无故障；

步骤22，扭矩模型/磁传感器检测滤波器故障检测过程步骤如下：

步骤221，计算k时刻扭矩模型/磁传感器检测滤波器的状态估计值及估计均方误差

kz0(k)＝kz0(k-1)

kz1(k)＝kz1(k-1)

kz2(k)＝kz2(k-1)

式中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻的四元素；为k时刻机体系相对于导航系的角速度在机体系x、y轴上的分量，通过x、y轴陀螺输出获得；为k时刻估计均方误差；i3×3为3×3的单位矩阵，03×1为

3×1的零矩阵，03×4为3×4的零矩阵，01×4为1×4的零矩阵，04×3为4×3的零矩阵，

wd2(k-1)＝[wk0(k-1)wk1(k-1)wk2(k-1)wmz(k-1)wgx(k-1)wgy(k-1)wωz(k-1)]^twgx(k-1)、wgy(k-1)分别为机体系x、y轴陀螺的白噪声，wωz(k-1)为z轴角速度白噪声,01×3为1×3的零矩阵；04×1为4×1的零矩阵；

步骤222，计算k时刻检测滤波器的故障统计参数

式中，为k时刻扭矩模型/磁传感器检测器的统计参数，为k时刻的残差，yd2(k)＝ψm(k)，ψm(k)为k时刻磁航向角，通过磁传感器获得；为k时刻的残差方差，rd2(k)＝diag([wψm(k)]²)，wψm(k)为k时刻磁航向角白噪声；

步骤223，根据故障统计参数结果，计算k时刻检测滤波器的检测函数

式中，t2是阈值，当j2(k)＝1时，扭矩模型或磁传感器故障；当j2(k)＝0时，扭矩模型或磁传感器均无故障；

步骤23，z轴陀螺/磁传感器检测滤波器检测过程步骤如下：

步骤231，计算k时刻z轴陀螺/磁传感器检测滤波器的状态估计值及估计均方误差

式中，为雅可比矩阵；wd3(k-1)＝[wgx(k-1)wgy(k-1)wgz(k-1)]^t；

步骤232，计算k时刻检测滤波器的故障统计参数

式中，为k时刻z轴陀螺/磁传感器检测器的统计参数；为k时刻的残差；为k时刻的残差方差；yd3(k)＝ψm(k)；

步骤233，计算k时刻检测滤波器的检测函数

式中，t3是阈值，当j3(k)＝1时，z轴陀螺或磁传感器故障；当j3(k)＝0时，z轴陀螺或磁传感器均无故障；

步骤24，根据各个检测滤波器的检测函数结果，计算k时刻故障定位函数及根据故障定位函数计算结果，进行故障定位，步骤如下：

步骤241，计算k时刻预发生故障定位函数

式中，“∨”表示逻辑运算中“或”运算符，“∧”表示逻辑运算中“与”运算符；当fpre(k)＝1时，进入预发生故障阶段；当fpre(k)＝0时，未进入预发生故障阶段；

步骤242，计算z轴陀螺故障定位函数

式中，“∧”表示逻辑运算中“与”运算符，“-”表示逻辑运算中“非”运算符；当fg(k)＝1时，z轴陀螺故障；当fg(k)＝0时，z轴陀螺无故障；

步骤243，计算扭矩模型故障定位函数

当ft(k)＝1时，扭矩模型故障；当ft(k)＝0时，扭矩模型无故障；

步骤244，计算磁传感器故障定位函数

当fmag(k)＝1时，磁传感器故障；当fmag(k)＝0时，磁传感器无故障；

步骤245，计算无故障定位函数

当fno(k)＝1时，无故障。

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

步骤31，计算k时刻三个子滤波器的状态预测及预测均方误差

情况311，无故障或z轴陀螺故障或磁传感器故障时，状态预测及预测均方误差过程如下：

步骤3111，计算k时刻扭矩模型系数

kz0(k|k-1)＝kz0(k-1)

kz1(k|k-1)＝kz1(k-1)

kz2(k|k-1)＝kz2(k-1)

式中，kz0(k|k-1)、kz1(k|k-1)、kz2(k|k-1)为扭矩模型参数在k-1时刻到k时刻的状态一步预测；

步骤3112，计算k时刻角速度预测

式中，ωmz(k|k-1)为z轴角速度在k-1时刻到k时刻的状态一步预测；

步骤3113，计算k时刻四元素预测

式中，q0(k|k-1)、q1(k|k-1)、q2(k|k-1)、q3(k|k-1)为四元素在k-1时刻到

k时刻的状态一步预测；

步骤3114，计算一步预测均方误差pc(k|k-1)

pc(k|k-1)＝φc(k|k-1)pc(k-1)φc(k|k-1)^t+gc(k-1)qc(k-1)gc(k-1)^t

式中，pc(k|k-1)为k-1到k时刻的一步预测均方误差；为雅可比矩阵，为非线性状态方程，为k-1时刻状态量，qc(k-1)＝diag(wc(k-1)²)；wc(k-1)＝[wk0(k-1)wk1(k-1)wk2(k-1)wmz(k-1)wgx(k-1)wgy(k-1)wωz(k-1)]^t为系统噪声，

情况312，预发生故障或扭矩模型故障时，状态预测及预测均方误差过程如下：

3个子滤波器的状态方程中关于z轴角速度的预测方程修改为ωmz(k|k-1)＝ωmz(k-1)，雅可比矩阵修改为：其余同步骤311情况；

步骤32，扭矩模型/z轴陀螺子滤波器量测更新步骤如下：

步骤321，计算k时刻的扭矩模型/z轴陀螺子滤波器的滤波增益

kc1(k)＝pc(k|k-1)hc1(k)^t[hc1(k)pc(k|k-1)hc1(k)^t+rc1(k)]^-1

式中，hc1(k)＝[01×3101×4]；kc1(k)为k时刻的滤波增益，rc1(k)＝diag([wgz(k)]²)，为k时刻的量测噪声；

步骤322，计算k时刻扭矩模型/z轴陀螺子滤波器状态估计值

式中，为k时刻状态量的估计值；为k时刻的量测量；

步骤323，计算k时刻扭矩模型/z轴陀螺子滤波器估计均方误差pc1(k)

pc1(k)＝[i-kc1(k)hc1(k)]pc(k|k-1)

式中，pc1(k)为k时刻估计均方误差；i为单位矩阵；

步骤33，扭矩模型/磁传感器子滤波器量测更新步骤如下：

步骤331，计算k时刻的扭矩模型/磁传感器子滤波器的滤波增益

kc2(k)＝pc(k|k-1)hc2(k)^t[hc2(k)pc(k|k-1)hc2(k)^t+rc2(k)]^-1

式中，hc2(k)＝[01×4n1×4]，kc2(k)为k时刻的滤波增益，为k时刻的量测噪声，

为k时刻的磁航向白噪声；

步骤332，计算k时刻扭矩模型/磁传感器子滤波器状态估计值

式中，为k时刻状态量的估计；yc2(k)＝ψm，为k时刻的量测量；其中，q0＝q0(k|k-1)、q1＝q1(k|k-1)、q2＝q2(k|k-1)、q3＝q3(k|k-1)；

步骤333，计算k时刻扭矩模型/磁传感器子滤波器估计均方误差pc2(k)

pc2(k)＝[i-kc2(k)hc2(k)]pc(k|k-1)

式中，pc2(k)为k时刻估计均方误差，i为单位矩阵；

步骤34，扭矩模型/加速度计子滤波器量测更新步骤如下：

步骤341，计算k时刻的扭矩模型/加速度计子滤波器的滤波增益

kc3(k)＝pc(k|k-1)hc3(k)^t[hc3(k)pc(k|k-1)hc3(k)^t+rc3(k)]^-1

式中，hc3(k)＝[02×4ν2×4],g表示重力加速度；kc3(k)为k时刻的滤波增益；rc3(k)＝diag([wfbx(k)wfby(k)]²)，为k时刻的量测噪声，wfbx(k)、wfby(k)分别为机体系x、y轴加速度计白噪声；

步骤342，计算k时刻扭矩模型/加速度计子滤波器状态估计值

式中，为k时刻状态量的估计值，为k时刻的量测量，

步骤343，计算k时刻扭矩模型/加速度计子滤波器估计均方误差pc3(k)

pc3(k)＝[i-kc3(k)hc3(k)]pc(k|k-1)

式中，pc3(k)为k时刻估计均方误差，i为单位矩。

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

情况41，无故障时，各个子滤波器按照如下进行故障隔离

步骤411，根据故障定位结果，隔离故障子滤波器

三个子滤波器均参与全局滤波，无子滤波器被隔离；

步骤412，计算k时刻的全局滤波器的估计均方误差

pg(k)＝[pc1(k)^-1+pc2(k)^-1+pc3(k)^-1]^-1

步骤413，计算k时刻的全局滤波器的状态估计值

情况42，预发生故障时，各个子滤波器按照如下进行故障隔离

步骤421，根据故障定位结果，隔离故障子滤波器

采用扭矩模型故障时的状态方程，隔离扭矩模型/z轴陀螺子滤波器；

步骤422，计算k时刻的全局滤波器的估计均方误差

pg(k)＝[pc2(k)^-1+pc3(k)^-1]^-1

步骤423，计算k时刻的全局滤波器的状态估计值

情况43，z轴陀螺故障时，各个子滤波器按照如下进行故障隔离

步骤431，根据故障定位结果，隔离故障子滤波器

扭矩模型/z轴陀螺子滤波器被隔离，不进行全局融合滤波估计；

步骤432，计算k时刻的全局滤波器的估计均方误差

pg(k)＝[pc2(k)^-1+pc3(k)^-1]^-1

步骤433，计算k时刻的全局滤波器的状态估计值

情况44，扭矩模型故障时，各个子滤波器按照如下进行故障隔离

步骤441，根据故障定位结果，隔离故障子滤波器

采用扭矩模型故障时的状态方程，无子滤波器被隔离；

步骤442，计算k时刻的全局滤波器的估计均方误差

pg(k)＝[pc1(k)^-1+pc2(k)^-1+pc3(k)^-1]^-1

步骤443，计算k时刻的全局滤波器的状态估计值

情况45，磁传感器故障时，各个子滤波器按照如下进行故障隔离

步骤451，根据故障定位结果，隔离故障子滤波器

使用冗余的磁传感器代替故障的磁传感器进行子滤波器滤波；

步骤452，计算k时刻的全局滤波器的估计均方误差

pg(k)＝[pc1(k)^-1+pc2(k)^-1+pc3(k)^-1]^-1

步骤453，计算k时刻的全局滤波器的状态估计值

无子滤波器被隔离，按照步骤三中的磁传感器故障时，在扭矩模型/磁传感器子滤波器中使用冗余的磁传感器代替故障的磁传感器进行融合滤波。

步骤五：根据步骤四的故障定位和隔离结果对各个子滤波器、故障检测滤波器状态量进行重置，并执行系统重置策略：

步骤51，根据故障检测结果及全局滤波结果对各个子滤波器进行状态量及均方误差重置

情况511，无故障时，三个子滤波器状态量及均方误差重置

pc(k)＝3pg(k)

情况512，预发生故障时，三个子滤波器状态量及均方误差重置

pc(k)＝2pg(k)

情况513，z轴陀螺故障时，三个子滤波器状态量及均方误差重置

pc(k)＝2pg(k)

情况514，扭矩模型故障时，三个子滤波器状态量及均方误差重置

pc(k)＝3pg(k)

情况515，磁传感器故障时，三个子滤波器状态量及均方误差重置

pc(k)＝3pg(k)

步骤52，根据故障检测结果及全局滤波结果对各个检测滤波器进行状态量及均方误差重置

检测滤波器为n步预测卡方检测器，设置n步重置周期，在n步之内不进行重置，使用自身的状态估计结果进行状态递推，在第n步时使用全局滤波结果进行状态量重置，状态及均方误差重置根据滤波结果采用如下方式：

情况521，无故障时，第n步检测滤波器状态及均方误差重置

式中，表示使用状态向量中的第1列中，第i到第j行元素进行重置；pg(k)[i:j；i:j]表示使用误差估计矩阵中的第i到第j列、第i到第j行元素进行重置；

情况522，预发生故障时，检测滤波器状态及均方误差重置为如下：

扭矩模型/z轴陀螺检测滤波器、扭矩模型/磁传感器检测滤波器、z轴陀螺/磁传感器检测滤波器不进行重置更新，只进行累积直至确定故障位置；

情况523，z轴陀螺故障时，检测滤波器状态及均方误差重置

当检测到z轴陀螺故障时，对扭矩模型/z轴陀螺检测滤波器、z轴陀螺/磁传感器检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况524，扭矩模型故障时，检测滤波器状态及均方误差重置

当检测到扭矩模型故障时，对扭矩模型/z轴陀螺检测滤波器、扭矩模型/磁传感器检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况525，磁传感器故障时，检测滤波器状态及均方误差重置

当检测到磁传感器故障时，扭矩模型/磁传感器检测滤波器、z轴陀螺/磁传感器检测滤波器不使用n步重置规则，每一步均进行重置，其他的检测其按照n步重置规则，采用如下方法进行重置：

步骤53，当检测到故障后，基于故障特性，执行以下系统重置策略

设置z轴陀螺和扭矩模型故障隔离周期，当检测到z轴陀螺或扭矩模型故障后，进入故障隔离周期，在隔离周期内认为一直存在故障，在隔离周期内若再次检测到故障，重新开始隔离周期，若隔离周期结束后仍未检测到故障，则认为故障消失，系统处于无故障状态。