CN112284388B - 一种无人机多源信息融合导航方法 - Google Patents
一种无人机多源信息融合导航方法 Download PDFInfo
- Publication number
- CN112284388B CN112284388B CN202011025124.7A CN202011025124A CN112284388B CN 112284388 B CN112284388 B CN 112284388B CN 202011025124 A CN202011025124 A CN 202011025124A CN 112284388 B CN112284388 B CN 112284388B
- Authority
- CN
- China
- Prior art keywords
- state
- model
- aerial vehicle
- unmanned aerial
- measurement
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 67
- 230000004927 fusion Effects 0.000 title claims abstract description 26
- 239000002245 particle Substances 0.000 claims abstract description 87
- 238000005259 measurement Methods 0.000 claims abstract description 84
- 239000011159 matrix material Substances 0.000 claims abstract description 47
- 238000001914 filtration Methods 0.000 claims abstract description 37
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 18
- 230000008569 process Effects 0.000 claims abstract description 9
- 239000013598 vector Substances 0.000 claims description 74
- 238000005070 sampling Methods 0.000 claims description 21
- 238000012952 Resampling Methods 0.000 claims description 15
- 230000003287 optical effect Effects 0.000 claims description 13
- 230000002776 aggregation Effects 0.000 claims description 10
- 238000004220 aggregation Methods 0.000 claims description 10
- 238000004458 analytical method Methods 0.000 claims description 4
- 230000009467 reduction Effects 0.000 claims description 4
- 230000000737 periodic effect Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000004907 flux Effects 0.000 claims description 2
- 238000007499 fusion processing Methods 0.000 abstract 1
- 230000000052 comparative effect Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 6
- 238000005315 distribution function Methods 0.000 description 6
- 238000004088 simulation Methods 0.000 description 6
- XEEYBQQBJWHFJM-UHFFFAOYSA-N Iron Chemical compound [Fe] XEEYBQQBJWHFJM-UHFFFAOYSA-N 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000002411 adverse Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 229910052742 iron Inorganic materials 0.000 description 2
- 238000010606 normalization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 101100264172 Oryza sativa subsp. japonica XIAO gene Proteins 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005562 fading Methods 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 235000003642 hunger Nutrition 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000036632 reaction speed Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000037351 starvation Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种无人机多源信息融合导航方法，通过建立组合导航模型、初始化模型，采集量测量并进行多源信息融合，利用组合导航模型输出无人机组合导航***的位置、速度和姿态角信息，在信息融合过程中，采用Consider容积卡尔曼滤波与粒子滤波算法结合的Consider容积粒子滤波算法，将量测偏差的方差引入到滤波过程用以修正增益矩阵，进而提升状态估计的精度。本发明公开的无人机多源信息融合导航方法，能够消除无人机组合导航***中多源信息融合量测偏差，降低了无人机组合***中量测偏差带来的干扰影响，有效地提高了无人机导航***精度。
Description
技术领域
本发明涉及一种无人机多源信息融合导航方法，尤其涉及一种基于Consider容积粒子滤波的无人机多源信息融合导航方法，属于无人机导航领域。
背景技术
无人机具备能垂直起降、可定点悬停和机动能力强等优点，大量运用在民用和军事等领域，但是，伴随着无人机使用范围的增加，其面临的问题也愈来愈多，尤其无人机导航***的精度和可靠性，制约了无人机的进一步发展。
无人机导航***能够使无人机获取飞行的速度、位置、姿态等导航信息，由于惯性导航***(Inertial Navigation System，INS)具备较强的自主性、良好的隐蔽性、非常全面的输出信息和较宽的频带等优势，因此，目前主流的无人机组合导航***大都选用INS作为主导航***，并引入北斗、气压高度计、磁传感器、光流传感器辅助导航方式来建立组合导航***。
组合导航***是通过融合INS的输出信息和辅助导航方式的量测信息来校正INS的累积误差。但是，无人机组合导航***中的多源信息融合仍然面临着传感器量测偏差问题，如：陀螺仪和加速度计的偏置误差、漂移等；北斗卫星导航***的大气层电离层干扰、卫星星历误差、接收机钟差、多路径误差等；气压高度计的换算误差、温度误差等；磁传感器外部干扰磁场引起的软铁误差和硬铁误差等；光流传感器的漂移误差等。
传统的多源信息融合并未考虑上述传感器量测偏差问题，也未有较好的方法来降低量测偏差对***状态估计的不利影响。
因此，亟待设计一种能有效减轻传感器量测偏差影响，以及提升导航精度和可靠性的无人机多源信息融合导航方法。
发明内容
为了克服上述问题，本发明人进行了锐意研究，设计出一种无人机多源信息融合导航方法，包括以下步骤：
S1、建立组合导航模型；
S2、初始化模型；
S3、采集量测量并进行多源信息融合，利用组合导航模型输出无人机组合导航***的位置、速度和姿态角信息。
具体地，在步骤S1中，所述组合导航模型用于描述无人机的状态，所述组合导航模型的输入为传感器的量测值，输出为无人机的位置、速度和姿态角信息，将此信息传递至无人机CPU处理器对无人机进行控制。
优选地，所述组合导航模型的建立包括如下子步骤：
S11、建立组合导航状态模型；
S12、建立组合导航量测模型；
在步骤S11中，所述状态模型用于描述无人机组合导航的状态，其可以通过下式表达：
其中，F(x)表示连续状态转移函数，w为具有零均值的高斯白噪声，x为状态向量，为状态向量的导数；
所述状态向量可以表示为x＝[r v Ω]T，其中，r＝[re rn ru]T，re、rn、ru分别表示无人机的东北天方向的位置向量；v＝[ve vn vu]，ve、vn、vu分别表示无人机东北天方向的速度向量；Ω＝[γθψ]T，γ、θ、ψ分别表示无人机的横滚角、俯仰角和航向角。
在步骤S12中，所述组合导航量测模型用于描述无人机组合导航中多源传感器的量测量，所述组合导航量测模型可以通过下式表达：
z＝H(x)+p+vnoise
H(x)＝[mb rBDS vflow hbaro]T (式四)
其中，mb为磁传感器的磁北矢量；rBDS＝[rBe rBn rBu]为北斗测量的载***置；vflow＝[vflow_e vflow_n]为光流测量的载体速度矢量，hbaro为气压高度计测量的载体高度；
p＝[pme,pmn,pmu,pre,prn,pru,pve,pvn,ph]T为量测偏差，即不确定参数向量，pme、pmn和pmu分别是东北天方向的磁力计量测偏差，pre、prn和pru分别是东北天方向的北斗量测位置偏差，pve、pvn是东北向的光流量测速度偏差，ph为气压高度计的量测高度偏差；vnoise为量测噪声。
优选地，步骤S1中还可以包括子步骤：
S13、对组合导航状态模型和组合导航量测模型进行简化，获得状态简化模型和量测简化模型，
状态简化模型可表示为：
xk＝f(xk-1,p)+wk-1 (式十)
量测简化模型可表示为：
zk＝h(xk,p)+vnoise,k (式十一)
其中，xk是tk时刻的状态向量，xk-1是tk-1时刻的状态向量，zk是量测向量；f表示非线性状态方程函数；h表示非线性量测方程函数；p为不确定参数向量，wk-1为tk-1时刻的过程噪声，vnoise,k为tk时刻的量测噪声。
在步骤S2中，所述初始化模型为初始化组合导航状态模型；
初始化状态简化模型包括初始化状态估计值、初始化无人机所在位置、初始化无人机速度、初始化无人机姿态角、初始化状态误差方差矩阵、初始化位置方差阵、初始化速度方差阵、初始化陀螺仪常值漂移方差阵，以及初始化状态估计值粒子集合点、初始化状态估计误差方差粒子集合点、初始化化状态估计误差与参数的不确定性的互协方差粒子集合点、初始化参数的不确定性的方差粒子集合点。
在步骤S3中，对状态向量和传感器量测量进行粒子滤波，通过周期采样获得量测量，在每次采样时，进行如下子过程：
S31、重要性采样；
S32、重采样；
S33、利用模型输出无人机组合导航***的位置、速度和姿态角信息。
根据本发明，在步骤S31的重要性采样中，包括以下子步骤：
S311、对每一个粒子，用卡尔曼滤波法更新粒子；
S312、获取每个粒子的权重，并归一化权重。
在步骤S311中，在卡尔曼滤波算法更新粒子点时结合基于容积卡尔曼滤波的Consider分析方法，在考虑参数的不确定性时，不直接估计它们，而是将参数方差融入状态估计方差中，用以修正参数的估计，获得状态估计值粒子和粒子权重/>
根据本发明，在步骤S32中，所述重采样为根据归一化之后的权重选择或淘汰不同的状态估计值粒子和粒子权重/>
在步骤S33中，通过下式获得tk时刻的状态估计量：
状态估计量中包括无人机的位置、速度和姿态角信息，将其作为状态向量输出。
本发明所述的无人机多源信息融合导航方法，具有的有益效果包括：
(1)本发明提供的无人机多源信息融合导航方法，能够消除无人机组合导航***中多源信息融合量测偏差；
(2)本发明提供的无人机多源信息融合导航方法，通过将量测偏差的方差引入到Consider容积Kalman滤波算法中形成Consider容积粒子滤波算法来估计的位置误差小、估计精度更高，可靠性强，估计的速度和姿态角误差小、精度更高、滤波效果好，其鲁棒性明显优于容积粒子滤波算法；
(3)根据本发明提供的无人机多源信息融合导航方法，降低了无人机组合***中量测偏差带来的干扰影响，有效地提高了无人机导航***精度和导航***状态估计精度。
附图说明
图1示出一种优选实施方式的无人机多源信息融合导航方法流程示意图；
图2示出地理系磁向量示意图；
图3示出实施例1与对比例1位置误差比较图；
图4示出实施例1与对比例1速度误差比较图；
图5示出实施例1与对比例1姿态角误差比较图。
具体实施方式
下面通过附图对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。
本发明提供了一种无人机多源信息融合导航方法，如图1所示，包括以下步骤：
S1、建立组合导航模型；
S2、初始化模型；
S3、采集量测量并进行多源信息融合，利用组合导航模型输出无人机组合导航***的位置、速度和姿态角信息。
在步骤S1中，所述组合导航模型用于描述无人机的状态，进一步地，所述组合导航模型的输入为传感器的量测值，输出为无人机的位置、速度和姿态角信息，将此信息传递至无人机CPU处理器对无人机进行控制。
具体地，所述组合导航模型的建立包括如下子步骤：
S11、建立组合导航状态模型；
S12、建立组合导航量测模型；
优选地，还包括子步骤：
S13、对组合导航状态模型和组合导航量测模型进行简化，获得状态简化模型和量测简化模型。
在步骤S11中，所述状态模型用于描述无人机组合导航的状态，其可以通过下式表达：
其中，F(x)表示连续状态转移函数，w为具有零均值的高斯白噪声，x为状态向量，为状态向量的导数。
进一步地，所述状态向量可以表示为x＝[r v Ω]T，其中，r＝[re rn ru]T，re、rn、ru分别表示无人机的东北天方向的位置向量；v＝[ve vn vu]，ve、vn、vu分别表示无人机东北天方向的速度向量；Ω＝[γ θ ψ]T，γ、θ、ψ分别表示无人机的横滚角、俯仰角和航向角。
更进一步地，在本发明中，r、v、Ω和其导数满足动力学方程：
其中，是由机体坐标系转换到导航坐标系的矩阵；/>为加速度计三轴向在机体坐标系中输出的线加速度；ba为加速度计的常值漂移；g为地球重力加速度在导航坐标系中投影的加速度；/>为陀螺仪三轴向在机体坐标系中输出的瞬时旋转角速度，bw为陀螺仪的常值漂移，K为姿态运动学矩阵。
进一步地，所述姿态运动学矩阵K可以通过下式表示：
在步骤S12中，所述组合导航量测模型用于描述无人机组合导航中多源传感器的量测量。
在本发明中，所述多源传感器包括北斗、气压高度计、磁力计和光流传感器，多源传感器测量时间间隔恒定。
进一步地，所述组合导航量测模型可以通过下式表达：
z＝H(x)+p+vnoise
H(x)＝[mb rBDS vflow hbaro]T (式四)
其中，mb为磁传感器的磁北矢量；rBDS＝[rBe rBn rBu]为北斗测量的载***置；vflow＝[vflow_e vflow_n]为光流传感器测量的载体速度矢量，hbaro为气压高度计测量的载体高度；p＝[pme,pmn,pmu,pre,prn,pru,pve,pvn,ph]T为量测偏差，在实际情况下其为不确定参数向量，pme、pmn和pmu分别是东北天方向的磁力计量测偏差，pre、prn和pru分别是东北天方向的北斗量测位置偏差，pve、pvn是东北向的光流传感器量测速度偏差，ph为气压高度计的量测高度偏差；vnoise为量测噪声。
进一步地，所述量测噪声可以表示为：
vnoise＝[vme,vmn,vmu,vre,vrn,vru,vve,vvn,vh]T (式五)
其中，vme、vmn和vmu分别为导航坐标系中东北天方向磁力计量测值的随机误差；vre、vrn和vru分别为导航坐标系中东北天方向北斗位置量测值的随机误差；vve和vvn为东北方向光流测量速度的随机误差；vh为气压高度计测量高度的随机误差。
进一步地，在式四中，磁传感器的磁北矢量mb与无人机所在位置有关，可以表示为：
其中，mn是地理坐标系下的地磁场矢量，θdec为地磁偏角，θinc为磁倾角，如图2所示。
进一步地，在本发明中，所述光流测量的载体速度矢量可表示为：
其中，fflow为焦距；为角运动速度；vflow_x和vflow_y是光流传感器中像素点二维的光流速度，在本发明中，光流传感器至地面的高度Hflow＝ru。
在步骤S13中，对组合导航状态模型和组合导航量测模型进行简化，提高运算效率。优选地，通过对组合导航状态模型和组合导航量测模型进行离散化，有效降低模型的时间复杂度，从而达到简化模型，提高效率的目的。
具体的，对组合导航状态模型进行离散化后，可表达为下式：
xk＝xk-1+F(xk-1)·dt (式九)
其中，dt为组合导航量测模型中多源传感器的量测时间间隔，优选地，在本发明中，所述时间间隔为1s，tk表示第k时刻，xk是tk时刻的状态向量，xk-1是tk-1时刻的状态向量，k为正整数。
在一个优选的实施方式中，对离散化后的组合导航状态模型和组合导航量测模型进一步地简化，获得状态简化模型和量测简化模型，进一步地，
状态简化模型可表示为：
xk＝f(xk-1,p)+wk-1 (式十)
量测简化模型可表示为：
zk＝h(xk,p)+vnoise,k (式十一)
其中，zk是tk时刻的量测向量；f表示非线性状态方程函数；h表示非线性量测方程函数；p为不确定参数向量，wk-1为tk-1时刻的过程噪声，vk为tk时刻的量测噪声。
进一步地，所述不确定参数向量p具有方差Ppp，方差Ppp满足：
为不确定参数向量的均值。
更进一步地，发明人发现，wk-1和vnoise,k满足以下条件：
其中，δkJ为Kroneckerδ函数，当k＝J时，δkJ＝1；当k≠J时，δkJ＝0；Qk为噪声方差阵，Rk为量测噪声方差阵；J表示和k不相关的时刻，wJ表示tJ时刻的过程噪声，vnoise,J表示tJ时刻的量测噪声；为状态估计误差，xk为tk时刻状态向量，/>为tk时刻状态估计向量。
进一步地，根据本发明，在初始时刻，即k＝0时，
Pxp,k为k时刻的不确定参数，其初始时刻值为
在本发明中，所述噪声方差阵Qk和量测噪声方差阵Rk分别为：
在本发明中，通过粒子滤波构建建议分布函数，将采集的量测量结合到建议分布函数中，从而获得接近状态真实值的后验概率分布，进而通过组合导航模型输出无人机的位置、速度和姿态角信息。
粒子滤波是指：通过寻找一组在状态空间中传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差估计的过程，这些样本即称为“粒子”。粒子滤波是一种非线性算法，其基于蒙特卡罗仿真的近似贝叶斯滤波算法，通过对后验概率密度进行数值近似求解，将状态矢量表示为一组带有权值的随机样本，并基于这些样本和权值计算出状态估值，相对于其它获得后验概率的方式，该方法直接通过带权重的随机样本统计后验概率的均值，没有高斯噪声的限制。
在本发明中，对状态向量和传感器量测量进行粒子滤波。
在粒子滤波前，需要进行初始化，在步骤S2中，所述初始化模型为初始化组合导航状态模型，优选地，所述初始化模型为初始化状态简化模型。
进一步地，初始化状态简化模型包括初始化状态估计值、无人机所在位置、无人机速度、无人机姿态角、状态误差方差矩阵、位置方差阵、速度方差阵、陀螺仪常值漂移方差阵，以及初始化状态估计值粒子集合点、状态估计误差方差粒子集合点、状态估计误差与参数的不确定性的互协方差粒子集合点、参数的不确定性的方差粒子集合点。
具体地，初始状态估计向量表示为
初始状态估计向量初始无人机所在位置r0＝[0 0 5m]；初始的无人机速度v0＝[0 0 0]；初始的无人机姿态角Ω＝[00π/2]；初始状态估计误差方差矩阵为/>初始位置方差阵为/>初始速度方差阵为/>初始陀螺仪常值漂移方差阵为
初始的状态估计值粒子集合点：
式中，Np为状态估计的粒子点数，i表示不同的粒子点；
初始化状态估计误差方差粒子集合点：
初始化状态估计误差与参数的不确定性的互协方差粒子集合点：
初始化参数的不确定性的方差粒子集合点：
其中，Ppp,0＝[10-6 10-6 10-6 101 101 101 10-1 10-1 101]
在本发明中，通过将粒子滤波方法与Consider容积Kalman滤波算法相结合：Consider容积粒子滤波算法将偏差的统计特性(方差)引入到滤波过程不直接估计该偏差，同时，采用一种基于序列的粒子滤波器进行重要抽样。
具体地，在步骤S3中，对状态向量和传感器量测量进行粒子滤波，所述采集量测量，通过周期采样获得，优选地，每次采样的时间间隔为1s。
进一步地，在每次采样时，进行如下子过程：
S31、重要性采样；
S32、重采样；
S33、利用模型输出无人机组合导航***的位置、速度和姿态角信息。
滤波问题中的困难点在于后验概率的计算，在粒子滤波中，需要按上一时刻状态的后验概率分布来对粒子进行采样，然后更新粒子状态，但是上一时刻状态的后验概率分布是不能精确已知的，在本发明中，通过重要性采样来解决，从而使得粒子滤波的建议分布函数可以尽可能的逼近状态后验概率分布的真实值，并按照此概率分布来进行采样。
具体地，在步骤S31的重要性采样中，包括以下子步骤：
S311、对状态估计，用卡尔曼滤波法更新粒子；
S312、获取状态估计的权重，并归一化权重。
传统的卡尔曼滤波法通常直接忽略参数的不确定性，在大多数情况下，传感器的量测量中不确定参数对***的影响较小，将其忽略是合理的，对组合导航模型的输出进度无较大影响，但是在一些特殊情况下，某些量测量的不确定参数对组合导航模型的影响较大，会导致模型的输出出现无法预料的结果。
此外，还有在卡尔曼滤波法中采用将不确定参数扩维到状态向量进行估计的方法，虽然其结果较为精确，但是会面临着较大的计算代价和较长的运算时间，特别是状态空间维数较高时，运算时间增长明显，对导航反应速度有较大影响。
在本发明中，在卡尔曼滤波算法更新粒子点时结合基于容积卡尔曼滤波的Consider分析方法，在考虑未知参数的不确定性时，不直接估计它们，而是将其统计特性(方差)融入状态估计方差中，用以修正参数的估计，获得状态估计值粒子和粒子权重不需要消耗较多的计算量和运行时间并能够获得较为准确的结果。
所述Consider分析方法是Schmidt提出的，可以考虑***状态模型和量测模型中的不确定参数或偏差，有效地降低不确定参数或偏差对***的不利影响，将其统计信息(方差)引入到***的状态估计方差中，使用不确定参数或偏差的协方差来修正增益矩阵，但不直接估计不确定参数或偏差，例如SCHMIDT S F.Application of state space methodsto navigation problems[M]//Advanced in control systems.NewYork:AcademicPress,1966:293-340和LOU T,WANG Z,XIAO M,et al.Multiple Adaptive FadingSchmidt-Kalman Filter for Unknown Bias[J].Mathematical Problems inEngineering,2014,2014:8.中介绍的Consider方法。
具体地，在步骤S311中进行如下步骤：
S3111、采用Consider方法对状态向量和误差方差进行扩维，状态向量扩维后可表示为：
为tk-1时刻i粒子的状态估计向量，误差方差扩维后可表示为：
其中上标i表示不同的粒子。
S3112、获得Sigma点集合:
其中，上标j表示第j个Sigma点，且j＝1…2(nx+np)；(nx+np)为Xk-1的维度，nx为的维度，np为/>的维度。
S3113、时间更新，获得从k-1到k时刻的一步方差预测
通过下式获得Sigma点集合的一步预测：
其中，函数f与式十中相同；
通过下式获得Sigma点集合的均值：
其中，是2(nx+np)个状态采样点/>的均值；/>是2(nx+np)个不确定参数采样点/>的均值。
进一步地，
其中，表示第j个状态采样点，/>表示第j个不确定参数采样点；
进一步地，通过下式获得状态xk的方差矩阵xk和pk之间的互协方差矩阵
S3114、量测更新，获得后验协方差矩阵
通过扩维后的状态获取Sigma点集合的量测值/>
其中，非线性量测方程函数h与式十一中相同。
则量测均值为：
通过下式可获得状态和量测之间的互协方差矩阵量测的方差矩阵/>
通过下式获得增益矩阵Kx,k：
通过下式获得后验状态估计和后验不确定参数/>
进而可以获得后验协方差矩阵：
通过式二十一至式三十九，完成了Consider容积卡尔曼滤波的一步修正过程，并且将粒子更新与Consider容积卡尔曼滤波相结合。
在步骤S312中，对Consider容积卡尔曼滤波状态后验状态估计进行粒子更新并获得更新后粒子点的权重，在本发明中，粒子满足：
从而，可以获得每个粒子的权重：
其中，q、p表示不同的概率分布函数，N表示满足正态分布，在本发明中，对概率分布函数不做特别限制，本领域技术人员可以根据实际需要选择滤波中常用的任意一种概率分布函数；
进一步地，通过下式对权重归一化：
在步骤S32中，所述重采样为根据归一化之后的权重大小来选择或淘汰/>重采样算法是降低粒子匮乏现象的一种方法，通过对粒子和相应权表示的概率密度函数重新采样，增加权值较大的粒子数。
在本发明中，对重采样算法不做特别限制，可以是任何已知的方法，优选采用残差重采样法(Residual Resampling)选择则或淘汰所述残差重采样法(ResidualResampling)为Liu等人在1998年提出的算法。
在步骤S33中，根据步骤S32选择的粒子权重和i粒子的状态估计向量/>通过下式获得tk时刻的状态估计向量：
进一步地，在获得tk时刻的状态估计后，通过式四十四重置权重：
重复步骤S1～S3，依次获取不同时刻的状态估计向量将状态估计向量/>将此状态估计向量作为无人机组合导航***的状态向量x，输出其中的无人机的位置、速度和姿态角信息，将此信息传递至舵机对无人机进行控制。
实施例
实施例1
设计仿真模拟实验，仿真实验中对导航***的量测值设置随机偏差。其中，通过步骤S1建立组合导航模型，组合导航模型包括组合导航状态模型：
组合导航量测模型：
z＝H(x)+p+vnoise
H(x)＝[mb rBDS vflow hbaro]T (式四)
通过式子九对组合导航状态模型和组合导航量测模型进行离散化：
xk＝xk-1+F(xk-1)·dt (式九)
对离散化后的组合导航状态模型和组合导航量测模型进一步地简化，获得状态简化模型和量测简化模型，
状态简化模型可表示为：
xk＝f(xk-1,p)+wk-1 (式十)
量测简化模型可表示为：
zk＝h(xk,p)+vnoise,k (式十一)
其中，zk是tk时刻的量测向量；f表示非线性状态方程函数；h表示非线性量测方程函数；p为不确定参数向量，p＝[pme,pmn,pmu,pre,prn,pru,pve,pvn,ph]T为量测偏差，也即是不确定参数向量，pme、pmn和pmu分别是东北天方向的磁力计量测偏差，pre、prn和pru分别是东北天方向的北斗量测位置偏差，pve、pvn是东北向的光流传感器量测速度偏差，ph为气压高度计的量测高度偏差；vnoise为量测噪声，对不确定参数向量p设置随机偏差；wk-1为tk-1时刻的过程噪声，vk为tk时刻的量测噪声，不确定参数向量p具有方差Ppp，方差Ppp满足：
为不确定参数向量的均值。
wk-1和vk满足以下条件：
其中，δkJ为Kroneckerδ函数，当k＝J时，δkJ＝1；当k≠J时，δkJ＝0；Qk为噪声方差阵，Rk为量测噪声方差阵；J表示和k不相关的时刻，wJ表示tJ时刻的过程噪声，vnoise,J表示tJ时刻的量测噪声；为状态估计误差，xk为tk时刻状态向量，/>为tk时刻状态估计向量。
噪声方差阵Qk和量测噪声方差阵Rk分别为：
/>
在步骤S2中，初始化模型，初始状态估计向量为：
初始状态估计向量初始无人机所在位置r0＝[0 0 5m]；初始的无人机速度v0＝[0 0 0]；初始的无人机姿态角Ω＝[0 0 π/2]；初始状态估计误差方差矩阵为/>初始位置方差阵为/>初始速度方差阵为/>初始陀螺仪常值漂移方差阵为
初始化量测简化模型为：初始化状态估计值粒子集合点、状态估计误差方差粒子集合点、化状态估计误差与参数的不确定性的互协方差粒子集合点、参数的不确定性的方差粒子集合点，具体地：
初始的状态估计值粒子集合点：
式中，Np为状态估计的粒子点数，i表示不同的粒子点；
初始化状态估计误差方差粒子集合点：
初始化状态估计误差与参数的不确定性的互协方差粒子集合点：
初始化参数的不确定性的方差粒子集合点：
其中，Ppp,0＝[10-6 10-6 10-6 101 101 101 10-1 10-1 101]
在步骤S3中，进行如下子过程：
S31、重要性采样，其中对每一个粒子，用卡尔曼滤波法更新粒子：
采用Consider方法对状态向量和误差方差进行扩维，状态向量扩维后可表示为：
误差方差扩维后可表示为：
其中上标i表示不同的粒子。
获得Sigma点集合:
其中，上标j表示第j个Sigma点，且j＝1…2(nx+np)；(nx+np)为Xk-1的维度，nx为的维度，np为/>的维度；
时间更新，获得从k-1到k时刻的一步方差预测
通过下式获得Sigma点集合的一步预测：
其中，函数f与式十中相同；
通过下式获得Sigma点集合的均值：
其中，是2(nx+np)个状态采样点/>的均值；/>是2(nx+np)个不确定参数采样点/>的均值。
进一步地，
其中，表示第j个状态采样点，/>表示第j个不确定参数采样点；
进一步地，通过下式获得状态xk的方差矩阵xk和pk之间的互协方差矩阵
量测更新，获得后验协方差矩阵，通过扩维后的状态获取Sigma点集合的量测值/>
则量测均值为：
通过下式可获得状态和量测之间的互协方差矩阵量测的方差矩阵/>
通过下式获得增益矩阵Kx,k：
通过下式获得后验状态估计和后验不确定参数/>
进而可以获得后验协方差矩阵：
对每个粒子更新并获得每个粒子的权重：
粒子满足：
从而，可以获得每个粒子的权重：
其中，q、p表示不同的概率分布函数，N表示满足正态分布；
进一步地，通过下式对权重归一化：
在步骤S32中通过残差重采样法进行重采样，根据归一化之后的权重大小来选择或淘汰/>/>
在步骤S33中，通过下式获得tk时刻的状态估计向量：
利用模型输出无人机组合导航***的位置、速度和姿态角信息；
通过式四十四重置权重：
重复步骤S1～S3，获得不同时刻的状态估计向量输出其中的无人机的位置、速度和姿态角信息。
仿真实验结束后，统计无人机东北天位置误差、速度误差和姿态角误差。
对比例1
按照与实施例1中相同的步骤进行仿真模拟实验，区别在于，在步骤S311中，不引入Consider方法，只采用容积粒子滤波算法。
在仿真模拟实验中，对导航***的量测值设置与实施例1相同的偏差，进行仿真实验，统计无人机东北天位置误差、速度误差和姿态角误差。
实验例1
实施例1与对比例1位置误差比较图如图3所示；
实施例1与对比例1速度误差比较图如图4所示；
实施例1与对比例1姿态角误差比较图如图5所示。
从图2～4可以看出，Consider容积粒子滤波算法能有效地降低量测偏差对导航***产生的不利影响，并且滤波精度优于容积粒子滤波算法。
在本发明的描述中，需要说明的是，术语“上”、“下”、“内”和“外”等指示的方位或位置关系为基于本发明工作状态下的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。
Claims (4)
1.一种无人机多源信息融合导航方法，包括以下步骤：
S1、建立组合导航模型；
S2、初始化模型；
S3、采集传感器量测量并进行多源信息融合，利用组合导航模型输出无人机组合导航***的位置、速度和姿态角信息；
在步骤S1中，所述组合导航模型用于描述无人机的状态，所述组合导航模型的输入为传感器的量测值，输出为无人机的位置、速度和姿态角信息，将此信息传递至CPU处理器对无人机进行控制，所述组合导航模型的建立包括如下子步骤：
S11、建立组合导航状态模型；
S12、建立组合导航量测模型；
在步骤S11中，所述状态模型用于描述无人机组合导航的状态，其通过下式表达：
其中，F(x)表示连续状态转移函数，w为具有零均值的高斯白噪声，x为状态向量，为状态向量的导数；
所述状态向量表示为x＝[r v Ω]T，其中，r＝[re rn ru]T，re、rn、ru分别表示无人机的东北天方向的位置向量；v＝[ve vn vu]，ve、vn、vu分别表示无人机东北天方向的速度向量；Ω＝[γ θ ψ]T，γ、θ、ψ分别表示无人机的横滚角、俯仰角和航向角；
在步骤S12中，所述组合导航量测模型用于描述无人机组合导航中多源传感器的量测量，所述组合导航量测模型通过下式表达：
z＝H(x)+p+vnoise
H(x)＝[mb rBDS vflow hbaro]T (式四)
其中，mb为磁传感器的磁北矢量；rBDS＝[rBe rBn rBu]为北斗测量的载***置；vflow＝[vflow_e vflow_n]为光流测量的载体速度矢量，hbaro为气压高度计测量的载体高度；
p＝[pme,pmn,pmu,pre,prn,pru,pve,pvn,ph]T为量测偏差，即不确定参数向量，pme、pmn和pmu分别是东北天方向的磁力计量测偏差，pre、prn和pru分别是东北天方向的北斗量测位置偏差，pve、pvn是东北向的光流量测速度偏差，ph为气压高度计的量测高度偏差；vnoise为量测噪声；
在步骤S3中，对状态向量和传感器量测量进行粒子滤波，通过周期采样获得量测量，在每次采样时，进行如下子过程：
S31、重要性采样；
S32、重采样；
S33、利用模型输出无人机组合导航***的位置、速度和姿态角信息；
在步骤S31的重要性采样中，包括以下子步骤：
S311、对每一个粒子，用卡尔曼滤波法更新粒子；
S312、获取每个粒子的权重，并归一化权重；
在步骤S311中，在卡尔曼滤波算法更新粒子点时结合基于容积卡尔曼滤波的Consider分析方法，在考虑参数的不确定性时，不直接估计它们，而是将参数方差融入状态估计方差中，用以修正参数的估计，获得状态估计值粒子和粒子权重/>
2.根据权利要求1所述的无人机多源信息融合导航方法，其特征在于,
步骤S1中还包括子步骤：
S13、对组合导航状态模型和组合导航量测模型进行简化，获得状态简化模型和量测简化模型，
状态简化模型表示为：
xk＝f(xk-1,p)+wk-1(式十)
量测简化模型表示为：
zk＝h(xk,p)+vnoise,k(式十一)
其中，xk是tk时刻的状态向量，xk-1是tk-1时刻的状态向量，zk是量测向量；f表示非线性状态方程函数；h表示非线性量测方程函数；p为不确定参数向量，wk-1为tk-1时刻的过程噪声，vnoise,k为tk时刻的量测噪声。
3.根据权利要求2所述的无人机多源信息融合导航方法，其特征在于,
在步骤S2中，所述初始化模型是指初始化组合导航状态模型；
初始化状态简化模型包括初始化状态估计值、初始化无人机所在位置、初始化无人机速度、初始化无人机姿态角、初始化状态误差方差矩阵、初始化位置方差阵、初始化速度方差阵、初始化陀螺仪常值漂移方差阵；初始化状态估计值粒子集合点、初始化状态估计误差方差粒子集合点、初始化状态估计误差与参数的不确定性的互协方差粒子集合点、初始化参数的不确定性的方差粒子集合点。
4.根据权利要求1所述的无人机多源信息融合导航方法，其特征在于,
在步骤S32中，所述重采样为根据归一化之后的权重选择或淘汰不同的状态估计值粒子和粒子权重/>
在步骤S33中，通过下式获得tk时刻的状态估计量：
状态估计量中包括无人机的位置、速度和姿态角信息，将其作为状态向量输出。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011025124.7A CN112284388B (zh) | 2020-09-25 | 2020-09-25 | 一种无人机多源信息融合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011025124.7A CN112284388B (zh) | 2020-09-25 | 2020-09-25 | 一种无人机多源信息融合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112284388A CN112284388A (zh) | 2021-01-29 |
| CN112284388B true CN112284388B (zh) | 2024-01-30 |
Family
ID=74421998
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011025124.7A Active CN112284388B (zh) | 2020-09-25 | 2020-09-25 | 一种无人机多源信息融合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112284388B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113933879B (zh) * | 2021-10-18 | 2024-04-12 | 北京航空航天大学 | 一种无人机抗磁导航方法和*** |
| CN117930664B (zh) * | 2024-03-21 | 2024-06-07 | 哈尔滨华拓导航技术有限公司 | 基于北斗rtk差分定位的无人机降落控制优化*** |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018028711A1 (zh) * | 2016-08-12 | 2018-02-15 | 贵州火星探索科技有限公司 | 一种对无人机的噪声协方差进行估算的方法 |
| CN108759838A (zh) * | 2018-05-23 | 2018-11-06 | 安徽科技学院 | 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法 |
| WO2018214227A1 (zh) * | 2017-05-22 | 2018-11-29 | 深圳市靖洲科技有限公司 | 一种无人车实时姿态测量方法 |
| CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
| CN110196443A (zh) * | 2019-06-06 | 2019-09-03 | 中国人民解放军战略支援部队信息工程大学 | 一种飞行器的容错组合导航方法及*** |
| CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及*** |
| CN111190207A (zh) * | 2020-01-09 | 2020-05-22 | 郑州轻工业大学 | 基于pstcsdref算法的无人机ins bds组合导航方法 |
| CN111189442A (zh) * | 2020-01-11 | 2020-05-22 | 郑州轻工业大学 | 基于cepf的无人机多源导航信息状态预测方法 |
-
2020
- 2020-09-25 CN CN202011025124.7A patent/CN112284388B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018028711A1 (zh) * | 2016-08-12 | 2018-02-15 | 贵州火星探索科技有限公司 | 一种对无人机的噪声协方差进行估算的方法 |
| WO2018214227A1 (zh) * | 2017-05-22 | 2018-11-29 | 深圳市靖洲科技有限公司 | 一种无人车实时姿态测量方法 |
| CN108759838A (zh) * | 2018-05-23 | 2018-11-06 | 安徽科技学院 | 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法 |
| CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
| CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及*** |
| CN110196443A (zh) * | 2019-06-06 | 2019-09-03 | 中国人民解放军战略支援部队信息工程大学 | 一种飞行器的容错组合导航方法及*** |
| CN111190207A (zh) * | 2020-01-09 | 2020-05-22 | 郑州轻工业大学 | 基于pstcsdref算法的无人机ins bds组合导航方法 |
| CN111189442A (zh) * | 2020-01-11 | 2020-05-22 | 郑州轻工业大学 | 基于cepf的无人机多源导航信息状态预测方法 |
Non-Patent Citations (2)
| Title |
|---|
| 低成本多传感器组合导航***在小型无人机自主飞行中的研究与应用;曹娟娟 等;航空学报;第30卷(第10期);正文第1-3节 * |
| 无人机组合导航***的自适应滤波研究;郭强 等;计算机仿真;第29卷(第7期);正文第1-3节 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112284388A (zh) | 2021-01-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
| CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
| CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
| CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** | |
| CN111121766B (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| CN112562077B (zh) | 一种融合pdr和先验地图的行人室内定位方法 | |
| CN112857398B (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
| CN112284388B (zh) | 一种无人机多源信息融合导航方法 | |
| CN111189442B (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| CN116007620A (zh) | 一种组合导航滤波方法、***、电子设备及存储介质 | |
| CN113340298A (zh) | 一种惯导和双天线gnss外参标定方法 | |
| CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
| CN114526731A (zh) | 一种基于助力车的惯性组合导航方向定位方法 | |
| CN117053782A (zh) | 一种水陆两栖机器人组合导航方法 | |
| Anbu et al. | Integration of inertial navigation system with global positioning system using extended kalman filter | |
| CN108416387B (zh) | 基于gps与气压计融合数据的高度滤波方法 | |
| CN111207734B (zh) | 一种基于ekf的无人机组合导航方法 | |
| CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
| CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN112683265B (zh) | 一种基于快速iss集员滤波的mimu/gps组合导航方法 | |
| CN116519015A (zh) | 一种基于相对距离约束的分布式协同导航方法及*** | |
| US11834054B2 (en) | Method for motion estimation in a vehicle, corresponding device and computer program product | |
| CN113551669B (zh) | 基于短基线的组合导航定位方法及装置 | |
| CN115326062A (zh) | 基于仿生扑翼飞行机器人gnss ins组合导航方法、装置及介质 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |