CN101770235A - 路径计划装置、路径计划方法和计算机程序 - Google Patents

Abstract

本发明公开了路径计划装置、路径计划方法和计算机程序。路径计划装置包括：初始位置和姿态路径获得单元，其获得将控制对象的当前位置和姿态连接到目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在位置和姿态路径中的各个节点中构造与控制对象对应的刚体；干涉检测器，其检测障碍物与控制对象的干涉；外力计算器，其计算对于各个刚体的外力；以及多刚体***动力学模拟单元，其对多个刚体运动方程式执行多刚体***动力学模拟，并且其获得与位置和姿态路径中的节点对应的时间时的控制对象的位置和姿态。

Description

路径计划装置、路径计划方法和计算机程序

技术领域

本发明涉及路径计划装置、路径计划方法和计算机程序，其能够对机器人的运动控制、或诸如多关节臂的指尖之类的控制对象在运动空间内从当前位置和姿态到目标位置和姿态的运动路线或路径进行计划。更具体而言，本发明涉及路径计划装置、路径计划方法和计算机程序，其能够对为了避开障碍物来将控制对象的路径校正为平滑路径，以在运动控制或路径控制中容易地使用。

背景技术

例如，当诸如机器人或作为多关节臂的端部效应器的指尖之类的移动机械在运动中自治控制时，应该对在运动空间内从某个位置和姿态(当前位置和姿态：起始)到另一位置和姿态(目标位置和姿态：目标)的运动路线或路径进行计划。在以下说明中，“路线”和“路径”均被称为“路径”。除非另外具体说明，否则路径表示位置和姿态的路径。即，在三维空间内的路径表示用于描述物体的空间姿态的六自由度路径。

本发明主要针对迅速地产生机器人或多关节臂从当前位置和姿态到目标位置和姿态的平滑路径。发明人考虑到，为实现以上目的，应该满足以下四个要求：

(1)迅速地获得并非最优方案的、为实现目标位置和姿态路径的初始位置和姿态路径；

(2)获得平滑的位置和姿态路径；

(3)在位置和姿态路径中避免物体之间的渗透(碰撞或干涉)；以及

(4)尽可能接近理想的位置和姿态路径(反映设计者的意图)。

从无穷多路径中获得最优方案是困难的。已经提出了各种路径计划方法，例如势能法(potential method)(例如，见由ZYUNN OOTA等写的“Introduction to Intelligent Robot-The Solution of the Motion PlanningProblem”(CORNA出版有限公司，2001))、随机道路地图法(stochastic road map method)(例如，见由ZYUNN OOTA等撰写的“Introduction to Intelligent Robot-The Solution of the Motion PlanningProblem”(CORNA出版有限公司，2001))、以及迅速搜索随机树(RRT)法(例如，见由Steven M.Lavalle等人撰写的“Rapidly-ExploringRandom Trees：A New Tool for Path Planning”(TR.98-11，计算机系，爱荷华州立大学(Iowa state University)，1998年10月))。

在势能法中，通过重复地进行如下过程，来对路径进行计划：界定在运动空间内将控制对象引到目标姿态的“引力势能”和将控制对象与障碍物推斥的“推力势能”的势能场，计算控制对象在当前姿态下的势能场的梯度，并沿着势能减小的方向细微地移动控制对象。

例如，已经提出了一种路径产生装置，其利用根据移动物体、障碍物和对象物体的位置和形状的信息产生的运动空间内的地图，基于移动物体、障碍物和对象物体之间的相对位置关系来计算引力势能和推力势能，产生作为势能之和的合成势能，基于合成势能搜索地图中的路线以判定路线搜索的收敛位置是否是局部最小值，当收敛位置是局部最小值时产生通过将收敛位置的势能增大预定值获得的虚拟势能，并当收敛位置是目标位置时基于路线搜索的结果产生移动物体的运动路径(例如，见日本未经审查的专利申请公开No.2006-350776)。

在随机道路地图法中，从构造空间随机选择一个姿态，当能够移动到附近姿态时，该姿态被分支，当道路地图未被连接时在具有许多障碍物的区域中从特定姿态采取随机行走，将道路地图延展，并应用作为数学程序的迪杰斯特拉法(Dijkstra method)以计算道路地图中的最短距离。

例如，已经提出了一种最低成本路径搜索装置，其利用迪杰斯特拉法在网络中搜索从入口节点到出口节点的具有最低成本的路径(例如，见日本未经审查的专利申请公开No.2002-133351)。

RRT是适当地包围运动空间的树数据结构和用作产生树的基础的运算法则。在RRT运算法则中，初始位置首先设定在运动空间中，在相同运动空间中构造随机采样位置，搜索在初始位置和采样位置之间的最近位置，并将初始位置和最近位置连接到树并添加到树。此后，通过重复进行在所添加的最近位置配置采样位置、附加搜索最近位置以及将搜索到的最近位置添加到树，可以产生包围运动空间的树。RRT运算法则广泛地应用于实时路径计划或路径产生(例如，见由James J.Kuffner撰写的“Dynamically-stable Motion Planning for Humanoid Robots”(AutonomousRobots第12卷第1期，105-118页，2002))，并得到了广泛的研究。

在势能法中，难以有效计算稳定的运动路径，这是由于存在如下可能性：除了目标位置和姿态以外，还可能存在具有最小势能的多个位置和姿态，这是非常直观的。随机道路地图法具有如下缺点：难以在以多个障碍物围绕的狭窄空间(即，应该采样多个位置和姿态)内计划运动。RRT运算法则能够在多维构造空间内相对迅速地进行路径计划，但是具有如下缺点：由于随机搜索的使用，搜索到的路径包括许多绕路。

因此，过去的路径计划方法具有优点和缺点。需要对通过一种方法产生的路径进行校正以使路径平滑或避免障碍物。作为使路径平滑的方法，已经提出了利用最小跃度(jerk)(例如，见由T.Flash和N.Hogan撰写的“The coordination of Arm Movements：An Experimentally ConfirmedMathematical Model”(The Journal of Neuroscience，第5卷第7期，1688-1703页，1985))、最小转矩、多项式(仿样)的方法。

但是，没有方法能满足获得平滑的位置和姿态路径的(2)，避免物体之间的渗透的(3)和接近理想的位置和姿态路径的(4)的要求。换言之，不能获得在机器人的运动控制或用于多关节臂的指尖的路径计划的最终控制中所使用的令人满意的路径。

例如，已经提出了一种利用B-仿样使类人的运动计划路径平滑的方法(例如，见由HARADA、MORITAKU、MIURA、HUZIWARA和KAZITA撰写的“Smoothing of Humanoid Motion Planning Path using B-spline”(Proceedings of 2008 JSME Conference on Robotics andMechatronics，2P1-F16，2008))。在此方法中，可以利用空间中的位置和三自由度姿态来执行对路径的校正，但是存在计算量过大的问题。

发明内容

期望提供一种路径计划装置、路径计划方法和计算机程序，其能够对机器人的运动控制、或诸如多关节臂的指尖之类的控制对象在运动空间内从当前位置和姿态到目标位置和姿态的运动路线或路径进行计划。

还期望提供一种路径计划装置、路径计划方法和计算机程序，其能够搜索控制对象的路径以避开障碍物，并将路径校正为平滑路径以更方便地用于控制对象的运动控制或路径控制。

还期望提供一种路径计划装置、路径计划方法和计算机程序，其能够迅速地产生控制对象从当前位置和姿态到目标位置和姿态的平滑路径。

根据本发明的实施例，提供了一种路径计划装置，其对控制对象在运动空间内从当前位置和姿态到目标位置和姿态的位置和姿态路径进行计划，并包括：初始位置和姿态路径获得单元，其获得将所述控制对象的所述当前位置和姿态连接到所述目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在所述位置和姿态路径中的各个节点中构造与所述控制对象对应的刚体；干涉检测器，其检测存在于所述运动空间内的障碍物与所述位置和姿态路径中的所述控制对象的干涉；外力计算器，其计算对于各个刚体的外力，所述外力表达了用于使所述位置和姿态路径平滑和避免与所述障碍物干涉的要求；以及多刚体***动力学模拟单元，其对多个刚体运动方程式执行多刚体***动力学模拟，并且其基于当所述路径收敛到预定状态时的刚体的位置和姿态获得所述控制对象与所述位置和姿态路径中的节点对应的时间时的位置和姿态，在所述多个刚体运动方程式中，对所述多个刚体的耦合运动进行描述，并且由所述外力计算器计算得到的所述外力分别作用于所述多个刚体。

在该路径计划装置中，所述初始位置和姿态路径获得单元可以基于在所述多个节点被分支的多个路线并在所述运动空间内展开的搜索树来获得所述控制对象的所述当前位置和所述目标位置之间的路径，并对位于各个节点处的所述刚体赋予初始姿态。

在该路径计划装置中，所述干涉检测器可以应用GJK运算法则(Gilbert-Johnson-Keerthi距离运算法则)以检测各个刚体和所述障碍物之间的最短距离。

在该路径计划装置中，所述外力计算器可以利用如下动力学模型：界定被固定在各个刚体的局部坐标系中的虚拟刚体面，通过具有预定弹簧常数的弹簧和具有预定阻尼常数的阻尼器将相邻的所述虚拟刚体面的对应顶点彼此并列连接，以计算刚体间力和转矩作为表达了用于使所述位置和姿态路径平滑的要求的所述外力，所述刚体间力是作用在一个虚拟刚体面的顶点和相邻的虚拟刚体面的顶点之间的弹簧力和阻尼力的和值，所述转矩作用在所述刚体上，并围绕全局坐标系的原点，并包括由于所述弹簧力和所述阻尼力而作用在各个虚拟刚体的顶点上的力矩的和值。

在该路径计划装置中，所述干涉检测器可以检测位于所述位置和姿态路径中的各个节点处的所述刚体和所述障碍物之间的最短距离，并且所述外力计算机可以计算取决于所述最短距离作用在各个刚体上的推斥力，作为表达了用于使所述位置和姿态路径避免与所述障碍物干涉的要求的所述外力。

在该路径计划装置中，所述外力计算器可以计算与由固定在所述刚体的所述局部坐标系中的每个刚体的前移方向和相邻的所述刚体之间的所述位置和姿态路径的方向形成的角对应的力矩，作为表达了用于实现理想的位置和姿态路径的要求的外力。

根据本发明的另一实施例，提供了一种路径计划方法，其对控制对象在运动空间内从当前位置和姿态到目标位置和姿态的位置和姿态路径进行计划，并包括以下步骤：获得将所述控制对象的所述当前位置和姿态连接到所述目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在所述位置和姿态路径中的各个节点中构造与所述控制对象对应的刚体；检测存在于所述运动空间内的障碍物与所述位置和姿态路径中的所述控制对象的干涉；计算对于各个刚体的外力，所述外力表达了用于使所述位置和姿态路径平滑和避免与所述障碍物干涉的要求；对多个刚体运动方程式执行多刚体***动力学模拟，并且基于当所述路径收敛到预定状态时的刚体的位置和姿态获得所述控制对象与所述位置和姿态路径中的节点对应的时间时的位置和姿态，在所述多个刚体运动方程式中，对所述多个刚体的耦合运动进行描述，并且由所述外力计算器计算得到的所述外力分别作用于所述多个刚体。

根据本发明的另一实施例，提供了一种计算机程序，其以计算机可读的方式表现以使得计算机执行对控制对象在运动空间内从当前位置和姿态到目标位置和姿态的位置和姿态路径进行计划的处理，所述计算机程序使得所述计算机用作：初始位置和姿态路径获得单元，其获得将所述控制对象的所述当前位置和姿态连接到所述目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在所述位置和姿态路径中的各个节点中构造与所述控制对象对应的刚体；干涉检测器，其检测存在于所述运动空间内的障碍物与所述位置和姿态路径中的所述控制对象的干涉；外力计算器，其计算对于各个刚体的外力，所述外力表达了用于使所述位置和姿态路径平滑和避免与所述障碍物干涉的要求；以及多刚体***动力学模拟单元，其对多个刚体运动方程式执行多刚体***动力学模拟，并且其基于当所述路径收敛到预定状态时的刚体的位置和姿态获得所述控制对象与所述位置和姿态路径中的节点对应的时间时的位置和姿态，在所述多个刚体运动方程式中，对所述多个刚体的耦合运动进行描述，并且由所述外力计算器计算得到的所述外力分别作用于所述多个刚体。

上述计算机程序用于界定以计算机可读方式表现以在计算机中执行预定处理的计算机程序。换言之，通过将上述计算机程序安装在计算机中，可以由计算机执行协同操作，并且能够获得与上述根据本发明实施例的路径计划装置相同的操作优点。

根据本发明的实施例，可以提供一种路径计划装置、路径计划方法和计算机程序，其能够对机器人的运动控制、或诸如多关节臂的指尖之类的控制对象在运动空间内从当前位置和姿态到目标位置和姿态的运动路线或路径进行计划。

根据本发明的实施例，可以提供一种路径计划装置、路径计划方法和计算机程序，其能够搜索控制对象的路径以避开障碍物，并将路径校正为平滑路径以更方便地用于控制对象的运动控制或路径控制。

根据本发明的实施例，可以提供一种路径计划装置、路径计划方法和计算机程序，其能够迅速地产生控制对象从当前位置和姿态到目标位置和姿态的平滑路径。

根据本发明的实施例，通过将控制对象的路径中的节点处理作为刚体模型，除了可以处理控制对象的位置路径之外，还可以处理姿态路径。

根据本发明的实施例，可以通过引入多刚体***动力学模拟，来校正由初始位置和姿态路径获得单元获得的初始位置和姿态路径。

根据本发明的实施例，可以将通过表达用于使控制对象的位置和姿态路径平滑的要求来以力的维度校正位置和姿态路径的处理和避开与障碍物干涉的处理的执行整合为动力学模型，即动力学限制。

根据本发明的实施例，可以通过对多个刚体运动方程式施加多刚体***动力学模拟来获得刚体的位置和姿态，所述多个刚体运动方程式表达了包括受到动力学限制的多个刚体的耦合运动。位置和姿态对应于控制对象在与位置和姿态路径中的节点对应的时间时的位置和姿态。

根据本发明的实施例，可以实时避开障碍物，并实时校正目标位置和姿态。

根据本发明的实施例，初始位置和姿态路径获得单元可以在无需考虑路径的平滑化的情况下通过应用RRT运算法则来迅速并令人满意地搜索用于到达不一定被限制为最优方案的最低目的的路径。在RRT运算法则中，获得路径中的节点的位置，接着适当地赋予节点的初始姿态。节点中的姿态对应于刚体的局部坐标系，并且初始姿态可以设定在全局坐标系中。

根据本发明的实施例，通过利用用于计算凸形图形之间的距离的迭代方法(例如GJK运算法则)，可以检测与路径中的节点对应的各个刚体和障碍物的最近点对或最大渗透点对之间的距离。

根据本发明的实施例，可以通过利用将相邻的刚体彼此吸引的力(或推斥力)(即，刚体间力)的动力学限制表达，来满足“获得平滑的位置和姿态路径”的要求。这里，当“获得平滑的位置和姿态路径”的要求由刚体间力表达时，通过将刚体处理作为不具有体积的虚拟面，几乎不会引起相邻刚体重叠的问题。

根据本发明的实施例，可以通过利用使与控制对象和障碍物之间的最短距离对应的推斥力作用在刚体上的动力学限制表达，来满足“在位置和姿态路径中避免物体之间的渗透”的要求。

根据本发明的实施例，可以通过利用将控制对象的理想的姿态路径界定为“使得位于路径中的节点处的刚体的前移方向尽可能沿着路径”并且使与由刚体的前移方向和相邻刚体之间的位置和姿态路径的方向形成的角对应的力矩作用在刚体上的动力学限制表达，来满足“尽可能接近理想的位置和姿态路径”的要求。

基于本发明的实施例或附图，根据以下详细说明，本发明的其他目的、特征和优点将变得清楚。

附图说明

图1A是图示在RRT中基本用于产生树的运算法则的图。

图1B是图示在RRT中基本用于产生树的运算法则的图。

图1C是图示在RRT中基本用于产生树的运算法则的图。

图1D是图示在RRT中基本用于产生树的运算法则的图。

图1E是图示在RRT中基本用于产生树的运算法则的图。

图1F是图示在RRT中基本用于产生树的运算法则的图。

图1G是图示在RRT中基本用于产生树的运算法则的图。

图2A是图示利用RRT运算法则产生路径的示例的图(其中示出了控制对象)。

图2B是图示利用RRT运算法则产生路径的示例的图(其中未示出控制对象)。

图3A是图示利用RRT运算法则产生路径的示例的图(其中示出了控制对象)。

图3B是图示利用RRT运算法则产生路径的示例的图(其中未示出控制对象)。

图4是图示多刚体***在初始状态下的示例。

图5是图示为刚体界定的虚拟刚体面的图。

图6是图示作用在相邻的虚拟刚体面之间的力的图。

图7是图示用于实现控制对象的理想姿态路径的动力学模型的图。

图8是示意性地图示实施控制对象的路径计划的***的功能构造的图。

图9是图示实现如图8所示的***中的控制对象的路径计划的处理顺序的流程图。

图10是图示利用多刚体***动力学模拟由如图3所示的RRT运算法则获得的初始路径的校正结果的图。

图11是图示利用多刚体动力学模拟校正的路径的另一个示例的图。

图12是图示如图11所示的路径中刚体的位置的图。

图13是图示利用多刚体动力学模拟校正的路径的另一个示例的图。

图14是图示在如图13所示的路径中刚体的姿态的俯仰角的图。

图15是图示在弹簧常数是恒量的情况下另一个刚体***动力学模型的路径产生结果的快照的图。

图16是图示在弹簧常数是根据刚体i和刚体O(当前状态)或刚体N(目标状态)之间的距离而变化的变量的情况下的路径产生结果的快照的图。

图17是图示当弹簧常数是恒量和变量时产生的路径中的刚体(节点)的刚体姿态的图。

图18是图示在控制对象从如图3所示的初始状态向图中更深的障碍物移动的情况下多刚体***动力学模拟的结果。

图19是图示执行多刚体***动力学模拟并静态确定刚体组的状态的图。

图20是图示执行多刚体***动力学模拟并静态确定刚体组的状态的图。

图21是图示在将校正值给予目标位置和姿态并且在执行模拟之后模拟收敛的状态下的快照的图。

图22是图示在将校正值给予目标位置和姿态并且在执行模拟之后模拟收敛的状态下的快照的图。

图23是图示能够实施本发明的实施例的计算机的构造的图。

具体实施方式

此后，将参照附图详细说明本发明的实施例。

为了迅速地产生机器人或多关节臂从当前位置和姿态到目标位置和姿态的平滑路径，应该满足以下要求。在以下说明中，“控制对象”包括机器人或多关节臂。

(1)迅速地获得并非最优方案的、为实现目标位置和姿态路径的初始位置和姿态路径；

(2)获得平滑的位置和姿态路径；

(3)在位置和姿态路径中避免物体之间的渗透(碰撞或干涉)；以及

(4)尽可能接近理想的位置和姿态路径(反映设计者的意图)。

为了满足(1)的要求，如何在不考虑路径的平滑化的情况下迅速并令人满意地搜索路径以实现不一定限制为最优方案的最低目的是选择的标准。因此，在本发明的实施例中，前述RRT运算法则用于迅速地获得初始位置和姿态路径。RRT运算法则具有的缺点在于，其在多维度问题中缺乏直观性，但是RRT运算法则在低维度问题中不缺少直观性。RRT运算法则的一个特征在于能够相对迅速地在多维度构造空间内计划路径。

这里，将参照图1A至1G说明用作产生RRT运算法则中的树的基础的序列。

首先，在运动空间内设定初始位置q_init(见图1A)。

然后，在运动空间内随机地构造q_rand(见图1B)。

然后，在树(搜索树)中最接近q_rand的节点(即，最近节点)被设定为q_near，并在从q_near朝向q_rand间隔预定距离的位置处设定q_new(见图1C)。这里，仅q_init存在于初始树中(见图1A)。将q_near和q_new连接到树，且将q_new添加到树(见图1D)。

在运动空间内再次随机构造q_rand(见图1E)，将树中q_rand的最近节点设定为q_near，以与前述相同的方式由q_near和q_rand产生q_new，并将q_new添加到树(见图1E)。

通过重复执行上述操作，可以产生包围运动空间的树(见图1G)。

根据RRT，可以相对高速地计划路径，并产生不一定是最佳方案的合理路径。

图2A和2B以及图3A和3B示出了由RRT运算法则产生的路径的示例。在附图中，为了简化解释的目的，控制对象的形状被设定为盒形(或长方体)。但是，本发明不限于控制对象的特定形状，而可以应用于任意形状。应该注意，可以高速地对任意形状进行路径计划。后文将对路径计划方法的细节进行说明。

图2A和3A示出了盒状控制对象。图2B和3B未示出控制对象，而示出了其重心作为节点。在图2A和3A中，位于两端处的盒分别表示位于当前位置和姿态(-x方向)以及目标位置和姿态(+x方向)的控制对象，并且将他们连接的一系列盒表示在由RRT运算法则产生的路径中的控制对象。大致位于中心的盒表示在三位空间内存在的障碍物，并且为避免障碍物盒来搜索控制对象的路径。

如图2A和2B以及图3A和3B所示的RRT运算法则的构造空间具有位置(x，y，z)的三个维度，但可以具有包括姿态在内的六个维度，这不影响本发明的实施例。重点在于，在不频繁改变控制对象的姿态的情况下控制对象几乎不经过许多障碍物时，本发明发挥了强大的初始路径搜索的威力。但是，在此情况下，利用RRT运算法则进行初始路径搜索的速度的降低是不可否认的。因此，在此说明书中，将其中构造空间具有位置的三个维度的RRT运算法则作为用于更快速的初始路径搜索的实施例来进行说明。

当意图利用RRT运算法则获得初始位置和姿态路径时，存在避开障碍物时物体之间的渗透的问题。因此，当在本实施例中执行RRT运算法则时，对避开障碍物时对象之间的渗透的问题应用Gilbert-Johnson-Keerthi距离(GJK)运算法则。GJK运算法则是用于计算凸形图形之间的距离的迭代方法并用于输出代表碰撞点对(最近点对或最大渗透点对)或物体之间的最近距离作为干涉检测结果。使用动力学模型处理干涉检测结果，其细节将在下文描述。GJK运算法则的细节可以参考由G.Van den Bergen撰写的“Collision Detection in Interative 3D Enviroments”(Mogan Kaufmann出版社，2004)。

如由图2A和2B以及3A和3B可以看到，由RRT运算法则搜索的初始路径包括为避开障碍物的许多绕路，并作为路径而言较为浪费。因此，需要在利用RRT运算法则以高速获得初始位置和姿态路径之后满足(2)至(4)的要求。

在日本专利申请公开No.2008-55681所揭示的路径产生装置中，利用多质点***动力学模拟来满足(2)至(4)的要求。在此模拟中，控制对象的路径中的节点作为质点处理，构造其中表示路径计划的要求的力作用在路径中的相邻节点之间和最优路径中的对应节点之间的动力学模型，并通过执行动力学模拟演算来校正质点的位置，即路径。此处，因为路径中的节点被表示为质点，所以能够计算在路径中控制对象的位置，但是不能计算在路径中控制对象的姿态。

相反，在本实施例中，引入多刚体***动力学模拟来满足(2)至(4)的要求。“多刚体***动力学模拟”是通过将(2)至(4)的要求表达为作用在刚体上的外力并在相继将刚体模型与路径中的节点位置处的控制对象对应的同时执行动力学模拟演算来计算刚体的位置和姿态的方法。位于路径中的各个节点处的刚体的位置和姿态对应于当控制对象从当前位置和姿态移动到目标位置和姿态时的对应时间的控制对象的位置和姿态。多刚体***动力学模拟与多质点***动力学模拟的相似之处在于使表示路径计划的(2)至(4)的要求的力作用于控制对象的路径中的节点上，但是通过在各个节点中构造刚体来代替质点可以在产生路径时与控制对象的位置一起对控制对象的姿态进行处理。

首先，作为多刚体***动力学模拟的初始状态，由RRT运算法则搜索的路径被设定为控制对象的初始路径(如前所述)，并且初始路径中节点的位置被设定为刚体的代表点的初始位置。将初始姿态适当地分配给节点的刚体以构造包括初始位置和姿态的多刚体***动力学模型。例如，可以与全局坐标系相似地设定节点的刚体的初始姿态(即，各个刚体的局部坐标系)。图4示出了多刚体***，其中在全局坐标系中设定了初始姿态的刚***于初始路径中的节点的位置处。

将说明每个刚体的中心位置被设定为刚体的代表点，并且在节点中构造刚体，使得代表点对应于节点的位置，但是本发明不限于这种刚体构造方法。本发明不限于上述分配初始姿态的方法，利用仿样插值计算姿态或计划路径的设计者可以由他或她自己来设定姿态。

在此实施例中，将(2)至(4)的要求以施加到多刚体***的外力模型的形式表示为动力学限制，将动力学限制施加到多刚体***的刚体，然后执行多刚体***动力学模拟。因此，可以产生满足路径计划的全部(1)至(4)的要求的路径。即，通过以力的维度来统一全部要求，来组合解决问题。

现在将相继说明作为本发明的重点的、表示(2)至(4)的要求的动力学限制模型的方法。

首先，将说明“获得平滑的位置和姿态路径”的(2)的要求的动力学限制模型。

在图2A和2B以及3A和3B中，为了简化解释的目的，以盒形示出控制对象。但是，在现实中盒形较罕见，控制对象通常具有复杂形状。根据RRT运算法则的最短搜索距离(节点之间的距离)和物体形状之间的关系，在多刚体***的中被构造在路径中的节点处的相邻刚体可以彼此重叠。通过对采取最短搜索距离以产生尽可能平滑的路径进行考虑，多刚体***中的刚体不彼此重叠的情况很少发生。当由将相邻刚体彼此吸引的力(即，刚体间力)的动力学模型来表示“获得平滑的位置和姿态路径”的要求时，难以对动力学模型进行处理。

因此，在此实施例中，对各个刚体界定“虚拟刚体面”，以解决在其中在路径中的节点(与控制对象对应)处相继构造刚体的多刚体***中刚体的重叠问题。通过将刚体虚拟地处理为不具有体积的面，显然相邻刚体几乎不彼此重叠。

虚拟刚体面被固定在各个刚体中设定的姿态(即，局部坐标系)，并且与原刚体的相对位置关系在路径计划期间不发生改变。这里，可以在刚体的局部坐标系中任意界定虚拟刚体面，并且本发明不限于界定虚拟刚体面的上述方法。

图5示出了在计划好的路径中对第i个刚体界定的虚拟刚体面。在如图所示的示例中，刚体i的局部坐标系∑_i的xy面所取的截面被界定为虚拟刚体面。刚体i的虚拟刚体面具有矩形形状并由四个虚拟刚体顶点X_ij表示(其中，j是表示顶点序号并且是0至3的整数)。

在多刚体***动力学模型中，通过在相邻刚体之间施加力作为用于获得平滑的位置和姿态路径的力的限制表达，刚体的位置和姿态改变，并且路径被转换为有效连续平滑路径，从而能够满足(2)的要求。当将在位置和姿态路径中的各个节点处构造的刚体作为虚拟刚体面处理(图5)，并且通过利用其中用弹簧和阻尼器将相邻刚体面的对应顶点彼此并列连接的动力学模型来施加用于使相邻刚体彼此吸引(或推斥)的刚体间力以执行动力学模拟演算(后文说明)时，路径被转换为有效、连续且平滑的位置和姿态路径。因此，可以满足(2)的要求。

图6示出了其中通过具有弹簧常数K_s的弹簧和具有阻尼常数K_d的阻尼器将第i个刚体的虚拟刚体顶点X_ij和与其相邻的第(i+1)个刚体的虚拟刚体顶点X_i+1j彼此并列连接的多刚体***动力学模型(其中j＝0，1，2，3)。这里，在执行动力学模拟时，虚拟刚体顶点被转换为全局坐标系中的表达式。弹簧常数K_s和阻尼常数K_d在整个***中不是不变的，而是可以根据节点i或顶点j采用不同的弹簧常数和阻尼常数。

作用在相邻的虚拟刚体顶点X_ij和X_i+1j之间的弹簧力f^i，i+1 _s，j由表达式1表示。

表达式1

$f_{s,j}^{i,i+1}=\frac{-r_j^{i,i+1}}{\sqrt{r_j^{i,i+1^T}{r}_j^{i,i+1}}}{k}_{s,j}^{i,i+1}(\sqrt{r_j^{{i,i+1}^T}{r}_j^{i,i+1}}-l_j^{i,i+1})\·\·\·\left(1\right)$

这里，表达式1中的l^i，i+1 _j表示相邻的刚体顶点x_ij和x_i+1j之间的弹簧的自然长度，并且k^i，i+1 _s，j表示连接顶点的弹簧的弹簧常数。r^i，i+1 _j表示从虚拟刚体顶点x_ij指向相邻的对应虚拟刚体顶点x_i+1j的向量，并由表达式2表示。

表达式2

$r_j^{i,i+1}=(x_{i+1,j}-x_{i,j})/|x_{i+1,j}-x_{i,j}|$

由于弹簧而作用在相邻的虚拟刚体顶点x_ij和x_i+1j之间的弹簧力f_i，i+1 ^s，j如表达式1所表达的、由弹簧的归一方向和弹簧的归一力的乘积表示。

另一方面，由于阻尼器而作用在相邻的虚拟刚体顶点x_ij和x_i+1j之间的阻尼力f^i，i+1 _d，j由表达式3表示。这里，将顶点彼此连接的阻尼器的阻尼常数是k^i，i+1 _d，j，并且虚拟刚体顶点x_ij的速度向量是v^i，i+1 _j。

表达式3

$f_{d,j}^{i,i+1}=\frac{{-r}_j^{i,i+1}}{\sqrt{r_j^{{i,i+1}^T{r}_j^{i,i+1}}}}{k}_{d,j}^{i,i+1}\frac{r_j^{i,i+1^T}{v}_j^{i,i+1}}{\sqrt{r_j^{i,i+1}{r}_j^{i,i+1}}}\·\·\·\left(3\right)$

即，由两个相邻的虚拟刚体顶点的位置确定阻尼力的方向，并且相对速度的方向分量确定阻尼力的大小和正负号。

由表达式1和3表示的弹簧力f^i，i+1 _s，j和阻尼力f^i，i+1 _d，j是从第(i+1)个刚体的第j个虚拟刚体顶点作用在第i个刚体的第j个虚拟刚体顶点上的力，但通过转换f^i，i+1 _s，j和f^i，i+1 _d，j的符号来获得从第i个刚体的第j个虚拟刚体顶点作用在第(i+1)个刚体的第j个虚拟刚体顶点上的力。

从第(i+1)个刚体作用在第i个刚体上的刚体间力f^i，i+1 _n是作用在界定于刚体中的虚拟刚体面的虚拟刚体顶点j上的弹簧力f^i，i+1 _s，j和阻尼力f^i，i+1 _d，j的和值，并由表达式4表示。

表达式4

$f_n^{i,i+1}={\mathrm{\Σ}}_{j=0}^3{f}_{s,j}^{i,i+1}+{\mathrm{\Σ}}_{j=0}^3{f}_{d,j}^{i,i+1}\·\·\·\left(4\right)$

由于刚体间力f^i，i+1 _n而作用在第i个刚体上的关于全局坐标系的原点的力矩τ^i，i+1 _n是作用在虚拟刚体顶点上的力矩的和值，并由表达式5表示。

表达式5

${\mathrm{\τ}}_n^{i,i+1}={\mathrm{\Σ}}_{j=0}^3{x}_{i,j}\×f_{s,j}^{i,i+1}+{\mathrm{\Σ}}_{j=0}^3{x}_{i,j}\×f_{d,j}^{i,i+1}\·\·\·\left(5\right)$

作用在第i个刚体上的刚体间力fⁱ _n和τⁱ _n是由第(i-1)个和第(i+1)个刚体作用的刚体间力和力矩的和值，并由表达式6表示。

$f_n^i=f_n^{i,i-1}+f_n^{i,i+1}$

${\mathrm{\τ}}_n^i={\mathrm{\τ}}_n^{i,i-1}+{\mathrm{\τ}}_n^{i,i+1}\·\·\·\left(6\right)$

刚体间力f^i，i+1 _n改变刚体i的位置，而作用在相邻刚体之间的力矩τ^i，i+1 _n改变刚体i的姿态。即，从相邻的刚体(i-1)和(i+1)作用在刚体i上的推斥力fⁱ _p和力矩τⁱ _p是(4)的要求的动力学限制表达。通过将刚体间力和力矩施加到被构造在计划路径中的节点处的刚体，应该可以理解，获得了平滑的位置和姿态路径，并满足(2)的要求。

随后，将说明“在位置和姿态路径中避免物体之间的渗透”的(3)的要求的动力学限制表达。

在多刚体***动力学模型中，通过将与控制对象和障碍物之间的最短距离对应的推斥力施加到刚体作为为避免物体之间的渗透而赋予的力的限制表达，并执行多刚体***动力学模拟，来改变刚体的位置和姿态以获得有效、连续且平滑的位置和姿态路径，从而能够满足(3)的要求。

在执行多刚体***动力学模拟的过程中，开始基于GJK运算法则(上文描述)的物体间干涉检测功能以计算控制对象和障碍物之间的最短距离。这里，当代表碰撞点对(即，形成最短距离的障碍物中的点和控制对象中的点)是X^o _near(x^o _near，y^o _near，z^o _near)和X^r _near(x^r _near，y^r _near，z^r _near)时，控制对象和障碍物之间的推力势能由表达式7的高斯函数表示。这里，表达式中的k_p是表示势能的大小的常数，σ是表示势能的范围的常数。

表达式7

$U(x_{\mathrm{near}}^r,y_{\mathrm{near}}^r,z_{\mathrm{near}}^r)$

$=k_p\·\exp \left(\frac{{(x_{\mathrm{near}}^r-x_{\mathrm{near}}^o)}^2+{(y_{\mathrm{near}}^r-y_{\mathrm{near}}^o)}^2+{(z_{\mathrm{near}}^r-z_{\mathrm{near}}^o)}^2}{{\mathrm{\σ}}^2}\right)\·\·\·\left(7\right)$

这里，作用在控制对象(在路径中的第i个节点处构造的刚体)中的点X^r _near(x^r _near，y^r _near，z^r _near)上的人工推斥力fⁱ _p由表达式8表示。

表达式8

$f_{p,\mathrm{near}}^i=\left[\begin{array}{c}{f}_{p,\mathrm{nea}{r}_x}^i\\ f_{p,\mathrm{nea}{r}_y}^i\\ f_{p,\mathrm{nea}{r}_z}^i\end{array}\right]=-k\·\▿U(x_{\mathrm{near}}^r,y_{\mathrm{near}}^r,z_{\mathrm{near}}^r)$

${=[-\frac{\∂U}{\∂x_{\mathrm{near}}^r}-\frac{\∂U}{\∂y_{\mathrm{near}}^r}-\frac{\∂U}{\∂z_{\mathrm{near}}^r}]}^T$

$=\left[\begin{array}{c}{2k}_p\frac{x_{\mathrm{near}}^r-x_{\mathrm{near}}^o}{{\mathrm{\σ}}^2}\exp (-\frac{{(x_{\mathrm{near}}^r-x_{\mathrm{near}}^o)}^2+{(y_{\mathrm{near}}^r-y_{\mathrm{near}}^o)}^2+{(z_{\mathrm{near}}^r-z_{\mathrm{near}}^o)}^2}{{\mathrm{\σ}}^2})\\ {2k}_p\frac{y_{\mathrm{near}}^r-y_{\mathrm{near}}^o}{{\mathrm{\σ}}^2}\exp (-\frac{{(x_{\mathrm{near}}^r-x_{\mathrm{near}}^o)}^2+{(y_{\mathrm{near}}^r-y_{\mathrm{near}}^o)}^2+{(z_{\mathrm{near}}^r-z_{\mathrm{near}}^o)}^2}{{\mathrm{\σ}}^2})\\ {2k}_p\frac{z_{\mathrm{near}}^r-z_{\mathrm{near}}^o}{{\mathrm{\σ}}^2}\exp (-\frac{{(x_{\mathrm{near}}^r-x_{\mathrm{near}}^o)}^2+{(y_{\mathrm{near}}^r-y_{\mathrm{near}}^o)}^2+{(z_{\mathrm{near}}^r-z_{\mathrm{near}}^o)}^2}{{\mathrm{\σ}}^2})\end{array}\right]\·\·\·\left(8\right)$

在表达式(8)中使用了严密的干涉检测演算和最短距离计算演算，但是可以使用中心位置x_i和x^o _c(在全局坐标系中表示)来代替构成最短距离的、控制对象(刚体i)中的点和障碍物中的点，以减小计算量。在后者情况下，作用在控制对象(在路径中的第i个节点处构造的刚体)上的人工推斥力fⁱ _p，near由表达式9表示。

表达式9

$f_p^i=\left[\begin{array}{c}{f}_{p_x}^i\\ f_{p_y}^i\\ f_{p_z}^i\end{array}\right]=-k\·\▿U(x_i,y_i,z_i)=[-\frac{\∂U}{\∂x_i}-\frac{\∂U}{\∂y_i}-\frac{\∂U}{\∂z_i}{]}^T$

$=\left[\begin{array}{c}{2k}_p\frac{x_i-x_c^o}{{\mathrm{\σ}}^2}\exp (-\frac{{(x_i-x_c^o)}^2+{(y_i-y_c^o)}^2+{(z_i-z_c^o)}^2}{{\mathrm{\σ}}^2})\\ {2k}_p\frac{y_i^r-y_c^o}{{\mathrm{\σ}}^2}\exp (-\frac{{(x_i^r-x^c)}^2+{(y_i^r-y_c^o)}^2+{(z_i^r-z_c^o)}^2}{{\mathrm{\σ}}^2})\\ {2k}_p\frac{z_r^i-z_c^o}{{\mathrm{\σ}}^2}\exp (-\frac{{(x_i^r-x_c^o)}^2+{(y_i^r-y_c^o)}^2+{(z_i^r-z_c^o)}^2}{{\mathrm{\σ}}^2})\end{array}\right]\·\·\·\left(9\right)$

在表达式9中，(x^o _c，y^o _c，z^o _c)^T是障碍物的重心位置，并表示势能的中心。

由于推斥力fⁱ _p而作用在控制对象(在路径中的第i个节点处构造的刚体)上的关于全局坐标系的原点的力矩τⁱ _p由表达式10表示。

表达式10

${\mathrm{\τ}}_p^i=x_i\×f_p^i\·\·\·\left(10\right)$

人工推斥力fⁱ _p改变作用刚体i的位置，由于推斥力而作用在刚体i上的力矩τⁱ _p改变刚体的姿态。即，取决于控制对象和障碍物之间的距离而作用在刚体i上的推斥力fⁱ _p和力矩τⁱ _p是(3)的要求的动力学限制表达。以此方式，通过使推斥力和力矩作用在计划路径中的节点处构造的刚体上，应该理解，获得了避免物体之间的渗透的位置和姿态路径以满足(3)的要求。

现在将说明“尽可能接近理想的位置和姿态路径”的(4)的要求的动力学限制表达。

在此实施例中，控制对象的理想的姿态路径被界定为“使在路径中的节点处构造的刚体的前移方向尽可能地沿着路径”。这里，刚体的前移方向被固定于刚体的局部坐标系。这里，本发明不将刚体的前移方向限制为刚体的局部坐标系中的特定方向，而是可以将任意方向确定为前移方向，只要其固定于局部坐标系即可。

图7示出了当如上所述界定控制对象的理想姿态路径时的动力学模型。现在将参照附图详细说明“尽可能接近理想的位置和姿态路径”的动力学限制表达。

为了简化解释的目的，假定设定局部坐标系∑_I，使得将作为在位置和姿态路径中的各个节点处构造的刚体i的代表点的重心位置设定为原点。还假定刚体i的前移方向是将局部坐标系∑_i的原点作为起点来固定的方向dⁱ ₁。

在多刚体***动力学模型中，当第i和第(i+1)个刚体的重心位置分别是x_i和x_i+1时，相邻刚体之间的位置和姿态路径的方向d_i，i+1由表达式11表示。

表达式11

d_i，i+1＝(x_i+1-x_i)/|x_i+1-x_i|                               …(11)

根据理想的位置和姿态路径的上述界定，从刚体i指向刚体(i+1)并由表达式11表示的方向d_i，i+1是刚体i的前移方向dⁱ ₁上的理想位置和姿态路径。

此时，通过将弹簧常数k_τ乘以由从刚体i指向刚体(i+1)的方向d_i，i+1和固定于刚体i的局部坐标系的前移方向dⁱ ₁形成的角而获得的弹簧力作为围绕d_i，i+1和dⁱ ₁的外积方向的力而添加到多刚体***动力学模型，从而表示尽可能接近理想的姿态路径的动力学限制。即，为接近理想路径而作用的力矩τⁱ _o由表达式12表示。

表达式12

${\mathrm{\τ}}_o^i=k_{\mathrm{\τ}}^{i}a\tan 2\left(\right|d_{i,i+1}\×d_l^i|,d_{i,i+1}\·d_l^i)\·(d_{i,i+1}\×d_l^i/|d_{i,i+1\×d_l^{}}i\left|\right)\·\·\·\left(12\right)$

在表达式12中，k_τ被作为恒量来处理，但是可以被作为变量来处理。例如，k_τ可以被作为取决于从初始状态(与控制对象的当前位置对应的刚体O)到刚体i的距离或者从刚体i到目标状态(与控制对象的目标位置对应的刚体N)的距离而变化的变量来处理。以此方式，通过将k_τ作为取决于距离而变化的变量来处理，容易保持与其他动力学限制组合时的一致性，从而获得更平滑的路径。

作用在刚体i上以尽可能接近理想路径的力fⁱ _o是由表达式13表示的0。

表达式13

$f_o^i=0\·\·\·\left(13\right)$

在(4)的要求的动力学表达中，还引入基于诸如空气阻力之类的粘性阻力的力以控制多刚体动力学模拟的收敛。当刚体i的速度是vⁱ _c且作用在刚体i上的粘性系数是kⁱ _c时，基于作用在刚体i上的粘性阻力的力fⁱ _d由表达式14表示。

表达式14

$f_v^i=-k_v^i{v}_c^i\·\·\·\left(14\right)$

由于由表达式14表示的粘性力fⁱ _d引起的围绕全局坐标系的原点的力矩由表达式15表示。

表达式15

${\mathrm{\τ}}_v^i=-x_i\×k_v^i{v}_c^i\·\·\·\left(15\right)$

已经说明了表示(2)至(4)的要求的动力学限制并作用于多刚体***动力学模型中的刚体i上的力和转矩。通过将作用在刚体i上的外力相加来获得的合计外力F_i由表达式16表示。

表达式16

$F_i=\begin{array}{c}\left[\begin{array}{c}{f}_i\\ {\mathrm{\τ}}_i\end{array}\right]=\left[\begin{array}{c}{f}_n^i\\ {\mathrm{\τ}}_n^i\end{array}\right]\end{array}+\left[\begin{array}{c}{f}_p^i\\ {\mathrm{\τ}}_p^i\end{array}\right]+\left[\begin{array}{c}{f}_o^i\\ {\mathrm{\τ}}_o^i\end{array}\right]+\left[\begin{array}{c}{f}_v^i\\ {\mathrm{\τ}}_v^i\end{array}\right]\·\·\·\left(16\right)$

如表达式17和18所表示的，作为牛顿运动方程式和欧拉运动方程式，给出对刚体i的运动进行描述的六个独立方程式。

表达式17

$f_i=\frac{d}{\mathrm{dt}}{P}_i=m_i{\stackrel{\·\·}{x}}_i\·\·\·\left(17\right)$

表达式18

${\mathrm{\τ}}_i=\frac{d}{\mathrm{dt}}{L}_i=I_i{\stackrel{\·}{\mathrm{\ω}}}_i+{\mathrm{\ω}}_i+I_i{\mathrm{\ω}}_i\·\·\·\left(18\right)$

在表达式17中，P_i和m_i分别表示刚体i的动量和质量。在表达式18中，L_i、I_i、和ω_i分别表示刚体i的角动量、全局坐标系中的惯性张量、和角速度向量。

作用在刚体i上并由表达式16表示的合计外力F_i＝[f_i，τ_i]^T被添加作为由表达式17和18表示的牛顿运动方程式和欧拉运动方程式的外力，接着执行多刚体动力学模拟。

即，通过将N个刚体的耦合运动表示为N个刚体运动方程式并在模拟步骤中执行数值积分，可以获得各个时间(即，各个节点)的刚体i的位置和姿态。结果，实现了满足(1)至(4)的全部要求的路径计划，以达到迅速地产生从控制对象的当前位置和姿态到目标位置和姿态的平滑路径的目的。

图8示意性地示出了实现上述控制对象的路径计划的***的功能构造。所示***包括RRT执行单元81、干涉检测器82、外力计算器83、以及多刚体***动力学模拟单元84。

当接收到路径计划指令时，RRT执行单元81基于三维空间内的障碍物信息以及控制对象的当前位置和姿态及目标位置和姿态，利用RRT运算法则来搜索或产生从当前位置和姿态到目标位置和姿态的作为最低目的的路径。由RRT执行单元81获得的初始位置和姿态路径包括位于控制对象的当前位置和姿态与其目标位置和姿态之间的多个节点。

当RRT执行单元81搜索或产生了控制对象的初始位置和姿态路径时，干涉检测器82执行干涉检测演算，并在搜索或产生路径时对避免控制对象和障碍物之间的渗透进行考虑。因此，从RRT执行单元81输出的控制对象的初始位置路径包括许多绕路，但不与障碍物发生干涉。可以使用GJK运算法则(前文描述)来解决在避开障碍物时物体之间的渗透。

外力计算器83针对与在由RRT执行单元81搜索或产生的控制对象的初始位置和姿态路径中的每个节点处构造的控制对象对应的刚体，计算对在避免与障碍物的干涉的同时对位置和姿态路径进行校正的要求进行表达的外力。在本实施例中，外力计算器83包括刚体间力计算器83A、障碍物避开推斥力计算器83B和最优路径力计算器83C，刚体间力计算器83A计算刚体间力作为用于将初始位置和姿态路径校正为不具有绕路的平滑位置和姿态路径的动力学限制，障碍物避开推斥力计算器83B计算障碍物避开推斥力作为用于避免控制对象与存在于运动空间内的障碍物的干涉的动力学限制，最优路径力计算器83C计算最优路径力作为用于尽可能接近理想位置和姿态路径的动力学限制。

多刚体***动力学模拟单元84使得作用在路径中的各个刚体上并由外力计算器83计算的外力(即，刚体间力、障碍物避开推斥力和最优路径力)用作刚体运动的外力限制，并在每个模拟周期执行多刚体***动力学演算。

当路径收敛到设定状态(例如刚体的校正)时，由多刚体***动力学模拟单元84执行的多刚体***动力学演算结束，并且输出由多刚体***形成的节点序列作为平滑目标位置和姿态路径。

图9示出了图示实现如图8所示的***中的控制对象的路径计划的处理流程的流程图。

首先，作为路径搜索和产生处理，RRT执行单元81接收路径计划指令，并基于三维空间内的障碍物信息以及控制对象的当前位置和姿态及目标位置和姿态，来搜索或产生从当前位置和姿态到目标位置和姿态的作为最低目的的路径(步骤S1)。所产生的初始位置和姿态可以包括绕路。

然后，外力计算器83在每个模拟周期执行对用于实现路径平滑、避免障碍物干涉和向理想路径吸引的动力学限制进行表达的外力的计算(步骤S2)。

多刚体***动力学模拟单元84在每个模拟周期执行多刚体***动力学演算，并更新多刚体***的状态(即，在控制对象的路径中的各个节点的位置和姿态)(步骤S3)。

在控制对象的位置和姿态路径收敛到预定静态状态之前，从步骤S2起重复步骤S2至S4的处理。

当多刚体***收敛到设定状态时，结束多刚体***动力学模拟单元84的动力学演算，并结束整个路径产生处理。结果，产生了控制对象的位置和姿态路径，其不与障碍物发生干涉，是平滑的，且接近理想路径。

图10示出了利用多刚体***动力学模拟对如图3所示的由RRT运算法则获得的初始路径进行的校正结果。可以确认，所获得的控制对象的路径在位置和姿态方面较平滑，且避免了与障碍物的干涉。

图11示出了利用多刚体***动力学模拟校正的路径的另一个示例。图12示出了在如图11所示的路径中刚体的位置。从图12可见，全部x、y和z的路径被校正为平滑且有效的路径。

图13示出了利用多刚体***动力学模拟校正的路径的另一个示例。图14示出了在如图13所示的路径中刚体姿态的俯仰角。从图14可见，刚体姿态路径被校正为平滑且有效的路径。

在此实施例中，使弹簧力作用在相邻刚体之间(前文描述)作为“获得平滑的位置和姿态路径”的(2)的要求。这里，弹簧常数k_s可以被设定为在全***内不变(即恒量)或可以被设定为取决于从当前状态下的刚体O到目标状态下的刚体N的距离而改变的变量。

图15示出了在弹簧常数是恒量的情况下多刚体***动力学模型的路径产生结果的快照。图16示出了在弹簧常数不是恒量而是取决于刚体i和在当前状态下的刚体O或目标状态下的刚体N之间的距离而变化的变量的情况下路径产生结果的快照。图17示出了图示当弹簧常数被设定为恒量和变量时产生的路径中的刚体(节点)的刚体姿态的图。在图17中，由黑圆绘制的曲线表示当弹簧常数被设定为恒量时(即，与图15对应)刚体姿态的变化，由白圆绘制的曲线表示当弹簧常数被设定为取决于距离变化的变量时(即，与图16对应)刚体姿态的变化。从图17可见，当弹簧常数被设定为取决于距离变化的变量时路径更平滑且更不急剧变化。

在此实施例中，使与控制对象和障碍物之间的最短距离对应的推斥力作用在刚体上(前文描述)作为“在位置和姿态路径中避免物体之间的渗透”的(3)的要求的动力学限制表达。图18示出了当障碍物从如图3所示的初始状态移动到图中更深侧时多刚体***动力学模拟的结果。从图中可见，伴随障碍物移动，所产生的路径被校正。

在此实施例中，将(2)至(4)的要求表达为作用在刚体上的动力学限制，并且通过将作用在刚体上的外力相加获得的合计外力作为运动方程式的外力添加，从而执行多刚体***动力学模拟。

图19和20示出了执行多刚体***动力学模拟并且静态确定了刚体组的状态。图21和22示出了在将校正值给予目标位置和姿态并且在执行模拟之后模拟收敛的状态下的快照。在图19和21中，示出了控制对象物体。在图20和22中，未示出控制对象物体，将刚体组的重心位置示出作为节点。从图19至22可见，即使在实时校正目标位置和姿态的情况下也良好地产生了路径。

通过利用根据本发明实施例的上述路径计划方法，可见为达到上述目所需的要求全部得到了满足，并且能够良好地产生目标路径。根据此方法，在不需要准备不同的运算法则的情况下，统一处理并实时进行诸如避免障碍物干涉以及目标位置和姿态校正之类的控制。结果，此方法较为有效。

通过使诸如计算机之类的信息处理设备执行预定应用来实施产生控制对象的路径的处理(如图8所示的***)。图23示出了能够实施本发明的计算机的构造。

中央处理单元(CPU)1在由操作***(OS)提供的程序执行环境下执行只读存储器(ROM)2或硬盘驱动器(HDD)11中存储的程序。例如，可以通过使CPU 1执行预定程序，来实施多链结构的运动控制或产生其运动数据的处理。

ROM 2永久性地存储POST(开机自检)或BIOS(基本输入输出***)的程序代码。随机存取存储器(RAM)3用于装载ROM 2或HDD 11中存储的程序以在CPU 1中执行，或临时地存储正在执行的程序的工作数据。这些元件经由直接连接到CPU 1的本地针脚的本地总线4而彼此连接。

本地总线4经由桥接器5连接到诸如PCI(外设组件互连)总线之类的输入和输出总线6。

键盘8和诸如鼠标之类的指点装置9是由用户操作的输入装置。显示器10由LCD(液晶显示器)、CRT(阴极射线管)等形成，并用于以文字或图像显示各种信息。

HDD 11是具有硬盘作为内置记录介质的驱动单元，并用于驱动硬盘。硬盘用于安装诸如操作***或由CPU 1执行的各种应用之类的程序，或者用于存储数据和文件。例如，用于实施多链结构的运动控制或产生其运动数据的处理的应用程序被安装在HDD 11中，或者所产生的运动数据的文件被存储在HDD 11中。

通信单元12例如是有线通信或无线通信的LAN(局域网))的接口。例如，可以经由网络安装用于实现产生机器人或多关节臂的从当前位置和姿态到目标位置和姿态的路径的处理的应用程序。可以经由网络下载路径数据，或者可以经由网络上载所产生的路径数据。

本申请包含与2009年1月1日在日本专利局提交的日本在先专利申请JP2009-000002中揭示的主题相关的主题，其全文通过引用被结合于此。

本领域的技术人员应当理解，在权利要求或其等同方案的范围内，可以取决于设计要求和其他因素进行各种修改、组合、子组合和替换。

Claims (8)

1.一种路径计划装置，其对控制对象在运动空间内从当前位置和姿态到目标位置和姿态的位置和姿态路径进行计划，并包括：

初始位置和姿态路径获得单元，其获得将所述控制对象的所述当前位置和姿态连接到所述目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在所述位置和姿态路径中的各个节点中构造与所述控制对象对应的刚体；

干涉检测器，其检测存在于所述运动空间内的障碍物与所述位置和姿态路径中的所述控制对象的干涉；

外力计算器，其计算对于各个刚体的外力，所述外力表达了用于使所述位置和姿态路径平滑和避免与所述障碍物干涉的要求；以及

多刚体***动力学模拟单元，其对多个刚体运动方程式执行多刚体***动力学模拟，并且其基于当所述路径收敛到预定状态时的刚体的位置和姿态获得所述控制对象与所述位置和姿态路径中的节点对应的时间时的位置和姿态，在所述多个刚体运动方程式中，对所述多个刚体的耦合运动进行描述，并且由所述外力计算器计算得到的所述外力分别作用于所述多个刚体。

2.根据权利要求1所述的路径计划装置，其中，所述初始位置和姿态路径获得单元基于在所述多个节点被分支的多个路线并在所述运动空间内展开的搜索树来获得所述控制对象的所述当前位置和所述目标位置之间的路径，并对位于各个节点处的所述刚体赋予初始姿态。

3.根据权利要求1所述的路径计划装置，其中，所述干涉检测器应用GJK运算法则(Gilbert-Johnson-Keerthi距离运算法则)以检测各个刚体和所述障碍物之间的最短距离。

4.根据权利要求1所述的路径计划装置，其中，所述外力计算器利用如下动力学模型：定义被固定在各个刚体的局部坐标系中的虚拟刚体面，通过具有预定弹簧常数的弹簧和具有预定阻尼常数的阻尼器将相邻的所述虚拟刚体面的对应顶点彼此并列连接，以计算刚体间力和转矩作为表达了用于使所述位置和姿态路径平滑的要求的所述外力，所述刚体间力是作用在一个虚拟刚体面的顶点和相邻的虚拟刚体面的对应顶点之间的弹簧力和阻尼力的和值，所述转矩作用在所述刚体上，并围绕全局坐标系的原点并包括由于所述弹簧力和所述阻尼力而作用在各个虚拟刚体的顶点上的力矩的和值。

5.根据权利要求1所述的路径计划装置，其中，所述干涉检测器检测位于所述位置和姿态路径中的各个节点处的所述刚体和所述障碍物之间的最短距离，并且

其中，所述外力计算器计算取决于所述最短距离而作用在各个刚体上的推斥力，作为表达了用于使所述位置和姿态路径避免与所述障碍物干涉的要求的所述外力。

6.根据权利要求1所述的路径计划装置，其中，所述外力计算器计算与由固定在所述刚体的所述局部坐标系中的每个刚体的前移方向和相邻的所述刚体之间的所述位置和姿态路径的方向形成的角对应的力矩，作为表达了用于靠近理想的位置和姿态路径的要求的外力。

7.一种路径计划方法，其对控制对象在运动空间内从当前位置和姿态到目标位置和姿态的位置和姿态路径进行计划，并包括以下步骤：

获得将所述控制对象的所述当前位置和姿态连接到所述目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在所述位置和姿态路径中的各个节点中构造与所述控制对象对应的刚体；

检测存在于所述运动空间内的障碍物与所述位置和姿态路径中的所述控制对象的干涉；

计算对于各个刚体的外力，所述外力表达了用于使所述位置和姿态路径平滑和避免与所述障碍物干涉的要求；

对多个刚体运动方程式执行多刚体***动力学模拟，并且基于当所述路径收敛到预定状态时的刚体的位置和姿态获得与所述位置和姿态路径中的节点对应的时刻的所述控制对象的位置和姿态，在所述多个刚体运动方程式中，对所述多个刚体的耦合运动进行描述，并且由所述外力计算器计算得到的所述外力分别作用于所述多个刚体。

8.一种计算机程序，其以计算机可读的方式描述以使得计算机执行对控制对象在运动空间内从当前位置和姿态到目标位置和姿态的位置和姿态路径进行计划的处理，所述计算机程序使得所述计算机用作：

初始位置和姿态路径获得单元，其获得将所述控制对象的所述当前位置和姿态连接到所述目标位置和姿态的、包括多个节点的初始位置和姿态路径，并在所述位置和姿态路径中的各个节点中构造与所述控制对象对应的刚体；

干涉检测器，其检测存在于所述运动空间内的障碍物与所述位置和姿态路径中的所述控制对象的干涉；

外力计算器，其计算对于各个刚体的外力，所述外力表达了用于使所述位置和姿态路径平滑和避免与所述障碍物干涉的要求；以及

多刚体***动力学模拟单元，其对多个刚体运动方程式执行多刚体***动力学模拟，并且其基于当所述路径收敛到预定状态时的刚体的位置和姿态获得与所述位置和姿态路径中的节点对应的时刻的所述控制对象的位置和姿态，在所述多个刚体运动方程式中，对所述多个刚体的耦合运动进行描述，并且由所述外力计算器计算得到的所述外力分别作用于所述多个刚体。

Images