CN106017486B - 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法 - Google Patents

Abstract

本发明公开了一种面向无人车导航的基于轨迹拐点滤波的地图定位方法。在由节点和边构成的路网地图中，从已知起点出发，通过航位推算方法实时获得车辆初步位置，将每个车辆初步位置与起点连接形成车辆行驶初步轨迹，实时检测拐点获得拐点并标记，在各拐点将车辆行驶初步轨迹分段，分为各段车辆轨迹分段，获得每个拐点后对拐点进行实时更新，将各个实时更新后的拐点和起点按行驶时序依次连接作为最终车辆行驶轨迹。本发明能有效修正航位推算方法产生的累积定位误差，具备良好的定位精度，计算代价很小，实时性强。

Description

一种面向无人车导航的基于轨迹拐点滤波的地图定位方法

技术领域

本发明涉及机器人的导航定位方法，特别是涉及一种面向无人车导航的基于轨迹拐点滤波的地图定位方法。

背景技术

传统的定位方法包括使用GPS、惯性导航系统以及里程计等。GPS的优点是其良好的通用性。但在高楼林立的城市环境下或者桥梁、隧道中，卫星信号容易受到遮挡而无法获得良好的定位精度。惯导系统通过对系统加速度和角速度的积分来获得机器人所在方位，受到累积误差的影响较大，一般都结合GPS一起使用，很少单独应用。里程计方法分为传统轮式里程计和目前受到广泛关注的视觉里程计。轮式里程计一般只能提供相对起点的距离信息，需要结合移动方向信息才能实现定位。视觉里程计将单目或者双目图像序列作为输入，通过测量帧间对应像素的运动，来计算车辆相对与起点的方位，并获得运动轨迹，是一种成本较低的但定位精度较好的方法。但视觉里程计同样存在累积误差的问题，并不适合长距离定位。

为了解决累积误差的问题，一类方法是采用同时定位与地图构建(SLAM)的方法，将运动轨迹形成闭环，通过约束轨迹闭合误差来减少定位累计误差。该方法对运动轨迹有闭环要求，限制了使用环境，计算复杂度也较高。另一类方法是将视觉里程计或者惯导(以下统称航位推算方法)与GPS相结合，通过GPS对累计误差的定期修正，提高定位可靠度。

近年来随着互联网各类地图服务的不断普及，各类城市路网地图变得十分容易获得，现有技术中缺少了一种能累计误差用于实现无人车导航的精准实时定位方法。

发明内容

本发明的目的在于提供了一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，利用易得的道路地图，通过拐点滤波方法来实现对里程计或者惯导累积误差定位的有效修正，从而实现车辆的可靠定位。具体是通过先验地图信息，将其与航位推算方法相结合，来有效应对定位的累计误差。

本发明首先采用图模型建立导航区域的路网地图，通过视觉里程计或者惯导等航位推算方法得到车辆的初始运动轨迹，将该轨迹按照拐点进行分段后，通过对每个拐点实施粒子滤波方法来修正每个拐点在路网地图上的位置，从而消除航位推算方法产生的累积误差，提高定位的精度。

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

1)在主要由节点和边构成的路网地图中，车辆从已知位置的起点出发，在车辆行驶过程中，通过航位推算方法实时获得车辆初步位置，将每个时刻的车辆初步位置与起点依次连接形成车辆行驶初步轨迹，对车辆行驶初步轨迹实时检测拐点获得拐点并标记，并将起点作为拐点，在各拐点将车辆行驶初步轨迹用直线段进行分段，分为各段车辆轨迹分段；

2)获得每个拐点后对拐点进行实时更新，将各个实时更新后的拐点和起点按行驶时序依次连接作为最终车辆行驶轨迹。

所述路网地图用节点和边来构建：边作为道路，以节点作为道路与道路之间的交叉路口，节点的位置由经纬度表示，表示为n＝(lat,lon)，两个节点一段道路表示为e＝(n₁,n₂)，n₁,n₂分别表示为两个相邻的节点。

所述对车辆行驶初步轨迹实时检测拐点获得拐点具体是：

1.1)在车辆行驶过程中，将通过航位推算方法实时获得当前时刻下的车辆初步位置与行驶经过最近的一个拐点用直线进行连接，计算在当前时刻下的车辆初步位置与行驶过最近的一个拐点之间的每一个中间时刻下的车辆初步位置到该直线的垂直距离；

1.2)将垂直距离与拐点阈值进行比较：

若所有中间时刻下的车辆初步位置到该直线的垂直距离小于等于拐点阈值，则当前时刻下的车辆初步位置与行驶经过最近的一个拐点之间的车辆行驶初步轨迹不存在拐点；

若所有中间时刻下的车辆初步位置到该直线的垂直距离大于拐点阈值，则车辆初步位置到该直线的垂直距离最大所在的中间时刻的车辆初步位置作为新的拐点，通过新的拐点将当前时刻下的车辆初步位置与行驶经过最近的一个拐点之间的车辆行驶初步轨迹分为两段车辆轨迹分段，

1.3)重复迭代上述两个步骤，直到行驶经过最近的一段车辆轨迹分段中所有中间时刻下的车辆初步位置到该直线的垂直距离小于等于拐点阈值。

所述的对拐点进行实时更新具体是通过构建粒子并通过粒子滤波方法计算获得每一个粒子的权重，通过比较获得权重最大的粒子，并用该粒子对其所在拐点进行替换，从而实现对拐点的实时更新。

所述步骤2)实时更新中每次计算的具体过程如下：

2.1)对于每个当前拐点，在与上一个拐点最近的节点的各个相关节点周围附近随机生成一堆新粒子，每个新粒子归属于其所生成对应的节点，相关节点指的是该节点所在边上的节点，节点的所有相关节点包括了该节点所在边上的所有节点。用一堆粒子来表示其在路网地图上的位置，粒子不一定要分布在该节点上以此来弥补路网的不精确性；

当需要初始化第i+1组粒子的时候，首先找到第i组粒子所有属于的节点，利用路网地图的拓扑关系，找到所有其相邻的节点作为第i+1组粒子可能的初始位置，并将每个粒子的归属属性设为与该粒子位置最接近的节点。

2.2)每个当前拐点下生成的所有新粒子形成了该当前拐点下的一组粒子，对于生成的每一组粒子采用以下方式处理：

将每一个新生成的粒子分别与上一个拐点周围附近的粒子中在同一条边上对应的各个粒子进行两两组合用直线段进行连接，每两个粒子之间的直线段作为备选线段，计算每一备选线段的长度和角度，然后将每一条备选线段的长度和朝向角度与对应的车辆轨迹分段的长度和角度进行比较，获得每一条备选线段的长度误差和角度误差：

e_a＝|h_c-h_s|

式中，e_l为长度误差，l_c为备选线段的长度，l_s为对应的车辆轨迹分段的长度，e_a为角度误差，h_c为备选线段的角度，h_s为对应的车辆轨迹分段的角度；

对于起点后的第一个拐点，将该拐点生成的每一个粒子分别与起点进行组合用直线段进行连接，获得各条备选线段。

判断备选线段的长度误差和角度误差是否在阈值范围内，若在阈值范围内则保留该备选线段作为车辆可能的轨迹，否则舍弃该备选线段；

2.3)再通过以下公式计算每一条备选线段与其对应的车辆轨迹分段之间的相似度s_s：

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

其中，s_l为备选线段长度的相似度，s_a为备选线段角度的相似度，s_s为备选线段与车辆轨迹分段之间的相似度，α表示权重参数；

2.4)将步骤2.2)处理后保留的备选线段与上一次拐点迭代计算后剩余的备选线段通过树结构相连形成了所有可能的各条备选路径；对于当前拐点的每一个粒子，然后采用以下公式计算各条包含有该粒子的备选路径与车辆行驶初步轨迹之间的相似度s_r：

式中，s_s[i]表示一条备选路径中第i段备选线段与其对应的车辆轨迹分段之间的相似度，i表示备选路径的序号，M表示车辆行驶初步轨迹中车辆轨迹分段的总数，i表示车辆轨迹分段的段序数；

2.5)通过以下公式计算所有包含该粒子的备选路径与车辆行驶初步轨迹相似度的平均值，作为粒子的权重：

式中，w_i,j代表第i组第j个粒子的权重，s_r ^k表示第k条备选路径与车辆行驶初步轨迹之间的相似度，n代表该粒子下所有备选路径的总数，k表示该粒子下备选路径的序数；

2.6)对当前拐点下该组所有粒子是否归属于同一个节点进行判断：

若当前拐点下该组所有粒子是均归属于同一个节点，则认为这组粒子对应的拐点在路网地图上的位置已确定，这组粒子的迭代计算过程停止，取其中权重最大的粒子替换当前拐点作为当前在路网地图上的位置，将当前拐点周围附近的其余所有粒子舍弃；

若当前拐点下该组所有粒子不是均归属于同一个节点，则该组所有粒子全部保留并作为下一次计算的剩余粒子，返回到步骤2.1)继续进行迭代滤波，对所有粒子进行按照其权重重采样，进行实时更新。

所述步骤2.3)中的备选线段长度的相似度s_l和备选线段角度的相似度s_a采用以下公式计算：

式中，e表示自然常数，e_l为长度误差，e_a为角度误差。

所述步骤2.1)中，若上一个拐点计算后存在剩余粒子，所述当前拐点生成粒子时还要在上一个拐点的剩余粒子周围附近随机再生成一堆粒子。

如果通过航位推算得到车辆的行驶轨迹的终点离地图上的节点太远，则不通过滤波得到终点的位置，而是通过上一个拐点更新后的位置和航位推算直接得到终点的位置。

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

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

1)本发明能在无GPS等绝对定位信息的情况下，依赖视觉或者惯导等航位推算方法，结合地图实现可靠定位；

2)本发明能有效修正航位推算算法的累积误差，提高长距离定位的精度，即使在车辆长距离行驶的情况下仍然具备良好的定位精度；

3)本发明使用拐点滤波的方法框架，与传统的每个传感器数据到来时刻进行粒子滤波的方法不同，只在轨迹拐点处进行粒子滤波，大大减小了运算量，实时性强，并提高了定位效果。

附图说明

图1是本发明方法的流程图。

图2是实施例的粒子滤波实施步骤示意图。

图3是实施例KITTY08序列航位推算方法结果。

图4是实施例KITTY08序列本方法结果。

图5是实施例KITTY08序列真值轨迹。

图6是实施例KITTY08序列航位推算方法和本方法定位误差比较。

具体实施方式

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

如图1所示，本发明方法的实施例及其实施过程如下：

下面以一个简单的例子(由图2所示)来更清楚地表述本发明滤波的过程。图2.(a)为通过航位推算方法和轨迹分段得到车辆分段后的轨迹，轨迹中有五个拐点(把终点也当作拐点处理)，因此用五组粒子来表示这五个拐点在地图中对应的位置。以下为具体滤波的步骤：

步骤一：假设车辆起点已知(图2.(b)标号为0的节点)，当初始化第一组粒子的时候，找到所有与节点0相关的节点(图2.(b)灰色区域)，作为第一组粒子初始化分布的位置。根据上述的步骤更新粒子的权重，从图2.(c)中可见，所有粒子都归属于同一个节点(图2.(c)标号为1的节点)，因此不需要再次更新这堆粒子，找出权重最大的那个粒子作为第一个拐点对应的位置。

步骤二：下一步为搜索所有第一组粒子归属的节点(图2.(c)标号为1的节点)的相关节点，作为第二组粒子初始化的位置(图2.(c)灰色区域)。从图2.(d)可看出，经过粒子更新，第二组的粒子归属于三个节点(图2.(d)标号为2的三个节点)，因此需要保留所有粒子继续迭代。

步骤三：下一步为搜索所有第二组粒子归属的节点(图2.(d)标号为2的节点)的相关节点，作为第三组粒子初始化的位置(图2.(d)灰色区域)。从图2.(e)可看出，经过粒子更新，第二组粒子和第三组粒子分别有三个归属的节点(图2.(e)标号为2和3的节点)，因此第二组粒子和第三组粒子都需要保留所有粒子继续迭代更新。

步骤四：下一步为搜索所有第三组粒子归属的节点(图2.(e)标号为3的节点)的相关节点，作为第四组粒子初始化的位置(图2.(e)灰色区域)。从图2.(f)可看出，经过粒子更新，第二组粒子和第三组粒子都已经分别归属于一个节点(图2.(f)标号为2和3的节点)，而第四组粒子有两个归属的节点(图2.(f)标号为4的两个节点)，因此第二组粒子和第三组粒子都不需要再次更新，分别找出第二组粒子和第三组粒子中权重最大的粒子作为第二个拐点和第三个拐点对应的位置，而第四组粒子需要保留所有粒子继续迭代。

步骤五：下一步为搜索所有第四组粒子归属的节点(图2.(f)标号为4的节点)的相关节点，作为第五组粒子初始化的位置(图2.(f)灰色区域)。从图2.(g)可看出，经过粒子更新，第四组粒子和第五组粒子都已经分别归属于一个节点(图2.(g)标号为4和5的节点)，此时所有的拐点的位置都已经可以确定，找出每一组粒子中权重最大的粒子，其位置作为对应拐点更新后的位置。将各个拐点更新后的位置和起点按行驶时序依次用直线连接作为滤波后得到的车辆行驶轨迹(图2.(h))。

如果通过航位推算得到车辆的行驶轨迹的终点离地图上的节点太远，则不通过滤波得到终点的位置，而是通过上一个拐点更新后的位置和航位推算直接得到终点的位置。

另外测试了KITTY数据集来评估本方法的效果，以双目图像序列以及对应的路网地图作为输入，根据KITTY提供的真值，比较采用航位推算定位方法和本方法与真值的平均误差。

实验主要测试了KITTY图像序列08。序列08航位推算定位的结果如图3所示，本方法的结果如图4所示，真值如图5所示，可以看出航位推算定位在拐弯处累积了较大的角度误差，导致定位的累积误差大大增加，而本方法可以有效地消除累积误差，使定位误差一直维持在一个比较小的范围内。误差对比曲线如图6所示，平均定位误差从45.61m减小到3.25m，见下表1。

表1 航位推算方法与本方法定位误差比较

实验采用一台配备主频为2.8GHz的inter i5处理器的电脑，实验中每次迭代平均耗时70.62ms。由于拐点滤波不需要每一帧都进行迭代，所以相对于航位推算定位，增加的计算代价很小。

可以看出，航位推算定位误差随着距离的增加而增大，而采用本方法能有效地修正航位推算方法产生的累积定位误差，使定位误差一直处于一个比较小的范围内，而且计算代价小，可实时运行。

Claims (8)

1.一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于包括如下步骤：

1)在主要由节点和边构成的路网地图中，车辆从已知位置的起点出发，在车辆行驶过程中，通过航位推算方法实时获得车辆初步位置，将每个时刻的车辆初步位置与起点依次连接形成车辆行驶初步轨迹，对车辆行驶初步轨迹实时检测拐点获得拐点并标记，并将起点作为拐点，在各拐点将车辆行驶初步轨迹用直线段进行分段，分为各段车辆轨迹分段；

2)获得每个拐点后对拐点进行实时更新，将各个实时更新后的拐点和起点按行驶时序依次连接作为最终车辆行驶轨迹。

2.根据权利要求1所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述路网地图用节点和边来构建：边作为道路，以节点作为道路与道路之间的交叉路口。

3.根据权利要求1所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述对车辆行驶初步轨迹实时检测拐点获得拐点具体是：

1.1)在车辆行驶过程中，将通过航位推算方法实时获得当前时刻下的车辆初步位置与行驶经过最近的一个拐点用直线进行连接，计算在当前时刻下的车辆初步位置与行驶过最近的一个拐点之间的每一个中间时刻下的车辆初步位置到该直线的垂直距离；

1.2)将垂直距离与拐点阈值进行比较：

若所有中间时刻下的车辆初步位置到该直线的垂直距离小于等于拐点阈值，则当前时刻下的车辆初步位置与行驶经过最近的一个拐点之间的车辆行驶初步轨迹不存在拐点；

若所有中间时刻下的车辆初步位置到该直线的垂直距离大于拐点阈值，则车辆初步位置到该直线的垂直距离最大所在的中间时刻的车辆初步位置作为新的拐点，通过新的拐点将当前时刻下的车辆初步位置与行驶经过最近的一个拐点之间的车辆行驶初步轨迹分为两段车辆轨迹分段，

1.3)重复迭代上述两个步骤，直到行驶经过最近的一段车辆轨迹分段中所有中间时刻下的车辆初步位置到该直线的垂直距离小于等于拐点阈值。

4.根据权利要求1所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述的对拐点进行实时更新具体是通过构建粒子并通过粒子滤波方法计算获得每一个粒子的权重，通过比较获得权重最大的粒子，并用该粒子对其所在拐点进行替换，从而实现对拐点的实时更新。

5.根据权利要求1或4所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述步骤2)实时更新中每次计算的具体过程如下：

2.1)对于每个当前拐点，在与上一个拐点最近的节点的各个相关节点周围附近随机生成一堆新粒子，每个新粒子归属于其所生成对应的节点，相关节点指的是该节点所在边上的节点；

2.2)每个当前拐点下生成的所有新粒子形成了该当前拐点下的一组粒子，对于生成的每一组粒子采用以下方式处理：

将每一个新生成的粒子分别与上一个拐点周围附近的粒子中在同一条边上对应的各个粒子进行两两组合用直线段进行连接，每两个粒子之间的直线段作为备选线段，然后将每一条备选线段的长度和角度与对应的车辆轨迹分段的长度和角度进行比较，获得每一条备选线段的长度误差和角度误差：

e_a＝|h_c-h_s|

式中，e_l为长度误差，l_c为备选线段的长度，l_s为对应的车辆轨迹分段的长度，e_a为角度误差，h_c为备选线段的角度，h_s为对应的车辆轨迹分段的角度；

判断备选线段的长度误差和角度误差是否在阈值范围内，若在阈值范围内则保留该备选线段作为车辆可能的轨迹，否则舍弃该备选线段；

2.3)再通过以下公式计算每一条备选线段与其对应的车辆轨迹分段之间的相似度s_s：

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

其中，s_l为备选线段长度的相似度，s_a为备选线段角度的相似度，s_s为备选线段与车辆轨迹分段之间的相似度，α表示权重参数；

2.4)将步骤2.2)处理后保留的备选线段与上一次拐点迭代计算后剩余的备选线段通过树结构相连形成了所有可能的各条备选路径；对于当前拐点的每一个粒子，然后采用以下公式计算各条包含有该粒子的备选路径与车辆行驶初步轨迹之间的相似度s_r：

式中，s_s[i]表示一条备选路径中第i段备选线段与其对应的车辆轨迹分段之间的相似度，M表示车辆行驶初步轨迹中车辆轨迹分段的总数，i表示车辆轨迹分段的段序数；

2.5)通过以下公式计算所有包含该粒子的备选路径与车辆行驶初步轨迹相似度的平均值，作为粒子的权重：

式中，w_i,j代表第i组第j个粒子的权重，表示第k条备选路径与车辆行驶初步轨迹之间的相似度，n代表该粒子下所有备选路径的总数，k表示该粒子下备选路径的序数；

2.6)对当前拐点下该组所有粒子是否归属于同一个节点进行判断：

若当前拐点下该组所有粒子是均归属于同一个节点，则取其中权重最大的粒子替换当前拐点作为当前在路网地图上的位置，将当前拐点周围附近的其余所有粒子舍弃；

若当前拐点下该组所有粒子不是均归属于同一个节点，则该组所有粒子全部保留并作为下一次计算的剩余粒子，返回到步骤2.1)继续进行迭代滤波，对所有粒子进行按照其权重重采样，进行实时更新。

6.根据权利要求5所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述步骤2.2)中对于起点后的第一个拐点，将该拐点生成的每一个粒子分别与起点进行组合用直线段进行连接，获得各条备选线段。

7.根据权利要求5所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述步骤2.3)中的备选线段长度的相似度s_l和备选线段角度的相似度s_a采用以下公式计算：

式中，e表示自然常数，e_l为长度误差，e_a为角度误差。

8.根据权利要求5所述的一种面向无人车导航的基于轨迹拐点滤波的地图定位方法，其特征在于：所述步骤2.1)中，若上一个拐点计算后存在剩余粒子，所述当前拐点生成粒子时还要在上一个拐点的剩余粒子周围附近随机再生成一堆粒子。