CN105606094A - 一种基于mems/gps组合***的信息条件匹配滤波估计方法 - Google Patents
一种基于mems/gps组合***的信息条件匹配滤波估计方法 Download PDFInfo
- Publication number
- CN105606094A CN105606094A CN201610094618.8A CN201610094618A CN105606094A CN 105606094 A CN105606094 A CN 105606094A CN 201610094618 A CN201610094618 A CN 201610094618A CN 105606094 A CN105606094 A CN 105606094A
- Authority
- CN
- China
- Prior art keywords
- delta
- gyro
- acc
- axis
- carrier
- 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
Links
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
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- 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/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
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)
- Manufacturing & Machinery (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,该方法基于MEMS惯性仪表和GPS信号接收等信息建立组合导航物理模型,对***的传感器输入信息进行实时分析,设计了信息组合判据,对输入信息进行条件筛选和条件匹配,在信息满足组合判据的条件下进行滤波解算,最终获得运动载体准确的速度、姿态和位置信息。
Description
技术领域
本发明涉及一种组合导航***的导航参数估计方法,尤其涉及MEMS/GPS组合导航***中的信息条件匹配滤波方法,可用于导航估计领域。
背景技术
随着MEMS惯性仪表技术以及军用武器的小型化和智能化发展,MEMS体积小,成本低,功耗低等优势越来越明显,基于MEMS的惯性/GPS组合导航***可应用于空地制导武器,如航空制导炸弹,火箭弹、智能炮弹、无人机、无人靶机等军用领域,也可用于机器人控制、车载定位定向和微小型船舶***、卫星通信等民用领域。MEMS惯性仪表单独使用,存在随着时间累计误差漂移现象,同时由于精度相对低,因此需要和GPS导航***组合,构成完整的组合***,发挥各自优势,获得准确的导航信息。
基于kalman的组合滤波技术的成熟也应用于工程实际,算法框架和流程相对固定,但是在不同的应用状态下,并不能一概而论,依据不同的载体需要做细致的分析和规划。目前大部分工程应用并未完全考虑实际运动过程中的测量误差和误差效应,均采用一种解算流程和误差方程通用的思路,因此需要做技术改进。
发明内容
本发明的技术解决问题:克服现有技术的不足,提供了一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,该方法可用于不同运动状态的载体测量,环境适应性强,将获得的测量信息进行筛选和条件匹配,来判断进行组合滤波,获得估计结果,并提高了估计的准确程度。
本发明的技术解决方案:
一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,步骤如下:
(1)将MEMS/GPS组合***安装在载体上,在载体运动过程中,MEMS惯性仪表实时测量载体坐标系下运动载体的三轴加速度以及三轴角速度值,同时获得GPS测量的在地理坐标系下运动载体的三轴速度和位置信息;
(2)对步骤(1)中所述MEMS惯性仪表测量得到的载体坐标系下的运动载体三轴加速度以及三轴角速度值进行条件筛选匹配,剔除测量野值和无效值,满足筛选条件情况下进行捷联导航解算;
(3)对步骤(1)中所述GPS测量得到的地理坐标系下运动载体的三轴速度和位置信息进行条件筛选匹配,满足筛选条件的情况下表示GPS测量得到的运动载体的三轴速度和位置信息有效,进入步骤(4),否则表示无效,直接进行捷联导航解算;
(4)根据所述载体的运动状态确定进行捷联导航解算或者组合导航解算,进行捷联导航解算时,得到载体的导航信息,该导航信息包括速度信息、位置信息和姿态信息;
进行组合导航解算时,得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表误差;
所述载体的运动状态包括加减速状态、转弯状态和低速运行状态;
(5)将步骤(4)通过组合导航解算得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表误差,分别设置速度误差修正量、位置误差修正量、姿态误差修正量以及仪表误差修正量,对所述MEMS/GPS组合***进行误差校正,最终获得准确的导航信息,完成基于MEMS/GPS组合***的信息条件匹配滤波估计。
所述步骤(2)对三轴角速度值进行条件筛选匹配通过下述公式进行:
其中, ΔGyro_x(k)为第k时刻的X轴角速度值Gyro_x(k)与第k-1时刻的X轴角速度值Gyro_x(k-1)之间的差值,ΔGyro_x(k-1)为第k-1时刻的X轴角速度值Gyro_x(k-1)与第k-2时刻的X轴角速度值Gyro_x(k-2)之间的差值;
Yuzhi_Gyro_x、Yuzhi_Gyro_y和Yuzhi_Gyro_z为角速度预设阈值,K_Gyro_x、K_Gyro_y和K_Gyro_z为角速度比例系数。
所述步骤(2)对三轴加速度值进行条件筛选匹配通过下述公式进行:
其中, ΔAcc_x(k)为第k时刻的X轴加速度值Acc_x(k)与第k-1时刻的X轴加速度值Acc_x(k-1)之间的差值,ΔAcc_x(k-1)为第k-1时刻的X加角速度值Acc_x(k-1)与第k-2时刻的X轴加速度值Acc_x(k-2)之间的差值;
Yuzhi_Acc_x、Yuzhi_Acc_y和Yuzhi_Acc_z为加速度预设阈值,K_Acc_x、K_Acc_y和K_Acc_z为加速度比例系数。
所述角速度预设阈值和角速度比例系数具体为:
所述加速度预设阈值和加速度比例系数具体为:
所述步骤(3)对GPS测量得到的地理坐标系下运动载体的三轴速度和位置信息进行条件筛选匹配,具体为:
其中, ΔLong为GPS经度测量值与惯导解算的经度值之差,ΔLat为GPS纬度测量值与惯导解算的纬度值之差,ΔHeigh为GPS高度测量值与惯导解算的高度值之差,ΔVe为GPS东向速度测量值与惯导解算的东向速度值之差,ΔVn为GPS北向速度测量值与惯导解算的北向速度值之差,ΔVu为GPS天向速度测量值与惯导解算的天向速度值之差。
所述步骤(4)根据所述载体的运动状态确定进行捷联导航解算或者组合导航解算具体为:
(7.1)判断载体是否处于加减速状态,如果处于加速或者减速状态,则进行捷联导航解算,否则进入步骤(7.2);
通过载体的前向加速度来判断载体的加减速状态:|Acc_y|≥1m/s^2;
(7.2)判断载体是否处于转弯状态,如果处于转弯状态,则进行捷联导航解算,否则进入步骤(7.3);
用天向的陀螺来进行弯道检测,|Gyro_z|≥20°/s;
(7.3)判断载体是否处于低速运行状态,如果处于低速运行状态,则进行捷联导航解算,否则进行组合导航解算;
所述低速运行状态是指载体满足
所述步骤(5)中将通过组合导航解算得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表,分别设置速度误差修正量、位置误差修正量、姿态误差修正量以及仪表误差修正量,对所述MEMS/GPS组合***进行误差校正,最终获得准确的导航信息,具体为:
令速度误差修正量等于速度误差值,位置误差修正量等于位置误差值,姿态误差修正量为:其中,Δφx′、Δφy′和Δφz′为姿态误差修正量的三轴分量,Δφx、Δφy和Δφz为通过组合导航解算得到载体的姿态误差三轴分量,P1和P2为加权系数且P1+P2=1,k0z为MEMS惯性仪表中陀螺仪的漂移误差,ΔT=k,k为当前时刻的值;
对所述MEMS/GPS组合***进行误差校正具体为:
其中,Long为校正后的经度值,Lat为校正后的纬度值,Height为校正后的高度值,Ve为校正后的东向速度,Vn为校正后的北向速度,Vu为校正后的天向速度,为校正后的姿态矩阵,Long_Sins、Lat_Sins和Heigh_Sins分别为捷联解算获得的载体的经度值、纬度值和高度值,Ve_Sins、Vn_Sins和Vu_Sins分别为捷联解算获得的载体的东向速度值、北向速度值和天向速度值;ΔLong_klm、ΔLat_klm和ΔHeigh_klm分别为位置误差修正量中的经度误差修正量、纬度误差修正量以及高度误差修正量;ΔVe_klm、ΔVn_klm和ΔVu_klm分别为速度误差修正量中的东向速度误差修正量、北向速度误差修正量以及天向速度误差修正量;Δφx′、Δφy′、Δφz′分别为姿态误差修正量中的俯仰角误差修正量、横滚角误差修正量以及偏航角误差修正量,为由姿态误差修正量构成的修正矩阵,为捷联解算获得的姿态矩阵。
本发明与现有方法相比的优点在于:
(1)以往方法使用直接滤波估计,并不对当前时刻的运动状态和测量误差进行估计,这样会引入误差,甚至发散。本发明采用测量值对当前的运动状态进行预估,同时也对当前时刻的测量信息进行误差判断,依据实际动态试验的数据设定了滤波的条件,判断当前时刻是否满足滤波条件,满足则进入信息融合,不满足则进入纯解算或者纯卫星导航状态;
(2)本发明通过分析载体运动状态,考虑了不同运动过程中存在的误差放大等效应,设定了滤波匹配条件,使得kalman滤波适用于任何动态下都能准确的估计出载体的位置、速度和姿态信息;
(3)应用本发明可以保证任意时刻的估计误差均能达到最小状态,长时间可以对试验中的测量信息进行更详尽的分析,而以往的方法仅能粗略的获得当前的导航值,并不能时刻保持较高精度。
附图说明
图1为本发明方法流程图;
图2为信息条件筛选流程图;
图3为位置经纬度对比图;
图4为东向速度对比图;
图5为北向速度对比图;
图6为俯仰角对比图;
图7为横滚角对比图;
图8为偏航角对比图;
具体实施方式
在惯性和GPS卫星组合时,采用kalman信息融合算法将二者紧密结合是常见的方法,由于实际的数学模型线性化,未能完全描述载体的运动状态,直接进行组合会造成数学模型的非线性误差以及引入的测量误差使算法发散,并不能保证实际***的完整可靠,同时非线性滤波方法的运算量大,在DSP等数字平台上实现有困难。针对这些问题,本发明提出了将信息进行筛选衡量,条件匹配的信息融合滤波算法,通过实际搭载的平台进行了试验,获得了高精度的导航信息。
本发明方法流程图如图1所示,其步骤如下:
(1)将MEMS/GPS组合***即MEMS惯性仪表和GPS模块组成的导航***安装在载体上,在载体运动过程中,MEMS惯性仪表实时测量载体坐标系下运动载体的三轴加速度以及三轴角速度值,载体坐标系的原点为载体质心,X轴指向载体右侧,Y轴指向载体前向,Z轴满足右手定则垂直向上;同时获得GPS测量的在地理坐标系下运动载体的三轴速度和位置信息,地理坐标系是指向经纬度表示地面点位的球面坐标系,X、Y和Z分别指向东、北、天。
(2)当前时刻的MEMS惯性仪表的测量信息存在野值错误帧等情况,因此不能直接作为输入量进行导航解算即通过测量的加速度和角速度值进行积分得到导航信息,根据载体的运动特性,对运动状态进行了条件预估以及运动阈值设计。条件预估采用相邻两时刻的测量值,作为此时刻运动状态估计的依据。
对步骤(1)中所述MEMS惯性仪表测量得到的载体坐标系下的运动载体三轴加速度以及三轴角速度值进行条件筛选匹配,剔除测量野值和无效值,满足筛选条件情况下进行捷联导航解算;
对三轴角速度值进行条件筛选匹配通过下述公式进行:
其中, ΔGyro_x(k)为第k时刻的X轴角速度值Gyro_x(k)与第k-1时刻的X轴角速度值Gyro_x(k-1)之间的差值,ΔGyro_x(k-1)为第k-1时刻的X轴角速度值Gyro_x(k-1)与第k-2时刻的X轴角速度值Gyro_x(k-2)之间的差值;
Yuzhi_Gyro_x、Yuzhi_Gyro_y和Yuzhi_Gyro_z为角速度预设阈值,K_Gyro_x、K_Gyro_y和K_Gyro_z为角速度比例系数。
对三轴加速度值进行条件筛选匹配通过下述公式进行:
其中, ΔAcc_x(k)为第k时刻的X轴加速度值Acc_x(k)与第k-1时刻的X轴加速度值Acc_x(k-1)之间的差值,ΔAcc_x(k-1)为第k-1时刻的X加角速度值Acc_x(k-1)与第k-2时刻的X轴加速度值Acc_x(k-2)之间的差值;
Yuzhi_Acc_x、Yuzhi_Acc_y和Yuzhi_Acc_z为加速度预设阈值,K_Acc_x、K_Acc_y和K_Acc_z为加速度比例系数。
以上公式主要是获得三轴陀螺前后两时刻测量差值,差值与设计的阈值Yuzhi和比例系数的积即判定条件进行比较,若前后两时刻的差值小于判定条件,则认为此时刻陀螺的测量数据有效,有效数据进入导航解算。车载***,俯仰角和横滚角变化不大,航向角则在转弯和掉头等路况中变化大,所述角速度预设阈值和角速度比例系数具体为:
同样如车载***,车辆运动体现在加减速,因此Y轴的加速度计的变化量大于其他两轴,依据经验数据所述加速度预设阈值和加速度比例系数具体为:
(3)对步骤(1)中所述GPS测量得到的地理坐标系下运动载体的三轴速度和位置信息进行条件筛选匹配,剔除无效信息,满足筛选条件的情况下表示GPS测量得到的运动载体的三轴速度和位置信息有效,进入步骤(4),否则表示无效,直接进行捷联导航解算,信息条件筛选流程图如图2所示
在卫星信号失锁时,输出数据无效,在重捕获时,数据的有效性也需进行判断,同时冷启动和热启动因此也需要对GPS数据进行有效性判断,GPS仅用位置和速度作为观测量,因此仅对此参数进行阈值判断。主要思想是,当前获得的GPS值与惯导解算的值进行比较,在实时修正下,两者差别不大,当GPS满足以下条件时,可与惯性导航的结果进行信息融合。
对GPS测量得到的地理坐标系下运动载体的三轴速度和位置信息进行条件筛选匹配,具体为:
其中, ΔLong为GPS经度测量值与惯导解算的经度值之差,ΔLat为GPS纬度测量值与惯导解算的纬度值之差,ΔHeigh为GPS高度测量值与惯导解算的高度值之差,ΔVe为GPS东向速度测量值与惯导解算的东向速度值之差,ΔVn为GPS北向速度测量值与惯导解算的北向速度值之差,ΔVu为GPS天向速度测量值与惯导解算的天向速度值之差。
(4)通过获得的输入信息对载体的运动状态转弯、加速、减速等进行判断;通过GPS卫星导航信息与IMU捷联解算的导航值设置kalman(一种数据融合算法)信息匹配条件,满足筛选条件的进入组合导航即将捷联导航数据和GPS导航的数据通过kalman滤波算法进行数据融合,估计出导航误差,包括速度误差、位置误差和姿态误差。
根据所述载体的运动状态确定进行捷联导航解算或者组合导航解算,进行捷联导航解算时,得到载体的导航信息,该导航信息包括速度信息、位置信息和姿态信息;
进行组合导航解算时,得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表误差;
考虑到观测信息在转弯、加减速频繁和低速的情况下,定位定速精度相应减小,为避免kalman解算时引入更多的误差信息,因此在观测信息精度损失的情况下不进行组合,反之其他状态下进行组合。
所述载体的运动状态确定进行捷联导航解算或者组合导航解算具体为:
(4.1)判断载体是否处于加减速状态,如果处于加速或者减速状态,则进行捷联导航解算,否则进入步骤(4.2);
通过载体的前向加速度来判断载体的加减速状态:|Acc_y|≥1m/s^2;
(4.2)判断载体是否处于转弯状态,如果处于转弯状态,则进行捷联导航解算,否则进入步骤(4.3);
用天向的陀螺来进行弯道检测,|Gyro_z|≥20°/s;
(4.3)判断载体是否处于低速运行状态,如果处于低速运行状态,则进行捷联导航解算,否则进行组合导航解算;
所述低速运行状态是指载体满足
GPS失锁后,重捕获能力的快速性会影响到组合算法的精度,本方法采用时间计时器来判断,载体是否处于“GPS卫星信号的重捕获合的过渡状态”,如果是,则不进行kalman组合导航,具体判断阈值如下所示:
Cnt_flag≥3
其中Cnt_flag用于计算载体从GPS卫星信号的重捕获合的过渡状态的标识计数,经过长时间的纯捷联解算,重新进入组合状态时,误差很大,此时的修正会引入很大的过渡误差,因此头三次不进入kalman,采用简单直接取代的方式,之后进入kalman修正。
标识计数根据GPS重捕获的快速性来确定,其他参数值也根据实际情况而定,本方法的参数均为多次跑车试验的经验值。
(5)将步骤(4)通过组合导航解算得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表误差,分别设置速度误差修正量、位置误差修正量、姿态误差修正量以及仪表误差修正量,对所述MEMS/GPS组合***进行误差校正,最终获得准确的导航信息,完成基于MEMS/GPS组合***的信息条件匹配滤波估计。
由于陀螺仪存在漂移误差,本方法结合Kalman估计的角度误差和仪表的漂移误差,对角度修正量进行比例支配,P1和P2为比例系数,根据陀螺的误差特性设计。
由于建立的Kalman误差模型,航向角误差不可观,因此误差估计不准,但是实际上陀螺的漂移是随时间积累而客观存在的,因此需要进行补偿,求得两次修正时间内,航向陀螺仪的漂移量,与Kalman估计的航向误差角进行比例结合,获得最终的误差角,具体如下:
令速度误差修正量等于速度误差值,位置误差修正量等于位置误差值,姿态误差修正量为: 其中,Δφx′、Δφy′和Δφz′为姿态误差修正量的三轴分量,Δφx、Δφy和Δφz为通过组合导航解算得到载体的姿态误差三轴分量,P1和P2为加权系数且P1+P2=1,k0z为MEMS惯性仪表中陀螺仪的漂移误差,ΔT=k。
估计的速度和位置误差均可观测,因此可以通过GPS的观测量获得当前时刻纯捷联导航解算的误差值,采用输出校正方式,如下述公式所示,直接减去误差量,求得准确的速度和位置。
对所述MEMS/GPS组合***进行误差校正具体为:
俯仰角和横滚角可观测,因此直接采用估计的状态量参与姿态修正。修正矩阵如下:
其中,Long为校正后的经度值,Lat为校正后的纬度值,Height为校正后的高度值,Ve为校正后的东向速度,Vn为校正后的北向速度,Vu为校正后的天向速度,为校正后的姿态矩阵,Long_Sins、Lat_Sins和Heigh_Sins分别为捷联解算获得的载体的经度值、纬度值和高度值,Ve_Sins、Vn_Sins和Vu_Sins分别为捷联解算获得的载体的东向速度值、北向速度值和天向速度值;ΔLong_klm、ΔLat_klm和ΔHeigh_klm分别为位置误差修正量中的经度误差修正量、纬度误差修正量以及高度误差修正量;ΔVe_klm、ΔVn_klm和ΔVu_klm分别为速度误差修正量中的东向速度误差修正量、北向速度误差修正量以及天向速度误差修正量;Δφx′、Δφy′、Δφz′分别为姿态误差修正量中的俯仰角误差修正量、横滚角误差修正量以及偏航角误差修正量,为由姿态误差修正量构成的修正矩阵,为捷联解算获得的姿态矩阵。
实例1:将此方法在MEMS/GPS惯组平台上进行了跑车试验,并与高精度的激光惯组的导航参数进行比较,获得最终的导航误差。MEMS惯性陀螺的精度为60°/h,加速度计的精度为10mg,依据此MEMS仪表的特性,设定了陀螺和加表的数据有效比例值和航向角误差的加权值如下:
表1导航误差统计表
序号 | 导航参数 | 误差值 | 单位 |
1 | 俯仰角 | 0.09 | ° |
2 | 横滚角 | 0.08 | ° |
3 | 偏航角 | 2.76 | ° |
4 | 东向速度 | 0.26 | m/s |
5 | 北向速度 | 0.29 | m/s |
6 | 东向距离 | 4.81 | m |
7 | 北向距离 | 3.66 | m |
图3~图8为跑车试验结果图,本方法的结果与高精度惯组的导航结果进行比较,其中图3位置经纬度对比图,横坐标表示纬度,单位“°”,纵坐标表示经度,单位“°”;图4为东向速度对比图,横坐标表示时间,单位秒“s”,纵坐标表示北向速度,单位为“m/s”;图5为北向速度对比图,横坐标表示时间,单位秒“s”,纵坐标表示北向速度,单位为“m/s”;图6为俯仰角对比图横坐标表示时间,单位秒“°”,纵坐标表示北向速度,单位为“m/s”;图7为横滚角对比图,横坐标表示时间,单位秒“s”,纵坐标表示北向速度,单位为“°”;图8为偏航角对比图,横坐标表示时间,单位秒“s”,纵坐标表示北向速度,单位为“°”。跑车试验全程曲线趋势一致,证明了本发明方法的可靠性。
以上表格为此方法的跑车试验的导航误差值,姿态角误差在0.1°以下,偏航角误差在5°以下,速度误差在0.5m/s以内,位置误差在5.0m以内,由于惯性导航在垂直方向发散,因此天向的速度和位置信息不参与计算。从表格中可看出数据精度高,充分证明了本发明方法在实际使用过程中达到了比较好的试验效果,验证了此方法可行性。
本发明未详细描述内容为本领域技术人员公知技术。
Claims (8)
1.一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于步骤如下:
(1)将MEMS/GPS组合***安装在载体上,在载体运动过程中,MEMS惯性仪表实时测量载体坐标系下运动载体的三轴加速度以及三轴角速度值,同时获得GPS测量的在地理坐标系下运动载体的三轴速度和位置信息;
(2)对步骤(1)中所述MEMS惯性仪表测量得到的载体坐标系下的运动载体三轴加速度以及三轴角速度值进行条件筛选匹配,剔除测量野值和无效值,满足筛选条件情况下进行捷联导航解算;
(3)对步骤(1)中所述GPS测量得到的地理坐标系下运动载体的三轴速度和位置信息进行条件筛选匹配,满足筛选条件的情况下表示GPS测量得到的运动载体的三轴速度和位置信息有效,进入步骤(4),否则表示无效,直接进行捷联导航解算;
(4)根据所述载体的运动状态确定进行捷联导航解算或者组合导航解算,进行捷联导航解算时,得到载体的导航信息,该导航信息包括速度信息、位置信息和姿态信息;
进行组合导航解算时,得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表误差;
所述载体的运动状态包括加减速状态、转弯状态和低速运行状态;
(5)将步骤(4)通过组合导航解算得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表误差,分别设置速度误差修正量、位置误差修正量、姿态误差修正量以及仪表误差修正量,对所述MEMS/GPS组合***进行误差校正,最终获得准确的导航信息,完成基于MEMS/GPS组合***的信息条件匹配滤波估计。
2.根据权利要求1所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述步骤(2)对三轴角速度值进行条件筛选匹配通过下述公式进行:
其中, ΔGyro_x(k)为第k时刻的X轴角速度值Gyro_x(k)与第k-1时刻的X轴角速度值Gyro_x(k-1)之间的差值,ΔGyro_x(k-1)为第k-1时刻的X轴角速度值Gyro_x(k-1)与第k-2时刻的X轴角速度值Gyro_x(k-2)之间的差值;
Yuzhi_Gyro_x、Yuzhi_Gyro_y和Yuzhi_Gyro_z为角速度预设阈值,K_Gyro_x、K_Gyro_y和K_Gyro_z为角速度比例系数。
3.根据权利要求1所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述步骤(2)对三轴加速度值进行条件筛选匹配通过下述公式进行:
其中, ΔAcc_x(k)为第k时刻的X轴加速度值Acc_x(k)与第k-1时刻的X轴加速度值Acc_x(k-1)之间的差值,ΔAcc_x(k-1)为第k-1时刻的X加角速度值Acc_x(k-1)与第k-2时刻的X轴加速度值Acc_x(k-2)之间的差值;
Yuzhi_Acc_x、Yuzhi_Acc_y和Yuzhi_Acc_z为加速度预设阈值,K_Acc_x、K_Acc_y和K_Acc_z为加速度比例系数。
4.根据权利要求2所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述角速度预设阈值和角速度比例系数具体为:
5.根据权利要求3所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述加速度预设阈值和加速度比例系数具体为:
6.根据权利要求1所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述步骤(3)对GPS测量得到的地理坐标系下运动载体的三轴速度和位置信息进行条件筛选匹配,具体为:
|ΔLong|≤Yuzhi_Long,Yuzhi_Long=5×10-4
|ΔLat|≤Yuzhi_Lat,Yuzhi_Lat=5×10-4,
|ΔHeigh|≤Yuzhi_Heigh,Yuzhi_Heigh=10
|ΔVe|≤Yuzhi_Ve,Yuzhi_Ve=10
|ΔVn|≤Yuzhi_Vn,Yuzhi_Vn=10
|ΔVu|≤Yuzhi_Vu,Yuzhi_Vu=10
其中, ΔLong为GPS经度测量值与惯导解算的经度值之差,ΔLat为GPS纬度测量值与惯导解算的纬度值之差,ΔHeigh为GPS高度测量值与惯导解算的高度值之差,ΔVe为GPS东向速度测量值与惯导解算的东向速度值之差,ΔVn为GPS北向速度测量值与惯导解算的北向速度值之差,ΔVu为GPS天向速度测量值与惯导解算的天向速度值之差。
7.根据权利要求1所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述步骤(4)根据所述载体的运动状态确定进行捷联导航解算或者组合导航解算具体为:
(7.1)判断载体是否处于加减速状态,如果处于加速或者减速状态,则进行捷联导航解算,否则进入步骤(7.2);
通过载体的前向加速度来判断载体的加减速状态:|Acc_y|≥1m/s^2;
(7.2)判断载体是否处于转弯状态,如果处于转弯状态,则进行捷联导航解算,否则进入步骤(7.3);
用天向的陀螺来进行弯道检测,|Gyro_z|≥20°/s;
(7.3)判断载体是否处于低速运行状态,如果处于低速运行状态,则进行捷联导航解算,否则进行组合导航解算;
所述低速运行状态是指载体满足
8.根据权利要求1所述的一种基于MEMS/GPS组合***的信息条件匹配滤波估计方法,其特征在于:所述步骤(5)中将通过组合导航解算得到载体的速度误差、位置误差、姿态误差和MEMS惯性仪表,分别设置速度误差修正量、位置误差修正量、姿态误差修正量以及仪表误差修正量,对所述MEMS/GPS组合***进行误差校正,最终获得准确的导航信息,具体为:
令速度误差修正量等于速度误差值,位置误差修正量等于位置误差值,姿态误差修正量为: 其中,Δφx′、Δφy′和Δφz′为姿态误差修正量的三轴分量,Δφx、Δφy和Δφz为通过组合导航解算得到载体的姿态误差三轴分量,P1和P2为加权系数且P1+P2=1,k0z为MEMS惯性仪表中陀螺仪的漂移误差,ΔT=k,k为当前时刻的值;
对所述MEMS/GPS组合***进行误差校正具体为:
其中,Long为校正后的经度值,Lat为校正后的纬度值,Height为校正后的高度值,Ve为校正后的东向速度,Vn为校正后的北向速度,Vu为校正后的天向速度,为校正后的姿态矩阵,Long_Sins、Lat_Sins和Heigh_Sins分别为捷联解算获得的载体的经度值、纬度值和高度值,Ve_Sins、Vn_Sins和Vu_Sins分别为捷联解算获得的载体的东向速度值、北向速度值和天向速度值;ΔLong_klm、ΔLat_klm和ΔHeigh_klm分别为位置误差修正量中的经度误差修正量、纬度误差修正量以及高度误差修正量;ΔVe_klm、ΔVn_klm和ΔVu_klm分别为速度误差修正量中的东向速度误差修正量、北向速度误差修正量以及天向速度误差修正量;Δφx′、Δφy′、Δφz′分别为姿态误差修正量中的俯仰角误差修正量、横滚角误差修正量以及偏航角误差修正量,为由姿态误差修正量构成的修正矩阵,为捷联解算获得的姿态矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610094618.8A CN105606094B (zh) | 2016-02-19 | 2016-02-19 | 一种基于mems/gps组合***的信息条件匹配滤波估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610094618.8A CN105606094B (zh) | 2016-02-19 | 2016-02-19 | 一种基于mems/gps组合***的信息条件匹配滤波估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105606094A true CN105606094A (zh) | 2016-05-25 |
CN105606094B CN105606094B (zh) | 2018-08-21 |
Family
ID=55986226
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610094618.8A Active CN105606094B (zh) | 2016-02-19 | 2016-02-19 | 一种基于mems/gps组合***的信息条件匹配滤波估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105606094B (zh) |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106679657A (zh) * | 2016-12-06 | 2017-05-17 | 北京航空航天大学 | 一种运动载体导航定位方法及装置 |
CN106980134A (zh) * | 2017-03-07 | 2017-07-25 | 广州汽车集团股份有限公司 | 智能车长隧道出口gps漂移路段控制***及方法 |
CN107607976A (zh) * | 2017-07-28 | 2018-01-19 | 烟台持久钟表有限公司 | 北斗与自主传感器定位装置及其定位方法 |
CN107764260A (zh) * | 2017-09-30 | 2018-03-06 | 湖南城市学院 | 一种卫星航迹定位*** |
CN107783163A (zh) * | 2016-08-31 | 2018-03-09 | 地壳(北京)机器人科技有限公司 | 一种智能轮式机器人行进航向角融合方法 |
CN108007455A (zh) * | 2017-11-01 | 2018-05-08 | 千寻位置网络有限公司 | 惯导***的纠偏方法及装置、导航及服务终端、存储器 |
WO2018157309A1 (zh) * | 2017-02-28 | 2018-09-07 | 深圳市大疆创新科技有限公司 | 航线修正的方法、设备和无人机 |
CN109459772A (zh) * | 2018-11-28 | 2019-03-12 | 江苏理工学院 | 一种飞行器位置信号融合滤波方法 |
CN109521450A (zh) * | 2017-09-20 | 2019-03-26 | 高德信息技术有限公司 | 一种定位漂移检测方法和装置 |
CN110045152A (zh) * | 2019-05-14 | 2019-07-23 | 爱动超越人工智能科技(北京)有限责任公司 | 一种特种车辆行进状态检测方法及装置 |
CN110864688A (zh) * | 2019-11-28 | 2020-03-06 | 湖南率为控制科技有限公司 | 一种用于车载方位开环水平姿态角闭环的航姿方法 |
CN110926483A (zh) * | 2019-11-25 | 2020-03-27 | 奥特酷智能科技(南京)有限公司 | 一种用于自动驾驶的低成本传感器组合定位***及方法 |
CN110940344A (zh) * | 2019-11-25 | 2020-03-31 | 奥特酷智能科技(南京)有限公司 | 一种用于自动驾驶的低成本传感器组合定位方法 |
CN112882072A (zh) * | 2021-01-15 | 2021-06-01 | 中国人民解放军海军航空大学 | 一种基于误差反馈的飞行器惯性加gps混合高度测量方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1903306A2 (en) * | 2006-09-19 | 2008-03-26 | Alpine Electronics, Inc. | Method and system for estimating ground vehicle dynamics based on an integrated MEMS-INS/GPS navigation system |
CN104422948A (zh) * | 2013-09-11 | 2015-03-18 | 南京理工大学 | 一种嵌入式组合导航***及其方法 |
CN104570033A (zh) * | 2015-01-06 | 2015-04-29 | 中电科航空电子有限公司 | 一种飞机机载gps与惯性导航***组合定位方法 |
CN105021192A (zh) * | 2015-07-30 | 2015-11-04 | 华南理工大学 | 一种基于零速校正的组合导航***的实现方法 |
-
2016
- 2016-02-19 CN CN201610094618.8A patent/CN105606094B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1903306A2 (en) * | 2006-09-19 | 2008-03-26 | Alpine Electronics, Inc. | Method and system for estimating ground vehicle dynamics based on an integrated MEMS-INS/GPS navigation system |
CN104422948A (zh) * | 2013-09-11 | 2015-03-18 | 南京理工大学 | 一种嵌入式组合导航***及其方法 |
CN104570033A (zh) * | 2015-01-06 | 2015-04-29 | 中电科航空电子有限公司 | 一种飞机机载gps与惯性导航***组合定位方法 |
CN105021192A (zh) * | 2015-07-30 | 2015-11-04 | 华南理工大学 | 一种基于零速校正的组合导航***的实现方法 |
Non-Patent Citations (2)
Title |
---|
周艳丽 等,: ""基于GPS/MEMS微惯性导航***的混合校正方法研究"", 《导航与控制》 * |
王春霞 等,: ""动态捷联惯导/多卫星组合导航自适应联邦滤波算法研究"", 《信息与控制》 * |
Cited By (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107783163A (zh) * | 2016-08-31 | 2018-03-09 | 地壳(北京)机器人科技有限公司 | 一种智能轮式机器人行进航向角融合方法 |
CN106679657B (zh) * | 2016-12-06 | 2019-10-25 | 北京航空航天大学 | 一种运动载体导航定位方法及装置 |
CN106679657A (zh) * | 2016-12-06 | 2017-05-17 | 北京航空航天大学 | 一种运动载体导航定位方法及装置 |
WO2018157309A1 (zh) * | 2017-02-28 | 2018-09-07 | 深圳市大疆创新科技有限公司 | 航线修正的方法、设备和无人机 |
CN108885464A (zh) * | 2017-02-28 | 2018-11-23 | 深圳市大疆创新科技有限公司 | 航线修正的方法、设备和无人机 |
CN106980134A (zh) * | 2017-03-07 | 2017-07-25 | 广州汽车集团股份有限公司 | 智能车长隧道出口gps漂移路段控制***及方法 |
CN107607976A (zh) * | 2017-07-28 | 2018-01-19 | 烟台持久钟表有限公司 | 北斗与自主传感器定位装置及其定位方法 |
CN109521450A (zh) * | 2017-09-20 | 2019-03-26 | 高德信息技术有限公司 | 一种定位漂移检测方法和装置 |
CN107764260A (zh) * | 2017-09-30 | 2018-03-06 | 湖南城市学院 | 一种卫星航迹定位*** |
CN108007455A (zh) * | 2017-11-01 | 2018-05-08 | 千寻位置网络有限公司 | 惯导***的纠偏方法及装置、导航及服务终端、存储器 |
CN109459772A (zh) * | 2018-11-28 | 2019-03-12 | 江苏理工学院 | 一种飞行器位置信号融合滤波方法 |
CN110045152A (zh) * | 2019-05-14 | 2019-07-23 | 爱动超越人工智能科技(北京)有限责任公司 | 一种特种车辆行进状态检测方法及装置 |
CN110926483A (zh) * | 2019-11-25 | 2020-03-27 | 奥特酷智能科技(南京)有限公司 | 一种用于自动驾驶的低成本传感器组合定位***及方法 |
CN110940344A (zh) * | 2019-11-25 | 2020-03-31 | 奥特酷智能科技(南京)有限公司 | 一种用于自动驾驶的低成本传感器组合定位方法 |
CN110864688A (zh) * | 2019-11-28 | 2020-03-06 | 湖南率为控制科技有限公司 | 一种用于车载方位开环水平姿态角闭环的航姿方法 |
CN112882072A (zh) * | 2021-01-15 | 2021-06-01 | 中国人民解放军海军航空大学 | 一种基于误差反馈的飞行器惯性加gps混合高度测量方法 |
Also Published As
Publication number | Publication date |
---|---|
CN105606094B (zh) | 2018-08-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105606094A (zh) | 一种基于mems/gps组合***的信息条件匹配滤波估计方法 | |
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN110631593B (zh) | 一种用于自动驾驶场景的多传感器融合定位方法 | |
CN109556632B (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
CN109459044B (zh) | 一种gnss双天线辅助的车载mems惯导组合导航方法 | |
Georgy et al. | Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation | |
Wendel et al. | A performance comparison of tightly coupled GPS/INS navigation systems based on extended and sigma point Kalman filters | |
Iqbal et al. | An integrated reduced inertial sensor system—RISS/GPS for land vehicle | |
CN102169184B (zh) | 组合导航***中测量双天线gps安装失准角的方法和装置 | |
CN106885570A (zh) | 一种基于鲁棒sckf滤波的紧组合导航方法 | |
CN102706366B (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
CN101216321A (zh) | 捷联惯性导航***的快速精对准方法 | |
CN103076017B (zh) | 基于可观测度分析的火星进入段自主导航方案设计方法 | |
CN104344837B (zh) | 一种基于速度观测的冗余惯导***加速度计***级标定方法 | |
CN109556631B (zh) | 一种基于最小二乘的ins/gnss/偏振/地磁组合导航***对准方法 | |
CN107525503A (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
CN103792561B (zh) | 一种基于gnss通道差分的紧组合降维滤波方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN103245359A (zh) | 一种惯性导航***中惯性传感器固定误差实时标定方法 | |
CN105091907A (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN103674064B (zh) | 捷联惯性导航***的初始标定方法 | |
CN104374405A (zh) | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 | |
Park et al. | MEMS 3D DR/GPS integrated system for land vehicle application robust to GPS outages | |
Dawson et al. | Radar-based multisensor fusion for uninterrupted reliable positioning in GNSS-denied environments |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |