一种基于环境感知辅助粒子滤波的车辆定位方法技术

本发明专利技术公开了一种基于环境感知辅助粒子滤波的车辆定位方法。首先，获取激光点云数据并进行预处理；然后，构建周围环境的栅格化地图，以显示车辆周围是否有非地面目标；接着，构建定位车辆系统状态模型和观测模型；最后，利用栅格化地图辅助粒子权重更新，降低非地面区域上粒子的权重，以改进粒子滤波算法，进行车辆位置的迭代更新。本发明专利技术提出的方法，通过利用激光雷达获取真实环境信息，在传统粒子滤波算法中引入点云栅格化二维地图优化粒子权重公式，辅助粒子权重更新，改善粒子退化问题，提高车辆定位精度。高车辆定位精度。高车辆定位精度。

【技术实现步骤摘要】
一种基于环境感知辅助粒子滤波的车辆定位方法


[0001]本专利技术涉及一种车辆定位方法，尤其涉及一种基于环境感知辅助粒子滤波的车辆定位方法，属于智能驾驶汽车定位


技术介绍

[0002]目前智能车辆上针对定位技术相关的传感器主要有全球卫星导航系统(GNSS)、惯性测量单元(IMU)、激光雷达(LiDAR)、轮速计和摄像头等。这些传感器各有所长，单一传感器难以适应复杂的现实环境，如天气季节的变化、道路和周边建筑物的遮挡等，不同的传感器有着不同的固有缺点需要通过其他传感器弥补，通过多传感器组合可以形成优势互补，因此需要使用融合定位技术以及多传感器融合算法。常见的多传感器融合算法有加权平均法、D
‑
S证据推理法、滤波类算法以及基于神经网络技术的算法，其中滤波类的融合算法应用越来越广泛。
[0003]定位技术随着定位车辆运动模型复杂性的增强和对融合算法的性能要求不断提高，一些传统的线性滤波方法如标准卡尔曼滤波(KF)在有些应用方面已经不能满足要求，扩展卡尔曼滤波(EKF)、无迹卡尔曼(UKF)等通过局部线性化逼近非线性系统的方法对比标准卡尔曼滤波(KF)精度更高，但其对于强非线性、强非高斯系统仍具有局限性。相比之下，基于序列蒙特卡洛方法和递推贝叶斯估计的粒子滤波面向更为复杂的非线性模型、非高斯动态系统有着优良特性，但因为其建议分布和真实分布的不匹配问题，随着滤波迭代次数的增加，大部分粒子的权重会变得很小甚至接近0，只有很少的粒子具有较大的权重，发生不可避免的粒子退化问题。本专利技术提出的一种基于环境感知辅助粒子滤波的车辆定位方法通过利用激光雷达获取真实环境信息，引入生成的点云栅格化二维地图优化粒子权重公式，辅助粒子权重更新改善粒子退化问题，能够在一定程度上可以提高车辆定位精度。

技术实现思路

[0004]本专利技术的目的是提出了一种基于环境感知辅助粒子滤波的车辆定位方法。首先，获取激光点云数据并进行预处理；然后，基于预处理后的点云数据构建点云栅格化地图；其次，构建定位车辆系统状态模型和观测模型；最后，利用改进粒子滤波算法对定位车辆的运动状态进行滤波估计。具体步骤包括：
[0005]步骤一：获取激光点云数据并进行预处理
[0006]使用16线激光雷达作为点云的数据来源，安装方式为车顶对心安装；读取点云数据后并进行预处理，预处理过程具体如下：
[0007]子步骤一：裁剪指定范围内的点云
[0008]在车体坐标系下，x表示沿车辆纵向方向，y表示沿车辆横向方向，规定xy坐标系所在平面指向天向的方向为z轴方向，符合右手螺旋定则，为突出装载激光雷达传感器车辆周围的环境，以装载激光雷达传感器的车辆为中心设置感兴趣区域，单位米，裁剪指定范围内的点云，范围设定：x∈[
‑
20,20]，y∈[
‑
20,20]；
[0009]子步骤二：分割地平面和附近障碍物
[0010](1)提取地面平面并剔除地面平面点，使用RANSAC(随机抽样一致)算法检测和匹配地面平面具体流程如下：
[0011]a.1：在原始点云数据中随机选择三个点(x0,y0,z0)，计算其平面方程Ax0+By0+Cz0+D＝0；
[0012]a.2：计算所有点(x
i
,y
i
,z
i
),到流程a.1中得到的平面的代数距离d
i
＝|Ax
i
+By
i
+Cz
i
+D|，其中i＝1,2,
…
,N，N为点云数据长度，选取阈值d
max
＝0.2米，如果d
i
≤d
max
，就认为是模型内样本点(inliers，内点)，即认为所有内点在地面平面的0.2米以内，反之，认为是模型外样本点(outliers,外点)，并设置地面平面的法线方向沿z轴向上，即法向量为[0,0,1]，考虑实际道路可能存在较小的坡度，设置最大角距离5度，记录符合条件的内点个数；
[0013]a.3：重复以上步骤，选取最佳拟合参数，即内点数量最多的平面对应的模型；
[0014]a.4：迭代结束，输出地面平面模型系数[A,B,C,D]；
[0015](2)检索一定半径范围内的点，并标记为障碍物
[0016]首先，将激光雷达传感器置于点云地图坐标系的中心，依据裁剪范围设定半径为20米，在此半径范围内查找相邻点；
[0017]其次，标记障碍物点，使用欧式聚类分割算法分割障碍物并使用颜色标签标记障碍物点；
[0018]最后，将已经剔除过地面平面点并进行过标记的所有点云投影到使用RANSAC算法匹配到的地面平面，获取所有点云(x
i
,y
i
,z
i
)在地面平面投影后的点云数据(x
p
,y
p
,z
p
)，其中：
[0019][0020]步骤二：基于预处理后的点云数据构建点云栅格化地图
[0021]基于步骤一获取投影后的点云数据进行栅格化处理，具体步骤如下：
[0022]子步骤一：确定极值并设置栅格大小
[0023]对于一个给定点云数据P
i
(i＝1,2,
…
,N)，计算其数据点在坐标轴方向上的最大值与最小值x
max
，x
min
，y
max
，y
min
，并设置栅格大小为0.5*0.5，即栅格沿x轴方向的边长size_x＝0.5，沿y轴方向的边长size_y＝0.5；
[0024]子步骤二：划分栅格数量
[0025]依据给定的栅格边长划分为m个栅格，其中：
[0026][0027]子步骤三：点云数据存入栅格数组
[0028](1)将划分后的栅格信息存储在二维数组中；
[0029](2)分层搜索点云数据存储点云数据，具体流程如下：
[0030]b.1：沿x轴方向搜索点云，判断点云数据所处栅格的行数位置；
[0031]b.2：对所处同一行栅格的点云沿y轴方向搜索点云，判断点云数据所处栅格列数
位置；
[0032]b.3：重复以上步骤，直至对所有点云数据搜索完成；
[0033]b.4：依据搜索结果将所有点云数据存入其对应栅格的所对应的栅格数组；
[0034]子步骤四：依据子步骤三的存储结果信息构建点云栅格地图
[0035]步骤三：构建定位车辆系统状态模型和观测模型
[0036]定位车辆配备了高精度卫星导航系统接收机来实现车辆的三维定位，并依据车辆在行驶中的运动特性建立定位车辆系统状态方程和观测方程；
[0037]子步骤一：建立定位车辆状态估计模型
[0038]从k
‑
1时刻经过Δt到k时刻后，车辆的位置运动状态表示为
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于环境感知辅助粒子滤波的车辆定位方法，其特征在于：首先，获取激光点云数据并进行预处理；然后，基于预处理后的点云数据构建点云栅格化地图；其次，构建定位车辆系统状态模型和观测模型；最后，利用改进粒子滤波算法对定位车辆的运动状态进行滤波估计；具体步骤包括：步骤一：获取激光点云数据并进行预处理使用16线激光雷达作为点云的数据来源，安装方式为车顶对心安装；读取点云数据后并进行预处理，预处理过程具体如下：子步骤一：裁剪指定范围内的点云在车体坐标系下，x表示沿车辆纵向方向，y表示沿车辆横向方向，规定xy坐标系所在平面指向天向的方向为z轴方向，符合右手螺旋定则，为突出装载激光雷达传感器车辆周围的环境，以装载激光雷达传感器的车辆为中心设置感兴趣区域，单位米，裁剪指定范围内的点云，范围设定：x∈[
‑
20,20]，y∈[
‑
20,20]；子步骤二：分割地平面和附近障碍物(1)提取地面平面并剔除地面平面点，使用RANSAC算法检测和匹配地面平面具体流程如下：a.1：在原始点云数据中随机选择三个点(x0,y0,z0)，计算其平面方程Ax0+By0+Cz0+D＝0；a.2：计算所有点(x
i
,y
i
,z
i
),到流程a.1中得到的平面的代数距离d
i
＝|Ax
i
+By
i
+Cz
i
+D|，其中i＝1,2,
…
,N，N为点云数据长度，选取阈值d
max
＝0.2米，如果d
i
≤d
max
，就认为是模型内样本点即内点，并认为所有内点在地面平面的0.2米以内，反之，认为是模型外样本点即外点，并设置地面平面的法线方向沿z轴向上，即法向量为[0,0,1]，考虑实际道路可能存在较小的坡度，设置最大角距离5度，记录符合条件的内点个数；a.3：重复以上步骤，选取最佳拟合参数，即内点数量最多的平面对应的模型；a.4：迭代结束，输出地面平面模型系数[A,B,C,D]；(2)检索一定半径范围内的点，并标记为障碍物首先，将激光雷达传感器置于点云地图坐标系的中心，依据裁剪范围设定半径为20米，在此半径范围内查找相邻点；其次，标记障碍物点，使用欧式聚类分割算法分割障碍物并使用颜色标签标记障碍物点；最后，将已经剔除过地面平面点并进行过标记的所有点云投影到使用RANSAC算法匹配到的地面平面，获取所有点云(x
i
,y
i
,z
i
)在地面平面投影后的点云数据(x
p
,y
p
,z
p
)，其中：步骤二：基于预处理后的点云数据构建点云栅格化地图基于步骤一获取投影后的点云数据进行栅格化处理，具体步骤如下：子步骤一：确定极值并设置栅格大小
对于一个给定点云数据P
i
(i＝1,2,
…
,N)，计算其数据点在坐标轴方向上的最大值与最小值x
max
，x
min
，y
max
，y
min
，并设置栅格大小为0.5*0.5，即栅格沿x轴方向的边长size_x＝0.5，沿y轴方向的边长size_y＝0.5；子步骤二：划分栅格数量依据给定的栅格边长划分为m个栅格，其中：子步骤三：点云数据存入栅格数组(1)将划分后的栅格信息存储在二维数组中；(2)分层搜索点云数据存储点云数据，具体流程如下：b.1：沿x轴方向搜索点云，判断点云数据所处栅格的行数位置；b.2：对所处同一行栅格的点云沿y轴方向搜索点云，判断点云数据所处栅格列数位置；b.3：重复以上步骤，直至对所有点云数据搜索完成；b.4：依据搜索结果将所有点云数据存入其对应栅格的所对应的栅格数组；子步骤四：依据子步骤三的存储结果信息构建点云栅格地图步骤三：构建定位车辆系统状态模型和观测模型定位车辆配备了高精度卫星导航系统接收机来实现车辆的三维定位，并依据车辆在行驶中的运动特性建立定位车辆系统状态方程和观测方程；子步骤一：建立定位车辆状态估计模型从k
‑
1时刻经过Δt到k时刻后，车辆的位置运动状态表示为式中λ,h分别表示定位车辆的纬度、经度、高度，v
E
,v
N
,v
U
分别表示定位车辆的东向速度、北向速度、垂向速度，表示微分符号，R
M
和R
N
分别表示地球子午线半径和卯酉圈曲率半径；子步骤二：建立系统状态方程首先，基于式(3)建立车辆状态估计模型选取状态向量其中，上标T表示矩阵转置；其次，建立定位车辆状态方程模型：X＝f(X,U,W,γ)
ꢀꢀꢀꢀ
(4)式中，f(
·
)是非线性状态转移方程，X是系统状态向量，U是系统外部输入向量且U＝[v
E
,v
N
,v
...

【专利技术属性】
技术研发人员：徐启敏，尉雪珂，李旭，
申请(专利权)人：东南大学，
类型：发明
国别省市：