JP2011048706A - Device, method, and program for automatically generating map by sensor fusion and moving mobile body by using the map automatically generated - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a device and the like which creates a highly precise environmental map and is synchronously reflected in a movement of a movable body in the environment. <P>SOLUTION: The device for using a map representing an environment to estimate a state and controlling a movable body includes: a storage means for storing a map; a state estimation data storage/processing means for setting and updating an initial value of the state estimation data of the movable body; a distance measuring means for setting a measurement point in the environment and obtaining distance measurement data that measure a position relation between a position of the movable body and a measurement point obtained from the state estimation data; an image fetch means for obtaining a part of the environment as image data; a residual calculation means for calculating a residual of compound data which associate the distance measurement data with image data; and a movement means for outputting a movement amount of the movable body. The state estimation data storage/processing means updates an output and residual of the movement means, the distance measuring means updates a map by the distance measurement data, and the movement means adjusts the movement of the movable body with the updated state estimation data and the map. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

本明細書にて後述する実施形態は、地図の自動生成と、そうして自動生成された地図を用いた移動体の移動とに関する。   Embodiments to be described later in this specification relate to automatic generation of a map and movement of a moving body using the automatically generated map.

移動体（自律ロボットなど）が自律的に移動をしながら、地図（環境地図）を自動生成する技術は、SLAM（Simultaneous Localization And Mapping）法として汎く知られている。   A technique for automatically generating a map (environmental map) while a moving body (such as an autonomous robot) moves autonomously is widely known as a SLAM (Simultaneous Localization And Mapping) method.

測距センサを用いたSLAM法では、移動体中の状態推定系が、測定時間単位ごとに取得した断片的環境のレンジポイントを繋ぎあわせることで、地図の生成を行っている。つまり、レンジポイントの空間的距離を求めて、対応付けた点群として座標を算出することで、地図を生成するということである。   In the SLAM method using a distance measuring sensor, a state estimation system in a moving body generates a map by connecting range points of fragmentary environments acquired for each measurement time unit. That is, the map is generated by calculating the spatial distance of the range points and calculating the coordinates as the associated point group.

あるいは、ビジョンセンサを用いたSLAM法では、ステレオビジョン法を用いて得られる環境内ランドマークの位置データに基づいて、地図の生成を行う。   Alternatively, in the SLAM method using a vision sensor, a map is generated based on the position data of landmarks in the environment obtained using the stereo vision method.

特開２００５‐０７７１３０号公報Japanese Patent Laying-Open No. 2005-077130 特開２００７‐０２４７３１号公報JP 2007-024731 A 特開２００４‐３６２４６９号公報JP 2004-362469 A

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

概して、単一種類のセンサだけを用いた測定から得られる地図は誤差が大きくなりやすい。これは、センサがそれぞれ得意とする環境が異なるためである。   In general, maps obtained from measurements using only a single type of sensor are prone to error. This is because the environment that each sensor is good at is different.

例えば、構造的な特徴が一様である環境（特徴の乏しい整然とした廊下など）の場合、測距センサは正確な距離測定が難しくなり、生成される地図も不安定になってしまう。   For example, in an environment where the structural features are uniform (such as an orderly corridor with poor features), it is difficult for the distance measuring sensor to accurately measure the distance, and the generated map becomes unstable.

また、特徴が遠距離にのみ在る場所（広場など）でビジョンセンサのみを使って状態推定をする場合、ビジョンセンサは遠距離の計測精度が近距離のそれに較べ劣るため、やはり地図の精度は落ちてしまう。   In addition, when estimating a state using only a vision sensor in a place where the feature is only at a long distance (such as a plaza), the accuracy of the map is still inferior to that of a short distance because the vision sensor is inferior to that of a short distance. It will fall.

つまりこうした従来の方法で生成した地図にはひずみが多く、環境に即した精度が低いという問題があった。
精度が低い、すなわち現実からかけはなれた地図が生成されてしまうと、移動体が「そこに無い筈の壁」に衝突してしまうなどといった事故の原因となりうる。 In other words, there is a problem that the map generated by such a conventional method has many distortions and low accuracy according to the environment.
If a map with low accuracy, that is, a map that is far from reality, is generated, it may cause an accident such as a moving object colliding with a “wall of a wall that is not there”.

当該技術分野においては、より精度の高い環境地図を作成しつつ、同期的に移動体のその環境内での移動に反映する手法が希求されている。   In this technical field, there is a demand for a technique for creating a highly accurate environmental map and reflecting the movement of a moving object in the environment synchronously.

本発明の実施形態では、環境を表す地図を用いて、前記環境内を状態推定をしながら移動する移動体を制御するための装置であって、
(i) 前記地図を格納する、記憶手段と、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定し、前記記憶手段に格納して、格納した前記状態推定データの更新を行う、状態推定データ格納・処理手段と、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむ、測距手段と、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむ、画像取込手段と、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算する、残差算出手段と、
(vi)前記移動体を動かし、動かした移動量を出力する、移動手段と
を含み、
前記状態推定データ格納・処理手段が、前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新し、
前記測距手段が、前記測距データを用いて前記地図を更新し、
前記移動手段が、更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節する
という装置が提供される。 In an embodiment of the present invention, an apparatus for controlling a moving body that moves while estimating the state in the environment using a map representing the environment,
(i) storage means for storing the map;
(ii) a state estimation data storage / processing unit that sets an initial value of state estimation data for estimating the state of the moving body, stores the initial value in the storage unit, and updates the stored state estimation data; ,
(iii) A plurality of measurement points are set in the environment, and distance measurement data obtained by measuring a positional relationship between the position of the moving body obtained from the state estimation data and each of the plurality of measurement points is obtained for each unit time. A distance measuring means incorporated in the storage means;
(iv) image capturing means for capturing a part of the environment as image data in the storage means at unit time;
(v) creating a plurality of composite data associating the distance measurement data and the image data, and calculating a residual between the composite data;
(vi) moving means for moving the moving body and outputting the moved moving amount;
The state estimation data storage / processing unit updates the state estimation data using the output of the moving unit and the residual calculated by the residual calculation unit,
The ranging means updates the map with the ranging data,
An apparatus is provided in which the moving means adjusts the movement of the moving body using the updated state estimation data and the map.

また、そうした効果が得られる方法とプログラムも、本発明の或る実施形態において提供される。   Also, a method and a program capable of obtaining such an effect are provided in an embodiment of the present invention.

本実施形態にかかる装置、方法、もしくはプログラムにより、測距手段と画像取込手段という性質の異なるセンサ手段によるセンサフュージョンを行うことができる。こうしたセンサフュージョンにより、測距手段から取得したレンジポイントの対応付けの精度を向上させて、精度の高い地図を作成できる。そして同期的に、その地図を移動体の移動に反映させることで、移動の精度も向上できる。   By means of the apparatus, method, or program according to the present embodiment, sensor fusion can be performed by sensor means having different properties such as distance measurement means and image capturing means. By such sensor fusion, the accuracy of associating range points acquired from the distance measuring means can be improved, and a highly accurate map can be created. And the accuracy of movement can also be improved by reflecting the map on the movement of a mobile body synchronously.

実施形態にかかる移動体 100 のおおまかな構造を示す斜視図である。It is a perspective view which shows the rough structure of the moving body 100 concerning embodiment. 装置 200 の機能的な側面を説明するためのおおまかなブロック図である。FIG. 3 is a rough block diagram for explaining functional aspects of the apparatus 200. パーティクルフィルタを用いた、装置 300 の構造を示すブロック図である。It is a block diagram which shows the structure of the apparatus 300 using a particle filter. 測距センサが出力可能な測距データの一例を示す図である。It is a figure which shows an example of the ranging data which a ranging sensor can output. ビジョンセンサが出力可能な画像データの一例を示す図である。It is a figure which shows an example of the image data which a vision sensor can output. 実施形態にかかる残差の算出方法を説明する概要図（その1）である。FIG. 5 is a schematic diagram (part 1) for explaining a residual calculation method according to the embodiment. 実施形態にかかる残差の算出方法を説明する概要図（その2）である。FIG. 5 is a schematic diagram (part 2) for explaining a residual calculation method according to the embodiment. 実施形態にかかる残差の算出方法を説明する概要図（その3）である。FIG. 6 is a schematic diagram (part 3) for explaining a residual calculation method according to the embodiment. 実施形態にかかるパーティクルフィルタを用いた状態推定系の動作を説明するフローチャートである。It is a flowchart explaining operation | movement of the state estimation type | system | group using the particle filter concerning embodiment. 実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。It is a schematic diagram which shows the hardware structure which can execute the program concerning embodiment.

以下、本発明の実施形態を図面に基づいて詳細に説明するが、これは本発明を何ら限定するものではない。   Hereinafter, although an embodiment of the present invention is described in detail based on a drawing, this does not limit the present invention at all.

〔定義〕
本明細書において使用する術語についてこれより解説を行う。 [Definition]
The terminology used in this specification will be explained below.

「移動体」（"vehicle"）とは例えば、駆動装置を具えた移動可能なロボット（自律ロボットなど）のことを含むが、これには限定はされない。例えば状態推定系を、人間が操縦する移動機械系（車輌など）に搭載したものを移動体と称することもまた可能である。   The “mobile body” includes, for example, a movable robot (such as an autonomous robot) provided with a driving device, but is not limited thereto. For example, a state estimation system mounted on a mobile machine system (such as a vehicle) operated by a human can also be referred to as a moving body.

「自律ロボット」（"autonomous robot"）とは、人間の介入を（初期設定やメンテナンスを除き）基本的に必要とすることなく自律的に稼動できるロボットのことを指す。自律ロボットは例えば、或る環境内を移動するにあたり、実施形態にかかる状態推定系を使いつつ、かつ人間からの介入を要さないという動作（"自律航法"）を実施可能である。とはいえ、自律ロボットに対して、人間の介入を行うことができないというわけではないことに留意されたい。   An “autonomous robot” refers to a robot that can operate autonomously without the need for human intervention (except for initial settings and maintenance). For example, an autonomous robot can perform an operation ("autonomous navigation") that does not require human intervention while using the state estimation system according to the embodiment when moving in a certain environment. However, it should be noted that human intervention is not impossible for autonomous robots.

「環境」（"environment"）とは、移動体がその中を測定し移動することになる世界のことを指す。実施形態では、当該技術分野にて公知なあらゆる種類の環境を想定している。実施形態にて想定される環境としては例えば、整然とした環境（廊下など）、整備されていない雑然とした環境（研究室内など）、もしくは或る測距センサが計測範囲内に対象物を捉えにくい環境（屋外の広場など）といったものがあるが、これらに限定はされない。   “Environment” refers to the world in which a moving object measures and moves. Embodiments assume all types of environments known in the art. As an environment assumed in the embodiment, for example, an orderly environment (such as a corridor), an unorganized environment (such as a laboratory), or a distance measurement sensor is difficult to catch an object within a measurement range. There are things such as the environment (such as outdoor plazas), but are not limited to these.

「地図」（"map"）もしくは「環境地図」（"environmental map"）とは、状態推定を行う対象である系（移動体など）が関与する環境についてのデータの集合体のことを指す。こうした地図としては、location-based（位置情報ベース）な地図とfeature-based（特徴ベース）な地図との二種類が含まれる。location-basedな地図はvolumetric mapともいい、環境内のすべての点についてのラベルを有している。つまり、location-basedな地図では、物体（障害物や壁など）が（座標上の）或る点に存在するかどうかについての情報のみならず、何もない箇所（free space）が"在る"という情報も有している。一方、feature-basedな地図は、環境の形状を指定する地図といえる。つまりこれは、目標物（ランドマークともいう）の位置（極座標表示など）を有する地図である。   “Map” (“map”) or “environmental map” refers to a collection of data about an environment involving a system (such as a moving object) for which state estimation is performed. Such maps include two types: location-based maps and feature-based maps. A location-based map, also called a volumetric map, has labels for every point in the environment. In other words, a location-based map has “free space” as well as information about whether an object (obstacle, wall, etc.) exists at a certain point (on coordinates). I also have the information. On the other hand, a feature-based map is a map that specifies the shape of the environment. That is, this is a map having the position of a target (also referred to as a landmark) (such as polar coordinate display).

「センサ」（"sensor"）とは、当該技術分野において公知である任意の種類のセンサのことを指す。そうしたセンサは、当該技術分野では、内界センサと外界センサとに大別される。センサの例としては、姿勢センサ、タコメータ、ロータリーエンコーダ、レートジャイロ、歩幅計、加速度計、位置センサ、変位センサ、加速度センサ、測距（距離）センサ、方位センサ、触覚センサ、力覚センサ、視覚（ビジョン）センサ、ステレオビジョンセンサ、近接覚センサ、超音波センサ、赤外線センサ、傾斜角センサ、地磁気センサ、レーザーレンジファインダー（距離計）、2Dエリアスキャンセンサ、3Dエリアスキャンセンサなどが含まれるが、これらに限定はされない。   “Sensor” refers to any type of sensor known in the art. Such sensors are broadly classified into an internal sensor and an external sensor in the technical field. Examples of sensors include posture sensors, tachometers, rotary encoders, rate gyros, pedometers, accelerometers, position sensors, displacement sensors, acceleration sensors, distance measuring (distance) sensors, bearing sensors, tactile sensors, force sensors, visual (Vision) sensor, stereo vision sensor, proximity sensor, ultrasonic sensor, infrared sensor, tilt angle sensor, geomagnetic sensor, laser rangefinder (distance meter), 2D area scan sensor, 3D area scan sensor, etc. These are not limited.

「内界センサ」（"internal sensor"）とは、ロボットなどの系の内部の状態を知るためのセンサのことを指す。   An “internal sensor” refers to a sensor for knowing the internal state of a system such as a robot.

「外界センサ」（"external sensor"）とは、ロボットなどの系の外部の状況を計測し、系の行動のための判断材料とするためのセンサのことをいう。なお、上述した内界センサと外界センサの区別は排他的なものとはかぎらず、同種のセンサを双方に用いることもまた可能である。例えば、ビジョンセンサを内界センサにも外界センサにも使ってもよい。   An “external sensor” refers to a sensor for measuring a situation outside a system such as a robot and using it as a judgment material for system behavior. Note that the above-described distinction between the internal sensor and the external sensor is not exclusive, and it is also possible to use the same type of sensor for both. For example, the vision sensor may be used for both an internal sensor and an external sensor.

「センサノード」（"sensor node"）とは、上述したセンサを含んだ装置のことをいう。センサノードは例えば、センサへの入出力を司るユニットや、情報（データ）処理ユニットや、無線・有線通信ユニットなどを有してもよい。またセンサノードは、実施形態にかかる装置測位装置を有することも、実施形態にかかる装置測位方法もしくはプログラムを実行することも可能である。本明細書では、「センサノード」のことを略して「センサ」と表記することがある。   A “sensor node” refers to a device that includes the sensor described above. The sensor node may include, for example, a unit that controls input / output to the sensor, an information (data) processing unit, a wireless / wired communication unit, and the like. In addition, the sensor node can have the device positioning device according to the embodiment, or can execute the device positioning method or program according to the embodiment. In this specification, “sensor node” may be abbreviated as “sensor”.

「センサフュージョン」（センサ統合； "sensor fusion"）とは、複数のセンサの出力を統合的に処理して用いる手法のことをいう。そうしたセンサフュージョンには、複数個の同じ種類のセンサを用いてもよいし、あるいは、異なる種類のセンサをそれぞれ任意の個数だけ用いてもかまわない。本明細書において "統合（処理）" といったとき、それはセンサフュージョン（の一部）と同義であると捉えてもらってかまわない。   “Sensor fusion” (sensor fusion) refers to a technique of processing and using outputs of a plurality of sensors in an integrated manner. In such sensor fusion, a plurality of sensors of the same type may be used, or any number of different types of sensors may be used. In this specification, when “integration (processing)” is used, it may be understood that it is synonymous with (part of) sensor fusion.

「測距データ」とは、測距センサ（「測距計」とも称する）から得られる距離についてのデータのことをいう。測距データは例えば、測定装置からレンジポイントまでのベクトルである。なお「レンジポイント」とは、測距センサが使うビーム（レーザーレンジファインダーが用いるレーザービームなど）が、障害物（壁や天井など）に当った点のことを指す。こうしたレンジポイントを複数まとめて、「レンジデータ」とも称する。また本明細書では、測距データと同義に「レンジデータ」という語も用いることもある。   “Distance data” refers to data about distance obtained from a distance measuring sensor (also referred to as “rangefinder”). The distance measurement data is, for example, a vector from the measuring device to the range point. The “range point” refers to a point where a beam (such as a laser beam used by a laser range finder) used by a distance measuring sensor hits an obstacle (such as a wall or a ceiling). A plurality of such range points are collectively referred to as “range data”. In this specification, the term “range data” may be used synonymously with distance measurement data.

「ビジョンデータ」とは、視覚（ビジョン）センサから得られる画像データのことをいう。本明細書では、「ビジョンセンサ」という語と「カメラ」という語を同義に使用することがある。   “Vision data” refers to image data obtained from a vision (vision) sensor. In this specification, the terms “vision sensor” and “camera” may be used interchangeably.

「ステレオビジョン（法）」（"stereo vision"）とは、複数のビジョンセンサ（まとめて"ステレオカメラ"とも称する）を用いた、ランドマーク（となる物体）の空間位置を計算する手法のことをいう。ステレオビジョン法では、あらかじめ各ステレオカメラの物理的な設置位置の違いから生じる視差（いわゆる「両眼視差」、すなわち "binocular parallax" にあたる）を求めておく。そして、各ステレオカメラが環境の同じ箇所をランドマークを含めるようにして撮影する（ここでの "同じ" という語は便宜上の表現であり、厳密な同一性を言っているわけではないのは、ステレオカメラの構造を知る当業者にとっては自明のことであろう）。得られた複数の画像のうちから、基準となる画像（基準画像； "base image"）を定めて、それ以外を「参照画像」（"reference image"）とする。そして、基準画像に写っているランドマークが、参照画像のどこに写っているのかを画像処理によって求める。すると、その画像上での位置のずれと、視差と、環境を定めている地図とを用いて、ランドマークが環境内のどこに在るのかを幾何学的に（例えば三次元的に）計算することができる。こうした「どこに写っているか」を実現するための処理としては、公知技術である対応点検索法を使用できる。なお対応点検索法とは、一方の画像上のピクセルに対応するピクセルが、他方の画像の何処にあるのかを探す手法である。   "Stereo vision" ("stereo vision") is a method of calculating the spatial position of a landmark (the target object) using multiple vision sensors (collectively referred to as "stereo cameras"). Say. In the stereo vision method, a parallax (a so-called “binocular parallax”, that is, “binocular parallax”) resulting from a difference in physical installation position of each stereo camera is obtained in advance. And each stereo camera shoots the same part of the environment with landmarks included (the word "same" here is a convenience expression, not an exact identity, It will be obvious to those skilled in the art who know the structure of stereo cameras). A reference image (“base image”) is determined from the obtained plurality of images, and the others are set as “reference images” (“reference image”). Then, it is determined by image processing where the landmark in the reference image is in the reference image. Then, using the position shift on the image, the parallax, and the map that defines the environment, the location of the landmark in the environment is calculated geometrically (for example, three-dimensionally). be able to. As a process for realizing such “where is shown”, a corresponding point search method which is a known technique can be used. The corresponding point search method is a method of searching where the pixel corresponding to the pixel on one image is in the other image.

「ランドマーク」（"landmark"）とは、センサにとって測定の目印となりえる特徴のことをいう。センサの種類によってランドマークは異なってくる。例えば、ビジョンセンサであれば、特徴的な形状の物体をランドマークとすることが可能である。   A “landmark” is a feature that can be a landmark for a sensor. Landmarks differ depending on the type of sensor. For example, in the case of a vision sensor, an object having a characteristic shape can be used as a landmark.

「オドメトリ」（"odometer"）とは、移動体の持つ内界センサ（駆動装置に付随するセンサなど； オドメータともいう）から得られるデータ（"オドメトリデータ"）に基づいて、移動体の姿勢と位置を求める手法を指す。オドメトリデータとしては例えば、移動体の移動方向、移動距離、もしくは移動速度、またはそれらの組み合わせ、に関するデータが含まれる。オドメトリの一例として、車輪を二つ持つ移動体の場合、左右の車輪の回転速度から移動体の並進速度と角速度を求め、それらを積分して移動体の位置と姿勢を求める手法がある。その他、任意の内界センサを使って移動体の並進速度と角速度を求め、それらを積分して移動体の位置と姿勢を求めてもよい。なお「オドメトリモデル」とは、オドメトリデータの集合体のことをいう。また、オドメトリモデルを略して単に「オドメトリ」と称する場合もある。   “Odometry” means the attitude of the moving object based on the data (“odometry data”) obtained from the internal sensors (such as sensors attached to the drive; also referred to as odometer) of the moving object. Refers to the method of obtaining the position. The odometry data includes, for example, data related to the moving direction, moving distance, moving speed, or combination of the moving body. As an example of odometry, in the case of a moving body having two wheels, there is a method of obtaining the translation speed and angular velocity of the moving body from the rotational speeds of the left and right wheels and integrating them to obtain the position and orientation of the moving body. In addition, the translation speed and angular velocity of the moving body may be obtained using an arbitrary internal sensor, and the position and orientation of the moving body may be obtained by integrating them. The “odometry model” refers to a collection of odometry data. Further, the odometry model may be simply referred to as “odometry” for short.

「姿勢」とは、その系（移動体など）のとっている体勢のことをいう。例えば姿勢といえるパラメータとしては、ロール角、ピッチ角、ヨー角などがある。   “Posture” refers to the posture of the system (moving body, etc.). For example, parameters such as posture include a roll angle, a pitch angle, and a yaw angle.

「（系の）状態」（"state"）とは、環境を特徴づける変数のことであり、例えば環境中の系の位置や移動速度のことをいう。   The “(system) state” (“state”) is a variable that characterizes the environment, for example, the position or moving speed of the system in the environment.

「状態推定（法）」（"state estimation"）とは、系の状態を推定するための手法のことをいう。状態推定にあたっては例えば、Kalmanフィルタ、拡張Kalmanフィルタ、相補フィルタ、またはパーティクルフィルタといった時系列フィルタを使用可能である。また本明細書では、「測位」という語を、「状態推定」という語と同義に使用することがある。   “State estimation” refers to a technique for estimating the state of a system. In the state estimation, for example, a time series filter such as a Kalman filter, an extended Kalman filter, a complementary filter, or a particle filter can be used. In the present specification, the term “positioning” may be used synonymously with the term “state estimation”.

「信念」（"belief"）とは、時刻tにて得られた計測データz_tを条件とした、状態x_tの事後確率のことをいう。信念 bel(x_t) = p(x_t | z_t) である。 “Belief” refers to the posterior probability of state x _t on condition of measurement data z _t obtained at time t. The belief bel (x _t ) = p (x _t | z _t ).

「パーティクル」（"particle"）とは、或る状態推定系の属する位置や方位などを推定する仮説に関する情報（データ）のことを指す。言い換えれば、パーティクルとは事後確率分布（posterior distribution）の標本である。こうしたパーティクルのことを単に「仮説」や「粒子」と呼ぶこともある。例えばパーティクルには、位置推定対象物の状態（姿勢や速度など）を表すデータを含めることができる。またパーティクルとしては例えば、位置推定対象物の位置ベクトルと、位置推定対象物の速度ベクトルと、そのパーティクルが持つ重み（または評価値）と、を含んだデータを使用できるが、これに限定はされない。   “Particle” (“particle”) refers to information (data) related to a hypothesis for estimating the position, orientation, and the like to which a certain state estimation system belongs. In other words, a particle is a sample of a posterior distribution. These particles are sometimes simply called “hypotheses” or “particles”. For example, the particles can include data representing the state (posture, speed, etc.) of the position estimation target object. As the particles, for example, data including the position vector of the position estimation target object, the velocity vector of the position estimation target object, and the weight (or evaluation value) of the particle can be used, but the present invention is not limited to this. .

「パーティクルセット（パーティクル集合）」（"particle set"）とは、任意の個数のパーティクルの組のことをいう。実施形態では、或るモジュールに属するパーティクルをまとめて、パーティクルセットとして扱うことができる。そうしたパーティクルセットに含まれるパーティクルの個数は、パーティクルを処理する系の能力や機能、要求される自己位置推定の精度などに応じて任意に設定できる。例えば或る実施形態では、1個以上、10個以上、10²個以上、10³個以上、もしくは10⁴個以上のパーティクルを含んだパーティクルセットを設定できるが、これらに限定はされない。 “Particle set” refers to a set of an arbitrary number of particles. In the embodiment, particles belonging to a certain module can be collectively handled as a particle set. The number of particles contained in such a particle set can be arbitrarily set according to the ability and function of the system for processing particles, the required accuracy of self-position estimation, and the like. For example, in some embodiments, one or more, 10 or more, 10 ² or more, 10 ³ or more, or can be set a particle set containing 10 ⁴ or more particles, but are not limited to.

「パーティクルフィルタ」（"particle filter"）とは、時系列フィルタの一種であって、上述したパーティクルを用いて系の状態推定を行うために使用できるものである。またパーティクルフィルタはBayesフィルタの一種でもある。パーティクルフィルタを実現するアルゴリズムとしては例えば、パーティクルの尤度を重みとして、その重みに基づいたリサンプリング（復元抽出）を行い、パーティクルを予測に基づいて移動させ、現在の観測（測位）結果とパーティクルの状態から尤度を計算する、といったものがある。   A “particle filter” is a type of time-series filter that can be used to estimate the state of a system using the above-described particles. The particle filter is also a kind of Bayes filter. As an algorithm for realizing a particle filter, for example, the likelihood of a particle is used as a weight, resampling (restoration extraction) based on the weight is performed, the particle is moved based on the prediction, the current observation (positioning) result and the particle Likelihood is calculated from the state of.

「パーティクルの淘汰（新陳代謝／更新）」または「パーティクルのリサンプリング」とは、何らかの基準に満たないパーティクルを処理の対象から外し（捨て）、新たなパーティクルを得ることをいう。ここではわかりやすく説明するため、パーティクルセットにパーティクルが五個だけある例を引いてみる。そしてパーティクルの評価値が順に0.5、0.5、0.9、0.7、0.4だったとする（あくまで簡単な例であることに留意されたい）。すると平均値（mean）は0.6となる。仮に淘汰する条件が平均値未満であることだとすれば、このパーティクルセットのうちでは二個のパーティクルだけを残すように淘汰が行われることになる。もちろんこの淘汰条件は一例であり、中央値を使った条件や複数種の評価値を使う条件についても想定されている。そして、パーティクルセットのうち特定のもの（重みの大きなものなど）だけを残してパーティクルを淘汰したうえで、残ったパーティクルをその重みに基づいて新たなパーティクルを "播く" 。例えば、重みが大きいパーティクルほど、その重みに比例して多くの新たなパーティクルの元となることができる。直近の例での評価値が重みであるとすれば、評価値が0.9のパーティクルに基づいて三個の新たなパーティクルを、評価値が0.7のパーティクルに基づいて二個の新たなパーティクルを、それぞれリサンプリングできる（もちろんこの手法は一例に過ぎない）。こうした新たなパーティクルの取得数は、簡単な例でいえばテーブルに評価値と個数の対として定めておくこともできる。このように新たなパーティクルを作成する際には、元となったパーティクルをそのままコピーしてもよいし、何らかの変更を加えたうえで作成してもかまわない。   “Particle trap (metabolism / update)” or “particle re-sampling” means that particles that do not meet some criteria are removed (discarded) from processing targets and new particles are obtained. For the sake of simplicity, let's look at an example where there are only five particles in the particle set. Assume that the particle evaluation values are 0.5, 0.5, 0.9, 0.7, and 0.4 in this order (note that this is a simple example). Then, the average value (mean) becomes 0.6. If the condition for wandering is less than the average value, wrinkles are performed so that only two particles remain in this particle set. Of course, this dredging condition is an example, and a condition using a median value and a condition using multiple types of evaluation values are also assumed. Then, leave only a specific set (such as one with a large weight) of the particle set and hesitate the particles, and then "sow" new particles based on the remaining particles. For example, a particle having a larger weight can be a source of many new particles in proportion to the weight. If the evaluation value in the most recent example is a weight, then three new particles based on a particle with an evaluation value of 0.9, two new particles based on a particle with an evaluation value of 0.7, Resampling is possible (of course, this is just an example). In a simple example, the number of new particles acquired can be determined as a pair of evaluation value and number in a table. Thus, when creating a new particle, the original particle may be copied as it is, or it may be created after some change.

「パーティクルの良さ」とは、そのパーティクルが推定している系の状態と、その系の "現実" の状態（観測結果など）とのマッチングの度合の高さのことをいう。そうした度合は当該技術分野において公知なさまざまな手法を使って算出可能である。後述する実施例では、そうした手法を例示してある。無論、そうした系の "現実" が、人間が同じ系に対して知覚できる現実とは異なっている場合もあるのは、当業者には理解できることだろう。当然のことながら、状態推定系が複数存在すれば、その認識する "現実" はそれぞれ異なってくるであろう。   “Goodness of particles” refers to the degree of matching between the state of the system estimated by the particles and the “real” state (observation results, etc.) of the system. Such a degree can be calculated using various methods known in the art. Examples described later exemplify such a method. Of course, those skilled in the art will understand that the “reality” of such systems may differ from the reality that humans can perceive for the same system. Naturally, if there are multiple state estimation systems, the "reality" that they recognize will be different.

「残差」（"residual"）とは、本明細書では或るデータと別の或るデータの差分のことをいう。例えば残差とは尤度の残差（尤度残差）のことを指す。あるいは、観測されたデータと、モデルから推定されたデータとの差を、残差ということもある。残差が大きいほど、比較されたデータ間の違いも大きいということになる。   “Residual” refers herein to the difference between some data and some other data. For example, the residual means a residual of likelihood (likelihood residual). Alternatively, the difference between the observed data and the data estimated from the model may be referred to as a residual. The larger the residual, the greater the difference between the compared data.

「動的計画（法）」（"dynamic programming (DP)"）とは、Bellmanの最適性の原理に基づいて、多変数最適化問題を解くための手法をいう。本明細書に記載された実施形態では、動的計画法を実現するためのアルゴリズムを用いることができる。本明細書では、動的計画法を用いて二種のデータ α, β から残差 D を算出して得ることを、 D = φ(α,β) と表現することがある。   “Dynamic programming (DP)” is a method for solving multivariable optimization problems based on Bellman's principle of optimality. In the embodiments described herein, an algorithm for implementing dynamic programming can be used. In this specification, the calculation of the residual D from the two types of data α and β using dynamic programming may be expressed as D = φ (α, β).

「破綻」（"failure"）とは、ここでは状態推定系が自己位置を（一時的であれ）推定できなくなることをいう。例えば、近距離しか計測できない測距センサだけを搭載した自律ロボットが広場に進出したとする。そして測距センサの測定可能距離・角度内に測定できる対象物が何も無くなってしまったとする。すると、そのロボット（の状態推定系）は "破綻" することがある。このような破綻をきたしたロボットは、望ましくない方向に誤って移動することがあり、事故の原因となりうる。なおこの場合、 "誤った" のはあくまでも外部の系（ユーザーとしての人間など）から観測してわかることであり、自律ロボットの持つ状態推定系が自身が "誤った" と判断できるとはかぎらない。   “Failure” here means that the state estimation system cannot estimate its own position (if it is temporary). For example, it is assumed that an autonomous robot equipped only with a distance measuring sensor that can measure only a short distance has advanced into a plaza. Then, it is assumed that there are no objects that can be measured within the measurable distance / angle of the distance measuring sensor. Then, the robot (its state estimation system) may “fail”. A robot that has failed in this way may accidentally move in an undesired direction, causing an accident. In this case, “wrong” can only be understood by observation from an external system (such as a human being), and the state estimation system of an autonomous robot cannot be judged as “wrong” by itself. Absent.

〔移動体〕
ここより、本発明の実施形態を実施可能な程度に詳細に説明してゆく。
図１は、実施形態にかかる移動体 100 のおおまかな構造を示す斜視図である。移動体 100 は、センサノードとして、二基のビジョンセンサ 110, 120 （"ひとつのステレオカメラ"）と、測距センサ 140 を有する。この実施例では、ビジョンセンサ 110, 120 に関する光軸 130 と、測距センサ 140 に関する光軸 150 とは平行であると仮定してある。当然のことながら、二つの光軸が平行でない場合も想定されている。またこの実施例では、簡単のため、図１に示した瞬間には、移動体 100 は、光軸 130, 150 と平行な方向に進んでいるものと仮定する。つまり、ビジョンセンサ 110, 120 および測距センサ 140 は、移動体 100 の進行方向をほぼ向いているということになる。 [Moving object]
From here, it explains in detail to such an extent that the embodiment of the present invention can be implemented.
FIG. 1 is a perspective view illustrating a general structure of a moving body 100 according to the embodiment. The moving body 100 includes two vision sensors 110 and 120 (“one stereo camera”) and a distance measuring sensor 140 as sensor nodes. In this embodiment, it is assumed that the optical axis 130 for the vision sensors 110 and 120 and the optical axis 150 for the distance measuring sensor 140 are parallel. Of course, it is also assumed that the two optical axes are not parallel. In this embodiment, for the sake of simplicity, it is assumed that the moving body 100 is traveling in a direction parallel to the optical axes 130 and 150 at the moment shown in FIG. That is, the vision sensors 110 and 120 and the distance measuring sensor 140 are substantially directed in the traveling direction of the moving body 100.

ビジョンセンサ 110 とビジョンセンサ 120 は、光軸 130 を挟んで横に並んでいる。なおここで "横（方向）" というのは、便宜的に、移動体が進む走行面に対して水平な方向であってかつ光軸に垂直な方向と定めたものである。このように横に並んだビジョンセンサからなるステレオカメラでは、横方向に特異的な特徴をランドマークとして使用しやすいという効果が得られる。具体的には、縦に立った壁の屈曲部（エッジ）をランドマークとしやすいという効果を得ることができる。なおここで "縦（方向）" というのは、便宜的に、前述した走行面に対して垂直な方向であってかつ光軸に垂直な方向のこととする。つまり「縦に立った壁」というのは、日頃人間が生活する環境でいうところの壁のことだと想定してもらってもいい。   The vision sensor 110 and the vision sensor 120 are arranged side by side with the optical axis 130 in between. Here, for the sake of convenience, the term “lateral (direction)” is defined as a direction that is horizontal to the traveling surface along which the moving body travels and that is perpendicular to the optical axis. In such a stereo camera composed of vision sensors arranged side by side, it is possible to obtain an effect that it is easy to use a specific characteristic in the horizontal direction as a landmark. Specifically, it is possible to obtain an effect that a bent portion (edge) of a vertically standing wall can be easily used as a landmark. Here, for the sake of convenience, the term “longitudinal (direction)” is a direction perpendicular to the above-described traveling surface and perpendicular to the optical axis. In other words, it can be assumed that the “wall standing vertically” is the wall in the environment where people live.

別の実施形態では、ビジョンセンサ 110, 120 が、縦に並んだ二基のビジョンセンサであってもよい。このように縦に並んだビジョンセンサからなるステレオカメラでは、縦方向に特異的な特徴をランドマークとして使用しやすいという効果が得られる。具体的には、天井に付いた、ステレオカメラから見て横方向に走る蛍光灯（のエッジ）といったものを、ランドマークとしやすいという効果が得られる。   In another embodiment, the vision sensors 110 and 120 may be two vision sensors arranged vertically. In such a stereo camera composed of vision sensors arranged vertically, an effect that it is easy to use features specific to the vertical direction as landmarks can be obtained. Specifically, it is possible to obtain an effect that a fluorescent lamp (an edge thereof) attached to the ceiling and running in the horizontal direction as viewed from the stereo camera can be easily used as a landmark.

さらに別の実施形態では、ビジョンセンサ 110, 120 の代わりに、三基のビジョンセンサを配置してもよい。その配置法としては、正三角形や二等辺三角形の頂点に各ビジョンセンサを並べるやりかたなどがある。こうした三角型の配置では、上述した横並べ式と縦並べ式の双方の効果を併せて得ることが可能である。   In still another embodiment, three vision sensors may be arranged instead of the vision sensors 110 and 120. As an arrangement method, there is a method of arranging each vision sensor at the apex of an equilateral triangle or an isosceles triangle. With such a triangular arrangement, it is possible to obtain the effects of both the horizontal arrangement and the vertical arrangement described above.

ビジョンセンサ 110, 120 でのランドマークの抽出にあたっては、任意の公知技術を使用でき、例えば勾配やラプラシアンを使った演算を用いることができる。   For extracting landmarks by the vision sensors 110 and 120, any known technique can be used, for example, calculation using gradient or Laplacian can be used.

測距センサ 140 は、（この実施例では）光軸 130 と光軸 150 が平行であるので、ビジョンセンサ 110, 120 と概して平行に配置されている。そして、測距センサ 140 は何らかの測定手段を環境へ投射して、その環境からの反射の度合いから、レンジポイントについてのデータ（まとめて「レンジデータ」とも呼ぶ）を得る。例えば測距センサ 140 がレーザーレンジファインダであるなら、測定手段はレーザービームとなる。
この実施例では、測距センサ 140 は（複数本の）スキャンライン 142 に沿って測定手段を逐次投射してゆく。 Ranging sensor 140 is arranged generally parallel to vision sensors 110 and 120 because optical axis 130 and optical axis 150 are parallel (in this embodiment). Then, the distance measuring sensor 140 projects some measuring means to the environment, and obtains data about the range point (collectively referred to as “range data”) from the degree of reflection from the environment. For example, if the distance measuring sensor 140 is a laser range finder, the measuring means is a laser beam.
In this embodiment, the distance measuring sensor 140 sequentially projects the measuring means along the scan line 142.

以下に説明するように、移動体 100 は、ビジョンセンサ 110, 120 を用いたステレオビジョン法による複数の画像の取得と同期して、測距センサ 140 から得られた測距データをその複数の画像の各々に対応づける。そして測距データを対応づけた画像同士の残差を求めて、その残差を状態推定データの評価値（重み）とする。なおここで状態推定データとは、上述した状態推定法にて用いられるデータであり、例えばパーティクルである。おおまかに言えば、測距センサ 140 は近傍にある障害物の存在を検出するのに向いており、ビジョンセンサ 110, 120 は環境の奥行を検出するのに向いているといえる。これら二種のセンサを組みあわせることで、互いの長所を組み合わせ弱点を補うという効果が得られる。   As will be described below, the moving object 100 synchronizes with the acquisition of a plurality of images by the stereo vision method using the vision sensors 110 and 120, and converts the distance measurement data obtained from the distance measurement sensor 140 into the plurality of images. Correspond to each of these. Then, a residual between the images associated with the distance measurement data is obtained, and the residual is used as an evaluation value (weight) of the state estimation data. Here, the state estimation data is data used in the state estimation method described above, and is, for example, a particle. Roughly speaking, the distance measuring sensor 140 is suitable for detecting the presence of an obstacle in the vicinity, and the vision sensors 110 and 120 are suitable for detecting the depth of the environment. By combining these two types of sensors, it is possible to combine the advantages of each other to compensate for the weak points.

図２は、実施形態にかかる装置 200 の機能的な側面を説明するためのおおまかなブロック図である。なおここでは一例として、装置 200 が移動体である場合を説明する。しかしながら他の実施形態では、移動体が装置200 の一部の要素を含み、その他の要素は移動体の外部の装置に含まれるようにもできる。例えば移動体が、測距センサ 230 とステレオカメラ 240 を含み、その他の要素は移動体の外部の別の装置が含むようにしてもよい。言い換えれば、装置 200 は移動体そのものであってもよいし、あるいは、有線もしくは無線で通信可能に接続された移動体と別の装置を含むものであってもよい。   FIG. 2 is a rough block diagram for explaining functional aspects of the apparatus 200 according to the embodiment. Here, as an example, a case where the apparatus 200 is a moving body will be described. However, in other embodiments, the mobile body may include some elements of the device 200 and the other elements may be included in a device external to the mobile body. For example, the moving body may include the distance measuring sensor 230 and the stereo camera 240, and the other elements may be included in another device outside the moving body. In other words, the device 200 may be the mobile body itself, or may include a device different from the mobile body connected to be communicable by wire or wirelessly.

装置 200 は、測定手段として、測距センサ 230 とステレオカメラ 240 を含む。また装置 200 は、測距センサ 230 とステレオカメラ 240 との同期をとった時系列的な測定（たとえば1秒おきの測定など）を可能にするためのクロックユニット 202 も含む。測距センサ 230 とステレオカメラ 240 は、上述した「外界センサ」に相当する。なお別の実施形態では、測距センサ 230 とステレオカメラ 240 がそれぞれクロックを有し、何らかの手法を以って各々を同期させることで、測距センサ 230 とステレオカメラ 240 の同期的な動作を実現させてもよい。なお、ステレオカメラ 240 は、（図１に関して上述したように）複数基のビジョンセンサを含むことができる。なお、ステレオカメラ 240 は、得た複数の画像のうちからいずれかを基準画像として定め、それ以外を参照画像として定めることができる。例えば、ステレオカメラ 240 が二基のビジョンセンサを含んでいるとした場合、一方のビジョンセンサが得て出力する画像を基準画像とし、他方が出力する画像を参照画像とすることができる。あるいは別の手法として、時間などの条件に応じて基準画像と参照画像を出力するビジョンセンサを切り換えるようにしてもよい。   The apparatus 200 includes a distance measuring sensor 230 and a stereo camera 240 as measurement means. The apparatus 200 also includes a clock unit 202 for enabling time series measurement (for example, measurement every other second) synchronized with the distance measurement sensor 230 and the stereo camera 240. The distance measuring sensor 230 and the stereo camera 240 correspond to the above-mentioned “external sensor”. In another embodiment, the distance measurement sensor 230 and the stereo camera 240 each have a clock, and synchronize each other by some method, thereby realizing the synchronous operation of the distance measurement sensor 230 and the stereo camera 240. You may let them. Note that the stereo camera 240 can include multiple vision sensors (as described above with respect to FIG. 1). The stereo camera 240 can determine one of the obtained images as a standard image and the other as a reference image. For example, if the stereo camera 240 includes two vision sensors, an image obtained and output by one vision sensor can be used as a standard image, and an image output by the other can be used as a reference image. Alternatively, as another method, a vision sensor that outputs a base image and a reference image may be switched according to conditions such as time.

また装置 200 は、データの保存をするための記憶ユニット 210 （図１０に関して後述する外部記憶装置 1008 などに対応）も含む。記憶ユニット 210 には、装置 200 （あるいは装置 200 に含まれる移動体）の居る環境についてのデータである地図 272 と、装置 200 の持つ移動手段（図２には示していない）から得られたデータであるオドメトリモデル 270 も含まれる。地図 272 としては、上述したlocation-basedな地図やfeature-basedな地図を使用できる。   The device 200 also includes a storage unit 210 for storing data (corresponding to an external storage device 1008 described later with reference to FIG. 10). The storage unit 210 has a map 272 that is data about the environment in which the device 200 (or a mobile body included in the device 200) is located, and data obtained from the moving means (not shown in FIG. 2) of the device 200. The odometry model 270 is also included. As the map 272, the above-mentioned location-based map or feature-based map can be used.

装置 200 にはさらに、測距センサ 230 の出力した測距データを保存するための測距データ格納ユニット 232 と、ステレオカメラ 240 の出力した画像データを保存するための基準画像格納ユニット 242 および参照画像格納ユニット 244 とが含まれる。   The apparatus 200 further includes a distance measurement data storage unit 232 for storing distance measurement data output from the distance measurement sensor 230, a standard image storage unit 242 for storing image data output from the stereo camera 240, and a reference image. And storage unit 244.

装置 200 はまた、地図 272 と測距データ・基準画像・参照画像とを用いて残差を算出するための、残差算出ユニット 250 も含む。   The apparatus 200 also includes a residual calculation unit 250 for calculating a residual using the map 272 and the ranging data, the reference image, and the reference image.

装置 200 はまたさらに、残差算出ユニット 250 が算出した残差とオドメトリモデル 270 とから状態推定データを算出して格納し処理するための、状態推定データ格納／処理ユニット 260 も含む。   The apparatus 200 further includes a state estimation data storage / processing unit 260 for calculating, storing and processing state estimation data from the residuals calculated by the residual calculation unit 250 and the odometry model 270.

なお図２では簡単のため、移動体が二種類のセンサをひとつずつ持っていると記載してはあるが、これはあくまで一例であることに留意されたい。或る実施形態では、移動体が同じ種類のセンサを複数個有し、それらの出力を平均して用いてもかまわない。   In FIG. 2, for the sake of simplicity, it is described that the mobile body has two types of sensors one by one, but it should be noted that this is only an example. In an embodiment, the moving body may include a plurality of sensors of the same type, and their outputs may be used on average.

また図２では、記憶ユニット 210 が一個だけ存在していてデータ（ソフトウェア的に実施されるユニットを含む）がすべてのその中に格納されているかのように記載されているが、これも一例に過ぎない。或る実施形態では、複数の記憶ユニットを用いて、データを分けて格納してもかまわない。   FIG. 2 also shows that there is only one storage unit 210 and data (including software-implemented units) is stored in all of them. This is also an example. Not too much. In some embodiments, data may be stored separately using multiple storage units.

あるいは別の観点として、測距データ格納ユニット 232 と、基準画像格納ユニット 242 と、参照画像格納ユニット 244 とがそれぞれ個別の記憶装置を含むと考えてもよい。この場合、残差算出ユニット250 がそうした個別の記憶装置からデータを読みだして処理をしているのだと考えてもよい。状態推定データ格納／処理ユニット 260 についても同様のことが言える。   Alternatively, as another viewpoint, it may be considered that the distance measurement data storage unit 232, the standard image storage unit 242 and the reference image storage unit 244 each include a separate storage device. In this case, it may be considered that the residual calculation unit 250 reads and processes data from such individual storage devices. The same can be said for the state estimation data storage / processing unit 260.

なおプログラムとして実施形態を実施する場合には、移動体（例えば装置 200 、もしくは装置 200 に含まれる移動体）に含まれる要素（の一部）を、記憶ユニット（図１０に関して後述する外部記憶装置 1008 など）に保存されたデータに基づいたものとできる。それらのデータは、バッファ（図１０に関して後述するメモリ 1002 など）に読み出したうえで、実行ユニット（図１０に関して後述するCPU 1000 など）によって実行できる。   When the embodiment is implemented as a program, an element (part of the device) included in the mobile body (for example, the device 200 or the mobile body included in the device 200) is stored in a storage unit (external storage device described later with reference to FIG. 10). 1008 etc.). The data can be read out to a buffer (such as a memory 1002 described later with reference to FIG. 10) and executed by an execution unit (such as a CPU 1000 described later with reference to FIG. 10).

以上述べた記法・画法上の留意点については、以降の図においても矛盾を生じないかぎりは同様に考慮されたい。   The points noted in the notation and drawing method described above should be taken into consideration in the following figures as long as no contradiction arises.

装置 200 が行う処理動作を以下に概括する。初期状態（t=0）において、あらかじめ状態推定データ格納／処理ユニット 260 には、状態推定データの初期値を入力しておく。また、地図 272 の初期値として、測距センサ 230 がt=0で得た測距データを用いる。   The processing operations performed by the device 200 are summarized below. In the initial state (t = 0), the initial value of the state estimation data is input to the state estimation data storage / processing unit 260 in advance. Further, as the initial value of the map 272, the distance measurement data obtained by the distance sensor 230 at t = 0 is used.

残差算出ユニット 250 は、基準画像もしくは参照画像に、測距データを対応づける。すなわち、或る種の"合成データ" を作成すると考えてもよい。あるいは、こうした対応づけたデータを予測したもの（予測した「未来」のデータ）を "合成データ" として得てもよい。こうした "合成データ" としては例えば、以下に列挙するデータがある。或る時刻に得られた基準画像データに測距データを対応づけたもの。或る時刻に得られた参照画像データに測距データを対応づけたもの。或る時刻での地図（地図データ）から未来の時点での測距データを予測して得て、その未来の時点に対応する時刻に得られた基準画像データもしくは参照画像データに対応づけたもの。   The residual calculation unit 250 associates the distance measurement data with the standard image or the reference image. In other words, it may be considered that some kind of “composite data” is created. Alternatively, the predicted data (predicted “future” data) may be obtained as “composite data”. Examples of such “synthesis data” include the data listed below. Ranging data associated with reference image data obtained at a certain time. Ranging data associated with reference image data obtained at a certain time. What is obtained by predicting distance measurement data at a future time point from a map (map data) at a certain time, and corresponding to reference image data or reference image data obtained at a time corresponding to the future time point .

なおここで時刻の単位としては任意の単位時間を使用できる旨に留意されたい。つまり、時刻t-1と時刻tの間隔は、特定の時間に限定はされない。本実施形態では、クロックユニット 202 の設定により、1 secや1 msecや20 minなどの任意の単位時間を使用できる。   Note that any unit time can be used as the unit of time. That is, the interval between time t-1 and time t is not limited to a specific time. In the present embodiment, any unit time such as 1 sec, 1 msec, or 20 min can be used depending on the setting of the clock unit 202.

そして残差算出ユニット 250 は、或る時刻t（t>0）における残差 D を、前述した "合成データ" を複数種類用いて算出する。具体的には、或る合成データと別の合成データを（動的計画法などで）比較して残差を得ることができる。   Then, the residual calculation unit 250 calculates the residual D at a certain time t (t> 0) using a plurality of types of “composite data” described above. Specifically, a residual can be obtained by comparing (for example, dynamic programming) one composite data with another composite data.

状態推定データ格納／処理ユニット 260 は、状態推定データの初期値を設定して記憶ユニット（記憶ユニット 210 でもよいし別の記憶媒体でもかまわない）に格納する。そして状態推定データ格納／処理ユニット 260 はまた、オドメトリモデル 270 から得た時刻tでの移動体の移動量 u_t を使って、状態推定データの遷移を行う。ここで「遷移」とは例えば、状態推定データにて定められていた装置 200 の座標を、移動量 u_t だけずらす（座標に移動量 u_t を加算する）ことをいう。当然のことながらこれは一例に過ぎず、例えば移動量 u_t に何らかの重みをつけてから座標に加算してもかまわない。 The state estimation data storage / processing unit 260 sets an initial value of the state estimation data and stores it in a storage unit (the storage unit 210 or another storage medium may be used). Then, the state estimation data storage / processing unit 260 also performs transition of the state estimation data using the moving amount u _t of the moving object at the time t obtained from the odometry model 270. Here, “transition” refers to, for example, shifting the coordinates of the apparatus 200 defined by the state estimation data by the movement amount u _t (adding the movement amount u _t to the coordinates). As a matter of course, this is only an example, and for example, it is possible to add some weight to the moving amount u _t and add it to the coordinates.

また状態推定データ格納／処理ユニット 260 は、時刻tに関する残差 D を、状態推定データの評価値として定める。   The state estimation data storage / processing unit 260 determines the residual D with respect to time t as an evaluation value of the state estimation data.

そして、測距データ格納ユニット 232 は、時刻tでの測距データを、状態推定データに関する地図 272 に追加する。言い換えれば、環境内で移動体が進むことによって新たに計測することになった地点についてのデータを地図 272 に書き加えることで、地図 272 を更新するのである。このように地図 272 が更新されるということは、移動体が環境に関して新たに得た情報を用いて、環境を記述する手段である地図を再構築することを意味している。さらに言えば、状態推定データにより（例えば確率的に）推定される移動体の位置から、 "現実の環境" を計測したことになる測距データを、地図 272 に加えるのだと考えてもよい。つまり測距の原点が状態推定データによって定まるのだと考えてもらってもかまわない。（なぜなら、自律的に移動する移動体にとって、環境は、絶対座標系ではなく相対座標系で捉えるものであるからである。）   Then, the distance measurement data storage unit 232 adds the distance measurement data at time t to the map 272 regarding the state estimation data. In other words, the map 272 is updated by adding to the map 272 data on points that are to be newly measured as the moving object advances in the environment. The update of the map 272 in this way means that the mobile body reconstructs a map, which is a means for describing the environment, using information newly obtained about the environment. Furthermore, you may think that ranging data, which is the measurement of the “real environment”, is added to the map 272 from the position of the moving body estimated by the state estimation data (for example, probabilistically). . In other words, you may think that the origin of distance measurement is determined by the state estimation data. (This is because, for a moving body that moves autonomously, the environment is captured in a relative coordinate system, not an absolute coordinate system.)

そのようにして再構築された地図 272 を用いた移動を行うことにより、移動体の移動の精度を高めることが可能となる。   By performing movement using the map 272 reconstructed in such a manner, it is possible to improve the accuracy of movement of the moving object.

そうしてから、状態推定データ格納／処理ユニット 260 は、状態推定データを評価値に基づいて更新する。   The state estimation data storage / processing unit 260 then updates the state estimation data based on the evaluation value.

こうした処理を移動体の移動にともない時間的にくりかえすことで、より正確な地図とその地図に基づいた適切な移動が可能になるという効果が得られる。   By repeating such a process in time with the movement of the moving body, an effect that a more accurate map and appropriate movement based on the map can be obtained can be obtained.

図３は、パーティクルフィルタを用いた、装置 300 の構造を示すブロック図である。この実施形態では、状態推定データとしてパーティクルを使用する。装置 200 と同様に、装置 300 も移動体であってよいし、あるいは移動体を含んでもよい。   FIG. 3 is a block diagram showing the structure of the apparatus 300 using a particle filter. In this embodiment, particles are used as the state estimation data. Similar to device 200, device 300 may be a moving body or may include a moving body.

装置 300 は、状態推定系 303 と、走行制御系 380 とを含む。状態推定系 303 は、移動体の状態（移動体の位置や姿勢など）を推定するための系である。走行制御系 380 は、移動体の移動を状態推定に基づいて制御するための系である。   Device 300 includes a state estimation system 303 and a travel control system 380. The state estimation system 303 is a system for estimating the state of the moving body (such as the position and orientation of the moving body). The travel control system 380 is a system for controlling the movement of the moving body based on the state estimation.

状態推定系 303 は、測距センサ 330 と、ステレオカメラ 340 と、クロックユニット 302 と、測距データ格納ユニット 332 と、基準画像格納ユニット 342 と、参照画像格納ユニット 344 とを含む。これらの要素の役割と機能は、参照番号の下二桁が等しい図２の構成要素と同様であるので、ここでは説明をくりかえさない。   The state estimation system 303 includes a distance measurement sensor 330, a stereo camera 340, a clock unit 302, a distance measurement data storage unit 332, a standard image storage unit 342, and a reference image storage unit 344. Since the role and function of these elements are the same as those in FIG. 2 in which the last two digits of the reference numbers are the same, description thereof will not be repeated here.

また状態推定系 303 は、残差算出ユニット 350 およびパーティクルフィルタ処理ユニット 360 を含む。   The state estimation system 303 includes a residual calculation unit 350 and a particle filter processing unit 360.

残差算出ユニット 350 は、対応付けユニット 352 と、予測ユニット 356 と、計算ユニット 358 とを含む。対応付けユニット 352 は、画像情報抽出ユニット 354 を含む。   The residual calculation unit 350 includes an association unit 352, a prediction unit 356, and a calculation unit 358. The association unit 352 includes an image information extraction unit 354.

対応付けユニット 352 は、測距データを、基準画像もしくは参照画像と対応づける。なおここでいう"対応づける" とは、測距データに含まれるレンジポイントが、基準画像もしくは参照画像に写っているうちの何処にあたるのかを判断することを指す。対応付けユニット 352 は具体的には、測距データに含まれるレンジポイントの座標が、画像データの何処の点に相当するのかを判断する。例えば説明のためにごく簡単な例を述べると、測距センサの設置座標が (0,0,0) でステレオカメラの座標が (0,0,1) であったとする（つまり測距センサとステレオカメラの位置関係が既知であって座標系を共有できるとする）。すると、測距センサが得たレンジポイントの座標が (1,0,0) であったとしたら、その点がステレオカメラから見てx座標で1、z座標で-1離れた箇所に相当することは容易にわかる。また当然のことながら、測距センサの用いる座標系と、ステレオカメラが用いる座標系が別であって、その系間の変換規則を定めるようにしてもよい。見方を変えれば、対応付けユニット 352 は、その対応付けにあたり、測距センサ 330 からレンジポイントまでの投射線（スキャンライン）を、ステレオカメラ 340 が見ている空間に投影しているとも見做せる。そうして得られた対応付けたデータのことを、本明細書では「合成データ」とも称する。なお上述したように、本実施形態では、あくまで測距データは状態推定に基づいた原点からのものであることには留意されたい。   The association unit 352 associates the distance measurement data with the standard image or the reference image. Here, “associating” refers to determining where the range point included in the distance measurement data is in the standard image or the reference image. Specifically, the associating unit 352 determines where the coordinates of the range point included in the distance measurement data correspond to the point of the image data. For example, to explain a very simple example, suppose that the distance sensor installation coordinates are (0,0,0) and the stereo camera coordinates are (0,0,1). (The stereo camera's positional relationship is known and the coordinate system can be shared). Then, if the coordinates of the range point obtained by the distance measuring sensor are (1,0,0), that point corresponds to a point 1 away from the x coordinate and -1 away from the z coordinate when viewed from the stereo camera. Is easy to understand. As a matter of course, the coordinate system used by the distance measuring sensor and the coordinate system used by the stereo camera are different, and conversion rules between the systems may be determined. In other words, the mapping unit 352 can be regarded as projecting the projection line (scan line) from the distance measuring sensor 330 to the range point into the space that the stereo camera 340 is viewing. . The associated data thus obtained is also referred to as “composite data” in this specification. Note that, as described above, in the present embodiment, the distance measurement data is from the origin based on the state estimation.

こうした「合成データ」としては例えば、画像データ上に複数のレンジポイントをプロットしたものが想定できる。あるいは、そうしたプロットしたレンジポイントを連結するように補間処理を施したものを含めた「合成データ」としてもよい。こうすることで、プロットしたレンジポイントがスパース（疎）であっても、測距データと画像データの適切な合成（すなわち、適切なセンサフュージョン）を行うことが可能となる。   As such “composite data”, for example, a plurality of range points plotted on image data can be assumed. Or it is good also as "synthesis data" including what performed the interpolation process so that the plotted range points might be connected. By doing so, even if the plotted range points are sparse, it is possible to perform appropriate synthesis (ie, appropriate sensor fusion) of the distance measurement data and the image data.

対応付けユニット 352 に含まれる画像情報抽出ユニット 354 は、ステレオカメラ 340 から得た画像データから、画像情報を抽出する。そうした画像情報としては、色相、彩度、明度、もしくはエッジがあるが、これらに限定はされない。画像情報抽出ユニット 354 は画像データ中の任意の点から画像情報を得ることが可能である。例えば、上述したように画像データ上にプロットされたレンジポイントを連結する線上の点から画像情報を得てもよい。   The image information extraction unit 354 included in the association unit 352 extracts image information from the image data obtained from the stereo camera 340. Such image information includes, but is not limited to, hue, saturation, brightness, or edge. Image information extraction unit 354 can obtain image information from any point in the image data. For example, as described above, image information may be obtained from points on a line connecting range points plotted on image data.

予測ユニット 356 は、地図 372 を使い、測距データの予測を行う。具体的には、或る時点で移動体が居る地点について測距センサ 330 が観測を行ったら得られるであろう「予測測距データ」を、予測ユニット 356 は算出する。こうした「予測測距データ」は例えば、地図 372 から描かれる "空間" に測距センサ 330 のスキャンラインを投影するという"仮想的" な処理により得ることが可能である。こうした処理は、例えば座標系の変換によって行うことが可能である。そして予測ユニット 356 は、対応付けユニット 352 を介して得た基準画像もしくは参照画像に、そうした予測測距データを対応づけることで、「予測的な合成データ」を得ることが可能である。なお別の実施形態では、予測ユニット 356 が、対応付けユニット 352 を介さずに、基準画像格納ユニット 342 および参照画像格納ユニット 344 からそれぞれ基準画像データと参照画像データを得ることも可能である。   The prediction unit 356 uses the map 372 to predict distance measurement data. Specifically, the prediction unit 356 calculates “predicted distance measurement data” that will be obtained when the distance measurement sensor 330 observes a point where a moving body is present at a certain time. Such “predicted distance measurement data” can be obtained by, for example, a “virtual” process of projecting the scan line of the distance measurement sensor 330 onto the “space” drawn from the map 372. Such processing can be performed by, for example, coordinate system conversion. The prediction unit 356 can obtain “predictive synthesized data” by associating such predicted distance measurement data with the standard image or reference image obtained via the association unit 352. In still another embodiment, the prediction unit 356 can obtain the standard image data and the reference image data from the standard image storage unit 342 and the reference image storage unit 344, respectively, without using the association unit 352.

計算ユニット 358 は、対応付けユニット 352 から得た「合成データ」もしくは予測ユニット 356 から得た「予測的な合成データ」を複数種得て、それら同士の残差を計算する。残差の算出に際しては例えば、そうした「合成データ」から抽出した画像情報を使って残差を算出してもよい。   The calculation unit 358 obtains a plurality of types of “synthesis data” obtained from the association unit 352 or “predictive synthesis data” obtained from the prediction unit 356, and calculates a residual between them. When calculating the residual, for example, the residual may be calculated using image information extracted from such “composite data”.

さらに状態推定系 303 は、記憶ユニット 310 を含む。記憶ユニット 310 は、記憶ユニット 210 とは違い、地図 372 を含むがオドメトリモデル 270 に相当するものは含んでいない。見方を変えれば、オドメトリモデル 270 は走行制御系 380 に相当するといえる。   Further, the state estimation system 303 includes a storage unit 310. Unlike the storage unit 210, the storage unit 310 includes a map 372 but does not include an equivalent to the odometry model 270. In other words, it can be said that the odometry model 270 corresponds to the traveling control system 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 は正規化のための定数である。 The particle filter processing unit 360 has a particle set 362 including particles that are information for estimating the state of the moving object. The initial values of the particles included in the particle set 362 can be arbitrarily set. For example, the initial values of the evaluation values (weights) of each particle may be uniform. The particle filter processing unit 360 updates the particle set 362 in response to the output from the residual calculation unit 350 described above and the output from the self-state storage memory # 1 386 described later. Specifically, the particle filter processing unit 360 uses the value of the particle set 362 (particles included) as the relative movement amount u _t from the self-state storage memory # 1 386 to the moving object from time t-1 to time t. Can be added (or any operation can be performed). Further, the particle filter processing unit 360 can set the residual D at the time t obtained from the residual calculation unit 350 as the evaluation value of the particle set 362. At this time, for example, if the evaluation value of the i-th particle (i is a natural number) is set to w _i , w _i = e ^{(−D / T)} can be obtained. Where e is the base of the natural logarithm and T is a constant for normalization.

なお図３では地図 372 が単独であるかのように記載してあるが、別の実施形態では任意の数の地図を記憶ユニット 310 が格納してもよい。例えば或る実施形態では、パーティクルセット 362 に含まれるパーティクルの各々について独立に、地図 372 が存在してもよい。   In FIG. 3, the map 372 is described as if it is a single unit. However, in another embodiment, the storage unit 310 may store any number of maps. For example, in some embodiments, a map 372 may exist independently for each of the particles included in the particle set 362.

以下にパーティクルの評価値についての具体的な例示を行うが、本発明の実施形態はこれには限定はされない。   Specific examples of particle evaluation values are given below, but embodiments of the present invention are not limited to this.

パーティクルセットをX_tとおくと、
X_t:= x_t ^[1], x_t ^[2], ... , x_t ^[M] (1)
と表せる。ここでMはパーティクルの個数である。おのおののパーティクル x_t ^[m](1≦m≦M) は、時刻tでの状態に関するひとつの仮説である。 If the particle set is X _t ,
X _t : = x _t ^[1] , x _t ^[2] , ..., x _t ^[M] (1)
It can be expressed. Here, M is the number of particles. Each particle x _t ^[m] (1 ≦ m ≦ M) is a hypothesis about the state at time 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の起こる確率である。 The likelihood that a certain state particle (hypothesis) x _t is included in the particle set X _t is ideally proportional to the posterior belief bel (x _t ) of the Bayes filter. That is, particle x _t
x _t ^[m] 〜 p (x _t | z _{1: t} , u _{1: t} ) (2)
Should be chosen to be Here, p (A | B) is Bayes's conditional probability, which is the probability of occurrence of another event A under the condition that a certain event B occurs.

時刻tのパーティクルx_t ^[m]は、以下のようにサンプリング（採取）できる。
x_t ^[m]〜 p(x_t | u_t,x_t-1 ^[m]) (3) The particle x _t ^{[m] at} time t can be sampled (collected) as follows.
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）データであり、例えば上述したオドメトリモデルに相当する。 Evaluation value for the particle x _t ^[m] (weight) w _t ^[m] can be calculated as follows.
w _t ^[m] = p (z _t | x _t ^[m] ) (4)
Here, z _t is measurement data at time t, and corresponds to, for example, the odometry model described above.

つまり、w_t ^[m]により重みづけられたパーティクルからなるパーティクルセットは、Bayesフィルタの事後信念 bel(x_t) の近似表現となる。 In other words, a particle set consisting of particles weighted by w _t ^[m] is an approximate expression of the posterior belief bel (x _t ) of the Bayes filter.

図３の説明に戻る。走行制御系 380 は、内界センサ 382 と、自己状態推定ユニット 384 と、自己状態記憶メモリユニット#1 386 と、自己状態記憶メモリユニット#2 388 とが含まれる。   Returning to the description of FIG. The travel control system 380 includes an internal sensor 382, a self-state estimating unit 384, a self-state storage memory unit # 1 386, and a self-state storage memory unit # 2 388.

内界センサ 382 は、移動機構 390 などを測定し、その結果となるデータは自己状態推定ユニット 384 へと送られる。   The internal sensor 382 measures the moving mechanism 390 and the like, and the resulting data is sent to the self-state estimation unit 384.

自己状態推定ユニット 384 では、内界センサ 382 の出力を基にして、移動体の状態推定を行う。自己状態推定ユニット 384 は例えば、地図（図３に示していないものでもよいし、あるいは、地図 372 を使用してもよい）を参照して、移動体の位置を推測することができる。あるいは、移動体の速度や、向いている方位を推測することも可能である。   The self-state estimation unit 384 estimates the state of the moving object based on the output of the internal sensor 382. For example, the self-state estimation unit 384 can estimate the position of the moving object with reference to a map (not shown in FIG. 3 or the map 372 may be used). Alternatively, it is possible to estimate the speed of the moving body and the direction it is facing.

自己状態推定ユニット 384 の出力は、自己状態記憶メモリユニット#1 386 と自己状態記憶メモリユニット#2 388 とへ送られる。このとき、双方へ同じ内容のデータを送ってもよいし、もしくは異なる重みをつけて送信してもよい。同じ内容のデータを用いる場合には、初期の時点（すなわち、装置 300 の稼動直後）では、自己状態記憶メモリユニット#1 386 と自己状態記憶メモリユニット#2 388 とは同じデータを持つことになる。その後、自己状態記憶メモリユニット#1 386 からの出力が状態推定系 303 へ送られてやがて自己状態記憶メモリユニット#2 388 へとフィードバックされると、内界センサ 382 の出力結果を補ったより精度の高い状態推定が可能になるという効果を得ることができる。   The output of the self-state estimation unit 384 is sent to the self-state storage memory unit # 1 386 and the self-state storage memory unit # 2 388. At this time, data having the same content may be sent to both sides, or may be sent with different weights. If the same data is used, the self-state storage memory unit # 1 386 and the self-state storage memory unit # 2 388 have the same data at the initial time point (that is, immediately after the operation of the device 300). . After that, when the output from the self-state storage memory unit # 1 386 is sent to the state estimation system 303 and eventually fed back to the self-state storage memory unit # 2 388, the output result of the internal sensor 382 is compensated more accurately. The effect that high state estimation becomes possible can be acquired.

〔センサの出力データ〕
図４は、測距センサ（測距センサ 230 など）が出力可能な測距データの一例を示す図である。例えば測距センサがレーザーレンジファインダである場合、センサから発せられたレーザービームが環境中の障害物（壁や天井など）にヒットし、レンジポイントの座標を得ることができる。 [Sensor output data]
FIG. 4 is a diagram illustrating an example of distance measurement data that can be output by a distance measurement sensor (such as the distance measurement sensor 230). For example, when the distance measuring sensor is a laser range finder, the laser beam emitted from the sensor hits an obstacle (such as a wall or ceiling) in the environment, and the coordinates of the range point can be obtained.

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

ｘ ｙ ｚ
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
The following are examples of data (range data) that are three-dimensional coordinates.

x y z
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座標を床に投影して、二次元的なデータとしている。しかし実施形態はこれには限定はされず、三次元的なデータをそのまま測距データとしてもかまわない。   In the example of FIG. 4, the x-coordinate and y-coordinate of the range data obtained in this way are projected on the floor to obtain two-dimensional data. However, the embodiment is not limited to this, and three-dimensional data may be used as distance measurement data as it is.

また、図４では、移動体（自律ロボットなど）の位置を丸で示してあり、丸についた短い直線が移動体の向きを示している。
図５は、ビジョンセンサ（ステレオカメラ 240 など）が出力可能な画像データの一例を示す図である。パネル(A)とパネル(B)にそれぞれ基準画像と参照画像が表示されている。これらの画像は、パネル(C)の平面地図に対応したものである。 In FIG. 4, the position of a moving body (such as an autonomous robot) is indicated by a circle, and a short straight line attached to the circle indicates the direction of the moving body.
FIG. 5 is a diagram illustrating an example of image data that can be output by a vision sensor (such as the stereo camera 240). A standard image and a reference image are displayed on the panels (A) and (B), respectively. These images correspond to the planar map of panel (C).

パネル(A)とパネル(B)には、実線で画像データが示してある。また、破線で測距センサがスキャンを行ったライン（すなわち、スキャンラインの投射軌跡）を示してある。この軌跡上に、レンジポイントがプロットされることになる。また図５のパネル(C)では、移動体（自律ロボットなど）の位置を丸で示してあり、丸についた短い矢印が移動体の向きもしくは進行方向を示している。   In the panel (A) and the panel (B), image data is indicated by solid lines. A broken line indicates a line scanned by the distance measuring sensor (that is, a projected trajectory of the scan line). Range points are plotted on this locus. Further, in the panel (C) of FIG. 5, the position of the moving body (autonomous robot or the like) is indicated by a circle, and a short arrow attached to the circle indicates the direction or traveling direction of the moving body.

〔センサフュージョン手法〕
図６は、実施形態にかかるセンサフュージョンの手法のうち、「経時変化を観る」ための残差 D_b の導出手法を説明するための概要図である。ここでは、移動体600 が時刻t-1から時刻tにかけて得る測距データの変化を観ていると考えてもらってもよい。 [Sensor fusion method]
6, of the approach of the sensor fusion according to the embodiment, is a schematic diagram illustrating the derivation method of the residual D _b for "watch the change with time." Here, it may be considered that the moving body 600 is watching a change in distance measurement data obtained from time t-1 to time 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 など）が取得した環境を写した画像も含まれていることになる（図５も併せて参照されたい）。 In the panel (A) of FIG. 6, the scan lines projected from the distance measuring sensor of the moving body 600 to the environment (the shaded area) are drawn radially. The intersection of the environment and the scan line corresponds to the range point. In FIG. 6, the environment is depicted two-dimensionally for simplicity, but this is merely for convenience of expression, and the distance measurement data and the image data are not intended to be limited to two dimensions. . Panel (A) also schematically shows composite data l ^b _t-1 610 in which image data (reference image data in this example) and distance measurement data S _t-1 are associated with each other. Here, the superscript attached to the symbol “l” (lowercase letter L) indicates whether the composite data is base image data (“b” in base) or reference image data (“r” in reference) It is shown. The subscript attached to the symbol “l” indicates the time to which the composite data belongs. l ^b _t-1 610 contains the projected range points (in this example, (K + 1), K is an integer greater than or _{equal to} 0), and the range points are interpolated and connected by lines Has been. The value of K can be arbitrarily determined depending on the setting and performance of the distance measuring sensor. In other words, such an “interpolation line” is a line obtained by two-dimensionally projecting a plane that cuts the environment space from the scan plane formed by the trajectory of the scan line of the distance measuring sensor. In Fig. 6, for the sake of paper width, l ^b _t-1 610 only draws range points and interpolation lines, but actually includes images that capture the environment acquired by a vision sensor (such as stereo camera 240). (See also FIG. 5).

さて図６のパネル(B)には、単位時間だけ時間が進んで時刻tになったときの移動体 600 の状態を示してある。移動体 600 は、 u_t だけ移動したことになる（これは簡易的な表現であり、実際にはu_t には位置座標のほか姿勢などに関する情報も含めることができる）。移動体 600 の持つ測距センサから環境へ投射されたスキャンラインを放射状に描いてあるのはパネル(A)と同様である。しかし移動体 600 は移動しているので、ビジョンセンサから見える環境は異なっている（換言すれば、違った位置から同じものを見るため見かけが変わる）。そしてパネル(B)にはまた、画像データ（この例では基準画像データ）と測距データ S_tを対応づけた合成データ l^b _t 612 を模式的に示してある。 l^b _t 612 にも、(K+1)個のレンジポイントが投影されている。 Now, the panel (B) in FIG. 6 shows the state of the moving body 600 when the time advances by unit time and reaches time t. The moving body 600 has moved by u _t (this is a simple expression, and actually, u _t can include information on the posture in addition to the position coordinates). The scan lines projected to the environment from the distance measuring sensor of the moving body 600 are drawn in the same manner as the panel (A). However, since the moving body 600 is moving, the environment seen from the vision sensor is different (in other words, the appearance changes because the same thing is seen from different positions). And also the panel (B), the synthetic data l ^b _t 612 that associates the distance measurement data S _t (the reference image data in this example) image data is shown schematically. (K + 1) range points are also projected on l ^b _t 612.

そして図６のパネル(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 は、従来技術により、例えば画像データを明度画像（グレースケール画像）に変換して得ることが可能である。 A panel (C) in FIG. 6 shows a method for obtaining a residual between l ^b _t-1 610 and l ^b _t 612. In this example, image information (for example, hue, saturation, brightness, or edge) is extracted along the interpolation line. In panel (C), the image information is drawn as a curve developed along the axis of length K. Where the vertical axis of the graph corresponds to l ^b _t-1 610, the horizontal axis corresponds to l ^b _t 612, and the grid of the graph corresponds to the number of range points (0, 1, ..., K) . The image information extraction unit (such as image information extraction unit 354) starts from l ^b _t-1 610 and follows the hue hue _t-1 , saturation sat _t-1 , lightness v _t-1 , edge dI _t- Extract ₁ Image information extraction units in the same manner from l ^b _t 612, the hue hue _t along interpolated line (or near) saturation sat _t, lightness v _t, extracts an edge dI _t. The extracted data can be stored in a storage unit (such as storage unit 310). Note that the edge dI can be obtained by converting image data into a lightness image (grayscale image), for example, by conventional techniques.

そして動的計画法により、 D_b = φ(l^b _t-1,l^b _t) として残差を求める。実際には、 l^b _t-1 もしくは l^b _t から抽出された画像情報を使って残差を求めている（例えば hue_t-1 とhue_t から求めている）と考えてもよい。 Then, by dynamic programming, the residual is obtained as D _b = φ (l ^b _t-1 , l ^b _t ). Actually, it may be considered that the residual is obtained using image information extracted from l ^b _t-1 or l ^b _t (for example, obtained from hue _t-1 and hue _t ).

さらに動的計画法について詳述する。
実施形態にかかる動的計画法は、例えば従来技術である音声マッチングと同様の手法（画像データの勾配やラプラシアンを用いた演算など）をとってもよい。あるいは、以下に示すアルゴリズム1-3を用いてもよい。 Furthermore, dynamic programming is described in detail.
The dynamic programming method according to the embodiment may take, for example, the same technique as that of speech matching that is a conventional technique (such as a calculation using image data gradient or Laplacian). Alternatively, algorithm 1-3 shown below may be used.

{アルゴリズム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}
となるとする。 {Algorithm 1: Dynamic programming initial setting}
First, a simple example will be described as an explanation of symbols. If 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}
Suppose that

また、
f{i,j} = E_i - E_j
とする。ここでEは任意の画像情報であり、例えば上述した hue, sat, v, dI である。 Also,
f {i, j} = E _i -E _j
And Here, E is arbitrary image information, for example, hue, sat, v, dI described above.

そして f{i,j} から g{i,j} を以下のようにして定める。   Then g {i, j} is determined from f {i, j} as follows.

アルゴリズム1ではつまり、点 (i,j) での残差である f{i,j} をそのまま求めるのではなく、別に誘導した g{i,j} を介して求める前処理をしている。このように多項式問題に変換することで、処理にかかる時間を直接 f{i,j} を再帰的に求めた場合よりも軽減できる。   In other words, algorithm 1 does not calculate the residual f {i, j} at the point (i, j) as it is, but preprocesses it through g {i, j} derived separately. By converting to a polynomial problem in this way, the processing time can be reduced compared to the case where f {i, j} is directly obtained recursively.

{アルゴリズム2： 動的計画法におけるコスト計算}
{Algorithm 2: Cost calculation in dynamic programming}

ここで Y = argmin{X} は、Yを最小化させるXを示す。
アルゴリズム2では、コスト s{i,j} とg{i,j} を求めて残差 D を求める下準備をしている。 Here Y = argmin {X} indicates X that minimizes Y.
In Algorithm 2, the cost s {i, j} and g {i, j} are calculated to prepare for the residual D.

{アルゴリズム3： 動的計画法による残差D の算出}
{Algorithm 3: Calculation of residual D by dynamic programming}

アルゴリズム3では、コスト s{i,j} とg{i,j} から、残差 D を求めている。   In Algorithm 3, the residual D is obtained from the costs s {i, j} and g {i, j}.

ふたたびセンサフュージョンの手法の説明に戻る。   Returning to the description of the sensor fusion technique again.

図７は、実施形態にかかるセンサフュージョンの手法のうち、「予測された測距データを基準画像と参照画像にそれぞれ投影したものの差を観る」ための残差 D_w の導出手法を説明するための概要図である。ここでは、時刻tにおいて予測される移動体 700 の位置から投射される時刻tにおいて予測できる測距データを、異なる画像データへ投影したときの差異を観ていると考えてもらってもよい。 FIG. 7 is a diagram for explaining a method for deriving a residual D _w for “observing a difference between projected distance measurement data on a reference image and a reference image” among the sensor fusion methods according to the embodiment. FIG. Here, it may be considered that the distance measurement data that can be predicted at time t projected from the position of the moving body 700 predicted at time t is viewed as a difference when projected onto different image data.

図７のパネル(A)では、移動体 700 （の持つパーティクルセットのうちの）の持つパーティクルに対応する地図を用いて、測距データの予測を行っている。図７の上パネルでは、i番目のパーティクルが持つ時刻t-1における地図 Mⁱ _t-1 を例として示してある。ここで地図をパーティクルごとに設定しているのは、パーティクルにより定められる移動体 700 の位置・姿勢はそれぞれに異なる場合がある（異なるほうが普通）ためである。つまり、図７の移動体 700 の位置・姿勢・進行方向は、あくまで状態推定を経て確率的に予測されたものである（破線で描いてあるのはそういう意味である）。周知のように、パーティクルフィルタを使うことで、 "評価値のぶんだけ尤もらしく" 移動体の位置・姿勢・進行方向を確率的に定めることが可能である。 In the panel (A) of FIG. 7, the distance measurement data is predicted using a map corresponding to the particles of the moving object 700 (of the particle set of the moving object 700). In the upper panel of FIG. 7, a map M ⁱ _t-1 at time t-1 of the i-th particle is shown as an example. Here, the map is set for each particle because the position / orientation of the moving body 700 determined by the particle may be different (it is usually different). That is, the position / posture / traveling direction of the moving body 700 in FIG. 7 is only probabilistically predicted through state estimation (that is, the fact that it is drawn with a broken line is the meaning). As is well known, by using a particle filter, it is possible to determine the position / posture / traveling direction of a mobile object “probably as much as the evaluation value”.

さていま移動体 700 が持つパーティクルがN個（1≦i≦N）であるとすると、移動体 700 はN回予測をしてその平均をとって合成データを得ることができる。別の実施形態では、N以外の回数、予測を行ってもよい。または別の手法として、パーティクルの評価値に応じて、加重平均をとってもよい。あるいは任意の統計数学的手法を適用して合成データを得てもよい。   Assuming that the moving body 700 has N particles (1 ≦ i ≦ N), the moving body 700 can predict N times and take the average to obtain composite data. In another embodiment, the prediction may be performed a number of times other than N. Alternatively, as another method, a weighted average may be taken according to the evaluation value of the particles. Alternatively, synthetic data may be obtained by applying any statistical mathematical method.

こうして、時刻tでの基準画像についての「予測的な合成データ」 (l^b _t)' 710 、および、時刻tでの参照画像についての「予測的な合成データ」 (l^r _t)' 712 を得ることができる。なおここで記号lに付された括弧とプライムは予測的なものであることを示している。 Thus, “predictive composite data” (l ^b _t ) ′ 710 for the reference image at time t and “predictive composite data” (l ^r _t ) ′ 712 for the reference image at time t Obtainable. Here, the parenthesis and prime attached to the symbol l indicate that they are predictive.

そして図７のパネル(B)には、 (l^b _t)' 710 と (l^r _t)' 712 の残差を求める手法を示してある。処理の流れは図６に関して説明したところと同様であるのでここではくりかえさない。動的計画法により、 D_w = φ((l^b _t)',(l^r _t)') を求める。 In the panel (B) of FIG. 7, a technique for obtaining the residual of (l ^b _t ) ′ 710 and (l ^r _t ) ′ 712 is shown. Since the processing flow is the same as that described with reference to FIG. 6, it will not be repeated here. D _w = φ ((l ^b _t ) ', (l ^r _t )') is obtained by dynamic programming.

図８は、実施形態にかかるセンサフュージョンの手法のうち、「実測測距データと予測された測距データをそれそれ画像データに投影したものの差を観る」ための残差 D_t の導出手法を説明するための概要図である。ここでは、時刻tにおける移動体 800 から得られた実際の測距データと、時刻tにおいて予測される移動体 802 の位置から投射される時刻tにおいて予測できる測距データを、同じ画像データ（この例では基準画像データ）へ投影したときの差異を観ていると考えてもらってもよい。 FIG. 8 shows a method of deriving a residual D _t for “observing the difference between the measured distance data and the predicted distance data projected on the image data” among the sensor fusion techniques according to the embodiment. It is a schematic diagram for demonstrating. Here, the actual distance measurement data obtained from the mobile object 800 at time t and the distance measurement data that can be predicted at time t projected from the position of the mobile object 802 predicted at time t are the same image data (this In the example, it may be considered that the difference when projected onto the reference image data) is viewed.

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)') と求める。 Since l ^b _t 810 and (l ^b _t ) ′ 812 are the same as l ^b _t 612 and (l ^b _t ) ′ 710 in FIG. 6, respectively, the derivation thereof is not repeated here for the convenience of the paper width. Then, as described in the panel (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 を算出してもよい。 The three types of residuals obtained from FIG. 6 to FIG. 8 can be used as an evaluation value of the state estimation data by the state estimation data storage / processing unit (such as the particle filter processing unit 360). As such an evaluation value, any one of these three types may be used, or a combination obtained by performing some kind of calculation may be used. In some embodiments, the residual D used as the evaluation value of the state estimation data can be calculated as _{D = D t · D w ·} D b. In another embodiment, it can be calculated as _{D = (D t + D w} ) / D b. Any other calculation may be used to calculate D.

残差 D_b はオドメトリモデルに依存するので、 D_bはオドメトリモデルの精度の確認のためのパラメータとして使用可能である。また、残差 D_t, D_w は地図に依存するので、 D_t, D_w は地図の精度の確認のためのパラメータとして使用可能である。 Since the residual D _b depends on odometry model, D _b can be used as a parameter for the confirmation of the accuracy of the odometry model. Since the residuals D _t and D _w depend on the map, D _t and D _w can be used as parameters for checking the accuracy of the map.

変形例として、図６から図８にかけて求めてきた三種類の残差に代えて、 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 を得てもよい。 As a modification, D _b '= φ (l ^r _t−1 , l ^r _t ), D _w = φ ((l ^b _t ) ′ instead of the three types of residuals obtained from FIGS. _{^{, (l r t) ')}} , D t' = φ (l r t, (l r t) may be obtained D using three types of ').

図９は、実施形態にかかるパーティクルフィルタを用いた状態推定系（状態推定系 303 など）の動作を説明するためのフローチャートである。
まずステップ S900 にて、パーティクルフィルタ処理ユニット（パーティクルフィルタ処理ユニット360 など）が、パーティクルの初期設定を行う。例えば、N個のパーティクルを一様に設定することができる。 FIG. 9 is a flowchart for explaining the operation of a state estimation system (such as the state estimation system 303) using the particle filter according to the embodiment.
First, in step S900, a particle filter processing unit (such as a particle filter processing unit 360) performs initial setting of particles. For example, N particles can be set uniformly.

次にステップ S902 、時刻tにて、測距センサ（測距センサ 330 など）が、測距データを取得して（測距データ格納ユニット 332 などへ）格納する。
そしてステップ S904 にて、ステレオカメラ（ステレオカメラ 340 など）が、画像データを取得して（基準画像格納ユニット 342 および参照画像格納ユニット 344 などへ）格納する。 Next, at step S902, at time t, the distance measuring sensor (the distance measuring sensor 330 or the like) acquires the distance measurement data (in the distance measurement data storage unit 332 or the like) and stores it.
In step S904, the stereo camera (stereo camera 340 or the like) acquires the image data (stores in the standard image storage unit 342 and the reference image storage unit 344 or the like) and stores it.

ステップ S906 では、残差算出ユニット（残差算出ユニット 350 に含まれる対応付けユニット 352 など）が、画像データへ測距データを投影する（対応づける処理を行う）ことで、投影線（例えば上述した補間線）を得る。換言すれば、ここで上述した合成データを得ていると見做してもよい。   In step S906, the residual calculation unit (such as the association unit 352 included in the residual calculation unit 350) projects the distance measurement data onto the image data (performs the association process), thereby producing a projection line (for example, the above-described case). Interpolation line) is obtained. In other words, it may be considered that the composite data described above is obtained.

そしてステップ S908 では、残差算出ユニット（残差算出ユニット 350 に含まれる画像情報抽出ユニット 354 など）が、投影線の上もしくはその近傍から、画像情報を抽出して、時刻tにおける合成データ l^b _t を得る。 In step S908, a residual calculation unit (such as the image information extraction unit 354 included in the residual calculation unit 350) extracts image information from or near the projection line, and combines the composite data l ^{b at} time t. get _t .

ステップ S910 では、残差算出ユニット（残差算出ユニット 350 に含まれる画像情報抽出ユニット 354 など）が、記憶ユニット（記憶ユニット 310 など）に保存していた時刻t-1における合成データ l^b _t-1 を得る。そして、残差算出ユニット（残差算出ユニット 350 に含まれる計算ユニット 358 など）が、残差 D_b を算出する。その算出に際しては動的計画法を使用できる。 In step S910, the composite data l ^b _{t− at} time t−1 stored in the storage unit (eg, storage unit 310) by the residual calculation unit (eg, image information extraction unit 354 included in residual calculation unit 350). _{Get one} . Then, the residual difference calculation unit (such as a calculation unit 358 contained in the residual calculating unit 350) calculates a residual D _b. Dynamic programming can be used for the calculation.

ステップ S912 からステップ S930 まで続くループを開始する。このループは各パーティクルに関して行われる。   Start the loop from step S912 to step S930. This loop is performed for each particle.

ステップ S914 では、走行系（走行制御系 380 など）によりオドメトリモデルが出力（更新）される。それを承けてステップ S916 では、パーティクルフィルタ処理ユニットが、パーティクルの持つ位置・姿勢といったデータを遷移させる。   In step S914, the odometry model is output (updated) by the traveling system (such as the traveling control system 380). In response, in step S916, the particle filter processing unit transitions data such as the position and orientation of the particles.

ステップ S918 では、残差算出ユニット（残差算出ユニット 350 に含まれる予測ユニット 356 など）が、i番目のパーティクルに対応する最新の地図（例えば、時刻t-1での地図 Mⁱ _t-1 ）を用いて、測距センサの計測値を予測する。（換言すれば、予測測距データを得る。） In step S918, the residual calculation unit (such as the prediction unit 356 included in the residual calculation unit 350) updates the latest map corresponding to the i-th particle (for example, the map M ⁱ _t-1 at time _t-1 ). Is used to predict the measured value of the distance measuring sensor. (In other words, predictive ranging data is obtained.)

ステップ S920 では、残差算出ユニットが、ステップ S918 で得た予測測距データと基準画像を使って、投影線を含んだ予測合成データ (l^b _t)', (l^r _t)' を得る。そして残差算出ユニットが投影線の上もしくは近傍での画像情報を抽出する。 In step S920, the residual calculation unit obtains predicted composite data (l ^b _t ) ′ and (l ^r _t ) ′ including the projection line using the predicted distance measurement data and the reference image obtained in step S918. Then, the residual calculation unit extracts image information on or near the projection line.

ステップ S922 では、残差算出ユニットが、ステップ S918 で得た予測測距データと参照画像を使って、投影線を含んだ予測合成データ (l^r _t)' を得る。そして残差算出ユニットが投影線の上もしくは近傍での画像情報を抽出する。 In step S922, the residual calculating unit, using the reference image and the prediction distance data obtained in step S918, obtaining the prediction synthesis data including projection line (l ^r _t) '. Then, the residual calculation unit extracts image information on or near the projection line.

そしてステップ S924 では、残差算出ユニット（残差算出ユニット 350 に含まれる計算ユニット 358 など）が、 l^b _t と (l^b _t)' とから、残差 D_tを算出する。 In step S924, a residual calculation unit (such as the calculation unit 358 included in the residual calculation unit 350) calculates a residual D _t from l ^b _t and (l ^b _t ) ′.

続いてステップ S926 では、残差算出ユニットが、 (l^b _t)' と (l^r _t)' とから、残差 D_w を算出する。 Subsequently, in step S926, the residual calculation unit calculates a residual D _w from (l ^b _t ) ′ and (l ^r _t ) ′.

こうしてステップ S928 では、残差算出ユニットが、 これまでに得た残差 D_b, D_t, D_w から D = D_b・D_t・D_w を算出する。 In this way the step S928, the residual difference calculation unit, so far obtained residual D _b, D _t, to calculate the _{D = D b · D t ·} D w from D _w.

そしてステップ S930 では、パーティクルフィルタ処理ユニットが、i番目のパーティクルの評価値 w_i を D とする。そしてループはステップ S916 へ戻る。 In step S930, the particle filter processing unit sets the evaluation value w _i of the i-th particle to D. The loop then returns to step S916.

すべてのパーティクルについて上述の処理が完了したところで、フローはステップ S932 へ進む。ステップ S932 では、パーティクルフィルタ処理ユニットが、各パーティクルの評価値 w を、
となるように正規化する。 When the above processing is completed for all particles, the flow proceeds to step S932. In step S932, the particle filtering unit calculates the evaluation value w of each particle,
Normalize so that

そしてステップ S934 では、パーティクルフィルタ処理ユニットが、評価値 w を用いてパーティクルのリサンプリング（淘汰）を行う。そして測距データ格納ユニットが、時刻tでの測距データを地図 Mⁱ _t-1 に追加して更新し、記憶ユニットは地図 Mⁱ _t を得る。そうしてからフローはステップ S902 へ戻り、処理が続いてゆく。 In step S934, the particle filter processing unit performs resampling (淘汰) of the particles using the evaluation value w. Then, the ranging data storage unit adds the ranging data at time t to the map M ⁱ _t-1 and updates it, and the storage unit obtains the map M ⁱ _t . The flow then returns to step S902 and processing continues.

なおここでパーティクルのリサンプリングは、N個のパーティクルのうち評価値が所定の閾値に満たなかったものを削除し、残ったパーティクルを評価値に応じた数だけコピーして増やすことで行うことが可能である。   Note that the resampling of particles can be performed by deleting the N particles whose evaluation value did not reach the predetermined threshold and copying and increasing the number of remaining particles according to the evaluation value. Is possible.

なお、本明細書では実施形態の各種データについて、理解しやすいよう簡単な例を開示してはいるが、この例に限らず、データを対応付けて管理可能なXML形式やツリー構造形式やテーブル形式などの他の管理形式を採用してもよい。   In this specification, a simple example is disclosed for easy understanding of the various data of the embodiment. However, the present invention is not limited to this example. Other management formats such as formats may be employed.

〔ハードウェア〕
図１０は、実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。ここでは、CPU 1000 、メモリ 1002 、入力装置 1004 、出力装置 1006 、外部記憶装置 1008 、媒体駆動装置 1010 、可搬記録媒体 1012 、ネットワーク接続装置 1014 が、バス 1016 によって接続されている。 〔hardware〕
FIG. 10 is a schematic diagram illustrating a hardware structure capable of executing the program according to the embodiment. Here, a CPU 1000, a memory 1002, an input device 1004, an output device 1006, an external storage device 1008, a medium drive device 1010, a portable recording medium 1012, and a network connection device 1014 are connected by a bus 1016.

またこうしたハードウェア構造は、サブモジュールごとに構築してもよいし、移動体ごとに構築してもかまわないことに留意されたい。   It should be noted that such a hardware structure may be constructed for each submodule or for each mobile unit.

CPU（中央処理ユニット） 1000 としては、任意の種類のプロセッサを使用でき、例えばMPUや組み込み用途CPUを使用できる。CPU 1000 は、実施形態にかかるプログラムをバッファ（メモリ 1002 など）に読み出したうえで実行できる。   As the CPU (central processing unit) 1000, any kind of processor can be used, for example, an MPU or an embedded CPU. The CPU 1000 can execute the program according to the embodiment after reading the program according to the embodiment to a buffer (memory 1002 or the like).

メモリ 1002 としては、ダイナミックランダムアクセスメモリ（DRAM）が含まれるがこれには限定はされない。そうしたDRAMとしては、SDRAM、SLDRAM、RDRAM、および他のDRAMなどがある。メモリ 1002 にはまた、フラッシュメモリなどの不揮発性メモリを含めてもよい。   Memory 1002 includes, but is not limited to, dynamic random access memory (DRAM). Such DRAMs include SDRAM, SLDRAM, RDRAM, and other DRAMs. Memory 1002 may also include non-volatile memory such as flash memory.

入力装置 1004 としては、キーボード、マウス、トラックボール、タッチパネル、ゲームコントローラー、もしくは音声認識装置といった、ユーザーが情報を入力しできる任意の装置が含まれる。   The input device 1004 includes any device that allows the user to input information, such as a keyboard, mouse, trackball, touch panel, game controller, or voice recognition device.

出力装置 1006 としては、2Dディスプレイ、スピーカー、3Dディスプレイといった、情報を出力してユーザーへ伝達できる任意の装置が含まれる。
外部記憶装置 1008 としては、同様に特定の用途に適するひとつ以上の記憶媒体を使用でき、例えば、ハードディスクドライブ、SSD（Solid State Drive）を使用できる。 The output device 1006 includes any device that can output information and transmit it to the user, such as a 2D display, a speaker, or a 3D display.
Similarly, one or more storage media suitable for a specific application can be used as the external storage device 1008. For example, a hard disk drive or an SSD (Solid State Drive) can be used.

媒体駆動装置 1010 には、任意の種類の可搬記録媒体 1012 を駆動できる任意のドライブが含まれる。可搬記録媒体 1012 としては、フレキシブルディスケット、コンパクトディスク（CD）、デジタルビデオディスク（DVD）、などといったものが含まれる。   The medium driving device 1010 includes any drive that can drive any type of portable recording medium 1012. The portable recording medium 1012 includes a flexible diskette, a compact disk (CD), a digital video disk (DVD), and the like.

ネットワーク接続装置 1014 には、ネットワークに接続可能な任意の装置が含まれる。ネットワーク接続装置1014 としては、イーサネット（登録商標）アダプタ、有線LAN（Local Area Network）ルータ、無線LANルータなどが含まれる。   The network connection device 1014 includes any device that can be connected to the network. Examples of the network connection device 1014 include an Ethernet (registered trademark) adapter, a wired LAN (Local Area Network) router, and a wireless LAN router.

実施形態にかかるプログラムは、外部記憶装置 1008 もしくは媒体駆動装置 1010 によって駆動される可搬記録媒体 1012 に格納することができる。また、ハードウェアは、ネットワーク接続装置 914 を介してネットワークと接続することができ、入力装置 1004 および出力装置 1006 を通じて実施形態にかかるプログラムを入出力することが可能である。また、サーバが持つ記憶装置に実施形態にかかるプログラムを保存し、クライアントからそのプログラムを実行させることで、実施形態にかかる効果を得ることも可能である。そうしたサーバ-クライアントネットワークは、例えばP2P（peer to peer）ネットワークであってもかまわない。   The program according to the embodiment can be stored in the portable storage medium 1012 driven by the external storage device 1008 or the medium driving device 1010. The hardware can be connected to the network via the network connection device 914, and the program according to the embodiment can be input / output through the input device 1004 and the output device 1006. In addition, it is possible to obtain the effect according to the embodiment by storing the program according to the embodiment in a storage device of the server and causing the client to execute the program. Such a server-client network may be a peer-to-peer (P2P) network, for example.

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 バス 100 mobile
110 Vision sensor
120 vision sensor
130 optical axis
140 Ranging sensor
200 devices
202 clock unit
210 Storage unit
230 Distance sensor
232 Ranging data storage unit
240 stereo camera
242 Reference image storage unit
244 Reference image storage unit
250 residual calculation unit
260 State estimation data storage / processing unit
270 odometry model
272 maps
300 devices
302 clock unit
303 State estimation system
310 storage unit
330 Distance sensor
332 Ranging data storage unit
340 stereo camera
342 Reference image storage unit
344 Reference image storage unit
350 residual calculation unit
352 mapping unit
354 Image information extraction unit
356 prediction unit
358 calculation units
360 particle filter processing unit
362 Particle Set
372 maps
380 Travel control system
382 Interior sensor
384 Self-state estimation unit
386 Self-state memory unit # 1
388 Self-state memory unit # 2
390 Movement control mechanism
392 Movement mechanism
600 mobile
610 Composite data l ^b _t-1
612 Composite data l ^b _t
700 mobile
710 Composite data (l ^b _t ) '
712 Composite data (l ^r _t ) '
800 mobile
810 Composite data l ^b _t
812 Composite data (l ^b _t ) '
1000 CPU
1002 memory
1004 Input device
1006 Output device
1008 External storage device
1010 Media drive
1012 Portable recording media
1014 Network connection device
1016 Bus

Claims (7)

環境を表す地図を用いて、前記環境内を状態推定をしながら移動する移動体を制御するための装置であって、
(i) 前記地図を格納する、記憶手段と、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定し、前記記憶手段に格納して、格納した前記状態推定データの更新を行う、状態推定データ格納・処理手段と、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむ、測距手段と、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむ、画像取込手段と、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算する、残差算出手段と、
(vi)前記移動体を動かし、動かした移動量を出力する、移動手段と
を含み、
前記状態推定データ格納・処理手段が、前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新し、
前記測距手段が、前記測距データを用いて前記地図を更新し、
前記移動手段が、更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節する
ことを特徴とする、装置。 A device for controlling a moving body that moves while estimating the state in the environment using a map representing the environment,
(i) storage means for storing the map;
(ii) a state estimation data storage / processing unit that sets an initial value of state estimation data for estimating the state of the moving body, stores the initial value in the storage unit, and updates the stored state estimation data; ,
(iii) A plurality of measurement points are set in the environment, and distance measurement data obtained by measuring a positional relationship between the position of the moving body obtained from the state estimation data and each of the plurality of measurement points is obtained for each unit time. A distance measuring means incorporated in the storage means;
(iv) image capturing means for capturing a part of the environment as image data in the storage means at unit time;
(v) creating a plurality of composite data associating the distance measurement data and the image data, and calculating a residual between the composite data;
(vi) moving means for moving the moving body and outputting the moved moving amount;
The state estimation data storage / processing unit updates the state estimation data using the output of the moving unit and the residual calculated by the residual calculation unit,
The ranging means updates the map with the ranging data,
The apparatus is characterized in that the moving means adjusts the movement of the moving body using the updated state estimation data and the map. 前記残差算出手段が、
前記地図に対する測距データを予測して、未来における合成データを得る、予測手段
を含む
ことを特徴とする、請求項１記載の装置。 The residual calculation means is
The apparatus according to claim 1, further comprising a prediction unit that predicts ranging data for the map to obtain composite data in the future. 前記予測手段が、或る時刻における測距データを、前記時刻の単位時間前における前記地図に対して予測することで、予測測距データを得て、
前記予測手段が、前記画像データに前記予測測距データを対応づけることで、前記測距データを得た時刻よりも未来における合成データを得る
ことを特徴とする、請求項２記載の装置。 The prediction means obtains predicted distance measurement data by predicting distance measurement data at a certain time with respect to the map before a unit time of the time,
3. The apparatus according to claim 2, wherein the prediction means associates the predicted distance measurement data with the image data to obtain composite data in the future from the time when the distance measurement data was obtained. 前記画像データが、基準画像データと、前記基準画像データ以外の参照画像データとを含み、
前記残差算出手段が、前記測距データと前記基準画像データを対応づけた合成データ、もしくは、前記測距データと前記参照画像データを対応づけた合成データを作成する
ことを特徴とする、請求項３記載の装置。 The image data includes standard image data and reference image data other than the standard image data,
The residual calculation unit creates composite data in which the distance measurement data and the reference image data are associated with each other, or composite data in which the distance measurement data and the reference image data are associated with each other. Item 3. The apparatus according to Item 3. 前記残差算出手段が、前記合成データを、
時刻t-1に得られた基準画像データに時刻t-1に得られた測距データを対応づけた合成データと、
時刻tに得られた基準画像データに時刻tに得られた測距データを対応づけた合成データと、
時刻t-1に得られた参照画像データに時刻t-1に得られた測距データを対応づけた合成データと、
時刻tに得られた参照画像データに時刻tに得られた測距データを対応づけた合成データと、
時刻tでの測距データを時刻t-1での地図を用いて予測したものを、時刻tに得られた基準画像データに対応づけた合成データと、
時刻tでの測距データを時刻t-1での地図を用いて予測したものを、時刻tに得られた参照画像データに対応づけた合成データと
から成る群から選択して得る
ことを特徴とする、請求項４記載の装置。 The residual calculation means converts the composite data into
Composite data in which the distance measurement data obtained at time t-1 is associated with the reference image data obtained at time t-1,
Composite data in which the distance measurement data obtained at time t is associated with the reference image data obtained at time t,
Composite data in which the distance measurement data obtained at time t-1 is associated with the reference image data obtained at time t-1,
Composite data in which the distance measurement data obtained at time t is associated with the reference image data obtained at time t,
A combination of the distance measurement data at time t predicted using the map at time t-1 and the reference image data obtained at time t,
What is obtained by selecting the ranging data at time t using the map at time t-1 is selected from the group consisting of composite data associated with the reference image data obtained at time t The apparatus of claim 4. 環境を表す地図を用いて、前記環境内を移動する移動体の状態推定と前記地図の更新を行うための方法であって、
(i) 前記地図を記憶手段に格納するステップと、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定して前記記憶手段に格納するステップと、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむステップと、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむステップと、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算するステップと、
(vi)前記移動体を動かし、動かした移動量を出力するステップと、
(vii) 前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新するステップと、
(viii)前記測距データを用いて前記地図を更新するステップと、
(ix)更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節するステップと
を含むことを特徴とする、方法。 A method for estimating a state of a moving object moving in the environment and updating the map using a map representing the environment,
(i) storing the map in a storage means;
(ii) setting an initial value of state estimation data for estimating the state of the moving body and storing the initial value in the storage unit;
(iii) A plurality of measurement points are set in the environment, and distance measurement data obtained by measuring a positional relationship between the position of the moving body obtained from the state estimation data and each of the plurality of measurement points is obtained for each unit time. The step of incorporating into the storage means;
(iv) capturing a part of the environment as image data in the storage means every unit time;
(v) creating a plurality of composite data associating the distance measurement data with the image data, and calculating a residual between the composite data;
(vi) moving the moving body and outputting the moved amount;
(vii) updating the state estimation data using the output of the moving means and the residual calculated by the residual calculating means;
(viii) updating the map using the ranging data;
(ix) adjusting the movement of the moving body using the updated state estimation data and the map. 環境を表す地図を用いて、前記環境内を移動する移動体の状態推定と前記地図の更新を行うための方法を、コンピュータに実行させるためのプログラムであって、
前記方法が、
(i) 前記地図を記憶手段に格納するステップと、
(ii)前記移動体の状態を推定するための状態推定データの初期値を設定して前記記憶手段に格納するステップと、
(iii) 複数の測定点を前記環境内に設定し、前記状態推定データから得た前記移動体の位置と前記複数の測定点の各々との位置関係を測定した測距データを、単位時間ごとに前記記憶手段に取りこむステップと、
(iv)前記環境の一部を画像データとして前記記憶手段に単位時間ごとに取りこむステップと、
(v) 前記測距データと前記画像データを対応づけた合成データを複数作成し、前記合成データ同士の残差を計算するステップと、
(vi)前記移動体を動かし、動かした移動量を出力するステップと、
(vii) 前記移動手段の出力と、前記残差算出手段の計算した前記残差とを用いて、前記状態推定データを更新するステップと、
(viii)前記測距データを用いて前記地図を更新するステップと、
(ix)更新された前記状態推定データと前記地図とを用いて、前記移動体の移動を調節するステップと
を含む
ことを特徴とする、プログラム。 A program for causing a computer to execute a method for estimating a state of a moving object moving in the environment and updating the map using a map representing the environment,
The method comprises
(i) storing the map in a storage means;
(ii) setting an initial value of state estimation data for estimating the state of the moving body and storing the initial value in the storage unit;
(iii) A plurality of measurement points are set in the environment, and distance measurement data obtained by measuring a positional relationship between the position of the moving body obtained from the state estimation data and each of the plurality of measurement points is obtained for each unit time. The step of incorporating into the storage means;
(iv) capturing a part of the environment as image data in the storage means every unit time;
(v) creating a plurality of composite data associating the distance measurement data with the image data, and calculating a residual between the composite data;
(vi) moving the moving body and outputting the moved amount;
(vii) updating the state estimation data using the output of the moving means and the residual calculated by the residual calculating means;
(viii) updating the map using the ranging data;
(ix) A step of adjusting the movement of the moving body using the updated state estimation data and the map.