JP2014219721A - 自律移動体 - Google Patents

Abstract

【課題】自律移動体において、環境地図の環状経路問題を解決するとともに、環境地図の分割を自動化することを可能にする。
【解決手段】自律移動体１は、教示走行モードにおいて、操作者から入力された走行命令からモータ９３，９４の制御量を出力し、環境地図上の自己位置を推定し、自機の周囲における障害物の位置情報を取得し、障害物の位置情報を取得した時刻に対応させて障害物の位置情報を環境地図復元用データとして記憶部２５に記憶させ、走行スケジュールを作成して記憶部２５に記憶させる。自律移動体１は、再現走行モードにおいて、環境地図上の自己位置を推定し、自機の周囲における障害物の位置情報を取得し、推定された自己位置に対応する環境地図復元用データを読み出して環境地図を更新し、更新された環境地図上を走行スケジュールに沿って走行するようにモータ９３，９４の制御量を作成して走行部２７に入力する。
【選択図】図２

Description

本発明は、環境地図作成機能、経路計画機能、及び自律移動機能を有する自律移動体に関する。

従来から、周囲環境の中を自律して移動する自律移動体が知られている。自律移動体が周囲環境の中を自律して移動するためには、移動空間内の物体（以下、障害物と称す）が存在する領域と存在しない領域とを表した環境地図が必要となる。このような環境地図の取得方法については、種々の方法が考案されているが、近年、移動しつつ、リアルタイムで自己位置の推定と環境地図の作成とを行う技術としてＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）が注目されている。ＳＬＡＭを利用して、レーザレンジファインダ（ＬＲＦ：Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）又はカメラによる距離計測の結果得られた地形データを用いて環境地図を生成する移動ロボットが提案されている。

ＳＬＡＭを利用して環境地図を作成する方法では、計測誤差が累積することにより、環状の環境地図を作成する場合に、開始部分と終了部分とが一致しない場合が生じる（いわゆる、環状経路問題）。
特許文献１では、このような環状経路問題が発生しないように、環境地図を複数の部分地図に分割することを提案している。

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

特許文献１に記載された自律移動体では、環境地図を構成する複数の部分地図を作成する際に、複数の連結点を設定し、設定した連結点の中から連結する部分地図の連結点を選択して連結関係を設定する。
この場合、環境地図をどのように分割して部分地図を作成するかの計画を立てることが難しく、自動化することが困難である。

本発明の課題は、自律移動体において、環境地図の環状経路問題を解決することを可能にすることにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。

本発明の一見地による自律移動体は、操作者の手動操作により、走行開始位置から走行終了位置まで手動走行させながら、通過時刻と前記通過時刻における通過点データの集合でなる走行スケジュールを作成する教示走行モードと、走行スケジュールを再現しながら、走行開始位置から走行終了位置まで自律的に走行する再現走行モードとを実行する。
自律移動体は、記憶部、走行部、教示データ作成部、走行制御部、障害物情報取得部、地図作成部、自己位置推定部を備える。記憶部は、データを記憶する。走行部は、制御量が入力される複数のアクチュエータを有する。教示データ作成部は、走行スケジュールを作成する。走行制御部は、走行命令を解釈してアクチュエータに入力する制御量を作成し走行部に入力する。障害物情報取得部は、自機の周囲に存在する障害物の位置情報を取得する。地図作成部は、障害物情報取得部により取得した障害物の位置情報から環境地図を作成する。自己位置推定部は、環境地図上の自己位置を推定する。

自律移動体は、教示走行モードにおいて、所定のタイムスケジュール毎に、（ａ）走行制御部が、操作者から入力された走行命令からアクチュエータの制御量を作成して走行部に出力し、（ｂ）自己位置推定部が、環境地図上の自己位置を推定し、（ｃ）障害物情報取得部が、自機の周囲における障害物の位置情報を取得し、（ｄ）地図作成部が、障害物の位置情報を取得した時刻に対応させて障害物の位置情報を環境地図復元用データとして記憶部に記憶させ、（ｅ）教示データ作成部が、走行スケジュールを作成して記憶部に記憶させる。地図作成部は、作成した環境地図復元用データを、自己位置推定部により推定された環境地図上の自己位置とともに、記憶部に記憶することができる。
また、自律移動体は、再現走行モードにおいて、所定のタイムスケジュール毎に、（ｆ）自己位置推定部が、環境地図上の自己位置と時刻とを推定し、（ｇ）障害物情報取得部が、自機の周囲における障害物の位置情報を取得し、（ｈ）地図作成部が、推定された自己位置と時刻に対応する環境地図復元用データを読み出して環境地図を更新し、（ｉ）走行制御部が、更新された環境地図上を走行スケジュールに沿って走行するようにアクチュエータの制御量を作成して走行部に入力する。地図作成部は、環境地図を更新する際に、教示モードの時刻ｔ地点における自己位置（ｘ，ｙ）周辺領域の環境地図を復元し、現在の自己位置（ｘ１，ｙ１）から一定の距離だけ離れた領域にある環境地図を削除することができる。

この自律移動体では、操作者が手動操作により自律移動体を操作することで、環境地図復元用データを生成して記憶部に格納している。また、自律移動体は、再現走行モードにおいて、環境地図上の自己位置及び時刻を推定して、推定された自己位置及び時刻に対応する環境地図復元用データを用いて環境地図を更新しながら、走行スケジュールに沿って走行駆動している。このことから、操作性を容易にし、走行経路が環状である場合であっても、操作者が環境地図の作成方法を考慮する必要がないので、車輪のスリップやセンサの計測誤差による環状経路が閉じない問題を解決することができる。

ここで、教示走行モードにおいて、教示データ作成部は、所定のタイムスケジュール毎に、環境地図上の自己位置と姿勢とを通過点データとして、その時刻に対応させることができる。
この場合、自律移動体は、環境地図上の自己位置と姿勢に基づいて、教示モードにおいて作成された走行スケジュールを再現することにより、再現走行モードにおける自律走行を正確に行うことが可能となる。

再現走行モードにおいて、地図作成部は、所定のタイムスケジュール毎に、現在の通過点よりも先の時刻に対応する環境地図復元用データを読み出して、これに基づいて既に作成した環境地図を更新することができる。
この場合、自律移動体の環境地図上における自己位置から推定した時刻に基づいて、環境地図復元用データを読み出して、これに基づいて環境地図を更新していることから、車輪のスリップやセンサの計測誤差などによる自己位置のずれを補正することができる。

自律移動体は、障害物情報取得部により取得された障害物の位置情報に基づいて局所地図を作成し、更新された環境地図と作成された局所地図とを比較するマップマッチング部をさらに備えることができる。ここで、地図作成部は、マップマッチング部の比較結果に応じて、環境地図上の自己位置と姿勢及び環境地図上の自己位置に対応する走行スケジュール中の通過点データを推定することができる。
この場合、自律移動体は、推定された自己位置に基づいて走行スケジュール中の通過点データを推定して、自己位置を補正することができる。

自律移動体において、地図作成部は、所定のタイムスケジュール毎に、環境地図のうち既に通過済みの領域であって、推定した自己位置（ｘ，ｙ，θ）から一定の距離が離れた領域の環境地図を削除することができる。
この場合、走行経路が環状である場合に、車輪のスリップやセンサの計測誤差の累積により生じる環状経路が閉じない問題を解消して、再現走行モード中における確実な自律走行を再現できる。

本発明の構成によれば、環境地図の環状経路が閉じない問題を解決するとともに、環境地図の分割を考慮することなく、操作者が容易に走行経路の教示を行うことが可能な自律移動体を提供できる。

一実施形態による自律移動体の概略構成を示すブロック図。 一実施形態による清掃用ロボットの制御ブロック図。 自律移動体の制御動作の概略を示す制御フローチャート。 教示走行モードにおける自律移動体の制御フローチャート。 レーザレンジセンサの走査範囲の一例を示す説明図。 レーザレンジセンサの走査範囲の他の例を示す説明図。 レーザレンジセンサにより検出した障害物の位置情報の一例を示すテーブル。 センサを中心とした座標系でのローカルマップの説明図。 自己位置推定処理の制御フローチャート。 事後確率分布の一例を示す説明図。 事前確率分布の一例を示す説明図。 尤度計算処理の制御フローチャート。 尤度計算処理により得られた尤度分布の一例を示す説明図。 事後確率計算処理により得られた事後確率分布の一例を示す説明図。 グローバルマップ更新処理の概念構成を示す説明図。 走行スケジュールの説明図。 グローバルマップの更新・削除の説明図。 環状の走行経路の説明図。 再現走行モードにおける自律移動体の制御フローチャート。 動的環境地図の更新処理の説明図。 自己位置推定時における尤度計算の説明図。

（１）概略構成
（１−１）構成
以下、図面を参照して本発明の実施形態について詳細に説明する。なお、各図において、同一要素には同一符号を付して重複する説明を省略する。

図１は、本発明の一実施形態による自律移動体の概略構成を示すブロック図である。
自律移動体１は、走行車本体内に設けられる制御部１１、測距センサ３１、操作部３２、表示部３３、走行部２７を備えている。

測距センサ３１は、自律移動体１の走行方向前側にある障害物を検出するセンサである。測距センサ３１は、例えば、レーザ発振器によりパルス発振されたレーザ光を目標物に照射し、目標物から反射した反射光をレーザ受信器により受信することにより、目標物までの距離を算出するレーザレンジファインダ（ＬＲＦ：Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）である。測距センサ３１は、照射するレーザ光を回転ミラーを用いて所定の角度で扇状にレーザ光を操作することができる。このような測距センサ３１は、自律移動体１の後方にも配置することができる。

操作部３２は、自律移動体１を手動操作により走行させる際に、操作者が操作するユーザインターフェイスであって、走行速度の指示を受け付けるスロットル、走行方向の指示を受け付けるハンドルなどを備える。
表示部３３は、現在の走行状況に関する情報、その他の各種情報を表示するものであって、液晶ディスプレイ、ＬＥＤランプなどで構成される。
走行部２７は、走行経路を走行するための複数の走行車輪（図示せず）を備え、これら走行車輪を駆動するためのアクチュエータとしての複数の走行モータ（図示せず）を備えている。

制御部１１は、ＣＰＵ、ＲＡＭ、ＲＯＭを備えるコンピュータであり、所定のプログラムを実行することで走行制御を行う。
制御部１１は、障害物情報取得部２１、地図作成部２２、自己位置推定部２３、教示データ作成部２４、記憶部２５、走行制御部２６を備える。

障害物情報取得部２１は、測距センサ３１の検出信号に基づいて、自機の周囲に存在する障害物の位置情報を取得する。
地図作成部２２は、障害物情報取得部２１により取得した障害物の位置情報に基づいて、自律移動体１の走行経路を含む環境地図をグローバルマップとして作成する。

自己位置推定部２３は、グローバルマップ上の自己位置を推定する。
走行制御部２６は、入力される走行命令を解釈し、アクチュエータの制御量を生成して走行部２７に入力する。
教示データ作成部２４は、通過時刻と通過時刻に対応する通過点データの集合とからなる走行スケジュールを作成する。
記憶部２５は、前述の走行スケジュールと、環境地図復元用データとを記憶する。環境地図復元用データとは、自機の周囲における障害物の位置情報を取得した時刻に対応させてなるデータであり、地図作成部２２によって作成される。

（１−２）教示走行モード
この自律移動体１は、操作者の手動操作に基づいて走行駆動する教示走行モードを実行する。教示走行モードでは、走行開始位置から走行終了位置までの間、操作者の手動操作による走行命令を操作部３２を介して受け付けて、走行制御部２６によりアクチュエータの制御量を生成する。その結果、走行部２７により走行が実行される。
この時、所定のタイムスケジュール毎に、障害物情報取得部２１が、測距センサ３１からの出力信号に基づいて、障害物の位置情報を取得する。
自己位置推定部２３は、所定のタイムスケジュール毎に、走行部２７による移動量に基づいて自己位置及び姿勢を推定し、さらにグローバルマップと障害物情報取得部２１により取得した障害物の位置情報に基づいて、推定した自己位置の補正を行う。
教示データ作成部２４は、グローバルマップ上の自己位置及び姿勢を通過点データとし、それに通過時刻に対応させた走行スケジュールを、記憶部２５に記憶させる。

（１−３）再現走行モード
自律移動体１は、記憶部２５に記憶された走行スケジュールに基づいて、自律走行を行う再現走行モードを実行する。
再現走行モードでは、所定のタイムスケジュール毎に以下の動作が実行される。最初に、自己位置推定部２３がグローバルマップ上の自己位置を推定し、次に、障害物情報取得部２１が自機の周囲における障害物の位置情報を取得する。さらに地図作成部２２は、推定された自己位置に基づいて、記憶部２５に記憶された環境地図復元用データを読み出してグローバルマップを更新する。そして、走行制御部２６が、更新されたグローバルマップ上を走行スケジュールに沿って走行するようにアクチュエータの制御量を作成して、それを走行部２７に入力する。

（２）機能ブロック
本発明の第１実施形態による自律移動体を以下に説明する。
図２は、本発明の第１実施形態が採用される清掃用ロボットの制御ブロック図である。
清掃用ロボット４０は、自律移動体１と、スロットル４２、清掃部ユーザインターフェイス４３を備える。清掃用ロボット４０は、さらに、清掃部（図示せず）を有している。清掃部は、自律移動体１の下部に設けられており、自律移動体１の移動と共に床面を清掃するための装置である。

スロットル４２は、図１の操作部３２に対応するものであり、操作者の手動操作による指示入力を受け付ける。スロットル４２は、軸周りの回転角度により制御の指示入力を受け付けるスロットルグリップを左右独立して設けることができる。また、スロットル４２は、前進速度を受け付けるスロットルグリップと、操舵方向の指示を受け付けるハンドルの組合せとすることも可能である。さらに、スロットル４２は、スロットルレバーやその他の入力手段を組み合わせたものとすることができる。

清掃部ユーザインターフェイス４３は、清掃部（図示せず）による清掃動作の指示を操作者から受け付けるものであり、例えば、操作ボタン、タッチパネル、その他操作スイッチの組合せで構成できる。

自律移動体１は、コントロールボード５１、ブレーカ５２、前方レーザレンジセンサ５３、後方レーザレンジセンサ５４、バンパースイッチ５５、非常停止スイッチ５６、スピーカ５７、走行部２７を備える。

ブレーカ５２は、主電源スイッチであって、操作者の操作に基づいて、自律移動体１の各部にバッテリ（図示せず）からの電源電圧供給のオン、オフを行う。
前方レーザレンジセンサ５３は、自律移動体１の前方に位置する障害物の位置情報を検出するものであり、所定の角度範囲で水平方向にレーザ光を照射して、障害物から反射される反射光を受信して、自機から障害物までの距離を測定する。
後方レーザレンジセンサ５４は、自律移動体１の後方に位置する障害物の位置情報を検出するものであり、所定の角度範囲で水平方向にレーザ光を照射して、障害物から反射される反射光を受信して、自機から障害物までの距離を測定する。

バンパースイッチ５５は、自律移動体１の車体外周に取り付けられた感圧スイッチとすることができ、障害物に接触したことを検出して、検出信号を出力する。
非常停止スイッチ５６は、自律移動体１の動作を緊急停止させる指示を受け付けるものであって、操作者による操作が可能なスイッチである。
スピーカ５７は、自律移動体１の動作中における各種情報を音により操作者に通知する。

コントロールボード５１は、ＣＰＵ、ＲＯＭ、ＲＡＭを搭載する回路基板であり、制御部１１、教示データ作成部２４、ＳＬＡＭ処理部６３、不揮発メモリ６４、障害物情報取得部２１、記憶部２５、走行制御部２６を含む。

記憶部２５は、各種データを記憶するものであり、教示走行モードにおいて作成された走行スケジュール及び環境地図復元用データを、時刻又は時刻に関連付けた識別子に関連付けて記憶する。

制御部１１は、コントロールボード５１に搭載されるマイクロプロセッサであり、所定のプログラムを実行することにより、自律移動体１の各部の動作を制御する。
教示データ作成部２４は、教示走行モードにおける通過時刻と通過時刻に対応する通過点データの集合である走行スケジュールを作成する。

ＳＬＡＭ処理部６３は、自己位置推定と環境地図作成を同時に行うＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）処理を実行する機能ブロックであって、地図作成部２２、自己位置推定部２３、マップマッチング部７３を備える。

地図作成部２２は、障害物情報取得部２１により取得した障害物の位置情報に基づいて、ローカルマップ（局所地図）及びグローバルマップ（環境地図）を作成する。地図作成部２２は、教示走行モードにおいて、自機の周囲に位置する障害物の位置情報を取得した時刻に対応させ、それを環境地図復元用データとして記憶部２５に記憶する。また、地図作成部２２は、再現走行モードにおいて、現在の通過点よりも先の時刻に対応する環境地図復元用データを記憶部２５から読み出して、それに基づいてグローバルマップを更新する。

自己位置推定部２３は、前回の自己位置からの移動量を前回の自己位置に加算することで、現在の自己位置を推定する。
マップマッチング部７３は、自機の周囲に位置する障害物の位置情報に基づいて作成されたローカルマップ（局所地図）を、次に地図作成部２２により更新されたグローバルマップ（環境地図）と比較することで、自己位置推定部２３により推定された自己位置を補正する。

不揮発メモリ６４は、主制御部６１のブートプログラム、走行制御に関するプログラム、各種パラメータなどを記憶する。
障害物情報取得部２１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４の検出信号に基づいて、自機の周囲に位置する障害物の位置情報を取得する。
記憶部２５は、教示データ作成部２４により作成された走行スケジュールと、地図作成部２２により作成された環境地図復元用データとを記憶する。

走行制御部（モーションコントローラ）２６は、入力される走行命令に基づいて走行部２７のアクチュエータの制御量を生成して、それを走行部２７に入力する。入力される走行命令は、教示走行モードにおいては、スロットル４２を介して入力される操作者からの指示入力である。また、入力される走行命令は、再現走行モードにおいては、ＳＬＡＭ処理部６３により推定されるグローバルマップ上の自己位置、更新されたグローバルマップ及び走行スケジュールに基づいて生成される走行命令である。
走行制御部２６は、演算部８１、判断部８２、移動制御部８３を備えている。判断部８２は、入力された走行命令を解釈する。演算部８１は、判断部８２により解釈された走行命令に基づいて、走行部２７のアクチュエータの制御量を算出する。移動制御部８３は、演算部８１により算出された制御量を走行部２７に出力する。

走行部２７は、２つの走行車輪（図示せず）に対応して、一対のモータ９３、９４を有し、それぞれの回転位置を検出するエンコーダ９５、９６と、モータ駆動部９１、９２を備えている。
モータ駆動部（モータドライバ）９１は、走行制御部２６から入力される制御量と、エンコーダ９５により検出されるモータ９３の回転位置とに基づいて、モータ９３をフィードバック制御する。モータ駆動部９２は、同様に、走行制御部２６から入力される制御量と、エンコーダ９６により検出されるモータ９４の回転位置とに基づいて、モータ９４をフィードバック制御する。

教示データ作成部２４、障害物情報取得部２１、走行制御部２６、及びＳＬＡＭ処理部６３は、それぞれ制御部１１がプログラムを実行することにより実現される機能ブロックとすることができ、また、それぞれ独立した集積回路で構成することも可能である。

（３）制御動作
図３は、自律移動体の制御動作の概略を示す制御フローチャートである。以下、図２における制御部１１が所定のプログラムを実行することにより、教示データ作成部２４、ＳＬＡＭ処理部６３、障害物情報取得部２１、及び走行制御部２６の各機能ブロックを実現するものとして説明する。

ステップＳ３１において、制御部１１は、操作者によりモード選択が行われたか否かを判別する。具体的には、操作者による操作部３２の操作により指示入力を受け付けた場合、又はリモコンからの指示入力信号を受信した場合に、制御部１１はステップＳ３２に移行する。

ステップＳ３２において、制御部１１は選択されたモードが教示走行モードであるか否かを判別する。制御部１１は、教示走行モードを選択する指示入力を受け付けたと判断した場合、ステップＳ３３に移行し、そうでない場合にはステップＳ３４に移行する。
ステップＳ３３において、制御部１１は教示走行モードを実行し、その後、ステップＳ３６に移行する。
ステップＳ３４において、制御部１１は選択されたモードが再現走行モードであるか否かを判別する。制御部１１は、再現走行モードを選択する指示入力を受け付けたと判断した場合、ステップＳ３５に移行する。

ステップＳ３５において、制御部１１は再現走行モードを実行し、その後、ステップＳ３６に移行する。
ステップＳ３６において、制御部１１は、終了指示を受け付けたか否か、もしくは教示された走行スケジュールを終えたか否かを判別する。具体的には、制御部１１は、操作者による操作部３２の操作により処理終了する旨の指示入力があった場合、リモコンにより処理終了する旨の指示入力信号を受信した場合、あるいは、教示走行モードにより作成された走行スケジュールを終了したと判断した場合には、処理を終了し、そうでない場合には、ステップＳ３１に移行する。

（３−１）教示走行モード
図４は、教示走行モードにおける自律移動体の制御フローチャートである。
ステップＳ４０において、制御部１１は、初期設定を行う。具体的に、制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４の検出信号に基づいて、自機の周囲に位置する障害物の位置情報を取得し、これに基づいて自機の周囲におけるローカルマップを作成する。

図５は、レーザレンジセンサの走査範囲の一例を示す説明図である。
自律移動体１は、前端部に前方レーザレンジセンサ５３が取り付けられており、パルス発振されたレーザ光を回転ミラーにより扇状に走査して、そして自律移動体１の前方に位置する障害物から反射する反射光を受信する。
図５に示す例では、前方レーザレンジセンサ５３は、前方約１８０度の走査領域１０１にレーザ光を送信する。前方レーザレンジセンサ５３は、障害物から反射した反射光を所定周期で受信し、送信したレーザ光の発信パルスと受信した反射光の受信パルスとを比較して、障害物までの距離を検出する。

自律移動体１には、後端部に後方レーザレンジセンサ５４が取り付けられている。後方レーザレンジセンサ５４は、前方レーザレンジセンサ５３と同様に、パルス発振されたレーザ光を回転ミラーにより扇状に走査して、その結果、自律移動体１の後方に位置する障害物から反射する反射光を受信する。
図５に示す例では、後方レーザレンジセンサ５４は、後方約１８０度の走査領域１０３にレーザ光を送信する。ただし、自律移動体１は、後述する教示走行モードにおいて、障害物の検出を行わないマスク領域１０５を設け、第１走査領域１１５及び第２走査領域１１７に位置する障害物の位置情報を検出する。なお、マスク領域１５０は、自律移動体１の後方に位置する操作者に対応させて設けられている。

図６は、レーザレンジセンサの走査範囲の他の例を示す説明図である。
図５に示す例では、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４が、約１８０度の角度でレーザ光の走査を行っているのに対して、図６に示す例では、それぞれ約２４０度の角度でレーザ光の走査を行っている点で異なる。
具体的には、前方レーザレンジセンサ５３による走査領域１０１は前方２４０度の角度範囲である。また、後方レーザレンジセンサ５４による走査領域１０３は後方２４０度の角度範囲であり、マスク領域１０５を除く第１走査領域１１５及び第２走査領域１１７に位置する障害物の位置情報を検出する。

図７は、レーザレンジセンサにより検出した障害物の位置情報の一例を示すテーブルである。
前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４は、所定の走査領域にレーザ光を照射し、さらに障害物からの反射光を受信して、その後に照射したレーザ光の発信パルスと受信した反射光の受信パルスとに基づいて、障害物までの距離Ｒを出力値として算出する。また、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４は、障害物までの距離Ｒを得た時のセンサ角度Ｎも出力する。
制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４の出力を、所定の角度毎に取得し、それに基づいてセンサ角度Ｎと障害物までの距離Ｒとを対応付けた位置情報テーブルを作成する。図７に示す位置情報テーブルは、０．７２度毎にセンサ出力を取得して障害物の位置情報テーブルとしたものである。

制御部１１は、センサ座標系によるローカルマップの作成処理を実行する。具体的には、制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４により取得した障害物の位置情報テーブル（例えば、図７に示す障害物の位置情報テーブル）に基づいて、センサを中心とした座標系における自己位置近傍の環境地図であるローカルマップを作成する。

例えば、図５及び図６に示すように、制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４により走査される走査領域１０１、１０３内を、所定の面積のグリッドに分割し、次に、障害物の位置情報テーブルに基づいて、各グリッドにおける障害物存在確率を算出する。

図８は、センサを中心とした座標系でのローカルマップの説明図である。
制御部１１は、前方レーザレンジセンサ５３又は後方レーザレンジセンサ５４のセンサ中心を原点１３２とし、障害物の位置情報テーブルのセンサ角度Ｎに対応する走査線１３１上のグリッドに対して障害物存在確率を決定する。ここで、障害物が存在するグリッドの障害物存在確率を「１．０」とし、障害物が存在するか否か不明であるグリッドの障害物存在確率を「０」とし、障害物と原点との間に位置するグリッドの障害物存在確率を「−１＋（ｒ／Ｒ）^２＝−１．０〜０」とする。

制御部１１は、走査線１３１上であって、原点１３２からの距離ｒが障害物までの距離Ｒと一致するグリッドを検出グリッド１３３として、そのグリッドの障害物存在確率を「１．０」とする。
制御部１１は、走査線１３１上であって、原点１３２からの距離ｒが障害物までの距離Ｒ未満にあるグリッドには障害物が存在しないと判断する。図８において、原点１３２と検出グリッド１３３との間に位置する中間グリッド１３４〜１３８の障害物存在確率を「−１＋（ｒ／Ｒ）^２＝−１．０〜０」とする。
制御部１１は、走査線１３１上であって、原点１３２からの距離ｒが障害物までの距離Ｒよりも遠くにあるグリッドには、障害物が存在するか否かは不明であると判断して、不明グリッド１３９の障害物存在確率を「０」とする。

このことにより、図５又は図６に示すように、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４を中心として、障害物存在確率が「１．０」である検出グリッド１０７と、障害物存在確率が「−１．０〜０」である中間グリッド１０９と、障害物存在確率が「０」である不明グリッド１１１とを含むセンサ座標系によるローカルマップが作成される。
ただし、後方レーザレンジセンサ５４のマスク領域１０５に位置するグリッドは、不明グリッド１１３として障害物存在確率が「０」に設定される。
図５及び図６に示す例では、自律移動体１の前方に位置して直線状に検出グリッド１０７が存在しており、したがって走行経路を構成する壁面又は大型の障害物が存在すると推定することができる。
このようにして、センサ座標系のローカルマップ作成処理では、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４を中心とする周囲に位置する各グリッドの障害物存在確率を算出して、これをセンサ座標系のローカルマップとして出力する。

ステップＳ４０における初期設定では、前述したように、制御部１１では、障害物情報取得部２１が、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４の検出信号から障害物の位置情報を取得し、さらに、地図作成部２２が自機の周囲に位置するローカルマップを作成する。制御部１１は、障害物の位置情報を取得した時刻（ｔ１）における自己位置の座標と姿勢を含む自己位置（ｘ１，ｙ１，θ１）が絶対座標系の原点（あるいは所定の座標）になるように、ローカルマップをグローバルマップとして置き換える。

この時、制御部１１は、グローバルマップにおける現在の自己位置（ｘ１，ｙ１，θ１）が走行開始位置として適切であるか否かの判断を行う。例えば、自己位置の周辺に障害物がほとんど存在しない場合又は単純形状が連続する場合には、再現走行モードにおいて走行開始位置を判定することが困難であることから、制御部１１は表示部（図示せず）又はスピーカ５７を通じて、走行開始位置として不適切である旨の通知を行って、操作者に走行開始位置の変更を促す。

制御部１１は、自己位置（ｘ１，ｙ１，θ１）が走行開始位置として適切であると判断した場合には、障害物情報取得部２１により取得した障害物の位置情報を時刻（ｔ１）と関係付けて環境地図復元用データとして、それを記憶部２５に記憶させる。また、制御部１１は、自己位置の座標（ｘ１，ｙ１）と自機の姿勢（θ１）及び時刻（ｔ１）を関係付けた通過点データ（ｘ１，ｙ１，θ１，ｔ１）を走行スケジュールとして、記憶部２５に記憶させる。
さらに、制御部１１は、地図作成部２２により作成された自己位置（ｘ１，ｙ１，θ１）を中心としたグローバルマップの一部を、記憶部２５に一時的に記憶させる。

ステップＳ４１において、制御部１１は、操作者が入力した走行命令に基づいて、走行部２７のアクチュエータの制御量を作成し、それを走行部２７に出力する。教示走行モードでは、制御部１１は、操作者がスロットル４２を操作することにより入力する走行速度及び操舵に関する指示入力を受け付け、走行経路内における走行制御を行う。操作者による指示入力は、リモコンによる無線での指示入力信号を受信する方式でもよい。

ステップＳ４２において、制御部１１はデータ計測処理を実行する。具体的には、制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４により、レーザ光を照射し、さらに障害物から反射した反射光を受信することで、障害物の距離及び方向に関する障害物情報を得る。
また、制御部１１は、さらに自律移動体１の一定時間内の移動量に関する情報を取得する。具体的には、制御部１１は、走行部２７のエンコーダ９５、９６から、それぞれ対応するモータ９３、９４の回転位置に関する情報を取得し、一定時間内の移動量を算出する。

一定時間内の移動量は、自律移動体１の前回の計測時点（ｔ−１）の２次元座標（ｘ（ｔ−１），ｙ（ｔ−１））からの移動量と、進行方向の変更量とを備える移動量（ｄｘ，ｄｙ，ｄθ）として表すことができる。

ステップＳ４３において、制御部１１は、センサ座標系によるローカルマップの作成処理を実行する。具体的には、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４により取得した障害物の位置情報テーブル（例えば、図７に示す障害物の位置情報テーブル）に基づいて、センサを中心とした座標系における自機の周囲に位置する所定範囲を所定の大きさのグリッドに分割し、さらに各グリッドに算出された障害物存在確率を対応付けたローカルマップを作成する。

ステップＳ４４において、制御部１１は、自己位置推定部２３により、時刻（ｔ−１）における自己位置に時刻（ｔ−１）〜時刻（ｔ）間の移動量（ｄｘ，ｄｙ，ｄθ）を加算することで時刻（ｔ）の自己位置を推定する。また、制御部１１は、自己位置のパラメータとして、２次元座標（ｘ，ｙ）と進行方向（θ）を有する３次元のグリッド空間（ｘ，ｙ，θ）を想定し、さらに移動前後における各グリッドの自己位置の確率分布を算出することで、マップマッチングを行う。

図９は、自己位置推定処理の制御フローチャートである。
ステップＳ９１において、制御部１１は、事前確率計算処理を実行する。
制御部１１は、現在時刻である時刻（ｔ）を用いて、時刻（ｔ−１）の事後確率分布と、時刻（ｔ−１）〜時刻（ｔ）間の移動量（ｄｘ，ｄｙ，ｄθ）とに基づいて、時刻（ｔ）における自己位置の事前確率分布を算出する。

図１０は、事後確率分布の一例を示す説明図であり、図１１は、事前確率分布の一例を示す説明図である。
制御部１１は、図１０に示す時刻（ｔ−１）の事後確率分布に、時刻（ｔ−１）〜時刻（ｔ）間の移動量（ｄｘ，ｄｙ，ｄθ）を加算して、座標をシフトさせる。
制御部１１は、さらに、正規分布を畳み込み演算することによって、図１１に示すような時刻（ｔ）における事前確率分布を得る。
制御部１１は、このような事前確率計算処理を３次元グリッド空間（ｘ，ｙ，θ）内の各グリッドに対して実行することにより、車輪のスリップなどを考慮しながら、エンコーダ９５、９６から取得した移動量（ｄｘ，ｄｙ，ｄθ）を加えた自己位置の確率分布を更新する。

ステップＳ９２において、制御部１１は尤度計算処理を実行する。
図１２は、尤度計算処理の制御ブローチャートである。
ステップＳ１２１において、制御部１１は、仮の自己位置座標を抽出する。具体的には、制御部１１は、事前確率分布処理により算出された時刻（ｔ）における事前確率のうち閾値を超える座標を１つ抽出し、これを仮の自己位置座標とする。制御部１１は、時刻（ｔ）の事前確率分布、ローカルマップ、及び作成中のグルーバルマップに基づいて、抽出した仮の自己位置座標における尤度を算出する。

ステップＳ１２２において、制御部１１は、座標変換処理を実行する。具体的には、制御部１１は、センサ座標系によるローカルマップを、仮の自己位置座標に基づいて、絶対座標系のローカルマップに変換する。
制御部１１は、事前確率分布に基づいて抽出された仮の自己位置座標を（ｘ，ｙ，θ）とし、センサ座標系によるローカルマップの各グリッドの座標を（ｌｘ，ｌｙ）として、絶対座標系のローカルマップの各グリッドの座標（ｇｘ，ｇｙ）を数式１により算出する。

ステップＳ１２３において、制御部１１は、マップマッチング処理を実行する。
制御部１１は、作成中のグローバルマップと絶対座標系のローカルマップとのマップマッチング処理を行い、それに基づいて絶対座標系のローカルマップ上における各グリッドの障害物存在確率を算出する。
制御部１１は、絶対座標系のローカルマップにおいて、障害物存在確率「１．０」のグリッドがＮ個存在した場合、ｉ番目のグリッドの座標を（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））とし、グローバルマップ上の座標（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））における障害物存在確率をＧＭＡＰ（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））として、以下の数式２により一致度を表すスコアｓを算出する。



制御部１１は、複数の仮の自己位置に対してそれぞれスコアｓを算出して、それを自己位置の尤度分布とする。
図１３は、尤度計算処理により得られた尤度分布の一例を示す説明図である。

ステップＳ９３において、制御部１１は、事後確率計算処理を実行する。具体的に、制御部１１は、絶対座標系のローカルマップの各グリッドに対して、時刻（ｔ）における事前確率分布に、時刻（ｔ）における尤度分布を掛けることにより、時刻（ｔ）における事後確率を算出する。

図１４は、事後確率計算処理により得られた事後確率分布の一例を示す説明図である。図１４では、図１１に示す時刻（ｔ）における事前確率分布と、図１３に示す時刻（ｔ）における尤度分布とを掛けて得られた事後確率分布を示している。
制御部１１は、複数の仮の自己位置に対して事後確率計算処理を実行し、その結果事後確率が最大となるものを自己位置とすることができる。

上述のような構成とすることにより、尤度が同程度の値を持つ仮の自己位置に対して、事前確率の差を事後確率に反映させることができ、さらに、前方レーザレンジセンサ５３、後方レーザレンジセンサ５４と走行部２７のエンコーダ９５，９６の出力信号に基づいてデッドレコニング（Ｄｅａｄ−Ｒｅｃｋｏｎｉｎｇ）により、自己推定を推定することが可能となる。また、時刻（ｔ）における事前確率計算を行う際に、時刻（ｔ−１）の事後確率を用いていることから、走行部２７における車輪（図示せず）のスリップや前方レーザレンジセンサ５３、後方レーザレンジセンサ５４のノイズなどの外乱要因を阻止でき、つまり自己位置を正確に推定できる。

ステップＳ４５において、制御部１１は、障害物情報取得部２１により、グローバルマップ上における現在の自己位置の周囲に位置する障害物の位置情報を取得する。

ステップＳ４６において、制御部１１は、障害物情報取得部２１により取得した障害物の位置情報を時刻に対応させて環境地図復元用データを作成し、それを記憶部２５に記憶させる。
ここで、記憶部２５に記載される環境地図復元用データは、グローバルマップ上の自己位置の周囲を所定面積の単位グリッドで分割し、さらに各グリッドに障害物存在確率を対応付けて、障害物の位置情報を取得した時刻（ｔ）と関連つけたものとすることができる。

ステップＳ４７において、制御部１１は、グローバルマップ更新処理を実行する。具体的に、制御部１１は、時刻（ｔ−１）におけるグローバルマップに対して、自己位置推定処理により推定された自己位置により座標変換を行った絶対座標系のローカルマップを足し合わせすることで、時刻（ｔ）におけるグローバルマップを作成する。

図１５は、グローバルマップ更新処理の概念構成を示す説明図である。図１５に示す例では、自己位置近傍の４×４のグリッドを対象にしており、（ａ）は時刻（ｔ−１）におけるグローバルマップ、（ｂ）は時刻（ｔ）における絶対座標系のローカルマップ、（ｃ）は更新された時刻（ｔ）におけるグローバルマップを示す。
図１５に示すように、時刻（ｔ−１）におけるグローバルマップ及び時刻（ｔ）におけるローカルマップでは、それぞれのグリッドに障害物存在確率が記憶されている。前述したように、障害物が存在していると推定されるグリッドの障害物存在確率は「１．０」、障害物が存在していないと推定されるグリッドの障害物存在確率は「−１．０〜０」、障害物の存在が不明であるグリッドの障害物存在確率は「０」に設定されるものとする。

グローバルマップ更新処理では、制御部１１は、自己位置推定処理により推定された自己位置に基づいて、絶対座標系に変換されたローカルマップの各グリッド（図１５（ｂ））をグローバルマップの各グリッド（図１５（ａ））に対応させ、各グリッドの障害物存在確率を加算する。
制御部１１は、各グリッドの障害物存在確率が演算により「１．０」以上になる場合、及び「−１．０」以下になる場合には、切り捨て計算を行って、それぞれ「１．０」、「−１．０」とする。このことにより、繰り返し障害物が存在すると判別されたグリッドの障害物存在確率は「１．０」に近い値となり、繰り返し障害物が存在しないと判別されたグリッドの障害物存在確率は「−１．０」に近い値となる。
このようにして、制御部１１は、図１５（ｃ）に示す時刻（ｔ）におけるグローバルマップの各グリッドの障害物存在確率を算出することで、グローバルマップを更新する。

時刻（ｔ）におけるグローバルマップは、少なくとも次の時刻（ｔ＋１）において、制御部１１が自己位置推定処理を実行するために必要な範囲を含むものであり、一時的に記憶部２５に記憶される。
このグローバルマップ更新処理では、グローバルマップの各グリッドの障害物存在確率が、絶対座標系のローカルマップの対応する各グリッドの障害物存在確率を加算することで更新するので、車輪のスリップやセンサの計測誤差に基づくノイズの影響を極力少なくすることができる。

ステップＳ４８において、制御部１１は、自律移動体１が通過後の通過点におけるグローバルマップを削除する。
前述したように、時刻（ｔ）において更新されるグローバルマップが本当に必要なのは、次の時刻（ｔ＋１）において、制御部１１が自己位置推定処理を実行するために必要な範囲だけである。したがって、次の時刻（ｔ＋１）において、制御部が自己位置推定処理を実行するために必要でない範囲は、ステップＳ４８において削除される。このため、環境地図において、環状経路が閉じないという問題を考慮する必要がなくなる。

図１６は、走行スケジュールの説明図、図１７は、グローバルマップの更新・削除の説明図、図１８は、環状の走行経路の説明図である。
図１６に示すように、教示走行モードにおいて、自律移動体１が操作者の手動操作に基づいて、環状の走行経路１６０１上の通過点１６１１−１・・・に沿って走行するものとする。

図１７に示すように、時刻（ｔ１）における通過点１６１１−１において、制御部１１は自己位置の周囲に位置する所定の範囲でローカルマップ１７１２−１を作成し、これによりグローバルマップを更新する。同様にして、時刻（ｔｎ）における通過点１６１１−ｎにおいて、制御部１１は自己位置の周囲に位置する所定の範囲でローカルマップ１７１２−ｎを作成し、これによりグローバルマップを更新する。この時、制御部１１は、グローバルマップのうち、現在の通過点１６１１−ｎから一定の距離が離れた領域のデータを削除する。

図１８に示すように、時刻（ｔｍ）における通過点１６１１−ｍでは、ローカルマップ１７１２−ｍによりグローバルマップが更新されるが、時刻（ｔ１）のローカルマップ１７１２−１により更新されるグローバルマップと重複する部分を含む。
前述したように、グローバルマップのうち、現在の自己位置から一定の距離が離れた領域を削除していることから、時刻（ｔｍ）において自律移動体１が通過点１６１１−ｍに到達した時には、時刻（ｔ１）においてローカルマップ１７１２−１に基づいて更新されたグローマップのデータは既に削除されている。したがって、ローカルマップにより更新されるグローバルマップが、前に作成された同一座標近傍のグローバルマップと重複することが防止され、車輪のスリップやセンサの計測誤差などがあっても、環状経路が閉じないという問題を防止できる。

ステップＳ４９において、制御部１１は、教示データ作成部２４により、走行スケジュールを作成し記憶部２５に記憶させる。具体的には、図１７に示すような走行経路１６０１上の通過点１６１１−１・・・に沿って走行する場合、制御部１１は、時刻ｔにおいて走行経路１６０１上の通過点１６１１のグローバルマップ上における座標（ｘ，ｙ）と、自律移動体１の走行方向を示す姿勢（θ）及び時刻（ｔ）でなる通過点データ（ｘ，ｙ，θ，ｔ）を、記憶部２５に記憶させる。

所定の時間間隔（タイムスケジュール）に基づいて、時刻ｔ１、ｔ２・・・において、自律移動体１が通過点１６１１−１、１６１１−２・・・１６１１−ｎを通過する場合、教示データ作成部２４は、第１通過点１６１１−１、第２通過点１６１１−２、・・・第ｎ通過点１６１１−ｎにおける通過点データ（ｘ１，ｙ１，θ１，ｔ１）、（ｘ２，ｙ２，θ２，ｔ２）・・・（ｘｎ，ｙｎ，θｎ，ｔｎ）の集合でなる走行スケジュールを記憶部２５に記憶させる。

ステップＳ５０において、制御部１１は、終了指示を受け付けたか否かを判別する。具体的には、制御部１１は、操作者による操作部３２の操作により処理終了する旨の指示入力があった場合、又はリモコンにより処理終了する旨の指示入力信号を受信した場合には、終了処理を実行することで処理を終了し、そうでない場合には、プロセスはステップＳ４１に移行する。

（３−２）再現走行モード
図１９は、再現走行モードにおける自律移動体の制御フローチャートである。
ステップＳ１９１において、制御部１１は、データ計測処理を実行する。具体的には、制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４により、レーザ光を照射しさらに障害物から反射した反射光を受信することで、障害物の距離及び方向に関する障害物情報を得る。ここでのデータ計測処理は、教示走行モードにおけるデータ計測処理と同様であり、詳細説明は省略する。

ステップＳ１９２において、制御部１１は、センサ座標系によるローカルマップの作成処理を実行する。具体的には、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４により取得した障害物の位置情報テーブル（例えば、図７に示す障害物の位置情報テーブル）に基づいて、センサを中心とした座標系における自己位置近傍の各グリッドに障害物存在確率を対応させたローカルマップを作成する。

ステップＳ１９３において、制御部１１は、自己位置推定部２３により、グローバルマップ上の自己位置を推定する。自己位置の推定処理は、教示走行モードにおける自己位置の推定処理と同様に、時刻（ｔ−１）における自己位置に時刻（ｔ−１）〜時刻（ｔ）間の移動量（ｄｘ，ｄｙ，ｄθ）を加算することで時刻（ｔ）の自己位置として推定する。また、制御部１１は、自己位置のパラメータとして、２次元座標（ｘ，ｙ）と進行方向（θ）を有する３次元のグリッド空間（ｘ，ｙ，θ）を想定した上で、移動前後における各グリッドの自己位置の確率分布を算出することでマップマッチングを行って、それにより自己位置の補正を行うことができる。

ステップＳ１９４において、制御部１１は、推定された自己位置に対応する環境地図復元用データを用いてグローバルマップを復元し、このグローバルマップと、障害物情報取得部２１により取得した障害物の位置情報から作成したローカルマップとを比較することで、現在の自己位置に対応する走行スケジュール中の時刻を推定する。
ステップＳ１９４において、制御部１１は、記憶部２５に記憶されている走行スケジュールのうち推定した時刻に対応する通過点データを、記憶部２５から読み出す。

ステップＳ１９５において、制御部１１は、推定された自己位置に対応する環境地図復元用データを用いて、グローバルマップを更新する。具体的には、既に作成されているグローバルマップの各グリッドの障害物存在確率に、復元されたグローバルマップの各グリッドの障害物存在確率を加算することで、グローバルマップの各グリッドの障害物存在確率を更新する。

ステップ１９６において、制御部１１は、自律移動体１が通過後の通過点におけるグローバルマップを削除する。ここでは、教示走行モードと同様に、時刻（ｔ）においてグローバルマップを更新する際に、次の時刻（ｔ＋１）において、制御部１１が自己位置推定処理を実行するために必要でない範囲が、グローバルマップから削除される。

ステップＳ１９８において、制御部１１は、走行部２７のアクチュエータの制御量を作成し、それを走行部２７に出力する。具体的には、制御部１１は、グローバルマップ上の自己位置と、記憶部２５から読み出された通過点データとに基づいて、走行スケジュールに沿うようにアクチュエータの制御量を決定し、これに基づく走行命令を走行部２７に入力する。

ステップＳ１９９において、制御部１１は、再現走行モードを終了するか否かを判別する。具体的には、制御部１１は、記憶部２５に記憶された走行スケジュールのうち最終の通過点データに到達した場合、操作者により非常停止スイッチ５６が操作された場合、バンパースイッチ５５により障害物に衝突したことを検出した場合、障害物情報取得部２１により検出された障害物の位置情報が所定距離以下に近接した場合などにおいて、制御部１１は再現走行モードを終了すると判断する。制御部１１は、再現走行モードを終了しないと判断した場合にはステップＳ１９１に移行する。

以上のようにして、教示走行モードにおいて、グローバルマップを復元するための環境地図復元用データと、通過点データの集合である走行スケジュールとを、記憶部２５に記憶させている。このことにより、再現走行モードにおいて、環境地図復元用データを用いて自己位置の周囲近傍のグローバルマップを復元して、さらに復元されたグローバルマップ上の自己位置の推定を行う。この場合に、通過した後のグローバルマップのデータは削除していることから、環状経路問題を排除することができ、つまり、車輪のスリップやセンサの計測誤差などに基づくグローバルマップのずれによる不整合を防止できる。
また、再現走行モードにおいて、走行スケジュールの通過点データに沿って走行制御を行っていることから、グローバルマップの座標系から外れて自律走行に支障を来すことがない。

（４）実施形態の特徴
上記実施形態は、下記の構成及び機能を有している。
自律移動体１（自律移動体の一例）は、教示走行モードと、再現走行モードとを実行する。教示走行モードは、操作者の手動操作により、走行開始位置から走行終了位置まで自律移動体１を手動走行させながら、通過時刻と通過時刻における通過点データ（例えば、通過点データ（ｘ，ｙ，θ，ｔ））の集合でなる走行スケジュールを作成する。再現走行モードは、走行スケジュールを再現しながら、走行開始位置から走行終了位置まで自律移動体１が自律的に走行する。
自律移動体は、記憶部２５（記憶部の一例）、走行部２７（走行部の一例）、教示データ作成部２４（走行データ作成部の一例）、走行制御部２６（走行制御部の一例）、障害物情報取得部２１（障害物情報取得部の一例）、地図作成部２２（地図作成部の一例）、自己位置推定部２３（自己位置推定部の一例）を備える。記憶部２５は、データを記憶する。走行部２７は、制御量が入力される複数のモータ９３，９４（アクチュエータの一例）を有する。教示データ作成部２４は、走行スケジュールを作成する。走行制御部２６は、走行命令を解釈してアクチュエータに入力する制御量を作成し走行部２７に入力する。障害物情報取得部２１は、自機の周囲に存在する障害物の位置情報を取得する。地図作成部２２は、障害物情報取得部２１により取得した障害物の位置情報から環境地図を作成する。自己位置推定部２３は、環境地図上の自己位置を推定する。

自律移動体は、教示走行モードにおいて、所定のタイムスケジュール毎に、
（ａ）走行制御部２６が、操作者から入力された走行命令からモータ９３，９４の制御量を作成してそれを走行部２７に出力し（例えば、ステップＳ４１）、
（ｂ）自己位置推定部２３が、環境地図上の自己位置を推定し（例えば、ステップＳ４４）、
（ｃ）障害物情報取得部２１が、自機の周囲における障害物の位置情報を取得し（例えば、ステップＳ４５）、
（ｄ）地図作成部２２が、障害物の位置情報を取得した時刻に対応させて障害物の位置情報を環境地図復元用データとして記憶部２５に記憶させ（例えば、ステップＳ４６）、
（ｅ）教示データ作成部２４が、走行スケジュールを作成して記憶部２５に記憶させる（例えば、ステップＳ４９）。

また、自律移動体１は、再現走行モードにおいて、所定のタイムスケジュール毎に、
（ｆ）自己位置推定部２３が、環境地図上の自己位置を推定し（ステップＳ１９３）、
（ｇ）障害物情報取得部２１が、自機の周囲における障害物の位置情報を取得し（例えば、ステップＳ１９５）、
（ｈ）地図作成部２２が、推定された自己位置に対応する環境地図復元用データを読み出して環境地図を更新し（例えば、ステップＳ１９６）、
（ｉ）走行制御部２６が、更新された環境地図上を走行スケジュールに沿って走行するようにモータ９３，９４の制御量を作成して走行部２７に入力する（例えば、ステップＳ１９８）。

この自律移動体１では、操作者が手動操作により自律移動体１を操作することで、環境地図復元用データを生成して記憶部２５に格納している。また、自律移動体１は、再現走行モードにおいて、環境地図上の自己位置を推定して、推定された自己位置に対応する環境地図復元用データを用いて環境地図を更新しながら、走行スケジュールに沿って走行駆動している。このことから、操作者が環境地図の作成方法を考慮することなく、操作性を容易にし、走行経路が環状である場合であっても、車輪のスリップやセンサの計測誤差による環状経路が閉じないという問題を考慮する必要がなくなる。

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

（５−１）環境地図の更新・削除処理
制御部１０は、環境地図を、静的環境地図と動的環境地図の２層構造で管理することが可能である。静的環境地図は、教示走行モードで作成される地図復元用データに基づいて復元することができる。また、動的環境地図は、時刻（ｔ）の動的環境地図と、時刻（ｔ＋１）において障害物情報取得部２１が取得する自己位置の周辺における障害物の位置情報に基づくローカルマップとの重ね合わせにより作成することができる。

例えば、時刻（ｔ）の動的環境地図の各グリッドにおける障害物存在確率を、ＤｙｎａｍｉｃＭａｐ（ｔ）とし、静的環境地図の各グリッドにおける障害物存在確率を、ＳｔａｔｉｃＭａｐとする。また、時刻（ｔ）において、静的環境地図からの障害物の配置変化を示す差分地図の各グリッドにおける障害物存在確率を、ＤｉｆｆＭａｐ（ｔ）とする。
さらに、動的環境地図の各グリッドの値の範囲を決定するパラメータをＰ１とすると、
ＤｙｎａｍｉｃＭａｐ（ｔ）＝ＳｔａｔｉｃＭａｐ・Ｐ１＋ＤｉｆｆＭａｐ（ｔ）
として算出できる。

制御部１０は、障害物情報取得部２１により取得した自機の周囲に位置する障害物情報と、環境地図復元用データから復元された静的環境地図を用いて自己位置を推定し、絶対座標系のローカルマップを作成する。
時刻（ｔ＋１）における動的環境地図は、時刻（ｔ）における動的環境地図と、時刻（ｔ＋１）におけるローカルマップとを用いて、各グリッドの障害物存在確率が算出される。絶対座標系のローカルマップにおける各グリッドの障害物存在確率をＬｏｃａｌＭａｐ（ｔ＋１）とすると、
ＤｙｎａｍｉｃＭａｐ（ｔ＋１）＝ＤｙｎａｍｉｃＭａｐ（ｔ）＋ＬｏｃａｌＭａｐ（ｔ＋１）
として算出できる。

制御部１０は、時刻（ｔ＋１）における動的環境地図と、静的環境地図との差分を求めて、時刻（ｔ＋１）における差分地図の各グリッドの障害物存在確率であるＤｉｆｆＭａｐ（ｔ＋１）を
ＤｉｆｆＭａｐ（ｔ＋１）＝ＤｙｎａｍｉｃＭａｐ（ｔ＋１）−ＳｔａｔｉｃＭａｐ・Ｐ１
として作成し、これを用いて動的環境地図を更新する。

図２０は、動的環境地図の更新処理の説明図である。図２０に示す例では、自己位置近傍の４×４のグリッドを対象にしており、（ａ）は時刻（ｔ）における動的環境地図、（ｂ）は静的環境地図、（ｃ）は時刻（ｔ）における差分地図を示す。
図２０に示すように、時刻（ｔ）における差分地図ＤｉｆｆＭａｐ（ｔ）中の各グリッドの障害物存在確率と、静的環境地図ＳｔａｔｉｃＭａｐ中の各グリッドの障害物存在確率にパラメータＰ１を乗算した値とを加算して、時刻（ｔ）における動的環境地図ＤｙｎａｍｉｃＭａｐ（ｔ）が算出される。各グリッドの障害物存在確率は、−１．０〜１．０の範囲の値であり、パラメータＰ１を「０．５」としている。
このことから、ＤｙｎａｍｉｃＭａｐ（ｔ）＝ＳｔａｔｉｃＭａｐ・Ｐ１＋ＤｉｆｆＭａｐ（ｔ）で算出された動的環境地図の各グリッドの障害物存在確率は、図２０（ｃ）のようになる。

図２１は、自己位置推定時における尤度計算の説明図である。図２１に示す例では、自己位置近傍の４×４のグリッドを対象にしており、（ａ）は絶対座標系のローカルマップ、（ｂ）は静的環境地図、（ｃ）は動的環境地図である。
障害物情報取得部２１により取得した自機の周辺における障害物位置情報に基づいて、絶対座標系のローカルマップに変換した後の各グリッドの障害物存在確率が、図２１（ａ）に示される。

絶対座標系のローカルマップにおいて、障害物存在確率が「１．０」であるグリッドがＮ個存在する時、ｉ番目のグリッドの座標を（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））とする。静的環境地図の座標（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））における障害物存在確率をＳｔａｔｉｃＭａｐ（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））とし、動的環境地図の座標（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））における障害物存在確率をＤｙｎａｍｉｃＭａｐ（ｇｘ＿ｏｃｃｕｐｙ（ｉ），ｇｙ＿ｏｃｃｕｐｙ（ｉ））とする。

絶対座標系のローカルマップにおいて、障害物存在確率が「１．０」になるグリッドのそれぞれに対し、静的環境地図と動的環境地図のうち、障害物存在確率が高い方を用いて、以下の数式により尤度計算を行う。


このことにより、図２１に示す例では、ｓ＝（１．０＋１．０＋０．３＋０．３）／４＝０．６５として尤度を算出することができる。

図１７に示すような走行経路を走行する場合、時刻（ｔ１）における通過点１６１１−１において、制御部１１は自己位置の周囲に位置する所定の範囲でローカルマップ１７１２−１により更新された動的環境地図を作成する。同様にして、時刻（ｔｎ）における通過点１６１１−ｎにおいて、制御部１１は自己位置の周囲に位置する所定の範囲でローカルマップ１７１２−ｎにより更新された部分グローバルマップを作成する。この時、制御部１１は、時刻（ｔｎ−１）における通過点（図示せず）において作成された動的環境地図を削除し、さらに、時刻（ｔｎ）におけるローカルマップ１７１２−ｎを用いて動的環境地図を更新することができる。

このように、グローバルマップとして静的環境地図と動的環境地図の２層で構成する場合にも、図１８の時刻（ｔｍ）における通過点１６１１−ｍでは、時刻（ｔ１）において作成された動的環境地図が削除されていることから、環状経路が閉じないという問題を防止できる。
また、教示走行モードで作成された環境地図復元用データを用いて静的環境地図を復元した際に、障害物の位置情報が変化しているような場合であっても、差分地図を用いて障害物の配置の変化を検知することが可能となる。従って、自律移動体１は、走行経路上のレイアウト変更や移動体の存在に対応して、障害物との接触を回避することが可能となる。

本発明は、清掃用ロボット、搬送用ロボット、その他の自律走行を行う自律移動体に適用することができる。

１ 自律移動体
１１ 制御部
２１ 障害物情報取得部
２２ 地図作成部
２３ 自己位置推定部
２４ 教示データ作成部
２５ 記憶部
２６ 走行制御部
２７ 走行部
３１ 測距センサ
３２ 操作部
３３ 表示部
４０ 清掃用ロボット
４２ スロットル
４３ 清掃部ユーザインターフェイス
５１ コントロールボード
５２ ブレーカ
５３ 前方レーザレンジセンサ
５４ 後方レーザレンジセンサ
５５ バンパーセンサ
５６ 非常停止スイッチ
５７ スピーカ
６３ ＳＬＡＭ処理部
６４ 不揮発メモリ
７３ マップマッチング部
８１ 演算部
８２ 判断部
８３ 移動制御部
９１ モータ駆動部
９２ モータ駆動部
９３ モータ
９４ モータ
９５ エンコーダ
９６ エンコーダ
１０１ 走査領域
１０３ 走査領域
１０５ マスク領域
１０７ 検出グリッド
１０９ 中間グリッド
１１１ 不明グリッド
１１５ 第１走査領域
１１７ 第２走査領域
１３１ 走査線
１３２ 原点
１３３ 検出グリッド
１３４ 中間グリッド
１３５ 中間グリッド
１３６ 中間グリッド
１３７ 中間グリッド
１３８ 中間グリッド
１３９ 不明グリッド
１６０１ 走行経路
１６１１ 通過点
１７１２ ローカルマップ

Claims (5)

1. 操作者の手動操作により、走行開始位置から走行終了位置まで手動走行させながら、通過時刻と前記通過時刻における通過点データの集合でなる走行スケジュールを作成する教示走行モードと、前記走行スケジュールを再現しながら、前記走行開始位置から前記走行終了位置まで自律的に走行する再現走行モードとを実行する自律移動体であって、
   データを記憶する記憶部と、
   制御量が入力される複数のアクチュエータを有する走行部と、
   前記走行スケジュールを作成する教示データ作成部と、
   走行命令を解釈して前記アクチュエータに入力する制御量を作成し前記走行部に入力する走行制御部と、
   自機の周囲に存在する障害物の位置情報を取得する障害物情報取得部と、
   前記障害物情報取得部により取得した障害物の位置情報から環境地図を作成する地図作成部と、
   前記環境地図の上の自己位置を推定する自己位置推定部と、
   を備え、
   前記教示走行モードにおいて、所定のタイムスケジュール毎に、
   （ａ）前記走行制御部が、前記操作者から入力された走行命令から前記アクチュエータの制御量を作成して前記走行部に出力し、
   （ｂ）前記自己位置推定部が、前記環境地図上の自己位置を推定し、
   （ｃ）前記障害物情報取得部が、自機の周囲における障害物の位置情報を取得し、
   （ｄ）前記地図作成部が、前記障害物の位置情報を取得した時刻に対応させて前記障害物の位置情報を環境地図復元用データとして前記記憶部に記憶させ、
   （ｅ）前記教示データ作成部が、前記走行スケジュールを作成して前記記憶部に記憶させ、
   前記再現走行モードにおいて、所定のタイムスケジュール毎に、
   （ｆ）前記自己位置推定部が、前記環境地図上の自己位置を推定し、
   （ｇ）前記障害物情報取得部が、自機の周囲における障害物の位置情報を取得し、
   （ｈ）前記地図作成部が、前記推定された自己位置に対応する環境地図復元用データを読み出して前記環境地図を更新し、
   （ｉ）前記走行制御部が、前記更新された環境地図上を前記走行スケジュールに沿って走行するように前記アクチュエータの制御量を作成して前記走行部に入力する、
   自律移動体。
2. 前記教示走行モードにおいて、前記教示データ作成部は、所定のタイムスケジュール毎に、前記環境地図上の自己位置と姿勢とを前記通過点データとして、その時刻に対応させる、請求項１に記載の自律移動体。
3. 前記再現走行モードにおいて、前記地図作成部は、所定のタイムスケジュール毎に、現在の通過点よりも先の時刻に対応する前記環境地図復元用データを読み出して、これに基づいて既に作成した環境地図を更新する、請求項１又は２に記載の自律移動体。
4. 前記障害物情報取得部により取得された障害物の位置情報に基づいて局所地図を作成し、前記更新された環境地図と前記作成された局所地図とを比較するマップマッチング部をさらに有し、
   前記地図作成部は、前記マップマッチング部の比較結果に応じて、前記環境地図上の自己位置と姿勢及び前記環境地図上の自己位置に対応する前記走行スケジュールの中の通過点データを推定する、請求項３に記載の自律移動体。
5. 前記地図作成部は、所定のタイムスケジュール毎に、前記環境地図のうち既に通過済みの領域におけるデータを削除する、請求項１〜４のいずれかに記載の自律移動体。