CN113219975B - 一种路线优化方法、路径规划方法、芯片及机器人 - Google Patents
一种路线优化方法、路径规划方法、芯片及机器人 Download PDFInfo
- Publication number
- CN113219975B CN113219975B CN202110502487.3A CN202110502487A CN113219975B CN 113219975 B CN113219975 B CN 113219975B CN 202110502487 A CN202110502487 A CN 202110502487A CN 113219975 B CN113219975 B CN 113219975B
- Authority
- CN
- China
- Prior art keywords
- node
- route
- path
- nodes
- grid
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 163
- 238000005457 optimization Methods 0.000 title claims abstract description 43
- 238000012216 screening Methods 0.000 claims abstract description 103
- 238000004891 communication Methods 0.000 claims description 207
- 238000004422 calculation algorithm Methods 0.000 claims description 38
- 230000008569 process Effects 0.000 claims description 36
- 230000004888 barrier function Effects 0.000 claims description 26
- 238000010845 search algorithm Methods 0.000 claims description 21
- 238000011156 evaluation Methods 0.000 claims description 18
- 230000001965 increasing effect Effects 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000005304 joining Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 abstract description 7
- 238000009826 distribution Methods 0.000 description 17
- 208000031481 Pathologic Constriction Diseases 0.000 description 6
- 230000008901 benefit Effects 0.000 description 5
- 230000036262 stenosis Effects 0.000 description 5
- 208000037804 stenosis Diseases 0.000 description 5
- 238000003860 storage Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 4
- 238000001514 detection method Methods 0.000 description 3
- 230000002349 favourable effect Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000004590 computer program Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000010408 sweeping Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 241001122767 Theaceae Species 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开公开一种路线优化方法、路径规划方法、芯片及机器人,机器人在狭窄区域内运动搜索局部路线的基础上,先后以随机生成的自由栅格点、局部路线的起止点为搜索中心,基于不同筛选条件下,基于不同筛选条件下往已构建的地图区域内搜索可通行的自由栅格点,增加困难区域内分布的自由栅格点的采样数目,使得规划出的可行路线结合所述局部路线尽可能地覆盖到相应地图区域的所有自由栅格区域,进而互连形成更加完备的路线网络。
Description
技术领域
本发明涉及机器人路径规划的技术领域,涉及一种基于栅格地图的路线优化方法、路径规划方法、芯片及机器人。
背景技术
路径规划是指在某种环境内,按照一定的评价标准,如路径最短或规划时间最少等,寻找一条从起点到终点的无碰撞的路径。
随机路径图法(Probabilistic Roadmaps,PRM),是一种使用较为普遍的路图方法,随机路图方法的一个巨大优点在于,其复杂度主要依赖于寻找路径的难度,跟整个规划环境的复杂度和规划空间的维数基本无关,实际操作性较强,适合工程上应用。
随机路径图法(Probabilistic Roadmaps,PRM),相较传统图搜索算法不仅搜索速度更快,而且可以重复利用;然而当障碍物较多或者采样点太少,分布又不合理时,PRM算法就有可能导致路径规划失败,因此PRM算法是概率不完备的。具体地,在一个规划空间中,包括困难区域、正常区域以及障碍物区域,其中,困难区域可分为空旷区域以及障碍物之间的夹缝即狭窄通;由于一般PRM方法是完全等概率地均匀生成自由节点,容易使得空旷区域中生成的随机自由节点数目不够,造成路线图的不完备性;且使得狭窄区域内的自由节点分布不理想,造成某些可行路径的遗漏,往往造成规划出的路线是不完备的、在相应区域内的覆盖率不高。
总之,现有技术采用一般PRM算法进行路径规划的过程中,由于机器人存在打滑情况、用于定位的传感器存在累计误差或视觉地图优化也产生误差,所以机器人容易在已构建的栅格地图上将这些狭窄区域标记为障碍物占据的区域,而实际运动场景中没有被真正堵住,从而不能准确完整地搜寻到通过该困难区域的可行路径,由于规划空间中的狭窄区域内的自由节点数目分布不完整,所以易遗漏某些真正可通行路径而导致导航脱困失败。
发明内容
为了解决上述技术问题,本发明技术方案公开一种路线优化方法、路径规划方法、芯片及机器人,机器人在狭窄区域内移动搜索局部路线的基础上,先后以随机生成的自由栅格点、局部路线的起止点为搜索中心,基于不同筛选条件下往已构建的地图区域内搜索可通行的自由栅格点,增加困难区域内分布的自由栅格点的采样数目,使得规划出的可行路线结合所述局部路线尽可能地覆盖到相应地图区域的所有自由栅格区域,进而形成更加完备的路线网络。具体技术方案如下:
一种路线优化方法,包括:首先控制机器人沿着预设路径移动并规划出至少一条局部路线;其中,预设路径是预先设置的支持机器人通过狭窄区域的路径;然后,在机器人已构建的栅格地图内随机搜索出第一预设数量的满足第一连通筛选条件的栅格点;分别以所述满足第一连通筛选条件的栅格点、所有局部路线的起点及其终点为第一搜索中心,搜索出满足第二连通筛选条件的栅格点,再将第一搜索中心及其搜索出的所有满足第二连通筛选条件的栅格点相连接成第一可行路线,进而确定第一可行路线与所述局部路线相连接;当所有局部路线的起点的子节点及所有局部路线的终点的子节点中都存在所述满足第一连通筛选条件的栅格点时,确定第一可行路线与所述局部路线相连接形成完备的候选导航网络;当所有局部路线的起点的子节点及所有局部路线的终点的子节点中不是都存在所述满足第一连通筛选条件的栅格点时,分别以所有局部路线的起点及其终点为第二搜索中心,搜索出满足第三连通筛选条件的栅格点,再将第二搜索中心及其搜索出的所有满足第三连通筛选条件的栅格点相连接成第二可行路线,进而确定第一可行路线、第二可行路线与所述局部路线相连接形成完备的候选导航网络。
与现有技术相比,本技术方案分别利用机器人在狭窄区域内遍历形成的局部路线、机器人已经构建的代表较为空旷区域的栅格地图内随机搜索的具备第一连通筛选条件的栅格点扩展出满足第二连通筛选条件的栅格点,并与当前的搜索中心互连成第一可行路线,以提高用于进出所述困难区域的自由栅格点的分布密度;然后在连成的第一可行路线的基础上,继续根据第一连通筛选条件的栅格点在所述局部路线的两端的邻域内的分布情况,确定是否利用局部路线的两端继续扩展出对连通性的要求更低的满足第三连通筛选条件的栅格点,并通过进一步与当前的搜索中心互连成第二可行路线,进而互连形成更完备的候选导航网络。
因此,无论是面对空旷地图区域还是障碍物分布比较复杂的狭窄区域,所述局部路线的两端连接上的栅格点越多,由前述局部路线和扩展互连形成的可行路线形成的候选导航网络更加完备,对可通行区域的覆盖率越高,更有利于机器人搜索出有效穿过狭窄区域的路径,减少遗漏通过所述困难区域的可行路线的概率,加快通过穿过狭窄区域的导航路径的规划准确度。
进一步地,所述在机器人已构建的栅格地图内随机搜索出第一预设数量的满足第一连通筛选条件的栅格点的方法具体包括:基于机器人已构建的栅格地图的尺寸与单个栅格的边长的比值,作乘积运算求取机器人已构建的栅格地图中分布所有栅格的总数量,并确定为所述第一预设数量;其中,栅格点用对应一个栅格的栅格中心点表示;以机器人的当前位置为搜索中心,在所述栅格地图内随机搜索出满足第一连通筛选条件的栅格点并加入预先创建的可行路线集合,直到随机搜索并获得所述第一预设数量的满足第一连通筛选条件的栅格点;其中,将满足第一连通筛选条件的栅格点设置为第一连通节点。该技术方案使用机器人已构建的栅格地图中分布的栅格的总数量来表示节点在栅格地图中的分布密度,符合栅格地图这一离散地图的特性;进而通过随机搜索出用于表示栅格地图中分布的栅格的总数量的第一连通节点,提高了栅格地图区域内的第一连通节点的分布密度,保证了空旷区域中自由栅格点个数的最小值,使得生成的候选导航网络得以优化,候选导航网络的完备性得到改善。
进一步地,所述第一连通筛选条件是:当前随机搜索的自由栅格点不存在于所述可行路线节点集合内,且以当前随机搜索的自由栅格点为圆心、搜索半径为一个机身直径的区域内不存在障碍物栅格点且不存在未知栅格点,使得当前随机搜索的自由栅格点为所述第一连通节点。使得所述第一连通节点附近的可探索区域内都是已探索过的自由栅格点,让已构建的栅格地图内随机搜索的第一连通节点更具备通行意义。
进一步地,所述路线优化方法还包括:将所述局部路线的起点及其终点都设置为第二连通节点;同时在所述局部路线上,将所述局部路线的起点及其终点之间的中间节点设置为第三连通节点;在预测通行节点集合内记录第二连通节点的子节点的位置信息,再将第二连通节点加入所述可行路线节点集合;并且,在同一个预测通行节点集合内记录第三连通节点的子节点的位置信息,再将第三连通节点加入所述可行路线节点集合;以使得所述可行路线节点集合内存有一个所述预测通行节点集合内用于代表对应的唯一一条局部路线的完整路径节点;所述预测通行节点集合是机器人沿着所述预设路径移动过程中创建的;其中,第二连通节点及其子节点的位置关系是相邻且相连通的;第三连通节点及其子节点的位置关系是相邻且相连通的。从而将机器人行走规划出的局部路线的路径节点逐个地由所述预测通行节点集合转移到所述可行路线节点集合内,实现基于记录的子节点的位置信息在所述可行路线节点集合内连接还原出原来的局部路线。
进一步地,所述路线优化方法还包括:当所述第二连通节点和第三连通节点加入所述可行路线节点集合时,基于记录的子节点的位置信息,将所述可行路线节点集合内的所述第二连通节点和第三连通节点按照子节点及其父节点的邻接关系依次连接为所述局部路线。其中,基于子节点的位置信息,以所述第二连通节点为起点,按照预设步长扩展新节点扩展出所述第三连通节点,再以当前扩展的所述第三连通节点为新的起点,继续扩展出后续的第三连通节点,直至扩展到新的所述第二连通节点为止,再按照先后扩展的顺序依次连接出完整的所述局部路线。
进一步地,所述分别以所述满足第一连通筛选条件的栅格点、所有局部路线的起点及其终点为第一搜索中心,搜索出满足第二连通筛选条件的栅格点,再将第一搜索中心及其搜索出的所有满足第二连通筛选条件的栅格点相连接成第一可行路线的方法具体包括:分别以所述第一连通节点和所述第二连通节点为所述第一搜索中心,在所述可行路线节点集合内搜索出满足第二连通筛选条件的栅格点,再将满足第二连通筛选条件的栅格点设置为对应的第一搜索中心的第一合法子节点并加入对应第一搜索中心的子节点集合内,其中,对应第一搜索中心的子节点集合也存在于所述可行路线节点集合内,使得每个所述第一连通节点和每个所述第二连通节点在所述可行路线节点集合内都存在一个对应的子节点集合;然后将同一个第一搜索中心和所有第一合法子节点相连接为所述第一可行路线,进而确定第一可行路线与所述局部路线相连接。
本技术方案同时在所述局部路线的起止点的周围区域内以及所述第一连通节点的周围区域内选取满足第二连通筛选条件的栅格点,不用像传统的PRM算法那样局限于关注每个自由栅格点的邻域并局限于在邻域内增加子节点,搜索效率较低;另一方面,相对于仅限于在所述狭窄区域内(所述局部路线的起止点的周围区域所代表的区域)搜索满足第二连通筛选条件的栅格点,本技术方案能够在更广的区域筛选出满足第二连通筛选条件的栅格点并记录为用于连接所述局部路线的合法子节点,这样做的好处是通过融合连接所述局部路线和所述第一可行路线,生成若干连接所述狭窄区域、空旷区域、正常区域以及障碍物区域的路线,改善所述候选导航网络在狭窄区域和空旷区域的连通性。
进一步地,第二连通筛选条件包括:与所述第一搜索中心的连线上不存在障碍物栅格点、且与所述第一搜索中心的连线长度小于预设距离阈值的栅格点是所述第一连通节点或所述第二连通节点;其中,预设距离阈值是根据具体环境情况而预先设定的;预设距离阈值大于单个栅格的边长。使得在所述可行路线节点集合对应的离散栅格区域内,所述第一搜索中心的相应距离范围内存在自由栅格点足以连接成路线,避免该区域自由栅格点的个数不会太少而无法生成符合要求的第一合法子节点。
进一步地,所述当所有局部路线的起点的子节点及所有局部路线的终点的子节点中都存在所述满足第一连通筛选条件的栅格点时,确定第一可行路线与所述局部路线相连接形成完备的候选导航网络的方法包括:若检测到所述可行路线节点集合内所有第二连通节点的子节点中都存在第一连通节点,则确定所有第二连通节点所属的局部路线的两端分别和加入所述对应第一搜索中心的子节点集合内的所有第一连通节点相连接,进而确定所述局部路线和所述第一可行路线连接形成完备的候选导航网络;其中,当前检测的第二连通节点的子节点是处于同一个第二连通节点对应的子节点集合内。
与现有技术相比,开阔区域对应的栅格地图内随机搜索出的第一连通节点,与狭窄区域内规划的局部路线,在具备合理的自由栅格点数量和连通环境后,最终形成完备的候选导航网络,弥补了一般PRM算法完全等概率均匀分布自由节点导致路图生成完备性较差的缺点,不易遗漏通过狭窄区域和空旷区域的可行路径。
进一步地,所述当所有局部路线的起点的子节点及所有局部路线的终点的子节点中不是都存在所述满足第一连通筛选条件的栅格点时,分别以所有局部路线的起点及其终点为第二搜索中心,搜索出满足第三连通筛选条件的栅格点,再将第二搜索中心及其搜索出的所有满足第三连通筛选条件的栅格点相连接成第二可行路线的方法包括:当检测到所述可行路线节点集合内所有的第二连通节点的子节点中不是都存在第一连通节点时,分别以所述可行路线节点集合内每个第二连通节点为所述第二搜索中心,然后在所述可行路线节点集合内搜索出满足第三连通筛选条件的栅格点,再将满足第三连通筛选条件的栅格点设置为对应的第二搜索中心的第三合法子节点并加入对应第二搜索中心的子节点集合内,使得每个所述第二连通节点对应的子节点集合的元素增加,然后将同一个第二搜索中心和所有第三合法子节点相连接为所述第二可行路线,进而确定所述局部路线和所述第二可行路线相连接;其中,当前检测的第二连通节点的子节点是处于同一个第二连通节点对应的子节点集合内。该技术方案相对于搜索出满足第二连通筛选条件的栅格点,限缩搜索的范围,增加所述第二连通节点对应的子节点集合的元素,相对提高第二搜索中心邻域内自由栅格点的分布概率。减少了遗漏通过所述困难区域的可行路径的概率。
进一步地,所述第三连通筛选条件包括:在与所述第二搜索中心的连线上不存在障碍物栅格点且不存在未知栅格点的所有栅格点中,与第二搜索中心的连线长度最短的栅格点是所述第一连通节点。在自由栅格点不足的区域内地图中,通过增加在所述局部路线的起止点附近的采点量来搜寻出最短距离的可通行的路径节点,降低对节点之间的连通性的标准,从而强化算法在窄通道中的路径搜索,使其更适用于复杂地形。所述第三连通筛选条件相匹配的搜索区域相对于所述第二连通筛选条件相匹配的搜索区域减小,提高路线规划的速度。
进一步地,在所述完备的候选导航网络中,所述局部路线的两端分别用于连接所述第一可行路线的一端点和/或所述第二可行路线的一端点,但是所述第一可行路线和/或所述第二可行路线都不连接上所述局部路线的中间节点;以便于利用路径搜索算法在所述完备的候选导航网络中搜索出一条由预先设置的导航起点通往预先设置的导航终点的路线;其中,所述狭窄区域是两个或两个以上障碍物的夹缝通道,夹缝通道是其中两个障碍物之间的最窄处对应的缝隙,这个缝隙宽度是大于或等于机器人的机身直径;其中,节点集合是占据内存空间的数据结构。本技术方案采取前述步骤生成的随机自由栅格点与所述局部路线的两端连接成边进而形成一个图,该图即是生成的能够覆盖狭窄区域和空旷区域的候选导航网络,作为路径搜索算法在狭窄区域内的相对完整的候选导航路线的搜索源,较好地找到了更符合要求的最优导航路径,减少了遗漏通过所述困难区域的可行路径的概率。
进一步地,所述局部路线的规划方法包括:步骤S1、控制机器人沿着预设路径移动的过程中,直至判断到预搜索区域满足第一预设圆域通行条件,再进入步骤S2;步骤S2、将机器人的当前位置记录为第一路径节点,同时创建一个新的预测通行节点集合,并将第一路径节点加入所述预测通行节点集合,然后进入步骤S3;步骤S3、每当机器人沿着所述预设路径移动一个机身直径的直线距离时,将机器人的当前位置记录为第二路径节点,再进入步骤S4;步骤S4、当判断到第二路径节点满足第二预设圆域通行条件时,将第二路径节点加入步骤S2所述的预测通行节点集合;其中,所述预测通行节点集合内存储的路径节点,按照先后加入的次序连接成对应的一条局部路线,以使得一个所述预测通行节点集合对应代表唯一的一条局部路线。
与现有技术相比,本技术方案适用于机器人正常工作状态下搜索用于经过狭窄区域的局部路线,先设置第一预设圆域通行条件作为狭窄区域的预判条件,并提供所述预测通行节点集合对应的局部路线的路径节点来源;再让机器人沿着原路径继续移动,然后设置第二预设圆域通行条件作为狭窄区域的精判条件,并继续提供所述预测通行节点集合对应的局部路线的栅格点来源,使得移动一定的距离后机器人才具备判断狭窄区域的资格,所述预测通行节点集合收集的用于连成栅格点的局部路线更加的完整,更加适应于狭窄区域下的地图栅格误差环境,为机器人提供实际可通行的路线而无需逐个关注路线上的栅格的标记信息。
进一步地,当判断到第二路径节点不满足第二预设圆域通行条件时,还包括:步骤S6、根据所述预测通行节点集合内部存有的路径节点的数目,保存所述预测通行节点集合到一个局部路线节点集合的内部,然后返回步骤S1;其中,一个局部路线节点集合存在至少一个预测通行节点集合;在同一个局部路线节点集合内,任一个所述预测通行节点集合内部的首元素及其尾元素都是唯一的,且同一个所述预测通行节点集合内部的首元素及其尾元素不是相同的。在上述技术方案的基础上,若不满足相应的预设圆域通行条件时,则停止为所述预测通行节点集合继续搜索,并确定单独一个所述预测通行节点集合内的路径节点可以连接为一条独立的局部路线。机器人在正常移动的状态下,通过迭代执行前述相关步骤来搜索出能够克服地图漂移误差的路线点集合。增加机器人在障碍物空间布局较复杂的场景中找到有效导航路径的成功率。
进一步地,所述根据所述预测通行节点集合内部存有的路径节点的数目,保存所述预测通行节点集合到同一所述局部路线节点集合的内部,然后返回步骤S1的方法具体包括:当所述预测通行节点集合内部存有的路径节点的数目小于2时,删除所述预测通行节点集合,再返回步骤S1以创建一个新的预测通行节点集合;当所述预测通行节点集合内部存有的路径节点的数目大于或等于2时,使用所述预测通行节点集合单独表示一条局部路线,并保存到所述所述局部路线节点集合内,以供路径搜索算法调用,再返回步骤S1以创建一个新的预测通行节点集合;其中,每一个预测通行节点集合内的首元素对应的路径节点是对应的一条局部路线的起点,每一个预测通行节点集合内的尾元素对应的路径节点是对应的一条局部路线的终点。使得每一个预测通行节点集合内的路径节点都匹配连接成一条局部路线,形成预测机器人在相应区域内能够无障碍通行的局部路线;在该技术方案中,若一个预测通行节点集合最终获取的路径节点的数目少到难以连接出一条线时,减少无效的路径节点;一个预测通行节点集合最终获取的路径节点作为整体保存到用于保存一套局部路线的所述所述局部路线节点集合,使得局部路线的存取结构合理有序。
进一步地,所述第一预设圆域通行条件包括:第一不可通行区域的面积与所述预搜索区域的面积的比值大于第一通行评估值;其中,第一不可通行区域是在所述第一圆形区域对应的栅格区域内,由未知栅格点和障碍物栅格点组成的栅格区域;第一通行评价值是为了克服构建栅格地图中存在的自由栅格点的标记误差而设置的预判阈值;其中,所述预搜索区域是以机器人的当前位置为圆心、机器人的机身直径为半径的第一圆形区域。可以用于初步判断机器人是否开始进入狭窄区域,属于一种粗糙判断条件,后续有赖于机器人在继续移动过程中作进一步的判断。但不用逐个栅格地考虑所述第一圆形区域对应的栅格区域是否允许机器人通行,减少栅格的标记误差的影响。
进一步地,所述第二预设圆域通行条件包括:在以机器人的当前位置为圆心、机器人的机身直径为半径的第二圆形区域内,第二不可通行区域的面积与第二圆形区域的面积的比值大于第二通行评价值;其中,第二不可通行区域是在第二圆形区域对应的栅格区域内,由未知栅格点和障碍物栅格点组成的栅格区域;第二通行评价值是用于克服构建栅格地图中存在的自由栅格点的标记误差而设置的判断阈值,且大于第一通行评价值。以提高判断精度。相对于第一预设圆域通行条件的判断,该技术方案在机器人移动至距离最新记录的路径节点一个机器人的机身直径后,通过对第二圆形区域内的不可通行区域所占的面积比例进行更高精度的通行性的判断,提高狭窄区域的判断精度,减少单个栅格点的标记信息的影响。
进一步地,所述第二预设圆域通行条件包括:在以最新记录的第一路径节点为圆心、预设倍数的机身直径为半径的第二圆形区域内,使用路径搜索算法搜索出最新记录的第二路径节点通向最新记录的第一路径节点的路径;其中,预设倍数的机身直径设置为:让第二圆形区域不与其它已标记的栅格区域交叉。以避免将其它已标记的栅格区域内已经规划出的路径参与判断,进而避免出现误判。
该技术方案对于第二圆形区域这一搜索区域的大小设置合理,避免与其他已知地图区域相交叉以把相关区域内规划的路线加入其中,导致机器人被导向其他区域而不再被引导通过当前的狭窄区域;另一方面,还通过判断是否能够在当前探索的第二圆形区域内规划出一段起点通往终点的完整的导航路径,来证明这个第二圆形区域的可通行区域已经不受障碍物栅格的标记位置的影响;从而提高所述狭窄区域的判断精度。
进一步地,在执行所述步骤S1的过程中,还包括:先判断机器人是否移动至所述预设路径的终点,是则停止执行所述局部路线的规划方法,否则控制机器人继续沿着预设路径移动,并判断所述预搜索区域是否满足第一预设圆域通行条件;在执行步骤S3的过程中,还包括:先判断机器人是否移动至所述预设路径的终点,是则停止执行所述局部路线的规划方法,否则控制机器人继续沿着预设路径移动。及时制止机器人继续移动和执行所述局部路线的规划方法,以满足智能化的规划要求,避免机器人无限制地进行路线搜索,导致消耗过多的不必要的计算资源。
一种路径规划方法,该路径规划方法包括:执行所述的路线优化方法;在执行完所述路线优化方法之后,将所述第一可行路线、所述第二可行路线和所述局部路线都配置为候选导航网络中的候选导航路线;还包括:步骤1、在栅格地图中设置导航起点和导航终点;步骤2、将导航起点设置为当前父节点,并设置为待遍历的节点;步骤3、以当前父节点为搜索中心在栅格地图中进行邻域搜索,其中,与当前父节点相邻的8个栅格点分别作为其子节点;步骤4、当步骤3在邻域搜索到的子节点是预先搜索出的所述候选导航网络内对应的一条候选导航路线的一个端点时,将对应的一条候选导航路线的另一个端点设置为待遍历的节点;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与该端点都设置为已遍历的节点,并将步骤3搜索到的不在所述对应的一条候选导航路线上的自由栅格点设置为待遍历的节点;然后进入步骤5;其中,所有设置为待遍历的节点都被对应记录下其父节点的位置信息,用以后续回溯路径节点;步骤5、从所有的待遍历的节点中选择出路径代价和值最小的节点作为下一个父节点,再判断所述下一个父节点是否为所述导航终点,是则基于前述记录的父节点的位置信息,从所述导航终点开始,除了步骤4所述候选导航路线的所有中间节点及其相应的一个端点之外,依次连接子节点及其父节点,直至连接到所述导航起点,规划出从所述导航起点到所述导航终点的路径;否则将所述下一个父节点更新步骤3所述的当前父节点,再返回步骤3。
与现有技术相比,该路径规划方案从导航起点开始,依次从所有的待遍历的节点当中选择出路径代价和值最小的节点作为父节点进行邻域搜索,并将邻域搜索得到的不处于候选导航路线上的自由栅格点和处于候选导航路线上具有标识意义的一个端点(起点或终点)都加入待遍历节点缓存空间,并记录相应的父节点以作为路径节点回溯信息,同时排除掉候选导航路线上的中间节点(虽然都是适用于这些狭窄区域的导航目标点)的对重复搜索造成的干扰,直到作为父节点是导航终点,再按照子节点和父节点的位置关系,从导航终点开始通过连接搜索到的候选导航路线反向连接到导航起点,从而通过成熟的路径搜索算法搜索符合通行条件的候选导航路线以规划出一条导航路径,克服映射到栅格地图中的狭窄区域较窄而容易产生栅格标记误差,使得机器人沿着规划好的整体导航路径有效进出这一狭窄区域,减少路径导航失败的概率。
进一步地,步骤4还包括:步骤3在邻域搜索到的子节点不是所有候选导航路线的端点时,将步骤3在邻域搜索到的自由栅格点都设置为待遍历的节点,并记录步骤3在邻域搜索到的自由栅格点的父节点的位置信息,然后进入步骤5。从而扩大子节点的搜索范围,也有利于在更大的范围内去扩展搜索所述候选导航网络内的路线起止点,使得所规划的导航路径具有更好的实用性和代表性。
进一步地,在所述步骤4中,当步骤3在邻域搜索到的子节点是所述候选导航网络内对应的一条候选导航路线的起点时,将对应的一条候选导航路线的终点设置为待遍历的节点,并记录所述对应的一条候选导航路线的终点的父节点的位置信息,以使得在所述候选导航网络内,每条候选导航路线使用其终点作为路线标识信息;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与同一条候选导航路线的起点都设置为已遍历的节点;其中,沿着所述对应的一条候选导航路线的起点指向其终点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处。
进一步地,在所述步骤4中,当步骤3在邻域搜索到的子节点是所述候选导航网络内对应的一条候选导航路线的终点时,将对应的一条候选导航路线的起点设置为待遍历的节点,并记录对应的一条候选导航路线的起点的父节点的位置信息,以使得在所述所述候选导航网络内,每条候选导航路线使用其起点作为路线标识信息;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与同一条候选导航路线的终点都设置为已遍历的节点;其中,沿着所述对应的一条候选导航路线的终点指向其起点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处。
步骤4所对应的两种技术方案,将所述候选导航路线的起点或其终点配置为能唯一确定一条候选导航路线,作为对应一条候选导航路线的标识并记录对应的父节点信息,而同一候选导航路线的剩余节点则配置为已重复搜索的节点,减少路径规划过程中的搜索量。
进一步地,在所述步骤5中,所述基于前述记录的父节点的位置信息,从所述导航终点开始,除了步骤4所述候选导航路线的所有中间节点及其相应的一个端点之外,依次连接子节点及其父节点,直至连接到所述导航起点的具体步骤包括:步骤51、基于前述记录的父节点的位置信息,从所述导航终点开始,连接所述导航终点及其父节点;然后进入步骤52;步骤52、以当前确定的父节点为子节点,再基于前述记录的父节点的位置信息,连接该子节点的父节点;步骤53、重复步骤52,直至连接到步骤4所述对应的一条候选导航路线的一个端点,然后从步骤4所述对应的一条候选导航路线的另一个端点开始,连接步骤4所述对应的一条候选导航路线的另一个端点及其父节点,然后返回步骤52;步骤54、重复步骤52和步骤53,直至连接到所述导航起点,实现逆向连接得到从导航起点到导航终点的一条导航路径。
与现有技术相比,规划出上述导航路径之后,本技术方案实现:在机器人经过狭窄区域的过程中,运用路径搜索算法搜索路径时,若检测到狭窄区域,则在狭窄区域附近由进入路径搜索算法搜索出的导航路径切换为进入所述候选导航路线;若检测到离开狭窄区域,则由所述候选导航路线切换为进入路径搜索算法搜索出的导航路径;让机器人可以有效穿过狭窄区域,提高穿越狭窄区域的导航成功率。
进一步地,在所述步骤5中,所述路径代价和值是已遍历路径代价与预测路径代价相加或加权得到,其中,已遍历路径代价是所有的待遍历的节点中的一个指定节点距离所述导航起点的代价,预测路径代价是同一个指定节点距离所述导航终点的代价;当所述路径代价和值越小,则配置指定节点在所有的待遍历的节点当中的遍历优先级越高;其中,两个相邻节点之间的移动代价使用曼哈顿距离、对角距离或欧式距离来表示。有利于搜寻出导航成本最低的最短路径。
进一步地,从导航起点开始,依次连接每个已到遍历过的栅格中心点直至连接到所述指定节点,再以栅格的边长获取当前连接而成的连线的长度,并将该长度作为所述已遍历路径代价;将所述指定节点设置为父节点,获取该父节点对应的每个子节点到导航终点的所有连线方案,再以栅格的边长获取每个子节点对应的每个连线方案的长度,选取每个子节点对应的最短长度作为其对应的预测路径代价;其中,栅格点用栅格中心点表示,用于代表一个栅格的位置特征。简化路径代价的计算方式。其中,本技术方案计算的代价值是对机器人运动轨迹成本的度量,表示由起点行驶到指定节点,再行驶到终点的成本,包括路径长度。
进一步地,设置为所述已遍历的节点后,不允许将同一节点配置为待遍历的节点。以实现在规划过程中,标识出已搜索出的节点。
进一步地,所述步骤4还包括:若将当前父节点的邻域中的不位于所述对应的一条候选导航路线上的自由栅格点或当前父节点的邻域中的所有自由栅格点都配置为待遍历的节点后,则将当前父节点由待遍历的节点改为已遍历的节点。防止以同一搜索中心进行重复扩展。
一种芯片,该芯片用于存储程序代码,程序代码用于执行所述的一种路线优化方法,和/或执行所述的路径规划方法。基于整体栅格区域的区域可通行条件,控制机器人在正常工作移动的状态下搜索出能够克服地图漂移误差造成栅格不可通行问题的完备的路线点集合。
一种机器人,该机器人内置所述芯片,该芯片用于执行所述的一种路线优化方法,和/或执行所述的路径规划方法。使得机器人后续在检测到所述狭窄区域并规划经过所述狭窄区域时,使用从预先搜索出完备的候选导航路线在当前的狭窄区域内布局出完整的候选导航网络,进而利用路径搜索算法从完整的候选导航网络中规划出导航路径,有效克服传感器累计误差(间接原因)、地图漂移(直接原因)造成的栅格标记误差的影响,沿着相应的候选导航路线连接而成的导航路径,无障碍地穿过所述狭窄区域。
附图说明
图1是本发明一实施例公开一种运用图3规划出的候选导航路线的路径规划方法的流程图。
图2是本发明又一实施例公开一种在狭窄区域内规划出的局部路线的方法流程图。
图3是本发明另一实施例公开的结合连接图2规划的局部路线的路线优化方法的流程图。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明。
需要说明的是,一般PRM算法操作流程:首先在即时构建的栅格地图中随机生成足够数目自由栅格点,其次采取某种策略直接将所生成的随机的自由栅格点连接成可行边进而形成一个图,该图即是生成的路线图,这些随机的自由栅格点都是在栅格地图内完全等概率均匀分布的自由节点,其中,导致路线图生成完备性较差的缺点。所以,PRM算法跟传统图搜索算法的不同之处在于,路线图在规划空间中不是以确定的方式来构造,而是基于概率论,使用某种随机概率的方法来构造的,其中,传统图搜索算法包括但不限于A*算法、D*算法;PRM算法生成路线图后,利用A*等传统图搜索算法在路线图上进行路径规划,比如路径规划或最优路径的查询,让PRM算法能在障碍物较少的环境下用相对较少的随机采样点找到一条导航路径,当障碍物较多或者采样点太少,节点在相应区域内的分布又不合理时,PRM算法采样的节点就有可能路径规划失败,因此PRM算法规划出的路线变得不完备。
需要说明的是,对于本领域技术人员可理解:即时构建的栅格地图中标记有机器人的当前位置周围的环境信息,机器人构建的地图区域内的栅格包括标记为自由(free)、占用(occupied) 和未知(unknown)三种状态;这些栅格在本实施例中使用栅格点表示,即栅格的中心点;自由状态的栅格点是指未被障碍所占用的栅格,是机器人可到达的栅格位置点,是自由栅格点,可以组成未占用区域;占用状态的栅格点是指被障碍物所占用的栅格,是障碍物栅格点,可以组成占用区域;未知栅格点是指机器人构建地图过程中的不清楚具体情况的栅格区域,其位置点处往往被障碍物所遮挡,可以组成未知区域。
需要说明的是,现有技术的智能扫地机器人经常移动至家庭环境中凳子的四条脚、茶几的入口等由各种家具组件之间限制形成的狭窄区域、房间的门打开形成的狭窄区域。需要补充的是,所述狭窄区域是各个障碍物的夹缝通道,其中,各个障碍物的夹缝通道是其中两个障碍物之间的最窄处对应的缝隙,这个缝隙宽度是大于机器人的机身直径的,是允许机器人通行的。由于机器人存在打滑情况、用于定位的传感器存在累计误差或视觉地图优化也产生误差,所以机器人容易在即时扫描构建的栅格地图上将所述狭窄区域标记为障碍物占据的区域,即将所述狭窄区域原本映射的自由栅格点错误地标记为障碍物占据的区域,导致映射到机器人构建的栅格地图中的狭窄区域的入口是堵住的,而实际运动场景中没有被真正堵住,使得机器人无法使用成熟的路径搜索算法完成一条贯穿所述狭窄区域的导航路径的规划,也使得机器人无法使用PRM算法在所述狭窄区域这一地形复杂的区域内采样足够多的自由栅格点,以便于进行路径规划。
因此,为了解决前述技术缺陷,本发明公开一种路线优化方法、路径规划方法、芯片及机器人,机器人在狭窄区域内的运动轨迹搜索并拟合出局部路线的基础上,先后以随机生成的自由栅格点、局部路线的起止点为搜索中心,基于不同筛选条件下,运用PRM算法层层递进地往空旷的地图区域内搜索可通行的最近的自由栅格点,增加困难区域内分布的自由栅格点的采样数目,使得规划出的可行路线连接所述局部路线尽可能地覆盖到相应地图区域的所有自由栅格区域,进而形成更加具备完备性的候选导航路线网络。
作为一种实施例,如图3所示,公开一种路线优化方法,具体包括如下步骤:
步骤S301、控制机器人沿着预设路径移动并规划出至少一条局部路线;然后进入步骤S302。其中,预设路径是预先设置的支持机器人通过狭窄区域的路径。无论是局部路线还是预设路径,连接成这些路径的路径节点都用栅格点表示;预设路径是机器人正常工作路径或导航路径,当机器人是扫地机器人时,预设路径可以是弓字型移动路径、沿边行走路径、回字形路径等规划清扫路径,当机器人在所述预设路径上移动时,机器人在执行正常工作过程中穿越所述狭窄区域进行相应的路线搜索,在同一过程中,机器人一边沿着所述预先路径移动,一边采集符合通行条件或辅助通行的路径节点,在所述狭窄区域附近或所述狭窄区域的内部规划出支持机器人进出所述狭窄区域的局部路线,规划出的局部路线的数量不作限制,但是这些局部路线是相互连接为路线图,即栅格地图内的路线网络。
步骤S302、在机器人已构建的栅格地图内随机搜索出第一预设数量的满足第一连通筛选条件的栅格点;然后进入步骤S303。其中,机器人已构建的栅格地图是机器人从启动至执行步骤S302的过程中实时构建起的栅格地图、或者是预先在空旷区域扫描构建的栅格地图,这里的空旷区域是除了狭窄区域、障碍物区域、正常行走区域之外的区域。优选地,本实施例公开的机器人已构建的栅格地图是机器人记录到的最大栅格地图。
在步骤S302中,机器人在所述空旷区域内随机搜索出第一预设数量的满足第一连通筛选条件的栅格点,具体包括:
计算机器人已构建的栅格地图的长度与单个栅格的边长的第一比值,并对该第一比值取整处理,即丢弃第一比值的小数部分,而保留第一比值的整数部分,获得已构建的栅格地图的横轴方向(X轴方向)上最多能容纳的栅格数量;同时,计算机器人已构建的栅格地图的宽度与单个栅格的边长的第二比值,并对该第二比值取整处理,即丢弃第二比值的小数部分,而保留第二比值的整数部分,获得已构建的栅格地图的纵轴方向(Y轴方向)上最多能容纳的栅格数量;然后将第一比值的取整处理结果和第二比值的取整处理结果作乘积运算,其乘积结果作为机器人已构建的栅格地图中分布的栅格的总数量,确定为第一预设数量,也可以表示节点在栅格地图中的分布密度;其中,栅格点用于对应一个栅格的栅格中心点表示。
以机器人的当前位置为搜索中心,在所述栅格地图内随机搜索出满足第一连通筛选条件的栅格点并加入预先创建的可行路线集合,直到随机搜索并获得所述第一预设数量的满足第一连通筛选条件的栅格点,为获取上述第一预设数量的满足第一连通筛选条件的栅格点,本实施例在所述栅格地图内重复执行随机搜索的次数至少为所述第一预设数量。这里的随机搜索是:以完全等概率的方式对栅格地图内的每一个栅格地进行搜索,具体地,在栅格地图的最大横轴坐标与最小横轴坐标之间随机搜索出横轴方向上的随机整数坐标x,同时在栅格地图的最大纵轴坐标与最小纵轴坐标之间随机搜索出纵轴方向上的随机整数坐标y,生成待筛选节点的坐标c(x,y),x最大值等于所述最大横轴坐标,x最小值等于所述最小横轴坐标,y最大值等于所述最大纵轴坐标,y最小值等于所述最小纵轴坐标,这些最值坐标都是整数值,符合栅格地图这一离散地图的特性;然后按照第一连通筛选条件对这些随机搜索出的待筛选节点进行筛选,将满足第一连通筛选条件的栅格点加入可行路线集合;其中,可行路线集合作为节点集合,用于存储符合不同筛选条件的路径节点,但不能重复存储同一个路径节点,其中,某两个节点之间生成一条可行路线。为了方便描述,本实施例将满足第一连通筛选条件的栅格点设置为第一连通节点,即满足第一连通筛选条件的栅格点以第一连通节点的形式保存在所述可行路线集合的内部。
具体地,所述第一连通筛选条件是:当前随机搜索的自由栅格点不存在于所述可行路线节点集合内,且以当前随机搜索的自由栅格点为圆心、搜索半径为一个机身直径的区域内不存在障碍物栅格点和未知栅格点,使得当前随机搜索的自由栅格点为所述第一连通节点。使得所述第一连通节点附近的可探索区域内不存在阻挡机器人通行的障碍物且都是可通行的栅格点,让已构建的栅格地图内随机搜索的第一连通节点更具备通行意义。为所述狭窄区域引入空旷区域内随机生成的满足第一连通筛选条件的栅格点,增强整个栅格地图区域内的自由栅格点的数量,提高第一连通节点在所述栅格地图内的分布密度。本实施例使用机器人已构建的栅格地图中分布的栅格的总数量来表示节点在栅格地图中的分布密度,进而通过随机搜索出用于表示栅格地图中分布的栅格的总数量的第一连通节点,提高了栅格地图区域内的自由节点的分布密度,保证了空旷区域中自由栅格点个数的最小值,使得生成的候选导航网络得以优化,候选导航网络的完备性得到改善。
作为一种实施例,在执行步骤S302和步骤S303之间,还需将所述局部路线存入所述可行路线集合,具体步骤包括:
将所述局部路线的起点及其终点都设置为第二连通节点;同时在所述局部路线上,将所述局部路线的起点及其终点之间的中间节点设置为第三连通节点;其中,所述局部路线的起点、终点及其中间节点都存储于预测通行节点集合内;所述预测通行节点集合是机器人沿着所述预设路径移动过程中创建的。然后,本实施例在预测通行节点集合内记录第二连通节点的子节点的位置信息,再将第二连通节点加入所述可行路线节点集合;并且,在同一个预测通行节点集合内记录第三连通节点的子节点的位置信息,再将第三连通节点加入所述可行路线节点集合;以使得所述可行路线节点集合内存有一个所述预测通行节点集合内用于代表对应的唯一一条局部路线的完整路径节点;所述预测通行节点集合是机器人沿着所述预设路径移动过程中创建的;其中,无论是加入所述预测通行节点集合还是加入所述可行路线节点集合,代表同一条局部路线的第二连通节点及其子节点是相邻且相连通的;代表同一条局部路线的第三连通节点及其子节点是相邻且相连通的。每一个预测通行节点集合内所存储的路径节点及其子节点整体代表一个所述局部路线节点集合内唯一的一条局部路线。从而将机器人行走规划出的局部路线的路径节点逐个地由所述预测通行节点集合转移到所述可行路线节点集合内,实现基于记录的子节点的位置信息在所述可行路线节点集合内连接还原出原来的局部路线。
在本实施例中,当所述第二连通节点和第三连通节点加入所述可行路线节点集合时,基于记录的子节点的位置信息,将所述可行路线节点集合内的所述第二连通节点和第三连通节点按照子节点及其父节点的邻接关系依次连接为所述局部路线。具体地,本实施例基于子节点的位置信息,以所述第二连通节点为起点,按照预设步长扩展新节点扩展出所述第三连通节点,再以当前扩展的所述第三连通节点为新的起点,继续扩展出后续的第三连通节点,直至扩展到新的所述第二连通节点为止,再按照先后扩展的顺序依次连接出完整的所述局部路线。完整地还原出原本存储于所述预测通行节点集合内的局部路线。
步骤S303、分别以所述满足第一连通筛选条件的栅格点、所有局部路线的起点及其终点为第一搜索中心,搜索出满足第二连通筛选条件的栅格点,再将第一搜索中心及其搜索出的所有满足第二连通筛选条件的栅格点相连接成第一可行路线,进而确定第一可行路线与所述局部路线相连接;然后进入步骤S304。
步骤S303具体包括:分别以所述第一连通节点和所述第二连通节点为所述第一搜索中心,在所述可行路线节点集合内搜索出满足第二连通筛选条件的栅格点,即以所述第一连通节点为搜索中心从所述可行路线节点集合现存的元素中搜索出满足第二连通筛选条件的栅格点但不一定局限于邻域内进行扩展,同时以所述第二连通节点为搜索中心从所述可行路线节点集合现存的元素中搜索出满足第二连通筛选条件的栅格点但不一定局限于其邻域内进行扩展;然后将满足第二连通筛选条件的栅格点设置为对应的第一搜索中心的第一合法子节点并加入对应第一搜索中心的子节点集合内,其中,对应第一搜索中心的子节点集合也存在于所述可行路线节点集合内,依附于所述第一搜索中心且以结构体的形式存在,使得每个所述第一连通节点和每个所述第二连通节点在所述可行路线节点集合内都存在一个对应的子节点集合;在此数据结构的基础上,本实施例将同一个第一搜索中心和所有第一合法子节点相连接为所述第一可行路线,第一搜索中心和各个第一合法子节点可以是相互连接以增加区域中的路线分布的完备性,优选地,第一搜索中心的子节点集合内的各个第一合法子节点都不与所述可行路线节点集合内加入的第三连通节点相连接;进而确定第一可行路线与所述局部路线相连接,但仅限于第一可行路线与所述局部路线的起止点相互连接。本实施例同时在所述局部路线的起止点的周围区域内以及所述第一连通节点的周围区域内选取满足第二连通筛选条件的栅格点,不用像传统的PRM算法那样局限于关注每个自由栅格点的邻域并局限于在邻域内增加子节点,搜索效率较低;另一方面,相对于仅限于在所述狭窄区域内(所述局部路线的起止点的周围区域所代表的区域)搜索满足第二连通筛选条件的栅格点,本实施例能够在更广的区域筛选出满足第二连通筛选条件的栅格点并记录为用于连接所述局部路线的合法子节点,这样做的好处是通过融合连接所述局部路线和所述第一可行路线,生成若干连接所述狭窄区域、空旷区域、正常区域以及障碍物区域的路线,改善所述候选导航网络在狭窄区域和空旷区域的连通性。
在上述实施例中,第二连通筛选条件包括:与所述第一搜索中心的连线上不存在障碍物栅格点、且与所述第一搜索中心的连线长度小于预设距离阈值的栅格点是所述第一连通节点或所述第二连通节点;其中,预设距离阈值是根据具体环境情况而预先设定的,由于狭窄区域和空旷区域环境情况的不同,具体的数值也可能不同;本实施例将所述预设距离阈值设置为大于单个栅格的边长,筛选出能连成有效的第一可行路线的节点。因此,满足第二连通筛选条件的栅格点是与所述第一搜索中心的连线上不存在障碍物栅格点、且与所述第一搜索中心的连线长度小于预设距离阈值的第一连通节点或第二连通节点。本实施例中的,距离阈值概念的提出是为了使某个区域内自由栅格点的个数达到一定的数值,使得在所述可行路线节点集合对应的离散栅格区域内,所述第一搜索中心的相应距离范围内存在自由栅格点足以连接成路线,避免该区域自由栅格点的个数不会太少而无法生成符合要求的第一合法子节点。以保证该区域自由栅格点的个数不会太少而无法生成符合要求的路图以形成导航网络。
步骤S304、判断所有局部路线的起点的子节点及所有局部路线的终点的子节点中是否都存在所述满足第一连通筛选条件的栅格点,是则进入步骤S305,否则进入步骤S306。这里的所有局部路线的起点及其终点都是已经加入所述可行路线节点集合内,进而根据所述可行路线节点集合内对应的节点的子节点集合内存储的子节点进行判断,但都是栅格地图中相应节点的邻域内的节点。
步骤S305、确定第一可行路线与所述局部路线相连接形成完备的候选导航网络,即两种路线融合完备;具体是:若检测到所述可行路线节点集合内所有第二连通节点的子节点中都存在第一连通节点,则确定所有第二连通节点所属的局部路线的两端分别和加入所述对应第一搜索中心的子节点集合内的所有第一连通节点相连接,进而确定所述局部路线和所述第一可行路线连接形成完备的候选导航网络;其中,步骤S304检测的第二连通节点的子节点是处于同一个第二连通节点对应的子节点集合内。与现有技术相比,本实施例公开了代表开阔区域的栅格地图内随机搜索出的第一连通节点,与狭窄区域内规划的局部路线,在具备合理的自由栅格点数量和连通环境后,最终形成完备的候选导航网络,弥补了一般PRM算法完全等概率均匀分布自由栅格点,导致生成的可行路线的完备性较差的缺点,不易遗漏通过狭窄区域和空旷区域之间存在的可行路径。
步骤S306、分别以所有局部路线的起点及其终点为第二搜索中心,搜索出满足第三连通筛选条件的栅格点,再将第二搜索中心及其搜索出的所有满足第三连通筛选条件的栅格点相连接成第二可行路线。具体地,相对于步骤S303,步骤S306只以所述第二连通节点为搜索中心从所述可行路线节点集合现存的元素中搜索出满足第三连通筛选条件的栅格点,但不局限于邻域内进行扩展,且第三连通筛选条件不同于第二连通筛选条件;然后将满足第三连通筛选条件的栅格点设置为对应的第二搜索中心的第二合法子节点并加入对应第二搜索中心的子节点集合内,其中,对应第二搜索中心的子节点集合也存在于所述可行路线节点集合内,依附于所述第二搜索中心且以结构体的形式存在,使得每个所述第二连通节点在所述可行路线节点集合内都存在一个对应的子节点集合;在此数据结构的基础上,本实施例将同一个第二搜索中心和所有第二合法子节点相连接为所述第二可行路线,第二搜索中心和各个第二合法子节点(包括邻域内的各个节点)可以是相互连接以增加步骤S303在同一区域内已经生成的可行路线,提高生成的路线的完备性,优选地,第二搜索中心的子节点集合内的各个第二合法子节点都不与所述可行路线节点集合内加入的第三连通节点相连接;进而确定进而确定第一可行路线、第二可行路线与所述局部路线相连接形成更加完备的候选导航网络,但第二可行路线只与所述局部路线的起止点相互连接。步骤S304检测的第二连通节点的子节点是处于同一个第二连通节点对应的子节点集合内。因此,本实施例相对于搜索出满足第二连通筛选条件的栅格点,限缩搜索的范围,增加所述第二连通节点对应的子节点集合的元素,相对提高第二搜索中心邻域内自由栅格点的分布概率。减少了遗漏通过所述困难区域的可行路径的概率。
所述第三连通筛选条件包括:在与所述第二搜索中心的连线上不存在障碍物栅格点且不存在未知栅格点的所有栅格点中,与第二搜索中心的连线长度最短的栅格点是第一连通节点。即满足所述第三连通筛选条件的栅格点是:与所述第二搜索中心的连线上不存在障碍物栅格点和未知栅格点、且与第二搜索中心的连线长度最短的第一连通节点。本实施例在自由栅格点不足的区域内地图中,通过增加在所述局部路线的起止点附近的采点量来搜寻出最短距离的可通行的路径节点,降低对节点之间的连通性的标准,从而强化算法在窄通道中的路径搜索,使其更适用于复杂地形。所述第三连通筛选条件相匹配的搜索区域相对于所述第二连通筛选条件的要求减小,提高路线规划的速度。
在本实施例中,在所述完备的候选导航网络中,所述局部路线的两端分别用于连接所述第一可行路线的一端点和/或所述第二可行路线的一端点,但是所述第一可行路线和/或所述第二可行路线都不连接上所述局部路线的中间节点;以便于利用路径搜索算法在所述完备的候选导航网络中搜索出一条由预先设置的导航起点通往预先设置的导航终点的路线;所述候选导航网络是通过执行前述步骤S301至步骤S306构建的一个完备的可供路径搜索算法重复利用的导航路线图。需要说明的是,在前述实施例中,所述狭窄区域是两个或两个以上障碍物的夹缝通道,夹缝通道是其中两个障碍物之间的最窄处对应的缝隙,这个缝隙宽度是大于或等于机器人的机身直径;其中,节点集合是占据内存空间的数据结构。空旷区域代表机器人已经构建的栅格地图区域,可以包括狭窄区域附近的开阔区域,只存在孤岛障碍物不足以阻挡机器人的正常行走、或障碍物分布比较离散以使得障碍物之间形成的距离是机身直径的预设倍数,这个预设倍数根据机器人所处的室内环境具体配置的。本实施例采取前述步骤生成的随机自由栅格点与所述局部路线的两端连接成边进而形成一个图,该图即是生成的能够覆盖狭窄区域和空旷区域的候选导航网络,作为路径搜索算法在狭窄区域内的相对完整的候选导航路线的搜索源,较好地找到了更符合要求的最优导航路径,减少了遗漏通过所述困难区域的可行路径的概率。
本发明另一实施例公开所述局部路线的规划方法,如图2所示,具体包括:
步骤S201、在控制机器人沿着预设路径移动的过程中,实时判断预搜索区域是否满足第一预设圆域通行条件,是则进入步骤S202,否则控制机器人继续沿着预设路径移动。其中,所述预设路径是机器人预先规划的路径;路径节点支持用栅格点表示;预设路径是机器人正常工作路径或导航路径,当机器人是扫地机器人时,预设路径可以是弓字型移动路径、沿边行走路径、回字形路径等规划清扫路径,在所述预设路径上移动的机器人,可以在正常工作过程中进行适用于穿越所述狭窄区域的路线坐标点或路线搜索,或者在进入狭窄区域过程中进行相应的路线搜索。需要说明的是,所述预搜索区域是以机器人的当前位置为圆心、机器人的机身直径为半径的第一圆形区域,覆盖到机器人周围的最小可通行区域。
优选地,在执行所述步骤S201的过程中,还包括:先判断机器人是否移动至所述预设路径的终点,是则停止执行所述局部路线的规划方法,否则控制机器人继续沿着预设路径移动,并实时判断所述预搜索区域是否满足第一预设圆域通行条件;所述步骤S201及时制止机器人继续移动和执行所述局部路线的规划方法,以满足智能化的规划要求,避免机器人无限制地进行路线坐标的搜索,进而导致计算资源消耗过大。
步骤S202、将机器人的当前位置记录为第一路径节点,同时创建一个新的预测通行节点集合,并将第一路径节点存入所述预测通行节点集合,然后进入步骤S203。在本实施例中,所述预测通行节点集合内部的元素是配置为按记录的时间顺序存储第一路径节点。
需要说明的是,所述预测通行节点集合内部的首元素及其尾元素都是唯一的,使得所述预测通行节点集合内部的首元素或其尾元素成为其表示的路线的唯一标识信息,可以作为规划路径的过程中的索引节点或作为回溯路径的标示节点,而且,同一个所述预测通行节点集合内部的首元素及其尾元素不是相同的,因为在同一个所述预测通行节点集合内部的首元素及其尾元素相同时,所述预测通行节点集合内部的元素按照记录的先后顺序首尾互连形成封闭图形,形成没有导航意义的路径。
在本实施例中,所述第一预设圆域通行条件包括:所述预搜索区域内的第一不可通行区域所占的面积比例大于第一通行评估值;其中,第一不可通行区域是在所述第一圆形区域对应的栅格区域内,由未知栅格点和障碍物栅格点组成的栅格区域;第一通行评价值是为了克服构建栅格地图中存在的自由栅格的标记误差而设置的预判阈值,是在所述狭窄区域内反复实验的结果;第一通行评价值优选地设置为50%。当机器人判断到所述第一圆形区域对应的栅格区域内的第一不可通行区域所占的面积比例小于或等于50%时,控制机器人维持沿着预设路径移动,直到机器人判断到所述第一圆形区域对应的栅格区域内的第一不可通行区域所占的面积比例大于50%。因此,所述第一预设圆域通行条件用于初步判断机器人是否开始进入狭窄区域,属于一种粗糙判断条件,后续有赖于机器人在继续移动过程中作进一步的判断。但不用考虑连接成规划路径的每个栅格的标记信息是否允许机器人通行,减少单个栅格标记误差的影响。
步骤S203、在控制机器人继续沿着所述预设路径移动,实时判断机器人是否移动至与最新记录的路径节点的直线距离为大于或等于机器人的机身直径的位置,是则进入步骤S204,否则控制机器人继续沿着所述预设路径移动。该步骤S203检测的机器人与上一次记录的路径节点的直线距离,包括机器人的当前位置至与第一路径节点的直线距离,其中,机器人保持实时检测当前位置与上一次记录的路径节点的直线距离,也可以是每隔一定的检测周期才进行距离采样检测,在满足一定的通行条件时才将实时检测到的路径节点记录下来。
执行步骤S203的过程中,还包括:先判断机器人是否移动至所述预设路径的终点,是则停止执行所述局部路线的规划方法,否则控制机器人继续沿着预设路径移动,并判断机器人是否移动至与最新记录的路径节点的直线距离为大于或等于机器人的机身直径的位置。从而及时制止机器人继续移动和执行所述局部路线的规划方法,以满足智能化路径规划要求,避免机器人无限制地移动以持续进行路线坐标搜索,导致计算资源消耗过大。
步骤S204、检测到机器人的当前位置与最新记录的路径节点的直线距离为大于或等于机器人的机身直径时,将机器人的当前位置记录为第二路径节点,然后进入步骤S205。
步骤S205、判断所述第二路径节点是否满足第二预设圆域通行条件,是则进入步骤S206,否则进入步骤S207;因此,前述步骤S204最新记录的路径节点包括上一次记录的第一路径节点或上一次记录的第二路径节点。其中,步骤S204记录的第二路径节点相对于上一次执行步骤S204对应记录下的第二路径节点是新的第二路径节点,或步骤S204记录的第二路径节点相对于步骤S202对应记录下的第一路径节点是新的路径节点。
作为第二预设圆域通行条件的一种实施方式,所述第二预设圆域通行条件包括:在以机器人的当前位置(即最新记录的第二路径节点)为圆心、机器人的机身直径为半径的第二圆形区域内,第二不可通行区域所占的面积比例大于第二通行评价值;其中,第二不可通行区域是在第二圆形区域对应的栅格区域内,由未知栅格点和障碍物栅格点组成的栅格区域;第二通行评价值是用于克服构建栅格地图中存在的自由栅格的标记误差而设置的判断阈值,且大于第一通行评价值以提高判断精度。第二通行评价值优选地设置为75%。当机器人判断到所述第二圆形区域对应的栅格区域内的第二不可通行区域所占的面积比例小于或等于75%时,进入步骤S207。其中,第二圆形区域的覆盖区域相对于第一圆形区域的覆盖区域不同,因为机器人的位置发生变化;因此在所述第一圆形区域的基础上或上一次搜索的第二圆形区域的基础上,机器人移动至距离最新记录的路径节点一个机器人的机身直径后,进一步地作第二不可通行区域所占的面积比例的判断,不用逐个栅格点地考虑标记信息的影响,提高狭窄区域的判断精度。本实施例只是将所述第二圆形区域的探索半径设置为机器人的机身直径,而不是更大的数值,避免控制不相关(探测范围之外的)栅格区域参与计算,减少时间开销。
作为第二预设圆域通行条件的另一种实施方式,所述第二预设圆域通行条件包括:在以最新记录的第一路径节点为圆心、预设倍数的机身直径为半径的第二圆形区域内,使用路径搜索算法搜索到最新记录的第二路径节点通向最新记录的第一路径节点的路径,用于证明当前的第二圆形区域内的路径搜索已经不受障碍物的影响或障碍物栅格的错误标记的影响;由于所述第二圆形区域可能处于狭窄区域的附近,所以最新记录的第二路径节点通向最新记录的第一路径节点之间可能存在障碍物阻碍,且本实施例将可能发生碰撞的节点称为无效节点或非法节点,在规划路径时需要避开这些节点,因此才有使用成熟稳定的路径搜索算法去检验区域的可通行性的必要,本实施例使用的路径搜索算法是A*算法,有效快速地在所述第二圆形区域内搜索出最新记录的第二路径节点通向最新记录的第一路径节点的导航路径。需要说明的是,本实施例可以将所述第一圆形区域或所述第二圆形区域视为清扫区域。其中,预设倍数的机身直径设置为:让第二圆形区域不与其它已标记的栅格区域交叉,以避免将其它已标记的栅格区域内已经规划出的路径参与判断,进而避免出现误判。所述预设倍数的机身直径设置为三分之二的机身直径,使得形成的第二圆形区域大于所述第一圆形区域内。与现有技术相比,本实施例对于第二圆形区域这一搜索区域的大小设置合理,避免与其他已知地图区域相交叉以把相关区域内规划的路线加入其中,导致机器人被导向其他区域而不再被引导通过当前的狭窄区域;另一方面,还通过判断是否能够在当前探索的第二圆形区域内规划出一段起点通往终点的完整的导航路径,来证明这个第二圆形区域的可通行区域已经不受障碍物的影响或不受障碍物栅格的标记位置的影响;从而提高所述狭窄区域的判断精度。
步骤S206、判定机器人当前处于所述狭窄区域,并将第二路径节点加入步骤S202所述的预测通行节点集合内,再返回步骤S203;因为在机器人识别出其当前位置处于所述狭窄区域后,通过返回步骤S203,继续在所述狭窄区域内沿着所述预设路径移动。但是机器人可能已经移动到所述预设路径的终点,所以,优选地控制机器人由步骤S206进入步骤S208作进一步的路径节点判断。
因此,在本实施例中,机器人在搜索到第一路径节点后,每移动一个机身直径的直线距离时,即当前位置与上一个记录路径节点的位置的直线距离为机身直径时,判断机器人的当前位置或当前的探索区域是否满足第二预设圆域通行条件;然后,通过第二预设圆域通行条件确定机器人当前处于所述狭窄区域时,继续沿着所述预设路径移动于所述狭窄区域,继续搜索出新的第二路径节点以连接出用于穿过所述狭窄区域的路线,直到判断到所述第二路径节点不满足第二预设圆域通行条件。需要说明的是,其中,所述狭窄区域是两个或两个以上障碍物的夹缝通道,夹缝通道是其中两个障碍物之间的最窄处对应的缝隙,这个缝隙宽度是大于或等于机器人的机身直径。
步骤S207、判定机器人当前没有处于所述狭窄区域,或刚好离开当前的狭窄区域来到不可通行区域的面积占比较少的空旷区域,同时根据所述预测通行节点集合内部存有的路径节点的数目,保存所述预测通行节点集合到同一个所述局部路线节点集合的内部,并将所述预测通行节点集合内存储的路径节点按照先后加入的次序连接成对应的一条局部路线,相当于向所述预测通行节点集合加入对应局部路线的终点,使得一个预测通行节点集合在所述所述局部路线节点集合内成为代表一条局部路线的节点集合;然后通过返回步骤S201,来控制机器人在新的位置重新进行一轮所述狭窄区域的判断,即迭代执行前述步骤S201至步骤S207以在所述所述局部路线节点集合内创建一个新的预测通行节点集合,以描述一条新的局部路线;但是机器人可能已经移动到所述预设路径的终点,所以,优选地控制机器人由步骤S207进入步骤S209作进一步的路径节点判断。
因此,步骤S208、判断机器人是否移动至所述预设路径的终点,是则结束执行所述局部路线的规划方法,否则返回步骤S203。
步骤S209、判断机器人是否移动至所述预设路径的终点,是则结束执行所述局部路线的规划方法,否则返回步骤S201。从而及时制止机器人继续移动和执行所述局部路线的规划方法,避免机器人无限制地进行路线规划,导致计算资源消耗过大。
需要说明的是,一个预测通行节点集合内是允许包括障碍物栅格点,表示对应的局部路线是有可能存在障碍物栅格点。由步骤S201至步骤S209可知,机器人自身移动至一个特定的路径节点,比如在一次迭代执行的过程中执行到步骤S202(满足初始状态条件)、或者在一次迭代执行的过程中执行到步骤S206时对应的机器人当前位置,且在该路径节点上判断到满足所述第一预设圆域通行条件或所述第二预设圆域通行条件时,则将该路径节点加入所述预测通行节点集合,作为连接到对应局部路线上的一个路径节点,然而由于传感器探测误差、地图漂移误差等,该路径节点在栅格地图上的标记的栅格信息可能不是自由状态,反而被误标记为障碍物占据状态,即所述障碍物栅格点,该路径节点虽然在栅格地图中标记为障碍物栅格点,但是也加入所述预测通行节点集合,作为对应局部路线上的一个路径节点,而实际机器人是可以移动至该路径节点,证明该路径节点是可通行的。
值得注意的是,同一个局部路线节点集合的内部,存在多个所述预测通行节点集合,分别代表其它的互不相同的局部路线,因为同一个所述局部路线节点集合的内部,任一个所述预测通行节点集合内部的首元素及其尾元素都是唯一的。
具体地,根据所述预测通行节点集合内部存有的路径节点的数目,保存所述预测通行节点集合到同一个所述局部路线节点集合的方法具体包括:当所述预测通行节点集合内部存有的路径节点的数目小于2时,表示机器人可能检测到难以逾越的障碍物或被困或出现其它异常情况,使得收集到所述预测通行节点集合内部存有的路径节点是无效节点并代表无效的局部路线,则选择删除所述预测通行节点集合,再返回步骤S201以创建一个新的预测通行节点集合。因此,在本实施例中,若一个预测通行节点集合最终获取的路径节点的数目少到难以连接出一条线时,可删除相关的集合以减少无效的路径节点。
当所述预测通行节点集合内部存有的路径节点的数目大于或等于2时,使用所述预测通行节点集合表示单独一条局部路线,并保存到所述所述局部路线节点集合内,以供路径搜索算法调用,可以是启发式搜索算法使用,再通过返回步骤S201来创建一个新的预测通行节点集合;优选地,一个预测通行节点集合最终获取的路径节点可以作为整体路线保存到所述所述局部路线节点集合,使得局部路线的存储数据合理有序。其中,所述第一路径节点和所述第二路径节点是按照记录的先后顺序加入所述预测通行节点集合内,使得所述预测通行节点集合内存储的路径节点以记录好的次序连接成对应的一条局部路线。需要说明的是,每一个预测通行节点集合内的首元素对应的路径节点是对应的一条局部路线的起点,每一个预测通行节点集合内的尾元素对应的路径节点是对应的一条局部路线的终点。使得每一个预测通行节点集合内的路径节点都匹配连接成一条局部路线,形成用于预测机器人在相应区域内无障碍通行的局部路线;
与现有技术相比,前述步骤S201至步骤S207所述局部路线的规划方法适用于机器人进入狭窄区域的状态下实施路线规划,先设置第一预设圆域通行条件作为狭窄区域的预判条件,并提供所述预测通行节点集合对应的局部路线的路径节点来源;再让机器人沿着原路径继续移动,然后设置第二预设圆域通行条件作为狭窄区域的精判条件,并继续提供所述预测通行节点集合对应的局部路线的栅格点来源,使得移动一定的距离后机器人才具备判断狭窄区域的资格,所述预测通行节点集合收集的用于连成栅格点的局部路线更加的完整,更加适应于狭窄区域下的地图栅格误差环境,为机器人提供实际可通行的路线而无需逐个关注该路线上的栅格标记信息。另外,在此基础上,若不满足相应的预设圆域通行条件时,则停止为所述预测通行节点集合继续搜索路径节点,并确定单独一个所述预测通行节点集合内的路径节点可以连接为一条独立的局部路线。通过迭代执行前述相关步骤,机器人在正常工作移动的状态下搜索能够克服地图漂移误差的路线点集合。增加机器人在障碍物空间布局较复杂的场景中找到有效导航路径的成功率。
优选地,若机器人当前检测到由处于所述狭窄区域变为没有处于所述狭窄区域时,则开始使用所述所述局部路线节点集合内的多个所述预测通行节点集合来执行前述实施例的路线优化方法,以规划出支持机器人自由进出所述狭窄区域的局部路线,克服地图漂移误差。克服在狭窄区域处搜索到的自由栅格点容易被标记为障碍物栅格点而导致规划的路径无法通过狭窄区域的问题。
优选地,若机器人的当前位置被障碍物包围,使其无法继续沿着所述预设路径移动时,停止执行所述局部路线的规划方法和前述实施例提及的局部路线的规划方法。
作为一种实施例,本发明实施例公开一种路径规划方法,本发明实施例需先控制机器人执行前述实施例的路线优化方法来规划出可供搜索的候选导航路线,然后使用启发式搜索算法(主要是这一类搜索算法的路径节点搜索思路)和所述候选导航路线规划出整体导航路径,克服地图栅格标记误差,以便解决机器人在栅格地图中的通道较窄而容易产生标记误差的可通行区域内的导航路径规划问题和机器人自由通过狭窄区域的技术问题。该路径规划方法可以实施于机器人完全离开所述狭窄区域之后、或者离开当前的狭窄区域后即将进入新的狭窄区域之前。
所述路径规划方法包括:
首先执行前述实施例公开的路线优化方法;在执行完所述路线优化方法之后,将所述第一可行路线、所述第二可行路线和所述局部路线都配置为所述候选导航网络中的候选导航路线。其中,所述候选导航网络中形成的候选导航路线的数量越多,则对地图区域的覆盖率越高,即候选导航路线的完备性越佳。
在执行完所述路线优化方法之后且配置好候选导航路线,所述路径规划方法还包括:
步骤S101、在所述栅格地图中设置导航起点和导航终点,可以创建待遍历节点缓存空间;然后进入步骤S102。其中,栅格地图是机器人构建的包括起点和终点的局部地图,这个栅格地图还包括各种布局的障碍物信息,并包含足够的空间区域以便机器人根据导航起点和导航终点进行路径轨迹的规划。导航起点和导航终点可以通过栅格地图的栅格坐标、栅格中心点坐标、或其它类型的导航数据表示,对此不做限定,但都能转换到栅格地图中参与路径规划。另一方面,步骤S101创建待遍历节点缓存空间,并初始化为空的,其内部元素的优先级是与路径代价相关,元素的优先级可以是遍历优先级、规划优先级。
步骤S102、将导航起点设置为当前父节点,具体是以导航起点所在的栅格或栅格点设置为当前父节点,并设置为待遍历的节点以备后续邻域搜索过程中选择使用,或选择加入待遍历节点缓存空间;然后进入步骤S103。在一种可选的实施方式中,在将导航起点加入待遍历节点缓存空间前,初始的待遍历节点缓存空间可以为空,则导航起点是加入待遍历节点缓存空间的第一个节点。优选地,位于机器人坐标系的原点(局部坐标系的原点)处的父节点与其在所述四个象限区域内扩展搜索出的子节点都是相邻接的位置关系。
其中,所述待遍历节点缓存空间视为本实施例的路径规划的候选路径节点的集合,将机器人可能经过的路径节点加入其中,以便后续的筛选处理。所述待遍历节点缓存空间可以记录内部的每个路径节点的位置、以及机器人移动到对应路径节点的状态。本示例性实施方式中,对于栅格地图中的路径节点,通过状态参数表示机器人移动到达路径节点时的状态,至于路径节点的状态参数,至少包括:时刻,即机器人到达该路径节点的时刻;此外,状态参数还可以包括:姿态,包括机器人到达路径节点时的方向、转向角(即机器人本身处于直行还是转向的状态,以及转向的角度)等;速度,即机器人到达路径节点时的运动速度。在开始进行路径规划时,可以先确定导航起点的状态参数,其中,可以将当前时刻或零时刻作为导航起点的时刻,将当前机器人运动状态作为起点对应的姿态和速度,例如机器人的起始为静止状态,则导航起点对应的转向角和速度均为0。在一种示例性实施例中,所述待遍历节点缓存空间是优先队列的数据结构,以先入先出的存储顺序加入对应的待遍历的节点,保存于机器人的存储空间内。
步骤S103、以当前父节点为搜索中心在栅格地图中进行邻域搜索,其中,与当前父节点相邻的8个栅格点分别作为其子节点;然后进入步骤S104。其中,从父节点(步骤S103所述的当前父节点)开始搜索邻域上的子节点的过程可称为“扩展”。在一次扩展的过程中,可以在当前父节点的邻域中朝着顺时针方向逐个栅格点地进行搜索(邻域搜索),也可以在当前父节点的邻域中朝着逆时针方向逐个栅格点地进行搜索(邻域搜索),以搜索出符合相关条件的子节点。
步骤S104、判断步骤S103在邻域搜索到的子节点是否属于预先搜索出的候选导航网络内对应的一条候选导航路线的一个端点,是则进入步骤S105,否则进入步骤S106。即将判断步骤S103在邻域搜索到的子节点是否属于预先搜索出的候选导航网络内代表候选导航路线的起点或其终点,以便于将规划的导航路径连接上搜索到的候选导航路线,用于实际导航行走,而不用考虑实时检测到的栅格点上的标记信息的误差对常规的成熟的路径搜索算法规划的导航路径的影响,尤其是误将自由栅格点标记为障碍物栅格点,让常规的成熟的路径算法规划的导航路径无法通过该误标记下的障碍物栅格点,不利于机器人通过所述狭窄区域。其中,所述候选导航路线的一个端点是起点时,所述候选导航路线的另一个端点是终点;所述候选导航路线的一个端点是终点时,所述候选导航路线的另一个端点是起点。
步骤S105、将步骤S104搜索到的子节点所属的一条候选导航路线的另一个端点设置为待遍历的节点,并可以选择加入所述待遍历节点缓存空间,同时将步骤S103搜索到的不在步骤S104对应的一条候选导航路线上的自由栅格点都设置为待遍历的节点,并可以选择加入步骤S101创建的同一个待遍历节点缓存空间内,同时,记录加入所述待遍历的节点的父节点的位置信息,优选地,将路径节点的位置信息连同对应的父节点的位置信息一起存入所述待遍历节点缓存空间内;在当前执行的步骤S105中设置为所述待遍历的节点当中,除了所属的一条候选导航路线的另一个端点之外,其余路径节点的父节点都是步骤S103所述的当前父节点;优选地,所属的一条候选导航路线的另一个端点的父节点是在所属候选导航路线上的相邻的节点或是所属候选导航路线之外的相邻的节点,以便于搜索到该端点时直接通过其父节点来回溯出该条候选导航路线的位置分布信息,加快路径规划的速度。
其中,选择所属的一条候选导航路线的其中一个端点而不是两个端点以及这两个端点之间的中间节点设置为所述待遍历的节点的原因在于:所属的一条候选导航路线是预先搜索出来的,在执行所述路径融合规划方法之前就已知搜索出来相关坐标点集合并能依次连接成路径段,这时只需要使用该候选导航路线的其中一个端点就可以起到相对应的路线的标识作用或作为该路线上相应的起始点的索引,因此,该步骤只使用邻域搜索到的一个端点所属的一条候选导航路线上的另一个端点(等效于未搜索到的路径节点)设置为所述待遍历的节点,从而后续可以通过以所述待遍历的节点为中心继续搜索与该候选导航路线相关联的具备通行意义的新的路径节点。至于不在步骤S104对应的一条候选导航路线上的属于所述当前父节点的邻域的自由栅格点,则用以提供具有最短路径基础的自由栅格点。
因此,在执行步骤S107的过程中,将步骤S104搜索到的子节点所属的一条候选导航路线的两个端点之间的所有中间节点与步骤S104搜索到的端点都设置为已遍历的节点。优选地,本实施例还为此专门创建一个已遍历节点缓存空间,用于存储已遍历的节点;需要说明的是,设置为所述已遍历的节点后,不允许将同一节点配置为待遍历的节点,即存在于所述已遍历节点缓存空间内节点不允许加入所述待遍历节点缓存空间,以实现在规划过程中,标识出已遍历的节点或视为已搜索出的节点,避免被重复搜索处理。其中,步骤S105和步骤S107可以是同时执行的。在执行完步骤S107之后进入执行步骤S108。
优选地,若将当前父节点的邻域中的不位于所述对应的一条候选导航路线上的自由栅格点或当前父节点的邻域中的所有自由栅格点都配置为待遍历的节点后,则将当前父节点由待遍历的节点改为已遍历的节点。优选地,若将所述当前父节点的邻域中的不位于所述对应的一条候选导航路线上的自由栅格点或当前父节点的邻域中的所有自由栅格点都加入待遍历节点缓存空间,则从所述待遍历节点缓存空间中移除这个当前父节点,再将这个当前父节点加入所述已遍历节点缓存空间,防止出现同一搜索中心进行重复扩展的现象,因为当前父节点已经在步骤S103执行过一次扩展。所述已遍历节点缓存空间可以以列表这一数据存储结构的形式存在于机器人的内部。
值得注意的是,所述候选导航网络内对应的一条候选导航路线是允许穿过栅格地图上标记的障碍物栅格点,而不考虑这个障碍物栅格点是否为地图误差而标记出的。
在本实施例中,栅格标记误差是由于传感器误差或地图漂移误差或障碍物移动等误差反映到栅格地图中形成的。
步骤S106、步骤S103在邻域搜索到的子节点不是所述候选导航网络内的所有候选导航路线的端点时,将步骤S103在邻域搜索到的自由栅格点都设置为待遍历的节点,并可以选择加入所述待遍历节点缓存空间内,并记录步骤S103在邻域搜索到的自由栅格点的父节点的位置信息,然后进入步骤S108。从而扩大邻域搜索过程中的子节点的搜索范围,也有利于在更大的范围内去扩展搜索所述候选导航网络内的路线起止点,使得所规划的路径具有更好的实用性和代表性。
作为步骤S105的一种实施例,当步骤S103在邻域搜索到的子节点是所述候选导航网络内对应的一条候选导航路线的起点时,将对应的一条候选导航路线的终点设置为待遍历的节点,并可以选择加入所述待遍历节点缓存空间,并记录对应的一条候选导航路线的终点的父节点的位置信息,以使得在所述候选导航网络内,每个候选导航路线使用其终点作为路线标识信息;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与同一条候选导航路线的起点都设置为已遍历的节点。沿着所述对应的一条候选导航路线的起点指向其终点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处。具体地,在所述候选导航网络内,从所述对应的一条候选导航路线的起点开始,从这条候选导航路线的起点指向其终点的第一路径延伸方向,依次记录这条候选导航路线的每个节点的父节点的位置信息,直至记录到这条候选导航路线的终点的父节点的位置信息,其中,这条候选导航路线的每一个节点的父节点是其在所属候选导航路线的第一路径延伸方向上的相邻的节点。所以,这条候选导航路线的终点的父节点是位于所属候选导航路线之外且与这条候选导航路线的终点相邻,而这条候选导航路线的终点的子节点是位于所属候选导航路线上且与这条候选导航路线的终点相邻;这条候选导航路线的起点的父节点是位于所属候选导航路线上且与这条候选导航路线的起点相邻,而这条候选导航路线的起点的子节点是位于所属候选导航路线上且与这条候选导航路线的起点相邻。
作为步骤S105的另一种实施例,当步骤S103在邻域搜索到的子节点是所述候选导航网络内对应的一条候选导航路线的终点时,将对应的一条候选导航路线的起点设置为待遍历的节点,并可以选择加入所述待遍历节点缓存空间,并记录对应的一条候选导航路线的起点的父节点的位置信息,以使得在所述候选导航网络内,每个候选导航路线使用其起点作为路线标识信息;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与同一条候选导航路线的终点都设置为已遍历的节点。其中,沿着所述对应的一条候选导航路线的终点指向其起点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处。具体地,在所述候选导航网络内,从这条候选导航路线的终点开始,沿着所述对应的一条候选导航路线的终点指向其起点的第二路径延伸方向,依次记录这条候选导航路线的每个节点的父节点的位置信息,直至记录到这条候选导航路线的起点的父节点的位置信息,这条候选导航路线的每一个节点的父节点是其在所属候选导航路线的第二路径延伸方向上的相邻的节点,所以,这条候选导航路线的起点的父节点是位于所属候选导航路线之外且与这条候选导航路线的起点相邻,而这条候选导航路线的起点的子节点是位于所属候选导航路线上且与这条候选导航路线的起点相邻。
上述基于步骤S105的两种实施例,将所述候选导航路线的起点或其终点配置为能唯一确定一条候选导航路线,作为对应一条候选导航路线的标识并记录对应的父节点信息,而同一候选导航路线的剩余节点则配置为已遍历的节点,减少路径规划过程中的搜索量,加快回溯出相匹配的候选导航路线。
步骤S108、从前述待遍历的节点中选择出路径代价和值最小的节点作为下一个父节点,再进入步骤S109。在本实施例中,通过比较所有的待遍历的节点的路径代价,选取一个更优的节点作为下一个父节点;其中,路径代价和值越小的节点优先级越高,路径代价和值最小(优先级最高)的节点作为下一个父节点,开始新的一次的邻域搜索,继续以所述下一个父节点为搜索中心搜索并加入新的邻域栅格点(区别于已存在于所述已遍历节点缓存空间内的节点)。
在本实施例中,所述路径代价和值是已遍历路径代价与预测路径代价相加或加权获得的,其中,已遍历路径代价是所有设置为待遍历的节点当中或所述待遍历节点缓存空间内的一个指定节点距离所述导航起点的代价,预测路径代价是同一个指定节点距离所述导航终点的代价,这些路径代价的定义是来源于A*算法的启发函数。当所述路径代价和值越小,则配置指定节点在所有的待遍历的节点当中的遍历优先级越高;本实施例基于A*启发函数,在计算两个相邻节点之间的移动代价时使用曼哈顿距离、对角距离或欧式距离来表示;其中,如果机器人当前所处的地图区域中只允许朝上下左右四个方向移动,则可以使用曼哈顿距离;如果机器人当前所处的地图区域中允许朝八个方向移动,则可以使用对角距离;如果机器人当前所处的地图区域中允许朝任何方向移动,则可以使用欧式距离。有利于搜寻出导航成本最低的最短路径。
在计算路径代价时,从导航起点开始,依次连接每个已到遍历过的栅格中心点直至连接到所述指定节点,再以栅格的边长获取当前连接而成的连线的长度,并将该长度作为所述已遍历路径代价;在计算路径代价时,将所述指定节点设置为父节点,获取该父节点对应的每个子节点到导航终点的所有连线方案,再以栅格的边长获取每个子节点对应的每个连线方案的长度,选取每个子节点对应的最短长度作为其对应的预测路径代价;其中,前述栅格点用栅格中心点表示,用于代表一个栅格的位置特征。本实施例计算的代价值是对机器人运动轨迹成本的度量,表示由起点行驶到指定节点,再行驶到终点的成本,包括路径长度。
需要说明的是,对于每个节点,可以计算其代价值,代价值是对机器人的运动轨迹成本的度量,表示由起点运动到该节点,再运动到终点的成本,包括路径长度、所需时间、是否发生碰撞、是否频繁切换速度方向等因素。
步骤S109、判断所述下一个父节点是否为所述导航终点,是则进入步骤S110,否则进入步骤S111。
步骤S111、将所述下一个父节点更新步骤S103所述的当前父节点,再返回步骤S103。
对于栅格地图,前述步骤对应的实施方式实质上是从导航起点开始,不断扩展到达导航终点的过程,扩展搜索的路径节点与所述候选导航路线连接形成最终规划出的导航路径。因此,步骤S111是属于循环执行的触发步骤,每一次返回步骤S103则为一次扩展,使得机器人从当前父节点出发,经过预设间隔时间的行驶,所能到达的节点为子节点;简而言之,每一次扩展表示机器人“走一步”,对应在地图中跨越一次栅格;需要说明的是,预设间隔时间是每次扩展的周期性时间,表示较小的时间单位,例如可以是5秒、10秒等,预设间隔时间越短,所规划的导航路径越精细,因此可以根据实际需求确定预设间隔时间。
在本实施例中,后续以下一个父节点为搜索中心进行扩展的过程中,若在邻域中搜索到当前父节点作为所述下一个父节点的子节点,则不用将当前父节点设置为所述待遍历的节点,并加入所述待遍历节点缓存空间内。
步骤S110、确定在前述的邻域搜索过程中已经搜索到(或扩展至)导航终点,则基于前述步骤记录下的父节点的位置信息,从所述导航终点开始,除了步骤S104所述候选导航路线的所有中间节点及其相应的一个端点之外,依次连接子节点及其父节点,直至连接到所述导航起点,规划出从所述导航起点到所述导航终点的路径。
与现有技术相比,前述实施例控制机器人在同一位置处,从导航起点开始选择出路径代价和值最小的节点作为父节点进行邻域搜索,并将邻域搜索得到的不处于候选导航路线上的自由栅格点和处于候选导航路线上具有标识意义的一个端点(起点或终点)都加入待遍历节点缓存空间,并记录相应的父节点以作为路径节点回溯信息,同时排除掉候选导航路线上的中间节点(虽然都是适用于这些狭窄区域的导航目标点)的对重复搜索造成的干扰,直到作为父节点是导航终点,再按照子节点和父节点的位置关系,从导航终点开始通过连接搜索到的候选导航路线反向连接到导航起点,从而将符合通行条件的候选导航路线融合到启发式搜索算法中以规划出一条导航路径,克服地图栅格中通道较窄而容易产生标记误差,使得机器人沿着规划好的整体导航路径有效进出这一狭窄区域,减少路径导航失败的概率。
具体地,当判断到所述的下一个父节点为所述导航终点时,所述基于前述记录的父节点的位置信息,从所述导航终点开始,除了步骤4所述候选导航路线的所有中间节点及其相应的一个端点之外,依次连接子节点及其父节点,直至连接到所述导航起点的具体步骤包括:
步骤51、从所述导航终点开始,基于已经记录下的父节点的位置信息或存储于所述待遍历节点缓存空间内的父节点及子节点的位置信息,连接所述导航终点及其父节点;然后进入步骤52。从而开始逐个节点地进行逆向连接。
步骤52、以当前确定的父节点为子节点,即将步骤51确定的父节点更新为子节点,包括位置信息;再基于前述记录的父节点的位置信息,连接该子节点的父节点;然后进入步骤53。
步骤53、重复步骤52,直至连接到步骤S105确定搜索出的对应一条候选导航路线的一个端点,然后从这条候选导航路线的另一个端点开始,连接所述对应的一条候选导航路线的另一个端点及其父节点,然后返回步骤52。其中,所述对应的一条候选导航路线的另一个端点的父节点是:步骤S105确定搜索出的端点指向所属的一条候选导航路线的另一端点的路径延伸方向上,与所述对应的一条候选导航路线的另一个端点相邻的且位于这条候选导航路线的外部的节点。
作为一种实施例,当步骤53连接到步骤S105确定搜索出的对应一条候选导航路线的起点时,基于前述实施例可知,所述对应一条候选导航路线的起点是步骤52更新过的子节点的父节点,即所述对应一条候选导航路线的起点成为所述对应的一条候选导航路线的起点指向其终点的路径延伸方向的反方向上与其相邻的节点的父节点,由此可确定沿着所述对应的一条候选导航路线的起点指向其终点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处,同时确定这条候选导航路线的起点及中间节点都属于不可重复搜索的节点,即已遍历的路径节点。所以步骤53从这条候选导航路线的终点开始,连接这条候选导航路线的终点及其父节点,因为这条候选导航路线的终点的父节点在这条候选导航路线的外部,以便于返回步骤52继续回溯至这条候选导航路线的外部区域。
作为另一种实施例,当步骤53连接到步骤S105确定搜索出的对应一条候选导航路线的终点时,基于前述实施例可知,所述对应一条候选导航路线的终点是步骤52更新过的子节点的父节点,即所述对应一条候选导航路线的终点成为所述对应的一条候选导航路线的起点指向其终点的路径延伸方向上与其相邻的节点的父节点,由此可确定沿着所述对应的一条候选导航路线的终点指向其起点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处,同时确定这条候选导航路线的起点及中间节点都属于不可重复搜索的节点,即已遍历的路径节点。所以步骤53从这条候选导航路线的起点开始,连接这条候选导航路线的起点及其父节点,因为这条候选导航路线的起点的父节点在这条候选导航路线的外部,以便于返回步骤52继续回溯至这条候选导航路线的外部区域。
步骤54、重复前述步骤52和前述步骤53,直至连接到所述导航起点,实现基于前述记录下的父节点的位置信息将前述步骤S101至步骤S111搜索出的父节点逐个逆向连接以得到从导航起点到导航终点的一条导航路径。在执行步骤51至步骤54的过程中,依次连接该子节点(包括候选导航路线的端点)的父节点再以该父节点为子节点的过程可称为“逆向扩展”,实质上是从导航终点开始,通过不断扩展并连接到候选导航路线的起止点,直至到达导航起点的路径节点回溯过程,逆向扩展的轨迹即:获得连接导航起点、能够穿越障碍物栅格点的候选导航路线、导航终点的导航路径。
与现有技术相比,经过前述步骤规划出上述导航路径之后,本实施例实现:在机器人经过狭窄区域的过程中,运用启发式搜索算法或增量启发式搜索算法搜索路径时,若检测到狭窄区域,则在狭窄区域附近由进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径切换为进入所述候选导航路线;若检测到离开狭窄区域,则由所述候选导航路线切换为进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径;让机器人可以有效穿过狭窄区域,提高穿越狭窄区域的导航成功率。
优选地,还可以将每个节点的状态参数标记入所述导航路径中,其中,每个节点的状态参数包括每个节点的时刻,即根据规划,机器人移动至每个节点的时刻。由此,可以将状态参数添加到所述导航路径中,例如将每个节点的时刻标注到导航路径中,当地图中存在所述狭窄区域时,可以推算出机器人是否会长时间困于所述狭窄区域内,从而在复杂障碍物布局场景中实现有效的路径规划,满足实际需求。
在前述实施例的基础上,若机器人当前检测到由处于所述狭窄区域变为没有处于所述狭窄区域,进入该新区域也可以视为机器人脱困,则开始执行所述路径规划方法,即开始迭代执行前述步骤S101至步骤S111,以规划出支持机器人自由进出所述狭窄区域的导航路径,克服地图漂移误差。从而在机器人离开所述狭窄区域时或离开当前的狭窄区域而即将进入新的狭窄区域之前,将已搜索过的适应于穿过所述狭窄区域的候选导航路线的坐标点融合到当前位置处使用启发式搜索算法中以规划出一条导航路径,而不是只依靠邻域搜索来进行导航路径规划,克服在狭窄区域处搜索到的自由栅格点容易被标记为障碍物栅格点而导致规划的路径无法通过狭窄区域的问题。
优选地,本实施例将非狭窄区域(包括空旷区域、障碍物区域)与机器人当前遍历的狭窄区域之间的间隔设定在一预设距离作为判定机器人是否进入新区域的一个依据,该预设距离可以为60厘米,如果大于或者等于60厘米,则表明机器人进入了一个新的区域,离开所述狭窄区域,此种情况也可以视为脱困。
一种芯片,该芯片用于存储程序代码,程序代码用于执行所述的一种路线优化方法,和/或执行所述的路径规划方法。基于整体栅格区域的区域可通行条件,控制机器人在正常工作移动的状态下搜索出能够克服地图漂移误差造成栅格不可通行问题的完备的路线点集合。
一种机器人,该机器人内置所述芯片,该芯片用于执行所述的一种路线优化方法,和/或执行所述的路径规划方法。使得机器人后续在检测到所述狭窄区域并规划经过所述狭窄区域时,使用从预先搜索出完备的候选导航路线在当前的狭窄区域内布局出完整的候选导航网络,进而利用路径搜索算法从完整的候选导航网络中规划出导航路径,有效克服传感器累计误差(间接原因)、地图漂移(直接原因)造成的栅格标记误差的影响,沿着相应的候选导航路线连接而成的导航路径,无障碍地穿过所述狭窄区域。
本领域内的技术人员应明白,本申请的实施例可提供为方法、***、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。 而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
以上所述,仅是本发明的较佳实施例而己,并非是对本发明作其它形式的限制,任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型,仍属于本发明技术方案的保护范围。
Claims (26)
1.一种路线优化方法,其特征在于,包括:
首先控制机器人沿着预设路径移动并规划出至少一条局部路线;其中,预设路径是预先设置的支持机器人通过狭窄区域的路径;
然后,在机器人已构建的栅格地图内随机搜索出第一预设数量的满足第一连通筛选条件的栅格点;
分别以所述满足第一连通筛选条件的栅格点、所有局部路线的起点及其终点为第一搜索中心,搜索出满足第二连通筛选条件的栅格点,再将第一搜索中心及其搜索出的所有满足第二连通筛选条件的栅格点相连接成第一可行路线,进而确定第一可行路线与所述局部路线相连接;
当所有局部路线的起点的子节点及所有局部路线的终点的子节点中都存在所述满足第一连通筛选条件的栅格点时,确定第一可行路线与所述局部路线相连接形成完备的候选导航网络;
当所有局部路线的起点的子节点及所有局部路线的终点的子节点中不是都存在所述满足第一连通筛选条件的栅格点时,分别以所有局部路线的起点及其终点为第二搜索中心,搜索出满足第三连通筛选条件的栅格点,再将第二搜索中心及其搜索出的所有满足第三连通筛选条件的栅格点相连接成第二可行路线,进而确定第一可行路线、第二可行路线与所述局部路线相连接形成完备的候选导航网络;
所述第一连通筛选条件是:当前随机搜索的自由栅格点不存在于可行路线节点集合内,且以当前随机搜索的自由栅格点为圆心、搜索半径为一个机身直径的区域内不存在障碍物栅格点且不存在未知栅格点,使得当前随机搜索的自由栅格点为第一连通节点;
所述第二连通筛选条件包括:与所述第一搜索中心的连线上不存在障碍物栅格点、且与所述第一搜索中心的连线长度小于预设距离阈值的栅格点是第一连通节点或第二连通节点;其中,预设距离阈值是根据具体环境情况而预先设定的;预设距离阈值大于单个栅格的边长;
所述第三连通筛选条件包括:在与所述第二搜索中心的连线上不存在障碍物栅格点且不存在未知栅格点的所有栅格点中,与第二搜索中心的连线长度最短的栅格点是第一连通节点。
2.根据权利要求1所述路线优化方法,其特征在于,所述在机器人已构建的栅格地图内随机搜索出第一预设数量的满足第一连通筛选条件的栅格点的方法具体包括:
基于机器人已构建的栅格地图的尺寸与单个栅格的边长的比值,作乘积运算求取机器人已构建的栅格地图中分布所有栅格的总数量,并确定为所述第一预设数量;其中,栅格点用对应一个栅格的栅格中心点表示;
以机器人的当前位置为搜索中心,在所述栅格地图内随机搜索出满足第一连通筛选条件的栅格点并加入预先创建的可行路线集合,直到随机搜索并获得所述第一预设数量的满足第一连通筛选条件的栅格点;
其中,将满足第一连通筛选条件的栅格点设置为第一连通节点。
3.根据权利要求1所述路线优化方法,其特征在于,所述路线优化方法还包括:
将所述局部路线的起点及其终点都设置为第二连通节点;同时在所述局部路线上,将所述局部路线的起点及其终点之间的中间节点设置为第三连通节点;
在预测通行节点集合内记录第二连通节点的子节点的位置信息,再将第二连通节点加入所述可行路线节点集合;并且,在同一个预测通行节点集合内记录第三连通节点的子节点的位置信息,再将第三连通节点加入所述可行路线节点集合;以使得所述可行路线节点集合内存有一个所述预测通行节点集合内用于代表对应的唯一一条局部路线的完整路径节点;所述预测通行节点集合是机器人沿着所述预设路径移动过程中创建的;
其中,第二连通节点及其子节点的位置关系是相邻且相连通的;第三连通节点及其子节点的位置关系是相邻且相连通的。
4.根据权利要求3所述路线优化方法,其特征在于,所述路线优化方法还包括:当所述第二连通节点和第三连通节点加入所述可行路线节点集合时,基于记录的子节点的位置信息,将所述可行路线节点集合内的所述第二连通节点和第三连通节点按照子节点及其父节点的邻接关系依次连接为所述局部路线。
5.根据权利要求2所述路线优化方法,其特征在于,所述分别以所述满足第一连通筛选条件的栅格点、所有局部路线的起点及其终点为第一搜索中心,搜索出满足第二连通筛选条件的栅格点,再将第一搜索中心及其搜索出的所有满足第二连通筛选条件的栅格点相连接成第一可行路线的方法具体包括:
分别以所述第一连通节点和所述第二连通节点为所述第一搜索中心,在所述可行路线节点集合内搜索出满足第二连通筛选条件的栅格点,再将满足第二连通筛选条件的栅格点设置为对应的第一搜索中心的第一合法子节点并加入对应第一搜索中心的子节点集合内,其中,对应第一搜索中心的子节点集合也存在于所述可行路线节点集合内,使得每个所述第一连通节点和每个所述第二连通节点在所述可行路线节点集合内都存在一个对应的子节点集合;
然后将同一个第一搜索中心和所有第一合法子节点相连接为所述第一可行路线,进而确定第一可行路线与所述局部路线相连接。
6.根据权利要求5所述路线优化方法,其特征在于,所述当所有局部路线的起点的子节点及所有局部路线的终点的子节点中都存在所述满足第一连通筛选条件的栅格点时,确定第一可行路线与所述局部路线相连接形成完备的候选导航网络的方法包括:
若检测到所述可行路线节点集合内所有第二连通节点的子节点中都存在第一连通节点,则确定所有第二连通节点所属的局部路线的两端分别和加入所述对应第一搜索中心的子节点集合内的所有第一连通节点相连接,进而确定所述局部路线和所述第一可行路线连接形成完备的候选导航网络;
其中,当前检测的第二连通节点的子节点是处于同一个第二连通节点对应的子节点集合内。
7.根据权利要求6所述路线优化方法,其特征在于,所述当所有局部路线的起点的子节点及所有局部路线的终点的子节点中不是都存在所述满足第一连通筛选条件的栅格点时,分别以所有局部路线的起点及其终点为第二搜索中心,搜索出满足第三连通筛选条件的栅格点,再将第二搜索中心及其搜索出的所有满足第三连通筛选条件的栅格点相连接成第二可行路线的方法包括:
当检测到所述可行路线节点集合内所有的第二连通节点的子节点中不是都存在第一连通节点时,分别以所述可行路线节点集合内每个第二连通节点为所述第二搜索中心,然后在所述可行路线节点集合内搜索出满足第三连通筛选条件的栅格点,再将满足第三连通筛选条件的栅格点设置为对应的第二搜索中心的第三合法子节点并加入对应第二搜索中心的子节点集合内,使得每个所述第二连通节点对应的子节点集合的元素增加,然后将同一个第二搜索中心和所有第三合法子节点相连接为所述第二可行路线,进而确定所述局部路线和所述第二可行路线相连接;
其中,当前检测的第二连通节点的子节点是处于同一个第二连通节点对应的子节点集合内。
8.根据权利要求1至7任一项所述路线优化方法,其特征在于,在所述完备的候选导航网络中,所述局部路线的两端分别用于连接所述第一可行路线的一端点和/或所述第二可行路线的一端点,但是所述第一可行路线和/或所述第二可行路线都不连接上所述局部路线的中间节点;以便于利用路径搜索算法在所述完备的候选导航网络中搜索出一条由预先设置的导航起点通往预先设置的导航终点的路线;
其中,所述狭窄区域是两个或两个以上障碍物的夹缝通道,夹缝通道是其中两个障碍物之间的最窄处对应的缝隙,这个缝隙宽度是大于或等于机器人的机身直径;
其中,节点集合是占据内存空间的数据结构。
9.根据权利要求8所述路线优化方法,其特征在于,所述局部路线的规划方法包括:
步骤S1、控制机器人沿着预设路径移动的过程中,直至判断到预搜索区域满足第一预设圆域通行条件,再进入步骤S2;
步骤S2、将机器人的当前位置记录为第一路径节点,同时创建一个新的预测通行节点集合,并将第一路径节点加入所述预测通行节点集合,然后进入步骤S3;
步骤S3、每当机器人沿着所述预设路径移动一个机身直径的直线距离时,将机器人的当前位置记录为第二路径节点,再进入步骤S4;
步骤S4、当判断到第二路径节点满足第二预设圆域通行条件时,将第二路径节点加入步骤S2所述的预测通行节点集合;
其中,所述预测通行节点集合内存储的路径节点,按照先后加入的次序连接成对应的一条局部路线,以使得一个所述预测通行节点集合对应代表唯一的一条局部路线。
10.根据权利要求9所述路线优化方法,其特征在于,当判断到第二路径节点不满足第二预设圆域通行条件时,还包括:
步骤S6、根据所述预测通行节点集合内部存有的路径节点的数目,保存所述预测通行节点集合到一个局部路线节点集合的内部,然后返回步骤S1;
其中,一个局部路线节点集合存在至少一个预测通行节点集合;在同一个局部路线节点集合内,任一个所述预测通行节点集合内部的首元素及其尾元素都是唯一的,且同一个所述预测通行节点集合内部的首元素及其尾元素不是相同的。
11.根据权利要求10所述路线优化方法,其特征在于,所述根据所述预测通行节点集合内部存有的路径节点的数目,保存所述预测通行节点集合到同一所述局部路线节点集合的内部,然后返回步骤S1的方法具体包括:
当所述预测通行节点集合内部存有的路径节点的数目小于2时,删除所述预测通行节点集合,再返回步骤S1以创建一个新的预测通行节点集合;
当所述预测通行节点集合内部存有的路径节点的数目大于或等于2时,使用所述预测通行节点集合单独表示一条局部路线,并保存到所述局部路线节点集合内,以供路径搜索算法调用,再返回步骤S1以创建一个新的预测通行节点集合;
其中,每一个预测通行节点集合内的首元素对应的路径节点是对应的一条局部路线的起点,每一个预测通行节点集合内的尾元素对应的路径节点是对应的一条局部路线的终点。
12.根据权利要求11所述路线优化方法,其特征在于,所述第一预设圆域通行条件包括:
第一不可通行区域的面积与所述预搜索区域的面积的比值大于第一通行评估值;
其中,第一不可通行区域是在第一圆形区域对应的栅格区域内,由未知栅格点和障碍物栅格点组成的栅格区域;第一通行评价值是为了克服构建栅格地图中存在的自由栅格点的标记误差而设置的预判阈值;
其中,所述预搜索区域是以机器人的当前位置为圆心、机器人的机身直径为半径的第一圆形区域。
13.根据权利要求9所述路线优化方法,其特征在于,所述第二预设圆域通行条件包括:
在以机器人的当前位置为圆心、机器人的机身直径为半径的第二圆形区域内,第二不可通行区域的面积与第二圆形区域的面积的比值大于第二通行评价值;其中,第二不可通行区域是在第二圆形区域对应的栅格区域内,由未知栅格点和障碍物栅格点组成的栅格区域;第二通行评价值是用于克服构建栅格地图中存在的自由栅格点的标记误差而设置的判断阈值,且大于第一通行评价值。
14.根据权利要求9所述路线优化方法,其特征在于,所述第二预设圆域通行条件包括:
在以最新记录的第一路径节点为圆心、预设倍数的机身直径为半径的第二圆形区域内,使用路径搜索算法搜索出最新记录的第二路径节点通向最新记录的第一路径节点的路径;
其中,预设倍数的机身直径设置为:让第二圆形区域不与其它已标记的栅格区域交叉。
15.根据权利要求9所述路线优化方法,其特征在于,在执行所述步骤S1的过程中,还包括:先判断机器人是否移动至所述预设路径的终点,是则停止执行所述局部路线的规划方法,否则控制机器人继续沿着预设路径移动,并判断所述预搜索区域是否满足第一预设圆域通行条件;
在执行步骤S3的过程中,还包括:先判断机器人是否移动至所述预设路径的终点,是则停止执行所述局部路线的规划方法,否则控制机器人继续沿着预设路径移动。
16.一种路径规划方法,其特征在于,该路径规划方法包括:执行权利要求1至15任一项所述的路线优化方法;
在执行完所述路线优化方法之后,将所述第一可行路线、所述第二可行路线和所述局部路线都配置为候选导航网络中的候选导航路线;还包括:
步骤1、在栅格地图中设置导航起点和导航终点;
步骤2、将导航起点设置为当前父节点,并设置为待遍历的节点;
步骤3、以当前父节点为搜索中心在栅格地图中进行邻域搜索,其中,与当前父节点相邻的8个栅格点分别作为其子节点;
步骤4、当步骤3在邻域搜索到的子节点是预先搜索出的所述候选导航网络内对应的一条候选导航路线的一个端点时,将对应的一条候选导航路线的另一个端点设置为待遍历的节点;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与该端点都设置为已遍历的节点,并将步骤3搜索到的不在所述对应的一条候选导航路线上的自由栅格点设置为待遍历的节点;然后进入步骤5;其中,所有设置为待遍历的节点都被对应记录下其父节点的位置信息,用以后续回溯路径节点;
步骤5、从所有的待遍历的节点中选择出路径代价和值最小的节点作为下一个父节点,再判断所述下一个父节点是否为所述导航终点,是则基于前述记录的父节点的位置信息,从所述导航终点开始,除了步骤4所述候选导航路线的所有中间节点及其相应的一个端点之外,依次连接子节点及其父节点,直至连接到所述导航起点,规划出从所述导航起点到所述导航终点的路径;否则将所述下一个父节点更新步骤3所述的当前父节点,再返回步骤3。
17.根据权利要求16所述路径规划方法,其特征在于,步骤4还包括:
步骤3在邻域搜索到的子节点不是所有候选导航路线的端点时,将步骤3在邻域搜索到的自由栅格点都设置为待遍历的节点,并记录步骤3在邻域搜索到的自由栅格点的父节点的位置信息,然后进入步骤5。
18.根据权利要求17所述路径规划方法,其特征在于,在所述步骤4中,当步骤3在邻域搜索到的子节点是所述候选导航网络内对应的一条候选导航路线的起点时,将对应的一条候选导航路线的终点设置为待遍历的节点,并记录所述对应的一条候选导航路线的终点的父节点的位置信息,以使得在所述候选导航网络内,每条候选导航路线使用其终点作为路线标识信息;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与同一条候选导航路线的起点都设置为已遍历的节点;
其中,沿着所述对应的一条候选导航路线的起点指向其终点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处。
19.根据权利要求17所述路径规划方法,其特征在于,在所述步骤4中,当步骤3在邻域搜索到的子节点是所述候选导航网络内对应的一条候选导航路线的终点时,将对应的一条候选导航路线的起点设置为待遍历的节点,并记录对应的一条候选导航路线的起点的父节点的位置信息,以使得在所述候选导航网络内,每条候选导航路线使用其起点作为路线标识信息;同时将对应的一条候选导航路线的两个端点之间的所有中间节点与同一条候选导航路线的终点都设置为已遍历的节点;
其中,沿着所述对应的一条候选导航路线的终点指向其起点的路径延伸方向,所述对应的一条候选导航路线上的每一个节点的父节点位于与该节点相邻的节点位置处。
20.根据权利要求18所述路径规划方法,其特征在于,在所述步骤5中,所述基于前述记录的父节点的位置信息,从所述导航终点开始,除了步骤4所述候选导航路线的所有中间节点及其相应的一个端点之外,依次连接子节点及其父节点,直至连接到所述导航起点的具体步骤包括:
步骤51、基于前述记录的父节点的位置信息,从所述导航终点开始,连接所述导航终点及其父节点;然后进入步骤52;
步骤52、以当前确定的父节点为子节点,再基于前述记录的父节点的位置信息,连接该子节点的父节点;
步骤53、重复步骤52,直至连接到步骤4所述对应的一条候选导航路线的一个端点,然后从步骤4所述对应的一条候选导航路线的另一个端点开始,连接步骤4所述对应的一条候选导航路线的另一个端点及其父节点,然后返回步骤52;
步骤54、重复步骤52和步骤53,直至连接到所述导航起点,实现逆向连接得到从导航起点到导航终点的一条导航路径。
21.根据权利要求20所述路径规划方法,其特征在于,在所述步骤5中,所述路径代价和值是已遍历路径代价与预测路径代价相加或加权得到,其中,已遍历路径代价是所有的待遍历的节点中的一个指定节点距离所述导航起点的代价,预测路径代价是同一个指定节点距离所述导航终点的代价;当所述路径代价和值越小,则配置指定节点在所有的待遍历的节点当中的遍历优先级越高;
其中,两个相邻节点之间的移动代价使用曼哈顿距离、对角距离或欧式距离来表示。
22.根据权利要求21所述路径规划方法,其特征在于,从导航起点开始,依次连接每个已到遍历过的栅格中心点直至连接到所述指定节点,再以栅格的边长获取当前连接而成的连线的长度,并将该长度作为所述已遍历路径代价;
将所述指定节点设置为父节点,获取该父节点对应的每个子节点到导航终点的所有连线方案,再以栅格的边长获取每个子节点对应的每个连线方案的长度,选取每个子节点对应的最短长度作为其对应的预测路径代价;
其中,栅格点用栅格中心点表示,用于代表一个栅格的位置特征。
23.根据权利要求16所述路径规划方法,其特征在于,设置为所述已遍历的节点后,不允许将同一节点配置为待遍历的节点。
24.根据权利要求23所述路径规划方法,其特征在于,所述步骤4还包括:若将当前父节点的邻域中的不位于所述对应的一条候选导航路线上的自由栅格点或当前父节点的邻域中的所有自由栅格点都配置为待遍历的节点后,则将当前父节点由待遍历的节点改为已遍历的节点。
25.一种芯片,该芯片用于存储程序代码,其特征在于,程序代码用于执行权利要求1至15任一项所述的一种路线优化方法,和/或执行权利要求16所述的路径规划方法。
26.一种机器人,其特征在于,该机器人内置权利要求25所述芯片,该芯片用于执行权利要求1至15任一项所述的一种路线优化方法,和/或执行权利要求16所述的一种路径规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110502487.3A CN113219975B (zh) | 2021-05-08 | 2021-05-08 | 一种路线优化方法、路径规划方法、芯片及机器人 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110502487.3A CN113219975B (zh) | 2021-05-08 | 2021-05-08 | 一种路线优化方法、路径规划方法、芯片及机器人 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113219975A CN113219975A (zh) | 2021-08-06 |
CN113219975B true CN113219975B (zh) | 2024-04-05 |
Family
ID=77094281
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110502487.3A Active CN113219975B (zh) | 2021-05-08 | 2021-05-08 | 一种路线优化方法、路径规划方法、芯片及机器人 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113219975B (zh) |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113156970B (zh) * | 2021-05-08 | 2023-06-09 | 珠海一微半导体股份有限公司 | 一种通行区域的路径融合规划方法、机器人及芯片 |
CN113759905A (zh) * | 2021-08-30 | 2021-12-07 | 北京盈迪曼德科技有限公司 | 机器人路径规划方法、装置及机器人 |
CN114355887B (zh) * | 2021-12-03 | 2023-08-08 | 云鲸智能(深圳)有限公司 | 机器人窄道通行方法、装置、机器人及存储介质 |
CN114397893B (zh) * | 2021-12-28 | 2024-02-02 | 深圳银星智能集团股份有限公司 | 路径规划方法、机器人清扫方法及相关设备 |
CN114355925B (zh) * | 2021-12-29 | 2024-03-19 | 杭州海康机器人股份有限公司 | 路径规划方法、装置、设备及计算机可读存储介质 |
CN114756026B (zh) * | 2022-04-07 | 2024-04-19 | 青岛沃柏斯智能实验科技有限公司 | 一种实验环境安全检查的巡视控制*** |
CN115061470B (zh) * | 2022-06-30 | 2023-03-21 | 哈尔滨理工大学 | 适用狭窄空间的无人车改进teb导航方法 |
CN115014362B (zh) * | 2022-08-09 | 2022-11-15 | 之江实验室 | 一种基于合成单元的牛耕式全覆盖路径规划方法和装置 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101963510A (zh) * | 2010-10-26 | 2011-02-02 | 广东威创视讯科技股份有限公司 | 一种随机路线图的快速路径规划方法及增强方法 |
CN105929843A (zh) * | 2016-04-22 | 2016-09-07 | 天津城建大学 | 一种基于改进蚁群算法的机器人路径规划方法 |
CN107063280A (zh) * | 2017-03-24 | 2017-08-18 | 重庆邮电大学 | 一种基于控制采样的智能车辆路径规划***及方法 |
CN107943025A (zh) * | 2017-11-09 | 2018-04-20 | 珠海市微半导体有限公司 | 机器人被困的检测方法及脱困的处理方法 |
CN109445444A (zh) * | 2018-12-25 | 2019-03-08 | 同济大学 | 一种障碍物集中环境下的机器人路径生成方法 |
CN109443363A (zh) * | 2018-11-09 | 2019-03-08 | 厦门大学 | 分治确定性路径寻优算法 |
CN109506655A (zh) * | 2018-10-19 | 2019-03-22 | 哈尔滨工业大学(威海) | 基于非均匀建模的改进蚁群路径规划算法 |
CN110083165A (zh) * | 2019-05-21 | 2019-08-02 | 大连大学 | 一种机器人在复杂狭窄环境下路径规划方法 |
CN110595482A (zh) * | 2019-10-28 | 2019-12-20 | 深圳市银星智能科技股份有限公司 | 一种带避障权重的路径规划方法、装置及电子设备 |
CN112180924A (zh) * | 2020-09-28 | 2021-01-05 | 珠海市一微半导体有限公司 | 一种导航至密集障碍物的移动控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11530921B2 (en) * | 2018-09-28 | 2022-12-20 | Intel Corporation | Method of generating a collision free path of travel and computing system |
-
2021
- 2021-05-08 CN CN202110502487.3A patent/CN113219975B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101963510A (zh) * | 2010-10-26 | 2011-02-02 | 广东威创视讯科技股份有限公司 | 一种随机路线图的快速路径规划方法及增强方法 |
CN105929843A (zh) * | 2016-04-22 | 2016-09-07 | 天津城建大学 | 一种基于改进蚁群算法的机器人路径规划方法 |
CN107063280A (zh) * | 2017-03-24 | 2017-08-18 | 重庆邮电大学 | 一种基于控制采样的智能车辆路径规划***及方法 |
CN107943025A (zh) * | 2017-11-09 | 2018-04-20 | 珠海市微半导体有限公司 | 机器人被困的检测方法及脱困的处理方法 |
CN109506655A (zh) * | 2018-10-19 | 2019-03-22 | 哈尔滨工业大学(威海) | 基于非均匀建模的改进蚁群路径规划算法 |
CN109443363A (zh) * | 2018-11-09 | 2019-03-08 | 厦门大学 | 分治确定性路径寻优算法 |
CN109445444A (zh) * | 2018-12-25 | 2019-03-08 | 同济大学 | 一种障碍物集中环境下的机器人路径生成方法 |
CN110083165A (zh) * | 2019-05-21 | 2019-08-02 | 大连大学 | 一种机器人在复杂狭窄环境下路径规划方法 |
CN110595482A (zh) * | 2019-10-28 | 2019-12-20 | 深圳市银星智能科技股份有限公司 | 一种带避障权重的路径规划方法、装置及电子设备 |
CN112180924A (zh) * | 2020-09-28 | 2021-01-05 | 珠海市一微半导体有限公司 | 一种导航至密集障碍物的移动控制方法 |
Non-Patent Citations (2)
Title |
---|
基于狭窄通道识别的机器人路径规划研究;钟建冬;《国博士学位论文全文数据库 (信息科技辑)》(第4期);第I140-24页 * |
车型移动机器人的SPRM路径规划方法;孙凤池等;《机器人》;第27卷(第4期);第325-329页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113219975A (zh) | 2021-08-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113219975B (zh) | 一种路线优化方法、路径规划方法、芯片及机器人 | |
CN113156970B (zh) | 一种通行区域的路径融合规划方法、机器人及芯片 | |
CN109115226B (zh) | 基于跳点搜索的多机器人冲突避免的路径规划方法 | |
CN105739504B (zh) | 一种机器人工作区域的排序方法及排序*** | |
EP4002049A1 (en) | Systems and methods for optimizing route plans in an operating environment | |
CN107913039B (zh) | 用于清洁机器人的区块选择方法、装置及机器人 | |
CN111938513B (zh) | 一种机器人越障的沿边路径选择方法、芯片及机器人 | |
CN113156956B (zh) | 机器人的导航方法、芯片及机器人 | |
CN105652838A (zh) | 一种基于时间窗的多机器人路径规划方法 | |
CN104615138A (zh) | 一种划分移动机器人室内区域动态覆盖方法及其装置 | |
CN113190010B (zh) | 一种沿边绕障路径规划方法、芯片及机器人 | |
CN108189039B (zh) | 一种移动机器人的行进方法及装置 | |
CN113009916B (zh) | 一种基于全局地图探索的路径规划方法、芯片及机器人 | |
CN113110497B (zh) | 基于导航路径的沿边绕障路径选择方法、芯片及机器人 | |
CN113741453B (zh) | 一种非结构化环境的路径规划方法、装置、设备和介质 | |
CN112947486A (zh) | 移动机器人的路径规划方法、芯片及移动机器人 | |
CN113867336B (zh) | 一种适用于复杂场景下移动机器人路径导航及规划方法 | |
CN113467482A (zh) | 一种自清洁的清洁机器人清扫路径规划方法及清洁机器人 | |
CN113110499A (zh) | 一种通行区域的判定方法、路线搜索方法、机器人及芯片 | |
CN111643008A (zh) | 一种扩展分区清洁方法、装置及计算机可读存储介质 | |
CN111552290B (zh) | 一种机器人沿墙找直线的方法及清扫方法 | |
CN115685982A (zh) | 基于连通图和迭代搜索的导航路径规划方法 | |
CN113238549A (zh) | 机器人基于可直达的节点的路径规划方法、芯片及机器人 | |
CN116429138A (zh) | 路径规划方法、装置、车辆及存储介质 | |
CN113110473B (zh) | 基于连通性的区域判定方法、芯片及机器人 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
CB02 | Change of applicant information |
Address after: 519000 2706, No. 3000, Huandao East Road, Hengqin new area, Zhuhai, Guangdong Applicant after: Zhuhai Yiwei Semiconductor Co.,Ltd. Address before: 519000 room 105-514, No. 6, Baohua Road, Hengqin new area, Zhuhai City, Guangdong Province (centralized office area) Applicant before: AMICRO SEMICONDUCTOR Co.,Ltd. |
|
CB02 | Change of applicant information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |