JP2021149229A - 移動体、及び、位置推定方法 - Google Patents

Abstract

【課題】移動体の位置推定の精度を向上させる。【解決手段】移動体１００、１００’、１００’’は、走行部２と、物体情報取得センサ４と、磁気センサ３と、回転量測定センサ２５ａ、２５ｂと、記憶部５１と、制御部５と、を備える。物体情報取得センサ４は、走行部の周囲の物体情報を取得する。磁気センサ３は、磁場ベクトルを測定する。回転量測定センサ２５ａ、２５ｂは、走行部２の車輪２１ａ、２１ｂの回転量を測定する。記憶部５１は、環境地図ＥＭを記憶する。制御部５は、移動前の第１位置情報と、車輪２１ａ、２１ｂによる移動量と、移動前後の磁場ベクトルの変化量と、移動後の局所地図と環境地図ＥＭとのマップマッチング結果と、に基づいて、第２位置情報を推定する。【選択図】図３

Description

本発明は、移動体、及び、当該移動体の位置推定方法に関する。

車輪の回転により所定の移動環境を自律的に移動する移動体が知られている。この移動体は、一般的には、周囲に存在する障害物と移動体との間の距離に関する情報（障害物情報と呼ぶ）を取得するセンサと、移動前後の車輪の回転量を測定するセンサとを備える。これらセンサを備える移動体は、障害物情報から生成される地図と移動環境を表す環境地図とのマップマッチング結果と、移動前後の車輪の回転量から算出される移動量とに基づいて、現在位置及び移動体の姿勢を推定する（例えば、特許文献１を参照）。

特開２０１０−２２４７５５号公報

上記センサから取得した情報を用いた位置推定では、走行面と車輪との「すべり」、センサから取得した情報に含まれるノイズや測定誤差などにより、実際の位置と推定された位置との間に誤差が生じる。特に、走行面と車輪とのすべりにより、センサにより測定された車輪の回転量から算出された移動量及び姿勢変化と、実際の移動量及び姿勢変化が対応しないことが多い。

走行面と車輪とのすべりに影響されずに姿勢変化を測定できるセンサとして、ジャイロセンサ（慣性センサ）、磁気センサが考えられる。これらのセンサを用いた場合でも、移動体の姿勢変化の測定値に大きな誤差が生じる場合がある。
ジャイロセンサは、周囲の温度によって測定値がドリフトする。磁気センサは、周囲に存在する磁性体等の影響によって、実際の地磁気の向きからずれた方向を磁界の向きとして検出する。この結果、上記のセンサを用いた場合であっても、移動体の姿勢の測定結果に誤差が生じる。

本発明の目的は、位置推定を行う移動体において、移動体周囲の状態及びセンサによる測定誤差の影響を最小限に抑えて位置推定の精度を向上させることにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地に係る移動体は、所定の移動環境を自律的に移動する。この移動体は、走行部と、物体情報取得センサと、磁気センサと、回転量測定センサと、記憶部と、制御部と、を備える。
走行部は、車輪を有する。
物体情報取得センサは、走行部の周囲に存在する物体に関する物体情報を取得する。
磁気センサは、走行部における磁場の方向を表す磁場ベクトルを測定する。
回転量測定センサは、車輪の回転量を測定する。
記憶部は、上記の所定の移動環境を表す環境地図を記憶する。
制御部は、走行部が所定の移動環境の第１位置から第２位置に移動したときに、第１位置情報と、車輪による移動量と、第１位置から第２位置に移動後の磁場ベクトルの変化量と、マップマッチング結果と、に基づいて、第２位置情報を推定する。
第１位置情報は、第１位置、及び、第１位置における走行部の姿勢を表す情報である。
第２位置情報は、第２位置、及び、第２位置における走行部の姿勢を表す情報である。
車輪による移動量は、第１位置から第２位置に移動後の車輪の回転量に基づいて算出された移動量である。
マップマッチング結果は、第２位置で取得した物体情報から生成された局所地図と環境地図とをマップマッチングした結果である。

上記の移動体では、移動環境における位置推定に、車輪の回転量から算出される移動体の移動量と、第２位置における局所地図と環境地図とのマップマッチング結果に加えて、第１位置から第２位置に移動後の磁場ベクトルの変化量が用いられている。位置推定に用いる情報を増加することにより、位置推定の精度を向上できる。

また、位置推定に用いる磁場ベクトルの情報として、第１位置における磁場ベクトル、第２位置における磁場ベクトルとの絶対値的な測定結果ではなく、第１位置から第２位置に移動後の磁場ベクトルの変化量といった相対値的な測定結果が用いられている。
移動環境に存在する磁性体等が地磁気に与える影響は、第１位置と第２位置とが近い場合には、移動前の第１位置と移動後の第２位置との間ではほぼ同じであるので、第１位置から第２位置へ移動後の磁場ベクトルの変化量を位置推定に用いることで、移動体周囲の状態及びセンサによる測定誤差の位置推定に対する影響を最小限に抑えることができる。

制御部は、第１分布算出部と、第２分布算出部と、第３分布算出部と、第１位置推定部と、を有してもよい。
第１分布算出部は、第１位置情報と車輪による移動量とに基づいて、第１分布を算出する。第１分布は、第１位置から車輪による移動により走行部が到達しうる位置及び姿勢の確率分布を表す。
第２分布算出部は、第１位置情報と磁場ベクトルの変化量とに基づいて、第２分布を算出する。第２分布は、第１位置からの移動後にとりうる走行部の姿勢の確率分布を表す。
第３分布算出部は、マップマッチング結果に基づいて、第３分布を算出する。第３分布は、第１位置から移動後に到達しうる走行部の位置及び姿勢の確率分布を表す。
第１位置推定部は、第１分布、第２分布、及び第３分布を重ね合わせて最終分布を算出し、当該最終分布において最大の確率を示す走行部の位置及び姿勢を第２位置情報として推定する。
これにより、車輪の回転量から算出される移動体の移動量と、第１位置から第２位置に移動後の磁場ベクトルの変化量と、第２位置における局所地図と環境地図とのマップマッチング結果と、を考慮した最終分布を用いて精度よく第２位置情報を推定できる。

前記制御部は、第４分布算出部と、第５分布算出部と、第６分布算出部と、第２位置推定部と、を有してもよい。
第４分布算出部は、第１位置情報と車輪による移動量とに基づいて、第４分布を算出する。第４分布は、第１位置から車輪による移動により走行部が到達しうる位置及び姿勢の確率分布を表す。
第５分布算出部は、磁場ベクトルの変化量と第４分布とに基づいて、第５分布を算出する。
第６分布算出部は、マップマッチング結果と第５分布とに基づいて、第６分布を算出する。
第２位置推定部は、第６分布において最大の確率を示す走行部の位置及び姿勢を第２位置情報として推定する。
これにより、車輪の回転量から算出される移動体の移動量に基づく第４分布を、第１位置から第２位置に移動後の磁場ベクトルの変化量及び第２位置における局所地図と環境地図とのマップマッチング結果を用いて修正した第６分布を得られ、当該第６分布を用いて精度よく第２位置情報を推定できる。

制御部は、磁場ベクトルの変化量が所定の閾値より大きければ、第２位置情報の推定における磁場ベクトルの変化量の影響割合を小さくしてもよい。
これにより、第１位置と第２位置との間で磁場ベクトルの状態が大きく異なり、磁場ベクトルの変化量から算出される移動体の姿勢（変化）に誤差が生じる可能性がある場合に、位置推定における磁場ベクトルの変化量の寄与を減らして、精度よく第２位置情報を推定できる。

制御部は、さらに、第１位置にて取得した物体情報から生成された第１局所地図と第２位置にて取得した物体情報から生成された第２局所地図との間のずれ量を加味して、第２位置情報を推定してもよい。すなわち、制御部は、車輪の回転量から算出される移動体の移動量と、第２位置の周囲に存在する物体に関する物体情報と環境地図とのマップマッチング結果と、第１位置から第２位置に移動後の磁場ベクトルの変化量に加えて、さらに、第１位置における第１局所地図と第２位置における第２局所地図とのずれ量を加味して第２位置情報を推定してもよい。これにより、位置推定の精度を向上できる。

本発明の他の見地に係る位置推定方法は、車輪を有する走行部を備える移動体が、所定の移動環境を第１位置から第２位置に自律的に移動したときに、第２位置情報を推定する方法である。第２位置情報は、第２位置及び第２位置における走行部の姿勢を表す。
位置推定方法は、以下のステップを備える。
◎第１位置から第２位置に移動後の磁場の方向を表す磁場ベクトルの変化量を測定するステップ。
◎第１位置から第２位置に移動後の車輪の回転量を測定するステップ。
◎第２位置において前記走行部の周囲に存在する物体に関する物体情報を取得するステップ。
◎第１位置情報と、車輪による移動量と、第１位置から第２位置に移動後の磁場ベクトルの変化量と、マップマッチングの結果と、に基づいて第２位置情報を推定するステップ。第１位置情報は、第１位置、及び、第１位置における走行部の姿勢を表す情報である。車輪による移動量は、第１位置から第２位置に移動後の車輪の回転量に基づいて算出された移動量である。マップマッチング結果は、第２位置で取得した物体情報から生成された局所地図と環境地図とをマップマッチングした結果である。

上記の位置推定方法では、車輪の回転量から算出される移動体の移動量と、第２位置における局所地図と環境地図とのマップマッチング結果に加えて、第１位置から第２位置に移動後の磁場ベクトルの変化量が用いられている。位置推定に用いる情報を増加することにより、位置推定の精度を向上できる。

また、位置推定に用いる磁場ベクトルの情報として、第１位置における磁場ベクトル、第２位置における磁場ベクトルとの絶対値的な測定結果ではなく、第１位置から第２位置に移動後の磁場ベクトルの変化量といった相対値的な測定結果が用いられている。
移動環境に存在する磁性体等が地磁気に与える影響は、第１位置と第２位置とが近い場合には、移動前の第１位置と移動後の第２位置との間ではほぼ同じであるので、第１位置から第２位置へ移動後の磁場ベクトルの変化量を位置推定に用いることで、移動体周囲の状態及び測定誤差の位置推定に対する影響を最小限に抑えることができる。

第２位置情報を推定するステップは、以下のステップを含んでもよい。
◎第１位置情報と車輪による移動量とに基づいて、第１位置から車輪による移動により走行部が到達しうる位置及び姿勢の確率分布を表す第１分布を算出するステップ。
◎第１位置情報と磁場ベクトルの変化量とに基づいて、第１位置からの移動後にとりうる走行部の姿勢の確率分布を表す第２分布を算出するステップ。
◎マップマッチング結果に基づいて、第１位置からの移動後に到達しうる走行部の位置及び姿勢の確率分布を表す第３分布を算出するステップ。
◎第１分布、前記第２分布、及び前記第３分布を重ね合わせて最終分布を算出するステップ。
◎最終分布において最大の確率を示す走行部の位置及び姿勢を第２位置情報として推定するステップ。
これにより、車輪の回転量から算出される移動体の移動量と、第１位置から第２位置に移動後の磁場ベクトルの変化量と、第２位置における局所地図と環境地図とのマップマッチング結果と、を考慮した最終分布を用いて精度よく第２位置情報を推定できる。

第２位置情報を推定するステップは、以下のステップを含んでもよい。
◎第１位置情報と車輪による移動量とに基づいて、第１位置から車輪による移動により走行部が到達しうる位置及び姿勢の確率分布を表す第４分布を算出するステップ。
◎磁場ベクトルの変化量と第４分布とに基づいて、第５分布を算出するステップ。
◎マップマッチング結果と第５分布とに基づいて、第６分布を算出するステップ。
◎第６分布において最大の確率を示す走行部の位置及び姿勢を第２位置情報として推定するステップ。
これにより、第１位置から第２位置に移動後の磁場ベクトルの変化量及び第２位置における局所地図と環境地図とのマップマッチング結果を用いて車輪の回転量から算出される移動体の移動量に基づく第４分布を修正した第６分布に基づいて、精度よく第２位置情報を推定できる。

第２位置情報を推定するステップにおいて、磁場ベクトルの変化量が所定の閾値より大きければ、第２位置情報の推定における磁場ベクトルの変化量の影響割合を小さくしてもよい。
これにより、第１位置と第２位置との間で磁場ベクトルの状態が大きく異なり、磁場ベクトルの変化量から算出される移動体の姿勢（変化）に誤差が生じる可能性がある場合に、位置推定における磁場ベクトルの変化量の寄与を減らして、精度よく第２位置情報を推定できる。

第２位置情報を推定するステップは、以下のステップをさらに含んでもよい。
◎第１位置において走行部の周囲に存在する物体に関する物体情報を取得するステップ。
◎第１位置情報と、車輪による移動量と、磁場ベクトルの変化量と、第１局所地図と第２局所地図との間のずれ量と、第２位置で取得した物体情報から生成された局所地図と環境地図とのマップマッチング結果と、に基づいて第２位置情報を推定するステップ。第１局所地図は、第１位置にて取得した物体情報から生成された局所地図である。第２局所地図は、第２位置にて取得した物体情報から生成された局所地図である。
これにより、車輪の回転量から算出される移動体の移動量と、第２位置における局所地図と環境地図とのマップマッチング結果と、第１位置から第２位置に移動後の磁場ベクトルの変化量に加えて、さらに、第１位置における第１局所地図と第２位置における第２局所地図とのずれ量を加味して第２位置情報を推定して、位置推定の精度を向上できる。

位置推定を行う移動体において、移動体周囲の状態及びセンサによる測定誤差の影響を最小限に抑えて位置推定の精度を向上できる。その結果、移動環境における移動体の正確な自己位置を知ることができる。

第１実施形態の移動体の構成を示す図。 制御部の制御構成を示す図。 第１実施形態に係る走行制御部の機能ブロック構成を示す図。 移動体の移動環境における位置及び姿勢の定義の一例を示す図。 移動体の自律移動動作を示すフローチャート。 第１実施形態に係る位置推定動作を示すフローチャート。 第１分布の一例を示す図。 第２分布の一例を示す図。 第３分布の一例を示す図。 最終分布の一例を示す図。 移動環境の磁気マップの一例を示す図。 第２実施形態に係る走行制御部の機能ブロック構成を示す図。 第２実施形態に係る位置推定動作を示すフローチャート。 第３実施形態に係る走行制御部の機能ブロック構成を示す図。 第３実施形態に係る位置推定動作を示すフローチャート。

１．第１実施形態
（１）移動体の全体構成
以下、図１及び図２を用いて、本発明の第１実施形態に係る移動体１００の全体構成を説明する。図１は、第１実施形態の移動体の構成を示す図である。図２は、制御部の制御構成を示す図である。
第１実施形態に係る移動体１００は、所定の移動環境を自律的に走行する装置であり、例えば、自律走行型のロボット、当該ロボットに使用される走行装置である。図１に示すように、移動体１００は、本体１と、走行部２と、磁気センサ３と、物体情報取得センサ４（図２）と、制御部５と、備える。

本体１は、移動体１００の本体を構成する筐体であり、本体１の下部に設けられた下部フレームＬＦと、下部フレームＬＦから所定の高さの位置に設けられた中間フレームＭＦと、中間フレームＭＦから所定の高さの位置に設けられた上部フレームＵＦと、を有する。

走行部２は、移動体１００を走行駆動する。
具体的には、走行部２は、それぞれが下部フレームＬＦに並んで設けられた一対の車輪２１ａ、２１ｂと、車輪２１ａ、２１ｂのそれぞれに対応して設けられた一対のモータ２３ａ、２３ｂと、を有している。本実施形態の走行部２は、各モータ２３ａ、２３ｂが対応する車輪２１ａ、２１ｂを独立して回転させる二輪差動型の走行部である。

なお、二輪差動型の走行部２は、一対の車輪２１ａ、２１ｂが並んだ方向とは同一平面内で垂直な方向に進んだときに直進する。従って、以下の説明では、一対の車輪２１ａ、２１ｂが並んだ方向とは同一平面内で垂直な方向を、走行部２（移動体１００）の直進方向と呼ぶことにする。その一方、一対の車輪２１ａ、２１ｂが並んだ方向と平行な方向を走行部２（移動体１００）の幅方向と呼ぶことにする。

一対のモータ２３ａ、２３ｂの出力回転軸には、それぞれ、回転量測定センサ２５ａ、２５ｂ（図２）が接続されている。回転量測定センサ２５ａ、２５ｂは、それぞれ、車輪２１ａ、２１ｂの回転量を測定する。回転量測定センサ２５ａ、２５ｂは、例えば、インクリメンタル型のエンコーダである。インクリメンタル型のエンコーダである回転量測定センサ２５ａ、２５ｂは、モータ２３ａ、２３ｂの出力回転軸、すなわち、車輪２１ａ、２１ｂが回転するとパルス信号を発生する。車輪２１ａ、２１ｂの回転量は、回転量測定センサ２５ａ、２５ｂから発生したパルス数に基づいて算出できる。

二輪差動型の走行部２は、車輪２１ａ、２１ｂを同一の回転速度にて回転することにより、移動体１００（本体１）を直進させることができる。また、２つの車輪２１ａ、２１ｂの回転方向を同一としつつ回転速度を異ならせることにより、移動体１００は移動方向を変化させながら直進又は後進する。さらに、２つの車輪２１ａ、２１ｂを互いに逆方向に同一の回転速度にて回転させることにより、移動体１００をその場で回転できる。

磁気センサ３は、上部フレームＵＦに取り付けられ、移動体１００（本体１）が移動する移動環境における磁場を測定する。具体的には、磁気センサ３は、移動体１００の直進方向における磁場の強度成分I_１と、移動体１００の幅方向における磁場の強度成分Ｉ_２と、移動体１００の高さ方向における磁場の強度成分Ｉ_３と、を測定する。これら３つの強度成分を三次元ベクトルの各成分とするベクトル（（Ｉ_１，Ｉ_２，Ｉ_３）と表されるベクトル）は、ゼロ磁場（すなわち、磁場が存在しない）を原点とする磁場の方向を表すベクトルであり、以降の説明では「磁場ベクトル」と呼ぶことにする。

物体情報取得センサ４は、走行部２の周囲に存在する物体に関する情報（物体情報と呼ぶ）を取得する。具体的には、物体情報取得センサ４は、周囲にある物体から移動体１００までの距離を物体情報として二次元的に又は三次元的に取得するためのセンサであり、本体１において、移動体１００の直進方向の前後にそれぞれ設けられた前方レーザレンジセンサ及び後方レーザレンジセンサである。

レーザレンジセンサは、例えば、レーザ発振器によりパルス発振されたレーザ光を障害物などの目標物に照射し、目標物から反射した反射光をレーザ受信機により受信することにより、目標物までの二次元的な距離を物体情報として算出するレーザレンジファインダ（ＬＲＦ：Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）である。これらは、照射するレーザ光を回転ミラーを用いて所定の角度で扇状にレーザ光を走査できる。
また、物体情報取得センサ４としては、三次元的な距離を物体情報として取得するものとして、ステレオカメラやＴＯＦ（Ｔｉｍｅ Ｏｆ Ｆｌｉｇｈｔ）カメラを採用できる。

制御部５は、中間フレームＭＦに取り付けられ、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ストレージとしての記憶装置（ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）などの記憶装置により構成される）、及び各種インターフェースなどを備えるコンピュータシステムである。
制御部５は、磁気センサ３にて得られた磁気ベクトルと、物体情報取得センサ４にて得られた物体情報と、回転量測定センサ２５ａ、２５ｂにて得られた車輪２１ａ、２１ｂの回転量とに基づいて、移動体１００の移動環境における位置及び当該位置における移動体１００の向き（姿勢）を推定し、推定された位置及び姿勢に基づいて、移動体１００の移動を制御する。この機能を有する制御部５の詳細については、後ほど説明する。

移動体１００は、必要に応じて、他の構成要素を有していてもよい。図１に示すように、移動体１００は、操作部６を有してもよい。操作部６は、例えばキーボード又はタッチパネルなどの入力インターフェースであり、ユーザによる移動体１００に関する各種設定の入力を受け付ける。操作部６にて受け付けたユーザによる入力は、適切な信号変換がなされて、制御部５に出力される。

操作部６は、現在の走行状況に関する情報、その他の各種情報を表示するディスプレイを有してもよい。ディスプレイとしては、例えば、液晶ディスプレイ、有機ＥＬディスプレイなどを用いることができる。また、ディスプレイとタッチパネルは一体となっていてもよい。

（２）制御部の構成
（２−１）全体構成
以下、移動体１００を制御する制御部５の構成を説明する。まず、図２を用いて、制御部５の全体構成を説明する。本実施形態の制御部５は、回転量測定センサ２５ａ、２５ｂ、磁気センサ３、物体情報取得センサ４から得られた情報に基づいて、走行部２を制御する。この機能を実現するため、制御部５は、記憶部５１と、走行制御部５３と、モータ駆動部５５ａ、５５ｂと、を機能ブロックとして有する。
制御部５が有する上記の機能ブロックの一部又は全部は、記憶装置に記憶された所定のプログラムを実行することで実現されてもよい。上記の機能ブロックの一部又は全部を、ＳｏＣ（Ｓｙｓｔｅｍ ｏｎ Ｃｈｉｐ）などのハードウェアにより実現していてもよい。

記憶部５１は、制御部５の記憶装置の記憶領域の少なくとも一部であり、移動体１００を自律的に移動させるために用いられるデータを記憶する。具体的には、記憶部５１は、走行経路データＴＳと、環境地図ＥＭと、を記憶する。
走行経路データＴＳは、移動体１００を自律的に移動させたい移動経路を表すデータである。走行経路データＴＳは、例えば、移動経路上の通過点を表す座標値と、当該通過点における移動体１００の姿勢（姿勢角）と、当該通過点に到達する到達時間と、を関連付けて記憶する。その他、走行経路データＴＳには、各通過点で実行したい作業に関するデータ、各通過点を通過する際の速度、などが記憶されていてもよい。
環境地図ＥＭは、移動体１００の移動環境全体を表す地図情報である。環境地図ＥＭは、物体情報取得センサ４にて取得した物体情報から生成された地図情報とマップマッチングされて、当該地図情報を取得したときの移動体１００の位置推定をするために用いられる。

記憶部５１は、その他、制御部５の機能を実現するプログラム、移動体１００に関する各種設定値などの必要なパラメータ等を記憶する。

走行制御部５３は、移動環境における移動体１００（走行部２）の自律移動を実現する。具体的には、走行制御部５３は、回転量測定センサ２５ａ、２５ｂにて得られた車輪２１ａ、２１ｂの回転量に基づいて算出された車輪２１ａ、２１ｂによる移動体１００の移動量と、物体情報取得センサ４にて得られた物体情報から生成された地図情報と環境地図ＥＭとのマップマッチング結果と、磁気センサ３にて得られた磁気ベクトルと、に基づいて移動体１００の位置を推定する。
走行制御部５３は、推定された現在位置と、走行経路データＴＳに示された目標とする通過点の座標値との比較に基づいて、現在位置から目標とする通過点まで移動するために必要なモータ２３ａ、２３ｂの制御量を算出し、モータ駆動部５５ａ、５５ｂに出力する。

モータ駆動部５５ａ、５５ｂは、それぞれ、走行制御部５３から入力される制御量と、対応する回転量測定センサ２５ａ、２５ｂにより検出されるモータ２３ａ、２３ｂの回転速度とに基づいて、対応するモータ２３ａ、２３ｂをフィードバック制御する。

（２−２）走行制御部の機能ブロック構成
次に、図３を用いて、走行部２を制御する走行制御部５３の機能ブロック構成を説明する。図３は、第１実施形態に係る走行制御部の機能ブロック構成を示す図である。上記にて説明したように、走行制御部５３は、移動体１００の位置推定と、モータ２３ａ、２３ｂの制御量の算出を実行する。
この機能を実現するため、走行制御部５３は、第１分布算出部５３ａと、第２分布算出部５３ｂと、第３分布算出部５３ｃと、第１位置推定部５３ｄと、制御量算出部５３ｅと、を機能ブロックとして有する。

第１分布算出部５３ａは、前回の位置推定（制御タイミング）から今回の位置推定（制御タイミング）までの間に回転量測定センサ２５ａから出力されたパルス数から車輪２１ａ、２１ｂの回転量を算出し、前回の位置推定時における推定位置及び姿勢と、算出された車輪２１ａ、２１ｂの回転量とに基づいて、当該車輪２１ａ、２１ｂの回転により移動体１００が到達しうる位置及び姿勢を推定する。

本実施形態において、第１分布算出部５３ａは、車輪２１ａ、２１ｂと移動環境の走行面との間の滑りを考慮して、車輪２１ａ、２１ｂの回転による移動により移動体１００が到達しうる複数の位置の候補と各位置に到達する確率とを関連付けた確率分布を算出する。また、車輪２１ａ、２１ｂの回転により移動体１００がとりうる複数の姿勢の候補と各姿勢を取りうる確率とを関連付けた確率分布を算出する。
上記の複数の位置の候補と各位置に到達する確率とを関連付けた確率分布と、上記の複数の姿勢の候補と各姿勢を取りうる確率とを関連付けた確率分布と、を合わせて「第１分布」と呼ぶ。

第２分布算出部５３ｂは、前回の位置推定時に磁気センサ３にて測定された磁気ベクトルと今回の位置推定時に磁気センサ３にて測定された磁気ベクトルとの間の変化量（磁気ベクトルの変化量）を算出し、前回の位置推定時における推定姿勢と、算出された磁場ベクトルの変化量と、に基づいて、今回の位置推定時における移動体１００の姿勢を推定する。
本実施形態において、第２分布算出部５３ｂは、磁気センサ３による磁気ベクトルの測定誤差を考慮して、磁気ベクトルの変化量から推定される複数の姿勢の候補と各姿勢を取りうる確率とを関連付けた確率分布（第２分布と呼ぶ）を算出する。

第３分布算出部５３ｃは、今回の位置推定時に物体情報取得センサ４にて取得された物体情報から地図情報を生成し、当該地図情報と環境地図ＥＭとのマップマッチング結果に基づいて、第３分布を算出する。
なお、物体情報は移動体１００の周囲の物体に関する情報であるので、物体情報から生成される地図情報は、移動体１００の周囲の物体の配置を表す情報である。従って、物体情報から生成される地図情報を、「局所地図」と呼ぶ。

第１位置推定部５３ｄは、第１分布算出部５３ａにて算出された第１分布、第２分布算出部５３ｂにて算出された第２分布、及び、第３分布算出部５３ｃにて算出された第３分布を重ね合わせて最終分布を算出する。その後、第１位置推定部５３ｄは、算出された最終分布において最大の確率を示す位置の候補及び姿勢の候補を、今回の位置推定時における移動体１００の位置及び姿勢と推定する。
なお、本実施形態において、第１分布、第２分布、及び第３分布を「重ね合わせる」とは、例えば、第１分布と第２分布の和を算出し、第１分布と第２分布の和と第３分布の積とを算出することをいう。その他、第１分布、第２分布、及び第３分布の積を算出することを「重ね合わせる」と定義することもできる。第１分布、第２分布、及び第３分布を具体的にどのように「重ね合わせる」かは、必要に応じて適宜決定できる。

制御量算出部５３ｅは、第１位置推定部５３ｄにて推定された移動体１００の位置及び姿勢と、走行経路データＴＳに示された次の目標点（通過点）の位置及び当該目標点における姿勢（目的の姿勢）との比較に基づいて、移動体１００を次の目標点に到達させ、当該目標点において移動体１００を目的の姿勢とさせるために必要なモータ２３ａ、２３ｂへの制御量を算出し、対応するモータ駆動部５５ａ、５５ｂに出力する。

（３）移動体の自律移動動作
（３−１）定義
以下、上記にて説明した構成を有する移動体１００の自律移動動作を説明する。自律移動動作を具体的に説明する前に、図４を用いて、移動体１００の移動環境における位置及び姿勢の定義を説明する。図４は、移動体の移動環境における位置及び姿勢の定義の一例を示す図である。
本実施形態においては、移動環境は、図４に示すように二次元の座標系（Ｘ−Ｙ座標系）にて表すものとする。移動体１００の位置は、移動環境における本体１（走行部２）の中心位置ＣをＸ−Ｙ座標系の座標値として表したものとする。また、本実施形態においては、移動環境を表すＸ−Ｙ座標系において、例えば、正のＸ方向を北向き、正のＹ方向を東向きと定義する。

なお、必要に応じて、移動環境を三次元の座標系（Ｘ−Ｙ−Ｚ座標系）で表してもよいし、移動環境を定義する座標系においていずれの軸方向（Ｘ軸方向、Ｙ軸方向、Ｚ軸方向）を北向きとするかについては任意に定義できる。

一方、移動体１００（走行部２）の姿勢は、図４に示すように、移動環境において移動体１００の前進方向が向いている方向を正のＸ軸方向を基準（０°）とする角度（θ）として表すものとする。以降の説明では、移動体１００の姿勢を表す角度（θ）を「姿勢角θ」と呼ぶことにする。姿勢角θは、正のＸ軸方向から時計回りを正方向（角度が増加する方向）とする。

以下の説明においては、移動体１００の位置と姿勢とを三次元の座標値（Ｘ，Ｙ，θ）としてまとめて標記する場合がある。移動体１００の位置と姿勢とをまとめて標記した座標値を、移動体の位置及び姿勢を表す「位置情報」と呼ぶことにする。

（３−２）自律移動動作
次に、図５を用いて、移動体１００の自律移動動作を説明する。図５は、移動体の自律移動動作を示すフローチャートである。
移動体１００を、走行経路データＴＳに示された最初の通過点（移動開始点）まで移動させた後、例えば操作部６によりユーザから自律移動を開始するように指示がなされると、走行制御部５３が、ステップＳ１において、記憶部５１に記憶された走行経路データＴＳを参照し、自律移動の目標の通過点を決定する。目標の通過点を決定後、当該目標の通過点まで自律移動するための制御が開始される。

まず、走行制御部５３が、ステップＳ２において、現在が走行部２の制御タイミングであるか否かを判定する。この制御タイミングは、あらかじめ決められた所定の周期（制御周期）毎に発生する。すなわち、走行制御部５３は、所定の周期毎に走行部２を制御する。
現在が制御タイミングでない場合（ステップＳ２で「Ｎｏ」）、走行制御部５３は制御タイミングとなるまで待機する。
一方、制御タイミングであると判定された場合（ステップＳ２で「Ｙｅｓ」）、走行制御部５３は、ステップＳ３において、移動体１００の移動環境における現在位置を推定する。

本実施形態において、走行制御部５３は、前回の位置推定時（すなわち、前回の制御タイミング）における位置（第１位置と呼ぶ）と当該第１位置における姿勢角を表す位置情報（第１位置情報と呼ぶ）と、前回の制御タイミングから今回の制御タイミングまでの間（第１位置から現在位置（第２位置と呼ぶ）まで移動後）の車輪２１ａ、２１ｂの回転量に基づいて算出された車輪２１ａ、２１ｂによる移動量と、前回の制御タイミングから今回の制御タイミングまでの間の磁場ベクトルの変化量と、現在位置で取得した物体情報から生成された局所地図と環境地図ＥＭとのマップマッチング結果と、に基づいて、移動体１００の現在位置と現在位置における姿勢を表す位置情報（第２位置情報と呼ぶ）を推定する。具体的には、現在位置のＸ−Ｙ座標系における座標値と、現在位置における姿勢角θと、を第２位置情報として推定する。
走行制御部５３における具体的な位置推定動作は、後ほど詳しく説明する。

第２位置情報を推定後、走行制御部５３は、ステップＳ４において、ステップＳ３で推定された第２位置情報と、ステップＳ１で定めた目標の通過点を表す位置情報（目標の通過点の座標値と、当該通過点における姿勢角θ）とを比較して、移動体１００が目標の通過点に到達したか否かを判定する。

推定された第２位置情報と目標の通過点を表す位置情報とが一致しないか、又は、推定された第２位置情報が目標の通過点から所定の範囲より外側に存在しており、移動体１００が目標の通過点に到達していないと判定された場合（ステップＳ４で「Ｎｏ」）、走行制御部５３は、ステップＳ５において、移動体１００を目標の通過点に移動させるための制御を実行する。
具体的には、走行制御部５３の制御量算出部５３ｅが、走行経路データＴＳにおいて定められた到達時間に移動体１００が目標の通過点に到達するために必要な各モータ２３ａ、２３ｂの回転速度を算出し、当該回転速度に対応したモータ２３ａ、２３ｂの制御量を算出する。その後、制御量算出部５３ｅが、算出した制御量を対応するモータ２３ａ、２３ｂに出力する。

ステップＳ５の走行制御が実行された後、自律移動動作プロセスは、ステップＳ２に戻る。すなわち、走行制御部５３は、次の制御タイミングにおいて、ステップＳ３の位置推定と必要に応じてステップＳ５の走行制御を実行する。
このように、本実施形態では、走行制御部５３は、移動体１００の位置推定と走行制御とを、所定の制御周期毎に実行する。

一方、ステップＳ４において、推定された第２位置情報と目標の通過点を表す位置情報とが一致するか、又は、推定された第２位置情報が目標の通過点から所定の範囲内に存在しており、移動体１００が目標の通過点に到達していると判定された場合（ステップＳ４で「Ｙｅｓ」）、走行制御部５３は、ステップＳ６において、到達した通過点が走行経路データＴＳに示された移動終了位置であるか否かを判定する。
到達した通過点が移動終了位置であると判定された場合（ステップＳ６で「Ｙｅｓ」）、自律移動動作プロセスは終了する。
一方、到達した通過点が移動終了位置ではないと判定された場合（ステップＳ６で「Ｎｏ」）、自律移動動作プロセスは、ステップＳ１に戻る。すなわち、走行制御部５３は、次の目標の通過点を決定し、当該次の通過点に到達するための走行制御を実行する。

走行制御部５３が上記のステップＳ１〜Ｓ６を実行することにより、移動体１００（走行部２）は、走行経路データＴＳに示された移動開始位置から移動終了位置まで自律的に移動できる。

（３−３）位置推定動作
以下、図６を用いて、図５の自律移動動作を示すフローチャートのステップＳ３で実行される位置推定動作を説明する。図６は、第１実施形態に係る位置推定動作を示すフローチャートである。
上記のように、本実施形態において、位置推定動作は所定の制御周期毎に実行される。従って、本実施形態の位置推定動作は、前回の制御タイミングにおける移動体１００の位置及び姿勢を基準とし、前回の制御タイミングから今回の制御タイミングまでの移動体１００の移動量を上記基準の位置及び姿勢に加えることで、今回の制御タイミングにおける移動体１００の位置及び姿勢を推定する。

以下の説明では、前回の制御タイミングにおける移動体１００の位置を第１位置と呼ぶ。また、第１位置と第１位置における移動体１００の姿勢（姿勢角θ）とを表す情報を第１位置情報と呼び、便宜上、（Ｘ１，Ｙ１，θ１）との座標値で表されるとする。
また、今回の制御タイミングにおける移動体１００の位置を第２位置と呼ぶ。第２位置と第２位置における移動体１００の姿勢（姿勢角θ）とを表す情報を第２位置情報と呼び、便宜上、（Ｘ２，Ｙ２，θ２）との座標値で表されるものとする。

また、本実施形態における位置推定動作は、車輪２１ａ、２１ｂの滑り、各センサの測定誤差、及び推定誤差などを考慮して、車輪２１ａ、２１ｂの回転により移動体１００が移動したと推測される位置及び姿勢の確率分布（第１分布）と、磁気ベクトルの変化量から推測される移動体１００の姿勢の確率分布（第２分布）と、移動体１００の周囲の物体の状態を表す局所地図と移動環境全体を表す環境地図ＥＭとのマップマッチングから推測される移動体１００の位置及び姿勢の確率分布（第３分布）と、を重ね合わせた確率分布（最終分布）を用いて実行される。

最初に、第１分布算出部５３ａが、車輪２１ａ、２１ｂの回転による移動体１００の移動量に基づいて、第１分布を算出する。
まず、第１分布算出部５３ａは、ステップＳ３１において、前回の制御タイミングから今回の制御タイミングまでに移動体１００が車輪２１ａ、２１ｂの回転により移動した移動量を算出する。

具体的には、第１分布算出部５３ａは、前回の制御タイミングから今回の制御タイミングまでの間に回転量測定センサ２５ａ、２５ｂから発生したパルス数から、前回の制御タイミングから今回の制御タイミングまでの車輪２１ａ、２１ｂの回転量を算出する。
次に、第１分布算出部５３ａは、上記のようにして算出した車輪２１ａ、２１ｂの回転量から、前回の制御タイミングから今回の制御タイミングまでに移動体１００が車輪２１ａ、２１ｂの回転により移動した移動量を算出する。移動量が、（ΔＸ１，ΔＹ１，Δθ１）との座標値として算出されたとする。

車輪２１ａ、２１ｂの回転による移動量を算出後、第１分布算出部５３ａは、ステップＳ３２において、第１分布を算出する。
具体的には、第１分布算出部５３ａは、まず、ステップＳ３１にて算出した車輪２１ａ、２１ｂによる移動量（ΔＸ１，ΔＹ１，Δθ１）を、前回の制御タイミングにて推定した第１位置情報の推定値（Ｘ１，Ｙ１，θ１）に加算して、今回の制御タイミングにおいて車輪２１ａ、２１ｂの回転により到達したと推定される移動体１００の位置及び姿勢角θの推定値（Ｘ１＋ΔＸ１，Ｙ１＋ΔＹ１，θ１＋Δθ１）と算出する。
ここで、上記車輪２１ａ、２１ｂの回転による位置及び姿勢角θの推定値を、Ｘ２１＝Ｘ１＋ΔＸ１、Ｙ２１＝Ｙ１＋ΔＹ１、θ２１＝θ１＋Δθ１と定義し、（Ｘ２１，Ｙ２１，θ２１）と新たに表すものとする。

次に、第１分布算出部５３ａは、車輪２１ａ、２１ｂの滑りを考慮して、例えば、図７に示すような、上記にて算出した位置及び姿勢角θの推定値（Ｘ２１，Ｙ２１，θ２１）を平均とし、第１標準偏差σ１を標準偏差とする正規分布を、第１分布として算出する。このようにして算出された第１分布は、第１位置から車輪２１ａ、２１ｂによる移動により移動体１００が到達しうる位置及び姿勢の確率分布を表す。
図７は、第１分布の一例を示す図である。

第１分布の第１標準偏差σ１は、車輪２１ａ、２１ｂの滑りを表すパラメータであるので、車輪２１ａ、２１ｂの滑りの度合いに応じてその大きさを適宜決定できる。具体的には、車輪２１ａ、２１ｂの滑りが大きければ第１標準偏差σ１を大きくし、滑りが小さければ第１標準偏差σ１を小さくできる。

次に、第２分布算出部５３ｂが、前回の制御タイミングにて測定された磁気ベクトルから今回の制御タイミングにて測定された磁気ベクトルへの変化量に基づいて、第２分布を算出する。
まず、第２分布算出部５３ｂは、ステップＳ３３において、今回の制御タイミングにおける磁気ベクトルの測定値を磁気センサ３から取得し、今回取得した磁気ベクトルと前回の制御タイミングにて取得した磁気ベクトルとの差分を、磁気ベクトルの変化量として算出する。この磁気ベクトルの変化量は、前回の制御タイミングにおける移動体１００の姿勢から今回の制御タイミングにおける移動体１００の姿勢への姿勢変化量に対応するものである。従って、ここでは、この磁気ベクトルの変化量を、姿勢角θの変化量としてΔθ２と表す。

次に、第２分布算出部５３ｂは、ステップＳ３４において、ステップＳ３３にて算出した磁気ベクトルの変化量（Δθ２）に基づいて、第２分布を算出する。
具体的には、第２分布算出部５３ｂは、まず、前回の制御タイミングにおける移動体１００の姿勢を表すθ１に磁気ベクトルの変化量Δθ２を加算して、今回の制御タイミングにおける姿勢角θの推定値をθ１＋Δθ２として算出する。ここで、θ１＋Δθ２＝θ２２とする。
次に、第２分布算出部５３ｂは、磁気センサ３の測定誤差等を考慮して、例えば、図８に示すような、上記にて算出した姿勢角θの推定値θ２２を平均とし、第２標準偏差σ２を標準偏差とする正規分布を、第２分布として算出する。このようにして算出された第２分布は、第１位置から第２位置に移動後にとりうる移動体１００の姿勢（姿勢角θ）の確率分布を表す。
図８は、第２分布の一例を示す図である。

第２分布を算出後、さらに、第３分布算出部５３ｃが、今回の制御タイミング、すなわち、移動環境の第２位置で取得した局所地図と、記憶部５１に記憶されている環境地図ＥＭとのマップマッチング結果に基づいて、第３分布を算出する。
具体的には、まず、第３分布算出部５３ｃは、ステップＳ３５において、今回の制御タイミングにおいて物体情報取得センサ４にて取得した物体情報を用いて、第２位置に存在する移動体１００の周囲に存在する物体の状態を表す局所地図を生成する。より具体的には、第３分布算出部５３ｃは、例えば、物体が存在する方向（角度）と当該物体までの距離とが関連付けられた物体情報を、Ｘ−Ｙ座標系の座標値に座標変換することで局所地図を生成する。

次に、第３分布算出部５３ｃは、ステップＳ３６において、ステップＳ３５にて生成した第２位置における局所地図と環境地図ＥＭとのマップマッチングを実行して、第３分布を算出する。
具体的には、第３分布算出部５３ｃは、環境地図ＥＭ上に、第２位置（（Ｘ２，Ｙ２））に対応すると推定される複数の候補位置と、第２位置における姿勢角θ２に対応すると推定される複数の候補姿勢角と、を定める。その後、第３分布算出部５３ｃは、１つの候補位置に局所地図を配置し、当該候補位置で特定の候補姿勢角だけ局所地図を回転させて、この候補位置及び候補角度における局所地図と環境地図ＥＭとの一致度を算出する。この局所地図と環境地図ＥＭとの一致度の算出を、全ての候補位置及び候補姿勢角に対して実行し、各候補位置及び各候補姿勢角と対応する一致度とを関連付けて、図９に示すような、局所地図と環境地図ＥＭとの一致度の分布（確率分布）としての第３分布を算出する。このようにして算出された第３分布は、第１位置から第２位置に移動後に到達しうる移動体１００の位置及び姿勢の確率分布を表す。
図９は、第３分布の一例を示す図である。

なお、上記の複数の候補位置及び複数の候補姿勢角は、例えば、前回の制御タイミングから今回の制御タイミングまでの車輪２１ａ、２１ｂの回転量に基づいて算出できる。

第１分布、第２分布、及び第３分布を算出後、第１位置推定部５３ｄは、ステップＳ３７において、第１分布、第２分布、第３分布を重ね合わせて最終分布を算出する。具体的には、第１位置推定部５３ｄは、第１分布と第２分布との和を算出し、当該和と第３分布との積を算出することで、図１０に示すような最終分布を算出する。図１０は、最終分布の一例を示す図である。
なお、第２分布は姿勢角θの確率分布のみを表すものであるので、第１分布、第２分布、第３分布の全てが重ね合わされた最終分布は、姿勢角θに対してのみ算出される。移動体１００の位置についての最終分布は、第１分布と第３分布とを重ね合わせて生成される。

図１０に示すように、ステップＳ３７にて算出される最終分布においては、第１分布のピーク位置（確率の最大値を示す姿勢角）、第２分布のピーク位置、第３分布のピーク位置とは異なる位置（姿勢角θ）において確率の最大値が見られる。これは、上記３つの分布を重ね合わせて算出された最終分布が、より多くの情報量を用いて、車輪２１ａ、２１ｂの滑り及び各センサの測定誤差の影響を抑制できていることを示している。すなわち、最終分布は、１つのデータのみに基づいて算出された分布（第１分布、第２分布、第３分布）と比較して、今回の制御タイミングにおける移動体１００の位置及び姿勢の確率分布を最も正確に表している。

最終分布を算出後、第１位置推定部５３ｄは、ステップＳ３８において、ステップＳ３７において算出された最終分布を用いて、今回の制御タイミングにおける移動体の位置及び姿勢、すなわち、第２位置情報（Ｘ２，Ｙ２，θ２）を推定する。
具体的には、第１位置推定部５３ｄは、最終分布において確率が最大となる位置（座標値）及び姿勢（姿勢角θ）を、第２位置情報（Ｘ２，Ｙ２，θ２）と推定する。

（４）第１実施形態のまとめ
このように、第１実施形態に係る移動体１００では、移動環境における位置推定に、車輪２１ａ、２１ｂの回転量から算出される移動体１００の移動量と、第２位置における局所地図と環境地図ＥＭとのマップマッチング結果に加えて、第１位置から第２位置に移動後の磁場ベクトルの変化量が用いられている。位置推定に用いる情報を増加することにより、位置推定の精度を向上できる。

また、位置推定に用いる磁場ベクトルの情報として、第１位置から第２位置に移動後、すなわち、制御の１周期が経過後の磁気ベクトルの変化量を用いることで、移動体１００周囲の状態及び各センサによる測定誤差の位置推定に対する影響を最小限に抑えることができる。

以下、移動環境における地磁気に対する磁気ベクトルのずれ量を表す地図情報（磁気マップと呼ばれる）を用いて、短時間（短距離の移動前後）の磁気ベクトルの変化量を用いることで移動体の周囲の状態に影響を受けにくい位置推定が可能となる理由を具体的に説明する。図１１は、移動環境の磁気マップの一例を示す図である。図１１では、ハッチングの密度により、移動環境による地磁気への影響の大小を表している。
図１１に示すように、移動環境に存在する磁性体等が地磁気に与える影響は、距離が離れた２点間においては大きく異なる可能性がある。例えば、磁性体が存在する位置から大きく離れた位置から、磁性体が近くに存在する位置（図１１では、破線で囲んだ領域内の位置）まで長い距離移動した場合、移動後に測定した磁気ベクトルは、磁性体の影響を受ける。その結果、例えば、磁性体から離れた位置から磁性体が近くに存在する位置まで、姿勢を変更することなく直進した場合であっても、移動後に測定される磁気ベクトルは、移動前に測定された磁気ベクトルとは異なる。

その一方で、磁性体から離れた２点間で短距離直進した場合、又は、磁性体が近くに存在する２点間で短距離直進した場合には、磁性体の影響を受けない領域から受ける領域へと長距離直進する場合と比較して、直進した前後の磁気ベクトルの向きはほとんど変化しない。すなわち、短距離だけ移動した前後の磁気ベクトルは、磁性体の影響を受けにくい。
従って、短時間（短距離移動前後）の磁気ベクトルの変化量との相対的な値を用いることで、移動体１００の各位置において測定される磁気ベクトルといった絶対的な値を用いる場合と比較して、移動環境に存在する磁性体の影響をほとんど受けることなく、位置推定を実行できる。

なお、磁気ベクトルの変化量を用いて移動体１００の各位置における姿勢角θを算出する場合には、各位置における姿勢角θは、移動開始位置から当該位置までの磁気ベクトルの変化量の累積値（総和）として算出される。

（５）変形例
上記の実施形態において最終分布を算出する際に、第１分布、第２分布、第３分布は、算出されたそのままの状態で重ね合わされていた。しかし、これに限られず、第１位置推定部５３ｄは、最終分布を算出する際に、第１分布、第２分布、第３分布のそれぞれに重み付けを付して重ね合わせてもよい。
具体的には、第１分布をＤ１、第１分布の重み付けをａ、第２分布をＤ２、第２分布の重み付けをｂ、第３分布をＤ３、第３分布の重み付けをｃとした場合に、第１位置推定部５３ｄは、（ａ＊Ｄ１＋ｂ＊Ｄ２）＊ｃ＊Ｄ３との式から最終分布を算出してもよい。
なお、重み付けａ、ｂ、ｃは、ともに正の数である。また、各分布の重み付けの値の大小は、例えば、各センサによる測定値の信頼性等に基づいて決定できる。

例えば、磁気センサ３にて測定された磁気ベクトルの変化量が所定の閾値より大きくなった場合に、第２分布の重み付けｂの値を、他の重み付けａ、ｃの値よりも小さくして、第２位置情報の推定における磁気ベクトルの変化量の影響割合を小さくしてもよい。
これにより、前回の制御タイミングと今回の制御タイミングとの間で磁場ベクトルの状態が大きく異なり、磁場ベクトルの変化量から算出される移動体１００の姿勢に誤差が生じる可能性がある場合に、位置推定における磁場ベクトルの変化量の寄与を減らして、精度よく第２位置情報を推定できる。

２．第２実施形態
（１）第２実施形態の概略
第２実施形態に係る移動体１００’においては、移動体１００’の位置推定に用いる確率分布（第１実施形態では最終分布）を算出する際に、車輪２１ａ、２１ｂの回転による移動量に基づいた分布（第４分布と呼ぶ）を、他のセンサ情報を用いて修正し、当該修正した分布を用いて移動体１００’の位置推定を実行する。
第２実施形態に係る移動体１００’においては、走行制御部５３’の構成が第１実施形態と異なること、及び、位置推定動作のフローが第１実施形態と異なること以外は、第１実施形態の移動体１００と同じ構成及び機能を有する。従って、以下では、走行制御部５３’の構成と、位置推定動作のフローのみを説明し、他の構成及び機能の説明は省略する。

（２）第２実施形態に係る走行制御部の機能ブロック構成
以下、図１２を用いて、第２実施形態に係る走行制御部５３’の機能ブロック構成を説明する。図１２は、第２実施形態に係る走行制御部の機能ブロック構成を示す図である。
走行制御部５３’は、第４分布算出部５３ａ’と、第５分布算出部５３ｂ’と、第６分布算出部５３ｃ’と、第２位置推定部５３ｄ’と、制御量算出部５３ｅ’と、を有する。
第４分布算出部５３ａ’は、第１位置情報（Ｘ１，Ｙ１，θ１）と車輪２１ａ、２１ｂによる移動量とに基づいて、第１位置から車輪２１ａ、２１ｂによる移動により移動体１００が到達しうる位置及び姿勢の確率分布を表す第４分布を算出する。

第５分布算出部５３ｂ’は、磁場ベクトルの変化量と、第４分布算出部５３ａ’にて算出された第４分布とに基づいて、第５分布を算出する。
第６分布算出部５３ｃ’は、第２位置の局所地図と環境地図ＥＭとのマップマッチング結果と、第５分布算出部５３ｂ’にて算出された第５分布とに基づいて、第６分布を算出する。

第２位置推定部５３ｄ’は、第６分布算出部５３ｃ’にて算出した第６分布を用いて、第２位置情報（Ｘ２，Ｙ２，θ２）を推定する。
制御量算出部５３ｅ’は、第１実施形態に係る制御量算出部５３ｅと同様の機能を有するので、ここでは説明を省略する。

（３）第２実施形態に係る位置推定動作
次に、図１３を用いて、第２実施形態に係る位置推定動作を説明する。図１３は、第２実施形態に係る位置推定動作を示すフローチャートである。
位置推定動作が開始されると、第４分布算出部５３ａ’が、ステップＳ３１’において、前回の制御タイミングから今回の制御タイミングまでの間の車輪２１ａ、２１ｂの回転量から、車輪２１ａ、２１ｂの回転による移動体１００’の移動量を算出する。車輪２１ａ、２１ｂによる移動体１００’の移動量の算出に関する詳細は、第１実施形態で説明したのと同様であるので、説明を省略する。
その後、第４分布算出部５３ａ’が、ステップＳ３２’において、ステップＳ３１’にて算出した移動体の移動量に基づいて、第４分布を算出する。第４分布の算出方法は、第１実施形態において説明した第１分布の算出方法と同じである。従って、第４分布の算出方法の説明は省略する。

第４分布を算出後、第５分布算出部５３ｂ’が、ステップＳ３３’において、第１実施形態において説明したのと同様にして、前回の制御タイミングから今回の制御タイミングまでの間の磁気ベクトルの変化量を算出する。
その後、第５分布算出部５３ｂ’は、ステップＳ３４’において、ステップＳ３３’にて算出した磁気ベクトルの変化量を用いて第４分布を変更して、第５分布を算出する。具体的には、第５分布算出部５３ｂ’は、第１実施形態において説明したようにして、磁気ベクトルの変化量から第１実施形態の第２分布に対応する分布を算出し、この分布を第４分布に重ね合わせて第４分布を変更して、第５分布を算出する。例えば、第５分布は、磁気ベクトルの変化量に基づく分布と第４分布との和として算出できる。

第５分布を算出後、第６分布算出部５３ｃ’が、ステップＳ３５’において、第２位置の局所地図を生成する。局所地図の生成方法については、第１実施形態で説明したので、ここでは説明を省略する。
局所地図を生成後、第６分布算出部５３ｃ’が、ステップＳ３６’において、ステップＳ３５’にて算出した局所地図と環境地図ＥＭとのマップマッチング結果に基づいて、第５分布算出部５３ｂ’にて算出された第５分布を変更して、第６分布を算出する。
具体的には、第６分布算出部５３ｃ’は、第１実施形態において説明したようにして、第２位置の局所地図と環境地図ＥＭとのマップマッチング結果に基づいて第３分布に対応する分布を算出し、この分布と第５分布とを重ね合わせて第５分布を変更して、第６分布を算出する。例えば、第６分布を、マップマッチングの結果に基づく分布と第５分布との積として算出できる。

第６分布を算出後、第２位置推定部５３ｄ’が、ステップＳ３７’において、ステップＳ３６’にて算出した第６分布を用いて、第２位置情報（Ｘ２，Ｙ２，θ２）を推定する。具体的には、第２位置推定部５３ｄ’は、第６分布において最大の確率を示す位置及び姿勢を、第２位置情報（Ｘ２，Ｙ２，θ２）として推定する。

上記のステップＳ３１’〜Ｓ３７’を実行することによっても、第１実施形態と同様に、移動体１００の位置及び姿勢、すなわち、第２位置情報（Ｘ２，Ｙ２，θ２）を精度よく推定できる。なぜなら、車輪２１ａ、２１ｂの回転量から算出される移動体１００の移動量に基づく第４分布を、磁場ベクトルの変化量及びマップマッチング結果を用いて変更することで算出される第６分布は、第１実施形態の最終分布と同じだからである。

３．第３実施形態
（１）第３実施形態の概略
第３実施形態に係る移動体１００’’においては、車輪２１ａ、２１ｂの回転による移動量、磁気ベクトルの変化量、第２位置の局所地図と環境地図ＥＭとのマップマッチング結果に加えて、他の情報に基づいて移動体１００の位置及び姿勢を推定する。具体的には、第３実施形態に係る移動体１００’’は、前回の制御タイミングにおいて生成した局所地図（第１位置の局所地図）と第２位置の局所地図との間のずれ量をさらに加味して、第２位置情報を推定する。
第３実施形態に係る移動体１００’’においては、走行制御部５３’’の構成と位置推定動作のフローが、第１実施形態及び第２実施形態と異なる以外は、第１実施形態及び第２実施形態の移動体１００、１００’と同じ構成及び機能を有する。従って、以下では、走行制御部５３’’の構成と、位置推定動作のフローのみを説明し、他の構成及び機能の説明は省略する。

（２）第３実施形態に係る走行制御部の機能ブロック構成
以下、図１４を用いて、第３実施形態に係る走行制御部５３’’の機能ブロック構成を説明する。図１４は、第３実施形態に係る走行制御部の機能ブロック構成を示す図である。
走行制御部５３’’は、第７分布算出部５３ａ’’と、第８分布算出部５３ｂ’’と、第９分布算出部５３ｃ’’と、第１０分布算出部５３ｄ’’と、第３位置推定部５３ｅ’’と、制御量算出部５３ｆ’’と、を有する。
上記の第７分布算出部５３ａ’’と、第８分布算出部５３ｂ’’と、制御量算出部５３ｆ’’は、それぞれ、第２実施形態の第４分布算出部５３ａ’、第５分布算出部５３ｂ’と、制御量算出部５３ｅ’と同様の機能を有するので、ここでは説明を省略する。

第９分布算出部５３ｃ’’は、車輪２１ａ、２１ｂの回転による移動量に基づいて算出された第７分布を磁場ベクトルの変化量に基づいて変更して算出した第８分布を、第１位置にて生成した局所地図（第１局所地図と呼ぶ）と第２位置にて生成した局所地図（第２局所地図と呼ぶ）とのずれ量に基づいて変更して、第９分布を算出する。
上記の第１局所地図は、前回の制御タイミングにおいて取得した物体情報から生成された局所地図（第２局所地図）である。従って、本実施形態では、今回の制御タイミングにおいて生成された第２局所地図は、例えば、記憶部５１などに少なくとも一時的に記憶される。

第１０分布算出部５３ｄ’’は、第２位置の局所地図と環境地図ＥＭとのマップマッチング結果と、第９分布算出部５３ｃ’’にて算出された第９分布とに基づいて、第１０分布を算出する。
第３位置推定部５３ｅ’’は、第１０分布算出部５３ｄ’’にて算出した第１０分布を用いて、第２位置情報（Ｘ２，Ｙ２，θ２）を推定する。

（３）第３実施形態に係る位置推定動作
次に、図１５を用いて、第３実施形態に係る位置推定動作を説明する。図１５は、第３実施形態に係る位置推定動作を示すフローチャートである。
第７分布算出部５３ａ’’により第７分布が算出されるプロセス（ステップＳ３１’’〜Ｓ３２’’）、及び、第８分布算出部５３ｂ’’により第８分布が算出されるプロセス（ステップＳ３３’’〜Ｓ３４’’）は、それぞれ、第２実施形態の第４分布算出部５３ａ’により第４分布が算出されるプロセス（ステップＳ３１’〜Ｓ３２’）、及び、第５分布算出部５３ｂ’により第５分布が算出されるプロセス（ステップＳ３３’〜Ｓ３４’）と同じである。従って、ここでは説明を省略する。

車輪２１ａ、２１ｂの回転による移動量に基づいて第７分布を算出し、磁場ベクトルの変化量に基づいて第７分布を変更して第８分布を算出後、第９分布算出部５３ｃ’’が、ステップＳ３５’’において、第２位置の局所地図（第２局所地図）を生成し、記憶部５１に記憶する。局所地図の生成方法については、第１実施形態で説明したので、ここでは説明を省略する。

第２局所地図を生成後、第９分布算出部５３ｃ’’は、ステップＳ３６’’において、第１局所地図と第２局所地図とのずれ量に基づいて第８分布を変更して、第９分布を算出する。
具体的には、第９分布算出部５３ｃ’’は、例えば、Ｘ−Ｙ座標上において、第１局所地図を平行移動及び／又は回転させて移動させ、移動後の第１局所地図と今回生成した第２局所地図とが最も一致したときの第１局所地図の平行移動量及び回転量を、第１局所地図と第２局所地図との間のずれ量として算出する。上記の第１局所地図と第２局所地図との間のずれ量の算出は、例えば、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法、ヒストグラムマッチング法、又はＩＣＰ法とヒストグラムマッチング法とを組み合わせた方法により実行できる。

第９分布算出部５３ｃ’’は、第１局所地図と第２局所地図との間のずれ量（局所地図の移動量と回転量）を第１位置情報に加算して、第２位置情報を推定する。さらに、第９分布算出部５３ｃ’’は、第８分布のピーク位置を上記の第２位置情報の推定値と一致させて、第９分布を算出する。

第９分布の算出方法の変形例として、第９分布算出部５３ｃ’’は、例えば、第１局所地図を、複数の平行移動量だけ移動し、及び／又は、複数の回転量だけさせて回転させて、それぞれの平行移動及び回転後の第１局所地図と第２局所地図との一致度を算出する。
その後、第９分布算出部５３ｃ’’は、各平行移動及び／又は回転後の第１局所地図の位置と、対応する各位置における第１局所地図と第２局所地図との一致度と、を関連付けて分布を算出し、当該分布と第８分布とを重ね合わせて第９分布を算出することもできる。

なお、上記のようにして第９分布を算出する場合には、第１局所地図の代わりに第２局所地図を平行移動及び／又は回転させてもよい。

第９分布を算出後、第１０分布算出部５３ｄ’’が、ステップＳ３７’’において、第２位置の局所地図（第２局所地図）と環境地図ＥＭとのマップマッチング結果に基づいて、第９分布算出部５３ｃ’’にて算出された第９分布を変更して、第１０分布を算出する。
ステップＳ３７’’における第１０分布算出部５３ｄ’’による第１０分布の算出プロセスは、第２実施形態のステップＳ３６’における第６分布算出部５３ｃ’による第６分布の算出プロセスと同じであるので、ここでは説明を省略する。

第１０分布を算出後、第３位置推定部５３ｅ’’が、ステップＳ３８’’において、ステップＳ３７’’にて算出した第１０分布を用いて、第２位置情報（Ｘ２，Ｙ２，θ２）を推定する。具体的には、第３位置推定部５３ｅ’’は、第１０分布において最大の確率を示す位置及び姿勢を、第２位置情報（Ｘ２，Ｙ２，θ２）として推定する。

上記のステップＳ３１’’〜Ｓ３８’’を実行して、車輪２１ａ、２１ｂの回転量から算出される移動体１００の移動量と、第２位置の周囲に存在する物体に関する物体情報と環境地図ＥＭとのマップマッチング結果と、第１位置から第２位置に移動後の磁場ベクトルの変化量に加えて、さらに、第１位置における第１局所地図と第２位置における第２局所地図とのずれ量を加味して第２位置情報を推定することにより、より多くの情報量を用いて位置推定の精度を向上できる。

４．他の実施形態
以上、本発明の複数の実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
（Ａ）図５、図６、図１３、及び図１５に示されたフローチャートにおける各ステップの実行順序、及び／又は、各ステップの処理内容は、発明の要旨を逸脱しない範囲で変更可能である。

（Ｂ）第３実施形態で説明した、第１局所地図と第２局所地図との間のずれ量に基づく分布の算出方法は、第１実施形態に対しても適用できる。
上記局所地図のずれ量による分布の算出方法を第１実施形態に適用する場合には、例えば、第１局所地図の各平行移動及び／又は回転後の位置と、対応する各位置における第１局所地図と第２局所地図との一致度と、を関連付けて分布を算出し、この分布を第１分布〜第３分布と重ね合わせて、最終分布を生成できる。

本発明は、移動体の位置推定に広く適用できる。

１００、１００’、１００’’移動体
１ 本体
ＬＦ 下部フレーム
ＭＦ 中間フレーム
ＵＦ 上部フレーム
２ 走行部
２１ａ、２１ｂ車輪
２３ａ、２３ｂモータ
２５ａ、２５ｂ回転量測定センサ
３ 磁気センサ
４ 物体情報取得センサ
５ 制御部
６ 操作部
５１ 記憶部
ＥＭ 環境地図
ＴＳ 走行経路データ
５３、５３’、５３’’ 走行制御部
５３ａ 第１分布算出部
５３ｂ 第２分布算出部
５３ｃ 第３分布算出部
５３ａ’ 第４分布算出部
５３ｂ’ 第５分布算出部
５３ｃ’ 第６分布算出部
５３ａ’’ 第７分布算出部
５３ｂ’’ 第８分布算出部
５３ｃ’’ 第９分布算出部
５３ｄ’’ 第１０分布算出部
５３ｄ 第１位置推定部
５３ｄ’ 第２位置推定部
５３ｅ’’ 第３位置推定部
５３ｅ、５３ｅ’、５３ｆ’’制御量算出部
５５ａ、５５ｂモータ駆動部

Claims (10)

1. 所定の移動環境を自律的に移動する移動体であって、
   車輪を有する走行部と、
   前記走行部の周囲に存在する物体に関する物体情報を取得する物体情報取得センサと、
   前記走行部における磁場の方向を表す磁場ベクトルを測定する磁気センサと、
   前記車輪の回転量を測定する回転量測定センサと、
   前記所定の移動環境を表す環境地図を記憶する記憶部と、
   前記走行部が前記所定の移動環境の第１位置から第２位置に移動したときに、前記第１位置及び前記第１位置における前記走行部の姿勢を表す第１位置情報と、前記第１位置から前記第２位置に移動後の前記車輪の回転量に基づいて算出された車輪による移動量と、前記第１位置から前記第２位置に移動後の磁場ベクトルの変化量と、前記第２位置で取得した物体情報から生成された局所地図と前記環境地図とのマップマッチング結果と、に基づいて、前記第２位置及び前記第２位置における前記走行部の姿勢を表す第２位置情報を推定する制御部と、
   を備える移動体。
2. 前記制御部は、
   前記第１位置情報と前記車輪による移動量とに基づいて、前記第１位置から前記車輪による移動により前記走行部が到達しうる位置及び姿勢の確率分布を表す第１分布を算出する第１分布算出部と、
   前記第１位置情報と前記磁場ベクトルの変化量とに基づいて、前記第１位置からの移動後にとりうる前記走行部の姿勢の確率分布を表す第２分布を算出する第２分布算出部と、
   前記マップマッチング結果に基づいて、前記第１位置からの移動後に到達しうる前記走行部の位置及び姿勢の確率分布を表す第３分布を算出する第３分布算出部と、
   前記第１分布、前記第２分布、及び前記第３分布を重ね合わせて最終分布を算出し、当該最終分布において最大の確率を示す前記走行部の位置及び姿勢を前記第２位置情報として推定する第１位置推定部と、
   を有する、請求項１に記載の移動体。
3. 前記制御部は、
   前記第１位置情報と前記車輪による移動量とに基づいて、前記第１位置から前記車輪による移動により前記走行部が到達しうる位置及び姿勢の確率分布を表す第４分布を算出する第４分布算出部と、
   前記磁場ベクトルの変化量と前記第４分布とに基づいて、第５分布を算出する第５分布算出部と、
   前記マップマッチング結果と前記第５分布とに基づいて、第６分布を算出する第６分布算出部と、
   前記第６分布において最大の確率を示す前記走行部の位置及び姿勢を前記第２位置情報として推定する第２位置推定部と、
   を有する、請求項１に記載の移動体。
4. 前記制御部は、前記磁場ベクトルの変化量が所定の閾値より大きければ、前記第２位置情報の推定における前記磁場ベクトルの変化量の影響割合を小さくする、請求項１〜３のいずれかに記載の移動体。
5. 前記制御部は、さらに、前記第１位置にて取得された前記物体情報から生成された第１局所地図と前記第２位置にて取得された前記物体情報から生成された第２局所地図との間のずれ量を加味して、前記第２位置情報を推定する、請求項１〜４のいずれかに記載の移動体。
6. 車輪を有する走行部を備える移動体が所定の移動環境を第１位置から第２位置に自律的に移動したときに、前記第２位置及び前記第２位置における前記走行部の姿勢を表す第２位置情報を推定する位置推定方法であって、
   前記第１位置から前記第２位置に移動後の磁場の方向を表す磁場ベクトルの変化量を測定するステップと、
   前記第１位置から前記第２位置に移動後の前記車輪の回転量を測定するステップと、
   前記第２位置において前記走行部の周囲に存在する物体に関する物体情報を取得するステップと、
   前記第１位置及び前記第１位置における前記走行部の姿勢を表す第１位置情報と、前記車輪の回転量に基づいて算出された車輪による移動量と、前記磁場ベクトルの変化量と、前記第２位置で取得した物体情報から生成された局所地図と前記所定の移動環境を表す環境地図とのマップマッチング結果と、に基づいて前記第２位置情報を推定するステップと、
   を備える位置推定方法。
7. 前記第２位置情報を推定するステップは、
   前記第１位置情報と前記車輪による移動量とに基づいて、前記第１位置から前記車輪による移動により前記走行部が到達しうる位置及び姿勢の確率分布を表す第１分布を算出するステップと、
   前記第１位置情報と前記磁場ベクトルの変化量とに基づいて、前記第１位置からの移動後にとりうる前記走行部の姿勢の確率分布を表す第２分布を算出するステップと、
   前記マップマッチング結果に基づいて、前記第１位置からの移動後に到達しうる前記走行部の位置及び姿勢の確率分布を表す第３分布を算出するステップと、
   前記第１分布、前記第２分布、及び前記第３分布を重ね合わせて最終分布を算出するステップと、
   前記最終分布において最大の確率を示す前記走行部の位置及び姿勢を前記第２位置情報として推定するステップと、
   を含む、請求項６に記載の位置推定方法。
8. 前記第２位置情報を推定するステップは、
   前記第１位置情報と前記車輪による移動量とに基づいて、前記第１位置から前記車輪による移動により前記走行部が到達しうる位置及び姿勢の確率分布を表す第４分布を算出するステップと、
   前記磁場ベクトルの変化量と前記第４分布とに基づいて、第５分布を算出するステップと、
   前記マップマッチング結果と前記第５分布とに基づいて、第６分布を算出するステップと、
   前記第６分布において最大の確率を示す前記走行部の位置及び姿勢を前記第２位置情報として推定するステップと、
   を含む、請求項６に記載の位置推定方法。
9. 前記第２位置情報を推定するステップにおいて、前記磁場ベクトルの変化量が所定の閾値より大きければ、前記第２位置情報の推定における前記磁場ベクトルの変化量の影響割合を小さくする、請求項６〜８のいずれかに記載の位置推定方法。
10. 前記第２位置情報を推定するステップは、
    前記第１位置において前記走行部の周囲に存在する物体に関する物体情報を取得するステップと、
    前記第１位置情報と、前記車輪による移動量と、前記磁場ベクトルの変化量と、前記第１位置にて取得した前記物体情報から生成された第１局所地図と前記第２位置にて取得した前記物体情報から生成された第２局所地図との間のずれ量と、前記第２位置で取得した前記物体情報から生成された局所地図と前記環境地図とのマップマッチング結果と、に基づいて前記第２位置情報を推定するステップと、
    を含む、請求項６〜９のいずれかに記載の位置推定方法。
    

Images