JP7405730B2 - Trajectory planning device, trajectory planning method, and trajectory planning program - Google Patents

Trajectory planning device, trajectory planning method, and trajectory planning program Download PDF

Info

Publication number
JP7405730B2
JP7405730B2 JP2020195708A JP2020195708A JP7405730B2 JP 7405730 B2 JP7405730 B2 JP 7405730B2 JP 2020195708 A JP2020195708 A JP 2020195708A JP 2020195708 A JP2020195708 A JP 2020195708A JP 7405730 B2 JP7405730 B2 JP 7405730B2
Authority
JP
Japan
Prior art keywords
trajectory
information
route
trajectory planning
time
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
Application number
JP2020195708A
Other languages
Japanese (ja)
Other versions
JP2022084092A (en
Inventor
雄太 山内
信昭 中須
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hitachi Ltd
Original Assignee
Hitachi Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hitachi Ltd filed Critical Hitachi Ltd
Priority to JP2020195708A priority Critical patent/JP7405730B2/en
Priority to PCT/JP2021/037231 priority patent/WO2022113538A1/en
Publication of JP2022084092A publication Critical patent/JP2022084092A/en
Application granted granted Critical
Publication of JP7405730B2 publication Critical patent/JP7405730B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/4093—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by part programming, e.g. entry of geometrical information as taken from a technical drawing, combining this with machining and material information to obtain control information, named part programme, for the NC machine
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/42—Recording and playback systems, i.e. in which the programme is recorded from a cycle of operations, e.g. the cycle of operations being manually controlled, after which this record is played back on the same machine

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Geometry (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Manipulator (AREA)

Description

本発明は軌道計画装置、軌道計画方法、及び軌道計画プログラムに関する。 The present invention relates to a trajectory planning device, a trajectory planning method, and a trajectory planning program.

本技術分野の背景技術として、特開２００５－１０５５号公報（特許文献１）、及び特開２０１９－１４１９３９号公報（特許文献２）がある。 As background art in this technical field, there are Japanese Patent Application Publication No. 2005-1055 (Patent Document 1) and Japanese Patent Application Publication No. 2019-141939 (Patent Document 2).

特許文献１には、「ロボットアーム先端に装着されたハンドで容器を把持し、容器内に収容された液体、粉体等を搬送する。容器の姿勢は、搬送開始時には重力加速度ベクトルと同方向（真下）を向いているが、加速過程で加速度が増加するにつれて慣性加速度ベクトルが徐々に大きくなり、加速度の向きと逆方向に傾斜が増し、加速度が減少に転ずると慣性加速度ベクトルが徐々に小さくなり、その傾斜は減少する。加速過程が完了して速度一定となると傾斜は解消される。減速過程では、減速度が増すにつれて、加速過程とは逆方向に傾斜が増し、減速度が減少に転ずるとその傾斜は減少し、ロボットが停止すると傾斜は解消され、搬送開始時の真下方向を向く姿勢に戻る。」と記載されている。 Patent Document 1 states, ``The hand attached to the tip of the robot arm grips the container and transports the liquid, powder, etc. contained in the container.At the start of transport, the attitude of the container is in the same direction as the gravitational acceleration vector. However, as the acceleration increases during the acceleration process, the inertial acceleration vector gradually increases, and the inertial acceleration vector gradually increases in the direction opposite to the direction of acceleration, and as the acceleration starts to decrease, the inertial acceleration vector gradually decreases. The slope decreases.When the acceleration process is completed and the speed becomes constant, the slope disappears.In the deceleration process, as the deceleration increases, the slope increases in the opposite direction to the acceleration process, and the deceleration decreases. When the robot falls, the inclination decreases, and when the robot stops, the inclination is eliminated and the robot returns to the position facing straight down when it started transporting.''

また、特許文献２には、「シミュレーション装置１０は、ワーク、ワークを保持した弾性を有する吸着パッド、及び吸着パッドが取り付けられたロボットアームを含むシミュレーションモデルを動作指令に基づいて加速度運動させるシミュレーションを実行した際、ロボットアームの加速度とワークの加速度との差により吸着パッドに発生する負荷モーメントを導出する負荷導出部１０５と、負荷導出部１０５で導出した負荷モーメントが閾値より大きい場合に、負荷導出部１０５で導出される負荷モーメントが閾値以下となるように、シミュレーションの実行条件を変更するための処理を行なう実行条件変更部１０８と、を含む。」と記載されている。 Further, Patent Document 2 states, ``The simulation device 10 performs a simulation in which a simulation model including a workpiece, an elastic suction pad holding the workpiece, and a robot arm to which the suction pad is attached is acceleratedly moved based on a motion command. When executed, a load derivation unit 105 derives the load moment generated on the suction pad due to the difference between the acceleration of the robot arm and the acceleration of the workpiece, and when the load moment derived by the load derivation unit 105 is larger than a threshold value, and an execution condition changing unit 108 that performs processing to change the execution conditions of the simulation so that the load moment derived by the unit 105 is equal to or less than a threshold value.

特開２００５－１０５５号公報Japanese Patent Application Publication No. 2005-1055 特開２０１９－１４１９３９号公報Japanese Patent Application Publication No. 2019-141939

特許文献１に記載の技術は、慣性加速度ベクトルに従って把持物の姿勢を変更するが、例えば、遠心力などの慣性力が把持物にかからないように、把持物が通過する位置を変更することはできない。特許文献２に記載の技術は、シミュレーション結果に従って、吸着パッドにっ掛かるモーメントを小さくするように、加速度を調整するが、モーメントを低下させるように把持物が通る軌道を変更することはできない。 The technique described in Patent Document 1 changes the posture of the held object according to the inertial acceleration vector, but cannot change the position through which the held object passes so that, for example, inertial force such as centrifugal force is not applied to the held object. . The technique described in Patent Document 2 adjusts the acceleration so as to reduce the moment caught on the suction pad according to the simulation results, but cannot change the trajectory of the object to be held so as to reduce the moment.

つまり、特許文献１に記載の技術及びと特許文献２に記載の技術は、把持物にかかる力やモーメントを考慮していないため、把持物に与えるダメージが大きくなるおそれがある。そこで本発明の一態様は、把持物に与えるダメージを抑制しつつロボットを高速に動作させる。 In other words, the technology described in Patent Document 1 and the technique described in Patent Document 2 do not take into account the force or moment applied to the held object, and therefore, there is a risk that damage to the held object may be increased. Accordingly, one aspect of the present invention allows a robot to operate at high speed while suppressing damage to a gripped object.

上記課題を解決するために、本発明の一態様は以下の構成を採用する。可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道を計画する軌道計画装置は、プロセッサとメモリとを有し、前記メモリは、前記把持物の重量及び重心位置を示す把持物情報と、前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、前記軌道における経路の各点の位置を示す軌道情報と、を保持し、前記プロセッサは、前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を実行する。 In order to solve the above problems, one embodiment of the present invention employs the following configuration. A trajectory planning device that plans a trajectory of the movable axis of a robot arm that has a movable axis and that grasps an object has a processor and a memory, and the memory stores the weight and center of gravity of the object to be grasped. , constraint information indicating constraints on gripping force and moment applied to the grasped object, and trajectory information indicating the position of each point on the path on the trajectory, and the processor Based on the weight and the center of gravity position indicated by the information, assign a time at which the movable axis passes each point on the path indicated by the trajectory information so as to satisfy the constraints of the gripping force and the moment indicated by the constraint information. and a time assigning process of storing the assigned time in the trajectory information, and reducing the gripping force and/or moment applied to the grasped object based on the weight and the center of gravity position indicated by the grasped object information. , a route optimization process for changing the positions of points included in the route in the trajectory information.

本発明の一態様によれば、把持物に与えるダメージを抑制しつつロボットを高速に動作させることができる。 According to one aspect of the present invention, a robot can be operated at high speed while suppressing damage to a gripped object.

上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。 Problems, configurations, and effects other than those described above will be made clear by the following description of the embodiments.

実施例１における軌道計画装置の構成例を示すブロック図である。1 is a block diagram showing a configuration example of a trajectory planning device in Example 1. FIG. 実施例１におけるロボットアーム構成テーブルの一例である。2 is an example of a robot arm configuration table in Example 1. 実施例１における干渉物構成テーブルの一例である。3 is an example of an interfering object configuration table in Example 1. 実施例１における開始関節角度テーブルの一例である。3 is an example of a starting joint angle table in Example 1. 実施例１における目標位置姿勢テーブルの一例である。3 is an example of a target position and orientation table in the first embodiment. 実施例１における把持力・モーメント制約テーブルの一例である。It is an example of the grip force/moment constraint table in Example 1. 実施例１における把持物情報テーブルの一例である。3 is an example of a held object information table in the first embodiment. 実施例１における把持方法情報テーブルの一例である。3 is an example of a gripping method information table according to the first embodiment. 実施例１における軌道テーブルの一例である。It is an example of the orbit table in Example 1. 実施例１における軌道計画処理の一例を示すフローチャートである。5 is a flowchart illustrating an example of trajectory planning processing in the first embodiment. 実施例１における制御時刻付与処理の一例を示すフローチャートである。7 is a flowchart illustrating an example of control time provision processing in the first embodiment. 実施例１における把持物考慮経路最適化処理の一例を示すフローチャートである。7 is a flowchart illustrating an example of a route optimization process considering a held object in the first embodiment. 実施例１における結果出力画面の一例である。3 is an example of a result output screen in Example 1. 実施例１における把持物にかかる把持力、及び把持物にかかるモーメントの計算処理の一例を示す説明図である。FIG. 3 is an explanatory diagram showing an example of calculation processing of a gripping force applied to a gripped object and a moment applied to the gripped object in Example 1; 実施例１における軌道計画処理における軌道の変化の一例を示す説明図である。FIG. 4 is an explanatory diagram showing an example of a trajectory change in trajectory planning processing in the first embodiment.

以下、本発明の実施形態を図面に基づいて詳細に説明する。本実施形態において、同一の構成には原則として同一の符号を付け、繰り返しの説明は省略する。なお、本実施形態は本発明を実現するための一例に過ぎず、本発明の技術的範囲を限定するものではないことに注意すべきである。 Hereinafter, embodiments of the present invention will be described in detail based on the drawings. In this embodiment, in principle, the same components are given the same reference numerals, and repeated explanations will be omitted. Note that this embodiment is merely an example for implementing the present invention, and does not limit the technical scope of the present invention.

［システム構成］
図１は、軌道計画装置の構成例を示すブロック図である。軌道計画装置１００は、例えば、ＣＰＵ（Ｃｏｎｔｒｏｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１０、メモリ１２０、補助記憶装置１３０、通信装置１４０、及び入出力インターフェース１５０を有する計算機（例えば、ＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）又はサーバ等）によって構成される。 [System configuration]
FIG. 1 is a block diagram showing an example of the configuration of a trajectory planning device. The trajectory planning device 100 is configured by, for example, a computer (such as a PC (Personal Computer) or a server) having a CPU (Control Processing Unit) 110, a memory 120, an auxiliary storage device 130, a communication device 140, and an input/output interface 150. be done.

ＣＰＵ１１０は、プロセッサを含み、メモリ１２０に格納されたプログラムを実行する。メモリ１２０は、不揮発性の記憶素子であるＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）及び揮発性の記憶素子であるＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）を含む。ＲＯＭは、不変のプログラム（例えば、ＢＩＯＳ（Ｂａｓｉｃ Ｉｎｐｕｔ／Ｏｕｔｐｕｔ Ｓｙｓｔｅｍ））などを格納する。ＲＡＭは、ＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）のような高速かつ揮発性の記憶素子であり、ＣＰＵ１１０が実行するプログラム及びプログラムの実行時に使用されるデータを一時的に格納する。 CPU 110 includes a processor and executes programs stored in memory 120. The memory 120 includes a ROM (Read Only Memory) that is a nonvolatile storage element and a RAM (Random Access Memory) that is a volatile storage element. The ROM stores an unchangeable program (eg, BIOS (Basic Input/Output System)) and the like. The RAM is a high-speed and volatile storage element such as DRAM (Dynamic Random Access Memory), and temporarily stores programs executed by the CPU 110 and data used when executing the programs.

補助記憶装置１３０は、例えば、磁気記憶装置（ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ））、フラッシュメモリ（ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ））等の大容量かつ不揮発性の記憶装置であり、ＣＰＵ１１０が実行するプログラム及びプログラムの実行時に使用されるデータを格納する。すなわち、プログラムは、補助記憶装置１３０から読み出されて、メモリ１２０にロードされて、ＣＰＵ１１０によって実行される。 The auxiliary storage device 130 is a large-capacity, non-volatile storage device such as a magnetic storage device (HDD (Hard Disk Drive)), flash memory (SSD (Solid State Drive)), etc., and stores programs and programs executed by the CPU 110. Stores data used during execution. That is, the program is read from the auxiliary storage device 130, loaded into the memory 120, and executed by the CPU 110.

入出力インターフェース１５０は、オペレータからの入力を受けるインターフェースであり、プログラムの実行結果をオペレータが視認可能な形式で出力するインターフェースでもある。入出力インターフェース１５０には、キーボードやマウスなどの入力装置、及びディスプレイ装置やプリンタなどの出力装置を含む入出力装置１６０が接続される。また、入出力インターフェース１５０には、把持物情報や干渉物情報を計測する３次元センサ１７０が接続される。３次元センサ１７０は、例えば、画像センサ、レーザースキャナ、及びライダーを含む。なお、スマートフォンやタブレットＰＣ等の計算機が入出力装置１６０として機能してもよい。 The input/output interface 150 is an interface that receives input from an operator, and is also an interface that outputs program execution results in a format that can be viewed by the operator. Connected to the input/output interface 150 are input/output devices 160 including input devices such as a keyboard and mouse, and output devices such as a display device and a printer. Furthermore, a three-dimensional sensor 170 is connected to the input/output interface 150 to measure information on a held object and information on an interfering object. Three-dimensional sensor 170 includes, for example, an image sensor, a laser scanner, and a lidar. Note that a computer such as a smartphone or a tablet PC may function as the input/output device 160.

通信装置１４０は、所定のプロトコルに従って、他の装置との通信を制御するネットワークインターフェース装置である。また、通信装置１４０は、例えば、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）等のシリアルインターフェースを含んでもよい。 The communication device 140 is a network interface device that controls communication with other devices according to a predetermined protocol. Further, the communication device 140 may include, for example, a serial interface such as a USB (Universal Serial Bus).

ＣＰＵ１１０が実行するプログラムは、リムーバブルメディア（ＣＤ－ＲＯＭ、フラッシュメモリなど）又はネットワークを介して軌道計画装置１００に提供され、非一時的記憶媒体である不揮発性の補助記憶装置１３０に格納される。このため、軌道計画装置１００は、リムーバブルメディアからデータを読み込むインターフェースを有するとよい。 The program executed by the CPU 110 is provided to the trajectory planning device 100 via a removable medium (CD-ROM, flash memory, etc.) or a network, and is stored in the non-volatile auxiliary storage device 130, which is a non-temporary storage medium. For this reason, the trajectory planning device 100 preferably has an interface for reading data from removable media.

ＣＰＵ１１０は、例えば、データ読込部２０１、干渉判定部２０２、動力学計算部２０３、軌道計画部２０４、初期経路計画部２０５、把持物考慮時刻付与部２０６、把持物考慮経路最適化部２０７、及び結果出力部２０８を含む。 The CPU 110 includes, for example, a data reading section 201, an interference determination section 202, a dynamics calculation section 203, a trajectory planning section 204, an initial path planning section 205, a grasped object consideration time assigning section 206, a grasped object consideration path optimization section 207, and It includes a result output unit 208.

例えば、ＣＰＵ１１０は、メモリ１２０にロードされたデータ読込プログラムに従って動作することで、データ読込部２０１として機能し、メモリ１２０にロードされた干渉判定部プログラムに従って動作することで、干渉判定部２０２として機能する。ＣＰＵ１１０に含まれる他の機能部についても、プログラムと機能部の関係は同様である。 For example, the CPU 110 functions as the data reading section 201 by operating according to the data reading program loaded into the memory 120, and functions as the interference determining section 202 by operating according to the interference determining section program loaded into the memory 120. do. The relationship between programs and functional units is the same for other functional units included in the CPU 110 as well.

データ読込部２０１は、例えばユーザからの入力指示に従って、補助記憶装置１３０に格納された各種データを読み込む。干渉判定部２０２は、ロボットアーム及び把持物が干渉物と干渉するかを判定する。動力学計算部２０３は、把持物にかかる把持力、及び把持物にかかるモーメントを算出する。 The data reading unit 201 reads various data stored in the auxiliary storage device 130, for example, according to input instructions from the user. The interference determination unit 202 determines whether the robot arm and the object to be held interfere with an interfering object. The dynamics calculation unit 203 calculates the gripping force applied to the gripped object and the moment applied to the gripped object.

軌道計画部２０４は、ロボットアームの軌道を計画する。初期経路計画部２０５は、ロボットアームの初期経路を生成する。以下、ロボットアームが通過する経路に時刻を付与したものを軌道と呼ぶ（即ち経路は時刻を問わないロボットアームの道筋を示す概念である）。把持物考慮時刻付与部２０６は、軌道の各点における関節角速度、関節角速度、及び制御時刻を付与又は修正する。把持物考慮経路最適化部２０７は、把持物にかかる把持力、及びモーメントが小さくなるよう軌道の経路を最適化する。結果出力部２０８は、軌道計画の結果を入出力装置１６０に出力する。 The trajectory planning unit 204 plans the trajectory of the robot arm. The initial path planning unit 205 generates an initial path for the robot arm. Hereinafter, the path taken by the robot arm with a time assigned to it will be referred to as a trajectory (in other words, a path is a concept that indicates the path of the robot arm regardless of time). The grasped object consideration time assigning unit 206 assigns or corrects the joint angular velocity, joint angular velocity, and control time at each point on the trajectory. The gripped object consideration path optimization unit 207 optimizes the path of the trajectory so that the gripping force and moment applied to the gripped object are reduced. The result output unit 208 outputs the result of the trajectory planning to the input/output device 160.

軌道計画装置１００は、物理的に一つの計算機上で、又は、論理的又は物理的に構成された複数の計算機上で構成される計算機システムであり、同一の計算機上で別個のスレッドで動作してもよく、複数の物理的計算機資源上に構築された仮想計算機上で動作してもよい。 The trajectory planning device 100 is a computer system that is physically configured on one computer or on multiple logically or physically configured computers, and operates in separate threads on the same computer. It may also operate on a virtual computer built on multiple physical computer resources.

なお、図示しないが、軌道計画装置１００は、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｔｅｍ）、ミドルウェア、及びアプリケーションなどの構成を有し、特にディスプレイなどの入出力装置１６０にＧＵＩ（Ｇｒａｐｈｉｃａｌ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ）を表示するための処理機能を有する。ＣＰＵ１１０は、当該処理機能により、所定の画面を描画し表示する処理や、ユーザが入力したデータの処理などを行うことができる。 Although not shown, the trajectory planning device 100 has a configuration such as an OS (Operating System), middleware, and applications, and in particular, a process for displaying a GUI (Graphical User Interface) on an input/output device 160 such as a display. Has a function. The CPU 110 can perform processes such as drawing and displaying a predetermined screen and processing data input by a user using the processing functions.

なお、ＣＰＵ１１０に含まれる機能部による機能の一部又は全部が、例えば、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）やＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等のハードウェアによって実現されてもよい。 Note that some or all of the functions provided by the functional units included in the CPU 110 may be realized by hardware such as an ASIC (Application Specific Integrated Circuit) or an FPGA (Field-Programmable Gate Array).

補助記憶装置１３０は、例えば、ロボットアーム構成テーブル３１０、干渉物構成テーブル３２０、開始関節角度テーブル３３０、目標位置姿勢テーブル３４０、把持物・モーメント制約テーブル３５０、把持物情報テーブル３６０、把持方法情報テーブル３７０、及び軌道テーブル３８０を含む。なお、補助記憶装置１３０に格納されている一部又は全部の情報は、メモリ１２０に格納されていてもよいし、軌道計画装置１００に接続されているデータベース等に格納されていてもよい。 The auxiliary storage device 130 includes, for example, a robot arm configuration table 310, an interfering object configuration table 320, a starting joint angle table 330, a target position/posture table 340, a grasped object/moment constraint table 350, a grasped object information table 360, and a grasping method information table. 370, and a trajectory table 380. Note that some or all of the information stored in the auxiliary storage device 130 may be stored in the memory 120, or in a database connected to the trajectory planning device 100.

ロボットアーム構成テーブル３１０は、軌道計画や逆運学計算、動力学計算などで用いられるロボットアーム構成の情報を保持する。干渉物構成テーブル３２０は、軌道計画における干渉判定で用いられる、干渉物構成の情報を保持する。開始関節角度テーブル３３０は、軌道計画でのロボットアームの始点となる開始関節角度の情報を保持する。 The robot arm configuration table 310 holds information on the robot arm configuration used in trajectory planning, inverse dynamics calculations, dynamic calculations, and the like. The interfering object configuration table 320 holds information on interfering object configurations used in interference determination in trajectory planning. The starting joint angle table 330 holds information on starting joint angles that are the starting points of the robot arm in trajectory planning.

目標位置姿勢テーブル３４０は、軌道計画でのロボットアームの終点となる目標姿勢の情報を保持する。把持力・モーメント制約テーブル３５０は、軌道計画の時刻付与や最適化計算に用いられる、把持物への把持力及び把持物のモーメントに関する制約の情報を保持する。 The target position and orientation table 340 holds information on the target orientation that is the end point of the robot arm in trajectory planning. The gripping force/moment constraint table 350 holds information on constraints regarding the gripping force on the gripping object and the moment of the gripping object, which is used for time assignment and optimization calculation of trajectory planning.

把持物情報テーブル３６０は、軌道計画において、ロボットアームが把持する把持物の情報を保持する。把持方法情報テーブル３７０は、軌道計画で用いる、把持方法データ４０７を記憶する部分である。軌道テーブル３８０は、結果出力部２０８で出力する軌道データ４０８を記憶する部分である。 The grasped object information table 360 holds information on grasped objects held by the robot arm in trajectory planning. The gripping method information table 370 is a portion that stores gripping method data 407 used in trajectory planning. The trajectory table 380 is a part that stores trajectory data 408 output by the result output unit 208.

なお、本実施形態において、病院選定システムが使用する情報は、データ構造に依存せずどのようなデータ構造で表現されていてもよい。本実施形態ではテーブル形式で情報が表現されているが、例えば、リスト、データベース又はキューから適切に選択したデータ構造体が、情報を格納することができる。 In addition, in this embodiment, the information used by the hospital selection system may be expressed in any data structure without depending on the data structure. Although the information is represented in table form in this embodiment, the information may be stored in a data structure appropriately selected from, for example, a list, a database, or a queue.

［ロボットアーム構成テーブル３１０］
図２は、ロボットアーム構成テーブル３１０の一例である。ロボットアーム構成テーブル３１０は、例えば、ロボットごとに定義されている。ロボットアーム構成テーブル３１０は、例えば、項番欄３１１、分類欄３１２、項目欄３１３、及び値欄３１４を含む。項番欄３１１は、ロボットアーム構成テーブル３１０のレコードを識別する番号を保持する。分類欄３１２は、ロボットアームの構成が属する分類を示す情報を保持する。 [Robot arm configuration table 310]
FIG. 2 is an example of the robot arm configuration table 310. The robot arm configuration table 310 is defined for each robot, for example. The robot arm configuration table 310 includes, for example, an item number column 311, a classification column 312, an item column 313, and a value column 314. The item number column 311 holds a number that identifies a record in the robot arm configuration table 310. The classification column 312 holds information indicating the classification to which the configuration of the robot arm belongs.

ロボットアームの構成が属する分類は、例えば、関節情報及びリンク情報を含む。ロボットの可動軸は関節とリンクとを含む。項目欄３１３は、分類に対応する項目名を示す情報を保持する。値欄３１４は、各項目の値を示す情報を保持する。なお、関節情報は、ロボットアームに含まれる各関節の情報であり、例えば、関節名、関節の種類、関節の位置、関節の向き、関節の動作下限、関節の動作上限、最大加速度、及び最大速度などの情報を含む。 The classification to which the configuration of the robot arm belongs includes, for example, joint information and link information. The movable axis of the robot includes joints and links. The item column 313 holds information indicating the item name corresponding to the classification. The value column 314 holds information indicating the value of each item. Note that the joint information is information on each joint included in the robot arm, such as joint name, joint type, joint position, joint orientation, lower limit of joint movement, upper limit of joint movement, maximum acceleration, and maximum Contains information such as speed.

リンク情報は、ロボットアームに含まれるリンクの情報であり、例えば、リンク名、親関節名、子関節名、及びリンク形状などの情報を含む。リンク形状とは、リンクの実際の形状である。ＳＴＥＰ（Ｓｔａｎｄａｒｄ ｆｏｒ ｔｈｅ Ｅｘｃｈａｎｇｅ ｏｆ Ｐｒｏｄｕｃｔ ｍｏｄｅｌ ｄａｔａ）などのフォーマットで保存されるソリッドデータや、ＳＴＬ（ＳＴｅｒｅｏＬｉｔｈｏｇｒａｐｈｙ）などのフォーマットで保存されるポリゴンデータ等で表現されるである。 The link information is information on links included in the robot arm, and includes information such as a link name, a parent joint name, a child joint name, and a link shape. The link shape is the actual shape of the link. It is expressed as solid data stored in a format such as STEP (Standard for the Exchange of Product model data), polygon data stored in a format such as STL (STereoLithography), etc.

［干渉物構成テーブル３２０］
図３は、干渉物構成テーブル３２０の一例である。干渉物構成テーブル３２０は、例えば、干渉物ＩＤ欄３２１、干渉物形状欄３２２、及び干渉物姿勢欄３２３を含む。干渉物ＩＤ欄３２１は、干渉物を識別する干渉物ＩＤを保持する。 [Interference object configuration table 320]
FIG. 3 is an example of an interference object configuration table 320. The interference object configuration table 320 includes, for example, an interference object ID column 321, an interference object shape column 322, and an interference object posture column 323. The interfering object ID column 321 holds an interfering object ID that identifies an interfering object.

干渉物形状欄３２２は、干渉物の形状を示す情報を保持する。干渉物の形状は、例えば、ＳＴＥＰなどのフォーマットで保存されるソリッドデータや、ＳＴＬなどのフォーマットで保存されるポリゴンデータなどで表現される。干渉物姿勢欄３２３は、干渉物が空間上のどの位置に置かれているかを示す情報である。干渉物の姿勢は、例えば、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表現される。 The interference object shape column 322 holds information indicating the shape of the interference object. The shape of the interfering object is expressed, for example, by solid data stored in a format such as STEP, polygon data stored in a format such as STL, or the like. The interfering object position column 323 is information indicating where in space the interfering object is placed. The posture of the interfering object is expressed, for example, by an AFFINE transformation matrix, a position in a three-dimensional space, and the Roll-Pitch-Yaw at that time.

［開始関節角度テーブル３３０］
図４は、開始関節角度テーブル３３０の一例である。開始関節角度テーブル３３０は、例えば、関節名欄３３１及び開始関節角度欄３３２を有する。関節名欄３３１は、関節の名称（例えばロボットアーム構成テーブル３１０の関節名の項目に対応する値と同じ値が含まれる）を保持する。開始関節角度欄３３２は、関節の動作開始時の角度（即ち初期角度）を示す情報を保持する。 [Start joint angle table 330]
FIG. 4 is an example of the starting joint angle table 330. The starting joint angle table 330 has, for example, a joint name column 331 and a starting joint angle column 332. The joint name column 331 holds joint names (for example, includes the same value as the value corresponding to the joint name item in the robot arm configuration table 310). The starting joint angle field 332 holds information indicating the angle at the start of joint motion (ie, initial angle).

［目標位置姿勢テーブル３４０］
図５は、目標位置姿勢テーブル３４０の一例である。目標位置姿勢テーブル３４０は、姿勢ＩＤ欄３４１、対象リンク名欄３４２、及び位置姿勢情報欄３４３を含む。姿勢ＩＤ欄３４１は、目標位置姿勢を識別する姿勢ＩＤを保持する。 [Target position/posture table 340]
FIG. 5 is an example of the target position/orientation table 340. The target position and orientation table 340 includes an orientation ID field 341, a target link name field 342, and a position and orientation information field 343. The posture ID column 341 holds a posture ID that identifies the target position and posture.

対象リンク名欄３４２は、対象のリンクの名称（例えばロボットアーム構成テーブル３１０のリンク名の項目に対応する値と同じ値が含まれる）を保持する。また、位置姿勢情報欄３４３はリンクの目標位置が空間上のどの位置であるかを示す情報である。目標位置姿勢は、例えば、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表現される。 The target link name column 342 holds the name of the target link (for example, includes the same value as the value corresponding to the link name item of the robot arm configuration table 310). Further, the position/orientation information field 343 is information indicating where the target position of the link is in space. The target position/orientation is expressed, for example, by an AFFINE transformation matrix, a position in a three-dimensional space, and the Roll-Pitch-Yaw at that time.

［把持力・モーメント制約テーブル３５０］
図６は、把持力・モーメント制約テーブル３５０の一例である。把持力・モーメント制約テーブル３５０は、例えば、ロボットごとに定義されていてもよいし、アーム及びリンクごとに定義されていてもよいし、把持物ごとに定義されていてもよい。把持力・モーメント制約テーブル３５０は、例えば、項番欄３５１、項目欄３５２、及び値欄３５３を含む。項番欄３５１は、把持力・モーメント制約テーブル３５０のレコードを識別する番号を保持する。 [Gripping force/moment constraint table 350]
FIG. 6 is an example of the gripping force/moment constraint table 350. The gripping force/moment constraint table 350 may be defined for each robot, each arm and link, or each gripped object, for example. The gripping force/moment constraint table 350 includes, for example, an item number column 351, an item column 352, and a value column 353. The item number column 351 holds a number that identifies a record in the gripping force/moment constraint table 350.

項番欄３５１は、把持力・モーメント制約テーブル３５０のレコードを識別する番号を保持する。項目欄３５２は、把持力やモーメントに関する制約を示す項目を保持する。把持力やモーメントに関する制約は、例えば、把持物にかかる最大把持力、及び把持物にかかる最大モーメントなどを含む。最大把持力は、把持物やロボットのハンドが許容できる最大の把持力であり、例えば、３次元の情報で表現される。最大モーメントは、把持物やハンドが許容できる最大のモーメントであり、例えば、３次元の情報で表現される。 The item number column 351 holds a number that identifies a record in the gripping force/moment constraint table 350. The item column 352 holds items indicating constraints regarding gripping force and moment. The constraints regarding the gripping force and moment include, for example, the maximum gripping force applied to the gripped object, the maximum moment applied to the gripped object, and the like. The maximum gripping force is the maximum gripping force that the object to be gripped or the hand of the robot can allow, and is expressed by, for example, three-dimensional information. The maximum moment is the maximum moment that the object to be held or the hand can allow, and is expressed by, for example, three-dimensional information.

［把持物情報テーブル３６０］
図７は、把持物情報テーブル３６０の一例である。把持物情報テーブル３６０は、例えば、把持物ごとに定義されている。把持物情報テーブル３６０は、例えば、項番欄３６１、項目欄３６２、及び値欄３６３を含む。項番欄３６１は、把持物情報テーブル３６０のレコードを識別する番号を保持する。 [Holded object information table 360]
FIG. 7 is an example of the held object information table 360. The held object information table 360 is defined for each held object, for example. The held object information table 360 includes, for example, an item number column 361, an item column 362, and a value column 363. The item number column 361 holds a number that identifies a record in the held object information table 360.

項目欄３６２は、把持物に関する情報の項目を保持する。項目欄３６２に格納される項目は、例えば、把持物名、把持物形状、把持物の重心位置、及び把持物の質量等を含む。把持物形状は、例えば、ＳＴＥＰなどのフォーマットで保存されるソリッドデータや、ＳＴＬなどのフォーマットで保存されるポリゴンデータなどで表現される。重心位置は、把持物形状の重心位置を示し、例えば、３次元データで表現される。質量は、把持物の質量示し、例えば、スカラー値で表現される。 The item column 362 holds items of information regarding the held object. The items stored in the item field 362 include, for example, the name of the held object, the shape of the held object, the position of the center of gravity of the held object, and the mass of the held object. The shape of the object to be held is expressed by, for example, solid data saved in a format such as STEP, polygon data saved in a format such as STL, or the like. The center of gravity position indicates the center of gravity position of the shape of the object to be held, and is expressed by, for example, three-dimensional data. The mass indicates the mass of the object to be held, and is expressed, for example, as a scalar value.

［把持方法情報テーブル３７０］
図８は、把持方法情報テーブル３７０の一例である。把持方法情報テーブル３７０は、例えば、把持方法ごとに定義されている。把持方法情報テーブル３７０は、例えば、項番欄３７１、項目欄３７２、及び値欄３７３を含む。項番欄３７１は、把持方法情報テーブル３７０のレコードを識別する番号を保持する。 [Gripping method information table 370]
FIG. 8 is an example of the gripping method information table 370. The gripping method information table 370 is defined for each gripping method, for example. The gripping method information table 370 includes, for example, an item number column 371, an item column 372, and a value column 373. The item number column 371 holds a number that identifies a record in the gripping method information table 370.

項目欄３７２は、把持方法に関する情報の項目を保持する。項目欄３７２に格納される項目は、例えば、把持方法及び把持位置等を含む。把持方法は、ロボットがどのようにして把持物を把持するかを示し、吸着を示すＤＩＯ、及び摩擦把持を示すＦＲＩＣＴＩＯＮなどは、いずれも把持方法の一例である。把持位置とは、把持物情報テーブル４６０が示す把持物がどの位置で把持されるかを示し、例えば、３次元データで表現される。 The item column 372 holds items of information regarding the gripping method. Items stored in the item column 372 include, for example, a gripping method, a gripping position, and the like. The grasping method indicates how the robot grasps the object, and DIO indicating suction and FRICTION indicating frictional grasping are both examples of grasping methods. The gripping position indicates the position at which the gripped object indicated by the gripped object information table 460 is gripped, and is expressed by, for example, three-dimensional data.

［軌道テーブル３８０］
図９は、軌道テーブル３８０の一例である。軌道テーブル３８０は、軌道計画装置１００による処理によって出力されるテーブルである。軌道テーブル３８０は、例えば、軌道点ＩＤ欄３８１、関節角度情報欄３８２、関節角速度情報欄３８３、関節角加速度情報欄３８４、及び制御時刻欄３８５を含む。 [Trajectory table 380]
FIG. 9 is an example of the trajectory table 380. The trajectory table 380 is a table output by processing by the trajectory planning device 100. The trajectory table 380 includes, for example, a trajectory point ID column 381, a joint angle information column 382, a joint angular velocity information column 383, a joint angular acceleration information column 384, and a control time column 385.

軌道点ＩＤ欄３８１は、ロボットアーム（関節）の軌道の経路上の点を識別する軌道点ＩＤを保持する。関節角度情報欄３８２は、制御時刻欄３８５が示す時刻における各関節の角度を示す情報を保持する。図９の例では、関節角度情報欄３８２のセルには、制御時刻欄３８５が示す時刻における各関節の角度を要素とするベクトルが格納されている。関節角速度情報欄３８３は、制御時刻欄３８５が示す時刻における各関節の角速度を示す情報を保持する。図９の例では、関節角速度情報欄３８３のセルには、制御時刻欄３８５が示す時刻における各関節の角速度を要素とするベクトルが格納されている。 The trajectory point ID column 381 holds trajectory point IDs that identify points on the trajectory of the robot arm (joint). The joint angle information column 382 holds information indicating the angle of each joint at the time indicated by the control time column 385. In the example of FIG. 9, the cells of the joint angle information field 382 store vectors whose elements are the angles of each joint at the time indicated by the control time field 385. The joint angular velocity information column 383 holds information indicating the angular velocity of each joint at the time indicated by the control time column 385. In the example of FIG. 9, the cells of the joint angular velocity information column 383 store vectors whose elements are the angular velocities of each joint at the time indicated by the control time column 385.

関節角加速度情報欄３８４は、制御時刻欄３８５が示す時刻における各関節の角加速度を示す情報を保持する。図９の例では、関節角加速度情報欄３８４のセルには、制御時刻欄３８５が示す時刻における各関節の角加速度を要素とするベクトルが格納されている。制御時刻欄３８５は、把持物が各軌道点の経路を通る時刻を示す情報を保持する。 The joint angular acceleration information column 384 holds information indicating the angular acceleration of each joint at the time indicated by the control time column 385. In the example of FIG. 9, the cells of the joint angular acceleration information field 384 store vectors whose elements are the angular accelerations of each joint at the time indicated by the control time field 385. The control time column 385 holds information indicating the time at which the object to be held passes through the path of each trajectory point.

［フローチャート］
図１０は、軌道計画処理の一例を示すフローチャートである。データ読込部２０１は、軌道の計画に必要な各種データを読み込む（Ｓ１０１）。具体的には、データ読込部２０１は、ユーザによって入出力装置１６０に入力された情報に基づいて（例えば、軌道計画の対象のロボット、把持物、関節、及び／又は把持方法等が入力され、これらに対応するテーブルを読み込む）、ロボットアーム構成テーブル３１０、干渉物構成テーブル３２０、開始関節角度テーブル３３０、目標位置姿勢テーブル３４０、及び把持力・モーメント制約テーブル３５０を読み込む。なお、これらのテーブルの情報はいずれも、例えば、ユーザが入出力装置１６０に入力することによって登録される。 [flowchart]
FIG. 10 is a flowchart illustrating an example of trajectory planning processing. The data reading unit 201 reads various data necessary for trajectory planning (S101). Specifically, the data reading unit 201 reads data based on information input by the user to the input/output device 160 (for example, the robot to be trajectory planned, the object to be grasped, the joints, and/or the grasping method, etc. are input, The robot arm configuration table 310, interfering object configuration table 320, starting joint angle table 330, target position/posture table 340, and grip force/moment constraint table 350 are read. Note that the information in these tables is registered, for example, by the user inputting it into the input/output device 160.

また、データ読込部２０１は、（３次元センサ１７０を通して得られた情報である）把持物情報テーブル３６０及び把持方法情報テーブル３７０を読み込む。なお、ここではデータ読込部２０１が読み込んだデータが、入出力装置１６０や３次元センサ１７０を経由して取得される例を説明したが、過去の入力データをはじめとする、既に取得済みのデータが用いられてもよい。 The data reading unit 201 also reads the held object information table 360 (which is information obtained through the three-dimensional sensor 170) and the holding method information table 370. Note that although an example has been described here in which the data read by the data reading unit 201 is acquired via the input/output device 160 or the three-dimensional sensor 170, data that has already been acquired such as past input data may be used.

初期経路計画部２０５は、開始関節角度テーブル３３０と、目標位置姿勢テーブル３４０と、干渉物構成テーブル３２０と、に基づいて、動作する関節の開始関節角度θ１から、当該関節に結ばれたリンクの目標姿勢Ｐｎまでをロボットアーム及び把持物が干渉物と干渉しないよう繋ぐ初期経路を算出する（Ｓ１０２）。初期経路計画部２０５は、どのような手段で干渉しない初期経路を求めてもよいが、例えば、ＲＲＴ（Ｒａｐｉｄｌｙ－ｅｘｐｒｏｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅ）法を用いて初期経路を算出する。そして、初期経路計画部２０５は、算出した初期経路の各点に軌道点ＩＤを付与し、各軌道点ＩＤに示す各点（各軌道点）おける関節角度を軌道テーブル３８０に格納する。 The initial path planning unit 205 calculates the links connected to the joint from the starting joint angle θ 1 of the joint to be operated based on the starting joint angle table 330, the target position/posture table 340, and the interference object configuration table 320. An initial path is calculated that connects the robot arm and the gripped object up to the target posture Pn so as not to interfere with any interfering objects (S102). The initial route planning unit 205 may use any means to find an initial route that does not cause interference; for example, the initial route planning unit 205 uses the RRT (Rapidly-exploding Random Tree) method to calculate the initial route. Then, the initial route planning unit 205 assigns a trajectory point ID to each point on the calculated initial route, and stores the joint angle at each point (each trajectory point) indicated by each trajectory point ID in the trajectory table 380.

把持物考慮時刻付与部２０６は、軌道テーブル３８０に格納されている各軌道点に対して、把持力・モーメント制約テーブル３５０に基づいて、関節角速度、関節角速度、及び制御時刻を付与又は修正する（Ｓ１０３）。なお、ステップＳ１０３の詳細については図１１を用いて後述する。 The grasped object consideration time assigning unit 206 assigns or modifies joint angular velocity, joint angular velocity, and control time to each trajectory point stored in the trajectory table 380 based on the grasping force/moment constraint table 350 ( S103). Note that details of step S103 will be described later using FIG. 11.

軌道計画部２０４は、軌道テーブル３８０の軌道の変化が無くなったか判定する（Ｓ１０４）。具体的には、例えば、軌道計画部２０４は、直前のステップＳ１０５で更新された軌道における各軌道点における位置又は時刻と、当該更新の直前の軌道における各軌道点における位置又は時刻と、の差の合計値が所定値以下である場合に軌道の変化が無くなったと判定し、所定値を超える場合に軌道の変化があったと判定する。 The trajectory planning unit 204 determines whether there is no change in the trajectory in the trajectory table 380 (S104). Specifically, for example, the trajectory planning unit 204 calculates the difference between the position or time at each orbit point in the trajectory updated in the immediately previous step S105 and the position or time at each orbit point in the trajectory immediately before the update. When the total value of is less than or equal to a predetermined value, it is determined that there is no change in the trajectory, and when it exceeds the predetermined value, it is determined that there has been a change in the trajectory.

なお、初回のステップＳ１０４においては、軌道計画部２０４は、例えば、必ず軌道テーブル３８０の値の変化があったと判定してもよいし、ステップＳ１０３で付与した各軌道点の位置又は時刻と、所定の軌道の各軌道点の位置又は時刻と、の差の合計値に基づいて軌道の変化があったかないかを判定してもよい。軌道計画部２０４は、軌道テーブル３８０の軌道の変化が無くなったと判定した場合（Ｓ１０４：ＹＥＳ）、ステップＳ１０６に遷移する。 In addition, in the first step S104, the trajectory planning unit 204 may, for example, determine that there is always a change in the value of the trajectory table 380, or may determine that the position or time of each trajectory point given in step S103 and a predetermined It may be determined whether or not there is a change in the trajectory based on the total value of the difference between the position or time of each orbit point of the trajectory. When the trajectory planning unit 204 determines that there is no change in the trajectory in the trajectory table 380 (S104: YES), the process proceeds to step S106.

軌道計画部２０４が軌道テーブル３８０の軌道の変化があったと判定した場合（Ｓ１０４：ＮＯ）、把持物考慮経路最適化部２０７は、把持力・モーメント制約テーブル３５０に基づいて、動力学計算部２０３等を用いて、把持物にかかる把持力、及びモーメントが小さくなるよう軌道の経路を変更し、軌道テーブル３８０の関節角度、関節角速度、及び関節角速度を更新することで軌道を最適化する（Ｓ１０５）。なお、ステップＳ１０５の詳細については図１２を用いて後述する。 If the trajectory planning unit 204 determines that there has been a change in the trajectory of the trajectory table 380 (S104: NO), the gripped object consideration path optimization unit 207 calculates the dynamics calculation unit 203 based on the gripping force/moment constraint table 350. etc., the path of the trajectory is changed so that the grasping force and moment applied to the grasped object are reduced, and the trajectory is optimized by updating the joint angle, joint angular velocity, and joint angular velocity in the trajectory table 380 (S105 ). Note that details of step S105 will be described later using FIG. 12.

結果出力部２０８は、補助記憶装置１３０に格納されているデータに基づいて結果表示画面を生成し、入出力装置１６０に表示する（Ｓ１０６）。結果表示画面の一例については、図１３を用いて後述する。 The result output unit 208 generates a result display screen based on the data stored in the auxiliary storage device 130, and displays it on the input/output device 160 (S106). An example of the result display screen will be described later using FIG. 13.

［把持物考慮時刻付与処理］
図１１は、ステップＳ１０３における制御時刻付与処理の一例を示すフローチャートである。把持物考慮時刻付与部２０６は、本処理においては、軌道テーブル３８０における関節角度は変更せず、関節角速度、関節角加速度、及び制御時刻を変更する。以下、本処理について説明する。 [Gripped object consideration time assignment process]
FIG. 11 is a flowchart illustrating an example of the control time provision process in step S103. In this process, the grasped object consideration time providing unit 206 does not change the joint angle in the trajectory table 380, but changes the joint angular velocity, joint angular acceleration, and control time. This process will be explained below.

把持物考慮時刻付与部２０６は、軌道テーブル３８０から軌道のデータを取得する（Ｓ２０１）。把持物考慮時刻付与部２０６は、ロボットアーム構成テーブル３１０に格納されている、各関節軸の最大速度情報に基づいて、全ての軌道点に対して時刻を付与し、付与した時刻を軌道テーブル３８０の制御時刻欄３８５に格納する。把持物考慮時刻付与部２０６は、この時刻Δｔｉを、例えば、以下の式によって決定する。 The held object consideration time providing unit 206 acquires trajectory data from the trajectory table 380 (S201). The gripped object consideration time assigning unit 206 assigns times to all trajectory points based on the maximum velocity information of each joint axis stored in the robot arm configuration table 310, and stores the assigned times in the trajectory table 380. is stored in the control time column 385. The held object consideration time assigning unit 206 determines this time Δt i using the following equation, for example.

Figure 0007405730000001
Figure 0007405730000001

なお、上記式のｋは関節を識別する関節番号であり、ｉは軌道点ＩＤでありθｋ，ｉは関節番号ｋの関節の軌道点ＩＤがｉの軌道点における関節角度である。また、Ｖｍａｘｋは、関節ｋの最大速度情報である。 Note that k in the above formula is a joint number that identifies a joint, i is a trajectory point ID, and θ k,i is a joint angle at a trajectory point whose trajectory point ID is i of the joint with joint number k. Further, Vmax k is maximum velocity information of joint k.

把持物考慮時刻付与部２０６は、ステップＳ２０４からステップＳ２０８までの処理を、時刻の更新がなくなるまで繰り返す（Ｓ２０３）。把持物考慮時刻付与部２０６は、ステップＳ２０５からステップＳ２０８までの処理を、軌道テーブル３８０に格納されている全ての軌道点に対して順番に行う。 The held object consideration time assigning unit 206 repeats the processing from step S204 to step S208 until the time is no longer updated (S203). The held object consideration time assigning unit 206 sequentially performs the processes from step S205 to step S208 for all trajectory points stored in the trajectory table 380.

把持物考慮時刻付与部２０６は、軌道点ｉにおける関節角速度、関節角加速度を計算し、軌道テーブル３８０の関節角速度、関節角加速度を修正する（Ｓ２０５）。なお、把持物考慮時刻付与部２０６は、関節角速度θ’ｉ、及び関節角加速度θ’’ｉを、例えば、以下の数値微分による式によって算出する。 The grasped object consideration time assigning unit 206 calculates the joint angular velocity and joint angular acceleration at the trajectory point i, and corrects the joint angular velocity and joint angular acceleration in the trajectory table 380 (S205). Note that the grasped object consideration time assigning unit 206 calculates the joint angular velocity θ′ i and the joint angular acceleration θ″ i using, for example, the following equations based on numerical differentiation.

Figure 0007405730000002
Figure 0007405730000002

Figure 0007405730000003
Figure 0007405730000003

動力学計算部２０３は、軌道点ｉにおいて把持物にかかる把持力、及び把持物にかかるモーメントを算出し、把持物考慮時刻付与部２０６は、算出された把持力、及びモーメントが、把持力・モーメント制約テーブル３５０に格納されている制約を満たしているかを判定する（Ｓ２０６）。 The dynamics calculation unit 203 calculates the gripping force applied to the gripped object and the moment applied to the gripped object at the trajectory point i, and the gripped object consideration time giving unit 206 calculates the gripping force and moment applied to the gripped object at trajectory point i. It is determined whether the constraints stored in the moment constraint table 350 are satisfied (S206).

ステップＳ２０６における計算処理を、図１４を用いて説明する。図１４は、把持物にかかる把持力、及び把持物にかかるモーメントの計算処理の一例を示す説明図である。図１４の例では、ロボットアームに装着された吸盤が把持物を吸着把持する。まず、動力学計算部２０３は、軌道点ｉにおける関節角速度θ’ｉ及び関節角加速度θ’’ｉから、把持物の加速度ａｉを以下の式によって算出する。 The calculation process in step S206 will be explained using FIG. 14. FIG. 14 is an explanatory diagram illustrating an example of calculation processing of the gripping force applied to the gripped object and the moment applied to the gripped object. In the example of FIG. 14, a suction cup attached to a robot arm suctions and grips an object. First, the dynamics calculation unit 203 calculates the acceleration a i of the grasped object from the joint angular velocity θ' i and the joint angular acceleration θ'' i at the trajectory point i using the following formula.

Figure 0007405730000004
Figure 0007405730000004

なお、Ｊｉは、軌道点ｉにおける把持物の位置及び姿勢の変化方向を表すヤコビ行列である。動力学計算部２０３は、上記の式を計算することで、水平方向の移動に伴う加速度だけでなく、円運動に伴う中心方向に向かう遠心加速度等をまとめて計算できる。図１４の状態において、把持物の重心と一緒に動く座標系上においては、把持物は静止しているとみなせるため、重力ｍｇと把持力Ｔｉ、及び慣性力－ｍａｉの力のつり合いより以下の式が成り立つ。 Note that J i is a Jacobian matrix representing the direction of change in the position and orientation of the object held at orbit point i. By calculating the above equation, the dynamics calculation unit 203 can collectively calculate not only the acceleration accompanying the horizontal movement but also the centrifugal acceleration toward the center due to the circular movement. In the state of FIG. 14, the object to be held can be considered to be stationary on the coordinate system that moves together with the center of gravity of the object to be held. The following formula holds.

Figure 0007405730000005
Figure 0007405730000005

また、動力学計算部２０３は、把持物の重心周りでのモーメントＭｉを、把持物の重心位置から見た把持位置ｒを用いて以下の式によって算出する。 Furthermore, the dynamics calculation unit 203 calculates the moment M i around the center of gravity of the object to be held, using the grip position r seen from the center of gravity of the object to be held, using the following equation.

Figure 0007405730000006
Figure 0007405730000006

把持物考慮時刻付与部２０６は、ステップＳ２０６で算出された把持力Ｔｉ、およびモーメントＭｉ等が、把持力・モーメント制約テーブル３５０に格納されている制約を満たしているかを判定する。 The gripped object consideration time assigning unit 206 determines whether the gripping force T i and the moment M i calculated in step S206 satisfy the constraints stored in the gripping force/moment constraint table 350.

図１１の説明に戻る。把持物考慮時刻付与部２０６は、ステップＳ２０６において制約を満たしていないと判定した場合（Ｓ２０６：ＮＯ）、Δｔｉを所定の時刻分増加させ、軌道テーブル３８０の制御時刻を更新する（Ｓ２０７）。ステップＳ２０５～ステップＳ２０７の処理のループによって制御時刻の増加が繰り返されることで、関節角速度及び関節角速度が小さくなっていくため、把持物にかかる慣性力やモーメントも小さくなり、ステップＳ２０６の判定における制約がいずれ満たされることになり、最終的にステップＳ２０３のループを抜ける条件が満たされる。 Returning to the explanation of FIG. 11. If it is determined in step S206 that the constraint is not satisfied (S206: NO), the held object consideration time assigning unit 206 increases Δt i by a predetermined time and updates the control time in the trajectory table 380 (S207). As the control time is repeatedly increased through the processing loop from step S205 to step S207, the joint angular velocity and the joint angular velocity become smaller, so the inertia force and moment applied to the grasped object also become smaller, which limits the determination in step S206. will eventually be satisfied, and finally the condition for exiting the loop of step S203 is satisfied.

そして、把持物考慮時刻付与部２０６がΔｔｉを少しずつ増加させて制御時刻を伸ばしていくことで、ステップＳ２０３のループを抜けるタイミングでは、制約を満たしつつも制約値に近い値でロボットアームが動く軌道を実現できる。 Then, the gripped object consideration time assigning unit 206 increases Δt i little by little to extend the control time, so that at the timing of exiting the loop in step S203, the robot arm is set to a value close to the constraint value while satisfying the constraint. A moving trajectory can be realized.

［把持物考慮経路最適化処理］
図１２は、ステップＳ１０５における把持物考慮経路最適化処理の一例を示すフローチャートである。本処理では、把持物考慮経路最適化部２０７は、軌道テーブル３８０おける制御時刻は変更せず、関節角度、関節角速度、及び関節角加速度を変更する。本処理では、把持物考慮経路最適化部２０７は、軌道における経路を曲線σととらえ、経路に対する評価関数Ｆ（σ）を定義し、この評価関数が最小となる経路を生成することで、把持力・モーメントが小さくなるように軌道テーブル３８０の軌道を修正する。軌道に対する評価関数の一例を以下に示す。 [Gripped object-considering route optimization process]
FIG. 12 is a flowchart illustrating an example of the route optimization process considering the held object in step S105. In this process, the grasped object consideration path optimization unit 207 does not change the control time in the trajectory table 380, but changes the joint angle, joint angular velocity, and joint angular acceleration. In this process, the gripped object-considering path optimization unit 207 regards the path in the trajectory as a curve σ, defines an evaluation function F(σ) for the path, and generates a path that minimizes this evaluation function. The trajectory of the trajectory table 380 is corrected so that the force/moment is reduced. An example of the evaluation function for the trajectory is shown below.

Figure 0007405730000007
Figure 0007405730000007

ここで、積分内の第一項は把持力に関する評価値であり、第二項はモーメントに関する評価値であり、第三項は軌道の滑らかさに関する評価値である。なお、出力される軌道が関節角度空間でも平滑化されるようにするために、上記式の第三項のような軌道の滑らかさを評価する指標が、Ｆ（σ）に含まれているとよいが、含まれていなくてもよい。また、α、β、γはそれぞれ、各評価値の重みである。φはソフトプラス関数であり、入力値が閾値以上で大きくなるほど出力の傾きが増し、入力値が閾値より小さい場合には傾きが緩やかである関数である。以下、本処理について説明する。 Here, the first term in the integral is an evaluation value regarding gripping force, the second term is an evaluation value regarding moment, and the third term is an evaluation value regarding trajectory smoothness. In addition, in order to ensure that the output trajectory is smoothed in the joint angle space, an index for evaluating the smoothness of the trajectory, such as the third term in the above equation, is included in F(σ). Yes, but it doesn't have to be included. Moreover, α, β, and γ are weights of each evaluation value, respectively. φ is a soft plus function, and the slope of the output increases as the input value becomes greater than or equal to the threshold value, and the slope becomes gentler when the input value is smaller than the threshold value. This process will be explained below.

上記積分内の第一項の把持力に関する評価値が低下することにより把持物を把持するロボットアームの吸着パッド等が外れる可能性が低下し、第二項のモーメントに関する評価値が低下することにより把持物が破損する可能性が低下し、第三項の軌道の滑らかさに関する評価値が低下することにより角加速度が低下する。 As the evaluation value related to the gripping force in the first term in the above integral decreases, the possibility that the suction pad etc. of the robot arm that grips the object will come off decreases, and the evaluation value related to the second term in the moment decreases. The possibility of the object being held being damaged is reduced, and the evaluation value regarding the smoothness of the trajectory in the third term is reduced, thereby reducing the angular acceleration.

把持物考慮経路最適化部２０７は、軌道テーブル３８０に格納されている軌道のデータを取得する（Ｓ３０１）。把持物考慮経路最適化部２０７は、最適化計算によって軌道σが収束するまでステップＳ３０３からステップＳ３０８までの処理をループ演算する（Ｓ３０２）。 The grasped object consideration route optimization unit 207 acquires trajectory data stored in the trajectory table 380 (S301). The grasped object-considering path optimization unit 207 performs a loop calculation of the processes from step S303 to step S308 until the trajectory σ converges by the optimization calculation (S302).

把持物考慮経路最適化部２０７は、把持力やモーメントからなる評価関数Ｆ（σ）から評価値を計算する（Ｓ３０３）。把持物考慮経路最適化部２０７は、把持力やモーメントを、ステップＳ２０６の説明で述べた手法で算出できる。 The grasped object consideration path optimization unit 207 calculates an evaluation value from an evaluation function F(σ) consisting of grasping force and moment (S303). The grasped object consideration path optimization unit 207 can calculate the grasping force and moment using the method described in the explanation of step S206.

把持物考慮経路最適化部２０７は、評価関数Ｆ（σ）を小さくする方向を示す汎関数微分δＦ（σ）／δσを計算する（Ｓ３０４）。把持物考慮経路最適化部２０７は、例えば、経路を少しずつ変更することで数値微分として汎関数微分を計算してもよいし、ＳＴＯＭＰ（Ｓｔｏｃｈａｓｔｉｃ Ｔｒａｊｅｃｔｏｒｙ Ｏｐｔｉｍｉｚａｔｉｏｎ ｆｏｒ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ）法によって、正規分布に従ったノイズを与えた軌道を複数作成し、軌道の変化と評価関数の変化から汎関数微分を計算してもよい。 The grasped object-considering path optimization unit 207 calculates a functional differential δF(σ)/δσ indicating the direction in which the evaluation function F(σ) is to be reduced (S304). For example, the grasped object-considering path optimization unit 207 may calculate a functional differential as a numerical differential by changing the path little by little, or may calculate a functional differential using a STOMP (Stochastic Trajectory Optimization for Motion Planning) method according to a normal distribution. Alternatively, a plurality of trajectories with added noise may be created, and functional differentials may be calculated from changes in the trajectories and changes in the evaluation function.

把持物考慮経路最適化部２０７は、軌道σｎｅｗを以下の式で算出する（Ｓ３０５）。 The grasped object-considering route optimization unit 207 calculates the trajectory σ new using the following formula (S305).

Figure 0007405730000008
Figure 0007405730000008

干渉判定部２０２は、ロボットアーム構成テーブル３１０と干渉物構成テーブル３２０とを参照して使い、軌道σｎｅｗにおいてロボットアーム及び把持物が干渉物と干渉していないかを判定する（Ｓ３０６）。 The interference determination unit 202 refers to and uses the robot arm configuration table 310 and the interference object configuration table 320 to determine whether the robot arm and the object to be held are interfering with an interference object in the trajectory σ new (S306).

干渉判定部２０２は、干渉が発生していないと判定した場合（Ｓ３０６：ＮＯ）、把持物考慮経路最適化部２０７は、軌道σｎｅｗを新たな軌道σとして採用して、軌道テーブル３８０の関節角度を更新し、さらに制御時刻の情報にしたがい、各軌道点における関節速度及び関節角加速度を、ステップＳ２０５で説明した数式を用いて更新する（Ｓ３０７）。干渉判定部２０２は、干渉が発生していると判定した場合（Ｓ３０６：ＹＥＳ）、ステップＳ３０７の処理へと遷移せずにステップＳ３０２のループ判定へと遷移する。 If the interference determination unit 202 determines that no interference has occurred (S306: NO), the grasped object consideration path optimization unit 207 adopts the trajectory σ new as a new trajectory σ, and adjusts the joints of the trajectory table 380. The angle is updated, and the joint velocity and joint angular acceleration at each trajectory point are updated using the formula described in step S205 in accordance with the control time information (S307). When the interference determination unit 202 determines that interference has occurred (S306: YES), the interference determination unit 202 does not proceed to the process of step S307, but transitions to the loop determination of step S302.

把持物考慮経路最適化部２０７がステップＳ３０３からステップＳ３０８の処理を繰り返すことで、把持力及び／又はモーメントが小さくなるように経路を変更することができる。 By repeating the processing from step S303 to step S308, the gripped object consideration path optimization unit 207 can change the path so that the gripping force and/or moment are reduced.

図１５は、軌道計画処理における軌道の変化の一例を示す説明図である。図１５の、横方向には把持物を平行移動する軌道に関する、最適化計算の回数と付与される時刻、およびその時の把持物の姿勢と把持物にかかるモーメントの関係を示す。ステップＳ１０２で算出された初期軌道（ａ）の各軌道点に対して、仮に最大速度で時刻が付与されると、モーメントが上限値を超える経由点が発生する。 FIG. 15 is an explanatory diagram illustrating an example of a trajectory change in the trajectory planning process. In FIG. 15, the horizontal direction shows the number of optimization calculations, the given time, and the relationship between the posture of the gripped object and the moment applied to the gripped object, regarding the trajectory of parallel translation of the gripped object. If time is given to each orbit point of the initial trajectory (a) calculated in step S102 at the maximum speed, a way point where the moment exceeds the upper limit value will occur.

そこで、把持物考慮時刻付与部２０６は、ステップＳ１０３の処理において、各軌道点に付与する時間間隔をのばすことで、角加速度を小さくし、モーメントが上限値を超えないような時刻を付与することで（ｂ）の軌道が得られる。 Therefore, in the process of step S103, the held object consideration time assigning unit 206 extends the time interval assigned to each trajectory point to decrease the angular acceleration and assign a time such that the moment does not exceed the upper limit value. The trajectory (b) is obtained.

次に、把持物考慮経路最適化部２０７は、ステップＳ１０５の処理において、モーメントが最小となるよう、把持物を傾けた経路を生成することで（ｃ）の軌道が得られる。これにより、把持物を傾ける軌道が得られるためモーメントが上限値より小さくなり、加速度を上げることができるため、把持物考慮時刻付与部２０６は、再度ステップＳ１０３の処理を実行して、時間間隔を短くするような時刻を付与して（ｄ）の軌道を得る。これらの処理が繰り返されることで、最大速度、最大加速度で把持物を移動させながら、モーメントが上限値以下となる軌道が生成される。 Next, in the process of step S105, the held object consideration path optimization unit 207 generates a path in which the held object is tilted so that the moment is minimized, thereby obtaining the trajectory (c). As a result, a trajectory for tilting the held object is obtained, so that the moment becomes smaller than the upper limit value, and the acceleration can be increased. Therefore, the held object consideration time giving unit 206 executes the process of step S103 again to set the time interval. The trajectory (d) is obtained by assigning a shorter time. By repeating these processes, a trajectory is generated in which the held object is moved at the maximum speed and maximum acceleration while the moment is below the upper limit value.

［出力画面］
図１３は、結果出力画面の一例である。結果出力画面１３００は、例えば、読込ボタン１３０１、計算開始ボタン１３０２、動作時間表１３０３、把持力変化グラフ１３０４、モーメント変化グラフ１３０５、及びシミュレート表示１３０６を含む。読込ボタン１３０１が選択されると、入出力インターフェース１５０を介して入力された入力データが読み込まれる。 [Output screen]
FIG. 13 is an example of a result output screen. The result output screen 1300 includes, for example, a read button 1301, a calculation start button 1302, an operation time table 1303, a grip force change graph 1304, a moment change graph 1305, and a simulation display 1306. When the read button 1301 is selected, input data input via the input/output interface 150 is read.

計算開始ボタン１３０２が選択されると、軌道計画計算（ステップＳ１０２以降の処理）が実行され、動作時間表１３０３、把持力変化グラフ１３０４、モーメント変化グラフ１３０５、及びシミュレート表示１３０６が表示される。把持力変化グラフ１３０４は、軌道の各制御時刻における把持力の変化を示し、モーメント変化グラフ１３０５は、軌道の各制御時刻におけるモーメントの変化を示す。なお、結果出力画面１３００において、把持力変化グラフ１３０４又はモーメント変化グラフ１３０５の一方が表示されなくてもよい。 When the calculation start button 1302 is selected, trajectory planning calculation (processing after step S102) is executed, and an operation time table 1303, a grip force change graph 1304, a moment change graph 1305, and a simulation display 1306 are displayed. A gripping force change graph 1304 shows a change in gripping force at each control time of the trajectory, and a moment change graph 1305 shows a change in moment at each control time of the trajectory. Note that on the result output screen 1300, either the gripping force change graph 1304 or the moment change graph 1305 may not be displayed.

動作時間表１３０３には、例えば、把持力・モーメント制約テーブル３５０、把持物情報テーブル３６０、及び把持方法情報テーブル３７０等の条件や制約に従って軌道計画を行った場合の動作時間が表示される。また、これらのテーブルの条件が変更されることで、把持力変化グラフ１３０４やモーメント変化グラフ１３０５も変化する。また、動作時間表１３０３内の動作再生ボタンが選択されると、シミュレート表示領域においてロボットアームの動作が再生表示される。 The operation time table 1303 displays, for example, the operation time when the trajectory is planned according to the conditions and constraints of the gripping force/moment constraint table 350, the gripping object information table 360, the gripping method information table 370, and the like. Moreover, by changing the conditions of these tables, the gripping force change graph 1304 and the moment change graph 1305 also change. Furthermore, when the motion replay button in the motion time table 1303 is selected, the motion of the robot arm is reproduced and displayed in the simulation display area.

［効果等］
以上に説明したように本実施例の軌道計画装置１００は、把持物にかかる把持力やモーメントの制約を満たしつつ、始点から終点までを高速に繋ぐ軌道を生成することができる。 [Effects etc.]
As described above, the trajectory planning device 100 of this embodiment can generate a trajectory that connects the starting point to the ending point at high speed while satisfying the constraints on the gripping force and moment applied to the gripped object.

なお、本発した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。 Note that the present invention is not limited to the present embodiment, and includes various modifications. For example, the embodiments described above are described in detail to explain the present invention in an easy-to-understand manner, and the present invention is not necessarily limited to having all the configurations described. Furthermore, it is possible to replace a part of the configuration of one embodiment with the configuration of another embodiment, and it is also possible to add the configuration of another embodiment to the configuration of one embodiment. Further, it is possible to add, delete, or replace a part of the configuration of each embodiment with other configurations.

また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。 Further, each of the above-mentioned configurations, functions, processing units, processing means, etc. may be partially or entirely realized in hardware by designing, for example, an integrated circuit. Further, each of the above-mentioned configurations, functions, etc. may be realized by software by a processor interpreting and executing a program for realizing each function. Information such as programs, tables, files, etc. that implement each function can be stored in a memory, a recording device such as a hard disk, an SSD (Solid State Drive), or a recording medium such as an IC card, an SD card, or a DVD.

また、制御線や情報線は説明上必要と考えられるものを示しており、製品上必ずしも全ての制御線や情報線を示しているとは限らない。実際には殆ど全ての構成が相互に接続されていると考えてもよい。 Further, the control lines and information lines are shown to be necessary for explanation purposes, and not all control lines and information lines are necessarily shown in the product. In reality, almost all components may be considered to be interconnected.

１００ 軌道計画装置、１１０ ＣＰＵ、１２０ メモリ、１３０ 補助記憶装置、１４０ 通信装置、１５０ 入出力インターフェース、１６０ 入出力装置、１５０ ３次元センサ、２０１ データ読込部、２０２ 干渉判定部、２０３ 動力学計算部、２０４ 軌道計画部、２０５ 初期経路探索部、２０６ 把持物考慮時刻付与部、２０７ 把持物考慮経路最適化部、２０８ 結果出力部、３１０ ロボットアーム構成テーブル、３２０ 干渉物構成テーブル、３３０ 開始関節角度テーブル、３４０ 目標位置姿勢テーブル、３５０ 把持力・モーメント制約テーブル、３６０ 把持物情報テーブル、３７０ 把持方法情報テーブル、３８０ 軌道テーブル 100 trajectory planning device, 110 CPU, 120 memory, 130 auxiliary storage device, 140 communication device, 150 input/output interface, 160 input/output device, 150 three-dimensional sensor, 201 data reading unit, 202 interference determination unit, 203 dynamics calculation unit , 204 Trajectory Planning Unit, 205 Initial Path Search Unit, 206 Grasped Object Consideration Time Adding Unit, 207 Grasped Object Consideration Path Optimization Unit, 208 Result Output Unit, 310 Robot Arm Configuration Table, 320 Interfering Object Configuration Table, 330 Start Joint Angle table, 340 target position/posture table, 350 gripping force/moment constraint table, 360 gripped object information table, 370 gripping method information table, 380 trajectory table

Claims (13)

可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道を計画する軌道計画装置であって、
プロセッサとメモリとを有し、
前記メモリは、
前記把持物の重量及び重心位置を示す把持物情報と、
前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、
前記軌道における経路の各点の位置を示す軌道情報と、を保持し、
前記プロセッサは、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を実行する軌道計画装置。 A trajectory planning device that plans a trajectory along which a movable axis of a robot arm that has a movable axis and grasps an object,
has a processor and memory;
The memory is
gripped object information indicating the weight and center of gravity of the gripped object;
constraint information indicating constraints on the gripping force and moment applied to the gripped object;
and trajectory information indicating the position of each point on the route on the trajectory,
The processor includes:
Based on the weight and the center of gravity position indicated by the grasped object information, the movable axis passes through each point on the path indicated by the trajectory information so as to satisfy the constraints of the grasping force and the moment indicated by the constraint information. a time assignment process of assigning a time and storing the assigned time in the orbit information;
Route optimization processing for changing the positions of points included in the route in the trajectory information so as to reduce the gripping force and/or moment applied to the grasped object, based on the weight and the center of gravity position indicated by the grasped object information. and a trajectory planning device that executes. 請求項１に記載の軌道計画装置であって、
前記プロセッサは、
前記時刻付与処理と、前記経路最適化処理と、を交互に繰り返して実行し、
前記時刻付与処理の直後に、前記軌道の経路又は当該経路の各点に付与された時刻と、前回の時刻付与処理の直後の前記軌道の経路又は当該経路の各点に付与された時刻と、の変化量に基づいて、前記時刻付与処理と、前記経路最適化処理と、を終了する、軌道計画装置。 The trajectory planning device according to claim 1,
The processor includes:
Alternately repeating the time assigning process and the route optimization process,
a time assigned to the route of the trajectory or each point on the route immediately after the time assignment process; a time assigned to the route of the trajectory or each point on the route immediately after the previous time assignment process; A trajectory planning device that terminates the time assignment process and the route optimization process based on the amount of change. 請求項１に記載の軌道計画装置であって、
前記メモリは、前記可動軸の角速度及び角加速度の制約を示すロボットアーム情報を保持し、
前記プロセッサは、前記時刻付与処理において、前記ロボットアーム情報が示す前記角速度及び前記角加速度の制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与する、軌道計画装置。 The trajectory planning device according to claim 1,
the memory holds robot arm information indicating constraints on angular velocity and angular acceleration of the movable axis;
In the time assigning process, the processor assigns a time at which the movable axis passes through each point on the path indicated by the trajectory information so as to satisfy constraints on the angular velocity and the angular acceleration indicated by the robot arm information. Trajectory planning device. 請求項１に記載の軌道計画装置であって、
前記プロセッサは、前記経路最適化処理において、前記軌道情報が示す経路が滑らかになるよう前記軌道情報における経路に含まれる点の位置を変更する、軌道計画装置。 The trajectory planning device according to claim 1,
In the route optimization process, the processor changes the positions of points included in the route in the trajectory information so that the route indicated by the trajectory information becomes smooth. 請求項１に記載の軌道計画装置であって、
前記メモリは、干渉物の位置を示す干渉物情報を保持し、
前記プロセッサは、前記経路最適化処理において、前記干渉物情報が示す前記干渉物の位置に基づいて、前記軌道情報における経路に含まれる点の位置を変更する、軌道計画装置。 The trajectory planning device according to claim 1,
The memory holds interference object information indicating the position of the interference object,
A trajectory planning device, wherein, in the route optimization process, the processor changes the position of a point included in the route in the trajectory information based on the position of the interference object indicated by the interference object information. 請求項１に記載の軌道計画装置であって、
表示装置に接続され、
前記プロセッサは、前記時刻付与処理と前記経路最適化処理とが実行された後の前記軌道情報における軌道を前記可動軸が移動する場合において前記把持物にかかる把持力及び／又はモーメントの時間変化を示す情報を、前記表示装置に表示する、軌道計画装置。 The trajectory planning device according to claim 1,
connected to a display device,
The processor is configured to calculate temporal changes in gripping force and/or moment applied to the gripped object when the movable axis moves along a trajectory in the trajectory information after the time assigning process and the route optimization process are executed. A trajectory planning device that displays information on the display device. 可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道を計画する軌道計画装置による軌道計画方法であって、
前記軌道計画装置は、プロセッサとメモリとを有し、
前記メモリは、
前記把持物の重量及び重心位置を示す把持物情報と、
前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、
前記軌道における経路の各点の位置を示す軌道情報と、を保持し、
前記軌道計画方法は、
前記プロセッサが、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を実行する軌道計画方法。 A trajectory planning method using a trajectory planning device for planning a trajectory of a movable axis of a robot arm having a movable axis and grasping an object, the method comprising:
The trajectory planning device has a processor and a memory,
The memory is
gripped object information indicating the weight and center of gravity of the gripped object;
constraint information indicating constraints on the gripping force and moment applied to the gripped object;
and trajectory information indicating the position of each point on the route on the trajectory,
The trajectory planning method includes:
The processor,
Based on the weight and the center of gravity position indicated by the grasped object information, the movable axis passes through each point on the path indicated by the trajectory information so as to satisfy the constraints of the grasping force and the moment indicated by the constraint information. a time assignment process of assigning a time and storing the assigned time in the orbit information;
Route optimization processing for changing the positions of points included in the route in the trajectory information so as to reduce the gripping force and/or moment applied to the grasped object, based on the weight and the center of gravity position indicated by the grasped object information. and a trajectory planning method for executing. 請求項７に記載の軌道計画方法であって、
前記プロセッサが、前記時刻付与処理と、前記経路最適化処理と、を交互に繰り返して実行し、
前記プロセッサが、前記時刻付与処理の直後に、前記軌道の経路又は当該経路の各点に付与された時刻と、前回の時刻付与処理の直後の前記軌道の経路又は当該経路の各点に付与された時刻と、の変化量に基づいて、前記時刻付与処理と、前記経路最適化処理と、を終了する、軌道計画方法。 The trajectory planning method according to claim 7,
the processor alternately and repeatedly executes the time assigning process and the route optimization process;
Immediately after the time assignment process, the processor includes a time assigned to the route of the trajectory or each point on the route, and a time assigned to the route of the trajectory or each point on the route immediately after the previous time assignment process. A trajectory planning method, wherein the time assigning process and the route optimization process are terminated based on the amount of change in the time. 請求項７に記載の軌道計画方法であって、
前記メモリは、前記可動軸の角速度及び角加速度の制約を示すロボットアーム情報を保持し、
前記軌道計画方法は、前記プロセッサが、前記時刻付与処理において、前記ロボットアーム情報が示す前記角速度及び前記角加速度の制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与する、軌道計画方法。 The trajectory planning method according to claim 7,
the memory holds robot arm information indicating constraints on angular velocity and angular acceleration of the movable axis;
In the trajectory planning method, the processor, in the time assigning process, causes the movable axis to pass through each point on the path indicated by the trajectory information so as to satisfy the constraints of the angular velocity and the angular acceleration indicated by the robot arm information. A trajectory planning method that assigns a time. 請求項７に記載の軌道計画方法であって、
前記プロセッサが、前記経路最適化処理において、前記軌道情報が示す経路が滑らかになるよう前記軌道情報における経路に含まれる点の位置を変更する、軌道計画方法。 The trajectory planning method according to claim 7,
A trajectory planning method, wherein the processor changes the positions of points included in the route in the trajectory information so that the route indicated by the trajectory information becomes smooth in the route optimization process. 請求項７に記載の軌道計画方法であって、
前記メモリは、干渉物の位置を示す干渉物情報を保持し、
前記軌道計画方法は、前記プロセッサが、前記経路最適化処理において、前記干渉物情報が示す前記干渉物の位置に基づいて、前記軌道情報における経路に含まれる点の位置を変更する、軌道計画方法。 The trajectory planning method according to claim 7,
The memory holds interference object information indicating the position of the interference object,
In the trajectory planning method, the processor changes the position of a point included in the route in the trajectory information based on the position of the interferer indicated by the interferer information in the route optimization process. . 請求項７に記載の軌道計画方法であって、
前記軌道計画装置は、表示装置に接続され、
前記軌道計画方法は、前記プロセッサが、前記時刻付与処理と前記経路最適化処理とが実行された後の前記軌道情報における軌道を前記可動軸が移動する場合において前記把持物にかかる把持力及び／又はモーメントの時間変化を示す情報を、前記表示装置に表示する、軌道計画方法。 The trajectory planning method according to claim 7,
The trajectory planning device is connected to a display device,
In the trajectory planning method, the processor calculates the gripping force and/or Or a trajectory planning method, in which information indicating a temporal change in moment is displayed on the display device. 可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道の計画を軌道計画装置に実行させる軌道計画プログラムであって、
前記軌道計画装置は、プロセッサとメモリとを有し、
前記メモリは、
前記把持物の重量及び重心位置を示す把持物情報と、
前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、
前記軌道における経路の各点の位置を示す軌道情報と、を保持し、
前記軌道計画プログラムは、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を前記プロセッサに実行させる軌道計画プログラム。 A trajectory planning program that causes a trajectory planning device to plan a trajectory along which a movable axis of a robot arm that grasps an object has a movable axis, the program comprising:
The trajectory planning device has a processor and a memory,
The memory is
gripped object information indicating the weight and center of gravity of the gripped object;
constraint information indicating constraints on the gripping force and moment applied to the gripped object;
and trajectory information indicating the position of each point on the route on the trajectory,
The trajectory planning program is
Based on the weight and the center of gravity position indicated by the grasped object information, the movable axis passes through each point on the path indicated by the trajectory information so as to satisfy the constraints of the grasping force and the moment indicated by the constraint information. a time assignment process of assigning a time and storing the assigned time in the orbit information;
Route optimization processing for changing the positions of points included in the route in the trajectory information so as to reduce the gripping force and/or moment applied to the grasped object, based on the weight and the center of gravity position indicated by the grasped object information. and a trajectory planning program that causes the processor to execute the following.
JP2020195708A 2020-11-26 2020-11-26 Trajectory planning device, trajectory planning method, and trajectory planning program Active JP7405730B2 (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2020195708A JP7405730B2 (en) 2020-11-26 2020-11-26 Trajectory planning device, trajectory planning method, and trajectory planning program
PCT/JP2021/037231 WO2022113538A1 (en) 2020-11-26 2021-10-07 Trajectory planning device, trajectory planning method, and non-temporary recording medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2020195708A JP7405730B2 (en) 2020-11-26 2020-11-26 Trajectory planning device, trajectory planning method, and trajectory planning program

Publications (2)

Publication Number Publication Date
JP2022084092A JP2022084092A (en) 2022-06-07
JP7405730B2 true JP7405730B2 (en) 2023-12-26

Family

ID=81755778

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2020195708A Active JP7405730B2 (en) 2020-11-26 2020-11-26 Trajectory planning device, trajectory planning method, and trajectory planning program

Country Status (2)

Country Link
JP (1) JP7405730B2 (en)
WO (1) WO2022113538A1 (en)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040265110A1 (en) 2002-05-28 2004-12-30 Bernd Schnoor Method and apparatus for moving a handling system
JP2019141939A (en) 2018-02-19 2019-08-29 オムロン株式会社 Simulation device, simulation method and simulation program

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3442221B2 (en) * 1996-05-21 2003-09-02 株式会社リコー Robot control method
JP7145700B2 (en) * 2018-09-06 2022-10-03 株式会社東芝 hand control device
US10369701B1 (en) * 2018-10-30 2019-08-06 Mujin, Inc. Automated package registration systems, devices, and methods
JP2021091013A (en) * 2019-12-06 2021-06-17 キヤノン株式会社 Control device, robot device, simulation device, control method, simulation method, article manufacturing method, program, and recording medium

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040265110A1 (en) 2002-05-28 2004-12-30 Bernd Schnoor Method and apparatus for moving a handling system
JP2019141939A (en) 2018-02-19 2019-08-29 オムロン株式会社 Simulation device, simulation method and simulation program

Also Published As

Publication number Publication date
JP2022084092A (en) 2022-06-07
WO2022113538A1 (en) 2022-06-02

Similar Documents

Publication Publication Date Title
US9811074B1 (en) Optimization of robot control programs in physics-based simulated environment
JP6036662B2 (en) Robot simulation apparatus, program, recording medium and method
US11090807B2 (en) Motion generation method, motion generation device, system, and computer program
Cruciani et al. Benchmarking in-hand manipulation
JP7459372B2 (en) Apparatus and method for planning contact interaction trajectories - Patents.com
WO2019167765A1 (en) Simulation device, method, and program
Tian et al. Realtime hand-object interaction using learned grasp space for virtual environments
JP2009099082A (en) Dynamics simulation device, dynamics simulation method, and computer program
WO2020075423A1 (en) Robot control device, robot control method and robot control program
CN114641375A (en) Dynamic programming controller
JP2019171501A (en) Robot interference determination device, robot interference determination method and program
JP2019152971A (en) Simulation device, simulation method and simulation program
JP2021146434A (en) Handling device, control device and program
JP7405730B2 (en) Trajectory planning device, trajectory planning method, and trajectory planning program
CN112236272B (en) Gripping posture evaluation device and gripping posture evaluation program
JP2017202549A (en) Robot control device, robot, and robot system
JP5404516B2 (en) Neighborhood determination method and system in computer-aided engineering analysis
JP7263217B2 (en) CONTROL DEVICE, CONTROL METHOD, AND ROBOT SYSTEM
WO2018180300A1 (en) Robot task management system and robot task management program
Badrikouhi et al. Smooth trajectory planning based on direct collocation method for cable-driven parallel robots with central spine
US20230256596A1 (en) Information processing device, method, and program
US20230202026A1 (en) Robot Training System
WO2024134801A1 (en) Processing device, control device, simulation system, processing method, control method, and recording medium
WO2023199456A1 (en) Control device, robot system, control method, and recording medium
WO2023137272A1 (en) Computationally generated grippers

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20230519

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20231205

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20231214

R150 Certificate of patent or registration of utility model

Ref document number: 7405730

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150