一种带有轨道约束的激光雷达定位系统及方法技术方案

本发明专利技术请求保护一种带有轨道约束的激光雷达定位系统及方法。该系统由数据采集模块，预处理模块，轨道数据接口模块，定位模块组成。数据采集模块接收激光雷达获得的点云数据，并对来自多个激光雷达的点云进行融合。预处理模块接收融合后的点云数据，根据场景特征筛选感兴趣区域，并通过点云滤波除去杂波。轨道数据接口模块读取预先存储的轨道地理信息点集，根据点集拟合出轨道约束方程。定位模块接收预处理后的点云和轨道约束方程，基于粒子滤波算法计算位置，通过限制粒子仅分布于轨道约束方程上，缩小粒子分布区域，得到更准确定位结果。该系统能有效改善矿井、隧道、车间等环境下轨道运输车辆定位精度，实现轨道运输车辆和其他设备的准确协同。备的准确协同。备的准确协同。

【技术实现步骤摘要】
一种带有轨道约束的激光雷达定位系统及方法


[0001]本专利技术属于智能移动平台定位与导航领域，具体涉及一种带有轨道约束的激光雷达定位系统。

技术介绍

[0002]定位技术是多个应用领域的关键技术之一，其中室内定位技术主要包括基于超带宽(UWB Ultra Wideband)、射频识别(RFID Radio Frequency Identification)等无线通信技术方案，也有基于激光雷达、视觉的方案。在工业现场及矿井等复杂场景下，具有烟尘大、电磁干扰严重，光线不足等特点，而激光雷达作为一种主要的感知传感器，以其精度高、抗干扰能力强的优势在自主无人系统的定位功能中具有重要作用。
[0003]中国专利申请：一种适用于轨道机器人的定位系统和方法(申请号：CN201810594133.4)，公开了一种适用于轨道机器人的定位方法，其特征在于，包括：利用机器人上搭载光电传感器和增量式编码器，在轨道上设置固定位置的码板实现对轨道机器人位置信息的矫正。该方法需布置大量码板，且没有利用轨道空间位置信息。
[0004]中国专利申请：车载定位器和包括该车载定位器的矿用机车(申请号：CN201610648150.2)，公开了一种车载定位器，使用超宽带技术进行定位。该方法会随矿井长度增加从而需要增加更多的超带宽标签，且没有利用轨道空间位置信息。
[0005]中国专利申请：一种基于车号系统的列车定位系统及相应的方法(申请号：CN 201910980197.2)，公开了一种基于车号系统的列车定位系统及相应的方法，使用RFID技术测定列车在线路上的位置。该方法每节车厢都需要一个标签，并且没有利用轨道空间位置信息。
[0006]轨道车辆在工业领域具有重要作用，但以上方法没有有效利用轨道的先验信息，信息利用不够充分，从而影响定位效果。

技术实现思路

[0007]本专利技术旨在解决以上现有技术的问题。提出了一种带有轨道约束的激光雷达定位系统及方法。本专利技术的技术方案如下：
[0008]带有轨道约束的激光雷达定位系统，包括数据采集模块，预处理模块，轨道数据接口模块，定位模块四个模块，其中：
[0009]所述数据采集模块用于获取激光雷达检测的点云数据，并将来自多个激光雷达的点云数据融合后发送给预处理模块；
[0010]所述预处理模块接收融合后的点云数据，根据场景特征筛选感兴趣区域，并对点云滤波除去杂波后发送到定位模块；
[0011]所述轨道数据接口模块读取预先存储的轨道地理信息点集，根据点的坐标顺序拟合出轨道约束方程，并发送到定位模块；
[0012]所述定位模块接收预处理后的点云和轨道约束方程，基于粒子滤波算法计算位
置，通过限制粒子分布于轨道约束方程上从而缩小粒子分布区域，并输出准确定位结果。
[0013]一种带有轨道约束的激光雷达定位方法，其步骤包括：
[0014](1)数据采集：获取来自n个激光雷达的点云数据(P1，
…
，P
n
)；
[0015](2)点云融合：点云来自于多个不同的激光雷达，根据预先标定的雷达之间的旋转平移参数，将多个雷达的点云变换到移动平台固联坐标系下，并形成融合点云P
′
；
[0016](3)点云预处理：接收融合后的点云P
′
，根据不同场景筛选感兴趣区域，并对p
′
滤波除去杂波后得到预处理后的点云P，发送到定位模块；
[0017](4)轨道约束生成：读取预先存储的轨道地理信息点集Z
c
，并变换到坐标系O
‑
XYZ下，按照点集坐标顺序拟合生成轨道约束方程L
c
；
[0018](5)定位计算：接收经预处理后的点云P以及轨道约束方程L
c
，计算并得到定位结果X
result
＝[x，y，θ]T
。
[0019]进一步的，所述定位计算方法步骤(5)的详细流程如下：
[0020](1)粒子初始化：n个代表位姿的粒子均匀地分布在约束方程L
c
之上，所有粒子组成粒子集每一粒子的状态为表示k时刻粒子的x坐标，y坐标，航向角，评分，且满足(x
k
，y
k
，θ
k
)∈L
c
；
[0021](2)粒子运动：根据粒子运动方程X
k+1
＝F
m
(X
k
，L
c
)，得到k+1时刻粒子的[x
k+1
，y
k+1
，θ
k+1
]T
；
[0022](3)粒子评分：利用粒子状态X
k+1
将点云P变换到坐标系O
‑
XYZ下，记作P
c
，根据粒子评分方程s
k+1
＝F
s
(P
c
，P
map
)得到粒子k+1时刻评分s
k+1
并对其归一化，其中P
map
为预先读取的三维环境点云地图；
[0023](4)粒子重采样：根据G
o
中每一粒子对应的概率为其评分s
k+1
并构建概率密度函数f
r
，根据f
r
随机取样得到粒子集G
n
并替换G
o
，前后粒子数保持一致；
[0024](5)定位输出：输出粒子集G
n
的几何中心位置，X
result
＝[x，y，θ]T
作为定位结果，其中x，y，θ分别为G
n
中所有粒子x
k+1
，y
k+1
，θ
k+1
的算数平均值；
[0025](6)重复上述步骤(2)至(5)，实现实时定位。
[0026]一种带有轨道约束的激光雷达定位方法
[0027]进一步的，所述定位方法步骤(4)中的拟合轨道约束方程包括：
[0028]以折线拟合轨道，根据Z
c
中每相邻两点坐标得到直线方程L
a
＝y＝kx+b，m个分段方程构成轨道约束方程
[0029]以二次样条函数拟合轨道，根据Z
c
中每相邻三点坐标得到二次样条曲线方程L
b
＝y＝ax2+bx+c，k个分段方程构成轨道约束方程
[0030]进一步的，所述定位计算流程步骤(2)中的粒子运动方程包括：
[0031]如存在轮速，获取轮速信息v，根据X
k+1
＝F
m
＝f
m1
(X
k
，L
c
，v)，得到k+1时刻粒子的状态X
k+1
；
[0032]如不存本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种带有轨道约束的激光雷达定位系统，其特征在于，包括：数据采集模块、预处理模块、轨道数据接口模块及定位模块，其中：所述数据采集模块用于获取激光雷达检测的点云数据，并将来自多个激光雷达的点云数据融合后发送给预处理模块；所述预处理模块用于接收融合后的点云数据，根据场景特征筛选感兴趣区域，并对点云滤波除去杂波后发送到定位模块；所述轨道数据接口模块用于读取预先存储的轨道地理信息点集，根据点集坐标顺序拟合出轨道约束方程，并发送到定位模块；所述定位模块接收预处理后的点云和轨道约束方程，基于粒子滤波算法计算位置，通过限制粒子分布于轨道约束方程上从而缩小粒子分布区域，粒子根据约束方程初始化和移动，并输出准确定位结果。2.根据权利要求1所述的一种带有轨道约束的激光雷达定位系统，其特征在于，所述定位模块基于粒子滤波定位计算方法，通过限制粒子分布于轨道约束方程上从而缩小粒子分布区域，粒子根据约束方程初始化和移动，具体包括：2.1粒子初始化：n个代表位姿的粒子均匀地分布在轨道约束方程L
c
上，组成粒子集每一粒子的状态为表示k时刻粒子的x坐标，y坐标，航向角，评分；2.2粒子运动：根据粒子运动方程X
k+1
＝F
m
(X
k
)，得到k+1时刻粒子的状态X
k+1
，其中F
m
表示轨道约束方程L
c
和粒子位移量组成的方程组；2.3粒子评分：根据粒子状态X
k+1
，将点云P变换到坐标系O
‑
XYZ下为点云P
c
，根据粒子评分方程s
k+1
＝F
s
(P
c
，P
map
)更新粒子评分并对其归一化，其中P
map
为预先读取的环境点云，F
s
表示似然域评分方程组；2.4粒子重采样：根据G
o
中每一个粒子被选中的概率为其评分s
k+1
并构建概率密度函数f
r
，根据该概率密度函数随机取样得到粒子集G
n
替换G
o
；2.5定位输出：输出G
n
的几何中心位置X
result
＝[x，y，θ]
T
作为定位结果；2.6重复上述步骤2.2至2.5，实现实时定位。3.根据权利要求2所述定位计算方法，其特征在于，所述步骤2.2粒子运动：根据粒子运动方程X
k+1
＝F
m
(X
k
)，得到k+1时刻粒子的状态X
k+1
，具体包括：根据轮速信息或点云信息推算粒子运动，得到k+1时刻粒子的状态：a)存在轮速信息时：1)获取k+1时刻的轮速v，以及k+1与k时刻的时间增量Δt，得到粒子位移量Δe...

【专利技术属性】
技术研发人员：岑明，左卫，郭健，洪友芳，岑汝平，
申请(专利权)人：重庆邮电大学，
类型：发明
国别省市：