(57)【特許請求の範囲】
【請求項１】
モータにより駆動される押圧部をワークに対して接近させた後該押圧部をワークに対し押圧させる力制御装置であって、
上記モータに一定値の力指令値を発生する力指令発生手段と、
上記モータの回転角度を検出する位置検出手段と、
上記モータの回転速度を検出する速度検出手段と、
上記押圧部に作用する力を検出する力検出手段と、
上記力指令値と上記力検出手段の検出値との差を求め、該差を増幅して力偏差として出力する第１の減算手段と、
上記位置検出手段の回転角度検出値に基づいて、移動開始から所定の加速度で加速し、第１の速度で等速移動し、所定の切換え位置から所定の減速度で減速し、第１の速度より小さな第２の速度で等速移動する速度パターンに従った速度指令を生成する速度指令生成手段と、
前記検出回転角度が前記切換え位置に到達していない場合でかつ前記力偏差が第１の値Ｆ１より大きいときは第１の制限値ＶＬ１を上限として速度指令に比例した値を出力し、前記検出回転角度が前記切換え位置を越えた場合は、前記力偏差が第１の値Ｆ１より小さい第２の値Ｆ２以下のときは、第１の制限値ＶＬ１より小さい第２の制限値ＶＬ２を上限として力偏差に比例した値を出力すると共に、前記力偏差が第２の値Ｆ２より大きいときは第２の制限値ＶＬ２を上限として速度指令に比例した値を出力する入出力特性が設定されたリミット手段と、
該リミット手段の出力値と上記速度検出手段の検出値との差を求める第２の減算手段と、
該第２の減算手段の上記差に基づいて上記モータを駆動する制御手段と、
を備え、前記上記力検出手段の検出値がゼロのときの前記力偏差が前記第１の値Ｆ１より大きくなるように力指令値を設定し、位置制御からトルク制御に連続的に移行させることを特徴とする力制御装置。
【請求項２】
上記押圧部の停止位置と、上記押圧部の停止開始を定める停止開始位置とを記憶する記憶手段と、
上記角度検出手段の検出値に基づいて上記停止開始位置を越えたと判断する判断手段と、
この判断手段により上記停止開始位置を越えたと判断した場合、上記停止位置に上記モータが停止させるように上記モータの回転速度を制限する速度制限手段と、
を備えたことを特徴とする請求項１に記載の力制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、位置制御からトルク制御に円滑に切換える力制御装置の改良に関するものである。
【０００２】
【従来の技術】
従来の力制御装置を特開昭６３−１９１４５号公報に開示された図７によって説明する。かかる力制御装置は上肢体などの機能の回復するために用いられるリハビリテーションに用いられるものである。
図７において、力制御装置は、駆動部、検出部、制御部から成り、駆動部は上肢体が取り付けられる端部２を有するアーム１と、アーム１の基端部３に固定された回転軸４と、回転軸４を介して減速機７に連結された出力軸６を有するサーボモータ５とから成っている。
【０００３】
検出部は、サーボモータ５の出力軸６の回転速度を検出する速度検出器８と、アーム１に固定されると共に、アーム１に作用する力を検出する力検出器１１と、アーム１の回転角度により位置を検出する位置検出器２１とから成っている。
【０００４】
制御部は、力検出器１１の検出値を制限するリミッタ１４と、位置検出器２１の検出値に対して所定の値まで出力をゼロとする不感帯を有する不感帯器２３と、この不感帯器２３の出力値を乗算させる係数器２５と、係数器２５の出力値とリミッタ１４の出力値との差分を求める減算器２７と、減算器２７の出力値と速度検出器８の検出値との差分を求める減算器２９と、減算器２９の出力値を増幅する電力増幅器３０とから成っている。
【０００５】
上記のように構成された力制御装置の動作を図７によって説明する。上肢体がアーム１を駆動すると、位置検出器２１はアーム１の位置を検出して不感帯器２３に入力し、不感帯器２３は、不感帯特性に基づいて出力値θ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を減算器２９に与えて電力増幅器３０を介してサーボモータ５をアーム１とは反対方向に回転させる。
従って、アーム１の位置フィードバック動作がなされ、アーム１は上下位置θmax〜θminの可動範囲を大きく越えないようになる。
【０００８】
上肢体がアーム１に力を加えて等速制御をしている時に、アーム１の位置がθmax,θminになると、位置指令Ｌrすなわち係数器２５の出力がゼロになり、リミッタ１４は上肢体がアーム１に加えた力に応じた速度指令を減算器２７、電力増幅器３０に与え、上肢体がアーム１を回転していた方向にサーボモータ５を回転する。したがって、アーム１の急激な停止が防止でき、衝撃力が上肢体に加わることを防止できるものである。
【０００９】
【発明が解決しようとする課題】
しかしながら、上記のように構成された力制御装置は、位置制御で動作する範囲が不感帯器２３の特性によって定まる。すなわち、角度位置θmax,θminを越えないと、位置制御がされず、角度位置θmax,θminでは、等速制御（トルク制御）になり、位置制御がされない。よって、このような力制御装置は、所定の位置までは、トルク制御を実行し、所定の位置に達したら位置制御を実行するものである。
このため、所定の位置までは、位置制御を実行し、所定の位置に達したらトルク制御を実行する力制御装置、例えば、台の上に載せられている金物を、所定の距離離れているアームにより、アームを位置制御により移動して、トルク制御により所定量押圧する切替えが円滑にできないという問題点があった。
【００１０】
この発明は、上記のような課題を解決するためになされたもので、位置制御からトルク制御に円滑に切換る力制御装置を提供することを目的としている。
【００１１】
【課題を解決するための手段及び発明の作用、効果】
第１の発明に係る力制御装置は、モータにより駆動される押圧部をワークに対して接近させた後該押圧部をワークに対し押圧させる力制御装置であって、上記モータに一定値の力指令値を発生する力指令発生手段と、上記モータの回転角度を検出する位置検出手段と、上記モータの回転速度を検出する速度検出手段と、上記押圧部に作用する力を検出する力検出手段と、上記力指令値と上記力検出手段の検出値との差を求め、該差を増幅して力偏差として出力する第１の減算手段と、上記位置検出手段の回転角度検出値に基づいて、移動開始から所定の加速度で加速し、第１の速度で等速移動し、所定の切換え位置から所定の減速度で減速し、第１の速度より小さな第２の速度で等速移動する速度パターンに従った速度指令を生成する速度指令生成手段と、前記検出回転角度が前記切換え位置に到達していない場合でかつ前記力偏差が第１の値Ｆ１より大きいときは第１の制限値ＶＬ１を上限として速度指令に比例した値を出力し、前記検出回転角度が前記切換え位置を越えた場合は、前記力偏差が第１の値Ｆ１より小さい第２の値Ｆ２以下のときは、第１の制限値ＶＬ１より小さい第２の制限値ＶＬ２を上限として力偏差に比例した値を出力すると共に、前記力偏差が第２の値Ｆ２より大きいときは第２の制限値ＶＬ２を上限として速度指令に比例した値を出力する入出力特性が設定されたリミット手段と、該リミット手段の出力値と上記速度検出手段の検出値との差を求める第２の減算手段と、該第２の減算手段の上記差に基づいて上記モータを駆動する制御手段と、を備え、前記上記力検出手段の検出値がゼロのときの前記力偏差が前記第１の値Ｆ１より大きくなるように力指令値を設定し、位置制御からトルク制御に連続的に移行させることを特徴とするものである。
かかる解決手段によれば、位置制御の場合には、リミッタ手段により速度指令生成手段の指令速度で動作し、力検出手段からの力指令に近づいた場合には、力指令発生手段からの力指令に自動的に切替わるので、位置制御からトルク制御に衝撃なく、切替えることができる。
従って、例えば、台の上に載せられている金物を、所定の距離離れているアームにより、アームを位置制御により移動して、トルク制御により所定量押圧する切替えが自動的に円滑にできるという効果がある。
【００１３】
第２の発明に係る力制御装置は第１又は第２の発明において、押圧部の停止位置と、上記押圧部の停止開始を定める停止開始位置とを記憶する記憶手段と、上記角度検出手段の検出値に基づいて上記停止開始位置を越えたと判断する判断手段と、この判断手段により上記停止開始位置を越えたと判断した場合、上記停止位置に上記モータが停止させるように上記モータの回転速度を制限する速度制限手段とを備えたことを特徴とするものである。
かかる解決手段によれば、判断手段は、角度検出手段に基づいて停止開始位置を越えたと判断すると、速度制限手段は停止位置にモータが停止させるように上記モータの回転速度を制限する。
従って、トルク制御を行っていても停止位置を超えないようにモータ停止させることができるという効果がある。
【００１４】
【発明の実施の形態】
実施の形態１．
この発明の一実施の形態を図１及び図２によって説明する。図１は、この発明の一実施の形態を示す力制御装置の全体ブロック図、図２は速度指令発生部の記憶部のメモリマップ図、図３は力制御装置の各部の動作を示すタイムチャートである。図１中、従来と同一符号は同一又は相当部分を示し、説明を省略する。
図１及び図２において、力制御装置は、柱状の台１０５の上に載せられている金物１０７を、所定の距離離れているアーム１を位置制御によって移動し、トルク制御に切替えて金物１０７をアーム１の押圧部１０３によって所定量押圧するように構成されている。
【００１５】
力制御装置は、検出手段を有する駆動部１００、制御部２００から成り、駆動部１００はアーム１の先端部に設けられた押圧部１０３と、アーム１の基端部３が固定された回転軸４と、回転軸４に連結された減速機７を介して出力軸６を有するサーボモータ５とから成っている。
【００１６】
検出手段は、サーボモータ５の出力軸６の回転速度を検出すると共に、該回転速度に比例した出力特性８ａを有する速度検出器８と、アーム１の押圧部１０３に作用する力を検出すると共に、該力に比例した出力特性１１ａを有する力検出器１１と、アーム１の回転角度により位置を検出する位置検出器２１とから成っている。
【００１７】
制御部２００は、サーボモータ５の回転位置にかかわらず一定の力指令値Ｆrを出力する力指令部２０１と、力検出器１１の検出値と力指令部２０１の力指令値との差を求める減算器２０３と、減算器２０３の出力値を増幅する増幅器２０５と、位置検出器２１により検出された位置に基づいて所定の速度指令を生成する速度指令発生部２０７と、速度指令発生部２０７の指令値及びアーム１の押圧部１０３の位置に基づいて出力値を二段階ＶＬ1，ＶＬ2に制限するリミット部２２０と、リミット部２２０の出力値と速度検出器８の検出値との差を求める減算器２２２と、減算器２２２の出力値を増幅すると共に、モータ５を駆動制御する電力増幅器２２４とを備えている。
【００１８】
速度指令発生部２０７は、押圧部１０３の押圧面から金物１０７までの距離に対して図３に示すようなアーム１が回転する速度指令を発生するものであって、特に、アーム１の押圧部１０３の押圧面が金物１０７と僅かに離れた距離Ｌ1から速度Ｖrβで減速して低速度Ｖr2に達すると一定低速Ｖr2により押圧部１０３の先端が移動し、押圧部１０３が金物１０７を押圧して該押圧量がＬPに達すると停止指令を発生させるように形成されており、位置検出器２１により検出された検出信号を入力する入力インターフェース（以下、インターフェースをＩ／Ｆという）２０９と、周知のＣＰＵ２１１と、処理手順のプログラム、及び設定データなどが記憶された記憶部２１３と、デジタル出力信号をアナログ信号に変換するＤ／Ａ変換器２１５と、Ｄ／Ａ変換器２１５の出力に接続された出力Ｉ／Ｆ２１５とから成っている。
【００１９】
記憶部２１３は、第一速度指令Ｖr1が記憶された第１速度領域２１３ａと、第二速度指令Ｖr2としてのクリープ速度が記憶された第２速度領域２１３ｂと、第１速度指令Ｖr1から第２速度指令Ｖr2への切替え位置Ｌ1が記憶された切替え位置領域２１３ｃと、スタートから速度指令Ｖr1になるまでの加速度αが記憶された加速度領域２１３ｄと、速度指令Ｖr1から速度指令Ｖr2になるまでの減速度が記憶された減速度領域２１３ｅとを有している。
【００２０】
リミッタ部２００は、増幅器２０５からの入力値Ｆiとして速度ＶＬを出力とする図１に示すような入出特性を有し、アーム１の押圧部１０３を位置検出器２１により検出した検出位置Ｌa＜切替え位置Ｌ1で、且つ速度指令Ｖr≧Ｖr1では入力値がゼロからＦ1までは、速度値ＶＬ1の限度において、減算器２０５からの入力値Ｆiに比例した値を出力すると共に、速度指令Ｖｒの値の大きさに比例して連続的に上限値及び下限値が制限されるように形成されている。
さらに、位置検出器２１の検出位置Ｌa＞切替え位置Ｌ1を越え、且つ速度指令Ｖr≧Ｖr2では、入力値がゼロからＦ2までは、速度値ＶＬ2の限度において増幅器２０５からの入力値Ｆiに比例した値を出力すると共に、速度指令Ｖrの値の大きさに比例して連続的に上限値及び下限値が制限されるように形成されている。
【００２１】
そして、増幅器２０５は入力値ＦrにゲインＧを乗算させた出力値Ｆiを、リミッタ部２２０の入力値Ｆ1，Ｆ2よりも大きくなるように形成されている（式で表現すればＦi＝Ｆr×Ｇ＞Ｆ１＞Ｆ２）。これは、位置制御、すなわち、力検出器１１の検出値がゼロで、力指令部２０１が力指令値Ｆrの場合、リミッタ部２２０は、出力がＶＬ1又はＶＬ2の制限となり、力指令による制御をすることなく、位置指令に基づく速度指令によってサーボモータ５を動作させるためである。
【００２２】
上記のように構成された力制御装置を図１乃至図４によって説明する。図４は速度指令発生部の動作を示すフローチャートである。
図１に示すように台１０５に載せられた金物１０７と、押圧部１０３の先端とが所定の距離隔てられた状態において、時間ｔsで、押圧部１０３のスタート指令が制御部２００に入力されると、力指令部２０１は一定の力指令Ｆrを出力するが、押圧部１０３には、力が付与されていないので、力検出器１１の検出値がゼロとなり、減算器２０３は、力指令Ｆrと該検出値との偏差を求め、増幅器２０５は、偏差値を増幅してリミッタ部２２０に入力する。
【００２３】
一方、速度指令発生部２０７のＣＰＵ２１１は、スタート指令が入力されたか否かにより動作開始を判断し（ステップＳ１０１）、動作開始していれば、位置検出器２１から検出した位置Ｌaを入力Ｉ／Ｆ２０９を介して読み込みと共に、前回位置に変化量となるΔ距離との和を今回の位置として求め（ステップＳ１０３）、今回位置と切替え開始位置Ｌ1とを比較し（ステップＳ１０５）、アーム１の押圧部１０３が移動しだしてから間もないので、今回位置＜切替え位置Ｌ1となる。
【００２４】
従って、ＣＰＵ２１１は、前回の速度指令Ｖrに加速度αに演算周期ｔrを乗算したαｔrを加算して、今回の速度指令Ｖrを求め（ステップＳ１０７）、該速度指令Ｖrが速度指令Ｖr1を越えているか否かを判断し（ステップＳ１０９）、アーム１の加速当初は、速度指令Ｖr＜Ｖr1であるので、速度指令Ｖrを出力Ｉ／Ｆ２１７を介して出力する。
【００２５】
一方、減算器２０３は、力指令部２０１の指令値Ｆrから力検出器１１の検出値ゼロを減算した値Ｆrを増幅器２０５に入力し、増幅器２０５は、該値ＦrにゲインＧを乗算してＦiを求め、リミッタ部２２０に入力する。
リミッタ部２２０は、位置検出器２１の検出位置Ｌaと切替え位置Ｌ1との関係がＬa＜Ｌ1、入力値Ｆi＞Ｆ1であるので、制限値ＶＬ1の範囲において、速度指令発生部２０７の速度指令Ｖｒの値に応じた制限値ＶＬを出力し、減算器２２２は、速度検出器８から検出されたサーボモータ５の回転速度値Ｖaと、リミッタ部２２０の出力値ＶＬとの偏差を求め、電力増幅器２２４は該偏差を増幅してサーボモータ５を駆動制御してアーム１を回転させる。
すなわち、アーム１が図３に示すように速度Ｖrαで加速している段階では、リミット部２２０は、力指令部２０１の力指令Ｆrにより制限値ＶＬ1の範囲において、速度指令発生部２０７からの位置検出器２１の検出指令に基づく速度指令Ｖrに基づいてサーボモータ５を制御しているので、アーム１は位置制御を実行しているものである。
【００２６】
やがて、アーム１の押圧部１０３の速度が上昇してステップＳ１０９において、Ｖr＞Ｖr1が成立すると、速度指令発生部２０７の速度指令Ｖr1にして出力Ｉ／Ｆ２１７を介してリミッタ部２２０に入力する（ステップＳ１１１）。
さらに、アーム１が移動し、押圧部１０３が金物１０７の近傍となる切替え位置Ｌ1を超えると、ステップＳ１０５において今回位置＞切替え位置Ｌ1が成立し、ＣＰＵ２１１は、前回速度指令から減速度βに演算周期ｔrを乗算したβｔrを減算して今回速度指令を求め（ステップＳ１１９）、速度指令Ｖrを、予め定められたクリープ速度Ｖr2と比較し（ステップＳ１２１）、速度指令Ｖrがクリープ速度Ｖr2よりも高ければ、Ｄ／Ａ変換器２１５によりデジタル指令をアナログ指令に変換し、出力Ｉ／Ｆ２１７を介してリミッタ部２２０に入力する。
やがて、アーム１の押圧部１０３の速度がＶr2に達すると、速度指令発生部２０７は、Ｖr2をリミッタ部２２０に入力する（ステップＳ１２３）。
【００２７】
切替え位置Ｌ1を超えると、リミッタ部２２０の最高制限出力は、ＶＬ1からＶＬ2となり、やがて、アーム１の先端の押圧部１０３が金物１０７に当接し始める位置Ｌ0に達すると、力検出器１１の検出した力値Ｆaが増加し、減算器２０３は、力指令Ｆrと該力値Ｆaとの偏差を求め、この偏差が減少し、増幅器２０５は、Ｆ2よりも小さな値Ｆ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＝Ｖp2・ｔβmax／（２Ｖmax）・・・・（４）
したがって、停止開始位置Ｌfは下式となる。
Ｌf＝Ｌs−Ｖp2・ｔβmax／（２Ｖmax）・・・・・（５）
【００３４】
アーム１が半時計方向に回転して停止の開始位置を越えたならば、停止位置にアーム１を停止させるための制限速度ＶrLを前記（５）を変形して下式により求める。
ＶrL＝｛（２Ｌe・Ｖmax）／ｔβmax｝｝1/2・・・（６）
すなわち、ストロークリミッタ部３００のＣＰＵ３１１は、位置検出器２１からの検出位置Ｌaを入力Ｉ／Ｆ３０９を介して読み込み、記憶部３１３に設定された停止の開始位置Ｌfと比較して、開始位置Ｌfが越えた場合には、アーム１の制限速度ＶrLを前記（６）式により求め、Ｄ／Ａ変換器３１５を介して出力Ｉ／Ｆ３０９から制限速度指令ＶrLを減算器２２２に入力してサーボモータ５を駆動する。
【００３５】
なお、ストロークリミッタ部３００は、アーム１が停止の開始位置Ｌfを越えるまでは、単なるバッファ回路として動作する。
また、前記実施の形態は、アーム１を半時計方向に回転させる前進について説明したが、アーム１を時計方向に回転させる後退についても前記実施の形態と同様の考え方に基づいて速度を制限すれば良い。
【図面の簡単な説明】
【図１】  この発明の一実施の形態を示す力制御装置の全体構成を示すブロック図である。
【図２】  図１に示す速度指令発生部の記憶部のメモリマップ図である。
【図３】  図１に示す力制御装置の各部の動作を示すタイムチャートである。
【図４】  図１に示す速度指令発生部の動作を示すフローチャートである。
【図５】  この発明の他の実施の形態を示す力制御装置の全体構成を示すブロック図である。
【図６】  図５に示す速度指令発生部の記憶部のメモリマップ図である。
【図７】  従来の力制御装置を示す全体ブロック図である。
【符号の説明】
５  サーボモータ、８  速度検出器（速度検出手段）、１１  力検出器（力検出手段）、２１  位置検出器（角度検出手段）、２０１  力指令部（力指令発生手段）、２０３  第１の減算手段（減算器）、２０７  速度指令発生部（速度指令生成手段）、２２０  リミッタ部（リミット手段）、２２２  第２の減算手段（減算器）、２２４  電力増幅器（制御手段）、３００  ストロークリミッタ部（速度制限手段）、３１３  記憶部（記憶手段）。
【図１】
0003998415.tif 000002
【図２】
0003998415.tif 000003
【図３】
0003998415.tif 000004
【図４】
0003998415.tif 000005
【図５】
0003998415.tif 000006
【図６】
0003998415.tif 000007
【図７】
0003998415.tif 000008