地磁辅助的惯性导航与激光测速仪组合导航方法

1.本发明涉及导航技术领域，具体是一种地磁辅助的惯性导航与激光测速仪组合导航方法。


背景技术：

2.导弹发射车是发射导弹的军用武器，如何能够快速精确地测定发射点位的地理坐标是十分重要的问题,因此对定位定向技术的精度有很高的要求。传统的车载导弹利用的是有依托发射技术，在预设阵地发射,机动性和灵活性比较差，无法发挥车载优势。而无依托技术能够缩短发射的准备时间,提高导弹快速发射的能力,增强导弹的战场生存能力。当前的技术主要以惯性导航为主，结合其他定位技术进行定位，例如，惯性导航/卫星导航组合，如基于北斗、uwb及惯导的高精度列车定位终端(申请号：cn201922479723.5)就公开过类似方法，但该种方法中卫星信号容易受到外界干扰，不能保证定位信息的可靠性。


技术实现要素：

3.针对上述现有技术中的不足，本发明提供一种地磁辅助的惯性导航与激光测速仪组合导航方法，通过地磁匹配辅助ins/ldv组合进行导航定位，进一步提高载体的定位精度，实现高精度、高可靠的载体定位。
4.为实现上述目的，本发明提供一种地磁辅助的惯性导航与激光测速仪组合导航方法，包括如下步骤：
5.步骤1，基于ins/ldv组合获取载体在一段时间内的指示轨迹点的信息，并经过卡尔曼滤波导航解算，得到载体在各所述指示轨迹点的参考定位信息；
6.步骤2，基于地磁传感器在载体运行的过程中实时测量各所述指示轨迹点的实测地磁值，形成用以地磁匹配的实测地磁序列；
7.步骤3，根据所述参考定位信息匹配地磁基准图，提取所述地磁基准图中各所述参考定位信息对应点组成的地磁等值线；
8.步骤4，基于所述实测地磁序列，在所述地磁等值线中搜索每个指示轨迹点的最邻近参考点，得到与各所述指示轨迹点对应的匹配地磁序列，并基于所述地磁基准图将所述匹配地磁序列转换为各所述指示轨迹点的匹配定位信息；
9.步骤5，判断各所述指示轨迹点的匹配定位信息是否满足精度评估要求，并在满足精度评估要求时将各所述指示轨迹点的匹配定位信息返回给ins/ldv组合，通过滤波修正ins/ldv的累积误差后，重复进行步骤1至步骤5。
10.与现有技术相比，本发明具有如下有益技术效果：
11.本发明提出的一种地磁辅助的惯性导航与激光测速仪组合导航方法，在惯性导航系统/激光测速仪组合导航系统的基础上，将地磁传感器引入组合系统，构成以地磁辅助的惯性导航系统/激光测速仪组合导航系统，进一步提高列车定位精度和可靠性，实现高精度、高可靠的载体定位。
附图说明
12.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
13.图1为本发明实施例中地磁辅助的惯性导航与激光测速仪组合导航方法的流程图。
14.本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
15.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
16.另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
17.本实施例公开了一种地磁辅助的惯性导航与激光测速仪组合导航方法，在惯性导航系统/激光测速仪组成的ins/ldv组合导航系统的基础上，将地磁传感器引入组合系统，构成以地磁辅助的惯性导航系统/激光测速仪组合导航系统，进一步提高列车定位精度和可靠性，实现高精度、高可靠的载体定位。
18.参考图1，本实施例中的地磁辅助的惯性导航与激光测速仪组合导航方法具体包括如下步骤1至步骤5。
19.步骤1，基于ins/ldv组合获取载体在一段时间内的指示轨迹点的信息，并经过卡尔曼滤波导航解算，得到载体在各指示轨迹点的参考定位信息。具体过程为：将导航坐标系下ins输出的速度与误差模型补偿后的ldv速度作差，并把速度差作为卡尔曼滤波的观测量，再通过卡尔曼滤波器输出载体的姿态、速度、位置的误差状态量并进行修正，从而得到各指示轨迹点的参考定位信息。
20.在具体实施过程中，ins/ldv组合确定参考定位信息的过程为：
21.步骤1.1，ins在导航中可以得到载体的姿态、位置、速度信息。
22.在,ins中，姿态更新是指通过传感器的输出实时解算方向余弦矩阵姿态矩阵微分形式为：
[0023][0024][0025][0026]
式中，为b系(载体坐标系)相对于n系(导航坐标系)的旋转角速度的斜对称
矩阵形式，为陀螺输出在b系相对于i系(惯性坐标系)的旋转角速度，rm为地球的子午圈半径，rn为地球的卯酉圈半径，t为矩阵的装置；
[0027]
ins取n系为导航坐标系，速度方程的微分形式为：
[0028][0029]
式中，为载体行驶速度在n系中的投影，fb表示加表测量得到的在b系中的比力值，gn表示载体的重力加速度；
[0030]
载体的位置更新方程为：
[0031][0032]
式中，λ为载体的经度，l为载体的纬度，h为载体的海拔高度；
[0033]
根据陀螺仪输出的角速度信息和惯性导航系统解算得到的速度信息和位置信息，可以实时更新姿态余弦矩阵从而解算输出载体的姿态信息；
[0034]
步骤1.2，抑制ins定位误；
[0035]
在实际情况中，由于惯导系统器件误差、初始值误差与计算误差等因素的存在，系统输出的导航信息将不可避免地存在误差。为了抑制ins的定位误差发散，推导误差方程和建立误差模型如下：
[0036]
载体的姿态可以用姿态转换矩阵表示，因姿态角为小量，所以实际估计值与真实值之间的关系可表示为：
[0037][0038]
ins的姿态误差可以表示为：
[0039][0040]
实际的速度计算值为：
[0041][0042]
式中，vc＝vn+δvn，gc＝gn+δgn，，
[0043]
其中：
[0044]
δka＝diag[δk
ax δk
ay δk
az
]
[0045][0046][0047]
位置误差为：
[0048][0049][0050]
步骤1.3，,通过ldv测量载体运行中的实时速度；
[0051]
ldv测速公式为：
[0052][0053]
式中，v是光源的移动速度，即载体的线速度；λ是激光波长；c为光速；θ是速度方向与探测光束方向的夹角；fd为探测器接收散射光的频率和光源发射激光的频率的频率差值。
[0054]
定义m系与b系的转换矩阵为则在n系下的真实速度为：
[0055]
定义测速仪刻度系数误差为δk，一般测速仪刻度系数k等于1，测速仪输出速度为：
[0056]
步骤1.4，计算ldv测速误差；
[0057]
定义安装误差角为η＝[α
θ α
γ α
ψ
]
t
，其中，α
θ
为俯仰安装误差角，α
γ
为横滚安装误差角，α
ψ
为航向安装误差角。一般情况下安装比较精确，所以将安装误差角看做小量，则由m系至b系的转换矩阵可表示为：
[0058]
测速仪输出速度在n系下的投影为：
[0059]
进一步地，
[0060]
进一步地，测速仪在n系下的测速误差为：进一步地，测速仪在n系下的测速误差为：
[0061]
步骤1.5，对ins/ldv组合进行卡尔曼滤波，得到载体参考定位信息；
[0062]
ins/ldv组合系统中卡尔曼滤波器的状态变量由ins的状态量构成。ins系统状态量选取如下：东向、北向、天向速度误δve、δvn、δvu，横滚角误差、俯仰角误差、航向角误差δφ、δθ、δφ，北向、东向、天向位置误差δλ、δl、δh。卡尔曼滤波器的状态量为：x＝[δv
e δv
n δv
u δφ δθ δψ δλ δl δh]
t
[0063]
根据ins的误差方程，可以得到：
[0064]
系统状态方程：
[0065][0066]
量测方程：
[0067][0068]
进一步地，
[0069][0070]
取上式中的速度差为量测量，可得卡尔曼滤波器量测方程为：
[0071][0072]
式中，h为量测矩阵，v(t)为量测噪声矢量。
[0073]
步骤2，基于地磁传感器在载体运行的过程中实时测量各指示轨迹点的实测地磁值，形成用以地磁匹配的实测地磁序列。
[0074]
步骤3，根据参考定位信息匹配地磁基准图，提取地磁基准图中各参考定位信息对应点组成的地磁等值线。
[0075]
步骤4，基于实测地磁序列，在地磁等值线中搜索每个指示轨迹点的最邻近参考点，得到与各指示轨迹点对应的匹配地磁序列，并基于地磁基准图将匹配地磁序列转换为各指示轨迹点的匹配定位信息。
[0076]
在具体实施过程中，在地磁等值线中搜索每个指示轨迹点的最邻近参考点，具体为：
[0077]
步骤4.1，记各指示轨迹点的实测地磁值在地磁基准图中的点位为p1、p2、
···
、pn，并定义集合p＝{pi:i＝1,2,
···
,n}，其中，n为指示轨迹点的数量；
[0078]
步骤4.2，记点位p1、p2、
···
、pn在地磁等值线上的最邻近参考点分别为y1、y2、
···
、yn，并定义集合y＝{yi:i＝1,2,
···
,n}；
[0079]
步骤4.3，求解刚性变换t，使集合p与集合y的欧氏距离最小，即步骤4.3，求解刚性变换t，使集合p与集合y的欧氏距离最小，即
[0080]
步骤4.4，令p＝tp后迭代步骤4.2至步骤4.4直至刚性变换t收敛，并输出刚性变换t收敛后的集合p，输出的集合p中的点p1、p2、
···
、pn即为与各指示轨迹点对应的匹配地磁序列，其中，刚性变换t收敛的条件为：前后两次迭代间刚性变换t的差值小于门限值ε。
[0081]
步骤5，判断各指示轨迹点的匹配定位信息是否满足精度评估要求，并在满足精度评估要求时将各指示轨迹点的匹配定位信息返回给ins/ldv组合，通过滤波修正ins/ldv的累积误差后，重复进行步骤1至步骤5。
[0082]
本实施例中，判断各指示轨迹点的匹配定位信息是否满足精度评估要求的具体过程为：
[0083]
记各指示轨迹点的参考定位信息的对应点分别为a1、a2、
···
、an，其中，n为指示轨迹点的数量；
[0084]
记步骤4中输出的各指示轨迹点的匹配定位信息的对应点分别为b1、b2、
···
、bn；
[0085]
令di为点ai与点bi间的距离，其中，i＝1,2,
···
,n，为距离平均值；
[0086]
判断时候成立，若是则判定当前的匹配定位信息满足精度评估要求，否则判定当前的匹配定位信息不满足精度评估要求，其中，σ为精度阈值。
[0087]
作为有选地实施方式，当各指示轨迹点的匹配定位信息不满足精度评估要求时的
具体操作过程为：
[0088]
步骤5.1，令外循环次数j＝1；
[0089]
步骤5.3，判断j《j
max
是否成立，其中，j
max
为最大外循环次数：
[0090]
若是，则令j＝j+1，并更新门限值为ε＝x
·
ε后再次进行步骤4.2至步骤4.4以及步骤5，其中，x为梯度系数；
[0091]
否则，直接重复进行步骤1至步骤5。
[0092]
本实施例中，梯度系数取值x＝0.5～0.9。
[0093]
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

技术特征：
1.一种地磁辅助的惯性导航与激光测速仪组合导航方法，其特征在于，包括如下步骤：步骤1，基于ins/ldv组合获取载体在一段时间内的指示轨迹点的信息，并经过卡尔曼滤波导航解算，得到载体在各所述指示轨迹点的参考定位信息；步骤2，基于地磁传感器在载体运行的过程中实时测量各所述指示轨迹点的实测地磁值，形成用以地磁匹配的实测地磁序列；步骤3，根据所述参考定位信息匹配地磁基准图，提取所述地磁基准图中各所述参考定位信息对应点组成的地磁等值线；步骤4，基于所述实测地磁序列，在所述地磁等值线中搜索每个指示轨迹点的最邻近参考点，得到与各所述指示轨迹点对应的匹配地磁序列，并基于所述地磁基准图将所述匹配地磁序列转换为各所述指示轨迹点的匹配定位信息；步骤5，判断各所述指示轨迹点的匹配定位信息是否满足精度评估要求，并在满足精度评估要求时将各所述指示轨迹点的匹配定位信息返回给ins/ldv组合，通过滤波修正ins/ldv的累积误差后，重复进行步骤1至步骤5。2.根据权利要求1所述的地磁辅助的惯性导航与激光测速仪组合导航方法，其特征在于，步骤4中，所述在所述地磁等值线中搜索每个指示轨迹点的最邻近参考点，具体为：步骤4.1，记各所述指示轨迹点的实测地磁值在所述地磁基准图中的点位为p1、p2、
···
、p
n
，并定义集合p＝{p
i
:i＝1,2,
···
,n}，其中，n为所述指示轨迹点的数量；步骤4.2，记点位p1、p2、
···
、p
n
在地磁等值线上的最邻近参考点分别为y1、y2、
···
、y
n
，并定义集合y＝{y
i
:i＝1,2,
···
,n}；步骤4.3，求解刚性变换t，使集合p与集合y的欧氏距离最小，即步骤4.3，求解刚性变换t，使集合p与集合y的欧氏距离最小，即步骤4.4，令p＝tp后迭代步骤4.2至步骤4.4直至刚性变换t收敛，并输出刚性变换t收敛后的集合p，输出的集合p中的点p1、p2、
···
、p
n
即为与各所述指示轨迹点对应的匹配地磁序列。3.根据权利要求2所述的地磁辅助的惯性导航与激光测速仪组合导航方法，其特征在于，步骤4.4中，所述刚性变换t收敛的条件为：前后两次迭代间刚性变换t的差值小于门限值ε。4.根据权利要求3所述的地磁辅助的惯性导航与激光测速仪组合导航方法，其特征在于，步骤5中，各所述指示轨迹点的匹配定位信息不满足精度评估要求时的具体操作过程为：步骤5.1，令外循环次数j＝1；步骤5.3，判断j<j
max
是否成立，其中，j
max
为最大外循环次数：若是，则令j＝j+1，并更新门限值为ε＝x
·
ε后再次进行步骤4.2至步骤4.4以及步骤5，其中，x为梯度系数；否则，直接重复进行步骤1至步骤5。5.根据权利要求4所述的地磁辅助的惯性导航与激光测速仪组合导航方法，其特征在于，所述梯度系数x＝0.5～0.9。6.根据权利要求1至5任一项所述的地磁辅助的惯性导航与激光测速仪组合导航方法，
其特征在于，步骤5中，判断各所述指示轨迹点的匹配定位信息是否满足精度评估要求的具体过程为：记各所述指示轨迹点的参考定位信息的对应点分别为a1、a2、
···
、a
n
，其中，n为所述指示轨迹点的数量；记步骤4中输出的各所述指示轨迹点的匹配定位信息的对应点分别为b1、b2、
···
、b
n
；令d
i
为点a
i
与点b
i
间的距离，其中，i＝1,2,
···
,n，为距离平均值；判断时候成立，若是则判定当前的匹配定位信息满足精度评估要求，否则判定当前的匹配定位信息不满足精度评估要求，其中，σ为精度阈值。7.根据权利要求1至5任一项所述的地磁辅助的惯性导航与激光测速仪组合导航方法，其特征在于，步骤1中，所述参考定位信息的确定过程为：将导航坐标系下ins输出的速度与误差模型补偿后的ldv速度作差，并把速度差作为卡尔曼滤波的观测量，再通过卡尔曼滤波器输出载体的姿态、速度、位置的误差状态量并进行修正，从而得到各所述指示轨迹点的参考定位信息。

技术总结
本发明公开了一种地磁辅助的惯性导航与激光测速仪组合导航方法，包括如下步骤：基于INS/LDV组合获取载体在指示轨迹点的参考定位信息；基于地磁传感器测量各指示轨迹点的实测地磁值；提取地磁基准图中各参考定位信息对应点组成的地磁等值线；在地磁等值线中搜索每个指示轨迹点的最邻近参考点，得到各指示轨迹点的匹配定位信息；判断各指示轨迹点的匹配定位信息是否满足精度评估要求，并在满足精度评估要求时将各指示轨迹点的匹配定位信息返回给INS/LDV组合，通过滤波修正INS/LDV的累积误差后。本发明应用于导航领域，通过地磁匹配辅助INS/LDV组合进行导航定位，进一步提高载体的定位精度，实现高精度、高可靠的载体定位。高可靠的载体定位。高可靠的载体定位。


技术研发人员：周健 张亚洁 段成芳 王琦 聂晓明 黄荣 陈兰剑 席崇宾
受保护的技术使用者：中国人民解放军国防科技大学
技术研发日：2023.03.07
技术公布日：2023/7/12