JP7199772B2 - 自己位置推定装置 - Google Patents
自己位置推定装置 Download PDFInfo
- Publication number
- JP7199772B2 JP7199772B2 JP2022505711A JP2022505711A JP7199772B2 JP 7199772 B2 JP7199772 B2 JP 7199772B2 JP 2022505711 A JP2022505711 A JP 2022505711A JP 2022505711 A JP2022505711 A JP 2022505711A JP 7199772 B2 JP7199772 B2 JP 7199772B2
- Authority
- JP
- Japan
- Prior art keywords
- self
- map
- geometric features
- past
- current
- 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
- 238000011156 evaluation Methods 0.000 claims description 38
- 238000004364 calculation method Methods 0.000 claims description 34
- 239000000284 extract Substances 0.000 claims description 18
- 238000000605 extraction Methods 0.000 claims description 16
- 238000001514 detection method Methods 0.000 claims description 14
- 239000013598 vector Substances 0.000 description 24
- 238000000034 method Methods 0.000 description 21
- 238000012545 processing Methods 0.000 description 21
- 238000010586 diagram Methods 0.000 description 11
- 230000008569 process Effects 0.000 description 7
- 238000013519 translation Methods 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000000513 principal component analysis Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000001678 irradiating effect Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3848—Data obtained from both position sensors and additional sensors
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/08—Systems determining position data of a target for measuring distance only
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4808—Evaluating distance, position or velocity data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Aviation & Aerospace Engineering (AREA)
- Computer Networks & Wireless Communication (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Optics & Photonics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
本発明は、移動体の自己位置推定装置に関する。
従来、移動ロボットや自動運転車等の自律走行可能な移動体が自律走行するための技術として、SLAM(Simultaneous Localization and Mapping) と称される自己位置推定と環境地図作成の技術が用いられている。環境地図としては、移動体の移動範囲における物体の有無に基づいて作成された占有格子地図や点群地図が用いられることがある。占有格子地図は、移動範囲の平面や空間を複数の区画(セル)に分割して記憶するとともに、分割した各区画に対して物体の有無に応じたセル値が付与されている。点群地図は、移動範囲の平面や空間に存在する物体を微小領域ごとに離散化した点(座標)として表し、その集合である点群データが地図として利用される。
移動体が目的地まで自律走行する場合、現在地から目的地までの移動経路を求め、その移動経路に沿って目的地まで走行する。この際、効率よく移動するために、あらかじめ作成しておいた地図(既存地図)を用いて移動経路を算出し、その既存地図上で移動体が自己位置を確認(推定)しながら走行することが行われる。このような既存地図上での自己位置推定は、移動体の自律走行にとって不可欠の技術である。
移動体の自己位置推定の技術として、距離センサの出力で得られる距離データを用いて、予め準備した環境地図に対して移動体の位置合わせを行って自己位置を推定する方法が一般的である。移動体の位置合わせの方法として、例えば、ICP(Iterative Closest Point)といわれる方法があり、この方法では、点群データから抽出した点の集合の幾何学的特徴に基づき、環境地図の特徴部位と照合することにより移動体の位置を判別する(例えば、特許文献1参照)。特許文献1に記載の自己位置推定方法は、距離センサによって周辺を観測することによって、物体表面を示す複数の三次元点を取得し、その観測結果に基づいて、近距離の三次元点と、特徴量が所定の閾値以上である遠距離の三次元点とを抽出し、抽出した複数の三次元点と、環境地図における複数の三次元点とをマッチングすることによって自己位置を推定している。
しかしながら、特許文献1に記載された発明では、複数の三次元点をマッチングすることによって自己位置を推定しているので、屋外環境などのように三次元点(三次元点群データ)の個数が多い環境にて自己位置を推定する場合には、処理時間は長くなってしまうという問題がある。
本発明の目的は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができ、精度よく自己位置を推定することができる自己位置推定装置を提供することである。
本発明の自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、移動体の周囲の物体までの距離を三次元点群データとして検出する検出手段と、移動体の自己位置を推定する推定手段とを備え、推定手段は、検出手段にて検出された三次元点群データに基づいて、移動体の周囲の地図を生成する地図生成部と、地図生成部にて生成された地図を記憶する記憶部と、記憶部に記憶された地図に基づいて、平面、直線、および球体の3種類の幾何特徴のうち、少なくともいずれか1種類の幾何特徴を移動体の現在位置周辺の地図としての現在地図から現在幾何特徴として抽出するとともに、移動体の過去位置周辺の地図としての過去地図から過去幾何特徴として抽出する幾何特徴抽出部と、幾何特徴抽出部にて抽出した現在幾何特徴のうち、2個または3個の現在幾何特徴を選択するとともに、幾何特徴抽出部にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組とし、幾何特徴の組に基づいて、過去地図における自己位置を算出する自己位置算出部と、自己位置算出部にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する自己位置評価部とを備えることを特徴とする。
このような構成によれば、自己位置算出部は、三次元点よりも個数が大幅に少ない幾何特徴を用いるため、その組合せも少なくて済む。三次元点を用いて自己位置を求める場合、現在地図および過去地図からそれぞれ3個の三次元点を選び、その組を用いて自己位置の計算を行う。三次元点は膨大な個数(数千個から数万個)があるため、その組合せも膨大であり、その中から一致度の高い組を求めるには多大な処理時間がかかる。一方、本発明の幾何特徴は、三次元点に比べて個数が少なく(数個から数百個)、組合せ数も少なくて済む。したがって、自己位置推定装置は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができる。
本発明では、自己位置評価部は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価することが好ましい。
このような構成によれば、自己位置評価部は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価するので、三次元点群データよりも個数の少ない球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価することができる。したがって、自己位置推定装置は、処理時間を更に削減することができる。
本発明では、自己位置評価部は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価することが好ましい。
このような構成によれば、自己位置評価部は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価するので、球体群の個数を段階的に増加させつつ所定の閾値よりも一致度の高い自己位置を絞り込んでいくことができる。したがって、自己位置推定装置は、更に精度よく自己位置を推定することができる。
本発明では、自己位置評価部は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定することが好ましい。
このような構成によれば、自己位置評価部は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定するので、自己位置推定装置は、はじめから三次元点群データに基づき自己位置を推定する場合と比較して処理時間を削減することができ、しかも精度よく自己位置を推定することができる。
本発明では、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないことが好ましい。
ここで、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間の整合性は、過去地図における自己位置を算出すべく座標変換しても変化することはないので、整合性のない幾何特徴の組に基づき算出された自己位置に基づいて、現在地図および過去地図の一致度を評価した場合には、その一致度は低くなると考えられる。
本発明によれば、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置は、処理時間を更に削減することができる。
本発明によれば、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置は、処理時間を更に削減することができる。
本発明では、自己位置算出部は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択することが好ましい。
ここで、自己位置算出部は、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度が平行に近い場合には、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができない場合がある。
本発明によれば、自己位置算出部は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択するので、自己位置算出部は、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができる。なお、所定の角度は、明確に平行とならない角度が望ましく、例えば、30度以上とすることが望ましい。
本発明によれば、自己位置算出部は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択するので、自己位置算出部は、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができる。なお、所定の角度は、明確に平行とならない角度が望ましく、例えば、30度以上とすることが望ましい。
本発明では、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出することが好ましい。
このような構成によれば、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するので、自己位置推定装置は、幾何特徴の組に複数の平面を含んでいる場合には、平面同士の組み合わせ数を削減することができ、ひいては処理時間を更に削減することができる。
本発明では、自己位置算出部は、平面および球体の現在幾何特徴および平面および球体の過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択することが好ましい。
このような構成によれば、自己位置算出部は、平面および球体の現在幾何特徴および平面および球体の過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択するので、自己位置推定装置は、幾何特徴の組に地面平面を含んでいる場合には、球体の選択数を削減することができ、ひいては処理時間を更に削減することができる。
以下、本発明の一実施形態を図面に基づいて説明する。
図1は、本発明の一実施形態に係る移動ロボットの自己位置推定装置の構成を示す図である。なお、本実施形態では、三次元空間における移動ロボット1の位置(並進成分)と、方向(回転成分)とを合わせて自己位置と呼ぶ。
図1は、本発明の一実施形態に係る移動ロボットの自己位置推定装置の構成を示す図である。なお、本実施形態では、三次元空間における移動ロボット1の位置(並進成分)と、方向(回転成分)とを合わせて自己位置と呼ぶ。
移動ロボット1は、自律走行可能な移動体である。この移動ロボット1は、図1に示すように、移動ロボット1の自己位置を推定する自己位置推定装置2と、モータ等の駆動部(図示略)、およびこの駆動部にて回転駆動される車輪(図示略)とを有し、移動ロボット1を自律走行させる移動手段3とを備えている。
自己位置推定装置2は、移動ロボット1の周囲の物体までの距離を三次元点群データとして検出する検出手段4と、移動ロボット1の自己位置を推定する推定手段5とを備えている。
自己位置推定装置2は、移動ロボット1の周囲の物体までの距離を三次元点群データとして検出する検出手段4と、移動ロボット1の自己位置を推定する推定手段5とを備えている。
検出手段4は、移動ロボット1の周囲にレーザービームを照射することで物体の有無を検出する三次元レーザースキャナあるいはLIDAR(Light Detection and Ranging)を有し、周囲形状を表す三次元点群データを取得する。三次元レーザースキャナは、赤外線レーザー等の装置(あるいはミラー)をセンサ中心回りに回転させながら、レーザービームを周囲に照射して、所定の角度ごとに物体の有無と距離を測定する。物体が存在した場合、その物体にレーザービームが当たった位置が、センサ中心からの距離と方向の形式で得られる。この距離と方向から、センサ中心を原点とするセンサ座標系での三次元点(x,y,z)Tが得られる。
したがって、このようなレーザー装置を、センサ中心軸に沿って複数台積載して回転させれば、三次元的に分布する三次元点群データを得ることができる。また、レーザー装置一台の場合には、レーザービームを縦と横の両方向に走査して、三次元点群データを得ることもできる。
検出手段4では、レーザースキャナの1周期(例えば、1回転360°)分で得られる三次元点群データをスキャンデータまたは単にスキャンと呼び、処理の単位として用いる。
したがって、このようなレーザー装置を、センサ中心軸に沿って複数台積載して回転させれば、三次元的に分布する三次元点群データを得ることができる。また、レーザー装置一台の場合には、レーザービームを縦と横の両方向に走査して、三次元点群データを得ることもできる。
検出手段4では、レーザースキャナの1周期(例えば、1回転360°)分で得られる三次元点群データをスキャンデータまたは単にスキャンと呼び、処理の単位として用いる。
推定手段5は、CPU(Central Processing Unit)や、メモリなどによって構成され、このメモリに記憶された所定のプログラムに従って情報処理を実行する。この推定手段5は、地図生成部51と、記憶部52と、幾何特徴抽出部53と、自己位置算出部54と、自己位置評価部55とを備えている。
地図生成部51は、検出手段4にて検出された三次元点群データに基づいて、移動ロボット1の周囲の地図を生成する。
記憶部52は、地図生成部51にて生成された地図を記憶する。なお、記憶部52は、地図生成部51にて生成された地図に加え、検出手段4にて検出された三次元点群データを記憶してもよい。
記憶部52は、地図生成部51にて生成された地図を記憶する。なお、記憶部52は、地図生成部51にて生成された地図に加え、検出手段4にて検出された三次元点群データを記憶してもよい。
幾何特徴抽出部53は、記憶部52に記憶された地図に基づいて、平面、直線、および球体の3種類の幾何特徴のうち、少なくともいずれか1種類の幾何特徴を移動ロボット1の現在位置周辺の地図としての現在地図から現在幾何特徴として抽出するとともに、移動ロボット1の過去位置周辺の地図としての過去地図から過去幾何特徴として抽出する。
図2は、平面、直線、および球体の3種類の幾何特徴を模式的に示す図である。
本実施形態では、幾何特徴は、図2に示すように、検出手段4にて検出された三次元点群データから抽出される幾何学的特徴を有する図形(平面、直線、および球体)の総称とする。
本実施形態では、幾何特徴は、図2に示すように、検出手段4にて検出された三次元点群データから抽出される幾何学的特徴を有する図形(平面、直線、および球体)の総称とする。
平面(P)の幾何特徴(幾何特徴P)は、図2のP1,P2のように、検出手段4にて検出された三次元点群データに基づいて、平坦な部分を連結して抽出したものである。この平面の幾何特徴は、例えば、ハフ変換や、RANSAC(Random Sample Consensus)や、主成分分析などを用いて抽出することができる。
直線(L)の幾何特徴(幾何特徴L)は、図2のL1,L2のように、検出手段4にて検出された三次元点群データに基づいて、直線的な部分を連結して抽出したものである。この直線の幾何特徴は、平面の幾何特徴と同様に、例えば、ハフ変換や、RANSACや、主成分分析などを用いて抽出することができる。
球体(B)の幾何特徴(幾何特徴B)は、図2のB1,B2のように、検出手段4にて検出された三次元点群データに基づいて、任意の三次元点Cを中心とした半径Rの球の中に含まれる三次元点群データを、この任意の点Cにて代表させて抽出したものである。幾何特徴Bは、この代表点Cで表す。なお、幾何特徴Bは、半径Rの球の内部に含まれる三次元点群の重心を代表として抽出するようにしてもよい。また、本発明の球体は、真球に限らず、楕円球や、立方体などの形状を含むものとする。換言すれば、幾何特徴Bは、一定の空間を有する三次元図形を1個の代表点で表したものである。
自己位置算出部54は、幾何特徴抽出部53にて抽出した現在幾何特徴のうち、2個または3個の現在幾何特徴を選択するとともに、幾何特徴抽出部53にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組とし、この幾何特徴の組に基づいて、過去地図における自己位置を算出する。
自己位置評価部55は、自己位置算出部54にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する。
以下、図3~図8に基づいて、自己位置算出部54にて移動ロボット1の自己位置を算出し、自己位置評価部55にて一致度の高い自己位置を選択する概念について説明する。
自己位置評価部55は、自己位置算出部54にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する。
以下、図3~図8に基づいて、自己位置算出部54にて移動ロボット1の自己位置を算出し、自己位置評価部55にて一致度の高い自己位置を選択する概念について説明する。
図3は、移動ロボットを走行させる実環境を鉛直上方側から見て平面的に示す模式図である。
移動ロボット1を走行させる実環境は、図3に示すように、地面Gと、地面Gに立設された壁Wや、地面Gに存在する樹木や障害物などの物体OB1~OB6を有している。
移動ロボット1は、このような実環境下を走行することによって、検出手段4にて検出された三次元点群データに基づいて、地図生成部51に移動ロボット1の周囲の地図を生成させる。
移動ロボット1を走行させる実環境は、図3に示すように、地面Gと、地面Gに立設された壁Wや、地面Gに存在する樹木や障害物などの物体OB1~OB6を有している。
移動ロボット1は、このような実環境下を走行することによって、検出手段4にて検出された三次元点群データに基づいて、地図生成部51に移動ロボット1の周囲の地図を生成させる。
図4は、移動ロボットの1回の走行において、例えばICP等の位置合せ手法を用いてスキャンをつなぎ合わせて生成された全体地図を示す模式図である。ここで、図4の矢印は、移動ロボットの移動軌跡を示している。
この例では、移動ロボットは位置PO2を通った後、位置PO1を通っているが、両位置は近い場所にあるため、三次元レーザースキャナによって同じ物体が計測される。しかしながら、移動とともに移動ロボットの位置の誤差が累積していくため、位置PO2で計測した物体と位置PO1で計測した物体は、同じ物体ではあるが、少しずれて地図上に記録されている。
なお、この図4では、物体のずれが明確に描かれているが、実際には三次元点群が何重にも重なって地図が歪んでいくことが多い。
この例では、移動ロボットは位置PO2を通った後、位置PO1を通っているが、両位置は近い場所にあるため、三次元レーザースキャナによって同じ物体が計測される。しかしながら、移動とともに移動ロボットの位置の誤差が累積していくため、位置PO2で計測した物体と位置PO1で計測した物体は、同じ物体ではあるが、少しずれて地図上に記録されている。
なお、この図4では、物体のずれが明確に描かれているが、実際には三次元点群が何重にも重なって地図が歪んでいくことが多い。
このような地図の歪みを解消するために、全体地図から抽出した部分地図を用いる。ここでは、位置PO1を現在位置として、位置PO1周辺の部分地図を現在地図と呼ぶ。図4では、PO1を中心とした点線の円内が現在地図である。また、位置PO2は過去に通った位置として、位置PO2周辺の部分地図を過去地図と呼ぶ。図4では、PO2を中心とした点線の円内が過去地図である。
図5は、現在地図および過去地図を表す模式図である。図5(A)は、図4の全体地図から位置PO1周辺を切り出した現在地図であり、図5(B)は、図4の全体地図から位置PO2周辺を切り出した過去地図である。なお、現在地図の範囲および過去地図の範囲の大きさは、同一とは限らない。また、現在地図の座標系および過去地図の座標系は、互いに異なっている。
次に、現在地図と過去地図が一致するように、現在地図の中心PO1における移動ロボット1の自己位置を求める。本実施形態では、自己位置算出部54にて移動ロボット1の自己位置を算出し、自己位置評価部55にて現在地図と過去地図の一致度の高い自己位置を選択する。
図6は、一致度の高い自己位置を用いて現在地図の配置を修正して、現在地図および過去地図を結合した全体地図を示す模式図である。
このように、本実施形態では、推定手段5は、図6に示すように、現在地図および過去地図の三次元点群データを、ずれることなく重ね合わせるように移動ロボット1の自己位置を推定する
このように、本実施形態では、推定手段5は、図6に示すように、現在地図および過去地図の三次元点群データを、ずれることなく重ね合わせるように移動ロボット1の自己位置を推定する
図7は、移動ロボットの自己位置推定装置による自己位置推定処理の流れを示すフローチャートである。
移動ロボット1の自己位置推定装置2による自己位置推定処理では、推定手段5は、図7に示すように、ステップST1~ST7の処理を実行する。
移動ロボット1の自己位置推定装置2による自己位置推定処理では、推定手段5は、図7に示すように、ステップST1~ST7の処理を実行する。
地図生成部51は、検出手段4にて検出された三次元点群データに基づいて、移動ロボット1の現在位置周辺の地図としての現在地図を生成し、記憶部52に記憶させる(ステップST1:現在地図生成ステップ)。
次に、推定手段5は、移動ロボット1の過去位置周辺の地図としての過去地図を記憶部52から取得する(ステップST2:過去地図取得ステップ)。
ここで、過去地図は、例えば、移動ロボット1の移動軌跡を辿って、現在位置に近い過去位置を選択し、この過去位置周辺の地図を取得してもよく、これまでに作成された全体地図(過去地図と現在地図とを統合することによって作成された環境地図)に基づいて、所定の範囲の領域を選択していくことによって、この過去位置周辺の地図を取得してもよい。
次に、推定手段5は、移動ロボット1の過去位置周辺の地図としての過去地図を記憶部52から取得する(ステップST2:過去地図取得ステップ)。
ここで、過去地図は、例えば、移動ロボット1の移動軌跡を辿って、現在位置に近い過去位置を選択し、この過去位置周辺の地図を取得してもよく、これまでに作成された全体地図(過去地図と現在地図とを統合することによって作成された環境地図)に基づいて、所定の範囲の領域を選択していくことによって、この過去位置周辺の地図を取得してもよい。
現在地図生成ステップST1において、現在地図を生成し、過去地図取得ステップST2において、過去地図を取得した後、幾何特徴抽出部53は、記憶部52に記憶された地図に基づいて、平面(P)、直線(L)、および球体(B)の3種類の幾何特徴を現在地図から現在幾何特徴として抽出するとともに、過去地図から過去幾何特徴として抽出する(ステップST3:幾何特徴抽出ステップ)。
なお、本実施形態では、幾何特徴抽出部53は、3種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出しているが、1種類の幾何特徴、または2種類の幾何特徴を抽出してもよい。要するに、幾何特徴抽出部は、平面、直線、および球体の3種類の幾何特徴のうち、少なくともいずれか1種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出すればよい。
なお、本実施形態では、幾何特徴抽出部53は、3種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出しているが、1種類の幾何特徴、または2種類の幾何特徴を抽出してもよい。要するに、幾何特徴抽出部は、平面、直線、および球体の3種類の幾何特徴のうち、少なくともいずれか1種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出すればよい。
自己位置算出部54は、幾何特徴抽出部53にて抽出した現在幾何特徴のうち、2個または3個の現在幾何特徴を選択するとともに、幾何特徴抽出部53にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して、現在幾何特徴と過去幾何特徴の組を作る(ステップST4:幾何特徴選択ステップ)。例えば、現在地図から幾何特徴PPL(平面P2個、直線L1個)を抽出し、過去地図からも幾何特徴PPLを抽出して、2個の幾何特徴PPLからなる組を作る。
そして、自己位置算出部54は、この幾何特徴の組に基づいて、現在の自己位置(図4のPO1)が過去地図でどこに位置するかを算出する(ステップST5:自己位置算出ステップ)。
そして、自己位置算出部54は、この幾何特徴の組に基づいて、現在の自己位置(図4のPO1)が過去地図でどこに位置するかを算出する(ステップST5:自己位置算出ステップ)。
この計算においては、2個または3個の現在幾何特徴と、同種同数の過去幾何特徴を正しく対応づける必要がある。しかしながら、どの幾何特徴が対応するかあらかじめわからないことが多いため、幾何特徴の各組合せに対して自己位置を計算し、その自己位置の正しさを検証する。
例えば、現在幾何特徴P1P2L1と過去幾何特徴P3P4L2から自己位置を計算する場合は2通りの組合せがある。1つはP1とP3、P2とP4、L1とL2を対応づける場合、もう1つはP1とP4、P2とP3、L1とL2を対応づける場合である。
例えば、現在幾何特徴P1P2L1と過去幾何特徴P3P4L2から自己位置を計算する場合は2通りの組合せがある。1つはP1とP3、P2とP4、L1とL2を対応づける場合、もう1つはP1とP4、P2とP3、L1とL2を対応づける場合である。
ここで、自己位置算出部54は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上(例えば、30度以上)とするように選択する。これは、例えば、前述の幾何特徴PPLを例にすると、2つの平面が平行な場合や、平面と直線が平行な場合、幾何特徴の組から一意に自己位置の計算ができなくなることがあるためである。
なお、本実施形態では、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように現在幾何特徴および過去幾何特徴を選択しているが、このような制限を設けなくてもよい。
なお、本実施形態では、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように現在幾何特徴および過去幾何特徴を選択しているが、このような制限を設けなくてもよい。
また、自己位置算出部54は、平面を含む現在幾何特徴および平面を含む過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定する。そして、自己位置算出部54は、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択する。
ここで、本実施形態では、移動ロボット1は、車輪を有する移動手段3を備えているので、地面平面が含まれているか否かの判定は、センサ等を用いて比較的容易に可能である。
なお、本実施形態では、自己位置算出部54は、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択しているが、このような制限を設けなくてもよい。
ここで、本実施形態では、移動ロボット1は、車輪を有する移動手段3を備えているので、地面平面が含まれているか否かの判定は、センサ等を用いて比較的容易に可能である。
なお、本実施形態では、自己位置算出部54は、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択しているが、このような制限を設けなくてもよい。
現在幾何特徴と過去幾何特徴の組から過去地図における自己位置を計算するには種々の方法があるが、回転成分および並進成分に分けて算出する方法が簡便である。
まず、回転成分は、2個以上の方向ベクトルの組に基づき算出する。ここで、1個の現在幾何特徴の方向ベクトルをdi
rとし、これに対応する1個の過去幾何特徴の方向ベクトルをdi
cとし、求める自己位置の回転成分を表す行列をRとすれば、これらの関係は、以下の式(1)のようになる。この式が2個以上あれば回転成分を計算できる。
そこで、回転成分は、三次元点群データの誤差を考慮して、以下の式(2),(3)のCを最小にすることによって、回転行列Rを求めて算出する。
ここで、方向ベクトルは、平面の法線ベクトル、直線の方向ベクトル、および2個の球体を繋いだ直線の方向ベクトルを用いる。自己位置算出部54は、例えば、幾何特徴PPP(平面P3個)、幾何特徴PPL(平面P2個と直線L1個)、幾何特徴PLL(平面P1個と直線L2個)を選択した場合には、3個の方向ベクトルを用いて回転成分を算出する。また、自己位置算出部54は、例えば、幾何特徴PBB(平面P1個と球体B2個)を選択した場合には、2個の方向ベクトル(平面の法線ベクトルおよび2個の球体を繋いだ直線の方向ベクトル)を用いて回転成分を算出する。また、幾何特徴LB(直線L1個と球体B1個)を選択した場合には、2個の方向ベクトル(直線の方向ベクトルおよび球体から直線に降ろした垂線の方向ベクトル)を用いて回転成分を算出する。なお、式(2),(3)のmは、方向ベクトルの個数である。
次に、並進成分は、現在幾何特徴および過去幾何特徴の種類に応じて算出する。現在幾何特徴の方向ベクトルおよび現在地図における三次元点群データは、先に求めた回転行列Rにて予め回転変換しておく。これによれば、互いに対応する幾何特徴の方向ベクトルは同じになる。そして、並進成分は、求める自己位置の並進ベクトルをtとし、現在幾何特徴および過去幾何特徴の種類ごとに以下の3つの式に基づき算出する。
平面(P)の幾何特徴の場合には、その法線ベクトルをniとし、現在地図の平面上の1点をci
cとし、過去地図の平面上の1点をci
rとすれば、これらの関係は、式(4),(5)のようになる。ここで、pは平面上の任意の点である。また、式(6)は、式(4),(5)に基づき得られる。
直線(L)の幾何特徴の場合には、その方向ベクトルをdiとし、現在地図の直線上の1点をci
cとし、過去地図の直線上の1点をci
rとすれば、これらの関係は、式(7),(8)のようになる。ここで、pは直線上の任意の点、u、vは直線方程式の媒介変数である。また、式(9)は、式(7),(8)に基づき得られる。
球体(B)の幾何特徴の場合には、現在地図の球体の代表位置をci
cとし、過去地図の球体の代表位置をci
rとすれば、これらの関係は、式(10)のようになる。
そして、並進ベクトルtに関する連立方程式である以下の式(11)は、現在幾何特徴および過去幾何特徴の種類に応じた式(6),(9),(10)に基づき得られる。
ここで、Aは、式(6),(9),(10)の各左辺をスタックした行列であり、qは、式(6),(9),(10)の各右辺をスタックしたベクトルである。この連立方程式は、2個または3個の現在幾何特徴を選択するとともに、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組としていれば、並進ベクトルt=(x,y,z)の3変数に対して解くことができる。
ここで、自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定する。そして、自己位置算出部54は、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないようにする。
図8は、幾何特徴間の角度または距離の例を示す模式図である。
ここで、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴PPLを抽出した場合には、図8(A)に示すように、平面P1および平面P2のなす角や、平面P1および直線L1のなす角a1や、平面P2および直線L1の距離d1を用いることができる。
また、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴PLLを抽出した場合には、図8(B)に示すように、平面P3および直線L2,L3のなす角や、直線L2および直線L3の距離d2を用いることができる。
ここで、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴PPLを抽出した場合には、図8(A)に示すように、平面P1および平面P2のなす角や、平面P1および直線L1のなす角a1や、平面P2および直線L1の距離d1を用いることができる。
また、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴PLLを抽出した場合には、図8(B)に示すように、平面P3および直線L2,L3のなす角や、直線L2および直線L3の距離d2を用いることができる。
また、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴PBBを抽出した場合には、図8(C)に示すように、平面P4および球体B1,B2距離h1,h2や、球体B1および球体B2の距離d3を用いることができる。
さらに、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴LBを抽出した場合には、図8(D)に示すように、直線L4および球体B3の距離d4を用いることができる。
さらに、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部53にて幾何特徴LBを抽出した場合には、図8(D)に示すように、直線L4および球体B3の距離d4を用いることができる。
なお、本実施形態では、自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないようになっているが、この幾何特徴の組に基づいて、過去地図における自己位置を算出してもよい。
また、自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定する。そして、自己位置算出部54は、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出する。
なお、本実施形態では、自己位置算出部54は、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するが、地面平面を対応させて過去地図における自己位置を算出しなくてもよい。
なお、本実施形態では、自己位置算出部54は、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するが、地面平面を対応させて過去地図における自己位置を算出しなくてもよい。
自己位置算出ステップST5において、過去地図における自己位置を算出した後、自己位置評価部55は、自己位置算出部54にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する(ステップST6:自己位置評価ステップ)。
具体的には、自己位置評価部55は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価する。
はじめに、自己位置評価部55は、自己位置算出部54にて算出された自己位置(仮説)をposeとし、現在地図の球体群をPcとし、過去地図の球体群をPrとする。
そして、以下の(A1)~(A5)の手順に従って、幾何特徴の組ごとに現在地図および過去地図の一致度を評価する。
そして、以下の(A1)~(A5)の手順に従って、幾何特徴の組ごとに現在地図および過去地図の一致度を評価する。
(A1)スコアS=0とする。
(A2)pi c∈Pcをposeにて座標変換したものをqi cとする。
(A3)qi cの半径r内にPrの点が存在する場合には、S=S+1とする。
(A4)Pcのすべての点において、上記(A2)~(A3)の処理を繰り返す。
(A5)スコアSが閾値sthre以上の場合には、自己位置(仮説)は、正しいとする。
(A2)pi c∈Pcをposeにて座標変換したものをqi cとする。
(A3)qi cの半径r内にPrの点が存在する場合には、S=S+1とする。
(A4)Pcのすべての点において、上記(A2)~(A3)の処理を繰り返す。
(A5)スコアSが閾値sthre以上の場合には、自己位置(仮説)は、正しいとする。
ここで、poseにてpi
cを座標変換する場合には、以下の式(12)を用いる。Rは、poseの回転行列であり、tは、poseの並進ベクトルである。
なお、本実施形態では、上記(A3)の処理は、kd木を用いた最近傍探索にて行っている。これに対して、自己位置評価部55は、上記(A3)の処理に代えて、例えば、Prの点のうち、qi
cに最も近いものを求め、その点までの距離が半径r内であるか否かに基づいて、Sを更新する処理を採用してもよい。
また、自己位置評価部55は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価する。換言すれば、自己位置評価部55は、以下の(B1)~(B4)の手順に従って、球体群の個数を増加させつつ階層的に一致度を評価していく。
(B1)k回目の評価における球体群の数をmkとし、スコアSの閾値をsthrekとする。
(B2)上記(A1)~(A5)の処理を行って、スコアSが閾値をsthrek以上の自己位置(仮説)を残す。
(B3)残った自己位置(仮説)のスコアSの最大値smaxkを求め、スコアSがratio×smaxk未満の自己位置(仮説)は削除する。
(B4)mkを増加させ、sthrekを大きくし、(B2)の処理に戻る。その後、(B2)~(B4)の処理を所定の回数だけ繰り返す。
(B2)上記(A1)~(A5)の処理を行って、スコアSが閾値をsthrek以上の自己位置(仮説)を残す。
(B3)残った自己位置(仮説)のスコアSの最大値smaxkを求め、スコアSがratio×smaxk未満の自己位置(仮説)は削除する。
(B4)mkを増加させ、sthrekを大きくし、(B2)の処理に戻る。その後、(B2)~(B4)の処理を所定の回数だけ繰り返す。
なお、本実施形態では、ratioの値として0.85を設定したが、これとは異なる値に設定してもよい。
そして、自己位置評価部55は、最後に残った自己位置(仮説)をスコアSの順に並べ、上位の自己位置(仮説)を一致度の高い自己位置として選択する。なお、ここで選択する自己位置は、1個であってもよく、2個以上の複数個であってもよい。2個以上の複数個の自己位置を選択した場合には、後述の自己位置推定ステップST7において、これらの自己位置を再評価してもよい。
自己位置評価ステップST6において、自己位置評価部55は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定する(ステップST7:自己位置推定ステップ)。
具体的には、自己位置評価部55は、自己位置評価ステップST6において、一致度の高い自己位置として選択された自己位置(仮説)をposeとし、現在地図の三次元点群データをPcとし、過去地図の三次元点群データをPrとする。
そして、自己位置評価部55は、ICP等の位置合せ手法を用いて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定する。
そして、自己位置評価部55は、ICP等の位置合せ手法を用いて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定する。
なお、本実施形態では、自己位置評価部55は、前述した(B1)~(B4)の手順に従って、球体群の個数を増加させつつ階層的に一致度を評価しているが、階層的に一致度を評価しなくてもよい。
また、本実施形態では、自己位置推定ステップST7において、自己位置評価部55は、前述した(A1)~(A5)の手順に従って、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定しているが、自己位置推定ステップST7は、省略されていてもよい。この場合には、自己位置評価ステップST6において、自己位置評価部55にて選択した自己位置を過去地図における自己位置として推定すればよい。
また、本実施形態では、自己位置推定ステップST7において、自己位置評価部55は、前述した(A1)~(A5)の手順に従って、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定しているが、自己位置推定ステップST7は、省略されていてもよい。この場合には、自己位置評価ステップST6において、自己位置評価部55にて選択した自己位置を過去地図における自己位置として推定すればよい。
このような本実施形態によれば、以下の作用・効果を奏することができる。
(1)自己位置算出部54は、三次元点よりも個数が大幅に少ない幾何特徴を用いるため、その組合せも少なくて済む。三次元点を用いて自己位置を求める場合、現在地図および過去地図からそれぞれ3個の三次元点を選び、その組を用いて自己位置の計算を行う。三次元点は膨大な個数(数千個から数万個)があるため、その組合せも膨大であり、その中から一致度の高い組を求めるには多大な処理時間がかかる。一方、本実施形態の幾何特徴は、三次元点に比べて個数が少なく(数個から数百個)、組合せ数も少なくて済む。したがって、自己位置推定装置2は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができる。
(1)自己位置算出部54は、三次元点よりも個数が大幅に少ない幾何特徴を用いるため、その組合せも少なくて済む。三次元点を用いて自己位置を求める場合、現在地図および過去地図からそれぞれ3個の三次元点を選び、その組を用いて自己位置の計算を行う。三次元点は膨大な個数(数千個から数万個)があるため、その組合せも膨大であり、その中から一致度の高い組を求めるには多大な処理時間がかかる。一方、本実施形態の幾何特徴は、三次元点に比べて個数が少なく(数個から数百個)、組合せ数も少なくて済む。したがって、自己位置推定装置2は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができる。
(2)自己位置評価部55は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価するので、三次元点群データよりも個数の少ない球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価することができる。したがって、自己位置推定装置2は、処理時間を更に削減することができる。
(3)自己位置評価部55は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価するので、球体群の個数を段階的に増加させつつ所定の閾値よりも一致度の高い自己位置を絞り込んでいくことができる。したがって、自己位置推定装置2は、更に精度よく自己位置を推定することができる。
(3)自己位置評価部55は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価するので、球体群の個数を段階的に増加させつつ所定の閾値よりも一致度の高い自己位置を絞り込んでいくことができる。したがって、自己位置推定装置2は、更に精度よく自己位置を推定することができる。
(4)自己位置評価部55は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、ICP等の位置合せ手法を用いて、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定するので、自己位置推定装置2は、はじめから三次元点群データに基づき自己位置を推定する場合と比較して処理時間を削減することができ、しかも精度よく自己位置を推定することができる。
(5)自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置2は、処理時間を更に削減することができる。
(5)自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置2は、処理時間を更に削減することができる。
(6)自己位置算出部54は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上(例えば、30度以上)とするように選択するので、自己位置算出部54は、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができる。
(7)自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するので、自己位置推定装置2は、幾何特徴の組に複数の平面を含んでいる場合には、平面同士の組み合わせ数を削減することができ、ひいては処理時間を更に削減することができる。
(7)自己位置算出部54は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するので、自己位置推定装置2は、幾何特徴の組に複数の平面を含んでいる場合には、平面同士の組み合わせ数を削減することができ、ひいては処理時間を更に削減することができる。
(8)自己位置算出部54は、平面および球体の現在幾何特徴および平面および球体の過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択するので、自己位置推定装置2は、幾何特徴の組に地面平面を含んでいる場合には、球体の選択数を削減することができ、ひいては処理時間を更に削減することができる。
〔実施形態の変形〕
なお、本発明は、前記実施形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前記実施形態では、移動体としての移動ロボット1は、具体的なものを例示していなかったが、サービスロボット、ホームロボットなどの移動ロボットを例示できる。より具体的には、移動体は、掃除ロボット、警備ロボット、運搬ロボット、案内ロボットなどを例示できる。また、移動体は、自動運転自動車や、作業車などであってもよい。
なお、本発明は、前記実施形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前記実施形態では、移動体としての移動ロボット1は、具体的なものを例示していなかったが、サービスロボット、ホームロボットなどの移動ロボットを例示できる。より具体的には、移動体は、掃除ロボット、警備ロボット、運搬ロボット、案内ロボットなどを例示できる。また、移動体は、自動運転自動車や、作業車などであってもよい。
前記実施形態では、移動ロボット1は、自己位置推定装置2を備え、この自己位置推定装置2は、地図生成部51等の機能を備えていた。これに対して、移動体は、自己位置推定装置を備えていなくてもよい。例えば、自己位置推定装置は、移動体と通信可能な他の機器に設けられていてもよい。
前記実施形態では、移動ロボット1は、移動手段3を備えていたが、これを備えていなくてもよく、例えば、自己位置推定装置を備えた台車等を使用者の手作業にて移動させるような構成であってもよい。
前記実施形態では、移動ロボット1は、移動手段3を備えていたが、これを備えていなくてもよく、例えば、自己位置推定装置を備えた台車等を使用者の手作業にて移動させるような構成であってもよい。
以上のように、本発明は、移動体の自己位置推定装置に好適に利用できる。
1 移動ロボット
2 自己位置推定装置
3 移動手段
4 検出手段
5 推定手段
51 地図生成部
52 記憶部
53 幾何特徴抽出部
54 自己位置算出部
55 自己位置評価部
2 自己位置推定装置
3 移動手段
4 検出手段
5 推定手段
51 地図生成部
52 記憶部
53 幾何特徴抽出部
54 自己位置算出部
55 自己位置評価部
Claims (8)
- 移動体の自己位置を推定する自己位置推定装置であって、
前記移動体の周囲の物体までの距離を三次元点群データとして検出する検出手段と、
前記移動体の自己位置を推定する推定手段とを備え、
前記推定手段は、
前記検出手段にて検出された三次元点群データに基づいて、前記移動体の周囲の地図を生成する地図生成部と、
前記地図生成部にて生成された地図を記憶する記憶部と、
前記記憶部に記憶された地図に基づいて、平面、直線、および球体の3種類の幾何特徴のうち、少なくともいずれか1種類の幾何特徴を前記移動体の現在位置周辺の地図としての現在地図から現在幾何特徴として抽出するとともに、前記移動体の過去位置周辺の地図としての過去地図から過去幾何特徴として抽出する幾何特徴抽出部と、
前記幾何特徴抽出部にて抽出した現在幾何特徴のうち、2個または3個の現在幾何特徴を選択するとともに、前記幾何特徴抽出部にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組とし、前記幾何特徴の組に基づいて、前記過去地図における自己位置を算出する自己位置算出部と、
前記自己位置算出部にて算出された自己位置に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を評価し、一致度の高い自己位置を選択する自己位置評価部とを備えることを特徴とする自己位置推定装置。 - 請求項1に記載された自己位置推定装置において、
前記自己位置評価部は、前記現在地図および前記過去地図のそれぞれから球体群を抽出し、当該球体群に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を評価することを特徴とする自己位置推定装置。 - 請求項2に記載された自己位置推定装置において、
前記自己位置評価部は、所定の閾値よりも一致度の高い自己位置に対し、前記現在地図および前記過去地図のそれぞれから球体群の個数を増加させて抽出し、当該球体群に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を再び評価することを特徴とする自己位置推定装置。 - 請求項2または請求項3に記載された自己位置推定装置において、
前記自己位置評価部は、前記球体群に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を評価し、一致度の高い自己位置を選択した後、当該自己位置に基づいて、前記現在地図に係る三次元点群データと、前記過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定することを特徴とする自己位置推定装置。 - 請求項1から請求項4のいずれかに記載された自己位置推定装置において、
前記自己位置算出部は、前記幾何特徴の組に含まれる前記現在幾何特徴間の角度または距離と、当該幾何特徴の組に含まれる前記過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、当該幾何特徴の組に基づいて、前記過去地図における自己位置を算出しないことを特徴とする自己位置推定装置。 - 請求項1から請求項5のいずれかに記載された自己位置推定装置において、
前記自己位置算出部は、前記現在幾何特徴および前記過去幾何特徴を前記幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択することを特徴とする自己位置推定装置。 - 請求項1から請求項6のいずれかに記載された自己位置推定装置において、
前記自己位置算出部は、前記幾何特徴の組に含まれる前記現在幾何特徴、および当該幾何特徴の組に含まれる前記過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、前記地面平面が含まれていると判定した場合には、当該幾何特徴の組における前記地面平面を対応させて前記過去地図における自己位置を算出することを特徴とする自己位置推定装置。 - 請求項1から請求項7のいずれかに記載された自己位置推定装置において、
前記自己位置算出部は、平面および球体の前記現在幾何特徴および平面および球体の前記過去幾何特徴を前記幾何特徴の組として選択する場合に、当該幾何特徴の組に含まれる前記現在幾何特徴、および当該幾何特徴の組に含まれる前記過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、前記地面平面が含まれていると判定した場合には、前記地面平面から所定の高さに位置する球体のみを選択することを特徴とする自己位置推定装置。
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/JP2020/011218 WO2021181686A1 (ja) | 2020-03-13 | 2020-03-13 | 自己位置推定装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPWO2021181686A1 JPWO2021181686A1 (ja) | 2021-09-16 |
JP7199772B2 true JP7199772B2 (ja) | 2023-01-06 |
Family
ID=77670960
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2022505711A Active JP7199772B2 (ja) | 2020-03-13 | 2020-03-13 | 自己位置推定装置 |
Country Status (5)
Country | Link |
---|---|
US (1) | US20230110609A1 (ja) |
EP (1) | EP4120039A4 (ja) |
JP (1) | JP7199772B2 (ja) |
CN (1) | CN115210668A (ja) |
WO (1) | WO2021181686A1 (ja) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004280138A (ja) | 2003-03-12 | 2004-10-07 | Nissan Motor Co Ltd | 道路形状認識装置 |
JP2015144013A (ja) | 2015-03-31 | 2015-08-06 | 株式会社日立製作所 | ロボットシステム及び地図更新方法 |
JP2017117386A (ja) | 2015-12-25 | 2017-06-29 | 学校法人千葉工業大学 | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3144765B1 (en) * | 2015-09-18 | 2020-01-08 | Samsung Electronics Co., Ltd. | Apparatus for localizing cleaning robot, cleaning robot, and controlling method of cleaning robot |
JP6528641B2 (ja) | 2015-10-26 | 2019-06-12 | トヨタ自動車株式会社 | 自己位置推定方法 |
US11927965B2 (en) * | 2016-02-29 | 2024-03-12 | AI Incorporated | Obstacle recognition method for autonomous robots |
GB2559157A (en) * | 2017-01-27 | 2018-08-01 | Ucl Business Plc | Apparatus, method and system for alignment of 3D datasets |
CN108732584B (zh) * | 2017-04-17 | 2020-06-30 | 百度在线网络技术(北京)有限公司 | 用于更新地图的方法和装置 |
CN111492265B (zh) * | 2017-09-13 | 2024-03-26 | 威力登激光雷达美国有限公司 | 基于3d激光雷达测量的多分辨率、同时定位和地图绘制 |
JP7139762B2 (ja) * | 2018-07-31 | 2022-09-21 | カシオ計算機株式会社 | 自律移動装置、自律移動方法及びプログラム |
JP6988873B2 (ja) * | 2019-11-18 | 2022-01-05 | トヨタ自動車株式会社 | 位置推定装置および位置推定用コンピュータプログラム |
-
2020
- 2020-03-13 US US17/905,015 patent/US20230110609A1/en active Pending
- 2020-03-13 EP EP20924757.6A patent/EP4120039A4/en active Pending
- 2020-03-13 WO PCT/JP2020/011218 patent/WO2021181686A1/ja active Application Filing
- 2020-03-13 CN CN202080097822.XA patent/CN115210668A/zh active Pending
- 2020-03-13 JP JP2022505711A patent/JP7199772B2/ja active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004280138A (ja) | 2003-03-12 | 2004-10-07 | Nissan Motor Co Ltd | 道路形状認識装置 |
JP2015144013A (ja) | 2015-03-31 | 2015-08-06 | 株式会社日立製作所 | ロボットシステム及び地図更新方法 |
JP2017117386A (ja) | 2015-12-25 | 2017-06-29 | 学校法人千葉工業大学 | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム |
Also Published As
Publication number | Publication date |
---|---|
US20230110609A1 (en) | 2023-04-13 |
EP4120039A1 (en) | 2023-01-18 |
JPWO2021181686A1 (ja) | 2021-09-16 |
WO2021181686A1 (ja) | 2021-09-16 |
CN115210668A (zh) | 2022-10-18 |
EP4120039A4 (en) | 2023-11-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112476434B (zh) | 一种基于协作机器人的视觉3d取放方法及系统 | |
CN107741234B (zh) | 一种基于视觉的离线地图构建及定位方法 | |
Nieto et al. | Recursive scan-matching SLAM | |
Veľas et al. | Calibration of rgb camera with velodyne lidar | |
CN113409410B (zh) | 一种基于3d激光雷达的多特征融合igv定位与建图方法 | |
Luck et al. | Registration of range data using a hybrid simulated annealing and iterative closest point algorithm | |
CN112525202A (zh) | 一种基于多传感器融合的slam定位导航方法及系统 | |
Song et al. | CAD-based pose estimation design for random bin picking using a RGB-D camera | |
US8116519B2 (en) | 3D beverage container localizer | |
CN115774265B (zh) | 用于工业机器人的二维码和激光雷达融合定位方法与装置 | |
JP2014063475A (ja) | 情報処理装置、情報処理方法、及びコンピュータプログラム | |
US20220390954A1 (en) | Topology Processing for Waypoint-based Navigation Maps | |
Rodrigues et al. | Low-level active visual navigation: Increasing robustness of vision-based localization using potential fields | |
US20230419531A1 (en) | Apparatus and method for measuring, inspecting or machining objects | |
Olson et al. | Wide-baseline stereo vision for Mars rovers | |
Zhu et al. | Indoor multi-robot cooperative mapping based on geometric features | |
Yoon et al. | Targetless multiple camera-LiDAR extrinsic calibration using object pose estimation | |
Xu et al. | A new monocular vision measurement method to estimate 3D positions of objects on floor | |
JP7199772B2 (ja) | 自己位置推定装置 | |
Kotake et al. | A fast initialization method for edge-based registration using an inclination constraint | |
Parra et al. | A novel method to estimate the position of a mobile robot in underfloor environments using RGB-D point clouds | |
Kolu et al. | A mapping method tolerant to calibration and localization errors based on tilting 2D laser scanner | |
Hwang et al. | Robust 2D map building with motion-free ICP algorithm for mobile robot navigation | |
Melcher et al. | Automated data acquisition and processing for factory layout planning | |
CN111915632A (zh) | 一种基于机器学习的贫纹理目标物体真值数据库构建方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220509 |
|
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: 20221213 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20221214 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7199772 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |