JP3731846B2 - リンクアーム機構の制御方法 - Google Patents
リンクアーム機構の制御方法 Download PDFInfo
- Publication number
- JP3731846B2 JP3731846B2 JP07059498A JP7059498A JP3731846B2 JP 3731846 B2 JP3731846 B2 JP 3731846B2 JP 07059498 A JP07059498 A JP 07059498A JP 7059498 A JP7059498 A JP 7059498A JP 3731846 B2 JP3731846 B2 JP 3731846B2
- Authority
- JP
- Japan
- Prior art keywords
- slider
- moving member
- arm
- moved
- height
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、例えば自動遠心機のように検体試料のハンドリング機構を有する遠心機のリンクアーム機構の制御方法に関するものである。
【０００２】
【従来の技術】
この種のリンクアーム機構に関する公知例として特開昭５８−１６００７１号公報があり、移動部材を２つのスライダ間に配置させたリンクアーム機構について記載されているが、移動部材を垂直に上下させる制御手法に関しては詳細に触れていない。
【０００３】
【発明が解決しようとする課題】
上記した従来のリンクアーム機構に於いては、移動部材は水平に配置された２本の案内部材よりも下方のみ若しくは上方のみ移動可能であるが、摺動する２つのスライダ間に移動部材を配置しているため、上方から下方へ、又は下方から上方へと案内部材を跨って移動することはできず、可動範囲が狭いという欠点があった。この欠点を解決するためには、平行な２本の案内部材を鉛直方向に配置し、移動部材の左右のどちらか片側にスライダを２つ共配置すると、移動部材は案内部材を跨って上下移動することは可能となるが、移動部材を垂直に移動させるためには、２つのスライダが複雑な動きになるという新たな問題が生じる。
【０００４】
また、移動部材が穴部等に入っている状態で電源を投入し２つのスライダを原点位置へ移動させる場合、移動部材が穴部内壁に衝突しないように移動部材を垂直に上方へ移動させなければならない。更に移動部材を上方へ移動させようと駆動した後、実際に移動部材が所定の高さまで移動したかどうかが分からないという問題がある。
【０００５】
本発明の目的は、移動部材が垂直移動する際の移動部材の高さと２つのスライダの位置関係を表す関係式を開示し、移動部材を垂直移動させる制御方法を提供することにある。
【０００６】
本発明の他の目的は、移動部材を高速に垂直移動させる制御方法を提供することにある。
【０００７】
本発明の他の目的は、電源投入時であっても移動部材が穴部等の周囲の物と干渉衝突しないように、移動部材を垂直移動させる制御方法を提供することにある。
【０００８】
本発明の更なる他の目的は、移動部材が所定の高さまで移動したかどうかを確認しながら移動部材を移動させる制御方法を提供することにある。
【０００９】
【課題を解決するための手段】
上記した問題を解決するために、互いに平行な第１の案内部材と第２の案内部材と、該第１の案内部材上には駆動モータにより移動可能な第１のスライダを設け、同様に該第２の案内部材上にも駆動モータにより移動可能な第２のスライダを設け、該第１のスライダには前記第１の案内部材と第２の案内部材の軸芯の間隔より長い第１のアームの一端を軸着し、同様に該第２のスライダにも前記第１の案内部材と第２の案内部材の軸芯の間隔より長い第２のアームの一端を軸着し、前記第１のアームの他端と前記第２のアームの他端は作業手段を備えた移動部材に同一軸上で軸着し、該移動部材の片側に前記第１のスライダと第２のスライダを配置させ、前記第１のアームの長さをＬ１、前記第２のアームの長さをＬ２、前記第１のスライダと第２のスライダ間の上下方向の軸芯間の距離をｄ、前記移動部材の摺動方向の位置をｘ１、前記第１のスライダの摺動方向の位置をＡｘとするとき、前記第２のスライダの摺動方向の位置Ｂｘは、
【００１０】
という関係になるように前記第１のスライダと前記第２のスライダを制御することにより達成される。
【００１１】
また、前記移動部材の所望の速度カーブから、前記第１のスライダの速度カーブを計算する工程１と前記第２のスライダの速度カーブを計算する工程２を設け、前記移動部材を移動させる以前に前記工程１と前記工程２を計算し、移動時は得られた速度カーブに従い第１のスライダ及び第２のスライダを制御することにより達成される。
【００１２】
次に、前記スライダの少なくとも１つに前記アームの角度を検出する角度センサを設け、前記移動部材を移動させる前に該角度センサから前記移動部材の現在の高さを検出することにより達成される。
【００１３】
更に、前記移動部材を移動させた後、該角度センサから前記移動部材の高さを検出することにより達成される。
【００１４】
【発明の実施の形態】
本発明の具体的な実施例を図面を参照しながら以下詳細に説明する。本発明のリンクアーム機構１の制御方法を適用した自動遠心機１１０を図１１に示す。自動遠心機１１０は、真空採決管に血液等の検体１１１が通常５本入ったラック１１２を移動部材１６に取り付けられたハンド１５で掴み、リンクアーム機構１で遠心機内のバケット１１３に入れる。所定のラック数を入れた後、ロ−タ１１４を回転させ検体１１１を遠心分離する。分離後、バケット１１３からラック１１２をハンド１５により取り出し、ラック置場１１５へ置く。上述のようにラック１１２をハンドリングする際の移動部材１６の垂直動作について、図１を参照しながら説明する。図１は、リンクアーム機構１の構成図と、リンクアーム機構１を制御するＣＰＵボード２及びドライブボード３を示す。リンクアーム機構１は、断面がＨ形のベース４に互いに平行な第１の案内部材５と第２の案内部材６を備え、第１の案内部材５上を摺動する第１のスライダ７と第２の案内部材６上を摺動する第２のスライダ８を備えている。第１のスライダ７はタイミングベルト９に固定され、このタイミングベルト９を稼動する駆動モータ１０を回転させることにより左右へ移動し、同様に第２のスライダ８はタイミングベルト１１に固定され、このタイミングベルト１１を稼動する駆動モータ１２を回転させることにより左右へ移動する。また、第１のスライダ７には、第１のアーム１３の一端（A）が軸着され、同様に第２のスライダ８にも、第２のアーム１４の一端（B）が軸着されている。第１のアーム１３の他端と第２のアーム１４の他端は、ハンド等の作業手段１５を備えた移動部材１６に同一軸上（H）で軸着され、移動部材１６の片側へ上記２つのスライダ７，８を配置している。更に、移動部材１６と第１のスライダ７は、第１のアーム１３と平行な平行リンク１７で軸着され、移動部材１６の姿勢を一定に保っている。上記２つの駆動モータ１０，１２は、それぞれモータケーブル１８，１９とコネクタ２０を介してドライブボード３に接続されている。また、マイクロコンピュータ（以下ＭＣＵと称す）２７及びメモリ２８等から構成されるＣＰＵボード２と駆動モータ１０，１２を駆動するドライブボード３は、コネクタ２１，２２とフラットケーブル２３を介して接続されている。前述したように駆動モータ１０，１２をベース４に固定したリンクアーム機構１は、可動部に駆動モータを設けた直交形ロボットのようなケーブルの曲折が無いため、曲折による断線は解消される構成となっている。また停電等があった場合、駆動モータ１０，１２の位置保持力が無くなり移動部材１６の自重で落下する恐れがあるが、駆動モータ１０，１２のロータシャフトをバネ力で固定する電磁ロックブレーキ機能を持つモータを採用し、落下を防止している。
【００１５】
前述のように構成したリンクアーム機構１の座標定義図を図２に示す。座標軸はＸ−Ｚ軸の直交座標とし、点Ａは第１のスライダ７の軸芯，点Ｂは第２のスライダ８の軸芯，点Ｈはハンド等の作業手段１５を備えた移動部材１６の軸芯を表わす。第１のアーム１３の長さをＬ１，第２のアーム１４の長さをＬ２，第１のスライダ７と第２のスライダ８の上下方向つまりＺ軸方向の軸芯間の距離をｄとすると、点Ａは直線Ｚ＝Ｌ１上を、点Ｂは直線Ｚ＝Ｌ１−ｄ上を移動し、点ＨのＺ軸方向の可動範囲は０〜（Ｌ１＋Ｌ２−ｄ）となる。又、直線Ｚ＝Ｌ１と第１のアーム１３のなす角をα（−π／２≦α≦sin-1{(Ｌ２−ｄ)／Ｌ１｝）とし、点ＨからＸ軸方向へ垂線を下ろし直線Ｚ＝Ｌ１と交わる点をＥ，直線Ｚ＝Ｌ１−ｄと交わる点をＦとすると、線分ＨＥ，ＡＥ，ＨＦ，ＢＦの長さは、
【００１６】
となる。今、点Ｈが直線Ｘ＝ｘ１上を移動する場合、点Ａと点Ｂの座標をそれぞれ（Ａｘ，Ｌ１）（Ｂｘ，Ｌ１−ｄ）とすると、Ｘ軸方向の位置Ａｘ，Ｂｘは、
【００１７】
【００１８】
となり、式（５）と（６）式からαを消去すると、
【００１９】
が導かれる。式（７）は、点Ｈが直線Ｘ＝ｘ１上を垂直移動する時の点Ａと点Ｂの位置を表わす関係式であり、逆に第１のスライダ７と第２のスライダ８の位置を式（７）を満足するように制御することにより、移動部材１６は垂直移動が可能となる。
【００２０】
なお、式（５）及び式（６）は、図１及び図２に示すように、第１及び第２のスライダ７，８が移動部材１６の左側に位置している状態を想定して式化したものである。したがって、例えば第１及び第２のスライダ７，８が移動部材１６の右側に位置している状態を想定して式化すれば、上記式（５）〜式（７）は次のようになる。
【００２１】
【００２２】
しかし、第１及び第２のスライダ７，８が移動部材１６の左側に位置しているか、右側に位置しているかは、リンクアーム機構１を表から見るか裏から見るか程度の差でしかないため、式（７）と式（７ａ）の差は、Ｘ軸の方向をどちらに取るかの考え方の差でしかない。したがって、式（７）を満足する制御を行うということと、式（７ａ）を満足する制御を行うということは全く同じ意味である。
【００２３】
次に、点Ｈが図２に示すように直線Ｘ＝ｘ１上を移動前の既知の高さＨ1Zから移動先の所望の高さＨ2Zまで移動時間Ｔ秒経て移動する場合について説明する。先ず、移動部材１６の垂直移動中の時間ｔと角度αについて関係式α(t)を導く。移動部材１６の軸芯点ＨのＺ軸成分をＨｚとすると、
【００２４】
となる。時間ｔと高さＨｚの関係をcos関数の半周期を用いて、振幅を移動距離、角度を時間に置き換えて変換すると、
【００２５】
となり、時間ｔと高さＨｚの関係は図３に示すような曲線３０になる。ここで、Ｈ2Z−Ｈ1Z＝Ｍとし、式（８）（９）からαについて解くと、
【００２６】
となり、移動部材１６の垂直移動中の時間ｔと角度αについての関係式α(t)を得る。移動部材１６の高さＨｚと時間ｔをcos関数を用いたため、移動部材１６の速度カーブ４１は図４に示すような半周期のsin波となり、これを移動部材１６の所望の速度カーブ４１とする。
【００２７】
続いて、移動部材１６の所望の速度カーブ４１から、第１のスライダ７の速度カーブ４２を計算する工程１と、第２のスライダ８の速度カーブ４３を計算する工程２に関して、駆動モータ１０，１２にエンコーダ付きサーボモータを適用した場合の計算手法について説明する。サーボモータの制御は、制御計算周期毎にエンコーダの回転パルス信号をフィードバックしサーボモータの回転角度及び角速度を求め、指令速度とサーボモータの角速度の差から比例項・積分項・微分項の３つの項を加算するＰＩＤ演算を行い、ＰＷＭ（パルス・ワイド・モジュレーション）制御によりサーボモータを駆動制御する。制御計算周期の時間をΔｔとすると、移動中の時間ｔはΔｔの累積であるから、
【００２８】
で表わせられ、ｉは制御回数、ｎは移動部材１６を所望の高さＨ2Zまで移動させた時の制御回数を表わし、移動時間ＴはＴ＝ｎ・Δｔとなる。
【００２９】
フローチャート図５を用いて工程１の計算手法を、制御計算周期に制御回数を乗じた時間ｔiについて説明すると、処理５００で式（１０）からアーム角度α(ti)を求め、処理５０１で点ＨのＸ座標成分ｘ１及びアーム角度α(ti)を式（５）へ代入し第１のスライダ７の位置Ａｘ(ti)を計算する。処理５０２で制御計算周期毎の第１のスライダ７の位置Ａｘの差、つまり
【００３０】
を計算すると、Δｔ時間後の第１のスライダ７の移動距離が得られ、その移動距離とΔｔ時間から第１のスライダ７の速度を得る。更に、処理５０３で１回転当りの第１のスライダ７のピッチと第１のスライダ７の速度から、駆動モータ１０の指令速度に変換する。処理５０４でこれらの一連の計算をｉ＝０から順次ｎまで繰り返し行い、処理５０５で駆動モータ１０の指令速度データテーブルを作成し、ＣＰＵボード２のメモリ２８に記憶する。
【００３１】
次に工程２についてフローチャート図６を用いて説明する。時間ｔiの場合、処理６００で点ＨのＸ座標成分ｘ１及び工程１で求めたＡｘ(ti)を式（７）へ代入し、第２のスライダ８の位置Ｂｘ(ti)を得る。処理６０１で工程１と同様に、制御計算周期毎の第２のスライダ８の位置Ｂｘの差
【００３２】
から、Δｔ時間後の第２のスライダ８の移動距離が得られ、その移動距離から第２のスライダ８の速度を得る。更に、処理６０２で１回転当りの第２のスライダ８のピッチと第２のスライダ８の速度から、駆動モータ１２の指令速度に変換する。処理６０３でこれらの一連の計算をｉ＝０から順次ｎまで繰り返し行い、処理６０４で駆動モータ１２の指令速度データテーブルを作成し、ＣＰＵボード２のメモリ２８に記憶する。図４に工程１と工程２の計算より得た第１のスライダ７の速度カーブ４２と第２のスライダ８の速度カーブ４３を示す。
【００３３】
以上の工程１及び工程２を経て、移動部材１６を移動させる場合は、工程１で求めた速度カーブ４２になるようにＣＰＵボード２のメモリ２８に記憶させた駆動モータ１０の指令速度データを制御計算周期毎に順次読み出し、第１のスライダ７を速度制御する。同様に工程２で求めた速度カーブ４３になるようにＣＰＵボード２のメモリ２８に記憶させた駆動モータ１２の指令速度データを制御計算周期毎に順次読み出し、第２のスライダ８を速度制御する。速度制御の手法は、前述したようにエンコーダフィードバックによる角速度変換と指令速度の差からＰＩＤ演算により、駆動モータ１０，１２を制御する。２つの駆動モータ１０，１２の制御は、例えばＣＰＵ１個で時分割によりタスクを切り替える方式で同時に制御駆動し、移動部材１６を高さＨ1Zから所望の高さＨ2Zへ垂直に移動させる。
【００３４】
以上の説明では移動部材１６を上昇させる場合について説明したが、高さＨ2Zから高さＨ1Zへ下降させる場合は、ＣＰＵボード２のメモリ２８に記憶させた指令速度データの読み出し順番を逆にして駆動モータ１０，１２を制御する。また、移動部材１６を水平に移動させる場合は、駆動モータ１０，１２を同時に同じ指令速度で駆動すれば良い。
【００３５】
前述ではエンコーダ付きサーボモータを適用した場合の計算工程１と計算工程２について説明したが、以下駆動モータ１０，１２にパルスレートで駆動するステッピングモータを適用した場合について説明する。ステッピングモータを１ステップ回転させると、スライダ７，８は１ステップ回転角に相当する分だけ移動する。その１ステップ分の移動量ΔＡｘ，ΔＢｘは、
【００３６】
となる。ここで、ｓはステップ角度，１／Ｎは減速比，ｐはピッチ（mm/１回転）である。このようにステッピングモータの場合、パルスレートで駆動するためスライダ７，８の停止位置は離散的となる。そのため、離散的な位置とその位置を通過する時間からパルスレートを決定する計算手法を導く。今、点Ｈの移動前の既知の高さＨ1Zと移動先の所望の高さＨ2Zが与えられるとし、図４の移動部材１６の所望の速度カーブ４１を示す式（９）からＨ1Z−Ｈ2Z＝Ｍとして時間ｔについて解くと、
【００３７】
となる。式（１５）は時間ｔを移動部材１６の高さＨｚの関数で表わしたものである。
【００３８】
先ず、第１のスライダ７の速度カーブ４２を計算する工程１について説明する。移動部材１６の高さＨｚを第１のスライダ７の位置Ａｘで表現すると、
【００３９】
式（１６）を式（１５）に代入して、
【００４０】
を得る。式（１７）は時間ｔを第１のスライダ７の位置Ａｘの関数で表現したものであり、第１のスライダ７の離散的な位置をＡｘに代入するとその位置を通過する時間ｔが得られる。上式（１６）（１７）の±について図２を参照しながら説明すると、点Ｈの高さＨｚが直線Ｚ＝Ｌ１より上か下かで場合分けされる。移動開始時の計算上の点Ｈの高さＨｚ＝Ｈ1Zで直線Ｚ＝Ｌ１より下方のため−であり、直角三角形ＡＨＥの辺ＨＥが０になるまで計算上の点ＡをＸ軸負方向へΔＡｘずつ移動させる。次に計算上の点ＡをＸ軸正方向へΔＡｘ分移動させ、計算上の点Ｈの高さをＬ１より上方とみなし（＋）、点Ｈの高さがＬ１からＨ2Zまで計算上の点ＡをＸ軸の正方向へΔＡｘずつ移動させる。点Ｈの移動前の高さＨ1ZからＬ１までの、点ＡのＸ軸負方向へ移動する距離は、
【００４１】
となり、その移動パルス数は式（１８）の移動距離を１ステップ分の移動量ΔＡｘで割った数となる。点Ａの位置Ａｘは１ステップ移動前の位置Ａｘ−ΔＡｘより得て、式（１７）に代入し、時間ｔを求める。ΔＡｘをＡｘから順次引き更新し、式（１７）より点Ａの離散的な位置と通過時間を得て、その１ステップ分の時間の差からパルスレートを求める。パルス出力方式は、図７に示すようなブロック回路でＭＣＵ２７が有するタイマＡ７１の割り込み機能を用い、パルス周波数を変化させる方式とし、タイマクロック周期と移動前後の時間差から割り込み間隔のタイマ値Ａｔは、
【００４２】
より得る。次に点Ｈの高さＬ１から移動先のＨ2Zまでの、点ＡのＸ軸正方向へ移動する距離は、
【００４３】
となり、その移動パルス数は式（２０）の移動距離を１ステップ分の移動量ΔＡｘで割った数となる。同様に上述した計算方法により、１ステップ分の移動前後の時間差からタイマ値Ａｔを求め、各移動分のタイマ値をテーブル化し、ＣＰＵボード２のメモリ２８に記憶する。
【００４４】
続いて、第２のスライダ８の速度カーブ４３を計算する工程２について説明する。移動部材１６の高さＨｚを第２のスライダ８の位置Ｂｘで表現すると、
【００４５】
式（２１）を式（１５）に代入して、
【００４６】
を得る。式（２２）は時間ｔを第２のスライダ８の位置Ｂｘの関数で表現したものであり、前述した第１のスライダ７の場合と同様、第２のスライダ８の離散的な位置をＢｘに代入するとその位置を通過する時間ｔが得られる。上式（２１）（２２）の±については、点Ｈの高さＨｚが直線Ｚ＝Ｌ１−ｄより上か下かで場合分けされるが、計算上の点Ｈの高さＨｚが直線Ｚ＝Ｌ１−ｄより下方の場合は−であり、直角三角形ＢＨＦの辺ＨＦが０になるまで計算上の点ＢをＸ軸負方向へΔＢｘずつ移動させる。次に計算上の点ＢをＸ軸正方向へΔＢｘ分移動させ、計算上の点Ｈの高さをＬ１−ｄより上方とみなし（＋）、点Ｈの高さがＬ１−ｄからＨ2Zまで計算上の点ＢをＸ軸の正方向へΔＢｘずつ移動させる。点Ｈの移動前の高さＨ1ZからＬ１−ｄまでの、点ＢのＸ軸負方向へ移動する距離は、
【００４７】
となり、その移動パルス数は式（２３）の移動距離を１ステップ分の移動量ΔＢｘで割った数となる。点Ｂの位置Ｂｘは１ステップ移動前の位置Ｂｘ−ΔＢｘより得て、式（２２）に代入し時間ｔを求める。ΔＢｘをＢｘから順次引き更新し、式（２２）より点Ｂの離散的な位置と通過時間を得て、その１ステップ分の時間の差からパルスレートを求める。第１のスライダ７と同様、パルス出力方式をＭＣＵ２７が有するタイマＢ７２の割り込み機能を用い、パルス周波数を変化させる方式とし、タイマクロック周期と移動前後の時間差から割り込み間隔のタイマ値Ｂｔは、
【００４８】
より得る。次に点Ｈの高さＬ１−ｄから移動先のＨ2Zまでの、点ＢのＸ軸正方向への移動距離は、
【００４９】
となり、その移動パルス数は式（２５）の移動距離を１ステップ分の移動量ΔＢｘで割った数となる。同様に上述した計算方法により、１ステップ分の移動前後の時間差からタイマ値Ｂｔを求め、各移動分のタイマ値をテーブル化し、ＣＰＵボード２のメモリ２８に記憶する。ステッピングモータの場合もサーボモータと同様、各スライダ７，８の速度カーブは図４に示すような第１のスライダ７の速度カーブ４２及び第２のスライダ８の速度カーブ４３となる。又、メモリ２８には各スライダ７，８の移動方向別のパルス数とタイマ値が、図８に示すような構成で整然と格納されている。８０１は第１のスライダ７の負方向パルス数で、８０２は第１のスライダ７の正方向パルス数で、８０３は第２のスライダ８の負方向パルス数で、８０４は第２のスライダ８の正方向パルス数で、８０５は第１のスライダ７の負方向移動時のタイマ値テーブルで、８０６は第１のスライダ７の正方向移動時のタイマ値テーブルで、８０７は第２のスライダ８の負方向移動時のタイマ値テーブルで、８０８は第２のスライダ８の正方向移動時のタイマ値テーブルである。
【００５０】
ここで、駆動モータ１０，１２にステッピングモータを適用した時のブロック回路を示した図７と、フロチャート図９，図１０を参照しながら、前述した計算の工程１及び工程２より得たタイマ値を用いて、移動部材１６を上昇させる場合について説明する。図９は移動部材１６を移動させる時のサブルーチンであり、図１０はタイマ割り込み処理の上昇動作部分のフローチャートである。回転開始時、処理９０１で出力パルスの数をカウントするカウンタＡ及びカウンタＢをリセットし、処理９０２でメモリ２８から最初のタイマ値をそれぞれ読み出し、タイマＡ及びタイマＢのタイマ値を設定する。その後、処理９０３でタイマＡ及びタイマＢの割り込みを許可して割り込み機能を起動させる。タイマ割り込みが発生すると、プログラムは割り込み処理へ移行する。タイマＡ及びタイマＢの割り込み処理は、別々に設けているが処理内容は図１０に示す内容と同様であるため、図１０を用いて割り込み処理の内容を説明する。割り込み処理では、処理１００１で負方向の駆動パルス出力が終了したかどうかを判断し、未だであれば処理１００２で負方向への駆動パルスを出力し、処理１００３で負方向出力パルスをカウントし、処理１００４で次回までの割り込み間隔時間となるタイマ値をメモリ２８から順次読み出し更新する。上記処理を負方向へのパルス出力が終了するまで割り込み発生毎に繰り返し実行する。駆動パルスは、５相コントローラ７３，７４で相切り換え信号に変換され、ドライバ３から駆動モータ１０，１２に電流を流し、駆動モータ１０，１２は回転する。負方向へのパルス出力が終了すると、処理１００５で正方向への出力パルス数の判定、処理１００６で正方向への駆動パルスの出力、処理１００７で出力パルスのカウント、処理１００８でタイマ値の更新を行い、全パルス出力するまで割り込み発生毎に繰り返し実行し、処理１００９でパルス出力終了後タイマ割り込み機能を禁止状態にする。図９に示すサブルーチンでは、処理９０４でタイマＡのパルス出力が終了したかどうか、処理９０５でタイマＢのパルス出力が終了したかどうかを判定し、移動部材１６の上昇終了を判断する。これらのプログラムは読み出し専用メモリＲＯＭ７０に格納されている。以上説明した制御手法により、移動部材１６は高さＨ1Zから所望の高さＨ2Zへ垂直に移動する。また、高さＨ2Zから高さＨ1Zへ下降させたい場合は、サーボモータ適用時と同様メモリ２８のテーブル状に記憶させたタイマ値の読み出し順番を逆にして、駆動モータ１０，１２を制御する。更に移動部材１６を水平に移動させる場合は、駆動モータ１０，１２を同時に同じパルスレートで駆動すれば良い。
【００５１】
以上今までの説明では、移動部材１６を既知の高さＨ1Zから所望の高さＨ2Zまで垂直に移動させる制御方法について触れたが、次に未知の高さから垂直移動させる場合について説明する。電源投入直後を考えると、絶対値エンコーダを有したサーボモータの場合はモータの回転角度からスライダ７，８の位置を求め、移動部材１６の高さを計算できるが、ステッピングモータの場合はモータの回転角度が不明なため、スライダ７，８の位置が判らず移動部材１６の高さを求めることができない。そこで、図１に示すリンクアーム機構１の第１のスライダ７の平行リンク１７の同一軸上に角度センサ２４を新たに設け、第１のアーム１３と第１の案内部材５とのなす角αを角度センサ２４で検出する。第１のアーム１３の長さＬ１は既知であるため、移動部材１６の高さは式（８）より求めることができる。本実施例では角度センサ２４をポテンショメータとし、ケーブル２５でＣＰＵボード２へコネクタ２６を介してＭＰＵ２７のＡ／Ｄ変換器７５へ接続する。電源投入後、移動部材１６を上昇移動させる時のメインルーチンを示す図１３を参照しながら説明すると、処理１３０１でＭＰＵ２７が、ポテンショメータから出力されるアナログ電圧を入力し、Ａ／Ｄ変換器７５でアナログ値をデジタル値に変換し、予めＲＯＭ７０に記憶させていた図１２に示す変換グラフ１２０１から移動部材１６の高さを得る。処理１３０２で移動部材１６の高さから、上記説明した計算手法によりタイマ値テーブルを作成する。処理１３０３では、図９に示すサブルーチンをコールし、上記説明した手法でステッピングモータを駆動し、移動部材１６を所望の高さＨ2Zまで垂直に移動させる。移動後、処理１３０４でポテンショメータから出力されるアナログ電圧を再び入力し、変換グラフ１２０１から移動部材１６の高さを得る。処理１３０５で所定の高さまで移動したかどうかを確認し、正常な場合は上昇移動完了と判断する。所定の高さまで移動しなかった場合は処理１３０６のエラー処理へ分岐し、ステッピングモータの脱調、タイミングベルト９、１１の断線等を表すエラーメッセージを出す。再スタートの要求で処理１３０１へジャンプし、再び前述した処理を実行する。下降及び水平移動の場合も、処理１３０３から処理１３０６と同様の手法で、角度センサ２４で移動部材１６の高さを検出し、正常な移動かどうかを確認しながらリンクアーム機構１を制御する。
【００５２】
【発明の効果】
本発明によれば、第１の案内部材と平行な第２の案内部材を設け、該第１の案内部材上には駆動モータにより移動可能な第１のスライダを設け、同様に該第２の案内部材上にも駆動モータにより移動可能な第２のスライダを設け、該第１のスライダには前記第１の案内部材と第２の案内部材の間隔より長い第１のアームの一端を軸着し、同様に該第２のスライダにも前記第１の案内部材と第２の案内部材の間隔より長い第２のアームの一端を軸着し、前記第１のアームの他端と前記第２のアームの他端は作業手段を備えた移動部材に同一軸上で軸着し、該移動部材の片側に前記第１のスライダと第２のスライダを配置させ、前記第１のアームの長さをＬ１、前記第２のアームの長さをＬ２、前記第１のスライダと第２のスライダ間の上下方向の軸芯間の距離をｄ、前記移動部材の摺動方向の位置をｘ１、前記第１のスライダの摺動方向の位置をＡｘとするとき、前記第２のスライダの摺動方向の位置Ｂｘは、
【００５３】
という関係になるように前記第１のスライダと前記第２のスライダを制御したので、移動部材を垂直に移動させることができる。
【００５４】
また、前記移動部材の所望の速度カーブから、前記第１のスライダの速度カーブを計算する工程１と前記第２のスライダの速度カーブを計算する工程２を設け、前記移動部材を移動させる前に前記工程１と前記工程２を計算し、移動時は得られた速度カーブに従い第１のスライダ及び第２のスライダを制御するようにしたので、移動部材を高速に垂直移動させることができる。
【００５５】
次に、前記スライダの少なくとも１つに前記アームの角度を検出する角度センサを設け、前記移動部材を移動させる前に該角度センサから前記移動部材の高さを検出するようにしたので、電源投入時であっても移動部材が穴部等の周囲の物と干渉衝突せず、移動部材を垂直移動させることができる。
【００５６】
更に、前記移動部材を移動させた後、該角度センサから前記移動部材の高さを検出するようにしたので、移動部材が所定の高さまで移動したかどうかを確認しながら移動部材を移動させることができる。
【図面の簡単な説明】
【図１】 本発明の一実施例を示すリンクアーム機構の構成図と制御装置の全体図。
【図２】 本発明の一実施例を示すリンクアーム機構の座標定義図。
【図３】 本発明の一実施例を示す移動部材の時間と高さの関係曲線図。
【図４】 本発明の一実施例を示す移動部材の速度カーブと各スライダの速度カーブ図。
【図５】 本発明の一実施例を示すサーボモータ適用時の計算工程１のフローチャート。
【図６】 本発明の一実施例を示すサーボモータ適用時の計算工程２のフローチャート。
【図７】 本発明の一実施例を示すステッピングモータ適用時の制御ブロック回路図。
【図８】 本発明の一実施例を示すステッピングモータ適用時のタイマテーブルのメモリ格納例。
【図９】 本発明の一実施例を示すステッピングモータ適用時のサブルーチンのフローチャート。
【図１０】 本発明の一実施例を示すステッピングモータ適用時のタイマ割り込み処理のフローチャート。
【図１１】 本発明の一実施例を示す自動遠心機の斜視図。
【図１２】 本発明の一実施例を示す移動部材の高さとＡ／Ｄ変換値を表す変換グラフ。
【図１３】 本発明の一実施例を示すステッピングモータ適用時のメインルーチンのフローチャート。
【符号の説明】
１はリンクアーム機構、５は第１の案内部材、６は第２の案内部材、７は第１のスライダ、８は第２のスライダ、１０，１２は駆動モータ、１３は第１のアーム、１４は第２のアーム、１５は作業手段、１６は移動部材、２４は角度センサ、４１は所望の速度カーブ、４２は第１のスライダの速度カーブ、４３は第２のスライダの速度カーブである。
【発明の属する技術分野】
本発明は、例えば自動遠心機のように検体試料のハンドリング機構を有する遠心機のリンクアーム機構の制御方法に関するものである。
【０００２】
【従来の技術】
この種のリンクアーム機構に関する公知例として特開昭５８−１６００７１号公報があり、移動部材を２つのスライダ間に配置させたリンクアーム機構について記載されているが、移動部材を垂直に上下させる制御手法に関しては詳細に触れていない。
【０００３】
【発明が解決しようとする課題】
上記した従来のリンクアーム機構に於いては、移動部材は水平に配置された２本の案内部材よりも下方のみ若しくは上方のみ移動可能であるが、摺動する２つのスライダ間に移動部材を配置しているため、上方から下方へ、又は下方から上方へと案内部材を跨って移動することはできず、可動範囲が狭いという欠点があった。この欠点を解決するためには、平行な２本の案内部材を鉛直方向に配置し、移動部材の左右のどちらか片側にスライダを２つ共配置すると、移動部材は案内部材を跨って上下移動することは可能となるが、移動部材を垂直に移動させるためには、２つのスライダが複雑な動きになるという新たな問題が生じる。
【０００４】
また、移動部材が穴部等に入っている状態で電源を投入し２つのスライダを原点位置へ移動させる場合、移動部材が穴部内壁に衝突しないように移動部材を垂直に上方へ移動させなければならない。更に移動部材を上方へ移動させようと駆動した後、実際に移動部材が所定の高さまで移動したかどうかが分からないという問題がある。
【０００５】
本発明の目的は、移動部材が垂直移動する際の移動部材の高さと２つのスライダの位置関係を表す関係式を開示し、移動部材を垂直移動させる制御方法を提供することにある。
【０００６】
本発明の他の目的は、移動部材を高速に垂直移動させる制御方法を提供することにある。
【０００７】
本発明の他の目的は、電源投入時であっても移動部材が穴部等の周囲の物と干渉衝突しないように、移動部材を垂直移動させる制御方法を提供することにある。
【０００８】
本発明の更なる他の目的は、移動部材が所定の高さまで移動したかどうかを確認しながら移動部材を移動させる制御方法を提供することにある。
【０００９】
【課題を解決するための手段】
上記した問題を解決するために、互いに平行な第１の案内部材と第２の案内部材と、該第１の案内部材上には駆動モータにより移動可能な第１のスライダを設け、同様に該第２の案内部材上にも駆動モータにより移動可能な第２のスライダを設け、該第１のスライダには前記第１の案内部材と第２の案内部材の軸芯の間隔より長い第１のアームの一端を軸着し、同様に該第２のスライダにも前記第１の案内部材と第２の案内部材の軸芯の間隔より長い第２のアームの一端を軸着し、前記第１のアームの他端と前記第２のアームの他端は作業手段を備えた移動部材に同一軸上で軸着し、該移動部材の片側に前記第１のスライダと第２のスライダを配置させ、前記第１のアームの長さをＬ１、前記第２のアームの長さをＬ２、前記第１のスライダと第２のスライダ間の上下方向の軸芯間の距離をｄ、前記移動部材の摺動方向の位置をｘ１、前記第１のスライダの摺動方向の位置をＡｘとするとき、前記第２のスライダの摺動方向の位置Ｂｘは、
という関係になるように前記第１のスライダと前記第２のスライダを制御することにより達成される。
【００１１】
また、前記移動部材の所望の速度カーブから、前記第１のスライダの速度カーブを計算する工程１と前記第２のスライダの速度カーブを計算する工程２を設け、前記移動部材を移動させる以前に前記工程１と前記工程２を計算し、移動時は得られた速度カーブに従い第１のスライダ及び第２のスライダを制御することにより達成される。
【００１２】
次に、前記スライダの少なくとも１つに前記アームの角度を検出する角度センサを設け、前記移動部材を移動させる前に該角度センサから前記移動部材の現在の高さを検出することにより達成される。
【００１３】
更に、前記移動部材を移動させた後、該角度センサから前記移動部材の高さを検出することにより達成される。
【００１４】
【発明の実施の形態】
本発明の具体的な実施例を図面を参照しながら以下詳細に説明する。本発明のリンクアーム機構１の制御方法を適用した自動遠心機１１０を図１１に示す。自動遠心機１１０は、真空採決管に血液等の検体１１１が通常５本入ったラック１１２を移動部材１６に取り付けられたハンド１５で掴み、リンクアーム機構１で遠心機内のバケット１１３に入れる。所定のラック数を入れた後、ロ−タ１１４を回転させ検体１１１を遠心分離する。分離後、バケット１１３からラック１１２をハンド１５により取り出し、ラック置場１１５へ置く。上述のようにラック１１２をハンドリングする際の移動部材１６の垂直動作について、図１を参照しながら説明する。図１は、リンクアーム機構１の構成図と、リンクアーム機構１を制御するＣＰＵボード２及びドライブボード３を示す。リンクアーム機構１は、断面がＨ形のベース４に互いに平行な第１の案内部材５と第２の案内部材６を備え、第１の案内部材５上を摺動する第１のスライダ７と第２の案内部材６上を摺動する第２のスライダ８を備えている。第１のスライダ７はタイミングベルト９に固定され、このタイミングベルト９を稼動する駆動モータ１０を回転させることにより左右へ移動し、同様に第２のスライダ８はタイミングベルト１１に固定され、このタイミングベルト１１を稼動する駆動モータ１２を回転させることにより左右へ移動する。また、第１のスライダ７には、第１のアーム１３の一端（A）が軸着され、同様に第２のスライダ８にも、第２のアーム１４の一端（B）が軸着されている。第１のアーム１３の他端と第２のアーム１４の他端は、ハンド等の作業手段１５を備えた移動部材１６に同一軸上（H）で軸着され、移動部材１６の片側へ上記２つのスライダ７，８を配置している。更に、移動部材１６と第１のスライダ７は、第１のアーム１３と平行な平行リンク１７で軸着され、移動部材１６の姿勢を一定に保っている。上記２つの駆動モータ１０，１２は、それぞれモータケーブル１８，１９とコネクタ２０を介してドライブボード３に接続されている。また、マイクロコンピュータ（以下ＭＣＵと称す）２７及びメモリ２８等から構成されるＣＰＵボード２と駆動モータ１０，１２を駆動するドライブボード３は、コネクタ２１，２２とフラットケーブル２３を介して接続されている。前述したように駆動モータ１０，１２をベース４に固定したリンクアーム機構１は、可動部に駆動モータを設けた直交形ロボットのようなケーブルの曲折が無いため、曲折による断線は解消される構成となっている。また停電等があった場合、駆動モータ１０，１２の位置保持力が無くなり移動部材１６の自重で落下する恐れがあるが、駆動モータ１０，１２のロータシャフトをバネ力で固定する電磁ロックブレーキ機能を持つモータを採用し、落下を防止している。
【００１５】
前述のように構成したリンクアーム機構１の座標定義図を図２に示す。座標軸はＸ−Ｚ軸の直交座標とし、点Ａは第１のスライダ７の軸芯，点Ｂは第２のスライダ８の軸芯，点Ｈはハンド等の作業手段１５を備えた移動部材１６の軸芯を表わす。第１のアーム１３の長さをＬ１，第２のアーム１４の長さをＬ２，第１のスライダ７と第２のスライダ８の上下方向つまりＺ軸方向の軸芯間の距離をｄとすると、点Ａは直線Ｚ＝Ｌ１上を、点Ｂは直線Ｚ＝Ｌ１−ｄ上を移動し、点ＨのＺ軸方向の可動範囲は０〜（Ｌ１＋Ｌ２−ｄ）となる。又、直線Ｚ＝Ｌ１と第１のアーム１３のなす角をα（−π／２≦α≦sin-1{(Ｌ２−ｄ)／Ｌ１｝）とし、点ＨからＸ軸方向へ垂線を下ろし直線Ｚ＝Ｌ１と交わる点をＥ，直線Ｚ＝Ｌ１−ｄと交わる点をＦとすると、線分ＨＥ，ＡＥ，ＨＦ，ＢＦの長さは、
となる。今、点Ｈが直線Ｘ＝ｘ１上を移動する場合、点Ａと点Ｂの座標をそれぞれ（Ａｘ，Ｌ１）（Ｂｘ，Ｌ１−ｄ）とすると、Ｘ軸方向の位置Ａｘ，Ｂｘは、
となり、式（５）と（６）式からαを消去すると、
が導かれる。式（７）は、点Ｈが直線Ｘ＝ｘ１上を垂直移動する時の点Ａと点Ｂの位置を表わす関係式であり、逆に第１のスライダ７と第２のスライダ８の位置を式（７）を満足するように制御することにより、移動部材１６は垂直移動が可能となる。
【００２０】
なお、式（５）及び式（６）は、図１及び図２に示すように、第１及び第２のスライダ７，８が移動部材１６の左側に位置している状態を想定して式化したものである。したがって、例えば第１及び第２のスライダ７，８が移動部材１６の右側に位置している状態を想定して式化すれば、上記式（５）〜式（７）は次のようになる。
【００２１】
しかし、第１及び第２のスライダ７，８が移動部材１６の左側に位置しているか、右側に位置しているかは、リンクアーム機構１を表から見るか裏から見るか程度の差でしかないため、式（７）と式（７ａ）の差は、Ｘ軸の方向をどちらに取るかの考え方の差でしかない。したがって、式（７）を満足する制御を行うということと、式（７ａ）を満足する制御を行うということは全く同じ意味である。
【００２３】
次に、点Ｈが図２に示すように直線Ｘ＝ｘ１上を移動前の既知の高さＨ1Zから移動先の所望の高さＨ2Zまで移動時間Ｔ秒経て移動する場合について説明する。先ず、移動部材１６の垂直移動中の時間ｔと角度αについて関係式α(t)を導く。移動部材１６の軸芯点ＨのＺ軸成分をＨｚとすると、
となる。時間ｔと高さＨｚの関係をcos関数の半周期を用いて、振幅を移動距離、角度を時間に置き換えて変換すると、
となり、時間ｔと高さＨｚの関係は図３に示すような曲線３０になる。ここで、Ｈ2Z−Ｈ1Z＝Ｍとし、式（８）（９）からαについて解くと、
となり、移動部材１６の垂直移動中の時間ｔと角度αについての関係式α(t)を得る。移動部材１６の高さＨｚと時間ｔをcos関数を用いたため、移動部材１６の速度カーブ４１は図４に示すような半周期のsin波となり、これを移動部材１６の所望の速度カーブ４１とする。
【００２７】
続いて、移動部材１６の所望の速度カーブ４１から、第１のスライダ７の速度カーブ４２を計算する工程１と、第２のスライダ８の速度カーブ４３を計算する工程２に関して、駆動モータ１０，１２にエンコーダ付きサーボモータを適用した場合の計算手法について説明する。サーボモータの制御は、制御計算周期毎にエンコーダの回転パルス信号をフィードバックしサーボモータの回転角度及び角速度を求め、指令速度とサーボモータの角速度の差から比例項・積分項・微分項の３つの項を加算するＰＩＤ演算を行い、ＰＷＭ（パルス・ワイド・モジュレーション）制御によりサーボモータを駆動制御する。制御計算周期の時間をΔｔとすると、移動中の時間ｔはΔｔの累積であるから、
で表わせられ、ｉは制御回数、ｎは移動部材１６を所望の高さＨ2Zまで移動させた時の制御回数を表わし、移動時間ＴはＴ＝ｎ・Δｔとなる。
【００２９】
フローチャート図５を用いて工程１の計算手法を、制御計算周期に制御回数を乗じた時間ｔiについて説明すると、処理５００で式（１０）からアーム角度α(ti)を求め、処理５０１で点ＨのＸ座標成分ｘ１及びアーム角度α(ti)を式（５）へ代入し第１のスライダ７の位置Ａｘ(ti)を計算する。処理５０２で制御計算周期毎の第１のスライダ７の位置Ａｘの差、つまり
を計算すると、Δｔ時間後の第１のスライダ７の移動距離が得られ、その移動距離とΔｔ時間から第１のスライダ７の速度を得る。更に、処理５０３で１回転当りの第１のスライダ７のピッチと第１のスライダ７の速度から、駆動モータ１０の指令速度に変換する。処理５０４でこれらの一連の計算をｉ＝０から順次ｎまで繰り返し行い、処理５０５で駆動モータ１０の指令速度データテーブルを作成し、ＣＰＵボード２のメモリ２８に記憶する。
【００３１】
次に工程２についてフローチャート図６を用いて説明する。時間ｔiの場合、処理６００で点ＨのＸ座標成分ｘ１及び工程１で求めたＡｘ(ti)を式（７）へ代入し、第２のスライダ８の位置Ｂｘ(ti)を得る。処理６０１で工程１と同様に、制御計算周期毎の第２のスライダ８の位置Ｂｘの差
から、Δｔ時間後の第２のスライダ８の移動距離が得られ、その移動距離から第２のスライダ８の速度を得る。更に、処理６０２で１回転当りの第２のスライダ８のピッチと第２のスライダ８の速度から、駆動モータ１２の指令速度に変換する。処理６０３でこれらの一連の計算をｉ＝０から順次ｎまで繰り返し行い、処理６０４で駆動モータ１２の指令速度データテーブルを作成し、ＣＰＵボード２のメモリ２８に記憶する。図４に工程１と工程２の計算より得た第１のスライダ７の速度カーブ４２と第２のスライダ８の速度カーブ４３を示す。
【００３３】
以上の工程１及び工程２を経て、移動部材１６を移動させる場合は、工程１で求めた速度カーブ４２になるようにＣＰＵボード２のメモリ２８に記憶させた駆動モータ１０の指令速度データを制御計算周期毎に順次読み出し、第１のスライダ７を速度制御する。同様に工程２で求めた速度カーブ４３になるようにＣＰＵボード２のメモリ２８に記憶させた駆動モータ１２の指令速度データを制御計算周期毎に順次読み出し、第２のスライダ８を速度制御する。速度制御の手法は、前述したようにエンコーダフィードバックによる角速度変換と指令速度の差からＰＩＤ演算により、駆動モータ１０，１２を制御する。２つの駆動モータ１０，１２の制御は、例えばＣＰＵ１個で時分割によりタスクを切り替える方式で同時に制御駆動し、移動部材１６を高さＨ1Zから所望の高さＨ2Zへ垂直に移動させる。
【００３４】
以上の説明では移動部材１６を上昇させる場合について説明したが、高さＨ2Zから高さＨ1Zへ下降させる場合は、ＣＰＵボード２のメモリ２８に記憶させた指令速度データの読み出し順番を逆にして駆動モータ１０，１２を制御する。また、移動部材１６を水平に移動させる場合は、駆動モータ１０，１２を同時に同じ指令速度で駆動すれば良い。
【００３５】
前述ではエンコーダ付きサーボモータを適用した場合の計算工程１と計算工程２について説明したが、以下駆動モータ１０，１２にパルスレートで駆動するステッピングモータを適用した場合について説明する。ステッピングモータを１ステップ回転させると、スライダ７，８は１ステップ回転角に相当する分だけ移動する。その１ステップ分の移動量ΔＡｘ，ΔＢｘは、
となる。ここで、ｓはステップ角度，１／Ｎは減速比，ｐはピッチ（mm/１回転）である。このようにステッピングモータの場合、パルスレートで駆動するためスライダ７，８の停止位置は離散的となる。そのため、離散的な位置とその位置を通過する時間からパルスレートを決定する計算手法を導く。今、点Ｈの移動前の既知の高さＨ1Zと移動先の所望の高さＨ2Zが与えられるとし、図４の移動部材１６の所望の速度カーブ４１を示す式（９）からＨ1Z−Ｈ2Z＝Ｍとして時間ｔについて解くと、
となる。式（１５）は時間ｔを移動部材１６の高さＨｚの関数で表わしたものである。
【００３８】
先ず、第１のスライダ７の速度カーブ４２を計算する工程１について説明する。移動部材１６の高さＨｚを第１のスライダ７の位置Ａｘで表現すると、
式（１６）を式（１５）に代入して、
を得る。式（１７）は時間ｔを第１のスライダ７の位置Ａｘの関数で表現したものであり、第１のスライダ７の離散的な位置をＡｘに代入するとその位置を通過する時間ｔが得られる。上式（１６）（１７）の±について図２を参照しながら説明すると、点Ｈの高さＨｚが直線Ｚ＝Ｌ１より上か下かで場合分けされる。移動開始時の計算上の点Ｈの高さＨｚ＝Ｈ1Zで直線Ｚ＝Ｌ１より下方のため−であり、直角三角形ＡＨＥの辺ＨＥが０になるまで計算上の点ＡをＸ軸負方向へΔＡｘずつ移動させる。次に計算上の点ＡをＸ軸正方向へΔＡｘ分移動させ、計算上の点Ｈの高さをＬ１より上方とみなし（＋）、点Ｈの高さがＬ１からＨ2Zまで計算上の点ＡをＸ軸の正方向へΔＡｘずつ移動させる。点Ｈの移動前の高さＨ1ZからＬ１までの、点ＡのＸ軸負方向へ移動する距離は、
となり、その移動パルス数は式（１８）の移動距離を１ステップ分の移動量ΔＡｘで割った数となる。点Ａの位置Ａｘは１ステップ移動前の位置Ａｘ−ΔＡｘより得て、式（１７）に代入し、時間ｔを求める。ΔＡｘをＡｘから順次引き更新し、式（１７）より点Ａの離散的な位置と通過時間を得て、その１ステップ分の時間の差からパルスレートを求める。パルス出力方式は、図７に示すようなブロック回路でＭＣＵ２７が有するタイマＡ７１の割り込み機能を用い、パルス周波数を変化させる方式とし、タイマクロック周期と移動前後の時間差から割り込み間隔のタイマ値Ａｔは、
より得る。次に点Ｈの高さＬ１から移動先のＨ2Zまでの、点ＡのＸ軸正方向へ移動する距離は、
となり、その移動パルス数は式（２０）の移動距離を１ステップ分の移動量ΔＡｘで割った数となる。同様に上述した計算方法により、１ステップ分の移動前後の時間差からタイマ値Ａｔを求め、各移動分のタイマ値をテーブル化し、ＣＰＵボード２のメモリ２８に記憶する。
【００４４】
続いて、第２のスライダ８の速度カーブ４３を計算する工程２について説明する。移動部材１６の高さＨｚを第２のスライダ８の位置Ｂｘで表現すると、
式（２１）を式（１５）に代入して、
を得る。式（２２）は時間ｔを第２のスライダ８の位置Ｂｘの関数で表現したものであり、前述した第１のスライダ７の場合と同様、第２のスライダ８の離散的な位置をＢｘに代入するとその位置を通過する時間ｔが得られる。上式（２１）（２２）の±については、点Ｈの高さＨｚが直線Ｚ＝Ｌ１−ｄより上か下かで場合分けされるが、計算上の点Ｈの高さＨｚが直線Ｚ＝Ｌ１−ｄより下方の場合は−であり、直角三角形ＢＨＦの辺ＨＦが０になるまで計算上の点ＢをＸ軸負方向へΔＢｘずつ移動させる。次に計算上の点ＢをＸ軸正方向へΔＢｘ分移動させ、計算上の点Ｈの高さをＬ１−ｄより上方とみなし（＋）、点Ｈの高さがＬ１−ｄからＨ2Zまで計算上の点ＢをＸ軸の正方向へΔＢｘずつ移動させる。点Ｈの移動前の高さＨ1ZからＬ１−ｄまでの、点ＢのＸ軸負方向へ移動する距離は、
となり、その移動パルス数は式（２３）の移動距離を１ステップ分の移動量ΔＢｘで割った数となる。点Ｂの位置Ｂｘは１ステップ移動前の位置Ｂｘ−ΔＢｘより得て、式（２２）に代入し時間ｔを求める。ΔＢｘをＢｘから順次引き更新し、式（２２）より点Ｂの離散的な位置と通過時間を得て、その１ステップ分の時間の差からパルスレートを求める。第１のスライダ７と同様、パルス出力方式をＭＣＵ２７が有するタイマＢ７２の割り込み機能を用い、パルス周波数を変化させる方式とし、タイマクロック周期と移動前後の時間差から割り込み間隔のタイマ値Ｂｔは、
より得る。次に点Ｈの高さＬ１−ｄから移動先のＨ2Zまでの、点ＢのＸ軸正方向への移動距離は、
となり、その移動パルス数は式（２５）の移動距離を１ステップ分の移動量ΔＢｘで割った数となる。同様に上述した計算方法により、１ステップ分の移動前後の時間差からタイマ値Ｂｔを求め、各移動分のタイマ値をテーブル化し、ＣＰＵボード２のメモリ２８に記憶する。ステッピングモータの場合もサーボモータと同様、各スライダ７，８の速度カーブは図４に示すような第１のスライダ７の速度カーブ４２及び第２のスライダ８の速度カーブ４３となる。又、メモリ２８には各スライダ７，８の移動方向別のパルス数とタイマ値が、図８に示すような構成で整然と格納されている。８０１は第１のスライダ７の負方向パルス数で、８０２は第１のスライダ７の正方向パルス数で、８０３は第２のスライダ８の負方向パルス数で、８０４は第２のスライダ８の正方向パルス数で、８０５は第１のスライダ７の負方向移動時のタイマ値テーブルで、８０６は第１のスライダ７の正方向移動時のタイマ値テーブルで、８０７は第２のスライダ８の負方向移動時のタイマ値テーブルで、８０８は第２のスライダ８の正方向移動時のタイマ値テーブルである。
【００５０】
ここで、駆動モータ１０，１２にステッピングモータを適用した時のブロック回路を示した図７と、フロチャート図９，図１０を参照しながら、前述した計算の工程１及び工程２より得たタイマ値を用いて、移動部材１６を上昇させる場合について説明する。図９は移動部材１６を移動させる時のサブルーチンであり、図１０はタイマ割り込み処理の上昇動作部分のフローチャートである。回転開始時、処理９０１で出力パルスの数をカウントするカウンタＡ及びカウンタＢをリセットし、処理９０２でメモリ２８から最初のタイマ値をそれぞれ読み出し、タイマＡ及びタイマＢのタイマ値を設定する。その後、処理９０３でタイマＡ及びタイマＢの割り込みを許可して割り込み機能を起動させる。タイマ割り込みが発生すると、プログラムは割り込み処理へ移行する。タイマＡ及びタイマＢの割り込み処理は、別々に設けているが処理内容は図１０に示す内容と同様であるため、図１０を用いて割り込み処理の内容を説明する。割り込み処理では、処理１００１で負方向の駆動パルス出力が終了したかどうかを判断し、未だであれば処理１００２で負方向への駆動パルスを出力し、処理１００３で負方向出力パルスをカウントし、処理１００４で次回までの割り込み間隔時間となるタイマ値をメモリ２８から順次読み出し更新する。上記処理を負方向へのパルス出力が終了するまで割り込み発生毎に繰り返し実行する。駆動パルスは、５相コントローラ７３，７４で相切り換え信号に変換され、ドライバ３から駆動モータ１０，１２に電流を流し、駆動モータ１０，１２は回転する。負方向へのパルス出力が終了すると、処理１００５で正方向への出力パルス数の判定、処理１００６で正方向への駆動パルスの出力、処理１００７で出力パルスのカウント、処理１００８でタイマ値の更新を行い、全パルス出力するまで割り込み発生毎に繰り返し実行し、処理１００９でパルス出力終了後タイマ割り込み機能を禁止状態にする。図９に示すサブルーチンでは、処理９０４でタイマＡのパルス出力が終了したかどうか、処理９０５でタイマＢのパルス出力が終了したかどうかを判定し、移動部材１６の上昇終了を判断する。これらのプログラムは読み出し専用メモリＲＯＭ７０に格納されている。以上説明した制御手法により、移動部材１６は高さＨ1Zから所望の高さＨ2Zへ垂直に移動する。また、高さＨ2Zから高さＨ1Zへ下降させたい場合は、サーボモータ適用時と同様メモリ２８のテーブル状に記憶させたタイマ値の読み出し順番を逆にして、駆動モータ１０，１２を制御する。更に移動部材１６を水平に移動させる場合は、駆動モータ１０，１２を同時に同じパルスレートで駆動すれば良い。
【００５１】
以上今までの説明では、移動部材１６を既知の高さＨ1Zから所望の高さＨ2Zまで垂直に移動させる制御方法について触れたが、次に未知の高さから垂直移動させる場合について説明する。電源投入直後を考えると、絶対値エンコーダを有したサーボモータの場合はモータの回転角度からスライダ７，８の位置を求め、移動部材１６の高さを計算できるが、ステッピングモータの場合はモータの回転角度が不明なため、スライダ７，８の位置が判らず移動部材１６の高さを求めることができない。そこで、図１に示すリンクアーム機構１の第１のスライダ７の平行リンク１７の同一軸上に角度センサ２４を新たに設け、第１のアーム１３と第１の案内部材５とのなす角αを角度センサ２４で検出する。第１のアーム１３の長さＬ１は既知であるため、移動部材１６の高さは式（８）より求めることができる。本実施例では角度センサ２４をポテンショメータとし、ケーブル２５でＣＰＵボード２へコネクタ２６を介してＭＰＵ２７のＡ／Ｄ変換器７５へ接続する。電源投入後、移動部材１６を上昇移動させる時のメインルーチンを示す図１３を参照しながら説明すると、処理１３０１でＭＰＵ２７が、ポテンショメータから出力されるアナログ電圧を入力し、Ａ／Ｄ変換器７５でアナログ値をデジタル値に変換し、予めＲＯＭ７０に記憶させていた図１２に示す変換グラフ１２０１から移動部材１６の高さを得る。処理１３０２で移動部材１６の高さから、上記説明した計算手法によりタイマ値テーブルを作成する。処理１３０３では、図９に示すサブルーチンをコールし、上記説明した手法でステッピングモータを駆動し、移動部材１６を所望の高さＨ2Zまで垂直に移動させる。移動後、処理１３０４でポテンショメータから出力されるアナログ電圧を再び入力し、変換グラフ１２０１から移動部材１６の高さを得る。処理１３０５で所定の高さまで移動したかどうかを確認し、正常な場合は上昇移動完了と判断する。所定の高さまで移動しなかった場合は処理１３０６のエラー処理へ分岐し、ステッピングモータの脱調、タイミングベルト９、１１の断線等を表すエラーメッセージを出す。再スタートの要求で処理１３０１へジャンプし、再び前述した処理を実行する。下降及び水平移動の場合も、処理１３０３から処理１３０６と同様の手法で、角度センサ２４で移動部材１６の高さを検出し、正常な移動かどうかを確認しながらリンクアーム機構１を制御する。
【００５２】
【発明の効果】
本発明によれば、第１の案内部材と平行な第２の案内部材を設け、該第１の案内部材上には駆動モータにより移動可能な第１のスライダを設け、同様に該第２の案内部材上にも駆動モータにより移動可能な第２のスライダを設け、該第１のスライダには前記第１の案内部材と第２の案内部材の間隔より長い第１のアームの一端を軸着し、同様に該第２のスライダにも前記第１の案内部材と第２の案内部材の間隔より長い第２のアームの一端を軸着し、前記第１のアームの他端と前記第２のアームの他端は作業手段を備えた移動部材に同一軸上で軸着し、該移動部材の片側に前記第１のスライダと第２のスライダを配置させ、前記第１のアームの長さをＬ１、前記第２のアームの長さをＬ２、前記第１のスライダと第２のスライダ間の上下方向の軸芯間の距離をｄ、前記移動部材の摺動方向の位置をｘ１、前記第１のスライダの摺動方向の位置をＡｘとするとき、前記第２のスライダの摺動方向の位置Ｂｘは、
という関係になるように前記第１のスライダと前記第２のスライダを制御したので、移動部材を垂直に移動させることができる。
【００５４】
また、前記移動部材の所望の速度カーブから、前記第１のスライダの速度カーブを計算する工程１と前記第２のスライダの速度カーブを計算する工程２を設け、前記移動部材を移動させる前に前記工程１と前記工程２を計算し、移動時は得られた速度カーブに従い第１のスライダ及び第２のスライダを制御するようにしたので、移動部材を高速に垂直移動させることができる。
【００５５】
次に、前記スライダの少なくとも１つに前記アームの角度を検出する角度センサを設け、前記移動部材を移動させる前に該角度センサから前記移動部材の高さを検出するようにしたので、電源投入時であっても移動部材が穴部等の周囲の物と干渉衝突せず、移動部材を垂直移動させることができる。
【００５６】
更に、前記移動部材を移動させた後、該角度センサから前記移動部材の高さを検出するようにしたので、移動部材が所定の高さまで移動したかどうかを確認しながら移動部材を移動させることができる。
【図面の簡単な説明】
【図１】 本発明の一実施例を示すリンクアーム機構の構成図と制御装置の全体図。
【図２】 本発明の一実施例を示すリンクアーム機構の座標定義図。
【図３】 本発明の一実施例を示す移動部材の時間と高さの関係曲線図。
【図４】 本発明の一実施例を示す移動部材の速度カーブと各スライダの速度カーブ図。
【図５】 本発明の一実施例を示すサーボモータ適用時の計算工程１のフローチャート。
【図６】 本発明の一実施例を示すサーボモータ適用時の計算工程２のフローチャート。
【図７】 本発明の一実施例を示すステッピングモータ適用時の制御ブロック回路図。
【図８】 本発明の一実施例を示すステッピングモータ適用時のタイマテーブルのメモリ格納例。
【図９】 本発明の一実施例を示すステッピングモータ適用時のサブルーチンのフローチャート。
【図１０】 本発明の一実施例を示すステッピングモータ適用時のタイマ割り込み処理のフローチャート。
【図１１】 本発明の一実施例を示す自動遠心機の斜視図。
【図１２】 本発明の一実施例を示す移動部材の高さとＡ／Ｄ変換値を表す変換グラフ。
【図１３】 本発明の一実施例を示すステッピングモータ適用時のメインルーチンのフローチャート。
【符号の説明】
１はリンクアーム機構、５は第１の案内部材、６は第２の案内部材、７は第１のスライダ、８は第２のスライダ、１０，１２は駆動モータ、１３は第１のアーム、１４は第２のアーム、１５は作業手段、１６は移動部材、２４は角度センサ、４１は所望の速度カーブ、４２は第１のスライダの速度カーブ、４３は第２のスライダの速度カーブである。
Claims (4)
- 互いに平行な第１の案内部材と第２の案内部材と、該第１の案内部材上を移動可能な第１のスライダと、該第２の案内部材上を移動可能な第２のスライダを設け、該第１のスライダには前記第１の案内部材と第２の案内部材の軸芯の間隔より長い第１のアームの一端が軸着され、該第２のスライダには前記第１の案内部材と第２の案内部材の軸芯の間隔より長い第２のアームの一端が軸着され、前記第１のアームの他端と前記第２のアームの他端は作業手段を備えた移動部材に軸着され、該移動部材の片側に前記第１のスライダと第２のスライダを配置させたリンクアーム機構の制御方法において、前記第１のアームの長さをＬ１、前記第２のアームの長さをＬ２、前記第１のスライダと第２のスライダ間の上下方向の軸芯間の距離をｄ、前記移動部材の移動方向の位置をｘ１、前記第１のスライダの摺動方向の位置をＡｘとして、前記移動部材を垂直移動させる場合に前記第２のスライダの摺動方向の位置Ｂｘと前記第１のスライダの摺動方向の位置Ａｘとを、下式をほぼ満足するよう制御することを特徴としたリンクアーム機構の制御方法。
- 前記移動部材を垂直移動させる場合の所望の速度カーブから、前記第１のスライダの速度カーブを計算する工程１と前記第２のスライダの速度カーブを計算する工程２を有し、前記移動部材を移動させる前に前記工程１と前記工程２を計算することを特徴とした請求項１記載のリンクアーム機構の制御方法。
- 前記スライダの少なくとも１つに前記アームの角度を検出する角度センサを設け、前記移動部材を移動させる前に該角度センサと前記第一のアームの長さから、前記移動部材の移動可能な面内であって、且つ前記スライダの摺動方向に対して垂直方向の前記移動部材の高さを検出することを特徴とした請求項１又は２記載のリンクアーム機構の制御方法。
- 前記スライダの少なくとも１つに前記アームの角度を検出する角度センサを設け、前記移動部材を垂直方向に移動させた後該角度センサと前記第一のアームの長さから、前記移動部材の移動可能な面であって、且つ前記スライダの摺動方向に対して垂直方向の前記移動部材の高さを検出することを特徴とした請求項１乃至３のいずれか記載のリンクアーム機構の制御方法。
Priority Applications (4)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP07059498A JP3731846B2 (ja) | 1998-03-19 | 1998-03-19 | リンクアーム機構の制御方法 |
| US09/266,605 US6196961B1 (en) | 1998-03-19 | 1999-03-11 | Automatic centrifugal machine employing a link arm mechanism |
| DE19912496A DE19912496B4 (de) | 1998-03-19 | 1999-03-19 | Zentrifugenbetriebssystem mit automatischer Zentrifuge und Fließband |
| US09/726,438 US6701219B2 (en) | 1998-03-19 | 2000-12-01 | Control method for a link arm mechanism and an automatic centrifugal machine employing this link arm mechanism |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP07059498A JP3731846B2 (ja) | 1998-03-19 | 1998-03-19 | リンクアーム機構の制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH11262881A JPH11262881A (ja) | 1999-09-28 |
| JP3731846B2 true JP3731846B2 (ja) | 2006-01-05 |
Family
ID=13436053
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP07059498A Expired - Fee Related JP3731846B2 (ja) | 1998-03-19 | 1998-03-19 | リンクアーム機構の制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3731846B2 (ja) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005230918A (ja) * | 2004-02-04 | 2005-09-02 | Janome Sewing Mach Co Ltd | ロボット |
| JP5778653B2 (ja) * | 2012-12-05 | 2015-09-16 | 株式会社藤商事 | 遊技機 |
| JP6902379B2 (ja) * | 2017-03-31 | 2021-07-14 | 東京エレクトロン株式会社 | 処理システム |
| CN111531568B (zh) * | 2020-05-07 | 2024-01-02 | 吴伟 | 一种用于机械手的联动式机械手指拨片机构 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS60132816A (ja) * | 1983-12-22 | 1985-07-15 | Shinmei Kogyo Kk | 物品搬送装置 |
| JP2588418B2 (ja) * | 1988-03-31 | 1997-03-05 | 新明和工業株式会社 | ３次元マニピュレータ |
| JPH0290080U (ja) * | 1988-12-29 | 1990-07-17 | ||
| JPH08119409A (ja) * | 1994-10-27 | 1996-05-14 | Tokyo Electron Ltd | 集合処理装置 |
| JP2785860B2 (ja) * | 1995-02-07 | 1998-08-13 | 和也 廣瀬 | 多自由度位置決めステージ |
-
1998
- 1998-03-19 JP JP07059498A patent/JP3731846B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH11262881A (ja) | 1999-09-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0130570B1 (en) | Method and apparatus for controlling a robot hand along a predetermined path | |
| EP0232424B1 (en) | Industrial robot | |
| EP0055915A2 (en) | Arm locus control method and apparatus | |
| EP0208788A1 (en) | Speed control system for servo motors | |
| Tomizuka et al. | Experimental evaluation of the preview servo scheme for a two-axis positioning system | |
| EP0201605A1 (en) | System for controlling the speed of a servo motor | |
| JP3731846B2 (ja) | リンクアーム機構の制御方法 | |
| WO1998010340A1 (en) | Control by means of a set-point generator | |
| JP2010063343A (ja) | モータ制御装置とそのトルクリップル補正方法及びモータ制御システム | |
| US6701219B2 (en) | Control method for a link arm mechanism and an automatic centrifugal machine employing this link arm mechanism | |
| CN110053042B (zh) | 机器人控制装置 | |
| EP0271590A1 (en) | Robot control apparatus | |
| JP3479922B2 (ja) | 電動機駆動系の負荷定数測定方法 | |
| JPH06225565A (ja) | 電動機駆動系の慣性負荷測定方式 | |
| JP2906255B2 (ja) | サーボ制御装置 | |
| JPH04142604A (ja) | サーボモータの制御方式 | |
| JP2819411B2 (ja) | 定位置停止制御装置 | |
| JPH0623928B2 (ja) | ロボツトハンドの軌道修正方法 | |
| GB2273177A (en) | Actuator position control. | |
| Pears et al. | Guidance of an autonomous guided vehicle using low-level ultrasonic and odometry sensor systems | |
| JPH0446715B2 (ja) | ||
| JP2528540B2 (ja) | 指令信号発生方法 | |
| JP2706743B2 (ja) | ベルト伝動機構の制御方法 | |
| JP2564326B2 (ja) | デイジタル位置サーボ装置 | |
| JPS63193213A (ja) | パルスエンコ−ダを有するデイジタルサ−ボシステムの速度制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20020927 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20040203 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20040402 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20040518 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20040616 |
|
| A911 | Transfer of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20040623 |
|
| A912 | Removal of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A912 Effective date: 20040716 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20051007 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20081021 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091021 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101021 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101021 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111021 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111021 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121021 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131021 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20141021 Year of fee payment: 9 |
|
| LAPS | Cancellation because of no payment of annual fees |