一种曲线道路环境下基于三维点云的车辆运动状态估计方法技术

本发明专利技术提供了一种曲线道路环境下基于三维点云的车辆运动状态估计方法，方法包括：S1，根据路缘空间结构特征建立道路中心曲线模型；S2，通过观测和计算确定自车坐标系、全局坐标系、道路曲线坐标系之间的转换关系；S3，构建道路曲线坐标系下车辆目标运动模型；S4，利用无迹卡尔曼滤波对车辆目标进行跟踪，估计得到运动状态参数。本发明专利技术通过构建道路模型提高车辆目标运动状态估计精度，特别是在曲率连续变化的曲线道路环境下，实现准确稳定的车辆目标跟踪。踪。踪。

【技术实现步骤摘要】
一种曲线道路环境下基于三维点云的车辆运动状态估计方法


[0001]本专利技术涉及自动驾驶
，特别是涉及一种曲线道路环境下基于三维点云的车辆运动状态估计方法。

技术介绍

[0002]随着人工智能技术的快速发展，自动驾驶汽车已成为智能交通领域的研究热点。可靠的环境感知能力是自动驾驶汽车完成后续行为决策和自主控制任务的前提条件。
[0003]目前环境感知传感器主要有摄像头、毫米波雷达、激光雷达等。相比于摄像头、毫米波雷达等传感器，激光雷达通过采集的丰富点云信息能够有效识别各类目标，包括处于静止或运动状态的车辆、行人等，同时不受光照条件影响，环境感知鲁棒性较好。
[0004]现有车辆目标运动状态估计的方法主要有：申请号为201910498228.0的专利技术提出“一种基于三维激光点云的在线目标跟踪系统的跟踪方法”，通过高斯过程回归算法和道路边界地图获得道路边界点，再利用聚类和数据关联算法匹配每帧场景独立点云簇，利用增量式高斯混合体对独立点云簇建模，采用矩形模型对点云簇进行拟合并应用卡尔曼滤波获得跟踪结果；申请号为202210109436.9的专利技术提出“一种基于点云的车辆速度估计方法、装置、设备及存储介质”，通过目标标注信息对当前点云帧车辆目标锚点进行匹配，计算车辆目标位置时序信息，并利用轨迹限定条件运动模型估计车辆目标运动状态。上述现有车辆运动状态估计方法大多仅考虑车辆自身运动状态，未考虑道路曲率影响。相比直线道路，曲线道路场景中道路曲率变化使得车辆运动状态变化不确定性较高，同时相邻帧同一车辆点云目标包围框的交并比较小，导致传统目标跟踪算法难以准确稳定追踪周围车辆目标，车辆运动状态参数估计误差较大，无法为自车提供可靠的行为决策依据。
[0005]因此，曲线道路环境下可以考虑通过构建道路曲线模型来提高车辆目标运动状态估计精度。目前尚未发现对此类技术的报导。

技术实现思路

[0006]本专利技术的目的在于，针对上述现有技术缺陷，提出一种曲线道路环境下基于三维点云的车辆目标运动状态估计方法。
[0007]为达到上述目的，本专利技术通过以下技术方案实现：
[0008]一种曲线道路环境下基于三维点云的车辆运动状态估计方法，包括如下步骤：
[0009]S1，根据路缘与地面间的点云结构特征差异筛选候选路缘点云，再通过绝对高度阈值和离群点过滤得到路缘点云，利用K
‑
means算法聚类生成两侧路缘点云集合，进一步采用最小二乘法拟合得到路缘曲线函数，构建基于三次样条插值函数的道路中心曲线模型；
[0010]S2，分别构建自车坐标系、全局坐标系、曲线坐标系，通过观测和计算确定各坐标系间的转换关系，将车辆目标检测网络输出的独立帧周围车辆目标状态参数从自车坐标系转换到道路曲线坐标系下；
[0011]S3，利用恒定转弯率和加速度模型构建曲线坐标系下车辆目标运动状态方程，并
将检测到的周围车辆目标状态参数转换到道路曲线坐标系下作为观测值，构建观测方程；
[0012]S4，基于无迹卡尔曼滤波预测道路曲线坐标系下车辆目标运动状态，通过状态预测值与观测值之间的马氏距离进行数据关联，利用Kuhn
‑
Munkres算法求取匹配最优解，再通过滤波更新车辆目标下一帧运动状态参数。
[0013]优选地，所述步骤S1还包括以下步骤：
[0014]S11，根据空间结构特征差异分割路缘和地面点云，利用三维点云水平连续性、垂直连续性、夹角阈值等约束条件筛选得到候选路缘点云，再结合绝对高度阈值过滤环境中车辆、行人等障碍物目标，并进一步剔除离群点得到路缘点云集合P
c
；
[0015]S12，采用K
‑
means算法聚类得到两侧路缘点集合P
c1
和P
c2
；
[0016]S13，利用最小二乘拟合对离散分布的两侧路缘点进行曲线拟合，通过最小化偏差平方和寻找得到两侧路缘曲线的最佳匹配函数f
p1
(x)、f
p2
(x)；
[0017]S14，由两侧路缘最佳曲线函数f
p1
(x)、f
p2
(x)得到一系列道路中心坐标点，通过三次样条插值函数构建道路中心曲线模型。
[0018]优选地，所述步骤S11还包括以下步骤：
[0019]S111，在x
‑
y平面上建立圆周方向同一激光束相邻点云水平连续约束，设定水平距离阈值l
xyt
，当相邻扫描点间距离l
xy
＞l
xyt
时，该相邻点可能为路缘点；
[0020]S112，在x
‑
y平面垂直方向建立圆周方向同一激光束相邻点云垂直连续约束，设定垂直距离阈值l
zt
，当相邻扫描点垂直间距离l
z
＞l
zt
时，该相邻点可能为路缘点；
[0021]S113，对圆周方向同一激光束相邻扫描点{p
i
‑1,p
i
,p
i+1
}，计算相邻扫描点间向量u
ia
、u
ib
的夹角ε
i
，设定角度阈值ε
t
，当ε
i
＜ε
t
时，扫描点p
i
可能为路缘点；
[0022]S114，将同时满足S111、S112、S113条件的点云记为路缘候选点云集合P
c0
。设定高度阈值H
R
，计算相邻连续扫描点{p
i
,p
i+1
,p
i+2
,
···
,p
i+m
}最大路缘垂直高度差H
i
，若H
i
＜H
R
且H
i+1
＞H
R
，则{p
i
,p
i+1
,p
i+2
,
···
,p
i+m
}为路缘点，生成路缘点集合P
c
；
[0023]S115，通过设置空间邻域半径r
n
，计算某一扫描点p
j
与邻域范围内点云间距离均值和标准差，获得点云距离阈值d
max
，若任意扫描点p
i
和p
j
间的距离d
i
大于d
max
则剔除p
i
。
[0024]优选地，所述步骤S2还包括以下步骤：
[0025]S21,以曲线道路某一起始中心点O为原点，将原点处道路中心曲线切线方向设为Y轴，法线方向设为X轴，建立全局坐标系X
G
OY
G
；以曲线道路起始中心点O为原点，将道路中心曲线方向设为本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种曲线道路环境下基于三维点云的车辆运动状态估计方法，其特征在于，包括以下步骤：S1，根据路缘与地面间的点云结构特征差异筛选候选路缘点云，再通过绝对高度阈值和离群点过滤得到路缘点云，利用K
‑
means算法聚类生成两侧路缘点云集合，进一步采用最小二乘法拟合得到路缘曲线函数，构建基于三次样条插值函数的道路中心曲线模型；S2，分别构建自车坐标系、全局坐标系、曲线坐标系，通过观测和计算确定各坐标系间的转换关系，将车辆目标检测网络输出的独立帧周围车辆目标状态参数从自车坐标系转换到道路曲线坐标系下；S3，利用恒定转弯率和加速度模型构建曲线坐标系下车辆目标运动状态方程，并将检测到的周围车辆目标状态参数转换到道路曲线坐标系下作为观测值，构建观测方程；S4，基于无迹卡尔曼滤波预测道路曲线坐标系下车辆目标运动状态，通过状态预测值与观测值之间的马氏距离进行数据关联，利用Kuhn
‑
Munkres算法求取匹配最优解，再通过滤波更新车辆目标下一帧运动状态参数。2.根据权利要求1所述的曲线道路环境下基于三维点云的车辆运动状态估计方法，其特征在于，所述步骤S1还包括以下步骤：S11，根据空间结构特征差异分割路缘和地面点云，利用三维点云水平连续性、垂直连续性、夹角阈值等约束条件筛选得到候选路缘点云，再结合绝对高度阈值过滤环境中车辆、行人等障碍物目标，并进一步剔除离群点得到路缘点云集合P
c
；S12，采用K
‑
means算法聚类得到两侧路缘点集合P
c1
和P
c2
；S13，利用最小二乘拟合对离散分布的两侧路缘点进行曲线拟合，通过最小化偏差平方和寻找得到两侧路缘曲线的最佳匹配函数f
p1
(x)、f
p2
(x)；S14，由两侧路缘最佳曲线函数f
p1
(x)、f
p2
(x)得到一系列道路中心坐标点，通过三次样条插值函数构建道路中心曲线模型。3.根据权利要求2所述的曲线道路环境下基于三维点云的车辆运动状态估计方法，其特征在于，所述步骤S11还包括以下步骤：S111，在x
‑
y平面上建立圆周方向同一激光束相邻点云水平连续约束，设定水平距离阈值l
xyt
，当相邻扫描点间距离l
xy
＞l
xyt
时，该相邻点可能为路缘点；S112，在x
‑
y平面垂直方向建立圆周方向同一激光束相邻点云垂直连续约束，设定垂直距离阈值l
zt
，当相邻扫描点垂直间距离l
z
＞l
zt
时，该相邻点可能为路缘点；S113，对圆周方向同一激光束相邻扫描点{p
i
‑1,p
i
,p
i+1
}，计算相邻扫描点间向量u
ia
、u
ib
的夹角ε
i
，设定角度阈值ε
t
，当ε
i
＜ε
t
时，扫描点p
i
可能为路缘点；S114，将同时满足S111、S112、S113条件的点云记为路缘候选点云集合P
c0
。设定高度阈值H
R
，计算相邻连续扫描点{p
i
,p
i+1
,p
i+2
,
···
,p
i+m
}最大路缘垂直高度差H
i
，若H
i
＜H
R
且H
i+1
＞H
R
，则{p
i
,p
i+1
,p
i+2
,
···
,p
i+m
}为路缘点，生成路缘点集合P
c
；S115，通过设置空间邻域半径r
n
，计算某一扫描点p
j
与邻域范围内点云间距离均值和标准差，获得点云距离阈值d
max
，若任意扫描点p
i
和p
j
间的距离d
...

【专利技术属性】
技术研发人员：张名芳，吴禹峰，刘颖，王力，
申请(专利权)人：北方工业大学，
类型：发明
国别省市：