一种基于局部最优卷积评价的机器人路径规划方法

摘要

本发明公开了一种基于局部最优卷积评价的机器人路径规划方法，包括：移动机器人建立工作环境的二维网格化地图，确定出发点与目标点位置；移动机器人感知当前位置周围的环境信息，从候选移动方向中筛选出可行移动方向；计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影，获取局部最优移动方向；利用卷积评价指标检验局部最优移动方向的合理性；在初次路径规划未能找到可行路径时，进行二次路径规划，并取消对局部最优移动方向合理性的检验环节。本发明能够实现移动机器人在未知环境下的路径规划，决策思路符合移动机器人的实际运行特点，具有运算量小、遍历性强、运行效率高的优点。

说明书

技术领域

本发明属于机器人学及导航学的技术领域，尤其涉及一种基于局部最优卷积评价的机器人路径规划方法。

背景技术

移动机器人的路径规划问题是机器人技术在消防救灾、医疗服务、工业巡检等众多领域投入应用所必须攻破的难点。目前广泛应用于移动机器人路径规划的方法主要有A*算法、蚁群算法、人工势场法等。A*算法建立在机器人拥有全局环境地图的基础上，如果直接应用于未知环境中，将因规划过程中路径的跳变使规划效率变得极低；蚁群算法需要生成大量虚拟个体对地图进行探索，一定程度上仅适合在实验仿真中使用，难以具备实际操作性；人工势场法存在易陷入局部平衡点的问题，尽管目前针对人工势场法存在众多优化方案，但在复杂大环境下的路径规划鲁棒性仍然存在缺陷。

经过对上述及其它路径规划方法的研究，发现深度优先搜索算法从行为上符合移动机器人在未知环境中的工作特点，但传统深度优先搜索算法不具备启发性，且仅能进行四个方向的移动，遍历性强但效率低，效果也不理想。

所以，需要一个新的技术方案来解决这个问题。

发明内容

发明目的：为了克服现有技术中存在的不足，提供一种基于局部最优卷积评价的机器人路径规划方法，以解决现有方法对移动机器人在未知环境中的路径规划运行效率低、鲁棒性差、难以投入实际应用的问题。

技术方案：为实现上述目的，本发明提供一种基于局部最优卷积评价的机器人路径规划方法，包括如下步骤：

S1：建立移动机器人工作环境的二维网格化地图，确定出发点与目标点位置；

S2：移动机器人感知相邻节点的环境信息，并从8个候选移动方向中筛选出可行移动方向；

S3：若存在可行移动方向，则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值，选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向作为局部最优移动方向；若不存在可行移动方向，则移动机器人认定是否需要返回父节点；

S4：通过局部最优卷积评价方法判断局部最优移动方向是否具备合理性，若具备合理性，移动机器人向局部最优移动方向移动至相邻节点；若不具备合理性，移动机器人认定需要返回父节点；

S5：当移动机器人认定需要返回父节点时，若当前节点不是出发点，即当前节点存在父节点，则移动机器人正常返回父节点，并称机器人进行了一次“后退”操作。；若当前节点是出发点，即当前节点不存在父节点，如果当前阶段为初次路径规划，则初次路径规划结束，进行二次路径规划；如果当前阶段为二次路径规划，则表明不存在可行路径，路径规划失败；

S6：路径规划开始时移动机器人处于初次路径规划阶段。移动机器人在进行初次路径规划时，重复上述步骤S2至步骤S4，直至到达目标点或进入二次路径规划阶段；移动机器人在进行二次路径规划时，重复上述步骤S2至步骤S3，并且不检验局部最优移动方向的合理性，直接向局部最优移动方向进行移动，直至到达目标点或路径规划失败。

进一步地，所述步骤S1具体为：

A1：建立移动机器人工作环境的二维网格化地图，地图中的每一网格为地图的一个节点，节点形状为正方形，取正右方为横轴正方向，正上方为纵轴正方向；

A2：为各节点添加位置属性Location＝(m，n)，m为节点横坐标，n为节点纵坐标；由于每个节点与它的位置属性一一对应，因此若节点N的位置属性为(m

A3：为各节点添加类型属性Type，节点N的相邻节点定义为与节点N存在边接触或顶点接触的节点，则每个节点共有8个相邻节点；类型属性用于在移动机器人进行决策时，判断8个相邻节点对应的移动方向是否为可行移动方向，类型属性Type的取值范围为{Unknown，Unvisited，Visited，Obstacle}，其中Unknown代表节点尚未被探索过，Unvisited代表节点不存在障碍物且未到达过，Visited代表节点不存在障碍物且已到达过，Obstacle代表节点存在障碍物；初始时刻出发点的类型属性为Visited，地图内部所有其他节点的类型属性均为Unknown，地图边界的所有节点的类型属性均为Obstacle；

A4：为各节点添加父节点属性Father；节点N的父节点定义为机器人在工作过程中通过“前进”操作进入节点N所处的上一个节点，父节点属性用于记录节点的父节点，父节点属性Father的取值范围为{0，1，2，3，4，5，6，7，8}，其中0代表节点不存在父节点，1、2、3、4、5、6、7、8分别代表父节点为正右方、右上方、正上方、左上方、正左方、左下方、正下方、右下方的相邻节点，初始时刻所有节点的父节点属性均为0。

进一步地，所述步骤S2中候选移动方向的数量为8个，可行移动方向的帅选包括：

B1：设当前节点为节点Now，取横轴正方向的单位向量为

式中，i∈{1，2，3，4，5，6，7，8}，当前节点Now共有8个相邻节点，且每个候选移动方向向量分别指向1个相邻节点的中心，因此候选移动方向向量与相邻节点一一对应；

B2：记当前节点Now在候选移动方向向量

B3：机器人对当前节点Now的8个相邻节点中类型属性为Unknown的所有节点N'，根据感知到的环境信息将节点N'的类型属性N'.Type赋为Unvisited或Obstacle；

B4：机器人根据以下规则，计算可行性判断函数Feasible(i)的值：

当i为奇数时，若满足

N

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

当i为偶数时，若同时满足以下三个条件

N

N

N

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

B5：在当前节点Now处，从集合{1，2，3，4，5，6，7，8}中选取能使

Feasible(o)＝True

的元素o，构成集合D，将o带入步骤B1中式1计算得到的

进一步地，所述步骤S3具体为：若存在可行移动方向，则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值，选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向为局部最优移动方向；若不存在可行移动方向，则移动机器人认定是否需要返回父节点。

进一步地，所述步骤S3具体包括如下步骤：

C1：若集合D为空集，则移动机器人认定需要返回父节点；

C2：若集合D为非空集，设当前节点Now的位置属性Now.Location为(x

Now.Location＝(x

定义目标点距离函数Distance为

目标点距离函数Distance的值即为当前节点Now与目标点之间的距离；

C3：根据以下公式(3)，计算目标点距离函数的梯度ΔDistance:

由于机器人在当前节点Now处的移动方向(dx

其中，目标点距离函数负梯度方向矢量

C4：机器人在当前节点Now处，对于集合D中的每个元素o，根据以下公式(5)计算机器人在

C5：机器人在当前节点Now处，对于集合D中的每个元素o，分别计算相应的单位移动得分Score(o)后，取使Score(o)最大的元素o，记为I

进一步地，所述步骤S4具体为：

D1：定义合理性阈值E

D2：当机器人在当前节点Now处计算出局部最优单位移动得分S

E

式中K

K

式中e为自然常数；p为评价聚焦度，是一个可调参数，取值范围为全体正实数；

D3：若预期卷积评价指标E

E

则认定局部最优移动方向不具备合理性，机器人认定需要返回父节点；

D4：若预期卷积评价指标E

E

则认定局部最优移动方向具备合理性，机器人移动至当前节点Now在

进一步地，所述步骤C1中移动机器人认定需要返回父节点的方法为：

定义有效移动步数Step，初始时刻Step＝0；定义有效移动得分函数s(x)，初始时刻对全体负实数x有s(x)＝1、对全体非负实数x有s(x)＝0；定义预期移动得分函数s

s

s

其中R为实数集；

记当前节点Now的父节点属性Now.Father为F，即

F＝Now.Father

机器人在认定需要返回父节点时，若当前节点Now不为出发点，即

F≠0

则机器人由当前节点Now返回父节点，父节点为当前节点Now在

E1：将有效移动得分函数s(x)在x∈[Step-1，Step)上的函数值赋为0，即

s(x)←0，x∈[Step-1，Step)

E2：将有效移动步数Step自减1，即

Step←Step-1

E3：设父节点N

N

计算父节点处的目标点距离函数负梯度方向矢量

并根据以下公式(9)为卷积评价指标E赋值：

式中

E4：将当前节点Now赋为父节点N

Now←N

机器人在认定需要返回父节点时，若当前节点Now为出发点，即

F＝0

如果当前阶段为初次路径规划，则结束初次路径规划，所有数据恢复至初始时刻，并进行二次路径规划性；如果当前阶段为二次路径规划，则表明没有可行路径，路径规划失败。

进一步地，所述步骤D4具体为：

机器人由当前节点Now移动至

F1：将下一节点的类型属性

F2：将下一节点的父节点属性

mod表示前者对后者进行取模运算；

F3：将有效移动步数Step自增1，即

Step←Step+1

F4：将卷积评价指标E赋为预期卷积评价指标E

E←E

F5：将有效移动得分函数s(x)的函数值赋为预期移动得分函数s

s(x)←s

F6：将当前节点Now赋为下一节点

进一步地，所述步骤D2具体为：

根据以下公式(11)定义评价权重函数k(x)，其中p为评价聚焦度

卷积评价指标E的定义式为：

E＝(k(x)*s(x))|

预期卷积评价指标E

E

即卷积评价指标E是评价权重函数k(x)与有效移动得分函数s(x)的卷积在Step处的值，预期卷积评价指标E

又由于

因此

由此得到步骤D2中的迭代计算式即式(6)，因此移动机器人在实际工作中无需根据定义式记录有效移动得分函数s(x)、预期移动得分函数s

卷积评价指标E有以下特点：

①值域为(-1，1]；

②机器人进行的每一次有效移动的局部最优单位移动得分都会对E产生影响，且越早的有效移动影响权重越小，越晚的有效移动影响权重越大；

③当机器人进行的每一次有效移动的局部最优单位移动得分均为1时，有

E＝1

④评价聚焦度p越大，早期有效移动的影响权重越小，晚期有效移动的影响权重越大，即机器人更多地聚焦于最近几次有效移动的局部最优单位移动得分。

预期卷积评价指标E

采用局部最优卷积评价方法检验局部最优移动方向的合理性的意义在于，由于当某一步前期的决策中可行移动方向较少且均背离目标点方向时，会导致得到的局部最优单位移动得分极低，将机器人导向歧途，从而导致后续路径规划效率降低，因此需要对后期局部最优单位移动得分施加更大的权重进行考虑，并适时选择返回。

采用此方法检验局部最优移动方向的合理性的缺点在于当可行路径较少，且均存在部分路段局部最优单位移动得分极低时，机器人会在半途时将所有可行路径均舍弃，因此需要进行二次路径规划并取消局部最优卷积评价环节，对全局地图进行遍历式的探索。

本发明在深度优先搜索算法的基础上，引入梯度下降方法使其具备启发性，并将移动方向拓展至八个方向，同时创造了局部最优卷积评价方法使移动机器人能够自行判断路径合理性并主动纠正，从而提出了一种基于局部最优卷积评价的机器人路径规划方法。本发明具有运算量小、遍历性强、运行效率高的优点，对实现移动机器人在未知环境中的导航功能具有较好的理论价值及意义。

有益效果：本发明与现有技术相比，在深度优先搜索算法的基础上，引入梯度下降方法使其具备启发性，并将移动方向拓展至八个方向，同时创造了局部最优卷积评价方法使移动机器人能够自行判断路径合理性并主动纠正；在未知环境中，用目标点距离函数负梯度方向向量做启发式引导，按照深度优先的探索规则提高遍历性，同时创造了局部最优卷积评价方法避免机器人因前期的局部最优而陷入后期歧途，并且对传感器要求低，能适应极其狭窄的感知范围，极大地提高了移动机器人在未知环境中进行路径规划的效率、鲁棒性与可操作性，对于移动机器人在消防救灾、医疗服务、工业巡检等众多领域的实际应用具有很好的工程价值及意义。

附图说明

图1是本发明实施例提供的一种基于局部最优卷积评价的机器人路径规划方法的工作流程示意图；

图2是本发明实施例提供的全局地图；

图3是本发明实施例提供的机器人视角地图；

图4是本发明实施例提供的移动机器人工作过程中单步决策结果示意图；

图5是本发明实施例提供的单步决策过程中所生成的所有候选移动方向示意图；

图6是本发明实施例提供的单步决策过程中所生成的可行移动方向示意图；

图7是本发明实施例提供的单步决策过程中目标点距离函数负梯度方向向量示意图；

图8是本发明实施例提供的机器人在包含卷积评价阶段情况下完成路径规划的最终结果图；

图9是本发明实施例提供的机器人在不包含卷积评价阶段情况下完成路径规划的最终结果图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

本发明提供一种基于局部最优卷积评价的机器人路径规划方法，如图1所示，包括以下步骤：

步骤1：建立移动机器人工作环境的二维网格化地图，确定出发点与目标点位置。

步骤2：移动机器人感知相邻节点的环境信息，并从8个候选移动方向中筛选出可行移动方向。

步骤3：若存在可行移动方向，则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值，选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向为局部最优移动方向；若不存在可行移动方向，则移动机器人认定需要返回父节点。

步骤4：通过局部最优卷积评价方法判断局部最优移动方向是否具备合理性，具体实施方法为：计算局部最优移动方向的预期卷积评价指标，再与合理性阈值进行比较，若预期卷积评价指标大于或等于合理性阈值，则认定局部最优移动方向具备合理性，移动机器人向局部最优移动方向移动至相邻节点，并称机器人进行了一次“前进”操作；若预期卷积评价指标小于合理性阈值，则认定局部最优移动方向不具备合理性，移动机器人认定需要返回父节点。

步骤5：当移动机器人认定需要返回父节点时，若当前节点不是出发点，即当前节点存在父节点，则移动机器人正常返回父节点，并称机器人进行了一次“后退”操作。若当前节点是出发点，即当前节点不存在父节点，如果当前阶段为初次路径规划，则初次路径规划结束，进行二次路径规划；如果当前阶段为二次路径规划，则表明不存在可行路径，路径规划失败。

步骤6：路径规划开始时移动机器人处于初次路径规划阶段。移动机器人在进行初次路径规划时，重复上述步骤2至步骤4，直至到达目标点或进入二次路径规划阶段；移动机器人在进行二次路径规划时，重复上述步骤2至步骤3，并且不检验局部最优移动方向的合理性，直接向局部最优移动方向进行移动，直至到达目标点或路径规划失败。

基于上述技术方案，本实施例将该基于局部最优卷积评价的机器人路径规划方法进行实例应用，本实施例中使用的IDE为Visual Studio 2017，基于C++语言进行仿真，生成102*102的网格化地图(地图最外侧网格为边界)，在地图内部随机生成密度为30％的障碍物，设定右上角节点为出发点，左下角节点为目标点，并使用openCV库将地图可视化。本方法可应用于消防救灾、医疗服务、工业巡检等众多领域，结合附图2～9，以下对具体的过程进行说明：

在本发明实施例中，随机生成的全局地图如图2所示，移动机器人将在该全局地图中完成路径规划工作，机器人感知地图如图3所示，其中地图内部右上角节点为出发点，地图内部左下角节点为目标点，浅灰色节点为未探索过的节点，黑色节点为存在障碍物的节点，白色节点为无障碍物且未到达过的节点，深灰色节点为无障碍物且已到达过的节点。

步骤1包括：

步骤1.1：建立移动机器人工作环境的二维网格化地图，地图中的每一网格为地图的一个节点，节点形状为正方形，取正右方为横轴正方向，正上方为纵轴正方向。

步骤1.2：为各节点添加位置属性Location＝(m，n)，m为节点横坐标，n为节点纵坐标。由于每个节点与它的位置属性一一对应，因此若节点N的位置属性为(m

步骤1.3∶为各节点添加类型属性Type。节点N的相邻节点定义为与节点N存在边接触或顶点接触的节点，则每个节点共有8个相邻节点。类型属性用于在移动机器人进行决策时，判断8个相邻节点对应的移动方向是否为可行移动方向。类型属性Type的取值范围为{Unknown，Unvisited，Visited，Obstacle}，其中Unknown代表节点尚未被探索过，Unvisited代表节点不存在障碍物且未到达过，Visited代表节点不存在障碍物且已到达过，Obstacle代表节点存在障碍物。初始时刻出发点的类型属性为Visited，地图内部所有其他节点的类型属性均为Unknown，地图边界的所有节点的类型属性均为Obstacle。

步骤1.4：为各节点添加父节点属性Father。节点N的父节点定义为机器人在工作过程中通过“前进”操作进入节点N所处的上一个节点。父节点属性用于记录节点的父节点，父节点属性Father的取值范围为{0，1，2，3，4，5，6，7，8}，其中0代表节点不存在父节点，1、2、3、4、5、6、7、8分别代表父节点为正右方、右上方、正上方、左上方、正左方、左下方、正下方、右下方的相邻节点，初始时刻所有节点的父节点属性均为0。

参照图4箭头所示的决策过程中，步骤2，包括：

步骤2.1：当前节点为节点(4，9)(即箭头起点所在的深灰色节点)，取横轴正方向的单位向量为

式中i∈{1，2，3，4，5，6，7，8}。当前节点Now共有8个相邻节点，且每个候选移动方向向量分别指向1个相邻节点的中心，因此候选移动方向向量与相邻节点一一对应。

步骤2.2：记当前节点Now在候选移动方向向量

步骤2.3：机器人对当前节点Now的8个相邻节点中类型属性为Unknown的所有节点N'，根据感知到的环境信息将节点N'的类型属性N'.Type赋为Unvisited或Obstacle。

步骤2.4：机器人根据以下规则，计算可行性判断函数Feasible(i)的值：

(1)当i为奇数时，若满足

N

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

(2)当i为偶数时，若同时满足以下三个条件

N

N

N

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

步骤2.5：在当前节点Now处，从集合{1，2，3，4，5，6，7，8}中选取能使

Feasible(o)＝True

的元素o，构成集合D，将o带入步骤2.1中式1计算得到的

参照图4箭头所示的决策过程中，步骤3包括：

步骤3.1：由于不满足集合D为空集，故移动机器人无需返回父节点；

步骤3.2：由于满足集合D非空集，设当前节点Now的位置属性Now.Location为(x

Now.Location＝(x

定义目标点距离函数Distance为

目标点距离函数Distance的值即为当前节点Now与目标点之间的距离。

步骤3.3：根据以下公式，计算目标点距离函数的梯度

由于机器人在当前节点Now处的移动方向(dx

目标点距离函数负梯度方向矢量

如图7中箭头所示。

步骤3.4：机器人在当前节点Now处，对于集合D中的每个元素o，根据以下公式计算机器人在

步骤3.5：机器人在当前节点Now处，对于集合D中的每个元素o，分别计算相应的单位移动得分Score(o)后，取使Score(o)最大的元素o，记为I

对于本实施例的决策过程，有

I

S

参照图4箭头所示的决策过程中，步骤4包括：

步骤4.1：对于本实施例的决策过程，合理性阈值E

步骤4.2：当机器人在当前节点Now处计算出局部最优单位移动得分S

E

式中，K

K

式中，e为自然常数；p为评价聚焦度，是一个可调参数，取值范围为全体正实数。

对于本实施例的决策过程，评价聚焦度p设置为0.1，则

K

E

步骤4.3：由于不满足预期卷积评价指标小于合理性阈值，即

E

不成立，故机器人不认定需要返回父节点。

步骤4.4：由于满足预期卷积评价指标大于或等于合理性阈值，即

E

成立，故认定该局部最优移动方向具备合理性，机器人移动至

重复上述过程，最终机器人到达目标点。

通过上述技术方案的实施，本发明的优点是：(1)提供了移动机器人在未知环境下的路径规划方法；(2)对机器人感知范围要求低，降低机器人传感器所需成本；(3)采用二维网格化地图，数据量小，运算速度快，降低机器人处理器所需成本；(4)创造了局部最优卷积评价方法，提高了移动机器人进行路径规划的总体效率；(5)决策流程符合机器人实际运行状态，不仅可实现计算机仿真，且易于投入实际应用。

为展示优点(4)的具体表现，本发明还提供了与上述实施例全局地图相同但机器人决策过程中不含局部最优卷积评价方法的路径规划结果对比。图8所示为机器人决策过程包含局部最优卷积评价方法的最终路径规划结果，机器人花费的总路程为1372.2；图9所示为机器人决策过程不含局部最优卷积评价方法的最终路径规划结果，机器人花费的总路程为2155.2。由此可见，在上述实施例中，局部最优卷积评价方法有效地避免了移动机器人在地图底部错误地向右侧探索而浪费大量的路程，从而较大幅度地提高了路径规划的效率。