ホーム > 特許ランキング > 学校法人千葉工業大学
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2022-12-23
(45)【発行日】2023-01-06
(54)【発明の名称】自己位置推定装置
(51)【国際特許分類】
G01C 21/30 20060101AFI20221226BHJP
G05D 1/02 20200101ALI20221226BHJP
【ＦＩ】
G01C21/30
G05D1/02 H
【請求項の数】
8
(21)【出願番号】P 2022505711
(86)(22)【出願日】2020-03-13
(86)【国際出願番号】 JP2020011218
(87)【国際公開番号】W WO2021181686
(87)【国際公開日】2021-09-16
【審査請求日】2022-05-09
(73)【特許権者】
【識別番号】598163064
【氏名又は名称】学校法人千葉工業大学
(74)【代理人】
【識別番号】100167900
【弁理士】
【氏名又は名称】福井 仁
(72)【発明者】
【氏名】友納 正裕
【審査官】白石 剛史
(56)【参考文献】
【文献】特開２０１７－１１７３８６（ＪＰ，Ａ）
【文献】特開２０１５－１４４０１３（ＪＰ，Ａ）
【文献】特開２００４－２８０１３８（ＪＰ，Ａ）
【文献】特開２０２０－２１２５７（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｃ ２１／３０
Ｇ０５Ｄ １／０２
(57)【特許請求の範囲】
【請求項1】
移動体の自己位置を推定する自己位置推定装置であって、前記移動体の周囲の物体までの距離を三次元点群データとして検出する検出手段と、
前記移動体の自己位置を推定する推定手段とを備え、
前記推定手段は、
前記検出手段にて検出された三次元点群データに基づいて、前記移動体の周囲の地図を生成する地図生成部と、
前記地図生成部にて生成された地図を記憶する記憶部と、
前記記憶部に記憶された地図に基づいて、平面、直線、および球体の３種類の幾何特徴のうち、少なくともいずれか１種類の幾何特徴を前記移動体の現在位置周辺の地図としての現在地図から現在幾何特徴として抽出するとともに、前記移動体の過去位置周辺の地図としての過去地図から過去幾何特徴として抽出する幾何特徴抽出部と、
前記幾何特徴抽出部にて抽出した現在幾何特徴のうち、２個または３個の現在幾何特徴を選択するとともに、前記幾何特徴抽出部にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組とし、前記幾何特徴の組に基づいて、前記過去地図における自己位置を算出する自己位置算出部と、
前記自己位置算出部にて算出された自己位置に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を評価し、一致度の高い自己位置を選択する自己位置評価部とを備えることを特徴とする自己位置推定装置。
【請求項2】
請求項１に記載された自己位置推定装置において、前記自己位置評価部は、前記現在地図および前記過去地図のそれぞれから球体群を抽出し、当該球体群に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を評価することを特徴とする自己位置推定装置。
【請求項3】
請求項２に記載された自己位置推定装置において、前記自己位置評価部は、所定の閾値よりも一致度の高い自己位置に対し、前記現在地図および前記過去地図のそれぞれから球体群の個数を増加させて抽出し、当該球体群に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を再び評価することを特徴とする自己位置推定装置。
【請求項4】
請求項２または請求項３に記載された自己位置推定装置において、前記自己位置評価部は、前記球体群に基づいて、前記幾何特徴の組ごとに前記現在地図および前記過去地図の一致度を評価し、一致度の高い自己位置を選択した後、当該自己位置に基づいて、前記現在地図に係る三次元点群データと、前記過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定することを特徴とする自己位置推定装置。
【請求項5】
請求項１から請求項４のいずれかに記載された自己位置推定装置において、前記自己位置算出部は、前記幾何特徴の組に含まれる前記現在幾何特徴間の角度または距離と、当該幾何特徴の組に含まれる前記過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、当該幾何特徴の組に基づいて、前記過去地図における自己位置を算出しないことを特徴とする自己位置推定装置。
【請求項6】
請求項１から請求項５のいずれかに記載された自己位置推定装置において、前記自己位置算出部は、前記現在幾何特徴および前記過去幾何特徴を前記幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択することを特徴とする自己位置推定装置。
【請求項7】
請求項１から請求項６のいずれかに記載された自己位置推定装置において、前記自己位置算出部は、前記幾何特徴の組に含まれる前記現在幾何特徴、および当該幾何特徴の組に含まれる前記過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、前記地面平面が含まれていると判定した場合には、当該幾何特徴の組における前記地面平面を対応させて前記過去地図における自己位置を算出することを特徴とする自己位置推定装置。
【請求項8】
請求項１から請求項７のいずれかに記載された自己位置推定装置において、前記自己位置算出部は、平面および球体の前記現在幾何特徴および平面および球体の前記過去幾何特徴を前記幾何特徴の組として選択する場合に、当該幾何特徴の組に含まれる前記現在幾何特徴、および当該幾何特徴の組に含まれる前記過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、前記地面平面が含まれていると判定した場合には、前記地面平面から所定の高さに位置する球体のみを選択することを特徴とする自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の自己位置推定装置に関する。
【背景技術】
【0002】
従来、移動ロボットや自動運転車等の自律走行可能な移動体が自律走行するための技術として、ＳＬＡＭ(Simultaneous Localization and Mapping) と称される自己位置推定と環境地図作成の技術が用いられている。環境地図としては、移動体の移動範囲における物体の有無に基づいて作成された占有格子地図や点群地図が用いられることがある。占有格子地図は、移動範囲の平面や空間を複数の区画（セル）に分割して記憶するとともに、分割した各区画に対して物体の有無に応じたセル値が付与されている。点群地図は、移動範囲の平面や空間に存在する物体を微小領域ごとに離散化した点（座標）として表し、その集合である点群データが地図として利用される。
【0003】
移動体が目的地まで自律走行する場合、現在地から目的地までの移動経路を求め、その移動経路に沿って目的地まで走行する。この際、効率よく移動するために、あらかじめ作成しておいた地図（既存地図）を用いて移動経路を算出し、その既存地図上で移動体が自己位置を確認（推定）しながら走行することが行われる。このような既存地図上での自己位置推定は、移動体の自律走行にとって不可欠の技術である。
【0004】
移動体の自己位置推定の技術として、距離センサの出力で得られる距離データを用いて、予め準備した環境地図に対して移動体の位置合わせを行って自己位置を推定する方法が一般的である。移動体の位置合わせの方法として、例えば、ＩＣＰ（Iterative Closest Point）といわれる方法があり、この方法では、点群データから抽出した点の集合の幾何学的特徴に基づき、環境地図の特徴部位と照合することにより移動体の位置を判別する（例えば、特許文献１参照）。特許文献１に記載の自己位置推定方法は、距離センサによって周辺を観測することによって、物体表面を示す複数の三次元点を取得し、その観測結果に基づいて、近距離の三次元点と、特徴量が所定の閾値以上である遠距離の三次元点とを抽出し、抽出した複数の三次元点と、環境地図における複数の三次元点とをマッチングすることによって自己位置を推定している。
【先行技術文献】
【特許文献】
【0005】
【文献】特開２０１７－０８３２３０号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、特許文献１に記載された発明では、複数の三次元点をマッチングすることによって自己位置を推定しているので、屋外環境などのように三次元点（三次元点群データ）の個数が多い環境にて自己位置を推定する場合には、処理時間は長くなってしまうという問題がある。
【0007】
本発明の目的は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができ、精度よく自己位置を推定することができる自己位置推定装置を提供することである。
【課題を解決するための手段】
【0008】
本発明の自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、移動体の周囲の物体までの距離を三次元点群データとして検出する検出手段と、移動体の自己位置を推定する推定手段とを備え、推定手段は、検出手段にて検出された三次元点群データに基づいて、移動体の周囲の地図を生成する地図生成部と、地図生成部にて生成された地図を記憶する記憶部と、記憶部に記憶された地図に基づいて、平面、直線、および球体の３種類の幾何特徴のうち、少なくともいずれか１種類の幾何特徴を移動体の現在位置周辺の地図としての現在地図から現在幾何特徴として抽出するとともに、移動体の過去位置周辺の地図としての過去地図から過去幾何特徴として抽出する幾何特徴抽出部と、幾何特徴抽出部にて抽出した現在幾何特徴のうち、２個または３個の現在幾何特徴を選択するとともに、幾何特徴抽出部にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組とし、幾何特徴の組に基づいて、過去地図における自己位置を算出する自己位置算出部と、自己位置算出部にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する自己位置評価部とを備えることを特徴とする。
【0009】
このような構成によれば、自己位置算出部は、三次元点よりも個数が大幅に少ない幾何特徴を用いるため、その組合せも少なくて済む。三次元点を用いて自己位置を求める場合、現在地図および過去地図からそれぞれ３個の三次元点を選び、その組を用いて自己位置の計算を行う。三次元点は膨大な個数（数千個から数万個）があるため、その組合せも膨大であり、その中から一致度の高い組を求めるには多大な処理時間がかかる。一方、本発明の幾何特徴は、三次元点に比べて個数が少なく（数個から数百個）、組合せ数も少なくて済む。したがって、自己位置推定装置は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができる。
【0010】
本発明では、自己位置評価部は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価することが好ましい。
【0011】
このような構成によれば、自己位置評価部は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価するので、三次元点群データよりも個数の少ない球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価することができる。したがって、自己位置推定装置は、処理時間を更に削減することができる。
【0012】
本発明では、自己位置評価部は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価することが好ましい。
【0013】
このような構成によれば、自己位置評価部は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価するので、球体群の個数を段階的に増加させつつ所定の閾値よりも一致度の高い自己位置を絞り込んでいくことができる。したがって、自己位置推定装置は、更に精度よく自己位置を推定することができる。
【0014】
本発明では、自己位置評価部は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定することが好ましい。
【0015】
このような構成によれば、自己位置評価部は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定するので、自己位置推定装置は、はじめから三次元点群データに基づき自己位置を推定する場合と比較して処理時間を削減することができ、しかも精度よく自己位置を推定することができる。
【0016】
本発明では、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないことが好ましい。
【0017】
ここで、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間の整合性は、過去地図における自己位置を算出すべく座標変換しても変化することはないので、整合性のない幾何特徴の組に基づき算出された自己位置に基づいて、現在地図および過去地図の一致度を評価した場合には、その一致度は低くなると考えられる。
本発明によれば、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置は、処理時間を更に削減することができる。
本発明によれば、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置は、処理時間を更に削減することができる。
【0018】
本発明では、自己位置算出部は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択することが好ましい。
【0019】
ここで、自己位置算出部は、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度が平行に近い場合には、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができない場合がある。
本発明によれば、自己位置算出部は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択するので、自己位置算出部は、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができる。なお、所定の角度は、明確に平行とならない角度が望ましく、例えば、３０度以上とすることが望ましい。
本発明によれば、自己位置算出部は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように選択するので、自己位置算出部は、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができる。なお、所定の角度は、明確に平行とならない角度が望ましく、例えば、３０度以上とすることが望ましい。
【0020】
本発明では、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出することが好ましい。
【0021】
このような構成によれば、自己位置算出部は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するので、自己位置推定装置は、幾何特徴の組に複数の平面を含んでいる場合には、平面同士の組み合わせ数を削減することができ、ひいては処理時間を更に削減することができる。
【0022】
本発明では、自己位置算出部は、平面および球体の現在幾何特徴および平面および球体の過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択することが好ましい。
【0023】
このような構成によれば、自己位置算出部は、平面および球体の現在幾何特徴および平面および球体の過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択するので、自己位置推定装置は、幾何特徴の組に地面平面を含んでいる場合には、球体の選択数を削減することができ、ひいては処理時間を更に削減することができる。
【図面の簡単な説明】
【0024】
【図1】本発明の一実施形態に係る移動ロボットの自己位置推定装置の構成を示す図
【図2】平面、直線、および球体の３種類の幾何特徴を模式的に示す図
【図3】移動ロボットを走行させる実環境を鉛直上方側から見て平面的に示す模式図
【図4】移動ロボットの１回の走行において、例えばＩＣＰ等の位置合せ手法を用いてスキャンをつなぎ合わせて生成された全体地図を示す模式図
【図5】現在地図および過去地図を表す模式図
【図6】一致度の高い自己位置を用いて現在地図の配置を修正して、現在地図および過去地図を結合した全体地図を示す模式図
【図7】移動ロボットの自己位置推定装置による自己位置推定処理の流れを示すフローチャート
【図8】幾何特徴間の角度または距離の例を示す模式図
【発明を実施するための形態】
【0025】
以下、本発明の一実施形態を図面に基づいて説明する。
図１は、本発明の一実施形態に係る移動ロボットの自己位置推定装置の構成を示す図である。なお、本実施形態では、三次元空間における移動ロボット１の位置（並進成分）と、方向（回転成分）とを合わせて自己位置と呼ぶ。
図１は、本発明の一実施形態に係る移動ロボットの自己位置推定装置の構成を示す図である。なお、本実施形態では、三次元空間における移動ロボット１の位置（並進成分）と、方向（回転成分）とを合わせて自己位置と呼ぶ。
【0026】
移動ロボット１は、自律走行可能な移動体である。この移動ロボット１は、図１に示すように、移動ロボット１の自己位置を推定する自己位置推定装置２と、モータ等の駆動部（図示略）、およびこの駆動部にて回転駆動される車輪（図示略）とを有し、移動ロボット１を自律走行させる移動手段３とを備えている。
自己位置推定装置２は、移動ロボット１の周囲の物体までの距離を三次元点群データとして検出する検出手段４と、移動ロボット１の自己位置を推定する推定手段５とを備えている。
自己位置推定装置２は、移動ロボット１の周囲の物体までの距離を三次元点群データとして検出する検出手段４と、移動ロボット１の自己位置を推定する推定手段５とを備えている。
【0027】
検出手段４は、移動ロボット１の周囲にレーザービームを照射することで物体の有無を検出する三次元レーザースキャナあるいはＬＩＤＡＲ（Light Detection and Ranging）を有し、周囲形状を表す三次元点群データを取得する。三次元レーザースキャナは、赤外線レーザー等の装置（あるいはミラー）をセンサ中心回りに回転させながら、レーザービームを周囲に照射して、所定の角度ごとに物体の有無と距離を測定する。物体が存在した場合、その物体にレーザービームが当たった位置が、センサ中心からの距離と方向の形式で得られる。この距離と方向から、センサ中心を原点とするセンサ座標系での三次元点（ｘ，ｙ，ｚ）Ｔが得られる。
したがって、このようなレーザー装置を、センサ中心軸に沿って複数台積載して回転させれば、三次元的に分布する三次元点群データを得ることができる。また、レーザー装置一台の場合には、レーザービームを縦と横の両方向に走査して、三次元点群データを得ることもできる。
検出手段４では、レーザースキャナの１周期（例えば、１回転３６０°）分で得られる三次元点群データをスキャンデータまたは単にスキャンと呼び、処理の単位として用いる。
したがって、このようなレーザー装置を、センサ中心軸に沿って複数台積載して回転させれば、三次元的に分布する三次元点群データを得ることができる。また、レーザー装置一台の場合には、レーザービームを縦と横の両方向に走査して、三次元点群データを得ることもできる。
検出手段４では、レーザースキャナの１周期（例えば、１回転３６０°）分で得られる三次元点群データをスキャンデータまたは単にスキャンと呼び、処理の単位として用いる。
【0028】
推定手段５は、ＣＰＵ（Central Processing Unit）や、メモリなどによって構成され、このメモリに記憶された所定のプログラムに従って情報処理を実行する。この推定手段５は、地図生成部５１と、記憶部５２と、幾何特徴抽出部５３と、自己位置算出部５４と、自己位置評価部５５とを備えている。
【0029】
地図生成部５１は、検出手段４にて検出された三次元点群データに基づいて、移動ロボット１の周囲の地図を生成する。
記憶部５２は、地図生成部５１にて生成された地図を記憶する。なお、記憶部５２は、地図生成部５１にて生成された地図に加え、検出手段４にて検出された三次元点群データを記憶してもよい。
記憶部５２は、地図生成部５１にて生成された地図を記憶する。なお、記憶部５２は、地図生成部５１にて生成された地図に加え、検出手段４にて検出された三次元点群データを記憶してもよい。
【0030】
幾何特徴抽出部５３は、記憶部５２に記憶された地図に基づいて、平面、直線、および球体の３種類の幾何特徴のうち、少なくともいずれか１種類の幾何特徴を移動ロボット１の現在位置周辺の地図としての現在地図から現在幾何特徴として抽出するとともに、移動ロボット１の過去位置周辺の地図としての過去地図から過去幾何特徴として抽出する。
【0031】
図２は、平面、直線、および球体の３種類の幾何特徴を模式的に示す図である。
本実施形態では、幾何特徴は、図２に示すように、検出手段４にて検出された三次元点群データから抽出される幾何学的特徴を有する図形（平面、直線、および球体）の総称とする。
本実施形態では、幾何特徴は、図２に示すように、検出手段４にて検出された三次元点群データから抽出される幾何学的特徴を有する図形（平面、直線、および球体）の総称とする。
【0032】
平面（Ｐ）の幾何特徴（幾何特徴Ｐ）は、図２のＰ１，Ｐ２のように、検出手段４にて検出された三次元点群データに基づいて、平坦な部分を連結して抽出したものである。この平面の幾何特徴は、例えば、ハフ変換や、ＲＡＮＳＡＣ（Random Sample Consensus）や、主成分分析などを用いて抽出することができる。
【0033】
直線（Ｌ）の幾何特徴（幾何特徴Ｌ）は、図２のＬ１，Ｌ２のように、検出手段４にて検出された三次元点群データに基づいて、直線的な部分を連結して抽出したものである。この直線の幾何特徴は、平面の幾何特徴と同様に、例えば、ハフ変換や、ＲＡＮＳＡＣや、主成分分析などを用いて抽出することができる。
【0034】
球体（Ｂ）の幾何特徴（幾何特徴Ｂ）は、図２のＢ１，Ｂ２のように、検出手段４にて検出された三次元点群データに基づいて、任意の三次元点Ｃを中心とした半径Ｒの球の中に含まれる三次元点群データを、この任意の点Ｃにて代表させて抽出したものである。幾何特徴Ｂは、この代表点Ｃで表す。なお、幾何特徴Ｂは、半径Ｒの球の内部に含まれる三次元点群の重心を代表として抽出するようにしてもよい。また、本発明の球体は、真球に限らず、楕円球や、立方体などの形状を含むものとする。換言すれば、幾何特徴Ｂは、一定の空間を有する三次元図形を１個の代表点で表したものである。
【0035】
自己位置算出部５４は、幾何特徴抽出部５３にて抽出した現在幾何特徴のうち、２個または３個の現在幾何特徴を選択するとともに、幾何特徴抽出部５３にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組とし、この幾何特徴の組に基づいて、過去地図における自己位置を算出する。
自己位置評価部５５は、自己位置算出部５４にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する。
以下、図３～図８に基づいて、自己位置算出部５４にて移動ロボット１の自己位置を算出し、自己位置評価部５５にて一致度の高い自己位置を選択する概念について説明する。
自己位置評価部５５は、自己位置算出部５４にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する。
以下、図３～図８に基づいて、自己位置算出部５４にて移動ロボット１の自己位置を算出し、自己位置評価部５５にて一致度の高い自己位置を選択する概念について説明する。
【0036】
図３は、移動ロボットを走行させる実環境を鉛直上方側から見て平面的に示す模式図である。
移動ロボット１を走行させる実環境は、図３に示すように、地面Ｇと、地面Ｇに立設された壁Ｗや、地面Ｇに存在する樹木や障害物などの物体ＯＢ１～ＯＢ６を有している。
移動ロボット１は、このような実環境下を走行することによって、検出手段４にて検出された三次元点群データに基づいて、地図生成部５１に移動ロボット１の周囲の地図を生成させる。
移動ロボット１を走行させる実環境は、図３に示すように、地面Ｇと、地面Ｇに立設された壁Ｗや、地面Ｇに存在する樹木や障害物などの物体ＯＢ１～ＯＢ６を有している。
移動ロボット１は、このような実環境下を走行することによって、検出手段４にて検出された三次元点群データに基づいて、地図生成部５１に移動ロボット１の周囲の地図を生成させる。
【0037】
図４は、移動ロボットの１回の走行において、例えばＩＣＰ等の位置合せ手法を用いてスキャンをつなぎ合わせて生成された全体地図を示す模式図である。ここで、図４の矢印は、移動ロボットの移動軌跡を示している。
この例では、移動ロボットは位置ＰＯ２を通った後、位置ＰＯ１を通っているが、両位置は近い場所にあるため、三次元レーザースキャナによって同じ物体が計測される。しかしながら、移動とともに移動ロボットの位置の誤差が累積していくため、位置ＰＯ２で計測した物体と位置ＰＯ１で計測した物体は、同じ物体ではあるが、少しずれて地図上に記録されている。
なお、この図４では、物体のずれが明確に描かれているが、実際には三次元点群が何重にも重なって地図が歪んでいくことが多い。
この例では、移動ロボットは位置ＰＯ２を通った後、位置ＰＯ１を通っているが、両位置は近い場所にあるため、三次元レーザースキャナによって同じ物体が計測される。しかしながら、移動とともに移動ロボットの位置の誤差が累積していくため、位置ＰＯ２で計測した物体と位置ＰＯ１で計測した物体は、同じ物体ではあるが、少しずれて地図上に記録されている。
なお、この図４では、物体のずれが明確に描かれているが、実際には三次元点群が何重にも重なって地図が歪んでいくことが多い。
【0038】
このような地図の歪みを解消するために、全体地図から抽出した部分地図を用いる。ここでは、位置ＰＯ１を現在位置として、位置ＰＯ１周辺の部分地図を現在地図と呼ぶ。図４では、ＰＯ１を中心とした点線の円内が現在地図である。また、位置ＰＯ２は過去に通った位置として、位置ＰＯ２周辺の部分地図を過去地図と呼ぶ。図４では、ＰＯ２を中心とした点線の円内が過去地図である。
【0039】
図５は、現在地図および過去地図を表す模式図である。図５（Ａ）は、図４の全体地図から位置ＰＯ１周辺を切り出した現在地図であり、図５（Ｂ）は、図４の全体地図から位置ＰＯ２周辺を切り出した過去地図である。なお、現在地図の範囲および過去地図の範囲の大きさは、同一とは限らない。また、現在地図の座標系および過去地図の座標系は、互いに異なっている。
【0040】
次に、現在地図と過去地図が一致するように、現在地図の中心ＰＯ１における移動ロボット１の自己位置を求める。本実施形態では、自己位置算出部５４にて移動ロボット１の自己位置を算出し、自己位置評価部５５にて現在地図と過去地図の一致度の高い自己位置を選択する。
【0041】
図６は、一致度の高い自己位置を用いて現在地図の配置を修正して、現在地図および過去地図を結合した全体地図を示す模式図である。
このように、本実施形態では、推定手段５は、図６に示すように、現在地図および過去地図の三次元点群データを、ずれることなく重ね合わせるように移動ロボット１の自己位置を推定する
このように、本実施形態では、推定手段５は、図６に示すように、現在地図および過去地図の三次元点群データを、ずれることなく重ね合わせるように移動ロボット１の自己位置を推定する
【0042】
図７は、移動ロボットの自己位置推定装置による自己位置推定処理の流れを示すフローチャートである。
移動ロボット１の自己位置推定装置２による自己位置推定処理では、推定手段５は、図７に示すように、ステップＳＴ１～ＳＴ７の処理を実行する。
移動ロボット１の自己位置推定装置２による自己位置推定処理では、推定手段５は、図７に示すように、ステップＳＴ１～ＳＴ７の処理を実行する。
【0043】
地図生成部５１は、検出手段４にて検出された三次元点群データに基づいて、移動ロボット１の現在位置周辺の地図としての現在地図を生成し、記憶部５２に記憶させる（ステップＳＴ１：現在地図生成ステップ）。
次に、推定手段５は、移動ロボット１の過去位置周辺の地図としての過去地図を記憶部５２から取得する（ステップＳＴ２：過去地図取得ステップ）。
ここで、過去地図は、例えば、移動ロボット１の移動軌跡を辿って、現在位置に近い過去位置を選択し、この過去位置周辺の地図を取得してもよく、これまでに作成された全体地図（過去地図と現在地図とを統合することによって作成された環境地図）に基づいて、所定の範囲の領域を選択していくことによって、この過去位置周辺の地図を取得してもよい。
次に、推定手段５は、移動ロボット１の過去位置周辺の地図としての過去地図を記憶部５２から取得する（ステップＳＴ２：過去地図取得ステップ）。
ここで、過去地図は、例えば、移動ロボット１の移動軌跡を辿って、現在位置に近い過去位置を選択し、この過去位置周辺の地図を取得してもよく、これまでに作成された全体地図（過去地図と現在地図とを統合することによって作成された環境地図）に基づいて、所定の範囲の領域を選択していくことによって、この過去位置周辺の地図を取得してもよい。
【0044】
現在地図生成ステップＳＴ１において、現在地図を生成し、過去地図取得ステップＳＴ２において、過去地図を取得した後、幾何特徴抽出部５３は、記憶部５２に記憶された地図に基づいて、平面（Ｐ）、直線（Ｌ）、および球体（Ｂ）の３種類の幾何特徴を現在地図から現在幾何特徴として抽出するとともに、過去地図から過去幾何特徴として抽出する（ステップＳＴ３：幾何特徴抽出ステップ）。
なお、本実施形態では、幾何特徴抽出部５３は、３種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出しているが、１種類の幾何特徴、または２種類の幾何特徴を抽出してもよい。要するに、幾何特徴抽出部は、平面、直線、および球体の３種類の幾何特徴のうち、少なくともいずれか１種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出すればよい。
なお、本実施形態では、幾何特徴抽出部５３は、３種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出しているが、１種類の幾何特徴、または２種類の幾何特徴を抽出してもよい。要するに、幾何特徴抽出部は、平面、直線、および球体の３種類の幾何特徴のうち、少なくともいずれか１種類の幾何特徴を現在幾何特徴および過去幾何特徴として抽出すればよい。
【0045】
自己位置算出部５４は、幾何特徴抽出部５３にて抽出した現在幾何特徴のうち、２個または３個の現在幾何特徴を選択するとともに、幾何特徴抽出部５３にて抽出した過去幾何特徴のうち、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して、現在幾何特徴と過去幾何特徴の組を作る（ステップＳＴ４：幾何特徴選択ステップ）。例えば、現在地図から幾何特徴ＰＰＬ（平面Ｐ２個、直線Ｌ１個）を抽出し、過去地図からも幾何特徴ＰＰＬを抽出して、２個の幾何特徴ＰＰＬからなる組を作る。
そして、自己位置算出部５４は、この幾何特徴の組に基づいて、現在の自己位置（図４のＰＯ１）が過去地図でどこに位置するかを算出する（ステップＳＴ５：自己位置算出ステップ）。
そして、自己位置算出部５４は、この幾何特徴の組に基づいて、現在の自己位置（図４のＰＯ１）が過去地図でどこに位置するかを算出する（ステップＳＴ５：自己位置算出ステップ）。
【0046】
この計算においては、２個または３個の現在幾何特徴と、同種同数の過去幾何特徴を正しく対応づける必要がある。しかしながら、どの幾何特徴が対応するかあらかじめわからないことが多いため、幾何特徴の各組合せに対して自己位置を計算し、その自己位置の正しさを検証する。
例えば、現在幾何特徴Ｐ１Ｐ２Ｌ１と過去幾何特徴Ｐ３Ｐ４Ｌ２から自己位置を計算する場合は２通りの組合せがある。１つはＰ１とＰ３、Ｐ２とＰ４、Ｌ１とＬ２を対応づける場合、もう１つはＰ１とＰ４、Ｐ２とＰ３、Ｌ１とＬ２を対応づける場合である。
例えば、現在幾何特徴Ｐ１Ｐ２Ｌ１と過去幾何特徴Ｐ３Ｐ４Ｌ２から自己位置を計算する場合は２通りの組合せがある。１つはＰ１とＰ３、Ｐ２とＰ４、Ｌ１とＬ２を対応づける場合、もう１つはＰ１とＰ４、Ｐ２とＰ３、Ｌ１とＬ２を対応づける場合である。
【0047】
ここで、自己位置算出部５４は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上（例えば、３０度以上）とするように選択する。これは、例えば、前述の幾何特徴ＰＰＬを例にすると、２つの平面が平行な場合や、平面と直線が平行な場合、幾何特徴の組から一意に自己位置の計算ができなくなることがあるためである。
なお、本実施形態では、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように現在幾何特徴および過去幾何特徴を選択しているが、このような制限を設けなくてもよい。
なお、本実施形態では、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上とするように現在幾何特徴および過去幾何特徴を選択しているが、このような制限を設けなくてもよい。
【0048】
また、自己位置算出部５４は、平面を含む現在幾何特徴および平面を含む過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定する。そして、自己位置算出部５４は、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択する。
ここで、本実施形態では、移動ロボット１は、車輪を有する移動手段３を備えているので、地面平面が含まれているか否かの判定は、センサ等を用いて比較的容易に可能である。
なお、本実施形態では、自己位置算出部５４は、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択しているが、このような制限を設けなくてもよい。
ここで、本実施形態では、移動ロボット１は、車輪を有する移動手段３を備えているので、地面平面が含まれているか否かの判定は、センサ等を用いて比較的容易に可能である。
なお、本実施形態では、自己位置算出部５４は、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択しているが、このような制限を設けなくてもよい。
【0049】
現在幾何特徴と過去幾何特徴の組から過去地図における自己位置を計算するには種々の方法があるが、回転成分および並進成分に分けて算出する方法が簡便である。
【0050】
まず、回転成分は、２個以上の方向ベクトルの組に基づき算出する。ここで、１個の現在幾何特徴の方向ベクトルをｄｉ
ｒとし、これに対応する１個の過去幾何特徴の方向ベクトルをｄｉ
ｃとし、求める自己位置の回転成分を表す行列をＲとすれば、これらの関係は、以下の式（１）のようになる。この式が２個以上あれば回転成分を計算できる。
【0051】
【数1】
【0052】
そこで、回転成分は、三次元点群データの誤差を考慮して、以下の式（２），（３）のＣを最小にすることによって、回転行列Ｒを求めて算出する。
【0053】
【数2】
【0054】
ここで、方向ベクトルは、平面の法線ベクトル、直線の方向ベクトル、および２個の球体を繋いだ直線の方向ベクトルを用いる。自己位置算出部５４は、例えば、幾何特徴ＰＰＰ（平面Ｐ３個）、幾何特徴ＰＰＬ（平面Ｐ２個と直線Ｌ１個）、幾何特徴ＰＬＬ（平面Ｐ１個と直線Ｌ２個）を選択した場合には、３個の方向ベクトルを用いて回転成分を算出する。また、自己位置算出部５４は、例えば、幾何特徴ＰＢＢ（平面Ｐ１個と球体Ｂ２個）を選択した場合には、２個の方向ベクトル（平面の法線ベクトルおよび２個の球体を繋いだ直線の方向ベクトル）を用いて回転成分を算出する。また、幾何特徴ＬＢ（直線Ｌ１個と球体Ｂ１個）を選択した場合には、２個の方向ベクトル（直線の方向ベクトルおよび球体から直線に降ろした垂線の方向ベクトル）を用いて回転成分を算出する。なお、式（２），（３）のｍは、方向ベクトルの個数である。
【0055】
次に、並進成分は、現在幾何特徴および過去幾何特徴の種類に応じて算出する。現在幾何特徴の方向ベクトルおよび現在地図における三次元点群データは、先に求めた回転行列Ｒにて予め回転変換しておく。これによれば、互いに対応する幾何特徴の方向ベクトルは同じになる。そして、並進成分は、求める自己位置の並進ベクトルをｔとし、現在幾何特徴および過去幾何特徴の種類ごとに以下の３つの式に基づき算出する。
【0056】
平面（Ｐ）の幾何特徴の場合には、その法線ベクトルをｎｉとし、現在地図の平面上の１点をｃｉ
ｃとし、過去地図の平面上の１点をｃｉ
ｒとすれば、これらの関係は、式（４），（５）のようになる。ここで、ｐは平面上の任意の点である。また、式（６）は、式（４），（５）に基づき得られる。
【0057】
【数3】
【0058】
直線（Ｌ）の幾何特徴の場合には、その方向ベクトルをｄｉとし、現在地図の直線上の１点をｃｉ
ｃとし、過去地図の直線上の１点をｃｉ
ｒとすれば、これらの関係は、式（７），（８）のようになる。ここで、ｐは直線上の任意の点、ｕ、ｖは直線方程式の媒介変数である。また、式（９）は、式（７），（８）に基づき得られる。
【0059】
【数4】
【0060】
球体（Ｂ）の幾何特徴の場合には、現在地図の球体の代表位置をｃｉ
ｃとし、過去地図の球体の代表位置をｃｉ
ｒとすれば、これらの関係は、式（１０）のようになる。
【0061】
【数5】
【0062】
そして、並進ベクトルｔに関する連立方程式である以下の式（１１）は、現在幾何特徴および過去幾何特徴の種類に応じた式（６），（９），（１０）に基づき得られる。
【0063】
【数6】
【0064】
ここで、Ａは、式（６），（９），（１０）の各左辺をスタックした行列であり、ｑは、式（６），（９），（１０）の各右辺をスタックしたベクトルである。この連立方程式は、２個または３個の現在幾何特徴を選択するとともに、選択された現在幾何特徴と同種かつ同数の過去幾何特徴を選択して幾何特徴の組としていれば、並進ベクトルｔ＝（ｘ，ｙ，ｚ）の３変数に対して解くことができる。
【0065】
ここで、自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定する。そして、自己位置算出部５４は、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないようにする。
【0066】
図８は、幾何特徴間の角度または距離の例を示す模式図である。
ここで、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＰＰＬを抽出した場合には、図８（Ａ）に示すように、平面Ｐ１および平面Ｐ２のなす角や、平面Ｐ１および直線Ｌ１のなす角ａ１や、平面Ｐ２および直線Ｌ１の距離ｄ１を用いることができる。
また、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＰＬＬを抽出した場合には、図８（Ｂ）に示すように、平面Ｐ３および直線Ｌ２，Ｌ３のなす角や、直線Ｌ２および直線Ｌ３の距離ｄ２を用いることができる。
ここで、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＰＰＬを抽出した場合には、図８（Ａ）に示すように、平面Ｐ１および平面Ｐ２のなす角や、平面Ｐ１および直線Ｌ１のなす角ａ１や、平面Ｐ２および直線Ｌ１の距離ｄ１を用いることができる。
また、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＰＬＬを抽出した場合には、図８（Ｂ）に示すように、平面Ｐ３および直線Ｌ２，Ｌ３のなす角や、直線Ｌ２および直線Ｌ３の距離ｄ２を用いることができる。
【0067】
また、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＰＢＢを抽出した場合には、図８（Ｃ）に示すように、平面Ｐ４および球体Ｂ１，Ｂ２距離ｈ１，ｈ２や、球体Ｂ１および球体Ｂ２の距離ｄ３を用いることができる。
さらに、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＬＢを抽出した場合には、図８（Ｄ）に示すように、直線Ｌ４および球体Ｂ３の距離ｄ４を用いることができる。
さらに、整合性を判定するための幾何特徴間の角度または距離は、幾何特徴抽出部５３にて幾何特徴ＬＢを抽出した場合には、図８（Ｄ）に示すように、直線Ｌ４および球体Ｂ３の距離ｄ４を用いることができる。
【0068】
なお、本実施形態では、自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないようになっているが、この幾何特徴の組に基づいて、過去地図における自己位置を算出してもよい。
【0069】
また、自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定する。そして、自己位置算出部５４は、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出する。
なお、本実施形態では、自己位置算出部５４は、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するが、地面平面を対応させて過去地図における自己位置を算出しなくてもよい。
なお、本実施形態では、自己位置算出部５４は、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するが、地面平面を対応させて過去地図における自己位置を算出しなくてもよい。
【0070】
自己位置算出ステップＳＴ５において、過去地図における自己位置を算出した後、自己位置評価部５５は、自己位置算出部５４にて算出された自己位置に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択する（ステップＳＴ６：自己位置評価ステップ）。
【0071】
具体的には、自己位置評価部５５は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価する。
【0072】
はじめに、自己位置評価部５５は、自己位置算出部５４にて算出された自己位置（仮説）をｐｏｓｅとし、現在地図の球体群をＰｃとし、過去地図の球体群をＰｒとする。
そして、以下の（Ａ１）～（Ａ５）の手順に従って、幾何特徴の組ごとに現在地図および過去地図の一致度を評価する。
そして、以下の（Ａ１）～（Ａ５）の手順に従って、幾何特徴の組ごとに現在地図および過去地図の一致度を評価する。
【0073】
（Ａ１）スコアＳ＝０とする。
（Ａ２）ｐｉ ｃ∈Ｐｃをｐｏｓｅにて座標変換したものをｑｉ ｃとする。
（Ａ３）ｑｉ ｃの半径ｒ内にＰｒの点が存在する場合には、Ｓ＝Ｓ＋１とする。
（Ａ４）Ｐｃのすべての点において、上記（Ａ２）～（Ａ３）の処理を繰り返す。
（Ａ５）スコアＳが閾値ｓｔｈｒｅ以上の場合には、自己位置（仮説）は、正しいとする。
（Ａ２）ｐｉ ｃ∈Ｐｃをｐｏｓｅにて座標変換したものをｑｉ ｃとする。
（Ａ３）ｑｉ ｃの半径ｒ内にＰｒの点が存在する場合には、Ｓ＝Ｓ＋１とする。
（Ａ４）Ｐｃのすべての点において、上記（Ａ２）～（Ａ３）の処理を繰り返す。
（Ａ５）スコアＳが閾値ｓｔｈｒｅ以上の場合には、自己位置（仮説）は、正しいとする。
【0074】
ここで、ｐｏｓｅにてｐｉ
ｃを座標変換する場合には、以下の式（１２）を用いる。Ｒは、ｐｏｓｅの回転行列であり、ｔは、ｐｏｓｅの並進ベクトルである。
【0075】
【数7】
【0076】
なお、本実施形態では、上記（Ａ３）の処理は、ｋｄ木を用いた最近傍探索にて行っている。これに対して、自己位置評価部５５は、上記（Ａ３）の処理に代えて、例えば、Ｐｒの点のうち、ｑｉ
ｃに最も近いものを求め、その点までの距離が半径ｒ内であるか否かに基づいて、Ｓを更新する処理を採用してもよい。
【0077】
また、自己位置評価部５５は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価する。換言すれば、自己位置評価部５５は、以下の（Ｂ１）～（Ｂ４）の手順に従って、球体群の個数を増加させつつ階層的に一致度を評価していく。
【0078】
（Ｂ１）ｋ回目の評価における球体群の数をｍｋとし、スコアＳの閾値をｓｔｈｒｅｋとする。
（Ｂ２）上記（Ａ１）～（Ａ５）の処理を行って、スコアＳが閾値をｓｔｈｒｅｋ以上の自己位置（仮説）を残す。
（Ｂ３）残った自己位置（仮説）のスコアＳの最大値ｓｍａｘｋを求め、スコアＳがｒａｔｉｏ×ｓｍａｘｋ未満の自己位置（仮説）は削除する。
（Ｂ４）ｍｋを増加させ、ｓｔｈｒｅｋを大きくし、（Ｂ２）の処理に戻る。その後、（Ｂ２）～（Ｂ４）の処理を所定の回数だけ繰り返す。
（Ｂ２）上記（Ａ１）～（Ａ５）の処理を行って、スコアＳが閾値をｓｔｈｒｅｋ以上の自己位置（仮説）を残す。
（Ｂ３）残った自己位置（仮説）のスコアＳの最大値ｓｍａｘｋを求め、スコアＳがｒａｔｉｏ×ｓｍａｘｋ未満の自己位置（仮説）は削除する。
（Ｂ４）ｍｋを増加させ、ｓｔｈｒｅｋを大きくし、（Ｂ２）の処理に戻る。その後、（Ｂ２）～（Ｂ４）の処理を所定の回数だけ繰り返す。
【0079】
なお、本実施形態では、ｒａｔｉｏの値として０．８５を設定したが、これとは異なる値に設定してもよい。
【0080】
そして、自己位置評価部５５は、最後に残った自己位置（仮説）をスコアＳの順に並べ、上位の自己位置（仮説）を一致度の高い自己位置として選択する。なお、ここで選択する自己位置は、１個であってもよく、２個以上の複数個であってもよい。２個以上の複数個の自己位置を選択した場合には、後述の自己位置推定ステップＳＴ７において、これらの自己位置を再評価してもよい。
【0081】
自己位置評価ステップＳＴ６において、自己位置評価部５５は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定する（ステップＳＴ７：自己位置推定ステップ）。
【0082】
具体的には、自己位置評価部５５は、自己位置評価ステップＳＴ６において、一致度の高い自己位置として選択された自己位置（仮説）をｐｏｓｅとし、現在地図の三次元点群データをＰｃとし、過去地図の三次元点群データをＰｒとする。
そして、自己位置評価部５５は、ＩＣＰ等の位置合せ手法を用いて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定する。
そして、自己位置評価部５５は、ＩＣＰ等の位置合せ手法を用いて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定する。
【0083】
なお、本実施形態では、自己位置評価部５５は、前述した（Ｂ１）～（Ｂ４）の手順に従って、球体群の個数を増加させつつ階層的に一致度を評価しているが、階層的に一致度を評価しなくてもよい。
また、本実施形態では、自己位置推定ステップＳＴ７において、自己位置評価部５５は、前述した（Ａ１）～（Ａ５）の手順に従って、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定しているが、自己位置推定ステップＳＴ７は、省略されていてもよい。この場合には、自己位置評価ステップＳＴ６において、自己位置評価部５５にて選択した自己位置を過去地図における自己位置として推定すればよい。
また、本実施形態では、自己位置推定ステップＳＴ７において、自己位置評価部５５は、前述した（Ａ１）～（Ａ５）の手順に従って、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定しているが、自己位置推定ステップＳＴ７は、省略されていてもよい。この場合には、自己位置評価ステップＳＴ６において、自己位置評価部５５にて選択した自己位置を過去地図における自己位置として推定すればよい。
【0084】
このような本実施形態によれば、以下の作用・効果を奏することができる。
（１）自己位置算出部５４は、三次元点よりも個数が大幅に少ない幾何特徴を用いるため、その組合せも少なくて済む。三次元点を用いて自己位置を求める場合、現在地図および過去地図からそれぞれ３個の三次元点を選び、その組を用いて自己位置の計算を行う。三次元点は膨大な個数（数千個から数万個）があるため、その組合せも膨大であり、その中から一致度の高い組を求めるには多大な処理時間がかかる。一方、本実施形態の幾何特徴は、三次元点に比べて個数が少なく（数個から数百個）、組合せ数も少なくて済む。したがって、自己位置推定装置２は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができる。
（１）自己位置算出部５４は、三次元点よりも個数が大幅に少ない幾何特徴を用いるため、その組合せも少なくて済む。三次元点を用いて自己位置を求める場合、現在地図および過去地図からそれぞれ３個の三次元点を選び、その組を用いて自己位置の計算を行う。三次元点は膨大な個数（数千個から数万個）があるため、その組合せも膨大であり、その中から一致度の高い組を求めるには多大な処理時間がかかる。一方、本実施形態の幾何特徴は、三次元点に比べて個数が少なく（数個から数百個）、組合せ数も少なくて済む。したがって、自己位置推定装置２は、三次元点群データの個数が多い環境にて自己位置を推定する場合であっても処理時間を削減することができる。
【0085】
（２）自己位置評価部５５は、現在地図および過去地図のそれぞれから球体群を抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価するので、三次元点群データよりも個数の少ない球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価することができる。したがって、自己位置推定装置２は、処理時間を更に削減することができる。
（３）自己位置評価部５５は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価するので、球体群の個数を段階的に増加させつつ所定の閾値よりも一致度の高い自己位置を絞り込んでいくことができる。したがって、自己位置推定装置２は、更に精度よく自己位置を推定することができる。
（３）自己位置評価部５５は、所定の閾値よりも一致度の高い自己位置に対し、現在地図および過去地図のそれぞれから球体群の個数を増加させて抽出し、この球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を再び評価するので、球体群の個数を段階的に増加させつつ所定の閾値よりも一致度の高い自己位置を絞り込んでいくことができる。したがって、自己位置推定装置２は、更に精度よく自己位置を推定することができる。
【0086】
（４）自己位置評価部５５は、球体群に基づいて、幾何特徴の組ごとに現在地図および過去地図の一致度を評価し、一致度の高い自己位置を選択した後、ＩＣＰ等の位置合せ手法を用いて、この自己位置に基づいて、現在地図に係る三次元点群データと、過去地図に係る三次元点群データとの一致度が高くなるように自己位置を更に推定するので、自己位置推定装置２は、はじめから三次元点群データに基づき自己位置を推定する場合と比較して処理時間を削減することができ、しかも精度よく自己位置を推定することができる。
（５）自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置２は、処理時間を更に削減することができる。
（５）自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴間の角度または距離と、この幾何特徴の組に含まれる過去幾何特徴間の角度または距離との間に整合性があるか否かを判定し、整合性がないと判定した場合には、この幾何特徴の組に基づいて、過去地図における自己位置を算出しないので、自己位置推定装置２は、処理時間を更に削減することができる。
【0087】
（６）自己位置算出部５４は、現在幾何特徴および過去幾何特徴を幾何特徴の組として選択する場合に、各幾何特徴内の平面同士のなす角度、直線同士のなす角度、並びに平面と直線のなす角度を所定の角度以上（例えば、３０度以上）とするように選択するので、自己位置算出部５４は、この幾何特徴の組に基づき過去地図における自己位置を適切に算出することができる。
（７）自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するので、自己位置推定装置２は、幾何特徴の組に複数の平面を含んでいる場合には、平面同士の組み合わせ数を削減することができ、ひいては処理時間を更に削減することができる。
（７）自己位置算出部５４は、幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、この幾何特徴の組における地面平面を対応させて過去地図における自己位置を算出するので、自己位置推定装置２は、幾何特徴の組に複数の平面を含んでいる場合には、平面同士の組み合わせ数を削減することができ、ひいては処理時間を更に削減することができる。
【0088】
（８）自己位置算出部５４は、平面および球体の現在幾何特徴および平面および球体の過去幾何特徴を幾何特徴の組として選択する場合に、この幾何特徴の組に含まれる現在幾何特徴、およびこの幾何特徴の組に含まれる過去幾何特徴に地面に相当する平面としての地面平面が含まれているか否かを判定し、地面平面が含まれていると判定した場合には、地面平面から所定の高さに位置する球体のみを選択するので、自己位置推定装置２は、幾何特徴の組に地面平面を含んでいる場合には、球体の選択数を削減することができ、ひいては処理時間を更に削減することができる。
【0089】
〔実施形態の変形〕
なお、本発明は、前記実施形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前記実施形態では、移動体としての移動ロボット１は、具体的なものを例示していなかったが、サービスロボット、ホームロボットなどの移動ロボットを例示できる。より具体的には、移動体は、掃除ロボット、警備ロボット、運搬ロボット、案内ロボットなどを例示できる。また、移動体は、自動運転自動車や、作業車などであってもよい。
なお、本発明は、前記実施形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前記実施形態では、移動体としての移動ロボット１は、具体的なものを例示していなかったが、サービスロボット、ホームロボットなどの移動ロボットを例示できる。より具体的には、移動体は、掃除ロボット、警備ロボット、運搬ロボット、案内ロボットなどを例示できる。また、移動体は、自動運転自動車や、作業車などであってもよい。
【0090】
前記実施形態では、移動ロボット１は、自己位置推定装置２を備え、この自己位置推定装置２は、地図生成部５１等の機能を備えていた。これに対して、移動体は、自己位置推定装置を備えていなくてもよい。例えば、自己位置推定装置は、移動体と通信可能な他の機器に設けられていてもよい。
前記実施形態では、移動ロボット１は、移動手段３を備えていたが、これを備えていなくてもよく、例えば、自己位置推定装置を備えた台車等を使用者の手作業にて移動させるような構成であってもよい。
前記実施形態では、移動ロボット１は、移動手段３を備えていたが、これを備えていなくてもよく、例えば、自己位置推定装置を備えた台車等を使用者の手作業にて移動させるような構成であってもよい。
【産業上の利用可能性】
【0091】
以上のように、本発明は、移動体の自己位置推定装置に好適に利用できる。
【符号の説明】
【0092】
１ 移動ロボット
２ 自己位置推定装置
３ 移動手段
４ 検出手段
５ 推定手段
５１ 地図生成部
５２ 記憶部
５３ 幾何特徴抽出部
５４ 自己位置算出部
５５ 自己位置評価部
２ 自己位置推定装置
３ 移動手段
４ 検出手段
５ 推定手段
５１ 地図生成部
５２ 記憶部
５３ 幾何特徴抽出部
５４ 自己位置算出部
５５ 自己位置評価部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】