(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2023-11-14
(45)【発行日】2023-11-22
(54)【発明の名称】自己位置推定装置
(51)【国際特許分類】
G05D 1/02 20200101AFI20231115BHJP
G01C 21/28 20060101ALI20231115BHJP
【ＦＩ】
G05D1/02 H
G01C21/28
【請求項の数】
3
(21)【出願番号】P 2019134472
(22)【出願日】2019-07-22
(65)【公開番号】P2021018638
(43)【公開日】2021-02-15
【審査請求日】2022-04-01
(73)【特許権者】
【識別番号】000000262
【氏名又は名称】株式会社ダイヘン
(74)【代理人】
【識別番号】100115749
【弁理士】
【氏名又は名称】谷川 英和
(74)【代理人】
【識別番号】100121223
【弁理士】
【氏名又は名称】森本 悟道
(72)【発明者】
【氏名】土方 優明
【審査官】牧 初
(56)【参考文献】
【文献】国際公開第２０１９／１１１７０１（ＷＯ，Ａ１）
【文献】特開２０１８－１２８３８６（ＪＰ，Ａ）
【文献】国際公開第２０１６／０５９９３０（ＷＯ，Ａ１）
【文献】特開２０１６－０２４５９８（ＪＰ，Ａ）
【文献】特開２０１６－１８８８０６（ＪＰ，Ａ）
【文献】特開２０１９－２０７１７７（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／００－１／１２
Ｇ０１Ｃ ２１／００－２１／３６
Ｇ０１Ｃ ２３／００－２５／００
Ｇ０８Ｇ １／００－９９／００
(57)【特許請求の範囲】
【請求項1】
移動体の自己位置を推定する自己位置推定装置であって、前記移動体の位置に関する推定に用いられる情報を取得するセンサ部と、
前記センサ部によって取得された同じ情報を用いて、それぞれ異なる方法によって前記移動体の位置に関する推定を行うと共に、当該推定結果に関する信頼度を取得する複数の位置推定部と、
前記複数の位置推定部によってそれぞれ取得された複数の信頼度のうち、最も高い信頼度に対応する推定結果に応じて前記移動体の自己位置を取得する取得部と、を備えた自己位置推定装置。
【請求項2】
前記複数の位置推定部は、前記移動体の位置を推定するものであり、前記取得部は、前記複数の信頼度のうち、最も高い信頼度に対応する推定位置を前記移動体の自己位置として選択する、請求項１記載の自己位置推定装置。
【請求項3】
前記複数の位置推定部は、前回の自己位置に加算される移動量を推定するものであり、前記取得部は、前回の自己位置に、前記複数の信頼度のうち、最も高い信頼度に対応する移動量を加算することによって自己位置を取得する、請求項１記載の自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の自己位置を推定する自己位置推定装置に関する。
【背景技術】
【0002】
従来、移動体に関する自己位置を取得して移動することが行われており、その自己位置の取得を高精度で行うことが検討されている。例えば、複数の移動体全体の位置の信頼度を高い状態で維持するため、自己位置の信頼度を回復させるように移動制御することが行われている（例えば、特許文献１参照）。また、例えば、航法衛星を用いた自己位置推定の精度の安定化のため、常に受信可能となる航法衛星の組合せを選択して自己位置を推定することも行われている（例えば、特許文献２参照）。また、例えば、ステレオカメラ等を用いた環境測定の結果と、エンコーダ等を用いて取得された情報とを統合した位置推定も行われている（例えば、特許文献３参照）。
【先行技術文献】
【特許文献】
【0003】
【文献】特開２０１７－１８８０６６号公報
【文献】特開２０１７－１８２６９２号公報
【文献】特開２００８－２３４３５０号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
上記特許文献１に記載された技術は、複数の移動体を用いて位置推定の精度を向上させるものであり、単体の移動体における自己位置推定の精度向上には効果的ではない。
【0005】
また、上記特許文献２に記載された技術は、複数の衛星を利用して位置推定を行うものであるが、複数の衛星の選択基準は、対象の衛星が利用可能かどうかである。したがって、例えば、位置推定に用いていた衛星の信号を受信できなくなる事態を回避することによって、測位精度が低下することを防止することはできる。一方、障害物の多い空間を移動している場合には、障害物の影響を受けない複数の衛星を選択できないことも考えられ、そのような状況においては、位置推定の精度が低下する可能性がある。
【0006】
また、上記特許文献３に記載された技術では、環境測定の結果と、エンコーダ等を用いて取得された情報とを統合して位置推定を行うため、例えば、一方の精度が低下した場合には、統合結果においても、精度が低下することになるという問題がある。
【0007】
本発明は、上記課題を解決するためになされたものであり、移動体に関するより精度の高い自己位置推定を実現することができる自己位置推定装置を提供することを目的とする。
【課題を解決するための手段】
【0008】
上記目的を達成するため、本発明による自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、移動体の位置に関する推定に用いられる情報を取得するセンサ部と、センサ部によって取得された情報を用いて移動体の位置に関する推定を行うと共に、推定結果に関する信頼度を取得する複数の位置推定部と、複数の位置推定部によってそれぞれ取得された複数の信頼度のうち、最も高い信頼度に対応する推定結果に応じて移動体の自己位置を取得する取得部と、を備えたものである。
このような構成により、信頼度の最も高い推定結果に応じた自己位置を取得することができ、より精度の高い自己位置推定を実現することができる。また、複数の移動体が存在しなくても、単体の移動体における自己位置推定の精度を向上させることができる。また、例えば、ある位置推定部がＧＰＳ（Global Positioning System）によって位置推定を行うものであり、障害物が多い環境などのようにマルチパスの多い環境を移動している場合であっても、信頼度に応じて、別の位置推定部による、ＧＰＳ以外の位置推定の結果を用いることによって、精度の高い自己位置の取得が可能となる。また、複数の推定結果のうち、信頼度の最も高い推定結果に応じた自己位置を取得することにより、仮にいずれかの推定結果の精度が著しく低下した場合であっても、それ以外の推定結果に応じた自己位置の取得を行うことによって、自己位置の精度の低下を回避することができる。
このような構成により、信頼度の最も高い推定結果に応じた自己位置を取得することができ、より精度の高い自己位置推定を実現することができる。また、複数の移動体が存在しなくても、単体の移動体における自己位置推定の精度を向上させることができる。また、例えば、ある位置推定部がＧＰＳ（Global Positioning System）によって位置推定を行うものであり、障害物が多い環境などのようにマルチパスの多い環境を移動している場合であっても、信頼度に応じて、別の位置推定部による、ＧＰＳ以外の位置推定の結果を用いることによって、精度の高い自己位置の取得が可能となる。また、複数の推定結果のうち、信頼度の最も高い推定結果に応じた自己位置を取得することにより、仮にいずれかの推定結果の精度が著しく低下した場合であっても、それ以外の推定結果に応じた自己位置の取得を行うことによって、自己位置の精度の低下を回避することができる。
【0009】
また、本発明による自己位置推定装置では、センサ部は、異なる情報を取得するセンサである複数のセンサを有し、複数の位置推定部は、複数のセンサによって取得された複数の情報をそれぞれ用いて移動体の位置に関する推定を行ってもよい。
このような構成により、複数のセンサによってそれぞれ取得された情報を用いて、複数の推定結果が取得されることになる。したがって、いずれかのセンサによって取得された情報を用いて取得された推定結果の信頼度が低い場合に、それ以外のセンサによって取得された情報を用いて取得された推定結果に応じて自己位置が取得される。センサにはそれぞれ、長所や短所があるが、このような自己位置の取得を行うことによって、相互に短所を補完することができ、より精度の高い自己位置の取得を実現することができる。
このような構成により、複数のセンサによってそれぞれ取得された情報を用いて、複数の推定結果が取得されることになる。したがって、いずれかのセンサによって取得された情報を用いて取得された推定結果の信頼度が低い場合に、それ以外のセンサによって取得された情報を用いて取得された推定結果に応じて自己位置が取得される。センサにはそれぞれ、長所や短所があるが、このような自己位置の取得を行うことによって、相互に短所を補完することができ、より精度の高い自己位置の取得を実現することができる。
【0010】
また、本発明による自己位置推定装置では、複数の位置推定部は、センサ部によって取得された同じ情報を用いて、それぞれ異なる方法によって移動体の位置に関する推定を行ってもよい。
このような構成により、より信頼度の高い方法によって取得された推定結果に応じて自己位置が取得される。位置に関する推定の方法に応じて精度が異なることがあるが、このような自己位置の取得を行うことによって、より精度の高い自己位置の取得を実現することができる。例えば、確率的な方法を用いて位置に関する推定を行う場合には、パラメータ等を少し変更するだけでも、推定結果が異なることもある。そのような場合に、信頼度の最も高い推定方法の推定結果を用いて自己位置を取得することにより、精度が向上することになる。
このような構成により、より信頼度の高い方法によって取得された推定結果に応じて自己位置が取得される。位置に関する推定の方法に応じて精度が異なることがあるが、このような自己位置の取得を行うことによって、より精度の高い自己位置の取得を実現することができる。例えば、確率的な方法を用いて位置に関する推定を行う場合には、パラメータ等を少し変更するだけでも、推定結果が異なることもある。そのような場合に、信頼度の最も高い推定方法の推定結果を用いて自己位置を取得することにより、精度が向上することになる。
【0011】
また、本発明による自己位置推定装置では、複数の位置推定部は、移動体の位置を推定するものであり、取得部は、複数の信頼度のうち、最も高い信頼度に対応する推定位置を移動体の自己位置として選択してもよい。
このような構成により、信頼度の最も高い推定位置を、移動体の自己位置とすることができ、推定結果を用いて自己位置を取得する処理が選択のみとなるため、自己位置の取得の処理が簡易な処理となる。
このような構成により、信頼度の最も高い推定位置を、移動体の自己位置とすることができ、推定結果を用いて自己位置を取得する処理が選択のみとなるため、自己位置の取得の処理が簡易な処理となる。
【0012】
また、本発明による自己位置推定装置では、複数の位置推定部は、前回の自己位置に加算される移動量を推定するものであり、取得部は、前回の自己位置に、複数の信頼度のうち、最も高い信頼度に対応する移動量を加算することによって自己位置を取得してもよい。
このような構成により、自己位置の取得に用いる推定結果を、信頼度に応じて、ある推定結果から別の推定結果に変更する場合であっても、変更後の自己位置が大きく変動することを回避できる。したがって、シームレスに推定結果を切り替えることができ、滑らかな自己位置の推定を実現することができる。
このような構成により、自己位置の取得に用いる推定結果を、信頼度に応じて、ある推定結果から別の推定結果に変更する場合であっても、変更後の自己位置が大きく変動することを回避できる。したがって、シームレスに推定結果を切り替えることができ、滑らかな自己位置の推定を実現することができる。
【発明の効果】
【0013】
本発明による自己位置推定装置によれば、信頼度の最も高い推定結果に応じた自己位置を取得することができ、より精度の高い自己位置推定を実現することができる。
【図面の簡単な説明】
【0014】
【発明を実施するための形態】
【0015】
以下、本発明による自己位置推定装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。本実施の形態による自己位置推定装置は、複数の位置推定部によって推定された移動体の位置に関する推定結果のうち、信頼度が最も高い推定結果に応じて自己位置を取得するものである。
【0016】
図１は、本実施の形態による移動体１の構成を示すブロック図である。本実施の形態による移動体１は、自己位置を取得し、その自己位置を用いて移動するものであり、移動体１の自己位置を推定する自己位置推定装置２と、移動機構１１と、移動制御部１２とを備える。また、自己位置推定装置２は、センサ部２１と、第１の位置推定部２２と、第２の位置推定部２３と、取得部２４とを備える。なお、本実施の形態では、自己位置推定装置２が２個の位置推定部２２，２３を備える場合について主に説明するが、そうでなくてもよい。自己位置推定装置２は、後述するように、３個以上の位置推定部を備えていてもよい。
【0017】
移動体１は、例えば、陸上を走行する走行体であってもよく、または、空中を飛行する飛行体であってもよく、水上を移動する水上航行体（例えば、船舶など）であってもよく、水中を移動する水中航行体（例えば、潜水艇など）であってもよい。本実施の形態では、移動体１が走行体である場合について主に説明する。
【0018】
走行体である移動体１は、例えば、台車であってもよく、自動運転車であってもよく、移動するロボットであってもよい。ロボットは、例えば、エンターテインメントロボットであってもよく、監視ロボットであってもよく、搬送ロボットであってもよく、清掃ロボットであってもよく、動画や静止画を撮影するロボットであってもよく、その他のロボットであってもよい。
【0019】
飛行体は、例えば、回転翼機であってもよく、飛行機であってもよく、飛行船であってもよく、その他の飛行体であってもよい。任意の位置に移動可能であるという観点からは、飛行体は、回転翼機であることが好適である。回転翼機は、例えば、ヘリコプターであってもよく、３個以上の回転翼（ロータ）を有するマルチコプターであってもよい。マルチコプターは、例えば、４個の回転翼を有するクワッドロータであってもよく、その他の個数の回転翼を有するものであってもよい。
【0020】
また、移動体１は、例えば、自律的に移動するものであってもよい。なお、移動体１が自律的に移動するとは、移動体１がユーザ等から受け付ける操作指示に応じて移動するのではなく、自らの判断によって目的地に移動することであってもよい。その目的地は、例えば、手動で決められたものであってもよく、または、自動的に決定されたものであってもよい。また、その目的地までの移動は、例えば、移動経路に沿って行われてもよく、または、そうでなくてもよい。また、自らの判断によって目的地に移動するとは、例えば、進行方向、移動や停止などを移動体１が自ら判断することによって、目的地まで移動することであってもよい。また、例えば、移動体１が、障害物に衝突しないように移動することであってもよい。
【0021】
次に、移動体１の自己位置推定装置２の構成について説明する。
センサ部２１は、移動体１の位置に関する推定に用いられる情報を取得する。なお、移動体１の位置に関する推定は、移動体１の位置のみに関する推定であってもよく、または、移動体１の位置及び姿勢（角度）に関する推定であってもよい。本実施の形態では、後者の場合について主に説明する。ここでは、図１で示されるように、センサ部２１が、第１のセンサ３１と、第２のセンサ３２とを備える場合について主に説明し、それ以外の場合については後述する。第１及び第２のセンサ３１，３２は、それぞれ異なる情報を取得するセンサである。異なる情報を取得するセンサとは、例えば、異なる種類のセンシングによって情報を取得するセンサ（例えば、画像センサと測距センサなど）であってもよく、同じ種類のセンシングによって異なる対象の情報を取得するセンサ（例えば、移動体１のある方向の範囲（前方の範囲など）について測距を行う測距センサと、別の方向の範囲（後方の範囲など）について測距を行う測距センサなど）であってもよい。第１及び第２のセンサ３１，３２は、特に限定されるものではないが、それぞれ独立して、例えば、移動体１の周囲の情報を取得するセンサ、角位置センサ、加速度センサ、ＧＰＳセンサ等であってもよい。移動体１の周囲の情報を取得するセンサは、例えば、測距センサ、画像センサ等であってもよい。なお、第１及び第２のセンサ３１，３２による情報の取得は、同期して行われることが好適である。第１及び第２の位置推定部２２，２３によって、同時点の位置に関する推定が行われることが好適だからである。
センサ部２１は、移動体１の位置に関する推定に用いられる情報を取得する。なお、移動体１の位置に関する推定は、移動体１の位置のみに関する推定であってもよく、または、移動体１の位置及び姿勢（角度）に関する推定であってもよい。本実施の形態では、後者の場合について主に説明する。ここでは、図１で示されるように、センサ部２１が、第１のセンサ３１と、第２のセンサ３２とを備える場合について主に説明し、それ以外の場合については後述する。第１及び第２のセンサ３１，３２は、それぞれ異なる情報を取得するセンサである。異なる情報を取得するセンサとは、例えば、異なる種類のセンシングによって情報を取得するセンサ（例えば、画像センサと測距センサなど）であってもよく、同じ種類のセンシングによって異なる対象の情報を取得するセンサ（例えば、移動体１のある方向の範囲（前方の範囲など）について測距を行う測距センサと、別の方向の範囲（後方の範囲など）について測距を行う測距センサなど）であってもよい。第１及び第２のセンサ３１，３２は、特に限定されるものではないが、それぞれ独立して、例えば、移動体１の周囲の情報を取得するセンサ、角位置センサ、加速度センサ、ＧＰＳセンサ等であってもよい。移動体１の周囲の情報を取得するセンサは、例えば、測距センサ、画像センサ等であってもよい。なお、第１及び第２のセンサ３１，３２による情報の取得は、同期して行われることが好適である。第１及び第２の位置推定部２２，２３によって、同時点の位置に関する推定が行われることが好適だからである。
【0022】
また、移動体１の位置の推定が、移動体１の位置及び姿勢の推定である場合には、第１のセンサ３１や第２のセンサ３２は、２以上のセンサを有していてもよい。例えば、第１のセンサ３１及び／または第２のセンサ３２は、加速度センサと、移動体１の姿勢の検出に用いられるセンサとを有していてもよく、ＧＰＳセンサと、移動体１の姿勢の検出に用いられるセンサとを有していてもよい。移動体１の姿勢の検出に用いられるセンサは、例えば、ジャイロセンサであってもよく、地磁気センサ（電子コンパス）であってもよい。ジャイロセンサは、例えば、移動体１の姿勢を検出するものであってもよく、移動体１の角速度や角加速度を検出するものであってもよい。
【0023】
測距センサは、複数方向に関して周囲の物体までの距離を測定するセンサであり、例えば、レーザセンサや、超音波センサ、マイクロ波を用いた距離センサ、ステレオカメラによって撮影されたステレオ画像を用いた距離センサなどであってもよい。レーザセンサは、レーザレンジセンサ（レーザレンジスキャナ）であってもよい。なお、それらの測距センサについてはすでに公知であり、それらの説明を省略する。本実施の形態では、測距センサがレーザレンジセンサである場合について主に説明する。また、測距センサは、１個のレーザレンジセンサを有していてもよく、または、２個以上のレーザレンジセンサを有していてもよい。後者の場合には、２個以上のレーザレンジセンサによって、全方向がカバーされてもよい。また、測距センサが超音波センサや、マイクロ波を用いた距離センサなどである場合に、測距センサの測距方向を回転させることによって複数方向の距離を測定してもよく、複数方向ごとに配置された複数の測距センサを用いて複数方向の距離を測定してもよい。複数方向の距離を測定するとは、例えば、あらかじめ決められた角度範囲や全周囲（３６０度）について、あらかじめ決められた角度間隔で複数方向の距離を測定することであってもよい。その角度間隔は、例えば、１度間隔や２度間隔、５度間隔などのように一定であってもよい。測距センサによって取得される情報は、例えば、移動体１のある向きを基準とした複数の方位角のそれぞれに関する周辺の物体までの距離であってもよい。その距離を用いることによって、移動体１のローカル座標系において、移動体１の周囲にどのような物体が存在するのかを知ることができるようになる。
【0024】
画像センサは、例えば、ＣＣＤイメージセンサや、ＣＭＯＳイメージセンサなどであってもよい。なお、それらの画像センサについてはすでに公知であり、それらの説明を省略する。画像センサは、例えば、画像センサ上に結像させるためのレンズ等の光学系を含んでいてもよい。また、画像センサは、単眼であってもよく、双眼（ステレオカメラ）であってもよい。画像センサは、通常、動画を撮影するもの、すなわち、連続した画像フレームを取得するものである。移動している移動体１の自己位置を取得するために撮影画像を取得することから、そのフレームレートは、移動速度に対して十分大きいものであることが好適である。例えば、フレームレートは、約３０ｆｐｓなどであってもよい。
【0025】
角位置センサは、例えば、ロータリーエンコーダ等であってもよい。なお、角位置センサについてはすでに公知であり、その説明を省略する。角位置センサは、例えば、移動体１の車輪等の駆動軸の回転量を検出するために用いられてもよい。角位置センサによって取得される情報は、例えば、回転軸の回転量や角度であってもよい。
【0026】
加速度センサは、移動体１の加速度を計測する。この加速度センサは、例えば、２軸の加速度センサであってもよく、３軸の加速度センサであってもよい。なお、加速度センサについてはすでに公知であり、その説明を省略する。移動方向が所定の平面内に限定される場合（例えば、移動体１が床面を移動する走行体である場合など）には、２軸の加速度センサが用いられてもよい。加速度センサによって取得される情報は、例えば、移動体１の加速度であってもよい。
【0027】
ＧＰＳセンサは、ＧＰＳ衛星からの電波を受信するセンサである。なお、ＧＰＳセンサについてはすでに公知であり、その説明を省略する。ＧＰＳセンサによって取得される情報は、例えば、ＧＰＳ衛星から送信された電波であってもよい。
【0028】
ジャイロセンサによって取得される情報は、例えば、移動体１の姿勢（角度）や、角速度、角加速度であってもよい。また、地磁気センサによって取得される情報は、例えば、移動体１の姿勢であってもよい。移動体１の姿勢は、例えば、移動体１の方位角や、移動体１の方位角及び仰俯角であってもよい。
【0029】
第１及び第２の位置推定部２２，２３はそれぞれ、センサ部２１が有する第１及び第２のセンサ３１，３２によって取得された情報を用いて移動体１の位置に関する推定を行うと共に、その推定結果に関する信頼度を取得する。なお、第１及び第２の位置推定部２２，２３はそれぞれ、第１及び第２のセンサ３１，３２の種類に応じた位置に関する推定や信頼度の取得を行うものとする。例えば、第１のセンサ３１が測距センサである場合には、第１の位置推定部２２は、測距センサによって取得された情報を用いた位置に関する推定や信頼度の取得を行い、第１のセンサ３１が画像センサである場合には、第１の位置推定部２２は、画像センサによって取得された情報を用いた位置に関する推定や信頼度の取得を行う。したがって、第１及び第２の位置推定部２２，２３は、特に限定されるものではないが、それぞれ独立して、例えば、移動体１の周囲の情報を取得するセンサによって取得された情報、角位置センサによって取得された情報、加速度センサによって取得された情報、ＧＰＳセンサによって取得された情報等を用いて位置に関する推定や信頼度の取得を行うものであってもよい。移動体１の周囲の情報を取得するセンサによって取得された情報は、例えば、測距センサによって取得された情報、画像センサによって取得された情報等であってもよい。以下、第１の位置推定部２２によって、第１のセンサ３１のセンシング結果を用いて取得された推定結果及び信頼度を、第１の推定結果及び第１の信頼度と呼び、第２の位置推定部２３によって、第２のセンサ３２のセンシング結果を用いて取得された推定結果及び信頼度を、第２の推定結果及び第２の信頼度と呼ぶことがある。
【0030】
ここで、センサによって取得された情報を用いた位置に関する推定、及び信頼度の取得について説明する。なお、移動体１の位置に関する推定は、例えば、移動体１の位置そのもの（例えば、ワールド座標系における位置など）の推定であってもよく、前回の推定結果に応じた移動体１の自己位置に加算する移動量（位置の差分）の推定であってもよい。後者の場合において、例えば、角位置センサや加速度センサ等を用いた位置推定のように、移動量を推定し、その推定した移動量を用いて移動体１の位置を推定するときには、その推定した移動量そのものが用いられてもよく、ＧＰＳセンサ等を用いた位置推定のように、移動量の推定を行わないときには、その推定した自己位置の差分を算出することによって移動量を取得してもよい。
【0031】
まず、測距センサによって取得された情報を用いた位置に関する推定について説明する。第１の位置推定部２２及び／または第２の位置推定部２３は、測距センサによって測定された距離、及び移動体１の移動領域における障害物の位置を示す環境地図を用いて、移動体１の位置に関する推定を行ってもよい。測定された距離は、厳密には、測定された各方向の距離である。環境地図は、第１の位置推定部２２及び／または第２の位置推定部２３がアクセス可能な図示しない記録媒体において記憶されているものとする。その障害物は、測距センサによる距離の測定対象となる壁や設備等の物体であってもよい。推定位置は、通常、ワールド座標系における位置である。そのような測距結果を用いた自己位置推定は、測距結果を用いたＳＬＡＭ（Simultaneous Localization and Mapping）においてすでに知られているため、その詳細な説明を省略する。なお、第１の位置推定部２２及び／または第２の位置推定部２３は、環境地図の作成を行ってもよく、または行わなくてもよい。前者の場合には、第１の位置推定部２２及び／または第２の位置推定部２３は、測距結果を用いたＳＬＡＭと同様のものであってもよく、後者の場合には、第１の位置推定部２２及び／または第２の位置推定部２３は、既存の環境地図を用いて自己位置推定を行うものであってもよい。
【0032】
測距結果を用いた位置に関する推定結果の信頼度は、その推定結果を信頼できる程度、すなわち確からしさを示すものであり、例えば、測定結果と環境地図とのマッチングの程度に応じて取得されてもよく、測定結果の精度に応じて取得されてもよく、その両方であってもよく、その他の方法によって取得されてもよい。したがって、信頼度は、例えば、第１のセンサ３１及び／または第２のセンサ３２による測定結果と、移動体１が推定位置に存在するとした場合における環境地図の示す障害物までの距離とのマッチングの程度が高いほど大きい値となるものであってもよい。この場合には、信頼度が次のように算出されてもよい。
信頼度＝（ΣＦ（Ｌi））／Ｎ１
信頼度＝（ΣＦ（Ｌi））／Ｎ１
【0033】
ここで、関数Ｆ（ｘ）は、ｘ＝０のときに１となり、ｘが正の大きい値になるほど小さくなる減少関数である。但し、Ｆ（ｘ）≧０とする。また、Ｌiは、ｉ番目の測距方向における測距結果と、環境地図によって示される障害物のエッジまでの距離との差の絶対値である。また、Ｎ１はｉの総数、すなわち測距の総数であり、総和Σは、すべてのｉについて取られるものとする。Ｎ１によって規格化されているため、信頼度は、０以上、１以下の実数となる。なお、Ｆ（ｘ）は、どのような関数であってもよいが、例えば、Ｆ（ｘ）＝ｅｘｐ（－α×ｘ）であってもよい。αは正の実数である。
【0034】
また、レーザ等による測距結果は、距離が長くなるほど反射等の影響によって誤差が大きくなるため、信頼度は、例えば、測距結果が大きいほど小さい値となるものであってもよい。この場合には、信頼度が次のように算出されてもよい。
信頼度＝（ΣＧ（Ｄi））／Ｎ１
信頼度＝（ΣＧ（Ｄi））／Ｎ１
【0035】
ここで、関数Ｇ（ｘ）は、ｘ＝０のときに１となり、ｘが正の大きい値になるほど小さくなる減少関数である。但し、Ｇ（ｘ）≧０とする。また、Ｄiは、ｉ番目の測距方向における測距結果である。また、Ｎ１及び総和Σは、上記のとおりである。この場合にも、Ｎ１によって規格化されているため、信頼度は、０以上、１以下の実数となる。また、Ｇ（ｘ）も、上記関数Ｆ（ｘ）と同様のものであってもよい。また、信頼度は、例えば、測距結果と、環境地図の示す障害物の位置とのマッチングの程度が高いほど大きい値となると共に、測距結果が大きいほど小さい値となるものであってもよい。
【0036】
なお、推定位置を一意に決定できない場合にも、信頼度が低い値とされてもよい。例えば、移動体１が長い廊下のように単調な環境に存在する場合には、測距結果を用いて、推定位置を一意に決定できないこともありうる。そのような場合には、信頼度は低い値とされてもよい。
【0037】
次に、画像センサによって取得された情報を用いた位置に関する推定について説明する。第１の位置推定部２２及び／または第２の位置推定部２３は、画像センサによって取得された情報を用いて移動体１の位置に関する推定を行うことによって、推定結果と、その推定結果に対応する信頼度とを取得してもよい。その位置に関する推定において、画像センサによって取得された撮影画像において特定された特徴点、及び移動体１の移動領域における特徴点の位置を示す環境地図を用いて、位置に関する推定が行われてもよい。なお、環境地図は、第１の位置推定部２２及び／または第２の位置推定部２３がアクセス可能な図示しない記録媒体において記憶されているものとする。推定位置は、通常、ワールド座標系における位置である。そのような撮影画像を用いた自己位置推定は、ｖｉｓｕａｌ－ＳＬＡＭとしてすでに知られているため、その詳細な説明を省略する。なお、第１の位置推定部２２及び／または第２の位置推定部２３は、環境地図の作成を行ってもよく、または行わなくてもよい。前者の場合には、第１の位置推定部２２及び／または第２の位置推定部２３は、ｖｉｓｕａｌ－ＳＬＡＭと同様のものであってもよく、後者の場合には、第１の位置推定部２２及び／または第２の位置推定部２３は、既存の環境地図を用いて自己位置推定を行うものであってもよい。
【0038】
なお、撮影画像において特定される特徴点は、例えば、ＯＲＢ－ＳＬＡＭと同様の自己位置推定において用いられる所定個数のＦＡＳＴキーポイントであってもよく、ＳＩＦＴキーポイントやＳＵＲＦキーポイントなどであってもよい。また、信頼度は、推定結果の信頼できる程度、すなわち確からしさを示すものであり、例えば、撮影画像において特定された特徴点と環境地図に含まれる特徴点とのマッチングの程度に応じて取得されてもよく、撮影画像において特定された特徴点の精度に応じて取得されてもよく、その両方であってもよく、その他の方法によって取得されてもよい。したがって、信頼度は、例えば、画像センサによって取得された撮影画像において特定された特徴点の特徴記述子（特徴量）と、移動体１が推定位置に存在するとした場合における、それらの特徴点に対応する環境地図に含まれる特徴点の特徴記述子との類似度が大きいほど大きい値となるものであってもよい。その場合には、信頼度が次のように算出されてもよい。
信頼度＝（ΣＲi）／Ｎ２
信頼度＝（ΣＲi）／Ｎ２
【0039】
ここで、Ｒiは、撮影画像において特定されたｉ番目の特徴点の特徴記述子と、その特徴点に対応する環境地図に含まれる特徴点の特徴記述子との類似度であり、０から１までの値に規格化されていることが好適である。その類似度は、値が大きいほど、類似の程度が大きいものとする。また、撮影画像において特定されたｉ番目の特徴点に対応する特徴点が環境地図に含まれない場合には、類似度は０であってもよい。また、Ｎ２は、ｉの総数、すなわち撮影画像において特定された特徴点の総数であり、総和Σは、すべてのｉについて取られるものとする。Ｎ２によって規格化されているため、信頼度は、０以上、１以下の実数となる。なお、特徴記述子が二値ベクトルである場合（例えば、ＯＲＢ－ＳＬＡＭの場合）には、ハミング距離を用いて類似度を算出してもよい。特徴記述子が二値ベクトルではない場合（例えば、ＳＩＦＴキーポイントやＳＵＲＦキーポイントの場合）には、類似度をコサイン距離やユークリッド距離を用いて算出してもよい。
【0040】
また、撮影画像における特徴点は、露出がオーバーになって白とびになっている場合や、露出がアンダーになって黒つぶれになっている場合には、適切に特定できないため、信頼度は、例えば、特徴点の特定において用いられる評価値が大きいほど大きい値となるものであってもよい。なお、評価値の高い特徴点の候補が、特徴点として特定されることになるものとする。例えば、ＯＲＢ－ＳＬＡＭでは、ハリスのコーナー尺度の上位から所定個数のＦＡＳＴキーポイントが選ばれるため、その評価値は、ハリスのコーナー尺度であってもよい。すなわち、信頼度は、撮影画像において特定された複数の特徴点に関するハリスのコーナー尺度の代表値であってもよい。代表値は、例えば、平均値や中央値、最大値等であってもよい。また、例えば、ＳＩＦＴによる特徴点（キーポイント）の特定では、ローコントラストのキーポイント候補を削除するため、キーポイント候補点の極値の大きさを計算し、その大きさが閾値未満の候補点を削除する。したがって、特定された複数の特徴点に関する極値の大きさの代表値を信頼度として用いてもよい。他の特徴点についても同様である。また、信頼度は、例えば、撮影画像において特定された特徴点の特徴記述子と、それらの特徴点に対応する環境地図に含まれる特徴点の特徴記述子との類似度が大きいほど大きい値となると共に、特徴点の特定において用いられる評価値が大きいほど大きい値となるものであってもよい。
【0041】
なお、キーポイントや特徴点、特徴記述子（feature descriptor）等については、以下の文献を参照されたい。
文献１：Edward Rosten, Tom Drummond, "Machine learning for high-speed corner detection", in 9th European Conference on Computer Vision, vol. 1, 2006, pp. 430-443.
文献２：C. Harris, M. Stephens, "A Combined Corner and Edge Detector", In Proceedings of the 4th Alvey Vision Conference (1988), pp. 147-151.
文献３：Ethan Rublee, Vincent Rabaud, Kurt Konolige, Gary R. Bradski, "ORB: An efficient alternative to SIFT or SURF", ICCV 2011, pp.2564-2571.
文献４：D. Lowe, "Distinctive image features from scale-invariant keypoints", Int. Journal of Computer Vision (2004), 60(2), pp.91-110.
文献５：H. Bay, A. Ess, T. Tuytelaars, L. Van Gool, "SURF: Speeded Up Robust Features", Computer Vision and Image Understanding (CVIU) (2008), 110(3), pp.346-359.
文献６：R. Mur-Artal, J.M. Montiel, J.D. Tardos, "ORB-SLAM: A Versatile and Accurate Monocular SLAM System", IEEE Transactions on Robotics 31 (2015), pp.1147-1163.
文献１：Edward Rosten, Tom Drummond, "Machine learning for high-speed corner detection", in 9th European Conference on Computer Vision, vol. 1, 2006, pp. 430-443.
文献２：C. Harris, M. Stephens, "A Combined Corner and Edge Detector", In Proceedings of the 4th Alvey Vision Conference (1988), pp. 147-151.
文献３：Ethan Rublee, Vincent Rabaud, Kurt Konolige, Gary R. Bradski, "ORB: An efficient alternative to SIFT or SURF", ICCV 2011, pp.2564-2571.
文献４：D. Lowe, "Distinctive image features from scale-invariant keypoints", Int. Journal of Computer Vision (2004), 60(2), pp.91-110.
文献５：H. Bay, A. Ess, T. Tuytelaars, L. Van Gool, "SURF: Speeded Up Robust Features", Computer Vision and Image Understanding (CVIU) (2008), 110(3), pp.346-359.
文献６：R. Mur-Artal, J.M. Montiel, J.D. Tardos, "ORB-SLAM: A Versatile and Accurate Monocular SLAM System", IEEE Transactions on Robotics 31 (2015), pp.1147-1163.
【0042】
次に、エンコーダ等の角位置センサによって取得された情報を用いた位置に関する推定について説明する。角位置センサによって取得された回転量や角度によって、車輪等の回転量を把握することができる。したがって、第１の位置推定部２２及び／または第２の位置推定部２３は、角位置センサによって取得された情報を用いて、位置に関する推定結果を取得してもよい。ここでは、対向２輪型の移動体１の例について具体的に説明する。
【0043】
角位置センサによって取得された左右の車輪の回転量をそれぞれΔθL、ΔθRとし、車輪径をＲとすると、左右の車輪の移動量ΔｄL、ΔｄRはそれぞれ次のようになる。
ΔｄL＝ＲΔθL
ΔｄR＝ＲΔθR
ΔｄL＝ＲΔθL
ΔｄR＝ＲΔθR
【0044】
また、左右の車輪間の距離をＬとすると、移動体１の移動量Δｄと旋回量Δθは、次式のように算出できる。
Δｄ＝（ΔｄR＋ΔｄL）／２
Δθ＝（ΔｄR－ΔｄL）／Ｌ
Δｄ＝（ΔｄR＋ΔｄL）／２
Δθ＝（ΔｄR－ΔｄL）／Ｌ
【0045】
移動体１の位置を（Ｘ，Ｙ，Θ）とすると、移動体１の相対移動量（ΔＸ，ΔＹ，ΔΘ）は、次式のようになる。なお、（Ｘ，Ｙ）は、２次元ＸＹ座標系における座標値であり、Θは、２次元座標系におけるＸ軸を基準とした角度であってもよい。また、移動体１の位置（Ｘ，Ｙ，Θ）は、ワールド座標系における位置であってもよい。
（ΔＸ，ΔＹ，ΔΘ）＝（Δｄ×ｃｏｓΘ，Δｄ×ｓｉｎΘ，Δθ）
（ΔＸ，ΔＹ，ΔΘ）＝（Δｄ×ｃｏｓΘ，Δｄ×ｓｉｎΘ，Δθ）
【0046】
したがって、相対移動量（ΔＸ，ΔＹ，ΔΘ）を前回（時刻：ｔ－１）の自己位置（Ｘt-1，Ｙt-1，Θt-1）に加算することによって、現在（時刻：ｔ）の自己位置（Ｘt，Ｙt，Θt）を取得することができる。
（Ｘt，Ｙt，Θt）＝（Ｘt-1，Ｙt-1，Θt-1）＋（ΔＸ，ΔＹ，ΔΘ）
（Ｘt，Ｙt，Θt）＝（Ｘt-1，Ｙt-1，Θt-1）＋（ΔＸ，ΔＹ，ΔΘ）
【0047】
なお、上式の相対移動量は、ｔ－１からｔまでの間の移動量である。前回の自己位置は、例えば、角位置センサによって取得された情報を用いて推定されたものであってもよく、後述するように、取得部２４によって取得された自己位置であってもよい。また、ここでは、移動体１が対向２輪型である場合について具体的に説明したが、移動体１が他の形式である場合であっても、同様にして、エンコーダ等の角位置センサによって取得された情報を用いて、移動体１の位置に関する推定を行うことができることは言うまでもない。
【0048】
角位置センサによって取得された情報を用いた位置に関する推定結果の信頼度は、その推定結果を信頼できる程度である。角位置センサを用いた位置推定では、例えば、走行路面の影響によって信頼性が低下することになる。具体的には、濡れている走行路面や砂利や砂、芝生などの走行路面においては、車輪が滑りやすく、角位置センサを用いた位置推定の信頼性が低下することになる。したがって、第１の位置推定部２２及び／または第２の位置推定部２３は、車輪の滑りやすい走行路面の領域を示す地図を用いて、移動体１の現在の位置に対応する走行路面の状態（車輪が滑りやすいかどうかを示す状態）を特定し、その特定結果に応じて信頼度を取得してもよい。例えば、移動体１が、滑りやすい走行路面に存在する場合には、低い信頼性が取得され、滑りにくい走行路面に存在する場合には、高い信頼性が取得されてもよい。この場合には、例えば、走行路面の滑りやすさの程度に応じて信頼性が取得されてもよい。具体的には、移動体１が存在している走行路面が滑りやすいほど、低くなる信頼性が取得されてもよい。また、この場合の信頼性は、通常、上記相対移動量（ΔＸ，ΔＹ，ΔΘ）に関する信頼性である。また、移動体１が滑りやすい走行路面に存在するかどうかは、例えば、角位置センサを用いて推定された推定位置を用いて判断されてもよく、取得部２４によって取得された自己位置を用いて判断されてもよい。後者の場合には、例えば、前回の自己位置を用いて判断が行われてもよい。
【0049】
なお、角位置センサによって取得された情報のみを用いて、現在の自己位置が推定される場合には、現在の自己位置までの経路に滑りやすい走行路面が存在するときに、低い信頼性が取得され、その経路に滑りやすい走行路面が存在しないときに、高い信頼性が取得されてもよい。また、この場合には、例えば、経路に含まれる滑りやすい走行路面の程度に応じて信頼性が取得されてもよい。具体的には、経路に含まれる滑りやすい走行路面の程度が多いほど、低くなる信頼性が取得されてもよい。
【0050】
また、エンコーダ等の角位置センサによって取得された情報のみを用いて位置推定を行う場合には、移動距離が長くなるほど、信頼性がより低くなることが知られている。誤差が累積するからである。したがって、例えば、移動体１が、あらかじめ位置が確定している箇所に存在する際に、角位置センサを用いて推定した現在の位置を、そのあらかじめ確定している位置に合わせる場合には、その位置合わせを行った後の移動距離に応じて、信頼度を取得してもよい。その場合には、その移動距離が長くなるほど、低くなる信頼度が取得されることになる。
【0051】
次に、加速度センサによって取得された情報を用いた位置に関する推定について説明する。加速度センサは、移動体１の加速度を取得する。そして、第１の位置推定部２２及び／または第２の位置推定部２３は、その取得された加速度を２階積分することによって、移動量（移動距離）を取得することができる。そのようにして取得した移動距離を順次、加算することによって、現在の位置を推定することができる。この場合には、移動距離が長くなるほど、信頼性がより低くなる。また、この場合には、移動距離を取得することしかできないため、初期位置は入力する必要がある。したがって、例えば、初期位置を設定した位置からの移動体１の移動距離に応じて、信頼度を取得してもよい。また、初期位置以外にも、位置合わせを行う場合には、その位置合わせを行った後の移動体１の移動距離に応じて、信頼度を取得してもよい。この場合には、初期位置からの移動距離または位置合わせ後の移動距離が長くなるほど、低くなる信頼度が取得されることになる。
【0052】
また、測距結果や撮影画像、車輪等の回転量、加速度等を用いた位置に関する推定において、例えば、パーティクルフィルタやカルマンフィルタ等の状態空間モデルが用いられてもよく、ネルダーミード法（Nelder-Mead法；滑降シンプレックス法）が用いられてもよく、その他の手法が用いられてもよい。パーティクルフィルタが用いられる場合には、例えば、各パーティクルの尤度の代表値（例えば、平均値や中央値、最大値等）が、推定結果に応じた信頼度として取得されてもよい。
【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】
また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いる閾値や数式、アドレス等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、または長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、または、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、または、図示しない読み出し部が行ってもよい。
【0079】
また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いる閾値やアドレス、各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、または、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。
【0080】
また、上記実施の形態において、自己位置推定装置２に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、または、別々のデバイスを有してもよい。
【0081】
また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、または、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。また、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、または分散処理を行ってもよい。
【0082】
また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。
【産業上の利用可能性】
【0083】
以上より、本発明による自己位置推定装置によれば、精度の高い自己位置を取得できるという効果が得られ、例えば、自律移動する移動体において用いられる自己位置推定装置等として有用である。
【符号の説明】
【0084】
１ 移動体、２ 自己位置推定装置、２１ センサ部、２２ 第１の位置推定部、２３ 第２の位置推定部、２４ 取得部、３１ 第１のセンサ、３２ 第２のセンサ
【図1】
【図2】
【図3】