JP2014219722A - 自律移動体 - Google Patents

Abstract

【課題】自律走行体において、自己位置の推定を確実に行い、走行スケジュールに沿った走行を適切に行うこを可能にする。
【解決手段】自律移動体１は、制御量が入力される複数のアクチュエータを有する走行部２５と、走行命令に基づいてアクチュエータに入力する制御量を生成する走行制御部２４と、所定の制御周期毎に、周囲に存在する障害物の位置情報を取得する障害物情報取得部２１と、障害物の位置情報に基づいて、本体１ａの周辺領域における障害物の有無を示す局所地図を作成する地図作成部２２と、局所地図が所定の条件を満たした場合に、走行制御部２４に強制停止の走行命令を出力する走行命令生成部２３とを備える。
【選択図】図１

Description

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

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

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

自律移動体では、例えば、車輪を駆動するアクチュエータ（モータ）の制御量に基づいて、自律移動体の移動量を累積加算して自己位置を推定する。この場合、車輪の摩耗の大小により、推定される自己位置が大きく異なってくる。同様に、車輪のスリップやセンサの計測誤差が累積されると、自律移動体が推定する自己位置にずれが生じ、走行スケジュールに沿った自律走行ができないという問題がある。
前述したように、自律移動体の本体の周辺に位置する障害物の位置情報に基づいて、環境地図上の自己位置を推定しながら自律走行を行う自律移動体の場合、アクチュエータの制御に基づいて推定された自己位置にずれがあっても、環境地図上の自己位置をマップマッチングにより補正を行うことが可能である。
しかしながら、このような自律移動体においても、本体の周辺に障害物がほとんど存在しない空間や類似する形状が連続する空間では、自己位置を正確に推定できないおそれがある。
また、操作者の手動操作に基づいて環境地図を作成しながら走行スケジュールを作成し、この走行スケジュールを自律走行により再現する自律移動体がある。このような自律移動体では、前述したような本体の周辺に障害物がほとんど存在しない空間や類似する形状が連続する空間に走行開始位置を設定すると、再現走行時に、走行開始位置を特定することができず、走行スケジュールに沿った自律走行ができない。
本発明の課題は、自律移動体において、自己位置の推定を確実に行い、走行スケジュールに沿った走行を適切に行うことを可能にすることにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地による自律移動体は、走行部と、走行制御部と、障害物情報取得部と、地図作成部と、走行命令生成部とを備える。
走行部は、制御量が入力される複数のアクチュエータを有する。走行制御部は、走行命令に基づいてアクチュエータに入力する制御量を生成する。障害物情報取得部は、所定の制御周期毎に、周囲に存在する障害物の位置情報を取得する。地図作成部は、障害物の位置情報に基づいて、本体の周辺領域における障害物の有無を示す局所地図を作成する。走行命令生成部は、局所地図が所定の条件を満たした場合に、走行制御部に強制停止の走行命令を出力する。また、所定の条件に近づきつつある状況の時に、音声出力や画像出力により、操作者にその旨を通知することもできる。例えば、所定位置から移動量１５ｍで強制停止するという条件であれば、所定位置からの移動量１０ｍを超えた時点で警告音又は音声通知、および／又は警告ランプ、警告表示などにより通知することができる。
地図作成部が作成した局所地図により、自己位置を推定することが困難であるような所定の条件を満たす場合には、走行命令生成部が、強制停止の走行命令を生成して走行部により走行を停止させる。
このようにして、自己位置の推定が困難な場所では、車輪の摩耗やスリップ、センサの計測誤差などに起因して所定の走行スケジュールと異なる位置を走行する事態を未然に防ぐことができる。

ここで、周辺領域の内に障害物が存在する割合が所定値以下であるような局所地図が所定の走行距離又は所定の走行時間連続する場合に、走行命令生成部は局所地図が所定の条件を満たしたと判断してもよい。
一般に、本体の周辺に障害物がほとんどないような場合には、環境地図上の自己位置の推定は、走行部の移動量にのみ依存することになる。したがって、このような場合には、車輪の摩耗やスリップ、センサの計測誤差などに起因して、自律移動体が所定の走行スケジュールと異なる位置を走行している可能性がある。このような場合に、本発明に係る自律移動体では、上記の判断に基づいて強制停止の走行命令を出力することで、走行スケジュールと異なる位置を走行することを未然に防止できる。

また、周辺領域内に存在する障害物が形状変化の小さい単純形状であるような局所地図が所定の走行距離又は所定の走行時間連続する場合に、走行命令生成部は局所地図が所定の条件を満たしたと判断してもよい。
例えば、走行経路に沿って平行な壁面が連続する場合や円形状の壁面が連続する場合、障害物情報に基づいて同様の形状の局所地図が連続することになる。このような場合についても、本発明に係る自律移動体では、上記の判断に基づいて強制停止の走行命令を出力することで、所定の走行スケジュールと異なる位置を走行することを未然に防ぐことができる。

自律移動体は、周囲環境の環境地図を記憶する記憶部と、環境地図上における自己位置を推定する自己位置推定部とをさらに備えてもよい。その場合、走行命令生成部は、自己位置推定部により推定された自己位置周辺の環境地図と、局所地図とを比較し、所定の割合を超えて相違する状態が、所定の走行距離又は所定の走行時間連続する場合に、所定の条件を満たしたと判断する。
この場合には、地図作成部において作成された局所地図と、自己位置の周辺に位置する環境地図とを比較して、大きく異なる場合には、走行中のずれが生じたと判断して強制停止させることができる。

走行開始前に地図作成部により作成された局所地図において、周辺領域内に障害物が存在する割合が所定値以下である場合、周辺領域内に存在する障害物が形状変化の小さい単純形状である場合、又は自己位置周辺の環境地図と所定の割合を超えて相違する場合に、走行命令生成部は、局所地図が所定の条件を満たしたと判断してもよい。
上記の条件の場合は、自律移動体は所定の走行スケジュールと異なる位置を走行する可能性がある。その理由は以下の通りである。第１に、周辺領域内に障害物がほとんどない位置又は同様形状の障害物を有する位置が多数あるような位置を走行開始位置とした場合、自律移動体は走行スケジュールと異なる走行開始位置を選択する可能性がある。第２に、局所地図上の障害物の形状が環境地図上の障害物の形状と大きく異なる場合には、走行スケジュールの走行開始位置と異なる可能性が高い。
このような場合には、走行命令生成部が強制停止の走行命令を出力することで、所定の走行スケジュールと異なる位置を走行することを未然に防ぐことができる。

自律移動体は、操作者の手動操作により走行開始位置から走行終了位置まで手動走行させながら、通過時刻と通過時刻における通過点データの集合でなる走行スケジュールを作成する教示走行モードと、走行スケジュールを再現しながら、走行開始位置から走行終了位置まで自律的に走行する再現走行モードとを実行してもよい。さらに、走行命令生成部は、教示走行モードにおいて局所地図が所定の条件を満たすか否かを判別してもよい。
この場合、教示走行モードにおいて、自己位置の推定が困難になるような通過点データを排除して、適切な走行スケジュールを作成することができる。

走行命令生成部は、再現走行モードにおいて局所地図が所定の条件を満たすか否かを判別してもよい。
この場合、再現走行モードにおいて、走行開始位置が走行スケジュールから大きくずれている場合に強制停止を実行して、さらに操作者に走行開始位置の再設定を促すことができる。

本発明の構成によれば、自己位置の推定を確実に行い、走行スケジュールに沿った走行を適切に行うことが可能な自律移動体を提供できる。

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

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

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

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

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

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

障害物情報取得部２１は、所定の制御周期毎に、本体１ａ（図５及び図６を参照）の周囲に存在する障害物の位置情報を取得する。
地図作成部２２は、障害物の位置情報に基づいて、本体１ａから所定距離の周辺領域における障害物の有無を示す局所地図を作成する。
走行制御部２４は、走行命令に基づいてアクチュエータに入力する制御量を生成する。
走行命令生成部２３は、局所地図が所定の条件を満たした場合に、走行制御部２４に強制停止の走行命令を出力する。

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

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

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

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

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

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

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

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

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

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

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

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

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

走行命令生成部２３は、走行制御部２４に走行命令を出力するものである。走行命令生成部２３は、教示走行モードにおいて、スロットル４２を介して入力される操作者からの指示入力に基づいて走行命令を走行制御部２４に出力する。また、走行命令生成部２３は、再現走行モードにおいて、ＳＬＡＭ処理部６３により推定されるグローバルマップ上の自己位置、更新されたグローバルマップ及び走行スケジュールに基づいて走行命令を生成し、走行制御部２４に出力する。
さらに、走行命令生成部２３は、地図作成部２２により作成されるローカルマップ上の障害物の位置情報が、所定の条件を満たすか否かを判別し、所定の条件を満たすと判断した場合に、走行制御部２４に強制停止の走行命令を出力する。

走行制御部（モーションコントローラ）２４は、入力される走行命令に基づいて走行部２５のアクチュエータの制御量を生成して、それを走行部２５に入力する。

走行制御部２４は、演算部８１、判断部８２、移動制御部８３を備えている。判断部８２は、入力された走行命令を解釈する。演算部８１は、判断部８２により解釈された走行命令に基づいて、走行部２５のアクチュエータの制御量を算出する。移動制御部８３は、演算部８１により算出された制御量を走行部２５に出力する。

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

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

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

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

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

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

（３−１）教示走行モード
図４は、教示走行モードにおける自律移動体の制御フローチャートである。
ステップＳ４０において、制御部１１は、初期設定を行う。具体的に、制御部１１は、前方レーザレンジセンサ５３及び後方レーザレンジセンサ５４の検出信号に基づいて、本体１ａの周囲に位置する障害物の位置情報を取得し、これに基づいて自機の周囲におけるローカルマップを作成する。
グローバルマップが予め記憶部２７に記憶されていない場合には、制御部１１は、障害物の位置情報に基づいて作成した時刻（ｔ１）における自己位置（ｘ１，ｙ１）が絶対座標系の原点（あるいは所定の座標）になるように、ローカルマップをグローバルマップとして置き換える。

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

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

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

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

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

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

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

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

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

ステップＳ４１において、制御部１１は、走行開始位置が適切であるか否かを判別する。具体的には、障害物情報取得部２１により取得した障害物の位置情報に基づいて作成されたローカルマップが、本体１ａの周辺にほとんど障害物がない位置、あるいは、単純形状が連続するような位置であれば、制御部１１は、ローカルマップが所定の条件を満たすと判断する。

図９は、走行経路の説明図である。
図９に示すように、環状の走行経路９０１内を自律移動体１が走行するものとし、走行スケジュールにおける任意の通過点を第１通過点９１１〜第５通過点９１５とする。
各第１通過点９１１〜第５通過点９１５において、地図作成部２２が、それぞれ障害物の位置情報に基づいて周辺領域のローカルマップ９２１〜９２５を作成する。

教示走行モードの走行開始位置として、図９の第３通過点９１３を選択した場合、本体１ａの周辺領域のローカルマップ９２３には、障害物情報が含まれていない。したがって、ローカルマップ９２３の各グリッドの障害物存在確率は「−１．０〜０」である。

図９において、走行経路９０１の第３通過点９１３の近傍には、各グリッドの障害物存在確率が「−１．０〜０」であるローカルマップが作成される通過点が複数存在する。したがって、このような第３通過点９１３を走行開始位置として選択した場合、再現走行モードにおいて、走行開始位置が走行スケジュールの走行開始位置に合致しているか否かを判別することができなくなる。
また、再現走行モードにおいて、偶然、走行開始位置を第３通過点９１３と合致させることができたとしても、走行方向（姿勢θ）を特定することができないことから、以後の自律走行制御が正確に行うことが困難となる。

教示走行モードの走行開始位置として、図９の第２通過点９１２を選択した場合、本体１ａの周辺領域のローカルマップ９２２は、左右両側の平行な壁面に対応する位置にグリッドの障害物存在確率が「１．０」であり、その間に位置するグリッドの障害物存在確率が「−１．０〜０」となる。
図９に示す例では、第２通過点９１２の走行方向前後に位置して、平行な壁面が左右に存在するだけの単純形状が連続しており、第２通過点９１２の近傍では、同様のローカルマップとなる通過点が複数存在している。また、走行経路９０１中には、第２通過点９１２から離れた位置であっても、ローカルマップと同様の形状となる点が存在しており、例えば、第４通過点９１４におけるローカルマップ９２４、第５通過点におけるローカルマップ９２５が類似するローカルマップとなる。
したがって、このような第２通過点９１２を走行開始位置として選択した場合、再現走行モードにおいて、走行開始位置が走行スケジュールの走行開始位置に合致しているか否かを判別することができなくなる。

このようにして、走行開始前に、地図作成部２２が作成したローカルマップ中に、障害物が存在する割合が非常に低い場合、あるいは障害物が存在する割合が所定値以下である場合には、制御部１１は、ローカルマップが所定条件を満たすと判断して（ステップＳ４１でＮｏ）、ステップＳ４２に移行し、そうでない場合には（ステップＳ４１でＹｅｓ）、ステップＳ４３に移行する。

ステップＳ４２において、制御部１１は、走行部２５における走行を強制停止させる。具体的には、走行命令生成部２３が、強制停止の走行命令を生成し、これを走行制御部２４に出力する。走行制御部２４は、強制停止の走行命令に基づいて、走行部２５のモータ９３，９４を停止させるための制御信号を送信する。この後、制御部１１は、処理を終了する。

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

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

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

ステップＳ４６において、制御部１１は、作成したローカルマップが所定の条件を満たしているか否かを判別する。具体的には、地図作成部２２により作成されたローカルマップ中に、障害物が存在する割合が所定値以下である場合、あるいは、ローカルマップ中に含まれる障害物の形状が単純形状である場合には、制御部１１は、ローカルマップが所定条件を満たすと判断してステップＳ４７に移行する。また、記憶部２７に予め記憶されたグローバルマップがある場合には、自己位置近傍に位置するグローバルマップと、作成されたローカルマップとを比較して、対応する各グリッドにおける障害物存在確率が大きく異なる場合にも、制御部１１は、ローカルマップが所定の条件を満たすと判断して、ステップＳ４７に移行する。

ステップＳ４７において、制御部１１は、作成されたローカルマップが所定の条件を満たす状態が、所定の走行距離で連続しているか、あるいは、所定の時間で連続しているか否かを判別する。具体的には、例えば、図９の第３通過点９１３の近傍を通過中である場合、ローカルマップ中に障害物が存在する割合が所定値以下である状態が所定距離又は所定時間連続して生じる。したがって、このような場合には、制御部１１は、グローバルマップ上の正確な自己位置を特定できないと判断し（ステップＳ４７でＹｅｓ）、ステップＳ４８に移行する。
また、例えば、図９の第２通過点の近傍を通過中であるような場合、ローカルマップ中に存在する障害物が単純形状である状態が所定距離又は所定時間連続する状態が生じる。このような場合にも、制御部１１は、グローバルマップ上の正確な自己位置を特定できないと判断し（ステップＳ４７でＹｅｓ）、ステップＳ４８に移行する。
さらに、制御部１１は、記憶部２７に格納されたグローバルマップと、地図作成部２２により作成されたローカルマップとを比較して、大きく異なる状態が所定距離又は所定時間連続していると判断した場合には（ステップＳ４７でＹｅｓ）、ステップＳ４８に移行する。例えば、グローバルマップが作成された時点から、走行経路中のレイアウトが大きく変更された場合には、制御部１１は、グローバルマップ上の正確な自己位置を特定できないと判断してステップＳ４８に移行する。
ステップＳ４７において、制御部１１は、ローカルマップが所定の条件を満たす状態が所定距離又は所定時間連続していないと判断した場合には（ステップＳ４７でＮｏ）、ステップＳ４９に移行する。

ステップＳ４８において、制御部１１は、走行部２５における走行を強制停止させる。具体的には、制御部１１は、走行命令生成部２３に強制停止の走行命令を生成させて、走行制御部２４に出力させる。
この後、制御部１１は、処理を終了する。

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

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

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

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

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

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



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

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

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

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

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

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

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

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

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

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

制御部１１は、教示走行モードにおいて、走行経路中のローカルマップをつなぎ合わせていき、全体のグローバルマップを作成し、これを記憶部２７に記憶させることができる。その場合、環状経路問題を解決するために、部分マップに分割し、各部分マップの連結点を選択設定できるように構成することが好ましい。また、ステップＳ５１において、通過時刻に対応する環境地図復元用データを作成して記憶部２７に記憶させる場合には、通過点近傍におけるグローバルマップを記憶部２７に残し、通過後のグローバルマップの情報を削除することも可能である。この場合には、環境地図において、環状経路が閉じないという問題を考慮する必要がなくなる。

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

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

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

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

ステップＳ１７３において、制御部１１は、地図作成部２２により作成されたローカルマップが、所定の条件を満たしているか否かを判別する。具体的には、地図作成部２２により作成されたローカルマップ中に、障害物が存在する割合が所定値以下である場合、あるいは、ローカルマップ中に含まれる障害物の形状が単純形状である場合には、制御部１１は、ローカルマップが所定条件を満たすと判断してステップＳ１７４に移行する。また、記憶部２７に記憶されているグルーバルマップのうち、自己位置近傍に位置するグローバルマップと、作成されたローカルマップとを比較して、対応する各グリッドにおける障害物存在確率が大きく異なる場合にも、制御部１１は、ローカルマップが所定の条件を満たすと判断して、ステップＳ１７４に移行する。

ステップＳ１７４において、制御部１１は、作成されたローカルマップが所定の条件を満たす状態が、所定の走行距離で連続しているか、あるいは、所定の時間で連続しているか否かを判別する。
具体的には、例えば、図９の第３通過点９１３の近傍を通過中である場合、ローカルマップ中に障害物が存在する割合が所定値以下である状態が所定距離又は所定時間連続して生じる。したがって、このような場合には、制御部１１は、グローバルマップ上の正確な自己位置を特定できないと判断し（ステップＳ１７４でＹｅｓ）、ステップＳ１７５に移行する。
また、例えば、図９の第２通過点の近傍を通過中であるような場合、ローカルマップ中に存在する障害物が単純形状である状態が所定距離又は所定時間連続する状態が生じる。このような場合にも、制御部１１は、グローバルマップ上の正確な自己位置を特定できないと判断し（ステップＳ１７４でＹｅｓ）、ステップＳ１７５に移行する。
さらに、制御部１１は、記憶部２７に格納されたグローバルマップと、地図作成部２２により作成されたローカルマップとを比較して、大きく異なる状態が所定距離又は所定時間連続していると判断した場合には、ステップＳ１７５に移行する。例えば、グローバルマップが作成された時点から、走行経路中のレイアウトが大きく変更された場合には、制御部１１は、グローバルマップ上の正確な自己位置を特定できないと判断してステップＳ１７５に移行する。
ステップＳ１７４において、制御部１１は、ローカルマップが所定の条件を満たす状態が所定距離又は所定時間連続していないと判断した場合には（ステップＳ１７４でＮｏ）、ステップＳ１７６に移行する。

ステップＳ１７５において、制御部１１は、走行部２５における走行を強制停止させる。具体的には、制御部１１は、走行命令生成部２３に強制停止の走行命令を生成させて、走行制御部２４に出力させる。
この後、制御部１１は、処理を終了する。

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

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

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

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

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

以上のようにして、図４を用いて説明した教示走行モードにおいて、走行開始位置が不適切である場合には（ステップＳ４１でＮｏ）、走行命令生成部２３により強制停止の走行命令を生成して、走行部２５による走行を停止させている（ステップＳ４２）。したがって、その後の再現走行モードにおいて、走行開始位置を特定できずに走行不能となる事態を回避できる。
また、教示走行モード（図４）及び再現走行モード（図１７）の両方において、自己位置の推定が困難な形状が連続する場合にも（教示走行モードの場合はステップＳ４７でＹｅｓ、再現走行モードの場合はステップＳ１７４でＹｅｓ）、走行命令生成部２３により強制停止の走行命令を生成して、走行部２５による走行を停止させている（教示走行モードの場合はステップＳ４８、再現走行モードの場合はステップＳ１７５）。この場合、車輪の摩耗やスリップ、センサの計測誤差などに起因して、自己位置の推定に大きなずれが生じて走行スケジュールに沿った走行ができないという状態を回避できる。
さらに、グローバルマップ作成時から大きなレイアウト変更があった場合にも、自己位置を特定することが難しくなり、走行不能となることを回避できる。

（４）実施形態の特徴
上記実施形態は、下記の構成及び機能を有している。
自律移動体１（自律移動体の一例）は、走行部２５（走行部の一例）と、走行制御部２４（走行制御部の一例）と、障害物情報取得部２１（障害物情報取得部の一例）と、地図作成部２２（地図作成部の一例）と、走行命令生成部２３（走行命令生成部の一例）とを備える。走行部２５は、制御量が入力される複数のモータ９３，９４（アクチュエータの一例）を有する。走行制御部２４は、走行命令に基づいてモータ９３，９４に入力する制御量を生成する（例えば、ステップＳ４３、ステップＳ１８０）。障害物情報取得部２１は、所定の制御周期毎に、周囲に存在する障害物の位置情報を取得する（例えば、ステップＳ４４、ステップＳ１７１）。地図作成部２２は、障害物の位置情報に基づいて、本体１ａの周辺領域における障害物の有無を示すローカルマップ（局所地図の一例）を作成する（例えば、ステップＳ４５、ステップＳ１７２）。走行命令生成部２３は、ローカルマップが所定の条件を満たした場合に、走行制御部２４に強制停止の走行命令を出力する（例えば、ステップＳ４２，Ｓ４８，Ｓ１７５）。

この自律移動体１では、障害物情報取得部２１により取得した障害物の位置情報に基づいて本体１ａ周辺に位置するローカルマップを作成し、確実な自己位置の推定が困難な通過点が連続する場合や確実な自己位置の推定が困難な位置を走行開始位置に設定された場合のように所定の条件を満たす時、走行命令生成部２３により強制停止の走行命令を生成する。このことにより、自律移動体１の自律走行時に、車輪の摩耗やスリップ、センサの計測誤差などに起因して、走行スケジュールを外れて走行することを防止できる。
（５）他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。

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

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

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

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

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

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

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

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


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

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

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

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

Claims (7)

1. 本体と、
   制御量が入力される複数のアクチュエータを有する走行部と、
   走行命令に基づいて前記アクチュエータに入力する制御量を生成する走行制御部と、
   所定の制御周期毎に、周囲に存在する障害物の位置情報を取得する障害物情報取得部と、
   前記障害物の位置情報に基づいて、本体の周辺領域における障害物の有無を示す局所地図を作成する地図作成部と、
   前記局所地図が所定の条件を満たした場合に、前記走行制御部に強制停止の走行命令を出力する走行命令生成部と、
   を備える自律移動体。
2. 前記周辺領域の内に障害物が存在する割合が所定値以下であるような前記局所地図が所定の走行距離又は所定の走行時間連続する場合に、前記走行命令生成部は前記局所地図が所定の条件を満たしたと判断する、請求項１に記載の自律移動体。
3. 前記周辺領域内に存在する障害物が形状変化の小さい単純形状であるような前記局所地図が所定の走行距離又は所定の走行時間連続する場合に、前記走行命令生成部は前記局所地図が所定の条件を満たしたと判断する、請求項１又は２に記載の自律移動体。
4. 周囲環境の環境地図を記憶する記憶部と、
   前記環境地図の上における自己位置を推定する自己位置推定部と、
   をさらに備え、
   前記走行命令生成部は、前記自己位置推定部により推定された自己位置周辺の環境地図と、前記局所地図とを比較し、所定の割合を超えて相違する状態が、所定の走行距離又は所定の走行時間連続する場合に、所定の条件を満たしたと判断する、請求項１〜３のいずれかに記載の自律移動体。
5. 走行開始前に前記地図作成部により作成された局所地図において、前記周辺領域内に障害物が存在する割合が所定値以下である場合、前記周辺領域内に存在する障害物が形状変化の小さい単純形状である場合、又は自己位置周辺の環境地図と所定の割合を超えて相違する場合に、前記走行命令生成部は、前記局所地図が所定の条件を満たしたと判断する、請求項４に記載の自律移動体。
6. 操作者の手動操作により走行開始位置から走行終了位置まで手動走行させながら、通過時刻と前記通過時刻における通過点データの集合でない走行スケジュールを作成する教示走行モードと、前記走行スケジュールを再現しながら、前記走行開始位置から前記走行終了位置まで自律的に走行する再現走行モードとを実行し、
   前記走行命令生成部は、前記教示走行モードにおいて前記局所地図が所定の条件を満たすか否かを判別する、請求項５に記載の自律移動体。
7. 前記走行命令生成部は、前記再現走行モードにおいて前記局所地図が所定の条件を満たすか否かを判別する、請求項６に記載の自律移動体。