JP5776577B2 - マスタスレーブ制御方法及び制御装置並びにシステム - Google Patents

Description

本発明はマスタスレーブ制御方法及び制御装置並びにシステムに関し、特にマスタの動作にスレーブの動作を追従させるマスタスレーブ制御方法及び制御装置並びにシステムに関する。

近年、マスタ機器とスレーブ機器とを用いて、マスタ機器の動作にスレーブ機器の動作を追従させるマスタスレーブシステムが多く提案されている。このようなマスタスレーブシステムの一例が特許文献１に開示されている。

特許文献１に開示される位置制御装置では、外部から入力した第１の位置指令に基づいて実先行移動量を出力する先行移動量演算部と、第１の位置指令と実先行移動量の和信号である第２の位置指令を入力し制御対象の位置が第２の位置指令に追従するように制御を行う追従制御部を備える。そして、先行移動量演算部は前記第１の位置指令の微分あるいは疑似微分である疑似指令速度に予め設定した先行時間Ｔｐを乗じた信号と第１の位置指令の２階微分あるいは２階疑似微分である疑似指令加速度に予め設定した加速度増幅定数を乗じた信号との加算に基づいて実先行移動量を演算する。

これにより、特許文献１に記載の位置制御装置では、マスタ機器から入力する第１の位置指令におけるノイズ等の影響を低減するための高域遮断特性を維持しながら、停止時の制定時間の短縮を実現する。

特開２００１−２４２９３８号公報

マスタスレーブシステムでは、マスタ機器の位置にスレーブ機器の位置を即時追従させることの要求がある。しかしながら、マスタ機器がスレーブ機器の許容動作範囲を超える急加速、逆回転等の急制動を行った場合、スレーブ機器の機構に過大な負荷がかかることがある。このようにスレーブ機器に対して過大な負荷がかかった場合、スレーブ機器の機構が疲労により劣化するおそれがある。

上記課題に鑑みて、本発明は、スレーブ機器の機構を保護することを目的としてなされたものである。

本発明の一態様は、マスタ機器の位置変位に追従してスレーブ機器の位置を制御するマスタスレーブシステムのマスタスレーブ制御方法であって、前記マスタ機器の位置情報と、前記スレーブ機器の位置情報と、を取得する位置情報取得ステップと、前記マスタ機器の位置、前記スレーブ機器の位置と、の差分値を算出する位置差算出ステップと、前記スレーブ機器の変位後の位置を指示する位置指令値として前記スレーブ機器の可動範囲内の値から算出された仮想位置指令値を生成し、当該仮想位置指令値により前記スレーブ機器を前記マスタ機器の位置に追従させる保護処理ステップと、を有するマスタスレーブ制御方法である。

この一態様において、前記スレーブ機器が現速度から停止状態となるまでに要する停止距離を算出し、前記マスタ機器の位置情報により示される目標位置との位置差が前記停止距離と等しくなる位置を減速開始位置として算出する減速開始位置算出ステップをさらに有し、前記保護処理ステップは、前記スレーブ機器が前記減速開始位置に達したことに応じて前記スレーブ機器を減速させる前記仮想位置指令値を生成し、当該仮想位置指令値により前記スレーブ機器を減速させる減速処理ステップを有していても良い。

この一態様において、前記停止距離は、現在の前記スレーブ機器の位置情報と、１つ前の制御周期の前記スレーブ機器の位置情報と、に基づき算出しても良い。

この一態様において、前記保護処理ステップは、前記スレーブ機器の速度が予め定められた最高速度未満である場合に前記スレーブ機器の速度を加速させる前記仮想位置指令値を生成する加速処理ステップと、前記スレーブ機器の速度が前記最高速度以上である場合に最大速度を維持する前記仮想位置指令値を生成する定速処理ステップと、を有していても良い。

この一態様において、前記マスタ機器の位置情報が、前記スレーブ機器に許容された可動範囲を超えた位置を示す場合に前記マスタ機器の位置情報の値を前記スレーブ機器の可動範囲の上限値又は下限値に置き換える飽和処理ステップを有していても良い。

この一態様において、前記スレーブ機器の可動範囲内の値は、前記スレーブ機器の動作能力に基づき予め決定された値であっても良い。

本発明の別の態様は、マスタ機器の位置情報に基づきスレーブ機器の変位後の位置を指示する位置指令値を生成するマスタスレーブ機器の制御装置であって、前記マスタ機器の位置情報と、前記スレーブ機器の位置情報と、を取得する位置センサと、前記マスタ機器の位置と前記スレーブ機器の位置との差分値を算出する位置差算出部と、前記スレーブ機器の可動範囲内の値から前記スレーブ機器を予め定められた加速度で加速させる加速位置指令値と、前記スレーブ機器を定速で制御する定速位置指令値と、前記スレーブ機器を予め定められた減速度で減速させる減速位置指令値と、を算出する保護処理部と、前記スレーブ機器に与える位置指令値として前記加速位置指令値と、前記定速位置指令値と、前記減速位置指令値と、のいずれを与えるかを前記スレーブ機器の位置及び速度に基づき選択する仮想位置指令値判別部と、を有する制御装置である。

この別の態様において、前記スレーブ機器が現速度から停止状態となるまでに要する停止距離を算出し、前記マスタ機器の位置情報により示される目標位置との位置差が前記停止距離と等しくなる位置を減速開始位置として算出する減速開始位置算出部をさらに有し、前記仮想位置指令値判別部は、前記スレーブ機器が前記減速開始位置に達したことに応じて前記減速位置指令値を前記位置指令値として選択することができる。

この別の態様において、前記減速開始位置算出部は、現在の前記スレーブ機器の位置情報と、１つ前の制御周期の前記スレーブ機器の位置情報と、に基づき前記停止距離を算出しても良い。

この別の態様において、前記仮想位置指令値判別部は、前記スレーブ機器の速度が予め定められた最高速度未満である場合に前記加速位置指令値を前記位置指令値として選択し、前記スレーブ機器の速度が前記最高速度以上である場合に前記定速位置指令値を前記位置指令値として選択することができる。

この別の態様において、前記マスタ機器の位置情報が、前記スレーブ機器に許容された可動範囲を超えた位置を示す場合に前記マスタ機器の位置情報の値を前記スレーブ機器の可動範囲の上限値又は下限値に置き換える飽和処理部を有していても良い。

この別の態様において、前記スレーブ機器の可動範囲内の値は、前記スレーブ機器の動作能力に基づき予め決定された値であっても良い。

また、本発明は、上記制御装置と同様の技術思想に基づくシステムである。

本発明によれば、スレーブ機器の機構の保護をより確実なものとすることができる。

実施の形態１にかかる制御装置のブロック図である。 実施の形態１にかかるマスタスレーブシステムにおけるマスタからの入力位置指令値と飽和位置指令値との関係を示す図である。 実施の形態１にかかる飽和処理部の入出力関係を示すグラフである。 実施の形態１にかかる制御装置における減速開始位置について説明するための図である。 実施の形態１にかかる制御装置における減速開始位置について説明するための図である。 実施の形態１にかかる仮想位置指令値判別部において仮想位置指令値の判別に用いる加減速状態管理表の一例を示す図である。 実施の形態１にかかる制御装置の処理フローを示すフローチャートである。 実施の形態１にかかる制御装置によるマスタスレーブシステムの動作を示すフローチャートである。 実施の形態１にかかる制御装置における第１の動作例を示す図である。 実施の形態１にかかる制御装置における第２の動作例を示す図である。 実施の形態１にかかる制御装置における第３の動作例を示す図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。本発明にかかるマスタスレーブシステムは、マスタ機器と、スレーブ機器と、制御装置とを有する。以下で説明する実施の形態では、制御装置により本発明にかかるマスタスレーブ制御方法を実現する。そして、制御装置は、マスタ機器から位置情報を受け取り、当該位置情報に基づき位置指令値（本発明では仮想位置指令値と称す）を生成し、当該仮想位置指令値によりスレーブ機器を動作させる。また、以下の説明では、マスタ機器とスレーブ機器は、それぞれ動作機構としてモータを有し、制御装置は、当該モータの回転位置に基づきマスタ機器の位置とスレーブ機器の位置を認識するものとする。

また、制御装置は、コンピュータ等の演算装置上で動作するソフトウェアにより実現される。ソフトウェアは、ＣＤ−ＲＯＭ、ハードディスク、メモリ等の情報媒体に格納され、演算装置は当該情報媒体からソフトウェアを読み出して、当該ソフトウェアを実行するものとする。なお、当該ソフトウェアによって実現される機能をハードウェアによって実装することで制御装置を実現することもできる。

実施の形態１にかかる制御装置１のブロック図を図１に示す。図１に示すブロック図は、ソフトウェアにより実現される機能をブロック図として表したものである。図１に示すように、制御装置１は、マスタ位置センサ１０、飽和処理部１１、スレーブ位置センサ１２、速度最大値保持部１３、加減速最大時間保持部１４、加減速カウンタ１５、位置差算出部１６、減速開始位置算出部１７、保護処理部１８、仮想位置指令値判別部２２、ポートスイッチ２３、スレーブ位置制御部２４を有する。なお、実施の形態１にかかる制御装置１は、処理周期毎にマスタ機器の位置情報を取得して、取得した位置情報に基づきスレーブ機器に対する位置指令値（以下、仮想位置指令値と称す）を生成し、当該仮想位置指令値に基づきスレーブ機器の位置制御を行うものとする。

マスタ位置センサ１０は、マスタ機器の位置情報を取得して、マスタ機器の変位量を示す入力位置指令値ＭａｓＰｏｓを出力する。飽和処理部１１は、マスタ機器の位置情報ＭａｓＰｏｓが、スレーブ機器に許容された可動範囲を超えた位置を示す場合にマスタ機器の位置情報（例えば、入力位置指令値ＭａｓＰｏｓの値をスレーブ機器の可動範囲の上限値又は下限値に置き換えて、飽和位置指令値ＳＭＰを出力する。

ここで、飽和処理部１１の処理について詳細に説明する。まず、マスタからの入力位置指令値と飽和位置指令値との関係を示す図を図２に示す。図２に示すように、スレーブ機器には、動作可能範囲が上限可動制限値と下限可動制限値とにより制限される。一方、マスタ機器の可動範囲は、スレーブ機器の可動範囲よりも大きいことがある。そのため、マスタ機器より入力される入力位置指令値ＭａｓＰｏｓは、スレーブ機器の動作可能範囲よりも大きな値を示すことがある。そこで、飽和処理部１１では、取り得る値の範囲がスレーブ機器の動作可能範囲に対応する飽和位置指令値ＳＭＰを生成する。なお、飽和位置指令値ＳＭＰ及び入力位置指令値ＭａｓＰｏｓは、ともにスレーブ機器の基準位置からの差分値を示すものである。

また、飽和処理部１１の入出力特性を示すグラフを図３に示す。図３に示すように、飽和処理部１１は、入力位置指令値ＭａｓＰｏｓが上限可動制限値と下限可動制限値との間の値である場合は飽和位置指令値ＳＭＰとして入力位置指令値ＭａｓＰｏｓをそのまま出力する。一方、飽和処理部１１は、入力位置指令値ＭａｓＰｏｓが上限可動制限値を超える値である場合は上限可動制限値を飽和位置指令値ＳＭＰとして出力し、入力位置指令値ＭａｓＰｏｓが下限可動制限値を超える値である場合は下限可動制限値を飽和位置指令値ＳＭＰとして出力する。

スレーブ位置センサ１２は、スレーブ機器の現時点での位置を示すスレーブ位置情報ＳｌａＰｏｓとして出力する。

速度最大値保持部１３は、スレーブ機器を駆動するモータの最大回転速度から算出される速度最大値ＭａｘＶｅｌを保持する。速度最大値保持部１３には予め算出された速度最大値ＭａｘＶｅｌが格納される。この速度最大値ＭａｘＶｅｌは、モータの速度をＭｒ［ｒｐｍ］とすると、（１）式より算出することができる。

加減速最大時間保持部１４は、加減速最大時間ＭａｘＡｃｃを保持する。加減速最大時間はモータが停止した状態から最高速度に達するまでに要する時間であり、モータの性能から算出可能な値である。

加減速カウンタ１５は、加速処理が行われた回数と減速処理が行われた回数との差を示す加減速カウント値ＡｃｃＬを出力する。この加減速カウント値は、処理が行われた回数から算出される論理的な値である。より具体的には、加減速カウンタ１５は、加速処理が行われた場合には制御周期毎に加減速カウント値ＡｃｃＬを増加させ、減速処理が行われた場合には制御周期毎に加減速カウント値ＡｃｃＬを減少させる。実施の形態１にかかる制御装置１では、加減速カウント値ＡｃｃＬに１回の制御周期の時間を乗算することで、この加減速カウント値ＡｃｃＬを加速時間として利用する。

位置差算出部１６は、マスタ機器の位置とスレーブ機器の位置との差分値ＰｏｓＤｅｆを算出する。より具体的には、位置差算出部１６は、飽和位置指令値ＳＭＰとスレーブ位置情報ＳｌａＰｏｓとの差分値ＰｏｓＤｅｆを出力する。

減速開始位置算出部１７は、スレーブ機器が現速度から停止状態となるまでに要する停止距離を算出し、マスタ機器の位置情報により示される目標位置との位置差が停止距離と等しくなる位置を減速開始位置ＤｅｃＰｏｓとして算出する。つまり、スレーブ機器は、減速開始位置ＤｅｃＰｏｓから減速処理を開始すれば目標位置で停止することができる。また、減速開始位置算出部１７は、スレーブ機器の現在の速度を示す現速度Ｖｅｌを算出する。この現速度Ｖｅｌは、スレーブ機器の実際の位置を示すスレーブ位置情報ＳｌａＰｏｓから算出されるものであり、モータの回転誤差等を加味した値となる。

より具体的には、減速開始位置算出部１７は、スレーブ機器が現速度から停止状態となるまでに要する停止距離を算出し、マスタ機器の位置情報により示される目標位置との位置差が停止距離と等しくなる位置を減速開始位置として算出する。

ここで、減速開始位置ＤｅｃＰｏｓの算出方法について詳細に説明する。まず、減速開始位置ＤｅｃＰｏｓについて説明するための図を図４、図５に示す。図４に示す例は、スレーブ機器の速度変化が台形型で遷移する場合におけるスレーブ機器の速度変化とスレーブ機器の位置の変位を示す図である。このように、スレーブ機器の速度変化が台形型となるのは、スレーブ機器とマスタ機器との位置差が大きく、スレーブ機器が最高速度で定速移動する期間がある場合である。また、図５に示す例は、、スレーブ機器の速度変化が三角形型で遷移する場合におけるスレーブ機器の速度変化とスレーブ機器の位置の変位を示す図である。このように、スレーブ機器の速度変化が三角形型となるのは、マスタ機器とスレーブ機器との位置差が小さくスレーブ機器が最高速度に達する前に、減速を開始しなければマスタ位置で停止できない場合である。このように、スレーブ機器の速度と、マスタ機器とスレーブ機器との位置差と、に応じて位置が変動する。

減速開始位置算出部１７においては、このような停止開始位置ＤｅｃＰｏｓを以下のような計算により算出する。まず、減速開始位置算出部１７は、減速に要した時間を示す減速時間Ｔｓを（２）式〜（４）式を用いて算出する。

（２）式〜（４）式において、［］内の単位は各項の単位を示すものであり、ＳｌａＰｐｓはスレーブ位置情報であり、Ｖｅｌはスレーブ機器の現速度であり、ＭａｘＡｃｃは加減速最大時間であり、ＭａｘＶｅｌは速度最大値であり、ＡｃｃＲはスレーブ機器の実際の位置情報から算出されたスレーブ機器の現実の加速状態を示すものであり、Ｔｓは減速に要した時間を示す減速時間である。以下、同じ記号は同じ意味で利用する。また、（２）式で算出された現速度Ｖｅｌは、減速開始位置算出部１７から仮想位置指令値判別部２２に出力される。

続いて、減速開始位置算出部１７では、（４）式で算出された値を利用して減速処理を行う期間中のスレーブ機器の移動量を示す減速移動量Ｍｓを（５）式を用いて算出する。

そして、減速開始位置算出部１８は、（６）式に基づき減速開始位置ＤｅｃＰｏｓを算出する。なお、（６）式におけるＳＭＰは、飽和処理部１１から出力されるマスタ機器の位置情報ＳＭＰである。

保護処理部１８は、位置指令値としてスレーブ機器の動作可能範囲内の値から算出された仮想位置指令値を生成する。より具体的には、保護処理部１８は、加速移動量算出部１９、定速移動量算出部２０、減速移動量算出部２１を有する。

加速移動量算出部１９は、予め定められた加速度でスレーブ機器を加速させる仮想位置指令値（例えば、加速位置指令値）ｉｎｓ１を生成する。より具体的には、加速位置指令値ｉｎｓ１は、加速処理が開始された時点でのスレーブ機器の位置を示す加速開始位置に位置変位量を加算することで生成される。加速移動量算出部１９では、この位置変位量の増加量を制御周期の回数が増加する毎に大きくする。加速移動量算出部１９は、（７）式及び（８）式に基づき加速位置指令値ｉｎｓ１を生成する。なお、加速移動量算出部１９は、加速処理を開始する時点におけるスレーブ位置情報ＳｌａＰｏｓを保持し、加速処理を再開するごとに保持するスレーブ位置情報ＳｌａＰｏｓを更新するものとする。

なお、（８）式におけるＡｃｃＬは加減速カウンタ１５から出力される加減速カウント値である。

定速移動量算出部２０は、予め定められた速度（例えば、最大速度値ＭａｘＶｅｌで示される最高速度）でスレーブ機器を制御する仮想位置指令値（例えば、定速位置指令値）ｉｎｓ２を生成する。より具体的には、定速位置指令値ｉｎｓ２は、スレーブ位置情報ＳｌａＰｏｓに、スレーブ機器において定められている能力に基づき算出される定速増加量を、加えて制御周期終了後のスレーブ機器の位置を指定する定速位置指令値ｉｎｓ２を生成する。定速移動量算出部２０は、（９）式及び（１０）式に基づき定速位置指令値ｉｎｓ２を生成する。

減速移動量算出部２１は、予め定められた減速度でスレーブ機器を減速させる仮想位置指令値（例えば、減速位置指令値）ｉｎｓ３を生成する。より具体的には、減速位置指令値ｉｎｓ３は、減速処理の開始時点のマスタ位置を示す減速終了位置（つまり、目標位置）から位置変位量を減算することで生成される。減速移動量算出部２１では、この位置変位量の増加量を制御周期の回数が増加する毎に小さくする。減速移動量算出部２１は、（１１）式に基づき減速位置指令値ｉｎｓ３を生成する。

続いて、仮想位置指令値判別部２２及びポートスイッチ２３について説明する。ポートスイッチ２３は、加速移動量算出部１９、定速移動量算出部２０、減速移動量算出部２１から仮想位置指令値ｉｎｓ１〜ｉｎｓ３を受ける。そして、ポートスイッチ２３は、仮想位置指令値判別部２２が指定する仮想位置指令値を選択して最終的な仮想位置指令値ｉｎｓを出力する。この仮想位置指令値ｉｎｓは、スレーブ位置制御部２４に与えられる。スレーブ位置制御部２４は、スレーブ機器の位置を与えられた仮想位置指令値ｉｎｓに基づき制御する。

仮想位置指令値判別部２２は、保護処理部１８が出力するスレーブ機器に与える仮想位置指令値として加速位置指令値ｉｎｓ１と、定速位置指令値ｉｎｓ２と、減速位置指令値ｉｎｓ３と、のいずれを与えるかをスレーブ機器の位置及び速度に基づき選択する。

ここで、仮想位置指令値判別部２２が選択する仮想位置指令値を決定する手順についてより詳細に説明する。保護処理部１８は、仮想位置指令値ｉｎｓ１〜ｉｎｓ３を出力するが、仮想位置指令値判別部２２が指定する仮想位置指令値は１つである。仮想位置指令値判別部２２は、スレーブ機器が減速開始位置ＤｅｃＰｏｓに達したことに応じて減速移動量算出部２２が生成した減速位置指令値ｉｎｓ３を選択する選択信号を出力する。仮想位置指令値判別部２２は、スレーブ機器の速度が予め定められた速度最大値ＭａｘＶｅｌ未満である場合には加速移動量算出部１９が生成した加速位置指令値ｉｎｓ１を選択する選択信号を出力し、スレーブ機器の速度が速度最大値ＭａｘＶｅｌ以上である場合には定速移動量算出部２０が生成した定速位置指令値ｉｎｓ２を選択する選択信号を出力する。

また、仮想位置指令値判別部２２は、スレーブ機器が動作している状態において現在のモータの回転方向と逆の回転方向の動作が指示された場合にスレーブ機器のモータに過大な負荷がかかることを防止するために、スレーブ機器の加減速状態を管理する。より具体的には、仮想位置指令値判別部２２は、加減速状態管理表を有する。この加減速状態管理表は、加速処理、定速処理、減速処理の各処理状態の間の状態遷移の制限を定義するものである。

そこで、加減速状態管理表の一例を図６に示す。図６に示すように、加減速状態管理表では、マスタ機器の位置とスレーブ機器の位置との大小関係が異なる３つの条件と、スレーブ機器の軸の回転方向が異なる３つの条件と、の組み合わせた９つの条件のそれぞれがどのような加速状態となるかを示す。そして、加減速状態管理表では、１つの加減速状態から遷移しうる他の加減速状態を矢印で示した。

この加減速状態管理表に基づき加減速状態を制御した場合、図６において、ハッチングが施された矢印で示される状態遷移が行われる。より具体的には、マスタ機器が＋方向の回転動作から急に−方向の回転動作に切り替わった場合、スレーブ機器は、＋方向の回転動作を行い、速度をゼロに減速した後に、−方向の回転動作に移行する。また、同じ回転方向で動作中にマスタ位置が変位した場合は、減速動作から加速動作の間で加減速状態が遷移する。

続いて、実施の形態１にかかる制御装置１の全体の動作について説明する。そこで、図７に制御装置１の処理フローを示すフローチャートを示す。

図７に示すように、実施の形態１にかかる制御装置１では、制御周期の開始に合わせて、マスタ機器の位置情報とスレーブ機器の位置情報とを取得する位置情報取得ステップを実行する（ステップＳ１）。そして、取得したマスタ機器の位置情報（例えば、入力位置指令値）から飽和位置指令値ＳＭＰを生成する（ステップＳ２）。続いて、制御装置１は、減速開始位置計算部１７において現速度Ｖｅｌ及び加減速カウント値ＡｃｃＲを算出する（ステップＳ３）。

続いて、制御装置１は、仮想位置指令値判定部２２において現在のスレーブ機器の速度がゼロであるか否かを判断する（ステップＳ４）。このステップＳ４において、現在のスレーブ機器の速度がゼロでなかった場合、マスタ機器の位置とスレーブ機器の位置との位置差の判定処理（ステップＳ５）は行わずに、ステップＳ６の機構保護処理に基づくスレーブ機器の位置制御を行う。一方、ステップＳ４において、現在のスレーブ機器の速度がゼロでなかった場合、ステップＳ５のマスタ機器の位置とスレーブ機器の位置との位置差の判定処理を行う。

ステップＳ５において、前記スレーブ機器の位置と、の差分値を算出する位置差算出ステップを実行する。そして、ステップＳ５において、マスタ機器の位置とスレーブ機器の位置との位置差がゼロ（つまり、マスタ機器の位置とスレーブ機器の位置とが一致している状態）であると判断された場合、スレーブ機器の位置制御は行わずに処理を終了する。なお、仮想位置指令値判別部２２では、マスタ機器の位置とスレーブ機器の位置との差分値ＰｏｓＤｅｆに基づきステップＳ５の位置差の判断処理を行う。一方、ステップＳ５において、マスタ機器の位置とスレーブ機器の位置との位置差があると判断された場合、ステップＳ６のスレーブ機器の位置制御を行う。

ステップＳ６の保護処理ステップでは、スレーブ機器の変位後の位置を指示する位置指令値としてスレーブ機器の動作可能範囲内の値から算出された仮想位置指令値を生成し、当該仮想位置指令値によりスレーブ機器をマスタ機器の位置に追従させる。より具体的には、保護処理ステップでは、仮想位置指令値ｉｎｓとして保護処理部１８が出力する仮想位置指令値ｉｎｓ１〜ｉｎｓ３のいずれかを選択する。ステップＳ６では、まず、減速開始位置算出部１７において減速開始位置ＤｅｃＰｏｓの算出処理を行う（ステップＳ１０）。ステップＳ１０では、（２）式〜（６）式に基づき減速開始位置ＤｅｃＰｏｓを算出する。そして、仮想位置指令値判定部２２では、この減速開始位置ＤｅｃＰｏｓとスレーブ機器の位置情報ＳｌａＰｏｓとに基づき、スレーブ機器と減速開始位置ＤｅｃＰｏｓとの位置関係についての判定処理を行う（ステップＳ１１）。

ステップＳ１１においてスレーブ機器の位置ＳｌａＰｏｓが未だに減速開始位置ＤｅｃＰｏｓに達していないと判断された場合、スレーブ機器の現時点での速度についての判定処理が行われる（ステップＳ１２）。ステップＳ１２において、スレーブ機器の速度Ｖｅｌが速度最大値ＭａｘＶｅｌよりも遅いと判断された場合、制御装置１は、仮想位置指令値ｉｎｓとして加速移動量算出部１９が生成した仮想位置指令値（例えば、加速位置指令値ｉｎｓ１）を選択する（ステップＳ１３）。このステップＳ１３の処理が行われる一連の処理フローが加速処理ステップに相当する処理である。一方、ステップＳ１２において、スレーブ機器の速度Ｖｅｌが速度最大値ＭａｘＶｅｌ以上であると判断された場合、制御装置１は、仮想位置指令値ｉｎｓとして定速移動量算出部２０が生成した仮想位置指令値（例えば、定速位置指令値ｉｎｓ２）を選択する（ステップＳ１４）。このステップＳ１４の処理が行われる一連の処理フローが定速処理ステップに相当する処理である。また、ステップＳ１１においてスレーブ機器が減速開始位置ＤｅｃＰｏｓを超えた位置にあると判断された場合、制御装置１は、仮想位置指令値ｉｎｓとして減速移動量算出部２１が生成した仮想位置指令値（例えば、減速位置指令値ｉｎｓ３）を選択する（ステップＳ１５）。このステップＳ１５の処理が行われる一連の処理フローが減速処理ステップに相当する処理である。

なお、ステップＳ１３で選択される加速位置指令値ｉｎｓ１は（８）式に基づき算出される値であり、ステップＳ１４で選択される定速位置指令値ｉｎｓ２は（１０）式に基づき算出される値であり、ステップＳ１５で選択される減速位置指令値ｉｎｓ３は（１１）式に基づき算出される値である。

そして、実施の形態１にかかる制御装置１は、ステップＳ１３〜Ｓ１５で選択した仮想位置指令値に基づきスレーブ機器の位置制御を行い、１回の制御周期を終了する（ステップＳ７）。

続いて、実施の形態１にかかる制御装置１により制御されるマスタスレーブシステムの動作を説明する。図８に実施の形態１にかかる制御装置１により制御されるマスタスレーブシステムの動作を示す。

図８に示すように、制御装置１では、マスタ機器に位置の変位が生じたことを検出することでスレーブ機器の位置制御が開始される。制御装置１は、マスタ機器に位置の変位が生じたことを検出した場合、まず、加速処理を行う。この加速処理では、仮想位置指令値ｉｎｓとして加速移動量算出部１９が生成する仮想位置指令値ｉｎｓ１が選択される。そして、この加速処理は、スレーブ機器の速度が最高速度ＭａｘＶｅｌに達するか、スレーブ機器の位置が減速開始位置ＤｅｃＰｏｓに達するまで継続される。

スレーブ機器の速度が最高速度ＭａｘＶｅｌに達した場合、制御装置１は、スレーブ機器を定速処理により制御する。定速処理では、定速移動量算出部２０が生成する仮想位置指令値ｉｎｓ２が仮想位置指令値ｉｎｓとして選択される。そして、定速処理を継続している期間にスレーブ機器の位置が減速開始位置ＤｅｃＰｏｓに達した場合、制御装置１は、スレーブ機器の制御処理を定速処理から減速処理に遷移させる。また、加速処理中にスレーブ機器の位置が減速開始位置ＤｅｃＰｏｓに達した場合、制御装置１は、スレーブ機器の制御処理を加速処理から減速処理に遷移させる。

そして、制御装置１は、スレーブ機器の位置がマスタ位置と一致するまで減速処理を継続する。この減速処理では、減速移動量算出部２３が生成する仮想位置指令値ｉｎｓ３が仮想位置指令値ｉｎｓとして選択される。そして、減速処理により、スレーブ機器の位置とマスタ機器の位置とが一致し、かつ、スレーブ機器の速度がゼロになった場合には、制御装置１は制御状態を静止状態に遷移させる（状態遷移条件ＴＳ１）。しかし、減速処理中に減速開始位置ＤｅｃＰｏｓの位置がさらに遠くに変位した場合には、制御装置１は、再度加速処理を行う（状態遷移条件ＴＳ２）。また、減速処理中に減速開始位置ＤｅｃＰｏｓの位置が当初の減速開始位置ＤｅｃＰｏｓよりも手前の位置に変位した場合には、制御装置１は、スレーブ機器の速度がゼロとした後に加速処理を行う（状態遷移条件ＴＳ３）。

ここで、スレーブ機器の位置制御について具体例を示してさらに説明する。まず、状態遷移条件ＴＳ１による状態遷移が生じた場合のスレーブ機器の位置変位の一例を第１の動作例として図９に示す。図９に示すように、第１の動作例では、初期状態（例えばタイミングＴ１０）において、マスタ機器の位置とスレーブ機器の位置とが一致している。そして、タイミングＴ１１においてマスタ機器の位置に変位が生じる。そのため、タイミングＴ１１では、制御装置１がスレーブ機器を加速処理により制御する。また、タイミングＴ１１では、減速開始位置算出部１７が（４）式に基づき減速開始位置ＤｅｃＰｏｓを算出する。

そして、マスタ機器の位置がさらに進んだ場合、変位後のマスタ機器に追いつくためにスレーブ機器の加速処理が継続して行われる（タイミングＴ１２）。このとき、制御装置１は、スレーブ機器の速度に応じて減速開始位置ＤｅｃＰｏｓを更新する。その後、マスタ機器が所定の位置に達し、その状態を維持する場合、減速開始位置ＤｅｃＰｏｓは、マスタ機器が停止した後は同じ値を維持する（タイミングＴ１３）。そのため、制御装置１は、スレーブ機器が減速開始位置ＤｅｃＰｏｓに達した後に減速処理を開始する。そして、制御装置１は、スレーブ機器の速度がゼロに達し、かつ、マスタ機器の位置とスレーブ機器の位置とが一致するまで減速処理を継続する。

このように、スレーブ機器が減速開始位置ＤｅｃＰｏｓを通過した後にマスタ機器の停止位置が変化しない場合、スレーブ機器は、加速動作、定速動作、減速動作を順に行いマスタ機器の位置で停止する。なお、動作開始時と動作停止時のスレーブ機器の位置が近い場合、定速動作は行わずに、加速動作と減速動作のみとなる。

続いて、状態遷移条件ＴＳ２による状態遷移が生じた場合のスレーブ機器の位置変位の一例を第２の動作例として図１０に示す。図１０に示す例では、タイミングＴ２０でスレーブ機器が減速動作を開始している。このとき、マスタ機器の停止位置が変化しない場合、制御装置１はスレーブ機器の減速動作を継続する。しかし、図１０に示す例では、タイミングＴ２１で、マスタ機器の位置がさらに進む。そのため、タイミングＴ２１では、制御装置１で算出されている減速開始位置ＤｅｃＰｏｓが変化する。具体的には、（６）式のマスタ機器の位置情報ＳＭＰが変化するため、当該変化に基づき減速開始位置ＤｅｃＰｏｓが変化する。

そして、減速開始位置ＤｅｃＰｏｓが変化したことに伴い、図７のステップＳ１１の減速開始位置判定処理において、スレーブ機器の位置が減速開始位置ＤｅｃＰｏｓよりも遅れた位置にあると判断されるため、ステップＳ１２の速度判定が行われる。また、タイミングＴ２０、Ｔ２１では、スレーブ機器が減速動作を開始しているためステップＳ１２においてスレーブ機器の現速度Ｖｅｌが速度最大値ＭａｘＶｅｌよりも遅くなっていると判断される。従って、タイミングＴ２１では、制御装置１の仮想位置指令値判別部２２は、加速位置指令値ｉｎｓ１を仮想位置指令値として選択する。これにより、タイミングＴ２１では、制御装置１は、スレーブ機器の再加速処理を行う（図８の状態遷移条件ＴＳ２の遷移を行う。）

その後、スレーブ機器が新たな減速開始位置ＤｅｃＰｏｓを超えたことに応じて、制御装置１は、スレーブ機器を減速動作に移行させる（タイミングＴ２２）。そして、タイミングＴ２３において、スレーブ機器とマスタ機器との位置が一致する。制御装置１は、スレーブ機器とマスタ機器との位置が一致したことに応じてスレーブ機器の位置制御を静止状態に移行させる。

続いて、状態遷移条件ＴＳ３による状態遷移が生じた場合のスレーブ機器の位置変位の一例を第３の動作例として図１１に示す。図１１に示す例では、タイミングＴ３０でスレーブ機器が減速動作を開始している。このとき、マスタ機器の停止位置が変化しない場合、制御装置１はスレーブ機器の減速動作を継続する。しかし、図１１に示す例では、タイミングＴ３１で、マスタ機器の位置が現在のスレーブ機器の回転方向とは逆の方向に戻るつまり、図１１に示す例では、タイミングＴ３１においてマスタ機器の位置がタイミングＴ３０の停止位置より手前に移動する。そのため、タイミングＴ２１では、制御装置１で算出されている減速開始位置ＤｅｃＰｏｓが変化する。具体的には、（６）式のマスタ機器の位置情報ＳＭＰが変化するため、当該変化に基づき減速開始位置ＤｅｃＰｏｓが変化する。しかし、タイミングＴ３１では、スレーブ機器の位置が減速開始位置ＤｅｃＰｏｓよりも進んだ位置にあり（図７のステップＳ１１からステップＳ１５に進む枝）、かつ、速度Ｖｅｌがゼロでない（図７のステップＳ４のＮｏの枝）ため、制御装置１は、スレーブ機器の減速処理を継続する。

その後、タイミングＴ３２で、スレーブ機器の現速度Ｖｅｌがゼロになる。このとき、図１１に示す例では、スレーブ機器の位置とマスタ機器の位置に差があるため、制御装置１の仮想位置指令値判定部２２は、図７のステップＳ５の位置判断処理においてＮｏの枝に進む処理（ステップＳ６の機構保護処理を行う判断）を行う。そして、制御装置１は、図６の加減速状態管理表に従ってスレーブ機器の加速処理を行う。

その後、スレーブ機器が新たな減速開始位置ＤｅｃＰｏｓを超えたことに応じて、制御装置１は、スレーブ機器を減速動作に移行させる（タイミングＴ３３）。そして、タイミングＴ３３以降の期間に、スレーブ機器とマスタ機器との位置が一致したことに応じて、制御装置１は、スレーブ機器の位置制御を静止状態に移行させる。

上記説明より、実施の形態１にかかるマスタスレーブ制御方法では、入力位置指令値に代えて、スレーブ機器の動作可能範囲内の値（スレーブ機器の動作能力に基づき予め決定された値であって、例えば、速度最大値ＭａｘＶｅｌ、加減速最大時間ＭａｘＡｃｃ等）に基づき生成した仮想位置指令値（例えば、仮想位置指令値ｉｎｓ１〜ｉｎｓ３）を用いる。これにより、マスタ機器がスレーブ機器の能力を超える入力位置指令値を出力する異常状態においても、スレーブ機器に過大な負荷を与えることなくスレーブ機器の追従制御を行うことができる。

また、実施の形態１にかかるマスタスレーブ制御方法では、急な逆方向への方向転換等のマスタ機器に対する急制動があった場合においても、減速開始位置算出部１７が加減速状態管理表に基づきスレーブ機器に対する制御状態を管理することで、スレーブ機器が急制動に無理に追従することを防止する。そして、実施の形態１にかかるマスタスレーブ制御方法では、スレーブ機器に過大な負荷がかかることを防止することができる。特に、突発的な事故の発生によりマスタ機器に急激な変位が生じることがあるが、実施の形態１にかかるマスタスレーブ制御方法によれば、このような自体に対してもスレーブ機器の保護を行うことができる。

また、実施の形態１にかかるマスタスレーブ制御方法では、減速開始位置ＤｅｃＰｏｓの算出を実際のスレーブ機器の位置情報に基づき算出する。これにより、実施の形態１にかかるマスタスレーブ制御方法では、モータの回転誤差等を補正しながらスレーブ機器の位置制御を行うことができる。また、実施の形態１にかかるマスタスレーブ制御方法では、加速位置指令値ｉｎｓ１、定速位置指令値ｉｎｓ２、減速位置指令値ｉｎｓ３については制御時間から論理的に算出される加減速カウント値ＡｃｃＬに基づき算出する。これにより、実施の形態１にかかるマスタスレーブ制御方法では、仮想位置指令値ｉｎｓが誤差を含む不安定な値により変動することを防止し、スレーブ機器をより安定した状態で制御することができる。

また、実施の形態１にかかるマスタスレーブ制御方法は、例外処理によらず、通常の処理フロー中で入力位置指令値を仮想位置指令値に置き換える処理を行う。これにより、実施の形態１にかかるマスタスレーブ制御方法は、例外処理による機構保護処理に比べて制御論理を簡略化することができる。

また、実施の形態１にかかるマスタスレーブ制御方法は、スレーブ機器の機構に併せた仮想位置指令値を行うため、マスタ機器とスレーブ機器の機構が異なる構成を有している場合であっても、確実にスレーブ機器の機構を保護することができる。特に、実施の形態１にかかるマスタスレーブ制御方法では、入力位置指令値ＭａｓＰｏｓに対して飽和処理を施す。これにより、実施の形態１にかかるマスタスレーブ制御方法は、スレーブ機器の動作範囲では対応できない過大な入力位置指令値ＭａｓＰｏｓが入力された場合であってもスレーブ機器が過大な入力位置指令値ＭａｓＰｏｓに無理な追従を行うことを防止して、スレーブ機器の機構の保護をおこなうことができる。

ロボットアーム等の複数の動作軸を有するシステムでは、軸毎に可動許容範囲が異なる。実施の形態１にかかるマスタスレーブ制御方法により制御されるマスタスレーブシステムでは、軸の特性に合わせた仮想位置指令値を生成することができる。そのため、ロボットアームのような多軸のシステムにおいて、実施の形態１にかかるマスタスレーブ制御方法を採用することで、各軸を無理なく加減速させることが可能になり、軸の耐久性及び耐故障性の高いシステムを構成することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

１ 制御装置
１０ マスタ位置センサ
１１ 飽和処理部
１２ スレーブ位置センサ
１３ 速度最大値保持部
１４ 加減速最大時間保持部
１５ 加減速カウンタ
１６ 位置差算出部
１７ 減速開始位置算出部
１８ 保護処理部
１９ 加速移動量算出部
２０ 定速移動量算出部
２１ 減速移動量算出部
２２ 仮想位置指令値判別部
２３ ポートスイッチ
２４ スレーブ位置制御部

Claims (12)

1. マスタ機器の位置変位に追従してスレーブ機器の位置を制御するマスタスレーブシステムのマスタスレーブ制御方法であって、
   前記マスタ機器の位置情報と、前記スレーブ機器の位置情報と、を取得する位置情報取得ステップと、
   前記マスタ機器の位置、前記スレーブ機器の位置と、の差分値を算出する位置差算出ステップと、
   前記スレーブ機器の変位後の位置を指示する位置指令値として前記スレーブ機器の可動範囲内の値から算出された仮想位置指令値を生成し、当該仮想位置指令値により前記スレーブ機器を前記マスタ機器の位置に追従させる保護処理ステップと、
   前記スレーブ機器が現速度から停止状態となるまでに要する停止距離を算出し、前記マスタ機器の位置情報により示される目標位置との位置差が前記停止距離と等しくなる位置を減速開始位置として算出する減速開始位置算出ステップと、を有し、
   前記保護処理ステップは、前記スレーブ機器が前記減速開始位置に達したことに応じて前記スレーブ機器を減速させる前記仮想位置指令値を生成し、当該仮想位置指令値により前記スレーブ機器を減速させる減速処理ステップを有するマスタスレーブ制御方法。
2. 現在の前記スレーブ機器の位置情報と、１つ前の制御周期の前記スレーブ機器の位置情報と、に基づき前記減速開始位置を算出する請求項１に記載のマスタスレーブ制御方法。
3. 前記保護処理ステップは、
   前記スレーブ機器の速度が予め定められた最高速度未満である場合に前記スレーブ機器の速度を加速させる前記仮想位置指令値を生成する加速処理ステップと、
   前記スレーブ機器の速度が前記最高速度以上である場合に最大速度を維持する前記仮想位置指令値を生成する定速処理ステップと、
   を有する請求項１又は２に記載のマスタスレーブ制御方法。
4. 前記マスタ機器の位置情報が、前記スレーブ機器に許容された可動範囲を超えた位置を示す場合に前記マスタ機器の位置情報の値を前記スレーブ機器の可動範囲の上限値又は下限値に置き換える飽和処理ステップを有する請求項１乃至３のいずれか１項に記載のマスタスレーブ制御方法。
5. 前記スレーブ機器の可動範囲内の値は、前記スレーブ機器の動作能力に基づき予め決定された値である請求項１乃至４のいずれか１項に記載のマスタスレーブ制御方法。
6. マスタ機器の位置情報に基づきスレーブ機器の変位後の位置を指示する位置指令値を生成するマスタスレーブ機器の制御装置であって、
   前記マスタ機器の位置情報と、前記スレーブ機器の位置情報と、を取得する位置センサと、
   前記マスタ機器の位置と前記スレーブ機器の位置との差分値を算出する位置差算出部と、
   前記スレーブ機器の可動範囲内の値から前記スレーブ機器を予め定められた加速度で加速させる加速位置指令値と、前記スレーブ機器を定速で制御する定速位置指令値と、前記スレーブ機器を予め定められた減速度で減速させる減速位置指令値と、を算出する保護処理部と、
   前記スレーブ機器に与える位置指令値として前記加速位置指令値と、前記定速位置指令値と、前記減速位置指令値と、のいずれを与えるかを前記スレーブ機器の位置及び速度に基づき選択する仮想位置指令値判別部と、
   を有する制御装置。
7. 前記スレーブ機器が現速度から停止状態となるまでに要する停止距離を算出し、前記マスタ機器の位置情報により示される目標位置との位置差が前記停止距離と等しくなる位置を減速開始位置として算出する減速開始位置算出部をさらに有し、
   前記仮想位置指令値判別部は、前記スレーブ機器が前記減速開始位置に達したことに応じて前記減速位置指令値を前記位置指令値として選択する請求項６に記載の制御装置。
8. 前記減速開始位置算出部は、現在の前記スレーブ機器の位置情報と、１つ前の制御周期の前記スレーブ機器の位置情報と、に基づき前記減速開始位置を算出する請求項７に記載の制御装置。
9. 前記仮想位置指令値判別部は、
   前記スレーブ機器の速度が予め定められた最高速度未満である場合に前記加速位置指令値を前記位置指令値として選択し、前記スレーブ機器の速度が前記最高速度以上である場合に前記定速位置指令値を前記位置指令値として選択する請求項６乃至８のいずれか１項に記載の制御装置。
10. 前記マスタ機器の位置情報が、前記スレーブ機器に許容された可動範囲を超えた位置を示す場合に前記マスタ機器の位置情報の値を前記スレーブ機器の可動範囲の上限値又は下限値に置き換える飽和処理部を有する請求項６乃至９のいずれか１項に記載の制御装置。
11. 前記スレーブ機器の可動範囲内の値は、前記スレーブ機器の動作能力に基づき予め決定された値である請求項６乃至１０のいずれか１項に記載の制御装置。
12. 請求項６〜請求項１１のいずれか１項に記載の制御装置と、
    前記マスタ機器と、
    前記スレーブ機器と、を有するシステム。

Images