JP6079415B2 - 自律移動体 - Google Patents

Description

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

従来から、超音波センサ、赤外線センサ、バンパーセンサなどのセンサにより周囲の障害物を検知し、障害物との接触を避けながらランダムに走行する自律移動体が提案されている。例えば、特許文献１には、自律移動体として、本体から壁面などの障害物までの距離を計測して、本体の姿勢を制御しながら自律走行する自走式掃除機が開示されている。

特許文献１に記載されているような自走式掃除機において、ランダムに走行する方式を適用した場合、清掃が必要であるにも関わらず自律移動体が到達しない空間が生じたり、清掃が不要な空間を通過したり、あるいは同じ空間を複数回通過するという不都合が生じる。

また、操作者の手動操作により走行経路を手動走行させながら、走行経路上の通過点又は操作内容を記憶させる教示走行モードを設け、この教示走行モードで記憶された走行経路と操作内容を自律的に再現する再現走行モードを実行する場合には、前述したような不都合を解消することができる。
しかしながら、このような自律移動体は、再現走行モードを行う際に、障害物との接触を避けるためには、進行方向の障害物との距離が所定距離以下になった場合に、走行停止の制御を行う必要がある。

上記の場合、教示走行モードで記憶された走行経路のうち、壁面などの障害物に接近し過ぎた通過点がある場合には、その地点で走行停止状態となることから、自律移動体の走行スケジュールを完了できないという問題がある。

特開平０１−１０６２０４号公報

本発明の課題は、自律移動体において、教示走行モードにおいて適切な走行スケジュールを設定し、それにより再現走行モードにおいて走行スケジュールに沿った自律走行を完了できるようにすることにある。

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

本発明の一見地による自律移動体は、操作者の手動操作により、走行経路を手動走行させながら、通過時刻と通過時刻における通過点データの集合でなる走行スケジュールを作成する教示走行モードと、走行スケジュールを再現しながら、走行経路を自律的に走行する再現走行モードとを実行する。

自律移動体は、本体と、走行部と、走行制御部と、障害物情報取得部と、記憶部と、自己位置推定部と、教示データ作成部と、走行命令生成部と、通知部とを備えている。
走行部は、制御量が入力される複数のアクチュエータを有する。走行制御部は、アクチュエータに入力する制御量を作成し走行部に入力する。障害物情報取得部は、本体の周囲に存在する障害物の位置情報を取得する。記憶部は、走行経路に対応する環境地図を記憶する。自己位置推定部は、環境地図上における自己位置を推定する。教示データ作成部は、教示走行モードにおいて、自己位置推定部により推定される環境地図上の自己位置を、その通過時刻と対応させた通過点データとして走行スケジュールを作成する。走行命令生成部は、再現走行モードにおいて、走行スケジュールに沿って走行するように走行命令を生成するとともに、障害物情報取得部により取得した障害物の位置情報に基づいて進行方向に障害物が存在する場合に減速又は停止の走行命令を生成し、走行制御部に入力する。通知部は、教示走行モードにおいて、障害物情報取得部より取得した障害物の位置情報に基づいて、本体と障害物との距離が所定値以下である場合に、障害物に接近している旨の通知を行う。通知のレベルは、本体と障害物との距離の接近度合いに応じて多段階とすることができる。

この自律移動体では、教示走行モードにおいて、自律移動体の本体と障害物との距離が所定距離以下になると、通知部を介して操作者に通知を行う。このことにより、教示走行モードにおいて作成される走行スケジュールは、自律移動体の本体と障害物との距離が所定距離以下になる箇所を含みにくい。したがって、再現走行モードにおいて、走行スケジュールの途中で減速又は停止することが減る。

通知部は、本体と障害物との距離が所定値以下である場合に、アラーム音を発生させるスピーカとしてもよい。
この場合、アラーム音により、障害物との距離が接近していることを操作者に通知して、通過点を適切な位置に変更することを促すことができる。

通知部は、本体と障害物との距離が所定値以下である場合に、障害物が接近している旨の表示を行う表示装置としてもよい。
表示装置は、例えば、液晶ディスプレイ、ＬＥＤランプ、その他のディスプレイ装置とすることができ、スピーカのアラーム音と併用して、操作者に通知を行うことが可能である。

本発明に係る自律移動体では、教示走行モードにおいて適切な走行スケジュールを設定できる。また、再現走行モードにおいて走行スケジュールに沿った自律走行を完了できる。

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

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

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

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

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

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

障害物情報取得部２１は、測距センサ３１の検出信号に基づいて、本体１ａ（図５及び図６を参照）の周囲に存在する障害物の位置情報を取得する。記憶部２２は、走行経路に対応する環境地図を記憶する。自己位置推定部２３は、環境地図上における自己位置を推定する。
教示データ作成部２４は、教示走行モードにおいて、自己位置推定部２３により推定される環境地図上の自己位置を、その通過時刻と対応させた通過点データとして走行スケジュールを作成する。
走行命令生成部２５は、再現走行モードにおいて、走行スケジュールに沿って走行するように走行命令を生成するとともに、障害物情報取得部２１により取得した障害物の位置情報に基づいて進行方向に障害物が存在する場合に減速又は停止の走行命令を生成し、走行制御部２６に入力する。走行制御部２６は、アクチュエータに入力する制御量を作成し走行部２７に入力する。

自律移動体１は、教示走行モードにおいて、操作者が操作部３２を手動操作することにより、走行経路を手動走行させながら、通過時刻と通過時刻における通過点データの集合でなる走行スケジュールを作成する。この時、障害物情報取得部２１より取得した障害物の位置情報に基づいて、本体１ａと障害物との距離が所定値以下である場合に、通知部３３を介して、操作者に対して障害物に接近している旨の通知を行う。通知部３３をスピーカとした場合には、所定のアラーム音を発生して操作者に障害物の接近を通知することができる。また、通知部３３を液晶ディスプレイやＬＥＤランプ等の表示装置とした場合には、所定の通知表示を行うことで、操作者に障害物の接近を通知できる。

（２）機能ブロック
本発明の第１実施形態による自律移動体を以下に説明する。
図２は、本発明の第１実施形態が採用される清掃用ロボットの制御ブロック図である。
この清掃用ロボット４０は、操作者の手動操作により走行開始位置から走行終了位置まで手動走行させながら、通過時刻と通過時刻における通過点データの集合でなる走行スケジュールと環境地図復元用データを作成する教示走行モードと、走行スケジュールを再現しながら、走行開始位置から走行終了位置まで自律的に走行する再現走行モードとを実行する。
清掃用ロボット４０は、自律移動体１と、スロットル４２、清掃部ユーザインターフェイス４３を備える。清掃用ロボット４０は、さらに、清掃部（図示せず）を有している。清掃部は、自律移動体１の下部に設けられており、自律移動体１の移動と共に床面を清掃するための装置である。

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

清掃部ユーザインターフェイス４３は、清掃部（図示せず）による清掃動作の指示を操作者から受け付けるものであり、例えば、液晶ディスプレイ、操作ボタン、タッチパネル、その他操作スイッチの組合せで構成できる。清掃部ユーザインターフェイス４３に、液晶ディスプレイやタッチパネルなどの表示装置を備える場合には、制御部１１から出力される各種情報をこの清掃部ユーザインターフェイス４３に表示させることができ、教示走行モードにおいて障害物が接近した旨の通知もこの表示装置を介して表示させることができる。

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

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

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

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

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

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

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

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

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

不揮発メモリ６４は、制御部１１のブートプログラム、走行制御に関するプログラム、各種パラメータなどを記憶する。
障害物情報取得部２１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４の検出信号に基づいて本体１ａの周囲に位置する障害物の位置情報を取得する。

走行命令生成部２５は、走行制御部２６に走行命令を出力するものである。走行命令生成部２５は、教示走行モードにおいて、スロットル４２を介して入力される操作者からの指示入力に基づいて走行命令を走行制御部２６に出力する。また、走行命令生成部２５は、再現走行モードにおいて、ＳＬＡＭ処理部６３により推定されるグローバルマップ上の自己位置、更新されたグローバルマップ及び走行スケジュールに基づいて走行命令を生成し、走行制御部２６に出力する。
さらに、走行命令生成部２５は、再現走行モードにおいて、障害物情報取得部２１により取得した障害物の位置情報に基づいて進行方向に障害物が存在する場合に減速又は停止の走行命令を生成し、走行制御部２６に入力する。

走行制御部（モーションコントローラ）２６は、入力される走行命令に基づいて走行部２７のアクチュエータの制御量を生成して、走行部２７に入力する。
走行制御部２６は、演算部８１、判断部８２、移動制御部８３を備えている。判断部８２は、入力された走行命令を解釈する。演算部８１は、判断部８２により解釈された走行命令に基づいて、走行部２７のアクチュエータの制御量を算出する。移動制御部８３は、演算部８１により算出された制御量を走行部２７に出力する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

この時、制御部１１は、グローバルマップにおける現在の自己位置（ｘ１，ｙ１，θ１）が走行開始位置として適切であるか否かの判断を行う。例えば、自己位置の周辺に障害物がほとんど存在しない場合又は単純形状が連続する場合には、再現走行モードにおいて走行開始位置を判定することが困難であることから、制御部１１は清掃部ユーザインターフェイス４３及び／又はスピーカ５７を通じて、走行開始位置として不適切である旨の通知を行って、操作者に走行開始位置の変更を促す。
また、制御部１１は、自己位置（ｘ１，ｙ１，θ１）と、グローバルマップ上に存在する障害物との距離が所定距離以下であれば、走行開始位置として不適切であると判断することもできる。この場合も、同様に、制御部１１が清掃部ユーザインターフェイス４３又はスピーカ５７を介して走行開始位置が不適切である旨の通知を行うことができる。

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

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

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

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

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

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

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

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

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

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

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



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

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

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

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

ステップＳ４５において、制御部１１は、障害物情報取得部２１より取得した障害物の位置情報に基づいて、本体１ａと障害物との距離が所定値以下であるか否かを判別する。具体的には、制御部１１は、本体１ａの進行方向前方に位置して障害物存在確率「１．０」であるグリッドが存在しており、自己位置からその障害物までの距離が所定値以下であると判断した場合に（ステップＳ４５でＹｅｓ）、ステップＳ４６に移行し、自己位置からその障害物までの距離が所定距離を超えると判断した場合に、ステップＳ４６をスキップしてステップＳ４７に移行する。ここでの所定値は、再現走行モードにおいて、本体１ａと障害物との距離が接近した場合に、減速又は停止の走行命令を生成する閾値とすることができる。

ステップＳ４６において、制御部１１は、操作者に対して障害物に接近している旨の通知を行う。具体的には、制御部１１は、スピーカ５７を介して所定のアラーム音を発生させて、操作者に進路を変更することを促す。これと同時に、制御部１１は、清掃部ユーザインターフェイス４３の表示部に、障害物に接近している旨の警告表示を作成して表示させることもできる。この後、制御部１１はステップＳ４７に移行する。
上記の通知により、教示走行モードにおいて作成される走行スケジュールは、自律移動体１の本体１ａと障害物との距離が所定距離以下になる箇所を含みにくい。

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

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

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

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

時刻（ｔ）におけるグローバルマップは、少なくとも次の時刻（ｔ＋１）において、制御部１１が自己位置推定処理を実行するために必要な範囲を含むものであり、一時的に記憶部２２に記憶される。
このグローバルマップ更新処理では、グローバルマップの各グリッドの障害物存在確率が、絶対座標系のローカルマップの対応する各グリッドの障害物存在確率を加算することで更新されるので、車輪のスリップやセンサの計測誤差に基づくノイズの影響を極力少なくすることができる。
ここで、制御部１１は、さらに、自律移動体１が通過後の通過点におけるグローバルマップを削除することもできる。この点は、時刻（ｔ）において更新されるグローバルマップとして必要なものは、次の時刻（ｔ＋１）において、制御部１１が自己位置推定処理を実行するために必要な範囲だけある。したがって、次の時刻（ｔ＋１）において、制御部が自己位置推定処理を実行するために必要でない範囲は削除することが可能である。このため、環境地図において、環状経路が閉じないという問題を考慮する必要がなくなる。

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

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

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

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

ステップＳ１６４において、制御部１１は、本体１ａと障害物との距離が所定値以下であるか否かを判別する。再現走行モードでは、自律移動体１が自律走行を実行していることから、障害物との接触を回避するために、障害物と接近した場合には、走行速度を減速するか、あるいは停止する必要がある。したがって、制御部１１は、本体１ａと障害物との距離が所定距離以下であると判断した場合には（ステップＳ１６４でＹｅｓ）、ステップＳ１６５に移行し、本体１ａと障害物との距離が所定距離を超えていると判断した場合には（ステップＳ１６４でＮｏ）、ステップＳ１６６に移行する。

ステップＳ１６５において、制御部１１は、走行命令生成部２５により停止の走行命令を生成させる。具体的に、制御部１１は、走行制御部２６から走行部２７に出力されるアクチュエータの制御量が０になるように、走行命令生成部２５に停止命令を出力させる。ステップＳ１６５において、本体１ａと障害物との接近を判断するための閾値を大きめに設定している場合には、制御部１１は走行命令生成部２５に減速命令を出力させることもできる。この後、制御部１１は処理を終了する。

ステップＳ１６６において、制御部１１は、推定された自己位置に対応するグローバルマップと、障害物情報取得部２１により取得した障害物の位置情報から作成したローカルマップと比較して、現在の自己位置に対応する走行スケジュール中の時刻を推定する。このとき、制御部１１は、予め記憶部２２に記憶されたグローバルマップ、又は教示走行モードにおいて作成し記憶部２２に記憶されたグローバルマップを用いることができる。また、教示走行モードにおいて、環境地図復元用データを記憶部２２に記憶させた場合には、制御部１１は、環境地図復元用データを用いて自己位置近傍のグローバルマップを復元し、これをローカルマップと比較することもできる。

ステップＳ１６７において、制御部１１は、記憶部２２に記憶されている走行スケジュールのうち推定した時刻に対応する通過点データを記憶部２２から読み出す。
ステップＳ１６８において、制御部１１はグローバルマップ更新処理を実行する。具体的に、制御部１１は、時刻（ｔ−１）におけるグローバルマップに対して、自己位置推定処理により推定された自己位置により座標変換を行った絶対座標系のローカルマップを足し合わせて、時刻（ｔ）におけるグローバルマップを更新する。このグローバルマップ更新処理では、グローバルマップの各グリッドの障害物存在確率が、絶対座標系のローカルマップの対応する各グリッドの障害物存在確率を加算することで更新されるため、車輪のスリップや摩耗、センサの計測誤差に基づくノイズの影響を極力少なくすることができる。
制御部１１は、教示走行モードにおいて、通過時刻に対応する環境地図復元用データを作成して記憶部２２に記憶させる場合には、通過点近傍におけるグローバルマップを記憶部２２に残し、通過後のグローバルマップの情報を削除することも可能である。

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

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

以上に述べた実施形態では、図１６に示す再現走行モードにおいて、自律移動体１と障害物との距離が所定値以下になった場合には（ステップＳ１６４でＹｅｓ）、減速又は停止を行っている（ステップＳ１６５）。しかし、この実施形態では、図４に示す教示走行モードにおいて、自律移動体１と障害物との距離が所定値以下になった場合には（ステップＳ４５でＹｅｓ）、操作者に通知を行って経路変更を促している（ステップＳ４６）。このことにより、教示走行モードにおいて作成される走行スケジュールは、自律移動体１の本体１ａと障害物との距離が所定距離以下になる箇所を含みにくい。その結果、図１６に示す再現走行モードにおいて、自律移動体１が走行スケジュール完了前に強制的に停止することを極力回避できる。

（４）実施形態の特徴
上記実施形態は、下記の構成及び機能を有している。
自律移動体１（自律移動体の一例）は、教示走行モード（例えば、図４のステップＳ４０〜Ｓ５０）と、再現走行モード（例えば、図１６のステップＳ１６１〜Ｓ１７０）とを実行する。教示走行モードは、操作者の手動操作により、走行経路を手動走行させながら、通過時刻と通過時刻における通過点データの集合でなる走行スケジュールを作成する。再現走行モードは、走行スケジュールを再現しながら、走行経路を自律的に走行する。
自律移動体１は、本体１ａ（本体の一例）と、走行部２７（走行部の一例）と、走行制御部２６（走行制御部の一例）と、障害物情報取得部２１（障害物情報取得部の一例）と、記憶部２２（記憶部の一例）と、自己位置推定部２３（自己位置推定部の一例）と、教示データ作成部２４（教示データ作成部の一例）と、走行命令生成部２５（走行命令生成部の一例）と、通知部３３（通知部の一例）とを備えている。
走行部２７は、制御量が入力されるモータ９３，９４（アクチュエータの一例）を有する。走行制御部２６は、モータ９３，９４に入力する制御量を作成し走行部２７に入力する（例えば、ステップＳ４１、ステップＳ１６９）。障害物情報取得部２１は、本体１ａの周囲に存在する障害物の位置情報を取得する（例えば、ステップＳ４２、ステップＳ１６１）。記憶部２２は、走行経路に対応する環境地図を記憶する。自己位置推定部２３は、環境地図の上における自己位置を推定する（例えば、ステップＳ４４、Ｓ１６３）。教示データ作成部２４は、教示走行モードにおいて、自己位置推定部２３により推定される環境地図上の自己位置を、その通過時刻と対応させた通過点データとして走行スケジュールを作成する（例えば、ステップＳ４９）。走行命令生成部２５は、再現走行モードにおいて、走行スケジュールに沿って走行するように走行命令を生成するとともに（例えば、ステップＳ１６９）、障害物情報取得部２１により取得した障害物の位置情報に基づいて進行方向に障害物が存在する場合に減速又は停止の走行命令を生成し、走行制御部２６に入力する（例えば、ステップＳ１６５）。通知部３３は、教示走行モードにおいて、障害物情報取得部２１より取得した障害物の位置情報に基づいて、本体１ａと障害物との距離が所定値以下である場合に、障害物に接近している旨の通知を行う（例えば、ステップＳ４６）。

この自律移動体１では、再現走行モードにおいて、進行方向に障害物が存在する場合に（具体的には、本体１ａと障害物との距離が所定値以下になる場合に）、走行命令生成部２５により停止又は減速の走行命令を生成して、障害物との接触を回避する。一方、教示走行モードにおいては、本体１ａと障害物との距離が所定値以下になると、通知部３３を介して操作者に通知を行う。このことにより、教示走行モードにおいて作成される走行スケジュールは、本体１ａと障害物との距離が所定距離以下になる箇所を含みにくい。その結果、再現走行モードにおいて、走行スケジュールの途中で減速又は停止することが減り、自律走行を円滑に行うことができる。

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

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

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

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

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

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

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

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

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



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

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

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

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

Claims (3)

1. 操作者の手動操作により、走行経路を手動走行させながら、通過時刻と前記通過時刻における通過点データの集合でなる走行スケジュールを作成する教示走行モードと、前記走行スケジュールを再現しながら、前記走行経路を自律的に走行する再現走行モードとを実行する自律移動体であって、
   本体と、
   制御量が入力される複数のアクチュエータを有する走行部と、
   前記アクチュエータに入力する制御量を作成し前記走行部に入力する走行制御部と、
   前記本体の周囲に存在する障害物の位置情報を取得する障害物情報取得部と、
   前記走行経路に対応する環境地図を記憶する記憶部と、
   前記環境地図の上における自己位置を推定する自己位置推定部と、
   前記教示走行モードにおいて、前記自己位置推定部により推定される前記環境地図上の自己位置を、その通過時刻と対応させた通過点データとして走行スケジュールを作成する教示データ作成部と、
   前記再現走行モードにおいて、前記走行スケジュールに沿って走行するように走行命令を生成するとともに、前記障害物情報取得部により取得した障害物の位置情報に基づいて進行方向に障害物が存在する場合に減速又は停止の走行命令を生成し、前記走行制御部に入力する走行命令生成部と、
   前記教示走行モードにおいて、前記障害物情報取得部より取得した障害物の位置情報に基づいて、前記本体と障害物との距離が所定値以下である場合に、障害物に接近している旨の通知を行う通知部と、
   を備える自律移動体。
2. 前記通知部は、前記本体と障害物との距離が所定値以下である場合に、アラーム音を発生させるスピーカである、請求項１に記載の自律移動体。
3. 前記通知部は、前記本体と障害物との距離が所定値以下である場合に、障害物が接近している旨の表示を行う表示装置である、請求項１に記載の自律移動体。