JP2018120460A - 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム - Google Patents
移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム Download PDFInfo
- Publication number
- JP2018120460A JP2018120460A JP2017012051A JP2017012051A JP2018120460A JP 2018120460 A JP2018120460 A JP 2018120460A JP 2017012051 A JP2017012051 A JP 2017012051A JP 2017012051 A JP2017012051 A JP 2017012051A JP 2018120460 A JP2018120460 A JP 2018120460A
- Authority
- JP
- Japan
- Prior art keywords
- point
- scan data
- timing
- revisit
- vector
- 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
- 230000033001 locomotion Effects 0.000 title claims abstract description 190
- 238000000034 method Methods 0.000 title claims description 46
- 239000013598 vector Substances 0.000 claims abstract description 158
- 230000008859 change Effects 0.000 claims abstract description 8
- 238000004364 calculation method Methods 0.000 claims description 134
- 238000005259 measurement Methods 0.000 claims description 38
- 238000001514 detection method Methods 0.000 claims description 16
- 238000012937 correction Methods 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims 1
- 238000012545 processing Methods 0.000 abstract description 23
- 230000008569 process Effects 0.000 description 16
- 230000006870 function Effects 0.000 description 11
- 230000001186 cumulative effect Effects 0.000 description 10
- 238000013519 translation Methods 0.000 description 5
- 230000005484 gravity Effects 0.000 description 3
- 239000002699 waste material Substances 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 230000010365 information processing Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000000513 principal component analysis Methods 0.000 description 2
- 230000001629 suppression Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000010248 power generation Methods 0.000 description 1
Abstract
Description
本発明の第1実施形態に係る移動軌跡算定システム100について、図面を参照しながら説明する。
移動体1000は、例えば自動運転車、ヒューマノイドロボット、パーソナルモビリティなど、自らの判断で自らの運動を自律的に制御することが可能な装置である。
距離センサ200は、外界の存在物までの距離を計測するセンサである。本実施形態に係る距離センサ200は、レーザパルスを出射する光源と、レーザパルスの反射光を検出する検出器とを備えた2次元(2D)レーザスキャナであり、所定の方向に向けて出射したレーザパルスが往復する時間を計測することで計測点までの距離を算定する。
移動軌跡算定システム100は、移動体1000に搭載され、外界の存在物との相対的な位置の変化を検出することにより、移動体1000の移動軌跡を算定する装置である。詳細は後述する。
運動制御部400は、移動軌跡算定システム100により推定された移動体1000の所定時間毎の並進量及び回転量(移動ベクトル)や、距離センサ200により得られるスキャンデータを元に作成される外界の地図を用いて、移動体1000を移動させるためのアクチュエータ500を駆動する。
アクチュエータ500は、移動体1000を移動させるためのモータやバルブ、エンジンなどの動力発生装置である。
次に、上述した移動軌跡算定システム100について詳細に説明する。
スキャンデータ取得部110は、距離センサ200から出力される外界の各計測点(スキャン点)までの距離及び方向を示す計測結果を順次取得する。そしてスキャンデータ取得部110は、距離センサ200による距離の計測方向が1回転する毎に、1回転分の計測結果をスキャンデータPt(フレームPt)としてまとめ、後述する経路情報記憶部120に記憶する。
つぎに、経路情報記憶部120を図2に示す。
図1に戻って、移動ベクトル算出部130は、ある第1タイミングiで取得したスキャンデータPiと、この第1タイミングから第1所定時間後の第2タイミングjで取得したスキャンデータPjと、を照合することにより、第1所定時間における移動体1000の並進量及び回転量、すなわち移動ベクトルmjを求める。そして移動ベクトル算出部130は、第1所定時間毎に繰り返し移動ベクトルを算出する。
つぎに位置情報算出部140について説明する。
累積移動距離算出部160は、移動体1000の移動ベクトルmiの大きさ|mi|を累積することにより、移動体1000の移動開始地点r0から各通過地点rt(t=0〜n)までのそれぞれの累積移動距離Ltを求める。
次に、再訪点検出部150について説明する。再訪点検出部150は、移動体1000の各通過地点rt(t=0〜n)の中から、移動体1000の最新の通過地点である現在位置rnからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体1000の再訪点rsとして検出する。
次に、図1に戻って、地図作成部180について説明する。 地図作成部180は、スキャンデータ取得部110が距離センサ200から新たなスキャンデータPtを取得する毎に、この新たなスキャンデータPtを、移動体1000を基準にした局所座標で表わされた値から、移動体1000の位置に依存しない所定の共通座標で表わされた値に変換する。そして地図作成部180はこれらの共通座標に変換されたスキャンデータを順次、地図情報記憶部190に蓄積することにより、外界の地図を作成する。
ループ閉じ込み処理部170は、移動体1000の現在位置rnにおいて得られたスキャンデータPnと、再訪点rsにおいて得られたスキャンデータPsと、を照合することにより、現在位置rnと再訪点rsとの位置ずれを修正する。
x=x1-(x2cosθ-y2sinθ) (6)
y=y1-(x2sinθ+y2cosθ) (7)
理想的にはこれで相対位置q=(x,y,θ)が求まるが、実際には誤対応や誤差などにより、一組のスキャン点で得た解は信頼性が低い。
y = y1 + (x2sinθ1 + y2cosθ1) (9)
θ=θ1 + θ2 (10)
第2相対位置ベクトル算出部172は、この推定を例えば1フレームごとに行う。1フレーム分の移動体1000の移動量mrは小さいので、移動体1000が再訪点rsを通過したタイミングsの1タイミング前のタイミングs−1において確定した再訪点の位置r’s−1を基準にすることで、現在位置rnと再訪点rsとの間の第2相対位置ベクトルv2,nの初期値Δrnを効率よく求めることができる。
次に、位置情報修正部173は、ループ閉じ込みを行う。具体的には、位置情報修正部173は、第1相対位置ベクトルv1と第2相対位置ベクトルv2とが求められたら、第1相対位置ベクトルv1が第2相対位置ベクトルv2に近づくように、移動体1000の各通過地点の位置情報rt(t=0〜n)を修正する。
次に、移動軌跡算定システム100による処理の流れを、図9を参照しながら説明する。
次に、第2実施形態に係る移動軌跡算定システム100について説明する。
次に、第3実施形態に係る移動軌跡算定システム100について説明する。
第1実施形態から第3実施形態では、スキャンデータ取得部110は、距離センサ200から取得した各スキャン点までの距離及び方位の計測結果をそのままスキャンデータPtとして経路情報記憶部120に記憶したが、第4実施形態に係るスキャンデータ取得部110は、距離センサ200から各スキャン点までの距離及び方位の計測結果を取得した際に、隣り合うスキャン点の間隔が全体として平準化されるように、各計測結果を補正する。
第5実施形態に係る移動軌跡算定システム100は、第2実施形態から第4実施形態でそれぞれ説明した構成をすべて含む形態である。
110 スキャンデータ取得部
120 経路情報記憶部
130 移動ベクトル算出部
140 位置情報算出部
150 再訪点検出部
160 累積移動距離算出部
170 ループ閉じ込み処理部
171 第1相対位置ベクトル算出部
172 第2相対位置ベクトル算出部
173 位置情報修正部
180 地図作成部
190 地図情報記憶部
200 距離センサ
400 運動制御部
500 アクチュエータ
1000 移動体
Claims (11)
- 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムであって、
前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得するスキャンデータ取得部と、
第1タイミングで取得したスキャンデータと、前記第1タイミングから第1所定時間後の第2タイミングで取得したスキャンデータと、を照合することにより、前記第1所定時間における前記移動体の移動ベクトルを求める移動ベクトル算出部と、
前記第1所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第1所定時間毎の各通過地点の位置情報を求める位置情報算出部と、
前記各通過地点の中から、前記移動体の現在位置からの距離が所定値よりも小さく、かつ、極小となる通過地点を、前記移動体の再訪点として検出する再訪点検出部と、
を備えることを特徴とする移動軌跡算定システム。 - 請求項1に記載の移動軌跡算定システムであって、
前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求める累積移動距離算出部と、
をさらに備え、
前記再訪点検出部は、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの累積移動距離の差が所定値以内のもの同士が同一グループになるように、前記複数の再訪点を分類し、前記各グループから一つの再訪点を選出する
ことを特徴とする移動軌跡算定システム。 - 請求項2に記載の移動軌跡算定システムであって、
前記再訪点検出部は、前記各グループから一つの再訪点を選出する際には、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出する
ことを特徴とする移動軌跡算定システム。 - 請求項1〜3のいずれかに記載の移動軌跡算定システムであって、
前記移動体の現在位置の位置情報と、前記再訪点の位置情報と、に基づいて、前記現在位置と前記再訪点との間の相対位置を示す第1相対位置ベクトルを求める第1相対位置ベクトル算出部と、
前記移動体が前記再訪点を通過した第3タイミングに取得したスキャンデータと、前記移動体が前記現在位置を通過した第4タイミングに取得したスキャンデータと、を照合することにより、前記現在位置と前記再訪点との間の相対位置を示す第2相対位置ベクトルを求める第2相対位置ベクトル算出部と、
前記第1相対位置ベクトルが前記第2相対位置ベクトルに近づくように、前記移動体の前記各通過地点の位置情報を修正する位置情報修正部と、
を備えることを特徴とする移動軌跡算定システム。 - 請求項4に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記第4タイミングに取得したスキャンデータから得られる外界の各計測点の位置が、前記第3タイミングに取得したスキャンデータから得られる外界の各計測点の位置に近づくように、前記第2相対位置ベクトルを求める
ことを特徴とする移動軌跡算定システム。 - 請求項5に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記第4タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、前記第3タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、を比較し、前記各記述子の一致度合いが所定の判定値よりも大きな第1計測点及び第2計測点を前記各スキャンデータからそれぞれ抽出し、前記第1計測点の位置が前記第2計測点になるような変位ベクトルを、前記第2相対位置ベクトルの推定値として用いる
ことを特徴とする移動軌跡算定システム。 - 請求項5に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記移動体が前記現在位置を通過した前記第4タイミングよりも第2所定期間前の第5タイミングから前記第4タイミングまでの前記移動体の移動ベクトルを、前記移動体が前記再訪点を通過した前記第3タイミングよりも前記第2所定期間前の第6タイミングにおける前記移動体の通過地点の位置情報に加えることにより、前記再訪点の推定位置を求め、前記移動体の前記現在位置と、前記再訪点の前記推定位置と、の間の変位ベクトルを、前記第2相対位置ベクトルの推定値として用いる
ことを特徴とする移動軌跡算定システム。 - 請求項4〜7のいずれかに記載の移動軌跡算定システムであって、
前記スキャンデータ取得部が前記距離センサから新たなスキャンデータを取得する毎に、前記新たなスキャンデータを、前記移動体を基準にした局所座標で表わされた値から、前記移動体に依存しない所定の共通座標で表わされた値に変換して蓄積することにより、外界の地図を作成する地図作成部と、
をさらに備え、
前記第2相対位置ベクトル算出部は、前記地図に蓄積されている前記共通座標で表わされたスキャンデータを、前記移動体の前記現在位置を基準とした局所座標上での値に変換することにより、前記現在位置から外界の各計測点までの距離及び方位を示す第1仮想スキャンデータを生成すると共に、前記地図に蓄積されている前記共通座標で表されたスキャンデータを、前記再訪点を基準とした局所座標上での値に変換することにより、前記再訪点から外界の各計測点までの距離及び方位を示す第2仮想スキャンデータを生成し、前記第1仮想スキャンデータ及び前記第2仮想スキャンデータを、それぞれ、前記第3タイミングにおける前記スキャンデータ及び前記第4タイミングにおける前記スキャンデータであるものとして照合することにより、前記現在位置と前記再訪点との間の位置差を示す前記第2相対位置ベクトルを求める
ことを特徴とする移動軌跡算定システム。 - 請求項8に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記第1仮想スキャンデータを生成する際には、前記移動体が前記現在位置を通過した前記第4タイミングから第3所定期間内に前記距離センサから取得したスキャンデータに由来して前記地図に蓄積されたスキャンデータを用いると共に、前記第2仮想スキャンデータを生成する際には、前記移動体が前記再訪点を通過した前記第3タイミングから前記第3所定期間内に前記距離センサから取得したスキャンデータに由来して前記地図に蓄積されたスキャンデータを用いる
ことを特徴とする移動軌跡算定システム。 - 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムの制御方法であって、
前記移動軌跡算定システムが、前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得し、
前記移動軌跡算定システムが、第1タイミングで取得したスキャンデータと、前記第1タイミングから第1所定時間後の第2タイミングで取得したスキャンデータと、を照合することにより、前記第1所定時間における前記移動体の移動ベクトルを求め、
前記移動軌跡算定システムが、前記第1所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第1所定時間毎の各通過地点の位置情報を求め、
前記移動軌跡算定システムが、前記各通過地点の中から、前記移動体の現在位置からの距離が所定値よりも小さく、かつ、極小となる通過地点を、前記移動体の再訪点として検出する
ことを特徴とする移動軌跡算定システムの制御方法。 - 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムに、
前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得する手順と、
第1タイミングで取得したスキャンデータと、前記第1タイミングから第1所定時間後の第2タイミングで取得したスキャンデータと、を照合することにより、前記第1所定時間における前記移動体の移動ベクトルを求める手順と、
前記第1所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第1所定時間毎の各通過地点の位置情報を求める手順と、
前記各通過地点の中から、前記移動体の現在位置からの距離が所定値よりも小さく、かつ、極小となる通過地点を、前記移動体の再訪点として検出する手順と、
を実行させるためのプログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2017012051A JP6913339B2 (ja) | 2017-01-26 | 2017-01-26 | 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2017012051A JP6913339B2 (ja) | 2017-01-26 | 2017-01-26 | 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2018120460A true JP2018120460A (ja) | 2018-08-02 |
JP6913339B2 JP6913339B2 (ja) | 2021-08-04 |
Family
ID=63043816
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2017012051A Active JP6913339B2 (ja) | 2017-01-26 | 2017-01-26 | 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6913339B2 (ja) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111862203A (zh) * | 2019-04-30 | 2020-10-30 | 高新兴科技集团股份有限公司 | 基于3d地图的球机位置与姿态参数校准方法、存储介质 |
JP2022504787A (ja) * | 2018-10-31 | 2022-01-13 | ルノー エス.ア.エス. | 自動車両の移動ベクトルを決定するための方法、車両の速度を判断するための方法、および関連する車両 |
CN111862203B (zh) * | 2019-04-30 | 2024-05-17 | 高新兴科技集团股份有限公司 | 基于3d地图的球机位置与姿态参数校准方法、存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012064131A (ja) * | 2010-09-17 | 2012-03-29 | Tokyo Institute Of Technology | 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置 |
JP2013537995A (ja) * | 2010-09-24 | 2013-10-07 | エボリューション・ロボティクス・インコーポレイテッド | Vslam最適化のためのシステムおよび方法 |
US20150172869A1 (en) * | 2013-06-28 | 2015-06-18 | Intel Corporation | Systems and methods for revisit location detection |
JP2016103158A (ja) * | 2014-11-28 | 2016-06-02 | 村田機械株式会社 | 移動量推定装置、自律移動体、及び移動量の推定方法 |
-
2017
- 2017-01-26 JP JP2017012051A patent/JP6913339B2/ja active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012064131A (ja) * | 2010-09-17 | 2012-03-29 | Tokyo Institute Of Technology | 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置 |
JP2013537995A (ja) * | 2010-09-24 | 2013-10-07 | エボリューション・ロボティクス・インコーポレイテッド | Vslam最適化のためのシステムおよび方法 |
US20150172869A1 (en) * | 2013-06-28 | 2015-06-18 | Intel Corporation | Systems and methods for revisit location detection |
JP2016103158A (ja) * | 2014-11-28 | 2016-06-02 | 村田機械株式会社 | 移動量推定装置、自律移動体、及び移動量の推定方法 |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2022504787A (ja) * | 2018-10-31 | 2022-01-13 | ルノー エス.ア.エス. | 自動車両の移動ベクトルを決定するための方法、車両の速度を判断するための方法、および関連する車両 |
JP7166447B2 (ja) | 2018-10-31 | 2022-11-07 | ルノー エス.ア.エス. | 自動車両の移動ベクトルを決定するための方法、車両の速度を判断するための方法、および関連する車両 |
CN111862203A (zh) * | 2019-04-30 | 2020-10-30 | 高新兴科技集团股份有限公司 | 基于3d地图的球机位置与姿态参数校准方法、存储介质 |
CN111862203B (zh) * | 2019-04-30 | 2024-05-17 | 高新兴科技集团股份有限公司 | 基于3d地图的球机位置与姿态参数校准方法、存储介质 |
Also Published As
Publication number | Publication date |
---|---|
JP6913339B2 (ja) | 2021-08-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
Se et al. | Vision-based global localization and mapping for mobile robots | |
US8831778B2 (en) | Method of accurate mapping with mobile robots | |
Luck et al. | Registration of range data using a hybrid simulated annealing and iterative closest point algorithm | |
Kümmerle et al. | Monte carlo localization in outdoor terrains using multilevel surface maps | |
US9239580B2 (en) | Autonomous mobile robot, self position estimation method, environmental map generation method, environmental map generation apparatus, and data structure for environmental map | |
US9317735B2 (en) | Information processing apparatus, information processing method, and program to calculate position and posture of an object having a three-dimensional shape | |
JP6782903B2 (ja) | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム | |
Liang et al. | Visual laser-SLAM in large-scale indoor environments | |
JP2005528707A (ja) | データ組間の特徴マッピング | |
McGreavy et al. | Next best view planning for object recognition in mobile robotics | |
Park et al. | Global localization for mobile robots using reference scan matching | |
Watkins-Valls et al. | Mobile manipulation leveraging multiple views | |
Heinemann et al. | A combined monte-carlo localization and tracking algorithm for robocup | |
JP6913339B2 (ja) | 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム | |
Tiar et al. | ICP-SLAM methods implementation on a bi-steerable mobile robot | |
Zingaretti et al. | Appearance based robotics | |
Kaplan et al. | Practical extensions to vision-based monte carlo localization methods for robot soccer domain | |
Karlsson et al. | The vSLAM algorithm for navigation in natural environments | |
Tungadi et al. | Multiple laser polar scan matching with application to SLAM | |
Lenac et al. | A novel genetic scan-matching-based registration algorithm for supporting moving objects tracking effectively and efficiently | |
Muñoz | Global pose estimation and tracking for RGB-D localization and 3D mapping | |
Park et al. | Coarse-to-fine global localization for mobile robots with hybrid maps of objects and spatial layouts | |
Chen et al. | Pose-based GraphSLAM algorithm for robotic fish with a mechanical scanning sonar | |
JP2004132933A (ja) | アクティブセンサの位置・姿勢推定方法及びその装置、並びにアクティブセンサの位置・姿勢推定プログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20191218 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20201125 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20201215 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20210106 |
|
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: 20210622 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20210705 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6913339 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |