CN112747744A

CN112747744A - 一种结合航位推算和多车道路网地图的车辆定位方法

Info

Publication number: CN112747744A
Application number: CN202011531431.2A
Authority: CN
Inventors: 项志宇; 王天乐; 钱琦; 张至可
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2020-12-22
Filing date: 2020-12-22
Publication date: 2021-05-04
Anticipated expiration: 2040-12-22
Also published as: CN112747744B

Abstract

本发明公开了一种基于航位推算和多车道路网地图的车辆定位方法。建立由路网节点、边和包含各车道中心位置的多车道路网地图，给定起点位置范围以及起始航向角范围；获得当前车辆相对于车辆行驶起点的位置和车辆行驶轨迹，并进行变道检测和道路锚点检测获得锚点，根据道路锚点的实时检测结果确定多车道路网地图的起点位置与起始航向角；对于后续的车辆行驶轨迹进行实时锚点检测，利用多位置联合滤波对锚点进行修正，并且将车辆行驶轨迹的其余位置投影到多车道路网地图上完成轨迹修正。本发明能有效地实现起点位姿确定，修正长距离航位推算方法产生的累积定位误差，在多车道宽道路环境下仍然有着良好的定位精度，计算代价小，实时性强。

Description

一种结合航位推算和多车道路网地图的车辆定位方法

技术领域

本发明涉及智能车辆的导航定位技术方法，特别是涉及了一种结合航位推算和多车道路网地图的车辆定位方法。

背景技术

无人驾驶是现在发展在科学技术前沿最活跃的领域之一，定位算法又是其中十分重要的一个模块，目前已经成为一个学术意义和应用价值兼得的研究热点。传统的车辆定位方法大多是利用GPS来实现的车辆定位，GPS定位的原理是利用卫星信号来获取自身位置，因此定位精度与接收到的有效的卫星信号有关，在一些特殊地形，例如“城市峡谷”、隧道等环境下无法取得较好的定位精度。轮式里程计可以提供车辆相对于起点位置的里程信息，进一步结合惯性测量单元(IMU)则可以获得当前位置的大致估计，但该方式定位精度不高，而且随着运动距离的增加定位会出现较大的偏移。视觉里程计是一种依靠一个或多个相机的图像来得到车辆位姿变化的方法，因为它的成本比激光雷达低得多，所以被较为广泛地应用。视觉里程计方法也存在累积误差的问题，一般采用BA与闭环优化来消除累计误差，但在一般的实际车辆行驶中极少出现闭环，因此实际效果较差。

近年来随着各类地图服务的不断普及，城市路网地图变得十分容易获得，可以利用路网地图作为定位的先验信息来约束车辆轨迹，帮助减小累计误差。目前采用路网地图辅助定位的方法在城市场景下会因为道路较宽导致定位精度的下降。本发明利用车道级的路网地图，利用联合粒子滤波来修正视觉里程计的累计误差，实现了车道级的精确车辆定位。

发明内容

为了解决背景技术中的技术问题，本发明的目的在于提供一种基于车辆轨迹和多车道路网地图的多位置联合滤波定位方法，用于解决GPS遮挡等复杂城市环境下的可靠车辆定位问题。

本发明利用惯导系统或者视觉里程计获得车辆的实时位置，形成车辆行驶轨迹，对运动轨迹按照锚点进行分段。利用多位置联合滤波将分段后的轨迹与包含车道信息的城市路网地图进行滤波，确定起点并修正每个锚点的位置，从而减小甚至消除航位推算方法产生的累积误差，使定位更加精确。

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

(1)建立由路网节点、边和包含各车道中心位置的多车道路网地图，给定车辆行驶起点在多车道路网地图中的起点位置范围以及起始航向角范围；

所述的起点位置范围是以车辆行驶起点为圆心的固定半径的一个圆形区域。所述的道路包含了至少一条车道。

(2)通过航位推算方法实时获得当前车辆相对于车辆行驶起点的位置和车辆行驶轨迹，并进行变道检测获得变道锚点，同时进行道路锚点检测获得道路锚点，道路锚点和变道锚点共同构成了锚点，将各锚点相连形成初步分段轨迹；根据道路锚点的实时检测结果确定多车道路网地图的起点位置与起始航向角；

初步分段轨迹是各个锚点依次相连得到的线段，这些线段用于多位置联合滤波，从而将轨迹上的锚点修正到地图上。相当于多位置联合滤波的一个输入(实际上这个输入也可以直接是锚点)。

(3)在确定起点位置与起始航向角后，对于后续的车辆行驶轨迹进行实时锚点检测，利用多位置联合滤波对锚点进行修正，并且将车辆行驶轨迹的其余位置投影到多车道路网地图上完成轨迹修正。

所述的车辆行驶轨迹的其余位置是指轨迹上除了起点和锚点位置以外的其他位置。

所述步骤(1)中，用包含路网节点、边以及其对应车道信息的拓扑图结构来构建多车道路网地图：以路网地图中的路口中心点、弯曲道路上的间隔分段点为路网节点，以路网节点间连接的道路中心线作为边；路网节点n＝(lat,lon)的位置由经纬度表示，lat、lon分别表示纬度、经度，边e＝(n₁,n₂,lane)表示路网节点n₁和路网节点n₂之间的连接关系，每个边包含的道路信息由lane＝(head,num,length)表示，head、num和length分别表示两个路网节点之间的道路中车道的朝向、车道序号和车道的横向长度(即车道宽度)。

所述步骤(2)中，通过航位推算方法实时得到当前车辆初始位置，利用变道前后的车辆的航向角变化、横向距离变化与时间变化来进行变道检测，并在变道处设置一个变道锚点。

具体是按照以下方式获得航向角变化、横向距离变化与时间变化：

变道前后车辆的航向角变化为△θ＝|θ2-θ1|，其中变道前车辆的航向角为θ1，变道后车辆的航向角为θ2；

变道前后的时间变化为△t＝t2-t1，其中变道前的时刻为t1，变道后时刻为t2；

变道前后的车辆横向距离变化为

其中(x1,y1)表示变道前的车辆位置坐标，(x2,y2)表示变道后的车辆位置坐标；

变道检测考虑三个阈值，γ表示航向角变化的阈值，τ表示变道时间阈值，d_threshold表示变道横向距离阈值。当△θ<γ、△t<τ、d>d_threshold情况下，γ表示航向角变化的阈值，τ表示变道时间阈值，d_threshold表示变道横向距离阈值，则认为车辆发生了变道，并根据航向角变化、车辆横向距离变化d与车道的横向长度确定变道后的车辆所在车道，并在变道前后的车辆位置之间的中心点建立为一个变道锚点。

另外可以通过航位推算方法实时得到当前车辆位置，将当前车辆位置与上一个锚点的位置用直线相连，计算从上一个锚点(若无上一个锚点则选择起点位置)到当前位置的曲线轨迹中每个点到该直线的距离，若某位置到该直线距离大于预设的阈值，则将该位置设定为备选锚点，否则认为当前车辆位置到上一个锚点之间没有锚点；对一定范围内紧邻的多个备选锚点，计算它们之间的航向角变化，选取航向角变化最大的备选锚点作为真实锚点。

当车辆位置距离上一个锚点的距离超过预设的阈值时，将当前车辆位置设定为一个新的锚点。

在步骤(2)进行道路锚点检测时：

当由拐弯产生的第一个道路锚点被检测到时，根据车辆行驶起点的起点位置范围与起始航向角范围推算第一个道路锚点的位置范围，在第一个锚点的位置范围内的各个路网节点上均使用粒子堆来代表锚点候选位置，这些粒子都将保留并与后续锚点进行联合滤波；

当检测得到由拐弯产生的后续道路锚点后，通过粒子滤波确定第一个道路锚点对应的路网节点，然后在起点位置范围内的各车道中心线上沿自身均匀生成粒子，再与第一个道路锚点和第二个道路锚点进行联合粒子滤波，选取权重最大的粒子作为多车道路网地图中的起点位置，并根据道路方向确定多车道路网地图中的起始航向角。

通过多次多位置联合滤波确定第一个锚点所对应的路网节点时，具体方法为每当在轨迹上检测到一个新的锚点时，在地图根据与上个锚点的邻接关系找到和当前锚点之间距离小于误差阈值范围内的所有路网节点作为与当前锚点可能对应的路网节点，然后按照以下三种不同情况区分处理：

SA、有且仅有一个与当前锚点对应的路网节点时，在该路网节点附近生成一堆粒子，并直接滤波计算各个粒子的权重；

SB、有两个以上与当前锚点对应的路网节点时，在每个路网节点附近生成一堆粒子并且这些粒子均将被保留；当下一个锚点被检测到后，这些产生的粒子会与下一个锚点生成的粒子进行联合滤波，得到各个粒子权重；

SC、没有找到与当前锚点对应的路网节点时，在当前锚点与上一个锚点之间可能的地图边上生成粒子，粒子均将被保留；当下一个锚点被检测到后，这些产生的粒子会与下一次锚点生成的粒子进行联合滤波，得到各个粒子权重。

在步骤(2)中的联合粒子滤波过程中，粒子按照以下初始化：

(1)在当前道路锚点有对应的一个或者多个路网节点时，

若路网节点为弯曲道路上的间隔分段点，则将路网节点垂直投影到所在弯曲道路的各个弯曲车道中心上的点分别作为混合高斯分布的中心，以车道的横向长度横向长度作为方差生成粒子，按照高斯分布生成粒子；

若路网节点为路口中心点，则将路网节点所在路口的各个车道中心线间的交叉点作为混合高斯分布的各个中心，以车道的横向长度横向长度作为方差生成粒子，按照高斯分布生成粒子；

(3)在当前道路锚点没有对应的路网节点时，将当前道路锚点与上一个道路锚点分别投影到各条车道中心线上，从而在各条车道中心线上均获得两个投影点，每条车道中心线上的两个投影点间按照混合高斯分布生成粒子。

具体实施中，路网节点和当前锚点是否对应是根据路网节点和当前锚点之间的距离和航向角是否在阈值范围内进行判断。具体在判断轨迹上锚点与路网那个节点是否匹配的时候可以用两个依据，一是根据路网地图的连接关系，二是两个锚点间的距离、航向与两个路网节点间的距离、航向的相似程度。

所述步骤(3)中，将车辆行驶轨迹的其余位置投影到多车道路网地图上完成轨迹修正，具体为：

根据步骤(2)获得的各个锚点与多车道路网地图，将车辆行驶轨迹的其余位置投影到多车道路网地图上再依次相连作为修正后的车辆行驶轨迹，根据锚点和车辆行驶轨迹中车辆航向角的变化，确定车辆拐弯前后点的时刻，将车辆行驶轨迹划分为拐弯段与非拐弯段，划分同时获得拐弯前点和拐弯后点的位置，对于非拐弯段和非拐弯段的车辆行驶轨迹采用不同的投影方式进行处理：

(A)对于非拐弯段的轨迹：

车辆均在道路中的某条车道上行驶，只需要知道车辆所在车道以及车辆在道路上的纵向位置就可以完成车辆位置到地图的投影。具体利用原始的车辆行驶轨迹以及锚点分别修正确定车辆沿道路的纵向位置与横向位置：

(A.1)沿道路纵向位置

对于锚点AP1和锚点AP2之间的某一时刻t，通过计算该时刻t的车辆位置到锚点AP1的纵向距离以及锚点AP1到锚点AP2之间的纵向距离的比值来计算获得时刻t定位修正后的纵向位置p_tx，然后将纵向位置p_tx垂直投影到道路上获得车辆沿道路的纵向位置：

其中

分别代表原始的车辆行驶轨迹中的锚点AP1位置、锚点AP2位置与时刻t车辆在锚点AP1和锚点AP2连线方向上的位置；

(A.2)沿道路横向位置

若锚点为由拐弯产生的锚点，则根据在锚点附近的原始的车辆行驶轨迹与道路边沿确定在拐弯前车辆所在的车道，并直接以该车道的车道中心线作为横向位置；

若锚点为由变道产生的锚点，则根据每个时刻的车辆位置到变道前的位置之间的距离与每个时刻的车辆位置到变道后的位置之间的距离的比值，根据比值将车辆位置垂直投影到垂直于车道方向的直线上，作为横向位置；

(B)对于拐弯段的轨迹

以修正后的锚点、拐弯前点和拐弯后点位置为基准，在保持原始的车辆行驶轨迹的局部形状情况下，将原始的车辆行驶轨迹投影到多车道路网地图上：

先利用拐弯前点和锚点对处于拐弯前点和锚点之间的车辆行驶轨迹进行修正：

首先，根据原始的车辆行驶轨迹确定拐弯前点所在车道，并采用和(A.1)、(A.2)相同方式处理获得拐弯前点的沿所在车道的纵向位置和沿所在车道的横向位置，进而将拐弯前点修正到该车道中心线上，进行了拐弯前点的修正；

然后，根据修正前的拐弯前点相对于修正前的锚点的位置相比于修正后的拐弯前点相对于修正前的锚点的位置的变化，锚点的修正通过多位置联合滤波完成对车辆行驶轨迹进行修正，具体是计算修正前的拐弯前点绕修正前的锚点旋转、距离缩放后变换到修正后的拐弯前点相对于修正后的锚点的相对位置的转化矩阵，将修正前的处于拐弯前点和锚点之间的车辆行驶轨迹利用转化矩阵进行变换得到修正后的处于拐弯前点和锚点之间的车辆行驶轨迹；

最后利用拐弯后点和锚点对处于锚点和拐弯后点之间的车辆行驶轨迹进行修正，处理方式和处于拐弯前点和锚点之间的车辆行驶轨迹的处理方式相同，即锚点到拐弯后点的轨迹按照相同的方法处理。

本发明首先从OpenStreetMap获取城市道路信息，建立车辆行驶区域的多车道路网地图。通过惯导或者视觉里程计等航位推算方法得到原始车辆行驶轨迹。为了有效地适用于弯曲道路与城市多车道的环境，本发明使用了基于锚点(AP)的轨迹表示方法，锚点代表了轨迹上主要的弯曲点与变道点。将车辆轨迹按照锚点进行分段后，根据由拐弯产生的第一个锚点所在位置范围确定匹配候选路网节点，通过多位置联合滤波确定第一个、第二个锚点所在位置，再对起点位置的可能范围与第一、二个锚点进行联合滤波确定起点位置与航向角。根据确定的车辆行驶起点与起始航向角利用多位置联合滤波方法，结合多车道路网地图对后续的轨迹进行修正，从而消除航位推算方法的累计误差，完成车道级的定位精度。

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

(1)本发明能不依靠GPS，单纯依赖视觉或者惯导等航位推算方法，结合多车道路网地图实现可靠的城市车道级定位。

(2)本发明能够有效地解决只有道路中心线的简单路网地图在多车道的道路中定位精度不足的问题，实现车道级的定位。

(3)本发明能够有效地应对起点位置误差较大、直线道路、弯曲道路等各种复杂情况。

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

附图说明

图1是多车道路网地图的示意图；

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

图3是起点位置与航向角确定的实施步骤示意图；

图4是基于多车道路网地图的多位置联合滤波实时步骤示意图；

图5是APOLLOSCAPE ROAD12序列视觉里程计结果图；

图6是APOLLOSCAPE ROAD12序列本方法与真值对比结果图。

具体实施方式

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

下面用一个例子来更清楚地表述本发明如何根据轨迹与多车道路网地图确定起点位置与航向角并实现后续实时定位。

本发明完整方法实施的例子如下：

(1)建立由路网节点、边以及其对应车道数的多车道路网地图，如图1所示，其中黑色表示道路边界，虚线用于区分车道，灰线表示车道中心线，灰色圆圈表示车道中心线交点。给定车辆行驶起点在多车道路网地图中的起点位置范围以及起始航向角范围；

(2)通过航位推算方法实时获得相对于车辆行驶起点的车辆位置，以及车辆行驶轨迹，并进行变道检测获得变道锚点，同时进行道路锚点检测获得道路锚点，车道锚点和变道锚点共同构成了锚点，得到如图2所示的车辆行驶轨迹；

(2.1)通过航位推算方法实时得到当前车辆位置，利用变道前后的车辆的航向角变化、横向位移距离与变道时间三个方面来进行变道检测，并在变道处设置一个锚点；如图3(a)所示，在车辆行驶到1位置时，检测到车辆发生了变道，因此在该处设置了一个变道锚点(AP1)；

(2.2)通过航位推算方法实时得到当前车辆位置，将当前车辆位置与上一个锚点的位置用直线相连，计算从上一个锚点(若无上一个锚点则选择起点位置)到当前位置的轨迹中每个位置到直线的距离，若某位置到直线距离大于预设的阈值，则将该位置设定为备选锚点，反之则认为当前车辆位置到上一个锚点之间没有检测到备选锚点；对相邻的多个备选锚点计算其航向角变化，选取航向角变化最大的备选锚点作为道路锚点；

(2.3)当车辆位置距离上一个锚点的距离超过预设的阈值时，将当前车辆位置设定为一个新的道路锚点。

(3)当第一个由非(2.3)产生的道路锚点被检测到时，根据起点位置范围与起始航向角范围推算该锚点的位置范围，在地图中该范围内的所有路网节点上使用粒子堆来代表锚点候选位置，并将这些路网那个节点称为将候选路网节点，这些粒子都将保留与后续锚点上生成的粒子进行联合滤波；如图3(a)所示，在车辆行驶到2位置时，检测到车辆发生了拐弯，因此在该位置处设置一个锚点(AP2)；根据图3(b)中灰色圆形范围所示的车辆起点位置可能范围，以及到AP2为止的车辆行驶轨迹，可以得到图3(c)中灰色圆形范围所示的AP2位置范围；搜索AP2位置范围内的所有路网节点，这些路网节点都是AP2的可能位置，它们的位置由图3(c)中灰色圆点表示，并在各个路网节点周围生成候选粒子。

(4)当后续探测到锚点时，根据与锚点匹配的路网节点数采用多位置联合滤波，实时更新粒子权重，选取权重最大的粒子作为锚点位置，随着车辆不断行驶，通过多位置联合滤波确定第一个锚点的所对应的路网节点；图3(d)(e)(f)(g)表示以不同路网节点作为AP2的位置，推算得到的AP3位置范围，其中灰色点代表了锚点，灰色扇形区域代表从该候选路网节点开始到AP3时，车辆可能的行驶轨迹。对于每个候选路网节点对应的AP3，在可能的位置周围搜索相邻的路网节点，通过锚点可能位置与路网节点的连接关系判断该候选锚点是否应该被移除。其中，图3(d)(e)所示候选路网节点对应的AP3没有对应的路网节点，因此它们将从候选路网节点中被移除，它们所对应的候选粒子也将被移除。图3(f)(g)所示候选路网节点对应的AP3有着对应的路网节点，它们将被保留，图3(h)(i)表示以剩下的候选路网节点作为AP2的位置，推算得到的AP4位置范围，通过上述方法可以将图(i)对应的候选路网节点及其候选粒子移除，此时候选路网节点中仅剩下图3(j)中AP2对应的路网节点，该路网节点被认为是第一个拐弯AP2所对应的路网节点。

(5)确定第一个锚点的具体位置后，在可能的起点位置范围(图3(j)所示的灰色圆圈)内沿道路边均匀生成粒子(图3(j)灰色粗线不表示粒子所处位置)，并图3(k)所示，将粒子与第一、二个锚点的粒子进行滤波更新粒子权重，选取权重最大的粒子作为起点的位置(图(l)灰色点0)，并根据道路方向确定起点的航向角；

(6)根据已经推算得到的车辆行驶起点与起始航向角对到当前位置为止的车辆轨迹进行多位置联合滤波，在利用修正后的锚点与多车道路网地图将车辆行驶轨迹的其余位置投影到车道路路网地图上作为修正后的车辆行驶轨迹。这里以一个简单的例子来说明如何使用多车道路网地图来进行车辆定位：

步骤一：图4(a)为航位推算方法得到车辆原始轨迹，图4(b)是车辆行驶区域多车道路网地图。当检测到轨迹上的第一个锚点AP1时，发现AP1是由于车辆变道产生的，因此在如图4(d)所示，在变道处根据车辆行驶方向沿着各车道中心线生成粒子。

步骤二：当检测到轨迹上的第二个锚点AP2时，通过路网节点的连接关系以及轨迹与路网地图相似度判断AP2可能与图4(c)中的两个路网节点匹配，因此在如图4(e)的多车道路网地图中根据转弯方向以相对应的车道与车道中心线交叉点为中心分别以车道的横向长度为方差初始化粒子，这些粒子将被保留，等待下次确定锚点时进行联合滤波，其中灰色直线代表了车道中心线，箭头表示了左右车道的行驶方向。

步骤三：当轨迹上检测到了第三个锚点AP3时，通过路网节点的连接关系，发现AP3有且只有一个匹配的路网节点，并根据该路网节点与AP2对应路网节点的连接关系以及轨迹与路网地图相似度确定AP2对应的路网节点，如图4(f)所示。此时在如图4(g)的多车道路网地图中根据转弯方向以相对应的车道与车道中心线交叉点为中心分别以车道的横向长度为方差初始化粒子。这些粒子与之前AP1与AP2生成的粒子以及起点位置进行多位置联合滤波，更新粒子权重，并将权重最大的粒子作为锚点所在位置。

步骤四：按照上述步骤对轨迹进行锚点检测并逐一的分段与修正，如图4(h)所示，当修正完轨迹上的AP5后，检测到第六个锚点AP6，此时发现AP6没有对应的路网节点，在如图4(i)所示，在AP5到AP6的多车道路网地图上沿着各个车道中心线均匀生成粒子，其中灰色粗线代表沿车道中心线生成的粒子堆，这些粒子都将被保留，等待下次确定锚点时进行联合滤波。

步骤五：如图4(j)所示，当检测到第七个锚点AP7时，发现AP7有且只有一个匹配的路网节点，在AP7对应的路网节点上生成粒子，并与AP6生成的粒子进行联合滤波，更新粒子权重，并将权重最大的粒子作为锚点所在位置。

步骤六：在对整条轨迹按照上述方法进行分段与滤波后，得到了每个锚点修正后的位置，根据锚点位置将其余非拐弯轨迹点位置投影到对应的车道中心线上。在AP3与AP4之间，由于没有检测到变道，根据锚点以及锚点附近的原始轨迹判断锚点前车辆所在车道，并将轨迹投影到车道中心线上，如图4(k)所示，黑色直线为AP3与AP4之间的多车道路网上修正后的车辆轨迹；在AP0与AP2之间，由于还存在一个变道产生的锚点AP1，除了将变道前后的轨迹分别投影到车道中心线上，还需要用直线将变道前后的轨迹相连，如图4(k)所示，黑色线段为AP0与AP2之间的多车道路网上修正后的车辆轨迹。在拐弯位置，根据修正后锚点与拐弯前后点位置，保留原始轨迹形状信息将各个位置投影到地图上，得到了如图4(l)放大展示区域的修正后轨迹。

步骤六：在完成原始轨迹到多车道路网地图的投影后，最终得到了如图4(l)所示的修正后轨迹。

测试KITTI数据集来评估本方法的效果，以双目图像序列以及对应的多车道路网地图作为输入，根据KITTI提供的真值，比较起点位置与航向角的差别。

实验主要测试了KITTI图像序列00、02与09的起点位置误差，其中起点位置的可能范围设定为以真实起点位置为圆心的，半径为20m的圆内，初点位置的粒子服从均匀分布，粒子间隔为0.2m。经过本方法修正后，序列00、02与09的起点位置误差结果详见下表1。

表1修正后起点位置的定位误差

序列	00	02	09
				起点位置误差	1.75m	2.56m	3.12m

KITTI数据集在的道路环境较为简单，并且大部分路段仅仅为简单的单车道道路。为了验证在多车道环境下本发明的定位精度，本发明选择在ApolloScape数据集上进行测试。本发明主要测试了ApolloScape中Road12序列，根据ApolloScape给的真值，比较了视觉里程计、简单路网的多位置联合滤波、本方法的平均误差与最大误差，详见下表2。

表2本方法与其他方法定位误差比较

定位方法	平均误差	最大误差	纵向误差	横向误差
					视觉里程计	109.79m	160.49m	x	x
多位置联合滤波	17.68m	29.15m	12.64m	9.81m
					本方法	6.21m	16.32m	5.58m	2.05m

序列Road12视觉里程计的结果如图5所示，本方法的结果与真值的对比如图6所示。可以看出视觉里程计在拐弯处累积了较大的角度误差，导致定位的累积误差大大增加；多位置联合滤波方法得到的结果虽然消除了视觉里程计的累积误差，但是由于简单路网地图精度的限制，在修正的效果上不如本方法。对比原始的视觉里程计定位，采用本方法后平均定位误差从109.79米降到了6.21米，最大误差从160.49米降到了16.32米，在横向误差上平均误差为2.05米，达到了车道级定位的要求。

实验采用一台配备主频为3.6GHz的intel i7处理器和16G内存的电脑，实验中每次迭代平均耗时193.75ms。本方法将起点位置确定分成了两步，首先利用多位置联合滤波确定第一个锚点匹配的路网节点，再在可能的起点位置周围生成粒子后与第一、二个锚点联合滤波确定起点位置。相比于其他基于滤波的方法，粒子间的组合变少，大大减小了计算代价。在确定了起点位置后利用多位置联合滤波将车辆轨迹与多车道路网地图进行联合滤波得到车辆在各个位置的位置，滤波过程只在轨迹锚点处发生，无需每帧进行，所以相对于原始视觉里程计，增加的计算代价很小。

综上所述，采用本发明的方法能够在多车道的城市环境下有效减小视觉里程计产生的累积误差，得到车道级的定位精度。

本发明能有效地实现起点位姿确定，修正长距离航位推算方法产生的累积定位误差，在多车道宽道路环境下仍然有着良好的定位精度，计算代价小，实时性强。

Claims

1.一种基于航位推算和多车道路网地图的车辆定位方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的一种基于航位推算和多车道路网地图的车辆定位方法，其特征在于：所述步骤(1)中，用包含路网节点、边以及其对应车道信息的拓扑图结构来构建多车道路网地图：以路网地图中的路口中心点、弯曲道路上的间隔分段点为路网节点，以路网节点间连接的道路中心线作为边；路网节点n＝(lat,lon)的位置由经纬度表示，lat、lon分别表示纬度、经度，边e＝(n₁,n₂,lane)表示路网节点n₁和路网节点n₂之间的连接关系，每个边包含的道路信息由lane＝(head,num,length)表示，head、num和length分别表示两个路网节点之间的道路中车道的朝向、车道序号和车道的横向长度。

3.根据权利要求1所述的一种基于航位推算和多车道路网地图的车辆定位方法，其特征在于：所述步骤(2)中，通过航位推算方法实时得到当前车辆初始位置，利用变道前后的车辆的航向角变化、横向距离变化与时间变化来进行变道检测，并在变道处设置一个变道锚点。

4.根据权利要求1所述的一种基于航位推算和多车道路网地图的车辆定位方法，其特征在于：在步骤(2)进行道路锚点检测时：

当由拐弯产生的第一个道路锚点被检测到时，根据车辆行驶起点的起点位置范围与起始航向角范围推算第一个道路锚点的位置范围，在第一个锚点的位置范围内的各个路网节点上均使用粒子堆；

5.根据权利要求4所述的一种基于航位推算和多车道路网地图的车辆定位方法，其特征在于：在步骤(2)中的联合粒子滤波过程中，粒子按照以下初始化：

(1)在当前道路锚点有对应的一个或者多个路网节点时，

若路网节点为弯曲道路上的间隔分段点，则将路网节点垂直投影到所在弯曲道路的各个弯曲车道中心上的点分别作为混合高斯分布的中心，以车道的横向长度作为方差生成粒子；

若路网节点为路口中心点，则将路网节点所在路口的各个车道中心线间的交叉点作为混合高斯分布的各个中心，以车道的横向长度作为方差生成粒子；

(2)在当前道路锚点没有对应的路网节点时，将当前道路锚点与上一个道路锚点分别投影到各条车道中心线上，从而在各条车道中心线上均获得两个投影点，每条车道中心线上的两个投影点间按照混合高斯分布生成粒子。

6.根据权利要求1所述的一种基于航位推算和多车道路网地图的车辆定位方法，其特征在于：所述步骤(3)中，将车辆行驶轨迹的其余位置投影到多车道路网地图上完成轨迹修正，具体为：

(A)对于非拐弯段的轨迹：

利用原始的车辆行驶轨迹以及锚点分别修正确定车辆沿道路的纵向位置与横向位置：

(A.1)沿道路纵向位置

其中

(A.2)沿道路横向位置

(B)对于拐弯段的轨迹：

先利用拐弯前点和锚点对处于拐弯前点和锚点之间的车辆行驶轨迹进行修正：根据原始的车辆行驶轨迹确定拐弯前点所在车道，并采用和(A.1)、(A.2)相同方式处理获得拐弯前点的沿所在车道的纵向位置和沿所在车道的横向位置，进而将拐弯前点修正到该车道中心线上，进行了拐弯前点的修正；

然后，根据修正前的拐弯前点相对于修正前的锚点的位置相比于修正后的拐弯前点相对于修正前的锚点的位置的变化对车辆行驶轨迹进行修正，具体是计算修正前的拐弯前点绕修正前的锚点旋转、距离缩放后变换到修正后的拐弯前点相对于修正后的锚点的相对位置的转化矩阵，将修正前的处于拐弯前点和锚点之间的车辆行驶轨迹利用转化矩阵进行变换得到修正后的处于拐弯前点和锚点之间的车辆行驶轨迹；

最后利用拐弯后点和锚点对处于锚点和拐弯后点之间的车辆行驶轨迹进行修正，处理方式和处于拐弯前点和锚点之间的车辆行驶轨迹的处理方式相同。