CN115980765A - 一种基于环境感知辅助粒子滤波的车辆定位方法 - Google Patents

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：在原始点云数据中随机选择三个点(x₀,y₀,z₀)，计算其平面方程Ax₀+By₀+Cz₀+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，就认为是模型内样本点(inliers，内点)，即认为所有内点在地面平面的0.2米以内，反之，认为是模型外样本点(outliers,外点)，并设置地面平面的法线方向沿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时刻后，车辆的位置运动状态表示为

式中

分别表示定位车辆的纬度、经度、高度，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^U]^T，其中，v^E，v^N，v^U均可由卫星导航***获取，W是***噪声矩阵，γ是输入噪声矩阵；

设采样周期为T，将公式(4)离散化处理得到k时刻***状态方程：

X(k)＝f{X(k-1)，U(k-1)，W(k-1)，γ(k-1)}  (5)

式中，***过程噪声表示为W＝[w₁，w₂，w₃]^T，W(k-1)对应的高斯白噪声协方差阵为

输入噪声

且噪声分量隐藏在***的三个外部输入中，状态转移方程f(·)表达式为：

其中，

f₃{X(k-1)，U(k-1)，W(k-1)，γ(k-1)}＝h(k-1)+Tv^U(k)

子步骤三：建立***观测方程

***观测方程表示为：

Z＝h(X)+V  (7)

式中，***观测向量

其中

分别表示卫星导航***获得纬度、经度和高度信息，H为***观测矩阵，V是***观测噪声并与W互不相关；

将公式(7)离散化处理得到k时刻***观测方程：

Z(k)＝h{X(k)，V(k)}＝h[X(k)]+V(k)  (8)

式中，

***观测噪声V对应的测量噪声协方差矩阵

其中

和

均由卫星导航***的位置测量噪声统计特性确定；

步骤四：改进粒子滤波算法对定位车辆的运动状态变量进行滤波估计

子步骤一：初始化粒子

在时刻k，以高斯分布来初始化粒子集{(X^j(k-1),η^j(k-1))|j＝1,…,N}，其中η^j(k-1)＝1/N，η^j(k-1)表示粒子权重，N表示粒子数，协方差阵为Φ，并设平均值为上一时刻的状态估计值，即：

式中，

表示k-1时刻状态估计值；

子步骤二：预测

在时刻k>0时，用公式(9)的非线性状态转移方程更新粒子群{(X^j(k-1),η^j(k-1))|j＝1,…,N}获得新的粒子群

子步骤三：结合点云栅格化地图更新粒子权重并做归一化处理

(1)每个粒子的权重表示为

式中，

表示观测似然函数，由于粒子样本符合高斯分布，所以

可以表示为：

其中，m表示观测向量的维数，|R|表示测量噪声协方差矩阵的行列式；

(2)引入点云栅格地图辅助更新部分粒子权重，具体流程如下：

c.1：计算当前时刻每个纬度和经度粒子预测值在车身坐标系下的坐标值

c.2：结合栅格化处理流程计算

落在所划分栅格的索引值(d,l)；

c.3：判断当前索引对应的栅格数组值A(b,l)是否为0，若对应数组值不为0，表示当前索引对应的栅格被车辆周围的物体所占据，即对应的粒子落在了是车辆实际位置之外的物体上，将对应粒子权重

赋值为0；

c.4：重复以上步骤，直至所有粒子判决完成；

(3)依据当前时刻的观测值，结合公式(10)和流程c.1～c.4来更新全部粒子权重值，并且做归一化处理，获得新的权值

即：

选取先验概率分布作为重要性密度函数，即

引入点云栅格地图后权重公式化简为

式中，d＝0,1,…,m-1，l＝0,1,…,m-1，m为栅格数量，A(b,l)表示第b行l列的栅格数组值；

(4)进行权重归一化处理，即

子步骤四：状态估计

利用获得的有权重的粒子

来近似后验概率密度p(X(k)|Z(k))，进而获得k时刻的状态估计，即，

子步骤五：重采样

依据粒子权重值的大小对粒子集

重新采样，以获取新的粒子群{(X^j(k),η^j(k))|i＝1,…,N}，其中，η^j(k)＝1/N，并返回重复(1)进行下一时刻的迭代；

经过上述公式(10)、公式(11)、公式(12)构成的基于环境感知辅助粒子滤波递推计算，输出的状态估计

为定位车辆的纬度、经度、高度运动状态信息；

综上，当结合激光雷达获取真实环境信息，引入生成的点云栅格化二维地图优化粒子权重公式辅助粒子滤波算法进行车辆定位时。首先，获取激光点云数据并进行预处理；然后，基于“步骤一”预处理后的点云数据构建点云栅格化地图；其次，构建定位车辆***状态模型和观测模型；最后，利用改进粒子滤波算法对定位车辆的位置运动状态进行滤波估计。

本发明的优点及显著效果：

本发明所提出的基于环境感知辅助粒子滤波的车辆定位方法，通过激光雷获取车辆周围环境信息，引入点云栅格化地图，并结合车辆周围真实环境对粒子权重进行进一步优化辅助粒子权重更新，提高粒子滤波算法的滤波精度，在一定程度上提高车辆定位精度。

附图说明

图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：在原始点云数据中随机选择三个点(x₀,y₀,z₀)，计算其平面方程Ax₀+By₀+Cz₀+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，就认为是模型内样本点(inliers，内点)，即认为所有内点在地面平面的0.2米以内，反之，认为是模型外样本点(outliers,外点)，并设置地面平面的法线方向沿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时刻后，车辆的位置运动状态表示为

式中

分别表示定位车辆的纬度、经度、高度，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^U]^T，其中，v^E,v^N,v^U均可由卫星导航***获取，W是***噪声矩阵，γ是输入噪声矩阵；

设采样周期为T，将公式(4)离散化处理得到k时刻***状态方程：

X(k)＝f{X(k-1)，U(k-1)，W(k-1)，γ(k-1))  (5)

式中，***过程噪声表示为W＝[w₁,w₂,w₃]^T，W(k-1)对应的高斯白噪声协方差阵为

输入噪声

且噪声分量隐藏在***的三个外部输入中，状态转移方程f(·)表达式为：

其中，

f₃{X(k-1),U(k-1),W(k-1),γ(k-1)}＝h(k-1)+Tv^U  (k)

子步骤三：建立***观测方程

***观测方程表示为：

Z＝h(X)+V  (7)

式中，***观测向量

其中

分别表示卫星导航***获得纬度、经度和高度信息，H为***观测矩阵，V是***观测噪声并与W互不相关；

将公式(7)离散化处理得到k时刻***观测方程：

Z(k)＝h{X(k),V(k)}＝h[X(k)]+V(k)  (8)

式中，

***观测噪声V对应的测量噪声协方差矩阵

其中

和

均由卫星导航***的位置测量噪声统计特性确定；

步骤四：改进粒子滤波算法对定位车辆的运动状态变量进行滤波估计

改进的粒子滤波算法流程如图3所示，具体步骤包括：

子步骤一：初始化粒子

在时刻k，以高斯分布来初始化粒子集{(X^j(k-1),η^j(k-1))|j＝1,…,N}，其中η^j(k-1)＝1/N，η^j(k-1)表示粒子权重，N表示粒子数，协方差阵为Φ，并设平均值为上一时刻的状态估计值，即：

式中，

表示k-1时刻状态估计值；

子步骤二：预测

在时刻k>0时，用公式(9)的非线性状态转移方程更新粒子群{(X^j(k-1),η^j(k-1))|j＝1,…,N}获得新的粒子群

子步骤三：结合点云栅格化地图更新粒子权重并做归一化处理

(1)每个粒子的权重表示为

式中，

表示观测似然函数，由于粒子样本符合高斯分布，所以

可以表示为：

其中，m表示观测向量的维数，|R|表示测量噪声协方差矩阵的行列式；

(2)引入点云栅格地图辅助更新部分粒子权重，具体流程如图4所示，步骤如下：

c.1：计算当前时刻每个纬度和经度粒子预测值在车身坐标系下的坐标值

c.2：结合栅格化处理流程计算

落在所划分栅格的索引值(d,l)；

c.3：判断当前索引对应的栅格数组值A(b,l)是否为0，若对应数组值不为0，表示当前索引对应的栅格被车辆周围的物体所占据，即对应的粒子落在了是车辆实际位置之外的物体上，将对应粒子权重

赋值为0；

c.4：重复以上步骤，直至所有粒子判决完成；

(3)依据当前时刻的观测值，结合公式(10)和流程c.1～c.4来更新全部粒子权重值，并且做归一化处理，获得新的权值

即：

选取先验概率分布作为重要性密度函数，即

引入点云栅格地图后权重公式化简为

式中，d＝0,1,…,m-1，l＝0,1,…,m-1，m为栅格数量，A(b,l)表示第b行l列的栅格数组值；

(4)进行权重归一化处理，即

子步骤四：状态估计

利用获得的有权重的粒子

来近似后验概率密度p(X(k)|Z(k))，进而获得k时刻的状态估计，即，

子步骤五：重采样

依据粒子权重值的大小对粒子集

重新采样，以获取新的粒子群{(X^j(k),η^j(k))|i＝1,…,N}，其中，η^j(k)＝1/N，并返回重复(1)进行下一时刻的迭代；

经过上述公式(10)、公式(11)、公式(12)构成的基于环境感知辅助粒子滤波递推计算，输出的状态估计

为定位车辆的纬度、经度、高度运动状态信息；

综上，当结合激光雷达获取真实环境信息，引入生成的点云栅格化二维地图优化粒子权重公式辅助粒子滤波算法进行车辆定位时。首先，获取激光点云数据并进行预处理；然后，基于“步骤一”预处理后的点云数据构建点云栅格化地图；其次，构建定位车辆***状态模型和观测模型；最后，利用改进粒子滤波算法对定位车辆的位置运动状态进行滤波估计。

本发明所提出的基于环境感知辅助粒子滤波的车辆定位方法，通过激光雷达点云数据引入显示车辆周围真实环境的点云栅格化地图，结合车辆周围真实环境对粒子权重进行进一步优化辅助粒子权重更新，提高粒子滤波算法的滤波精度，在一定程度上提高车辆定位精度，优点显著。

Claims (1)

1.一种基于环境感知辅助粒子滤波的车辆定位方法，其特征在于：首先，获取激光点云数据并进行预处理；然后，基于预处理后的点云数据构建点云栅格化地图；其次，构建定位车辆***状态模型和观测模型；最后，利用改进粒子滤波算法对定位车辆的运动状态进行滤波估计；具体步骤包括：

步骤一：获取激光点云数据并进行预处理

使用16线激光雷达作为点云的数据来源，安装方式为车顶对心安装；读取点云数据后并进行预处理，预处理过程具体如下：

子步骤一：裁剪指定范围内的点云

在车体坐标系下，x表示沿车辆纵向方向，y表示沿车辆横向方向，规定xy坐标系所在平面指向天向的方向为z轴方向，符合右手螺旋定则，为突出装载激光雷达传感器车辆周围的环境，以装载激光雷达传感器的车辆为中心设置感兴趣区域，单位米，裁剪指定范围内的点云，范围设定：x∈[-20,20]，y∈[-20,20]；

子步骤二：分割地平面和附近障碍物

(1)提取地面平面并剔除地面平面点，使用RANSAC算法检测和匹配地面平面具体流程如下：

a.1：在原始点云数据中随机选择三个点(x₀,y₀,z₀)，计算其平面方程Ax₀+By₀+Cz₀+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^U]^T，其中，v^E,v^N,v^U均可由卫星导航***获取，W是***噪声矩阵，γ是输入噪声矩阵；

设采样周期为T，将公式(4)离散化处理得到k时刻***状态方程：

X(k)＝f{X(k-1),U(k-1),W(k-1),γ(k-1)}    (5)

式中，***过程噪声表示为W＝[w₁,w₂,w₃]^T，W(k-1)对应的高斯白噪声协方差阵为

输入噪声

且噪声分量隐藏在***的三个外部输入中，状态转移方程f(·)表达式为：

其中，

f₃{X(k-1),U(k-1),W(k-1),γ(k-1)}＝h(k-1)+Tv^U(k)

子步骤三：建立***观测方程

***观测方程表示为：

Z＝h(X)+V    (7)

式中，***观测向量

其中

λ_g,h_G分别表示卫星导航***获得纬度、经度和高度信息，H为***观测矩阵，V是***观测噪声并与W互不相关；

将公式(7)离散化处理得到k时刻***观测方程：

Z(k)＝h{X(k),V(k)}＝h[X(k)]+V(k)    (8)

式中，

***观测噪声V对应的测量噪声协方差矩阵

其中

和

均由卫星导航***的位置测量噪声统计特性确定；

步骤四：改进粒子滤波算法对定位车辆的运动状态变量进行滤波估计

子步骤一：初始化粒子

在时刻k，以高斯分布来初始化粒子集{(X^j(k-1),η^j(k-1))|j＝1,…,N}，其中η^j(k-1)＝1/N，η^j(k-1)表示粒子权重，n表示粒子数，协方差阵为Φ，并设平均值为上一时刻的状态估计值，即：

式中，

表示k-1时刻状态估计值；

子步骤二：预测

在时刻k>0时，用公式(9)的非线性状态转移方程更新粒子群{(X^j(k-1),η^j(k-1))|j＝1,…,N}获得新的粒子群

子步骤三：结合点云栅格化地图更新粒子权重并做归一化处理

(1)每个粒子的权重表示为

式中，

表示观测似然函数，由于粒子样本符合高斯分布，所以

可以表示为：

其中，m表示观测向量的维数，|R|表示测量噪声协方差矩阵的行列式；

(2)引入点云栅格地图辅助更新部分粒子权重，具体流程如下：

c.1：计算当前时刻每个纬度和经度粒子预测值在车身坐标系下的坐标值

c.2：结合栅格化处理流程计算

落在所划分栅格的索引值(d,l)；

c.3：判断当前索引对应的栅格数组值A(b,l)是否为0，若对应数组值不为0，表示当前索引对应的栅格被车辆周围的物体所占据，即对应的粒子落在了是车辆实际位置之外的物体上，将对应粒子权重

赋值为0；

c.4：重复以上步骤，直至所有粒子判决完成；

(3)依据当前时刻的观测值，结合公式(10)和流程c.1～c.4来更新全部粒子权重值，并且做归一化处理，获得新的权值

即：

选取先验概率分布作为重要性密度函数，即

引入点云栅格地图后权重公式化简为

式中，d＝0,1,…,m-1，l＝0,1,…,m-1，m为栅格数量，A(b,l)表示第b行l列的栅格数组值；

(4)进行权重归一化处理，即

子步骤四：状态估计

利用获得的有权重的粒子

来近似后验概率密度p(X(k)|Z(k))，进而获得k时刻的状态估计，即，

子步骤五：重采样

依据粒子权重值的大小对粒子集

重新采样，以获取新的粒子群{(X^j(k),η^j(k))|i＝1,…,N}，其中，η^j(k)＝1/N，并返回重复(1)进行下一时刻的迭代；

经过上述公式(10)、公式(11)、公式(12)构成的基于环境感知辅助粒子滤波递推计算，输出的状态估计

为定位车辆的纬度、经度、高度运动状态信息；

综上，当结合激光雷达获取真实环境信息，引入生成的点云栅格化二维地图优化粒子权重公式辅助粒子滤波算法进行车辆定位时；首先，获取激光点云数据并进行预处理；然后，基于“步骤一”预处理后的点云数据构建点云栅格化地图；其次，构建定位车辆***状态模型和观测模型；最后，利用改进粒子滤波算法对定位车辆的位置运动状态进行滤波估计。

Images