JP2019016089A - 自律移動装置、自律移動方法及びプログラム - Google Patents

Abstract

【課題】自律移動装置の高精度の地図が作成できない場合の対応を改善する。【解決手段】自律移動装置１００は、駆動系位置推定部１１と、非駆動系位置推定部１２と、物体位置取得部１３と、地図作成部１４と、地図制御部１６と、を備える。駆動系位置推定部１１は駆動部からの情報に基づいて自己位置を推定する。非駆動系位置推定部１２は駆動部以外からの情報に基づいて自己位置を推定する。物体位置取得部１３は周囲の物体の位置を取得する。地図作成部１４は駆動系位置推定部１１又は非駆動系位置推定部１２が推定した自己位置と物体位置取得部１３が取得した位置とに基づき、環境地図を地図記憶部２３に作成する。地図制御部１６は非駆動系位置推定部１２が自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに地図記憶部２３に記憶されている環境地図を、自己位置を推定できるトラッキングタイミングになった場合に使用する。【選択図】図１

Description

本発明は、自律移動装置、自律移動方法及びプログラムに関する。

用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、屋内の掃除のために自律的に移動する自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自己位置の推定を行う必要がある。

実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。ＳＬＡＭ法では、カメラの撮影する動画像中の複数のフレームから、同一の特徴点を追跡することによって、自己位置（自機の３次元位置）と特徴点の３次元位置とを交互に推定する処理を行う。例えば特許文献１には、ＳＬＡＭ法を用いて周囲の地図を作成しながら移動することができる移動式ロボットが開示されている。

特開２０１６−５２５１５号公報

ＳＬＡＭ法は、特徴点の３次元位置を用いて自己位置を推定するため、３次元位置を推定可能な特徴点の個数が少ないと、自己位置の推定精度が低下したり、自己位置の推定が不可能（ロスト状態）になったりする。自律移動装置が作成する地図は、自己位置の情報を用いて作成するため、自己位置の推定精度が低下すると、作成する地図の精度も低下してしまう。そして、特許文献１には、高精度の地図を作成できない（作成する地図の精度が低下してしまう）場合にどのような処理をすべきかについての記載はない。したがって、従来の自律移動装置では、高精度の地図を作成できない場合の対応に改善の余地がある。

本発明は、上記問題を解決するためになされたものであり、自律移動装置の高精度の地図を作成できない場合の対応を改善することを目的とする。

上記目的を達成するため、本発明の一態様は、
駆動部からの情報に基づいて自己位置を推定する駆動系位置推定部と、
前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定部と、
周囲の物体の位置を取得する物体位置取得部と、
前記駆動系位置推定部又は前記非駆動系位置推定部が推定した自己位置と、前記物体位置取得部が取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成部と、
前記非駆動系位置推定部が前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御部と、
を備える自律移動装置を提供するものである。

本発明によれば、自律移動装置の高精度の地図を作成できない場合の対応を改善することができる。

本発明の実施形態１に係る自律移動装置の機能構成を示す図である。 実施形態１に係る自律移動装置の外観を示す図である。 （Ａ）測域センサでのスキャン例を示す図である。（Ｂ）測域センサで得られる測距データの例を示す図である。 実施形態１に係る自律移動装置のソフトウェアモジュールの全体構成を示す図である。 実施形態１に係る自律移動装置が解決する課題を説明する図である。 実施形態１に係る自律移動装置による課題解決方法を説明する図である。 実施形態１に係る地図作成モジュールのフローチャートである。 実施形態１に係る自己位置推定処理のフローチャートである。 実施形態１に係る移動制御モジュールのフローチャートである。 本発明の実施形態２に係る自律移動装置による課題解決方法を説明する図である。 実施形態２に係る自律移動装置の機能構成を示す図である。 実施形態２に係る地図作成モジュールのフローチャートである。 本発明の変形例２に係る自律移動装置の機能構成を示す図である。 変形例２に係る自律移動装置の外観を示す図である。 変形例２に係る自律移動装置のソフトウェアモジュールの全体構成を示す図である。 変形例２に係る地図作成モジュールのフローチャートである。

以下、本発明の実施形態に係る自律移動装置、自律移動方法及びプログラムについて、図表を参照して説明する。なお、図中同一又は相当部分には同一符号を付す。

（実施形態１）
本発明の実施形態に係る自律移動装置は、周囲の地図を作成しながら、用途に応じて自律的に移動する装置である。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用等である。

図１に示すように、本発明の実施形態１に係る自律移動装置１００は、制御部１０、記憶部２０、センサ部３０、撮像部４１、駆動部４２、通信部４３、を備える。

制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（駆動系位置推定部１１、非駆動系位置推定部１２、物体位置取得部１３、地図作成部１４、移動制御部１５、地図制御部１６）の機能を実現する。また、制御部１０は、タイマー（図示せず）を備え、経過時間をカウントすることができる。

記憶部２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ)、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等で構成され、機能的に、画像記憶部２１、特徴点記憶部２２、地図記憶部２３、地図保管部２４及び測距データ記憶部２５を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラムや、プログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。

画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量を節約するために、撮影した全ての画像を記憶しなくてもよい。自律移動装置１００は、画像記憶部２１に記憶された複数の画像を用いて、ＳＬＡＭ処理により、自機位置（自己位置及び向き）の推定を行う。自機位置の推定に用いた画像については、その画像の情報と共に、その画像を撮影した時の自機位置の情報も記憶される。

特徴点記憶部２２には、画像記憶部２１に記憶された画像に含まれている特徴点のうち、実空間における３次元位置（Ｘ，Ｙ，Ｚ）が求められた特徴点について、その３次元位置とその特徴点の特徴量とが紐付けられて記憶される。特徴点とは画像中のコーナー部分等、画像内の特徴的な部分のことであり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄｅｄ Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。また、特徴点の特徴量は、例えばＳＩＦＴ等で得られる特徴量である。上述のＳＬＡＭ処理では、特徴点記憶部２２に記憶されている特徴点の３次元位置に基づいて自己位置の推定を行うため、特徴点記憶部２２は、ＳＬＡＭ用の地図の情報を記憶していると考えることもできる。

地図記憶部２３には、センサ部３０からの情報に基づいて地図作成部１４が作成した環境地図が記憶される。環境地図は、床面を例えば５ｃｍ×５ｃｍの格子（グリッド）に分割し、各格子点に対応する位置（格子点を中心とする、該格子と同一の形状の領域）における障害物の存在確率を、該格子点の値として表した占有格子地図である。なお、「各格子点に対応する位置」の代わりに、「各格子に対応する位置（格子の領域）」を用いても良い。なお、占有格子地図の各格子点の値は、その場所に障害物が存在する可能性が高いほど大きな値になり、障害物が存在しない可能性が高いほど小さな値になるので、便宜上「存在確率」と呼んでいるだけで、厳密な存在確率である必要はない。占有格子地図の各格子点の値として記録する存在確率の数値としては、存在確率Ｐ（０から１までの範囲の実数）の値そのものでも良いが、本実施形態においては対数オッズＬ（整数）を用いる。ＰとＬとの関係はｋを正の定数とすると、以下の式（１）で表すことができる。
Ｐ＝１−１／（１＋ｅｘｐ（ｋＬ））・・・（１）

式（１）から分かるように、Ｐ＝０．５の時にＬ＝０、Ｐ＝０の時にＬ＝−∞、Ｐ＝１の時にＬ＝∞となり、対数オッズを用いることによって、確率を整数値で扱うことができる。そして、各格子点の値Ｌを基準値と比較した時の大小によって、その格子点に対応する位置における障害物の存否を表すことができる。なお、本実施形態においては、Ｌの値として、最小値（Ｌｍｉｎ）を−１２７、最大値（Ｌｍａｘ）を１２７とし、占有格子地図の各格子点の値の初期値はＬ＝０とすることとする。そして、例えばＬが存在基準値（例えば１０）より大きければその格子点に障害物が存在し、Ｌが不存在基準値（例えば−１０）未満ならその格子点には障害物は存在せず、Ｌが不存在基準値以上、存在基準値未満ならその格子点における障害物の存否は不明として扱うこととする。もっともこれらの値は任意に変更可能である。

地図保管部２４は、地図制御部１６が、地図記憶部２３に記憶されている環境地図の情報を保存するために用いる記憶領域である。

測距データ記憶部２５には、後述する測域センサ３１が検出した物体までの距離を示す測距データが、該物体が検出された角度の情報とともに記憶される。

センサ部３０は、自律移動装置１００の周囲に存在する物体を検出するセンサを備える。実施形態１においては、センサ部３０は、周囲に存在する物体を検出し、該物体までの距離（測距データ）を取得する測域センサ３１を備える。測域センサ３１は、例えば、図２に示すように自律移動装置１００の上部に備えられた２次元レーザースキャナにより構成される。測域センサ３１は、図３に示すように、所定の角度範囲（例えば−３０度から＋３０度までの範囲）でレーザー光によるスキャニングをすることにより、周囲の当該角度範囲に存在する物体までの距離（測距データ）を所定の角度（例えば１度）毎に取得することができる。その方向に物体が無ければ、その角度に対する測距データは無しとなる。例えば、測域センサ３１が図３（Ａ）に示すスキャンをすると、図３（Ｂ）に示す測距データを取得できる。測域センサ３１は、定期的に（例えば１秒間に３０回の頻度で）、周囲を観測（スキャン）し、物体を検出すると、該物体までの距離を示す測距データと該物体が検出された角度の情報とを制御部１０に送信する。すると、制御部１０は、これらの情報（物体までの距離及び該物体が検出された角度）を測距データ記憶部２５に登録する。

撮像部４１は、図２に示すように、単眼の撮像装置（カメラ）を備える。撮像部４１は、例えば、３０ｆｐｓ（ｆｒａｍｅｓ ｐｅｒ ｓｅｃｏｎｄ）で画像（フレーム）を取得する。自律移動装置１００は、撮像部４１が逐次取得した画像に基づいて、ＳＬＡＭ処理により、自己位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。

駆動部４２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。図２では、右の車輪を駆動部４２ａ、左の車輪を駆動部４２ｂとして示している。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。

例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲ_Ｒ、左車輪の回転数をＲ_Ｌとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（Ｒ_Ｌ−Ｒ_Ｒ）／（２×Ｉ）となる。この並進移動量や回転量を逐次足し合わせていくことで、駆動部４２は、いわゆるオドメトリとして機能し、自機の位置（移動開始時の自己位置及び向きを基準とした自己位置及び向き）を推定するのに用いることができる。ここでは、自機の「向き」のことを自機の「姿勢」とも言うこととする。

ただし、オドメトリから得られる自己位置及び向きの精度は、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。特に誤差が累積していく関係上、時間経過と共に精度が悪化する。なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自己位置や向き（姿勢）の推定が可能である。

通信部４３は、外部装置と通信するためのモジュールであり、外部装置と無線通信する場合にはアンテナを含む無線モジュールである。例えば、通信部４３は、Ｂｌｕｅｔｏｏｔｈ（登録商標）に基づく近距離無線通信を行うための無線モジュールである。通信部４３を用いることにより、自律移動装置１００は、外部とデータの受け渡し等を行うことができる。例えば、ユーザが自律移動装置１００に目的地を指示する際には、この通信部４３を介して目的地の情報を伝えても良い。

次に、制御部１０の機能について説明する。制御部１０は、駆動系位置推定部１１、非駆動系位置推定部１２、物体位置取得部１３、地図作成部１４、移動制御部１５及び地図制御部１６を含み、自己位置の推定、環境地図の作成、自律移動装置１００の移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して実行することができる。

駆動系位置推定部１１は、駆動部４２からオドメトリの情報を取得して、自律移動装置１００の自己位置及び姿勢を推定する。

非駆動系位置推定部１２は、撮像部４１が撮影し画像記憶部２１に記憶された画像（撮影画像）を複数用いて、ＳＬＡＭ処理により自律移動装置１００の自己位置及び姿勢を推定する。ＳＬＡＭ処理においては、画像中の特徴点の数が少ないと自己位置の推定を行うことができない。ここでは、ＳＬＡＭ処理で自己位置の推定を行うことが可能か否かを表す情報をトラッキング情報と呼び、非駆動系位置推定部１２は、このトラッキング情報も取得する。トラッキング情報は、非駆動系位置推定部１２が自己位置の推定が可能であれば「トラッキング中」又は「トラッキング状態」であり、自己位置の推定ができなければ「ロスト中」又は「ロスト状態」である。

物体位置取得部１３は、センサ部３０からの情報を用いて周囲の物体の位置（自律移動装置１００の位置及び姿勢を基準とする相対位置）を取得する。より具体的には、物体位置取得部１３は、センサ部３０が備える測域センサ３１が検出した物体までの距離を示す測距データと、該物体が検出された角度と、を取得する。

地図作成部１４は、駆動系位置推定部１１又は非駆動系位置推定部１２が推定した自己位置及び姿勢と、物体位置取得部１３が取得した周囲の物体の位置（自律移動装置１００の位置及び姿勢を基準とする相対位置）と、に基づき、該物体の存在する位置を記録した環境地図を作成し、地図記憶部２３に記憶する。この環境地図は、上述したように平面を格子に分割し、各格子点にセンサ部３０で検出された物体の存在確率を示す数値を記録したものである。

移動制御部１５は、地図記憶部２３に記憶されている環境地図の情報に基づいて、目的地までの移動経路を設定し、設定した移動経路に沿って自機を移動させるように、駆動部４２を制御する。

地図制御部１６は、非駆動系位置推定部１２が自己位置を推定できなくなった場合に、その時点で地図記憶部２３に記憶されている環境地図の情報を固定する。ここで「環境地図の情報を固定する」とは、環境地図の情報を地図保管部２４に保存することと、環境地図の情報の更新を停止することが含まれる。そして、地図制御部１６は、その後、非駆動系位置推定部１２が自己位置を推定できるようになった場合に、環境地図の情報の固定を解除する。ここで「環境地図の情報の固定を解除する」とは、環境地図の情報の固定を環境地図の情報を地図保管部２４に保存することによって行っていた場合には、地図保管部２４に保存した環境地図を地図記憶部２３に戻すことを意味する。また、環境地図の情報の固定を環境地図の情報の更新を停止することによって行っていた場合には、環境地図の更新を再開することを意味する。

以上、自律移動装置１００の機能構成について説明した。次に、自律移動装置１００の制御部１０が実行するソフトウェアモジュールの全体構成について、図４を参照して説明する。図４中、非駆動系位置推定モジュール５１は上記機能構成で説明した非駆動系位置推定部１２に対応し、駆動系位置推定モジュール５２は上記機能構成で説明した駆動系位置推定部１１に対応する。また、地図作成モジュール５３は地図作成部１４に対応し、移動制御モジュール５４は移動制御部１５に対応する。

これらのソフトウェアモジュールは、自律移動装置１００の電源が入ると、それぞれ別スレッドとして起動し、並行して実行が開始される。非駆動系位置推定モジュール５１は、撮像部４１から取得する画像情報を用いてＳＬＡＭ処理を行い、自己位置及び姿勢を推定する。駆動系位置推定モジュール５２は、駆動部４２から取得するオドメトリ情報を用いて、自己位置及び姿勢を推定する。地図作成モジュール５３は、非駆動系位置推定モジュール５１又は駆動系位置推定モジュール５２が推定した自己位置及び姿勢と、測域センサ３１から得られる測距データ及び角度と、に基づき、環境地図を作成する。

移動制御モジュール５４は、非駆動系位置推定モジュール５１又は駆動系位置推定モジュール５２が推定した自己位置及び姿勢と、地図作成モジュール５３が作成した環境地図と、に基づき、経路を設定し、設定した経路に基づき、駆動部４２を制御する移動制御情報（主に速度情報）を生成する。そして、駆動部４２は、移動制御モジュール５４が生成した移動制御情報に基づいて駆動され、目的地への移動が行われる。

ここで、自律移動装置１００が解決する課題について、図５を参照して説明する。図５の（１）に示すように、自律移動装置１００がＡ地点からＢ地点に左旋回しながら移動して、障害物７０に接近する動きをした場合を考える。この時、撮像部４１で取得した画像中に特徴点が多数含まれていれば、非駆動系位置推定部１２がＳＬＡＭ処理により自己位置を推定でき（トラッキング中）、地図作成部１４は、図５の（２）に示すような精度の高い環境地図を作成することができる。

しかし、撮像部４１で取得した画像中に特徴点が基準値（例えば１０個）以下しか含まれていなければ、非駆動系位置推定部１２はＳＬＡＭ処理による自己位置の推定が不可能（ロスト中）になる。すると、自律移動装置１００は、駆動系位置推定部１１のみによる自己位置の推定を行うことになるが、駆動系位置推定部１１による自己位置の推定は、上述したように、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。そのため、地図作成部１４が作成する環境地図は、図５の（３）に示すような精度の低い地図になってしまう。地図の精度が低いと、不適切な経路が設定されたり、経路が見つからなかったりすることがある。

そこで、図６に示すように、自律移動装置１００は、ＳＬＡＭ処理がロスト状態になったらその時点（ロストタイミング：Ｔ１１）の環境地図を保存し、ＳＬＡＭ処理がトラッキング状態に復帰したらその時点（トラッキングタイミング：Ｔ２）で先ほど保存した環境地図を読み込むようにすれば、ロスト中の精度の低い環境地図の情報を削除することができるので、環境地図の精度低下を防止できる。なお、環境地図を保存するタイミングを基準タイミング（Ｔ１２）と呼ぶことにする。本実施形態においては、基準タイミング（Ｔ１２）＝ロストタイミング（Ｔ１１）である。

では、自律移動装置１００が、図５の（２）に示すような精度の高い環境地図をできるだけ長時間保つことができるように、ＳＬＡＭ処理による自己位置の推定が不可能になった場合に、環境地図を一時的に保存し、ＳＬＡＭ処理による自己位置の推定が可能になった時に、一時保存した環境地図を復帰させる仕組みについて、以下に説明する。まず、地図作成モジュール５３の処理内容について、図７を参照して説明する。なお、以下では、地図記憶部２３に記憶される環境地図を２次元配列変数ＭＡ［ｉ，ｊ］で表し、地図保管部２４に記憶される環境地図を２次元配列ＭＳ［ｉ，ｊ］で表すものとする。ここで、ｘｓｉｚｅを環境地図のＸ方向の最大の格子点の座標、ｙｓｉｚｅを環境地図のＹ方向の最大の格子点の座標とすると、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅである。また、地図保管部２４に環境地図が保存されているか否かを示すフラグ変数ＳＦも地図保管部２４に記憶されているものとする（ＳＦ＝０なら環境地図が保存されておらず、ＳＦ＝１なら保存されていることを示す）。

まず、地図作成部１４は、地図記憶部２３に記憶されている環境地図を初期化する（ステップＳ１０１）。また、この時、地図保管部２４も初期化する。環境地図の初期化は、占有格子地図の全ての格子点の値を０にすることにより行われる。具体的には、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅのすべてのｉ及びｊについて、ＭＡ［ｉ，ｊ］＝０を実行する。そして、地図保管部２４に環境地図が保存されているか否かを示すフラグ変数ＳＦに０をセットする。

次に制御部１０は、自律移動装置１００が動作を終了するか否かを判定する（ステップＳ１０２）。ユーザが自律移動装置１００の電源を落とした場合や、バッテリーの残量が所定量（例えば残り３％等）を下回った場合等に、自律移動装置１００は動作を終了する。自律移動装置１００が動作を終了するなら（ステップＳ１０２；Ｙｅｓ）、処理を終了する。動作を終了しないなら（ステップＳ１０２；Ｎｏ）、制御部１０は、自己位置推定処理を行って現在の自己位置及び姿勢を取得する（ステップＳ１０３）。自己位置推定処理の詳細については、後述する。

次に地図制御部１６は、非駆動系位置推定部１２が自己位置を推定できなくなった（ＳＬＡＭ処理のトラッキング情報がトラッキング状態からロスト状態に変化した）か否かを判定する（ステップＳ１０４）。非駆動系位置推定部１２が自己位置を推定できなくなった場合に（ステップＳ１０４；Ｙｅｓ）、地図制御部１６は、地図記憶部２３に記憶されている環境地図を、地図保管部２４に保存する（ステップＳ１０５）。具体的には、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅのすべてのｉ及びｊについて、ＭＳ［ｉ，ｊ］＝ＭＡ［ｉ，ｊ］を実行し、変数ＳＦに１をセットする。ステップＳ１０５は地図制御ステップとも呼ばれる。そして、ステップＳ１０８に進む。

トラッキング情報がトラッキング状態からロスト状態に変化していないなら（ステップＳ１０４；Ｎｏ）、地図制御部１６は、非駆動系位置推定部１２が自己位置を推定できるようになった（ＳＬＡＭ処理のトラッキング情報がロスト状態からトラッキング状態に変化した）か否かを判定する（ステップＳ１０６）。非駆動系位置推定部１２が自己位置を推定できるようになっていなければ（ステップＳ１０６；Ｎｏ）、ステップＳ１０８に進む。

非駆動系位置推定部１２が自己位置を推定できるようになった場合に（ステップＳ１０６；Ｙｅｓ）、地図制御部１６は、地図記憶部２３の環境地図をクリアして、地図保管部２４に保存されている環境地図を地図記憶部２３に読み込む（ステップＳ１０７）。具体的には、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅのすべてのｉ及びｊについて、ＭＡ［ｉ，ｊ］＝ＭＳ［ｉ，ｊ］を実行し、変数ＳＦに０をセットする。そして、ステップＳ１０８に進む。

ステップＳ１０８では、物体位置取得部１３は、測距データ記憶部２５から測距データ及び角度を取得する。ステップＳ１０８は、物体位置取得ステップとも呼ばれる。そして、地図作成部１４は、ステップＳ１０３で取得した自己位置及び姿勢の情報を用いて、物体位置取得部１３が取得した測距データ及び角度を環境地図における座標に変換し、環境地図に登録する（ステップＳ１０９）。ステップＳ１０９は、地図作成ステップとも呼ばれる。そして、ステップＳ１０２に戻る。

ステップＳ１０９における、測距データを環境地図に登録する具体例を以下に説明する。例えば、測域センサ３１で検出された物体の環境地図における座標が［ｍ，ｎ］であったとすると、地図作成部１４は、まず、ＭＡ［ｍ，ｎ］＝ＭＡ［ｍ，ｎ］＋Δｒを実行する。ここで、Δｒは、測域センサ３１が各格子点における物体の存在を確認した時に該格子点の値に加算する更新量であり、Δｒの値は例えば１０である。ただし、Δｒを加算するとＭＡ［ｍ，ｎ］が最大値Ｌｍａｘを超える場合には、ＭＡ［ｍ，ｎ］＝Ｌｍａｘとする。

次に、地図作成部１４は、測域センサ３１の観測（スキャン）範囲内で障害物が検出されなかった格子点の値から更新量Δｒを減算する。具体的には、障害物が検出された方向については［ｍ，ｎ］の手前の全ての［ｉ，ｊ］、障害物が検出されなかった方向については測域センサ３１の観測範囲内のすべての［ｉ，ｊ］に対して、ＭＡ［ｉ，ｊ］＝ＭＡ［ｉ，ｊ］−Δｒを実行する。ただし、Δｒを減算するとＭＡ［ｉ，ｊ］が最小値Ｌｍｉｎを下回る場合には、ＭＡ［ｉ，ｊ］＝Ｌｍｉｎとする。

以上の処理により、測域センサで物体が観測された格子点の値は大きくなり、測域センサの観測範囲内で物体が観測されなかった格子点の値は小さくなる。これにより、各格子点の値は、物体の存在確率を対数オッズで表したものとみなせるようになる。なお、環境地図の各格子点の値として、対数オッズＬの代わりに、観測回数（該格子点を測域センサ３１で観測（スキャン）した回数）と検出回数（該格子点の位置に障害物が存在することを測域センサ３１が検出した回数）のペアを記録し、各格子点における障害物の存在確率を、観測回数に対する検出回数の割合として定義しても良い。

以上で、図７に示す地図作成モジュール５３のフローチャートの説明を終えるが、図７の処理において、ロスト状態（ステップＳ１０４でＹｅｓ）となってからトラッキング状態（ステップＳ１０６でＹｅｓ）になるまでの間（ロスト期間）の環境地図は、ステップＳ１０７で地図保管部から元の環境地図（本来の環境地図）が読み込まれるまでの間の一時的な環境地図なので、簡易地図とも言う。そして、この一時的な環境地図（簡易地図）を用いている間は本来の環境地図（地図保管部２４に保存されている環境地図）の更新は停止されていると考えることができる。したがって、ステップＳ１０５の処理は、環境地図（の情報）を固定する処理と言える。また、ステップＳ１０７の処理により、簡易地図は本来の環境地図に置き換わり、本来の環境地図の更新が再開されるので、ステップＳ１０７の処理は、環境地図（の情報）の固定を解除する処理と言える。

次に、図７のステップＳ１０３で行われる自己位置推定処理について、図８を参照して説明する。まず、駆動系位置推定部１１が、駆動部４２から得られるオドメトリ情報に基づいて、自律移動装置１００の自己位置及び姿勢を取得する（ステップＳ２０１）。ステップＳ２０１は、駆動系位置推定ステップとも呼ばれる。次に、非駆動系位置推定部１２は、撮像部４１が撮影した画像の情報に基づくＳＬＡＭ処理のトラッキング情報を取得する（ステップＳ２０２）。

そして、制御部１０は、ＳＬＡＭ処理のトラッキング情報が「トラッキング中」であるか否かを判定する（ステップＳ２０３）。ＳＬＡＭ処理のトラッキング情報が「トラッキング中」なら（ステップＳ２０３；Ｙｅｓ）、非駆動系位置推定部１２は、撮像部４１が撮影した画像の情報に基づいて、ＳＬＡＭ処理により自律移動装置１００の自己位置及び姿勢を取得する（ステップＳ２０４）。ステップＳ２０４は、非駆動系位置推定ステップとも呼ばれる。そして、非駆動系位置推定部１２は、ＳＬＡＭ処理による自己位置及び姿勢を、制御部１０に出力し（ステップＳ２０５）、処理を終了する。

一方、ステップＳ２０３で、ＳＬＡＭ処理のトラッキング情報が「トラッキング中」でないなら（ステップＳ２０３；Ｎｏ）、駆動系位置推定部１１は、前回の自己位置推定処理のステップＳ２０１で取得した自己位置及び姿勢と、今回のステップＳ２０１で取得した自己位置及び姿勢と、の差分を求め、前回の自己位置推定処理で制御部１０に出力した自己位置及び姿勢に該差分を加算したものを、制御部１０に出力し（ステップＳ２０６）、処理を終了する。

以上で、図８に示す自己位置推定処理のフローチャートの説明を終えたので、次に、地図作成モジュール５３で作成された地図を用いて目的地までの経路を設定し、目的地まで移動制御を行う移動制御モジュール５４の処理内容について、図９を参照して説明する。

まず、制御部１０は、自律移動装置１００が動作を終了するか否かを判定する（ステップＳ３０１）。動作を終了するなら（ステップ３０１；Ｙｅｓ）、処理を終了する。動作を終了しないなら（ステップＳ３０１；Ｎｏ）、移動制御部１５は、目的地が設定されたか否かを判定する（ステップＳ３０２）。目的地は、自律移動装置１００のユーザが通信部４３を介して設定することもあるし、自律移動装置１００が必要に応じて（例えば、バッテリー残量が所定量（例えば１０％）を下回った場合に、充電ステーションを目的地に設定する等）自律的に設定することもある。

目的地が設定されていないなら（ステップＳ３０２；Ｎｏ）、ステップＳ３０１に戻る。目的地が設定されているなら（ステップＳ３０２；Ｙｅｓ）、移動制御部１５は、地図作成部１４が作成した、その時点で最新の環境地図を取得する（ステップＳ３０３）。次に、移動制御部１５は、図８に示す自己位置推定処理によって現在の自己位置及び姿勢を取得する（ステップＳ３０４）。そして、移動制御部１５は、取得した環境地図並びに自己位置及び姿勢と設定された目的地とに基づき、現在位置から目的地までの経路を設定する（ステップＳ３０５）。

そして、移動制御部１５は、経路が設定できたか否か（経路が存在するか否か）を判定する（ステップＳ３０６）。経路が存在しないなら（ステップＳ３０６；Ｎｏ）、ユーザにその旨を通知する等のエラー処理を行い（ステップＳ３０７）、ステップＳ３０１に戻る。経路が存在するなら（ステップＳ３０６；Ｙｅｓ）、移動制御部１５は、自機が目的地に到着したか否かを判定する（ステップＳ３０８）。なお、経路の情報には、目的地の情報も含まれるため、移動制御部１５は、現在の位置が経路に含まれる目的地と一致するか否かを判定することにより、目的地に到着したか否かを判定することができる。

目的地に到着したなら（ステップＳ３０８；Ｙｅｓ）、移動制御部１５は、駆動部４２を制御して移動を停止させ（ステップＳ３１０）、ステップＳ３０１に戻る。目的地に到着していないなら（ステップＳ３０８；Ｎｏ）、移動制御部１５は、経路に沿って移動するように駆動部４２を制御し（ステップＳ３０９）、ステップＳ３０３に戻る。

以上で、図９に示す移動制御モジュール５４のフローチャートの説明を終える。ＳＬＡＭ処理で自己位置の推定が行えない状態（ロスト状態）が長時間続くと環境地図の精度が低くなってしまうが、以上説明したように、自律移動装置１００は、ＳＬＡＭ処理がロスト状態になったときに環境地図を保存し、トラッキング状態に戻った時に該保存した環境地図を読み込むことによって、ロスト中に環境地図に登録された精度の低い情報を削除することができ、環境地図の精度を向上させることができる。

（実施形態２）
実施形態１にかかる自律移動装置１００は、ＳＬＡＭ処理がロスト中に環境地図に登録されたデータ（障害物の位置等の情報）は、ＳＬＡＭ処理がトラッキング状態に復帰した際に、失われてしまう。ＳＬＡＭ処理がロスト中は自己位置の推定精度が低いため、これらのデータの精度も低くなってしまうから、自律移動装置１００は、ＳＬＡＭ処理がロスト中のデータをＳＬＡＭ処理がトラッキング状態に復帰した時にすべて消去することで、環境地図には精度の高い情報のみを残すようにしている。しかし、トラッキング状態からロスト状態になった直後の一定期間はオドメトリの情報の精度の低下はそれほど大きくない。また、トラッキング状態に復帰する直前のオドメトリの情報は、復帰後の自己位置に基づいて逆算することによって、精度の低下を抑えることができる。そこで、ＳＬＡＭ処理のトラッキング情報がロスト状態に変化した直後及びトラッキング状態に変化する直前のそれぞれの一定期間は、オドメトリの情報を用いて推定した自己位置の情報を利用する実施形態２について説明する。

図１０に示すように、実施形態２に係る自律移動装置は、ＳＬＡＭ処理がロスト状態（ロストタイミング：Ｔ１１）になってもオドメトリの情報で自己位置を推定し、所定の時間（基準遅延時間）が経過してからその時点（基準タイミング：Ｔ１２）の環境地図を保存する。そして、ＳＬＡＭ処理がトラッキング状態に復帰したらその時点（トラッキングタイミング：Ｔ２）で先ほど保存した環境地図を読み込むとともに、トラッキング状態に復帰する直前のオドメトリの情報をトラッキング状態に復帰後の自己位置に基づいて補正し、この補正したオドメトリ情報に基づいてロスト中に得られた物体の位置を環境地図に登録する。このようにすれば、ロスト直後とトラッキング直前の、精度があまり低下していない環境地図の情報を失わずに、ロスト中の精度の低い環境地図の情報を削除することができるので、環境地図の精度低下を防止できる。本実施形態においては、基準タイミング（Ｔ１２）＝ロストタイミング（Ｔ１１）＋基準遅延時間である。

実施形態２に係る自律移動装置１０１は、図１１に示すように、実施形態１に係る自律移動装置１００の記憶部２０に物体位置記憶部２６が追加された構成である。

物体位置記憶部２６には、非駆動系位置推定部１２のＳＬＡＭ処理のトラッキング情報がロスト状態である間に、物体位置取得部１３が取得した周囲の物体の位置（自律移動装置１０１の位置及び姿勢を基準とする相対位置）と、駆動系位置推定部１１が推定した自己位置及び姿勢と、の基準保存時間（例えば３分間）分の履歴が記憶される。物体位置取得部１３は、測域センサ３１が検出した物体までの距離を示す測距データと、該物体が検出された角度と、を取得するので、物体位置記憶部２６は、この測距データ及び角度と、駆動系位置推定部１１が推定した自己位置及び姿勢と、の履歴が記憶される。

自律移動装置１０１の制御部１０が実行するソフトウェアモジュールの全体構成は、実施形態１に係る自律移動装置１００と同じであり、図４に示される。ただし、自律移動装置１０１の地図作成モジュール５３の処理は、実施形態１に係る地図作成モジュール５３の処理（図７）と異なるため、図１２を参照して説明する。この点以外は、自律移動装置１０１は、自律移動装置１００と同じである。

なお、実施形態１と同様、以下では、地図記憶部２３に記憶される環境地図を２次元配列変数ＭＡ［ｉ，ｊ］で表し、地図保管部２４に記憶される環境地図を２次元配列ＭＳ［ｉ，ｊ］で表し、地図保管部２４に環境地図が保存されているか否かを示すフラグ変数を変数ＳＦで表すものとする。また、物体位置記憶部２６に保存されるデータ（測域センサ３１から得られる測距データ及び角度、並びに、駆動系位置推定部１１から得られる自己位置及び姿勢）を、構造体の１次元配列変数ＢＳ［ｋ］で表し、これらのデータを保存した時刻を１次元配列変数ＴＳ［ｋ］で表し、物体位置記憶部２６に保存されているデータの個数を変数ＢＣで表すものとする。ＢＣの初期値は０であり、ｋの値の範囲は０≦ｋ＜ＢＣとなる。

まず、地図作成部１４は、地図記憶部２３に記憶されている環境地図を初期化する（ステップＳ４０１）。また、この時、地図保管部２４及び物体位置記憶部２６も初期化する。具体的には、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅのすべてのｉ及びｊについて、ＭＡ［ｉ，ｊ］＝０を実行する。そして、変数ＳＦ及び変数ＢＣに０をセットする。

次に制御部１０は、自律移動装置１００が動作を終了するか否かを判定する（ステップＳ４０２）。自律移動装置１００が動作を終了するなら（ステップＳ４０２；Ｙｅｓ）、処理を終了する。動作を終了しないなら（ステップＳ４０２；Ｎｏ）、物体位置取得部１３は、測距データ記憶部２５から測距データ及び角度を取得する（ステップＳ４０３）。そして、制御部１０は、自己位置推定処理を行って現在の自己位置及び姿勢を取得する（ステップＳ４０４）。この自己位置推定処理は、実施形態１と同じであり、図８に示す処理内容である。

次に、制御部１０は、ＳＬＡＭ処理のトラッキング状態が「ロスト中」であるか否かを判定する（ステップＳ４０５）。ここで判定対象となるトラッキング状態は、ステップＳ４０４で実行された自己位置推定処理（図８）のステップＳ２０２で取得されている。ＳＬＡＭ処理のトラッキング状態が「ロスト中」でなければ（ステップＳ４０５；Ｎｏ）、制御部１０は環境地図が地図保管部２４に保存されているか否かを判定する（ステップＳ４１１）。これはフラグ変数ＳＦの値が１か０かにより判定できる。環境地図が地図保管部２４に保存されていなければ（ステップＳ４１１；Ｎｏ）、ステップＳ４１９に進む。

環境地図が地図保管部２４に保存されているなら（ステップＳ４１１；Ｙｅｓ）、地図制御部１６は、地図記憶部２３の環境地図をクリアして、地図保管部２４に保存されている環境地図を地図記憶部２３に読み込み、地図保管部２４に保存されていた環境地図を削除する（ステップＳ４１２）。具体的には、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅのすべてのｉ及びｊについて、ＭＡ［ｉ，ｊ］＝ＭＳ［ｉ，ｊ］を実行し、変数ＳＦに０をセットする。

そして、地図制御部１６は物体位置記憶部２６にデータが存在するか否かを判定する（ステップＳ４１３）。具体的には変数ＢＣが１以上か０かを判定する。ＢＣが１以上ならデータが存在し、ＢＣが０ならデータが存在しないことを表す。物体位置記憶部２６にデータが存在しなければ（ステップＳ４１３；Ｎｏ）、ステップＳ４１９に進む。

物体位置記憶部２６にデータが存在するなら（ステップＳ４１３；Ｙｅｓ）、地図制御部１６は物体位置記憶部２６から測距データ及び角度並びに自己位置及び姿勢を取得する（ステップＳ４１４）。具体的には地図制御部１６は、構造体の１次元配列変数ＢＳ［ＢＣ−１］の値を取得する。そして、地図制御部１６は、ステップＳ４１４で取得した自己位置及び姿勢と、駆動系位置推定部１１が推定した最新の自己位置及び姿勢と、の差分を求める（ステップＳ４１５）。次に、地図制御部１６は、非駆動系位置推定部１２が推定した最新の自己位置及び姿勢に、ステップＳ４１５で求めた差分を加算して、過去の自己位置を推定する（ステップＳ４１６）。

そして、地図制御部１６は、推定した過去の自己位置の情報を用いて、ステップＳ４１４で取得した測距データ及び角度を環境地図における座標に変換し、環境地図に登録する（ステップＳ４１７）。そして、地図制御部１６は、ステップＳ４１４で取得した測距データ及び角度並びに自己位置及び姿勢を物体位置記憶部２６から削除する（ステップＳ４１８）。具体的には、変数ＢＣの値を１減らす。そして、ステップＳ４１３に戻る。

一方、ステップＳ４０５の判定で、ＳＬＡＭ処理のトラッキング状態が「ロスト中」なら（ステップＳ４０５；Ｙｅｓ）、地図制御部１６は、ＳＬＡＭ処理のトラッキング状態が「トラッキング中」から「ロスト中」に変化してから基準遅延時間（例えば３分）が経過したか否かを判定する（ステップＳ４０６）。基準遅延時間が経過していないなら（ステップＳ４０６；Ｎｏ）、ステップＳ４１９に進む。

基準遅延時間が経過したなら（ステップＳ４０６；Ｙｅｓ）、地図制御部１６は、地図保管部２４に環境地図が保存済みか否かを判定する（ステップＳ４０７）。具体的には変数ＳＦが１か０かを判定する。環境地図が地図保管部２４に保存済みなら（ステップＳ４０７；Ｙｅｓ）、ステップＳ４０９に進む。環境地図が地図保管部２４に保存済みでないなら（ステップＳ４０７；Ｎｏ）、地図制御部１６は、地図記憶部２３に記憶されている環境地図を地図保管部２４に保存する（ステップＳ４０８）。具体的には、０≦ｉ≦ｘｓｉｚｅ，０≦ｊ≦ｙｓｉｚｅのすべてのｉ及びｊについて、ＭＳ［ｉ，ｊ］＝ＭＡ［ｉ，ｊ］を実行し、変数ＳＦに１をセットする。そして、ステップＳ４０９に進む。

ステップＳ４０９では、地図制御部１６は、ステップＳ４０３で取得した測距データ及び角度と、ステップＳ４０４で取得した自己位置及び姿勢と、を物体位置記憶部２６に保存する。具体的には構造体の１次元配列変数ＢＳ［ＢＣ］にこれらの値（測距データ及び角度並びに自己位置及び姿勢）を保存し、現在時刻を１次元配列変数ＴＳ［ＢＣ］に保存し、変数ＢＣの値を１加算する。

次に、地図制御部１６は、物体位置記憶部２６に保存されているデータのうち、基準保存時間より前のデータを物体位置記憶部２６から削除する（ステップＳ４１０）。具体的には０≦ｋ＜ＢＣ−１の範囲の１次元配列ＴＳ［ｋ］の値のうち、（現在時刻−基準保存時間）より小さいものを探し、見つかったら（見つかったものをＴＳ［ｐ］とする）、０≦ｋ＜ＢＣ−ｐ−１となるすべてのｋについて、ｋの小さい方から順にＢＳ［ｋ］＝ＢＳ［ｋ＋ｐ＋１］及びＴＳ［ｋ］＝ＴＳ［ｋ＋ｐ＋１］を実行する。そして、変数ＢＣからｐ＋１を引く。

そして、地図作成部１４は、ステップＳ４０４で取得した自己位置及び姿勢の情報を用いて、ステップＳ４０３で取得した測距データ及び角度を環境地図における座標に変換し、環境地図に登録する（ステップＳ４１９）。ここでの環境地図への登録の具体例は、図７のステップＳ１０９における具体例と同様である。そして、ステップＳ４０２に戻る。

以上説明した実施形態２に係る地図作成モジュールでは、非駆動系位置推定部１２のＳＬＡＭ処理がロスト状態になっても、基準遅延時間が経過するまでの間にＳＬＡＭ処理がトラッキング状態に戻れば、その間に環境地図に記録された情報を失わずに済む。また、基準遅延時間が経過するまでの間にＳＬＡＭ処理がトラッキング状態に戻れない場合でも、環境地図に情報が記録されない時間を短くすることができる。そして、基準遅延時間の間及び基準保存時間の間は、オドメトリの情報に含まれる誤差は小さいため、環境地図の精度の低下も抑えることができる。

なお、実施形態２では、図１２のステップＳ４０６で、地図制御部１６は、ＳＬＡＭ処理のトラッキング状態が「トラッキング中」から「ロスト中」に変化してから基準遅延時間が経過したか否かを判定しているが、これに限られない。例えば、ステップＳ４０６において、地図制御部１６は、ＳＬＡＭ処理のトラッキング状態が「トラッキング中」から「ロスト中」に変化してから自律移動装置１０１が所定距離（例えば３ｍ）移動したか否かを判定し、移動していたらステップＳ４０７に進み、移動していなければステップＳ４１９に進む処理を行っても良い。自律移動装置１０１のオドメトリの精度の低下は移動距離にも依存するので、あまり移動していない間はオドメトリによる自己位置の推定をし続けても精度の低下は少ないと考えられるからである。これにより、環境地図に情報が記録されない時間をさらに短くすることができる。

また、実施形態２では、図１２のステップＳ４１０で、地図制御部１６は、物体位置記憶部２６に保存されているデータのうち、基準保存時間より前のデータを物体位置記憶部２６から削除しているが、これに限られない。例えば、ステップＳ４１０において、地図制御部１６は、物体位置記憶部２６に保存されている過去のデータのうち、現在の自己位置から基準移動距離（例えば３ｍ）よりも（移動経路に沿った道のりで）離れているデータを物体位置記憶部２６から削除しても良い。これにより、自律移動装置１０１はトラッキング回復時に、その時点（トラッキングタイミング：Ｔ２）の自己位置から基準移動距離以内の移動範囲における物体位置記憶部のデータを環境地図に登録することができる。上述したように、移動距離が短い間はオドメトリによる自己位置の推定精度の低下は少ないと考えられるからである。これにより、環境地図に情報が記録されない時間をさらに短くすることができる。

（変形例１）
上記の各実施形態では、非駆動系位置推定部１２は、ＳＬＡＭ処理により自律移動装置１００，１０１の自己位置及び姿勢を推定しているが、これに限られない。例えば、自律移動装置がＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波を取得する電波受信部を備え、非駆動系位置推定部１２は、ＧＰＳ衛星からの電波に含まれる情報に基づいて自己位置及び姿勢を推定しても良い。また、ＧＰＳ衛星からの電波に限らず、携帯電話の基地局からの電波、無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）の基地局からの電波、ＲＦＩＤ（Ｒａｄｉｏ Ｆｒｅｑｕｅｎｃｙ Ｉｄｅｎｔｉｆｉｅｒ）等を用いて自己位置を推定しても良い。また、この場合、自律移動装置が方位センサを備えることにより、自律移動装置の向き（姿勢）の情報を得ることができる。

非駆動系位置推定部１２が電波を用いて自己位置を推定する場合、制御部１０は、電波の受信状況に基づいて、非駆動系位置推定部１２が自己位置を推定できるか否かを判定する。つまり、非駆動系位置推定部１２が自己位置を推定できるだけの電波を受信できるならトラッキング状態であり、自己位置を推定できるだけの電波を受信できないならロスト状態である。

以上の変形例１に係る自律移動装置は、ＳＬＡＭ処理により自己位置の推定を行えない場合でも、電波受信部からの情報に基づいて、精度の高い環境地図を作成することができる。また、電波受信部から情報が得られなくなった場合に環境地図を保存し、電波受信部から情報が得られるようになった場合に該保存した環境地図を読み込むことによって、電波受信部で情報が得られなくなっている間に環境地図に登録された精度の低い情報を削除することができ、環境地図の精度を向上させることができる。

（変形例２）
上記の各実施形態では、センサ部３０は、測域センサ３１を備えているが、これに限られない。ここでは、センサ部３０が物体に衝突したことを検出する衝突センサを備える変形例２について説明する。

変形例２に係る自律移動装置１０２は、図１３に示すように、センサ部３０に物体への衝突を検出する衝突センサ３２が追加され、記憶部２０に衝突データ記憶部２７が追加された構成である。衝突データ記憶部２７には、衝突を検出した衝突センサ３２の自律移動装置１０２における位置に基づいて得られる、衝突した物体の位置（自律移動装置１０２の中心位置及び正面を基準とする相対位置及び方向）を示すデータ（衝突データ）が記憶される。

衝突センサ３２の個数は任意であるが、図１４に示す例では、自律移動装置１０２は、衝突センサ３２として、自律移動装置１０２の真正面の衝突センサ３２ｂと、正面より少し右寄りの衝突センサ３２ａと、正面より少し左寄りの衝突センサ３２ｃを備える。物体がこれらの衝突センサ３２に衝突すると、衝突された衝突センサ３２は衝突を検出したことを制御部１０に通知する。すると制御部１０は、その時点における自律移動装置１０２の自己位置及び向きと、衝突検出を通知した衝突センサ３２の自律移動装置１０２における位置と、に基づき、衝突した物体の位置を示す衝突データを衝突データ記憶部２７に登録する。

自律移動装置１０２の制御部１０が実行するソフトウェアモジュールの全体構成は、図１５に示すように、図４を参照して説明した自律移動装置１００のソフトウェアモジュールの全体構成に、衝突センサ３２が追加された構成になっている。自律移動装置１０２の地図作成モジュール５３は、衝突センサ３２から得られる衝突データも用いて環境地図を作成する。この点以外は、自律移動装置１００と同じである。

自律移動装置１０２の地図作成モジュール５３の処理内容について、図１６を参照して説明する。この処理は、図７を参照して説明した自律移動装置１００の地図作成モジュール５３の処理内容と一部を除き共通なので、異なる点を中心に説明する。

ステップＳ１０１からステップＳ１０９までの処理は、図７の処理と同じである。ステップＳ１０９の次に、物体位置取得部１３は、衝突データ記憶部２７から衝突データを取得する（ステップＳ１１１）。そして、地図作成部１４は、ステップＳ１０３で取得した自己位置及び姿勢の情報を用いて、ステップＳ１１１で取得した衝突データを環境地図における座標に変換し、環境地図に登録する（ステップＳ１１２）。そして、ステップＳ１０２に戻る。

ステップＳ１１２における、衝突データを環境地図に登録する具体例を以下に説明する。例えば、衝突センサ３２で検出された物体の環境地図における座標が［ｍ，ｎ］であったとすると、地図作成部１４は、まず、ＭＡ［ｍ，ｎ］＝ＭＡ［ｍ，ｎ］＋Δｃを実行する。ここで、Δｃは、衝突センサ３２が各格子点における物体の存在を確認した時に該格子点の値に加算する更新量であり、Δｃの値は例えば１０である。ただし、Δｃを加算するとＭＡ［ｍ，ｎ］が最大値Ｌｍａｘを超える場合には、ＭＡ［ｍ，ｎ］＝Ｌｍａｘとする。

次に、地図作成部１４は、自律移動装置１０２の現在の自己位置に対応する格子点の値から更新量Δｃを減算する。具体的には、自己位置の座標が［ｐ，ｑ］であったとすると、ＭＡ［ｐ，ｑ］＝ＭＡ［ｐ，ｑ］−Δｃを実行する。ただし、Δｃを減算するとＭＡ［ｐ，ｑ］が最小値Ｌｍｉｎを下回る場合には、ＭＡ［ｐ，ｑ］＝Ｌｍｉｎとする。

なお、上記は地図更新方法の一例である。測域センサ３１から得られるデータは誤差が含まれることが多いが、衝突センサ３２から得られるデータには誤差がほとんど含まれないと考えられる。したがって、衝突センサ３２で検出された物体の環境地図における座標が［ｍ，ｎ］であった場合、地図作成部１４はＭＡ［ｍ，ｎ］＝Ｌｍａｘを実行し、その時の自己位置の座標が［ｐ，ｑ］であった場合、地図作成部１４はＭＡ［ｐ，ｑ］＝Ｌｍｉｎを実行しても良い。また、Δｃの値を格子点の各座標毎に値を変更できるようにしても良い。

また、地図作成部１４は、測域センサ３１から得られる環境地図と衝突センサ３２から得られる環境地図とを別々に用意し、これらを統合した環境地図を作成しても良い。例えば、地図作成部１４は、測域センサ３１から得られる環境地図を２次元配列ＭＡ［ｉ，ｊ］に作成し、衝突センサ３２から得られる環境地図を２次元配列ＭＢ［ｉ，ｊ］に作成し、これらを統合した環境地図として２次元配列ＭＩ［ｉ，ｊ］を以下のように作成しても良い。
（１）もし、ＭＡ［ｉ，ｊ］＜０かつＭＢ［ｉ，ｊ］＜０ならＭＩ［ｉ，ｊ］＝ｍｉｎ（ＭＡ［ｉ，ｊ］，ＭＢ［ｉ，ｊ］）
（２）もし、ＭＡ［ｉ，ｊ］≧０又はＭＢ［ｉ，ｊ］≧０ならＭＩ［ｉ，ｊ］＝ｍａｘ（ＭＡ［ｉ，ｊ］，ＭＢ［ｉ，ｊ］）

地図作成部１４が、このようにセンサ部３０に含まれる複数のセンサ毎に複数の環境地図を用意する場合、地図保管部２４にも複数の環境地図を保存できる領域を用意し、地図制御部１６は、複数の環境地図をそれぞれ地図保管部２４に保存する。例えば、地図作成部１４が、測域センサ３１から得られる環境地図を２次元配列ＭＡ［ｉ，ｊ］に作成し、衝突センサ３２から得られる環境地図を２次元配列ＭＢ［ｉ，ｊ］に作成する場合、地図保管部２４には、測域センサ３１から得られる環境地図を保存するための２次元配列ＭＡＳ［ｉ，ｊ］と、衝突センサ３２から得られる環境地図を保存するための２次元配列ＭＢＳ［ｉ，ｊ］と、が記憶される。

以上の地図作成モジュールの処理によって、変形例２に係る自律移動装置１０２は、測域センサ３１で検出できない物体が存在しても、その物体に衝突したら、衝突センサ３２によってその物体を検出でき、環境地図に登録することができる。そして、その登録の際にＳＬＡＭ処理がトラッキング状態であれば、その物体の情報は精度の高い情報として扱われるので環境地図から削除されず、登録の際にＳＬＡＭ処理がロスト状態であればその物体の情報は精度の低い情報として扱われトラッキング状態に復帰した際に削除される。このようにして、環境地図の精度を向上させることができる。

（変形例３）
センサ部３０が備えるセンサは、測域センサ３１及び衝突センサ３２に限られない。センサ部３０が、周囲に床面が存在するか否かを検出することによって、床面が存在しない場所であるクリフ（穴、崖等）を検出するクリフセンサを備え、地図作成部１４はクリフセンサが検出したクリフの情報も環境地図に記録するようにしても良い。また、センサ部３０は、例えば悪路を検出する床面センサ、床の濡れている部分や水たまりを検出する水分センサ等を備えても良い。センサ部３０が備えるセンサの種類を増やすことによって、自律移動装置は様々な環境により柔軟に対応した環境地図を作成できるようになる。

なお、上記実施形態１及び変形例２では、測距データ記憶部２５には、測域センサ３１が検出した物体までの距離を示す測距データ（自律移動装置からの相対位置データ）が、該物体が検出された角度の情報とともに記憶されているが、これに限られない。例えば、測距データ記憶部２５には、測域センサ３１で周囲を観測（スキャン）した時の自律移動装置の自己位置及び向きと、測域センサ３１が取得した測距データ及び角度と、に基づいて得られる、測域センサ３１で検出された物体（障害物）の位置（絶対位置）を示すデータを記憶しても良い。このようにすると、地図作成モジュール（図７、図１６）において、測距データを環境地図に登録する際に、測距データを地図の座標に変換する処理が不要となる。

上記変形例２における衝突データ記憶部２７についても同様であり、衝突センサ３２が衝突を検出した時の自律移動装置１０２の自己位置及び向きと、該衝突を検出した衝突センサ３２の自律移動装置１０２における位置と、に基づいて得られる、衝突した物体（障害物）の位置（絶対位置）を示すデータを記憶しても良い。このようにすると、地図作成モジュール（図１６）において、衝突データを環境地図に登録する際に、衝突データを地図の座標に変換する処理が不要となる。

また、上述する各実施形態において、測域センサ３１を備えない代わりに、デプスカメラ又はソナーを備え、これらによって観測された障害物を環境地図に記録するようにしても良い。

また、上述する各実施形態において、測域センサ３１を備えず、撮像部４１が撮影した画像を用いたｖｉｓｕａｌ ＳＬＡＭによって障害物を検出するようにし、ｖｉｓｕａｌ ＳＬＡＭで検出された障害物を環境地図に記録するようにしても良い。

なお、自律移動装置１００，１０１，１０２の各機能は、通常のＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置１００，１０１，１０２が行う自律移動制御処理のプログラムが、記憶部２０のＲＯＭに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）及びＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ Ｄｉｓｃ）等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。

以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。

（付記１）
駆動部からの情報に基づいて自己位置を推定する駆動系位置推定部と、
前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定部と、
周囲の物体の位置を取得する物体位置取得部と、
前記駆動系位置推定部又は前記非駆動系位置推定部が推定した自己位置と、前記物体位置取得部が取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成部と、
前記非駆動系位置推定部が前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御部と、
を備える自律移動装置。

（付記２）
前記地図制御部は、前記基準タイミングに前記地図記憶部に記憶されている環境地図を地図保管部に保存し、前記トラッキングタイミングになった場合に前記地図保管部に保存した環境地図を前記地図記憶部に戻す、
付記１に記載の自律移動装置。

（付記３）
前記地図制御部は、前記基準タイミングに前記地図記憶部に記憶されている環境地図の更新を停止し、前記トラッキングタイミングになった場合に前記環境地図の更新を再開する、
付記１に記載の自律移動装置。

（付記４）
周囲の画像を取得する撮像部をさらに備え、
前記非駆動系位置推定部は、前記撮像部が取得した画像の情報に基づいて自己位置を推定する、
付記１から３のいずれか１つに記載の自律移動装置。

（付記５）
前記物体位置取得部は、前記物体までの距離である測距データを取得する測域センサからの情報を用いて前記物体の位置を取得する、
付記１から４のいずれか１つに記載の自律移動装置。

（付記６）
前記物体位置取得部は、前記物体に衝突したことを検出する衝突センサからの情報を用いて前記物体の位置を取得する、
付記１から５のいずれか１つに記載の自律移動装置。

（付記７）
前記基準タイミングは、前記非駆動系位置推定部で前記自己位置を推定できなくなったロストタイミングから基準遅延時間後または所定距離移動後である、
付記１から６のいずれか１つに記載の自律移動装置。

（付記８）
前記物体位置取得部が取得した物体の位置及び前記駆動系位置推定部が推定した自己位置の基準保存時間前まで又は基準移動距離以内の履歴を保存する物体位置記憶部をさらに備え、
前記地図制御部は、前記トラッキングタイミングになった場合に、前記非駆動系位置推定部が推定した自己位置並びに前記物体位置記憶部に保存された物体の位置及び自己位置に基づき、前記基準保存時間前まで又は前記基準移動距離以内の前記物体の位置を前記基準タイミングの環境地図に記録して使用する、
付記７に記載の自律移動装置。

（付記９）
前記地図制御部は、前記トラッキングタイミングになった場合に、前記非駆動系位置推定部が推定した自己位置と前記駆動系位置推定部が推定した自己位置との差分を求め、前記差分を前記物体位置記憶部に保存された物体の位置及び自己位置に加算することにより、前記基準保存時間前まで又は前記基準移動距離以内の前記物体の位置を推定して、前記環境地図に記録する、
付記８に記載の自律移動装置。

（付記１０）
前記地図作成部は、前記物体位置取得部が取得した前記物体の位置に対応する前記環境地図上の位置における前記物体の存在確率を算出し、前記存在確率を前記環境地図に記録する、
付記１から９のいずれか１つに記載の自律移動装置。

（付記１１）
前記物体位置取得部は、所定の方向毎に物体の存否を確認し、物体の存在を確認した場合には前記物体までの距離である測距データを取得し、
前記地図作成部は、前記物体位置取得部が前記物体の存否を確認した回数に対する前記測距データを取得した回数の割合を前記存在確率として前記環境地図に記録する、
付記１０に記載の自律移動装置。

（付記１２）
前記地図作成部は、前記存在確率を対数オッズにより前記環境地図に記録する、
付記１０又は１１に記載の自律移動装置。

（付記１３）
前記地図作成部は、前記物体位置取得部が前記物体の位置を取得するために用いる情報の種類毎に複数の環境地図を作成し、
前記地図制御部は、前記複数の環境地図をそれぞれ別々に固定する、
付記１から１２のいずれか１つに記載の自律移動装置。

（付記１４）
環境地図を作成しながら自己位置を推定する自律移動装置において、
自己位置を推定できなくなった場合に前記環境地図を地図保管部に保存または前記環境地図の更新を停止し、自己位置を推定できるようになった場合に前記保存した環境地図または更新を停止した環境地図を使用する、
自律移動装置。

（付記１５）
環境地図を作成するための撮影画像で自己位置を推定できなくなっているロスト期間中に、オドメトリの情報を使用して簡易地図を作成して使用し、前記撮影画像で自己位置を推定できるようになった場合に、前記簡易地図を前記自己位置を推定できなくなった場合における保存した環境地図または更新を停止した環境地図に置き換える、
付記１４に記載の自律移動装置。

（付記１６）
環境地図を作成するための撮影画像で自己位置を推定できなくなっているロスト期間中に、オドメトリの情報を保存しておき、前記撮影画像で自己位置を推定できるようになった場合に、前記保存しておいたオドメトリの情報から自己位置を基準として過去の自己位置を推定し、前記環境地図に反映させる、
付記１４に記載の自律移動装置。

（付記１７）
駆動部からの情報に基づいて自己位置を推定する駆動系位置推定ステップと、
前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定ステップと、
周囲の物体の位置を取得する物体位置取得ステップと、
前記駆動系位置推定ステップ又は前記非駆動系位置推定ステップで推定した自己位置と、前記物体位置取得ステップで取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成ステップと、
前記非駆動系位置推定ステップで前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御ステップと、
を含む自律移動方法。

（付記１８）
コンピュータに、
駆動部からの情報に基づいて自己位置を推定する駆動系位置推定ステップ、
前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定ステップ、
周囲の物体の位置を取得する物体位置取得ステップ、
前記駆動系位置推定ステップ又は前記非駆動系位置推定ステップで推定した自己位置と、前記物体位置取得ステップで取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成ステップ、及び、
前記非駆動系位置推定ステップで前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御ステップ、
を実行させるためのプログラム。

１０…制御部、１１…駆動系位置推定部、１２…非駆動系位置推定部、１３…物体位置取得部、１４…地図作成部、１５…移動制御部、１６…地図制御部、２０…記憶部、２１…画像記憶部、２２…特徴点記憶部、２３…地図記憶部、２４…地図保管部、２５…測距データ記憶部、２６…物体位置記憶部、２７…衝突データ記憶部、３０…センサ部、３１…測域センサ、３２，３２ａ，３２ｂ，３２ｃ…衝突センサ、４１…撮像部、４２，４２ａ，４２ｂ…駆動部、４３…通信部、５１…非駆動系位置推定モジュール、５２…駆動系位置推定モジュール、５３…地図作成モジュール、５４…移動制御モジュール、７０…障害物、１００，１０１，１０２…自律移動装置

Claims (18)

1. 駆動部からの情報に基づいて自己位置を推定する駆動系位置推定部と、
   前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定部と、
   周囲の物体の位置を取得する物体位置取得部と、
   前記駆動系位置推定部又は前記非駆動系位置推定部が推定した自己位置と、前記物体位置取得部が取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成部と、
   前記非駆動系位置推定部が前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御部と、
   を備える自律移動装置。
2. 前記地図制御部は、前記基準タイミングに前記地図記憶部に記憶されている環境地図を地図保管部に保存し、前記トラッキングタイミングになった場合に前記地図保管部に保存した環境地図を前記地図記憶部に戻す、
   請求項１に記載の自律移動装置。
3. 前記地図制御部は、前記基準タイミングに前記地図記憶部に記憶されている環境地図の更新を停止し、前記トラッキングタイミングになった場合に前記環境地図の更新を再開する、
   請求項１に記載の自律移動装置。
4. 周囲の画像を取得する撮像部をさらに備え、
   前記非駆動系位置推定部は、前記撮像部が取得した画像の情報に基づいて自己位置を推定する、
   請求項１から３のいずれか１項に記載の自律移動装置。
5. 前記物体位置取得部は、前記物体までの距離である測距データを取得する測域センサからの情報を用いて前記物体の位置を取得する、
   請求項１から４のいずれか１項に記載の自律移動装置。
6. 前記物体位置取得部は、前記物体に衝突したことを検出する衝突センサからの情報を用いて前記物体の位置を取得する、
   請求項１から５のいずれか１項に記載の自律移動装置。
7. 前記基準タイミングは、前記非駆動系位置推定部で前記自己位置を推定できなくなったロストタイミングから基準遅延時間後または所定距離移動後である、
   請求項１から６のいずれか１項に記載の自律移動装置。
8. 前記物体位置取得部が取得した物体の位置及び前記駆動系位置推定部が推定した自己位置の基準保存時間前まで又は基準移動距離以内の履歴を保存する物体位置記憶部をさらに備え、
   前記地図制御部は、前記トラッキングタイミングになった場合に、前記非駆動系位置推定部が推定した自己位置並びに前記物体位置記憶部に保存された物体の位置及び自己位置に基づき、前記基準保存時間前まで又は前記基準移動距離以内の前記物体の位置を前記基準タイミングの環境地図に記録して使用する、
   請求項７に記載の自律移動装置。
9. 前記地図制御部は、前記トラッキングタイミングになった場合に、前記非駆動系位置推定部が推定した自己位置と前記駆動系位置推定部が推定した自己位置との差分を求め、前記差分を前記物体位置記憶部に保存された物体の位置及び自己位置に加算することにより、前記基準保存時間前まで又は前記基準移動距離以内の前記物体の位置を推定して、前記環境地図に記録する、
   請求項８に記載の自律移動装置。
10. 前記地図作成部は、前記物体位置取得部が取得した前記物体の位置に対応する前記環境地図上の位置における前記物体の存在確率を算出し、前記存在確率を前記環境地図に記録する、
    請求項１から９のいずれか１項に記載の自律移動装置。
11. 前記物体位置取得部は、所定の方向毎に物体の存否を確認し、物体の存在を確認した場合には前記物体までの距離である測距データを取得し、
    前記地図作成部は、前記物体位置取得部が前記物体の存否を確認した回数に対する前記測距データを取得した回数の割合を前記存在確率として前記環境地図に記録する、
    請求項１０に記載の自律移動装置。
12. 前記地図作成部は、前記存在確率を対数オッズにより前記環境地図に記録する、
    請求項１０又は１１に記載の自律移動装置。
13. 前記地図作成部は、前記物体位置取得部が前記物体の位置を取得するために用いる情報の種類毎に複数の環境地図を作成し、
    前記地図制御部は、前記複数の環境地図をそれぞれ別々に固定する、
    請求項１から１２のいずれか１項に記載の自律移動装置。
14. 環境地図を作成しながら自己位置を推定する自律移動装置において、
    自己位置を推定できなくなった場合に前記環境地図を地図保管部に保存または前記環境地図の更新を停止し、自己位置を推定できるようになった場合に前記保存した環境地図または更新を停止した環境地図を使用する、
    自律移動装置。
15. 環境地図を作成するための撮影画像で自己位置を推定できなくなっているロスト期間中に、オドメトリの情報を使用して簡易地図を作成して使用し、前記撮影画像で自己位置を推定できるようになった場合に、前記簡易地図を前記自己位置を推定できなくなった場合における保存した環境地図または更新を停止した環境地図に置き換える、
    請求項１４に記載の自律移動装置。
16. 環境地図を作成するための撮影画像で自己位置を推定できなくなっているロスト期間中に、オドメトリの情報を保存しておき、前記撮影画像で自己位置を推定できるようになった場合に、前記保存しておいたオドメトリの情報から自己位置を基準として過去の自己位置を推定し、前記環境地図に反映させる、
    請求項１４に記載の自律移動装置。
17. 駆動部からの情報に基づいて自己位置を推定する駆動系位置推定ステップと、
    前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定ステップと、
    周囲の物体の位置を取得する物体位置取得ステップと、
    前記駆動系位置推定ステップ又は前記非駆動系位置推定ステップで推定した自己位置と、前記物体位置取得ステップで取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成ステップと、
    前記非駆動系位置推定ステップで前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御ステップと、
    を含む自律移動方法。
18. コンピュータに、
    駆動部からの情報に基づいて自己位置を推定する駆動系位置推定ステップ、
    前記駆動部以外からの情報に基づいて自己位置を推定する非駆動系位置推定ステップ、
    周囲の物体の位置を取得する物体位置取得ステップ、
    前記駆動系位置推定ステップ又は前記非駆動系位置推定ステップで推定した自己位置と、前記物体位置取得ステップで取得した位置と、に基づき、前記物体の位置を記録した環境地図を地図記憶部に作成する地図作成ステップ、及び、
    前記非駆動系位置推定ステップで前記自己位置を推定できなくなったロストタイミングによって定まる基準タイミングに前記地図記憶部に記憶されている環境地図を、前記自己位置を推定できるトラッキングタイミングになった場合に使用する、地図制御ステップ、
    を実行させるためのプログラム。

Images