JP2012248032A - Map processing method, program and robot system - Google Patents
Map processing method, program and robot system Download PDFInfo
- Publication number
- JP2012248032A JP2012248032A JP2011119676A JP2011119676A JP2012248032A JP 2012248032 A JP2012248032 A JP 2012248032A JP 2011119676 A JP2011119676 A JP 2011119676A JP 2011119676 A JP2011119676 A JP 2011119676A JP 2012248032 A JP2012248032 A JP 2012248032A
- Authority
- JP
- Japan
- Prior art keywords
- node
- reliability
- integration
- estimator
- estimators
- 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.)
- Granted
Links
- 238000003672 processing method Methods 0.000 title claims abstract description 18
- 230000010354 integration Effects 0.000 claims abstract description 143
- 238000000034 method Methods 0.000 claims abstract description 89
- 230000008569 process Effects 0.000 claims abstract description 62
- 238000013210 evaluation model Methods 0.000 claims abstract description 16
- 238000012854 evaluation process Methods 0.000 claims abstract description 6
- 238000011156 evaluation Methods 0.000 claims description 59
- 238000012545 processing Methods 0.000 claims description 39
- 238000010168 coupling process Methods 0.000 abstract description 5
- 230000033001 locomotion Effects 0.000 description 26
- 238000012423 maintenance Methods 0.000 description 15
- 238000005259 measurement Methods 0.000 description 13
- 238000010586 diagram Methods 0.000 description 12
- 239000011159 matrix material Substances 0.000 description 12
- 238000004364 calculation method Methods 0.000 description 9
- 238000013138 pruning Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 7
- 230000010365 information processing Effects 0.000 description 6
- 230000002950 deficient Effects 0.000 description 5
- 230000000052 comparative effect Effects 0.000 description 4
- 230000008878 coupling Effects 0.000 description 4
- 238000005859 coupling reaction Methods 0.000 description 4
- 238000003745 diagnosis Methods 0.000 description 4
- 238000000605 extraction Methods 0.000 description 4
- 230000001174 ascending effect Effects 0.000 description 3
- 230000012447 hatching Effects 0.000 description 3
- 238000011084 recovery Methods 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 238000012217 deletion Methods 0.000 description 2
- 230000037430 deletion Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 101100434480 Arabidopsis thaliana AFB2 gene Proteins 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013075 data extraction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping 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
- 230000010076 replication Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
本発明は、地図処理方法及びプログラム、並びにロボットシステムに関する。本発明は、地図処理プログラムを格納したコンピュータ読み取り可能な記憶媒体にも関する。 The present invention relates to a map processing method and program, and a robot system. The present invention also relates to a computer-readable storage medium storing a map processing program.
自律移動型ロボットは、実環境内で障害物、壁などと衝突すること無く自由に移動できるように、例えばロボットに搭載されたセンサの計測情報と環境地図との照合に基づいて自己位置の推定を行う。ロボットが実環境下で自己位置を確実、且つ、正確に推定するために、複数の異なる地図を併用する方式がある。また、環境地図を自動的に作成する技術としては、例えばＳＬＡＭ（Simultaneous Localization And Mapping）が知られている。 Autonomous mobile robots are able to move freely without colliding with obstacles, walls, etc. in the real environment. I do. There is a method in which a plurality of different maps are used together in order for a robot to reliably and accurately estimate its own position in an actual environment. As a technique for automatically creating an environmental map, for example, SLAM (Simultaneous Localization And Mapping) is known.
複数の地図を自動的に作成する方式は、一般的には以下の２つの方式に分類できる。第１の方式では、ある種類のセンサの計測情報に基づいて最初の地図を作成する。その後、他の種類のセンサの計測情報を最初の地図に足し加えて２番目の地図を作成し、以下同様にして３番目以降の地図を作成する。一方、第２の方式では、複数の異なる種類のセンサの計測情報を推定器に同時観測情報として入力して、複数の地図を同時に、且つ、自動的に作成する。 The method of automatically creating a plurality of maps can be generally classified into the following two methods. In the first method, an initial map is created based on measurement information of a certain type of sensor. Thereafter, the measurement information of other types of sensors is added to the first map to create a second map, and the third and subsequent maps are created in the same manner. On the other hand, in the second method, measurement information of a plurality of different types of sensors is input to the estimator as simultaneous observation information, and a plurality of maps are created simultaneously and automatically.
第１の方式の場合、２番目以降に作成される地図が最初に作成された地図に依存する。このため、２番目以降に作成された地図の精度は、最初に作成された地図の精度に依存してしまう。一方、第２の方式の場合、センサの種類が多くなると、推定器の観測空間が大きくなるため、推定器の計算量が多くなると共に計算時間がかかる。また、異なる種類のセンサは異なる計測レートを用いるため、使用するセンサの計測レートを最も遅い計測レートに合わせる必要がある。 In the case of the first method, the second and subsequent maps depend on the map created first. For this reason, the accuracy of the map created after the second depends on the accuracy of the map created first. On the other hand, in the case of the second method, as the number of types of sensors increases, the observation space of the estimator increases, so that the calculation amount of the estimator increases and calculation time is required. Also, since different types of sensors use different measurement rates, it is necessary to match the measurement rate of the sensor used with the slowest measurement rate.
しかし、複数の地図を同時に生成する場合、異なる種類のセンサの異なる誤差モデルを反映しながら各センサ独自の計測レートを用いることが望ましい。このように、独立して作成された複数の地図では、各地図の座標系が異なると共に、推定誤差によって夫々の地図が異なる歪みを含んでいる。このため、ロボットが自己位置の推定を行う段階では、各センサの計測情報と対応する地図とのマッチングにより推定された自己位置が、異なる座標系で表現されたものであるため、例えばロボットの移動経路を計画あるいは決定する経路計画（または、経路計画）などを行うためには推定した自己位置の座標系を統合（または、統一）する必要がある。 However, when a plurality of maps are generated simultaneously, it is desirable to use a measurement rate unique to each sensor while reflecting different error models of different types of sensors. As described above, in a plurality of maps created independently, the coordinate system of each map is different, and each map includes a different distortion due to an estimation error. For this reason, at the stage where the robot estimates its own position, the self-position estimated by matching the measurement information of each sensor with the corresponding map is expressed in a different coordinate system. In order to perform route planning (or route planning) for planning or determining a route, it is necessary to integrate (or unify) the estimated coordinate system of the self-location.
自律移動型ロボットのナビゲーションシステムにおいて、複数の自己位置推定器が推定した位置姿勢を統合する従来の統合方式では、例えば各自己位置推定器の推定結果を１つのベクトルにまとめ、統合処理の観測データとして入力することで確率的フィルタにより統合する。しかし、従来の統合方式では、以下のようなデメリットが生じてしまう。 In a conventional integration method that integrates the positions and orientations estimated by a plurality of self-position estimators in a navigation system for autonomous mobile robots, for example, the estimation results of each self-position estimator are combined into one vector, and the observation data of the integration process Are integrated by a stochastic filter. However, the conventional integration method has the following disadvantages.
第１に、確率的フィルタによる統合を行うために、各自己位置推定器の推定精度を表す推定結果の共分散行列をある同一の基準に合わせる必要がある。しかし、各自己位置推定器が独立に開発されているため、自己位置推定器間では共分散行列の計算方法や設定パラメータなどに違いがある。従って、このような共分散行列に基づく統合処理では、複数の自己位置推定器が独立して推定した自己位置の座標系を高精度に統合することは難しい。 First, in order to perform integration by a probabilistic filter, it is necessary to match the covariance matrix of the estimation result representing the estimation accuracy of each self-position estimator to a certain same reference. However, since each self-position estimator has been independently developed, there are differences in the calculation method and setting parameters of the covariance matrix among the self-position estimators. Therefore, in such integration processing based on the covariance matrix, it is difficult to integrate the coordinate system of the self-position estimated independently by a plurality of self-position estimators with high accuracy.
第２に、例えばある自己位置推定器の推定精度が低下すると（例えば、推定処理が破綻した場合も含む）、統合結果が推定精度の低下した自己位置推定器の推定結果の影響を受けて統合性能が低下してしまう。 Second, for example, when the estimation accuracy of a certain self-position estimator decreases (for example, even when the estimation process fails), the integration result is affected by the estimation result of the self-position estimator whose estimation accuracy has decreased. Performance will be degraded.
従来の統合方式では、複数の推定器が推定した位置姿勢を高精度に統合することは難しいという問題があった。 The conventional integration method has a problem that it is difficult to integrate the positions and orientations estimated by a plurality of estimators with high accuracy.
そこで、本発明は、複数の推定器が推定した位置姿勢を高精度に統合することのできる地図処理方法及びプログラム、並びにロボットシステムを提供することを目的とする。 In view of the above, an object of the present invention is to provide a map processing method and program, and a robot system capable of highly accurately integrating the positions and orientations estimated by a plurality of estimators.
本発明の一観点によれば、互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理をコンピュータに実行させることを特徴とする地図処理方法が提供される。 According to one aspect of the present invention, an evaluation process for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators. And a map processing method characterized by causing a computer to execute a combination process for performing different integration processes according to the reliability on the estimation results from the plurality of estimators.
本発明の一観点によれば、互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順をコンピュータに実行させることを特徴とするプログラムが提供される。 According to an aspect of the present invention, an evaluation procedure for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators. And a program for causing a computer to execute a combination procedure for performing different integration processes according to the reliability on the estimation results from the plurality of estimators.
本発明の一観点によれば、自律移動型ロボットに設けられた互いに異なるセンサと、前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部を備えたことを特徴とするロボットシステムが提供される。 According to one aspect of the present invention, different sensors provided in an autonomous mobile robot, a plurality of estimators that estimate self-positions based on inputs from the different sensors, and reliability of the plurality of estimators An evaluation unit that evaluates using a reliability evaluation model corresponding to the plurality of estimators, and a combining unit that performs different integration processing according to the reliability on the estimation results from the plurality of estimators. A robot system is provided.
開示の地図処理方法及びプログラム、並びにロボットシステムによれば、複数の推定器が推定した位置姿勢を高精度に統合することが可能になる。 According to the disclosed map processing method and program, and the robot system, the positions and orientations estimated by a plurality of estimators can be integrated with high accuracy.
開示の地図処理方法及びプログラム、並びにロボットシステムでは、統合器が各推定器からの入力情報の信頼性を評価して、異なる信頼性のレベルに応じた異なる方式で統合履歴と関連付ける。この関連付けにより、統合履歴を表した統合ツリーが時間的に更新され、規模が増大していく。信頼性のレベルとは、信頼性を数量化した信頼度をさらに例えば「高」、「中」、「低」の３段階に分けたものを言う。 In the disclosed map processing method and program, and the robot system, the integrator evaluates the reliability of the input information from each estimator and associates it with the integration history in different manners according to different levels of reliability. With this association, the integration tree representing the integration history is updated in time, and the scale increases. The level of reliability is a level obtained by further dividing the reliability obtained by quantifying the reliability into, for example, three levels of “high”, “medium”, and “low”.
また、統合ツリーの最新ノードから過去に辿って得られた推定経路を評価して下位のノードを削除することで、ツリー全体のメンテナンスを行いツリー規模を一定以下に維持するようにしても良い。 Alternatively, the estimated path obtained in the past from the latest node of the integrated tree may be evaluated and the lower nodes may be deleted to maintain the entire tree and keep the tree scale below a certain level.
以下に、開示の地図処理方法及びプログラム、並びにロボットシステムの各実施例を図面と共に説明する。 Hereinafter, embodiments of the disclosed map processing method and program, and robot system will be described with reference to the drawings.
図１は、ロボットの一例を示す斜視図である。図１に示すロボット１００は、例えばモータなどにより駆動される台車１０１、車輪１０２、各種内的センサ及び外的センサ、及び例えばＣＰＵ（Central Processing Unit）などのプロセッサにより形成された制御部を有する周知の基本構成を有する自律移動型ロボットで形成可能である。内的センサには、レーザレンジファインダ（ＬＲＦ：Laser Range Finder）２，３、及びカメラ３が含まれる。ＬＲＦ２とＬＲＦ３は、例えば異なる種類のＬＲＦであっても良い。異なる種類のＬＲＦには、計測範囲の異なるＬＲＦも含まれる。
FIG. 1 is a perspective view showing an example of a robot. A
図２は、ロボットを含むロボットシステムの一例を示すブロック図である。この例では、ロボットシステム２００は、自律移動型ロボット１００のナビゲーションシステムとしても機能する。ここでは説明の便宜上、推定器が３つの場合を説明するが、推定器は少なくとも２つあれば良い。
FIG. 2 is a block diagram illustrating an example of a robot system including a robot. In this example, the
推定器４は、上記ＣＰＵで形成可能であり、ＬＲＦ１と記憶部５に格納された地図１Ａに基づいてロボット１００の自己位置を推定するソフトウェアモジュール（推定モジュール）を形成する。推定器６は、上記ＣＰＵで形成可能であり、ＬＲＦ２と記憶部７に格納された地図１Ｂに基づいてロボット１００の自己位置を推定するソフトウェアモジュールを形成する。推定器８は、上記ＣＰＵで形成可能であり、カメラ３と記憶部９に格納された地図１Ｃに基づいてロボット１００の自己位置を推定するソフトウェアモジュールを形成する。
The
統合器１０は、上記ＣＰＵで形成可能であり、推定器４，６，８の自己位置推定結果を統合するソフトウェアモジュール（統合モジュール）を形成する。統合器１０において、推定器信頼性評価部１１は、推定器４，６，８からの入力の信頼性を評価して、例えば「高」＞「中」＞「低」の３段階の信頼性のレベルに分ける。信頼性のレベルとは、信頼性を数量化した信頼度をさらに例えば「高」、「中」、「低」の３段階に分けたものを言う。各信頼性のレベルによって、後述する統合ツリーへの結合が異なる。結合部１２は、信頼性のレベルが「高」（以下、説明の便宜上、信頼度が「高」とも言う）の場合、推定器からの入力を統合するためのフィルタの観測値としてフィルタ状態の更新処理を行い、新しいノードを作成して統合ツリーに追加する、推定器データの統合ツリーへの結合処理を行う。信頼性のレベルが「低」の場合、オドメトリのみでフィルタ状態の更新処理を行い、新しいノードを作成して統合ツリーに追加する。信頼性のレベルが「中」の場合、「高」と「低」の場合で行った処理を全て行う。これにより、統合ツリーの規模が増大していく。
The
統合器１０において、結合良さの評価部１３は、統合ツリーの規模を一定以下にするための枝刈を必要に応じて行うために、統合ツリーへの結合の良さを評価する。この評価は、統合器１０において推定器からの入力を統合するフィルタが推定した位置姿勢からロボット１００の動きを算出して、算出した動きとオドメトリ（Odometry）部２０で計測した動きと比較して動きの尤度を算出することで行うことができる。オドメトリ部２０は、ロボット１００の移動量を、モータ１８または台車１０１の車輪１０２の回転を検出するエンコーダ、または、ロボット１００に設けられたジャイロセンサ（または、ジャイロスコープ）などの内的センサからの検出信号に基づいて計算する周知の構成を有する。図２では、エンコーダ及びジャイロセンサがエンコーダ・ジャイロ部１９に含まれる。
In the
統合ツリーのメンテナンス部１４は、推定経路中の評価対象で下位のノードを統合ツリーから削除し、次に統合ツリー全体をスキャンして、子を持っていないノードを削除する。さらに１つの子だけを持っている親ノードを子ノードと併合する。統合ツリーは、記憶部１５に格納される。
The
統合器１０は、例えば、推定器４で推定された地図Ａの座標系の自己位置（または、推定器８で推定された地図Ｃの座標系の自己位置）と、推定器６で推定された地図Ｂの座標系の自己位置を統合する統合処理を行い、統合結果を例えばロボット１００の移動経路を計画あるいは決定する移動経路計画部１６に供給する。この例では、移動経路計画部１６が決定した移動経路は、上記ＣＰＵで形成可能な台車移動コントローラ１７に供給される。台車移動コントローラ１７は、移動経路に応じてモータ１８を駆動して車輪１０２を回線することで台車１０１を例えば図１の矢印方向へ移動させる。オドメトリ部２０で計算されたロボット１００の移動量は、各推定器４，６，８に供給される。
The
統合器１０は、例えば、推定器４で推定された地図Ａの座標系の自己位置と、推定器６で推定された地図Ｂの座標系の自己位置と、推定器８で推定された地図Ｃの座標系の自己位置を統合する統合処理を行っても良いことは言うまでもない。
The
ロボットシステム２００がロボット１００で形成されている場合、図２に示す各部はロボット１００に設けられる。一方、ロボットシステム２００がロボット１００及び当該ロボット１００を制御する当該ロボット１００とは別体の情報処理装置（すなわち、サーバなどのコンピュータ）で形成されている場合、ＬＲＦ１，２及びカメラ３以外の部分を情報処理装置に設けても、統合器１０を情報処理装置に設けても、統合器１０及び移動経路計画部１６を情報処理装置に設けても良い。情報処理装置は、ロボット１００と通信可能な通信部（または、インタフェース）、ＣＰＵなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。
When the
少なくとも統合器１０が行う統合処理を実行するプログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
Various known computer-readable storage media including a semiconductor storage device, a magnetic, optical, and magneto-optical recording medium can be used as a storage unit that stores at least a program for executing an integration process performed by the
図３及び図４は、推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。図３は任意の時刻ｔ１，ｔ２での処理、図４は任意の時刻ｔ３，ｔ４での処理を夫々示し、ｔ１＜ｔ２＜ｔ３＜ｔ４である。つまり、時刻ｔ１が最も早く、時刻ｔ４が最も遅い時刻である。図３及び図４において、左側が推定器側の処理を示し、右側が統合器側の処理を示す。図３及び図４では説明の便宜上、推定器４で推定された地図Ａの座標系の自己位置と、推定器６で推定された地図Ｂの座標系の自己位置を統合する統合処理を行うものとする。また、推定器４，６を夫々推定器Ａ，Ｂと図示し、推定器４のＩＤはＡ、推定器６のＩＤはＢで示す。
3 and 4 are diagrams for explaining how the integrated tree grows by sequential input from the estimator side. 3 shows processing at arbitrary times t1 and t2, and FIG. 4 shows processing at arbitrary times t3 and t4, respectively, where t1 <t2 <t3 <t4. That is, time t1 is the earliest time and time t4 is the latest time. 3 and 4, the left side shows processing on the estimator side, and the right side shows processing on the integrator side. 3 and 4, for convenience of explanation, an integration process for integrating the self-position of the coordinate system of the map A estimated by the
図３に示す時刻ｔ１では、統合器１０は、ステップＳＴ１で推定器Ａ，Ｂから入力した推定値をツリーの持つ情報のうち観測値へコピーし、ノードの持つの情報のうち状態ベクトルへコピーする。また、ステップＳＴ１は、推定器Ａ，Ｂから入力したオドメトリをツリーの持つ情報のうちオドメトリへコピーし、図３にハッチング付きの○印で表すルートノード（Root Node）を生成する。さらに、ステップＳＴ１は、ノードの持つ情報のうち推定器Ａ，Ｂの隠れマルコフモデル（ＨＭＭ：Hidden Markov Model）を初期化する。以下の説明では、ルートノードはハッチング付きの○印で表し、子ノードはハッチング無しの○印で表す。
At time t1 shown in FIG. 3, the
時刻ｔ２では、統合器１０は、ステップＳＴ２で推定器Ａ，Ｂから入力した推定値及びオドメトリをツリーの持つ情報の観測値及びオドメトリへコピーする。これにより、統合器１０は、ステップＳＴ３でオドメトリ及び状態ベクトルに基づきＨＭＭを用いて推定器Ｂへの信頼度を計算する。統合部１０は、ステップＳＴ４で計算された推定器Ｂへの信頼度が「高」、「中」、「低」であるか（この例では「中」）に応じて子ノードを追加し、各子ノードの状態ベクトルを更新すると共に、親ノード（この場合、ルートノード）のＨＭＭを各子ノードへコピーする。
At time t2, the
図４に示す時刻ｔ３では、統合器１０は、ステップＳＴ５で推定器Ａ，Ｂから入力した推定値及びオドメトリをツリーの持つ情報の観測値及びオドメトリへコピーする。これにより、統合器１０は、ステップＳＴ６で各ノードについて、上記ステップＳＴ３と同様にＨＭＭを用いて推定器Ａへの信頼度を計算し、上記ステップＳＴ４と同様に信頼度によって各ノードに子ノードを追加し、子ノードの状態ベクトルを更新すると共に、親ノードのＨＭＭを各子ノードへコピーする。
At time t3 shown in FIG. 4, the
時刻ｔ４では、統合器１０は、ステップＳＴ７で上記ステップＳＴ５と同様の処理を行い、ステップＳＴ８で上記ステップＳＴ６と同様の処理を行う。
At time t4, the
このようにして、種類の異なる複数の内的センサからの入力（計測情報）に基づいて自己位置を推定する複数の推定器の信頼度をＨＭＭを用いて計算する。各推定器に対応するＨＭＭの計算に用いるパラメータは、当該推定器からの入力（推定値及びオドメトリ）に含まれる。また、信頼度に応じてルートノードに子ノードを追加することで、統合ツリーを作成する。従って、記憶部１５に格納される統合ツリーの規模が成長して増大していく。この統合ツリーに含まれる最新のノード（最新レイヤーのノード）から過去のノードを辿ってルートノードまでの経路を評価して、削除対象するべきノードを検出して削除することで、統合ツリーの規模を一定以下に抑えることもできる。
In this way, the reliability of a plurality of estimators that estimate self-positions based on inputs (measurement information) from a plurality of different types of internal sensors is calculated using the HMM. Parameters used for calculation of the HMM corresponding to each estimator are included in inputs (estimated values and odometry) from the estimators. Also, an integrated tree is created by adding child nodes to the root node according to the reliability. Accordingly, the scale of the integrated tree stored in the
図５は、統合ツリーの規模を一定以下に抑えるための枝刈を説明する図である。図５中、ノードは図３及び図４と同様に図示されており、さらに、削除対象のノードを●印で表す。図５中、破線で囲まれたノードは、最新レイヤーのノードを示す。図５（ａ）に示す統合ツリーの推定経路の評価の結果、例えば図５（ｂ）に●印で表す不良ノードが検出されると、図５（ｃ）に示すようにこれらの不良ノードを削除する。次に、子ノードを持っていない図５（ｄ）に●印で表すノードを抽出する。この場合、子ノードを持っていないノードのうち、最新レイヤーに存在するノードは抽出の対象外とする。次に、図５（ｅ）に示すように、抽出された、子ノードを持っていないノードを削除する。さらに、図５（ｆ）に破線で囲んで示すように、１つの子だけを持つノードを抽出する。この場合も、１つの子だけを持つノードのうち、最新レイヤーに存在するノードは抽出の対象外とする。そして、図５（ｇ）に示すように、図５（ｆ）に破線で囲んで示す子ノードと親ノードを１つのノードにマージする。このようにして、この例では図５（ａ）に示す統合ツリーの規模を一定以下に抑えるための枝刈を行った結果、統合ツリーの規模は図５（ｇ）に示す如き規模となる。なお、統合ツリーの規模を一定以下に抑えるための枝刈の詳細については後述する。 FIG. 5 is a diagram for explaining pruning to keep the scale of the integrated tree below a certain level. In FIG. 5, the nodes are illustrated in the same manner as in FIGS. 3 and 4, and the nodes to be deleted are indicated by ● marks. In FIG. 5, nodes surrounded by broken lines indicate nodes of the latest layer. As a result of the evaluation of the estimated path of the integrated tree shown in FIG. 5A, for example, when defective nodes represented by ● are detected in FIG. 5B, these defective nodes are displayed as shown in FIG. delete. Next, a node represented by a mark ● in FIG. 5D that does not have a child node is extracted. In this case, among the nodes that do not have child nodes, nodes existing in the latest layer are excluded from extraction. Next, as shown in FIG. 5E, the extracted nodes that do not have child nodes are deleted. Further, a node having only one child is extracted as shown by being surrounded by a broken line in FIG. Also in this case, among the nodes having only one child, the nodes existing in the latest layer are excluded from extraction. Then, as shown in FIG. 5G, the child node and the parent node surrounded by a broken line in FIG. 5F are merged into one node. In this way, in this example, as a result of performing pruning to keep the scale of the integrated tree shown in FIG. 5 (a) below a certain level, the scale of the integrated tree becomes the scale shown in FIG. 5 (g). Details of pruning to keep the scale of the integrated tree below a certain level will be described later.
図６は、統合器の動作の概要を説明するフローチャートである。図６において、統合器１０は一点鎖線で囲まれた処理を実行し、（１）〜（２８）は図６の各処理でやり取りされる情報の流れの一例を示す。ステップＳ１０４〜Ｓ１２０は、統合部１０を形成するＣＰＵの各機能が実行する処理（または、手順）に相当する。推定器４，６，８の計測値と地図Ａ，Ｂ，Ｃとに基づいて推定されたロボット１００の位置姿勢は、（１），（２），（３）で示すように統合器１０に入力される。
FIG. 6 is a flowchart for explaining an outline of the operation of the integrator. In FIG. 6, the
統合器１０は、初めて推定器４からの入力を受け取った時、ｔ＝０として統合ツリーを初期化する。具体的には、図５に示したようなルートノードを１つ生成して、推定器４からの表１に示す如き入力をルートノードにコピーすると共に、表２に如きデータセットを持たせる。表２中、Ｎはロボットシステム２００が有する推定器に任意に割り当てられた「１」から開始する連番を示し、この例では推定器のＩＤ番号とは異なる。この例では、３つの推定器４，６，８を用いているので、表２において「推定器[1]」は推定器４、「推定器[2]」は推定器６、「推定器[N]」は推定器８を表す。
When the
（１） 推定器４は、表１の如きデータを統合器１０に入力する。
(1) The
（２） 推定器６は、表１の如きデータを統合器１０に入力する。
(2) The
（３） 推定器８は、表１の如きデータを統合器１０に入力する。
(3) The
（４） ステップＳ１０４では、統合器１０が推定器４，６，８からの最新入力を待ち合わせる入力待ち状態となり、新しい入力があるか否かを示すフラグｆ１を管理する。上記の如く、統合器１０への入力は表１に示す如き内容のデータである。ステップＳ１０５は、フラグｆ１が推定器からの新しい入力があることを示すと、新たな入力値を共有メモリに書き込む。共有メモリは、統合器１０内の記憶部で形成可能である。
(4) In step S104, the
（５） ステップＳ１０６では、統合器１０が共有メモリから入力値を読み出し、統合ツリーを初期化するか否かを判定する。統合器１０が初めて推定器４，６，８からの入力を受け取った場合、判定結果はＹＥＳであり上記の如く統合ツリーの初期化を行う。
(5) In step S106, the
（６） 統合器１０は、初期化を行うか否かを示すフラグｆ２と共有メモリから読み出した推定器からの入力値を後述するステップＳ１０７に入力する。
(6) The
（８） 統合器１０は、推定器からの入力に含まれているオドメトリ成分を後述するステップＳ１１０に入力する。
(8) The
（９） 統合器１０は、推定器からの入力に含まれているロボット１００の位置姿勢の推定結果成分を後述するステップＳ１１１に入力する。
(9) The
（１０） 統合器１０は、推定器からの入力に含まれている推定器のＩＤ、及びロボット１００の位置姿勢の推定結果成分を後述するステップＳ１１２に入力する。
(10) The
ステップＳ１０７では、統合器１０が上記の如く統合ツリーを初期化する。
In step S107, the
（７） ステップＳ１０８では、統合器１０が統合ツリーを統合器１０内の記憶部で形成可能な共有メモリに保持する。
(7) In step S <b> 108, the
（１１） 統合器１０は、共有メモリに保持された統合ツリーをステップＳ１０９に入力する。
(11) The
（１９） 統合器１０は、共有メモリに保持された統合ツリーを後述するステップＳ１１５に入力する。
(19) The
（１２） ステップＳ１０９では、統合器１０が統合ツリーの最新レイヤーの全ノードを取得する。統合ツリーが既に初期化されていれば、最新レイヤーにある全てのノードをリストとして取得する。統合器１０は、最新レイヤーから抽出したノードのリストをステップＳ１１０に入力する。ステップＳ１１０では、統合器１０がレイヤー上の全ノードの状態をオドメトリによる予測に基づいて更新する。具体的には、ステップＳ１０９で取得したノードのリスト（ノードリスト）の全てのノード（メンバー）に対して、図１０と共に後述する式（１１）、式（１２）に基づいて予測更新を行う。
(12) In step S109, the
（１３） 統合器１０は、図１０と共に後述する式（２０）を計算するための予測更新したフィルタ状態、及び予測した観測値の共分散行列をステップＳ１１１に入力する。ステップＳ１１１は、各ノードの持つＨＭＭによる入力値の信頼性評価を図１０と共に後述する式（２１）〜（２４）に基づいて行い、推定器から入力したロボット１００の位置姿勢の推定結果の信頼性のレベルを計算する。
(13) The
（１４） 位置姿勢の推定結果の信頼性のレベルが「高」であると、統合器１０は、信頼性のレベル、図１０と共に後述する式（１８），（１９）を計算するための予測更新したフィルタ状態、予測した観測値、及び予測した観測値の共分散行列をステップＳ１１２に入力する。信頼性のレベルが「高」であると、ステップＳ１１２では、統合器１０が信頼性のレベルが「高」と評価されたノードについてノードの状態を図１０と共に後述する式（１８），（１９）に基づいて更新する。
(14) When the reliability level of the position / orientation estimation result is “high”, the
（１５） 位置姿勢の推定結果の信頼性のレベルが「低」であると、統合器１０は、信頼性のレベル、及び予測更新したフィルタ状態を後述するステップＳ１１３に入力する。
(15) If the reliability level of the position / orientation estimation result is “low”, the
（１５−１） 位置姿勢の推定結果の信頼性のレベルが「中」であると、統合器１０は、信頼性のレベル、図１０と共に後述する式（１８），（１９）を計算するための予測更新したフィルタ状態、予測した観測値、及び予測した観測値の共分散行列をステップＳ１１２に入力する。信頼性のレベルが「中」であると、ステップＳ１１２では、統合器１０が信頼性のレベルが「中」と評価されたノードについてノードの状態を図１０と共に後述する式（１８），（１９）に基づいて更新する。
(15-1) When the reliability level of the position / orientation estimation result is “medium”, the
（１５−２） 位置姿勢の推定結果の信頼性のレベルが「中」であると、統合器１０はさらに、信頼性のレベル、及び予測更新したフィルタ状態を後述するステップＳ１１３に入力する。
(15-2) If the reliability level of the position / orientation estimation result is “medium”, the
（１６） 統合器１０は、観測更新したフィルタ状態をステップＳ１１３に入力する。ステップＳ１１３は、親ノードに追加する子ノードを生成する。
(16) The
（１７） 統合器１０は、新しいフィルタ状態を持った子ノードのリストをステップＳ１１４に入力する。これにより、ステップＳ１１４では、統合器１０が生成した子ノードを各親ノードに追加する。信頼性のレベルが「高」と評価されたノードには１つだけの子ノードを追加する。信頼性のレベルが「低」と評価されたノードにも１つだけの子ノードを追加する。信頼性のレベルが「中」と評価されたノードには２つの子ノードを追加する。
(17) The
図７は、統合ツリーの規模の増大を説明する図である。図７中、ＨＬ，ＭＬ，ＬＬは、夫々信頼性のレベルが「高」、「中」、「低」のノードを示す。例えば時刻ｔ＋１に推定器からの最新の入力があった時に、統合ツリーから時刻ｔに作成したノードを全て抽出する。ただし、ｔ＋１＝１の場合、抽出されるノードはルートノードのみとなる。次に、各ノードに対して以下の処理を行う。ここで、新しいノードΦを１つ作成しておき、現在処理しているノードをΨで表すものとする。ノードΨからオドメトリを抽出して、入力された最新のオドメトリと合わせて、図１０と共に後述する式（１）〜（６）のようにノードΨに保持している推定状態を更新する。更新した推定情報は、ノードΦに保存し、これと同時に、最新のオドメトリもノードΦに保存する。信頼性のレベルは、図１０と共に供述する式（２１）〜（２４）に基づいて計算することができる。
FIG. 7 is a diagram for explaining an increase in the scale of the integrated tree. In FIG. 7, HL, ML, and LL indicate nodes whose reliability levels are “high”, “medium”, and “low”, respectively. For example, when there is the latest input from the estimator at
信頼性のレベルが「高（ＨＬ）」の場合、図１０と共に後述する式（１６）〜（１９）に基づいて新しいノードΦのフィルタ状態の観測更新を行って、ノードΨに図７中時刻ｔ＋１の左側に示すように子ノードとして追加する。信頼性のレベルが「低（ＬＬ）」の場合、新しいノードΦのフィルタ状態の観測更新は行わず、ノードΨに図７中時刻ｔ＋１の中央に示すように子ノードとして直接追加する。信頼性のレベルが「中（ＭＬ）」の場合、図７中時刻ｔ＋１の右側に示すように新しいノードΦをコピーして、もう１つのノードΦ’を作成する。ノードΦ’のフィルタ状態の観測更新を行い、ノードΨに子ノードとして追加する。また、ノードΦのフィルタ状態の観測更新は行わず、ノードΨに２番目の子ノードとして直接追加する。 When the reliability level is “high (HL)”, the observation state of the filter state of the new node Φ is updated based on the equations (16) to (19) described later with reference to FIG. It is added as a child node as shown on the left side of t + 1. When the level of reliability is “low (LL)”, the observation update of the filter state of the new node Φ is not performed, and the node Ψ is directly added as a child node as shown in the center at time t + 1 in FIG. When the reliability level is “medium (ML)”, the new node Φ is copied to create another node Φ ′ as shown on the right side of the time t + 1 in FIG. Update the observation of the filter state of the node Φ ′ and add it as a child node to the node Ψ. Also, the observation update of the filter state of the node Φ is not performed, and the node Ψ is directly added as a second child node.
（１８） 統合器１０は、子ノードを追加した統合ツリーをステップＳ１０８に入力する。ステップＳ１０８では、統合器１０が統合ツリーを保持するメモリを更新する。統合ツリーは、統合器１０内の記憶部で形成可能である。
(18) The
（１９） 統合器１０は、メモリから統合ツリーを読み出してステップＳ１１５に入力する。ステップＳ１１５は、統合ツリーに含まれている最新レイヤー上にある全てのノードをリストとして取得する。ステップＳ１１５で取得されるノードは、ステップＳ１１４で統合ツリーに新たに追加した子ノードである。
(19) The
（２０） 統合器１０は、最新ノード、すなわち、ステップＳ１１４で新たに追加した子ノードを、ステップＳ１１６に入力する。ステップＳ１１６では、統合器１０がリスト内の各ノードに対して、ルートノードまで辿って得られた全経路を抽出する。
(20) The
（２１） 統合器１０は、抽出した全経路のリストをステップＳ１１７に入力する。ステップＳ１１７では、統合器１０が抽出した各経路に対して、オドメトリで測定した運動パターンとフィルタで推定した運動パターンを比較して、図１０と共に後述する式（２５）に基づいてリスト中の経路を評価する経路評価を行う。
(21) The
（２２） 統合器１０は、経路評価により得られた経路の良さを表す評価スコアのうち、評価スコアが閾値より低い下位のノードのリストをステップＳ１１８入力する。ステップＳ１１８では、統合器１０が下位のノードを統合ツリーから削除する。
(22) The
図８は、統合ツリーの最新レイヤーから不良ノードを削除する処理を説明する図である。最新レイヤーにＫ個のノードが含まれているとすると、Ｋが事前に設定した閾値Ｋthreshより大きくなるとノードの削除処理を行う。この場合、Ｋ−Ｋthresh個のノードを削除することになる。各ノードの対応する経路の評価スコアは、例えばΩ（Ｐ（Φｋ）），ｋ＝０，１，．．．，ｋ，．．．Ｋで表される。そこで、ノードを評価スコアΩ（Ｐ（Φｋ））の昇順でソートし、下位のＫ−Ｋthresh個のノードを最新レイヤーから削除することで、統合ツリーは図８（ａ）に示す状態から図８（ｂ）に示す状態に変化する。図８（ａ）では、削除対象の不良ノードを●印で表す。 FIG. 8 is a diagram illustrating processing for deleting a defective node from the latest layer of the integration tree. Assuming that K nodes are included in the latest layer, node deletion processing is performed when K is greater than a preset threshold value K thresh . In this case, K−K thresh nodes are deleted. The evaluation score of the path corresponding to each node is, for example, Ω (P (Φ k )), k = 0, 1,. . . , K,. . . Represented by K. Therefore, by sorting the nodes in the ascending order of the evaluation score Ω (P (Φ k )) and deleting the lower K−K thresh nodes from the latest layer, the integrated tree is in the state shown in FIG. The state changes to the state shown in FIG. In FIG. 8A, a defective node to be deleted is represented by a mark ●.
（２３） 統合器１０は、ステップＳ１１７の経路評価により得られた経路の良さを表す評価スコアをステップＳ１２０に入力する。ステップＳ１２０では、統合器１０が統合結果と信頼性の評価結果を出力する。具体的には、経路の評価スコアのトップノードが持っているフィルタ推定値を統合ツリーの処理結果とし、後述するアプリケーションに出力する。また、これと同時に、この処理結果と推定した信頼性のレベルを推定モジュールを形成する推定器にフィードバックする。
(23) The
（２４） 統合器１０は、ステップＳ１１８により下位ノードが削除された統合ツリーをステップＳ１０８に入力して、統合ツリーを保持するメモリを更新する。
(24) The
なお、ステップＳ１１９では、統合器１０が統合ツリーのメンテナンスを行う。図９は、統合ツリーのメンテナンスを説明する図である。統合ツリーのメンテナンスでは、図９（ａ）に示すように、最新レイヤーを除き、子ノードを持っていないノードを統合ツリーから抽出し、図９（ｂ）に示すように抽出した子ノードを統合ツリーから削除する。また、図９（ｃ）に破線で囲んで示すように、最新レイヤーを除き、１つの子だけを持つノードを統合ツリーから抽出する。そして、図９（ｄ）に示すように、図９（ｃ）に破線で囲んで示す子ノードと親ノードを１つの新しいノードにマージすることで、子ノードの保持しているデータを新しいノードのデータとする。このような統合ツリーのメンテナンスは、図５（ｄ）〜図５（ｆ）と共に説明した統合ツリーの規模を一定以下に抑えるための枝刈と同様である。
In step S119, the
（２５） 統合器１０は、メンテナンスを行った統合ツリーをステップＳ１０８に入力してメモリに保持する。
(25) The
（２６） 統合器１０は、経路の評価スコアのトップノードが持っているフィルタ推定値を統合ツリーの処理結果として、例えばアプリケーションを実行するステップＳ１２１に入力する。これにより、アプリケーションには、統合ツリー方式で推定したロボット１００の位置姿勢の情報を入力できる。アプリケーションは、例えば統合ツリーから推定したロボット１００の位置姿勢を用いてロボット１００の移動経路を計画あるいは決定する移動経路計画部１６が実行するものである。
(26) The
（２７） 統合器１０は、上記の一連の統合処理が終了したことを示すフラグｆ３をステップＳ１０４に入力する。
(27) The
（２８） 統合器１０は、統合ツリー方式で推定したロボット１００の位置姿勢、推定した信頼性のレベルを、推定モジュールへの診断結果として、推定モジュールを形成する推定器にフィードバックする。
(28) The
図１０は、統合器の動作をより詳細に説明するフローチャートである。図１０において、 (1)〜(48)は図１０の各ステップでやり取りされる情報の流れの一例を示す。ステップＳ２０１〜Ｓ２３４は、統合部１０を形成するＣＰＵの各機能が実行する処理（または、手順）に相当する。ステップＳ２０１は、ロボット１００またはロボット１００とは別体の情報処理装置から入力される統合処理開始コマンドに応答して統合処理を開始する。
FIG. 10 is a flowchart for explaining the operation of the integrator in more detail. 10, (1) to (48) show an example of the flow of information exchanged at each step of FIG. Steps S201 to S234 correspond to processes (or procedures) executed by each function of the CPU forming the
(1) 統合処理の開始を示す開始コマンドは、ステップＳ２０２に入力され、ステップＳ２０２は、推定器４，６，８からの入力待ち状態となり、推定器４，６，８から新しい入力があるか否かをチェックする。
(1) A start command indicating the start of the integration process is input to step S202, and step S202 enters a state of waiting for input from the
(2) 新しい入力があると、新しい入力があることを示すフラグｆ１をステップＳ２０３に入力する。ステップＳ２０３は、入力されたフラグが終了フラグであるか否かを判定する。ステップＳ２０３の判定結果がＮＯであると、フラグをステップＳ２０５に入力する。他方、ステップＳ２０３の判定結果がＹＥＳであると、ステップＳ２０４で統合処理は終了する。 (2) When there is a new input, a flag f1 indicating that there is a new input is input to step S203. In step S203, it is determined whether or not the input flag is an end flag. If the decision result in the step S203 is NO, a flag is inputted to the step S205. On the other hand, if the decision result in the step S203 is YES, the integration process ends in a step S204.
ステップＳ２０５は、入力されたフラグが新しい入力を示すフラグｆ１であるか否かを判定する。ステップＳ２０５の判定結果がＹＥＳであると処理はステップＳ２０６へ進み、判定結果がＮＯであると処理はステップＳ２０２へ戻る。 In step S205, it is determined whether or not the input flag is a flag f1 indicating a new input. If the determination result in step S205 is YES, the process proceeds to step S206, and if the determination result is NO, the process returns to step S202.
(5) ステップＳ２０５の判定結果がＮＯであると、新しい入力の待ちコマンドをステップＳ２０２に入力する。 (5) If the decision result in the step S205 is NO, a new input wait command is inputted to the step S202.
(6) ステップＳ２０５の判定結果がＹＥＳであると、推定器からの入力データを例えば上記の表１の如き形式でステップＳ２０６に入力する。ステップＳ２０６は、このような推定器からの入力データから推定器の推定情報（すなわち、推定結果）だけを抽出する。 (6) If the decision result in the step S205 is YES, the input data from the estimator is inputted to the step S206 in the format as shown in Table 1 above, for example. In step S206, only the estimation information (that is, the estimation result) of the estimator is extracted from the input data from such an estimator.
(7) ステップＳ２０６の後、データの抽出完了フラグｆ４、及び推定器の入力データをステップＳ２０７に入力する。ステップＳ２０７は、例えば上記の表１の如き形式の推定器の入力データからオドメトリを抽出する。 (7) After step S206, the data extraction completion flag f4 and the input data of the estimator are input to step S207. In step S207, for example, odometry is extracted from the input data of the estimator having the format shown in Table 1 above.
(8) ステップＳ２０６の後、推定器が推定したロボット１００の位置姿勢、及び推定器の推定精度を表す推定結果の共分散行列を後述するステップＳ２２３に入力する。
(8) After step S206, the position and orientation of the
(10) ステップＳ２０６の後、上記(8)と同様に、推定器が推定したロボット１００の位置姿勢、及び推定器の推定精度を表す推定結果の共分散行列を後述するステップＳ２１８に入力する。
(10) After step S206, as in (8) above, the position and orientation of the
(9) ステップＳ２０７の後、オドメトリの抽出が完了したことを示すフラグｆ５、及び推定器の入力データをステップＳ２０８に入力する。ステップＳ２０８は、入力元の推定器のＩＤ番号を識別して抽出する。 (9) After step S207, a flag f5 indicating that extraction of odometry is completed and input data of the estimator are input to step S208. In step S208, the ID number of the input estimator is identified and extracted.
(21) ステップＳ２０７の後、オドメトリを後述するステップＳ２１６に入力する。 (21) After step S207, odometry is input to step S216 described later.
(11) ステップＳ２０８の後、推定器のＩＤ番号の抽出が完了したことを示すフラグｆ６をステップＳ２０９に入力する。ステップＳ２０９は、統合ツリーの最新レイヤーにあるノードを全て抽出して、抽出したノードのノードリストを作成する。ここで、最新レイヤーとは、前回の処理で統合ツリーに追加した新しいノードを含むレイヤーを言う。 (11) After step S208, a flag f6 indicating that the extraction of the ID number of the estimator is completed is input to step S209. In step S209, all the nodes in the latest layer of the integration tree are extracted, and a node list of the extracted nodes is created. Here, the latest layer is a layer including a new node added to the integration tree in the previous process.
(12) ステップＳ２０８の後、抽出した推定器のＩＤ番号を後述するステップＳ２１４に入力する。 (12) After step S208, the extracted ID number of the estimator is input to step S214 described later.
(13) 抽出された、最新レイヤーにあるノードの数（ノード数）をステップＳ２１０に入力する。ステップＳ２１０は、ノード数がゼロ（０）であるか否かを判定し、判定結果がＹＥＳであれば統合ツリーが構築されていないことが分かるので処理はステップＳ２１１へ進み、判定結果がＮＯであれば処理はステップＳ２１２へ進む。 (13) The number of extracted nodes in the latest layer (number of nodes) is input to step S210. In step S210, it is determined whether or not the number of nodes is zero (0). If the determination result is YES, it is understood that an integrated tree has not been constructed, and thus the process proceeds to step S211. The determination result is NO. If so, the process proceeds to step S212.
(14) ステップＳ２１０の判定結果がＹＥＳであると、統合ツリーの初期化コマンドをステップＳ２１１に入力する。ステップＳ２１１は、初期化コマンドに応答して統合ツリーを上記の如く初期化する。 (14) If the decision result in the step S210 is YES, an integration tree initialization command is inputted to the step S211. Step S211 initializes the integration tree as described above in response to the initialization command.
(15) ステップＳ２１１の後、入力待ちコマンドをステップＳ２０２に入力する。 (15) After step S211, an input waiting command is input to step S202.
(16) ステップＳ２１０の判定結果がＮＯであると、作成したノードリストをステップＳ２１２に入力する。ステップＳ２１２は、ノードリストからノードを１つ抽出して以下に説明するステップＳ２１３，Ｓ２１４，Ｓ２１５の処理を開始し、抽出したノードをＮＤとする処理を、ノードリスト中の未処理の各ノードに対して行う。 (16) If the decision result in the step S210 is NO, the created node list is inputted to the step S212. In step S212, one node is extracted from the node list and the processing of steps S213, S214, and S215 described below is started, and the processing of setting the extracted node as ND is performed on each unprocessed node in the node list. Against.
(17) ステップＳ２１２の後、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１３に入力する。 (17) After step S212, the node ND having the data set shown in Table 2 is input to step S213.
(18) ステップＳ２１２の後、上記(17)と同様にして、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１４に入力する。 (18) After step S212, the node ND having the data set shown in Table 2 is input to step S214 in the same manner as in (17) above.
(19) ステップＳ２１２の後、上記(17)と同様にして、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１５に入力する。 (19) After step S212, the node ND having the data set shown in Table 2 is input to step S215 in the same manner as in (17) above.
ステップＳ２１３は、ノードＮＤの持つフィルタデータからこれまでにフィルタが推定したロボット１００の位置姿勢、共分散行列、及びオドメトリを抽出する。
In step S213, the position and orientation of the
(20) ステップＳ２１３の後、フィルタがこれまでに推定したロボット１００の位置姿勢、及びオドメトリをステップＳ２１６に入力する。
(20) After step S213, the position and orientation of the
ステップＳ２１４は、ステップＳ２０８の後(12)で入力した推定器のＩＤ番号を使ってノードＮＤから推定器ＩＤに対応したＨＭＭを抽出する。ノードＮＤは、推定器数分のＨＭＭを持っているが、この処理では、統合器１０にデータを出力した推定器の対応するＨＭＭだけを抽出する。
In step S214, the HMM corresponding to the estimator ID is extracted from the node ND using the ID number of the estimator input in (12) after step S208. The node ND has HMMs corresponding to the number of estimators. In this process, only the HMMs corresponding to the estimators that output data to the
(25) ステップＳ２１４の後、ＨＭＭデータ、すなわち、これまで計算した状態「１」にある確率、状態「２」にある確率をステップＳ２１８に入力する。ＨＭＭデータについては、図１３と共に供述する。 (25) After step S214, the HMM data, that is, the probability in state “1” and the probability in state “2” calculated so far are input to step S218. The HMM data will be described together with FIG.
ステップＳ２１５は、ノードからこれまでの経路の評価スコアを抽出する。具体的には、以下の経路評価処理を行うために、ノードＮＤに保存されているルートノードからの経路に対する評価スコアを抽出する。 In step S215, the evaluation score of the route so far is extracted from the node. Specifically, in order to perform the following route evaluation process, the evaluation score for the route from the route node stored in the node ND is extracted.
(48) ステップＳ２１５の後、ノードＮＤまでの経路評価スコアを後述するステップＳ２２９に入力する。 (48) After step S215, the route evaluation score to the node ND is input to step S229 described later.
ステップＳ２１６は、オドメトリで位置姿勢を予測更新する。具体的には、(21)で入力した新しいオドメトリと、(20)でノードＮＤに保存されているオドメトリを使って、以下に説明する式（１５）までの処理を行う。 In step S216, the position and orientation are predicted and updated by odometry. Specifically, using the new odometry input in (21) and the odometry stored in the node ND in (20), processing up to equation (15) described below is performed.
図１３は、隠れマルコフモデル（ＨＭＭ）を説明する図である。ＨＭＭは、各推定器の性能に合わせて構築される。図１３に示すようなHMMでは、状態「１」は推定器の入力が信頼できる状態、状態「２」推定器の入力が信頼できない状態を表す。観測シンボル「０」は、推定器の入力がフィルタの予測ゲート外にあることを示し、観測シンボル「１」は推定器の入力がフィルタの予測ゲート内にあることを示す。 FIG. 13 is a diagram for explaining a hidden Markov model (HMM). The HMM is constructed according to the performance of each estimator. In the HMM as shown in FIG. 13, the state “1” represents a state where the input of the estimator is reliable, and the state “2” represents a state where the input of the estimator is not reliable. Observation symbol “0” indicates that the input of the estimator is outside the prediction gate of the filter, and observation symbol “1” indicates that the input of the estimator is within the prediction gate of the filter.
次に、ＨＭＭのパラメータの設定について説明する。ＨＭＭの状態遷移確率というパラメータは、ＨＭＭを使って評価する対象の推定器の性能によって決める。例えば、ＬＲＦによる自己位置推定と、カメラによる（例えば、画像ランドマークを用いた）自己位置推定を行う２つの推定器（図１の推定器４または６と、推定器８に相当）があるものとする。この場合、２つの推定器は以下のような性質ｃ１，ｃ２を有する。
Next, setting of HMM parameters will be described. The HMM state transition probability parameter is determined by the performance of the estimator to be evaluated using the HMM. For example, there are two estimators (equivalent to the
ｃ１：ＬＲＦの計測情報を使って自己位置推定を行う際、前回のロボット１００の推定位置の近傍で今回の新しい位置を探す。この場合、前回の推定位置が間違っていたら、今回探した結果も間違いである可能性が高い。
c1: When self-position estimation is performed using LRF measurement information, the current new position is searched in the vicinity of the previous estimated position of the
ｃ２：逆に、画像ランドマークを使う場合、前回のロボット１００の推定位置が間違っていたとしても、今回推定した結果は真の位置に近づいていく。
c2: On the contrary, when using the image landmark, even if the previous estimated position of the
このため、状態遷移マトリクスでは、例えば
ＬＲＦの場合： a00=0.90, a01=0.10, a10=0.01,a11=0.99;
カメラの場合： a00=0.95, a01=0.05, a10=0.10,a11=0.90;
に設定される。ここで、
a00： 前回は信頼でき、今回も信頼できる確率を示し、ＬＲＦとカメラの特性からカメラの方はより高く設定されている。
a01： 前回は信頼でき、今回は信頼できない確率を示し、a00+a01=1となるように設定している。
a10： 前回は信頼できず、今回は信頼できる確率（リカバリー性能）を示し、ＬＲＦの場合はリカバリー機能が弱いため、カメラに比べで低く設定されている。
a11： 前回は信頼できず、今回も信頼できない確率を示し、a10+a11=1となるように設定される。
図１０のステップＳ２１６以降の処理の説明に戻る。
Therefore, in the state transition matrix, for example, in the case of LRF: a00 = 0.90, a01 = 0.10, a10 = 0.01, a11 = 0.99;
For the camera: a00 = 0.95, a01 = 0.05, a10 = 0.10, a11 = 0.90;
Set to here,
a00: The previous time is reliable and this time also indicates the probability of being reliable, and the camera is set higher from the characteristics of the LRF and the camera.
a01: The last time is reliable, and this time is an unreliable probability, and a00 + a01 = 1 is set.
a10: Unreliable last time, this time shows a reliable probability (recovery performance), and in the case of LRF, the recovery function is weak, so it is set lower than the camera.
a11: The probability that the previous time is unreliable and this time is also unreliable is set so that a10 + a11 = 1.
Returning to the description of the processing after step S216 in FIG.
(22) ステップＳ２１６の後、推定器の推定値に対しての予測値、及び予測ゲート（上記の式（１４）と式（１５）の計算結果）のデータをステップＳ２１８に入力する。 (22) After step S216, the predicted value for the estimated value of the estimator and the data of the prediction gate (the calculation results of the above equations (14) and (15)) are input to step S218.
(23) ステップＳ２１６の後、上記の式（１１）と式（１２）の計算結果のデータをステップＳ２１７に入力する。ステップＳ２１７は、新しいノードを１つ作成し、そのノードに(23)で入力されるデータを保存する。 (23) After step S216, the calculation result data of the above equations (11) and (12) is input to step S217. In step S217, one new node is created, and the data input in (23) is stored in that node.
(24) ステップＳ２１７の後、新しいノードのデータをステップＳ２２２に入力する。 (24) After step S217, the new node data is input to step S222.
ステップＳ２１８は、上記の式（２０）に基づいて入力した推定器の推定値の尤度を計算し、尤度を上記の式（２２）のように２値化することで、観測シンボルを生成する。 Step S218 calculates the likelihood of the estimated value of the estimator input based on the above equation (20), and generates an observation symbol by binarizing the likelihood as in the above equation (22). To do.
(26) ステップＳ２１８の後、２値化の結果である観測シンボルをステップＳ２１９に入力する。ステップＳ２１９は、上記の式（２１）に基づいてＨＭＭの各状態を更新する。 (26) After step S218, the observation symbol that is the result of binarization is input to step S219. In step S219, each state of the HMM is updated based on the above equation (21).
(27) ステップＳ２１９の後、上記の式（２１）の計算結果から得られたＨＭＭの各状態にある尤度をステップＳ２２０に入力する。ステップＳ２２０は、以下の式（２３）に基づいて信頼性を算出し、以下の式（２４）に基づいて信頼性のレベルを算出する。 (27) After step S219, the likelihood in each state of the HMM obtained from the calculation result of the above equation (21) is input to step S220. In step S220, the reliability is calculated based on the following equation (23), and the level of reliability is calculated based on the following equation (24).
(28) ステップＳ２２０の後、式（２４）に基づいて算出された信頼性のレベルεｔをステップＳ２２１に入力する。ステップＳ２２１は、信頼性のレベルεｔがＨＬ（高）、ＭＬ（中）、ＬＬ（低）のいずれかであるかを判定する。ステップＳ２２１の判定結果がＨＬであると、処理はステップＳ２２２へ進む。ステップＳ２２１の判定結果がＬＬであると、処理はステップＳ２２４へ進む。また、ステップＳ２２１の判定結果がＭＬであると、処理はステップＳ２２２及びステップＳ２２４へ進む。 (28) After step S220, the reliability level ε t calculated based on the equation (24) is input to step S221. In step S221, it is determined whether the reliability level ε t is HL (high), ML (medium), or LL (low). If the determination result of step S221 is HL, the process proceeds to step S222. If the determination result of step S221 is LL, the process proceeds to step S224. If the determination result of step S221 is ML, the process proceeds to step S222 and step S224.
(29) ステップＳ２２１の判定結果がＨＬであると、ＨＬを示す処理フラグをステップＳ２２２に入力する。 (29) If the determination result in step S221 is HL, a processing flag indicating HL is input to step S222.
(30) ステップＳ２２１の判定結果がＬＬであると、ＬＬを示す処理フラグをステップＳ２２４に入力する。 (30) If the determination result in step S221 is LL, a processing flag indicating LL is input to step S224.
(31) ステップＳ２２１の判定結果がＭＬであると、ＭＬを示す処理フラグをステップＳ２２２及びステップＳ２２４に入力する。 (31) If the determination result in step S221 is ML, a processing flag indicating ML is input to steps S222 and S224.
ステップＳ２２２は、新しいノードを１つ作成して(24)で入力されたデータをそのノードに保存することで、新しいノードを複製する。 In step S222, one new node is created, and the data input in (24) is stored in the node, thereby duplicating the new node.
(32) ステップＳ２２２の後、複製したノードのデータをステップＳ２２３に入力する。ステップＳ２２３は、上記の式（１６）〜式（１９）に基づいて複製したノードの状態を更新すると共に、式（１８）及び式（１９）の計算結果のデータを複製したノードに保存する。 (32) After step S222, the copied node data is input to step S223. In step S223, the state of the duplicated node is updated based on the above equations (16) to (19), and the calculation result data of equations (18) and (19) is stored in the duplicated node.
(33) ステップＳ２２３の後、状態を更新した複製ノードのデータをステップＳ２２５に入力する。 (33) After step S223, the data of the replication node whose state has been updated is input to step S225.
一方、ステップＳ２２４は、新しいノードを１つ作成して(24)で入力されたデータをそのノードに保存することで、新しいノードを複製する。 On the other hand, in step S224, one new node is created, and the data input in (24) is stored in the node, thereby duplicating the new node.
(34) ステップＳ２２４の後、新しいノードに保存したデータをステップＳ２２５に入力する。ステップＳ２２５は、図７と共に説明したように、ＨＬの場合はステップＳ２２３で処理したノード１つ、ＬＬの場合はステップＳ２２４で複製したノード１つを親ノードＮＤに子ノードとして追加し、ＭＬの場合がステップＳ２２３及びステップＳ２２３で処理した２つのノードを親ノードＮＤの子ノードとして追加する、ノードを子ノードとして親ノードに追加する処理を行う。 (34) After step S224, the data stored in the new node is input to step S225. In step S225, as described with reference to FIG. 7, in the case of HL, one node processed in step S223 is added, and in the case of LL, one node copied in step S224 is added as a child node to the parent node ND. In some cases, the two nodes processed in step S223 and step S223 are added as child nodes of the parent node ND, and the node is added as a child node to the parent node.
(35) ステップＳ２２５の後、処理したノードＮＤのデータをステップＳ２２６に入力する。ステップＳ２２６は、全てのノードの処理が完了したか否かを判定し、判定結果がＮＯであると(36)で処理はステップＳ２１２へ戻り、判定結果がＹＥＳであると処理はステップＳ２２７へ進む。具体的には、ステップＳ２２６は、(16)で入力されたノードリストからノードＮＤを削除し、ノードリスト中の残りのノード数がゼロ（０）であるか否かを判定する。残りのノード数がゼロであれば判定結果がＹＥＳであり、残りのノード数がゼロでなければ判定結果がＮＯである。 (35) After step S225, the processed node ND data is input to step S226. In step S226, it is determined whether or not all nodes have been processed. If the determination result is NO (36), the process returns to step S212, and if the determination result is YES, the process proceeds to step S227. . Specifically, in step S226, the node ND is deleted from the node list input in (16), and it is determined whether the number of remaining nodes in the node list is zero (0). If the number of remaining nodes is zero, the determination result is YES. If the number of remaining nodes is not zero, the determination result is NO.
(37) ステップＳ２２６の判定結果がＹＥＳであると、全てのノードの処理が完了したことを示す処理完了フラグをステップＳ２２７に入力する。ステップＳ２２７は、統合ツリーから最新レイヤーの全てのノードを抽出してノードリストを作成する。この場合、最新ノードは、ステップＳ２２５で追加したばかりの子ノードである。 (37) If the decision result in the step S226 is YES, a process completion flag indicating that the processes of all the nodes are completed is inputted to the step S227. In step S227, all nodes in the latest layer are extracted from the integration tree to create a node list. In this case, the latest node is the child node just added in step S225.
(38) ステップＳ２２７の後、ノードリストをステップＳ２２８に入力する。ステップＳ２２８は、ノードリストから未処理のノードを１つを抽出する。 (38) After step S227, the node list is input to step S228. In step S228, one unprocessed node is extracted from the node list.
(39) ステップＳ２２８の後、抽出されたノードをステップＳ２２９に入力する。ステップＳ２２９は、上記の式（１２）に基づいて、抽出されたノードからルートノードまで辿った経路の良さを表す評価スコアを更新し、式（１２）の計算結果をそのノードに保存する。 (39) After step S228, the extracted node is input to step S229. In step S229, the evaluation score representing the goodness of the route traced from the extracted node to the root node is updated based on the equation (12), and the calculation result of the equation (12) is stored in the node.
(40) ステップＳ２２９の後、処理されたノードのデータをステップＳ２３０に入力する。ステップＳ２３０は、全てのノードの処理が完了したか否かを判定し、判定結果がＮＯであると処理はステップＳ２２８へ戻り、判定結果がＹＥＳであると処理はステップＳ２３１へ進む。具体的には、ステップＳ２３０は、(38)で入力されたノードリストから、(40)で入力された処理されたノードを削除し、ノードリスト中の残りのノード数がゼロ（０）であるか否かを判定する。残りのノード数がゼロであれば判定結果がＹＥＳであり、残りのノード数がゼロでなければ判定結果がＮＯである。 (40) After step S229, the processed node data is input to step S230. In step S230, it is determined whether or not all nodes have been processed. If the determination result is NO, the process returns to step S228, and if the determination result is YES, the process proceeds to step S231. Specifically, in step S230, the processed node input in (40) is deleted from the node list input in (38), and the number of remaining nodes in the node list is zero (0). It is determined whether or not. If the number of remaining nodes is zero, the determination result is YES. If the number of remaining nodes is not zero, the determination result is NO.
(41) ステップＳ２３０の判定結果がＹＥＳであると、処理完了フラグ、各経路とその経路への評価スコアをステップＳ２３１に入力する。 (41) If the decision result in the step S230 is YES, a process completion flag, each route and an evaluation score for the route are inputted in a step S231.
(42) ステップＳ２３０の判定結果がＮＯであると、ノードリスト中の残りのノードのデータをステップＳ２２８に入力する。 (42) If the decision result in the step S230 is NO, the data of the remaining nodes in the node list are inputted to the step S228.
ステップＳ２３１は、各経路への評価スコアを昇順でソートすると共に、評価スコアが低い下位の経路に含まれている子ノードを抽出してリストを作成する。 In step S231, the evaluation scores for each route are sorted in ascending order, and a child node included in a lower route having a low evaluation score is extracted to create a list.
(43) ステップＳ２３１の後、削除対象となる、評価スコアが低い下位の経路に含まれている子ノードのリストをステップＳ２３２に入力する。ステップＳ２３２は、図８と共に説明した処理を行うことで、統合ツリーから(43)で入力されたリストに含まれている子ノードを削除する、子ノード削除処理を行う。 (43) After step S231, a list of child nodes included in the lower path with a low evaluation score to be deleted is input to step S232. In step S232, a child node deletion process is performed in which the child node included in the list input in (43) is deleted from the integration tree by performing the processing described with reference to FIG.
(44) ステップＳ２３２の後、子ノードを削除した統合ツリーのデータをステップＳ２３３に入力する。ステップＳ２３３は、図９と共に説明したように、統合ツリーのメンテナンス処理を行う。具体的には、統合ツリーの最新レイヤーからルートノードまでを辿って（スキャンして）、子ノードを持っていないノードを削除する。また、子が１つだけの親ノードを子ノードとマージする。 (44) After step S232, the integrated tree data from which the child nodes have been deleted is input to step S233. In step S233, the integrated tree maintenance process is performed as described with reference to FIG. Specifically, the node that does not have a child node is deleted by tracing (scanning) from the latest layer of the integration tree to the root node. In addition, a parent node having only one child is merged with the child node.
(45) ステップＳ２３３の後、メンテナンスを行った統合ツリーのデータをステップＳ２３４に入力する。ステップＳ２３４は、信頼性評価結果及び統合結果出力処理を行う。具体的には、ステップＳ２２０で計算して(28)で入力されたデータ、及び経路の評価スコアが最大値のノードにあるロボット１００の位置姿勢の状態を共有メモリに書き込み、アプリケーションと共有する。アプリケーションは、例えば統合ツリーから推定したロボット１００の位置姿勢を用いてロボット１００の移動経路を計画あるいは決定する移動経路計画部１６が実行するものである。
(45) After step S233, the maintenance-integrated tree data is input to step S234. In step S234, reliability evaluation result and integration result output processing is performed. Specifically, the data calculated in step S220 and input in (28) and the position / posture state of the
(46) ステップＳ２３４の後、入力待ちコマンドをステップＳ２０２に入力する。 (46) After step S234, an input waiting command is input to step S202.
(47) ステップＳ２３４の後、推定器への信頼性のレベル、及び統合ツリーから推定したロボットの位置姿勢の結果をステップＳ２３５に入力する。ステップＳ２３５は、統合器１０の外部、例えば、図２に示す移動経路計画部１６により実行される。ステップＳ２３５は、推定器への信頼性のレベル、及び統合ツリーから推定したロボットの位置姿勢の結果に基づいて、例えば移動経路計画などを実行する。
(47) After step S234, the level of reliability to the estimator and the result of the position and orientation of the robot estimated from the integration tree are input to step S235. Step S235 is executed outside the
図１４は、統合ツリーの一例を示す図である。図１４中、ノードは○印で表し、最新ノードは◎印で表すと共に、◎印の中央部分が濃い程経路の評価スコアが高いことを示す。また、隣り合う２本の水平線で囲まれた帯状の領域は１つのレイヤーを示し、ノードの横に示されている数字は経路番号を示す。 FIG. 14 is a diagram illustrating an example of an integrated tree. In FIG. 14, a node is represented by a circle, a latest node is represented by a circle, and the darker the central portion of the circle, the higher the evaluation score of the route. A band-like region surrounded by two adjacent horizontal lines indicates one layer, and a number shown beside the node indicates a path number.
図１５は、図１４に示す統合ツリーの各ノードに対する経路評価で得られる評価スコアの一例を示す図である。図１５では、１行に１０個の異なるノード番号の評価スコアが示されている。ノードのノード番号は、例えば統合ツリーの最上位レイヤーから最下位レイヤーまで、各レイヤーにおいて図１４中左から右の順番で各ノードに割り当てられる。 FIG. 15 is a diagram showing an example of an evaluation score obtained by route evaluation for each node of the integrated tree shown in FIG. In FIG. 15, evaluation scores of 10 different node numbers are shown in one line. The node numbers of the nodes are assigned to the nodes in the order from the left to the right in FIG. 14, for example, from the highest layer to the lowest layer of the integrated tree.
図１６は、比較例における統合結果の一例を説明する図である。図１６及び後述する図１７は、ロボット１００が部屋の中を自律移動した場合に自己位置の推定結果から得られる移動経路の平面図を示す。図１６及び図１７において、部屋の中にある５個の四辺形は、例えば机である。
FIG. 16 is a diagram illustrating an example of the integration result in the comparative example. FIG. 16 and FIG. 17 to be described later show a plan view of a movement path obtained from the estimation result of the self position when the
図１６中、（ａ）はカメラ３を用いる推定器８によるロボット１００の自己位置の推定結果に基づく経路を一点鎖線Ｌ１で示し、（ｂ）は５．０ｍの計測範囲を有するＬＲＦ２を用いる推定器６によるロボット１００の自己位置の推定結果に基づく経路を二点鎖線Ｌ２で示し、（ｃ）は従来のカルマンフィルタを用いた（すなわち、統合ツリーを用いない）統合結果に基づく経路を実線Ｌ３で示す。また、破線で囲まれた箇所は、図１６（ａ）では経路が精度良く推定されているものの、図１６（ｂ）では経路の精度が著しく低下しているため、図１６（ａ），（ｂ）が統合された図１６（ｃ）に示す経路では統合結果が図１６（ｂ）の精度が著しく低下した経路の悪影響を受けて精度が著しく低下している。このように、比較例では、統合結果の特に破線で囲まれた箇所での経路の精度が低いことが確認された。
16A shows a path based on the estimation result of the self-position of the
図１７は、実施例における統合結果の一例を説明する図である。図１７中、（ａ）はカメラ３を用いる推定器８によるロボット１００の自己位置の推定結果に基づく経路を一点鎖線Ｌ１で示し、（ｂ）は５．０ｍの計測範囲を有するＬＲＦ２を用いる推定器６によるロボット１００の自己位置の推定結果に基づく経路を二点鎖線Ｌ２で示し、（ｃ）は上記実施例の統合ツリーを用いた統合結果に基づく経路を実線Ｌ３０で示す。また、破線で囲まれた箇所は、図１７（ａ）では経路が精度良く推定されているものの、図１７（ｂ）では経路の精度が著しく低下している箇所を示す。しかし、本実施例によれば、図１７（ｃ）に示すように、統合ツリーを用いるため、統合結果の破線で囲まれた箇所においても、経路の良い精度が維持されることが確認された。つまり、複数の推定器が推定した位置姿勢を高精度に統合することが可能であることが確認された。
FIG. 17 is a diagram illustrating an example of the integration result in the embodiment. 17A shows a path based on the estimation result of the self-position of the
上記の比較例の場合、統合結果が例えば破綻した推定器の推定結果の影響を受けて、最悪の場合は統合結果が使えない程推定経路の精度が低下してしまう。一方、上記実施例のように統合ツリーを用いることで、統合ツリーから例えば破綻した推定器を特定して隔離することができる。このため、統合結果が例えば破綻した推定器の推定結果の影響を受けないように処理を行うことができ、ロボットシステムまたはナビゲーションシステムの安定性を大きく向上することができる。 In the case of the above comparative example, the integration result is affected by, for example, the estimation result of the broken estimator, and in the worst case, the accuracy of the estimation path is lowered so that the integration result cannot be used. On the other hand, by using the integrated tree as in the above embodiment, for example, a failed estimator can be identified and isolated from the integrated tree. For this reason, it is possible to perform processing so that the integration result is not affected by the estimation result of the broken estimator, for example, and the stability of the robot system or the navigation system can be greatly improved.
次に、上記実施例における入力情報の信頼性評価、信頼性に基づき異なる統合処理、統合ツリーの枝刈、及び診断機能のフィードバックについて説明する。 Next, the reliability evaluation of input information, the different integration processing based on the reliability, the pruning of the integration tree, and the feedback of the diagnosis function in the above embodiment will be described.
入力情報の信頼性評価：
入力情報の信頼性評価にＨＭＭを用いることで、推定器からの入力情報に対する信頼性を評価する。統合器が各推定器からの位置、姿勢、位置姿勢共分散行列といった入力情報に対して、信頼性という状態量を追加して、入力情報を位置、姿勢、位置姿勢共分散行列、信頼性に拡張する。信頼性については、統合器が過去の推定履歴に基づいて推定する。また、各推定器が上記の推定情報と共に、推定を行った地点でのオドメトリ位置も入力する。
Reliability evaluation of input information:
The reliability of the input information from the estimator is evaluated by using the HMM for the reliability evaluation of the input information. The integrator adds a state quantity called reliability to the input information such as position, posture, and position / posture covariance matrix from each estimator, and converts the input information into position, posture, position / posture covariance matrix, and reliability. Expand. The reliability is estimated by the integrator based on the past estimation history. Each estimator also inputs the odometry position at the point where the estimation is performed together with the above estimation information.
統合器は、推定器数分の信頼性評価モデルを用意する。信頼性評価モデルは、ＨＭＭに基づいて構築する。ＨＭＭは２つの状態を持ち、各状態において２つのシンボルが観測できる。このシンボルは実際の入力値が統合器の統合履歴から予測した新しい入力値の近傍に入るかどうかを表す「０」と「１」の２値である。このシンボルが観測されると、ＨＭＭ内部の状態を更新する。この２つの状態の確率の比から信頼性を算出する。 The integrator prepares as many reliability evaluation models as the number of estimators. The reliability evaluation model is constructed based on the HMM. The HMM has two states, and two symbols can be observed in each state. This symbol is a binary value of “0” and “1” indicating whether or not the actual input value is in the vicinity of the new input value predicted from the integration history of the integrator. When this symbol is observed, the state inside the HMM is updated. Reliability is calculated from the ratio of the probabilities of these two states.
ＨＭＭのパラメータは、各推定器の性能に合わせて設定される。例えばカメラを用いる推定器のエラーリカバリー機能がＬＲＦを用いる推定器より高く、信頼できない状態から信頼できる状態への遷移確率が高く設定されている。 HMM parameters are set according to the performance of each estimator. For example, the error recovery function of an estimator using a camera is higher than that of an estimator using LRF, and the transition probability from an unreliable state to a reliable state is set high.
信頼性に基づき異なる統合処理：
信頼性に基づき異なる統合処理は、異なる信頼性の入力情報に対して、統合ツリーを利用して異なる統合処理を行う。統合器が推定した信頼性を、例えば「高」、「中」、「低」の３つのレベルに離散化し、各レベルによって異なる統合処理を行う。信頼性の各レベルに対して異なる処理を行うために統合ツリーを導入する。統合ツリーの深さ方向は時間方向に対応する。統合ツリーの最も低いレイヤーが最新時間に対応する。統合ツリーの各ノードがそのノードの対応する時刻での統合結果であり、統合して得られたロボットの現在位置が含まれている。また、ノードは推定器から新しい入力がくる度に作成され、その時入力されたオドメトリの情報も含まれている。推定器から新しい入力がきた時、現在のノードに新しい子ノードを追加して、親ノードの統合結果を更新して子ノードに保存する。
Different integration processes based on reliability:
Different integration processes based on reliability perform different integration processes on input information with different reliability by using an integration tree. The reliability estimated by the integrator is discretized into, for example, three levels of “high”, “medium”, and “low”, and different integration processing is performed for each level. Introduce an integration tree to do different processing for each level of reliability. The depth direction of the integration tree corresponds to the time direction. The lowest layer in the integration tree corresponds to the latest time. Each node of the integration tree is an integration result at the time corresponding to the node, and the current position of the robot obtained by the integration is included. A node is created each time a new input is received from the estimator, and includes information on the odometry input at that time. When a new input is received from the estimator, a new child node is added to the current node, and the integration result of the parent node is updated and stored in the child node.
子ノードの追加は、例えば以下のように行う。先ず、現在最新のレイヤーにある全てのノードに対して、推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加する。入力情報を観測情報として、親ノードの状態を更新して子ノードに保存する。さらに、新しい入力に含まれているオドメトリの情報も子ノードに保存する。 For example, child nodes are added as follows. First, when it is determined that the reliability level of the new input from the estimator is “high” for all nodes currently in the latest layer, only one child node is added to the parent node. Using the input information as observation information, the state of the parent node is updated and stored in the child node. Further, the odometry information included in the new input is also stored in the child node.
信頼性のレベルが「低」であると判断した場合、親ノードに１つのみの子ノードを追加する。入力情報を観測情報とせず、親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存する。 When it is determined that the reliability level is “low”, only one child node is added to the parent node. The input information is not used as observation information, and the state of the parent node is updated using only odometry information and stored in the child node.
信頼性のレベルが「中」であると判断した場合、信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果親ノードが２つの子を持つようになる。 When it is determined that the reliability level is “medium”, the processing when the reliability level is determined to be “high” and “low” is performed, and as a result, the parent node has two children. .
このようにして、新しい入力が逐時に入ることによって統合ツリーのノード数と深さが増大していき、ツリーの規模が大きくなる。 In this way, the number of nodes and the depth of the integrated tree increase as new inputs enter each time, and the scale of the tree increases.
統合ツリーの枝刈：
統合ツリーの枝刈は、推定した経路の速度パターンによる評価に基づき、統合ツリーの規模を一定以下に抑えるために行う。この統合ツリーの枝刈を行うため、ツリーの最新ノードからルートノードまでの経路の良さを評価する。評価スコアが低い経路の最新ノードは削除する。なお、経路とは最新レイヤーにあるノードからツリーのルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスである。経路上の各ノードに保存されているオドメトリから速度パターンを求め、これと各ノードで統合処理によって得られたロボットの位置姿勢から求めた速度パターンとの類似度を計算して評価スコアとする。各経路に対して評価を行って得られた評価スコアで各経路を昇順にソートして、下位の経路に対して最新ノードを統合ツリーから削除する。その後、統合ツリーの全体をスキャンして、子を持っていないノードを削除する。１つの子だけを持っている親ノードを子ノードと併合して、新しいノードを生成する。新しいノードでは親ノードの情報がなくなり、子ノードの情報のみを保存する。
Integrated tree pruning:
The pruning of the integrated tree is performed in order to keep the scale of the integrated tree below a certain level based on the evaluation based on the estimated path speed pattern. In order to prun the integrated tree, the goodness of the path from the latest node of the tree to the root node is evaluated. The latest node of the route with a low evaluation score is deleted. The route is a sequence of a plurality of nodes arranged in the order of time extracted from the node in the latest layer to the root node of the tree. A speed pattern is obtained from the odometry stored in each node on the route, and the similarity between this and the speed pattern obtained from the position and orientation of the robot obtained by the integration processing at each node is calculated as an evaluation score. Each route is sorted in ascending order by the evaluation score obtained by evaluating each route, and the latest node is deleted from the integrated tree for the lower route. After that, the entire integration tree is scanned to remove nodes that do not have children. Merge a parent node with only one child with a child node to create a new node. The new node loses the parent node information and stores only the child node information.
診断機能のフィードバック：
診断機能のフィードバックでは、統合器が各推定器に対して行う信頼性の評価結果を推定器への診断結果として統合結果と共に出力する。これにより、推定器が診断結果をチェックして推定器のリセットなどを行うこともできる。
Diagnostic feature feedback:
In the feedback of the diagnostic function, the reliability evaluation result that the integrator performs for each estimator is output together with the integration result as a diagnosis result to the estimator. Thus, the estimator can check the diagnosis result and reset the estimator.
上記の如く、開示の地図処理方法及びプログラム、並びにロボットシステムでは、統合器が各推定器からの入力情報の信頼性を評価して、異なる信頼性のレベルに応じた異なる方式で統合履歴と関連付ける。この関連付けにより、統合履歴を表した統合ツリーが時間的に更新され、規模が増大していく。また、統合ツリーの最新ノードから過去に辿って得られた推定経路を評価して下位のノードを削除することで、ツリー全体のメンテナンスを行いツリー規模を一定以下に維持することもできる。さらに、例えば推定精度の低下した推定器の推定結果を統合処理から隔離できるため、統合結果が推定精度の低下した推定器の推定結果に影響されることなく、例えばナビゲーションの安定性を向上することができる。 As described above, in the disclosed map processing method and program, and in the robot system, the integrator evaluates the reliability of the input information from each estimator and associates it with the integration history in different ways according to different levels of reliability. . With this association, the integration tree representing the integration history is updated in time, and the scale increases. In addition, by evaluating an estimated route obtained by tracing the latest node of the integrated tree in the past and deleting lower nodes, the entire tree can be maintained and the tree scale can be maintained below a certain level. Furthermore, for example, the estimation result of an estimator with reduced estimation accuracy can be isolated from the integration process, so that the integration result is not affected by the estimation result of the estimator with reduced estimation accuracy, for example, improving navigation stability. Can do.
以上の実施例を含む実施形態に関し、さらに以下の付記を開示する。
（付記１）
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理
をコンピュータに実行させることを特徴とする、地図処理方法。
（付記２）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記１記載の地図処理方法。
（付記３）
前記評価処理は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記１または２記載の地図処理方法。
（付記４）
前記結合処理は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記３記載の地図処理方法。
（付記５）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する処理と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス処理
をさらに前記コンピュータに実行させることを特徴とする、付記３または４記載の地図処理方法。
（付記６）
前記メンテナンス処理は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記５記載の地図処理方法。
（付記７）
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順
をコンピュータに実行させることを特徴とする、プログラム。
（付記８）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記７記載のプログラム。
（付記９）
前記評価手順は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記７または８記載のプログラム。
（付記１０）
前記結合手順は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記９記載のプログラム。
（付記１１）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する手順と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス手順
をさらに前記コンピュータに実行させることを特徴とする、付記９または１０記載のプログラム。
（付記１２）
前記メンテナンス手順は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記１１記載のプログラム。
（付記１３）
自律移動型ロボットに設けられた互いに異なるセンサと、
前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、
前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部
を備えたことを特徴とする、ロボットシステム。
（付記１４）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記１３記載のロボットシステム。
（付記１５）
前記評価部は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記１３または１４記載のロボットシステム。
（付記１６）
前記結合部は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記１５記載のロボットシステム。
（付記１７）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する評価部と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス部
をさらに備えたことを特徴とする、付記１５または１６記載のロボットシステム。
（付記１８）
前記メンテナンス部は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記１７記載のロボットシステム。
The following supplementary notes are further disclosed with respect to the embodiments including the above examples.
(Appendix 1)
An evaluation process for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators;
A map processing method, comprising: causing a computer to perform a combining process for performing different integration processes according to the reliability on estimation results from the plurality of estimators.
(Appendix 2)
The map processing method according to
(Appendix 3)
In the evaluation process, the reliability is discretized into three levels, and an integration tree including a plurality of nodes is created to perform different integration processes depending on each level.
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. The map processing method according to
(Appendix 4)
The combining process includes
If all nodes in the latest layer of the integration tree have determined that the new input from one estimator has a high level of reliability, the parent node will have only one child node. And adding the input from the estimator as observation information to update the state of the parent node and storing it in the child node, storing the odometry information included in the new input in the child node,
When it is determined that the reliability level is “low”, only one child node is added to the parent node, and the input from the estimator is not used as observation information, and the state of the parent node is changed to odometry. Update using only the information of and save to the child node,
When it is determined that the reliability level is “medium”, the processing when the reliability level is determined as “high” and “low” is performed, respectively. As a result, the parent node has two children. The map processing method according to
(Appendix 5)
From the odometry stored in each node on the route, an evaluation score for evaluating the goodness of the route formed by a sequence of a plurality of nodes arranged in order of time extracted from the latest node to the root node of the integration tree is extracted. A process of calculating from the similarity between the obtained speed pattern and the speed pattern obtained by the integration process at each node on the route;
The map processing method according to
(Appendix 6)
The maintenance process deletes the latest node on the route, scans the entire integration tree to delete nodes that do not have children, and merges a parent node that has only one child with a child node. The map processing method according to
(Appendix 7)
An evaluation procedure for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators,
A program for causing a computer to execute a combination procedure for performing different integration processes according to the reliability on estimation results from the plurality of estimators.
(Appendix 8)
The program according to
(Appendix 9)
The evaluation procedure discretizes the reliability into three levels, and creates an integration tree including a plurality of nodes to perform different integration processing for each level,
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. The program according to
(Appendix 10)
The coupling procedure includes:
If all nodes in the latest layer of the integration tree have determined that the new input from one estimator has a high level of reliability, the parent node will have only one child node. And adding the input from the estimator as observation information to update the state of the parent node and storing it in the child node, storing the odometry information included in the new input in the child node,
When it is determined that the reliability level is “low”, only one child node is added to the parent node, and the input from the estimator is not used as observation information, and the state of the parent node is changed to odometry. Update using only the information of and save to the child node,
When it is determined that the reliability level is “medium”, the processing when the reliability level is determined as “high” and “low” is performed, respectively. As a result, the parent node has two children. The program according to
(Appendix 11)
From the odometry stored in each node on the route, an evaluation score for evaluating the goodness of the route formed by a sequence of a plurality of nodes arranged in order of time extracted from the latest node to the root node of the integration tree is extracted. A procedure for calculating from the similarity between the obtained speed pattern and the speed pattern obtained by the integration process at each node on the route;
The program according to
(Appendix 12)
The maintenance procedure deletes the latest node on the path, scans the entire integration tree to delete nodes that do not have children, and merges a parent node that has only one child with a child node. The program according to
(Appendix 13)
Different sensors provided on the autonomous mobile robot,
A plurality of estimators for estimating a self-position based on inputs from the different sensors;
An evaluation unit that evaluates reliability of the plurality of estimators using a reliability evaluation model corresponding to the plurality of estimators;
A robot system, comprising: a combining unit that performs different integration processing according to the reliability on estimation results from the plurality of estimators.
(Appendix 14)
14. The robot system according to
(Appendix 15)
The evaluation unit discretizes the reliability into three levels, and creates an integration tree including a plurality of nodes in order to perform different integration processing depending on each level,
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. 15. The robot system according to
(Appendix 16)
The coupling portion is
If all nodes in the latest layer of the integration tree have determined that the new input from one estimator has a high level of reliability, the parent node will have only one child node. And adding the input from the estimator as observation information to update the state of the parent node and storing it in the child node, storing the odometry information included in the new input in the child node,
When it is determined that the reliability level is “low”, only one child node is added to the parent node, and the input from the estimator is not used as observation information, and the state of the parent node is changed to odometry. Update using only the information of and save to the child node,
When it is determined that the reliability level is “medium”, the processing when the reliability level is determined as “high” and “low” is performed, respectively. As a result, the parent node has two children. The robot system according to
(Appendix 17)
From the odometry stored in each node on the route, an evaluation score for evaluating the goodness of the route formed by a sequence of a plurality of nodes arranged in order of time extracted from the latest node to the root node of the integration tree is extracted. An evaluation unit that calculates from the similarity between the obtained speed pattern and the speed pattern obtained by the integration process at each node on the route;
The robot system according to
(Appendix 18)
The maintenance unit deletes the latest node on the route, then scans the entire integration tree to delete nodes that do not have children, and merges a parent node that has only one child with a child node. 18. The robot system according to
以上、開示の地図処理方法及びプログラム、並びにロボットシステムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。 The disclosed map processing method and program, and robot system have been described above by way of examples. However, the present invention is not limited to the above examples, and various modifications and improvements can be made within the scope of the present invention. Needless to say.
４，６，８ 推定器
５，７，９ 記憶部
１０ 統合器
１１ 推定器信頼性評価部
１２ 結合部
１３ 結合の良さの評価部
１４ 統合ツリーのメンテナンス部
１５ 記憶部（統合ツリー）
２０ オドメトリ部
4, 6, 8
20 Odometry section
Claims (5)
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順
をコンピュータに実行させることを特徴とする、プログラム。 An evaluation procedure for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators,
A program for causing a computer to execute a combination procedure for performing different integration processes according to the reliability on estimation results from the plurality of estimators.
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、請求項１または２記載のプログラム。 The evaluation procedure discretizes the reliability into three levels, and creates an integration tree including a plurality of nodes to perform different integration processing for each level,
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. The program according to claim 1 or 2, wherein the program is characterized.
前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、
前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部
を備えたことを特徴とする、ロボットシステム。 Different sensors provided on the autonomous mobile robot,
A plurality of estimators for estimating a self-position based on inputs from the different sensors;
An evaluation unit that evaluates reliability of the plurality of estimators using a reliability evaluation model corresponding to the plurality of estimators;
A robot system, comprising: a combining unit that performs different integration processing according to the reliability on estimation results from the plurality of estimators.
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理
をコンピュータに実行させることを特徴とする、地図処理方法。 An evaluation process for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators;
A map processing method, comprising: causing a computer to perform a combining process for performing different integration processes according to the reliability on estimation results from the plurality of estimators.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011119676A JP5776332B2 (en) | 2011-05-27 | 2011-05-27 | Map processing method and program, and robot system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011119676A JP5776332B2 (en) | 2011-05-27 | 2011-05-27 | Map processing method and program, and robot system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2012248032A true JP2012248032A (en) | 2012-12-13 |
| JP5776332B2 JP5776332B2 (en) | 2015-09-09 |
Family
ID=47468413
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011119676A Active JP5776332B2 (en) | 2011-05-27 | 2011-05-27 | Map processing method and program, and robot system |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5776332B2 (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101454824B1 (en) * | 2013-04-03 | 2014-11-03 | 국방과학연구소 | System and Method for estimating positions of an autonomous mobile vehicle |
| CN105091885A (en) * | 2014-05-08 | 2015-11-25 | 株式会社日立制作所 | Robot and own-position estimation method |
| JP2017211825A (en) * | 2016-05-25 | 2017-11-30 | 村田機械株式会社 | Self-position estimation device and self-position estimation method |
| US10169880B2 (en) | 2014-08-26 | 2019-01-01 | Sony Corporation | Information processing apparatus, information processing method, and program |
| US10412319B2 (en) | 2016-12-08 | 2019-09-10 | Panasonic Intellectual Property Management Co., Ltd. | Imaging apparatus including image sensor, optical system, control circuit, and signal processing circuit |
| JP2020017173A (en) * | 2018-07-27 | 2020-01-30 | 株式会社ダイヘン | Moving entity |
| US10549430B2 (en) | 2015-08-28 | 2020-02-04 | Panasonic Intellectual Property Corporation Of America | Mapping method, localization method, robot system, and robot |
| JP2021105832A (en) * | 2019-12-26 | 2021-07-26 | 株式会社豊田自動織機 | Self-position estimation device, moving body, self-position estimation method, and self-position estimation program |
| US11216973B2 (en) | 2019-03-07 | 2022-01-04 | Mitsubishi Heavy Industries, Ltd. | Self-localization device, self-localization method, and non-transitory computer-readable medium |
| WO2023189721A1 (en) * | 2022-03-30 | 2023-10-05 | ソニーグループ株式会社 | Information processing device, information processing method, and information processing program |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07116981A (en) * | 1993-10-25 | 1995-05-09 | Internatl Business Mach Corp <Ibm> | Guiding device for mobile robot |
| JP2005108246A (en) * | 2003-09-30 | 2005-04-21 | Samsung Electronics Co Ltd | Method and device for estimating position of unmanned mobile body by use of sensor fusing, and computer-readable storage medium recording program |
| JP2007322138A (en) * | 2006-05-30 | 2007-12-13 | Toyota Motor Corp | Moving device, and own position estimation method for moving device |
| JP2011047836A (en) * | 2009-08-28 | 2011-03-10 | Fujitsu Ltd | Device, method and program for estimating state of moving body using new sensor fusion procedure |
| JP2012133226A (en) * | 2010-12-22 | 2012-07-12 | Sogo Keibi Hosho Co Ltd | Sound recognition device and sound recognition method |
-
2011
- 2011-05-27 JP JP2011119676A patent/JP5776332B2/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07116981A (en) * | 1993-10-25 | 1995-05-09 | Internatl Business Mach Corp <Ibm> | Guiding device for mobile robot |
| JP2005108246A (en) * | 2003-09-30 | 2005-04-21 | Samsung Electronics Co Ltd | Method and device for estimating position of unmanned mobile body by use of sensor fusing, and computer-readable storage medium recording program |
| JP2007322138A (en) * | 2006-05-30 | 2007-12-13 | Toyota Motor Corp | Moving device, and own position estimation method for moving device |
| JP2011047836A (en) * | 2009-08-28 | 2011-03-10 | Fujitsu Ltd | Device, method and program for estimating state of moving body using new sensor fusion procedure |
| JP2012133226A (en) * | 2010-12-22 | 2012-07-12 | Sogo Keibi Hosho Co Ltd | Sound recognition device and sound recognition method |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101454824B1 (en) * | 2013-04-03 | 2014-11-03 | 국방과학연구소 | System and Method for estimating positions of an autonomous mobile vehicle |
| CN105091885A (en) * | 2014-05-08 | 2015-11-25 | 株式会社日立制作所 | Robot and own-position estimation method |
| JP2015215651A (en) * | 2014-05-08 | 2015-12-03 | 株式会社日立製作所 | Robot and own position estimation method |
| US10169880B2 (en) | 2014-08-26 | 2019-01-01 | Sony Corporation | Information processing apparatus, information processing method, and program |
| US10549430B2 (en) | 2015-08-28 | 2020-02-04 | Panasonic Intellectual Property Corporation Of America | Mapping method, localization method, robot system, and robot |
| JP2017211825A (en) * | 2016-05-25 | 2017-11-30 | 村田機械株式会社 | Self-position estimation device and self-position estimation method |
| US10412319B2 (en) | 2016-12-08 | 2019-09-10 | Panasonic Intellectual Property Management Co., Ltd. | Imaging apparatus including image sensor, optical system, control circuit, and signal processing circuit |
| JP2020017173A (en) * | 2018-07-27 | 2020-01-30 | 株式会社ダイヘン | Moving entity |
| JP7063760B2 (en) | 2018-07-27 | 2022-05-09 | 株式会社ダイヘン | Mobile |
| US11216973B2 (en) | 2019-03-07 | 2022-01-04 | Mitsubishi Heavy Industries, Ltd. | Self-localization device, self-localization method, and non-transitory computer-readable medium |
| JP2021105832A (en) * | 2019-12-26 | 2021-07-26 | 株式会社豊田自動織機 | Self-position estimation device, moving body, self-position estimation method, and self-position estimation program |
| JP7285517B2 (en) | 2019-12-26 | 2023-06-02 | 株式会社豊田自動織機 | Self-position estimation device, mobile object, self-position estimation method, and self-position estimation program |
| WO2023189721A1 (en) * | 2022-03-30 | 2023-10-05 | ソニーグループ株式会社 | Information processing device, information processing method, and information processing program |
Also Published As
| Publication number | Publication date |
|---|---|
| JP5776332B2 (en) | 2015-09-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5776332B2 (en) | Map processing method and program, and robot system | |
| CN109682368B (en) | Robot, map construction method, positioning method, electronic device and storage medium | |
| US10133278B2 (en) | Apparatus of controlling movement of mobile robot mounted with wide angle camera and method thereof | |
| US11092444B2 (en) | Method and system for recording landmarks in a traffic environment of a mobile unit | |
| KR101503903B1 (en) | Apparatus and method for building map used in mobile robot | |
| JP6760114B2 (en) | Information processing equipment, data management equipment, data management systems, methods, and programs | |
| KR102628778B1 (en) | Method and apparatus for positioning, computing device, computer-readable storage medium and computer program stored in medium | |
| JP2022524069A (en) | Distributed processing of pose graphs to generate high-precision maps for navigation of autonomous vehicles | |
| US11506511B2 (en) | Method for determining the position of a vehicle | |
| JP6773471B2 (en) | Autonomous mobile and environmental map updater | |
| CN113330279A (en) | Method and system for determining the position of a vehicle | |
| CN111680747B (en) | Method and apparatus for closed loop detection of occupancy grid subgraphs | |
| JP2021175972A (en) | Method, apparatus, computing device, storage medium, and computer program for detecting environmental change | |
| CN110986956A (en) | Autonomous learning global positioning method based on improved Monte Carlo algorithm | |
| TW202036030A (en) | Information processing device and mobile robot | |
| JP7047576B2 (en) | Cartography device | |
| CN114879660B (en) | Robot environment sensing method based on target drive | |
| CN114114367A (en) | AGV outdoor positioning switching method, computer device and program product | |
| CN114186112B (en) | Robot navigation method based on Bayesian optimization multiple information gain exploration strategy | |
| JP2022034861A (en) | Forklift, location estimation method, and program | |
| US7613673B2 (en) | Iterative particle reduction methods and systems for localization and pattern recognition | |
| Van Zwynsvoorde et al. | Building topological models for navigation in large scale environments | |
| JP2023538946A (en) | Multi-agent map generation | |
| Muravyev et al. | Evaluation of topological mapping methods in indoor environments | |
| US12025462B2 (en) | Method for determining the position of a vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20140204 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20141118 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20141119 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20150114 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20150609 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150622 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5776332 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |