CN109945873B - 一种用于室内移动机器人运动控制的混合路径规划方法 - Google Patents

Abstract

本发明公开一种用于室内移动机器人运动控制的混合路径规划方法，属于机器人技术领域，其结合了静态环境的全局路径规划方法和动态环境的局部路径规划方法。本发明通过外部传感器为系统提供外部环境信息输入，由此建立栅格模型；本发明提出并行Bi‑directional A‑star算法与改进人工势场Artificial Potential Field算法融合的混合路径规划方法，通过并行Bi‑directional A‑star算法进行全局、静态路径搜索，通过改进APF算法进行局部、动态路径搜索。最后由以上混合方法得到的路径数据控制机器人的运动。本发明采用的混合路径规划方法对室内环境具有更强的适应能力，并且本发明所采用的一系列措施和优化方法所得到的路径具有很强的完备性和较好的最优性。

Description

一种用于室内移动机器人运动控制的混合路径规划方法

技术领域

本发明涉及机器人技术领域，特别涉及一种用于室内移动机器人运动控制的混合路径规划方法

背景技术

随着机器人技术的不断发展和成熟，机器人研究向智能化发展，机器人的应用已经不再局限于在传统的制造业上帮助人类完成一些有难度的工作，开始进军到太空探索、医疗、娱乐、服务等众多领域。其中，自主移动机器人在各个应用领域占据主导地位。

路径规划作为机器人领域的一个重要部分，主要负责为机器人在存在障碍物的环境中，提供一条由起始状态到目标状态的无碰撞最优路径。具有路径规划功能的移动机器人能够实现已知环境、部分已知环境，甚至是完全未知环境的自主避障与导航功能。路径规划根据不同的应用场景，可以分为全局路径规划和局部路径规划。已知环境下采用全局路径规划方法即可实现机器人自主导航，该类方法可以搜索到最优解，但是计算量庞大，时效性差；常用的全局路径规划算法有Dijkstra、A-star算法等，其中，Dijkstra算法以运行效率为代价获得最优解的成功率，而A-star算法在路径搜索过程中，极大的减小了搜索节点，提高了路径搜索效率，具有较好的鲁棒性。对动态环境或未知环境下一般采用局部路径规划实现动态避障的要求，该类方法具有实时性较高的优点，但是无法保证搜索到最优路径；局部路径规划算法有遗传算法、模糊控制算法、人工势场法(Artifitial PotentialField,APF)等，遗传算法路径搜索过程的计算量较大，局部寻优能力较差；模糊控制法实时性好，但对于复杂环境下，该算法的模糊控制规则会急剧膨胀；人工势场法结构简单，路径轨迹平滑，避障能力强，但是存在目标不可达和局部最小值缺点。

发明内容

本发明提供了一种室内移动机器人路径规划的方法，通过全局与局部路径规划的融合，解决全局规划时效性差和局部规划最优性差的问题，并采用并行Bi-directionalA-star算法以较少的路径最优性为代价大幅度提升全局搜索效率，采用改进的APF解决传统APF碰撞、目标不可达、局部最小值问题，极大的提升局部路径规划的完备性，同时提出一种window-APF实现方式大幅减少了动态环境下的计算量，进一步提高了系统的时效性。

本发明采用一下技术方案解决上述技术问题：

一种用于室内移动机器人运动控制的混合路径规划方法，具体包含如下步骤；

步骤1，建立室内机器人工作环境的全局栅格地图模型，并设置机器人运动的起点和目标点；

步骤2，对栅格地图中的障碍物进行预处理；

步骤3，在当前时刻的栅格地图中，对机器人采用并行Bi-directionalA-star算法实施全局路径规划策略，搜索获取由当前位置到目标点的无碰撞较优路径；

步骤4，分割步骤3所述方法所得的无碰撞较优路径，提取出一系列局部目标点，具体指起点、拐点以及目标点的集合；

步骤5，开启动态地图线程，实时更新传感器扫描重建的局部栅格地图信息；

步骤6，开启局部路径规划线程，实时读取步骤5所述动态地图线程提供的局部地图信息，以步骤4所述的局部目标点为当前目标点，对机器人采用改进ArtificialPotential Field算法实施局部动态路径规划策略，规划出一条躲避新增障碍物和运动障碍物的局部无碰撞路径；

步骤7，开启机器人运动控制线程，实时读取步骤6所述局部路径规划线程提供的机器人位置信息，控制机器人跟随局部路径运动。

作为本发明用于室内移动机器人运动控制的混合路径规划方法的进一步优选方案，在步骤1中，使用环境描述方式简单、易于维护的栅格模型作为路径规划的地图模型。

作为本发明用于室内移动机器人运动控制的混合路径规划方法的进一步优选方案，在步骤2中，根据机器人自身大小和最小安全距离对栅格地图所表示的障碍物进行膨胀处理，对障碍物栅格位置的相邻栅格作障碍物栅格标记。

作为本发明用于室内移动机器人运动控制的混合路径规划方法的进一步优选方案，在步骤3中，开启并行的两个线程同时进行正向和反向的A-star路径搜索过程；其中，路径搜索过程中，相对目标点的位置保持不变；搜索结束条件是两个搜索方向的待考察点集合相遇；路径回溯是从相遇的待考察点开始。

作为本发明用于室内移动机器人运动控制的混合路径规划方法的进一步优选方案，在步骤4中，采用消除多余路径点的方法，得到只包含起点、拐点、目标点的路径点集合，然后以拐点为分界点，分割全局路径。

作为本发明用于室内移动机器人运动控制的混合路径规划方法的进一步优选方案，在步骤6中，开启局部路径规划线程，实时读取当前时刻动态地图线程提供的局部地图信息，对机器人实施改进APF算法，使得机器人到达当前的局部目标点；通过更新局部起点和局部目标点，完成机器人从起点到目标点的分段路径规划；其中，提出window-APF的实现方式，将过程根据障碍物影响距离计算整个地图的势场优化为过程计算当前点为中心、障碍物影响距离为半径的窗口地图的势场；同时，采用改进斥力势场函数，在原有的斥力势场基础上添加当前位置与目标点距离D_curr_goal因子，并通过设定该因子的指数参数，达到平衡解决碰撞和目标不可达问题；采用回退-虚拟目标点法处理局部极小值陷阱问题，将处于局部极小值位置的机器人回退到N步，并添加虚拟目标点，其中虚拟目标点的设置满足当前位置-目标点-虚拟目标点构成等边三角形，且当前位置与虚拟目标点的距离D_curr_virtual等于目标点与虚拟目标点的距离D_goal_virtual；然后以当前位置为起点，虚拟目标点为终点，进行APF路径规划的迭代过程。

本发明采用上述技术方案，能产生如下技术效果：

1、本发明结合全局与局部路径规划策略采用混合路径规划方法，能够运行在已知、未知、静态、动态以及四种环境组合的环境特征下，能够解决更加复杂环境的路径规划问题，具有很强的完备性和较好的最优性；

2、在全局静态路径规划方面，使用并行Bi-directional A-star，得到的路径最优性接近传统的A-star算法，但是通过开启两个并行的线程，从两端同时进行A-star路径搜索，因此在执行效率上得到大幅度提升，增强系统的实时性；

3、在局部动态路径规划方面，第一，使用window-APF的实现方式，将障碍物的影响距离转换为机器人的被影响区域，该实现方式与传统的APF实现方式达到同样的效果，但在计算量和循环次数上得到了极大的改善；第二，采用同一斥力势场函数，只需调整一个参数即可解决传统APF的碰撞和目标不可达问题；第三，采用回退法与虚拟目标点法结合的方式处理局部极小值陷阱问题，并使用计算方便、完备性好的虚拟目标点选取策略，相对于传统的比较当前位置两侧障碍物夹角之和来设置虚拟目标点的方法，本发明采用使虚拟目标点的设置满足当前位置-目标点-虚拟目标点构成等边三角形，且当前位置与虚拟目标点的距离等于目标点与虚拟目标点的距离即可，该方法能节省很大的计算量，处理效率更高。

4、本发明利用消除多余路径点的方法作为局部目标点的选取策略，一方面能够将全局搜索得到的路径最大化简化，得到只包含起点、拐点、目标点的路径点集合，且此时得到的拐点数量将是最少的；另一方面，由拐点分割得到的分段路径是最简单的、最优路径，该路径上不存在任何障碍物，因此在此基础上进行局部路径规划时，只需要躲避新增障碍物和动态障碍物即可

附图说明

图1为实施方法流程图；

图2为全局路径规划方法流程图；

图3为局部路径规划方法流程图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

如图1所示，本发明设计了一种用于室内移动机器人运动控制的混合路径规划方法及系统，包括以下步骤：

步骤1、建立室内机器人工作环境的全局栅格地图模型，并设置机器人运动的起点和目标点；

首先使用Kinect传感器扫描整个环境信息，通过SLAM技术获得室内环境的全局栅格地图信息，栅格以数值0-1分别表示实际环境中的可达-不可达区域。

步骤2、栅格地图中障碍物预处理；

通过不可达栅格的相邻栅格不可达化，实现全局地图环境的膨胀处理，将膨胀处理后的新栅格地图作为路径规划的地图模型；其中，相邻栅格是指当前栅格的东、南、西、北、西北、东南、东北、西南8个方向的栅格，其中这8个方向的膨胀半径满足r＝R+1，r表示栅格地图膨胀半径，R表示机器人自身大小半径。

步骤3、在当前时刻的栅格地图中，对机器人采用并行Bi-directionalA-star算法实施全局路径规划策略，搜索到由当前位置到目标点的无碰撞较优路径；

如图2所示，开启正、反搜索方向的线程，正向搜索线程T1以起点作为正向搜索起点，目标点作为正向搜索目标点实施A-star算法路径规划；反向搜索线程T2以目标点作为反向搜索起点，起点作为反向搜索目标点实施A-star算法路径规划；其中两个搜索方向的目标点保持不变；为保持两个方向的搜索步调一致，对两个线程的运行次数进行统计，通过延时使得两个方向的搜索步数之差不超过一个阈值T；两个线程将共享一个Flag二维矩阵，该Flag矩阵标记了对应位置的栅格状态，以数值0-1-2分别表示未考察-正向待考察-反向待考察状态，当两个搜索方向的待考察节点重合，即Flag矩阵的某个节点P被同时标记为1和2，则结束两个线程；路径以重合节点P开始，向正反搜索方向回溯路径父节点，然后合并两条路径，得到全局路径点集合{p_1,p_2,…,p_k}，全局路径规划结束。

步骤4、分割步骤3所述方法所得的全局路径，提取出一系列局部目标点；

采用消除多余路径点的方法，得到只包含起点、拐点、目标点的路径点集合{node_1,node_2,…,node_k}，并且将路径中的拐点数量减少到最低；然后以拐点为分界点，分割全局路径；其中消除多余路径点的措施如下，连接任意两个路径点p_i、p_j，判断p_i、p_j构成的线段是否穿过障碍物位置，若未经过障碍物，则删除p_i、p_j之间的所有路径点，否则，对其他路径点进行如上的重复操作。

步骤5、开启动态地图线程，实时更新传感器扫描重建的局部栅格地图信息；

使用Kinect传感器实时扫描机器人当前位置的环境信息，通过SLAM技术重建机器人当前位置的局部栅格地图，实时更新局部栅格地图信息。

步骤6、开启局部路径规划线程，实时读取步骤5所述动态地图线程提供的局部地图信息，以步骤4所述的局部目标点为当前目标点，对机器人采用改进ArtificialPotential Field(APF)算法实施局部动态路径规划策略，规划出一条躲避新增障碍物和运动障碍物的局部无碰撞路径；

如图3所示，首先以起点node_1为第一步局部路径规划的起点，以node_2为第一步局部路径规划的目标点，并实时读取当前时刻步骤5所述动态地图线程提供的局部地图信息，对机器人实施改进APF算法，当机器人到达node_2附近时，以当前机器人位置为第二步局部路径规划的起点，node_3为第二步局部路径规划的目标点，同样读取当前时刻的局部地图，实施改进APF算法；按此规律，直到采用改进APF使得机器人到达node_k位置附近；其中，改进的APF算法步骤如下：

S1：读取步骤5提供的局部地图数据；

S2：根据障碍物影响距离d0设置对应的窗口矩阵window＝[-d:d,-d:d]，满足d＝d0+1；并根据机器人当前位置、窗口矩阵window、全局地图、局部地图获得以机器人当前位置为中心的、窗口矩阵大小的局部窗口地图；具体设置如下描述：

假设全局地图为MAP矩阵，且Y轴方向高度为Height、X轴方向宽度为Width，当前位置坐标为(ix,iy)，窗口矩阵半径为d，则有窗口矩阵地图：

window＝MAP[ix-a1:ix+a2,iy-a3:iy+a4]

其中，ix-a1、ix+a2表示X坐标范围，iy-a3、iy+a4表示Y坐标范围，且a1、a2、a3、a4满足如下公式：

a1＝min(d,ix-1)

a2＝min(d,Width-ix)

a3＝min(d,iy-1)

a4＝min(d,Height-iy)

S3：根据S2描述的局部窗口地图信息，对局部窗口地图的障碍物进行遍历，求得当前位置与障碍物的距离，目标位置与障碍物的距离；

S4：计算引力、斥力及其合力；根据合力大小和方向计算机器人的下一步位置坐标；引力势场函数、单个障碍物的斥力势场函数、总斥力势场函数、合力函数如下，其中D(q,q_goal)、D(q,q_obsi)分别表示当前点与目标点的距离、当前点与障碍物i的距离，d₀表示障碍物影响距离：

其中，采用改进斥力势场函数，在原有的斥力势场基础上添加当前位置与目标点距离D_curr_goal因子，并通过设定该因子的指数参数，达到平衡解决碰撞和目标不可达问题；一方面使得当机器人在远离目标点时，靠近障碍物附，通过D_curr_goal平方因子的大数值来增大斥力势场，防止碰撞；一方面使得机器人在距离目标点很近时，靠近障碍物，通过D_curr_goal开平方因子的小数值来削弱斥力，顺利达到目标点；

S5：判断机器人当前位置是否进入局部极小值陷阱，若当前位置对应的坐标和合力与前N步以内的坐标和合力存在重复，则判定机器人陷入局部极小值陷阱，进入S6，否则跳转S7；

S6：处理局部极小值陷阱问题子程序；将机器人回退到前N步的位置，添加虚拟目标点，满足当前位置-目标点-虚拟目标点形成等边三角形，且当前位置与虚拟目标点的距离D_curr_virtual等于目标点与虚拟目标点的距离D_goal_virtual；然后以当前位置为起点，虚拟目标点为终点，进行APF路径规划的迭代过程，设定迭代次数J满足J＝k*栅格地图单位/机器人运动步长；迭代完成或到达虚拟目标点，则处理局部极小值陷阱问题子程序执行完毕；

S7：重复S2～S5，直到找到目标点或到达迭代次数上限值。

步骤7、开启机器人运动控制线程，实时读取步骤6所述局部路径规划线程提供的机器人位置信息，控制机器人跟随局部路径运动。

综上所述，本发明提出一种用于室内移动机器人运动控制的混合路径规划方法及系统，通过本发明采用的混合路径规划方法对室内环境具有更强的适应能力，可以处理静态和动态环境、已知和未知环境，并且本发明所采用的一系列措施和优化方法所得到的路径具有很强的完备性和较好的最优性。通过本方法实现的路径规划系统可以应用于物流仓储机器人、智能服务机器人、自动驾驶等领域，在机器人自主导航领域有着广泛的应用前景。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (5)

1.一种用于室内移动机器人运动控制的混合路径规划方法，其特征在于：具体包含如下步骤；

步骤1，建立室内机器人工作环境的全局栅格地图模型，并设置机器人运动的起点和目标点；

步骤2，对栅格地图中的障碍物进行预处理；

步骤3，在当前时刻的栅格地图中，对机器人采用并行Bi-directionalA-star算法实施全局路径规划策略，搜索获取由当前位置到目标点的无碰撞较优路径；

步骤4，分割步骤3所述方法所得的无碰撞较优路径，提取出一系列局部目标点，具体指起点、拐点以及目标点的集合；

步骤5，开启动态地图线程，实时更新传感器扫描重建的局部栅格地图信息；

步骤6，开启局部路径规划线程，实时读取步骤5所述动态地图线程提供的局部地图信息，以步骤4所述的局部目标点为当前目标点，对机器人采用改进ArtificialPotentialField算法实施局部动态路径规划策略，规划出一条躲避新增障碍物和运动障碍物的局部无碰撞路径；

步骤7，开启机器人运动控制线程，实时读取步骤6所述局部路径规划线程提供的机器人位置信息，控制机器人跟随局部路径运动；

在步骤6中，开启局部路径规划线程，实时读取当前时刻动态地图线程提供的局部地图信息，对机器人实施改进APF算法，使得机器人到达当前的局部目标点；通过更新局部起点和局部目标点，完成机器人从起点到目标点的分段路径规划；其中，提出window-APF的实现方式，将过程根据障碍物影响距离计算整个地图的势场优化为过程计算当前点为中心、障碍物影响距离为半径的窗口地图的势场；同时，采用改进斥力势场函数，在原有的斥力势场基础上添加当前位置与目标点距离D_curr_goal因子，并通过设定该因子的指数参数，达到平衡解决碰撞和目标不可达问题；采用回退-虚拟目标点法处理局部极小值陷阱问题，将处于局部极小值位置的机器人回退到N步，并添加虚拟目标点，其中虚拟目标点的设置满足当前位置-目标点-虚拟目标点构成等边三角形，且当前位置与虚拟目标点的距离D_curr_virtual等于目标点与虚拟目标点的距离D_goal_virtual；然后以当前位置为起点，虚拟目标点为终点，进行APF路径规划的迭代过程。

2.根据权利要求1所述的用于室内移动机器人运动控制的混合路径规划方法，其特征在于，在步骤1中，使用环境描述方式简单、易于维护的栅格模型作为路径规划的地图模型。

3.根据权利要求1所述的用于室内移动机器人运动控制的混合路径规划方法，其特征在于，在步骤2中，根据机器人自身大小和最小安全距离对栅格地图所表示的障碍物进行膨胀处理，对障碍物栅格位置的相邻栅格作障碍物栅格标记。

4.根据权利要求1所述的用于室内移动机器人运动控制的混合路径规划方法，其特征在于，在步骤3中，开启并行的两个线程同时进行正向和反向的A-star路径搜索过程；其中，路径搜索过程中，相对目标点的位置保持不变；搜索结束条件是两个搜索方向的待考察点集合相遇；路径回溯是从相遇的待考察点开始。

5.根据权利要求1所述的用于室内移动机器人运动控制的混合路径规划方法，其特征在于，在步骤4中，采用消除多余路径点的方法，得到只包含起点、拐点、目标点的路径点集合，然后以拐点为分界点，分割全局路径。

Images