移动机器人路径规划和避障方法及系统的利记博彩app

移动机器人路径规划和避障方法及系统的利记博彩app
【专利摘要】本发明公开了一种移动机器人路径规划和避障方法及系统，该移动机器人路径规划方法包括：利用已知障碍物环境信息建立二维栅格地图；在所述二维栅格地图中，以移动机器人所在位置建立全局坐标系，设置所述移动机器人的起点和终点；采用跳点搜索算法确定所述起点到所述终点之间的最短路径；所述最短路径包括若干顺序相连的局部目标点；控制所述移动机器人向每一所述局部目标点移动过程中，采用局部避障算法避开动态障碍物。本发明所提供的移动机器人路径规划和避障方法，采用跳点搜索算法快速获取最短路径，可提高路径搜索效率，节省存储空间；采用局部避障算法可保证移动机器人路径规划和避障的准确性和实时性，实现移动机器人的自主导航。
【专利说明】
移动机器人路径规划和避障方法及系统
技术领域
[0001] 本发明设及机器人避障技术领域，尤其设及一种移动机器人路径规划和避障方法 及系统。
【背景技术】
[0002] 近年来，随着计算机技术和传感器技术的迅速发展，移动机器人技术也得W快速 发展。由于移动机器人具有智能、自主、高效和便捷等特点，其在工业、农业、航天等许多领 域发挥着越来越重要的作用。而路径规划和避障作为导航研究中的关键技术，是移动机器 人智能化的一个重要标志。移动机器人路径规划和避障是指在环境障碍条件下，选择一条 从起点到目标的路径，使得移动机器人可W安全、无碰撞地通过所有障碍。
[0003] 目前，国内外的研究者在移动机器人路径规划方面已做了大量研究工作。路径规 划按照环境信息是否已知可分为全局和局部路径规划。现有全局路径规划算法的理论研究 较为完善，但存在预取的环境信息存储量和计算量大，且获取的环境信息往往不够完善，无 法适应变化的环境。A*算法是全局路径规划算法中在静态路网中求解最短路径最有效的直 接捜索方法，但存在捜索效率低，节点存储量大等缺点。局部路径规划与全局路径规划相 比，更具有实时性和实用性，但其缺乏全局信息，易丧失目标，甚至陷入死锁。如局部路径规 划中的模糊逻辑控制容易出现振荡、死锁等现象，容易影响移动机器人的正常工作。

【发明内容】

[0004] 本发明要解决的技术问题在于，针对现有机器人的全局路径规划对动态环境的不 适用和局部规划易丧失目标点的问题，提供一种移动机器人路径规划和避障方法及系统。
[0005] 本发明解决其技术问题所采用的技术方案是:一种移动机器人路径规划和避障方 法，包括：
[0006] 利用已知障碍物环境信息建立二维栅格地图；
[0007] 在所述二维栅格地图中，W移动机器人所在位置建立全局坐标系，设置所述移动 机器人的起点和终点；
[000引采用跳点捜索算法确定所述起点到所述终点之间的最短路径;所述最短路径包括 若干顺序相连的局部目标点；
[0009] 控制所述移动机器人向每一所述局部目标点移动过程中，采用局部避障算法避开 动态障碍物。
[0010] 优选地，采用跳点捜索算法确定所述起点到所述终点之间的最短路径，包括：
[0011] 建立open列表和close列表;open列表用于存放未遍历访问的节点；close列表用 于存放已经遍历访问过的节点；
[0012] 将所述移动机器人的起点放入open列表；
[0013] 遍历所述起点的上、下、左、右、左上、左下、右上和右下八个方向的子节点；
[0014] 判断所述子节点是否已在open列表或close列表中：若所述子节点已在open列表 中，则重新计算所述子节点的f(n)值，判断所述子节点的f(n)值是否减小;若所述子节点的 f(n)值减小，则更新open列表中所述子节点的f(n)值和父节点;若所述子节点已在close列 表中，则重新计算所述子节点的f(n)值，判断所述子节点的f(n)值是否减小;若所述子节点 的f (n)值减小，将所述子节点从C10Se列表中移除，重新更新所述子节点的f (n)值和父节 点，将所述子节点加入open列表中；若所述子节点不在open列表或close列表，则将所述子 节点放入open列表中；
[0015] 从open列表中选取f(n)值最小的节点并将所述f(n)值最小的节点的父节点放入 close列表；
[0016] 判断所述f(n)值最小的节点是否为移动机器人的终点;若是，则返回最短路径，最 短路径中的节点为局部目标点；若否，则将所述f(n)值最小的节点的父节点放入close列 表，并跳转到对该节点进行跳点捜索处理；
[0017] 对所述f(n)值最小的节点，分别进行水平方向捜索、垂直方向捜索和对角线方向 捜索，W捜索跳点；其中，所述对角线方向捜索包括水平方向捜索和垂直方向捜索;待各方 向捜索完毕，跳转到从open列表中选取f(n)值最小的节点，继续处理。
[0018] 优选地，所述采用局部避障算法避开动态障碍物，包括：
[0019] 实时检测所述动态障碍物，获取所述动态障碍物相对于所述移动机器人的实时距 离和方位角度；
[0020] 根据所述实时距离和方位角度，控制所述移动机器人转动，W避开所述动态障碍 物。
[0021] 优选地，所述根据所述实时距离和方位角度，控制所述移动机器人转动，W避开所 述动态障碍物，包括：
[0022] 预设若干距离范围阔值，每一距离范围阔值与一避障转动角度相应；
[0023] 根据所述实时距离确定其所属的距离范围阔值及对应的避障转动角度；
[0024] 控制所述移动机器人转动所述避障转动角度，W避开所述动态障碍物。
[0025] 优选地，所述采用局部避障算法避开动态障碍物，还包括:实时获取所述移动机器 人的移动距离和转动角度。
[0026] 本发明还提供一种移动机器人路径规划和避障系统，包括：
[0027] 地图建立单元:用于利用已知障碍物环境信息建立二维栅格地图；
[00%]坐标建立单元：用于在所述二维栅格地图中，W移动机器人所在位置建立全局坐 标系，设置所述移动机器人的起点和终点；
[0029] 路径确定单元：用于采用跳点捜索算法确定所述起点到所述终点之间的最短路 径;所述最短路径包括若干顺序相连的局部目标点；
[0030] 移动避障单元：用于控制所述移动机器人向每一所述局部目标点移动过程中，采 用局部避障算法避开动态障碍物。
[0031] 优选地，所述路径确定单元包括：
[0032] 列表建立模块：用于建立open列表和close列表;open列表用于存放未遍历访问的 节点；close列表用于存放已经遍历访问过的节点；
[0033] 起点设置模块:用于将所述移动机器人的起点放入open列表；
[0034] 子节点遍历模块：用于遍历所述起点的上、下、左、右、左上、左下、右上和右下八个 方向的子节点；
[0035] 子节点判断模块:用于判断所述子节点是否已在open列表或close列表中：若所述 子节点已在open列表中，则重新计算所述子节点的f(n)值，判断所述子节点的f(n)值是否 减小;若所述子节点的f (n)值减小，则更新open列表中所述子节点的f (n)值和父节点;若所 述子节点已在close列表中，则重新计算所述子节点的f(n)值，判断所述子节点的f(n)值是 否减小;若所述子节点的f(n)值减小，将所述子节点从close列表中移除，重新更新所述子 节点的f(n)值和父节点，将所述子节点加入open列表中；若所述子节点不在open列表或 close列表，则将所述子节点放入open列表中；
[0036] 节点选取模块:用于从open列表中选取f(n)值最小的节点并将所述f(n)值最小的 节点的父节点放入close列表；
[0037] 路径确定模块:用于判断选取f(n)值最小节点是否为移动机器人的终点;若是，贝U 返回最短路径，最短路径中的节点为局部目标点;若否，则将所述f(n)值最小的节点的父节 点放入close列表，并跳转到节点捜索模块。
[0038] 节点捜索模块:用于对所述f(n)值最小的节点，分别进行水平方向捜索、垂直方向 捜索和对角线方向捜索，W捜索跳点；其中，所述对角线方向捜索包括水平方向捜索和垂直 方向捜索，待各方向捜索完毕，跳转到节点选取模块，继续处理；
[0039] 优选地，所述移动避障单元包括：
[0040] 障碍物检测模块：用于实时检测所述动态障碍物，获取所述动态障碍物相对于所 述移动机器人的实时距离和方位角度；
[0041 ]避障控制模块:用于根据所述实时距离和方位角度，控制所述移动机器人转动，W 避开所述动态障碍物。
[0042] 优选地，所述避障控制模块包括：
[0043] 预设范围子模块：用于预设若干距离范围阔值，每一距离范围阔值与一避障转动 角度相应；
[0044] 避障确定子模块:用于根据所述实时距离确定其所属的距离范围阔值及对应的避 障转动角度；
[0045] 转动控制子模块：用于控制所述移动机器人转动所述避障转动角度，W避开所述 动态障碍物。
[0046] 优选地，所述移动避障单元还包括:状态检测模块，用于实时获取所述移动机器人 的移动距离和转动角度。
[0047] 本发明与现有技术相比具有如下优点:本发明所提供的路径规划和避障方法及系 统，通过在已知障碍物环境信息采用跳点捜索算法快速获取从起点到终点的最短路径，可 提高路径捜索效率、节省移动机器人路径规划时间，且其捜索过程中所需的存储空间小;在 控制移动机器人向每一局部目标点移动过程中，采用局部避障算法实时避开动态障碍物， W保证移动机器人路径规划和避障的准确性和实时性，实现移动机器人的自主导航。
【附图说明】
[0048] 下面将结合附图及实施例对本发明作进一步说明，附图中：
[0049] 图1是本发明实施例1中移动机器人路径规划和避障方法的一流程图。
[0050]图2是本发明实施例2中移动机器人路径规划和避障方法的一流程图。
[0051 ]图3是本发明实施例2中跳点捜索算法的一流程图。
[0052] 图4是本发明实施例2中水平方向捜索的示意图。
[0053] 图5是本发明实施例2中对角线方向捜索的示意图。
[0054] 图6是本发明实施例2中执行跳点捜索算法的一示意图。
[0055] 图7是本发明实施例2中执行跳点捜索算法的另一示意图。
[0056] 图8是本发明实施例3中移动机器人路径规划和避障方法的一原理框图。
[0057] 图9是本发明实施例4中移动机器人路径规划和避障方法的一原理框图。
[005引图中：10、地图建立单元;20、坐标建立单元;30、路径确定单元;31、列表建立模块； 32、起点设置模块;33、子节点遍历模块;34、子节点判断模块;35、节点选取模块;36、节点捜 索模块;37、路径确定模块;40、移动避障单元;41、障碍物检测模块;42、避障控制模块;421、 预设范围子模块;422、避障确定子模块;423、转动控制子模块;43、状态检测模块。
【具体实施方式】
[0059] 为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明 本发明的【具体实施方式】。
[0060] 实施例1
[0061] 图1示出本实施例中的移动机器人路径规划和避障方法。如图1所示，该移动机器 人路径规划和避障方法包括如下步骤：
[0062] Sll:利用已知障碍物环境信息建立二维栅格地图。本实施例中，采用二维数组存 储已经障碍物环境信息，并建立二维栅格地图，在二维栅格地图中，1表示有障碍物的障碍 物栅格，0表示无障碍物的自由栅格。其中，二维栅格地图可W通过人工预先输入。可W理解 地，也可结合建图和定位技术，通过slam(Simultaneous Localization and Ma卵ing，即时 定位与地图构建)建立二维栅格地图，将移动机器人走过的位置设有为0,没走过的位置设 为1。基于已知障碍物环境信息建立的二维栅格地图具有简单、易于实现和维护的优点。
[0063] S12:在二维栅格地图中，W移动机器人所在位置建立全局坐标系，设置移动机器 人的起点和终点。
[0064] S13:采用跳点捜索算法确定起点到终点之间的最短路径;最短路径包括若干顺序 相连的局部目标点。跳点捜索（即Jump Point Search，简称肝S)算法生成并扩展的节点数 量很少，而且到达目标的速度很快.因为跳点捜索能够消除路径间的对称性，通过在直线和 对角线方向上修剪节点来识别后继，在捜索时跳过了大量可能会添加到open列表和closed 列表中的中间节点W及其他计算，运使捜索速度有了很大提升，且其过程中节点存储量少。
[0065] S14:控制移动机器人向每一局部目标点移动过程中，采用局部避障算法避开动态 障碍物。其中，动态障碍物是在二维栅格地图中除已经障碍物W外的其他障碍物，在移动机 器人移动过程中通过局部避障算法可避开动态障碍物，保证移动机器人的正常工作。
[0066] 本实施例所提供的路径规划和避障方法，通过在已知障碍物环境信息采用跳点捜 索算法快速获取从起点到终点的最短路径，可提高路径捜索效率、节省移动机器人路径规 划时间，且其过程中所需的存储空间较小；在控制移动机器人向每一局部目标点移动过程 中，采用局部避障算法实时避开动态障碍物，W保证移动机器人路径规划和避障的准确性 和实时性，实现移动机器人的自主导航。
[0067] 实施例2
[0068] 图2示出本实施例中的移动机器人路径规划和避障方法。如图2所示，该移动机器 人路径规划和避障方法包括如下步骤：
[0069] S21:利用已知障碍物环境信息建立二维栅格地图。本实施例中，采用二维数组存 储已经障碍物环境信息并建立二维栅格地图，在二维栅格地图中，1表示有障碍物的障碍物 栅格，0表示无障碍物的自由栅格。其中，二维栅格地图可W通过人工预先输入。可W理解 地，也可结合建图和定位技术，通过slam(Simultaneous Localization and Ma卵ing，即时 定位与地图构建)建立二维栅格地图，将移动机器人走过的位置设有为0,没走过的位置设 为1。基于已知障碍物环境信息建立的二维栅格地图具有简单、易于实现和维护的优点。
[0070] S22:在二维栅格地图中，W移动机器人所在位置建立全局坐标系，设置移动机器 人的起点和终点。
[0071] S23:采用跳点捜索算法确定起点到终点之间的最短路径;最短路径包括若干顺序 相连的局部目标点。在跳点捜索算法中采用代价估计函数f (n) = g(n)+h(n)，其中，f (n)是 起点到终点的移动耗费，g(n)是当前节点到起点的移动耗费，h(n)是当前节点到终点的移 动耗费。如图3所示，跳点捜索算法具体包括如下步骤：
[0072] S231:建立open列表和close列表;open列表用于存放未遍历访问的节点；close列 表用于存放已经遍历访问过的节点。
[0073] S232:将移动机器人的起点放入open列表。
[0074] S233:遍历起点的上、下、左、右、左上、左下、右上和右下八个方向的子节点。
[00巧]S234:判断子节点是否已在open列表或close列表中。
[0076] 若子节点已在open列表中，则重新计算所述子节点的f (n)值，判断子节点的f (n) 值是否减小;若子节点的f(n)值减小，则更新open列表中子节点的f(n)值和父节点。
[0077] 若子节点已在close列表中，则重新计算子节点的f(n)值，判断子节点的f(n)值是 否减小;若子节点的f(n)值减小，将子节点从close列表中移除，重新更新子节点的f(n)值 和父节点，将子节点加入open列表中。
[0078] 若子节点不在open列表或close列表，则将子节点放入open列表中。
[0079] S235:从open列表中选取f(n)值最小的节点，并将f(n)值最小的节点的父节点放 入close列表，跳转到S236执行。
[0080] S236:判断所述选取f(n)值最小的节点是否为移动机器人的终点；若是，则返回最 短路径，最短路径中的节点为局部目标点;若否，则将所述f(n)值最小的节点的父节点放入 close列表，并跳转到S237处理。
[0081] S237:对f(n)值最小的节点分别进行水平方向捜索、垂直方向捜索和对角线方向 捜索，W捜索跳点；其中，所述对角线方向捜索包括水平方向捜索和垂直方向捜索;待各方 向捜索结束，跳转到S235继续执行。
[0082] 具体地，水平方向捜索是指从一节点开始同时向左右两个方向上的节点进行查 找，直到遇到跳点、障碍物或地图边界才停止水平方向捜索，并将跳点或空值返回的捜索过 程。相应地，垂直方向捜索是从一节点开始同时向上下两个方向上的节点进行查找，直到遇 到跳点、障碍物或地图边界才停止垂直方向捜索，并将跳点或空值返回的捜索过程。如图4 所示，设bl为当前捜索节点，从左向右进行捜索。若bl为障碍物，则结束捜索。若bl为终点， 贝IJ结束捜索并返邮1节点。若bl为自由栅格，则判断Cl和c2,若Cl为障碍物，c2为自由栅格， 则添加 C2节点。同理，若al为障碍物，曰2为自由栅格，则添加曰2节点。若有曰2或c2节点被添 加，则继续添加 b2节点，结束捜索。若都没节点添加，则继续向右捜索。垂直方向捜索与水平 方向捜索的过程一致，在此不一一寶述。
[0083] 具体地，对角线方向捜索是指从一节点开始同时向对角线方向的节点进行查找， 直到遇到跳点、障碍物或地图边界才停止对角线方向捜索，并将跳点或空值返回的捜索过 程，该对角线方向捜索包括水平方向捜索和垂直方向捜索。如图5所示，Cl为当前节点，向右 上方对角方向捜索。若Cl为障碍物栅格，则结束对角线捜索。若Cl为终点，则结束捜索并返 回Cl节点。若Cl为自由栅格，则判断bl和al节点，若bl为障碍物，al为自由栅格，则将a巧口曰2 加入open表，同理，判断c2和c3，若c2为障碍物，c3为自由栅格，则将c3和b3加入open表，基 于前面两个判断，若有节点添加 open表，则将a3也添加 open表。若都无节点添加，则向上方 向采用垂直方向捜索、向左方向采用水平方向捜索及对角线方向采用对角线方向捜索继续 捜索跳点。
[0084] 可W理解地，该跳点捜索法中的每一节点的节点数据包含节点的行列下标、下个 捜索方向和其父节点。捜索过程中，从open表中选取代价估计函数f(n)值最小的节点，对该 节点的下个捜索方向分别进行水平方向捜索、垂直方向捜索和对角线方向捜索，W捜索跳 点，将捜索到的跳点添加入open表，并将该节点添入close表。重复上述捜索过程，不断从 open表中取出节点，进行跳点捜索，直至取到终点，停止捜索，返回路径节点数组，该路径节 点数组为最短路径，最短路径中的每一节点为一局部目标点。相比A*算法，跳点捜索法可同 时采用水平方向捜索、垂直方向捜索和对角线方向捜索，消除地图中大量的对称节点，提高 捜索效率，且可有效减少节点存储量，节省存储空间。
[0085] 本实施例中的跳点捜索法，只需遍历起点的8个方向的子节点一次，将8个方向的 栅格节点都添加到open列表中，添加过程采用节点类进行添加，方向通过V和H进行设置(比 如V=I ,H= 1，则表示右上方方向。若V=I，H=0,则表示右边水平方向），计算其各自的代价 估计函数f(n)，并按代价估计函数f(n)从小到大进行排列，然后将起点放入close列表。然 后分别进行水平方向捜索、垂直方向捜索和对角线方向捜索，W获取跳点进行添加查找。其 中，节点类表示如下：
[008W class Node-; private int X; /VX 化争示 private int y; //Y 坐标 private Node parent Node;//父类节点 pri vate i nt H，V; //搜索方向
[0087] 如vate int装 /链滿带点到起点的移动雜葬 private int h; //当前节点到终点的移动耗费 priyate int f; //f=g+k
[0088] 如图6所示，图中节点0为起点，从节点0开始进行水平方向捜索，查找到的跳点有 节点10和节点11，垂直方向捜索没有查找到跳点，右上方对角线方向捜索查找到的跳点有 节点12。将运些跳点添加到open列表并按代价函数f(n)的大小进行排列，作为下一次捜索 的候选节点。同时对角线方向捜索时，假如当节点2和节点3为障碍物栅格时，贝怀对节点8 进行对角线方向捜索，W保证机器人不会穿过障碍物运动。同时若水平方向捜索和垂直方 向捜索均没有捜索到跳点，将地图边界栅格添加入open列表。如图7所示，若起点为（1，10)， 终点为(47,30)，图中白色栅格为自由栅格，黑色栅格为障碍物栅格，图7示出执行本实施例 的跳点捜索算法的结果。
[0089] S24:控制移动机器人向每一局部目标点移动过程中，采用局部避障算法避开动态 障碍物。其中，动态障碍物是在二维栅格地图中除已经障碍物W外的其他障碍物，在移动机 器人移动过程中通过局部避障算法可避开动态障碍物，保证移动机器人的正常工作。步骤 S24具体包括如下步骤：
[0090] S241:实时检测动态障碍物，获取动态障碍物相对于移动机器人的实时距离和方 位角度。在移动机器人向局部目标点移动过程中，采用超声波传感器实时检测是否存在不 是已经障碍物的动态障碍物，若是检测到存在运样的动态障碍物，即获取动态障碍物相对 于移动机器人的实时距离和方位角度，W便于控制移动机器人转动，W避开该动态障碍物。 可W理解地，采用超声波传感器检测动态障碍物，具有低成本、检测精度高且不受环境光线 强弱的限制的优点。
[0091] S242:根据实时距离和方位角度，控制移动机器人转动，W避开动态障碍物，W达 到局部避障的目的。步骤S242具体包括如下步骤：
[0092] S2421:预设危险规则库，所述危险规则库包括若干距离范围阔值和与所述距离范 围阔值相应的避障转动角度。本实施例中，确定50cm、40cm和IOcm几个阔值，设X为距离范围 阔值;若X含50cm，则说明较为安全，不进行处理;若40兰x<50,则说明危险系数较小，其相应 的避障转动角度较小;若10兰x<40,则说明危险系数中等，其相应的避障转动角度也设为中 等值;若X兰10,则说明危险较大，其相应的避障转动角度较大。
[0093] S2422:根据实时距离确定其所属的距离范围阔值，并获取与距离范围阔值相应的 避障转动角度。可W理解地，根据超声波传感器检测到的动态障碍物距离移动机器人的实 时距离，判断该实时距离所在的距离范围阔值，并获取避障转动角度，W便于控制移动机器 人工作。
[0094] S2423:控制移动机器人转动避障转动角度，W避开动态障碍物。
[00M] S243:实时获取移动机器人的移动距离和转动角度。本实施例中，采用光流传感器 和角速度传感器检测移动机器人在室内移动的移动距离和转动角度，进行机器人当前位置 更新和调整，保证路径规划的实时性的准确性，W保障精确控制移动机器人在移动过程中 避免已知障碍物和动态障碍物，实现自主导航。光流传感器和角速度传感器成本较低，实际 应用中可采用多个传感器进行检测，将获取的数据进行融合处理，提高数据的准确度。
[0096] 可W理解地，本实施例中的移动机器人设有控制处理器、超声波传感器、光流传感 器和角速度传感器和用于实现移动的电机部件。其中，控制处理器用于输入和存储二维栅 格地图，运行跳点捜索算法和局部避障算法，接收各传感器的检测数据，并控制机器人移 动。
[0097] 本实施例所提供的路径规划和避障方法，通过在已知障碍物环境信息采用跳点捜 索算法快速获取从起点到终点的最短路径，可提高路径捜索效率、节省移动机器人路径规 划时间，且其过程中所需的存储空间较小。在控制移动机器人向每一局部目标点移动过程 中，采用超声波传感器实现局部避障，采用光流传感器和角速度传感器获取移动机器人的 移动距离和转动角度，结合航迹推演对移动机器人的位置进行实时更新调整。可W理解地， 采用局部避障算法实时避开动态障碍物，W保证移动机器人路径规划和避障的准确性和实 时性，实现移动机器人在复杂环境下的自主导航。
[009引实施例3
[0099] 图8示出本实施例中的移动机器人路径规划和避障系统。如图8所示，该移动机器 人路径规划和避障系统包括：
[0100] 地图建立单元10:用于利用已知障碍物环境信息建立二维栅格地图。本实施例中， 采用二维数组存储已经障碍物环境信息，并建立二维栅格地图，在二维栅格地图中，1表示 有障碍物的障碍物栅格，0表示无障碍物的自由栅格。其中，二维栅格地图可W通过人工预 先输入。可W理解地，也可结合建图和定位技术，通过slam(Simultaneous Localization and Mapping,即时定位与地图构建)建立二维栅格地图，将移动机器人走过的位置设有为 0,没走过的位置设为1。基于已知障碍物环境信息建立的二维栅格地图具有简单、易于实现 和维护的优点。
[0101 ]坐标建立单元20:用于在二维栅格地图中，W移动机器人所在位置建立全局坐标 系，设置移动机器人的起点和终点。
[0102]路径确定单元30:用于采用跳点捜索算法确定起点到终点之间的最短路径;最短 路径包括若干顺序相连的局部目标点。跳点捜索（即Jump Point Search,简称肝S)算法生 成并扩展的节点数量很少，而且到达目标的速度很快.因为跳点捜索能够消除路径间的对 称性，通过在直线和对角线方向上修剪节点来识别后继，在捜索时跳过了大量可能会添加 到open列表和closed列表中的中间节点W及其他计算，运使捜索速度有了很大提升，且其 过程中节点存储量少。
[0103] 移动避障单元40:用于控制移动机器人向每一局部目标点移动过程中，采用局部 避障算法避开动态障碍物。其中，动态障碍物是在二维栅格地图中除已经障碍物W外的其 他障碍物，在移动机器人移动过程中通过局部避障算法可避开动态障碍物，保证移动机器 人的正常工作。
[0104] 本实施例所提供的路径规划和避障方法，通过地图建立单元10利用已知障碍物环 境信息建立二维栅格地图；再采用坐标建立单元20建立全局坐标系，设置移动机器人的起 点和终点；再通过路径确定单元30采用跳点捜索算法快速获取从起点到终点的最短路径， 可提高路径捜索效率、节省移动机器人路径规划时间，且其过程中所需的存储空间较小;然 后通过移动避障单元40控制移动机器人向每一局部目标点移动过程中，采用局部避障算法 实时避开动态障碍物，W保证移动机器人路径规划和避障的准确性和实时性，实现移动机 器人的自主导航。
[0105] 实施例4
[0106] 图9示出本实施例中的移动机器人路径规划和避障系统。如图9所示，该移动机器 人路径规划和避障系统包括：
[0107] 地图建立单元10:用于利用已知障碍物环境信息建立二维栅格地图。本实施例中， 采用二维数组存储已经障碍物环境信息，并建立二维栅格地图，在二维栅格地图中，1表示 有障碍物的障碍物栅格，0表示无障碍物的自由栅格。其中，二维栅格地图可W通过人工预 先输入。可W理解地，也可结合建图和定位技术，通过slam(Simultaneous Localization and Mapping,即时定位与地图构建)建立二维栅格地图，将移动机器人走过的位置设有为 0,没走过的位置设为1。基于已知障碍物环境信息建立的二维栅格地图具有简单、易于实现 和维护的优点。
[0108] 坐标建立单元20:用于在二维栅格地图中，W移动机器人所在位置建立全局坐标 系，设置移动机器人的起点和终点。
[0109] 路径确定单元30:用于采用跳点捜索算法确定起点到终点之间的最短路径;最短 路径包括若干顺序相连的局部目标点。跳点捜索（即Jump Point Search,简称肝S)算法生 成并扩展的节点数量很少，而且到达目标的速度很快.因为跳点捜索能够消除路径间的对 称性，通过在直线和对角线方向上修剪节点来识别后继，在捜索时跳过了大量可能会添加 到open列表和closed列表中的中间节点W及其他计算，运使捜索速度有了很大提升，且其 过程中节点存储量少。在跳点捜索算法中采用代价估计函数f(n)=g(n)+h(n)，其中，f(n) 是起点到终点的移动耗费，g(n)是当前节点到起点的移动耗费，h(n)是当前节点到终点的 移动耗费。路径确定单兀30包括：
[0110] 列表建立模块31，用于建立open列表和close列表;open列表用于存放未遍历访问 的节点；close列表用于存放已经遍历访问过的节点。
[0111] 起点设置模块32，用于将移动机器人的起点放入open列表。
[0112] 子节点遍历模块33,用于遍历起点的上、下、左、右、左上、左下、右上和右下八个方 向的子节点。
[0113] 子节点判断模块34,用于判断子节点是否已在open列表或close列表中。若子节点 已在open列表中，则重新计算所述子节点的f(n)值，判断子节点的f(n)值是否减小;若子节 点的f (n)值减小，则更新open列表中子节点的f (n)值和父节点。
[0114] 若子节点已在close列表中，则重新计算子节点的f(n)值，判断子节点的f(n)值是 否减小;若子节点的f(n)值减小，将子节点从close列表中移除，重新更新子节点的f(n)值 和父节点，将子节点加入open列表中。
[0115] 若子节点不在open列表或close列表，则将子节点放入open列表中。
[0116] 节点选取模块35,用于从open列表中选取f(n)值最小的节点，并将f(n)值最小的 节点的父节点放入C1 OS e列表.，跳转到路径确定模块36，进行处理。
[0117] 路径确定模块36,用于判断所述选取f(n)值最小的节点是否为移动机器人的终 点;若是，则返回最短路径，最短路径中的节点为局部目标点;若否，则将所述f(n)值最小的 节点的父节点放入close列表，并跳转到节点捜索模块37,进行处理。
[0118] 节点捜索模块37,用于对f(n)值最小的节点分别进行水平方向捜索、垂直方向捜 索和对角线方向捜索，W捜索跳点；其中，所述对角线方向捜索包括水平方向捜索和垂直方 向捜索;待各方向捜索结束，跳转到节点选取模块35,继续处理。
[0119] 具体地，水平方向捜索是指从一节点开始同时向左右两个方向上的节点进行查 找，直到遇到跳点、障碍物或地图边界才停止水平方向捜索，并将跳点或空值返回的捜索过 程。相应地，垂直方向捜索是从一节点开始同时向上下两个方向上的节点进行查找，直到遇 到跳点、障碍物或地图边界才停止垂直方向捜索，并将跳点或空值返回的捜索过程。如图4 所示，设bl为当前捜索节点，从左向右进行捜索。若bl为障碍物，则结束捜索。若bl为终点， 贝IJ结束捜索并返邮1节点。若bl为自由栅格，则判断Cl和c2,若Cl为障碍物，c2为自由栅格， 则添加 C2节点。同理，若al为障碍物，曰2为自由栅格，则添加曰2节点。若有曰2或c2节点被添 加，则继续添加 b2节点，结束捜索。若都没节点添加，则继续向右捜索。垂直方向捜索与水平 方向捜索的过程一致，在此不一一寶述。
[0120] 具体地，对角线方向捜索是指从一节点开始同时向对角线方向的节点进行查找， 直到遇到跳点、障碍物或地图边界才停止对角线方向捜索，并将跳点或空值返回的捜索过 程，该对角线方向捜索包括水平方向捜索和垂直方向捜索。如图5所示，Cl为当前节点，向右 上方对角方向捜索。若Cl为障碍物栅格，则结束对角线捜索。若Cl为终点，则结束捜索并返 回Cl节点。若Cl为自由栅格，则判断bl和al节点，若bl为障碍物，al为自由栅格，则将a巧口曰2 加入open表，同理，判断c2和c3，若c2为障碍物，c3为自由栅格，则将c3和b3加入open表，基 于前面两个判断，若有节点添加 open表，则将a3也添加 open表。若都无节点添加，则向上方 向采用垂直方向捜索、向左方向采用水平方向捜索及对角线方向采用对角线方向捜索继续 捜索跳点。
[0121] 可W理解地，该跳点捜索法中的每一节点的节点数据包含节点的行列下标、下个 捜索方向和其父节点。捜索过程中，从open表中选取代价估计函数f(n)值最小的节点，对该 节点的下个捜索方向分别进行水平方向捜索、垂直方向捜索和对角线方向捜索，W捜索跳 点，将捜索到的跳点添加入open表，并将该节点添入close表。重复上述捜索过程，不断从 open表中取出节点，进行跳点捜索，直至取到终点，停止捜索，返回路径节点数组，该路径节 点数组为最短路径，最短路径中的每一节点为一局部目标点。相比A*算法，跳点捜索法可同 时采用水平方向捜索、垂直方向捜索和对角线方向捜索，消除地图中大量的对称节点，提高 捜索效率，且可有效减少节点存储量，节省存储空间。
[0122] 本实施例中的跳点捜索法，只需遍历起点的8个方向的子节点一次，将8个方向的 栅格节点都添加到open列表中，添加过程采用节点类进行添加，方向通过V和H进行设置(比 如V=I ,H= 1，则表示右上方方向。若V=I，H=0,则表示右边水平方向），计算其各自的代价 估计函数f(n)，并按代价估计函数f(n)从小到大进行排列，然后将起点放入close列表。然 后分别进行水平方向捜索、垂直方向捜索和对角线方向捜索，W获取跳点进行添加查找。其 中，节点类表示如下： class Node{ private rnt x; 坐标 private int y; //Y 坐标 private Node parent Node;//SC 1'J/I
[0123] private int H,V; //搜索方向 private mt g; 公当煎节点到起点的移动耗费 private int h; 众当前唯慮到終煮的移动耗费 private int f; //f=g+h
[0124] I
[0125] 如图6所示，图中节点0为起点，从节点0开始进行水平方向捜索，查找到的跳点有 节点10和节点11，垂直方向捜索没有查找到跳点，右上方对角线方向捜索查找到的跳点有 节点12。将运些跳点添加到open列表并按代价函数f(n)的大小进行排列，作为下一次捜索 的候选节点。同时对角线方向捜索时，假如当节点2和节点3为障碍物栅格时，贝怀对节点8 进行对角线方向捜索，W保证机器人不会穿过障碍物运动。同时若水平方向捜索和垂直方 向捜索均没有捜索到跳点，将地图边界栅格添加入open列表。如图7所示，若起点为（1，10)， 终点为(47,30)，图中白色栅格为自由栅格，黑色栅格为障碍物栅格，图7示出执行本实施例 的跳点捜索算法的结果。
[01%] 移动避障单元40:用于控制移动机器人向每一局部目标点移动过程中，采用局部 避障算法避开动态障碍物。其中，动态障碍物是在二维栅格地图中除已经障碍物W外的其 他障碍物，在移动机器人移动过程中通过局部避障算法可避开动态障碍物，保证移动机器 人的正常工作。移动避障单元40具体包括：
[0127] 障碍物检测块41，用于实时检测动态障碍物，获取动态障碍物相对于移动机器人 的实时距离和方位角度。在移动机器人向局部目标点移动过程中，采用超声波传感器实时 检测是否存在不是已经障碍物的动态障碍物，若是检测到存在运样的动态障碍物，即获取 动态障碍物相对于移动机器人的实时距离和方位角度，W便于控制移动机器人转动，W避 开该动态障碍物。可W理解地，采用超声波传感器检测动态障碍物，具有低成本、检测精度 高且不受环境光线强弱的限制的优点。
[0128] 避障控制模块42，用于根据实时距离和方位角度，控制移动机器人转动，W避开动 态障碍物，W达到局部避障的目的。避障控制模块42具体包括：
[0129] 预设范围子模块421，用于预设危险规则库，所述危险规则库包括若干距离范围阔 值和与所述距离范围阔值相应的避障转动角度。本实施例中，确定50cm、40cm和IOcm几个阔 值，设X为距离范围阔值;若x^50cm，则说明较为安全，不进行处理;若40^x<50,则说明危 险系数较小，其相应的避障转动角度较小;若10自x<40,则说明危险系数中等，其相应的避 障转动角度也设为中等值;若X兰10,则说明危险较大，其相应的避障转动角度较大。
[0130] 避障确定子模块422,用于根据实时距离确定其所属的距离范围阔值，并获取与距 离范围阔值相应的避障转动角度。可W理解地，根据超声波传感器检测到的动态障碍物距 离移动机器人的实时距离，判断该实时距离所在的距离范围阔值，并获取避障转动角度，W 便于控制移动机器人工作。
[0131] 转动控制子模块423,用于控制移动机器人转动避障转动角度，W避开动态障碍 物。
[0132] 状态检测模块43,用于实时获取移动机器人的移动距离和转动角度。本实施例中， 采用光流传感器和角速度传感器检测移动机器人在室内移动的移动距离和转动角度，进行 机器人当前位置更新和调整，保证路径规划的实时性的准确性，W保障精确控制移动机器 人在移动过程中避免已知障碍物和动态障碍物，实现自主导航。光流传感器和角速度传感 器成本较低，实际应用中可采用多个传感器进行检测，将获取的数据进行融合处理，提高数 据的准确度。
[0133] 可W理解地，本实施例中的移动机器人设有控制处理器、超声波传感器、光流传感 器和角速度传感器和用于实现移动的电机部件。其中，控制处理器用于输入和存储二维栅 格地图，运行跳点捜索算法和局部避障算法，接收各传感器的检测数据，并控制机器人移 动。
[0134] 本实施例所提供的路径规划和避障方法，通过地图建立单元10利用已知障碍物环 境信息建立二维栅格地图；再采用坐标建立单元20建立全局坐标系，设置移动机器人的起 点和终点；再通过路径确定单元30采用跳点捜索算法快速获取从起点到终点的最短路径， 可提高路径捜索效率、节省移动机器人路径规划时间，且其过程中所需的存储空间较小;然 后通过移动避障单元40控制移动机器人向每一局部目标点移动过程中，采用局部避障算法 实时避开动态障碍物，W保证移动机器人路径规划和避障的准确性和实时性，实现移动机 器人的自主导航。其中，在移动避障单元40中采用超声波传感器实现局部避障，采用光流传 感器和角速度传感器获取移动机器人的移动距离和转动角度，结合航迹推演对移动机器人 的位置进行实时更新调整。可W理解地，采用局部避障算法实时避开动态障碍物，W保证移 动机器人路径规划和避障的准确性和实时性，实现移动机器人在复杂环境下的自主导航。
[0135] 本发明是通过几个具体实施例进行说明的，本领域技术人员应当明白，在不脱离 本发明范围的情况下，还可W对本发明进行各种变换和等同替代。另外，针对特定情形或具 体情况，可W对本发明做各种修改，而不脱离本发明的范围。因此，本发明不局限于所公开 的具体实施例，而应当包括落入本发明权利要求范围内的全部实施方式。
【主权项】
1. 一种移动机器人路径规划和避障方法，其特征在于，包括： 利用已知障碍物环境信息建立二维栅格地图； 在所述二维栅格地图中，以移动机器人所在位置建立全局坐标系，设置所述移动机器 人的起点和终点； 采用跳点搜索算法确定所述起点到所述终点之间的最短路径;所述最短路径包括若干 顺序相连的局部目标点； 控制所述移动机器人向每一所述局部目标点移动过程中，采用局部避障算法避开动态 障碍物。2. 根据权利要求1所述的移动机器人路径规划和避障方法，其特征在于，采用跳点搜索 算法确定所述起点到所述终点之间的最短路径，包括： 建立open列表和close列表;open列表用于存放未遍历访问的节点；close列表用于存 放已经遍历访问过的节点； 将所述移动机器人的起点放入open列表； 遍历所述起点的上、下、左、右、左上、左下、右上和右下八个方向的子节点； 判断所述子节点是否已在open列表或close列表中：若所述子节点已在open列表中，则 重新计算所述子节点的f(n)值，判断所述子节点的f(n)值是否减小;若所述子节点的f (η) 值减小，则更新open列表中所述子节点的f (η)值和父节点；若所述子节点已在close列表 中，则重新计算所述子节点的f(n)值，判断所述子节点的f(n)值是否减小;若所述子节点的 f (η)值减小，将所述子节点从c 1 〇se列表中移除，重新更新所述子节点的f (η)值和父节点， 将所述子节点加入open列表中；若所述子节点不在open列表或close列表，则将所述子节点 放入open列表中； 从open列表中选取f (η)值最小的节点并将所述f (η)值最小的节点的父节点放入close 列表； 判断所述f(n)值最小的节点是否为移动机器人的终点；若是，则返回最短路径，最短路 径中的节点为局部目标点；若否，则将所述f (η)值最小的节点的父节点放入close列表，并 跳转到对所述f (η)值最小的节点分别进行水平方向搜索、垂直方向搜索和对角线方向搜 索，以搜索跳点； 对所述f(n)值最小的节点分别进行水平方向搜索、垂直方向搜索和对角线方向搜索， 以搜索跳点；其中，所述对角线方向搜索包括水平方向搜索和垂直方向搜索;待各方向搜索 完毕，跳转到从open列表中选取f (η)值最小的节点，继续处理。3. 根据权利要求1所述的移动机器人路径规划和避障方法，其特征在于，所述采用局部 避障算法避开动态障碍物，包括： 实时检测所述动态障碍物，获取所述动态障碍物相对于所述移动机器人的实时距离和 方位角度； 根据所述实时距离和方位角度，控制所述移动机器人转动，以避开所述动态障碍物。4. 根据权利要求3所述的移动机器人路径规划和避障方法，其特征在于，所述根据所述 实时距离和方位角度，控制所述移动机器人转动，以避开所述动态障碍物，包括： 预设若干距离范围阈值，每一距离范围阈值与一避障转动角度相应； 根据所述实时距离确定其所属的距离范围阈值及对应的避障转动角度； 控制所述移动机器人转动所述避障转动角度，以避开所述动态障碍物。5. 根据权利要求1所述的移动机器人路径规划和避障方法，其特征在于，所述采用局部 避障算法避开动态障碍物，还包括:实时获取所述移动机器人的移动距离和转动角度。6. -种移动机器人路径规划和避障系统，其特征在于，包括： 地图建立单元:用于利用已知障碍物环境信息建立二维栅格地图； 坐标建立单元：用于在所述二维栅格地图中，以移动机器人所在位置建立全局坐标系， 设置所述移动机器人的起点和终点； 路径确定单元：用于采用跳点搜索算法确定所述起点到所述终点之间的最短路径;所 述最短路径包括若干顺序相连的局部目标点； 移动避障单元：用于控制所述移动机器人向每一所述局部目标点移动过程中，采用局 部避障算法避开动态障碍物。7. 根据权利要求6所述的移动机器人路径规划和避障系统，其特征在于，所述路径确定 单元包括： 列表建立模块：用于建立open列表和c lose列表;open列表用于存放未遍历访问的节 点；close列表用于存放已经遍历访问过的节点； 起点设置模块:用于将所述移动机器人的起点放入open列表； 子节点遍历模块：用于遍历所述起点的上、下、左、右、左上、左下、右上和右下八个方向 的子节点； 子节点判断模块:用于判断所述子节点是否已在open列表或close列表中：若所述子节 点已在open列表中，则重新计算所述子节点的f (η)值，判断所述子节点的f (η)值是否减小； 若所述子节点的f (η)值减小，则更新open列表中所述子节点的f (η)值和父节点；若所述子 节点已在close列表中，则重新计算所述子节点的f (η)值，判断所述子节点的f (η)值是否减 小;若所述子节点的f (η)值减小，将所述子节点从c 1 〇 se列表中移除，重新更新所述子节点 的f(n)值和父节点，将所述子节点加入open列表中；若所述子节点不在open列表或close列 表，则将所述子节点放入open列表中； 节点选取模块：用于从open列表中选取f (η)值最小的节点并将所述f (η)值最小的节点 的父节点放入close列表； 路径确定模块:用于判断所述f(n)值最小的节点是否为移动机器人的终点;若是，则返 回最短路径，最短路径中的节点为局部目标点;若否，则将所述f (η)值最小的节点的父节点 放入close列表，并跳转到节点搜索模块。 节点搜索模块：用于对所述f (η)值最小的节点分别进行水平方向搜索、垂直方向搜索 和对角线方向搜索，以搜索跳点;其中，所述对角线方向搜索包括水平方向搜索和垂直方向 搜索;待各方向搜索完毕，跳转到节点选取模块，继续处理。8. 根据权利要求6所述的移动机器人路径规划和避障系统，其特征在于，所述移动避障 单元包括： 障碍物检测模块：用于实时检测所述动态障碍物，获取所述动态障碍物相对于所述移 动机器人的实时距离和方位角度； 避障控制模块：用于根据所述实时距离和方位角度，控制所述移动机器人转动，以避开 所述动态障碍物。9. 根据权利要求8所述的移动机器人路径规划和避障系统，其特征在于，所述避障控制 丰吴块包括： 预设范围子模块：用于预设若干距离范围阈值，每一距离范围阈值与一避障转动角度 相应； 避障确定子模块：用于根据所述实时距离确定其所属的距离范围阈值及对应的避障转 动角度； 转动控制子模块：用于控制所述移动机器人转动所述避障转动角度，以避开所述动态 障碍物。10. 根据权利要求6所述的移动机器人路径规划和避障系统，其特征在于，所述移动避 障单元还包括:状态检测模块，用于实时获取所述移动机器人的移动距离和转动角度。
【文档编号】G05D1/02GK105955280SQ201610573390
【公开日】2016年9月21日
【申请日】2016年7月19日
【发明人】张海燕
【申请人】Tcl集团股份有限公司