(57)【特許請求の範囲】
【請求項１】
移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、
移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、
上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、
上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する初期角度推定手段と、
現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、
上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と、
現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えていることを特徴とする状態推定装置。
【請求項２】
移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、
移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、
上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、
上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する絶対角度測定手段と、
上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する初期角度推定手段と、
現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、
上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と、
現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えていることを特徴とする状態推定装置。
【請求項３】
上記相対角度測定手段は、上記絶対角度測定手段が絶対角度を測定するタイミングにおいて既に測定された相対姿勢角を補完し、その補完された相対姿勢角を、上記初期角度推定手段に出力することを特徴とする請求項２に記載の状態推定装置。
【請求項４】
上記相対位置測定手段は、上記絶対位置測定手段が絶対位置を測定するタイミングにおいて既に測定された相対位置を補完し、その補完された相対位置を、上記初期位置推定手段に出力することを特徴とする請求項１または２に記載の状態推定装置。
【請求項５】
移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第１ステップと、
移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第２ステップと、
上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第３ステップと、
上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する第４ステップと、
現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第５ステップと、
上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第６ステップと、
現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第７ステップと、を備えていることを特徴とする状態推定方法。
【請求項６】
移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第１ステップと、
移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第２ステップと、
上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第３ステップと、
上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する第４ステップと、
上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する第５ステップと、
現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第６ステップと、
上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第７ステップと、
現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第８ステップと、を備えていることを特徴とする状態推定方法。
【請求項７】
請求項５に記載の状態推定方法における第４ステップから第７ステップまで、または請求項６に記載の状態推定方法における第５ステップから第８ステップまでを、コンピュータに実行させることを特徴とする状態推定プログラム。
【請求項８】
請求項７に記載の状態推定プログラムを記録したことを特徴とするコンピュータ読取可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、自動車、船舶等の存在する位置を推定する状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体に関する。
【背景技術】
【０００２】  船舶や自動車、航空機など、現在の社会においては、さまざまな移動体が用いられている。移動体については、カーナビや船舶の状態監視など、移動体の状態を計測することが必要になる状況も非常に多い。【０００３】  移動体の状態計測手法は、大別すると、デッドレコニング的な手法と、天測航法的な手法との２つに分類される。天測航法的な手法は、ＧＰＳやカメラなどの外界センサを用いて大域情報を直接計測する手法であり、デッドレコニング的な手法は、ロータリーエンコーダ、ジャイロセンサ、加速度センサなどの内界センサを用いて、微小変化を連続して計測し、積分を行うことによって、初期状態からの変化量を計測する手法である。【０００４】  天測航法的な手法は、累積誤差が存在せず、大域的な状態を直接計測できるが、計測値の分散が大きく、サンプリングタイムが長いといった欠点がある。【０００５】  一方、デッドレコニング的な手法は、サンプリングタイムが短く、計測値の分散が小さい。しかし、デッドレコニング的な手法には、大域情報を得るためには初期情報が必要であり、車輪の横滑りなど内界センサで計測できないような外乱に非常に弱く、計測誤差が累積するといった欠点がある。【０００６】  このような欠点を克服するために、上述の２つの手法を組み合わせて用いるセンサフュージョンに関する研究が盛んに行われており、代表的な手法としてＫａｌｍａｎフィルタを用いた手法が挙げられる。Ｋａｌｍａｎフィルタを用いる手法は、デッドレコニング的な手法および天測航法的な手法による計測値の信頼性を評価する誤差分散行列を利用し、最尤推定法を用いて移動体の状態を推定する。【０００７】  しかしながら、デッドレコニング的な手法は、移動体の移動に応じて誤差が累積するので、誤差分散行列を求めることが困難である。また、Ｋａｌｍａｎフィルタを用いる手法は、最尤推定法を用いて移動体の状態を推定するので、デッドレコニング的な手法と天測航法的な手法との間で誤差分散行列の差が大きい場合には、ほとんど効果がない。さらに、初期状態が未知の場合、デッドレコニング的な手法は大域情報を計測できないため、Ｋａｌｍａｎフィルタによる推定手法は用いることができない。
【発明の開示】
【０００８】  本発明は、上記従来の課題に鑑みなされたものであって、誤差分散行列を必要とせず、初期状態が未知の場合でも使用できる状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体を提供することを目的とする。
【０００９】  本発明の状態推定装置は、上記課題を解決するために、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する初期角度推定手段と、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えていることを特徴としている。
【００１０】  上記構成によれば、誤差分散行列を用いずに、たとえば後述する式（１）を用いて、移動体の現在姿勢角を、現在角度測定手段により求め、さらに移動体の現在位置を現在位置推定手段により求めることができる。よって、本発明によれば、従来のデッドレコニング的な手法とは異なり、相対位置測定手段および相対角度測定手段として実現される内界センサにおける測定誤差の累積に関わらず、移動体の現在位置および現在姿勢角を推定することができる。その上、本発明によれば、Ｋａｌｍａｎフィルタを用いる方法とは異なり、デッドレコニング的な手法と天測航法的な手法との間で誤差分散行列の差が大きくなっても、移動体の現在位置および現在姿勢角を推定することができる。
【００１１】  さらに、本発明においては、初期角度推定手段により移動体の初期姿勢角が推定され、さらに初期位置推定手段により移動体の初期位置が推定される。
【００１２】  すなわち、初期角度推定手段においては、上記相対姿勢角および上記絶対姿勢角を含む漸化式（たとえば、後述する式（５））で表現される数列の収束値（θｏ）が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する。また、初期位置推定手段においては、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式（たとえば、後述する式（８））で表現される数列の収束値（ｑｏ）が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する。
【００１３】  よって、本発明によれば、従来のＫａｌｍａｎフィルタを用いる方法とは異なり、初期状態が未知の場合であっても、移動体の現在位置および現在姿勢角を推定することができる。
【００１４】  また、本発明の状態推定装置は、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する絶対角度測定手段と、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する初期角度推定手段と、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えていることを特徴としている。
【００１５】  上記構成によれば、絶対角度測定手段を備えているので、上記作用効果に加えて、移動体の絶対姿勢角を精度良く求めることができる。したがって、本発明において絶対姿勢角に基づき推定される値、すなわち初期姿勢角および初期位置の推定精度が高められ、さらに現在位置および現在姿勢角の推定精度も高められる。
【００１６】  また、本発明の状態推定方法は、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第１ステップと、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第２ステップと、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第３ステップと、上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する第４ステップと、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第５ステップと、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第６ステップと、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第７ステップと、を備えていることを特徴としている。
【００１７】  上記構成の状態推定方法によれば、第１ステップから第７ステップまでにより、本発明の状態推定装置と同様の作用が実現されているので、本発明の状態推定装置と同様の作用効果を得ることができる。
【００１８】  また、本発明の状態推定方法は、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第１ステップと、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第２ステップと、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第３ステップと、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する第４ステップと、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する第５ステップと、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第６ステップと、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第７ステップと、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第８ステップと、を備えている構成であってもよい。
【００１９】  上記構成の状態推定方法によれば、第１ステップから第８ステップまでにより、本発明の状態推定装置と同様の作用が実現されているので、本発明の状態推定装置と同様の作用効果を得ることができる。
【図面の簡単な説明】
【００２０】
【図１】本発明の一実施形態に係る状態推定装置の構成を示すブロック図である。
【図２】移動体の状態を表現する変数を説明するための図である。
【図３】局所座標系および大域座標系を説明するための図である。
【図４】局所情報を補完するタイミングを説明するための図である。
【図５】移動体が大域座標内を（ｘｇ，ｎ−ｋ，ｙｇ，ｎ−ｋ）から（ｘｇ，ｎ，ｙｇ，ｎ）まで移動し、局所座標系内を（ｘｌ，ｎ−ｋ，ｙｌ，ｎ−ｋ）から（ｘｌ，ｎ，ｙｌ，ｎ）まで移動した状態を示す図である。
【図６】本発明の効果確認実験で使用したシステムの構成を示す図である。
【図７（ａ）】図６のシステムで用いた二輪車両型ロボットの斜視図である。
【図７（ｂ）】図６のシステムで用いた二輪車両型ロボットの底面図である。
【図８】図６のシステムにおけるビジョンシステムが、二輪車両型ロボットについて計測した２点の位置と、姿勢角との関係を示す図である。
【図９】本発明の効果確認実験で得られる値（ｘ，ｙ，θ）のそれぞれに関する推定値を示すグラフである。
【図１０】本発明の効果確認実験で得られる値（ｘ）に関し、初期状態の推定値を示すグラフである。
【図１１】本発明の効果確認実験で得られる値（ｙ）に関し、初期状態の推定値を示すグラフである。
【図１２】本発明の効果確認実験で得られる値（θ）に関し、初期状態の推定値を示すグラフである。
【符号の説明】
【００２１】１  状態推定装置
２  相対位置測定部（相対位置測定手段）
３  絶対位置測定部（絶対位置測定手段）
４  相対角度測定部（相対角度測定手段）
５  絶対角度測定部（絶対角度測定手段）
６  初期角度推定部（初期角度推定手段）
７  現在角度推定部（現在角度推定手段）
８  初期位置推定部（初期位置推定手段）
９  現在位置推定部（現在位置推定手段）
【発明を実施するための最良の形態】
【００２２】  〔１．状態推定装置の構成〕
本発明の一実施形態に係る状態推定装置の構成について、図１を用いて説明する。図１に示すように、状態推定装置１は、相対位置測定部２と、絶対位置測定部３と、相対角度測定部４と、絶対角度測定部５と、初期角度推定部６と、現在角度推定部７と、初期位置推定部８と、現在位置推定部９とを備えている。【００２３】  相対位置測定部２および相対角度測定部４は、いわゆる内界センサにより実現される。ここで、内界センサとは、移動体の相対位置および相対的な姿勢角（相対姿勢角）を測定するセンサである。すなわち、相対位置測定部２は、内界センサが相対位置を測定する機能を表現したものであり、相対角度測定部４は、内界センサが相対姿勢角を測定する機能を表現したものである。【００２４】  内界センサは、たとえば、オドメトリを用いるセンサ、加速度センサ、またはジャイロセンサにより実現可能であり、一般的に、（１）測定誤差が小さい、（２）サンプリングレートが高い、（３）初期値が未知の場合には使えない、（４）測定誤差が累積するといった４つの特性を有している。【００２５】  また、絶対位置測定部３および絶対角度測定部５は、いわゆる外界センサにより実現される。ここで、外界センサとは、移動体の絶対的な位置（絶対位置）または絶対的な姿勢角（絶対姿勢角）を測定するものである。【００２６】  たとえば、外界センサの一例であるＧＰＳセンサは、移動体の絶対位置を測定できるので、絶対位置測定部３として利用可能である。また、外界センサの一例である地磁気センサは、移動体の絶対姿勢角を測定できるので、絶対角度測定部５として利用可能である。さらに、ビジョンシステム、ランドマークセンサ、およびレンジファインダは、移動体の絶対位置および絶対姿勢角を測定できるので、絶対位置測定部３および絶対角度測定部５として利用可能である。【００２７】  なお、たとえば緯度や経度のように、移動体の状態に関わらず移動体の位置を特定しえるパラメータが、「絶対位置」に相当する。また、たとえば地軸に対する角度のように、移動体の状態に関わらず移動体の姿勢角を特定しえるパラメータが、「絶対姿勢角」に相当する。【００２８】  初期角度推定部６は、相対角度測定部４から得られる移動体の相対姿勢角、および絶対角度測定部５から得られる移動体の絶対姿勢角に基づき、移動体の初期状態における絶対姿勢角を推定するものである。初期角度推定部６における演算処理に関しては後述する。【００２９】  現在角度推定部７は、相対角度測定部４から得られる移動体の相対姿勢角、および初期角度推定部６により推定された移動体の初期状態における絶対姿勢角に基づき、移動体の現在の状態における絶対姿勢角を推定するものである。現在角度推定部７における演算処理に関しては後述する。【００３０】  初期位置推定部８は、相対位置測定部２から得られる移動体の相対位置、および絶対位置測定部３から得られる移動体の絶対位置に基づき、移動体の初期状態における絶対位置を推定するものである。初期位置推定部８における演算処理に関しては後述する。【００３１】  現在位置推定部９は、初期位置推定部８により推定された移動体の初期状態における絶対位置、および相対位置測定部２から得られる移動体の相対位置に基づき、移動体の現在の状態における絶対位置を推定するものである。現在位置推定部９における演算処理に関しては後述する。【００３２】  以上の構成により、状態推定装置１は、初期状態における移動体の絶対位置および絶対姿勢角を推定するとともに、その絶対位置および絶対姿勢角を用いて、現在の状態における移動体の絶対位置および絶対姿勢角を推定するものである。以下、状態推定装置１を構成する各ブロックの演算処理の詳細を説明する。【００３３】  〔２．移動体の状態を表現する変数の説明および前提条件〕
ある座標系上を運動する移動体を一意に表現するためには、位置および姿勢角を表す３つの状態変数（ｘ，ｙ，θ）が必要となる。ここで、図２に示すように、位置ｑ＝［ｘ，ｙ］Ｔは移動体の中心を表し，姿勢角θは移動体と座標軸との相対角度を示すものとする。今、ある与えられた大域座標系ΣＧにおいて、現在の移動体の状態を示す（ｑｇ，θｇ）を、状態推定装置１により推定することを考える。【００３４】  さらに、状態推定装置１により（ｑｇ，θｇ）を推定するための前提条件として、以下のことを仮定する。【００３５】  （１）初期状態における移動体の座標を示すｑｏおよびθｏは未知である。【００３６】  （２）局所情報ｑｌおよびθｌは、デッドレコニング的な手法で計測した初期状態からの変化量とし、外乱以外の誤差は生じないとする。【００３７】  （３）大域情報ｑｇおよびθｇは、天測航法的な手法で計測した現在位置および姿勢角とし、計測値に平均０のホワイトノイズが付加しているとする。【００３８】  このような前提条件では、初期状態のｑｏおよびθｏが未知であるため、デッドレコニング的な手法では大域情報が計測できない。また，天測航法的な手法は、ホワイトノイズが付加しているため、計測値の分散が大きく、この手法のみを用いることは好ましくない。さらに、初期状態のｑｏおよびθｏが未知であるため、Ｋａｌｍａｎフィルタを用いることができない。【００３９】  これらの問題は、本実施形態の状態推定装置１により実現される初期状態オブザーバを用いて大域情報を推定することで、解決される。以下、初期状態オブザーバについて具体的に説明する。【００４０】  〔３．初期状態オブザーバを用いた（ｑｇ，θｇ）の推定〕
まず、図３に示すように、局所座標ΣＬを定義する。大域座標系ΣＧでの移動体の状態（ｑｇ，θｇ）は、次に示す（１）式によって表現できる。【００４１】
【数１】
0004774522.tif 000002
ここで、ｑｇ（＝［ｘｇ，ｙｇ］Ｔ）およびθｇはそれぞれ大域座標系ΣＧでの位置と姿勢角を表し、ｑｌ（＝［ｘｌ，ｙｌ］Ｔ）およびθｌは、それぞれ局所座標系ΣＬでの位置および姿勢角を表すものである。また、ｑｏ（＝［ｘｏ，ｙｏ］Ｔ）およびθｏは、それぞれ大域座標系ΣＧでの初期状態における移動体の位置と姿勢角を表すものである。【００４２】  本明細書では、局所座標系ΣＬでの移動体の位置，姿勢角を、局所情報といい、局所座標系ΣＬでの移動体の位置および姿勢角のそれぞれが、「相対位置」、「相対姿勢角」に対応している。また、大域座標系ΣＧでの位置，姿勢角を大域情報といい、大域座標系ΣＧでの位置および姿勢角のそれぞれが、「絶対位置」、「絶対姿勢角」に対応している。なお、Ｔ（ｘ）は、以下のように示される姿勢変換行列である。【００４３】
【数２】
0004774522.tif 000003
もし、デッドレコニング的な手法で計測した局所情報ｑｌ，θｌに誤差が生じなければ、移動体の初期状態ｑｏ，θｏを推定し、座標変換式（１）式を用いることで大域情報（ｑｇ，θｇ）を推定することができる。【００４４】  〔４．初期位置オブザーバの導出〕
以下では、初期位置（初期状態における位置）ｑｏが未知、初期姿勢角（初期状態における姿勢角）θｏが既知である条件のもとに、移動体の初期位置を推定するオブザーバを導出する。【００４５】  まず、局所情報と大域情報から初期位置を推定するオブザーバを導出するためにつぎの補題を与える。【００４６】  （補題２．１）図２に示す移動体、および図３に示す局所座標系ΣＬと大域座標系ΣＧについて考える。移動体の初期位置ｑｏが既知であるとし、下記の式（２）で示される離散時間オブザーバを与える。【００４７】
【数３】
0004774522.tif 000004
ここで、【００４８】
【数４】
0004774522.tif 000005
は、ｑｏの推定値であり、単に「ｑｏｂ，ｎ」と記載する場合もある。また、Ｋ：＝ｄｉａｇ（ｋｘ，ｋｙ）とする。【００４９】  この式（２）で示されるオブザーバは、０＜ｋｘ＜２かつ０＜ｋｙ＜２を満たすならば、ｎ→∞のとき、ｑｏｂ，ｎ→ｑｏとなる。【００５０】  （証明）
式（２）の両辺からｑｏを引くと、以下のようになる。【００５１】
【数５】
0004774522.tif 000006
したがって、−１＜１−ｋｘ＜１かつ−１＜１−ｋｙ＜１を満たすならば、ｎ→∞のとき、ｑｏｂ，ｎ→ｑｏとなる。【００５２】  以上により、補題２．１を証明できる。さらに、補題２．１を用いると、初期位置オブザーバに関するつぎの補題２．２を得ることができる。【００５３】  （補題２．２）図２に示す移動体、および図３に示す局所座標系ΣＬと大域座標系ΣＧについて考える。移動体の初期位置ｑｏが未知であり、初期姿勢角θｏが既知であるとして、以下の式（３）で示される離散時間オブザーバを与える。【００５４】
【数６】
0004774522.tif 000007
このオブザーバは、０＜ｋｘ＜２かつ０＜ｋｙ＜２を満たすならば、ｎ→∞のとき、ｑｏｂ，ｎ→ｑｏとなる。【００５５】  （証明）式（１）より、つぎの式（４）を得る。【００５６】
【数７】
0004774522.tif 000008
また、式（３）および式（４）より、以下の方程式を導くことができる。【００５７】
【数８】
0004774522.tif 000009
よって、補題２．１より、−１＜１−ｋｘ＜１かつ−１＜１−ｋｙ＜１を満たすなら，ｎ→∞のとき、ｑｏｂ，ｎ→ｑｏとなる。【００５８】  〔５．初期姿勢角が未知の場合への拡張〕
初期位置ｑｏおよび初期姿勢角θｏが未知の場合について考える。まず、つぎに示す初期姿勢角オブザーバに関する命題を与える。【００５９】  （命題２．１）図２に示す移動体、および図３に示す局所座標系ΣＬと大域座標系ΣＧについて考える。移動体の初期姿勢角θｏが未知であるとし、以下の式（５）で示される離散時間オブザーバを与える。【００６０】
【数９】
0004774522.tif 000010
ここで、【００６１】
【数１０】
0004774522.tif 000011
は、θｏの推定値であり、単に「θｏｂ，ｎ」と記載する場合もある。【００６２】  また、θｌ，ｎは局所座標系ΣＬにおける姿勢角であり、θｇ，ｎは大域座標系ΣＧにおける姿勢角である。【００６３】  さらに、式（５）においては、θｌ，ｎ＋θｏｂ，ｎ−θｇ，ｎの値について、−π＜θｌ，ｎ＋θｏｂ，ｎ−θｇ，ｎ≦πの範囲に正規化されているとする。【００６４】  このとき、０＜ｋθ＜２を満たすならば、ｎ→∞のとき、θｏｂ，ｎ→θ０となる。【００６５】  （命題２．１の証明）
式（５）の両辺からθ０を引き、θｇ，ｎ＝θｌ，ｎ＋θ０を式（５）に代入すると、下式を得ることができる。【００６６】
【数１１】
0004774522.tif 000012
さらに、θｏｂ，ｎ−θｏ＝ｅθ，ｎを用いると、下式（６）を得ることができる。【００６７】
【数１２】
0004774522.tif 000013
この式（６）に対して、【００６８】
【数１３】
0004774522.tif 000014
はＬｙａｐｕｎｏｖ関数となるから、ｎ→∞のときｅθ，ｎ→０となるので、ｎ→∞のときθｏｂ，ｎ→θ０となる（命題２．１の証明終わり）。【００６９】  そして、命題２．１を用いると、初期状態オブザーバに関して次の定理２．１を得ることができる。【００７０】  （定理２．１）図２に示す移動体、および図３に示す局所座標系ΣＬと大域座標系ΣＧについて考える。移動体の初期位置ｑｏおよび移動体の初期姿勢角θｏが未知であるとし、式（５）および以下の式（８）で示される離散時間オブザーバを与える。【００７１】
【数１４】
0004774522.tif 000015
ここで、【００７２】
【数１５】
0004774522.tif 000016
は、ｑｏの推定値であり、単に「ｑｏｂ，ｎ」と記載する場合もある。また、ｑｌ，ｎは局所座標系ΣＬでの移動体の位置を示すものであり、ｑｇ，ｎは、大域座標系ΣＧでの移動体の位置を示すものであり、Ｋ：＝ｄｉａｇ（ｋｘ，ｋｙ）とする。【００７３】  この式（８）で示されるオブザーバは、ｑｌ，ｎが有界で、０＜ｋｘ＜２かつ０＜ｋｙ＜２であるならば、ｎ→∞のときｑｏｂ，ｎ→ｑｏとなる。【００７４】  （証明）
式（８）の両辺から、ｑｏを引き、式（８）にｑｇ，ｎ＝Ｔ（θｏ）ｑｌ，ｎ＋ｑｏを代入する。さらにεｎ＝ｑｏｂ，ｎ−ｑｏと置くと、以下の式（９）が得られる。【００７５】
【数１６】
0004774522.tif 000017
さらに、式（１０）に示すｚを与える。【００７６】
【数１７】
0004774522.tif 000018
ｚの性質として、ｑｌ，ｎが有界であるとの仮定および命題２．１より、ｎ→∞のときｚ→０となる。このｚを用いると、式（９）は下式（１１）に変形できる。【００７７】
【数１８】
0004774522.tif 000019
と変形できる。ここで、ｚおよび状態ｑを入力とみなすと、式（１１）は離散時間ＩＳＳ（ｉｎｐｕｔ−ｔｏ−ｓｔａｔｅ  ｓｔａｂｌｅ）となり、式（１１）は漸近安定となる。【００７８】  ゆえに，ｑｌ，ｎが有界で、０＜ｋｘ＜２かつ０＜ｋｙ＜２であるならば、ｎ→０のとき、（ｑｏｂ，ｎ，θｏｂ，ｎ）→（ｑｏ，θｏ）となる。【００７９】  〔６．装置構成とオブザーバとの関係〕
本実施形態における状態推定装置１（図１参照）は、上述した初期状態オブザーバ式（５）および式（８）を用いて、移動体の初期状態を推定し、さらに式（１）を用いることによって、移動体の現在の状態における絶対位置および絶対姿勢角を推定するものである。以下、その詳細について説明する。【００８０】  まず、状態推定装置１における初期角度推定部６の処理について説明する。初期角度推定部６は、上述した初期状態オブザーバを示す式（５）を用いて、移動体の初期状態における絶対姿勢角を演算する。【００８１】  すなわち、相対角度測定部４により、上述した初期状態オブザーバを示す式（５）におけるθｌ，ｎが取得される。さらに、絶対角度測定部５により、式（５）におけるθｇ，ｎが取得される。したがって、初期角度推定部６は、式（５）にθｌ，ｎおよびθｇ，ｎを代入するとともに、θｏｂ，０として、絶対角度測定部５から得られる絶対姿勢角を推定値として用いることで、θｏｂ，ｎの収束値であるθｏを推定することができる。【００８２】  次に、現在角度推定部７における処理について説明する。現在角度推定部７は、上述した式（１）を用いて、移動体の現在の状態における絶対姿勢角を推定する。すなわち、現在角度推定部７は、式（１）におけるθｌとして、相対角度測定部４から得られるθｌ，ｎを代入し、さらに、式（１）におけるθｏとして、初期角度推定部６により推定されたものを代入する。これにより、現在角度推定部７は、移動体の現在の状態における絶対姿勢角を示すθｇを演算することができる。【００８３】  次に、初期位置推定部８における処理について説明する、初期位置推定部８は、上述した初期状態オブザーバを示す式（８）を用いて、移動体の初期状態における絶対位置を演算する。【００８４】  すなわち、初期角度推定部６により、式（８）におけるθｏｂ，ｎの収束値であるθｏが演算される。さらに、式（８）におけるｑｌ，ｎは相対位置測定部２から取得でき、ｑｇ，ｎは絶対位置測定部３から取得できる。したがって、初期位置推定部８は、式（８）にｑｌ，ｎおよびｑｇ，ｎを代入するとともに、ｑｏｂ，０として、絶対位置測定部３から得られる絶対位置を推定値として用いることで、ｑｏを推定することができる。【００８５】  最後に、現在位置推定部９における処理について説明する。現在位置推定部９は、式（１）を用いて、移動体の現在の状態における絶対位置を推定する。すなわち、現在位置推定部９は、式（１）におけるθｏとして初期角度推定部６により推定されたものを代入し、ｑｌとして相対位置測定部２から得られるｑｌ，ｎを代入し、ｑＯとして初期位置推定部８により推定されたものを代入する。これにより、現在位置推定部９は、移動体の現在の状態における絶対位置を示すｑｇを演算することができる。【００８６】  このようにして、状態推定装置１は、移動体の初期状態（θｏ，ｑｏ）を推定し、さらに、移動体の現在の状態における絶対位置（ｑｇ）をおよび絶対姿勢角（θｇ）を推定することができる。さらに、式（５）および式（８）は、ローパスフィルタとして機能し、高周波ノイズを除去することができる。【００８７】  〔７．計測誤差の影響〕
以下、計測値に生じる誤差の影響について説明する。今、時刻ｋにおいて横滑りなどにより位置にΔｑｌ、姿勢角にΔθｌの外乱が生じたとする。このとき、時刻ｋ以降の局所情報の推定値は、つぎの式（１２）で表される。【００８８】
【数１９】
0004774522.tif 000020
ただし、ｎ≧ｋである。【００８９】  なお、式（１２）における左辺の値が局所情報の推定値を示しており、右辺における【００９０】
【数２０】
0004774522.tif 000021
および【００９１】
【数２１】
0004774522.tif 000022
の値が、局所情報の計測値を示している。【００９２】  この局所情報に生じた誤差が初期状態の推定値に与える影響を考えるためにつぎの補題を与える。【００９３】  （補題２．３）式（５）および（８）で示した初期状態オブザーバについて考える。時刻ｋにおいて位置にΔｑｌ、姿勢角にΔθｌの外乱が生じたとする。このとき、初期状態オブザーバは、ｎ→∞のとき（ｑｏｂ，ｎ，θｏｂ，ｎ）→（ｑｏ＋Ｔ（θｏ）ｑｌ，ｎ−Ｔ（θｏ＋Δθｌ）（ｑｌ，ｎ＋Δｑｌ），θｏ＋Δθｌ）となる。【００９４】  （証明）まず、時刻ｋ以降の初期姿勢角オブザーバ式（５）について考える。式（１２）より、【００９５】
【数２２】
0004774522.tif 000023
となる。命題２．１より、ｎ→∞のとき、θｏｂ，ｎ→θｌ，ｎ＋Δθｌとなる。【００９６】  続いて初期位置オブザーバについて考える。今、初期状態の姿勢角の推定値θｏｂ，ｎが、θｌ，ｎ＋Δθｌに収束したとする。【００９７】  このとき、初期位置オブザーバは、【００９８】
【数２３】
0004774522.tif 000024
となる。したがって、定理２．１より、ｎ→∞のとき、ｑｏｂ，ｎ→ｑｏ＋Ｔ（θｏ）ｑｌ，ｎ−Ｔ（θｏ＋Δθｌ）（ｑｌ，ｎ＋Δｑｌ）となる。【００９９】  そして、補題２．３より、初期状態オブザーバを用いた大域情報の推定値に関して、つぎの定理２．２が成り立つ。【０１００】  （定理２．２）図２に示す移動体、図３に示す局所座標系ΣＬ・大域座標系ΣＧ，および初期状態オブザーバ（式（５）および（８））について考える。時刻ｋにおいて位置にΔｑｌ，姿勢角にΔθｌの外乱が生じたとする。つぎに示す〜ｑｇ，ｎ，〜θｇ，ｎを与える。【０１０１】
【数２４】
0004774522.tif 000025
ここで、〜ｑｇ，ｎ，および〜θｇ，ｎのそれぞれは、大域情報ｑｇ，θｇの推定値である。このとき、ｎ→∞ならば（〜ｑｇ，ｎ，〜θｇ，ｎ）→（ｑｇ，ｎ，θｇ，ｎ）となる。【０１０２】  （証明）補題２．３より、ｎ→∞のとき、（ｑｏｂ，ｎ，θｏ）→（ｑｏ＋Ｔ（θｏ）ｑｌ，ｎ−Ｔ（θｏ＋Δθｌ）（ｑｌ，ｎ＋Δｑｌ），θｏ＋Δθｌ）となる。これを座標変換のために式（１３）に代入すると、【０１０３】
【数２５】
0004774522.tif 000026
となる。【０１０４】  定理２．２は、初期状態オブザーバを使用することで局所情報に生じた誤差の影響を打ち消すことができることを示している。つまり、初期状態オブザーバは、ローパスフィルタとして機能し、高周波ノイズを除去することができる。【０１０５】  さらに換言すれば、本実施形態の状態推定装置１においては、相対位置測定部２から取得されるｑｌ，ｎ、または相対角度測定部４から取得されるθｌ，ｎに誤差が生じても、式（５）、式（８）、および式（１３）を用いることにより、移動体の現在位置（ｑｇ，ｎ，θｇ，ｎ）を推定できる。【０１０６】  〔８．初期状態オブザーバの更新タイミングについて〕
式（５）および式（８）で示される初期状態オブザーバの更新、すなわち式（５）または式（８）を用いて初期状態における移動体の絶対位置や、絶対姿勢角を算出するためには、相対位置、相対姿勢角、絶対位置、および絶対姿勢角について、同時刻のものが必要である。【０１０７】  しかしながら、図４に示すように、外界センサの測定タイミングに対して、内界センサの測定タイミングは、その周期が短い。したがって、初期状態オブザーバを用いて初期状態を推定するタイミングを外界センサの測定タイミングと一致させると、そのタイミングにおける相対位置および相対姿勢角が得られない場合がある。【０１０８】  そこで、初期状態オブザーバを用いて移動体の初期状態を推定するタイミングにおいて、内界センサから得られる相対位置および相対姿勢角を補完することが好ましい。以下、その補完の手順について説明する。なお、相対姿勢角の補完は、相対角度測定部４により行われ、相対位置の補完は、相対位置測定部２により行われる。【０１０９】  〔８−１．前提条件〕
測定条件を以下のように考える。【０１１０】  まず、ｎサンプル時の絶対位置・姿勢の測定値を、以下のように記載する。【０１１１】
【数２６】
0004774522.tif 000027
なお、これらの絶対位置・姿勢の測定値は、単に「ｘｇ，ｎ」「θｇ，ｎ」と記載する場合もある。【０１１２】  また、ｎサンプル時の絶対位置・姿勢を取得した時刻を、ｔｇ，ｎとして記載する。【０１１３】  さらに、ｍサンプル時の絶対位置・姿勢の測定値を、以下のように記載する。【０１１４】
【数２７】
0004774522.tif 000028
なお、これらの絶対位置・姿勢の測定値は、単に「ｘｌ，ｍ」「θｌ，ｍ」と記載する場合もある。また、ｍサンプル時の絶対位置・姿勢を取得した時刻を、ｔｌ，ｍとして記載する。【０１１５】  ここで、ｎサンプル時の絶対位置・姿勢が測定された瞬間を考える。相対位置・姿勢はｍサンプルまで取得されているとする。このとき、以下の２つの状態を記載できる。【０１１６】  状態１．ｔｇ，ｎ≦ｔｌ，ｍ
状態２．ｔｇ，ｎ＞ｔｌ，ｍ
状態１．の場合、すでに得られている相対位置・姿勢より前の時間の絶対位置を取得していることになり、状態２．の場合にはもっとも新しい相対位置・姿勢を取得した時間より新しい情報がえられたことになるため、この２つの状態を別々に扱うことで、精度を高めることができる。なお、状態１．の場合、
ｔｌ，ｍ−ｋ−１＜ｔｇ，ｎ≦ｔｌ，ｍ−ｋ
を満たす整数ｋ≧０が存在する。【０１１７】  〔８−２．ｔｇ，ｎ≦ｔｌ，ｍの場合の補完〕
この場合、相対位置・姿勢を簡単に補完することができる。補完方法は様々な方法が考えられるが、本実施形態では、（１）オドメトリ、（２）拡張オドメトリ、および（３）線形補完のそれぞれを用いる補完方法について説明する。【０１１８】  〔８−２−１．オドメトリを用いた補完〕
オドメトリを用いる場合、ｍ−ｋ−１サンプル時の姿勢角速度ωｌ，ｍ−ｋ−１，および並進速度ｖｌ，ｍ−ｋ−１が既知であるとする。【０１１９】  このとき，ｔｇ，ｎにおける相対的な位置（相対位置）・姿勢ｘｌ，ｎ，ｙｌ，ｎ，θｌ，ｎは、以下のように書ける。【０１２０】
【数２８】
0004774522.tif 000029
〔８−２−２．拡張オドメトリを用いた補完〕
拡張オドメトリを用いる場合、特別な情報は必要としない。この場合、（１）θｌ，ｍ−ｋ−１＝θｌ，ｍ−ｋの場合、および（２）θｌ，ｍ−ｋ−１≠θｌ，ｍ−ｋの場合に分けて補完する。【０１２１】  先ず、θｌ，ｍ−ｋ−１＝θｌ，ｍ−ｋの場合は、以下のように補完する。【０１２２】
【数２９】
0004774522.tif 000030
また、θｌ，ｍ−ｋ−１≠θｌ，ｍ−ｋの場合は、以下のようにまず角度を補完する。【０１２３】
【数３０】
0004774522.tif 000031
さらに、位置の補完のために、次のようなｒａ，ｎ，ｒｂ，ｎ，ｒｎを考える。【０１２４】
【数３１】
0004774522.tif 000032
このとき、ｘｌ，ｎ，ｙｌ，ｎは次のようになる。【０１２５】
【数３２】
0004774522.tif 000033
〔８−２−３．線形補完〕
ｘｌ，ｎ，ｙｌ，ｎ，θｌ，ｎの線形補完は、下式に基づき行われる。【０１２６】
【数３３】
0004774522.tif 000034
〔８−３．ｔｇ，ｎ＞ｔｌ，ｍの場合の補完〕
この場合、前節と若干異なり、（１）オドメトリを用いる補完、（２）拡張オドメトリを用いる補完、（３）速度・角速度が得られない場合の補完、および（４）線形補完について説明する。【０１２７】  〔８−３−１．オドメトリを用いた補完〕
オドメトリを用いる場合、ｍサンプル時の姿勢角速度ωｌ，ｍ，および並進速度ｖｌ，ｍが取得可能であるとする。【０１２８】  このとき、ｔｇ，ｎにおける相対位置・姿勢ｘｌ，ｎ，ｙｌ，ｎ，θｌ，ｎは、以下のように書ける。【０１２９】
【数３４】
0004774522.tif 000035
〔８−３−２．拡張オドメトリを用いた補完〕
拡張オドメトリを用いる場合も、ｍサンプル時の姿勢角速度ωｌ，ｍ，および並進速度ｖｌ，ｍが取得可能であるとする。【０１３０】  拡張オドメトリを用いる場合には、角度を以下のように補完する。【０１３１】
【数３５】
0004774522.tif 000036
さらに、以下のような変数δｌ，ｎを考える。【０１３２】  δｌ，ｎ＝ωｌ，ｍ（ｔｇ，ｎ−ｔｌ，ｍ）
このとき、ｘｌ，ｎおよびｙｌ，ｎは、以下のようにして求める。【０１３３】
【数３６】
0004774522.tif 000037
〔８−３−３．速度・角速度が得られない場合〕
この場合、（１）θｌ，ｍ−１＝θｌ，ｍの場合と、（２）θｌ，ｍ−１≠θｌ，ｍの場合とに分けて説明する。【０１３４】  まず、θｌ，ｍ−１＝θｌ，ｍの場合は、下式に基づき、ｘｌ，ｎ，ｙｌ，ｎ，θｌ，ｎを補完する。【０１３５】
【数３７】
0004774522.tif 000038
また、θｌ，ｍ−１≠θｌ，ｍの場合は、以下のように、まず角度を補完する。【０１３６】
【数３８】
0004774522.tif 000039
さらに、位置を補完するために、以下に示すｒａ，ｎ，ｒｂ，ｎ，ｒｎを考える。【０１３７】
【数３９】
0004774522.tif 000040
このとき、ｘｌ，ｎ，ｙｌ，ｎは次のようになる。【０１３８】
【数４０】
0004774522.tif 000041
〔８−３−４．線形補完〕
線形補完は、下式に基づき行われる。【０１３９】
【数４１】
0004774522.tif 000042
〔９．絶対角度測定部がない場合〕
本実施形態の状態推定装置１においては、絶対角度測定部５は必須の構成ではない。すなわち、状態推定装置１において絶対角度測定部５を省略しても、後述するように、移動体の初期状態を推定することは可能である。【０１４０】  先ず、図５に示すように、移動体が大域座標内を（ｘｇ，ｎ−ｋ，ｙｇ，ｎ−ｋ）から（ｘｇ，ｎ，ｙｇ，ｎ）まで移動し、局所座標系内を（ｘｌ，ｎ−ｋ，ｙｌ，ｎ−ｋ）から（ｘｌ，ｎ，ｙｌ，ｎ）まで移動したとする。この場合、移動後の状態における移動体の相対姿勢角θｌ，ｎおよび絶対姿勢角θｇ，ｎは、下式のように表現される。【０１４１】  θｌ，ｎ＝ａｒｇ（（ｘｌ，ｎ−ｘｌ，ｎ−ｋ）＋ｉ（ｙｌ，ｎ−ｙｌ，ｎ−ｋ））
θｇ，ｎ＝ａｒｇ（（ｘｇ，ｎ−ｘｇ，ｎ−ｋ）＋ｉ（ｙｇ，ｎ−ｙｇ，ｎ−ｋ））
さらに、上述の式（５）で表現される初期角度オブザーバを、下式のように記述することで、初期状態における移動体のθｏを演算することができる。【０１４２】  θｏｂ，ｎ＋１＝θｏｂ，ｎ−ｋθ（θｇ，ｎ−θｌ，ｎ）
ここで、θｏを演算するにあたり、必要なパラメータは（ｘｇ，ｎ−ｋ，ｙｇ，ｎ−ｋ）および（ｘｇ，ｎ，ｙｇ，ｎ）である。これらのパラメータは、相対位置測定部２および絶対位置測定部３（図１参照）から得られるものである。そして、相対角度測定部４および絶対角度測定部５から得られるパラメータは、θｏを演算するために用いられていない。【０１４３】  よって、初期状態における移動体の姿勢角を推定するためには、相対角度測定部４および絶対角度測定部５は省略可能であり、上式の（ｘｇ，ｎ−ｋ，ｙｇ，ｎ−ｋ）および（ｘｇ，ｎ，ｙｇ，ｎ）を用いた演算を初期角度推定部６にて実行すればよいといえる。ただし、現在状態における移動体の絶対姿勢角を推定するのであれば、式（１）におけるθｌが必要となるので、相対角度測定部４が必要となる。【０１４４】  なお、移動体が移動しない場合、すなわち（ｘｌ，ｎ−ｘｌ，ｎ−ｋ，ｙｌ，ｎ−ｙｌ，ｎ−ｋ）＝（０，０）かつ（ｘｇ，ｎ−ｘｇ，ｎ−ｋ，ｙｇ，ｎ−ｙｇ，ｎ−ｋ）＝（０，０）の場合には、上述のようにθｌ，ｎおよびθｇ，ｎを求めることができないが、現在位置は推定可能である。また、以下のように初期位置推定部と相対位置測定部を更新することで推定精度が向上する。【０１４５】  ｘｏｂ，ｎ＋１＝Ｔ（θｏｂ，ｎ）ｘｌ＋ｘｏｂ，ｎ      ｘｌ＝０
〔１０．検証実験〕
前述した状態推定装置１の有効性の確認を行うため、二輪車両型ロボットを用いた制御実験を行った。本実験では、デッドレコニング的な手法として改良型オドメトリを用い、天測航法的な手法としてＣＣＤカメラを用いた位置計測システムを用いる。【０１４６】  〔１０−１．実験装置〕
本実験で使用したシステムの構成を図６に示す。図６に示すように、本実験で用いたシステムにおいては、移動体として二輪車両型ロボットを用い、そのロボットの移動をＰＣで制御しつつ、ビジョンシステム（イメージプロセッサおよびＣＣＤカメラから構成される）にて測定した。【０１４７】  また、本実験で使用した二輪車両型ロボットの構成を図７（ａ）および図７（ｂ）に示す。図７（ａ）および図７（ｂ）に示すように、二輪車両型ロボットは、両輪にそれぞれＤＣモータとロータリーエンコーダが取り付けられており、各車輪の角度がそれぞれ独立して計測できる。また、二輪車両型ロボットはデスクトップ型のＰＣで制御されており、本実験でのサンプリングタイムは約９［ｍｓｅｃ］である。【０１４８】  本実験で使用するビジョンシステムのサンプリングタイムは約１７［ｍｓｅｃ］である。また、ビジョンシステムではロボットの姿勢角を直接計測できないため、図８に示すように２点の位置を計測し、下式を用いて姿勢角を演算する。【０１４９】
【数４２】
0004774522.tif 000043
なお、ビジョンシステムの原点における計測値の誤差平均および誤差分散を、表１に示す。【０１５０】
【表１】
0004774522.tif 000044
〔１０−２．実験手法〕
初期状態（ｘ［ｃｍ］，ｙ［ｃｍ］，θ［ｒａｄ］）＝（５．００，−１０．００，０．００）から、目標状態（ｘｒ［ｃｍ］，ｙｒ［ｃｍ］，θｒ［ｒａｄ］）＝（０．００，０．００，０．００）へと、二輪車両型ロボットの移動を制御した。さらに、その制御則として、同次有限時間整定制御則を用い、１８０秒間の制御を行った。【０１５１】  なお、二輪車両型ロボットの初期状態をビジョンシステムで計測した値を、初期状態オブザーバの初期値として用い、オブザーバゲイン（ｋｘ，ｋｙ，ｋθ）は（０．０００７，０．０００７，０．０００７）とする。さらに、二輪車両型ロボットが最終的に移動した地点（最終地点）に対し、本実施形態の状態推定装置で推定した推定値、およびノギスと分度器を用いて計測した実測値を記録する。【０１５２】  なお、本実験では初期状態オブザーバの更新は、ビジョンシステムのサンプリングタイムと同期して行っている。理論上は推定値の更新には同時刻の局所情報と大域情報が必要であるため、今回のように天測航法的な手法とデッドレコニング的な手法のサンプリングタイムが違う場合、オブザーバの推定値に影響を与える。本実験では、簡略化のため、サンプリングタイムの違いによる影響は十分小さいと仮定し、特別の補完はしていない。【０１５３】  〔１０−３．実験結果〕
大域情報の推定値を図９に、初期状態の推定値を図１０〜図１２に示す。また、最終地点における大域情報の推定値および実測値を表２に、実測値と推定値の誤差平均および誤差分散を表３に示す。【０１５４】
【表２】
0004774522.tif 000045
【０１５５】
【表３】
0004774522.tif 000046
図１０より、ｘｏｂ，ｎは制御開始から約８０秒後に５．１２［ｃｍ］に収束し、約１２０秒後から別の値に収束し始めていることが確認できる。また、図９を参照すると、約１２０秒後においてロボットが再び動いており、移動した際に横滑りなどの外乱が生じたと推測できる。同じことがｙｏｂ，ｎおよびθｏｂ，ｎについてもいえる。【０１５６】  〔１１．補足〕
最後に、状態推定装置１の各ブロック、特に初期角度推定部６、現在角度推定部７、初期位置推定部８、および現在位置推定部９は、ハードウェアロジックによって構成してもよいし、次のようにＣＰＵを用いてソフトウェアによって実現してもよい。【０１５７】  すなわち、状態推定装置１は、各機能を実現する制御プログラムの命令を実行するＣＰＵ（ｃｅｎｔｒａｌ  ｐｒｏｃｅｓｓｉｎｇ  ｕｎｉｔ）、上記プログラムを格納したＲＯＭ（ｒｅａｄ  ｏｎｌｙ  ｍｅｍｏｒｙ）、上記プログラムを展開するＲＡＭ（ｒａｎｄｏｍ  ａｃｃｅｓｓ  ｍｅｍｏｒｙ）、上記プログラムおよび各種データを格納するメモリ等の記憶装置（記録媒体）などを備えている。そして、本発明の目的は、上述した機能を実現するソフトウェアである状態推定装置１の制御プログラムのプログラムコード（実行形式プログラム、中間コードプログラム、ソースプログラム）をコンピュータで読み取り可能に記録した記録媒体を、上記状態推定装置１に供給し、そのコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に記録されているプログラムコードを読み出し実行することによっても、達成可能である。【０１５８】  上記記録媒体としては、例えば、磁気テープやカセットテープ等のテープ系、フレキシブルディスク／ハードディスク等の磁気ディスクやＣＤ−ＲＯＭ／ＭＯ／ＭＤ／ＤＶＤ／ＣＤ−Ｒ等の光ディスクを含むディスク系、ＩＣカード（メモリカードを含む）／光カード等のカード系、あるいはマスクＲＯＭ／ＥＰＲＯＭ／ＥＥＰＲＯＭ／フラッシュＲＯＭ等の半導体メモリ系などを用いることができる。【０１５９】  また、状態推定装置１を通信ネットワークと接続可能に構成し、上記プログラムコードを通信ネットワークを介して供給してもよい。この通信ネットワークとしては、特に限定されず、例えば、インターネット、イントラネット、エキストラネット、ＬＡＮ、ＩＳＤＮ、ＶＡＮ、ＣＡＴＶ通信網、仮想専用網（ｖｉｒｔｕａｌ  ｐｒｉｖａｔｅ  ｎｅｔｗｏｒｋ）、電話回線網、移動体通信網、衛星通信網等が利用可能である。また、通信ネットワークを構成する伝送媒体としては、特に限定されず、例えば、ＩＥＥＥ１３９４、ＵＳＢ、電力線搬送、ケーブルＴＶ回線、電話線、ＡＤＳＬ回線等の有線でも、ＩｒＤＡやリモコンのような赤外線、Ｂｌｕｅｔｏｏｔｈ（登録商標）、８０２．１１無線、ＨＤＲ、携帯電話網、衛星回線、地上波デジタル網等の無線でも利用可能である。なお、本発明は、上記プログラムコードが電子的な伝送で具現化された、搬送波に埋め込まれたコンピュータデータ信号の形態でも実現され得る。【０１６０】  本発明は、相対姿勢角や相対位置に生じた誤差に影響を受けず、移動体の初期位置や初期姿勢角が未知の場合でも、移動体の現在位置および現在姿勢角を推定できるという特徴をもつ。また、Ｋａｌｍａｎフィルタを利用する手法と違い，計測値の誤差分散行列が不要であるため、誤差分散を求めることが難しい場合でも問題なく利用できる。【０１６１】  さらに、本発明の状態推定装置は、上記相対角度測定手段が、上記絶対角度測定手段が絶対角度を測定するタイミングにおいて既に測定された相対姿勢角を補完し、その補完された相対姿勢角を、上記初期角���推定手段に出力することが好ましい。【０１６２】  すなわち、初期角度推定手段が初期姿勢角を推定するタイミングにおいては、移動体の相対姿勢角および絶対姿勢角に関し、同時刻の値が揃っていることが好ましい。しかしながら、一般的に、絶対角度測定手段が絶対姿勢角を測定する周期は、相対角度測定手段が相対姿勢角を測定する周期に比べると長いので、初期姿勢角が推定されるタイミングにおいて、相対姿勢角が得られない場合もある。【０１６３】  そこで、上記構成においては、相対角度測定手段が、補完された相対姿勢角を、初期角度推定手段に出力する。よって、初期姿勢角が推定されるタイミングにおける相対姿勢角が、相対角度測定手段により測定されていない場合でも、補完された相対姿勢角を、相対姿勢角の測定値に替えて用いることで、初期姿勢角を推定することができる。【０１６４】  さらに、本発明の状態推定装置は、上記相対位置測定手段が、上記絶対位置測定手段が絶対位置を測定するタイミングにおいて既に測定された相対位置を補完し、その補完された相対位置を、上記初期位置推定手段に出力することが好ましい。【０１６５】  すなわち、初期位置推定手段が初期位置を推定するタイミングにおいては、移動体の相対位置および絶対位置に関し、同時刻の値が揃っていることが好ましい。しかしながら、一般的に、絶対位置測定手段が絶対位置を測定する周期は、相対位置測定手段が相対位置を測定する周期に比べると長いので、初期位置が推定されるタイミングにおいて、相対位置が得られない場合もある。【０１６６】  そこで、上記構成においては、相対位置測定手段が、補完された相対位置を、初期位置推定手段に出力する。よって、初期位置が推定されるタイミングにおける相対位置が、相対位置測定手段により測定されていない場合でも、補完された相対位置を、相対位置の測定値に替えて用いることで、初期位置を推定することができる。【０１６７】  なお、上記構成の状態推定方法における一部のステップをしてコンピュータに実行させる状態推定プログラムにより、コンピュータを用いて本発明の状態推定装置と同様の作用効果を得ることができる。さらに、上記状態推定プログラムをコンピュータ読み取り可能な記録媒体に記憶させることにより、任意のコンピュータ上で上記状態推定プログラムを実行させることができる。
【産業上の利用可能性】
【０１６８】  本発明によれば、たとえば自動車や船舶である移動体の現在位置および現在姿勢角を、初期位置や初期姿勢角が未知である場合にも推定できる。
【図１】
0004774522.tif 000047
【図２】
0004774522.tif 000048
【図３】
0004774522.tif 000049
【図４】
0004774522.tif 000050
【図５】
0004774522.tif 000051
【図６】
0004774522.tif 000052
【図７（ａ）】
0004774522.tif 000053
【図７（ｂ）】
0004774522.tif 000054
【図８】
0004774522.tif 000055
【図９】
0004774522.tif 000056
【図１０】
0004774522.tif 000057
【図１１】
0004774522.tif 000058
【図１２】
0004774522.tif 000059