JP2016103269A - 移動量推定装置、自律移動体、及び移動量の推定方法 - Google Patents

Abstract

【課題】センサにより取得したデータを用いた移動量の算出において、精度よく高速に移動量を算出する。【解決手段】移動量推定装置５３は、位置データ取得部３と、第１移動量算出部５３１２と、位置データ移動部５３１３と、第２移動量算出部５３１４と、第３移動量算出部５３１５と、を備える。位置データ取得部３は位置データを取得する。第１移動量算出部５３１２は第１ヒストグラムと第２ヒストグラムとが最も一致したときの第２ヒストグラムの移動量を第１移動量として算出する。位置データ移動部５３１３は複数の第２位置データを第１移動量だけ移動して複数の第２移動位置データを算出する。第２移動量算出部５３１４は第１投影物体像と第２移動投影物体像とが一致したときの第１移動位置データの移動量を第２移動量と算出する。第３移動量算出部５３１５は第１移動量と第２移動量とを積算して移動体１００の移動量を算出する。【選択図】図８

Description

本発明は、自身の位置を推定しながら移動可能な移動体の移動量を推定する移動量推定装置、当該移動量の推定方法、及び上記の移動量推定装置を備える自律移動体に関する。

従来、自身の位置を推定しながら自律移動可能な自律移動体が知られている。例えば、特許文献１には、自律移動体の自己の周辺に存在する障害物をセンシングするセンサを用いてセンシングされるデータを使い、自身の位置を推定する自己位置認識システムを備えた自律移動体が開示されている。

特許文献１に記載の自律移動体においては、データ値の出現頻度を表す２つのヒストグラムの比較結果に基づいて、自律移動体の回転方向の回転量や並進方向の移動量を補正して、自己位置（自身の位置）の算出結果を補正している。

特許第５０３２９５３号公報

上記の特許文献１においては、自律移動体の回転方向の回転量や並進方向の移動量を、２つのヒストグラムの一致度に基づいて補正する際に、２つのヒストグラムの相互相関関数により２つのヒストグラムの一致度を算出している。相互相関関数などを用いて２つのヒストグラムの一致度を算出する方法（ヒストグラムマッチング法）においては、回転方向や並進方向の「おおよそ」の回転量や移動量を算出できるが、精度のよい回転量や移動量の算出には向いていない。

本発明の課題は、周囲にある物体をセンシングするセンサにより取得したデータを用いた移動量の算出において、精度よく高速に移動量を算出することにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地に係る移動量推定装置は、移動体の移動量を推定する装置である。移動量推定装置は、位置データ取得部と、第１移動量算出部と、位置データ移動部と、第２移動量算出部と、第３移動量算出部と、を備える。
位置データ取得部は、複数の位置データを取得する。複数の位置データは、所定の座標上に投影物体像を形成する。投影物体像は、移動体の周囲に存在する物体を所定の座標上に投影した像である。

第１移動量算出部は、第１ヒストグラムと第２ヒストグラムとが最も一致したときの第２ヒストグラムの移動量を、第１移動量として算出する。第１ヒストグラムは、複数の第１位置データの所定の座標軸における座標値から生成されるヒストグラムである。複数の第１位置データは、移動体の移動前又は移動後の一方において取得した位置データである。第２ヒストグラムは、複数の第２位置データの所定の座標軸における座標値から生成されるヒストグラムである。複数の第２位置データは、移動前又は移動後の他方において取得した位置データである。

なお、上記の「移動前又は移動後の他方」は、複数の第１位置データが移動体の移動前に取得された場合には、移動体の移動後のことを指す。一方、複数の第１位置データが移動体の移動後に取得された場合には、移動体の移動前のことを指す。

位置データ移動部は、複数の第２位置データを第１移動量に対応する移動量だけ移動して複数の第１移動位置データを算出する。

第２移動量算出部は、第１投影物体像と第２移動投影物体像とが一致すると判断したときの複数の第２移動位置データを算出したときの複数の第１移動位置データの移動量を、第２移動量として算出する。第１投影物体像と第２移動投影物体像とが一致するかどうかは、第１投影物体像と第２移動投影物体像との間の距離と所定の閾値との比較に基づいて判断される。
第１投影物体像は、複数の第１位置データにより形成される投影物体像である。第２移動投影物体像は、複数の第２移動位置データにより形成される投影物体像である。複数の第２移動位置データは、複数の第１移動位置データを平行移動及び／又は回転移動して算出される。

第３移動量算出部は、第１移動量と第２移動量とを積算して、移動体の移動前後の移動量を算出する。

上記の移動量推定装置においては、まず、第１移動量算出部が、複数の第１位置データの座標値から生成される第１ヒストグラムと、複数の第２位置データの座標値から生成される第２ヒストグラムとを用いて第１移動量を算出する。具体的には、第１ヒストグラムと第２ヒストグラムとが最も一致したときの第２ヒストグラムの移動量を第１移動量として算出する。つまり、第１ヒストグラムと第２ヒストグラムとが最も一致したときの、第２ヒストグラムの移動量を第１移動量とする。

次に、位置データ移動部が、複数の第２位置データを上記の第１移動量に対応する移動量だけ移動して、複数の第１移動位置データを算出する。

その後、第２移動量算出部が、上記の複数の第１移動位置データをさらに平行移動及び／又は回転移動して算出した複数の第２移動位置データにより形成される第２移動投影物体像と第１投影物体像との間の距離と所定の閾値との比較に基づいて、第１投影物体像と第２移動投影物体像とが一致するかどうかを判断する。

第１投影物体像と第２移動投影物体像とが一致すると判断した場合、第２移動量算出部は、第１投影物体像と第２移動投影物体像とが一致すると判断したときの複数の第２移動位置データを算出したときの複数の第１移動位置データの移動量を、第２移動量として算出する。

第２移動量を算出後、第３移動量算出部が、上記の第１移動量と第２移動量とを積算し、積算された移動量を移動体の移動前後の移動量として算出する。

上記の移動量推定装置においては、（ｉ）第１位置データと第２位置データとを用いて第１移動量算出部が第１移動量を算出する、（ｉｉ）第２位置データを第１移動量だけ移動して第１移動位置データ算出する、（ｉｉｉ）第１移動位置データと第１位置データを用いて第２移動量算出部が第２移動量を算出する、（ｉｖ）第１移動量と第２移動量とを積算し最終的な移動体の移動量を算出する、という段階的な計算により移動体の移動量を算出している。

上記の第２移動量算出部は、２種類の位置データ（第１移動位置データと第１位置データ）が互いに近い位置にあるときには高速にかつ精度よく第２移動量を算出できるという特徴を有している。また、第１移動量算出部は、少ない計算量にて第１移動量を算出できるという特徴を有している。

従って、上記の移動量推定装置において、第１移動量を算出後に第２移動量を算出することにより、上記の２つの移動量算出部の特徴を利用して、最終的な移動体の移動量を高速にかつ精度よく算出できる。

第１移動量算出部は、姿勢角度差を第１移動量として算出してもよい。姿勢角度差は、移動体の移動前の姿勢角と移動後の姿勢角との差である。これにより、上記の第２移動量算出部における回転運動の移動量の算出の短所を第１移動量算出部において吸収できる。その結果、より高速にかつ精度よく移動体の移動量を算出できる。

第２移動量算出部は、複数の第２移動位置データの複数の第１移動位置データからの平行移動の移動量を第２移動量として算出してもよい。これにより、第２移動量算出部では平行移動の移動量のみを算出することで、より高速に移動体の移動量を算出できる。

第２移動量算出部は、複数の第２移動位置データの複数の第１移動位置データからの平行移動の移動量に加えて回転移動の移動量を第２移動量として算出してもよい。これにより、平行移動の移動量及び回転移動の移動量を精度よく算出できるという第２移動量算出部の特徴を生かして、第２移動量算出部が平行移動の移動量のみを算出する場合と比較して、より精度よく移動体の移動量を算出できる。

第１移動量算出部は、（１）複数の第１位置データから生成されヒストグラムデータ間隔が第１ヒストグラムデータ間隔である第３ヒストグラムを第１ヒストグラムとし、複数の第２位置データから生成されヒストグラムデータ間隔が第２ヒストグラムデータ間隔である第４ヒストグラムを第２ヒストグラムとして用いて、第１移動量の候補である候補移動量を算出し、（２）その後、複数の第１位置データから生成され第１ヒストグラムデータ間隔よりも小さな第３ヒストグラムデータ間隔を有する第５ヒストグラムを第１ヒストグラムとし、複数の第２位置データから生成した第２ヒストグラムデータ間隔よりも小さな第４ヒストグラムデータ間隔を有するヒストグラムを候補移動量だけシフトすることにより生成された第６ヒストグラムを第２ヒストグラムとして用いて補正移動量を算出し、（３）（２）において算出された補正移動量を第１移動量としてもよい。

上記の第１移動量算出部は、最初にヒストグラムデータ間隔の大きなヒストグラムを用いて第１移動量の「候補」である候補移動量を算出している。これにより、第１移動量の算出の初期段階における計算量を減少できる。
上記の候補移動量を算出後に、第１移動量算出部は、候補移動量を算出するために用いたヒストグラムのヒストグラムデータ間隔よりも小さなヒストグラムデータ間隔を有する２つのヒストグラムのヒストグラムマッチングを実行して、候補移動量を補正して補正移動量を算出している。また、第１移動量算出部は、補正移動量を算出するためのヒストグラムマッチングの前に、当該ヒストグラムマッチングに用いる２つのヒストグラムのうちの１つを、上記にて算出した候補移動量だけシフトしている。これにより、ヒストグラムデータ間隔を小さくしてより精度よく補正移動量を算出しつつ、ヒストグラムマッチングのための計算量を少なくできる。

候補移動量を算出後に候補移動量を補正した補正移動量を第１移動量として算出する上記の第１移動量の算出においては、候補移動量をより少ない計算量で算出できるとの効果と、補正移動量の算出においてヒストグラムマッチングのための計算量を減少できるとの効果とが相乗効果となり、初期段階からヒストグラムデータ間隔の小さい２つのヒストグラムを用いて第１移動量を１度のヒストグラムマッチングにて算出する場合と比較して、より短時間に、かつ、従来と同等の精度にて第１移動量を算出できる。

補正移動量は、第５ヒストグラムと第６ヒストグラムとが最も一致したときの第６ヒストグラムのシフト量である補正量を候補移動量に加えることにより算出されてもよい。これにより、候補移動量を補正量にて補正して、実際の第１移動量により近い補正移動量を短時間に算出できる。

第１移動量算出部は、補正量の算出を複数回実行してもよい。このとき、補正移動量は、候補移動量に当該複数の補正量を加算することにより算出される。これにより、実際の第１移動量により近い補正移動量を短時間に算出できる。

補正量の算出を複数回実行する場合、第１移動量算出部は、今回の補正量の算出における第３ヒストグラムデータ間隔及び第４ヒストグラムデータ間隔を、それぞれ、前回の補正量の算出における第３ヒストグラムデータ間隔及び第４ヒストグラムデータ間隔よりも小さくしてもよい。これにより、より正確な補正移動量を段階的に算出できる。

本発明の他の見地に係る自律移動体は、上記の移動量推定装置と、走行部と、位置推定部と、走行制御部と、を備える。走行部は、自律移動体を移動させる。位置推定部は、上記の移動量推定装置において推定される移動量に基づいて現在位置を推定する。走行制御部は、自律移動体の現在位置から所定の目標位置まで走行するよう走行部を制御する。

上記の自律移動体においては、上記の移動量推定装置により、自律移動体の移動量が精度よくかつ高速に算出されている。これにより、位置推定部は、精度よくかつ高速に自律移動体の現在位置を推定できる。また、走行制御部は、現在位置から予め決められた目標位置に確実に到達可能なように、走行部を制御遅れなく制御できる。その結果、自律移動体は、意図した走行経路を正確に、かつ、制御遅れを発生することなく移動できる。

本発明のさらに他の見地に係る移動量の推定方法は、移動体の移動量の推定方法である。移動量の推定方法は、以下のステップを含む。
◎移動体の移動前及び移動後に、移動体の周囲に存在する物体を所定の座標上に投影した投影物体像を形成する複数の位置データを取得するステップ。
◎複数の第１位置データの所定の座標軸における座標値から生成される第１ヒストグラムと、複数の第２位置データの所定の座標軸における座標値から生成される第２ヒストグラムとが最も一致したときの、第２ヒストグラムの移動量を第１移動量として算出するステップ。
◎複数の第２位置データを第１移動量に対応する移動量だけ移動して複数の第１移動位置データを算出するステップ。
◎第１投影物体像と複数の第１移動位置データを平行移動及び／又は回転移動して算出される複数の第２移動位置データにより形成される第２移動投影物体像との間の距離と所定の閾値との比較に基づいて第１投影物体像と第２移動投影物体像とが一致するかどうかを判断し、第１投影物体像と第２移動投影物体像とが一致すると判断したときの複数の第２移動位置データを算出したときの複数の第１移動位置データの移動量を、第２移動量として算出するステップ。
◎第１移動量と第２移動量とを積算して、移動体の移動前後の移動量を算出するステップ。

上記の移動量の推定方法においては、少ない計算量にて算出可能な第１移動量を算出後に第２移動量を算出することにより、最終的な移動体の移動量を高速にかつ精度よく算出できる。

第１移動量を算出ステップは、以下のステップを含んでいてもよい。
◎複数の第１位置データから生成されヒストグラムデータ間隔が第１ヒストグラムデータ間隔である第３ヒストグラムを第１ヒストグラムとし、複数の第２位置データから生成されヒストグラムデータ間隔が第２ヒストグラムデータ間隔である第４ヒストグラムを第２ヒストグラムとして用いて、第１移動量の候補である候補移動量を算出するステップ。
◎複数の第１位置データから生成され第１ヒストグラムデータ間隔よりも小さな第３ヒストグラムデータ間隔を有する第５ヒストグラムを第１ヒストグラムとし、複数の第２位置データから生成した第２ヒストグラムデータ間隔よりも小さな第４ヒストグラムデータ間隔を有するヒストグラムを候補移動量だけシフトすることにより生成された第６ヒストグラムを第２ヒストグラムとして用いて算出される補正移動量を、第１移動量とするステップ。
これにより、第１移動量の算出精度を維持しつつ、より短時間に第１移動量を算出できる。

周囲にある物体をセンシングするセンサにより取得したデータを用いた移動量の算出を行う移動量推定装置において、精度よく高速に移動量を算出できる。

自律移動体の全体構成を示す図。 制御部の構成を示す図。 移動量算出部（移動量算出部）の構成を示す図。 第１移動量の算出方法を示すフローチャート。 第１移動量の算出方法を模式的に示す図。 第１移動量の算出方法を位置データの移動として模式的に示す図。 第２移動量の算出方法を示すフローチャート。 第２移動量の算出を模式的に示す図。 自律移動体の動作を示すフローチャート。 第２実施形態における第１移動量の算出方法を示すフローチャート。 複数のヒストグラムデータの集合体の一例を示す図。 第２実施形態における補正移動量の算出方法を示すフローチャート。

１．第１実施形態
（１）自律移動体の全体構成
以下、本発明の自律移動体について説明する。まず、第１実施形態に係る自律移動体１００の全体構成について、図１を用いて説明する。図１は、自律移動体の全体構成を示す図である。本実施形態の自律移動体１００は、予め決められた所望の予定走行経路を自律的に走行する移動体である。
自律移動体１００は、本体部１と、走行部２と、位置データ取得部３と、制御部５と、を主に備える。本体部１は、自律移動体１００の本体を構成する。

走行部２は、図１に示すように、車輪２１ａ、２１ｂを有し、車輪２１ａ、２１ｂが移動領域の床面などの走行面に接するように、モータ２３ａ、２３ｂ（後述）を介して、本体部１の底部の中央付近に設置されている。また、車輪２１ａ、２１ｂは、本体部１の左右に設置されたモータ２３ａ、２３ｂの出力回転軸のそれぞれに、当該出力回転軸周りに回転可能に取り付けられている。

モータ２３ａ、２３ｂは、制御部５から駆動信号（後述）を受信し、当該制御部５からの駆動信号により、それぞれの出力回転軸を回転させる。車輪２１ａ、２１ｂは、制御部５からの駆動信号に応じた回転量及び／又は回転速度にて回転する。車輪２１ａ、２１ｂの回転により、走行部２は、本体部１（すなわち、自律移動体１００）を移動できる。モータ２３ａ、２３ｂとしては、例えば、ブラシレスモータなどの電動モータを用いることができる。

また、モータ２３ａ、２３ｂのそれぞれの出力回転軸には、図示しない、モータ２３ａ、２３ｂのそれぞれの出力回転軸の回転数（回転量）を測定する装置が取り付けられている。そして、当該装置により測定された出力回転軸の回転量は、制御部５へ出力される。

これにより、制御部５は、モータ２３ａ、２３ｂのそれぞれの出力回転軸の回転量に基づいて、車輪２１ａ、２１ｂの回転量を算出できる。上記のモータ２３ａ、２３ｂの出力回転軸の回転量を測定する装置としては、例えば、エンコーダを用いることができる。例えば、インクリメンタル型のエンコーダを用いて出力回転軸の回転量を測定した場合、出力回転軸の回転量は、エンコーダから出力されるパルス信号に含まれる所定時間あたりのパルス数として測定できる。

位置データ取得部３は、自律移動体１００の周囲に存在する障害物や壁などの物体を、所定の座標上に投影した像（以後、「投影物体像」と呼ぶことにする）を、複数の位置データとして取得する。

上記の投影物体像を形成可能な複数の位置データは、例えば、レーザレンジファインダ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ、ＬＲＦ）を位置データ取得部３として用いることにより取得できる。その他、ＴｏＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）カメラなど、自身の周囲にある物体の自身からの距離を測定できる装置を、位置データ取得部３として用いることができる。

本実施形態では、レーザレンジファインダを位置データ取得部３として用いるが、複数の位置データは、例えば、自律移動体１００から上記の物体の所定の箇所（例えば、レーザレンジファインダから出力された信号を反射する物体の表面の一部）までの距離と、上記の物体の箇所が存在する自律移動体１００からの方向を示す角度とが関連付けられた二次元のデータとして取得される。
なお、上記の物体の箇所が存在する自律移動体１００からの方向を示す角度は、レーザレンジファインダである位置データ取得部３の走査角度範囲内の角度である。

位置データ取得部３により得られる位置データは、車輪２１ａ、２１ｂの使用による径変化の影響を受けていない座標値データである。従って、本実施形態においては、位置データ取得部３において取得された複数の位置データを、移動領域における自律移動体１００の実際の移動量により近い移動量を算出するために用いる。

なお、自律移動体１００の移動量を位置データを用いて算出する際、上記の距離と角度とを座標値として有する位置データを用いて移動量を算出してもよいし、適当な座標変換を行った新たな位置データを用いて移動量を算出してもよい。
位置データの座標変換を行う場合は、例えば、距離と角度の座標値である位置データを、二次元ならＸ−Ｙ座標、三次元ならＸ−Ｙ−Ｚ座標上の位置データに変換できる。

また、図１に示すように、位置データ取得部３は、前方データ取得部３１と後方データ取得部３３とにより構成されている。前方データ取得部３１は、自律移動体１００の前方に取り付けられており、自律移動体１００の前方に存在する物体からの位置データを取得できる。一方、後方データ取得部３３は、自律移動体１００の進行方向の後方に取り付けられており、自律移動体１００の後方に存在する物体からの位置データを取得できる。

本実施形態においては、前方データ取得部３１の位置データ取得可能範囲が、後方データ取得部３３の位置データ取得可能領域よりも広くなっている。例えば、本実施形態においては、後方データ取得部３３が後方データ取得部３３を中心とした半径１０ｍ程度の自律移動体１００の後方の１８０°程度の範囲の物体を検出できる一方、前方データ取得部３１は、前方データ取得部３１を中心とした半径３０ｍ程度の自律移動体１００の前方の２７０°程度の範囲に存在する物体を検出できる。

これにより、位置データ取得部３は、自律移動体１００を中心とした（特に、自律移動体１００の前方において）より広い範囲に存在する物体を検出できる。なお、上記の前方データ取得部３１の物体の検出範囲と後方データ取得部３３の物体の検出範囲は、上記の検出範囲に限られず、必要に応じて、適宜適切な検出範囲を設定できる。

また、本実施形態において、前方データ取得部３１及び／又は後方データ取得部３３は、上記の本体部１の鉛直方向の上方（の前方及び後方）に取り付けられている。これにより、例えば、移動領域の床面などに設置された物体の移動が頻繁に起こる場合に（例えば、大規模小売店などのように、棚や看板のレイアウト変動が頻繁に行われる場合に）、前方データ取得部３１及び後方データ取得部３３により取得された複数の位置データにより形成される投影物体像の時間的な変化（特に、ある時間に存在した物体が、他の時間において存在しないような変化）の影響をできるだけ排除できる。

従来の技術では、移動領域に設置された物体の移動が頻繁に起こる場合、自律移動体１００の移動前の投影物体像と、移動後の投影物体像とが大きく異なる場合がある。移動前後において投影物体像が大きく異なっていると、移動前後の投影物体像の一方をどのように移動しても、他方の投影物体像に一致しない。この場合、複数の位置データを用いた自律移動体１００の移動量の推定が困難となる。

また、従来の技術では、移動前後の投影物体像が大きく異なる場合に、移動させた投影物体像と基準となる投影物体像とが一致したと判断されても、移動させた投影物体像の移動量から推定される自律移動体１００の移動量が、実際の自律移動体１００の移動量とは大きくかけ離れる場合がある。つまり、複数の位置データに基づいて推定した自律移動体１００の移動量と、実際の自律移動体１００の移動量との間に、大きな誤差が生じる場合がある。

それらの問題を解決するために、本実施形態では、上述したように、前方データ取得部３１及び／又は後方データ取得部３３を本体部１の上部（例えば、人の頭や商品棚の高さを超える位置）に設置して移動が頻繁に行なわれる物体を検知する可能性を低減している。この結果、前方データ取得部３１及び後方データ取得部３３（位置データ取得部３）により取得した複数の位置データを用いることで、大きな誤差などを生じることなく、つまり精度よく自律移動体１００の移動量を推定できる。

制御部５は、走行部２を制御する。具体的には、制御部５は、自律移動体１００の現在位置を、複数の位置データに基づいて算出された自律移動体１００の移動量に基づいて推定する。また、制御部５は、推定された移動量や現在位置などに基づいて、自律移動体１００の現在位置を推定する。

さらに、制御部５は、推定された現在位置から制御部５の記憶部（後述）などに記憶された予め決められた目標位置（あるいは、複数ある目標位置のうちの１つ）まで自律移動体１００を移動させるための制御信号を算出し、当該制御信号に基づいて、走行部２を駆動するための駆動信号を算出し、走行部２（のモータ２３ａ、２３ｂ）に出力する。
なお、制御部５の詳細な構成、及び、制御部５における自律移動体１００における移動量の算出方法や現在位置の推定方法などについては、後ほど説明する。

自律移動体１００は移動領域における自身の存在位置を、車輪２１ａ、２１ｂの回転量及び／又は位置データ取得部３により取得した複数の位置データを用いて推定できる。また、自律移動体１００は、目標位置まで移動するための制御信号を、推定された自律移動体１００の（現在）位置に基づいて、自律移動体１００内部（制御部５）において算出できる。その結果、自律移動体１００は、予め決められた走行経路を、自律的に移動できる。

図１に示すように、第１実施形態の自律移動体１００は、取付部材９を介して本体部１に取り付けられた操作部７をさらに備えている。図１に示すように、操作部７は、回動可能な操作ハンドル７１ａ、７１ｂを有し、操作ハンドル７１ａ、７１ｂのそれぞれの操作量（回動量）を制御部５に出力可能となっている。これにより、制御部５は、操作ハンドル７１ａ、７１ｂの操作量に応じて、モータ２３ａ、２３ｂを制御する駆動信号を算出可能となる。

自律移動体１００が上記の操作部７を備えることにより、操作者が自律移動体１００を操作可能となる。具体的には、操作者が操作ハンドル７１ａ、７１ｂを回動するなどして車輪２１ａ、２１ｂの回動量や回転速度を調整することにより、操作者は自律移動体１００を操作できる。

また、上記のように、制御部５は自律移動体１００自身の移動領域における位置を推定可能となっている。そのため、例えば、操作者が操作部７を用いて自律移動体１００を操作している時に所定の時間間隔にて自律移動体１００の位置を推定し、推定された位置を記憶部５１などに記憶することにより、自律移動体１００（制御部５）に、操作者による自律移動体１００の操作により走行した経路を、上記の予め決められた走行経路として教示できる。

なお、自律移動体１００に走行経路を教示する方法は、上記した、操作者が操作部７を用いて自律移動体１００を操作する間に自律移動体１００の位置を推定して記憶する方法に限られない。例えば、移動領域を表す移動座標上の（複数の）座標点を記憶したデータにおいて、座標点を示す値を直接操作（あるいは直接入力）することによっても、自律移動体１００に走行経路を教示できる。または、自律移動体１００の移動領域における位置（移動座標の座標値）を指定するその他の方法によって、自律移動体１００に走行経路を教示できる。

また、第１実施形態の自律移動体１００は、補助輪部８をさらに備える。補助輪部８は、２つの補助車輪８ａ、８ｂを有する。２つの補助車輪８ａと８ｂは、それぞれが独立に回転可能なように本体部１の後方底部に取り付けられている。補助輪部８を備えることにより、自律移動体１００は安定に、かつ、スムーズに移動できる。

（２）制御部の構成
Ｉ．制御部の全体構成
次に、制御部５の全体構成について図２を用いて説明する。図２は、制御部の構成を示す図である。制御部５は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）と、ハードディスク装置と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）と、記憶媒体読み出し装置などにより構成される記憶装置と、信号変換を行うインターフェースなどと、を備えたマイコンシステムなどにより実現できる。また、以下に示す制御部５の各部の機能の一部又は全部は、プログラムとして実現されていてもよい。さらに、当該プログラムは、マイコンボードの記憶装置に記憶されていてもよい。又は、制御部５の各部の機能の一部又は全部は、カスタムＩＣなどにより実現されていてもよい。

また、制御部５は、複数のマイコンシステムにより構成されていてもよい。例えば、後述する移動量推定部５３（移動量推定装置の一例）と、走行制御部５７と、制御部５の残りの構成要素とを個別のマイコンシステムにより構成してもよい。

制御部５は、記憶部５１と、移動量推定部５３と、位置推定部５５と、走行制御部５７と、を有する。記憶部５１は、制御部５を構成する記憶装置の記憶領域の少なくとも一部に対応する。記憶部５１は、自律移動体１００が移動領域を自律的に移動するために必要な移動経路に関する情報などを記憶する。移動経路に関する情報は、例えば、移動領域の地図情報（グローバルマップ、または、環境地図と呼ばれることもある）などである。その他、記憶部５１は、自律移動体１００の動作に必要な各種設定値などを記憶している。

移動量推定部５３は、自律移動体１００が移動した距離を推定する。移動量推定部５３は移動量算出部５３１を有している。移動量算出部５３１は、複数の位置データに基づいて、自律移動体１００が移動した距離を算出する。なお、移動量算出部５３１の構成の詳細及び移動量の算出方法については、後ほど詳しく説明する。

位置推定部５５は、自律移動体１００の移動領域における存在位置を推定する。本実施形態において、位置推定部５５は、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）法などを用いて、自律移動体１００の周囲の壁や障害物などの物体の位置を示す地図情報（ローカルマップ）と、予め記憶されていた移動領域（の全体、又は、自律移動体１００が移動する領域を少なくとも含む一部）を示す地図情報（グローバルマップ、環境地図）とのマップマッチング結果に基づいて、自律移動体１００の現在位置を推定する。

具体的には、位置推定部５５は、まず、移動量推定部５３にて推定された移動量に所定の誤差分を追加して算出した「仮の現在位置」を複数個生成する。上記の仮の現在位置は、例えば、移動前に推定されていた自律移動体１００の位置に、上記の誤差分を追加した移動量を加えることにより算出できる。

次に、上記の仮の現在位置のそれぞれに対して、現在位置において位置データ取得部３を用いて取得した複数の位置データを用いて上記のローカルマップを生成する。その後、位置推定部５５は、複数個生成したローカルマップのうち、上記の環境地図と最もよく一致したローカルマップを生成したときの仮の現在位置を、自律移動体１００の実際の現在位置と推定する。

また、位置推定部５５は、操作者が操作部７を用いて自律移動体１００を操作しているときには、所定の時間間隔（例えば、制御周期毎）にて上記の方法により自律移動体１００の位置を推定して、推定した位置についての情報（座標値など）を記憶部５１に記憶する。これにより、位置推定部５５は、操作者による自律移動体１００の移動操作を記憶部５１に記憶する。その結果、自律移動体１００は、記憶された操作者による移動操作を用いて自律的に移動することにより、操作者による移動操作を自律的に再現できる。

なお、位置推定部５５は、現在位置において作成したローカルマップを用いて、上記の環境地図を更新して記憶部５１に記憶してもよい。これにより、移動領域中において障害物などの位置が変更された場合でも、障害物などの位置が変更された後の新しい環境地図を作成し記憶できる。

走行制御部５７は、自律移動体１００が自律的に移動するときには、自律移動体１００を、現在位置から、記憶部５１に記憶された予め決められた移動経路に含まれる所定の目標位置まで走行するように、走行部２を制御する。

具体的には、走行制御部５７は、まず、現在位置から上記の所定の目標位置までの距離や方向などに基づいて、現在位置から所定の目標位置まで、所定の速度及び／又は加速度にて自律移動体１００を移動させる制御指令を生成する。次に、走行制御部５７は、上記の制御指令に基づいて、モータ２３ａ、２３ｂを制御するための駆動信号を算出し、モータ２３ａ、２３ｂに出力する。

なお、走行制御部５７は、モータ２３ａ、２３ｂのそれぞれに対して個別に駆動信号を出力する。すなわち、モータ２３ａ、２３ｂはそれぞれが異なる回転速度にて回転できる。例えば、自律移動体１００を直進させたい場合には、モータ２３ａ、２３ｂを同じ回転速度にて回転させる。一方、モータ２３ａ、２３ｂのそれぞれを異なる回転速度にて回転させると、自律移動体１００は回転運動（右折、左折）できる。
図１に示した走行部２のように、２つの車輪２１ａ、２１ｂのそれぞれを個別にモータ２３ａ、２３ｂにより回転させて走行する走行部を、「差動二輪型」の走行部と呼ぶこともある。

また、走行制御部５７は、操作部７から操作ハンドル７１ａ、７１ｂの回動量（操作量）を入力し、入力した回動量から上記の駆動信号を算出し、モータ２３ａ、２３ｂに出力可能となっている。これにより、自律移動体１００は、操作ハンドル７１ａ、７１ｂを操作する操作者により操作可能となる。

走行制御部５７としては、例えば、フィードバック制御理論を用いた制御装置などを用いることができる。フィードバック制御理論を用いた制御装置を走行制御部５７として用いる場合、走行制御部５７は、モータ２３ａ、２３ｂの出力回転軸に取り付けられたエンコーダなどからのパルス信号を入力し、モータ２３ａ、２３ｂの回転数を制御指令に示したとおりに制御できる。

制御部５が上記の構成を有することにより、自律移動体１００は、記憶部５１に記憶されている走行経路などの予め決められた走行経路を、自律的に移動できる。

（３）移動量算出部の構成
次に、第１実施形態の移動量算出部５３１の構成について、図３を用いて説明する。図３は、移動量算出部の構成を示す図である。移動量算出部５３１は、位置データ記憶部５３１１と、第１移動量算出部５３１２と、位置データ移動部５３１３と、第２移動量算出部５３１４と、第３移動量算出部５３１５と、を有している。

位置データ記憶部５３１１は、制御部５のマイコンシステムの記憶装置の記憶領域、又は、記憶部５１の記憶領域の一部である。位置データ記憶部５３１１は、自律移動体１００の移動前と移動後において位置データ取得部３から取得した（座標変換した）複数の位置データを記憶する。本実施形態においては、第１位置データを自律移動体１００の移動前に取得した位置データとし、第２位置データを自律移動体１００の移動後に取得した位置データとする。
なお、これとは逆に、自律移動体１００の移動前に取得した位置データを第２位置データとし、移動後に取得した位置データを第１位置データとしてもよい。

第１移動量算出部５３１２は、ヒストグラムマッチング法を用いて、自律移動体１００の移動量を第１移動量として算出する。本実施形態においては、第１移動量算出部５３１２は、ヒストグラムマッチング法を用いて、自律移動体１００の移動前の姿勢角（姿勢角は、例えば、自律移動体１００の前方方向の、所定の座標のＸ軸に対する角度として定義）と移動後の姿勢角との差である姿勢角度差を第１移動量として算出する。

そのため、本実施形態において、第１移動量算出部５３１２は、位置データ記憶部５３１１から複数の第１位置データ及び複数の第２位置データを読み出して、複数の第１位置データから、１つの第１位置データと当該１つの第１位置データに隣接する第１位置データとを結ぶ直線の所定の座標の１つの軸（本実施形態においてはＸ軸。しかし、Ｘ軸に限られずＹ軸としてもよい）に対する角度が現れる頻度を示す第１ヒストグラムを生成する。

また、第１移動量算出部５３１２は、複数の第２位置データから、１つの第２位置データと当該１つの第２位置データに隣接する第２位置データとを結ぶ直線の所定の座標の１つの軸に対する角度が現れる頻度を示す第２ヒストグラムを生成する。

その後、第１移動量算出部５３１２は、相互相関関数を用いて上記の第１ヒストグラムと第２ヒストグラムとの一致度を算出し、上記の第１ヒストグラムと第２ヒストグラムとが最も一致する（典型的には相互相関関数が最大値になる）姿勢角度変化を、第１移動量として算出する。なお、上記の第１ヒストグラムと第２ヒストグラムとが「最も一致する」の定義としては、本実施形態で用いる「相互相関関数が最大になる場合」の他に、「ある姿勢角度変化量（すなわちヒストグラムの移動量）とその前後の角度変化量とにおける相互相関関数の合計値が最大になる場合」などを適宜用いてもよい。また、本実施形態においては相互相関関数を用いて上記の第１ヒストグラムと第２ヒストグラムとの一致度を算出しているが、これに限らず、正規化相互相関関数やゼロ平均正規化相互相関関数などを適宜用いて第１ヒストグラムと第２ヒストグラムの一致度を算出してもよい。

位置データ移動部５３１３は、位置データ記憶部５３１１から入力した複数の第２位置データを、第１移動量算出部５３１２において算出した第１移動量に対応する移動量だけ移動し、複数の第１移動位置データを算出する。

本実施形態においては、第１移動量算出部５３１２において姿勢角度差を第１移動量として算出しているため、位置データ移動部５３１３は、複数の第２位置データを回転移動させて複数の第１移動位置データを算出する。複数の位置データの回転移動は、例えば、座標の回転を示す公知の数式などを用いて実行できる。

なお、位置データ移動部５３１３は、複数の位置データを平行移動可能となっていてもよい。複数の位置データの平行移動は、全ての位置データの座標値に対して平行移動量に対応する値を加えることにより実行できる。

第２移動量算出部５３１４は、位置データ移動部５３１３にて算出された複数の第１移動位置データと、位置データ記憶部５３１１に記憶された複数の第１位置データとを用いて第２移動量を算出する。具体的には、第２移動量算出部５３１４は、複数の第１位置データにより所定の座標上に形成される第１投影物体像と複数の第２移動位置データ（後述）により形成される第２移動投影物体像とが一致すると判断したときの上記の第２移動位置データを算出したときの、複数の第１移動位置データの移動量（第２移動位置データは、第１移動位置データを所定の座標上において移動して算出される）を第２移動量として算出する。

本実施形態において、第２移動量算出部５３１４は、第１投影物体像と第２移動投影物体像とが一致すると判断したときの第２移動位置データを算出したときの、第１移動位置データの平行移動量を第２移動量として算出する。

第３移動量算出部５３１５は、第１移動量算出部５３１２において算出された第１移動量と、第２移動量算出部５３１４において算出された第２移動量とを積算して、自律移動体１００の移動前後の移動量を算出する。

移動量算出部５３１が上記の構成を有することにより、自律移動体１００の（一定期間の）移動前後の移動量を、移動前及び移動後において取得した複数の位置データを用いて、高速かつ精度よく算出できる。

（４）自律移動体の動作
Ｉ．第１移動量の算出方法
次に、本実施形態の自律移動体１００の動作について説明する。まず、第１移動量算出部５３１２における第１移動量の算出方法について、図４を用いて説明する。
第１移動量算出部５３１２は、角度θの関数である第１ヒストグラムＨ１１（θ）及び第２ヒストグラムＨ２１（θ）の相互相関関数から、自律移動体１００の移動前後の姿勢角度差（回転運動の移動量）を算出する。

具体的には、まず、第１移動量算出部５３１２は、複数の第１位置データから第１ヒストグラムＨ１１（θ）を、複数の第２位置データから第２ヒストグラムＨ２１（θ）を生成する（ステップＳ１００１）。

具体的には、例えば、まず、１つの第１位置データと隣接する第１位置データとにより形成される直線と、第１位置データを定義した所定の座標（例えば、Ｘ−Ｙ座標）の１つの座標軸（例えば、Ｘ軸）とのなす角度θを、全ての（あるいは所定の数の）第１位置データに対して算出する。次に、算出した角度θの出現頻度を各角度θに対して算出し、当該出現頻度と角度θとを関連づけて、角度θの関数としての第１ヒストグラムＨ１１（θ）が生成される。

第２ヒストグラムＨ２１（θ）も、複数の第２位置データを用いて、上記の第１ヒストグラムＨ１１（θ）の算出方法により同様に生成できる。

ヒストグラムを生成後、第１移動量算出部５３１２は、相互相関関数を算出する（ステップＳ１００２）。具体的には、Ｈ１１（θ）と、Ｈ２１（θ）をａだけ移動したヒストグラムとの積であるＨ１１（θ）＊Ｈ２１（θ−ａ）をとりうるθの値だけ加算した関数（Ｈ１１（θ）＊Ｈ２１（θ−ａ）のθに対する積分値であるとも言える）を相互相関関数として算出する。これにより、相互相関関数は、値ａを変数とした関数として算出される。算出された各ａ値における相互相関関数（の値）は、記憶部５１などに記憶される。

相互相関関数の値をあるａ値において算出後、決められた範囲内（例えば、角度θの検出可能範囲）のａ値をすべて用いて相互相関関数の値を算出したかどうかを判断する（ステップＳ１００３）。
まだ相互相関関数の値を算出すべきａ値が存在する場合（ステップＳ１００３において「Ｎｏ」の場合）、ａ値を他の値に変更した（ステップＳ１００４）後に、第１移動量の算出プロセスは、ステップＳ１００２に戻り、相互相関関数の算出を継続する。

一方、とりうる全てのａ値に対して相互相関関数の値を算出した場合（ステップＳ１００３において「Ｙｅｓ」の場合）、第１移動量算出部５３１２は、各ａ値に対して算出された複数の相互相関関数の値を比較して、ａ値の関数としての相互相関関数が最大値となっているときのａ値を、第１移動量（姿勢角度差）として算出する（ステップＳ１００５）。その後、第１移動量を位置データ移動部５３１３に出力して（又は記憶部５１に記憶して）、第１移動量の算出を終了する。

上記のステップＳ１００１〜Ｓ１００５に示した相互相関関数を用いた第１移動量の算出方法を模式的に示すと、図５Ａのようになる。すなわち、複数の第１位置データから生成した第１ヒストグラムＨ１１（θ）（図５Ａの実線にて示したヒストグラム）と、複数の第２位置データから生成した第２ヒストグラムＨ２１（θ）（図５Ａの点線にて示したヒストグラム）とは、典型的には、図５Ａの上図に示すように、互いに形状は類似しているが、互いがヒストグラムを定義した軸方向にずれたような状態となっている。

相互相関関数においてａ値を変化させることは、図５Ａにおいて、第２ヒストグラムＨ２１（θ）を軸方向にａ値に対応する移動量だけシフトさせることに対応し、相互相関関数が最大値（図５においては、ａ＝Ａのときに最大値となるとする）となることは、図５Ａの下図のように、第１ヒストグラムＨ１１（θ）とシフト後の第２ヒストグラムＨ２１（θ−Ａ）とが最も一致することに対応する。

上記のステップＳ１００１〜Ｓ１００５を実行することにより、複数の第１位置データと複数の第２位置データとの関係がどのような変化をするかを、図５Ｂを用いて説明する。
例えば、自律移動体１００を原点ＯとしたＸ−Ｙ座標上において、複数の第１位置データにより第１投影物体像が図５Ｂの（１）の実線にて示すように形成され、複数の第２位置データにより第２投影物体像が図５Ｂの（１）の点線にて示すように形成されていたとする。

上記の状態において、第２ヒストグラムＨ２１（θ）がシフト量（移動量）Ａだけシフト（移動）することにより、第２投影物体像が原点Ｏの回りにシフト量Ａに対応する角度だけ回転する。第１ヒストグラムＨ１１（θ）とシフト後の第２ヒストグラムＨ２１（θ−Ａ）とが最も一致するとき、第１投影物体像と回転後（移動後）の第２投影物体像は、互いに、平行移動すると一致するような位置関係となる（図５Ｂの（２））。

従って、上記の第１ヒストグラムＨ１１（θ）と第２ヒストグラムＨ２１（θ）との相互相関関数が最大値となるときのａ値（図５Ａの例では、シフト量Ａ）である第１移動量を、自律移動体１００の移動前後の姿勢角度変化と定義できる。

上記のように、第１移動量算出部５３１２は、ヒストグラムマッチング法を用いて自律移動体１００の姿勢角度差のみを第１移動量として算出できる。これにより、第１移動量算出部５３１２は、回転移動の中心点がずれることなく、正確な回転移動の移動量（姿勢角度変化）を算出できる。

また、ヒストグラムマッチング法においては、より少ない計算量にて相互相関関数が最大値となるヒストグラムのシフト量を算出できる。その結果、第１移動量算出部５３１２は、高速に第１移動量を算出できる。

ＩＩ．第２移動量の算出方法
次に、第２移動量算出部５３１４における第２移動量の算出方法について、図６を用いて説明する。第２移動量算出部５３１４における第２移動量の算出方法は、複数の第２移動位置データ（複数の第１移動位置データ）を、複数の第２移動位置データにより形成される投影物体像（第２移動投影物体像（図７））と第１投影物体像とが一致するまで、典型的には複数回移動させて移動量を算出する方法である。このような第２移動量の算出方法を、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法と呼ぶこともある。

具体的には、まず、位置データ移動部５３１３が、位置データ記憶部５３１１に記憶されている複数の第２位置データを、第１移動量算出部５３１２において上記のステップＳ１００１〜Ｓ１００５を実行することにより算出された第１移動量に対応する移動量だけ移動して、複数の第１移動位置データを算出する（ステップＳ２００１）。本実施形態においては、複数の第２位置データを、第１移動量（姿勢角度差）に対応する回転移動の移動量だけ移動させて、複数の第１移動位置データを算出する。その結果、複数の第１移動位置データにより、上記の図５Ｂの（２）の点線にて示すような投影物体像が形成される。

次に、第２移動量算出部５３１４が、ステップＳ２００１にて算出した複数の第１移動位置データと複数の第１位置データとを用いて、第２移動量を算出する。具体的には、以下のようにして第２移動量が算出される。

まず、第２移動量算出部５３１４は、複数の第１移動位置データをさらに平行移動して複数の第２移動位置データを算出する。なお、位置データ移動部５３１３において、複数の位置データの平行移動が可能であれば、第２移動量算出部５３１４は、位置データ移動部５３１３に対して、複数の第１移動位置データを平行移動して複数の第２移動位置データを算出するよう指令してもよい。

複数の第２移動位置データを算出後、第２移動量算出部５３１４は、複数の第２移動位置データにより形成される第２移動投影物体像と第１投影物体像との間の距離を算出する（ステップＳ２００２）。

具体的には、例えば、第２移動量算出部５３１４は、複数の第１位置データに含まれる１つの第１位置データと、複数の第２移動位置データに含まれ上記の１つの第１位置データに対応する１つの第２移動位置データと、その近傍の第２移動位置データにより定義される平面との距離をデータ間距離として算出し、上記のデータ間距離を所定の数だけ合計したときの合計距離を、第１投影物体像と第２移動投影物体像との間の距離として算出する。

なお、第２移動量算出部５３１４は、１つの第１位置データと当該１つの第１位置データに対応する１つの第２移動位置データとの間の距離を１つのデータ間距離として算出してもよいし、１つの第１位置データと複数の第２移動位置データに含まれ当該１つの第１位置データの比較的近傍に存在する複数の第２動位置データのそれぞれとの距離を複数のデータ間距離として算出してもよい。

１つの第１位置データに対して複数のデータ間距離を算出する場合、第２移動量算出部５３１４は、算出された複数のデータ間距離のうち所定の距離以上のものを使用しないようにしてもよい。所定の距離以上の平面距離を使用しないときには、１つの第１位置データに対してデータ間距離が全く算出されない場合、１つの移動位置データに対して複数のデータ間距離が算出される場合、１つの第１位置データに対して１つのデータ間距離が算出される場合がある。

上記のデータ間距離としては、座標空間における距離を表す様々な距離を用いることができる。例えば、ユークリッド距離やマンハッタン距離（２つの座標点の各座標値の差を合計した距離）などを、位置データ間の距離として用いることができる。

または、第２移動量算出部５３１４は、１つの第１位置データと複数の第２移動位置データにより形成される投影物体像との間の距離を、上記のデータ間距離としてもよい。１つの第１位置データと複数の第２移動位置データにより形成される投影物体像との間の距離は、例えば、１つの第１位置データと、１つの第１位置データから当該投影物体像に伸ばした垂線と当該投影物体像との交点との間の距離として算出できる。

上記のようにして第１投影物体像と第２移動投影物体像との間の距離を算出後、第２移動量算出部５３１４は、第１投影物体像と第２移動投影物体像との間の距離と所定の閾値との比較に基づいて、第１投影物体像と第２移動投影物体像とが一致したかどうかを判断する（ステップＳ２００３）。
例えば、第２移動量算出部５３１４は、第１投影物体像と第２移動投影物体像との間の距離が所定の閾値以下となったときに、第１投影物体像と第２移動投影物体像とが一致すると判断する。

上記の２つの投影物体像の間の距離が所定の閾値より大きく、第１投影物体像と第２移動投影物体像とが一致しないと判断された場合（ステップＳ２００３において「Ｎｏ」の場合）、第２移動量算出部５３１４は、複数の第２移動位置データを平行移動する（ステップＳ２００４）。このとき、第２移動量算出部５３１４は、位置データ移動部５３１３に対して、複数の第２移動位置データを平行移動するように指令してもよい。

その後、第２移動量の算出プロセスはステップＳ２００２に戻り、新たに算出された複数の第２移動位置データにより形成される第２移動投影物体像が第１投影物体像と一致するかどうか判断される。第１投影物体像と第２移動投影物体像とが一致すると判断されるまで、上記のステップＳ２００２〜ステップＳ２００４が繰り返される。

一方、第１投影物体像と第２移動投影物体像とが一致すると判断された場合（ステップＳ２００３において「Ｙｅｓ」の場合）、第２移動量算出部５３１４は、今回の複数の第２移動位置データが算出されるまでの、複数の第１移動位置データからの平行移動の移動量を、第２移動量として算出する（ステップＳ２００５）。

上記のステップＳ２００１〜Ｓ２００５を実行することによるＩＣＰ法による第２移動量の算出方法を模式的に示すと、図７のようになる。ステップＳ２００１を実行して複数の第２位置データを第１移動量（姿勢角度差）に対応する移動量だけ移動して複数の第１移動位置データを算出すると、算出された複数の第１移動位置データは、図７の（１）（図５の（２）と同一）の点線に示すような第１移動投影物体像を形成する。

ステップＳ２００２〜Ｓ２００５を実行前（すなわち、ＩＣＰ法による第２移動量の算出前）に、図７の（１）に示すように、第１移動投影物体像は、第１投影物体像を平行移動した像とほぼ一致している。この場合、図７の（２）に示すように、第２移動量算出部５３１４は、第２移動位置データを算出する際に第１移動位置データを平行移動するだけで、複数の第２移動位置データにより形成される第２移動投影物体像を第１投影物体像にほぼ一致させることができる。

ＩＣＰ法により回転運動の移動量を算出しようとすると、回転運動の中心点が動いてしまうことがあるため、ＩＣＰ法により回転運動の移動量（姿勢角度差）を正確に算出できない場合がある。しかしながら、上記のように、第１移動量算出部５３１２において予め自律移動体１００の移動前後の姿勢角度差を算出することにより、第２移動量算出部５３１４において姿勢角度差を算出する必要がなくなる。その結果、自律移動体１００の移動量の算出において、上記の回転運動の中心点がずれて姿勢角度差が正確に算出できなくなることを回避して、自律移動体１００の移動量を姿勢角度差も含めて正確に算出できる。

また、上記のように第１移動量算出部５３１２は第１移動量を高速に算出でき、また、複数の第１移動位置データは複数の第２位置データよりも複数の第１位置データにより
近い（一致度が高い）位置データであるため、第２移動量算出部５３１４は、第１位置データと第２位置データとを用いて算出するよりも高速に第２移動量を算出できる。

実際、同一の第１位置データ及び第２位置データを用いて自律移動体１００の移動量を算出した場合、本実施形態のように第１移動量算出部５３１２により第１移動量を算出後に第２移動量算出部５３１４により第２移動量を算出して第１移動量と第２移動量とを積算して自律移動体１００の移動量を算出した方が、第２移動量算出部５３１４のみにて第１位置データと第２位置データとを用いて自律移動体１００の移動量を算出するよりも、３．５倍程度高速に自律移動体１００の移動量を算出できた。

さらに、上記の第１移動量算出部５３１２及び第２移動量算出部５３１４においては、複数の位置データにより形成される投影物体像、第１ヒストグラム、及び第２ヒストグラムといった、複数の位置データを用いて算出される値に基づいて、自律移動体１００の移動量が算出されている。これにより、位置データ取得部３にて取得した位置データに測定誤差（ノイズなどを原因としている）が含まれていても、第１移動量算出部５３１２及び第２移動量算出部５３１４は、精度よく第１移動量及び第２移動量を算出できる。

ＩＩＩ．自律移動体の動作
次に、本実施形態の自律移動体１００の具体的な動作について、図８を用いて説明する。以下においては、自律移動体１００が記憶部５１などに記憶された予め決められた複数の目標点を通過しながら、最終的な目標点まで自律的に移動するときの動作について説明する。
自律移動体１００が自律的な移動を開始すると、まず、自律移動体１００の移動前及び移動後において、位置データ取得部３が複数の位置データを取得する（ステップＳ１）。そして、取得した複数の位置データ（第１位置データ、第２位置データ）が位置データ記憶部５３１１に記憶される。

複数の第１位置データ及び第２位置データを取得後、これらの複数の位置データを用いて自律移動体１００の移動前後の移動量が算出される。具体的には、まず、第１移動量算出部５３１２が、前述のステップＳ１００１〜Ｓ１００５を実行して、第１移動量（姿勢角度差）を算出する（ステップＳ２）。

第１移動量の算出後、第２移動量算出部５３１４が、上記のステップＳ２００１〜Ｓ２００５を実行して、第２移動量を算出する（ステップＳ３）。
第１移動量及び第２移動量を算出後、第３移動量算出部５３１５が、第１移動量と第２移動量とを積算して、自律移動体１００の移動量を算出する（ステップＳ４）。

自律移動体１００の移動量を算出後、位置推定部５５は、移動量推定部５３から上記のステップＳ４において算出された移動量を入力し、入力した移動量に基づいて、上記に説明したマップマッチングによる自律移動体１００の位置の推定方法を用いて、自律移動体１００の現在位置を推定する（ステップＳ５）。

自律移動体１００の現在位置を推定後、走行制御部５７は、自律移動体１００が現在位置から次の目標位置まで移動するための駆動信号を算出し、走行部２のモータ２３ａ、２３ｂに出力する（ステップＳ６）。

具体的には、走行制御部５７は、まず、位置推定部５５から上記のステップＳ５にて推定された自律移動体１００の現在位置を入力する。また、走行制御部５７は、入力した現在位置に基づいて、次の目標位置を記憶部５１などから読み出す。その後、走行制御部５７は、自律移動体１００の現在位置から、読み出した次の目標位置まで自律移動体１００を移動させるための制御指令を算出する。
走行制御部５７にて算出される制御指令は、例えば、現在位置から次の目標位置まで移動する間の時間と自律移動体１００の速度（車輪２１ａ、２１ｂの回転速度、すなわち、モータ２３ａ、２３ｂの回転速度）との関係を表したデータとして算出される。

制御指令を算出後、走行制御部５７は、算出した制御指令に基づいて、モータ２３ａ、２３ｂを駆動（制御）する駆動信号を算出し、算出した駆動信号を走行部２のモータ２３ａ、２３ｂに出力する。

駆動信号を走行部２に出力後、制御部５は、例えば、記憶部５１に記憶されている全ての目標位置を自律移動体１００が通過したかどうかを判断して、自律移動を終了するかどうかを判断する（ステップＳ７）。
例えば、自律移動体１００が全ての目標位置を通過していないと判断して、自律移動を継続すると判断した場合（ステップＳ７において「Ｎｏ」の場合）、自律移動のプロセスはステップＳ１に戻り、自律移動を継続する。

一方、自律移動体１００が全ての目標位置を通過して自律移動を終了すると判断した場合（ステップＳ７において「Ｙｅｓ」の場合）、自律移動のプロセスを終了する。

本実施形態の自律移動体１００においては、第１移動量算出部５３１２における第１移動量の算出と、第２移動量算出部５３１４における移動量の算出とを上記のように組み合わせて高速にかつ精度よく自律移動体１００の移動量が算出されている。これにより、位置推定部５５は、精度よくかつ高速に自律移動体１００の現在位置を推定できる。また、走行制御部５７は、現在位置から予め決められた目標位置に確実に到達可能なように、走行部２を制御遅れなく制御できる。その結果、自律移動体１００は、意図した走行経路を正確に、かつ、制御遅れを発生することなく移動できる。

２．第２実施形態
（１）第２実施形態の概略
上記の第１実施形態において、第１移動量算出部５３１２は、ヒストグラムデータ間隔が小さい２つのヒストグラムのヒストグラムマッチングを１度実行して、第１移動量を算出していた。この場合、ヒストグラムマッチングを実行するために膨大な計算量が必要となるため、第１移動量の算出に時間がかかっていた。
上記の問題を解決するため、第２実施形態に係る第１移動量算出部５３１２は、最初にヒストグラムデータ間隔の比較的大きなヒストグラムを用いたヒストグラムマッチングを実行して第１移動量の「候補」を算出する。その後、第１移動量算出部５３１２は、ヒストグラムデータ間隔を小さくしたヒストグラムを用いたヒストグラムマッチングを実行して、当該第１移動量の「候補」を補正して最終的な第１移動量を算出している。

上記のように、第２実施形態においては、第１移動量算出部５３１２における第１移動量の算出方法が異なるのみで、自律移動体１００の構成、第２移動量の算出方法、及び最終的な移動量の算出方法は、第１実施形態と同じである。従って、ここでは、第１移動量算出部５３１２における第１移動量の算出方法のみを説明し、自律移動体１００の他の構成や算出方法の説明は省略する。

（２）第２実施形態における第１移動量の算出方法
以下、第２実施形態における第１移動量の算出方法を、図９を用いて説明する。図９は、第２実施形態における第１移動量の算出方法を示すフローチャートである。以下では、補正移動量（後述）の算出が複数回実行される場合を例にとって、第１移動量の算出方法を説明する。
以下の説明においては、角度θについて第１移動量を算出する場合を例にとって説明する。角度θについての位置データは、例えば、互いに隣接する複数の位置データにより形成される直線と、位置データを定義した所定の座標（例えば、Ｘ−Ｙ座標）の１つの座標軸（例えば、Ｘ軸）とのなす角度θを、全ての位置データに対して算出することにより作成される。このようにして作成された角度θについての位置データは、位置データ記憶部５３１１に記憶される。

第１移動量の算出を開始すると、第１移動量算出部５３１２は、まず、第３ヒストグラムと第４ヒストグラムを生成する（ステップＳ２１）。具体的には、第１移動量算出部５３１２は、位置データ記憶部５３１１に記憶された角度θについての複数の第１位置データを用いて、当該複数の第１位置データにおいて出現する角度θと当該角度θの出現頻度とを関連付けて、第３ヒストグラムのための複数の第１ヒストグラムデータを生成する。
また、位置データ記憶部５３１１に記憶された角度θについての複数の第２位置データを用いて、当該複数の第２位置データにおいて出現する角度θと当該角度θの出現頻度とを関連付けて、第４ヒストグラムのための複数の第２ヒストグラムデータを生成する。

より具体的には、複数の第１位置データから第３ヒストグラムを生成する場合を例にとると、第３ヒストグラム（複数の第１ヒストグラムデータ）は以下のようにして生成される。以下の説明では、第３ヒストグラムのヒストグラムデータ間隔をＭ１（＝ｒ_１＊ｍ（ｒ_１：整数、ｍ：最小ヒストグラムデータ間隔））（第１ヒストグラムデータ間隔の一例）とし、複数の第１位置データにおいてとりうる角度θの範囲は−ｑｍ〜＋ｑｍ（ｑ：整数）の範囲であるとする。
第１移動量算出部５３１２は、まず、角度θが−（ｑ−（ｓ−１）＊ｒ_１）＊ｍから−（ｑ−ｓ＊ｒ_１）＊ｍ（ｓ：整数）までの範囲内となっている第１位置データの個数を計数し、当該第１位置データの個数を、−（ｑ−（ｓ−１）＊ｒ_１）＊ｍから−（ｑ−ｓ＊ｒ_１）＊ｍまでの範囲内の角度が出現する出現頻度Ｆ_ｓとする。次に、第１移動量算出部５３１２は、当該出現頻度Ｆ_ｓと角度−（ｑ−ｓ＊ｒ_１）＊ｍとを関連付けて、ｓ番目の第１ヒストグラムデータとする。
上記の第１ヒストグラムデータの算出を、ｓを１から１ずつ増加して（２＊ｑ）／ｒ_１となるまで繰り返すことにより、図１０のような複数の第１ヒストグラムデータの集合体が生成される。図１０は、複数のヒストグラムデータの集合体の一例を示す図である。

また、上記と同様にして、複数の第２位置データから、第４ヒストグラムのための複数の第２ヒストグラムデータも生成できる。なお、本実施形態において、第４ヒストグラムのヒストグラムデータ間隔（第２ヒストグラムデータ間隔）は、上記の第１ヒストグラムデータ間隔Ｍ１と同じとする。

なお、複数の第１ヒストグラムデータ（第３ヒストグラム）及び複数の第２ヒストグラムデータ（第４ヒストグラム）の生成方法は、第３ヒストグラム及び第４ヒストグラムのヒストグラムデータ間隔が第１ヒストグラムデータ間隔Ｍ１（＝ｒ_１＊ｍ）を有していればよいので、上記の例に限られない。例えば、ｓ番目の第１ヒストグラムデータの出現頻度Ｆ_ｓを、複数の第１位置データにおいて、−（ｑ−ｓ＊ｒ_１）＊ｍを中心とした所定の範囲内の角度が出現する頻度としてもよい。具体的には、ｓ番目の第１ヒストグラムデータの出現頻度Ｆ_ｓを、複数の第１位置データにおいて−（ｑ−（ｓ−０．５）＊ｒ_１）＊ｍから−（ｑ−（ｓ＋０．５）＊ｒ１）＊ｍまでの範囲内の角度θが出現する頻度としてもよい。

また、自律移動体１００の角度θの移動量（姿勢角度差）と平行移動量とを両方含んだ第１移動量を算出する場合には、第１移動量算出部５３１２は、角度θの出現頻度についてのヒストグラム（複数のヒストグラムデータ）だけでなく、複数の第１位置データ及び複数の第２位置データの座標値の出現頻度についてのヒストグラム（複数のヒストグラムデータ）も生成する。
具体的には、例えば、複数の第１位置データにおいて所定の範囲内の座標値（Ｘ座標値、Ｙ座標値）が出現する頻度と当該所定の範囲内のある１つの座標値とを関連付けて、座標値の出現頻度についての第３ヒストグラム（複数の第１ヒストグラムデータ）を生成できる。

第３ヒストグラム（複数の第１ヒストグラムデータ）と第４ヒストグラム（複数の第２ヒストグラムデータ）とを生成後、第１移動量算出部５３１２は、第３ヒストグラムと第４ヒストグラムとが一致するときの第４ヒストグラムのシフト量を、第１移動量の候補である候補移動量φｃとして算出する（ステップＳ２２）。

具体的には、まず、第１移動量算出部５３１２は、第４ヒストグラムＨ４（φ）をシフト量ｂだけシフトしたときの複数の第２ヒストグラムデータを作成する。例えば、ｓ番目の第２ヒストグラムデータの角度を−（ｑ−ｓ＊ｒ_１）＊ｍ＋ｂとし、ｓ番目の第２ヒストグラムデータの出現頻度をＦ_ｓのままとして、シフト量ｂだけシフト後の第４ヒストグラムＨ４（φ−ｂ）のための新たな第２ヒストグラムデータを生成できる。
次に、第３ヒストグラムＨ３（φ）（複数の第１ヒストグラムデータ）とシフト量ｂだけシフトした第４ヒストグラムＨ４（φ−ｂ）（複数の新たな第２ヒストグラムデータ）との相互相関関数を算出する。具体的には、第３ヒストグラムＨ３（φ）とシフト量ｂだけシフトした第４ヒストグラムＨ４（φ−ｂ）との積Ｈ３（φ）＊Ｈ４（φ−ｂ）をとりうるφの値（第１ヒストグラムデータの個数分又は第２ヒストグラムデータの個数分）だけ加算した値（Ｈ３（φ）＊Ｈ４（φ−ｂ）のφに対する定積分値であるとも言える）を、１つのシフト量ｂに対する相互相関関数として算出する。その後、当該相互相関関数は、当該１つのシフト量ｂと関連付けられて、記憶部５１などに記憶される。

上記の１つのシフト量ｂに対する相互相関関数の算出は、とりうる範囲内の全てのシフト量ｂ（例えば、−１８０°〜１８０°の範囲内の全てのシフト量ｂ）のそれぞれ対して実行される。これにより、とりうる範囲内の全てのシフト量ｂと関連付けられた相互相関関数の集合体が、記憶部５１などに記憶される。

相互相関関数の集合体を算出後、第１移動量算出部５３１２は、算出された相互相関関数の集合体のうち最大値の相互相関関数に関連付けられているシフト量ｂを、第３ヒストグラムＨ３（φ）と第４ヒストグラムＨ４（φ）とが最も一致したときの第４ヒストグラムＨ４（φ）のシフト量とする。すなわち、第１移動量算出部５３１２は、上記の相互相関関数の集合体のうち最大値の相互相関関数に関連付けられたシフト量ｂを、候補移動量φｃと決定する。

上記のステップＳ２１及びＳ２２においては、第３ヒストグラム（複数の第１ヒストグラムデータ）及び第４ヒストグラム（複数の第２ヒストグラムデータ）のヒストグラムデータ間隔（第１ヒストグラムデータ間隔、第２ヒストグラムデータ間隔）は、比較的大きなヒストグラムデータ間隔として設定されている。これにより、第３ヒストグラムとシフト後の第４ヒストグラムとの積Ｈ３（φ）＊Ｈ４（φ−ｂ）の算出個数を減少して、１つのシフト量ｂに対する相互相関関数の計算量を減少できる。
このように、１つのシフト量ｂに対する相互相関関数の算出に必要な計算量を減少することにより、シフト量ｂのとりうる範囲が広く多数の相互相関関数を算出する必要があっても、候補移動量φｃの算出のために必要な全体の計算量を、最初からヒストグラムデータ間隔が小さいヒストグラムのヒストグラムマッチングにより第１移動量を算出する場合と比較して減少できる。

上記のように算出された候補移動量φｃは、実際の第１移動量に近い値として算出されている一方、ヒストグラムデータ間隔の大きなヒストグラムを用いたヒストグラムマッチングにより算出されているので、実際の第１移動量と比較すると誤差を含んでいる。
従って、候補移動量φｃを算出後、第１移動量算出部５３１２は、候補移動量φｃを、よりヒストグラムデータ間隔の小さいヒストグラムを用いたヒストグラムマッチングにより補正して、補正移動量を算出する。

具体的には、まず、位置データ記憶部５３１１に記憶されている角度θについての複数の第１位置データから、上記のヒストグラムデータ間隔Ｍ１（第１ヒストグラムデータ間隔）よりも小さなヒストグラムデータ間隔Ｍ２（第３ヒストグラムデータ間隔の一例）を有する第５ヒストグラムＨ５（φ）を生成する。
また、位置データ記憶部５３１１に記憶されている角度θについての複数の第２位置データから、上記のヒストグラムデータ間隔Ｍ１（第２ヒストグラムデータ間隔）よりも小さなヒストグラムデータ間隔Ｍ２（第４ヒストグラムデータ間隔の一例）を有する第６ヒストグラムＨ６（φ）を生成する（ステップＳ２３）。

例えば、ヒストグラムデータ間隔Ｍ２をｒ_２＊ｍ（ｒ_２：整数）と定義したとすると、ｒ_２の絶対値をｒ_１の絶対値よりも小さな値として、上記のステップＳ２１にて説明したのと同様の方法により、位置データ記憶部５３１１に記憶されている角度θについての複数の第１位置データから、第５ヒストグラムＨ５（φ）のための複数の第３ヒストグラムデータを生成する。
また、上記のｒ_２の絶対値をｒ_１の絶対値よりも小さな値として、上記のステップＳ２１にて説明したのと同様の方法により、位置データ記憶部５３１１に記憶されている角度θについての複数の第２位置データから、第６ヒストグラムＨ６（φ）のための複数の第４ヒストグラムデータを生成する。

第５ヒストグラムＨ５（φ）及び第６ヒストグラムＨ６（φ）を生成後、第１移動量算出部５３１２は、第５ヒストグラムＨ５（φ）を第１ヒストグラムに対応するものとし、第６ヒストグラムＨ６（φ）を第２ヒストグラムに対応するものとして、これら２つのヒストグラムのヒストグラムマッチングにより、候補移動量φｃを補正した補正移動量を算出する（ステップＳ２４）。ステップＳ２４においては、候補移動量φｃを中心とした所定の範囲内（−ΔφからΔφまでの範囲）にて第６ヒストグラムをシフトしたときに、第５ヒストグラムと第６ヒストグラムとが最も一致したときの第６ヒストグラムのシフト量から補正移動量を算出する。
具体的には、図１１に示すフローチャートに示す処理の流れに従って、補正移動量を算出する。図１１は、補正移動量の算出方法を示すフローチャートである。

まず、第１移動量算出部５３１２は、第６ヒストグラムＨ６（φ）をステップＳ２２にて算出した候補移動量φｃだけシフトさせて、新たな第６ヒストグラムＨ６（φ−φｃ）を生成する（ステップＳ２４０１）。
具体的には、ステップＳ２３にて生成した複数の第４ヒストグラムデータ中の各角度に候補移動量φｃを加算して新たな複数の第４ヒストグラムデータを作成する。すなわち、ｓ番目の第４ヒストグラムデータの角度−（ｑ−ｓ＊ｒ_２）＊ｍに候補移動量φｃを加算して−（ｑ−ｓ＊ｒ_２）＊ｍ＋φｃとし、ｓ番目の第４ヒストグラムデータの出現頻度を元の出現頻度Ｆ_ｓのままとすることにより、候補移動量φｃだけシフトしたあとの第６ヒストグラムＨ６（φ−φｃ）のための新たな複数の第４ヒストグラムデータを生成できる。

次に、第１移動量算出部５３１２は、第５ヒストグラムとシフト後の第６ヒストグラムとの相互相関関数を算出する（ステップＳ２４０２）。具体的には、第５ヒストグラムＨ５（φ）とシフト量ｃだけシフトした第６ヒストグラムＨ６（φ−φｃ−ｃ）との積Ｈ５（φ）＊Ｈ６（φ−φｃ−ｃ）を、とりうるφの値（第３ヒストグラムデータの個数分又は第４ヒストグラムデータの個数分）だけ加算した値を、１つのシフト量ｃに対する相互相関関数として算出する。その後、算出した相互相関関数を当該１つのシフト量ｃと関連付けて、記憶部５１などに記憶する。

相互相関関数をある１つのシフト量ｃに対して算出後、第１移動量算出部５３１２は、シフト量ｃを−ΔφからΔφまでの範囲で変化させて相互相関関数を算出したか否かを判断する（ステップＳ２４０３）。
候補移動量φｃだけシフトした後の新たな第６ヒストグラムＨ６（φ−φｃ）は、第５ヒストグラムＨ５（φ）からわずかにずれているだけであるので、ステップＳ２４０３においては、上記の第６ヒストグラムのシフト量ｃの変化量（２Δφ）を、候補移動量φｃを算出する際の第４ヒストグラムのシフト量ｂの変化量（例えば、３６０°）よりも小さな値に設定する。

現在のシフト量ｃが−Δφよりも大でかつ＋Δφよりも小であり、相互相関関数を算出すべきシフト量ｃがまだ存在していると判断した場合（ステップＳ２４０３において「Ｎｏ」の場合）、第１移動量算出部５３１２がシフト量ｃの値を変更後（ステップＳ２４０４）、補正移動量の算出プロセスは、ステップＳ２４０２に戻る。これにより、新たなシフト量ｃに対して相互相関関数が新たに算出される。
シフト量ｃが−Δφから＋Δφまで変化するまで上記のステップＳ２４０２〜Ｓ２４０４を繰り返し実行することにより、第１移動量算出部５３１２は、−Δφから＋Δφまでの範囲の各シフト量ｃと相互相関関数とが関連付けられた、相互相関関数の集合体を算出できる。

一方、−Δφから＋Δφまでの範囲の全てのシフト量ｃに対して相互相関関数を算出したと判断した場合（ステップＳ２４０３において「Ｙｅｓ」の場合）、第１移動量算出部５３１２は、補正移動量の算出を開始する（ステップＳ２４０５）。
具体的には、まず、第１移動量算出部５３１２は、算出された相互相関関数の集合体のうち最大値の相互相関関数に関連付けられているシフト量ｃを、候補移動量φｃを実際の第１移動量に近づけるための補正量（φ_ｄとする）とする。すなわち、補正量φ_ｄは、候補移動量φｃだけシフトした新たな第６ヒストグラムＨ６（φ−φｃ）を第５ヒストグラムＨ５（φ）と一致させるようさらにシフトした時のシフト量であるといえる。
次に、第１移動量算出部５３１２は、候補移動量φｃに上記の補正量φ_ｄを加えて、補正移動量をφｃ＋φ_ｄと算出する。

上記のステップＳ２４においては、ステップＳ２２にて算出された候補移動量φｃを、ステップＳ２２におけるヒストグラムデータ間隔（第１ヒストグラムデータ間隔、第２ヒストグラムデータ間隔）よりも小さいヒストグラムデータ間隔（第３ヒストグラムデータ間隔、第４ヒストグラムデータ間隔）のヒストグラム（複数のヒストグラムデータ）を用いて算出された補正量φ_ｄを用いて補正して、補正移動量を算出している。これにより、実際の第１移動量により近い精度の高い補正移動量を算出できる。
ステップＳ２４においては、ヒストグラムデータ間隔の小さなヒストグラムを用いることにより、１つのシフト量ｃに対する相互相関関数の計算量が、ステップＳ２２における１つのシフト量ｂに対する相互相関関数の計算量よりも増加している。その一方で、ステップＳ２４においては、候補移動量φｃだけシフトした新たな第６ヒストグラムを用いてヒストグラムマッチングが実行されている。すなわち、ステップＳ２４においては、シフト量ｃの変化範囲を小さくして、相互相関関数の算出数（算出回数）を減少している。これにより、１つのシフト量ｃに対する相互相関関数の計算量が増加しても、相互相関関数の算出数を減少して、全体として補正移動量の計算量を減少できる。

上記のステップＳ２４０１〜Ｓ２４０５を実行して補正移動量を算出後、第１移動量算出部５３１２は、補正移動量（補正量φ_ｄ）の算出が所定回数実行されたか否かを判断する（ステップＳ２５）。具体的には、第１移動量算出部５３１２は、上記のステップＳ２３〜Ｓ２４が所定回数実行されたか否かを判断する。
補正移動量の算出が所定回数実行されていないと判断した場合（ステップＳ２５において「Ｎｏ」の場合）、第１移動量の算出プロセスはステップＳ２３に戻り、第１移動量算出部５３１２は、上記のステップＳ２３〜Ｓ２４を所定回数だけ繰り返し実行する。

ステップＳ２３〜Ｓ２４を複数回繰り返して実行する場合、上記のステップＳ２３において、第１移動量算出部５３１２は、今回の補正量φ_ｄを算出する際に用いる第５ヒストグラム（複数の第３ヒストグラムデータ）のヒストグラムデータ間隔（第３ヒストグラムデータ間隔）及び第６ヒストグラム（複数の第４ヒストグラムデータ）のヒストグラムデータ間隔（第４ヒストグラムデータ間隔）を、それぞれ、前回の補正量φ_ｄを算出した際に用いた第５ヒストグラムのヒストグラムデータ間隔（第３ヒストグラムデータ間隔）及び第６ヒストグラムのヒストグラムデータ間隔（第４ヒストグラムデータ間隔）よりも小さくする。

例えば、前回（（ｈ−１）回目）の第５ヒストグラム及び第６ヒストグラムのヒストグラムデータ間隔Ｍ（ｈ−１）をｒ_ｈ−１＊ｍ（ｒ_ｈ−１：整数）とした場合には、今回（ｈ回目）の補正移動量の算出を実行する際に、ｒ_ｈ−１の絶対値よりも絶対値が小さいｒ_ｈ（ｒ_ｈ：整数）を用いて、ヒストグラムデータ間隔Ｍｈをｒ_ｈ＊ｍと定義する。その後、上記のステップＳ２１にて説明したのと同様の方法により、今回の算出に用いるヒストグラムデータ間隔Ｍｈを有する第５ヒストグラム（複数の第３ヒストグラムデータ）、及び、ヒストグラムデータ間隔Ｍｈを有する第６ヒストグラム（複数の第４ヒストグラムデータ）を、それぞれ、複数の第１位置データ及び複数の第２位置データを用いて生成する。

上記のように、今回の補正量φ_ｄの算出に用いる第５ヒストグラムの第３ヒストグラムデータ間隔及び第６ヒストグラムの第４ヒストグラムデータ間隔を、それぞれ、前回の補正量の算出に用いた第５ヒストグラムの第３ヒストグラムデータ間隔及び第６ヒストグラムの第４ヒストグラムデータ間隔よりも小さくすることにより、より正確な（精度のよい）補正移動量を段階的に算出できる。

また、ステップＳ２３〜Ｓ２４を複数回繰り返して実行する場合、上記のステップＳ２４０１において、第１移動量算出部５３１２は、第６ヒストグラムＨ６（φ）を前回算出された補正移動量だけシフトさせた第６ヒストグラムと、第５ヒストグラムＨ５（φ）とのヒストグラムマッチングを実行する。これにより、第１移動量算出部５３１２は、前回算出された補正移動量に補正量φ_ｄをさらに加えて新たな補正移動量を算出できる。

一方、補正移動量の算出が所定回数実行されたと判断した場合（ステップＳ２５において「Ｙｅｓ」の場合）、第１移動量算出部５３１２は、当該所定回数目に算出された最新の補正移動量を第１移動量と決定し（ステップＳ２６）、第１移動量の算出プロセスを終了する。

上記のように補正量φ_ｄの算出を複数回繰り返して（例えば、ｎ回繰り返したとする）最終的に算出される補正移動量は、ｋ回目のヒストグラムマッチング（補正量の算出）において算出されたシフト量ｃ（補正量）をφ_ｄｋ（ｋ＝１、２、・・・ｎ）とすると、φｃ＋φ_ｄ１＋φ_ｄ２＋・・・＋φ_ｄｎとなる。すなわち、最終的に算出される補正移動量は、候補移動量φｃに、各補正量の算出段階において算出された複数の補正量φ_ｄｋがすべて加算されることにより算出される。

このように、ステップＳ２３〜Ｓ２４を複数回繰り返して実行することにより、前回算出された補正移動量を前回よりもヒストグラムデータ間隔の小さいヒストグラムのヒストグラムマッチングにより補正して、実際の第１移動量により近い補正移動量を最終的に算出できる。

上記のステップＳ２１〜Ｓ２６を実行して段階的に第１移動量を算出することにより、 第１移動量の候補である候補移動量φｃを少ない計算量で算出できるとの効果と、ヒストグラムデータ間隔がより小さいヒストグラムを用いて精度よく補正移動量を算出しつつ相互相関関数の算出数を減らして補正移動量（補正量φ_ｄ）に必要な計算量を減少できるとの効果とが相乗効果となり、ヒストグラムデータ間隔が小さいヒストグラムのヒストグラムマッチングを１回のみ実行して第１移動量を算出する場合と比較して、短時間に、かつ、ヒストグラムマッチングを１回のみ実行して算出された第１移動量と同等の精度にて第１移動量を算出できる。

実際に、以下の算出工程（ｉ）〜（ｉｖ）にて第１移動量の算出を行った場合の第１移動量の算出時間は、同じ複数の第１位置データ及び第２位置データから生成されたヒストグラムデータ間隔が０．１である２つのヒストグラムのヒストグラムマッチングを１回だけ実行して第１移動量を算出した場合の算出時間の９％程度であった。また、以下の算出工程（ｉ）〜（ｉｖ）にて算出された第１移動量と実際の第１移動量との間のずれは、ヒストグラムデータ間隔が０．１であるヒストグラムのヒストグラムマッチングを１回だけ実行して算出された第１移動量と実際の第１移動量との間のずれと同程度であった。
（ｉ）複数の第１位置データからヒストグラムデータ間隔Ｍ１が１．０（例えば、ｍ＝０．１、ｒ_１＝１０）である第３ヒストグラム（複数の第１ヒストグラムデータ）を、複数の第２位置データからヒストグラムデータ間隔Ｍ１が１．０である第４ヒストグラム（複数の第２ヒストグラムデータ）を生成。
（ｉｉ）（ｉ）にて生成された第３ヒストグラムと第４ヒストグラムとのマップマッチングにより、候補移動量φｃを算出。
（ｉｉｉ）複数の第１位置データからヒストグラムデータ間隔Ｍ２が０．１（例えば、ｍ＝０．１、ｒ_２＝１）である第５ヒストグラムを、複数の第２位置データからヒストグラムデータ間隔Ｍ２が０．１である第６ヒストグラムを生成。
（ｉｖ）（ｉｉｉ）にて生成された第５ヒストグラムと、（ｉｉ）にて算出された候補移動量φｃだけシフトされた新たな第６ヒストグラムとのマップマッチングにより、補正量（補正移動量）を算出。本工程において算出された補正移動量を第１移動量とする。

３．他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。

（Ａ）走行部の他の実施形態
上記の第１実施形態における走行部２は、二輪差動型の走行部であった。しかし、これに限られず、オムニホイールなどの他の車輪を有する走行部を用いてもよい。また、走行部２は車輪２１ａ、２１ｂの回転により移動するものに限られず、車輪以外の他のアクチュエータの動きにより移動可能なものであってもよい。

（Ｂ）移動量推定部の他の実施形態
上記の第１実施形態においては、移動量推定部５３は、位置データ取得部３にて取得した位置データのみを用いて、自律移動体１００の移動量を算出していた。しかし、これに限られず、移動量推定部５３は、車輪２１ａ、２１ｂの回転量に基づいても、自律移動体１００の移動量を算出可能であってもよい。

この場合、移動量推定部５３は、例えば、モータ２３ａ、２３ｂのエンコーダなどからのパルス信号に含まれるパルス数から、車輪２１ａ、２１ｂの回転量を算出できる。これにより、例えば、自律移動体１００が、自律移動体１００の周囲に壁や障害物などの物体がないために位置データの取得が不可能な場合や、直線的な壁により形成された直線的な通路を通過中の場合など複数の位置データにより形成される投影物体像に時間的な変化があまりないような場合に、移動量推定部５３は、車輪２１ａ、２１ｂの回転に基づいて、自律移動体１００の移動量を算出できる。

また、上記の車輪２１ａ、２１ｂの回転量に基づく移動量は、上記の第１移動量及び／又は第２移動量を用いて補正されてもよい。これにより、例えば、車輪２１ａ、２１ｂの使用により車輪２１ａ、２１ｂの径が変化した場合でも、車輪２１ａ、２１ｂの回転量に基づいて正確に自律移動体１００の移動量を算出できる。

さらに、上記の第１実施形態においては、第１移動量算出部５３１２が自律移動体１００の移動前後の姿勢角度差を算出し、第２移動量算出部５３１４が自律移動体１００の移動前後の平行移動量を算出していた。しかし、これに限られず、第１移動量算出部５３１２が姿勢角度差に加えて、自律移動体１００の平行移動量も算出してもよい。これにより、移動量推定部５３における移動量の算出速度を高めることができる。

また、第２移動量算出部５３１４は、平行移動量に加えて姿勢角度差（回転移動量）を算出してもよい。これにより、最終的な姿勢角度差の算出を行い、より精度よく姿勢角度差を算出できる。

（Ｃ）第２移動量の算出方法の他の実施形態
上記の第１実施形態においては、第２移動量算出部５３１４は、複数の位置データの全てを用いて第２移動量を算出していた。しかし、これに限られず、複数の位置データを用いて移動量を算出する際、複数の位置データを、予め決められた条件に基づいて決定されるいくつかのグループに分類し、各グループに含まれる位置データを用いて多段的に第２移動量の算出を行ってもよい。これにより、第２移動量を算出するために必要な計算量を減少できる。

（Ｄ）本体部の他の実施形態
上記の第１実施形態に係る自律移動体１００の本体部１には、位置データ取得部３の位置データ検出可能領域に干渉するような部材は特に取り付けられていなかった。しかし、これに限られず、本体部１に、位置データ取得部３の位置データ検出可能領域に少なくとも一部が入るように、既知の形状の干渉部材を取り付けてもよい。

本体部１に、少なくとも一部が位置データ検出可能領域に入るように干渉部材を取り付けることにより、位置データ取得部３が正常に動作しているかどうかを確認できる。例えば、自律移動体１００の周囲に壁や障害物などの物体が存在しない場合などに、干渉部材の一部の形状に対応する投影物体像が複数の位置データのいくつかにより形成されていれば、位置データ取得部３の異常により位置データが取得されないのではなく、自律移動体１００が、物体が存在しない広い空間を移動中であると判断できる。

（Ｅ）補正量の算出に用いるヒストグラムの他の実施形態（その１）
上記の第２実施形態において、補正量の算出に用いる第５ヒストグラム及び第６ヒストグラムのヒストグラムデータ間隔は、最小でも０．１であった。補正量の算出に用いるヒストグラムのヒストグラムデータ間隔は、候補移動量φｃの算出に用いたヒストグラムのヒストグラムデータ間隔よりも小さいヒストグラムデータ間隔を有していればよいので、補正量の算出に用いるヒストグラムのヒストグラムデータ間隔の最小値は０．１に限られない。
例えば、補正量の算出に用いるヒストグラムのヒストグラムデータ間隔を０．０５としてもよい。これにより、ヒストグラムデータ間隔が０．１であるヒストグラムを用いて１回のみのヒストグラムマッチングにより算出した第１移動量よりも実際の第１移動量により近い補正移動量（第１移動量）を、ヒストグラムデータ間隔が０．１であるヒストグラムを用いて１回のみのヒストグラムマッチングにより第１移動量を算出した場合と同程度の算出時間にて算出できる。

（Ｆ）補正量の算出に用いるヒストグラムの他の実施形態（その２）
上記の第２実施形態において、複数の第１位置データから生成されるヒストグラムのヒストグラムデータ間隔と、複数の第２位置データから生成されるヒストグラムのヒストグラムデータ間隔は同一としていた。しかし、これに限られず、複数の第１位置データから生成されるヒストグラムの形状（プロファイル）と、複数の第２位置データから生成されるヒストグラムの形状（プロファイル）とが大きく異なってしまわない範囲で、これらのヒストグラムデータ間隔を異ならせてもよい。

（Ｇ）第６ヒストグラムのシフト量についての他の実施形態
上記の第２実施形態において、補正量を算出する際の第６ヒストグラムのシフト範囲（シフト量ｃの変化範囲）は、補正量の算出回数によらず一定としていた。補正量を算出する際の第６ヒストグラムのシフト範囲は、算出された候補移動量φｃ及び補正移動量と実際の第１移動量との間の予測されるずれの範囲内で決定できるので、上記に限られない。
例えば、補正量を算出する際の第６ヒストグラムのシフト範囲を、補正量の算出回数毎に減少してもよい。これにより、補正量の算出精度を維持しつつ、補正量の算出時間を短縮できる。

または、上記とは逆に、第６ヒストグラムのシフト範囲を、補正量の算出回数毎に増加してもよい。これにより、上記の第２実施形態に示した方法により算出される第１移動量のロバスト性を向上できる。

（Ｈ）候補移動量についての他の実施形態
上記の第２実施形態において、候補移動量φｃは、第３ヒストグラムＨ３（φ）と第４ヒストグラムＨ４（φ）との一致度が最も高いときの、第４ヒストグラムＨ４（φ）のシフト量としていた。すなわち、上記の第２実施形態において算出される候補移動量φｃは、上記の２つのヒストグラムが最も一致する場合の第４ヒストグラムＨ４（φ）のシフト量１つのみであった。
候補移動量φｃは、実際の第１移動量に近い移動量として算出されればよいので、候補移動量φｃの数は１つに限られない。
例えば、２つのヒストグラムが最も一致する場合の第４ヒストグラムＨ４（φ）のシフト量に加えて、第３ヒストグラムＨ３（φ）と第４ヒストグラムＨ４（φ）との一致度が２番目に高いときの第４ヒストグラムＨ４（φ）のシフト量、３番目に高いときのシフト量、・・・など複数のシフト量を、複数の候補移動量として算出してもよい。

また、移動前後での車輪２１ａ、２１ｂ（モータ２３ａ、２３ｂ）の回転量に基づいて算出される移動量（デッドレコニングによる移動量、又は、車輪オドメトリによる移動量）を、候補移動量としてもよい。車輪２１ａ、２１ｂの回転量に基づいた候補移動量は、例えば、自律移動体１００の移動前と移動後との間に、モータ２３ａ、２３ｂの出力回転軸に取り付けられたエンコーダから出力されるパルス数から算出できる。

上記のように複数の候補移動量を算出した場合には、複数の候補移動量のそれぞれに対して、上記の第２実施形態にて説明したのと同様の方法にて、複数の補正移動量が算出される。
算出された複数の補正移動量のうちいずれを第１移動量とするかは、例えば、最終段階の補正移動量の算出において算出された複数の補正移動量のうち、第５ヒストグラムと第６ヒストグラムとの一致度が最も高くなった補正移動量を、第１移動量とできる。

または、複数の補正移動量の全てを複数の第１移動量として、当該複数の第１移動量に基づいて算出される複数の移動量を、位置推定部５５における自律移動体１００の現在位置の推定に用いてもよい。

上記のように、複数の候補移動量を算出し、その中から適切な候補移動量を選択することにより、実際の第１移動量により近い移動量を最終的に算出できる。この結果、より精度よく第１移動量を算出できる。

（Ｉ）第１移動量算出部の他の実施形態
上記の第２実施形態の第１移動量算出部５３１２は、第１実施形態の第２移動量算出部５３１４と第３移動量算出部５３１５と組み合わされていた。しかし、これに限られず、第２実施形態の第１移動量算出部５３１２は、移動量推定部５３（移動量推定装置）などの移動量の推定を行う装置において、単独で備わっていてもよい。
または、上記の位置データを用いた移動量の推定を行う一般的な移動量推定装置に、第２実施形態において説明した第１移動量の算出方法を適用してもよい。

本発明は、自身の位置を推定しながら移動可能な移動体に広く適用できる。

１００ 自律移動体
１ 本体部
２ 走行部
２１ａ、２１ｂ 車輪
２３ａ、２３ｂ モータ
３ 位置データ取得部
３１ 前方データ取得部
３３ 後方データ取得部
５ 制御部
５１ 記憶部
５３ 移動量推定部
５３１ 移動量算出部
５３１１ 位置データ記憶部
５３１２ 第１移動量算出部
５３１３ 位置データ移動部
５３１４ 第２移動量算出部
５３１５ 第３移動量算出部
５５ 位置推定部
５７ 走行制御部
７ 操作部
７１ａ、７１ｂ 操作ハンドル
８ 補助輪部
８ａ、８ｂ 補助車輪
９ 取付部材
Ｈ１１（θ） 第１ヒストグラム
Ｈ２１（θ） 第２ヒストグラム
Ｈ３（φ） 第３ヒストグラム
Ｈ４（φ） 第４ヒストグラム
Ｈ５（φ） 第５ヒストグラム
Ｈ６（φ） 第６ヒストグラム
θ 角度
φ 変数
φｃ 候補移動量
φ_ｄ、φ_ｄｋ 補正量
ｍ 最小ヒストグラムデータ間隔
Ｍ１、Ｍ２ ヒストグラムデータ間隔
Ｆ_１、Ｆ_２、・・・、Ｆ_ｓ 出現頻度
Ａ、ｂ、ｃ シフト量

Claims (11)

1. 移動体の移動量を推定する移動量推定装置であって、
   前記移動体の周囲に存在する物体を所定の座標上に投影した投影物体像を形成する複数の位置データを取得する位置データ取得部と、
   前記移動体の移動前又は移動後の一方において取得した複数の第１位置データの所定の座標軸における座標値から生成される第１ヒストグラムと、前記移動前又は前記移動後の他方において取得した複数の第２位置データの前記所定の座標軸における座標値から生成される第２ヒストグラムとが最も一致したときの前記第２ヒストグラムの移動量を、第１移動量として算出する第１移動量算出部と、
   前記複数の第２位置データを前記第１移動量に対応する移動量だけ移動して複数の第１移動位置データを算出する位置データ移動部と、
   前記複数の第１位置データにより形成される第１投影物体像と前記複数の第１移動位置データを平行移動及び／又は回転移動して算出される複数の第２移動位置データにより形成される第２移動投影物体像との間の距離と所定の閾値との比較に基づいて、前記第１投影物体像と前記第２移動投影物体像とが一致するかどうかを判断し、前記第１投影物体像と前記第２移動投影物体像とが一致すると判断したときの前記複数の第２移動位置データを算出したときの前記複数の第１移動位置データの移動量を、第２移動量として算出する第２移動量算出部と、
   前記第１移動量と前記第２移動量とを積算して、前記移動体の移動前後の移動量を算出する第３移動量算出部と、
   を備える移動量推定装置。
2. 前記第１移動量算出部は、前記移動前の姿勢角と前記移動後の姿勢角との姿勢角度差を前記第１移動量として算出する、請求項１に記載の移動量推定装置。
3. 前記第２移動量算出部は、前記複数の第２移動位置データの前記複数の第１移動位置データからの平行移動の移動量を前記第２移動量として算出する、請求項１又は２に記載の移動量推定装置。
4. 前記第２移動量算出部は、前記複数の第２移動位置データの前記複数の第１移動位置データからの平行移動の移動量及び回転移動の移動量を前記第２移動量として算出する、請求項１〜３のいずれかに記載の移動量推定装置。
5. 前記第１移動量算出部は、前記複数の第１位置データから生成されヒストグラムデータ間隔が第１ヒストグラムデータ間隔である第３ヒストグラムを前記第１ヒストグラムとし、前記複数の第２位置データから生成されヒストグラムデータ間隔が第２ヒストグラムデータ間隔である第４ヒストグラムを前記第２ヒストグラムとして用いて、前記第１移動量の候補である候補移動量を算出し、
   その後、前記複数の第１位置データから生成され前記第１ヒストグラムデータ間隔よりも小さな第３ヒストグラムデータ間隔を有する第５ヒストグラムを前記第１ヒストグラムとし、前記複数の第２位置データから生成した前記第２ヒストグラムデータ間隔よりも小さな第４ヒストグラムデータ間隔を有するヒストグラムを前記候補移動量だけシフトすることにより生成された第６ヒストグラムを前記第２ヒストグラムとして用いて算出された補正移動量を前記第１移動量とする、
   請求項１〜４のいずれかに記載の移動量推定装置。
6. 前記補正移動量は、前記第５ヒストグラムと前記第６ヒストグラムとが最も一致したときの前記第６ヒストグラムのシフト量である補正量を前記候補移動量に加えることにより算出される、
   請求項５に記載の移動量推定装置。
7. 前記第１移動量算出部は前記補正量の算出を複数回実行し、
   前記補正移動量は、前記候補移動量に複数の前記補正量を加算することにより算出される、請求項６に記載の移動量推定装置。
8. 前記補正量の算出を複数回実行する場合、前記第１移動量算出部は、今回の前記補正量の算出における前記第３ヒストグラムデータ間隔及び前記第４ヒストグラムデータ間隔を、それぞれ、前回の前記補正量の算出における前記第３ヒストグラムデータ間隔及び前記第４ヒストグラムデータ間隔よりも小さくする、請求項７に記載の移動量推定装置。
9. 請求項１〜８のいずれかに記載の移動量推定装置と、
   走行部と、
   前記移動量推定装置において推定される移動量に基づいて現在位置を推定する位置推定部と、
   前記現在位置から所定の目標位置まで走行するよう前記走行部を制御する走行制御部と、
   を備える自律移動体。
10. 移動体の移動量の推定方法であって、
    前記移動体の移動前及び移動後に、前記移動体の周囲に存在する物体を所定の座標上に投影した投影物体像を形成する複数の位置データを取得するステップと、
    前記移動前又は前記移動後の一方において取得した複数の第１位置データの所定の座標軸における座標値から生成される第１ヒストグラムと、前記移動前又は前記移動後の他方において取得した複数の第２位置データの前記所定の座標軸における座標値から生成される第２ヒストグラムとが最も一致したときの前記第２ヒストグラムの移動量を第１移動量として算出するステップと、
    前記複数の第２位置データを前記第１移動量に対応する移動量だけ移動して複数の第１移動位置データを算出するステップと、
    前記複数の第１位置データにより形成される第１投影物体像と前記複数の第１移動位置データを平行移動及び／又は回転移動して算出される複数の第２移動位置データにより形成される第２移動投影物体像との間の距離と所定の閾値との比較に基づいて、前記第１投影物体像と前記第２移動投影物体像とが一致するかどうかを判断し、前記第１投影物体像と前記第２移動投影物体像とが一致すると判断したときの前記複数の第２移動位置データを算出したときの前記複数の第１移動位置データの移動量を、第２移動量として算出するステップと、
    前記第１移動量と前記第２移動量とを積算して、前記移動体の移動前後の移動量を算出するステップと、
    を含む移動量の推定方法。
11. 前記第１移動量を算出ステップは、
    前記複数の第１位置データから生成されヒストグラムデータ間隔が第１ヒストグラムデータ間隔である第３ヒストグラムを前記第１ヒストグラムとし、前記複数の第２位置データから生成されヒストグラムデータ間隔が第２ヒストグラムデータ間隔である第４ヒストグラムを前記第２ヒストグラムとして用いて、前記第１移動量の候補である候補移動量を算出するステップと、
    前記複数の第１位置データから生成され前記第１ヒストグラムデータ間隔よりも小さな第３ヒストグラムデータ間隔を有する第５ヒストグラムを前記第１ヒストグラムとし、前記複数の第２位置データから生成した前記第２ヒストグラムデータ間隔よりも小さな第４ヒストグラムデータ間隔を有するヒストグラムを前記候補移動量だけシフトすることにより生成された第６ヒストグラムを前記第２ヒストグラムとして用いて算出される補正移動量を、前記第１移動量とするステップと、
    を含む、請求項１０に記載の移動量の推定方法。

Images