(57)【要約】【課題】  移動するロボット及びロボットの位置を推定する方法を提供する。【解決手段】  ジャイロスコープの回転角度に対する情報を提供するジャイロスコープモジュールと、ホイールの動作を感知して移動速度及び前記ホイールの回転角度に対する情報を提供するエンコーダモジュール、及び前記エンコーダモジュールと前記ジャイロスコープモジュールから提供される情報に基づいて所定の方法によって現在の位置を推定する制御モジュールを含むが、前記制御モジュールは前記ジャイロスコープモジュールによって提供される回転角度に対する情報を前記位置推定に反映するロボット。【選択図】    図１
2007132938.tif 000002
【特許請求の範囲】
【請求項１】
ジャイロスコープの回転角度に対する情報を提供するジャイロスコープモジュールと、
ホイールの動作を感知して移動速度及び前記ホイールの回転角度に対する情報を提供するエンコーダモジュールと、
前記エンコーダモジュールと前記ジャイロスコープモジュールから提供される情報に基づいて所定の方法によって現在の位置を推定する制御モジュールとを含むが、
前記制御モジュールは前記ジャイロスコープモジュールによって提供される回転角度に対する情報を前記位置推定に反映することを特徴とするロボット。
【請求項２】
前記方法は、制限条件があるカルマンフィルタ方法を含むことを特徴とする請求項１に記載のロボット。
【請求項３】
前記カルマンフィルタ方法は、クーン・タッカー最適化条件とペナルティ関数定理を満たすことを特徴とする請求項２に記載のロボット。
【請求項４】
前記制限条件は、ペナルティ関数で表現されることを特徴とする請求項２に記載のロボット。
【請求項５】
前記制御モジュールは、前記エンコーダモジュールによって推定されたロボットの位置と、前記ジャイロスコープモジュールによって推定されたロボットの位置誤差を状態変数として、位置を推定することを特徴とする請求項１に記載のロボット。
【請求項６】
エンコーダモジュールとジャイロスコープモジュールを備えたロボットの位置推定方法において、
前記ジャイロスコープモジュールからジャイロスコープの回転角度に対する情報が提供される（ａ）ステップと、
前記エンコーダモジュールからホイールの動作による移動速度及び前記ホイールの回転角度に対する情報が提供される（ｂ）ステップと、
前記エンコーダモジュールと前記ジャイロスコープモジュールから提供される情報に基づいて前記ロボットの位置を推定する（ｃ）ステップとを含むが、
前記（ｃ）ステップは、前記ジャイロスコープモジュールによって提供される回転角度に対する情報を反映して前記ロボットの位置を推定するステップを含むことを特徴とするロボットの位置推定方法。
【請求項７】
前記（ｃ）ステップは、
プロセスモデルによって前記ロボットの位置を推定するための状態変数を予測するステップと、
測定方程式によって観測モデルを観測するステップと、
前記予測された状態変数を更新するステップとを含むことを特徴とする請求項６に記載のロボットの位置推定方法。
【請求項８】
前記プロセスモデルは
【数１】
2007132938.tif 000027
のように表すステップを含むが、式中、
【数２】
2007132938.tif 000028
は前記ジャイロスコープモジュールから提供された回転角度を使用して推定されたロボットの位置を示し、
【数３】
2007132938.tif 000029
は前記ジャイロスコープモジュールによって推定されたロボットの位置誤差を意味し、φは前記ジャイロスコープモジュールから提供された回転角度を示し、ｗはプロセスノイズを示すことを特徴とする請求項７に記載のロボットの位置推定方法。
【請求項９】
前記観測モデルは、
「測定方程式＝エンコーダで推定したロボットの位置−ジャイロスコープで推定したロボットの位置＋ノイズ」と表現される測定方程式によって行われるステップを含むことを特徴とする請求項７に記載のロボットの位置推定方法。
【請求項１０】
前記測定方程式は、
【数４】
2007132938.tif 000030
の形態で提供されることを特徴とする請求項９に記載のロボットの位置推定方法。
【請求項１１】
前記予測される状態変数は、
【数５】
2007132938.tif 000031
によって更新されるが、式中、Ｋｋはカルマンゲイン、Ｈｋは観測行列、Ｐｋは共分散行列、Ｒｋはノイズに対する分散、
【数６】
2007132938.tif 000032
は更新前の状態変数、
【数７】
2007132938.tif 000033
は更新後の状態変数、ｚｋは測定方程式
【数８】
2007132938.tif 000034
を、Ｔは転置行列を、Ｉは単位行列を示すことを特徴とする請求項７に記載のロボットの位置推定方法。
【請求項１２】
前記（ｃ）ステップは、制限条件があるカルマンフィルタ方法によって前記ロボットの位置を推定するステップを含むことを特徴とする請求項６に記載のロボットの位置推定方法。
【請求項１３】
前記カルマンフィルタ方法は、クーン・タッカー最適化条件とペナルティ関数定理を満たすことを特徴とする請求項１２に記載のロボットの位置推定方法。
【請求項１４】
前記制限条件は、ペナルティ関数で表現されることを特徴とする請求項１２に記載のロボットの位置推定方法。
【請求項１５】
前記制限条件があるカルマンフィルタ方法による前記ロボットの状態方程式は、
【数９】
2007132938.tif 000035
のように表され、前記状態変数ｘｋから前記ロボットの位置を推定するが、Ｗは任意の正の定符号行列、
【数１０】
2007132938.tif 000036
はカルマンフィルタされた状態変数、ｇｉ（ｘ）は不等制限条件、ｈｊ（ｘ）は等制限条件、ｓはペナルティパラメータ、λとμはラグランジュマルチプライヤであり、Ｔは転置行列であり、
【数１１】
2007132938.tif 000037
を満たすことを特徴とする請求項１２に記載のロボットの位置推定方法。
【請求項１６】
初期にはε＝０、α＝正の定数に、所定の時間後にεを小さな正の定数に変更してｓ、λ、μを更新することを特徴とする請求項１５に記載のロボットの位置推定方法。
【請求項１７】
前記αは、所定の時間後に他の正の関数に変更されることを特徴とする請求項１６に記載のロボットの位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの位置推定方法に関し、より詳しくは、既知の制限条件があるカルマンフィルタ及びジャイロスコープを使用して移動するロボットの位置を推定する方法及びこのような方法によって自己位置を推定するロボットに関する。
【背景技術】
【０００２】
最近、技術の発達に伴い多様な形態のロボットが登場するようになり、特に家庭内で自ら移動しながら人間の仕事を代わりに行うロボットも登場するようになった。
【０００３】
ところが、このように自ら移動するロボットにおいて、自己位置を把握することは非常に重要な問題であり、これを解決するために多くの方法が提案された。
【０００４】
従来にはロボットの位置を推定するためにロボットの駆動部（例：ホイール）にエンコーダ（または走行距離計ともいう）を付着したり、さらにはロボットの回転角度を正確に測定するためにロボットにジャイロスコープを搭載した。
【０００５】
しかし、ロボットの位置を推定するときにはホイールの滑りあるいは機械的ドリフトによって誤差が発生するようになるが、このような誤差が微小でも累積し続ければ、結局大きい誤差が生じて問題が発生し得る。
【０００６】
一方、従来にはロボットの位置を推定するためにカルマンフィルタアルゴリズムを適用したが、この場合、制限条件を加えないため推定されたパラメータが物理的な制限条件を違反する場合が発生することもあった。例えば、移動するロボットのホイール間の距離をＷｂとする場合、Ｗｂが物理的に０．３（ｍ）≦Ｗｂ≦０．４（ｍ）範囲に属しなければならないが、推定されたＷｂがこの範囲を超える場合が発生し得る。またカルマンフィルタを基盤として天井ビジョンを使用するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｉｎｇ）の場合、天井高さをｚとすれば、ｚは２．４（ｍ）≦Ｗｂ≦２．５（ｍ）範囲に属しなければならないが、推定されたｚが前記範囲を超える場合が発生し得る。
【０００７】
したがって、ロボットの位置推定において誤差を減らすことによってロボットの安定性を図り、カルマンフィルタ適用時に推定されたパラメータが物理的な制限条件を満たすようにすることによってロボットの性能を向上させる必要がある。
【非特許文献１】Ｈ．Ｃｈｕｎｇ，Ｌ．Ｏｊｅｄａ，ａｎｄ  Ｊ．Ｂｏｒｅｎｓｔｅｉｎ，  “Ａｃｃｕｒａｔｅ  ｍｏｂｉｌｅ  ｒｏｂｏｔ  ｄｅａｄ−ｒｅｃｋｏｎｉｎｇ  ｗｉｔｈ  ａ  ｐｒｅｃｉｓｉｏｎ−ｃａｌｉｂｒａｔｅｄ  ｆｉｂｅｒ−ｏｐｔｉｃ  ｇｙｒｏｓｃｏｐｅ，”  ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．１７，Ｉｓｓｕｅ  １，ｐｐ．８０−８４，Ｆｅｂ  ２００１．
【発明の開示】
【発明が解決しようとする課題】
【０００８】
本発明は、ロボットの位置推定時に観測可能行列の階数を調節することによって位置推定に対する誤差を減らすことを目的とする。
【０００９】
また、本発明は既知の制限条件があるパラメータのためのカルマンフィルタ方法を適用することによって制限条件を満たせるようにすることを目的とする。
【００１０】
本発明の目的は以上で言及した目的に制限されず、言及していないさらなる目的は下記によって当業者に明確に理解できる。
【課題を解決するための手段】
【００１１】
また、前記目的を達成するために、本発明の実施形態によるロボットは、ジャイロスコープの回転角度に対する情報を提供するジャイロスコープモジュールと、ホイールの動作を感知して移動速度及び前記ホイールの回転角度に対する情報を提供するエンコーダモジュール、及び前記エンコーダモジュールと前記ジャイロスコープモジュールから提供される情報に基づいて所定の方法によって現在の位置を推定する制御モジュールを含むが、前記制御モジュールは前記ジャイロスコープモジュールによって提供される回転角度に対する情報を前記位置推定に反映する。
【００１２】
また、前記目的を達成するために、エンコーダモジュールとジャイロスコープモジュールを備えたロボットの位置推定方法において、本発明の実施形態によるロボットの位置推定方法は、前記ジャイロスコープモジュールからジャイロスコープの回転角度に対する情報が提供される（ａ）ステップと、前記エンコーダモジュールからホイールの動作による移動速度及び前記ホイールの回転角度に対する情報が提供される（ｂ）ステップ、及び前記エンコーダモジュールと前記ジャイロスコープモジュールから提供される情報に基づいて前記ロボットの位置を推定する（ｃ）ステップを含むが、前記（ｃ）ステップは前記ジャイロスコープモジュールによって提供される回転角度に対する情報を反映して前記ロボットの位置を推定するステップを含む。
【発明の効果】
【００１３】
本発明によれば、観測可能行列の階数を向上させてロボットの安定性を図ることができる効果がある。
【００１４】
また、本発明によれば、一般的な非線形制限条件に対して制限条件を満たすようにカルマンフィルタを適用することによってシステムの安定性が向上する効果がある。
【００１５】
さらに、本発明によれば、制限条件を満たすように更新するとき、最適化条件（Ｋ−Ｔ  Ｏｐｔｉｍａｌｉｔｙ  Ｃｏｎｄｉｔｉｏｎ）を満たせるように設計されるため、現在状態で最適の解を提供することができる効果がある。
【発明を実施するための最良の形態】
【００１６】
その他、実施形態の具体的な事項は詳細な説明及び図面に含まれている。
【００１７】
本発明の利点及び特徴、そしてそれらを達成する方法は添付する図面とともに詳細に後述する実施形態を参照すれば明確になる。しかし、本発明は以下に開示される実施形態に限定されず、相異なる多様な形態で具現でき、単に本実施形態は本発明の開示を完全なものにし、本発明の属する技術分野における通常の知識を有する者に発明の範疇を完全に知らせるために提供するものであって、本発明は請求項の範疇によってのみ定義される。
【００１８】
以下、本発明の実施形態により、ロボット及び前記ロボットの位置を推定する方法を説明するためのブロック図または処理フローチャートに関する図面を参考して本発明について説明する。この場合、処理フローチャートの各ブロックとフローチャートの組み合せは、コンピュータプログラムインストラクションによって行われることが理解できる。これらコンピュータプログラムインストラクションは、汎用コンピュータ、特殊用コンピュータまたはその他プログラム可能なデータプロセシング装備のプロセッサに搭載できるので、コンピュータまたはその他プログラム可能なデータプロセシング装備のプロセッサによって行われるインストラクションはフローチャートブロックで説明する機能を行う手段を生成する。これらコンピュータプログラムインストラクションは、特定方式で機能を具現するために、コンピュータまたはその他プログラム可能なデータプロセシング装備に備えられるコンピュータによって利用可能または読取可能なメモリに格納できるので、そのコンピュータによって利用可能または読取可能なメモリに格納したインストラクションは、フローチャートブロックで説明する機能を行うインストラクション手段を含む製造品目を生産するのに用いることができる。また、コンピュータプログラムインストラクションは、コンピュータまたはその他プログラム可能なデータプロセシング装備上に搭載できるので、コンピュータまたはその他プログラム可能なデータプロセシング装備上で一連の動作ステップが行われ、コンピュータで実行されるプロセスを生成してコンピュータまたはその他プログラム可能なデータプロセシング装備の機能を行うインストラクションは、フローチャートブロックで説明する各ステップの機能を実行することもできる。
【００１９】
また、各ブロックは特定の論理的機能を実行するための１つ以上の実行可能なインストラクションを含むモジュール、セグメントまたはコードの一部を表すことができる。また、複数の代替実行例ではブロックで言及した機能が順序から外れて発生し得ることに注目しなければならない。例えば、連なって図示している２つのブロックは、実質的に同時に行われることが可能であり、またはそのブロックが場合によって該当する機能に応じて逆順に行われることも可能である。
【００２０】
図１は、本発明の一実施形態によってロボットの位置推定のための方法を示すフローチャートである。
【００２１】
図１を参照すれば、まず状態変数を予測するが（Ｓ１１０）、本発明では従来の技術による状態方程式に新しい状態変数を追加して観測可能行列の階数の大きさを向上させるようにする。これは結局ロボットの安定性に繋がる。
【００２２】
Ｃｈｕｎｇなどによって提案された従来の間接カルマンフィルタ方法は、プロセスモデルとして走行距離計誤差モデル（またはエンコーダ誤差モデル）とジャイロスコープの回転角度誤差モデルの組合わせを使用し、測定モデルとしてジャイロスコープと走行距離計の間の測定された回転角の差を使用する。
【００２３】
間接カルマンフィルタについては、非特許文献１に具体的に開示されている。
【００２４】
従来の間接カルマンフィルタ方法における状態方程式は、エンコーダで推定したロボット位置の誤差、ジャイロスコープで推定したロボット角度の誤差、スケールファクタ、ホイール間の距離などのような各種パラメータ予測値、そして各種ノイズに対する状態を含むが、図２は及び図３にこれを示している。
【００２５】
図２は、移動ロボットの状態変数及びパラメータを示しており、図３ではこれに基づく状態方程式を示している。
【００２６】
図３において、「３１０」に該当する状態変数はエンコーダで推定したロボット位置の誤差を、「３２０」に該当する状態変数はジャイロスコープで推定したロボット角度の誤差を、「３３０」に該当する状態変数は各種パラメータ予測値を、「３４０」に該当する状態変数はノイズを示している。
【００２７】
図３において、ＳＲ（ｋ）、ＳＬ（ｋ）はそれぞれ右側ホイールと左側ホイールのスケールファクタ誤差を示し、δＤ（ｋ）は両ホイール間の距離誤差を示す。
【００２８】
しかし、図３のような状態方程式による間接カルマンフィルタは観測可能性がよくない。すなわち図３の状態方程式で状態ベクトルのディメンションは９であるが、観測可能行列の階数は状態転移行列が時間不変システムの正常状態にあると仮定する場合、単に２にすぎない。
【００２９】
すなわち、ロボットの状態が完全に観測されないので、残り７つの状態に対する収束可否を保障することができなくなる。これは単に回転角状態のみ観測行列に含まれているので、ロボットの位置（ｘ、ｙ）の誤差のような状態が真値に収束するか否かは保障できないのである。システムが時間変移である場合でも観測可能行列の階数は６ほどであるため、まだ不安定であるとみなされる。
【００３０】
したがって、本発明ではこれを解決するために、観測可能なシステム状態を追加して予測する。
【００３１】
一般に、離散時間区間（ｄｉｓｃｒｅｔｅ−ｔｉｍｅ  ｉｎｔｅｒｖａｌ）においてロボットのような動的システムモデルの観測可能行列は（式１）のように表すことができる。
【００３２】
【数１２】
2007132938.tif 000003
式中、Ｈｋはｋ時間における観測行列を示し、Ｆｋは１≦ｋ≦ｋｆに対してｋから（ｋ＋１）までの状態転移行列、Ｔは転置行列を示す。
【００３３】
このとき、ロボットの状態転移行列が時間不変である場合、観測可能性は（式２）のような行列の階数によって決定できる。
【００３４】
【数１３】
2007132938.tif 000004
もし（式２）で階数がｎである場合、該当システムは観測可能であるとみなされる。このとき、ｎはシステム状態ベクトルのディメンションを示す。
【００３５】
前述したように、従来の方法に従うと、階数が２であるためロボットシステムを完全に観測することができない。
【００３６】
本発明ではジャイロスコープと２つのホイールに対するエンコーダを使用するので、ジャイロスコープの回転角度が反映されたロボットの位置は観測可能である。
【００３７】
このとき、ロボットの位置とロボットのヘディング角度（φ（ｋ））は（式３）のように表すことができる。
【００３８】
【数１４】
2007132938.tif 000005
式中、
【００３９】
【数１５】
2007132938.tif 000006
は計算されたジャイロスコープの回転角度を使用して推定されたロボットの位置を示し、
【００４０】
【数１６】
2007132938.tif 000007
はジャイロスコープによって推定されたロボットの位置誤差を示す。そして、φはジャイロスコープの回転角度を示す。
【００４１】
したがって、
【００４２】
【数１７】
2007132938.tif 000008
を新たな状態変数として図３に示した状態方程式に追加すれば図４のような状態方程式になる。式中、４１０と表示される状態変数が新たに追加された状態変数であり、４２０と表示される部分が新たに追加されたシステム行列部分である。前記状態方程式は時間の流れにともなう状態変数の変化をモデリングするプロセスモデルとして理解しえる。
【００４３】
図４に基づく状態予測は（式４）のように表すことができる。
【００４４】
【数１８】
2007132938.tif 000009
式中、ｘは図４における状態変数を示し、ｕはロボットの速度成分を示す。また、Ｆは図４における行列Ｆを示し、Ｐは共分散行列、Ｑはノイズに対する分散を示す。また、
【００４５】
【数１９】
2007132938.tif 000010
を示す。
【００４６】
図４のように状態変数が予測された状態方程式を基盤として状態を観測する（Ｓ１２０）。
【００４７】
このために測定方程式のような観測モデル（Ｏｂｓｅｒｖａｔｉｏｎ  ｍｏｄｅｌ）が使用できるが、本発明による測定方程式は（式５）のように概略的に表すことができ、これを具体化した測定方程式を図５に示している。このとき観測モデルは観測が可能な状態変数の組合せに対するモデリングを表す。
【００４８】
測定方程式＝エンコーダで推定したロボットの位置−ジャイロスコープで推定したロボットの位置＋ノイズ                                        （式５）
図５でｖは測定ノイズを示す。
【００４９】
一方、従来の技術による測定方程式はそれぞれ（式６）と図６のように表すことができるが、図６では単に回転角状態のみ観測行列Ｈに含まれていることが分かる。
【００５０】
測定方程式＝エンコーダで推定したロボットの方向−ジャイロスコープで推定したロボットの方向＋ノイズ
（式６）
図６でｖは測定ノイズを示す。
【００５１】
これに反して、図５に示した本発明による測定方程式における観測行列Ｈは回転角の状態だけでなく、ロボットの位置に対する状態も含まれているので、（式１）で開示している観測可能行列の階数が向上する。
【００５２】
すなわち、本発明による方法を使用すれば、時間不変システムの場合、ｒａｎｋ（Ｏ）＝４になってｘ、ｙ距離の誤差が真値に収束するのに十分な条件を満たし、また、時間変移システムの場合、ｒａｎｋ（Ｏ）＝１０になってシステムが安定する。このとき、図４でδＢｓ（ｋ＋１）状態変数はジャイロスコープのスケールファクタ誤差であり、事前補正に係わるものであるので、階数演算時に除外する。
【００５３】
Ｓ１２０ステップでロボットの状態を観測して観測行列Ｈを含むＺｋを得れば（ここで、ＨとＺｋは図６に示したＨとＺを意味する）、前記Ｈに基づいてカルマンゲインを得、カルマンゲイン及び前記Ｚｋを使用して（式４）における状態変数ｘと共分散Ｐを更新する。これを（式７）で表している。
【００５４】
【数２０】
2007132938.tif 000011
式中、Ｋｋはカルマンゲインを、Ｒｋはノイズに対する分散を、Ｔは転置行列を、Ｉは単位行列を（ｉｄｅｎｔｉｔｙ  ｍａｔｒｉｘ）を示す。そして
【００５５】
【数２１】
2007132938.tif 000012
を示す。
【００５６】
一方、移動ロボットに制限条件が存在しない場合は（Ｓ１４０）、Ｓ１３０ステップで更新された状態変数
【００５７】
【数２２】
2007132938.tif 000013
を使用して移動ロボットの位置を推定するようになり、Ｓ１１０ステップを繰り返すようになる。
【００５８】
しかし、移動ロボットに制限条件が存在する場合は（Ｓ１４０）、前記制限条件に適合するようにＳ１３０ステップで更新された状態変数
【００５９】
【数２３】
2007132938.tif 000014
を補正することによって、補正された状態変数に基づいて移動ロボットの位置を推定するようになる（Ｓ１５０）。
【００６０】
このとき、ジャイロスコープを搭載した移動ロボットの制限条件として、両ホイール間の距離、ホイール径、温度変化によるジャイロスコープのスケールファクタとバイアスなどがあり、前記制限条件は線形制限条件、非線形制限条件、不等制限条件、等制限条件などを含むことができる。
【００６１】
図７は、本発明の一実施形態によって図１のＳ１５０ステップをより具体的に示すフローチャートであって、Ｓ１５０ステップは制限条件を使用して状態変数
【００６２】
【数２４】
2007132938.tif 000015
を更新するステップ（Ｓ１５２）と、ペナルティパラメータ及びラグランジュマルチプライヤを更新するステップ（Ｓ１５４）とを含む。
【００６３】
一方、本発明は制限条件があるカルマンフィルタ問題とみなすことができ、これを解決するために制限条件のない状態変数を制限条件がある平面に投射することによってその解を得ることができる。
【００６４】
すなわち、制限条件をｇ、ｈとすれば、ｇｉ（ｘ）≦０、ｈｊ（ｘ）＝０のような条件で（式８）を満足する解を求める。
【００６５】
【数２５】
2007132938.tif 000016
式中、ｇｉ（ｘ）とｈｊ（ｘ）はそれぞれ不等制限条件と等制限条件を示し、
【００６６】
【数２６】
2007132938.tif 000017
はカルマンフィルタされた状態、Ｗは単位行列またはプロセスノイズ共分散行列のような任意の正の定符号行列（ｐｏｓｉｔｉｖｅ  ｄｅｆｉｎｉｔｅ  ｍａｔｒｉｘ）を示す。
【００６７】
（式８）のような制限条件最適化問題において局部的に前記制限条件最適化問題を最小化させる状態の存在のための必要条件は、ラグランジュマルチプライヤを必要とするが、従来のクーン・タッカー最適化条件（Ｋｕｈｎ−Ｔｕｃｋｅｒ  ｏｐｔｉｍａｌｉｔｙ  ｃｏｎｄｉｔｉｏｎｓ）がこのような条件を提供することができる。
【００６８】
一方、前記（式８）は、コンベクス問題（凸問題、ｃｏｎｖｅｘ  ｐｒｏｂｌｅｍ）に該当するので、（式８）に従来のペナルティ関数定理を適用することができる。
【００６９】
ペナルティ関数定理は、コンベクス問題でペナルティパラメータｓｋが徐々に増加すれば、ラグランジュ関数の解は最適解に収束するという定理であって、（式９）のようなペナルティ関数によって表すことができる。
【００７０】
【数２７】
2007132938.tif 000018
式中、
【００７１】
【数２８】
2007132938.tif 000019
である。
【００７２】
したがって、（式８）にペナルティ関数定理を適用することによって制限条件を使用して更新された状態変数に対する解を（式１０）のように得ることができる。
【００７３】
【数２９】
2007132938.tif 000020
このとき、
【００７４】
【数３０】
2007132938.tif 000021
を満足する。
【００７５】
式中、
【００７６】
【数３１】
2007132938.tif 000022
はＳ１３０ステップで更新された状態変数を示し、
【００７７】
【数３２】
2007132938.tif 000023
は、制限条件を使用して更新された状態変数を示す。また、ｇは不等制限条件を、ｈは等制限条件を示し、ｓはペナルティパラメータ、λとμはラグランジュマルチプライヤを示す。
【００７８】
ここで、ペナルティパラメータｓは前述したペナルティ関数定理を基盤として選択できる。
【００７９】
一方、ペナルティパラメータｓとラグランジュマルチプライヤλとμに対する更新は、次のように行われる。
【００８０】
初期にはε＝０、α＝正の定数にしてから、一定時間後にεも小さな正の定数に変更することによってｓ、λ、μを更新することができる。α値は収束速度を向上させるために一定時間後に必要に応じて他の正の定数に変更することもできる。
【００８１】
結局、（式１０）のように制限条件があるカルマンフィルタの更新は、クーン・タッカー最適化条件とペナルティ関数定理を満足するので、最適化された解とみなすことができる。
【００８２】
特に、従来技術は線形制限条件のみ考慮し、解を求めるためには任意の正の定符号行列Ｗの逆関数も計算されなければならないが、本発明では制限条件が任意の非線形関数であるときにも適用可能であり、（式１０）のようにＷの逆関数に対する計算が不要なので、従来に比べて非常に改善したアルゴリズムとみなすことができる。
【００８３】
図８は、本発明の一実施形態によるロボットの構造を示すブロックであって、本発明によるロボット８００はジャイロスコープモジュール８１０、エンコーダモジュール８２０、駆動モジュール８３０、そして制御モジュール８４０を含む。
【００８４】
このとき、本実施形態で使用される「モジュール」という用語は、ソフトウェアまたはＦＰＧＡまたはＡＳＩＣのようなハードウェア構成要素を意味し、モジュールはある役割を行う。しかし、モジュールはソフトウェアまたはハードウェアに限定される意味ではない。モジュールはアドレシングできる格納媒体にあるように構成することができ、１つまたはそれ以上のプロセッサを実行させるように構成することもできる。したがって、一例としてモジュールは、ソフトウェア構成要素、オブジェクト指向ソフトウェア構成要素、クラス構成要素及びタスク構成要素などのような構成要素と、プロセス、関数、属性、プロシージャ、サブルーチン、プログラムコードのセグメント、ドライバ、ファームウェア、マイクロコード、回路、データ、データベース、データ構造、テーブル、アレイ、及び変数を含む。構成要素とモジュールにより提供される機能は、さらに小さい数の構成要素及びモジュールで結合されたり、追加的な構成要素とモジュールにさらに分離することができる。さらに、構成要素及びモジュールはデバイスまたは保安マルチメディアカード内の１つまたはそれ以上のＣＰＵを実行させるように具現できる。
【００８５】
また、本発明における「ロボット」とは、ジャイロスコープとエンコーダを使用して自己位置を推定する物体を意味する。
【００８６】
ジャイロスコープモジュール８１０は、移動ロボット８００に付着されてジャイロスコープが回転する角度に対する情報を提供する。これは図４に示したφに該当する。
【００８７】
そして、エンコーダモジュール８２０は、ホイールなどを含む駆動モジュール８３０の動作を感知して移動ロボット８００の移動速度（図４におけるＵＬ、ＵＲ）及びホイールの回転角度（図４におけるΨ）に対する情報を提供する。
【００８８】
制御モジュール８４０は、エンコーダモジュール８２０とジャイロスコープモジュール８１０から提供される情報を入力とし、前述した方法のように制限条件があるカルマンフィルタ方法によって移動ロボット８００の位置を推定するようになる。このとき、前記φ情報が反映された移動ロボット８００の位置情報を考慮することによって移動ロボット８００システムの安定性を図るようになる。
【００８９】
制御モジュール８４０が移動ロボット８００の位置を推定するようになれば、それに対応して駆動モジュール８３０を動作させることによって移動ロボット８００は移動するようになる。
【００９０】
一方、本発明の一実施形態として、ロボットパラメータは、ホイール間の距離（Ｄ）を０．３７８ｍ、ホイール径（Ｒ）を０．１２２ｍにし、ジャイロスコープパラメータは、ジャイロスコープスケールファクタ（角度スケールファクタに対する比率）（Ｂｓ）を０．０１に、バイアス（Ｂｂ）は０（ｄｅｇ／ｓｅｃ）にした。
【００９１】
また一例としてパラメータの制限条件を次の通りした。
【００９２】
【数３３】
2007132938.tif 000024
このとき、図９Ａは従来技術によって制限条件を反映しない場合の実験結果を、図９Ｂは本発明の実施によって制限条件を反映した場合の実験結果を示すグラフである。
【００９３】
図９Ａ及び図９Ｂを比較すれば、図９Ａでは
【００９４】
【数３４】
2007132938.tif 000025
に対する制限条件を満足していないが、図９Ｂでは
【００９５】
【数３５】
2007132938.tif 000026
に対する制限条件を満足していることが分かる。
【００９６】
図１０は、カルマンフィルタの１−サイクル当たり平均処理時間を比較するグラフである。
【００９７】
図１０を参照すれば、「ＥＫＦ」に区別されるグラフは従来技術による平均処理時間を、「Ｃ−ＫＦ」に区別されるグラフは本発明による平均処理時間を示し、「ＵＫＦ」に区別されるグラフは「Ｕｎｓｃｅｎｔｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ」方式による平均処理時間を、「ＳＲ−ＵＫＦ」は「Ｓｑｕａｒｅ−Ｒｏｏｔ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ」方式による平均処理時間を示す。
【００９８】
この中、従来技術と本発明による平均処理時間を比較すれば、ほとんど差がないことが分かる。すなわち、従来技術に比べて、平均処理時間は保持しつつ、移動ロボットの位置を推定するのにおいて、物理的な制限条件を満足してロボットの位置に対する誤差をさらに減らすことができるようになる。
【００９９】
一方、本発明では制限条件があるカルマンフィルタをロボットの位置推定に適用したものを開示しているが、これは例示的なものに過ぎず、これに限定されるものではない。
【０１００】
以上、添付する図面を参照して本発明の実施形態を説明したが、本発明の属する技術分野における通常の知識を有する者は本発明がその技術的思想や必須的な特徴を変更せずに他の具体的な形態によって実施できることを理解することができる。したがって前述した実施形態はすべての面で例示的なものであって、限定的なものではないことを理解しなければならない。
【図面の簡単な説明】
【０１０１】
【図１】本発明の一実施形態によってロボットの位置推定のための方法を示すフローチャートである。
【図２】本発明の一実施形態による移動ロボットの状態変数及びパラメータを示す例示図である。
【図３】図２に対する従来技術による状態方程式である。
【図４】図２に対する本発明の状態方程式である。
【図５】図２に対する本発明による測定方程式である。
【図６】図２に対する従来技術による測定方程式である。
【図７】本発明の一実施形態によって図１のＳ１５０ステップをより具体的に示すフローチャートである。
【図８】本発明の一実施形態によるロボットの構造を示すブロック図である。
【図９Ａ】従来技術によって制限条件を反映しない場合の実験結果を示すグラフである。
【図９Ｂ】本発明の実施によって制限条件を反映した場合の実験結果を示すグラフである。
【図１０】カルマンフィルタの１−サイクル当たり平均処理時間（ｍｅａｎ  ｐｒｏｃｅｓｓｉｎｇ  ｔｉｍｅ）を比較するグラフである。
【符号の説明】
【０１０２】
８１０      ジャイロスコープモジュール
８２０      エンコーダモジュール
８３０      駆動モジュール
８４０      制御モジュール
【図１】
2007132938.tif 000038
【図２】
2007132938.tif 000039
【図３】
2007132938.tif 000040
【図４】
2007132938.tif 000041
【図５】
2007132938.tif 000042
【図６】
2007132938.tif 000043
【図７】
2007132938.tif 000044
【図８】
2007132938.tif 000045
【図９Ａ】
2007132938.tif 000046
【図９Ｂ】
2007132938.tif 000047
【図１０】
2007132938.tif 000048