CN110986945B - 基于语义高度地图的局部导航方法和系统 - Google Patents

Abstract

本发明提供了一种基于语义高度地图的局部导航方法和系统，使用语义分割神经网络得到语义分割图像，再结合估计的位姿把图像投影至地图坐标与已有地图进行概率融合，最后对地图进行语义可通行概率计算；使用传感器的点云信息以及估计的位姿构建局部高度地图；结合语义地图的分类结果和可靠度，以及高度地图的高度差等信息，计算移动损耗；通过计算所有候选路径的总移动损耗，执行损耗最低的候选路径。本发明可以提高导航系统对更复杂场景的适应性，融合语义和高度信息进行路径规划，提高了所规划路径的可靠性和安全性。

Description

基于语义高度地图的局部导航方法和系统

技术领域

本发明涉及地面机器人局部安全导航技术领域，具体地，涉及一种基于语义高度的局部导航方法和系统。

背景技术

目前，自主导航是地面机器人基本功能之一，可通行地图是保障地面机器人安全可靠导航的必要条件。传统地面机器人主要基于高度地图进行导航。高度地图可以表示地面的三维空间信息，地面明显的凸起或凹下的区域可以在高度地图上明显表示，地面机器人可以根据地面高度变化信息躲避相应区域。但是对于一些高度区分度较低的区域(如草地中央的小道，人行道和自行车道)，高度地图几乎无法有效区分。基于神经网络的语义分割可以对图像中的物体进行像素级的识别，可以识别视野内的物体是道路、草坪等类别，可有效提高导航系统处理高度区分度较低的区域。

与本申请相关的现有技术是专利文献CN105045263B，公开了一种基于Kinect的机器人自定位方法，通过Kinect获取环境的RGB图像和深度图像，通过融合视觉和物理里程计的信息，估计机器人的相对运动量，并根据上一时刻机器人位姿，实现位姿跟踪；将深度信息转换成三维点云，在点云中提取出地面，并根据地面自动标定Kinect相对于地面的高度和俯仰角，从而将三维点云投影到地面，得到类似于激光数据的二维点云，将其与事先构建的环境栅格地图进行匹配，从而修正机器人跟踪过程中的累积误差，准确的估计机器人的位姿。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于语义高度地图的局部导航方法和系统。

根据本发明提供的一种基于语义高度地图的局部导航方法，包括以下步骤：

局部语义地图构建步骤：使用语义分割神经网络得到语义分割图像，将语义分割图像投影至地图坐标进行概率融合，对地图进行语义可通行概率计算，构建局部语义地图；

局部高度地图构建步骤：使用传感器的点云信息，构建局部高度地图；

可通行度计算步骤：结合局部语义地图的分类结果和可靠度概率，以及局部高度地图的高度差，计算移动损耗；

局部路径规划步骤：通过计算所有候选路径的移动损耗总和，将移动损耗的总和最低的候选路径作为执行路径。

优选地，所述局部语义地图构建步骤包括：

语义分割步骤：使用深度神经网络模型对获取的彩色图像进行语义分割，获得彩色图像的语义识别结果；

图像投影步骤：利用相机内参和深度图结果，把语义分割的图像投影至相机坐标系下的三维空间，获得三维语义点；

概率融合步骤：把三维语义点通过贝叶斯概率的方式融合至语义网格地图的相应位置，先根据一个三维语义点计算其所属语义地图的网格，再提取所属网格内所有语义类别的概率，根据三维语义点的语义类别，把网格内相应类别的概率乘以预设概率a(1>a>0.5)，其他类别的概率乘以预设概率b(0<b<0.5)，对相乘后所有概率进行归一化。

优选地，所述局部高度地图构建步骤包括：

三维计算步骤：根据一个传感器的三维点计算其所属高度地图的网格；

高度更新步骤：提取所属网格内的高度和方差信息，使用卡尔曼滤波更新方式，利用三维点的高度更新所属网格的高度和方差。

优选地，所述可靠度计算步骤：

地图分类步骤：根据语义地图网格内最大概率的类别生成多层二值语义地图，每层地图对应一个语义类别，二值语义地图的1值表示此网格内此类别概率最大，否则为0值；

概率推算步骤：分别对二值语义地图进行多次腐蚀操作，根据腐蚀叠加结果计算可靠概率；

损失计算步骤：结合网格语义可靠度概率、网格的语义类别和网格与周围网格的高度差计算可通行度，记为通行损失。

优选地，所述局部路径规划步骤：

路径预设步骤：预设若干从自身所处位置通向不同方向的路径；

路径执行步骤：在路径上以一定间隔采样，提取相应通行损失，计算每条路径的总损失，并结合到目的地距离以及行走距离计算路径得分，控制地面机器人执行得分最高的路径。

根据本发明提供的一种基于语义高度地图的局部导航系统，包括以下模块：

局部语义地图构建模块：使用语义分割神经网络得到语义分割图像，将语义分割图像投影至地图坐标进行概率融合，对地图进行语义可通行概率计算，构建局部语义地图；

局部高度地图构建模块：使用传感器的点云信息，构建局部高度地图；

可通行度计算模块：结合局部语义地图的分类结果和可靠度概率，以及局部高度地图的高度差，计算移动损耗；

局部路径规划模块：通过计算所有候选路径的移动损耗总和，将移动损耗的总和最低的候选路径作为执行路径。

优选地，所述局部语义地图构建模块包括：

语义分割模块：使用深度神经网络模型对获取的彩色图像进行语义分割，获得彩色图像的语义识别结果；

图像投影模块：利用相机内参和深度图结果，把语义分割的图像投影至相机坐标系下的三维空间，获得三维语义点；

概率融合模块：把三维语义点通过贝叶斯概率的方式融合至语义网格地图的相应位置，先根据一个三维语义点计算其所属语义地图的网格，再提取所属网格内所有语义类别的概率，根据三维语义点的语义类别，把网格内相应类别的概率乘以预设概率a(1>a>0.5)，其他类别的概率乘以预设概率b(0<b<0.5)，对相乘后所有概率进行归一化。

优选地，所述局部高度地图构建模块包括：

三维计算模块：根据一个传感器的三维点计算其所属高度地图的网格；

高度更新模块：提取所属网格内的高度和方差信息，使用卡尔曼滤波更新方式，利用三维点的高度更新所属网格的高度和方差。

优选地，所述可靠度计算模块：

地图分类模块：根据语义地图网格内最大概率的类别生成多层二值语义地图，每层地图对应一个语义类别，二值语义地图的1值表示此网格内此类别概率最大，否则为0值；

概率推算模块：分别对二值语义地图进行多次腐蚀操作，根据腐蚀叠加结果计算可靠概率；

损失计算模块：结合网格语义可靠度概率、网格的语义类别和网格与周围网格的高度差计算可通行度，记为通行损失。

优选地，所述局部路径规划模块：

路径预设模块：预设若干从自身所处位置通向不同方向的路径；

路径执行模块：在路径上以一定间隔采样，提取相应通行损失，计算每条路径的总损失，并结合到目的地距离以及行走距离计算路径得分，控制地面机器人执行得分最高的路径。

与现有技术相比，本发明具有如下的有益效果：

1、本发明可以提高导航系统对更复杂场景(高度区分度不明显，但是有丰富视觉特征可以明确区分物体类别的场景)的适应性；

2、本发明提供了地图的语义可靠概率，可以使地面机器人更靠近可通行区域的中央进行导航，提高导航安全性；

3、本发明提供了计算语义-高度地图的可通行性计算方法，可把语义-高度信息转变为可计量的可通行度(通行损失)，便于导航计算最优路径；

4、本发明提供了基于语义-高度地图可通行度的路径规划算法，融合语义和高度信息进行路径规划，提高了所规划路径的可靠性和安全性。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明算法流程图；

图2为本发明实施例中某帧场景截图；

图3为本发明实施例中语义分割示意图；

图4为本发明实施例中语义地图标签层示意图；

图5为本发明实施例中语义地图可靠性概率示意图；

图6为本发明实施例中某帧点云截图；

图7为本发明实施例中高度地图；

图8为本发明实施例中预设路径示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明的具体应用场景包括与地面机器人实时在具体环境细节未知的情况下进行安全导航的系统中。

如图2至图8所示，基于局部语义-高度地图的局部导航系统为本发明一可选的实施例，可以认为，本领域的技术人员在不改变本发明精神和内容的范围内能够对其进行修改和润色。

本实施例提供了局部语义-高度地图的局部导航系统，其中提供了：基于语义分割的局部语义地图的构建方法；基于激光点云的局部高度地图构建方法；基于局部语义-高度地图的可通行度计算方法；基于局部语义-高度地图的路径规划方法。

主要过程通过C++来实现，如图1所示，该方法包括如下步骤：

局部语义地图构建步骤：使用语义分割神经网络得到语义分割图像，再把图像投影至地图坐标进行概率融合，最后对地图进行语义可靠度概率计算；

局部高度地图构建步骤：使用传感器的点云信息构建局部高度地图；

可通行度计算步骤：结合语义地图的分类结果和可靠度概率，以及高度地图的高度差等信息，计算移动损耗；

局部路径规划步骤：通过计算所有候选路径的总移动损耗，执行损耗最低的候选路径。

在局部语义地图构建步骤，首先使用深度神经网络对RGB图像进行语义分割，利用相机内参矩阵把语义分割图像的语义点投影到相机坐标系的三维空间上获得三维语义点，通过机器人的估计位姿把三维语义点转移至地图坐标系下，计算每个三维语义点所处的地图网格，根据语义点的语义类别按贝叶斯概率公式更新网格内所有类别的概率。

局部高度地图构建步骤，首先通过机器人的估计位姿把三维点云转移至地图坐标系下，计算每个三维点所处的地图网格，使用卡尔曼滤波的方式更新网格的高度以及高度的方差。

可通行度计算步骤，计算相邻两个地图网格的可通行度，首先计算语义地图的可靠度，然后获取两个网格的高度值，计算网格高度差，获取网格的最大概率的类别，获取网格可靠度，最后利用预设的计算公式计算两个网格直接的可通行度。

在计算语义地图的可靠度时，首先把多层概率语义地图按最大概率提取每个网格的最可能类别，组成每个类别的二值地图，若此网格内该类别概率最大则置1，否则置0，然后对所有二值地图进行多次腐蚀，根据多次腐蚀的地图结果叠加，地图值越大可靠性越高。

局部路径规划步骤，首先从机器人位置出发预设若干路径，然后按一定间隔在路径上采样，计算两两采样点的可通行度，求的每条路径的总可通行度，再结合路径末端到目的地的距离以及路径的长度计算路径总得分，最后执行得分最高的路径。

以下将对其中内容进行进一步展开，其中只详细描述本方法提出算法的部分内容。

步骤一、局部语义地图构建

每当获得新一帧图像，如图2，本方法使用语义分割网络把RGB图像生成语义分割图像，如图3。然后，把语义分割图像的像素点投影至相机坐标系下的三维空间上：

I_i＝KP_i ^cam (1)

式中，

I_i表示图像第i个像素点的坐标；

K表示相机内参矩阵；

P_i ^cam表示第i个像素点对应的相机坐标系下的三维点坐标；

进一步利用位姿估计的结果，把三维语义点转移至地图坐标系下：

P_i ^map＝[R|t]P_i ^cam (2)

式中，

P_i ^map表示第i个像素点对应的地图坐标系系下的三维点坐标；

[R|t]表示齐次旋转平移矩阵，由位姿估计算法提供；

R表示3*3旋转矩阵；

T表示3*1平移矩阵；

此时可以获得当前观测的语义点在地图坐标下的三维位置，可以由此计算三维语义点所属的语义地图网格，对每个被观测到的网格用贝叶斯方法更新其各个类别的概率：

式中，

z表示观测，x表示状态；

p(x_t|z_1:t)表示从开始到t时刻观测下该网格各个类别的概率向量；

p(x_t-1|z_1:t-1)表示从开始到t-1时刻观测下该网格各个类别的概率向量；

p(x_t|z_t)表示在t时刻观测为z的情况下状态是x的概率；

p(x_t)表示状态为x的概率，常量；

p(z_t)表示观测到z的概率，常量；

p(z_t|z_1:t-1)表示在t-1时刻以前获得总观测的情况下t时刻观测为z的概率，常量；

表示归一化的系数；

其中，p(x_t|z_t)为预设常量，其表达式为：

式中，

c表示分割结果为此类时地图观测更新的概率常量；

N表示语义分割总类别数；

至此为语义网格地图生成过程，所得语义网格地图内每个网格保存其各个类别的概率。

步骤二、局部高度地图构建

高度地图构建使用的具体过程描述如下。

每当获得新一帧点云，如图6，利用式(2)把点云从传感器坐标系转移至地图坐标系，然后把点云转变为水平坐标和高度表示，其中高度表示包括高度均值和方差，利用卡尔曼滤波的方式更新相应高度地图的网格：

式中，

h(t)表示t时刻某网格的高度均值；

h(t-1)表示t-1时刻某网格的高度均值；

表示t时刻某网格高度方差；

表示t-1时刻某网格高度方差；

z_t表示t时刻某网格观测高度均值；

表示t时刻某网格观测高度方差；

再对高度地图网格进行3乘3大小的卷积，填补空洞：

C＝∑w_i,j

式中，

w_i,j表示在坐标i、j下的卷积权重；

h_f(x,y)表示卷积后的高度值；

最后得到高度地图如图7所示。

步骤三、可通行度计算

首先计算语义地图的可靠度，对语义地图的每个网格进行取最大值操作，生成一个语义类别地图(此地图中每个网格存储语义地图中最大概率的类别，如图4)，和N个类别对应的二值地图(每个类别的二值地图中每个网格的信息为，若语义地图对应网格最大概率类别为此类别则置1，否则置0)，对所有类别二值地图进行多次腐蚀操作，最后一次腐蚀剩下的区域可靠度为1，第一次腐蚀消失的区域可靠度置0，中间腐蚀消失的区域按线性划分可靠度，如图5所示。

此时，利用两个地图的信息(包括语义类别、区域可靠度、高度)计算相邻两个网格之间的通行度(通行损失)，计算公式为：

cost(i,j)＝c_j(2-p_r(j))(Δh(i,j)+g) (7)

式中，

i、j分别表示出发网格和到达网格；

cost(i,j)表示从i网格到j网格的通行损失；

c_j表示目的网格的类别系数；

p_r(j)表示目的网格的区域可靠度；

Δh(i,j)表示高度差；

g表示相邻网格的距离；

步骤四、局部路径规划

首先，从机器人自身出发依照机器人运动模型按一定弧度间隔计算一定距离的运动轨迹，并以一定间隔对轨迹进行采样，需要注意的是，轨迹会以高度差阈值判断是否截断，计算每条轨迹得分score(k)：

式中，

score(k)表示第k条轨迹的得分；

k表示第k条轨迹；

L(k)表示轨迹移动的距离；

α_L表示移动系数，鼓励多移动；

d(k)表示轨迹末端到终点的距离；

α_d表示轨迹末端到终点距离的得分系数；

最后，选择得分最高的轨迹，并执行对应的控制。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (8)

1.一种基于语义高度地图的局部导航方法，其特征在于，包括以下步骤：

局部语义地图构建步骤：使用语义分割神经网络得到语义分割图像，将语义分割图像投影至地图坐标进行概率融合，对地图进行语义可靠度概率计算，构建局部语义地图；

局部高度地图构建步骤：使用传感器的点云信息，构建局部高度地图；

可通行度计算步骤：结合局部语义地图的分类结果和可靠度概率，以及局部高度地图的高度差，计算移动损耗；

局部路径规划步骤：通过计算所有候选路径的移动损耗总和，将移动损耗的总和最低的候选路径作为执行路径；

所述局部语义地图构建步骤包括：

语义分割步骤：使用深度神经网络模型对获取的彩色图像进行语义分割，获得彩色图像的语义识别结果；

图像投影步骤：利用相机内参和深度图结果，把语义分割的图像投影至相机坐标系下的三维空间，获得三维语义点；

概率融合步骤：把三维语义点通过贝叶斯概率的方式融合至语义网格地图的相应位置，先根据一个三维语义点计算其所属语义地图的网格，再提取所属网格内所有语义类别的概率，根据三维语义点的语义类别，把网格内相应类别的概率乘以预设概率a，1>a>0.5，其他类别的概率乘以预设概率b，0<b<0.5，对相乘后所有概率进行归一化。

2.根据权利要求1所述的基于语义高度地图的局部导航方法，其特征在于，所述局部高度地图构建步骤包括：

三维计算步骤：根据一个传感器的三维点计算其所属高度地图的网格；

高度更新步骤：提取所属网格内的高度和方差信息，使用卡尔曼滤波更新方式，利用三维点的高度更新所属网格的高度和方差。

3.根据权利要求1所述的基于语义高度地图的局部导航方法，其特征在于，所述可通行度计算步骤：

地图分类步骤：根据语义地图网格内最大概率的类别生成多层二值语义地图，每层地图对应一个语义类别，二值语义地图的1值表示此网格内此类别概率最大，否则为0值；

概率推算步骤：分别对二值语义地图进行多次腐蚀操作，根据腐蚀叠加结果计算可靠度概率；

损失计算步骤：结合网格语义可靠度概率、网格的语义类别和网格与周围网格的高度差计算可通行度，记为通行损失。

4.根据权利要求1所述的基于语义高度地图的局部导航方法，其特征在于，所述局部路径规划步骤：

路径预设步骤：预设若干从自身所处位置通向不同方向的路径；

路径执行步骤：在路径上以一定间隔采样，提取相应通行损失，计算每条路径的总损失，并结合到目的地距离以及行走距离计算路径得分，控制地面机器人执行得分最高的路径。

5.一种基于语义高度地图的局部导航系统，其特征在于，包括以下模块：

局部语义地图构建模块：使用语义分割神经网络得到语义分割图像，将语义分割图像投影至地图坐标进行概率融合，对地图进行语义可靠度概率计算，构建局部语义地图；

局部高度地图构建模块：使用传感器的点云信息，构建局部高度地图；

可通行度计算模块：结合局部语义地图的分类结果和可靠度概率，以及局部高度地图的高度差，计算移动损耗；

局部路径规划模块：通过计算所有候选路径的移动损耗总和，将移动损耗的总和最低的候选路径作为执行路径；

所述局部语义地图构建模块包括：

语义分割模块：使用深度神经网络模型对获取的彩色图像进行语义分割，获得彩色图像的语义识别结果；

图像投影模块：利用相机内参和深度图结果，把语义分割的图像投影至相机坐标系下的三维空间，获得三维语义点；

概率融合模块：把三维语义点通过贝叶斯概率的方式融合至语义网格地图的相应位置，先根据一个三维语义点计算其所属语义地图的网格，再提取所属网格内所有语义类别的概率，根据三维语义点的语义类别，把网格内相应类别的概率乘以预设概率a，1>a>0.5，其他类别的概率乘以预设概率b，0<b<0.5，对相乘后所有概率进行归一化。

6.根据权利要求5所述的基于语义高度地图的局部导航系统，其特征在于，所述局部高度地图构建模块包括：

三维计算模块：根据一个传感器的三维点计算其所属高度地图的网格；

高度更新模块：提取所属网格内的高度和方差信息，使用卡尔曼滤波更新方式，利用三维点的高度更新所属网格的高度和方差。

7.根据权利要求5所述的基于语义高度地图的局部导航系统，其特征在于，所述可通行度计算模块：

地图分类模块：根据语义地图网格内最大概率的类别生成多层二值语义地图，每层地图对应一个语义类别，二值语义地图的1值表示此网格内此类别概率最大，否则为0值；

概率推算模块：分别对二值语义地图进行多次腐蚀操作，根据腐蚀叠加结果计算可靠度概率；

损失计算模块：结合网格语义可靠度概率、网格的语义类别和网格与周围网格的高度差计算可通行度，记为通行损失。

8.根据权利要求5所述的基于语义高度地图的局部导航系统，其特征在于，所述局部路径规划模块：

路径预设模块：预设若干从自身所处位置通向不同方向的路径；

路径执行模块：在路径上以一定间隔采样，提取相应通行损失，计算每条路径的总损失，并结合到目的地距离以及行走距离计算路径得分，控制地面机器人执行得分最高的路径。

Images