WO2006126535A1

WO2006126535A1 - 状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体

Info

Publication number: WO2006126535A1
Application number: PCT/JP2006/310241
Authority: WO
Inventors: Hisakazu Nakamura; Muneaki Higuchi; Hirokazu Nishitani
Original assignee: National University Corporation NARA Institute of Science and Technology
Priority date: 2005-05-23
Filing date: 2006-05-23
Publication date: 2006-11-30
Also published as: JPWO2006126535A1; DE112006001314T5; JP4774522B2

Abstract

　　誤差分散行列を必要とせず、初期状態が未知の場合でも使用できる状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体を提供する。相対角度測定部（４）から得られる相対姿勢角、と絶対角度測定部（５）から得られる絶対姿勢角とに基づき、移動体の姿勢角（初期姿勢角）を推定する初期角度推定部（６）と、上記相対姿勢角と、上記初期姿勢角とに基づき、移動体の姿勢角（現在姿勢角）を推定する現在角度推定部（７）と、上記初期姿勢角、相対位置測定部（２）から得られる移動体の相対位置、および絶対位置測定部（３）から得られる絶対位置に基づき、移動体の位置（初期位置）を推定する初期位置推定部（８）と、移動体の現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定部（９）と、を備えている。これにより、移動体の現在位置および現在姿勢角を推定することができる。

Description

明細書

状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体

技術分野

[0001] 本発明は、自動車、船舶等の存在する位置を推定する状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体に関する。背景技術

[0002] 船舶や自動車、航空機など、現在の社会においては、さまざまな移動体が用いられている。移動体については、カーナビや船舶の状態監視など、移動体の状態を計測することが必要になる状況も非常に多い。

[0003] 移動体の状態計測手法は、大別すると、デッドレコニング的な手法と、天測航法的な手法との 2つに分類される。天測航法的な手法は、 GPSやカメラなどの外界センサを用いて大域情報を直接計測する手法であり、デッドレコユング的な手法は、ロータリ一エンコーダ、ジャイロセンサ、加速度センサなどの内界センサを用いて、微小変化を連続して計測し、積分を行うことによって、初期状態からの変化量を計測する手法である。

[0004] 天測航法的な手法は、累積誤差が存在せず、大域的な状態を直接計測できるが、計測値の分散が大きぐサンプリングタイムが長いといった欠点がある。

[0005] 一方、デッドレコユング的な手法は、サンプリングタイムが短く、計測値の分散が小さい。しかし、デッドレコユング的な手法には、大域情報を得るためには初期情報が必要であり、車輪の横滑りなど内界センサで計測できないような外乱に非常に弱ぐ計測誤差が累積するといつた欠点がある。

[0006] このような欠点を克服するために、上述の 2つの手法を組み合わせて用いるセンサフュージョンに関する研究が盛んに行われており、代表的な手法として Kalmanフィルタを用いた手法が挙げられる。 Kalmanフィルタを用いる手法は、デッドレコユング的な手法および天測航法的な手法による計測値の信頼性を評価する誤差分散行列を利用し、最尤推定法を用いて移動体の状態を推定する。 [0007] し力しながら、デッドレコユング的な手法は、移動体の移動に応じて誤差が累積するので、誤差分散行列を求めることが困難である。また、 Kalmanフィルタを用いる手法は、最尤推定法を用いて移動体の状態を推定するので、デッドレコニング的な手法と天測航法的な手法との間で誤差分散行列の差が大き、場合には、ほとんど効果がない。さらに、初期状態が未知の場合、デッドレコニング的な手法は大域情報を計測できな、ため、 Kalmanフィルタによる推定手法は用いることができな!/、。

発明の開示

[0008] 本発明は、上記従来の課題に鑑みなされたものであって、誤差分散行列を必要とせず、初期状態が未知の場合でも使用できる状態推定装置、状態推定方法、状態推定プログラム、およびコンピュータ読取可能な記録媒体を提供することを目的とする。

[0009] 本発明の状態推定装置は、上記課題を解決するために、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等、ものとして、初期姿勢角を推定する初期角度推定手段と、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えて、ることを特徴として！/、る。

[0010] 上記構成によれば、誤差分散行列を用いずに、たとえば後述する式（1)を用いて、移動体の現在姿勢角を、現在角度測定手段により求め、さらに移動体の現在位置を現在位置推定手段により求めることができる。よって、本発明によれば、従来のデッドレコニング的な手法とは異なり、相対位置測定手段および相対角度測定手段として実現される内界センサにおける測定誤差の累積に関わらず、移動体の現在位置および現在姿勢角を推定することができる。その上、本発明によれば、 Kalmanフィルタを用いる方法とは異なり、デッドレコニング的な手法と天測航法的な手法との間で誤差分散行列の差が大きくなつても、移動体の現在位置および現在姿勢角を推定することがでさる。

[0011] さらに、本発明においては、初期角度推定手段により移動体の初期姿勢角が推定され、さらに初期位置推定手段により移動体の初期位置が推定される。

[0012] すなわち、初期角度推定手段においては、上記相対姿勢角および上記絶対姿勢角を含む漸ィ匕式 (たとえば、後述する式 (5) )で表現される数列の収束値（ 0 )力初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する。また、初期位置推定手段においては、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式 (たとえば、後述する式 (8) )で表現される数列の収束値 (q )が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等、ものとして、初期位置を推定する。

[0013] よって、本発明によれば、従来の Kalmanフィルタを用いる方法とは異なり、初期状態が未知の場合であっても、移動体の現在位置および現在姿勢角を推定することができる。

[0014] また、本発明の状態推定装置は、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する絶対角度測定手段と、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する初期角度推定手段と、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えて!/ヽることを特徴として、る。

[0015] 上記構成によれば、絶対角度測定手段を備えているので、上記作用効果に加えて、移動体の絶対姿勢角を精度良く求めることができる。したがって、本発明において絶対姿勢角に基づき推定される値、すなわち初期姿勢角および初期位置の推定精度が高められ、さらに現在位置および現在姿勢角の推定精度も高められる。

[0016] また、本発明の状態推定方法は、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第 1ステップと、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第 2ステップと、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第 3ステップと、上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する第 4ステップと、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第 5ステップと、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第 6ステップと、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第 7ステップと、を備えて、ることを特徴として、る。

[0017] 上記構成の状態推定方法によれば、第 1ステップ力第 7ステップまでにより、本発明の状態推定装置と同様の作用が実現されているので、本発明の状態推定装置と同様の作用効果を得ることができる。

[0018] また、本発明の状態推定方法は、移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第 1ステップと、移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第 2ステップと、上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第 3ステップと、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する第 4ステップと、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等、ものとして、初期姿勢角を推定する第 5ステップと、現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第 6ステップと、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等、ものとして、初期位置を推定する第 7ステップと、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第 8ステップと、を備えている構成であってもよい。

[0019] 上記構成の状態推定方法によれば、第 1ステップ力第 8ステップまでにより、本発明の状態推定装置と同様の作用が実現されているので、本発明の状態推定装置と同様の作用効果を得ることができる。図面の簡単な説明

[0020] [図 1]本発明の一実施形態に係る状態推定装置の構成を示すブロック図である。

[図 2]移動体の状態を表現する変数を説明するための図である。

[図 3]局所座標系および大域座標系を説明するための図である。

[図 4]局所情報を補完するタイミングを説明するための図である。

[図 5]移動体が大域座標内を (X , y )から (X , y )まで移動し、局所座標系内

g，n_k g,n-k g,n g,n

を (χ , y ) (χ , y )

l,n— k ι,η— kから Ι,η Ι,ηまで移動した状態を示す図である。

[図 6]本発明の効果確認実験で使用したシステムの構成を示す図である。

[図 7(a)]図 6のシステムで用いた二輪車両型ロボットの斜視図である。

[図 7(b)]図 6のシステムで用いた二輪車両型ロボットの底面図である。

[図 8]図 6のシステムにおけるビジョンシステム力二輪車両型ロボットについて計測した 2点の位置と、姿勢角との関係を示す図である。

[図 9]本発明の効果確認実験で得られる値 (x,y, Θ )のそれぞれに関する推定値を示すグラフである。

[図 10]本発明の効果確認実験で得られる値 (X)に関し、初期状態の推定値を示すグラフである。

[図 11]本発明の効果確認実験で得られる値 (y)に関し、初期状態の推定値を示すグラフである。

[図 12]本発明の効果確認実験で得られる値（ Θ )に関し、初期状態の推定値を示すグラフである。

符号の説明

[0021] 1 状態推定装置

2 相対位置測定部 (相対位置測定手段）

3 絶対位置測定部 (絶対位置測定手段）

4 相対角度測定部 (相対角度測定手段）

5 絶対角度測定部 (絶対角度測定手段）

6 初期角度推定部 (初期角度推定手段）

7 現在角度推定部 (現在角度推定手段） 8 初期位置推定部 (初期位置推定手段）

9 現在位置推定部 (現在位置推定手段）

発明を実施するための最良の形態

[0022] 〔 1.状態推定装置の構成〕

本発明の一実施形態に係る状態推定装置の構成について、図 1を用いて説明する。図 1に示すように、状態推定装置 1は、相対位置測定部 2と、絶対位置測定部 3と、相対角度測定部 4と、絶対角度測定部 5と、初期角度推定部 6と、現在角度推定部 7 と、初期位置推定部 8と、現在位置推定部 9とを備えている。

[0023] 相対位置測定部 2および相対角度測定部 4は、いわゆる内界センサにより実現される。ここで、内界センサとは、移動体の相対位置および相対的な姿勢角（相対姿勢角 )を測定するセンサである。すなわち、相対位置測定部 2は、内界センサが相対位置を測定する機能を表現したものであり、相対角度測定部 4は、内界センサが相対姿勢角を測定する機能を表現したものである。

[0024] 内界センサは、たとえば、ォドメトリを用いるセンサ、加速度センサ、またはジャイロセンサにより実現可能であり、一般的に、（1)測定誤差が小さい、（2)サンプリングレートが高い、（3)初期値が未知の場合には使えない、（4)測定誤差が累積するといつた 4つの特性を有して、る。

[0025] また、絶対位置測定部 3および絶対角度測定部 5は、いわゆる外界センサにより実現される。ここで、外界センサとは、移動体の絶対的な位置 (絶対位置)または絶対的な姿勢角（絶対姿勢角）を測定するものである。

[0026] たとえば、外界センサの一例である GPSセンサは、移動体の絶対位置を測定できるので、絶対位置測定部 3として利用可能である。また、外界センサの一例である地磁気センサは、移動体の絶対姿勢角を測定できるので、絶対角度測定部 5として利用可能である。さら〖こ、ビジョンシステム、ランドマークセンサ、およびレンジファインダは、移動体の絶対位置および絶対姿勢角を測定できるので、絶対位置測定部 3および絶対角度測定部 5として利用可能である。

[0027] なお、たとえば緯度や経度のように、移動体の状態に関わらず移動体の位置を特定しえるパラメータが、「絶対位置」に相当する。また、たとえば地軸に対する角度のように、移動体の状態に関わらず移動体の姿勢角を特定しえるパラメータが、「絶対姿勢角」に相当する。

[0028] 初期角度推定部 6は、相対角度測定部 4から得られる移動体の相対姿勢角、および絶対角度測定部 5から得られる移動体の絶対姿勢角に基づき、移動体の初期状態における絶対姿勢角を推定するものである。初期角度推定部 6における演算処理に関しては後述する。

[0029] 現在角度推定部 7は、相対角度測定部 4から得られる移動体の相対姿勢角、および初期角度推定部 6により推定された移動体の初期状態における絶対姿勢角に基づき、移動体の現在の状態における絶対姿勢角を推定するものである。現在角度推定部 7における演算処理に関しては後述する。

[0030] 初期位置推定部 8は、相対位置測定部 2から得られる移動体の相対位置、および絶対位置測定部 3から得られる移動体の絶対位置に基づき、移動体の初期状態における絶対位置を推定するものである。初期位置推定部 8における演算処理に関しては後述する。

[0031] 現在位置推定部 9は、初期位置推定部 8により推定された移動体の初期状態における絶対位置、および相対位置測定部 2から得られる移動体の相対位置に基づき、移動体の現在の状態における絶対位置を推定するものである。現在位置推定部 9における演算処理に関しては後述する。

[0032] 以上の構成により、状態推定装置 1は、初期状態における移動体の絶対位置および絶対姿勢角を推定するとともに、その絶対位置および絶対姿勢角を用いて、現在の状態における移動体の絶対位置および絶対姿勢角を推定するものである。以下、状態推定装置 1を構成する各ブロックの演算処理の詳細を説明する。

[0033] 〔2.移動体の状態を表現する変数の説明および前提条件〕

ある座標系上を運動する移動体を一意に表現するためには、位置および姿勢角を表す 3つの状態変数 (x,y, Θ )が必要となる。ここで、図 2に示すように、位置 q = [x,y]^T は移動体の中心を表し，姿勢角 Θは移動体と座標軸との相対角度を示すものとする。今、ある与えられた大域座標系∑ において、現在の移動体の状態を示す (q , Θ )

G g g を、状態推定装置 1により推定することを考える。 [0034] さらに、状態推定装置 1により (q , Θ )を推定するための前提条件として、以下のこ g g

とを仮定する。

[0035] (1)初期状態における移動体の座標を示す qおよび 0 は未知である。

[0036] (2)局所情報 qおよび Θは、デッドレコユング的な手法で計測した初期状態力の

1 1

変化量とし、外乱以外の誤差は生じないとする。

[0037] (3)大域情報 qおよび Θ は、天測航法的な手法で計測した現在位置および姿勢 g g

角とし、計測値に平均 0のホワイトノイズが付加して、るとする。

[0038] このような前提条件では、初期状態の qおよび Θ が未知であるため、デッドレコニング的な手法では大域情報が計測できない。また，天測航法的な手法は、ホワイトノィズが付加しているため、計測値の分散が大きぐこの手法のみを用いることは好ましくない。さらに、初期状態の qおよび Θ が未知であるため、 Kalmanフィルタを用いることができない。

[0039] これらの問題は、本実施形態の状態推定装置 1により実現される初期状態ォブザーバを用いて大域情報を推定することで、解決される。以下、初期状態オブザーバについて具体的に説明する。

[0040] 〔3.初期状態オブザーバを用いた (q , Θ )の推定〕

g g

まず、図 3に示すように、局所座標∑ を定義する。大域座標系∑ での移動体の

L G

状態 (q , Θ )は、次に示す（1)式によって表現できる。

g g

[0041] [数 1]

q_g = T(0_o)q_l + q_o

( 1 )

θ, = θ_{ι +} θ₀ ここで、 q ( = [x ,y ]^T)および Θ はそれぞれ大域座標系∑ での位置と姿勢角を表 g g g g G

U q ( = k ,y]^T)

1 1 1 および Θ

1は、それぞれ局所座標系∑での

し位置および姿勢角を表すものである。また、 q ( = [x ,y ]^τ)および Θ は、それぞれ大域座標系∑ での初期状態における移動体の位置と姿勢角を表すものである。

[0042] 本明細書では、局所座標系∑ での移動体の位置，姿勢角を、局所情報といい、し

局所座標系∑ での移動体の位置および姿勢角のそれぞれが、「相対位置」、「相対し

姿勢角」に対応して、る。また、大域座標系∑ での位置，姿勢角を大域情報と、、大域座標系∑ での位置および姿勢角のそれぞれが、「絶対位置」、「絶対姿勢角

G

」に対応している。なお、 TO は、以下のように示される姿勢変換行列である。

[0043] [数 2]

もし、デッドレコユング的な手法で計測した局所情報 q , Θに誤差が生じなければ、

1 1

移動体の初期状態 q , Θ を推定し、座標変換式 (1)式を用いることで大域情報 (q , Θ )を推定することができる。

[0044] 〔4.初期位置オブザーバの導出〕

以下では、初期位置 (初期状態における位置) qが未知、初期姿勢角（初期状態における姿勢角） Θ が既知である条件のもとに、移動体の初期位置を推定するォブザーバを導出する。

[0045] まず、局所情報と大域情報力初期位置を推定するオブザーバを導出するためにつぎの補題を与える。

[0046] (補題 2.1)図 2に示す移動体、および図 3に示す局所座標系∑ と大域座標系∑

L G

について考える。移動体の初期位置 qが既知であるとし、下記の式（2)で示される離散時間オブザーバを与える。

[0047] [数 3]

ここで、

[0048] [数 4] 。

は、 qの推定値であり、単に「q 」と記載する場合もある。また、 K:= diag(k ,k )とする o ob,n x y

[0049] この式（2)で示されるオブザーバは、 0〈k〈2かつ 0〈k〈2を満たすならば、 n→∞のと

X y

き →q

ob,n 0となる。

[0050] (証明）式（2)の両辺から q。を引くと、以下のようになる。

[0051] [数 5]

— ¾ = —q₀— ^KL„ - q₀)

= (i- K)( _b,„- Qo)

したがって、 1〈1— k〈1かつ 1〈1— k〈1を満たすならば、 n→∞のとき、 q →q

x y ob,n

。となる。

[0052] 以上により、補題 2.1を証明できる。さらに、補題 2.1を用いると、初期位置ォブザーバに関するつぎの補題 2.2を得ることができる。

[0053] (補題 2.2)図 2に示す移動体、および図 3に示す局所座標系∑ と大域座標系∑

L G

について考える。移動体の初期位置 qが未知であり、初期姿勢角 Θ が既知であるとして、以下の式 (3)で示される離散時間オブザーバを与える。

[0054] 園

このオブザーバは、 0〈k <2かつ 0〈k〈2を満たすならば、 n→∞のとき q →qとなる

X y ob,n o

[0055] (証明）式（1)より、つぎの式 (4)を得る。

[0056] [数 7]

¾,„ ^{= 7,}(¾ ( 4 )

また、式（3)および式 (4)より、以下の方程式を導くことができる。

[0057] [数 8]

よって、補題 2.1より、 1〈 1— k〈1かつ一 1〈1— k〈1を満たすなら， n→∞のとき、 q

X y ob

→q

,η οとなる。

[0058] [5.初期姿勢角が未知の場合への拡張〕

初期位置 q。および初期姿勢角 Θ。が未知の場合について考える。まず、つぎに示す初期姿勢角オブザーバに関する命題を与える。

[0059] (命題 2.1)図 2に示す移動体、および図 3に示す局所座標系∑ と大域座標系∑ について考える。移動体の初期姿勢角 Θ。が未知であるとし、以下の式（5)で示される離散時間オブザーバを与える。

[0060] [数 9]

Θ ₊、 =θ。 - k_eW +Θ。 - θ .„) (5) ここで、

[0061] [数 10] θ_οΚη は、 Θ の推定値であり、単に「θ 」と記載する場合もある。

[0062] また、 Θ は局所座標系∑ における姿勢角であり、 Θ は大域座標系∑ における姿勢角である。

[0063] さらに、式（5)においては、 0 + Θ - Θ の値について、一 π< 0 + Θ - θ ≤ πの範囲に正規化されているとする。

[0064] このとき、 0<k く 2を満たすならば、 n→∞のとき、 θ →θ となる。

[0065] (命題 2.1の証明）

式（5)の両辺から 0 を引き、 0 = Θ + 0 を式（5)に代入すると、下式を得ることができる。

[0066] [数 11]

0 、。 - +ひ -

さらに、 0 - Θ =e を用いると、下式 (6)を得ることができる。

β

[0067] [数 12]

この式（6)に対して、

[0068] [数 13] (7) は Lyapunov関数となるから、 n→∞のとき e →0となるので、 n→∞のとき θ → Θ となる (命題 2.1の証明終わり）。

[0069] そして、命題 2.1を用いると、初期状態オブザーバに関して次の定理 2.1を得ることができる。

[0070] (定理 2.1)図 2に示す移動体、および図 3に示す局所座標系∑ と大域座標系∑

L G

について考える。移動体の初期位置 qおよび移動体の初期姿勢角 0 が未知であるとし、式（5)および以下の式 (8)で示される離散時間オブザーバを与える。

[0071] [数 14]

U ₀ -K(TH +q_ob,_n -¾,„) ( 8 ) ここで、

[0072] [数 15]

は、 qの

0 推定値であり、単に「q

οο,η」と記載する場合もある。また、 q

Ι,ηは局所座標系∑ での移動体の位置を示すものであり、 q は、大域座標系∑ での移動体の位置を

L g,n G

示すものであり、 K:=diag(k ,k )とする。

[0073] この式（8)で示されるオブザーバは、 q が有界で、 0<k〈2かつ 0〈k〈2であるならば

1，π

、 _n→∞のとき q →qとなる。

ob,n o

[0074] (証明）

式 (8)の両辺から、 qを引き、式 (8)に q =Τ( Θ )q +qを代入する。さらに ε =q

o g,n o l,n o n o

-qと置くと、以下の式（9)が得られる。

b,n 0

[0075] [数 16]

_£„₊^ (I -K)_£n -K[(T(0_oh„) -T(0_o)]q_{l n} ( 9 ) さらに、式（10)に示す zを与える。

[0076] [数 17]

zの性質として、 q が有界であるとの仮定および命題 2.1より、 n→∞のとき z→0となる。この zを用いると、式（9)は下式（11)に変形できる。

[0077] [数 18] ε„₊ = (Ι - Κ)ε„ + ζ と変形できる。ここで、 ζおよび状態 qを入力とみなすと、式（11)は離散時間 ISSGnpu t- to -state stable)となり、式（11)は漸近安定となる。

[0078] ゆえに， q が有界で、 0〈k〈2かつ 0〈k〈2であるならば、 n→0のとき、（q , Θ )→(q

Ι,η χ y ob,n ob,n o

, Θ。；)となる。

[0079] [6.装置構成とオブザーバとの関係〕

本実施形態における状態推定装置 1 (図 1参照）は、上述した初期状態オブザーバ式 (5)および式 (8)を用いて、移動体の初期状態を推定し、さらに式（1)を用いることによって、移動体の現在の状態における絶対位置および絶対姿勢角を推定するものである。以下、その詳細について説明する。

[0080] まず、状態推定装置 1における初期角度推定部 6の処理について説明する。初期角度推定部 6は、上述した初期状態オブザーバを示す式 (5)を用いて、移動体の初期状態における絶対姿勢角を演算する。

[0081] すなわち、相対角度測定部 4により、上述した初期状態オブザーバを示す式 (5)における Θ が取得される。さらに、絶対角度測定部 5により、式（5)における Θ が取

l，n g,n 得される。したがって、初期角度推定部 6は、式（5)に Θ および Θ を代入するとと

l，n g,n

もに、 Θ として、絶対角度測定部 5から得られる絶対姿勢角を推定値として用いる ob,0

ことで、 Θ の収束値である 0 を推定することができる。

ob,n 0

[0082] 次に、現在角度推定部 7における処理について説明する。現在角度推定部 7は、上述した式（1)を用いて、移動体の現在の状態における絶対姿勢角を推定する。すなわち、現在角度推定部 7は、式（1)における Θとして、相対角度測定部 4から得ら

1

れる Θ を代入し、さらに、式（1)における Θ として、初期角度推定部 6により推定さ

Ι,η 0

れたものを代入する。これにより、現在角度推定部 7は、移動体の現在の状態における絶対姿勢角を示す Θ を演算することができる。

g

[0083] 次に、初期位置推定部 8における処理について説明する、初期位置推定部 8は、上述した初期状態オブザーバを示す式 (8)を用いて、移動体の初期状態における絶対位置を演算する。

[0084] すなわち、初期角度推定部 6により、式 (8)における 0 の収束値である 0 が演算

ob,n 0 される。さらに、式 (8)における q は相対位置測定部 2から取得でき、 q は絶対位置

l，n g,n

測定部 3から取得できる。したがって、初期位置推定部 8は、式 (8)に q および q を

l，n g,n 代入するとともに、 q として、絶対位置測定部 3から得られる絶対位置を推定値とし

ob,0

て用いることで、 qを推定することができる。

[0085] 最後に、現在位置推定部 9における処理について説明する。現在位置推定部 9は、式（1)を用いて、移動体の現在の状態における絶対位置を推定する。すなわち、現在位置推定部 9は、式（1)における Θ として初期角度推定部 6により推定されたものを代入し、 qとして相対位置測定部 2から得られる q を代入し、 qとして初期位置推定

1 Ι,η Ο 部 8により推定されたものを代入する。これにより、現在位置推定部 9は、移動体の現在の状態における絶対位置を示す qを演算することができる。

g

[0086] このようにして、状態推定装置 1は、移動体の初期状態（ Θ , q )を推定し、さらに、移動体の現在の状態における絶対位置 (q )をおよび絶対姿勢角（ Θ )を推定するこ g g

とができる。さらに、式（5)および式 (8)は、ローノスフィルタとして機能し、高周波ノィズを除去することができる。

[0087] [7.計測誤差の影響〕

以下、計測値に生じる誤差の影響について説明する。今、時刻 kにおいて横滑りなどにより位置に A q、姿勢角に Δ Θの外乱が生じたとする。このとき、時刻 k以降の局

1 1

所情報の推定値は、つぎの式（12)で表される。

[0088] [数 19]

ただし、 n≥kである。

[0089] なお、式（12)における左辺の値が局所情報の推定値を示しており、右辺における [0090] [数 20] および

[0091] [数 21]

の値が、局所情報の計測値を示している。

[0092] この局所情報に生じた誤差が初期状態の推定値に与える影響を考えるためにつぎのネ ΐ題を与える。

[0093] (補題 2.3)式（5)および (8)で示した初期状態オブザーバにつ!/、て考える。時刻 k において位置に Aq、姿勢角に Δ Θの外乱が生じたとする。このとき、初期状態ォブ

1 1

ザーバは、 n→∞のとき (q ,θ )→(q +Τ( θ )q — Τ(θ + Δ θ )(q +Aq), θ +Δ

ob,n ob,n ο ο Ι,η ο 1 Ι,η 1 ο θ )となる。

1

[0094] (証明）まず、時刻 k以降の初期姿勢角オブザーバ式（5)について考える。式（12) より、

[0095] [数 22]

= ," -ん - Δ + θ_{ο η}一 (θ,„ + θ₀)

=^,„- [^,„ -( + となる。命題 2.1より、 η→∞のとき、 θ → Θ となる。

ob,n 1,η+ Δ θ 1

[0096] 続いて初期位置オブザーバについて考える。今、初期状態の姿勢角の推定値 Θ ob, が、 θ +Δ Θに収束したとする。

n l,n 1

[0097] このとき、初期位置オブザーバは、

[0098] [数 23] =υ、τφ。 _η、 + 。 ₄,„— ,„)

= ,,„― K[q_a,,„― (¾ + ,"一 ( , , ] となる。したがって、定理 2.1より、 n→∞のとき、 q →q +Τ( Θ )q ~Τ(Θ + Δ θ )(

ob,n ο ο Ι,η ο 1 q +Aq)となる。 [0099] そして、補題 2.3より、初期状態オブザーバを用いた大域情報の推定値に関して、つぎの定理 2.2が成り立つ。

[0100] (定理 2.2)図 2に示す移動体、図 3に示す局所座標系∑ ·大域座標系∑ ,および

L G

初期状態オブザーバ（式（5)および (8))について考える。時刻 kにおいて位置に Aq 1,姿勢角に Δ θ 1の外乱が生じたとする。つぎに示す〜 q ,〜 Θ を与える。

g,n g,n

[0101] [数 24]

ここで、〜q ,および〜 Θ のそれぞれは、大域情報 q , Θ の推定値である。このとき

g,n g,n g g

、n→∞ならば (〜q θ )→(q ,θ )となる。

[0102] (証明）補題 2.3より、 n→∞のとき、 (q , Θ )→(q +Τ(Θ )q — T( θ +Δ θ )(q,n +

ob,n ο ο ο Ι,η ο 1 1

Aq),0 +Δ θ )となる。これを座標変換のために式（13)に代入すると、

1 0 1

[0103] [数 25]

= Τ{θ_α + Αθ,)((3,„ +Aq_l) + q₀ + T(e_a)q_ln = ¾,,,

= Θ,„-ΑΘ_Ι+Θ₀+ΑΘ_Ι となる。

[0104] 定理 2.2は、初期状態オブザーバを使用することで局所情報に生じた誤差の影響を打ち消すことができることを示している。つまり、初期状態オブザーバは、ローパスフィルタとして機能し、高周波ノイズを除去することができる。

[0105] さらに換言すれば、本実施形態の状態推定装置 1においては、相対位置測定部 2 力取得される q 、または相対角度測定部 4から取得される Θ に誤差が生じても、

Ι,η Ι,η

式（5)、式（8)、および式（13)を用いることにより、移動体の現在位置（q , Θ )を

g,n g,n 推定できる。

[0106] 〔8.初期状態オブザーバの更新タイミングについて〕式 (5)および式 (8)で示される初期状態オブザーバの更新、すなわち式 (5)または式 (8)を用いて初期状態における移動体の絶対位置や、絶対姿勢角を算出するためには、相対位置、相対姿勢角、絶対位置、および絶対姿勢角について、同時刻のものが必要である。

[0107] し力しながら、図 4に示すように、外界センサの測定タイミングに対して、内界センサの測定タイミングは、その周期が短い。したがって、初期状態オブザーバを用いて初期状態を推定するタイミングを外界センサの測定タイミングと一致させると、そのタイミングにおける相対位置および相対姿勢角が得られなヽ場合がある。

[0108] そこで、初期状態オブザーバを用いて移動体の初期状態を推定するタイミングにお V、て、内界センサ力も得られる相対位置および相対姿勢角を補完することが好ま Uヽ。以下、その補完の手順について説明する。なお、相対姿勢角の補完は、相対角度測定部 4により行われ、相対位置の補完は、相対位置測定部 2により行われる。

[0109] 〔8— 1.前提条件〕

測定条件を以下のように考える。

[0110] まず、 nサンプル時の絶対位置 ·姿勢の測定値を、以下のように記載する。

[0111] [数 26]

なお、これらの絶対位置 ·姿勢の測定値は、単に「χ 」「θ 」と記載する場合もある

g,n g,n

[0112] また、 nサンプル時の絶対位置 ·姿勢を取得した時刻を、 t として記載する。

[0113] さらに、 mサンプル時の絶対位置 ·姿勢の測定値を、以下のように記載する。

[0114] [数 27]

l,m l,m

。また、 mサンプル時の絶対位置 ·姿勢を取得した時刻を、 t として記載する。

l，m

[0115] ここで、 nサンプル時の絶対位置 ·姿勢が測定された瞬間を考える。相対位置'姿勢は mサンプルまで取得されているとする。このとき、以下の 2つの状態を記載できる。

[0116] 状態 l. t ≤t

g,n lm

状態 2. t >t

g,n lm

状態 1.の場合、すでに得られている相対位置 '姿勢より前の時間の絶対位置を取得していることになり、状態 2.の場合にはもつとも新しい相対位置 ·姿勢を取得した時間より新しい情報がえられたことになるため、この 2つの状態を別々に扱うことで、精度を高めることができる。なお、状態 1.の場合、

t <t ≤t

lm— k— 1 g,n lm— k

を満たす整数 k≥0が存在する。

[0117] [8-2. t ≤t の場合の補完〕

g,n lm

この場合、相対位置 ·姿勢を簡単に補完することができる。補完方法は様々な方法が考えられるが、本実施形態では、（1)ォドメトリ、（2)拡張ォドメトリ、および (3)線形補完のそれぞれを用いる補完方法にっ、て説明する。

[0118] [8-2-1.ォドメトリを用いた補完〕

ォドメトリを用いる場合、 m—k—lサンプル時の姿勢角速度 ω ,および並進速

l,m-k-l

度 V が既知であるとする。

l,m-k-l

[0119] このとき， t における相対的な位置 (相対位置）'姿勢 X ,y ,θ は、以下のように

g,n Ι,η Ι,η Ι,η

書ける。

[0120] [数 28]

- ,"— cos ( _m

y = + - Η ,',— , sin ( — !)

H — > - [8-2-2.拡張ォドメトリを用いた補完〕

拡張ォドメトリを用いる場合、特別な情報は必要としない。この場合、 (1) Θ

l,m-k-l Θ の場合、および（2) θ ≠ Θ の場合に分けて補完する。

lm— k lm— k— 1 lm— k

[0121] 先ず、 0 = Θ の場合は、以下のように補完する。

lm_k_l lm_k

[0122] [数 29]

また、 0 ≠ Θ の場合は、以下のようにまず角度を補完する。

lm— k— 1 lm— k

[0123] [数 30]

^ (t _m-_k - t_g, ._m—_k—、+ .„ -t,,_m -^) _Lm-,

さらに、位置の補完のために、次のような r r , rを考; ^る。

b.n n

[0124] [数 31] r_a,_n = (s (0_{l m}__k― 6>,, cos θ_{Ι η<}― -(卜 cos(6>,„,

r _n = (sin ( sin θ_{Ι ηΜ} + (1— cos(¾„„

r_a , ,„—_k一 /, + "（ _m—— , i)

— ^Z

このとき、 x , y は次のようになる。

Ι,η Ι,η

[0125] [数 32] =r„ sin(6»_;„-6i_{/ m}^_！) cos^ ,^,., + x,„ =r_n sinC^ - ^„,_,_,) sin 6>, _m__t_, + r_n(l-cos(0, ,„_,_,)) cos 6»,__m_

[8-2- 3.線形補完〕

x , y , Θ の線形補完は、下式に基づき行われる。

Ι,η Ι,η

[0126] [数 33]

.'+( —

/," -,-!)

( " ：— し -, ：'"一

(,, ,-t,,,,,

/," ( ― ,

-,-,)

[8-3. t >t の場合の補完〕

g,n lm

この場合、前節と若干異なり、（1)ォドメトリを用いる補完、（2)拡張ォドメトリを用いる補完、（3)速度'角速度が得られない場合の補完、および (4)線形補完について説明する。

[0127] 〔8— 3— 1.ォドメトリを用いた補完〕

ォドメトリを用いる場合、 mサンプル時の姿勢角速度 ω ,および並進速度 V が取

l,m l,m 得可能であるとする。

[0128] このとき、 t における相対位置 ·姿勢 X , y , Θ は、以下のように書ける。

Ι,η Ι,η ι,η

[0129] [数 34]

y,,_n = ,, _m + .„-t J _m sin(^,„)

m

[8-3-2.拡張ォドメトリを用いた補完〕

拡張ォドメトリを用いる場合も、 mサンプル時の姿勢角速度 ω ,および並進速度 V

l,m 1 mが取得可能であるとする。

[0130] 拡張ォドメトリを用いる場合には、角度を以下のように補完する。

[0131] [数 35]

さらに、以下のような変数 δ を考える。

Ι,η

[0132] δ ω (t -t

l,n= l，m g,n l，m)

このとき、 x および y は、以下のようにして求める。

Ι,η Ι,η

[0133] [数 36]

1. „≠οのとき

X,„ = ― -―——― (sin <5,„ cos ,„ - (1 - cos (5,„ ) sin Θ, _m ) + „, y,„ = ^V ' - (sin δ,„ sin 9_lm+(\~∞sS,_n)∞_Se, ,,, ) + y_lm

' ' '

2. „=0のとき

= — , + ( ,"— "— ."—！ ^cos ( ," -,)

[8-3-3.速度 '角速度が得られない場合〕この場合、（1) θ = θ の場合と、 (2) θ ≠ θ の場合とに分けて説明する

i i

[0134] まず、 θ = θ の場合は、下式に基づき、 X , y , Θ を補完する。

[0135] [数 37]

- _ ('/,,„ + ¾,„ -— 一 .》

,,

一 /

I！ — —

！また、 θ ≠ Θ の場合は、以下のように、まず角度を補完する。

[0136] [数 38]

( .,, ,- ( -

'■" ( さらに、位置を補完するために、以下に示す r , r , rを考える。

[0137] [数 39] r。 (sin ( ,― ―,） cos — ,— (1— cos ( X —

„ = (sin(^ - 9_{l w}__x ) sin + (1 - cos(^ _M θ_{1 ιη} )) cos _m_ )

一 ¾ ) + ^- 1 )

, ,+ ,,，このとき、 χ , y は次のようになる。

[0138] [数 40] ,_n sin(f_;,„ -,) cos6>,_m_, -r„(l-cos(^„ -^,„,_,)) sin », _m„,

= , sin(¾„ - sin — +r„(l— cos ( ,, - cos /, ,„_,

[8-3-4.線形補完〕

線形補完は、下式に基づき行われる

[数 41]

- H-■ ι + ¾ ：," -v )

=

〔9.絶対角度測定部がない場合〕

本実施形態の状態推定装置 1においては、絶対角度測定部 5は必須の構成ではない。すなわち、状態推定装置 1において絶対角度測定部 5を省略しても、後述するように、移動体の初期状態を推定することは可能である。

[0140] 先ず、図 5に示すように、移動体が大域座標内を (X , y )から (X , y )まで

g,n-k g,n-k g,n g,n 移動し、局所座標系内を (χ , y )

l,n— k l,n— kから (X , y )

Ι,η Ι,ηまで移動したとする。この場合、移動後の状態における移動体の相対姿勢角 0

l，nおよび絶対姿勢角 0

g,nは、下式のように表現される。

[0141] Θ =arg( (x x ) +i(y y ))

Ι,η Ι,η Ι,η— k Ι,η Ι,η— k

θ = arg( (χ — χ ) +i(y y ))

g,n g,n g,n-k g,n g,n~k

さらに、上述の式（5)で表現される初期角度オブザーバを、下式のように記述することで、初期状態における移動体の Θ を演算することができる。

[0142] θ = Θ -k ( θ - Θ )

ob,n+l ob,n Θ g,n Ι,η

ここで、 θ を演算するにあたり、必要なパラメータは (χ , y )および (X , y )

0 g,n-k g,n-k g,n g,n である。これらのパラメータは、相対位置測定部 2および絶対位置測定部 3 (図 1参照 )力ら得られるものである。そして、相対角度測定部 4および絶対角度測定部 5から得られるパラメータは、 Θ を演算するために用いられていない。

[0143] よって、初期状態における移動体の姿勢角を推定するためには、相対角度測定部 4および絶対角度測定部 5は省略可能であり、上式の (X , y )および (X , y ) g，n_k g,n-k g,n g,n を用いた演算を初期角度推定部 6にて実行すればよいといえる。ただし、現在状態における移動体の絶対姿勢角を推定するのであれば、式（1)における Θ

1が必要となるので、相対角度測定部 4が必要となる。

[0144] なお、移動体が移動しない場合、すなわち (X — X , y -y ) = (0, 0)かつ (x

l,n l,n— k Ι,η Ι,η— k -x , y -y ) = (0, 0)の場合には、上述のように 0 および 0 を求めるこ g n g,n-k g,n g,n~k ι,η g,n

とができないが、現在位置は推定可能である。また、以下のように初期位置推定部と相対位置測定部を更新することで推定精度が向上する。

[0145] X =Τ ( θ ) χ + χ χ =0

ob,n+l ob,n 1 ob,n i

〔10.検証実験〕

前述した状態推定装置 1の有効性の確認を行うため、二輪車両型ロボットを用いた制御実験を行った。本実験では、デッドレコユング的な手法として改良型ォドメトリを用い、天測航法的な手法として CCDカメラを用いた位置計測システムを用いる。

[0146] 〔10— 1.実験装置〕

本実験で使用したシステムの構成を図 6に示す。図 6に示すように、本実験で用いたシステムにおいては、移動体として二輪車両型ロボットを用い、そのロボットの移動を PCで制御しつつ、ビジョンシステム（イメージプロセッサおよび CCDカメラ力も構成される）にて測定した。

[0147] また、本実験で使用した二輪車両型ロボットの構成を図 7 (a)および図 7 (b)に示す

。図 7 (a)および図 7 (b)に示すように、二輪車両型ロボットは、両輪にそれぞれ DCモータとロータリーエンコーダが取り付けられており、各車輪の角度がそれぞれ独立して計測できる。また、二輪車両型ロボットはデスクトップ型の PCで制御されており、本実験でのサンプリングタイムは約 9[msec]である。

[0148] 本実験で使用するビジョンシステムのサンプリングタイムは約 17[msec]である。また、ビジョンシステムではロボットの姿勢角を直接計測できないため、図 8に示すように 2 点の位置を計測し、下式を用いて姿勢角を演算する。

[0149] [数 42]

θ,, arctan

a_r - a, なお、ビジョンシステムの原点における計測値の誤差平均および誤差分散を、表: に示す。 [0150] [表 1]

1 ：均および分散（データ数 5000)

[10- 2.実験手法〕

初期状態 (x[cm],y[cm] Θ [rad])=(5.00,— 10.00,0.00)から、目標状態 (x [cm], y [cm], Θ [rad])=(0.00,0.00,0.00)へと、二輪車両型ロボットの移動を制御した。さらに、その制御則として、同次有限時間整定制御則を用い、 180秒間の制御を行った。

[0151] なお、二輪車両型ロボットの初期状態をビジョンシステムで計測した値を、初期状態オブザーバの初期値として用い、オブザーバゲイン (k ,k ,k )は (0.0007,0.0007,0.00 χ y θ

07)とする。さらに、二輪車両型ロボットが最終的に移動した地点（最終地点）に対し、本実施形態の状態推定装置で推定した推定値、およびノギスと分度器を用いて計測した実測値を記録する。

[0152] なお、本実験では初期状態オブザーバの更新は、ビジョンシステムのサンプリングタイムと同期して行っている。理論上は推定値の更新には同時刻の局所情報と大域情報が必要であるため、今回のように天測航法的な手法とデッドレコニング的な手法のサンプリングタイムが違う場合、オブザーバの推定値に影響を与える。本実験では、簡略化のため、サンプリングタイムの違いによる影響は十分小さいと仮定し、特別の補完はしていない。

[0153] 〔10— 3.実験結果〕

大域情報の推定値を図 9に、初期状態の推定値を図 10〜図 12に示す。また、最終地点における大域情報の推定値および実測値を表 2に、実測値と推定値の誤差平均および誤差分散を表 3に示す。

[0154] [表 2]

[0155] [表 3] 3 ：実測と推定値との間における、誤差平均おょぴ誤差分散

図 10より、 x は制御開始力約 80秒後に 5.12[cm]に収束し、約 120秒後から別の ob,n

値に収束し始めていることが確認できる。また、図 9を参照すると、約 120秒後においてロボットが再び動いており、移動した際に横滑りなどの外乱が生じたと推測できる。同じことが y および 0 についてもいえる。

ob,n ob,n

[0156] 〔11.補足〕

最後に、状態推定装置 1の各ブロック、特に初期角度推定部 6、現在角度推定部 7 、初期位置推定部 8、および現在位置推定部 9は、ハードウアロジックによって構成してもょ、し、次のように CPUを用いてソフトウェアによって実現してもよ!/、。

[0157] すなわち、状態推定装置 1は、各機能を実現する制御プログラムの命令を実行する CPU (.central processing unit)、上記プログラムを格納した ROM (read only memory 上記プログラムを展開する RAM (random access memory)、上記プログラムおよび各種データを格納するメモリ等の記憶装置 (記録媒体)などを備えている。そして、本発明の目的は、上述した機能を実現するソフトウェアである状態推定装置 1の制御プログラムのプログラムコード（実行形式プログラム、中間コードプログラム、ソースプログラム)をコンピュータで読み取り可能に記録した記録媒体を、上記状態推定装置 1 に供給し、そのコンピュータ (または CPUや MPU)が記録媒体に記録されているプログラムコードを読み出し実行することによつても、達成可能である。

[0158] 上記記録媒体としては、例えば、磁気テープやカセットテープ等のテープ系、フレキシブルディスク Zハードディスク等の磁気ディスクや CD— ROMZMOZMDZD

VDZCD—R等の光ディスクを含むディスク系、 icカード (メモリカードを含む) Z光カード等のカード系、あるいはマスク ROM/EPROM/EEPROM/フラッシュ RO M等の半導体メモリ系などを用いることができる。

[0159] また、状態推定装置 1を通信ネットワークと接続可能に構成し、上記プログラムコードを通信ネットワークを介して供給してもよい。この通信ネットワークとしては、特に限定されず、例えば、インターネット、イントラネット、エキストラネット、 LAN, ISDN, V AN、 CATV通信網、仮想専用網（virtual private network)、電話回線網、移動体通信網、衛星通信網等が利用可能である。また、通信ネットワークを構成する伝送媒体としては、特に限定されず、例えば、 IEEE1394、 USB、電力線搬送、ケーブル TV 回線、電話線、 ADSL回線等の有線でも、 IrDAやリモコンのような赤外線、 Bluetoo th (登録商標）、 802. 11無線、 HDR、携帯電話網、衛星回線、地上波デジタル網等の無線でも利用可能である。なお、本発明は、上記プログラムコードが電子的な伝送で具現化された、搬送波に埋め込まれたコンピュータデータ信号の形態でも実現され得る。

[0160] 本発明は、相対姿勢角や相対位置に生じた誤差に影響を受けず、移動体の初期位置や初期姿勢角が未知の場合でも、移動体の現在位置および現在姿勢角を推定できるという特徴をもつ。また、 Kalmanフィルタを利用する手法と違い，計測値の誤差分散行列が不要であるため、誤差分散を求めることが難しい場合でも問題なく利用できる。

[0161] さらに、本発明の状態推定装置は、上記相対角度測定手段が、上記絶対角度測定手段が絶対角度を測定するタイミングにおいて既に測定された相対姿勢角を補完し、その補完された相対姿勢角を、上記初期角度推定手段に出力することが好ましい。

[0162] すなわち、初期角度推定手段が初期姿勢角を推定するタイミングにおいては、移動体の相対姿勢角および絶対姿勢角に関し、同時刻の値が揃っていることが好ましい。しかしながら、一般的に、絶対角度測定手段が絶対姿勢角を測定する周期は、相対角度測定手段が相対姿勢角を測定する周期に比べると長いので、初期姿勢角が推定されるタイミングにお、て、相対姿勢角が得られな、場合もある。

[0163] そこで、上記構成においては、相対角度測定手段が、補完された相対姿勢角を、初期角度推定手段に出力する。よって、初期姿勢角が推定されるタイミングにおける相対姿勢角が、相対角度測定手段により測定されていない場合でも、補完された相対姿勢角を、相対姿勢角の測定値に替えて用いることで、初期姿勢角を推定することがでさる。

[0164] さらに、本発明の状態推定装置は、上記相対位置測定手段が、上記絶対位置測定手段が絶対位置を測定するタイミングにおいて既に測定された相対位置を補完し、その補完された相対位置を、上記初期位置推定手段に出力することが好ましい。

[0165] すなわち、初期位置推定手段が初期位置を推定するタイミングにおいては、移動体の相対位置および絶対位置に関し、同時刻の値が揃っていることが好ましい。しかしながら、一般的に、絶対位置測定手段が絶対位置を測定する周期は、相対位置測定手段が相対位置を測定する周期に比べると長いので、初期位置が推定されるタイミングにお、て、相対位置が得られな、場合もある。

[0166] そこで、上記構成にぉ、ては、相対位置測定手段が、補完された相対位置を、初期位置推定手段に出力する。よって、初期位置が推定されるタイミングにおける相対位置が、相対位置測定手段により測定されていない場合でも、補完された相対位置を、相対位置の測定値に替えて用いることで、初期位置を推定することができる。

[0167] なお、上記構成の状態推定方法における一部のステップをしてコンピュータに実行させる状態推定プログラムにより、コンピュータを用いて本発明の状態推定装置と同様の作用効果を得ることができる。さらに、上記状態推定プログラムをコンピュータ読み取り可能な記録媒体に記憶させることにより、任意のコンピュータ上で上記状態推定プログラムを実行させることができる。産業上の利用可能性

[0168] 本発明によれば、たとえば自動車や船舶である移動体の現在位置および現在姿勢角を、初期位置や初期姿勢角が未知である場合にも推定できる。

Claims

請求の範囲

[1] 移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、

移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する絶対位置測定手段と、

上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、

上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等、ものとして、初期姿勢角を推定する初期角度推定手段と、

現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する現在角度推定手段と、

上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えて!/ゝることを特徴とする状態推定装置。

[2] 移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する相対位置測定手段と、

上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する相対角度測定手段と、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する絶対角度測定手段と、

上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値力初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する初期角度推定手段と、

上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する初期位置推定手段と現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する現在位置推定手段と、を備えて!ゝることを特徴とする状態推定装置。

[3] 上記相対角度測定手段は、上記絶対角度測定手段が絶対角度を測定するタイミングにおいて既に測定された相対姿勢角を補完し、その補完された相対姿勢角を、上記初期角度推定手段に出力することを特徴とする請求項 2に記載の状態推定装置。

[4] 上記相対位置測定手段は、上記絶対位置測定手段が絶対位置を測定するタイミングにおいて既に測定された相対位置を補完し、その補完された相対位置を、上記初期位置推定手段に出力することを特徴とする請求項 1または 2に記載の状態推定装置。

[5] 移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第 1ステップと、

移動体の移動とは無関係に座標軸が固定された大域座標系における移動体の位置を、絶対位置として測定する第 2ステップと、

上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第 3ステップと、上記相対位置および上記絶対位置から、上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として算出し、さらに、上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等、ものとして、初期姿勢角を推定する第 4ステップと、

現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第 5ステップと、

上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第 6ステップと、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第 7ステップと、を備えて!/、ることを特徴とする状態推定方法。

[6] 移動体の移動に応じて座標軸が変化する局所座標系における移動体の位置を、相対位置として測定する第 1ステップと、

上記局所座標系の座標軸に対して移動体がなす角を、相対姿勢角として測定する第 3ステップと、

上記大域座標系の座標軸に対して移動体がなす角を、絶対姿勢角として測定する第 4ステップと、

上記相対姿勢角および上記絶対姿勢角を含む漸化式で表現される数列の収束値力初期状態における移動体の上記大域座標系内での姿勢角を示す初期姿勢角に等しいものとして、初期姿勢角を推定する第 5ステップと、

現在の状態における移動体の上記大域座標系内での姿勢角を示す現在姿勢角を、上記相対姿勢角と、上記推定された初期姿勢角との和として推定する第 6ステップと、上記推定された初期姿勢角、上記相対位置、および上記絶対位置を含む漸化式で表現される数列の収束値が、初期状態における移動体の上記大域座標系内での位置を示す初期位置に等しいものとして、初期位置を推定する第 7ステップと、現在の状態における移動体の上記大域座標系内での位置を示す現在位置を、上記相対位置と、上記推定された初期姿勢角と、上記推定された初期位置とに基づき推定する第 8ステップと、を備えて!/、ることを特徴とする状態推定方法。

[7] 請求項 5に記載の状態推定方法における第 4ステップ力第 7ステップまで、または請求項 6に記載の状態推定方法における第 5ステップ力第 8ステップまでを、コンビユータに実行させることを特徴とする状態推定プログラム。

[8] 請求項 7に記載の状態推定プログラムを記録したことを特徴とするコンピュータ読取可能な記録媒体。