JP4042072B2

JP4042072B2 - Training equipment

Info

Publication number: JP4042072B2
Application number: JP24445297A
Authority: JP
Inventors: 俊充入江; 慎悟安藤; 満徳川辺
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1997-09-09
Filing date: 1997-09-09
Publication date: 2008-02-06
Anticipated expiration: 2017-09-09
Also published as: JPH1176328A

Description

【０００１】
【発明の属する技術分野】
本発明は、リハビリテーション機器および訓練機器に適用される訓練装置の非常停止装置に関する。
【０００２】
【従来の技術】
人体の肢体の機能回復や鍛錬のために、可動機構に肢体を固定し、可動機構を強制的に駆動させる装置（以下、「訓練装置」という）が各種提案されている。このような訓練装置においては、肢体に無理な力が掛かったときに動作を停止して肢体を保護する非常停止装置が設けられている。
従来の訓練装置の非常停止装置のブロック図を図８に示す。図８において、１０１は位置指令生成部、１０２は位置ループゲイン（Ｋｐ）部、１０５は速度ループゲイン（Ｋｖ）部、１０９はトルク制御器、１１０は速度検出器、１１１は位置検出器、１１２Ａ〜１１２Ｃはサーボモータ、１１３は肢体（脚）、１１４は訓練装置、１１５は位置指令、１１６は速度指令、１２０はトルク指令、１２３はサーボモータ１１２Ａの速度、１２４はサーボモータ１１２Ａの位置、８０５はサーボアンプである。なお、図８では訓練装置の非常停止装置のブロック図をサーボモータ１１２Ａについてのみ示したが、他のサーボモータ１１２Ｂ〜１１２Ｃについても同様である。
【０００３】
まず、訓練装置の通常動作（すなわち非常停止を必要としない状態）の時の動作について説明する。位置指令生成部１０１ではサーボモータ１１２Ａを動作させる位置指令１１５を生成し出力する。
位置ループゲイン（Ｋｐ）部１０２では、位置指令生成部１０１の出力である位置指令１１５とサーボモータ１１２Ａの位置を検出する位置検出器１１１の出力であるサーボモータ１１２Ａの位置１２４との差分を入力し、その差分に位置ループゲインＫｐを乗じ速度指令１１６として出力する。
速度ループゲイン（Ｋｖ）部１０５では、位置ループゲイン（Ｋｐ）部１０２の出力である速度指令１１６とサーボモータ１１２Ａの速度を検出する速度検出器１１０の出力であるサーボモータ１１２Ａの速度１２３との差分を入力し、その差分に速度ループゲインＫｖを乗じトルク指令１２０として出力する。
トルク制御器１０９では入力されたトルク指令１２０に従ってサーボモータ１１２Ａを駆動する。
なお、以上の通常動作時の動作説明はサーボモータ１１２Ｂ〜１１２Ｃについても同様であり、これらサーボモータ１１２Ａ〜１１２Ｃを駆動することにより訓練装置１１４を動作させ、訓練装置１１４を動作させることにより肢体（脚）１１３の運動を行う。
【０００４】
次に訓練装置の非常停止時、例えば肢体に掛かる力が所定の値を越えた場合、また、肢体の動作速度が所定の値を越えた場合等（すなわち非常停止を必要とする状態）の時の動作について説明する。
非常停止時の第１の方法は、サーボアンプ８０５に供給する電源を遮断することである。この方法では、トルク制御器１０９がサーボモータ１１２Ａを駆動できなくなる。同様にサーボモータ１１２Ｂ〜１１２Ｃも駆動されなくなり、訓練装置１１４および肢体（脚）１１３は訓練装置１１４および肢体（脚）１１３の自重により落下運動をする。
非常停止時の第２の方法は、サーボモータ１１２Ａのブレーキを動作させることである。この方法では、サーボモータ１１２Ａが直ちに停止する。同様にサーボモータ１１２Ｂ〜１１２Ｃも停止し、訓練装置１１４および肢体（脚）１１３は直ちに停止する。
非常停止時の第３の方法は、サーボアンプ８０５に供給する速度指令１１６を０にすることである。この方法では、サーボモータ１１２Ａの速度１２３が０になった時点で速度ループゲイン（Ｋｖ）部１０５への入力が０となり、速度ループゲイン（Ｋｖ）部１０５の出力であるトルク指令１２０がトルク制御器１０９に入力される。従ってサーボモータ１１２Ａは速度０の状態を維持する、すなわち直ちに停止する。同様にサーボモータ１１２Ｂ〜１１２Ｃも停止し、訓練装置１１４および肢体（脚）１１３は停止する。
【０００５】
【発明が解決しようとする課題】
ところが、従来の技術における、サーボアンプ８０５に供給する電源を遮断する非常停止時の第１の方法では、訓練装置および肢体の自重により訓練装置および肢体が落下運動をするが、訓練装置および肢体が簡単に動作してしまうことにより、訓練装置が訓練装置の動作限界まで達してしまい訓練装置が破壊される恐れがある、また、肢体が肢体にとって危険な位置または姿勢または位置および姿勢になる危険性がある、という問題点があった。
また、従来の技術における、サーボモータ１１２Ａのブレーキを動作させる非常停止時の第２の方法、あるいは、サーボアンプ８０５に供給する速度指令１１６を０にする非常停止時の第３の方法では、訓練装置および肢体の動作を直ちに停止させるが、訓練装置および肢体を瞬時に停止することにより訓練装置および肢体に大きな加速度を発生し衝撃を与えるので、訓練装置を破壊してしまう恐れがある、また、肢体に与える危険性が高くなる、という問題点があった。
そこで、本発明は、例えば肢体に与える力が所定の値を越えた場合、また肢体の動作速度が所定の値を越えた場合等の非常停止時において、肢体が危険な位置または姿勢または位置および姿勢になる危険性を軽減し、かつ訓練装置が動作限界まで達することによって生じる破壊の恐れを軽減することのできる訓練装置の非常停止装置を提供することを目的とする。
また、非常停止時において、肢体が大きな加速度または衝撃または力を受ける危険性を軽減し、かつ訓練装置が大きな加速度または衝撃または力を受けることによって生じる破壊の恐れを軽減することのできる訓練装置の非常停止装置を提供することを目的とする。
【０００６】
【課題を解決するための手段】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、リンクで構成されて肢体の一部を固定する可動機構と、前記可動機構を減速機を介して駆動する駆動手段（１１２）と、前記駆動手段の位置を検出する位置検出器（１１１）と、前記駆動手段の速度を検出する速度検出器（１１０）と、前記駆動手段の位置指令を出力する位置指令生成部（１０１）と、前記位置指令と前記検出された位置とを比較し、その位置の差分を出力する第１比較部と、前記第１比較部が出力する位置の差分に位置ループゲインを乗じて速度指令を出力する位置ループゲイン部（１０２）と、前記速度指令と前記検出された速度指令とを比較し、その速度の差分を出力する第２比較部と、前記速度の差分に速度ループゲインを乗じてトルク指令を出力する速度ループゲイン部（１０５）と、前記トルク指令に従って前記駆動手段を駆動するトルク制御器（１０９）と、を備え、前記可動機構を動作させることにより前記肢体を動作させる訓練装置において、前記第２比較部の入力側に設けられて位置ループを断続する位置ループオン・オフスイッチ（１０３）と、前記速度ループゲイン部（１０５）が出力するトルク指令の値を所定の値に制限して出力するトルクリミット設定部（１０７）と、前記トルクリミット設定部（１０７）の出力側に設けられ、重力による前記可動機構の落下動作を防止する重力補償後のトルク指令を前記トルク制御器（１０９）に出力する重力補償設定部（１０８）と、前記速度ループゲイン部（１０５）の出力側に設けられ、前記速度ループゲイン部（１０５）が出力するトルク指令の出力先を前記トルク制御器（１０９）または前記トルクリミット設定部（１０７）に切り換えるトルクリミットオン・オフスイッチ（１０６）と、前記位置ループオン・オフスイッチと前記トルクリミットオン・オフスイッチとを制御する非常停止信号監視部（１０４）と、を備え、前記非常停止信号監視部（１０４）は、非常停止信号が入力されると、直ちに、前記速度指令が前記第２比較部に出力されないように前記位置ループオン・オフスイッチ（１０３）を制御し、前記トルク指令が前記トルクリミット設定部（１０７）に出力されるように前記トルクリミットオン・オフスイッチ（１０６）を制御することにより、前記肢体が次式（ｍ＋Ｍ）ｄ ^２ｘ／ｄｔ ^２＋Ｄｄｘ／ｄｔ＝−ｆｅ−ｆ _ｌｉｍ・ｓｉｇｎ（ｄｘ／ｄｔ）（但し、ｍ：前記肢体の質量、Ｍ：前記リンクの質量、Ｄ：当該訓練装置の持つ粘性、ｆｅ：前記肢体が前記リンクから受ける力、ｆ _ｌｉｍ：前記減速機の減速比を１：ｋとし、前記リンクの長さをＲとし、前記トルクリミット設定部（１０７）で設定されるトルクリミット値をτ _ｌｉｍとした時にｆ _ｌｉｍ＝ｋＲ・τ _ｌｉｍで表される値、ｘ：肢体の固定位置、ｔ：時間）で表されるモデルに従って動作することを特徴とするものである。
また、請求項２に記載の発明は、リンクで構成されて肢体の一部を固定する可動機構と、前記可動機構を減速機を介して駆動する駆動手段（１１２）と、前記駆動手段の位置を検出する位置検出器（１１１）と、前記駆動手段の速度を検出する速度検出器（１１０）と、前記駆動手段の位置指令を出力する位置指令生成部（１０１）と、前記位置指令と前記検出された位置とを比較し、その位置の差分を出力する第１比較部と、前記第１比較部が出力する位置の差分に位置ループゲインを乗じて速度指令を出力する位置ループゲイン部（１０２）と、前記速度指令と前記検出された速度指令とを比較し、その速度の差分を出力する第２比較部と、前記第２比較部が出力する速度の差分に第１速度ループゲインを乗じてトルク指令を出力する速度ループゲイン部（１０５）と、前記トルク指令に従って前記駆動手段を駆動するトルク制御器（１０９）と、を備え、前記可動機構を動作させることにより前記肢体を動作させる訓練装置において、前記第２比較部の入力側に設けられて位置ループを断続する位置ループオン・オフスイッチ（１０３）と、前記第２比較部が出力する速度の差分に前記第１速度ループゲインよりも小さい第２速度ループゲインを乗じてトルク指令を出力する速度ループゲイン調整部（６０７）と、前記速度ループゲイン調整部（６０７）の出力側に設けられ、重力による前記可動機構の落下動作を防止する重力補償後のトルク指令を前記トルク制御器に出力する重力補償設定部（１０８）と、前記第２比較部の出力側に設けられ、前記第２比較部が出力する速度の差分の出力先を前記速度ループゲイン部（１０５）または前記速度ループゲイン調整部（６０７）に切り換える速度ループゲイン調整オン・オフスイッチ（６０５）と、前記位置ループオン・オフスイッチ（１０３）と前記速度ループゲイン調整オン・オフスイッチ（６０５）とを制御する非常停止信号監視部と、を備え、前記非常停止信号監視部（６０４）は、非常停止信号が入力されると、直ちに、前記速度指令が前記第２比較部に出力されないように前記位置ループオン・オフスイッチ（１０３）を制御し、前記第２比較部が出力する速度の差分が速度ループゲイン調整部（６０７）に出力されるように前記速度ループゲイン調整オン・オフスイッチ（６０５）を制御することにより、前記肢体が次式（ｍ＋Ｍ）ｄ ^２ｘ／ｄｔ ^２＋（Ｄ＋Ｋｖ’）ｄｘ／ｄｔ＝−ｆｅ（但し、ｍ：前記肢体の質量、Ｍ：前記リンクの質量、Ｄ：当該訓練装置の持つ粘性、Ｋｖ’：前記第２速度ループゲイン、ｆｅ：前記肢体が前記リンクから受ける力、ｘ：肢体の固定位置、ｔ：時間）で表されるモデルに従って動作することを特徴とするものである。
【０００７】
【発明の実施の形態】
以下、本発明の実施の形態を図に基づいて説明する。
図１は本発明の第１の実施の形態に係る訓練装置の非常停止装置のブロック図である。なお、図８で示した従来の訓練装置の非常停止装置のブロック図における構成要素と同一のものについては、同一の符号を付して説明を省略する。
図１において、１０３は位置ループオン・オフスイッチ、１０４は非常停止信号監視部、１０６はトルクリミットオン・オフスイッチ、１０７はトルクリミット設定部、１０８は重力補償設定部、１１７は非常停止信号、１１８，１１９はトルクリミットオン・オフスイッチ１０６の第１および第２の接点、１２１はトルクリミット後トルク指令、１２２は重力補償後トルク指令である。なお、図１では訓練装置の非常停止装置のブロック図をサーボモータ１１２Ａについてのみ示したが、他のサーボモータ１１２Ｂ〜１１２Ｃについても同様である。
まず、訓練装置の通常動作（すなわち非常停止を必要としない状態）の時の動作について説明する。
非常停止信号監視部１０４は非常停止信号１１７の入力の有無を監視し、非常停止信号１１７の有無に従って位置ループオン・オフスイッチ１０３およびトルクリミットオン・オフスイッチ１０６の制御を行う。通常動作時すなわち非常停止信号１１７の入力が無い場合には、位置ループオン・オフスイッチ１０３はオンの状態（接続されている状態）、トルクリミットオン・オフスイッチ１０６はオフの状態（第１の接点１１８に接続されている状態）となる。この状態は、従来の技術において図８を用いて説明した訓練装置の通常動作時と同じである。
【０００８】
次に、訓練装置の非常停止時、例えば肢体にかかる力が所定の値を越えた場合、また、肢体の動作速度が所定の値を越えた場合等（すなわち非常停止を必要とする状態）の時の動作について、まず説明する。なお、位置指令生成部１０１、位置ループゲイン（Ｋｐ）部１０２、速度ループゲイン（Ｋｖ）部１０５、トルク制御器１０９、速度検出器１１０、位置検出器１１１の各動作は、従来の技術において図８を用いて説明した各構成要素と同一である。
前述のように、非常停止信号監視部１０４は非常停止信号１１７の入力の有無を監視し、非常停止信号１１７の有無に従って位置ループオン・オフスイッチ１０３およびトルクリミットオン・オフスイッチ１０６の制御を行う。例えば肢体（脚）１１３にかかる力が所定の値を越えた場合、また、肢体（脚）１１３の動作速度が所定の値を越えた場合等（すなわち非常停止時）には、非常停止信号１１７が非常停止信号監視部１０４に入力される。非常停止信号１１７が非常停止信号監視部１０４に入力された場合には、位置ループオン・オフスイッチ１０３はオフの状態（位置ループが切断された状態）、トルクリミットオン・オフスイッチ１０６はオンの状態（第２の接点１１９に接続されている状態）となる。
従って位置ループゲイン（Ｋｐ）部１０２の出力である速度指令１１６は、サーボモータ１１２Ａの速度を検出する速度検出器１１０の出力であるサーボモータ１１２Ａの速度１２３との差分をとる時に０であり、０とサーボモータ１１２Ａの速度１２３との差分が速度ループゲイン（Ｋｖ）部１０５に入力される。
上記の非常停止信号監視部１０４の説明から、非常停止時は速度ループゲイン（Ｋｖ）部１０５の出力であるトルク指令１２０がトルクリミット設定部１０７に入力される。トルクリミット設定部１０７では入力されるトルク指令１２０の値を監視し、トルク指令１２０が所定の値を越えている場合（すなわちトルクリミット値を越えた場合）にはトルク指令１２０の値を制限し、所定の値以内の場合にはトルク指令１２０に何の操作も与えない。
トルクリミット設定部１０７の出力（トルクリミット後トルク指令１２１）はトルクリミット設定部１０７で制限された場合にはトルクリミット値、制限されなかった場合にはトルクリミット設定部１０７に入力されたトルク指令１２０である。
【０００９】
図２にトルクリミット設定部１０７の動作概念図を示す。図２において、２０１は入力トルク指令（横軸）、２０２は出力トルク指令（縦軸）、２０３は原点、２０４は正側トルクリミット値、２０５は負側トルクリミット値、２０６は正側トルクリミット点、２０７は負側トルクリミット点、２０８は入力トルク指令２０１と出力トルク指令２０２とを関係付けるトルク線Ａ、２０９は入力トルク指令２０１と出力トルク指令２０２とを関係付けるトルク線Ａ２０８とは異なったトルク線Ｂである。
トルク線Ａ２０８において、負側トルクリミット点２０７から原点２０３を介して正側トルクリミット点２０６までの間は入力トルク指令２０１と出力トルク指令２０２とは係数１の比例関係を持ち、負側トルクリミット点２０７より入力トルク指令２０１が小さい場合は負側トルクリミット値２０５で横軸と平行となり、正側トルクリミット点２０６より入力トルク指令２０１が大きい場合は正側トルクリミット値２０４で横軸と平行となる。
従って、トルクリミット設定部１０７に入力されるトルク指令１２０が正側トルクリミット値２０４以下、かつ負側トルクリミット値２０５以上ならば、トルクリミット設定部１０７から出力されるトルクリミット後トルク指令１２１は入力されるトルク指令１２０と等しくなる。
【００１０】
また、トルクリミット設定部１０７に入力されるトルク指令１２０が正側トルクリミット値２０４より大きい、または負側トルクリミット値２０５より小さい場合には、入力されるトルク指令１２０は、それぞれ、正側トルクリミット値２０４、負側トルクリミット値２０５に抑えられ、トルクリミット設定部１０７から出力されるトルクリミット後トルク指令１２１はそれぞれ、正側トルクリミット値２０４、負側トルクリミット値２０５となる。
正側トルクリミット値２０４および負側トルクリミット値２０５は、共に絶対値を等しく設定できることもできるし、また各々独自に設定することもできる。また、正側トルクリミット値２０４および負側トルクリミット値２０５は可変である。
更に、正側トルクリミット値２０４および負側トルクリミット値２０５は、訓練装置１１４あるいは肢体（脚）１１３の位置または姿勢または位置および姿勢の変化に伴い可変である。尚、上述の説明では入力トルク指令２０１と出力トルク指令２０２との関係はトルク線Ａ２０８で示したものを使用していたが、これに限らずトルク線Ｂ２０９に示したような関係を使用してもよい。
【００１１】
重力補償設定部１０８では、訓練装置１１４の重量と訓練装置１１４に装着した肢体（脚）１１３の重量との和で訓練装置１１４が落下動作を行わないように訓練装置１１４および肢体（脚）１１３の重力分を補償する、サーボモータ１１２Ａに供給するトルク指令値（重力補償分のトルク指令値）を設定し、その重力補償分のトルク指令値とトルクリミット設定部１０７の出力であるトルクリミット後トルク指令１２１を加算する。重力補償設定部１０８の出力は重力補償後トルク指令１２２である。
重力補償分のトルク指令値は、訓練装置１１４あるいは肢体（脚）１１３の位置または姿勢または位置および姿勢の変化に伴い可変である。重力補償設定部１０８の出力である重力補償後トルク指令１２２はトルク制御器１０９に入力される。
なお、以上の非常動作時の動作説明はサーボモータ１１２Ｂ〜１１２Ｃについても同様であり、これらサーボモータ１１２Ａ〜１１２Ｃを駆動することにより訓練装置１１４を動作させ、訓練装置１１４を動作させることにより肢体（脚）１１３の運動を行う。
【００１２】
次に、訓練装置の非常停止時の実際の運動状態について説明する。簡単のため、ここではサーボモータを１つにして説明する。図３にサーボモータを１つにした訓練装置の１自由度のモデル図を示す。
図３において、３０１はモータ、３０２はモータ出力軸、３０３は減速機、３０４は減速機出力軸、３０５はリンク（質量Ｍ）、３０６は訓練装置（モデル）、３０７は物体（質量ｍ）、３０８は物体３０７が運動するＸ軸方向、３０９は物体３０７がリンク３０５から受ける力（ｆｅ）、３１０はリンク３０５が物体３０７から受ける力すなわち物体３０７がリンク３０５から受ける力３０９の反作用（−ｆｅ）である。
モータ３０１の駆動によりモータ出力軸３０２が駆動され、モータ出力軸３０２の駆動により減速機３０３が駆動され、減速機３０３の駆動により減速機出力軸３０４が駆動される。減速機３０３の減速比を１：ｋとすると、減速機出力軸３０４の回転はモータ出力軸３０２の回転の１／ｋの速度となりｋ倍のトルクを発生する。
減速機出力軸３０４の駆動により減速機出力軸３０４に固定されているリンク３０５が駆動され、リンク３０５の駆動によりリンク３０５に設置されている物体３０７が駆動される。図１に示したブロック図との対比において、モータ３０１がサーボモータ１１２Ａに、訓練装置（モデル）３０６が訓練装置１１４に、物体３０７が肢体（脚）１１３に、それぞれ相当する。
【００１３】
図３に示した訓練装置の１自由度モデルを使用して、図１に示したブロック図の非常停止時、すなわち、非常停止信号監視部１０４に非常停止信号１１７が入力され、位置ループオン・オフスイッチ１０３およびトルクリミットオン・オフスイッチ１０６が非常停止信号監視部１０４によって制御された時の運動状態を説明する。
まず、トルクリミット設定部１０７におけるトルクリミット値の設定がある場合と比較するため、位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつトルクリミットオン・オフスイッチ１０６がオフの状態（第１の接点１１８に接続されている状態）の運動状態を想定する。図４にこの運動状態、すなわち、位置ループは切断されているがトルクリミット機能なしの時の運動状態を示す。
【００１４】
図４において、（ａ）が時間経過に対する速度指令および物体速度を表す速度線図、（ｂ）が時間経過に対するトルク指令を表すトルク線図、（ｃ）が時間経過に対する物体に作用する力を表す力線図である。また、４０１は速度、４０２は時間、４０３は訓練装置（モデル）３０６の制御装置（図示せず）の速度指令、４０４（破線）は物体３０７の速度、４０５はトルク、４０７は訓練装置（モデル）３０６の制御装置のトルク指令、４０８は力、４１０は物体３０７がリンクから受ける力３０９である。
図１において、非常停止信号１１７が非常停止信号監視部１０４に入力されると、位置ループオン・オフスイッチ１０３が直ちにオフになり位置ループが切断される。従って図４の（ａ）に示すように訓練装置（モデル）３０６の制御装置の速度指令４０３が直ちに０になる。訓練装置（モデル）３０６の制御装置の速度指令４０３が直ちに０になると、この場合、図１におけるトルクリミットオン・オフスイッチ１０６がオフの状態であるので速度ループゲイン（Ｋｖ）部１０５の出力であるトルク指令１２０すなわち訓練装置（モデル）３０６の制御装置のトルク指令４０７は図４の（ｂ）に示すようになる。物体３０７がリンク３０５から受ける力３０９は図４の（ｃ）に示すようになりかなり大きな力を受けることになる。
【００１５】
この位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつトルクリミットオン・オフスイッチ１０６がオフの状態（第１の接点１１８に接続されている状態）の時、すなわち、位置ループは切断されているがトルクリミット機能なしの時、リンク３０５の質量をＭ、物体３０７の質量をｍ、訓練装置（モデル）３０６の持つ粘性をＤ、速度ループゲインをＫｖとすると、訓練装置（モデル）３０６の運動方程式は次式で表される。
（ｍ＋Ｍ）d²x/dt²＋（Ｄ＋Ｋｖ）dx/dt＝−ｆｅ（式１）
従来の技術で説明したサーボアンプ８０５に供給する速度指令１１６を０にする非常停止時の第３の方法はこの位置ループを切断しトルクリミット機能なしの時と等価であり運動方程式は（式１）で表現される。
次に、トルクリミット設定部１０７におけるトルクリミット値の設定がある場合、すなわち位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつトルクリミットオン・オフスイッチ１０６がオンの状態（第２の接点１１９に接続されている状態）の運動状態を想定する。図５にこの運動状態すなわち位置ループが切断されておりトルクリミット機能ありの時の運動状態を示す。
【００１６】
図５において、図４と同様、（ａ）が時間経過に対する速度指令および物体速度を表す速度線図、（ｂ）が時間経過に対するトルク指令を表すトルク線図、（ｃ）が時間経過に対する物体に作用する力を表す力線図である。また、５０３は訓練装置（モデル）３０６の制御装置の速度指令、５０４（破線）は物体３０７の速度、５０７は訓練装置（モデル）３０６の制御装置のトルク指令、５１０は物体３０７がリンクから受ける力３０９である。
図１において、非常停止信号１１７が非常停止信号監視部１０４に入力されると、位置ループオン・オフスイッチ１０３が直ちにオフになり位置ループが切断され、かつ、トルクリミットオン・オフスイッチ１０６がオンになりトルクリミット設定部１０７が有効になる。
従って位置ループが直ちに切断されることから図５の（ａ）に示すように訓練装置（モデル）３０６の制御装置の速度指令５０３が直ちに０になる。訓練装置（モデル）３０６の制御装置の速度指令５０３が直ちに０になるとトルクリミット機能なしの場合には訓練装置（モデル）３０６の制御装置のトルク指令４０７は図４の（ｂ）に示すようになるが、図１におけるトルクリミットオン・オフスイッチ１０６がオンの状態であるので速度ループゲイン（Ｋｖ）部１０５の出力であるトルク指令１２０すなわち訓練装置（モデル）３０６の制御装置のトルク指令５０７はトルクリミット設定部１０７にてトルクリミット値を越える場合にはトルクリミットがかけられ、トルクリミット値を越えることはできなくなる。また、訓練装置（モデル）３０６の制御装置のトルク指令４０７がトルクリミット値以内の場合には何の操作も受けず、そのままの値となる。従って、図５の（ｂ）に示すトルク指令５０７になる。
【００１７】
図１における肢体（脚）に相当する図３における物体３０７がリンクから受ける力は図５の（ｃ）に示す物体３０７がリンクから受ける力５１０となり図４で示したトルクリミットがない場合の物体３０７がリンクから受ける力４１０と比較してかなり物体３０７に作用する力を抑えることができる。
なお、非常停止時に図１における位置ループオン・オフスイッチ１０３により位置ループを切断するようにしたのは、非常停止時に位置指令１１５に従って訓練装置１１４が元の位置または姿勢または位置および姿勢に戻ることを防止するためである。この位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつトルクリミットオン・オフスイッチ１０６がオンの状態（第２の接点１１９に接続されている状態）の時、すなわち、位置ループが切断されておりトルクリミット機能ありの時、訓練装置（モデル）３０６の運動方程式は、訓練装置（モデル）３０６の制御装置のトルク指令５０７がトルクリミット値以内の場合には（式１）で、トルクリミット値を越える場合には次式で表される。
（ｍ＋Ｍ）d²x/dt²＋Ｄdx/dt＝−ｆｅ−ｆ_lim・ｓｉｇｎ（dx/dt）（式２）
ここでｆ_limは減速機３０３の減速比を１：ｋ、リンク３０５の長さをＲ、トルクリミット設定部１０７で設定されるトルクリミット値をτ_limとした時に
ｆ_lim＝ｋＲ・τ_lim （式３）
となり、つまりトルクリミット設定部１０７で設定されるトルクリミット値τ_limで物体３０７がリンク３０５から受ける力３０９の力リミット値（ｆ_lim）を表現したものである。ｓｉｇｎ（）は符号関数である。従って、この位置ループが切断されトルクリミット機能ありの状態で、かつ、訓練装置（モデル）３０６の制御装置のトルク指令５０７がトルクリミット値を越える場合は訓練装置（モデル）３０６は、Ｄdx/dtで表される粘性力とｆ_lim・ｓｉｇｎ（dx/dt）で表される動摩擦力との和に従った動作となる。
従来の技術で説明したサーボアンプ８０５に供給する電源を遮断する非常停止時の第１の方法では訓練装置はＤdx/dtで表される粘性摩擦力のみに従った動作となるので、従来の技術の第１の方法と比較して動作しにくくなる。
【００１８】
更に、訓練装置の非常停止後の運動状態について説明する。非常停止後は図１において、位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつトルクリミットオン・オフスイッチ１０６がオンの状態（第２の接点１１９に接続されている状態）、すなわち、位置ループが切断されトルクリミット機能がありとなっている。従って、非常停止後にトルク指令１２０がトルクリミット値以内であれば（式１）と同じ状態すなわち従来の技術で説明したサーボアンプ８０５に供給する速度指令１１６を０にする非常停止時の第３の方法と同じ状態となり、訓練装置１１４は動作しにくい。しかし、肢体（脚）１１３が訓練装置１１４に与える力を増加することによってトルク指令１２０がトルクリミット値を越えた場合には訓練装置１１４は（式２）に従い動き出すことができる。動き出した後の訓練装置１１４の運動状態は、第１の実施の形態で説明した非常停止時の訓練装置の運動状態と同じである。
【００１９】
図６は本発明の第２の実施の形態に係る訓練装置の非常停止装置のブロック図である。なお、図８で示した従来の訓練装置の非常停止装置のブロック図における構成要素および図１で示した本発明の第１実施例の訓練装置の非常停止装置のブロック図における構成要素と同一のものに対する説明は同一の符号を付して説明を省略する。図６において、６０４は非常停止信号監視部、６０５は速度ループゲイン（Ｋｖ）調整オン・オフスイッチ、６０７は速度ループゲイン（Ｋｖ’）調整部、６１８および６１９は速度ループゲイン調整オン・オフスイッチ６０５の第１および第２の接点、６２１は速度ループゲイン調整後トルク指令である。なお、図６では訓練装置の非常停止装置のブロック図をサーボモータ１１２Ａについてのみ示したが、他のサーボモータ１１２Ｂ〜１１２Ｃについても同様である。
まず、訓練装置の通常動作（すなわち非常停止を必要としない状態）の時の動作について説明する。非常停止信号監視部６０４は非常停止信号１１７の入力の有無を監視し、非常停止信号１１７の有無に従って位置ループオン・オフスイッチ１０３および速度ループゲイン調整オン・オフスイッチ６０５の制御を行う。通常動作時すなわち非常停止信号１１７の入力が無い場合には、位置ループオン・オフスイッチ１０３はオンの状態（接続されている状態）、速度ループゲイン調整オン・オフスイッチ６０５はオフの状態（第１の接点６１８に接続されている状態）となる。この状態は、従来の技術において図８を用いて説明した訓練装置の通常動作時と同じである。
【００２０】
次に、訓練装置の非常停止時、例えば肢体にかかる力が所定の値を越えた場合、また、肢体の動作速度が所定の値を越えた場合等（すなわち非常停止を必要とする状態）の時の動作について、まず説明する。なお、位置指令生成部１０１、位置ループゲイン（Ｋｐ）部１０２、位置ループオン・オフスイッチ１０３、速度ループゲイン（Ｋｖ）部１０５、重力補償設定部１０８、トルク制御器１０９、速度検出器１１０、位置検出器１１１の各動作は、従来の技術の図８および第１の実施の形態の図１を用いて説明した各構成要素と同一である。前述のように、非常停止信号監視部６０４は非常停止信号１１７の入力の有無を監視し、非常停止信号１１７の有無に従って位置ループオン・オフスイッチ１０３および速度ループゲイン調整オン・オフスイッチ６０５の制御を行う。例えば肢体（脚）１１３にかかる力が所定の値を越えた場合、また、肢体（脚）１１３の動作速度が所定の値を越えた場合等（すなわち非常停止時）には、非常停止信号１１７が非常停止信号監視部６０４に入力される。
非常停止信号１１７が非常停止信号監視部６０４に入力された場合には、位置ループオン・オフスイッチ１０３はオフの状態（位置ループが切断された状態）、速度ループゲイン調整オン・オフスイッチ６０５はオンの状態（第２の接点６１９に接続されている状態）となる。従って位置ループゲイン（Ｋｐ）部１０２の出力である速度指令１１６は、サーボモータ１１２Ａの速度を検出する速度検出器１１０の出力であるサーボモータ１１２Ａの速度１２３との差分をとる時に０であり、０とサーボモータ１１２Ａの速度１２３との差分が速度ループゲイン調整オン・オフスイッチ６０５に入力される。
【００２１】
上記の非常停止信号監視部６０４の説明から、非常停止時は０とサーボモータ１１２Ａの速度１２３との差分が速度ループゲイン（Ｋｖ’）調整部６０７に入力される。速度ループゲイン（Ｋｖ’）調整部６０７は、速度ループゲインＫｖ’の値を調整できる機能を持ち、速度ループゲイン（Ｋｖ）部１０５が持つ速度ループゲインＫｖの値よりも減少させることができる。すなわち次式を満たす。
Ｋｖ’＜Ｋｖ（式４）
ここで調整できる速度ループゲインＫｖ’の値は可変である。また、ここで調整できる速度ループゲインＫｖ’の値は、訓練装置１１４あるいは肢体（脚）１１３の位置または姿勢または位置および姿勢の変化に伴い可変である。
速度ループゲイン（Ｋｖ’）調整部６０７では入力される０とサーボモータ１１２Ａの速度１２３との差分に調整された速度ループゲインＫｖ’を乗じ速度ループゲイン調整後トルク指令６２１として出力する。重力補償設定部１０８では、訓練装置１１４の重量と訓練装置１１４に装着した肢体（脚）１１３の重量との和で訓練装置１１４が落下動作を行わないように訓練装置１１４および肢体（脚）１１３の重力分を補償する、サーボモータ１１２Ａに供給するトルク指令値（重力補償分のトルク指令値）を設定し、その重力補償分のトルク指令値と速度ループゲイン（Ｋｖ’）調整部６０７の出力である速度ループゲイン調整後トルク指令６２１を加算する。
重力補償設定部１０８の出力は重力補償後トルク指令１２２である。重力補償分のトルク指令値は、訓練装置１１４あるいは肢体（脚）１１３の位置または姿勢または位置および姿勢の変化に伴い可変である。重力補償設定部１０８の出力である重力補償後トルク指令１２２はトルク制御器１０９に入力される。
なお、以上の非常動作時の動作説明はサーボモータ１１２Ｂ〜１１２Ｃについても同様であり、これらサーボモータ１１２Ａ〜１１２Ｃを駆動することにより訓練装置１１４を動作させ、訓練装置１１４を動作させることにより肢体（脚）１１３の運動を行う。次に、訓練装置の非常停止時の実際の運動状態について説明する。
【００２２】
簡単のため、ここでも図３に示したサーボモータを１つにした訓練装置の１自由度モデルを使用する。図６に示したブロック図の非常停止時、すなわち、非常停止信号監視部６０４に非常停止信号１１７が入力され、位置ループオン・オフスイッチ１０３および速度ループゲイン調整オン・オフスイッチ６０５が非常停止信号監視部１０４によって制御された時の運動状態を説明する。
まず、速度ループゲイン（Ｋｖ’）調整部６０７における速度ループゲインＫｖ’の値の調整機能がある場合と比較するため、位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつ速度ループゲイン調整オン・オフスイッチ６０５がオフの状態（第１の接点６１８に接続されている状態）の運動状態を想定するが、この状態すなわち位置ループは切断されているが速度ループゲイン調整機能なしの時は、第１の実施の形態で説明した位置ループは切断されているがトルクリミット機能なしの時と同じであり、その運動状態は位置ループは切断されているがトルクリミット機能なしの時の運動状態を表した図４となる。
従って、図３において物体３０７がリンク３０５から受ける力３０９は図４の（ｃ）に示すようになりかなり大きな力を受けることになる。訓練装置（モデル）３０６の運動方程式も、位置ループは切断されているがトルクリミット機能なしの時の訓練装置（モデル）３０６の運動方程式を表した（式１）となる。
次に、速度ループゲイン（Ｋｖ’）調整部６０７における速度ループゲインＫｖ’の値の調整機能がある場合、すなわち位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつ速度ループゲイン調整オン・オフスイッチ６０５がオンの状態（第２の接点６１９に接続されている状態）の運動状態を想定する。図７にこの運動状態すなわち位置ループが切断されており速度ループゲイン調整機能ありの運動状態を示す。
【００２３】
図７において、図４および図５と同様、（ａ）が時間経過に対する速度指令および物体速度を表す速度線図、（ｂ）が時間経過に対するトルク指令を表すトルク線図、（ｃ）が時間経過に対する物体に作用する力を表す力線図である。また、７０３は訓練装置（モデル）３０６の制御装置の速度指令、７０４（破線）は物体３０７の速度、７０７は訓練装置（モデル）３０６の制御装置のトルク指令、７１０は物体３０７がリンクから受ける力３０９である。
図６において、非常停止信号１１７が非常停止信号監視部６０４に入力されると、位置ループオン・オフスイッチ１０３が直ちにオフになり位置ループが切断され、かつ、速度ループゲイン調整オン・オフスイッチ６０５がオンになり速度ループゲイン（Ｋｖ’）調整部６０７が有効になる。従って位置ループが直ちに切断されることから図７の（ａ）に示すように訓練装置（モデル）３０６の制御装置の速度指令７０３が直ちに０になる。訓練装置（モデル）３０６の制御装置の速度指令７０３が直ちに０になると、速度ループゲイン調整機能なしの場合には訓練装置（モデル）３０６の制御装置のトルク指令４０７は図４の（ｂ）に示すようになるが、図６における速度ループゲイン調整オン・オフスイッチ６０５がオンの状態であるので０とサーボモータ１１２Ａの速度１２３との差分が速度ループゲイン（Ｋｖ）部１０５の速度ループゲインＫｖの値より小さい速度ループゲインＫｖ’の値に乗じられ、速度ループゲイン調整後トルク指令６２１はトルク指令１２０より小さい値に抑えられる。従って、図７の（ｂ）に示すトルク指令７０７になる。
図６における肢体（脚）に相当する図３における物体３０７が受ける力は図７の（ｃ）に示す物体３０７がリンクから受ける力７１０となり図４で示した速度ループゲイン調整機能がない場合の物体３０７がリンクから受ける力４１０と比較してかなり物体に作用する力を抑えることができる。
【００２４】
なお、非常停止時に図６における位置ループオン・オフスイッチ１０３により位置ループを切断するようにしたのは、非常停止時に位置指令１１５に従って訓練装置１１４が元の位置または姿勢または位置および姿勢に戻ることを防止するためである。この位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつ速度ループゲイン調整オン・オフスイッチ６０５がオンの状態（第２の接点６１９に接続されている状態）の時、すなわち、位置ループが切断されており速度ループゲイン調整機能ありの時、訓練装置（モデル）３０６の運動方程式は速度ループゲインＫｖがＫｖ’となる（式１）で表される。
従って、この位置ループが切断され速度ループゲイン調整機能ありの状態では訓練装置（モデル）３０６は、（Ｄ＋Ｋｖ’）という粘性に従った動作となる。従来の技術で説明したサーボアンプ８０５に供給する電源を遮断する非常停止時の第１の方法では訓練装置（モデル）３０６は（Ｄ）という粘性に従った動作となるので、この位置ループが切断され速度ループゲイン調整機能ありの場合、訓練装置は従来の技術の第１の方法と比較するとＫｖ’分の粘性だけ増加している分だけ動作しにくくなる。
【００２５】
更に、訓練装置の非常停止後の運動状態について説明する。非常停止後は図６において、位置ループオン・オフスイッチ１０３がオフの状態（位置ループが切断された状態）でかつ速度ループゲイン調整オン・オフスイッチ６０５がオンの状態（第２の接点６１９に接続されている状態）、すなわち、位置ループが切断され速度ループゲイン調整機能がありとなっている。従って、非常停止後もＫｖがＫｖ’となる（式１）と同じ状態すなわち訓練装置１１４は（Ｄ＋Ｋｖ’）という粘性に従って動作開始可能である。何故なら、サーボアンプ８０５に供給する速度指令１１６を０にする非常停止時の第３の方法と比較すると、（Ｄ＋Ｋｖ）に対して（Ｄ＋Ｋｖ’）という粘性となり、（式４）から全体の持つ粘性が小さくなるからである。動き出した後の訓練装置１１４の運動状態は、第２の実施の形態で説明した非常停止時の訓練装置の運動状態と同じである。
【００２６】
本発明の第１の実施の形態で説明したように、本実施の形態は、訓練装置の非常停止装置の制御装置において、位置ループが切断できる機能およびトルク指令にトルクリミット値が設定できる機能を備えたため、非常停止時に肢体および訓練装置に大きな衝撃または力を与えることなく訓練装置を停止できるようになる。また、本実施の形態における訓練装置の非常停止時の運動状態は、従来の技術であるサーボアンプに供給する電源を遮断する非常停止時の第１の方法と比較して、動摩擦力の分だけ訓練装置は動作しにくくなる。更に、本実施の形態における訓練装置の非常停止後の運動状態は、肢体が訓練装置の与える力を増加することによってトルク指令がトルクリミット値を越えた場合には訓練装置が動き出すことができる。
本発明の第２の実施の形態で説明したように、本実施の形態は、訓練装置の非常停止装置の制御装置において、位置ループが切断できる機能および速度ループゲインの値が調整できる機能を備えたため、非常停止時に肢体および訓練装置に大きな衝撃または力を与えることなく訓練装置を停止できるようになる。また、本実施の形態における訓練装置の非常停止時の運動状態は、従来の技術であるサーボアンプに供給する電源を遮断する非常停止時の第１の方法と比較して、速度ループゲインの粘性の分だけ訓練装置は動作しにくくなる。更に、本実施の形態における訓練装置の非常停止後の運動状態は、速度ループゲインの値によって全体の持つ粘性が小さくできるので訓練装置が動き出すことができる。
【００２７】
【発明の効果】
以上述べたように、本発明によれば、次の効果を奏する。
（１）訓練装置の非常停止装置の制御装置において、位置ループが切断できる機能、およびトルク指令にトルクリミット値が設定できる機能、または速度ループゲインの値が調整できる機能とを備えたため、非常停止時に肢体および訓練装置に大きな衝撃または力を与えることなく訓練装置を停止できるようになる。従って肢体が大きな加速度または衝撃または力を受ける危険性を軽減でき、かつ、訓練装置が大きな加速度または衝撃または力を受けることによって生じる破壊の恐れを軽減できる。
（２）訓練装置の非常停止時の運動状態は、従来の技術であるサーボアンプに供給する電源を遮断する非常停止時の第１の方法と比較して、動摩擦力、または速度ループゲインの粘性の分だけ訓練装置は動作しにくくなるので、肢体が危険な位置または姿勢または位置および姿勢になる危険性を軽減でき、かつ、訓練装置が動作限界まで達することによって生じる破壊の恐れを軽減できる。
（３）訓練装置の非常停止後の運動状態は、肢体が訓練装置の与える力を増加することによってトルク指令がトルクリミット値を越えた場合には訓練装置が動き出すことができる。あるいは、速度ループゲインの値によって全体の持つ粘性が小さくできるので訓練装置が動き出すことができる。従って、肢体自身が肢体にとって安全な位置または姿勢または位置および姿勢に訓練装置を動作できるので、従来の技術であるサーボモータのブレーキを動作させる非常停止時の第２の方法、あるいは、サーボアンプに供給する速度指令を０にする非常停止時の第３の方法と比較して危険性が軽減できる。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係る訓練装置の非常停止装置のブロック図である。
【図２】本発明の第１の実施の形態に係るトルクリミット設定部の動作概念図である。
【図３】本発明の実施の形態に係る訓練装置の１自由度モデル図である。
【図４】本発明の実施の形態に係る訓練装置の非常停止時のトルクリミット機能なしの運動状態を表すグラフ（（ａ）速度線図、（ｂ）トルク線図、（ｃ）力線図）である。
【図５】本発明の第１の実施の形態に係る訓練装置の非常停止時のトルクリミット機能ありの運動状態を表すグラフ（（ａ）速度線図、（ｂ）トルク線図、（ｃ）力線図）である。
【図６】本発明の第２の実施の形態に係る訓練装置の非常停止装置のブロック図である。
【図７】本発明の第２の実施の形態に係る訓練装置の非常停止時の速度ループゲイン調整機能ありの運動状態を表すグラフ（（ａ）速度線図、（ｂ）トルク線図、（ｃ）力線図）である。
【図８】従来の訓練装置の非常停止装置のブロック図である。
【符号の説明】
１０１位置指令生成部、１０２位置ループゲイン（Ｋｐ）部、１０３位置ループオン・オフスイッチ、１０４非常停止信号監視部、１０５速度ループゲイン（Ｋｖ）部、１０６トルクリミットオン・オフスイッチ、１０７トルクリミット設定部、１０８重力補償設定部、１０９トルク制御器、１１０速度検出器、１１１位置検出器、１１２Ａ〜１１２Ｃサーボモータ、１１３肢体（脚）、１１４訓練装置、１１５位置指令、１１６速度指令、１１７非常停止信号、１１８第１の接点、１１９第２の接点、１２０トルク指令、１２１トルクリミット後トルク指令、１２２重力補償後トルク指令、１２３サーボモータ１１２Ａの速度、１２４サーボモータ１１２Ａの位置、２０１入力トルク指令、２０２出力トルク指令、２０３原点、２０４正側トルクリミット値、２０５負側トルクリミット値、２０６正側トルクリミット点、２０７負側トルクリミット点、２０８トルク線Ａ、２０９トルク線Ｂ、３０１モータ、３０２モータ出力軸、３０３減速機、３０４減速機出力軸、３０５リンク（質量Ｍ）、３０６訓練装置（モデル）、３０７物体（質量ｍ）、３０８物体３０７が運動するＸ軸方向、３０９物体３０７がリンク３０５から受ける力（ｆｅ）、３１０リンク３０５が物体３０７から受ける力（−ｆｅ）、４０１速度、４０２時間、４０３速度指令、４０４物体速度、４０５トルク、４０７トルク指令、４０８力、４１０物体がリンクから受ける力、５０３速度指令、５０４物体速度、５０７トルク指令、５１０物体がリンクから受ける力、６０４非常停止信号監視部、６０５速度ループゲイン調整オン・オフスイッチ、６０７速度ループゲイン（Ｋｖ’）調整部、６１８第１の接点、６１９第２の接点、６２１速度ループゲイン調整後トルク指令、７０３速度指令、７０４物体速度、７０７トルク指令、７１０物体がリンクから受ける力、８０５サーボアンプ[0001]
BACKGROUND OF THE INVENTION
The present invention relates to an emergency stop device for a training device applied to a rehabilitation device and a training device.
[0002]
[Prior art]
Various devices (hereinafter referred to as “training devices”) for fixing the limbs to the movable mechanism and forcibly driving the movable mechanism have been proposed for the functional recovery and training of the human limbs. Such a training device is provided with an emergency stop device that stops the operation and protects the limb when an excessive force is applied to the limb.
A block diagram of an emergency stop device of a conventional training device is shown in FIG. In FIG. 8, 101 is a position command generation unit, 102 is a position loop gain (Kp) unit, 105 is a speed loop gain (Kv) unit, 109 is a torque controller, 110 is a speed detector, 111 is a position detector, 112A ˜112C is a servo motor, 113 is a limb (leg), 114 is a training device, 115 is a position command, 116 is a speed command, 120 is a torque command, 123 is a speed of the servo motor 112A, 124 is a position of the servo motor 112A, 805 Is a servo amplifier. In addition, in FIG. 8, although the block diagram of the emergency stop apparatus of a training apparatus was shown only about the servomotor 112A, it is the same also about other servomotors 112B-112C.
[0003]
First, the operation at the time of the normal operation of the training apparatus (that is, a state that does not require an emergency stop) will be described. The position command generator 101 generates and outputs a position command 115 for operating the servo motor 112A.
The position loop gain (Kp) unit 102 inputs a difference between the position command 115 output from the position command generation unit 101 and the position 124 of the servo motor 112A which is the output of the position detector 111 that detects the position of the servo motor 112A. Then, the difference is multiplied by the position loop gain Kp and output as a speed command 116.
In the speed loop gain (Kv) unit 105, a speed command 116 that is an output of the position loop gain (Kp) unit 102 and a speed 123 of the servo motor 112A that is an output of the speed detector 110 that detects the speed of the servo motor 112A. The difference is input, and the difference is multiplied by the speed loop gain Kv and output as a torque command 120.
The torque controller 109 drives the servo motor 112A according to the input torque command 120.
The above description of the normal operation is the same for the servo motors 112B to 112C. The training apparatus 114 is operated by driving these servo motors 112A to 112C, and the limbs ( Leg) 113 is exercised.
[0004]
Next, at the time of emergency stop of the training device, for example, when the force applied to the limb exceeds a predetermined value, or when the movement speed of the limb exceeds a predetermined value (that is, a state requiring an emergency stop) Will be described.
The first method at the time of emergency stop is to cut off the power supplied to the servo amplifier 805. In this method, the torque controller 109 cannot drive the servo motor 112A. Similarly, the servo motors 112 B to 112 C are not driven, and the training device 114 and the limbs (legs) 113 perform a drop motion due to the weights of the training device 114 and the limbs (legs) 113.
The second method at the time of emergency stop is to operate the brake of the servo motor 112A. In this method, the servo motor 112A stops immediately. Similarly, the servo motors 112B to 112C are also stopped, and the training device 114 and the limb (leg) 113 are immediately stopped.
The third method at the time of emergency stop is to set the speed command 116 supplied to the servo amplifier 805 to zero. In this method, when the speed 123 of the servo motor 112A becomes 0, the input to the speed loop gain (Kv) unit 105 becomes 0, and the torque command 120, which is the output of the speed loop gain (Kv) unit 105, is torque controlled. Is input to the device 109. Therefore, the servo motor 112A maintains the state of zero speed, that is, stops immediately. Similarly, the servo motors 112B to 112C are also stopped, and the training device 114 and the limbs (legs) 113 are stopped.
[0005]
[Problems to be solved by the invention]
However, in the first method of emergency stop in which the power supplied to the servo amplifier 805 is cut off in the prior art, the training device and the limb move by the weight of the training device and the limb. There is a risk that the training device may reach the operating limit of the training device due to simple movement, and the training device may be destroyed, and the risk that the limb will be in a dangerous position or posture or position and posture for the limb There was a problem that there was.
Further, in the second method at the time of emergency stop for operating the brake of the servo motor 112A or the third method at the time of emergency stop for setting the speed command 116 supplied to the servo amplifier 805 to 0 in the prior art, training is performed. Immediately stop the movement of the device and the limbs, but the training device and the limbs are instantaneously stopped to generate a large acceleration and give an impact to the training device and the limbs. There was a problem that the risk to the limbs increased.
Therefore, the present invention provides a position or posture or position where the limb is in a dangerous position in an emergency stop such as when the force applied to the limb exceeds a predetermined value or when the movement speed of the limb exceeds a predetermined value. An object of the present invention is to provide an emergency stop device for a training device that can reduce the risk of becoming a posture and can reduce the risk of destruction caused by the training device reaching its operating limit.
Further, in the event of an emergency stop, a training apparatus that can reduce the risk of the limb receiving a large acceleration, impact, or force and that can reduce the risk of destruction caused by the training apparatus receiving a large acceleration, impact, or force. An object is to provide an emergency stop device.
[0006]
[Means for Solving the Problems]
 In order to solve the above problem, the present invention is configured as follows.
 According to the first aspect of the present invention, there is provided a movable mechanism configured by a link and fixing a part of a limb, a driving means (112) for driving the movable mechanism via a speed reducer, and detecting a position of the driving means. Position detector (111), a speed detector (110) for detecting the speed of the drive means, a position command generator (101) for outputting a position command for the drive means, and the position command and the detected A first comparison unit that compares the position and outputs a difference between the positions, and a position loop gain unit that outputs a speed command by multiplying the position difference output by the first comparison unit by a position loop gain (102) A second comparison unit that compares the speed command with the detected speed command and outputs a difference between the speeds, and a speed loop gain unit that outputs a torque command by multiplying the speed difference by a speed loop gain (105) And a torque controller (109) for driving the driving means in accordance with the torque command, and provided on the input side of the second comparison unit in a training apparatus for operating the limb by operating the movable mechanism. A position loop on / off switch (103) for intermittently switching the position loop, a torque limit setting unit (107) for limiting and outputting a torque command value output by the speed loop gain unit (105), and the A gravity compensation setting unit (108) which is provided on the output side of the torque limit setting unit (107) and outputs a torque command after gravity compensation to prevent the falling operation of the movable mechanism due to gravity to the torque controller (109); , Provided on the output side of the speed loop gain section (105), the output destination of the torque command output by the speed loop gain section (105) Torque limit on / off switch (106) for switching to a torque controller (109) or the torque limit setting unit (107), and an emergency stop signal monitoring for controlling the position loop on / off switch and the torque limit on / off switch The emergency stop signal monitoring unit (104) is configured to turn on and off the position loop so that the speed command is not immediately output to the second comparison unit when an emergency stop signal is input. By controlling the switch (103) and controlling the torque limit on / off switch (106) so that the torque command is output to the torque limit setting unit (107), the limb is expressed by the following equation (m + M) d ² x / dt ² + Ddx / dt = -fe-f _lim Sign (dx / dt) (where m: mass of the limb, M: mass of the link, D: viscosity of the training device, fe: force the limb receives from the link, f _lim : The reduction ratio of the reduction gear is 1: k, the length of the link is R, and the torque limit value set by the torque limit setting unit (107) is τ. _lim When f _lim = KR · τ _lim (X: fixed position of limb body, t: time).
 The invention according to claim 2 is a movable mechanism configured by a link and fixing a part of a limb, a driving means (112) for driving the movable mechanism via a speed reducer, and a position of the driving means. A position detector (111) for detecting the speed, a speed detector (110) for detecting the speed of the driving means, a position command generating unit (101) for outputting a position command for the driving means, the position command and the A first comparison unit that compares the detected position and outputs a difference between the positions, and a position loop gain unit that outputs a speed command by multiplying the position difference output by the first comparison unit by a position loop gain ( 102), the speed command and the detected speed command are compared, a second comparison unit that outputs a difference between the speeds, and a first speed loop gain is added to the speed difference output by the second comparison unit. Multiplying speed to output torque command A training apparatus comprising a loop gain unit (105) and a torque controller (109) for driving the driving means according to the torque command, and operating the limb by operating the movable mechanism, the second comparison A position loop on / off switch (103), which is provided on the input side of the section, and interrupts the position loop, and a second speed loop gain smaller than the first speed loop gain as a speed difference output from the second comparison section. A speed loop gain adjustment unit (607) that multiplies and outputs a torque command, and a torque command after gravity compensation that is provided on the output side of the speed loop gain adjustment unit (607) and prevents the moving mechanism from dropping due to gravity. Is provided on the output side of the second comparison unit, and is output from the second comparison unit. The speed of the output destination of the degree of difference Speed loop gain adjustment on / off switch (605), position loop on / off switch (103) and speed loop gain adjustment on / off switching to degree loop gain section (105) or speed loop gain adjustment section (607) An emergency stop signal monitoring unit that controls the switch (605), and the emergency stop signal monitoring unit (604) immediately sends the speed command to the second comparison unit when the emergency stop signal is input. The position loop on / off switch (103) is controlled so as not to be output, and the speed loop gain adjustment on / off is performed so that the speed difference output from the second comparison unit is output to the speed loop gain adjustment unit (607). By controlling the off switch (605), the limb is expressed by the following equation (m + M) d. ² x / dt ² + (D + Kv ′) dx / dt = −fe (where m: mass of the limb, M: mass of the link, D: viscosity of the training device, Kv ′: the second speed loop gain, fe: the above The limb is operated according to a model expressed by a force received from the link, x: a fixed position of the limb, t: time).
[0007]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 is a block diagram of an emergency stop device for a training apparatus according to a first embodiment of the present invention. In addition, the same code | symbol is attached | subjected about the component same in the block diagram of the emergency stop apparatus of the conventional training apparatus shown in FIG. 8, and description is abbreviate | omitted.
In FIG. 1, 103 is a position loop on / off switch, 104 is an emergency stop signal monitoring unit, 106 is a torque limit on / off switch, 107 is a torque limit setting unit, 108 is a gravity compensation setting unit, 117 is an emergency stop signal, 118 , 119 are first and second contacts of the torque limit on / off switch 106, 121 is a torque command after torque limit, and 122 is a torque command after gravity compensation. In addition, in FIG. 1, although the block diagram of the emergency stop apparatus of a training apparatus was shown only about the servomotor 112A, it is the same also about other servomotors 112B-112C.
First, the operation at the time of the normal operation of the training apparatus (that is, a state that does not require an emergency stop) will be described.
The emergency stop signal monitoring unit 104 monitors whether the emergency stop signal 117 is input, and controls the position loop on / off switch 103 and the torque limit on / off switch 106 according to the presence / absence of the emergency stop signal 117. During normal operation, that is, when the emergency stop signal 117 is not input, the position loop on / off switch 103 is turned on (connected state), and the torque limit on / off switch 106 is turned off (first contact). 118). This state is the same as in the normal operation of the training apparatus described with reference to FIG. 8 in the prior art.
[0008]
Next, at the time of emergency stop of the training device, for example, when the force applied to the limb exceeds a predetermined value, or when the movement speed of the limb exceeds a predetermined value (ie, a state that requires an emergency stop) The operation at the time will be described first. The operations of the position command generation unit 101, the position loop gain (Kp) unit 102, the speed loop gain (Kv) unit 105, the torque controller 109, the speed detector 110, and the position detector 111 are illustrated in the related art. 8 are the same as those described with reference to FIG.
As described above, the emergency stop signal monitoring unit 104 monitors whether the emergency stop signal 117 is input, and controls the position loop on / off switch 103 and the torque limit on / off switch 106 according to the presence / absence of the emergency stop signal 117. For example, when the force applied to the limb (leg) 113 exceeds a predetermined value, or when the operation speed of the limb (leg) 113 exceeds a predetermined value (that is, during an emergency stop), the emergency stop signal 117 Is input to the emergency stop signal monitoring unit 104. When the emergency stop signal 117 is input to the emergency stop signal monitoring unit 104, the position loop on / off switch 103 is off (the position loop is disconnected), and the torque limit on / off switch 106 is on. (A state where the second contact point 119 is connected).
Therefore, the speed command 116 that is the output of the position loop gain (Kp) unit 102 is 0 when taking a difference from the speed 123 of the servo motor 112A that is the output of the speed detector 110 that detects the speed of the servo motor 112A. The difference between 0 and the speed 123 of the servo motor 112 A is input to the speed loop gain (Kv) unit 105.
From the description of the emergency stop signal monitoring unit 104 described above, the torque command 120 that is the output of the speed loop gain (Kv) unit 105 is input to the torque limit setting unit 107 during an emergency stop. The torque limit setting unit 107 monitors the value of the torque command 120 input, and limits the value of the torque command 120 when the torque command 120 exceeds a predetermined value (that is, exceeds the torque limit value). If it is within the predetermined value, no operation is given to the torque command 120.
The output of the torque limit setting unit 107 (torque command after torque limit 121) is a torque limit value when limited by the torque limit setting unit 107, and the torque command input to the torque limit setting unit 107 when not limited. 120.
[0009]
FIG. 2 shows a conceptual diagram of the operation of the torque limit setting unit 107. In FIG. 2, 201 is an input torque command (horizontal axis), 202 is an output torque command (vertical axis), 203 is an origin, 204 is a positive torque limit value, 205 is a negative torque limit value, and 206 is a positive torque limit. , 207 is a negative torque limit point, 208 is a torque line A relating the input torque command 201 and the output torque command 202, and 209 is different from the torque line A208 relating the input torque command 201 and the output torque command 202. Torque line B.
In the torque line A208, the input torque command 201 and the output torque command 202 have a proportional relationship of coefficient 1 between the negative torque limit point 207 and the positive torque limit point 206 via the origin 203, and the negative torque limit When the input torque command 201 is smaller than the point 207, the negative torque limit value 205 is parallel to the horizontal axis, and when the input torque command 201 is larger than the positive torque limit point 206, the positive torque limit value 204 is parallel to the horizontal axis. It becomes.
Therefore, if the torque command 120 input to the torque limit setting unit 107 is equal to or less than the positive torque limit value 204 and negative torque limit value 205, the post-torque limit torque command 121 output from the torque limit setting unit 107 is It becomes equal to the input torque command 120.
[0010]
When the torque command 120 input to the torque limit setting unit 107 is larger than the positive torque limit value 204 or smaller than the negative torque limit value 205, the input torque command 120 is the positive torque The post-torque limit torque command 121 that is suppressed to the limit value 204 and the negative torque limit value 205 and is output from the torque limit setting unit 107 becomes the positive torque limit value 204 and the negative torque limit value 205, respectively.
Both the positive side torque limit value 204 and the negative side torque limit value 205 can be set to have equal absolute values, or can be set independently. Further, the positive side torque limit value 204 and the negative side torque limit value 205 are variable.
Furthermore, the positive side torque limit value 204 and the negative side torque limit value 205 are variable as the position or posture of the training apparatus 114 or the limb body (leg) 113 or the change of the position and posture. In the above description, the relationship between the input torque command 201 and the output torque command 202 is the one shown by the torque line A208. However, the relationship shown by the torque line B209 is not limited to this. Also good.
[0011]
In the gravity compensation setting unit 108, the training device 114 and the limbs (legs) 113 are prevented so that the training device 114 does not perform a falling motion by the sum of the weight of the training device 114 and the weight of the limbs (legs) 113 attached to the training device 114. The torque command value (torque command value for gravity compensation) to be supplied to the servo motor 112A is set to compensate for the gravity component of the torque, and after the torque limit which is the torque command value for the gravity compensation and the output of the torque limit setting unit 107 The torque command 121 is added. The output of the gravity compensation setting unit 108 is a torque command 122 after gravity compensation.
The torque command value for gravity compensation is variable as the position or posture of the training apparatus 114 or the limb (leg) 113 or the change of the position and posture. An after-gravity compensation torque command 122 that is an output of the gravity compensation setting unit 108 is input to the torque controller 109.
The above description of the operation during an emergency operation is the same for the servo motors 112B to 112C. The training apparatus 114 is operated by driving these servo motors 112A to 112C, and the limbs ( Leg) 113 is exercised.
[0012]
Next, the actual movement state at the time of emergency stop of the training apparatus will be described. For the sake of simplicity, the description will be given here with one servo motor. FIG. 3 shows a model diagram of one degree of freedom of the training apparatus with one servo motor.
In FIG. 3, 301 is a motor, 302 is a motor output shaft, 303 is a reduction gear, 304 is a reduction gear output shaft, 305 is a link (mass M), 306 is a training device (model), 307 is an object (mass m), 308 is an X-axis direction in which the object 307 moves, 309 is a force (fe) that the object 307 receives from the link 305, 310 is a reaction of the force 309 that the link 305 receives from the object 307, that is, the force 309 that the object 307 receives from the link 305 (-fe ).
The motor output shaft 302 is driven by driving the motor 301, the speed reducer 303 is driven by driving the motor output shaft 302, and the speed reducer output shaft 304 is driven by driving the speed reducer 303. Assuming that the reduction ratio of the reduction gear 303 is 1: k, the rotation of the reduction gear output shaft 304 is 1 / k of the rotation of the motor output shaft 302 and generates k times the torque.
The link 305 fixed to the speed reducer output shaft 304 is driven by driving the speed reducer output shaft 304, and the object 307 installed on the link 305 is driven by driving the link 305. In comparison with the block diagram shown in FIG. 1, the motor 301 corresponds to the servo motor 112 A, the training device (model) 306 corresponds to the training device 114, and the object 307 corresponds to the limb body (leg) 113.
[0013]
Using the one-degree-of-freedom model of the training apparatus shown in FIG. 3, the emergency stop signal 117 is input to the emergency stop signal monitoring unit 104 at the time of emergency stop in the block diagram shown in FIG. The motion state when the switch 103 and the torque limit on / off switch 106 are controlled by the emergency stop signal monitoring unit 104 will be described.
First, in order to compare with the case where the torque limit setting unit 107 has a torque limit value set, the position loop on / off switch 103 is in an off state (the position loop is disconnected) and the torque limit on / off switch 106 is Assume a motion state in an off state (a state where the first contact point 118 is connected). FIG. 4 shows this movement state, that is, the movement state when the position loop is cut but the torque limit function is not provided.
[0014]
In FIG. 4, (a) is a speed diagram representing a speed command and an object speed over time, (b) is a torque diagram representing a torque command over time, and (c) is a force acting on an object over time. FIG. In addition, 401 is a speed, 402 is a time, 403 is a speed command of a control device (not shown) of a training device (model) 306, 404 (broken line) is a speed of an object 307, 405 is a torque, and 407 is a training device (model). 306 is a torque command of the control device 306, 408 is a force, 410 is a force 309 that the object 307 receives from the link.
In FIG. 1, when the emergency stop signal 117 is input to the emergency stop signal monitoring unit 104, the position loop on / off switch 103 is immediately turned off and the position loop is disconnected. Accordingly, the speed command 403 of the control device of the training device (model) 306 immediately becomes 0 as shown in FIG. If the speed command 403 of the control device of the training device (model) 306 immediately becomes 0, in this case, since the torque limit on / off switch 106 in FIG. 1 is in an off state, the output of the speed loop gain (Kv) unit 105 A torque command 120, that is, a torque command 407 of the control device of the training device (model) 306 is as shown in FIG. The force 309 that the object 307 receives from the link 305 is as shown in FIG.
[0015]
When the position loop on / off switch 103 is off (the position loop is disconnected) and the torque limit on / off switch 106 is off (connected to the first contact 118), that is, When the position loop is cut but there is no torque limit function, the mass of the link 305 is M, the mass of the object 307 is m, the viscosity of the training device (model) 306 is D, and the velocity loop gain is Kv. The equation of motion of the training device (model) 306 is expressed by the following equation.
(M + M) d²x / dt²+ (D + Kv) dx / dt = -fe (Formula 1)
The third method at the time of emergency stop that sets the speed command 116 supplied to the servo amplifier 805 to 0 explained in the prior art is equivalent to the case where the position loop is cut and the torque limit function is not provided. ).
Next, when a torque limit value is set in the torque limit setting unit 107, that is, the position loop on / off switch 103 is in an off state (a state loop is disconnected) and the torque limit on / off switch 106 is on. A state of motion (a state where the second contact point 119 is connected) is assumed. FIG. 5 shows the motion state, that is, the motion state when the position loop is cut and the torque limit function is provided.
[0016]
In FIG. 5, as in FIG. 4, (a) is a speed diagram representing a speed command and object speed over time, (b) is a torque diagram representing a torque command over time, and (c) is an object over time. It is a force line diagram showing the force which acts on. Reference numeral 503 denotes a speed command of the control device of the training device (model) 306, 504 (broken line) denotes the speed of the object 307, 507 denotes a torque command of the control device of the training device (model) 306, and 510 denotes the object 307 from the link. Force 309.
In FIG. 1, when the emergency stop signal 117 is input to the emergency stop signal monitoring unit 104, the position loop on / off switch 103 is immediately turned off, the position loop is disconnected, and the torque limit on / off switch 106 is turned on. The torque limit setting unit 107 becomes effective.
Accordingly, since the position loop is immediately cut, the speed command 503 of the control device of the training device (model) 306 is immediately zero as shown in FIG. When the speed command 503 of the control device of the training device (model) 306 immediately becomes 0, when there is no torque limit function, the torque command 407 of the control device of the training device (model) 306 is as shown in FIG. However, since the torque limit on / off switch 106 in FIG. 1 is in the on state, the torque command 120 which is the output of the speed loop gain (Kv) unit 105, that is, the torque command 507 of the control device of the training device (model) 306 is When the torque limit setting unit 107 exceeds the torque limit value, the torque limit is applied and the torque limit value cannot be exceeded. Further, when the torque command 407 of the control device of the training device (model) 306 is within the torque limit value, no operation is performed and the value remains as it is. Therefore, the torque command 507 shown in FIG.
[0017]
The force that the object 307 in FIG. 3 corresponding to the limb (leg) in FIG. 1 receives from the link is the force 510 that the object 307 shown in FIG. 5C receives from the link, and the object without the torque limit shown in FIG. Compared with the force 410 received by the link 307 from the link, the force acting on the object 307 can be significantly suppressed.
Note that the position loop is turned off by the position loop on / off switch 103 in FIG. 1 at the time of emergency stop because the training device 114 returns to the original position or posture or the position and posture according to the position command 115 at the time of emergency stop. This is to prevent it. When the position loop on / off switch 103 is off (the position loop is disconnected) and the torque limit on / off switch 106 is on (connected to the second contact 119), that is, When the position loop is cut and the torque limit function is provided, the equation of motion of the training device (model) 306 is expressed as follows when the torque command 507 of the control device of the training device (model) 306 is within the torque limit value. When the torque limit value is exceeded in 1), it is expressed by the following equation.
(M + M) d²x / dt²+ Ddx / dt = -fe-f_lim・ Sign (dx / dt) (Formula 2)
Where f_limIndicates the reduction ratio of the reduction gear 303 is 1: k, the length of the link 305 is R, and the torque limit value set by the torque limit setting unit 107 is τ._limWhen
f_lim= KR · τ_lim (Formula 3)
That is, the torque limit value τ set by the torque limit setting unit 107_limThe force limit value of the force 309 that the object 307 receives from the link 305 (f_lim). sign () is a sign function. Accordingly, when this position loop is cut and the torque limit function is provided and the torque command 507 of the control device of the training device (model) 306 exceeds the torque limit value, the training device (model) 306 is given a Ddx / dt And the viscous force expressed by_limThe operation is in accordance with the sum of the dynamic friction force represented by sign (dx / dt).
In the first method at the time of emergency stop for shutting off the power supplied to the servo amplifier 805 described in the prior art, the training device operates only in accordance with the viscous friction force represented by Ddx / dt. It becomes difficult to operate as compared with the first method.
[0018]
Furthermore, the exercise state after the emergency stop of the training apparatus will be described. In FIG. 1, after the emergency stop, the position loop on / off switch 103 is off (the position loop is disconnected) and the torque limit on / off switch 106 is on (connected to the second contact 119). In other words, the position loop is cut and the torque limit function is provided. Therefore, if the torque command 120 is within the torque limit value after an emergency stop, the same state as in (Equation 1), that is, the third command at the time of emergency stop, in which the speed command 116 supplied to the servo amplifier 805 described in the prior art is 0 It becomes the same state as the method, and the training device 114 is difficult to operate. However, when the torque command 120 exceeds the torque limit value by increasing the force that the limb (leg) 113 applies to the training device 114, the training device 114 can start to move according to (Equation 2). The motion state of the training device 114 after starting to move is the same as the motion state of the training device at the time of emergency stop described in the first embodiment.
[0019]
FIG. 6 is a block diagram of the emergency stop device of the training device according to the second embodiment of the present invention. In addition, the component in the block diagram of the emergency stop apparatus of the conventional training apparatus shown in FIG. 8 and the component in the block diagram of the emergency stop apparatus of the training apparatus of 1st Example of this invention shown in FIG. 1 are the same. The same reference numerals are assigned to the descriptions of the objects, and the description is omitted. In FIG. 6, 604 is an emergency stop signal monitoring unit, 605 is a speed loop gain (Kv) adjustment on / off switch, 607 is a speed loop gain (Kv ′) adjustment unit, and 618 and 619 are speed loop gain adjustment on / off switches. Reference numeral 621 denotes a first and second contact 605, and a torque command after speed loop gain adjustment. In addition, in FIG. 6, although the block diagram of the emergency stop apparatus of a training apparatus was shown only about the servomotor 112A, it is the same also about other servomotors 112B-112C.
First, the operation at the time of the normal operation of the training apparatus (that is, a state that does not require an emergency stop) will be described. The emergency stop signal monitoring unit 604 monitors whether the emergency stop signal 117 is input, and controls the position loop on / off switch 103 and the speed loop gain adjustment on / off switch 605 according to the presence / absence of the emergency stop signal 117. During normal operation, that is, when the emergency stop signal 117 is not input, the position loop on / off switch 103 is in an on state (connected state), and the speed loop gain adjustment on / off switch 605 is in an off state (first state). Connected to the contact 618 of FIG. This state is the same as in the normal operation of the training apparatus described with reference to FIG. 8 in the prior art.
[0020]
Next, at the time of emergency stop of the training device, for example, when the force applied to the limb exceeds a predetermined value, or when the movement speed of the limb exceeds a predetermined value (ie, a state that requires an emergency stop) The operation at the time will be described first. Note that the position command generation unit 101, the position loop gain (Kp) unit 102, the position loop on / off switch 103, the speed loop gain (Kv) unit 105, the gravity compensation setting unit 108, the torque controller 109, the speed detector 110, the position Each operation of the detector 111 is the same as each component described with reference to FIG. 8 of the prior art and FIG. 1 of the first embodiment. As described above, the emergency stop signal monitoring unit 604 monitors the presence / absence of the emergency stop signal 117 and controls the position loop on / off switch 103 and the speed loop gain adjustment on / off switch 605 according to the presence / absence of the emergency stop signal 117. Do. For example, when the force applied to the limb (leg) 113 exceeds a predetermined value, or when the operation speed of the limb (leg) 113 exceeds a predetermined value (that is, during an emergency stop), the emergency stop signal 117 Is input to the emergency stop signal monitoring unit 604.
When the emergency stop signal 117 is input to the emergency stop signal monitoring unit 604, the position loop on / off switch 103 is turned off (the position loop is disconnected), and the speed loop gain adjustment on / off switch 605 is turned on. (The state connected to the second contact 619). Therefore, the speed command 116 that is the output of the position loop gain (Kp) unit 102 is 0 when taking a difference from the speed 123 of the servo motor 112A that is the output of the speed detector 110 that detects the speed of the servo motor 112A. The difference between 0 and the speed 123 of the servo motor 112A is input to the speed loop gain adjustment on / off switch 605.
[0021]
From the above description of the emergency stop signal monitoring unit 604, the difference between 0 and the speed 123 of the servo motor 112A is input to the speed loop gain (Kv ') adjustment unit 607 at the time of emergency stop. The speed loop gain (Kv ′) adjustment unit 607 has a function of adjusting the value of the speed loop gain Kv ′, and can reduce the value of the speed loop gain Kv that the speed loop gain (Kv) unit 105 has. That is, the following equation is satisfied.
Kv ′ <Kv (Formula 4)
The value of the speed loop gain Kv ′ that can be adjusted here is variable. The value of the speed loop gain Kv ′ that can be adjusted here is variable in accordance with the position or posture of the training device 114 or the limb (leg) 113 or a change in the position and posture.
The speed loop gain (Kv ′) adjustment unit 607 multiplies the speed loop gain Kv ′ adjusted to the difference between 0 input and the speed 123 of the servo motor 112 A, and outputs it as a speed command after adjusting the speed loop gain 621. In the gravity compensation setting unit 108, the training device 114 and the limbs (legs) 113 are prevented so that the training device 114 does not perform a falling motion by the sum of the weight of the training device 114 and the weight of the limbs (legs) 113 attached to the training device 114. The torque command value (torque command value for gravity compensation) supplied to the servo motor 112A is set to compensate for the gravity component, and the torque command value for gravity compensation and the output of the speed loop gain (Kv ′) adjustment unit 607 are set. The torque command 621 after adjusting the speed loop gain is added.
The output of the gravity compensation setting unit 108 is a torque command 122 after gravity compensation. The torque command value for gravity compensation is variable as the position or posture of the training device 114 or the limb (leg) 113 or a change in the position and posture. An after-gravity compensation torque command 122 that is an output of the gravity compensation setting unit 108 is input to the torque controller 109.
The above description of the operation during an emergency operation is the same for the servo motors 112B to 112C. The training apparatus 114 is operated by driving these servo motors 112A to 112C, and the limbs ( Leg) 113 is exercised. Next, the actual movement state at the time of emergency stop of the training apparatus will be described.
[0022]
For simplicity, the one-degree-of-freedom model of the training apparatus with one servo motor shown in FIG. 3 is used here. In the emergency stop of the block diagram shown in FIG. 6, that is, the emergency stop signal 117 is input to the emergency stop signal monitoring unit 604, and the position loop on / off switch 103 and the speed loop gain adjustment on / off switch 605 monitor the emergency stop signal. The motion state when controlled by the unit 104 will be described.
First, the position loop on / off switch 103 is in an off state (a state in which the position loop is disconnected) in order to compare with the case where the speed loop gain Kv ′ adjustment unit 607 has a function of adjusting the value of the speed loop gain Kv ′. And a motion state in which the speed loop gain adjustment on / off switch 605 is off (connected to the first contact 618), this state, that is, the position loop is disconnected but the speed loop gain is set. When there is no adjustment function, the position loop described in the first embodiment is cut, but it is the same as when the torque limit function is not provided. FIG. 4 shows the state of motion when there is nothing.
Therefore, the force 309 that the object 307 receives from the link 305 in FIG. 3 is as shown in FIG. The equation of motion of the training device (model) 306 is also (Equation 1) representing the equation of motion of the training device (model) 306 when the position loop is cut but the torque limit function is not provided.
Next, when there is a function for adjusting the value of the speed loop gain Kv ′ in the speed loop gain (Kv ′) adjustment unit 607, that is, the position loop on / off switch 103 is in an off state (a state where the position loop is disconnected) and Assume a motion state in which the speed loop gain adjustment on / off switch 605 is on (connected to the second contact 619). FIG. 7 shows this motion state, that is, the motion state with the position loop cut and the speed loop gain adjustment function.
[0023]
In FIG. 7, as in FIGS. 4 and 5, (a) is a speed diagram representing the speed command and the object speed over time, (b) is a torque diagram representing the torque command over time, and (c) is time. It is a force line diagram showing the force which acts on the object with respect to progress. Reference numeral 703 denotes a speed command of the control device of the training device (model) 306, reference numeral 704 (broken line) denotes a speed of the object 307, reference numeral 707 denotes a torque command of the control device of the training device (model) 306, and reference numeral 710 denotes an object 307 received from the link. Force 309.
In FIG. 6, when the emergency stop signal 117 is input to the emergency stop signal monitoring unit 604, the position loop on / off switch 103 is immediately turned off, the position loop is disconnected, and the speed loop gain adjustment on / off switch 605 is set. Turned on and the speed loop gain (Kv ′) adjustment unit 607 becomes effective. Therefore, since the position loop is immediately cut, the speed command 703 of the control device of the training device (model) 306 is immediately zero as shown in FIG. When the speed command 703 of the control device of the training device (model) 306 immediately becomes 0, when there is no speed loop gain adjustment function, the torque command 407 of the control device of the training device (model) 306 is shown in FIG. As shown in FIG. 6, since the speed loop gain adjustment on / off switch 605 in FIG. 6 is on, the difference between 0 and the speed 123 of the servo motor 112A is the speed loop gain Kv of the speed loop gain (Kv) unit 105. Is multiplied by the value of the speed loop gain Kv ′ smaller than the value of, and the torque command 621 after the speed loop gain adjustment is suppressed to a value smaller than the torque command 120. Therefore, the torque command 707 shown in FIG.
The force received by the object 307 in FIG. 3 corresponding to the limb (leg) in FIG. 6 is the force 710 that the object 307 shown in FIG. 7C receives from the link, and there is no speed loop gain adjustment function shown in FIG. Compared with the force 410 that the object 307 receives from the link, the force acting on the object can be suppressed considerably.
[0024]
Note that the position loop is turned off by the position loop on / off switch 103 in FIG. 6 at the time of emergency stop because the training device 114 returns to the original position or posture or the position and posture according to the position command 115 at the time of emergency stop. This is to prevent it. When the position loop on / off switch 103 is off (position loop is disconnected) and the speed loop gain adjustment on / off switch 605 is on (connected to the second contact 619) That is, when the position loop is cut and the speed loop gain adjustment function is provided, the equation of motion of the training device (model) 306 is expressed by (Expression 1) in which the speed loop gain Kv is Kv ′.
Accordingly, in a state where the position loop is cut and the speed loop gain adjustment function is provided, the training device (model) 306 operates according to the viscosity of (D + Kv ′). In the first method at the time of emergency stop that cuts off the power supplied to the servo amplifier 805 described in the prior art, the training device (model) 306 operates according to the viscosity (D), so this position loop is disconnected. When the speed loop gain adjustment function is provided, the training apparatus becomes difficult to operate as much as the viscosity increases by Kv ′ as compared with the first method of the prior art.
[0025]
Furthermore, the exercise state after the emergency stop of the training apparatus will be described. In FIG. 6, after the emergency stop, the position loop on / off switch 103 is off (the position loop is disconnected) and the speed loop gain adjustment on / off switch 605 is on (connected to the second contact 619). In other words, the position loop is cut and the speed loop gain adjustment function is provided. Therefore, even after an emergency stop, Kv becomes Kv ′, the same state as in (Equation 1), that is, the training device 114 can start operating according to the viscosity of (D + Kv ′). This is because the viscosity is (D + Kv ′) with respect to (D + Kv) as compared with the third method at the time of emergency stop, in which the speed command 116 supplied to the servo amplifier 805 is set to 0, and the entire equation is obtained from (Equation 4). This is because the viscosity is reduced. The motion state of the training device 114 after starting to move is the same as the motion state of the training device at the time of emergency stop described in the second embodiment.
[0026]
As described in the first embodiment of the present invention, this embodiment has a function that can cut a position loop and a function that can set a torque limit value in a torque command in the control device of the emergency stop device of the training device. Since it is provided, the training apparatus can be stopped without giving a large impact or force to the limbs and the training apparatus at the time of emergency stop. In addition, the state of motion at the time of emergency stop of the training apparatus in the present embodiment is equal to the amount of dynamic friction force compared to the first method at the time of emergency stop that shuts off the power supplied to the servo amplifier, which is a conventional technique. The training device becomes difficult to operate. Furthermore, the exercise state after the emergency stop of the training apparatus in the present embodiment allows the training apparatus to start moving when the torque command exceeds the torque limit value by increasing the force that the limbs give the training apparatus.
As described in the second embodiment of the present invention, this embodiment includes a function capable of cutting the position loop and a function capable of adjusting the value of the speed loop gain in the control device of the emergency stop device of the training device. Therefore, the training apparatus can be stopped without giving a large impact or force to the limbs and the training apparatus during an emergency stop. In addition, the motion state at the time of emergency stop of the training apparatus in the present embodiment is the viscosity of the speed loop gain compared to the first method at the time of emergency stop that shuts off the power supplied to the servo amplifier, which is a conventional technique. Therefore, the training device becomes difficult to operate. Furthermore, in the exercise state after the emergency stop of the training apparatus in the present embodiment, the overall viscosity can be reduced by the value of the speed loop gain, so that the training apparatus can start to move.
[0027]
【The invention's effect】
As described above, the present invention has the following effects.
(1) The emergency stop device control device of the training device has a function that can cut the position loop, a function that can set a torque limit value in the torque command, or a function that can adjust the value of the speed loop gain. Sometimes it becomes possible to stop the training device without giving a large impact or force to the limbs and the training device. Accordingly, it is possible to reduce the risk that the limb is subjected to large acceleration, impact, or force, and to reduce the risk of destruction caused by the training device receiving large acceleration, impact, or force.
(2) The motion state of the training device at the time of emergency stop is higher than that of the first method at the time of emergency stop in which the power supplied to the servo amplifier, which is a conventional technique, is cut off. Therefore, the training apparatus becomes harder to operate, so that the risk of the limb becoming a dangerous position or posture or position and posture can be reduced, and the risk of destruction caused by the training apparatus reaching the operating limit can be reduced.
(3) The exercise state after the emergency stop of the training apparatus can be started when the torque command exceeds the torque limit value by increasing the force that the limb gives the training apparatus. Or since the viscosity which the whole has can be made small by the value of a speed loop gain, a training apparatus can start moving. Therefore, since the limb itself can operate the training device in a position or posture safe for the limb or in the position and posture, the second method at the time of emergency stop for operating the brake of the servo motor, which is a conventional technique, or the servo amplifier Compared with the third method at the time of emergency stop where the supplied speed command is 0, the danger can be reduced.
[Brief description of the drawings]
FIG. 1 is a block diagram of an emergency stop device of a training device according to a first embodiment of the present invention.
FIG. 2 is an operation conceptual diagram of a torque limit setting unit according to the first embodiment of the present invention.
FIG. 3 is a one-degree-of-freedom model diagram of the training apparatus according to the embodiment of the present invention.
FIG. 4 is a graph ((a) speed diagram, (b) torque diagram, (c) force diagram) showing an exercise state without a torque limit function at the time of emergency stop of the training apparatus according to the embodiment of the present invention. ).
FIG. 5 is a graph ((a) speed diagram, (b) torque diagram, (c) representing an exercise state with a torque limit function at the time of emergency stop of the training apparatus according to the first embodiment of the present invention. Field diagram).
FIG. 6 is a block diagram of an emergency stop device of a training device according to a second embodiment of the present invention.
FIG. 7 is a graph ((a) speed diagram, (b) torque diagram, showing a motion state with a speed loop gain adjustment function at the time of emergency stop of the training apparatus according to the second embodiment of the present invention; c) Force diagram.
FIG. 8 is a block diagram of an emergency stop device of a conventional training device.
[Explanation of symbols]
101 Position command generation unit, 102 Position loop gain (Kp) unit, 103 Position loop on / off switch, 104 Emergency stop signal monitoring unit, 105 Speed loop gain (Kv) unit, 106 Torque limit on / off switch, 107 Torque limit setting Part, 108 gravity compensation setting part, 109 torque controller, 110 speed detector, 111 position detector, 112A-112C servo motor, 113 limbs (leg), 114 training device, 115 position instruction, 116 speed instruction, 117 emergency stop Signal, 118 first contact, 119 second contact, 120 torque command, 121 torque command after torque limit, 122 torque command after gravity compensation, 123 speed of servo motor 112A, 124 position of servo motor 112A, 201 input torque command 202 output Torque command, 203 origin, 204 positive torque limit value, 205 negative torque limit value, 206 positive torque limit point, 207 negative torque limit point, 208 torque line A, 209 torque line B, 301 motor, 302 motor output Axis, 303 Reducer, 304 Reducer output shaft, 305 Link (mass M), 306 Training device (model), 307 Object (mass m), 308 X-axis direction in which object 307 moves, 309 Object 307 from link 305 Force received (fe), 310 Force received by link 305 from object 307 (-fe), 401 speed, 402 hours, 403 speed command, 404 object speed, 405 torque, 407 torque command, 408 force, 410 Object receives from link Force, 503 speed command, 504 object speed, 507 torque command , 510 force received by the object from the link, 604 emergency stop signal monitoring unit, 605 speed loop gain adjustment on / off switch, 607 speed loop gain (Kv ′) adjustment unit, 618 first contact, 619 second contact, 621 Torque command after speed loop gain adjustment, 703 Speed command, 704 Object speed, 707 Torque command, 710 Force that object receives from link, 805 Servo amplifier

Claims

A movable mechanism for fixing a part of the configuration limb member by a link,
Drive means (112) for driving the movable mechanism via a speed reducer ;
A position detector (111) for detecting the position of the driving means;
A speed detector (110) for detecting the speed of the drive means;
A position command generator (101) for outputting a position command of the driving means;
A first comparison unit that compares the position command with the detected position and outputs a difference between the positions;
A position loop gain unit (102) that outputs a speed command by multiplying a position loop gain by a position difference output by the first comparison unit;
A second comparison unit that compares the speed command with the detected speed command and outputs a difference between the speed commands;
A speed loop gain unit (105) that outputs a torque command by multiplying the speed difference by a speed loop gain, and a torque controller (109) that drives the driving means according to the torque command ,
In training device Ru operates the limb by operating the moving mechanism,
A position loop on / off switch (103) that is provided on the input side of the second comparison section and interrupts the position loop ;
A torque limit setting unit (107) for limiting and outputting a torque command value output by the speed loop gain unit (105) to a predetermined value ;
A gravity compensation setting unit (108) that is provided on the output side of the torque limit setting unit (107) and outputs a torque command after gravity compensation to prevent the moving mechanism from dropping due to gravity to the torque controller (109). When,
Provided on the output side of the speed loop gain section (105), the output destination of the torque command output by the speed loop gain section (105) is switched to the torque controller (109) or the torque limit setting section (107). Torque limit on / off switch (106) ,
Wherein the position loop on-off switch and the torque limit on-off controlling a switch emergency stop signal monitoring section (104), provided with,
The emergency stop signal monitoring unit (104) immediately receives an emergency stop signal,
The position loop on / off switch (103) is controlled so that the speed command is not output to the second comparison unit, and the torque limit on / off control is performed so that the torque command is output to the torque limit setting unit (107). By controlling the off switch (106),
The limb is
(M + M) d ² x / dt ² + Ddx / dt = −fe−f _lim · sign (dx / dt)
(Where m: mass of the limb, M: mass of the link, D: viscosity of the training device, fe: force that the limb receives from the link, f _lim : reduction ratio of the reducer 1: k Where the length of the link is R, and the torque limit value set by the torque limit setting unit (107) is τ _lim , f _lim = kR · τ _lim , x: limb fixation Position, t: time)
A training apparatus that operates according to a model represented by:

A movable mechanism for fixing a part of the configuration limb member by a link,
Drive means (112) for driving the movable mechanism via a speed reducer ;
A position detector (111) for detecting the position of the driving means;
A speed detector (110) for detecting the speed of the drive means;
A position command generator (101) for outputting a position command of the driving means;
A first comparison unit that compares the position command with the detected position and outputs a difference between the positions;
Position loop gain section for outputting a speed command by multiplying a position loop gain to the difference of the position where the first comparison unit outputs (102),
A second comparison unit that compares the speed command with the detected speed command and outputs a difference between the speed commands;
A speed loop gain unit (105) that outputs a torque command by multiplying a difference in speed output by the second comparison unit by a first speed loop gain;
A torque controller (109) for driving the driving means according to the torque command ,
In training device Ru operates the limb by operating the moving mechanism,
A position loop on / off switch (103) that is provided on the input side of the second comparison section and interrupts the position loop ;
A speed loop gain adjustment unit (607) that outputs a torque command by multiplying a difference in speed output by the second comparison unit by a second speed loop gain smaller than the first speed loop gain ;
A gravity compensation setting unit (108) that is provided on the output side of the speed loop gain adjustment unit (607) and outputs a torque command after gravity compensation to prevent the moving mechanism from dropping due to gravity to the torque controller;
A speed loop gain that is provided on the output side of the second comparison unit and switches the output destination of the speed difference output from the second comparison unit to the speed loop gain unit (105) or the speed loop gain adjustment unit (607). Adjustment on / off switch (605) ;
And a emergency stop signal monitoring section for controlling said position loop on-off switch (103) and the velocity loop gain adjustment on-off switch (605),
When the emergency stop signal is input, the emergency stop signal monitoring unit (604) immediately
The position loop on / off switch (103) is controlled so that the speed command is not output to the second comparison unit, and the speed difference output from the second comparison unit is output to the speed loop gain adjustment unit (607). By controlling the speed loop gain adjustment on / off switch (605) as follows:
The limb is
(M + M) d ² x / dt ² + (D + Kv ′) dx / dt = −fe
(Where m is the mass of the limb, M is the mass of the link, D is the viscosity of the training apparatus, Kv ′ is the second speed loop gain, fe is the force that the limb receives from the link, and x is the limb. Fixed position, t: time)
A training apparatus that operates according to a model represented by: