CN114111768A - 一种基于偏角异常补偿的轨道线型拟合方法 - Google Patents

Abstract

本发明提供了一种基于偏角异常补偿的轨道线型拟合方法，包括：将里程计和惯导系统固联在轨检车上，将轨检车设置在轨道上行进；利用里程计实时获取轨检车的里程值；利用惯导系统实时获取轨检车的位置坐标和航向角；获取轨检车每一个里程点的位置坐标，获取轨道实测线型；获取轨道实测线型的曲率变化；基于曲率变化将轨道实测线型分为直线段、圆曲线段和缓和曲线段；利用直线方程、圆曲线方程和缓和曲线方程对直线段、圆曲线段和缓和曲线段进行初步拟合，得到初步拟合的轨道线型和偏角异常点的位置坐标；根据偏角异常点对初步拟合的轨道线型进行补偿，从而得到补偿后的轨道线型。本发明能够解决现有线型拟合方法的拟合精度较差的技术问题。

Description

一种基于偏角异常补偿的轨道线型拟合方法

技术领域

本发明涉及轨道检测技术领域，尤其涉及一种基于偏角异常补偿的轨道线型拟合方法。

背景技术

随着我国铁路建设的不断发展，既有线路的改建、扩建以及新建项目越来越多，线路测绘在工程建设中起着决定性的作用，它直接影响到工程的质量、成本及工期。为保障轨道交通的运营安全，近年来，轨道精密检测技术得到快速发展，多个国家投入了大量的人力物力来研制和更新各种轨道检测方法，以满足当今铁路高速和重载的要求。在实际线路测量当中，部分场景下是没有设计线型资料的，因此需要根据实测值对设计线型进行拟合，通常采用惯性导航系统加其他辅助传感器对轨道设计线型进行获取。

铁路轨道在设计过程中分成直线、缓和曲线和圆曲线三部分组成，在拐弯过程中通常采用缓和曲线对直线和圆曲线进行连接，因此在曲线拟合过程通常根据惯导测量值和设计线型的曲线方程对轨迹进行处理，然而在进行曲线拟合的过程中，通常采用样条插值或多阶曲线方程等数学拟合方法，失去了实际曲线的物理意义，导致精度损失。

发明内容

本发明提供了一种基于偏角异常补偿的轨道线型拟合方法，能够解决现有线型拟合方法的拟合精度较差的技术问题。

根据本发明的一方面，提供了一种基于偏角异常补偿的轨道线型拟合方法，所述方法包括：

将里程计和惯导系统固联在轨检车上，将轨检车设置在轨道上行进；

利用里程计实时获取轨检车的里程值；

利用惯导系统实时获取轨检车的位置坐标和航向角；

基于里程值和位置坐标获取轨检车每一个里程点的位置坐标，并基于每一个里程点的位置坐标获取轨道实测线型；

基于航向角获取轨道实测线型的曲率变化；

基于曲率变化将轨道实测线型分为直线段、圆曲线段和缓和曲线段；

利用直线方程、圆曲线方程和缓和曲线方程对直线段、圆曲线段和缓和曲线段进行初步拟合，得到初步拟合的轨道线型和偏角异常点的位置坐标，其中，在圆曲线段的拟合过程中，将圆曲线段分为前半圆曲线段和后半圆曲线段，将前半圆曲线段的前端与前面的缓和曲线段拟合，将后半圆曲线段的后端与后面的缓和曲线段拟合，则前半圆曲线段的后端与后半圆曲线段的前端的交点为偏角异常点；

经过偏角异常点分别作前半圆曲线段和后半圆曲线段的切线，得到两条切直线；

设置预设长度阈值，在两条切直线围成的下侧区域作出与两条切直线均相切的内切圆，内切圆与任一条切直线的切点到偏角异常点的距离均等于预设长度阈值，基于预设长度阈值和两条切直线的夹角得到内切圆的半径；

获取第一条切直线上从与前半圆曲线段的切点到偏角异常点的线段上所有点的位置坐标，将所有点记作第一待修正点；

获取第二条切直线上从偏角异常点到与后半圆曲线段的切点的线段上所有点的位置坐标，将所有点记作第二待修正点；

获取第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标，并基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量；

获取第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标，并基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量；

利用第一补偿量对对应的每个第一待修正点进行补偿，得到所有的第一修正点的位置坐标，利用第二补偿量对对应的每个第二待修正点进行补偿，得到所有的第二修正点的位置坐标；

将所有的第一修正点和所有的第二修正点依次相连，得到前半圆曲线段和后半圆曲线段之间的拟合线型，从而得到补偿后的轨道线型。

优选的，利用惯导系统实时获取轨检车的位置坐标和航向角包括：

将里程计和惯导系统作为组合导航系统，构建组合导航系统的状态方程；

构建组合导航系统的观测方程；

基于状态方程和观测方程构建卡尔曼滤波方程，利用卡尔曼滤波方程对组合导航系统误差进行估计；

基于估计的组合导航系统误差对惯导系统进行修正，以实时获取轨检车的位置坐标和航向角。

优选的，通过下式构建组合导航系统的状态方程：

其中，

通过下式构建组合导航系统的状态方程：

Z_k＝H_kX_k+V_k；

其中，Z_k＝ΔV_Ins-ΔV_d；

式中，X(t)为状态向量，F为状态转移矩阵，w(t)为系统噪声，δV_N、δV_E分别为北向速度误差、东向速度误差，

δh、δλ分别为纬度误差、高度误差、经度误差，φ_N、φ_U、φ_E分别为北、天、东三个方向的姿态误差，ε_x、ε_y、ε_z分别为x轴、y轴、z轴陀螺漂移，

分别为x轴、y轴、z轴加速度计零位，Z_k为k时刻的观测量，H_k为观测矩阵，X_k为k时刻的状态向量，V_k为观测噪声，ΔV_Ins为惯导解算速度误差，ΔV_d为里程计速度误差。

优选的，通过下式对组合导航系统误差进行估计：

P_k＝(I-K_kH_k)P_k/k-1；

式中，

为状态一步预测，Φ_k,k-1为状态转移矩阵，

为k-1时刻的状态估计，P_k/k-1为一步预测均方误差阵，P_k-1为k-1时刻的估计均方误差阵，w_k-1为k-1时刻的系统噪声，K_k为滤波增益，

为k时刻的状态估计，P_k为k时刻的估计均方误差阵，I为单位矩阵。

优选的，直线方程如下式所示：

y＝k′x+b′；

圆曲线方程如下式所示：

a＝x₁-x₂

b＝y₁-y₂

c＝x₁-x₃；

d＝y₁-y₃

缓和曲线方程如下式所示：

式中，k′为直线方程的斜率，b′为直线方程的截距，(x₁,y₁)、(x₂,y₂)、(x₃,y₃)分别为圆曲线段上三点的位置坐标，(x₀,y₀)为圆曲线段圆心的位置坐标，l为缓和曲线段的起始位置到待修正点的里程，l_s为缓和曲线段的总里程，R为缓和曲线段上的待修正点的曲率半径。

优选的，基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量包括：

基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标得到第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离；

基于第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量。

优选的，通过下式得到第一补偿量：

式中，dr₁为第一补偿量，l₁为第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离，r为内切圆的半径。

优选的，基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量包括：

基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标得到第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离；

基于第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量。

优选的，通过下式得到第二补偿量：

式中，dr₂为第二补偿量，l₂为第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离，r为内切圆的半径。

根据本发明的又一方面，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述任一所述方法。

应用本发明的技术方案，通过对轨道实测线型进行分段，解析出其物理意义，利用直线方程、圆曲线方程和缓和曲线方程对实测线型进行初步拟合，再根据初步拟合的轨道线型和偏角异常点的位置坐标获取每个待修正点的补偿量，从而得到补偿后的轨道线型，提高了轨道设计线型的准确性，使轨道线型更加平顺。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明的一种实施例提供的基于偏角异常补偿的轨道线型拟合方法的流程图；

图2示出了根据本发明的一种实施例提供的偏角异常补偿示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

如图1和图2所示，本发明提供了一种基于偏角异常补偿的轨道线型拟合方法，所述方法包括：

S10、将里程计和惯导系统固联在轨检车上，将轨检车设置在轨道上行进；

S20、利用里程计实时获取轨检车的里程值；

S30、利用惯导系统实时获取轨检车的位置坐标和航向角；

S40、基于里程值和位置坐标获取轨检车每一个里程点的位置坐标，并基于每一个里程点的位置坐标获取轨道实测线型；

S50、基于航向角获取轨道实测线型的曲率变化；

S60、基于曲率变化将轨道实测线型分为直线段、圆曲线段和缓和曲线段；

S70、利用直线方程、圆曲线方程和缓和曲线方程对直线段、圆曲线段和缓和曲线段进行初步拟合，得到初步拟合的轨道线型和偏角异常点的位置坐标，其中，在圆曲线段的拟合过程中，将圆曲线段分为前半圆曲线段和后半圆曲线段，将前半圆曲线段的前端与前面的缓和曲线段拟合，将后半圆曲线段的后端与后面的缓和曲线段拟合，则前半圆曲线段的后端与后半圆曲线段的前端的交点为偏角异常点；

S80、经过偏角异常点分别作前半圆曲线段和后半圆曲线段的切线，得到两条切直线；

S90、设置预设长度阈值，在两条切直线围成的下侧区域作出与两条切直线均相切的内切圆，内切圆与任一条切直线的切点到偏角异常点的距离均等于预设长度阈值，基于预设长度阈值和两条切直线的夹角得到内切圆的半径；

S100、获取第一条切直线上从与前半圆曲线段的切点到偏角异常点的线段上所有点的位置坐标，将所有点记作第一待修正点；

S110、获取第二条切直线上从偏角异常点到与后半圆曲线段的切点的线段上所有点的位置坐标，将所有点记作第二待修正点；

S120、获取第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标，并基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量；

S130、获取第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标，并基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量；

S140、利用第一补偿量对对应的每个第一待修正点进行补偿，得到所有的第一修正点的位置坐标，利用第二补偿量对对应的每个第二待修正点进行补偿，得到所有的第二修正点的位置坐标；

S150、将所有的第一修正点和所有的第二修正点依次相连，得到前半圆曲线段和后半圆曲线段之间的拟合线型，从而得到补偿后的轨道线型。

本发明通过对轨道实测线型进行分段，解析出其物理意义，利用直线方程、圆曲线方程和缓和曲线方程对实测线型进行初步拟合，再根据初步拟合的轨道线型和偏角异常点的位置坐标获取每个待修正点的补偿量，从而得到补偿后的轨道线型，提高了轨道设计线型的准确性，使轨道线型更加平顺。

根据本发明的一种实施例，在本发明的S30中，利用惯导系统实时获取轨检车的位置坐标和航向角包括：

S31、将里程计和惯导系统作为组合导航系统，构建组合导航系统的状态方程；

S32、构建组合导航系统的观测方程；

S33、基于状态方程和观测方程构建卡尔曼滤波方程，利用卡尔曼滤波方程对组合导航系统误差进行估计；

S34、基于估计的组合导航系统误差对惯导系统进行修正，以实时获取轨检车的位置坐标和航向角。

进一步地，在本发明的S31中，通过下式构建组合导航系统的状态方程：

其中，

进一步地，在本发明的S32中，通过下式构建组合导航系统的状态方程：

Z_k＝H_kX_k+V_k；

其中，Z_k＝ΔV_Ins-ΔV_d；

式中，X(t)为状态向量，F为状态转移矩阵，w(t)为系统噪声，δV_N、δV_E分别为北向速度误差、东向速度误差，

δh、δλ分别为纬度误差、高度误差、经度误差，φ_N、φ_U、φ_E分别为北、天、东三个方向的姿态误差，ε_x、ε_y、ε_z分别为x轴、y轴、z轴陀螺漂移，

分别为x轴、y轴、z轴加速度计零位，Z_k为k时刻的观测量，H_k为观测矩阵，X_k为k时刻的状态向量，V_k为观测噪声，ΔV_Ins为惯导解算速度误差，ΔV_d为里程计速度误差。

进一步地，在本发明的S33中，通过下式对组合导航系统误差进行估计：

P_k＝(I-K_kH_k)P_k/k-1；

式中，

为状态一步预测，Φ_k,k-1为状态转移矩阵，

为k-1时刻的状态估计，P_k/k-1为一步预测均方误差阵，P_k-1为k-1时刻的估计均方误差阵，w_k-1为k-1时刻的系统噪声，K_k为滤波增益，

为k时刻的状态估计，P_k为k时刻的估计均方误差阵，I为单位矩阵。

根据本发明的一种实施例，在本发明的S70中，可以采用点斜式确定直线方程，例如，可根据直线段的多个点的位置坐标，采用最小二乘方法拟合出k′和b′的值，直线方程如下式所示：

y＝k′x+b′；

在本发明的S70中，可以采用三点法确定圆心位置和圆半径，圆曲线方程如下式所示：

a＝x₁-x₂

b＝y₁-y₂

c＝x₁-x₃；

d＝y₁-y₃

在本发明的S70中，可以采用多阶拟合函数的思想确定缓和曲线方程，具体如下式所示：

式中，k′为直线方程的斜率，b′为直线方程的截距，(x₁,y₁)、(x₂,y₂)、(x₃,y₃)分别为圆曲线段上三点的位置坐标，(x₀,y₀)为圆曲线段圆心的位置坐标，l为缓和曲线段的起始位置到待修正点的里程，l_s为缓和曲线段的总里程，R为缓和曲线段上的待修正点的曲率半径。

在本实施例中，在圆曲线段的拟合过程中，为了使缓和曲线段到圆曲线段的连接更加平顺，将圆曲线段分为前半圆曲线段和后半圆曲线段，将前半圆曲线段的前端与前面的缓和曲线段拟合，将后半圆曲线段的后端与后面的缓和曲线段拟合，此时被分成两半的前半圆曲线段和后半圆曲线段之间会产生一个偏角，因此将前半圆曲线段的后端与后半圆曲线段的前端的交点作为偏角异常点，在计算设计线型和拟合线型差值的时候，这个偏角会引起一个突变的误差，因此需要对产生的偏角异常点进行补偿处理，从而解决完整曲线的不平顺问题。

根据本发明的一种实施例，在本发明的S120中，基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量包括：

S121、基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标得到第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离；

S122、基于第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量。

根据本发明的一种实施例，在本发明的S122中，通过下式得到第一补偿量：

式中，dr₁为第一补偿量，l₁为第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离，r为内切圆的半径。

根据本发明的一种实施例，在本发明的S130中，基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量包括：

S131、基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标得到第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离；

S132、基于第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量。

根据本发明的一种实施例，在本发明的S132中，通过下式得到第二补偿量：

式中，dr₂为第二补偿量，l₂为第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离，r为内切圆的半径。

根据本发明的又一方面，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述任一所述方法。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于偏角异常补偿的轨道线型拟合方法，其特征在于，所述方法包括：

将里程计和惯导系统固联在轨检车上，将轨检车设置在轨道上行进；

利用里程计实时获取轨检车的里程值；

利用惯导系统实时获取轨检车的位置坐标和航向角；

基于里程值和位置坐标获取轨检车每一个里程点的位置坐标，并基于每一个里程点的位置坐标获取轨道实测线型；

基于航向角获取轨道实测线型的曲率变化；

基于曲率变化将轨道实测线型分为直线段、圆曲线段和缓和曲线段；

利用直线方程、圆曲线方程和缓和曲线方程对直线段、圆曲线段和缓和曲线段进行初步拟合，得到初步拟合的轨道线型和偏角异常点的位置坐标，其中，在圆曲线段的拟合过程中，将圆曲线段分为前半圆曲线段和后半圆曲线段，将前半圆曲线段的前端与前面的缓和曲线段拟合，将后半圆曲线段的后端与后面的缓和曲线段拟合，则前半圆曲线段的后端与后半圆曲线段的前端的交点为偏角异常点；

经过偏角异常点分别作前半圆曲线段和后半圆曲线段的切线，得到两条切直线；

设置预设长度阈值，在两条切直线围成的下侧区域作出与两条切直线均相切的内切圆，内切圆与任一条切直线的切点到偏角异常点的距离均等于预设长度阈值，基于预设长度阈值和两条切直线的夹角得到内切圆的半径；

获取第一条切直线上从与前半圆曲线段的切点到偏角异常点的线段上所有点的位置坐标，将所有点记作第一待修正点；

获取第二条切直线上从偏角异常点到与后半圆曲线段的切点的线段上所有点的位置坐标，将所有点记作第二待修正点；

获取第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标，并基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量；

获取第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标，并基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量；

利用第一补偿量对对应的每个第一待修正点进行补偿，得到所有的第一修正点的位置坐标，利用第二补偿量对对应的每个第二待修正点进行补偿，得到所有的第二修正点的位置坐标；

将所有的第一修正点和所有的第二修正点依次相连，得到前半圆曲线段和后半圆曲线段之间的拟合线型，从而得到补偿后的轨道线型。

2.根据权利要求1所述的方法，其特征在于，利用惯导系统实时获取轨检车的位置坐标和航向角包括：

将里程计和惯导系统作为组合导航系统，构建组合导航系统的状态方程；

构建组合导航系统的观测方程；

基于状态方程和观测方程构建卡尔曼滤波方程，利用卡尔曼滤波方程对组合导航系统误差进行估计；

基于估计的组合导航系统误差对惯导系统进行修正，以实时获取轨检车的位置坐标和航向角。

3.根据权利要求1或2所述的方法，其特征在于，通过下式构建组合导航系统的状态方程：

其中，

通过下式构建组合导航系统的状态方程：

Z_k＝H_kX_k+V_k；

其中，Z_k＝ΔV_Ins-ΔV_d；

式中，X(t)为状态向量，F为状态转移矩阵，w(t)为系统噪声，δV_N、δV_E分别为北向速度误差、东向速度误差，

δh、δλ分别为纬度误差、高度误差、经度误差，φ_N、φ_U、φ_E分别为北、天、东三个方向的姿态误差，ε_x、ε_y、ε_z分别为x轴、y轴、z轴陀螺漂移，▽_x、▽_y、▽_z分别为x轴、y轴、z轴加速度计零位，Z_k为k时刻的观测量，H_k为观测矩阵，X_k为k时刻的状态向量，V_k为观测噪声，ΔV_Ins为惯导解算速度误差，ΔV_d为里程计速度误差。

4.根据权利要求2所述的方法，其特征在于，通过下式对组合导航系统误差进行估计：

P_k＝(I-K_kH_k)P_k/k-1；

式中，

为状态一步预测，Φ_k,k-1为状态转移矩阵，

为k-1时刻的状态估计，P_k/k-1为一步预测均方误差阵，P_k-1为k-1时刻的估计均方误差阵，w_k-1为k-1时刻的系统噪声，K_k为滤波增益，

为k时刻的状态估计，P_k为k时刻的估计均方误差阵，I为单位矩阵。

5.根据权利要求1或2所述的方法，其特征在于，直线方程如下式所示：

y＝k′x+b′；

圆曲线方程如下式所示：

a＝x₁-x₂

b＝y₁-y₂

c＝x₁-x₃；

d＝y₁-y₃

缓和曲线方程如下式所示：

式中，k′为直线方程的斜率，b′为直线方程的截距，(x₁,y₁)、(x₂,y₂)、(x₃,y₃)分别为圆曲线段上三点的位置坐标，(x₀,y₀)为圆曲线段圆心的位置坐标，l为缓和曲线段的起始位置到待修正点的里程，l_s为缓和曲线段的总里程，R为缓和曲线段上的待修正点的曲率半径。

6.根据权利要求1或2所述的方法，其特征在于，基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量包括：

基于第二条切直线上从与内切圆的切点到偏角异常点的线段上所有点的位置坐标得到第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离；

基于第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离和内切圆的半径得到与每个第一待修正点一一对应的第一补偿量。

7.根据权利要求6所述的方法，其特征在于，通过下式得到第一补偿量：

式中，dr₁为第一补偿量，l₁为第二条切直线上从与内切圆的切点到偏角异常点的线段上的任一点到偏角异常点的距离，r为内切圆的半径。

8.根据权利要求1或2所述的方法，其特征在于，基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量包括：

基于第一条切直线上从偏角异常点到与内切圆的切点的线段上所有点的位置坐标得到第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离；

基于第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离和内切圆的半径得到与每个第二待修正点一一对应的第二补偿量。

9.根据权利要求8所述的方法，其特征在于，通过下式得到第二补偿量：

式中，dr₂为第二补偿量，l₂为第一条切直线上从偏角异常点到与内切圆的切点的线段上的任一点到偏角异常点的距离，r为内切圆的半径。

10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至9任一所述方法。

Images