JP4807600B2 - 移動体システム - Google Patents

Description

この発明は移動体システムに関し、特に許容範囲内（インポジション）に移動体が停止できるか否かの判定に関する。

多軸の移動体に対して、第１軸の位置が所定範囲内（インポジション範囲内）に入ったことを条件に、第２軸を動作させることが多い。例えば天井走行車では、走行方向位置が所定範囲内に入ると、昇降台の昇降あるいは横送りを開始する。スタッカークレーンと無人搬送車等では、走行方向位置（及びスタッカークレーンでの昇降方向位置）が所定範囲内に入ると、スライドフォーク等の移載装置を動作させる。また工作機械などでは、第１軸のｘ方向位置あるいはｘｙ面内位置が所定範囲内に入ると、第２軸のｚ方向に沿って工具を移動させ、加工を開始する。

第１軸と第２軸とのシーケンス動作に対してインポジション判定が用いられ、第１軸の位置がインポジション範囲内に入ると、第２軸の動作を開始することが行われている。例えば特許文献１（JP2000-231412A）はｘｙ面内での移動に引き続いて、ｚ方向の移動を行う際に、ｘｙ面内での合成移動方向に関するインポジション判定を行い、２次元移動に対して１次元でのインポジション判定を行うことを開示している。

しかしながら現在位置がインポジション範囲内にあるか否かのみを用いると、インポジションと判定した後に、オーバーシュートにより移動体がインポジション範囲から抜け出すことがある。この状況を図６，図７により説明する。図６は振動なしに移動体が停止する状況を示し、図７は移動体の振動のためオーバーシュートが生じる状況を示す。図６，図７のa)は位置の軌跡を示し、b)は速度と位置との位相面上の軌跡を示し、c)はラフとファインの２段階のインポジション判定結果を示す。図６では移動体は振動せずに目的位置へ向けて減速し、オーバーシュートは生じない。これに対して図７では、位置と速度が振動し、位相面での軌跡は螺旋状で、一旦成立したインポジション判定を途中で取り消す必要がある。

JP2000-231412A

この発明の課題は、オーバーシュートによりインポジション範囲から抜け出す可能性の有無を、迅速かつ正確に判定することにある。

この発明は、移動体がインポジション範囲内に入った際にインポジション判定を行うシステムであって、移動体の位置を求めるためのセンサと、前記センサにより求めた移動体の位置の時系列データ｛Pi｝から速度の時系列データ｛vi｝を求めると共に、求めた速度の時系列データから加速度の時系列データ｛ai｝を求め、ここにiは時系列を表す添え字で、iは現在を表し、かつ現在位置Piから停止位置までの距離をほぼ −vi ^２ ／ai として求めることにより、移動体の停止位置がインポジション範囲内か否かを推定するための推定手段とを設けたことを特徴とする。

この発明では、現在位置がインポジション範囲内にあることと、推定停止位置もインポジション範囲内にあることの双方からインポジション判定を行う。従って、オーバーシュート等により、インポジション範囲から移動体が抜け出す可能性があれば、インポジションの判定は行わず、信頼性のある判定ができる。またこの発明では、移動体の実際の位置と速度と加速度とから判定し、移動体のモデルを用いる必要がない。このため移動体のモデル化に基づく誤差がない。

ここに「ほぼ」とは、 −vi^２／ai に０．８〜１．２程度の定数を乗算しても良く、あるいはインポジション範囲の１／１０〜１／１００程度のオフセットを加減算しても良いことを意味する。ほぼ −vi^２／ai の距離は停止位置までの距離の上限に相当するので、現在位置からほぼ −vi^２／ai だけ進んだ位置がインポジション範囲内にあれば、オーバーシュート等によりインポジション範囲から抜け出す可能性は実質的にないと判定できる。しかもこの判定は、簡単な演算で迅速にできる。

好ましくは、前記センサは移動体の位置を求めるリニアセンサとし、短周期で正確に第１軸方向の位置を測定する。

実施例の移動体システムの要部ブロック図 実施例でのインポジション判定部のブロック図 実施例でのインポジション判定アルゴリズムを示すフローチャート 実施例での停止位置の推定アルゴリズムを示すフローチャート 実施例での、位相面を用いた停止位置の推定を示す図 従来例でのインポジション判定を示し、a)は移動体が振動せずに目的位置へ接近する際の軌跡を示し、b)は移動体の位相面上の軌跡を示し、c)はラフインポジションの判定信号とファインインポジションの判定信号とを示す。 従来例でのインポジション判定を示し、a)は移動体がオーバーシュートしながら目的位置へ接近する際の軌跡を示し、b)は移動体の位相面上の軌跡を示し、c)はラフインポジションの判定信号とファインインポジションの判定信号とを示す。

以下に本発明を実施するための最適実施例を示す。この発明の範囲は、特許請求の範囲の記載に基づき、明細書の記載とこの分野での周知技術とを参酌し、当業者の理解に従って定められるべきである。

図１〜図５に、実施例の移動体システム２を示す。各図において、４は第１軸コントローラ、１０は第２軸コントローラで、各々サーボアンプ６，１２を介してモータＭ１，Ｍ２を駆動する。リニアセンサ８，１４は、各々第１軸方向の移動体の位置と第２軸方向の移動体の位置とを求め、コントローラ４，１０へ入力する。

インポジション判定部１６は、第１軸方向のリニアセンサ８からの位置の時系列データを元に、速度の時系列データを生成し、速度の時系列データから加速度の時系列データを生成する。そして現在位置と現在速度、及び現在の加速度から停止位置を推定し、これがインポジション範囲内にあるかどうかを判定する。インポジション判定部１６は、現在位置がインポジション範囲内で、推定停止位置もインポジション範囲内の際に、インポジションと判定し、これに基づいて第２軸コントローラ１０はモータＭ２を起動する。

コントローラ４〜インポジション判定部１６は移動体に設けるが、例えばモータＭ１，Ｍ２が地上１次のリニアモータで、移動体にモータＭ１，Ｍ２の２次側を設ける場合、コントローラ４〜インポジション判定部１６を地上側に設ける。リニアセンサ８，１４も移動体に設けても地上側に設けてもよく、リニアセンサ８，１４は例えば複数のコイルから成り、被検出用の磁気マークに対する位置をコイルのインダクタンスの変化から検出する。

図２に、インポジション判定部１６の構成を示す。位置データ記憶部２０はリニアセンサ８からの位置の時系列データ｛Pi｝を記憶し、演算部２３は位置の時系列データの差分から速度の時系列データ｛ｖi｝を生成し、速度データ記憶部２１が時系列データ｛ｖi｝を記憶する。演算部２３は速度の時系列データから加速度の時系列データ｛ａi｝を生成し、加速度データ記憶部２２で記憶する。演算部２３は、現在位置Piと現在の速度vi並びに現在の加速度aiを用い、停止位置を Pi−vi²／ai として求める。ここで負号は、減速時に加速度は負であることに対応する。推定停止位置は、厳密に Pi−vi²／ai である必要はなく、ほぼ Pi−vi²／ai であればよい。例えば vi²／ai の項に、０．８〜１．２程度の係数を乗算しても良く、あるいは Pi−vi²／aiの項にインポジション範囲の幅の1/10〜1/100程度のオフセットを加減算してもよい。

図３〜図５に、インポジション判定の手法を示す。ここではインポジションはラフインポジションとファインインポジションの２段階で判定するが、１段階としても、あるいは３段階以上としてもよい。現在位置がラフインポジション範囲内で推定停止位置もラフインポジション範囲内の場合に、ラフインポジションと判定する。次に現在位置もファインインポジション範囲内で推定停止位置もファインインポジション範囲内の場合に、ファインインポジションと判定する。

停止位置の推定メカニズムを示す。位置Piの時系列データから速度の時系列データviを得、これから加速度の時系列データａiを得る。停止推定位置は Pi−vi²／ai で与えられ、前記のように vi²／ai の項に０．８〜１．２の係数を乗算しても、あるいはインポジション範囲の1/10〜1/100程度のオフセットを加減算してもよい。なお位置Piの時系列データは、リニアセンサから短い周期で正確に得ることができるが、測定周期の長いレーザ距離センサなどから得ても良い。

Pi−vi²／ai の項の意味を図５に示す。図５は位置Piと速度viとの位相面を示し、位相面上の座標がＱ0，Ｑ1，Ｑ2，Ｑ3の順に目標停止位置へ接近しているものとする。なお目標停止位置で、位置と速度は共に０とする。ここでラフインポジション範囲内に入ったＱ1の時点で、例えば直前の点Ｑ0と結ぶ軌跡の接線（図５の破線）と位置軸との切片を求め、これがラフインポジション範囲内かどうかを判定する。また現在位置がファインインポジション範囲内に入ったＱ3の時点で、直前の点Ｑ2と点Ｑ3を結ぶ接線と位置軸との切片を求め、これがファインインポジション範囲内かどうかを判定する。ここでは現在位置と直前の２点から接線を発生させているが、例えば点Ｑ3，Ｑ2の中点を現在位置とし、点Ｑ1，Ｑ0の中点を１つ前の位置として、４点から接線を発生させても良い。

このようにして得られた切片の意味を説明する。Ｑ0〜Ｑ3などの点は、リニアセンサから得たもので、移動体の制御上のモデルから得たものではない。また移動体には目的位置で停止するように加速度を加えているので、実際には図５の白丸の列のように、図５の接線よりも目的位置に近い側で停止する。例えば移動体を等加速度運動で減速させた場合、停止位置は Pi−vi²／2ai となり、−vi²／aiの項は、等加速度運動の場合の２倍だけ現在位置から進んで停止するものと仮定している。このように、図５での位置軸との切片は、移動体の停止位置を最悪の状況で見積もったものである。

図５の評価では、現実の移動体の位置の時系列データから停止位置を推定し、移動体の制御上のモデルを含んでいない。このため移動体をモデル化する際の誤差の影響を受けない。従って目的停止位置からの偏差の上限を推定できる。また図５の推定を行う時点で、移動体の位置はラフインポジション範囲内、あるいはファインインポジション範囲内にあり、減速運動を行っているので、インポジション範囲の手前側で停止することもあり得ない。これらのため、インポジション範囲内で停止するかどうかを、正確にかつ簡単な演算で迅速に判定できる。

インポジション範囲内で停止できるか否かを、正確かつ迅速に判定できると、より正確な位置決めができるだけでなく、次の第２軸の運動をより速やかに開始させることができる。

２ 移動体システム
４，１０ コントローラ
６，１２ サーボアンプ
８，１４ リニアセンサ
１６ インポジション判定部
２０ 位置データ記憶部
２１ 速度データ記憶部
２２ 加速度データ記憶部
２３ 演算部
２４ 停止推定位置記憶部

Ｍ１，Ｍ２ モータ

Claims (1)

1. 移動体がインポジション範囲内に入った際にインポジション判定を行うシステムであって、
   移動体の位置を求めるためのセンサと、
   前記センサにより求めた移動体の位置の時系列データ｛Pi｝から速度の時系列データ｛vi｝を求めると共に、求めた速度の時系列データから加速度の時系列データ｛ai｝を求め、ここにiは時系列を表す添え字で、iは現在を表し、かつ現在位置Piから停止位置までの距離をほぼ −vi ^２ ／ai として求めることにより、移動体の停止位置がインポジション範囲内か否かを推定するための推定手段とを設けたことを特徴とする、移動体システム。

Images