JP5444952B2

JP5444952B2 - センサフュージョンによる地図の自動生成、およびそのように自動生成された地図を用いて移動体の移動をするための、装置、方法、ならびにプログラム

Info

Publication number: JP5444952B2
Application number: JP2009197532A
Authority: JP
Inventors: 彬陳
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 2009-08-28
Filing date: 2009-08-28
Publication date: 2014-03-19
Anticipated expiration: 2029-08-28
Also published as: JP2011048706A

Description

本明細書にて後述する実施形態は、地図の自動生成と、そうして自動生成された地図を用いた移動体の移動とに関する。

移動体（自律ロボットなど）が自律的に移動をしながら、地図（環境地図）を自動生成する技術は、SLAM（Simultaneous Localization And Mapping）法として汎く知られている。

測距センサを用いたSLAM法では、移動体中の状態推定系が、測定時間単位ごとに取得した断片的環境のレンジポイントを繋ぎあわせることで、地図の生成を行っている。つまり、レンジポイントの空間的距離を求めて、対応付けた点群として座標を算出することで、地図を生成するということである。

あるいは、ビジョンセンサを用いたSLAM法では、ステレオビジョン法を用いて得られる環境内ランドマークの位置データに基づいて、地図の生成を行う。

特開２００５‐０７７１３０号公報特開２００７‐０２４７３１号公報特開２００４‐３６２４６９号公報

Sebastian Thrun, Wolfram Burgard, and Dieter Fox, "Probabilistic Robotics", The MIT Press, 2006

概して、単一種類のセンサだけを用いた測定から得られる地図は誤差が大きくなりやすい。これは、センサがそれぞれ得意とする環境が異なるためである。

例えば、構造的な特徴が一様である環境（特徴の乏しい整然とした廊下など）の場合、測距センサは正確な距離測定が難しくなり、生成される地図も不安定になってしまう。

また、特徴が遠距離にのみ在る場所（広場など）でビジョンセンサのみを使って状態推定をする場合、ビジョンセンサは遠距離の計測精度が近距離のそれに較べ劣るため、やはり地図の精度は落ちてしまう。

つまりこうした従来の方法で生成した地図にはひずみが多く、環境に即した精度が低いという問題があった。
精度が低い、すなわち現実からかけはなれた地図が生成されてしまうと、移動体が「そこに無い筈の壁」に衝突してしまうなどといった事故の原因となりうる。

当該技術分野においては、より精度の高い環境地図を作成しつつ、同期的に移動体のその環境内での移動に反映する手法が希求されている。

本発明の実施形態では、環境を表す地図を用いて、前記環境内を状態推定をしながら移動する移動体を制御するための装置であって、
(i) 前記地図を格納する、記憶手段と、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定し、前記記憶手段に格納して、格納した前記状態推定データの更新を行う、状態推定データ格納・処理手段と、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむ、測距手段と、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむ、画像取込手段と、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算する、残差算出手段と、
(vi)前記移動体を動かし、動かした移動量を出力する、移動手段と
を含み、
前記状態推定データ格納・処理手段が、前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新し、
前記測距手段が、前記測距データを用いて前記地図を更新し、
前記移動手段が、更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節する
という装置が提供される。

また、そうした効果が得られる方法とプログラムも、本発明の或る実施形態において提供される。

本実施形態にかかる装置、方法、もしくはプログラムにより、測距手段と画像取込手段という性質の異なるセンサ手段によるセンサフュージョンを行うことができる。こうしたセンサフュージョンにより、測距手段から取得したレンジポイントの対応付けの精度を向上させて、精度の高い地図を作成できる。そして同期的に、その地図を移動体の移動に反映させることで、移動の精度も向上できる。

実施形態にかかる移動体 100 のおおまかな構造を示す斜視図である。装置 200 の機能的な側面を説明するためのおおまかなブロック図である。パーティクルフィルタを用いた、装置 300 の構造を示すブロック図である。測距センサが出力可能な測距データの一例を示す図である。ビジョンセンサが出力可能な画像データの一例を示す図である。実施形態にかかる残差の算出方法を説明する概要図（その1）である。実施形態にかかる残差の算出方法を説明する概要図（その2）である。実施形態にかかる残差の算出方法を説明する概要図（その3）である。実施形態にかかるパーティクルフィルタを用いた状態推定系の動作を説明するフローチャートである。実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。

以下、本発明の実施形態を図面に基づいて詳細に説明するが、これは本発明を何ら限定するものではない。

〔定義〕
本明細書において使用する術語についてこれより解説を行う。

「移動体」（"vehicle"）とは例えば、駆動装置を具えた移動可能なロボット（自律ロボットなど）のことを含むが、これには限定はされない。例えば状態推定系を、人間が操縦する移動機械系（車輌など）に搭載したものを移動体と称することもまた可能である。

「自律ロボット」（"autonomous robot"）とは、人間の介入を（初期設定やメンテナンスを除き）基本的に必要とすることなく自律的に稼動できるロボットのことを指す。自律ロボットは例えば、或る環境内を移動するにあたり、実施形態にかかる状態推定系を使いつつ、かつ人間からの介入を要さないという動作（"自律航法"）を実施可能である。とはいえ、自律ロボットに対して、人間の介入を行うことができないというわけではないことに留意されたい。

「環境」（"environment"）とは、移動体がその中を測定し移動することになる世界のことを指す。実施形態では、当該技術分野にて公知なあらゆる種類の環境を想定している。実施形態にて想定される環境としては例えば、整然とした環境（廊下など）、整備されていない雑然とした環境（研究室内など）、もしくは或る測距センサが計測範囲内に対象物を捉えにくい環境（屋外の広場など）といったものがあるが、これらに限定はされない。

「地図」（"map"）もしくは「環境地図」（"environmental map"）とは、状態推定を行う対象である系（移動体など）が関与する環境についてのデータの集合体のことを指す。こうした地図としては、location-based（位置情報ベース）な地図とfeature-based（特徴ベース）な地図との二種類が含まれる。location-basedな地図はvolumetric mapともいい、環境内のすべての点についてのラベルを有している。つまり、location-basedな地図では、物体（障害物や壁など）が（座標上の）或る点に存在するかどうかについての情報のみならず、何もない箇所（free space）が"在る"という情報も有している。一方、feature-basedな地図は、環境の形状を指定する地図といえる。つまりこれは、目標物（ランドマークともいう）の位置（極座標表示など）を有する地図である。

「センサ」（"sensor"）とは、当該技術分野において公知である任意の種類のセンサのことを指す。そうしたセンサは、当該技術分野では、内界センサと外界センサとに大別される。センサの例としては、姿勢センサ、タコメータ、ロータリーエンコーダ、レートジャイロ、歩幅計、加速度計、位置センサ、変位センサ、加速度センサ、測距（距離）センサ、方位センサ、触覚センサ、力覚センサ、視覚（ビジョン）センサ、ステレオビジョンセンサ、近接覚センサ、超音波センサ、赤外線センサ、傾斜角センサ、地磁気センサ、レーザーレンジファインダー（距離計）、2Dエリアスキャンセンサ、3Dエリアスキャンセンサなどが含まれるが、これらに限定はされない。

「内界センサ」（"internal sensor"）とは、ロボットなどの系の内部の状態を知るためのセンサのことを指す。

「外界センサ」（"external sensor"）とは、ロボットなどの系の外部の状況を計測し、系の行動のための判断材料とするためのセンサのことをいう。なお、上述した内界センサと外界センサの区別は排他的なものとはかぎらず、同種のセンサを双方に用いることもまた可能である。例えば、ビジョンセンサを内界センサにも外界センサにも使ってもよい。

「センサノード」（"sensor node"）とは、上述したセンサを含んだ装置のことをいう。センサノードは例えば、センサへの入出力を司るユニットや、情報（データ）処理ユニットや、無線・有線通信ユニットなどを有してもよい。またセンサノードは、実施形態にかかる装置測位装置を有することも、実施形態にかかる装置測位方法もしくはプログラムを実行することも可能である。本明細書では、「センサノード」のことを略して「センサ」と表記することがある。

「センサフュージョン」（センサ統合； "sensor fusion"）とは、複数のセンサの出力を統合的に処理して用いる手法のことをいう。そうしたセンサフュージョンには、複数個の同じ種類のセンサを用いてもよいし、あるいは、異なる種類のセンサをそれぞれ任意の個数だけ用いてもかまわない。本明細書において "統合（処理）" といったとき、それはセンサフュージョン（の一部）と同義であると捉えてもらってかまわない。

「測距データ」とは、測距センサ（「測距計」とも称する）から得られる距離についてのデータのことをいう。測距データは例えば、測定装置からレンジポイントまでのベクトルである。なお「レンジポイント」とは、測距センサが使うビーム（レーザーレンジファインダーが用いるレーザービームなど）が、障害物（壁や天井など）に当った点のことを指す。こうしたレンジポイントを複数まとめて、「レンジデータ」とも称する。また本明細書では、測距データと同義に「レンジデータ」という語も用いることもある。

「ビジョンデータ」とは、視覚（ビジョン）センサから得られる画像データのことをいう。本明細書では、「ビジョンセンサ」という語と「カメラ」という語を同義に使用することがある。

「ステレオビジョン（法）」（"stereo vision"）とは、複数のビジョンセンサ（まとめて"ステレオカメラ"とも称する）を用いた、ランドマーク（となる物体）の空間位置を計算する手法のことをいう。ステレオビジョン法では、あらかじめ各ステレオカメラの物理的な設置位置の違いから生じる視差（いわゆる「両眼視差」、すなわち "binocular parallax" にあたる）を求めておく。そして、各ステレオカメラが環境の同じ箇所をランドマークを含めるようにして撮影する（ここでの "同じ" という語は便宜上の表現であり、厳密な同一性を言っているわけではないのは、ステレオカメラの構造を知る当業者にとっては自明のことであろう）。得られた複数の画像のうちから、基準となる画像（基準画像； "base image"）を定めて、それ以外を「参照画像」（"reference image"）とする。そして、基準画像に写っているランドマークが、参照画像のどこに写っているのかを画像処理によって求める。すると、その画像上での位置のずれと、視差と、環境を定めている地図とを用いて、ランドマークが環境内のどこに在るのかを幾何学的に（例えば三次元的に）計算することができる。こうした「どこに写っているか」を実現するための処理としては、公知技術である対応点検索法を使用できる。なお対応点検索法とは、一方の画像上のピクセルに対応するピクセルが、他方の画像の何処にあるのかを探す手法である。

「ランドマーク」（"landmark"）とは、センサにとって測定の目印となりえる特徴のことをいう。センサの種類によってランドマークは異なってくる。例えば、ビジョンセンサであれば、特徴的な形状の物体をランドマークとすることが可能である。

「オドメトリ」（"odometer"）とは、移動体の持つ内界センサ（駆動装置に付随するセンサなど；オドメータともいう）から得られるデータ（"オドメトリデータ"）に基づいて、移動体の姿勢と位置を求める手法を指す。オドメトリデータとしては例えば、移動体の移動方向、移動距離、もしくは移動速度、またはそれらの組み合わせ、に関するデータが含まれる。オドメトリの一例として、車輪を二つ持つ移動体の場合、左右の車輪の回転速度から移動体の並進速度と角速度を求め、それらを積分して移動体の位置と姿勢を求める手法がある。その他、任意の内界センサを使って移動体の並進速度と角速度を求め、それらを積分して移動体の位置と姿勢を求めてもよい。なお「オドメトリモデル」とは、オドメトリデータの集合体のことをいう。また、オドメトリモデルを略して単に「オドメトリ」と称する場合もある。

「姿勢」とは、その系（移動体など）のとっている体勢のことをいう。例えば姿勢といえるパラメータとしては、ロール角、ピッチ角、ヨー角などがある。

「（系の）状態」（"state"）とは、環境を特徴づける変数のことであり、例えば環境中の系の位置や移動速度のことをいう。

「状態推定（法）」（"state estimation"）とは、系の状態を推定するための手法のことをいう。状態推定にあたっては例えば、Kalmanフィルタ、拡張Kalmanフィルタ、相補フィルタ、またはパーティクルフィルタといった時系列フィルタを使用可能である。また本明細書では、「測位」という語を、「状態推定」という語と同義に使用することがある。

「信念」（"belief"）とは、時刻tにて得られた計測データz_tを条件とした、状態x_tの事後確率のことをいう。信念 bel(x_t) = p(x_t | z_t) である。

「パーティクル」（"particle"）とは、或る状態推定系の属する位置や方位などを推定する仮説に関する情報（データ）のことを指す。言い換えれば、パーティクルとは事後確率分布（posterior distribution）の標本である。こうしたパーティクルのことを単に「仮説」や「粒子」と呼ぶこともある。例えばパーティクルには、位置推定対象物の状態（姿勢や速度など）を表すデータを含めることができる。またパーティクルとしては例えば、位置推定対象物の位置ベクトルと、位置推定対象物の速度ベクトルと、そのパーティクルが持つ重み（または評価値）と、を含んだデータを使用できるが、これに限定はされない。

「パーティクルセット（パーティクル集合）」（"particle set"）とは、任意の個数のパーティクルの組のことをいう。実施形態では、或るモジュールに属するパーティクルをまとめて、パーティクルセットとして扱うことができる。そうしたパーティクルセットに含まれるパーティクルの個数は、パーティクルを処理する系の能力や機能、要求される自己位置推定の精度などに応じて任意に設定できる。例えば或る実施形態では、1個以上、10個以上、10²個以上、10³個以上、もしくは10⁴個以上のパーティクルを含んだパーティクルセットを設定できるが、これらに限定はされない。

「パーティクルフィルタ」（"particle filter"）とは、時系列フィルタの一種であって、上述したパーティクルを用いて系の状態推定を行うために使用できるものである。またパーティクルフィルタはBayesフィルタの一種でもある。パーティクルフィルタを実現するアルゴリズムとしては例えば、パーティクルの尤度を重みとして、その重みに基づいたリサンプリング（復元抽出）を行い、パーティクルを予測に基づいて移動させ、現在の観測（測位）結果とパーティクルの状態から尤度を計算する、といったものがある。

「パーティクルの淘汰（新陳代謝／更新）」または「パーティクルのリサンプリング」とは、何らかの基準に満たないパーティクルを処理の対象から外し（捨て）、新たなパーティクルを得ることをいう。ここではわかりやすく説明するため、パーティクルセットにパーティクルが五個だけある例を引いてみる。そしてパーティクルの評価値が順に0.5、0.5、0.9、0.7、0.4だったとする（あくまで簡単な例であることに留意されたい）。すると平均値（mean）は0.6となる。仮に淘汰する条件が平均値未満であることだとすれば、このパーティクルセットのうちでは二個のパーティクルだけを残すように淘汰が行われることになる。もちろんこの淘汰条件は一例であり、中央値を使った条件や複数種の評価値を使う条件についても想定されている。そして、パーティクルセットのうち特定のもの（重みの大きなものなど）だけを残してパーティクルを淘汰したうえで、残ったパーティクルをその重みに基づいて新たなパーティクルを "播く" 。例えば、重みが大きいパーティクルほど、その重みに比例して多くの新たなパーティクルの元となることができる。直近の例での評価値が重みであるとすれば、評価値が0.9のパーティクルに基づいて三個の新たなパーティクルを、評価値が0.7のパーティクルに基づいて二個の新たなパーティクルを、それぞれリサンプリングできる（もちろんこの手法は一例に過ぎない）。こうした新たなパーティクルの取得数は、簡単な例でいえばテーブルに評価値と個数の対として定めておくこともできる。このように新たなパーティクルを作成する際には、元となったパーティクルをそのままコピーしてもよいし、何らかの変更を加えたうえで作成してもかまわない。

「パーティクルの良さ」とは、そのパーティクルが推定している系の状態と、その系の "現実" の状態（観測結果など）とのマッチングの度合の高さのことをいう。そうした度合は当該技術分野において公知なさまざまな手法を使って算出可能である。後述する実施例では、そうした手法を例示してある。無論、そうした系の "現実" が、人間が同じ系に対して知覚できる現実とは異なっている場合もあるのは、当業者には理解できることだろう。当然のことながら、状態推定系が複数存在すれば、その認識する "現実" はそれぞれ異なってくるであろう。

「残差」（"residual"）とは、本明細書では或るデータと別の或るデータの差分のことをいう。例えば残差とは尤度の残差（尤度残差）のことを指す。あるいは、観測されたデータと、モデルから推定されたデータとの差を、残差ということもある。残差が大きいほど、比較されたデータ間の違いも大きいということになる。

「動的計画（法）」（"dynamic programming (DP)"）とは、Bellmanの最適性の原理に基づいて、多変数最適化問題を解くための手法をいう。本明細書に記載された実施形態では、動的計画法を実現するためのアルゴリズムを用いることができる。本明細書では、動的計画法を用いて二種のデータ α, β から残差 D を算出して得ることを、 D = φ(α,β) と表現することがある。

「破綻」（"failure"）とは、ここでは状態推定系が自己位置を（一時的であれ）推定できなくなることをいう。例えば、近距離しか計測できない測距センサだけを搭載した自律ロボットが広場に進出したとする。そして測距センサの測定可能距離・角度内に測定できる対象物が何も無くなってしまったとする。すると、そのロボット（の状態推定系）は "破綻" することがある。このような破綻をきたしたロボットは、望ましくない方向に誤って移動することがあり、事故の原因となりうる。なおこの場合、 "誤った" のはあくまでも外部の系（ユーザーとしての人間など）から観測してわかることであり、自律ロボットの持つ状態推定系が自身が "誤った" と判断できるとはかぎらない。

〔移動体〕
ここより、本発明の実施形態を実施可能な程度に詳細に説明してゆく。
図１は、実施形態にかかる移動体 100 のおおまかな構造を示す斜視図である。移動体 100 は、センサノードとして、二基のビジョンセンサ 110, 120 （"ひとつのステレオカメラ"）と、測距センサ 140 を有する。この実施例では、ビジョンセンサ 110, 120 に関する光軸 130 と、測距センサ 140 に関する光軸 150 とは平行であると仮定してある。当然のことながら、二つの光軸が平行でない場合も想定されている。またこの実施例では、簡単のため、図１に示した瞬間には、移動体 100 は、光軸 130, 150 と平行な方向に進んでいるものと仮定する。つまり、ビジョンセンサ 110, 120 および測距センサ 140 は、移動体 100 の進行方向をほぼ向いているということになる。

ビジョンセンサ 110 とビジョンセンサ 120 は、光軸 130 を挟んで横に並んでいる。なおここで "横（方向）" というのは、便宜的に、移動体が進む走行面に対して水平な方向であってかつ光軸に垂直な方向と定めたものである。このように横に並んだビジョンセンサからなるステレオカメラでは、横方向に特異的な特徴をランドマークとして使用しやすいという効果が得られる。具体的には、縦に立った壁の屈曲部（エッジ）をランドマークとしやすいという効果を得ることができる。なおここで "縦（方向）" というのは、便宜的に、前述した走行面に対して垂直な方向であってかつ光軸に垂直な方向のこととする。つまり「縦に立った壁」というのは、日頃人間が生活する環境でいうところの壁のことだと想定してもらってもいい。

別の実施形態では、ビジョンセンサ 110, 120 が、縦に並んだ二基のビジョンセンサであってもよい。このように縦に並んだビジョンセンサからなるステレオカメラでは、縦方向に特異的な特徴をランドマークとして使用しやすいという効果が得られる。具体的には、天井に付いた、ステレオカメラから見て横方向に走る蛍光灯（のエッジ）といったものを、ランドマークとしやすいという効果が得られる。

さらに別の実施形態では、ビジョンセンサ 110, 120 の代わりに、三基のビジョンセンサを配置してもよい。その配置法としては、正三角形や二等辺三角形の頂点に各ビジョンセンサを並べるやりかたなどがある。こうした三角型の配置では、上述した横並べ式と縦並べ式の双方の効果を併せて得ることが可能である。

ビジョンセンサ 110, 120 でのランドマークの抽出にあたっては、任意の公知技術を使用でき、例えば勾配やラプラシアンを使った演算を用いることができる。

測距センサ 140 は、（この実施例では）光軸 130 と光軸 150 が平行であるので、ビジョンセンサ 110, 120 と概して平行に配置されている。そして、測距センサ 140 は何らかの測定手段を環境へ投射して、その環境からの反射の度合いから、レンジポイントについてのデータ（まとめて「レンジデータ」とも呼ぶ）を得る。例えば測距センサ 140 がレーザーレンジファインダであるなら、測定手段はレーザービームとなる。
この実施例では、測距センサ 140 は（複数本の）スキャンライン 142 に沿って測定手段を逐次投射してゆく。

以下に説明するように、移動体 100 は、ビジョンセンサ 110, 120 を用いたステレオビジョン法による複数の画像の取得と同期して、測距センサ 140 から得られた測距データをその複数の画像の各々に対応づける。そして測距データを対応づけた画像同士の残差を求めて、その残差を状態推定データの評価値（重み）とする。なおここで状態推定データとは、上述した状態推定法にて用いられるデータであり、例えばパーティクルである。おおまかに言えば、測距センサ 140 は近傍にある障害物の存在を検出するのに向いており、ビジョンセンサ 110, 120 は環境の奥行を検出するのに向いているといえる。これら二種のセンサを組みあわせることで、互いの長所を組み合わせ弱点を補うという効果が得られる。

図２は、実施形態にかかる装置 200 の機能的な側面を説明するためのおおまかなブロック図である。なおここでは一例として、装置 200 が移動体である場合を説明する。しかしながら他の実施形態では、移動体が装置200 の一部の要素を含み、その他の要素は移動体の外部の装置に含まれるようにもできる。例えば移動体が、測距センサ 230 とステレオカメラ 240 を含み、その他の要素は移動体の外部の別の装置が含むようにしてもよい。言い換えれば、装置 200 は移動体そのものであってもよいし、あるいは、有線もしくは無線で通信可能に接続された移動体と別の装置を含むものであってもよい。

装置 200 は、測定手段として、測距センサ 230 とステレオカメラ 240 を含む。また装置 200 は、測距センサ 230 とステレオカメラ 240 との同期をとった時系列的な測定（たとえば1秒おきの測定など）を可能にするためのクロックユニット 202 も含む。測距センサ 230 とステレオカメラ 240 は、上述した「外界センサ」に相当する。なお別の実施形態では、測距センサ 230 とステレオカメラ 240 がそれぞれクロックを有し、何らかの手法を以って各々を同期させることで、測距センサ 230 とステレオカメラ 240 の同期的な動作を実現させてもよい。なお、ステレオカメラ 240 は、（図１に関して上述したように）複数基のビジョンセンサを含むことができる。なお、ステレオカメラ 240 は、得た複数の画像のうちからいずれかを基準画像として定め、それ以外を参照画像として定めることができる。例えば、ステレオカメラ 240 が二基のビジョンセンサを含んでいるとした場合、一方のビジョンセンサが得て出力する画像を基準画像とし、他方が出力する画像を参照画像とすることができる。あるいは別の手法として、時間などの条件に応じて基準画像と参照画像を出力するビジョンセンサを切り換えるようにしてもよい。

また装置 200 は、データの保存をするための記憶ユニット 210 （図１０に関して後述する外部記憶装置 1008 などに対応）も含む。記憶ユニット 210 には、装置 200 （あるいは装置 200 に含まれる移動体）の居る環境についてのデータである地図 272 と、装置 200 の持つ移動手段（図２には示していない）から得られたデータであるオドメトリモデル 270 も含まれる。地図 272 としては、上述したlocation-basedな地図やfeature-basedな地図を使用できる。

装置 200 にはさらに、測距センサ 230 の出力した測距データを保存するための測距データ格納ユニット 232 と、ステレオカメラ 240 の出力した画像データを保存するための基準画像格納ユニット 242 および参照画像格納ユニット 244 とが含まれる。

装置 200 はまた、地図 272 と測距データ・基準画像・参照画像とを用いて残差を算出するための、残差算出ユニット 250 も含む。

装置 200 はまたさらに、残差算出ユニット 250 が算出した残差とオドメトリモデル 270 とから状態推定データを算出して格納し処理するための、状態推定データ格納／処理ユニット 260 も含む。

なお図２では簡単のため、移動体が二種類のセンサをひとつずつ持っていると記載してはあるが、これはあくまで一例であることに留意されたい。或る実施形態では、移動体が同じ種類のセンサを複数個有し、それらの出力を平均して用いてもかまわない。

また図２では、記憶ユニット 210 が一個だけ存在していてデータ（ソフトウェア的に実施されるユニットを含む）がすべてのその中に格納されているかのように記載されているが、これも一例に過ぎない。或る実施形態では、複数の記憶ユニットを用いて、データを分けて格納してもかまわない。

あるいは別の観点として、測距データ格納ユニット 232 と、基準画像格納ユニット 242 と、参照画像格納ユニット 244 とがそれぞれ個別の記憶装置を含むと考えてもよい。この場合、残差算出ユニット250 がそうした個別の記憶装置からデータを読みだして処理をしているのだと考えてもよい。状態推定データ格納／処理ユニット 260 についても同様のことが言える。

なおプログラムとして実施形態を実施する場合には、移動体（例えば装置 200 、もしくは装置 200 に含まれる移動体）に含まれる要素（の一部）を、記憶ユニット（図１０に関して後述する外部記憶装置 1008 など）に保存されたデータに基づいたものとできる。それらのデータは、バッファ（図１０に関して後述するメモリ 1002 など）に読み出したうえで、実行ユニット（図１０に関して後述するCPU 1000 など）によって実行できる。

以上述べた記法・画法上の留意点については、以降の図においても矛盾を生じないかぎりは同様に考慮されたい。

装置 200 が行う処理動作を以下に概括する。初期状態（t=0）において、あらかじめ状態推定データ格納／処理ユニット 260 には、状態推定データの初期値を入力しておく。また、地図 272 の初期値として、測距センサ 230 がt=0で得た測距データを用いる。

残差算出ユニット 250 は、基準画像もしくは参照画像に、測距データを対応づける。すなわち、或る種の"合成データ" を作成すると考えてもよい。あるいは、こうした対応づけたデータを予測したもの（予測した「未来」のデータ）を "合成データ" として得てもよい。こうした "合成データ" としては例えば、以下に列挙するデータがある。或る時刻に得られた基準画像データに測距データを対応づけたもの。或る時刻に得られた参照画像データに測距データを対応づけたもの。或る時刻での地図（地図データ）から未来の時点での測距データを予測して得て、その未来の時点に対応する時刻に得られた基準画像データもしくは参照画像データに対応づけたもの。

なおここで時刻の単位としては任意の単位時間を使用できる旨に留意されたい。つまり、時刻t-1と時刻tの間隔は、特定の時間に限定はされない。本実施形態では、クロックユニット 202 の設定により、1 secや1 msecや20 minなどの任意の単位時間を使用できる。

そして残差算出ユニット 250 は、或る時刻t（t>0）における残差 D を、前述した "合成データ" を複数種類用いて算出する。具体的には、或る合成データと別の合成データを（動的計画法などで）比較して残差を得ることができる。

状態推定データ格納／処理ユニット 260 は、状態推定データの初期値を設定して記憶ユニット（記憶ユニット 210 でもよいし別の記憶媒体でもかまわない）に格納する。そして状態推定データ格納／処理ユニット 260 はまた、オドメトリモデル 270 から得た時刻tでの移動体の移動量 u_t を使って、状態推定データの遷移を行う。ここで「遷移」とは例えば、状態推定データにて定められていた装置 200 の座標を、移動量 u_t だけずらす（座標に移動量 u_t を加算する）ことをいう。当然のことながらこれは一例に過ぎず、例えば移動量 u_t に何らかの重みをつけてから座標に加算してもかまわない。

また状態推定データ格納／処理ユニット 260 は、時刻tに関する残差 D を、状態推定データの評価値として定める。

そして、測距データ格納ユニット 232 は、時刻tでの測距データを、状態推定データに関する地図 272 に追加する。言い換えれば、環境内で移動体が進むことによって新たに計測することになった地点についてのデータを地図 272 に書き加えることで、地図 272 を更新するのである。このように地図 272 が更新されるということは、移動体が環境に関して新たに得た情報を用いて、環境を記述する手段である地図を再構築することを意味している。さらに言えば、状態推定データにより（例えば確率的に）推定される移動体の位置から、 "現実の環境" を計測したことになる測距データを、地図 272 に加えるのだと考えてもよい。つまり測距の原点が状態推定データによって定まるのだと考えてもらってもかまわない。（なぜなら、自律的に移動する移動体にとって、環境は、絶対座標系ではなく相対座標系で捉えるものであるからである。）

そのようにして再構築された地図 272 を用いた移動を行うことにより、移動体の移動の精度を高めることが可能となる。

そうしてから、状態推定データ格納／処理ユニット 260 は、状態推定データを評価値に基づいて更新する。

こうした処理を移動体の移動にともない時間的にくりかえすことで、より正確な地図とその地図に基づいた適切な移動が可能になるという効果が得られる。

図３は、パーティクルフィルタを用いた、装置 300 の構造を示すブロック図である。この実施形態では、状態推定データとしてパーティクルを使用する。装置 200 と同様に、装置 300 も移動体であってよいし、あるいは移動体を含んでもよい。

装置 300 は、状態推定系 303 と、走行制御系 380 とを含む。状態推定系 303 は、移動体の状態（移動体の位置や姿勢など）を推定するための系である。走行制御系 380 は、移動体の移動を状態推定に基づいて制御するための系である。

状態推定系 303 は、測距センサ 330 と、ステレオカメラ 340 と、クロックユニット 302 と、測距データ格納ユニット 332 と、基準画像格納ユニット 342 と、参照画像格納ユニット 344 とを含む。これらの要素の役割と機能は、参照番号の下二桁が等しい図２の構成要素と同様であるので、ここでは説明をくりかえさない。

また状態推定系 303 は、残差算出ユニット 350 およびパーティクルフィルタ処理ユニット 360 を含む。

残差算出ユニット 350 は、対応付けユニット 352 と、予測ユニット 356 と、計算ユニット 358 とを含む。対応付けユニット 352 は、画像情報抽出ユニット 354 を含む。

対応付けユニット 352 は、測距データを、基準画像もしくは参照画像と対応づける。なおここでいう"対応づける" とは、測距データに含まれるレンジポイントが、基準画像もしくは参照画像に写っているうちの何処にあたるのかを判断することを指す。対応付けユニット 352 は具体的には、測距データに含まれるレンジポイントの座標が、画像データの何処の点に相当するのかを判断する。例えば説明のためにごく簡単な例を述べると、測距センサの設置座標が (0,0,0) でステレオカメラの座標が (0,0,1) であったとする（つまり測距センサとステレオカメラの位置関係が既知であって座標系を共有できるとする）。すると、測距センサが得たレンジポイントの座標が (1,0,0) であったとしたら、その点がステレオカメラから見てx座標で1、z座標で-1離れた箇所に相当することは容易にわかる。また当然のことながら、測距センサの用いる座標系と、ステレオカメラが用いる座標系が別であって、その系間の変換規則を定めるようにしてもよい。見方を変えれば、対応付けユニット 352 は、その対応付けにあたり、測距センサ 330 からレンジポイントまでの投射線（スキャンライン）を、ステレオカメラ 340 が見ている空間に投影しているとも見做せる。そうして得られた対応付けたデータのことを、本明細書では「合成データ」とも称する。なお上述したように、本実施形態では、あくまで測距データは状態推定に基づいた原点からのものであることには留意されたい。

こうした「合成データ」としては例えば、画像データ上に複数のレンジポイントをプロットしたものが想定できる。あるいは、そうしたプロットしたレンジポイントを連結するように補間処理を施したものを含めた「合成データ」としてもよい。こうすることで、プロットしたレンジポイントがスパース（疎）であっても、測距データと画像データの適切な合成（すなわち、適切なセンサフュージョン）を行うことが可能となる。

対応付けユニット 352 に含まれる画像情報抽出ユニット 354 は、ステレオカメラ 340 から得た画像データから、画像情報を抽出する。そうした画像情報としては、色相、彩度、明度、もしくはエッジがあるが、これらに限定はされない。画像情報抽出ユニット 354 は画像データ中の任意の点から画像情報を得ることが可能である。例えば、上述したように画像データ上にプロットされたレンジポイントを連結する線上の点から画像情報を得てもよい。

予測ユニット 356 は、地図 372 を使い、測距データの予測を行う。具体的には、或る時点で移動体が居る地点について測距センサ 330 が観測を行ったら得られるであろう「予測測距データ」を、予測ユニット 356 は算出する。こうした「予測測距データ」は例えば、地図 372 から描かれる "空間" に測距センサ 330 のスキャンラインを投影するという"仮想的" な処理により得ることが可能である。こうした処理は、例えば座標系の変換によって行うことが可能である。そして予測ユニット 356 は、対応付けユニット 352 を介して得た基準画像もしくは参照画像に、そうした予測測距データを対応づけることで、「予測的な合成データ」を得ることが可能である。なお別の実施形態では、予測ユニット 356 が、対応付けユニット 352 を介さずに、基準画像格納ユニット 342 および参照画像格納ユニット 344 からそれぞれ基準画像データと参照画像データを得ることも可能である。

計算ユニット 358 は、対応付けユニット 352 から得た「合成データ」もしくは予測ユニット 356 から得た「予測的な合成データ」を複数種得て、それら同士の残差を計算する。残差の算出に際しては例えば、そうした「合成データ」から抽出した画像情報を使って残差を算出してもよい。

さらに状態推定系 303 は、記憶ユニット 310 を含む。記憶ユニット 310 は、記憶ユニット 210 とは違い、地図 372 を含むがオドメトリモデル 270 に相当するものは含んでいない。見方を変えれば、オドメトリモデル 270 は走行制御系 380 に相当するといえる。

パーティクルフィルタ処理ユニット 360 は、移動体の状態を推定するための情報であるパーティクルを含んだパーティクルセット 362 を有する。パーティクルセット 362 に含まれるパーティクルの初期値は任意に設定でき、例えば各パーティクルの持つ評価値（重み）の初期値はすべて均一にしてもよい。パーティクルフィルタ処理ユニット 360 は、前述した残差算出ユニット 350 からの出力と後述する自己状態記憶メモリ#1 386 からの出力を承けて、パーティクルセット 362 を更新する。具体的には、パーティクルフィルタ処理ユニット 360 は、自己状態記憶メモリ#1 386 から移動体の時刻t-1から時刻tにかけての相対移動量 u_tをパーティクルセット 362 （に含まれるパーティクル）の持つ値に加算できる（もしくは任意の演算を施せる）。またパーティクルフィルタ処理ユニット 360 は、残差算出ユニット 350 から得られた時刻tでの残差 D をパーティクルセット362 の評価値として設定できる。このとき例えば、i番目（iは自然数）のパーティクルの評価値を w_i とおくと、 w_i = e^(-D/T)とできる。なおここで e は自然対数の底、 T は正規化のための定数である。

なお図３では地図 372 が単独であるかのように記載してあるが、別の実施形態では任意の数の地図を記憶ユニット 310 が格納してもよい。例えば或る実施形態では、パーティクルセット 362 に含まれるパーティクルの各々について独立に、地図 372 が存在してもよい。

以下にパーティクルの評価値についての具体的な例示を行うが、本発明の実施形態はこれには限定はされない。

パーティクルセットをX_tとおくと、
X_t:= x_t ^[1], x_t ^[2], ... , x_t ^[M] (1)
と表せる。ここでMはパーティクルの個数である。おのおののパーティクル x_t ^[m](1≦m≦M) は、時刻tでの状態に関するひとつの仮説である。

或る状態のパーティクル（仮説）x_tがパーティクルセットX_tに含まれる尤度（likelihood）は、理想的にはBayesフィルタの事後信念（posterior belief） bel(x_t) に比例する。すなわち、パーティクルx_tは
x_t ^[m]〜 p(x_t | z_1:t,u_1:t) (2)
となるように選ばれるべきである。ここでp(A | B)はBayesの条件付き確率（conditional probability）であり、或る事象Bが起こるという条件下での別の事象Aの起こる確率である。

時刻tのパーティクルx_t ^[m]は、以下のようにサンプリング（採取）できる。
x_t ^[m]〜 p(x_t | u_t,x_t-1 ^[m]) (3)

パーティクルx_t ^[m]に対する評価値（重み）w_t ^[m]は、以下のように計算できる。
w_t ^[m]= p(z_t | x_t ^[m]) (4)
ここでz_tは時刻tでの計測（measurement）データであり、例えば上述したオドメトリモデルに相当する。

つまり、w_t ^[m]により重みづけられたパーティクルからなるパーティクルセットは、Bayesフィルタの事後信念 bel(x_t) の近似表現となる。

図３の説明に戻る。走行制御系 380 は、内界センサ 382 と、自己状態推定ユニット 384 と、自己状態記憶メモリユニット#1 386 と、自己状態記憶メモリユニット#2 388 とが含まれる。

内界センサ 382 は、移動機構 390 などを測定し、その結果となるデータは自己状態推定ユニット 384 へと送られる。

自己状態推定ユニット 384 では、内界センサ 382 の出力を基にして、移動体の状態推定を行う。自己状態推定ユニット 384 は例えば、地図（図３に示していないものでもよいし、あるいは、地図 372 を使用してもよい）を参照して、移動体の位置を推測することができる。あるいは、移動体の速度や、向いている方位を推測することも可能である。

自己状態推定ユニット 384 の出力は、自己状態記憶メモリユニット#1 386 と自己状態記憶メモリユニット#2 388 とへ送られる。このとき、双方へ同じ内容のデータを送ってもよいし、もしくは異なる重みをつけて送信してもよい。同じ内容のデータを用いる場合には、初期の時点（すなわち、装置 300 の稼動直後）では、自己状態記憶メモリユニット#1 386 と自己状態記憶メモリユニット#2 388 とは同じデータを持つことになる。その後、自己状態記憶メモリユニット#1 386 からの出力が状態推定系 303 へ送られてやがて自己状態記憶メモリユニット#2 388 へとフィードバックされると、内界センサ 382 の出力結果を補ったより精度の高い状態推定が可能になるという効果を得ることができる。

〔センサの出力データ〕
図４は、測距センサ（測距センサ 230 など）が出力可能な測距データの一例を示す図である。例えば測距センサがレーザーレンジファインダである場合、センサから発せられたレーザービームが環境中の障害物（壁や天井など）にヒットし、レンジポイントの座標を得ることができる。

以下に掲げるのは、三次元座標であるデータ（レンジデータ）の例である。

ｘｙｚ
0.813691, -3.412359, 0.302000
0.875252, -3.412136, 0.302000
0.940315, -3.426451, 0.302000
1.001578, -3.420039, 0.302000
1.063323, -3.414471, 0.302000
1.125890, -3.410684, 0.302000
1.192166, -3.418247, 0.302000

図４の例では、こうして得られたレンジデータのx座標とy座標を床に投影して、二次元的なデータとしている。しかし実施形態はこれには限定はされず、三次元的なデータをそのまま測距データとしてもかまわない。

また、図４では、移動体（自律ロボットなど）の位置を丸で示してあり、丸についた短い直線が移動体の向きを示している。
図５は、ビジョンセンサ（ステレオカメラ 240 など）が出力可能な画像データの一例を示す図である。パネル(A)とパネル(B)にそれぞれ基準画像と参照画像が表示されている。これらの画像は、パネル(C)の平面地図に対応したものである。

パネル(A)とパネル(B)には、実線で画像データが示してある。また、破線で測距センサがスキャンを行ったライン（すなわち、スキャンラインの投射軌跡）を示してある。この軌跡上に、レンジポイントがプロットされることになる。また図５のパネル(C)では、移動体（自律ロボットなど）の位置を丸で示してあり、丸についた短い矢印が移動体の向きもしくは進行方向を示している。

〔センサフュージョン手法〕
図６は、実施形態にかかるセンサフュージョンの手法のうち、「経時変化を観る」ための残差 D_b の導出手法を説明するための概要図である。ここでは、移動体600 が時刻t-1から時刻tにかけて得る測距データの変化を観ていると考えてもらってもよい。

図６のパネル(A)には、移動体 600 の持つ測距センサから環境（網掛け部分）へ投射されたスキャンラインを放射状に描いてある。環境とスキャンラインの交点がレンジポイントに相当する。なお図６では環境を簡単のため二次元的に描写しているが、これは表現上の都合に過ぎず、測距データと画像データを二次元に限定することを意図しているわけではない。パネル(A)にはまた、画像データ（この例では基準画像データ）と測距データ S_t-1を対応づけた合成データ l^b _t-1 610 を模式的に示してある。ここで記号 "l" （英小文字エル）に付した上付き文字は、その合成データが基準画像データ（baseの "b"）であるか参照画像データ（referenceの "r"）であるかを示すものである。また記号 "l" に付した下付き文字は、その合成データの属する時刻を示している。 l^b _t-1 610 には、投影されたレンジポイント（この例では(K+1)個；Kは0以上の整数）が含められており、そのレンジポイント間が補間処理されて線で接続されている。なおKの値は、測距センサの設定と性能により任意に定めることが可能である。つまり、そうした"補間線" は、測距センサのスキャンラインの軌跡がつくるスキャン平面が、環境空間を切断する面を、二次元的に投影した線にあたる。なお図６では紙幅の都合上、 l^b _t-1 610 にはレンジポイントと補間線しか描いていないが、実際にはビジョンセンサ（ステレオカメラ 240 など）が取得した環境を写した画像も含まれていることになる（図５も併せて参照されたい）。

さて図６のパネル(B)には、単位時間だけ時間が進んで時刻tになったときの移動体 600 の状態を示してある。移動体 600 は、 u_t だけ移動したことになる（これは簡易的な表現であり、実際にはu_t には位置座標のほか姿勢などに関する情報も含めることができる）。移動体 600 の持つ測距センサから環境へ投射されたスキャンラインを放射状に描いてあるのはパネル(A)と同様である。しかし移動体 600 は移動しているので、ビジョンセンサから見える環境は異なっている（換言すれば、違った位置から同じものを見るため見かけが変わる）。そしてパネル(B)にはまた、画像データ（この例では基準画像データ）と測距データ S_tを対応づけた合成データ l^b _t 612 を模式的に示してある。 l^b _t 612 にも、(K+1)個のレンジポイントが投影されている。

そして図６のパネル(C)には、 l^b _t-1 610 と l^b _t 612 の残差を求める手法を示してある。この例では、補間線に沿って画像情報（例えば、色相、彩度、明度、もしくはエッジ）を抽出している。パネル(C)では、画像情報を長さKの軸に沿わせて展開した曲線として描いてある。ここでグラフの縦軸は l^b _t-1 610 に、横軸は l^b _t612 に対応し、グラフのグリッドはレンジポイントの番号（0, 1, ..., K）に対応している。画像情報抽出ユニット（画像情報抽出ユニット 354 など）は、 l^b _t-1 610 から、補間線に沿って色相 hue_t-1 、彩度 sat_t-1 、明度 v_t-1 、エッジ dI_t-1 を抽出する。同様に画像情報抽出ユニットは、 l^b _t 612 から、補間線（もしくはその近傍）に沿って色相 hue_t 、彩度 sat_t 、明度 v_t 、エッジ dI_t を抽出する。抽出したデータは記憶ユニット（記憶ユニット 310 など）に保存可能である。なおエッジ dI は、従来技術により、例えば画像データを明度画像（グレースケール画像）に変換して得ることが可能である。

そして動的計画法により、 D_b = φ(l^b _t-1,l^b _t) として残差を求める。実際には、 l^b _t-1 もしくは l^b _t から抽出された画像情報を使って残差を求めている（例えば hue_t-1 とhue_t から求めている）と考えてもよい。

さらに動的計画法について詳述する。
実施形態にかかる動的計画法は、例えば従来技術である音声マッチングと同様の手法（画像データの勾配やラプラシアンを用いた演算など）をとってもよい。あるいは、以下に示すアルゴリズム1-3を用いてもよい。

{アルゴリズム1：動的計画法の初期設定}
まず記号の説明として簡単な例から説明する。 k₁ = (-1,0), k₂ = (-1,-1), k₃ = (0,-1) とすると、
{i,j} + k₁ = {i-1,j}
{i,j} + k₂ = {i-1,j-1}
{i,j} + k₃ = {i,j-1}
となるとする。

また、
f{i,j} = E_i - E_j
とする。ここでEは任意の画像情報であり、例えば上述した hue, sat, v, dI である。

そして f{i,j} から g{i,j} を以下のようにして定める。

アルゴリズム1ではつまり、点 (i,j) での残差である f{i,j} をそのまま求めるのではなく、別に誘導した g{i,j} を介して求める前処理をしている。このように多項式問題に変換することで、処理にかかる時間を直接 f{i,j} を再帰的に求めた場合よりも軽減できる。

{アルゴリズム2：動的計画法におけるコスト計算}

ここで Y = argmin{X} は、Yを最小化させるXを示す。
アルゴリズム2では、コスト s{i,j} とg{i,j} を求めて残差 D を求める下準備をしている。

{アルゴリズム3：動的計画法による残差D の算出}

アルゴリズム3では、コスト s{i,j} とg{i,j} から、残差 D を求めている。

ふたたびセンサフュージョンの手法の説明に戻る。

図７は、実施形態にかかるセンサフュージョンの手法のうち、「予測された測距データを基準画像と参照画像にそれぞれ投影したものの差を観る」ための残差 D_w の導出手法を説明するための概要図である。ここでは、時刻tにおいて予測される移動体 700 の位置から投射される時刻tにおいて予測できる測距データを、異なる画像データへ投影したときの差異を観ていると考えてもらってもよい。

図７のパネル(A)では、移動体 700 （の持つパーティクルセットのうちの）の持つパーティクルに対応する地図を用いて、測距データの予測を行っている。図７の上パネルでは、i番目のパーティクルが持つ時刻t-1における地図 Mⁱ _t-1 を例として示してある。ここで地図をパーティクルごとに設定しているのは、パーティクルにより定められる移動体 700 の位置・姿勢はそれぞれに異なる場合がある（異なるほうが普通）ためである。つまり、図７の移動体 700 の位置・姿勢・進行方向は、あくまで状態推定を経て確率的に予測されたものである（破線で描いてあるのはそういう意味である）。周知のように、パーティクルフィルタを使うことで、 "評価値のぶんだけ尤もらしく" 移動体の位置・姿勢・進行方向を確率的に定めることが可能である。

さていま移動体 700 が持つパーティクルがN個（1≦i≦N）であるとすると、移動体 700 はN回予測をしてその平均をとって合成データを得ることができる。別の実施形態では、N以外の回数、予測を行ってもよい。または別の手法として、パーティクルの評価値に応じて、加重平均をとってもよい。あるいは任意の統計数学的手法を適用して合成データを得てもよい。

こうして、時刻tでの基準画像についての「予測的な合成データ」 (l^b _t)' 710 、および、時刻tでの参照画像についての「予測的な合成データ」 (l^r _t)' 712 を得ることができる。なおここで記号lに付された括弧とプライムは予測的なものであることを示している。

そして図７のパネル(B)には、 (l^b _t)' 710 と (l^r _t)' 712 の残差を求める手法を示してある。処理の流れは図６に関して説明したところと同様であるのでここではくりかえさない。動的計画法により、 D_w = φ((l^b _t)',(l^r _t)') を求める。

図８は、実施形態にかかるセンサフュージョンの手法のうち、「実測測距データと予測された測距データをそれそれ画像データに投影したものの差を観る」ための残差 D_t の導出手法を説明するための概要図である。ここでは、時刻tにおける移動体 800 から得られた実際の測距データと、時刻tにおいて予測される移動体 802 の位置から投射される時刻tにおいて予測できる測距データを、同じ画像データ（この例では基準画像データ）へ投影したときの差異を観ていると考えてもらってもよい。

l^b _t 810 と (l^b _t)' 812 はそれぞれ、図６の l^b _t 612 と (l^b _t)' 710 と同様であるので、その導出については紙幅の都合上ここではくりかえさない。そしてパネル(C)に記載したように D_t = φ(l^b _t,(l^b _t)') と求める。

図６から図８にかけて求めてきた三種類の残差は、状態推定データ格納／処理ユニット（パーティクルフィルタ処理ユニット 360 など）が状態推定データの評価値として利用できる。そうした評価値として、これら三種類のうちいずれかを用いてもよいし、あるいは何らかの演算を施して組み合わせたものを使ってもかまわない。或る実施形態では、状態推定データの評価値として用いる残差 D を、 D = D_t・D_w・D_b として算出できる。別の実施形態では、 D = (D_t + D_w)/D_b として算出可能である。その他任意の演算を用いて D を算出してもよい。

残差 D_b はオドメトリモデルに依存するので、 D_bはオドメトリモデルの精度の確認のためのパラメータとして使用可能である。また、残差 D_t, D_w は地図に依存するので、 D_t, D_w は地図の精度の確認のためのパラメータとして使用可能である。

変形例として、図６から図８にかけて求めてきた三種類の残差に代えて、 D_b' = φ(l^r _t-1,l^r _t) 、 D_w = φ((l^b _t)',(l^r _t)') 、 D_t' = φ(l^r _t,(l^r _t)') の三種を用いて D を得てもよい。

図９は、実施形態にかかるパーティクルフィルタを用いた状態推定系（状態推定系 303 など）の動作を説明するためのフローチャートである。
まずステップ S900 にて、パーティクルフィルタ処理ユニット（パーティクルフィルタ処理ユニット360 など）が、パーティクルの初期設定を行う。例えば、N個のパーティクルを一様に設定することができる。

次にステップ S902 、時刻tにて、測距センサ（測距センサ 330 など）が、測距データを取得して（測距データ格納ユニット 332 などへ）格納する。
そしてステップ S904 にて、ステレオカメラ（ステレオカメラ 340 など）が、画像データを取得して（基準画像格納ユニット 342 および参照画像格納ユニット 344 などへ）格納する。

ステップ S906 では、残差算出ユニット（残差算出ユニット 350 に含まれる対応付けユニット 352 など）が、画像データへ測距データを投影する（対応づける処理を行う）ことで、投影線（例えば上述した補間線）を得る。換言すれば、ここで上述した合成データを得ていると見做してもよい。

そしてステップ S908 では、残差算出ユニット（残差算出ユニット 350 に含まれる画像情報抽出ユニット 354 など）が、投影線の上もしくはその近傍から、画像情報を抽出して、時刻tにおける合成データ l^b _t を得る。

ステップ S910 では、残差算出ユニット（残差算出ユニット 350 に含まれる画像情報抽出ユニット 354 など）が、記憶ユニット（記憶ユニット 310 など）に保存していた時刻t-1における合成データ l^b _t-1 を得る。そして、残差算出ユニット（残差算出ユニット 350 に含まれる計算ユニット 358 など）が、残差 D_b を算出する。その算出に際しては動的計画法を使用できる。

ステップ S912 からステップ S930 まで続くループを開始する。このループは各パーティクルに関して行われる。

ステップ S914 では、走行系（走行制御系 380 など）によりオドメトリモデルが出力（更新）される。それを承けてステップ S916 では、パーティクルフィルタ処理ユニットが、パーティクルの持つ位置・姿勢といったデータを遷移させる。

ステップ S918 では、残差算出ユニット（残差算出ユニット 350 に含まれる予測ユニット 356 など）が、i番目のパーティクルに対応する最新の地図（例えば、時刻t-1での地図 Mⁱ _t-1 ）を用いて、測距センサの計測値を予測する。（換言すれば、予測測距データを得る。）

ステップ S920 では、残差算出ユニットが、ステップ S918 で得た予測測距データと基準画像を使って、投影線を含んだ予測合成データ (l^b _t)', (l^r _t)' を得る。そして残差算出ユニットが投影線の上もしくは近傍での画像情報を抽出する。

ステップ S922 では、残差算出ユニットが、ステップ S918 で得た予測測距データと参照画像を使って、投影線を含んだ予測合成データ (l^r _t)' を得る。そして残差算出ユニットが投影線の上もしくは近傍での画像情報を抽出する。

そしてステップ S924 では、残差算出ユニット（残差算出ユニット 350 に含まれる計算ユニット 358 など）が、 l^b _t と (l^b _t)' とから、残差 D_tを算出する。

続いてステップ S926 では、残差算出ユニットが、 (l^b _t)' と (l^r _t)' とから、残差 D_w を算出する。

こうしてステップ S928 では、残差算出ユニットが、これまでに得た残差 D_b, D_t, D_w から D = D_b・D_t・D_w を算出する。

そしてステップ S930 では、パーティクルフィルタ処理ユニットが、i番目のパーティクルの評価値 w_i を D とする。そしてループはステップ S916 へ戻る。

すべてのパーティクルについて上述の処理が完了したところで、フローはステップ S932 へ進む。ステップ S932 では、パーティクルフィルタ処理ユニットが、各パーティクルの評価値 w を、
となるように正規化する。

そしてステップ S934 では、パーティクルフィルタ処理ユニットが、評価値 w を用いてパーティクルのリサンプリング（淘汰）を行う。そして測距データ格納ユニットが、時刻tでの測距データを地図 Mⁱ _t-1 に追加して更新し、記憶ユニットは地図 Mⁱ _t を得る。そうしてからフローはステップ S902 へ戻り、処理が続いてゆく。

なおここでパーティクルのリサンプリングは、N個のパーティクルのうち評価値が所定の閾値に満たなかったものを削除し、残ったパーティクルを評価値に応じた数だけコピーして増やすことで行うことが可能である。

なお、本明細書では実施形態の各種データについて、理解しやすいよう簡単な例を開示してはいるが、この例に限らず、データを対応付けて管理可能なXML形式やツリー構造形式やテーブル形式などの他の管理形式を採用してもよい。

〔ハードウェア〕
図１０は、実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。ここでは、CPU 1000 、メモリ 1002 、入力装置 1004 、出力装置 1006 、外部記憶装置 1008 、媒体駆動装置 1010 、可搬記録媒体 1012 、ネットワーク接続装置 1014 が、バス 1016 によって接続されている。

またこうしたハードウェア構造は、サブモジュールごとに構築してもよいし、移動体ごとに構築してもかまわないことに留意されたい。

CPU（中央処理ユニット） 1000 としては、任意の種類のプロセッサを使用でき、例えばMPUや組み込み用途CPUを使用できる。CPU 1000 は、実施形態にかかるプログラムをバッファ（メモリ 1002 など）に読み出したうえで実行できる。

メモリ 1002 としては、ダイナミックランダムアクセスメモリ（DRAM）が含まれるがこれには限定はされない。そうしたDRAMとしては、SDRAM、SLDRAM、RDRAM、および他のDRAMなどがある。メモリ 1002 にはまた、フラッシュメモリなどの不揮発性メモリを含めてもよい。

入力装置 1004 としては、キーボード、マウス、トラックボール、タッチパネル、ゲームコントローラー、もしくは音声認識装置といった、ユーザーが情報を入力しできる任意の装置が含まれる。

出力装置 1006 としては、2Dディスプレイ、スピーカー、3Dディスプレイといった、情報を出力してユーザーへ伝達できる任意の装置が含まれる。
外部記憶装置 1008 としては、同様に特定の用途に適するひとつ以上の記憶媒体を使用でき、例えば、ハードディスクドライブ、SSD（Solid State Drive）を使用できる。

媒体駆動装置 1010 には、任意の種類の可搬記録媒体 1012 を駆動できる任意のドライブが含まれる。可搬記録媒体 1012 としては、フレキシブルディスケット、コンパクトディスク（CD）、デジタルビデオディスク（DVD）、などといったものが含まれる。

ネットワーク接続装置 1014 には、ネットワークに接続可能な任意の装置が含まれる。ネットワーク接続装置1014 としては、イーサネット（登録商標）アダプタ、有線LAN（Local Area Network）ルータ、無線LANルータなどが含まれる。

実施形態にかかるプログラムは、外部記憶装置 1008 もしくは媒体駆動装置 1010 によって駆動される可搬記録媒体 1012 に格納することができる。また、ハードウェアは、ネットワーク接続装置 914 を介してネットワークと接続することができ、入力装置 1004 および出力装置 1006 を通じて実施形態にかかるプログラムを入出力することが可能である。また、サーバが持つ記憶装置に実施形態にかかるプログラムを保存し、クライアントからそのプログラムを実行させることで、実施形態にかかる効果を得ることも可能である。そうしたサーバ-クライアントネットワークは、例えばP2P（peer to peer）ネットワークであってもかまわない。

100 移動体
110 ビジョンセンサ
120 ビジョンセンサ
130 光軸
140 測距センサ
200 装置
202 クロックユニット
210 記憶ユニット
230 測距センサ
232 測距データ格納ユニット
240 ステレオカメラ
242 基準画像格納ユニット
244 参照画像格納ユニット
250 残差算出ユニット
260 状態推定データ格納／処理ユニット
270 オドメトリモデル
272 地図
300 装置
302 クロックユニット
303 状態推定系
310 記憶ユニット
330 測距センサ
332 測距データ格納ユニット
340 ステレオカメラ
342 基準画像格納ユニット
344 参照画像格納ユニット
350 残差算出ユニット
352 対応付けユニット
354 画像情報抽出ユニット
356 予測ユニット
358 計算ユニット
360 パーティクルフィルタ処理ユニット
362 パーティクルセット
372 地図
380 走行制御系
382 内界センサ
384 自己状態推定ユニット
386 自己状態記憶メモリユニット#1
388 自己状態記憶メモリユニット#2
390 移動制御機構
392 移動機構
600 移動体
610 合成データ l^b _t-1
612 合成データ l^b _t
700 移動体
710 合成データ (l^b _t)'
712 合成データ (l^r _t)'
800 移動体
810 合成データ l^b _t
812 合成データ (l^b _t)'
1000 CPU
1002 メモリ
1004 入力装置
1006 出力装置
1008 外部記憶装置
1010 媒体駆動装置
1012 可搬記録媒体
1014 ネットワーク接続装置
1016 バス

Claims

環境を表す地図を用いて、前記環境内を状態推定をしながら移動する移動体を制御するための装置であって、
(i) 前記地図を格納する、記憶手段と、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定し、前記記憶手段に格納して、格納した前記状態推定データの更新を行う、状態推定データ格納・処理手段と、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむ、測距手段と、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむ、画像取込手段と、
(v)前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算する、残差算出手段と、
(vi)前記移動体を動かし、動かした移動量を出力する、移動手段と
を含み、
前記残差算出手段が、或る時刻における測距データを、前記時刻の単位時間前における前記地図に対して予測することで、予測測距データを得て、前記画像データに前記予測測距データを対応づけることで、前記測距データを得た時刻よりも未来における合成データを得る予測手段を含み、
前記状態推定データ格納・処理手段が、前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新し、
前記測距手段が、前記測距データを用いて前記地図を更新し、
前記移動手段が、更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節する
ことを特徴とする、装置。
前記画像データが、基準画像データと、前記基準画像データ以外の参照画像データとを含み、
前記残差算出手段が、前記測距データと前記基準画像データを対応づけた合成データ、もしくは、前記測距データと前記参照画像データを対応づけた合成データを作成する
ことを特徴とする、請求項１記載の装置。
前記残差算出手段が、前記合成データを、
時刻t-1に得られた基準画像データに時刻t-1に得られた測距データを対応づけた合成データと、
時刻tに得られた基準画像データに時刻tに得られた測距データを対応づけた合成データと、
時刻t-1に得られた参照画像データに時刻t-1に得られた測距データを対応づけた合成データと、
時刻tに得られた参照画像データに時刻tに得られた測距データを対応づけた合成データと、
時刻tでの測距データを時刻t-1での地図を用いて予測したものを、時刻tに得られた基準画像データに対応づけた合成データと、
時刻tでの測距データを時刻t-1での地図を用いて予測したものを、時刻tに得られた参照画像データに対応づけた合成データと
から成る群から選択して得る
ことを特徴とする、請求項２記載の装置。
環境を表す地図を用いて、前記環境内を移動する移動体の状態推定と前記地図の更新を行うための方法であって、
(i) 前記地図を記憶手段に格納するステップと、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定して前記記憶手段に格納するステップと、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむステップと、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむステップと、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算するステップと、
(vi)前記移動体を動かし、動かした移動量を出力するステップと、
(vii) 前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新するステップと、
(viii)前記測距データを用いて前記地図を更新するステップと、
(ix)更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節するステップと
を含み、
前記残差を計算するステップが、或る時刻における測距データを、前記時刻の単位時間前における前記地図に対して予測することで、予測測距データを得て、前記画像データに前記予測測距データを対応づけることで、前記測距データを得た時刻よりも未来における合成データを得る
ことを特徴とする、方法。
環境を表す地図を用いて、前記環境内を移動する移動体の状態推定と前記地図の更新を行うための方法を、コンピュータに実行させるためのプログラムであって、
前記方法が、
(i) 前記地図を記憶手段に格納するステップと、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定して前記記憶手段に格納するステップと、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむステップと、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむステップと、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算するステップと、
(vi)前記移動体を動かし、動かした移動量を出力するステップと、
(vii) 前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新するステップと、
(viii)前記測距データを用いて前記地図を更新するステップと、
(ix)更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節するステップと
を含み、
前記残差を計算するステップが、或る時刻における測距データを、前記時刻の単位時間前における前記地図に対して予測することで、予測測距データを得て、前記画像データに前記予測測距データを対応づけることで、前記測距データを得た時刻よりも未来における合成データを得る
ことを特徴とする、プログラム。