CN114440892A - 一种基于拓扑地图和里程计的自定位方法 - Google Patents

Abstract

本发明属于机器人自定位技术领域，提供一种基于拓扑地图和里程计的自定位方法，S1提取某区域的拓扑地图；S2时序地获取里程计的定位结果，并判断是否完成初始化过程，是转到S7，否则转到S3，进入初始化阶段；S3提取里程计轨迹的特征点；S4利用轨迹特征点与拓扑地图做形状匹配；S5检测S4中匹配结果的道路一致性；S6计算里程计坐标系和拓扑地图坐标系之间的二维坐标变换，完成初始化过程；S7将里程计轨迹投影到拓扑地图上；S8里程计与拓扑地图间进行地图匹配；S9利用卡尔曼滤波对匹配结果进行融合；S10修正里程计的累积定位误差。本发明仅利用拓扑地图能有效约束里程计随时间增加的累积定位误差，同时具有普适性可适用不同形式的里程计。

Description

一种基于拓扑地图和里程计的自定位方法

技术领域

本发明属于机器人自定位领域，具体涉及一种基于拓扑地图和里程计的自定位方法。

背景技术

移动机器人的自定位作为整个系统的上游技术，定位质量会影响规划和控制模块的效果。因此准确的自定位是移动机器人最基本，也是最重要的任务之一。目前，移动机器人在大尺度室外场景中的自定位主要依赖基于全球定位系统(Global PositioningSystem，GPS)的定位技术。但是，GPS信号在一些环境中容易受到干扰和遮挡，如高楼林立的城市环境中GPS存在多路径反射效应，茂密的树林和隧道中GPS信号会被遮挡。因此，移动机器人在室外大尺度环境中的自定位仅靠GPS不够稳定可靠。

在GPS受限的场景中，里程计可以作为移动机器人的辅助定位方式。根据传感器类型，可以将常见的里程计分为视觉里程计、激光雷达里程计和轮速计等。然而，这些里程计都会不可避免地产生累积定位误差。一般来说，有以下几种方法可以用来解决里程计累积误差的问题。首先是引入全局定位信息，比如利用GPS信息修正里程计的漂移误差，但是这种方法在GPS受限的情况下不可用。其次，通过回环检测可以有效消除里程计的累积误差，该方法在基于视觉/激光雷达的同步定位与地图构建(Simultaneous Localization andMapping，SLAM)技术中经常用到，可以在小范围场景中通过频繁的回环检测修正误差。但是这种方法在大尺度的室外场景中不一定能够起到作用，因为回环发生的频率很低甚至没有回环的发生，里程计的累积误差就无法得到消除。再者，通过多传感器融合的策略可以充分发挥每种传感器的优势，有效降低里程计的累积误差，例如可以将视觉、激光雷达、轮速计和IMU(Inertial Measurement Unit)等进行有效融合。但是多传感器的融合会引入额外的问题，例如多传感器间的时空标定，系统会变得更加复杂，且这种方式无法彻底抑制定位误差随时间的增长。最后，通过与先验的度量地图做匹配也可以修正里程计定位误差，但是需要提前采集数据和制作度量地图，且该地图的更新和维护也是昂贵和低效的。

因此，在GPS受限的大尺度场景中，如何约束里程计的累积定位误差具有重要的研究价值。针对现有方法的不足，本发明提出了一种基于拓扑地图和里程计的自定位方法。与度量地图相比，拓扑地图更加轻量化，且易于获取和维护。

发明内容

本发明的目的在于提供一种移动机器人在卫星受限的大尺度环境中的定位方法，本发明要解决的技术问题是限制现有里程计定位方式在大范围场景中的累积定位误差。

为达到上述目的，本发明提出一种基于拓扑地图和里程计的自定位方法，包括以下步骤：

S1：提取某区域的拓扑地图，地图中包含该区域的道路信息和道路上节点的坐标。

S2：时序地获取里程计的定位结果，并判断是否完成初始化过程，是的话转到步骤S7，否则转到步骤S3，进入初始化阶段。

S3：提取里程计轨迹的特征点。

S4：利用S3中的轨迹特征点与拓扑地图做形状匹配。

S5：检测S4中匹配结果的道路一致性。

S6：根据S5中得到的结果计算里程计坐标系和拓扑地图坐标系之间的二维坐标变换，完成初始化过程。

S7：根据S6中的坐标变换关系将里程计轨迹投影到拓扑地图上。

S8：里程计与拓扑地图间进行地图匹配。

S9：利用卡尔曼滤波对S8中的匹配结果进行融合。

S10：根据S9中状态估计的结果修正里程计的累积定位误差。

优选地，步骤S3进一步包括：

S300：分段提取里程计轨迹的特征点，每一段的起点都位于上一个提取的轨迹特征点之后，第一段的起点是里程计轨迹的起始点。

S301：将每段里程计轨迹的首尾相连，形成一条直线L_t，计算该段轨迹中每个里程计采样点到该直线的距离d_i，这些距离的集合记为D，D中的最大值max(D)若大于设定的特征提取阈值d_th，则max(D)对应的里程计采样点就是该段轨迹的特征点。

优选地，步骤S4进一步包括：

S400：当S3中提取的里程计轨迹特征点数量超过设定的阈值N_F时，按照特征点提取的先后顺序计算相邻特征点间形成的长度l、角度θ和方向φ信息。计算公式如下：

l_k＝||P_k,P_k+1||

φ_k＝sign((P_k-P_k-1)×(P_k+1-P_k))

其中，P_k表示第k个轨迹特征点的位置坐标，|| ||表示两点之间的欧式距离，sign(x)函数根据x的符号表示特征点的旋转方向。通过特征点之间的位置关系可以得到轨迹的特征序列F＝(L,Θ,Φ)_F，l_k∈L，θ_k∈Θ，φ_k∈Φ。利用特征序列F与拓扑路网中的节点序列做形状匹配。

S401：与轨迹特征序列匹配的拓扑路网节点序列N＝(L,Θ,Φ)_N具有相似的曲线形状特征，匹配的结果应满足如下关系：

其中，α和β分别是里程计长度和角度的漂移系数。

优选地，步骤S5进一步包括：

S500：对S4中每种匹配结果做奇异值分解，得到轨迹的特征点与每种匹配的节点序列间的坐标变换关系。

S501：根据坐标变换关系将每种可能的结果投影到拓扑地图上，然后统计该段轨迹中每个里程计采样点附近的地图节点数量，数量最多的即为最终的匹配结果。

优选地，步骤S8进一步包括：

S800：当里程计的定位结果与拓扑地图中的某个地图节点间的欧式距离小于设定的匹配阈值d_th2时，该地图节点即为待匹配的点；

S801：本发明提出一种延迟更新策略，让里程计继续运行，寻找与待匹配的地图节点距离最近的里程计位置，认为该里程计位置与待匹配的地图节点是匹配的。

优选地，步骤S9中利用卡尔曼滤波对里程计和拓扑地图的信息进行融合，其中将里程计运动模型看做状态方程，拓扑地图提供的信息被视为观测方程。

本发明的有效收益：

1、本发明是面向卫星受限的大尺度室外环境中基于拓扑地图和里程计的机器人自定位方法。本发明无需提供载体的初始位置，通过提取里程计轨迹的特征点，与拓扑地图做形状匹配来确定载体的初始定位信息。

2、本发明视拓扑地图为一种传感器，将其提供的信息建模为系统的观测方程，同时将里程计的运动模型建模为系统的状态方程，借助卡尔曼滤波器对二者信息进行融合，融合过程中本发明提出了一种新颖的地图匹配和更新策略，实现了对里程计漂移误差的估计和修正。

3、本发明无需先验的高精度度量地图，仅利用环境的拓扑地图限制里程计的累积定位误差，与现有技术相比，可以在卫星受限的大尺度室外场景显著提升里程计的定位精度，同时具有普适性，可适用不同形式的里程计。

附图说明

图1为本发明一种基于拓扑地图和里程计的自定位方法框架；

图2为本发明具体实施例中对应的开源地图和从中提取的拓扑地图；

图3为本发明提取里程计轨迹特征点的示意图；

图4为本发明具体实施例中初始化阶段里程计轨迹与拓扑地图的匹配结果；

图5为本发明具体实施例中里程计完成初始化行驶的轨迹；

图6为本发明里程计采样点与拓扑地图匹配的示意图；

图7为本发明利用卡尔曼滤波融合里程计与拓扑地图信息的示意图。

具体实施方式

以下通过特定的具体实例并结合说明书附图对本发明作进一步的详细描述，本领域技术人员可由本说明书所揭示的内容轻易地了解本发明的其它优点与功效。本发明亦可通过其它不同的实例加以施行或应用，本说明书中的各项细节亦可基于不同的观点与应用，在不背离本发明的精神下进行各种修饰与变更。

图1给出了本发明实施例的一种基于拓扑地图和里程计的自定位方法的框架。如图1所示，本发明一种基于拓扑地图和里程计的自定位方法包括以下步骤：

S1：提取某区域的拓扑地图，地图中包含该区域的道路信息和道路上节点的坐标。

具体地，以KITTI里程计数据集中的00序列为例进行说明，该序列对应的拓扑地图如图2(a)所示，线段表示道路，黑色点是地图中的节点。图2(b)是该序列对应的开源地图，拓扑地图从开源地图中解析提取得到。

S2：以视觉里程计(Visual Odometry)为例，时序地获取里程计的定位结果，并判断是否完成初始化过程，是的话转到步骤S7，否则转到步骤S3，进入初始化阶段。

S3：提取里程计轨迹的特征点。

具体地，轨迹特征点指里程计轨迹中曲率较大的点，图3是里程计轨迹特征点提取原理图，其中三角形的里程计采样点是我们要提取的轨迹特征点。所述步骤S3进一步包括：

S300：分段提取里程计轨迹的特征点，每一段的起点都位于上一个提取的轨迹特征点之后，第一段的起点是里程计轨迹的起始点。

S301：将每段里程计轨迹的首尾相连，形成一条直线L_t，计算该段轨迹中每个里程计采样点到该直线的距离d_i，这些距离的集合记为D，D中的最大值max(D)若大于设定的特征提取阈值d_th，则max(D)对应的里程计采样点就是该段轨迹的特征点。

S4：利用S3中的轨迹特征点与拓扑地图做形状匹配。

具体地，利用提取的轨迹特征点形成的长度、角度和方向信息计算轨迹特征，利用轨迹特征在拓扑地图的节点中做深度优先搜索，找到与里程计轨迹特征点序列形状相似的节点序列。步骤S4进一步包括：

S400：当S3中提取的里程计轨迹特征点数量超过设定的阈值N_F时，按照特征点提取的先后顺序计算相邻特征点间形成的长度l、角度θ和方向φ信息。计算公式如下：

l_k＝||P_k,P_k+1||

φ_k＝sign((P_k-P_k-1)×(P_k+1-P_k))

其中，P_k表示第k个轨迹特征点的位置坐标，|| ||表示两点之间的欧式距离，sign(x)函数根据x的符号表示特征点的旋转方向。通过特征点之间的位置关系可以得到轨迹的特征序列F＝(L,Θ,Φ)_F，l_k∈L，θ_k∈Θ，φ_k∈Φ。利用特征序列F与拓扑路网中的节点序列做形状匹配。

S401：与轨迹特征序列匹配的拓扑路网节点序列N＝(L,Θ,Φ)_N具有相似的曲线形状特征，匹配的结果应满足如下关系：

其中，α和β分别是里程计长度和角度的漂移系数。

图4展示了里程计轨迹特征点序列与图2中的拓扑地图形状匹配结果，拓扑地图中有三个节点序列与里程计的轨迹特征点序列有较高的曲线相似度。

S5：检测S4中匹配结果的道路一致性。

具体地，步骤S5进一步包括：

S500：对S4中每种匹配结果做奇异值分解，得到轨迹的特征点与每种匹配的节点序列间的坐标变换关系。

S501：根据坐标变换关系将每种可能的结果投影到拓扑地图上，然后统计该段轨迹中每个里程计采样点附近的地图节点数量，数量最多的即为最终的匹配结果。

图4中①号匹配结果与拓扑地图中的道路是一致的，而②号和③号的匹配结果跨越了拓扑地图中不存在的道路，是不一致的。通过步骤S5可以获得道路一致的匹配结果。

S6：根据S5中得到的结果计算里程计坐标系和拓扑地图坐标系之间的二维坐标变换，完成初始化过程。

S7：根据S6中的坐标变换关系将里程计轨迹投影到拓扑地图上。

具体地，将里程计的行驶轨迹投影到图2中的拓扑地图上，结果如图5所示，其中红色曲线是里程计完成初始化走过的轨迹。

S8：里程计与拓扑地图间进行地图匹配。

具体地，图6是里程计与拓扑地图匹配的示意图。步骤S8进一步包括：

S800：当里程计的定位结果与拓扑地图中的某个地图节点间的欧式距离小于设定的匹配阈值d_th2时，该地图节点即为待匹配的点；图6中五角星里程计采样点与拓扑地图中三角形节点间的距离首次小于阈值d_th2，因此该三角形地图节点被选为待匹配节点。

S801：本发明提出一种延迟更新策略，让里程计继续运行，寻找与待匹配的地图节点距离最近的里程计位置，认为该里程计位置与待匹配的地图节点是匹配的。图6中正方形的里程计采样点与三角形节点间距离最小，本发明中认为它们是匹配的。

S9：利用卡尔曼滤波对S8中的匹配结果进行融合。

具体地，步骤S9中利用卡尔曼滤波对里程计和拓扑地图的信息进行融合，其中将里程计运动模型看作状态方程，拓扑地图提供的信息被视为观测方程。状态方程和观测方程如下：

状态方程：

其中，

表示k+1时刻地图坐标系下的里程计位置状态；

和

分别表示k时刻局部坐标系下里程计的位置和相邻时刻之间的相对变化量；

表示从局部坐标系到地图坐标系的变换矩阵；ω_k表示相对变化量

的计算误差，本发明中假设ω_k的概率密度函数服从零均值高斯分布。

观测方程：z_k＝h(x_k)+n_k

其中，z_k是拓扑路网中匹配节点的坐标；x_k是里程计估计得到的定位状态；h(·)表示量测函数；n_k表示拓扑路网提供的观测误差，本发明假设其概率密度函数同样服从零均值高斯分布。

图7展示了利用卡尔曼滤波对拓扑地图和里程计信息融合的示意图，四角星是融合后的里程计定位结果，该点与正方形的里程计采样点间的距离即为里程计的累积定位误差。

S10：根据S9中状态估计的结果修正里程计的累积定位误差。

具体地，本发明将S9中计算得到的累积定位误差平均分配到里程计的历史轨迹T中，T的选取是上一次更新的里程计位置到当前要更新的里程计位置之间的轨迹。

为了更好地说明本发明的技术效果，发明人以视觉里程计为例，在KITTI里程计数据集的00、02、05、08和09序列上进行了实验。视觉里程计基于开源的视觉SLAM方案VINS-fusion，KITTI里程计数据集是一个公开的无人车数据集，包含22个序列，每个序列包含有双目图像数据、激光雷达数据以及对应的标定参数，常用来评估基于视觉/激光雷达定位算法的性能，本发明选择这22个序列中里程较长且有轨迹真值的00、02、05、08、09序列进行测试。以无回环修正和拓扑地图辅助的视觉里程计定位结果为基准，实验结果如表1所示：

表1 有/无拓扑地图辅助的视觉里程计平均定位误差比较(单位：米)

从表1的结果可以看出，本发明相对无拓扑地图辅助的视觉里程计定位精度有较大提高，尤其在02序列上，平均定位误差从155.4米降低到4.9米。这验证了本方法利用拓扑地图约束里程计累积误差的有效性和在面对里程计大漂移误差时的鲁棒性。

虽然本发明已通过优选实施例进行了描述，然而本发明并非局限于这里所描述的实施例，在不脱离本发明范围的情况下还包括所做出的各种改变以及变化。

Claims (6)

1.一种基于拓扑地图和里程计的自定位方法，其特征在于，包括以下步骤：

S1：提取某区域的拓扑地图，地图中包含该区域的道路信息和道路上节点的坐标；

S2：时序地获取里程计的定位结果，并判断是否完成初始化过程，完成跳转到步骤S7，否则转到步骤S3，进入初始化阶段；

S3：提取里程计轨迹的特征点；

S4：利用S3中的轨迹特征点与拓扑地图做形状匹配；

S5：检测S4中匹配结果的道路一致性；

S6：根据S5中得到的结果计算里程计坐标系和拓扑地图坐标系之间的二维坐标变换，完成初始化过程；

S7：根据S6中的坐标变换关系将里程计轨迹投影到拓扑地图上；

S8：里程计与拓扑地图间进行地图匹配；

S9：利用卡尔曼滤波对S8中的匹配结果进行融合；

S10：根据S9中状态估计的结果修正里程计的累积定位误差。

2.根据权利要求1所述的一种基于拓扑地图和里程计的自定位方法，其特征在于，所述S3采用如下步骤实现：

S300：分段提取里程计轨迹的特征点，每一段的起点都位于上一个提取的轨迹特征点之后，第一段的起点是里程计轨迹的起始点；

S301：将每段里程计轨迹的首尾相连，形成一条直线L_t，计算该段轨迹中每个里程计采样点到该直线的距离d_i，这些距离的集合记为D，D中的最大值max(D)若大于设定的特征提取阈值d_th，则max(D)对应的里程计采样点就是该段轨迹的特征点。

3.根据权利要求2所述的一种基于拓扑地图和里程计的自定位方法，其特征在于，所述S4采用如下步骤实现：

S400：当S3中提取的里程计轨迹特征点数量超过设定的阈值N_F时，按照特征点提取的先后顺序计算相邻特征点间形成的长度l、角度θ和方向φ信息，计算公式如下：

l_k＝||P_k,P_k+1||

φ_k＝sign((P_k-P_k-1)×(P_k+1-P_k))

其中，P_k表示第k个轨迹特征点的位置坐标，|| ||表示两点之间的欧式距离，sign(x)函数根据x的符号表示特征点的旋转方向；通过特征点之间的位置关系可以得到轨迹的特征序列F＝(L,Θ,Φ)_F，l_k∈L，θ_k∈Θ，φ_k∈Φ；利用特征序列F与拓扑路网中的节点序列做形状匹配；

S401：与轨迹特征序列匹配的拓扑路网节点序列N＝(L,Θ,Φ)_N具有相似的曲线形状特征，匹配的结果应满足如下关系：

其中，α和β分别是里程计长度和角度的漂移系数。

4.根据权利要求3所述的一种基于拓扑地图和里程计的自定位方法，其特征在于，所述S5采用如下步骤实现：

S500：对S4中每种匹配结果做奇异值分解，得到轨迹的特征点与每种匹配的节点序列间的坐标变换关系；

S501：根据坐标变换关系将每种可能的结果投影到拓扑地图上，然后统计该段轨迹中每个里程计采样点附近的地图节点数量，数量最多的即为最终的匹配结果。

5.根据权利要求1所述的一种基于拓扑地图和里程计的自定位方法，其特征在于，所述S8采用如下步骤实现：

S800：当里程计的定位结果与拓扑地图中的某个地图节点间的欧式距离小于设定的匹配阈值d_th2时，该地图节点即为待匹配的点；

S801：设置一种延迟更新策略，让里程计继续运行，寻找与待匹配的地图节点距离最近的里程计位置，认为该里程计位置与待匹配的地图节点是匹配的。

6.根据权利要求1-5任一所述的一种基于拓扑地图和里程计的自定位方法，其特征在于，所述S9利用卡尔曼滤波对里程计和拓扑地图的信息进行融合，其中将里程计运动模型看做状态方程，拓扑地图提供的信息被视为观测方程。

Images