JPH11262881A - リンクアーム機構の制御方法 - Google Patents
リンクアーム機構の制御方法Info
- Publication number
- JPH11262881A JPH11262881A JP10070594A JP7059498A JPH11262881A JP H11262881 A JPH11262881 A JP H11262881A JP 10070594 A JP10070594 A JP 10070594A JP 7059498 A JP7059498 A JP 7059498A JP H11262881 A JPH11262881 A JP H11262881A
- Authority
- JP
- Japan
- Prior art keywords
- slider
- moving member
- arm
- moving
- 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.)
- Granted
Links
Landscapes
- Manipulator (AREA)
Abstract
(57)【要約】
【課題】平行な２軸上をそれぞれ独立した駆動モータに
より摺動するスライダに２軸間隔より長いアームの一端
を各々軸着し、作業手段を備えた移動部材に前記アーム
の他端を同一軸で軸着し、移動部材の片側にスライダを
２つ共配置させたリンクアーム機構に於いて、移動部材
を垂直移動させる時の２つのスライダは複雑な動きをす
る。本発明の目的は移動部材を垂直移動させる制御方法
を提供することにある。 【解決手段】２つのアームの長さ、アームの傾き等から
移動部材が垂直移動する時の２つのスライダの位置関係
式を得て、この位置関係式に従ってスライダの位置を制
御することにより、移動部材を垂直に移動させることが
できる。
より摺動するスライダに２軸間隔より長いアームの一端
を各々軸着し、作業手段を備えた移動部材に前記アーム
の他端を同一軸で軸着し、移動部材の片側にスライダを
２つ共配置させたリンクアーム機構に於いて、移動部材
を垂直移動させる時の２つのスライダは複雑な動きをす
る。本発明の目的は移動部材を垂直移動させる制御方法
を提供することにある。 【解決手段】２つのアームの長さ、アームの傾き等から
移動部材が垂直移動する時の２つのスライダの位置関係
式を得て、この位置関係式に従ってスライダの位置を制
御することにより、移動部材を垂直に移動させることが
できる。
Description
【０００１】
【発明の属する技術分野】本発明は、例えば自動遠心機
のように検体試料のハンドリング機構を有する遠心機の
リンクアーム機構の制御方法に関するものである。
のように検体試料のハンドリング機構を有する遠心機の
リンクアーム機構の制御方法に関するものである。
【０００２】
【従来の技術】この種のリンクアーム機構に関する公知
例として特開昭５８−１６００７１号公報があり、移動
部材を２つのスライダ間に配置させたリンクアーム機構
について記載されているが、移動部材を垂直に上下させ
る制御手法に関しては詳細に触れていない。
例として特開昭５８−１６００７１号公報があり、移動
部材を２つのスライダ間に配置させたリンクアーム機構
について記載されているが、移動部材を垂直に上下させ
る制御手法に関しては詳細に触れていない。
【０００３】
【発明が解決しようとする課題】上記した従来のリンク
アーム機構に於いては、移動部材は水平に配置された２
本の案内部材よりも下方のみ若しくは上方のみ移動可能
であるが、摺動する２つのスライダ間に移動部材を配置
しているため、上方から下方へ、又は下方から上方へと
案内部材を跨って移動することはできず、可動範囲が狭
いという欠点があった。この欠点を解決するためには、
平行な２本の案内部材を鉛直方向に配置し、移動部材の
左右のどちらか片側にスライダを２つ共配置すると、移
動部材は案内部材を跨って上下移動することは可能とな
るが、移動部材を垂直に移動させるためには、２つのス
ライダが複雑な動きになるという新たな問題が生じる。
アーム機構に於いては、移動部材は水平に配置された２
本の案内部材よりも下方のみ若しくは上方のみ移動可能
であるが、摺動する２つのスライダ間に移動部材を配置
しているため、上方から下方へ、又は下方から上方へと
案内部材を跨って移動することはできず、可動範囲が狭
いという欠点があった。この欠点を解決するためには、
平行な２本の案内部材を鉛直方向に配置し、移動部材の
左右のどちらか片側にスライダを２つ共配置すると、移
動部材は案内部材を跨って上下移動することは可能とな
るが、移動部材を垂直に移動させるためには、２つのス
ライダが複雑な動きになるという新たな問題が生じる。
【０００４】また、移動部材が穴部等に入っている状態
で電源を投入し２つのスライダを原点位置へ移動させる
場合、移動部材が穴部内壁に衝突しないように移動部材
を垂直に上方へ移動させなければならない。更に移動部
材を上方へ移動させようと駆動した後、実際に移動部材
が所定の高さまで移動したかどうかが分からないという
問題がある。
で電源を投入し２つのスライダを原点位置へ移動させる
場合、移動部材が穴部内壁に衝突しないように移動部材
を垂直に上方へ移動させなければならない。更に移動部
材を上方へ移動させようと駆動した後、実際に移動部材
が所定の高さまで移動したかどうかが分からないという
問題がある。
【０００５】本発明の目的は、移動部材が垂直移動する
際の移動部材の高さと２つのスライダの位置関係を表す
関係式を開示し、移動部材を垂直移動させる制御方法を
提供することにある。
際の移動部材の高さと２つのスライダの位置関係を表す
関係式を開示し、移動部材を垂直移動させる制御方法を
提供することにある。
【０００６】本発明の他の目的は、移動部材を高速に垂
直移動させる制御方法を提供することにある。
直移動させる制御方法を提供することにある。
【０００７】本発明の他の目的は、電源投入時であって
も移動部材が穴部等の周囲の物と干渉衝突しないよう
に、移動部材を垂直移動させる制御方法を提供すること
にある。
も移動部材が穴部等の周囲の物と干渉衝突しないよう
に、移動部材を垂直移動させる制御方法を提供すること
にある。
【０００８】本発明の更なる他の目的は、移動部材が所
定の高さまで移動したかどうかを確認しながら移動部材
を移動させる制御方法を提供することにある。
定の高さまで移動したかどうかを確認しながら移動部材
を移動させる制御方法を提供することにある。
【０００９】
【課題を解決するための手段】上記した問題を解決する
ために、互いに平行な第１の案内部材と第２の案内部材
と、該第１の案内部材上には駆動モータにより移動可能
な第１のスライダを設け、同様に該第２の案内部材上に
も駆動モータにより移動可能な第２のスライダを設け、
該第１のスライダには前記第１の案内部材と第２の案内
部材の軸芯の間隔より長い第１のアームの一端を軸着
し、同様に該第２のスライダにも前記第１の案内部材と
第２の案内部材の軸芯の間隔より長い第２のアームの一
端を軸着し、前記第１のアームの他端と前記第２のアー
ムの他端は作業手段を備えた移動部材に同一軸上で軸着
し、該移動部材の片側に前記第１のスライダと第２のス
ライダを配置させ、前記第１のアームの長さをＬ１、前
記第２のアームの長さをＬ２、前記第１のスライダと第
２のスライダ間の上下方向の軸芯間の距離をｄ、前記移
動部材の摺動方向の位置をｘ１、前記第１のスライダの
摺動方向の位置をＡｘとするとき、前記第２のスライダ
の摺動方向の位置Ｂｘは、
ために、互いに平行な第１の案内部材と第２の案内部材
と、該第１の案内部材上には駆動モータにより移動可能
な第１のスライダを設け、同様に該第２の案内部材上に
も駆動モータにより移動可能な第２のスライダを設け、
該第１のスライダには前記第１の案内部材と第２の案内
部材の軸芯の間隔より長い第１のアームの一端を軸着
し、同様に該第２のスライダにも前記第１の案内部材と
第２の案内部材の軸芯の間隔より長い第２のアームの一
端を軸着し、前記第１のアームの他端と前記第２のアー
ムの他端は作業手段を備えた移動部材に同一軸上で軸着
し、該移動部材の片側に前記第１のスライダと第２のス
ライダを配置させ、前記第１のアームの長さをＬ１、前
記第２のアームの長さをＬ２、前記第１のスライダと第
２のスライダ間の上下方向の軸芯間の距離をｄ、前記移
動部材の摺動方向の位置をｘ１、前記第１のスライダの
摺動方向の位置をＡｘとするとき、前記第２のスライダ
の摺動方向の位置Ｂｘは、
【００１０】という関係になるように前記第１のスライ
ダと前記第２のスライダを制御することにより達成され
る。
ダと前記第２のスライダを制御することにより達成され
る。
【００１１】また、前記移動部材の所望の速度カーブか
ら、前記第１のスライダの速度カーブを計算する工程１
と前記第２のスライダの速度カーブを計算する工程２を
設け、前記移動部材を移動させる以前に前記工程１と前
記工程２を計算し、移動時は得られた速度カーブに従い
第１のスライダ及び第２のスライダを制御することによ
り達成される。
ら、前記第１のスライダの速度カーブを計算する工程１
と前記第２のスライダの速度カーブを計算する工程２を
設け、前記移動部材を移動させる以前に前記工程１と前
記工程２を計算し、移動時は得られた速度カーブに従い
第１のスライダ及び第２のスライダを制御することによ
り達成される。
【００１２】次に、前記スライダの少なくとも１つに前
記アームの角度を検出する角度センサを設け、前記移動
部材を移動させる前に該角度センサから前記移動部材の
現在の高さを検出することにより達成される。
記アームの角度を検出する角度センサを設け、前記移動
部材を移動させる前に該角度センサから前記移動部材の
現在の高さを検出することにより達成される。
【００１３】更に、前記移動部材を移動させた後、該角
度センサから前記移動部材の高さを検出することにより
達成される。
度センサから前記移動部材の高さを検出することにより
達成される。
【００１４】
【発明の実施の形態】本発明の具体的な実施例を図面を
参照しながら以下詳細に説明する。本発明のリンクアー
ム機構１の制御方法を適用した自動遠心機１１０を図１
１に示す。自動遠心機１１０は、真空採決管に血液等の
検体１１１が通常５本入ったラック１１２を移動部材１
６に取り付けられたハンド１５で掴み、リンクアーム機
構１で遠心機内のバケット１１３に入れる。所定のラッ
ク数を入れた後、ロ−タ１１４を回転させ検体１１１を
遠心分離する。分離後、バケット１１３からラック１１
２をハンド１５により取り出し、ラック置場１１５へ置
く。上述のようにラック１１２をハンドリングする際の
移動部材１６の垂直動作について、図１を参照しながら
説明する。図１は、リンクアーム機構１の構成図と、リ
ンクアーム機構１を制御するＣＰＵボード２及びドライ
ブボード３を示す。リンクアーム機構１は、断面がＨ形
のベース４に互いに平行な第１の案内部材５と第２の案
内部材６を備え、第１の案内部材５上を摺動する第１の
スライダ７と第２の案内部材６上を摺動する第２のスラ
イダ８を備えている。第１のスライダ７はタイミングベ
ルト９に固定され、このタイミングベルト９を稼動する
駆動モータ１０を回転させることにより左右へ移動し、
同様に第２のスライダ８はタイミングベルト１１に固定
され、このタイミングベルト１１を稼動する駆動モータ
１２を回転させることにより左右へ移動する。また、第
１のスライダ７には、第１のアーム１３の一端（A）が
軸着され、同様に第２のスライダ８にも、第２のアーム
１４の一端（B）が軸着されている。第１のアーム１３
の他端と第２のアーム１４の他端は、ハンド等の作業手
段１５を備えた移動部材１６に同一軸上（H）で軸着さ
れ、移動部材１６の片側へ上記２つのスライダ７，８を
配置している。更に、移動部材１６と第１のスライダ７
は、第１のアーム１３と平行な平行リンク１７で軸着さ
れ、移動部材１６の姿勢を一定に保っている。上記２つ
の駆動モータ１０，１２は、それぞれモータケーブル１
８，１９とコネクタ２０を介してドライブボード３に接
続されている。また、マイクロコンピュータ（以下ＭＣ
Ｕと称す）２７及びメモリ２８等から構成されるＣＰＵ
ボード２と駆動モータ１０，１２を駆動するドライブボ
ード３は、コネクタ２１，２２とフラットケーブル２３
を介して接続されている。前述したように駆動モータ１
０，１２をベース４に固定したリンクアーム機構１は、
可動部に駆動モータを設けた直交形ロボットのようなケ
ーブルの曲折が無いため、曲折による断線は解消される
構成となっている。また停電等があった場合、駆動モー
タ１０，１２の位置保持力が無くなり移動部材１６の自
重で落下する恐れがあるが、駆動モータ１０，１２のロ
ータシャフトをバネ力で固定する電磁ロックブレーキ機
能を持つモータを採用し、落下を防止している。
参照しながら以下詳細に説明する。本発明のリンクアー
ム機構１の制御方法を適用した自動遠心機１１０を図１
１に示す。自動遠心機１１０は、真空採決管に血液等の
検体１１１が通常５本入ったラック１１２を移動部材１
６に取り付けられたハンド１５で掴み、リンクアーム機
構１で遠心機内のバケット１１３に入れる。所定のラッ
ク数を入れた後、ロ−タ１１４を回転させ検体１１１を
遠心分離する。分離後、バケット１１３からラック１１
２をハンド１５により取り出し、ラック置場１１５へ置
く。上述のようにラック１１２をハンドリングする際の
移動部材１６の垂直動作について、図１を参照しながら
説明する。図１は、リンクアーム機構１の構成図と、リ
ンクアーム機構１を制御するＣＰＵボード２及びドライ
ブボード３を示す。リンクアーム機構１は、断面がＨ形
のベース４に互いに平行な第１の案内部材５と第２の案
内部材６を備え、第１の案内部材５上を摺動する第１の
スライダ７と第２の案内部材６上を摺動する第２のスラ
イダ８を備えている。第１のスライダ７はタイミングベ
ルト９に固定され、このタイミングベルト９を稼動する
駆動モータ１０を回転させることにより左右へ移動し、
同様に第２のスライダ８はタイミングベルト１１に固定
され、このタイミングベルト１１を稼動する駆動モータ
１２を回転させることにより左右へ移動する。また、第
１のスライダ７には、第１のアーム１３の一端（A）が
軸着され、同様に第２のスライダ８にも、第２のアーム
１４の一端（B）が軸着されている。第１のアーム１３
の他端と第２のアーム１４の他端は、ハンド等の作業手
段１５を備えた移動部材１６に同一軸上（H）で軸着さ
れ、移動部材１６の片側へ上記２つのスライダ７，８を
配置している。更に、移動部材１６と第１のスライダ７
は、第１のアーム１３と平行な平行リンク１７で軸着さ
れ、移動部材１６の姿勢を一定に保っている。上記２つ
の駆動モータ１０，１２は、それぞれモータケーブル１
８，１９とコネクタ２０を介してドライブボード３に接
続されている。また、マイクロコンピュータ（以下ＭＣ
Ｕと称す）２７及びメモリ２８等から構成されるＣＰＵ
ボード２と駆動モータ１０，１２を駆動するドライブボ
ード３は、コネクタ２１，２２とフラットケーブル２３
を介して接続されている。前述したように駆動モータ１
０，１２をベース４に固定したリンクアーム機構１は、
可動部に駆動モータを設けた直交形ロボットのようなケ
ーブルの曲折が無いため、曲折による断線は解消される
構成となっている。また停電等があった場合、駆動モー
タ１０，１２の位置保持力が無くなり移動部材１６の自
重で落下する恐れがあるが、駆動モータ１０，１２のロ
ータシャフトをバネ力で固定する電磁ロックブレーキ機
能を持つモータを採用し、落下を防止している。
【００１５】前述のように構成したリンクアーム機構１
の座標定義図を図２に示す。座標軸はＸ−Ｚ軸の直交座
標とし、点Ａは第１のスライダ７の軸芯，点Ｂは第２の
スライダ８の軸芯，点Ｈはハンド等の作業手段１５を備
えた移動部材１６の軸芯を表わす。第１のアーム１３の
長さをＬ１，第２のアーム１４の長さをＬ２，第１のス
ライダ７と第２のスライダ８の上下方向つまりＺ軸方向
の軸芯間の距離をｄとすると、点Ａは直線Ｚ＝Ｌ１上
を、点Ｂは直線Ｚ＝Ｌ１−ｄ上を移動し、点ＨのＺ軸方
向の可動範囲は０〜（Ｌ１＋Ｌ２−ｄ）となる。又、直
線Ｚ＝Ｌ１と第１のアーム１３のなす角をα（−π／２
≦α≦sin-1{(Ｌ２−ｄ)／Ｌ１｝）とし、点ＨからＸ軸
方向へ垂線を下ろし直線Ｚ＝Ｌ１と交わる点をＥ，直線
Ｚ＝Ｌ１−ｄと交わる点をＦとすると、線分ＨＥ，Ａ
Ｅ，ＨＦ，ＢＦの長さは、
の座標定義図を図２に示す。座標軸はＸ−Ｚ軸の直交座
標とし、点Ａは第１のスライダ７の軸芯，点Ｂは第２の
スライダ８の軸芯，点Ｈはハンド等の作業手段１５を備
えた移動部材１６の軸芯を表わす。第１のアーム１３の
長さをＬ１，第２のアーム１４の長さをＬ２，第１のス
ライダ７と第２のスライダ８の上下方向つまりＺ軸方向
の軸芯間の距離をｄとすると、点Ａは直線Ｚ＝Ｌ１上
を、点Ｂは直線Ｚ＝Ｌ１−ｄ上を移動し、点ＨのＺ軸方
向の可動範囲は０〜（Ｌ１＋Ｌ２−ｄ）となる。又、直
線Ｚ＝Ｌ１と第１のアーム１３のなす角をα（−π／２
≦α≦sin-1{(Ｌ２−ｄ)／Ｌ１｝）とし、点ＨからＸ軸
方向へ垂線を下ろし直線Ｚ＝Ｌ１と交わる点をＥ，直線
Ｚ＝Ｌ１−ｄと交わる点をＦとすると、線分ＨＥ，Ａ
Ｅ，ＨＦ，ＢＦの長さは、
【００１６】となる。今、点Ｈが直線Ｘ＝ｘ１上を移動
する場合、点Ａと点Ｂの座標をそれぞれ（Ａｘ，Ｌ１）
（Ｂｘ，Ｌ１−ｄ）とすると、Ｘ軸方向の位置Ａｘ，Ｂ
ｘは、
する場合、点Ａと点Ｂの座標をそれぞれ（Ａｘ，Ｌ１）
（Ｂｘ，Ｌ１−ｄ）とすると、Ｘ軸方向の位置Ａｘ，Ｂ
ｘは、
【００１７】
【００１８】となり、式（５）と（６）式からαを消去
すると、
すると、
【００１９】が導かれる。式（７）は、点Ｈが直線Ｘ＝
ｘ１上を垂直移動する時の点Ａと点Ｂの位置を表わす関
係式であり、逆に第１のスライダ７と第２のスライダ８
の位置を式（７）を満足するように制御することによ
り、移動部材１６は垂直移動が可能となる。
ｘ１上を垂直移動する時の点Ａと点Ｂの位置を表わす関
係式であり、逆に第１のスライダ７と第２のスライダ８
の位置を式（７）を満足するように制御することによ
り、移動部材１６は垂直移動が可能となる。
【００２０】なお、式（５）及び式（６）は、図１及び
図２に示すように、第１及び第２のスライダ７，８が移
動部材１６の左側に位置している状態を想定して式化し
たものである。したがって、例えば第１及び第２のスラ
イダ７，８が移動部材１６の右側に位置している状態を
想定して式化すれば、上記式（５）〜式（７）は次のよ
うになる。
図２に示すように、第１及び第２のスライダ７，８が移
動部材１６の左側に位置している状態を想定して式化し
たものである。したがって、例えば第１及び第２のスラ
イダ７，８が移動部材１６の右側に位置している状態を
想定して式化すれば、上記式（５）〜式（７）は次のよ
うになる。
【００２１】
【００２２】しかし、第１及び第２のスライダ７，８が
移動部材１６の左側に位置しているか、右側に位置して
いるかは、リンクアーム機構１を表から見るか裏から見
るか程度の差でしかないため、式（７）と式（７ａ）の
差は、Ｘ軸の方向をどちらに取るかの考え方の差でしか
ない。したがって、式（７）を満足する制御を行うとい
うことと、式（７ａ）を満足する制御を行うということ
は全く同じ意味である。
移動部材１６の左側に位置しているか、右側に位置して
いるかは、リンクアーム機構１を表から見るか裏から見
るか程度の差でしかないため、式（７）と式（７ａ）の
差は、Ｘ軸の方向をどちらに取るかの考え方の差でしか
ない。したがって、式（７）を満足する制御を行うとい
うことと、式（７ａ）を満足する制御を行うということ
は全く同じ意味である。
【００２３】次に、点Ｈが図２に示すように直線Ｘ＝ｘ
１上を移動前の既知の高さＨ1Zから移動先の所望の高さ
Ｈ2Zまで移動時間Ｔ秒経て移動する場合について説明す
る。先ず、移動部材１６の垂直移動中の時間ｔと角度α
について関係式α(t)を導く。移動部材１６の軸芯点Ｈ
のＺ軸成分をＨｚとすると、
１上を移動前の既知の高さＨ1Zから移動先の所望の高さ
Ｈ2Zまで移動時間Ｔ秒経て移動する場合について説明す
る。先ず、移動部材１６の垂直移動中の時間ｔと角度α
について関係式α(t)を導く。移動部材１６の軸芯点Ｈ
のＺ軸成分をＨｚとすると、
【００２４】となる。時間ｔと高さＨｚの関係をcos関
数の半周期を用いて、振幅を移動距離、角度を時間に置
き換えて変換すると、
数の半周期を用いて、振幅を移動距離、角度を時間に置
き換えて変換すると、
【００２５】となり、時間ｔと高さＨｚの関係は図３に
示すような曲線３０になる。ここで、Ｈ2Z−Ｈ1Z＝Ｍと
し、式（８）（９）からαについて解くと、
示すような曲線３０になる。ここで、Ｈ2Z−Ｈ1Z＝Ｍと
し、式（８）（９）からαについて解くと、
【００２６】となり、移動部材１６の垂直移動中の時間
ｔと角度αについての関係式α(t)を得る。移動部材１
６の高さＨｚと時間ｔをcos関数を用いたため、移動部
材１６の速度カーブ４１は図４に示すような半周期のsi
n波となり、これを移動部材１６の所望の速度カーブ４
１とする。
ｔと角度αについての関係式α(t)を得る。移動部材１
６の高さＨｚと時間ｔをcos関数を用いたため、移動部
材１６の速度カーブ４１は図４に示すような半周期のsi
n波となり、これを移動部材１６の所望の速度カーブ４
１とする。
【００２７】続いて、移動部材１６の所望の速度カーブ
４１から、第１のスライダ７の速度カーブ４２を計算す
る工程１と、第２のスライダ８の速度カーブ４３を計算
する工程２に関して、駆動モータ１０，１２にエンコー
ダ付きサーボモータを適用した場合の計算手法について
説明する。サーボモータの制御は、制御計算周期毎にエ
ンコーダの回転パルス信号をフィードバックしサーボモ
ータの回転角度及び角速度を求め、指令速度とサーボモ
ータの角速度の差から比例項・積分項・微分項の３つの
項を加算するＰＩＤ演算を行い、ＰＷＭ（パルス・ワイ
ド・モジュレーション）制御によりサーボモータを駆動
制御する。制御計算周期の時間をΔｔとすると、移動中
の時間ｔはΔｔの累積であるから、
４１から、第１のスライダ７の速度カーブ４２を計算す
る工程１と、第２のスライダ８の速度カーブ４３を計算
する工程２に関して、駆動モータ１０，１２にエンコー
ダ付きサーボモータを適用した場合の計算手法について
説明する。サーボモータの制御は、制御計算周期毎にエ
ンコーダの回転パルス信号をフィードバックしサーボモ
ータの回転角度及び角速度を求め、指令速度とサーボモ
ータの角速度の差から比例項・積分項・微分項の３つの
項を加算するＰＩＤ演算を行い、ＰＷＭ（パルス・ワイ
ド・モジュレーション）制御によりサーボモータを駆動
制御する。制御計算周期の時間をΔｔとすると、移動中
の時間ｔはΔｔの累積であるから、
【００２８】で表わせられ、ｉは制御回数、ｎは移動部
材１６を所望の高さＨ2Zまで移動させた時の制御回数を
表わし、移動時間ＴはＴ＝ｎ・Δｔとなる。
材１６を所望の高さＨ2Zまで移動させた時の制御回数を
表わし、移動時間ＴはＴ＝ｎ・Δｔとなる。
【００２９】フローチャート図５を用いて工程１の計算
手法を、制御計算周期に制御回数を乗じた時間ｔiにつ
いて説明すると、処理５００で式（１０）からアーム角
度α(ti)を求め、処理５０１で点ＨのＸ座標成分ｘ１及
びアーム角度α(ti)を式（５）へ代入し第１のスライダ
７の位置Ａｘ(ti)を計算する。処理５０２で制御計算周
期毎の第１のスライダ７の位置Ａｘの差、つまり
手法を、制御計算周期に制御回数を乗じた時間ｔiにつ
いて説明すると、処理５００で式（１０）からアーム角
度α(ti)を求め、処理５０１で点ＨのＸ座標成分ｘ１及
びアーム角度α(ti)を式（５）へ代入し第１のスライダ
７の位置Ａｘ(ti)を計算する。処理５０２で制御計算周
期毎の第１のスライダ７の位置Ａｘの差、つまり
【００３０】を計算すると、Δｔ時間後の第１のスライ
ダ７の移動距離が得られ、その移動距離とΔｔ時間から
第１のスライダ７の速度を得る。更に、処理５０３で１
回転当りの第１のスライダ７のピッチと第１のスライダ
７の速度から、駆動モータ１０の指令速度に変換する。
処理５０４でこれらの一連の計算をｉ＝０から順次ｎま
で繰り返し行い、処理５０５で駆動モータ１０の指令速
度データテーブルを作成し、ＣＰＵボード２のメモリ２
８に記憶する。
ダ７の移動距離が得られ、その移動距離とΔｔ時間から
第１のスライダ７の速度を得る。更に、処理５０３で１
回転当りの第１のスライダ７のピッチと第１のスライダ
７の速度から、駆動モータ１０の指令速度に変換する。
処理５０４でこれらの一連の計算をｉ＝０から順次ｎま
で繰り返し行い、処理５０５で駆動モータ１０の指令速
度データテーブルを作成し、ＣＰＵボード２のメモリ２
８に記憶する。
【００３１】次に工程２についてフローチャート図６を
用いて説明する。時間ｔiの場合、処理６００で点Ｈの
Ｘ座標成分ｘ１及び工程１で求めたＡｘ(ti)を式（７）
へ代入し、第２のスライダ８の位置Ｂｘ(ti)を得る。処
理６０１で工程１と同様に、制御計算周期毎の第２のス
ライダ８の位置Ｂｘの差
用いて説明する。時間ｔiの場合、処理６００で点Ｈの
Ｘ座標成分ｘ１及び工程１で求めたＡｘ(ti)を式（７）
へ代入し、第２のスライダ８の位置Ｂｘ(ti)を得る。処
理６０１で工程１と同様に、制御計算周期毎の第２のス
ライダ８の位置Ｂｘの差
【００３２】から、Δｔ時間後の第２のスライダ８の移
動距離が得られ、その移動距離から第２のスライダ８の
速度を得る。更に、処理６０２で１回転当りの第２のス
ライダ８のピッチと第２のスライダ８の速度から、駆動
モータ１２の指令速度に変換する。処理６０３でこれら
の一連の計算をｉ＝０から順次ｎまで繰り返し行い、処
理６０４で駆動モータ１２の指令速度データテーブルを
作成し、ＣＰＵボード２のメモリ２８に記憶する。図４
に工程１と工程２の計算より得た第１のスライダ７の速
度カーブ４２と第２のスライダ８の速度カーブ４３を示
す。
動距離が得られ、その移動距離から第２のスライダ８の
速度を得る。更に、処理６０２で１回転当りの第２のス
ライダ８のピッチと第２のスライダ８の速度から、駆動
モータ１２の指令速度に変換する。処理６０３でこれら
の一連の計算をｉ＝０から順次ｎまで繰り返し行い、処
理６０４で駆動モータ１２の指令速度データテーブルを
作成し、ＣＰＵボード２のメモリ２８に記憶する。図４
に工程１と工程２の計算より得た第１のスライダ７の速
度カーブ４２と第２のスライダ８の速度カーブ４３を示
す。
【００３３】以上の工程１及び工程２を経て、移動部材
１６を移動させる場合は、工程１で求めた速度カーブ４
２になるようにＣＰＵボード２のメモリ２８に記憶させ
た駆動モータ１０の指令速度データを制御計算周期毎に
順次読み出し、第１のスライダ７を速度制御する。同様
に工程２で求めた速度カーブ４３になるようにＣＰＵボ
ード２のメモリ２８に記憶させた駆動モータ１２の指令
速度データを制御計算周期毎に順次読み出し、第２のス
ライダ８を速度制御する。速度制御の手法は、前述した
ようにエンコーダフィードバックによる角速度変換と指
令速度の差からＰＩＤ演算により、駆動モータ１０，１
２を制御する。２つの駆動モータ１０，１２の制御は、
例えばＣＰＵ１個で時分割によりタスクを切り替える方
式で同時に制御駆動し、移動部材１６を高さＨ1Zから所
望の高さＨ2Zへ垂直に移動させる。
１６を移動させる場合は、工程１で求めた速度カーブ４
２になるようにＣＰＵボード２のメモリ２８に記憶させ
た駆動モータ１０の指令速度データを制御計算周期毎に
順次読み出し、第１のスライダ７を速度制御する。同様
に工程２で求めた速度カーブ４３になるようにＣＰＵボ
ード２のメモリ２８に記憶させた駆動モータ１２の指令
速度データを制御計算周期毎に順次読み出し、第２のス
ライダ８を速度制御する。速度制御の手法は、前述した
ようにエンコーダフィードバックによる角速度変換と指
令速度の差からＰＩＤ演算により、駆動モータ１０，１
２を制御する。２つの駆動モータ１０，１２の制御は、
例えばＣＰＵ１個で時分割によりタスクを切り替える方
式で同時に制御駆動し、移動部材１６を高さＨ1Zから所
望の高さＨ2Zへ垂直に移動させる。
【００３４】以上の説明では移動部材１６を上昇させる
場合について説明したが、高さＨ2Zから高さＨ1Zへ下降
させる場合は、ＣＰＵボード２のメモリ２８に記憶させ
た指令速度データの読み出し順番を逆にして駆動モータ
１０，１２を制御する。また、移動部材１６を水平に移
動させる場合は、駆動モータ１０，１２を同時に同じ指
令速度で駆動すれば良い。
場合について説明したが、高さＨ2Zから高さＨ1Zへ下降
させる場合は、ＣＰＵボード２のメモリ２８に記憶させ
た指令速度データの読み出し順番を逆にして駆動モータ
１０，１２を制御する。また、移動部材１６を水平に移
動させる場合は、駆動モータ１０，１２を同時に同じ指
令速度で駆動すれば良い。
【００３５】前述ではエンコーダ付きサーボモータを適
用した場合の計算工程１と計算工程２について説明した
が、以下駆動モータ１０，１２にパルスレートで駆動す
るステッピングモータを適用した場合について説明す
る。ステッピングモータを１ステップ回転させると、ス
ライダ７，８は１ステップ回転角に相当する分だけ移動
する。その１ステップ分の移動量ΔＡｘ，ΔＢｘは、
用した場合の計算工程１と計算工程２について説明した
が、以下駆動モータ１０，１２にパルスレートで駆動す
るステッピングモータを適用した場合について説明す
る。ステッピングモータを１ステップ回転させると、ス
ライダ７，８は１ステップ回転角に相当する分だけ移動
する。その１ステップ分の移動量ΔＡｘ，ΔＢｘは、
【００３６】となる。ここで、ｓはステップ角度，１／
Ｎは減速比，ｐはピッチ（mm/１回転）である。このよ
うにステッピングモータの場合、パルスレートで駆動す
るためスライダ７，８の停止位置は離散的となる。その
ため、離散的な位置とその位置を通過する時間からパル
スレートを決定する計算手法を導く。今、点Ｈの移動前
の既知の高さＨ1Zと移動先の所望の高さＨ2Zが与えられ
るとし、図４の移動部材１６の所望の速度カーブ４１を
示す式（９）からＨ1Z−Ｈ2Z＝Ｍとして時間ｔについて
解くと、
Ｎは減速比，ｐはピッチ（mm/１回転）である。このよ
うにステッピングモータの場合、パルスレートで駆動す
るためスライダ７，８の停止位置は離散的となる。その
ため、離散的な位置とその位置を通過する時間からパル
スレートを決定する計算手法を導く。今、点Ｈの移動前
の既知の高さＨ1Zと移動先の所望の高さＨ2Zが与えられ
るとし、図４の移動部材１６の所望の速度カーブ４１を
示す式（９）からＨ1Z−Ｈ2Z＝Ｍとして時間ｔについて
解くと、
【００３７】となる。式（１５）は時間ｔを移動部材１
６の高さＨｚの関数で表わしたものである。
６の高さＨｚの関数で表わしたものである。
【００３８】先ず、第１のスライダ７の速度カーブ４２
を計算する工程１について説明する。移動部材１６の高
さＨｚを第１のスライダ７の位置Ａｘで表現すると、
を計算する工程１について説明する。移動部材１６の高
さＨｚを第１のスライダ７の位置Ａｘで表現すると、
【００３９】式（１６）を式（１５）に代入して、
【００４０】を得る。式（１７）は時間ｔを第１のスラ
イダ７の位置Ａｘの関数で表現したものであり、第１の
スライダ７の離散的な位置をＡｘに代入するとその位置
を通過する時間ｔが得られる。上式（１６）（１７）の
±について図２を参照しながら説明すると、点Ｈの高さ
Ｈｚが直線Ｚ＝Ｌ１より上か下かで場合分けされる。移
動開始時の計算上の点Ｈの高さＨｚ＝Ｈ1Zで直線Ｚ＝Ｌ
１より下方のため−であり、直角三角形ＡＨＥの辺ＨＥ
が０になるまで計算上の点ＡをＸ軸負方向へΔＡｘずつ
移動させる。次に計算上の点ＡをＸ軸正方向へΔＡｘ分
移動させ、計算上の点Ｈの高さをＬ１より上方とみなし
（＋）、点Ｈの高さがＬ１からＨ2Zまで計算上の点Ａを
Ｘ軸の正方向へΔＡｘずつ移動させる。点Ｈの移動前の
高さＨ1ZからＬ１までの、点ＡのＸ軸負方向へ移動する
距離は、
イダ７の位置Ａｘの関数で表現したものであり、第１の
スライダ７の離散的な位置をＡｘに代入するとその位置
を通過する時間ｔが得られる。上式（１６）（１７）の
±について図２を参照しながら説明すると、点Ｈの高さ
Ｈｚが直線Ｚ＝Ｌ１より上か下かで場合分けされる。移
動開始時の計算上の点Ｈの高さＨｚ＝Ｈ1Zで直線Ｚ＝Ｌ
１より下方のため−であり、直角三角形ＡＨＥの辺ＨＥ
が０になるまで計算上の点ＡをＸ軸負方向へΔＡｘずつ
移動させる。次に計算上の点ＡをＸ軸正方向へΔＡｘ分
移動させ、計算上の点Ｈの高さをＬ１より上方とみなし
（＋）、点Ｈの高さがＬ１からＨ2Zまで計算上の点Ａを
Ｘ軸の正方向へΔＡｘずつ移動させる。点Ｈの移動前の
高さＨ1ZからＬ１までの、点ＡのＸ軸負方向へ移動する
距離は、
【００４１】となり、その移動パルス数は式（１８）の
移動距離を１ステップ分の移動量ΔＡｘで割った数とな
る。点Ａの位置Ａｘは１ステップ移動前の位置Ａｘ−Δ
Ａｘより得て、式（１７）に代入し、時間ｔを求める。
ΔＡｘをＡｘから順次引き更新し、式（１７）より点Ａ
の離散的な位置と通過時間を得て、その１ステップ分の
時間の差からパルスレートを求める。パルス出力方式
は、図７に示すようなブロック回路でＭＣＵ２７が有す
るタイマＡ７１の割り込み機能を用い、パルス周波数を
変化させる方式とし、タイマクロック周期と移動前後の
時間差から割り込み間隔のタイマ値Ａｔは、
移動距離を１ステップ分の移動量ΔＡｘで割った数とな
る。点Ａの位置Ａｘは１ステップ移動前の位置Ａｘ−Δ
Ａｘより得て、式（１７）に代入し、時間ｔを求める。
ΔＡｘをＡｘから順次引き更新し、式（１７）より点Ａ
の離散的な位置と通過時間を得て、その１ステップ分の
時間の差からパルスレートを求める。パルス出力方式
は、図７に示すようなブロック回路でＭＣＵ２７が有す
るタイマＡ７１の割り込み機能を用い、パルス周波数を
変化させる方式とし、タイマクロック周期と移動前後の
時間差から割り込み間隔のタイマ値Ａｔは、
【００４２】より得る。次に点Ｈの高さＬ１から移動先
のＨ2Zまでの、点ＡのＸ軸正方向へ移動する距離は、
のＨ2Zまでの、点ＡのＸ軸正方向へ移動する距離は、
【００４３】となり、その移動パルス数は式（２０）の
移動距離を１ステップ分の移動量ΔＡｘで割った数とな
る。同様に上述した計算方法により、１ステップ分の移
動前後の時間差からタイマ値Ａｔを求め、各移動分のタ
イマ値をテーブル化し、ＣＰＵボード２のメモリ２８に
記憶する。
移動距離を１ステップ分の移動量ΔＡｘで割った数とな
る。同様に上述した計算方法により、１ステップ分の移
動前後の時間差からタイマ値Ａｔを求め、各移動分のタ
イマ値をテーブル化し、ＣＰＵボード２のメモリ２８に
記憶する。
【００４４】続いて、第２のスライダ８の速度カーブ４
３を計算する工程２について説明する。移動部材１６の
高さＨｚを第２のスライダ８の位置Ｂｘで表現すると、
３を計算する工程２について説明する。移動部材１６の
高さＨｚを第２のスライダ８の位置Ｂｘで表現すると、
【００４５】式（２１）を式（１５）に代入して、
【００４６】を得る。式（２２）は時間ｔを第２のスラ
イダ８の位置Ｂｘの関数で表現したものであり、前述し
た第１のスライダ７の場合と同様、第２のスライダ８の
離散的な位置をＢｘに代入するとその位置を通過する時
間ｔが得られる。上式（２１）（２２）の±について
は、点Ｈの高さＨｚが直線Ｚ＝Ｌ１−ｄより上か下かで
場合分けされるが、計算上の点Ｈの高さＨｚが直線Ｚ＝
Ｌ１−ｄより下方の場合は−であり、直角三角形ＢＨＦ
の辺ＨＦが０になるまで計算上の点ＢをＸ軸負方向へΔ
Ｂｘずつ移動させる。次に計算上の点ＢをＸ軸正方向へ
ΔＢｘ分移動させ、計算上の点Ｈの高さをＬ１−ｄより
上方とみなし（＋）、点Ｈの高さがＬ１−ｄからＨ2Zま
で計算上の点ＢをＸ軸の正方向へΔＢｘずつ移動させ
る。点Ｈの移動前の高さＨ1ZからＬ１−ｄまでの、点Ｂ
のＸ軸負方向へ移動する距離は、
イダ８の位置Ｂｘの関数で表現したものであり、前述し
た第１のスライダ７の場合と同様、第２のスライダ８の
離散的な位置をＢｘに代入するとその位置を通過する時
間ｔが得られる。上式（２１）（２２）の±について
は、点Ｈの高さＨｚが直線Ｚ＝Ｌ１−ｄより上か下かで
場合分けされるが、計算上の点Ｈの高さＨｚが直線Ｚ＝
Ｌ１−ｄより下方の場合は−であり、直角三角形ＢＨＦ
の辺ＨＦが０になるまで計算上の点ＢをＸ軸負方向へΔ
Ｂｘずつ移動させる。次に計算上の点ＢをＸ軸正方向へ
ΔＢｘ分移動させ、計算上の点Ｈの高さをＬ１−ｄより
上方とみなし（＋）、点Ｈの高さがＬ１−ｄからＨ2Zま
で計算上の点ＢをＸ軸の正方向へΔＢｘずつ移動させ
る。点Ｈの移動前の高さＨ1ZからＬ１−ｄまでの、点Ｂ
のＸ軸負方向へ移動する距離は、
【００４７】となり、その移動パルス数は式（２３）の
移動距離を１ステップ分の移動量ΔＢｘで割った数とな
る。点Ｂの位置Ｂｘは１ステップ移動前の位置Ｂｘ−Δ
Ｂｘより得て、式（２２）に代入し時間ｔを求める。Δ
ＢｘをＢｘから順次引き更新し、式（２２）より点Ｂの
離散的な位置と通過時間を得て、その１ステップ分の時
間の差からパルスレートを求める。第１のスライダ７と
同様、パルス出力方式をＭＣＵ２７が有するタイマＢ７
２の割り込み機能を用い、パルス周波数を変化させる方
式とし、タイマクロック周期と移動前後の時間差から割
り込み間隔のタイマ値Ｂｔは、
移動距離を１ステップ分の移動量ΔＢｘで割った数とな
る。点Ｂの位置Ｂｘは１ステップ移動前の位置Ｂｘ−Δ
Ｂｘより得て、式（２２）に代入し時間ｔを求める。Δ
ＢｘをＢｘから順次引き更新し、式（２２）より点Ｂの
離散的な位置と通過時間を得て、その１ステップ分の時
間の差からパルスレートを求める。第１のスライダ７と
同様、パルス出力方式をＭＣＵ２７が有するタイマＢ７
２の割り込み機能を用い、パルス周波数を変化させる方
式とし、タイマクロック周期と移動前後の時間差から割
り込み間隔のタイマ値Ｂｔは、
【００４８】より得る。次に点Ｈの高さＬ１−ｄから移
動先のＨ2Zまでの、点ＢのＸ軸正方向への移動距離は、
動先のＨ2Zまでの、点ＢのＸ軸正方向への移動距離は、
【００４９】となり、その移動パルス数は式（２５）の
移動距離を１ステップ分の移動量ΔＢｘで割った数とな
る。同様に上述した計算方法により、１ステップ分の移
動前後の時間差からタイマ値Ｂｔを求め、各移動分のタ
イマ値をテーブル化し、ＣＰＵボード２のメモリ２８に
記憶する。ステッピングモータの場合もサーボモータと
同様、各スライダ７，８の速度カーブは図４に示すよう
な第１のスライダ７の速度カーブ４２及び第２のスライ
ダ８の速度カーブ４３となる。又、メモリ２８には各ス
ライダ７，８の移動方向別のパルス数とタイマ値が、図
８に示すような構成で整然と格納されている。８０１は
第１のスライダ７の負方向パルス数で、８０２は第１の
スライダ７の正方向パルス数で、８０３は第２のスライ
ダ８の負方向パルス数で、８０４は第２のスライダ８の
正方向パルス数で、８０５は第１のスライダ７の負方向
移動時のタイマ値テーブルで、８０６は第１のスライダ
７の正方向移動時のタイマ値テーブルで、８０７は第２
のスライダ８の負方向移動時のタイマ値テーブルで、８
０８は第２のスライダ８の正方向移動時のタイマ値テー
ブルである。
移動距離を１ステップ分の移動量ΔＢｘで割った数とな
る。同様に上述した計算方法により、１ステップ分の移
動前後の時間差からタイマ値Ｂｔを求め、各移動分のタ
イマ値をテーブル化し、ＣＰＵボード２のメモリ２８に
記憶する。ステッピングモータの場合もサーボモータと
同様、各スライダ７，８の速度カーブは図４に示すよう
な第１のスライダ７の速度カーブ４２及び第２のスライ
ダ８の速度カーブ４３となる。又、メモリ２８には各ス
ライダ７，８の移動方向別のパルス数とタイマ値が、図
８に示すような構成で整然と格納されている。８０１は
第１のスライダ７の負方向パルス数で、８０２は第１の
スライダ７の正方向パルス数で、８０３は第２のスライ
ダ８の負方向パルス数で、８０４は第２のスライダ８の
正方向パルス数で、８０５は第１のスライダ７の負方向
移動時のタイマ値テーブルで、８０６は第１のスライダ
７の正方向移動時のタイマ値テーブルで、８０７は第２
のスライダ８の負方向移動時のタイマ値テーブルで、８
０８は第２のスライダ８の正方向移動時のタイマ値テー
ブルである。
【００５０】ここで、駆動モータ１０，１２にステッピ
ングモータを適用した時のブロック回路を示した図７
と、フロチャート図９，図１０を参照しながら、前述し
た計算の工程１及び工程２より得たタイマ値を用いて、
移動部材１６を上昇させる場合について説明する。図９
は移動部材１６を移動させる時のサブルーチンであり、
図１０はタイマ割り込み処理の上昇動作部分のフローチ
ャートである。回転開始時、処理９０１で出力パルスの
数をカウントするカウンタＡ及びカウンタＢをリセット
し、処理９０２でメモリ２８から最初のタイマ値をそれ
ぞれ読み出し、タイマＡ及びタイマＢのタイマ値を設定
する。その後、処理９０３でタイマＡ及びタイマＢの割
り込みを許可して割り込み機能を起動させる。タイマ割
り込みが発生すると、プログラムは割り込み処理へ移行
する。タイマＡ及びタイマＢの割り込み処理は、別々に
設けているが処理内容は図１０に示す内容と同様である
ため、図１０を用いて割り込み処理の内容を説明する。
割り込み処理では、処理１００１で負方向の駆動パルス
出力が終了したかどうかを判断し、未だであれば処理１
００２で負方向への駆動パルスを出力し、処理１００３
で負方向出力パルスをカウントし、処理１００４で次回
までの割り込み間隔時間となるタイマ値をメモリ２８か
ら順次読み出し更新する。上記処理を負方向へのパルス
出力が終了するまで割り込み発生毎に繰り返し実行す
る。駆動パルスは、５相コントローラ７３，７４で相切
り換え信号に変換され、ドライバ３から駆動モータ１
０，１２に電流を流し、駆動モータ１０，１２は回転す
る。負方向へのパルス出力が終了すると、処理１００５
で正方向への出力パルス数の判定、処理１００６で正方
向への駆動パルスの出力、処理１００７で出力パルスの
カウント、処理１００８でタイマ値の更新を行い、全パ
ルス出力するまで割り込み発生毎に繰り返し実行し、処
理１００９でパルス出力終了後タイマ割り込み機能を禁
止状態にする。図９に示すサブルーチンでは、処理９０
４でタイマＡのパルス出力が終了したかどうか、処理９
０５でタイマＢのパルス出力が終了したかどうかを判定
し、移動部材１６の上昇終了を判断する。これらのプロ
グラムは読み出し専用メモリＲＯＭ７０に格納されてい
る。以上説明した制御手法により、移動部材１６は高さ
Ｈ1Zから所望の高さＨ2Zへ垂直に移動する。また、高さ
Ｈ2Zから高さＨ1Zへ下降させたい場合は、サーボモータ
適用時と同様メモリ２８のテーブル状に記憶させたタイ
マ値の読み出し順番を逆にして、駆動モータ１０，１２
を制御する。更に移動部材１６を水平に移動させる場合
は、駆動モータ１０，１２を同時に同じパルスレートで
駆動すれば良い。
ングモータを適用した時のブロック回路を示した図７
と、フロチャート図９，図１０を参照しながら、前述し
た計算の工程１及び工程２より得たタイマ値を用いて、
移動部材１６を上昇させる場合について説明する。図９
は移動部材１６を移動させる時のサブルーチンであり、
図１０はタイマ割り込み処理の上昇動作部分のフローチ
ャートである。回転開始時、処理９０１で出力パルスの
数をカウントするカウンタＡ及びカウンタＢをリセット
し、処理９０２でメモリ２８から最初のタイマ値をそれ
ぞれ読み出し、タイマＡ及びタイマＢのタイマ値を設定
する。その後、処理９０３でタイマＡ及びタイマＢの割
り込みを許可して割り込み機能を起動させる。タイマ割
り込みが発生すると、プログラムは割り込み処理へ移行
する。タイマＡ及びタイマＢの割り込み処理は、別々に
設けているが処理内容は図１０に示す内容と同様である
ため、図１０を用いて割り込み処理の内容を説明する。
割り込み処理では、処理１００１で負方向の駆動パルス
出力が終了したかどうかを判断し、未だであれば処理１
００２で負方向への駆動パルスを出力し、処理１００３
で負方向出力パルスをカウントし、処理１００４で次回
までの割り込み間隔時間となるタイマ値をメモリ２８か
ら順次読み出し更新する。上記処理を負方向へのパルス
出力が終了するまで割り込み発生毎に繰り返し実行す
る。駆動パルスは、５相コントローラ７３，７４で相切
り換え信号に変換され、ドライバ３から駆動モータ１
０，１２に電流を流し、駆動モータ１０，１２は回転す
る。負方向へのパルス出力が終了すると、処理１００５
で正方向への出力パルス数の判定、処理１００６で正方
向への駆動パルスの出力、処理１００７で出力パルスの
カウント、処理１００８でタイマ値の更新を行い、全パ
ルス出力するまで割り込み発生毎に繰り返し実行し、処
理１００９でパルス出力終了後タイマ割り込み機能を禁
止状態にする。図９に示すサブルーチンでは、処理９０
４でタイマＡのパルス出力が終了したかどうか、処理９
０５でタイマＢのパルス出力が終了したかどうかを判定
し、移動部材１６の上昇終了を判断する。これらのプロ
グラムは読み出し専用メモリＲＯＭ７０に格納されてい
る。以上説明した制御手法により、移動部材１６は高さ
Ｈ1Zから所望の高さＨ2Zへ垂直に移動する。また、高さ
Ｈ2Zから高さＨ1Zへ下降させたい場合は、サーボモータ
適用時と同様メモリ２８のテーブル状に記憶させたタイ
マ値の読み出し順番を逆にして、駆動モータ１０，１２
を制御する。更に移動部材１６を水平に移動させる場合
は、駆動モータ１０，１２を同時に同じパルスレートで
駆動すれば良い。
【００５１】以上今までの説明では、移動部材１６を既
知の高さＨ1Zから所望の高さＨ2Zまで垂直に移動させる
制御方法について触れたが、次に未知の高さから垂直移
動させる場合について説明する。電源投入直後を考える
と、絶対値エンコーダを有したサーボモータの場合はモ
ータの回転角度からスライダ７，８の位置を求め、移動
部材１６の高さを計算できるが、ステッピングモータの
場合はモータの回転角度が不明なため、スライダ７，８
の位置が判らず移動部材１６の高さを求めることができ
ない。そこで、図１に示すリンクアーム機構１の第１の
スライダ７の平行リンク１７の同一軸上に角度センサ２
４を新たに設け、第１のアーム１３と第１の案内部材５
とのなす角αを角度センサ２４で検出する。第１のアー
ム１３の長さＬ１は既知であるため、移動部材１６の高
さは式（８）より求めることができる。本実施例では角
度センサ２４をポテンショメータとし、ケーブル２５で
ＣＰＵボード２へコネクタ２６を介してＭＰＵ２７のＡ
／Ｄ変換器７５へ接続する。電源投入後、移動部材１６
を上昇移動させる時のメインルーチンを示す図１３を参
照しながら説明すると、処理１３０１でＭＰＵ２７が、
ポテンショメータから出力されるアナログ電圧を入力
し、Ａ／Ｄ変換器７５でアナログ値をデジタル値に変換
し、予めＲＯＭ７０に記憶させていた図１２に示す変換
グラフ１２０１から移動部材１６の高さを得る。処理１
３０２で移動部材１６の高さから、上記説明した計算手
法によりタイマ値テーブルを作成する。処理１３０３で
は、図９に示すサブルーチンをコールし、上記説明した
手法でステッピングモータを駆動し、移動部材１６を所
望の高さＨ2Zまで垂直に移動させる。移動後、処理１３
０４でポテンショメータから出力されるアナログ電圧を
再び入力し、変換グラフ１２０１から移動部材１６の高
さを得る。処理１３０５で所定の高さまで移動したかど
うかを確認し、正常な場合は上昇移動完了と判断する。
所定の高さまで移動しなかった場合は処理１３０６のエ
ラー処理へ分岐し、ステッピングモータの脱調、タイミ
ングベルト９、１１の断線等を表すエラーメッセージを
出す。再スタートの要求で処理１３０１へジャンプし、
再び前述した処理を実行する。下降及び水平移動の場合
も、処理１３０３から処理１３０６と同様の手法で、角
度センサ２４で移動部材１６の高さを検出し、正常な移
動かどうかを確認しながらリンクアーム機構１を制御す
る。
知の高さＨ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 true JPH11262881A (ja) | 1999-09-28 |
| JP3731846B2 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) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005230918A (ja) * | 2004-02-04 | 2005-09-02 | Janome Sewing Mach Co Ltd | ロボット |
| JP2013063302A (ja) * | 2012-12-05 | 2013-04-11 | Fujishoji Co Ltd | 遊技機 |
| CN108695202A (zh) * | 2017-03-31 | 2018-10-23 | 东京毅力科创株式会社 | 处理系统 |
| CN111531568A (zh) * | 2020-05-07 | 2020-08-14 | 吴伟 | 一种用于机械手的联动式机械手指拨片机构 |
| CN112847315A (zh) * | 2020-11-12 | 2021-05-28 | 德阳瑞能电力科技有限公司 | 一种工程施工机器人用机械臂结构 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS60132816A (ja) * | 1983-12-22 | 1985-07-15 | Shinmei Kogyo Kk | 物品搬送装置 |
| JPH01252379A (ja) * | 1988-03-31 | 1989-10-09 | Shin Meiwa Ind Co Ltd | ３次元マニピュレータ |
| JPH0290080U (ja) * | 1988-12-29 | 1990-07-17 | ||
| JPH08119409A (ja) * | 1994-10-27 | 1996-05-14 | Tokyo Electron Ltd | 集合処理装置 |
| JPH08211173A (ja) * | 1995-02-07 | 1996-08-20 | Kazuya Hirose | 多自由度位置決めステージ |
-
1998
- 1998-03-19 JP JP07059498A patent/JP3731846B2/ja not_active Expired - Fee Related
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS60132816A (ja) * | 1983-12-22 | 1985-07-15 | Shinmei Kogyo Kk | 物品搬送装置 |
| JPH01252379A (ja) * | 1988-03-31 | 1989-10-09 | Shin Meiwa Ind Co Ltd | ３次元マニピュレータ |
| JPH0290080U (ja) * | 1988-12-29 | 1990-07-17 | ||
| JPH08119409A (ja) * | 1994-10-27 | 1996-05-14 | Tokyo Electron Ltd | 集合処理装置 |
| JPH08211173A (ja) * | 1995-02-07 | 1996-08-20 | Kazuya Hirose | 多自由度位置決めステージ |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005230918A (ja) * | 2004-02-04 | 2005-09-02 | Janome Sewing Mach Co Ltd | ロボット |
| JP2013063302A (ja) * | 2012-12-05 | 2013-04-11 | Fujishoji Co Ltd | 遊技機 |
| CN108695202A (zh) * | 2017-03-31 | 2018-10-23 | 东京毅力科创株式会社 | 处理系统 |
| JP2018174210A (ja) * | 2017-03-31 | 2018-11-08 | 東京エレクトロン株式会社 | 処理システム |
| CN111531568A (zh) * | 2020-05-07 | 2020-08-14 | 吴伟 | 一种用于机械手的联动式机械手指拨片机构 |
| CN111531568B (zh) * | 2020-05-07 | 2024-01-02 | 吴伟 | 一种用于机械手的联动式机械手指拨片机构 |
| CN112847315A (zh) * | 2020-11-12 | 2021-05-28 | 德阳瑞能电力科技有限公司 | 一种工程施工机器人用机械臂结构 |
| CN112847315B (zh) * | 2020-11-12 | 2024-11-29 | 德阳瑞能电力科技有限公司 | 一种工程施工机器人用机械臂结构 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP3731846B2 (ja) | 2006-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0130570B1 (en) | Method and apparatus for controlling a robot hand along a predetermined path | |
| EP0232424B1 (en) | Industrial robot | |
| US20070030271A1 (en) | Robot controller and robot control method using spline interpolation | |
| Tomizuka et al. | Experimental evaluation of the preview servo scheme for a two-axis positioning system | |
| CN109434873B (zh) | 机器人关节伺服电机转矩常数的测量方法 | |
| EP0146085A2 (en) | Method and apparatus for welding line tracer control | |
| JPH07261853A (ja) | ロボットの振動低減装置 | |
| WO1998010340A1 (en) | Control by means of a set-point generator | |
| JP2010063343A (ja) | モータ制御装置とそのトルクリップル補正方法及びモータ制御システム | |
| JPH11262881A (ja) | リンクアーム機構の制御方法 | |
| JPS63148881A (ja) | サ−ボモ−タの速度制御装置 | |
| JP4196911B2 (ja) | 軌道決定システムおよび軌道決定方法 | |
| JP2737725B2 (ja) | ロボット制御装置及び方法 | |
| JPH07210233A (ja) | ロボットの軌道の補間方法と補間装置 | |
| JPH06138950A (ja) | フィードバック制御方式のロボット制御装置 | |
| JPH04348885A (ja) | ロボットを用いた部品組立作業における嵌合位置検出方法 | |
| JPH0677910B2 (ja) | 産業用ロボツトの制御方法 | |
| JPH0623928B2 (ja) | ロボツトハンドの軌道修正方法 | |
| JP2703099B2 (ja) | 産業用ロボットのコンベア追従方法 | |
| Pears et al. | Guidance of an autonomous guided vehicle using low-level ultrasonic and odometry sensor systems | |
| GB2273177A (en) | Actuator position control. | |
| JPS6010309A (ja) | ロボツトハンドの経路補間方法 | |
| JP2528540B2 (ja) | 指令信号発生方法 | |
| WO1991013388A1 (en) | Improvements in or relating to actuator control | |
| JPH05274040A (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 |