CN103697889B - 一种基于多模型分布式滤波的无人机自主导航与定位方法 - Google Patents
一种基于多模型分布式滤波的无人机自主导航与定位方法 Download PDFInfo
- Publication number
- CN103697889B CN103697889B CN201310741915.3A CN201310741915A CN103697889B CN 103697889 B CN103697889 B CN 103697889B CN 201310741915 A CN201310741915 A CN 201310741915A CN 103697889 B CN103697889 B CN 103697889B
- Authority
- CN
- China
- Prior art keywords
- unmanned plane
- navigation system
- inertial navigation
- omega
- output
- 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 60
- 238000001914 filtration Methods 0.000 title claims abstract description 56
- 230000004807 localization Effects 0.000 title claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 239000011159 matrix material Substances 0.000 claims description 22
- 230000008878 coupling Effects 0.000 claims description 11
- 238000010168 coupling process Methods 0.000 claims description 11
- 238000005859 coupling reaction Methods 0.000 claims description 11
- 230000007704 transition Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 239000004576 sand Substances 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 239000000284 extract Substances 0.000 claims description 2
- 238000012545 processing Methods 0.000 claims description 2
- HOWHQWFXSLOJEF-MGZLOUMQSA-N systemin Chemical compound NCCCC[C@H](N)C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H]([C@@H](C)O)C(=O)N[C@@H](CC(O)=O)C(=O)OC(=O)[C@@H]1CCCN1C(=O)[C@H]1N(C(=O)[C@H](CC(O)=O)NC(=O)[C@H](CCCN=C(N)N)NC(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H]2N(CCC2)C(=O)[C@H]2N(CCC2)C(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H](CCC(N)=O)NC(=O)[C@@H](NC(=O)[C@H](C)N)C(C)C)CCC1 HOWHQWFXSLOJEF-MGZLOUMQSA-N 0.000 claims 1
- 108010050014 systemin Proteins 0.000 claims 1
- 230000007774 longterm Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 6
- 238000005070 sampling Methods 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 2
- 238000013213 extrapolation Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 241000209202 Bromus secalinus Species 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000004744 fabric Substances 0.000 description 1
- 238000011835 investigation 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
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
本发明公开了一种基于多模型分布式滤波的无人机自主导航与定位方法,目的是不仅要降低无人机自主导航与定位方法的计算复杂度,满足嵌入式处理器实时处理的要求,还要保证对无人机进行稳定控制,而且在卫星导航定位系统短期或长期不可用时,也能保证无人机实现自主导航和定位。该方法通过建立不同阶次的多模型分布式滤波状态估计的系统状态方程,利用不同量测系统和传感器获得无人机运动的量测值,采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差,为无人机提供连续、可靠的导航和定位信息。本发明能够长时间连续、稳定地为无人机控制系统提供精确的导航和定位信息。
Description
技术领域
本发明涉及一种无人机自主导航和定位方法,特别是一种基于多模型分布式滤波的无人机自主导航与定位方法,适用于无人飞行器和有人飞行器自主导航和定位系统。
背景技术
与有人机相比,无人机是低风险和低成本,而且在军民用领域中被广泛应用,例如空中监视、地面侦察、灾后重建、遥感探测以及地面目标跟踪等。在实际应用中,无人机对自主导航系统精度和可靠性要求越来越高,要求实时、可靠地提供无人飞行器的位置、速度和姿态信息。由于低成本惯性导航系统误差发散很快,依靠单一的低成本惯性导航系统很难满足要求。将惯性导航系统与卫星定位系统信息进行融合来提供载体的运动信息已被广泛应用。但由于卫星定位信号易受干扰或欺骗,甚至信号转发源被摧毁,不能够长期提供稳定可靠的定位信息,在军民用领域中尚不能完全依赖于卫星定位来辅助惯性导航实现飞行器的精确导航定位。因此,发展一些替代的导航系统用于处理卫星导航系统短期和长期故障时的无人机自主导航和定位问题,已成为军民用领域无人机应用研究的的重点。视觉相机作为无人机配重包中的标准传感器既可用于导航传感器获取无人机的导航和定位信息,又可以用于任务传感器执行特定的任务,例如侦查,运动目标跟踪以及遥感探测,已成为无人机自主导航和定位的有效补充导航源之一。
根据无人机飞行的环境是否已知将基于视觉信息的导航和定位分为两类,一类是飞行环境未知,常用的方法有同时完成定位和生成环境地图的SLAM算法和视觉测距方法,前者在室内无人机和机器人导航定位中已广泛应用,但在室外环境多变的大视场环境无法有效使用;虽然后者可以用于室外多变环境,但需要精确获取无人机的飞行姿态和稳定平台的姿态;另一类是飞行环境已知,常用方法有陆标匹配和图像匹配定位,可以给出无人机精确的导航和位置信息。
本发明为中提到的一种基于多模型分布式滤波的无人机自主导航与定位方法,该方法在东北天地里坐标系下,根据惯性导航系统力学方程编排以及惯性导航系统误差方程建立不同阶次的多模型分布式滤波状态估计的系统状态方程,利用不同量测系统和传感器获得无人机运动的量测值,采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差,为无人机提供连续、可靠的导航和定位信息。
本发明与其它无人机导航和定位方法不同在于:在当前已公开发表的文献中,依靠惯性导航/GPS组合导航或者是依靠视觉辅助惯性导航来提供无人机的导航和定位信息,但这些导航方法在实际应用中依然存在计算复杂度高以及在一些环境中无法使用的局限性,特别是在卫星导航定位系统信号长时间不可用时,导航系统无法提供长时间稳定的姿态信息而导致无人机及其机载设备损毁的风险。
发明内容
本发明要解决的技术问题:为解决在卫星导航定位系统长时间不可用时,导航系统能够长时间提供稳定的姿态和定位信息来控制无人机稳定、可靠地飞行,提出一种基于多模型分布式滤波的无人机自主导航与定位方法。
本发明采用的技术方案为:一种基于多模型分布式滤波的无人机自主导航与定位方法,该方法在东北天地里坐标系下,根据惯性导航系统力学方程编排以及惯性导航系统误差方程建立不同阶次的多模型分布式滤波系统状态方程,利用不同量测系统和传感器获得无人机运动的量测值,采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差,为无人机提供连续、可靠的导航和定位信息。保证在卫星导航定位系统短期或长期不可用时,能够长时间连续、稳定地为无人机提供精确的导航和定位信息。具体实现步骤为:
步骤(1)、当卫星导航系统可用时,采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差,使惯性导航系统输出精确的位置、速度和姿态信息,其具体方法为:
a1、根据惯性导航系统力学方程编排,利用加速度计和陀螺仪的输出值实时计算无人机的运动信息,包括位置、速度和姿态;
a2、根据惯性导航系统误差方程建立惯性导航/卫星导航组合导航系统状态方程为:
式中,X(t)T=[φE,φN,φU,δVE,δVN,δL,δλ]为连续时间系统状态;上标T为转置;E、N和U分别为东北天地理坐标系的坐标轴指向;t为时间;φE、φN和φU分别为惯性导航系统在东、北和天向平台误差角;δVE和δVN分别为惯性导航系统东向和北向速度误差;δL和δλ分别为惯性导航系统北向和东向位置误差;W(t)为系统噪声;F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵;RN和RM分别为卯酉圈和子午面半径;
a3、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为:
式中,Z1为卫星导航系统与惯性导航系统输出值相减获得的量测值;和分别为惯性导航系统输出的东向和北向速度;LI和λI分别指惯性导行系统输出北向位置和东向位置;和分别指卫星导航系统输出的东向和北向速度;LS和λS分别指卫星导航定位系统输出的北向和东向位置;上标或下标中的I和S分别为惯性导航系统和卫星导航定位系统的输出值;V1=[vEvNvLvλ]T为卫星导航系统接收机的噪声;
a4、对式(1)进行离散化后,利用七状态反馈Kalman滤波方法实时估计并补偿惯性导航系统的误差,获得无人机精确的位置、速度和姿态信息。
步骤(2)、采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差,获得惯性导航系统输出精确的高度和垂直通道速度信息,其具体方法为:
b1、根据惯性导航系统力学方程编排,利用加速度计和陀螺仪输出信息计算无人机的高度和垂向速度;
b2、根据惯性导航垂直通道的力学方程编排以及气压计误差模型建立垂直通道误差估计的系统状态方程为:
Xk=Φk|k-1Xk-1+Wk-1(4)
式中, 为系统状态;δh和δVU分别为惯性导航系统输出的高度和垂向速度;δha和分别为气压高计输出高度误差和高度误差的一阶马尔科夫过程;Φk|k-1为系统从k-1到k时刻一步状态转移矩阵;Wk-1为系统在k-1时刻的量测噪声;Δt为采样时间;α和β为气压计误差模型中参数;
b3、利用气压计输出的高度与惯性导航系统输出的高度建立系统的量测方程为:
Z2=H2Xk+V2(6)
式中,Z2为气压计与惯性导航系统输出的高度值相减获得的量测值;H2=[10-10]T;V2为气压计的测量噪声;
b4、根据式(4)和式(6),利用四状态反馈Kalman滤波方法实时估计并补偿惯性导航系统垂直通道的误差,获得无人机运动过程中精确的高度和垂向速度信息。
步骤(3)、当卫星导航定位系统不可用时,采用四状态扩展Kalman滤波估计并补偿四元数误差,保证惯性导航系统长时间输出稳定的姿态信息,其具体方法为:
c1、利用加速度计、陀螺仪和磁传感器输出的信息计算无人机的姿态为:
式中,tan-1(*)为反正切函数;和分别为加速度计沿机体坐标系b输出的x、y和z轴向的比力;和分别为磁传感器沿机体坐标系b输出的x、y和z轴向的磁场强度;γa、θa和ψm分别为由加速度计和磁传感器信息计算出的无人机滚动角、俯仰角和航向角;
c2、根据惯性导航系统的力学方程编排建立四元数估计的系统状态方程为:
式中,为连续时间系统状态量; 为陀螺仪在载体坐标系b输出的理想值; 为陀螺仪在载体坐标系b输出的实际值; 为陀螺仪在载体坐标系b输出的漂移值;q0、q1、q2和q3为四元数的四个分量;
c3、利用加速度计和磁传感器输出值计算出的无人机姿态以及根据惯性导航力学方程编排和陀螺仪输出计算的载体姿态建立系统的量测方程为:
式中,为系统时间t有关的观测值;ψq、和γq分别为根据惯性导航系统力学方程和四元数计算出的无人机偏航角、俯仰角和滚动角;H(t,q)为与时间t和四元数q有关的非线性方程;V(t)为系统量测噪声;
c4、对式(10)和式(11)进行线性化后,利用四状态扩展Kalman滤波实时估计四元数,并实时计算无人机的俯仰角、滚动角和航向角。
步骤(4)、当卫星导航定位系统不可用时,采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差;保证惯性导航系统输出稳定的位置和速度信息,其具体方法为:
d1、当无人机飞过区域的地物特征丰富且预装了飞行区域的基准图像,采用基于灰度信息的景像匹配定位方法获取无人机的位置,基于灰度信息的景像匹配方法有粗匹配和精匹配两个过程,粗匹配采用交叉相关算法,精匹配采用最小二乘算法;
d2、当无人机飞过的区域地物特征不丰富或无人机未预装飞行区域的基准图,采用加速鲁棒特征SURF算法提取相邻两帧实时图像的特征点并进行同名特征点匹配,利用随机抽样一致性RANSAC算法剔除误匹配点,并根据相邻两帧的同名特征点计算无人机位置信息为:
式中,为无人机的相对位移;下标i为像平面上的第i个特征点;下标j为图像序列中的第j帧;下标p为像素坐标系;u和v为像素坐标;为从像素坐标系p到导航坐标系n的转换矩阵;为无人机的位置信息;为卫星导航系统或景像匹配系统在不可用前获取的最后时刻无人机精确位置信息,并将该时刻记为t0;
d3、对步骤d1和d2利用相机获取的无人机位置信息进行可靠性评估,舍弃不可靠位置后获得的可靠定位信息频率低且是非等间隔,根据系统状态方程式(1)和利用视觉传感器获得的视觉定位信息,建立视觉辅助惯性导航系统的量测方程为:
式中,Z2为视觉导航系统输出的位置与惯性导航系统输出的位置相减获得的量测值;Lc和λc分别为视觉导航系统输出的北向和东向位置;H2=[02×5diag(RmRNcosLI)]为量测矩阵;
d4、对式(1)进行离散化后,利用不等间隔Kalman滤波实时估计并补偿惯性导航系统的位置误差,实时获得无人机精确的位置和速度信息。
本发明的优点在于:针对不同的飞行条件,建立不同阶次的惯性导航系统误差估计的状态方程,采用分布式滤波方法估计并补偿惯性导航系统的误差,有效降低了计算复杂度;当卫星导航定位系统不可用时,通过陀螺仪、加速度计、磁传感器和视觉传感器来计算无人机的位置、速度和姿态信息,保证了无人机可以长时间、稳定可靠地飞行和操控。
附图说明
图1为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的原理图;
图2为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的Kalman滤波工原理图;
图3为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的景像匹配流程图;
图4为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的景像匹配粗匹配示意图;
图5为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的SURF特征点匹配示意图;
图6为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的RANSAC算法剔除误匹配点后的示意图;
图7为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的利用视觉推算位置信息的示意图。
具体实施方式
下面结合附图和具体实施方式对本发明进一步说明,本发明的具体实现步骤如下:
(1)对加速度计、陀螺仪、磁传感器、气压计和相机进行标定和误差补偿;
(2)如图1所示,根据惯性导航系统力学方程编排,利用加速度计和陀螺仪的输出值实时计算无人机的运动信息,包括位置、速度和姿态;
(3)如图1和2所示,当卫星导航系统可用时,采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差,使惯性导航系统输出精确的位置、速度和姿态信息,其具体步骤为:
a1、根据惯性导航系统误差方程建立惯性导航/卫星导航组合导航系统状态方程为:
式中,X(t)T=[φE,φN,φU,δVE,δVN,δL,δλ]为连续时间系统状态;上标T为转置;E、N和U分别为东北天地理坐标系的坐标轴指向;t为时间;φE、φN和φU分别为惯性导航系统在东、北和天向平台误差角;δVE和δVN分别为惯性导航系统东向和北向速度误差;δL和δλ分别为惯性导航系统北向和东向位置误差;W(t)为系统噪声;F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵;RN和RM分别为卯酉圈和子午面半径;
a2、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为:
式中,Z1为卫星导航系统与惯性导航系统输出值相减获得的量测值;和分别为惯性导航系统输出的东向和北向速度;LI和λI分别指惯性导行系统输出北向位置和东向位置;和分别指卫星导航系统输出的东向和北向速度;LS和λS分别指卫星导航定位系统输出的北向和东向位置;上标或下标中的I和S分别为惯性导航系统和卫星导航定位系统的输出值;V1=[vEvNvLvλ]T为卫星导航系统接收机的噪声;
a3、七状态反馈Kalman滤波器初始化,包括初始协方差矩阵,测量噪声方差矩阵和系统噪声反差矩阵;
a4、对式(1)进行离散化后,利用七状态反馈Kalman滤波方法实时估计并补偿惯性导航系统的误差,获得无人机精确的位置、速度和姿态信息;
(4)如图1和2所示,采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差,获得惯性导航系统输出精确的高度和垂直通道速度信息,其具体步骤为:
b1、根据惯性导航垂直通道的力学方程编排以及气压计误差模型建立垂直通道误差估计的系统状态方程为:
Xk=Φk|k-1Xk-1+Wk-1(4)
式中, 为系统状态;δh和δVU分别为惯性导航系统输出的高度和垂向速度;δha和分别为气压高计输出高度误差和高度误差的一阶马尔科夫过程;Φk|k-1为系统从k-1到k时刻一步状态转移矩阵;Wk-1为系统在k-1时刻的量测噪声;Δt为采样时间;α和β为气压计误差模型中参数;
b2、利用气压计输出的高度与惯性导航系统输出的高度建立系统的量测方程为:
Z2=H2Xk+V2(6)
式中,Z2为气压计与惯性导航系统输出的高度值相减获得的量测值;H2=[10-10]T;V2为气压计的测量噪声;
b3、四状态反馈Kalman滤波器初始化,包括初始协方差矩阵,测量噪声方差矩阵和系统噪声反差矩阵;
b4、根据式(4)和式(6),利用四状态反馈Kalman滤波方法实时估计并补偿惯性导航系统垂直通道的误差,获得无人机运动过程中精确的高度和垂向速度信息。
(5)当卫星导航定位系统不可用时,采用四状态扩展Kalman滤波估计并补偿四元数误差,保证惯性导航系统长时间输出稳定的姿态信息,其具体步骤为:
c1、利用加速度计、陀螺仪和磁传感器输出的信息计算无人机的姿态为:
式中,tan-1(*)为反正切函数;和分别为加速度计沿机体坐标系b输出的x、y和z轴向的比力;和分别为磁传感器沿机体坐标系b输出的x、y和z轴向的磁场强度;γa、θa和ψm分别为由加速度计和磁传感器信息计算出的无人机滚动角、俯仰角和航向角;
c2、根据惯性导航系统的力学方程编排建立四元数估计的系统状态方程为:
式中,为连续时间系统状态量; 为陀螺仪在载体坐标系b输出的理想值; 为陀螺仪在载体坐标系b输出的实际值; 为陀螺仪在载体坐标系b输出的漂移值;q0、q1、q2和q3为四元数的四个分量;
c3、如图1和图2所示,利用加速度计和磁传感器输出值计算出的无人机姿态以及根据惯性导航力学方程编排和陀螺仪输出计算的载体姿态建立系统的量测方程为:
式中,为系统时间t有关的观测值;ψq、和γq分别为根据惯性导航系统力学方程和四元数计算出的无人机偏航角、俯仰角和滚动角;H(t,q)为与时间t和四元数q有关的非线性方程;V(t)为系统量测噪声;
c4、四状态扩展Kalman滤波器初始化,包括初始协方差矩阵,测量噪声方差矩阵和系统噪声反差矩阵;
c5、对式(10)和式(11)进行线性化后,利用四状态扩展Kalman滤波实时估计四元数,并实时计算无人机的俯仰角、滚动角和航向角;
(6)如图1和图2所示,当卫星导航定位系统不可用时,采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差;保证惯性导航系统输出稳定的位置和速度信息,其具体步骤为:
d1、如图3和图4所示,当无人机飞过区域的地物特征丰富且预装了飞行区域的基准图像,采用基于灰度信息的景像匹配定位方法获取无人机的位置,基于灰度信息的景像匹配方法有粗匹配和精匹配两个过程,粗匹配采用交叉相关算法,精匹配采用最小二乘算法;
d2、如图1、图5、图6和图7所示,当无人机飞过的区域地物特征不丰富或无人机未预装飞行区域的基准图,采用加速鲁棒特征SURF算法提取相邻两帧实时图像的特征点并进行同名特征点匹配,利用随机抽样一致性RANSAC算法剔除误匹配点,并根据相邻两帧的同名特征点计算无人机位置信息为:
式中,为无人机的相对位移;下标i为像平面上的第i个特征点;下标j为图像序列中的第j帧;下标p为像素坐标系;u和v为像素坐标;为从像素坐标系p到导航坐标系n的转换矩阵;为无人机的位置信息;为卫星导航系统或景像匹配系统在不可用前获取的最后时刻无人机精确位置信息,并将该时刻记为t0;
d3、对步骤d1和d2利用相机获取的无人机位置信息进行可靠性评估,舍弃不可靠位置后获得的可靠定位信息频率低且是非等间隔,根据系统状态方程式(1)和利用视觉传感器获得的视觉定位信息,建立视觉辅助惯性导航系统的量测方程为:
式中,Z2为视觉导航系统输出的位置与惯性导航系统输出的位置相减获得的量测值;Lc和λc分别为视觉导航系统输出的北向和东向位置;H2=[02×5diag(RmRNcosLI)]为量测矩阵; 为视觉导航系统的量测噪声;
d4、对不等间隔量测值采用内插和外推的方法实现,量测值等间隔输出;
d5、对式(1)进行离散化后,利用不等间隔Kalman滤波实时估计并补偿惯性导航系统的位置误差,实时获得无人机精确的位置和速度信息。
惯性导航系统力学方程编排、惯性导航系统误差方程以及内插和外推算法都是本领域公知和常用的方法,不再赘述。
Claims (5)
1.一种基于多模型分布式滤波的无人机自主导航与定位方法,其特征在于:该方法在东北天地理坐标系下,根据惯性导航系统力学方程编排以及惯性导航系统误差方程建立不同阶次的多模型分布式滤波状态估计的系统状态方程,利用不同量测系统和传感器获得无人机运动的量测值,采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差,其具体实现过程如下:
步骤(1)、当卫星导航系统可用时,采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差,使惯性导航系统输出精确的位置、速度和姿态信息;
步骤(2)、采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差,获得惯性导航系统输出精确的高度和垂直通道速度信息;
步骤(3)、当卫星导航定位系统不可用时,采用四状态扩展Kalman滤波估计并补偿四元数误差,保证惯性导航系统长时间输出稳定的姿态信息;
步骤(4)、当卫星导航定位系统不可用时,采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差;保证惯性导航系统输出稳定的位置和速度信息。
2.根据权利要求1所述的一种基于多模型分布式滤波的无人机自主导航与定位方法,其特征在于:所述步骤(1)当卫星导航系统可用时,采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差,使惯性导航系统输出精确的位置、速度和姿态信息,其具体方法为:
a1、根据惯性导航系统力学方程编排,利用加速度计和陀螺仪的输出值实时计算无人机的运动信息,包括位置、速度和姿态;
a2、根据惯性导航系统误差方程建立惯性导航/卫星导航组合导航系统状态方程为:
式中,X(t)T=[φE,φN,φU,δVE,δVN,δL,δλ]为连续时间系统状态;上标T为转置;E、N和U分别为东北天地理坐标系的坐标轴指向;t为时间;φE、φN和φU分别为惯性导航系统在东、北和天向平台误差角;δVE和δVN分别为惯性导航系统东向和北向速度误差;δL和δλ分别为惯性导航系统北向和东向位置误差;W(t)为系统噪声;F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵;RN和RM分别为卯酉圈和子午面半径;
a3、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为:
式中,Z1为卫星导航系统与惯性导航系统输出值相减获得的量测值;和分别为惯性导航系统输出的东向和北向速度;LI和λI分别指惯性导行系统输出北向位置和东向位置;和分别指卫星导航系统输出的东向和北向速度;LS和λS分别指卫星导航定位系统输出的北向和东向位置;上标或下标中的I和S分别为惯性导航系统和卫星导航定位系统的输出值;V1=[vEvNvLvλ]T为卫星导航系统接收机的噪声;
a4、对式(1)进行离散化后,利用七状态反馈Kalman滤波方法实时估计并补偿惯性导航系统的误差,获得无人机精确的位置、速度和姿态信息。
3.根据权利要求1所述的一种基于多模型分布式滤波的无人机自主导航与定位方法,其特征在于:所述步骤(2)采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差,获得惯性导航系统输出精确的高度和垂直通道速度信息,其具体方法为:
b1、根据惯性导航系统力学方程编排,利用加速度计和陀螺仪输出信息计算无人机的高度和垂向速度;
b2、根据惯性导航垂直通道的力学方程编排以及气压计误差模型建立垂直通道误差估计的系统状态方程为:
Xk=Φk|k-1Xk-1+Wk-1(4)
式中, 为系统状态;δh和δVU分别为惯性导航系统输出的高度和垂向速度;δha和分别为气压高计输出高度误差和高度误差的一阶马尔科夫过程;Φk|k-1为系统从k-1到k时刻一步状态转移矩阵;Wk-1为系统在k-1时刻的量测噪声;Δt为采样时间;α和β为气压计误差模型中参数;
b3、利用气压计输出的高度与惯性导航系统输出的高度建立系统的量测方程为:
Z2=H2Xk+V2(6)
式中,Z2为气压计与惯性导航系统输出的高度值相减获得的量测值;H2=[10-10]T;V2为气压计的测量噪声;
b4、根据式(4)和式(6),利用四状态反馈Kalman滤波方法实时估计并补偿惯性导航系统垂直通道的误差,获得无人机运动过程中精确的高度和垂向速度信息。
4.根据权利要求1所述的一种基于多模型分布式滤波的无人机自主导航与定位方法,其特征在于:所述步骤(3)当卫星导航定位系统不可用时,采用四状态扩展Kalman滤波估计并补偿四元数误差,保证惯性导航系统长时间输出稳定的姿态信息,其具体方法为:
c1、利用加速度计、陀螺仪和磁传感器输出的信息计算无人机的姿态为:
式中,tan-1(*)为反正切函数;和分别为加速度计沿机体坐标系b输出的x、y和z轴向的比力;和分别为磁传感器沿机体坐标系b输出的x、y和z轴向的磁场强度;γa、θa和ψm分别为由加速度计和磁传感器信息计算出的无人机滚动角、俯仰角和航向角;
c2、根据惯性导航系统的力学方程编排建立四元数估计的系统状态方程为:
式中,为连续时间系统状态量; 为陀螺仪在载体坐标系b输出的理想值; 为陀螺仪在载体坐标系b输出的实际值; 为陀螺仪在载体坐标系b输出的漂移值;q0、q1、q2和q3为四元数的四个分量;
c3、利用加速度计和磁传感器输出值计算出的无人机姿态以及根据惯性导航力学方程编排和陀螺仪输出计算的载体姿态建立系统的量测方程为:
式中,为系统时间t有关的观测值;ψq、和γq分别为根据惯性导航系统力学方程和四元数计算出的无人机偏航角、俯仰角和滚动角;H(t,q)为与时间t和四元数q有关的非线性方程;V(t)为系统量测噪声;
c4、对式(10)和式(11)进行线性化后,利用四状态扩展Kalman滤波实时估计四元数,并实时计算无人机的俯仰角、滚动角和航向角。
5.根据权利要求2所述的一种基于多模型分布式滤波的无人机自主导航与定位方法,其特征在于:所述步骤(4)当卫星导航定位系统不可用时,采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差;保证惯性导航系统输出稳定的位置和速度信息,其具体方法为:
d1、当无人机飞过区域的地物特征丰富且预装了飞行区域的基准图像,采用基于灰度信息的景像匹配定位方法获取无人机的位置,基于灰度信息的景像匹配方法有粗匹配和精匹配两个过程,粗匹配采用交叉相关算法,精匹配采用最小二乘算法;
d2、当无人机飞过的区域地物特征不丰富或无人机未预装飞行区域的基准图,采用加速鲁棒特征SURF算法提取相邻两帧实时图像的特征点并进行同名特征点匹配,利用随机抽样一致性RANSAC算法剔除误匹配点,并根据相邻两帧的同名特征点计算无人机位置信息为:
式中,为无人机的相对位移;下标i为像平面上的第i个特征点;下标j为图像序列中的第j帧;下标p为像素坐标系;u和v为像素坐标;为从像素坐标系p到导航坐标系n的转换矩阵;为无人机的位置信息;为卫星导航系统或景像匹配系统在不可用前获取的最后时刻无人机精确位置信息,并将该时刻记为t0;
d3、对步骤d1和d2利用相机获取的无人机位置信息进行可靠性评估,舍弃不可靠位置后获得的可靠定位信息频率低且是非等间隔,根据系统状态方程式(1)和利用视觉传感器获得的视觉定位信息,建立视觉辅助惯性导航系统的量测方程为:
式中,Z2为视觉导航系统输出的位置与惯性导航系统输出的位置相减获得的量测值;Lc和λc分别为视觉导航系统输出的北向和东向位置;H2=[02×5diag(RmRNcosLI)]为量测矩阵; 为视觉导航系统的量测噪声;
d4、对式(1)进行离散化后,利用不等间隔Kalman滤波实时估计并补偿惯性导航系统的位置误差,实时获得无人机精确的位置和速度信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310741915.3A CN103697889B (zh) | 2013-12-29 | 2013-12-29 | 一种基于多模型分布式滤波的无人机自主导航与定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310741915.3A CN103697889B (zh) | 2013-12-29 | 2013-12-29 | 一种基于多模型分布式滤波的无人机自主导航与定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103697889A CN103697889A (zh) | 2014-04-02 |
CN103697889B true CN103697889B (zh) | 2016-05-25 |
Family
ID=50359501
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310741915.3A Active CN103697889B (zh) | 2013-12-29 | 2013-12-29 | 一种基于多模型分布式滤波的无人机自主导航与定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103697889B (zh) |
Families Citing this family (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104035328B (zh) * | 2014-06-21 | 2017-01-04 | 电子科技大学 | 一种采用干扰估计器的多运动体跟踪控制方法 |
JP6486024B2 (ja) | 2014-07-02 | 2019-03-20 | 三菱重工業株式会社 | 構造物の屋内監視システム及び方法 |
WO2016059930A1 (ja) | 2014-10-17 | 2016-04-21 | ソニー株式会社 | 装置、方法及びプログラム |
CN104536453B (zh) * | 2014-11-28 | 2017-08-04 | 深圳一电航空技术有限公司 | 飞行器的控制方法及装置 |
CN104729506B (zh) * | 2015-03-27 | 2017-11-14 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
CN104792336B (zh) * | 2015-03-31 | 2019-01-25 | 北京航空航天大学 | 一种飞行状态测量方法及装置 |
CN106170676B (zh) * | 2015-07-14 | 2018-10-09 | 深圳市大疆创新科技有限公司 | 用于确定移动平台的移动的方法、设备以及系统 |
CN105259757B (zh) * | 2015-10-22 | 2017-10-17 | 山东科技大学 | 一种受控随机系统的无限时域鲁棒控制器的控制方法 |
CN105783911A (zh) * | 2016-02-29 | 2016-07-20 | 青岛科技大学 | 多传感器信息采集导航系统及方法 |
CN107543540B (zh) * | 2016-06-27 | 2020-05-15 | 杭州海康机器人技术有限公司 | 一种飞行设备的数据融合和飞行模式切换方法及装置 |
CN106403940B (zh) * | 2016-08-26 | 2018-10-19 | 杨百川 | 一种抗大气参数漂移的无人机飞行导航系统高度信息融合方法 |
CN106774409B (zh) * | 2016-12-31 | 2019-11-22 | 北京博鹰通航科技有限公司 | 一种无人机的半自主仿地飞行系统及其控制方法 |
CN107360093B (zh) * | 2017-07-19 | 2020-06-19 | 哈尔滨工业大学深圳研究生院 | 无人机与卫星混合网络通信路由方法及系统 |
CN108957496A (zh) * | 2018-04-18 | 2018-12-07 | 广州市中海达测绘仪器有限公司 | Uav抗gnss失效定位定向接收机及其应用方法 |
CN108931155B (zh) * | 2018-07-09 | 2020-08-14 | 北京航天控制仪器研究所 | 一种不依赖卫星导航增程制导弹药自主制导系统 |
CN109931926B (zh) * | 2019-04-04 | 2023-04-25 | 山东智翼航空科技有限公司 | 一种基于站心坐标系的小型无人机无缝自主式导航方法 |
CN110455286A (zh) * | 2019-07-22 | 2019-11-15 | 深圳联合飞机科技有限公司 | 一种无人机导航方法、导航装置、电子设备及存储介质 |
CN110455285A (zh) * | 2019-07-22 | 2019-11-15 | 深圳联合飞机科技有限公司 | 一种在卫星导航信号失效时的无人机导航方法及导航装置 |
CN111811502B (zh) * | 2020-07-10 | 2022-07-22 | 北京航空航天大学 | 一种运动载体多源信息融合导航方法及系统 |
CN116383966B (zh) * | 2023-03-30 | 2023-11-21 | 中国矿业大学 | 一种基于交互多模型的多无人系统分布式协同定位方法 |
CN118092477A (zh) * | 2024-04-28 | 2024-05-28 | 浙江省圣翔协同创新发展研究院 | 一种基于移动式停机坪的降落方法及装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6856894B1 (en) * | 2003-10-23 | 2005-02-15 | International Business Machines Corporation | Navigating a UAV under remote control and manual control with three dimensional flight depiction |
EP1975646A2 (en) * | 2007-03-28 | 2008-10-01 | Honeywell International Inc. | Lader-based motion estimation for navigation |
CN101598556A (zh) * | 2009-07-15 | 2009-12-09 | 北京航空航天大学 | 一种未知环境下无人机视觉/惯性组合导航方法 |
CN101833104A (zh) * | 2010-04-27 | 2010-09-15 | 北京航空航天大学 | 一种基于多传感器信息融合的三维可视化导航方法 |
CN102353377A (zh) * | 2011-07-12 | 2012-02-15 | 北京航空航天大学 | 一种高空长航时无人机组合导航系统及其导航定位方法 |
-
2013
- 2013-12-29 CN CN201310741915.3A patent/CN103697889B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6856894B1 (en) * | 2003-10-23 | 2005-02-15 | International Business Machines Corporation | Navigating a UAV under remote control and manual control with three dimensional flight depiction |
EP1975646A2 (en) * | 2007-03-28 | 2008-10-01 | Honeywell International Inc. | Lader-based motion estimation for navigation |
CN101598556A (zh) * | 2009-07-15 | 2009-12-09 | 北京航空航天大学 | 一种未知环境下无人机视觉/惯性组合导航方法 |
CN101833104A (zh) * | 2010-04-27 | 2010-09-15 | 北京航空航天大学 | 一种基于多传感器信息融合的三维可视化导航方法 |
CN102353377A (zh) * | 2011-07-12 | 2012-02-15 | 北京航空航天大学 | 一种高空长航时无人机组合导航系统及其导航定位方法 |
Non-Patent Citations (4)
Title |
---|
Vision-Based Guidance and Control of a Hovering Vehicle in Unknown, GPS-denied Environments;Spencer Ahrens et al;《2009 IEEE International Conference on Robotics and Automation》;20090517;第2643-2648页 * |
低成本多传感器组合导航系统在小型无人机自主飞行中的研究与应用;曹娟娟等;《航空学报》;20091031;第30卷(第10期);第1923-1929页 * |
基于多传感器信息融合的无人机自主精确导航技术;卜彦龙;《系统仿真学报》;20100228;第22卷;第70-74页 * |
用分布式滤波器的GPS-捷联惯性组合导航系统;袁信等;《中国惯性技术学报》;19890615;第13-22页 * |
Also Published As
Publication number | Publication date |
---|---|
CN103697889A (zh) | 2014-04-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103697889B (zh) | 一种基于多模型分布式滤波的无人机自主导航与定位方法 | |
CN104729506B (zh) | 一种视觉信息辅助的无人机自主导航定位方法 | |
US10565732B2 (en) | Sensor fusion using inertial and image sensors | |
EP3158293B1 (en) | Sensor fusion using inertial and image sensors | |
CN101858748B (zh) | 高空长航无人机的多传感器容错自主导航方法 | |
Mourikis et al. | Vision-aided inertial navigation for spacecraft entry, descent, and landing | |
CN102506868B (zh) | 基于联邦滤波的sins/smans/trns组合导航方法及系统 | |
WO2016187759A1 (en) | Sensor fusion using inertial and image sensors | |
US20100176987A1 (en) | Method and apparatus to estimate vehicle position and recognized landmark positions using GPS and camera | |
WO2016187758A1 (en) | Sensor fusion using inertial and image sensors | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN111649737B (zh) | 一种面向飞机精密进近着陆的视觉-惯性组合导航方法 | |
CN101598556A (zh) | 一种未知环境下无人机视觉/惯性组合导航方法 | |
CN108709552A (zh) | 一种基于mems的imu和gps紧组合导航方法 | |
CN102538792A (zh) | 一种位置姿态系统的滤波方法 | |
Yun et al. | IMU/Vision/Lidar integrated navigation system in GNSS denied environments | |
CN109544696A (zh) | 一种基于视觉惯性组合的机载增强合成视景虚实图像精确配准方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
Johnson et al. | Design and analysis of map relative localization for access to hazardous landing sites on mars | |
CN112146655A (zh) | 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 | |
CN105928515A (zh) | 一种无人机导航系统 | |
CN107024206A (zh) | 一种基于ggi/gps/ins的组合导航系统 | |
Suzuki et al. | Development of a SIFT based monocular EKF-SLAM algorithm for a small unmanned aerial vehicle | |
Mostafa et al. | Optical flow based approach for vision aided inertial navigation using regression trees | |
CN109341685A (zh) | 一种基于单应变换的固定翼飞机视觉辅助着陆导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right |
Effective date of registration: 20241030 Address after: Room 801, 8th Floor, Building 65, No. 109 Jinghai 3rd Road, Beijing Economic and Technological Development Zone, Daxing District, Beijing, with a budget of 100000 RMB Patentee after: Beijing Weike Zhifei Technology Co.,Ltd. Country or region after: China Address before: 100191 No. 37, Haidian District, Beijing, Xueyuan Road Patentee before: BEIHANG University Country or region before: China |