JP5168134B2 - 環境地図生成プログラム、環境地図生成方法及び移動ロボット - Google Patents
環境地図生成プログラム、環境地図生成方法及び移動ロボット Download PDFInfo
- Publication number
- JP5168134B2 JP5168134B2 JP2008332139A JP2008332139A JP5168134B2 JP 5168134 B2 JP5168134 B2 JP 5168134B2 JP 2008332139 A JP2008332139 A JP 2008332139A JP 2008332139 A JP2008332139 A JP 2008332139A JP 5168134 B2 JP5168134 B2 JP 5168134B2
- Authority
- JP
- Japan
- Prior art keywords
- measurement point
- measurement
- stationary
- value
- estimated
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 49
- 230000007613 environmental effect Effects 0.000 title claims description 21
- VJYFKVYYMZPMAB-UHFFFAOYSA-N ethoprophos Chemical compound CCCSP(=O)(OCC)SCCC VJYFKVYYMZPMAB-UHFFFAOYSA-N 0.000 title 1
- PWPJGUXAGUPAHP-UHFFFAOYSA-N lufenuron Chemical compound C1=C(Cl)C(OC(F)(F)C(C(F)(F)F)F)=CC(Cl)=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F PWPJGUXAGUPAHP-UHFFFAOYSA-N 0.000 title 1
- 238000005259 measurement Methods 0.000 claims description 556
- 230000033001 locomotion Effects 0.000 claims description 137
- 239000006185 dispersion Substances 0.000 claims description 70
- 238000003860 storage Methods 0.000 claims description 39
- 238000012545 processing Methods 0.000 claims description 35
- 238000009826 distribution Methods 0.000 claims description 13
- 230000003068 static effect Effects 0.000 claims description 10
- 238000011156 evaluation Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 description 34
- 230000006870 function Effects 0.000 description 15
- 239000011159 matrix material Substances 0.000 description 15
- 238000010586 diagram Methods 0.000 description 9
- 238000004364 calculation method Methods 0.000 description 8
- 238000004519 manufacturing process Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
図9は、従来のSLAM技術を用いた移動ロボットの構成例を示した図である。なお、図9は、SLAM処理を実行する移動ロボットの処理部のみの構成を示した図である。
図1は、発明の概要を示した図である。
推定位置:pi(t)=[xi(t) yi(t) zi(t)]T,
推定誤差の分散行列:Pi(t) ・・・(1)
と表す。なお、[ ]Tは、転置行列を表す。
pi(t+T)=pi(t),
Pi(t+T)=Pi(t) ・・・(2)
と表された。すなわち、時刻(t+T)における計測点iの推定位置と推定誤差の分散行列は、時刻tの時点と同じになる。
運動モデル予測生成手段15は、計測点が運動していると仮定したときの運動モデルを用いて、現時の計測点iの予測値を算出する。
計測点iの時刻tにおける推定位置をpi(t)、時刻(t−T)における推定位置をpi(t−T)、時刻(t+T)における推定位置をpi(t+T)とすると、pi(t−T)からpi(t)までの移動速度vi(t)は、図に示したように、
vi(t)={pi(t)−pi(t−T)}/T ・・・(5)
と表すことができる。pi(t+T)まで同じ速度で移動することから、運動モデルを適用した場合の時刻(t+T)における予測値は、
計測点静止/運動判定手段16は、静止モデル予測生成手段14が静止モデルを適用した計測点iの予測値と、運動モデル予測生成手段15が運動モデルを適用した計測点iの予測値とを取得する。これらの予測値を、移動体の現在位置・姿勢の概略推定値を利用して位置検出用センサの座標系に変換し、それぞれのモデルについて計測点iの現在の計測値及び計測誤差の分散の予測値を生成する。これにより、計測点iが静止していると仮定したときに得られる計測値の予測値及び計測誤差の分散の予測値が生成される。同様に、計測点iが運動していると仮定したときに得られる計測値の予測値及び計測誤差の分散の予測値が生成される。そして、現在の実計測値と計測誤差の分散と、それぞれのモデルについて算出された計測値及び計測誤差の分散の予測値との統計的一致度を算出する。そして、統計的一致度が高い予測値を生成したモデルを選択することにより、計測点の静止/運動を判別する。さらに、計測点が運動している運動計測点と判別されたときは、その運動計測点を位置情報の登録から削除し、位置情報を更新する。
移動体に搭載される図示しない位置検出用センサが検知した計測点の3次元計測値が取得され、処理が開始される。
移動ロボット(制御部)100は、CPU(Central Processing Unit)101によって全体が制御されている。CPU101には、バス107を介してRAM(Random Access Memory)102、ハードディスクドライブ(HDD:Hard Disk Drive)103、入力インタフェース104、通信インタフェース105及び走行部インタフェース106が接続されている。
次に、このような構成の移動ロボットの環境地図生成処理部のソフトウェア構成について説明する。図4は、移動ロボットの環境地図生成処理部のソフトウェア構成を示した図である。
速度分散モデルを用いて、対象の計測点の過去の推定位置及び推定誤差の分散に速度分散を付加する。
静止計測点位置情報1100には、静止計測点1101、推定位置1102及び推定誤差1103の情報項目が設定される。
図6では、位置検出用センサ108は、時刻t1でa地点で各計測点の計測を行い、時刻t2に移動ロボットがb地点まで移動したとき、再度各計測点の計測を行うとする。
そして、各計測点について速度分散モデルに基づく式(3)を用いて、速度分散が付加された計測点の過去の推定位置及び推定誤差の分散の予測値を算出する。これを予測値aとする。現在位置推定部130は、3次元計測値と、速度分散モデル予測生成部120が算出した予測値aとを用いて、以下のカルマンフィルタにより、移動ロボットの現在位置・姿勢及び推定誤差の分散を算出する。まず、状態変数を以下のように定義する。
推定位置: pi(t)=[xi(t) yi(t) zi(t)]T,
推定誤差の共分散行列: Pi(t)、
なお、x,y,zは、3次元座標の水平、垂直及び奥行き方向の座標を表す。
そして、以下に示すカルマンフィルタを適用し、移動ロボットの現在位置・姿勢及び推定誤差の分散の予測値を得る。
静止計測点位置更新部180では、各計測点について、3次元計測値と、静止計測位置情報に登録される過去の推定位置と推定誤差の分散を用いて、状態変数を、
このように、運動計測点が削除された静止計測点位置情報を用いて再度推定された移動ロボットの現在位置・姿勢及び推定誤差の分散を用いて各計測点の推定位置及び推定誤差の分散を算出する。これにより、推定時に誤差の要因となる運動計測点が含まれる可能性を抑えることができるため、安定的かつ高い精度で計測点の位置を推定することが可能となる。
[ステップS01] 位置計測部190が、位置検出用センサが検知した進行方向あるいは周囲の物体の特徴点などの計測点のセンサ情報を取得し、3次元計測値を生成する。
[ステップS12] 速度分散モデル予測生成部120が、ステップS11で計測された各計測点について、計測点が速度0、速度誤差の分散を持って移動しているとみなす速度分散モデルに基づく予測値を生成する。
(付記1) 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する処理を実行させる環境地図生成プログラムにおいて、
コンピュータを、
前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段、
前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する現在位置推定手段、
として機能させることを特徴とする環境地図生成プログラム。
前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が静止していたとする静止モデルを適用し、前記計測点が静止計測点である場合の現在の推定位置を予測する静止モデル予測生成手段、
前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が運動していたとする運動モデルを適用し、前記計測点が運動計測点である場合の現在の推定位置を予測する運動モデル予測生成手段、
前記計測点が、前記静止計測点の場合の現在の推定位置と、前記運動計測点の場合の現在の推定位置と、を取得してそれぞれの場合の計測値の予測値を算出し、前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、評価結果に基づいて前記計測点が前記静止計測点または前記運動計測点のいずれであるかを判定する計測点静止/運動判定手段、
として機能させることを特徴とする付記1記載の環境地図生成プログラム。
前記現在位置推定手段は、前記計測点静止/運動判定手段によって前記運動計測点が削除され前記静止計測点のみが残された前記位置情報を参照し、前記位置情報に登録される前記静止計測点の過去の推定位置と、前記計測点の計測値と、を用いて前記移動体の現在位置を再度推定し、前記移動体の現在位置の詳細推定値を算出し、
静止計測点位置更新手段は、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値と、前記計測点の計測値と、前記位置情報に登録される前記静止計測点の過去の推定位置と、に基づいて前記静止計測点の推定位置を更新する、
ことを特徴とする付記3記載の環境地図生成プログラム。
ことを特徴とする付記7記載の環境地図生成プログラム。
速度分散モデル予測手段が、前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する手順と、
現在位置推定手段が、前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する手順と、
を有することを特徴とする環境地図生成方法。
前記環境内で過去に推定された前記計測点の推定位置を含む位置情報が格納される記憶手段と、
前記計測点の計測値が取得されると、前記記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段と、
前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて自身の現在位置を推定する現在位置推定手段と、
を有することを特徴とする移動ロボット。
12 速度分散モデル予測生成手段
13 第1の現在位置推定手段
14 静止モデル予測生成手段
15 運動モデル予測生成手段
16 計測点静止/運動判定手段
17 第2の現在位置推定手段
18 静止計測点位置更新手段
Claims (6)
- 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する処理を実行させる環境地図生成プログラムにおいて、
コンピュータを、
前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段、
前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する現在位置推定手段、
として機能させることを特徴とする環境地図生成プログラム。 - 前記コンピュータを、さらに、
前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が静止していたとする静止モデルを適用し、前記計測点が静止計測点である場合の現在の推定位置を予測する静止モデル予測生成手段、
前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が運動していたとする運動モデルを適用し、前記計測点が運動計測点である場合の現在の推定位置を予測する運動モデル予測生成手段、
前記計測点が、前記静止計測点の場合の現在の推定位置と、前記運動計測点の場合の現在の推定位置と、を取得してそれぞれの場合の計測値の予測値を算出し、前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、評価結果に基づいて前記計測点が前記静止計測点または前記運動計測点のいずれであるかを判定する計測点静止/運動判定手段、
として機能させることを特徴とする請求項1記載の環境地図生成プログラム。 - 前記計測点静止/運動判定手段は、前記統計的一致度の評価に応じて前記計測点が前記運動計測点であると判定されたときは、前記運動計測点であると判定された前記計測点に対応する前記位置情報を前記記憶手段から削除し、
前記現在位置推定手段は、前記計測点静止/運動判定手段によって前記運動計測点が削除され前記静止計測点のみが残された前記位置情報を参照し、前記位置情報に登録される前記静止計測点の過去の推定位置と、前記計測点の計測値と、を用いて前記移動体の現在位置を再度推定し、前記移動体の現在位置の詳細推定値を算出し、
静止計測点位置更新手段は、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値と、前記計測点の計測値と、前記位置情報に登録される前記静止計測点の過去の推定位置と、に基づいて前記静止計測点の推定位置を更新する、
ことを特徴とする請求項2記載の環境地図生成プログラム。 - 前記計測点静止/運動判定手段は、前記運動計測点が検出されたときは、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値を用いて前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、前記運動計測点に対応する前記位置情報を前記記憶手段から削除する、
ことを特徴とする請求項3記載の環境地図生成プログラム。 - 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する環境地図生成方法において、
速度分散モデル予測手段が、前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する手順と、
現在位置推定手段が、前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する手順と、
を有することを特徴とする環境地図生成方法。 - 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて自身の現在位置と前記計測点の位置を推定し、環境地図を生成する移動ロボットにおいて、
前記環境内で過去に推定された前記計測点の推定位置を含む位置情報が格納される記憶手段と、
前記計測点の計測値が取得されると、前記記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段と、
前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて自身の現在位置を推定する現在位置推定手段と、
を有することを特徴とする移動ロボット。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2008332139A JP5168134B2 (ja) | 2008-12-26 | 2008-12-26 | 環境地図生成プログラム、環境地図生成方法及び移動ロボット |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2008332139A JP5168134B2 (ja) | 2008-12-26 | 2008-12-26 | 環境地図生成プログラム、環境地図生成方法及び移動ロボット |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2010152787A JP2010152787A (ja) | 2010-07-08 |
JP5168134B2 true JP5168134B2 (ja) | 2013-03-21 |
Family
ID=42571773
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2008332139A Active JP5168134B2 (ja) | 2008-12-26 | 2008-12-26 | 環境地図生成プログラム、環境地図生成方法及び移動ロボット |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5168134B2 (ja) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2012086029A1 (ja) * | 2010-12-22 | 2012-06-28 | 株式会社日立製作所 | 自律移動システム |
WO2013076829A1 (ja) * | 2011-11-22 | 2013-05-30 | 株式会社日立製作所 | 自律移動システム |
CN107727104B (zh) * | 2017-08-16 | 2019-04-30 | 北京极智嘉科技有限公司 | 结合标识的同时定位和地图创建导航方法、装置及系统 |
KR102625488B1 (ko) * | 2017-12-05 | 2024-01-17 | 소니그룹주식회사 | 제어 장치 및 제어 방법, 프로그램, 그리고 이동체 |
JP6939513B2 (ja) * | 2017-12-21 | 2021-09-22 | 株式会社Ihi | モデル予測制御装置 |
DE102022208089A1 (de) | 2022-08-03 | 2024-02-08 | Robert Bosch Gesellschaft mit beschränkter Haftung | Vorrichtung und Verfahren zum Steuern eines Roboters |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4270386B2 (ja) * | 2004-08-11 | 2009-05-27 | 国立大学法人東京工業大学 | 移動体移動量算出装置 |
JP4407665B2 (ja) * | 2006-04-18 | 2010-02-03 | 株式会社豊田中央研究所 | 物体検知装置 |
-
2008
- 2008-12-26 JP JP2008332139A patent/JP5168134B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2010152787A (ja) | 2010-07-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR101572851B1 (ko) | 동적 환경에서 모바일 플랫폼의 지도 작성방법 | |
CN110178048B (zh) | 交通工具环境地图生成和更新的方法和系统 | |
JP6849330B2 (ja) | 地図生成方法、自己位置推定方法、ロボットシステム、およびロボット | |
US9058521B2 (en) | Mobile robot and simultaneous localization and map building method thereof | |
KR102453933B1 (ko) | 3차원 공간을 추적하는 차량 및 센싱 장치, 그리고 저장매체에 저장된 컴퓨터 프로그램 | |
JP4672175B2 (ja) | 位置検出装置、位置検出方法、及び位置検出プログラム | |
JP5168134B2 (ja) | 環境地図生成プログラム、環境地図生成方法及び移動ロボット | |
KR102238522B1 (ko) | 3차원 공간에 대응되는 맵을 생성하는 차량 및 방법 | |
KR101663650B1 (ko) | 거리 신호를 이용하여 위치를 인식하는 장치 및 방법 | |
JP6405778B2 (ja) | 対象追跡方法及び対象追跡装置 | |
WO2018221455A1 (ja) | 更新装置、制御方法、プログラム及び記憶媒体 | |
JP6150950B1 (ja) | 運転支援装置、運転支援システム、運転支援方法及び運転支援プログラム | |
KR102456151B1 (ko) | 레이더 및 카메라 기반의 센서 퓨전 시스템 및 주변 차량의 위치 산출 방법 | |
JPWO2012176249A1 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、及び移動体 | |
JP2018066716A (ja) | 物体追跡装置 | |
US11429098B2 (en) | Path providing apparatus and path providing method | |
EP3940670B1 (en) | Information processing device and mobile robot | |
JP6523196B2 (ja) | 推定装置、方法及びプログラム | |
US20200118285A1 (en) | Device and method for determining height information of an object in an environment of a vehicle | |
JP2016218737A (ja) | 先行車両推定装置及びプログラム | |
US20220282987A1 (en) | Information processing system, information processing device, and information processing method | |
KR102325121B1 (ko) | 맵 정보과 영상 매칭을 통한 실시간 로봇 위치 예측 방법 및 로봇 | |
KR20210062453A (ko) | 운동 정보 결정 방법 및 장치 | |
KR102696340B1 (ko) | 객체 추적을 위한 방법 및 장치 | |
JP7540559B1 (ja) | 自己位置推定装置及び自己位置推定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20110907 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20121029 |
|
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: 20121127 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20121210 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5168134 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |