CN113405560A - 车辆定位和路径规划统一建模方法 - Google Patents

Abstract

本发明提供了一种车辆定位和路径规划统一建模方法，将定位与路径规划创新性的进行统一建模，以路径规划的轨迹作为一种约束，计算GPS定位结果与激光雷达定位结果到约束轨迹之间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果，利用激光定位数据修正GPS定位结果，得到车辆的精确定位信息。与现有技术相比，本发明将定位与路径规划关联起来，统一建模，融合GPS定位结果与激光雷达定位结果，解决了GPS在信号差的区域定位结果不准和激光雷达定位存在累计误差的问题，可以为车辆循迹导航提供精确的定位精度。

Description

车辆定位和路径规划统一建模方法

技术领域

本发明属于定位技术领域，具体涉及一种车辆定位和路径规划统一建模方法。

背景技术

近年来，随着汽车智能化的程度越来越高，车辆的定位问题与路径规划问题成为各方研 究的热点。目前主流的车辆定位方法主要有基于GPS的、基于视觉的和基于激光的定位。当 前基于GPS的车辆定位技术日趋成熟，然而其定位误差在10米左右，定位精度低，并且在 某些GPS信号弱或者接受不到的地方，其定位精度更是难以得到保证，单纯的GPS已经不能 满足车辆的定位需求。随着激光雷达传感器的广泛应用，基于激光的车辆定位技术得到快速 发展，激光雷达的优点是测量精确，能够比较精准的提供角度和距离信息，可以达到<1°的 角度精度以及厘米级别的测距精度，扫描范围广，但存在由于累计误差而导致的定位偏差问 题。

发明内容

针对现有技术的不足，本发明提供一种车辆定位和路径规划统一建模方法，提高定位精 度。

本发明所采用的技术方案如下：

一种车辆定位和路径规划统一建模方法，包括以下步骤：

S1、处理激光点云数据，得到二维栅格地图；

S2、通过标定点云与GPS数据之间的映射关系，将激光坐标系中的车辆轨迹信息和二 维栅格地图映射到GPS坐标系下；

S3、基于A星算法在二维栅格地图中确定路径规划的轨迹结果；

S4、以路径规划的轨迹作为约束轨迹，计算GPS定位结果与激光雷达定位结果到约束 轨迹之间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果。

优选地，步骤S1包括：

S11、对激光点云数据进行预处理，剔除噪声点、离群点，同时过滤掉场景中路面点云 信息，只保留场景中除路面信息之外的固有静态障碍信息；

S12、将预处理后的三维激光点云数据进行XOY平面的投影处理，然后将整个XOY平面均匀地划分为一定尺寸的栅格，规定存在点云的栅格为占据状态，无点云存在的栅格为自 由状态，构建二维栅格地图。

优选地，步骤S11包括：

S111、采用直通滤波器保留距离车辆一定距离内的点云数据，即：若激光点云数据中某 一点的坐标(x_i，y_i，z_i)满足：x_i∈(X₁,X₂)∩y_i∈(Y₁,Y₂)∩z_i∈(Z₁,Z₂)，则该点被保留；其 中，X_i、Y_i和Z_i分别为激光点云坐标系沿X、Y和Z轴方向设定的阈值；

S112、采用体素网格滤波器对激光点云数据进行滤波降采样处理，将点云数据划分为多 个体素网格，计算每一个体素网格的重心，以重心点代表体素网格中的所有点；

S113、将z坐标小于一定高度的点作为候选地面点集P'，并对这些点按照高度z从小 到大排序，求出平均高度z_ave，再计算高度小于z_ave+z_threshold的点的法线，Z_threshold表示预设阈 值，将与z轴的夹角大于预设角度的点保留，剩下的所有点作为最终地面点集P_ground，从而 剔除最终地面点集P_ground。

优选地，步骤S2包括：

S21、在激光点云扫描范围内选择一标志物G；

S22、采集标志物G的GPS坐标位置信息(Gx，Gy，Gz)^T；

S23、提取标志物G在激光坐标系下的激光坐标位置(x，y，z)^T；

S24、根据最小二乘问题求出旋转矩阵R和平移矩阵T。

优选地，步骤S23包括：

S231、剔除激光点云中的离群点与噪声点；

S232、利用RANSAC算法拟合得到标志物立杆G的点集，通过计算其密度公式得到立杆 的质心坐标位置(x，y，z)^T，以此坐标位置作为立杆G在激光坐标系下的激光坐标位置(x，y，z)^T。

优选地，步骤S24包括：

S241、标志物的坐标之间存在以下关系：

式中，R是激光坐标系到GPS坐标系下的旋转矩阵，T是激光坐标系到GPS坐标系下的 平移矩阵；

S242、将Z方向的坐标做近似0的处理，上式转换如下：

式中，[x，y]^T是标志物G在GPS坐标系下的坐标位置，[Gx,Gy]^T是标志物G在激光坐标系下的坐标位置；

S243、近似处理后的式子可变为：

转换为方程组形式：

进而变为矩阵形式：

即可转换为最小二乘问题：Ax＝b，求解得出方程的解x＝(A^TA)^-1A^Tb即求解出旋转矩 阵R和平移矩阵T。

优选地，步骤S3包括：

S31、在二维栅格地图中设定路径的起点与终点，确定起点与终点在二维栅格地图中的 位置信息；

S32、根据A*算法的启发函数选择最优的方向展开搜索；

S33、计算欧几里得距离：

式中，(x_n,y_n)代表当前节点的坐标，(x_d,y_d)代表目标节点的坐标；

S34、结合A*算法使用OPEN列表和CLOSED列表来进行搜索，在栅格地图中生成由起点 到终点的路径规划轨迹结果。

优选地，步骤S4包括：

S41、将当前车辆位置采集点记做点P，实时获取车辆位置采集点P的GPS位置信息[x,y]^T，并将其转换到UTM坐标系下：U_P＝(x_P,y_P)＝trans(x,y)；(x_P,y_P)为位置采集点P在UTM坐标系下的GPS位置信息，trans()为将GPS坐标系转换为UTM坐标系下的转换关系；

同时，根据旋转矩阵R和平移矩阵T以及GPS坐标系到UTM坐标系下的转换关系trans(),得出车辆位置采集点P从激光坐标系下的位置[Gx,Gy]^T转换到UTM坐标系下的位置：U_G＝(x_G,y_G)＝trans(R*[Gx,Gy]^T+T)；(x_G,y_G)为位置采集点P在UTM坐标系下的激光雷达位 置信息；

根据变换关系将路径规划轨的迹转换到UTM坐标系下；

S42、分别计算GPS定位结果与激光雷达定位结果到规划轨迹之间的横向距离；

S43、以路径规划的轨迹作为约束轨迹，GPS定位结果与激光雷达定位结果到约束轨迹 之间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果。

优选地，步骤S42包括：

S421、路径规划轨迹由直线段构成，由两点式直线方程计算出规划轨迹的直线方程： l:a_ix+b_iy+c_i＝0；

S422、根据车辆位置采集点当前时刻在UTM坐标系下的GPS位置信息U_P＝(x_P,y_P)及激光 雷达位置信息U_G＝(x_G,y_G)，分别求解出横向距离：

优选地，步骤S43包括：

S431、以当前位置x_k和N个历史位置共同组成的轨迹作为当前状态： X_k＝[x_k x_k-1… x_k-N]，每个位置均包括横纵两个方向的信息：x_k＝[x_k ^X x_k ^Y]^T；基于运 动学匀变速公式：X_k＝2X_k-1-X_k-2，得到状态转移矩阵

预测状态 协方差矩阵

其中Q为GPS定位精度；

S432、融合当前车辆的GPS定位结果U_P和激光雷达定位结果U_G以及横向距离D₁、D₂，建 立观测值z：λ₁U_P+λ₂U_G；当D₁＜D₂时，取

当D₂＜D₁时， 取

S433、基于运动学匀变速运动公式预测下一时刻的位置，融合车辆的GPS定位结果U_P和 激光雷达定位结果，以λ₁U_P+λ₂U_G作为观测值，以GPS定位结果与激光雷达定位结果到路径规划轨迹之间的横向距离作为卡尔曼滤波器中的约束，由kalman滤波公式得到融合后的定位结果[x_p ^X x_p ^Y]，该融合的定位结果为UTM坐标系下的结果。

本发明的有益效果为：本发明将定位与路径规划创新性的进行统一建模，以路径规划的 轨迹作为一种约束，计算GPS定位结果与激光雷达定位结果到约束轨迹之间的横向距离作为 卡尔曼滤波器中的约束，融合GPS定位与激光定位结果，利用激光定位数据修正GPS定位结 果，得到车辆的精确定位信息。与现有技术相比，本发明将定位与路径规划相互关联，统一 建模，融合GPS定位结果与激光雷达定位结果，解决了GPS在信号差的区域定位结果不准和 激光雷达定位存在累计误差的问题，可以为车辆循迹导航提供精确的定位精度。

附图说明

图1为本发明一实施例的车载设备示意图。

图2为本发明一实施例的坐标标定示意图。

图3为本发明一实施例的车辆定位和路径规划统一建模方法流程图。

图中：1-车辆，2-电源，3-工控机，4-激光雷达点云采集设备，5-GPS接收器，6-显示器，7-差分GPS基站。

具体实施方式

下面将结合附图对本发明作进一步的说明：

本发明提供一种车辆定位和路径规划统一建模方法，首先通过激光雷达采集到的三维点 云数据进行相关滤波处理和俯视投影得到二维栅格地图；然后通过标定点云局部坐标与GPS 数据的映射关系将激光坐标系中的轨迹信息和栅格地图映射到GPS坐标系下；最后基于A星 算法在二维栅格地图中确定路径规划的轨迹结果，以路径规划的轨迹作为约束轨迹，计算 GPS定位结果与激光定位结果到约束轨迹之间的横向距离作为卡尔曼滤波器中的约束，融合 GPS定位与激光定位结果，利用激光定位数据修正GPS定位结果，得到车辆在全局坐标系下 的精确定位信息。该方法利用卡尔曼滤波器融合GPS、激光雷达与约束轨迹三者之间的结果， 解决了GPS在信号差的区域定位结果不准和激光雷达定位存在累计误差的问题，可以为车辆 的循迹导航提供精确的定位精度。

本发明实施例的车辆定位和路径规划统一建模方法，如图3所示，包括地图构建、坐标 标定和车辆定位三个阶段，其中车辆定位包括GPS定位、激光雷达定位和融合定位三个部分。 为了实现本发明的方法，所需的硬件如图1所示，它包括：

数据采集模块，具体包括车辆1及车辆1上的激光雷达点云采集设备4，GPS信号接收 器5；其中激光雷达点云采集设备用于采集车辆当前时刻下的点云信息，点云中包含有标定 过程所用的标志物-立杆。GPS信号接收器用于接收来自差分GPS基站7所发出的信号，实 现当前车辆在GPS坐标系下的定位。

数据传输模块，包括将激光雷达采集的点云信息和GPS接收器接收到的报文信息传输到 工控机3。

数据处理模块，包括对点云信息与坐标信息在工控机3中进行信息同步处理，并提前对 采集的点云信息进行建图处理，构建栅格地图，规划路径轨迹，完成对定位结果的融合。

此外，电源2为上述设备供电，显示器6实时显示规划路径与当前定位结果。

本发明实施例中，车辆定位和路径规划统一建模方法，具体包括以下步骤：

S1、激光点云数据处理得到二维栅格地图。

S11、激光点云预处理，在预处理中，需要将噪声点、离群点剔除，同时还需要过滤掉 场景中路面点云信息，只保留场景中除路面信息之外的固有静态障碍信息，如建筑物、树木 等。

采用直通滤波器剔除掉距离车辆较远处的点云数据，假设原始激光点云中某一点的坐标， 若该坐标满足：x_i∈(X₁,X₂)∩y_i∈(Y₁,Y₂)∩z_i∈(Z₁,Z₂)，则该点被保留，否则将该点剔除掉。 其中，X_i、Y_i和Z_i分别为激光点云坐标系沿X、Y和Z轴方向设定的阈值。

采用体素网格滤波器对激光原始点云数据进行滤波降采样处理，通过对点云的获取得到 激光原始点云数据，对原始点云数据网格化处理，将点云数据划分为5cm×5cm的体素网格， 然后通过计算每一个体素网格的重心，以重心点来代表该体素网格中的所有点。

利用条件筛选法检测出地面点集P_ground。将z小于一定高度的点作为候选地面点集P'， 然后对这些点按照高度z从小到大排序，求出平均高度z_ave，再计算高度小于z_ave+z_threshold的 点的法线，Z_threshold表示预设阈值，将与z轴的夹角大于10°的点保留，剩下的所有点作为 最终地面点集P_ground，剔除最终地面点集P_ground。

S12、将预处理后的三维激光点云数据进行XOY平面的投影处理。然后将整个XOY平面 均匀地划分为一定尺寸的栅格，栅格尺寸根据实际情况人为设定，规定存在点云的栅格为占 据状态“1”，无点云存在的栅格为自由状态“0”，构建二维栅格地图。

S2、通过标定点云与RTK/INS数据的映射关系将激光坐标系中的轨迹信息和栅格地图映 射到GPS坐标系下。

S21、为了标定点云与RTK/INS数据的映射关系，在激光点云扫描范围内选择一个标志 物，如图2所示，本实施例选用立杆。

S22、运用GPS接收机、组合惯导系统以及RTK基站采集该立杆的GPS坐标位置信息(Gx,Gy,Gz)^T。

S23、提取立杆G在激光坐标系下的激光坐标位置(x，y，z)^T。

首先，在获取得到的激光扫描数据中，通过对点云的获取得到激光原始点云数据，剔除 激光点云中的离群点与噪声点。最后，利用RANSAC(Random Sample Consensus)算法对标 志物立杆G稳健估计其位置。立杆G可以近似的看作一个细小的圆柱体，圆柱方程可以表示 为：

通过RANSAC算法拟 合得到的标志物立杆G的点集，通过计算其密度公式f(x,y),可以得到圆柱体的质心坐标位 置(x，y，z)^T，以此坐标位置作为立杆G在激光坐标系下的激光坐标位置(x，y，z)^T。

S24、根据最小二乘问题求出旋转矩阵R和平移矩阵T。

通过对标志物立杆G获取其GPS坐标系下的坐标(Gx,Gy,Gz)^T,那么在不同坐标系下， 立杆的坐标之间存在以下关系：

其中R是激光坐标系到GPS坐标系下的旋 转矩阵，T是激光坐标系到GPS坐标系下的平移矩阵。

考虑到车辆在正常行驶过程中车辆紧贴地面行驶，且立杆紧靠地面，可将其Z方向的坐 标做近似0的处理，因此上式可转换如下：

[x，y]^T是立杆G在GPS坐标系下的坐标位置，[Gx,Gy]^T是立杆G在激光坐标系下的坐标位置。在二维情况下，旋转矩阵由两个未知数构成，即

平移向量可表示为

则上式可变为：

转换为方程组形式：

可变为矩阵形式：

即可转换为最小二乘问题：Ax＝b，求解得出方程的解 x＝(A^TA)^-1A^Tb。即可求解出旋转矩阵R和平移矩阵T。

S3、基于A星算法在二维栅格地图中确定路径规划的轨迹结果。

S31、在栅格地图设定路径的起点与终点，确定起点与终点在栅格地图中的位置信息。

S32、根据启发函数来选择最优的方向展开搜索。A*算法的启发函数表达式为： f(n)＝g(n)+h(n)，其中，f(n)表示在当前节点n时，从初始节点到达目标节点的总代价函数；g(n)表示从初始节点到达当前节点n实际代价值的大小；h(n)表示从当前节点n到达目标节点的最小估计代价值的大小。

S33、计算欧几里得距离

其中(x_n,y_n)代表当前节点的坐标； (x_d,y_d)代表目标节点的坐标。

S34、结合A*算法使用OPEN列表和CLOSED列表来进行搜索，在栅格地图中生成由起点 到终点的路径规划轨迹结果。

S4、以路径规划的轨迹作为约束轨迹，计算GPS定位结果与激光定位结果到约束轨迹之 间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果。

S41、坐标转换。为了便于计算横向距离，将GPS坐标系与激光雷达坐标系转换到UTM (Universal Transverse Mercator)坐标系，即通用横轴墨卡托投影系统。把当前车辆测试采集点位置记做点P，利用GPS采集设备实时获取到车辆测试点P的GPS位置信息[x,y]^T，将其转换到UTM坐标系底下U_P＝(x_P,y_P)＝trans(x,y),(x_P,y_P)为测试采集点P的UTM坐标位置 信息，trans()为将GPS坐标系转换为UTM坐标系下的转换关系。同时，根据旋转矩阵R和平移矩阵T，以及GPS坐标系到UTM坐标系下的转换关系trans(),可以得出车辆测试采集点P从激光坐标系下的位置[Gx,Gy]^T转换到UTM坐标系下的位置 U_G＝(x_G,y_G)＝trans(R*[Gx,Gy]^T+T)。同时根据变换关系将路径规划轨迹转换到UTM坐标系下。

S42、通过GPS定位与激光雷达定位计算定位结果到规划轨迹之间的横向距离。路径规 划轨迹由直线段构成，由两点式直线方程可以计算出规划轨迹的直线方程l:a_ix+b_iy+c_i＝0， 根据上一步骤的车辆采集点当前时刻在UTM坐标系下的GPS位置信息U_P＝(x_P,y_P)和激光雷达 位置信息U_G＝(x_G,y_G)可以求解出横向距离

S43、以路径规划的轨迹作为约束轨迹，上一步骤所述的GPS定位结果与激光定位结果 到约束轨迹之间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果。卡 尔曼滤波器融合定位具体为：

以当前位置和N个历史位置，由N+1个位置组成的轨迹作为当前状态(本实例中，取N＝2)：X_k＝[x_k x_k-1 … x_k-N]，每个位置包括横纵两个方向的信息：x_k＝[x_k ^X x_k ^Y]^T， 基于运动学匀变速公式：X_k＝2X_k-1-X_k-2，从而可以得到状态转移矩阵

预测状态协方差矩阵

其中Q为GPS定位精度。这里取Q＝10。

将当前车辆在GPS定位下的结果U_P和激光定位下的结果U_G，以及横向距离D₁、D₂，把λ₁U_P+λ₂U_G作为观测值z，当D₁＜D₂时，取

当D₂＜D₁时， 取

为了求解出观测矩阵H，根据关系有观测值 z＝H[x_k x_k-1x_k-2]^T；其中H为观测矩阵，求解得

观测噪声协方差矩阵

其中δ₁ ²，δ₂ ²，...，δ_n ²为多次测量的各测量误差数据的方差。

根据Kalman滤波公式：

其中，X_k'为融合定位后当前车辆位置的结果，误差精度可达到厘米级，满足当前车辆 定位需求。

基于运动学匀变速运动公式得到的预测数据和上述步骤中融合车辆在GPS定位下的结果 和激光定位下的结果λ₁U_P+λ₂U_G作为观测值，以GPS定位结果与激光定位结果到路径规划 轨迹之间的横向距离作为卡尔曼滤波器中的约束，由Kalman滤波公式得到融合后的定位结 果[x_p ^X x_p ^Y]，该融合的结果为UTM坐标系下的结果，再根据坐标转换关系： (x,y)＝trans^-1(x_p ^X,x_p ^Y)，得到在全局坐标系下精确地融合定位结果。由于解决了GPS在信 号差的区域定位结果不准和激光雷达定位存在累计误差的问题，本发明设备简单，定位结果 鲁棒性高，可以为车辆循迹导航提供精确的定位精度，适用于复杂环境下的循迹车辆的导航 定位，尤其适用于无人驾驶中智能车的定位。

本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明， 凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保 护范围之内。

Claims (10)

1.一种车辆定位和路径规划统一建模方法，其特征在于，包括以下步骤：

S1、处理激光点云数据，得到二维栅格地图；

S2、通过标定点云与GPS数据之间的映射关系，将激光坐标系中的车辆轨迹信息和二维栅格地图映射到GPS坐标系下；

S3、基于A星算法在二维栅格地图中确定路径规划的轨迹结果；

S4、以路径规划的轨迹作为约束轨迹，计算GPS定位结果与激光雷达定位结果到约束轨迹之间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果。

2.根据权利要求1所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S1包括：

S11、对激光点云数据进行预处理，剔除噪声点、离群点，同时过滤掉场景中路面点云信息，只保留场景中除路面信息之外的固有静态障碍信息；

S12、将预处理后的三维激光点云数据进行XOY平面的投影处理，然后将整个XOY平面均匀地划分为一定尺寸的栅格，规定存在点云的栅格为占据状态，无点云存在的栅格为自由状态，构建二维栅格地图。

3.根据权利要求2所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S11包括：

S111、采用直通滤波器保留距离车辆一定距离内的点云数据，即：若激光点云数据中某一点的坐标(x_i，y_i，z_i)满足：x_i∈(X₁,X₂)∩y_i∈(Y₁,Y₂)∩z_i∈(Z₁,Z₂)，则该点被保留；其中，X_i、Y_i和Z_i分别为激光点云坐标系沿X、Y和Z轴方向设定的阈值；

S112、采用体素网格滤波器对激光点云数据进行滤波降采样处理，将点云数据划分为多个体素网格，计算每一个体素网格的重心，以重心点代表体素网格中的所有点；

S113、将z坐标小于一定高度的点作为候选地面点集P'，并对这些点按照高度z从小到大排序，求出平均高度z_ave，再计算高度小于z_ave+z_threshold的点的法线，Z_threshold表示预设阈值，将与z轴的夹角大于预设角度的点保留，剩下的所有点作为最终地面点集P_ground，从而剔除最终地面点集P_ground。

4.根据权利要求1所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S2包括：

S21、在激光点云扫描范围内选择一标志物G；

S22、采集标志物G的GPS坐标位置信息(Gx，Gy，Gz)^T；

S23、提取标志物G在激光坐标系下的激光坐标位置(x，y，z)^T；

S24、根据最小二乘问题求出旋转矩阵R和平移矩阵T。

5.根据权利要求4所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S23包括：

S231、剔除激光点云中的离群点与噪声点；

S232、利用RANSAC算法拟合得到标志物立杆G的点集，通过计算其密度公式得到立杆的质心坐标位置(x，y，z)^T，以此坐标位置作为立杆G在激光坐标系下的激光坐标位置(x，y，z)^T。

6.根据权利要求4所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S24包括：

S241、标志物的坐标之间存在以下关系：

式中，R是激光坐标系到GPS坐标系下的旋转矩阵，T是激光坐标系到GPS坐标系下的平移矩阵；

S242、将Z方向的坐标做近似0的处理，上式转换如下：

式中，[x，y]^T是标志物G在GPS坐标系下的坐标位置，[Gx,Gy]^T是标志物G在激光坐标系下的坐标位置；

S243、近似处理后的式子可变为：

转换为方程组形式：

进而变为矩阵形式：

即可转换为最小二乘问题：Ax＝b，求解得出方程的解x＝(A^TA)^-1A^Tb即求解出旋转矩阵R和平移矩阵T。

7.根据权利要求1所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S3包括：

S31、在二维栅格地图中设定路径的起点与终点，确定起点与终点在二维栅格地图中的位置信息；

S32、根据A*算法的启发函数选择最优的方向展开搜索；

S33、计算欧几里得距离：

式中，(x_n,y_n)代表当前节点的坐标，(x_d,y_d)代表目标节点的坐标；

S34、结合A*算法使用OPEN列表和CLOSED列表来进行搜索，在栅格地图中生成由起点到终点的路径规划轨迹结果。

8.根据权利要求1所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S4包括：

S41、将当前车辆位置采集点记做点P，实时获取车辆位置采集点P的GPS位置信息[x,y]^T，并将其转换到UTM坐标系下：U_P＝(x_P,y_P)＝trans(x,y)；(x_P,y_P)为位置采集点P在UTM坐标系下的GPS位置信息，trans()为将GPS坐标系转换为UTM坐标系下的转换关系；

同时，根据旋转矩阵R和平移矩阵T以及GPS坐标系到UTM坐标系下的转换关系trans(),得出车辆位置采集点P从激光坐标系下的位置[Gx,Gy]^T转换到UTM坐标系下的位置：U_G＝(x_G,y_G)＝trans(R*[Gx,Gy]^T+T)；(x_G,y_G)为位置采集点P在UTM坐标系下的激光雷达位置信息；

根据变换关系将路径规划轨的迹转换到UTM坐标系下；

S42、分别计算GPS定位结果与激光雷达定位结果到规划轨迹之间的横向距离；

S43、以路径规划的轨迹作为约束轨迹，GPS定位结果与激光雷达定位结果到约束轨迹之间的横向距离作为卡尔曼滤波器中的约束，融合GPS定位与激光定位结果。

9.根据权利要求8所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S42包括：

S421、路径规划轨迹由直线段构成，由两点式直线方程计算出规划轨迹的直线方程：l:a_ix+b_iy+c_i＝0；

S422、根据车辆位置采集点当前时刻在UTM坐标系下的GPS位置信息U_P＝(x_P,y_P)及激光雷达位置信息U_G＝(x_G,y_G)，分别求解出横向距离：

10.根据权利要求9所述的车辆定位和路径规划统一建模方法，其特征在于，步骤S43包括：

S431、以当前位置x_k和N个历史位置共同组成的轨迹作为当前状态：X_k＝[x_k x_k-1 …x_k-N]，每个位置均包括横纵两个方向的信息：x_k＝[x_k ^X x_k ^Y]^T；基于运动学匀变速公式：X_k＝2X_k-1-X_k-2，得到状态转移矩阵

预测状态协方差矩阵

其中Q为GPS定位精度；

S432、融合当前车辆的GPS定位结果U_P和激光雷达定位结果U_G以及横向距离D₁、D₂，建立观测值z：λ₁U_P+λ₂U_G；当D₁＜D₂时，取

当D₂＜D₁时，取

S433、基于运动学匀变速运动公式预测下一时刻的位置，融合车辆的GPS定位结果U_P和激光雷达定位结果，以λ₁U_P+λ₂U_G作为观测值，以GPS定位结果与激光雷达定位结果到路径规划轨迹之间的横向距离作为卡尔曼滤波器中的约束，由kalman滤波公式得到融合后的定位结果[x_p ^X x_p ^Y]，该融合的定位结果为UTM坐标系下的结果。

Images