(57)【特許請求の範囲】
【請求項１】
アクチュエータによって相対的に駆動される一対の部材を備えている関節機構の制御装置であって、
メジャーループである前記関節機構の位置制御ループに対して位置指令を入力する位置指令入力要素と、
前記関節機構の応答性向上のための第１指令を、前記関節機構の位置制御ループのマイナーループに対して入力する第１指令入力要素と、
前記関節機構の実位置を検知する位置検知要素と、
前記位置検知要素により検知された前記関節機構の実位置と限界位置との偏差である変位許容量が閾値以下の範囲において減少していることを要件として、第１指定値を連続的または断続的に低下させながら、前記第１指令入力要素に前記第１指令を前記第１指定値以下に制限させた上で前記マイナーループに入力させ、かつ、前記関節機構の変位許容量が０に到達したことを要件として、前記位置指令入力要素に前記位置指令が前記限界位置を越えないように前記位置指令を制限させた上で前記メジャーループに入力させるリミッタ調節要素と、を備えていることを特徴とする関節機構の制御装置。
【請求項２】
請求項１記載の関節機構の制御装置において、
前記リミッタ調節要素が、前記関節機構の変位許容量が前記閾値以下の範囲において減少から増大に転じたことを要件として、前記第１指令入力要素に前記第１指令の制限を解除させた上で前記マイナーループに入力させることを特徴とする関節機構の制御装置。
【請求項３】
請求項２記載の関節機構の制御装置において、
前記第１指令に対応する前記関節機構の応答性に関する第１状態量を検知する状態量検知要素をさらに備え、
前記リミッタ調節要素が、前記第１指令と前記第１状態量との偏差が基準値以下であることをさらなる要件として、前記第１指令入力要素に前記第１指令の制限を解除させた上で前記マイナーループに入力させることを特徴とする関節機構の制御装置。
【請求項４】
請求項３記載の関節機構の制御装置において、
前記リミッタ調節要素が、前記第１指令と前記第１状態量との偏差が基準値以下であるという要件が満たされていない場合、前記第１指定値を連続的または断続的に低下させながら、前記第１指令入力要素に前記第１指令を前記第１指定値以下に制限させた上で前記マイナーループに入力させることを特徴とする関節機構の制御装置。
【請求項５】
請求項１〜４のうちいずれか１つに記載の関節機構の制御装置において、
前記関節機構の出力低下を補填するための第２指令を、前記マイナーループに対して入力する第２指令入力要素と、
前記リミッタ調節要素が、前記第１指令入力要素による前記第１指令の制限または制限解除と同様の態様で、前記第２指令を前記第２指令の制限または制限解除させることを特徴とする関節機構の制御装置。
【請求項６】
アクチュエータによって相対的に駆動される一対の部材を備えている関節機構の制御方法であって、
前記関節機構の実位置を検知する過程と、
前記関節機構の実位置と限界位置との偏差である変位許容量が閾値以下の範囲において減少していることを要件として、第１指定値を連続的または断続的に低下させながら、前記関節機構の応答性向上のための第１指令を、前記第１指定値以下に制限させた上でメジャーループである前記関節機構の位置制御ループのマイナーループに入力させる過程と、
前記関節機構の変位許容量が０に到達したことを要件として、位置指令が前記限界位置を越えないように前記位置指令を制限した上で前記メジャーループに入力する過程と、を含んでいることを特徴とする関節機構の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットなどを構成する関節機構の制御技術に関する。
【背景技術】
【０００２】
関節機構を構成する一対の部材がなす角度が小さいほど、当該関節機構を駆動するアクチュエータの出力トルクが小さくなるように、当該出力トルクを制限する技術が提案されている（特許文献１参照）。これにより、一対の部材により指が挟まれた際のこの指の怪我の防止が図られている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第３１３０００８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、従来技術によれば、関節機構の可動域が小さくなるにつれて出力トルクが制限されるため、関節角許容限界付近ではロボットの姿勢が保持できなくなるおそれがあった。
【０００５】
そこで、本発明は、関節機構の変位許容量が少ない状況でも、高速動作を維持しうるように当該関節機構の動作を制御する装置および方法を提供することを課題とする。
【課題を解決するための手段】
【０００６】
本発明の関節機構の制御装置は、アクチュエータによって相対的に駆動される一対の部材を備えている関節機構の制御装置であって、メジャーループである前記関節機構の位置制御ループに対して位置指令を入力する位置指令入力要素と、前記関節機構の応答性向上のための第１指令を、前記関節機構の位置制御ループのマイナーループに対して入力する第１指令入力要素と、前記関節機構の実位置を検知する位置検知要素と、前記位置検知要素により検知された前記関節機構の実位置と限界位置との偏差である変位許容量が閾値以下の範囲において減少していることを要件として、第１指定値を連続的または断続的に低下させながら、前記第１指令入力要素に前記第１指令を前記第１指定値以下に制限させた上で前記マイナーループに入力させ、かつ、前記関節機構の変位許容量が０に到達したことを要件として、前記位置指令入力要素に前記位置指令が前記限界位置を越えないように前記位置指令を制限させた上で前記メジャーループに入力させるリミッタ調節要素と、を備えていることを特徴とする。
【０００７】
本発明の関節機構の制御方法は、アクチュエータによって相対的に駆動される一対の部材を備えている関節機構の制御方法であって、前記関節機構の実位置を検知する過程と、前記関節機構の実位置と限界位置との偏差である変位許容量が閾値以下の範囲において減少していることを要件として、第１指定値を連続的または断続的に低下させながら、前記関節機構の応答性向上のための第１指令を、前記第１指定値以下に制限させた上でメジャーループである前記関節機構の位置制御ループのマイナーループに入力させる過程と、前記関節機構の変位許容量が０に到達したことを要件として、位置指令が前記限界位置を越えないように前記位置指令を制限した上で前記メジャーループに入力する過程と、を含んでいることを特徴とする。
【０００８】
本発明の関節機構の制御装置において、前記リミッタ調節要素が、前記関節機構の変位許容量が前記閾値以下の範囲において減少から増大に転じたことを要件として、前記第１指令入力要素に前記第１指令の制限を解除させた上で前記マイナーループに入力させることが好ましい。
【０００９】
本発明の関節機構の制御装置において、前記第１指令に対応する前記関節機構の応答性に関する第１状態量を検知する状態量検知要素をさらに備え、前記リミッタ調節要素が、前記第１指令と前記第１状態量との偏差が基準値以下であることをさらなる要件として、前記第１指令入力要素に前記第１指令の制限を解除させた上で前記マイナーループに入力させることが好ましい。
【００１０】
本発明の関節機構の制御装置において、前記リミッタ調節要素が、前記第１指令と前記第１状態量との偏差が基準値以下であるという要件が満たされていない場合、前記第１指定値を連続的または断続的に低下させながら、前記第１指令入力要素に前記第１指令を前記第１指定値以下に制限させた上で前記マイナーループに入力させることが好ましい。
【００１１】
本発明の関節機構の制御装置において、前記関節機構の出力低下を補填するための第２指令を、前記マイナーループに対して入力する第２指令入力要素をさらに備え、前記リミッタ調節要素が、前記第１指令入力要素による前記第１指令の制限または制限解除と同様の態様で、前記第２指令を前記第２指令の制限または制限解除させることが好ましい。
【発明の効果】
【００１２】
本発明の関節機構の制御装置および制御方法によれば、関節機構の変位許容量が閾値以下の範囲で減少している場合、第１指令値が当該減少に伴って連続的または断続的に低下されながら、第１指令が当該第１指令値以下になるように制限された上で、関節機構の位置制御ループのマイナーループに対して入力される。そして、関節機構の変位許容量が０に到達した場合、関節機構の位置指令が限界位置を越えないように制限された上でメジャーループである位置制御ループに対して入力される。
【００１３】
これらの結果、関節機構の変位許容量が閾値以下になる程度にまで、関節機構の実位置が限界位置に接近している間も関節機構の高速動作が維持され、かつ、関節機構の実位置が限界位置を越えないように関節機構の動作が制限される。
【００１４】
可動域の末端でも必要トルクを保持しつつ高速に動作可能となり、干渉エリアが小さくても停止可能な関節駆動制御が可能となる。
【図面の簡単な説明】
【００１５】
【図１】本発明の一実施形態における関節機構の構成説明図。
【図２】本発明の一実施形態としての関節機構の駆動回路の構成説明図。
【図３】本発明の一実施形態としての関節機構の制御方法に関する説明図。
【図４】変位許容量およびＦＦ指令の関係に関する説明図。
【発明を実施するための形態】
【００１６】
（構成）
図１に示されている本発明の一実施形態としての関節機構１は、第１部材１２と、第２部材１４と、第１部材１２および第２部材１４を相対的に運動させるための電動モータにより構成されるアクチュエータ１６と、アクチュエータ１６の動力を第１部材１２および第２部材１４の伝達する減速機構１８と、を備えている。このような関節機構１は、例えば特許第５８５９０３６号公報に記載されている脚式移動ロボットのほか、作業用のアームロボットなどの関節機構として採用される。
【００１７】
関節機構１の動作を制御する制御装置２は、コンピュータ（ＣＰＵ（演算処理装置または）ＲＯＭおよびＲＡＭなどのメモリ（記憶装置）、入出力Ｉ／Ｆ回路等により構成されている。）により構成されている。
【００１８】
図２に示されている、関節機構１の制御装置２の機能を表わす制御ブロック図は、リミッタ調節要素２０のほか、関節機構１の位置、関節機構１の出力トルク、関節機構１の速度、およびアクチュエータ１６（電動モータ）に対して供給される電流のそれぞれを制御するための位置制御ループ２２、トルク制御ループ２３、速度制御ループ２４および電流制御ループ２６を備えている。速度制御ループ２４および電流制御ループ２６は、メインループとしての位置制御ループ２２のマイナーループに相当する。このブロック図の各構成要素のうち少なくとも一部は、ハードウェアとして存在している必要はなく、ソフトウェアによりその機能が実現されていてもよい。
【００１９】
位置制御ループ２２は、位置指令リミッタ２２０と、位置検知器２２１と、算定器２２２と、伝達要素２２４（ゲイン係数：Ｋ1）と、を備えている。
【００２０】
位置指令リミッタ２２０（「位置指令入力要素」に相当する。）は、有効化された際に位置指令θcmdをこれが指定位置範囲（限界位置θ0を越えない範囲）に収まるように制限する。位置検知器２２１（「位置検知要素」に相当する。）は、関節機構１の実位置θactを検知する。算定器２２２は、位置指令リミッタ２２０の出力θcmd_ltdおよび関節機構１の実位置θactの偏差（＝θcmd_ltd−θact）を算定する。伝達要素２２４には、算定器２２２の出力が入力される。
【００２１】
トルク制御ループ２３は、関節機構１の実トルクＴｑactを検知するトルク検知器２３１と、実トルクＴｑactの１階時間微分を算定する１階微分要素２３２と、トルク検知器２３１および１階微分要素２３２に加えて、位置検知器２１１の出力が入力される伝達要素２３４と、を備えている。
【００２２】
速度制御ループ２４は、速度ＦＦ指令リミッタ２４０と、速度検知器２４１と、算定器２４２と、伝達要素２４４（ゲイン係数：Ｋ2）と、トルク電流指令リミッタ２４６と、算定器２４８と、を備えている。以下、「ＦＦ」という記載は「フィードフォワード」を意味する。
【００２３】
速度ＦＦ指令リミッタ２４０（「第１指令入力要素」に相当する。）は、有効化された際に速度ＦＦ指令ωff（「第１指令」に相当する。）を、これが指定速度範囲（第１指定値以下の範囲）に収まるように制限する。１階微分要素である速度検知器２４１（「状態量検知要素」に相当する。）は、実位置θactの１階時間微分である実速度ωactを算定する。算定器２４２は、速度ＦＦ指令リミッタ２４０の出力ωff_ltdおよび伝達要素２２４の出力Ｋ1（θcmd_ltd−θact）の和と、速度検知器２４１の出力ωactに対する偏差（＝Ｋ1（θcmd_ltd−θact）＋ωff_ltd−ωact）を算定する。伝達要素２４４（ゲイン係数：Ｋ2）には、算定器２４２の出力が入力される。トルク電流指令リミッタ２４６（「第２指令入力要素」に相当する。）は、有効化された際に伝達要素２３４の出力であるトルク制御のためのアクチュエータ１６電流指令値Ｉｔcmd（「第２指令」に相当する。）を、これが指定トルク電流値範囲（第２指定値以下の範囲）に収まるように制限する。算定器２４８は、伝達要素２４４の出力（＝Ｋ2｛Ｋ1（θcmd_ltd−θact）＋ωff_ltd−ωact｝）およびトルク電流指令リミッタ２４６の出力Ｉｔcmd_ltdの偏差を算定する。
【００２４】
電流制御ループ２６は、電流ＦＦ指令リミッタ２６０と、電流検知器２６１と、算定器２６２と、伝達要素２６４（ゲイン係数：Ｋ3）と、を備えている。
【００２５】
電流ＦＦ指令リミッタ２６０（「第１指令入力要素」に相当する。）は、電流ＦＦ指令Ｉｑff（「第１指令」に相当する。）を、これが指定電流値範囲（電流値の第１指定値以下の範囲）に収まるように制限する。電流検知器２６１（「状態量検知要素」に相当する。）は、アクチュエータ１６に対して供給される実電流値Ｉｑactを検知する。算定器２６２は、算定器２４８の出力および電流ＦＦ指令リミッタ２６０の出力Ｉｑff_ltdと実電流値Ｉｑactとの偏差（＝Ｋ2｛Ｋ1（θcmd_ltd−θact）＋ωff_ltd−ωact｝−Ｉｔcmd_ltd＋Ｉｑff_ltd−Ｉｑact）を電流指令値Ｉｑcmdとして算定する。伝達要素２６４には、算定器２６２の出力が入力される。伝達要素２６４の出力がアクチュエータ１６（またはその駆動回路）に対して付与される。
【００２６】
（関節機構１の制御方法）
制御装置２により実行される関節機構１の制御方法について説明する。位置検知器２２１が、関節機構１の実位置θact（第１部材１２および第２部材１４のなす角度）を検知する（図３／ＳＴＥＰ０２）。
【００２７】
リミッタ調節要素２０が、関節機構１の実位置θactと、限界位置θ0との偏差である変位許容量Δθ（＝θact−θ0）に基づき、以下に説明する手順にしたがって、位置指令リミッタ２２０、速度ＦＦ指令リミッタ２４０、トルク電流指令リミッタ２４６および電流ＦＦ指令リミッタ２６０のそれぞれのリミット特性を調節する。
【００２８】
変位許容量Δθが閾値Δθth以下であるか否かが判定される（図３／ＳＴＥＰ０４）。当該判定結果が否定的である場合（図３／ＳＴＥＰ０４‥ＮＯ）、実位置θactの検知（図３／ＳＴＥＰ０２参照）以降の処理が繰り返される。一方、当該判定結果が肯定的である場合（図３／ＳＴＥＰ０４‥ＹＥＳ）、変位許容量Δθの時間微分ｄΔθ／ｄｔが負であるか否かが判定される（図３／ＳＴＥＰ０６）。この判定は、関節機構１の位置θactがその許容値θ0に近づくように当該関節機構１が動いているか否かの判定に相当する。
【００２９】
ｄΔθ／ｄｔが負であると判定された場合（図３／ＳＴＥＰ０６‥ＹＥＳ）、速度ＦＦ指令リミッタ２４０、トルク電流指令リミッタ２４６および電流ＦＦ指令リミッタ２６０が有効化される（図３／ＳＴＥＰ０８）。この際、例えば図４に実線で示されているように、関節機構１の実位置θactが限界位置θ0に近づくにつれ（変位許容量Δθが０に近づくにつれ）、各リミッタの制限値（第１指定値または第２指定値）が断続的または段階的に低下するように調節される。そのほか、図４に破線で示されているように、関節機構１の実位置θactが限界位置θ0に近づくにつれ、各リミッタの制限値が連続的または漸減的に低下するように調節されてもよい。
【００３０】
さらに、関節機構１の変位許容量Δθが０になったか否か（関節機構１の実位置θactが限界位置θ0に到達したか否か）が判定される（図３／ＳＴＥＰ１０）。当該判定結果が否定的である場合（図３／ＳＴＥＰ１０‥ＮＯ）、実位置θactの検知（図３／ＳＴＥＰ０２参照）以降の処理が繰り返される。その一方、当該判定結果が肯定的である場合（図３／ＳＴＥＰ１０‥ＹＥＳ）、位置指令リミッタ２２０が有効化される（図３／ＳＴＥＰ１２）。
【００３１】
ｄΔθ／ｄｔが０または正であると判定された場合（図３／ＳＴＥＰ０６‥ＮＯ）、速度ＦＦ指令リミッタ２４０、トルク電流指令リミッタ２４６および電流ＦＦ指令リミッタ２６０のそれぞれが有効化されているか否かが判定される（図３／ＳＴＥＰ１４）。当該判定結果が否定的である場合、実位置θactの検知（図３／ＳＴＥＰ０２参照）以降の処理が繰り返される。
【００３２】
その一方、当該判定結果が肯定的である場合（図３／ＳＴＥＰ１４‥ＹＥＳ）、速度指令ωcmdおよび実速度ωactの偏差の大きさ、電流指令値Ｉｑcmdおよび実電流値Ｉｑactの偏差の大きさ、ならびに、トルク電流指令値Ｉｔcmdに対応するトルク指令Ｔｑcmdおよび実トルクＴｑactの偏差の大きさのそれぞれが、各偏差に対して設定されている各基準値を超えているか否かが判定される（図３／ＳＴＥＰ１６）。
【００３３】
当該判定結果が肯定的である場合（図３／ＳＴＥＰ１６‥ＹＥＳ）、速度ＦＦ指令リミッタ２４０、トルク電流指令リミッタ２４６および電流ＦＦ指令リミッタ２６０のそれぞれの制限値が低下される（図３／ＳＴＥＰ１８）。具体的には、当該偏差の基準値超過量が大きいほど、当該制限値の低下量が大きくなるように各位リミッタ２４０、２４６、２６０のリミット特性が調節される（図４参照）。
【００３４】
その一方、当該判定結果が否定的である場合（図３／ＳＴＥＰ１６‥ＮＯ）、速度ＦＦ指令リミッタ２４０、トルク電流指令リミッタ２４６および電流ＦＦ指令リミッタ２６０のそれぞれが無効化され、該当指令の制限が解除される（図３／ＳＴＥＰ２０）。
【００３５】
（作用効果）
前記した関節機構１の制御装置２および制御方法によれば、関節機構１の変位許容量Δθが閾値Δθth以下の範囲で減少している場合、第１指令（関節機構１の応答性を向上させるためのＦＦ指令）である速度ＦＦ指令ωffが速度指定範囲（制限値（第１指定値）以下の範囲）に収まるように制限され、かつ、同じく第１指令である電流ＦＦ指令Ｉｑffが電流値指定範囲（同上）に収まるように制限される（図３／ＳＴＥＰ０４‥ＹＥＳ→ＳＴＥＰ０６‥ＹＥＳ→ＳＴＥＰ０８参照）。この場合、第２指令（関節機構１の出力トルクを維持するためのＦＦ指令）であるトルク電流指令Ｉｔcmdが、トルク電流値指定範囲（制限値（第１指定値）以下の範囲）に収まるように制限される。
【００３６】
この際、変位許容量Δθの減少に伴って各指定範囲を画定する制限値（第１指定値および第２指定値のそれぞれ）が連続的または断続的に低下される。その上で、制限された第１指令および第２指令が、関節機構１の位置制御ループ２２のマイナーループ２４および２６のそれぞれに対して入力される（図２／速度ＦＦ指令リミッタ２４０および電流ＦＦ指令リミッタ２６０参照）。
【００３７】
そして、関節機構１の変位許容量Δθが０に到達した場合、関節機構１の位置指令θcmdが限界位置θ0を越えないように制限された上でメジャーループである位置制御ループ２２に対して入力される（図３／ＳＴＥＰ１０‥ＹＥＳ→ＳＴＥＰ１２参照）。
【００３８】
これらの結果、関節機構１の変位許容量Δθが閾値Δθth以下になる程度にまで、関節機構１の実位置θactが限界位置θ0に接近している間も関節機構１の高速動作が維持され、関節機構１の出力トルクの過度な低下が防止され、かつ、関節機構１の実位置θactが限界位置θ0を越えないように関節機構１の動作が制限される。
【００３９】
また、状態変数の実値および指令の偏差の大きさ｜ωcmd−ωact｜、｜Ｉｑcmd−Ｉｑact｜および｜Ｔｑcmd−Ｔｑact｜のそれぞれが各基準値を超えている場合、各リミッタ２４０、２４６および２６０の制限値が低下される（図３／ＳＴＥＰ１６‥ＹＥＳ→ＳＴＥＰ１８参照）。これにより、当該偏差の過大に由来して、関節機構１の動作が不連続的または不安定になる事態が防止される。
【００４０】
（本発明の他の実施形態）
前記実施形態では、関節機構１の応答性の向上を図るまたは応答遅れを補填するための「第１指令」として「速度ＦＦ指令ωff」および「電流ＦＦ指令Ｉｑff」のそれぞれが、位置制御ループ２２のマイナーループである速度制御ループ２４および電流制御ループ２６のそれぞれに対して入力されたが、他の実施形態として、第１指令として速度ＦＦ指令ωffおよび電流ＦＦ指令Ｉｑffのうち一方のみが、当該マイナーループに対して入力されてもよい。例えば、電流ＦＦ指令Ｉｑffの入力が省略される場合、電流ＦＦ指令リミッタ２６０が省略されてもよい。速度ＦＦ指令ωffの入力が省略される場合、速度ＦＦ指令リミッタ２４０が省略されてもよい。
【００４１】
前記実施形態において、トルク制御ループ２３が省略されることにより、関節機構１の出力トルクを維持するための第２指令の電流制御ループ２６に対する入力が省略されてもよい。
【符号の説明】
【００４２】
１‥関節機構、２‥制御装置、１１‥第１部材、１２‥第２部材、２０‥リミッタ調節要素、２２‥位置制御ループ（メインループ）、２４‥速度制御ループ（マイナーループ）、２６‥電流制御ループ（マイナーループ）、２２０‥位置指令リミッタ（位置指令入力要素）、２２１‥位置検知器（位置検知要素）、２４０‥速度ＦＦ指令リミッタ（第１指令入力要素）、２４１‥速度検知器（状態量検知要素）、２４６‥トルク電流指令リミッタ（第２指令入力要素）、２６０‥電流ＦＦ指令リミッタ（第１指令入力要素）、２６１‥電流検知器（状態量検知要素）。
【図１】
0006637810.tif 000002
【図２】
0006637810.tif 000003
【図３】
0006637810.tif 000004
【図４】
0006637810.tif 000005