CN108981702A - 一种多位置联合粒子滤波的车辆定位方法 - Google Patents

Abstract

本发明公开了一种多位置联合粒子滤波的车辆定位方法。建立车辆行驶区域的路网地图，实时通过航位推算方法获得车辆位置并探测锚点，形成初始的车辆分段行驶轨迹；对于每次探测到的锚点，使用一堆粒子来表示锚点在路网地图上对应的位置，通过联合粒子滤波实时计算更新每一堆粒子权重；对于每一堆粒子，以权重最大的粒子作为锚点在路网地图上的定位点，依次连接定位点作为修正后的车辆分段行驶轨迹。本发明能有效修正视觉里程计等航位推算方法产生的累积误差，在车辆长距离的行驶过程中均能获得良好的定位精度，计算代价小，实时性强。

Description

一种多位置联合粒子滤波的车辆定位方法

技术领域

本发明涉及智能车辆的导航定位技术方法，特别是涉及了一种多位置联合粒子滤波的车辆定位方法。

背景技术

定位是指车辆决定自己位置的能力，可靠和准确的车辆定位是自主车辆导航的先决条件。为了解决这个问题，传统的导航***如GPS、轮式里程计都得到了广泛的应用。然而，由于众所周知的“城市峡谷”效应，GPS在城市环境容易受到遮挡而无法获得良好的定位精度。在这种情况下，通常需要昂贵的GPS/INS***来提供车辆位置。而轮式里程计在长距离时也会受到车轮打滑等影响而产生误差。目前，许多基于视觉的二维和三维定位方法被不断提出。虽然激光雷达等三维传感器可以准确地获得地标的位置，但其高成本限制了其广泛使用。另一方面，视觉里程计是一种仅使用单个或多个摄像机的图像来估计车辆运动的方法。由于它的高性价比，视觉里程计慢慢成为导航***中一个有价值的潜在定位组件。然而，由于视觉里程计逐步计算车辆位置，每一帧都会引入的微小误差，并随着时间的推移而累积。减少漂移最常用的方法是捆束调整(BA)和同时定位与地图构建(SLAM)。BA通过最小化整体重投射误差的成本函数给出最优解。SLAM执行闭环以减少漂移积累，通过约束轨迹闭合误差来减少定位累计误差。

近年来，各类地图服务(如OpenStreetMap)不断普及，各类城市路网地图变得十分容易获得，将其作为先验信息，可以用来帮助约束定位错误。本发明利用路网地图，通过锚点的表示方法和灵活的多位置联合粒子滤波方法来实现对累计误差的修正，从而使车辆的定位更为可靠。

发明内容

为了解决背景技术中存在的问题，本发明的目的在于提供一种多位置联合粒子滤波的车辆定位方法，适用于面向城市环境下的智能车辆定位。

本发明在视觉里程计或者惯导***的支持下，可以获得车辆当前的初步定位。将获得的车辆运动轨迹和已知的路网地图结合，将运动轨迹按锚点位置分段后通过对每个锚点位置进行滤波，修正每个锚点的位置，从而消除航位推算方法产生的累积误差，使定位更加精确。

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

(1)建立车辆行驶区域的路网地图，路网地图由包含节点和边的拓扑图结构构成，车辆行驶起点在路网地图中的位置已知；

(2)实时通过航位推算方法获得车辆位置并探测锚点，形成初始的车辆分段行驶轨迹；

(3)对于每次探测到的锚点，使用一堆粒子来表示锚点在路网地图上对应的位置，通过联合粒子滤波实时计算更新每一堆粒子权重；

(4)对于每一堆粒子，以权重最大的粒子作为锚点在路网地图上的定位点，依次连接定位点作为修正后的车辆分段行驶轨迹。

所述的航位推算方法采用视觉里程计等。

所述步骤(1)中，用包含节点和边的拓扑图结构来建立路网地图：节点n＝(lat,lon)的位置由经纬度表示，lat,lon分别表示纬度和经度，边e＝(n₁,n₂)表示节点n₁和n₂之间的连接关系。

所述步骤(2)具体为：实时通过航位推算方法获得每个时刻的车辆位置，形成原始车辆行驶轨迹；实时探测原始车辆行驶轨迹中的锚点，标记各个锚点并将锚点前后依次线段连接形成车辆分段行驶轨迹。

所述步骤(2)，具体采用以下两种方式同时探测：

(2.1)通过航位推算方法实时得到当前车辆位置，将当前车辆位置与最近位置点用直线相连，计算直线方程；若当前车辆位置之前还没有探测到锚点，以车辆行驶起点作为最近位置点；若当前车辆位置之前已经探测到锚点，以离当前车辆位置时间最近的锚点作为最近位置点；

计算当前车辆位置与最近位置点之间各个时刻的车辆位置到直线的垂直距离，将垂直距离和预先设定的距离阈值比较：如果每个垂直距离均小于距离阈值，则认为当前车辆位置与最近位置点之间没有探测到新锚点；如果存在至少一时刻的车辆位置到直线的垂直距离不小于距离阈值，则找到垂直距离最大时刻所在的车辆位置作为新锚点；

所述的各个帧的车辆位置为通过航位推算方法实时得到的车辆的当前初步位置。

(2.2)为了防止行驶距离过长却仍未探测到锚点的状况，同时当当前车辆位置距离最近位置点行驶超过预先设定的路程阈值时，直接将当前车辆位置作为新锚点；

对于每探测到的新锚点，将其与最近位置点用直线相连，由此将各个锚点依次连接构成车辆分段行驶轨迹，相邻锚点的连线形成线段，车辆在两个锚点间的行驶轨迹用该线段近似表示。

所述步骤(3)中，将车辆的行驶轨迹分段后，用一堆粒子来表示每个锚点在路网地图上可能对应的位置然后进行联合滤波过程，具体如下步骤：

对于每个当前锚点，在路网地图中找到和当前锚点之间距离小于误差阈值范围内的节点作为与当前锚点对应的节点，然后按照以下三种不同情况区分处理：

(1)找到一个与当前锚点对应的节点，在该节点附近生成一堆粒子，具体实施以该节点为中心高斯分布生成粒子，并直接计算各个粒子的权重；

(2)找到两个以上与当前锚点对应的节点，在这些每个节点附近生成一堆粒子，粒子均将保留与当前锚点之后的锚点产生的粒子进行联合滤波(先不直接计算，而是和之后锚点的粒子进行一起计算)，得到各个粒子权重；

(3)没有找到与当前锚点对应的节点，直接在和当前锚点之间垂直距离小于误差阈值范围内的边上生成粒子，粒子均将保留与当前锚点之后的锚点产生的粒子进行联合滤波(先不直接计算，而是和之后锚点的粒子进行一起计算)，得到各个粒子权重。

所述的粒子权重采用以下方式计算：

首先，将粒子与相邻锚点下的所有各个粒子进行连接组合形成候选路径，相邻锚点为与粒子对应锚点前后相邻的锚点，计算每一条组合形成的候选路径的长度和朝向，与车辆分段行驶轨迹中的线段进行比较，通过以下公式(1)和(2)计算长度和角度误差：

e_h＝|h_c-h_s|

式中，l_c为候选路径的长度，l_s为候选路径在与车辆分段行驶轨迹中对应的线段的长度，e_l为长度误差；h_c为候选路径的朝向，h_s为候选路径在与车辆分段行驶轨迹中对应的线段的朝向，e_h为朝向角度误差；

再采用以下公式计算每一条组合形成的候选路径与车辆分段行驶轨迹中对应的线段之间的相似度s_s：

s_s＝α*s_l+(1-α)*s_h

其中，s_s为候选路径和车辆分段行驶轨迹中对应的线段之间的相似度，s_l为长度的相似度，s_h为朝向角的相似度，α作为权重参数，其大小由实验获得；

接着，对于每一条组合形成的候选路径，候选路径是由N条锚点相连线段连接形成，采用以下公式计算N条线段相似度的平均值作为候选路径的相似度：

其中，s_p为候选路径的相似度，s_s[i]为候选路径包含的第i条线段，N表示候选路径所包含的线段总数；

然后，同一个粒子可以在不同的候选路径，对于一个包含Q条不同候选路径的粒子，即对于一个粒子，在Q条不同候选路径出现，采用以下公式计算粒子的权重：

其中，w为粒子的权重，s_p[j]为粒子所在的第j条候选路径，Q表示粒子所出现的候选路径的总条数；

最后，采用以下公式对一个锚点下的所有粒子权重进行归一化，：

其中，M表示锚点下的粒子的总数，w[k]表示锚点下的第k个粒子的权重，表示归一化后锚点下的第k个粒子的权重。

所述步骤(4)中，以锚点下权重最大的粒子作为锚点在路网地图上的定位点，依次连接定位点作为修正后的车辆分段行驶轨迹之后，相邻两个定位点之间的最终车辆位置采用以下方式确定：

对于锚点AP1和锚点AP2之间的某一时刻f，通过计算该时刻f的车辆位置到锚点AP1的距离以及锚点AP1到锚点AP2之间的距离的比值来计算获得时刻f定位修正后的位置；

式中，和分别表示两个锚点的位置，p_AP1和p_AP2分别表示两个锚点和对应的定位点，为时刻f的车辆位置，p_f为时刻f定位修正后的车辆位置。

本发明首先采用图模型建立车辆行驶区域的路网地图。通过视觉里程计等航位推算方法得到原始车辆行驶轨迹。为了有效地匹配各种弯曲轨迹与道路，本发明提出了一种基于锚点(AP)的表示方法，锚点表征了轨迹上主要的弯曲点。将车辆轨迹按照锚点进行分段后，通过实施多位置联合粒子滤波的方法来修正每个锚点在路网地图上的对应位置，从而消除航位推算方法产生的累积误差，提高定位的精度。

与背景技术相比，本发明具有的有益效果是：

(1)本发明能可单纯依赖视觉或者惯导等航位推算方法，结合路网地图，即可实现可靠定位；

(2)本发明能有效修正航位推算算法产生的累积误差，提高长距离定位的精度；

(3)本发明能有效应对直线路段、曲线路段等多种道路情况；

(4)本发明使用多位置联合滤波的方法框架，与传统的每帧数据到来时刻进行粒子滤波的方法不同，只在轨迹锚点处进行粒子滤波，大大减小了运算量，并且与传统的每帧得到滤波结果不同，实施多位置的联合滤波，结合前后信息，提高了定位效果。

综合来说，本发明能有效修正视觉里程计等航位推算方法产生的累积误差，在直线、曲线等路段均能适用，并在车辆长距离的行驶过程中均能获得良好的定位精度。同时本发明具有实时获取轨迹中的锚点，并自适应地估计一系列锚点位置的能力。另外，与传统的粒子滤波方法相比，本方法只在轨迹的锚点位置进行滤波，计算代价小，实时性强。

附图说明

图1是实施例的路网地图示意图；

图2是车辆原始轨迹分段及锚点示意图；

图3是多位置联合粒子滤波实施步骤示意图；

图4是KITTY00序列视觉里程计结果图；

图5是KITTY00序列本方法结果图；

图6是KITTY00序列真值轨迹图；

图7是KITTY00序列视觉里程计和本方法定位误差比较图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

按照本发明发明内容完整方法实施的实施例如下：

(1)建立车辆行驶区域的路网地图，如图1所示，路网地图由包含节点和边的拓扑图结构构成，车辆行驶起点在路网地图中的位置已知；

(2)实时通过航位推算方法获得每个时刻的车辆位置，形成原始车辆行驶轨迹；实时探测原始车辆行驶轨迹中的锚点，标记各个锚点并将锚点前后依次线段连接形成车辆分段行驶轨迹，如图2所示。

(2.1)通过航位推算方法实时得到当前车辆位置，将当前车辆位置与最近位置点用直线相连；若当前车辆位置之前还没有探测到锚点，以车辆行驶起点作为最近位置点；若当前车辆位置之前已经探测到锚点，以离当前车辆位置时间最近的锚点作为最近位置点；计算当前车辆位置与最近位置点之间各个时刻的车辆位置到直线的垂直距离，将垂直距离和预先设定的距离阈值比较：如果每个垂直距离均小于距离阈值，则认为当前车辆位置与最近位置点之间没有探测到新锚点；如果存在至少一时刻的车辆位置到直线的垂直距离不小于距离阈值，则找到垂直距离最大时刻所在的车辆位置作为新锚点；

(2.2)同时当当前车辆位置距离最近位置点行驶超过预先设定的路程阈值时，直接将当前车辆位置作为新锚点；对于每探测到的新锚点，将其与最近位置点用直线相连，由此将各个锚点依次连接构成车辆分段行驶轨迹，相邻锚点的连线形成线段。

(3)对于每次探测到的锚点，使用一堆粒子来表示锚点在路网地图上对应的位置，通过联合粒子滤波实时计算更新每一堆粒子权重；

下面以一个简单的例子(由图3所示)来表述多位置联合粒子滤波的思想与具体实施步骤。图3(a)为航位推算方法得到车辆原始轨迹并实施探测锚点而得到的车辆分段后的轨迹，轨迹中有五个锚点(把终点也当作锚点处理)，因此我们用五堆粒子来表示这五个锚点在地图中对应的位置。以下为具体滤波的步骤：

步骤一：假设起始节点在路网中位置已知(节点0)，当我们在探测到第一个锚点(AP1)，路网地图中两个节点(灰色区域)被认为是当前锚点可能的对应节点，我们在这两个节点附近生成粒子，在这种情况下，无需立即启动滤波，各个粒子均被保留，AP1的位置将在之后的步骤中确定，如图3(b)所示。

步骤二：在图3(c)中，探测到了一个新的锚点(AP2)。我们发现AP2的对应节点可以唯一确定，我们在对应节点附近生成一堆新的粒子。由于AP2在地图上对应的节点确定，AP1对应的节点也同时可以确定。此时实施多位置联合滤波算法，与两个锚点对应的两组粒子的权重得到了更新。每组粒子中权重最大的粒子作为锚点在路网地图上的定位点。然后将起点与这两个定位点依次相连，得到与滤波结果对应的修正路径，如图3(c)所示虚线轨迹。

步骤三：当探测到一个新的锚点(AP3)时，由于可能存在的车辆轨迹及路网地图误差，在阈值范围内无法找到AP3在路网中可能对应的节点。此时，我们直接在符合误差阈值范围的路网地图的边上生成粒子，粒子均将保留与后续锚点产生的粒子进行联合滤波，如图3(d)所示。在这种情况下，AP3的位置将在之后的步骤中确定。

步骤四：当检测到一个新的锚点(AP4)时，其在路网中的对应节点可以很好地确定，如图3(e)所示。与步骤二中的情况类似，实施多位置联合滤波，得到AP3和AP4在路网地图上的定位点。

步骤五：当检测到一个新的锚点(AP5)时，可以直接确定其对应的节点，实施多位置联合滤波决定其最终位置。最后，根据图3(f)中的虚线，可以得到车辆原始轨迹的修正路径。

(4)对于每一堆粒子，以权重最大的粒子作为锚点在路网地图上的定位点，依次连接定位点作为修正后的车辆分段行驶轨迹。

测试KITTY数据集来评估本方法的效果，以双目图像序列以及对应的路网地图作为输入，根据KITTY提供的真值，比较视觉里程计和本方法与真值的平均误差和最大误差。

实验主要测试了KITTY图像序列00。序列00视觉里程计的结果如图4所示，本方法的结果如图5所示，真值如图6所示，可以看出视觉里程计在拐弯处累积了较大的角度误差，导致定位的累积误差大大增加，而本方法可以有效地消除累积误差，使定位误差一直维持在一个比较小的范围内，且车辆行驶距离较长的情况下，也一直有较好的定位结果。误差对比曲线如图7所示，平均定位误差从55.8m减小到3.5m，最大定位误差从181.1m减小到9.2m，详见下表1。

表1视觉里程计与本方法定位误差比较

序列00	平均误差/m	最大误差/m
			视觉里程计	55.8	181.1
本方法	3.5	9.2

实验采用一台配备主频为3.4GHz的intel i7处理器和8G内存的电脑，实验中每次迭代平均耗时115.21ms。由于本方法滤波过程只在轨迹锚点处发生，无需每帧进行，所以相对于原始视觉里程计，增加的计算代价很小。

可以看出，采用本发明方法能有效地减小视觉里程计产生的累积误差，即使在较长的行驶路径中，定位误差也一直处于一个比较小的范围内，而且计算代价较小。

Claims (7)

1.一种多位置联合粒子滤波的车辆定位方法，其特征在于，包括如下步骤：

(1)建立车辆行驶区域的路网地图，路网地图由包含节点和边的拓扑图结构构成，车辆行驶起点在路网地图中的位置已知；

(2)实时通过航位推算方法获得车辆位置并探测锚点，形成初始的车辆分段行驶轨迹；

(3)对于每次探测到的锚点，使用一堆粒子来表示锚点在路网地图上对应的位置，通过联合粒子滤波实时计算更新每一堆粒子权重；

(4)对于每一堆粒子，以权重最大的粒子作为锚点在路网地图上的定位点，依次连接定位点作为修正后的车辆分段行驶轨迹。

2.根据权利要求1所述的一种多位置联合粒子滤波的车辆定位方法，其特征在于：所述步骤(1)中，用包含节点和边的拓扑图结构来建立路网地图：节点n＝(lat,lon)的位置由经纬度表示，lat,lon分别表示纬度和经度，边e＝(n₁,n₂)表示节点n₁和n₂之间的连接关系。

3.根据权利要求1所述的一种多位置联合粒子滤波的车辆定位方法，其特征在于：所述步骤(2)具体为：实时通过航位推算方法获得每个时刻的车辆位置，形成原始车辆行驶轨迹；实时探测原始车辆行驶轨迹中的锚点，标记各个锚点并将锚点前后依次线段连接形成车辆分段行驶轨迹。

4.根据权利要求1或3所述的一种多位置联合粒子滤波的车辆定位方法，其特征在于：所述步骤(2)，具体采用以下两种方式同时探测：

(2.1)通过航位推算方法实时得到当前车辆位置，将当前车辆位置与最近位置点用直线相连；若当前车辆位置之前还没有探测到锚点，以车辆行驶起点作为最近位置点；若当前车辆位置之前已经探测到锚点，以离当前车辆位置时间最近的锚点作为最近位置点；

计算当前车辆位置与最近位置点之间各个时刻的车辆位置到直线的垂直距离，将垂直距离和预先设定的距离阈值比较：如果每个垂直距离均小于距离阈值，则认为当前车辆位置与最近位置点之间没有探测到新锚点；如果存在至少一时刻的车辆位置到直线的垂直距离不小于距离阈值，则找到垂直距离最大时刻所在的车辆位置作为新锚点；

(2.2)同时当当前车辆位置距离最近位置点行驶超过预先设定的路程阈值时，直接将当前车辆位置作为新锚点；

对于每探测到的新锚点，将其与最近位置点用直线相连，由此将各个锚点依次连接构成车辆分段行驶轨迹，相邻锚点的连线形成线段。

5.根据权利要求1所述的一种多位置联合粒子滤波的车辆定位方法，其特征在于：所述步骤(3)中，将车辆的行驶轨迹分段后，用一堆粒子来表示每个锚点在路网地图上可能对应的位置然后进行联合滤波过程，具体如下步骤：

对于每个当前锚点，在路网地图中找到和当前锚点之间距离小于误差阈值范围内的节点作为与当前锚点对应的节点，然后按照以下三种不同情况区分处理：

(1)找到一个与当前锚点对应的节点，在该节点附近生成一堆粒子，并直接计算各个粒子的权重；

(2)找到两个以上与当前锚点对应的节点，在这些每个节点附近生成一堆粒子，粒子均将保留与当前锚点之后的锚点产生的粒子进行联合滤波，得到各个粒子权重；

(3)没有找到与当前锚点对应的节点，直接在和当前锚点之间垂直距离小于误差阈值范围内的边上生成粒子，粒子均将保留与当前锚点之后的锚点产生的粒子进行联合滤波，得到各个粒子权重。

6.根据权利要求1所述的一种多位置联合粒子滤波的车辆定位方法，其特征在于：所述的粒子权重采用以下方式计算：

首先，将粒子与相邻锚点下的所有各个粒子进行连接组合形成候选路径，相邻锚点为与粒子对应锚点前后相邻的锚点，计算每一条组合形成的候选路径的长度和朝向，与车辆分段行驶轨迹中的线段进行比较，通过以下公式(1)和(2)计算长度和角度误差：

e_h＝|h_c-h_s|

式中，l_c为候选路径的长度，l_s为候选路径在与车辆分段行驶轨迹中对应的线段的长度，e_l为长度误差；h_c为候选路径的朝向，h_s为候选路径在与车辆分段行驶轨迹中对应的线段的朝向，e_h为朝向角度误差；

再采用以下公式计算每一条组合形成的候选路径与车辆分段行驶轨迹中对应的线段之间的相似度s_s：

s_s＝α*s_l+(1-α)*s_h

其中，s_s为候选路径和车辆分段行驶轨迹中对应的线段之间的相似度，s_l为长度的相似度，s_h为朝向角的相似度，α作为权重参数；

接着，对于每一条组合形成的候选路径，候选路径是由N条锚点相连线段连接形成，采用以下公式计算候选路径的相似度：

其中，s_p为候选路径的相似度，s_s[i]为候选路径包含的第i条线段，N表示候选路径所包含的线段总数；

然后，对于一个粒子，在Q条不同候选路径出现，采用以下公式计算粒子的权重：

其中，w为粒子的权重，s_p[j]为粒子所在的第j条候选路径，Q表示粒子所出现的候选路径的总条数；

最后，采用以下公式对一个锚点下的所有粒子权重进行归一化，：

其中，M表示锚点下的粒子的总数，w[k]表示锚点下的第k个粒子的权重，表示归一化后锚点下的第k个粒子的权重。

7.根据权利要求1所述的一种多位置联合粒子滤波的车辆定位方法，其特征在于：

所述步骤(4)中，以锚点下权重最大的粒子作为锚点在路网地图上的定位点，依次连接定位点作为修正后的车辆分段行驶轨迹之后，相邻两个定位点之间的最终车辆位置采用以下方式确定：

对于锚点AP1和锚点AP2之间的某一时刻f，通过计算该时刻f的车辆位置到锚点AP1的距离以及锚点AP1到锚点AP2之间的距离的比值来计算获得时刻f定位修正后的位置；

式中，和分别表示两个锚点的位置，p_AP1和p_AP2分别表示两个锚点和对应的定位点，为时刻f的车辆位置，p_f为时刻f定位修正后的车辆位置。