JPH0969005A - サーボ系の制御装置および制御方法 - Google Patents

Abstract

(57)【要約】 【課題】任意の線分組合わせに対して二つの線分間で経
路変更を行なえ、タクトタイムを低減でき、プログラミ
ングを簡略化する。 【解決手段】 経由目標地点である第２地点と第３地点
を結ぶ線分を含む直線をＸ軸とし、第１地点、第２地点
及び第３地点を含む平面上の直線であって、Ｘ軸と直交
する直線をＹ軸とする経路座標系を基準座標系に対して
仮定し、第１地点を初期位置とし、第３地点を目標位置
とし、かつ、第１地点と第２地点を結ぶ直線上の分岐点
から円弧を描いてＸ軸上に経路を移行させるべく初期位
置から目標位置に向かう際の分岐点における初速度、目
標速度、目標加速度をパラメータとして時間関数を生成
し、この時間関数に基づいて第１地点から第３地点まで
被制御装置を直線軌跡制御によりサーボ駆動するので、
経路途中で停止することなく略Ｌ字上の軌跡を描いて被
制御装置を駆動でき、タクトタイムを低減し、プログラ
ムを簡略化できる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明はサーボ系の制御装置
および制御方法に係り、特に直線軌跡制御を行なうサー
ボ系の制御装置および制御方法に関する。

【０００２】ロボットやＮＣ（Numerical Control ）工
作機械等のサーボ制御機構を備える装置において、サー
ボ制御系の目標位置（動作制御位置）を正確に生成する
ことは、組立や加工精度に直接的な影響を及ぼすため、
重要な課題となっている。

【０００３】しかしながら、目標位置を正確に生成する
だけでは、作業時間（タクトタイム）が長くなることと
なり必ずしも効率がよくないという不都合が生じる。そ
こで、このようなサーボ制御機構を備える装置におい
て、目標位置を正確に生成するばかりでなく、タクトタ
イムを低減することが望まれている。

【０００４】

【従来の技術】従来のサーボ制御機構を備える装置にお
いて、直線軌跡制御は最も多用される制御方法である。

【０００５】一般にアーチモーションと呼ばれる動作で
は、ピックアンドプレース動作に用いる比較的単純な線
分（運動軌跡）の組合わせを連続的に結ぶことが可能で
ある。

【０００６】例えば図１２に示すように、ピックアンド
プレース動作に用いる直線軌跡の組合わせにより制御を
行なう場合、各結合点Ｐ₁ 、…、Ｐ₈ において一旦動作
を停止ししてしまうこととなっていた。

【０００７】

【発明が解決しようとする課題】上記従来のアーチモー
ション動作を用いて制御を行なう場合、複雑な組立作業
を行なうためには、任意の線分組合わせに対して二つの
線分間で経路変更を行なえる必要がある。

【０００８】また、このような経路変更部分の軌跡につ
いては、溶接等の特殊な作業を除いて、その形状が問題
となることはない。さらに教示動作では、直線軌跡で経
由点を指示する必要があるため、再生動作時においても
直線軌跡の組合わせとして実行できれば、制御プログラ
ミングの観点からも都合が良い。

【０００９】そこで、本発明の目的は、 １） 任意の線分組合わせに対して二つの線分間で経路
変更を行なえ、 ２） タクトタイムを低減でき、 ３） プログラミングを簡略化することが可能な、 サーボ系の制御装置および制御方法を提供することにあ
る。

【００１０】

【課題を解決するための手段】上記課題を解決するた
め、請求項１記載の発明は、第１地点（ｘ₁ 、ｙ₁ 、ｚ
₁ ）から第３地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）に向かって非直
線的に被制御装置をサーボ駆動するサーボ系の制御装置
であって、経由目標地点である第２地点（ｘ₂ 、ｙ₂ 、
ｚ₂ ）と前記第３地点を結ぶ線分を含む直線をＸ軸と
し、前記第１地点、前記第２地点及び前記第３地点を含
む平面上の直線であって、前記Ｘ軸と直交する直線をＹ
軸とする経路座標系を基準座標系に対して仮定し、前記
第１地点を初期位置とし、前記第３地点を目標位置と
し、かつ、前記第１地点と前記第２地点を結ぶ直線上の
分岐点から円弧を描いて前記Ｘ軸上に経路を移行させる
べく前記初期位置から前記目標位置に向かう際の前記分
岐点における初速度、目標速度、目標加速度をパラメー
タとして時間関数を生成する時間関数生成手段と、前記
時間関数に基づいて前記第１地点から前記第３地点まで
被制御装置を直線軌跡制御によりサーボ駆動する駆動制
御手段と、を備えて構成する。

【００１１】請求項１記載の発明によれば、時間関数生
成手段は、経由目標地点である第２地点（ｘ₂ 、ｙ₂ 、
ｚ₂ ）と第３地点を結ぶ線分を含む直線をＸ軸とし、第
１地点、第２地点及び第３地点を含む平面上の直線であ
って、Ｘ軸と直交する直線をＹ軸とする経路座標系を基
準座標系に対して仮定し、第１地点を初期位置とし、第
３地点を目標位置とし、かつ、第１地点と第２地点を結
ぶ直線上の分岐点から円弧を描いてＸ軸上に経路を移行
させるべく初期位置から目標位置に向かう際の分岐点に
おける初速度、目標速度、目標加速度をパラメータとし
て時間関数を生成する。

【００１２】これにより駆動制御手段は、生成された時
間関数に基づいて第１地点から第３地点まで被制御装置
を直線軌跡制御によりサーボ駆動する。請求項２記載の
発明は、第１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第４地点
（ｘ₄ 、ｙ₄ 、ｚ₄ ）に向かって非直線的に被制御装置
をサーボ駆動するサーボ系の制御装置であって、経由目
標地点である第３地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）と前記第４
地点を結ぶ線分を含む直線をＸ軸とし、第２地点
（ｘ₂ 、ｙ₂ 、ｚ₂ ）、前記第３地点及び前記第４地点
を含む平面上の直線であって、前記Ｘ軸と直交する直線
をＹ軸とする経路座標系を基準座標系に対して仮定し、
前記第１地点を初期位置とし、前記第４地点を目標位置
とし、かつ、前記第１地点と前記第２地点を結ぶ直線上
の第１の分岐点から円弧を描いて前記第２地点と前記第
３地点を結ぶ直線上へ向かう軌跡上の第２の分岐点から
円弧を描いて前記Ｘ軸上に経路を移行させるべく前記初
期位置から前記目標位置に向かう際の前記第１の分岐点
及び前記第２の分岐点における初速度、目標速度、目標
加速度をパラメータとして時間関数を生成する時間関数
生成手段と、前記時間関数に基づいて前記第１地点から
前記第４地点まで被制御装置を直線軌跡制御によりサー
ボ駆動する駆動制御手段と、を備えて構成する。

【００１３】請求項２記載の発明によれば、時間関数生
成手段は、経由目標地点である第３地点と第４地点を結
ぶ線分を含む直線をＸ軸とし、第２地点、第３地点及び
第４地点を含む平面上の直線であって、Ｘ軸と直交する
直線をＹ軸とする経路座標系を基準座標系に対して仮定
し、第１地点を初期位置とし、第４地点を目標位置と
し、かつ、第１地点と第２地点を結ぶ直線上の第１の分
岐点から円弧を描いて第２地点と第３地点を結ぶ直線上
へ向かう軌跡上の第２の分岐点から円弧を描いてＸ軸上
に経路を移行させるべく初期位置から目標位置に向かう
際の第１の分岐点及び第２の分岐点における初速度、目
標速度、目標加速度をパラメータとして時間関数を生成
する。

【００１４】駆動制御手段は、生成された時間関数に基
づいて第１地点から第４地点まで被制御装置を直線軌跡
制御によりサーボ駆動する。請求項３記載の発明は、第
１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第３地点（ｘ₃ 、
ｙ₃ 、ｚ₃ ）に向かって非直線的に被制御装置をサーボ
駆動するサーボ系の制御方法であって、経由目標地点で
ある第２地点（ｘ₂ 、ｙ₂ 、ｚ₂ ）と前記第３地点を結
ぶ線分を含む直線をＸ軸とし、前記第１地点、前記第２
地点及び前記第３地点を含む平面上の直線であって、前
記Ｘ軸と直交する直線をＹ軸とする経路座標系を基準座
標系に対して仮定し、前記第１地点を初期位置とし、前
記第３地点を目標位置とし、かつ、前記第１地点と前記
第２地点を結ぶ直線上の分岐点から円弧を描いて前記Ｘ
軸上に経路を移行させるべく前記初期位置から前記目標
位置に向かう際の前記分岐点における初速度、目標速
度、目標加速度をパラメータとして時間関数を生成する
時間関数生成工程と、前記時間関数に基づいて前記第１
地点から前記第３地点まで被制御装置を直線軌跡制御に
よりサーボ駆動する駆動制御工程と、を備えて構成す
る。

【００１５】請求項３記載の発明によれば、時間関数生
成工程は、経由目標地点である第２地点と第３地点を結
ぶ線分を含む直線をＸ軸とし、第１地点、第２地点及び
第３地点を含む平面上の直線であって、Ｘ軸と直交する
直線をＹ軸とする経路座標系を基準座標系に対して仮定
し、第１地点を初期位置とし、第３地点を目標位置と
し、かつ、第１地点と第２地点を結ぶ直線上の分岐点か
ら円弧を描いてＸ軸上に経路を移行させるべく初期位置
から目標位置に向かう際の分岐点における初速度、目標
速度、目標加速度をパラメータとして時間関数を生成す
る。

【００１６】これにより駆動制御工程は、時間関数に基
づいて第１地点から第３地点まで被制御装置を直線軌跡
制御によりサーボ駆動する。請求項４記載の発明は、第
１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第４地点（ｘ₄ 、
ｙ₄ 、ｚ₄ ）に向かって非直線的に被制御装置をサーボ
駆動するサーボ系の制御方法であって、経由目標地点で
ある第３地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）と前記第４地点を結
ぶ線分を含む直線をＸ軸とし、第２地点（ｘ₂ 、ｙ₂ 、
ｚ₂ ）、前記第３地点及び前記第４地点を含む平面上の
直線であって、前記Ｘ軸と直交する直線をＹ軸とする経
路座標系を基準座標系に対して仮定し、前記第１地点を
初期位置とし、前記第４地点を目標位置とし、かつ、前
記第１地点と前記第２地点を結ぶ直線上の第１の分岐点
から円弧を描いて前記第２地点と前記第３地点を結ぶ直
線上へ向かう軌跡上の第２の分岐点から円弧を描いて前
記Ｘ軸上に経路を移行させるべく前記初期位置から前記
目標位置に向かう際の前記第１の分岐点及び前記第２の
分岐点における初速度、目標速度、目標加速度をパラメ
ータとして時間関数を生成する時間関数生成工程と、前
記時間関数に基づいて前記第１地点から前記第４地点ま
で被制御装置を直線軌跡制御によりサーボ駆動する駆動
制御工程と、を備えて構成する。

【００１７】請求項４記載の発明によれば、時間関数生
成工程は、経由目標地点である第３地点と第４地点を結
ぶ線分を含む直線をＸ軸とし、第２地点、第３地点及び
第４地点を含む平面上の直線であって、Ｘ軸と直交する
直線をＹ軸とする経路座標系を基準座標系に対して仮定
し、第１地点を初期位置とし、第４地点を目標位置と
し、かつ、第１地点と第２地点を結ぶ直線上の第１の分
岐点から円弧を描いて第２地点と第３地点を結ぶ直線上
へ向かう軌跡上の第２の分岐点から円弧を描いてＸ軸上
に経路を移行させるべく初期位置から目標位置に向かう
際の第１の分岐点及び第２の分岐点における初速度、目
標速度、目標加速度をパラメータとして時間関数を生成
する。

【００１８】これにより駆動制御工程は、時間関数に基
づいて第１地点から第４地点まで被制御装置を直線軌跡
制御によりサーボ駆動する。

【００１９】

【発明の実施の形態】まず実施態様の説明に先立ち、第
１地点Ｐ₁ から第２地点Ｐ₂ に向かわせるための直線軌
跡を台形速度カーブを用いて描かせるための原理につい
て説明する。 モード１動作 図１（ａ）に示すように第１地点Ｐ₁ から第２地点Ｐ₂
に向かう直線軌跡を各座標成分に単純に分解し台形速度
カーブを生成すると図１（ｂ）にそれぞれ示すようなも
のとなる。

【００２０】ところで加減速時間は各座標成分で一致さ
せる必要がある、すなわち、 ｔ_1x＝ｔ_1y＝ｔ_1z かつ、 ｔ_2x＝ｔ_2y＝ｔ_2z を満たす必要がある。

【００２１】このため、一般に第１地点Ｐ₁ 及び第２地
点Ｐ₂ を結ぶ線分の方向ベクトルｎに沿って線分の長さ
ｌに関して図１（ｃ）に示すように線形補間を行ない、
（１）式から第１地点Ｐ₁ 及び第２地点Ｐ₂ を結ぶ線分
上の地点Ｐを求める手法が取られている。

【００２２】 Ｐ＝Ｐ₁ ＋^l Ｐ・ｎ …（１） このように線分の長さに着目して軌跡演算を行なう制御
方法では、経路変更に充分に対応することができない。

【００２３】すなわち、図２（ａ）に示すように、第１
地点Ｐ₁ から第２地点Ｐ₂ へ向けて一定速度で移動中
に、第１地点Ｐ₁ 及び第２地点Ｐ₂ を結ぶ直線上の目標
地点Ｐで位置決めをする場合には、図３に示すように、
位相平面ｌ−ｖｌ上で計画した軌跡に沿って（図３
（ａ）参照）、長さ成分に依存する時間関数を生成する
ことにより（図３（ｂ）参照）、第１地点Ｐ₁ 及び第２
地点Ｐ₂ を結ぶ直線上で経路変更を行なわせることが可
能である。

【００２４】しかしながら、図２（ｂ）に示すように、
第１地点Ｐ₁ 及び第２地点Ｐ₂ を結ぶ直線上の地点Ｐか
ら第２地点Ｐ₂ から地点Ｐ₃ へ向かう直線軌跡に経路変
更する場合には、長さ成分に依存した時間関数を用い
て、経路変更を実現することはできない。

【００２５】そこで、本実施形態では、被制御装置の駆
動経路である直線軌跡の方向に併せて随時別個の座標系
を導入することで問題の解決を図っている。以下、この
ように導入する座標系を経路座標系、経路座標系にかか
わらず一定の座標系を基準座標系というものとする。

【００２６】図４（ａ）に示すように、停止状態の第１
地点Ｐ₁ から出発し、第２地点Ｐ₂で停止する直線軌跡
を考える。２点Ｐ₁ 、Ｐ₂ が与えられたとき、下記の
（２）式に示すように、第１地点Ｐ ₁ と第２地点Ｐ₂ を
結ぶ線分を含む直線をｘ軸（第１地点Ｐ₁ から第２地点
Ｐ₂へ向かう方向を正方向とする）とする経路座標系Σ
ｌを基準座標系Σｂについて下記の（３）式のように定
める。

【００２７】 ｎ＝（Ｐ₂ −Ｐ₁ ）／‖Ｐ₂ −Ｐ₁ ‖ …（２）

【００２８】

【数１】 この場合において、ｙ軸及びｚ軸は互いに直交し、か
つ、ｘ軸と直交する向きに任意に設定する。

【００２９】第１地点Ｐ₁ 及び第２地点Ｐ₂ に対応する
経路座標系Σｌにおける座標 ^lＰ₁， ^lＰ₂ は（４）式
で表される。 ^lＰ₁ ＝Ｒ^t Ｐ₁ ， ^lＰ₂ ＝Ｒ^t Ｐ₂ …（４） さらに直線軌跡に沿った経路座標系Σｌにおける速度ベ
クトル ^lｖ_m 及び加速度ベクトル ^lａ_m は、基準座標系
Σｂに関して予め与えられている速度ベクトルＶ_m 及び
加速度ベクトルａ_m によりそれぞれ（５）式、（６）式
により与えられる。

【００３０】 ^lｖ_m ＝Ｒ^t ｖ_m …（５） ^lａ_m ＝Ｒ^t ａ_m …（６） このとき、第１地点Ｐ₁ と第２地点Ｐ₂ を結ぶ線分上の
地点Ｐのｘ座標は図４（ｂ）に示すようにして得ること
ができる。地点Ｐのｙ座標及びｚ座標は、経路座標系Σ
ｌ上では変化しないので、基準座標系Σｂにおける地点
Ｐの座標は、経路座標系Σｌにおける地点 ^lＰに基づい
て（７）式により表される。

【００３１】 Ｐ＝Ｒ ^lＰ ここで ^lＰ＝（ ^lＰ_x ^lＰ_2y ^lＰ_2z）’ …（７） このように経路変更を行なわない場合を、以下、モード
１と呼ぶこととする。 モード２動作 次に第１地点Ｐ₁ から第２地点Ｐ₂ への移動中に地点Ｐ
から第３地点へ向かう直線軌跡に経路変更する場合（以
下、モード２と呼ぶ。）について図５を参照して説明す
る。

【００３２】経路座標系Σｌのｘ軸はモード１の場合と
同様に第２地点Ｐ₂ から第３地点Ｐ ₃ を結ぶ線分を含む
直線をｘ軸（第２地点Ｐ₂ から第３地点Ｐ₃ へ向かう方
向を正方向とする）とし、ｙ軸は第１地点Ｐ₁ と第２地
点Ｐ₂ を結ぶ線分を含む直線とｘ軸を含む平面ｓ₂ 上、
かつ、ｘ軸と直交する軸とし、ｚ軸は平面ｓ₂ に垂直な
軸とする。

【００３３】これにより、第１地点Ｐ₁ から地点Ｐを経
由して第３地点Ｐ₃ へ至る経路は経路座標系Σｌにおけ
るｘ−ｙ平面上の問題として処理できることがわかる。
すなわち、３点Ｐ₁ 、Ｐ₂ 、Ｐ₃ が与えられたとき、下
記の（８）式に示すように、第２地点Ｐ₂ から第３地点
Ｐ₃ を結ぶ線分を含む直線をｘ軸とし、ｙ軸は第１地点
Ｐ₁ と第２地点Ｐ₂ を結ぶ線分を含む直線とｘ軸を含む
平面ｓ₂ 上、かつ、ｘ軸と直交する軸とする経路座標系
Σｌを基準座標系Σｂについて下記の（９）式〜（１
１）式のように定める。

【００３４】 ｎ＝（Ｐ₃ −Ｐ₂ ）／‖Ｐ₃ −Ｐ₂ ‖ …（８） ａ＝ｎ×（Ｐ₂ −Ｐ₁ ）／‖ｎ×（Ｐ₂ −Ｐ₁ ）‖ …（９） ｏ＝ａ×ｎ …（１０） Σｂ→Σｌ：Ｒ＝（ｎ ｏ ａ） …（１１） また、経路変更開始点である地点Ｐ及び終点である第３
地点Ｐ₃ は経路座標系Σｌにおいて（１２）式で与えら
れる。

【００３５】 ^lＰ＝Ｒ^t Ｐ， ^lＰ₃ ＝Ｒ^t Ｐ₃ …（１２） さらに、第１地点Ｐ₁ から第２地点Ｐ₂ に向かうモード
１の経路座標系において、モード１に対応するｘ軸方向
（第１地点Ｐ₁ から第２地点Ｐ₂ に向かう軸方向）に関
する地点Ｐにおける速度の大きさｖ０を用いて、モード
２におけるｘ軸、ｙ軸方向の速度成分 ^lｖ_x0、^l ｖ_y0を
表すと、それぞれ（１３）式、（１４）式により表され
る。

【００３６】 ^lｖ_x0＝ｖ₀ ・（Ｐ₂ −Ｐ₁ ）／‖Ｐ₂ −Ｐ₁ ‖・ｎ …（１３） ^lｖ_y0＝ｖ₀ ・（Ｐ₂ −Ｐ₁ ）／‖Ｐ₂ −Ｐ₁ ‖・ｏ …（１４） 但し、ｖ₀ はモード１における地点Ｐにおける速度 さらにまた、経路座標系Σｌにおける速度ベクトル ^lｖ
_m 及び加速度ベクトル ^lａ_m は、基準座標系Σｂに関し
て予め与えられている速度ベクトルＶ_m 及び加速度ベク
トルａ_m によりそれぞれ（１５）式、（１６）式により
与えられる。

【００３７】 ^lｖ_m ＝Ｒ^t ｖ_m …（１５） ^lａ_m ＝Ｒ^t ａ_m …（１６） ここで、地点Ｐから第２地点Ｐ₂ 及び第３地点Ｐ₃ を結
ぶ直線上への経路変更問題は、以下に示す二つの時間関
数生成問題と等価である。

【００３８】１）地点 ^lＰ_x から初速度 ^lｖ_x0で移動を
開始し、地点 ^lＰ_3xに至り停止するための時間関数を求
める（図６（ａ）参照）。 ２）地点 ^lＰ_y から初速度 ^lｖ_y0で移動を開始し、地点
^lＰ_3yに至り停止するための時間関数を求める（図６
（ｂ）参照）。

【００３９】これらの結果、経路変更過程における時間
関数は（１７）式により与えられることとなる。 Ｐ＝Ｒ ^lＰ …（１７） ここで、^l Ｐ＝（ ^lＰ_x ^lＰ_y ^lＰ_3z） である。

【００４０】なお、ｔ＞ｔ_c においては、経路座標系Σ
ｌにおいてｙ軸方向の軌跡生成は完了するため、モード
１と等価な状態となる。そして、ｔ＞ｔ_c 以降において
第３地点Ｐ₃ に到達すれば、停止状態となり、第３地点
Ｐ₃ に到達する前に新たな経路変更指定を与えれば、再
びモード２に移行する。モード３動作 一方、ｔ≦ｔ_c において、新たな経路変更指定を与えれ
ば、以下に説明するモード３に移行することとなる。

【００４１】ここで、図７を参照してモード３における
経路変更動作について説明する。第１地点Ｐ₁ から第２
地点Ｐ₂ への移動中に地点Ｐ’から第３地点へ向かう直
線軌跡に経路変更し、さらに第３地点への移動中に地点
Ｐから第４地点へ向かう直線軌跡に経路変更する場合に
ついて説明する。

【００４２】経路座標系Σｌのｘ軸はモード２の場合と
同様に第３地点Ｐ₃ から第４地点Ｐ ₄ を結ぶ線分を含む
直線をｘ軸（第３地点Ｐ₃ から第４地点Ｐ₄ へ向かう方
向を正方向とする）とし、ｙ軸は第２地点Ｐ₂ 、第３地
点Ｐ₃ 及び第４地点Ｐ₄ を含む平面ｓ₃ 上、かつ、ｘ軸
と直交する軸とし、ｚ軸は平面ｓ₃ に垂直な軸とする。

【００４３】この場合において、第２モードと同様に経
路座標系Σｌを基準座標系Σｂについて下記の（１８）
式〜（２１）式のように定める。 ｎ＝（Ｐ₄ −Ｐ₃ ）／‖Ｐ₄ −Ｐ₃ ‖ …（１８） ａ＝ｎ×（Ｐ₃ −Ｐ₂ ）／‖ｎ×（Ｐ₃ −Ｐ₂ ）‖ …（１９） ｏ＝ａ×ｎ …（２０） Σｂ→Σｌ：Ｒ＝（ｎ ｏ ａ） …（２１） また、経路変更開始点である地点Ｐ及び終点である第４
地点Ｐ₄ は経路座標系Σ_ｌにおいて（２２）式で与えら
れる。

【００４４】 ^lＰ＝Ｒ^t Ｐ， ^lＰ₄ ＝Ｒ^t Ｐ₄ …（２２） さらに、第１地点Ｐ₁ から第２地点Ｐ₂ に向かうモード
１の経路座標系において、モード２における方向ベクト
ルｎ₂ （ｘ軸方向）、方向ベクトルｏ₂ （ｙ方向）に対
応する地点Ｐにおける速度の大きさｖ_2x、ｖ_2yを用い
て、モード３におけるｘ軸、ｙ軸、ｚ軸方向の速度成分
^lｖ_x0、 ^lｖ_y0、 ^lｖ_z0を表すと、それぞれ（２３）
式、（２４）式、（２５）式により表される。

【００４５】 ^lｖ_x0＝（ｖ_2x・ｎ₂ ＋ｖ_2y・ｏ₂ ）・ｎ …（２３） ^lｖ_y0＝（ｖ_2x・ｎ₂ ＋ｖ_2y・ｏ₂ ）・ｏ …（２４） ^lｖ_z0＝（ｖ_2x・ｎ₂ ＋ｖ_2y・ｏ₂ ）・ａ …（２５） さらにまた、経路座標系Σｌにおける速度ベクトル ^lｖ
_m 及び加速度ベクトル ^lamは、基準座標系Σｂに関して
予め与えられている速度ベクトルＶ_m 及び加速度ベクト
ルａ_m によりそれぞれ（２６）式、（２７）式により与
えられる。

【００４６】 ^lｖ_m ＝Ｒ^t ｖ_m …（２６） ^lａ_m ＝Ｒ^t ａ_m …（２７） ここで、地点Ｐから第３地点Ｐ₃ 及び第４地点Ｐ₄ を結
ぶ直線上への経路変更問題は、以下に示す三つの時間関
数生成問題と等価である。

【００４７】１）地点 ^lＰ_x から初速度 ^lｖ_x0で移動を
開始し、地点 ^lＰ_4xに至り停止するための時間関数を求
める（図８（ａ）参照）。 ２）地点 ^lＰ_y から初速度 ^lｖ_y0で移動を開始し、地点
^lＰ_4yに至り停止するための時間関数を求める（図８
（ｂ）参照）。

【００４８】３）地点 ^lＰ_z から初速度 ^lｖ_z0で移動を
開始し、地点 ^lＰ_4zに至り停止するための時間関数を求
める（図８（ｃ）参照）。 これらの結果、経路変更過程における時間関数は（２
８）式により与えられることとなる。

【００４９】 Ｐ＝Ｒ ^lＰ …（２８） ここで、^l Ｐ＝（ ^lＰ_X ^lＰ_Y ^lＰ_Z ）’ である。

【００５０】なお、ｔ_c1＜ｔ＜ｔ_c2においては、ｙ軸方
向の軌跡生成は完了するため、モード２と等価な状態と
なり、さらにｔ＞ｔ_c2においては、経路座標系Σ_l にお
いてｚ軸方向の軌跡生成は完了するため、モード１と等
価な状態となる。

【００５１】また、ｔ＜ｔ_c1において、第５地点Ｐ₅ へ
向かう新たな経路変更指定を与えれば、モード３の状態
で、上記（２３）〜（２５）式を以下の３式（２３）’
〜（２５）’式に置き換えることにより制御可能であ
る。

【００５２】 ^lｖ_x0＝（ｖ_3x・ｎ₃ ＋ｖ_3y・ｏ₃ ＋ｖ_3z・ａ₃ ）・ｎ …（２３）’ ^lｖ_y0＝（ｖ_3x・ｎ₃ ＋ｖ_3y・ｏ₃ ＋ｖ_3z・ａ₃ ）・ｏ …（２４）’ ^lｖ_z0＝（ｖ_3x・ｎ₃ ＋ｖ_3y・ｏ₃ ＋ｖ_3z・ａ₃ ）・ａ …（２５）’ 以上におけるモード遷移状態を図９に示す。

【００５３】例えば、上述したｔ＜ｔ_c1において、第５
地点Ｐ₅ へ向かう新たな経路変更指定を与えた場合の状
態遷移は図９において、Ｃ₈ の遷移状態に相当する。以
上の説明のように本実施形態によれば、モード１乃至モ
ード３の動作を組合わせることにより、直線移動命令を
用いて任意の方向に経路を変更することが可能となる。

【００５４】この場合において、新しい目標地点以外の
パラメータを与える必要なく、自動的に経路変更をする
ことができ、動作開始地点から最終目標地点まで停止す
ることなく連続動作を行なうことが可能となり、タクト
タイムを短縮することが可能となる。

【００５５】さらに経路変更のための特別な命令が不要
となるため、プログラミングを簡略化して工数を削減す
ることができる。

【００５６】

【実施例】図１０及び図１１にアーチモーション動作を
実際に行なわせる場合のプログラミング例及びその経路
軌跡を示す。この場合において、具体的に実現するため
には、直線軌跡制御可能なロボットを被制御装置とし、
このロボットを実際に駆動するための駆動装置及び時間
関数生成手段としてのコントロール装置が必要となる。

【００５７】図１０（ａ）は、経路変更を伴わない場合
の軌跡であり、図１０（ｂ）は対応するコントロール装
置のためのプログラム例である。図１０（ｂ）におい
て、 ＭＯＶ Ａ は現在位置からＡ点へ直線移動を行なわせる命令であ
り、 ＷＡＩＴ ＵＮＴＩＬ （Ｂ） は条件Ｂが満たされるまで待機状態（Wait）とするため
の命令である。

【００５８】この結果、図１０の実施例によれば、第１
地点Ｐ₁ を出発して、第２地点Ｐ₂及び第３地点Ｐ₃ を
経由して、第４地点Ｐ₄ で停止することとなる。図１１
（ａ）は、経路変更を伴う場合の軌跡であり、図１１
（ｂ）は対応するコントロール装置のためのプログラム
例である。

【００５９】この結果、図１１の実施例によれば、第１
地点Ｐ₁ を出発して、第２地点Ｐ₂及び第３地点Ｐ₃ を
経由せずに、第４地点Ｐ₄ に至り、停止することとな
る。以上の説明のように本実施例によれば、図１１に示
す経路変更を行なう場合でも特別な命令は不要で、単に
直線移動命令（＝ＭＯＶ）の開始タイミングを異ならせ
るだけで、連続する経路軌跡を描かせることが可能とな
る。

【００６０】

【発明の効果】以上説明したように、請求項１記載の発
明によれば、時間関数生成手段は、経由目標地点である
第２地点（ｘ₂ 、ｙ₂ 、ｚ₂ ）と第３地点を結ぶ線分を
含む直線をＸ軸とし、第１地点、第２地点及び第３地点
を含む平面上の直線であって、Ｘ軸と直交する直線をＹ
軸とする経路座標系を基準座標系に対して仮定し、第１
地点を初期位置とし、第３地点を目標位置とし、かつ、
第１地点と第２地点を結ぶ直線上の分岐点から円弧を描
いてＸ軸上に経路を移行させるべく初期位置から目標位
置に向かう際の分岐点における初速度、目標速度、目標
加速度をパラメータとして時間関数を生成し、駆動制御
手段は、生成された時間関数に基づいて第１地点から第
３地点まで被制御装置を直線軌跡制御によりサーボ駆動
するので、直線軌跡制御にもかかわらず、経路途中で停
止することなく略Ｌ字状の軌跡を描いて被制御装置を駆
動することができ、タクトタイムを低減することができ
るとともに、直線軌跡制御命令のみで制御が可能である
ので、プログラムを簡略化してプログラミングの工程を
削減することができる。

【００６１】請求項２記載の発明によれば、時間関数生
成手段は、経由目標地点である第３地点と第４地点を結
ぶ線分を含む直線をＸ軸とし、第２地点、第３地点及び
第４地点を含む平面上の直線であって、Ｘ軸と直交する
直線をＹ軸とする経路座標系を基準座標系に対して仮定
し、第１地点を初期位置とし、第４地点を目標位置と
し、かつ、第１地点と第２地点を結ぶ直線上の第１の分
岐点から円弧を描いて第２地点と第３地点を結ぶ直線上
へ向かう軌跡上の第２の分岐点から円弧を描いてＸ軸上
に経路を移行させるべく初期位置から目標位置に向かう
際の第１の分岐点及び第２の分岐点における初速度、目
標速度、目標加速度をパラメータとして時間関数を生成
し、駆動制御手段は、生成された時間関数に基づいて第
１地点から第４地点まで被制御装置を直線軌跡制御によ
りサーボ駆動するので、経路途中で停止することなく任
意の軌跡を描いて被制御装置を駆動することができ、タ
クトタイムを低減することができるとともに、直線軌跡
制御命令のみで制御が可能であるので、プログラムを簡
略化してプログラミングの工程を削減することができ
る。

【００６２】請求項３記載の発明によれば、時間関数生
成工程は、経由目標地点である第２地点と第３地点を結
ぶ線分を含む直線をＸ軸とし、第１地点、第２地点及び
第３地点を含む平面上の直線であって、Ｘ軸と直交する
直線をＹ軸とする経路座標系を基準座標系に対して仮定
し、第１地点を初期位置とし、第３地点を目標位置と
し、かつ、第１地点と第２地点を結ぶ直線上の分岐点か
ら円弧を描いてＸ軸上に経路を移行させるべく初期位置
から目標位置に向かう際の分岐点における初速度、目標
速度、目標加速度をパラメータとして時間関数を生成
し、駆動制御工程は、時間関数に基づいて第１地点から
第３地点まで被制御装置を直線軌跡制御によりサーボ駆
動するので、経路途中で停止することなく略Ｌ字状の軌
跡を描いて被制御装置を駆動することができ、タクトタ
イムを低減することができるとともに、直線軌跡制御命
令のみで制御が可能であるので、プログラムを簡略化し
てプログラミングの工程を削減することができる。請求
項４記載の発明によれば、時間関数生成工程は、経由目
標地点である第３地点と第４地点を結ぶ線分を含む直線
をＸ軸とし、第２地点、第３地点及び第４地点を含む平
面上の直線であって、Ｘ軸と直交する直線をＹ軸とする
経路座標系を基準座標系に対して仮定し、第１地点を初
期位置とし、第４地点を目標位置とし、かつ、第１地点
と第２地点を結ぶ直線上の第１の分岐点から円弧を描い
て第２地点と第３地点を結ぶ直線上へ向かう軌跡上の第
２の分岐点から円弧を描いてＸ軸上に経路を移行させる
べく初期位置から目標位置に向かう際の第１の分岐点及
び第２の分岐点における初速度、目標速度、目標加速度
をパラメータとして時間関数を生成し、駆動制御工程
は、時間関数に基づいて第１地点から第４地点まで被制
御装置を直線軌跡制御によりサーボ駆動するので、経路
途中で停止することなく任意の軌跡を描いて被制御装置
を駆動することができ、タクトタイムを低減することが
できるとともに、直線軌跡制御命令のみで制御が可能で
あるので、プログラムを簡略化してプログラミングの工
程を削減することができる。

【図面の簡単な説明】

【図１】直線軌跡制御の原理説明図である。

【図２】直線軌跡制御の説明図である。

【図３】長さ成分に依存する直線軌跡制御の説明図であ
る。

【図４】直線軌跡制御の説明図（モード１）である。

【図５】直線軌跡制御の説明図（モード２）である。

【図６】直線軌跡制御の説明図（モード２）である。

【図７】直線軌跡制御の説明図（モード３）である。

【図８】直線軌跡制御の説明図（モード３）である。

【図９】動作モード遷移状態の説明図である。

【図１０】実施例（経路変更なし）の説明図である。

【図１１】実施例（経路変更有り）の説明図である。

【図１２】従来の動作説明図である。

【符号の説明】

Ｐ、Ｐ’…地点 Ｐ₁ …第１地点 Ｐ₂ …第２地点 Ｐ₃ …第３地点 Ｐ₄ …第４地点

Claims (4)

【特許請求の範囲】
1. 【請求項１】 第１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第３
   地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）に向かって非直線的に被制御
   装置をサーボ駆動するサーボ系の制御装置であって、 経由目標地点である第２地点（ｘ₂ 、ｙ₂ 、ｚ₂ ）と前
   記第３地点を結ぶ線分を含む直線をＸ軸とし、前記第１
   地点、前記第２地点及び前記第３地点を含む平面上の直
   線であって、前記Ｘ軸と直交する直線をＹ軸とする経路
   座標系を基準座標系に対して仮定し、前記第１地点を初
   期位置とし、前記第３地点を目標位置とし、かつ、前記
   第１地点と前記第２地点を結ぶ直線上の分岐点から円弧
   を描いて前記Ｘ軸上に経路を移行させるべく前記初期位
   置から前記目標位置に向かう際の前記分岐点における初
   速度、目標速度、目標加速度をパラメータとして時間関
   数を生成する時間関数生成手段と、 前記時間関数に基づいて前記第１地点から前記第３地点
   まで被制御装置を直線軌跡制御によりサーボ駆動する駆
   動制御手段と、 を備えたことを特徴とするサーボ系の制御装置。
2. 【請求項２】 第１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第４
   地点（ｘ₄ 、ｙ₄ 、ｚ₄ ）に向かって非直線的に被制御
   装置をサーボ駆動するサーボ系の制御装置であって、 経由目標地点である第３地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）と前
   記第４地点を結ぶ線分を含む直線をＸ軸とし、第２地点
   （ｘ₂ 、ｙ₂ 、ｚ₂ ）、前記第３地点及び前記第４地点
   を含む平面上の直線であって、前記Ｘ軸と直交する直線
   をＹ軸とする経路座標系を基準座標系に対して仮定し、
   前記第１地点を初期位置とし、前記第４地点を目標位置
   とし、かつ、前記第１地点と前記第２地点を結ぶ直線上
   の第１の分岐点から円弧を描いて前記第２地点と前記第
   ３地点を結ぶ直線上へ向かう軌跡上の第２の分岐点から
   円弧を描いて前記Ｘ軸上に経路を移行させるべく前記初
   期位置から前記目標位置に向かう際の前記第１の分岐点
   及び前記第２の分岐点における初速度、目標速度、目標
   加速度をパラメータとして時間関数を生成する時間関数
   生成手段と、 前記時間関数に基づいて前記第１地点から前記第４地点
   まで被制御装置を直線軌跡制御によりサーボ駆動する駆
   動制御手段と、 を備えたことを特徴とするサーボ系の制御装置。
3. 【請求項３】 第１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第３
   地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）に向かって非直線的に被制御
   装置をサーボ駆動するサーボ系の制御方法であって、 経由目標地点である第２地点（ｘ₂ 、ｙ₂ 、ｚ₂ ）と前
   記第３地点を結ぶ線分を含む直線をＸ軸とし、前記第１
   地点、前記第２地点及び前記第３地点を含む平面上の直
   線であって、前記Ｘ軸と直交する直線をＹ軸とする経路
   座標系を基準座標系に対して仮定し、前記第１地点を初
   期位置とし、前記第３地点を目標位置とし、かつ、前記
   第１地点と前記第２地点を結ぶ直線上の分岐点から円弧
   を描いて前記Ｘ軸上に経路を移行させるべく前記初期位
   置から前記目標位置に向かう際の前記分岐点における初
   速度、目標速度、目標加速度をパラメータとして時間関
   数を生成する時間関数生成工程と、 前記時間関数に基づいて前記第１地点から前記第３地点
   まで被制御装置を直線軌跡制御によりサーボ駆動する駆
   動制御工程と、 を備えたことを特徴とするサーボ系の制御方法。
4. 【請求項４】 第１地点（ｘ₁ 、ｙ₁ 、ｚ₁ ）から第４
   地点（ｘ₄ 、ｙ₄ 、ｚ₄ ）に向かって非直線的に被制御
   装置をサーボ駆動するサーボ系の制御方法であって、 経由目標地点である第３地点（ｘ₃ 、ｙ₃ 、ｚ₃ ）と前
   記第４地点を結ぶ線分を含む直線をＸ軸とし、第２地点
   （ｘ₂ 、ｙ₂ 、ｚ₂ ）、前記第３地点及び前記第４地点
   を含む平面上の直線であって、前記Ｘ軸と直交する直線
   をＹ軸とする経路座標系を基準座標系に対して仮定し、
   前記第１地点を初期位置とし、前記第４地点を目標位置
   とし、かつ、前記第１地点と前記第２地点を結ぶ直線上
   の第１の分岐点から円弧を描いて前記第２地点と前記第
   ３地点を結ぶ直線上へ向かう軌跡上の第２の分岐点から
   円弧を描いて前記Ｘ軸上に経路を移行させるべく前記初
   期位置から前記目標位置に向かう際の前記第１の分岐点
   及び前記第２の分岐点における初速度、目標速度、目標
   加速度をパラメータとして時間関数を生成する時間関数
   生成工程と、 前記時間関数に基づいて前記第１地点から前記第４地点
   まで被制御装置を直線軌跡制御によりサーボ駆動する駆
   動制御工程と、 を備えたことを特徴とするサーボ系の制御方法。