(57)【要約】【課題】複数のトルクセンサを切り替えて用いる場合に、ＬＰＦによる遅れの影響を最小限にしつつ、トルクセンサによるトルク制御を正確に行うこと。【解決手段】トルク指令出力部は、トルク指令値を第１の周波数によりカットオフして電流制御器へ出力する第１のローパスフィルタを含み、トルク制御装置は、複数のトルクセンサのうち第１のトルクセンサにより検出された第１のトルク値が所定条件を満たす場合に、第２のトルクセンサにより検出された第２のトルク値をトルク指令出力部へ出力するように切り替える切替部を備える。切替部は、第２のトルク値を第２の周波数によりカットオフしてトルク指令出力部へ出力する第２のローパスフィルタを含み、切り替え時から所定時間の間に第２のローパスフィルタを使用し、当該所定時間経過後に第２のローパスフィルタを使用せずに第２のトルク値をトルク指令出力部へ出力し、第２の周波数は、第１の周波数より高い。【選択図】図１
2018106446.tif 000002
【特許請求の範囲】
【請求項１】
先端に負荷が配置され、入力される電流に基づきトルクを出力するモータと、
前記モータが出力したトルクの出力値を検出する複数のトルクセンサと、
前記負荷の目標位置と前記複数のトルクセンサのいずれかが検出したトルクの出力値とに基づきトルク指令値を算出するトルク指令出力部と、
前記トルク指令値に基づき前記電流を制御する電流制御器と、
を備えるトルク制御装置であって、
前記トルク指令出力部は、
前記トルク指令値を第１の周波数によりカットオフして前記電流制御器へ出力する第１のローパスフィルタを含み、
前記トルク制御装置は、
前記複数のトルクセンサのうち第１のトルクセンサにより検出された第１のトルク値が所定条件を満たす場合に、第２のトルクセンサにより検出された第２のトルク値を前記トルク指令出力部へ出力するように切り替える切替部をさらに備え、
前記切替部は、
前記第２のトルク値を第２の周波数によりカットオフして前記トルク指令出力部へ出力する第２のローパスフィルタを含み、
前記切り替え時から所定時間の間に前記第２のローパスフィルタを使用し、当該所定時間経過後に前記第２のローパスフィルタを使用せずに前記第２のトルク値を前記トルク指令出力部へ出力し、
前記第２の周波数は、前記第１の周波数より高い、
トルク制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、トルク制御装置に関する。
【背景技術】
【０００２】
特許文献１には、特定の信号に対してモータへ指令値を出力する際に、モータへ急激な入力値の変動が発生することを抑制するため、信号処理手段として２種類の演算手段を備え、２種類の演算手段を適宜切り替えて使用して信号を処理する制御装置が開示されている。そして、２種類の演算手段よりも後であり、かつ、モータへの指令値を算出するよりも前の工程に、ローパスフィルタ（ＬＰＦ）を備えるものである。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００４−３５２１６４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１にかかる技術であれば、複数のトルクセンサのうち、一つのトルクセンサが故障した際に他のトルクセンサを用いる場合であっても、使用するセンサ状態を切り替えた後にＬＰＦがあるため、センサ毎の出力値の差を減らすことができる。尚、トルクセンサ値以外にも、トルクセンサにより接続された機器同士の共振を除去するためなど、トルクセンサを用いて正確な制御を行うためにＬＰＦを用いる場合がある。
【０００５】
しかし、ＬＰＦの処理後の信号が元の信号から遅れるため、制御システム全体としての処理速度が低下してしまう。そのため、複数あるトルクセンサを切り替えて用いる場合であっても、ＬＰＦによる遅れの影響を最小限にしつつ、トルクセンサによるトルク制御を正確に行うことが困難であった。
【０００６】
本発明は、このような問題を解決するためになされたものであり、複数のトルクセンサを切り替えて用いる場合に、ＬＰＦによる遅れの影響を最小限にしつつ、トルクセンサによるトルク制御を正確に行うためのトルク制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明にかかるトルク制御装置は、先端に負荷が配置され、入力される電流に基づきトルクを出力するモータと、前記モータが出力したトルクの出力値を検出する複数のトルクセンサと、前記負荷の目標位置と前記複数のトルクセンサのいずれかが検出したトルクの出力値とに基づきトルク指令値を算出するトルク指令出力部と、前記トルク指令値に基づき前記電流を制御する電流制御器と、を備える。前記トルク指令出力部は、前記トルク指令値を第１の周波数によりカットオフして前記電流制御部へ出力する第１のローパスフィルタを含み、前記トルク制御装置は、前記複数のトルクセンサのうち第１のトルクセンサにより検出された第１のトルク値が所定条件を満たす場合に、第２のトルクセンサにより検出された第２のトルク値を前記トルク指令出力部へ出力するように切り替える切替部をさらに備え、前記切替部は、前記第２のトルク値を第２の周波数によりカットオフして前記トルク指令出力部へ出力する第２のローパスフィルタを含み、前記切り替え時から所定時間の間に前記第２のローパスフィルタを使用し、当該所定時間経過後に前記第２のローパスフィルタを使用せずに前記第２のトルク値を前記トルク指令出力部へ出力し、前記第２の周波数は、前記第１の周波数より高い。
【発明の効果】
【０００８】
本発明によれば、複数のトルクセンサを切り替えて用いる場合に、ＬＰＦによる遅れの影響を最小限にしつつ、トルクセンサによるトルク制御を正確に行うためのトルク制御装置を提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施の形態１にかかるトルクセンサ制御装置の構成を示すブロック図である。
【図２】本発明の実施の形態１にかかるトルクセンサ制御装置の内部構成を示すブロック図である。
【図３】本発明の実施の形態１にかかるトルクセンサ切替処理の流れを示すフローチャートである。
【発明を実施するための形態】
【００１０】
以下では、上述した各態様を含む本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略する。
【００１１】
＜発明の実施の形態１＞
上述した特許文献１では、２つの制御演算手段の出力を切り替えた後にローパスフィルタ（ＬＰＦ）を入れて、後段のモータ駆動機への入力の急激な変化を抑えている。ここで、制御演算手段の出力を切り替える際に急激な信号の変化が生じる。これを抑制するために、制御演算手段の出力に常に特定のカットオフ周波数のＬＰＦを入れている。但し、出力を切り替えない通常時にも常にＬＰＦが入っているため、常に出力信号に遅れが生じてしまう。
【００１２】
これに対し、本発明は、複数のトルクセンサ値を切り替えてフィードバックする際に、切替時のみローパスフィルタを通すことで、通常時は速いトルクセンサ信号を得て、切替時はトルクセンサ信号の急激な変化を抑制するものである。
【００１３】
図１は、本発明の実施の形態１にかかるトルク制御装置１０の構成を示すブロック図である。また、図２は、本発明の実施の形態１にかかるトルクセンサ制御装置の内部構成を示すブロック図である。以下の説明では、主に図１を参照して説明し、必要に応じて図２を参照するものとする。
【００１４】
トルク制御装置１０は、位置制御器１１と、トルク制御器１２と、電流制御器１３と、モータ１４と、トルクセンサ１５１及び１５２と、ロボットハンド１６と、トルクセンサ切替判断器１７と、トルクセンサ切替器１８とを少なくとも備える。
【００１５】
位置制御器１１は、外部から位置指令Ｐｍを受け付け、モータ１４からロボットハンド１６の現在の位置Ｐを受け付け、位置Ｐを位置指令Ｐｍに近付けるようにトルク指令Ｕｍを生成してトルク制御器１２へ出力する。
【００１６】
トルク制御器１２は、トルク指令出力部の一例であり、トルク指令Ｕｍとトルク値とを受け付け、モータトルク指令τを生成して電流制御器１３へ出力する。言い換えると、トルク制御器１２は、ロボットハンド１６の目標位置と複数のトルクセンサのいずれかが検出したトルク値とに基づきトルク指令値（モータトルク指令τ）を算出する。また、トルク制御器１２は、算出したトルク指令値を第１の周波数によりカットオフして電流制御器１３へ出力する第１のローパスフィルタ（ＬＰＦ１＿１２１）を含む。尚、ＬＰＦ１＿１２１は、トルク制御器１２と電流制御器１３の間に配置されてもよい。
【００１７】
電流制御器１３は、モータトルク指令τに基づきモータ１４へ入力する電流ｉを制御する。具体的には、電流制御器１３は、モータトルク指令τに所定の係数を乗じたものを電流ｉとしてモータ１４へ出力する。
【００１８】
モータ１４は、先端にロボットハンド１６が配置され、入力される電流ｉに基づきトルクτｔを出力する。ロボットハンド１６は、モータ１４の先端に配置された負荷の一例である。そのため、ロボットハンド１６の代わりにロボットアーム、脚等であっても良い。
【００１９】
トルクセンサ１５１及び１５２は、第１及び第２のトルクセンサの一例であり、モータ１４が出力したトルクτｔをそれぞれが計測トルク値τｔ１及びτｔ２として検出する。尚、本発明にかかる複数のトルクセンサは、３以上であってもよい。
【００２０】
トルクセンサ切替判断器１７及びトルクセンサ切替器１８は、切替部の一例である。トルクセンサ切替判断器１７は、トルクセンサ１５１及び１５２からの出力を監視し、現在、出力として選択されているトルクセンサ値（例えば、計測トルク値τｔ１）の異常を検出した場合に、他のトルクセンサ値（例えば、計測トルク値τｔ２）を選択するようにトルクセンサ切替器１８（のスイッチ部１８１）を制御する。ここで、異常の検出とは、例えば、トルクセンサ１５１及び１５２の出力である計測トルク値τｔ１及びτｔ２を比較し、差分が閾値以上の場合、計測トルク値τｔ１の出力だけゼロ値が続く場合、計測トルク値τｔ１が上限値又は下限値が続く場合、又は、トルクセンサ１５１との通信が遮断する場合等が挙げられる。
【００２１】
トルクセンサ切替器１８は、トルクセンサ切替判断器１７によるトルクセンサの出力の選択指示に基づいて、トルクセンサの出力を選択して切り替える。また、トルクセンサ切替器１８は、トルク値を第２の周波数によりカットオフしてトルク制御器１２へ出力する第２のローパスフィルタ（ＬＰＦ２＿１８３）を含む。ここで、第２のローパスフィルタにおける第２の周波数は、上述した第１のローパスフィルタにおける第１の周波数よりも高いものとする。よって、第２のローパスフィルタによる信号の遅れが小さいため、トルク制御器１２における信号の遅れの影響を小さくすることができる。
【００２２】
トルクセンサ切替器１８は、トルクセンサの切替（すなわち、スイッチ部１８１による切替）時から所定時間の間、ＬＰＦ２＿１８３を用いて計測トルクをフィルタする。そして、トルクセンサ切替器１８は、所定時間の経過後に、ＬＰＦ２＿１８３を用いずに計測トルクをトルク制御器１２へ出力する。例えば、トルクセンサ切替器１８は、スイッチ部１８２及びスイッチ部１８４について、トルクセンサの切替時から所定時間の間、ＬＰＦ２＿１８３を用いるように設定し、所定時間経過後に、ＬＰＦ２＿１８３を用いないように設定する。尚、トルクセンサ切替器１８は、スイッチ部１８１による切替がない場合、スイッチ部１８２及びスイッチ部１８４について、ＬＰＦ２＿１８３を用いないように設定し、維持する。尚、所定時間は、ＬＰＦ２＿１８３の時定数から算出される値とする。
【００２３】
図３は、本発明の実施の形態１にかかるトルクセンサ切替処理の流れを示すフローチャートである。まず、トルクセンサ切替判断器１７は、トルクセンサ値が異常か否かを判定する（Ｓ１１）。トルクセンサ値が異常と判定された場合、トルクセンサ切替判断器１７は、トルクセンサを切り替えるようにトルクセンサ切替器１８に指示を行う。これに応じて、トルクセンサ切替器１８のスイッチ部１８１は、トルクセンサの出力の切替を行う（Ｓ１２）。例えば、スイッチ部１８１がトルクセンサ１５１の出力を選択している場合、トルクセンサ切替器１８は、トルクセンサ１５１により検出された第１のトルク値（例えば、計測トルク値τｔ１）が所定条件を満たす場合に、トルクセンサ１５２により検出された第２のトルク値（例えば、計測トルク値τｔ２）をスイッチ部１８２へ出力するようにスイッチ部１８１を切り替える。
【００２４】
また、ステップＳ１１においてトルクセンサ値が異常と判定された場合、トルクセンサ切替器１８は、ステップＳ１２と並行して、ＬＰＦ２＿１８３を通すようにスイッチ部１８２及び１８４を切り替える（Ｓ１３）。その後、トルクセンサ切替器１８は、所定時間経過したか否かを判定する（Ｓ１４）。所定時間経過後、トルクセンサ切替器１８は、ＬＰＦ２＿１８３を通さないようにスイッチ部１８２及び１８４を切り替える（Ｓ１５）。
【００２５】
尚、ステップＳ１４に代えて、トルクセンサ切替器１８は、トルクセンサ値がＬＰＦ２＿１８３の通過前後でほぼ一致するか否かを判定条件としてもよい。または、ステップＳ１３の前に、トルクセンサ切替器１８は、スイッチ部１８１の切替時に出力されるトルクセンサ値の変化の大きさが閾値以下であれば、ステップＳ１３からＳ１５を行わず、スイッチ部１８２及び１８４について、ＬＰＦ２＿１８３を用いないように設定し、維持してもよい。
【００２６】
このように、本実施の形態では、トルクセンサの切替時に信号に急激な変化生じた場合のみＬＰＦを用いるため、通常時は、不要な遅延が生じにくい。また、トルクセンサの切替時に信号に急激な変化生じた場合でも、ＬＰＦ２＿１８３を用いることで後段のトルク制御器１２への信号入力の急激な変化を防ぎ、制御演算の飽和や発振を防ぐことができる。
【００２７】
尚、図２に示すように、トルク制御器１２は、摩擦トルク推定オブザーバ１２２を内蔵している。摩擦トルク推定オブザーバ１２２は、トルクセンサの摩擦をキャンセルするものである。そして、本実施の形態にかかるトルク制御装置１０は、トルクτｔの後段にトルクセンサ切替器１８が配置されており、トルクセンサ１５１に異常が生じた場合にはトルクセンサ１５２に切り替え、計測トルク値τｔ２を出力する。このとき、切替時から所定時間だけはＬＰＦ２＿１８３に通した信号を出力する。これにより、切替時には摩擦トルク推定オブザーバ１２２への入力の急激な変化を抑制でき、摩擦トルク推定オブザーバ１２２の演算の飽和や発振などを防ぐことができる。つまり、トルクフィードバック信号（トルクセンサ切替器１８からの出力信号）の急激な変化も抑制できるため、トルク制御系全体の演算飽和や発振などを防ぐことができる。
【００２８】
また、トルクセンサの切替を行わない通常時には、トルクセンサ値（計測トルク）をＬＰＦ２＿１８３に通さないため、トルクセンサ本来の速い信号を摩擦トルク推定オブザーバ１２２に入力できる。そのため、摩擦トルク推定オブザーバ１２２の推定誤差を抑えられる。また、トルクフィードバック信号の遅れもないため、トルク制御系全体の応答が速くなる。
【００２９】
尚、摩擦トルク推定オブザーバ１２２内には、常時利用されるＬＰＦ相当が含まれる。そして、当該ＬＰＦの周波数は、第１の周波数と第２の周波数の間であることが望ましい。これにより、トルク制御をより安定化させることができる。
【００３０】
尚、本実施の形態の特徴を言い換えると次のようになる。すなわち、トルクセンサを複数備え、トルク指令値から電流指令値にする際に使用する第１のＬＰＦと、一つのトルクセンサが故障した場合など、使用するトルクセンサを切り替える場合にのみ使用する第２のＬＰＦと、を備え、二つのＬＰＦのうち、切替時に使用する第２のＬＰＦの方が高い周波数帯をカットオフするものである。
【００３１】
このように、第１のＬＰＦを備えることで、機械の共振の発生を抑えられる。また、第２のＬＰＦを切替時のみ使用するようにすることで、必要なときのみ第２のＬＰＦを通すことができ、必要以上に第２のＬＰＦにより処理されることを抑制できる。
【００３２】
さらに、第２のＬＰＦは、トルクセンサ出力に乗っている電気的なノイズ成分をカットオフするようにされているため、第２のＬＰＦを用いる場合であっても必要以上にカットオフする処理を行わなくて済む。
【００３３】
以上より、複数あるトルクセンサを切り替えて用いる場合であっても第２のＬＰＦによる遅れの影響を最小限にしつつ、トルクセンサによるトルク制御を正確に行うことができる。
【００３４】
＜発明の実施の形態２＞
本発明の実施の形態２は、上述した実施の形態１の変形例である。トルクセンサ１５１及び１５２の両方とも故障した場合に、トルクセンサ値の代わりに電流値を用いたトルク推定値によりトルク制御を行うものである。例えば、切替部は、第１のトルクセンサ及び第２のトルクセンサの出力に加え、電流値を用いてトルク推定値を算出する第３のトルクセンサ（不図示）の出力を受け付ける。そして、第１から第３のトルクセンサによりそれぞれ検出されたトルクセンサ値に基づいて、トルクセンサの切替を行う。すなわち、第１又は第２のトルクセンサから第３のトルクセンサに切り替える場合に、トルク推定値を所定時間の間、第２のＬＰＦに通すものである。これにより、切替時の出力の急激な変化をさらに抑えることができる。特に、第１又は第２のトルクセンサから第３のトルクセンサに切り替える場合は、値の急激な変化が大きいため、当該手法が効果的である。
【００３５】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００３６】
１０  トルク制御装置
１１  位置制御器
１２  トルク制御器
１２１  ＬＰＦ１
１２２  摩擦トルク推定オブザーバ
１３  電流制御器
１４  モータ
１５１  トルクセンサ
１５２  トルクセンサ
１６  ロボットハンド
１７  トルクセンサ切替判断器
１８  トルクセンサ切替器
１８１  スイッチ部
１８２  スイッチ部
１８３  ＬＰＦ２
１８４  スイッチ部
Ｋｔ  トルクフィードバック係数
Ｋｉ  トルク定数
Ｋｔｓ  トルクセンサ剛性
Ｋε１  トルクセンサ計測換算係数1
Ｋε２  トルクセンサ計測換算係数2
ｉ  モータ電流
Ｒｍ  モータ抵抗
Ｌｍ  モータインダクタンス
Ｊｍ  モータイナーシャ
Ｊｍｎ  モータノミナルイナーシャ
Ｊｌ  負荷イナーシャ
Ｌ  オブザーバゲイン
Ｎ  減速比
Ｕｍ  トルク指令
θｍ  モータ角度
θｌ  負荷角度
＾θｍ  モータ角度の推定値
τｍ  中間トルク指令
τｆ  摩擦トルク（外乱）
τ  モータトルク指令
τｒ  モータ出力トルク
τｔ  トルク
τｅｘｔ  外力トルク（環境）
τｇ  重力トルク
τｔ１  計測トルク値
τｔ２  計測トルク値
＾τｆ  推定摩擦トルク
Ｐｍ  位置指令
Ｐ  位置
【図１】
2018106446.tif 000003
【図２】
2018106446.tif 000004
【図３】
2018106446.tif 000005