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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

A2：为各节点添加位置属性Location＝(m，n)，m为节点横坐标，n为节点纵坐标；由于每个节点与它的位置属性一一对应，因此若节点N的位置属性为(m₁，n₁)，则节点N亦可基于它的位置属性称为为节点(m₁，n₁)；m与n的取值范围为整数集，出发点的位置属性为(x₀，y₀)，目标点的位置属性为(0，0)；

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，取横轴正方向的单位向量为

向量，按逆时针方向将

向量每次旋转45°，依次得

向量，这8个向量为候选移动方向向量，它们的方向为候选移动方向，数值可根据以下公式计算：

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

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

方向上的相邻节点为N_i，当前节点Now在候选移动方向向量

顺时针旋转45°方向上的相邻节点为N_i-，当前节点Now在候选移动方向向量

逆时针旋转45°方向上的相邻节点为N_i+，它们的类型属性分别为N_i.Type、N_i-.Type、N_i+.Type；

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

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

当i为奇数时，若满足

N_i.Type＝Unvisited

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

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

N_i.Type＝Unvisited

N_i-.Type≠Obstacle

N_i+.Type≠Obstacle

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

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

Feasible(o)＝True

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

向量为当前节点Now处的可行移动方向向量，

向量的方向为当前节点Now处的可行移动方向。

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

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

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

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

Now.Location＝(x₁，y₁)

定义目标点距离函数Distance为

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

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

由于机器人在当前节点Now处的移动方向(dx₁，dy₁)与目标点距离函数的梯度

的负方向越接近，目标点距离函数减小地越快，因此根据以下公式(4)定义当前节点Now处的目标点距离函数负梯度方向矢量

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

的方向与目标点距离函数的梯度

的负方向相同；

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

向量方向上的单位移动得分Score(o)：

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

向量为局部最优移动方向向量，

向量的方向为局部最优移动方向，将I_m带入步骤C4中式(5)计算得到局部最优单位移动得分S_m，即

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

D1：定义合理性阈值E_T，E_T是一个可调参数，取值范围为-1到1之间的全体实数，不含-1和1；定义卷积评价指标E，初始时刻E＝1；

D2：当机器人在当前节点Now处计算出局部最优单位移动得分S_m后，根据以下公式(6)计算预期卷积评价指标E_p：

E_p＝(1-K_p)·E+K_p·S_m (6)

式中K_p为实时评价权重，是一个常数，根据以下公式(7)确定

K_p＝1-e^-p (7)

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

D3：若预期卷积评价指标E_p小于合理性阈值E_T，即

E_p<E_T

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

D4：若预期卷积评价指标E_p大于或等于合理性阈值，即

E_p≥E_T

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

方向上的相邻节点

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

定义有效移动步数Step，初始时刻Step＝0；定义有效移动得分函数s(x)，初始时刻对全体负实数x有s(x)＝1、对全体非负实数x有s(x)＝0；定义预期移动得分函数s_p(x)，当移动机器人在当前节点Now处计算出局部最优单位移动得分S_m后，预期移动得分函数s_p(x)的函数值为在有效移动得分函数s(x)的基础上，将x∈[Step，Step+1)的部分赋为S_m，即依次进行以下两步赋值操作：

s_p(x)←s(x)，x∈R

s_p(x)←S_m，x∈[Step，Step+1)

其中R为实数集；

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

F＝Now.Father

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

F≠0

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

向量方向上的相邻节点N_F，

向量由F带入步骤B1中式(1)计算得到，机器人由当前节点Now返回父节点N_F需进行以下操作：

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

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

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

Step←Step-1

E3：设父节点N_F的位置属性N_F.Location为(x'，y')，即

N_F.Location＝(x'，y')

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

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

式中

E4：将当前节点Now赋为父节点N_F，即

Now←N_F

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

F＝0

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

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

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

向量方向上的相邻节点

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

需进行以下操作：

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

赋为Visited，即

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

赋为[(I_m+3)mod 8]+1，即

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

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

Step←Step+1

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

E←E_p

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

s(x)←s_p(x)，x∈R

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

即

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

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

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

E＝(k(x)*s(x))|_x＝step (12)

预期卷积评价指标E_p的定义式为：

E_p＝(k(x)*s_p(x))|_x＝Step+1 (13)

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

又由于

因此

由此得到步骤D2中的迭代计算式即式(6)，因此移动机器人在实际工作中无需根据定义式记录有效移动得分函数s(x)、预期移动得分函数s_p(x)与评价权重函数k(x)，再进行卷积计算从而得到卷积评价指标E与预期卷积评价指标E_p，而仅需按照步骤D2，根据评价聚焦度p计算出实时评价权重K_p，并利用实时评价权重K_p结合局部最优单位移动得分S_m对卷积评价指标E与预期卷积评价指标E_p进行反复迭代。

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

①值域为(-1，1]；

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

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

E＝1

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

预期卷积评价指标E_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₁，n₁)，则节点N亦可基于它的位置属性称为为节点(m₁，n₁)。m与n的取值范围为整数集，出发点的位置属性为(99，99)，目标点的位置属性为(0，0)。

步骤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)(即箭头起点所在的深灰色节点)，取横轴正方向的单位向量为

向量，按逆时针方向将

向量每次旋转45°，依次得

向量，这8个向量为候选移动方向向量，如图5箭头所示，它们的方向为候选移动方向，数值可根据以下公式计算：

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

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

方向上的相邻节点为N_i，当前节点Now在候选移动方向向量

顺时针旋转45°方向上的相邻节点为N_i-，当前节点Now在候选移动方向向量

逆时针旋转45°方向上的相邻节点为N_i+，它们的类型属性分别为N_i.Type、N_i-.Type、N_i+.Type。

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

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

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

N_i.Type＝Unvisited

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

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

N_i.Type＝Unvisited

N_i-.Type≠Obstacle

N_i+.Type≠Obstacle

则

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计算得到的

向量为当前节点Now处的可行移动方向向量，

向量的方向为当前节点Now处的可行移动方向。对于本实施例的决策过程，D＝{1，5，6，7}，对应的可行移动方向如图6箭头所示。

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

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

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

Now.Location＝(x₁，y₁)

定义目标点距离函数Distance为

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

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

为

由于机器人在当前节点Now处的移动方向(dx₁，dy₁)与目标点距离函数的梯度

的负方向越接近，目标点距离函数减小地越快，因此根据以下公式定义当前节点Now处的目标点距离函数负梯度方向矢量

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

的方向与目标点距离函数的梯度

的负方向相同。对于本实施例的决策过程，有

如图7中箭头所示。

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

向量方向上的单位移动得分Score(o)：

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

向量为局部最优移动方向向量，

向量的方向为局部最优移动方向，将I_m带入步骤3.4中式5计算得到局部最优单位移动得分S_m，即

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

I_m＝6

S_m＝0.933

向量即如图4箭头所示。

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

步骤4.1：对于本实施例的决策过程，合理性阈值E_T设置为0，卷积评价指标E在实施移动前为0.721。

步骤4.2：当机器人在当前节点Now处计算出局部最优单位移动得分S_m＝0.933后，根据以下公式计算预期卷积评价指标E_p：

E_p＝(1-K_p)·E+K_p·S_m (6)

式中，K_p为实时评价权重，是一个常数，根据以下公式确定：

K_p＝1-e^-p (7)

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

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

K_p＝1-e^-p＝0.09516

E_p＝(1-K_p)·E+K_p·S_m＝0.741

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

E_p<E_T

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

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

E_p≥E_T

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

向量方向上的相邻节点N₆，即节点(3，8)，如图4箭头终点所示。

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

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

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

Claims (9)

1.一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

2.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤S1具体为：

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

A2：为各节点添加位置属性Location＝(m，n)，m为节点横坐标，n为节点纵坐标；由于每个节点与它的位置属性一一对应，因此若节点N的位置属性为(m₁，n₁)，则节点N亦可基于它的位置属性称为为节点(m₁，n₁)；m与n的取值范围为整数集，出发点的位置属性为(x₀，y₀)，目标点的位置属性为(0，0)；

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。

3.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤S2中候选移动方向的数量为8个，可行移动方向的帅选包括：

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

向量，按逆时针方向将

向量每次旋转45°，依次得

向量，这8个向量为候选移动方向向量，它们的方向为候选移动方向，数值可根据以下公式计算：

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

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

方向上的相邻节点为N_i，当前节点Now在候选移动方向向量

顺时针旋转45°方向上的相邻节点为N_i-，当前节点Now在候选移动方向向量

逆时针旋转45°方向上的相邻节点为N_i+，它们的类型属性分别为N_i.Type、N_i-.Type、N_i+.Type；

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

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

当i为奇数时，若满足

N_i.Type＝Unvisited

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

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

N_i.Type＝Unvisited

N_i-.Type≠Obstacle

N_i+.Type≠Obstacle

则

Feasible(i)＝True

否则

Feasible(i)＝Flase

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

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

向量为当前节点Now处的可行移动方向向量，

向量的方向为当前节点Now处的可行移动方向。

4.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤S3具体为：若存在可行移动方向，则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值，选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向为局部最优移动方向；若不存在可行移动方向，则移动机器人认定是否需要返回父节点。

5.根据权利要求4所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤S3具体包括如下步骤：

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

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

Now.Location＝(x₁，y₁)

定义目标点距离函数Distance为

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

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

根据以下公式(4)定义当前节点Now处的目标点距离函数负梯度方向矢量

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

的方向与目标点距离函数的梯度

的负方向相同；

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

向量方向上的单位移动得分Score(o)：

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

向量为局部最优移动方向向量，

向量的方向为局部最优移动方向，将I_m带入步骤C4中式(5)计算得到局部最优单位移动得分S_m，即

6.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤S4具体为：

D1：定义合理性阈值E_T，E_T是一个可调参数，取值范围为-1到1之间的全体实数，不含-1和1；定义卷积评价指标E，初始时刻E＝1；

D2：当机器人在当前节点Now处计算出局部最优单位移动得分S_m后，根据以下公式(6)计算预期卷积评价指标E_p：

E_p＝(1-K_p)·E+K_p·S_m (6)

式中K_p为实时评价权重，是一个常数，根据以下公式(7)确定

K_p＝1-e^-p (7)

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

D3：若预期卷积评价指标E_p小于合理性阈值E_T，即

E_p＜E_T

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

D4：若预期卷积评价指标E_p大于或等于合理性阈值，即

E_p≥E_T

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

方向上的相邻节点

7.根据权利要求5所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤C1中移动机器人认定需要返回父节点的方法为：

定义有效移动步数Step，初始时刻Step＝0；定义有效移动得分函数s(x)，初始时刻对全体负实数x有s(x)＝1、对全体非负实数x有s(x)＝0；定义预期移动得分函数s_p(x)，当移动机器人在当前节点Now处计算出局部最优单位移动得分S_m后，预期移动得分函数s_p(x)的函数值为在有效移动得分函数s(x)的基础上，将x∈[Step，Step+1)的部分赋为S_m，即依次进行以下两步赋值操作：

s_p(x)←s(x)，x∈R

s_p(x)←S_m，x∈[Step，Step+1)

其中R为实数集；

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

F＝Now.Father

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

F≠0

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

向量方向上的相邻节点N_F，

向量由F带入步骤B1中式(1)计算得到，机器人由当前节点Now返回父节点N_F需进行以下操作：

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

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

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

Step←Step-1

E3：设父节点N_F的位置属性N_F.Location为(x′，y′)，即

N_F.Location＝(x′，y′)

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

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

式中

E4：将当前节点Now赋为父节点N_F，即

Now←N_F

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

F＝0

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

8.根据权利要求6所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤D4具体为：

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

向量方向上的相邻节点

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

需进行以下操作：

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

赋为Visited，即

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

赋为[(I_m+3)mod 8]+1，即

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

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

Step←Step+1

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

E←E_p

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

s(x)←s_p(x)，x∈R

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

即

9.根据权利要求6所述的一种基于局部最优卷积评价的机器人路径规划方法，其特征在于，所述步骤D2具体为：

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

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

E＝(k(x)*s(x))|_x＝Step (12)

预期卷积评价指标E_p的定义式为：

E_p＝(k(x)*s_p(x))|_x＝Step+1 (13)

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

又由于

因此

由此得到步骤D2中的迭代计算式即式(6)，因此移动机器人在实际工作中无需根据定义式记录有效移动得分函数s(x)、预期移动得分函数s_p(x)与评价权重函数k(x)，再进行卷积计算从而得到卷积评价指标E与预期卷积评价指标E_p，而仅需按照步骤D2，根据评价聚焦度p计算出实时评价权重K_p，并利用实时评价权重K_p结合局部最优单位移动得分S_m对卷积评价指标E与预期卷积评价指标E_p进行反复迭代。

Images