JP5516865B2 - Trajectory information generation device for mobile device - Google Patents

Description

本発明は、移動装置を移動させるための軌道及び軌道上の速度に関する軌道情報を生成する装置に係り、特に移動装置の適正な移動を実現する軌道情報を得る軌道情報生成装置に関する。   The present invention relates to an apparatus for generating trajectory information related to a trajectory for moving a moving device and a speed on the trajectory, and more particularly to a trajectory information generating device for obtaining trajectory information for realizing proper movement of the moving device.

移動装置の一種である搬送装置は、特許文献１に例示されるように、ロボットアーム等の搬送機構を用いて搬送対象物を移動させる装置であり、予め設定された軌道及び軌道上の速度に関する軌道情報を用いて移動制御を行うのが一般的である。この移動制御に用いる軌道情報は、軌道情報生成装置によって移動制御の事前に予め生成される。軌道情報生成装置の多くは、特許文献２に例示されるように、ロボットアームの動力学モデルをもとに最適化手法を用いて所望の始点から終点までの移動時間が最短となる適切な軌道情報を生成するのが一般的である。   A transfer device that is a type of moving device is a device that moves a transfer object using a transfer mechanism such as a robot arm as exemplified in Patent Document 1, and relates to a preset trajectory and a speed on the trajectory. In general, movement control is performed using trajectory information. The trajectory information used for the movement control is generated in advance by the trajectory information generation device in advance of the movement control. Many of the trajectory information generation devices, as exemplified in Patent Document 2, use an optimization method based on a dynamic model of a robot arm to provide an appropriate trajectory that minimizes the travel time from a desired start point to an end point. It is common to generate information.

特開２００３−１４５４６１号公報JP 2003-145461 A 特開平７−２０００３０号公報JP-A-7-200030

しかしながら、上記従来の軌道情報生成装置で得られる軌道情報は、移動速度を可能な限り高速化して移動時間を最短とするものであるが、移動によって生ずる振動が考慮されておらず、高速移動に伴い移動制御に無視できない振動が発生する場合があり、発生する振動が問題とならない程度に移動速度を低減させる等の処置を要し、移動効率の低下を招く場合がある。   However, the trajectory information obtained by the conventional trajectory information generation device is intended to increase the moving speed as much as possible to minimize the moving time. Along with this, vibration that cannot be ignored may occur in the movement control, and it is necessary to take measures such as reducing the movement speed to such an extent that the generated vibration does not cause a problem, which may cause a decrease in movement efficiency.

本発明は、このような課題に着目してなされたものであって、その目的は、所望の始点から終点までの移動時間の短縮と移動によって生ずる振動の低減とを両立した軌道情報を生成する新たな軌道情報生成装置を提供することである。   The present invention has been made paying attention to such a problem, and its purpose is to generate trajectory information that achieves both a reduction in travel time from a desired start point to an end point and a reduction in vibration caused by the movement. It is to provide a new trajectory information generation device.

本発明は、かかる目的を達成するために、次のような手段を講じたものである。   In order to achieve this object, the present invention takes the following measures.

すなわち、本発明の移動装置の軌道情報生成装置は、
所望の始点から終点まで移動装置を移動させるための軌道及び軌道上の速度に関する軌道情報を生成する装置であって、始点情報及び終点情報を受け付ける受付手段と、前記受付手段に受け付けられた情報に基づいて始点から終点までの移動に要する時間をパラメータの一つとして含む評価関数の値が最小となるように最適化手法を用いて前記軌道情報を生成する軌道情報生成手段とを具備してなり、前記軌道情報生成手段は、、予め設定された質量−バネモデルからなる移動装置の振動特性モデルを用いて、当該移動装置を移動させる際に生ずるはずの振動である模擬振動の大きさを算出し、算出した模擬振動の大きさをパラメータの一つとして前記評価関数に含め、少なくとも上記２つのパラメータを含む前記評価関数の値が最小となるように前記軌道情報を生成することを特徴とする。 That is, the trajectory information generation device of the mobile device of the present invention is
An apparatus for generating trajectory information related to a trajectory for moving a moving device from a desired start point to an end point and a speed on the trajectory, and a receiving unit that receives start point information and end point information, and information received by the receiving unit And a trajectory information generating means for generating the trajectory information using an optimization method so that the value of the evaluation function including the time required for moving from the start point to the end point as a parameter is minimized. The trajectory information generation means calculates a magnitude of simulated vibration that is vibration that should occur when the moving device is moved , using a vibration characteristic model of the moving device including a preset mass-spring model. The calculated magnitude of the simulated vibration is included in the evaluation function as one of the parameters, and the value of the evaluation function including at least the two parameters is minimized. And generating an urchin the orbit information.

この構成によれば、移動装置を移動させる際に生ずるはずの振動である模擬振動の大きさと移動時間とを合わせた評価関数の値が最小となるように最適化手法を用いて軌道情報を生成するので、振動抑制を考慮したうえで移動時間が最短となり、移動効率を向上させることができる。 According to this configuration, trajectory information is generated using an optimization method so that the value of the evaluation function that combines the magnitude of the simulated vibration, which should be generated when moving the moving device, and the movement time is minimized. As a result, the movement time is minimized in consideration of vibration suppression, and the movement efficiency can be improved.

特に、複数のリンクを回転可能に接続した多関節ロボットを用いて搬送対象物を移動させる移動装置の移動効率を向上させるためには、前記軌道情報生成手段は、前記リンクの回転動作によって生ずるはずの模擬振動の大きさと始点から終点までの移動時間とを含む評価関数の値が最小となるように前記軌道情報を生成することが好ましい。 In particular, in order to improve the movement efficiency of a moving device that moves an object to be conveyed using an articulated robot in which a plurality of links are rotatably connected, the trajectory information generating means should be generated by the rotation operation of the links. Preferably, the trajectory information is generated so that the value of the evaluation function including the magnitude of the simulated vibration and the travel time from the start point to the end point is minimized.

軌道情報の生成に要する演算時間を低減させつつ障害物回避を実現するためには、前記軌道情報生成手段は、矩形状をなす位置情報を制約条件として前記軌道の位置を制約することが望ましい。   In order to achieve obstacle avoidance while reducing the calculation time required to generate the trajectory information, it is desirable that the trajectory information generation means constrains the position of the trajectory using rectangular position information as a constraint condition.

振動抑制又は移動時間低減のいずれを優先するかを容易に変更可能とするためには、変更操作を受け付けて当該変更操作に応じて前記評価関数に対する模擬振動の大きさと移動時間との重み付けを変更する重み付け変更手段を有することが好ましい。 In order to be able to easily change which of vibration suppression or travel time reduction is prioritized, a change operation is accepted, and the weight of the simulated vibration magnitude and the travel time for the evaluation function is changed according to the change operation It is preferable to have a weight changing means .

勿論、上記の軌道情報生成装置の構成をプログラムに対応させることも可能である。 Certainly theory, it is possible to correspond to the program structure of the track information generation device.

本発明は、以上説明したように、移動装置を移動させる際に生ずるはずの振動である模擬振動の大きさと移動時間とを含む評価関数の値が最小となるように最適化手法を用いて軌道情報を生成しているので、所望の始点から終点までの移動時間の短縮と移動によって生ずる振動の低減とを両立することが可能となり、移動効率を向上させることが可能となる。 As described above, the present invention uses an optimization method so that the value of the evaluation function including the magnitude of the simulated vibration , which should be generated when moving the moving device, and the moving time is minimized. Since the information is generated, it is possible to reduce both the movement time from the desired start point to the end point and to reduce the vibration caused by the movement, thereby improving the movement efficiency.

本発明の一実施形態に係る移動装置の軌道情報生成装置を模式的に示す構成図。The block diagram which shows typically the track | orbit information generation apparatus of the moving apparatus which concerns on one Embodiment of this invention. 同移動装置の搬送機構を示す平面図。The top view which shows the conveyance mechanism of the movement apparatus. 振動特性モデルを模試的に示す図。The figure which shows a vibration characteristic model typically. 同移動装置の可動領域を示す図。The figure which shows the movable area | region of the movement apparatus. 生成される軌道情報に関する図。The figure regarding the orbit information produced | generated. 本実施形態に係る装置および従来の装置によってそれぞれ生成された軌道を比較して示す図。The figure which compares and shows the track | orbit respectively produced | generated by the apparatus which concerns on this embodiment, and the conventional apparatus. 本実施形態に係る装置および従来の装置によってそれぞれ生成された軌道情報を用いて移動制御を行った結果を示す図。The figure which shows the result of having performed movement control using the trajectory information each produced | generated by the apparatus which concerns on this embodiment, and the conventional apparatus.

以下、本発明の一実施形態を、図面を参照して説明する。   Hereinafter, an embodiment of the present invention will be described with reference to the drawings.

図１に示すようにロボットアーム等の搬送機構１３１を駆動することにより半導体ウエハー等の搬送対象物Ｗを移動させる移動装置１０３は、予めメモリに記憶されている軌道及び軌道上の速度に関する軌道情報１３２に基づいて搬送対象物Ｗの移動制御を行うものである。その軌道は、図４に示す矩形状の可動領域に隣接配置された複数のロードポート等の入り口同士間を結ぶものである。本実施形態に係る移動装置の軌道情報生成装置２は、図１に示すように、上記移動装置１０３の移動制御に用いられる軌道情報１３２を生成する装置であり、受付手段２１と、軌道情報生成手段２２とを有している。   As shown in FIG. 1, a moving device 103 that moves a transfer object W such as a semiconductor wafer by driving a transfer mechanism 131 such as a robot arm has track information about a track and a speed on the track stored in advance in a memory. The movement control of the conveyance object W is performed based on 132. The track connects the entrances of a plurality of load ports and the like arranged adjacent to the rectangular movable region shown in FIG. The trajectory information generation device 2 of the mobile device according to the present embodiment is a device that generates trajectory information 132 used for movement control of the mobile device 103 as shown in FIG. Means 22.

受付手段２１は、ディスプレイやキーボード、マウス等の既知の操作部を用いて、軌道情報１３２を生成するために必要な軌道の始点や終点、制約条件となる位置情報等の各種情報の入力を受け付けるものである。勿論、受付手段をティーチングペンダント（教示装置）として構成したり、上位システムや支援ツールからの通信による設定を受け付けるようにしたりしてもよい。   The accepting means 21 accepts input of various information such as a starting point and an ending point of the trajectory necessary for generating the trajectory information 132, and positional information serving as a constraint condition using a known operation unit such as a display, a keyboard, and a mouse. Is. Of course, the receiving means may be configured as a teaching pendant (teaching device), or may be configured to receive settings by communication from a host system or a support tool.

軌道情報生成手段２２は、受付手段２１に受け付けられた情報に基づいて始点から終点まで最適な軌道情報を得る最適化処理部２３を有しており、この最適化処理部２３は、ＣＰＵ、メモリ、各種インターフェイス等を備えたパソコン等の情報処理装置においてＣＰＵが予め記憶されている図示しない軌道情報生成処理ルーチンを実行することによりソフトウェア及びハードウェアが協働して実現されるものである。   The trajectory information generation means 22 has an optimization processing section 23 that obtains optimal trajectory information from the start point to the end point based on the information received by the reception means 21. The optimization processing section 23 includes a CPU, a memory, In an information processing apparatus such as a personal computer provided with various interfaces, the CPU and the hardware execute in advance a track information generation processing routine (not shown), and the software and hardware cooperate to realize.

この最適化処理部２３は、予めメモリに記憶されている動力学モデルと振動特性モデルと制約条件とに基づいて導き出せる軌道情報のうち、その移動時間及び移動に伴い生ずる振動が最小となる適切な軌道情報を最適化手法により生成するものである。   The optimization processing unit 23 is an appropriate trajectory information that can be derived based on a dynamic model, a vibration characteristic model, and a constraint condition that are stored in advance in the memory. Orbital information is generated by an optimization method.

以下、具体的に説明すると、駆動対象となる搬送機構１３１は、図２に示すように、複数のリンク１３４・１３５・１３６を回転可能に直列接続した、いわゆる多関節ロボットアームであり、ロボットアームの先端に設定された保持部１３６ｂに搬送対象物Ｗを載置した状態で各関節にある図示しないモータの駆動により各々のリンク間の角度θ_１・θ_２・θ_３を変更して搬送対象物Ｗを所望の位置に移動させるものである。本実施形態の搬送機構１３１は、ベース１３３に基端１３４ａが接続された第１のリンク１３４と、第１のリンク１３４の先端１３４ｂに基端１３５ａが接続された第２のリンク１３５と、第２のリンク１３５の先端１３５ｂに基端１３６ａが接続され先端に搬送対象物Ｗを載置するための保持部１３６ｂが設定された第３のリンク１３６とを備え、各リンク１３４〜１３６はそれぞれ水平方向に回転可能に接続されて三軸水平多関節ロボットを構成している。各リンク１３４〜１３６の長さはそれぞれＬ_１、Ｌ_２、Ｌ_３に設定してある。 Specifically, the transport mechanism 131 to be driven is a so-called articulated robot arm in which a plurality of links 134, 135, and 136 are rotatably connected in series as shown in FIG. The object to be conveyed is changed by changing the angles θ ₁ , θ _2, and θ ₃ between the links by driving a motor (not shown) in each joint in a state where the object to be conveyed W is placed on the holding portion 136b set at the tip of the link. The object W is moved to a desired position. The transport mechanism 131 of the present embodiment includes a first link 134 having a base end 134a connected to the base 133, a second link 135 having a base end 135a connected to the tip 134b of the first link 134, And a third link 136 in which a base 136a is connected to the distal end 135b of the second link 135 and a holding portion 136b for placing the object to be conveyed W is set at the distal end. The links 134 to 136 are each horizontal. A three-axis horizontal articulated robot is configured so as to be rotatable in the direction. The lengths of the links 134 to 136 are set to L ₁ , L ₂ , and L ₃ , respectively.

この搬送機構１３１の動力学モデルは、各リンク１３４・１３５・１３６の質量分布がリンク先端での集中負荷であるとして簡略的に以下の式で表される。なお、ｉはリンクの番号を示し、例えば、ｉ＝１は第１のリンク１３４、ｉ＝２は第２のリンク１３５、ｉ＝３は第３のリンク１３６である。ここでは各リンクの質量分布がリンク先端での集中負荷であるとして式を構成しているが、勿論、各リンクの質量分布がリンクの重心周りにあるとして式を構成してもよい。

Ｊ＝ｄｉａｇ｛Ｊ_ｉ｝， Ｊ_ｉ＞０
Ｇ＝ｄｉａｇ｛Ｇ_ｉ｝， Ｇ_ｉ＞０
ｉ＝１，２，３
θ＝［θ_１，θ_２，θ_３］^Ｔ

は加速度を表す。
Ｇ_ｉは第ｉ関節を駆動するモータに取り付けられた減速器のギア比である。
各関節のモータの駆動トルク：
τ＝［τ_１，τ_２，τ_３］^Ｔ
各関節のモータの慣性モーメント：
Ｊ_１＝（Ｇ_１）^２Ｉ_１＋ｍ_１Ｌ_１ ^２＋ｍ_２（Ｌ_１＋Ｌ_２）^２＋ｍ_３（Ｌ_１＋Ｌ_２＋Ｌ_３）^２
Ｊ_２＝（Ｇ_２）^２Ｉ_２＋ｍ_２Ｌ_２ ^２＋ｍ_３（Ｌ_２＋Ｌ_３）^２
Ｊ_３＝Ｊ_ｍ＋Ｊ_ａ
Ｊ_ｍ＝（Ｇ_３）^２Ｉ_３
Ｊ_ａ＝ｍ_３Ｌ_３ ^２
Ｉ_ｉは第ｉ関節を駆動するモータの慣性モーメント。
ｍ_ｉは各リンクの質量。
なお、この動力学モデルに係る情報は、図１に示すように、予め軌道情報生成手段２２のメモリに記憶されているが、受付手段２１を通じて設定可能に構成してもよい。 The dynamic model of the transport mechanism 131 is simply expressed by the following equation assuming that the mass distribution of each link 134, 135, 136 is a concentrated load at the link tip. Note that i indicates a link number. For example, i = 1 is the first link 134, i = 2 is the second link 135, and i = 3 is the third link 136. Here, the formula is constructed assuming that the mass distribution of each link is a concentrated load at the link tip, but the formula may of course be constructed assuming that the mass distribution of each link is around the center of gravity of the link.

J = diag {J _i }, J _i > 0
G = diag {G _i }, G _i > 0
i = 1, 2, 3
θ = [θ ₁ , θ ₂ , θ ₃ ] ^T

Represents acceleration.
G _i is the gear ratio of the speed reducer attached to the motor that drives the i-th joint.
Driving torque of each joint motor:
τ = [τ ₁ , τ ₂ , τ ₃ ] ^T
Moment of inertia of motor at each joint:
J ₁ = (G ₁ ) ² I ₁ + m ₁ L ₁ ² + m ₂ (L ₁ + L ₂ ) ² + m ₃ (L ₁ + L ₂ + L ₃ ) ²
J ₂ = (G ₂ ) ² I ₂ + m ₂ L ₂ ² + m ₃ (L ₂ + L ₃ ) ²
J ₃ = J _m + J _a
J _m = (G ₃ ) ² I ₃
J _a = m ₃ L ₃ ²
I _i is the moment of inertia of the motor that drives the i-th joint.
_mi is the mass of each link.
As shown in FIG. 1, the information related to the dynamic model is stored in advance in the memory of the trajectory information generation unit 22, but may be configured to be set through the reception unit 21.

移動により生ずる振動を考慮するための振動特性モデルは、図３に示すような質量−バネモデルが適用されており、以下の式で表される。

Ｊ_ｍ、Ｊ_ａはそれぞれ第３のリンク１３６のモータの慣性モーメント、エンドエフェクタ（保持部１３６ｂ）の先端に集中質量があるとした場合の慣性モーメントである。バネ定数Ｋは、実験により計測した固有振動数ｆ及び下記の式を用いて算出する。
Ｋ＝Ｊ_ａ（２πｆ）^２
なお、この振動特性モデルに係る情報は、図１に示すように、予め軌道情報生成手段２２のメモリに記憶されているが、受付手段２１を通じて設定可能に構成してもよい。 A mass-spring model as shown in FIG. 3 is applied as a vibration characteristic model for considering vibration caused by movement, and is represented by the following equation.

J _m and J _a are the moment of inertia of the motor of the third link 136 and the moment of inertia when there is a concentrated mass at the tip of the end effector (holding portion 136b), respectively. The spring constant K is calculated using the natural frequency f measured by experiment and the following equation.
K = J _a (2πf) ²
As shown in FIG. 1, the information relating to the vibration characteristic model is stored in advance in the memory of the trajectory information generation unit 22, but may be configured to be settable through the reception unit 21.

軌道情報の制約条件の一つとして、各リンク１３４・１３５・１３６を回転させるモータの駆動制約が図１に示すメモリに予め設定されている。このモータの駆動制約はモータが出力可能な最大トルクを用いており、以下の式で示される。τ_ｌｉは逆回転（θ_ｉが負の方向に駆動しようと）する場合の最大トルクであり、τ_ｕｉは正回転（θ_ｉが正の方向に駆動しようと）する場合の最大トルクを示す。勿論、速度や加速度で表現されるものであってもよい。

As one of the restriction conditions of the trajectory information, the drive restriction of the motor that rotates each link 134, 135, 136 is preset in the memory shown in FIG. This motor drive constraint uses the maximum torque that the motor can output, and is expressed by the following equation. τ _li is the maximum torque in the case of reverse rotation (attempts to drive θ _i in a negative direction), and τ _ui represents the maximum torque in the case of forward rotation (attempts to drive θ _i in a positive direction). Of course, it may be expressed by speed or acceleration.

また、本実施形態では、搬送機構１３１が図４（ａ）に示す矩形状の可動領域内で動作するものであるので、以下の式で示される矩形状をなす位置情報を軌道の位置の制約条件の一つとしている。

（ｘ_ｉ，ｙ_ｉ）は図４（ｂ）に示すように各関節の位置を示し、搬送対象物Ｗの中心位置（ｘ_３，ｙ_３）は、搬送対象物Ｗが可動範囲から外れないように半径ｒ分狭くしている。
なお、この可動領域としての制約条件に係る情報は、図１に示すように、予め軌道情報生成手段２２のメモリに記憶されているが、受付手段２１を通じて設定可能に構成している。 In this embodiment, since the transport mechanism 131 operates within the rectangular movable region shown in FIG. 4A, the rectangular position information represented by the following equation is used as a restriction on the position of the trajectory. One of the conditions.

(X _i , y _i ) indicates the position of each joint as shown in FIG. 4B, and the center position (x ₃ , y ₃ ) of the transport target W does not deviate from the movable range. As shown in FIG.
As shown in FIG. 1, the information related to the constraint condition as the movable region is stored in advance in the memory of the trajectory information generation unit 22, but can be set through the reception unit 21.

受付手段２１で受け付けられた始点及び終点の二つの位置情報は、ＸＹ座標で示される始点位置及び終点位置、並びに始点速度及び終点速度であるが、搬送対象物Ｗの移動は、各リンク１３４〜１３６の角度θ_１・θ_２・θ_３を変化させることにより行うので、始点及び終点に関する情報を以下の式で示すように角度情報に変換し、これを制約条件の一つとしている。なお、本実施形態では、始点速度及び終点速度を０として取り扱っている。

は速度、ｔ_ｅは移動時間を表す。θ_ｓ，θ_ｅは始点及び終点位置の情報を関節角度の情報（関節角度ベクトル）に変換したものである。 The two pieces of position information of the start point and the end point received by the receiving unit 21 are the start point position and the end point position indicated by the XY coordinates, and the start point speed and the end point speed. Since the angle θ ₁ · θ ₂ · θ ₃ of 136 is changed, the information about the start point and the end point is converted into angle information as shown in the following equation, which is one of the constraint conditions. In the present embodiment, the start point speed and the end point speed are treated as zero.

Speed, _{t e} represents the travel time. θ _s and θ _e are information obtained by converting the information of the start point and the end point into information of the joint angle (joint angle vector).

上記のモデル及び制約条件の下で、以下の式で示される評価関数の値が最小となるように軌道及び軌道上の速度に関する軌道情報を最適化により求める。

この評価関数は、振動特性モデルで算出される、移動装置１０３を移動させる際に生ずるはずの振動である模擬振動の大きさ（θ_３−θ_ａ）をパラメータの一つとし、移動時間ｔ_ｅをパラメータの一つとして、これら２つのパラメータを少なくとも含む関数である。具体的には、模擬振動の大きさ（θ_３−θ_ａ）に重み付け情報である重み付け係数αを乗じて移動時間ｔ_ｅと結合したものである。なお、最適化手法については一般的な手法を用いているので、ここでは説明を省略する。また、図１に示すように、この重み付け係数αを、受付手段２１になされる変更操作に応じて変更する重み付け変更手段２４が設けられており、重み付け係数αを変更することで振動抑制又は最短時間短縮のいずれかを優先するかを変更することが可能となる。本実施形態では、模擬振動の大きさ（θ_３−θ_ａ）が負の値を含むため、これを二乗して正の値にしているが、（θ_３−θ_ａ）の絶対値を取る式にしてもよい。 Under the above model and constraint conditions, trajectory information related to the trajectory and the velocity on the trajectory is obtained by optimization so that the value of the evaluation function represented by the following equation is minimized.

The evaluation function is calculated by the vibration characteristic model, the magnitude of a vibration simulate vibration which would occur when moving the moving apparatus 103 (θ ₃ -θ _a) as one of the parameters, the travel time t _e Is a function including at least these two parameters. Specifically, those bound to travel time t _e is multiplied by the a weighting information on the size of the simulated vibration (θ ₃ -θ _a) weighting factor alpha. In addition, since the general method is used about the optimization method, description is abbreviate | omitted here. Further, as shown in FIG. 1, there is provided a weight changing means 24 for changing the weighting coefficient α in accordance with a changing operation performed by the receiving means 21. By changing the weighting coefficient α, vibrations can be suppressed or minimized. It is possible to change whether priority is given to time reduction. In this embodiment, since the magnitude (θ ₃ −θ _a ) of the simulated vibration includes a negative value, this is squared to obtain a positive value, but the absolute value of (θ ₃ −θ _a ) is taken. It may be a formula.

ここで、上記の最適化問題を解くにあたり演算時間を低減するために離散化を考え、これに伴い軌道情報１３２も離散化して生成している。具体的には、軌道情報１３２は、図５に模式的に示すように、移動軌道Ｐｔを、時間間隔Ｔで（Ｎ−１）等分し、始点Ｓ及び終点Ｅを含む複数の点Ｐ_１〜Ｎで表現する離散データであり、各々の点Ｐ_ｋのｘｙ座標（ｘ_ｋ，ｙ_ｋ）を、以下の式を用いて各リンク１３４〜１３６の角度θ_１ｋ・θ_２ｋ・θ_３ｋで表現している。
ｘ_ｋ＝Ｌ_１ｃｏｓθ_１ｋ＋Ｌ_２ｃｏｓ（θ_１ｋ＋θ_２ｋ）＋Ｌ_３ｃｏｓ（θ_１ｋ＋θ_２ｋ＋θ_３ｋ）
ｙ_ｋ＝Ｌ_１ｓｉｎθ_１ｋ＋Ｌ_２ｓｉｎ（θ_１ｋ＋θ_２ｋ）＋Ｌ_３ｓｉｎ（θ_１ｋ＋θ_２ｋ＋θ_３ｋ） Here, discretization is considered in order to reduce the calculation time in solving the above optimization problem, and the trajectory information 132 is also discretized and generated accordingly. Specifically, the trajectory information 132, as schematically shown in FIG. 5, divides the moving trajectory Pt into (N−1) equal time intervals T, and includes a plurality of points P ₁ including a start point S and an end point E. _Are expressed as _˜N , and the xy coordinates (x _k , y _k ) of each point P _k are expressed by the angles θ _1k · θ _2k · θ _3k of the links 134 to 136 using the following equations. doing.
x _k = L ₁ cos θ _1k + L ₂ cos (θ _1k + θ _2k ) + L ₃ cos (θ _1k + θ _2k + θ _3k )
y _k = L ₁ sin θ _1k + L ₂ sin (θ _1k + θ _2k ) + L ₃ sin (θ _1k + θ _2k + θ _3k )

そして、上記で述べたモデル及び制約条件を中心差分法を用いて離散化すると以下の式で表される。なお、以下では、ｋ時点目の点Ｐ_ｋの角度θ_ｉｋをθ_ｉ［ｋ］と略記する。
振動を考慮したモデル：

モータの駆動制約：

τ_ｉ［ｋ］は、ｋ時点目の第ｉ関節の駆動トルクを表す。
軌道の位置の制約条件：

ｘ_ｉ［ｋ］，ｙ_ｉ［ｋ］は、ｋ時点目の位置ｘ_ｉ，ｙ_ｉを表す。
始点及び終点の制約条件：

評価関数：

Then, when the model and the constraint condition described above are discretized using the central difference method, it is expressed by the following expression. Hereinafter, the angle theta _ik point _{P k} of the k time th abbreviated as θ _i [k].
Model considering vibration:

Motor drive constraints:

τ _i [k] represents the driving torque of the i-th joint at the k-th time point.
Orbital position constraints:

x _i [k] and y _i [k] represent the positions x _i and y _i at the k-th time point.
Start and end point constraints:

Evaluation function:

上記のモデル及び制約条件を用いて軌道情報１３２を生成するにあたり、具体的には、初期値として始点Ｓから終点Ｅまで直線で結んだ軌道情報を生成し、この初期軌道をもとに上記モデル及び制約条件に基づいて既知の逐次二次計画法を用いた最適化計算を行い、リンク角度θ_ｉ［ｋ］及び時間間隔Ｔを求める。 When the trajectory information 132 is generated using the above model and constraint conditions, specifically, trajectory information connected as a straight line from the start point S to the end point E is generated as an initial value, and the model is based on the initial trajectory. Then, an optimization calculation using a known sequential quadratic programming method is performed based on the constraint condition, and the link angle θ _i [k] and the time interval T are obtained.

ここで、従来の軌道情報生成装置との比較実験を行うために、上記重み付け係数αを０として従来の最短時間のみを追求した図６に示す軌道Ｐｔ’と、本実施形態に係る振動抑制を考慮した図６に示す軌道Ｐｔとを生成し、それぞれの軌道情報を駆動指令に変換してロボットアームを駆動させ、移動によって発生する振動を計測したところ、図７に示すように、移動に要する時間にほとんど変化がなかったものの、従来の軌道情報に比べて本実施形態の軌道情報は発生する振動が少なくとも半分以下に抑えられていることがわかる。   Here, in order to perform a comparison experiment with the conventional trajectory information generation apparatus, the trajectory Pt ′ shown in FIG. 6 in which the weighting coefficient α is set to 0 and only the conventional shortest time is pursued, and the vibration suppression according to the present embodiment are suppressed. The trajectory Pt shown in FIG. 6 considered is generated, the respective trajectory information is converted into drive commands, the robot arm is driven, and the vibration generated by the movement is measured. As shown in FIG. Although there is almost no change in time, it can be seen that the generated vibration is suppressed to at least half or less in the track information of this embodiment compared to the conventional track information.

また、ロボットの関節毎に振動モデルを定義する等の厳密な運動特性を用いると計算量が莫大となり実運用に好ましくなかったが、本実施形態のように単一の質量−バネモデルに簡略化することで実用性を保つ範囲内で計算量を低減している。また、一般的に障害物回避には莫大な計算を要するものであるが、本実施形態のように移動装置の可動範囲が矩形状であることに着目し、矩形状をなす位置情報を制約条件の一つとしているので、同様に実用性を保つ範囲内で計算量を低減している。軌道情報を生成するのに要する時間は、環境により種々変化するが、一例であるが、ある環境では従来に比べて数十〜数百分の１程度の時間に短縮することができた。   In addition, using strict motion characteristics such as defining a vibration model for each joint of the robot is not preferable for actual operation because the calculation amount is enormous, but it is simplified to a single mass-spring model as in this embodiment. Therefore, the amount of calculation is reduced within the range that keeps practicality. In general, a large amount of calculation is required for obstacle avoidance. However, focusing on the fact that the movable range of the moving device is rectangular as in this embodiment, the position information that forms a rectangle is a constraint condition. Therefore, the amount of calculation is reduced within a range that keeps practicality. The time required to generate the trajectory information varies depending on the environment, but is only an example, but in some environments, the time can be reduced to about several tens to several hundreds of times compared to the conventional case.

以上のように本実施形態に係る移動装置の軌道情報生成装置は、所望の始点Ｓから終点Ｅまで移動装置１０３を移動させるための軌道及び軌道上の速度に関する軌道情報１３２を生成する装置であって、始点情報及び終点情報を受け付ける受付手段２１と、受付手段２１に受け付けられた情報に基づいて始点Ｓから終点Ｅまでの移動に要する時間ｔ_ｅをパラメータの一つとして含む評価関数の値が最小となるように最適化手法を用いて軌道情報１３２を生成する軌道情報生成手段２２とを具備してなり、軌道情報生成手段２２は、予め設定された移動装置の振動特性を用いて移動により生ずるはずの振動である模擬振動の大きさ（θ_３−θ_ａ）を算出し、算出した模擬振動の大きさ（θ_３−θ_ａ）をパラメータの一つとして評価関数に含め、少なくとも上記２つのパラメータである移動時間ｔ_ｅ及び模擬振動の大きさ（θ_３−θ_ａ）を含む評価関数の値が最小となるように軌道情報を生成する。 As described above, the trajectory information generation device of the mobile device according to the present embodiment is a device that generates the trajectory information 132 relating to the trajectory for moving the mobile device 103 from the desired start point S to the end point E and the speed on the trajectory. Te, a receiving unit 21 that receives the point information and end point information, the value of the evaluation function including time t _e it takes to move from the start point S to the end point E on the basis of the information received in the receiving means 21 as one of the parameters The trajectory information generating means 22 generates trajectory information 132 using an optimization method so as to minimize the trajectory information generating means 22, and the trajectory information generating means 22 uses a predetermined vibration characteristic of the moving device. calculating the magnitude of the simulated vibration is a vibration which should produce (θ ₃ -θ _a), containing the evaluation function the size of the calculated simulated vibrated (θ ₃ -θ _a) as one of the parameters , It generates the trajectory information so that the value of the evaluation function including at least the two parameters a travel time t _e and the simulated vibration magnitude is a (θ ₃ -θ _a) is minimized.

このように、移動により生ずるはずの模擬振動の大きさ（θ_３−θ_ａ）と移動時間ｔ_ｅとを合わせた評価関数の値が最小となるように逐次二次計画法を始めとする最適化手法を用いて軌道情報１３２を生成するので、振動抑制を考慮したうえで移動時間が最短となり、移動効率を向上させることが可能となる。 Best to this manner, the magnitude of the simulated vibration that would occur by moving (θ ₃ -θ _a) and as the value of the evaluation function obtained by combining the travel time t _e is minimized sequentially started quadratic programming Since the trajectory information 132 is generated using the optimization method, the movement time is minimized in consideration of vibration suppression, and the movement efficiency can be improved.

さらに、本実施形態では、複数のリンク１３４・１３５・１３６を回転可能に接続した多関節ロボットたる搬送機構１３１を用いて搬送対象物Ｗを移動させる移動装置１０３を駆動させるための軌道情報１３２を生成するものであり、軌道情報生成手段２２が、リンク１３４〜１３６の回転動作によって生ずるはずの模擬振動の大きさ（θ_３−θ_ａ）と始点Ｓから終点Ｅまでの移動時間ｔ_ｅとを含む評価関数の値が最小となるように軌道情報を生成するので、複雑な制御を要する多関節ロボットに適した軌道及びその軌道上の速度を選定して、軌道効率を向上させることが可能となる。 Furthermore, in this embodiment, the trajectory information 132 for driving the moving device 103 that moves the conveyance target W using the conveyance mechanism 131 that is an articulated robot in which a plurality of links 134, 135, and 136 are rotatably connected is provided. is intended to be generated, orbit information generating means 22, the magnitude of the simulated vibration that would occur by rotation of the link 134~136 (θ ₃ -θ _a) and the moving time from the start point S to the end point E _{t e} Since the trajectory information is generated so that the value of the evaluation function including it is minimized, it is possible to improve trajectory efficiency by selecting a trajectory suitable for an articulated robot that requires complex control and the speed on the trajectory. Become.

さらにまた、本実施形態では、軌道情報生成手段２２は、矩形状をなす位置情報を制約条件として軌道の位置を制約しているので、簡易な形状であるものの障害物回避を実現することが可能となる。しかも、制約条件が矩形状であるので、複雑な形状である場合に比べて軌道情報の生成に要する演算時間を著しく低減させることが可能となる。   Furthermore, in the present embodiment, the trajectory information generation means 22 restricts the position of the trajectory using the rectangular position information as a constraint condition, so it is possible to achieve obstacle avoidance with a simple shape. It becomes. In addition, since the constraint condition is a rectangular shape, it is possible to significantly reduce the calculation time required for generating the trajectory information compared to the case of a complicated shape.

加えて、本実施形態では、受付手段２１を介して変更操作を受け付けて変更操作に応じて評価関数に対する模擬振動の大きさ（θ_３−θ_ａ）と移動時間ｔ_ｅとの重み付けを表す重み付け係数αを変更する重み付け変更手段２４を有するので、変更操作を行うことで、評価関数に対する模擬振動（θ_３−θ_ａ）と移動時間ｔ_ｅとの重み付けを表す重み付け係数αが変更され、振動抑制又は移動時間低減のいずれを優先するかを変更することが可能となる。しかも、ワンパラメータとなるので、パラメータ設定が取り扱いやすくなる。 In addition, in the present embodiment, weighting represents the weighting of the simulated vibration magnitude for the evaluation function in response to a change accepting change operation via the receiving unit 21 operation (θ ₃ -θ _a) and travel time t _e because it has a weighting changing means 24 for changing the coefficient alpha, by performing the change operation, the weighting coefficients representing the weights of the simulated vibration (θ ₃ -θ _a) and travel time t _e for the evaluation function alpha is changed, the vibration It is possible to change which of priority is given to suppression or travel time reduction. Moreover, since it is a one parameter, it is easy to handle parameter setting.

従来では、新たなロードポート等が追加されて始点又は終点が追加された場合に、かかる始点又は終点に関する情報を入力して適切な軌道情報を生成するにあたり、始点及び終点以外に軌道上の経由点をユーザが指定するものであったが、本実施形態では、始点情報及び終点情報を受け付ける受付手段２１と、受付手段２１に受け付けられた始点Ｓ及び終点Ｅの二つの位置情報から二点間の軌道Ｐｔ及びその軌道上の速度に関する軌道情報１３２を生成する軌道情報生成手段２２と有するので、始点Ｓ及び終点Ｅ以外に経由点を指定することなく軌道情報が生成され、簡易な操作で適切な軌道情報を生成することが可能となり、例えば新たなロードポート等を追加した場合でも容易な対応を実現することができる。   Conventionally, when a new load port or the like is added and a start point or end point is added, information on the start point or end point is input to generate appropriate trajectory information. The point is specified by the user, but in this embodiment, the receiving unit 21 that receives the starting point information and the ending point information, and the two points of the starting point S and the end point E that are received by the receiving unit 21 Trajectory information generating means 22 for generating trajectory information 132 relating to the trajectory Pt and the speed on the trajectory, so that trajectory information can be generated without designating a transit point in addition to the start point S and the end point E. Trajectory information can be generated. For example, even when a new load port or the like is added, an easy response can be realized.

さらに、軌道情報生成手段２２は、矩形状をなす位置情報を制約条件として軌道の位置を制約するので、適切な障害物回避を実現している。また、受付手段２１が、移動時間ｔ_ｅに対する移動により生ずる振動の大きさ（θ_３−θ_ａ）の重み付け情報を受け付け、軌道情報生成手段２２が、受付手段２１に受け付けられた情報に応じた重み付けとなるように軌道情報１３２を生成するので、振動抑制又は移動時間低減のいずれを優先するかを容易に変更可能としている。 Furthermore, since the trajectory information generation means 22 restricts the position of the trajectory using rectangular position information as a constraint condition, appropriate obstacle avoidance is realized. Furthermore, acceptance means 21 accepts the weighting information for the magnitude of the vibration generated by the movement against the travel time _{t e (θ 3 -θ a)} , orbit information generating means 22, in accordance with the information received in the receiving means 21 Since the trajectory information 132 is generated so as to be weighted, it is possible to easily change which of vibration suppression or movement time reduction is prioritized.

その他、上記軌道情報を生成するにあたり、以下のプログラムが有用である。   In addition, the following programs are useful in generating the trajectory information.

所望の始点から終点まで移動装置を移動させるための軌道及び軌道上の速度に関する軌道情報を生成する機能をコンピュータに実現させるプログラムであって、始点情報及び終点情報を受け付ける受付機能と、受付機能に受け付けられた情報に基づいて始点から終点までの移動に要する時間をパラメータの一つとして含む評価関数の値が最小となるように最適化手法を用いて軌道情報を生成する軌道情報生成機能とを実現し、軌道情報生成機能は、予め設定された移動装置の振動特性を用いて移動により生ずるはずの模擬振動を算出し、算出した模擬振動をパラメータの一つとして評価関数に含め、少なくともこれら２つのパラメータを含む評価関数の値が最小となるように軌道情報を生成する移動装置の軌道情報生成プログラム。 A program for causing a computer to generate a trajectory information about a trajectory for moving the moving device from a desired start point to an end point and a trajectory information on the trajectory. A trajectory information generation function that generates trajectory information using an optimization method so that the value of the evaluation function including the time required for movement from the start point to the end point as one of the parameters based on the received information is minimized. The trajectory information generation function realizes simulated vibration that should be generated by movement using the vibration characteristics of the mobile device set in advance, and includes the calculated simulated vibration in the evaluation function as one of the parameters. A trajectory information generation program for a mobile device that generates trajectory information so that the value of an evaluation function including two parameters is minimized.

所望の始点から終点まで移動装置を移動させるための軌道及び軌道上の速度に関する軌道情報を生成する機能をコンピュータに実現させるプログラムであって、始点情報及び終点情報を受け付ける受付機能と、受付機能に受け付けられた始点及び終点の二つの位置情報から二点間の軌道及びその軌道上の速度に関する軌道情報を生成する軌道情報生成機能とを実現する移動装置の軌道情報生成プログラム。   A program for causing a computer to generate a trajectory information related to a trajectory for moving a moving device from a desired start point to an end point and a speed on the trajectory, and a reception function for receiving start point information and end point information, and a reception function A trajectory information generation program for a mobile device that realizes a trajectory information generation function for generating trajectory information related to a trajectory between two points and a speed on the trajectory from the received two position information of a start point and an end point.

以上、本発明の実施形態について図面に基づいて説明したが、具体的な構成は、これらの実施形態に限定されるものでないと考えられるべきである。本発明の範囲は、上記した実施形態の説明だけではなく特許請求の範囲によって示され、さらに特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれる。   As mentioned above, although embodiment of this invention was described based on drawing, it should be thought that a specific structure is not limited to these embodiment. The scope of the present invention is shown not only by the above description of the embodiments but also by the scope of claims for patent, and further includes all modifications within the meaning and scope equivalent to the scope of claims for patent.

例えば、本実施形態では、ロボットアームの振動特性モデルを定義するにあたり、アーム先端部分に着目して単一の質量−バネモデルとしているが、アームの根元部分に着目してアーム全体を単一の質量−バネモデルとして定義してもよく、振動を考慮するモデルとして他のモデルを採用してもよい。また、本実施形態では、始点速度及び終点速度を０として取り扱っているが、これら速度が０でない場合にも適用可能である。   For example, in this embodiment, in defining the vibration characteristic model of the robot arm, a single mass-spring model is formed by focusing on the tip of the arm. -You may define as a spring model and may employ | adopt another model as a model which considers a vibration. Further, in this embodiment, the start point speed and the end point speed are handled as 0, but the present invention can also be applied when these speeds are not 0.

その他、上記に述べた軌道情報生成は、複数のリンクを回転可能に直列接続したロボットアーム式の搬送機構１３１を用いて搬送対象物Ｗを移動する移動装置１０３に適用しているが、複数のリンクを並列接続したパラレルマニピュレータ等の搬送装置など、ロボットアーム式以外の移動装置にも適用可能である。さらには装置自体が移動する移動装置にも適用可能である。図１に示す各機能部は、所定プログラムをプロセッサで実行することにより実現しているが、各機能部を専用回路で構成してもよい。   In addition, the trajectory information generation described above is applied to the moving device 103 that moves the conveyance object W using a robot arm type conveyance mechanism 131 in which a plurality of links are rotatably connected in series. The present invention can also be applied to a moving device other than a robot arm type, such as a transport device such as a parallel manipulator in which links are connected in parallel. Furthermore, the present invention can be applied to a moving device in which the device itself moves. Each functional unit shown in FIG. 1 is realized by executing a predetermined program by a processor, but each functional unit may be configured by a dedicated circuit.

各部の具体的な構成は、上述した実施形態のみに限定されるものではなく、本発明の趣旨を逸脱しない範囲で種々変形が可能である。   The specific configuration of each unit is not limited to the above-described embodiment, and various modifications can be made without departing from the spirit of the present invention.

１０３…移動装置
１３１…搬送機構（多関節ロボット）
１３２…軌道情報
１３４・１３５・１３６…リンク
２１…受付手段
２２…軌道情報生成手段
２４…重み付け変更手段
Ｓ…始点
Ｅ…終点
Ｗ…搬送対象物
Ｐｔ…軌道
ｔ_ｅ…移動時間
（θ_３−θ_ａ）…模擬振動の大きさ 103 ... Moving device 131 ... Conveying mechanism (articulated robot)
132 ... orbit information 134, 135, 136 ... link 21 ... receiving unit 22 ... orbit information generating means 24 ... weighting changing unit S ... starting E ... end point W ... object to be transported Pt ... trajectory _{t e} ... moving time (theta ₃ - [theta] _a ) The magnitude of the simulated vibration

Claims (4)

所望の始点から終点まで移動装置を移動させるための軌道及び軌道上の速度に関する軌道情報を生成する装置であって、
始点情報及び終点情報を受け付ける受付手段と、
前記受付手段に受け付けられた情報に基づいて始点から終点までの移動に要する時間をパラメータの一つとして含む評価関数の値が最小となるように最適化手法を用いて前記軌道情報を生成する軌道情報生成手段とを具備してなり、
前記軌道情報生成手段は、予め設定された質量−バネモデルからなる移動装置の振動特性モデルを用いて、当該移動装置を移動させる際に生ずるはずの振動である模擬振動の大きさを算出し、算出した模擬振動の大きさをパラメータの一つとして前記評価関数に含め、少なくとも上記２つのパラメータを含む前記評価関数の値が最小となるように前記軌道情報を生成することを特徴とする移動装置の軌道情報生成装置。 A device for generating trajectory information related to a trajectory for moving the moving device from a desired start point to an end point and a speed on the trajectory,
Receiving means for receiving start point information and end point information;
A trajectory for generating the trajectory information using an optimization method so that the value of the evaluation function including the time required for movement from the start point to the end point as one of the parameters is minimized based on the information received by the accepting unit Comprising information generating means,
The trajectory information generation means calculates a magnitude of simulated vibration that is vibration that should be generated when the moving device is moved , using a vibration characteristic model of the moving device that includes a preset mass-spring model. A magnitude of the simulated vibration is included in the evaluation function as one of the parameters, and the trajectory information is generated so that the value of the evaluation function including at least the two parameters is minimized. Orbit information generator. 複数のリンクを回転可能に接続した多関節ロボットを用いて搬送対象物を移動させる移動装置を駆動させるための軌道及び軌道上の速度に関する軌道情報を生成する装置であって、
前記軌道情報生成手段は、前記リンクの回転動作によって生ずるはずの模擬振動の大きさと始点から終点までの移動時間とを含む評価関数の値が最小となるように前記軌道情報を生成する請求項１に記載の移動装置の軌道情報生成装置。 A device that generates trajectory information related to a trajectory and a speed on the trajectory for driving a moving device that moves a conveyance target using an articulated robot in which a plurality of links are rotatably connected,
The trajectory information generation means generates the trajectory information so that a value of an evaluation function including a magnitude of a simulated vibration that should be generated by a rotation operation of the link and a moving time from a start point to an end point is minimized. The trajectory information generation device for the mobile device described in 1. 前記軌道情報生成手段は、矩形状をなす位置情報を制約条件として前記軌道の位置を制約する請求項１又は２に記載の移動装置の軌道情報生成装置。   The trajectory information generation device of the mobile device according to claim 1, wherein the trajectory information generation means restricts the position of the trajectory using rectangular position information as a constraint condition. 変更操作を受け付けて当該変更操作に応じて前記評価関数に対する模擬振動の大きさと移動時間との重み付けを変更する重み付け変更手段を有する請求項１〜３のいずれかに記載の移動装置の軌道情報生成装置。   The trajectory information generation of the mobile device according to any one of claims 1 to 3, further comprising weight changing means for receiving a change operation and changing a weight of a magnitude of a simulated vibration and a movement time for the evaluation function in accordance with the change operation. apparatus.

Images