JP2011047836A - 新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム - Google Patents
新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム Download PDFInfo
- Publication number
- JP2011047836A JP2011047836A JP2009197531A JP2009197531A JP2011047836A JP 2011047836 A JP2011047836 A JP 2011047836A JP 2009197531 A JP2009197531 A JP 2009197531A JP 2009197531 A JP2009197531 A JP 2009197531A JP 2011047836 A JP2011047836 A JP 2011047836A
- Authority
- JP
- Japan
- Prior art keywords
- sensors
- data set
- output
- state
- sensor
- 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
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
【解決手段】系の状態を推定するための装置であって、(a) ひとつ以上の第一のセンサと、(b) ひとつ以上の第二のセンサと、(c) ひとつ以上の第一のセンサのいずれかから各々が出力を受けて記憶手段に格納する複数の下位モジュールとを含み、下位モジュールが有する記憶手段は下位モジュールの各々に対応する地図と、系の状態を推定するための情報でありひとつ以上の第二のセンサのいずれかの出力から生成され第一のセンサのいずれかの出力と地図とに基づいて更新をされる第一のデータセットとを有し、さらに(d) 複数の下位モジュールの各々から出力された第一のデータセットを受けとり下位モジュールの各々に第一のデータセットの改善を行わせ第一のデータセットを統合し系の状態を推定して下位モジュールの各々へフィードバックする上位モジュールを含む。
【選択図】図3
Description
(a) ひとつ以上の第一のセンサと、
(b) ひとつ以上の第二のセンサと、
(c) 前記ひとつ以上の第一のセンサのいずれかから各々が出力を受けて記憶手段に格納する、複数の下位モジュールと
を含み、
前記複数の下位モジュールの各々が有する記憶手段は、
前記複数の下位モジュールの各々に対応する地図と、
前記系の状態を推定するための情報であり、前記ひとつ以上の第二のセンサのいずれかの出力から生成され、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新をされる、第一のデータセットと
を有し、
また前記装置はさらに、
(d) 前記複数の下位モジュールの各々から出力された前記第一のデータセットを受けとり、前記複数の下位モジュールの各々に前記第一のデータセットの改善を行わせ、前記第一のデータセットを統合することで、前記系の状態を推定して前記複数の下位モジュールの各々へフィードバックする、上位モジュール
を含んだ装置が提供される。
〔定義〕
本明細書において使用する術語についてこれより解説を行う。
ここより、本発明の実施形態を実施可能な程度に詳細に説明してゆく。
パーティクルの個数をN、iを1≦i≦Nの自然数として、第i番目のパーティクルの状態をsiと表す。また、第i番目のパーティクルの評価値(重み)をwiとする。評価値の最大値をwmax、平均値をwmeanとする。このとき、
IF
wmax < THmax or wmean < THmean
THEN
v = 0
ELSE
v = 1;
Xt := xt [1], xt [2], ... , xt [M] (1)
と表せる。ここでMはパーティクルの個数である。おのおののパーティクル xt [m](1≦m≦M) は、時刻tでの状態に関するひとつの仮説である。
xt [m] 〜 p(xt| z1:t,u1:t) (2)
となるように選ばれるべきである。ここでp(A | B)はBayesの条件付き確率(conditional probability)であり、或る事象Bが起こるという条件下での別の事象Aの起こる確率である。
xt [m] 〜 p(xt| ut,xt-1[m]) (3)
wt [m] = p(zt | xt [m]) (4)
ここでztは時刻tでの計測(measurement)データであり、例えば上述したオドメトリモデルに相当する。
パーティクルフィルタ 450 には、ノーマルパーティクルセット 452 と、補助的パーティクルセット 454 と、オドメトリモデル 456 とが含まれる。
図7は、状態推定系 707 を含んだ、移動体 700 を描いた模式図である。
移動体 700 はさらに、走行系 770 も含む。走行系 770 は、例えば図2のオドメトリモデル 230 に相当しうる。走行系 770 は、状態推定系 707 へと移動体の状態(位置・姿勢など)についての情報を送る。そして状態推定系 707 からのフィードバックを走行系 770 が受けて、移動制御機構 780 を介して調節を行う。移動制御機構 780 は、移動機構 790 を適切に制御し、移動体の適切な移動を実現できる。
状態推定系 707 には、統合処理モジュール 710 と、外界センサ#1 720 と、外界センサ#2 730 と、サブモジュール#1 740 と、サブモジュール#2 750 と、サブモジュール#3 760 とが含まれる。
x y z
0.813691, -3.412359, 0.302000
0.875252, -3.412136, 0.302000
0.940315, -3.426451, 0.302000
1.001578, -3.420039, 0.302000
1.063323, -3.414471, 0.302000
1.125890, -3.410684, 0.302000
1.192166, -3.418247, 0.302000
図8の例では、こうして得られたレンジデータのx座標とy座標を床に投影して、二次元的なデータとしている。
また、図7の例では、ひとつの外界センサから複数のサブモジュールへ別々の出力を行っていることにも留意されたい。実施形態では、或る外界センサが任意の個数のサブモジュールへの出力を行うことを想定している。
統合処理モジュール 710 は、サブモジュール 740, 750, 760 からの出力を上述した "縦割式" の評価などで適切に取捨選択してから統合できる。そして統合処理モジュール 710 は、移動体 700 の状態を修正するデータを、自己状態記憶メモリユニット#2 778 へとフィードバックできる。
図12は、実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。ここでは、CPU 1200 、メモリ 1202 、入力装置 1204 、出力装置 1206 、外部記憶装置 1208 、媒体駆動装置 1210 、可搬記録媒体 1212 、ネットワーク接続装置 1214 が、バス 1216 によって接続されている。
110 統合処理モジュール
120 サブモジュール#1
122 センサ#1
128 地図#1
130 サブモジュール#2
132 センサ#2
138 地図#2
200 パーティクルフィルタ
210 ノーマルパーティクルセット
220 補助的パーティクルセット
230 オドメトリモデル
290 記憶ユニット
300 状態推定ユニット
310 統合処理モジュール
320 サブモジュール#1
322 センサ#1
324 改善指示受入ユニット
328 地図#1
330 サブモジュール#2
332 センサ#2
334 改善指示受入ユニット
338 地図#2
340 パーティクルフィルタ
342 ノーマルパーティクルセット
344 補助的パーティクルセット
350 パーティクルフィルタ
352 ノーマルパーティクルセット
354 補助的パーティクルセット
360a 縦割式評価ユニット
360b 縦割式評価ユニット
364a 改善指示提出ユニット
364b 改善指示提出ユニット
400 状態推定ユニット
410 統合処理モジュール
420 サブモジュール#1
422 センサ#1
426 他者評価受入ユニット
428 地図#1
430 サブモジュール#2
432 センサ#2
434 他者評価受入ユニット
438 地図#2
440 パーティクルフィルタ
442 ノーマルパーティクルセット
444 補助的パーティクルセット
446 オドメトリモデル
450 パーティクルフィルタ
452 ノーマルパーティクルセット
454 補助的パーティクルセット
456 オドメトリモデル
600 状態推定ユニット
610 統合処理モジュール
620 サブモジュール#1
622 センサ#1
626 他者評価受入ユニット
628 地図#1
630 サブモジュール#2
632 センサ#2
634 他者評価受入ユニット
638 地図#2
640 パーティクルフィルタ
642 ノーマルパーティクルセット
644 補助的パーティクルセット
650 パーティクルフィルタ
652 ノーマルパーティクルセット
654 補助的パーティクルセット
660 記憶ユニット
666 オドメトリモデル
700 移動体
707 状態推定系
710 統合処理モジュール
720 外界センサ#1
730 外界センサ#2
740 サブモジュール#1
746 他者評価受入ユニット
748 地図
750 サブモジュール#2
756 他者評価受入ユニット
758 地図
760 サブモジュール#3
766 他者評価受入ユニット
768 地図
770 走行系
772 内界センサ
774 自己状態推定ユニット
776 自己状態記憶メモリユニット#1
778 自己状態記憶メモリユニット#2
780 移動制御機構
790 移動機構
1000 装置
1010 上位統合処理モジュール
1020 移動体#1
1022 センサ#1
1024 下位モジュール#1a
1026 下位モジュール#1b
1028 統合処理モジュール#1
1030 移動体#2
1032 センサ#2
1034 下位モジュール#2a
1036 下位モジュール#2b
1038 統合処理モジュール#2
1100 装置
1110 上位統合処理モジュール
1120 移動体#1
1124 下位モジュール#1a
1126 下位モジュール#1b
1128 統合処理モジュール#1
1130 移動体#2
1134 下位モジュール#2a
1136 下位モジュール#2b
1138 統合処理モジュール#2
1140 センサ
1200 CPU
1202 メモリ
1204 入力装置
1206 出力装置
1208 外部記憶装置
1210 媒体駆動装置
1212 可搬記録媒体
1214 ネットワーク接続装置
1216 バス
Claims (7)
- 系の状態を推定するための装置であって、
(a) ひとつ以上の第一のセンサと、
(b) ひとつ以上の第二のセンサと、
(c) 前記ひとつ以上の第一のセンサのいずれかから各々が出力を受けて記憶手段に格納する、複数の下位モジュールと
を含み、
前記複数の下位モジュールの各々が有する記憶手段は、
前記複数の下位モジュールの各々に対応する地図と、
前記系の状態を推定するための情報であり、前記ひとつ以上の第二のセンサのいずれかの出力から生成され、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新をされる、第一のデータセットと
を有し、
また前記装置はさらに、
(d) 前記複数の下位モジュールの各々から出力された前記第一のデータセットを受けとり、前記複数の下位モジュールの各々に前記第一のデータセットの改善を行わせ、前記第一のデータセットを統合することで、前記系の状態を推定して前記複数の下位モジュールの各々へフィードバックする、上位モジュール
を含む
ことを特徴とする、装置。 - 前記上位モジュールが、
前記複数の下位モジュールの各々に関する前記第一のデータセットを統合するにあたり、前記第一のデータセットの良さを定量化し、定量化された前記第一のデータセットの前記良さを第一の閾値と比較することで、前記第一のデータセットが良いか否かを評価する、縦割式評価手段
を含み、
前記複数の下位モジュールの各々に関する前記第一のデータセットのうち、前記縦割式評価手段によって良いとされた第一のデータセットのみを、前記上位モジュールが統合する
ことを特徴とする、請求項1記載の装置。 - 前記複数の下位モジュールの各々が有する前記記憶手段がさらに、
前記系の状態を推定するための情報であり、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とから生成される、第二のデータセット
を有し、
前記上位モジュールがさらに、
前記縦割式評価手段により良いとされなかった第一のデータセットに関する下位モジュールに対して改善指示を与える、改善指示手段
を含み、
前記改善指示手段から改善指示を受けた下位モジュールが、前記第二のデータセットを用いて、前記第一のデータセットの更新を行う
ことを特徴とする、請求項2記載の装置。 - さらに
(e) 前記系を移動させるための、移動手段
を含み、
前記ひとつ以上の第二のセンサが、前記移動手段の出力を測定し、
前記上位モジュールからの前記移動手段へのフィードバックにより、前記移動手段による移動が調節される
ことを特徴とする、請求項1記載の装置。 - 請求項1〜4のいずれか一項に記載の装置を搭載して状態推定を行うことを特徴とする、移動体。
- 系の状態を推定するための方法であって、
地図を記憶手段に格納するステップと、
ひとつ以上の第一のセンサからの出力を、前記記憶手段に格納するステップと、
ひとつ以上の第二のセンサのいずれかの出力から、データセットを生成するステップと、
前記データセットを、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新するステップと、
更新された前記データセットを統合することで、前記系の状態を推定するステップと
を含む、方法。 - 系の状態を推定する方法をコンピュータに実行させるためのプログラムであって、
前記方法が、
地図を記憶手段に格納するステップと、
ひとつ以上の第一のセンサからの出力を、前記記憶手段に格納するステップと、
ひとつ以上の第二のセンサのいずれかの出力から、データセットを生成するステップと、
前記データセットを、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新するステップと、
更新された前記データセットを統合することで、前記系の状態を推定するステップと
を含む
ことを特徴とする、プログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009197531A JP5589324B2 (ja) | 2009-08-28 | 2009-08-28 | 新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009197531A JP5589324B2 (ja) | 2009-08-28 | 2009-08-28 | 新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2011047836A true JP2011047836A (ja) | 2011-03-10 |
JP5589324B2 JP5589324B2 (ja) | 2014-09-17 |
Family
ID=43834307
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2009197531A Expired - Fee Related JP5589324B2 (ja) | 2009-08-28 | 2009-08-28 | 新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5589324B2 (ja) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012248032A (ja) * | 2011-05-27 | 2012-12-13 | Fujitsu Ltd | 地図処理方法及びプログラム、並びにロボットシステム |
WO2013001704A1 (ja) * | 2011-06-30 | 2013-01-03 | 日本電気株式会社 | 解析エンジン制御装置 |
JP2014016831A (ja) * | 2012-07-09 | 2014-01-30 | Fujitsu Ltd | 更新装置、更新方法、および更新プログラム |
JP2015155844A (ja) * | 2014-02-20 | 2015-08-27 | 多摩川精機株式会社 | 移動体位置計測システム、転圧作業軌跡作成システムおよび移動体の相対位置記録方法 |
CN106017475A (zh) * | 2016-07-04 | 2016-10-12 | 四川九洲电器集团有限责任公司 | 一种航迹更新方法及装置 |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10429508B2 (en) | 2015-10-30 | 2019-10-01 | Ricoh Company, Ltd. | Distance measuring device, moving system, and distance measurement method |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH02275310A (ja) * | 1989-04-17 | 1990-11-09 | Sumitomo Electric Ind Ltd | 位置検出装置 |
JP2008234350A (ja) * | 2007-03-20 | 2008-10-02 | Fujitsu Ltd | 自律移動体および移動方法 |
US7539593B2 (en) * | 2007-04-27 | 2009-05-26 | Invensys Systems, Inc. | Self-validated measurement systems |
JP2009192325A (ja) * | 2008-02-13 | 2009-08-27 | Furuno Electric Co Ltd | 衛星航法/推測航法統合測位装置 |
-
2009
- 2009-08-28 JP JP2009197531A patent/JP5589324B2/ja not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH02275310A (ja) * | 1989-04-17 | 1990-11-09 | Sumitomo Electric Ind Ltd | 位置検出装置 |
JP2008234350A (ja) * | 2007-03-20 | 2008-10-02 | Fujitsu Ltd | 自律移動体および移動方法 |
US7539593B2 (en) * | 2007-04-27 | 2009-05-26 | Invensys Systems, Inc. | Self-validated measurement systems |
JP2009192325A (ja) * | 2008-02-13 | 2009-08-27 | Furuno Electric Co Ltd | 衛星航法/推測航法統合測位装置 |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012248032A (ja) * | 2011-05-27 | 2012-12-13 | Fujitsu Ltd | 地図処理方法及びプログラム、並びにロボットシステム |
WO2013001704A1 (ja) * | 2011-06-30 | 2013-01-03 | 日本電気株式会社 | 解析エンジン制御装置 |
US9607008B2 (en) | 2011-06-30 | 2017-03-28 | Nec Corporation | Analysis engine control device |
JP2014016831A (ja) * | 2012-07-09 | 2014-01-30 | Fujitsu Ltd | 更新装置、更新方法、および更新プログラム |
JP2015155844A (ja) * | 2014-02-20 | 2015-08-27 | 多摩川精機株式会社 | 移動体位置計測システム、転圧作業軌跡作成システムおよび移動体の相対位置記録方法 |
CN106017475A (zh) * | 2016-07-04 | 2016-10-12 | 四川九洲电器集团有限责任公司 | 一种航迹更新方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
JP5589324B2 (ja) | 2014-09-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Kolhatkar et al. | Review of SLAM algorithms for indoor mobile robot with LIDAR and RGB-D camera technology | |
CN112525202A (zh) | 一种基于多传感器融合的slam定位导航方法及系统 | |
Indelman et al. | Factor graph based incremental smoothing in inertial navigation systems | |
JP5444952B2 (ja) | センサフュージョンによる地図の自動生成、およびそのように自動生成された地図を用いて移動体の移動をするための、装置、方法、ならびにプログラム | |
Thrun | Simultaneous localization and mapping | |
JP5589324B2 (ja) | 新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム | |
Li et al. | An algorithm for safe navigation of mobile robots by a sensor network in dynamic cluttered industrial environments | |
KR101864949B1 (ko) | 이동 로봇을 통한 격자 지도 작성 방법 | |
Raja et al. | PFIN: An efficient particle filter-based indoor navigation framework for UAVs | |
Pippin et al. | Trust modeling in multi-robot patrolling | |
Zhao et al. | Parallax angle parametrization for monocular SLAM | |
Abbadi et al. | Safe path planning using cell decomposition approximation | |
JP2022502791A (ja) | ロボットの姿勢を推定するシステムおよび方法、ロボット、並びに記憶媒体 | |
WO2022238189A1 (en) | Method of acquiring sensor data on a construction site, construction robot system, computer program product, and training method | |
Martín et al. | Kullback–Leibler divergence-based global localization for mobile robots | |
Vemprala et al. | Vision based collaborative path planning for micro aerial vehicles | |
Milstein et al. | A method for fast encoder‐free mapping in unstructured environments | |
Sarkar et al. | A novel method for computation of importance weights in Monte Carlo localization on line segment-based maps | |
Martins | MRSLAM-multi-robot simultaneous localization and mapping | |
JP2021081758A (ja) | 制御装置、制御方法及びプログラム | |
Xu et al. | Probabilistic Membrane Computing‐Based SLAM for Patrol UAVs in Coal Mines | |
Morenville et al. | Simultaneous localization and mapping and autonomous navigation on SPOT robot from Boston Dynamics | |
Tang | A Semi-autonomous Wheelchair Navigation System | |
WO2023219058A1 (ja) | 情報処理方法、情報処理装置及び情報処理システム | |
Martínez et al. | A closed-loop approach for tracking a humanoid robot using particle filtering and depth data |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20120510 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20120810 |
|
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: 20140701 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140714 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5589324 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
LAPS | Cancellation because of no payment of annual fees |