CN111220153B - 基于视觉拓扑节点和惯性导航的定位方法 - Google Patents

Abstract

本发明公开了一种基于视觉拓扑节点和惯性导航的定位方法，所述方法包括：读取地图以获得各个拓扑节点信息，基于拓扑路径规划得到驶向最终目标点的全局拓扑路径；当前机器人所在位置为P_t，当前在所述全局拓扑路径中由拓扑节点N_n驶向拓扑节点N_m的过程中，在全局路径规划下基于机器人所在环境进行局部路径规划；机器人接受并执行所述控制信息，执行结束后进行状态判断，当前的机器人定位信息与当前目标拓扑节点N_m进行比较，若判断未达到目标拓扑节点，继续进行局部路径规划和控制；若由当前的定位信息判断机器人目前已到达拓扑节点，调用视觉定位，利用视觉定位结果强制更新机器人定位，修正在拓扑节点N_n到N_m运行过程中产生的漂移。

Description

基于视觉拓扑节点和惯性导航的定位方法

技术领域

本发明属于移动机器人定位导航技术领域，特别是一种基于视觉拓扑节点和惯性导航的定位方法。

背景技术

移动机器人自主导航定位是指利用移动机器人负载的各类传感器对环境进行感知，估计机器人自身的位姿，机器人根据自身位姿，设定的目的地以及周围的环境信息规划路径，并在行驶过程中实时调整和优化路径，并能规避障碍物，达到目的地，完成任务。移动机器人的定位导航技术是用来解决机器人行走的问题，是移动智能体应用的基础。

目前，移动机器人的定位导航技术分为有标记和无标记两类，其中有标记导航技术通过人为地在地图中加入标记信息来进行定位，如专利《基于二维码导引与可见光定位的室内AGV导航方法及系统》(公布号CN107943051A)，利用二维码标识进行定位，专利《一种基于RFID的精准定位系统及方法》(公开号:CN108229605A),利用RFID进行定位，有标记导航技术需要对环境进行人为改造，仅能用在特定区域，不具有普适性；无标记技术通过算法寻找环境中的特征信息进行定位导航，对环境适应性更强，是目前被普遍接受的方法，无标记导航技术主要包括激光和视觉两大类：

1、激光定位导航：如专利《一种激光SLAM导航的临时障碍物处理方法(公开号CN108303986A)。通过激光传感器测定机器人所在环境的轮廓，将轮廓与已知地图进行匹配，从而估算机器人的位置。

2、视觉定位导航：如专利《一种基于Harris角点检测的视觉导航方法》(公开号CN110119670).通过检测环境中的Harris角点作为地标信息，通过几何方法匹配计算出机器人位姿。

以上提到的方案中仅使用单一传感器进行定位与导航，其导航精度完全受制于传感器，但由于传感器不可避免的含有噪声，因而在实际使用中，激光和视觉定位技术总会带来一定的漂移，为了减少传感器带来的漂移，目前的技术中，常在单一传感器系统中引入其他传感器进行滤波，由于激光传感器造价较高，且在室外条件下效果较差，视觉+其他传感器成为目前研究的主流，其也带来一个问题，基于视觉的导航方法要求机器人在每一个控制周期，都要计算视觉特征点，并将其进行匹配，此两步对计算量有较高的需求，极大的限制了自主定位导航技术在实际场景中的有效应用。

在背景技术部分中公开的上述信息仅仅用于增强对本发明背景的理解，因此可能包含不构成在本国中本领域普通技术人员公知的现有技术的信息。

发明内容

针对现有技术中存在的计算量过大且无法进行避障的问题，本发明提出一种基于视觉拓扑节点和惯性导航的定位方法。

本发明的目的是通过以下技术方案予以实现，一种基于视觉拓扑节点和惯性导航的定位方法包括以下步骤：

第一步骤中，读取地图以获得各个拓扑节点信息，记作N₁，N₂......N_n，拓扑节点具有以下数据结构：

N_n：{

E_mn，

NP_n，

KPT：{KPT₁，KPT₂......KPT_n}，

DES：{DES₁，DES₂......DES_n}

}，其中，E_mn表示从拓扑节点N_m到拓扑节点N_n这个方向是可以行驶到达的；NP_n表示拓扑节点所在世界坐标系的坐标；KPT_k表示第k个视觉特征点的特征信息；DES_k表示第k个视觉特征点的描述符；第二步骤中，基于拓扑路径规划得到驶向最终目标点的全局拓扑路径；

第三步骤中，当前机器人所在位置为P_t，当前在所述全局拓扑路径中由拓扑节点N_n驶向拓扑节点N_m的过程中，在全局路径规划下基于机器人所在环境进行局部路径规划，其中，基于惯性导航得到机器人定位信息，基于视觉定位方法更新机器人定位信息，基于机器人定位信息和障碍物信息局部路径规划得到机器人控制信息；

第四步骤中，机器人接受并执行所述控制信息，执行结束后进行状态判断，当前的机器人定位信息与当前目标拓扑节点N_m进行比较，若判断未达到目标拓扑节点，继续进行局部路径规划和控制；若由当前的定位信息判断机器人目前已到达拓扑节点，调用视觉定位，利用视觉定位结果强制更新机器人定位，修正在拓扑节点N_n到N_m运行过程中产生的漂移，完成定位更新后，判断当前是否已达最终目标点，若未达到，回到局部路径规划，继续产生规划路径进行控制；若判断达到最终目标点，定位结束。

所述的方法中，拓扑地图的拓扑节点信息包括拓扑节点的位姿信息和视觉信息。

所述的方法中，所述位姿信息包括三维位置信息及三维转向角信息，所述视觉信息包括视觉的特征点及所述特征点的描述符。

所述的方法中，第一步骤中，读取地图时，将机器人启动时的位置作为世界坐标系原点，记作P₀。

所述的方法中，第二步骤中，基于路径规划算法如：Dijkstra得到驶向最终目标点的全局拓扑路径。

所述的方法中，第三步骤中，基于惯性导航得到机器人定位信息时，机载惯性测量单元IMU在预定固定频率f下采样机器人的瞬时加速度并对测量值积分得到机器人相对于出发原点P₀的相对位移ΔP，得到的机器人在世界坐标系下的位置P_t＝P₀+ΔP。

所述的方法中，第三步骤中，基于视觉定位方法更新机器人定位信息时，读入当前机载摄像头采集的图像信息以提取特征点，得到当前时刻机器人所在位置的特征点信息KPT_t：{KPT_t1，KPT_t2......KPT_tn}，针对每一个特征点提取描述符得到当前时刻描述符信息DES_t：{DES_t1，DES_t2......DES_tn}，比较和匹配当前时刻的特征信息与拓扑节点信息得到当前画面和拓扑节点的匹配点对，根据不同视角下的匹配点对的对极几何约束，计算得到当前机器人所在位置与拓扑节点的精确的相对位置关系。

所述的方法中，第三步骤中，采用基于神经网络的语义分割方法环境障碍分析，使用道路分割数据集训练，训练完成后对网络输入机载摄像头采集的图像输出图像中的可行使道路，根据图像和道路信息，建立机器人所在环境的带有障碍物信息的二维栅格局部地图。

所述的方法中，第三步骤中，基于如ERFNET语义分割算法环境障碍分析得到障碍物信息。

所述的方法中，第三步骤中，基于机器人定位信息和障碍物信息采用如DWA算法进行局部路径规划。

有益效果

现有技术的定位导航方式要求移动机器人在任意时刻都能很精确的估计自身位姿，本发明对任意时刻的状态的精确度要求较低，仅有一个模糊的位置估计，在导航中通过验证特定的路标点，就能完成导航任务，本发明使用视觉拓扑图，相比目前广泛使用的二维栅格地图，地图体积小，且全局规划路径在拓扑节点上进行，计算简单对算力要求很低，且仅在拓扑节点保存视觉信息，因此，本发明可以有效降低建图工作的难度和复杂度，同时拓扑地图存储占用空间小，传输占用带宽小，有利于完成机载计算和多智能体间的地图共享；本发明使用粗略定位与精确定位相结合的方式，在拓扑节点间使用惯导结合轮速计进行粗略定位，在拓扑节点处进行视觉精确定位，相比纯惯导定位，仅仅在拓扑节点处增加了视觉信息的计算，增加的算力需求较少，但拓扑节点处的精确定位，可以进行信息校准，有效消除了惯导的累计误差，提高了系统精度。因此，本发明兼顾了导航系统中精度与运算能力的平衡，减少了对于软硬件性能的需求，更适合在机载和车载嵌入式平台上完成。本发明在导航控制中引入语义分割神经网络进行道路分割，划分了可行使区域及障碍物信息，并通过嵌入式加速技术，达到了实时计算的能力因此，本发明具有实时避障的功能，加强了移动机器人对动态场景的适应能力，提高了自主定位导航系统的可靠性。

附图说明

通过阅读下文优选的具体实施方式中的详细描述，本发明各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。说明书附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。显而易见地，下面描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。而且在整个附图中，用相同的附图标记表示相同的部件。

在附图中：

图1是具体实施方式所述的一种基于视觉的融合地标的拓扑地图构建方法的流程示图。

以下结合附图和实施例对本发明作进一步的解释。

具体实施方式

下面将参照附图1更详细地描述本发明的具体实施例。虽然附图中显示了本发明的具体实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。

需要说明的是，在说明书及权利要求当中使用了某些词汇来指称特定组件。本领域技术人员应可以理解，技术人员可能会用不同名词来称呼同一个组件。本说明书及权利要求并不以名词的差异来作为区分组件的方式，而是以组件在功能上的差异来作为区分的准则。如在通篇说明书及权利要求当中所提及的“包含”或“包括”为一开放式用语，故应解释成“包含但不限定于”。说明书后续描述为实施本发明的较佳实施方式，然所述描述乃以说明书的一般原则为目的，并非用以限定本发明的范围。本发明的保护范围当视所附权利要求所界定者为准。

为便于对本发明实施例的理解，下面将结合附图以具体实施例为例做进一步的解释说明，且各个附图并不构成对本发明实施例的限定。

为了更好地理解，图1为一个基于视觉拓扑节点和惯性导航的定位方法工作流程图，如图1所示，一种基于视觉拓扑节点和惯性导航的定位方法包括以下步骤：

第一步骤S1中，读取地图以获得各个拓扑节点信息，记作N₁，N₂......N_n，拓扑节点具有以下数据结构：

N_n：{

E_mn，

NP_n，

KPT：{KPT₁，KPT₂......KPT_n}，

DES：{DES₁，DES₂......DES_n}

}，其中，E_mn表示从拓扑节点N_m到拓扑节点N_n这个方向是可以行驶到达的；NP_n表示拓扑节点所在世界坐标系的坐标；KPT_k表示第k个视觉特征点的特征信息；DES_k表示第k个视觉特征点的描述符；

第二步骤S2中，基于拓扑路径规划得到驶向最终目标点的全局拓扑路径；

第三步骤S3中，当前机器人所在位置为P_t，当前在所述全局拓扑路径中由拓扑节点N_n驶向拓扑节点N_m的过程中，在全局路径规划下基于机器人所在环境进行局部路径规划，其中，基于惯性导航得到机器人定位信息，基于视觉定位方法更新机器人定位信息，基于机器人定位信息和障碍物信息局部路径规划得到机器人控制信息；

第四步骤S4中，机器人接受并执行所述控制信息，执行结束后进行状态判断，当前的机器人定位信息与当前目标拓扑节点N_m进行比较，若判断未达到目标拓扑节点，继续进行局部路径规划和控制；若由当前的定位信息判断机器人目前已到达拓扑节点，调用视觉定位，利用视觉定位结果强制更新机器人定位，修正在拓扑节点N_n到N_m运行过程中产生的漂移，完成定位更新后，判断当前是否已达最终目标点，若未达到，回到局部路径规划，继续产生规划路径进行控制；若判断达到最终目标点，定位结束。

在描述步骤前，首先介绍一下本方法中对地图描述的需求，一般的拓扑地图在拓扑节点上仅记录拓扑节点的位姿信息三维位置信息及三维转向角信息共六个自由度，本方法需要在拓扑节点上附加拓扑节点所在场景的视觉信息，主要包括：所在环境的视觉特征点，特征点的描述符。为了进一步理解本发明，在一个实施例中，定位方法包括，

第一步，系统将从存储器中读取地图，读取得到各个拓扑节点信息，记作N₁，N₂......N_n，拓扑节点具有以下数据结构：

N_n：{

E_mn，

NP_n，

KPT：{KPT₁，KPT₂......KPT_n}，

DES：{DES₁，DES₂......DES_n}

}

E_mn——表示从拓扑节点N_m到拓扑节点N_n这个方向是可以行驶到达的；

NP_n——表示拓扑节点所在世界坐标系的坐标；

KPT_k——表示第k个视觉特征点的特征信息；

DES_k——表示第k个视觉特征点的描述符。

特殊的，将机器人启动时的位置记为本方法的世界坐标系原点，记作P₀，机器人在任意时刻t的位置记作P_t，地图加载完成后，第一步完成。

第二步，载入全局地图后，使用改进的拓扑路径规划算法，如：Dijkstra。得到驶向最终目标点的一条拓扑路径。

第三步，假定当前机器人所在位置为P_t，当前正在第二步输出的拓扑路径中由拓扑节点N_n驶向拓扑节点N_m的过程中，此时需要在全局路径规划下基于移动机器人所在环境进行局部路径规划。进行局部定位需要传感器提供机器人定位信息和环境障碍信息，下面分别进行描述：

1机器人定位：机器人定位信息主要有两个来源：①机器人机载惯性测量单元，②拓扑节点附近的视觉精确定位。机载惯性测量单元IMU能够测量能在某个固定频率f下，对机器人的瞬时加速度进行采样，并对测量值进行积分。

对IMU测量结果积分后得到机器人相对于出发原点P₀的相对位移ΔP，则在任意时刻，由IMU估计得到的机器人在世界坐标系下的位置为：

P_t＝P₀+ΔP

惯性测量单元在机器人运行过程中一直保持运行状态，其能一直提供一个定位估计；

拓扑节点附近的视觉定位，主要基于计算机视觉中的特征点匹配的方法，首先程序读入当前机载摄像头采集的图像信息，进行提取特征点的操作，得到当前时刻机器人所在位置的特征点信息KPT_t：{KPT_t1，KPT_t2......KPT_tn}，然后针对每一个特征点提取特征描述符，得到当前时刻描述符信息DES_t：{DES_t1，DES_t2......DES_tn}，将当前时刻的特征信息与拓扑节点信息进行比较和匹配，得到当前画面和拓扑节点的匹配点对，再根据不同视角下的匹配点对的对极几何约束，计算得到当前机器人所在位置与拓扑节点的精确的相对位置关系，记作ΔP_v。

在本发明的定位方式中，视觉定位方法仅在惯导定位判断到达拓扑节点位置时才运行，视觉定位运行相对频率较低，考虑到由于惯性测量单元自身存在测量误差，进行积分时存在量化误差，基于IMU的定位会随着时间产生一定的漂移，而视觉定位方法相对而言具有更高的准确度，故在系统调用视觉定位方法并得到正确输出后，基于视觉定位方法更新机器人定位信息：

P_t＝NP_n+ΔP_v

2环境障碍分析：本方法采用基于神经网络的语义分割方法进行环境障碍分析，如ERFNET，本网络使用道路分割数据集进行训练，训练完成后对网络输入机载摄像头采集的图像，即可输出图像中的可行使道路，根据图像和道路信息，建立机器人所在环境的二维栅格局部地图，此局部地图包含了环境障碍信息。

在获得机器人定位和障碍物信息带有障碍物信息的局部二维栅格地图后，使用局部路径规划，如DWA算法进行局部路径规划，计算得到这个周期的机器人控制信息，第三步结束。

第四步，机器人接受第三步输出的控制信息并执行，执行结束后进行状态判断，本系统会读入目前的机器人定位信息，与当前目标拓扑节点N_m进行比较，若判断未达到目标拓扑节点，继续进行局部路径规划和控制；若由当前的定位信息判断机器人目前已到达拓扑节点，系统将调用视觉定位，利用视觉定位结果强制更新机器人定位，修正在拓扑节点N_n到N_m运行过程中IMU产生的漂移，完成定位更新后，系统将判断目前是否已达最终目标点，若未达到，回到局部路径规划，继续产生规划路径进行控制；若判断达到最终目标点，导航任务结束，本发明运行结束。

本方法是对传统基于惯性定位导航技术的扩充，在惯性导航中加入视觉信息，对于移动机器人来说，仅需增加摄像头模块，即可达到避障和提高运行精确度的功能。从理论上分析，目前的道路分割网络已经能够取得较好的分割结果，且能够在计算能力较低的嵌入式平台如Nvidia TX2上实现帧率超过10的运算水平，此帧率完全满足机器人局部地图规划的需要频率；本方法使用但不一直使用基于特征点的视觉定位方法，仅在达到拓扑节点时计算一次，且因为记录了目标时间戳信息，在设备计算完成后，将计算结果更新到该时间戳以后的位置信息，就能完成精确定位，故而其没有实时性的需求，几乎不增加设备的运算压力。

所述的方法的优选实施方式中，拓扑地图的拓扑节点信息包括拓扑节点的位姿信息和视觉信息。

所述的方法的优选实施方式中，所述位姿信息包括三维位置信息及三维转向角信息，所述视觉信息包括视觉的特征点及所述特征点的描述符。

所述的方法的优选实施方式中，第一步骤S1中，读取地图时，将机器人启动时的位置作为世界坐标系原点，记作P₀。

所述的方法的优选实施方式中，第二步骤S2中，采用全局规划算法，如Dijkstra得到驶向最终目标点的全局拓扑路径。

所述的方法的优选实施方式中，第三步骤S3中，基于惯性导航得到机器人定位信息时，机载惯性测量单元IMU在预定固定频率f下采样机器人的瞬时加速度并对测量值积分得到机器人相对于出发原点P₀的相对位移ΔP，得到的机器人在世界坐标系下的位置P_t＝P₀+ΔP。

所述的方法的优选实施方式中，第三步骤S3中，基于视觉定位方法更新机器人定位信息时，读入当前机载摄像头采集的图像信息以提取特征点，得到当前时刻机器人所在位置的特征点信息KPT_t：{KPT_t1，KPT_t2......KPT_tn}，针对每一个特征点提取描述符得到当前时刻描述符信息DES_t：{DES_t1，DES_t2......DES_tn}，比较和匹配当前时刻的特征信息与拓扑节点信息得到当前画面和拓扑节点的匹配点对，根据不同视角下的匹配点对的对极几何约束，计算得到当前机器人所在位置与拓扑节点的精确的相对位置关系。

所述的方法的优选实施方式中，第三步骤S3中，采用基于神经网络的语义分割方法环境障碍分析，使用道路分割数据集训练，训练完成后对网络输入机载摄像头采集的图像输出图像中的可行使道路，根据图像和道路信息，建立机器人所在环境的带有障碍物信息的二维栅格局部地图。

所述的方法的优选实施方式中，第三步骤S3中，基于如ERFNET语义分割算法环境障碍分析得到障碍物信息。

所述的方法的优选实施方式中，第三步骤S3中，基于机器人定位信息和障碍物信息采用局部路径规划算法，如DWA算法进行局部路径规划。

尽管以上结合附图对本发明的实施方案进行了描述，但本发明并不局限于上述的具体实施方案和应用领域，上述的具体实施方案仅仅是示意性的、指导性的，而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明权利要求所保护的范围的情况下，还可以做出很多种的形式，这些均属于本发明保护之列。

Claims (10)

1.一种基于视觉拓扑节点和惯性导航的定位方法，所述方法包括以下步骤：

第一步骤(S1)中，读取地图以获得各个拓扑节点信息，记作N₁，N₂……N_n，拓扑节点具有以下数据结构：

N_n：{

E_mn，

NP_n，

KPT：{KPT₁，KPT₂……KPT_n}，

DES：{DES₁，DES₂……DES_n}

}，其中，E_mn表示从拓扑节点N_m到拓扑节点N_n这个方向是可以行驶到达的；NP_n表示拓扑节点所在世界坐标系的坐标；KPT_k表示第k个视觉特征点的特征信息，每个特征点信息由两个数字表示，(m，n)，其表示图片中第m行n列对应的像素点为特征点；DES_k表示第k个视觉特征点的描述符，

第二步骤(S2)中，基于拓扑路径规划得到驶向最终目标点的全局拓扑路径；

第三步骤(S3)中，当前机器人所在位置为P_t，当前在所述全局拓扑路径中由拓扑节点N_n驶向拓扑节点N_m的过程中，在全局路径规划下基于机器人所在环境进行局部路径规划，其中，基于惯性导航得到机器人定位信息，基于视觉定位方法更新机器人定位信息，基于机器人定位信息和障碍物信息局部路径规划得到机器人控制信息；

第四步骤(S4)中，机器人接受并执行所述控制信息，执行结束后进行状态判断，当前的机器人定位信息与当前目标拓扑节点N_m进行比较，若判断未达到目标拓扑节点，继续进行局部路径规划和控制；若由当前的定位信息判断机器人目前已到达拓扑节点，调用视觉定位，利用视觉定位结果强制更新机器人定位，修正在拓扑节点N_n到N_m运行过程中产生的漂移，完成定位更新后，判断当前是否已达最终目标点，若未达到，回到局部路径规划，继续产生规划路径进行控制；若判断达到最终目标点，定位结束。

2.根据权利要求1所述的方法，其中，拓扑地图的拓扑节点信息包括拓扑节点的位姿信息和视觉信息。

3.根据权利要求2所述的方法，其中，所述位姿信息包括三维位置信息及三维转向角信息，所述视觉信息包括视觉的特征点及所述特征点的描述符。

4.根据权利要求1所述的方法，其中，第一步骤(S1)中，读取地图时，将机器人启动时的位置作为世界坐标系原点，记作P₀。

5.根据权利要求1所述的方法，其中，第二步骤(S2)中，基于路径规划算法如：Dijkstra得到驶向最终目标点的全局拓扑路径。

6.根据权利要求1所述的方法，其中，第三步骤(S3)中，基于惯性导航得到机器人定位信息时，机载惯性测量单元IMU在预定固定频率f下采样机器人的瞬时加速度并对测量值积分得到机器人相对于出发原点P₀的相对位移ΔP，得到的机器人在世界坐标系下的位置P_t＝P₀+ΔP。

7.根据权利要求1所述的方法，第三步骤(S3)中，基于视觉定位方法更新机器人定位信息时，读入当前机载摄像头采集的图像信息以提取特征点，得到当前时刻机器人所在位置的特征点信息KPT_t：{KPT_t1，KPT_t2……KPT_tn}，针对每一个特征点提取描述符得到当前时刻描述符信息DES_t：{DES_t1，DES_t2……DES_tn}，比较和匹配当前时刻的特征信息与拓扑节点信息得到当前画面和拓扑节点的匹配点对，根据不同视角下的匹配点对的对极几何约束，计算得到当前机器人所在位置与拓扑节点的精确的相对位置关系。

8.根据权利要求1所述的方法，第三步骤(S3)中，采用基于神经网络的语义分割方法环境障碍分析，使用道路分割数据集训练，训练完成后对网络输入机载摄像头采集的图像输出图像中的可行使道路，根据图像和道路信息，建立机器人所在环境的带有障碍物信息的二维栅格局部地图。

9.根据权利要求1所述的方法，其中，第三步骤(S3)中，采用基于ERFNET语义分割算法环境障碍分析得到障碍物信息。

10.根据权利要求1所述的方法，其中，第三步骤(S3)中，基于机器人定位信息和障碍物信息采用局部路径规划算法，DWA进行局部路径规划。

Images