(57)【要約】【課題】複数の制御対象のモデル化を低い次数かつ少ない計算量で容易に行うことができ、非線形な制御対象にも適用可能であり、簡易に構成可能な状態フィードバック制御装置を提供する。【解決手段】  制御対象１３０１〜１３０ｍの位置検出手段と、制御指令値を前記制御対象１３０１〜１３０ｍに出力するフィードバック加速度制御器１２０１〜１２０ｍと、加速度指令値を前記加速度制御器１２０１〜１２０ｍに出力するフィードバック速度制御器１１０１〜１１０ｍと、速度指令値を前記速度制御器１１０１〜１１０ｍに出力するフィードバック位置制御器１０００とを備え、前記制御対象１３０１〜１３０ｍと、前記加速度制御器１２０１〜１２０ｍと、前記速度制御器１１０１〜１１０ｍとを含めてそれぞれ１次遅れ系でモデル化し、前記位置制御器１０００は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行う。【選択図】図１
2005078328.tif 000002
【特許請求の範囲】
【請求項１】
複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される加速度データおよび加速度指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の加速度制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の加速度制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の加速度制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする状態フィードバック制御装置。
【請求項２】
前記入力される加速度データおよび前記入力される速度データは、前記位置検出手段により検出された位置データから求める請求項１に記載の状態フィードバック制御装置。
【請求項３】
複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される電流データおよび電流指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の電流制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の電流制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の電流制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする状態フィードバック制御装置。
【請求項４】
前記入力される電流データおよび前記入力される速度データは、前記位置検出手段により検出された位置データから求める請求項３に記載の状態フィードバック制御装置。
【請求項５】
複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象の加速度および速度を求めることが可能な物理量を検出する複数の検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される加速度データおよび加速度指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の加速度制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の加速度制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の加速度制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする状態フィードバック制御装置。
【請求項６】
前記複数の検出手段は、前記制御対象の加速度を検出する請求項５に記載の状態フィードバック制御装置。
【請求項７】
複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象の電流および速度を求めることが可能な物理量を検出する複数の検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される電流データおよび電流指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の電流制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の電流制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の電流制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする状態フィードバック制御装置。
【請求項８】
前記複数の検出手段は、前記制御対象の速度を検出する請求項５および７に記載の状態フィードバック制御装置。
【請求項９】
前記複数の検出手段は、前記制御対象の電流を検出する請求項７に記載の状態フィードバック制御装置。
【請求項１０】
前記制御対象、前記加速度制御器および前記速度制御器を含む速度フィードバック制御器の特性は、下記式（１）で表され、
下記式（２）に示す状態方程式により、
下記式（３）のように近似的にモデル化される請求項１ないし９のいずれかに記載の状態フィードバック制御装置。
【数１】
2005078328.tif 000015
【数２】
2005078328.tif 000016
【数３】
2005078328.tif 000017
【請求項１１】
前記速度制御器は、ＰＩＤ制御器またはファジー制御器である請求項１ないし１０のいずれかに記載の状態フィードバック制御装置。
【請求項１２】
前記制御対象は、アクチュエータである請求項１ないし１１のいずれかに記載の状態フィードバック制御装置。
【請求項１３】
複数の関節と、該関節を駆動させる複数のアクチュエータとを備えた多関節アームロボットの制御に用いるものである請求項１ないし１２のいずれかに記載の状態フィードバック制御装置。
【請求項１４】
前記多関節アームロボットのヨー角度、ヨー角速度、ヨー角加速度、ロール角度、ロール角速度、ロール角加速度、ピッチ角度、ピッチ角速度およびピッチ角加速度のうちの少なくとも１つを制御する請求項１３に記載の状態フィードバック制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態フィードバック制御装置に関するものである。
【背景技術】
【０００２】
従来、多入力多出力型に適応可能な状態フィードバック制御装置は、その制御対象を物理パラメータで同定したのち、線形化して状態空間モデルとして表現したものを利用して構成していた。
例えば、状態フィードバック制御を高次系システムに適用した、多入力多出力型の状態フィードバック制御装置に関する技術が開示されている。（特許文献１および特許文献２参照）
【０００３】
このように、多入力多出力型の複雑な制御対象を制御するために状態フィードバック制御器を構成する場合には、その制御対象を物理パラメータで同定した後、線形化して状態空間モデルとして表現して利用している。すなわち、状態フィードバック制御器を構成するには、その制御対象をモデル化（数式化）することが不可欠である。
しかしながら、従来の多入力多出力型に適応可能な状態フィードバック制御装置においては、制御対象が複雑になるとモデル化することが非常に難しくなる。従って、多入力多出力型に適応可能な状態フィードバック制御装置を実際に構成できないことが多い。
【０００４】
また、実際のモデルは次数が高い（３次以上）ので、それを利用して構成される状態フィードバック制御も複雑になって計算量が増加する。これにより、多入力多出力型に適応可能な状態フィードバック制御装置を実際に実現することは困難となる。
また、従来の状態フィードバック制御は微妙な調整が困難であり、モデルの感度が高いので、モデルが時系列で変化するような制御対象に対しては適応が困難である。
また、基本的には、非線形な制御対象や制御対象の非線形領域には適応不可能である。
【０００５】
【特許文献１】特開平５−５３６０２号公報
【特許文献２】特開平５−５３６０３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
本発明の目的は、複数の制御対象のモデル化を低い次数（２次以下）かつ少ない計算量で容易に行うことができ、非線形な制御対象にも適用可能であり、簡易に構成することができる状態フィードバック制御装置を提供することにある。
【課題を解決するための手段】
【０００７】
このような目的は、下記の本発明により達成される。
本発明の状態フィードバック制御装置は、複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される加速度データおよび加速度指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の加速度制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の加速度制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の加速度制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする。
これにより、複雑な制御対象をモデル化することができる。また、非線形な制御対象や制御対象の非線形領域にも適応可能となる。
本発明の状態フィードバック制御装置では、前記入力される加速度データおよび前記入力される速度データは、前記位置検出手段により検出された位置データから求めることが好ましい。
これにより、よりセンサの数を減らすことができ、コストを低減することができる。
【０００８】
本発明の状態フィードバック制御装置は、複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される電流データおよび電流指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の電流制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の電流制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の電流制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする。
これにより、複雑なサーボ系をモデル化することができる。また、非線形な制御対象や制御対象の非線形領域にも適応可能となる。
本発明の状態フィードバック制御装置では、前記入力される電流データおよび前記入力される速度データは、前記位置検出手段により検出された位置データから求めることが好ましい。
これにより、センサの数を減らすことができ、コストを低減することができる。
【０００９】
本発明の状態フィードバック制御装置は、複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象の加速度および速度を求めることが可能な物理量を検出する複数の検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される加速度データおよび加速度指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の加速度制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の加速度制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の加速度制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする。
これにより、複雑な制御対象をモデル化することができる。また、非線形な制御対象や制御対象の非線形領域にも適応可能となる。
【００１０】
本発明の状態フィードバック制御装置では、前記複数の検出手段は、前記制御対象の加速度を検出することが好ましい。
これにより、制御対象を好適に制御することができる。
本発明の状態フィードバック制御装置は、複数の制御対象の位置を検出する複数の位置検出手段と、
前記複数の制御対象の電流および速度を求めることが可能な物理量を検出する複数の検出手段と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される電流データおよび電流指令値データに基づいて、フィードバック制御を行い、前記制御対象に出力する複数の電流制御器と、
前記複数の制御対象にそれぞれ対応して設けられ、入力される速度データおよび速度指令値データに基づいて、フィードバック制御を行い、前記複数の電流制御器に出力する複数の速度制御器と、
前記位置検出手段の検出結果に基づいて得られる位置データおよび予め設定された位置指令値データを用いてフィードバック制御を行い、得られる速度指令値データを前記複数の速度制御器にそれぞれ出力する位置制御器とを備え、
前記複数の制御対象と、前記複数の電流制御器と、前記複数の速度制御器とを含めてそれぞれ１次遅れ系でモデル化し、
前記位置制御器は、前記１次遅れ系のモデルを使用して状態フィードバック制御を行うよう構成されていることを特徴とする。
これにより、複雑なサーボ系をモデル化することができる。また、非線形な制御対象や制御対象の非線形領域にも適応可能となる。
【００１１】
本発明の状態フィードバック制御装置では、前記複数の検出手段は、前記制御対象の速度を検出することが好ましい。
これにより、センサの数を減らすことができ、コストをより低減することができる。
本発明の状態フィードバック制御装置では、前記複数の検出手段は、前記制御対象の電流を検出することが好ましい。
これにより、制御対象を好適に制御することができる。
【００１２】
本発明の状態フィードバック制御装置では、前記制御対象、前記加速度制御器および前記速度制御器を含む速度フィードバック制御器の特性は、下記式（１）で表され、
下記式（２）に示す状態方程式により、
下記式（３）のように近似的にモデル化されることが好ましい。
【００１３】
【数１】
2005078328.tif 000003
【００１４】
【数２】
2005078328.tif 000004
【００１５】
【数３】
2005078328.tif 000005
これにより、状態フィードバック制御の微妙な調整ができ、モデルの感度が高くても状態フィードバック制御の適応が可能となる。
【００１６】
本発明の状態フィードバック制御装置では、前記速度制御器は、ＰＩＤ制御器またはファジー制御器であることが好ましい。
これにより、速度フィードバック制御器を簡易な構成とすることができ、複雑な制御対象をモデル化する場合には、速度フィードバック制御器の次数を低く抑え、かつ、計算量を少なくすることができる。
本発明の状態フィードバック制御装置では、前記制御対象は、アクチュエータであることが好ましい。
かかる構成により、複雑なメカ体等に好適に適用できる。
【００１７】
本発明の状態フィードバック制御装置では、複数の関節と、該関節を駆動させる複数のアクチュエータとを備えた多関節アームロボットの制御に用いるものであることが好ましい。
かかる構成により、多関節アームロボットの姿勢制御に適用できる。
本発明の状態フィードバック制御装置では、前記多関節アームロボットのヨー角度、ヨー角速度、ヨー角加速度、ロール角度、ロール角速度、ロール角加速度、ピッチ角度、ピッチ角速度およびピッチ角加速度のうちの少なくとも１つを制御することが好ましい。
かかる構成により、多関節アームロボットの姿勢制御にさらに好適に適用できる。
【発明を実施するための最良の形態】
【００１８】
以下、図１〜図１１を参照して本発明の状態フィードバック制御装置を説明する。なお、この実施形態は例示として挙げるものであり、これにより本発明の内容を限定的に解釈すべきではない。
図１は、本発明の状態フィードバック制御装置の構成を模式的に示すブロック図である。
【００１９】
本発明の状態フィードバック制御装置は、制御対象群１３００と、加速度制御対象群１２００と、速度制御器群１１００と、位置制御器１０００とを備える。
制御対象群１３００は、複数の制御対象１３０１〜１３０ｍ（ｍ＝１、２、３、・・・、ｎ）を有している。これらの複数の制御対象１３０１〜１３０ｍは、加速度制御器１２０１〜１２０ｍのうちの対応する加速度制御器からの制御により、その加速度が制御される。
【００２０】
加速度制御器群１２００は、複数の加速度制御器１２０１〜１２０ｍを有しており、複数の加速度制御器１２０１〜１２０ｍは複数の制御対象１３０１〜１３０ｍにそれぞれ対応して設けられている。各加速度制御器１２０１〜１２０ｍは、制御対象１３０１〜１３０ｍを実際に測定した加速度データを、速度制御器１１０１〜１１０ｍから出力される加速度指令値データから減算した値に基づいて、制御対象１３０１〜１３０ｍの加速度を制御する。
ここで、加速度指令値データとは、各制御対象１３０１〜１３０ｍを所定の加速度で動作させるデータ（指示データ）である。
【００２１】
速度制御器群１１００は、複数の速度制御器１１０１〜１１０ｍを有しており、複数の速度制御器１１０１〜１１０ｍは複数の制御対象１３０１〜１３０ｍにそれぞれ対応して設けられている。各速度制御器１１０１〜１１０ｍは、制御対象１３０１〜１３０ｍを加速度データから演算した速度データを、位置制御器１０００から出力される速度指令値データから減算した値に基づいて、制御対象１３０１〜１３０ｍの速度を制御する。
ここで、速度指令値データとは、各制御対象１３０１〜１３０ｍを所定の速度で動作させるデータ（指示データ）である。
【００２２】
なお、加速度制御器１２０１〜１２０ｍと、速度制御器１１０１〜１１０ｍとは、ＰＩＤ制御器またはファジー制御器等のより簡易な構成を用いることが好ましい。これにより、制御対象１３０１〜１３０ｍ、加速度制御器１２０１〜１２０ｍ、および速度制御器１１０１〜１１０ｍから構成される速度フィードバック制御器を簡易な構成とすることができる。これにより、複雑な制御対象をモデル化する場合には、速度フィードバック制御器の次数を低く抑え、かつ、計算量を少なくすることができる。
【００２３】
位置制御器１０００には、複数の制御対象１３０１〜１３０ｍそれぞれに対応する位置データを入力する。この位置データおよび予め設定された位置指令値データＲを用いて状態フィードバック制御を行い、得られる速度指令値データを速度制御器１１０１〜１１０ｍに出力する。これにより、制御対象１３０１〜１３０ｍの位置が制御される。
ここで、位置指令値データＲ１〜Ｒｎは、複数の制御対象１３０１〜１３０ｍを、たとえば、絶対方位に対して所定の位置に移動させるデータ（指示データ）である。また、位置データは、図示しない位置検出手段により検出されて位置制御器１０００に入力される。
【００２４】
図１に示す位置制御器１０００においては、Ｋはフィードバックゲインを、Ｈはオブザーバー（観測器）ゲインを、ｕは入力を、Ｓは自乗数を、ｘは状態変数を、ｘ（ｎ）は状態変数ｘをｎ回微分したものを、Ａ、Ｂ、Ｃは状態変数ｘの各要素の行列ベクトルを示す。
なお、本実施形態においては、状態変数ｘの代わりに、オブザーバー（観測器）による推定値ｘ’を用いて状態フィードバック制御を行っている。
【００２５】
次に、状態フィードバック制御器である、位置制御器の構成方法について説明する。
まず、複数の制御対象１３０１〜１３０ｍに対して、加速度制御器１２０１〜１２０ｍ、速度制御器１１０１〜１１０ｍをそれぞれ構成する。
なお、この加速度制御器１２０１〜１２０ｍおよび、速度制御器１１０１〜１１０ｍは、フィードバック制御器で構成する。
【００２６】
この速度フィードバック制御器の構成については特に限定されないが、ＰＩＤや、ファジー制御により構成されるのが好ましい。これにより、フィードバック制御器を簡易な構成とすることができる。
さらに、複数の制御対象１３０１〜１３０ｍと、複数の加速度制御器１２０１〜１２０ｍとを含んで成る複数の速度フィードバック制御器の全てをそれぞれ１次遅れ系にてモデル化する。
このとき、前記速度フィードバック制御器を含めた状態フィードバック制御器の特性は、式（１）で表すことができる。
【００２７】
【数４】
2005078328.tif 000006
これにより、位置制御器を式（３）のように、低次かつ、簡易的なモデルで構成することができる。
【００２８】
【数６】
2005078328.tif 000007
次に式（３）を式（２）に示す状態方程式により、記述する。
【００２９】
【数５】
2005078328.tif 000008
式（２）は、一般的な状態フィードバック制御における状態方程式であるため、数式についての詳細な説明は省略する。
一般的に、状態変数ｘは、入力をｕ、フィードバック係数行列をＫ（ゲインを表す）とするときに、式（４）で表される。
【００３０】
【数７】
2005078328.tif 000009
式（４）を式（５）に代入することにより、式（６）が得られる。
【００３１】
【数８】
2005078328.tif 000010
【００３２】
【数９】
2005078328.tif 000011
式（６）のＫを調整することにより、位置制御器の安定性（収束性）や応答性を求めることができる。
【００３３】
さらに、この状態フィードバック制御器に観測器（オブザーバー）を用いて本実施形態の位置制御器全体のシステムを構成する。
また、フィードバック係数行列Ｋを乗算した値は、新たな速度指令値データとして、１１０１〜１１０ｍのうちの対応する速度制御器に出力される。
上述したように、本発明の状態フィードバック制御装置は、制御対象、加速度制御器および速度制御器により構成する複数の速度フィードバック制御器の全てを、それぞれ１次遅れ系にてモデル化し、これらを使用して状態フィードバック制御を行う構成とするので、ロボット等の複雑なメカ体や複数の制御対象が干渉し合うような複雑な制御対象を簡易にモデル化することができる。
【００３４】
また、状態フィードバック制御の微妙な調整ができるので、モデルの感度が高くてモデルが時系列で変化するような制御対象に対しても適応が可能となる。
また、非線形な制御対象や制御対象の非線形領域にも適応可能となる。
なお、本発明の状態フィードバック制御装置における状態フィードバック制御には、その他の先進制御等も含まれるものである。
【００３５】
また、加速度制御器の代わりに角加速度制御器を用い、速度制御器の代わりに角速度制御器を用い、位置制御器の代わりに角度制御器を用いる構成としても良い。
また、本実施形態において、速度データ、加速度データは、実際に測定したものを用いたが、これに限らず、例えば、位置データなど他の物理量から演算して求めた速度データ、加速度データを用いてもよい。
【００３６】
なお、加速度データの代わりに電流データを測定することにより、サーボ系にも適用することができる。
本発明の状態フィードバック制御装置の制御対象としては特に限定されないが、例えばアクチュエータを用いることができる。具体的には、たとえば、本発明の状態フィードバック制御装置の制御対象としての複数の関節を回転駆動させるアームロボットに、複数のアクチュエータを用いることにより、アームロボットの姿勢を容易に制御することが可能になる。
【００３７】
ここで、図１に示す本発明の状態フィードバック制御装置を多関節アームロボットの姿勢制御に適用した場合の模式的なブロック構成を図２に示す。図２に示す構成は、図１に示す構成とほぼ同様であるので、相違点のみを説明する。
また、本実施形態においては、加速度制御器の代わりに角加速度制御器を用い、速度制御器の代わりに角速度制御器を用い、位置制御器の代わりに角度制御器を用いる構成としている。
【００３８】
制御対象群１３００は、アクチュエータ１３０１〜１３０１２を制御対象として有している。すなわち、多関節アームロボットは、ヨー（θｚ）、ロール（θｘ）およびピッチ（θｙ）をそれぞれ制御する複数のアクチュエータを備えている。
角加速度制御器群１６００は、アクチュエータ１３０１〜１３０１２を制御する角加速度制御器１６０１〜１６０１２を有している。
【００３９】
また、角速度制御器群１５００は、角加速度制御器１６０１〜１６０１２を制御する角速度制御器１５０１〜１５０１２を有している。
また、角度制御器１４００には、角度指令値データとして、アクチュエータ１３０１〜１３０１２のそれぞれの角度を制御する角度指令値データ、角度１〜角度１２が入力される。
【００４０】
角度制御器１４００は、アクチュエータ１３０１〜１３０１２それぞれを測定した角度データおよび角度指令値データ、角度１〜角度１２を用いて、状態フィードバック制御を行う。これにより、後述するように、多関節アームロボットのヨー（θｚ）、ロール（θｘ）、ピッチ（θｙ）それぞれの角度が好適に制御される。
なお、図２では、多関節アームロボットの姿勢制御のために、アクチュエータ（制御対象）１３０１〜１３０１２を用いているが、制御対象は、これらに限定されるものではない。
【００４１】
次に、図２に示す状態フィードバック制御装置の制御処理を説明する。図３は、図２に示す状態フィードバック制御装置における状態フィードバック制御の処理を示すフローチャートである。まず、図２に示す状態フィードバック制御装置では、角度制御器１４００、角速度制御器１１０１〜１１０１２および角加速度制御器１６０１〜１６０１２の各パラメータを初期化すると共に、積分器および微分器（図示せず）の値を初期化する（ステップＳ１０１）。角度制御器１４００、角速度制御器１５０１〜１５０１２、角加速度制御器１６０１〜１６０１２、積分器、および微分器が初期化された状態で、１２個の角度検出手段（図示せず）により検出した現在の角度は、サンプリングされて角度データ（アナログ信号）としてセンサ信号入力手段に出力され、１２個の角速度検出手段（図示せず）により検出した現在の角速度は、サンプリングされて角速度データ（アナログ信号）としてセンサ信号入力手段に出力され、１２個の角加速度検出手段（図示せず）により検出した現在の角加速度はサンプリングされて角加速度データ（アナログ信号）としてセンサ信号入力手段（図示せず）に出力される（ステップＳ１０２）。
【００４２】
センサ信号入力手段はＡＤＣ（アナログ／デジタルコンバータ）およびカウンタ等から成り、角度データおよび角速度データをＡ／Ｄ変換（アナログデ―タからデジタルデータに変換）する。ＡＤＣによってＡ／Ｄ変換された実測角加速度データ、角速度データ、角度データをそれぞれ記憶部に格納する（ステップＳ１０３、Ｓ１０４）。
なお、本実施形態においては、検出手段によりサンプリングされた角度データを用いたが、本発明では、角加速度データまたは角速度データを、積分器により積分処理することにより得た角度データを用いてもよい。
【００４３】
また、本実施形態においては、検出手段によりサンプリングされた角速度データを用いたが、本発明では、角度データを微分器により微分処理することにより得た角速度データを用いてもよい。
また、本実施形態においては、検出手段によりサンプリングされた角加速度データを用いたが、本発明では、角度データまたは角速度データを、微分器により微分処理することにより得た角加速度データを用いてもよい。
【００４４】
多関節アームロボットは、図示しないリモートコントローラなどからユーザ（操縦者）によって入力され、送信される、アクチュエータ１３０１〜１３０１２を角度制御する角度指令値データ角度１〜角度１２を受信し（ステップＳ１０５）、この角度指令値データから、記憶部に格納されている実測角度をそれぞれ減算する（ステップＳ１０６）。
ここで、ステップＳ１０７において、上記処理で得られた減算結果（角度差データ）が異常な値であるか否かを判断する。角度差データが異常値である場合には、ステップＳ１０１に移行し（戻り）、再度、角度制御器１４００、角速度制御器１５０１〜１５０１２、角加速度制御器１６０１〜１６０１２、積分器、および微分器を初期化し、同様の処理を繰り返す。一方、角度指令値データが異常値でない（正常な）場合には、ステップＳ１０８に移行する。
ステップＳ１０８では、アクチュエータ１３０１〜１３０１２と、それらに対応する角加速度制御器１６０１〜１６０１２および角速度制御器１５０１〜１５０１２とを含めてそれぞれ１次遅れ系、式（１）にてモデル化する。
【００４５】
【数１０】
2005078328.tif 000012
次に、式（１）にて得られたモデルに対し、状態方程式（２）を用いて記述し（ステップＳ１０９）、角度制御器１４００を状態フィードバック制御アルゴリズムにしたがい構成する。
【００４６】
【数１１】
2005078328.tif 000013
前記ステップＳ１０３、Ｓ１０４により得られた、角度指令値データと実測角度データとを、前記状態フィードバック制御器に入力し、出力された角速度指令値データを記憶部に格納する（ステップＳ１０８）。
なお、この処理は、リアルタイムにモデル化を有する場合にのみ行うべき状態フィードバック制御のための処理である。すなわち、リアルタイムにモデル化を有していない場合には、オフラインで前記方法により状態フィードバック制御器を構成しておく。
【００４７】
ここで、ステップＳ１１０において、記憶部に格納されている角速度指令値データ（角速度データ）が異常値であるか否かを判断する。角速度指令値データが異常値である場合には、ステップＳ１０１に移行し（戻り）、再度、角度制御器１４００、角速度制御器１５０１〜１５０１２、角加速度制御器１６０１〜１６０１２、積分器、および微分器を初期化し、同様の処理を繰り返す。一方、角速度指令値データが異常値でない（正常な）場合には、ステップＳ１１１に移行する。
【００４８】
角速度指令値データが異常値でない場合、すなわち、角速度指令値データが正常値である場合には、その角速度指令値データを出力する。この角速度指令値から、ＡＤＣによってサンプリングされた実測角速度データを減算し（ステップＳ１１１）、その減算結果を角速度制御器１５０１〜１５０１２に入力する。
入力された減算結果（角速度データ）に基づいて、浮揚体の角速度をＰＩＤ制御して、角加速度指令値を出力する（ステップＳ１１２）。
【００４９】
ここで、ステップＳ１１３において、ステップＳ１１２において出力された角加速度指令値データが異常値であるか否かを判断する。角加速度指令値データが異常値である場合には、ステップＳ１０１に移行し（戻り）、再度、角度制御器１４００、角速度制御器１５０１〜１５０１２、角加速度制御器１６０１〜１６０１２、積分器、および微分器を初期化し、同様の処理を繰り返す。一方、角速度指令値データが異常値でない（正常な）場合には、ステップＳ１１４に移行する。
【００５０】
角加速度指令値データが異常値でない場合、すなわち、角加速度指令値データが正常値である場合には、その角加速度指令値データを出力する。この角加速度指令値からＡＤＣによってサンプリングされた実測角加速度データを減算し（ステップＳ１１４）、その減算結果を角加速度制御器１６０１〜１６０１２に入力する。
入力された減算結果（角速度データ）に基づいて、アクチュエータの角加速度をＰＩＤ制御して、角加速度差制御信号を出力する（ステップＳ１１５）。
上記角加速度差制御信号を基に、対応するアクチュエータに駆動指令を出力し、アクチュエータを回転駆動することによって、多関節アームロボットを制御する（ステップＳ１１６）。
【００５１】
次に、図４および図５を参照して、本発明の状態フィードバック制御装置により制御される多関節アームロボット１０を説明する。なお、この多関節アームロボット１０は例示として挙げるものであり、これにより本発明の状態フィードバック制御装置の内容を限定的に解釈すべきではない。
図４および図５は、本発明による角加速度、角速度制御器を備えた状態フィードバック制御器を適用した多関節アームロボット１０の一実施形態の構成を示している。図４において、多関節アームロボット１０は、本体である上体１１と、上体１１の下部両側に取り付けられた中間に肘部１２Ｌ、１２Ｒを備えた二本の腕部１３Ｌ、１３Ｒと、各腕部１３Ｌ、１３Ｒの下端に取り付けられた手部１４Ｌ、１４Ｒとを含んでいる。
【００５２】
ここで、前記腕部１３Ｌ、１３Ｒはそれぞれ六個の関節部、即ち上方から順に、上体１１に対する肩の腕部ヨー方向回旋用（ｚ軸周り）の関節部ＬＡ１、ＲＡ１と、肩のロール方向（ｘ軸周り）の関節部ＬＡ２、ＲＡ２と、肩のピッチ方向（ｙ軸周り）の関節部ＬＡ３、ＲＡ３と、肘部１２Ｌ、１２Ｒのピッチ方向の関節部ＬＡ４、ＲＡ４と、手部１４Ｌ、１４Ｒに対する手首部のロール方向の関節部ＬＡ５、ＲＡ５と、手首部のピッチ方向の関節部ＬＡ６、ＲＡ６とを備えている。なお、各関節部ＬＡ１、ＲＡ１ないしＬＡ６、ＲＡ６は、それぞれ関節駆動用アクチュエータにより構成されている。
【００５３】
また、各関節駆動用アクチュエータにはそれぞれ検出用にロータリエンコーダが備えられている。
手関節は、関節部ＬＡ５、ＲＡ５、ＬＡ６、ＲＡ６から構成されることになる。さらに、肩関節と肘関節との間は、上腕リンク２１Ｌ、２１Ｒにより連結されており、また肘関節と手関節との間は、前腕リンク２２Ｌ、２２Ｒにより連結されている。これにより、多関節アームロボット１０の左右両側の腕部１３Ｌ、１３Ｒおよび手部１４Ｌ、１４Ｒは、それぞれ６自由度を与えられることになり、１２個の関節部をそれぞれ駆動モータにより適宜の角度に駆動制御することにより、腕部１３Ｌ、１３Ｒ、手部１４Ｌ、１４Ｒ全体に所望の動作を与えて、任意に三次元空間の姿勢制御ができるように構成されている。
【００５４】
図５は、図４に示した多関節アームロボット１０の電気的構成を示している。
図４において、多関節アームロボット１０は、要求動作に対応して位置指令値を生成する位置指令生成部２４と、この位置指令値に基づいて、駆動手段、即ち前述した各関節部、即ち関節駆動用アクチュエータＬＡ１、ＲＡ１ないしＬＡ６、ＲＡ６を駆動制御する制御装置３０とを備えている。なお、多関節アームロボット１０の座標系として、図５において、前後方向をピッチ軸方向（前方＋）、横方向をロール軸方向（内方＋）そして上下方向をヨー軸方向（上方＋）とするｘｙｚ座標系を使用する。
【００５５】
前記制御装置３０は、角度計測ユニット３１と、制御部３２とから構成されている。角度計測ユニット３１は、各関節部ＬＡ１、ＲＡ１ないしＬＡ６、ＲＡ６の関節駆動用アクチュエータに備えられた、例えばロータリエンコーダ等により、各関節駆動用アクチュエータの角度情報が入力されることにより、各関節駆動用アクチュエータの角度を計測して、制御部３２に出力する。
【００５６】
制御部３２は、状態フィードバック制御器（角度制御器）、角速度制御器、角加速度制御器とから構成されている。
制御部３２は角度計測ユニット３１から出力された角度データ（角度実測値）と、位置指令生成部からの角度指令値とを基に、角加速度指令値データを出力する。
前記角加速度指令値データは、対応するアクチュエータ１３０１〜１３０１２に出力される。
【００５７】
次に、本実施形態による、状態フィードバック制御器（角度制御器）の構成について述べる。
まず、制御対象である、各関節部ＬＡ１、ＲＡ１ないしＬＡ６、ＲＡ６のそれぞれ関節駆動用アクチュエータ１３０１〜１３０１２に対して、各アクチュエータ１３０１〜１３０１２を制御する角加速度制御器１６０１〜１６０１２をそれぞれ構成する。
【００５８】
また、角加速度制御器１６０１〜１６０１２に対して、角加速度制御器１６０１〜１６０１２を制御する角速度制御器１５０１〜１５０１２をそれぞれ構成する。
なお、前記角加速度制御器１６０１〜１６０１２と、角速度制御器１５０１〜１５０１２とは、それぞれフィードバック制御器で構成されている。
このフィードバック制御器の構成については特に限定されないが、ＰＩＤ制御や、ファジー制御により構成されるのが好ましい。これにより、フィードバック制御器を簡易な構成とすることができる。
次に、アクチュエータ１３０１〜１３０１２を含んで成る複数の角加速度フィードバック制御器、角速度フィードバック制御器の全てをそれぞれ１次遅れ系にてモデル化する。
モデル化された特性を式（２）で記述し、角度制御器を状態フィードバックアルゴリズムに従い構成する。
【００５９】
【数１２】
2005078328.tif 000014
これにより、複数の制御対象のモデル化を低い次数かつ少ない計算量で簡易に構成することができる。
次に、多関節アームロボット１０に対して、本発明の状態フィードバック制御装置を実際に用いた場合の実験値を示す。図６〜図１１は、初期状態において０（ｒａｄ）を向いている多関節アームロボット１０の各関節部ＬＡ１ないしＬＡ６に、０．５秒の時点で、ヨー軸方向、ロール軸方向、ピッチ軸方向が最適となるような角度指令値を入力した際の、ステップ応答を示す図である。
【００６０】
各図において、（Ａ）は、多関節アームロボット１０の角度（実験値）のステップ応答を示し、（Ｂ）は、多関節アームロボット１０の角速度（実験値）のステップ応答を示し、（Ｃ）は、多関節アームロボット１０の角加速度（実験値）のステップ応答を示す。
また、図６はＬＡ１、図７はＬＡ２、図８はＬＡ３、図９はＬＡ４、図１０はＬＡ５、図１１はＬＡ６の各応答を示す。
これら図６〜図１１において、互いの干渉は小さく抑えられており、迅速にその指令された角度に収束していることが分かる。
【００６１】
以上説明したように、この多関節アームロボット１０は、各関節駆動用アクチュエータにより、その姿勢を変更、すなわち、姿勢を制御することができるものであるが、これに本発明の状態フィードバック制御装置を適用することにより、より簡易な構成により、多関節アームロボット１０を任意の方向（任意の角度）に容易に移動させることができる。
また，本実施形態では，関節を駆動させるアクチュエータは特に限定されず、サーボモータ、通常のモータ、油圧・空圧シリンダ、形状記憶合金等を用いてもよい。また、アクチュエータの駆動力を各関節に伝達する機構は、本実施形態の例に限定されず、例えばギヤ、ベルト、ワイヤ、チェーン等を介すなど、多様な態様でアクチュエータの駆動力を伝達してもよい。
【００６２】
また、本実施形態では、アクチュエータの検出装置として、ロータリエンコーダを用いたが、これに限定されず、例えば、ホールセンサ、絶縁アンプ、レゾルバ、タコジェネレータ等を用いてもよい。
以上、本発明の状態フィードバック制御装置を図示の各実施形態に基づいて説明したが、本発明は、これに限定されるものではなく、状態フィードバック制御装置を構成する各部は、同様の機能を発揮し得る任意の構成のものと置換することができる。また、本発明の状態フィードバック制御装置に、他の任意の構成物が付加されていてもよい。
【図面の簡単な説明】
【００６３】
【図１】本発明の状態フィードバック制御装置の一実施形態の構成を模式的に示すブロック図である。
【図２】図１に示す状態フィードバック制御装置を多関節アームロボットに適用した際の構成を模式的に示すブロック図である。
【図３】図２に示す状態フィードバック制御装置の制御処理を示すフローチャートである。
【図４】多関節アームロボットの斜視図である。
【図５】図２に示す状態フィードバック制御装置を適用する多関節アームロボットの斜視図である。
【図６】図４に示す多関節アームロボットのアクチュエータ１のステップ応答を示す図である。
【図７】図４に示す多関節アームロボットのアクチュエータ２のステップ応答を示す図である。
【図８】図４に示す多関節アームロボットのアクチュエータ３のステップ応答を示す図である。
【図９】図４に示す多関節アームロボットのアクチュエータ４のステップ応答を示す図である。
【図１０】図４に示す多関節アームロボットのアクチュエータ５のステップ応答を示す図である。
【図１１】図４に示す多関節アームロボットのアクチュエータ６のステップ応答を示す図である。
【符号の説明】
【００６４】
１０……多関節アームロボット  １１……上体  １２Ｌ、１２Ｒ……肘部  １３Ｌ、１３Ｒ……腕部  １４Ｌ、１４Ｒ……手部  １５……位置指令値  ２１Ｌ、２１Ｒ……上腕リンク  ２２Ｌ、２２Ｒ……前腕リンク  ２４……位置指令生成部  ３０……制御装置  ３１……角度計測ユニット  ３２……制御部  ３３……角度指令生成部  ３３１……角度指令値  １０００……位置制御器  １１００……速度制御器群  １１０１〜１１０ｎ……速度制御器  １２００……加速度制御器群  １２０１〜１２０ｎ……加速度制御器  １３００……制御対象群  １３０１〜１３０ｎ……制御対象  １４００……角度制御器  １５００……角速度制御器群  １５０１〜１５０１２……角速度制御器  １６００……角加速度制御器群  １６０１〜１６０１２……角加速度制御器  ＬＡ１〜ＬＲ６……関節部、ＲＡ１〜ＲＡ６……関節部  Ｓ１０１〜Ｓ１１６……ステップ  
【図１】
2005078328.tif 000018
【図２】
2005078328.tif 000019
【図３】
2005078328.tif 000020
【図４】
2005078328.tif 000021
【図５】
2005078328.tif 000022
【図６】
2005078328.tif 000023
【図７】
2005078328.tif 000024
【図８】
2005078328.tif 000025
【図９】
2005078328.tif 000026
【図１０】
2005078328.tif 000027
【図１１】
2005078328.tif 000028