路径计划装置及方法、成本评估装置、以及移动体的制作方法

专利名称：路径计划装置及方法、成本评估装置、以及移动体的制作方法
技术领域：
本发明涉及在被包含于用网格表示的区域中的从起点至终点的路径之中选择其 成本(cost)最小的路径的路径计划装置及方法、用于评估该路径计划装置下的成本的成 本评估装置、以及安装了该路径计划装置的移动体。
背景技术：
关于以往的路径探索装置，专利文献1公开了求出移动机器人从出发点前往目的
地的路径的移动机器人用路径计划系统。该路径计划系统包括使用环境中的物体的几何
图生成表示概率性地表示了位置姿势的物体所占有的空间的分布的网格图的网格图生成
单元、根据生成的网格图并使用与物体的位置姿势的不确定性对应的距离生成沃罗诺伊图
(Voronoi图)的沃罗诺伊图生成单元、以及在生成的沃罗诺伊图上将路径长度和与物体的
冲撞可能性作为成本来探索从所提供的出发点前往目的地的路径的路径探索单元。 根据该构成，使用与物体的位置姿势的不确定性对应的距离生成沃罗诺伊图，在
所生成的分配了权重的沃罗诺伊图上进行成本计算并进行路径的探索，因此可生成比以往
的沃罗诺伊图更安全的路径。 如此，关于以往的路径计划技术，公知有如上述专利文献1那样将移动空间近似 成网格状并对连接邻接的网格点的路径的成本进行评估的技术。在这样的路径探索系统 中，被探索的路径被表现为连接邻接的网格点间的线段的集合。因此，将路径的成本作为各 线段之和来进行评估。 专利文献1 :日本专利文献特开2005-32196号公报

发明内容
发明要解决的问题 这里，如果仅用线段的长度进行成本评估，则有时发生当进行了最短路径探索时 存在多个最短路径的情况。图7F是表示从起点至终点的最短距离的图，图7A至图7E是表 示将网格点之间连接起来时的最短距离的图。图7A至图7E中示出的五个路径成本没有差 异。尽管图7F中示出的理想型路径是用直线连接起点和终点的路径，但是在图7A至图7E 中示出的五个路径之中，与该理想型路径接近的是图7E中示出的路径。可是，在以往的路 径探索系统中，存在选择图7A至图7D中示出的路径的情况，例如当安装了该路径探索系统 时，存在发现机器人会不自然地移动之类的问题。 S卩，在离散成网格格子状的探索空间中，由于路径长度被表示为连接网格点的线
段的单纯的总和，因此没能对连接任意两点的直线达到最佳的情况进行识别。因此，在以往
的路径探索系统中，由于计算出图7A至图7E的路径长度是相同的，因此有时会生成图7A
至图7D中示出的路径而不是图7E中示出的路径。如此，在以往的路径探索系统中存在以
下问题路径的角度分解度低，无法探索接近于理想型最短路径的网格路径。 本发明是为了解决这种问题而作出的，其目的在于提供一种可在用网格表示的探索空间中生成最佳路径的路径探索装置和路径探索方法、用于生成在路径探索装置中使用 的最佳路径的成本评估装置、以及安装了路径探索装置的移动体。
用于解决问题的手段 本发明的路径探索装置在被包含于用网格表示的区域的从起点至终点的路径之 中选择其成本最小的路径，所述路径探索装置的特征在于，包括路径成本计算部，如下计 算所述起点至所述终点的路径成本将各网格作为节点，将从当前注视的注视节点向与其 邻接的邻接节点延伸的边的成本作为通过将从该注视节点沿路径向所述起点方向追溯了 预定数量的节点作为基准节点并以(表示从该基准节点至所述邻接节点的最短距离的第 一距离)-(表示从所述基准节点至所述注视节点的最短距离的第二距离)而求出的值；以 及路径确定部，根据所述路径成本确定路径。 在本发明中，将从注视节点向与邻接节点延伸的边的成本从从基准节点至邻接节 点(位于边的顶端的顶端节点)第一距离和从基准节点至注视节点的(位于边的根源的顶 端节点)第二距离求出，而不是单纯地作为其距离。由于基准节点是从注视节点追溯了一 个以上的节点，并基于多个节点间的最短距离计算边的成本，因此与一律用边的距离来进 行成本评估的相比，可进行基于其边的模式的更详细的成本评估，从而能够生成路径的角 度分解度上升而与理想型的最短距离更接近的路径。 另外，也可以是，所述路径成本计算部将从所述起点至所述终点的路径成本计算 为构成该路径的边的成本之和。可将用(第一距离-第二距离)求出的边的成本的总和作 为路径成本，而不是单纯的距离。 另外，也可以是，所述路径成本计算部参照存储了所述第一距离和所述第二距离 或者使用(第一距离-第二距离)求出的成本的数据库来计算从所述起点至所述终点的成 本。通过参照数据库，可高速化路径成本计算。 另外，也可以是，还包括数据库生成部，该数据库生成部通过计算(所述第一距 离-所述第二距离)来构建所述数据库。例如可在执行路径探索的同时，生成数据库。
另外，也可以是，包括边生成部，当将从所述起点至任意的节点的成本作为该任 意的节点的成本时，在位于边的顶端的顶端节点之中，将节点的成本最小的节点作为注视 节点，并生成向位于注视节点的至少前方、斜右前、以及斜左前的邻接节点延伸的边；以及 边成本计算/读出部，计算或从数据库读出所述第一距离和所述第二距离；所述路径成本 计算部将从所述起点至所述邻接节点的路径成本计算为构成该路径的边的成本之和，当所 述邻接节点为所述终点时，所述路径确定部选择从所述起点至所述终点的成本最小的路 径。从注视节点生成边，并求出边的成本，从而可将构成路径的边的成本的总和求出为路径 成本。 另外，也可以是，当所述注视节点为所述起点时，所述边生成部将边延伸到该起点 的八个近旁的邻接节点。由此，即使在对起点的终点位置不清楚的情况下也能够延伸边。另 夕卜，也可以从起点仅向终点附近的方向延伸边。 另外，也可以是，所述边成本计算/读出部将从所述注视节点延伸至所述邻接节
点的边的成本作为所述注视节点与所述邻接节点的距离，直至从所述起点至所述邻接节点
的节点数量成为所述预定数量，在没有达到预定数量时，将节点的成本作为距离。 同样，也可以是，所述路径成本计算部将所述邻接节点的成本作为构成从所述起点至所述邻接节点的路径的边的距离之和，直至从所述起点至所述邻接节点的节点数量成 为所述预定数量。 本发明的成本评估装置在路径探索中使用，该路径探索在被包含于用网格表示的
区域的从起点至终点的路径之中选择其成本最小的路径，所述成本评估装置的特征在于，
包括距离准备部，当将各网格作为节点、将从当前注视的注视节点延伸边的节点作为邻接
节点、以及将从所述注视节点沿路径向所述起点方向追溯了预定数量的节点作为基准节点
时，计算或从数据库读出表示从该基准节点至所述邻接节点的最短距离的第一距离和表示
从所述基准节点至所述注视节点的最短距离的第二距离；以及边成本计算部，将从所述注
视节点向所述邻接节点延伸的边的成本计算为(所述第一距离_第二距离)。 本发明的路径探索方法在被包含于用网格表示的区域的从起点至终点的路径之
中选择其成本最小的路径，所述路径探索方法的特征在于，包括路径成本计算工序，如下
计算所述起点至所述终点的路径成本将各网格作为节点，将从当前注视的注视节点向与
其邻接的邻接节点延伸的边的成本作为通过将从该注视节点沿路径向所述起点方向追溯
了预定数量的节点作为基准节点并以(表示从该基准节点至所述邻接节点的最短距离的
第一距离)-(表示从所述基准节点至所述注视节点的最短距离的第二距离)而求出的值；
以及路径确定工序，根据所述路径成本确定路径。 本发明的路径探索方法在被包含于用网格表示的区域的从起点至终点的路径之 中选择其成本最小的路径，所述路径探索方法的特征在于，包括第一工序，将各网格作为 节点，将边延伸到所述起点的八个近旁的邻接节点，将从该起点至所述邻接节点的距离计 算为该邻接节点的成本；第二工序，当将从所述起点至一个节点的成本作为该一个节点的 成本时，在位于边的顶端的顶端节点之中，将节点的成本最小的节点作为注视节点，将边延 伸至位于该注视节点的至少前方、斜右前、以及斜左前的邻接节点，计算该邻接节点的成 本；以及第三工序，当所述邻接节点为所述终点时，选择从所述起点至所述终点的成本最小 的路径；所述第二工序重复至所述邻接节点成为所述终点，所述第二工序包括距离准备 工序，当将从所述注视节点沿路径向起点方向追溯了预定数量的节点作为基准节点时，计 算或从数据库读出表示从该基准节点至所述邻接节点的最短距离的第一距离和表示从所 述基准节点至所述注视节点的最短距离的第二距离；边成本计算工序，将从所述注视节点 向所述邻接节点延伸的边的成本计算为(所述第一距离-第二距离)；以及路径成本计算 工序，将从所述起点至所述邻接节点的路径成本计算为构成该路径的边的成本之和。
在本发明中，通过(第一距离-第二距离)计算边成本，将该边成本之和作为路径 成本，在增加边的同时重复处理，直至邻接节点成为终点，由此可准确地评估路径探索下的 成本，可使路径的角度分解度上升。 本发明的移动体包括移动单元；驱动单元，驱动所述移动单元；路径探索单元， 将周围的环境用网格表示，并在被包含于该网格空间的从起点至终点的路径之中选择其成 本最小的路径；以及控制单元，基于所述路径探索单元的路径探索结果，对所述驱动单元进 行控制；所述路径探索单元包括路径成本计算单元，如下计算所述起点至所述终点的路 径成本将各网格作为节点，将从当前注视的注视节点向与其邻接的邻接节点延伸的边的 成本作为通过将从该注视节点沿路径向所述起点方向追溯了预定数量的节点作为基准节 点并以(表示从该基准节点至所述邻接节点的最短距离的第一距离)-(表示从所述基准节点至所述注视节点的最短距离的第二距离)而求出的值；以及路径确定单元，根据所述路 径成本确定路径。 根据本发明，提供一种可在用网格表示的探索空间中生成最佳的路径的路径探索 装置和路径探索方法、用于生成在路径探索装置中使用的最佳的路径的成本评估装置、以 及安装了路径探索装置的移动体。


图1是表示本发明的实施方式的双腿行走型的机器人的立体图； 图2是表示本发明的实施方式的机器人的框图； 图3是详细地表示本发明的实施方式的路径探索模块的图； 图4是用于说明在本发明的实施方式的路径确定装置的路径确定处理中使用的 边(edge)及其成本的模式图； 图5是同样用于说明在本发明的实施方式的路径确定装置的路径确定处理中使 用的边及其成本的模式图； 图6A是追溯两个部分的边时的边形状的图； 图6B是追溯两个部分的边时的边形状的图； 图6C是追溯两个部分的边时的边形状的图； 图6D是追溯两个部分的边时的边形状的图； 图6E是追溯两个部分的边时的边形状的图； 图6F是追溯两个部分的边时的边形状的图； 图6G是追溯两个部分的边时的边形状的图； 图6H是追溯两个部分的边时的边形状的图； 图7A是表示节点n7为起点并节点n24为终点的路径的图； 图7B是表示节点n7为起点并节点n24为终点的路径的图； 图7C是表示节点n7为起点并节点n24为终点的路径的图； 图7D是表示节点n7为起点并节点n24为终点的路径的图； 图7E是表示节点n7为起点并节点n24为终点的路径的图； 图7F是表示节点n7为起点并节点n24为终点的路径的图； 图8是表示以往的机器人的移动路径的图； 图9是表示本发明的实施方式的机器人的移动路径的图； 图10是表示本发明的实施方式的路径确定方法的流程图； 图11是表示本发明的实施方式的路径确定装置的边的成本计算方法的流程图； 图12A是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图12B是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图12C是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图12D是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图12E是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图12F是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图12G是按工序顺序表示本发明的实施方式的路径探索工序的模式 图12H是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图121是按工序顺序表示本发明的实施方式的路径探索工序的模式图； 图13A是按工序顺序表示以往的路径探索工序的模式图； 图13B是按工序顺序表示以往的路径探索工序的模式图； 图13C是按工序顺序表示以往的路径探索工序的模式图； 图13D是按工序顺序表示以往的路径探索工序的模式图； 图13E是按工序顺序表示以往的路径探索工序的模式图； 图13F是按工序顺序表示以往的路径探索工序的模式图； 图13G是按工序顺序表示以往的路径探索工序的模式图； 图13H是按工序顺序表示以往的路径探索工序的模式图； 图131是按工序顺序表示以往的路径探索工序的模式图； 图13J是按工序顺序表示以往的路径探索工序的模式图； 图13K是按工序顺序表示以往的路径探索工序的模式图； 图14是表示通过本发明的实施方式的路径探索方法生成的路径的图； 图15A是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存
在障碍物时的路径探索工序； 图15B是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15C是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15D是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15E是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15F是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15G是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15H是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图151是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存 在障碍物时的路径探索工序； 图15J是按工序顺序表示本发明的实施方式的路径探索工序的模式图并且是存
在障碍物时的路径探索工序。
标号说明
l机器人
2头部单元
3a、3b臂部单元
4a、4b腿部单元
9
5躯干部单元6拍摄部12图像识别模块13路径探索模块14行动确定模块15语音识别模块20路径输出部21边生成部22边成本计算/读出部22a距离准备部22b边成本计算部23路径成本计算部24路径确定部43障碍物101控制部102输入输出部103驱动部104电源部105外部存储部121相机122内置麦克123扬声器125中央部131马达132驱动器141电池142电池控制部
具体实施例方式
下面，参照附图来详细地说明适用本发明的具体实施方式
。该实施方式是将本发 明使用于安装在机器人等上的路径探索装置的实施方式。 在本实施方式的路径探索装置中，在网格状的探索空间中的路径探索中生成尽量 接近理想的最短距离的网格路径。另外，不仅考虑路径的线段的长度，而且还考虑其路径的 经过地点，由此评估路径中的各边(线段)的成本。这里，首先说明一下安装了这种路径探 索装置的机器人。 图1是表示本发明的实施方式的机器人的立体图。如图1所示，在机器人1的躯 干部单元5的预定位置连结有头部单元2、左右两个的臂部单元3a和3b、以及左右两个的 腿部单元4a和4b。本实施方式的机器人1构成为可通过内置于腿部单元4a、4b中的车轮 进行移动。另外，机器人也可以是双足或四足步行式的机器人。
10
头部单元2包括拍摄部6，可对周围预定范围进行拍摄。另外，头部单元2包括没 有图示的麦克、扬声器等，可识别来自用户的呼唤，或者对该呼唤进行回答。头部单元2被 连接成能够相对于躯干部单元5在平行于地面的水平面内向左右方向转动，通过转动头部 单元2而对应于状况改变拍摄范围，从而可对周围的环境进行拍摄。 通过内置于躯干部单元5的控制部所具有的计算处理部，按照预定的控制程序对 驱动包含在各臂部单元3a、3b中的关节部和包含在各腿部单元4a、4b中的车轮的量进行控 制，以确定各关节的关节驱动角度和车轮的旋转角度，从而使臂部单元3a、3b和腿部单元 4a、4b采取希望的位置和姿势。 图2是表示本实施方式的机器人的框图。机器人1包括控制部101、输入输出部 102、驱动部103、电源部104、以及外部存储部105等。 输入输出部102包括由用于获得周围的影像的CCD(Charge CoupledDevice)等形 成的相机121、用于收集周围的语音的一个或多个内置麦克122、用于输出语音而与用户进 行对话等的扬声器123、用于表现对用户的反应或感情等的LED 124、以及由接触传感器等 形成的传感器部125等。 另外，驱动部103包括马达131 、以及驱动马达的驱动器132等，按照用户的指示等 而使腿部单元4a、4b和臂部单元3a、3b采取动作。电源部104包括电池141、以及对其充放 电进行控制的电池控制部142，向各部供应电源。 外部存储部105由可装卸的HDD、光盘、光磁盘等形成，存储有各种程序和控制参 数等，将这些程序和数据根据需要供应给控制部101内的存储器(没有图示)等。
控制部包括CPU (Central Processing Unit) 、 ROM (Read On 1 yMemory) 、 RAM (Random Access Memory)、以及用于无线通信的接口等，对机器人1的各种动作进行控制。另外，该控 制部101例如包括按照存储在ROM中的控制程序对通过相机121获得的影像进行分析的图 像识别模块12、基于各种识别结果进行路径探索的路径探索模块13、基于各种识别结果选 择应采取的行动的行动确定模块14、以及进行语音识别的语音识别模块15等。特别是，在 本实施方式中，通过路径探索模块13生成与理想接近的探索路径，基于此控制驱动部103， 因此机器人1表现出更自然的移动动作。 这里，本实施方式的机器人1在路径探索模块13中将移动空间近似成网格状并探 索应选的路径。接着，说明本实施方式的路径探索模块。 图3是详细地表示本实施方式的路径探索模块的图。路径探索模块13包括路径输 出部20、以及距离和成本数据库(DB)30。在距离和成本DB30中存储有预先计算出的从各 网格至各网格的成本和各网格间的距离。特别是，在本实施方式中，当将各网格作为节点， 并将从当前注视的注视节点延伸出边的节点作为邻接节点，并将从注视节点沿路径向起点 方向追溯了预定数量的节点作为基准节点时，存储有表示从基准节点至邻接节点的最短距 离的第一距离、以及表示从基准节点至注视节点的最短距离的第二距离。由此，可从从基准 节点至邻接节点(位于边的顶端的顶端节点)的最短距离的第一距离和从基准节点至注视 节点(位于边的根源的根源节点)的最短距离的第二距离求出从注视节点延伸至邻接节点 的边的成本。基准节点是从注视节点追溯了预定数量的节点，并基于多个节点间的最短距 离计算边的成本，因此与一律用边的距离来进行成本评估的相比，可进行基于其边的模式 的更详细的成本评估，从而能够生成路径的角度分解度上升而与理想型的最短距离更接近的路径。 路径输出部20包括边生成部21、边成本计算/读出部22、路径成本计算部23、以 及路径确定部24。另外，在本实施方式中对具有存储了成本和距离的距离和成本DB 30并 路径输出部20通过适当参照该数据进行路径探索的进行了说明，但是也可以在路径输出 部20中随时计算成本和距离。另外，距离和成本DB 30也可以通过路径输出部20的边成 本计算/读出部22和路径成本计算部23来构建。 这里，在本实施方式中，每识别新的环境，将该环境识别为用网格表示的探索空 间。另外，机器人装置可着重于将各网格作为节点的探索的问题上，并通过迪克斯特拉 (Dijkstra)算法、AA探索、DP匹配等公知的算法来求出最佳路径。此时，计算节点间的距 离和成本，并注册到距离和成本DB 30。另外，也可以当对于机器人来说成为新的环境时从 外部输入该环境下的节点间的距离和成本。另外，关于边生成、边成本读出/计算、路径成 本计算、路径确定的各处理，以数毫秒至数秒一次的比例实施，按照该定时生成路径。另外， 按照该定时将该障碍物信息等输入为来自图像识别模块和传感器的信息。下面，对各部分 进行详细的说明。 当将从起点至一个节点的成本作为了该一个节点的成本时，边生成部21在位于 边的顶端的顶端节点之中，将节点的成本最小的节点作为注视节点，并至少生成向位于注 视节点的前方、斜右前、以及斜左前的邻接节点延伸的边。这里，前方是节点所在的边所指 的方向。图4和图5是用于说明边及其成本的模式图。 边生成部21从注视的节点向其前方、斜右前、斜左前这三个方向生成边。图4和图 5示出4X4的网格(nl n16)。在该图4中，当注视的节点为n7时，从节点n6朝向节点 n7的方向为前方，由此前方节点为节点n8，斜右前节点为节点n4，斜左前节点为节点n12。 边生成部21生成从节点n7向这些节点n4、n8、n12延伸的边。这里，如后述，当注视节点为 起点时，对上下左右斜方向的所有的八个近旁的邻接节点生成边，而不是三个方向。另外， 在本实施方式中说明了在起点以外的节点仅向前方、斜右前、斜左前这三个方向生成边的 情况，但是也可以在整个节点中对八个近旁的邻接节点生成边，或者例如也可以对除前方、 右斜前、左斜前以外还包括右横和左横节点的五个节点生成边。如果如本实施方式那杆仅 向三个方向生成边，则计算量少，使处理高速化。另外，在本实施方式中，在起点向所有的八 个近旁的邻接节点生成边，但是也可以求出从起点朝向终点的方向，仅对与该方向靠近的 一部分的邻接节点生成边。 这里，边生成部21将后述的各节点的成本最小的节点作为注视节点，从该节点向 三个近旁节点生成边。在本说明书中各节点的成本表示从起点至其节点的成本。重复以下 处理一旦生成了边，则进一步在当前存在的所有边的顶端节点所具有的节点的成本之中， 选择成为最小的节点成本的节点，并同样地生成边。 边成本计算/读出部22包括距离准备部22a和边成本计算部22b，具中所述距离 准备部22a计算或从数据库读出表示从基准节点至邻接节点的最短距离的第一距离以及 表示从基准节点至注视节点的最短距离的第二距离，所述边成本计算部22b将从注视节点 向邻接节点延伸的边计算为成本(所述第一距离-第二距离)。另外，边成本也可以针对各 模式预先计算并存储在距离和成本DB 30中。此时，不需要边成本计算部22b。
接着，说明存储在距离和成本DB 30中的边成本的计算方法。以往，在评估边的成
12本时单纯地仅使用边的长度，与此相反，在本实施方式中从到达至注视的节点的边的形状 评估(计算)边的成本。边的形状的模式取决于追溯至何处评估边。纳入评估的路径长度 越长(使边数量多)，后述的路径的角度分解度越上升。图6A至图6H是追溯两个部分的 边时的边形状的图。此时，边的总数为三个，因此具有23 = 8组的边的模式。另外，与用以 往的方法计算的成本不同的是图6A 图6C以及图6E 图6G，图6D和图6H成为成本与 以往方法相同。如图6A至图6H所示，表示从注视的节点g2延伸的边的顶端节点(邻接节 点)g3的路径模式存在8种。这里，虽然将从该注视的节点g2沿路径向起点方向追溯了预 定数量的节点作为基准节点，但是在本例子中将追溯了两个节点(边)的节点gl作为基准 节点。可从基准节点gl至顶端节点g3的距离和从基准节点gl至注视节点g2的距离求出 注视节点g2 —顶端节点g3的成本。另外，在本实施方式中说明了将边追溯到了两个部分 的情况，但是也可以追述到三个以上，或者也可以仅追溯到一个。 具体地说明边成本的计算。当将图4中示出的节点n7作为将注视节点并将节点 nl2、n8、n4作为从节点n7延伸的边的顶端节点时，从节点n7通过边生成部21生成边E1、 E2、 E2。这里说明求出n7 — n12的边El的成本的方法。 如上述，基准节点成为从节点n7追溯了两个部分的边的节点n5。将从该基准节 点n5至作为边的顶端的节点n12的距离Ll从距离和成本DB30读出。距离Ll表示节点 n5 —nl2的最短距离。将此作为第一距离。另外，在本说明书中将各边间的距离作为1进 行了说明。在本例子中，距离1^1成为{(32+ l2) (10) "3.16。 接着，从距离和成本DB 30读出从基准节点n5至作为边的根源的注视节点n7的 距离L2。距离L2表示节点n5 —n7的最短距离。将此作为第二距离。在本例子中，距离 L2成为"2"。另外，可求出为边E1的成本二第一距离L1-第二距离L2 = 3. 16-2 = 1. 16。
关于其它的边E2、E3，也能够同样地求出。即，边E2的成本成为{从基准节点n5 至边E2的顶端节点n8的距离(第一距离)} - {从基准节点n5至边E2的根源节点n7的距 离(第二距离)} =3-2=1。另外，边E3的成本成为(从基准节点n5至边E3的顶端节点 n4的距离(第一距离)} - {从基准节点n5至边E3的根源节点n7的距离(第二距离)}= 3. 16-2 = 1. 16。 同样，在图5中，从注视节点n 11将边追溯了两个的节点n5成为基准节点，从注 视节点nil向节点n12延伸的边E4的成本成为{从基准节点n5至边E5的顶端节点n16 的距离(第一距离M-(从基准节点n5至边E5的根源节点nll的距离(第二距离)}= 3. 61-2. 24 = 1. 37。并且，边E6的成本成为{从基准节点n5至边E6的顶端节点n8的距 离(第一距离M-(从基准节点n5至边E6的根源节点nll的距离(第二距离)} =3-2. 24 =0. 76。 另外，边E5的成本成为(从基准节点n5至边E5的顶端节点nl6的成本(第一距 离)} _{从基准节点n5至边E5的根源节点nil的距离(第二距离)} = 3. 61-2. 24 = 1. 37。 另外，边E6的成本成为(从基准节点n5至边E6的顶端节点n8的距离(第一距离)}_{从 基准节点n5至边E6的根源节点nil的距离(第二距离)} = 3-2. 24 = 0. 76。
路径成本计算部23计算出从起点至通过边生成部21生成的边的顶端节点的成 本。此时，各边的成本存储在距离和成本DB 30中，因此将各边的成本读出，并合计即可。例 如，在图4示出的例子中，起点至节点n12的成本用(起点至节点n5的成本)+ (n5 —n6的成本)+ (n6 — n7的成本)+ (边El的成本=1.16)表示。这里，节点n5 — n6的成本和节 点n6 — n7的成本根据与对该节点n5、 n6的基准节点而会不同。即，有时不会单纯地成为 如以往的方法那样的这些的成本=距离=1。同样，在图5示出的例子中，起点至节点n12 的成本成为(起点至节点n5的成本)+ (n5 —n6的成本)+ (n6 —nil的成本)+ (nll — n12 的成本=0. 89)。这里，节点n5 — n6的成本和节点n6 — n7的成本根据与对该节点n5、n6 的基准节点而会不同。 当边生成部21生成的边的顶端节点成为了终点时，路径确定部24将从该起点至 终点的成本最小的选择为路径。图7A至图7F是表示节点n7为起点并节点n24为终点的 路径的图。在图7A至图7E中，在以往的成本计算方法下成为成本相同。因此，在路径确定 部24中有时选择如图7A至图7D所示那样偏离了理想型路径的路径。与此相反，作为本实 施方式的路径成本计算部23的路径计算结果，将图7E中示出的计算为成本最小。这里，关 于从节点n7至节点n24的路径，如果忽略节点，则单纯地用直线连接了这些节点的图7F中 示出的路径成为最短的理想型路径。在本实施方式中，不是将构成路径的边的成本作为单 纯的距离，而是追溯到基准节点，通过边的顶端与根源节点的离其基准节点的距离之差进 行评估，从而可选择与图7F中示出的理想型最短路径最接近的图7E中示出的路径。
接着，说明本实施方式的路径探索结果和生成的路径的分解度。当仅用连接节点 间的边表示路径时，可从某节点向八个近旁的节点延伸边。此时，边与边之间的角度最小为 45° 。对此，当如本实施方式那样追溯两个部分的边并用三个边表示时，如图6H、图6B、图 6C、图6D所示，在45。范围内包含四个模式。即，分解度成为约45/4" 11.3° ，与用一个 边表示时相比可成为四倍的分解度。由此，可生成如图7E那样的路径，而不是图7A至T7D。 从而，可生成旋转角度小于45°的机器人的路径，机器人可作平滑的动作而不会作急剧的 旋转。 图8和图9是分别表示以往和本实施方式的机器人的移动路径的图。如图8所示， 以往的机器人装置只能选取以斜右或斜左45。、或90°旋转的路径。对此，如果能够如本 实施方式那样追溯两个部分的边并用三个边表示，则如图9所示，在0至45°之间两个，即 也能够在11.3°附近和22.6°附近旋转，可描绘平滑的曲线路径，可使机器人装置表现出 自然的移动动作。 接着，参照图10和图11中示出的流程图以及图12至图15中示出的路径图来具
体地说明本实施方式的路径探索方法。图io是表示实施方式的路径确定方法的流程图，图 11是表示边的成本计算方法的流程图。图12A至图121是表示本实施方式的路径探索方法 的图，图13A至图13K是表示以往的路径探索方法的图，图14是表示通过本实施方式的路 径探索方法生成的路径的图，图15A至图17J是存在障碍物时的本实施方式的路径探索方 法。 如图10所示，首先，边生成部21 —旦确认注视的节点是起点的情况，则向左右上 下斜的邻接八个近旁节点的全部延伸边(步骤S1)。另外，在该阶段，以离起点的距离=节 点的成本而通过与以往相同的方法来计算成本(步骤S2)。 具体地说，如图12A所示，边生成部21对起点节点n7的邻接八个近旁节点的全部 生成边。另外，在本例子中，由于左侧为网格区域的边界，因此不能向左横、左上、左下延伸 边。由此，生成从起点节点n7向节点nl、 n2、 n8、 n14、 nl3延伸的边。然后，边成本计算/
14读出部22以从作为注视节点的起点n7向以下节点nl、n2、n8、nl4、n13延伸的边的成本= 距离而求出成本。这里，从起点n7延伸出来的边的成本成为各节点nl、n2、n8、nl4、n13的 成本。即，边成本计算/读出部22读出离起点节点n7的距离。 接着，边生成部21在各边的顶端节点之中选择成本最小的节点(步骤S3)。在图 12B中，由于从起点至nl、n2、n8、nl4、n13的成本为其距离，因此分别成为1、 1. 41、 1、 1. 41、 1。在其中，边生成部21选择最小成本的节点n8、n13。另外，nl是网格的最外周，成本只能 计算到此，因此以外的是属于计算对象外。 接着，边生成部21从n8、nl3将进入n8、n13的边的方向作为前方面向前方、右斜 前、左斜前的三方向延伸边(步骤S4)。接着，边成本计算/读出部22在求出边的成本之前 判断是否为从起点至该边的顶端节点的节点数量N > 3。在本实施方式中，由于从注视的节 点追溯两个边后进行成本的评估，因此与以往方法相同的成本计算方法使用至从起点至顶 端节点的数量N二3以上为止。 如图12B所示，由于至从n8、nl3延伸的边的顶端节点的节点数量是2，因此返回 步骤S2，与上述同样，能够以该边的成本二边的距离而求出。于是，各边的顶端节点的成本 成为离各自起点的距离。例如边从n8分别延伸至右斜前节点n3、前方节点n9、左斜前节点 nl5，各顶端节点的成本分别求出为2. 41、2、2. 41 (步骤S3)。 接着，节点成本最小的节点是n2、nl4，并且是1.41(步骤S3)。因此，将边从该节 点n2、nl4延伸到右斜前节点、前方节点、左斜前节点(步骤S4)。这里，不存在从n2的右 斜前方节点，左斜前节点为n3。此时，n3具有从n8延伸过来的边。在此情况下，采用节点 的成本低侧的路径。对于节点的成本相同的路径，采用先延伸的路径。在本例子中，来自节 点n2的路径下的节点n3的成本为2. 41，并来自节点n8的路径下的节点n3的成本为2，因 此来自节点n2的路径不被采用。同样，虽然边从节点n14向节点nl5、n21、n20延伸，但是 由于向节点n15、 n20延伸的边是分别采用来自节点n18的路径和来自节点n13的路径，因 此来自节点nl4的路径均不被采用。将从步骤S2起的处理重复至边的至延伸了的顶端节 点的数量成为3。 接着，边成本计算/读出部22如果选择顶端节点的成本最小的各节点，则在图12B 的状态下是选择节点n9、n19。这里，如图12C所示那样，将边从节点n9、nl9延伸到前方节 点、左斜前节点、右斜前节点。虽然接着计算边的成本，但是首先判断在边的延伸处的顶端 节点n4、 n10、 n16、 n25、 n26，从起点节点n7起的边数量是否为3以上。这里，边数量成为 3，进入步骤S6。 在步骤S6中，从当前的注视节点n9、 n19追溯两个边而分析其边模式。当从节点 n9追溯两个边时成为节点n7(起点节点)，其成为基准节点。边模式是从基准节点n7至顶 端节点n4、nl0、n16的模式。基于该边模式，计算顶端节点n4、nl0、n16的成本(步骤S7)。
这里，可通过路径成本计算部23根据从起点至基准节点的成本+基于从基准节点 至顶端节点的边模式的成本求出顶端节点n4、nl0、n16的成本。关于从起点至基准节点的 成本，将构成从起点至基准节点的路径的边的各成本通过边成本计算/读出部22从距离和 成本DB 30读出，并合计即可。在本例子中，由于是基准节点=起点，因此通过从基准节点 至顶端节点的边模式来求出其顶端节点的成本，并顶端节点n4、 n10、 n16的成本分别求出 为3. 16、3、3. 16。同样，顶端节点n25、 n26的成本分别求出为3、3. 16。将这些处理重复至顶端节点成为终点节点n24(步骤S8)，经图12D、图12E、图12F、图12G而成为图12H的状 态。此时，由于从节点n17延伸的边到达终点节点n24，因此成为顶端节点=终点节点n24。 在该时刻选择并输出最小成本的路径(步骤S9)。该路径成为了与图7F中示出的路径最接 近的路径。 接着，参照图11来说明计算从起点至边的顶端节点的成本(顶端节点的成本)时 的方法。如上述那样，注视节点n9的基准节点是作为起点的节点n7。首先，如图ll所示， 计算从基准节点n7至边的顶端节点n4的距离Ll ( = 3. 16)(步骤Sll)。接着，计算从基 准节点n7至边的根源节点n9的距离L2 ( = 2)(步骤S12)。边的成本可求出为Ll-L2 = 1. 16(步骤S13)。此时，可将距离L1和L2、边的成本注册到距离和成本DB 30中，从而构建 数据库。另外，边成本计算/读出部22可以如此计算边的成本，也可以将距离Ll、 L2从距 离和成本DB 30读出，并计算L1-L2。当构建数据库时，对全部节点进行距离和成本的计算 (步骤S15、S16)。 同样，n9 — n10的边成本成为{从基准节点n7至边的顶端节点n10的距离Ll (= 3)}_{从基准节点n7至边的根源节点n9的距离L2 ( = 2)}，可求出为1。另外，n9 — n16 的边成本成为{从基准节点n7至边的顶端节点n16的距离Ll ( = 3. 16)} - {从基准节点n7 至边的根源节点n9的距离L2( = 2)}，可求出为1. 16。由此，各节点n4、nl0、n16的成本分 别成为3. 16、3、3. 16。同样，节点n26的成本成为3. 16， n25的成本成为3。
接着，说明图12E中示出的节点n27的成本。是以下例子虽然在节点n27生成具 有从节点n20和节点n21延伸过来的边的路径，但是先生成的来自节点n20的路径的成本 比后生成的来自节点n21的路径大，而采用后生成的来自节点n21的路径。
首先，说明将边从n20向n27延伸而求出n27的成本的情况。节点n20 — n27的 边的成本通过(从基准节点n7至节点n27的距离)_(从基准节点n7至节点n20的距离) 来求出，成为3. 16-2. 24 = 1. 37。此时，n27的成本成为(n7 — n13的成本)+ (n13 — n20 的成本)+ (n20 — n27的成本)，成为1+1.41+1.37 = 3. 78。 之后，生成将边从节点n21向n27延伸的路径。此时，n21 — n27的边的成本通过 (从基准节点n7至n27的距离)-(从基准节点n7至n21的距离)来求出，成为3. 16-2. 82 =0.79。此时，n27的成本成为(n7 — n14的成本)+ (nl4 — n21的成本)+ (n21 — n27的 成本)，成为1.41+1.41+0.79 = 3.61。如此，虽然对于节点n27来说由从节点n20延伸的 边形成的路径先生成，但是对于节点n27的成本来说经过节点n21的路径便宜，因此在这里 将来自节点n20的路径采用为节点n27的路径，而不采用来自节点n21的路径。
对此，如图13A至图13K所示，如果通过以往方法来进行路径探索，则会提取如图 13K所示那样的成本。其理由是因为该路径在以往方法下成为与用虚线表示的理想路相 同的成本。在本实施方式中，由于基于从基准节点至边的顶端和根源节点的距离计算其边 的节点，因此可生成与理想型路径最接近的路径。 图14是表示从起点ST通过本实施方式的路径探索方法生成的路径的图。当不特 别地设定终点时，如图14所示那样继续生成路径。 图15A至图15J示出存在障碍物时的路径探索结果。如图15A至图15J所示，当 在包含节点n8、n9、nl4、nl5、n20、n21的区域中存在障碍物43时，虽然生成从障碍物43的 上方朝向终点方向生成的路径(n7 — n13 — n19 — n26 — n27 — n22)和从障碍物43的下方朝向终点方向生成的路径(n7 — n2 — n3 — n10 — nil — n18)，但是从障碍物43的下方 朝向终点方向生成的路径先到达终点n18。 在本实施方式中，从从基准节点至邻接节点(顶端节点)的第一距离和从基准节 点至注视节点(根源节点)的第二距离求出从注视节点向邻接节点延伸的边的成本，而不 是单纯地作为其距离。由于基准节点是从注视节点追溯了一个以上的节点，并基于多个节 点间的最短距离计算边的成本，因此与一律用边的距离来进行成本评估的相比，可进行基 于其边的模式的更详细的成本评估，从而能够生成路径的角度分解度上升而与理想型的最 短距离更接近的路径。另外，由于可通过基于其边的模式的更详细的成本评估来生成路径 的角度分解度上升而与理想型的最短距离更接近的路径，因此可使机器人表现出更自然的 移动动作。 另外，本发明不限于上述的实施方式，理所当然地可在不脱离本发明的宗旨的范 围内进行种种变更。例如，在上述的实施方式中说明了硬件的构成，但是不限于此，可将任 意的处理通过使CPU (Central ProcessingUnit)执行计算机程序来实现。此时，关于计算 机程序，可以通过记录在记录介质中来提供，或者也可以通过经由网络等其它传送介质传 送来提供。 产业上的可利用性 本发明可广泛地利用于在被包含于用网格表示的区域的从起点至终点的路径之 中选择其成本最小的路径的路径计划装置和路径计划方法、用于评估该路径计划装置下的 成本的成本评估装置、以及安装了该路径计划装置的移动体。
1权利要求
一种路径探索装置，在被包含于用网格表示的区域中的从起点至终点的路径之中选择成本最小的路径，所述路径探索装置的特征在于，包括路径成本计算部，如下计算所述起点至所述终点的路径成本将各网格作为节点，将从当前注视的注视节点向与其邻接的邻接节点延伸的边的成本作为通过将从该注视节点沿路径向所述起点方向追溯了预定数量节点的节点作为基准节点并由(表示从该基准节点至所述邻接节点的最短距离的第一距离)-(表示从所述基准节点至所述注视节点的最短距离的第二距离)而求出的值；以及路径确定部，根据所述路径成本来确定路径。
2. 如权利要求l所述的路径探索装置，其特征在于，所述路径成本计算部将从所述起点至所述终点的路径成本计算为构成该路径的边的 成本之和。
3. 如权利要求l所述的路径探索装置，其特征在于，所述路径成本计算部参照存储了所述第一距离和所述第二距离或者使用(第一距 离_第二距离)求出的成本的数据库来计算从所述起点至所述终点的成本。
4. 如权利要求3所述的路径探索装置，其特征在于，还包括数据库生成部，该数据库生成部通过计算(所述第一距离-所述第二距离)来 构建所述数据库。
5. 如权利要求1或2所述的路径探索装置，其特征在于， 包括边生成部，当将从所述起点至任意节点的成本作为该任意节点的成本时，在位于边的 顶端的顶端节点之中，将节点的成本最小的节点作为注视节点，并生成向位于注视节点的 至少前方、斜右前方、以及斜左前方的邻接节点延伸的边；以及边成本计算/读出部，计算出或从数据库读出所述第一距离和所述第二距离； 所述路径成本计算部将从所述起点至所述邻接节点的路径成本计算为构成该路径的 边的成本之和，当所述邻接节点为所述终点时，所述路径确定部选择从所述起点至所述终点的成本最 小的路径。
6. 如权利要求5所述的路径探索装置，其特征在于，当所述注视节点为所述起点时，所述边生成部将边延伸到该起点的八个近旁的邻接节点。
7. 如权利要求5或6所述的路径探索装置，其特征在于，所述边成本计算/读出部将从所述注视节点延伸至所述邻接节点的边的成本作为所 述注视节点与所述邻接节点的距离，直至从所述起点至所述邻接节点的节点数量成为所述预定数量。
8. 如权利要求5至7中任一项所述的路径探索装置，其特征在于， 所述路径成本计算部将所述邻接节点的成本计算为构成从所述起点至所述邻接节点的路径的边的距离之和，直至从所述起点至所述邻接节点的节点数量成为所述预定数量。
9. 一种成本评估装置，使用在路径探索中，该路径探索在被包含于用网格表示的区 域中的从起点至终点的路径之中选择成本最小的路径，所述成本评估装置的特征在于，包括距离准备部，当将各网格作为节点、将从当前注视的注视节点延伸边的节点作为邻接 节点、以及将从所述注视节点沿路径向所述起点方向追溯了预定数量节点的节点作为基准 节点时，计算出或从数据库读出表示从该基准节点至所述邻接节点的最短距离的第一距离 和表示从所述基准节点至所述注视节点的最短距离的第二距离；以及边成本计算部，将从所述注视节点向所述邻接节点延伸的边的成本计算为(所述第一 距离_第二距离)。
10. —种路径探索方法，在被包含于用网格表示的区域中的从起点至终点的路径之中 选择成本最小的路径，所述路径探索方法的特征在于，包括路径成本计算工序，如下计算所述起点至所述终点的路径成本将各网格作为节点，将 从当前注视的注视节点向与其邻接的邻接节点延伸的边的成本作为通过将从该注视节点 沿路径向所述起点方向追溯了预定数量节点的节点作为基准节点并由(表示从该基准节 点至所述邻接节点的最短距离的第一距离)-(表示从所述基准节点至所述注视节点的最 短距离的第二距离)而求出的值；以及路径确定工序，根据所述路径成本来确定路径。
11. 如权利要求io所述的路径探索方法，其特征在于，在所述路径成本计算工序中，将从所述起点至所述终点的路径成本计算为构成该路径 的边的成本之和。
12. 如权利要求10所述的路径探索方法，其特征在于，在所述路径成本计算工序中，参照存储了所述第一距离和所述第二距离或者使用(第 一距离_第二距离)求出的成本的数据库来计算从所述起点至所述终点的成本。
13. 如权利要求12所述的路径探索方法，其特征在于，还包括数据库生成工序，该数据库生成工序通过计算所述第一距离和所述第二距离或 者(所述第一距离_所述第二距离)来构建所述数据库。
14. 如权利要求10至13中任一项所述的路径探索方法，其特征在于， 在所述路径成本计算工序中，将从所述起点起的成本作为离该起点的距离，直至从所述起点起的节点数量成为所述预定数量。
15. —种路径探索方法，在被包含于用网格表示的区域中的从起点至终点的路径之中 选择成本最小的路径，所述路径探索方法的特征在于，包括第一工序，将各网格作为节点，将边延伸到所述起点的八个近旁的邻接节点，将从该起 点至所述邻接节点的距离计算为该邻接节点的成本；第二工序，当将从所述起点至一个节点的成本作为该一个节点的成本时，在位于边的 顶端的顶端节点之中，将节点的成本最小的节点作为注视节点，将边延伸至位于该注视节 点的至少前方、斜右前方、以及斜左前方的邻接节点，计算该邻接节点的成本；以及第三工序，当所述邻接节点为所述终点时，选择从所述起点至所述终点的成本最小的 路径；重复所述第二工序直至所述邻接节点成为所述终点， 所述第二工序包括距离准备工序，当将从所述注视节点沿路径向起点方向追溯了预定数量节点的节点作为基准节点时，计算出或从数据库读出表示从该基准节点至所述邻接节点的最短距离的第 一距离和表示从所述基准节点至所述注视节点的最短距离的第二距离；边成本计算工序，将从所述注视节点向所述邻接节点延伸的边的成本计算为(所述第 一距离-第二距离)；以及路径成本计算工序，将从所述起点至所述邻接节点的路径成本计算为构成该路径的边 的成本之和。
16. —种移动体，包括 移动单元；驱动单元，驱动所述移动单元；路径探索单元，将周围的环境用网格表示，并在被包含于该网格空间中的从起点至终 点的路径之中选择成本最小的路径；以及控制单元，基于所述路径探索单元的路径探索结果，对所述驱动单元进行控制； 所述路径探索单元包括路径成本计算单元，如下计算所述起点至所述终点的路径成本将各网格作为节点，将 从当前注视的注视节点向与其邻接的邻接节点延伸的边的成本作为通过将从该注视节点 沿路径向所述起点方向追溯了预定数量节点的节点作为基准节点并由(表示从该基准节 点至所述邻接节点的最短距离的第一距离)-(表示从所述基准节点至所述注视节点的最 短距离的第二距离)而求出的值；以及路径确定单元，根据所述路径成本来确定路径。
全文摘要
在用网格表示的探索空间中生成最佳的路径。路径探索装置将各网格作为节点，并包括边生成部(21)，当将从起点至任意的节点的成本作为该任意的节点的成本时，在位于边的顶端的顶端节点之中，将节点的成本最小的节点作为注视节点，并生成向位于注视节点的前方、斜右前、以及斜左前的邻接节点延伸的边；边成本计算/读出部(22)，当将从注视节点沿路径向起点方向追溯了预定数量的节点作为基准节点时，从数据库读出从基准节点至邻接节点的最短距离(第一距离)和从基准节点至注视节点的最短距离(第二距离)；路径成本计算部(23)，将从起点至所述终点的路径成本计算为构成路径的边的成本之和；以及路径确定部(24)，选择从起点至终点的成本最小的路径。
文档编号G01C21/34GK101755247SQ200880025179
公开日2010年6月23日 申请日期2008年7月4日 优先权日2007年7月18日
发明者朝原佳昭, 美马一博, 薮下英典 申请人:丰田自动车株式会社