JP2008065562A - Information processing device, localization method and computer program - Google Patents
Information processing device, localization method and computer program Download PDFInfo
- Publication number
- JP2008065562A JP2008065562A JP2006242095A JP2006242095A JP2008065562A JP 2008065562 A JP2008065562 A JP 2008065562A JP 2006242095 A JP2006242095 A JP 2006242095A JP 2006242095 A JP2006242095 A JP 2006242095A JP 2008065562 A JP2008065562 A JP 2008065562A
- Authority
- JP
- Japan
- Prior art keywords
- sensor
- information
- sensor patch
- identification
- processing
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000004807 localization Effects 0.000 title claims abstract description 127
- 238000000034 method Methods 0.000 title claims description 418
- 230000010365 information processing Effects 0.000 title claims description 66
- 238000004590 computer program Methods 0.000 title claims description 11
- 238000012545 processing Methods 0.000 claims abstract description 468
- 230000008569 process Effects 0.000 claims description 385
- 230000033001 locomotion Effects 0.000 claims description 120
- 238000003672 processing method Methods 0.000 claims description 41
- 239000011159 matrix material Substances 0.000 claims description 36
- 230000008859 change Effects 0.000 claims description 32
- 238000003860 storage Methods 0.000 claims description 24
- 238000004364 calculation method Methods 0.000 claims description 23
- 238000009499 grossing Methods 0.000 claims description 17
- 238000000605 extraction Methods 0.000 claims description 7
- 238000009825 accumulation Methods 0.000 claims description 6
- 230000004044 response Effects 0.000 claims description 6
- 230000036544 posture Effects 0.000 description 113
- 238000004458 analytical method Methods 0.000 description 19
- 230000006870 function Effects 0.000 description 13
- 238000009826 distribution Methods 0.000 description 9
- 238000011156 evaluation Methods 0.000 description 8
- CIWBSHSKHKDKBQ-JLAZNSOCSA-N Ascorbic acid Chemical compound OC[C@H](O)[C@H]1OC(=O)C(O)=C1O CIWBSHSKHKDKBQ-JLAZNSOCSA-N 0.000 description 7
- 101150069538 ROT1 gene Proteins 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 230000007613 environmental effect Effects 0.000 description 6
- 238000005259 measurement Methods 0.000 description 6
- 238000012790 confirmation Methods 0.000 description 5
- 239000002245 particle Substances 0.000 description 4
- 238000007796 conventional method Methods 0.000 description 3
- 238000000342 Monte Carlo simulation Methods 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- FFBHFFJDDLITSX-UHFFFAOYSA-N benzyl N-[2-hydroxy-4-(3-oxomorpholin-4-yl)phenyl]carbamate Chemical compound OC1=C(NC(=O)OCC2=CC=CC=C2)C=CC(=C1)N1CCOCC1=O FFBHFFJDDLITSX-UHFFFAOYSA-N 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000007429 general method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Abstract
Description
本発明は、情報処理装置、および同定処理方法、並びにコンピュータ・プログラムに関する。さらに詳細には、例えばロボットなど移動物体において移動環境を観測し、観測状況に応じた制御を実行する情報処理装置、および同定処理方法、並びにコンピュータ・プログラムに関する。 The present invention relates to an information processing apparatus, an identification processing method, and a computer program. More specifically, the present invention relates to an information processing apparatus, an identification processing method, and a computer program that observe a moving environment on a moving object such as a robot and execute control according to the observation state.
例えば、自律型ロボットのような自律制御デバイスは、周囲の外部状態やデバイス自身の内部状態に応じて自律的に動作を行うデバイスである。例えばカメラなどのセンサによって取得した環境情報に基づいて、デバイスの位置確認や方向確認などに基づく姿勢（ポーズ）制御を行う。 For example, an autonomous control device such as an autonomous robot is a device that autonomously operates in accordance with a surrounding external state or the internal state of the device itself. For example, based on environmental information acquired by a sensor such as a camera, posture control based on device position confirmation and direction confirmation is performed.
例えば、自律型ロボットが歩行動作を行なう場合、予め取得された地図データを適用した処理や、外部の障害物等の検出などの環境情報を取得して障害物を回避するような行動経路を計画する手法が行なわれる。逐次取得する環境情報の分析に基づいて経路を決定するロボットは、例えば予め指定された地図データに対して、自らが取得したカメラ撮影画像などのセンサ情報に基づく更新を実行して、自己の位置を確認して最適な経路決定を行なう。 For example, when an autonomous robot performs a walking motion, it plans an action route that avoids obstacles by acquiring environmental information such as processing using map data acquired in advance or detecting external obstacles. A technique is performed. The robot that determines the route based on the analysis of the environmental information acquired sequentially performs, for example, update based on sensor information such as a camera captured image acquired by itself on map data specified in advance. To determine the optimum route.
例えばロボットなどの自律制御デバイスにおいて実行される位置や方向などの姿勢（Ｐｏｓｅ）追跡処理は、ローカリゼーション（Ｌｏｃａｌｉｚａｔｉｏｎ）と呼ばれる。 For example, a position tracking process such as a position and a direction executed in an autonomous control device such as a robot is referred to as localization.
デバイスの位置や方向などの姿勢の同定を行う代表的な手法の１つとして「ランドマーク（Ｌａｎｄｍａｒｋ）」を利用した手法がある。ランドマークは、例えばロボットの置かれた環境に設定された目印である。ロボットは、例えばカメラなどのセンサによって視覚認識されたランドマークからの相対位置情報を基に自己位置を地理的に探索することができる。 One of the typical techniques for identifying the posture such as the position and direction of a device is a technique using “Landmark”. The landmark is a mark set in an environment where the robot is placed, for example. The robot can geographically search for its own position based on relative position information from a landmark visually recognized by a sensor such as a camera.
人工的なランドマークを含む環境における探索若しくは自己同定の方式として、例えば、マルコフ・ローカリゼーションや拡張カルマン・フィルタを適用したものが提案されている。 As a search or self-identification method in an environment including an artificial landmark, for example, a method using Markov localization or an extended Kalman filter has been proposed.
マルコフ・ローカリゼーションを用いた探索装置は、環境内の自分の位置を離散的なグリッド上の自己位置確率密度分布として保持し、環境内に設置されているランドマークを観測すると、ランドマークからの相対位置に基づいて自己位置を推定して、自己位置確率密度分布を更新する。また、自身の動作情報を観測すると、その動作情報に基づいて、自己位置確率密度を更新する。そして、各時刻において、自己位置確率密度分布の最も高い値を持つグリッドが、自己位置の推定結果となる。 A search device using Markov localization maintains its position in the environment as a self-position probability density distribution on a discrete grid and observes the landmarks installed in the environment. The self position is estimated based on the position, and the self position probability density distribution is updated. Further, when own motion information is observed, the self-position probability density is updated based on the motion information. At each time, the grid having the highest value of the self-position probability density distribution becomes the self-position estimation result.
他方、拡張カルマン・フィルタを用いた探索装置は、自己位置を状態変数［ｘ，ｙ，θ］の実測値として保持し、環境内に設置されたランドマークを観測すると、ランドマークからの相対位置に基づいて自己位置を推定する。また、自身の動作情報を観測すると、その動作情報に基づいて、状態量の推定を行う。 On the other hand, the search device using the extended Kalman filter holds the self-position as an actual measurement value of the state variables [x, y, θ], and observes a landmark installed in the environment, and thereby finds a relative position from the landmark. Based on the above, the self-position is estimated. Further, when own motion information is observed, the state quantity is estimated based on the motion information.
なお、人工的なランドマークを含む環境における探索若しくは自己同定方式を開示した従来技術として、例えば特許文献１がある。 For example, Patent Document 1 discloses a conventional technique that discloses a search or self-identification method in an environment including an artificial landmark.
特許文献１の他、デバイスの一や方向を同定する処理（ローカリゼーション）技術に関する構成を記述した従来技術としては、例えば以下の従来技術がある。
特許文献２は、ロボットの位置を中心位置として設定した複数のグリッドに仕切られた地図をロボットが保持し、ロボットが移動した場合、その移動量に基づいて、地図情報を更新する位置認識処理構成を開示している。
In addition to Patent Document 1, as a conventional technique describing a configuration relating to a processing (localization) technique for identifying one or the direction of a device, for example, there are the following conventional techniques.
Patent Document 2 discloses a position recognition processing configuration in which a robot holds a map partitioned by a plurality of grids set with the position of the robot as a central position, and when the robot moves, the map information is updated based on the amount of movement. Is disclosed.
特許文献３は、３次元の距離データを取得するステレオビジョンシステムを適用して、観測ノイズに対してロバストで高さ方向における高い解像度の環境地図を作成する構成を開示している。また、特許文献４は、カメラなどのセンサによって複数の異なる位置の画像データを取得し、取得した複数画像の組み合わせ情報に基づいて３次元位置を特定する構成を開示している。 Patent Document 3 discloses a configuration in which a stereo vision system that acquires three-dimensional distance data is applied to create an environment map that is robust against observation noise and has a high resolution in the height direction. Patent Document 4 discloses a configuration in which image data at a plurality of different positions is acquired by a sensor such as a camera, and a three-dimensional position is specified based on combination information of the acquired plurality of images.
しかしながら、上述した従来技術における同定処理（ローカリゼーション）においては、いずれも、例えばカメラによって撮影される画像データの解析に基づく自己の位置の確認を行なう場合、連続するフレーム画像データに含まれる大量のデータについての解析を行なうことを前提としており、処理時間および処理負荷が大きくなるという問題を有する。
本発明は、上述の問題点に鑑みてなされたものであり、例えば、自律走行ロボットなどの自律制御デバイスなどにおいて行なわれるデバイスの位置、方向を推定する同定処理（ローカリゼーション）を効率的にかつ高精度に実行することを可能とした情報処理装置、および同定処理方法、並びにコンピュータ・プログラムを提供することを目的とする。 The present invention has been made in view of the above-described problems. For example, the identification processing (localization) for estimating the position and direction of a device performed in an autonomous control device such as an autonomous traveling robot is efficiently and highly performed. An object of the present invention is to provide an information processing apparatus, an identification processing method, and a computer program that can be executed with high accuracy.
本発明の第１の側面は、
自装置の位置および方向を推定する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する情報処理装置であり、
自装置の周囲の情報としてのセンサ情報を取得するセンサと、
自装置の動き情報と。前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合しセンサパッチを生成するセンサパッチ生成部と、
前記センサパッチと、該センサパッチ生成期間における自装置の動き情報を入力し、該入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行する同定処理部と、
を有することを特徴とする情報処理装置にある。
The first aspect of the present invention is:
It is an information processing device that executes identification processing (Localization) for estimating the position and direction of the device itself,
A sensor that acquires sensor information as information around the device itself;
With movement information of own device. A sensor patch generation unit that integrates sensor information obtained in a certain time / space by integrating sensor information acquired by the sensor and generates a sensor patch;
An identification processing unit that inputs the sensor patch and movement information of the own apparatus in the sensor patch generation period, and executes an identification process for estimating the position and direction of the own apparatus based on the input information;
There is an information processing apparatus characterized by having.
さらに、本発明の情報処理装置の一実施態様において、前記センサパッチ生成部は、前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理と、新たに入力される動き情報に基づくデバイス姿勢を推定する処理を実行するとともに、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行する構成であることを特徴とする。 Furthermore, in an embodiment of the information processing apparatus of the present invention, the sensor patch generation unit includes a sensor patch update process based on new sensor information acquired by the sensor and a device based on newly input motion information. The configuration is characterized in that the process for estimating the posture is executed and the generation end determination process is executed in accordance with a preset determination criterion for the sensor patch being generated.
さらに、本発明の情報処理装置の一実施態様において、前記センサパッチ生成部は、前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理を実行する構成であり、該センサパッチ更新処理として、センサ情報の集積処理、ランドマークの抽出処理、およびグリッドマップの生成処理を実行する構成であることを特徴とする。 Furthermore, in an embodiment of the information processing apparatus of the present invention, the sensor patch generation unit is configured to execute a sensor patch update process based on new sensor information acquired by the sensor, and the sensor patch update process As a feature, the sensor information accumulation process, the landmark extraction process, and the grid map generation process are executed.
さらに、本発明の情報処理装置の一実施態様において、前記センサパッチ生成部は、新たに入力される動き情報に基づくデバイス姿勢を推定するデバイス姿勢推定処理を実行する構成であり、該デバイス姿勢推定処理を、
（ａ）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
（ｂ）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
上記（ａ）または（ｂ）いずれかの処理を適用して実行する構成であることを特徴とする。
Furthermore, in one embodiment of the information processing apparatus according to the present invention, the sensor patch generation unit is configured to execute device posture estimation processing for estimating a device posture based on newly input motion information, and the device posture estimation Processing
(A) Mean propagation method (mean propagation)
(B) Gaussian model
The present invention is characterized in that the processing is executed by applying either of the processes (a) or (b).
さらに、本発明の情報処理装置の一実施態様において、前記センサパッチ生成部は、前記センサによって取得されるセンサ情報を集積したセンサパッチと、該センサパッチ生成期間における自装置の動き情報とに基づいて、環境地図の生成情報として適用可能なグリッドマップの座標更新および自装置の姿勢更新処理を実行する構成であることを特徴とする。 Furthermore, in one embodiment of the information processing apparatus of the present invention, the sensor patch generation unit is based on a sensor patch in which sensor information acquired by the sensor is integrated, and movement information of the own apparatus in the sensor patch generation period. Thus, the configuration is such that the coordinate update of the grid map applicable as the generation information of the environment map and the posture update process of the own apparatus are executed.
さらに、本発明の情報処理装置の一実施態様において、前記センサパッチ生成部は、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行する構成であり、前記判定基準として、
（ａ）予め設定された時間の経過によるタイムアウト、
（ｂ）予め設定された閾値距離に自装置の移動距離が到達、
（ｃ）自装置の先行姿勢［Ｕ］に対応する共分散行列共分散行列Σｕのいずれかの要素（Ｄｅｔｅｒｍｉｎａｎｔ）の値が予め定めた閾値を超えた、
（ｄ）自装置の先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの対角要素（ｄｉａｇｏｎａｌ ｅｌｅｍｅｎｔｓ）の値が予め定めた閾値を超えた、
（ｅ）生成地図におけるセル中、占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］設定のなされたセル数が予め定めた閾値を越えた、
（ｆ）前記同定処理部からのセンサパッチ出力要求の受領、
上記（ａ）〜（ｆ）のいずれかの１つの基準、またはいずれかの基準の組み合わせを適用したセンサパッチの生成終了判定処理を実行する構成であることを特徴とする。
Furthermore, in an embodiment of the information processing apparatus of the present invention, the sensor patch generation unit is configured to execute a generation end determination process according to a predetermined determination criterion for the sensor patch being generated, and the determination criterion ,
(A) timeout due to elapse of a preset time;
(B) The movement distance of the device reaches a preset threshold distance;
(C) The value of any element (Determinant) of the covariance matrix covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold value,
(D) the value of any diagonal elements of the covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold;
(E) Among the cells in the generated map, the number of cells for which the occupation [Occupied] setting is made exceeds a predetermined threshold value.
(F) receiving a sensor patch output request from the identification processing unit;
The present invention is characterized in that the sensor patch generation end determination process is executed by applying any one of the above criteria (a) to (f) or a combination of any criteria.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、前記同定処理の成功判定処理を実行する構成であり、同定処理の成功の判定に基づいて、前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力する構成であることを特徴とする。 Furthermore, in one embodiment of the information processing apparatus of the present invention, the identification processing unit is configured to execute a success determination process of the identification process, and based on the determination of the success of the identification process, The sensor patch reset processing request is output.
さらに、本発明の情報処理装置の一実施態様において、前記センサパッチ生成部は、前記同定処理部からのリセット要求の入力に基づいて、新規センサパッチの生成処理を開始する構成であることを特徴とする。 Furthermore, in an embodiment of the information processing apparatus of the present invention, the sensor patch generation unit is configured to start generation processing of a new sensor patch based on an input of a reset request from the identification processing unit. And
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、前記同定処理の成功判定処理として、予め保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報との一致度を検証する処理を実行する構成であることを特徴とする。 Furthermore, in one embodiment of the information processing apparatus of the present invention, the identification processing unit, as the success determination process of the identification process, stores previously stored map data or already generated sensor patch information, and newly generated sensor patch information. It is the structure which performs the process which verifies the degree of coincidence with.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力することなく、前記センサパッチ生成部から、各センサパッチに重複するセンサ情報を含むセンサパッチを入力して、自装置の位置および方向を推定する同定処理を実行する構成であることを特徴とする。 Furthermore, in one embodiment of the information processing apparatus of the present invention, the identification processing unit is duplicated to each sensor patch from the sensor patch generation unit without outputting a sensor patch reset processing request to the sensor patch generation unit. The sensor patch including the sensor information to be input is input, and the identification process for estimating the position and direction of the device itself is executed.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、各センサパッチに重複するセンサ情報を含むセンサパッチを適用した同定処理の実行に際して、重複情報の過大評価を避けるための平滑化処理を実行する構成であることを特徴とする。 Furthermore, in an embodiment of the information processing apparatus of the present invention, the identification processing unit is configured to avoid overestimation of duplicate information when executing an identification process in which a sensor patch including sensor information that overlaps each sensor patch is applied. The configuration is characterized in that smoothing processing is executed.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）に従った処理を基本とした同定処理を実行する構成であり、拡張カルマン・フィルタ（ＥＫＦ）処理において、センサパッチに対応して設定される共分散行列Σｓを、
Σｓ←ｇ（Σｓ）
ただし、ｇ（）は、共分散行列Σｓを増加させる関数、
とする処理を実行して同定処理を行なう構成であることを特徴とする。
Furthermore, in an embodiment of the information processing apparatus of the present invention, the identification processing unit is configured to execute an identification process based on a process according to an extended Kalman filter (EKF). In the filter (EKF) process, the covariance matrix Σs set corresponding to the sensor patch is
Σs ← g (Σs)
Where g () is a function that increases the covariance matrix Σs,
The identification processing is performed by executing the processing as described above.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）に従った処理を基本とした同定処理を実行する構成であり、モンテカルロ同定処理（ＭＣＬ）において、センサパッチに基づく確率密度の算出式ｐ（ｚｔ｜ｘｔ）、
ただし、ｚｔは、時間ｔにおけるセンサ情報、ｘｔは時間ｔにおける装置姿勢、
上記式を、
ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））
ただし、ｇ（ｐ）は、ｐを増加させる関数、
とする処理を実行して同定処理を行なう構成であることを特徴とする。
Furthermore, in one embodiment of the information processing apparatus of the present invention, the identification processing unit is configured to execute an identification process based on a process according to Monte Carlo Localization (MCL), and the Monte Carlo identification process (MCL), a probability density calculation formula p (z t | x t ) based on the sensor patch,
Where zt is sensor information at time t, xt is the device attitude at time t,
The above formula
p (z t | x t ) ← g (p (z t | x t ))
Where g (p) is a function that increases p,
The identification processing is performed by executing the processing as described above.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、前記センサパッチ生成部から入力するセンサパッチに対応して実行された同定処理が成功した場合、処理済みのセンサパッチを、処理済みセンサパッチとして記憶部に格納し、新たにセンサパッチ生成部から入力する新規入力センサパッチと、前記処理済みセンサパッチとの比較に基づいて、変化情報を抽出し、抽出した変化情報に基づく同定処理を実行する構成であることを特徴とする。 Furthermore, in an embodiment of the information processing apparatus of the present invention, the identification processing unit, when the identification process executed corresponding to the sensor patch input from the sensor patch generation unit is successful, displays the processed sensor patch. The change information is stored in the storage unit as the processed sensor patch, and the change information is extracted based on the comparison between the new sensor patch newly input from the sensor patch generation unit and the processed sensor patch. It is the structure which performs the identification process based on.
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、
（Ａ）２つのセンサパッチの差分情報による処理、
（Ｂ）２つのセンサパッチの更新率情報による処理、
上記（Ａ）または（Ｂ）のいずれかの処理を実行する構成であることを特徴とする。
Furthermore, in one embodiment of the information processing apparatus of the present invention, the identification processing unit performs identification processing based on change information between the new input sensor patch and the processed sensor patch.
(A) Processing based on difference information between two sensor patches;
(B) Processing based on update rate information of two sensor patches,
It is the structure which performs the process in any one of said (A) or (B).
さらに、本発明の情報処理装置の一実施態様において、前記同定処理部は、前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、モンテカルロ同定処理（ＭＣＬ）処理を適用して、更新率情報に基づく同定処理を実行する構成であることを特徴とする。 Furthermore, in an embodiment of the information processing apparatus of the present invention, the identification processing unit performs a Monte Carlo identification process (MCL) process as an identification process based on change information between the new input sensor patch and the processed sensor patch. The present invention is characterized in that it is configured to execute identification processing based on update rate information.
さらに、本発明の第２の側面は、
情報処理装置において、自装置の位置および方向を推定する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する同定処理方法であり、
センサにおいて、自装置の周囲の情報としてのセンサ情報を取得するセンサ情報取得ステップと、
センサパッチ生成部において、自装置の動き情報と、前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチを生成するセンサパッチ生成ステップと、
同定処理部において、前記センサパッチと、該センサパッチ生成期間における自装置の動き情報を入力し、該入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行する同定処理ステップと、
を有することを特徴とする同定処理方法にある。
Furthermore, the second aspect of the present invention provides
In the information processing device, an identification processing method for executing identification processing (Localization) for estimating the position and direction of the device itself,
In the sensor, a sensor information acquisition step for acquiring sensor information as information around the device itself;
In the sensor patch generation unit, a sensor patch generation step of generating a sensor patch in which sensor information obtained in a certain time / space is integrated by integrating movement information of the device itself and sensor information acquired by the sensor; ,
In the identification processing unit, an identification processing step of inputting the sensor patch and movement information of the own apparatus during the sensor patch generation period and executing an identification process for estimating the position and direction of the own apparatus based on the input information; ,
It is in the identification processing method characterized by having.
さらに、本発明の同定処理方法の一実施態様において、前記センサパッチ生成ステップは、前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理と、新たに入力される動き情報に基づくデバイス姿勢を推定する処理を実行するとともに、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行するステップであることを特徴とする。 Furthermore, in one embodiment of the identification processing method of the present invention, the sensor patch generation step includes a sensor patch update process based on new sensor information acquired by the sensor, and a device based on newly input motion information. It is a step of executing a process for estimating a posture and executing a generation end determination process according to a predetermined determination criterion for a sensor patch being generated.
さらに、本発明の同定処理方法の一実施態様において、前記センサパッチ生成ステップは、前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理を実行するステップを含み、該センサパッチ更新処理として、センサ情報の集積処理、ランドマークの抽出処理、およびグリッドマップの生成処理を実行することを特徴とする。 Furthermore, in one embodiment of the identification processing method of the present invention, the sensor patch generation step includes a step of executing a sensor patch update process based on new sensor information acquired by the sensor, the sensor patch update process And a sensor information accumulation process, a landmark extraction process, and a grid map generation process.
さらに、本発明の同定処理方法の一実施態様において、前記センサパッチ生成ステップは、新たに入力される動き情報に基づくデバイス姿勢を推定するデバイス姿勢推定処理を実行するステップを含み、該デバイス姿勢推定処理を、
（ａ）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
（ｂ）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
上記（ａ）または（ｂ）いずれかの処理を適用して実行することを特徴とする。
Furthermore, in one embodiment of the identification processing method of the present invention, the sensor patch generation step includes a step of executing a device posture estimation process for estimating a device posture based on newly input motion information, the device posture estimation Processing
(A) Mean propagation method (mean propagation)
(B) Gaussian model
The processing is performed by applying either of the processes (a) or (b).
さらに、本発明の同定処理方法の一実施態様において、前記センサパッチ生成ステップは、前記センサによって取得されるセンサ情報を集積したセンサパッチと、該センサパッチ生成期間における自装置の動き情報とに基づいて、環境地図の生成情報として適用可能なグリッドマップの座標更新および自装置の姿勢更新処理を実行するステップであることを特徴とする。 Furthermore, in an embodiment of the identification processing method of the present invention, the sensor patch generation step is based on a sensor patch in which sensor information acquired by the sensor is integrated, and movement information of the device itself during the sensor patch generation period. Thus, it is a step of executing coordinate update of the grid map applicable as generation information of the environment map and posture update processing of the own apparatus.
さらに、本発明の同定処理方法の一実施態様において、前記センサパッチ生成ステップは、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行するステップであり、前記判定基準として、
（ａ）予め設定された時間の経過によるタイムアウト、
（ｂ）予め設定された閾値距離に自装置の移動距離が到達、
（ｃ）自装置の先行姿勢［Ｕ］に対応する共分散行列共分散行列Σｕのいずれかの要素（Ｄｅｔｅｒｍｉｎａｎｔ）の値が予め定めた閾値を超えた、
（ｄ）自装置の先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの対角要素（ｄｉａｇｏｎａｌ ｅｌｅｍｅｎｔｓ）の値が予め定めた閾値を超えた、
（ｅ）生成地図におけるセル中、占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］設定のなされたセル数が予め定めた閾値を越えた、
（ｆ）前記同定処理部からのセンサパッチ出力要求の受領、
上記（ａ）〜（ｆ）のいずれかの１つの基準、またはいずれかの基準の組み合わせを適用したセンサパッチの生成終了判定処理を実行することを特徴とする。
Furthermore, in an embodiment of the identification processing method of the present invention, the sensor patch generation step is a step of executing generation end determination processing according to a predetermined determination criterion for the sensor patch being generated, ,
(A) timeout due to elapse of a preset time;
(B) The movement distance of the device reaches a preset threshold distance;
(C) The value of any element (Determinant) of the covariance matrix covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold value,
(D) the value of any diagonal elements of the covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold;
(E) Among the cells in the generated map, the number of cells for which the occupation [Occupied] setting is made exceeds a predetermined threshold value.
(F) receiving a sensor patch output request from the identification processing unit;
A sensor patch generation end determination process to which any one of the above-mentioned criteria (a) to (f) or a combination of any criteria is applied is executed.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、前記同定処理の成功判定処理を実行するステップを含み、同定処理の成功の判定に基づいて、前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力することを特徴とする。 Further, in an embodiment of the identification processing method of the present invention, the identification processing step includes a step of executing a success determination process of the identification process, and based on the determination of the success of the identification process, A sensor patch reset processing request is output.
さらに、本発明の同定処理方法の一実施態様において、前記センサパッチ生成部は、前記同定処理部からのリセット要求の入力に基づいて、新規センサパッチの生成処理を開始することを特徴とする。 Furthermore, in an embodiment of the identification processing method of the present invention, the sensor patch generation unit starts a new sensor patch generation process based on a reset request input from the identification processing unit.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、前記同定処理の成功判定処理として、予め保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報との一致度を検証する処理を実行することを特徴とする。 Furthermore, in an embodiment of the identification processing method of the present invention, the identification processing step includes map data to be stored in advance or generated sensor patch information and newly generated sensor patch information as the success determination processing of the identification processing. A process for verifying the degree of coincidence with is performed.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力することなく、前記センサパッチ生成部から、各センサパッチに重複するセンサ情報を含むセンサパッチを入力して、自装置の位置および方向を推定する同定処理を実行することを特徴とする。 Furthermore, in one embodiment of the identification processing method of the present invention, the identification processing step is performed by the sensor patch generation unit so as to overlap each sensor patch without outputting a sensor patch reset processing request to the sensor patch generation unit. An identification process for estimating the position and direction of the device itself is executed by inputting a sensor patch including sensor information to be performed.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、各センサパッチに重複するセンサ情報を含むセンサパッチを適用した同定処理の実行に際して、重複情報の過大評価を避けるための平滑化処理を実行することを特徴とする。 Furthermore, in one embodiment of the identification processing method of the present invention, the identification processing step is for avoiding overestimation of duplicate information when executing an identification process in which a sensor patch including overlapping sensor information is applied to each sensor patch. A smoothing process is performed.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）に従った処理を基本とした同定処理を実行し、拡張カルマン・フィルタ（ＥＫＦ）処理において、センサパッチに対応して設定される共分散行列Σｓを、
Σｓ←ｇ（Σｓ）
ただし、ｇ（）は、共分散行列Σｓを増加させる関数、
とする処理を実行して同定処理を行なうことを特徴とする。
Furthermore, in an embodiment of the identification processing method of the present invention, the identification processing step executes an identification process based on a process according to an extended Kalman filter (EKF), and an extended Kalman filter ( In the (EKF) processing, the covariance matrix Σs set corresponding to the sensor patch is
Σs ← g (Σs)
Where g () is a function that increases the covariance matrix Σs,
The identification process is performed by executing the following process.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）に従った処理を基本とした同定処理を実行し、モンテカルロ同定処理（ＭＣＬ）において、センサパッチに基づく確率密度の算出式ｐ（ｚｔ｜ｘｔ）、
ただし、ｚｔは、時間ｔにおけるセンサ情報、ｘｔは時間ｔにおける装置姿勢、
上記式を、
ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））
ただし、ｇ（ｐ）は、ｐを増加させる関数、
とする処理を実行して同定処理を行なうことを特徴とする。
Furthermore, in an embodiment of the identification processing method of the present invention, the identification processing step executes an identification process based on a process according to Monte Carlo Localization (MCL), and a Monte Carlo identification process (MCL). A probability density calculation formula p (z t | x t ) based on the sensor patch,
Where zt is sensor information at time t, xt is the device attitude at time t,
The above formula
p (z t | x t ) ← g (p (z t | x t ))
Where g (p) is a function that increases p,
The identification process is performed by executing the following process.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、前記センサパッチ生成部から入力するセンサパッチに対応して実行された同定処理が成功した場合、処理済みのセンサパッチを、処理済みセンサパッチとして記憶部に格納し、新たにセンサパッチ生成部から入力する新規入力センサパッチと、前記処理済みセンサパッチとの比較に基づいて、変化情報を抽出し、抽出した変化情報に基づく同定処理を実行することを特徴とする。 Furthermore, in one embodiment of the identification processing method of the present invention, the identification processing step includes processing the processed sensor patch when the identification processing executed corresponding to the sensor patch input from the sensor patch generation unit is successful. The change information is stored in the storage unit as the processed sensor patch, and the change information is extracted based on the comparison between the new sensor patch newly input from the sensor patch generation unit and the processed sensor patch. The identification processing based on this is performed.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、
（Ａ）２つのセンサパッチの差分情報による処理、
（Ｂ）２つのセンサパッチの更新率情報による処理、
上記（Ａ）または（Ｂ）のいずれかの処理を実行することを特徴とする。
Furthermore, in one embodiment of the identification processing method of the present invention, the identification processing step includes, as identification processing based on change information between the new input sensor patch and the processed sensor patch,
(A) Processing based on difference information between two sensor patches;
(B) Processing based on update rate information of two sensor patches,
One of the processes (A) and (B) is performed.
さらに、本発明の同定処理方法の一実施態様において、前記同定処理ステップは、前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、モンテカルロ同定処理（ＭＣＬ）処理を適用して、更新率情報に基づく同定処理を実行するステップであることを特徴とする。 Furthermore, in an embodiment of the identification processing method of the present invention, the identification processing step includes a Monte Carlo identification processing (MCL) process as an identification process based on change information between the new input sensor patch and the processed sensor patch. It is a step of applying and executing identification processing based on update rate information.
さらに、本発明の第３の側面は、
情報処理装置において、自装置の位置および方向を推定する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行させるコンピュータ・プログラムであり、
センサにおいて、自装置の周囲の情報としてのセンサ情報を取得させるセンサ情報取得ステップと、
センサパッチ生成部において、自装置の動き情報と、前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチを生成するセンサパッチ生成ステップと、
同定処理部において、前記センサパッチと、該センサパッチ生成期間における自装置の動き情報を入力し、該入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行させる同定処理ステップと、
を実行させることを特徴とするコンピュータ・プログラムにある。
Furthermore, the third aspect of the present invention provides
In the information processing apparatus, a computer program for executing identification processing (Localization) for estimating the position and direction of the own apparatus,
In the sensor, a sensor information acquisition step for acquiring sensor information as information around the device itself;
In the sensor patch generation unit, a sensor patch generation step of generating a sensor patch in which sensor information obtained in a certain time / space is integrated by integrating movement information of the device itself and sensor information acquired by the sensor; ,
In the identification processing unit, an identification processing step of inputting the sensor patch and movement information of the own apparatus during the sensor patch generation period, and executing an identification process for estimating the position and direction of the own apparatus based on the input information; ,
In a computer program characterized by causing
なお、本発明のコンピュータ・プログラムは、例えば、様々なプログラム・コードを実行可能なコンピュータシステムに対して、コンピュータ可読な形式で提供する記憶媒体、通信媒体、例えば、ＣＤやＦＤ、ＭＯなどの記録媒体、あるいは、ネットワークなどの通信媒体によって提供可能なコンピュータ・プログラムである。このようなプログラムをコンピュータ可読な形式で提供することにより、コンピュータシステム上でプログラムに応じた処理が実現される。 Note that the computer program of the present invention is a recording medium provided in a computer-readable format for a computer system capable of executing various program codes, for example, a recording medium such as a CD, FD, or MO. A computer program that can be provided by a medium or a communication medium such as a network. By providing such a program in a computer-readable format, processing corresponding to the program is realized on the computer system.
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施例や添付する図面に基づくより詳細な説明によって明らかになるであろう。なお、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。 Other objects, features, and advantages of the present invention will become apparent from a more detailed description based on embodiments of the present invention described later and the accompanying drawings. In this specification, the system is a logical set configuration of a plurality of devices, and is not limited to one in which the devices of each configuration are in the same casing.
本発明の一実施例の構成では、センサによって取得される個別のセンサ情報を集積した情報、例えば個別のセンサ情報をＺｔとした場合、センサデータ集合｛Ｚ０∪Ｚ１∪Ｚ２｝などの集積センサ情報に基づくセンサパッチを生成し、このセンサパッチに基づくデバイスの位置および方向推定処理としての同定処理を実行する。本構成によれば、個別のセンサ情報のみからは得られない情報の適用が可能となり、ノイズを削減してＳＮＲの高いデータに基づく同定処理を行なうことが可能となる。また、処理タイミングも入力フレームのスピード、例えば３０フレーム／ｓｅｃごとに１つのフレーム画像の処理を行う必要がなくなり、処理負荷の低減が可能となる。 In the configuration of an embodiment of the present invention, information obtained by accumulating individual sensor information acquired by a sensor, for example, when individual sensor information is Zt, a sensor data set {Z 0 ∪Z 1 ∪Z 2 } A sensor patch based on the integrated sensor information is generated, and an identification process is performed as a device position and direction estimation process based on the sensor patch. According to this configuration, it is possible to apply information that cannot be obtained only from individual sensor information, and it is possible to reduce noise and perform identification processing based on data having a high SNR. Further, it is not necessary to process one frame image every input frame speed, for example, every 30 frames / sec, and the processing load can be reduced.
以下、図面を参照しながら本発明の実施形態に係るデバイス同定方法、および情報処理装置、並びにコンピュータ・プログラムの詳細について説明する。 Hereinafter, a device identification method, an information processing apparatus, and a computer program according to an embodiment of the present invention will be described in detail with reference to the drawings.
まず、本発明の装置において実行するデバイス同定処理の一実施例の概要について、図１、図２を参照して説明する。 First, an outline of an embodiment of device identification processing executed in the apparatus of the present invention will be described with reference to FIGS.
図１には、例えば、自律型ロボットのような自律制御を行なうデバイス１００の時間ｔ＝０，１，２の各時間におけるセンサ取得情報の遷移を示す図である。デバイス１００は、例えばカメラなどのセンサ１０１によって、自己の周囲の画像情報を取得し、取得した情報に基づいて、デバイス１００の位置確認や方向確認などに基づく姿勢（ポーズ）確認を行う。すなわちローカリゼーション（Ｌｏｃａｌｉｚａｔｉｏｎ）を行なうデバイスである。 FIG. 1 is a diagram illustrating the transition of sensor acquisition information at times t = 0, 1, and 2 of a device 100 that performs autonomous control such as an autonomous robot. For example, the device 100 acquires image information around itself using a sensor 101 such as a camera, and performs posture confirmation based on position confirmation, direction confirmation, and the like of the device 100 based on the obtained information. That is, it is a device that performs localization.
なお、本発明において「同定処理」は、ローカリゼーション（Ｌｏｃａｌｉｚａｔｉｏｎ）と同様の意味であり、少なくともデバイス位置の推定処理を含み、さらに、デバイス位置およびデバイスの向き（方向）を含むデバイスの姿勢（ポーズ）推定処理を含む場合もある。 In the present invention, the “identification process” has the same meaning as localization, includes at least a device position estimation process, and further includes a device posture and a device orientation (pose) including a device orientation (direction). An estimation process may be included.
まず、図１、図２を参照して本発明における同定処理の特徴について説明する。デバイス１００は、例えばカメラなどのセンサ１０１によって、自己の周囲の画像情報を取得する。取得する画像データは、例えば３０フレーム／ｓｅｃなどの連続する画像データである。図１において、各フレーム画像をセンサデータＺ０，Ｚ１，Ｚ２として示してある。図に示すように、
（ａ）時間ｔ＝０において、フレーム画像としてのセンサデータＺ０、
（ｂ）時間ｔ＝１において、フレーム画像としてのセンサデータＺ１、
（ｃ）時間ｔ＝２において、フレーム画像としてのセンサデータＺ２、
が順次デバイス１００のセンサ１０１において取得される。
First, the characteristics of the identification processing in the present invention will be described with reference to FIGS. The device 100 acquires image information around itself by a sensor 101 such as a camera. The image data to be acquired is continuous image data such as 30 frames / sec. In FIG. 1, each frame image is shown as sensor data Z 0 , Z 1 , Z 2 . As shown in the figure
(A) At time t = 0, sensor data Z 0 as a frame image,
(B) At time t = 1, sensor data Z 1 as a frame image,
(C) At time t = 2, sensor data Z 2 as a frame image,
Are sequentially acquired by the sensor 101 of the device 100.
従来のシステムにおけるデバイス同定処理の一般的な手法としては、それぞれのタイミングにおいて取得したフレーム画像としてのセンサデータＺ０、Ｚ１、Ｚ２の各々を個別に解析してデバイスの位置や方向の推定を行なう手法である。 As a general method of device identification processing in a conventional system, each of sensor data Z 0 , Z 1 , Z 2 as a frame image acquired at each timing is individually analyzed to estimate the position and direction of the device. It is a technique to do.
しかしながら、このように、センサデータＺ０、Ｚ１、Ｚ２の各々を、個別に順次解析を行なうと、解析処理の負荷が大きくなり、デバイスに求められるデータ処理能力を高くしなければならない。デバイスの処理能力が十分でない場合には、処理の遅延が発生する。例えばデバイスが自走デバイスである場合には、走行スピードを低下させてしまうといった問題が発生する。 However, if each of the sensor data Z 0 , Z 1 , and Z 2 is individually analyzed in this way, the analysis processing load increases, and the data processing capability required for the device must be increased. When the processing capability of the device is not sufficient, processing delay occurs. For example, when the device is a self-propelled device, there arises a problem that the traveling speed is reduced.
本発明においては、このような問題を解決して、効率的なデバイス同定処理を実現するものである。すなわち、センサデータＺ０、Ｚ１、Ｚ２の各々を、個別に順次解析を行なうのではなく、例えば、図２に示すように、複数のセンサデータＺ０、Ｚ１、Ｚ２をまとめて、センサデータの集合｛Ｚ０∪Ｚ１∪Ｚ２｝を設定して、このセンサデータ集合｛Ｚ０∪Ｚ１∪Ｚ２｝に基づく解析データｆ＝｛Ｚ０∪Ｚ１∪Ｚ２｝をセンサパッチとして、このセンサパッチに基づく同定処理を実行する。 In the present invention, such a problem is solved and an efficient device identification process is realized. That is, each of the sensor data Z 0 , Z 1 , Z 2 is not individually analyzed sequentially, but a plurality of sensor data Z 0 , Z 1 , Z 2 are collected together, for example, as shown in FIG. , a set of sensor data by setting {Z 0 ∪Z 1 ∪Z 2}, the sensor data set {Z 0 ∪Z 1 ∪Z 2} based on the analysis data f = {Z 0 ∪Z 1 ∪Z 2} As a sensor patch, identification processing based on this sensor patch is executed.
センサデータＺ０、Ｚ１、Ｚ２の各々を個別に解析しようとした場合、それぞれのセンサデータＺ０、Ｚ１、Ｚ２に重複領域が含まれる場合であっても、それぞれ個別に処理を行なうことが必要となり、データ処理負荷が増大し、また、例えばセンサデータＺ０、Ｚ１、Ｚ２が、３０フレーム／ｓｅｃのフレーム画像であるような場合、その連続フレームのスピートに追随した処理を行なうことが必要となる。 Sensor data Z 0, Z 1, when each of Z 2 tries individually analyzed, even if it contains the respective sensor data Z 0, Z 1, Z 2 in the overlap region, each separately processed When the sensor data Z 0 , Z 1 , Z 2 is a frame image of 30 frames / sec, for example, processing that follows the speed of the continuous frame is required. Must be performed.
また、センサデータＺ０、Ｚ１、Ｚ２には、位置識別に適用可能な有効な情報と意味のないノイズ情報が含まれるが、個別のセンサデータの処理を行なうと、有効情報の比率が低下、すなわちＳＮＲ（Ｓｉｇｎａｌ ｔｏ Ｎｏｉｓｅ Ｒａｔｉｏ）が低くなるという問題がある。 The sensor data Z 0 , Z 1 , and Z 2 include effective information applicable to position identification and meaningless noise information. However, when individual sensor data is processed, the ratio of effective information is increased. There is a problem that the SNR (Signal to Noise Ratio) decreases.
一方、図２に示すように、センサデータ集合｛Ｚ０∪Ｚ１∪Ｚ２｝に基づく解析データｆ＝｛Ｚ０∪Ｚ１∪Ｚ２｝をセンサパッチとして生成し、このセンサパッチに基づく同定処理を実行する構成では、複数のセンサデータＺ０、Ｚ１、Ｚ２の関連性など、個別のデータのみからは得られない情報の適用が可能となり、ノイズを削減してＳＮＲの高いデータに基づく同定処理を行なうことが可能となる。また、処理タイミングも入力フレームのスピード、例えば３０フレーム／ｓｅｃごとに１つのフレーム画像の処理を行う必要がなくなり、処理負荷の低減が可能となる。 On the other hand, as shown in FIG. 2, analysis data f = {Z 0 ∪Z 1 ∪Z 2 } based on the sensor data set {Z 0 ∪Z 1 ∪Z 2 } is generated as a sensor patch, and based on this sensor patch. In the configuration for executing the identification process, it is possible to apply information that cannot be obtained only from individual data, such as the relevance of a plurality of sensor data Z 0 , Z 1 , Z 2 , and data with high SNR by reducing noise. It is possible to perform an identification process based on. Further, it is not necessary to process one frame image every input frame speed, for example, every 30 frames / sec, and the processing load can be reduced.
本発明は、上述したように、センサパッチの生成と生成したセンサパッチに基づくデバイス同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行することを１つの特徴とする。 As described above, the present invention is characterized in that a sensor patch is generated and a device identification process (Localization) based on the generated sensor patch is executed.
センサパッチを適用した本発明の構成についての説明の前に、一般的なデバイスの位置や方向をデバイスの動き情報と、カメラ撮影画像などのセンサ情報とに基づいて検出する基本的なローカリゼーション構成について、図３以下を参照して説明する。 Before describing the configuration of the present invention to which a sensor patch is applied, a basic localization configuration for detecting the position and direction of a general device based on device motion information and sensor information such as a camera image. This will be described with reference to FIG.
図３に示すように、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行するデバイスは、同定処理部（Ｌｏｃａｌｉｚａｔｉｏｎ）２００を有し、先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）１５１と、動きデータ（Ｍｏｔｉｏｎ Ｄａｔａ）１７１と、センサデータ（Ｓｅｎｓｏｒ Ｄａｔａ）１７２を入力として、同定情報（Ｃｕｒｒｅｎｔ Ｂｅｌｉｅｆ）１８１を出力する。 As shown in FIG. 3, a device that executes identification processing (Localization) includes an identification processing unit (Localization) 200, preceding identification information (Previous Belief) 151, motion data (Motion Data) 171, and sensor data. Using (Sensor Data) 172 as input, identification information (Current Belief) 181 is output.
先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）１５１は、同定処理部２００において１サイクル前に同定処理のなされた同定情報である。なお、ここで同定情報は、デバイスの位置（Ｐｏｓｉｔｉｏｎ）、方向（Ｏｒｉｅｎｔａｔｉｏｎ）を含むデバイスの姿勢（Ｐｏｓｅ）情報である。 The preceding identification information (Preview Belief) 151 is identification information that has been subjected to identification processing one cycle before in the identification processing unit 200. Here, the identification information is device posture information including a device position (Position) and a direction (Orientation).
同定処理部２００は、先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）１５１に、最新のデバイスの動き情報（Ｍｏｔｉｏｎ Ｄａｔａ）１７１と、デバイスの有するセンサから入力するセンサ情報（Ｓｅｎｓｏｒ Ｄａｔａ）１７２とに基づいて、新たな同定情報（Ｃｕｒｒｅｎｔ Ｂｅｌｉｅｆ）１８１を生成して出力する。 Based on the latest identification information (motion data) 171 and sensor information (sensor data) 172 input from the sensor of the device, the identification processing unit 200 adds new identification information to the previous identification information (previous belief) 151. Identification information (Current Belief) 181 is generated and output.
さらに、同定処理部２００を具体化して示すと図４の構成となる。同定処理部２００は、予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１と、更新処理部（Ｕｐｄａｔｅ）２０１とによって構成される。 Furthermore, if the identification processing unit 200 is specifically shown, the configuration shown in FIG. 4 is obtained. The identification processing unit 200 includes a prediction processing unit (Prediction) 201 and an update processing unit (Update) 201.
予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１は、先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）１５１に対して、デバイスが検出する最新のデバイスの動き情報（Ｍｏｔｉｏｎ Ｄａｔａ）１７１を入力して、デバイスの動きを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する。 The prediction processing unit (Prediction) 201 inputs the latest device motion information (Motion Data) 171 detected by the device to the preceding identification information (Preview Belief) 151, and the position of the device in consideration of the device motion , And a predicted value of the device posture (Pose) including the direction.
更新処理部（Ｕｐｄａｔｅ）２０２は、デバイスの動きを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１から入力して、さらに、デバイスのセンサが取得するセンサ情報（Ｓｅｎｓｏｒ Ｄａｔａ）１７２を入力して、センサ情報を加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出し、これを新たな同定情報（Ｃｕｒｒｅｎｔ Ｂｅｌｉｅｆ）１８１として出力する。具体的には、例えばデバイスの位置および方向を定めたデバイス周りの地図（環境地図）を生成する。 The update processing unit (Update) 202 inputs a predicted value of the device posture (Pose) including the position and direction of the device in consideration of the movement of the device from the prediction processing unit (Prediction) 201, and further, the sensor of the device Sensor information (Sensor Data) 172 to be acquired is input, and a predicted value of a device posture (Pose) including the position and direction of the device in consideration of the sensor information is calculated, and this is used as new identification information (Current Belief) 181. Output as. Specifically, for example, a map around the device (environment map) that defines the position and direction of the device is generated.
予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１の入力するデバイスの動き情報（Ｍｏｔｉｏｎ Ｄａｔａ）１７１の具体例について、図５を参照して説明する。図５（ａ）には、デバイス２５０が、先行デバイス姿勢２５０ａから現デバイス姿勢２５０ｂへ変化した場合の例を示している。先行デバイス姿勢２５０ａは、前の同定処理サイクル時のデバイスの位置および方向を持ち、現デバイス姿勢２５０ｂは、今回の同定処理サイクル時のデバイスの位置および方向を持つ。各デバイス姿勢２５０ａ，２５０ｂに示す矢印がデバイスの方向（例えばデバイスがロボットである場合、ロボットの正面方向）を示している。Ｘバー、Ｙバー、θバーは、それぞれ例えばグローバル座標系におけるデバイスの位置と、方向を示す値である。なお、明細書中に記載する［バー］は、図において示すＸ，Ｙ，Ｚの上部に示すバーを意味するものとする。 A specific example of device motion information (Motion Data) 171 input by the prediction processing unit (Prediction) 201 will be described with reference to FIG. FIG. 5A shows an example in which the device 250 changes from the previous device posture 250a to the current device posture 250b. The preceding device posture 250a has the position and direction of the device at the previous identification processing cycle, and the current device posture 250b has the position and direction of the device at the current identification processing cycle. The arrows shown in the device postures 250a and 250b indicate the direction of the device (for example, the front direction of the robot when the device is a robot). X bar, Y bar, and θ bar are values indicating the position and direction of the device in the global coordinate system, for example. In addition, [bar] described in the specification means a bar shown above X, Y, and Z shown in the drawing.
予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１の入力するデバイスの動き情報（Ｍｏｔｉｏｎ Ｄａｔａ）ｕｔは、図５（ｂ）に示すように、
ｕｔ＝＜δｒｏｔ１，δｒｏｔ２，δｔｒａｎｓ＞
である。
δｒｏｔ１，は、図５（ａ）に示すように、先行デバイス姿勢２５０ａにおけるデバイスの方向を示す角度であり、先行デバイス姿勢２５０ａ位置と、現デバイス姿勢２５０ｂ位置とを結ぶ直線からの角度である。
δｒｏｔ２，は、図５（ａ）に示すように、現デバイス姿勢２５０ｂにおけるデバイスの方向を示す角度であり、先行デバイス姿勢２５０ａ位置と、現デバイス姿勢２５０ｂ位置とを結ぶ直線からの角度である。
δｔｒａｎｓ，は、図５（ａ）に示すように、先行デバイス姿勢２５０ａ位置と、現デバイス姿勢２５０ｂ位置との距離に相当する。
As shown in FIG. 5B, the motion information (Motion Data) u t of the device input by the prediction processing unit (Prediction) 201 is
u t = <δ rot1 , δ rot2 , δ trans >
It is.
As shown in FIG. 5A , δ rot1 is an angle indicating the direction of the device in the preceding device posture 250a, and is an angle from a straight line connecting the preceding device posture 250a position and the current device posture 250b position. .
δ rot2 , as shown in FIG. 5A , is an angle indicating the direction of the device in the current device posture 250b, and is an angle from a straight line connecting the preceding device posture 250a position and the current device posture 250b position. .
As shown in FIG. 5A, δ trans , corresponds to the distance between the preceding device posture 250a position and the current device posture 250b position.
δｔｒａｎｓ，δｒｏｔ１，δｒｏｔ２は、グローバル座標系によって示すと、以下のように表現される。
この動き情報は、デバイスの動作を観測するデバイス内の動き検出部の観測情報に基づいて得られる。なお、上述したデバイスの動き情報（Ｍｏｔｉｏｎ Ｄａｔａ）ｕｔは一例であり、予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１の入力する動き情報は、上述した表現に限られるものではなく、デバイスの動きを示すデータであれば様々な形態のデータが適用可能である。 This motion information is obtained based on observation information of a motion detector in the device that observes the operation of the device. Note that the above-described device motion information (Motion Data) u t is an example, and the motion information input by the prediction processing unit (Prediction) 201 is not limited to the above-described expression, and is data indicating device motion. Various forms of data are applicable if there are.
予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１は、先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）に対して、デバイスが検出する最新のデバイスの動き情報ｕｔを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌバー（ｘｔ）］を算出する。ｘｔは、時間ｔにおけるデバイスの姿勢（Ｐｏｓｅ）を示す。なお、このデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌバー（ｘｔ）］算出処理には、例えばベイズ推定が適用される。ベイズ推定を適用した算出処理については後述する。 The prediction processing unit (Prediction) 201 estimates the device posture (Pose) including the position and direction of the device in consideration of the latest device motion information u t detected by the device with respect to the preceding identification information (Previous Belief). The value [Bel bar (x t )] is calculated. x t represents the posture (Pose) of the device at time t. Note that, for example, Bayesian estimation is applied to the estimated value [Bel bar (x t )] of the device attitude (Pose). A calculation process using Bayesian estimation will be described later.
更新処理部（Ｕｐｄａｔｅ）２０２は、上述した予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１の算出した推定値［Ｂｅｌバー（ｘｔ）］、すなわち、デバイスの動き情報ｕｔを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌバー（ｘｔ）］を入力して、さらに、デバイスのセンサが取得するセンサ情報（Ｓｅｎｓｏｒ Ｄａｔａ）１７２を入力して、センサ情報を加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］を算出し、これを新たなデバイスの同定情報（Ｃｕｒｒｅｎｔ Ｂｅｌｉｅｆ）１８１として出力する。この情報に基づいて、例えばデバイスの位置および方向を定めたデバイス周りの地図（環境地図）を生成することができる。 The update processing unit (Update) 202 displays the estimated value [Bel bar (x t )] calculated by the prediction processing unit (Prediction) 201, that is, the device position and direction in consideration of the device motion information u t. The position of the device in consideration of the sensor information by inputting the estimated value [Bel bar (x t )] of the device attitude (Pose) including the sensor information (Sensor Data) 172 acquired by the sensor of the device. , And an estimated value [Bel (x t )] of the device attitude (Pose) including the direction, and outputs this as new device identification information (Current Belief) 181. Based on this information, for example, a map around the device (environment map) that defines the position and direction of the device can be generated.
更新処理部（Ｕｐｄａｔｅ）２０２の入力するセンサ情報（Ｓｅｎｓｏｒ Ｄａｔａ）１７２の具体例について、図６を参照して説明する。図６は、デバイス２５０のセンサ、例えばカメラによって取得され画像データを示している。図４に示すセンサ情報１７２［ｚｔ］に相当する。 A specific example of the sensor information (Sensor Data) 172 input by the update processing unit (Update) 202 will be described with reference to FIG. FIG. 6 shows image data acquired by a sensor of the device 250, such as a camera. This corresponds to the sensor information 172 [z t ] shown in FIG.
ここでは、センサ情報ｚｔは、時間ｔにおけるデバイスの姿勢「Ｘｔ」においてデバイスのセンサ、例えばカメラによって撮影される１つの画像フレームデータである。図６（ａ）に示す丸印（○）は画像データの画素ｓ１〜ｓｎを模式的に示している。センサ情報ｚｔは、複数の画素情報から構成され、
ｚｔ＝｛ｓ１，・・・ｓｎ｝
として示される。
Here, the sensor information z t is one image frame data photographed by a device sensor, for example, a camera in the device posture “Xt” at time t. Circles shown in FIG. 6 (a) (○) shows schematically a pixel s 1 ~s n image data. The sensor information zt is composed of a plurality of pixel information,
z t = {s 1 ,... sn}
As shown.
図４に示す更新処理部（Ｕｐｄａｔｅ）２０２は、上述した予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１がデバイスの動き情報ｕｔを考慮して算出したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌバー（ｘｔ）］を入力して、さらに、図６を参照して説明したデバイスのセンサが取得するセンサ情報（Ｓｅｎｓｏｒ Ｄａｔａ）１７２［ｚｔ］を入力して、センサ情報［ｚｔ］を加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］を算出し、これを新たなデバイスの同定情報（Ｃｕｒｒｅｎｔ Ｂｅｌｉｅｆ）１８１として出力する。このデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］算出処理には、ベイズ推定が適用される。 Update processing unit (Update) 202 shown in FIG. 4, the estimated position of the device the prediction processing unit (Prediction) 201 described above is calculated by considering the motion information u t of the device, and device orientation including direction (Pose) A value [Bel bar (x t )] is input, and further, sensor information (Sensor Data) 172 [z t ] acquired by the sensor of the device described with reference to FIG. 6 is input, and the sensor information [z position of the device in consideration of t], and the estimated value of the device orientation (pose) containing direction calculated [Bel (x t)], and outputs it as identification information (Current Belief) 181 for the new device. Bayesian estimation is applied to this device attitude (Pose) estimated value [Bel (x t )] calculation process.
ベイズ推定を適用したデバイス姿勢（Ｐｏｓｅ）の算出処理について説明する。更新処理部（Ｕｐｄａｔｅ）２０２の出力するデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］は、デバイスが、特定の姿勢［ｘｔ］となる確率を示す。図４に示すように、更新処理部（Ｕｐｄａｔｅ）２０２の出力するデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］は、以下のように示される。
Ｂｅｌ（ｘｔ）＝Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ）
上記式において、
ｚは、センサ情報
ｕは、動き情報
ｘは、デバイスの状態、すなわち姿勢（ｐｏｓｅ）を示すパラメータ
Ｐは確率密度関数を表している。
A device attitude (Pose) calculation process to which Bayesian estimation is applied will be described. The estimated value [Bel (x t )] of the device attitude (Pose) output from the update processing unit (Update) 202 indicates the probability that the device will have a specific attitude [x t ]. As shown in FIG. 4, the estimated value [Bel (x t )] of the device attitude (Pose) output from the update processing unit (Update) 202 is expressed as follows.
Bel (x t ) = P (x t | u 1: t , z 1: t )
In the above formula,
z is sensor information u is motion information x is a device state, that is, a parameter P indicating a posture P is a probability density function.
上記式：Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ）の意味は、動き情報ｕの時間１〜ｔの情報値：ｕ１：ｔと、センサ情報の時間１〜ｔの情報値：ｚ１：ｔとが与えられた条件において、時間ｔにおいて、デバイスが姿勢（ｘｔ）となる確率を算出する式である。 The meaning of the above formula: P (x t | u 1: t , z 1: t ) is as follows: information value of time 1 to t of motion information u: u 1: t and information value of time 1 to t of sensor information : Z is an equation for calculating the probability that the device will be in the posture (x t ) at time t under the condition given by 1: t .
図４において、予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１に入力する先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）１５１は、時間［ｔ−１］の同定情報であり、以下のように示される。
Ｂｅｌ（ｘｔ−１）＝Ｐ（ｘｔ−１｜ｕ１：ｔ−１，ｚ１：ｔ−１）
In FIG. 4, preceding identification information (Previous Belief) 151 input to the prediction processing unit (Prediction) 201 is identification information of time [t−1], and is represented as follows.
Bel (x t-1 ) = P (x t-1 | u 1: t-1 , z 1: t-1 )
予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１は、先行同定情報（Ｐｒｅｖｉｏｕｓ Ｂｅｌｉｅｆ）１５１に対して、デバイスが検出する最新のデバイスの動き情報（Ｍｏｔｉｏｎ Ｄａｔａ）１７１を入力して、デバイスの動きを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌバー（ｘｔ）］を算出する。ここでは、
入力する時間［ｔ−１］の先行同定情報
Ｂｅｌ（ｘｔ−１）＝Ｐ（ｘｔ−１｜ｕ１：ｔ−１，ｚ１：ｔ−１）
に、時間ｔにおける動き情報ｕｔを加えた推定処理が可能となり、この推定処理によって、［Ｂｅｌバー（ｘｔ）］が以下のように算出される。
Prior identification information of time [t−1] to be input Bel (x t−1 ) = P (x t−1 | u 1: t−1 , z 1: t−1 )
In addition, it is possible to perform the estimation process by adding the motion information u t at time t, and [Bel bar (x t )] is calculated as follows by this estimation process.
更新処理部（Ｕｐｄａｔｅ）２０２は、上述の予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１の算出した時間［ｔ］のデバイスの動き情報ｕｔを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌバー（ｘｔ）］を入力して、さらに、時間［ｔ］のデバイスのセンサが取得するセンサ情報（Ｓｅｎｓｏｒ Ｄａｔａ）ｚｔを入力して、センサ情報ｚｔを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］を算出する。これは、予測処理部（Ｐｒｅｄｉｃｔｉｏｎ）２０１の算出した推定値［Ｂｅｌバー（ｘｔ）］を適用して示すと、以下の式のように示すことができる。
上記式は、先に更新処理部（Ｕｐｄａｔｅ）２０２の出力するデバイス姿勢（Ｐｏｓｅ）の推定値［Ｂｅｌ（ｘｔ）］として説明した式、すなわち、
Ｂｅｌ（ｘｔ）＝Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ）
と等価である。このことは、以下に示す式によって証明される。
Bel (x t ) = P (x t | u 1: t , z 1: t )
Is equivalent to This is proved by the following equation.
このように、各時間（ｔ）において取得するデバイスの動き情報［ｕｔ］と、センサ情報［ｚｔ］を適用したベイズ推定により、デバイスの同定処理、すなわちデバイスの位置、方向の推定処理としてローカリセーションを行なうことができる。 As described above, device identification processing, that is, device position and direction estimation processing is performed by Bayesian estimation using device motion information [u t ] and sensor information [z t ] acquired at each time (t). Localization can be performed.
しかしながら、図４の構成による同定処理（ローカリゼーション）は、先に説明したように、センサデータＺ０、Ｚ１、Ｚ２の各々を、個別に順次解析を行なう構成である。センサデータＺ０、Ｚ１、Ｚ２の各々は、例えば、３０フレーム／ｓｅｃの撮影速度において取得される画像データであり、デバイスは、これらの画像データを連続的に解析することが必要となる。結果として、デバイスにおける解析処理の負荷が大きくなり、デバイスに求められるデータ処理能力を高くすることが必要となる。デバイスの処理能力が十分でない場合には、処理の遅延が発生する。 However, the identification process (localization) by the configuration of FIG. 4 is a configuration in which each of the sensor data Z 0 , Z 1 , Z 2 is sequentially analyzed individually as described above. Each of the sensor data Z 0 , Z 1 , and Z 2 is image data acquired at a shooting speed of 30 frames / sec, for example, and the device needs to continuously analyze these image data. . As a result, the load of analysis processing in the device increases, and it is necessary to increase the data processing capability required for the device. When the processing capability of the device is not sufficient, processing delay occurs.
本発明においては、このような問題を解決して、効率的なデバイス同定処理を実現するものであり、先に図２を参照して説明したように、センサデータＺ０、Ｚ１、Ｚ２の各々を、個別に順次解析を行なうのではなく、図２に示すように、複数のセンサデータＺ０、Ｚ１、Ｚ２をまとめて、センサデータの集合｛Ｚ０∪Ｚ１∪Ｚ２｝を設定して、このセンサデータ集合｛Ｚ０∪Ｚ１∪Ｚ２｝に基づく解析データｆ＝｛Ｚ０∪Ｚ１∪Ｚ２｝をセンサパッチとして、このセンサパッチに基づく同定処理を実行する。本発明は、このように、センサパッチの生成処理と、生成したセンサパッチに基づくデバイス同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 In the present invention, such a problem is solved and efficient device identification processing is realized. As described above with reference to FIG. 2, the sensor data Z 0 , Z 1 , Z 2 Are not individually analyzed sequentially, but as shown in FIG. 2, a plurality of sensor data Z 0 , Z 1 , Z 2 are collected and a set of sensor data {Z 0 ∪Z 1 ∪Z 2 } Is set, and analysis data f = {Z 0 ∪Z 1 ∪Z 2 } based on this sensor data set {Z 0 ∪Z 1 ∪Z 2 } is used as a sensor patch, and identification processing based on this sensor patch is executed. To do. In this way, the present invention executes a sensor patch generation process and a device identification process (Localization) based on the generated sensor patch.
図７以下を参照して、センサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）の詳細について説明する。図７は、本発明の一実施例に係る同定処理を実行する情報処理装置（デバイス）の構成を示すブロック図である。本発明の情報処理装置は、上述したように、センサパッチの生成と生成したセンサパッチに基づくデバイス同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する構成であり、図７に示すように、センサパッチ生成処理を実行するセンサパッチ生成部３０１と、同定処理を実行する同定処理部３０２を有する。 Details of the identification process (localization) to which the sensor patch is applied will be described with reference to FIG. FIG. 7 is a block diagram showing a configuration of an information processing apparatus (device) that executes identification processing according to an embodiment of the present invention. As described above, the information processing apparatus according to the present invention is configured to execute generation of a sensor patch and device identification processing (Localization) based on the generated sensor patch. A sensor patch generation unit 301 that performs identification processing, and an identification processing unit 302 that executes identification processing.
センサパッチ生成部３０１は、動き情報ｕｔとセンサ情報ｚｔを入力してセンサパッチ［ｐ］を生成し、さらに、デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］を生成して、同定処理部３０２に出力する。デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］は、例えばデバイスのある基準位置（例えば初期位置）からの変位と方向（ｘ，ｙ，θ）をパラメータとして持つ。 The sensor patch generation unit 301 inputs the motion information u t and the sensor information z t to generate a sensor patch [p], and further generates device posture information (pose transform) [U], and an identification processing unit It outputs to 302. The device attitude information (pose transform) [U] has, for example, the displacement and direction (x, y, θ) from a certain reference position (for example, initial position) of the device as parameters.
センサパッチ生成部３０１は、例えば複数の連続するセンサ情報（例えば複数の連続フレーム画像）に基づいて、１つのセンサパッチを生成する。図８に、センサパッチ生成例を示す。たとえばセンサパッチ［ｐ０］３１０は、センサパッチ生成部３０１において、複数のセンサ情報ｚ０〜ｚ３に基づいて生成される。センサ情報ｚ０は、例えば、時間（ｔ＝０）において撮影されたフレーム画像データである。センサ情報ｚ１は、例えば、時間（ｔ＝１）において撮影されたフレーム画像データである。 The sensor patch generation unit 301 generates one sensor patch based on, for example, a plurality of continuous sensor information (for example, a plurality of continuous frame images). FIG. 8 shows an example of sensor patch generation. For example, the sensor patch [p 0 ] 310 is generated by the sensor patch generation unit 301 based on a plurality of pieces of sensor information z 0 to z 3 . Sensor information z 0 is, for example, a frame image data captured at time (t = 0). Sensor information z 1 is, for example, a frame image data captured at time (t = 1).
図８において、センサパッチ［ｐ１］３１１は、センサパッチ生成部３０１において、複数のセンサ情報ｚ４〜ｚ６に基づいて生成される。センサパッチ［ｐ２］３１２は、センサパッチ生成部３０１において、複数のセンサ情報ｚ８〜ｚ１２に基づいて生成される。センサパッチ［ｐ３］３１３は、センサパッチ生成部３０１において、複数のセンサ情報ｚ１３〜ｚ１５に基づいて生成される。 In FIG. 8, the sensor patch [p 1 ] 311 is generated by the sensor patch generation unit 301 based on a plurality of pieces of sensor information z 4 to z 6 . The sensor patch [p 2 ] 312 is generated by the sensor patch generation unit 301 based on a plurality of pieces of sensor information z 8 to z 12 . The sensor patch [p 3 ] 313 is generated by the sensor patch generation unit 301 based on the plurality of sensor information z 13 to z 15 .
センサパッチ生成部３０１において生成されたセンサパッチ［ｐ］と、デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］は、同定処理部３０２に入力され、これらの入力情報に基づく同定処理、すなわち、情報処理装置（デバイス）の位置および方向を含む姿勢（ｐｏｓｅ）の判定処理が行われる。同定処理部３０２における同定処理によって、デバイス周りの地図（環境地図）に、デバイスの位置および方向を設定することが可能となる。 The sensor patch [p] and the device posture information (pose transform) [U] generated by the sensor patch generation unit 301 are input to the identification processing unit 302, and an identification process based on the input information, that is, information processing A determination process of a posture including the position and direction of the device (device) is performed. By the identification processing in the identification processing unit 302, the position and direction of the device can be set in a map around the device (environment map).
図９にセンサパッチ生成部３０１において生成されるセンサパッチ、同定処理部３０２において、センサパッチに基づく同定処理によって生成される環境地図、および同定処理部３０２において、センサパッチ［ｐ］と、デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］とに基づく同定処理結果によって得られるデバイスの位置および方向を環境地図上に設定した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）結果の例を示す。 In FIG. 9, the sensor patch generated by the sensor patch generation unit 301, the environment map generated by the identification processing based on the sensor patch in the identification processing unit 302, and the sensor patch [p] The example of the identification process (Localization) result which set the position and direction of the device obtained by the identification process result based on attitude | position information (pose transform) [U] on the environment map is shown.
図９において、
（ａ）センサパッチ
（ｂ）環境地図
（ｃ）同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）結果
を示している。
In FIG.
(A) Sensor patch (b) Environmental map (c) Identification processing (Localization) result.
センサパッチは、デバイスのセンサ情報（例えば撮影画像）に基づいて生成される画像情報であり、デバイスの有するセンサによって撮影された画像に基づく情報から構成される。本処理例では、複数の連続するセンサ情報（例えば複数の連続フレーム画像）に基づいて、１つのセンサパッチが生成される。図９の（ａ）センサパッチに示す矩形領域３１５が、図９（ｃ）同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）結果に示す矩形領域３１６に相当する。矩形領域３１５を左に９０°回転させると矩形領域３１６に対応する。同定処理部３０２は、センサパッチ［ｐ］と、デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］とに基づく同定処理結果によって、例えば図９（ｃ）に示すデバイスの位置および方向を環境地図上に設定した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）結果を生成する。 The sensor patch is image information generated based on sensor information (for example, a captured image) of the device, and includes information based on an image captured by the sensor of the device. In the present processing example, one sensor patch is generated based on a plurality of continuous sensor information (for example, a plurality of continuous frame images). A rectangular area 315 shown in (a) sensor patch in FIG. 9 corresponds to a rectangular area 316 shown in (c) identification processing (Localization) result in FIG. When the rectangular area 315 is rotated 90 ° to the left, it corresponds to the rectangular area 316. Based on the result of the identification process based on the sensor patch [p] and the device posture information (pose transform) [U], the identification processing unit 302 displays the position and direction of the device shown in FIG. A set identification process (Localization) result is generated.
図８に示すように、同定処理部３０２における同定処理（Ｌｏｃａｌｉｚｅ）は、各センサパッチの入力に基づいて実行される。図８に示すように、同定処理０（Ｌｏｃａｌｉｚｅ０）は、センサパッチ生成部３０１において、複数のセンサ情報ｚ０〜ｚ３に基づいて生成されたセンサパッチ［ｐ０］３１０に基づいて実行され、同定処理１（Ｌｏｃａｌｉｚｅ１）は、センサパッチ生成部３０１において、複数のセンサ情報ｚ４〜ｚ６に基づいて生成されたセンサパッチ［ｐ１］３１１に基づいて実行され、同定処理２（Ｌｏｃａｌｉｚｅ２）は、センサ情報ｚ８〜ｚ１２に基づいて生成されたセンサパッチ［ｐ０］３１０に基づいて実行され、同定処理３（Ｌｏｃａｌｉｚｅ３）は、センサ情報ｚ１３〜ｚ１５に基づいて生成されたセンサパッチ［ｐ３］３１３に基づいて実行されることになる。 As shown in FIG. 8, the identification process (Localize) in the identification processing unit 302 is executed based on the input of each sensor patch. As illustrated in FIG. 8, the identification process 0 (Localize0) is executed based on the sensor patch [p 0 ] 310 generated based on the plurality of pieces of sensor information z 0 to z 3 in the sensor patch generation unit 301. The identification process 1 (Localize 1) is executed based on the sensor patch [p 1 ] 311 generated based on the plurality of pieces of sensor information z 4 to z 6 in the sensor patch generation unit 301, and the identification process 2 (Localize 2) is performed. The sensor patch [p 0 ] 310 generated based on the sensor information z 8 to z 12 is executed, and the identification process 3 (Localize 3) is performed based on the sensor information z 13 to z 15. [P 3 ] 313 is executed.
このように、本発明の情報処理装置においては、センサ情報の取得タイミング毎に同定処理を実行するのではなく、１つのセンサパッチを単位とした同定処理を実行する。なお、図７、図８から理解されるように、センサパッチ生成部３０１でのセンサパッチ生成処理は、同定処理部３０２からのリセット信号の入力に基づいて開始される。センサパッチ生成処理の終了は、センサパッチ生成部３０１で判断を行ない、センサパッチ生成を終了する。この処理の詳細については後述する。センサパッチ生成部３０１では、センサ情報ｚｔと、動き情報ｕｔを入力値としたセンサパッチ生成処理を実行する。デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］の生成に際しても、センサパッチ生成期間における動き情報ｕｔ全てが反映された処理が実行されることになる。 As described above, in the information processing apparatus according to the present invention, the identification process is not performed at every sensor information acquisition timing, but the identification process is performed in units of one sensor patch. As can be understood from FIGS. 7 and 8, the sensor patch generation process in the sensor patch generation unit 301 is started based on the input of the reset signal from the identification processing unit 302. The end of the sensor patch generation process is determined by the sensor patch generation unit 301, and the sensor patch generation ends. Details of this processing will be described later. The sensor patch generation unit 301 executes sensor patch generation processing using the sensor information z t and the motion information u t as input values. When generating the device posture information (pose transform) [U], the process reflecting all the motion information u t in the sensor patch generation period is executed.
図１０を参照して、センサパッチ生成部３０１において実行する処理の詳細について説明する。まず、ステップＳ１０１において、生成済みの最新のセンサパッチ［ｐ］と、取得済みの最新のデバイス姿勢情報［Ｕ］を入力する。なお、これらの情報は初期状態では存在しない。ステップＳ１０２において、同定処理部３０２からのリセット信号の入力有無について判定を行ない、リセット信号が入力されていない場合は、ステップＳ１０３に進み、リセット信号が入力された場合は、ステップＳ１１１において生成パッチのリセットを実行した後、ステップＳ１０３に進む。 Details of processing executed in the sensor patch generation unit 301 will be described with reference to FIG. First, in step S101, the latest sensor patch [p] that has been generated and the latest device posture information [U] that has been acquired are input. Such information does not exist in the initial state. In step S102, it is determined whether or not a reset signal is input from the identification processing unit 302. If no reset signal is input, the process proceeds to step S103. If a reset signal is input, the generated patch is determined in step S111. After executing the reset, the process proceeds to step S103.
ステップＳ１０３において、センサ情報ｚｔを入力して、センサパッチの更新処理を実行する。ここでセンサ情報ｚｔは、例えばデバイスに備えられたセンサとしてのカメラの撮影した１枚のフレーム画像データである。 In step S103, enter the sensor information z t, it executes the update processing of the sensor patch. Here, the sensor information z t is, for example, one piece of frame image data taken by a camera as a sensor provided in the device.
ステップＳ１０３におけるセンサ情報ｚｔ入力に基づくセンサパッチ更新処理について、図１１を参照して説明する。センサパッチは、センサ情報ｚｔ入力に基づくデバイス周囲の地図を構成するセンサパッチ生成処理として実行される。このセンサパッチ生成処理には、例えば本出願人の出願に係る公開特許［特開２００３−２６６３４９、特開２００６−１１８８０］に記載された技術を適用することができる。すなわち、例えばセンサとしてのカメラの撮影画像に基づいて３次元の距離データを取得して地図（環境地図）としてのセンサパッチを作成する。 For the sensor patch update process based on the sensor information z t input at step S103, it will be described with reference to FIG. 11. The sensor patch is executed as a sensor patch generation process that forms a map around the device based on the sensor information z t input. For this sensor patch generation processing, for example, a technique described in published patents [JP 2003-266349, JP 2006-11880] according to the applicant's application can be applied. That is, for example, three-dimensional distance data is acquired based on a photographed image of a camera as a sensor to create a sensor patch as a map (environment map).
図１１に示すセンサパッチ（地図）３２０がセンサ情報ｚｔ入力に基づくデバイス周囲の地図の例である。地図は、所定の領域単位のセル（単位グリッドとも呼ばれる）毎に障害物があるか否かを記録した地図でありグリッドマップとも呼ばれる。
各セル（単位グリッド）は、
障害物（Ｏｂｓｔａｃｌｅ）
自由スペース（Ｆｒｅｅ）
不明（Ｕｎｋｎｏｗｎ）
の領域情報を有する。
A sensor patch (map) 320 shown in FIG. 11 is an example of a map around the device based on sensor information z t input. The map is a map that records whether or not there is an obstacle for each cell (also called a unit grid) in a predetermined area unit, and is also called a grid map.
Each cell (unit grid)
Obstacle
Free space (Free)
Unknown
Area information.
例えばデバイスのセンサから取得した画像を区分するグリッドを作成し、グリットによって区分された単位グリッド（＝セル）に対して、障害物（Ｏｂｓｔａｃｌｅ）、自由スペース（Ｆｒｅｅ）、不明（Ｕｎｋｎｏｗｎ）の領域判別を行なう。図１１において、デバイス３２１の視野領域（Ｆｉｅｌｄ ｏｆ Ｖｉｅｗ）にないセル（グレー領域）は、まだ測定していないセル、すなわち
不明（Ｕｎｋｎｏｗｎ）
として設定されるセルである。
For example, a grid for dividing an image acquired from a sensor of a device is created, and an obstruction, free space, and unknown area are identified for a unit grid (= cell) divided by grids. To do. In FIG. 11, a cell (gray area) that is not in the field of view (Field of View) of the device 321 is a cell that has not yet been measured, that is, Unknown (Unknown).
Cell set as.
デバイス３２１によって測定されたセルは黒［＝障害物（Ｏｂｓｔａｃｌｅ）］で表され、測定点にあたるセルとの間のセルは、白［＝自由スペース（Ｆｒｅｅ）］で表されている。測定対象点であるセルとデバイスを結ぶ線分の間に存在するセルに対して、エンプティ（Ｅｍｐｔｙ）処理を行う。続いて、測定点にあたるセルに対してオキュパイド（Ｏｃｃｕｐｉｅｄ）処理を行う。グリッドは、セルＣに対して障害物の存在確率（障害物によって占有されている確率）ｐ（ｃ）を保持しており、このエンプティ処理及びオキュパイド処理は、これら各セルに対する統計的な処理として実行される。 The cell measured by the device 321 is represented by black [= Obscule], and the cell between the cells corresponding to the measurement points is represented by white [= Free space (Free)]. An empty process is performed on a cell existing between a line segment connecting a cell as a measurement target point and the device. Subsequently, an occuped process is performed on the cell corresponding to the measurement point. The grid holds an obstacle existence probability (probability occupied by an obstacle) p (c) for the cell C, and this empty processing and occupy processing are statistical processing for each of these cells. Executed.
エンプティ処理は、障害物の存在確率を低下させる処理であり、オキュパイド処理は障害物の存在確率を増加させる処理である。エンプティ処理及びオキュパイド処理の存在確率を算出する手法の一例として、ベイズの更新則を用いる。エンプティ処理のとき下記式（１）により占有確率を減少させ、オキュパイド処理のとき下記式（２）により占有確率を増加させる。
上記式（１）は、セルＣの占有確率を表すｐ（ｃ）が占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］という条件下にあるセルＣの確率を表しており、式（２）は、セルＣの占有確率を表すｐ（ｃ）が非占有［エンプティ（Ｅｍｐｔｙ）］という条件下にあるセルＣの確率を表している。式（１）または式（２）においてセルＣが占有されているか、または占有されていないかを表す確率ｐ（ｓ｜・・・）、ｐ（−ｓ｜・・・）は、予め決められた閾値ｔｈである。 The above equation (1) represents the probability of the cell C under the condition that p (c) representing the occupation probability of the cell C is occupied [Occupied], and the equation (2) represents the occupation of the cell C. P (c) representing the probability represents the probability of the cell C under the condition of unoccupied [Empty]. The probabilities p (s | ...) and p (-s | ...) representing whether the cell C is occupied or not occupied in the formula (1) or the formula (2) are determined in advance. Threshold th.
このようにして、各セルの状態判定が実行されて、各セルについて、
障害物（Ｏｂｓｔａｃｌｅ）
自由スペース（Ｆｒｅｅ）
不明（Ｕｎｋｎｏｗｎ）
のいずれに属するがが決定されて、環境地図が作成される。ステップＳ１０３では、新規入力するセンサ情報ｚｔ入力に応じてセンサパッチの更新が行なわれる。
In this way, the state determination of each cell is performed, and for each cell,
Obstacle
Free space (Free)
Unknown
The environmental map is created by deciding which one of the two belongs. In step S103, updating of the sensor patch is performed in response to sensor information z t input for new input.
次に、ステップＳ１０４において、デバイスの動き情報ｕｔを入力し、座標更新およびデバイス姿勢（ｐｏｓｅ）更新処理を実行する。この座標更新およびデバイス姿勢（ｐｏｓｅ）更新処理について、図１２、図１３を参照して説明する。まず、座標更新処理について、図１２を参照して説明する。座標更新処理は、例えば本出願人の出願に係る公開特許［特開２００３−２６６３４９］に記載された技術を適用することができる。この処理は、デバイスの動きに応じて、地図、すなわちグリッドに分割された地図を更新する処理である。図１２（ａ），（ｂ），（ｃ）は、デバイスの移動量に応じた地図（環境地図）の更新処理例を示している。（ａ）〜（ｃ）とも上から下の地図に更新されている。 Next, in step S104, device movement information u t is input, and coordinate update and device posture update processing is executed. The coordinate update and device posture update process will be described with reference to FIGS. First, the coordinate update process will be described with reference to FIG. For the coordinate update processing, for example, a technique described in an open patent [Japanese Patent Laid-Open No. 2003-266349] related to the application of the present applicant can be applied. This process is a process of updating a map, that is, a map divided into grids according to the movement of the device. FIGS. 12A, 12B, and 12C show examples of a map (environment map) update process corresponding to the amount of movement of the device. (A) to (c) are updated from the top to the bottom map.
図１２（ａ）は、グリッドに分割された１つのセルの内部でのデバイス３３１の移動における更新処理例でありこの場合、座標は更新されず、デバイスの位置のみが同じ座標の地図において更新される。
図１２（ｂ）は、グリッドに分割されたセルを越えたデバイス３３１の移動における更新処理例でありこの場合、デバイスの地図上のセル位置を中心とする座標更新を実行し、地図およびデバイスの位置を更新する。
図１２（ｃ）は、グリッドに分割された１つのセルの内部でのデバイス３３１の回転における更新処理例でありこの場合、座標は更新されず、デバイスの方向（向き）のみが同じ座標の地図において更新される。
FIG. 12A is an example of update processing in the movement of the device 331 within one cell divided into grids. In this case, the coordinates are not updated, and only the device position is updated in the map of the same coordinates. The
FIG. 12B is an example of update processing in the movement of the device 331 beyond the cells divided into grids. In this case, the coordinate update centered on the cell position on the map of the device is executed, and the map and the device Update position.
FIG. 12C is an example of update processing in the rotation of the device 331 inside one cell divided into grids. In this case, the coordinates are not updated, and only the device direction (orientation) is the same coordinate map. Updated at
なお、地図（環境地図）の設定態様としては、グローバル座標系を適用する処理、デバイス依存のデバイス中心の座標としたデバイス中心座標系、パッチ依存のパッチ中心の座標としたパッチ中心座標系など、様々な座標系の適用が可能であり、座標更新処理においては、適用する座標系に応じた処理を実行することになる。 Note that the map (environment map) setting mode includes a process of applying a global coordinate system, a device center coordinate system that is device-dependent device center coordinates, a patch center coordinate system that is patch-dependent patch center coordinates, etc. Various coordinate systems can be applied, and in the coordinate update process, a process according to the applied coordinate system is executed.
次に図１３以下を参照して、ステップＳ１０４において実行するデバイスの動き情報ｕｔに基づく、デバイス姿勢（ｐｏｓｅ）更新処理について説明する。先に説明したように、センサパッチ生成部３０１は、センサパッチの生成、出力とともに、デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］を生成して同定処理部３０２に出力する。ステップＳ１０４では、デバイスの動き情報ｕｔを入力し、デバイス姿勢（ｐｏｓｅ）更新処理が実行される。デバイスの動き情報は、デバイス自身の駆動情報に基づいてデバイス自身が算出する。移動情報の累積に基づいて現在の位置および方向が算出され、デバイスの姿勢情報（ｐｏｓｅ ｔｒａｎｓｆｏｒｍ）［Ｕ］は、例えばデバイスの初期位置からの変化情報として算出される。 Referring now to FIG. 13 below, based on motion information u t devices running in step S104, it will be described the device orientation (pose) update process. As described above, the sensor patch generation unit 301 generates device posture information (pose transform) [U] together with the generation and output of the sensor patch and outputs the device posture information (U) to the identification processing unit 302. In step S104, device motion information u t is input, and device attitude (pose) update processing is executed. The device motion information is calculated by the device itself based on the drive information of the device itself. The current position and direction are calculated based on the accumulation of the movement information, and the device posture information (pose transform) [U] is calculated as change information from the initial position of the device, for example.
更新前に保持している先行するデバイス姿勢情報Ｕを、
Ｕ＝（ｘ，ｙ，θ）とする。ｘ，ｙは２次元平面における位置、θは角度を示し、適用する座標系に応じた値が設定される。例えば、それぞれ基準位置（例えば初期位置）におけるデバイス姿勢からの変位量として規定される。
この先行姿勢情報Ｕに対して、新たに検出されたデバイス動き情報ｕｔに基づく更新処理を実行して、最新姿勢情報［Ｕ'］を生成する。Ｕ，Ｕ'，ｕｔには、以下の関係式が成立する。
Let U = (x, y, θ). x and y are positions on a two-dimensional plane, θ is an angle, and a value corresponding to the coordinate system to be applied is set. For example, each is defined as a displacement amount from the device posture at a reference position (for example, an initial position).
An update process based on the newly detected device motion information u t is executed on the preceding posture information U to generate the latest posture information [U ′]. U, U ', the u t, the following equation is established.
最新姿勢情報［Ｕ'］を、先行姿勢情報［Ｕ］と、新たに検出されたデバイス動き情報［ｕｔ］とに基づいて算出する手法には、複数の手法がある。以下、
（１）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
（２）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
これらの手法を適用した算出処理例について説明する。
There are a plurality of methods for calculating the latest posture information [U ′] based on the preceding posture information [U] and the newly detected device motion information [u t ]. Less than,
(1) Mean value propagation method (mean propagation)
(2) Gaussian model
An example of calculation processing to which these methods are applied will be described.
（１）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
まず、図１３を参照して平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）による最新のデバイス姿勢情報［Ｕ'］の算出処理について説明する。図１３（ａ）に示すように、先行姿勢情報［Ｕ］から、最新姿勢情報［Ｕ'］を算出する。ここで、算出すべき最新姿勢情報［Ｕ'］と、先行姿勢情報［Ｕ］との差分が、デバイス動き情報［ｕｔ］として得られる情報となる。デバイス動き情報［ｕｔ］は、
ｕｔ＝（δｒｏｔ１，δｒｏｔ２，δｔｒａｎｓ）
として示される。ここで、δｒｏｔ１，δｒｏｔ２，δｔｒａｎｓは、図１３（ａ）に示すように、
δｒｏｔ１は、図１３（ａ）に示すように、先行姿勢［Ｕ］におけるデバイスの方向を示す角度であり、先行姿勢［Ｕ］の位置と、最新姿勢［Ｕ'］の位置とを結ぶ直線からの角度である。
δｒｏｔ２は、図１３（ａ）に示すように、最新姿勢［Ｕ'］におけるデバイスの方向を示す角度であり、先行姿勢［Ｕ］の位置と、最新姿勢［Ｕ'］の位置とを結ぶ直線からの角度である。
δｔｒａｎｓは、図１３（ａ）に示すように、先行姿勢［Ｕ］と、最新姿勢［Ｕ'］との距離に相当する。
(1) Mean value propagation method (mean propagation)
First, the calculation process of the latest device attitude information [U ′] by the mean value propagation method (mean propagation) will be described with reference to FIG. As shown in FIG. 13A, the latest posture information [U ′] is calculated from the preceding posture information [U]. Here, the difference between the latest posture information [U ′] to be calculated and the preceding posture information [U] is information obtained as the device motion information [u t ]. Device motion information [u t ]
u t = (δ rot1 , δ rot2 , δ trans )
As shown. Here, δ rot1 , δ rot2 , and δ trans are as shown in FIG.
As shown in FIG. 13A, δ rot1 is an angle indicating the direction of the device in the preceding posture [U], and is a straight line connecting the position of the preceding posture [U] and the position of the latest posture [U ′]. It is an angle from.
As shown in FIG. 13A, δ rot2 is an angle indicating the direction of the device in the latest posture [U ′], and connects the position of the preceding posture [U] and the position of the latest posture [U ′]. The angle from a straight line.
As shown in FIG. 13A, δ trans corresponds to the distance between the preceding posture [U] and the latest posture [U ′].
先行姿勢情報：Ｕ＝（ｘ，ｙ，θ）と、
デバイス動き情報：ｕｔ＝（δｒｏｔ１，δｒｏｔ２，δｔｒａｎｓ）
とに基づいて、
最新の姿勢情報［Ｕ'］を、以下の式に基づいて算出する。
Device motion information: u t = (δ rot1 , δ rot2 , δ trans )
And based on
The latest posture information [U ′] is calculated based on the following equation.
上記算出式によって、先行姿勢情報［Ｕ］と、デバイス動き情報ｕｔとに基づいて現在の最新のデバイス姿勢情報［Ｕ'］を算出することができる。 The current latest device posture information [U ′] can be calculated based on the preceding posture information [U] and the device motion information u t by the above calculation formula.
（２）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
次に、図１４を参照して、ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）を適用したデバイス姿勢算出処理例について説明する。図１４には、デバイスの移動に基づく３つの姿勢を示している。初期姿勢と、先行姿勢［Ｕ］と、算出すべき最新のデバイス姿勢情報［Ｕ'］である。パス（ｐａｔｈ）は、過去の計測において算出されたデバイスの経路である。
(2) Gaussian model
Next, with reference to FIG. 14, an example of device attitude calculation processing to which a Gaussian model is applied will be described. FIG. 14 shows three postures based on the movement of the device. The initial posture, the preceding posture [U], and the latest device posture information [U ′] to be calculated. The path is a device path calculated in the past measurement.
ここで、デバイス動き情報：ｕｔとして、
Δα＝α−α'
Δｄ＝ｄ−ｄ'
Δβ＝β−β'
を得ることができる。
図から理解されるように、
Δα＝α−α'は、最新姿勢［Ｕ'］と初期姿勢の向きの差分、
Δｄ＝ｄ−ｄ'は、最新姿勢［Ｕ'］と先行姿勢［Ｕ］との移動量差分、
Δβ＝β−β'は、最新姿勢［Ｕ'］と先行姿勢［Ｕ］との向きの差分、
をそれぞれ示す。
Here, the device motion information: as u t,
Δα = α−α ′
Δd = d−d ′
Δβ = β−β ′
Can be obtained.
As can be seen from the figure,
Δα = α−α ′ is the difference between the orientation of the latest posture [U ′] and the initial posture,
Δd = d−d ′ is a movement amount difference between the latest posture [U ′] and the preceding posture [U],
Δβ = β−β ′ is a difference in direction between the latest posture [U ′] and the preceding posture [U],
Respectively.
ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）では、先行姿勢［Ｕ］と、デバイス動き情報：ｕｔとに基づく入力データとして以下のデータを適用する。
上記式において、［Ｕバー］は、先行姿勢［Ｕ］の予測値であり、ΣＵは、先行姿勢［Ｕ］に対応する共分散行列である。同様に、［ｕｔバー］は、デバイス動き情報［ｕｔ］の予測値であり、Σｕｔは、デバイス動き情報［ｕｔ］に対応する共分散行列であり、上記式のように示される。 In the above equation, [U bar] is a predicted value of the preceding posture [U], and Σ U is a covariance matrix corresponding to the preceding posture [U]. Similarly, [u t bar] is a predicted value of the device motion information [u t ], and Σut is a covariance matrix corresponding to the device motion information [u t ], and is expressed as the above equation. .
これらの入力情報に基づいて、最新姿勢［Ｕ'］は以下のように示される。
上記式において、［Ｕ'バー］は、最新姿勢［Ｕ'］の予測値であり、ΣＵは、最新姿勢［Ｕ］に対応する共分散行列であり、［Ｕ'バー］、ΣＵは、以下のように表現される。
上記のガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）を適用した算出処理によって、先行姿勢情報［Ｕ］と、デバイス動き情報ｕｔとに基づいて現在の最新のデバイス姿勢情報［Ｕ'］を算出することができる。 The current latest device posture information [U ′] can be calculated based on the preceding posture information [U] and the device motion information u t by the calculation process using the Gaussian model.
図１０に示すフローのステップＳ１０４において、上述したいずれかの処理の適用により、先行姿勢情報［Ｕ］と、デバイス動き情報ｕｔとに基づいて最新のデバイス姿勢情報［Ｕ'］を算出すると、ステップＳ１０５に進む。 When the latest device posture information [U ′] is calculated based on the preceding posture information [U] and the device motion information u t by applying any of the processes described above in step S104 of the flow illustrated in FIG. Proceed to step S105.
ステップＳ１０５では、センサパッチ生成終了の判定処理を行なう。先に図８を参照して説明したように、本発明の情報処理装置（デバイス）では、複数のセンサ情報を適用したセンサパッチ生成処理を実行しており、１つのセンサパッチ生成に適用するセンサ情報の数が予め定められているものではなく、図７に示すセンサパッチ生成部３０１は、センサパッチ生成を終了するか否かについては、ある基準を適用した判定処理を実行する。 In step S105, a sensor patch generation end determination process is performed. As described above with reference to FIG. 8, the information processing apparatus (device) according to the present invention executes a sensor patch generation process to which a plurality of sensor information is applied, and the sensor is applied to one sensor patch generation. The number of pieces of information is not predetermined, and the sensor patch generation unit 301 illustrated in FIG. 7 executes a determination process that applies a certain criterion as to whether or not to end the sensor patch generation.
センサパッチ生成終了の判定基準としては、例えば、以下の判定基準が適用される。
（ａ）予め設定された時間の経過によるタイムアウト：例えば５秒ごとにセンサパッチ生成終了としてセンサパッチを出力する。
（ｂ）デバイスの移動距離：予め定めた閾値距離以上のデバイス移動が検出された場合にセンサパッチ生成終了としてセンサパッチを出力する。
（ｃ）先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの要素（Ｄｅｔｅｒｍｉｎａｎｔ）の値が閾値を超えたとき、センサパッチ生成終了としてセンサパッチを出力する。（これは、前述のガウシアンモデルの適用によるデバイスの姿勢の算出に適用する共分散行列を参照して実行する。）
（ｄ）先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの対角要素（ｄｉａｇｏｎａｌ ｅｌｅｍｅｎｔｓ）の値が閾値を超えたとき、センサパッチ生成終了としてセンサパッチを出力する。（これも、前述のガウシアンモデルの適用によるデバイスの姿勢の算出に適用する共分散行列を参照して実行する。）
（ｅ）生成した地図におけるセルの占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］という条件設定のなされたセル数が予め定めた閾値を越えた場合にセンサパッチ生成終了としてセンサパッチを出力する。
（ｆ）同定処理部からのセンサパッチ出力要求を受領した場合、センサパッチ生成終了としてセンサパッチを出力する。
（ｇ）上記（ａ）〜（ｆ）のいずれかの１つの条件、またはいずれかの条件の組み合わせが発生したとき、センサパッチ生成終了としてセンサパッチを出力する。
For example, the following determination criteria are applied as determination criteria for the end of sensor patch generation.
(A) Timeout due to elapse of a preset time: For example, a sensor patch is output as sensor patch generation completion every 5 seconds.
(B) Device movement distance: When a device movement equal to or greater than a predetermined threshold distance is detected, a sensor patch is output as the end of sensor patch generation.
(C) When the value of any element (Determinant) of the covariance matrix Σu corresponding to the preceding posture [U] exceeds the threshold, the sensor patch is output as the end of sensor patch generation. (This is performed with reference to the covariance matrix applied to the device attitude calculation by applying the Gaussian model described above.)
(D) When the value of one of the diagonal elements (diagonal elements) of the covariance matrix Σu corresponding to the preceding posture [U] exceeds the threshold value, the sensor patch is output as the end of sensor patch generation. (This is also executed with reference to the covariance matrix applied to the calculation of the device attitude by applying the Gaussian model described above.)
(E) When the number of cells for which the condition of occupying [Occupied] of the cell in the generated map exceeds a predetermined threshold, the sensor patch is output as the end of sensor patch generation.
(F) When a sensor patch output request is received from the identification processing unit, the sensor patch is output as the end of sensor patch generation.
(G) When any one of the above conditions (a) to (f) or any combination of conditions occurs, the sensor patch is output as the end of the sensor patch generation.
図１０に示すフローのステップＳ１０５では、センサパッチ生成部３０１において、上記（ａ）〜（ｇ）いずれかの判定基準を適用してセンサパッチ生成を終了するか否かの判定処理を行なう。 In step S105 of the flow shown in FIG. 10, the sensor patch generation unit 301 performs a determination process as to whether or not to end the sensor patch generation by applying one of the determination criteria (a) to (g).
ステップＳ１０５において、センサパッチ生成終了と判定した場合は、ステップＳ１０６に進み、生成したセンサパッチを同定処理部３０２（図７参照）に出力した後、ステップＳ１０１に戻り、次のセンサパッチ生成に移行する。ステップＳ１０５において、センサパッチ生成が未終了と判定した場合は、ステップＳ１０１に戻り、現在、生成中のセンサパッチの更新を継続して実行する。 If it is determined in step S105 that the sensor patch generation has been completed, the process proceeds to step S106, the generated sensor patch is output to the identification processing unit 302 (see FIG. 7), the process returns to step S101, and the process proceeds to the next sensor patch generation. To do. If it is determined in step S105 that sensor patch generation has not been completed, the process returns to step S101, and the update of the currently generated sensor patch is continued.
このように、図７に示すセンサパッチ生成部３０１は、ステップＳ１０５において、センサパッチ生成終了の判定がなされるまで、センサパッチ生成処理を継続して実行する。すなわち、継続したセンサパッチ生成期間に入力される全てのセンサ情報と、動き情報の全てが考量された１つのセンサパッチが生成されて、生成したセンサパッチと、デバイスの姿勢情報［Ｕ］が、同定処理部３０２に出力されることになる。 As described above, the sensor patch generation unit 301 illustrated in FIG. 7 continuously executes the sensor patch generation process until it is determined in step S105 that the generation of the sensor patch has been completed. That is, all sensor information input during the continuous sensor patch generation period and one sensor patch in which all of the motion information is taken into consideration are generated, and the generated sensor patch and device posture information [U] The data is output to the identification processing unit 302.
次に、センサパッチ生成部３０１からセンサパッチとデバイスの姿勢情報［Ｕ］を入力してデバイスの同定処理を実行する同定処理部３０２の処理の詳細について、図１５以下を参照して説明する。同定処理部３０２の実行する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）は、様々な処理の処理が可能であり、以下、以下に示す複数の処理例について順次、説明する。
同定処理例１．センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
同定処理例２．オーバラップセンサデータを含むセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
同定処理例３．オーバラップセンサデータを含むセンサパッチの変化分の解析処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
Next, details of the processing of the identification processing unit 302 that inputs the sensor patch and the device posture information [U] from the sensor patch generation unit 301 and executes device identification processing will be described with reference to FIG. The identification process (Localization) executed by the identification processing unit 302 can be performed in various processes. Hereinafter, a plurality of process examples shown below will be sequentially described.
Identification processing example 1. Identification processing (Localization) using sensor patches that do not contain overlap sensor data by sensor patch reset processing
Identification processing example 2 Identification processing using sensor patches including overlap sensor data (Localization)
Identification processing example 3 Identification processing (Localization) applying analysis processing of sensor patch change including overlap sensor data
［同定処理例１．センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）］
まず、同定処理例１として、センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）について説明する。図１５に示すフローチャートを参照して、同定処理部３０２の実行する一連の処理のシーケンスについて説明する。図１５に示すフローのステップＳ２０１とステップＳ２０２の処理は、先に図４を参照して説明した例えば１つのフレーム画像単位で同定処理を実行する構成における図４の予測処理部の処理と、更新処理部の処理に対応する。
[Identification Processing Example 1. Identification processing (Localization) applying sensor patches not including overlap sensor data by sensor patch reset processing]
First, as identification processing example 1, an identification process (Localization) to which a sensor patch that does not include overlap sensor data by a sensor patch reset process is applied will be described. A sequence of a series of processes executed by the identification processing unit 302 will be described with reference to the flowchart shown in FIG. The processing of step S201 and step S202 of the flow shown in FIG. 15 is the same as the processing of the prediction processing unit of FIG. 4 in the configuration in which the identification processing is executed in units of, for example, one frame image described above with reference to FIG. This corresponds to the processing of the processing unit.
ただし、図１５に示す処理におけるステップＳ２０１では、センサパッチ生成部３０１から入力する情報が、例えば１つのフレーム画像単位のセンサ情報や動き情報を個別に適用した処理として実行するのではなく、上述したセンサパッチ生成部３０１の処理によって、例えば複数の連続するフレーム画像に基づいて生成されるセンサパッチ［ｐ］と、その期間に対応するデバイスの姿勢情報［Ｕ］に基づいて実行する。 However, in step S201 in the process illustrated in FIG. 15, the information input from the sensor patch generation unit 301 is not executed as a process in which sensor information or motion information in one frame image unit is individually applied, for example, as described above. For example, the sensor patch generation unit 301 executes the processing based on the sensor patch [p] generated based on a plurality of continuous frame images and the device posture information [U] corresponding to the period.
ステップＳ２０１において、図４を参照して説明した処理と同様、例えばベイズ推定を用いて、先行同定情報［Ｂｅｌ（ｘｔ−１）］に対して、デバイスの姿勢情報［Ｕ］を適用して、デバイスの動きを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する。すなわち、図４を参照して説明した例えばフレーム画像単位の動き情報ｕｔの代わりに、センサパッチ生成部の生成したデバイスの姿勢情報［Ｕ］を適用した処理が実行される。 In step S201, the device attitude information [U] is applied to the preceding identification information [Bel ( xt-1 )] using, for example, Bayesian estimation, as in the process described with reference to FIG. Then, a predicted value of the device posture (Pose) including the position and direction of the device in consideration of the movement of the device is calculated. In other words, instead of the motion information u t reference to described for example a frame image unit of FIG. 4, processing using orientation information [U] of the produced devices of the sensor patch generation unit is performed.
ステップＳ２０２では、デバイスの姿勢情報［Ｕ］に、さらに、センサパッチ生成部３０１の処理によって生成されるセンサパッチ［ｐ］を入力して、例えば複数のセンサ情報からなるセンサパッチ［ｐ］を適用して、地図（環境地図）とデバイスの位置および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する。例えば、図９（ｃ）に示すようなデバイスの位置および方向を定めたデバイス周りの地図（環境地図）を生成する。 In step S202, the sensor patch [p] generated by the processing of the sensor patch generation unit 301 is further input to the device posture information [U], and for example, the sensor patch [p] including a plurality of sensor information is applied. Then, a predicted value of the device posture (Pose) including the map (environment map) and the position and direction of the device is calculated. For example, a map (environment map) around the device that defines the position and direction of the device as shown in FIG. 9C is generated.
ステップＳ２０３では、ステップＳ２０１とＳ２０２において実行された同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）が成功したか否かを判定する。同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）が成功したか否かの判定処理は、例えば、予めデバイスが保持する地図データと、生成したセンサパッチ情報との一致度を判定する処理として実行される。 In step S203, it is determined whether the identification process (Localization) executed in steps S201 and S202 has succeeded. The process for determining whether or not the identification process (Localization) has succeeded is executed as a process for determining the degree of coincidence between map data held in advance by the device and the generated sensor patch information, for example.
具体的には、予めデバイスが保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報に含まれるランドマークのマッチ度合いを検証する。あるいは、予めデバイスが保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報に含まれるグリッド単位のセルの占有（Ｏｃｃｕｐｚｎｃｙ）セルの対応のマッチ度合いを検証する処理などによって実行される。マッチ度合いの算出には、例えば、評価関数に基づくスコア算出や、２つの比較対照のデータの共分散解析などが適用可能である。 Specifically, the degree of match between the map data held by the device or the already generated sensor patch information and the landmark included in the newly generated sensor patch information is verified. Alternatively, it is executed by a process of verifying the degree of matching between the map data held in advance by the device or the already generated sensor patch information and the cell unit cell occupancy included in the newly generated sensor patch information. Is done. For example, score calculation based on an evaluation function or covariance analysis of two comparison data can be applied to the calculation of the match degree.
例えば、図１６に示すように予めデバイスが保持する先行生成センサパッチ３４１と、新たに生成した新規生成センサパッチ３４２との照合が行なわれる。具体的なマッチ度合いの評価関数としては、例えば、以下の算出式が適用される。
上記式は、グリッド単位のセルの占有（Ｏｃｃｕｐｚｎｃｙ）セルの対応のマッチ度合いを評価値（ｓｃｏｒｅ）として算出する式である。図１５に示すフローのステップＳ２０３において、例えば上記評価関数を適用し、予め定めた閾値以上の評価値が算出された場合は、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）の成功と判定する。予め定めた閾値以上の評価値が算出されなかった場合は、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）の失敗と判定し、ステップＳ２０１に戻り、処理を繰り返す。 The above equation is an equation for calculating the degree of matching of the cell occupancy (Occupzncy) cell in grid units as an evaluation value (score). In step S203 of the flow shown in FIG. 15, for example, when the evaluation function is applied and an evaluation value equal to or greater than a predetermined threshold is calculated, it is determined that the identification process (Localization) is successful. If an evaluation value equal to or greater than a predetermined threshold value is not calculated, it is determined that the identification process (Localization) has failed, the process returns to Step S201, and the process is repeated.
ステップＳ２０３において、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）成功と判定した場合は、ステップＳ２０４に進み、センサパッチ生成部３０１に対してセンサパッチリセット要求を出力する。センサパッチ生成部３０１は、同定処理部３０２からのセンサパッチリセット要求に応じて、生成済みのセンサパッチをリセットし、次の新たなセンサパッチの生成を開始することになる。 If it is determined in step S203 that the identification process (Localization) is successful, the process proceeds to step S204, and a sensor patch reset request is output to the sensor patch generation unit 301. In response to the sensor patch reset request from the identification processing unit 302, the sensor patch generation unit 301 resets the generated sensor patch and starts generating the next new sensor patch.
同定処理部３０２における同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）の処理例について、図１７を参照して説明する。先に説明したように、センサパッチ生成部３０１は、例えば複数の連続するセンサ情報に基づいて、１つのセンサパッチを生成する。たとえばセンサパッチ［ｐ０］３５０は、センサパッチ生成部３０１において、複数のセンサ情報ｚ０〜ｚ３に基づいて生成される。センサ情報ｚ０は、例えば、時間（ｔ＝０）において撮影されたフレーム画像データである。センサ情報ｚ１は、例えば、時間（ｔ＝１）において撮影されたフレーム画像データである。 A processing example of identification processing (Localization) in the identification processing unit 302 will be described with reference to FIG. As described above, the sensor patch generation unit 301 generates one sensor patch based on, for example, a plurality of continuous sensor information. For example, the sensor patch [p 0 ] 350 is generated by the sensor patch generation unit 301 based on the plurality of sensor information z 0 to z 3 . Sensor information z 0 is, for example, a frame image data captured at time (t = 0). Sensor information z 1 is, for example, a frame image data captured at time (t = 1).
同定処理部３０２は、センサパッチ［ｐ０］３５０を入力して、図１５に示すフローに従った同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。この同定処理が成功したと判定すると、センサパッチ生成部３０１に対してセンサパッチリセット要求を出力する。センサパッチ生成部３０１は、このリセット要求の入力に応じて、次の新たなセンサパッチ［ｐ１］３５１の生成を開始する。これは、複数のセンサ情報ｚ４〜ｚ６に基づいて生成されるセンサパッチである。 The identification processing unit 302 inputs the sensor patch [p 0 ] 350 and executes identification processing (Localization) according to the flow shown in FIG. If it is determined that the identification process is successful, a sensor patch reset request is output to the sensor patch generation unit 301. The sensor patch generation unit 301 starts generating the next new sensor patch [p 1 ] 351 in response to the input of the reset request. This is a sensor patch generated based on a plurality of sensor information z 4 to z 6 .
同定処理部３０２は、センサパッチ［ｐ１］３５１を入力して、図１５に示すフローに従った同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。ここで、この同定処理が失敗したと判定すると、同定処理部３０１は、センサパッチ生成部３０１に対するセンサパッチリセット要求は出力することなく、センサパッチ［ｐ１］３５１を含み、さらに新たなセンサ情報ｚ８〜ｚ１２を含むセンサパッチ［ｐ２］３５２を入力して、図１５に示すフローに従った同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 The identification processing unit 302 receives the sensor patch [p 1 ] 351 and executes identification processing (Localization) according to the flow shown in FIG. If it is determined that the identification process has failed, the identification processing unit 301 does not output a sensor patch reset request to the sensor patch generation unit 301, includes the sensor patch [p 1 ] 351, and further includes new sensor information. The sensor patch [p 2 ] 352 including z 8 to z 12 is input, and identification processing (Localization) according to the flow shown in FIG. 15 is executed.
この同定処理が成功したと判定すると、センサパッチ生成部３０１に対してセンサパッチリセット要求を出力する。センサパッチ生成部３０１は、このリセット要求の入力に応じて、次の新たなセンサパッチ［ｐ３］３５３の生成を開始する。 If it is determined that the identification process is successful, a sensor patch reset request is output to the sensor patch generation unit 301. The sensor patch generation unit 301 starts generating the next new sensor patch [p 3 ] 353 in response to the input of the reset request.
上述した処理例では、センサパッチのリセット処理を行なう構成としたが、このセンサパッチのリセット処理を行なう理由について、図１８を参照して説明する。例えば、同定処理を実行するセンサパッチを、図１８に示すように、
センサパッチ１＝ｆ｛ｚ０｝
センサパッチ２＝｛ｚ０∪ｚ１∪ｚ２｝
として、パッチリセットを行なうことなしに設定したとする。
In the processing example described above, the sensor patch reset process is performed. The reason for performing the sensor patch reset process will be described with reference to FIG. For example, as illustrated in FIG.
Sensor patch 1 = f {z 0 }
Sensor patch 2 = {z 0 ∪z 1 ∪z 2 }
Assuming that the setting is made without performing the patch reset.
このようなリセットなしのセンサパッチ生成を実行すると、センサパッチ１とセンサパッチ２では、同じセンサデータｚ０が重複（オーバラップ）して含まれることになる。すなわち、この場合、同定処理部３０２では、２回の同定処理で、同じセンサデータを適用した処理が実行される。 Running the sensor patch generation of such Reset, the sensor patch 1 and the sensor patch 2, the same sensor data z 0 is to be included in duplicate (overlap). In other words, in this case, the identification processing unit 302 executes a process to which the same sensor data is applied in two identification processes.
この結果、重複して適用されるデータが過大評価された同定処理結果が算出される場合がある。このような誤った結果の算出を防ぐためセンサパッチのリセットを実行する。このリセット処理によって、各センサパッチは、統計的に独立したデータからなるパッチによって構成されることになり、正しい同定処理が実現される。 As a result, an identification processing result may be calculated in which data to be applied redundantly is overestimated. In order to prevent such erroneous calculation of the result, the sensor patch is reset. By this reset process, each sensor patch is constituted by a patch composed of statistically independent data, and a correct identification process is realized.
しかし、一方で、デバイスの移動距離が大きい場合など、パッチの大きさが大きくなりすぎる場合などには、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）が困難となる場合がある。また、新しいパッチを生成するために、複数のフレームデータの適用が必要となる場合がある。また、生成したパッチは、例えばデバイスの経路（パス）のプランニング処理や障害物の回避処理などにも適用可能であり、生成したパッチを保存してパッチリセット処理を行なわない手法が要求される場合もある。以下では、このようなパッチリセットを実行しない処理例について説明する。 However, on the other hand, identification processing (Localization) may be difficult when the size of the patch becomes too large, such as when the movement distance of the device is large. In addition, it may be necessary to apply a plurality of frame data in order to generate a new patch. In addition, the generated patch can be applied to, for example, device route planning processing and obstacle avoidance processing, and a method that does not perform patch reset processing by storing the generated patch is required. There is also. Hereinafter, a processing example in which such a patch reset is not executed will be described.
［同定処理例２．オーバラップセンサデータを含むセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）］
以下、パッチリセットを伴わない同定処理例について説明する。本処理例では、同定処理を実行するセンサパッチを図１９に示すように設定する。すなわち、センサパッチ生成部３０１は、センサパッチ［ｐ０］３８０を、複数のセンサ情報ｚ０〜ｚ３に基づいて生成し、同定処理部３０２は、センサパッチ［ｐ０］３８０を入力して、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。
[Identification Processing Example 2. Identification process using sensor patch including overlap sensor data (Localization)]
Hereinafter, an example of identification processing that does not involve patch reset will be described. In this processing example, the sensor patch for executing the identification processing is set as shown in FIG. That is, the sensor patch generation unit 301 generates a sensor patch [p 0 ] 380 based on a plurality of pieces of sensor information z 0 to z 3 , and the identification processing unit 302 inputs the sensor patch [p 0 ] 380. Then, identification processing (Localization) is executed.
同定処理部３０１は、センサパッチ生成部３０１に対するセンサパッチリセット要求を出力しない。センサパッチ生成部３０１は、センサパッチ［ｐ０］３８０のセンサ情報ｚ０〜ｚ３を含み、さらに、センサ情報ｚ４〜ｚ６を追加した情報を含む新たなセンサパッチ［ｐ１］３８１を生成する。 The identification processing unit 301 does not output a sensor patch reset request to the sensor patch generation unit 301. The sensor patch generation unit 301 includes a new sensor patch [p 1 ] 381 that includes sensor information z 0 to z 3 of the sensor patch [p 0 ] 380 and further includes information obtained by adding the sensor information z 4 to z 6. Generate.
同定処理部３０２は、センサパッチ［ｐ１］３８１を入力して、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。さらに、センサパッチ生成部３０１は、センサパッチ［ｐ１］３８１のセンサ情報ｚ０〜ｚ６を含み、さらに、センサ情報ｚ７〜ｚ１２を追加した情報を含む新たなセンサパッチ［ｐ２］３８２を生成する。 The identification processing unit 302 inputs the sensor patch [p 1 ] 381 and executes identification processing (Localization). Furthermore, the sensor patch generation unit 301 includes the sensor information z 0 to z 6 of the sensor patch [p 1 ] 381 and further includes a new sensor patch [p 2 ] including information obtained by adding the sensor information z 7 to z 12 . 382 is generated.
同定処理部３０２は、センサパッチ［ｐ２］３８２を入力して、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。さらに、センサパッチ生成部３０１は、センサパッチ［ｐ２］３８２のセンサ情報ｚ０〜ｚ１２を含み、さらに、センサ情報ｚ１３〜ｚ１５を追加した情報を含む新たなセンサパッチ［ｐ３］３８３を生成する。同定処理部３０２は、センサパッチ［ｐ３］３８３を入力して、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 The identification processing unit 302 inputs the sensor patch [p 2 ] 382 and executes identification processing (Localization). Further, the sensor patch generation unit 301 includes sensor information z 0 to z 12 of the sensor patch [p 2 ] 382, and further includes a new sensor patch [p 3 ] including information obtained by adding the sensor information z 13 to z 15 . 383 is generated. The identification processing unit 302 inputs the sensor patch [p 3 ] 383 and executes identification processing (Localization).
このようにオーバラップしたセンサ情報を含むセンサパッチに基づく同定処理を実行した場合、前述したように、複数の同定処理において、同じセンサデータを適用した処理が実行され、重複して適用されるデータが過大評価された同定処理結果が算出される場合がある。このような誤った結果の算出を防ぐためには、同定処理の手法を変更することが必要となる。例えば同定処理において適用する処理の態様を変更して重複データの過大評価を防止する処理を行なう。 When identification processing based on sensor patches including overlapping sensor information is executed in this way, as described above, in the plurality of identification processing, processing using the same sensor data is executed, and data that is applied redundantly In some cases, an identification process result that is overestimated is calculated. In order to prevent such erroneous calculation of results, it is necessary to change the method of identification processing. For example, the processing mode applied in the identification processing is changed to perform processing for preventing overestimation of duplicate data.
パッチリセットを行なうことなくセンサデータの重複を許容したセンサパッチに基づく同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）の処理シーケンスについて、図２０に示すフローチャートを参照して説明する。同定処理部３０２は、図２０に示すフローのステップＳ３０１において、センサパッチ生成部３０１からセンサパッチ［ｐ］とデバイスの姿勢情報［Ｕ］を入力し、先行同定情報［Ｂｅｌ（ｘｔ−１）］に対して、デバイスの姿勢情報［Ｕ］を適用して、デバイスの動きを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する。 A processing sequence of identification processing (Localization) based on sensor patches that allow duplication of sensor data without performing patch reset will be described with reference to a flowchart shown in FIG. In step S301 of the flow shown in FIG. 20, the identification processing unit 302 inputs the sensor patch [p] and the device posture information [U] from the sensor patch generation unit 301, and precedes identification information [Bel (x t−1 ). ], The device attitude information [U] is applied to calculate a predicted value of the device attitude (Pose) including the position and direction of the device in consideration of the movement of the device.
さらに、ステップＳ３０２では、デバイスの姿勢情報［Ｕ］に、さらに、センサパッチ生成部３０１の処理によって生成される図１９を参照して説明したオーバラップセンサ情報が含まれるセンサパッチ［ｐ］を適用して、地図（環境地図）とデバイスの位置および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する。例えば、図９（ｃ）に示すようなデバイスの位置および方向を定めたデバイス周りの地図（環境地図）を生成する。 Further, in step S302, the sensor patch [p] including the overlap sensor information described with reference to FIG. 19 generated by the processing of the sensor patch generation unit 301 is further applied to the device attitude information [U]. Then, a predicted value of the device posture (Pose) including the map (environment map) and the position and direction of the device is calculated. For example, a map (environment map) around the device that defines the position and direction of the device as shown in FIG. 9C is generated.
これらの処理においては、前述したように、同定処理において適用する処理の態様を変更して重複データの過大評価を防止する処理を行なうことが必要である。このオバーラップデータに対応する同定処理（Ｌｏｃａｌｉｚａｔｏｉｏｎ）の処理例として、
（１）拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）に従ったローカライズ処理を基本とした同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
（２）モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）に従ったローカライズ処理を基本とした同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
これらの処理例について説明する。
In these processes, as described above, it is necessary to change the mode of the process applied in the identification process to prevent the overestimation of duplicate data. As an example of identification processing (Localization) corresponding to this overlap data,
(1) Identification processing (Localization) based on localization processing according to an extended Kalman filter (EKF)
(2) Identification processing based on localization processing (Localization) according to Monte Carlo Localization (MCL)
These processing examples will be described.
なお、拡張カルマン・フィルタ（ＥＫＦ）処理は、自己位置を状態変数［ｘ，ｙ，θ］の実測値として保持し、環境内に設置されたランドマークを観測すると、ランドマークからの相対位置に基づいて自己位置を推定する。また、自身の動作情報を観測すると、その動作情報に基づいて、状態量の推定を行う同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）手法である。
モンテカルロ同定処理（ＭＣＬ）は、モンテカルロ法により生成した多数の粒子により状態分布を近似する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）手法である。
In the extended Kalman filter (EKF) process, the self-position is held as an actual measurement value of the state variables [x, y, θ], and when a landmark installed in the environment is observed, the relative position from the landmark is obtained. Based on this, the self-position is estimated. In addition, it is an identification process (Localization) method for estimating the state quantity based on the motion information when the motion information of itself is observed.
The Monte Carlo identification process (MCL) is an identification process (Localization) technique that approximates a state distribution with a large number of particles generated by the Monte Carlo method.
（１）拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）をベースとした同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
本処理例において、拡張カルマン・フィルタ（ＥＫＦ）処理を適用する場合、拡張カルマン・フィルタ（ＥＫＦ）処理において、センサ情報（センサパッチ）に対応して算出される共分散行列Σｓをそのまま適用するのではなく、この共分散行列Σｓを、以下のように置きかえる平滑化処理を実行する。すなわち、
Σｓ←ｇ（Σｓ）
とする。
ここで、ｇ（）は、共分散行列Σｓを増加させる関数である。例えば、
ｇ（Σｓ）＝αΣｓ＋Σβ
ただし、α：α≧１のスケーリングファクタ（ｓｃａｌｉｎｇ ｆａｃｔｏｒ）
Σβ：固有値を持つ加算行列
である。
(1) Identification processing (Localization) based on Extended Kalman Filter (EKF: Extended Kalman Filter)
In this processing example, when the extended Kalman filter (EKF) process is applied, the covariance matrix Σs calculated corresponding to the sensor information (sensor patch) is applied as it is in the extended Kalman filter (EKF) process. Instead, a smoothing process is performed in which the covariance matrix Σs is replaced as follows. That is,
Σs ← g (Σs)
And
Here, g () is a function that increases the covariance matrix Σs. For example,
g (Σs) = αΣs + Σ β
Where α: scaling factor of α ≧ 1 (scaling factor)
Σ β : Addition matrix with eigenvalues.
上記手法の適用により、オーバラップするセンサデータを含むセンサパッチを繰り返して適用した同定処理において、重複データの過大評価を防止することが可能となる。 By applying the above method, it is possible to prevent over-evaluation of duplicate data in identification processing in which sensor patches including overlapping sensor data are repeatedly applied.
この拡張カルマン・フィルタ（ＥＫＦ）処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）において、Σｓ←ｇ（Σｓ）とする平滑化処理を実行した場合と、Σｓ←ｇ（Σｓ）とする平滑化処理を実行しなかった場合の同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）におけるデバイスの姿勢の確率分布は、図２１に示すような結果となる。 In the identification process (Localization) to which the extended Kalman filter (EKF) process is applied, a smoothing process of Σs ← g (Σs) is executed, and a smoothing process of Σs ← g (Σs) is executed. The probability distribution of the posture of the device in the identification process (Localization) when there is no result is as shown in FIG.
Σｓ←ｇ（Σｓ）とする平滑化処理を実行しなかった場合は、センサパッチに含まれるオーバラップするセンサ情報の影響により、するどいピークが出現し、正しいローカライズができない状態となっているが、Σｓ←ｇ（Σｓ）とする平滑化処理を実行した場合は、センサパッチに含まれるオーバラップするセンサ情報の影響が現象し、するどいピークが解消して、正しいローカライズが可能となる。 If the smoothing process of Σs ← g (Σs) is not executed, a gradual peak appears due to the influence of overlapping sensor information included in the sensor patch, and correct localization cannot be performed. When the smoothing process of Σs ← g (Σs) is executed, the influence of overlapping sensor information included in the sensor patch occurs, so that the peak is eliminated and correct localization becomes possible.
（２）モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）をベースとした同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
次に、モンテカルロ同定処理（ＭＣＬ）をベースとした同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を適用する場合の処理例について説明する。
(2) Identification processing (Localization) based on Monte Carlo Localization (MCL)
Next, an example of processing in the case of applying identification processing (Localization) based on Monte Carlo identification processing (MCL) will be described.
本処理例では、モンテカルロ同定処理（ＭＣＬ）において、センサ情報（センサパッチ）に基づく確率密度の算出式ｐ（ｚｔ｜ｘｔ）をそのまま適用するのではなく、この確率密度算出式ｐ（ｚｔ｜ｘｔ）を、以下のように置きかえる平滑化処理を実行する。すなわち、
ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））
とする。
ここで、ｇ（ｐ）は、ｐを増加させる関数である。例えば、
ｇ（ｐ）＝ｐα
ただし、α：０＜α≦１の指数要素（ｅｘｐｏｎａｔｉａｌ ｆａｃｔｏｒ）
である。
In this processing example, in the Monte Carlo identification processing (MCL), the probability density calculation formula p (z t | x t ) based on the sensor information (sensor patch) is not applied as it is, but this probability density calculation formula p (z A smoothing process is performed in which t | x t ) is replaced as follows. That is,
p (z t | x t ) ← g (p (z t | x t ))
And
Here, g (p) is a function that increases p. For example,
g (p) = p α
Where α is an exponential factor of 0 <α ≦ 1
It is.
上記手法の適用により、オーバラップするセンサデータを含むセンサパッチを繰り返して適用した同定処理において、重複データの過大評価を防止することが可能となる。 By applying the above method, it is possible to prevent over-evaluation of duplicate data in identification processing in which sensor patches including overlapping sensor data are repeatedly applied.
このモンテカルロ同定処理（ＭＣＬ）において、ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））とする平滑化処理を実行した場合と、ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））とする平滑化処理を実行しなかった場合の同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）におけるデバイスの姿勢の確率分布は、先に説明した図２１に示すような結果となり、ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））とする平滑化処理を実行した場合は、センサパッチに含まれるオーバラップするセンサ情報の影響が現象し、するどいピークが解消して、正しいローカライズが可能となる。 In this Monte Carlo identification process (MCL), a smoothing process of p (z t | x t ) ← g (p (z t | x t )) is executed, and p (z t | x t ) ← g The probability distribution of the posture of the device in the identification process (Localization) when the smoothing process (p (z t | x t )) is not executed is as shown in FIG. When the smoothing process of (z t | x t ) ← g (p (z t | x t )) is executed, the influence of overlapping sensor information included in the sensor patch occurs, and the tremendous peak is eliminated. As a result, correct localization is possible.
［同定処理例３．オーバラップセンサデータを含むセンサパッチの変化分の解析処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）］
次に、オーバラップセンサデータを含むセンサパッチの変化分の解析処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）例について説明する。
[Identification Processing Example 3. Identification processing applying analysis processing of sensor patch change including overlap sensor data (Localization)]
Next, an example of identification processing (Localization) to which analysis processing for changes in sensor patches including overlap sensor data is applied will be described.
本処理例では、先に同定処理例２において説明したように、センサパッチ生成部３０１からは、重複したセンサデータを含むセンサパッチを入力するが、同定処理部３０２において、同定処理の済んだセンサパッチと、新たに同定処理を実行する新たなセンサパッチとの変化分の解析を実行して変化データに基づく同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 In this processing example, as described above in the identification processing example 2, the sensor patch including the duplicated sensor data is input from the sensor patch generation unit 301. However, in the identification processing unit 302, the sensor for which the identification processing has been completed. The analysis of the change between the patch and the new sensor patch that newly executes the identification process is executed, and the identification process (Localization) based on the change data is executed.
図２２を参照して、本処理例における同定処理部における同定処理について説明する。図７に示すセンサパッチ生成部３０１は、センサパッチ［ｐ０］３９０を、複数のセンサ情報ｚ０〜ｚ３に基づいて生成し、同定処理部３０２は、センサパッチ［ｐ０］３９０を入力し、既に同定処理が終了した先行するセンサパッチを先行パッチとして記憶部に保持しており、入力したセンサパッチ［ｐ０］３９０と、記憶部に格納されている先行パッチとの差分を算出する。ただし、初期的には、先行パッチは存在させず、図に示す空のｅｍｐｔｙパッチ４０１との差分、すなわち、入力したセンサパッチ［ｐ０］３９０に基づく同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 With reference to FIG. 22, the identification process in the identification process part in this process example is demonstrated. The sensor patch generation unit 301 illustrated in FIG. 7 generates a sensor patch [p 0 ] 390 based on a plurality of pieces of sensor information z 0 to z 3 , and the identification processing unit 302 inputs the sensor patch [p 0 ] 390. The preceding sensor patch whose identification processing has already been completed is held in the storage unit as the preceding patch, and the difference between the input sensor patch [p 0 ] 390 and the preceding patch stored in the storage unit is calculated. . However, initially, there is no preceding patch, and an identification process (Localization) based on the difference from the empty empty patch 401 shown in the drawing, that is, the input sensor patch [p 0 ] 390 is executed.
同定処理部３０１は、センサパッチ生成部３０１に対するセンサパッチリセット要求は出力しない。センサパッチ生成部３０１は、センサパッチ［ｐ０］３９０のセンサ情報ｚ０〜ｚ３を含み、さらに、センサ情報ｚ４〜ｚ６を追加した情報を含む新たなセンサパッチ［ｐ１］３９１を生成する。 The identification processing unit 301 does not output a sensor patch reset request to the sensor patch generation unit 301. The sensor patch generation unit 301 includes a new sensor patch [p 1 ] 391 that includes sensor information z 0 to z 3 of the sensor patch [p 0 ] 390 and further includes information obtained by adding the sensor information z 4 to z 6. Generate.
同定処理部３０２は、センサパッチ［ｐ１］３９１を入力して、記憶部に格納されている先行パッチ、この場合は、処理済みのセンサパッチ［ｐ０］４０２との差分を算出する。算出した差分、すなわち、入力したセンサパッチ［ｐ１］３９１と、記憶部に格納された処理済みのセンサパッチ［ｐ０］４０２との差分情報に基づく同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。しかし、この同定処理に失敗したとする。 The identification processing unit 302 receives the sensor patch [p 1 ] 391 and calculates a difference from the preceding patch stored in the storage unit, in this case, the processed sensor patch [p 0 ] 402. An identification process (Localization) based on difference information between the calculated difference, that is, the input sensor patch [p 1 ] 391 and the processed sensor patch [p 0 ] 402 stored in the storage unit is executed. However, it is assumed that this identification process has failed.
同定処理に失敗した場合は、その処理対象としたセンサパッチ、すなわち、センサパッチ［ｐ１］３９１の記憶部への保存処理は実行されない。同定処理部３０２は、次の新たなセンサパッチ［ｐ２］３９２を入力して、記憶部に格納されている先行パッチ、この場合は、処理済みのセンサパッチ［ｐ０］４０２との差分を算出する。算出した差分、すなわち、入力したセンサパッチ［ｐ２］３９２と、記憶部に格納された処理済みのセンサパッチ［ｐ０］４０２との差分情報に基づく同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 If the identification process fails, the storage process of the sensor patch as the process target, that is, the sensor patch [p 1 ] 391 is not executed. The identification processing unit 302 inputs the next new sensor patch [p 2 ] 392 and calculates a difference from the preceding patch stored in the storage unit, in this case, the processed sensor patch [p 0 ] 402. calculate. An identification process (Localization) based on difference information between the calculated difference, that is, the input sensor patch [p 2 ] 392 and the processed sensor patch [p 0 ] 402 stored in the storage unit is executed.
この同定処理に成功した場合、記憶部に処理済みのセンサパッチ［ｐ２］４０３として記憶する。以下、同様に、同定処理部３０２は、センサパッチ［ｐ３］３９３を入力して、記憶部に格納されている先行パッチ［ｐ２］４０３との差分を算出し、差分情報に基づく同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 If the identification process is successful, the processed sensor patch [p 2 ] 403 is stored in the storage unit. Hereinafter, similarly, the identification processing unit 302 receives the sensor patch [p 3 ] 393, calculates a difference from the preceding patch [p 2 ] 403 stored in the storage unit, and performs an identification process based on the difference information. (Localization) is executed.
本処理例では、このように同定処理部３０２において、オーバラップするセンサ情報を含むセンサパッチをセンサパッチ生成部３０１から入力するが、記憶部に同定処理の済んだセンサパッチを格納して保持する構成として、新たな入力パッチとの差分を算出して同定処理を実行する構成としたので、先の処理例２で説明した特別な平滑化処理を行う必要がなく、通常のローカライズ手法を適用して、正しい同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行することが可能となる。 In this processing example, in the identification processing unit 302, sensor patches including overlapping sensor information are input from the sensor patch generation unit 301 as described above, but the sensor patches that have undergone the identification processing are stored and retained in the storage unit. Since the configuration is such that the difference is calculated from the new input patch and the identification processing is executed, it is not necessary to perform the special smoothing processing described in the previous processing example 2, and a normal localization method is applied. Thus, correct identification processing (Localization) can be performed.
本処理例の処理シーケンスについて、図２３に示すフローチャートを参照して説明する。ステップＳ４０１において、図４を参照して説明した処理と同様、例えばベイズ推定を用いて、先行同定情報［Ｂｅｌ（ｘｔ−１）］に対して、デバイスの姿勢情報［Ｕ］を適用して、デバイスの動きを加味したデバイスの位置、および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する。すなわち、図４を参照して説明した例えばフレーム画像単位の動き情報ｕｔの代わりに、センサパッチ生成部の生成したデバイスの姿勢情報［Ｕ］を適用した処理が実行される。 The processing sequence of this processing example will be described with reference to the flowchart shown in FIG. In step S401, the device attitude information [U] is applied to the preceding identification information [Bel ( xt-1 )] using, for example, Bayesian estimation, as in the process described with reference to FIG. Then, a predicted value of the device posture (Pose) including the position and direction of the device in consideration of the movement of the device is calculated. That is, instead of the motion information u t reference to described for example units of frame images to FIG. 4, the processing of applying the orientation information [U] of the produced devices of the sensor patch generation unit is performed.
ステップＳ４０２では、デバイスの姿勢情報［Ｕ］と、センサパッチ生成部３０１の処理によって新たに生成されるセンサパッチ［ｐ］と、記憶部に格納された同定処理の実行された処理済みセンサパッチ［ｐ'］との変化分データを解析して、地図（環境地図）とデバイスの位置および方向を含むデバイス姿勢（Ｐｏｓｅ）の予測値を算出する同定処理を実行する。例えば、図９（ｃ）に示すようなデバイスの位置および方向を定めたデバイス周りの地図（環境地図）を生成する。この同定処理の具体例については後述する。 In step S402, the device attitude information [U], the sensor patch [p] newly generated by the processing of the sensor patch generation unit 301, and the processed sensor patch [ p ′] is analyzed, and an identification process for calculating a predicted value of a device posture (Pose) including the map (environment map) and the position and direction of the device is executed. For example, a map (environment map) around the device that defines the position and direction of the device as shown in FIG. 9C is generated. A specific example of this identification process will be described later.
ステップＳ４０３では、ステップＳ４０１とＳ４０２において実行された同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）が成功したか否かを判定する。同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）が成功したか否かの判定処理は、例えば、予めデバイスが保持する地図データと、生成したセンサパッチ情報との一致度を判定する処理として実行される。 In step S403, it is determined whether or not the identification processing (Localization) executed in steps S401 and S402 has been successful. The process for determining whether or not the identification process (Localization) has succeeded is executed as a process for determining the degree of coincidence between map data held in advance by the device and the generated sensor patch information, for example.
具体的には、先に、図１６を参照して説明したように、予めデバイスが保持する地図データと、生成したセンサパッチ情報に含まれるグリッド単位のセルの占有（Ｏｃｃｕｐｚｎｃｙ）セルの対応のマッチ度合いを検証する処理や、予めデバイスが保持する地図データと、生成したセンサパッチ情報に含まれるランドマークのマッチ度合いを検証する処理などによって実行される。 Specifically, as described above with reference to FIG. 16, the correspondence between the map data held in advance by the device and the cell unit cell (Occupzncy) cell included in the generated sensor patch information It is executed by processing for verifying the degree, processing for verifying the degree of match between the map data held in advance by the device and the landmark included in the generated sensor patch information, and the like.
ステップＳ４０３において、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）の失敗と判定された場合は、ステップＳ４０１に戻り、処理を繰り返す。この場合、入力したパッチの記憶部への保存は行なわれない。 If it is determined in step S403 that the identification process (Localization) has failed, the process returns to step S401 and the process is repeated. In this case, the input patch is not stored in the storage unit.
ステップＳ４０３において、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）成功と判定した場合は、ステップＳ４０４に進み、同定処理の済んだ、入力センサパッチを記憶部へ保存する。 If it is determined in step S403 that the identification process (Localization) is successful, the process proceeds to step S404, and the input sensor patch that has been subjected to the identification process is stored in the storage unit.
ステップＳ４０２において実行するセンサパッチ生成部３０１の処理によって新たに生成されるセンサパッチ［ｐ］と、記憶部に格納された同定処理の実行された処理済みセンサパッチ［ｐ'］との変化分データに基づく同定処理の具体的な市より劣位について、以下説明する。この変化分データに基づく同定処理の処理態様としては、例えば、以下の処理が適用可能である。
（Ａ）２つのパッチの差分情報［Ｄｉｆｆ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＤｉｆｆ（ｐ，ｐ'）］
（Ｂ）２つのパッチの更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）］
以下、これら（Ａ），（Ｂ）の具体的な処理例について説明する。
Change data of the sensor patch [p] newly generated by the process of the sensor patch generation unit 301 executed in step S402 and the processed sensor patch [p ′] subjected to the identification process stored in the storage unit The inferiority of the specific city based on the identification process will be described below. As a processing mode of the identification processing based on the change data, for example, the following processing can be applied.
(A) Update process [UpdateDiff (p, p ′)] using difference information [Diff (p, p ′)] between two patches
(B) Update process [UpdateRatio (p, p ′)] based on update rate information [Ratio (p, p ′)] of two patches
Hereinafter, specific processing examples of (A) and (B) will be described.
（Ａ）２つのパッチの差分情報［Ｄｉｆｆ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＤｉｆｆ（ｐ，ｐ'）］
まず、センサパッチ生成部３０１の処理によって新たに生成されるセンサパッチ［ｐ］と、記憶部に格納された同定処理の実行された処理済みセンサパッチ［ｐ'］との差分情報［Ｄｉｆｆ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＤｉｆｆ（ｐ，ｐ'）例について説明する。
(A) Update process [UpdateDiff (p, p ′)] using difference information [Diff (p, p ′)] between two patches
First, the difference information [Diff (p) between the sensor patch [p] newly generated by the processing of the sensor patch generation unit 301 and the processed sensor patch [p ′] subjected to the identification processing stored in the storage unit. , P ′)], an example of update processing [UpdateDiff (p, p ′) will be described.
この処理は、以下の処理を伴う処理として実行する。
（ａ）新規入力センサパッチ［ｐ］に含まれるセンサ情報中、処理済みセンサパッチ［ｐ'］にも記録されたセンサ情報を削除する。
（ｂ）各センサパッチに設定されたタイムスタンプ情報を比較した差分抽出を実行する。
This process is executed as a process involving the following processes.
(A) The sensor information recorded in the processed sensor patch [p ′] is deleted from the sensor information included in the new input sensor patch [p].
(B) The difference extraction which compares the time stamp information set to each sensor patch is executed.
本処理例における同定処理について、図２４に示すフローチャートを参照して説明する。図２４に示すフローは、センサパッチのグリッド単位として設定されるセルに対するオキュパイド（Ｏｃｃｕｐｉｅｄ）処理を適用した処理例である。前述したように、グリッドは、セルＣに対して障害物の存在確率（障害物によって占有されている確率）＝占有確率：ｐ（ｃ）を保持しており、このエンプティ処理及びオキュパイド処理は、これら各セルに対する統計的な処理として実行される。 The identification processing in this processing example will be described with reference to the flowchart shown in FIG. The flow shown in FIG. 24 is a processing example in which an occuped process is applied to a cell set as a sensor patch grid unit. As described above, the grid holds an obstacle existence probability (probability occupied by an obstacle) = occupancy probability: p (c) for the cell C. The empty processing and the occupy processing are performed as follows: This is executed as a statistical process for each of these cells.
ステップＳ５０１において、新規入力センサパッチ［ｐ］と、処理済みセンサパッチ［ｐ'］とのセル対応付けを実行する。ステップＳ５０２において、新規入力センサパッチ［ｐ］と、処理済みセンサパッチ［ｐ'］との対応するセルにおいて算出された占有確率：ｐ（ｃ）およびｐ'（ｃ）の各データに対応付けられたタイムスタンプ情報を抽出し、ステップＳ５０３において、新規入力センサパッチ［ｐ］のセル対応のタイムスタンプが、処理済みセンサパッチ［ｐ'］の対応セルのタイムスタンプより新しいか否かを判定する。 In step S501, cell association between the new input sensor patch [p] and the processed sensor patch [p ′] is executed. In step S502, the occupancy probabilities calculated in the cells corresponding to the new input sensor patch [p] and the processed sensor patch [p ′] are associated with the respective data of p (c) and p ′ (c). In step S503, it is determined whether the time stamp corresponding to the cell of the new input sensor patch [p] is newer than the time stamp of the corresponding cell of the processed sensor patch [p ′].
ステップＳ５０３において、新規入力センサパッチ［ｐ］のセル対応のタイムスタンプが、処理済みセンサパッチ［ｐ'］の対応セルのタイムスタンプより新しいと判定した場合は、ステップＳ５０４に進み、新規入力センサパッチ［ｐ］のセルに対応する占有確率：ｐ（ｃ）をセンサパッチ更新、すなわち同定処理に適用する占有確率を示すデータｑ（ｃ）として設定する。 If it is determined in step S503 that the time stamp corresponding to the cell of the new input sensor patch [p] is newer than the time stamp of the corresponding cell of the processed sensor patch [p ′], the process proceeds to step S504, where the new input sensor patch The occupation probability corresponding to the cell of [p]: p (c) is set as data q (c) indicating the occupation probability applied to the sensor patch update, that is, the identification process.
一方、ステップＳ５０３において、新規入力センサパッチ［ｐ］のセル対応のタイムスタンプが、処理済みセンサパッチ［ｐ'］の対応セルのタイムスタンプより新しくないと判定した場合は、ステップＳ５０５に進み、そのセルに対応する占有確率を不明（ｕｎｋｎｏｗｎ）、すなわちｑ（ｃ）＝ｕｎｋｎｏｗｎとして設定する。 On the other hand, if it is determined in step S503 that the time stamp corresponding to the cell of the new input sensor patch [p] is not newer than the time stamp of the corresponding cell of the processed sensor patch [p ′], the process proceeds to step S505. The occupation probability corresponding to the cell is set as unknown (unknown), that is, q (c) = unknown.
ステップＳ５０４またはステップＳ５０５の処理の後、ステップＳ５０６に進み、未処理のセルがある場合は、ステップＳ５０１以下の処理を未処理セルに対して繰り返す。全てのセルについての処理が済むと、ステップＳ５０７に進み、新たに生成された占有確率：ｑ（ｃ）を持つセンサパッチを出力し、このセンサパッチに基づく更新処理、すなわち同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する。 After the process of step S504 or step S505, the process proceeds to step S506. If there is an unprocessed cell, the process from step S501 is repeated on the unprocessed cell. When the processing for all the cells is completed, the process proceeds to step S507, where a newly generated sensor patch having an occupation probability: q (c) is output, and update processing based on this sensor patch, that is, identification processing (Localization) is performed. Execute.
なお、上記処理において、ステップＳ５０５において実行するセルに対応する占有確率を不明（ｕｎｋｎｏｗｎ）とする処理、すなわちｑ（ｃ）＝ｕｎｋｎｏｗｎとする処理は、例えば、０〜１の範囲で占有確率を設定する場合、中間値としての［０．５］を設定するといった処理として実行してもよい。 In the above process, the process of setting the occupation probability corresponding to the cell to be executed in step S505 to be unknown (ie, the process of setting q (c) = unknown) sets the occupation probability in the range of 0 to 1, for example. In this case, it may be executed as a process of setting [0.5] as an intermediate value.
このようにして２つのセンサパッチの差分情報に基づいて生成した更新されたセンサパッチを適用した同定処理によって、重複データが排除された正確な同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を行なうことが可能となる。 Thus, it is possible to perform an accurate identification process (Localization) in which duplicate data is eliminated by the identification process using the updated sensor patch generated based on the difference information between the two sensor patches.
（Ｂ）２つのパッチの更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）］
次に、センサパッチ生成部３０１の処理によって新たに生成されるセンサパッチ［ｐ］と、記憶部に格納された同定処理の実行された処理済みセンサパッチ［ｐ'］との更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）例について説明する。この処理は、ベイズ推定を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）や、モンテカルロ同定処理（ＭＣＬ）において利用しやすい処理である。
(B) Update process [UpdateRatio (p, p ′)] based on update rate information [Ratio (p, p ′)] of two patches
Next, the update rate information [Ratio] between the sensor patch [p] newly generated by the processing of the sensor patch generation unit 301 and the processed sensor patch [p ′] subjected to the identification processing stored in the storage unit An example of the update process [UpdateRatio (p, p ′)] by (p, p ′)] will be described. This processing is easy to use in identification processing (Localization) applying Bayesian estimation or Monte Carlo identification processing (MCL).
まず、ベイズ推定を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）において、２つのパッチとして、
新規入力センサパッチ＝センサ情報ｚ１〜ｚｔを含むセンサパッチ［ｐ］、
処理済みセンサパッチ［ｐ'］＝センサ情報ｚ１〜ｚｔ−１を含むパッチ［ｐ'］
とした場合の、更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）例について説明する。ベイズ推定については、先に、図４を参照した同定処理の説明において式を参照して説明した。
First, in the identification process (Localization) using Bayesian estimation, as two patches,
New input sensor patch = sensor patch [p] including sensor information z1 to zt,
Processed sensor patch [p ′] = Patch [p ′] including sensor information z1 to zt−1
An example of update processing [UpdateRatio (p, p ′) using update rate information [Ratio (p, p ′)] will be described. Bayesian estimation was previously described with reference to equations in the description of the identification process with reference to FIG.
ベイズ推定による姿勢ｘｔの確率は、以下の式によって示すことができる。
Ｂｅｌ（ｘｔ）＝Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ）
上記式において、
ｚは、センサ情報
ｕは、動き情報
ｘは、デバイスの状態、すなわち姿勢（ｐｏｓｅ）を示すパラメータ
Ｐは確率密度関数を表している。
The probability of orientation x t Bayesian estimation can be illustrated by the following equation.
Bel (x t ) = P (x t | u 1: t , z 1: t )
In the above formula,
z is sensor information u is motion information x is a device state, that is, a parameter P indicating a posture P is a probability density function.
さらに、先に説明したように、
Ｂｅｌ（ｘｔ）＝Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ）＝Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ−１，ｚｔ）
＝ηＰ（ｚｔ｜ｘｔ，ｕ１：ｔ，ｚ１：ｔ−１）Ｐ（ｘｔ｜ｕ１：ｔ，ｚ１：ｔ−１）
また、Ｐ（ｚｔ｜ｘｔ，ｕ１：ｔ，ｚ１：ｔ−１）は、以下のように示すことができる。
Bel (x t) = P ( x t | u 1: t, z 1: t) = P (x t | u 1: t, z 1: t-1, z t)
= ΗP (z t | x t , u 1: t , z 1: t−1 ) P (x t | u 1: t , z 1: t−1 )
P (z t | x t , u 1: t , z 1: t−1 ) can be expressed as follows.
ここで、
新規入力センサパッチ＝センサ情報ｚ１〜ｚｔを含むセンサパッチ［ｐ］、
処理済みセンサパッチ［ｐ'］＝センサ情報ｚ１〜ｚｔ−１を含むパッチ［ｐ'］
である。従って、以下の関係式が成立する。
New input sensor patch = sensor patch [p] including sensor information z1 to zt,
Processed sensor patch [p ′] = Patch [p ′] including sensor information z1 to zt−1
It is. Therefore, the following relational expression is established.
上記式は、先に、図４を参照して説明した関係式、
におけるＰ（ｚｔ｜ｘｔ）を２つのセンサパッチの比率（Ｐ（ｚ１：ｔ｜ｘｔ））／（Ｐ（ｚ１：ｔ−１｜ｘｔ））に置きかえた式に相当し、更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）］がベイズ推定によって実行可能であることが証明される。 P in (z t | x t) the ratio of the two sensor patch (P (z 1: t | x t)) / (P (z 1: t-1 | x t)) corresponds to the replaced expression It is proved that the update process [UpdateRatio (p, p ′)] based on the update rate information [Ratio (p, p ′)] can be executed by Bayesian estimation.
次に、モンテカルロ同定処理（ＭＣＬ）における更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］を適用した更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）］例について説明する。 Next, an example of an update process [UpdateRatio (p, p ′)] to which update rate information [Ratio (p, p ′)] in the Monte Carlo identification process (MCL) is applied will be described.
モンテカルロ同定処理（ＭＣＬ）は、モンテカルロ法により生成した多数の粒子により状態分布を近似する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）手法である。生成する地図上に設定された多数の粒子により状態分布を近似する。このモンテカルロ同定処理（ＭＣＬ）処理において、新規入力センサパッチ［ｐ］と、記憶部に格納された同定処理の実行された処理済みセンサパッチ［ｐ'］との更新率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＲａｔｉｏ（ｐ，ｐ'）例について図２５に示すフローチャートを参照して説明する。 The Monte Carlo identification process (MCL) is an identification process (Localization) technique that approximates a state distribution with a large number of particles generated by the Monte Carlo method. The state distribution is approximated by a large number of particles set on the generated map. In this Monte Carlo identification processing (MCL) processing, update rate information [Ratio (p, p) between the new input sensor patch [p] and the processed sensor patch [p ′] that has been subjected to the identification processing stored in the storage unit. An example of update processing [UpdateRatio (p, p ′)] by “)” will be described with reference to the flowchart shown in FIG.
まず、ステップＳ６０１において、入力として、地図上に設定された多数の粒子の中の１つｘｉを設定し、そのｘｉに対応する２つの状態値としてのウェイト、すなわち、
新規入力センサパッチ［ｐ］に基づく状態値としてのウェイト：ｗ１＝ｐｒｏｂ（Ｐ｜ｘｉ）
処理済みセンサパッチ［ｐ'］に基づく状態値としてのウェイト：ｗ２＝ｐｒｏｂ（ｐ'｜ｘｉ）
を算出する。
First, in step S601, as input, one x i among many particles set on the map is set, and weights as two state values corresponding to the x i , that is,
Weight as state value based on new input sensor patch [p]: w 1 = prob (P | x i )
Weight as state value based on processed sensor patch [p ′]: w 2 = prob (p ′ | x i )
Is calculated.
次に、ステップＳ６０２において、ステップＳ６０１で算出した各センサパッチ対応の状態値としてのウェイト［ｗ１］、［ｗ２］の比率
ｗｉ＝ｗ１／ｗ２
を算出し、これをモンテカルロ同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）に適用する状態値としてのウェイトとする。
Next, in step S602, the ratio w i = w 1 / w 2 of the weights [w 1 ] and [w 2 ] as the state values corresponding to each sensor patch calculated in step S601.
Is used as a weight as a state value to be applied to the Monte Carlo identification process (Localization).
このようにして２つのセンサパッチの状態値の比率情報に基づいて生成した更新されたセンサパッチを適用した同定処理によって、重複データが排除された効率的な同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を行なうことが可能となる。 In this way, it is possible to perform an efficient identification process (Localization) in which duplicate data is eliminated by the identification process using the updated sensor patch generated based on the ratio information of the state values of the two sensor patches. It becomes.
以上、同定処理部３０２の実行する同定処理（Ｌｏｃｚｌｉｚａｔｉｏｎ）として、
同定処理例１．センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図１５〜図１７）
同定処理例２．オーバラップセンサデータを含むセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図１９〜図２０）
同定処理例３．オーバラップセンサデータを含むセンサパッチの変化分の解析処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）として、
（３−１）２つのパッチの差分情報［Ｄｉｆｆ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＤｉｆｆ（ｐ，ｐ'）］を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図２２〜図２４）
（３−２）２つのパッチの比率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＴａｔｉｏ（ｐ，ｐ'）］を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図２２、図２３、図２５）
As described above, as the identification processing (Loczization) performed by the identification processing unit 302,
Identification processing example 1. Identification processing (Localization) by applying sensor patch not including overlap sensor data by sensor patch reset processing (FIGS. 15 to 17)
Identification processing example 2 Identification process (Localization) using sensor patches including overlap sensor data (FIGS. 19 to 20)
Identification processing example 3 As identification processing (Localization) applying analysis processing of sensor patch change including overlap sensor data,
(3-1) Identification Processing (Localization) Applying Update Processing [UpdateDiff (p, p ′)] Based on Difference Information [Diff (p, p ′)] of Two Patches (FIGS. 22 to 24)
(3-2) Identification Process (Localization) Applying Update Process [UpdateTatio (p, p ′)] Based on Ratio Information [Ratio (p, p ′)] of Two Patches (FIGS. 22, 23, and 25)
これら処理例１，２，３−１，３−２について説明してきた。
上述の同定処理において、センサパッチ生成部３０１の生成するセンサパッチや、同定処理部３０２における同定処理において利用するセンサパッチや地図データは、センサとしてのカメラの撮影する画像情報の他、センサ情報を加工して得られる様々なデータが適用可能である。たとえば、
目印となるランドマーク情報のみからなるデータ、
オブジェクトの輪郭などの線分情報のみからなるデータ、
グリッド単位のセルの状態値（占有など）によって表現されるデータ、
などを適用することが可能である。
These processing examples 1, 2, 3-1, and 3-2 have been described.
In the above-described identification processing, the sensor patch generated by the sensor patch generation unit 301, the sensor patch used in the identification processing in the identification processing unit 302, and map data include sensor information in addition to image information captured by the camera as a sensor. Various data obtained by processing can be applied. For example,
Data consisting only of landmark information as a landmark,
Data consisting only of line information such as the outline of the object,
Data represented by cell state values (such as occupancy) in grid units,
Etc. can be applied.
目印となるランドマーク情報のみからなるセンサパッチのデータの例について、図２６を参照して説明する。図２６には、デバイス４２１が動き回る領域（Ｅｎｖｉｒｏｎｍｅｎｔ）４２０を示している。領域（Ｅｎｖｉｒｏｎｍｅｎｔ）４２０には、２つの同じ形を持つランドマークＡ（１），Ａ（２）と、Ａとは区別可能な１つのラントマークＢが存在する。 An example of sensor patch data including only landmark information serving as landmarks will be described with reference to FIG. FIG. 26 shows an area 420 in which the device 421 moves around. In the area 420, there are two landmarks A (1) and A (2) having the same shape, and one runtmark B that can be distinguished from A.
デハイス４２１は、センサ（カメラ）によるセンサ情報としてランドマーク情報のみからなるデータを取得する。例えば時間Ｔ＝０において、ランドマークＡからなるセンサ情報を取得し、次に時間Ｔ＝１において、ランドマークＢからなるセンサ情報を取得したとする。 The de-high speed 421 acquires data consisting only of landmark information as sensor information by a sensor (camera). For example, it is assumed that sensor information consisting of the landmark A is acquired at time T = 0, and then sensor information consisting of the landmark B is acquired at time T = 1.
この２つのセンサ情報に基づいて１つのセンサパッチ４２３が生成される。パッチ４２３には、２つのランドマークＡ，Ｂが、２つのランドマークの距離情報を含むデータとして設定されることになる。結果として、このセンサパッチに基づく同定処理を実行することで、センサパッチ４２３は、デバイス４２１が動き回る領域（Ｅｎｖｉｒｏｎｍｅｎｔ）４２０における矩形領域４２５に対応することが解析可能となる。 One sensor patch 423 is generated based on the two sensor information. In the patch 423, two landmarks A and B are set as data including distance information of the two landmarks. As a result, by performing identification processing based on this sensor patch, it becomes possible to analyze that the sensor patch 423 corresponds to the rectangular region 425 in the region 420 where the device 421 moves around.
１つのセンサ情報のみから同定処理を実行した場合には、デバイス４２１がジカンＴ＝０において取得したランドマークＡが、ランドマークＡ（１）なのか、ランドマークＡ（２）なのかの判別が不可能になるが、本処理例のように複数のセンサ情報を組み合わせて利用した同定処理によってＡとＢの位置関係が明確になり、時間Ｔ＝０において取得したランドマークＡが、ランドマークＡ（２）であることが判別され、正確な同定処理が可能となる。 When the identification process is executed from only one sensor information, it is determined whether the landmark A acquired by the device 421 at Zicant T = 0 is the landmark A (1) or the landmark A (2). Although it becomes impossible, the positional relationship between A and B is clarified by the identification processing using a combination of a plurality of sensor information as in this processing example, and the landmark A acquired at time T = 0 is the landmark A. (2) is discriminated, and accurate identification processing becomes possible.
従来処理例として説明した個別のセンサ情報と動き情報を逐次処理する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）と、上述した本発明に従った複数の処理例における同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）において適用する情報量と、処理時間の対応表を図２７に示す。 Information processing and processing time applied in identification processing (Localization) for sequentially processing individual sensor information and motion information described as a conventional processing example, and identification processing (Localization) in a plurality of processing examples according to the present invention described above The correspondence table is shown in FIG.
図２７には、各処理手法（Ｍｅｔｈｏｄ）に対応するデータとして、
（ａ）動き情報数（Ｄａｔａ／＃ ｍｏｔｉｏｎ）
（ｂ）センサ情報（Ｄａｔａ／＃ ｓｅｎｓｅ）
（ｃ）動き解析時間（Ｔｉｍｅ／ｓ ｍｏｔｉｏｎ）
（ｄ）センサ情報解析時間（Ｔｉｍｅ／ｓ ｓｅｎｓｅ）
（ｃ）全解析時間（Ｔｉｍｅ／ｓ ｔｏｔａｌ）
の各値について、
In FIG. 27, as data corresponding to each processing method (Method),
(A) Number of motion information (Data / # motion)
(B) Sensor information (Data / # sense)
(C) Motion analysis time (Time / s motion)
(D) Sensor information analysis time (Time / s sense)
(C) Total analysis time (Time / s total)
For each value of
（１）従来処理例［ｒａｗ（ｎｏ ｐａｔｃｈ］：個別のセンサ情報と動き情報を逐次処理する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
（２）同定処理例１［ｒｅｓｅｔ］：センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図１５〜図１７）
（３）同定処理例２［ｆｕｌｌ（ｎｏ ｒｅｓｅｔ）］：オーバラップセンサデータを含むセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図１９〜図２０）
(1) Conventional processing example [raw (no patch): Identification processing (Localization) for sequentially processing individual sensor information and motion information
(2) Identification processing example 1 [reset]: Identification processing (Localization) using sensor patches not including overlap sensor data by sensor patch reset processing (FIGS. 15 to 17)
(3) Identification processing example 2 [full (no reset)]: Identification processing (Localization) to which a sensor patch including overlap sensor data is applied (FIGS. 19 to 20)
さらに、同定処理例３．オーバラップセンサデータを含むセンサパッチの変化分の解析処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）として、
（４）［Ｄｉｆｆ］：２つのパッチの差分情報［Ｄｉｆｆ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＤｉｆｆ（ｐ，ｐ'）］を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図２２〜図２４）
（５）［Ｒａｔｉｏ］：２つのパッチの比率情報［Ｒａｔｉｏ（ｐ，ｐ'）］による更新処理［ＵｐｄａｔｅＴａｔｉｏ（ｐ，ｐ'）］を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）（図２２、図２３、図２５）
Furthermore, identification processing example 3. As identification processing (Localization) applying analysis processing of sensor patch change including overlap sensor data,
(4) [Diff]: Identification process (Localization) applying update process [UpdateDiff (p, p ′)] based on difference information [Diff (p, p ′)] of two patches (FIGS. 22 to 24)
(5) [Ratio]: Identification processing (Localization) applying update processing [UpdateTatio (p, p ′)] based on the ratio information [Ratio (p, p ′)] of two patches (FIG. 22, FIG. 23, FIG. 23). 25)
これら各処理例（１）〜（５）についての処理データ量と処理時間を示している。（２）〜（５）に示す本発明に従った処理例では、従来の処理例（１）に比較して、処理データ量、処理時間が削減されていることが分かる。このように、本発明の同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）によれば、従来の個別のセンサ情報や動き情報を逐次処理する構成に比較して、効率的な処理が可能となる。 The processing data amount and processing time for each of these processing examples (1) to (5) are shown. In the processing examples according to the present invention shown in (2) to (5), it can be seen that the processing data amount and the processing time are reduced as compared with the conventional processing example (1). As described above, according to the identification processing (Localization) of the present invention, it is possible to perform efficient processing as compared with the conventional configuration in which individual sensor information and motion information are sequentially processed.
上述した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する情報処理装置の機能構成について、図２８を参照して説明する。図２８は、同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する情報処理装置の機能構成を示すブロック図である。情報処理装置５００は、例えば、カメラ等のセンサ部を備え、かつデータ処理プロセッサを備えた情報処理装置によって構成される。 A functional configuration of the information processing apparatus that performs the above-described identification processing (Localization) will be described with reference to FIG. FIG. 28 is a block diagram illustrating a functional configuration of an information processing apparatus that executes identification processing (Localization). The information processing apparatus 500 includes, for example, an information processing apparatus including a sensor unit such as a camera and a data processing processor.
図２８に示すように情報処理装置５００は、カメラ等のセンサ５０１と、データ処理部５０２と、記憶部５０３を有する。データ処理部５０２は、ＣＰＵを備え、記憶部５０３に記録されたプログラムに従ったデータ処理を実行する。データ処理部５０２は、センサパッチ生成部５１１と、同定処理部５１２を有する。これらは、図７を参照して説明したセンサパッチ生成部３０１と、同定処理部３０２に対応する。データ処理部５０２においては、記憶部に記録されたプログラムに従って、各種の処理、すなわち、上述した各処理を実行する。 As illustrated in FIG. 28, the information processing apparatus 500 includes a sensor 501 such as a camera, a data processing unit 502, and a storage unit 503. The data processing unit 502 includes a CPU and executes data processing according to a program recorded in the storage unit 503. The data processing unit 502 includes a sensor patch generation unit 511 and an identification processing unit 512. These correspond to the sensor patch generation unit 301 and the identification processing unit 302 described with reference to FIG. In the data processing unit 502, various processes, that is, the above-described processes are executed in accordance with a program recorded in the storage unit.
情報処理装置５００は、自装置の位置および方向を推定する同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）を実行する装置（デバイス）であり、センサ５０１は、例えばカメラ等によって構成され、自装置の周囲の情報としてのセンサ情報を取得する。データ処理部５０２のセンサパッチ生成部５１１は、センサ５０１によって取得されるセンサ情報を集積したセンサパッチを生成するとともに、センサパッチ生成期間における自装置の動き情報を取得する。同定処理部５１２は、センサパッチと、動き情報を入力して、入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行する。 The information processing apparatus 500 is an apparatus (device) that performs identification processing (Localization) for estimating the position and direction of the own apparatus. The sensor 501 is configured by, for example, a camera or the like, and is a sensor as information around the own apparatus. Get information. The sensor patch generation unit 511 of the data processing unit 502 generates a sensor patch in which sensor information acquired by the sensor 501 is integrated, and acquires motion information of the own apparatus during the sensor patch generation period. The identification processing unit 512 inputs sensor patches and motion information, and executes identification processing for estimating the position and direction of the own device based on the input information.
センサパッチ生成部５１１は、センサ５０１によって取得される新たなセンサ情報に基づくセンサパッチの更新処理と、新たに入力される動き情報に基づくデバイス姿勢を推定する処理を実行する。
センサパッチ更新処理としては、例えば、センサ情報の集積処理、ランドマークの抽出処理、およびグリッドマップの生成処理を実行する。
また、デバイス姿勢推定処理としては、前述したように、
（ａ）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
（ｂ）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
上記（ａ）または（ｂ）いずれかの処理を適用して実行する。
The sensor patch generation unit 511 executes a sensor patch update process based on new sensor information acquired by the sensor 501 and a process for estimating a device posture based on newly input motion information.
As sensor patch update processing, for example, sensor information accumulation processing, landmark extraction processing, and grid map generation processing are executed.
As the device posture estimation process, as described above,
(A) Mean propagation method (mean propagation)
(B) Gaussian model
The processing is executed by applying one of the processes (a) and (b).
また、センサパッチ生成部５１１は、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行する。すなわち、先に説明した図１０のステップＳ１０５の処理であり、判定基準として、
（ａ）予め設定された時間の経過によるタイムアウト、
（ｂ）予め設定された閾値距離に自装置の移動距離が到達、
（ｃ）自装置の先行姿勢［Ｕ］に対応する共分散行列共分散行列Σｕのいずれかの要素（Ｄｅｔｅｒｍｉｎａｎｔ）の値が予め定めた閾値を超えた、
（ｄ）自装置の先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの対角要素（ｄｉａｇｏｎａｌ ｅｌｅｍｅｎｔｓ）の値が予め定めた閾値を超えた、
（ｅ）生成地図におけるセル中、占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］設定のなされたセル数が予め定めた閾値を越えた、
（ｆ）前記同定処理部からのセンサパッチ出力要求の受領、
上記（ａ）〜（ｆ）のいずれかの１つの基準、またはいずれかの基準の組み合わせを適用したセンサパッチの生成終了判定処理を実行する。
In addition, the sensor patch generation unit 511 executes a generation end determination process according to a predetermined determination criterion for the sensor patch being generated. That is, it is the process of step S105 of FIG.
(A) timeout due to elapse of a preset time;
(B) The movement distance of the device reaches a preset threshold distance;
(C) The value of any element (Determinant) of the covariance matrix covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold value,
(D) the value of any diagonal elements of the covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold;
(E) Among the cells in the generated map, the number of cells for which the occupation [Occupied] setting is made exceeds a predetermined threshold value.
(F) receiving a sensor patch output request from the identification processing unit;
A sensor patch generation end determination process to which one of the above-mentioned criteria (a) to (f) or a combination of any criteria is applied is executed.
データ処理部５０１の同定処理部５１２としては、先に説明したように、
同定処理例１．センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
同定処理例２．オーバラップセンサデータを含むセンサパッチを適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
同定処理例３．オーバラップセンサデータを含むセンサパッチの変化分の解析処理を適用した同定処理（Ｌｏｃａｌｉｚａｔｉｏｎ）
これらのいずれかの処理を実行する。
As described above, as the identification processing unit 512 of the data processing unit 501,
Identification processing example 1. Identification processing (Localization) using sensor patches that do not contain overlap sensor data by sensor patch reset processing
Identification processing example 2 Identification processing using sensor patches including overlap sensor data (Localization)
Identification processing example 3 Identification processing (Localization) applying analysis processing of sensor patch change including overlap sensor data
One of these processes is executed.
センサパッチリセット処理によるオーバラップセンサデータを含まないセンサパッチを適用した同定処理においては、同定処理の成功判定処理、すなわち、予め保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報との一致度を検証する処理などによる同定処理の成功判定を実行し、同定処理の成功の判定に基づいて、センサパッチ生成部５１１に対するセンサパッチのリセット処理要求を出力する。センサパッチ生成部５１１は、同定処理部５１２からのリセット要求の入力に基づいて、新規センサパッチの生成処理を開始する。 In the identification process using the sensor patch not including the overlap sensor data by the sensor patch reset process, the identification process success determination process, that is, the map data stored in advance or the generated sensor patch information and the newly generated sensor The identification processing success determination is performed by processing for verifying the degree of coincidence with the patch information, and a sensor patch reset processing request is output to the sensor patch generation unit 511 based on the determination of the identification processing success. The sensor patch generation unit 511 starts generation processing of a new sensor patch based on the reset request input from the identification processing unit 512.
また、上記の同定処理例２，３では、センサパッチ生成部５１１に対するセンサパッチのリセット処理要求を出力することなく、センサパッチ生成部５１２から、各センサパッチに重複するセンサ情報を含むセンサパッチを入力して、自装置の位置および方向を推定する同定処理を実行する。 In the identification processing examples 2 and 3 described above, the sensor patch generation unit 512 outputs a sensor patch including overlapping sensor information without outputting a sensor patch reset processing request to the sensor patch generation unit 511. An identification process for estimating the position and direction of the device itself is executed.
例えば処理例２では、同定処理部５１２は、各センサパッチに重複するセンサ情報を含むセンサパッチを適用した同定処理の実行に際して、重複情報の過大評価を避けるための平滑化処理を実行する。
具体的には、拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）に従った処理を基本とした同定処理を実行する場合、
拡張カルマン・フィルタ（ＥＫＦ）処理において、センサパッチに対応して設定される共分散行列Σｓを、
Σｓ←ｇ（Σｓ）
ただし、ｇ（）は、共分散行列Σｓを増加させる関数、
とする処理を実行して同定処理を行なう。
For example, in Processing Example 2, the identification processing unit 512 executes a smoothing process for avoiding overestimation of duplicate information when executing an identification process in which a sensor patch including sensor information that overlaps each sensor patch is applied.
Specifically, when executing an identification process based on a process according to an extended Kalman filter (EKF: Extended Kalman Filter),
In the extended Kalman filter (EKF) process, the covariance matrix Σs set corresponding to the sensor patch is
Σs ← g (Σs)
Where g () is a function that increases the covariance matrix Σs,
The identification process is performed by executing the process.
また、モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）に従った処理を基本とした同定処理を実行する構成では、モンテカルロ同定処理（ＭＣＬ）において、センサパッチに基づく確率密度の算出式ｐ（ｚｔ｜ｘｔ）、
ただし、ｚｔは、時間ｔにおけるセンサ情報、ｘｔは時間ｔにおける装置姿勢、
上記式を、
ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））
ただし、ｇ（ｐ）は、ｐを増加させる関数、
とする処理を実行して同定処理を行なう。
Further, in a configuration that executes an identification process based on a process according to a Monte Carlo identification process (MCL: Monte Carlo Localization), in the Monte Carlo identification process (MCL), a probability density calculation formula p (z t | xt ),
Where zt is sensor information at time t, xt is the device attitude at time t,
The above formula
p (z t | x t ) ← g (p (z t | x t ))
Where g (p) is a function that increases p,
The identification process is performed by executing the process.
また、処理例３では、同定処理部５１２は、センサパッチ生成部５１１から入力するセンサパッチに対応して実行された同定処理が成功した場合、処理済みのセンサパッチを、処理済みセンサパッチとして記憶部５０３に格納し、新たにセンサパッチ生成部から入力する新規入力センサパッチと、処理済みセンサパッチとの比較に基づいて、変化情報を抽出し、抽出した変化情報に基づく同定処理を実行する。 Further, in the processing example 3, the identification processing unit 512 stores the processed sensor patch as the processed sensor patch when the identification processing executed corresponding to the sensor patch input from the sensor patch generation unit 511 is successful. Change information is extracted based on the comparison between the new input sensor patch newly stored in the unit 503 and newly input from the sensor patch generation unit and the processed sensor patch, and identification processing based on the extracted change information is executed.
この変化分抽出処理を実行する場合、同定処理部５１２は、新規入力センサパッチと、処理済みセンサパッチとの変化情報に基づく同定処理として、
（Ａ）２つのセンサパッチの差分情報による処理、
（Ｂ）２つのセンサパッチの更新率情報による処理、
上記（Ａ）または（Ｂ）のいずれかの処理を実行する。例えば、モンテカルロ同定処理（ＭＣＬ）処理を適用して、更新率情報に基づく同定処理を実行する。
When executing the change extraction process, the identification processing unit 512 performs an identification process based on change information between the new input sensor patch and the processed sensor patch.
(A) Processing based on difference information between two sensor patches;
(B) Processing based on update rate information of two sensor patches,
The process (A) or (B) is executed. For example, a Monte Carlo identification process (MCL) process is applied to execute an identification process based on the update rate information.
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。すなわち、例示という形態で本発明を開示してきたのであり、限定的に解釈されるべきではない。本発明の要旨を判断するためには、特許請求の範囲の欄を参酌すべきである。 The present invention has been described in detail above with reference to specific embodiments. However, it is obvious that those skilled in the art can make modifications and substitutions of the embodiments without departing from the gist of the present invention. In other words, the present invention has been disclosed in the form of exemplification, and should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims should be taken into consideration.
なお、明細書中において説明した一連の処理はハードウェア、またはソフトウェア、あるいは両者の複合構成によって実行することが可能である。ソフトウェアによる処理を実行する場合は、処理シーケンスを記録したプログラムを、専用のハードウェアに組み込まれたコンピュータ内のメモリにインストールして実行させるか、あるいは、各種処理が実行可能な汎用コンピュータにプログラムをインストールして実行させることが可能である。 The series of processes described in the specification can be executed by hardware, software, or a combined configuration of both. When executing processing by software, the program recording the processing sequence is installed in a memory in a computer incorporated in dedicated hardware and executed, or the program is executed on a general-purpose computer capable of executing various processing. It can be installed and run.
例えば、プログラムは記録媒体としてのハードディスクやＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）に予め記録しておくことができる。あるいは、プログラムはフレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ），ＭＯ（Ｍａｇｎｅｔｏ ｏｐｔｉｃａｌ）ディスク，ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）、磁気ディスク、半導体メモリなどのリムーバブル記録媒体に、一時的あるいは永続的に格納（記録）しておくことができる。このようなリムーバブル記録媒体は、いわゆるパッケージソフトウエアとして提供することができる。 For example, the program can be recorded in advance on a hard disk or ROM (Read Only Memory) as a recording medium. Alternatively, the program is temporarily or permanently stored on a removable recording medium such as a flexible disk, a CD-ROM (Compact Disc Read Only Memory), an MO (Magneto Optical) disk, a DVD (Digital Versatile Disc), a magnetic disk, or a semiconductor memory. It can be stored (recorded). Such a removable recording medium can be provided as so-called package software.
なお、プログラムは、上述したようなリムーバブル記録媒体からコンピュータにインストールする他、ダウンロードサイトから、コンピュータに無線転送したり、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネットといったネットワークを介して、コンピュータに有線で転送し、コンピュータでは、そのようにして転送されてくるプログラムを受信し、内蔵するハードディスク等の記録媒体にインストールすることができる。 The program is installed on the computer from the removable recording medium as described above, or is wirelessly transferred from the download site to the computer, or is wired to the computer via a network such as a LAN (Local Area Network) or the Internet. The computer can receive the program transferred in this manner and install it on a recording medium such as a built-in hard disk.
なお、明細書に記載された各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。また、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。 Note that the various processes described in the specification are not only executed in time series according to the description, but may be executed in parallel or individually according to the processing capability of the apparatus that executes the processes or as necessary. Further, in this specification, the system is a logical set configuration of a plurality of devices, and the devices of each configuration are not limited to being in the same casing.
以上、説明したように、本発明の一実施例の構成によれば、センサによって取得される個別のセンサ情報を集積した情報、例えば個別のセンサ情報をＺｔとした場合、センサデータ集合｛Ｚ０∪Ｚ１∪Ｚ２｝などの集積センサ情報に基づくセンサパッチを生成し、このセンサパッチに基づくデバイスの位置および方向推定処理としての同定処理を実行する。本構成によれば、個別のセンサ情報のみからは得られない情報の適用が可能となり、ノイズを削減してＳＮＲの高いデータに基づく同定処理を行なうことが可能となる。また、処理タイミングも入力フレームのスピード、例えば３０フレーム／ｓｅｃごとに１つのフレーム画像の処理を行う必要がなくなり、処理負荷の低減が可能となる。 As described above, according to the configuration of the embodiment of the present invention, information obtained by accumulating individual sensor information acquired by a sensor, for example, when individual sensor information is Zt, a sensor data set {Z 0 A sensor patch based on integrated sensor information such as ∪Z 1 ∪Z 2 } is generated, and an identification process is performed as a device position and direction estimation process based on the sensor patch. According to this configuration, it is possible to apply information that cannot be obtained only from individual sensor information, and it is possible to reduce noise and perform identification processing based on data having a high SNR. Further, it is not necessary to process one frame image every input frame speed, for example, every 30 frames / sec, and the processing load can be reduced.
１００ デバイス（情報処理装置）
１０１ センサ
１５１ 先行同定情報
１７１ 動き情報
１７２ センサ情報
１８１ 同定情報
２００ 同定処理部
２０１ 予測処理部
２０２ 更新処理部
２５０ デバイス
３０１ センサパッチ生成部
３０２ 同定処理部
３１０〜３１３ センサパッチ
３２０ センサパッチ（地図）
３３１ デバイス
３４１ 先行生成センサパッチ
３４２ 新規生成センサパッチ
３５０〜３５３ センサパッチ
３８０〜３８３ センサパッチ
３９０〜３９３ センサパッチ
４０１ ｅｍｐｔｙパッチ
４０２ センサパッチ［ｐ０］
４０３ センサパッチ［ｐ２］
４２０ 領域（Ｅｎｖｉｒｏｎｍｅｎｔ）
４２１ デバイス
４２３ センサパッチ
４２５ 矩形領域
５００ 情報処理装置（デバイス）
５０１ センサ
５０２ データ処理部
５０３ 記憶部
５１１ センサパッチ生成部
５１２ 同定処理部
100 devices (information processing equipment)
DESCRIPTION OF SYMBOLS 101 Sensor 151 Prior identification information 171 Motion information 172 Sensor information 181 Identification information 200 Identification processing unit 201 Prediction processing unit 202 Update processing unit 250 Device 301 Sensor patch generation unit 302 Identification processing unit 310 to 313 Sensor patch 320 Sensor patch (map)
331 Device 341 Pre-generated sensor patch 342 Newly generated sensor patch 350 to 353 Sensor patch 380 to 383 Sensor patch 390 to 393 Sensor patch 401 empty patch 402 Sensor patch [p 0 ]
403 Sensor patch [p 2 ]
420 region (Environment)
421 Device 423 Sensor patch 425 Rectangular area 500 Information processing device (device)
501 Sensor 502 Data processing unit 503 Storage unit 511 Sensor patch generation unit 512 Identification processing unit
Claims (33)
自装置の周囲の情報としてのセンサ情報を取得するセンサと、
自装置の動き情報と、前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチを生成するセンサパッチ生成部と、
前記センサパッチと、該センサパッチ生成期間における自装置の動き情報を入力し、該入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行する同定処理部と、
を有することを特徴とする情報処理装置。 It is an information processing device that executes identification processing (Localization) for estimating the position and direction of the device itself,
A sensor that acquires sensor information as information around the device itself;
A sensor patch generation unit that generates sensor patches that integrate sensor information obtained in a certain time and space by accumulating movement information of the device itself and sensor information acquired by the sensor;
An identification processing unit that inputs the sensor patch and movement information of the own apparatus in the sensor patch generation period, and executes an identification process for estimating the position and direction of the own apparatus based on the input information;
An information processing apparatus comprising:
前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理と、新たに入力される動き情報に基づくデバイス姿勢を推定する処理を実行するとともに、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The sensor patch generator is
A sensor patch update process based on new sensor information acquired by the sensor and a device posture estimation process based on newly input motion information are executed, and a preset sensor patch being generated is set in advance. The information processing apparatus according to claim 1, wherein the information processing apparatus is configured to execute a generation end determination process according to a determination criterion.
前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理を実行する構成であり、該センサパッチ更新処理として、センサ情報の集積処理、ランドマークの抽出処理、およびグリッドマップの生成処理を実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The sensor patch generator is
The sensor patch update process is executed based on new sensor information acquired by the sensor. The sensor patch update process includes sensor information accumulation process, landmark extraction process, and grid map generation process. The information processing apparatus according to claim 1, wherein the information processing apparatus is configured to execute.
新たに入力される動き情報に基づくデバイス姿勢を推定するデバイス姿勢推定処理を実行する構成であり、
該デバイス姿勢推定処理を、
（ａ）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
（ｂ）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
上記（ａ）または（ｂ）いずれかの処理を適用して実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The sensor patch generator is
It is a configuration that executes device posture estimation processing for estimating a device posture based on newly input motion information,
The device attitude estimation process
(A) Mean propagation method (mean propagation)
(B) Gaussian model
The information processing apparatus according to claim 1, wherein the information processing apparatus is configured to execute by applying any one of the processes (a) and (b).
前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチと、該センサパッチ生成期間における自装置の動き情報とに基づいて、環境地図の生成情報として適用可能なグリッドマップの座標更新および自装置の姿勢更新処理を実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The sensor patch generator is
By accumulating the sensor information acquired by the sensor, based on the sensor patch integrating the sensor information obtained in a certain time / space and the movement information of the own device in the sensor patch generation period, The information processing apparatus according to claim 1, wherein the information processing apparatus is configured to execute a grid map coordinate update applicable as generation information and a posture update process of the own apparatus.
生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行する構成であり、前記判定基準として、
（ａ）予め設定された時間の経過によるタイムアウト、
（ｂ）予め設定された閾値距離に自装置の移動距離が到達、
（ｃ）自装置の先行姿勢［Ｕ］に対応する共分散行列共分散行列Σｕのいずれかの要素（Ｄｅｔｅｒｍｉｎａｎｔ）の値が予め定めた閾値を超えた、
（ｄ）自装置の先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの対角要素（ｄｉａｇｏｎａｌ ｅｌｅｍｅｎｔｓ）の値が予め定めた閾値を超えた、
（ｅ）生成地図におけるセル中、占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］設定のなされたセル数が予め定めた閾値を越えた、
（ｆ）前記同定処理部からのセンサパッチ出力要求の受領、
上記（ａ）〜（ｆ）のいずれかの１つの基準、またはいずれかの基準の組み合わせを適用したセンサパッチの生成終了判定処理を実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The sensor patch generator is
It is a configuration that performs generation end determination processing according to a predetermined determination criterion for the sensor patch being generated, and as the determination criterion,
(A) timeout due to elapse of a preset time;
(B) The movement distance of the device reaches a preset threshold distance;
(C) The value of any element (Determinant) of the covariance matrix covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold value,
(D) the value of any diagonal elements of the covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold;
(E) Among the cells in the generated map, the number of cells for which the occupation [Occupied] setting is made exceeds a predetermined threshold value.
(F) receiving a sensor patch output request from the identification processing unit;
2. The configuration according to claim 1, wherein the sensor patch generation end determination process is performed by applying any one of the criteria (a) to (f) or a combination of any criteria. Information processing device.
前記同定処理の成功判定処理を実行する構成であり、同定処理の成功の判定に基づいて、前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力する構成であることを特徴とする請求項１に記載の情報処理装置。 The identification processing unit
2. The configuration for executing a success determination process for the identification process, and a configuration for outputting a sensor patch reset process request to the sensor patch generation unit based on the determination of the success of the identification process. The information processing apparatus described in 1.
前記同定処理部からのリセット要求の入力に基づいて、新規センサパッチの生成処理を開始する構成であることを特徴とする請求項７に記載の情報処理装置。 The sensor patch generator is
The information processing apparatus according to claim 7, wherein the processing for generating a new sensor patch is started based on an input of a reset request from the identification processing unit.
前記同定処理の成功判定処理として、
予め保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報との一致度を検証する処理を実行する構成であることを特徴とする請求項７に記載の情報処理装置。 The identification processing unit
As the success determination process of the identification process,
The information processing apparatus according to claim 7, wherein the information processing apparatus is configured to execute a process of verifying a degree of coincidence between map data stored in advance or generated sensor patch information and newly generated sensor patch information.
前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力することなく、前記センサパッチ生成部から、各センサパッチに重複するセンサ情報を含むセンサパッチを入力して、自装置の位置および方向を推定する同定処理を実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The identification processing unit
Without outputting a sensor patch reset processing request to the sensor patch generation unit, a sensor patch including sensor information that overlaps each sensor patch is input from the sensor patch generation unit to estimate the position and direction of the device itself. The information processing apparatus according to claim 1, wherein the identification process is performed.
各センサパッチに重複するセンサ情報を含むセンサパッチを適用した同定処理の実行に際して、重複情報の過大評価を避けるための平滑化処理を実行する構成であることを特徴とする請求項１０に記載の情報処理装置。 The identification processing unit
The configuration according to claim 10, wherein a smoothing process for avoiding overestimation of duplicate information is performed when executing an identification process in which a sensor patch including overlapping sensor information is applied to each sensor patch. Information processing device.
拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）に従った処理を基本とした同定処理を実行する構成であり、
拡張カルマン・フィルタ（ＥＫＦ）処理において、センサパッチに対応して設定される共分散行列Σｓを、
Σｓ←ｇ（Σｓ）
ただし、ｇ（）は、共分散行列Σｓを増加させる関数、
とする処理を実行して同定処理を行なう構成であることを特徴とする請求項１０に記載の情報処理装置。 The identification processing unit
It is a configuration that executes identification processing based on processing according to an extended Kalman filter (EKF: Extended Kalman Filter),
In the extended Kalman filter (EKF) process, the covariance matrix Σs set corresponding to the sensor patch is
Σs ← g (Σs)
Where g () is a function that increases the covariance matrix Σs,
The information processing apparatus according to claim 10, wherein the identification process is performed by executing the process.
モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）に従った処理を基本とした同定処理を実行する構成であり、
モンテカルロ同定処理（ＭＣＬ）において、センサパッチに基づく確率密度の算出式ｐ（ｚｔ｜ｘｔ）、
ただし、ｚｔは、時間ｔにおけるセンサ情報、ｘｔは時間ｔにおける装置姿勢、
上記式を、
ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））
ただし、ｇ（ｐ）は、ｐを増加させる関数、
とする処理を実行して同定処理を行なう構成であることを特徴とする請求項１０に記載の情報処理装置。 The identification processing unit
It is a configuration that executes identification processing based on processing according to Monte Carlo Localization (MCL),
In Monte Carlo identification processing (MCL), a probability density calculation formula p (z t | x t ) based on sensor patches,
Where zt is sensor information at time t, xt is the device attitude at time t,
The above formula
p (z t | x t ) ← g (p (z t | x t ))
Where g (p) is a function that increases p,
The information processing apparatus according to claim 10, wherein the identification process is performed by executing the process.
前記センサパッチ生成部から入力するセンサパッチに対応して実行された同定処理が成功した場合、処理済みのセンサパッチを、処理済みセンサパッチとして記憶部に格納し、
新たにセンサパッチ生成部から入力する新規入力センサパッチと、前記処理済みセンサパッチとの比較に基づいて、変化情報を抽出し、抽出した変化情報に基づく同定処理を実行する構成であることを特徴とする請求項１に記載の情報処理装置。 The identification processing unit
When the identification process executed in response to the sensor patch input from the sensor patch generation unit is successful, the processed sensor patch is stored in the storage unit as a processed sensor patch,
It is configured to extract change information based on a comparison between a new input sensor patch newly input from a sensor patch generation unit and the processed sensor patch, and to perform identification processing based on the extracted change information The information processing apparatus according to claim 1.
前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、
（Ａ）２つのセンサパッチの差分情報による処理、
（Ｂ）２つのセンサパッチの更新率情報による処理、
上記（Ａ）または（Ｂ）のいずれかの処理を実行する構成であることを特徴とする請求項１４に記載の情報処理装置。 The identification processing unit
As identification processing based on change information between the new input sensor patch and the processed sensor patch,
(A) Processing based on difference information between two sensor patches;
(B) Processing based on update rate information of two sensor patches,
The information processing apparatus according to claim 14, wherein the information processing apparatus is configured to execute either of the processes (A) and (B).
前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、
モンテカルロ同定処理（ＭＣＬ）処理を適用して、更新率情報に基づく同定処理を実行する構成であることを特徴とする請求項１４に記載の情報処理装置。 The identification processing unit
As identification processing based on change information between the new input sensor patch and the processed sensor patch,
The information processing apparatus according to claim 14, wherein the information processing apparatus is configured to execute an identification process based on update rate information by applying a Monte Carlo identification process (MCL) process.
センサにおいて、自装置の周囲の情報としてのセンサ情報を取得するセンサ情報取得ステップと、
センサパッチ生成部において、自装置の動き情報と、前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチを生成するセンサパッチ生成ステップと、
同定処理部において、前記センサパッチと、該センサパッチ生成期間における自装置の動き情報を入力し、該入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行する同定処理ステップと、
を有することを特徴とする同定処理方法。 In the information processing device, an identification processing method for executing identification processing (Localization) for estimating the position and direction of the device itself,
In the sensor, a sensor information acquisition step for acquiring sensor information as information around the device itself;
In the sensor patch generation unit, a sensor patch generation step of generating a sensor patch in which sensor information obtained in a certain time / space is integrated by integrating movement information of the device itself and sensor information acquired by the sensor; ,
In the identification processing unit, an identification processing step of inputting the sensor patch and movement information of the own apparatus during the sensor patch generation period and executing an identification process for estimating the position and direction of the own apparatus based on the input information; ,
The identification processing method characterized by having.
前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理と、新たに入力される動き情報に基づくデバイス姿勢を推定する処理を実行するとともに、生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行するステップであることを特徴とする請求項１７に記載の同定処理方法。 The sensor patch generation step includes:
A sensor patch update process based on new sensor information acquired by the sensor and a device posture estimation process based on newly input motion information are executed, and a preset sensor patch being generated is set in advance. The identification processing method according to claim 17, wherein the generation end determination process is performed according to a determination criterion.
前記センサによって取得される新たなセンサ情報に基づくセンサパッチの更新処理を実行するステップを含み、該センサパッチ更新処理として、センサ情報の集積処理、ランドマークの抽出処理、およびグリッドマップの生成処理を実行することを特徴とする請求項１７に記載の同定処理方法。 The sensor patch generation step includes:
Including a step of executing a sensor patch update process based on new sensor information acquired by the sensor, and the sensor patch update process includes a sensor information accumulation process, a landmark extraction process, and a grid map generation process. The identification processing method according to claim 17, wherein the identification processing method is executed.
新たに入力される動き情報に基づくデバイス姿勢を推定するデバイス姿勢推定処理を実行するステップを含み、
該デバイス姿勢推定処理を、
（ａ）平均値伝播法（ｍｅａｎ ｐｒｏｐａｇａｔｉｏｎ）
（ｂ）ガウシアンモデル（Ｇａｕｓｓｉａｎ ｍｏｄｅｌ）
上記（ａ）または（ｂ）いずれかの処理を適用して実行することを特徴とする請求項１７に記載の同定処理方法。 The sensor patch generation step includes:
Executing a device posture estimation process for estimating a device posture based on newly input motion information,
The device attitude estimation process
(A) Mean propagation method (mean propagation)
(B) Gaussian model
The identification processing method according to claim 17, wherein the identification processing method is executed by applying one of the processes (a) and (b).
自装置の動き情報と、前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチと、該センサパッチ生成期間における自装置の動き情報とに基づいて、環境地図の生成情報として適用可能なグリッドマップの座標更新および自装置の姿勢更新処理を実行するステップであることを特徴とする請求項１７に記載の同定処理方法。 The sensor patch generation step includes:
By integrating the movement information of the own device and the sensor information acquired by the sensor, a sensor patch integrating the sensor information obtained in a certain time / space, and the movement information of the own device during the sensor patch generation period, 18. The identification processing method according to claim 17, wherein a step of executing coordinate update of the grid map applicable as generation information of the environment map and a posture update process of the own apparatus based on the step is performed.
生成中のセンサパッチについての予め設定された判定基準に従って生成終了判定処理を実行するステップであり、前記判定基準として、
（ａ）予め設定された時間の経過によるタイムアウト、
（ｂ）予め設定された閾値距離に自装置の移動距離が到達、
（ｃ）自装置の先行姿勢［Ｕ］に対応する共分散行列共分散行列Σｕのいずれかの要素（Ｄｅｔｅｒｍｉｎａｎｔ）の値が予め定めた閾値を超えた、
（ｄ）自装置の先行姿勢［Ｕ］に対応する共分散行列Σｕのいずれかの対角要素（ｄｉａｇｏｎａｌ ｅｌｅｍｅｎｔｓ）の値が予め定めた閾値を超えた、
（ｅ）生成地図におけるセル中、占有［オキュパイド（Ｏｃｃｕｐｉｅｄ）］設定のなされたセル数が予め定めた閾値を越えた、
（ｆ）前記同定処理部からのセンサパッチ出力要求の受領、
上記（ａ）〜（ｆ）のいずれかの１つの基準、またはいずれかの基準の組み合わせを適用したセンサパッチの生成終了判定処理を実行することを特徴とする請求項１７に記載の同定処理方法。 The sensor patch generation step includes:
This is a step of performing generation end determination processing according to a predetermined determination criterion for the sensor patch being generated, and as the determination criterion,
(A) timeout due to elapse of a preset time;
(B) The movement distance of the device reaches a preset threshold distance;
(C) The value of any element (Determinant) of the covariance matrix covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold value,
(D) the value of any diagonal elements of the covariance matrix Σu corresponding to the preceding posture [U] of the own device exceeds a predetermined threshold;
(E) Among the cells in the generated map, the number of cells for which the occupation [Occupied] setting is made exceeds a predetermined threshold value.
(F) receiving a sensor patch output request from the identification processing unit;
18. The identification processing method according to claim 17, wherein the sensor patch generation end determination process is performed by applying any one of the criteria (a) to (f) or a combination of any criteria. .
前記同定処理の成功判定処理を実行するステップを含み、同定処理の成功の判定に基づいて、前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力することを特徴とする請求項１７に記載の同定処理方法。 The identification processing step includes
18. The method according to claim 17, further comprising a step of executing a success determination process of the identification process, and outputting a sensor patch reset process request to the sensor patch generation unit based on the determination of the success of the identification process. Identification processing method.
前記同定処理部からのリセット要求の入力に基づいて、新規センサパッチの生成処理を開始することを特徴とする請求項２３に記載の同定処理方法。 The sensor patch generator is
The identification processing method according to claim 23, wherein a generation process of a new sensor patch is started based on an input of a reset request from the identification processing unit.
前記同定処理の成功判定処理として、
予め保持する地図データあるいは生成済みのセンサパッチ情報と、新たに生成したセンサパッチ情報との一致度を検証する処理を実行することを特徴とする請求項２３に記載の同定処理方法。 The identification processing step includes
As the success determination process of the identification process,
24. The identification processing method according to claim 23, wherein a process of verifying a degree of coincidence between map data held in advance or generated sensor patch information and newly generated sensor patch information is performed.
前記センサパッチ生成部に対するセンサパッチのリセット処理要求を出力することなく、前記センサパッチ生成部から、各センサパッチに重複するセンサ情報を含むセンサパッチを入力して、自装置の位置および方向を推定する同定処理を実行することを特徴とする請求項１７に記載の同定処理方法。 The identification processing step includes
Without outputting a sensor patch reset processing request to the sensor patch generation unit, a sensor patch including sensor information that overlaps each sensor patch is input from the sensor patch generation unit to estimate the position and direction of the device itself. The identification processing method according to claim 17, wherein the identification processing is performed.
各センサパッチに重複するセンサ情報を含むセンサパッチを適用した同定処理の実行に際して、重複情報の過大評価を避けるための平滑化処理を実行することを特徴とする請求項２６に記載の同定処理方法。 The identification processing step includes
27. The identification processing method according to claim 26, wherein a smoothing process for avoiding overestimation of duplicate information is executed when executing an identification process in which a sensor patch including overlapping sensor information is applied to each sensor patch. .
拡張カルマン・フィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）に従った処理を基本とした同定処理を実行し、
拡張カルマン・フィルタ（ＥＫＦ）処理において、センサパッチに対応して設定される共分散行列Σｓを、
Σｓ←ｇ（Σｓ）
ただし、ｇ（）は、共分散行列Σｓを増加させる関数、
とする処理を実行して同定処理を行なうことを特徴とする請求項２６に記載の同定処理方法。 The identification processing step includes
An identification process based on a process according to an extended Kalman filter (EKF) is executed.
In the extended Kalman filter (EKF) process, the covariance matrix Σs set corresponding to the sensor patch is
Σs ← g (Σs)
Where g () is a function that increases the covariance matrix Σs,
27. The identification processing method according to claim 26, wherein the identification processing is performed by executing the following processing.
モンテカルロ同定処理（ＭＣＬ：Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ）に従った処理を基本とした同定処理を実行し、
モンテカルロ同定処理（ＭＣＬ）において、センサパッチに基づく確率密度の算出式ｐ（ｚｔ｜ｘｔ）、
ただし、ｚｔは、時間ｔにおけるセンサ情報、ｘｔは時間ｔにおける装置姿勢、
上記式を、
ｐ（ｚｔ｜ｘｔ）←ｇ（ｐ（ｚｔ｜ｘｔ））
ただし、ｇ（ｐ）は、ｐを増加させる関数、
とする処理を実行して同定処理を行なうことを特徴とする請求項２６に記載の同定処理方法。 The identification processing step includes
An identification process based on the process according to Monte Carlo Localization (MCL) is executed,
In Monte Carlo identification processing (MCL), a probability density calculation formula p (z t | x t ) based on sensor patches,
Where zt is sensor information at time t, xt is the device attitude at time t,
The above formula
p (z t | x t ) ← g (p (z t | x t ))
Where g (p) is a function that increases p,
27. The identification processing method according to claim 26, wherein the identification processing is performed by executing the following processing.
前記センサパッチ生成部から入力するセンサパッチに対応して実行された同定処理が成功した場合、処理済みのセンサパッチを、処理済みセンサパッチとして記憶部に格納し、
新たにセンサパッチ生成部から入力する新規入力センサパッチと、前記処理済みセンサパッチとの比較に基づいて、変化情報を抽出し、抽出した変化情報に基づく同定処理を実行することを特徴とする請求項１７に記載の同定処理方法。 The identification processing step includes
When the identification process executed in response to the sensor patch input from the sensor patch generation unit is successful, the processed sensor patch is stored in the storage unit as a processed sensor patch,
A change information is extracted based on a comparison between a new input sensor patch newly input from a sensor patch generation unit and the processed sensor patch, and an identification process based on the extracted change information is executed. Item 18. The identification processing method according to Item 17.
前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、
（Ａ）２つのセンサパッチの差分情報による処理、
（Ｂ）２つのセンサパッチの更新率情報による処理、
上記（Ａ）または（Ｂ）のいずれかの処理を実行することを特徴とする請求項３０に記載の同定処理方法。 The identification processing step includes
As identification processing based on change information between the new input sensor patch and the processed sensor patch,
(A) Processing based on difference information between two sensor patches;
(B) Processing based on update rate information of two sensor patches,
31. The identification processing method according to claim 30, wherein the processing of either (A) or (B) is executed.
前記新規入力センサパッチと、前記処理済みセンサパッチとの変化情報に基づく同定処理として、
モンテカルロ同定処理（ＭＣＬ）処理を適用して、更新率情報に基づく同定処理を実行するステップであることを特徴とする請求項３０に記載の同定処理方法。 The identification processing step includes
As identification processing based on change information between the new input sensor patch and the processed sensor patch,
31. The identification processing method according to claim 30, wherein the identification processing method is a step of performing identification processing based on update rate information by applying Monte Carlo identification processing (MCL) processing.
センサにおいて、自装置の周囲の情報としてのセンサ情報を取得させるセンサ情報取得ステップと、
センサパッチ生成部において、自装置の動き情報と、前記センサによって取得されるセンサ情報を集積することで、ある時間・空間内で得られるセンサ情報を統合したセンサパッチを生成するセンサパッチ生成ステップと、
同定処理部において、前記センサパッチと、該センサパッチ生成期間における自装置の動き情報を入力し、該入力情報に基づいて、自装置の位置および方向を推定する同定処理を実行させる同定処理ステップと、
を実行させることを特徴とするコンピュータ・プログラム。 In the information processing apparatus, a computer program for executing identification processing (Localization) for estimating the position and direction of the own apparatus,
In the sensor, a sensor information acquisition step for acquiring sensor information as information around the device itself;
In the sensor patch generation unit, a sensor patch generation step of generating a sensor patch in which sensor information obtained in a certain time / space is integrated by integrating movement information of the device itself and sensor information acquired by the sensor; ,
In the identification processing unit, an identification processing step of inputting the sensor patch and movement information of the own apparatus during the sensor patch generation period, and executing an identification process for estimating the position and direction of the own apparatus based on the input information; ,
A computer program for executing
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2006242095A JP2008065562A (en) | 2006-09-06 | 2006-09-06 | Information processing device, localization method and computer program |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2006242095A JP2008065562A (en) | 2006-09-06 | 2006-09-06 | Information processing device, localization method and computer program |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2008065562A true JP2008065562A (en) | 2008-03-21 |
| JP2008065562A5 JP2008065562A5 (en) | 2009-09-17 |
Family
ID=39288231
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2006242095A Pending JP2008065562A (en) | 2006-09-06 | 2006-09-06 | Information processing device, localization method and computer program |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2008065562A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013132748A (en) * | 2011-12-23 | 2013-07-08 | Samsung Electronics Co Ltd | Moving device and method for recognizing position of the same |
| CN104331078A (en) * | 2014-10-31 | 2015-02-04 | 东北大学 | Multi-robot cooperative localization method based on position mapping algorithm |
| JP2018517184A (en) * | 2015-04-14 | 2018-06-28 | フェイス インターナショナル コーポレーション | System and method for generating a scattering selective wavelength of electromagnetic energy |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003166824A (en) * | 2001-11-30 | 2003-06-13 | Sony Corp | Self position identification system and self position identification method for robot |
-
2006
- 2006-09-06 JP JP2006242095A patent/JP2008065562A/en active Pending
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003166824A (en) * | 2001-11-30 | 2003-06-13 | Sony Corp | Self position identification system and self position identification method for robot |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013132748A (en) * | 2011-12-23 | 2013-07-08 | Samsung Electronics Co Ltd | Moving device and method for recognizing position of the same |
| US9563528B2 (en) | 2011-12-23 | 2017-02-07 | Samsung Electronics Co., Ltd. | Mobile apparatus and localization method thereof |
| CN104331078A (en) * | 2014-10-31 | 2015-02-04 | 东北大学 | Multi-robot cooperative localization method based on position mapping algorithm |
| JP2018517184A (en) * | 2015-04-14 | 2018-06-28 | フェイス インターナショナル コーポレーション | System and method for generating a scattering selective wavelength of electromagnetic energy |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101776621B1 (en) | Apparatus for recognizing location mobile robot using edge based refinement and method thereof | |
| KR101725060B1 (en) | Apparatus for recognizing location mobile robot using key point based on gradient and method thereof | |
| JP6445995B2 (en) | Adaptive mapping using spatial aggregation of sensor data | |
| JP5987823B2 (en) | Method and system for fusing data originating from image sensors and motion or position sensors | |
| KR101776622B1 (en) | Apparatus for recognizing location mobile robot using edge based refinement and method thereof | |
| KR101503903B1 (en) | Apparatus and method for building map used in mobile robot | |
| KR101776620B1 (en) | Apparatus for recognizing location mobile robot using search based correlative matching and method thereof | |
| US8755562B2 (en) | Estimation apparatus, control method thereof, and program | |
| US9466107B2 (en) | Bundle adjustment based on image capture intervals | |
| KR100866380B1 (en) | Method for esrimating location using ObjectionRecognition of a robot | |
| JP5148669B2 (en) | Position detection apparatus, position detection method, and position detection program | |
| JP5444952B2 (en) | Apparatus, method, and program for automatic generation of map by sensor fusion, and movement of moving object using such automatically generated map | |
| JP5687082B2 (en) | Moving object tracking device | |
| JP5793851B2 (en) | POSITION ESTIMATION METHOD, POSITION ESTIMATION DEVICE, AND PROGRAM | |
| WO2012176249A1 (en) | Self-position estimation device, self-position estimation method, self-position estimation program, and mobile object | |
| CN111611853A (en) | Sensing information fusion method and device and storage medium | |
| Konrad et al. | Localization in digital maps for road course estimation using grid maps | |
| EP3920001A1 (en) | Obstacle avoidance control device, obstacle avoidance control system, obstacle avoidance control method, and recording medium | |
| CN115661299B (en) | Method for constructing lane line map, computer device and storage medium | |
| Nobre et al. | Drift-correcting self-calibration for visual-inertial SLAM | |
| US11948312B2 (en) | Object detection/tracking device, method, and program recording medium | |
| JP2008065562A (en) | Information processing device, localization method and computer program | |
| JP2008185417A (en) | Information processing device, information processing method, and computer program | |
| JP2018041431A (en) | Point group matching method with correspondence taken into account, point group matching device with correspondence taken into account, and program | |
| EP3846073A1 (en) | Systems and methods for providing a representation of lanes on a roadway |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20090804 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20090804 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20110823 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20110906 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20120110 |