ホーム > 特許ランキング > 株式会社豊田自動織機
知財求人 - 知財ポータルサイト「IP Force」
▶ 株式会社豊田自動織機の特許一覧 ▶ 独立行政法人産業技術総合研究所の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-09-12
(45)【発行日】2024-09-24
(54)【発明の名称】自己位置推定装置、及び移動体
(51)【国際特許分類】
G01B 11/00 20060101AFI20240913BHJP
G01B 21/00 20060101ALI20240913BHJP
G05D 1/43 20240101ALI20240913BHJP
G06T 7/70 20170101ALI20240913BHJP
【ＦＩ】
G01B11/00 H
G01B21/00 D
G05D1/43
G06T7/70 A
【請求項の数】
2
(21)【出願番号】P 2020208504
(22)【出願日】2020-12-16
(65)【公開番号】P2022095273
(43)【公開日】2022-06-28
【審査請求日】2023-02-07
【前置審査】
(73)【特許権者】
【識別番号】000003218
【氏名又は名称】株式会社豊田自動織機
(73)【特許権者】
【識別番号】301021533
【氏名又は名称】国立研究開発法人産業技術総合研究所
(74)【代理人】
【識別番号】100088155
【弁理士】
【氏名又は名称】長谷川 芳樹
(74)【代理人】
【識別番号】100113435
【弁理士】
【氏名又は名称】黒木 義樹
(74)【代理人】
【識別番号】100124062
【弁理士】
【氏名又は名称】三上 敬史
(74)【代理人】
【識別番号】100148013
【弁理士】
【氏名又は名称】中山 浩光
(74)【代理人】
【識別番号】100162640
【弁理士】
【氏名又は名称】柳 康樹
(72)【発明者】
【氏名】古室 達也
(72)【発明者】
【氏名】加藤 紀彦
(72)【発明者】
【氏名】阪野 貴彦
(72)【発明者】
【氏名】横塚 将志
【審査官】松本 泰典
(56)【参考文献】
【文献】特開２０２０－１７１７３（ＪＰ，Ａ）
【文献】特開２０１６－１１０５７６（ＪＰ，Ａ）
【文献】特開２０２０－５７３０７（ＪＰ，Ａ）
【文献】特開２０１７－８３２３０（ＪＰ，Ａ）
【文献】特開２０１６－２４５９８（ＪＰ，Ａ）
【文献】特開２０１５－９３５３２（ＪＰ，Ａ）
【文献】特開２０１４－２０３１４４（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／００
Ｇ０１Ｂ １１／００
Ｇ０１Ｂ ２１／００
Ｇ０６Ｔ ７／７０
(57)【特許請求の範囲】
【請求項1】
取得された画像から抽出された特徴と、予め位置情報と前記特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定装置であって、前記画像を取得する一つの画像取得部と、
前記画像取得部で取得された前記画像から前記特徴を抽出する抽出部と、
前記抽出部で抽出された前記特徴と、前記データベースとをマッチングさせることで前記移動体の自己位置を推定する推定部と、を備え、
前記推定部は、
前記データベースのうちの複数の前記位置情報に対応する複数のデータに基づいて、前記移動体の事前推定位置を複数取得し、
複数の前記事前推定位置から前記自己位置を推定し、
前記推定部は、前回推定した自己位置に対して、少なくとも二つの最寄りのデータを選び、選択した少なくとも二つのデータのそれぞれと、前記一つの画像取得部で取得された前記画像から抽出された前記特徴と、のマッチングを行うことで、前記自己位置を推定し、
前記推定部は、
前記事前推定位置の元になった前記データとのマッチング率を各々の前記事前推定位置について算出し、
最終的な前記自己位置の推定結果が、前記マッチング率の高い前記事前推定位置に偏るように推定を行う、自己位置推定装置。
【請求項2】
請求項１に記載の自己位置推定装置を備える移動体。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムに関する。
【背景技術】
【0002】
従来の自己位置推定装置として、特許文献１に記載されたものが知られている。この自己位置推定装置は、事前に取得した画像と、移動体の走行中に取得した画像と、を用いることで、自己位置を推定している。ここで、画像から位置推定を行うことは、演算時間がかかるため、この自己位置推定装置は、機械センサ（オドメトリ）を使用することによって、演算中の自己位置の推定結果の補完を行っている。
【先行技術文献】
【特許文献】
【0003】
【文献】特開２０１８－８１００８号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
ここで、画像から自己位置の推定を行った場合の推定精度が常に良いとは限らない。この場合、推定精度が低い自己位置の推定結果を用いて、移動体の走行支援を行った場合、適切な走行支援を行うことができず、移動体が経路から外れて走行を行ってしまう可能性がある。また、走行経路の各位置に対して、事前にデータベースを準備しておき、走行中にデータベース中のデータを用いて自己位置を推定する方法が採用される場合がある。しかし、当該方法では、データが設定されている位置から離れた位置で自己位置の推定を行った場合、推定精度が低下する可能性がある。更に、自己位置の推定に用いるデータを切り替える場所では、推定する自己位置が急に変化してしまい不連続になる可能性がある。
【0005】
従って、本発明は、自己位置の推定結果の連続性を向上し、適切な移動体の走行支援を行うことができる自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【0006】
本発明の一態様に係る自己位置推定装置は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定装置であって、画像を取得する画像取得部と、画像取得部で取得された画像から特徴を抽出する抽出部と、抽出部で抽出された特徴と、データベースとをマッチングさせることで移動体の自己位置を推定する推定部と、を備え、推定部は、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体の事前推定位置を複数取得し、複数の事前推定位置から自己位置を推定する。
【0007】
自己位置推定装置は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するためのものである。ここで、例えば、画像取得部は、データベースのデータが作成されたときの位置から離れた位置で画像を取得する場合がある。このような場合、自己位置の推定に用いるデータが、別の位置のデータに切り替わる。従って、推定部が一つのデータだけで自己位置を推定した場合、推定する自己位置が急に変化する可能性がある。これに対して、推定部は、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体の事前推定位置を複数取得し、複数の事前推定位置から自己位置を推定する。これにより、推定部は、一つのデータのみならず、複数のデータを用いて自己位置を推定することで、データの切り替えなどによって推定結果が急に変化することを抑制できる。そのため、自己位置の推定結果の連続性を向上し、適切な移動体の走行支援を行うことができる。
【0008】
推定部は、前回推定した自己位置に対して、少なくとも二つの最寄りのデータを選び、選択した少なくとも二つのデータに基づいて自己位置を推定してよい。これにより、推定部は、移動体の近くに存在するデータに基づいて自己位置の推定を行うことができるため、取得した画像の特徴とデータとをマッチングさせやすく、推定精度を向上することができる。
【0009】
自己位置推定装置は、自己位置の推定精度を評価する評価部を更に備え、推定部は、評価部による評価が閾値以下となった場合に、複数の事前推定位置による自己位置の推定を実施してよい。これにより、推定部は、複数のデータを用いることによる自己位置の推定を必要なタイミングに絞って実施することができるため、演算の負荷を低減することができる。
【0010】
推定部は、事前推定位置の元になったデータとのマッチング率を各々の事前推定位置について算出し、最終的な自己位置の推定結果が、マッチング率の高い事前推定位置に偏るように推定を行ってよい。これにより、推定部は、マッチング率の高いデータを重み付けをした状態で自己位置の推定を行うことができるため、推定精度を向上することができる。
【0011】
本発明に係る移動体は、上述の自己位置推定装置を備える。
【0012】
本発明に係る自己位置推定方法は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定方法であって、画像を取得する画像取得ステップと、画像取得ステップで取得された画像から特徴を抽出する抽出ステップと、抽出ステップで抽出された特徴と、データベースとをマッチングさせることで移動体の自己位置を推定する推定ステップと、を備え、推定ステップでは、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体の事前推定位置を複数取得し、複数の事前推定位置から自己位置を推定する。
【0013】
本発明に係る自己位置推定プログラムは、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定プログラムであって、画像を取得する画像取得ステップと、画像取得ステップで取得された画像から特徴を抽出する抽出ステップと、抽出ステップで抽出された特徴と、データベースとをマッチングさせることで移動体の自己位置を推定する推定ステップと、をコンピュータシステムに実行させ、推定ステップでは、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体の事前推定位置を複数取得し、複数の事前推定位置から自己位置を推定する。
【0014】
これらの移動体、自己位置推定方法、及び自己位置推定プログラムによれば、上述の自己位置推定装置と同様な効果を得ることができる。
【発明の効果】
【0015】
本発明によれば、自己位置の推定結果の連続性を向上し、適切な移動体の走行支援を行うことができる自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムを提供することができる。
【図面の簡単な説明】
【0016】
【図1】本発明の実施形態に係る自己位置推定装置を備える自己位置推定システムを示す概略図である。
【図2】本発明の実施形態に係る自己位置推定装置を備える移動体のブロック構成を示すブロック構成図である。
【図3】データベース作成時の作業場の様子を示す概略図である。
【図4】データベースの作成方法を示すフローチャートである。
【図5】特徴を抽出する方法を説明するための図である。
【図6】カメラで取得された画像、及び抽出された特徴を示す図である。
【図7】特徴の三次元座標を取得する方法を示す概念図である。
【図8】移動体が自動走行を行うときの様子を示す概略図である。
【図10】データベースに登録されている位置情報に対応する位置からの距離と、自己位置の推定精度との関係を示すグラフである。
【図11】特徴点を評価するときの様子を示す概念図である。
【図12】走行経路における移動体の走行支援状況を示す概念図である。
【図13】自己位置の推定精度の評価、及び当該評価結果を用いた移動体の走行支援の方法を示すフローチャートである。
【図14】自己位置推定部による自己位置推定の内容を説明するための概念図である。
【図15】自己位置推定部による自己位置推定の内容を示すフローチャートである。
【図16】自己位置の推定結果を評価を行った実験結果を示す図である。
【発明を実施するための形態】
【0017】
以下、本発明の実施形態について、図面を参照して詳細に説明する。
【0018】
図１は、本実施形態に係る自己位置推定装置１を備える自己位置推定システム１００を示す概略図である。図１に示すように、自己位置推定システム１００は、複数の移動体５０のそれぞれに設けられた自己位置推定装置１と、管理部２と、を備える。
【0019】
本実施形態では、移動体５０として、フォークリフトが例示されている。図１には、移動体５０としてのフォークリフトが、倉庫や工場などの作業場Ｅ（所定の領域）にて、荷物の積み卸しの作業を行っている様子が示されている。作業場Ｅでは、複数の棚６０が配列されている。また、棚６０と棚６０との間に、移動体５０が通過するための通路６１が形成されている。自己位置推定装置１は、このような作業場Ｅ内における移動体５０の自己位置を推定する装置である。自己位置推定装置１は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体５０の自己位置を推定する。移動体５０は、自己位置推定装置１で推定された自己位置を用いることで、作業場Ｅ内で自動走行することができる。なお、自己位置推定装置１の詳細な構成については後述する。管理部２は、作業場Ｅ内の複数の移動体５０を管理するサーバーである。管理部２は、必要に応じて、複数の移動体５０から所定の情報を受信し、複数の移動体５０に対して所定の情報を送信する。
【0020】
図２は、本実施形態に係る自己位置推定装置１を備える移動体５０のブロック構成を示すブロック構成図である。図２に示すように、移動体５０は、走行部１１と、カメラ１２と、制御部２０と、を備える。走行部１１は、移動体５０を走行させるための駆動力を発生するモータなどの駆動系である。カメラ１２（画像取得部）は、移動体５０の周囲の画像を取得する機器である。カメラ１２は、取得した画像を自己位置推定部２６へ送信する。
【0021】
制御部２０は、移動体５０を統括的に管理するＥＣＵ[Electronic Control Unit]を備えている。ＥＣＵは、ＣＰＵ[Central Processing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]、ＣＡＮ［Controller Area Network］通信回路等を有する電子制御ユニットである。ＥＣＵでは、例えば、ＲＯＭに記憶されているプログラムをＲＡＭにロードし、ＲＡＭにロードされたプログラムをＣＰＵで実行することにより各種の機能を実現する。制御部２０は、経路計画部２１と、指令速度計算部２２と、通信部２３と、記憶部２４と、自己位置推定部２６（抽出部、推定部）と、オドメトリ計算部２７と、自己位置決定部２８と、評価部３１と、切替部３２と、を備える。このうち、記憶部２４、自己位置推定部２６、オドメトリ計算部２７、自己位置決定部２８、評価部３１、切替部３２、及びカメラ１２によって自己位置推定装置１が構成される。
【0022】
経路計画部２１は、移動体５０が移動する経路を計画する。経路計画部２１は、作業場Ｅの中の出発位置と目的位置とを設定すると共に、目的位置までの経路を計画する。経路計画部２１は、計画した経路の情報を指令速度計算部２２へ送信する。指令速度計算部２２は、走行部１１に対する指令速度、すなわちモータに対する指令回転数を計算する。指令速度計算部２２は、経路計画部２１から送信された経路、及び自己位置決定部２８から送信された自己位置に基づいて、指令回転数を計算する。通信部２３は、走行部１１との間で通信を行う。通信部２３は、走行部１１に対して走行に必要な制御信号を送信する。なお、通信部２３は、図示されないエンコーダからのエンコーダ値を取得すると共に、当該エンコーダ値をオドメトリ計算部２７に送信する。
【0023】
次に、自己位置推定装置１の各構成要素について説明する。記憶部２４は、自己位置推定に必要なデータベースを記憶する。データベースは、予め位置情報と、当該位置で取得された画像から抽出された特徴と、を関連付けた情報群である。データベースは、ある位置情報と特徴とが関連づけられたデータを複数有している。記憶部２４は、自己位置推定部２６に対してデータベースを送信する。
【0024】
自己位置推定部２６は、カメラ１２で取得された画像から特徴を抽出する。また、自己位置推定部２６は、カメラ１２で取得された画像から抽出された特徴と、記憶部２４から送信されたデータベースとをマッチングさせることで、移動体５０の自己位置を推定する。自己位置推定部２６は、推定した自己位置を自己位置決定部２８へ送信する。
【0025】
オドメトリ計算部２７は、通信部２３から取得したエンコーダ値に基づいて、オドメトリによる自己位置を計算する。オドメトリ計算部２７は、カメラ１２の画像によることなく、容易な計算によって自己位置を取得することができる。オドメトリ計算部２７は、オドメトリによる自己位置を自己位置決定部２８へ送信する。自己位置決定部２８は、自己位置推定部２６からの自己位置と、オドメトリ計算部２７からの自己位置を総合的に判断し、移動体５０の自己位置を決定する。自己位置決定部２８は、オドメトリ計算部２７からの自己位置をベースとしつつ、画像を用いた自己位置推定部２６からの自己位置推定結果を用いて、オドメトリ誤差を補正することで、自己位置を決定する。自己位置決定部２８は、決定した自己位置を指令速度計算部２２へ送信する。
【0026】
ここで、図３～図６を参照して、データベースの作成方法について説明する。データベースの作成は、実際に移動体５０が自己位置を推定しながら作業場Ｅで作業を行うよりも前段階において、予め行われる。データベースの作成は、作業場Ｅのうち、走行に重要なポイントにおいて画像を取得し、位置推定に必要な情報を算出してデータベース化して、作業場Ｅの地図に紐付けることによってなされる。重要なポイントは、例えば、図３において「１」～「１２」までの番号が付された位置である。以降の説明においては、「１」の番号の位置を「第１重要ポイント」と称するものとする。他の番号が付された場所も同様に「第ｎ重要ポイント」と称される。なお、ここでは、データベース作成のために、図２に示す移動体５０を用いるものとして説明する。後述のように、自己位置推定部２６が画像から特徴を抽出する機能や画像同士をマッチングする機能を有しているため、自己位置推定部２６がデータベース作成のための各種処理を行うものとする。しかし、データベース作成のために用いられる機器は、特に限定されず、以降の処理を実行できる機器であれば、どのようなものを採用してもよい。
【0027】
図４は、データベースの作成方法を示すフローチャートである。まず、カメラ１２は、データベースにしたい画像を１枚取得すると共に、その周辺の画像を１枚（複数でも可）取得する（ステップＳ１０：画像取得ステップ）。例えば、第１重要ポイントにおける画像をデータベースにする場合、カメラ１２は、第１重要ポイントで画像を１枚取得し、当該第１重要ポイントの近くから別の画像を取得する。次に、自己位置推定部２６は、特徴を抽出できるように、画像の前処理を行う（ステップＳ２０）。例えば、カメラ１２は、広範囲を撮影できるように魚眼レンズを有する場合がある。このときは、画像中の物体が歪んで映っているため、前処理を行って、実際の見え方に近い画像に調整する。
【0028】
次に、自己位置推定部２６は、二枚の画像からそれぞれ特徴を抽出する（ステップＳ３０：抽出ステップ）。ここで、図５を参照して、画像中の特徴の抽出の方法について説明する。図５は、特徴を抽出する方法を説明するための図である。図５（ａ）に示すように、自己位置推定部２６は、判定画素Ｘと周囲画素（ここでは１６画素）の輝度を比較することで、「明」「暗」「同」の三パターンに層別する。例えば、自己位置推定部２６は、「判定画素（Ｘ）－周囲画素（ｉ）＞明暗閾値」という関係が成り立つ周囲画素は「明」と判定し、「判定画素（Ｘ）－周囲画素（ｉ）＜明暗閾値」という関係が成り立つ周囲画素は「暗」と判定し、「（判定画素（Ｘ）－周囲画素（ｉ））の絶対値＜明暗閾値」という関係が成り立つ周囲画素は「同」と判定する。
【0029】
ここで、自己位置推定部２６は、「明」または「暗」の周囲画素の連続数がコーナー閾値以上となった場合、判定画素Ｘを画像中の特徴として抽出する。例えば、コーナー閾値を「１２」とした場合、図５（ｂ）の判定画素Ｘは「明」の周囲画素が１２連続以上であるため、特徴として抽出される。図５（ｃ）の判定画素Ｘは「明」の周囲画素が１１連続しかないため、判定画素Ｘは特徴ではないものとして破棄される。これにより、図６に示すように、画像ＰＣ中の棚、床、天井、壁などの構造物のコーナー部などが特徴（特徴点ＦＰ）として抽出される。その他、荷物、設置物、移動体などが画像に写っている場合、それらの物体のコーナー部も特徴として抽出される。
【0030】
本明細書では、特徴として抽出された画素を「特徴点ＦＰ」と称する場合がある。なお、自己位置推定に用いられる画像中の特徴は、点である場合のみならず、線や所定形状などである場合もある。すなわち、特徴は、画像中において特徴的な部分として画像処理によって抽出可能な部分であり、且つ、他の画像で抽出したものとマッチング可能な部分であれば、どのような態様であるかは限定されない。
【0031】
図４に戻り、自己位置推定部２６は、二枚の画像同士の特徴をマッチングする（ステップＳ４０）。そして、自己位置推定部２６は、特徴の３次元復元を行い、データベースとして登録する。データベースでは、重要ポイント（画像が取得された位置）の位置情報と、画像中の特徴の画像座標と、特徴の３次元座標とが、関連付けられた形で登録されている。なお、位置情報には、撮影時のカメラ１２の姿勢も含まれる。例えば、図７に示すように、重要ポイントにおける画像ＰＣ１中では、特徴点Ｇ，Ｒ，Ｂが抽出されており、それらの画像座標が特定されている。また、画像ＰＣ１が取得された場所の位置情報も特定されている。重要ポイントの周囲で取得された画像ＰＣ２中でも、特徴点Ｇ，Ｒ，Ｂが抽出されており、それらの画像座標が特定されている。また、画像ＰＣ２が取得された場所の位置情報も特定されている。周囲の撮影位置と、重要ポイントとの位置関係は、オドメトリ値を紐付けたり、画像から推定するなどの方法で把握される。自己位置推定部２６は、画像ＰＣ１と画像ＰＣ２の特徴同士をマッチングする。これにより、自己位置推定部２６は、三角測量の要領で、特徴点Ｇ，Ｒ，Ｂの三次元座標を取得することができる。
【0032】
図４に戻り、ステップＳ５０の処理が終了すると、一枚の画像のデータベース化が終了する。移動体５０は、次の重要ポイントへ移動し、当該重要ポイントにてカメラ１２で画像を取得して、再び図４の処理を行う。このようにして、作業場Ｅの全ての重要ポイントでの画像のデータベース化を行う。作成されたデータベースは、記憶部２４に格納される。
【0033】
次に、図８及び図９を参照して、移動体５０の自動走行の方法、及び自動走行のための自己位置推定部２６による自己位置推定方法について説明する。例えば、移動体５０は、図８に示す現在位置ＳＴから、目標位置ＧＬである第８重要ポイントまで自動走行するものとする。このとき、自己位置推定部２６は、移動体５０が現在どこに存在しているかを推定する。これにより、自己位置決定部２８は、現在位置ＳＴが「第１０重要ポイントから東へ２ｍ」であると決定できる。そして、経路計画部２１は、「第１１重要ポイントまで東に４８ｍ、第８重要ポイントまで北へ１６ｍ」という経路を計画する。これにより、移動体が自動走行を行う。なお、自己位置推定部２６は、自己位置の推定を行う際に、何個のデータを用いるかは限定されず、後述のように、複数の位置のデータを用いて自己位置の推定をしてもよいが、ここでは、内容の理解を促進するため、一つのデータを用いて自己位置推定する場合を例にして説明する。なお、複数のデータを用いて自己位置の推定を行う方法については、後述する。
【0034】
図９は、移動体５０の自己位置の推定方法を示すフローチャートである。自己位置推定部２６は、移動体５０の自己位置を推定するために図９に示す処理を実行する。図９に示すように、自己位置推定部２６は、カメラ１２から移動体５０の走行中の画像を取得する（ステップＳ１１０：画像取得ステップ）。例えば、カメラ１２は、第１０重要ポイント付近の現在位置ＳＴで画像を取得する（図８参照）。次に、自己位置推定部２６は、特徴を抽出できるように、画像の前処理を行う（ステップＳ１２０）。次に、自己位置推定部２６は、走行中の画像から特徴を抽出する（ステップＳ１３０：抽出ステップ）。なお、ステップＳ１２０，Ｓ１３０では、図４のステップＳ２０，Ｓ３０と同趣旨の処理がなされる。これにより、図６のような画像ＰＣが取得された場合には、特徴点ＦＰが取得される。
【0035】
次に、自己位置推定部２６は、ステップＳ１３０で抽出した特徴と、データベースの画像の特徴とをマッチングさせる（ステップＳ１４０：推定ステップ）。そして、自己位置推定部２６は、移動体５０の自己位置を推定する（ステップＳ１５０：推定ステップ）。
【0036】
例えば、移動体５０の走行中に、図６に示す画像ＰＣに類似する画像が得られた場合、自己位置推定部２６は、図６のものと類似する複数の特徴点ＦＰを抽出することができる。そして、自己位置推定部２６は、当該特徴点と、データベースの画像中の特徴点とを照らし合わせる。データベース作成時に第１０重要ポイントで図６に示す画像ＰＣが得られ、図６に示す複数の特徴点ＦＰが抽出されていた場合、自己位置推定部２６は、ステップＳ１３０で抽出された特徴点と、図６に示す特徴点ＦＰとをマッチングさせることができる。なお、特徴点同士をマッチングさせる方法としては、例えば、特徴点の特徴量記述子を計算し、データベースの特徴点の中から最も距離が小さい特徴点をマッチングさせる方法が挙げられるが、方法は特に限定されず、公知の方法を用いればよい。
【0037】
ここで、走行中に撮影した場所が、第１０重要ポイントからずれており、走行中に撮影したときの姿勢が、データベース作成時の撮影の姿勢からずれている場合、走行中に撮影された画像及び特徴点の画像座標は、図６の画像ＰＣ及び特徴点ＦＰの画像座標から若干のずれが生じている。従って、ステップＳ１５０では、自己位置推定部２６は、データベースから任意の３点の特徴点の座標を、走行中の画像の３点の特徴点に紐付ける。そして、自己位置推定部２６は、公知の３点法の技術を用いて、移動体５０の位置及び姿勢を推定する。なお、ステップＳ１５０に用いられる３点の特徴点は、画像中に多数存在する特徴点の中から、ランダムで選ばれる。
【0038】
次に、図２に示す評価部３１、及び切替部３２について詳細に説明する。評価部３１は、自己位置推定部２６の推定精度を評価する。ここで、図１０を参照して、データベースに登録されている位置情報に対応する位置からの距離と、自己位置の推定精度との関係について説明する。図１０のグラフ上段側の「走行」と示された直線上にプロットされたドットは、データベースに登録されている位置情報に対応する位置を示す。図１０の下段側には、移動体５０の走行範囲の各位置における、データベースに登録されている位置情報に対応する位置からの距離を示すグラフと、自己位置の推定結果の誤差が示されている。当該グラフは、事前に行われた実験の結果を示している。データベースに登録されている位置情報に対応する位置とは、前述の「第１～１２重要ポイント」の位置であり、事前にデータベース作成のために画像を取得した位置である。走行中の撮影位置（すなわち、自己位置推定を行う位置）が、各重要ポイントから離れれば離れるほど、データベースに登録されているものから画像の見え方の変化が大きくなる。従って、図１０に示すように、各重要ポイントからの距離が離れるほど、自己位置推定部２６の推定精度が低下する。
【0039】
具体的に、評価部３１は、特徴とデータベースとのマッチングに基づいて推定精度を評価する。評価部３１は、自己位置推定部２６によって抽出された特徴点ＦＰと、データベースに登録された画像の特徴点ＦＰとのマッチングを行う。評価部３１は、このときのマッチング成功数（またはマッチング成功割合）が高いときは自己位置推定部２６の推定精度が高いと評価し、低いときは自己位置推定部２６の推定精度が低いと評価する。評価部３１は、マッチング成功数（マッチング成功割合）に対して、予め閾値を設定し、当該閾値以上のときに、推定精度が高いと評価する。なお、閾値は、固定値であってもよく、事前に取得した画像から推定した値であってもよい。
【0040】
具体的に、図１１（ａ）に示すように、特徴点Ｒ，Ｇ，Ｂを用いて３点法で自己位置推定がなされた場合、評価部３１は、当該３点の特徴点Ｒ，Ｇ，Ｂ以外の特徴点ＦＰについてのマッチングの成否の判断を自動的に行う。図１１（ｂ）に示すように、３点の特徴点Ｒ，Ｇ，Ｂ以外の特徴点ＦＰのマッチングの成否を判断するとき、データベースの画像ＰＣ１の画像座標中では、判定対象の特徴点ＦＰは「特徴点ＦＰ１」で示され、走行中の画像ＰＣ２の画像座標中では、判定対象の特徴点ＦＰは「特徴点ＦＰ２」で示される。評価部３１は、特徴点ＦＰ１の三次元座標点を画像ＰＣ２上に再投影する。当該再投影された特徴点ＦＰ１は、画像ＰＣ２の画像座標中では「特徴点ＦＰ２ａ」と示される。評価部３１は、特徴点ＦＰ２ａと特徴点ＦＰ１との間の誤差の大きさを取得すると共に、当該誤差が閾値以下であるか否かを判定する。評価部３１は、誤差が閾値以下である場合、判定対象に係る特徴点ＦＰのマッチングが成功すると判定し、マッチング成功数のカウントを一つ増やす。一方、評価部３１は、誤差が閾値より大きい場合、判定対象に係る特徴点ＦＰのマッチングは失敗であると判定し、カウントしない。
【0041】
切替部３２は、評価部３１による評価結果に基づいて、自己位置推定部２６による自己位置の推定結果を走行支援に用いるか否かを切り替える。すなわち、切替部３２は、評価部３１による評価が高い推定結果の場合は、自己位置推定部２６による推定結果を走行支援で用いられるようにする。具体的には、切替部３２は、自己位置決定部２８において、自己位置推定部２６の推定結果が用いられるように設定を行う。これにより、自己位置決定部２８は、画像を用いた自己位置推定部２６からの自己位置推定結果を用いて、オドメトリ誤差を補正することで、自己位置を決定する。一方、切替部３２は、評価部３１による評価結果が低い推定結果の場合は、自己位置推定部２６による推定結果が走行支援で用いられないようにする。すなわち、切替部３２は、評価部３１による評価結果が低い場合、移動体５０の走行部１１からの走行情報のみに基づく走行支援に切り替える。ここでの走行情報とは、走行部１１からのエンコーダ値に基づくオドメトリである。具体的には、切替部３２は、自己位置決定部２８において、自己位置推定部２６の推定結果が用いられないように設定を切り替える。これにより、自己位置決定部２８は、画像を用いた自己位置推定部２６からの自己位置推定結果を用いずに、オドメトリ誤差を補正することなく、自己位置を決定する。
【0042】
図１２は、走行経路における移動体５０の走行支援状況を示す概念図である。図１２の「走行」と示された直線上にプロットされたドットは、データベースに登録されている位置情報に対応する位置（各重要ポイント）を示す。移動体５０は、重要ポイントに近い領域Ｅ１では、自己位置推定部２６からの自己位置推定結果を用いて、オドメトリ誤差を補正することによる、走行支援によって走行する。一方、移動体５０は、重要ポイントから遠い領域Ｅ２では、オドメトリのみに基づいた走行支援によって走行する。
【0043】
次に、図１３を参照して、自己位置の推定精度の評価、及び当該評価結果を用いた移動体の走行支援の方法について説明する。図１３は、自己位置の推定精度の評価、及び当該評価結果を用いた移動体の走行支援の方法を示すフローチャートである。この処理は、走行経路のある位置において、自己位置推定部２６が自己位置を推定した後に実行される。
【0044】
まず、評価部３１は、自己位置推定部２６による自己位置推定の推定精度を評価する（ステップＳ２１０：評価ステップ）。評価部３１は、自己位置推定部２６で抽出された特徴のマッチング成功数（成功割合）に基づいて、評価を行う。
【0045】
次に、評価部３１は、ステップＳ２１０における評価結果が高いか否かを判定する（ステップＳ２２０：評価ステップ）。ステップＳ２２０において、評価が高いと判定された場合、切替部３２は、自己位置推定部２６による推定結果を走行支援で用いられるように設定する（ステップＳ２３０：切替ステップ）。一方、ステップＳ２２０において、評価が低いと判定された場合、切替部３２は、自己位置推定部２６による推定結果が走行支援で用いられないようにし、オドメトリのみに基づいて走行支援が行われるように設定する（ステップＳ２４０：切替ステップ）。以上により、図１３に示す処理が終了する。
【0046】
次に、図１４～図１６を参照して、自己位置推定部２６が更に適切な自己位置を推定するための構成について説明する。図１４は、自己位置推定部２６による自己位置推定の内容を説明するための概念図である。図１５は、自己位置推定部２６による自己位置推定の内容を示すフローチャートである。図１６は、自己位置の推定結果を評価を行った実験結果を示す図である。
【0047】
前述の実施形態においては、自己位置推定部２６は、最寄りの一つのデータに基づいて自己位置を推定していた。これに対し、自己位置推定部２６は、複数のデータを用いて自己位置を推定してよい。すなわち、自己位置推定部２６は、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体５０の事前推定位置を複数取得する。また、自己位置推定部２６は、複数の事前推定位置から自己位置を推定する。自己位置推定部２６は、前回推定した自己位置に対して、少なくとも二つの最寄りのデータを選び、選択した少なくとも二つのデータに基づいて自己位置を推定する。
【0048】
まず、図１５に示すように、自己位置推定部２６は、マッチングの対象となる複数のデータを選択する（ステップＳ３１０：推定ステップ）。ここでは、自己位置推定部２６は、前回推定した自己位置に対して、二つの最寄りのデータを選ぶ。具体的に、図１４（ａ）に示すように、データベースは、移動体５０の走行経路ＲＬの各位置において、位置情報に対応する複数のデータを有している。図１４（ａ）では四つのデータが設定されているので、データＤＴ１，ＤＴ２，ＤＴ３，ＤＴ４と称する。例えば移動体５０による前回の推定に係る自己位置ＬＰがデータＤＴ１に対応する位置とデータＤＴ２に対応する位置との間に存在する場合、自己位置推定部２６は、マッチングの対象となるデータとして、一番近いデータＤ１と、二番目に近いデータＤ２と、を選択する。
【0049】
次に、図１５に示すように、自己位置推定部２６は、ステップＳ３１０で選択したデータに対してマッチングを行い、事前推定位置を複数取得する（ステップＳ３２０：推定ステップ）。なお、移動体５０が現在位置で取得した画像から抽出した特徴と、各データとをマッチングする具体的な内容は、図９での説明において、自己位置を推定した方法と同様の方法が採用される。例えば、図１４（ａ）では、自己位置推定部２６は、データＤＴ１とマッチングを行うことで、事前推定位置ＢＰ１を取得する。また、自己位置推定部２６は、データＤＴ２とマッチングを行うことで、事前推定位置ＢＰ２を取得する。マッチングの対象となるデータが異なっており、且つ各データを用いて推定した自己位置も実際の自己位置とは若干ずれるため、事前推定位置ＢＰ１と事前推定位置ＢＰ２は、互いに異なる位置に設定される。
【0050】
次に、図１５に示すように、自己位置推定部２６は、ステップＳ３２０で取得した複数の事前推定位置から自己位置を推定する（ステップＳ３３０：推定ステップ）。自己位置推定部２６は、複数の事前推定位置間の中間の位置に設定する。このとき、自己位置推定部２６は、事前推定位置の元になったデータとのマッチング率を各々の事前推定位置について算出する。マッチング率とは、事前推定位置が、元になったデータに対してどの程度合致しているかの度合いを示す値であり、当該度合いを示すことができるものであれば、具体的な算出方法は特に限定されない。特に限定されるものではないが、マッチング率は、例えば、ステップＳ１３０で抽出された特徴点と、図６に示す特徴点ＦＰとをマッチングさせた際の、各特徴点での偏差の和等、任意の指標を適用して算出される。また、自己位置推定部２６は、最終的な自己位置の推定結果が、マッチング率の高い事前推定位置に偏るように推定を行う。例えば、二つの事前推定位置のマッチング率が等しい場合（１：１の比率）、自己位置は、二つの事前推定位置の中点に設定される。これに対して、一方の事前推定位置のマッチング率が高い場合、その比率に応じた分だけ、自己位置が中点よりも一方の事前推定位置に寄る。
【0051】
具体的に、図１４（ｂ）に示すように、事前推定位置ＢＰ１のデータＤＴ１に対するマッチング率が９０％であり、事前推定位置ＢＰ２のデータＤＴ２に対するマッチング率が５０％である。この場合、事前推定位置ＢＰ１のマッチング率が事前推定位置ＢＰ２のマッチング率よりも１．８倍高い。従って、推定される自己位置ＥＰは、事前推定位置ＢＰ１と事前推定位置ＢＰ２との中点よりも、前記倍率の分だけ、事前推定位置ＢＰ１に近い位置に設定される。図１４（ｃ）に示すように、事前推定位置ＢＰ１のデータＤＴ１に対するマッチング率が７５％であり、事前推定位置ＢＰ２のデータＤＴ２に対するマッチング率が７０％である。この場合、事前推定位置ＢＰ１のマッチング率と事前推定位置ＢＰ２のマッチング率との間にほぼ差がない。従って、推定される自己位置ＥＰは、若干事前推定位置ＢＰ１に寄るものの、事前推定位置ＢＰ１と事前推定位置ＢＰ２との中点とほぼ同じ位置に設定される。なお、マッチング率の比率をどのように自己位置の偏りに反映するかの具体的な演算方法は特に限定されず、マッチング率の高さを重さと見立てた重心を算出する等、任意の演算方法を採用してよい。以上により、図１５に示す自己位置の推定の処理が完了する。
【0052】
なお、上述のように複数のデータを用いて自己位置の推定に対して、評価部３１による評価が介在してもよい。例えば、通常の状態では自己位置推定部２６は、一つのデータを用いて自己位置の推定を行い、推定精度が低下したときに、複数のデータを用いた自己位置推定を実行してよい。すなわち、評価部３１は、一つのデータによる自己位置の推定精度と閾値とを比較する。そして、評価部３１の評価が閾値を超えている場合は、当該自己位置を用いた走行支援が行われる。一方、評価部３１による評価が閾値以下となった場合に、自己位置推定部２６は、上述の複数の事前推定位置による自己位置の推定を実施する。なお、複数のデータを用いた自己位置の推定精度に対しても、評価のための閾値を設定してよい。これにより、複数のデータを用いて自己位置を推定しても推定精度が低いと評価された場合、オドメトリによる走行支援に切り替えてよい。
【0053】
次に、本実施形態に係る自己位置推定装置１、移動体５０、自己位置推定方法、及び自己位置推定プログラムの作用・効果について説明する。
【0054】
自己位置推定装置１は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するためのものである。ここで、例えば、カメラ１２は、データベースのデータが作成されたときの位置から離れた位置で画像を取得する場合がある。このような場合、自己位置の推定に用いるデータが、別の位置のデータに切り替わる。従って、自己位置推定部２６が一つのデータだけで自己位置を推定した場合、推定する自己位置が急に変化する可能性がある。
【0055】
例えば、図１６（ａ）は、実際にフォークリフトに自己位置推定装置１を搭載して、自己位置を推定しながらフォークリフトが走行する走行経路ＲＬを示す。走行経路の黒点はデータが作成されている位置を示す。なお、フォークリフトは、スタートポイントＳＰからゴールポイントＧＰまで走行する。まず、一つのデータのみを用いて自己位置を推定しながら、フォークリフトを走行経路ＲＬに沿って走行させ、各位置において推定された自己位置を集計する実験を行った。このうち、仮想線で囲まれる区間の自己位置の推定結果を図１６（ｂ）に示す。図１６（ｂ）では、推定された自己位置によって描かれる自己位置推定軌跡ＥＬが示されている。当該区間では、データＤＴ１とデータＤＴ２の中間位置ＣＴにて、用いられれるデータが切り替わる。すなわち、中間位置ＣＴよりもデータＤＴ１側の領域では、当該データＤＴ１に基づいた自己位置の推定が行われる。一方、中間位置ＣＴよりもデータＤＴ２側の領域では、当該データＤＴ１に基づいた自己位置の推定が行われる。また、中間位置ＣＴ付近では、いずれのデータＤＴ１，ＤＴ２からも距離が離れているため、推定精度も低下する。その結果、中間位置ＣＴでは、用いられるデータが切り替わることによって、急に自己位置の推定結果が変化する。これにより、自己位置推定軌跡ＥＬが切れてしまい、不連続になる。
【0056】
これに対して、自己位置推定部２６は、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体５０の事前推定位置を複数取得し、複数の事前推定位置から自己位置を推定する。これにより、自己位置推定部２６は、一つのデータのみならず、複数のデータを用いて自己位置を推定することで、データの切り替えなどによって推定結果が急に変化することを抑制できる。そのため、自己位置の推定結果の連続性を向上し、適切な移動体の走行支援を行うことができる。
【0057】
最寄りの二つのデータを用いて自己位置の推定を行う場合について、フォークリフトが図１６（ａ）に示す走行経路ＲＬを走行しながら、自己位置の推定結果を集計する実験を行った。このうち、仮想線で囲まれる区間の自己位置の推定結果を図１６（ｃ）に示す。図１６（ｃ）では、中間位置ＣＴよりもデータＤＴ１側の領域及びデータＤＴ２側の領域の両方において、データＤＴ１及びデータＤＴ２に基づいた自己位置の推定が行われる。すなわち、フォークリフトがデータＤＴ１の位置からデータＤＴ２の位置へ走行するまでに、自己位置推定部２６は、途中でデータが切り替わることなく、データＤＴ１，ＤＴ２を用いて自己位置の推定を行うことができる。また、二つのデータＤＴ１，ＤＴ２を用いることで、一つのデータの場合よりも推定精度を高くすることができる。従って、中間位置ＣＴにおいても、自己位置推定軌跡ＥＬが途切れることなく、連続した軌跡が描かれている。ここで、フォークリフトがＤＴ２から先に進むと、データＤＴ１に変えて、次のデータが用いられる。すなわち、データの切り替えが発生するが、データＤＴ２の近傍の位置では、データＤＴ２が近い位置に存在してるために、高い精度で自己位置を推定できる。そのため、データの切り替えが発生しても、自己位置の推定結果は大きく変動しない。以上より、ロバスト性を向上すると共に、自己位置の推定結果の連続性を向上できることが分かる。なお、図１６（ａ）の走行経路ＲＬを走行するときに一つのデータを用いて自己位置推定したときの推定精度の評価値は２８ｍｍであるのに対し、二つのデータを用いた場合は２１ｍｍまで低減することができた。なお、評価値とは真値と推定結果を比較した際の誤差のばらつきである。
【0058】
自己位置推定部２６は、前回推定した自己位置に対して、少なくとも二つの最寄りのデータを選び、選択した少なくとも二つのデータに基づいて自己位置を推定してよい。これにより、自己位置推定部２６は、移動体５０の近くに存在するデータに基づいて自己位置の推定を行うことができるため、取得した画像の特徴とデータとをマッチングさせやすく、推定精度を向上することができる。
【0059】
自己位置推定装置１は、自己位置の推定精度を評価する評価部３１を更に備え、自己位置推定部２６は、評価部３１による評価が閾値以下となった場合に、複数の事前推定位置による自己位置の推定を実施してよい。これにより、自己位置推定部２６は、複数のデータを用いることによる自己位置の推定を必要なタイミングに絞って実施することができるため、演算の負荷を低減することができる。
【0060】
自己位置推定部２６は、事前推定位置の元になったデータとのマッチング率を各々の事前推定位置について算出し、最終的な自己位置の推定結果が、マッチング率の高い事前推定位置に偏るように推定を行ってよい。これにより、自己位置推定部２６は、マッチング率の高いデータを重み付けをした状態で自己位置の推定を行うことができるため、推定精度を向上することができる。
【0061】
本実施形態に係る移動体５０は、上述の自己位置推定装置１を備える。
【0062】
本実施形態に係る自己位置推定方法は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定方法であって、画像を取得する画像取得ステップと、画像取得ステップで取得された画像から特徴を抽出する抽出ステップと、抽出ステップで抽出された特徴と、データベースとをマッチングさせることで移動体の自己位置を推定する推定ステップと、を備え、推定ステップでは、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体の事前推定位置を複数取得し、複数の事前推定位置から前記自己位置を推定する。
【0063】
本発明に係る自己位置推定プログラムは、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定プログラムであって、画像を取得する画像取得ステップと、画像取得ステップで取得された画像から特徴を抽出する抽出ステップと、抽出ステップで抽出された特徴と、データベースとをマッチングさせることで移動体の自己位置を推定する推定ステップと、をコンピュータシステムに実行させ、推定ステップでは、データベースのうちの複数の位置情報に対応する複数のデータに基づいて、移動体の事前推定位置を複数取得し、複数の事前推定位置から自己位置を推定する。
【0064】
これらの移動体５０、自己位置推定方法、及び自己位置推定プログラムによれば、上述の自己位置推定装置と同様な効果を得ることができる。
【0065】
本発明は、上述の実施形態に限定されるものではない。
【0066】
自己位置推定装置１は、上述の実施形態に加えて、更に追加の処理を行ってよい。例えば、評価部３１は、データベースに登録されている位置情報に対応する位置からどの程度離れたら推定精度が低下するかを評価してよい。これにより、推定精度が下がりやすいエリアについては、データベースに登録される位置情報を増やすことなどが可能になる。当該処理は、特に、図４に示すデータベース作成の時に行われてよい。これにより、推定精度が下がりやすいエリア（例えば、たくさんの物が近くに存在するような環境）については、重要ポイントを多く設定することで、データベースを多く設定することができる。これにより、推定精度を向上できる。また、推定精度が下がりにくいエリア（例えば、開けた環境）については、重要ポイントを少なめに設定することができる。これにより、データ容量を低減できる。なお、当該処理は、移動体５０の走行中に行われてもよく、必要に応じて、重要ポイントを後から増やして、データベースを新たに登録してよい。
【0067】
また、評価部３１は、移動体５０の走行範囲の中で、推定精度が高いと評価した位置を、移動体５０の位置合わせ用の位置としてよい。例えば、移動体５０が自己位置を見失った場合、当該移動体５０は、全てのデータベースに対し自己位置推定を行い、最も推定精度が高いと評価（例えば最もマッチング成功数が高い）された結果を自己位置と把握したうえで、走行に復帰することができる。
【0068】
また、評価部３１は、移動体５０の走行範囲の各位置における評価結果に基づいて、データベース自体の評価を行ってよい。例えば、データベースに登録された位置情報に対応する位置に近付いた場合であっても推定精度の評価が良くならない場合、データベース自体が作成に失敗している可能性がある。従って、評価部３１がデータベース自体の評価を行うことで、当該状況に対する対策をとることが可能になる。例えば、管理者に警告を出したり、データベースを作成しなおすなどの処理が行われてよい。なお、データベース自体の評価は、事前のティーチング時に収集した画像を用いて行われてもよく、走行中に行われてもよい。
【0069】
また、評価部３１は、移動体５０の走行範囲の各位置における評価結果に基づいて、移動体５０の経路からの乖離度合の評価を行ってよい。例えば、データベースに登録された位置情報に対応する位置に近付いた場合であっても推定精度の評価が良くならない場合、移動体５０が経路自体から反れてしまっている可能性がある。従って、評価部３１が移動体の経路からの乖離度合の評価を行うことで、当該状況に対する対策をとることが可能になる。この場合、一旦、移動体５０を停止させ、停止した状態で自己位置推定を繰り返しおこなう。自己位置推定装置１は、繰り返し計算結果のばらつきが小さい場合は、位置推定精度が高いとみなし、オドメトリ補正を行う。自己位置推定装置１は、繰り返し計算結果のばらつきが大きい場合は、補正が困難であるとみなし、管理者に警告を出す。
【0070】
上述の実施形態では、移動体５０に自己位置推定装置の構成要素が全て含まれていた。これに代えて、管理部２が自己位置推定装置の一部の機能を有してもよい。
【符号の説明】
【0071】
１…自己位置推定装置、１２…カメラ（画像取得部）、２６…自己位置推定部（抽出部、推定部）、３１…評価部、５０…移動体。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】