轨道交通的车辆位姿估计方法和系统技术方案

本发明专利技术提供了轨道交通的车辆位姿估计方法和系统，包括：采集IMU数据和原始点云数据；对IMU数据进行预处理，并利用预处理后的IMU数据对原始点云数据进行运动畸变校正，得到校正后的点云数据；将校正后的点云数据进行过滤，得到过滤后的点云数据；将过滤后的点云数据按照激光线束进行标记，并且基于曲率尺度空间方法提取每帧点云数据的特征点；将过滤后的特征点与上一时刻的点云数据进行帧间配准，将点到线的距离和点到平面的距离作为误差函数；采用非线性优化方法对误差函数计算最优解，从而得到两帧点云数据的位姿变换关系；根据两帧点云数据的位姿变换关系，计算车辆在当前时刻相对于世界坐标系下的位姿。于世界坐标系下的位姿。于世界坐标系下的位姿。

【技术实现步骤摘要】
轨道交通的车辆位姿估计方法和系统


[0001]本专利技术涉及轨道交通
，尤其是涉及轨道交通的车辆位姿估计方法和系统。

技术介绍

[0002]轨道交通系统具有运量大、占地少、单位运量能耗小、污染小和安全等诸多优点，在当前的城市交通系统、区域综合交通系统和国家综合交通运输体系中发挥着重要作用。为了确保城市轨道交通运行的安全性，轨道交通系统的运行控制系统，需要实时掌握列车的位置数据，因此列车精准定位是重要要求之一。列车的安全、实时、精确定位是影响轨道交通发展的重要因素，只有实时了解列车的位置，才能实现较为理想可靠的调度管理，保障其能够发挥出最强作用效能。
[0003]目前，轨道交通车辆的传统定位方法主要依赖轨道电路或位置信标等设备，需要对轨道电路进行后续维护，对信标安装的准确度要求较高。另一方面，传统车载定位技术的累计误差会随着行驶里程的增加而增大，定位效果和精度受打滑或空转影响较大，而只依赖惯性测量技术进行定位，轨道交通车辆定位误差会随时间不断累积。此外，基于卫星导航的列车组合定位技术可以根据不同的环境和条件选择适当的组合方案和信息融合算法，虽然可以提高列车定位的精度和可靠度，但在GNSS信号未覆盖区域无法实现连续定位。

技术实现思路

[0004]有鉴于此，本专利技术的目的在于提供轨道交通的车辆位姿估计方法和系统，结合激光雷达和惯性测量设备等多传感器信息，解决无卫星信号覆盖环境下的轨道交通车辆定位问题，满足精确定位要求。
[0005]第一方面，本专利技术实施例提供了轨道交通的车辆位姿估计方法，所述方法包括：
[0006]采集IMU数据和原始点云数据；
[0007]对所述IMU数据进行预处理，并利用预处理后的IMU数据对所述原始点云数据进行运动畸变校正，得到校正后的点云数据；
[0008]将所述校正后的点云数据进行过滤，得到过滤后的点云数据；
[0009]将所述过滤后的点云数据按照激光线束进行标记，并且基于曲率尺度空间方法提取每帧点云数据的特征点；
[0010]将所述每帧点云数据的特征点进行过滤，得到过滤后的特征点；
[0011]将所述过滤后的特征点与上一时刻的点云数据进行帧间配准，将点到线的距离和点到平面的距离作为误差函数；
[0012]采用非线性优化方法对所述误差函数计算最优解，从而得到两帧点云数据的位姿变换关系；
[0013]根据所述两帧点云数据的位姿变换关系，计算所述车辆在当前时刻相对于世界坐标系下的位姿。
[0014]进一步的，对所述IMU数据进行预处理，并利用预处理后的IMU数据对所述原始点云数据进行运动畸变校正，得到校正后的点云数据，包括：
[0015]消除所述IMU数据的零偏和重力加速度的影响；
[0016]选取任意一帧点云数据作为当前帧点云数据；
[0017]获取所述当前帧点云数据起始时刻的位姿和结束时刻的位姿；
[0018]计算所述结束时刻相对于所述起始时刻的位姿变换；
[0019]根据相对时间插值计算所述当前帧点云数据中的任意点相对所述起始时刻的位姿变换；
[0020]根据所述任意点相对所述起始时刻的位姿变换，计算所述校正后的点云数据。
[0021]进一步的，将所述校正后的点云数据进行过滤，得到过滤后的点云数据，包括：
[0022]从所述校正后的点云数据中去除远距离激光点云数据和可行区域之外的点云数据，并采用随机样本一致性方法进行地面分割；
[0023]对非地面点云数据进行聚类，得到聚类后的点云数据；
[0024]从所述聚类后的点云数据中滤除数量小于预设数量的点云簇，输出所述过滤后的点云数据。
[0025]进一步的，所述特征点包括边缘点和平面点，将所述过滤后的点云数据按照激光线束进行标记，并且基于曲率尺度空间方法提取每帧点云数据的特征点，包括：
[0026]计算当前帧点云数据中每两个相邻点之间的欧氏距离，并从起始点开始对每次计算的所述欧氏距离进行积分，得到各尺度下的曲率；
[0027]以当前点为中心，向前后各取两个点后，得到四个点；
[0028]计算所述四个点与所述当前点的曲率，并且每连续五个连续点进行曲率值比较，从而筛选出候选边缘点；
[0029]将所述当前帧点云数据内的每条线束分成n段区域；
[0030]从每段区域中提取两个所述边缘点和四个所述平面点；
[0031]选择所述每段区域的候选边缘点中曲率最大的两个点作为所述边缘点，所述曲率小于设定阈值的四个点作为所述平面点。
[0032]进一步的，将所述过滤后的特征点与上一时刻的点云数据进行帧间配准，将点到线的距离和点到平面的距离作为误差函数，包括：
[0033]利用KD树搜索到上一帧点云数据中距离边缘点最近的点j，并根据所述点j搜索到次近点l；
[0034]根据所述点j和所述点l得到第一线段jl；
[0035]计算所述边缘点到第一线段jl的第一距离；
[0036]利用所述KD树搜索到所述上一帧点云数据中距离平面点最近的点f；
[0037]获取与所述最近的点f相同扫描线束的最近点c；
[0038]搜索相邻帧中与点f最相近的点m；
[0039]根据点f、点c和点m得到第一平面；
[0040]计算所述平面点到所述第一平面的第二距离；
[0041]将所述第一距离和所述第二距离作为所述误差函数。
[0042]进一步的，采用非线性优化方法对所述误差函数计算最优解，从而得到两帧点云
数据的位姿变换关系，包括：
[0043]采用列文伯格
‑
马夸尔特法进行非线性优化，求解损失函数的雅克比矩阵；
[0044]采用链式求导法则求解偏导数；
[0045]选取轮速测量单元提供的初始位姿作为位姿初始值，初始化迭代步长；
[0046]根据第m次迭代，计算目标函数，并定义指标参数；
[0047]当所述指标参数大于设置阈值时，增大所述步长；
[0048]当所述指标参数小于所述设置阈值时，减小所述步长；
[0049]重新计算所述目标函数，直至所述指标参数大于所述设置阈值时更新位姿；
[0050]重复以上步骤直至达到收敛次数。
[0051]第二方面，本专利技术实施例提供了轨道交通的车辆位姿估计系统，所述系统包括：惯性测量设备、激光雷达和位姿计算单元；
[0052]所述惯性测量设备，用于采集车辆的IMU数据，其中，IMU数据包括加速度、角速度和姿态角信息；
[0053]所述激光雷达，用于测量所述车辆周围环境的原始点云数据；
[0054]所述位姿计算单元，用于对所述IMU数据进行预处理，并利用预处理后的IMU数据对所述原始点云数据进行运动畸变校正，得到校正后的点云数据；将所述校正后的点云数据进行过滤，得到过滤后的点云数据；将所述过滤后的点云数据按照激光线束进行标记，并且基于曲率尺度空间方法提本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轨道交通的车辆位姿估计方法，其特征在于，所述方法包括：采集IMU数据和原始点云数据；对所述IMU数据进行预处理，并利用预处理后的IMU数据对所述原始点云数据进行运动畸变校正，得到校正后的点云数据；将所述校正后的点云数据进行过滤，得到过滤后的点云数据；将所述过滤后的点云数据按照激光线束进行标记，并且基于曲率尺度空间方法提取每帧点云数据的特征点；将所述每帧点云数据的特征点进行过滤，得到过滤后的特征点；将所述过滤后的特征点与上一时刻的点云数据进行帧间配准，将点到线的距离和点到平面的距离作为误差函数；采用非线性优化方法对所述误差函数计算最优解，从而得到两帧点云数据的位姿变换关系；根据所述两帧点云数据的位姿变换关系，计算所述车辆在当前时刻相对于世界坐标系下的位姿。2.根据权利要求1所述的轨道交通的车辆位姿估计方法，其特征在于，对所述IMU数据进行预处理，并利用预处理后的IMU数据对所述原始点云数据进行运动畸变校正，得到校正后的点云数据，包括：消除所述IMU数据的零偏和重力加速度的影响；选取任意一帧点云数据作为当前帧点云数据；获取所述当前帧点云数据起始时刻的位姿和结束时刻的位姿；计算所述结束时刻相对于所述起始时刻的位姿变换；根据相对时间插值计算所述当前帧点云数据中的任意点相对所述起始时刻的位姿变换；根据所述任意点相对所述起始时刻的位姿变换，计算所述校正后的点云数据。3.根据权利要求1所述的轨道交通的车辆位姿估计方法，其特征在于，将所述校正后的点云数据进行过滤，得到过滤后的点云数据，包括：从所述校正后的点云数据中去除远距离激光点云数据和可行区域之外的点云数据，并采用随机样本一致性方法进行地面分割；对非地面点云数据进行聚类，得到聚类后的点云数据；从所述聚类后的点云数据中滤除数量小于预设数量的点云簇，输出所述过滤后的点云数据。4.根据权利要求1所述的轨道交通的车辆位姿估计方法，其特征在于，所述特征点包括边缘点和平面点，将所述过滤后的点云数据按照激光线束进行标记，并且基于曲率尺度空间方法提取每帧点云数据的特征点，包括：计算当前帧点云数据中每两个相邻点之间的欧氏距离，并从起始点开始对每次计算的所述欧氏距离进行积分，得到各尺度下的曲率；以当前点为中心，向前后各取两个点后，得到四个点；计算所述四个点与所述当前点的曲率，并且每连续五个连续点进行曲率值比较，从而筛选出候选边缘点；
将所述当前帧点云数据内的每条线束分成n段区域；从每段区域中提取两个所述边缘点和四个所述平面点；选择所述每段区域的候选边缘点中曲率最大的两个点作为所述边缘点，所述曲率小于设定阈值的四个点作为所述平面点。5.根据权利要求1所述的轨道交通的车辆位姿估计方法，其特征在于，将所述过滤后的特征点与上一时刻的点云数据进行帧间配准，将点到线的距离和点到平面的距离作为误差函数，包括：利用KD树搜索到上一帧点云数据中距离边缘点最近的点j，并根据所述点j搜索到次近点l；根据所述点j和所述点l得...

【专利技术属性】
技术研发人员：吴莹莹，袁魁，朱永辉，李国龙，梁霄，
申请(专利权)人：上海富欣智能交通控制有限公司，
类型：发明
国别省市：