CN112947415A - 一种基于障碍物语义信息的室内路径规划方法 - Google Patents

Abstract

本发明涉及一种基于障碍物语义信息的室内路径规划方法，具体为：获取室内三维地图，根据室内三维地图获取室内障碍物的语义信息；根据障碍物的语义信息，通过分类决策树评估获得障碍物的危险等级，根据室内三维地图和障碍物的危险等级生成危险等级三维地图；对危险等级三维地图中高度低于机器人高度的部分进行垂直投影，生成二维栅格地图；通过权重函数计算二维栅格地图中各个栅格的通行代价，生成代价地图；在二维栅格地图上选择初始栅格和结束栅格，通过评估函数在代价地图上生成一条从初始栅格至结束栅格的总通行代价最小的规划路径，机器人按照规划路径在室内行走。与现有技术相比，本发明具有准确性高、安全性好和容错率高等优点。

Description

一种基于障碍物语义信息的室内路径规划方法

技术领域

本发明涉及室内机器人全局路径规划领域，尤其是涉及一种基于障碍物语义信息的室内路径规划方法。

背景技术

移动机器人的导航能力是决定其自主能力的关键问题，是机器人领域一直以来研究的热门课题。全局路径规划是机器人导航的核心组成部分，也是影响机器人自主驾驶性能的一个重要因素，其是指在已知地图中规划出给定起始点和目标点间的最优路径。近年来，随着机器人在室内场景中的应用不断发展，规划室内机器人的最优路径也成为机器人导航领域的关键问题。与室外环境的结构化道路相比，室内场景中的机器人路径规划更加复杂，包含更多的不确定因素。这种复杂性反映在室内环境障碍物的结构和类型的多样性上，室内道路通常比室外窄，道路分布也不如室外结构化道路环境规则，且障碍物的类型也更加丰富。在不确定性方面，由于环境的复杂性和缺少GPS的辅助，机器人在室内环境中的定位能力弱于在室外环境，这使得机器人很容易偏离给定的路径，将更有可能与障碍物相撞。此外，在室内环境中，有更多无法预测的移动障碍，如人在行进时的路径通常是不规则的。在室内路径规划中，可能发生的路径偏离和大量移动障碍物的存在使路径规划存在更多不确定因素，室内机器人则需要更加频繁地改变路径以应对这种不确定性。而在路径改变的过程中更容易发生碰撞。因此，机器人室内路径规划在理论研究和实际应用中都具有重要意义。

室内机器人导航的核心是在复杂环境下规划出从起始点到目标点的无碰撞路径，避开不同类型的障碍物，定位能力较弱。但现有技术提出的室内路径规划和优化方法主要致力于使机器人与障碍物保持一定的距离，通过建立环境危险度图进行路径规划，其中危险度随着与障碍物距离的增加而减小，但这种危险度的设计通常是固定的，不同类型的障碍物发生碰撞所造成的后果有很大的差异，因此通过建立环境危险度图进行路径规划的方法对室内路径规划具有局限性，安全性差。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于障碍物语义信息的室内路径规划方法，准确性高，安全性好，容错率高。

本发明的目的可以通过以下技术方案来实现：

一种基于障碍物语义信息的室内路径规划方法，具体为：

1)获取室内三维地图，根据室内三维地图获取室内障碍物的语义信息；

2)从小到大划分若干个危险等级，根据障碍物的语义信息，通过分类决策树评估获得障碍物的危险等级，根据室内三维地图和障碍物的危险等级生成危险等级三维地图；

3)对危险等级三维地图中高度低于机器人高度的部分进行垂直投影，生成二维栅格地图；

4)通过权重函数计算二维栅格地图中各个栅格的通行代价，生成代价地图；

5)在二维栅格地图上选择初始栅格和结束栅格，通过A*算法在代价地图上生成一条从初始栅格至结束栅格的总通行代价最小的规划路径，所述的机器人按照规划路径在室内行走。

进一步地，所述的障碍物特征包括障碍物的高度、底面积与高度的比值以及障碍物离地面的高度。

进一步地，在二维栅格地图上建立坐标系，所述的A*算法采用的评估函数具体为：

F_i＝G_i+H_i

G_i表示从初始栅格起规划路径上第i个栅格的通过代价，H_i表示从第i个栅格到结束栅格的最优路径估计代价，x_i为当前栅格的x坐标，y_i为当前栅格的y坐标，x_t为结束栅格的x坐标，y_i为结束栅格的x坐标。

进一步地，所述的权重函数的计算公式为：

其中，f(d_i)为栅格的通行代价，d_i表示规划路径上从初始栅格起的第i个栅格到其最近障碍物的距离，k表示障碍物的危险等级，g(k)>1，r表示不同危险等级下障碍物影响区域的半径；

所述的r的计算公式为：

r＝β^k·w_R

其中w_R表示机器人垂直于行走方向的宽度，β和ε为常数，其中β为相邻两个危险等级的影响区域的半径之间的倍数。

进一步地，所述的障碍物的影响区域穿过墙壁时，在代价地图上去除该影响区域穿过墙壁的部分。

进一步地，步骤5)具体过程为：

501)将初始栅格作为当前栅格；

502)通过评估函数计算当前栅格周围的栅格的评估值，将当前栅格更新为评估值最小的栅格，并将前一当前栅格加入规划路径；

503)判断当前栅格是否位于结束栅格周围，若是则将当前栅格更新为结束栅格，规划结束，否则执行502)。

进一步地，采用分类回归树CART构建分类决策树，所述的分类决策树的每个节点采用基尼系数作为特征函数。

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

(1)本发明根据室内三维地图中障碍物的语义信息，通过分类决策树评估障碍物的危险等级，并将危险等级三维地图中高度低于机器人高度的部分进行垂直投影，生成二维栅格地图，通过权重函数计算二维栅格地图中各个栅格的通行代价，生成代价地图，通过A*算法在代价地图上生成一条从初始栅格至结束栅格的总通行代价最小的规划路径，本发明考虑了与不同类型的障碍物发生碰撞的危险等级，能够针对性地规避障碍物，高效地进行规划路径，障碍物危险等级越高，规划路径离该障碍物越远，准确性高，安全性好，即使机器人偏离规划路径，也能够使损失最小化，容错率高；

(2)本发明障碍物特征包括障碍物的高度、底面积与高度的比值以及障碍物离地面的高度，障碍物的语义信息表示障碍物的具体类型，障碍物的高度越高，越容易倒下，并且其倒下后更容易遮挡前方道路，改变地图结构，甚至砸坏机器人，底面积与高度的比值越小，障碍物越不稳定，越容易倒下，对于非直接摆放在地面上的障碍物，相比于摆放在地面上的障碍物，更容易倒下，而且距离地面的高度越高，发生触碰时更容易倒下，本发明采用的障碍物特征能提高评估障碍物危险等级的准确度，规划路径的安全性和可容错性更高；

(3)本发明A*算法引入了栅格通过代价的权重函数，考虑了障碍物的语义信息，在生成规划路径时参考了二维栅格地图的通过代价，能够生成代价和最小的规划路径，规划路径的安全性更高，遵循该规划路径时，机器人能够更加远离危险的障碍物，降低了室内路径规划的复杂性和不确定性带来的碰撞风险；

(4)本发明障碍物的影响区域穿过墙壁时，在代价地图上去除该影响区域穿过墙壁的部分，规划路径的准确度高。

附图说明

图1为本发明的方法流程图；

图2为实例1的室内三维地图；

图3为实例1的语义地图；

图4为实例1的危险等级三维地图；

图5为实例1的二维栅格地图；

图6为实例1的代价地图；

图7为实例2的室内三维地图；

图8为实例2的语义地图；

图9为实例2的二维栅格地图；

图10为实例2的规划路径示意图；

图11为实例2的规划路径对比示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

一种基于障碍物语义信息的室内路径规划方法，如图1，具体为：

1)获取室内三维地图，通过预测室内三维地图中障碍物的语义信息，获得语义地图；

2)根据障碍物的语义信息，通过分类决策树评估获得障碍物的危险等级，并生成危险等级三维地图；

3)截取危险等级三维地图中低于机器人高度的部分，并进行垂直投影，生成二维栅格地图；

4)通过权重函数计算二维栅格地图中栅格的通行代价，生成代价地图；

5)在二维栅格地图上选择初始栅格和结束栅格，通过A*算法在代价地图上生成一条从初始栅格至结束栅格的总通行代价最小的规划路径。

分类决策树的构建过程具体为：

根据障碍物的语义信息和障碍物特征，采用分类回归树CART构建分类决策树；

障碍物特征包括障碍物的高度、底面积与高度的比值以及障碍物离地面的高度；

障碍物的语义信息表示障碍物的具体类型，障碍物的高度越高，障碍物越容易倒下，且倒下后更容易阻挡机器人前方道路，改变地图结构，甚至砸伤机器人，离地面的高度大于零且小于机器人高度的障碍物在发生触碰时更容易倒下，且离地面的高度越高，该障碍物越容易倒下；

分别用A₁，A₂，A₃，A₄和A₅表示障碍物的语义信息、高度、底面积与高度的比值以及距离地面的高度这5个特征的集合，

生成包括m个样本的样本集D＝{a_j，1，…a_j，i…a_j，m}，a_j，i表示第i个样本的A_j特征值，j＝1,2,3,4,5，利用样本集，采用分类回归树CART构建分类决策树，每个节点采用基尼系数作为特征函数，基尼系数用于衡量障碍物特征纯度的参数，基尼系数越小，表示该特征纯度越高；

对于包含两个以上特征值的连续特征A_j，需对其进行二元离散，具体为：连续特征A_j的m个特征值从小到大排序，取相邻两个样本值的平均数作为划分点，第i个划分点T_i＝(a_j，i+a_j，i+1)/2，分别计算以每个划分点作为二元分类点的基尼系数，选择基尼系数最小的点作为该连续特征的二元离散分类点，对于基尼系数最小的特征点a_j，t，小于a_j，t的值为类别1，该部分样本用D₁表示，大于a_j，t的值为类别2，该部分样本用D₂表示，在连续特征A_j的条件下样本集D的基尼系数表达式为：

对于分类决策树的每个节点，取基尼系数Gini(D,A_j)最小的特征A_g，t为当前节点的最优特征，对应的最优特征值a_g，t为当前节点最优特征值，建立当前节点，根据最优特征和最优特征值将样本集D划分成当前节点的左节点和右节点，对左节点和右节点递归地进行最优特征和最优特征值的选取，直至节点样本个数或者样本集基尼系数小于设定阈值，生成分类决策树。

权重函数结合指数衰减函数和高斯衰减函数的特点，采用双曲正切函数的逆反函数，该函数的衰减速度先减小后增大，且存在两条水平渐近线，以保证被障碍物占据的栅格的通过代价无穷大；

α＝log_(r+ε)δ

g(k)＝k+1

其中，f(d_i)为栅格的通行代价，d_i表示规划路径上从初始栅格起的第i个栅格到其最近障碍物的距离，k表示障碍物的危险等级，g(k)>1，障碍物的影响区域是以障碍物为圆心的圆形区域，r表示不同危险等级下障碍物影响区域的半径，r的表达式为：

r＝β^k·w_R

其中w_R表示机器人垂直于行走方向的宽度，β为相邻两个危险等级的影响区域的半径之间的倍数，ε为常数，f(r+ε)＝-∞(ε＞0)，用于确保障碍物影响区域以外的代价趋于负无穷。

因此，障碍物周围每个栅格的通过代价的权重函数为：

从权重函数可以看出：

由于障碍物类型的不同，即其危险等级不同，因此障碍物影响区域的通过代价跟与障碍物之间的间距成反比，跟障碍物的危险等级成正比；

f(0)＝+∞，障碍物到栅格的距离为0，即该栅格被该障碍物占据，该栅格的通过代价为无穷大，机器人能够完全避开该栅格；

f(1)＝g(k)，表示在栅格构成的地图中，障碍物附近栅格的通过代价是取决于障碍物危险等级；

f(r)＝1，表示位于障碍物影响半径边缘的栅格的通过代价与空白栅格相同，均为1。

步骤5)具体过程为：

501)将初始栅格作为当前栅格；

502)通过评估函数计算当前栅格周围的栅格的评估值，将当前栅格更新为评估值最小的栅格，并将前一当前栅格加入规划路径；

503)判断当前栅格是否位于结束栅格周围，若是则将当前栅格更新为结束栅格，规划结束，否则执行502)。

在代价地图的栅格化环境的八领域中，通过启发式的方式进行搜索，对状态空间中每一个搜索到的位置进行评估，得到评估值最小的位置，再从这个位置开始进行下一次的搜索和评估，直到到达结束栅格。

在二维栅格地图上建立坐标系，通过A*算法在代价地图上寻找一条从初始栅格到结束栅格的总通行代价最小的规划路径，A*算法采用的评估函数具体为：

F_i＝G_i+H_i

其中，G_i表示从初始栅格到第i个栅格的通过代价，H_i表示从第i个栅格到结束栅格的最优路径估计代价，x_i为当前栅格的x坐标，y_i为当前栅格的y坐标，x_t为结束栅格的x坐标，y_i为结束栅格的x坐标。

对于经过n个栅格的规划路径，总通行代价的计算公式为：

当障碍物被放置在墙壁上或靠近墙壁时，受障碍物的影响区域会穿过墙壁，将穿过墙壁的影响区域过滤，经过权重函数的计算以及过滤，获得用于生成规划路径的代价地图。

本实施例进行了实例1和实例2：

实例1应用本实施例提出的室内路径规划方法在一个房间内进行路径规划，图2为房间的室内三维地图，图3为房间的语义地图，图4为实例1的危险等级三维地图，危险等级分为1到6级，图5为图4中低于机器人高度部分垂直投影生成的二维栅格地图，图6为根据图5生产的代价地图，通过灰度表示障碍物周围栅格的通过代价大小；

实例2应用本实施例提出的室内路径规划方法在一个房屋内进行路径规划，图7为房屋的室内三维地图，图8为根据图7生成的语义地图，图9为房屋的代价地图，图10为代价地图的规划路径示意图，如图11，现有技术提出的代价算法将室内环境划分为走廊和房间的方法，走廊和房间的通过代价不同，规划出一条遍历代价最小的路径，本实施例获得的规划路径能够远离危险等级较高的障碍物，以避免机器人碰撞，而现有技术提出的代价算法和A*算法生成的规划路径不能偏向或远离不同类型的障碍物。

本实施例提出了一种基于障碍物语义信息的室内路径规划方法，构建语义地图，通过障碍物的语义信息识别不同类型的障碍物，采用针对性的策略避让不同类型的障碍物，提高了规划路径的安全性以及可容错性，使得当机器人偏离规划路径时，能够将损失最小化。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种基于障碍物语义信息的室内路径规划方法，其特征在于，具体为：

1)获取室内三维地图，根据室内三维地图获取室内障碍物的语义信息；

2)根据障碍物的语义信息，通过分类决策树评估获得障碍物的危险等级，根据室内三维地图和障碍物的危险等级生成危险等级三维地图；

3)对危险等级三维地图中高度低于机器人高度的部分进行垂直投影，生成二维栅格地图；

4)通过权重函数计算二维栅格地图中各个栅格的通行代价，生成代价地图；

5)在二维栅格地图上选择初始栅格和结束栅格，通过A*算法在代价地图上生成一条从初始栅格至结束栅格的总通行代价最小的规划路径，所述的机器人按照规划路径在室内行走。

2.根据权利要求1所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的障碍物特征包括障碍物的高度、底面积与高度的比值以及障碍物离地面的高度。

3.根据权利要求1所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，步骤5)具体过程为：

501)将初始栅格作为当前栅格；

502)通过A*算法计算当前栅格周围的栅格的评估值，将当前栅格更新为评估值最小的栅格，并将前一当前栅格加入规划路径；

503)判断当前栅格是否位于结束栅格周围，若是则将当前栅格更新为结束栅格，规划结束，否则执行502)。

4.根据权利要求1所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的A*算法采用的评估函数具体为：

F_i＝G_i+H_i

其中，G_i表示从初始栅格起规划路径上第i个栅格的通过代价，H_i表示从第i个栅格到结束栅格的最优路径估计代价。

5.根据权利要求4所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的H_i的计算公式为：

其中，x_i为当前栅格的x坐标，y_i为当前栅格的y坐标，x_t为结束栅格的x坐标，y_i为结束栅格的x坐标。

6.根据权利要求4所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的G_i的计算公式为：

其中，x_i为当前栅格的x坐标，y_i为当前栅格的y坐标，x_t为结束栅格的x坐标，y_i为结束栅格的x坐标。

7.根据权利要求6所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的权重函数的计算公式为：

其中，f(d_i)为栅格的通行代价，d_i表示规划路径上从初始栅格起的第i个栅格到其最近障碍物的距离，k表示障碍物的危险等级，g(k)>1，r表示不同危险等级下障碍物影响区域的半径。

8.根据权利要求7所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的r的计算公式为：

r＝β^k·w_R

其中w_R表示机器人垂直于行走方向的宽度，β和ε为常数。

9.根据权利要求7所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，所述的障碍物的影响区域穿过墙壁时，在代价地图上去除该影响区域穿过墙壁的部分。

10.根据权利要求1所述的一种基于障碍物语义信息的室内路径规划方法，其特征在于，采用分类回归树CART构建分类决策树，所述的分类决策树的每个节点采用基尼系数作为特征函数。

Images