JPH08123531A

JPH08123531A - 軌道の制御方法

Info

Publication number: JPH08123531A
Application number: JP25785894A
Authority: JP
Inventors: Yasuyuki Momoi; 康行桃井; Norihisa Miyake; 徳久三宅
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1994-10-24
Filing date: 1994-10-24
Publication date: 1996-05-17

Abstract

(57)【要約】【目的】自動作業機械の可動部先端付近に設定した作業
点の軌道制御方法で連続する二つの教示経路の間を滑ら
かに結ぶ移行軌道の制御において、安定した制御を実現
し、かつ、教示経路上の最高速さ，加速度の大きさに依
存しない経路を生成し、経路の予測が容易な軌道の制御
方法及びそれを実現する装置を提供する。【構成】第一の教示経路Ｐ₀ Ｐ₁ と第二の教示経路Ｐ₁
Ｐ₂ の間を滑らかに結ぶ移行軌道Ｓ₁ Ｓ₄ を、第一の教
示経路上で減速を行う予減速区間Ｓ₁ Ｓ₂ ，二つの教示
経路から離れて移動する移行区間Ｓ₂ Ｓ₃ ，第二の教示
経路上で加速を行う後加速区間Ｓ₃ Ｓ₄ から構成し、各
区間毎に独立に加減速を行う。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は自動作業機械の軌道制御
において連続する二つの軌道を滑らかに結ぶ移行軌道を
制御する方法およびそれを実現する装置に関する。

【０００２】

【従来の技術】ロボットなどの自動作業機械では、ハン
ドなどの可動部先端付近に取り付けたツールもしくはワ
ーク上に設定される作業点を教示された経路と教示され
た速さに従って移動させる軌道制御が行われる。ところ
で、ある教示経路（第一の教示経路）からその教示経路
の終点を始点とする次の教示経路（第二の教示経路）へ
と作業点が移動するときに、二つの教示経路の間で移動
方向と速さが不連続に変化するような教示が行われる
と、二つの教示経路の接続点で大加速度が発生する。そ
の結果、振動が引き起こされ、必要とされる精度が得ら
れない、無理な力が加わりワークやツール、さらには機
械本体を破壊するなどの問題が生じる。この問題を解決
する方法として、例えば、特開昭64−26911 号公報に示
されているように、第一の教示経路方向の減速開始と同
時に第二の教示経路方向の加速を開始し、二つの教示経
路を滑らかに結ぶ軌道を第一の教示経路方向の動作と第
二の教示経路方向の動作の合成により生成する方法が知
られている。

【０００３】

【発明が解決しようとする課題】上記従来技術では、第
一の教示経路方向の減速を開始すると同時に第二の教示
経路方向の加速を開始するため、第一の教示経路上での
最高速さ，第一の教示経路方向の加速度の大きさ，第二
の教示経路上での最高速さ，第二の教示経路方向の加速
度の大きさが変化すると、第一の教示経路から離れて移
動を開始する位置，第二の教示経路上の移動を開始する
位置が変化し、生成される経路が変化する。さらに、二
つの教示経路上での最高速さ，加速度の大きさが異なる
場合には、生成される経路が二つの教示経路のなす角の
二等分線に対して非対称となるため、生成される経路の
形状予測が困難である。このことは、動作確認のために
行う低速でのテスト運転と、実際に作業を行うための高
速での連続運転では、生成される経路が異なるために、
テスト運転では回避できた障害物に連続運転で干渉する
という問題を引き起こし、適切に障害物を回避する経路
の教示を困難にしている。

【０００４】本発明の目的は、安定した制御を実現し、
かつ、教示経路上の最高速さ，加速度の大きさに依存し
ない経路を生成し、経路の予測が容易な軌道の制御方法
及びそれを実現する装置を提供することにある。

【０００５】

【課題を解決するための手段】上記目的を達成するため
に、本発明は自動作業機械の可動部先端付近に取り付け
たツールもしくはワーク上に設定される作業点を教示さ
れた経路と教示された速さに従って移動させる軌道の制
御方法であって、第一の教示経路の終点とそれに続く第
二の教示経路の始点とが同一の連続する二つの教示経路
の間を、二つの教示経路の接続点近傍で滑らかに結ぶ移
行軌道を生成し、生成された移行軌道に従って作業点の
運動を制御する方法において、生成される移行軌道を第
一の教示経路上で減速を行う長さ０以上の予減速区間，
二つの教示経路から離れて移動する長さ０以上の移行区
間，第二の教示経路上で加速を行う長さ０以上の後加速
区間から構成し、各区間毎に独立に加減速を行う。

【０００６】本方法では、予減速区間での加速度の大き
さ、移行区間での加速度の大きさ，後加速区間での加速
度の大きさを同一とするように加減速を行う。

【０００７】また、本方法では移行区間での加速度ベク
トルを第一の教示経路方向と第二の教示経路方向に分解
したときの各教示経路方向の加速度ベクトルの大きさが
同一で、かつ、二つの教示経路の接続点から第一の教示
経路から離脱する点（移行開始点）までの距離と二つの
教示経路から第二の教示経路に突入する点（移行終了
点）までの距離を同一とする、もしくは、移行開始点で
の速さと、移行終了点での速さを同一とするように加減
速を行う。このとき、移行開始点での速さ、移行終了点
での速さは、第一の教示経路上での最高速さ、もしく
は、第二の教示経路上での最高速さのうち、小さいもの
に設定する、かつ、予減速区間，移行区間，後加速区間
での加速度の大きさが安定した制御が実現可能な加速度
の最大値（最大許容加速度）に設定することが望まし
い。

【０００８】上記の考え方に基づいた演算処理を行う制
御装置は、教示経路の始点および終点，経路の形状，教
示経路上での最高速さ，加速度の大きさ，移行軌道に対
する条件などから補間周期毎の作業点の位置指令値であ
る補間位置を演算する補間処理を行い、得られた補間位
置から可動部のアクチュエータの駆動量を演算する軌道
制御手段が、補間処理を行うために必要で予め演算可能
な補間係数を演算する補間係数演算部，補間位置での速
さを演算する少なくとも一つの速度演算部，補間位置で
の速さを逐次加算して移動距離を演算する少なくとも一
つの移動距離演算部，移動距離から教示された経路の始
点から補間位置までのベクトルに相当する偏差ベクトル
を演算する少なくとも一つの偏差ベクトル演算部，教示
経路の始点に偏差ベクトルを加算して補間位置を演算す
る少なくとも一つの位置演算部，補間位置から可動部の
アクチュエータの駆動量を演算する座標変換部から構成
され、該速度演算部は二つの教示経路から離れて移動す
るときの教示経路方向に加速するときの速さを演算する
移行加速処理部，教示経路上で加速するときの速さを演
算する後加速処理部，教示経路上で等速移動するときの
速さを演算する等速処理部，教示経路上で減速するとき
の速さを演算する予減速処理部，二つの教示経路から離
れて移動するときの教示経路方向に減速するときの速さ
を演算する移行減速処理部とこれらの処理を切り換える
処理切換部から構成される。

【０００９】また、前記速度演算部は、教示経路上で加
速するときの速さを演算する加速処理部，教示経路上で
減速するときの速さを演算する減速処理部，教示経路上
で等速移動するときの速さを演算する等速処理部，二つ
の教示経路から離れて移動するときの各教示経路方向の
速さを演算する移行処理部とこれらの処理を切り換える
処理切換部から構成されるものでもよい。

【００１０】

【作用】本発明によれば、二つの教示経路を滑らかに結
ぶ移行軌道は、予減速区間，移行区間，後加速区間から
構成される。このとき、教示経路上の最高速さ，加速度
の大きさが変化することによる移行開始点，移行終了点
の位置の変化量を打ち消すように各区間での加減速方法
を調整し、各区間の長さを調整することにより、教示経
路上の最高速さ，加速度の大きさに依存しない経路の生
成が実現できる。

【００１１】また、移行区間での加速度の大きさ，予減
速区間での加速度の大きさ，後加速区間での加速度の大
きさをすべて同一とすることにより、移行軌道全域で加
速度の大きさが一定の安定した制御を実現する。

【００１２】また、移行区間での加速度ベクトルを第一
の教示経路方向と第二の教示経路方向に分解したときの
各教示経路方向の加速度ベクトルの大きさを同一とし、
二つの教示経路の接続点から移行開始点までの距離，移
行終了点までの距離を同一とする、もしくは、移行開始
点，移行終了点での速さを同一とすることにより、第一
の教示経路方向の減速と第二の教示経路方向の加速が二
つの教示経路のなす角の二等分線に対して対称に行われ
ることになり、二つの教示経路のなす角の二等分線に対
して対称な経路が生成され、経路の予測が容易になる。

【００１３】このとき、移行開始点，移行終了点での速
さを、第一の教示経路上の最高速さ、もしくは、第二の
教示経路上での最高速さのうち小さい方に設定すること
により、予減速区間、もしくは、後加速区間のうちのい
ずれかの長さが０となり、教示経路上での加減速が短い
経路が生成できる。さらに、予減速区間，移行区間，後
加速区間での加速度の大きさを最大許容加速度とする
と、等加速度運動を行う場合には各区間での加減速時間
が最短となり、対称で、かつ、タクトタイムが最も短い
軌道が生成できる。

【００１４】

【実施例】本発明の実施例を図を用いて説明する。

【００１５】図２は本発明が対象とする自動作業機械の
構成の一例である。教示操作手段１，記憶手段２，動作
手順制御手段５，軌道制御手段６，アクチュエータ制御
手段７，可動部８から構成される。可動部８はアクチュ
エータとそれを結ぶリンクから構成され、先端のリンク
（ハンド）にツールもしくはワークを取り付け、そのツ
ールもしくはワーク上に作業点を設定する。

【００１６】操作者は教示操作手段１を介して作業点の
軌道の教示，自動作業機械の操作などを行う。作業点の
軌道の教示は、経路と速さを教示することにより行われ
る。教示する経路の始点，終点，通過点や特徴点，代表
点など（これらを教示点と呼ぶ）の位置情報は、教示操
作手段１を介して記憶手段２のなかにある位置記憶手段
３に記憶される。教示点の位置の教示は、教示操作手段
１から位置情報を直接入力するほかに、教示操作手段１
を介しての手動操作などにより作業点を教示したい位置
に移動させその位置を記憶させることにより行ってもよ
い。教示点の通過順序や、経路の形状（直線，円弧な
ど），経路の形状決定に必要なパラメータ，教示経路上
での最高速さ，加速度の大きさ，移行軌道に対する条件
などの情報は教示操作手段１を介して記憶手段２のなか
にある動作手順記憶手段４に記憶される。位置の情報は
位置記憶手段３に記憶され、動作手順の情報は動作手順
記憶手段４に記憶されるが、これらは必ずしも別々に記
憶されている必要はなく、一括して記憶手段２に記憶さ
れてもよい。

【００１７】作業点の軌道の教示が完了したら、教示操
作手段１を介して動作手順制御手段５に動作開始命令を
発行する。動作手順制御手段５は、動作手順記憶手段４
に記憶された情報を読みだし解釈して、軌道制御手段６
に教示経路の始点および終点，経路の形状，教示経路上
での最高速さ，加速度の大きさ，移行軌道に対する条件
などからなる動作命令を発行する。軌道制御手段６は、
動作命令に基づき補間周期毎の作業点の位置指令値であ
る補間位置を演算する補間処理を行い、得られた補間位
置をアクチュエータの駆動量に変換する。アクチュエー
タ制御手段７は軌道制御手段６から出力されるアクチュ
エータの駆動量に従い可動部８のアクチュエータを動か
し、可動部先端に取り付けられた作業点の位置を制御す
る。

【００１８】図３は、本発明の実施例を実現するための
軌道制御手段６の構成の一例である。軌道制御手段６
は、補間係数演算部２１，速度演算部２２，移動距離演
算部２３，偏差ベクトル演算部２４，位置演算部２５，
座標変換部２６から構成される。補間係数演算部２１
は、動作手順制御手段５から発行される教示経路の始点
および終点，経路の形状，教示経路上での最高速さ，加
速度の大きさ，移行軌道に対する条件などからなる動作
命令から、補間処理を行うために必要で予め演算可能な
係数（補間係数）を演算する。速度演算部２２では、現
在の作業点の運動状態（加速，減速／等速，移行，動作
終了）に応じた速さが出力される。移動距離演算部２３
では、速度演算部２２の結果を逐次加算して移動距離
（道のり）を求める。偏差ベクトル演算部２４は、移動
距離から教示経路の始点から補間位置までのベクトルに
相当する偏差ベクトルを演算する。位置演算部２５は、
偏差ベクトルを教示された経路の始点に加算することに
より補間位置を演算する。座標変換手段２６は、位置演
算部２５で演算された補間位置をアクチュエータの駆動
量に変換する。

【００１９】本発明での装置の特徴は、予減速区間，移
行区間，後加速区間での加減速方法を区間毎に独立に設
定するために、速度演算部２２を、後加速区間での加速
移動時の速さを演算する加速処理部３０，等速移動時の
速さを演算する等速処理部３１，予減速区間での減速移
動時の速さを演算する減速処理部３２，移行区間での速
さを演算する移行処理部３３、そしてこれらの処理を切
り換える処理切換部３４から構成することである。

【００２０】図１は、第一の教示経路として点Ｐ₀ から
点Ｐ₁ を直線，第二の教示経路として点Ｐ₁ から点Ｐ₂
を直線で移動するように教示されたときに本発明を適用
したときの動作経路と二つの教示経路方向の速さの時間
変化、すなわち、速度パターンを示す図である。二つの
教示経路の間を滑らかに結ぶ移行軌道は、第一の教示経
路上で減速する予減速区間Ｓ₁ Ｓ₂ ，二つの教示経路か
ら離れて移動する移行区間Ｓ₂ Ｓ₃ ，第二の教示経路上
で加速する後加速区間Ｓ₃ Ｓ₄ から構成される。ここ
で、Ｓ₁ は第一の教示経路上で減速を開始する点（減速
開始点）、Ｓ₂ は第一の教示経路から離脱する点（移行
開始点）、Ｓ₃ は第二の教示経路に突入する点（移行終
了点）、Ｓ₄ は第二の教示経路上での加速を終了する点
（加速終了点）である。本発明を実現するために行う演
算処理の内容を、図１の動作に基づいて説明する。

【００２１】まず、処理を切り換えるタイミングを決定
するために補間係数演算部２１で行われている補間係数
演算処理の内容を図４を用いて説明する。なお、本実施
例では、移行軌道を二つの教示経路方向の動作の合成に
より求めるものとする。すなわち、時刻ｉでの作業点の
位置ベクトルＰ_(i)，速度ベクトルｖ_(i)，加速度ベクト
ルα_(i) を次式により求めるものとする。ただし、本発
明はこの移行軌道の生成方法により限定されるものでは
ない。

【００２２】

【数１】Ｐ_(i)＝Ｐ_s1＋Δｑ_1(i)＋Δｑ_2(i) …（数１）

【００２３】

【数２】ｖ_(i)＝ｖ_1(i)ｄ₁＋ｖ_2(i)ｄ₂ …（数２）

【００２４】

【数３】 α_(i)＝α_1(i)ｄ₁＋α_2(i)ｄ₂ …（数３）ここで、Ｐ_s1は第一の教示経路の始点の位置ベクトル
(Ｐ₀ の位置ベクトル)、Δｑ_1(i)はＰ₀ からＰ₁ へ移動
する第一の軌道に関する偏差ベクトル、Δｑ_2(i)はＰ₁
からＰ₂ へ移動する第二の軌道に関する偏差ベクトル、
ｖ_1(i)は第一の教示経路方向の速さ、ｄ₁ は第一の教示
経路方向の方向ベクトル、ｖ_2(i)は第二の教示経路方向
の速さ、ｄ₂ は第二の教示経路方向の方向ベクトル、α
_1(i)は第一の教示経路方向の加速度の大きさ、α_2(i)は
第二の教示経路方向の加速度の大きさである。

【００２５】本実施例では移行軌道に対する条件とし
て、二つの教示経路の接続点Ｐ₁ から移行開始点Ｓ₂ ま
での距離ｌ_dc1，Ｐ₁ から移行終了点Ｓ₃ までの距離ｌ
_ac2を与える場合について説明する。ただし、本発明は
この移行軌道に対する条件の設定方法により限定される
ものではない。

【００２６】さらに、本実施例では第一の教示経路上で
の最高速さをｖ_m1，第二の教示経路上での最高速さをｖ
_m2，予減速区間Ｓ₁ Ｓ₂ ，移行区間Ｓ₂ Ｓ₃ ，後加速区
間Ｓ₃ Ｓ₄ での加速度の大きさをα_max とする。

【００２７】図４は補間係数演算処理のフローチャート
である。動作手順制御手段５から動作命令が発行される
と、補間係数演算状態判定フラグsmooth_mode を判定し
て、補間係数が未計算の場合(smooth_mode＝０）にはＰ
₀ からＰ₁ へ移動する第一の軌道の補間係数が演算され
（ブロック１００）、第一の軌道の補間係数が演算完了
している場合(smooth_mode＝１）にはＰ₁ からＰ₂ へ移
動する第二の軌道の補間係数が演算される（ブロック２
００）。

【００２８】第一の軌道の補間係数演算（ブロック１０
０）の処理についてその詳細を説明する。

【００２９】ブロック１０１では、第一の教示経路の始
点の位置ベクトルＰ₀ ，終点の位置ベクトルＰ₁ から、
始点から終点までの距離（補間距離）ｌ₁ を演算する。

【００３０】

【数４】 ΔＰ₁＝Ｐ₁−Ｐ₀ …（数４）

【００３１】

【数５】ｌ₁＝｜ΔＰ₁｜ …（数５）ブロック１０２では、第一の教示経路方向の方向ベクト
ルｄ₁ を演算する。

【００３２】

【数６】ｄ₁＝ΔＰ₁／ｌ₁ …（数６）ブロック１０３では、指定された加速度α_max で第一の
教示経路上の最高速さｖ_m1まで加速に要する距離（加速
所要距離）ｌ_a1，最高速さｖ_m1から減速するのに要する
距離（減速所要距離）ｌ_d1を演算する。

【００３３】ｌ_a1，ｌ_d1の演算方法は、速度制御方式
（速度パターン）により異なる。ここでは等加速度運動
を仮定するが、本発明はこれにより限定されるものでは
ない。この場合、ｌ_a1，ｌ_d1は次式により演算される。

【００３４】

【数７】

【００３５】

【数８】

【００３６】以上の処理により第一の軌道の補間係数の
演算を完了すると、補間係数演算状態判定フラグへの第
一の軌道の補間係数演算完了を設定する（smooth_mode
＝１,ブロック１０４）。

【００３７】次に第二の軌道の補間係数演算（ブロック
２００）の処理についてその詳細を説明する。

【００３８】ブロック２０１では、第二の教示経路の始
点の位置ベクトルＰ₁ ，終点の位置ベクトルＰ₂ から、
始点から終点までの距離（補間距離）ｌ₂ を演算する。

【００３９】

【数９】 ΔＰ₂＝Ｐ₂−Ｐ₁ …（数９）

【００４０】

【数１０】ｌ₂＝｜ΔＰ₂｜ …（数１０）ブロック２０２では、第一の教示経路方向の方向ベクト
ルｄ₁ を演算する。

【００４１】

【数１１】ｄ₂＝ΔＰ₂／ｌ₂ …（数１１）ブロック２０３では移行区間での第一の教示経路方向の
加速度α_dc1 ，第二の教示経路方向の加速度α_ac2 の演
算を行う。

【００４２】移行区間での加速度ベクトルの大きさ｜α
_(i)｜は数３から次式のように求められる。

【００４３】

【数１２】

【００４４】ここで、θは第一の教示経路方向と第二の
教示経路方向のなす角である。

【００４５】本実施例では等加速度運動を仮定している
ので、α_1(i)，α_2(i)は次式のようにおける。

【００４６】

【数１３】 α_1(i)＝−α_dc1 …（数１３）

【００４７】

【数１４】 α_2(i)＝α_ac2 …（数１４）作業点が移行開始点Ｓ₂ から移行終了点Ｓ₃ に到達する
までの時間をｔ₂₃とすると、二つの教示経路の接続点か
ら移行開始点までの距離ｌ_dc1 ，移行終了点までの距離
ｌ_ac2 ，各教示経路方向の加速度の大きさα_dc1，α_ac2
には以下の関係が成り立つ。

【００４８】

【数１５】

【００４９】

【数１６】

【００５０】よって、

【００５１】

【数１７】

【００５２】

【数１８】 α_ac2＝ｎα_dc1 …（数１８）これを、数１２に代入すると、

【００５３】

【数１９】

【００５４】したがって、｜α_(i)｜＝α_maxより、α
_dc1は次式により求められ、α_ac2は求められたα_dc1 よ
り数１８により求められる。

【００５５】

【数２０】

【００５６】ブロック２０４では、移行開始点Ｓ₂ での
速さ(移行開始速さ）ｖ_dc1の演算を行う。移行区間では
第一の教示経路方向へは距離ｌ_dc1で速さをｖ_dc1から０
まで減速度α_dc1の減速を行っている。したがって、ｖ
_dc1は次式により求められる。

【００５７】

【数２１】

【００５８】ブロック２０５では最高速さｖ_m1から減速
度α_maxで速さｖ_dc1まで減速し、その後減速度α_dc1で
減速に要する距離（減速所要距離）ｌ_d1を演算する。

【００５９】

【数２２】

【００６０】ブロック２０６では、移行終了点Ｓ₃ での
速さ(移行終了速さ）ｖ_ac2の演算を行う。移行区間では
第二の教示経路方向へは距離ｌ_ac2で速さを０からｖ_ac2
まで加速度α_ac2の加速を行っている。したがって、ｖ
_ac2は次式により求められる。

【００６１】

【数２３】

【００６２】ブロック２０７では加速度α_ac2で速さｖ
_ac2まで加速した後、加速度α_maxで第二の教示経路上の
最高速さｖ_m2まで加速するのに要する距離(加速所要距
離)ｌ_a2と、加速度α_maxでｖ_m2から減速するのに要する
距離（減速所要距離）ｌ_d2の演算を行う。ｌ_a2，ｌ_d2は
次式により求められる。

【００６３】

【数２４】

【００６４】

【数２５】

【００６５】以上の処理により第二の軌道の補間係数の
演算を完了すると、補間係数演算状態判定フラグへの第
二の軌道の補間係数演算完了を設定する（smooth_mode
＝２,ブロック２０８）。

【００６６】上記のように演算処理された補間係数を用
いて、速度演算部２２，移動距離演算部２３，偏差ベク
トル演算部２４，位置演算部２５，座標変換部２６で補
間実行演算処理が行われる。この処理について図５から
図７を用いて説明する。図５は補間実行演算処理のフロ
ーチャート、図６は本実施例を実現するために用いる速
度パターン、図７は図６の速度パターンに基づいて速度
演算部２２で行われる速度演算処理のフローチャートで
ある。

【００６７】まず、ブロック３０１では初期化処理が行
われる。初期化処理では、運動状態判定フラグａｃを加
速状態（ａｃ＝１）に設定する。

【００６８】ブロック３０２からブロック３０７までは
第一の軌道の補間実行処理が完了するまで、すなわち、
第一の軌道の移動距離ｍ₁ が補間距離ｌ₁ になるまで繰
り返される。

【００６９】ブロック３０２では、速度演算部２２での
第一の教示経路方向の速さｖ_1(i)，第二の教示経路方向
の速さｖ_2(i)の演算が行われる。この速度演算処理につ
いて図６，図７を用いてその詳細を説明する。

【００７０】作業点の運動状態が加速状態（ａｃ＝１）
にあるときは、処理切換部３４により加速処理部３０が
選択され、後加速区間での第一の教示経路方向の速さｖ
_1(i)のα_max での加速が行われる（ブロック４０２）。
ｖ_1(i)≧ｖ_m1になると、運動状態判定フラグは減速／等
速状態に設定される（ａｃ＝２，ブロック４０１）。

【００７１】減速／等速状態（ａｃ＝２）では、作業点
が減速開始点Ｓ₁ に到達するまで、すなわち、第一の軌
道の補間距離ｌ₁ と移動距離ｍ₁ との差が減速所要距離
ｌ_d1になるまでは（ｌ₁−ｍ₁＞ｌ_d1）、処理切換部３４
により等速処理部３１が選択され、等速処理が行われる
（ブロック４０５）。作業点が減速開始点Ｓ₁ に到達し
たら（ｌ₁−ｍ₁≦ｌ_d1）、処理切換部３４により減速処
理部３２が選択され、予減速区間での第一の教示経路方
向の速さｖ_1(i)のα_max での減速が行われる（ブロック
４０３）。作業点が移行開始点Ｓ₂ に到達したら(ｌ₁−
ｍ₁≦ｌ_dc1）、運動状態判定フラグを移行状態に設定す
る（ａｃ＝３，ブロック４０４）。移行状態（ａｃ＝
３）では、処理切換部３４により移行処理部３３が選択
され、移行区間での加速度の大きさがα_maxになるよう
に第一の教示経路方向の速さｖ_1(i) のα_dc1での減速，
第二の教示経路方向の速さｖ_2(i)のα_ac2での加速が行
われる（ブロック４０６）。作業点が移行終了点Ｓ₃ に
到達したら（ｌ1−ｍ₁≦０）、運動状態判定フラグを終
了状態に設定する（ａｃ＝４，ブロック４０７）。

【００７２】ブロック３０３では、移動距離演算部２３
での第一の軌道の移動距離ｍ_1(i),第二の軌道の移動距
離ｍ_2(i)の演算を行う。移動距離演算部２３では時刻ｉ
−１までのそれぞれの軌道の移動距離ｍ_1(i-1)，ｍ
_2(i-1)に、ブロック３０２で演算された時刻ｉでの速さ
ｖ_1(i)，ｖ_2(i)に補間周期ｔ_s をかけて時刻ｉでの増分
を求め、移動距離ｍ_1(i)，ｍ_2(i)を次式により求める。

【００７３】

【数２６】ｍ_1(i)＝ｍ_1(i-1)＋ｖ_1(i)ｔ_s …（数２６）

【００７４】

【数２７】ｍ_2(i)＝ｍ_2(i-1)＋ｖ_2(i)ｔ_s …（数２７）ブロック３０４では、偏差ベクトル演算部２４での第一
の教示経路の始点から時刻ｉの作業点の位置指令値であ
る補間位置までのベクトルを第一の教示経路方向と第二
の教示経路方向に分配したときの、第一の教示経路方向
のベクトル（第一の軌道の偏差ベクトル）Δｑ_1(i)，第
二の教示経路方向のベクトル（第二の軌道の偏差ベクト
ル）Δｑ_2(i)を求める。

【００７５】

【数２８】 Δｑ_1(i)＝ｍ_1(i)ｄ₁ …（数２８）

【００７６】

【数２９】 Δｑ_2(i)＝ｍ_2(i)ｄ₂ …（数２９）ブロック３０５では、位置演算部２５における時刻ｉの
補間点の位置ベクトルＰ_(i) の演算を数１により行う。

【００７７】ブロック３０６では、時刻ｉでの補間点の
位置ベクトルＰ_(i) へ作業点を移動させるためのアクチ
ュエータの駆動量を演算する。ブロック３０７では、ア
クチュエータ制御手段７への出力処理が行われる。

【００７８】ブロック３０８では、次の補間処理を実行
するための切換処理が行われる。ここでは、第二の軌道
の補間係数を第一の軌道の補間係数として更新し、新た
に動作手順制御手段５から発行される動作命令により補
間係数演算部２１で演算された補間係数を第二の軌道の
補間係数として更新する。

【００７９】以上説明したように補間係数演算処理およ
び補間実行演算処理を行うことにより、補間係数演算部
２１で求めた減速開始点Ｓ₁の位置（Ｐ₁からの距離）ｌ
_d1，移行開始点Ｓ₂の位置（Ｐ₁からの距離）ｌ_dc1，速
さｖ_dc1，移行終了点Ｓ₃ の位置（Ｐ₁からの距離）ｌ
_ac2，速さｖ_ac2，加速終了点Ｓ₄での速さｖ_m2に基づい
て、速度演算部２２の処理を切り換え、移行軌道を予減
速区間Ｓ₁ Ｓ₂ ，移行区間Ｓ₂ Ｓ₃ ，後加速区間Ｓ₃ Ｓ
₄ から構成し、各区間毎に加減速を行うことが可能とな
る。

【００８０】このとき、数２１から数２４から分かるよ
うに、教示経路上の最高速さｖ_m1,ｖ_m2，加速度の大き
さα_maxの値が変更になっても、移行開始点Ｓ₂の位置ｌ
_dc1,移行終了点Ｓ₃の位置ｌ_ac2を変化させずに、予減速
区間の長さ（ｌ_d1−ｌ_dc1）,後加速区間の長さ（ｌ_a2−
ｌ_ac2），移行開始速さｖ_dc1，移行終了速さｖ_ac2 にｖ
_m1，ｖ_m2，α_max の変更の影響を吸収させる。これによ
り、教示経路上の最高速さ，加速度の大きさに依存しな
い一定の経路が得られる。

【００８１】さらに、移行区間での加速度の大きさを、
予減速区間，後加速区間での加速度の大きさと同一とす
ることにより、移行軌道全域で加速度の大きさが一定の
安定した制御が実現できる。

【００８２】ところで、移行軌道に対する条件として移
行区間での第一の教示経路方向の加速度の大きさ
α_dc1，第二の教示経路方向の加速度の大きさα_ac2を同
一の値α_cに設定する。さらに、二つの教示経路の接続
点Ｐ₁から移行開始点Ｓ₂までの距離ｌ_dc1，Ｐ₁から移行
終了点Ｓ₃までの距離ｌ_ac2を同一の値ｌ_c に設定する。
このとき、移行開始点Ｓ₂ での速さｖ_dc1，移行終了点
での速さｖ_ac2は数２１，数２３により求められ、ｖ_dc1
とｖ_ac2は同一の値となる。その値をｖ_c とする。移行
開始点Ｓ₂ から移行終了点Ｓ₃ に到達するまでの時間を
ｔ₂₃とすると、ｖ_c は次式のようになる。

【００８３】

【数３０】ｖ_c＝α_cｔ₂₃ …（数３０）このとき、移行区間での第一の教示経路方向の速さｖ
_1(t)，第二の教示経路方向の速さｖ_2(t)は次式のように
なる。

【００８４】

【数３１】ｖ_1(t)＝ｖ_c−α_c(ｔ−Ｔ₂) ＝α_c{ｔ₂₃−(ｔ−Ｔ₂)｝ …（数３１）

【００８５】

【数３２】ｖ_2(t)＝α_c(ｔ−Ｔ₂) …（数３２）ここで、Ｔ₂ は作業点が移行開始点Ｓ₂ に到達した時刻
である。この式から、移行区間での第一の教示経路方向
の減速と、第二の教示経路方向の加速は、二つの教示経
路のなす角の二等分線に対して対称に行われることが明
らかである。よって、この場合、移行区間では二つの教
示経路のなす角の二等分線に対して対称な放物線軌道が
得られることになり、経路の予測が容易に行える。

【００８６】また、移行軌道に対する条件として第一の
教示経路方向の加速度の大きさα_dc1，第二の教示経路
方向の加速度の大きさα_ac2を同一の値α_c に設定し、
移行開始点Ｓ₂での速さｖ_dc1，移行終了点での速さｖ
_ac2を同一の値ｖ_cに設定する。この場合、二つの教示経
路の接続点Ｐ₁から移行開始点Ｓ₂までの距離ｌ_dc1，Ｐ₁
から移行終了点Ｓ₃ までの距離ｌ_ac2は、次式のように
求められ、同一の値となる。その値をｌ_c とする。

【００８７】

【数３３】

【００８８】

【数３４】

【００８９】このときの第一の教示経路方向の速さｖ
_1(t)，第二の教示経路方向の速さｖ_2(t)は、数３１，
数３２となり、上記の場合と同様に移行区間での第一の
教示経路方向の減速と、第二の教示経路方向の加速は、
二つの教示経路のなす角の二等分線に対して対称に行わ
れ、移行区間では二つの教示経路のなす角の二等分線に
対して対称な放物線軌道が得られることになり、経路の
予測が容易に行える。

【００９０】また、移行開始点での速さと移行終了点で
の速さを、第一の教示経路上での最高速さ、もしくは、
第二の教示経路上での最高速さのうち小さい方に設定す
ると、Ｓ₁とＳ₂が同一、もしくは、Ｓ₃とＳ₄が同一とな
る。すなわち、予減速区間、もしくは、後加速区間のい
ずれかの区間の長さが０になり、教示経路上での加減速
が最小となる移行軌道が生成される。さらに、予減速区
間，移行区間，後加速区間での加速度の大きさを安定し
た制御が実現可能な加速度の最大値である最大許容加速
度とすると、等加速度運動を行う場合には各区間での加
減速時間が最短になり、タクトタイムが最も短い経路が
生成できる。

【００９１】図８に本発明の実施例を実現するための軌
道制御手段６の別の構成を示す。図８と図３に示した場
合の違いは、速度演算部２２，移動距離演算部２３，偏
差ベクトル演算部２４が第一の軌道，第二の軌道のそれ
ぞれに別々用意されていることである。さらに、このと
きは作業点の運動状態は移行加速，後加速，予減速／等
速，移行減速／等速，動作終了に分けられ、速度演算部
２２は、移行区間での教示経路方向に加速するときの速
さを演算する移行加速処理部４０，教示経路上で加速す
るときの速さを演算する後加速処理部４１，教示経路上
で等速移動するときの速さを演算する等速処理部４２，
教示経路上で減速するときの速さを演算する予減速処理
部４３，移行区間での教示経路方向に減速するときの速
さを演算する移行減速処理部４４とこれらの処理を切り
換える処理切換部４５から構成される。

【００９２】図８の構成をとる場合には、補間実行演算
処理も図５から図７に示した場合と異なる。この場合の
補間実行演算を図９から図１１により説明する。図９は
補間実行演算処理のフローチャート、図１０は本例で用
いる速度パターン、図１１は図１０に示す速度パターン
に基づく速度演算部での処理のフローチャートである。

【００９３】まず、ブロック５０１では初期化処理が行
われる。

【００９４】ブロック５０２からブロック５０９までは
第一の軌道の移動距離ｍ_1(i)が補間距離ｌ₁ に達するま
で繰り返される。

【００９５】ブロック５０２では、速度演算部２２−１
での第一の教示経路方向の速さｖ_1(i)の演算が行われ
る。速度演算処理について図１０，図１１を用いてその
詳細を説明する。作業点が前の軌道の移行開始点に到達
すると、運動状態判定フラグは移行加速状態に設定され
る（ａｃ＝１，ブロック６０１）。移行加速状態（ａｃ
＝１）では、処理切換部４５−１により移行加速処理部
４０−１が選択され、第一の軌道の速さｖ_1(i)はα_ac1
で加速される（ブロック６０３）。ｖ_1(i)≧ｖ_ac1にな
ると、運動状態判定フラグは後加速状態に設定される
（ａｃ＝２，ブロック６０２）。後加速状態（ａｃ＝
２）では、処理切換部４５−１により後加速処理部４１
−１が選択され、ｖ_1(i)はα_maxで加速される(ブロック
６０５)。ｖ_1(i)≧ｖ_m1になると、運動状態判定フラグ
を予減速／等速状態に設定する（ａｃ＝３，ブロック６
０４）。予減速／等速状態（ａｃ＝３）では、作業点が
減速開始点Ｓ₁に達するまでは（ｌ₁−ｍ_1(i)＞ｌ_d1）、
処理切換部４５−１により等速処理部４２−１が選択さ
れ、等速処理が行われる（ブロック６０８）。作業点が
減速開始点Ｓ₁に到達したら（ｌ₁−ｍ_1(i)≦ｌ_d1）、処
理切換部４５−１により予減速処理部４３−１が選択さ
れ、ｖ_1(i)はα_max で減速される（ブロック６０６）。
作業点が移行開始点Ｓ₂ に到達したら（ｌ₁−ｍ_1(i)≦
ｌ_dc1）、運動状態判定フラグを移行減速／等速状態に
設定し(ａｃ＝４，ブロック６０７)、第二の軌道の運動
状態判定フラグを移行加速状態に設定する。移行減速／
等速状態（ａｃ＝４）では、処理切換部４５−１により
移行減速処理部４４−１が選択され、ｖ_1(i)はα_dc1 で
減速される（ブロック６０９）。作業点が移行終了点Ｓ
₃に到達したら（ｌ₁−ｍ_1(i)≦０）、運動状態判定フラ
グを終了状態に設定する（ａｃ＝５，ブロック６１
０）。

【００９６】ブロック５０３では、移動距離演算部２３
−１での第一の軌道の移動距離ｍ_1(i)の演算を行う。移
動距離演算部２３−１では時刻ｉ−１までの移動距離ｍ
_1(i-1)に、ブロック５０２で演算された時刻ｉでの速さ
ｖ_1(i)に補間周期ｔ_sをかけて時刻ｉでの増分を求め、
時刻ｉでの移動距離ｍ_1(i)を数２６により求める。

【００９７】ブロック５０４では、偏差ベクトル演算部
２４−１での第一の軌道の偏差ベクトルΔｑ_1(i)の数２
８により演算を行う。

【００９８】ブロック５０５からブロック５０７までの
処理はブロック５０２からブロック５０４で行った速度
演算処理，移動距離演算処理，偏差ベクトルの演算を第
二の軌道に対して行い、第二の軌道に対する速さ
ｖ_2(i)，移動距離ｍ_2(i)，偏差ベクトルΔｑ_2(i)を求め
る。

【００９９】ブロック５０８は、位置演算部２５での時
刻ｉの補間点Ｐ_(i) の演算を数１により行う。

【０１００】ブロック５０９では、時刻ｉでの補間点Ｐ
_(i) へ作業点を移動させるためのアクチュエータの駆動
量を演算する。ブロック５１０では、アクチュエータ制
御手段７への出力処理が行われる。

【０１０１】第一の軌道に対する移動距離ｍ_1(i)が第一
の軌道の補間距離ｌ₁ に達したら、すなわち、移行終了
点Ｓ₃ へ作業点が到達したら第一の軌道に対する補間が
終了する。ブロック５１１では、次の補間処理を実行す
るための準備（切換処理）が行われる。ここでは、第二
の軌道の補間係数を第一の軌道の補間係数として更新
し、新たに動作手順制御手段５から発行される動作命令
により補間係数演算部で演算された補間係数を第二の軌
道の補間係数として更新する。

【０１０２】以上説明したように補間実行演算処理を行
うことによっても、補間係数演算部２１で求めた前の軌
道の移行終了点の速さｖ_ac1，最高速さｖ_m1，減速開始
点Ｓ₁の位置（Ｐ₁ からの距離）ｌ_d1，移行開始点Ｓ₂
の位置（Ｐ₁からの距離)ｌ_dc1,速さｖ_dc1 に基づいて、
速度演算部２２の処理を切り換え、移行軌道を予減速区
間，移行区間，後加速区間から構成し、各区間毎に加減
速を行うことが可能となる。

【０１０３】以上の実施例では直線経路と直線経路との
接続について説明したが、本発明は直線経路以外の経路
に対しても適用可能である。図１２は直線経路と円弧経
路との接続に対して本処理を適応した場合の動作線図で
ある。図１２を実現する場合には、図４に示す補間係数
演算処理のうち、補間距離の演算（ブロック１０１，ブ
ロック２０１），補間方向ベクトルの演算(ブロック１
０２，ブロック２０２)を円弧補間用に変更し、図５に
示す補間実行演算処理のうち偏差ベクトルの演算（ブロ
ック３０４）を円弧補間用に変更すればよい。

【０１０４】

【発明の効果】本発明によれば、連続する二つの教示経
路の間を結ぶ滑らかな移行軌道を、第一の教示経路上で
減速を行う予減速区間，二つの教示経路から離れて移動
する移行区間，第二の教示経路上で加速を行う後加速区
間から構成する。

【０１０５】教示経路上の最高速さ，加速度の大きさの
変化の影響を、各区間での加減速方法を調整することに
より、予減速区間の長さ，後加速区間の長さ，移行開始
速さ，移行終了速さに吸収させることにより、一定の経
路が生成可能となる。

【０１０６】また、予減速区間，移行区間，後加速区間
での加速度の大きさを同一とすることにより移行軌道全
域で加速度の大きさが一定の安定した制御が実現でき
る。

【０１０７】さらに、移行区間での加速度ベクトルを第
一の教示経路方向と第二の教示経路方向に分解したとき
の各教示経路方向の加速度ベクトルの大きさを同一と
し、二つの教示経路の接続点から移行開始点までの距
離，接続点から移行終了点までの距離を同一とする、も
しくは、移行開始点での速さ，移行終了点での速さを同
一とすることにより、二つの教示経路のなす角の二等分
線に対して対称な軌道が生成でき、経路の予測が容易に
なる。さらに、生成される経路を対称とすることによ
り、同じ教示経路を逆方向に移動する場合にも、正方向
に移動する時と同じ経路上を移動することが可能とな
る。

【０１０８】これにより、低速のテスト運転と、高速の
連続運転で同一の経路が生成でき、速さの変化による経
路の変化を考慮することなく、適切な教示が可能とな
る。

【図面の簡単な説明】

【図１】連続する二つの直線経路を滑らかに結ぶ軌道の
生成に本発明を適用した場合の動作経路と速度パターン
を示す説明図。

【図２】本発明が対象とする自動作業機械のブロック
図。

【図３】本発明を実現するための軌道制御手段の構成の
一例を示すブロック図。

【図４】補間係数演算のフローチャート。

【図５】軌道制御手段が図３に示す構成をとる場合の補
間実行演算のフローチャート。

【図６】軌道制御手段が図３に示す構成をとる場合の速
度パターンの説明図。

【図７】図６に示す速度パターンに基づく速度演算部の
処理のフローチャート。

【図８】本発明を実現するための軌道制御手段の構成の
別の例を示すブロック図。

【図９】軌道制御手段が図８に示す構成をとる場合の補
間実行演算のフローチャート。

【図１０】軌道制御手段が図８に示す構成をとる場合の
速度パターンを示す説明図。

【図１１】図１０に示す速度パターンに基づく速度演算
部の処理のフローチャート。

【図１２】連続する直線経路と円弧経路を滑らかに結ぶ
軌道の生成に本発明を適用した場合の動作経路を示す説
明図。

【符号の説明】

１…教示操作手段、２…記憶手段、３…位置記憶手段、
４…動作手順記憶手段、５…動作手順制御手段、６…軌
道制御手段、７…アクチュエータ制御手段、８…可動
部。

Claims

【特許請求の範囲】

【請求項１】自動作業機械の可動部先端付近に取り付け
たツールもしくはワーク上に設定される作業点を教示さ
れた経路と教示された速さに従って移動させる軌道の制
御方法であって、第一の教示経路の終点と第二の教示経
路の始点とが同一の連続する二つの教示経路の間を、二
つの教示経路の接続点近傍で滑らかに結ぶ移行軌道を生
成し、生成された移行軌道に従って作業点の運動を制御
する方法において、生成される移行軌道を第一の教示経
路上で減速を行う長さ０以上の予減速区間，二つの教示
経路から離れて移動する長さ０以上の移行区間，第二の
教示経路上で加速を行う長さ０以上の後加速区間から構
成し、各区間毎に独立に加減速方法を設定することを特
徴とする軌道の制御方法。