特開 2022-125563 自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022125563

(43)【公開日】2022-08-29

(54)【発明の名称】自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラム

(51)【国際特許分類】

   G05D 1/02 20200101AFI20220822BHJP

   G01C 21/30 20060101ALI20220822BHJP

   G06T 7/70 20170101ALI20220822BHJP

【ＦＩ】

G05D1/02 K

G01C21/30

G06T7/70 A

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2021023219

(22)【出願日】2021-02-17

(71)【出願人】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100124062

【弁理士】

【氏名又は名称】三上 敬史

(74)【代理人】

【識別番号】100148013

【弁理士】

【氏名又は名称】中山 浩光

(74)【代理人】

【識別番号】100162640

【弁理士】

【氏名又は名称】柳 康樹

(72)【発明者】

【氏名】古室 達也

【テーマコード（参考）】

2F129

5H301

5L096

【Ｆターム（参考）】

2F129AA03

2F129BB08

2F129BB50

2F129DD15

2F129EE52

2F129GG17

2F129HH21

2F129HH24

5H301AA01

5H301BB07

5H301CC03

5H301CC06

5H301CC10

5H301GG09

5H301GG12

5L096FA09

5L096FA69

5L096GA51

5L096JA11

(57)【要約】

【課題】移動体の自動走行の運用の質を向上させることができる自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムを提供する。
【解決手段】推定結果採用部３４は、評価が高い評価用自己位置を用いて、採用自己位置を演算することが可能となる。選定部３６は、複数の評価用自己位置の中から、評価部３３の評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する。信頼度演算部３７は、複数の選定自己位置のばらつきに基づいて、採用自己位置の信頼度を演算する。このように、信頼度演算部３７は、採用自己位置の評価だけを考慮するのではなく、複数の選定自己位置を考慮して信頼度を演算することで、信頼度の精度を向上できる。このように、精度の良い信頼度を演算することにより、当該信頼度を有効に用いることによって、移動体５０の自動走行の運用の質を向上させることができる。
【選択図】図８

【特許請求の範囲】

【請求項1】

取得された画像から抽出された特徴と、予め位置情報と前記特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定装置であって、
前記画像を取得する画像取得部と、
前記画像取得部で取得された前記画像から特徴点を抽出する抽出部と、
少なくとも三つの前記特徴点と、前記データベースとをマッチングさせることで前記移動体の評価用自己位置を推定する推定部と、
前記推定部で推定された前記評価用自己位置を評価する評価部と、
前記評価部の評価結果に基づいて、推定結果として採用する採用自己位置を演算する推定結果採用部と、
複数の評価用自己位置の中から、前記評価部の評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する選定部と、
複数の前記選定自己位置のばらつきに基づいて、前記採用自己位置の信頼度を演算する信頼度演算部と、を備える、自己位置推定装置。

【請求項2】

前記選定部は、複数の前記評価用自己位置を複数のグループに分け、それぞれの前記グループ内の複数の前記評価用自己位置の中から、一つの前記選定自己位置を選定する、請求項１に記載の自己位置推定装置。

【請求項3】

前記評価部は、前記評価用自己位置に対応する画像座標に対して、自己位置推定に用いられなかった複数の前記特徴点を再投影して誤差を演算し、当該誤差が所定の閾値以下となる前記特徴点のカウント数を取得することによって、評価を行い、
前記選定部は、前記カウント数に基づいて前記選定自己位置を選定する、請求項１又は２に記載の自己位置推定装置。

【請求項4】

前記信頼度演算部は、複数の前記選定自己位置のうち、他に比べて乖離が大きいものを除外して、前記信頼度を演算する、請求項１～３の何れか一項に記載の自己位置推定装置。

【請求項5】

前記推定部は、複数の前記特徴点の中から、推定精度を低下させるものを除外して推定を行う、請求項１～４の何れか一項に記載の自己位置推定装置。

【請求項6】

請求項１～５の何れか一項に記載された自己位置推定装置を備える移動体。

【請求項7】

取得された画像から抽出された特徴と、予め位置情報と前記特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定方法であって、
前記画像を取得する画像取得ステップと、
前記画像取得ステップで取得された前記画像から特徴点を抽出する抽出ステップと、
少なくとも三つの前記特徴点と、前記データベースとをマッチングさせることで前記移動体の評価用自己位置を推定する推定ステップと、
前記推定ステップで推定された前記評価用自己位置を評価する評価ステップと、
前記評価ステップの評価結果に基づいて、推定結果として採用する採用自己位置を演算する推定結果採用ステップと、
複数の評価用自己位置の中から、前記評価ステップの評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する選定ステップと、
複数の前記選定自己位置のばらつきに基づいて、前記採用自己位置の信頼度を演算する信頼度演算ステップと、を備える、自己位置推定方法。

【請求項8】

取得された画像から抽出された特徴と、予め位置情報と前記特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定プログラムであって、
前記画像を取得する画像取得ステップと、
前記画像取得ステップで取得された前記画像から特徴点を抽出する抽出ステップと、
少なくとも三つの前記特徴点と、前記データベースとをマッチングさせることで前記移動体の評価用自己位置を推定する推定ステップと、
前記推定ステップで推定された前記評価用自己位置を評価する評価ステップと、
前記評価ステップの評価結果に基づいて、推定結果として採用する採用自己位置を演算する推定結果採用ステップと、
複数の評価用自己位置の中から、前記評価ステップの評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する選定ステップと、
複数の前記選定自己位置のばらつきに基づいて、前記採用自己位置の信頼度を演算する信頼度演算ステップと、を制御部に実行させる、自己位置推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムに関する。

【背景技術】

【0002】

従来の自己位置推定装置として、特許文献１に記載されたものが知られている。この自己位置推定装置は、事前に取得した画像と、移動体の走行中に取得した画像と、を用いることで、自己位置を推定している。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１８－８１００８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

ここで、画像から自己位置の推定を行った場合の推定精度が常に良いとは限らない。この場合、推定精度が低い自己位置の推定結果を用いて、移動体の走行支援を行うと、適切な走行支援を行うことができず、移動体が経路から外れて走行を行ってしまう可能性がある。そのため、移動体の自動走行の運用の質を向上することが求められていた。

【0005】

従って、本発明は、移動体の自動走行の運用の質を向上させることができる自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムを提供することを目的とする。

【課題を解決するための手段】

【0006】

本発明の一態様に係る自己位置推定装置は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定装置であって、画像を取得する画像取得部と、画像取得部で取得された画像から特徴点を抽出する抽出部と、少なくとも三つの特徴点と、データベースとをマッチングさせることで移動体の評価用自己位置を推定する推定部と、推定部で推定された評価用自己位置を評価する評価部と、評価部の評価結果に基づいて、推定結果として採用する採用自己位置を演算する推定結果採用部と、複数の評価用自己位置の中から、評価部の評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する選定部と、複数の選定自己位置のばらつきに基づいて、採用自己位置の信頼度を演算する信頼度演算部と、を備える。

【0007】

自己位置推定装置は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するためのものである。ここで、推定部は、少なくとも三つの特徴点と、データベースとをマッチングさせることで移動体の評価用自己位置を推定する。これに対し、評価部は、推定部で推定された評価用自己位置を評価する。推定結果採用部は、評価部の評価結果に基づいて、推定結果として採用する採用自己位置を演算する。このように、推定結果採用部は、評価が高い評価用自己位置を用いて、採用自己位置を演算することが可能となる。これに対し、選定部は、複数の評価用自己位置の中から、評価部の評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する。そして、信頼度演算部は、複数の選定自己位置のばらつきに基づいて、採用自己位置の信頼度を演算する。このように、信頼度演算部は、採用自己位置の評価だけを考慮するのではなく、複数の選定自己位置を考慮して信頼度を演算することで、信頼度の精度を向上できる。このように、精度の良い信頼度を演算することにより、当該信頼度を有効に用いることによって、移動体の自動走行の運用の質を向上させることができる。

【0008】

選定部は、複数の評価用自己位置を複数のグループに分け、それぞれのグループ内の複数の評価用自己位置の中から、一つの選定自己位置を選定してよい。このように、全ての評価用自己位置を信頼度の演算に用いるのではなく、選定部が各グループの中から一つの選定自己位置を選定することで、演算の負荷を低減することができる。

【0009】

評価部は、評価用自己位置に対応する画像座標に対して、自己位置推定に用いられなかった複数の特徴点を再投影して誤差を演算し、当該誤差が所定の閾値以下となる特徴点のカウント数を取得することによって、評価を行い、選定部は、カウント数に基づいて選定自己位置を選定してよい。この場合、評価部は、誤差をカウントするだけのシンプルな演算方法によって、各評価用自己位置の評価を行うことができる。

【0010】

信頼度演算部は、複数の選定自己位置のうち、他に比べて乖離が大きいものを除外して、信頼度を演算してよい。この場合、信頼度演算部は、明かに信頼度の精度を低下させるような選定自己位置を除外することで、信頼度の精度を向上できる。

【0011】

推定部は、複数の特徴点の中から、推定精度を低下させるものを除外して推定を行ってよい。この場合、推定部は、精度が高い特徴点を用いて自己位置の推定を行うことで、自己位置の推定精度、及び信頼度を向上することができる。

【0012】

本発明の一態様に係る移動体は、上述の自己位置推定装置を備える。

【0013】

本発明の一態様に係る自己位置推定方法は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定方法であって、画像を取得する画像取得ステップと、画像取得ステップで取得された画像から特徴点を抽出する抽出ステップと、少なくとも三つの特徴点と、データベースとをマッチングさせることで移動体の評価用自己位置を推定する推定ステップと、推定ステップで推定された評価用自己位置を評価する評価ステップと、評価ステップの評価結果に基づいて、推定結果として採用する採用自己位置を演算する推定結果採用ステップと、複数の評価用自己位置の中から、評価ステップの評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する選定ステップと、複数の選定自己位置のばらつきに基づいて、採用自己位置の信頼度を演算する信頼度演算ステップと、を備える。

【0014】

本発明の一態様に係る自己位置推定プログラムは、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体の自己位置を推定するための自己位置推定プログラムであって、画像を取得する画像取得ステップと、画像取得ステップで取得された画像から特徴点を抽出する抽出ステップと、少なくとも三つの特徴点と、データベースとをマッチングさせることで移動体の評価用自己位置を推定する推定ステップと、推定ステップで推定された評価用自己位置を評価する評価ステップと、評価ステップの評価結果に基づいて、推定結果として採用する採用自己位置を演算する推定結果採用ステップと、複数の評価用自己位置の中から、評価ステップの評価結果に基づいて、信頼度演算用の複数の選定自己位置を選定する選定ステップと、複数の選定自己位置のばらつきに基づいて、採用自己位置の信頼度を演算する信頼度演算ステップと、を制御部に実行させる。

【0015】

これらの移動体、自己位置推定方法、及び自己位置推定プログラムによれば、上述の自己位置推定装置と同様な効果を得ることができる。

【発明の効果】

【0016】

本発明によれば、移動体の自動走行の運用の質を向上させることができる自己位置推定装置、移動体、自己位置推定方法、及び自己位置推定プログラムを提供することができる。

【図面の簡単な説明】

【0017】

【図1】本発明の実施形態に係る自己位置推定装置を備える自己位置推定システムを示す概略図である。

【図2】本発明の実施形態に係る自己位置推定装置を備える移動体のブロック構成を示すブロック構成図である。

【図3】データベース作成時の作業場の様子を示す概略図である。

【図4】データベースの作成方法を示すフローチャートである。

【図5】特徴を抽出する方法を説明するための図である。

【図6】カメラで取得された画像、及び抽出された特徴を示す図である。

【図7】特徴の三次元座標を取得する方法を示す概念図である。

【図8】自己位置推定部のブロック構成図である。

【図9】特徴点を用いて自己位置を推定する様子、及び自己位置を評価するときの様子を示す概念図である。

【図10】移動体が自動走行を行うときの作業場の様子を示す概略図である。

【図11】移動体の自己位置の推定方法を示すフローチャートである。

【図12】自己位置を推定するための詳細な制御処理内容を示すフローチャートである。

【図13】信頼度を演算するための詳細な制御処理内容を示すフローチャートである。

【図14】（ａ）は特徴点間の角度差を示す概念図であり、（ｂ）は複数のセンサによる信頼度を選ぶ様子を示す概念図である。

【発明を実施するための形態】

【0018】

以下、本発明の実施形態について、図面を参照して詳細に説明する。

【0019】

図１は、本実施形態に係る自己位置推定装置１を備える自己位置推定システム１００を示す概略図である。図１に示すように、自己位置推定システム１００は、複数の移動体５０のそれぞれに設けられた自己位置推定装置１と、管理部２と、を備える。

【0020】

本実施形態では、移動体５０として、フォークリフトが例示されている。図１には、移動体５０としてのフォークリフトが、倉庫や工場などの作業場Ｅ（所定の領域）にて、荷物の積み卸しの作業を行っている様子が示されている。作業場Ｅでは、複数の棚６０が配列されている。また、棚６０と棚６０との間に、移動体５０が通過するための通路６１が形成されている。自己位置推定装置１は、このような作業場Ｅ内における移動体５０の自己位置を推定する装置である。自己位置推定装置１は、取得された画像から抽出された特徴と、予め位置情報と特徴とを関連付けたデータベースとをマッチングさせることで移動体５０の自己位置を推定する。移動体５０は、自己位置推定装置１で推定された自己位置を用いることで、作業場Ｅ内で自動走行することができる。なお、自己位置推定装置１の詳細な構成については後述する。管理部２は、作業場Ｅ内の複数の移動体５０を管理するサーバーである。管理部２は、必要に応じて、複数の移動体５０から所定の情報を受信し、複数の移動体５０に対して所定の情報を送信する。

【0021】

図２は、本実施形態に係る自己位置推定装置１を備える移動体５０のブロック構成を示すブロック構成図である。図２に示すように、移動体５０は、走行部１１と、カメラ１２と、制御部２０と、を備える。走行部１１は、移動体５０を走行させるための駆動力を発生するモータなどの駆動系である。カメラ１２（画像取得部）は、移動体５０の周囲の画像を取得する機器である。カメラ１２は、取得した画像を自己位置推定部２６へ送信する。

【0022】

制御部２０は、移動体５０を統括的に管理するＥＣＵ[Electronic Control Unit]を備えている。ＥＣＵは、ＣＰＵ[Central Processing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]、ＣＡＮ［Controller Area Network］通信回路等を有する電子制御ユニットである。ＥＣＵでは、例えば、ＲＯＭに記憶されているプログラムをＲＡＭにロードし、ＲＡＭにロードされたプログラムをＣＰＵで実行することにより各種の機能を実現する。制御部２０は、経路計画部２１と、指令速度計算部２２と、通信部２３と、記憶部２４と、自己位置推定部２６と、オドメトリ計算部２７と、自己位置決定部２８と、を備える。このうち、記憶部２４、自己位置推定部２６、オドメトリ計算部２７、自己位置決定部２８、及びカメラ１２によって自己位置推定装置１が構成される。

【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】

これらの移動体５０、自己位置推定方法、及び自己位置推定プログラムによれば、上述の自己位置推定装置１と同様な効果を得ることができる。

【0072】

本発明は、上述の実施形態に限定されるものではない。

【0073】

例えば、上述の実施形態では、ステップＳ２８０において、バッファに格納された全ての選択自己位置を用いて信頼度を演算した。しかし、信頼度演算部３７は、複数の選定自己位置のうち、他に比べて乖離が大きいものを除外して、信頼度を演算してよい。すなわち、信頼度演算部３７は、平均値から明かにはずれている選択自己位置については、一度除外した上で、残りの選択自己位置を用いてばらつきを演算してよい。選定部３６は、グループ分けされた少ない数の評価用自己位置の中から選定自己位置を選定している。そのため、他の選定自己位置からの乖離が大きくなってしまうものが発生し得る。信頼度演算部３７は、このような明かに信頼度の精度を低下させるような乖離の大きい選定自己位置を除外することで、信頼度の精度を向上できる。

【0074】

上述の実施形態では、推定部３２は、全ての特徴点を用いて自己位置を推定した。しかし、推定部３２は、複数の特徴点の中から、推定精度を低下させるものを除外して推定を行ってよい。例えば、推定部３２は、数回の計算の後で、評価部３３によるカウントの際に、一度もカウントされていない特徴点が存在している場合、当該特徴点は推定精度を低下させるものとして除外する。無数の特徴点の中には、自己位置推定に用いてしまうと、自己位置推定の推定精度が低下してしまうような、精度の悪い特徴点も含まれるため、そのような特徴点を演算から除外できる。従って、自己位置の推定精度、及び信頼度の精度を向上できる。

【0075】

また、推定部３２は、ランダムに三つの特徴点を選ぶ際、図１４（ａ）の左側に示すように、角度差が小さい三点を選んでしまうと、誤差が大きくなって自己位置の推定精度が低下する。従って、推定部３２は、図１４（ａ）の右側に示すように、角度差を大きくして誤差が小さくなるように、角度差を考慮して三点を選んでよい。

【0076】

また、推定結果採用部３４は、最もカウント数が大きい評価用自己位置を採用自己位置とした。これに代えて、推定結果採用部３４は、他の演算方法によって採用自己位置を演算してよい。例えば、推定結果採用部３４は、複数の選定自己位置を平均することによって、採用自己位置を演算してよい。

【0077】

上述の実施形態では、移動体５０に自己位置推定装置の構成要素が全て含まれていた。これに代えて、管理部２が自己位置推定装置の一部の機能を有してもよい。

【符号の説明】

【0078】

１…自己位置推定装置、１２…カメラ（画像取得部）、３１…抽出部、３２…推定部、３３…評価部、３４…推定結果採用部、３６…選定部、３７…信頼度演算部、５０…移動体。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】