(57)【要約】【課題】対象とする環境が大規模であっても自己位置同定に必要なランドマーク情報の処理コストを軽減でき、なおかつ環境の変化に対しても柔軟に対応可能な移動体の自己位置同定装置を提供する。【解決手段】局所地図と大域地図とを備え、ランドマーク候補およびランドマーク照合結果を局所地図に保存して次回のランドマーク照合に利用する構成とした。更に、照合にあたって対応するランドマークがないランドマーク候補を新たにランドマークとして登録する一方、既に登録されているランドマークであっても長期間にわたって観測されないものは削除するようにした。【選択図】図１
2008165275.tif 000002
【特許請求の範囲】
【請求項１】
環境内のランドマークを観測して自己の位置を同定する自己位置同定装置を備え、前記自己位置同定装置によって前記環境内での自己位置を同定しながら移動する移動体において、
前記自己位置同定装置が、
前記移動体周辺の前記ランドマークを観測する外界センサの観測結果からランドマーク候補を抽出するランドマーク候補抽出部と、
前記ランドマーク候補に関する情報を収めた局所地図と、前記環境内の全域における前記ランドマークに関する情報を収めた大域地図とを構築する地図構築部と、
前記局所地図を保存する局所地図保存部と、
前記大域地図を保存する大域地図保存部と、
前記外界センサの観測結果と前記大域地図の情報とから前記移動体の最新の移動体位置推定値を算出する推定位置補正部と、
前記移動体の内界センサから得る情報と前記移動体位置推定値とから、前記環境内における自己位置同定結果を出力する位置推定部と、を備え、
前記地図構築部が、前記大域地図を構築するにあたって、前記ランドマーク候補と前記大域地図の登録済みの前記ランドマークとの一致関係を判断し、一致しないと判断したとき、前記ランドマーク候補を新たな前記ランドマークとして前記大域地図に追加登録する処理を行うことを特徴とする自己位置同定装置を備えた移動体。
【請求項２】
前記地図構築部は、前記大域地図を構築するにあたって、前記大域地図を複数の小領域毎に分けて構築するとともに、前記ランドマーク候補の情報をランドマーク観測結果として前記小領域毎に保存することを特徴とする請求項１記載の自己位置同定装置を備えた移動体。
【請求項３】
前記地図構築部は、前記判断によって前記ランドマーク候補が前記登録済みのランドマークと一致すると判断したとき、前記局所地図中に、これら前記ランドマーク候補と前記大域地図の登録済みのランドマークとの対応情報も含めて前記ランドマーク候補を保存する処理を行うことを特徴とする請求項１または２記載の自己位置同定装置を備えた移動体。
【請求項４】
前記地図構築部は、前記判断を行う前に、まず前記局所地図において、前記ランドマーク候補と、前回の処理で前記局所地図に保存された前ランドマーク候補との一致関係を照合し、前記照合において前記前ランドマーク候補と一致関係が得られた前記ランドマーク候補についてのみ、前記判断を行うことを特徴とする請求項３記載の自己位置同定装置を備えた移動体。
【請求項５】
前記地図構築部が前記判断を行うにあたって、前記照合において前記ランドマーク候補と一致関係の得られた前記前ランドマーク候補の前記対応情報を参照し、該前ランドマーク候補と一致関係がある前記登録済みのランドマークを、前記ランドマーク候補と一致するものと判断することを特徴とする請求項４記載の自己位置同定装置を備えた移動体。
【請求項６】
前記地図構築部は、前記照合において前記ランドマーク候補が前記前ランドマーク候補との前記一致関係が得られたが、前記参照において、前記前ランドマーク候補が前記登録済みのランドマークとの前記対応情報を所有していなかったとき、前記照合一致回数を加算し、該照合一致回数が閾値以上となった前記ランドマーク候補と、前記登録済みのランドマークとが一致するものがないか判別することを特徴とする請求項５記載の自己位置同定装置を備えた移動体。
【請求項７】
前記地図構築部は、前記判別において、前記ランドマーク候補が存在する前記小領域を特定し、該小領域中に存在する前記登録済みのランドマークの情報を順次読み出し、前記ランドマーク候補との距離をそれぞれ計算し、該計算した距離が閾値未満である前記登録済みのランドマークのうち、最も近い前記登録済みのランドマークが前記ランドマーク候補と一致しているものと判別することを特徴とする請求項６記載の自己位置同定装置を備えた移動体。
【請求項８】
前記地図構築部は、前記ランドマーク観測結果とあわせて観測時刻を前記大域地図に保存し、現在時刻から指定時間以上経過している前記登録済みのランドマークを前記大域地図から削除することを特徴とする請求項２から７いずれかに記載の自己位置同定装置を備えた移動体。
【請求項９】
前記指定時間は、前記移動体が前記環境内を周回する場合、前記周回の１周に要する時間よりも大きな値に設定されていることを特徴とする請求項８記載の自己位置同定装置を備えた移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、環境を観測しながら移動する移動体が環境中における自己位置を逐次同定するための自己位置同定装置に関する。
【背景技術】
【０００２】
本発明における自己位置同定装置とは、移動体に備えた外界センサおよび内界センサの観測結果にもとづき、環境中に定めた基準点に対する移動体の絶対的な位置を逐次同定するものである。
【０００３】
特許文献１には、移動体の自己位置同定のための基本的な構成が示されている。図３はその自己位置同定システムの全体構成を示すブロック図であって、図中３０１はカメラで得られる画像などからランドマークを抽出するランドマーク認識部、３０２はランドマーク認識部３０１から随時得られるランドマーク認識結果を用いてランドマークの列をランドマーク間の位置関係とともに求め、前記移動体の移動経緯を示す観測地図を作成・更新する観測地図作成部、３０３は観測地図情報を保存する観測地図情報保存部、３０４は移動体の概略の移動量を内界センサで獲得する移動量算出部、３０５は走行距離等により走行経路中の移動体概略位置を求める大局位置算出部、３０６は実際の環境中のランドマークを予め情報として収集してデータ化した地図データを保持する地図データ記憶部、３０７は地図データ記憶部３０６から大局位置算出部３０５により得られた概略位置周辺のデータを切り出す地図データ管理部、３０８は地図データ管理部より送られてきた地図データと観測地図とを照合することにより走行経路中の移動体の自己位置を同定する照合部である。特許文献１の自己位置同定システムは複数のランドマーク列をつなぎ合わせた観測地図と地図データとを照合することで自己位置を同定するものであって、この点が特徴として主張されている。
【０００４】
特許文献２では、ランドマークの存在する環境中の移動ロボット装置が複数の登録済みの環境から現在の環境を同定するための環境同定装置が開示されている。図４はそのブロック図であって、図中４０１は前記ロボット装置が現在おかれている環境のランドマーク情報を有する環境地図を構築する環境地図構築部、４０２は環境内のランドマークの位置情報とその環境に一意に割り振られる環境ＩＤを一つの組として、複数の環境地図及びその環境ＩＤからなるデータベースを有する環境地図記憶部、４０３は環境地図構築部４０１によって構築される現在前記ロボット装置が置かれている環境の環境地図と、環境地図記憶部４０２に記憶された既知の環境地図との類似度を比較することによって前記ロボット装置が現在置かれている環境を特定する環境同定部、４０４は新しい環境を認識するために前記ロボット装置を歩き回らせる環境探索部である。特許文献２の環境同定装置が特許文献１と異なるのは、前記ロボット装置が電源を切られた状態で別の環境に運ばれてから再び稼動させられる場合などであっても、環境を正しく同定することを目的としている点である。
【０００５】
特許文献１と特許文献２は、予め記憶しておいたランドマーク配置とランドマーク観測結果とを照合して対応付けることで自己位置を同定する点で、従来の多くの自己位置同定装置と共通している。
【０００６】
特許文献３には、照合に際して発生する計算コストや類似環境の存在による同定位置に誤りを生じる可能性に注目した自己位置同定機能を有する自律移動装置が開示されている。特許文献３において、自己位置の推定及び修正を行う自己位置認識手段は、環境情報取得手段で得た環境情報と地図情報に含まれる環境情報との照合に際し、推定自己位置にもとづいて、予め設定されている所定範囲内で照合を行い、この照合で修正情報を得られない時、前記所定範囲を広げて再度照合を行う、前記所定範囲をずらして再度照合を行う、などの措置をとる。照合する範囲を所定範囲に限定することで、照合のための計算コストを削減できる。また、所定範囲を推定自己位置付近とすることで、地図情報中の類似環境と誤って対応付けてしまう可能性を減らすことができる。
【０００７】
【特許文献１】特開平８−２４７７７５号公報（第１６頁、図１）
【特許文献２】特開２００４−１１０８０２号公報（第４０頁、図５）
【特許文献３】特開２００４−５５９３号公報（第４−５頁）
【発明の開示】
【発明が解決しようとする課題】
【０００８】
以上に示したように、予め記憶しておいたランドマーク配置とランドマーク観測結果とを照合して対応付けることで移動体の自己位置を同定する方式がある。しかしながら特許文献１や特許文献２では、特許文献３で指摘されているように照合に際して発生する計算のコストや類似環境の存在による同定位置に誤りを生じる可能性が問題となっていた。特許文献３はこれに対する一対処法を与えるものである。しかし、環境情報取得手段で得た環境情報と地図情報に含まれる環境情報とを照合する際に過去の照合結果を利用しない構成であるため、不要な計算処理が発生する可能性があった。また、走行中に環境状態が変化した場合や正確な観測ができなかった場合、実際のランドマーク観測結果と予め記憶しておいたランドマーク配置が相異なるので、照合が失敗するという問題があった。すなわち、予め記憶しておいたランドマークの一部が撤去された場合、予め記憶しておいたランドマーク以外のランドマークが新たに設置された場合、予め記憶しておいたランドマークの一部が他の移動物体の陰に入るなどして観測できなかった場合などには、実際のランドマーク観測結果と予め記憶しておいたランドマーク配置が相異なり、照合が失敗してしまう。この問題は、ランドマークは環境中の所定位置に常に存在し、安定して観測可能であるとの仮定に起因するものである。
【０００９】
本発明は以上のような問題を念頭に、少ない計算量で自己位置の同定が可能で、なおかつ環境の変化に大しても柔軟に適応しながら自己位置を同定できる移動体の自己位置同定装置を提供することを目的としている。
【課題を解決するための手段】
【００１０】
上記問題を解決するため、本発明は、次のような移動体の自己位置同定装置を構成するものである。
【００１１】
請求項１に記載の発明は、環境内のランドマークを観測して自己の位置を同定する自己位置同定装置を備え、前記自己位置同定装置によって前記環境内での自己位置を同定しながら移動する移動体において、前記自己位置同定装置が、前記移動体周辺の前記ランドマークを観測する外界センサの観測結果からランドマーク候補を抽出するランドマーク候補抽出部と、前記ランドマーク候補に関する情報を収めた局所地図と、前記環境内の全域における前記ランドマークに関する情報を収めた大域地図とを構築する地図構築部と、前記局所地図を保存する局所地図保存部と、前記大域地図を保存する大域地図保存部と、前記外界センサの観測結果と前記大域地図の情報とから前記移動体の最新の移動体位置推定値を算出する推定位置補正部と、前記移動体の内界センサから得る情報と前記移動体位置推定値とから、前記環境内における自己位置同定結果を出力する位置推定部と、を備え、前記地図構築部は、前記大域地図を構築するにあたって、前記ランドマーク候補と前記大域地図の登録済みの前記ランドマークとの一致関係を判断し、一致しないと判断したとき、前記ランドマーク候補を新たな前記ランドマークとして前記大域地図に追加登録する処理を行う自己位置同定装置を備えた移動体とした。
請求項２に記載の発明は、前記地図構築部は、前記大域地図を構築するにあたって、前記大域地図を複数の小領域毎に分けて構築するとともに、前記ランドマーク候補の情報をランドマーク観測結果として前記小領域毎に保存する請求項１記載の自己位置同定装置を備えた移動体とした。
請求項３に記載の発明は、前記地図構築部は、前記判断によって前記ランドマーク候補が前記登録済みのランドマークと一致すると判断したとき、前記局所地図中に、これら前記ランドマーク候補と前記大域地図の登録済みのランドマークとの対応情報も含めて前記ランドマーク候補を保存する処理を行う請求項１または２記載の自己位置同定装置を備えた移動体とした。
請求項４に記載の発明は、前記地図構築部は、前記判断を行う前に、まず前記局所地図において、前記ランドマーク候補と、前回の処理で前記局所地図に保存された前ランドマーク候補との一致関係を照合し、前記照合において前記前ランドマーク候補と一致関係が得られた前記ランドマーク候補についてのみ、前記判断を行う請求項３記載の自己位置同定装置を備えた移動体とした。
請求項５に記載の発明は、前記地図構築部が前記判断を行うにあたって、前記照合において前記ランドマーク候補と一致関係の得られた前記前ランドマーク候補の前記対応情報を参照し、該前ランドマーク候補と一致関係がある前記登録済みのランドマークを、前記ランドマーク候補と一致するものと判断する請求項４記載の自己位置同定装置を備えた移動体とした。
請求項６に記載の発明は、前記地図構築部は、前記照合において前記ランドマーク候補が前記前ランドマーク候補との前記一致関係が得られたが、前記参照において、前記前ランドマーク候補が前記登録済みのランドマークとの前記対応情報を所有していなかったとき、前記照合一致回数を加算し、該照合一致回数が閾値以上となった前記ランドマーク候補と、前記登録済みのランドマークとが一致するものがないか判別する請求項５記載の自己位置同定装置を備えた移動体とした。
請求項７に記載の発明は、前記地図構築部は、前記判別において、前記ランドマーク候補が存在する前記小領域を特定し、該小領域中に存在する前記登録済みのランドマークの情報を順次読み出し、前記ランドマーク候補との距離をそれぞれ計算し、該計算した距離が閾値未満である前記登録済みのランドマークのうち、最も近い前記登録済みのランドマークが前記ランドマーク候補と一致しているものと判別する請求項６記載の自己位置同定装置を備えた移動体とした。
請求項８に記載の発明は、前記地図構築部は、前記ランドマーク観測結果とあわせて観測時刻を前記大域地図に保存し、現在時刻から指定時間以上経過している前記登録済みのランドマークを前記大域地図から削除する請求項２から７いずれかに記載の自己位置同定装置を備えた移動体とした。
請求項９に記載の発明は、前記指定時間は、前記移動体が前記環境内を周回する場合、前記周回の１周に要する時間よりも大きな値に設定されている請求項８記載の自己位置同定装置を備えた移動体とした。
【発明の効果】
【００１２】
請求項１に記載の発明によると、ランドマーク環境が変化した場合でも自己位置同定装置で保持する大域地図を柔軟に適応させ、自己位置同定を継続することができる。
請求項３、請求項４，請求項５記載の発明により、前サイクルのランドマーク照合結果を利用することでランドマークの照合に要する計算量の大幅な削減が可能となる。
請求項６に記載の発明により、安定して観測できるランドマーク候補のみを大域地図のランドマークと一致するか判別するため、計算量を削減できる。更には、ランドマーク候補の観測結果に含まれる外界センサのノイズの影響を排除できる。
請求項６記載の発明により、観測されたランドマークと登録済みのランドマークとの一致の判断をより確実にすることができる。
請求項２、請求項７記載の発明により、互いに対応している可能性が高いランドマーク候補と登録済みランドマークとの照合を優先でき、照合にかかる計算量を削減できる。
請求項８記載の発明により、環境中から除去されたランドマーク情報を大域地図から消去し、記憶領域を節約することができる。
請求項９記載の発明により、特に同じ軌道を周回するような移動体について、環境中から除去されたランドマーク情報を大域地図から消去し、記憶領域を節約することができる。
【発明を実施するための最良の形態】
【００１３】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１４】
図１は、本発明における移動体の自己位置同定装置の基本構成を示したブロック図である。図において、点線内１００が自己位置同定装置、１０１が位置推定部、１０２がランドマーク候補抽出部、１０３が地図構築部、１０４が局所地図保存部、１０５が大域地図保存部、１０６が推定位置補正部であり、信号１２１と１２２は自己位置同定装置１００の入力信号であってそれぞれ外界センサ計測結果、内界センサ計測結果であり、信号１２３は自己位置同定装置１００の出力信号であって自己位置同定結果である。実施例１における自己位置同定装置１００は外部入出力インターフェィスを備えた計算機とその内部で動作する複数のタスクであって、各タスクはそれぞれ以降に述べる処理を周期的に実行する。各構成要素について以下で説明する。
【００１５】
位置推定部１０１は、移動体の動作状態を計測する内界センサの観測結果１２２にもとづき、前記移動体の環境中における推定位置を計算し自己位置同定結果として出力する処理を毎サイクル実行する。ここで内界センサとは、例えば姿勢角度センサや車輪回転速度センサといった移動体内部の情報を計測するためのセンサを指す。実施例１における内界センサは、前記移動体車輪の回転量を計測できるように設置された回転型エンコーダである。内界センサによる推定位置はセンサノイズや計測不可能な車輪の滑りなどの影響で真値から徐々に乖離していく。そこで位置推定部１０１は、後述の推定位置補正部１０６から修正自己位置同定結果を受け取った場合、自己位置同定結果を前記修正自己位置同定結果に置き換える。
実施例１における位置推定部１０１は、前記計算機内部で前記エンコーダの信号と移動体固有の運動学関係式を用いて移動体位置を推定(オドメトリ)するオドメトリ位置推定タスクである。
【００１６】
ランドマーク候補抽出部１０２は、外界センサ観測結果１２２を用いて環境中のランドマーク候補を抽出してその移動体に対する位置を特定し、地図構築部１０３へ送る処理を毎サイクル実行する。
ここで外界センサとは、例えば画像センサやレーザーレンジファインダといった移動体外部の環境情報を計測するためのセンサを指す。実施例１における外界センサは、二次元レーザーレンジファインダである。
ランドマーク候補とは、特徴的な模様を描いた板や反射板など人工的に設置した目印、あるいは物体の角や平板など特徴的な形状を有する環境中の物体や点を、現サイクルの外界センサ観測結果１２２から抽出したものである。実施例１におけるランドマーク候補は、環境中の物体に存在する所定範囲以内の角度を有する角(コーナー)である。
実施例１におけるランドマーク候補抽出部は、前記計算機内部で前記二次元レーザーレンジファインダの観測結果から所定範囲内の角度を有する角を抽出し、移動体に対する位置を特定する角抽出タスクである。ここで角とは、前記二次元レーザーレンジファインダの計測面において所定値以上の長さを有する２線分が交わることで構成される角を指し、例えば角柱の頂点や通路交差点の角などである。本実施例では、各サイクルで抽出された角をランドマーク候補としてその位置情報および角度情報を利用する。
ランドマークとは、自己位置同定に使用するため大域地図保存部１０５にその情報を登録・保存されたランドマーク候補を指す。ランドマークとして保存するランドマーク候補の選択方法は後述する。
なお、ランドマーク候補抽出部１０２の処理サイクルは位置推定部１０１の処理サイクルと同期している必要はない。
【００１７】
地図構築部１０３は、ランドマーク候補抽出部１０２で抽出されたランドマーク候補の情報である現ランドマーク候補情報と、前サイクルまでの局所地図とから新たな局所地図を構築し保存する処理、新たに得られた局所地図と前サイクルまでの大域地図と最近の位置推定結果とから新たな大域地図を構築し保存する処理、を毎サイクル実行する。地図構築部１０３の該サイクルにおける処理については後述する。
ここで局所地図とは、前記外界センサで計測される移動体周囲環境の局所的な地図であって、
・前サイクルにおいてランドマーク候補抽出部１０２で抽出されたランドマーク候補の情報である前ランドマーク候補情報、
・現サイクルにおいてランドマーク候補抽出部１０２で抽出されたランドマーク候補の情報である現ランドマーク候補情報、
・前ランドマーク候補と現ランドマーク候補の照合が連続して一致した回数である照合一致回数、
・どの前ランドマーク候補がどのランドマークに対応しているかを示す前ランドマーク候補−ランドマーク対応関係、
・どの現ランドマーク候補がどのランドマークに対応しているかを示す現ランドマーク候補−ランドマーク対応関係、
を含む。
また大域地図とは、推定位置補正部１０６で利用するための移動体の移動環境全体にわたる大域的な地図であって、前記ランドマーク情報の集合体である。大域地図は
・移動体位置同定結果、
・ランドマーク位置同定結果、
・ランドマーク観測結果、
を含む。なお、大域地図は環境を複数に分割してなる小領域毎に分けて構築され、該領域内に存在するランドマークのランドマーク情報(ランドマーク位置同定結果、ランドマーク観測結果)は該領域毎にまとめて保存される。
地図構築部１０３の処理サイクルはランドマーク候補抽出部１０２の処理サイクル終了と同時に開始される。
実施例１における地図構築部１０３は、前記計算機内部の地図構築タスクである。地図構築タスクは前記角抽出タスク完了と同時にその処理を開始する、
【００１８】
地図構築部１０３における前記サイクルの処理の流れを、図２を用いて説明する。処理の詳細については後述する。図２はランドマーク候補抽出部１０２で抽出された最新のランドマーク候補である現ランドマーク候補１つ１つについて実行する処理である。
図２において、
Ｓ２０１は、処理を開始するステップ、
Ｓ２０２は、現ランドマーク候補情報を局所地図に保存するステップ、
Ｓ２０３は、現ランドマーク候補を局所地図に保存してある前ランドマーク候補と照合した結果によって以降の処理を分岐するステップ、
Ｓ２０４は、現ランドマーク候補に対応する前ランドマーク候補の前ランドマーク候補−ランドマーク対応関係を参照した結果によって以降の処理を分岐するステップ、
Ｓ２０５は、前ランドマーク候補に対応していたランドマークを、現ランドマーク候補にも対応すると判断して、現ランドマーク候補−ランドマーク対応関係を更新し、局所地図に保存するステップ、
Ｓ２０６は、現ランドマーク候補情報を、対応する既存ランドマークの観測結果として大域地図に保存するステップ、
Ｓ２０７は、現ランドマーク候補が前ランドマーク候補との照合結果が連続して一致した回数である照合一致回数を前ランドマーク候補の照合一致回数に１を加算した値とするステップ、
Ｓ２０８は、現ランドマーク候補情報の一部として前記照合一致回数を局所地図に追加保存するステップ、
Ｓ２０９は、前記照合一致回数と予め設定した閾値とを比較した結果により、以降の処理を分岐させるステップ、
Ｓ２１０は、現ランドマークを、大域地図に登録されている既存ランドマークと照合した結果により、以降の処理を分岐させるステップ、
Ｓ２１１は、現ランドマーク候補情報を、新ランドマークの観測結果として大域地図に追加登録するステップ、
Ｓ２１２は、現ランドマーク候補の照合一致回数を零に初期化するステップ、
Ｓ２１３は、地図構築処理を完了するステップである。
【００１９】
以上で説明した地図構築処理の１サイクルの各ステップについて、さらに詳細に説明する。
ランドマーク候補抽出部１０２で抽出した現ランドマーク候補の情報は、ステップＳ２０２で局所地図に保存する。このとき、大域地図中の移動体位置推定値と現ランドマーク候補情報とから、現ランドマーク候補の大域的な位置の推定値を計算し、現ランドマーク候補推定位置として、あわせて局所地図に保存する。
【００２０】
次いでステップＳ２０３では、現ランドマーク候補と前ランドマーク候補の一致関係を照合する。照合は、以下の方法による。すなわち、前のサイクルにおけるステップＳ２０３で局所地図中に保存してあった前ランドマーク候補推定位置と、現ランドマーク候補推定位置とを比較し、両者の距離が予め定めた閾値未満である前ランドマーク候補のうち最も近い推定位置を有するものを該現ランドマーク候補と一致しているものとする。本ステップでは、現ランドマーク候補について、大域地図内の全てのランドマークとの照合をいきなり行うのではなく、まず、局所地図内において、現ランドマーク候補が、前のサイクルで保存された前ランドマーク候補のうち、どれと一致するかを照合する。こうすることによって、照合対象が前サイクルで抽出された前ランドマーク候補に限定されるので、大域地図中の全ランドマークと直接照合するよりもはるかに少ない計算量で処理を完了できる。照合の結果、現ランドマーク候補に一致する前ランドマーク候補が存在した場合はステップＳ２０４に、存在しなかった場合はステップＳ２１２に進む。
【００２１】
ステップＳ２０４は、局所地図中に保存されている前ランドマーク候補−ランドマーク対応関係を利用して、現ランドマーク候補と大域地図中のランドマークの一致あるいは不一致を調べるステップである。ここで前ランドマーク候補−ランドマーク対応関係とは、前述のように、どの前ランドマーク候補がどのランドマークに一致しているかを示すものであって、前サイクルで局所地図中に保存されている対応関係のことである。ステップＳ２０４に進んだ現ランドマーク候補は、一致する前ランドマーク候補を有しているので、該前ランドマーク候補のランドマークとの対応関係を参照することで現ランドマーク候補−ランドマーク対応関係を調べる探索処理を一部省略することができる。すなわち、対応する前ランドマーク候補が大域地図中のいずれかのランドマークに対応していた場合は、現ランドマーク候補も該ランドマークに対応していると判断して、探索処理を省略してステップＳ２０５に進む。対応する前ランドマーク候補がいずれのランドマークにも対応していなかった場合は探索処理のためにステップＳ２０７に進む。
【００２２】
ステップＳ２０５では、ステップＳ２０４にて一致すると判断した現ランドマーク候補−ランドマーク対応関係を局所地図に保存してステップＳ２０６に進む。
【００２３】
ステップＳ２０６では、まず現ランドマーク候補情報を、対応するランドマーク観測結果として大域地図に保存する。次いで、最近の（最も新しい）移動体位置同定結果と、ランドマーク位置同定結果（現ランドマーク候補とそれに対応するランドマークとを同定した結果）とを用いて、上記ランドマーク観測結果を、計算上で予想される観測結果推定値として算出し、この予想された観測結果推定値と実際に計測された上記ランドマーク観測結果とを比較して、それらの乖離量を計算し、これを小さくするように移動体位置同定結果とランドマーク位置同定結果とを修正する。この際、移動体位置とランドマーク位置とを状態方程式の状態量、ランドマーク観測結果を出力ととらえ、観測結果の推定値と実際の観測結果との偏差を用いて推定状態量を毎サイクルの計算によって真値に近付けるのであるが、具体的な修正計算の方法は、例えば下記文献などにより公知であるためここでは述べない。
M.W.M.Gamini Dissanayake et al、 "A Solution to the Simultaneous Localization and Map Building (SLAM) Problem"、 IEEE Trans. R&A、 Vol.17、 No.3、 2001
最後に、修正された移動体位置同定結果とランドマーク位置同定結果とを大域地図に保存し処理を完了する。なお、同定結果の修正にあたって使用するランドマーク観測結果はできるだけ多い方が良い。個々の観測結果にはセンサノイズなどに起因する誤差が含まれているため、複数のランドマーク情報を用いることで誤差の影響を薄めることができるからである。
【００２４】
ステップＳ２０７では、現ランドマーク候補が、前ランドマーク候補に対しては一致する候補が見つかったものの、その前ランドマーク候補が大域地図のランドマークと一致するものがなかったとき（前ランドマーク候補−ランドマーク対応関係を参照して対応関係が無かったとき）、現ランドマーク候補の照合一致回数を加算し、ステップＳ２０８に進む。
【００２５】
ステップＳ２０８では、ステップＳ２０７で得た照合一致回数を局所地図に保存しステップＳ２０９に進む。
【００２６】
ステップＳ２０９では、ステップＳ２０７で得た照合一致回数と前記閾値とを比較し、閾値以上であればステップＳ２１０に進み、閾値未満であれば処理を完了する。
ステップＳ２１０に進む現ランドマーク候補は、前記閾値回数以上連続して観測されたものである。前記閾値を設定することによって、ランドマーク候補抽出部１０２がセンサノイズなどの影響で偶然出力した、誤ったランドマーク候補情報を無視することができる。その結果、ステップＳ２１０でランドマークと照合するランドマーク候補の数を必要最小限に抑え、計算量を節約することができる。
【００２７】
ステップＳ２１０は、現ランドマーク候補が、前ランドマーク候補に対しては一致する候補が見つかったものの、その前ランドマーク候補が大域地図のランドマークと一致する対応情報がなかったとき、現ランドマーク候補と大域地図のランドマークとが一致するかどうかを判別するステップである。
このステップは、閾値回数以上連続して観測された現ランドマーク候補に対して、大域地図中の既存のランドマークと一致するものが無いかどうかの確認を行うことを目的としている。
一致不一致の照合は以下の手順による。まず、ステップＳ２０２で局所地図に保存した現ランドマーク候補位置推定値を読み出す。次いで、前記現ランドマーク候補位置推定値が存在する大域地図中の前記小領域を特定する。小領域特定後は、該小領域中に存在するランドマークのランドマーク位置推定値を順次読み出し、前記ランドマーク候補推定位置との距離をそれぞれ計算する。そして、計算した距離が予め定めた閾値未満であるランドマークのうち最も近い位置推定値を有するランドマークが該現ランドマーク候補と一致しているものとする。照合の結果、現ランドマーク候補に一致するランドマークが存在した場合はステップＳ２０５に、存在しなかった場合はステップＳ２１１に進む。
【００２８】
ステップＳ２１１では、現ランドマーク候補を新ランドマークとして大域地図に登録しステップＳ２０５に進む。新ランドマークの情報の初期値は、最近の移動体位置推定値と現ランドマーク候補情報にもとづいて計算された新ランドマーク大域的位置推定結果として登録する。
【００２９】
ステップＳ２１２では、現ランドマーク候補の照合一致回数を零に初期化して局所地図に保存し処理を完了する。
【００３０】
以上の手順によれば新たなランドマーク候補が出現した場合に、大域地図にランドマークを追加登録することができ、環境の実情をより正確に反映した大域地図を構築できる。自己位置同定に利用できるランドマークが増えるため、後述の推定位置補正部１０６においてより正確に自己位置同定値を得ることができるようになる。
【００３１】
一方、当初は存在していたランドマークが環境の変化によって観測されなくなることもあり得る。この場合は既に存在しないランドマーク情報を大域地図に保持し続けることになり、記憶領域を消費するばかりか照合に要する計算量が増大してしまう。これを防ぐために以下のような処理を追加しても良い。
まずステップＳ２０６において、ランドマーク観測結果とあわせて観測時刻をも大域地図に保存する。更に、地図構築部１０３の処理の後段に以下のような処理を追加する。すなわち、大域地図中のランドマークの観測時刻を調べ、現在時刻から指定時間以上経過しているランドマークは削除する。前記指定時間は必要なランドマーク情報を除去することがないように十分大きな値を設定する。移動体が同一環境内を周回する場合は、前記指定時間を１周に要する時間よりも大きな値とすればよい。
【００３２】
推定位置補正部１０６は、位置推定部１０１が出力する移動体位置同定結果を補正するものである。位置推定部１０１でオドメトリによって得られる値は、使用する運動学関係式と実際との乖離によって誤差を生じるため、これを外界センサによるランドマーク観測結果にもとづく自己位置同定値で置き換えることで誤差を打ち消すのである。
本実施例における推定位置補正部１０６は、地図構築部１０３の処理の後段で呼び出される推定位置補正ルーチンであって、大域地図から最近の移動体位置同定結果を読み出して位置推定部１０１に出力する。位置推定部１０１は移動体位置同定結果を受け取ると同時にそれまでの値を廃棄し、受け取った値に置き換える。
【００３３】
以上のような構成としたため、本発明による自己位置同定装置の出力である自己位置同定結果１２３は、外界センサ観測結果１２１にもとづいて補正されるため、内界センサ観測結果１２２のみを用いて推定される値よりも真値に近くなる。本発明は従来の自己位置同定装置(例えば特許文献１乃至３)とは異なり、ランドマーク候補抽出部１０２において連続して観測されるランドマーク候補を、ランドマークとして新たに登録する一方で、指定時間以上観測されないランドマーク情報を除去することにより、環境の変化に大域地図を柔軟に対応させることができる。また、前ランドマーク候補の照合結果を利用することで、現ランドマーク候補と大域地図のランドマークとを直接照合する数を極力減らし、計算量を大幅に削減している。
【産業上の利用可能性】
【００３４】
大域地図を環境の変化に応じて柔軟に適応させることができ、更には自己位置同定のためのランドマーク照合に要する計算量を大幅に削減できるため、時間とともに変化する環境中を、ランドマークを利用して自己位置を同定しながら移動する移動体全般に広く適用可能である。
【図面の簡単な説明】
【００３５】
【図１】本発明による自己位置同定装置の構成図
【図２】本発明による自己位置同定装置の地図構築部における処理の流れ図
【図３】従来技術における自己位置同定システムの構成を示したブロック図
【図４】従来技術における環境同定装置の構成を示したブロック図
【符号の説明】
【００３６】
１００  自己位置同定装置
１０１  位置推定部
１０２  ランドマーク候補抽出部
１０３  地図構築部
１０４  局所地図保存部
１０５  大域地図保存部
１０６  推定位置補正部
１２１  外界センサ計測結果
１２２  内界センサ計測結果
１２３  自己位置同定結果
３０１  ランドマーク認識部
３０２  観測地図作成部
３０３  移動量算出部
３０５  地図データ記憶部
３０７  地図データ管理部
３０８  照合部
４０１  環境地図構築部
４０２  環境地図記憶部
４０３  環境同定部
４０４  環境探索部
【図１】
2008165275.tif 000003
【図２】
2008165275.tif 000004
【図３】
2008165275.tif 000005
【図４】
2008165275.tif 000006