JP6913339B2 - 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム - Google Patents
移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム Download PDFInfo
- Publication number
- JP6913339B2 JP6913339B2 JP2017012051A JP2017012051A JP6913339B2 JP 6913339 B2 JP6913339 B2 JP 6913339B2 JP 2017012051 A JP2017012051 A JP 2017012051A JP 2017012051 A JP2017012051 A JP 2017012051A JP 6913339 B2 JP6913339 B2 JP 6913339B2
- Authority
- JP
- Japan
- Prior art keywords
- point
- scan data
- moving body
- revisit
- movement
- 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
- 230000033001 locomotion Effects 0.000 title claims description 182
- 238000004364 calculation method Methods 0.000 title claims description 126
- 238000000034 method Methods 0.000 title claims description 47
- 239000013598 vector Substances 0.000 claims description 158
- 238000005259 measurement Methods 0.000 claims description 40
- 230000001186 cumulative effect Effects 0.000 claims description 30
- 238000001514 detection method Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 8
- 238000013459 approach Methods 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 description 30
- 230000006870 function Effects 0.000 description 11
- 238000013519 translation Methods 0.000 description 5
- 230000005484 gravity Effects 0.000 description 3
- 238000013507 mapping Methods 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
- 238000002474 experimental method Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 230000010365 information processing Effects 0.000 description 2
- 238000000513 principal component analysis Methods 0.000 description 2
- 230000001629 suppression Effects 0.000 description 2
- 241000282412 Homo Species 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
- 230000004807 localization Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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 (9)
- 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムであって、
前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得するスキャンデータ取得部と、
第1タイミングで取得したスキャンデータと、前記第1タイミングから第1所定時間後の第2タイミングで取得したスキャンデータと、を照合することにより、前記第1所定時間における前記移動体の移動ベクトルを求める移動ベクトル算出部と、
前記第1所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第1所定時間毎の各通過地点の位置情報を求める位置情報算出部と、
前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求める累積移動距離算出部と、
前記各通過地点のうち前記移動体の現在位置からの距離が所定値よりも小さな通過地点を前記移動体の再訪点として検出し、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの前記累積移動距離の差が所定値以内のもの同士が同一グループになるように前記複数の再訪点を分類し、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出する再訪点検出部と、
を備えることを特徴とする移動軌跡算定システム。 - 請求項1に記載の移動軌跡算定システムであって、
前記移動体の現在位置の位置情報と、前記再訪点の位置情報と、に基づいて、前記現在位置と前記再訪点との間の相対位置を示す第1相対位置ベクトルを求める第1相対位置ベクトル算出部と、
前記移動体が前記再訪点を通過した第3タイミングに取得したスキャンデータと、前記移動体が前記現在位置を通過した第4タイミングに取得したスキャンデータと、を照合することにより、前記現在位置と前記再訪点との間の相対位置を示す第2相対位置ベクトルを求める第2相対位置ベクトル算出部と、
前記第1相対位置ベクトルが前記第2相対位置ベクトルに近づくように、前記移動体の前記各通過地点の位置情報を修正する位置情報修正部と、
を備えることを特徴とする移動軌跡算定システム。 - 請求項2に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記第4タイミングに取得したスキャンデータから得られる外界の各計測点の位置が、前記第3タイミングに取得したスキャンデータから得られる外界の各計測点の位置に近づくように、前記第2相対位置ベクトルを求める
ことを特徴とする移動軌跡算定システム。 - 請求項3に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記第4タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、前記第3タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、を比較し、前記各記述子の一致度合いが所定の判定値よりも大きな第1計測点及び第2計測点を前記各スキャンデータからそれぞれ抽出し、前記第1計測点の位置が前記第2計測点になるような変位ベクトルを、前記第2相対位置ベクトルの推定値として用いる
ことを特徴とする移動軌跡算定システム。 - 請求項3に記載の移動軌跡算定システムであって、
前記第2相対位置ベクトル算出部は、前記移動体が前記現在位置を通過した前記第4タイミングよりも第2所定期間前の第5タイミングから前記第4タイミングまでの前記移動体の移動ベクトルを、前記移動体が前記再訪点を通過した前記第3タイミングよりも前記第2所定期間前の第6タイミングにおける前記移動体の通過地点の位置情報に加えることにより、前記再訪点の推定位置を求め、前記移動体の前記現在位置と、前記再訪点の前記推定位置と、の間の変位ベクトルを、前記第2相対位置ベクトルの推定値として用いる
ことを特徴とする移動軌跡算定システム。 - 請求項2〜5のいずれかに記載の移動軌跡算定システムであって、
前記スキャンデータ取得部が前記距離センサから新たなスキャンデータを取得する毎に、前記新たなスキャンデータを、前記移動体を基準にした局所座標で表わされた値から、前記移動体に依存しない所定の共通座標で表わされた値に変換して蓄積することにより、外界の地図を作成する地図作成部と、
をさらに備え、
前記第2相対位置ベクトル算出部は、前記地図に蓄積されている前記共通座標で表わされたスキャンデータを、前記移動体の前記現在位置を基準とした局所座標上での値に変換することにより、前記現在位置から外界の各計測点までの距離及び方位を示す第1仮想スキャンデータを生成すると共に、前記地図に蓄積されている前記共通座標で表されたスキャンデータを、前記再訪点を基準とした局所座標上での値に変換することにより、前記再訪点から外界の各計測点までの距離及び方位を示す第2仮想スキャンデータを生成し、前記第1仮想スキャンデータ及び前記第2仮想スキャンデータを、それぞれ、前記第3タイミングにおける前記スキャンデータ及び前記第4タイミングにおける前記スキャンデータであるものとして照合することにより、前記現在位置と前記再訪点との間の位置差を示す前記第2相対位置ベクトルを求める
ことを特徴とする移動軌跡算定システム。 - 請求項6に記載の移動軌跡算定システムであって、
前記第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 JP2018120460A (ja) | 2018-08-02 |
JP6913339B2 true 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) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR3087884B1 (fr) * | 2018-10-31 | 2021-04-02 | Renault Sas | Procede de determination d’un vecteur de deplacement d’un vehicule automobile, procede de determination d’une vitesse du vehicule et vehicule associe |
CN111862203B (zh) * | 2019-04-30 | 2024-05-17 | 高新兴科技集团股份有限公司 | 基于3d地图的球机位置与姿态参数校准方法、存储介质 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012064131A (ja) * | 2010-09-17 | 2012-03-29 | Tokyo Institute Of Technology | 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置 |
EP2619742B1 (en) * | 2010-09-24 | 2018-02-28 | iRobot Corporation | Systems and methods for vslam optimization |
EP3014469B1 (en) * | 2013-06-28 | 2018-08-01 | Intel Corporation | Systems and methods for revisit location detection |
JP6481347B2 (ja) * | 2014-11-28 | 2019-03-13 | 村田機械株式会社 | 移動量推定装置、自律移動体、及び移動量の推定方法 |
-
2017
- 2017-01-26 JP JP2017012051A patent/JP6913339B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2018120460A (ja) | 2018-08-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及*** | |
Ström et al. | Robust exploration and homing for autonomous robots | |
Nieto et al. | Recursive scan-matching SLAM | |
US9239580B2 (en) | Autonomous mobile robot, self position estimation method, environmental map generation method, environmental map generation apparatus, and data structure for environmental map | |
JP4079690B2 (ja) | 物体追跡装置及び方法 | |
US8831778B2 (en) | Method of accurate mapping with mobile robots | |
Ström et al. | Predictive exploration considering previously mapped environments | |
JP2005528707A (ja) | データ組間の特徴マッピング | |
JP6782903B2 (ja) | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム | |
Bosse | ATLAS: a framework for large scale automated mapping and localization | |
WO2012176249A1 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、及び移動体 | |
TWI772743B (zh) | 資訊處理裝置以及移動機器人 | |
Ratter et al. | GPU accelerated graph SLAM and occupancy voxel based ICP for encoder-free mobile robots | |
JP6913339B2 (ja) | 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム | |
KR101738751B1 (ko) | 실내 자기장을 이용한 이동 로봇의 위치 인식 방법 및 장치 | |
Park et al. | Global localization for mobile robots using reference scan matching | |
Watkins-Valls et al. | Mobile manipulation leveraging multiple views | |
Thomas et al. | Delio: Decoupled lidar odometry | |
Tiar et al. | ICP-SLAM methods implementation on a bi-steerable mobile robot | |
KR20230056690A (ko) | 다중 에이전트 맵 생성 | |
Tungadi et al. | Multiple laser polar scan matching with application to SLAM | |
Hernández et al. | Occupancy grid mapping in an underwater structured environment | |
Wang et al. | Mobile robot pose estimation using laser scan matching based on fourier transform | |
Karlsson et al. | The vSLAM algorithm for navigation in natural environments | |
KR102624644B1 (ko) | 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법 |
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 |