移动机器人路径规划方法和系统与流程

本发明涉及移动机器人技术领域，具体地，涉及移动机器人路径规划方法和系统。

背景技术：

移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标的自主运动的系统。由于移动机器人在前往目标地点时可能要经过各种障碍物，因此需要对移动机器人的行进路径进行规划。

目前，在移动机器人的路径规划的诸多方法中包括：人工势场法和栅格地图法。但是，人工势场法的容易陷入局部极小点，从而导致移动机器人不能达到目标位置。传统的栅格地图法的基础组成部分为最小栅格刻度，以此将地图进行栅格划分。栅格地图法可以较为方便的使用递归算法求得任意两个通行栅格间的无碰撞路径，但是需要较大的资源与实践，且路径优化程度不理想。

因此，目前关于移动机器人的路径规划的方法效率低，无法从根本上避免人工势场法的局部最小点“死锁”问题。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种移动机器人路径规划方法和系统。

根据本发明提供的移动机器人路径规划方法，包括：

根据移动机器人的行进环境数据建立地图；

将所述地图划分为多个均匀的栅格，得到初始栅格地图，其中，所述初始栅格地图中的每个栅格对应于初始栅格矩阵中相应的一个可赋值的元素；

确定所述移动机器人在所述初始栅格地图上的起点位置和终点位置；

接收所述移动机器人上的传感器探测到的障碍物信号；

根据所述障碍物信号，确定所述移动机器人在当前位置上所受的斥力；

根据所述移动机器人在当前位置上所受的斥力构建障碍物区域的衰减的斥力场；

根据所述衰减的斥力场对所述初始栅格地图中的每个栅格进行赋值，得到赋值后的栅格地图；

根据所述赋值后的栅格地图，采用非遍历搜索方式生成所述移动机器人朝向终点位置的行进路径，其中，所述非遍历搜索方式是指：按照预设的搜索顺序确定移动机器人在当前位置的设定搜索半径范围内各个方向的路径上是否存在障碍物，当确定任一方向上无障碍物时，则朝向所述方向前进。

可选地，所述根据所述障碍物信号，确定所述移动机器人在当前位置上所受的斥力，包括：

根据接收到的所述障碍物信号确定障碍物与所述移动机器人之间的距离以及障碍物区域的范围。

可选地，所述根据所述移动机器人在当前位置上所受的斥力构建障碍物区域的衰减的斥力场，包括：

将障碍物区域作为斥力场的中心，通过g函数构建所述障碍物区域的衰减斥力场，其中所述g函数如下：

g是引力gravitation的简称,函数式如下

式中：ξ是引力势场常量，和分别代表机器人和目标点的距离。

可选地，所述基于所述赋值后的栅格地图，根据非遍历搜索方式生成所述移动机器人朝向终点位置的行进路径，包括：

假设所述移动机器人的起点位置的坐标为：(xs，ys)，终点位置的坐标为：(xf，yf)；基于sugeno模糊模型，确定非遍历搜索方式的搜索顺序如下：

在xs＞xf,ys＜yf,|xs-xf|＜|ys-yf|时，按照从西到北、由北到东、由东到南的方向进行搜索；

在xs＞xf,ys＞yf,|xs-xf|＜|ys-yf|时，按照从北到东、由东到南、由南到东的方向进行搜索；

在xs＜xf,ys＞yf,|xs-xf|＞|ys-yf|时，按照从东到北、由北到西、由西到南的方向进行搜索；

在xs＜xf,ys＞yf,|xs-xf|＜|ys-yf|时，按照从北到东、由东到南、由南到西的方向进行搜索；

在xs＜xf,ys＜yf,|xs-xf|＞|ys-yf|时，按照从东到南、由南到西、由西到北的方向进行搜索；

在xs＜xf,ys＜yf,|xs-xf|＜|ys-yf|时，按照从南到东、由东到北、由北到西的方向进行搜索；

在xs＞xf,ys＜yf,|xs-xf|＞|ys-yf|时，按照从西到南、由南到东、由东到北的方向进行搜索；

在xs＞xf,ys＜yf,|xs-xf|＜|ys-yf|时，按照从南到西、由西到东、由东到北的方向进行搜索。

可选地，所述基于所述赋值后的栅格地图，根据非遍历搜索方式生成所述移动机器人朝向终点位置的行进路径，包括：

根据终点位置确定所述移动机器人当前位置下的搜索方向的优先级顺序；

优先搜索朝向终点位置的方向上是否存在障碍物，若终点位置的方向存在障碍物，则按照剩余方向的优先级顺序进行顺次搜索。

可选地，在存在多个障碍物时，所述初始栅格地图中的每个栅格的赋值还需要叠加多个障碍物对于所述栅格的中心焦点所产生的引力和/或斥力值。

可选地，还包括：在所述移动机器人当前位置周围均存在障碍物时，进行完型合并操作，具体的：

假设搜索对角两个通行区的值时，所述对角的两个同行区为不可通行区域，则确定势场覆盖的区域是否小于预设的障碍区域合并阈值spsquare；若势场覆盖的区域小于预设的障碍区域合并阈值spsquare，则对障碍区域进行完形合并操作。

可选地，还包括：

检测所述移动机器人在所述赋值后的栅格地图上的路径点，其中，所述路径点为所述移动机器人移动路径所经过的栅格的中心点；

判断连续m个路径点构成的路径是否为折角路径；m为大于1的整数；

在连续m个路径点构成的路径为折角路径时，则将所述移动机器人的投影范围至折角路径上，在所述移动机器人的投影范围小于折角路径宽度时，将所示折角路径转化为直线路径。

根据本发明提供的移动机器人路径规划系统，包括：

地图生成模块，用于根据移动机器人的行进环境数据建立地图；

初始栅格地图生成模块，用于将所述地图划分为多个均匀的栅格，得到初始栅格地图，其中，所述初始栅格地图中的每个栅格对应于初始栅格矩阵中相应的一个可赋值的元素；

第一确定模块，用于确定所述移动机器人在所述初始栅格地图上的起点位置和终点位置；

接收模块，用于接收所述移动机器人上的传感器探测到的障碍物信号；

第二确定模块，用于根据所述障碍物信号，确定所述移动机器人在当前位置上所受的斥力；

斥力场构建模块，用于根据所述移动机器人在当前位置上所受的斥力构建障碍物区域的衰减的斥力场；

赋值地图生成模块，用于根据所述衰减的斥力场对所述初始栅格地图中的每个栅格进行赋值，得到赋值后的栅格地图；

路径生成模块，用于根据所述赋值后的栅格地图，采用非遍历搜索方式生成所述移动机器人朝向终点位置的行进路径，其中，所述非遍历搜索方式是指：按照预设的搜索顺序确定移动机器人在当前位置的设定搜索半径范围内各个方向的路径上是否存在障碍物，当确定任一方向上无障碍物时，则朝向所述方向前进。

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

本发明提供的移动机器人路径规划方法，通过栅格地图中每个栅格所具有的已经叠加完成的势场力值，并通过搜索算法以符合趋势引导的方式来控制进行方向朝向设定终点。由于路径规划时完全不涉及势力场的矢量计算，从根本上避免了人工市场法的局部最小点“死锁”问题。在规划最优的路径时完全不涉及到势场力的矢量计算，消除了人工势场法可能发生的局部最优点“死锁”的可能性。并且融合了sugeno模糊模型的思想和栅格地图算法、人工势场的优点，不仅可以避免“局部最小点锁死”的问题，而且还能使得算法在计算过程中的数据运算量降低。具有一次规划的优点，在可靠性、安全性、效率上均有较大提升。

附图说明

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

图1为本发明实施例一提供的移动机器人路径规划方法的流程示意图；

图2为本发明实施例二提供的移动机器人路径规划方法的栅格地图示意图；

图3为本发明实施例三提供的移动机器人路径规划方法的栅格地图示意图；

图4为特殊障碍区域的完型处理结果示意图；

图5为未优化的路径规划结果示意图；

图6为优化的路径规划结果示意图；

图7为12个方向的引导参数示意图。

具体实施方式

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

根据本发明提供的移动机器人路径规划方法，包括：根据移动机器人的行进环境数据建立地图；将地图划分为多个均匀的栅格，得到初始栅格地图，其中，初始栅格地图中的每个栅格对应于初始栅格矩阵中相应的一个可赋值的元素；确定移动机器人在初始栅格地图上的起点位置和终点位置；接收移动机器人上的传感器探测到的障碍物信号；根据障碍物信号，确定移动机器人在当前位置上所受的斥力；根据移动机器人在当前位置上所受的斥力构建障碍物区域的衰减的斥力场；根据衰减的斥力场对初始栅格地图中的每个栅格进行赋值，得到赋值后的栅格地图；根据赋值后的栅格地图，采用非遍历搜索方式生成移动机器人朝向终点位置的行进路径，其中，非遍历搜索方式是指：按照预设的搜索顺序确定移动机器人在当前位置的设定搜索半径范围内各个方向的路径上是否存在障碍物，当确定任一方向上无障碍物时，则朝向方向前进。

根据障碍物信号，确定移动机器人在当前位置上所受的斥力，包括：根据接收到的障碍物信号确定障碍物与移动机器人之间的距离以及障碍物区域的范围。根据移动机器人在当前位置上所受的斥力构建障碍物区域的衰减的斥力场，包括：将障碍物区域作为斥力场的中心，通过g函数构建障碍物区域的衰减斥力场，其中g函数如下：

g是引力gravitation的简称,函数式如下

式中：ξ是引力势场常量，和分别代表机器人和目标点的距离。

基于赋值后的栅格地图，根据非遍历搜索方式生成移动机器人朝向终点位置的行进路径，包括：假设移动机器人的起点位置的坐标为：(xs，ys)，终点位置的坐标为：(xf，yf)；基于sugeno模糊模型，确定非遍历搜索方式的搜索顺序如下：

在xs＞xf,ys＜yf,|xs-xf|＜|ys-yf|时，按照从西到北、由北到东、由东到南的方向进行搜索；

在xs＞xf,ys＞yf,|xs-xf|＜|ys-yf|时，按照从北到东、由东到南、由南到东的方向进行搜索；

在xs＜xf,ys＞yf,|xs-xf|＞|ys-yf|时，按照从东到北、由北到西、由西到南的方向进行搜索；

在xs＜xf,ys＞yf,|xs-xf|＜|ys-yf|时，按照从北到东、由东到南、由南到西的方向进行搜索；

在xs＜xf,ys＜yf,|xs-xf|＞|ys-yf|时，按照从东到南、由南到西、由西到北的方向进行搜索；

在xs＜xf,ys＜yf,|xs-xf|＜|ys-yf|时，按照从南到东、由东到北、由北到西的方向进行搜索；

在xs＞xf,ys＜yf,|xs-xf|＞|ys-yf|时，按照从西到南、由南到东、由东到北的方向进行搜索；

在xs＞xf,ys＜yf,|xs-xf|＜|ys-yf|时，按照从南到西、由西到东、由东到北的方向进行搜索。

基于赋值后的栅格地图，根据非遍历搜索方式生成移动机器人朝向终点位置的行进路径，包括：根据终点位置确定移动机器人当前位置下的搜索方向的优先级顺序；优先搜索朝向终点位置的方向上是否存在障碍物，若终点位置的方向存在障碍物，则按照剩余方向的优先级顺序进行顺次搜索。

在存在多个障碍物时，初始栅格地图中的每个栅格的赋值还需要叠加多个障碍物对于栅格的中心焦点所产生的引力和/或斥力值。

在移动机器人当前位置周围均存在障碍物时，进行完型合并操作，具体的：

假设搜索对角两个通行区的值时，对角的两个同行区为不可通行区域，则确定势场覆盖的区域是否小于预设的障碍区域合并阈值spsquare；若势场覆盖的区域小于预设的障碍区域合并阈值spsquare，则对障碍区域进行完形合并操作。

检测移动机器人在赋值后的栅格地图上的路径点，其中，路径点为移动机器人移动路径所经过的栅格的中心点；判断连续m个路径点构成的路径是否为折角路径；m为大于1的整数；在连续m个路径点构成的路径为折角路径时，则将移动机器人的投影范围至折角路径上，在移动机器人的投影范围小于折角路径宽度时，将所示折角路径转化为直线路径。

根据本发明提供的移动机器人路径规划系统，包括：地图生成模块，用于根据移动机器人的行进环境数据建立地图；初始栅格地图生成模块，用于将地图划分为多个均匀的栅格，得到初始栅格地图，其中，初始栅格地图中的每个栅格对应于初始栅格矩阵中相应的一个可赋值的元素；第一确定模块，用于确定移动机器人在初始栅格地图上的起点位置和终点位置；接收模块，用于接收移动机器人上的传感器探测到的障碍物信号；第二确定模块，用于根据障碍物信号，确定移动机器人在当前位置上所受的斥力；斥力场构建模块，用于根据移动机器人在当前位置上所受的斥力构建障碍物区域的衰减的斥力场；赋值地图生成模块，用于根据衰减的斥力场对初始栅格地图中的每个栅格进行赋值，得到赋值后的栅格地图；路径生成模块，用于根据赋值后的栅格地图，采用非遍历搜索方式生成移动机器人朝向终点位置的行进路径，其中，非遍历搜索方式是指：按照预设的搜索顺序确定移动机器人在当前位置的设定搜索半径范围内各个方向的路径上是否存在障碍物，当确定任一方向上无障碍物时，则朝向方向前进。

本发明所构建的栅格地图中在生成完成并计算障碍物斥力场后就不再进行势场力的矢量计算，而是通过栅格地图中每个栅格所具有的已经叠加完成的势场力值，并通过搜索符合趋势引导的方式来控制进行方向朝向设定终点。由于路径规划时完全不涉及势力场的矢量计算，从根本上避免了人工势场法的局部最小点“死锁”问题。

具体的，利用sugeno迷糊模型分段线性化的特点，结合人工势场的思想可以使用非遍历搜索方式，搜索半径为四个栅格。

以下为方便举例，搜索半径为两个栅格。在图2中，每个小栅格gi上(i＝1,2,...，25)代表栅格的位置，下方的数值表示按照人工势场法的思想通过g函数为障碍物规划的斥力场。在图2中，假设移动机器人当前处于点g13处，则首先将搜索其直接相邻的g12、g14、g8、g18四个方向。搜过方向根据每次路径规划时间不同设定起点与终点，根据设定起点(xs，ys)到终点(xf，yf)的坐标相对位置生成监测方向与优先级并赋予权重，wdirectiong[i]；i＝0,1,2,3为优先权重系数。结合sugeno模糊模型思想总结出以下八种搜索顺序：

①if:xs＞xf,ys＜yf,|xs-xf|＜|ys-yf|

then:w→n→e→s

②if:xs＞xf,ys＞yf,|xs-xf|＜|ys-yf|

then:n→w→s→e

③if:xs＜xf,ys＞yf,|xs-xf|＞|ys-yf|

then:e→n→w→s

④if:xs＜xf,ys＞yf,|xs-xf|＜|ys-yf|

then:n→e→s→w

⑤if:xs＜xf,ys＜yf,|xs-xf|＞|ys-yf|

then:e→s→w→n

⑥if:xs＜xf,ys＜yf,|xs-xf|＜|ys-yf|

then:s→e→n→w

⑦if:xs＞xf,ys＜yf,|xs-xf|＞|ys-yf|

then:w→s→e→n

⑧if:xs＞xf,ys＜yf,|xs-xf|＜|ys-yf|

then:s→w→e→n

式中：s表示南方向，w表示西方向，e表示东方向，n表示北方向，→表示方向转换。

进一步地，若假设图3的设置终点在g13点的e，s方向(东南方向)且关于终点位置的x方向上绝对距离较长，则规划算法将按照从g14、g18、g12、g8的优先级顺序依次搜索。则第一个搜索点为g14。累加以g14点为中心的7个剩余方向点(g9、g15、g19、g8、g10、g18、g20)的斥力场的和，记为sumpointn，并乘以该方向的优先权重系数wdirectiong[0]，则该点的方向引导参数的计算公式如下：

gguidingfactoupoint13-14＝sumpoint13-14*wdirectionorg[0]

式中：gguidingfactoupoint13-14表示13-14的引导参数，sumpoint13-14表示以13点为中心的(不包括13点)7个剩余方向点的斥力势场值的和，wdirectionorg[0]表示该点的在对应方向的优先权重系数。

同理：g18的方向引导参数为：

gguidingfactoupoint13-18＝sumpoint13-18*wdirectionorg[1]

式中：gguidingfactoupoint13-18表示13-18的引导参数，sumpoint13-18表示以13点为中心的(不包括13点)7个剩余方向点的斥力势场值的和，wdirectionorg[1]表示该点的在对应方向的优先权重系数。

g12的方向引导参数为：

gguidingfactoupoint13-12＝sumpoint13-12*wdirectionorg[2]

式中：gguidingfactoupoint13-12表示13-12的引导参数，sumpoint13-12表示以13点为中心的(不包括13点)7个剩余方向点的斥力势场值的和，wdirectionorg[2]表示该点的在对应方向的优先权重系数。

g8点的方向引导参数为：

gguidingfactoupoint13-8＝sumpoint13-8*wdirectionorg[3]

式中：gguidingfactoupoint13-8表示13-8的引导参数，sumpoint13-8表示以13点为中心的(不包括13点)7个剩余方向点的斥力势场值的和，wdirectionorg[3]表示该点的在对应方向的优先权重系数。

因假定搜索半径为2，则需要对第一层点的周边方向进行搜索，如图4所示，以对g14点周边方向进行搜索为例(不含推导点g13)。

若计算13-14-9的方向引导参数为

gguidingfactorpoint13-14-9＝sumpoint13-14-9*wdirectionorg[3]

式中：gguidingfactorpoint13-14-9表示13-14-9的引导参数，sumpoint13-14-9表示表示以13点为中心的(不包括13点)7个剩余方向点的斥力势场值的和，wdirectionorg[3]表示该点的在对应方向的优先权重系数。

按此循环得到g13点周边r＝2(栅格为2的路径长度)范围内的12个方向的引导参数。如图7所示gguidingfoctorpointm-n-i简写为gm-n-i，则以m为中心12个方向引导参数包括：gm-n-i,gm-n-o、gm-n-s、gm-r-s、gm-r-w、gm-r-q、gm-l-q、gm-l-k、gm-l-g、gm-h-g、gm-h-c、gm-h-i。并将12个方向的引导参数存入数据链表供回溯参照，检索其中的最小值，设定第一层点为路径点，并以该路径点为新的中心点，重复上述算法，直到最终找到目标点。如果出现取值相等的情况，则选择运动平台姿势变化最小的点为下一路路径点。

当出现如图4所示情况时，由于移动机器人在红色框体中搜索时，其传感器反馈四周环境得到障碍区域，当尝试覆盖的框体区域小于设定障碍区域合并阀值sprect时，这样应将该类障碍区域做完型合并操作。

由于路径规划的基础为栅格地图，在核心路径规划算法为人工势场的前提下，规划出的弧线路径反而成为了不断地折角，变相使得通过该路径的硬件累计误差增加。为优化此类问题，例如：将以三个、四个、五个路径点为一组的所构成的特殊折角形状模板(共四种)遍历搜索整个关键路径点队列生成前所储存的有序路径队列，将折角形状模板转化为对应的截弯取直取直模板。但直接套用模板连接对角点可能会造成移动机器人在临界行进时，其体积上有障碍物，故需要进行校验。

校验过程为通过上一次优化所得出的关键路径队列，将机器人垂直投影面积cabsolutefield投影到该路径，即生成全路径安全范围，如图6所示(注：示例中为方便说明，设cabsolutefield大小即为一个栅格，则蛇形栅格覆盖面积即为全路径安全范围)，栅格路径关键转折点的现实优化效果中发现，保留折线的部分若按照模板连接对角点其连线不在蛇形栅格覆盖的全路径安全范围内，故保留折角。

需要说明的是，本发明提供的移动机器人路径规划方法中的步骤，可以利用移动机器人路径规划系统中对应的模块、装置、单元等予以实现，本领域技术人员可以参照系统的技术方案实现方法的步骤流程，即，系统中的实施例可理解为实现方法的优选例，在此不予赘述。

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

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