(57)【要約】【課題】サーボモータ速度制御装置において、広い回転速度範囲で精度よくサーボモータの回転速度を算出する。【解決手段】サーボモータ速度制御装置１８０は、モータ速度指令部１８１１から出力されるモータ指令速度と、所定の第１回転速度及び第１回転速度よりも大きい速度である第２回転速度との大小を、回転速度判定部１８１２にて判定し、モータ指令速度が第１回転速度以下の時は、インプットキャプチャ回転速度算出部１８１３の算出結果を、モータ指令速度が第２回転速度以上の時は、位相計数回転速度算出部１８１４の算出結果を、サーボモータ１４２の回転速度として採用する。また、モータ指令速度が第１回転速度よりも大きく第２回転速度よりも小さい時は、インプットキャプチャ回転速度算出部１８１３及び位相計数回転速度算出部１８１４を動作させつつ、位相計数回転速度算出部１８１４の算出結果をサーボモータ１４２の回転速度として採用する。【選択図】図５
2014057432.tif 000002
【特許請求の範囲】
【請求項１】
マイコンと、所定の周期の割り込み処理によりサーボモータの回転速度の指示値であるモータ指令速度を指令するモータ速度指令部とを備え、所定の制御周期毎の割り込み処理により前記サーボモータの回転速度の制御を行うサーボモータ速度制御装置において、
前記モータ速度指令部から所定の周期で出力される前記モータ指令速度と、
所定の第１回転速度及び前記第１回転速度よりも大きい回転速度である第２回転速度との大小を判定する回転速度判定部と、
サーボモータの回転軸に取り付けられたエンコーダから出力されるエンコーダパルス間の基準クロックパルス数をカウントし、前記基準クロックパルス数からインプットキャプチャ回転速度を算出する、インプットキャプチャ回転速度算出部と、
前記エンコーダから出力されるパルス数に基づいて、位相計数回転速度を算出する、位相計数回転速度算出部と、を備え、
前記モータ指令速度が前記第１回転速度以下の時は、少なくとも前記インプットキャプチャ回転速度算出部により前記インプットキャプチャ回転速度を算出し、前記インプットキャプチャ回転速度を前記サーボモータの回転速度として採用し、
前記モータ指令速度が前記第２回転速度以上の時は、前記位相計数回転速度算出部により前記位相計数回転速度を算出し、前記位相計数回転速度を前記サーボモータの回転速度として採用し、
前記モータ指令速度が前記第１回転速度よりも大きく、前記第２回転速度よりも小さい範囲内にある時は、前記インプットキャプチャ回転速度算出部及び前記位相計数回転速度算出部により、それぞれ、前記インプットキャプチャ回転速度及び前記位相計数回転速度を算出し、前記位相計数回転速度を前記サーボモータの回転速度として採用する、サーボモータ速度制御装置。
【請求項２】
前記モータ速度指令部による処理と、前記インプットキャプチャ回転速度算出部による処理とが同時に指令された場合、前記モータ速度指令部による処理を先に行う、請求項１に記載のサーボモータ速度制御装置。
【請求項３】
前記サーボモータの動作開始時において、前記モータ指令速度が前記第１回転速度以下の範囲にあるときに、前記位相計数回転速度算出部が、前記エンコーダから出力される２つのパルスの位相差に基づき、前記サーボモータの軸回転方向を検出する、請求項１又は２に記載のサーボモータ速度制御装置。
【請求項４】
前記インプットキャプチャ回転速度算出部により前記インプットキャプチャ回転速度を算出するときに、前記インプットキャプチャ回転速度が所定の値以下になったときは、前記インプットキャプチャ回転速度算出部は、前記サーボモータが停止状態であるとインプットキャプチャ回転速度を算出する、請求項１から３のいずれかに記載のサーボモータ速度制御装置。
【請求項５】
前記モータ速度指令部による処理と、前記位相計数回転速度算出部による処理は同時に実行されないように設定されている、請求項１から４のいずれかに記載のサーボモータ速度制御装置。
【請求項６】
サーボモータの回転速度の指示値であるモータ指令速度を、所定の周期で出力する指令速度出力ステップと、
前記モータ指令速度が、所定の第１回転速度以下であるか、前記第１回転速度よりも大きい第２回転速度以上であるか、前記第１回転速度よりも大きく前記第２回転速度よりも小さい範囲内であるか、を判定する指令速度判定ステップと、
前記モータ指令速度が前記第１回転速度以下の時は、少なくとも、前記サーボモータの回転軸に取り付けられたエンコーダから出力されるエンコーダパルス間の基準クロックパルス数をカウントし、前記基準クロックパルス数からインプットキャプチャ回転速度を算出し、前記インプットキャプチャ回転速度を前記サーボモータの回転速度として採用し、前記モータ指令速度が前記第２回転速度以上の時は、前記エンコーダから出力されるパルス数に基づいて位相計数回転速度を算出し、前記位相計数回転速度をサーボモータの回転速度として採用し、前記モータ指令速度が前記第１回転速度よりも大きく、前記第２回転速度よりも小さい範囲内にある時は、前記インプットキャプチャ回転速度及び前記位相計数回転速度を算出し、前記位相計数回転速度を前記サーボモータの回転速度として採用する、サーボモータ回転速度決定ステップと、
を含むサーボモータ速度制御方法。
【請求項７】
前記モータ指令速度が前記第１回転速度以下の時、前記指令速度出力ステップと前記サーボモータ回転速度決定ステップの実行が同時に指令された場合、前記指令速度出力ステップは、前記サーボモータ回転速度決定ステップよりも優先する、請求項６に記載のサーボモータ速度制御方法。
【請求項８】
前記サーボモータの動作開始時において、前記モータ指令速度が前記第１回転速度以下の場合、前記エンコーダから出力される２つのパルスの位相差に基づき前記サーボモータの軸回転方向を検出するステップをさらに実行する、請求項６又は７に記載のサーボモータ速度制御方法。
【請求項９】
前記サーボモータ回転速度決定ステップにおいて、前記インプットキャプチャ回転速度を算出する際、前記インプットキャプチャ回転速度が所定の値以下となった場合、前記サーボモータは停止状態であるとする、請求項６から８のいずれかに記載のサーボモータ速度制御方法。
【請求項１０】
前記サーボモータ回転速度決定ステップにおいて、前記エンコーダから出力されるパルス数に基づいて前記位相計数回転速度を算出する場合、前記サーボモータ回転速度決定ステップと、前記サーボモータ指令速度出力ステップは同時に実行されない、請求項６から９のいずれかに記載のサーボモータ速度制御方法。
【請求項１１】
請求項１から５のいずれかに記載のサーボモータ速度制御装置と、前記サーボモータ速度制御装置により回転制御される車輪と、を備える走行装置。
【請求項１２】
請求項１から５のいずれかに記載のサーボモータの速度制御装置と、前記サーボモータの速度制御装置により傾動制御される操作ロッドと、を備える上肢訓練装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サーボモータ速度制御装置、及びサーボモータ速度制御方法に関する。特に、サーボモータの回転速度を広い回転速度範囲で算出可能な、サーボモータ速度制御装置、及び、サーボモータ速度制御方法に関する。
【背景技術】
【０００２】
サーボモータを広い回転速度範囲で精度よく制御するためには、広い回転速度範囲で精度よくサーボモータの回転速度の測定を行うことが好ましい。
【０００３】
サーボモータの回転速度の測定は、サーボモータの回転軸に取り付けられたエンコーダを用いて行われる。エンコーダは、サーボモータの回転速度に応じたパルス信号を発生させる。このパルス信号に基づいて、サーボモータの回転速度が測定される。
【０００４】
従来から知られている、エンコーダを用いたサーボモータの回転速度の測定方法としては、次の２つが知られている。１つ目は、一定時間内に発生したパルス信号のパルス数を測定する方法（位相計数測定法）である。２つ目は、パルス信号の周期を測定する方法（インプットキャプチャ法）である。
【０００５】
特許文献１には、ベルトコンベアの移動速度を測定する装置において、パルスカウント演算と周期演算とを同時に行うことにより移動速度を測定する技術が開示されている。これにより、上記２つの測定法の利点を生かしつつ、ベルトコンベアの移動速度が不安定になっても、正確な速度計測が可能となる。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００３−１９４８４０号公報
【特許文献２】特開２００９−２８８９３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、インプットキャプチャ法と位相計数測定法とを同時に実行すると、移動速度を測定する装置に過大な計算負荷がかかる。その結果、移動速度を精度よく測定できたとしても、移動速度の制御に遅延が発生し、そのため精度よく移動速度の制御が行えなくなることがある。
【０００８】
本発明の課題は、サーボモータ速度制御装置に過大な計算負荷をかけることなく、広い回転速度範囲において精度よくサーボモータの回転速度を測定することにある。
【課題を解決するための手段】
【０００９】
以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
【００１０】
本発明の一見地に係るサーボモータ速度制御装置は、マイコンと、モータ速度指令部と、回転速度判定部と、インプットキャプチャ回転速度算出部と、位相計数回転速度算出部と、を備える。
モータ速度指令部は、所定の周期の割り込み処理によりモータ指令速度を指令する。モータ指令速度は、モータ速度指令部がサーボモータの回転速度制御を行うために出力する、サーボモータの回転速度の指示値である。回転速度判定部は、モータ速度指令部から出力されるモータ指令速度と、所定の第１回転速度及び第１回転速度よりも大きい回転速度である第２回転速度との大小を判定する。インプットキャプチャ回転速度算出部は、サーボモータの回転軸に取り付けられたエンコーダから出力されるエンコーダパルス間の基準クロックパルス数をカウントする。そして、基準クロックパルス数からインプットキャプチャ回転速度を算出する。位相計数回転速度算出部は、エンコーダから出力されるパルス数に基づいて、位相計数回転速度を算出する。マイコンは、インプットキャプチャ回転速度と、位相計数回転速度とのいずれかをサーボモータの回転速度とするかを決定する処理を行う。
ここで、基準クロックパルスとは、インプットキャプチャ回転速度を算出するために使用される、一定の周期を有するパルス信号である。
インプットキャプチャ回転速度とは、エンコーダから出力されるパルスの周期から算出されるサーボモータの回転速度の計算値のことである。以後、インプットキャプチャ回転速度算出部において算出されたサーボモータの回転速度は、特に説明がなくとも、インプットキャプチャ回転速度であるとする。
また、位相計数回転速度とは、エンコーダから出力されるパルス数から算出されるサーボモータの回転速度の計算値のことである。以後、位相計数回転速度算出部において算出されたサーボモータの回転速度は、特に説明がなくとも、位相計数回転速度であるとする。
サーボモータ速度制御装置は、所定の制御周期毎の割り込み処理によりサーボモータの回転速度の制御を行う。
サーボモータ速度制御装置では、まず、回転速度判定部が、モータ指令速度と、第１回転速度及び第２回転速度との大小を判定する。そして、サーボモータ速度制御装置は、モータ指令速度と第１回転速度及び第２回転速度との大小関係に基づき、以下の３通りのいずれかの方法によりサーボモータの回転速度を決定する。
（１）モータ指令速度が第１回転速度以下：少なくともインプットキャプチャ回転速度算出部によりインプットキャプチャ回転速度を算出し、インプットキャプチャ回転速度をサーボモータの回転速度として採用する。
（２）モータ指令速度が第２回転速度以上：位相計数回転速度算出部により位相計数回転速度を算出し、位相計数回転速度をサーボモータの回転速度として採用する。
（３）モータ指令速度が第１回転速度よりも大きく、第２回転速度よりも小さい：インプットキャプチャ回転速度算出部及び位相計数回転速度算出部により、それぞれ、インプットキャプチャ回転速度及び位相計数回転速度を算出する。そして、位相計数回転速度をサーボモータの回転速度として採用する。
【００１１】
上記（１）及び（２）のように、モータ指令速度が存在する領域に応じて、最適な回転速度の算出演算方式を選択している。この結果、精度のよいサーボモータの回転速度の算出が可能となり、さらに、精度よくサーボモータの回転速度を制御できる。
また、２つの算出方法を同時に実行する場合を少なくすることで、マイコン（マイクロコンピュータ）の計算負荷を減少できる。その結果、マイコン（マイクロコンピュータ）によるサーボモータの回転速度の制御を遅延なく行える。
【００１２】
さらに、上記（３）のように、サーボモータの回転速度が第１回転速度と第２回転速度の間にあるときに、２つの算出方法のいずれも実行することにより、エンコーダがインクリメンタルタイプであったとしても、第１回転速度以下の低回転速度領域におけるサーボモータの回転速度をインプットキャプチャ回転速度算出部により精度よく算出できる。その結果、低回転領域におけるサーボモータの回転速度の制御を精度よく行うことができる。
【００１３】
モータ速度指令部による処理と、インプットキャプチャ回転速度算出部による処理とが同時に指令された（両方の処理の割り込みが同時に発生した）場合、モータ速度指令部による処理を先に行うようにしてもよい。
これにより、サーボモータの回転速度制御時に、モータ速度指令部によるモータ回転速度制御を優先することにより、制御対象の制御を遅延なく行うことができる。
【００１４】
サーボモータの動作開始時において、モータ指令速度が第１回転速度以下の範囲にあるときに、位相計数回転速度算出部が、エンコーダから出力される２つのパルスの位相差に基づき、サーボモータの軸回転方向を検出してもよい。
これにより、サーボモータの動作開始時の低速度領域において、エンコーダがインクリメンタル型であったとしても、インプットキャプチャ回転速度算出部によりインプットキャプチャ回転速度を算出しつつ、サーボモータの回転方向を把握できる。
【００１５】
インプットキャプチャ回転速度算出部によりインプットキャプチャ回転速度を算出するときに、インプットキャプチャ回転速度が所定の値以下になったときは、インプットキャプチャ回転速度算出部は、サーボモータが停止状態であるとインプットキャプチャ回転速度を算出してもよい。
ここで、サーボモータが停止状態とは、例えば、インプットキャプチャ回転速度の算出値（サーボモータの回転速度）が０であるとすることである。
これにより、インプットキャプチャ回転速度算出部がオーバーフローとなっても、計算負荷を増大させることなくサーボモータの回転速度を把握できる。
【００１６】
モータ速度指令部による処理と、位相計数回転速度算出部による処理は同時に実行されないように設定されていてもよい。
これにより、サーボモータの回転速度制御時に、位相計数回転速度算出部によるサーボモータの回転速度を算出するための計算負荷が、サーボモータ速度制御装置にかからなくなる。その結果、モータ速度指令値の出力の遅延をなくすことができる。
【００１７】
本発明の他の見地に係る、サーボモータ速度制御方法は、以下のステップを備えている。
◎サーボモータの回転速度の指示値であるモータ指令速度を、所定の周期で出力する指令速度出力ステップ。
◎モータ指令速度が、所定の第１回転速度以下であるか、第１回転速度よりも大きい第２回転速度以上であるか、第１回転速度よりも大きく第２回転速度よりも小さい範囲内であるか、を判定する指令速度判定ステップ。
◎指令速度判定ステップで判定した結果に基づいて、以下のいずれかを実行して、サーボモータの回転速度を決定するサーボモータ回転速度決定ステップ。
（１）モータ指令速度が第１回転速度以下の時：少なくとも、エンコーダから出力されるエンコーダパルス間の基準クロックパルス数をカウントし、基準クロックパルス数からインプットキャプチャ回転速度を算出し、インプットキャプチャ回転速度をサーボモータの回転速度として採用する。
（２）モータ指令速度が第２回転速度以上の時：エンコーダから出力されるパルス数に基づいて、位相計数回転速度を算出し、位相計数回転速度をサーボモータの回転速度として採用する。
（３）モータ指令速度が第１回転速度よりも大きく、第２回転速度よりも小さい時：インプットキャプチャ回転速度及び位相計数回転速度を算出する。サーボモータの回転速度として、位相計数回転速度を採用する。
【００１８】
この方法では、まず、モータ指令速度と、第１回転速度及び第２回転速度を比較する。そして、モータ指令速度と、第１回転速度及び第２回転速度との比較結果に基づき、以下のようにサーボモータの回転速度を決定する。
（１）モータ指令速度が第１回転速度以下：インプットキャプチャ回転速度をサーボモータの回転速度とする。
（２）モータ指令速度が第２回転速度以上：位相計数回転速度をサーボモータの回転速度とする。
（３）モータ指令速度が第１回転速度よりも大きく、第２回転速度よりも小さい：位相計数回転速度をサーボモータの回転速度とする。ただし、インプットキャプチャ回転速度及び位相計数回転速度の両方を算出する。
【００１９】
上記（１）及び（２）のように、この方法は、モータ指令速度が存在する領域に応じて、最適な回転速度の算出演算方式を選択している。この結果、精度のよいサーボモータの回転速度の算出が可能となり、さらに、精度よくサーボモータの回転速度を制御できる。
また、２つの算出方法を同時に実行する場合を少なくすることで、サーボモータ速度制御装置の計算負荷を減少できる。その結果、サーボモータの回転速度の制御が遅延なく行うことができる。
【００２０】
さらに、上記（３）のように、この方法は、モータ指令速度（サーボモータの回転速度）が第１回転速度と第２回転速度の間にあるときに、２つの算出方法のいずれも実行することにより、エンコーダがインクリメンタルタイプであったとしても、第１回転速度以下の低回転速度領域におけるサーボモータの回転速度を精度よく算出できる。その結果、低回転領域におけるサーボモータの回転速度の制御を精度よく行える。
【００２１】
モータ指令速度が第１回転速度以下の時、指令速度出力ステップとサーボモータ回転速度決定ステップの実行が同時に指令された場合、指令速度出力ステップは、サーボモータ回転速度決定ステップよりも優先してもよい。
これにより、制御対象の制御を遅延なく行うことができる。
【００２２】
サーボモータの動作開始時において、モータ指令速度が第１回転速度以下の場合、エンコーダから出力される２つのパルスの位相差に基づきサーボモータの軸回転方向を検出するステップをさらに実行してもよい。
これにより、サーボモータの動作開始時の低速度領域において、エンコーダがインクリメンタルタイプであったとしても、サーボモータ回転速度決定ステップの上記（１）において、予めサーボモータの回転方向を把握できる。
【００２３】
サーボモータ回転速度決定ステップにおいて、インプットキャプチャ回転速度を算出する際、インプットキャプチャ回転速度が所定の値以下となった場合、サーボモータは停止状態である（例えば、インプットキャプチャ回転速度の算出値を０）としてもよい。
これにより、インプットキャプチャ回転速度を算出する際に、計算負荷を増加させることなく、サーボモータの回転速度を把握できる。
【００２４】
サーボモータ回転速度決定ステップにおいて、エンコーダから出力されるパルス数に基づいて位相計数回転速度を算出する場合、サーボモータ回転速度決定ステップと、モータ指令速度出力ステップが同時に実行されなくてもよい。
これにより、モータ指令速度出力ステップの実行時間を短縮できる。その結果、モータ速度指令値の出力の遅延をなくすことができる。
【００２５】
本発明の他の見地に係る走行装置は、上記のサーボモータ速度制御装置と、上記のサーボモータ速度制御装置により回転制御される車輪と、を有している。
この走行装置では、上記のサーボモータ速度制御装置を有することにより、より実際に近い回転速度を把握することができる上に、速度指令出力の遅延がない。このため、走行装置の車輪を回転するサーボモータを精度よく制御できる。その結果、走行装置が指令に対して忠実に動作できる。
【００２６】
本発明の他の見地に係る上肢訓練装置は、上記のサーボモータ速度制御装置と、上記のサーボモータ速度制御装置により揺動制御される操作ロッドと、を有している。
この上肢訓練装置は、上記のサーボモータ速度制御装置を有することにより、より実際に近い回転速度を把握することができる上に、速度指令出力の遅延がない。このため、上肢訓練装置の操作ロッドが揺動する際、操作ロッドに意図しない動作のムラや変動が生じなくなる。その結果、操作ロッドの動作がスムーズになり、上肢訓練装置を使用するユーザが違和感や不快感を持たなくなる。
【発明の効果】
【００２７】
本発明のサーボモータ速度制御装置では、サーボモータの回転速度を精度よく制御できる。そして、本発明のサーボモータ速度制御装置が組み込まれたシステムでは、指令どおりの動作が可能であり、スムーズな動作が可能である。
【図面の簡単な説明】
【００２８】
【図１】自律走行ロボットの側面図。
【図２】走行制御コントローラの構成を示すブロック図。
【図３】サーボモータ速度制御装置の構成を示すブロック図。
【図４】自律走行ロボットが移動を開始してから目的地に到達するまでの動作を示すフローチャート。
【図５】サーボモータ速度制御装置の動作を示すフローチャート。
【図６】インプットキャプチャ法によりサーボモータの回転速度を算出する時の速度制御部の動作を示すフローチャート。
【図７】インプットキャプチャ速度算出処理のタイミング、サーボモータ制御処理、及びサーボモータ電流制御処理のタイミングを示す図。
【図８】インプットキャプチャ法によりサーボモータの回転速度を算出する方法を示す図。
【図９】エンコーダからの出力Ａ及び出力Ｂのパルス波形。
【図１０】位相計数法によりサーボモータの回転速度を算出する時の速度制御部の動作を示すフローチャート。
【図１１】位相計数速度算出処理のタイミングとサーボモータ制御処理のタイミングを示す図。
【図１２】位相計数法によりサーボモータの回転速度を算出する方法を示す図。
【図１３】逓倍処理を示す図。
【図１４】位相計数法とインプットキャプチャ法による回転速度の算出時における速度制御部の動作を示すフローチャート。
【図１５】上肢訓練装置の全体的な構成を示した図。
【発明を実施するための形態】
【００２９】
（１）自律走行ロボット
（１−１）自律走行ロボットの構成
図１を用いて、第１実施形態に係る自律走行ロボット１を説明する。本実施形態に係る自律走行ロボット１は、本発明の一実施形態のサーボモータ速度制御装置を組み込んだ走行装置の一例である。本実施形態に係る自律走行ロボット１は、障害物などを避けながら、目的地に自律的に移動するロボットである。
【００３０】
図１は、自律走行ロボット１の全体的な構成を模式的に示した側面図である。図１に示す自律走行ロボット１は、本体１２と、オムニホイール機構（移動部）１４と、レーザレンジファインダ（検知部）１６と、走行制御コントローラ（移動制御部）１８と、を備えている。
【００３１】
本体１２は、自律走行ロボット１の本体を構成している。また、本体１２の一側面にレーザレンジファインダ１６が取り付けられ、自律走行ロボットの視覚として働く。本体１２の内部には走行制御コントローラ１８が取り付けられ、下端部にオムニホイール機構１４が取り付けられている。
オムニホイール機構１４は、オムニホイール１４１と、サーボモータ１４２とを備え、自律走行ロボット１を移動させる。各オムニホイール１４１は、サーボモータ１４２の出力軸に接続され、自律走行ロボット１を移動させる。サーボモータ１４２は、オムニホイール１４１を回転させる動力源である。また、サーボモータ１４２のオムニホイール１４１が接続されていない側の出力軸には、図示されないエンコーダ１４２１が取り付けられている。エンコーダ１４２１は、サーボモータの出力軸の軸回転速度に応じたパルス信号を出力する。エンコーダ１４２１として、例えば、インクリメンタル型のエンコーダが選択されてもよい。
走行制御コントローラ１８には、レーザレンジファインダ１６と、エンコーダ１４２１と、サーボモータ１４２が接続されている。走行制御コントローラ１８は、エンコーダ１４２１からのパルス出力から、サーボモータ１４２の回転速度を算出する。また、走行制御コントローラ１８は、サーボモータ１４２の制御を行う。さらに、走行制御コントローラ１８は、レーザレンジファインダ１６からの信号に基づき、自律走行ロボット１の移動方向や移動速度を決定する。
【００３２】
次に、図２を参照して走行制御コントローラ１８について説明する。図２は走行制御コントローラ１８の構成を示すブロック図である。
【００３３】
図２に示すように、走行制御コントローラ１８は、サーボモータ速度制御装置１８０と上位装置１８１とを備える。
【００３４】
上位装置１８１は、４つのサーボモータ速度制御装置１８０と接続され、現在の自律走行ロボット１の位置、移動方向、及び速度（サーボモータ１４２の回転速度）などの情報に基づき、自律走行ロボット１の次の移動指令を生成する。そして、上位装置１８１は、生成された移動指令に基づいて、オムニホイール機構１４に取り付けられたそれぞれのサーボモータ１４２の制御指令を生成し、それぞれのサーボモータ速度制御装置１８０に制御指令を送信する。
上位装置１８１として、例えば、自律走行ロボット１の制御アルゴリズムを実行するマイクロコンピュータを用いることができる。制御アルゴリズムとしては、例えば、特許文献２に開示された制御アルゴリズムを用いることができる。
また、他の上位装置１８１として、例えば、シーケンサ（プログラマブルロジックコントローラ（ＰＬＣ））を用いることもできる。
なお、図２には図示しないが、レーザレンジファインダ１６が、上位装置１８１に接続されていてもよい。これにより、上位装置１８１は、レーザレンジファインダ１６からの情報も用いて、自律走行ロボット１の制御できる。
【００３５】
サーボモータ速度制御装置１８０は、オムニホイール機構１４が有する４つのサーボモータ１４２のそれぞれに接続され、上位装置１８１から送信されるサーボモータ１４２の制御指令に基づいて、それぞれのサーボモータ１４２を制御する。図２に示すように、１つのサーボモータ速度制御装置１８０は、１つのサーボモータ１４２のみを制御する。また、サーボモータ速度制御装置１８０は、それぞれのサーボモータ１４２の回転軸に取り付けられたエンコーダ１４２１の出力を受け入れ、サーボモータ１４２の回転速度を測定する。必要に応じて、測定されたサーボモータ１４２の回転速度は上位装置１８１にフィードバックされる。
【００３６】
次に、図３を用いてサーボモータ速度制御装置１８０の構成について説明する。図３はサーボモータ速度制御装置１８０の構成を示すブロック図である。
【００３７】
図３に示すように、サーボモータ速度制御装置１８０（図３中の点線で示した範囲）は、速度制御部１８１０と、モータ制御部１８２０と、エンコーダ出力分岐部１８３０とを備える。
【００３８】
モータ制御部１８２０は、速度制御部１８１０とサーボモータ１４２とに接続され、速度制御部１８１０から出力される指令電流値（後述）に基づいて、サーボモータ１４２に対してフィードバック電流を出力する。モータ制御部１８２０としては、サーボモータ１４２の種類（例えば、直流駆動、交流駆動など）に応じて、種々のモータ制御部を用いることができる。
【００３９】
エンコーダ出力分岐部１８３０は、速度制御部１８１０と、サーボモータ１４２の回転軸に取り付けられたエンコーダ１４２１とに接続され、エンコーダ１４２１からの出力を分岐し、分岐された出力を速度制御部１８１０へ出力する。
【００４０】
ここで、エンコーダ出力分岐部１８３０によるエンコーダ１４２１からの出力の分岐について説明する。エンコーダ１４２１は、２つのエンコーダ出力Ａ及びＢを有している。エンコーダ出力分岐部１８３０は、上記２つのエンコーダ出力を、エンコーダ出力Ａのみを出力する出力ＩＣと、ＡとＢの両方を出力する出力ＰＣとに分岐する。
なお、図３においては、出力ＩＣからＡのみが出力されている。しかし、これに限られない。出力ＩＣからＢのみが出力されるようにしてもよい。
【００４１】
エンコーダ出力分岐部１８３０は、図３に示すように、後述する速度制御部１８１０の回転速度判定部１８１２から出力される信号を入力し、当該信号に基づいて、出力ＩＣのみを出力するか、出力ＰＣのみを出力するか、これら両方を出力するかの３つを切り替える機能を有していてもよい。この場合、エンコーダ出力分岐部１８３０は、例えば、回転速度判定部１８１２から出力される信号により駆動されるリレー又はスイッチング素子により構成されていてもよい。
これにより、エンコーダ１４２１から、そのとき制御に不要な出力が速度制御部１８１０に入力されることを防げる。これにより、速度制御部１８１０の誤動作を防げる。
【００４２】
速度制御部１８１０（図３中の二点鎖線にて示された範囲）は、上位装置１８１及びモータ制御部１８２０に接続されている。速度制御部１８１０は、上位装置１８１から出力されるサーボモータ１４２の移動指令を、モータ指令速度に変換し、さらに、当該モータ指令速度を指令電流値に変換する。指令電流値は、速度制御部１８１０からモータ制御部１８２０へ送信される。なお、上記のモータ指令速度は、速度制御部１８１０内部における処理に用いられる。
また、速度制御部１８１０は、エンコーダ出力分岐部１８３０にも接続されている。速度制御部１８１０は、エンコーダ出力分岐部１８３０の出力ＩＣ、又は／及び、出力ＰＣから出力されるエンコーダ出力を用いて、サーボモータ１４２の回転速度の算出を行う。算出されたサーボモータ１４２の回転速度は、必要に応じて、上位装置１８１にフィードバックされる。
【００４３】
なお、速度制御部１８１０は、マイコン（マイクロコンピュータ）とこれに実行されるプログラムとにより構成されている。
【００４４】
速度制御部１８１０は、モータ速度指令部１８１１と、回転速度判定部１８１２と、インプットキャプチャ回転速度算出部１８１３と、位相計数回転速度算出部１８１４と、回転速度出力部１８１５とを備える。
【００４５】
モータ速度指令部１８１１は、上位装置１８１から出力されるサーボモータ１４２の制御指令から、所定の一定周期（モータ指令速度生成周期と呼ぶ）にてモータ指令速度を生成する。また、モータ速度指令部１８１１は、別の所定の一定周期（指令電流値出力周期）にて、サーボモータ１４２の回転速度がモータ指令速度と一致するように、サーボモータ１４２に流すべき指令電流値を算出する。指令電流値を算出後、モータ速度指令部は、算出された指令電流値をモータ制御部１８２０へ出力する。
なお、モータ指令速度生成周期と指令電流値出力周期とは異なっていることが好ましい。また、モータ指令速度の生成に対応するサーボモータ制御処理開始のタイミング（図７中のサーボモータ制御処理の立ち上がり）と、指令電流値の出力に対応するサーボモータ電流制御処理開始のタイミング（図７中のサーボモータ電流制御処理の立ち上がり）とが同時になる（図７中の二点鎖線で示した（３）のタイミングに対応）場合、図７に示すように、サーボモータ電流制御処理が優先して実行されることが好ましい。すなわち、サーボモータ１４２の制御遅延がない。
【００４６】
指令電流を算出する際、モータ速度指令部１８１１は、例えば、ＰＩＤ制御理論や現代制御などの制御理論を考慮して、指令電流を算出することが好ましい。これにより、モータ指令速度（上位装置１８１から出力される制御指令）が時間的に変化する場合でも、サーボモータ１４２の回転速度がこの時間変化に十分に追随できる。その結果、サーボモータ１４２の回転速度を、モータ指令速度どおりに精度よく制御可能となる。
【００４７】
インプットキャプチャ回転速度算出部１８１３は、エンコーダ１４２１からの出力のうち、出力Ａ（又は、出力Ｂ）のみが入力され、当該出力Ａに基づいて、サーボモータ１４２の回転速度（インプットキャプチャ回転速度）を算出する。算出された回転速度は、回転速度出力部１８１５へ出力される。
また、図３に示すように、インプットキャプチャ回転速度算出部１８１３は、回転速度判定部１８１２からの出力が入力されるようにしておいてもよい。この場合、回転速度判定部１８１２からの出力信号に基づいて、インプットキャプチャ回転速度算出部１８１３の動作（算出を実行するか停止するか）が決定される。
【００４８】
位相計数回転速度算出部１８１４は、エンコーダ１４２１からの出力Ａ及び出力Ｂの両方が入力され、当該出力Ａと出力Ｂの両方の出力に基づいて、サーボモータ１４２の回転速度（位相計数回転速度）を算出する。算出された回転速度は、回転速度出力部１８１５へ出力される。
位相計数回転速度算出部１８１４は、当該出力Ａと出力Ｂの両方の出力から、サーボモータ１４２の回転方向の判定も行う。
また、図３に示すように、位相計数回転速度算出部１８１４は、回転速度判定部１８１２からの出力が入力されるようにしておいてもよい。この場合、回転速度判定部１８１２からの出力信号に基づいて、位相計数回転速度算出部１８１４の動作（算出を実行するか停止するか）が決定される。
【００４９】
インプットキャプチャ回転速度算出部１８１３及び位相計数回転速度算出部１８１４による、サーボモータ１４２の回転速度算出方法は、後ほど詳細に説明する。
【００５０】
回転速度出力部１８１５は、回転速度判定部１８１２の出力に基づいて、上位装置１８１やモータ速度指令部１８１１などに出力するサーボモータ１４２の実際の回転速度として、インプットキャプチャ回転速度算出部１８１３により算出されたインプットキャプチャ回転速度を採用するか、位相計数回転速度算出部１８１４により算出された位相計数回転速度を採用するか、を決定する。
【００５１】
回転速度判定部１８１２は、モータ速度指令部１８１１において生成されたモータ指令速度の値と、予め設定された所定のモータ回転速度値との大小関係を判定する。
【００５２】
回転速度判定部１８１２は、例えば、図３に示すように、エンコーダ出力分岐部１８３０及び／又は回転速度出力部１８１５に対して、上記の大小関係の判定結果に基づいた信号を出力してもよい。また、これに限らず、上記の大小関係の判定結果に基づいた信号は、インプットキャプチャ回転速度算出部１８１３及び位相計数回転速度算出部１８１４にも出力されてよい。
【００５３】
上記の大小関係の判定結果に基づいた信号が、例えば、回転速度判定部１８１２からエンコーダ出力分岐部１８３０に出力された場合、当該信号により、エンコーダ出力分岐部１８３０は以下の３通りのいずれの出力を行うかを決定する。
（１）出力ＩＣから出力されるエンコーダ出力Ａのみを出力する。
（２）出力ＰＣから出力されるエンコーダ出力ＡとＢの両方のみを出力する。
（３）出力ＩＣと出力ＰＣの両方を同時に出力する。
【００５４】
また、上記の大小関係の判定結果に基づいた信号が、例えば、回転速度判定部１８１２から回転速度出力部１８１５に入力された場合、当該信号により、回転速度出力部１８１５は以下のいずれかの出力を行うかを決定する。
（１）インプットキャプチャ回転速度算出部１８１３から、インプットキャプチャ回転速度を出力する。
（２）位相計数回転速度算出部１８１４から、位相計数回転速度、及び／又は、サーボモータの回転方向を出力する。
【００５５】
さらに、上記の大小関係の判定結果に基づいた信号が、回転速度判定部１８１２からインプットキャプチャ回転速度算出部１８１３及び位相計数回転速度算出部１８１４に出力された場合、当該信号は、以下のいずれかの動作を行う。
（１）インプットキャプチャ回転速度算出部１８１３のみを動作させる。
（２）位相計数回転速度算出部１８１４のみを動作させる。
（３）インプットキャプチャ回転速度算出部１８１３と位相計数回転速度算出部１８１４の両方を同時に動作させる。
【００５６】
（１−２）自律走行ロボットの動作
次に、本実施形態の自律走行ロボット１の動作について、図４を用いて説明する。図４は、自律走行ロボット１が移動を開始してから目的地に到着するまでの動作を示す、フローチャートである。
【００５７】
ステップＳ１では、レーザレンジファインダ１６からの信号に基づいて障害物（物体２）の位置及び速度を取得する。
【００５８】
ステップＳ２では、目的地の位置と、ステップＳ１において取得した障害物（物体２）の位置と自律走行ロボット１の現在位置との相対的な位置、及び、障害物（物体２）の速度と自律走行ロボット１の現在の速度との相対的な速度に基づいて、上位装置１８１が自律走行ロボット１の移動方向、移動速度、及び移動距離などを算出し、これらの制御情報から移動指令を生成し、サーボモータ速度制御装置１８０に送信する。
【００５９】
ステップＳ３では、ステップＳ２において生成した移動指令に基づいて、サーボモータ速度制御装置１８０が、オムニホイール機構１４に取り付けられたそれぞれのサーボモータ１４２の回転速度（と回転量）を算出し、それぞれのサーボモータ１４２を制御する。
【００６０】
ステップＳ４では、自律走行ロボット１が目的地に到着したかどうかを判定する。自律走行ロボット１が未だ目的地に到着していない場合、ステップＳ１に戻る。自律走行ロボット１が目的地に到着した場合、自律走行ロボット１の移動を終了する。
【００６１】
なお、自律走行ロボット１に異常が見つかった場合などに、自律的又は外部からの指令により、自律走行ロボット１の移動動作が移動途中であっても停止できるようにしておいてもよい。
【００６２】
（１−２−１）サーボモータ速度制御装置の動作
自律走行ロボット１が、障害物にぶつかることなく正確に目的地まで到着するためには、上記ステップＳ３においてサーボモータ１４２の制御が精度よく行われることが好ましい。よって、次に、サーボモータ速度制御装置１８０による動作について図５を用いて詳しく説明する。
【００６３】
図５は、サーボモータ速度制御装置１８０の動作を示すフローチャートである。移動指令が上位装置１８１により生成され、サーボモータ速度制御装置１８０に送信されると、ステップＳ１０では、サーボモータ速度制御装置１８０のモータ速度指令部１８１１が、移動指令からモータ指令速度を生成する。
【００６４】
ステップＳ１１では、サーボモータ速度制御装置１８０に対して発生している割り込みについて判断する。サーボモータ制御の割り込みが発生している場合、ステップＳ１８へ進む。サーボモータの回転速度算出の割り込みが発生している場合、ステップＳ１２へ進む。
【００６５】
ステップＳ１２では、サーボモータ速度制御装置１８０の回転速度判定部１８１２が、モータ速度指令部１８１１により生成されたモータ指令速度が所定の第１回転速度以下であるかどうかを、判定する。
モータ指令速度が第１回転速度以下である場合、ステップＳ１４へ、モータ指令速度が第１回転速度より大きい場合、ステップＳ１３へ進む。
【００６６】
ステップＳ１３では、サーボモータ速度制御装置１８０の回転速度判定部１８１２が、モータ指令速度が所定の第２回転速度よりも小さいかどうかを判定する。ここで、第２回転速度は、第１回転速度よりもその絶対値が大きい所定の値である。
モータ指令速度が第２回転速度よりも小さい場合、ステップＳ１５へ、モータ指令速度が第２回転速度以上の場合、ステップＳ１６へ進む。
【００６７】
ここで、第１回転速度及び第２回転速度は、エンコーダ１４２１の分解能や、（特に、位相計数回転速度算出部１８１４において用いられる）回転速度の検出周期などにより、予め決定できる
【００６８】
ステップＳ１４では、サーボモータ速度制御装置１８０のインプットキャプチャ回転速度算出部１８１３が、エンコーダ１４２１の出力Ａ（又は、出力Ｂ）から、インプットキャプチャ法によりサーボモータ１４２の回転速度を算出する（インプットキャプチャ回転速度を算出する）。
【００６９】
ステップＳ１５では、サーボモータ速度制御装置１８０のインプットキャプチャ回転速度算出部１８１３及び位相計数回転速度算出部１８１４が、エンコーダ１４２１の出力Ａと出力Ｂから、インプットキャプチャ回転速度及び位相計数回転速度を算出する。
【００７０】
ステップＳ１６では、サーボモータ速度制御装置１８０の位相計数回転速度算出部１８１４が、エンコーダ１４２１の出力Ａ及び出力Ｂから、位相計数法によりサーボモータ１４２の回転速度を算出する（位相計数回転速度を算出する）。
【００７１】
ステップＳ１７では、サーボモータ速度制御装置１８０の回転速度出力部１８１５が、ステップＳ１４〜Ｓ１６のいずれかにおいて算出されたサーボモータ１４２の回転速度を、必要に応じて、上位装置１８１やモータ速度指令部１８１１に出力する。
ただし、ステップＳ１５が実行された場合、回転速度出力部１８１５は、インプットキャプチャ回転速度及び位相計数回転速度のうち、位相計数回転速度を出力する。
【００７２】
ステップＳ１８では、サーボモータ速度制御装置１８０のモータ速度指令部１８１１が、モータ指令速度と回転速度出力部１８１５から出力されるサーボモータ１４２の回転速度との差から、サーボモータ１４２に流す指令電流値を算出し、当該指令電流値をモータ制御部１８２０に出力する。そして、モータ制御部１８２０が、指令電流値に基づいて、サーボモータ１４２に電流を流し、サーボモータ１４２を制御する。
【００７３】
ステップＳ１９では、サーボモータ１４２の停止指令が出されているかどうかを判断する。サーボモータ１４２の停止指令が出されている場合、サーボモータ速度制御装置１８０はサーボモータ１４２を停止する。サーボモータ１４２の停止指令が出されていない場合、サーボモータ速度制御装置１８０は、ステップＳ１０に戻る。
【００７４】
次に、上記ステップＳ１４〜Ｓ１６におけるサーボモータ１４２の回転速度の算出法を説明する。
【００７５】
（１−２−１−１）インプットキャプチャ法による回転速度の算出（ステップＳ１４）
（Ｉ）インプットキャプチャ法による回転速度算出時の速度制御部の動作
図６は、インプットキャプチャ法によりサーボモータ１４２の回転速度を算出する時の速度制御部１８１０の動作を示す、フローチャートである。
【００７６】
ここでは、位相計数回転速度算出部１８１４によるサーボモータ１４２の回転速度の算出が行われている状態で、サーボモータ速度制御装置１８０がインプットキャプチャ法による回転速度の算出を開始するものとする。
自律走行ロボット１が静止状態から動作を開始した場合のように、位相計数回転速度算出部１８１４によるサーボモータ１４２の回転速度の算出が行われていない状態で、インプットキャプチャ法による回転速度の算出を開始することになった場合、位相計数回転速度算出部１８１４によるサーボモータ１４２の回転速度の算出を開始しておいてから、インプットキャプチャ法による回転速度の算出を開始してもよい。
【００７７】
ステップＳ４０１では、サーボモータ１４２が回転速度を増加し、加速中かどうかを判定する。サーボモータ１４２が加速中の場合、ステップＳ４０２へ、サーボモータ１４２が加速中でない（減速中）の場合、ステップＳ４０３へ進む。
サーボモータ１４２が加速中かどうかの判定は、回転速度判定部１８１２において行ってよく、現在のモータ指令速度と、所定の個数の過去のモータ指令速度値の絶対値の大小を比較して行ってよい。現在のモータ指令速度の絶対値が、過去のモータ指令速度の絶対値よりも大きければ、モータ速度指令部１８１１は、サーボモータ１４２が加速中であると、判定できる。
【００７８】
ステップＳ４０２では、位相計数回転速度算出部１８１４が、サーボモータ１４２の回転方向を判定する。このステップは、特に、自律走行ロボット１が静止状態から動作を開始したときに行われることが好ましい。なぜなら、静止状態から動作した場合、サーボモータ１４２の実際の回転方向は未知である一方、エンコーダ１４２１からの１つの出力のみから、サーボモータ１４２の回転方向を知ることは困難だからである。
【００７９】
サーボモータ１４２の回転方向は、位相計数回転速度算出部１８１４において、エンコーダ１４２１からの２つの出力Ａ及びＢの位相差から簡単に判定できる。位相計数回転速度算出部１８１４による、サーボモータ１４２の回転方向の判定法は、後ほど詳しく説明する。
【００８０】
ステップＳ４０３では、インプットキャプチャ回転速度算出部１８１３が、インプットキャプチャ法により、サーボモータ１４２の回転速度の算出を行う。
【００８１】
図７に、インプットキャプチャ速度算出処理のタイミング及びサーボモータ制御処理のタイミングを示す。インプットキャプチャ回転速度算出部１８１３による回転速度の算出開始のタイミング（図７のインプットキャプチャ速度算出処理の立ち上がり）が、モータ速度指令部１８１１による指令電流値をモータ制御部へ出力を開始するタイミング（サーボモータ制御処理の立ち上がり）と同時に発生する可能性がある（図中（１））。この場合、モータ速度指令部１８１１がモータ制御部１８２０に指令電流値を出力する処理を行ってから、インプットキャプチャ法による回転速度の算出処理を行うようにしてもよい。すなわち、図７中では、一点鎖線で示した（２）のタイミングから、インプットキャプチャ法による回転速度の算出処理が開始されてもよい。
【００８２】
上記のように、サーボモータ１４２の制御処理を優先して行うことにより、サーボモータ制御処理中に、速度制御部１８１０を構成するマイコン（マイクロコンピュータ）の計算負荷が大きくなることがない。その結果、サーボモータ１４２の制御処理の遅延がなくなる。
【００８３】
ステップＳ４０４では、インプットキャプチャ回転速度算出部１８１３は、インプットキャプチャ回転速度算出部１８１３によるサーボモータ１４２の回転速度の算出が、所定の回数以上行われたかを判断する。
インプットキャプチャ回転速度算出部１８１３によるサーボモータ１４２の回転速度の算出が所定の回数以上行われた場合、ステップＳ４０５へ進む。インプットキャプチャ回転速度算出部１８１３によるサーボモータ１４２の回転速度の算出が未だ所定の回数以上行われていない場合、ステップＳ４０６へ進む。
【００８４】
ステップＳ４０５では、回転速度出力部１８１５が、サーボモータ１４２の回転速度として、インプットキャプチャ回転速度算出部１８１３により算出した回転速度（インプットキャプチャ回転速度）を出力する。
【００８５】
ステップＳ４０６では、回転速度出力部１８１５が、サーボモータ１４２の回転速度として、位相計数回転速度算出部１８１４により算出した回転速度（位相計測回転速度）を出力する。
【００８６】
ステップＳ４０７では、位相計数回転速度算出部１８１４によるサーボモータ１４２の回転速度の算出が停止する。
【００８７】
ここでは、回転速度出力部１８１５は、インプットキャプチャ回転速度算出部１８１３による回転速度の算出が所定の回数以上行われた後に、インプットキャプチャ回転速度算出部１８１３により算出した回転速度を、サーボモータ１４２の実際の回転速度として出力する。
【００８８】
これは、インプットキャプチャ回転速度算出部１８１３が、回転速度の算出を開始した初期状態において、十分な精度でサーボモータ１４２の回転速度を算出することが困難だからである。この原因については、後ほど述べる。
【００８９】
（ＩＩ）インプットキャプチャ法による回転速度算出法
次に、図８を用いて、インプットキャプチャ法による回転速度算出法を説明する。図８は、インプットキャプチャ法によりサーボモータ１４２の回転速度を算出する方法を示す、模式図である。ここに示す機能は、本実施形態においては、インプットキャプチャ回転速度算出部１８１３に備わっている。
【００９０】
まず、インプットキャプチャ回転速度算出部１８１３がエンコーダ１４２１からの出力Ａ（エンコーダＡ相出力）のパルス間隔を測定する。出力Ａのパルス間隔は、例えば、図８では、出力Ａのパルスの立ち上がり区間（ＴＡ）であるとすることができる。出力Ａのパルス間隔ＴＡは、ＴＡ間に含まれる一定周期（Ｔｃ（ｓｅｃ．））で発生するクロックパルスの個数を計数することにより演算できる。出力Ａのパルス間隔ＴＡを測定するための、一定周期のクロックパルス信号を、基準クロックパルスと呼ぶ場合もある。
例えば、図８のように、出力Ａのパルスの立ち上がり区間に、ｍ個のクロックパルスが含まれた場合、出力Ａのパルス間隔ＴＡは、ＴＡ＝ｍＴｃ（ｓｅｃ．）となる。
【００９１】
次に、出力Ａのパルス間隔ＴＡからサーボモータ１４２の回転速度を算出する。サーボモータ１４２の回転速度は、エンコーダ１４２１の分解能（逓倍処理なし）をＰ（パルス／回転）とすると、１／Ｐ／ＴＡ＊６０＝６０／（ｍＰＴｃ）（ｒｐｍ）の式から算出される。
【００９２】
基準クロックパルスの個数の計数は、具体的には、エンコーダ１４２１からの出力Ａのパルスの立ち上がりと同時に基準クロックパルスの計数を開始し、次のパルスの立ち上がりと同時に基準クロックパルスの計数を停止することにより行われる。
【００９３】
なお、基準クロックとして、マイコン（マイクロコンピュータ）を動作させるためのクロック信号が用いられてもよい。マイクロコンピュータを動作させるためのクロック信号は、周期の変動が少なく、かつ、短周期のパルス信号であるからである。そのため、精度よく出力Ａのパルス周期を測定できる。
【００９４】
また、マイクロコンピュータがインプットキャプチャ機能を内部に備える場合は、その機能を用いて、インプットキャプチャ回転速度算出部１８１３の機能を実現してもよい。こうすることにより、サーボモータ速度制御装置１８０の部品数を少なくできる。
【００９５】
また、マイクロコンピュータを動作させるためのクロック信号とは別のクロックパルスを、エンコーダ１４２１からの出力Ａのパルス間隔測定（基準クロックパルス）に用いてもよい。この場合、サーボモータ１４２の使用回転速度範囲に最適な、所定の周波数のクロックパルスを用いて、エンコーダ１４２１からの出力のパルス周期ＴＡを測定できる。この場合、後述するオーバーフローの問題も発生しにくくなる。
【００９６】
インプットキャプチャ法において、エンコーダ１４２１からの出力のパルス間隔ＴＡとクロックパルスの周期Ｔｃの比が大きくなるほど、エンコーダ１４２１からの出力のパルス周期ＴＡの測定精度が高くなる。
【００９７】
これは、言い換えると、インプットキャプチャ法によるサーボモータ１４２の回転速度算出法では、サーボモータ１４２の回転速度が低くなるほど、サーボモータ１４２の回転速度の算出精度が高くなることを示している。
したがって、インプットキャプチャ法によるサーボモータ１４２の回転速度算出法は、サーボモータ１４２の回転速度が低いときに、好ましいサーボモータ回転速度算出法である。
【００９８】
また、一方で、サーボモータ１４２の回転速度が高くなった場合には、回転速度が低くなった場合と比較して、実際のサーボモータ１４２の回転速度と算出された回転速度の誤差が大きくなるのみならず、サーボモータ速度制御装置１８０の演算負荷が高くなる。これは、インプットキャプチャ法では、エンコーダ１４２１からの出力のパルスが立ち上がる（あるいは立ち下がる）ごとに、クロックパルスの計数開始と終了及びサーボモータ１４２の回転速度の算出（インプットキャプチャ回転速度の算出）を行うためである。このような点からも、インプットキャプチャ法は低回転速度の算出に使用されることが好ましい。
【００９９】
また、サーボモータ１４２の回転速度が極端に低くなった場合、計数されるクロックパルス数が極端に多くなる。そのため、サーボモータ１４２の回転速度が極端に低くなった場合、インプットキャプチャ回転速度算出部１８１３が計数できるクロックパルス数（計数容量）では、エンコーダ１４２１からの出力のパルス間隔ＴＡが測定できない場合（オーバーフロー）が発生する。
オーバーフロー状態が発生した場合、インプットキャプチャ回転速度算出部１８１３は計数クロックパルス数を０にリセットして、再びパルス数の計数を開始してしまう。この場合、インプットキャプチャ回転速度算出部１８１３により算出されるインプットキャプチャ回転速度と、実際のサーボモータの回転速度との誤差が無視できないほど大きくなる場合がある。
【０１００】
上記のオーバーフロー状態が発生することを避けるため、インプットキャプチャ回転速度算出部１８１３は、所定の数のクロックパルス数を計数した時点で、サーボモータ１４２の回転速度（インプットキャプチャ回転速度）を０（ｒｐｍ）であると算出してもよい。
このように、上記のオーバーフロー状態が発生する前に、サーボモータ１４２の回転速度（インプットキャプチャ回転速度）を０（ｒｐｍ）であるとすることにより、インプットキャプチャ回転速度算出部１８１３により算出されるサーボモータの回転速度と実際のサーボモータの回転速度の誤差が、無視できないほど大きくなることを抑制できる。その結果、自律走行ロボット１が動作途中（移動途中）に機能不全に陥ることなく、適正に動作できる。
【０１０１】
また、基準クロックパルスの周波数が高くなると、エンコーダ１４２１からのパルス出力の間隔が短い場合であっても、インプットキャプチャ回転速度算出部１８１３が上記のオーバーフロー状態になる場合がある。その結果、インプットキャプチャ回転速度算出部１８１３が、精度よくサーボモータの回転速度を算出できなくなることがある。この場合、低回転速度領域での回転速度算出精度が高いというインプットキャプチャ法の利点が生かせなくなる。
【０１０２】
これを避けるため、クロックパルス周期の所定倍数の周期を有した、別のクロックパルスを発生させ、当該別のクロックパルスを基準クロックパルスとして用いてエンコーダ１４２１からの出力のパルス間隔ＴＡを求めるようにしてもよい（プリスケーラ）。
【０１０３】
クロックパルス周期を所定倍数の周期にするには、例えば、クロックパルスを１段以上のフリップフロップに入力し、フリップフロップの出力を別のクロックパルスとする方法を採用できる。この場合、フリップフロップの段数をｎとすると、元のクロックパルスの２ｎ倍の周期を有する別のクロックパルスを発生できる。
【０１０４】
（ＩＩＩ）インプットキャプチャ回転速度算出部１８１３が動作初期において十分な精度の算出結果を出すことが困難な理由とその解決法
ここで、上記において述べた、インプットキャプチャ回転速度算出部１８１３が、回転速度の算出を開始する初期状態において、十分な精度でサーボモータ１４２の回転速度を算出することが困難であることの理由を説明する。
【０１０５】
この現象が起こる原因として、インプットキャプチャ回転速度算出部１８１３が動作を開始した初期状態において、インプットキャプチャ回転速度算出部１８１３が、エンコーダ１４２１からの出力のパルスの立ち上がりと同時に、クロックパルスの計数を開始することができない（すなわち、クロックパルス計数開始のタイミングが、エンコーダ１４２１からの出力パルスの立ち上がりよりも遅れる）ことが考えられた。
【０１０６】
エンコーダ１４２１からの出力のパルスの立ち上がりと同時に、基準クロックパルスの計数を開始することができないと、インプットキャプチャ回転速度算出部１８１３は間違った回転速度を算出してしまう。この現象は、特に、サーボモータ１４２の回転速度の減速時に発生することが多い。
【０１０７】
例えば、基準クロックパルス計数開始のタイミングが、エンコーダ１４２１からの出力パルスの立ち上がりよりも遅れた場合を考える。この場合、インプットキャプチャ回転速度算出部１８１３により算出されるエンコーダ１４２１からの出力パルスの間隔は、実際のエンコーダ１４２１からの出力パルスの間隔よりも短くなる。この結果、インプットキャプチャ回転速度算出部１８１３により算出される回転速度は、実際のサーボモータ１４２の回転速度よりも大きくなる。
【０１０８】
この問題点を解決するため、インプットキャプチャ回転速度算出部１８１３に「ダミー」の回転速度の算出を所定の回数行わせる。そして、インプットキャプチャ回転速度算出部１８１３がこの「ダミー」の回転速度の算出を行う間、回転速度出力部１８１５が位相計数回転速度算出部１８１４により算出された回転速度（位相計数回転速度）を、サーボモータ１４２の回転速度として出力する。その間に、インプットキャプチャ回転速度算出部１８１３は、エンコーダ１４２１からの出力パルスの立ち上がりと同期して、基準クロックパルスの計数を開始できるようになる。
【０１０９】
これにより、サーボモータ１４２の回転速度の算出精度が不十分である期間やサーボモータ１４２の回転速度の算出が行われない期間がなくなり、絶えず、サーボモータ１４２の回転速度が精度よく算出される。
【０１１０】
また、この「ダミー」の回転速度の算出回数は、数回程度（３回程度）で十分ある。そのため、インプットキャプチャ回転速度算出部１８１３と位相計数回転速度算出部１８１４が同時に動作する期間が短くなる。その結果、サーボモータ速度制御装置１８０は、その計算負荷が大きくならず、サーボモータ１４２の制御を遅延なく行える。
【０１１１】
（ＩＶ）位相計数回転速度算出部によるサーボモータの回転速度の判定
次に、位相計数回転速度算出部１８１４によるサーボモータ１４２の回転方向の判定法について、図９を用いて説明する。
【０１１２】
図９は、エンコーダ１４２１からの出力Ａ及び出力Ｂのパルス波形を示す。出力Ａと出力Ｂとのパルス波形の関係には、図９に示すように、２つの場合がある。
パターン１：出力Ａのパルスの立ち上がりが、出力Ｂのパルスよりも早く現れる場合（出力Ａのパルスの位相が、出力Ｂのパルスの位相よりも進んでいる場合）。
パターン２：出力Ａのパルスの立ち上がりが、出力Ｂのパルスよりも遅く現れる場合（出力Ａのパルスの位相が、出力Ｂのパルスの位相よりも遅れている場合）。
エンコーダ１４２１は、エンコーダ１４２１からの出力Ａと出力Ｂが、図９のように、異なる位相となるように設計されている。また、図９のパターン１の位相関係となるか、パターン２の位相関係になるかがサーボモータ１４２（エンコーダ１４２１）の回転方向により決まるよう、インクリメンタル型のエンコーダ１４２１は設計されている。
【０１１３】
位相計数回転速度算出部１８１４は、エンコーダ１４２１からの２つの出力Ａ及び出力Ｂの両方を入力しているので、エンコーダ１４２１からの２つの出力Ａ及び出力Ｂの位相関係から、サーボモータ１４２の回転方向を判定できる。
【０１１４】
なお、エンコーダ１４２１からの２つの出力Ａ及び出力Ｂはお互いに異なる位相のパルスとなるため、エンコーダ１４２１からの２つの出力Ａ及び出力Ｂは、それぞれ、エンコーダＡ相出力及びエンコーダＢ相出力と呼ばれることもある。
【０１１５】
（１−２−１−２）位相計数法による回転速度の算出（ステップＳ１６）
（Ｉ）位相計数法による回転速度算出時の速度制御部の動作
図１０を用いて、位相計数法により回転速度の算出を行うときの、速度制御部１８１０の動作を説明する。図１０は、位相計数法によりサーボモータ１４２の回転速度を算出する時の速度制御部１８１０の動作を示す、フローチャートである。
【０１１６】
ステップＳ６０１では、位相計数回転速度算出部１８１４が動作中であるかどうかを判定する。位相計数回転速度算出部１８１４が動作中の場合、ステップＳ６０３へ、位相計数回転速度算出部１８１４が動作中でない場合、ステップＳ６０２へ進む。
【０１１７】
ステップＳ６０２では、位相計数回転速度算出部１８１４によるサーボモータ１４２の回転速度の算出が開始される。
【０１１８】
図１１に、位相計数速度算出処理のタイミングとサーボモータ制御処理のタイミングを示す。なお、位相計数回転速度算出部１８１４による回転速度の算出（図１１中の位相計数速度算出処理）は、モータ速度指令部１８１１によるサーボモータ１４２への指令電流値の出力（図１１中のサーボモータ制御処理）と同時に実行されないように設定されることが好ましい。
これは、例えば、位相計数速度算出処理の周期ＴＢと、サーボモータ制御処理の周期（モータ指令速度生成周期）とを一致させ、位相計数速度算出処理後（位相計数計数算出処理の立ち下がり後）に、サーボモータ制御処理（サーボモータ指令速度の生成）を行うようにすることで実現できる。
【０１１９】
また、位相計数速度算出処理の周期ＴＢとサーボモータ制御処理の周期（モータ指令速度生成周期）とが一致しない場合であっても、図１１中の速度算出処理のタイミングとサーボモータ制御処理のタイミングが同時となった場合は、サーボモータ制御処理を優先して行えばよい。
【０１２０】
このように、位相計数回転速度算出部１８１４による回転速度の算出と、モータ速度指令部１８１１によるサーボモータ１４２への指令電流値の出力とが同時に行われないことにより、サーボモータ１４２の制御時にサーボモータ速度制御装置１８０の負荷が大きくならない。その結果、サーボモータ１４２の制御が遅延しなくなる。
【０１２１】
ステップＳ６０３では、インプットキャプチャ回転速度算出部１８１３が動作中であるかどうかを判定する。インプットキャプチャ回転速度算出部１８１３が動作中の場合、ステップＳ６０４へ、インプットキャプチャ回転速度算出部１８１３が動作中でない場合、ステップＳ６０５へ進む。
【０１２２】
Ｓ６０４では、動作中のインプットキャプチャ回転速度算出部１８１３が動作を停止する。
【０１２３】
ステップＳ６０５では、回転速度出力部１８１５が、位相計数回転速度算出部１８１４により算出した回転速度をサーボモータ１４２の回転速度として出力する。
【０１２４】
（ＩＩ）位相計数法によるサーボモータの回転速度算出法
次に、図１２を用いて、位相計数法によるサーボモータ１４２の回転速度算出法を説明する。図１２は、位相計数法によりサーボモータ１４２の回転速度を算出する方法を示す、模式図である。ここに示す機能は、本実施形態においては、位相計数回転速度算出部１８１４に備わっている。
【０１２５】
位相計数法は、一定期間内の、エンコーダ１４２１からの出力に含まれるパルス数を計数することによりエンコーダ１４２１からの出力のパルス周期を測定し、当該パルス周期からサーボモータ１４２の回転速度を算出する。具体的には、以下のように行われる。
【０１２６】
なお、位相計数法によりサーボモータ１４２の回転速度を算出する際、位相計数回転速度算出部１８１４において、エンコーダ１４２１からの出力のパルスの逓倍処理が行われてもよい。
【０１２７】
逓倍処理は、エンコーダ１４２１からの出力のパルス周波数を所定倍することである。エンコーダ１４２１からの出力のパルスの周波数（周期の逆数）を逓倍処理により所定倍することで、エンコーダ１４２１の分解能を向上できる。
【０１２８】
本実施形態では、位相の異なる２つのエンコーダ１４２１からの出力Ａ及び出力Ｂを用いて逓倍処理が行われる。図１３は、本実施形態における逓倍処理を示す、模式図である。
【０１２９】
すなわち、逓倍されたエンコーダ出力は、図１３に示すように、エンコーダ１４２１からの出力Ａ及び出力Ｂのパルスの立ち上がり及び立ち下がりにおいて立ち上がるパルス列とすることができる。
【０１３０】
図１３に示す場合、逓倍されたエンコーダ出力には、エンコーダ１４２１からの出力のパルス１周期に４つのパルスが存在している。このような場合、エンコーダ１４２１からの出力のパルスが４逓倍されたとも言う。
【０１３１】
まず、位相計数回転速度算出部１８１４が、一定期間ＴＢ（ｓｅｃ．）内（図１２中の回転速度算出処理パルスの立ち上がりの間）に含まれる逓倍されたエンコーダ１４２１からの出力のパルス数（図１２中のエンコーダ４逓倍後パルス）を計数する。
この時、回転速度算出処理パルスの立ち上がりにおいてパルス数の計数を開始し、次の回転速度算出処理パルスの立ち上がりにおいてパルス数の計数を停止することにより、逓倍されたエンコーダ１４２１からの出力のパルスが計数される。
【０１３２】
上記において計数されたパルス数から、逓倍されたエンコーダ１４２１からの出力のパルスの周期を算出する。例えば、図１２に示すように、一定期間ＴＢ内にｎ個の逓倍されたエンコーダ１４２１からの出力のパルスが含まれる場合、逓倍されたエンコーダ１４２１からの出力のパルス周期は、ＴＢ／ｎ（ｓｅｃ．）となる。
【０１３３】
次に、逓倍されたエンコーダ１４２１からの出力のパルス周期から、サーボモータ１４２の回転速度を算出する。サーボモータ１４２の回転速度は、逓倍後のエンコーダ１４２１の分解能をＰ’（パルス／回転）とすると、１／Ｐ’／（ＴＢ／ｎ）＊６０＝６０ｎ／（Ｐ’ＴＢ）（ｒｐｍ）の式から算出される。
【０１３４】
位相計数法において、一定期間ＴＢにおいて計数される、逓倍されたエンコーダ１４２１からの出力のパルス数が多くなるほど、サーボモータ１４２の回転速度算出精度が高くなる。
【０１３５】
すなわち、位相計数法によるサーボモータ１４２の回転速度算出法では、サーボモータ１４２の回転速度が高くなるほど、サーボモータ１４２の回転速度の算出精度が高くなる。したがって、サーボモータ１４２の回転速度が高いときに好適なサーボモータ回転速度算出法は、位相計数法である。
また、上記のように、インプットキャプチャ法では、基準クロックパルス数を計数する周期ＴＡは、サーボモータ１４２の回転速度が高くなるほど短くなる。そのため、インプットキャプチャ法では、サーボモータ１４２の回転速度が高くなるほど、サーボモータ速度制御装置１８０の計算負荷が増大する。さらに、特に高回転領域において、基準クロックパルス数の計数の小さな誤差は、サーボモータ１４２の回転速度の大きな算出誤差となる。なぜなら、インプットキャプチャ回転速度は、基準クロックパルス数の逆数に比例（反比例）するからである。
一方、位相計数法では、エンコーダ１４２１からの出力のパルス数を計数する周期ＴＢは所定の一定値である。そのため、周期ＴＢは、サーボモータ速度制御装置１８０の計算負荷が増大しない程度の適当な値に設定される。このように、位相計数法は、サーボモータ１４２の回転速度が高くなっても、サーボモータ速度制御装置１８０の計算負荷を高めることなく回転速度を精度よく算出できるという特徴も有している。
【０１３６】
（１−２−１−３）位相計数法とインプットキャプチャ法による回転速度の算出（ステップＳ１５）
（Ｉ）速度制御部の動作
次に、図１４を用いて、位相計数法とインプットキャプチャ法による回転速度の算出時における、速度制御部１８１０の動作について説明する。図１４は、位相計数法とインプットキャプチャ法による回転速度の算出時における速度制御部１８１０の動作を示す、フローチャートである。
【０１３７】
なお、ここで用いられる、インプットキャプチャ法及び位相計数法によるサーボモータ１４２の回転速度の算出法は、上記に示した方法と同様であるため、ここでは説明を省略する。
【０１３８】
ステップＳ５０１では、位相計数回転速度算出部１８１４が動作中であるかどうかを判定する。位相計数回転速度算出部１８１４が動作中でない場合、ステップＳ５０２へ進む。位相計数回転速度算出部１８１４が動作中の場合、ステップＳ５０３へ進む。
【０１３９】
ステップＳ５０２では、位相計数回転速度算出部１８１４が動作を開始する。それにより、位相計数法によるサーボモータ１４２の回転速度の算出が開始される。
【０１４０】
ステップＳ５０３では、インプットキャプチャ回転速度算出部１８１３が動作中であるかどうかを判定する。インプットキャプチャ回転速度算出部１８１３が動作中でない場合、ステップＳ５０４へ進む。インプットキャプチャ回転速度算出部１８１３が動作中である場合、ステップＳ５０５へ進む。
【０１４１】
ステップＳ５０４では、インプットキャプチャ回転速度算出部１８１３が回転速度の算出を開始する。
【０１４２】
ステップＳ５０５では、回転速度出力部１８１５が、位相計数回転速度算出部１８１４により算出された回転速度を、サーボモータ１４２の回転速度として出力する。
【０１４３】
モータ指令速度が、第１回転速度よりも大きく、第２回転速度よりも小さいときに、位相計数法及びインプットキャプチャ法による回転速度の算出が行われる。この時、上記に述べたように、インプットキャプチャ回転速度算出部１８１３と位相計数回転速度算出部１８１４は同時に動作している。これは、特に、サーボモータ１４２の減速時に、モータ指令速度が第１回転速度以下になったときに、インプットキャプチャ回転速度算出部１８１３がサーボモータ１４２の回転速度（インプットキャプチャ回転速度）の算出を精度よく行うためである。
【０１４４】
ところで、ある１つの所定の回転速度を境として、インプットキャプチャ回転速度算出部１８１３と位相計数回転速度算出部１８１４の動作を単純に切り替えた場合、サーボモータ１４２の制御の精度が不十分であることが予想される。
【０１４５】
この原因として、２つの理由が考えられる。
１つ目は、上記に述べたように、インプットキャプチャ回転速度算出部１８１３が、動作開始時において、十分な精度でサーボモータ１４２の回転速度を算出することが困難であることである。
【０１４６】
上記１つ目の原因は、上記でも述べたように、インプットキャプチャ回転速度算出部１８１３に、「ダミー」となる測定を数回程度行わせることにより解決できる。
【０１４７】
２つ目は、インプットキャプチャ回転速度算出部１８１３が動作を開始する際、インプットキャプチャ回転速度算出部１８１３が、サーボモータ１４２の回転方向を正確に把握できていないことである。
【０１４８】
上記２つ目の原因を解決するため、位相計数回転速度算出部１８１４により予め回転方向を判定した上で、インプットキャプチャ回転速度算出部１８１３によりサーボモータ１４２の回転速度の算出を開始する。
【０１４９】
このように、モータ指令速度が所定の第１回転速度よりも大きく、第２回転速度よりも小さいときに、インプットキャプチャ回転速度算出部１８１３に「ダミー」となる測定を数回程度行わせ、位相計数回転速度算出部１８１４により予め回転方向を判定した上で、インプットキャプチャ回転速度算出部１８１３によるサーボモータ１４２の回転速度の算出を開始することで、インプットキャプチャ回転速度算出部１８１３と位相計数回転速度算出部１８１４の動作がスムーズに切り替えられる。
【０１５０】
すなわち、サーボモータ１４２の回転速度の算出結果の精度を下げることなく、インプットキャプチャ回転速度算出部１８１３と位相計数回転速度算出部１８１４の動作が切り替えられる。そのため、絶えず精度のよいサーボモータ１４２の回転速度が得られる。その結果、サーボモータ速度制御装置１８０が、機能不全に陥ることなく、絶えず精度よくサーボモータ１４２の制御を行える。
【０１５１】
なお、特にサーボモータ１４２の減速時に、サーボモータ１４２の回転速度の変化量が大きくなった結果、モータ指令速度が第１回転速度よりも大きく、第２回転速度よりも小さい範囲にある期間が短くなった場合、インプットキャプチャ回転速度算出部１８１３による「ダミー」の回転速度の算出の回数が少なく、上記の効果を十分に得られなくなる可能性がある。
【０１５２】
この場合、インプットキャプチャ回転速度算出部１８１３による「ダミー」の回転速度の算出を、モータ指令速度が第１回転速度以下となっても引き続き行い、所定の回数以上の「ダミー」の回転速度の算出を行ってから、インプットキャプチャ回転速度算出部１８１３により算出された回転速度を、サーボモータ１４２の回転速度として出力するようにすればよい。
【０１５３】
これにより、インプットキャプチャ回転速度算出部１８１３による「ダミー」の回転速度の算出を十分な回数行うことができる。その結果、サーボモータ１４２の回転速度が第１回転速度以下となったときに、インプットキャプチャ回転速度算出部１８１３により精度よくサーボモータ１４２の回転速度を算出できる。
【０１５４】
（１−３）本実施形態の作用効果
ここで、第１実施形態における自律走行ロボット１の作用効果について述べる。第１実施形態における自律走行ロボット１に取り付けられたサーボモータ速度制御装置１８０は、モータ指令速度が第１回転速度以下の低速度領域では、インプットキャプチャ法により自律走行ロボット１のオムニホイール機構１４に取り付けられたサーボモータ１４２の回転速度を算出し、第２回転速度以上の高速度領域では、位相計数法によりサーボモータ１４２の回転速度を算出し、第１回転速度よりも大きく第２回転速度よりも小さい回転速度領域では、上記両方の回転速度算出法による回転速度の算出を行いつつ、位相計数法による回転速度算出結果（位相計数回転速度）を、サーボモータ１４２の回転速度として採用する。
【０１５５】
このように、それぞれの長所を生かした回転速度算出法を適切な回転速度範囲で用いることにより、サーボモータ速度制御装置１８０は、精度よくサーボモータ１４２の回転速度を算出できる。
【０１５６】
さらに、回転速度の算出精度が向上すれば、サーボモータ速度制御装置１８０は精度よくサーボモータ１４２を制御できる。そのため、自律走行ロボット１は、上位装置１８１からの移動方向及び移動速度などの制御指令に対して忠実に移動できる。その結果、自律走行ロボット１は、障害物にぶつかることなく最短距離で目的地へ移動できる。
【０１５７】
（２）上肢訓練装置
（２−１）全体構成
次に、本発明の一実施形態のサーボモータ速度制御装置を備えた他の実施形態（第２実施形態）である上肢訓練装置３について図１５を用いて説明する。図１５は、上肢訓練装置３の全体的な構成を示した、模式図である。
上肢訓練装置３は、脳血管障害及び脊髄損傷等の障害のために、上肢（特に、腕）の運動機能が損なわれた患者に対して回復運動を行うための装置である。
【０１５８】
上肢訓練装置３は、訓練装置本体３２と、椅子３４と、連結機構３６と、を備える。なお、以降の説明において、前後方向とは図１５のＸ方向であり、左右方向とは図１５のＹ方向であり、上下方向とは図１５のＺ方向である。
【０１５９】
椅子３４は、訓練装置本体３２と連結機構３６を介して接続され、上肢訓練装置３を用いて上肢の訓練を行うユーザを座らせる。
連結機構３６は、一端が訓練装置本体３２に回転可能に取り付けられ、他の一端が椅子３４に回転可能に取り付けられ、訓練装置本体３２と椅子３４とを連結する。連結機構３６は、訓練装置本体３２及び椅子３４と回転可能に取り付けられているため、椅子３４を訓練装置本体３２に対して所定の向きに向かせることができる。また、連結機構３６には、連結機構３６の途中にさらなる回転機構３６１を備えていてもよい。この場合、さらに、椅子３４と訓練装置本体３２との距離を調整できる。これにより、訓練装置本体３２と椅子３４との配置関係を、それぞれのユーザに合わせた配置関係に調整できる。
【０１６０】
訓練装置本体３２は、連結機構３６を介して椅子３４と接続され、椅子３４に座ったユーザの上肢の訓練を行う。
【０１６１】
訓練装置本体３２は、本体３１と、操作ロッド３３と、モニタスタンド３５と、モニタ３７と、を備える。
【０１６２】
本体３１は、椅子３４からユーザの視線方向（Ｘ方向の前方向）に所定の距離離れた端部にモニタスタンド３５を固定する。また、本体３１は、内部に操作ロッド傾動機構３１０（図示せず）を備え、この操作ロッド傾動機構３１０により操作ロッド３３が所定の傾動中心に前後左右に傾動する。この操作ロッド傾動機構３１０にサーボモータと、当該サーボモータの回転速度を制御する本実施形態のサーボモータ速度制御装置とを採用できる。
【０１６３】
操作ロッド傾動機構３１０に、サーボモータと、当該サーボモータの回転速度を制御する本実施形態のサーボモータ速度制御装置とを採用することにより、操作ロッド３３の傾動時にサーボモータ制御を行う際、より実際に近いサーボモータの回転速度を把握することができる上に、速度指令出力の遅延がなくなる。これにより、意図しない動作のムラや変動が生じなくなる。その結果、上肢訓練装置３を使用するユーザが違和感や不快感を持たなくなる。
【０１６４】
（３）他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
【産業上の利用可能性】
【０１６５】
本発明に係るサーボモータ速度制御装置は、サーボモータを広い回転速度範囲で精度よく制御するサーボモータ速度制御装置に広く適用できる。
【符号の説明】
【０１６６】
１          自律走行ロボット
１２        本体
１４        オムニホイール機構
１４１      オムニホイール
１４２      サーボモータ
１４２１    エンコーダ
１６        レーザレンジファインダ
１８        走行制御コントローラ
１８０      サーボモータ速度制御装置
１８１０    速度制御部
１８１１    モータ速度指令部
１８１２    回転速度判定部
１８１３    インプットキャプチャ回転速度算出部
１８１４    位相計数回転速度算出部
１８１５    回転速度出力部
１８２０    モータ制御部
１８３０    エンコーダ出力分岐部
１８１      上位装置
２          物体
３          上肢訓練装置
３１        本体
３１０      操作ロッド傾動機構
３２        訓練装置本体
３３        操作ロッド
３４        椅子
３５        モニタスタンド
３６        連結機構
３６１      回転機構
３７        モニタ
【図１】
2014057432.tif 000003
【図２】
2014057432.tif 000004
【図３】
2014057432.tif 000005
【図４】
2014057432.tif 000006
【図５】
2014057432.tif 000007
【図６】
2014057432.tif 000008
【図７】
2014057432.tif 000009
【図８】
2014057432.tif 000010
【図９】
2014057432.tif 000011
【図１０】
2014057432.tif 000012
【図１１】
2014057432.tif 000013
【図１２】
2014057432.tif 000014
【図１３】
2014057432.tif 000015
【図１４】
2014057432.tif 000016
【図１５】
2014057432.tif 000017