JPH077005B2 - 物品移送装置の速度検出装置 - Google Patents

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、物品把持部を移動させて、物品を移送する物
品移送装置において、駆動機構部の移送速度を高精度・
低価格に検出するための速度検出装置に関するものであ
る。

（従来の技術） 従来この種物品移送装置の物品移送は、第２図に示した
速度制御装置を用いて行なわれていた。第２図におい
て、30は移送命令信号線、31は位置検出器、32は規準速
度発生器、33は速度比較器、34は移送機構、35は速度検
出器である。

移送命令が移送命令信号線30を介して上位装置から規準
発生器32に伝達されると、移送距離に対応した速度の規
準値が速度比較器33に入力される。この時、移送機構34
は停止しているので、その速度検出器35は零である。し
たがって、速度比較器33は速度の規準値に等しい出力を
移送機構34に印加し、移送機構34を加速する。移動を開
始した移送機構34の速度は速度検出器35によって検出さ
れ、再び、速度比較器33で速度の規準値を比較され、過
不足に応じて、加減速力が移送機構34に伝達され、移送
速度を規準速度に追従するよに制御する。一方、速度の
規準値は目標位置に近づくに従って減少するように規準
速度発生器32から発生されているので、この規準値に追
従している移送機構34の移送速度は、目標位置に近づく
につれて低速度に減速され、目標位置では零になる。こ
のような速度制御においては、目標位置での停止精度は
速度検出精度に依存するので、停止位置精度を高めるた
めには高精度速度検出装置の開発が重要である。従来の
速度検出装置としては以下の２種類の装置が知られてい
る。

モータ軸に取付けた発電機によりモータの回転速度
（以下速度）に比例した速度信号を得る機械的装置。

磁気ディスク装置のポジショナ速度検出等に用いられ
ている装置で、モータ電流の実測値の積分と位置信号の
微分とを合成して速度信号を得る電気的装置。

（発明が解決しようとする問題点） 機械的装置では、モータ軸にセンサが直接取付けられて
いるので、摩擦力等によってモータ速度は変動しても正
確に検出できる。しかし、検出器に金物（ハードウエ
ア）を用いているので、検出器コストおよび実装量が増
加するという欠点があった。

一方、電気的装置では電気回路が持つ演算機能を利用し
て速度を検出するので、検出器コストや実装量を削減で
きる。ところが、位置変化が緩やかな速度領域では、そ
の微分値が小さくなるので、モータ電流の積分値が速度
信号の主成分になる。ところがこのような低速領域では
摩擦力によりモータ発生トルクが削減され、モータ電流
のうち、摩擦力に打ち勝って実際にモータの回転に寄与
する電流は削減するので、モータ電流の積分値から求め
た速度は実際の速度より大きくなり、速度の検出誤差が
発生する。また、実際の系では、電力増幅器、積分器利
得等の電気回路特性が変動するので、駆動信号に対する
実際のモータ電流は変動する。そこで、電気的方法を適
用して速度を検出する場合、電気回路特性の変動は速度
検出精度におよぼす影響を除去するためにモータ電流を
実測する必要がある。また、最近では制御回路をマイク
ロプロセッサで構成して、回路コストの削減と実装量の
削減が図られている。このようなディジタル信号制御に
おいては、検出信号は離散化する必要があり、このた
め、電流実測にアナログ・ディジタル変換器（ADC）が
用いられていた。ところが、この種当業者には良く知ら
れているように、ADCのコストは変換速度と変換精度に
比例するので、速度検出を高速・高精度に行うためには
高価なADCが必要となり、検出回路が高価になるという
欠点があった。

（発明の目的） 本発明の目的は、高価なADCを用いず、かつ速度が遅い
領域での検出精度を高めることができる低価格・高精度
な物品移送装置の速度検出装置を提供することにある。

（問題点を解決するための手段） 本発明は上記の目的を達成するため、物品を把持する物
品把持部と、該物品把持部を上下又は左右に移動させる
ための駆動機構と、該物品把持部の位置を検出する位置
検出器とを有する物品移送装置において、駆動信号と速
度推定値とから１サンプリング時刻後の速度予測値を計
算する手段１、該予測値を累積する手段、駆動信号、速
度推定値および位置推定値とから１サンプリング時刻後
の位置予測値を計算する手段、該予測値を累積する手
段、サンプリング時間を測定し１サンプリング時刻経過
後、モータ位置を測定する上記位置センサで測定した測
定位置と該位置予測値とから位置予測誤差を計算する手
段、該位置予測誤差で該速度予測値を修正する手段から
なることを特徴とする。

（作用） 本発明によれば、定電流増幅器への駆動信号と、速度推
定値とから１サンプリング時刻後の速度予測値を計算，
累積し、駆動信号、速度推定値および位置推定値とから
１サンプリング後の位置予測値を計算，累積し、１サン
プリング時刻後に実測した位置とこの位置の予測値とか
ら予測誤差を計算し、この予測誤差で速度の予測値を修
正する。

（実施例） 第１図は本発明の実施例を説明する図であって、１は駆
動信号入力線、２は第１の係数器（係数1/2）、３は第
１の加算値切り換え器、４は第１の加算器、５は速度累
積器、６は第２の加算値切り換え器、７は第２の加算
器、８は位置累積器、９は第３の加算器、10は位置比較
器、11は第２の係数器（係数２）、12は推定速度信号
線、13は推定位置信号線、14は測定位置信号線、15は定
電流増幅器、16は直流モータ、17は回転角検出器、18は
アップ・ダウンカウンタ、19は零次ホールド回路、20は
加算値切り換え信号発生器、21は物品把持部、22はワイ
ヤロープ、23はドライブプーリ、24はアイドラプーリで
ある。

ワイヤロープ22に直結された物品把持部21はモータ16に
直結されたドライブプーリ23によって摩擦駆動される。
このような機構構成からなる駆動機構においては、物品
把持部21の位置信号はドライブプーリ23に直結された回
転角検出器17とアップ・ダウンカウンタ18を組み合わせ
て検出される。零次ホールド回路19で出力された駆動信
号は定電流増幅器15で増幅され、モータ16を加速する。
ドライブプーリ23の半径を１、駆動機構に作用する摩擦
力を無視すると、このモータ16で加速された物品把持部
21の速度Vp（ｋ）、位置Pp（ｋ）の予測値は次式で表す
ことができる。

Vp（ｋ）＝Ka×（Kt/J）×Ｔ×Ｉ（ｋ−１）＋Ve（ｋ−
１） ……（１） Pp（ｋ）＝Ka×（Kt/J）×T²×Ｉ（ｋ−１）/2＋Ve（ｋ
−１）×Ｔ＋Pe（ｋ−１） ……（２） ここで、 T:サンプリング時間、Ｉ（ｋ−１）：時刻ｋ−１でのモ
ータ電流、 Kt:モータトルク定数、 J:駆動機構の慣性負荷、 Ka;モータトルク定数、Ve（ｋ−１）,Pe（ｋ−１）：時
刻ｋ−１での速度、位置の推定値 である。

式（１）（２）において、Ka,Kt,J,Tが時間的に変化し
ない定係数の物品移送装置においては、式（１）をKa×
（Kt/J）×Ｔ、式（２）をKa×（Kt/J）×T²/2で正規化
し、それぞれを正規化速度Vo（ｋ）、正規化位置Po
（ｋ）と呼ぶと式（１）（２）は次式になる。

Vo（ｋ）＝Ｉ（ｋ−１）＋Voe（ｋ−１） ……（３） Po（ｋ）＝Ｉ（ｋ−１）/2＋Voe（ｋ−１）×Ｔ＋Poe
（ｋ−１） ……（４） 一方、時刻ｋでの実測位置をPa（ｋ）とすると、位置の
予測誤差εＰ（ｋ）はPa（ｋ）−Po（ｋ）となる。ここ
で、定電流駆動されたモータの速度と位置の関係は Ｖ×T/2＝Ｐ …（５） であることを考慮にいれると、位置の予測誤差がεＰの
場合の速度の予測誤差εＶは式（５）から次式のように
求まる。

εＶ＝２×εP/T …（６） したがって、求める時刻ｋでの速度推定値は、予測値を
予測誤差で修正することにより次式になる。

Voe（ｋ）＝Ｉ（ｋ−１）＋Voe（ｋ−１）＋２×εP/T
…（７） また、位置推定値は、 Poe（ｋ）＝Ｉ（ｋ−１）/2＋Voe（ｋ−１）×Ｔ＋Poe
（ｋ−１）＋εＰ …（８） となる。（以後の説明では記述の簡素化のため、係数oe
を省略する。） 本発明では、以上の速度推定計算を第１図の実施例で実
施する。すなわち、第１の加算値切り換え器３は、時刻
ｋ−１では加算切り換え信号発生器20によって駆動信号
線１側3aに切り換えられているので、第１の加算器４で
は駆動信号Ｉ（ｋ−１）と速度推定値が累積されている
速度累積器５の出力Ｖ（ｋ−１）が加算され、式（３）
に相当する時刻ｋにおける速度予測値が計算され、次ぎ
に速度累積器５に蓄積される。これと同時に第２の加算
値切り換え器６は、速度累積器５の出力Ｖ（ｋ−１）と
駆動信号Ｉ（ｋ−１）の1/2の値との和を加算する第３
の加算器９の出力側6aに切り換えらえているので、第２
の加算器７では、この和信号と時刻ｋ−１の位置推定値
が累積されている位置累積値８の出力Ｐ（ｋ−１）が加
算され、式（４）に相当する時刻ｋにおける位置予測値
が計算され、次に位置累積器８に蓄積される。

１サンプリング時間経過した時刻ｋでは、加算値切り換
え信号発生器20が第1,第２の加算値切り換え器3,6をそ
れぞれ3b,6b側に切り換えると同時に位置を実測する。
つぎに位置比較器10では信号線14を介して伝達された位
置の実測値と信号線13を介して伝達された前述の位置予
測値から位置の予測誤差を計算する。この予測誤差は速
度、位置の各予測値を修正するため、第２の加算値切り
換え器6b側に伝達されると同時に、係数器11で振幅を２
倍に増幅されて第１の加算値切り換え器３の3b側に伝達
される。この結果、時刻ｋにおいて計算された各予測値
が加算器4,7で位置の予測誤差を用いて修正計算され、
続いて、式（７）（８）に示した速度推定値および位置
推定値がそれぞれ速度累積器５、位置累積器８に蓄積さ
れる。

以上の推定計算が完了すると、加算値切り換え信号発生
器20が再び第1,第２の加算値切り換え器3,6をそれぞれ3
a,6a側に切り換え、再び上述した方法で時刻ｋ＋１にお
ける速度および位置の予測値を計算，蓄積する。

このように、本実施例では、モータ電流の実測値でなく
定電流増幅器への駆動信号から１サンプリング時刻後の
速度位置の予測値を計算し，次サンプリング時刻におい
て、位置の予測値と実測値とから予測誤差を計算し、こ
の予測誤差で予測値を修正して、速度の推定値を得るの
で、モータ電流を直接測定する高価なADCが不用にな
る。また、速度の予測値は毎サンプリング時刻ごとに予
測誤差で修正されるので、位置変化がゆるやかな領域に
おいても正確に速度を検出できる。

（発明の効果） 以上説明したように、本発明によれば定電流増幅器への
駆動信号から１サンプリング時刻後の速度、位置の予測
値を計算し、次サンプリング時刻において、位置の予測
値と実測値とから予測誤差を計算し、この予測誤差で予
測値を修正して、速度の推定値を得るので、モータ電流
を直接測定する高価なADCが不用となる。また、速度の
予測値は毎サンプリング時刻ごとに予測誤差で修正され
るので、位置変化がゆるやかな領域でも正確に速度を検
出できる。

【図面の簡単な説明】

第１図は本発明装置の実施例を説明するブロック図、第
２図は従来の速度制御装置のブロック図である。 １……駆動信号入力線、２……第１の係数器（係数1/
2）、３……第１の加算値切り換え器、４…第１の加算
器、５……速度累積器、６……第２の加算値切り換え
器、７……第２の加算器、８……位置累積器、９……第
３の加算器、10……位置比較器、11……第２の計数器
（計数２）、12……推定速度信号線、13……推定位置信
号線、14……測定位置信号線、15……定電流増幅器、16
……直流モータ、17……回転角検出器、18……アップ・
ダウンカウンタ、19……零次ホールド回路、20……加算
値切り換え信号発生器、21……物品把持部、22……ワイ
ヤロープ、23……ドライブプーリ、24……アイドラプー
リ。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 大井 建夫 神奈川県川崎市中原区上小田中1015番地 富士通株式会社内 (72)発明者 吉副 照秀 神奈川県川崎市中原区上小田中1015番地 富士通株式会社内

Claims (1)

【特許請求の範囲】
1. 【請求項１】物品を把持する物品把持部と、該物品把持
   部を上下又は左右に移動させるための、定電流増幅器及
   びモータを含む駆動機構と、該物品把持部の位置をモー
   タ位置を測定することにより検出する位置検出器とを有
   する物品移送装置において、上記定電流増幅器への駆動
   信号と速度推定値とから１サンプリング時刻後の速度予
   測値を計算する手段、該速度予測値を累積する手段、上
   記駆動信号、速度推定値および位置推定値とから１サン
   プリング時刻後の位置予測値を計算する手段、該位置予
   測値を累積する手段、サンプリング時間を測定し１サン
   プリング時刻経過後、上記位置検出器で測定した測定位
   置と該位置予測値とから位置予測誤差を計算する手段、
   該位置予測誤差で該速度予測値を修正する手段からなる
   ことを特徴とする物品移送装置の速度検出装置。