CN109827572B - 一种检测车位置预测的方法及装置 - Google Patents

Abstract

本发明公开了一种检测车位置预测的方法及装置，本发明所述的检测车位置预测的方法包括：基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量和历史量测量；将所述检测车的历史状态量输入到预设的姿态角预测模型，得到所述检测车的预测姿态信息；根据检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息。有鉴于此，本申请的目的在于提供一种预测方法，用于解决现有技术中惯导误差修正困难的问题。可以方便有效地修正惯导误差，并且结构简单，测量效率高。

Description

一种检测车位置预测的方法及装置

技术领域

本申请涉及测量领域，具体而言，涉及一种一种检测车位置预测的方法及装置。

背景技术

目前，轨道检测车多采用惯性导航，基于惯性导航可以使轨道检测车提供高精度、快速、连续的轨道参数测量。然而，惯性导航也存在弊端，惯性导航在不间断的长时间工作后会产生累计误差，从而导致轨道检测车姿态位置解算不准确。

为解决惯性导航的累计误差问题，目前解决方法主要是依赖于GPS或全站仪对惯性导航误差进行修正，但GPS测量精度低，无法满足轨道检测车高精度测量的需求。而全站仪操作测量过程中需要设站，测量效率受限，并且全站仪对于铁路CPⅢ点的依赖度很大。

发明内容

有鉴于此，本申请的目的在于提供一种检测车位置预测的方法及装置，用于解决现有技术中惯性导航的累计误差修正难度大的问题。可以方便有效地修正惯导误差，并且结构简单，测量效率高。

第一方面，本申请实施例提供了一种检测车位置预测的方法，该方法包括：

基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量；

将所述检测车的历史状态量输入到预设的预测模型，得到所述检测车的预测姿态信息；

根据所述检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息。

可选地，基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量，包括：

根据检测车的历史测量信息和预设信息，以及预设的姿态误差方程、速度误差方程和位置误差方程，确定所述检测车的历史状态量；

其中，姿态误差方程为：

速度误差方程为：

位置误差方程为：

δH＝δV_U；

其中，δθ为俯仰角误差，δλ为滚转角误差，δψ为航向角误差，δV_E为东向速度误差，δV_N为北向速度误差，δV_U为天向速度误差，δL为纬度误差，δλ为经度误差，δH为高度误差，L为纬度，λ为经度，H为高度，R_M为地球子午圈主曲率半径，R_N为地球卯酉圈主曲率半径，ω_ie为地球自转角速率，ε_E为东向陀螺漂移，ε_N为北向陀螺漂移，ε_U为天向陀螺漂移，f_E、f_N、f_U分别为载体坐标系在加速度计测量值在导航坐标系下东向、北向、天向的投影，V_E为东向速度，V_N为北向速度，V_U为天向速度，

为东向加速度计偏置，

为北向加速度计偏置，

为天向加速度计偏置。

可选地，将所述检测车的状态量输入到预设的预测模型，得到所述检测车的预测姿态信息，包括：

通过以下公式计算检测车的预测状态量：

其中，

为状态量一步预测；

为状态量估计；

为历史状态量；P_k/k-₁为一步预测误差方差阵；K_k为滤波增益阵；P_k为估计误差方差阵；e_k称为新息过程；Φ_k/k-1为状态转移矩阵；H_k为量测矩阵；R_k为量测噪声方差阵；Q_k-1为过程噪声方差阵；

为过程噪声矩阵；

根据所述预测状态量中的预测姿态误差，结合测量信息，通过计算得到所述检测车的预测姿态信息。

可选地，根据检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息，包括：

根据检测车的预测姿态信息，确定姿态转移矩阵；

根据所述姿态转移矩阵、历史量测量，以及所述检测车的历史位置信息，确定所述检测车的预测位置信息；

其中，其中，

为姿态转移矩阵，δV′_N为捷联解算后的北向速度，[0 δV′_N 0]^T为里程计速度，P_k-1为历史位置信息，P_k为预测位置信息。

可选地，检测车的预测位置里程计速度，包括：

预设滤波周期为1s时，所述预测位置里程计速度在数值上等同于，检测车从历史位置到预测位置的路程ΔL。

第二方面，本申请实施例提供了一种检测车位置预测的装置，该装置包括：

第一确定模块，用于基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量；

第二确定模块，用于将所述检测车的历史状态量输入到预设的预测模型，得到所述检测车的预测姿态信息；

位置预测模块，用于根据检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息。

可选地，第一确定模块还包括：

误差计算单元，用于根据检测车的历史测量信息和预设信息，以及预设的姿态误差方程、速度误差方程和位置误差方程，确定所述检测车历史状态量中的姿态误差、速度误差和位置误差。

可选地，第二确定模块还包括：

预测状态量单元，用于将检测车的历史状态量输入到预设预测模型，得到检测车的预测状态量；

预测姿态单元，用于根据预测状态量中的预测姿态误差，结合测量信息，通过计算得到所述检测车的预测姿态信息。

可选地，位置预测模块还包括：

姿态转移单元，用于根据检测车的预测姿态信息，确定姿态转移矩阵；

位置预测单元，用于根据检测车的预测位置里程计速度、所述检测车的预测姿态信息，以及所述检测车的历史位置信息，确定所述检测车的预测位置信息。

第三方面，本申请实施例提供了一种计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一方面实施例的任一种所述的方法的步骤。

第四方面，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器运行时执行上述第一方面实施例中任一种所述的方法的步骤。

申请实施例提出的检测车位置预测的方法及装置，通过基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量和历史量测量，再将所述检测车的历史状态量输入到预设的姿态角预测模型，得到所述检测车的预测姿态信息，然后根据所述检测车的预测姿态信息以及所述历史量测量，确定所述检测车的预测位置信息。本申请实施例所提出的检测车位置预测的方法及装置具有可以方便有效地修正惯导误差，并且结构简单，测量效率高的特点。

为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例提供的一种检测车位置预测的方法的流程示意图；

图2为本申请实施例提供的一种预测姿态信息确定方法的流程示意图；

图3为本申请实施例提供的一种预测位置信息确定方法的流程示意图；

图4为本申请实施例提供的一种检测车位置预测的装置的结构示意图；

图5为本申请实施例提供的一种计算机设备的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请实施例提供了一种检测车位置预测的方法，如图1所示，包括以下步骤：

101，基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量。

历史测量信息，包括：纬度L，经度λ，高度H，东向陀螺漂移ε_E，北向陀螺漂移ε_N，天向陀螺漂移ε_U，载体坐标系在加速度计测量值在导航坐标系下东向、北向、天向的投影f_E、f_N、f_U，东向速度V_E，北向速度V_N，天向速度V_U，东向加速度计偏置

北向加速度计偏置

天向加速度计偏置

地球子午圈主曲率半径R_M，地球卯酉圈主曲率半径R_N，地球自转角速率ω_ie，其中，

R_M＝R_e(1-2e+3esin²L)；

R_N＝R_e(1+esin²L)。

预设信息，包括：椭圆度e＝1/298.257，地球椭球长半轴R_e＝6378137m。

历史状态量，是历史时刻检测车的状态量，状态量如下：

其中，δθ为俯仰角误差，δλ为滚转角误差，δψ为航向角误差，δV_E为东向速度误差，δV_N为北向速度误差，δV_U为天向速度误差，δL为纬度误差，δλ为经度误差，δH为高度误差，ε_x为载体坐标系x轴陀螺常值漂移，ε_y为载体坐标系y轴陀螺常值漂移，ε_z为载体坐标系z轴陀螺常值漂移。

为载体坐标系x轴加速度计常值偏置，

为载体坐标系y轴加速度计常值偏置，

为载体坐标系z轴加速度计常值偏置。

具体地，状态量中包括的姿态(俯仰角、滚转角、航向角)误差、速度(东向速度，北向速度、天向速度)误差、位置(经度、纬度、高度)误差，需要通过误差方程进行解算得到，具体的误差方程如下：

姿态误差方程为：

速度误差方程为：

位置误差方程为：

δH＝δV_U；

将检测车的历史测量信息和预设信息输入误差方程中，就能得到相应的历史误差，将历史误差结合历史测量信息中的陀螺常值漂移和加速度计常值偏置，得到历史状态量。

102，将所述检测车的历史状态量输入到预设的预测模型，得到所述检测车的预测姿态信息。

预设的预测模型，可以使用能够得到检测车姿态信息的任一种预测模型，本申请实施例优选kalman滤波公式作为预测模型，具体采用的预测模型在这里不作限制。

本申请优选的kalman滤波公式为标准离散型的kalman滤波基本方程，具体方程如下：

其中，

为状态量一步预测；

为状态量估计；

为历史状态量；P_k/k-1为一步预测误差方差阵；K_k为滤波增益阵；P_k为估计误差方差阵；ek称为新息过程；Φ_k/k-1为状态转移矩阵；H_k为量测矩阵；R_k为量测噪声方差阵；Q_k-1为过程噪声方差阵；

为过程噪声矩阵。

状态转移矩阵Φ_k/k-1，其原型为：

滤波周期Δt可以为任意预设值，这里对于滤波周期不做限制，优选地，本申请实施例滤波周期Δt预设为1s，当Δt较短时，F可近似看作常阵，即：

F≈F(t_k-1)；t_k-1≤t＜t_k；

也就是，F_k≈F_k-1，

此时状态转移矩阵Φ_k/k-1有如下计算式：

量测矩阵H_k，H＝[H_V]^T，H_V＝[0_3×3diag(1，1，1) 0_3×9]。

量测噪声方差阵R_k，是本领域技术人员根据里程计的性能，凭借经验选取的。

过程噪声方差阵Q_k-1，是本领域技术人员根据惯导系统的惯性器件噪声水平，凭借经验选取的。

过程噪声矩阵

其具体形式为：

其中，

为载体坐标系到导航坐标系的姿态转移阵，姿态转移阵

具体形式为：

预测姿态信息，包括预测位置检测车的姿态角：俯仰角θ、滚转角γ、航向角ψ。

具体地，如图2所示，步骤102可以分为以下三个步骤：

1021，将所述检测车的历史状态量输入到预设的预测模型，得到预测状态量；

1022，通过对预测位置的加速度计和陀螺的测量信息进行捷联运算，得到预测位置未经修正的姿态角；

1023，根据预测状态量中的姿态角误差，对预测位置未经修正的姿态角进行误差修正，得到修正后的姿态角。

修正后的姿态角，就是所述检测车的预测姿态信息。

103，根据检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息。

预测位置信息，是检测车在预测位置的经度、纬度和高度。

具体地，如图3所示，该步骤可以分为以下两个步骤：

1031，根据检测车的预测姿态信息，确定姿态转移矩阵；

1032，根据检测车预测位置里程计速度、所述姿态转移矩阵，以及所述检测车的历史位置信息，确定所述检测车的预测位置信息。

具体地，步骤1031，根据步骤102得到的预测姿态信息，将修正后的姿态角代入姿态转移矩阵

中，得到预测位置的姿态转移矩阵；将检测车预测位置里程计速度δV′_N、所述姿态转移矩阵

以及所述检测车的历史位置信息P_k-1代入航迹递推公式中，航迹递推公式如下：

其中，[0 δV′_N 0]^T是预测位置检测车的量测量，量测量如下：

y＝[δV′_E δV′_N δV′_U]^T，

其中，δV′_E、δV′_N、δV′_U分别为捷联解算的东向速度、北向速度、天向速度，在本申请实施例中检测车的侧向速度和天向速度为0，前向速度为里程计速度，也就是，东向速度和天向速度为0，北向速度为里程计速度，所以上式可以表示为：

y＝[0 δV′_N 0]^T，

确定了预测位置检测车的里程计速度，也就确定了预测位置检测车的量测量。

得到的是预测位置信息相对于历史位置信息的偏移量。

预设滤波周期Δt为1s，所述预测位置里程计速度在数值上等同于，检测车从历史位置到预测位置的路程ΔL，所以，本申请实施例中的航迹递推公式也可写做：

本申请实施例提供了一种检测车位置预测的装置，如图4所示，包括以下：

第一确定模块41，用于基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量；

第二确定模块42，用于将所述检测车的历史状态量输入到预设的姿态角预测模型，得到所述检测车的预测姿态信息；

位置预测模块43，用于根据检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息。

可选地，第一确定模块还包括：

误差计算单元411，用于根据检测车的历史测量信息和预设信息，以及预设的姿态误差方程、速度误差方程和位置误差方程，确定所述检测车历史状态量中的姿态误差、速度误差和位置误差。

可选地，第二确定模块还包括：

预测状态量单元421，用于将检测车的历史状态量输入到预设预测模型，得到检测车的预测状态量；

预测姿态单元422，用于根据预测状态量中的预测姿态误差，结合测量信息，通过计算得到所述检测车的预测姿态信息。

可选地，位置预测模块还包括：

姿态转移单元431，用于根据检测车的预测姿态信息，确定姿态转移矩阵；

位置预测单元432，用于根据检测车的预测位置里程计速度、所述检测车的预测姿态信息，以及所述检测车的历史位置信息，确定所述检测车的预测位置信息。

对应于图1中的检测车位置预测的方法，本申请实施例还提供了一种计算机设备500，如图5所示，该设备包括存储器501、处理器502及存储在该存储器501上并可在该处理器502上运行的计算机程序，其中，上述处理器502执行上述计算机程序时实现上述检测车位置预测的方法。

具体地，上述存储器501和处理器502能够为通用的存储器和处理器，这里不做具体限定，当处理器502运行存储器501存储的计算机程序时，能够执行上述检测车位置预测的方法，解决了现有技术中惯性导航的累计误差修正难度大的问题。

对应于图1中的检测车位置预测的方法，本申请实施例还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行上述检测车位置预测的方法的步骤。

具体地，该存储介质能够为通用的存储介质，如移动磁盘、硬盘等，该存储介质上的计算机程序被运行时，能够执行上述检测车位置预测的方法，解决了现有技术中惯性导航的累计误差修正难度大的问题，本申请通过根据检测车的历史测量信息和预设信息，通过计算得到了所述检测车的历史状态量，将所述历史状态量输入到预设的预测模型里，得到所述检测车的预测姿态信息，再根据所述检测车的预测位置里程计速度和所述检测车的预测姿态信息，就能推算出所述检测车的预测位置信息，本申请提供的检测车位置预测的方法，使得检测车无需配备GPS或全站仪等绝对测量仪器，就能够方便有效地得到检测车的预测位置的位置信息，提高了检测车位置信息的测量效率。

在本申请所提供的实施例中，应该理解到，所揭露方法和装置，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请提供的实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

最后应说明的是：以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的精神和范围。都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

Claims (8)

1.一种检测车位置预测的方法，其特征在于，包括：

基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量；所述历史状态量包括历史姿态误差、历史速度误差、历史位置误差、以及历史测量信息中的陀螺常值漂移和加速度常值偏置；所述预设信息包括椭圆度和地球椭球长半轴；

将所述检测车的历史状态量输入到预设的预测模型，得到所述检测车的预测姿态信息；

根据所述检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息；

所述根据检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息，包括：

根据检测车的预测姿态信息，确定姿态转移矩阵；

根据检测车的预测位置里程计速度、所述姿态转移矩阵，以及所述检测车的历史位置信息，确定所述检测车的预测位置信息；

其中，

为姿态转移矩阵，δV′_N为捷联解算后的北向速度，[0 δV′_N 0]^T为里程计速度，P_k-1为历史位置信息，P_k为预测位置信息。

2.如权利要求1所述的方法，其特征在于，基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量，包括：

根据检测车的历史测量信息和预设信息，以及预设的姿态误差方程、速度误差方程和位置误差方程，确定所述检测车的历史状态量；

其中，姿态误差方程为：

速度误差方程为：

位置误差方程为：

δH＝δV_U；

其中，δθ为俯仰角误差，δλ为滚转角误差，δψ为航向角误差，δV_E为东向速度误差，δV_N为北向速度误差，δV_U为天向速度误差，δL为纬度误差，δλ为经度误差，δH为高度误差，L为纬度，λ为经度，H为高度，R_M为地球子午圈主曲率半径，R_N为地球卯酉圈主曲率半径，ω_ie为地球自转角速率，ε_E为东向陀螺漂移，ε_N为北向陀螺漂移，ε_U为天向陀螺漂移，f_E、f_N、f_U分别为载体坐标系在加速度计测量值在导航坐标系下东向、北向、天向的投影，V_E为东向速度，V_N为北向速度，V_U为天向速度，

为东向加速度计偏置，

为北向加速度计偏置，

为天向加速度计偏置。

3.如权利要求1所述的方法，其特征在于，将所述检测车的历史状态量输入到预设的预测模型，得到所述检测车的预测姿态信息，包括：

通过以下公式计算检测车的预测状态量：

其中，

为状态量一步预测；

为状态量估计；

为历史状态量；P_k/k-1为一步预测误差方差阵；K_k为滤波增益阵；P_k为估计误差方差阵；e_k称为新息过程；Φ_k/k-1为状态转移矩阵；H_k为量测矩阵；R_k为量测噪声方差阵；Q_k-1为过程噪声方差阵；

为过程噪声矩阵；

根据所述预测状态量中的预测姿态误差，结合测量信息，通过计算得到所述检测车的预测姿态信息。

4.如权利要求1所述的方法，所述检测车的预测位置里程计速度，其特征在于，包括：

预设滤波周期为1s时，所述预测位置里程计速度在数值上等同于，检测车从历史位置到预测位置的路程ΔL。

5.一种检测车位置预测的装置，其特征在于，包括：

第一确定模块，用于基于检测车的历史测量信息和预设信息，确定所述检测车的历史状态量；所述历史状态量包括历史姿态误差、历史速度误差、历史位置误差、以及历史测量信息中的陀螺常值漂移和加速度常值偏置；所述预设信息包括椭圆度和地球椭球长半轴；

第二确定模块，用于将所述检测车的历史状态量输入到预设的姿态角预测模型，得到所述检测车的预测姿态信息；

位置预测模块，用于根据所述检测车的预测位置里程计速度以及所述检测车的预测姿态信息，确定所述检测车的预测位置信息；

所述位置预测模块具体用于根据检测车的预测姿态信息，确定姿态转移矩阵；根据检测车的预测位置里程计速度、所述姿态转移矩阵，以及所述检测车的历史位置信息，确定所述检测车的预测位置信息；

其中，

为姿态转移矩阵，δV′_N为捷联解算后的北向速度，[0 δV′_N 0]^T为里程计速度，P_k-1为历史位置信息，P_k为预测位置信息。

6.如权利要求5所述的装置，其特征在于，所述第一确定模块还包括：

误差计算单元，用于根据检测车的历史测量信息和预设信息，以及预设的姿态误差方程、速度误差方程和位置误差方程，确定所述检测车的历史状态量中的姿态误差、速度误差和位置误差。

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

8.一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器运行时执行上述权利要求1-4中任一项所述的方法的步骤。

Images