CN110146910B - 一种基于gps与激光雷达数据融合的定位方法及装置 - Google Patents

Abstract

本发明公开了一种基于GPS与激光雷达数据融合的定位方法，包括：获取GPS定位坐标；获取道路环境信息，建立道路环境特征模型；根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据所GPS定位坐标确定出关键栅格区域；将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配；根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标；采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度和航向角进行数据融合，得到最终的定位结果。本发明结合了GPS在全局定位的优势和激光雷达在局部定位的优势，大幅度提高了GPS定位系统的定位精度。

Description

一种基于GPS与激光雷达数据融合的定位方法及装置

技术领域

本发明属于定位技术领域，具体涉及一种基于GPS与激光雷达数据融合的定位方法及装置。

背景技术

车辆定位在自动驾驶汽车领域的研究中是一个关键问题，扮演着极为重要的角色。一个高精度的定位效果不仅仅使自动驾驶汽车的轨迹跟随控制变得更容易，还能基于位置共享实现车与车(V2V)，车与路边基础设施(V2I)以及车与城市网络的互联，是实现智能交通的必由之路。

近年来导航定位技术不断发展，按照所使用的传感器设备不同可以分为基于磁传感器阵列的定位技术、航迹推算(Dead reckoning,DR)、惯性导航(Inertial navigation)、卫星定位、视觉定位、基于激光雷达的定位技术等。而在具体系统实现过程中，各种定位技术可以相互独立使用，也可以使用多种技术相互组合使用。

由车辆定位技术的研究和应用现状可知，现有的各种定位方法仍然存在许多不足之处。对于单一传感器定位领域，GPS定位应用便捷但由于其测量原理存在较大的误差；差分GPS定位虽然能提高定位精度，但是需要一定范围内具有基准站，难以普及；基于惯导系统的定位方法存在不可忽视的累积误差；基于磁传感器阵列的导航方法虽然较为精确，但是需要提前铺设，灵活度极差。现有的多传感器融合定位方法中，GPS与无线局域网或4G网络的融合定位方法，能够有效的解决室内GPS信号遮挡的问题，实现无缝定位，但对于定位精度的提高十分有限；其余如GPS与惯导系统、GPS与视觉传感等的融合定位方法虽然能够提升一定的定位精度，但是仍然不能满足未来自动驾驶汽车的定位需求。

发明内容

鉴于以上所述现有技术的缺点，本发明提供一种基于GPS与激光雷达数据融合的定位方法及装置，以解决现有的定位技术精度不够高的缺陷。

为实现上述目的及其他相关目的，本发明提供一种基于GPS与激光雷达数据融合的定位方法，所述定位方法包括：

获取GPS定位坐标；

获取道路环境信息，建立道路环境特征模型；

根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据所GPS定位坐标确定出关键栅格区域；

将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配；

根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标；

采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

进行数据融合，得到最终的定位结果。

可选地，所述采用2D激光雷达进行检测，获取道路环境信息，建立道路环境特征模型，具体包括：

获取2D激光雷达数据，以激光雷达装置位置为坐标原点，环境数据为点簇数据；

对所述点簇数据进行预处理；

采用split-merge算法对预处理后的点簇数据进行聚类，建模。

可选地，所述对所述点簇数据进行预处理，具体包括：

对点簇数据进行中值滤波，得到滤波后的点簇数据；

对激光雷达数据作误差补偿；

以车辆中心为原点、横轴为车辆水平方向、纵轴为车辆直行方向将极坐标形式的激光雷达数据转换为直角坐标。

可选地，将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配，具体包括：

以道路环境模型中关键点{X_j,Y_j}为原点，按顺时针方向提取道路环境特征模型中的特征角度θ和距离d得到特征集合{θ₁,θ₂,θ₃,d₁,θ₄,d₂…}；

将所述特征集合{θ₁,θ₂,θ₃,d₁,θ₄,d₂…}与所述关键栅格区域中的道路关键点信息依次进行比对，在一定阈值范围内均匹配成功则认为道路关键点匹配成功。

可选地，所述车辆经纬度坐标(long_L,lat_L)表示为：

其中，longk表示道路关键点经度坐标，latk表示道路关键点纬度坐标，d为关键点与激光雷达的欧氏距离；α位车辆行驶方位角，ARC为地球平均半径。

可选地，采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

进行数据融合，得到最终的定位结果，其中，扩展卡尔曼滤波器的状态方程和观测方程如下：

式中：f()是非线性系统的状态转移函数，H()是非线性系统的观测函数，w(k)和v(k)为系统噪声；U(k)是输入值，X(k)是状态变量，Z(k)是观测值；

则定位系统模型如下：

式中：t为采样间隔时间，x_k,y_k分别为所求定位经度、纬度。

为实现上述目的及其他相关目的，本发明还提供一种基于GPS与激光雷达数据融合的定位装置，该装置包括：

坐标获取模块，用于获取GPS定位坐标；

模型建立模块，用于获取道路环境信息，建立道路环境特征模型；

特征提取模块，用于根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据所GPS定位坐标确定出关键栅格区域；

特征匹配模块，用于将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配；

坐标计算模块，用于根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标；

数据融合模块，用于采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

进行数据融合，得到最终的定位结果。

为实现上述目的及其他相关目的，本发明还提供一种可读存储介质，存储计算机程序，所述计算机程序被处理器运行时执行所述的定位方法。

为实现上述目的及其他相关目的，本发明还提供一种电子终端，包括：处理器及存储器；

所述存储器用于存储计算机程序，所述处理器用于执行所述存储器存储的计算机程序，以使所述终端执行所述的定位方法。

如上所述，本发明的一种基于GPS与激光雷达数据融合的定位方法、装置，具有以下有益效果：

本发明一种基于GPS与激光雷达数据融合的定位方法，结合了GPS在全局定位的优势和激光雷达在局部定位的优势，运用GPS先进行粗定位确定大致区域范围，再利用激光雷达对附近环境进行建模、提取特征，与历史环境信息进行匹配，得到局部精确定位，最后将多种传感器的数据进行融合，得到最终定位结果，大幅度提高了GPS定位系统的定位精度。

附图说明

图1是本发明实施例的一种基于GPS与激光雷达数据融合的定位方法的流程图；

图2是本发明实施例的激光雷达数据修正几何示意图；

图3是本发明实施例的激光雷达数据split-merge聚类算法示意图；

图4是本发明实施例的道路环境特征模型图；

图5是本发明实施例的特征提取示意图；

图6是本发明实施例的经纬度计算示意图；

图7是本发明实施例的基于EKF的数据融合定位；

图8是本发明实施例的数据融合的三个阶段。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

如图1所示，本实施例的一种基于GPS与激光雷达数据融合的定位方法，提高了GPS定位精度，包括如下步骤：

步骤S1、获取GPS定位坐标；

具体地，将GPS原始数据从WGS-84坐标系转换为BD-09坐标系数据(long_G,lat_G)，其中long_G为经度，lat_G为纬度，则GPS定位坐标表示为(long_G,lat_G)。

步骤S2、获取道路环境信息，建立道路环境特征模型；具体地，采用2D激光雷达进行检测获取道路环境信息。

更加具体地，步骤S2包括以下子步骤：

步骤S21：获取2D激光雷达数据，以激光雷达装置位置为坐标原点，激光雷达数据为点簇数据{(L_i,θ_i)|i＝1,2,3,...,n}，其中点簇数据为极坐标形式，θ_i表示角度，L_i表示距离。

步骤S22：对激光雷达点簇数据进行预处理，预处理步骤包括：

步骤S221：对点簇数据进行中值滤波，得到滤波后的点簇数据{(L_i,θ_i)|j＝1,2,3,...,m,m＜n}，算法如下：

fori＝1to n；

j＝1；

L_j＝mid(L_i,L_i+1,L_i+2)；

j++；

步骤S222：激光雷达扫描一帧数据大约100ms时间，在这100ms中车辆处于移动过程中，激光束扫描过程中原点发生了变化，需要对激光雷达数据作误差补偿，如图2所示，公式如下：

for j＝1to m

式中：T为激光雷达扫描周期；γ为激光雷达扫描的角度分辨率；v为车辆行驶速度；S_t为当前时刻激光雷达数据中序号为i的所测距离；S_(t-T)为上一时刻激光雷达数据中序号为i的所测距离；Θ_j为所测障碍物与横坐标轴的夹角；S_real为修正后的激光雷达数据。

步骤S223：将极坐标形式的激光雷达数据转换为直角坐标，以车辆中心为原点，横轴为车辆水平方向，纵轴为车辆直行方向。

式中：l为车辆纵向长度。

步骤S23：采用split-merge算法对激光雷达数据进行聚类，建模；拟合方法采用最小二乘法，如图3所示，算法如下：

其中，采用IEPF(Iterative End Point Fit)方法进行线段类特征提取，

(1)取出激光雷达点簇数据，放入ListA中；

(2)取A中首尾两点拟合成直线l₁；

(3)搜索ListA中距离直线l₁最远的点，得到距离d；

(4)如果d小于分割阈值d_max，则执行步骤(6)；

(5)否则，将l₁分割为l₂和l₃，则执行步骤(2)；

(6)取各分割段间所有点，用最小二乘法拟合，最小二乘拟合公式如下：

式中：

为数据算术平均值；

β₀,β₁为回归系数，

为参数β₀,β₁的最小二乘估计值.

经步骤(6)得到线段集合{l₁,l₂,l₃......l_n}，确定线段端点，如图4所示，得到最终道路环境特征模型如下：

其中，

为线段端点，(x_j,y_j)为线段交点,

步骤S3：GPS误差最大约为30m，根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据步骤S1获得的GPS定位坐标(long_G,lat_G)确定出关键栅格区域。

步骤S4：采用步骤S2中建立的道路环境特征模型与关键栅格区域中的道路关键点信息进行匹配。

步骤S41：以道路环境特征模型中关键点{X_j,Y_j}为原点，按顺时针方向提取道路环境特征模型中可用特征角度θ和距离d得到{θ₁,θ₂,θ₃,d₁,θ₄,d₂…}，如图5所示，公式如下：

式中：k为直线斜率；d为原点到直线的距离；θ为从原点作各直线的垂线之间的夹角。

步骤S42：按上述特征提取后得到特征集合{θ₁,θ₂,θ₃,d₁,θ₄,d₂…}，与关键栅格区域中记录的道路关键点特征参数依次进行比对，在一定阈值范围内均匹配成功则认为关键点匹配成功，否则重复步骤S2、S3、S4。

步骤S5：根据匹配成功的道路关键点经纬度坐标(long_k,lat_k)和关键点对于激光雷达的坐标(x_j,y_j)计算得到车辆经纬度定位坐标(long_L,lat_L)，如图6所示，公式如下：

式中：d为关键点与激光雷达的欧氏距离；α位车辆行驶方位角，以正北方向为0°；ARC为地球平均半径，约为6371.393千米；(long_k,lat_k)为关键点经纬度坐标；(long_L,lat_L)为求取的车辆经纬度坐标。

步骤S6：经上述步骤得到由GPS获得的经纬度坐标(long_G,lat_G)，由激光雷达获得的经纬度坐标(long_L,lat_L)，结合由车辆直接得到的速度v和航向角

采用扩展卡尔曼滤波器(EKF)对以上数据进行融合，获得最终的定位结果。如图7所示。EKF的状态方程和观测方程如下：

式中：f()是非线性系统的状态转移函数；H()是非线性系统的观测函数；w(k)和v(k)为系统噪声；U(k)是输入值，X(k)是状态变量，Z(k)是观测值。

本发明的组合定位系统模型如下：

式中：v为车辆速度；t为采样间隔时间；θ为车辆航向方位角，以正北为0°；ARC为地球平均半径；w(k)为系统噪声；x_k,y_k分别为所求定位经度、纬度。

基于扩展卡尔曼滤波器数据融合的具体计算公式如下：

X(k)＝X(k|k-1)+K(k)×(Z(^k)-H(X(k|k^-1)))

P(k|k)＝P(k|k-1)-K(k)P(k|k-1)

式中：A_j为函数f的Jacobian矩阵，H_j为函数H的Jacobian矩阵；f为非线性系统的状态转移函数，H是非线性系统的观测函数；v(k)为系统噪声；P为状态变量协方差矩阵；Q为过程噪声协方差矩阵；R为观测噪声协方差矩阵；K()为卡尔曼增益；v为车辆速度；t为采样间隔时间；θ为车辆航向方位角，以正北为0°；ARC为地球平均半径。

本发明将数据融合过程分为三个阶段，如图8所示。

阶段一：检测到关键点之前，该阶段由于激光雷达还未检测到道路特征关键点，无法与地图进行匹配，因此经纬度坐标数据来源只有来自于GPS数据。该阶段的观测值为GPS获取的经纬度坐标(long_G,lat_G)，EKF的具体参数如下表：

阶段二：检测到关键点，该阶段激光雷达能够检测到道路特征关键点，通过本文基于GPS和激光雷达的精确定位方法可以获取到相对精确的经纬度坐标(long_L,lat_L)，将该数据作为观测值，EKF的具体参数如下表：

阶段三：离开关键点，该阶段虽然失去了道路关键点，但上一时刻的最优估计坐标对后续一段时间的数据具有一定的修正作用，将GPS数据(long_G,lat_G)作为观测值，EKF具体参数如下表：

本发明一种基于GPS与激光雷达数据融合的定位方法，采用GPS进行全局定位确定大致区域，再利用激光雷达在局部进行精确定位，最后将二者数据融合获得定位结果，能够有效地提高GPS定位系统的定位精度，而且GPS与激光雷达均为自动驾驶汽车必备传感器，不需额外添加设备。因此，本发明一种基于GPS与激光雷达数据融合的定位方法具有较高的实用性。

本实施例还提供一种基于GPS与激光雷达数据融合的定位装置，该装置包括：

坐标获取模块，用于获取GPS定位坐标；

模型建立模块，用于获取道路环境信息，建立道路环境特征模型；

特征提取模块，用于根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据所GPS定位坐标确定出关键栅格区域；

特征匹配模块，用于将所述道路环境特征模型与所述关键栅格区域中的道路关键点信息进行匹配；

坐标计算模块，用于根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标；

数据融合模块，用于采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

进行数据融合，得到最终的定位结果。

需要说明的是，由于装置部分的实施例与方法部分的实施例相互对应，因此装置部分的实施例的内容请参见方法部分的实施例的描述，这里暂不赘述。

本发明还提供一种存储介质，存储计算机程序，所述计算机程序被处理器运行时执行前述的定位方法。

本发明还提供一种电子终端，包括：

存储器，用于存储计算机程序；

处理器，用于执行所述存储器存储的计算机程序，以使所述设备执行前述的定位方法。

所述处理器可以是中央处理单元(Central Processing Unit,CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(FieldProgrammable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器可以是内部存储单元或外部存储设备，例如插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字卡(Secure Digital,SD)，闪存卡(Flash Card)等。进一步地，所述存储器还可以既包括内部存储单元，也包括外部存储设备。所述存储器用于存储所述计算机程序以及其他程序和数据。所述存储器还可以用于暂时地存储己经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括:能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM,Read-Only Memory)、随机存取存储器((RAM,Random Access Memory)、电载波信号、电信信号以及软件分发介质等。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (8)

1.一种基于GPS与激光雷达数据融合的定位方法，其特征在于，所述定位方法包括：

获取GPS定位坐标；

采用2D激光雷达进行检测，获取道路环境信息，建立道路环境特征模型；

根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据所GPS定位坐标确定出关键栅格区域；

将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配；

根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标；

采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

进行数据融合，得到最终的定位结果，其中，扩展卡尔曼滤波器的状态方程和观测方程如下：

式中：f()是非线性系统的状态转移函数，H()是非线性系统的观测函数，w(k)和v(k)为系统噪声；U(k)是输入值，X(k)是状态变量，Z(k)是观测值；

则定位系统模型如下：

式中：t为采样间隔时间，x_k,y_k分别为所求定位经度、纬度。

2.根据权利要求1所述的一种基于GPS与激光雷达数据融合的定位方法，其特征在于，

所述采用2D激光雷达进行检测，获取道路环境信息，建立道路环境特征模型，具体包括：

获取2D激光雷达数据，以激光雷达装置位置为坐标原点，环境数据为点簇数据；

对所述点簇数据进行预处理；

采用split-merge算法对预处理后的点簇数据进行聚类，建模。

3.根据权利要求2所述的一种基于GPS与激光雷达数据融合的定位方法，其特征在于，

所述对所述点簇数据进行预处理，具体包括：

对点簇数据进行中值滤波，得到滤波后的点簇数据；

对激光雷达数据作误差补偿；

以车辆中心为原点、横轴为车辆水平方向、纵轴为车辆直行方向将极坐标形式的激光雷达数据转换为直角坐标。

4.根据权利要求1所述的一种基于GPS与激光雷达数据融合的定位方法，其特征在于，

将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配，具体包括：

以道路环境模型中关键点{X_j,Y_j}为原点，按顺时针方向提取道路环境特征模型中的特征角度θ和距离d得到特征集合{θ₁,θ₂,θ₃,d₁,θ₄,d₂…}；

将所述特征集合{θ₁,θ₂,θ₃,d₁,θ₄,d₂…}与所述关键栅格区域中的道路关键点信息依次进行比对，在一定阈值范围内均匹配成功则认为道路关键点匹配成功。

5.根据权利要求1所述的一种基于GPS与激光雷达数据融合的定位方法，其特征在于，

所述车辆经纬度坐标(long_L,lat_L)表示为：

其中，longk表示道路关键点经度坐标，latk表示道路关键点纬度坐标，d为关键点与激光雷达的欧氏距离；α位车辆行驶方位角，ARC为地球平均半径。

6.一种基于GPS与激光雷达数据融合的定位装置，其特征在于，该装置包括：

坐标获取模块，用于获取GPS定位坐标；

模型建立模块，用于采用2D激光雷达进行检测，获取道路环境信息，建立道路环境特征模型；

特征提取模块，用于根据GPS误差将地图按一定的分辨率分割成为栅格地图，根据所GPS定位坐标确定出关键栅格区域；

特征匹配模块，用于将所述道路环境特征模型与所述关键栅格区域中的道路关键点特征参数进行匹配；

坐标计算模块，用于根据匹配成功的道路关键点的经纬度坐标和该道路关键点相对于激光雷达的坐标计算得到车辆经纬度坐标；

数据融合模块，用于采用扩展卡尔曼滤波器对所述GPS定位坐标、所述车辆经纬度坐标、车辆速度v和航向角

进行数据融合，得到最终的定位结果，其中，扩展卡尔曼滤波器的状态方程和观测方程如下：

式中：f()是非线性系统的状态转移函数，H()是非线性系统的观测函数，w(k)和v(k)为系统噪声；U(k)是输入值，X(k)是状态变量，Z(k)是观测值；

则定位系统模型如下：

式中：t为采样间隔时间，x_k,y_k分别为所求定位经度、纬度。

7.一种存储介质，存储计算机程序，其特征在于，所述计算机程序被处理器运行时执行如权利要求1至5中任一项所述的定位方法。

8.一种电子终端，其特征在于，包括：处理器及存储器；

所述存储器用于存储计算机程序，所述处理器用于执行所述存储器存储的计算机程序，以使所述终端执行如权利要求1至5中任一项所述的定位方法。

Images