CN111044051A - 一种复合翼无人机容错组合导航方法 - Google Patents

一种复合翼无人机容错组合导航方法 Download PDF

Info

Publication number
CN111044051A
CN111044051A CN201911401131.XA CN201911401131A CN111044051A CN 111044051 A CN111044051 A CN 111044051A CN 201911401131 A CN201911401131 A CN 201911401131A CN 111044051 A CN111044051 A CN 111044051A
Authority
CN
China
Prior art keywords
fault
gps
filter
sub
optical flow
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911401131.XA
Other languages
English (en)
Other versions
CN111044051B (zh
Inventor
何鹤
何明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Interstellar Intelligent Aviation Taicang Aviation Technology Co ltd
Original Assignee
Star Jiangsu Aviation Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Star Jiangsu Aviation Technology Co Ltd filed Critical Star Jiangsu Aviation Technology Co Ltd
Priority to CN201911401131.XA priority Critical patent/CN111044051B/zh
Publication of CN111044051A publication Critical patent/CN111044051A/zh
Application granted granted Critical
Publication of CN111044051B publication Critical patent/CN111044051B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明属于交通安全技术领域，具体涉及一种复合翼无人机容错组合导航方法，包括以下步骤：S1、处理无人机真实飞行数据，获得导航解算所需参数；S2、建立联邦卡尔曼滤波器；S3、为联邦卡尔曼滤波器注入故障信号；S4、进行故障检测，获取故障检测结果；S5、利用故障检测结果进行数据融合重构后，获得容错后的导航解算信息。本发明采用联邦卡尔曼滤波结构建立组合导航系统，通过残差卡方检验与改进的融合方法，实现对传感器故障的检测与处理，自适应的进行数据融合，加强系统的可靠性与精确性，提高了无人机组合导航系统的容错性能，扩展了无人机的可飞行场景。

Description

一种复合翼无人机容错组合导航方法
技术领域
本发明属于无人机导航技术领域，具体涉及一种复合翼无人机容错组合导航方法。
背景技术
目前应用最为广泛的无人机组合导航系统由惯性测量单元(IMU)和全球定位系统(GPS)组成，采用扩展卡尔曼滤波算法进行数据融合，解算导航信息，但是该方法对GPS依赖程度较高，当GPS信号受到干扰或遮挡时，组合导航系统解算精度急剧下降，威胁飞行安全。
此外，由于传统的无人机组合导航系统，大都由惯性测量单元(IMU)和全球定位系统(GPS)组成，采用如互补滤波、扩展卡尔曼滤波、粒子滤波等方法进行数据融合，这些方法虽然都能够抑制导航参数发散、提高解算精度，但是其可靠性较低，且不具备容错性。
发明内容
有鉴于此，本发明提供了一种复合翼无人机容错组合导航方法，以解决上述提出的技术问题。
本发明的技术方案是：
一种复合翼无人机容错组合导航方法，包括以下步骤：
S1、处理无人机真实飞行数据，获得导航解算所需参数；
S2、建立联邦卡尔曼滤波器；
S3、为联邦卡尔曼滤波器注入故障信号；
S4、进行故障检测，获取故障检测结果；
S5、利用故障检测结果进行数据融合重构后，获得容错后的导航解算信息。
优选的，步骤S1中处理无人机真实飞行数据，获得导航解算所需参数的方法，包括如下步骤：
S11、从原始飞行数据中选择三轴陀螺仪数据、三轴加速度计数据、GPS速度及位置数据、光流数据进行保存；
S12、从原始飞行数据中选择无人机正常飞行时的位置、速度、姿态角作为比较基准值。
优选的，步骤S2中建立联邦卡尔曼滤波器的方法，包括如下步骤：
S21、确定滤波器结构的类型；
S22、确定子滤波器所用传感器的类型，进行子滤波器的结构设计；
S23、以子滤波器的结构为基础，分别进行子滤波器的算法设计，建立状态方程和量测方程，确定量测量。
优选的，步骤S21中确定滤波器结构的类型为联邦卡尔曼滤波无反馈结构。
优选的，步骤S22中确定子滤波器的类型为GPS和光流传感器的组合。
优选的，步骤S23中确定子滤波器的算法为扩展卡尔曼滤波算法，选择量测量为GPS位置、GPS速度和光流速度。
优选的，步骤S4中采用改进的残差卡方检验法进行故障检测，包括如下步骤：
S41、利用式(1)计算故障检测函数值λk
Figure BDA0002347493590000021
其中，
Figure BDA0002347493590000022
rk为残差，Zk为量测量，Hk为量测矩阵，
Figure BDA0002347493590000023
为状态一步预测，
Ak为残差的方差，Pk/k-1为一步预测均方误差，
Figure BDA00023474935900000311
为量测矩阵的转置，Rk为量测噪声阵，λk服从自由度为m的卡方分布，即λk～χ2(m)，m为量测Zk的维数，
Figure BDA0002347493590000031
为残差的转置，
Figure BDA0002347493590000032
为残差的方差的逆，rk为残差；
S42、将计算出的故障检测函数λk与故障判断的上下门限值T1,T2进行对比，
当λk≤T1，判定无故障；
当T1<λk≤T2，判定故障较为轻微；
当λk>T2，判定完全故障。
优选的，S42中当故障判断结果是故障较为轻微时，出故障的子滤波器的输出会自适应的进入数据融合，进行数据融合重构的方法，包括如下步骤：
S421、利用式(2)计算数据融合的自适应因子ρ
Figure BDA0002347493590000033
其中，ρ为自适应因子，T1,T2为故障判断的门限值，λk为故障检测函数值；
S422、利用式(3)求解全局状态估计
Figure BDA0002347493590000034
Figure BDA0002347493590000035
其中，
αgps＝ρgps-ρgpsof，
Figure BDA0002347493590000036
αof＝ρof-ρgpsof，
Figure BDA0002347493590000037
αgpsof＝ρgpsof，
Figure BDA0002347493590000038
αgps是GPS子滤波器状态量生效程度，ρgps是GPS正常时的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子，
Figure BDA0002347493590000039
为GPS子滤波器状态估计，
Figure BDA00023474935900000310
为子滤波器1的状态估计，αof是光流子滤波器状态量生效程度，ρof是光流正常的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子，
Figure BDA0002347493590000041
为光流子滤波器状态估计，
Figure BDA0002347493590000042
为子滤波器2的状态估计，αgpsof是GPS与光流子滤波器的融合状态量生效程度，ρgpsof是GPS与光流均正常时的自适应因子，
Figure BDA0002347493590000043
为GPS与光流子滤波器融合状态估计，
Figure BDA0002347493590000044
为GPS子滤波器状态估计，P1为子滤波器的估计均方误差，P2为子滤波器2的估计均方误差。
与现有技术相比，本发明提供的一种复合翼无人机容错组合导航方法，在无人机实际飞行的数据基础上，为系统加入光流传感器作为GPS的余度，采用联邦卡尔曼滤波结构建立组合导航系统，通过残差卡方检验与改进的融合方法，实现对传感器故障的检测与处理，自适应的进行数据融合，加强系统的可靠性与精确性，提高了无人机组合导航系统的容错性能，扩展了无人机的可飞行场景，实用性好，值得推广。
附图说明
图1为本发明的技术方案流程图；
图2为本发明的联邦卡尔曼滤波结构框图；
图3为本发明的光流传感器信息解算示意图；
图4为本发明的改进的故障判断逻辑；
图5为本发明的无故障-东向速度误差；
图6为本发明的GPS注入故障-GPS子滤波器的故障检测函数λk；
图7为本发明的GPS注入故障-东向速度误差；
图8为本发明的GPS注入故障-自适应融合后东向速度误差；
图9为本发明的无故障-东向速度误差；
图10为本发明的OF注入故障-OF子滤波器的故障检测函数λk；
图11为本发明的OF注入故障-东向速度误差；
图12为本发明的OF注入故障-自适应融合后东向速度误差。
具体实施方式
下面结合附图1到图12对本发明提供的一种复合翼无人机容错组合导航方法的一个具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。
实施例1
如图1所示，本发明提供的一种复合翼无人机容错组合导航方法，包括以下步骤：
S1、处理无人机真实飞行数据，获得导航解算所需参数；
S2、建立联邦卡尔曼滤波器；
S3、为联邦卡尔曼滤波器注入故障信号；
S4、进行故障检测，获取故障检测结果；
S5、利用故障检测结果进行数据融合重构后，获得容错后的导航解算信息。
进一步的，步骤S1中处理无人机真实飞行数据，获得导航解算所需参数的方法，包括如下步骤：
S11、从原始飞行数据中选择三轴陀螺仪数据、三轴加速度计数据、GPS速度及位置数据、光流数据进行保存；
S12、从原始飞行数据中选择无人机正常飞行时的位置、速度、姿态角作为比较基准值。
进一步的，步骤S2中建立联邦卡尔曼滤波器的方法，包括如下步骤：
S21、确定滤波器结构的类型；
S22、确定子滤波器所用传感器的类型，进行子滤波器的结构设计；
S23、以子滤波器的结构为基础，分别进行子滤波器的算法设计，建立状态方程和量测方程，确定量测量。
进一步的，步骤S21中确定滤波器结构的类型为联邦卡尔曼滤波无反馈结构。
进一步的，步骤S22中确定子滤波器的类型为GPS和光流传感器的组合。
进一步的，步骤S23中确定子滤波器的算法为扩展卡尔曼滤波算法，选择量测量为GPS位置、GPS速度和光流速度。
进一步的，步骤S4中采用改进的残差卡方检验法进行故障检测，包括如下步骤：
S41、利用式(1)计算故障检测函数值λk
Figure BDA0002347493590000061
其中，
Figure BDA0002347493590000062
rk为残差，Zk为量测量，Hk为量测矩阵，
Figure BDA0002347493590000063
为状态一步预测，
Ak为残差的方差，Pk/k-1为一步预测均方误差，
Figure BDA0002347493590000064
为量测矩阵的转置，Rk为量测噪声阵，λk服从自由度为m的卡方分布，即λk～χ2(m)，m为量测Zk的维数，
Figure BDA0002347493590000065
为残差的转置，
Figure BDA0002347493590000066
为残差的方差的逆，rk为残差；
S42、将计算出的故障检测函数λk与故障判断的上下门限值T1,T2进行对比，
当λk≤T1，判定无故障；
当T1<λk≤T2，判定故障较为轻微；
当λk>T2，判定完全故障。
进一步的，S42中当故障判断结果是故障较为轻微时，出故障的子滤波器的输出会自适应的进入数据融合，进行数据融合重构的方法，包括如下步骤：
S421、利用式(2)计算数据融合的自适应因子ρ
Figure BDA0002347493590000067
其中，ρ为自适应因子，T1,T2为故障判断的门限值，λk为故障检测函数值；
S422、利用式(3)求解全局状态估计
Figure BDA0002347493590000071
Figure BDA0002347493590000072
其中，
αgps＝ρgps-ρgpsof，
Figure BDA0002347493590000073
αof＝ρof-ρgpsof，
Figure BDA0002347493590000074
αgpsof＝ρgpsof，
Figure BDA0002347493590000075
αgps是GPS子滤波器状态量生效程度，ρgps是GPS正常时的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子，
Figure BDA0002347493590000076
为GPS子滤波器状态估计，
Figure BDA0002347493590000077
为子滤波器1的状态估计，αof是光流子滤波器状态量生效程度，ρof是光流正常的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子，
Figure BDA0002347493590000078
为光流子滤波器状态估计，
Figure BDA0002347493590000079
为子滤波器2的状态估计，αgpsof是GPS与光流子滤波器的融合状态量生效程度，ρgpsof是GPS与光流均正常时的自适应因子，
Figure BDA00023474935900000710
为GPS与光流子滤波器融合状态估计，
Figure BDA00023474935900000711
为GPS子滤波器状态估计，P1为子滤波器的估计均方误差，P2为子滤波器2的估计均方误差。
实施例1
该实施例具体的以一种复合翼无人机容错组合导航方法为例，在利用无人机试飞试验获得导航原始数据后，对试飞导航原始数据进行处理，获得如下各导航解算所需信息：
三轴陀螺仪
Figure BDA0002347493590000081
其中，
gyro_rad[0],gyro_rad[1],gyro_rad[2]分别为三轴陀螺仪数据。
三轴加速度计
Figure BDA0002347493590000082
其中，
accelerometer_m_s2[0],accelerometer_m_s2[1],accelerometer_m_s2[2]
分别为三轴加速度计数据。
将无人机正常飞行状态下的位置、速度、姿态角作为比较的基准值，
Figure BDA0002347493590000083
其中，
lat为纬度、lon为经度、alt为高度。
Figure BDA0002347493590000084
其中，
vel_n为北向速度、vel_e为东向速度、vel_d为地向速度。
Figure BDA0002347493590000085
其中，
pitch为俯仰角、roll为滚转角、yaw为偏航角。
GPS位置、速度
Figure BDA0002347493590000091
其中，
lat为GPS纬度、lon为GPS经度、alt为GPS高度。
Figure BDA0002347493590000092
其中，
vel_n为GPS北向速度、vel_e为GPS东向速度、vel_d为GPS地向速度。
光流速度
以水平x方向上的光流速度求解为例，对光流速度解算作原理说明：
如图3所示，假定存在一点P，在光流传感器一个采样间隔的时间内，从P0移动到了P1，走过了距离X，该点到光流摄像头的距离为Z，即深度；在相同时间内，该点在成像平面上对应的点p移动了距离x。
光流传感器能够给导航系统直接提供一个采样间隔内，成像平面上的点p积累的弧度值：
Figure BDA0002347493590000093
其中，pixel_flow_x_integral为一个采样间隔内，光流传感器运动在x方向上积累的弧度值，pixel_flow_y_integral为一个采样间隔内，光流传感器运动在y方向上积累的弧度值
另外，光流传感器一般搭配测距仪使用，以提供深度信息Z。
所以，由光流传感器信息解算无人机在x方向上运动速度的公式为
Figure BDA0002347493590000101
其中，Vx为无人机在x方向上运动速度，pixel_flow_x_integral为一个采样间隔内，光流传感器运动在x方向上积累的弧度值，Z为深度信息，dt为单位采样间隔时间。
建立联邦卡尔曼滤波器
为了增加组合导航系统的容错能力，本发明选择无复位形式的联邦卡尔曼滤波结构，结构图如图2所示。
滤波方式
由于间接法滤波相较于直接法滤波具有低计算量、对计算周期无严格要求等便利优点，所以本发明此处选用间接滤波法。系统的误差量就是滤波器的状态量，选取系统的18个状态量如下：
Figure BDA0002347493590000105
其中，δL δλ δh分别是捷联惯性导航系统的经纬高误差；δVE δVN δVU分别是捷联惯性导航系统的东向、北向、垂直方向速度误差；φE φN φU是系统的三个姿态角误差；εbxεby εbz是陀螺仪的三个随机漂移；εrx εry εrz是陀螺仪三个方向的相关漂移；
Figure BDA0002347493590000102
是加速度计三个方向的随机漂移。
由卡尔曼滤波基本理论可知，系统的状态方程可以表达为：
Figure BDA0002347493590000103
其中，
Figure BDA0002347493590000104
为系统状态估计，F(t)为系统的状态转移矩阵，X(t)为系统的状态量，G(t)为系统的噪声分配矩阵，系统的白噪声W(t)为：
W(t)＝[ωgx ωgy ωgz ωrx ωry ωrz ωax ωay ωaz]T
其中，ωgx ωgy ωgz是陀螺仪随机漂移在载体坐标系下的白噪声；ωrx ωry ωrz是载体坐标系下陀螺仪相关漂移的白噪声；ωax ωay ωaz是载体坐标系下加速度计随机漂移的白噪声。
子滤波器1(GPS)设计
子滤波器1的观测量是GPS给出的速度信息和位置信息与捷联惯性导航系统的速度信息和位置信息的差值。
定义速度的观测量为：
Figure BDA0002347493590000111
其中，Zvet(t)为速度的观测量，VEINS,VNINS,VUINS为捷联惯导系统解算的东向、北向、天向速度，VEGPS,VNGPS,VUGPS为GPS的东向、北向、天向速度。
定义位置的观测量为：
Figure BDA0002347493590000112
其中，Zpos(t)为位置的观测量，LINS,λINS,hINS为捷联惯导系统解算的纬度、经度、高度，LGPS,λGPS,hGPS为GPS的纬度、经度、高度。
综合上式，可以知道子滤波器1的量测方程为：
Figure BDA0002347493590000113
其中，Z1(t)为子滤波器1的量测量，Zvet(t)为速度的观测量，Zpos(t)为位置的观测量，H1(t)为量测矩阵，X1(t)为子滤波器1的状态量，W1(t)为量测噪声矩阵。
子滤波器2(光流传感器)设计
子滤波器2的观测量是光流传感器给出的速度信息与捷联惯性导航系统的速度信息的差值。
子滤波器2的量测方程为：
Figure BDA0002347493590000121
其中，Z2(t)为子滤波器2的量测量，VEINS,VNINS,VUINS为捷联惯导系统解算的东向、北向、天向速度，VEOF,VNOF,VUOF为光流的的东向、北向、天向速度，H2(t)为量测矩阵，X2(t)为子滤波器2的状态量，W2(t)为量测噪声矩阵。
改进的残差卡方检验法与自适应融合方法
残差卡方检验法
每个子滤波器均为卡尔曼滤波器，其残差为
Figure BDA0002347493590000122
其中，rk为残差，Zk为量测量，Hk为量测矩阵，
Figure BDA0002347493590000123
为状态一步预测。
最优滤波器有如下性质：当无故障发生时卡尔曼滤波器的残差rk是零均值高斯白噪声，其方差为
Figure BDA0002347493590000124
其中，Ak为残差的方差，Hk为量测矩阵，Pk/k-1为一步预测均方误差，
Figure BDA0002347493590000125
为量测矩阵的转置，Rk为量测噪声阵。
当系统发生故障时，残差rk的均值就不再为零了。因此，通过对残差rk的均值的检验可以确定系统是否发生了故障。
相应的故障检测函数为：
Figure BDA0002347493590000126
式中λk是服从自由度为m的卡方分布，即λk～χ2(m)，m为量测Zk的维数，
Figure BDA0002347493590000131
为残差的转置，
Figure BDA0002347493590000132
为残差的方差的逆，rk为残差。
在实际情况中，SINS/GPS组合导航系统对于GPS信息的依赖程度较高，如果在故障程度较为轻微时判断其发生故障，并且弃置时间段内所有GPS信息，反而会导致导航解算精度下降。
由此，本发明提出对故障判断的二元逻辑进行修改，如图4所示：
①若λk≤T1，判定无故障；
②若λk>T2，判定完全故障，此时对应子滤波器状态估计应舍弃，不进入数据融合；
③若T1<λk≤T2，判定传感器故障较为轻微，随着λk的增加，故障的严重程度增加，此时对应子滤波器状态应当根据故障程度，自适应的进入融合。相应的，数据融合的自适应因子如下：
Figure BDA0002347493590000133
其中，ρ为自适应因子，T1,T2为故障判断的门限值，λk为故障检测函数值。
自适应数据融合
根据上述改进后的故障判断准则，可以使用自适应因子ρ表达对应情况下，各子滤波器的生效程度α：
1、计算GPS子滤波器状态量生效程度，
αgps＝ρgps-ρgpsof
其中，αgps是GPS子滤波器状态量生效程度，ρgps是GPS正常时的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子。
2、计算光流子滤波器状态量生效程度，
αof＝ρof-ρgpsof
其中，αof是光流子滤波器状态量生效程度，ρof是光流正常时的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子。
3、计算GPS与光流融合状态量生效程度，
αgpsof＝ρgpsof
其中，αgpsof是GPS与光流子滤波器的融合状态量生效程度，ρgpsof是GPS与光流均正常时的自适应因子。
相对应的，上述三种情况下，对应的状态估计为：
1、GPS子滤波器状态估计，
Figure BDA0002347493590000141
其中，
Figure BDA0002347493590000142
为GPS子滤波器状态估计，
Figure BDA0002347493590000143
为子滤波器1的状态估计。
2、光流子滤波器状态估计，
Figure BDA0002347493590000144
其中，
Figure BDA0002347493590000145
为光流子滤波器状态估计，
Figure BDA0002347493590000146
为子滤波器2的状态估计。
3、GPS与光流融合状态估计，
Figure BDA0002347493590000147
其中，
Figure BDA0002347493590000148
为GPS与光流子滤波器融合状态估计，
Figure BDA0002347493590000149
为GPS子滤波器状态估计，
Figure BDA00023474935900001410
为光流子滤波器状态估计，P1为子滤波器1的估计均方误差，P2为子滤波器2的估计均方误差，
Figure BDA00023474935900001411
为子滤波器1的估计均方误差的逆，
Figure BDA00023474935900001412
为子滤波器2的估计均方误差的逆。
综上所述，全局状态估计可表示为：
Figure BDA00023474935900001413
其中，
Figure BDA0002347493590000151
为全局状态估计，αgps是GPS子滤波器状态量生效程度，
Figure BDA0002347493590000152
为GPS子滤波器状态估计，αof是光流子滤波器状态量生效程度，
Figure BDA0002347493590000153
为光流子滤波器状态估计，αgpsof是GPS与光流子滤波器的融合状态量生效程度，
Figure BDA0002347493590000154
为GPS与光流子滤波器融合状态估计。
仿真验证
仿真使用复合翼无人机正常飞行试验中获取的组合导航解算结果作为精度比较的基准值，具有较高的融合精度，在实际试飞中表现良好，因此可以用它作为融合精度的基准。
故障判定的门限值分别取误警率为0.1和0.01时的值，即为：
Figure BDA0002347493590000155
其中，T1,T2为故障判断的门限值。
分别向GPS与光流的东向速度通道注入故障：
给序号为201到400的GPS数据注入故障，均值为0.5，方差为0.2。
给序号为201到600的光流数据注入故障，均值为0.2，方差为0.5。
下面分情况给出仿真结果图：
向GPS注入故障
注入故障前，导航解算东向速度与基准比较的误差曲线图如图5所示。
注入故障后，GPS子滤波器的故障检测函数λk曲线图如图6所示。
注入故障后，使用原始的联邦卡尔曼融合方法，导航解算东向速度与基准比较的误差曲线图如图7所示。
注入故障后，使用改进的自适应融合方法，导航解算东向速度与基准比较的误差曲线图如图8所示。
向光流传感器注入故障
注入故障前，导航解算东向速度与基准比较的误差曲线图如图9所示。
注入故障后，光流子滤波器的故障检测函数λk曲线图如图10所示。
注入故障后，使用原始的联邦卡尔曼融合方法，导航解算东向速度与基准比较的误差曲线图如图11所示。
注入故障后，使用改进的自适应融合方法，导航解算东向速度与基准比较的误差曲线图如图12所示。
本发明提供的一种复合翼无人机容错组合导航方法，在无人机实际飞行的数据基础上，为系统加入光流传感器作为GPS的余度，采用联邦卡尔曼滤波结构建立组合导航系统，通过残差卡方检验与改进的融合方法，实现对传感器故障的检测与处理，自适应的进行数据融合，加强系统的可靠性与精确性，提高了无人机组合导航系统的容错性能，扩展了无人机的可飞行场景，实用性好，值得推广。
以上公开的仅为本发明的较佳的具体实施例，但是，本发明实施例并非局限于此，任何本领域技术人员能思之的变化都应落入本发明的保护范围。

Claims (8)

1.一种复合翼无人机容错组合导航方法，其特征在于，包括以下步骤：
S1、处理无人机真实飞行数据，获得导航解算所需参数；
S2、建立联邦卡尔曼滤波器；
S3、为联邦卡尔曼滤波器注入故障信号；
S4、进行故障检测，获取故障检测结果；
S5、利用故障检测结果进行数据融合重构后，获得容错后的导航解算信息。
2.根据权利要求1所述的一种复合翼无人机容错组合导航方法，其特征在于，所述步骤S1中处理无人机真实飞行数据，获得导航解算所需参数的方法，包括如下步骤：
S11、从原始飞行数据中选择三轴陀螺仪数据、三轴加速度计数据、GPS速度及位置数据、光流数据进行保存；
S12、从原始飞行数据中选择无人机正常飞行时的位置、速度、姿态角作为比较基准值。
3.根据权利要求1所述的一种复合翼无人机容错组合导航方法，其特征在于，所述步骤S2中建立联邦卡尔曼滤波器的方法，包括如下步骤：
S21、确定滤波器结构的类型；
S22、确定子滤波器所用传感器的类型，进行子滤波器的结构设计；
S23、以子滤波器的结构为基础，分别进行子滤波器的算法设计，建立状态方程和量测方程，确定量测量。
4.根据权利要求3所述的一种复合翼无人机容错组合导航方法，其特征在于，所述步骤S21中确定滤波器结构的类型为联邦卡尔曼滤波无反馈结构。
5.根据权利要求3所述的一种复合翼无人机容错组合导航方法，其特征在于，所述步骤S22中确定子滤波器的类型为GPS和光流传感器的组合。
6.根据权利要求3所述的一种复合翼无人机容错组合导航方法，其特征在于，所述步骤S23中确定子滤波器的算法为扩展卡尔曼滤波算法，选择量测量为GPS位置、GPS速度和光流速度。
7.根据权利要求1或5所述的一种复合翼无人机容错组合导航方法，其特征在于，所述步骤S4中采用改进的残差卡方检验法进行故障检测，包括如下步骤：
S41、利用式(1)计算故障检测函数值λk
Figure FDA0002347493580000021
其中，
Figure FDA0002347493580000022
rk为残差，Zk为量测量，Hk为量测矩阵，
Figure FDA0002347493580000023
为状态一步预测，
Ak为残差的方差，Pk/k-1为一步预测均方误差，
Figure FDA0002347493580000024
为量测矩阵的转置，Rk为量测噪声阵，λk服从自由度为m的卡方分布，即λk～χ2(m)，m为量测Zk的维数，
Figure FDA0002347493580000025
为残差的转置，
Figure FDA0002347493580000026
为残差的方差的逆，rk为残差；
S42、将计算出的故障检测函数λk与故障判断的上下门限值T1,T2进行对比，
当λk≤T1，判定无故障；
当T1<λk≤T2，判定故障较为轻微；
当λk>T2，判定完全故障。
8.根据权利要求7所述的一种复合翼无人机容错组合导航方法，其特征在于，S42中当故障判断结果是故障较为轻微时，出故障的子滤波器的输出会自适应的进入数据融合，进行数据融合重构的方法，包括如下步骤：
S421、利用式(2)计算数据融合的自适应因子ρ
Figure FDA0002347493580000027
其中，ρ为自适应因子，T1,T2为故障判断的门限值，λk为故障检测函数值；
S422、利用式(3)求解全局状态估计
Figure FDA0002347493580000031
Figure FDA0002347493580000032
其中，
αgps＝ρgps-ρgpsof，
Figure FDA0002347493580000033
αof＝ρof-ρgpsof，
Figure FDA0002347493580000034
αgpsof＝ρgpsof，
Figure FDA0002347493580000035
αgps是GPS子滤波器状态量生效程度，ρgps是GPS正常时的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子，
Figure FDA0002347493580000036
为GPS子滤波器状态估计，
Figure FDA0002347493580000037
为子滤波器1的状态估计，αof是光流子滤波器状态量生效程度，ρof是光流正常时的自适应因子，ρgpsof是GPS与光流均正常时的自适应因子，
Figure FDA0002347493580000038
为光流子滤波器状态估计，
Figure FDA0002347493580000039
为子滤波器2的状态估计，αgpsof是GPS与光流子滤波器的融合状态量生效程度，ρgpsof是GPS与光流均正常时的自适应因子，
Figure FDA00023474935800000310
为GPS与光流子滤波器融合状态估计，
Figure FDA00023474935800000311
为GPS子滤波器状态估计，P1为子滤波器1的估计均方误差，P2为子滤波器2的估计均方误差。
CN201911401131.XA 2019-12-30 2019-12-30 一种复合翼无人机容错组合导航方法 Active CN111044051B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911401131.XA CN111044051B (zh) 2019-12-30 2019-12-30 一种复合翼无人机容错组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911401131.XA CN111044051B (zh) 2019-12-30 2019-12-30 一种复合翼无人机容错组合导航方法

Publications (2)

Publication Number Publication Date
CN111044051A true CN111044051A (zh) 2020-04-21
CN111044051B CN111044051B (zh) 2023-11-03

Family

ID=70242065

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911401131.XA Active CN111044051B (zh) 2019-12-30 2019-12-30 一种复合翼无人机容错组合导航方法

Country Status (1)

Country Link
CN (1) CN111044051B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112346471A (zh) * 2020-11-18 2021-02-09 苏州臻迪智能科技有限公司 一种无人机定高方法、装置、无人机及存储介质
CN113432604A (zh) * 2021-06-29 2021-09-24 广东工业大学 一种能够灵敏检测故障的imu/gps组合导航方法
CN113670339A (zh) * 2021-09-10 2021-11-19 中国航空工业集团公司西安飞行自动控制研究所 一种基于改进残差检验法的组合导航系统故障检测方法
CN113821059A (zh) * 2021-11-24 2021-12-21 中航金城无人系统有限公司 一种多旋翼无人机传感器故障安全飞行控制系统及方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法
CN108168509A (zh) * 2017-12-06 2018-06-15 南京航空航天大学 一种升力模型辅助的四旋翼飞行器高度容错估计方法
CN109373999A (zh) * 2018-10-23 2019-02-22 哈尔滨工程大学 基于故障容错卡尔曼滤波的组合导航方法
CN110095800A (zh) * 2019-04-19 2019-08-06 南京理工大学 一种多源融合的自适应容错联邦滤波组合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法
CN108168509A (zh) * 2017-12-06 2018-06-15 南京航空航天大学 一种升力模型辅助的四旋翼飞行器高度容错估计方法
CN109373999A (zh) * 2018-10-23 2019-02-22 哈尔滨工程大学 基于故障容错卡尔曼滤波的组合导航方法
CN110095800A (zh) * 2019-04-19 2019-08-06 南京理工大学 一种多源融合的自适应容错联邦滤波组合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张亮: "微小型旋翼飞行器室内外无缝融合导航技术", pages 37 - 47 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112346471A (zh) * 2020-11-18 2021-02-09 苏州臻迪智能科技有限公司 一种无人机定高方法、装置、无人机及存储介质
CN113432604A (zh) * 2021-06-29 2021-09-24 广东工业大学 一种能够灵敏检测故障的imu/gps组合导航方法
CN113432604B (zh) * 2021-06-29 2023-05-19 广东工业大学 一种能够灵敏检测故障的imu/gps组合导航方法
CN113670339A (zh) * 2021-09-10 2021-11-19 中国航空工业集团公司西安飞行自动控制研究所 一种基于改进残差检验法的组合导航系统故障检测方法
CN113670339B (zh) * 2021-09-10 2024-05-24 中国航空工业集团公司西安飞行自动控制研究所 一种基于改进残差检验法的组合导航系统故障检测方法
CN113821059A (zh) * 2021-11-24 2021-12-21 中航金城无人系统有限公司 一种多旋翼无人机传感器故障安全飞行控制系统及方法

Also Published As

Publication number Publication date
CN111044051B (zh) 2023-11-03

Similar Documents

Publication Publication Date Title
CN111044051B (zh) 一种复合翼无人机容错组合导航方法
EP3109589B1 (en) A unit and method for improving positioning accuracy
WO2021057894A1 (zh) 一种基于车辆零速检测的惯性导航误差修正方法
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
Jiang et al. A fault-tolerant tightly coupled GNSS/INS/OVS integration vehicle navigation system based on an FDP algorithm
Atia et al. Map-aided adaptive GNSS/IMU sensor fusion scheme for robust urban navigation
US20070050138A1 (en) Enhanced inertial system performance
CN109471144A (zh) 基于伪距/伪距率的多传感器紧组合列车组合定位方法
CN113029139B (zh) 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN110196049A (zh) 一种动态环境下四陀螺冗余式捷联惯导系统硬故障检测与隔离方法
CN112902967A (zh) 一种基于残差卡方-改进序贯概率比的抗欺骗导航方法
CN113432604B (zh) 一种能够灵敏检测故障的imu/gps组合导航方法
CN106979781A (zh) 基于分布式惯性网络的高精度传递对准方法
Wang et al. Attitude determination method by fusing single antenna GPS and low cost MEMS sensors using intelligent Kalman filter algorithm
Ahmed et al. Terrain-based vehicle localization using low cost MEMS-IMU sensors
Malvezzi et al. Train position and speed estimation by integration of odometers and IMUs
Palella et al. Sensor fusion for land vehicle slope estimation
Zhang et al. An Integration Method of Inertial Navigation System and Three‐Beam Lidar for the Precision Landing
CN113819911B (zh) Gnss失锁下基于自适应容错滤波的导航方法
Ghanbarpourasl et al. Integration of sensor fusion for enhancing GPS navigation
Atia et al. A novel systems integration approach for multi-sensor integrated navigation systems
Amin et al. A novel vehicle stationary detection utilizing map matching and IMU sensors
WO2022005419A1 (en) A navigation system
Fang et al. Integrating SINS sensors with odometer measurements for land vehicle navigation system
CN108931247B (zh) 一种导航方法和装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20231009

Address after: 215000, 2nd Floor, 11 #, No. 36 Dalian East Road, Taicang City, Suzhou City, Jiangsu Province

Applicant after: Interstellar Intelligent Aviation (Taicang) Aviation Technology Co.,Ltd.

Address before: No.1289, Huanghe West Road, Xinbei District, Changzhou City, Jiangsu Province 213022

Applicant before: Star (Jiangsu) Aviation Technology Co.,Ltd.

GR01 Patent grant
GR01 Patent grant