CN115540875A - 一种用于股道内列车车辆高精度检测定位的方法及系统 - Google Patents

Abstract

本发明公开了一种用于股道内列车车辆高精度检测定位的方法及系统，涉及机车检测技术领域，获取雷达定位数据和底盘定位数据；对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；本发明方法在雷达里程计间隔内，用底盘里程计加权插值，以提高定位数据频率，降低了雷达里程计数据的跳变，同时在雷达里程计更新后，消除了部分底盘里程计的累计误差；最终得到了一个在长距离环境下，高频高精度的最终定位结果。

Description

一种用于股道内列车车辆高精度检测定位的方法及系统

技术领域

本发明涉及机车检测技术领域，具体涉及一种用于股道内列车车辆高精度检测定位的方法及系统。

背景技术

在机车检测过程中，在狭窄的上方开口沟道中，因缺乏有效地图标志物或因两侧墙壁严重反光，一般定位方案的移动设备难以通过先验地图方式获取自身的位置坐标，进而造成导航失败。同时，一些现有技术定位精度高，但更新速度慢；一些设备响应频率高，但累计误差大。需要将两类定位结果融合解算，才能得到满足实际需要的，在长距离狭窄沟道运行环境下仍能保持高精度高频率的定位结果。

发明内容

本发明所要解决的技术问题是：传统定位方法单一采用底盘传感器组提供的定位数据，存在累计误差问题，无法满足长距离运行应用场景；本发明目的在于提供一种用于股道内列车车辆高精度检测定位的方法及系统，将高频率的定位结果和高精度的定位结果，通过动态加权算法进行融合，最终得到在长距离运行环境下仍能保持高精度高频率的最终定位结果。

本发明通过下述技术方案实现：

本方案提供一种用于股道内列车车辆高精度检测定位的方法，包括：

获取雷达定位数据和底盘定位数据；

对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；

将高精度定位结果和高频率定位结果进行动态加权融合处理得到最终定位结果，其中动态加权融合处理包括：在雷达里程计间隔内，用底盘里程计加权插值。

本方案工作原理：在实际机车检测过程中，由于地沟内两侧以及地面特征不明显，无法使用先验地图方案进行定位，传统定位方法单一采用底盘传感器组提供的定位数据，存在累计误差问题，无法满足长距离运行应用场景；同时，沟道上方开口范围内物体存在不定时的较大变动，也无法满足先验地图定位的使用场景。由此需要一种实时定位法能够满足长距离运行下同时保持高频率高精度定位的需求。本方案提供的用于地沟内机车检测定位方法，将高频率的底盘定位数据（高频率定位结果）和高精度的雷达定位数据（高精度定位结果），通过动态加权算法进行融合，最终得到在长距离运行环境下仍能保持高精度高频率的最终定位结果。在实际运行过程中，在狭长沟道中同步采集高精度定位数据与高频率定位数据的数值，高精度定位数据根据沟道顶部开口外的特征进行高精度定位，同时高频定位数据稳定最终定位结果的输出频率。当出现移动异物造成高精度定位数据发成偏差时，由于在雷达里程计间隔内，用底盘里程计加权插值，使得突变偏差对最终输出结果影响不显著；当应对较长距离的沟道时，由于高频里程计的数据被高精度的定位数据定时整定，使得在长距离环境下，最终输出结果累计误差不显著，数据稳定。

进一步优化方案为，所述雷达定位数据和底盘定位数据的获取方法包括：

使移动设备运行在待检机车与沟道之间进行数据采集：所述移动设备装有多个光学传感器、距离传感器、IMU传感器和驱动编码器；所述光学传感器和距离传感器用于获取移动设备的雷达定位数据；所述IMU传感器和驱动编码器用于获取移动设备的底盘定位数据。

光学传感器对沟道顶部开口方向进行扫描获取沟道外部的特征，根据特征的变化计算设备的高精度移动速度，其中光学传感器包括可见光，激光，红外光等可形成2维或3维投影视图的传感器。距离传感器通过对两侧墙壁的距离测量，保证移动设备始终位于沟道中间位置。所述距离传感器包括且不限于超声波，激光，红外等用于测距的传感器类型；

IMU传感器可以获取移动设备的实时加速度信息，IMU传感器指能够获取实时加速度的各种传感器类型，通过加速度积分可以获得高速变化的移动速度，结合电机驱动的编码器输出可以计算移动设备的驱动轮转速，进一步的可以通过转速获得高速变化的移动速度用以融合运算高频定位数据得到底盘定位数据。

通过上述传感器获取的速度，根据各自的特性采用加权算法，其加权凭据特征在于：

对于低精度高刷新传感器的输出值，其权值逐步减小；对于容易被突发干扰的传感器，其输出值经过置信度计算决定是否采用；置信度计算方法包括：

数值异常突变，其输出值应被剔除。

多个不同传感器值进行数学统计，根据其他传感器持有的定位数据置信度确定其置信度。

采用2维或3维数据集进行统计解算的，根据历史结果置信度，以及其结果是否在精度要求以内，动态得出最终置信度。

最终，通过以上方法实现，得到在长距离运行环境下仍能保持高精度高频率的定位结果。

进一步优化方案为，所述高精度定位结果获取方法包括：

S1，对雷达定位数据进行基本滤波预处理得到预处理数据；

S2，基于预处理数据维护雷达定位数据内的线面特征与移动设备之间的位姿变化估计出雷达位姿数据，同时对雷达位姿数据中当前视野内的线面特征与历史数据库中的线面特征进行匹配，进而更新雷达定位数据得到高精度定位结果。

进一步优化方案为，S2包括过程：

以维护一张局部的实时三维特征地图的方式，基于线面匹配算法和帧间匹配算法实时更新雷达定位数据；其中，以单帧数据形式进行帧间匹配算法，寻找出两帧数据间的重合部分用来更新雷达定位数据。在此过程中，雷达定位数据更新时，加权的更新雷达定位数据和雷达定位数据的置信度。其中，权值取值条件包括特征匹配度，特征偏移量、新增特征数量和消除特征数量等。置信度取值包括帧间数据重合度和单帧数据量等。

进一步优化方案为，雷达位姿数据估计方法包括：

以单帧点云数据形式对预处理数据进行线面特征提取得到面特征点集和线特征点集；其中当单帧点云数据内的面特征点数量小于阈值U时，舍弃本帧计算结果，并缓存当前帧数据用以和下一帧数据合并计算；

比较两帧数据面特征点集和线特征点集的差异从而估计出雷达位姿数据。

进一步优化方案为，面特征点集和线特征点集的提取方法包括：

从单帧点云数据中按数据编号取连续的点A、点B、点C、点D和点E；

取点C计算点C与其他4个点构成空间向量和的长度L，

；

如果L小于阈值M，则认为点C属于面特征点集，阈值M根据曲面的曲率确定；

当存在任一点f(x,y,z)，点f-2在点f-3、点f-4、点f-1和点f形成的曲面T1上，点f+2在点f+3、点f+4、点f+1和点f形成的曲面T2上；计算两曲面的共面向量点乘结果K，若丨K丨小于阈值G，则认为点f属于线特征点集；阈值G根据两面夹角度数确定；

。

进一步优化方案为，高频率定位结果获取方法包括：

基于底盘定位数据获取IMU姿态数据，并以IMU姿态数据作为高频率定位结果；

在更新雷达定位数据时，基于雷达位姿数据与底盘姿态数据的差异，动态加权的更新到IMU姿态数据和IMU姿态数据置信度上；其中加权值的取值条件包括：雷达位姿数据与底盘姿态数据的差异和底盘姿态数据的置信度。

进一步优化方案为，动态加权的过程遵循逻辑：

底盘姿态置信度与累计角度变化量成反比，当累计角度变化量达到设定阈值，根据IMU姿态数据置信度动态更新底盘姿态数据的权值；

IMU姿态数据与底盘姿态数据的差异变化曲率与IMU姿态数据置信度成反比。

获取的上述高精度定位结果和高频率定位结果分别存在以下弊端：高精度定位结果频率低，且受环境影响较大，如果在运行过程中，存在较多运动物体，将大大降低此定位数据的稳定性，会产生较大定位偏差；高频率定位结果存在累计误差问题，无法在长距离运行环境下保证满足应用需求的精度。

为解决上述弊端，本方案在得到上述两个定位数据后，在低频的雷达里程计间隔内，用底盘里程计加权插值，以提高定位数据频率，降低了雷达里程计数据的跳变，同时在雷达里程计更新后，消除了部分底盘里程计的累计误差，最终得到了一个在长距离环境下，高频高精度的定位数据。

进一步优化方案为，最终定位结果获取方法包括：

雷达里程计更新输出为高精度定位结果，底盘里程计更新输出为高频率定位结果；

在雷达里程计更新后，加权地将雷达里程计与底盘里程计的定位结果差值，更新到最终定位结果上，在下一次雷达里程计数据更新前，底盘里程计持有最终定位结果的更新权，当雷达里程计再次更新时，再次加权更新雷达里程计与底盘里程计的定位结果差值到底盘里程计上。

最终根据雷达里程计置信度，分配一定百分比的两里程计差值与当前底盘里程计进行坐标变换，得出最终定位结果。

雷达里程计置信度运算方法为，根据底盘里程计的置信度，当底盘里程计置信度在一定区间时，记录两里程计数据差异变化率，雷达里程计置信度与此变化率成反比；其中，底盘里程计置信度与其记录总里程成反比。

特殊的，当底盘里程计置信度低于阈值时，根据当前雷达里程计置信度，按一定百分比更新底盘里程计置信度。同时，根据当前雷达里程计置信度，按一定百分比将两里程计差值与底盘里程计做坐标变换，更新地盘里成绩数据。

高频定位数据权值维持在较高置信度，同时高精度定位数据的剧烈偏差降低了自身置信度，使得最终输出结果偏差不显著，数据稳定。

本方案还提供一种用于股道内列车车辆高精度检测定位的系统，用于实现上述方案的用于地沟内机车检测定位方法，包括：

采集模块，用于获取雷达定位数据和底盘定位数据；

计算模块，用于对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；

融合模块，用于将高精度定位结果和高频率定位结果进行动态加权融合得到最终定位结果，其中动态加权融合过程包括：在雷达里程计间隔内，用底盘里程计加权插值。

进一步优化方案为，所述采集模块包括：运行在待检机车与沟道之间进行数据采集的移动设备；

所述移动设备装有多个光学传感器、距离传感器、IMU传感器和驱动编码器；

所述光学传感器和距离传感器用于获取移动设备的雷达定位数据；

所述IMU传感器和驱动编码器用于获取移动设备的底盘定位数据。

本方案采用多种传感器及多种数据融合算法，利用传感器以及算法的不同特征进行动态加权插值的方法，最终得到在长距离运行环境下的高精度高频率的最终定位结果，其中多种传感器包括三维激光雷达、IMU和车轮编码器等可形成2维或3维位姿数据的光学传感器和能输出底盘位姿传感器组合，主要涉及移动底盘的运动学解算算法、IMU数据滤波算法、三维点云数据滤波算法和动态加权差值算法；

光学传感器随着移动设备对沟道顶部开口方向进行扫描，获取沟道外部的特征，以此获取移动设备的高精度定位信息即雷达定位信息；光学传感器包括且不限于可见光，激光，红外光等可形成2维或3维投影视图的光学传感器。

同时包括传感器和车轮驱动编码器用于获取高频定位数据即底盘定位数据，在对雷达定位信息和底盘定位数据经过处理后，最终通过动态加权算法，两种定位数据，得到高精度高频率的定位数据输出得到最终定位结果。

对于容易被突发干扰的传感器，其输出值经过动态加权算法和动态加权融合方法中的置信度计算后决定是否采用。

置信度计算方法包括数值异常突变，其输出值应被剔除。多个不同传感器值进行数学统计，根据其他传感器持有的定位数据置信度确定其置信度。采用2维或3维数据集进行统计解算的，根据历史结果置信度，以及其结果是否在精度要求以内，动态得出最终置信度。对于最终定位结果的输出值，其权值减小幅度不仅取决于行驶总距离，同时考虑高精度定位数据的置信度以及两定位数据的历史数据匹配度。同时，根据高精度定位信息的原始数据，提取沟道两侧距离信息，用以稳定运动姿态，保证高精度定位信息在长时间运行环境下的可靠性，减少高精度定位信息突变。

在高精度定位信息不定频率发布，同时高频率定位信息定频发布。在高精度定位信息发布时，计算此时高精度定位信息与高频率定位信息匹配度，将匹配差异动态加权地更新到最终输出的最终定位结果上，同时根据此时高精度定位信息置信度以及此时高频定位信息权值，动态更新高频率定位信息置信度。

本发明与现有技术相比，具有如下的优点和有益效果：

本发明提供的一种用于股道内列车车辆高精度检测定位的方法及系统, 在雷达里程计间隔内，用底盘里程计加权插值，以提高定位数据频率，降低了雷达里程计数据的跳变，同时在雷达里程计更新后，消除了部分底盘里程计的累计误差；最终得到了一个在长距离环境下，高频高精度的最终定位结果。本发明提供了一种新的技术构思：通过将两类定位结果融合计算的技术构思，来解决在长距离狭窄沟道运行环境下仍能保持高精度高频率的定位结果的问题，为机车检测技术提供更准确的数据基础。

附图说明

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

图1为用于地沟内机车检测定位方法流程示意图；

图2为动态加权融合处理原理示意图；

图3为高精度定位结果获取流程示意图；

图4为高频率定位结果获取流程示意图；

图5为最终定位结果获取流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

在实际机车检测过程中，由于地沟内两侧以及地面特征不明显，无法使用先验地图方案进行定位，传统定位方法单一采用底盘传感器组提供的定位数据，存在累计误差问题，无法满足长距离运行应用场景；同时，沟道上方开口范围内物体存在不定时的较大变动，也无法满足先验地图定位的使用场景。本发明提供以下实施例解决上述问题：

实施例1

本实施例提供一种用于股道内列车车辆高精度检测定位的方法，如图1所示，包括：

获取雷达定位数据和底盘定位数据；

对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；

将高精度定位结果和高频率定位结果进行动态加权融合处理得到最终定位结果，其中动态加权融合处理包括：在雷达里程计间隔内，用底盘里程计加权插值。本方法将高频率的底盘定位数据和高精度的雷达定位数据，通过动态加权算法进行融合，最终得到在长距离运行环境下仍能保持高精度高频率的定位结果，逻辑原理如图2所示。

所述雷达定位数据和底盘定位数据的获取方法包括：

使移动设备运行在待检机车与沟道之间进行数据采集：所述移动设备装有多个光学传感器、距离传感器、IMU传感器和驱动编码器；所述光学传感器和距离传感器用于获取移动设备的雷达定位数据；所述IMU传感器和驱动编码器用于获取移动设备的底盘定位数据。

光学传感器对沟道顶部开口方向进行扫描获取沟道外部的特征，根据特征的变化计算设备的高精度移动速度，其中光学传感器包括可见光，激光，红外光等可形成2维或3维投影视图的传感器。距离传感器通过对两侧墙壁的距离测量，保证移动设备始终位于沟道中间位置。所述距离传感器包括且不限于超声波，激光，红外等用于测距的传感器类型；距离传感器通过对两侧墙壁的距离测量，保证移动设备始终位于沟道中间位置。所述距离传感器包括超声波，激光，红外等用于测距的传感器类型；

IMU传感器可以获取移动设备的实时加速度信息，IMU传感器指能够获取实时加速度的各种传感器类型，通过加速度积分可以获得高速变化的移动速度，结合电机驱动的编码器输出可以计算移动设备的驱动轮转速，进一步的可以通过转速获得高速变化的移动速度用以融合运算高频定位数据得到底盘定位数据。

如图3所示，高精度定位结果获取方法包括：

S1，对雷达定位数据（对应附图中的雷达数据）进行基本滤波预处理得到预处理数据；

S2，基于预处理数据维护雷达定位数据内的线面特征与移动设备之间的位姿变化估计出雷达位姿数据，同时对雷达位姿数据中当前视野内的线面特征与历史数据库中的线面特征进行匹配，进而更新雷达定位数据得到高精度定位结果。其中历史数据库提供了高精度参数考数据。具体的S2包括过程：

以维护局部的实时三维点云地图的方式，基于线面匹配算法和帧间匹配算法实时更新雷达定位数据；其中，以单帧数据形式进行帧间匹配算法，寻找出两帧数据间的重合部分用来更新雷达定位数据。在此过程中，雷达定位数据更新时，加权的更新快速定位数据和快速定位数据的置信度，其中，权值取值条件包括：特征匹配度，特征偏移量、新增特征数量和消除特征数量等；置信度取值包括：帧间数据重合度和单帧特征量等。

雷达位姿数据估计方法包括：

以单帧点云数据形式对预处理数据进行线面特征提取得到面特征点集和线特征点集；其中当单帧点云数据内的面特征点数量小于阈值U时，舍弃本帧计算结果，并缓存当前帧数据用以和下一帧数据合并计算；

比较两帧数据面特征点集和线特征点集的差异从而估计出雷达位姿数据。

面特征点集和线特征点集的提取方法包括：

从单帧点云数据中按数据编号取连续的点A、点B、点C、点D和点E；

取点C计算点C与其他4个点构成空间向量和的长度L，

；

如果L小于阈值M，则认为点C属于面特征点集，阈值M根据曲面的曲率确定；

当存在任一点f(x,y,z)，点f-2在点f-3、点f-4、点f-1和点f形成的曲面T1上，点f+2在点f+3、点f+4、点f+1和点f形成的曲面T2上；计算两曲面的共面向量点乘结果K，若丨K丨小于阈值G，则认为点f属于线特征点集；阈值G根据两面夹角度数确定；

。

实施例2

在上一实施例的基础上，本实施例根据移动设备运动底盘方案，将驱动轮编码器数据和IMU传感器数据通过运动学解算，得到高频定位数据，其中位置微分数据由驱动轮编码值获取，姿态数据由车轮编码值解算和IMU姿态数据融合获取。高频定位数据权值维持在较高置信度，同时高精度定位数据的剧烈偏差降低了自身置信度，使得最终输出结果偏差不显著，数据稳定。

具体的，在不同的运动地盘方案环境下，采用相应的运动学解析，获取底盘定位数据。其中，姿态数据融合方式采用动态加权方式。具体的高频率定位结果获取方法包括：基于底盘定位数据获取IMU姿态数据，并以IMU姿态数据作为高频率定位结果；具体原理如图4所示；

在更新雷达定位数据时，基于雷达位姿数据（即图中雷达姿态数据）与底盘姿态数据的差异，动态加权的更新到IMU姿态数据和IMU姿态数据置信度上；其中加权值的取值条件包括：雷达位姿数据与底盘姿态数据的差异和底盘姿态数据的置信度。

动态加权的过程遵循逻辑：

底盘姿态置信度与累计角度变化量成反比，当累计角度变化量达到设定阈值，根据IMU姿态数据置信度动态更新底盘姿态数据的权值；

IMU姿态数据与底盘姿态数据的差异变化曲率与IMU姿态数据置信度成反比。

实施例3

在上述实施例的基础上，本实施例提供最终定位结果获取方法包括：

雷达里程计更新输出为高精度定位结果，底盘里程计更新输出为高频率定位结果；

在雷达里程计更新后，加权地将雷达里程计与底盘里程计的定位结果差值，更新到最终定位结果上，在下一次雷达里程计数据更新前，底盘里程计持有最终定位结果的更新权，当雷达里程计再次更新时，再次加权更新雷达里程计与底盘里程计的定位结果差值到底盘里程计上。具体如图5所示，描述了高精度定位数据与高频率定位数据融合过程，以及运算过程中各数据更新时机。

其中底盘里程计的计算过程包括：

其中x表示里程计的x坐标，正向为底盘里程计开始工作时，移动设备前进方向；y表示里程计y坐标，正向为底盘里程计开始工作时，为移动设备前进方向；V _L 表示移动设备左轮转速；V _R 表示移动设备右轮转速；r表示移动设备与坐标原点的连线的长度；

表示底盘里程计坐标系下的移动设备偏航角；d为移动设备两轮间距；

计算两航向角偏差：

其中

表示IMU坐标系下，移动设备偏航角；

统计当前偏差率：

其中n表示历史偏差数据编号；i表示前检测数据编号。

当偏差率

大于阈值时，分别计算里程计航向角突变幅度与IMU航向角突变幅度。取当前数据与前两帧数据用于判断，建立航向角-帧编号直角坐标系，将前两帧数据分别指向当前数据的向量点乘结果作为突变值，突变幅度E计算公式为：

当某一数据突变超出阈值，舍弃其本次数据。并采用间隔下采样的方式，分别判当前数据相对于历史数据的突变情况。

其中q表示数据下采样间隔，为整数

特殊的，当两航向角突变均超出阈值，且连续突变次数超出阈值，算法报出警告，建议移动设备暂停。

一般的，最终航向角输出为：

为里程计偏航角置信度:

其中k表示置信度系数，根据实际里程计稳定性和精度调整，里程计原稳定，k值应越小，k∈[0,1]。

为累计航向偏差:

其中

表示里程计航向角偏差微分值。

当

小于阈值，时更新里程计偏航角等于IMU偏航角。

实施例1和实施例2获取的上述两种定位数据（高精度定位结果和高频率定位结果）后分别存在以下弊端。精度定位结果频率低，且受环境影响较大，在运行过程中，如果视野内存在运动物体，会使得定位数据突变，将大大降低此定位数据的稳定性，会产生较大定位偏差；高频率定位结果存在累计误差问题，无法在长距离运行环境下保证满足应用需求的精度。

因此本实施例在低频的雷达里程计间隔内，用底盘里程计加权插值，以提高定位数据频率，降低雷达里程计数据的跳变，同时在雷达里程计更新后，消除了部分底盘里程计的累计误差。最终得到了一个在长距离环境下，高频高精度的定位数据。

具体的，当雷达里程计更新后，加权地将雷达里程计与底盘里程计的定位数据差值，更新到最终定位数据上，在下一次雷达里程计数据更新前，底盘里程计持有最终输出结果更新权，当雷达里程计再次更新时，再次加权更新两里程计差值到底盘里程计上，以此迭代。

其中，最终输出结果更新方法为，根据雷达里程计置信度，分配一定百分比的两里程计差值与当前底盘里程计进行坐标变换，得出最终定位结果。

其中P为最终定位输出矩阵，

为底盘里程计定位输出矩阵，

为两里程计定位偏差查，

为雷达里程计置信度。

雷达里程计置信度运算方法为，根据底盘里程计置信度，当底盘里程计置信度在一定区间时，记录两里程计数据差异变化率，雷达里程计置信度与差异变化率成反比。其中，底盘里程计置信度与其记录总里程成反比，当底盘里程计置信度低于阈值时，更新底盘里程计为雷达定位结果。

其中，雷达里程计置信度计算公式如下：

其中

为某次雷达里程计数据偏差率；i为当前雷达里程计偏差率数据编号；q为下采样间隔，为整数。

偏差率计算公式如下：

其中

为两里程计偏差度；n为统计偏差度数据编号，j为当前偏差度数据编号，

为统计数据量；

偏差度计算公式如下：

其中X _ODOM、 Y _ODOM、 Z _ODOM分别为底盘里程计三坐标数据，X _LIDAR、 Y _LIDAR、 Z _LIDAR分别为底盘里程计三坐标数据；

坐标偏差计算公式如下：

其中X _ODOM、 Y _ODOM、 Z _ODOM分别为底盘里程计三坐标数据，X _LIDAR、 Y _LIDAR、 Z _LIDAR分别为底盘里程计三坐标数据

特殊的，当底盘里程计置信度低于阈值时，根据当前雷达里程计置信度，按一定百分比更新底盘里程计置信度。同时，根据当前雷达里程计置信度，按一定百分比将两里程计差值与底盘里程计做坐标变换，更新地盘里成绩数据。

其中，底盘里程计置信度为：

其中dP_ODOM为里程计数据微分量，k_ODOM为置信度系数，根据实际里程计稳定性和精度调整，里程计原稳定，k值应越小，k∈[0,1]。

当底盘里程计置信度低于阈值时，重置底盘里程计置信度，更新底盘里程计坐标，公式如下：

其中P_ODOM为里程计数据，

为两里程计定位偏差查，C_LIDAR为雷达里程计置信度。

具体的，最终定位结果运算逻辑图所示，描述了高精度定位数据与高频率定位数据融合过程，以及运算过程中各数据更新时机。

在实际运行过程中，移动设备进入狭长沟道后，同步高精度定位数据（雷达定位信息）与高频率定位数据（底盘定位信息）的数值，同时初始化所有算法参数。在此期间高精度里程计建立了初始的局部三维地图并开始进行维护，同时高频率定位数据初始化完成。当移动设备开始进行，高精度定位数据根据沟道顶部开口外的特征进行高精度定位，同时高频定位数据稳定最终定位结果的输出频率。当出现移动异物造成高精度定位数据发成偏差时，由于高频定位数据权值维持在较高置信度，同时高精度定位数据的剧烈偏差降低了自身置信度，使得最终输出结果偏差不显著，数据稳定。当移动设备运行较长距离时，由于高频里程计的数据被高精度的定位数据定时整定，使得在长距离环境下，最终输出结果累计误差不显著，数据稳定。

实施例4

本实施例提供一种用于股道内列车车辆高精度检测定位的系统，用于实现上述实施例1-3所述的用于地沟内机车检测定位方法，包括：

采集模块，用于获取雷达定位数据和底盘定位数据；采集模块包括：运行在待检机车与沟道之间进行数据采集的移动设备；移动设备装有多个光学传感器、距离传感器、IMU传感器和驱动编码器；光学传感器和距离传感器用于获取移动设备的雷达定位数据；IMU传感器和驱动编码器用于获取移动设备的底盘定位数据。

计算模块，用于对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；

融合模块，用于将高精度定位结果和高频率定位结果进行动态加权融合得到最终定位结果，其中动态加权融合过程包括：在雷达里程计间隔内，用底盘里程计加权插值。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种用于股道内列车车辆高精度检测定位的方法，其特征在于，包括：

获取雷达定位数据和底盘定位数据；

对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；

将高精度定位结果和高频率定位结果进行动态加权融合处理得到最终定位结果，其中动态加权融合处理包括：在雷达里程计间隔内，用底盘里程计加权插值；所述高精度表示精度范围≤±3cm，所述高频率表示频率至少为50hZ。

2.根据权利要求1所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，所述雷达定位数据和底盘定位数据的获取方法包括：

使移动设备运行在待检机车与沟道之间进行数据采集：所述移动设备装有多个光学传感器、距离传感器、IMU传感器和驱动编码器；所述光学传感器和距离传感器用于获取移动设备的雷达定位数据；所述IMU传感器和驱动编码器用于获取移动设备的底盘定位数据。

3.根据权利要求2所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，所述高精度定位结果获取方法包括：

S1，对雷达定位数据进行基本滤波预处理得到预处理数据；

S2，基于预处理数据维护雷达定位数据内的线面特征与移动设备之间的位姿变化估计出雷达位姿数据，同时对雷达位姿数据中当前视野内的线面特征与历史数据库中的线面特征进行匹配，进而更新雷达定位数据得到高精度定位结果；包括过程：以维护一张局部的实时三维特征地图的方式，基于线面匹配算法和帧间匹配算法实时更新雷达定位数据；其中，以单帧数据形式进行帧间匹配算法，寻找出两帧数据间的重合部分用来更新雷达定位数据。

4.根据权利要求3所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，雷达位姿数据估计方法包括：

以单帧点云数据形式对预处理数据进行线面特征提取得到面特征点集和线特征点集；其中当单帧点云数据内的面特征点数量小于阈值U时，舍弃本帧计算结果，并缓存当前帧数据用以和下一帧数据合并计算；

比较两帧数据面特征点集和线特征点集的差异从而估计出雷达位姿数据。

5.根据权利要求4所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，面特征点集和线特征点集的提取方法包括：

从单帧点云数据中按数据编号取连续的点A、点B、点C、点D和点E；

取点C计算点C与其他4个点构成空间向量和的长度L，

；

如果L小于阈值M，则认为点C属于面特征点集，阈值M根据曲面的曲率确定；

当存在任一点f(x,y,z)，点f-2在点f-3、点f-4、点f-1和点f形成的曲面T1上，点f+2在点f+3、点f+4、点f+1和点f形成的曲面T2上；计算两曲面的共面向量点乘结果K，若丨K丨小于阈值G，则认为点f属于线特征点集；阈值G根据两面夹角度数确定；

。

6.根据权利要求5所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，高频率定位结果获取方法包括：

基于底盘定位数据获取IMU姿态数据，并以IMU姿态数据作为高频率定位结果；

在更新雷达定位数据时，基于雷达位姿数据与底盘姿态数据的差异，动态加权的更新到IMU姿态数据和IMU姿态数据置信度上；其中加权值的取值条件包括：雷达位姿数据与底盘姿态数据的差异和底盘姿态数据的置信度。

7.根据权利要求6所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，动态加权的过程遵循逻辑：

底盘姿态置信度与累计角度变化量成反比，当累计角度变化量达到设定阈值，根据IMU姿态数据置信度动态更新底盘姿态数据的权值；

IMU姿态数据与底盘姿态数据的差异变化曲率与IMU姿态数据置信度成反比。

8.根据权利要求1所述的一种用于股道内列车车辆高精度检测定位的方法，其特征在于，最终定位结果获取方法包括：

雷达里程计更新输出为高精度定位结果，底盘里程计更新输出为高频率定位结果；

在雷达里程计更新后，加权地将雷达里程计与底盘里程计的定位结果差值，更新到最终定位结果上，在下一次雷达里程计数据更新前，底盘里程计持有最终定位结果的更新权，当雷达里程计再次更新时，再次加权更新雷达里程计与底盘里程计的定位结果差值到底盘里程计上。

9.一种用于股道内列车车辆高精度检测定位的系统，其特征在于，用于实现权利要求1-8任一所述的用于股道内列车车辆高精度检测定位的方法，包括：

采集模块，用于获取雷达定位数据和底盘定位数据；

计算模块，用于对雷达定位数据进行快速度估计位姿更新处理得到高精度定位结果，同时对底盘定位数据进行动态加权更新处理得到高频率定位结果；

融合模块，用于将高精度定位结果和高频率定位结果进行动态加权融合得到最终定位结果，其中动态加权融合过程包括：在雷达里程计间隔内，用底盘里程计加权插值。

10.根据权利要求9所述的一种用于股道内列车车辆高精度检测定位的系统，其特征在于，所述采集模块包括：运行在待检机车与沟道之间进行数据采集的移动设备；

所述移动设备装有多个光学传感器、距离传感器、IMU传感器和驱动编码器；

所述光学传感器和距离传感器用于获取移动设备的雷达定位数据；

所述IMU传感器和驱动编码器用于获取移动设备的底盘定位数据。

Images