TOP > 国内特許検索 > 状態ベクトル推定方法および自律型移動体 > 明細書

明細書 :状態ベクトル推定方法および自律型移動体

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第4389046号 (P4389046)
公開番号 特開2007-052652 (P2007-052652A)
登録日 平成21年10月16日(2009.10.16)
発行日 平成21年12月24日(2009.12.24)
公開日 平成19年3月1日(2007.3.1)
発明の名称または考案の名称 状態ベクトル推定方法および自律型移動体
国際特許分類 G05D   1/02        (2006.01)
G01B  11/00        (2006.01)
FI G05D 1/02 Z
G01B 11/00 A
請求項の数または発明の数 6
全頁数 24
出願番号 特願2005-237541 (P2005-237541)
出願日 平成17年8月18日(2005.8.18)
審査請求日 平成20年8月14日(2008.8.14)
特許権者または実用新案権者 【識別番号】506086797
【氏名又は名称】独立行政法人沖縄科学技術研究基盤整備機構
発明者または考案者 【氏名】吉本 潤一郎
【氏名】孫野 道征
個別代理人の代理人 【識別番号】100064746、【弁理士】、【氏名又は名称】深見 久郎
【識別番号】100085132、【弁理士】、【氏名又は名称】森田 俊雄
【識別番号】100083703、【弁理士】、【氏名又は名称】仲村 義平
【識別番号】100096781、【弁理士】、【氏名又は名称】堀井 豊
【識別番号】100098316、【弁理士】、【氏名又は名称】野田 久登
【識別番号】100109162、【弁理士】、【氏名又は名称】酒井 將行
審査官 【審査官】佐藤 彰洋
参考文献・文献 特開2000-163395(JP,A)
特表2004-536400(JP,A)
特開2005-096068(JP,A)
特開2005-148851(JP,A)
特開2003-042718(JP,A)
調査した分野 G05D 1/00-1/12
G01B 11/00
B25J 5/00
B25J 13/08
B25J 19/04
G01B 11/00
G06F 15/18
特許請求の範囲 【請求項1】
対象とする物理システムの状態ベクトルが制御部からの複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から前記状態ベクトルを推定するための状態ベクトル推定方法であって、
事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成するステップと、
現タイムステップまでの観測情報の履歴が与えられた場合の前記状態ベクトルの条件付き確率密度関数が複数個の第1のガウス関数成分からなる第1の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を前記状態遷移モデルに基づいて算出する予測分布算出ステップと、
次タイムステップにおいて観測センサにより前記観測量を観測する観測ステップと、
前記観測モデル、前記算出された予測分布ならびに前記次タイムステップで観測された観測量に基づいて、前記次タイムステップまでの前記観測情報の履歴が与えられた場合の前記状態ベクトルの事後分布を複数個の第2のガウス関数成分からなる第2の混合ガウス分布として算出する事後分布算出ステップと、
前記事後分布に基づいて、前記状態ベクトルの期待推定量を算出する推定量算出ステップと、
前記予測分布算出ステップから前記推定量算出ステップまでを、前記第1のガウス関数分布成分を前記第2のガウス関数成分により置換して、所定の条件が満たされるまで繰り返すステップとを備える、状態ベクトル推定方法。
【請求項2】
前記制御部は、前記第1および第2のガウス関数成分を格納するための記憶装置を有し、
前記事後分布を算出した後に、前記事後分布を構成する複数個の第2のガウス関数成分のうち、混合比の上位から所定個を前記第2のガウス関数成分として、前記記憶装置に残すステップをさらに備える、請求項1記載の状態ベクトル推定方法。
【請求項3】
前記状態ベクトルは、前記制御部が搭載される自律型移動体の自己位置である、請求項1記載の状態推定方法。
【請求項4】
自律型移動体であって、
行動命令に従って、前記自律型移動体を駆動するための駆動手段と、
前記自律型移動体の環境の観測量を獲得するための観測センサと、
前記駆動手段に対して複数の前記行動命令を出力する制御部とを備え、前記制御部は、前記自律型移動体の状態ベクトルが前記複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される前記観測量から前記状態ベクトルを推定して、前記自律型移動体の動作を制御し、
前記制御部は、
事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成する観測モデル生成手段と、
現タイムステップまでの観測情報の履歴が与えられた場合の前記状態ベクトルの条件付き確率密度関数が複数個の第1のガウス関数成分からなる第1の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を前記状態遷移モデルに基づいて算出する予測分布算出手段と、
次タイムステップにおいて前記観測センサにより前記観測量を獲得する観測処理手段と、
前記観測モデル、前記算出された予測分布ならびに前記次タイムステップで観測された観測量に基づいて、前記次タイムステップまでの前記観測情報の履歴が与えられた場合の前記状態ベクトルの事後分布を複数個の第2のガウス関数成分からなる第2の混合ガウス分布として算出する事後分布算出手段と、
前記事後分布に基づいて、前記状態ベクトルの期待推定量を算出する推定量算出手段と、
前記予測分布算出手段、観測処理手段、事後分布算出手段および前記推定量算出手段による処理を、前記第1のガウス関数分布成分を前記第2のガウス関数成分により置換して、所定の条件が満たされるまで繰り返す反復手段とを備える、自律型移動体。
【請求項5】
前記制御部は、
前記第1および第2のガウス関数成分を格納するための記憶装置と、
前記事後分布を算出した後に、前記事後分布を構成する複数個の第2のガウス関数成分のうち、混合比の上位から所定個を前記第2のガウス関数成分として、前記記憶装置に残す選別手段とをさらに含む、請求項4記載の自律型移動体。
【請求項6】
前記状態ベクトルは、前記自律型移動体の自己位置である、請求項4記載の自律型移動体。
発明の詳細な説明 【技術分野】
【0001】
本発明は、状態ベクトル推定方法および状態ベクトル推定方法により制御される自律型移動体に関し、より特定的には、状態ベクトルの要素としての自己位置を同定することが可能な方法および自己位置を同定することが可能な自律型移動体に関する。
【背景技術】
【0002】
自律移動型ロボットにおいて、自己が環境に対してどのような状態にあるかを知ることは、ロボットが意志決定をする上で、重要な情報である。このような情報を推定する手法に関して多くの研究が行なわれている(たとえば、非特許文献1を参照)。
【0003】
近年、上記のような問題に対する対処として主流となっているのは、環境を確率モデルとして定式化し、ベイズ推定法を利用して状態を推定する手法である(たとえば、非特許文献2を参照)。特に、パーティクルフィルタによるモンテカルロ位置同定法(Monte Calro Localization:MCL)がよく知られている(たとえば、非特許文献3を参照)。

【非特許文献1】J.Borenstein et al.: Navigating Mobile Robots : Systems and Techniques. A.K. Peters, Ltd., 1996.
【非特許文献2】C.F.Olson : IEEE Trans. On Robotics and Automation, 16(1), 55-66, 2000.
【非特許文献3】S. Thrun et al. : Artificial Intelligence, 128(1-2), 99-141, 2001.
【発明の開示】
【発明が解決しようとする課題】
【0004】
上述したMCLは、パーティクルと呼ばれるサンプル集合を用いて状態を推定する手法であり、ロバスト性が高く、有効な手法の1つである。
【0005】
その一方で、MCLでは、精度の高い推定を行なうためには多数のサンプルが必要となり、サンプル数の増加とともに計算量が増加してしまうという問題がある。
【0006】
本発明は、上記のような問題を解決するためになされたものであって、その目的は、精度の高い状態ベクトルの推定を行ないつつ、計算量を削減することが可能な状態ベクトル推定方法および状態ベクトル推定方法により制御される自律型移動体を提供することである。
【0007】
この発明の他の目的は、精度の高い自己位置の同定を行ないつつ、計算量を削減することが可能な自己位置の同定方法およびこの同定方法を用いた自律型移動体を提供することである。
【課題を解決するための手段】
【0008】
このような目的を達成するために、本発明の状態ベクトル推定方法は、対象とする物理システムの状態ベクトルが制御部からの複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から状態ベクトルを推定するための状態ベクトル推定方法であって、事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成するステップと、現タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの条件付き確率密度関数が複数個の第1のガウス関数成分からなる第1の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を状態遷移モデルに基づいて算出する予測分布算出ステップと、次タイムステップにおいて観測センサにより観測量を観測する観測ステップと、観測モデル、算出された予測分布ならびに次タイムステップで観測された観測量に基づいて、次タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの事後分布を複数個の第2のガウス関数成分からなる第2の混合ガウス分布として算出する事後分布算出ステップと、事後分布に基づいて、状態ベクトルの期待推定量を算出する推定量算出ステップと、予測分布算出ステップから推定量算出ステップまでを、第1のガウス関数分布成分を第2のガウス関数成分により置換して、所定の条件が満たされるまで繰り返すステップとを備える。
【0009】
好ましくは、制御部は、第1および第2のガウス関数成分を格納するための記憶装置を有し、状態ベクトル推定方法は、事後分布を算出した後に、事後分布を構成する複数個の第2のガウス関数成分のうち、混合比の上位から所定個を第2のガウス関数成分として、記憶装置に残すステップをさらに備える。
【0010】
好ましくは、状態ベクトルは、制御部が搭載される自律型移動体の自己位置である。
この発明の他の局面に従うと、自律型移動体であって、行動命令に従って、自律型移動体を駆動するための駆動手段と、自律型移動体の環境の観測量を獲得するための観測センサと、駆動手段に対して複数の行動命令を出力する制御部とを備え、制御部は、自律型移動体の状態ベクトルが複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から状態ベクトルを推定して、自律型移動体の動作を制御し、制御部は、事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成する観測モデル生成手段と、現タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの条件付き確率密度関数が複数個の第1のガウス関数成分からなる第1の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を状態遷移モデルに基づいて算出する予測分布算出手段と、次タイムステップにおいて観測センサにより観測量を獲得する観測処理手段と、観測モデル、算出された予測分布ならびに次タイムステップで観測された観測量に基づいて、次タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの事後分布を複数個の第2のガウス関数成分からなる第2の混合ガウス分布として算出する事後分布算出手段と、事後分布に基づいて、状態ベクトルの期待推定量を算出する推定量算出手段と、予測分布算出手段、観測処理手段、事後分布算出手段および推定量算出手段による処理を、第1のガウス関数分布成分を第2のガウス関数成分により置換して、所定の条件が満たされるまで繰り返す反復手段とを備える。
【0011】
好ましくは、制御部は、第1および第2のガウス関数成分を格納するための記憶装置と、事後分布を算出した後に、事後分布を構成する複数個の第2のガウス関数成分のうち、混合比の上位から所定個を第2のガウス関数成分として、記憶装置に残す選別手段とをさらに含む。
【0012】
好ましくは、状態ベクトルは、自律型移動体の自己位置である。
【発明を実施するための最良の形態】
【0013】
以下、図面を参照して本発明の実施の形態について説明する。
(本発明の概要)
以下では、本発明の状態ベクトル推定方法が適用される制御対象の例としては自立型移動体で説明を行なう。そして、自立型移動体である自律型移動ロボットの例として、サイバーローデント(Cyber Rodent:CR)を取り上げ、このCRの自己位置同定問題を例として、本発明に係る状態ベクトル推定方法を適用した自己位置同定方法を説明する。
【0014】
図1は、このようなCR100の外観を示す図である。CR100は、ネズミ型移動ロボットであり、外界の画像を観測するためのカメラ202と、移動のための車輪212を備える。CR100は、所定の移動可能領域(以下、「フィールド」と呼ぶ)中を自律的に動き回ることができるものとする。
【0015】
簡単さのために、CR100の行動は、以下の2種類のマクロ行動命令によって制御されているものとする。1つは回転することなく25[cm]前進するための行動命令であり、命令Forwardで表される。もう1つは重心位置を変化させることなく反時計回り方向にπ/6[radian] 回転するための行動命令であり、命令Rotationで表される。実際には、より複雑な複数の移動命令のセット(移動量を指定した直進移動、移動量を指定した後退移動、回転量を指定した回転運動等)により、CR100の動作を制御することが可能である。
【0016】
後に詳しく説明するように、CR100には6つの赤外線(Infrared Radiation:IR)近接センサ搭載されており、これらのセンサを通してフィールド中に設置されている壁や障害物が近接しているかどうかを検知することができる。しかしながら、以下の説明では、CR100の位置や進行方向を明示的に伝える天井カメラのようなセンサは、CR100は直接利用できないものとする。その代わりに、CR100は、各フィールド上の位置と進行方向が与えられたときに、どのようなIRセンサ値が得られるのかという対応関係を表すキャリブレーション用データは事前に収集できるものとする。
【0017】
以上のような条件の下で、時々刻々と与えられるセンサ情報を利用して、本発明の自己位置の同定方法では、CR100がフィールド中のどの位置でどの進行方向を向いているのかを逐次推定する。
【0018】
(自己位置の同定方法の原理)
以下では、本発明の自己位置の同定方法の具体的なアルゴリズムの説明を行なう前提として、自己位置の同定方法の原理について概略説明する。
【0019】
1-1.環境の確率モデル
まず、以下のように、2種類の確率分布を使ってCR100の動作環境をモデル化する。
【0020】
図2は、CR100が自律的に動き回ることができるフィールドの一例を示す図である。
【0021】
図2で示されるように、フィールドの水平方向および垂直方向から計ったCR100の位置座標を、それぞれ、PhおよびPvで表し、水平方向の軸から計ったCR100の進行方向をq∈(-π,π]で表すものとする。そして、CR100 の位置と進行方向から状態ベクトルxをx≡(Ph, Pv, cos q, sin q)Tによって構成する。ここで、記号“T”は転置を表している。
【0022】
CR100 の行動するフィールドがスリップや摩擦などによって生じるノイズを全く含まない理想環境であることを仮定すると、状態ベクトルxは以下の方程式にしたがって遷移する。
【0023】
【数1】
JP0004389046B2_000002t.gif

【0024】
ここで、(t)は時間ステップを指し示すインデクスである。A(t-1)は行動命令に依存した4×4次行列であり、以下で与えられる。
【0025】
【数2】
JP0004389046B2_000003t.gif

【0026】
しかしながら、実環境では様々なノイズとなり得る情報源が存在する。そこで、状態遷移を撹乱させるノイズ源を4次元ベクトルξ(t)で表し、これが偏りのない白色ガウスノイズにしたがうことを仮定すれば、実環境における状態遷移方程式は以下で与えられる。
【0027】
【数3】
JP0004389046B2_000004t.gif

【0028】
ここで、Np(x;μ,Σ)はp次元平均ベクトルμとp×p次共分散行列Σをパラメータとする状態xのp次元ガウス確率密度関数であり、以下で定義される。
【0029】
【数4】
JP0004389046B2_000005t.gif

【0030】
なお、Qは白色ガウスノイズv(t)の共分散を表す4×4次のある正定値行列である。この状態遷移方程式から、前時刻t-1における状態ベクトルx(t-1)が与えられたときの現時刻tにおける状態ベクトルx(t)の確率密度関数は以下のように導くことができる。
【0031】
【数5】
JP0004389046B2_000006t.gif

【0032】
上記式(1)は状態遷移の特性を表す確率分布であるため、以下では「状態遷移モデル」と呼ぶ。
【0033】
次にセンサの特性を表すためのモデルを構築する。
事前に用意されたセンサキャリブレーション用データセットをD≡{(x(k),y(k));k=1,…,K}とする。ここで、yは状態xの時に観測されたセンサ値を6次元ベクトルと表したものであり、kはデータ順を表すインデクスである。
【0034】
そして、データセットDの入出力特性は以下で定義される確率的正規化動径基底関数(normalized Radial Basis Function; nRBF)ネットワークに変換される。
【0035】
【数6】
JP0004389046B2_000007t.gif

【0036】
ここで、vmおよびSmは、状態空間を滑らかに分割するための設計パラメータであり、Φm(x)は、確率分布P(y|x)を表現するために、m番目のガウス分布成分N(y;wm,Rm)が果たすべき寄与度に対応する。言い換えると、vmは状態空間中の離散的格子点に対応し、Smは、ユークリッド空間における最隣接格子点の二乗距離に対応する値を対角要素に代入した対角行列として設定される。vmは、4次元の状態空間を一様に覆い尽くすように設定し、その平均間隔となるようにSmを設定することが望ましいが、具体的な設定方法は後で述べる。6次元ベクトルwmおよび6×6次正定値対角行列Rmは、それぞれ、CR100が状態vm近傍に存在するとき得られるセンサ値の平均と分散を表すパラメータである。最尤推定法に基づけば、wmとRmは、データセットDとゲイティング関数の集合{Φm(・)|m=1,…,M}から以下によって自動的に決定することができる。
【0037】
【数7】
JP0004389046B2_000008t.gif

【0038】
式(2)は観測されるセンサ値の特性を表す確率分布であるため、以下では「観測モデル」と呼ぶ。
【0039】
なお、ここでは、CR100の自己位置推定問題に特化した状態遷移モデルや観測モデルの構築方法について述べたが、これらが、それぞれ、(1)式および(2)式で表現できるような自然現象や制御対象であれば、以下のアルゴリズムは任意のものに適用できる。
【0040】
1-2.混合カルマンフィルタによる逐次状態推定
状態遷移モデル(1)式および観測モデル(2)式が構築されると、各時刻において直接観測できない状態ベクトルは逐次ベイズ推定法によって推定することができる。
【0041】
今、時刻tまでの観測情報(センサ)の履歴Y1tが与えられたときの、状態ベクトルx(t)の条件付き確率密度関数が、以下の式(3)ようにN(t)個のガウス関数成分からなる混合ガウス分布によって与えられているものとする。
【0042】
【数8】
JP0004389046B2_000009t.gif

【0043】
ここで、式(3)において、n番目のガウス分布成分の寄与度を表す混合比と呼ばれるパラメータは、以下のように表されている。
【0044】
【数9】
JP0004389046B2_000010t.gif

【0045】
さらに、この混合比は、以下のような条件を満たす変数である。
【0046】
【数10】
JP0004389046B2_000011t.gif

【0047】
また、式(3)における他の変数の意味は以下のとおりである。
【0048】
【数11】
JP0004389046B2_000012t.gif

【0049】
この時、式(1)と式(3)から、観測情報の履歴Y1tが与えられたときの、次時刻の状態ベクトルx(t+1)の予測分布は以下の式(4)で解析的に求めることができる。
【0050】
【数12】
JP0004389046B2_000013t.gif

【0051】
ここで、以下の関係が成り立つ。
【0052】
【数13】
JP0004389046B2_000014t.gif

【0053】
式(4)が計算された後、新たな情報y(t+1)が観測されると、与えられた観測情報の履歴Y1t+1に対する状態ベクトルx(t + 1) の事後分布は以下の式(5)で表現することができる。
【0054】
【数14】
JP0004389046B2_000015t.gif

【0055】
特に、任意の状態ベクトルxに対して、以下のような近似がなりたつように、パラメータ{(vm,Sm)|m=1,…,M}が設定されているものとする。
【0056】
【数15】
JP0004389046B2_000016t.gif

【0057】
このような近似が成り立つとき、式(5)は以下の式(6)ように近似することができる。
【0058】
【数16】
JP0004389046B2_000017t.gif

【0059】
ここで、以下の関係が成り立つ。
【0060】
【数17】
JP0004389046B2_000018t.gif

【0061】
また、インデクスn´はn´=nM+m(n=0,1,…,N(t)-1;m=1,…M)が成り立つような整数である。式(5)と式(6)がいずれも混合ガウス分布のクラスに属するので、初期状態x(1)に対する事前分布p(x(t))を混合ガウス分布として与えれば、式(4)と式(6)によって構成される逐次更新式を使って、以下の事後分布PTDを得ることができる。
【0062】
【数18】
JP0004389046B2_000019t.gif

【0063】
この逐次計算法は、各ガウス関数成分のパラメータがカルマンフィルタアルゴリズムとによって推定されるので混合カルマンフィルタと呼ぶ。
【0064】
[発明の実施の形態]
図3は、本発明の自己位置同定方法を用いた制御装置が適用される自律型移動ロボットCR100の構成の一例を示す機能ブロック図である。
【0065】
図3を参照して、CR100は、CR100が追跡する目標対象物を映像として認識するための画像を撮影するカメラ202と、CR100と周囲の障害物、たとえば、壁との間の距離を測定するための赤外線近接センサ204.1~204.6と、バッテリ220と、バッテリ220から供給される電力に基づいて、CR100の各部に供給する電力を生成する電源回路部230と、制御ボード(以下、「制御部」と呼ぶ)300と、制御部300からの制御信号によりそれぞれ駆動される右駆動モータ部213および左駆動モータ部214(以下、両者を総称する場合は「駆動モータ部」と呼ぶ)と、駆動モータ部213および214によりそれぞれトルクを与えられてCR100を移動させるための車輪211および212とを備える。
【0066】
制御部300では、カメラ202を介して入力されたデジタル画像データは、一旦、インタフェース310を経由して、記憶装置304内の画像データバッファ部に格納される。演算部302は、たとえば、CPU(Central Processing Unit)であって、画像データバッファ部中のデジタル画像データを、必要に応じて、解析する。
【0067】
また、演算部302は、センサ204.1~204.6からの信号に基づいて、CR100と周囲の障害物との間の距離を認識し、後に説明するような自己位置の同定処理を行う。さらに、演算部302は、駆動モータを制御するための制御信号、たとえば、駆動モータ部213および214に与える駆動トルクに対応した制御信号を計算して、制御系インタフェース312を介して、駆動モータ部213および214に与える。
【0068】
なお、記憶装置304は、たとえば、ROM(Read Only Memory)およびRAM(Random Access Memory)を含んでおり、ROMにはCR100の制御プログラムが記憶されており、RAMは、この制御プログラムの動作のための一時記憶メモリとして動作する。あるいは、ROMには、制御プログラムを走らせるためのOS(Operating System)が記憶されており、制御プログラムは、RAMに読み込まれて実行されることとしてもよい。この場合は、RAMに読み込む制御プログラムを格納するための記憶装置(たとえば、ハードディスクや不揮発性メモリ)が、記憶装置304に含まれていてもよい。
【0069】
本発明の自己位置同定方法を実行するための主要部は、制御部300を構成するコンピュータハードウェアと、演算部302により実行されるソフトウェアとにより構成される。一般的にこうしたソフトウェアはメモリカード等の記憶媒体に格納されて流通させることも可能で、この場合は、ドライブ装置により記憶媒体から読取られて記憶装置304に一旦格納される。図3に示すようなコンピュータのハードウェア自体およびその動作原理は一般的なものである。したがって、本発明の最も本質的な部分は、記憶装置304に記憶されるソフトウェアである。
【0070】
ここで、センサ204.1は、赤外線光測距方式のセンサであって、壁に当たった赤外線の反射量を測定する。センサ204.1が受光した光量をADコンバータ(図示せず)でデジタル値に変換し、制御部300が読み取る。制御部300は、反射が多いときは壁近くにいる、反射が少ないときは壁から離れた位置にいると判断する。
【0071】
より詳しくは、センサ204.1は、発光素子PLと受光素子PDで構成される。発光素子PLには、たとえば、赤外線発光ダイオード(LED)が用いられる。受光素子PDには、たとえば、フォトトランジスタやフォトダイオードなどが用いられる。センサ204.1では、たとえば、規定周期的にLEDを一瞬だけ光らせ、受光側に投光側の同期と同期した反射光のみを取出すハイパスフィルタを設けることにより、周りからの外乱光の影響を受けずに距離の測定を行うことができる。なお、他のセンサ204.2~204.6も、同様の構成を有する。また、センサ204.1~204.6としては、CR100と周囲の障害物との間の距離を測定できるものであれば、他の方式のセンサであってもよい。
【0072】
図4は、図3に示したCR100の上部から見た距離測定の方向(センサの方向)を示す概念図である。
【0073】
図4に示すように、センサ204.1は、CR100の真正面をセンシング方向とし、センサ204.2および204.3は、センサ204.1のセンシング方向からそれぞれ左右に30°ずつずれた方向をセンシング方向とする。センサ204.4および204.5は、CR100のそれぞれ左側面および右側面方向をセンシング方向とし、センサ204.6は、CR100の後方をセンシング方向とする。
【0074】
[自己位置同定のための推定手法]
上述した自己位置の同定方法を実際に制御プログラムとして実装するにあたっては、以下のような工夫が必要である。
【0075】
すなわち、式(4)と式(6)によって構成される逐次更新式の一番の問題点は、時間経過とともに事後分布を表現するためのガウス関数成分の数が指数的に増大していくことである。例えば、初期状態x(1)の事前分布p(x(1))が単一成分からなるガウス関数によって与えられていたとしても、上記事後分布PTDは、Mt個のガウス関数成分からなる混合ガウス分布となる。
【0076】
この困難さを解消するために、以下のような発見的解決法を導入する。今、制御部300の記憶装置300がワーキングエリアとしてNmax個のガウス関数成分を保持できるだけのメモリ空間を有しているものとし、事後分布PTDを近似する場合を考える。この時、式(6)を計算した後、各ガウス関数成分が持つ以下の式で表される混合比の降順にmin(MN(t),Nmax)個のガウス関数成分のみを保持し、他の成分はメモリ空間から削除する。
【0077】
【数19】
JP0004389046B2_000020t.gif

【0078】
削除されるガウス関数成分が持つ混合比の和は最大でも1/Nmaxとなり、Nmaxが十分に大きければ1/Nmaxは、ほぼ0となる。したがって、削除されるガウス関数成分は、事後分布PTDの近似にほとんど役立たないために、このような発見論的方法が多くの実問題に対して有効に働く。
【0079】
また、CR100の問題の場合、観測情報の履歴Y1tから良い行動選択を推論する際には、現在の状態x(t)を式(6)のように確率分布として表現するよりも一つの値として表現するほうが容易に扱うことができる。この場合には、事後分布(6)の下での以下の式で表現されるx(t)の期待値によって、x(t)の期待推定値を表現することができる。
【0080】
【数20】
JP0004389046B2_000021t.gif

【0081】
(自己位置同定の手順)
(初期設定)
図5~7は、以上の説明した方法により、CR100の自己位置の同定処理を実行させるための計算手続きを示すフローチャートである。
【0082】
まず、図5を参照して、自己位置同定処理の初期処理として、ユーザからの指示によりキャリブレーション用に収集するデータ数Kを設定する(S100)。
【0083】
続いて、CR100がフィールド上を移動し、キャリブレーション用データDを収集する(S102)。このとき、特に限定されないが、たとえば、ユーザの指示により、フィールド内で位置が既知の地点にCR100を移動させ、そのときの方向の情報をユーザからCR100に与えることとしてもよい。
【0084】
さらに、確率的nRBFネットワークを構成する成分数Mを設定するとともに、設計パラメータ(vm、Sm)の設定を実行する(S104)。成分数Mの設定は、たとえば、ユーザが実験的に事前に求めた値とすることができるし、設計パラメータについても、ユーザがフィールドの大きさにより事前に設定するものとする。
【0085】
確率的nRBFネットワークの残りのパラメータ{(wm、Rm)|m=1,…,M}は、上述したように、以下の式にしたがって決定する。
【0086】
【数21】
JP0004389046B2_000022t.gif

【0087】
さらに、ユーザは、CR100が保持するガウス関数成分の数Nmaxをメモリ空間を超えない自然数に設定する(S108)。
【0088】
初期状態x(1)に対する事前確率分布p(x(1))を以下のようにして設定する(S110)。
【0089】
【数22】
JP0004389046B2_000023t.gif

【0090】
なお、ここで「任意の」とは、「ユーザによって、条件を満たす限り、各変数の初期値としては、任意の値、または、任意の値の集合を設定することができること」を意味する。
【0091】
以上の初期設定の後、自己位置の同定処理が行なわれる(S112)。
(自己位置の同定)
次に、図6および図7を参照して、図5のステップS112の処理について、さらに詳しく説明する。
【0092】
図6を参照して、まず、演算部302により、タイムステップの変数tが1に設定される(S200)。
【0093】
次に、演算部302は、変数tが所定の値T以下である場合は(S202)、次に、変数tの値が2以上であるか否かを判定する(S204)。
【0094】
ステップS204において、t=1であって変数tの値が2未満である場合は、処理をステップ処理をステップS208に移行する。一方、ステップS204において、変数tの値が2以上である場合は、演算部302は、以下の式に従って、パラメータの更新を行なう(S206)。
【0095】
【数23】
JP0004389046B2_000024t.gif

【0096】
これにより、以下のような予測分布が得られる。
【0097】
【数24】
JP0004389046B2_000025t.gif

【0098】
続いて、ステップS208では、演算部302は、センサ情報y(t)の観測を行ないセンサからの測定情報を受け取る(S208)。
【0099】
変数n′の値を0とし(S210)、続いて、変数nおよびmの値を1とする(S212)。
【0100】
さらに、演算部302は、変数nと変数N(t—1)の値とを比較し(S214)、変数nが変数N(t—1)以下であれば、処理はステップS216に移行し、変数nが変数N(t—1)の値を超えていれば、処理は、ステップS230に移行する。
【0101】
ステップS216においては、演算部302は、変数mと所定の成分数Mとを比較し、変数mが変数M以下であれば、処理はステップS220に移行し、変数mが変数Mの値を超えていれば、変数mを1に再設定し、変数nを1だけインクリメントして(S218)、処理は、ステップS214に移行する。
【0102】
ステップS220では、変数n′の値が1だけインクリメントされ、さらに、パラメータの値が以下の式により更新される(S222)。
【0103】
【数25】
JP0004389046B2_000026t.gif

【0104】
さらに、変数mの値が1だけインクリメントされて(S224)、処理は、ステップS216に移行する。
【0105】
図7を参照して、演算部302は、以下の通りパラメータ集合を混合比に関して降順のソートを行なう(S230)。
【0106】
【数26】
JP0004389046B2_000027t.gif

【0107】
さらに、変数N(t)の値を以下のようにして更新する(S232)。
【0108】
【数27】
JP0004389046B2_000028t.gif

【0109】
次に、演算部302は、以下のように混合比の大きな順序でパラメータ集合の番号の付け替えを行なう。
【0110】
すなわち、演算部302は、まず、変数nを1に設定した後(S233)、変数nと定数N(t)とを比較し(S234)、変数nが定数N(t)以下である場合、以下の式に従い、パラメータ集合の番号を付け替える。
【0111】
【数28】
JP0004389046B2_000029t.gif

【0112】
その後、変数nの値を1だけインクリメントして(S238)、処理はステップS234に復帰する。
【0113】
ステップS234で、変数nが定数N(t)を超える場合、このような番号の付け替えが終了して、以下の事後分布PTDが得られる(S240)。
【0114】
【数29】
JP0004389046B2_000030t.gif

【0115】
さらに、以下の式により、状態ベクトルx(t)の期待推定量が得られる(S242)。
【0116】
【数30】
JP0004389046B2_000031t.gif

【0117】
タイムステップの変数tが1だけインクリメントされて、処理がステップS202に復帰する。ステップS202において、変数tが所定の定数Tを超えていれば、自己位置の同定処理が終了する。
【0118】
このような自己位置の同定が行えれば、CR100の演算部302は、たとえば、同定された自己位置を利用して、CR100の移動動作を制御することができる。
[計算機シミュレーション結果]
実環境を計算機上で模倣したサイバーローデントシミュレータ(Cyber Rodent Simulator)を使って、上述したような自己位置の推定法のプログラムを実装し、その性能を評価した結果について、以下に説明する。
【0119】
まず、各試行においてCR100の初期位置および初期進行方向をランダムにセットした。そして、2つの行動命令ForwardとRotationが等確率でランダムに選択されるものとした。ただし、壁や障害物に衝突しそうな場合には必ずRotationが選択されるものとした。選択された行動命令が実行されると、CR100は遷移後の(未知の)状態でセンサ情報を観測し、自己位置同定アルゴリズムにしたがって現在の状態を推定した。この行動選択と状態推定の手続きを1000時間ステップまで交互に繰り返し、1回の試行が終わるものとした。
【0120】
確率的nRBFネットワークを構成するガウス関数成分の数はM=1728とし、各パラメータは以下のようにセットした。
【0121】
【数31】
JP0004389046B2_000032t.gif

【0122】
事後分布を近似するためにCR100が保持できるガウス関数成分の最大数はNmax=40に設定した。
【0123】
初期状態x(1)に関する事前知識は全くないので、その事前分布はほぼ無情報分布となるように以下の混合ガウス分布で与えられるものとした。
【0124】
【数32】
JP0004389046B2_000033t.gif

【0125】
状態遷移の際に生じるノイズの共分散行列は、Q=diag(1,1,0.01,0.01)で与えられるものとした。
【0126】
また、本発明の自己位置の同定方法の有効性を示すために、300個のランダムサンプル(パーティクル)を使ったMCL法もシミュレータに実装し、2つの手法の性能を比較した。
【0127】
以下では、本発明の自己位置同定法を混合カルマンフィルタ法(Mixture Kalman Filter based method; MKL法)と呼ぶ。
【0128】
図8は、MKL法とMCL法での位置推定誤差の時間経過を示した図である。図8では、位置に関する推定誤差ε(t)を以下の式で評価し、その時間経過を示している。
【0129】
【数33】
JP0004389046B2_000034t.gif

【0130】
ここで、推定誤差ε(t)の式において、以下の変数は、推定された状態ベクトルx(t)のi番目の要素を表している。
【0131】
【数34】
JP0004389046B2_000035t.gif

【0132】
図9は、MKL法とMCL法での進行方向に関する推定誤差の時間経過を示した図である。図9では、進行方向に関する推定誤差δ(t)を以下の式で評価し、その時間経過を示している。
【0133】
【数35】
JP0004389046B2_000036t.gif

【0134】
ここで、以下の関係が成り立つ。
【0135】
【数36】
JP0004389046B2_000037t.gif

【0136】
CR100に搭載されている赤外線近接センサは、たとえば、そこから25[cm]以内にある障害物や壁を検知できるものとした。
【0137】
そこで、図10は、ε(t)<25[cm]を満たす試行の割合を自己位置推定の成功率として評価し、その時間経過を示した図である。
【0138】
推定をはじめたばかりの初期段階では、MKL法はMCL法に比べて推定精度が劣っているものの、時間が経過することにしたがってその差はすぐに縮まってくる。
【0139】
そして、500時間ステップ以降では、MCL法に比べて、MKL法の推定精度は良い平均性能を示すようになり、かつ、推定分散も小さくなることが分かる。
【0140】
また、インテル社製CPU(Pentium(登録商標)4 クロック周波数:2.2GHz,キャッシュメモリ:512KB)、メインメモリ:512MBのコンピュータで計算した場合、1時間ステップあたりに要する計算時間はMCL法が、約1.1秒要するのに比べて、MKL法は0.8秒となった。
【0141】
これらの結果は、本発明のMKL法が従来のMCL法に比べて効率が良く、推定精度も良い計算アルゴリズムであることを示している。
【0142】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【図面の簡単な説明】
【0143】
【図1】CR100の外観を示す図である。
【図2】CR100が自律的に動き回ることができるフィールドの一例を示す図である。
【図3】本発明の自己位置同定方法を用いた制御装置が適用される自律型移動ロボットCR100の構成の一例を示す機能ブロック図である。
【図4】図3に示したCR100の上部から見た距離測定の方向(センサの方向)を示す概念図である。
【図5】CR100の自己位置の同定処理を実行させるための計算手続きを示す第1のフローチャートである。
【図6】CR100の自己位置の同定処理を実行させるための計算手続きを示す第2のフローチャートである。
【図7】CR100の自己位置の同定処理を実行させるための計算手続きを示す第3のフローチャートである。
【図8】MKL法とMCL法での位置推定誤差の時間経過を示した図である。
【図9】MKL法とMCL法での進行方向に関する推定誤差の時間経過を示した図である。
【図10】自己位置推定の成功率の時間経過を示した図である。
【符号の説明】
【0144】
100 サイバーローデント、202 カメラ、204.1~204.6 赤外線近接センサ、211 右車輪、212 左車輪、213 右駆動モータ部、214左駆動モータ部、220 バッテリ、230 電源回路部、300 制御部、302 演算部、304 記憶装置、310,312 インタフェース。
図面
【図3】
0
【図5】
1
【図6】
2
【図7】
3
【図1】
4
【図2】
5
【図4】
6
【図8】
7
【図9】
8
【図10】
9