(57)【要約】  対象のシステムのパラメータを同定するパラメータ同定装置（１０）は、システムの状態量を含む第１の量の一階微分値を、システムへの入力値および第１の量を用いて表す連続方程式である第１の方程式を記憶する第１の記憶部（１６）と、システムの出力を、状態量およびパラメータを含む拡大状態量と、一階微分値とを用いて表す第２の方程式を記憶する第２の記憶部（１８）と、第１の方程式と、第１の時間ステップでの第１の量と、第１の時間ステップでのシステムへの入力値とを用いて、第１の時間ステップの次の時間ステップである第２の時間ステップでの拡大状態量を算出する第１の算出部（２０）と、第１の方程式と、第２の方程式と、第１の時間ステップの拡大状態量と、第１の時間ステップの入力値とを用いて、第１の時間ステップにおけるシステムの出力を算出する第２の算出部（２２）と、時間ステップ毎に取得されるシステムへの入力値と、時間ステップ毎に取得されるシステムからの出力値と、第１の算出部と、第２の算出部とを用い、拡大状態量を推定する推定部（２４）と、を備えることを特徴とする。
2021014499.tif 000002
【特許請求の範囲】
【請求項１】
対象のシステムのパラメータを同定するパラメータ同定装置であって、
前記システムの状態量を含む第１の量の一階微分値を、前記システムへの入力値および前記第１の量を用いて表す連続方程式である第１の方程式を記憶する第１の記憶部と、
前記システムの出力を、前記状態量および前記パラメータを含む拡大状態量と、前記一階微分値とを用いて表す第２の方程式を記憶する第２の記憶部と、
前記第１の方程式と、第１の時間ステップでの第１の量と、前記第１の時間ステップでの前記システムへの入力値とを用いて、前記第１の時間ステップの次の時間ステップである第２の時間ステップでの前記拡大状態量を算出する第１の算出部と、
前記第１の方程式と、前記第２の方程式と、前記第１の時間ステップの前記拡大状態量と、前記第１の時間ステップの前記入力値とを用いて、前記第１の時間ステップにおける前記システムの出力を算出する第２の算出部と、
時間ステップ毎に取得される前記システムへの入力値と、時間ステップ毎に取得される前記システムからの出力値と、前記第１の算出部と、前記第２の算出部とを用い、前記拡大状態量を推定する推定部と、
を備えることを特徴とするパラメータ同定装置。
【請求項２】
前記第１の量は、前記拡大状態量であり、
前記第１の算出部は、前記第１の方程式の数値離散化を行うことで得られる第３の方程式を用いて前記拡大状態量を算出することを特徴とする請求項１に記載のパラメータ同定装置。
【請求項３】
前記パラメータは時不変であり、
前記第１の量は、前記状態量であり、
前記第１の算出部は、前記第１の方程式と、予め定められた数値積分手法とを用いて、前記第２の時間ステップでの前記状態量を算出し、算出した前記状態量と、前記パラメータが時不変であることとを用いて、前記第２の時間ステップでの前記拡大状態量を算出することを特徴とする請求項１に記載のパラメータ同定装置。
【請求項４】
前記拡大状態量および前記拡大状態量の一階微分値に基づいて推定外乱量を生成するための未知外乱推定モデルを記憶する第３の記憶部と、
前記第１の記憶部と、前記第３の記憶部とを用いて、第１の時間ステップでの前記拡大状態量および前記入力値に基づいて、前記推定外乱量を出力する外乱推定部と、
をさらに備え、
前記推定部は、前記推定外乱量を用いて前記拡大状態量を推定することを特徴とする請求項１から３のいずれか１項に記載のパラメータ同定装置。
【請求項５】
パラメータ同定装置が、対象のシステムのパラメータを同定するパラメータ同定方法であって、
時間ステップ毎に前記システムへの入力値を取得するステップと、
時間ステップ毎に前記システムからの出力値を取得するステップと、
前記システムの状態量を含む第１の量の一階微分値を、前記システムへの入力値および前記第１の量を用いて表す連続方程式である第１の方程式と、第１の時間ステップでの前記第１の量と、前記第１の時間ステップでの前記システムへの入力値とを用いて、前記第１の時間ステップの次の時間ステップである第２の時間ステップでの前記状態量および前記パラメータを含む拡大状態量を算出するステップと、
前記第１の方程式と、前記システムの出力を、前記拡大状態量と、前記一階微分値とを用いて表す第２の方程式と、前記第１の時間ステップでの前記拡大状態量と、前記第１の時間ステップでの前記入力値とを用いて、前記第１の時間ステップにおける前記システムの出力を算出するステップと、
前記拡大状態量を推定して前記パラメータを同定するステップと、
を含むことを特徴とするパラメータ同定方法。
【請求項６】
対象のシステムのパラメータを同定するためのコンピュータプログラムであって、
時間ステップ毎に前記システムへの入力値を取得するステップと、
時間ステップ毎に前記システムからの出力値を取得するステップと、
前記システムの状態量を含む第１の量の一階微分値を、前記システムへの入力値および前記第１の量を用いて表す連続方程式である第１の方程式と、第１の時間ステップでの前記第１の量と、前記第１の時間ステップでの前記システムへの入力値とを用いて、前記第１の時間ステップの次の時間ステップである第２の時間ステップでの前記状態量および前記パラメータを含む拡大状態量を算出するステップと、
前記第１の方程式と、前記システムの出力を、前記拡大状態量と、前記一階微分値とを用いて表す第２の方程式と、前記第１の時間ステップでの前記拡大状態量と、前記第１の時間ステップでの前記入力値とを用いて、前記第１の時間ステップにおける前記システムの出力を算出するステップと、
前記拡大状態量を推定して前記パラメータを同定するステップと、
をコンピュータに実行させることを特徴とするコンピュータプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象のシステムが有するパラメータを同定するパラメータ同定装置、パラメータ同定方法およびコンピュータプログラムに関する。
【背景技術】
【０００２】
パラメータ同定において、同定対象のパラメータを状態量に含めた拡大状態量を導入し、拡大状態量を用いて定義される拡大状態空間モデルにカルマンフィルタ、パーティクルフィルタなどの状態推定技術を適用して、状態量およびパラメータを同時に推定する技術がある。【０００３】
例えば、特許文献１には、拡大状態量を用いて対象のシステムのパラメータを同定する技術が開示されている。特許文献１では、任意のステップの拡大状態量を、任意のステップの１つ前のステップにおける拡大状態量を用いて表した離散拡大状態方程式と、任意のステップにおけるシステムの出力を、任意のステップにおける拡大状態量を用いて表した拡大観測方程式とを入力としている。【０００４】
拡大状態量を導入することで、状態量のデータ計測点数を削減することが可能になり、全ての状態量を計測できない場合であってもパラメータを同定することが可能になる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１７−０８３９２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記従来の技術によれば、状態量の一階微分値を算出することができないため、パラメータを同定する際に、状態量の一階微分値を用いる場合には適用することができないという問題があった。例えば、機械系のデータ計測においては、加速度センサが用いられることが多い。加速度センサの計測データを用いてパラメータを同定する場合、加速度センサの計測データをシステムの出力の要素の一部または全部とすると、システムの出力を示す観測方程式は、ある時刻での状態量と、ある時刻での状態量の一階微分値とを用いて記述される。このため、加速度センサの計測データを用いてパラメータを同定する場合には、状態量の一階微分値が用いられる。【０００７】
本発明は、上記に鑑みてなされたものであって、状態量の一階微分値を用いる場合であっても適用可能なパラメータ同定装置を得ることを目的とする。
【課題を解決するための手段】
【０００８】
上述した課題を解決し、目的を達成するために、本発明にかかる対象のシステムのパラメータを同定するパラメータ同定装置は、システムの状態量を含む第１の量の一階微分値を、システムへの入力値および第１の量を用いて表す連続方程式である第１の方程式を記憶する第１の記憶部と、システムの出力を、状態量およびパラメータを含む拡大状態量と、一階微分値とを用いて表す第２の方程式を記憶する第２の記憶部と、第１の方程式と、第１の時間ステップでの第１の量と、第１の時間ステップでのシステムへの入力値とを用いて、第１の時間ステップの次の時間ステップである第２の時間ステップでの拡大状態量を算出する第１の算出部と、第１の方程式と、第２の方程式と、第１の時間ステップの拡大状態量と、第１の時間ステップの入力値とを用いて、第１の時間ステップにおけるシステムの出力を算出する第２の算出部と、時間ステップ毎に取得されるシステムへの入力値と、時間ステップ毎に取得されるシステムからの出力値と、第１の算出部と、第２の算出部とを用い、拡大状態量を推定する推定部と、を備えることを特徴とする。
【発明の効果】
【０００９】
本発明によれば、状態量の一階微分値を用いる場合であっても適用可能なパラメータ同定装置を得ることができるという効果を奏する。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施の形態１にかかるパラメータ同定装置の機能構成を示す図
【図２】図１に示す第１の算出部の内部処理を説明するための図
【図３】図１の第２の算出部の内部処理を説明するための図
【図４】図１に示すパラメータ同定装置がパラメータを同定する処理について説明するためのフローチャート
【図５】本発明の実施の形態２にかかるパラメータ同定装置の機能構成を示す図
【図６】図５に示す第１の算出部の内部処理を説明するための図
【図７】図５に示す第２の算出部の内部処理を説明するための図
【図８】本発明の実施の形態３にかかるパラメータ同定装置の機能構成を示す図
【図９】本発明の実施の形態１〜３にかかるパラメータ同定装置の機能を実現するための専用のハードウェアを示す図
【図１０】本発明の実施の形態１〜３にかかるパラメータ同定装置の機能を実現するための制御回路の構成を示す図
【図１１】本発明の実施の形態１〜３にかかるパラメータ同定装置の適用例を示す図
【発明を実施するための形態】
【００１１】
以下に、本発明の実施の形態にかかるパラメータ同定装置、パラメータ同定方法およびコンピュータプログラムを図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１２】
実施の形態１．
図１は、本発明の実施の形態１にかかるパラメータ同定装置１０の機能構成を示す図である。パラメータ同定装置１０は、対象のシステムのパラメータθを同定する。パラメータ同定装置１０は、同定対象のパラメータθと状態量ｘとを含む拡大状態量ｚを用いて、パラメータθおよび状態量ｘの同時推定を行い、パラメータθを同定する機能を有する。
【００１３】
パラメータ同定装置１０は、入力値取得部１２と、観測値取得部１４と、第１の記憶部１６と、第２の記憶部１８と、第１の算出部２０と、第２の算出部２２と、推定部２４とを有する。
【００１４】
パラメータ同定装置１０は、オフラインで使用される。外部記憶媒体３０には、予め定められた期間における入力値データ３２および観測値データ３４が予め記憶されている。入力値データ３２は、対象のシステムへの入力値を示す時系列データであり、観測値データ３４は、対象のシステムからの出力の観測値を示す時系列データである。所定期間は、時間ｔが０からＴの間の期間である。
【００１５】
入力値取得部１２は、外部記憶媒体３０に記憶されている入力値データ３２から、時間ステップ毎に、つまり一定の周期で、入力値ｕを取得する。入力値取得部１２は、取得した入力値ｕを推定部２４に出力する。以下、ｋステップ目の入力値ｕをｕkと表す。その他の値についても同様に、特定の値を示す符号に下付の文字でステップ数を付す場合、当該ステップにおける値を示すものとする。ここで、Ｔｓを時間ステップの周期とした場合、ｋは、０からＴ／Ｔｓの値をとる。
【００１６】
観測値取得部１４は、外部記憶媒体３０に記憶されている観測値データ３４から、時間ステップ毎に、つまり一定の周期で観測値ｙkを取得する。観測値取得部１４は、取得した観測値ｙkを推定部２４に出力する。
【００１７】
第１の記憶部１６は、任意の時刻における状態を示す連続状態方程式である第１の方程式を記憶する。第１の方程式は、システムの状態量を含む第１の量の一階微分値を、システムへの入力値ｕおよび第１の量を用いて表す連続状態方程式である。本実施の形態では、第１の量は、状態量ｘと、パラメータθとを含む拡大状態量ｚである。
【００１８】
まず、対象のシステムの連続状態方程式は、以下の数式（１）を用いて表される。
【００１９】
【数１】
2021014499.tif 000003
【００２０】
ここで、ｆ0は既知の非線形関数、ｘはシステムの状態量、ｘdotは状態量の一階微分値、θは対象システムのパラメータである。状態量は、ベクトル量である。システムの状態量ｘの要素は、システムの並進運動または回転運動に関する位置および速度に関する変数で構成されている。
【００２１】
対象システムのパラメータθの時間変化量θdotは、以下に示す数式（２）で表される。
【００２２】
【数２】
2021014499.tif 000004
【００２３】
本実施の形態においては、パラメータθは経時的に変化する値をとる。数式（１）および数式（２）から、以下に示す拡大連続状態方程式である数式（３）が導かれる。本実施の形態において、第１の方程式は、数式（３）である。
【００２４】
【数３】
2021014499.tif 000005
【００２５】
数式（３）において、ｚは拡大状態量であり、ベクトル量である。ｚ＝（ｘ，θ）Tで定義され、ｚdotは、拡大状態量ｚを時間微分した一階微分値である。ｆは、数式（１），（２）から導出される既知の非線形関数である。数式（３）から明らかなように、ある時刻での拡大状態量ｚの一階微分値ｚdotは、当該時刻における拡大状態量ｚおよび入力値ｕに基づいて算出することができる。
【００２６】
第２の記憶部１８は、システムの出力である観測値ｙを、拡大状態量ｚと、拡大状態量ｚの一階微分値ｚdotとを用いて表す第２の方程式である拡大観測方程式を記憶している。第２の方程式である拡大観測方程式は、以下に示す数式（４）で表される。
【００２７】
【数４】
2021014499.tif 000006
【００２８】
ｙは、ある時刻でのシステムの観測値である。ｇは、既知の非線形関数である。数式（４）から明らかなように、ある時刻での観測値ｙは、ある時刻での拡大状態量ｚに加え、拡大状態量ｚの一階微分値ｚdotに基づいて算出される。例えば、観測値が加速度センサデータである場合、加速度センサデータはシステムの並進および回転運動に関する位置、速度、および加速度を用いて記述されることを示している。既知の非線形関数ｇは、例えば、対象システムのキネマティクスによって定式化される。
【００２９】
第１の算出部２０は、予め定められた数値積分手法に基づいて、第１の記憶部１６に記憶される第１の方程式の数値離散化を行い、第３の方程式を導出する。第３の方程式は、ｋ＋１ステップ目の拡大状態量ｚk+1を、ｋステップ目における拡大状態量ｚkと、入力値取得部１２が出力する入力値ｕkとを用いて表している。なお、ｋステップを第１の時間ステップと称した場合、ｋ＋１ステップを第１の時間ステップの次の時間ステップである第２の時間ステップと称することができる。第１の算出部２０が導出する第３の方程式は、以下の数式（５）で表される。
【００３０】
【数５】
2021014499.tif 000007
【００３１】
数式（５）において、ｆdは非定形関数である。例えば、数値積分法である４次ルンゲクッタ法を用いる場合、ｋ＋１ステップ目の拡大状態量ｚk+1は、以下に示す数式（６）を用いて計算される。
【００３２】
【数６】
2021014499.tif 000008
【００３３】
ここで、数式（６）中のｋ１〜ｋ４は、４次ルンゲクッタ法における勾配に関する変数であり、入力値ｕに関して０次ホールドを適用すると、以下の数式（７）〜（１０）で表される。
【００３４】
【数７】
2021014499.tif 000009
【００３５】
【数８】
2021014499.tif 000010
【００３６】
【数９】
2021014499.tif 000011
【００３７】
【数１０】
2021014499.tif 000012
【００３８】
上記のｋ１〜ｋ４は、第１の記憶部１６に記憶されている第１の方程式である数式（３）を用いて算出することができる。第１の算出部２０は、第１の方程式と、第１の時間ステップでの第１の量である拡大状態量ｚkと、第１の時間ステップでの入力値ｕkとを用いて、第２の時間ステップでの拡大状態量ｚk+1を算出する。
【００３９】
図２は、図１に示す第１の算出部２０の内部処理を説明するための図である。第１の算出部２０は、拡大状態量ｚkと、入力値ｕkおよびｕk+1と、数式（３）とを用いて、数式（７）〜（１０）に示すｋ1〜ｋ4を算出する。第１の算出部２０は、算出したｋ1〜ｋ4と数式（６）とを用いて、第２の時間ステップでの拡大状態量ｚk+1を算出する。
【００４０】
第２の算出部２２は、第１の記憶部１６および第２の記憶部１８を利用して、ｋステップ目における拡大状態量ｚkと、入力値取得部１２が出力する入力値ｕkとを用いて、ｋステップ目のシステムの出力である観測値ｙkを算出する。第２の算出部２２は、第１の方程式および第２の方程式を用いて得られる第４の方程式に、拡大状態量ｚkと、入力値ｕkとを入力して、観測値ｙkを算出する。第４の方程式は、以下に示す数式（１１）で表される。
【００４１】
【数１１】
2021014499.tif 000013
【００４２】
図３は、図１の第２の算出部２２の内部処理を説明するための図である。第２の算出部２２は、ｋステップ目における拡大状態量ｚkと、入力値取得部１２が出力する入力値ｕkと、第１の記憶部１６に記憶されている第１の方程式である数式（３）とに基づいて、ｋステップ目における拡大状態量ｚkの一階微分値ｚdotkを算出する。第２の算出部２２は、算出された一階微分値ｚdotkと、拡大状態量ｚkと、第２の方程式である数式（４）とを用いて、ｋステップ目の観測値ｙkを算出する。
【００４３】
上記の操作は、加速度センサデータによる状態推定技術を適用した状態量およびパラメータの同時推定において、連続状態方程式中のパラメータ自体が推定対象であって未知の値であるため、連続状態方程式を利用して状態量の時間微分値を算出することができない場合に、パラメータθに逐次推定されている値を採用し、状態量の時間微分値を算出可能にしている。
【００４４】
推定部２４は、任意の状態推定手法を用いて、入力値取得部１２が出力する入力値ｕkと、観測値取得部１４が出力する観測値ｙkと、第１の算出部２０から得られる第３の方程式である数式（５）と、第２の算出部２２から得られる第４の方程式である数式（１１）とに基づいて、拡大状態量ｚを推定する。
【００４５】
推定部２４が用いる状態推定手法は、限定されず、パーティクルフィルタ、拡張カルマンフィルタ、Uncentedカルマンフィルタなど他の状態推定手法であってもよい。
【００４６】
図４は、図１に示すパラメータ同定装置１０がパラメータθを同定する処理について説明するためのフローチャートである。なお、ここでは拡張カルマンフィルタを使用した例について説明する。なお、以下においてハット付の符号をその符号の後に＾を付して示す。同様に、バー付の符号をその符号の後に￣を付して示す。なお、ハット付の符号は、符号が示す値の推定値を示し、バー付の符号は、符号が示す値の予測値を示す。
【００４７】
推定部２４は、ｋ＝０での拡大状態量ｚkの推定値ｚk＾と、拡大状態量の共分散行列の推定値Ｐk＾と、システム雑音行列値Ｑと、観測雑音行列値Ｒとを設定する初期設定を行う（ステップＳ１０１）。
【００４８】
なお、パーティクルフィルタ、Uncentedカルマンフィルタのような粒子的フィルタを用いる場合、それぞれに対応する値の一般的な初期設定を行えばよい。
【００４９】
推定部２４は、入力値取得部１２が外部記憶媒体３０に記憶されている入力値データ３２から、時間ステップ毎に取得する入力値ｕkを取得する（ステップＳ１０２）。推定部２４は、観測値取得部１４が外部記憶媒体３０に記憶されている観測値データ３４から、時間ステップ毎に取得する観測値ｙkを取得する（ステップＳ１０３）。なお、ステップＳ１０１からステップＳ１０３に示す処理は順不同で実行することができる。
【００５０】
続いて推定部２４は、現在の時間ステップｋが、予め定められた数Ｎよりも小さいか否かを判断する（ステップＳ１０４）。
【００５１】
ｋがＮよりも小さい場合（ステップＳ１０４：Ｙｅｓ）、推定部２４は、予測処理を行う（ステップＳ１０５）。具体的には、推定部２４は、以下の数式（１２）に示すように、第１の算出部２０から得られる第３の方程式である数式（５）に、当該ステップでの拡大状態量ｚkの推定値ｚk＾と、入力値ｕkとを代入して、次のステップｋ＋１での拡大状態量ｚk+1を予測する。この拡大状態量予測値をｚk+1￣と称する。
【００５２】
【数１２】
2021014499.tif 000014
【００５３】
推定部２４は、続いて以下に示す数式（１３）で定義されるｆdのヤコビ行列Ａkを算出する。
【００５４】
【数１３】
2021014499.tif 000015
【００５５】
ヤコビ行列Ａkの算出においては、例えば、推定部２４は、第３の方程式である数式（５）の数値微分を用いることができる。数式（１３）によって得られるヤコビ行列Ａkと、当該ステップでの共分散行列の推定値Ｐk＾と、事前に設定したシステム雑音行列値Ｑとに基づいて、以下の数式（１４）に示すように、次のステップｋ＋１の共分散行列Ｐk+1を予測する。予測した共分散行列をＰk+1￣とする。
【００５６】
【数１４】
2021014499.tif 000016
【００５７】
なお、本ステップでの数式（５）を利用した計算過程について、前述したとおり、第１の算出部２０の内部で、第１の記憶部１６を利用して、予め定められた数値積分手法により、ｋステップ目における拡大状態量ｚkと、入力値ｕkとに基づいて、ｋ＋１ステップ目の拡大状態量ｚk+1を算出している。
【００５８】
推定部２４は、予測処理を行った後、更新処理を行う（ステップＳ１０６）。まず推定部２４は、以下の数式（１５）で定義されるｇdのヤコビ行列Ｃk+1を算出する。
【００５９】
【数１５】
2021014499.tif 000017
【００６０】
ヤコビ行列Ｃk+1の算出は、例えば、第４の方程式である数式（１１）に示す変形拡大観測方程式の数値微分によって求めることができる。続いて、ステップＳ１０５で得られる共分散行列予測値Ｐk+1￣と、数式（１５）で得られるヤコビ行列Ｃk+1と、事前に設定した観測雑音行列値Ｒとに基づいて、以下に示す数式（１６）を用いてカルマンゲインＧk+1を算出する。
【００６１】
【数１６】
2021014499.tif 000018
【００６２】
まず、以下の数式（１７）に示すように、推定部２４は、予測した拡大状態量ｚk+1￣と、カルマンゲインＧk+1と、観測値ｙk+1と入力値ｕk+1と、第４の方程式である数式（１１）とに基づいて、ステップｋ＋１の拡大状態量の推定値ｚk+1＾を算出する。
【００６３】
【数１７】
2021014499.tif 000019
【００６４】
また、以下の数式（１８）に示すように、推定部２４は、カルマンゲインＧk+1と、ヤコビ行列Ｃk+1と、共分散行列予測値Ｐk+1￣とに基づいて、ステップｋ＋１の共分散行列の推定値Ｐk+1＾を算出する。
【００６５】
【数１８】
2021014499.tif 000020
【００６６】
なお、本ステップでの第４の方程式である数式（１１）を利用した計算過程について、前述したとおり、第２の算出部２２内部で、第１の記憶部１６および第２の記憶部１８を利用し、ｋステップ目における拡大状態量ｚkと、入力値取得部１２から取得した入力値ｕkとに基づいて、ｋステップ目の観測値ｙkを算出している。
【００６７】
推定部２４は、ステップＳ１０６の処理を終わると、ｋの値をインクリメントしてｋ＝ｋ＋１とし（ステップＳ１０７）、ステップＳ１０４からステップＳ１０７を繰返すことで、同時推定が実行される。ｋがＮ以上になると（ステップＳ１０４：Ｎｏ）、パラメータ同定装置１０は、処理を終了する。
【００６８】
以上説明したように、本発明の実施の形態１によれば、パラメータ同定装置１０は、システムの状態量を含む第１の量である拡大状態量ｚの一階微分値ｚdotを、システムの入力値ｕおよび第１の量を用いて表す連続方程式である第１の方程式を記憶しており、第１の方程式を用いてパラメータ同定を行う。これにより、状態量の一階微分値の推定が必要になる場合、例えば、加速度センサ計測データを用いる場合であっても、パラメータの同定が可能になる。
【００６９】
また、加速度センサデータを数値積分などの方法によって位置または速度に関するデータに変換し、当該データを観測値の要素の一部または全部とすれば、観測方程式は、ある時刻での拡大状態量によってのみ記述される一般的な形式となる。しかしながら、この場合、加速度センサデータの数値積分を行う際に生じる積分誤差に対応する必要が生じ、例えば、誤差を除去するためのフィルタ設計作業の工数が増大する。これに対して、本実施の形態によれば、誤差に対応するためのフィルタ設計作業を省略することが可能である。
【００７０】
実施の形態２．
図５は、本発明の実施の形態２にかかるパラメータ同定装置１０−１の機能構成を示す図である。パラメータ同定装置１０−１は、対象システムのパラメータθが経時的に変化しない時不変である場合に好適である。パラメータ同定装置１０−１は、実施の形態１にかかるパラメータ同定装置１０の第１の記憶部１６の代わりに第１の記憶部１６−１を有し、第１の算出部２０の代わりに第１の算出部２０−１を有し、第２の算出部２２の代わりに第２の算出部２２−１を有する。
【００７１】
対象システムのパラメータθの時間変化量θdotは、対象システムの動的挙動に比較して遅いため、時不変とみなすことが可能な場合もある。つまり、θdot＝０とみなすことが可能である。この場合、ある時間ステップｋでのパラメータθkおよびステップｋ＋１でのパラメータθk+1について、以下の数式（１９）が成立する。
【００７２】
【数１９】
2021014499.tif 000021
【００７３】
本実施の形態において、第１の方程式は、上記の数式（１）で表される連続状態方程式であり、第１の量は状態量ｘである。第１の記憶部１６−１は、上記の数式（１）で表される第１の方程式を記憶している。
【００７４】
第１の算出部２０−１は、予め定められた数値積分手法に基づいて、第１の記憶部１６−１に記憶される第１の方程式の数値離散化を行い、第３の方程式を導出する。第３の方程式は、ｋ＋１ステップ目の拡大状態量ｚk+1を、ｋステップ目における拡大状態量ｚkと、入力値取得部１２が出力する入力値ｕkとを用いて表している。第１の算出部２０−１が導出する第３の方程式は、上記の数式（５）で表される。
【００７５】
例えば、第１の算出部２０−１が数値積分法として４次ルンゲクッタ法を採用する場合、ｋ＋１ステップ目の状態量ｘk+1は、以下の数式（２０）で表される。
【００７６】
【数２０】
2021014499.tif 000022
【００７７】
ここで、数式（２０）中のｋ1’〜ｋ4’は、４次ルンゲクッタ法における勾配に関する変数であり、入力値ｕに関して０次ホールドを適用すると、以下の数式（２１）〜（２４）で表される。
【００７８】
【数２１】
2021014499.tif 000023
【００７９】
【数２２】
2021014499.tif 000024
【００８０】
【数２３】
2021014499.tif 000025
【００８１】
【数２４】
2021014499.tif 000026
【００８２】
上記のｋ1’〜ｋ4’は、第１の記憶部１６−１に記憶されている第１の方程式である数式（１）を用いて算出可能である。ｋ＋１ステップ目の拡大状態量ｚk+1は、その定義に従って、数式（１９）および数式（２０）の計算結果からｚk+1＝（ｘk+1，θk+1）Tとして算出可能である。
【００８３】
図６は、図５に示す第１の算出部２０−１の内部処理を説明するための図である。第１の算出部２０−１は、第１の方程式である数式（１）と、ｋステップ目の状態量ｘkと、入力値ｕkと、パラメータθkと、ｋ＋１ステップ目の入力値ｕk+1とに基づいて、数式（２１）〜数式（２４）に示すｋ1’〜ｋ4’を算出する。第１の算出部２０−１は、ｋ1’〜ｋ4’と、数式（２０）とを用いて、状態量ｘk+1を算出する。
【００８４】
第２の算出部２２−１は、第１の記憶部１６−１および第２の記憶部１８を利用して、ｋステップ目における拡大状態量ｚkと、入力値取得部１２から取得した入力値ｕkとに基づいて、ｋステップ目の観測値ｙkを算出する。
【００８５】
第２の算出部２２−１は、第１の方程式および第２の方程式を用いて得られる第４の方程式に、拡大状態量ｚkと、入力値ｕkとを入力して、観測値ｙkを算出する。第４の方程式は、上記に示す数式（１１）で表される。
【００８６】
図７は、図５に示す第２の算出部２２−１の内部処理を説明するための図である。第２の算出部２２−１は、ｋステップ目における拡大状態量ｚkに含まれる状態量ｘkとパラメータθkと、入力値取得部１２から取得した入力値ｕkと、第１の方程式である数式（１）とを用いて、ｋステップ目における状態量の時間微分値ｘdotkを算出する。
【００８７】
拡大状態量の一階微分値ｚdotkは、その定義とパラメータθが時不変であるという仮定に従って、ｚdotk＝（ｘdotk，０）Tとなる。算出されるｋステップ目における拡大状態量ｚkおよび拡大状態量の一階微分値ｚdotkに基づいて、第２の記憶部１８に記憶されている第２の方程式である数式（４）を利用して、ｋステップ目の観測値ｙkを算出する。
【００８８】
パラメータ同定装置１０，１０−１は、数値離散化処理や同定処理の過程で、第１の方程式および第２の方程式を複数回実行することになる。実施の形態１の第１の方程式である数式（３）と、実施の形態２の第１の方程式である数式（１）とを比較すると、数式（１）の方が数式（３）よりもパラメータθを状態量に含まない分だけ、記憶領域および演算量を低減することができるという効果がある。
【００８９】
パラメータ同定装置１０−１がパラメータθを同定する処理は、パラメータ同定装置１０のパラメータθを同定する処理と同様である。
【００９０】
実施の形態３．
図８は、本発明の実施の形態３にかかるパラメータ同定装置１０−２の機能構成を示す図である。パラメータ同定装置１０−２は、実施の形態１にかかるパラメータ同定装置１０に、第３の記憶部２６と、外乱推定部２８とを加え、推定部２４の代わりに推定部２４−２を有する。また、パラメータ同定装置１０−２は、実施の形態２にかかるパラメータ同定装置１０−１に、第３の記憶部２６と、外乱推定部２８とを加え、推定部２４の代わりに推定部２４−２を有する構成であってもよい。
【００９１】
第３の記憶部２６は、拡大状態量ｚおよび拡大状態量の一階微分値ｚdotに基づいて推定外乱量ｕdを生成するための未知外乱推定モデルを記憶している。未知外乱推定モデルは、以下の数式（２５）で表される。
【００９２】
【数２５】
2021014499.tif 000027
【００９３】
数式（２５）において、ｕdは推定外乱量を示し、ｄoは外乱に関する関数を示している。ある時間ｔでの推定外乱量ｕdは、拡大状態量ｚと、拡大状態量ｚの一階微分値ｚdotとに基づいて算出される。例えば、対象システムの駆動部の摩擦力およびトルクを未知外乱とする場合、未知外乱は、位置、速度、加速度などで記述される。
【００９４】
外乱推定部２８は、第１の記憶部１６と、第３の記憶部２６とを用いて、第１の時間ステップであるｋステップ目における拡大状態量ｚkと、入力値ｕkとに基づいて、ｋステップ目の推定外乱量ｕd,kを算出し、算出した推定外乱量ｕd,kを出力する。外乱推定部２８の機能は、以下の数式（２６）に示す変形外乱モデルで表される。
【００９５】
【数２６】
2021014499.tif 000028
【００９６】
数式（２６）のｄは、変形後の外乱に関する関数である。
【００９７】
外乱推定部２８は、まず、ｋステップ目における拡大状態量ｚkと、入力値取得部１２から取得した入力値ｕkとに基づいて、第１の記憶部１６に記憶されている第１の方程式である数式（３）で示される拡大連続状態方程式を用いて、ｋステップ目における拡大状態量ｚkの一階微分値ｚdotkを算出する。外乱推定部２８は、算出された一階微分値ｚdotkと、ｋステップ目における拡大状態量に基づいて、第３の記憶部２６に記憶された未知外乱推定モデルを用いて、ｋステップ目の推定外乱量ｕd,kを算出する。
【００９８】
加速度依存性を有する未知外乱の影響を補償しつつ、状態推定技術を適用した状態量およびパラメータの同時推定を行う場合、駆動体位置情報およびＰＩ（Proportion  Integral）補償器とを用いた外乱推定器が考えられる。この場合、駆動体位置情報の二階微分、または二階微分に相当する操作を行って高周波ノイズ成分への対応が課題となる。これに対して本実施の形態では、駆動体加速度を直接推定することができるため、課題を解決することが可能である。
【００９９】
パラメータ同定装置１０−２がパラメータθを同定する処理については、図４と同様であり、ステップＳ１０５の詳細な動作が異なる。ステップＳ１０５の予測処理では、以下の数式（２７）に示すように、外乱推定部２８から得られる変形外乱モデルを示す数式（２６）に当該ステップでの拡大状態量の推定値ｚk＾と、入力値ｕkとを代入して、当該ステップでの推定外乱量ｕd,k＾を算出する。
【０１００】
【数２７】
2021014499.tif 000029
【０１０１】
以降の推定処理では、ｕk＝ｕk＋ｕd,k＾と置き換える。以上説明したように本発明の実施の形態３によれば、高精度に未知外乱を推定することができ、未知外乱の影響を補償しつつ、状態推定技術を適用した状態量およびパラメータの同時推定を行うことが可能になる。
【０１０２】
続いて、本発明の実施の形態１〜３にかかるパラメータ同定装置１０，１０−１，１０−２のハードウェア構成について説明する。パラメータ同定装置１０，１０−１，１０−２の各機能は、処理回路により実現される。これらの処理回路は、専用のハードウェアにより実現されてもよいし、ＣＰＵ（Central  Processing  Unit）を用いた制御回路であってもよい。
【０１０３】
上記の処理回路が、専用のハードウェアにより実現される場合、これらは、図９に示す処理回路９０により実現される。図９は、本発明の実施の形態１〜３にかかるパラメータ同定装置１０，１０−１，１０−２の機能を実現するための専用のハードウェアを示す図である。処理回路９０は、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application  Specific  Integrated  Circuit）、ＦＰＧＡ（Field  Programmable  Gate  Array）、またはこれらを組み合わせたものである。
【０１０４】
上記の処理回路が、ＣＰＵを用いた制御回路で実現される場合、この制御回路は例えば図１０に示す構成の制御回路９１である。図１０は、本発明の実施の形態１〜３にかかるパラメータ同定装置１０，１０−１，１０−２の機能を実現するための制御回路９１の構成を示す図である。図１０に示すように、制御回路９１は、プロセッサ９２と、メモリ９３とを備える。プロセッサ９２は、ＣＰＵであり、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Digital  Signal  Processor）などとも呼ばれる。メモリ９３は、例えば、ＲＡＭ（Random  Access  Memory）、ＲＯＭ（Read  Only  Memory）、フラッシュメモリ、ＥＰＲＯＭ（Erasable  Programmable  ＲＯＭ）、ＥＥＰＲＯＭ（登録商標）（Electrically  ＥＰＲＯＭ）などの不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital  Versatile  Disk）などである。
【０１０５】
上記の処理回路が制御回路９１により実現される場合、プロセッサ９２がメモリ９３に記憶された、各構成要素の処理に対応するコンピュータプログラムを読み出して実行することにより実現される。また、メモリ９３は、プロセッサ９２が実行する各処理における一時メモリとしても使用される。このコンピュータプログラムは、通信路を介して提供されてもよいし、記録媒体に記録された状態で提供されてもよい。
【０１０６】
実施の形態４．
図１１は、本発明の実施の形態１〜３にかかるパラメータ同定装置１０，１０−１，１０−２の適用例を示す図である。
【０１０７】
図１１に示す平面２リンクロボット４０は、対象のシステムの一例である。パラメータ同定装置１０，１０−１，１０−２は、図１１に示す平面２リンクロボット４０のパラメータを同定することができる。
【０１０８】
平面２リンクロボット４０は、第１のリンク４１と、第２のリンク４２とを有する。第１のリンク４１および第２のリンク４２は、剛体リンクである。第１のリンク４１は、地面に対して回転自在のジョイントによって結合され、回転モータ４３によって駆動される。第２のリンク４２は、結合部４４を介して第１のリンク４１に結合されている。結合部４４は、回転力を与える回転バネと、回転を減衰させる方向の力を加える回転減衰器とを含む。
【０１０９】
回転モータ４３には、角度センサであるエンコーダが取り付けられており、第２のリンク４２の先端には２軸加速度センサ４５が取り付けられている。
【０１１０】
パラメータ同定装置１０，１０−１，１０−２が平面２リンクロボット４０のパラメータを同定する場合、対象への入力値ｕは、回転モータ４３の印加トルクのデータであり、対象の観測値ｙは、回転モータ４３に取り付けられたエンコーダのデータ、つまり第１のリンク４１の回転角φ１および２軸加速度センサ４５の出力するデータａｘ，ａｙである。この場合、推定するパラメータは、結合部４４の回転バネの剛性値Ｋおよび回転減衰器の減衰値Ｃであり、θは、以下の数式（２８）に示されるように、合成値Ｋおよび減衰値Ｃからなるベクトルとなる。
【０１１１】
【数２８】
2021014499.tif 000030
【０１１２】
対象のシステムの連続状態方程式は、その運動方程式に基づいて、数式（１）の形式で記述可能である。なお、本例では、状態量ｘは、以下の数式（２９）に示すように、第１のリンク４１および第２のリンク４２の回転角φ１，φ２からなるベクトルである。
【０１１３】
【数２９】
2021014499.tif 000031
【０１１４】
対象のシステムの拡大状態量は、ｘ＝（ｘ，θ）Tと定義され、拡大連続状態方程式は、数式（３）の形式で記述可能である。
【０１１５】
観測値ｙは、以下の数式（３０）に示すように、回転モータ４３に取り付けられたエンコーダのデータ、つまり第１のリンクの回転角φ１および２軸加速度センサ４５の出力するデータａｘ，ａｙからなるベクトルとなる。
【０１１６】
【数３０】
2021014499.tif 000032
【０１１７】
そして、対象の拡大観測方程式は、キネマティクスに基づいて、数式（４）の形式で記述可能である。
【０１１８】
本発明の実施の形態１にかかるパラメータ同定装置１０の第１の方程式は、数式（３）で示される拡大連続状態方程式であり、第２の方程式は、数式（４）で示される観測方程式である。パラメータ同定装置１０の推定部２４は、拡大状態量ｚを推定する。これにより、推定部２４は、対象の状態量ｘ、つまり第１のリンク４１の回転角φ１および第２のリンク４２の回転角φ２と、パラメータθ、つまり回転バネの剛性値ｋおよび回転減衰器の減衰値Ｃを推定することができる。
【０１１９】
本発明の実施の形態２にかかるパラメータ同定装置１０−１の第１の方程式は、数式（１）で示される連続状態方程式であり、第２の方程式は、数式（４）で示される観測方程式である。パラメータ同定装置１０−１の推定部２４は、拡大状態量ｚを推定する。
【０１２０】
本発明の実施の形態３にかかるパラメータ同定装置１０−２の第１の方程式は、数式（３）で示される拡大連続状態方程式であり、第２の方程式は、数式（４）で示される観測方程式である。本実施の形態では、回転モータ４３の摩擦トルクが加速度依存性を有する未知外乱であり、数式（２５）に記述されるような推定モデルを構築しておく。これらにより、パラメータ同定装置１０−２の推定部２４−２は、拡大状態量ｚを推定する。
【０１２１】
図１１では、上記の処理が処理回路９０の内部にて行われる例を示しているが、制御回路９１であってもよい。
【０１２２】
なお、対象のシステムは、図１１に示す平面２リンクロボット４０に限定されず、３次元多剛体系を含む広く一般の機械系に適用可能である。推定するパラメータは、状態方程式中に現れる、質量、重心位置、慣性モーメント、線形剛性、減衰などに関連するパラメータであってよい。また、入力値ｕは、回転モータ４３の印加トルクのデータに限定されず、例えば、対象のシステムが直動駆動の系であれば、駆動推力などであってもよい。観測値ｙを取得するセンサは、レゾルバなどであってもよい。使用するセンサによって、観測値ｙは、角速度であってもよいし、角加速度であってもよい。また、対象のシステムが直動駆動の系である場合、観測値ｙを取得するセンサは、リニアエンコーダであってもよく、２軸加速度センサ４５の代わりに、３軸加速度センサが用いられてもよい。
【０１２３】
上記の例では、回転モータ４３に取り付けられるエンコーダおよび第２のリンク４２に取り付けられる２軸加速度センサ４５は、それぞれ１台としたが、エンコーダおよび２軸加速度センサ４５は、複数台設けられてもよい。推定に用いる入力値データおよび出力値データについて、動作パターンを限定するものではなく、一般的な位置決め動作、Ｍ系列・ランダム信号動作、周期的動作などであってもよい。
【０１２４】
以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。
【符号の説明】
【０１２５】
１０，１０−１，１０−２  パラメータ同定装置、１２  入力値取得部、１４  観測値取得部、１６，１６−１  第１の記憶部、１８  第２の記憶部、２０，２０−１  第１の算出部、２２，２２−１  第２の算出部、２４，２４−２  推定部、２６  第３の記憶部、２８  外乱推定部、３０  外部記憶媒体、３２  入力値データ、３４  観測値データ、４０  平面２リンクロボット、４１  第１のリンク、４２  第２のリンク、４３  回転モータ、４４  結合部、４５  ２軸加速度センサ、９０  処理回路、９１  制御回路、９２  プロセッサ、９３  メモリ。
【図１】
2021014499.tif 000033
【図２】
2021014499.tif 000034
【図３】
2021014499.tif 000035
【図４】
2021014499.tif 000036
【図５】
2021014499.tif 000037
【図６】
2021014499.tif 000038
【図７】
2021014499.tif 000039
【図８】
2021014499.tif 000040
【図９】
2021014499.tif 000041
【図１０】
2021014499.tif 000042
【図１１】
2021014499.tif 000043
【国際調査報告】
2021014499.tif 000044
2021014499.tif 000045
2021014499.tif 000046