轨道计划生成装置、轨道计划生成方法以及轨道计划生成程序与流程

轨道计划生成装置、轨道计划生成方法以及轨道计划生成程序
1.本技术主张在令和1年(2019年)11月19日申请的日本技术即日本特愿2019-208578的优先权，通过参照其内容而引入到本技术中。
技术领域
2.本发明涉及生成轨道计划的轨道计划生成装置、轨道计划生成方法以及轨道计划生成程序。


背景技术：

3.作为本技术领域的背景技术，有专利文献1。专利文献1的轨道计划装置具备：距离计算单元，其计算所计划的轨道上的机器人与障碍物的距离；离散化设定单元，其基于由距离计算单元计算出的距离，设定将从起点到终点的轨道间离散化的离散化宽度；干涉判定单元，其在以由离散化设定单元设定的离散化宽度进行离散化时的轨道上的各位置，判定机器人与障碍物是否干涉；以及轨道计划单元，其基于干涉判定单元的干扰判定结果，计划机器人的轨道。
4.现有技术文献
5.专利文献
6.专利文献1：日本特开2013-246553号公报


技术实现要素：

7.发明所要解决的课题
8.专利文献1的轨道计划装置基于干涉判定，计算出从周边的干涉物离开了一定距离以上的轨道。然而，专利文献1的轨道计划装置不考虑末端姿势而计算轨道，因此把持物的姿势有可能急剧变化，有可能对把持物造成损伤。另外，若为了削减对把持物的损伤而延迟控制时刻，则周期时间延长，生产率降低。
9.本发明的目的在于制定实现所保持的物体的稳定化的计划。
10.用于解决课题的手段
11.在本技术中公开的发明的一个方面的轨道计划生成装置具有执行程序的处理器和存储所述程序的存储设备，所述轨道计划生成装置生成具有多个关节的机器臂的前端部从起点向终点移动的轨道计划，其中，所述处理器执行如下处理：第一搜索处理，搜索从第一轨道信息起的预定距离内的成为所述前端部的移动目的地的多个位置候选，其中，所述第一轨道信息表示从所述起点到所述终点之间的所述前端部的位置以及姿势；第二搜索处理，搜索通过球面插补而在容许范围内变化的所述前端部的多个姿势候选，其中，所述球面插补基于所述起点和所述终点的所述前端部的姿势；决定处理，基于通过所述第一搜索处理搜索到的多个位置候选以及通过所述第二搜索处理搜索到的多个姿势候选，决定从所述第一轨道信息起的表示所述前端部的移动目的地的位置以及姿势的第二轨道信息；以及输
出处理，输出包含所述第一轨道信息以及通过所述决定处理决定的第二轨道信息的轨道数据。
12.发明效果
13.根据本发明的代表性的实施方式，能够制定实现所保持的物体的稳定化的计划。上述以外的课题、结构以及效果通过以下的实施例的说明而变得明确。
附图说明
14.图1是表示轨道计划生成例的说明图。
15.图2是表示轨道计划生成装置的硬件结构例的框图。
16.图3是表示机器臂结构db的一例的说明图。
17.图4是表示障碍物结构db的一例的说明图。
18.图5是表示开始关节角度db的一例的说明图。
19.图6是表示目标位置姿势db的一例的说明图。
20.图7是表示末端位置候选计算方法db的一例的说明图。
21.图8是表示末端姿势变动候选计算方法db的一例的说明图。
22.图9是表示控制时刻赋予方法db的一例的说明图。
23.图10是表示轨道点db的一例的说明图。
24.图11是表示末端搜索数据的一例的说明图。
25.图12是表示轨道计划生成装置的功能性结构例的框图。
26.图13是表示轨道计划生成装置的轨道计划生成处理步骤例的流程图。
27.图14是表示图13所示的搜索部的搜索处理(步骤s1302)的详细处理步骤例的流程图。
28.图15是表示基于步骤s1406的末端位置候选计算方法的轨道点候选的计算例的说明图。
29.图16是表示基于步骤s1407的末端位置候选计算方法的轨道点候选的计算例的说明图。
30.图17是表示图14所示的关节角度计算处理(步骤s1408)的详细的处理步骤例的流程图。
31.图18是表示图13所示的平滑化部的平滑处理(步骤s1303)的详细处理步骤例的流程图。
32.图19是表示步骤s1804中的插补曲线的计算例的说明图。
33.图20是表示图13所示的赋予部的赋予处理(步骤s1304)的详细处理步骤例的流程图。
具体实施方式
34.《轨道计划生成例》
35.图1是表示轨道计划生成例的说明图。轨道计划生成装置100计划机器臂101的连续的轨道点p1～pn(n为1以上的整数)，生成机器臂的轨道计划数据120。将轨道点p1～pn的一系列的机器臂101的动作轨道称为轨道数据t。
36.机器臂101是具有m个(m为2以上的整数，在图1中，m＝4)关节j1～jm、连接关节j1～jm之间的m个连杆l1～lm、以及与连杆lm连接的前端部(也称为“末端”)102的机械手。轨道点p是表示机器臂101的各关节以及前端部的3维位置与根据各关节j1～jm的动作而变化的各连杆l1～lm以及前端部102的姿势的组合的轨道信息。
37.此外，在不区分轨道点p1～pn的情况下，简单地表述为轨道点p。在不区分关节j1～jm的情况下，简单地标记为关节j。在不区分连杆l1～lm的情况下，简单地表述为连杆l。机器臂101能够通过轨道计划生成装置100或其他计算机进行动作控制。
38.此外，在图1中，在将由相互正交的x轴、y轴以及z轴展开的3维空间设为全局坐标系110的情况下，机器臂101载置于xy平面。xy平面是由x轴和y轴展开的平面。在图1中，z轴例如表示铅垂方向，将从下向上的方向设为 z方向，将从上向下的方向设为-z方向。另外，将绕x轴的旋转角及旋转方向用翻滚(roll)r表示，将绕y轴的旋转角及旋转方向用俯仰(pitch)p表示，将绕x轴的旋转角及旋转方向用偏航(yaw)y表示。另外，关节j1位于原点o。
39.另外，对于各关节jm的局部坐标系，将该坐标系的原点on设为关节jm的位置，设为由相互正交的xm轴、ym轴以及zm轴展开的3维空间。另外，将绕xm轴、ym轴以及zm轴的各轴的旋转角以及旋转方向表述为翻滚rm、俯仰pm以及偏航ym。xm轴是连杆l(m-1)的长度方向，ym轴是与xy平面平行的轴。其中，关节j1的xm轴为全局坐标系110的z轴。
40.关节jm使连杆lm或前端部102旋转。具体而言，例如，关节j1使连杆l1在偏航方向y(翻滚方向r1)旋转。关节j1不使连杆l1在翻滚方向r以及俯仰方向p(俯仰方向p1以及偏航方向y1)旋转。
41.关节j2使连杆l2在翻滚方向r2以及俯仰方向p2旋转。关节j3使连杆l3在翻滚方向r3及俯仰方向p3旋转。关节j4使前端部102在翻滚方向r4以及俯仰方向p4旋转。前端部102保持物体103。具体而言，前端部102例如是吸嘴、磁铁或手持件。
42.在机器臂101保持有物体103的情况下，为了稳定地保持物体103，轨道计划生成装置100计划尽可能不使前端部102的姿势变化且不与障碍物104干涉的轨道点p1～pn，并输出轨道数据t。
43.轨道计划数据120是用于再生轨道数据t的数据。轨道计划数据120具有读入按钮121、计算开始按钮122、信息显示部123和模拟机器臂101的轨道的模拟动画124。轨道计划数据120显示于轨道计划生成装置100或与轨道计划生成装置100可通信地连接的其他计算机的显示器。
44.当按下读入按钮121时，读入轨道计划数据120的生成所需的数据。当按下计算开始按钮122时，使用读入的数据，开始用于生成轨道计划数据120的计算，生成轨道数据t。当计算结束时，显示信息显示部123。
45.信息显示部123具有计算轨道计划数据120时的条件131、该条件131下的机器臂101的动作时间132、以及再生模拟动画124的动作再生按钮133。当按下动作再生按钮133时，在该动作时间132再生模拟动画124。由此，用户能够根据每个条件131的模拟动画124来再生轨道数据t，确认机器臂101的轨道。
46.《轨道计划生成装置100的硬件结构例》
47.图2是表示轨道计划生成装置100的硬件结构例的框图。轨道计划生成装置100具有处理器201、存储设备202、输入设备203、输出设备204以及通信接口(通信if)205。处理器
201、存储设备202、输入设备203、输出设备204以及通信if205通过总线206连接。处理器201控制轨道计划生成装置100。存储设备202是处理器201的作业区域。另外，存储设备202是存储各种程序、数据的非暂时性或暂时性的记录介质。作为存储设备202，例如有rom(read only memory：只读存储器)、ram(random access memory：随机存取存储器)、hdd(hard disk drive：硬盘驱动器)、闪存。输入设备203是输入数据的设备。作为输入设备203，例如有键盘、鼠标、触摸面板、数字键、扫描仪。输出设备204是输出数据的设备。作为输出设备204，例如有显示器、打印机。通信if205与网络连接，收发数据。
48.《数据结构例》
49.在此，具体说明存储设备202所存储的数据。存储设备202存储机器臂结构db211、障碍物结构db212、开始关节角度db213、目标位置姿势db214、末端位置候选计算方法db215、末端姿势变动候选计算方法db216、控制时刻赋予方法db217、轨道点db218以及末端搜索数据219。此外，这些数据也可以存储于轨道计划生成装置100能够访问的其他计算机的存储设备202。以下，使用图3～图11进行说明。
50.图3是表示机器臂结构db211的一例的说明图。机器臂结构db211是存储与构成机器臂101的关节j和连杆l相关的数据的数据库，具体而言，例如具有关节信息db301和连杆信息db302。
51.关节信息db301是存储与关节j相关的信息的数据库。具体而言，例如，关节信息db301具有关节名311、动作种类312、关节位置313、旋转轴314、动作下限315、动作上限316、最大速度317以及最大加速度318作为字段。同一行的各字段的值的组合构成1个关节j。
52.关节名311是唯一地确定关节j的名称。动作种类312表示旋转或平行移动等与关节j的动作相关的种类。关节位置313表示相对于全局坐标系110中的前1个关节j(m-1)的全局坐标系110的3维位置(x，y，z)。此外，关节j1的关节位置313是全局坐标系110的原点o。
53.旋转轴314表示在该关节j的局部坐标系中关节j能够旋转的轴(a、b、c)(a、b、c＝0或1)。a与xm轴对应，b与ym轴对应，c与zm轴对应，“0”表示不是旋转轴314，“1”表示是旋转轴314。在图3中，关节j1使连杆l1在绕x1轴(z轴)的翻滚方向r1(翻滚方向r)旋转，因此关节j1的旋转轴314由(1，0，0)表现。
54.动作下限315是关节j能够动作的下限值，动作上限316是关节能够动作的上限值，关节j能够在从动作下限315到动作上限316的范围内进行动作种类312所示的动作。
55.最大速度317是关节j的动作速度的最大值。如果动作种类312为“旋转”，则动作速度为旋转速度。最大加速度318是关节j的动作加速度的最大值。如果动作种类312为“旋转”，则动作速度为旋转加速度。
56.连杆信息db302是存储与连杆l相关的信息的数据库。具体而言，例如，连杆信息db302具有连杆名321、母关节名322、子关节名323以及连杆形状324作为字段。同一行的各字段的值的组合规定1个连杆l的关节信息。
57.连杆名321是唯一地确定连杆l的名称。母关节名322是使连杆l动作的关节j的关节名311。子关节名323是与连杆lm连接的关节j(m 1)的关节名311。例如，在连杆l1的情况下，母关节名322为j1，子关节名323为j2。
58.连杆形状324是连杆l的实际形状，例如是以step(standard for the exchange of product model data：产品模型数据交换标准)等格式保存的实体数据、或以stl
(stereo lithography：立体光刻)等格式保存的多边形数据。
59.图4是表示障碍物结构db212的一例的说明图。障碍物结构db212是存储构成障碍物的数据的数据库。具体而言，例如，障碍物结构db212具有障碍物id401、障碍物形状402以及障碍物位置姿势403作为字段。同一行的各字段的值的组合构成1个障碍物104。
60.障碍物id401是唯一地确定障碍物104的识别信息。障碍物形状402是障碍物104的实际的形状，例如是以step等格式保存的实体数据、或以stl等格式保存的多边形数据。障碍物位置姿势403是规定全局坐标系110中的障碍物104的中心位置(x，y，z)和姿势(翻滚角r、俯仰角p、偏航角y)的信息。
61.图5是表示开始关节角度db213的一例的说明图。开始关节角度db213是存储关节j的开始关节角度501的数据库。开始关节角度501是由关节名311确定的关节绕旋转轴314的角度的初始值、即动作开始前的角度。在存在多个旋转轴314的情况下(例如，旋转轴314＝(1，1，0))，按照每个旋转轴314设定开始关节角度501。将全部关节j1～j4的开始关节角度501的值的列称为开始关节角度矢量θ1。
62.图6是表示目标位置姿势db214的一例的说明图。目标位置姿势db214是存储前端部102的目标位置和姿势的数据库。具体而言，例如，目标位置姿势db214具有姿势id601、对象连杆名602和位置姿势信息603作为字段。同一行的各字段的值的组合构成成为1个目标位置姿势的轨道点pn(以下，有时也表述为目标位置姿势pn)。
63.姿势id601是唯一地确定姿势的识别信息。对象连杆名602是与前端部102连接的连杆l的连杆名321。位置姿势信息603是规定全局坐标系110中的前端部102的中心位置(x，y，z)和姿势的信息。具体而言，前端部102的姿势例如是连接前端部102的关节jm的姿势(翻滚角rm、俯仰角pm、偏航角ym)。目标位置姿势db214的条目存在1个以上，但从外部选择任意1个条目。
64.图7是表示末端位置候选计算方法db215的一例的说明图。末端位置候选计算方法db215是规定末端位置候选计算方法701的数据库。具体而言，例如，末端位置候选计算方法db215具有末端位置候选计算方法701和1个步骤中的最大移动距离702作为字段。同一行的各字段的值的组合构成1个末端位置候选计算方法701。
65.末端位置候选计算方法701是相当于计算末端位置候选的方法的各种计算方法。例如，存在轴并行(axis paralell)、随机等方法。1个步骤中的最大移动距离702是1个步骤的处理(后述的步骤s1406的1次处理)中的前端部102的移动距离的最大值。
66.图8是表示末端姿势变动候选计算方法db216的一例的说明图。末端姿势变动候选计算方法db216是规定末端姿势变动候选计算方法的数据库。具体而言，例如，末端姿势变动候选计算方法db216具有翻滚角最大位移801、俯仰角最大位移802、偏航角最大位移803以及1个步骤中的最大位移804作为字段。同一行的各字段的值的组合构成1个末端姿势变动候选计算方法。
67.翻滚角最大位移801是翻滚角rm的位移量的最大值。俯仰角最大位移802是俯仰角pm的位移量的最大值。偏航角最大位移803是偏航角ym的位移的最大值。翻滚角最大位移801、俯仰角最大位移802以及偏航角最大位移803在末端位置候选计算方法701中使用，是相对于基准姿势的最大位移角。基准姿势是指以连杆l(m-1)的长度方向为xm轴的情况下的关节jm的姿势(rm，pm，ym)。1个步骤中的最大位移804是1次处理(后述的步骤s1407的1次处
理)中的前端部102的姿势的最大位移。
68.图9是表示控制时刻赋予方法db217的一例的说明图。控制时刻赋予方法db217是存储控制时刻赋予方法的数据库。具体而言，例如，控制时刻赋予方法db217具有参数id901、末端最大速度902以及末端最大加速度903作为字段。同一行的各字段的值的组合构成1个控制时刻赋予方法。
69.参数id901是唯一地确定由末端最大速度902和末端最大加速度903规定的控制时刻赋予方法的识别信息。末端最大速度902是前端部102的速度的最大值。末端最大加速度903是前端部102的加速度的最大值。
70.图10是表示轨道点db218的一例的说明图。轨道点db218是存储轨道点数据的数据库。轨道点数据是成为轨道计划数据120的基础的数据。轨道点db218具有轨道点id1001、关节角度1002、关节角速度1003、关节角加速度1004、可否平滑化1005以及控制时刻1006作为字段。同一行的各字段的值的组合构成表示1个轨道点p的轨道点数据。
71.轨道点id1001是唯一地确定轨道点p的识别信息。有时将轨道点p1～pn中的初始位置的轨道点p1称为“起点”，将最终位置的轨道点pn称为“终点”。
72.关节角度1002是图1所示的轨道点p1～pn各自的关节j绕旋转轴314的角度。例如，如果轨道点id1001的“p1”是起点，则该关节角度1002成为开始关节角度501(θ1)。
73.关节角速度1003是图1所示的轨道点p1～pn各自的关节j绕旋转轴314的角速度。关节角加速度1004是图1所示的轨道点p1～pn各自的关节j绕旋转轴314的角加速度。
74.可否平滑化1005是表示可否进行轨道点p的平滑化的信息。平滑化是使机器臂101的轨道平滑的处理。“假：false”表示该轨道点p不可平滑化，“真：true”表示该轨道点p能够平滑化。若初始值为“真”，执行平滑化，则更新为“假”(步骤s1810)。但是，关于起点p1和终点pn，可否平滑化1005的初始值为“假”。
75.控制时刻1006是机器臂101被控制到轨道点p的时间。将起点p1的控制时刻1006设为“0”，其他轨道点p的控制时刻1006成为从起点p1的控制时刻1006起的经过时间。另外，控制时刻1006也可以是从前1个轨道点p起的经过时间。控制时刻1006在后述的图13以及图20的控制时刻1006赋予处理(步骤s1304)中被赋予。
76.图11是表示末端搜索数据219的一例的说明图。末端搜索数据219是暂时存储在存储设备202的缓冲存储器中并且作为日志而保存的数据。末端搜索数据219是用于搜索轨道点的数据。
77.末端搜索数据219具有搜索点id1101、关节角度1102、末端位置1103、差分姿势1104、前搜索点id1105、成本1106以及搜索完成标志1107作为字段。
78.搜索点id1101是唯一地确定搜索点的识别信息。搜索点是表示前端部102的候选位置以及候选姿势的前端部102的候选点。除了初始的搜索点(搜索点id：d001)之外，生成多个候选点。对候选点赋予副编号。在图11的(a)中，作为从初始的搜索点(搜索点id：d001)的末端位置1103起的移动目的地候选，生成了多个搜索点id：d002-1、d002-2、d002-3的条目。
79.关节角度1102是成为前端部102的候选姿势的各关节j的绕旋转轴314的角度。末端位置1103是成为前端部102的候选位置的全局坐标系110的3维位置。基于关节角度1102通过正运动学来决定末端位置1103。此外，末端位置1103的姿势成为与前端部102连接的关
节jm的姿势(翻滚角、俯仰角、偏航角)。
80.差分姿势1104是当前的末端姿势相对于基于球面插补函数slerp的球面插补姿势的差分(δr
current
、δp
current
、δy
current
)。具体而言，例如，差分姿势1104是对前1个搜索点的差分姿势1104加上位移量δr、δp、δy而得到的值。关于初始的搜索点(搜索点id：d001)的差分姿势1104，由于不存在前1个搜索点，所以成为(0，0，0)。位移量δr、δp、δy都是1个步骤中的最大位移804，能够分别相加到翻滚角最大位移801、俯仰角最大位移802、偏航角最大位移803。
81.前搜索点id1105是前1个搜索点id1101。关于初始的搜索点(搜索点id1101：d001)，由于不存在前1个搜索点，所以前搜索点id1105为“null”。成本1106是基于由搜索点id1101确定的搜索点与目标位置之间的距离的值。在本例中，该距离越长，则成本1106越高。初始的搜索点(搜索点id1101：d001)的成本1106被设定为“0”。
82.在图11的(a)中，作为从初始的搜索点(搜索点id1101：d001)的末端位置1103起的移动目的地候选，生成了多个搜索点id1101：d002-1、d002-2、d002-3的条目。其中，成本1106最小的条目是搜索点id1101：d002-2。因此，如(b)所示，搜索点id1101：d002-2确定为从初始的搜索点(搜索点id1101：d001)的末端位置1103起的移动目的地，搜索点id1101：d002-1、d002-3的条目被删除。
83.搜索完成标志1107是表示该条目的搜索点是否搜索完成的标志。如果搜索完成则为“真”，如果没有搜索完成则为“假”。搜索完成标志1107的初始值是“假”。在该条目的搜索点确定为前1个所确定的搜索点的前端部102的移动目的地的情况下，搜索完成标志1107从“假”更新为“真”。未更新的条目被删除。
84.在图11的例子中，搜索点id1101：d002-2确定为从初始的搜索点(搜索点id1101：d001)的末端位置1103起的移动目的地，因此搜索完成标志1107从“假”更新为“真”。然后，删除搜索完成标志1107没有从“假”更新为“真”的搜索点id1101：d002-1、d002-3的条目。
85.《轨道计划生成装置100的功能性结构例》
86.图12是表示轨道计划生成装置100的功能性结构例的框图。轨道计划生成装置100具有取得部1201、搜索部1202、平滑化部1203、赋予部1204以及输出部1205。具体而言，取得部1201、搜索部1202、平滑化部1203、赋予部1204以及输出部1205例如是通过使处理器201执行存储于存储设备202的程序而实现的功能。
87.《轨道计划生成处理步骤例》
88.图13是表示由轨道计划生成装置100进行的轨道计划生成处理步骤例的流程图。取得部1201从存储设备202取得搜索部1202、平滑化部1203以及赋予部1204所需的数据，并输出到搜索部1202、平滑化部1203以及赋予部1204(步骤s1301)。
89.接着，搜索部1202执行搜索到目标位置为止的搜索点的搜索处理(步骤s1302)。具体地，例如，搜索部1202通过a-star方法计算出从作为机器臂101的开始关节角度501(θ1)的轨道点p1到前端部102的目标位置姿势pn为止的轨道数据t。对于搜索处理(步骤s1302)的详细内容，在图14中后述。
90.接着，平滑化部1203执行对轨道数据t进行平滑化的平滑化处理(步骤s1303)。对于平滑化处理(步骤s1303)的详细内容，在图18中后述。接着，赋予部1204考虑前端部102的姿势变化，执行对轨道点p赋予控制时刻1006的赋予处理(步骤s1304)。对于赋予处理(步骤
s1304)的详细内容，在图20中后述。
91.接着，如图1所示，输出部1205生成能够再生轨道数据t的轨道计划数据120，并从输出设备204或通信if205输出。
92.《搜索处理(步骤s1302))》
93.图14是表示图13所示的搜索部1202的搜索处理(步骤s1302)的详细处理步骤例的流程图。搜索部1202通过取得部1201从开始关节角度db213取得开始关节角度501(θ1)，通过取得部1201从目标位置姿势db214取得目标位置姿势pn。然后，搜索部1202对作为末端搜索数据219的搜索点id1101的d001进行编号，并且将所取得的开始关节角度501(θ1)存储在d001的条目的关节角度1102中(步骤s1401)。
94.接着，搜索部1202执行步骤s1403～s1408的循环处理(步骤s1402)。具体而言，例如，搜索部1202从末端搜索数据219中选择未搜索(搜索完成标志1107为假)且成本1106最小的搜索点，决定轨道点p
current
(步骤s1403)。接着，搜索部1202判断轨道点p
current
是否接近目标位置姿势pn(步骤s1404)。具体而言，例如，搜索部1202判断轨道点p
current
是否位于以目标位置姿势pn为中心的半径r的球1501(参照图15)内。
95.在较远的情况下，即不位于半径r的球内的情况下(步骤s1404：否)，搜索部1202判断步骤s1402的循环次数是奇数还是偶数(步骤s1405)。步骤s1405是用于交替地执行步骤s1406和s1407的处理。
96.在循环次数为奇数的情况下(步骤s1405：奇数)，基于末端位置候选计算方法db215的末端位置候选计算方法701，计算出多个从当前的前端部102的轨道点p
current
起的成为移动目的地的下一个轨道点候选p
next
的搜索点(步骤s1406)，转移至步骤s1408。由此，能够使末端位置平滑地变化。具体而言，例如，当前的前端部102的轨道点p
current
是在步骤s1403中从在前1个循环的步骤s1407中得到的多个搜索点中选择出的搜索点。
97.但是，在循环次数为第一次的情况下，当前的前端部102的轨道点是轨道点p1。搜索部1202发布多个搜索点id1101，在搜索点id1101的各条目的末端位置1103存储成为轨道点候选p
next
的前端部102的3维位置，另外，还存储前搜索点id1105。
98.在此，为了便于说明，将构成下一个轨道点候选p
next
的信息中的位置信息设为o，将姿势信息设为q。若将当前的末端位置1103设为o
current
，将与球面插补姿势的差分姿势1104设为δq
current
，则通过下述式(1)～(3)，按每个搜索点计算出构成下一个轨道点候选p
next
的下一个位置信息o
next
。
99.[数式1]
[0100]onext
＝o
current
十δo
…
(1)
[0101][0102]qnext
＝slerp(q1、qn、t
next
)δq
current
…
(3)
[0103]
上述式(1)的δo是按照末端位置候选计算方法701生成的移动量，例如，在指定轴并行(axis parallel)作为末端位置候选计算方法701的情况下，若将最大移动距离702设为d，则δo如以下的6个那样成为前端部102的移动方向矢量。这样，作为下一个位置信息o
next
，得到6个位置信息oi1～oi6。
[0104]
(
±
d，0，0)
t
[0105]
(0，
±
d，0)
t
[0106]
(0，0，
±
d)
t
[0107]
另外，上述式(3)的slerp意味着球面插补函数。使用图15说明步骤s1406。
[0108]
《基于末端位置候选计算方法701的轨道点候选p
next
的计算例》
[0109]
图15是表示基于步骤s1406的末端位置候选计算方法701的轨道点候选p
next
的计算例的说明图。在图15中，五边形的图形表示前端部102，突出的顶点1500与物体103的保持面对应。在图15中，示出了按照轨道点p1、p2、p3、p4、p5、p6、
…
、pi的顺序生成并通过步骤s1406生成了搜索点pi1～pi6作为下一个轨道点候选p
next
的状态。搜索点pi1～pi6的位置由上述的位置信息oi1～oi6规定。
[0110]
搜索部1202通过上述式(2)计算ti1～ti6，作为从搜索点pi1～pi6向连接了轨道点p1和目标位置姿势pn的矢量1502的正投影的长度。搜索部1202使用该ti1～ti6，通过上述式(3)的球面插补函数slerp，计算出各搜索点pi1～pi6处的姿势信息qi1～qi6。这样，得到搜索点pi1＝(oi1，qi1)～pi6＝(oi6，qi6)。通过使用上述式(1)～(3)，计算出能够使前端部102的姿势变化最小且在空间上移动的前端部102的轨道点候选p
next
。
[0111]
返回到图14。另一方面，在循环次数为偶数的情况下(步骤s1405：奇数)，基于末端姿势变动候选计算方法db216，计算出多个成为当前的前端部102的轨道点p
current
处的位移目的地的下一个轨道点候选p
next
(步骤s1407)，并转移至步骤s1408。由此，能够使末端姿势平滑地变化。具体而言，例如，当前的前端部102的轨道点p
current
是在步骤s1403中从在前1个循环的步骤s1406中得到的多个搜索点中选择出的搜索点。搜索部1202发布多个搜索点id1101，在搜索点id1101的各条目的末端位置1103存储成为轨道点候选p
next
的前端部102的3维位置，还存储前搜索点id1105。
[0112]
具体而言，例如，搜索部1202使用下述式(4)～(10)来计算出步骤s1406的轨道点候选p
next
。
[0113]
[数式2]
[0114][0115]
δq
next
＝r
x
(δr
next
)ry(δp
next
)rz(δy
next
)
…
(7)
[0116][0117]qnext
＝slerp(q1、qn、t
next
)δq
next
…
(9)
[0118]onext
＝o
current
…
(10)
[0119]
上述式(4)～(7)的δr
next
、δp
next
、δy
next
表示翻滚角r、俯仰角p、偏航角y的位移量，是末端姿势变动候选计算方法db216的1个步骤中的最大位移804。其中，δr
next
、δp
next
、δy
next
在每次生成轨道点p时被累积相加，因此被设定为不超过翻滚角最大位移801、俯仰角最大位移802以及偏航角最大位移803。上述式(1)～(3)的右边的δr
current
、δp
current
以及
δy
current
是相对于球面插补姿势的差分姿态1104。
[0120]
另外，上述式(7)是计算姿势位移量δq
next
的式子。上述式(7)的右边的rx、ry以及rz分别意味着绕关节jn的xn轴、yn轴、zn轴的旋转。搜索部1202根据这些值来计算出相对于基准姿势的姿势位移量δq
next
，并且基于姿势位移量δq
next
来计算出下一姿势信息q
next
。另外，此时，轨道点候选p
next
的位置不变化。使用图16对该候选计算处理进行说明。
[0121]
《基于末端姿势变动候选计算方法的轨道点候选p
next
的计算例》
[0122]
图16是表示步骤s1407的基于末端位置候选计算方法计算轨道点候选p
next
的示例的说明图。与图15同样地，五边形的图形表示前端部102，突出的顶点1500与物体103的保持面对应。
[0123]
在图16中，表示按照轨道点p1、p2、p3、p4、p5、p6、
…
、pj的顺序生成，作为下一个轨道点候选p
next
，通过步骤s1407生成了搜索点pj1～pj2的状态。此外，轨道点pj是在1个步骤前的步骤s1406中生成的搜索点pi1～pi6中成为成本1106最小的搜索点的轨道点候选p
next
。
[0124]
搜索点pj1～pj2的位置信息oj1～oj2与轨道点pj的位置信息相同(参照上述式(10))，成为下一个轨道点候选p
next
的搜索点pj1～pj2的姿势信息qi1～qi2是上述式(9)的姿势信息q
next
。根据上述式(4)～(7)，搜索部1202能够计算出不使前端部102的位置变化而仅使姿势变化的候选。通过该姿势探索，不仅能够通过球面插补使姿势变化，还能够探索允许从该位置的许可的姿势。
[0125]
返回到图14，在步骤s1406或s1407之后，搜索部1202执行关节角度计算处理(步骤s1408)。关节角度计算处理(步骤s1408)是计算成为前端部102的轨道点候选p
next
那样的机器臂101的关节角度θ
next
的处理。关节角度计算处理(步骤s1408)的详细内容在图17中后述。在通过关节角度计算处理(步骤s1408)计算出关节角度θ
next
后，通过循环处理(步骤s1402)返回到步骤s1403。
[0126]
《关节角度计算处理(步骤s1408))》
[0127]
图17是表示图14所示的关节角度计算处理(步骤s1408)的详细的处理步骤例的流程图。搜索部1202执行在步骤s1406或s1407中得到的轨道点候选量的循环处理(步骤s1701)。具体而言，例如，搜索部1202通过逆运动学来计算出成为轨道点候选p
next
的关节角度θ
next
(步骤s1702)。接下来，搜索部1202确定由轨道点候选p
next
和关节角度θ
next
指定的机器臂101是否干涉障碍物结构db212的障碍物104(步骤s1703)。
[0128]
在不干涉的情况下(步骤s1703：否)，搜索部1202在末端搜索数据219的轨道点候选p
next
的条目中，对关节角度1102追加关节角度θ
next
，对前搜索点id1105追加当前的搜索点id1101，对差分姿势1104追加(δr
next
，δp
next
，δy
next
)(步骤s1704)。然后，搜索部1202计算出轨道点候选p
next
的成本1106(cost
next
)(步骤s1705)。通过下述式(11)计算出轨道点候选p
next
的成本1106。
[0129]
cost
next
＝cost
current
distance(p
current
，p
next
)...(11)
[0130]
distance(a，b)是计算地点a、b间的欧几里得距离的距离函数。根据上述式(11)，在步骤s1406中得到了多个轨道点候选p
next
的情况下，计算出轨道点p
current
的位置与轨道点候选p
next
的位置之间的距离。另外，轨道点p
current
的位置及轨道点候选p
next
的位置分别可以是前端部102的位置，也可以是前端部102及关节j1～jn的位置。
[0131]
另外，在步骤s1407中得到了多个轨道点候选p
next
的情况下，计算出轨道点p
current
的姿势与轨道点候选p
next
的姿势之间的差分作为距离。另外，轨道点p
current
的姿势和轨道点候选p
next
的姿势分别可以是前端部102的姿势，也可以是前端部102和关节j1～jn的姿势。
[0132]
轨道点p
current
与轨道点候选p
next
之间的距离越短，则成本1106越小，越容易在步骤s1403中被选择。然后，搜索部1202将搜索完成标志1107从“假”更新为“真”(步骤s1706)。然后，转移到步骤s1708。
[0133]
另一方面，在步骤s1703中，在发生干涉的情况下(步骤s1703：是)，搜索部1202从末端搜索数据219中删除该轨道点候选p
next
的条目(步骤s1707)，转移到步骤s1708。在步骤s1708中，如果存在未选择的轨道点候选p
next
，则搜索部1202返回到步骤s1702，如果不存在未选择的轨道点候选p
next
，则返回到步骤s1403。
[0134]
《平滑化处理(步骤s1303))》
[0135]
图18是表示图13所示的平滑化部1203的平滑化处理(步骤s1303)的详细的处理步骤例的流程图。平滑化部1203执行步骤s1802～s1812的循环处理，直到轨道数据t内的各轨道点p1～pn的平滑化结束为止。即，在步骤s1813中，如果存在未被平滑化的轨道点p，则返回到步骤s1802，如果不存在未被平滑化的轨道点p，则转移到赋予处理(步骤s1304)。
[0136]
平滑化部1203得到被轨道数据t内的不可平滑化点夹着的可平滑化点的集合(步骤s1802)。不可平滑化点是指可否平滑化1005为“假”的轨道点p。在初始状态下，可否平滑化1005为“假”的轨道点p是轨道点p1、pn。可平滑化点是指可否平滑化1005为“真”的轨道点p。在初始状态下，轨道数据t内的被不可平滑化点夹着的可否平滑化1005为“真”的轨道点p是轨道点p2～pn-1。
[0137]
平滑化部1203针对可平滑化点的集合执行步骤s1804至s1811的循环处理(步骤s1803)。即，在步骤s1812中，如果在步骤s1802中有未取得的集合，则返回到步骤s1804，如果没有未取得的集合，则返回到步骤s1802。
[0138]
平滑化部1203计算出关节角度空间1900上的插补曲线(步骤s1804)。关节角度空间1900是指由关节j1～jn伸展且存在各轨道点的关节角度1002的矢量空间。将处于可平滑化点的集合的前后的2个不可平滑化点的关节角度1002分别设为θa、θb。根据该关节角度θa、θb和其前后的关节角度θ
a-1
、θ
b 1
的值，计算出满足以下制约的插补方程式f(t)(也称为插补曲线)。
[0139]
f(0)＝θa...(12)
[0140]
f(1)＝θb...(13)
[0141]f′
(0)＝θa﹣θ
a-1
...(14)
[0142]f′
(1)＝θ
b 1
﹣θb...(15)
[0143]f″
(0)＝f
″
(1)＝0...(16)
[0144]
上述式(12)～(16)是将区间[θ
a-1
、θa]、[θb、θ
b-1
]设为直线时的插补曲线中的制约。该插补曲线的求出方法是一个例子，例如，也可以是通过与全部的不可平滑化的轨道点对应的关节角度1002的3次样条插补曲线中的、相当于该区间的曲线。
[0145]
平滑化部1203在计算插补曲线后，按照集合内的可平滑化点的数量，执行步骤s1806～s1810的循环处理(步骤s1805)。具体而言，例如，平滑化部1203针对关节角度θi计算出平滑化后的θi
′
(步骤s1806)。平滑化部1203在关节角度θi为θi
′
的情况下，判断机器臂
101是否与障碍物104产生干涉(步骤s1808)。
[0146]
在不干涉的情况下(步骤s1808：否)，将轨道数据t内的关节角度θi更新为θi
′
(步骤s1809)，并转移至步骤s1811。在步骤s1811中，如果在集合中存在未选择的可平滑点，则平滑化部1203返回到步骤s1806，如果不存在未选择的可平滑化点，则转移到步骤s1812。
[0147]
另一方面，在发生干涉的情况下(步骤s1808：是)，平滑化部1203将轨道数据t内的关节角度θi的可否平滑化1005更新为“不可”(步骤s1810)，并转移至步骤s1812。在步骤s1812中，如果存在未选择的可平滑化点的集合，则平滑化部1203转移到步骤s1803，选择未选择的可平滑化点的集合，如果不存在未选择的可平滑化点的集合，则转移到步骤s1813。
[0148]
图19是表示步骤s1804中的插补曲线的计算例的说明图。图19表示对区间[θa，θb]进行插补的例子。θa是某个轨道点pa的关节j的关节角度1102。θ
a-1
是某个轨道点p
a-1
的关节j的关节角度1102。轨道点p
a-1
是在时间上在起点p1以后且轨道点pa以前出现的轨道点。轨道点p
a-1
也可以是起点p1。θb是某个轨道点pb的关节j的关节角度1102。θ
b 1
是某个轨道点p
b 1
的关节j的关节角度1102。轨道点pb是在时间上在轨道点pa之后出现的轨道点。轨道点p
b 1
是在时间上在轨道点pb之后且终点pn以前出现的轨道点。轨道点p
b 1
也可以是起点终点pn。插补曲线1901存在于关节角度空间1900中。关节角度空间1900是以关节j1～jn为坐标轴的矢量空间。
[0149]
区间[θa，θb]的实线是在平滑化前将区间[θa，θb]的轨道点中的关节角度θi连接的插补对象。区间[θa，θb]的点划线是将轨道数据t内的关节角度θi更新为θi
′
时的插补曲线1901。区间[θa，θb]的虚线是在步骤s1804中计算出的插补曲线1901。这样，平滑化部1203根据前后的关节角度θ
a-1
、θ
b 1
求出插补方程式，朝向插补方程式上的点一点一点地变更关节角度1002，由此实现轨道数据t的平滑化。由此，机器臂101通过平滑的轨道数据t使物体103稳定地移动。
[0150]
《赋予处理(步骤s1304))》
[0151]
图20是表示图13所示的赋予部1204的赋予处理(步骤s1304)的详细的处理步骤例的流程图。赋予部1204取得平滑化处理(步骤s1303)后的轨道数据t(步骤s2001)。接着，赋予部1204基于存储于关节信息db301的各关节j的最大速度317，对所有的轨道点p1～pn赋予时刻δti，并写入到作为图10的条目的轨道点数据的控制时刻1006。通过下述式(17)计算出该时刻δti。
[0152]
[数式3]
[0153][0154]
在上述式(17)中，k表示关节编号，i表示轨道点id1001。另外，vmaxk表示关节jk的最大速度317。
[0155]
接着，赋予部1204执行步骤s2003～s2007的循环处理，直到控制时刻1006的更新消失为止(步骤s2003)。在步骤s2008中，在控制时刻1006的更新消失的情况下，转移到步骤s1305。
[0156]
接着，赋予部1204对所有的轨道点p执行步骤s2005～s2006的循环处理(步骤s2004)。在步骤s2007中，如果存在未选择的轨道点p，则返回到步骤s2005，如果不存在未选
择的轨道点p，则转移到步骤s2008。
[0157]
赋予部1204判断在当前的控制时刻1006是否满足角加速度制约、前端部102的速度制约以及加速度制约(步骤s2005)。分别通过下述式(18)～(21)计算出轨道点pi的关节的角速度θi
′
、角加速度θi
″
、前端部102的速度vi、前端部102的加速度ai。
[0158]
[数式4]
[0159][0160][0161][0162][0163]
此外，在上述式(18)～(21)中，j表示雅可比矩阵。通过上述式(18)～(21)，能够计算出当前的控制时刻1006的角加速度、前端部102的速度、前端部102的加速度这样的参数。如果计算出的这些参数全部为对应的上限值(关节j的最大速度317、最大加速度318、末端最大速度902、末端最大加速度903)以下，则赋予部1204判定为满足制约(步骤s2005：是)，并转移到步骤s2007，在只要1个不满足的情况下，判定为不满足制约(步骤s2005：否)，并转移到步骤s2006。在步骤s2006中，赋予部1204对当前的控制时刻1006加上时刻δti(步骤s2006)，并转移到步骤s2007。
[0164]
通过该赋予处理(步骤s1304)，各轨道点间的动作成为满足制约且控制时间内的动作。因此，能够实现轨道计划的再现性的提高。
[0165]
如以上说明的那样，根据轨道计划生成装置100，能够生成通过轨道点p且平滑地连接前端部102的姿势、机器臂101容易动作的轨道数据t。由此，能够生成稳定地保持物体103、抑制对障碍物的缓冲、高速地动作的机器臂101的轨道数据t的计划。
[0166]
另外，上述的实施例所涉及的轨道计划生成装置100也能够如下述(1)～(13)那样构成。
[0167]
(1)例如，轨道计划生成装置100执行如下处理：第一搜索处理(步骤s1406)，搜索从第一轨道点p
current
起的的预定距离(1个步骤的最大移动距离702)内的成为前端部102的移动目的地的多个位置候选，其中，所述第一轨道点p
current
表示从起点p1到终点(目标位置姿势)pn之间的前端部102的位置以及姿势；第二搜索处理(步骤s1407)，搜索通过球面插补在允许范围(1个步骤的最大位移804)内变化的前端部102的多个姿势候选，其中，球面插补基于起点p1以及终点pn处的前端部102的姿势；决定处理(步骤s1403)，基于通过第一搜索处理搜索到的多个位置候选以及通过第二搜索处理搜索到的多个姿势候选，决定表示从第一轨道点p
current
起的前端部102的移动目的地的位置以及姿势的第二轨道点p
next
；以及输出处理(步骤s1305)，输出包含第一轨道点p
current
以及通过决定处理决定出的第二轨道点p
next
的轨道数据t。
[0168]
由此，能够制定实现由前端部102保持的物体103的稳定化的计划。
[0169]
(2)另外，在上述(1)中，轨道计划生成装置100针对多个位置候选的每一个，执行
计算与距第一轨道点p
current
的距离相关的成本的计算处理(步骤s1705)，在决定处理(步骤s1403)中，轨道计划生成装置100基于通过计算处理计算出的多个成本，决定第二轨道点p
next
的位置。
[0170]
由此，能够将由前端部102保持的物体103稳定化的适当的位置候选决定为第二轨道点p
next
的位置。
[0171]
(3)另外，在上述(2)中，在决定处理(步骤s1403)中，轨道计划生成装置100将成为多个成本中的最小成本的计算源的位置候选决定为第二轨道点p
next
的位置。
[0172]
由此，能够将前端部102的移动距离最小的位置候选决定为第二轨道点p
next
的位置，能够制定实现由前端部102保持的物体103的稳定化的计划。
[0173]
(4)另外，在上述(1)中，轨道计划生成装置100针对多个姿势候选的每一个，执行计算与相对于第一轨道点p
current
的姿势变化相关的成本的计算处理(步骤s1705)，在决定处理(步骤s1403)中，轨道计划生成装置100基于通过计算处理计算出的多个成本，决定第二轨道点p
next
的姿势。
[0174]
由此，能够将由前端部102保持的物体103稳定化的适当的姿势候选决定为第二轨道点p
next
的姿势。
[0175]
(5)另外，在上述(4)中，在决定处理(步骤s1403)中，轨道计划生成装置100将成为多个成本中的最小成本的计算源的姿势候选决定为第二轨道点p
next
的姿势。
[0176]
由此，能够将前端部102的姿势变化最小的位置候选决定为第二轨道点p
next
的姿势，能够制定实现由前端部102保持的物体103的稳定化的计划。
[0177]
(6)另外，在上述(1)中，轨道计划生成装置100执行如下处理：计算处理(步骤s1702)，针对多个位置候选的每一个，计算多个关节j的旋转角；以及判定处理(步骤s1703)，基于配置于机器臂101所存在的空间的障碍物104的构造数据(障碍物结构db212)，判定在基于计算处理的计算结果的机器臂101的多个姿势的各姿势下机器臂101是否与障碍物104干涉，在决定处理(步骤s1403)中，轨道计划生成装置100基于判定处理的判定结果，决定第二轨道点p
next
。
[0178]
由此，能够以不与障碍物104干涉的方式决定机器臂101的形状，能够抑制物体103因与障碍物104的干涉而从前端部102脱落。
[0179]
(7)另外，在上述(1)中，轨道计划生成装置100执行使第一轨道点与第二轨道点之间的轨道平滑化的平滑化处理(步骤s1303)，在输出处理(步骤s1305)中，轨道计划生成装置100输出基于平滑化处理的平滑化后的轨道数据t。
[0180]
由此，即使机器臂101可动，也能够制定实现由前端部102保持的物体103的稳定化的计划。
[0181]
(8)另外，在上述(1)中，在平滑化处理(步骤s1303)中，轨道计划生成装置100基于预定的制约来生成将起点p以后且第一轨道点p
current
之前的第三轨道点处的所述多个关节j的关节角度θa与第二轨道点p
next
之后且所述终点pn以前的第四轨道点处的所述多个关节j的关节角度θb之间连接的插补曲线1901，更新第一轨道点p
current
处的多个关节j的关节角度和第二轨道点p
next
处的多个关节j的关节角度，以便接近插补曲线1901(s1806、图19)。
[0182]
能够确定使保持物体103的前端部102的移动轨道稳定化的机器臂101的形状。
[0183]
(9)另外，在上述(8)中，预定的制约是第三轨道点处的多个关节的角速度与第四
轨道点处的多个关节的角速度之差在允许范围内(式(14)、(15))。
[0184]
(10)另外，在上述(8)中，预定的制约是第三轨道点处的多个关节的角加速度与第四轨道点处的多个关节的角加速度之差在容许范围内(式(16))。
[0185]
由此，能够抑制物体103因机器臂101的可动而从前端部102脱落。
[0186]
(11)另外，在上述(8)中，在平滑化处理(步骤s1303)中，轨道计划生成装置100基于配置于机器臂101所存在的空间的障碍物104的构造数据(障碍物结构db212)，判定在基于第一轨道点p
current
处的更新后的多个关节的关节角度和第二轨道点p
next
处的更新后的多个关节的关节角度的机器臂101的姿势下，机器臂101是否与障碍物104干涉，在判定为机器臂101不与障碍物104干涉的情况下，根据第一轨道点p
current
处的更新后的多个关节的关节角度和第二轨道点p
next
处的更新后的多个关节的关节角度，来更新第一轨道点p
current
以及第二轨道点p
next
(s1809)。
[0187]
由此，能够以不与障碍物104干涉的方式决定机器臂101的移动轨道，能够抑制物体103因与障碍物104的干涉而从前端部102脱落。
[0188]
(12)另外，在上述(1)中，轨道计划生成装置100执行赋予处理(步骤s1304)，该赋予处理基于关节j的速度制约，赋予机器臂101以成为从第一轨道点p
current
向第二轨道点p
next
的轨道的方式动作的控制时间。
[0189]
由此，能够确认使由前端部102保持的物体103稳定化的机器臂101的动作时间。
[0190]
(13)另外，在上述(12)中，在赋予处理(步骤s1304)中，轨道计划生成装置100在不满足速度制约的情况下，对从第一轨道点p
current
向第二轨道点p
next
的轨道赋予追加时间。
[0191]
由此，能够赋予与机器臂101的动作对应的时间，能够实现机器臂101的动作的再现性的提高。
[0192]
此外，本发明并不限定于上述的实施例，包含所附的技术方案的主旨内的各种变形例以及同等的结构。例如，上述的实施例是为了容易理解地说明本发明而详细说明的，本发明并不限定于必须具备所说明的全部结构。另外，也可以将某实施例的结构的一部分置换为其他实施例的结构。另外，也可以在某实施例的结构中添加其他实施例的结构。另外，也可以对各实施例的结构的一部分进行其他结构的追加、删除或置换。
[0193]
另外，上述的各结构、功能、处理部、处理单元等的一部分或全部例如可以通过集成电路设计等而由硬件实现，也可以通过处理器201解释并执行实现各个功能的程序而由软件实现。
[0194]
实现各功能的程序、表、文件等信息能够保存在存储器、硬盘、ssd(solid state drive：固态硬盘)等存储装置、或者ic(integrated circuit：集成电路)卡、sd卡、dvd(digital versatile disc：数字多功能光盘)的记录介质中。
[0195]
另外，控制线、信息线示出了认为说明上需要的部分，不限于示出安装上需要的全部控制线、信息线。实际上，可以认为几乎所有的结构都相互连接。