一种融合路侧稀疏点云和道路特征的车辆位姿估计方法技术

本发明专利技术涉及车辆跟踪技术领域，公开了一种融合路侧稀疏点云和道路特征的车辆位姿估计方法，进行了粗配准和精配准，使用两阶段配准提高准确率，使本发明专利技术既有无模型方法鲁棒性好的优点，又有模型方法精度高的优点，解决了稀疏点云位姿估计鲁棒性和准确度不高的问题。并且在进行粗配准时，利用了地面点云的法向量，使用环境特征代替目标车辆点云特征作为约束，环境特征不随目标车辆和雷达间距变化，提高了算法的鲁棒性，解决了稀疏点云车辆特征少、车辆特征随距离变化大的问题。另外，本发明专利技术实施例使用一个目标车辆全视角稠密点云模板代替目标车辆多视角稠密点云库，降低了模型存储成本和搜索时间成本，实现了在相同运行速度下计算设备成本的降低。算设备成本的降低。算设备成本的降低。

【技术实现步骤摘要】
一种融合路侧稀疏点云和道路特征的车辆位姿估计方法


[0001]本专利技术涉及车辆跟踪
，特别是涉及一种融合路侧稀疏点云和道路特征的车辆位姿估计方法。

技术介绍

[0002]精确的车辆位姿估计可以评估车辆的行驶状态，预警不正常的车辆行驶状态。目前的车辆位姿估计按照传感器类型主要可以分为基于摄像头和基于激光雷达的方法，基于摄像头的车辆位姿估计存在较大误差，无法满足估计车辆的行驶状态的要求。基于激光雷达的车辆位姿估计可以分为有模型的方法和无模型的方法。
[0003]有模型的方法首先需要提前建立目标对象多视角下的点云模板库，然后计算目标点云的特征，通过搜索算法从模板库中找到与目标点云相似视角的模板点云，最后通过配准检测点云和模板点云转换得到位姿。首先建立完备的目标对象的模板库耗时耗力，存储要求大，而且更新困难，如果模板库没有相似视角的模板点云可能会导致后续配准过程陷入局部最优解，最终求解失败；其次搜索模板点云需要准确的特征衡量目标点云和模板点云之间的相似性，而准确的特征需要稠密的点云进行计算，这会提高激光雷达的硬件要求，从而大大增加布设的成本，不利于大范围推广；最后对模板库进行邻近搜索需要时间，矛盾点在于一个完备的目标模板库有更多视角的目标点云，可以降低配准求解失败的概率但是会增加邻近搜索的时间，一个不完备的目标模板库有更少的目标点云，可以减少邻近搜索的时间但是增加配准求解失败的概率，从而出现鲁棒的方法缺少实时性，实时的方法鲁棒性不好的情况。
[0004]无模型的方法首先计算聚类得到的点云的包围盒，然后计算包围盒的位姿从而计算目标点云的位姿。首先检测的点云由于遮挡一定是不完整的，对一个不完整的点云计算包围盒很容易出现包围盒比实际缺少一部分的情况，直接导致位移量估计错误，进一步导致角度偏移量的错误；其次检测点云依赖于分割聚类算法的效果，如果分割聚类有偏差将直接导致包围盒计算错误，从而导致姿态估计错误。所以无模型的方法一般准确性较差。
[0005]现有技术公开了一种基于路侧雷达长距离多车辆高精度检测追踪方法，包括：获取第一雷达的第一点云数据；根据第一雷达的第一点云数据进行聚类识别，确定至少一个第一识别目标；当所述至少一个识别目标进入到第一雷达和第二雷达之间的重叠区域，获取第二雷达的第二点云数据，根据所述第二点云数据进行聚类识别，确定至少一个第二识别目标；将所述至少一个第二识别目标进行坐标转换后，与所述至少一个第一识别目标进行位姿比较和特征矩阵比较，确定是否属于同一追踪目标。该专利在位姿识别时，是通过聚类识别的，如果分割聚类有偏差将直接导致包围盒计算错误，从而导致姿态估计错误。所以无模型的方法一般准确性较差。

技术实现思路

[0006]本专利技术的目的是提供一种鲁棒性好、准确率高的融合路侧稀疏点云和道路特征的
车辆位姿估计方法。
[0007]为了实现上述目的，本专利技术提供了一种融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，包括如下步骤：
[0008]S1：获取路侧激光雷达采集到的实时点云数据；
[0009]S2：提取步骤S1得到的点云数据中位于车辆行驶区域的点云；
[0010]S3：拟合地面，根据拟合地面，将车辆行驶区域的点云分为地面点云和地上点云；
[0011]S4：将地上点云进行聚类，得到目标车辆点云聚类；
[0012]S5：估计地面点云的法向量，得到地面点云法向量[GN
x
,GN
y
,GN
z
]；
[0013]S6：将步骤S4得到的目标车辆点云和步骤S5得到的地面点云法向量通过主成分分析法进行粗配准，得到粗配准矩阵和粗配准后的目标点云；
[0014]S7：将粗配准后的目标点云和点云模板进行精配准，得到精配准矩阵；
[0015]S8：将步骤S6得到的粗配准矩阵和步骤S7得到的精配准矩阵相乘，得到最终配准矩阵；
[0016]S9：根据步骤S8得到的最终配准矩阵转换成位姿表达式。
[0017]具体地，在步骤S2中，在提取位于车辆行驶区域的点云时，通过距离信息在步骤S1得到的点云数据中提取。
[0018]具体地，距离信息包括点云坐标的x轴和y轴信息，将x轴坐标和y轴坐标在车辆行驶区域范围的点云进行提取。
[0019]具体地，在步骤S3中，使用RANSAC算法拟合地面。
[0020]具体地，在步骤4中，使用自适应欧式聚类方法地上点云进行聚类。
[0021]具体地，步骤S6包括：
[0022]S6.1：将步骤S4得到的目标车辆点云聚类PC
c
∈R3×
N
作为目标点云；
[0023]S6.2：计算目标点云的中心点坐标p
center
；
[0024]S6.3：计算目标点云最大特征值对应的特征向量u
′1；
[0025]S6.4：通过下式计算旋转转换矩阵：
[0026]u2＝Normalized(EC
×
u
′1)
[0027]u1＝u2×
EC
[0028][0029]其中Normalized(
·
)表示向量标准化；
[0030]S6.5：用下式计算平移转换矩阵：
[0031]T
ECPC
＝R
ECPC
·
p
center
；
[0032]S6.6：结合旋转转换矩阵和平移转换矩阵得到配准矩阵，
[0033][0034]具体地，在步骤S6.3中，使用SVD分解计算目标点云最大特征值对应的特征向量u
′1。
[0035]具体地，在步骤S7中，点云模板采用目标车辆的一个全视角的稠密点云模板。
[0036]具体地，在步骤S7中，使用迭代最近点算法将粗配准后的目标点云和点云模板进行精配准，得到精配准矩阵，精配准矩阵的形式与步骤S6的粗配准矩阵形式相同。
[0037]具体地，在步骤S9中，最终配准矩阵包括最终旋转转换矩阵和最终平移转换矩阵，然后通过下式转换为位姿表示形式：
[0038][P
x
,P
y
,P
z
]＝T
T
[0039][R
x
,R
y
,R
z
]＝[1,0,0]·
R
T
[0040]其中T表示最终的平移转换矩阵，R表示最终的旋转转换矩阵，[P
x
,P
y
,P
z
]是空间位置信息，表示车辆中心在世界坐标系三个坐标轴上的位移量；[R
x
,R
y
,R
z
]是空间姿态信息，表示车辆纵轴在世界坐标系下的空间朝向的单位向量。
[0041]与现有技术相比，本专利技术的有益效果在于：
[0042]本发本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，包括如下步骤：S1：获取路侧激光雷达采集到的实时点云数据；S2：提取步骤S1得到的点云数据中位于车辆行驶区域的点云；S3：拟合地面，根据拟合地面，将车辆行驶区域的点云分为地面点云和地上点云；S4：将地上点云进行聚类，得到目标车辆点云聚类；S5：估计地面点云的法向量，得到地面点云法向量[
x
,N
y
,N
z
]；S6：将步骤S4得到的目标车辆点云和步骤S5得到的地面点云法向量通过主成分分析法进行粗配准，得到粗配准矩阵和粗配准后的目标点云；S7：将粗配准后的目标点云和点云模板进行精配准，得到精配准矩阵；S8：将步骤S6得到的粗配准矩阵和步骤S7得到的精配准矩阵相乘，得到最终配准矩阵；S9：根据步骤S8得到的最终配准矩阵转换成位姿表达式。2.根据权利要求1所述的融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，在步骤S2中，在提取位于车辆行驶区域的点云时，通过距离信息在步骤S1得到的点云数据中提取。3.根据权利要求2所述的融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，距离信息包括点云坐标的x轴和y轴信息，将x轴坐标和y轴坐标在车辆行驶区域范围的点云进行提取。4.根据权利要求1所述的融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，在步骤S3中，使用RANSAC算法拟合地面。5.根据权利要求1所述的融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，在步骤4中，使用自适应欧式聚类方法地上点云进行聚类。6.根据权利要求1所述的融合路侧稀疏点云和道路特征的车辆位姿估计方法，其特征在于，步骤S6包括：S6.1：将步骤S4得到的目标车辆点云聚类PC
c
∈R3×
N
作为目标点云；S6.2：计算目标点云的中心点坐标p
center
；S6.3：计算目标点云最大特征值对应的特征向量u
′1；S6.4：通过下式计算旋转转换矩阵：u2＝Normalized(EC

【专利技术属性】
技术研发人员：熊会元，张秋，刘羽，潘跃龙，张学岭，
申请(专利权)人：中广核工程有限公司，
类型：发明
国别省市：