JP2002177352A - 力制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 位置制御からトルク制御に円滑に切換る力制
御装置を得ること。 【解決手段】 モータ５に力指令を発生する力指令部２
０１と、モータ５の回転角度を検出する位置検出器２１
と、モータ３の回転速度を検出する速度検出器８と、押
圧部１０３の力を検出する力検出器１１と、位置検出器
２１の回転角度検出値に基づいて速度指令を生成する速
度指令発生部２０７と、力指令値と力検出器１１の検出
値との差を求める減算器２０３と、減算器２０３からの
入力値に基づいて所定の値を出力すると共に、速度指令
発生部２０７の出力及び位置検出器２１の検出値に基づ
いて出力の上限値又は下限値を変更するリミット部２２
０と、リミット部２２０の出力値と速度検出器８の検出
値との差を求める減算器２２２と、減算器２２２の出力
に基づいてモータ５を駆動する電力増幅器２２４とを備
えている。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、位置制御からト
ルク制御に円滑に切換える力制御装置の改良に関するも
のである。

【０００２】

【従来の技術】従来の力制御装置を特開昭６３−１９１
４５号公報に開示された図７によって説明する。かかる
力制御装置は上肢体などの機能の回復するために用いら
れるリハビリテーションに用いられるものである。図７
において、力制御装置は、駆動部、検出部、制御部から
成り、駆動部は上肢体が取り付けられる端部２を有する
アーム１と、アーム１の基端部３に固定された回転軸４
と、回転軸４を介して減速機７に連結された出力軸６を
有するサーボモータ５とから成っている。

【０００３】検出部は、サーボモータ５の出力軸６の回
転速度を検出する速度検出器８と、アーム１に固定され
ると共に、アーム１に作用する力を検出する力検出器１
１と、アーム１の回転角度により位置を検出する位置検
出器２１とから成っている。

【０００４】制御部は、力検出器１１の検出値を制限す
るリミッタ１４と、位置検出器２１の検出値に対して所
定の値まで出力をゼロとする不感帯を有する不感帯器２
３と、この不感帯器２３の出力値を乗算させる係数器２
５と、係数器２５の出力値とリミッタ１４の出力値との
差分を求める減算器２７と、減算器２７の出力値と速度
検出器８の検出値との差分を求める減算器２９と、減算
器２９の出力値を増幅する電力増幅器３０とから成って
いる。

【０００５】上記のように構成された力制御装置の動作
を図７によって説明する。上肢体がアーム１を駆動する
と、位置検出器２１はアーム１の位置を検出して不感帯
器２３に入力し、不感帯器２３は、不感帯特性に基づい
て出力値θ_rを係数器２５に与え、係数器２５は、出力
値θｒに定数Ｋｅが乗算された値Ｌ_rを減算器２７に入
力する。減算器２７は、リミッタ１４の出力値Ｖ_aと、
係数器２５の出力値Ｌ_rを差し引いた速度指令値Ｖ_rを求
めて減算器２９に入力し、減算器２９は、速度指令値Ｖ
_rと速度検出器８の検出速度値Ｖ_tを差し引いた値を電力
増幅器３０に与えてサーボモータ５を制御する。

【０００６】アーム１の角度位置が、不感帯器２３にお
いて予め設定された値θ_max〜θ_minの可動範囲にある状
態とすると、位置検出器２１の検出値が不感帯器２３に
入力されるが、不感帯特性のために不感帯器２３の出力
値θ_rがゼロとなり、係数器２５の出力値Ｌ_rもゼロとな
る。一方、上肢体によってアーム１に力が作用し、力検
出器１１は検出した力値Ｆ_aをリミッタ１４に入力す
る。リミッタ１４は力値Ｆ_aが小さい場合、力値Ｆ_aと傾
きＫLの積で定まる出力値Ｖ_aを発生する。従って、減算
器２７は、リミッタ１４の出力値Ｖ_aに基づいた速度指
令値Ｖ_rを減算器２９に入力し、減算器２９は速度指令
値Ｖ_rと速度検出器８の検出値Ｖ_tとの差を求めて、電力
増幅器３０は該差に基づいてサーボモータ５を回転して
上肢体のアーム１の回転を補助する。

【０００７】また、上肢体がアーム１の力を増すことに
よりアーム１の角度がθ_maxを超える場合には、力検出
器１１が検出した力値Ｆａがリミッタ１４の＋ＦＬを越
えてリミッタ１４は、制限された値ＶＬ（Ｖ_a）を出力
する。一方、位置検出器２１は検出位置θ_aを不感帯器
２３に入力し、不感帯器２３は検出位置θ_aからθ_maxを
差し引いたθ_eに、大きな傾きＫ_pが乗算された値θ_rを
係数器２５に入力し、係数器２５は定数Ｋ_eを値θ_rに乗
算した値Ｌ_rを減算器２７に入力する。減算器２７は、
値ＶＬから値Ｌ_rを差し引くがＬ_r≫ＶＬであるので、ア
ーム１の力が加わる方向と反対方向の速度指令Ｖ_rを減
算器２９に与えて電力増幅器３０を介してサーボモータ
５をアーム１とは反対方向に回転させる。従って、アー
ム１の位置フィードバック動作がなされ、アーム１は上
下位置θ_{ma x}〜θ_minの可動範囲を大きく越えないように
なる。

【０００８】上肢体がアーム１に力を加えて等速制御を
している時に、アーム１の位置がθ_{ma x,}θ_minになる
と、位置指令Ｌ_rすなわち係数器２５の出力がゼロにな
り、リミッタ１４は上肢体がアーム１に加えた力に応じ
た速度指令を減算器２７、電力増幅器３０に与え、上肢
体がアーム１を回転していた方向にサーボモータ５を回
転する。したがって、アーム１の急激な停止が防止で
き、衝撃力が上肢体に加わることを防止できるものであ
る。

【０００９】

【発明が解決しようとする課題】しかしながら、上記の
ように構成された力制御装置は、位置制御で動作する範
囲が不感帯器２３の特性によって定まる。すなわち、角
度位置θ_max,θ_minを越えないと、位置制御がされず、
角度位置θ_max,θ_minでは、等速制御（トルク制御）に
なり、位置制御がされない。よって、このような力制御
装置は、所定の位置までは、トルク制御を実行し、所定
の位置に達したら位置制御を実行するものである。この
ため、所定の位置までは、位置制御を実行し、所定の位
置に達したらトルク制御を実行する力制御装置、例え
ば、台の上に載せられている金物を、所定の距離離れて
いるアームにより、アームを位置制御により移動して、
トルク制御により所定量押圧する切替えが円滑にできな
いという問題点があった。

【００１０】この発明は、上記のような課題を解決する
ためになされたもので、位置制御からトルク制御に円滑
に切換る力制御装置を提供することを目的としている。

【００１１】

【課題を解決するための手段及び発明の作用、効果】第
１の発明に係る力制御装置は、モータにより駆動される
押圧部を位置制御からトルク制御に連続的に移行させる
力制御装置であって、上記モータに力指令を発生する力
指令発生手段と、上記モータの回転角度を検出する位置
検出手段と、上記モータの回転速度を検出する速度検出
手段と、上記押圧部の力を検出する力検出手段と、上記
位置検出手段の回転角度検出値に基づいて速度指令を生
成する速度指令生成手段と、上記力指令値と上記力検出
手段の検出値との差を求める第１の減算手段と、該第１
の減算手段からの入力値に基づいて、所定の値を出力す
ると共に、上記速度指令生成手段の出力及び上記角度検
出手段の検出値に基づいて上記出力の上限値又は下限値
を変更するリミット手段と、該リミット手段の出力値と
上記速度検出手段の検出値との差を求める第２の減算手
段と、該第２の減算手段の上記差に基づいて上記モータ
を駆動する制御手段とを備えたことを特徴とするもので
ある。かかる解決手段によれば、位置制御の場合には、
リミッタ手段により速度指令生成手段の指令速度で動作
し、力検出手段からの力指令に近づいた場合には、力指
令発生手段からの力指令に自動的に切替わるので、位置
制御からトルク制御に衝撃なく、切替えることができ
る。従って、例えば、台の上に載せられている金物を、
所定の距離離れているアームにより、アームを位置制御
により移動して、トルク制御により所定量押圧する切替
えが自動的に円滑にできるという効果がある。

【００１２】第２の発明に係る力制御装置は、第１の発
明におけるリミット手段が上限値又は下限値を速度指令
生成手段の出力値に基づいて連続して変更する、ことを
特徴とするものである。かかる解決手段によれば、位置
制御の場合には、リミッタ手段により速度指令生成手段
の指令速度に基づいてリミッタ値を連続して動作させる
ので、より一層、位置制御からトルク制御に衝撃なく、
切替えることができるという効果がある。

【００１３】第３の発明に係る力制御装置は第１又は第
２の発明において、押圧部の停止位置と、上記押圧部の
停止開始を定める停止開始位置とを記憶する記憶手段
と、上記角度検出手段の検出値に基づいて上記停止開始
位置を越えたと判断する判断手段と、この判断手段によ
り上記停止開始位置を越えたと判断した場合、上記停止
位置に上記モータが停止させるように上記モータの回転
速度を制限する速度制限手段とを備えたことを特徴とす
るものである。かかる解決手段によれば、判断手段は、
角度検出手段に基づいて停止開始位置を越えたと判断す
ると、速度制限手段は停止位置にモータが停止させるよ
うに上記モータの回転速度を制限する。従って、トルク
制御を行っていても停止位置を超えないようにモータ停
止させることができるという効果がある。

【００１４】

【発明の実施の形態】実施の形態１．この発明の一実施
の形態を図１及び図２によって説明する。図１は、この
発明の一実施の形態を示す力制御装置の全体ブロック
図、図２は速度指令発生部の記憶部のメモリマップ図、
図３は力制御装置の各部の動作を示すタイムチャートで
ある。図１中、従来と同一符号は同一又は相当部分を示
し、説明を省略する。図１及び図２において、力制御装
置は、柱状の台１０５の上に載せられている金物１０７
を、所定の距離離れているアーム１を位置制御によって
移動し、トルク制御に切替えて金物１０７をアーム１の
押圧部１０３によって所定量押圧するように構成されて
いる。

【００１５】力制御装置は、検出手段を有する駆動部１
００、制御部２００から成り、駆動部１００はアーム１
の先端部に設けられた押圧部１０３と、アーム１の基端
部３が固定された回転軸４と、回転軸４に連結された減
速機７を介して出力軸６を有するサーボモータ５とから
成っている。

【００１６】検出手段は、サーボモータ５の出力軸６の
回転速度を検出すると共に、該回転速度に比例した出力
特性８ａを有する速度検出器８と、アーム１の押圧部１
０３に作用する力を検出すると共に、該力に比例した出
力特性１１ａを有する力検出器１１と、アーム１の回転
角度により位置を検出する位置検出器２１とから成って
いる。

【００１７】制御部２００は、サーボモータ５の回転位
置にかかわらず一定の力指令値Ｆ_rを出力する力指令部
２０１と、力検出器１１の検出値と力指令部２０１の力
指令値との差を求める減算器２０３と、減算器２０３の
出力値を増幅する増幅器２０５と、位置検出器２１によ
り検出された位置に基づいて所定の速度指令を生成する
速度指令発生部２０７と、速度指令発生部２０７の指令
値及びアーム１の押圧部１０３の位置に基づいて出力値
を二段階ＶＬ₁，ＶＬ₂に制限するリミット部２２０と、
リミット部２２０の出力値と速度検出器８の検出値との
差を求める減算器２２２と、減算器２２２の出力値を増
幅すると共に、モータ５を駆動制御する電力増幅器２２
４とを備えている。

【００１８】速度指令発生部２０７は、押圧部１０３の
押圧面から金物１０７までの距離に対して図３に示すよ
うなアーム１が回転する速度指令を発生するものであっ
て、特に、アーム１の押圧部１０３の押圧面が金物１０
７と僅かに離れた距離Ｌ₁から速度Ｖ_{r β}で減速して低速
度Ｖ_r2に達すると一定低速Ｖ_r2により押圧部１０３の先
端が移動し、押圧部１０３が金物１０７を押圧して該押
圧量がＬ_Pに達すると停止指令を発生させるように形成
されており、位置検出器２１により検出された検出信号
を入力する入力インターフェース（以下、インターフェ
ースをＩ／Ｆという）２０９と、周知のＣＰＵ２１１
と、処理手順のプログラム、及び設定データなどが記憶
された記憶部２１３と、デジタル出力信号をアナログ信
号に変換するＤ／Ａ変換器２１５と、Ｄ／Ａ変換器２１
５の出力に接続された出力Ｉ／Ｆ２１５とから成ってい
る。

【００１９】記憶部２１３は、第一速度指令Ｖ_r1が記憶
された第１速度領域２１３ａと、第二速度指令Ｖ_r2とし
てのクリープ速度が記憶された第２速度領域２１３ｂ
と、第１速度指令Ｖ_r1から第２速度指令Ｖ_r2への切替え
位置Ｌ₁が記憶された切替え位置領域２１３ｃと、スタ
ートから速度指令Ｖ_r1になるまでの加速度αが記憶され
た加速度領域２１３ｄと、速度指令Ｖ_r1から速度指令Ｖ
_r2になるまでの減速度が記憶された減速度領域２１３ｅ
とを有している。

【００２０】リミッタ部２００は、増幅器２０５からの
入力値Ｆ_iとして速度ＶＬを出力とする図１に示すよう
な入出特性を有し、アーム１の押圧部１０３を位置検出
器２１により検出した検出位置Ｌ_a＜切替え位置Ｌ₁で、
且つ速度指令Ｖ_r≧Ｖ_r1では入力値がゼロからＦ₁まで
は、速度値ＶＬ₁の限度において、減算器２０５からの
入力値Ｆ_iに比例した値を出力すると共に、速度指令Ｖ
ｒの値の大きさに比例して連続的に上限値及び下限値が
制限されるように形成されている。さらに、位置検出器
２１の検出位置Ｌ_a＞切替え位置Ｌ₁を越え、且つ速度指
令Ｖ _r≧Ｖ_r2では、入力値がゼロからＦ₂までは、速度値
ＶＬ₂の限度において増幅器２０５からの入力値Ｆ_iに比
例した値を出力すると共に、速度指令Ｖ_rの値の大きさ
に比例して連続的に上限値及び下限値が制限されるよう
に形成されている。

【００２１】そして、増幅器２０５は入力値Ｆ_rにゲイ
ンＧを乗算させた出力値Ｆ_iを、リミッタ部２２０の入
力値Ｆ₁，Ｆ₂よりも大きくなるように形成されている
（式で表現すればＦ_i＝Ｆ_r×Ｇ＞Ｆ₂＞Ｆ₁）。これは、
位置制御、すなわち、力検出器１１の検出値がゼロで、
力指令部２０１が力指令値Ｆ_rの場合、リミッタ部２２
０は、出力がＶＬ₁又はＶＬ₂の制限となり、力指令によ
る制御をすることなく、位置指令に基づく速度指令によ
ってサーボモータ５を動作させるためである。

【００２２】上記のように構成された力制御装置を図１
乃至図４によって説明する。図４は速度指令発生部の動
作を示すフローチャートである。図１に示すように台１
０５に載せられた金物１０７と、押圧部１０３の先端と
が所定の距離隔てられた状態において、時間ｔ_sで、押
圧部１０３のスタート指令が制御部２００に入力される
と、力指令部２０１は一定の力指令Ｆ_rを出力するが、
押圧部１０３には、力が付与されていないので、力検出
器１１の検出値がゼロとなり、減算器２０３は、力指令
Ｆ_rと該検出値との偏差を求め、増幅器２０５は、偏差
値を増幅してリミッタ部２２０に入力する。

【００２３】一方、速度指令発生部２０７のＣＰＵ２１
１は、スタート指令が入力されたか否かにより動作開始
を判断し（ステップＳ１０１）、動作開始していれば、
位置検出器２１から検出した位置Ｌ_aを入力Ｉ／Ｆ２０
９を介して読み込みと共に、前回位置に変化量となるΔ
距離との和を今回の位置として求め（ステップＳ１０
３）、今回位置と切替え開始位置Ｌ₁とを比較し（ステ
ップＳ１０５）、アーム１の押圧部１０３が移動しだし
てから間もないので、今回位置＜切替え位置Ｌ₁とな
る。

【００２４】従って、ＣＰＵ２１１は、前回の速度指令
Ｖ_rに加速度αに演算周期ｔ_rを乗算したαｔ_rを加算し
て、今回の速度指令Ｖ_rを求め（ステップＳ１０７）、
該速度指令Ｖ_rが速度指令Ｖ_r1を越えているか否かを判
断し（ステップＳ１０９）、アーム１の加速当初は、速
度指令Ｖ_r＜Ｖ_r1であるので、速度指令Ｖ_rを出力Ｉ／Ｆ
２１７を介して出力する。

【００２５】一方、減算器２０３は、力指令部２０１の
指令値Ｆ_rから力検出器１１の検出値ゼロを減算した値
Ｆ_rを増幅器２０５に入力し、増幅器２０５は、該値Ｆ_r
にゲインＧを乗算してＦ_iを求め、リミッタ部２２０に
入力する。リミッタ部２２０は、位置検出器２１の検出
位置Ｌ_aと切替え位置Ｌ₁との関係がＬ_a＜Ｌ₁、入力値Ｆ
_i＞Ｆ₁であるので、制限値ＶＬ₁の範囲において、速度
指令発生部２０７の速度指令Ｖｒの値に応じた制限値Ｖ
Ｌを出力し、減算器２２２は、速度検出器８から検出さ
れたサーボモータ５の回転速度値Ｖ_aと、リミッタ部２
２０の出力値ＶＬとの偏差を求め、電力増幅器２２４は
該偏差を増幅してサーボモータ５を駆動制御してアーム
１を回転させる。すなわち、アーム１が図３に示すよう
に速度Ｖ_{r α}で加速している段階では、リミット部２２
０は、力指令部２０１の力指令Ｆ_rにより制限値ＶＬ₁の
範囲において、速度指令発生部２０７からの位置検出器
２１の検出指令に基づく速度指令Ｖ _rに基づいてサーボ
モータ５を制御しているので、アーム１は位置制御を実
行しているものである。

【００２６】やがて、アーム１の押圧部１０３の速度が
上昇してステップＳ１０９において、Ｖ_r＞Ｖ_r1が成立
すると、速度指令発生部２０７の速度指令Ｖ_{r 1}にして出
力Ｉ／Ｆ２１７を介してリミッタ部２２０に入力する
（ステップＳ１１１）。さらに、アーム１が移動し、押
圧部１０３が金物１０７の近傍となる切替え位置Ｌ₁を
超えると、ステップＳ１０５において今回位置＞切替え
位置Ｌ₁が成立し、ＣＰＵ２１１は、前回速度指令から
減速度βに演算周期ｔ_rを乗算したβｔ_rを減算して今回
速度指令を求め（ステップＳ１１９）、速度指令Ｖ
_rを、予め定められたクリープ速度Ｖ_{r 2}と比較し（ステ
ップＳ１２１）、速度指令Ｖ_rがクリープ速度Ｖ_{r 2}より
も高ければ、Ｄ／Ａ変換器２１５によりデジタル指令を
アナログ指令に変換し、出力Ｉ／Ｆ２１７を介してリミ
ッタ部２２０に入力する。やがて、アーム１の押圧部１
０３の速度がＶ_r2に達すると、速度指令発生部２０７
は、Ｖ_r2をリミッタ部２２０に入力する（ステップＳ１
２３）。

【００２７】切替え位置Ｌ₁を超えると、リミッタ部２
２０の最高制限出力は、ＶＬ₁からＶＬ ₂となり、やが
て、アーム１の先端の押圧部１０３が金物１０７に当接
し始める位置Ｌ₀に達すると、力検出器１１の検出した
力値Ｆ_aが増加し、減算器２０３は、力指令Ｆ_rと該力値
Ｆ_aとの偏差を求め、この偏差が減少し、増幅器２０５
は、Ｆ₂よりも小さな値Ｆ_iをリミッタ部２２０に入力す
る。したがって、リミッタ部２２０は、入力値Ｆ_iに比
例した出力値ＶＬを出力し、減算器２２２は、速度検出
器８の検出値Ｖ_aと該出力値ＶＬとの偏差を求め、電力
増幅器２２４は、該偏差に基づいてサーボモータ５を駆
動する。すなわち、アーム１の押圧部１０３が金物１０
７を押圧する段階では、力指令部２０１の力指令値Ｆ_r
と力検出器１１の検出値Ｆ_aとの差Ｆ_iに基づいてリミッ
ト部２２０は、制限値ＶＬ₁内において出力ＶＬを発生
する、トルク制御を実行している。

【００２８】やがて、力指令部２０１の力指令値Ｆ
_iと、力検出器１１の検出値Ｆ_aとが一致した時に、減算
器２０３の出力がゼロになり、リミッタ部２２０の出力
もゼロとなりサーボモータ５の回転も停止し、アーム１
の押圧部１０３も位置Ｌ_pで停止する。

【００２９】実施の形態２．この発明の他の実施の形態
を図５よって説明する。図５は、この発明の他の実施の
形態を示す力制御装置のブロック図である。図５中、図
１と同一符号は同一又は相当部分を示し説明を省略す
る。この実施の形態では力制御装置において、トルク制
御を実行しても、アーム１が回転移動して停止開始位置
を越えたならば、停止位置に止めるためにアーム１の回
転速度を制限させるものである。そして、図５におい
て、アーム１が半時計方向に回転する方向を前進とい
い、前進方向の移動でアーム１を所定の停止位置に停止
させる例を以下に説明する。

【００３０】図５において、ストロークリミット部３０
０は、アーム１の前進における停止開始位置に基づいて
サーボモータ５に制限速度指令を発生するもので、速度
検出器８の検出信号、リミッタ部２２０の出力信号、位
置検出器２１の検出信号を入力する入力Ｉ／Ｆ３０９
と、周知のＣＰＵ３１１と、処理手順のプログラム、及
び設定データが記憶された記憶部３１３と、デジタル出
力信号をアナログ信号に変換するＤ／Ａ変換器３１５
と、Ｄ／Ａ変換器３１５の出力に接続された出力Ｉ／Ｆ
３１７とを有している。

【００３１】記憶部３１３は、アーム１の停止位置Ｌ_s
が記憶された停止位置領域３１３ａと、アーム１が減速
を開始する開始位置Ｌ_fが記憶された開始位置領域３１
３ｂと、アーム１の減速開始時の最高速度Ｖ_maxが記憶
された最高速度領域３１３ｃと、アーム１が最高速度か
ら停止するまでの最大減速時間を記憶した最大減速時間
領域３１３ｄとから成っている。

【００３２】いま、記憶部３１３には、開始位置Ｌ
_f（ｍ）、停止位置Ｌ_s（ｍ）、最高速度Ｖ_max（ｍ／
ｓ）、最大減速時間ｔ_{β max}（ｓ）の実測値が各記憶領
域に記憶されており、アーム１が停止の開始位置Ｌ_fを
越えると、アーム１の回転速度が制限される制限速度Ｖ
_rL（ｍ／ｓ）を下記のようにして求める。まず、アーム
１を停止位置Ｌ_sに停止させるための必要減速距離Ｌ
_e（ｍ）すると下式が成立する。 Ｌ_f＝Ｌ_s−Ｌ_e・・・・・（１） また、現在速度Ｖ_p（ｍ／ｓ）、現在速度Ｖ_pから停止す
るまでの減速時間ｔ_{β r}（ｓ）とすると必要減速距離Ｌ_e
は下式となる。 Ｌ_e＝Ｖ_p・ｔ_{β r}／２・・・・（２）

【００３３】一方、減速時間ｔ_{β r}を最大減速時間ｔ_β
max、最高速度Ｖ_maxとの関係は下式となる。 ｔ_{β r}＝ｔ_{β max}・（Ｖ_p／Ｖ_max）・・・・（３） （３）式の減速時間ｔ_{β r}を（２）式に代入して整理す
ると必要減速距離Ｌ_eは下式となる。 Ｌ_e＝Ｖ_p ²・ｔ_{β max}／（２Ｖ_max）・・・・（４） したがって、停止開始位置Ｌ_fは下式となる。 Ｌ_f＝Ｌ_s−Ｖ_p ²・ｔ_{β max}／（２Ｖ_max）・・・・・（５）

【００３４】アーム１が半時計方向に回転して停止の開
始位置を越えたならば、停止位置にアーム１を停止させ
るための制限速度Ｖ_rLを前記（５）を変形して下式によ
り求める。 Ｖ_{r L}＝｛（２Ｌ_e・Ｖ_max）／ｔ_{β max}｝｝^1/2・・・（６） すなわち、ストロークリミッタ部３００のＣＰＵ３１１
は、位置検出器２１からの検出位置Ｌ_aを入力Ｉ／Ｆ３
０９を介して読み込み、記憶部３１３に設定された停止
の開始位置Ｌ_fと比較して、開始位置Ｌ_fが越えた場合に
は、アーム１の制限速度Ｖ_rLを前記（６）式により求
め、Ｄ／Ａ変換器３１５を介して出力Ｉ／Ｆ３０９から
制限速度指令Ｖ_rLを減算器２２２に入力してサーボモー
タ５を駆動する。

【００３５】なお、ストロークリミッタ部３００は、ア
ーム１が停止の開始位置Ｌ_fを越えるまでは、単なるバ
ッファ回路として動作する。また、前記実施の形態は、
アーム１を半時計方向に回転させる前進について説明し
たが、アーム１を時計方向に回転させる後退についても
前記実施の形態と同様の考え方に基づいて速度を制限す
れば良い。

【図面の簡単な説明】

【図１】 この発明の一実施の形態を示す力制御装置の
全体構成を示すブロック図である。

【図２】 図１に示す速度指令発生部の記憶部のメモリ
マップ図である。

【図３】 図１に示す力制御装置の各部の動作を示すタ
イムチャートである。

【図４】 図１に示す速度指令発生部の動作を示すフロ
ーチャートである。

【図５】 この発明の他の実施の形態を示す力制御装置
の全体構成を示すブロック図である。

【図６】 図５に示す速度指令発生部の記憶部のメモリ
マップ図である。

【図７】 従来の力制御装置を示す全体ブロック図であ
る。

【符号の説明】

５ サーボモータ、８ 速度検出器（速度検出手段）、
１１ 力検出器（力検出手段）、２１ 位置検出器（角
度検出手段）、２０１ 力指令部（力指令発生手段）、
２０３ 第１の減算手段（減算器）、２０７ 速度指令
発生部（速度指令生成手段）、２２０ リミッタ部（リ
ミット手段）、２２２ 第２の減算手段（減算器）、２
２４ 電力増幅器（制御手段）、３００ ストロークリ
ミッタ部（速度制限手段）、３１３ 記憶部（記憶手
段）。

Claims (3)

【特許請求の範囲】
1. 【請求項１】 モータにより駆動される押圧部を位置制
   御からトルク制御に連続的に移行させる力制御装置であ
   って、上記モータに力指令を発生する力指令発生手段
   と、上記モータの回転角度を検出する位置検出手段と、
   上記モータの回転速度を検出する速度検出手段と、上記
   押圧部の力を検出する力検出手段と、上記位置検出手段
   の回転角度検出値に基づいて速度指令を生成する速度指
   令生成手段と、上記力指令値と上記力検出手段の検出値
   との差を求める第１の減算手段と、該第１の減算手段か
   らの入力値に基づいて、所定の値を出力すると共に、上
   記速度指令生成手段の出力及び上記角度検出手段の検出
   値に基づいて上記出力の上限値又は下限値を変更するリ
   ミット手段と、該リミット手段の出力値と上記速度検出
   手段の検出値との差を求める第２の減算手段と、 該第２の減算手段の上記差に基づいて上記モータを駆動
   する制御手段と、を備えたことを特徴とする力制御装
   置。
2. 【請求項２】 上記リミット手段は、上記上限値又は下
   限値を上記速度指令生成手段の出力値に基づいて連続し
   て変更する、 ことを特徴とする請求項１に記載の力制御装置。
3. 【請求項３】 上記押圧部の停止位置と、上記押圧部の
   停止開始を定める停止開始位置とを記憶する記憶手段
   と、上記角度検出手段の検出値に基づいて上記停止開始
   位置を越えたと判断する判断手段と、 この判断手段により上記停止開始位置を越えたと判断し
   た場合、上記停止位置に上記モータが停止させるように
   上記モータの回転速度を制限する速度制限手段と、を備
   えたことを特徴とする請求項１又は２に記載の力制御装
   置。