特許 6187623 自律移動装置、自律移動方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B1)

(11)【特許番号】6187623

(24)【登録日】2017年8月10日

(45)【発行日】2017年8月30日

(54)【発明の名称】自律移動装置、自律移動方法及びプログラム

(51)【国際特許分類】

   G05D 1/02 20060101AFI20170821BHJP

【ＦＩ】

   G05D1/02 H

【請求項の数】17

【全頁数】24

(21)【出願番号】特願2016-49350(P2016-49350)

(22)【出願日】2016年3月14日

【審査請求日】2017年1月6日

【早期審査対象出願】

(73)【特許権者】

【識別番号】000001443

【氏名又は名称】カシオ計算機株式会社

(74)【代理人】

【識別番号】100095407

【弁理士】

【氏名又は名称】木村 満

(72)【発明者】

【氏名】前野 泰士

【審査官】 加藤 啓

(56)【参考文献】

【文献】 特開２０１０−２８８１１２（ＪＰ，Ａ）

【文献】 特開２００８−０７１３５２（ＪＰ，Ａ）

【文献】 特開２００８−０５９２１８（ＪＰ，Ａ）

【文献】 特開２００４−００５５９３（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０５Ｄ １／０２

(57)【特許請求の範囲】

【請求項1】

撮像部が撮影した複数の画像の情報に基づいて自律的な移動を制御する移動制御部と、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得部と、
前記特徴点取得部が取得した特徴点の位置を記憶する地図記憶部と、
前記特徴点取得部が取得した特徴点と、前記地図記憶部に位置が記憶されている特徴点と、の対応数である特徴点対応数を取得する特徴点対応数取得部と、
前記地図記憶部が記憶している特徴点の位置に基づき自機位置を推定する位置推定部と、を備え、
前記位置推定部は前記特徴点対応数取得部が取得した特徴点対応数に基づき自機位置の推定状態が追跡成功状態にあるか追跡失敗状態にあるかを判定し、
前記特徴点対応数取得部は、前記判定が、追跡成功状態であれば前フレームを参照して特徴点対応数を取得し、追跡失敗状態であればキーフレームを参照して特徴点対応数を取得し、
前記移動制御部は、
前記特徴点対応数取得部が取得した特徴点対応数に応じて移動を制御する、
自律移動装置。

【請求項2】

前記特徴点対応数取得部は、前記前フレーム又は前記キーフレームを参照して取得した特徴点対応数が基準対応特徴点数以上であれば、さらに近傍のキーフレームを参照して特徴点対応数を取得し、
前記位置推定部は、前記特徴点対応数取得部が近傍のキーフレームを参照して取得した特徴点対応数が第２の基準対応特徴点数以上の場合に、自機位置推定状態を追跡成功状態と判定し、当該特徴点対応数を前記地図記憶部に特徴点の位置とともに記憶する、
請求項１に記載の自律移動装置。

【請求項3】

前記移動制御部は、前記特徴点対応数取得部が取得した特徴点対応数が、前記第２の基準対応特徴点数よりも大きい第１移動変更閾値以下の場合に、移動方向を変更する、
請求項２に記載の自律移動装置。

【請求項4】

前記移動制御部は、前記特徴点対応数取得部が取得した特徴点対応数が、前記第２の基準対応特徴点数よりも大きい第１移動変更閾値以下の場合に、移動速度を低下させる、
請求項２又は３に記載の自律移動装置。

【請求項5】

前記移動制御部は、前記特徴点対応数取得部が取得した特徴点対応数が、前記第１移動変更閾値よりも小さく前記第２の基準対応特徴点数よりも大きい第２移動変更閾値以下の場合に、所定の時間、前記移動速度を０にする、
請求項４に記載の自律移動装置。

【請求項6】

前記自律移動装置は、さらに、過去の移動履歴を記憶する移動履歴記憶部を備え、
前記移動制御部は、前記移動速度を０にしてから前記所定の時間が経過しても、前記特徴点対応数取得部が取得した特徴点対応数が前記第２移動変更閾値以下の場合には、前記移動履歴記憶部に記憶されている移動履歴に基づいて所定の回数だけ過去の位置に戻るように移動方向を変更する、
請求項５に記載の自律移動装置。

【請求項7】

前記自律移動装置は、さらに、移動の目的地を取得する目的地取得部を備え、
前記移動制御部は、前記所定の回数だけ過去の位置に戻るように移動方向を変更した後に、前記目的地取得部が取得した目的地に向かって移動するように移動方向を変更する、
請求項６に記載の自律移動装置。

【請求項8】

前記自律移動装置は、さらに、前記目的地取得部が取得した目的地を変更する目的地変更部を備え、
前記移動制御部が、前記過去の位置に戻るように移動方向を変更した後に、前記目的地取得部が取得した目的地に向かって移動するように移動方向を変更した後、前記過去の位置に戻る前の位置より先に進む前に、前記特徴点対応数取得部が取得した特徴点対応数が前記第２移動変更閾値以下になった場合は、前記目的地変更部が前記目的地を変更する、
請求項７に記載の自律移動装置。

【請求項9】

前記自律移動装置は、さらに、複数の目的地候補を選定する目的地候補選定部と、
前記目的地候補選定部が選定した目的地候補の方向の画像から特徴点を取得する目的地候補特徴点取得部と、を備え、
前記目的地変更部は、前記目的地を、前記目的地候補選定部が選定した複数の目的地候補の中で前記目的地候補特徴点取得部が取得した特徴点の数が最大となる目的地候補に変更する、
請求項８に記載の自律移動装置。

【請求項10】

前記撮像部はさらに周囲の明るさを検出する手段を備え、前記移動制御部は、前記周囲の明るさが所定の明るさよりも暗いときは移動速度を低下させるか、または停止させる、
請求項１から９のいずれか１項に記載の自律移動装置。

【請求項11】

撮像部が撮影した複数の画像の情報に基づいて自律的に移動するための自律移動方法であって、
前記撮像部が撮影した複数の画像から取得した画像特徴点と、実空間内での位置の推定を行った推定特徴点との対応数に応じて自律的に移動し、
前記画像特徴点と前記推定特徴点との対応数が所定の第２の基準対応特徴点数よりも大きい第１移動変更閾値以下になると移動速度を低下させる、
自律移動方法。

【請求項12】

前記画像特徴点と前記推定特徴点との対応数が前記第１移動変更閾値よりも小さく前記第２の基準対応特徴点数よりも大きい第２移動変更閾値以下になると、所定の時間停止する、
請求項１１に記載の自律移動方法。

【請求項13】

所定の時間停止していても前記画像特徴点と前記推定特徴点との対応数が前記第２移動変更閾値以下の場合には、過去の位置に戻る、
請求項１２に記載の自律移動方法。

【請求項14】

撮像部が撮影した複数の画像の情報に基づいて自律的な移動の方向及び速度を決定する移動方向速度決定ステップと、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得ステップと、
前記特徴点取得ステップで取得した特徴点の位置を記憶する地図記憶部に位置が記憶されている特徴点と、前記特徴点取得ステップで取得した特徴点と、の対応数を取得する特徴点対応数取得ステップと、
前記地図記憶部に記憶されている特徴点の位置に基づき自機位置を推定する位置推定ステップと、
前記特徴点対応数取得ステップで取得した特徴点対応数に応じて移動を制御する移動制御ステップと、を含み、
前記位置推定ステップではさらに前記特徴点対応数取得ステップで取得した特徴点対応数に基づき自機位置の推定状態が追跡成功状態にあるか追跡失敗状態にあるかを判定し、
前記特徴点対応数取得ステップは、前記判定が追跡成功状態であれば前フレームを参照して特徴点対応数を取得する第１取得ステップと、前記判定が追跡失敗状態であればキーフレームを参照して特徴点対応数を取得する第２取得ステップと、
を含む自律移動方法。

【請求項15】

前記特徴点対応数取得ステップは、前記第１取得ステップ又は前記第２取得ステップで取得した特徴点対応数が基準対応特徴点数以上であれば、さらに近傍のキーフレームを参照して特徴点対応数を取得する第３取得ステップを含み、
前記位置推定ステップは、前記特徴点対応数取得ステップの前記第３取得ステップで取得した特徴点対応数が第２の基準対応特徴点数以上の場合に、自機位置の推定状態を追跡成功状態と判定し、当該特徴点対応数を前記地図記憶部に特徴点の位置とともに記憶するステップを含む、
請求項１４に記載の自律移動方法。

【請求項16】

前記第２取得ステップでは、前記キーフレームの中から、前記地図記憶部に記憶されている特徴点との対応数が最大となるキーフレームを参照して特徴点対応数を取得する、
請求項１４又は１５に記載の自律移動方法。

【請求項17】

コンピュータに、
撮像部が撮影した複数の画像の情報に基づいて自律的な移動の方向及び速度を決定する移動方向速度決定ステップ、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得ステップ、
前記特徴点取得ステップで取得した特徴点の位置を記憶する地図記憶部に位置が記憶されている特徴点と、前記特徴点取得ステップで取得した特徴点と、の対応数を取得する特徴点対応数取得ステップ、
前記地図記憶部に記憶されている特徴点の位置に基づき自機位置を推定する位置推定ステップ、
前記特徴点対応数取得ステップで取得した特徴点対応数に応じて移動を制御する移動制御ステップ、
を実行させるためのプログラムであって、
前記位置推定ステップではさらに前記特徴点対応数取得ステップで取得した特徴点対応数に基づき自機位置の推定状態が追跡成功状態にあるか追跡失敗状態にあるかを判定し、
前記特徴点対応数取得ステップは、前記判定が追跡成功状態であれば前フレームを参照して特徴点対応数を取得する第１取得ステップと、前記判定が追跡失敗状態であればキーフレームを参照して特徴点対応数を取得する第２取得ステップと、
を含む、
プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。単眼カメラを用いたＳＬＡＭ技術の基本的な原理は、非特許文献１及び非特許文献２に記載されており、カメラの撮影する動画像の複数フレームから、同一の特徴点を追跡することで、自機の３次元位置（カメラ位置）と特徴点の３次元位置（これが集まって地図の情報を構成する）とを交互に推定する処理を行っている。

【0004】

この方法は、自機位置と特徴点の３次元位置とを交互に推定していくため、累積誤差が発生し、ループ状の移動等を行って元の位置に戻ってきた場合でも、元の位置に戻ってきたことを認識できない場合がある。このような累積誤差を修正する方法として、ループクロージング処理（ループ解決処理）という技術があり、非特許文献３に開示されている。これは、自機がループを検出すると（撮影したフレームが、過去撮影したフレームと類似していることが検出されると）、過去のその位置を正しい位置とみなして、そこから現在位置に至るまでの経路中の位置を修正することで誤差を減らすループクロージング処理が行われる。

【先行技術文献】

【非特許文献】

【0005】

【非特許文献1】Ａｎｄｒｅｗ Ｊ．Ｄａｖｉｓｏｎ， “Ｒｅａｌ−Ｔｉｍｅ Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ ｗｉｔｈ ａ Ｓｉｎｇｌｅ Ｃａｍｅｒａ”， Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ９ｔｈ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ Ｖｏｌｕｍｅ ２， ２００３， ｐｐ．１４０３−１４１０

【非特許文献2】Ｒｉｃｈａｒｄ Ｈａｒｔｌｅｙ， Ａｎｄｒｅｗ Ｚｉｓｓｅｒｍａｎ， “Ｍｕｌｔｉｐｌｅ Ｖｉｅｗ Ｇｅｏｍｅｔｒｙ ｉｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ”， Ｓｅｃｏｎｄ Ｅｄｉｔｉｏｎ， Ｃａｍｂｒｉｄｇｅ． Ｕｎｉｖｅｒｓｉｔｙ Ｐｒｅｓｓ， Ｍａｒｃｈ ２００４， ｃｈａｐｔｅｒ ９

【非特許文献3】奈良先端科学技術大学院大学 情報科学研究科准教授 佐藤智和、“ＳＳＩＩ２０１５チュートリアル 特徴点追跡による動画像からの逐次三次元復元とその応用 −座標系の基礎から応用事例・最新研究動向まで−”、［online］、２０１５年６月１０日、画像センシングシンポジウム チュートリアル講演会、［２０１６年２月２２日検索］、インターネット＜URL：http://yokoya.naist.jp/paper/datas/1407/SSII%E3%83%81%E3%83%A5%E3%83%BC%E3%83%88%E3%83%AA%E3%82%A2%E3%83%AB%E4%BD%90%E8%97%A4.pdf＞

【発明の概要】

【発明が解決しようとする課題】

【0006】

しかし、従来技術では、ループを検出しないとループクロージング処理を行うことができないため、誤差を減らせる機会が限られていた。

【0007】

本発明は、上記問題を解決するためになされたものであり、ループを検出しなくても誤差を減らすことができる自律移動装置等を提供することを目的する。

【課題を解決するための手段】

【0008】

本発明の一態様は、
撮像部が撮影した複数の画像の情報に基づいて自律的な移動を制御する移動制御部と、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得部と、
前記特徴点取得部が取得した特徴点の位置を記憶する地図記憶部と、
前記特徴点取得部が取得した特徴点と、前記地図記憶部に位置が記憶されている特徴点と、の対応数である特徴点対応数を取得する特徴点対応数取得部と、
前記地図記憶部が記憶している特徴点の位置に基づき自機位置を推定する位置推定部と、を備え、
前記位置推定部は前記特徴点対応数取得部が取得した特徴点対応数に基づき自機位置の推定状態が追跡成功状態にあるか追跡失敗状態にあるかを判定し、
前記特徴点対応数取得部は、前記判定が、追跡成功状態であれば前フレームを参照して特徴点対応数を取得し、追跡失敗状態であればキーフレームを参照して特徴点対応数を取得し、
前記移動制御部は、
前記特徴点対応数取得部が取得した特徴点対応数に応じて移動を制御する、
自律移動装置である。

【発明の効果】

【0009】

本発明によれば、ループを検出しなくても誤差を減らすことができる。

【図面の簡単な説明】

【0010】

【図1】実施形態に係る自律移動装置の外観を示す図である。

【図2】実施形態に係る自律移動装置の構成を示す図である。

【図3】実施形態に係る自律移動制御処理全体のフローチャートを示す図である。

【図4】実施形態に係る自律移動制御処理の中の自機位置推定スレッドの処理のフローチャートを示す図である。

【図5】実施形態に係る自律移動制御処理の中の地図作成スレッドの処理のフローチャートを示す図である。

【図6】実施形態に係る自律移動制御処理の中のループクロージングスレッドの処理のフローチャートを示す図である。

【図7】実施形態に係る自律移動制御処理の中の移動制御のフローチャートを示す図である。

【図8】実施形態に係る自律移動装置の移動の具体例を示す図である。

【発明を実施するための形態】

【0011】

以下、図１を参照しながら、本発明の実施形態に係る自律移動装置について説明する。自律移動装置１００は、用途に応じて自律的に移動する。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用などである。

【0012】

自律移動装置１００は、外観上、撮像部４１、駆動部４２を備える。

【0013】

撮像部４１は、単眼の撮像装置（カメラ）を備える。撮像部４１は、例えば、３０ｆｐｓで画像（フレーム）を取得する。自律移動装置１００は、撮像部４１が逐次取得した画像に基づいて、自機位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。

【0014】

駆動部４２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲ_Ｒ、左車輪の回転数をＲ_Ｌとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（Ｒ_Ｌ−Ｒ_Ｒ）／（２×Ｉ）となる。この並進移動量や回転量を逐次足し合わせていくことで、駆動部４２は、いわゆるオドメトリとして機能し、自機位置（移動開始時の位置及び向きを基準とした位置及び向き）を計測することができる。

【0015】

なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自機の位置や向きの計測が可能である。

【0016】

図２に示すように、自律移動装置１００は、撮像部４１、駆動部４２に加えて、制御部１０、記憶部２０、センサ部３０、入力部４３、通信部４４、電源４５、を備える。

【0017】

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

【0018】

記憶部２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ)、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等で構成され、画像記憶部２１、地図記憶部２２、特徴点対応数記憶部２３、移動履歴記憶部２４を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム（例えば、後述するＳＬＡＭ法の演算や自律移動制御処理に係るプログラム）や、プログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。

【0019】

画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくてもよく、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。重要な画像（後述するキーフレーム）については、画像の情報と共に、その画像を撮影した時の自機位置（自機の位置及び向き）の情報が記憶される。

【0020】

地図記憶部２２には、後述するＳＬＡＭ法や障害物センサ３１からの情報に基づいて地図作成部１２が作成した地図（特徴点や障害物の３次元位置の情報）が記憶される。

【0021】

特徴点対応数記憶部２３には、後述する自機位置推定スレッドにおいて、撮像部４１が撮影した画像の特徴点と、地図記憶部２２に記憶されているＭａｐ点との対応数が記憶される。

【0022】

移動履歴記憶部２４には、移動方向及び移動距離の履歴が記憶される。移動方向及び移動距離の代わりに、自機の３次元位置及び向きの履歴を記憶しても良い。移動履歴記憶部２４は、後入れ先出しのスタックの記憶部になっている。

【0023】

センサ部３０として、障害物センサ３１を備える。障害物センサ３１は、走行中の障害物を検知するセンサであって、例えば、赤外線センサ、超音波センサである。なお、独立した障害物センサ３１を搭載せずに、撮像部４１を用いて障害物を検知するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）、動作を検出する角速度センサや加速度センサ（いずれも図示せず）等を備えても良い。

【0024】

入力部４３として、自律移動装置１００を操作するための操作ボタンを備える。操作ボタンは、例えば、電源ボタン、モード切替ボタン（掃除モード、ペットモード等を切り替える）、初期化ボタン（地図の作成をやり直しさせる）等を含む。入力部４３として、音の入力を行うマイク（図示せず）と、自律移動装置１００への操作指示の音声を認識する音声認識部を備えても良い。

【0025】

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

【0026】

電源４５は、自律移動装置１００を動作させる電源であり、一般的には内蔵された充電池であるが、太陽電池であっても良いし、床面から無線で電力供給されるシステムであっても良い。電源４５が充電池の場合は、充電ステーション（ホームベース）に自律移動装置１００がドッキングすることで充電される。

【0027】

次に、制御部１０の機能について説明する。制御部１０は、特徴点取得部１１、地図作成部１２、位置推定部１３、特徴点対応数取得部１４、移動方向速度変更部１５、移動方向速度決定部１６を含み、後述するＳＬＡＭ法の演算や自律移動装置１００の移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して進めることができる。

【0028】

特徴点取得部１１は、撮像部４１が撮影した画像中の２Ｄ特徴点を取得する。２Ｄ特徴点とは画像中のエッジ部分など、画像内の特徴的な部分であり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｕｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。

【0029】

地図作成部１２は、画像記憶部２１に記憶されている画像の情報並びに該画像撮影時の自機の位置及び向きの情報に基づいてＳＬＡＭ法を用いて推定された特徴点（Ｍａｐ点）の３次元位置や、障害物センサ３１で障害物を検知したときの自機の位置及び向きの情報に基づいて得られた該障害物の３次元位置等を、地図の情報として地図記憶部２２に記憶する。

【0030】

位置推定部１３は、後述するＳＬＡＭ法に基づいて、ビジュアルオドメトリとして、自機の位置及び向きを推定する。

【0031】

特徴点対応数取得部１４は、特徴点取得部１１が取得した特徴点と地図記憶部２２に記憶されているＭａｐ点との対応数を取得する。

【0032】

移動方向速度変更部１５は、特徴点対応数取得部１４によって取得された特徴点対応数に基づいて、自機の移動速度を低下させたり、停止させたり、移動方向を変更させたりする。このようにすることによって、特徴点対応数を増やす。

【0033】

移動方向速度決定部１６は、自機の移動速度や移動方向を決定する。ここで決定された移動速度や移動方向に基づいて移動するように、制御部１０は駆動部４２を制御する。なお、移動方向速度変更部１５及び移動方向速度決定部１６は移動制御部に相当する。

【0034】

自律移動装置１００の自律移動制御処理のメインフローを図３を参照して説明する。制御部１０は、まず、移動状態変数に初期値である「通常」をセットし、前回履歴数に初期値である０をセットする（ステップＳ１０１）。移動状態変数と前回履歴数はいずれもステップＳ１０７の移動制御で使われる変数であり、詳細は移動制御の説明のところで述べる。次に、制御部１０は、自機位置推定スレッドを起動する（ステップＳ１０２）。そして、地図作成スレッドを起動する（ステップＳ１０３）。次に、ループクロージングスレッドを起動する（ステップＳ１０４）。自機位置推定スレッドや地図作成スレッドが動作することで、ＳＬＡＭ法に基づいて、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）の生成が開始される。その後、制御部１０は、動作終了かどうかを判定し（ステップＳ１０５）、動作終了なら（ステップＳ１０５；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０５；Ｎｏ）、障害物センサ３１とビジュアルオドメトリを利用して地図の更新を行う（ステップＳ１０６）。

【0035】

次に制御部１０は、自律移動するために駆動部４２に動作指示を出す移動制御を行い（ステップＳ１０７）、ステップＳ１０５に戻る。ここでの自律移動は、地図を作成するために部屋の中をくまなく動き回るような移動の場合もあるし、目的地への移動指示を受けた場合等、指示された目的地への最短経路に沿った移動の場合もある。また、明確な目的地は指示されずにタスク（地図の再作成、掃除、ペット等）が指示されることにより、その時々で、周囲の状況と指示されたタスクに基づき目的地が設定される場合もある。

【0036】

このメインフローの処理により、地図の情報に基づいて自律的に移動を行いつつ、適宜地図の情報を更新することができる。典型的な例としては、自律移動装置１００は最初、充電ステーションに置いてある状態で電源を投入すると、障害物センサ３１を頼りとして、家の各部屋をくまなく移動し、障害物センサ３１によって壁等の障害物位置を特定し、障害物位置を含む地図の情報を作成することができる。地図がある程度作成されると、地図の情報がまだないが移動可能と考えられる領域を知ることができ、その領域に自律的に移動する等して、より広範囲の地図の作成を促すこともできるようになる。そして、移動可能なほぼ全域の地図の情報が作成されれば、地図の情報を利用した効率的な移動動作が可能になる。例えば部屋のどの位置からでも最短経路で充電ステーションに戻ったり、効率的に部屋の掃除をしたりすることが可能になる。

【0037】

自律移動装置１００のメインフロー（図３）のステップＳ１０２で起動される自機位置推定スレッドについて、図４を参照して説明する。このスレッドは、位置推定部１３が、最初に初期化処理を行い、その後自機位置推定（撮像部４１で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。

【0038】

位置推定部１３は、まず、推定状態変数に「初期化中」をセットする（ステップＳ２０１）。推定状態変数は、その時点の自機位置推定処理の状態を示し、「初期化中」「追跡成功」「追跡失敗」の三つの値を取る。次に、動作終了か否かを判定する（ステップＳ２０２）。動作終了なら（ステップＳ２０２；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２０２；Ｎｏ）、推定状態変数が「初期化中」であるか否かを判定する（ステップＳ２０３）。初期化中でないなら（ステップＳ２０３；Ｎｏ）ステップＳ２２１以降の自機位置推定処理を行い、初期化中なら（ステップＳ２０３；Ｙｅｓ）ステップＳ２０４に進んで初期化処理を行う。まず初期化処理について説明する。

【0039】

初期化処理では位置推定部１３はまずフレームカウンタＮに−１をセットし（ステップＳ２０４）、撮像部４１で画像を取得する（ステップＳ２０５）。画像は例えば３０ｆｐｓで取得することができる（取得した画像はフレームとも呼ばれる）。次に、特徴点取得部１１は、撮像部４１で取得した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２０６）。

【0040】

取得した２Ｄ特徴点の個数が少ないと、後述するＴｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法での計算ができないため、位置推定部１３は、２Ｄ特徴点の取得数と基準値（例えば１０個）とを比較し（ステップＳ２０７）、基準値未満だった場合（ステップＳ２０７；Ｎｏ）はステップＳ２０５に戻り、基準値以上の２Ｄ特徴点数が得られるまで、画像の取得と２Ｄ特徴点の取得とを繰り返す。なお、この時点ではまだ地図の情報は作成できていないが、例えば上述した典型的な例では、障害物センサ３１を頼りとして、家の各部屋をくまなく移動し始めているため、この初期化処理で画像取得と２Ｄ特徴点取得を繰り返していれば、移動しながら画像取得を繰り返すことになる。従って、様々な画像が取得でき、いずれは２Ｄ特徴点数が多い画像を取得できることが期待できる。

【0041】

２Ｄ特徴点の取得数が基準値以上だった場合は（ステップＳ２０７；Ｙｅｓ）、位置推定部１３はフレームカウンタＮをインクリメントする（ステップＳ２０８）。そして、フレームカウンタＮが０かどうかを判定する（ステップＳ２０９）。フレームカウンタＮが０なら（ステップＳ２０９；Ｙｅｓ）画像をまだ一つしか取得していないということになるので、２枚目の画像を取得するためにステップＳ２０５に戻る。なお図４のフローチャートでは記載していないが、１枚目の画像を取得した時の自機の位置と２枚目の画像を取得する時の自機の位置とがある程度離れていた方が、これ以降の処理で推定する姿勢の精度が向上する。そこで、ステップＳ２０９からステップＳ２０５に戻る際に、メインフロー（図３）のステップＳ１０７での移動制御によりオドメトリによる並進距離が所定の距離（例えば１ｍ）以上となるまで待ってからステップＳ２０５に戻るようにしても良い。

【0042】

フレームカウンタＮが０でないなら（ステップＳ２０９；Ｎｏ）二つの画像を取得したことになるので、位置推定部１３は、これら二つの画像間で２Ｄ特徴点の対応（実環境上の同一の点がそれぞれの画像中に存在し、その対応が取れるもの）を取得する（ステップＳ２１０）。ここで特徴点の対応数が５未満であれば、後述する二つの画像間の姿勢の推定ができないため、位置推定部１３は特徴点の対応数が５未満かどうかを判定する（ステップＳ２１１）。５未満であれば（ステップＳ２１１；Ｙｅｓ）初期画像を取得し直すために、ステップＳ２０４に戻る。特徴点の対応数が５点以上であれば（ステップＳ２１１；Ｎｏ）、後述するＴｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法を用いることで、二つの画像間の姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定することができる（ステップＳ２１２）。

【0043】

この、Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法による姿勢の推定は、具体的には、対応する特徴点から基礎行列Ｅを求め、基礎行列Ｅを並進ベクトルｔと回転行列Ｒとに分解することによって行う（詳細は非特許文献２を参照）。なお、ここで得られる並進ベクトルｔ（３次元空間内で移動することを想定すると、最初の画像を取得した位置を原点として、Ｘ，Ｙ，Ｚの３要素を持つ）の各要素の値は実環境上での値とは異なる（Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法では実環境上の値自体を得ることはできず、実環境と相似する空間上での値を得ることになる。）ため、これらをＳＬＡＭ空間上での値とみなし、以下ではＳＬＡＭ空間上での座標（ＳＬＡＭ座標）を用いて説明する。

【0044】

二つの画像間の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まると、その値は、最初の画像を基準（最初の画像を取得した位置をＳＬＡＭ座標の原点、並進ベクトルは０ベクトル、回転行列は単位行列Ｉとする。）にした場合の、二枚目の画像の姿勢（二つ目の画像を取得した時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ））となる。ここで、二つの画像それぞれの姿勢（該画像（フレーム）撮影時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ）で、フレーム姿勢とも言う。）が求まっている場合、その二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）のＳＬＡＭ座標での３Ｄ位置を、以下の考え方に基づき、地図作成部１２が求める（ステップＳ２１３）。

【0045】

２Ｄ特徴点の画像中の座標（フレーム座標：既知）を（ｕ，ｖ）とし、その２Ｄ特徴点のＳＬＡＭ座標での３Ｄ位置（未知）を（Ｘ，Ｙ，Ｚ）とすると、これらを同次座標で表したときのこれらの関係は、透視投影行列Ｐを用いて下記の式（１）で表される。ここで、「〜」記号は「非零の定数倍を除いて等しい」（つまり、等しいか又は定数（非零）倍になっている）ことを表し、「’」記号は「転置」を表す。
（ｕ ｖ １）’〜Ｐ（Ｘ Ｙ Ｚ １）’…（１）

【0046】

上記の式（１）において、Ｐは３×４の行列で、カメラの内部パラメータを示す３×３の行列Ａと、その画像の姿勢（フレーム姿勢）を示す外部パラメータＲ及びｔから以下の式（２）で表される。ここで、（Ｒ｜ｔ）は、回転行列Ｒの右に並進列ベクトルｔを並べた行列を表す。
Ｐ＝Ａ（Ｒ｜ｔ）…（２）

【0047】

上記の式（２）において、Ｒ及びｔは上述したようにそのフレーム姿勢として求められている。また、カメラの内部パラメータＡは、焦点距離と撮像素子サイズにより決まるので、撮像部４１を決めておけば定数となる。

【0048】

二つの画像間で対応が取れている２Ｄ特徴点のうちの一つが、一つ目の画像のフレーム座標（ｕ_１，ｖ_１）と、二つ目の画像のフレーム座標（ｕ_２，ｖ_２）に写っているとすると、以下の式（３）及び式（４）ができる。ここで、Ｉは単位行列、０はゼロベクトル、（Ｌ｜ｒ）は、行列Ｌの右に列ベクトルｒを並べた行列を表す。
（ｕ_１ ｖ_１ １）’〜Ａ（Ｉ｜０）（Ｘ Ｙ Ｚ １）’…（３）
（ｕ_２ ｖ_２ １）’〜Ａ（Ｒ｜ｔ）（Ｘ Ｙ Ｚ １）’…（４）

【0049】

上記の式（３）及び式（４）において、ｕ_１，ｖ_１，ｕ_２，ｖ_２それぞれについての式ができるため、式は４つできるが、未知数はＸ，Ｙ，Ｚの３つなので、Ｘ，Ｙ，Ｚを求めることができ、これがその２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置となる。なお、式の個数の方が未知数の個数よりも多いため、例えばｕ_１，ｖ_１，ｕ_２で求めたＸ，Ｙ，Ｚとｕ_１，ｖ_１，ｖ_２で求めたＸ，Ｙ，Ｚとが異なる場合がありうる。このような場合は、過剰条件の連立一次方程式となり、一般には解が存在しないが、地図作成部１２は、最小二乗法を用いて、最も確からしいＸ，Ｙ，Ｚを求める。

【0050】

２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置（Ｘ，Ｙ，Ｚ）が求まったら、それをＭａｐ点として、地図作成部１２がＭａｐ点データベース（Ｍａｐ点ＤＢ（Ｄａｔａｂａｓｅ）とも言い、地図記憶部２２に格納される）に登録する（ステップＳ２１４）。Ｍａｐ点データベースに登録する要素としては、少なくとも、「２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置であるＸ，Ｙ，Ｚ」と、「その２Ｄ特徴点の特徴量」（例えばＳＩＦＴ等で得た特徴量）が必要である。

【0051】

そして、地図作成部１２は、二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）の全てをＭａｐ点データベースに登録したかを判定し（ステップＳ２１５）、まだ全ての登録ができていなかったら（ステップＳ２１５；Ｎｏ）ステップＳ２１３に戻り、全て登録できたら（ステップＳ２１５；Ｙｅｓ）ステップＳ２１６に進む。

【0052】

そして、位置推定部１３は、キーフレーム（後に続くスレッドでの処理対象となる画像を指す）のカウンタＮＫＦを０に初期化し（ステップＳ２１６）、二つ目の画像をキーフレームとしてフレームデータベース（フレームＤＢとも言い、画像記憶部２１に格納される）に登録する（ステップＳ２１７）。

【0053】

フレームデータベースに登録する要素は、「キーフレーム番号」（登録時点でのキーフレームカウンタＮＫＦの値）、「姿勢」（その画像撮影時の自機のＳＬＡＭ座標内での位置（並進ベクトルｔ）及び向き（回転行列Ｒ））、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、であるが、これらに加えて「オドメトリで計測した実環境上での姿勢」（実環境での駆動部４２による移動距離に基づいて求められる自機の位置及び向き）も登録しても良い。

【0054】

上記中、「キーフレーム自体の特徴」とは、キーフレーム間の画像類似度を求める処理を効率化するためのデータであり、通常は画像中の２Ｄ特徴点のヒストグラム等を用いるのが良いが、画像自体を「キーフレーム自体の特徴」としても良い。また、「オドメトリで計測した実環境上での姿勢」は、並進ベクトルｔと回転行列Ｒとで表すこともできるが、通常、本自律移動装置１００は２次元平面上を動くので、２次元データに単純化して、移動開始時の位置（原点）及び向きを基準にした２次元座標（Ｘ，Ｙ）及び向きφとして表しても良い。

【0055】

次に、位置推定部１３は、キーフレームが生成された事を地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキュー（キュー（Ｑｕｅｕｅ）は、先入れ先出しのデータ構造になっている）に、キーフレームカウンタＮＫＦをセットする（ステップＳ２１８）。

【0056】

以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１３は、推定状態変数に「追跡成功」をセットし（ステップＳ２１９）、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｄで除することによって、スケールＳを求める（ステップＳ２２０）。

【0057】

そして、ステップＳ２０２、ステップＳ２０３を経由して、初期化済の場合の処理であるステップＳ２２１へ進む。

【0058】

初期化済の場合の処理を説明する。この処理が、自機位置推定スレッドの通常時の処理であり、位置推定部１３が、逐次、現在の自機の位置及び向き（ＳＬＡＭ座標内での並進ベクトルｔと回転行列Ｒ）を推定する処理である。

【0059】

まず、位置推定部１３は、撮像部４１で画像を撮影し（ステップＳ２２１）、フレームカウンタＮをインクリメントする（ステップＳ２２２）。そして、特徴点取得部１１は、撮像部４１で撮影した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２２３）。

【0060】

次に、位置推定部１３は、推定状態変数が「追跡成功」か否かを判定する（ステップＳ２２４）。「追跡成功」なら（ステップＳ２２４；Ｙｅｓ）、特徴点対応数取得部１４は、フレームデータベースに登録されている一つ前のキーフレーム（キーフレーム番号がＮＫＦである画像）の情報から、その画像の情報に含まれている２Ｄ特徴点のうち、３Ｄ位置が既知である（Ｍａｐ点データベースに登録されているＭａｐ点になっている）２Ｄ特徴点を取得して、その中でステップＳ２２３で取得した特徴点と対応が取れる２Ｄ特徴点（対応特徴点）の個数（特徴点対応数）を取得し、特徴点対応数記憶部２３に記憶する（ステップＳ２２５）。

【0061】

推定状態変数の値が「追跡成功」でないなら（ステップＳ２２４；Ｎｏ）、特徴点対応数取得部１４は、フレームデータベースに登録されている二つ以上前のキーフレーム（キーフレーム番号がＮＫＦ未満である画像）の中から、Ｍａｐ点とステップＳ２２３で取得した特徴点との対応数が最大となるキーフレームと、その特徴点対応数を取得し、特徴点対応数記憶部２３に記憶する（ステップＳ２２６）。

【0062】

そして、位置推定部１３は、特徴点対応数が閾値（例えば３０。以下「基準対応特徴点数」という。）より大きいか否かを判定し（ステップＳ２２７）、基準対応特徴点数以下の場合（ステップＳ２２７；Ｎｏ）はＳＬＡＭ法で推定する姿勢の精度が悪くなるので、推定状態変数に「追跡失敗」をセットし（ステップＳ２３０）、位置の推定は行わずにステップＳ２０２に戻る。

【0063】

特徴点対応数が基準対応特徴点数より大きい場合は（ステップＳ２２７；Ｙｅｓ）、ステップＳ２２５又はステップＳ２２６において特徴点対応数を取得したキーフレームの近傍のキーフレーム（特徴点対応数を取得したキーフレームと、所定の割合以上（例えば３０％以上）特徴点の重なりが存在するキーフレーム）に含まれるＭａｐ点と対応が取れるステップＳ２２３で取得した特徴点（対応特徴点）の個数（特徴点対応数）を特徴点対応数取得部１４が取得し、特徴点対応数記憶部２３に記憶する（ステップＳ２２８）。

【0064】

そして、位置推定部１３は、特徴点対応数が閾値（例えば５０。以下「第２の基準対応特徴点数」という。）より大きいか否かを判定し（ステップＳ２２９）、第２の基準対応特徴点数以下なら（ステップＳ２２９；Ｎｏ）、推定状態変数に「追跡失敗」をセットし（ステップＳ２３０）、位置の推定は行わずにステップＳ２０２に戻る。特徴点対応数が第２の基準対応特徴点数よりも大きければ（ステップＳ２２９；Ｙｅｓ）、ステップＳ２３１に進む。

【0065】

ステップＳ２３１では、位置推定部１３は、推定状態変数に追跡成功をセットする（ステップＳ２３１）。位置推定部１３は、ステップＳ２２８で取得した対応特徴点それぞれの３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）をＭａｐ点データベースから取得し、ステップＳ２２１で取得した画像に含まれている対応特徴点との対応関係を用いて、現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）を推定する（ステップＳ２３２）。

【0066】

この自機の姿勢の推定方法について、補足説明する。ステップＳ２２１で取得した画像に含まれている対応特徴点のフレーム座標を（ｕ_ｉ，ｖ_ｉ）とし、その対応特徴点の３Ｄ位置を（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）とする（ｉは１から特徴点対応数までの値を取る）と、各対応特徴点の３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）を以下の式（５）によってフレーム座標系に投影した値（ｕｘ_ｉ，ｖｘ_ｉ）とフレーム座標（ｕ_ｉ，ｖ_ｉ）とは理想的には一致するはずである。
（ｕｘ_ｉ ｖｘ_ｉ １）’〜Ａ（Ｒ｜ｔ）（Ｘ_ｉ Ｙ_ｉ Ｚ_ｉ １）’…（５）

【0067】

しかし、実際には（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）にも（ｕ_ｉ，ｖ_ｉ）にも誤差が含まれているため、（ｕｘ_ｉ，ｖｘ_ｉ）と（ｕ_ｉ，ｖ_ｉ）とが一致することはめったにない。そして、未知数はＲとｔ（３次元空間ではそれぞれ３次元となり、３＋３＝６が未知数の個数である）だけなのに、数式は対応特徴点の個数の２倍存在する（対応特徴点一つに対して、フレーム座標のｕ，ｖそれぞれに対する式が存在するため）ことになるため、過剰条件の連立一次方程式になり、上述したように最小二乗法で求めることになる。具体的には、位置推定部１３は、以下の式（６）のコスト関数Ｅ１を最小化する姿勢（並進ベクトルｔ及び回転行列Ｒ）を求めることになる。これがＳＬＡＭ法で求めたＳＬＡＭ座標での自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）となる。このようにして、位置推定部１３は自機の姿勢を推定する（ステップＳ２３２）。

【数1】

【0068】

ＳＬＡＭ座標での現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求められたので、位置推定部１３は、これにスケールＳを乗算することで、ＶＯ（ビジュアルオドメトリ）を求める（ステップＳ２３３）。ＶＯは実環境での自機の位置及び向きとして利用できる。

【0069】

次に、位置推定部１３は、フレームＤＢに登録されている直前のキーフレーム（キーフレーム番号がＮＫＦである画像）を撮影した時の自機の位置から所定の距離（例えば１ｍ。以下「基準並進距離」という。）以上移動しているかを判定し（ステップＳ２３４）、移動しているなら（ステップＳ２３４；Ｙｅｓ）キーフレームカウンタＮＫＦをインクリメントしてから（ステップＳ２３５）、現フレームをキーフレームとしてフレームＤＢに登録する（ステップＳ２３６）。基準並進距離未満しか移動していないなら（ステップＳ２３４；Ｎｏ）ステップＳ２０２に戻る。

【0070】

ここで、基準並進距離と比較する自機の移動距離は、直前のキーフレームから現フレームまでの並進距離（両フレームの並進ベクトルの差のベクトルの絶対値（要素の二乗和の平方根））をオドメトリから取得しても良いし、上述したＶＯから求めても良い。フレームＤＢに登録する内容は上述したように、「キーフレーム番号」、「姿勢」、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、である。

【0071】

そして、位置推定部１３は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタＮＫＦをセットする（ステップＳ２３７）。そして、ステップＳ２０２に戻る。なお、キーフレームカウンタＮＫＦ、スケールＳ、Ｍａｐ点ＤＢ、フレームＤＢ、特徴点対応数はスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。

【0072】

次に、自律移動装置１００のメインフロー（図３）のステップＳ１０３で起動される地図作成スレッドについて、図５を参照して説明する。このスレッドは地図作成部１２が、キーフレーム中の対応特徴点の３Ｄ位置を計算して、地図の情報（Ｍａｐ点ＤＢ）を作成している。

【0073】

まず、地図作成部１２は、動作終了かを判定する（ステップＳ３０１）。動作終了なら（ステップＳ３０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ３０１；Ｎｏ）、キーフレームキューが空かどうかを判定する（ステップＳ３０２）。キーフレームキューが空だったら（ステップＳ３０２；Ｙｅｓ）ステップＳ３０１に戻り、空でなければ（ステップＳ３０２；Ｎｏ）、キーフレームキューからデータを取り出してＭＫＦ（地図作成スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ３０３）。地図作成部１２は、ＭＫＦが０より大きいか判定し（ステップＳ３０４）、ＭＫＦが０である場合は（ステップＳ３０４；Ｎｏ）ステップＳ３０１に戻ってキーフレームキューにデータが入るのを待つ。ＭＫＦが１以上の場合は（ステップＳ３０４；Ｙｅｓ）、以下の処理に進む。

【0074】

地図作成部１２は、フレームＤＢを参照し、前キーフレーム（キーフレーム番号がＭＫＦ−１のキーフレーム）の２Ｄ特徴点と現キーフレーム（キーフレーム番号がＭＫＦのキーフレーム）の２Ｄ特徴点とで対応が取れる２Ｄ特徴点（対応特徴点）を抽出する（ステップＳ３０５）。フレームＤＢにはそれぞれのキーフレームの姿勢（並進ベクトルｔと回転行列Ｒ）も登録されているので、自機位置推定スレッドの初期化時の処理の時と同様の方法で対応特徴点の３Ｄ位置を計算できる。地図作成部１２は、３Ｄ位置が計算できた対応特徴点をＭａｐ点としてＭａｐ点ＤＢに登録する（ステップＳ３０６）。地図作成部１２は、フレームＤＢ中の他のキーフレームに対しても今回３Ｄ位置を計算できた２Ｄ特徴点に対して３Ｄ位置を登録する（ステップＳ３０７）。

【0075】

なお、地図作成部１２が抽出した対応特徴点がすでにＭａｐ点ＤＢに登録済だった場合は、３Ｄ位置計算をスキップして次の対応特徴点（Ｍａｐ点ＤＢに未登録のもの）に対する処理に進んでも良いし、改めて３Ｄ位置計算を行って、Ｍａｐ点ＤＢに登録済の３Ｄ位置や、フレームＤＢ中の対応特徴点に対する３Ｄ位置を更新するようにしても良い。

【0076】

次に、地図作成部１２は、キーフレームキューが空かどうかを判定する（ステップＳ３０８）。空であれば（ステップＳ３０８；Ｙｅｓ）、全キーフレームの姿勢と全Ｍａｐ点の３Ｄ位置に対して、バンドルアジャストメント処理を行って、精度向上を図る（ステップＳ３０９）。そして、バンドルアジャストメント処理を行った結果、誤差が大きいＭａｐ点が見つかったら、それをＭａｐ点ＤＢから削除し（ステップＳ３１０）、ステップＳ３１１に進む。キーフレームキューが空でなければ（ステップＳ３０８；Ｎｏ）、何もせずにステップＳ３１１に進む。

【0077】

ステップＳ３１１では、地図作成部１２は、ループクロージングスレッドのキーフレームキューにＭＫＦをセットして（ステップＳ３１１）、ステップＳ３０１に戻る。

【0078】

なお、バンドルアジャストメント処理とは、カメラ姿勢（キーフレーム姿勢）とＭａｐ点の３Ｄ位置とを同時に推定する非線形最適化法であり、Ｍａｐ点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。

【0079】

このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とＭａｐ点の３Ｄ位置の精度向上を図ることができる。しかし、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、他の処理がない状態のとき（例えば、キーフレームキューが空の状態）であっても、この処理を毎回行う必要はない。

【0080】

また、バンドルアジャストメントの処理を行うと、キーフレーム上に投影させたときの誤差が所定の値よりも大きいＭａｐ点が見つかることがある。このような誤差の大きなＭａｐ点については、ＳＬＡＭ推定に悪影響を及ぼすため、ステップＳ３１０ではＭａｐ点ＤＢから削除している。なお、Ｍａｐ点ＤＢから削除するのではなく、Ｍａｐ点ＤＢにおいて、誤差の大きな注意を要するＭａｐ点であることを識別するためのフラグを立てておいても良い。

【0081】

次に自律移動装置１００のメインフロー（図３）のステップＳ１０４で起動されるループクロージングスレッドについて、図６を参照して説明する。このスレッドでは制御部１０は、ループクロージング処理ができるかどうかをチェックし続け、ループクロージング処理ができる場合にはループクロージング処理を行っている。なお、ループクロージング処理とは、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの軌跡中のキーフレームや、関連するＭａｐ点の３Ｄ位置を修正することをいう。

【0082】

まず、制御部１０は、動作終了かを判定する（ステップＳ４０１）。動作終了なら（ステップＳ４０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ４０１；Ｎｏ）キーフレームキューが空かどうかを判定する（ステップＳ４０２）。キーフレームキューが空なら（ステップＳ４０２；Ｙｅｓ）ステップＳ４０１に戻り、キーフレームキューが空でないなら（ステップＳ４０２；Ｎｏ）、キーフレームキューからデータを取り出してＬＫＦ（ループクロージングスレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ４０３）。次に制御部１０は、ＬＫＦが１より大きいかを判定する（ステップＳ４０４）。ＬＫＦが０又は１である場合（ステップＳ４０４；Ｎｏ）はステップＳ４０１に戻ってキーフレームキューにデータが入るのを待つ。そして、ＬＫＦが２以上の場合（ステップＳ４０４；Ｙｅｓ）は、以下の処理を行う。

【0083】

制御部１０は、フレームＤＢを参照し、現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）と「キーフレーム自体の特徴」の類似度が所定の類似度（例えば０．９。以下「基準画像類似度」という。）以上になるキーフレームをフレームＤＢから検索する（ステップＳ４０５）。ここで、この類似度は、画像（キーフレーム）の特徴を特徴ベクトルで表している場合は、二つの画像の特徴ベクトルの絶対値（要素の二乗和の平方根）を１に正規化したもの同士の内積を、その二つの画像の類似度とすることができる。また、二つの画像の特徴ベクトル（絶対値を１に正規化したもの）の距離（各要素の差の二乗和の平方根）の逆数を類似度としても良い。

【0084】

制御部１０は、「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームが発見されたかどうかを判定し（ステップＳ４０６）、発見されなければ（ステップＳ４０６；Ｎｏ）ステップＳ４０１へ戻り、発見されたら（ステップＳ４０６；Ｙｅｓ）、発見されたキーフレームから現キーフレームまでの軌跡中のキーフレームの姿勢と、軌跡中のキーフレームに含まれるＭａｐ点の３Ｄ位置を修正する（ステップＳ４０７）。例えば、制御部１０は、現キーフレームの姿勢を、発見されたキーフレームの姿勢と同じ姿勢として修正する。そして、発見されたキーフレームの姿勢と現キーフレームの姿勢との差分を用いて、発見されたキーフレームから現キーフレームまでの軌跡中の各キーフレームの姿勢に線形的に補正を加える。さらにこれらの各キーフレームに含まれるＭａｐ点の３Ｄ位置についても各キーフレームの姿勢の補正量に応じて修正する。そしてステップＳ４０１に戻る。

【0085】

次に自律移動装置１００のメインフロー（図３）のステップＳ１０７の処理である移動制御について説明する。これは、基本的には自律的に目的地へ移動するために駆動部４２に動作指示を出すが、特徴点対応数が少なくなると、速度を低下させたり、停止させたり、移動方向を変更させたりすることによって、特徴点対応数を増やす処理である。

【0086】

なお、移動制御の処理において、図３のステップＳ１０１で簡単に説明した移動状態変数と前回履歴数の二つの変数が使われるので、補足説明しておく。移動状態変数は、「通常」「低速度」「停止」「戻り」のいずれかの値を持ち、移動速度や移動方向の変更に用いられる。前回履歴数は、移動履歴記憶部２４に記憶された移動履歴の個数がセットされ、後述する「戻り」の移動後に、別の目的地を設定するか否かを決定する際に使われる。

【0087】

では、移動制御について、図７を参照して説明する。まず、移動方向速度変更部１５は、移動状態変数の値が「戻り」であるか否かを判定する（ステップＳ５０１）。移動状態変数の値が「戻り」であるなら（ステップＳ５０１；Ｙｅｓ）、ステップＳ５１９へ進む。ステップＳ５１９以降の処理は後述する。

【0088】

移動状態変数の値が「戻り」でなければ（ステップＳ５０１；Ｎｏ）、移動方向速度変更部１５は目的地が未設定であるか又は目的地に到着したかを判定する（ステップＳ５０２）。目的地が未設定であるか又は目的地に到着したならば（ステップＳ５０２；Ｙｅｓ）、制御部１０は、目的地を設定し（ステップＳ５０３）、ステップＳ５０４へ進む。ステップＳ５０３において、制御部１０は目的地設定部に相当する。目的地が設定されており、かつ、まだ目的地に到着していなければ（ステップＳ５０２；Ｎｏ）、ステップＳ５０４へ進む。

【0089】

ステップＳ５０４では、移動方向速度決定部１６は、現在位置、目的地及び地図記憶部２２に記憶されている地図の情報に基づき、移動方向を決定する（ステップＳ５０４）。

【0090】

次に、移動方向速度変更部１５は、特徴点対応数記憶部２３に記憶されている特徴点対応数が第１移動変更閾値（例えば１００）よりも大きいか否かを判定する（ステップＳ５０５）。第１移動変更閾値よりも大きければ（ステップＳ５０５；Ｙｅｓ）、移動状態変数に「通常」をセットし（ステップＳ５０６）、ステップＳ５０９へ進む。

【0091】

移動方向速度変更部１５は、特徴点対応数記憶部２３に記憶されている特徴点対応数が第１移動変更閾値以下であれば（ステップＳ５０５；Ｎｏ）、特徴点対応数が第２移動変更閾値（例えば３０）よりも大きいか否かを判定する（ステップＳ５０７）。第２移動変更閾値よりも大きければ（ステップＳ５０７；Ｙｅｓ）、移動状態変数に「低速度」をセットし（ステップＳ５０８）、ステップＳ５０９へ進む。なお、第１移動変更閾値と第２移動変更閾値はともに数十以上の任意の値を設定できるが、第１移動変更閾値は第２移動変更閾値よりも大きな値に設定しておく。

【0092】

ステップＳ５０９で、移動方向速度決定部１６は、ステップＳ５０４で決定した移動方向と、移動状態変数の値が示す速度から得られる移動距離と、を移動履歴記憶部２４に記憶する。なお、移動方向及び移動距離の代わりに、自機位置推定スレッドで取得している自機の姿勢（位置（並進ベクトルｔ）及び向き（回転行列Ｒ））を移動履歴記憶部２４に記憶していくようにしてもよい。

【0093】

そして、移動方向速度決定部１６は、ステップＳ５０４で決定した移動方向と、移動状態変数の値が示す速度に基づいて、駆動部４２を駆動させ（ステップＳ５１０）、処理を終了する。

【0094】

特徴点対応数記憶部２３に記憶されている特徴点対応数が第２移動変更閾値以下なら（ステップＳ５０７；Ｎｏ）、移動方向速度変更部１５は、移動状態変数の値が「停止」であるか否かを判定する（ステップＳ５１１）。移動状態変数の値が「停止」でなければ（ステップＳ５１１；Ｎｏ）、移動状態変数に「停止」をセットし（ステップＳ５１２）、制御部１０が備えるタイマーのカウントを開始させ（ステップＳ５１３）、終了する。

【0095】

移動状態変数の値が「停止」であるなら（ステップＳ５１１；Ｙｅｓ）、移動方向速度変更部１５は、ステップＳ５１３でスタートさせたタイマーの値が基準停止時間（例えば１０秒）よりも大きくなっているかを判定する（ステップＳ５１４）。タイマーの値が基準停止時間以下であれば（ステップＳ５１４；Ｎｏ）、終了する。

【0096】

タイマーの値が基準停止時間よりも大きければ（ステップＳ５１４；Ｙｅｓ）、移動方向速度変更部１５は、動作状態変数に「戻り」をセットし（ステップＳ５１５）、その時点で移動履歴記憶部２４に記憶されている履歴の個数が、前回履歴数より大きいか否かを判定する（ステップＳ５１６）。移動履歴記憶部２４に記憶されている履歴の個数が、前回履歴数より大きいなら（ステップＳ５１６；Ｙｅｓ）、終了する。

【0097】

移動履歴記憶部２４に記憶されている履歴の個数が、前回履歴数以下なら（ステップＳ５１６；Ｎｏ）、制御部１０は、前回履歴数に、移動履歴記憶部２４に記憶されている履歴の個数をセットし（ステップＳ５１７）、別の目的地を設定する（ステップＳ５１８）。ステップＳ５１８において、制御部１０は、目的地変更部に相当する。ステップＳ５１８の後、終了する。

【0098】

ステップＳ５０１で動作状態変数が「戻り」だった場合は（ステップＳ５０１；Ｙｅｓ）、移動方向速度決定部１６は、移動履歴記憶部２４に最後に記憶された移動方向と逆の方向を次の移動時の移動方向に決定し、移動履歴記憶部２４に最後に記憶された移動距離を次の移動時の移動距離に決定する（ステップＳ５１９）。もし、移動履歴記憶部２４に記憶されている情報が自機の位置及び向きであった場合は、移動方向速度決定部１６は、現在位置から移動履歴記憶部２４に最後に記憶された自機の位置及び向きに戻るための移動方向及び移動距離を次の移動時の移動方向及び移動距離に決定する（ステップＳ５１９）。

【0099】

次に、移動方向速度変更部１５は、所定の数の履歴を戻ったか否かを判定するために、前回履歴数からその時点で移動履歴記憶部２４に記憶されている履歴の個数を引いた数が、所定戻り数（例えば１０）以上か否かを判定する（ステップＳ５２０）。所定戻り数未満であれば（ステップＳ５２０；Ｎｏ）、まだ「戻り」の移動を継続するために、そのままステップＳ５２２に進む。

【0100】

所定戻り数以上であれば（ステップＳ５２０；Ｙｅｓ）、「戻り」の移動を終了させるために、動作状態変数に「通常」をセットし（ステップＳ５２１）、ステップＳ５２２に進む。

【0101】

ステップＳ５２２では、移動方向速度変更部１５は、移動履歴記憶部２４に最後に記憶された移動履歴を削除する（ステップＳ５２２）。そして、移動方向速度決定部１６は、ステップＳ５０４で決定した移動方向と、移動状態変数の値が示す速度に基づいて、駆動部４２を駆動させ（ステップＳ５１０）、処理を終了する。

【0102】

ステップＳ５０３における目的地の設定は、例えば、通信部４４により目的地の情報を取得して設定する。この時、通信部４４は目的地取得部に相当する。また、ステップＳ５１８における別の目的地の設定も、例えば、通信部４４により別の目的地の情報を取得して設定する。

【0103】

以上の処理がメインフロー（図３）に示すステップＳ１０７の移動制御処理として何度も繰り返し実行されることで、自律移動装置１００が停止したり、戻ったりする様子を、図８を参照して具体例で説明する。特徴点対応数が第２移動変更閾値よりも大きければ目的地に向かって移動を続ける（図８の１．ステップＳ５０６又はステップＳ５０８）。特徴点対応数が第２移動変更閾値以下になったら停止する（図８の２．ステップＳ５１２）。

【0104】

停止している間にバックグラウンドで動いている地図作成スレッドにより、Ｍａｐ点データベースへのＭａｐ点の登録が継続して行われているため、これによって特徴点対応数が第２移動変更閾値よりも大きくなれば、また移動が再開する（ステップＳ５１４；Ｎｏで終了した後、それ以後の移動制御のステップＳ５０６又はステップＳ５０８）。しかし、基準停止時間だけ停止していても特徴点対応数が第２移動変更閾値以下のままであったら、所定戻り数だけ戻る（図８の３．ステップＳ５１５、ステップＳ５２０）。なお、図８では、所定戻り数を２としている。

【0105】

所定戻り数だけ戻ると再度目的地に向かって移動する（図８の４．）。再度目的地に向かっている最中に特徴点対応数が第２移動変更閾値よりも大きい状態が続いていれば、そのまま移動を続ける（図８の５．ステップＳ５０６又はステップＳ５０８）。しかし、前回進んだ履歴よりも進まなかった場合には、その目的地に移動しようとしても特徴点対応数が少なくて途中で何度も戻ることになってしまうため、目的地を変更し（図８の５’．ステップＳ５１８）、所定戻り数だけ戻る（図８の６．ステップＳ５２０）。所定戻り数だけ戻った後に、変更された別の目的地に向かって移動する（図８の７．ステップＳ５０６又はステップＳ５０８）。

【0106】

以上のような移動制御をすることにより、自機位置推定スレッドによる自機位置の推定が可能な状態をできるだけ長い時間保つことができるようになる。なお、図７のステップＳ５０８では、ステップＳ５０４で決定した移動方向を変更するようにしても良い。この変更はランダムな方向に変更しても良いし、自律移動装置１００がその場で３６０度回転しつつ画像を撮影し、撮影した画像の中で最も特徴点の多い画像の方向に移動方向を変更するようにしても良い。また、図７のステップＳ５１８で設定している「別の目的地」は、複数の目的地が存在する場合は、第１候補→第２候補→第３候補と順番に変更していけば良い。また、同じ目的地に対して、複数の経路が存在している場合は、目的地は変えずに経路を変更しても良い。また、特に目的地はなく自由に移動している場合は、ランダムに目的地を設定しても良い。

【0107】

特に目的地がない場合は、特徴点の多い方向に移動するのが望ましい。このような移動方法としては、例えば、自律移動装置１００がその場で３６０度回転しつつ所定の角度毎に目的地候補となる方向の画像を撮影し、撮影した画像の中で最も特徴点の多い画像の方向を新たな目的地として、その方向に向かって移動する方法が挙げられる。ここで、制御部１０は、所定の角度毎に目的地候補を選定しているので、目的地候補選定部に相当する。そして、特徴点取得部１１は、目的地候補の方向の画像から特徴点を取得しているので、目的地候補特徴点取得部に相当する。このような処理を行うことで、ランダムに移動するよりも、特徴点対応数が多くなる方向に移動できる可能性が高まり、自機位置の推定が可能な状態に復帰しやすくなる。

【0108】

なお、特徴点対応数が少なくなる原因としては、周囲の景色自体に特徴点が少ないこと以外に、周囲が暗くて露光時間が長くなり、撮像部４１で撮影した画像にブレが生じてしまうことも挙げられる。図７の処理では、特徴点対応数が少なくなると速度を低下させたり、停止したりするので、撮影画像中のブレが少なくなるような移動制御になっている。この点に関して、特徴点対応数取得部１４で特徴点対応数を取得していない状態でも、周囲の明るさを明るさセンサ（図示せず）や撮像部４１で取得し、所定の明るさよりも暗かったら速度を低下させたり停止させたりすることでブレの少ない画像を取得できるようにしても良い。

【0109】

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

【0110】

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

【0111】

（付記１）
撮像部が撮影した複数の画像の情報に基づいて自律的な移動を制御する移動制御部と、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得部と、
前記特徴点取得部が取得した特徴点の位置を記憶する地図記憶部と、
前記特徴点取得部が取得した特徴点と、前記地図記憶部に位置が記憶されている特徴点と、の対応数である特徴点対応数を取得する特徴点対応数取得部と、を備え、
前記移動制御部は、
前記特徴点対応数取得部が取得した特徴点対応数に応じて移動を制御する、
自律移動装置。

【0112】

（付記２）
前記移動制御部は、前記特徴点対応数取得部が取得した特徴点対応数が、第１の閾値以下の場合に、移動方向を変更する、
付記１に記載の自律移動装置。

【0113】

（付記３）
前記移動制御部は、前記特徴点対応数取得部が取得した特徴点対応数が、第１の閾値以下の場合に、移動速度を低下させる、
付記１又は２に記載の自律移動装置。

【0114】

（付記４）
前記移動制御部は、前記特徴点対応数取得部が取得した特徴点対応数が、前記第１の閾値よりも小さい第２の閾値以下の場合に、所定の時間、前記移動速度を０にする、
付記３に記載の自律移動装置。

【0115】

（付記５）
前記自律移動装置は、さらに、過去の移動履歴を記憶する移動履歴記憶部を備え、
前記移動制御部は、前記移動速度を０にしてから前記所定の時間が経過しても、前記特徴点対応数取得部が取得した特徴点対応数が前記第２の閾値以下の場合には、前記移動履歴記憶部に記憶されている移動履歴に基づいて所定の回数だけ過去の位置に戻るように移動方向を変更する、
付記４に記載の自律移動装置。

【0116】

（付記６）
前記自律移動装置は、さらに、移動の目的地を取得する目的地取得部を備え、
前記移動制御部は、前記所定の回数だけ過去の位置に戻るように移動方向を変更した後に、前記目的地取得部が取得した目的地に向かって移動するように移動方向を変更する、
付記５に記載の自律移動装置。

【0117】

（付記７）
前記自律移動装置は、さらに、前記目的地取得部が取得した目的地を変更する目的地変更部を備え、
前記移動制御部が、前記過去の位置に戻るように移動方向を変更した後に、前記目的地取得部が取得した目的地に向かって移動するように移動方向を変更した後、前記過去の位置に戻る前の位置より先に進む前に、前記特徴点対応数取得部が取得した特徴点対応数が前記第２の閾値以下になった場合は、前記目的地変更部が前記目的地を変更する、
付記６に記載の自律移動装置。

【0118】

（付記８）
前記自律移動装置は、さらに、複数の目的地候補を選定する目的地候補選定部と、
前記目的地候補選定部が選定した目的地候補の方向の画像から特徴点を取得する目的地候補特徴点取得部と、を備え、
前記目的地変更部は、前記目的地を、前記目的地候補選定部が選定した複数の目的地候補の中で前記目的地候補特徴点取得部が取得した特徴点の数が最大となる目的地候補に変更する、
付記７に記載の自律移動装置。

【0119】

（付記９）
撮像部が撮影した複数の画像の情報に基づいて自律的に移動するための自律移動方法であって、
前記撮像部が撮影した複数の画像から取得した画像特徴点と、実空間内での位置の推定を行った推定特徴点との対応数に応じて自律的に移動する、
自律移動方法。

【0120】

（付記１０）
前記画像特徴点と前記推定特徴点との対応数が第１の閾値以下になると移動速度を低下させる、
付記９に記載の自律移動方法。

【0121】

（付記１１）
前記画像特徴点と前記推定特徴点との対応数が前記第１の閾値よりも小さい第２の閾値以下になると、所定の時間停止する、
付記１０に記載の自律移動方法。

【0122】

（付記１２）
所定の時間停止していても前記画像特徴点と前記推定特徴点との対応数が前記第２の閾値以下の場合には、過去の位置に戻る、
付記１１に記載の自律移動方法。

【0123】

（付記１３）
撮像部が撮影した複数の画像の情報に基づいて自律的な移動の方向及び速度を決定する移動方向速度決定ステップと、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得ステップと、
前記特徴点取得ステップで取得した特徴点の位置を記憶する地図記憶部に位置が記憶されている特徴点と、前記特徴点取得ステップで取得した特徴点と、の対応数を取得する特徴点対応数取得ステップと、
前記特徴点対応数取得ステップで取得した特徴点対応数に応じて移動を制御する移動制御ステップと、
を備える自律移動方法。

【0124】

（付記１４）
コンピュータに、
撮像部が撮影した複数の画像の情報に基づいて自律的な移動の方向及び速度を決定する移動方向速度決定ステップ、
前記撮像部が撮影した画像から特徴点を取得する特徴点取得ステップ、
前記特徴点取得ステップで取得した特徴点の位置を記憶する地図記憶部に位置が記憶されている特徴点と、前記特徴点取得ステップで取得した特徴点と、の対応数を取得する特徴点対応数取得ステップ、
前記特徴点対応数取得ステップで取得した特徴点対応数に応じて移動を制御する移動制御ステップ、
を実行させるためのプログラム。

【符号の説明】

【0125】

１００…自律移動装置、１０…制御部、１１…特徴点取得部、１２…地図作成部、１３…位置推定部、１４…特徴点対応数取得部、１５…移動方向速度変更部、１６…移動方向速度決定部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…特徴点対応数記憶部、２４…移動履歴記憶部、３０…センサ部、３１…障害物センサ、４１…撮像部、４２…駆動部、４３…入力部、４４…通信部、４５…電源

【要約】

【課題】ループを検出しなくても誤差を減らす。
【解決手段】自律移動装置１００の移動方向速度決定部１６は、撮像部が撮影した複数の画像の情報に基づいて自律的な移動の方向及び速度を決定する。特徴点取得部１１は撮像部が撮影した画像から特徴点を取得する。地図記憶部２２は特徴点取得部が取得した特徴点の位置を記憶する。特徴点対応数取得部１４は、特徴点取得部が取得した特徴点と地図記憶部に位置が記憶されている特徴点との対応数を取得する。移動方向速度変更部１５は、特徴点対応数取得部が取得した特徴点対応数が閾値以下の場合に、特徴点対応数を増加させるために、移動方向速度決定部１６による移動方向及び移動速度を変更する。
【選択図】図２

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】