一种基于惯导/视觉里程计的列车定位方法及系统与流程

本发明涉及轨道交通控制领域，更为具体地，涉及一种基于惯导/视觉里程计的列车组合导航方法与系统。
背景技术：
作为列车运行控制系统的重要组成部分，列车定位是列车运行控制系统做出相应决策的前提与保证。以往采用的列车定位技术主要包括:惯性传感器、里程计、列车-轨道信息交换设备(包括编码轨道电路、应答器等等)、轨道电路、多普勒雷达等。但是，这些列车定位技术都存在一定的缺点，例如：惯性传感器误差会随时间不断增大；轨道电路或计轴设备则需要大量轨旁设备，使得其建设和维护成本高昂；对于安装特定定位设备的机车和列车来说，它的定位只能在与轨旁设备兼容的范围内有效，如果列车运行范围内存在多种轨旁设备，那么就需要在列车上安装多种定位设备以保证兼容性。随着列车运行控制系统对定位精度和可靠性的要求日益增加，仅通过使用一种定位传感器很难满足其需求。传感器很大程度上存在一些无法克服的缺点，单一传感器的适用范围较小，无法适应所有情况，仅可提供部分的环境信息，导致感知的定位信息会存在不确定性以及偶发性的缺失和错误。另外，采用单一传感器的列车定位系统稳定性弱，传感器故障可能导致列车定位系统的失效，甚至会导致事故发生。因此，通过使用多传感器进行列车定位，融合处理多传感器信息，对列车安全可靠运行具有重要意义。目前使用最广泛的组合定位方法是基于ins/gps融合的列车定位方法，这种方法虽然利用了卫星导航系统和ins各自的优势进行互补，实现高频率的列车位置输出，提高了列车定位可靠性，降低了列车定位系统成本并且在卫星信号良好的情况下定位结果更加准确，但是ins/gps组合系统中仍存在很多问题，其中最主要的问题就是基于ins/gps的组合定位系统对gps信号的依赖程度过大，然而卫星信号是很容易受到有意或无意的干扰或遮挡的，这会造成gps的定位精度严重下降，而ins的定位误差会随时间积累而越来越大，进而整个列车定位系统的定位精度会大幅度地降低。技术实现要素：鉴于上述问题，本发明的目的是提供一种基于惯导/视觉里程计的列车组合导航方法与系统，以解决现有列车定位技术在卫星信号缺失情况下定位精度严重下降的问题。本发明提供一种基于惯导/视觉里程计的列车定位方法，包括：获取传感器实时数据，其中，所述传感器实时数据包括惯性传感器数据和摄像机采集到的图像数据；根据采集到的传感器实时数据建立系统模型、状态方程以及观测方程；通过惯性传感器对所述惯性传感器数据进行解算，获取列车的运动信息；通过视觉里程计对所述图像数据进行处理，获取列车的运动信息；采用改进的无迹卡尔曼滤波算法对通过惯性传感器以及通过视觉里程计获取的列车的运动信息进行数据融合处理，获取列车的运动信息；获取的列车的运动信息与线路数据库的信息进行匹配，当获取的列车的运动信息显示为进入公里标的捕获区域时，采集轨旁公里标图像，通过对采集到轨旁公里标图像进行处理确定当前列车的运动信息。本发明还提供一种基于惯导/视觉里程计的列车定位系统，包括：惯导解算模块，用于获取惯性传感数据，以及对获取的惯性传感数据进行处理获取列车的运动信息；视觉里程计模块，用于获取图像信息，以及对获取的图像信息进行处理获取列车的运动信息；数据融合模块，用于通过改进的无迹卡尔曼滤波算法对所述惯导解算模块获取列车的运动信息和所述视觉里程计模块获取的列车的运动信息进行数据融合处理，获取列车的运动信息；数据修正模块，用于将所述数据融合模块获取的列车的运动信息与线路数据库的信息进行匹配，当获取的列车的位置信息显示为进入公里标的捕获区域时，采集轨旁公里标图像，通过对采集到轨旁公里标图像进行处理确定当前列车的实际位置，以修正列车的运动信息。从上面的技术方案可知，本发明提供的基于惯导/视觉里程计的列车定位方法及系统，利用惯性传感器和视觉里程计分别进行载体运动信息的推算和融合，并使用轨旁公里标对定位误差进行修正，实现列车定位；视觉里程计可以避免车轮磨损、空转、打滑等对测量精度的影响，使用orb算法，在保证定位精度的同时提高了视觉计算的实时性；本发明相比安装大量的轨旁设备进行列车定位，成本较低，安装简单。为了实现上述以及相关目的，本发明的一个或多个方面包括后面将详细说明的特征。下面的说明以及附图详细说明了本发明的某些示例性方面。然而，这些方面指示的仅仅是可使用本发明的原理的各种方式中的一些方式。此外，本发明旨在包括所有这些方面以及它们的等同物。附图说明通过参考以下结合附图的说明，并且随着对本发明的更全面理解，本发明的其它目的及结果将更加明白及易于理解。在附图中：图1为根据本发明实施例的基于惯导/视觉里程计的列车定位方法流程示意图；图2为根据本发明实施例的视觉里程计算法框图；图3为orb算法流程图；图4为根据本发明实施例的无迹卡尔曼滤波算法流程图；图5为根据本发明实施例的基于惯导/视觉里程计的列车定位系统结构图。在所有附图中相同的标号指示相似或相应的特征或功能。具体实施方式在下面的描述中，出于说明的目的，为了提供对一个或多个实施例的全面理解，阐述了许多具体细节。然而，很明显，也可以在没有这些具体细节的情况下实现这些实施例。以下将结合附图对本发明的具体实施例进行详细描述。为了说明本发明提供的基于惯导/视觉里程计的列车定位方法，图1示出了根据本发明实施例的基于惯导/视觉里程计的列车定位方法流程。如图1所示，本发明提供的基于惯导/视觉里程计的列车定位方法包括：s110：获取传感器实时数据，其中，所述传感器实时数据包括惯性传感器数据和摄像机采集到的图像数据；s120：通过惯性传感器对所述惯性传感器数据进行解算，获取列车的运动信息；s130：通过视觉里程计对所述图像数据进行处理，获取列车的运动信息；s140：采用改进的无迹卡尔曼滤波算法对通过惯性传感器获取的列车的运动信息以及通过视觉里程计获取的列车的运动信息进行数据融合处理，获取列车的运动信息；s150：获取的列车的运动信息与线路数据库的信息进行匹配，当获取的列车的运动信息显示为进入公里标的捕获区域时，采集轨旁公里标图像，通过对采集到轨旁公里标图像进行处理确定当前列车的运动信息。上述为本发明的详细的列车定位方法，建立系统模型与状态方程以及观测方程，本发明采用改进的无迹卡尔曼滤波算法以及系统模组、状态方程以及观测方程对获取的列车的运动信息进行数据融合，获取列车新的运动信息；系统模型具体如下：状态方程为：其中，x为15维系统误差状态向量，f为一步预测状态转移矩阵，g为系统噪声矩阵，w为系统噪声。x＝[δreδveεedb]t式中，上标e表示在地固系下表示，δre为三维位置误差向量，δve为三维速度误差向量，εe为姿态误差向量，d为载体系下陀螺仪的三轴漂移向量，b为载体系下加速度计的三轴漂移向量。当gps信号无效时，利用视觉里程计计算的载体速度信息与惯导解算的速度信息的差作为组合定位方法的观测值。由于在实际情况下，环境因素影响、模型和参数的选取、设备的状况等都会造成系统误差随时间的积累，而无迹卡尔曼滤波方法必须建立准确的系统模型，否则在进行姿态估计时会出现较大的估计误差，甚至导致滤波发散，故将增量系统建模方法加入到列车定位模型建立的过程中。则列车组合定位系统观测方程为：δzk＝hkxk-hk-1xk-1+uk式中，δzk＝zk-zk-1，z为观测向量，h为观测矩阵，uk＝vk-vk-1，v为观测噪声；k表示第k步。h＝[03×3i3×303×9]其中，需要说明的是，在进行数据融合的过程中，除了采用改进的无迹卡尔曼滤波算法与系统模型、、状态方程以及观测方程相结合对数据进行融合，还可以采用其他的方式对数据进行融合，获取列车新的运动信息，本发明不在此一一进行举例。在步骤s120中，利用惯性传感器解算列车的运动信息步骤为：步骤121，速度更新：通过前一时刻系统的速度值与惯性传感器采集的陀螺仪与加速度计的数据，解算出当前时刻的速度值。步骤122，位置更新：通过前一时刻的位置值，前一时刻与当前时刻的速度值，计算地球参数，解算出当前时刻的位置值。步骤123，姿态更新：通过当前时刻惯性传感器采集的陀螺仪与加速度计的数据，采用辅助重力信息修正陀螺仪误差，通过姿态四元数更新方程，计算当前时刻的姿态四元数与姿态角。在步骤s130，视觉里程计计算载体运动信息。视觉里程计的算法框图如图2所示，其具体实现步骤主要包括：(1)通过张正友棋盘标定法对摄像头的畸变参数进行标定，然后使用标定获得的畸变参数对每一帧图像进行畸变修正，然后对图像进行高斯滤波以消除噪声，为特征点提取做准备；(2)，利用orb特征提取算法对校正后的图像对进行特征提取，并对提取到的特征点进行特征点匹配。orb算法流程图如图3所示。s310：输入图像；s320：是否为灰度图；否，则执行s330；是，则执行s340和s350；s330：灰度化，步骤s330后，则执行步骤s320；s340：fast算法；然后执行步骤s360；s350：高斯金字塔；s360：harris角点检测；然后执行s370和s380；s370：灰度质心法；s380：brief算法；s390：特征点匹配。在表1示出了为在具有5个尺度的2686张图像的数据集上计算orb时，它能够在42s内检测和计算超过2个特征。在相同的数据上，具有相同数量的特征(大约1000个)以及相同的尺度上，与sift和surf相比，orb比surf快一个数量级，比sift快两个数量级。使用左右图像提取和匹配成功的特征点信息进行计算，可以得到每个特征点的三维坐标；将两相邻时刻(即当前帧和下一帧)的图像特征点进行匹配，计算出摄像机的运动信息，包括旋转矩阵和平移矩阵；根据摄像机与载体之间的坐标变换，计算出载体的运动信息，即载体相对于初始时刻的位移和姿态变化。表1方法orbsurfsitftime-per-frame(ms)15.3217.35228.7在步骤s140中，使用改进的无迹卡尔曼滤波算法实现信息融合。图4为进行信息融合的无迹卡尔曼滤波算法流程图，其主要步骤为：对建立的非线性系统模型：步骤1)滤波参数初始化为状态变量的初始值，p0为协方差初始值。步骤2)状态估计，预测过程：计算sigma点:其中，χi|k-1为通过最小偏度单形采样策略获得的sigma点。最小偏度采样策略递推过程为：首先选择0≤w0＜1；然后确定sigma权值为迭代初始向量(状态为1维的情况)对于输入维数j＝2,…,n时，迭代公式为式中，j为向量的维数，i为采样点的顺序；定义sigma权值为：式中，wi为最小偏度采样中第i个采样点的权值，和分别为ut变换中均值和协方差的加权值。0＜α≤1，用于调整周围sigma点的分布；β≥0，用于包含高阶项信息，本实例中，β＝2为最优。最后，对所生成的sigma点集中的采样点加入x的均值和协方差信息时间传播方程：根据系统状态方程对sigma点集中的每一个sigma点进行f(·)非线性变换：χi,k|k-1＝f(χi|k-1)对变换后的点集进行加权，得到一步预测状态及方差步骤3)，测量更新方程，测量过程：根据测量方程对sigma点集进行非线性变换：γi,k|k-1＝h(χi,k|k-1)进行加权处理得到预测值计算系统量测输出变量的方差阵：计算协方差pxy,k:步骤s150，利用轨旁公里标修正定位误差。将列车组合定位系统输出的位置信息与线路数据库进行匹配，当检测到列车位置进入公里标的捕获区域时，高速相机开始对轨旁公里标进行捕捉，采集识别公里标特征点与数据库进行模版匹配，计算公里标距离列车的物理距离，识别出公里标所表示的里程数，计算当前列车的实际里程，修正惯导解算定位误差，从而提高列车组合定位精度。与上述方法相对应，本发明还提供一种基于惯导/视觉里程计的列车定位系统，图5示出了根据本发明实施例的基于惯导/视觉里程计的列车定位系统逻辑结构。如图5所示，本发明提供的基于惯导/视觉里程计的列车定位系统，包括惯导解算模块2、视觉里程计模块3、数据融合模块4、数据修正模块4、数据输出模块6、安全电源模块1以及上位机7。其中，惯导解算模块2电连接数据修正模块5，惯导解算模块2和视觉里程计模块3都电连接数据融合模块4，数据融合模块4电连接数据输出模块6，数据输出模块6电连接上位机7。具体地，惯导解算模块2，用于获取惯性传感数据，以及对获取的惯性传感数据进行处理获取列车的运动信息。视觉里程计模块3，用于获取图像信息，以及对获取的图像信息进行处理获取列车的运动信息。数据融合模块4，用于通过改进的无迹卡尔曼滤波算法对所述惯导解算模块获取列车的运动信息和所述视觉里程计模块获取的列车的运动信息进行数据融合处理，获取列车的运动信息。数据修正模块5，用于将所述数据融合模块获取的列车的运动信息与线路数据库的信息进行匹配，当获取的列车的位置信息显示为进入公里标的捕获区域时，采集轨旁公里标图像，通过对采集到轨旁公里标图像进行处理确定当前列车的实际位置，以修正列车的运动信息。安全电源模块1，用于对列车定位系统提供电源；数据输出模块6，用于将列车的位置信息输出至所述上位机7；上位机7，用于显示和下载列车的运动信息。其中，惯导解算模块2包括imu数据(惯性传感器数据)采集21以及数据解算22，imu传感器型号为3dm-ahrs300a，进行惯导解算的微处理器型号为stm32f103zet6。视觉里程计模块3包括图像信息采集31与运动状态估计32(通过运动状态估计对图像数据处理)，图像信息采集使用的摄像头为fl2-14s3c。数据融合模块4采用的微处理器型号为stm32f103zet6。数据修正模块5包括图像数据采集51和实际位置确定52，在图像数据采集51中进行图像匹配，在实际位置确定52中进行里程计算与数据修正，数据修正模块5采用的微处理器型号为stm32f103zet6。上位机7包括数据显示71和数据下载72，数据显示71的内容包括惯导/视觉里程计定位结果、列车行车轨迹、列车速度实时解算曲线。可下载数据包括：列车行车位置信息、列车行车路况图像、imu原始信息及相应的时间信息。此外，数据输出模块6与上位机7之间的电连接采用rs232串行通信连接。通过上述实施方式可以看出，本发明提供的基于惯导/视觉里程计的列车定位方法及系统，利用惯性传感器和视觉里程计分别进行载体运动信息的推算和融合，并使用轨旁公里标对定位误差进行修正，实现列车定位；视觉里程计可以避免车轮磨损、空转、打滑等对测量精度的影响，使用orb算法，在保证定位精度的同时提高了视觉计算的实时性；本发明相比安装大量的轨旁设备进行列车定位，成本较低，安装简单以上所述参照附图以示例的方式描述了根据本发明的一种基于惯导/视觉里程计的列车定位方法及系统，对本发明的目的、技术方案和有益效果进行了进一步的详细说明，本领域技术人员应当理解，对于上述本发明所提出的一种基于惯导/视觉里程计的列车定位方法及系统，还可以在不脱离本
发明内容的基础上做出各种改进。因此，本发明的保护范围应当由所附的权利要求书的内容确定。当前第1页12