CN115980765A - 一种基于环境感知辅助粒子滤波的车辆定位方法 - Google Patents
一种基于环境感知辅助粒子滤波的车辆定位方法 Download PDFInfo
- Publication number
- CN115980765A CN115980765A CN202211285811.1A CN202211285811A CN115980765A CN 115980765 A CN115980765 A CN 115980765A CN 202211285811 A CN202211285811 A CN 202211285811A CN 115980765 A CN115980765 A CN 115980765A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- vehicle
- cloud data
- particle
- grid
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000002245 particle Substances 0.000 title claims abstract description 113
- 238000001914 filtration Methods 0.000 title claims abstract description 46
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000008447 perception Effects 0.000 title claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000009826 distribution Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 11
- 230000004888 barrier function Effects 0.000 claims description 9
- 238000010606 normalization Methods 0.000 claims description 9
- 238000007781 pre-processing Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 9
- 238000005259 measurement Methods 0.000 claims description 8
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 6
- 238000012952 Resampling Methods 0.000 claims description 6
- 230000006870 function Effects 0.000 claims description 6
- 238000009434 installation Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000005520 cutting process Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 238000003860 storage Methods 0.000 claims description 3
- 230000026676 system process Effects 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 230000015556 catabolic process Effects 0.000 abstract description 5
- 238000006731 degradation reaction Methods 0.000 abstract description 5
- 238000005457 optimization Methods 0.000 abstract description 3
- 230000004927 fusion Effects 0.000 description 9
- 230000007613 environmental effect Effects 0.000 description 5
- 238000000342 Monte Carlo simulation Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000008187 granular material Substances 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于环境感知辅助粒子滤波的车辆定位方法。首先,获取激光点云数据并进行预处理;然后,构建周围环境的栅格化地图,以显示车辆周围是否有非地面目标;接着,构建定位车辆***状态模型和观测模型;最后,利用栅格化地图辅助粒子权重更新,降低非地面区域上粒子的权重,以改进粒子滤波算法,进行车辆位置的迭代更新。本发明提出的方法,通过利用激光雷达获取真实环境信息,在传统粒子滤波算法中引入点云栅格化二维地图优化粒子权重公式,辅助粒子权重更新,改善粒子退化问题,提高车辆定位精度。
Description
技术领域
本发明涉及一种车辆定位方法,尤其涉及一种基于环境感知辅助粒子滤波的车辆定位方法,属于智能驾驶汽车定位技术领域。
背景技术
目前智能车辆上针对定位技术相关的传感器主要有全球卫星导航***(GNSS)、惯性测量单元(IMU)、激光雷达(LiDAR)、轮速计和摄像头等。这些传感器各有所长,单一传感器难以适应复杂的现实环境,如天气季节的变化、道路和周边建筑物的遮挡等,不同的传感器有着不同的固有缺点需要通过其他传感器弥补,通过多传感器组合可以形成优势互补,因此需要使用融合定位技术以及多传感器融合算法。常见的多传感器融合算法有加权平均法、D-S证据推理法、滤波类算法以及基于神经网络技术的算法,其中滤波类的融合算法应用越来越广泛。
定位技术随着定位车辆运动模型复杂性的增强和对融合算法的性能要求不断提高,一些传统的线性滤波方法如标准卡尔曼滤波(KF)在有些应用方面已经不能满足要求,扩展卡尔曼滤波(EKF)、无迹卡尔曼(UKF)等通过局部线性化逼近非线性***的方法对比标准卡尔曼滤波(KF)精度更高,但其对于强非线性、强非高斯***仍具有局限性。相比之下,基于序列蒙特卡洛方法和递推贝叶斯估计的粒子滤波面向更为复杂的非线性模型、非高斯动态***有着优良特性,但因为其建议分布和真实分布的不匹配问题,随着滤波迭代次数的增加,大部分粒子的权重会变得很小甚至接近0,只有很少的粒子具有较大的权重,发生不可避免的粒子退化问题。本发明提出的一种基于环境感知辅助粒子滤波的车辆定位方法通过利用激光雷达获取真实环境信息,引入生成的点云栅格化二维地图优化粒子权重公式,辅助粒子权重更新改善粒子退化问题,能够在一定程度上可以提高车辆定位精度。
发明内容
本发明的目的是提出了一种基于环境感知辅助粒子滤波的车辆定位方法。首先,获取激光点云数据并进行预处理;然后,基于预处理后的点云数据构建点云栅格化地图;其次,构建定位车辆***状态模型和观测模型;最后,利用改进粒子滤波算法对定位车辆的运动状态进行滤波估计。具体步骤包括:
步骤一:获取激光点云数据并进行预处理
使用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:计算所有点(xi,yi,zi),到流程a.1中得到的平面的代数距离di=|Axi+Byi+Czi+D|,其中i=1,2,…,N,N为点云数据长度,选取阈值dmax=0.2米,如果di≤dmax,就认为是模型内样本点(inliers,内点),即认为所有内点在地面平面的0.2米以内,反之,认为是模型外样本点(outliers,外点),并设置地面平面的法线方向沿z轴向上,即法向量为[0,0,1],考虑实际道路可能存在较小的坡度,设置最大角距离5度,记录符合条件的内点个数;
a.3:重复以上步骤,选取最佳拟合参数,即内点数量最多的平面对应的模型;
a.4:迭代结束,输出地面平面模型系数[A,B,C,D];
(2)检索一定半径范围内的点,并标记为障碍物
首先,将激光雷达传感器置于点云地图坐标系的中心,依据裁剪范围设定半径为20米,在此半径范围内查找相邻点;
其次,标记障碍物点,使用欧式聚类分割算法分割障碍物并使用颜色标签标记障碍物点;
最后,将已经剔除过地面平面点并进行过标记的所有点云投影到使用RANSAC算法匹配到的地面平面,获取所有点云(xi,yi,zi)在地面平面投影后的点云数据(xp,yp,zp),其中:
步骤二:基于预处理后的点云数据构建点云栅格化地图
基于步骤一获取投影后的点云数据进行栅格化处理,具体步骤如下:
子步骤一:确定极值并设置栅格大小
对于一个给定点云数据Pi(i=1,2,…,N),计算其数据点在坐标轴方向上的最大值与最小值xmax,xmin,ymax,ymin,并设置栅格大小为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时刻后,车辆的位置运动状态表示为
子步骤二:建立***状态方程
其次,建立定位车辆状态方程模型:
X=f(X,U,W,γ) (4)
式中,f(·)是非线性状态转移方程,X是***状态向量,U是***外部输入向量且U=[vE,vN,vU]T,其中,vE,vN,vU均可由卫星导航***获取,W是***噪声矩阵,γ是输入噪声矩阵;
设采样周期为T,将公式(4)离散化处理得到k时刻***状态方程:
X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)} (5)
其中,
f3{X(k-1),U(k-1),W(k-1),γ(k-1)}=h(k-1)+TvU(k)
子步骤三:建立***观测方程
***观测方程表示为:
Z=h(X)+V (7)
将公式(7)离散化处理得到k时刻***观测方程:
Z(k)=h{X(k),V(k)}=h[X(k)]+V(k) (8)
步骤四:改进粒子滤波算法对定位车辆的运动状态变量进行滤波估计
子步骤一:初始化粒子
在时刻k,以高斯分布来初始化粒子集{(Xj(k-1),ηj(k-1))|j=1,…,N},其中ηj(k-1)=1/N,ηj(k-1)表示粒子权重,N表示粒子数,协方差阵为Φ,并设平均值为上一时刻的状态估计值,即:
子步骤二:预测
子步骤三:结合点云栅格化地图更新粒子权重并做归一化处理
(1)每个粒子的权重表示为
其中,m表示观测向量的维数,|R|表示测量噪声协方差矩阵的行列式;
(2)引入点云栅格地图辅助更新部分粒子权重,具体流程如下:
c.4:重复以上步骤,直至所有粒子判决完成;
式中,d=0,1,…,m-1,l=0,1,…,m-1,m为栅格数量,A(b,l)表示第b行l列的栅格数组值;
子步骤四:状态估计
子步骤五:重采样
综上,当结合激光雷达获取真实环境信息,引入生成的点云栅格化二维地图优化粒子权重公式辅助粒子滤波算法进行车辆定位时。首先,获取激光点云数据并进行预处理;然后,基于“步骤一”预处理后的点云数据构建点云栅格化地图;其次,构建定位车辆***状态模型和观测模型;最后,利用改进粒子滤波算法对定位车辆的位置运动状态进行滤波估计。
本发明的优点及显著效果:
本发明所提出的基于环境感知辅助粒子滤波的车辆定位方法,通过激光雷获取车辆周围环境信息,引入点云栅格化地图,并结合车辆周围真实环境对粒子权重进行进一步优化辅助粒子权重更新,提高粒子滤波算法的滤波精度,在一定程度上提高车辆定位精度。
附图说明
图1技术路线示意图;
图2RANSAC算法检测和匹配地面流程图;
图3改进的粒子滤波算法流程图;
图4引入点云栅格地图更新部分粒子权重流程图。
具体实施方式
在智能驾驶技术中,定位技术是一种让智能驾驶汽车获取自身确切位置的技术,在自动驾驶技术中,定位担负着相当车辆定位重要的职责。目前智能车辆上针对定位技术相关的传感器主要有全球卫星导航***(GNSS)、惯性测量单元(IMU)、激光雷达(LiDAR)、轮速计和摄像头等。这些不同的传感器能满足不同的特定需求,但由于现实环境的复杂性,如天气季节的变化、道路和周边建筑物的状况等,只用满足特定需求的单一传感器无法满足实际的需求,并且不同的传感器有着不同的固有缺点需要通过其他传感器弥补,因此出现了融合定位技术以及多传感器融合算法。常见的多传感器融合算法有加权平均法、D-S证据推理法、滤波类算法以及基于神经网络技术的算法,其中滤波类的融合算法应用越来越广泛。
定位技术随着定位车辆运动模型复杂性的增强和对滤波精度的不断提高,一些传统的非线性滤波方法如标准卡尔曼滤波(KF)在有些应用方面已经不能满足要求,扩展卡尔曼滤波(EKF)、无迹卡尔曼(UKF)等通过局部线性化逼近非线性***的方法对比标准卡尔曼滤波(KF)精度更高,但其对于强非线性、强非高斯***仍具有局限性。相比之下,基于序列蒙特卡洛方法和递推贝叶斯估计的粒子滤波面向更为复杂的非线性模型、非高斯动态***有着优良特性,但因为其建议分布和真实分布的不匹配问题,随着滤波迭代次数的增加,大部分粒子的权重会变得很小甚至接近0,只有很少的粒子具有较大的权重,发生不可避免的粒子退化问题。本发明提出的一种基于环境感知辅助粒子滤波的车辆定位方法通过利用激光雷达获取真实环境信息,引入生成的点云栅格化二维地图优化粒子权重公式,辅助粒子权重更新,改善粒子退化问题,能够在一定程度上可以提高车辆定位精度。
本发明的目的是提出了一种基于栅格化地图改进的粒子滤波车辆运动参数估计方法。首先,获取激光点云数据并进行预处理;然后,基于预处理后的点云数据构建点云栅格化地图;其次,构建定位车辆***状态模型和观测模型;最后,利用改进粒子滤波算法对定位车辆的运动状态进行滤波估计。如图1所示,具体步骤包括:
步骤一:获取激光点云数据并进行预处理
使用16线激光雷达作为点云的数据来源,安装方式为车顶对心安装;读取点云数据后并进行预处理,预处理过程具体如下:
子步骤一:裁剪指定范围内的点云
在车体坐标系下,x表示沿车辆纵向方向,y表示沿车辆横向方向,规定xy坐标系所在平面指向天向的方向为z轴方向,符合右手螺旋定则,为突出装载激光雷达传感器车辆周围的环境,以装载激光雷达传感器的车辆为中心设置感兴趣区域,单位米,裁剪指定范围内的点云,范围设定:x∈[-20,20],y∈[-20,20];
子步骤二:分割地平面和附近障碍物
(1)提取地面平面并剔除地面平面点,使用RANSAC(随机抽样一致)算法检测和匹配地面平面具体流程如图2所示,步骤如下:
a.1:在原始点云数据中随机选择三个点(x0,y0,z0),计算其平面方程Ax0+By0+Cz0+D=0;
a.2:计算所有点(xi,yi,zi),到流程a.1中得到的平面的代数距离di=|Axi+Byi+Czi+D|,其中i=1,2,…,N,N为点云数据长度,选取阈值dmax=0.2米,如果di≤dmax,就认为是模型内样本点(inliers,内点),即认为所有内点在地面平面的0.2米以内,反之,认为是模型外样本点(outliers,外点),并设置地面平面的法线方向沿z轴向上,即法向量为[0,0,1],考虑实际道路可能存在较小的坡度,设置最大角距离5度,记录符合条件的内点个数;
a.3:重复以上步骤,选取最佳拟合参数,即内点数量最多的平面对应的模型;
a.4:迭代结束,输出地面平面模型系数[A,B,C,D];
(2)检索一定半径范围内的点,并标记为障碍物
首先,将激光雷达传感器置于点云地图坐标系的中心,依据裁剪范围设定半径为20米,在此半径范围内查找相邻点;
其次,标记障碍物点,使用欧式聚类分割算法分割障碍物并使用颜色标签标记障碍物点;
最后,将已经剔除过地面平面点并进行过标记的所有点云投影到使用RANSAC算法匹配到的地面平面,获取所有点云(xi,yi,zi)在地面平面投影后的点云数据(xp,yp,zp),其中:
步骤二:基于预处理后的点云数据构建点云栅格化地图
基于步骤一获取投影后的点云数据进行栅格化处理,具体步骤如下:
子步骤一:确定极值并设置栅格大小
对于一个给定点云数据Pi(i=1,2,…,N),计算其数据点在坐标轴方向上的最大值与最小值xmax,xmin,ymax,ymin,并设置栅格大小为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时刻后,车辆的位置运动状态表示为
子步骤二:建立***状态方程
其次,建立定位车辆状态方程模型:
X=f(X,U,W,γ) (4)
式中,f(·)是非线性状态转移方程,X是***状态向量,U是***外部输入向量且U=[vE,vN,vU]T,其中,vE,vN,vU均可由卫星导航***获取,W是***噪声矩阵,γ是输入噪声矩阵;
设采样周期为T,将公式(4)离散化处理得到k时刻***状态方程:
X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)) (5)
其中,
f3{X(k-1),U(k-1),W(k-1),γ(k-1)}=h(k-1)+TvU (k)
子步骤三:建立***观测方程
***观测方程表示为:
Z=h(X)+V (7)
将公式(7)离散化处理得到k时刻***观测方程:
Z(k)=h{X(k),V(k)}=h[X(k)]+V(k) (8)
步骤四:改进粒子滤波算法对定位车辆的运动状态变量进行滤波估计
改进的粒子滤波算法流程如图3所示,具体步骤包括:
子步骤一:初始化粒子
在时刻k,以高斯分布来初始化粒子集{(Xj(k-1),ηj(k-1))|j=1,…,N},其中ηj(k-1)=1/N,ηj(k-1)表示粒子权重,N表示粒子数,协方差阵为Φ,并设平均值为上一时刻的状态估计值,即:
子步骤二:预测
子步骤三:结合点云栅格化地图更新粒子权重并做归一化处理
(1)每个粒子的权重表示为
其中,m表示观测向量的维数,|R|表示测量噪声协方差矩阵的行列式;
(2)引入点云栅格地图辅助更新部分粒子权重,具体流程如图4所示,步骤如下:
c.4:重复以上步骤,直至所有粒子判决完成;
式中,d=0,1,…,m-1,l=0,1,…,m-1,m为栅格数量,A(b,l)表示第b行l列的栅格数组值;
子步骤四:状态估计
子步骤五:重采样
综上,当结合激光雷达获取真实环境信息,引入生成的点云栅格化二维地图优化粒子权重公式辅助粒子滤波算法进行车辆定位时。首先,获取激光点云数据并进行预处理;然后,基于“步骤一”预处理后的点云数据构建点云栅格化地图;其次,构建定位车辆***状态模型和观测模型;最后,利用改进粒子滤波算法对定位车辆的位置运动状态进行滤波估计。
本发明所提出的基于环境感知辅助粒子滤波的车辆定位方法,通过激光雷达点云数据引入显示车辆周围真实环境的点云栅格化地图,结合车辆周围真实环境对粒子权重进行进一步优化辅助粒子权重更新,提高粒子滤波算法的滤波精度,在一定程度上提高车辆定位精度,优点显著。
Claims (1)
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:计算所有点(xi,yi,zi),到流程a.1中得到的平面的代数距离di=|Axi+Byi+Czi+D|,其中i=1,2,…,N,N为点云数据长度,选取阈值dmax=0.2米,如果di≤dmax,就认为是模型内样本点即内点,并认为所有内点在地面平面的0.2米以内,反之,认为是模型外样本点即外点,并设置地面平面的法线方向沿z轴向上,即法向量为[0,0,1],考虑实际道路可能存在较小的坡度,设置最大角距离5度,记录符合条件的内点个数;
a.3:重复以上步骤,选取最佳拟合参数,即内点数量最多的平面对应的模型;
a.4:迭代结束,输出地面平面模型系数[A,B,C,D];
(2)检索一定半径范围内的点,并标记为障碍物
首先,将激光雷达传感器置于点云地图坐标系的中心,依据裁剪范围设定半径为20米,在此半径范围内查找相邻点;
其次,标记障碍物点,使用欧式聚类分割算法分割障碍物并使用颜色标签标记障碍物点;
最后,将已经剔除过地面平面点并进行过标记的所有点云投影到使用RANSAC算法匹配到的地面平面,获取所有点云(xi,yi,zi)在地面平面投影后的点云数据(xp,yp,zp),其中:
步骤二:基于预处理后的点云数据构建点云栅格化地图
基于步骤一获取投影后的点云数据进行栅格化处理,具体步骤如下:
子步骤一:确定极值并设置栅格大小
对于一个给定点云数据Pi(i=1,2,…,N),计算其数据点在坐标轴方向上的最大值与最小值xmax,xmin,ymax,ymin,并设置栅格大小为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时刻后,车辆的位置运动状态表示为
子步骤二:建立***状态方程
其次,建立定位车辆状态方程模型:
X=f(X,U,W,γ) (4)
式中,f(·)是非线性状态转移方程,X是***状态向量,U是***外部输入向量且U=[vE,vN,vU]T,其中,vE,vN,vU均可由卫星导航***获取,W是***噪声矩阵,γ是输入噪声矩阵;
设采样周期为T,将公式(4)离散化处理得到k时刻***状态方程:
X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)} (5)
其中,
f3{X(k-1),U(k-1),W(k-1),γ(k-1)}=h(k-1)+TvU(k)
子步骤三:建立***观测方程
***观测方程表示为:
Z=h(X)+V (7)
将公式(7)离散化处理得到k时刻***观测方程:
Z(k)=h{X(k),V(k)}=h[X(k)]+V(k) (8)
步骤四:改进粒子滤波算法对定位车辆的运动状态变量进行滤波估计
子步骤一:初始化粒子
在时刻k,以高斯分布来初始化粒子集{(Xj(k-1),ηj(k-1))|j=1,…,N},其中ηj(k-1)=1/N,ηj(k-1)表示粒子权重,n表示粒子数,协方差阵为Φ,并设平均值为上一时刻的状态估计值,即:
子步骤二:预测
子步骤三:结合点云栅格化地图更新粒子权重并做归一化处理
(1)每个粒子的权重表示为
其中,m表示观测向量的维数,|R|表示测量噪声协方差矩阵的行列式;
(2)引入点云栅格地图辅助更新部分粒子权重,具体流程如下:
c.4:重复以上步骤,直至所有粒子判决完成;
式中,d=0,1,…,m-1,l=0,1,…,m-1,m为栅格数量,A(b,l)表示第b行l列的栅格数组值;
子步骤四:状态估计
子步骤五:重采样
综上,当结合激光雷达获取真实环境信息,引入生成的点云栅格化二维地图优化粒子权重公式辅助粒子滤波算法进行车辆定位时;首先,获取激光点云数据并进行预处理;然后,基于“步骤一”预处理后的点云数据构建点云栅格化地图;其次,构建定位车辆***状态模型和观测模型;最后,利用改进粒子滤波算法对定位车辆的位置运动状态进行滤波估计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211285811.1A CN115980765A (zh) | 2022-10-20 | 2022-10-20 | 一种基于环境感知辅助粒子滤波的车辆定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211285811.1A CN115980765A (zh) | 2022-10-20 | 2022-10-20 | 一种基于环境感知辅助粒子滤波的车辆定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115980765A true CN115980765A (zh) | 2023-04-18 |
Family
ID=85968756
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211285811.1A Pending CN115980765A (zh) | 2022-10-20 | 2022-10-20 | 一种基于环境感知辅助粒子滤波的车辆定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115980765A (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116300480A (zh) * | 2023-05-23 | 2023-06-23 | 西南科技大学 | 基于改进粒子滤波和生物启发神经网络的放射源搜寻方法 |
CN116796210A (zh) * | 2023-08-25 | 2023-09-22 | 山东莱恩光电科技股份有限公司 | 基于激光雷达的障碍物检测方法 |
CN117804452A (zh) * | 2023-12-07 | 2024-04-02 | 盐城中科高通量计算研究院有限公司 | 一种基于蒙特卡洛算法的充电平台车辆定位方法 |
-
2022
- 2022-10-20 CN CN202211285811.1A patent/CN115980765A/zh active Pending
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116300480A (zh) * | 2023-05-23 | 2023-06-23 | 西南科技大学 | 基于改进粒子滤波和生物启发神经网络的放射源搜寻方法 |
CN116796210A (zh) * | 2023-08-25 | 2023-09-22 | 山东莱恩光电科技股份有限公司 | 基于激光雷达的障碍物检测方法 |
CN116796210B (zh) * | 2023-08-25 | 2023-11-28 | 山东莱恩光电科技股份有限公司 | 基于激光雷达的障碍物检测方法 |
CN117804452A (zh) * | 2023-12-07 | 2024-04-02 | 盐城中科高通量计算研究院有限公司 | 一种基于蒙特卡洛算法的充电平台车辆定位方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108759833B (zh) | 一种基于先验地图的智能车辆定位方法 | |
CN115980765A (zh) | 一种基于环境感知辅助粒子滤波的车辆定位方法 | |
CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
Adams et al. | SLAM gets a PHD: New concepts in map estimation | |
JP5162849B2 (ja) | 不動点位置記録装置 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
JP4984659B2 (ja) | 自車両位置推定装置 | |
CN113592891B (zh) | 无人车可通行域分析方法及导航栅格地图制作方法 | |
CN115371662B (zh) | 一种基于概率栅格移除动态对象的静态地图构建方法 | |
Dawood et al. | Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera | |
CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
CN113515128B (zh) | 一种无人车实时路径规划方法及存储介质 | |
Farag | Real-time autonomous vehicle localization based on particle and unscented kalman filters | |
CN113640822A (zh) | 一种基于非地图元素过滤的高精度地图构建方法 | |
Sun et al. | Using dual‐polarization GPS antenna with optimized adaptive neuro‐fuzzy inference system to improve single point positioning accuracy in urban canyons | |
CN114608568B (zh) | 一种基于多传感器信息即时融合定位方法 | |
CN115183762A (zh) | 一种机场仓库内外建图方法、***、电子设备及介质 | |
CN115205803A (zh) | 自动驾驶环境感知方法、介质及车辆 | |
CN115496900A (zh) | 一种基于稀疏融合的在线碳语义地图构建方法 | |
CN115115790A (zh) | 预测模型的训练方法、地图预测方法及装置 | |
Charroud et al. | Fast and accurate localization and mapping method for self-driving vehicles based on a modified clustering particle filter | |
CN111089580B (zh) | 一种基于协方差交叉的无人战车同时定位与地图构建方法 | |
Hassani et al. | A new point-cloud-based lidar/imu localization method with uncertainty evaluation | |
CN113917503A (zh) | 卫星信号遮蔽场景建模方法、装置及程序产品 | |
CN117516572A (zh) | 一种基于强化学习的月面路径规划方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |