CN105606093B - 基于重力实时补偿的惯性导航方法及装置 - Google Patents
基于重力实时补偿的惯性导航方法及装置 Download PDFInfo
- Publication number
- CN105606093B CN105606093B CN201610067031.8A CN201610067031A CN105606093B CN 105606093 B CN105606093 B CN 105606093B CN 201610067031 A CN201610067031 A CN 201610067031A CN 105606093 B CN105606093 B CN 105606093B
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- msub
- coordinate system
- navigation
- 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.)
- Expired - Fee Related
Links
- 230000005484 gravity Effects 0.000 title claims abstract description 98
- 238000000034 method Methods 0.000 title claims abstract description 23
- 230000001133 acceleration Effects 0.000 claims abstract description 75
- 238000012545 processing Methods 0.000 claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 15
- 238000005070 sampling Methods 0.000 claims description 21
- 239000011159 matrix material Substances 0.000 claims description 20
- 230000009466 transformation Effects 0.000 claims description 8
- 238000012935 Averaging Methods 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification 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/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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明提供一种基于重力实时补偿的惯性导航方法及装置,包括:接收导航请求,所述导航请求中包括运动载体标识;根据所述导航请求,测量获取所述运动载体标识对应的运动载体在惯性参考系统中的加速度;建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;根据所述重力扰动的最优估计值,对所述加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理。本发明提供的基于重力实时补偿的惯性导航方法,提高了惯性导航系统的导航精度。
Description
技术领域
本发明涉及运动载体导航技术领域,尤其涉及一种基于重力实时补偿的惯性导航方法及装置。
背景技术
惯性导航系统(Inertial Navigation System,简称INS)也称作惯性参考系统,是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面,还可以在水下。INS的基本工作原理是以牛顿力学定律为基础,通过测量运动载体在惯性参考系统的加速度,将该加速度对时间进行积分,并将该加速度的积分结果变换到导航坐标系中,就能够得到在导航坐标系中的速度、偏航角和位置等信息,因此,运动载体在惯性参考系统中的加速度的测量,对惯性导航系统起到重要的作用。
现有技术中,采用加速度计测量运动载体的加速度,但是加速度计输出的是比力,比力既包括运动载体相对惯性空间的绝对加速度还包括有害加速度,因此,需要从加速度计输出的比力中补偿掉有害加速度从而获得载体相对惯性空间的绝对加速度,其中,有害加速度包括重力加速度,一般采用重力模型所提供的正常重力加速度对加速度计的输出结果进行重力加速度补偿。
但是,由于真实重力加速度与重力模型所提供的正常重力加速度存在偏差,因此,采用现有技术所获得的运动载体的绝对加速度不精确,从而导致了惯性导航精度下降。
发明内容
本发明提供一种基于重力实时补偿的惯性导航方法及装置,用于解决现有技术中惯性导航精度低下的问题。
本发明第一方面提供一种基于重力实时补偿的惯性导航方法,包括:
接收导航请求,所述导航请求中包括运动载体标识;
根据所述导航请求,测量获取所述运动载体标识对应的运动载体在惯性参考系统中的加速度;
建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;
根据所述重力扰动的最优估计值,对所述加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理。
本发明第二方面提供一种基于重力实时补偿的惯性导航装置,包括:
接收模块,用于接收导航请求,所述导航请求中包括运动载体标识;
获取模块,用于根据所述导航请求,测量获取所述运动载体标识对应的运动载体在惯性参考系统中的加速度;
所述获取模块还用于建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;
导航模块,用于根据所述重力扰动的最优估计值,对所述加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理。
本发明提供一种基于重力实时补偿的惯性导航方法及装置,包括,接收导航请求,导航请求中包括运动载体标识;根据导航请求,测量获取运动载体标识对应的运动载体在惯性参考系统中的加速度,建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;根据所述重力扰动的最优估计值,对所述加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理,这样,可以实时地获取运动载体重力扰动的最优估计值,并对测量获取的运动载体标识对应的运动载体在惯性参考系统中的加速度进行补偿处理,根据补偿后的加速度,进行相应的导航处理,提高了惯性导航系统的导航精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明提供的基于重力实时补偿的惯性导航方法实施例一的流程图;
图2为本发明提供的基于重力实时补偿的惯性导航方法实施例二的流程图;
图3为本发明提供的基于重力实时补偿的惯性导航方法实施例三的流程图;
图4为本发明提供的基于重力实时补偿的惯性导航装置实施例一的结构示意图;
图5为本发明提供的基于重力实时补偿的惯性导航装置实施例二的结构示意图;
图6为本发明提供的基于重力实时补偿的惯性导航装置实施例三的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
图1为本发明提供的基于重力实时补偿的惯性导航方法实施例一的流程图,如图1所示,该方法包括:
步骤101、接收导航请求,导航请求中包括运动载体标识。
在本实施例中,运动载体可以包括但并不限于以下几种:汽车、飞机、潜艇等,其中,每一个运动载体具有唯一的标识。
步骤102、根据导航请求,测量获取运动载体标识对应的运动载体在惯性参考系统中的加速度。
在本实施例中,具体地,采用加速度计测量运动载体在惯性参考系统中的加速度。
步骤103、建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值。
步骤104、根据重力扰动的最优估计值,对加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理。
在本实施例中,重力扰动是指空间中同一点实际重力加速度与正常重力加速度之差,这样根据重力模型所提供的正常重力加速度和重力扰动可以获取运动载体的实际重力加速度,并根据运动载体的实际重力加速度对测量获取的运动载体标识对应的运动载体在惯性参考系统中的加速度进行补偿处理。
在本实施例中,接收导航请求,导航请求中包括运动载体标识;根据导航请求,测量获取运动载体标识对应的运动载体在惯性参考系统中的加速度,建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;根据重力扰动的最优估计值,对加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理,这样,可以实时地获取运动载体重力扰动的最优估计值,并对测量获取的运动载体标识对应的运动载体在惯性参考系统中的加速度进行补偿处理,根据补偿后的加速度,进行相应的导航处理,提高了惯性导航系统的导航精度。
图2为本发明提供的基于重力实时补偿的惯性导航方法实施例二的流程图,如图2所示,该方法包括:
步骤201、接收导航请求,导航请求中包括运动载体标识。
在本实施例中,运动载体可以包括但并不限于以下几种:汽车、飞机、潜艇等,其中,每一个运动载体具有唯一的标识。
步骤202、根据导航请求,测量获取运动载体标识对应的运动载体在惯性参考系统中的加速度。
在本实施例中,具体地,采用加速度计测量运动载体在惯性参考系统中的加速度。
步骤203、采用公式(1)和(2)
z=Hx+v (2)
计算获取重力扰动的初始最优估计值。
其中,
w=[wgx,wgy,wgz,wax,way,waz,01×9]T,
H=[O3×6 I3×3 O3×6],v=[vL,vλ,vh]T,
x为状态向量,为姿态角,δvnT为导航坐标系下速度误差,为位置误差,εbT为载体坐标系下陀螺漂移,δgnT为导航坐标系下的重力扰动,为导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下投影的叉乘,为比力在导航坐标系下投影的叉乘,为载体坐标系到导航坐标系的姿态变换矩阵,为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影,为导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影,(vn×)为速度在导航坐标系下投影的叉乘,RM为沿子午圈的曲率半径,RN为沿卯酉圈的曲率半径,w为系统噪声阵。wgx,wgy,wgz为陀螺噪声,wax,way,waz为加速度计噪声,o3×6为零矩阵,I3×3为单位矩阵,vL,vλ,vh为量测信息的噪声,z为观测量,为导航坐标系下惯导的速度误差,为导航坐标系下GPS的速度误差,h为载体离大地水准面的高度,L为载体所在位置的纬度。
在本实施例中,根据公式(1)和(2)采用迭代算法计算获取重力扰动的最优估计值。
步骤204、根据预设的时间采样点,将相邻两个采样点之间的重力扰动的初始最优估计值进行求平均处理,获取各个采样点的重力扰动的最优估计值。
在本实施例中,将相邻两个采样点之间的重力扰动的初始最优估计值进行求平均处理,获取的是这两个相邻采样点中靠前的采样点对应的重力扰动的最优估计值。
步骤205、根据重力扰动的最优估计值,对加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理。
在本实施例中,采用公式(1)和(2)计算获取重力扰动的初始最优估计值,并根据预设的时间采样点,将相邻两个采样点之间的重力扰动的初始最优估计值进行求平均处理,获取各个采样点的重力扰动的最优估计值,这样可以实时地获取重力扰动的最优估计值,并提高了重力扰动的最优估计值的准确度。
图3为本发明提供的基于重力实时补偿的惯性导航方法实施例三的流程图,如图3所示,该方法包括:
步骤301、接收导航请求,导航请求中包括运动载体标识。
在本实施例中,运动载体可以包括但并不限于以下几种:汽车、飞机、潜艇等,其中,每一个运动载体具有唯一的标识。
步骤302、根据导航请求,测量获取运动载体标识对应的运动载体在惯性参考系统中的加速度。
在本实施例中,具体地,采用加速度计测量运动载体在惯性参考系统中的加速度。
步骤303、采用公式(1)和(2)
z=Hx+v (2)
计算获取重力扰动的初始最优估计值。
其中,
w=[wgx,wgy,wgz,wax,way,waz,01×9]T,
H=[O3×6 I3×3 O3×6],v=[vL,vλ,vh]T,
x为状态向量,为姿态角,δvnT为导航坐标系下速度误差,为位置误差,εbT为载体坐标系下陀螺漂移,δgnT为导航坐标系下的重力扰动,为导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下投影的叉乘,为比力在导航坐标系下投影的叉乘,为载体坐标系到导航坐标系的姿态变换矩阵,为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影,为导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影,(vn×)为速度在导航坐标系下投影的叉乘,RM为沿子午圈的曲率半径,RN为沿卯酉圈的曲率半径,w为系统噪声阵。wgx,wgy,wgz为陀螺噪声,wax,way,waz为加速度计噪声,o3×6为零矩阵,I3×3为单位矩阵,vL,vλ,vh为量测信息的噪声,z为观测量,为导航坐标系下惯导的速度误差,为导航坐标系下GPS的速度误差,h为载体离大地水准面的高度,L为载体所在位置的纬度。
步骤304、根据预设的时间采样点,将相邻两个采样点之间的重力扰动的初始最优估计值进行求平均处理,获取各个采样点的重力扰动的最优估计值。
步骤305、根据重力扰动的最优估计值,对加速度进行补偿处理。
步骤306、将补偿后的加速度对时间进行积分。
步骤307、将补偿后的加速度的积分结果变换到导航坐标系中,以获取在导航坐标系中的速度以及偏航角和位置。
步骤308、根据导航坐标系中的速度、偏航角和位置,进行导航处理。
在本实施例中,根据重力扰动的最优估计值,对加速度进行补偿处理,将补偿后的加速度对时间进行积分,并将补偿后的加速度的积分结果变换到导航坐标系中,以获取在导航坐标系中的速度以及偏航角和位置,并根据导航坐标系中的速度、偏航角和位置等信息,进行导航处理,从而提高了惯性导航系统的导航精度。
图4为本发明提供的基于重力实时补偿的惯性导航装置实施例一的结构示意图,如图4所示,该装置包括:接收模块10、获取模块11和导航模块12。
接收模块10,用于接收导航请求,导航请求中包括运动载体标识。
获取模块11,用于根据导航请求,测量获取运动载体标识对应的运动载体在惯性参考系统中的加速度。
获取模块11还用于建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值。
导航模块12,用于根据重力扰动的最优估计值,对加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理。
本实施例中的基于重力实时补偿的惯性导航装置可以执行图1所示方法实施例的技术方案,其实现原理和有益效果相类似,此处不再赘述。
图5为本发明提供的基于重力实时补偿的惯性导航装置实施例二的结构示意图,如图5所示,在图4所示实施例的基础上,该装置的获取模块11包括:
计算单元20,用于采用公式(1)和(2)
z=Hx+v (2)
计算获取重力扰动的初始最优估计值。
其中,
w=[wgx,wgy,wgz,wax,way,waz,01×9]T,
H=[O3×6 I3×3 O3×6],v=[vL,vλ,vh]T,
x为状态向量,为姿态角,δvnT为导航坐标系下速度误差,为位置误差,εbT为载体坐标系下陀螺漂移,δgnT为导航坐标系下的重力扰动,为导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下投影的叉乘,为比力在导航坐标系下投影的叉乘,为载体坐标系到导航坐标系的姿态变换矩阵,为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影,为导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影,(vn×)为速度在导航坐标系下投影的叉乘,RM为沿子午圈的曲率半径,RN为沿卯酉圈的曲率半径,w为系统噪声阵。wgx,wgy,wgz为陀螺噪声,wax,way,waz为加速度计噪声,o3×6为零矩阵,I3×3为单位矩阵,vL,vλ,vh为量测信息的噪声,z为观测量,为导航坐标系下惯导的速度误差,为导航坐标系下GPS的速度误差,h为载体离大地水准面的高度,L为载体所在位置的纬度。
计算单元20还用于根据预设的时间采样点,将相邻两个采样点之间的重力扰动的初始最优估计值进行求平均处理,获取各个采样点的重力扰动的最优估计值。
本实施例中的基于重力实时补偿的惯性导航装置可以执行图2所示方法实施例的技术方案,其实现原理和有益效果相类似,此处不再赘述。
图6为本发明提供的基于重力实时补偿的惯性导航装置实施例三的结构示意图,如图6所示,在图4所述实施例的基础上,导航模块12包括:补偿单元30、积分单元31、坐标变换单元31和导航处理单元33。
补偿单元30,用于根据重力扰动的最优估计值,对加速度进行补偿处理。
积分单元31,用于将补偿后的加速度对时间进行积分。
坐标变换单元32,用于将补偿后的加速度的积分结果变换到导航坐标系中,以获取在导航坐标系中的速度以及偏航角和位置。
导航处理单元33,用于根据导航坐标系中的速度、偏航角和位置,进行导航处理。
本实施例中的基于重力实时补偿的惯性导航装置可以执行图3所示方法实施例的技术方案,其实现原理和有益效果相类似,此处不再赘述。
最后应说明的是:以上各实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (4)
1.一种基于重力实时补偿的惯性导航方法,其特征在于,包括:
接收导航请求,所述导航请求中包括运动载体标识;
根据所述导航请求,测量获取所述运动载体标识对应的运动载体在惯性参考系统中的加速度;
建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;
根据所述重力扰动的最优估计值,对所述加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理;
建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值,包括:
采用公式和z=Hx+v计算获取重力扰动的初始最优估计值δgnT;
根据预设的时间采样点,将相邻两个采样点之间的所述重力扰动的初始最优估计值进行求平均处理,获取各个采样点的重力扰动的最优估计值;
其中,
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>S</mi>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>(</mo>
<msubsup>
<mi>f</mi>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>6</mn>
<mo>&times;</mo>
<mn>15</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mrow>
<msub>
<mi>R</mi>
<mi>M</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
</mrow>
</mfrac>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<msub>
<mi>R</mi>
<mi>N</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mrow>
<mi>tan</mi>
<mi> </mi>
<mi>L</mi>
</mrow>
<mrow>
<msub>
<mi>R</mi>
<mi>N</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<msup>
<mi>v</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mo>(</mo>
<mrow>
<mn>2</mn>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
w=[wgx,wgy,wgz,wax,way,waz,01×9]T,
H=[O3×6 I3×3 O3×6],v=[vL,vλ,vh]T,
x为状态向量,为姿态角,δvnT为导航坐标系下速度误差,为位置误差,εbT为载体坐标系下陀螺漂移,δgnT为导航坐标系下的重力扰动,为导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下投影的叉乘,为比力在导航坐标系下投影的叉乘,为载体坐标系到导航坐标系的姿态变换矩阵,为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影,为导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影,(vn×)为速度在导航坐标系下投影的叉乘,RM为沿子午圈的曲率半径,RN为沿卯酉圈的曲率半径,w为系统噪声阵,wgx,wgy,wgz为陀螺噪声,wax,way,waz为加速度计噪声,o3×6为零矩阵,I3×3为单位矩阵,vL,vλ,vh为量测信息的噪声,z为观测量,为导航坐标系下惯导的速度误差,为导航坐标系下GPS的速度误差,h为载体离大地水准面的高度,L为载体所在位置的纬度。
2.根据权利要求1所述的方法,其特征在于,所述根据补偿后的加速度,进行相应的导航处理包括:
将所述补偿后的加速度对时间进行积分;
将所述补偿后的加速度的积分结果变换到导航坐标系中,以获取在所述导航坐标系中的速度以及偏航角和位置;
根据所述导航坐标系中的速度、偏航角和位置,进行导航处理。
3.一种基于重力实时补偿的惯性导航装置,其特征在于,包括:
接收模块,用于接收导航请求,所述导航请求中包括运动载体标识;
获取模块,用于根据所述导航请求,测量获取所述运动载体标识对应的运动载体在惯性参考系统中的加速度;
所述获取模块还用于建立重力扰动的卡尔曼状态方程和量测方程,并获取重力扰动的最优估计值;
导航模块,用于根据所述重力扰动的最优估计值,对所述加速度进行补偿处理,并根据补偿后的加速度,进行相应的导航处理;
计算单元,用于采用公式和z=Hx+v计算获取重力扰动的初始最优估计值;
计算单元还用于根据预设的时间采样点,将相邻两个采样点之间的所述重力扰动的初始最优估计值进行求平均处理,获取各个采样点的重力扰动的最优估计值;
其中,
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>S</mi>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>(</mo>
<msubsup>
<mi>f</mi>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>6</mn>
<mo>&times;</mo>
<mn>15</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mrow>
<msub>
<mi>R</mi>
<mi>M</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
</mrow>
</mfrac>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<msub>
<mi>R</mi>
<mi>N</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mrow>
<mi>tan</mi>
<mi> </mi>
<mi>L</mi>
</mrow>
<mrow>
<msub>
<mi>R</mi>
<mi>N</mi>
</msub>
<mo>+</mo>
<mi>h</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<msup>
<mi>v</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<msub>
<mi>M</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mo>(</mo>
<mrow>
<mn>2</mn>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
w=[wgx,wgy,wgz,wax,way,waz,01×9]T,
H=[O3×6 I3×3 O3×6],v=[vL,vλ,vh]T,
x为状态向量,为姿态角,δvnT为导航坐标系下速度误差,为位置误差,εbT为载体坐标系下陀螺漂移,δgnT为导航坐标系下的重力扰动,为导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下投影的叉乘,为比力在导航坐标系下投影的叉乘,为载体坐标系到导航坐标系的姿态变换矩阵,为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影,为导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影,(vn×)为速度在导航坐标系下投影的叉乘,RM为沿子午圈的曲率半径,RN为沿卯酉圈的曲率半径,w为系统噪声阵,wgx,wgy,wgz为陀螺噪声,wax,way,waz为加速度计噪声,o3×6为零矩阵,I3×3为单位矩阵,vL,vλ,vh为量测信息的噪声,z是观测量,为导航坐标系下惯导的速度误差,为导航坐标系下GPS的速度误差,h为载体离大地水准面的高度,L为载体所在位置的纬度。
4.根据权利要求3所述的装置,其特征在于,所述导航模块包括:
补偿单元,用于根据所述重力扰动的最优估计值,对所述加速度进行补偿处理;
积分单元,用于将所述补偿后的加速度对时间进行积分;
坐标变换单元,用于将所述补偿后的加速度的积分结果变换到导航坐标系中,以获取在所述导航坐标系中的速度以及偏航角和位置;
导航处理单元,用于根据所述导航坐标系中的速度、偏航角和位置,进行导航处理。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610067031.8A CN105606093B (zh) | 2016-01-29 | 2016-01-29 | 基于重力实时补偿的惯性导航方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610067031.8A CN105606093B (zh) | 2016-01-29 | 2016-01-29 | 基于重力实时补偿的惯性导航方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105606093A CN105606093A (zh) | 2016-05-25 |
CN105606093B true CN105606093B (zh) | 2018-04-03 |
Family
ID=55986225
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610067031.8A Expired - Fee Related CN105606093B (zh) | 2016-01-29 | 2016-01-29 | 基于重力实时补偿的惯性导航方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105606093B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109059915B (zh) * | 2018-09-27 | 2020-12-01 | 清华大学 | 重力补偿方法、系统和装置 |
CN109470241B (zh) * | 2018-11-23 | 2020-11-10 | 中国船舶重工集团公司第七0七研究所 | 一种具备重力扰动自主补偿功能的惯性导航系统及方法 |
CN111912425A (zh) * | 2020-08-09 | 2020-11-10 | 长沙智来测控科技有限公司 | 管线测量系统与方法 |
CN112595314A (zh) * | 2020-12-11 | 2021-04-02 | 北京大学 | 一种可实时测量重力加速度的惯性导航系统 |
CN113203415B (zh) * | 2021-04-12 | 2022-05-17 | 北京航空航天大学 | 一种原子陀螺导航系统及其导航解算方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102052922A (zh) * | 2010-11-19 | 2011-05-11 | 中国人民解放军海军工程大学 | 实际重力场影响惯性导航系统扰动重力补偿方法 |
CN103364842A (zh) * | 2013-07-29 | 2013-10-23 | 中国人民解放军国防科学技术大学 | 一种捷联式航空重力仪误差分离方法 |
CN104359496A (zh) * | 2014-11-26 | 2015-02-18 | 中国人民解放军国防科学技术大学 | 基于垂线偏差补偿的高精度姿态修正方法 |
CN104374401A (zh) * | 2014-10-15 | 2015-02-25 | 哈尔滨工程大学 | 一种捷联惯导初始对准中重力扰动的补偿方法 |
-
2016
- 2016-01-29 CN CN201610067031.8A patent/CN105606093B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102052922A (zh) * | 2010-11-19 | 2011-05-11 | 中国人民解放军海军工程大学 | 实际重力场影响惯性导航系统扰动重力补偿方法 |
CN103364842A (zh) * | 2013-07-29 | 2013-10-23 | 中国人民解放军国防科学技术大学 | 一种捷联式航空重力仪误差分离方法 |
CN104374401A (zh) * | 2014-10-15 | 2015-02-25 | 哈尔滨工程大学 | 一种捷联惯导初始对准中重力扰动的补偿方法 |
CN104359496A (zh) * | 2014-11-26 | 2015-02-18 | 中国人民解放军国防科学技术大学 | 基于垂线偏差补偿的高精度姿态修正方法 |
Non-Patent Citations (1)
Title |
---|
基于Kalman滤波原理对惯导中重力扰动的估计及补偿方法;周潇等;《中国惯性技术学报》;20151231;第23卷(第6期);摘要、第1-2节 * |
Also Published As
Publication number | Publication date |
---|---|
CN105606093A (zh) | 2016-05-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
CN103235328B (zh) | 一种gnss与mems组合导航的方法 | |
CN105606093B (zh) | 基于重力实时补偿的惯性导航方法及装置 | |
JP4989035B2 (ja) | 慣性ナビゲーションシステムの誤差補正 | |
EP2502026B1 (en) | Spatial alignment determination for an inertial measurement unit (imu) | |
CN102486377B (zh) | 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN102538792B (zh) | 一种位置姿态系统的滤波方法 | |
JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
CN101187567A (zh) | 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法 | |
JP2010032398A (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
CN104880189B (zh) | 一种动中通天线低成本跟踪抗干扰方法 | |
CN105698822A (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN104697526A (zh) | 用于农业机械的捷联惯导系统以及控制方法 | |
CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及系统 | |
CN105091907A (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN109612460B (zh) | 一种基于静止修正的垂线偏差测量方法 | |
CN102116634A (zh) | 一种着陆深空天体探测器的降维自主导航方法 | |
CN103604428A (zh) | 基于高精度水平基准的星敏感器定位方法 | |
CN109470276B (zh) | 基于零速修正的里程计标定方法与装置 | |
CN103278165A (zh) | 基于剩磁标定的磁测及星光备份的自主导航方法 | |
CN113340298A (zh) | 一种惯导和双天线gnss外参标定方法 | |
CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
CN110133695A (zh) | 一种双天线gnss位置延迟时间动态估计系统及方法 |
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 | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20180403 Termination date: 20190129 |