(57)【特許請求の範囲】
【請求項１】
ロボットを駆動し、前記ロボットの現在の位置・姿勢情報をリアルタイムに取得する動作モジュールと、
画像センサーを含み、ロボット移動過程における環境画像を採取するカメラモジュールと、
前記画像センサーによって採取された前記環境画像に対して特徴抽出と特徴描写を行う画像処理モジュールと、
複数の特徴点を含む特徴データベースを構築し、前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記ロボットの位置・姿勢修正量を算出し、さらに前記ロボットの現在の位置・姿勢情報と前記位置・姿勢修正量に基づいて、修正後のロボットの位置・姿勢を取得する位置・姿勢推定モジュールと、を備え、
前記位置・姿勢推定モジュールは、
複数の特徴点に対する多次元描写に基づいて、前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記現在の環境画像の特徴点描写が特徴データベースにおける少なくとも1つの特徴点とマッチングしない場合に、前記環境画像の対応するデータを前記特徴データベースに保存するマッチングユニットと、
前記マッチングユニットが前記現在の環境画像の特徴点描写が前記特徴データベースにおける少なくとも1つの特徴点とマッチングすることを示した場合に、前記ロボットの位置・姿勢修正量を算出し、前記ロボットの現在の位置・姿勢と前記位置・姿勢修正量に基づいて、修正後のロボット位置・姿勢を取得する位置・姿勢修正ユニットと、を備え、
前記位置・姿勢修正ユニットは、
２つの画像におけるマッチング画素の画素距離に表される空間におけるカメラ位置間の距離値を算出し、
各２フレーム画像間の回転角度θと画素の並進量ｐｄに基づいて、１グループの現在のロボットの推定位置・姿勢を算出し、
ｋ−ｍｅａｎｓクラスタリング手法を用いて、ロボットの位置・姿勢の最終推定値を求め、
当該推定値とロボットの現在の未修正の位置・姿勢の結果を減算して、ロボット位置・姿勢の修正量を取得し、
現在の位置・姿勢と修正量を加算して、修正後の位置・姿勢を取得することを特徴とする視覚的測位によるナビゲーション装置。
【請求項２】
前記動作モジュールは、ジャイロスコープにより、ロボットの現在の偏向角度θを取得し、ロボットのホイール上の光電パルスカウンターにより、ロボットの移動距離ｄを取得し、以下の数式：
Ｒｔｈ２＝Ｒｔｈ１＋θ
Ｒｘ２＝Ｒｘ１＋ｄ＊ｃｏｓ（θ）
Ｒｙ２＝Ｒｙ１＋ｄ＊ｓｉｎ（θ）
を用いて、ロボットの現在の位置・姿勢（Ｒｘ、Ｒｙ、Ｒｔｈ）を算出することを特徴とする請求項１に記載の視覚的測位によるナビゲーション装置。
【請求項３】
前記カメラモジュールは、ロボットの真上に位置し、前記環境画像は、天井の写真であることを特徴とする請求項１に記載の視覚的測位によるナビゲーション装置。
【請求項４】
前記画像処理モジュールは、
前記環境画像に対して前処理を行うことにより、レンズの歪みが特徴描写に及ぼす影響を取り除く画像前処理ユニットと、
ＯＲＢ特徴点検出方法を用いて前記環境画像における特徴点を抽出し、前記特徴点を多次元描写する特徴点描写ユニットと、を備えることを特徴とする請求項１に記載の視覚的測位によるナビゲーション装置。
【請求項５】
前記マッチングユニットは、ユークリッド距離を特徴の類似性尺度として用いて、２フレーム画像における特徴点マッチングセットを取得することを特徴とする請求項１に記載の視覚的測位によるナビゲーション装置。
【請求項６】
前記位置・姿勢修正ユニットは、以下の数式：
Ｒｔｈ＝Ｄｔｈ＋θ  
Ｒｘ＝Ｄｘ＋ｐｄ＊ｋ＊ｃｏｓ（Ｒｔｈ）
Ｒｙ＝Ｄｙ＋ｐｄ＊ｋ＊ｓｉｎ（Ｒｔｈ）
を用いて、ロボットの現在の推定位置・姿勢を算出し、
ここで、kは、画素の画素距離と空間におけるカメラ位置間の距離との比であり、Ｄｘ、Ｄｙ、Ｄｔｈは、当時のロボットの位置・姿勢であることを特徴とする請求項１に記載の視覚的測位によるナビゲーション装置。
【請求項７】
ロボットを駆動しながら、ロボットの現在の位置・姿勢情報をリアルタイムに取得するステップと、
ロボット移動過程における環境画像を採取するステップと、
前記環境画像に対して特徴抽出と特徴描写を行うステップと、
複数の特徴点を含む特徴データベースを構築するステップと、
前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記ロボットの位置・姿勢修正量を算出し、さらに前記ロボットの現在の位置・姿勢情報と前記位置・姿勢修正量に基づいて、修正後のロボットの位置・姿勢を取得するステップと、を含み、
前記複数の特徴点を含む特徴データベースを構築し、前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記ロボットの位置・姿勢修正量を算出し、さらに前記ロボットの現在の位置・姿勢情報と前記位置・姿勢修正量に基づいて、修正後のロボットの位置・姿勢を取得するステップは、
複数の特徴点に対する多次元描写に基づいて、前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記現在の環境画像の特徴点描写が特徴データベースにおける少なくとも1つの特徴点とマッチングしない場合に、前記環境画像の対応するデータを前記特徴データベースに保存するステップと、
前記現在の環境画像の特徴点描写が前記特徴データベースにおける少なくとも1つの特徴点とマッチングする場合に、前記ロボットの位置・姿勢修正量を算出し、前記ロボットの現在の位置・姿勢と前記位置・姿勢修正量に基づいて、修正後のロボット位置・姿勢を取得するステップと、を含み、
前記ロボットの位置・姿勢修正量を算出し、さらに前記ロボットの現在の位置・姿勢と前記位置・姿勢修正量に基づいて、修正後のロボット位置・姿勢を取得するステップは、
２つの画像におけるマッチング画素の画素距離に表される空間におけるカメラ位置間の距離値を算出するステップと、
各２フレーム画像間の回転角度θと画素の並進量ｐｄに基づいて、１グループの現在のロボットの推定位置・姿勢を算出するステップと、
ｋ−ｍｅａｎｓクラスタリング手法を用いて、ロボット位置・姿勢の最終推定値を求めるステップと、
当該推定値とロボットの現在の未修正の位置・姿勢の結果を減算して、ロボットの位置・姿勢の修正量を取得するステップと、
現在の位置・姿勢と修正量を加算して、修正後の位置・姿勢を取得するステップと、を含むことを特徴とする視覚的測位によるナビゲーション方法。
【請求項８】
前記ロボットの現在の位置・姿勢情報をリアルタイムに取得するステップは、ジャイロスコープにより、ロボットの現在の偏向角度θを取得し、ロボットのホイール上の光電パルスカウンターにより、ロボットの移動距離ｄを取得し、以下の数式：
Ｒｔｈ２＝Ｒｔｈ１＋θ；
Ｒｘ２＝Ｒｘ１＋ｄ＊ｃｏｓ（θ）；
Ｒｙ２＝Ｒｙ１＋ｄ＊ｓｉｎ（θ）
を用いて、前記ロボットの現在の位置・姿勢（Ｒｘ，Ｒｙ，Ｒｔｈ）を算出するステップを含むことを特徴とする請求項７に記載の視覚的測位によるナビゲーション方法。
【請求項９】
前記環境画像は、天井の写真であることを特徴とする請求項７に記載の視覚的測位によるナビゲーション方法。
【請求項１０】
前記環境画像に対して特徴抽出と特徴描写を行うステップは、
前記環境画像に対して前処理を行うことにより、レンズの歪みが特徴描写に及ぼす影響を取り除くステップと、
ＯＲＢ特徴点検出方法を用いて前記環境画像における特徴点を抽出し、前記特徴点を多次元描写するステップと、を含むことを特徴とする請求項７に記載の視覚的測位によるナビゲーション方法。
【請求項１１】
前記複数の特徴点に対する多次元描写に基づいて、前記環境画像の特徴点描写をマッチングするステップは、
ユークリッド距離を特徴の類似性尺度として用いて、２フレーム画像における特徴点マッチングセットを取得するステップを含むことを特徴とする請求項７に記載の視覚的測位によるナビゲーション方法。
【請求項１２】
前記１グループの現在のロボットの推定位置・姿勢を算出するステップは、以下の数式：
Ｒｔｈ＝Ｄｔｈ＋θ  
Ｒｘ＝Ｄｘ＋ｐｄ＊ｋ＊ｃｏｓ（Ｒｔｈ）
Ｒｙ＝Ｄｙ＋ｐｄ＊ｋ＊ｓｉｎ（Ｒｔｈ）
により計算するステップを含み、
ここで、kは、画素の画素距離と空間におけるカメラ位置間の距離との比であり、Ｄｘ、Ｄｙ、Ｄｔｈは、当時のロボットの位置・姿勢であることを特徴とする請求項７に記載の視覚的測位によるナビゲーション方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御領域に関し、特に天井の特徴点に基づく視覚的測位によるナビゲーション装置およびその方法に関する。
【背景技術】
【０００２】
ホームオートメーションの発展に伴い、ロボット掃除機はより多くの家庭にて愛用されている。従来のロボット掃除機は、赤外線センサとカメラを利用することにより、視覚的測位によるナビゲーションを可能としているが、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ、自己位置推定と環境地図の同時作成）技術はロボット領域において古い課題である。通常、ＳＬＡＭ問題とは、ロボットは未知環境において、ある未知の位置から移動を開始し、移動過程において、位置・姿勢推定と地図に基づいて自己を測位し、同時に自己を測位した基礎の上に増分地図を作成し、ロボットの自主測位とナビゲーションを実現することをいう。ＶＳＬＡＭ（Ｖｉｓｕａｌ  Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ、視覚的な自己位置推定と環境地図の同時作成）技術とは、ロボットが視覚的システムを利用して自主測位と地図作成を実現することを指し、従来とは異なるナビゲーション技術であり、コンピュータの視覚的技術に基づくロボット測位方法であり、その情報量は多く、適用範囲は広い。ロボットは、リアルタイムに自主測位と地図作成を行わなければナビゲーションタスクを完了できない。
【発明の概要】
【発明が解決しようとする課題】
【０００３】
しかしながら、従来のＶＳＬＡＭ手段について、天井に在る物体は限りがあるため、測位とナナビゲーション精度を保証するための均等に分布するに足りる特徴点が不足している。
【課題を解決するための手段】
【０００４】
本発明は、視覚的測位によるナビゲーション装置およびその方法を提供しており、屋内天井のＯＲＢ（Oriented Fast and Rotated Brief）特徴点を計算・追跡した特徴情報に基づいて、周囲環境の地図を構築することにより、正確な測位ナビゲーション機能を実現する。
【０００５】
本発明は、ロボットを駆動し、前記ロボットの現在の位置・姿勢情報をリアルタイムに取得する動作モジュールと、画像センサーを含み、ロボット移動過程における環境画像を採取するカメラモジュールと、前記環境画像に対して特徴抽出と特徴描写を行う、画像処理モジュールと、複数の特徴点を含む特徴データベースを構築し、前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記ロボットの位置・姿勢修正量を算出し、さらに前記ロボットの現在の位置・姿勢情報と前記位置・姿勢修正量に基づいて、修正後のロボットの位置・姿勢を取得する位置・姿勢推定モジュールと、を備える視覚的測位によるナビゲーション装置を開示する。
【０００６】
本発明は、ロボットを駆動しながら、前記ロボットの現在の位置・姿勢情報をリアルタイムに取得するステップと、ロボット移動過程における環境画像を採取するステップと、前記環境画像に対して特徴抽出と特徴描写を行うステップと、前記環境画像の特徴点描写と前記特徴データベースにおける複数の特徴点とを比較し、前記ロボットの位置・姿勢修正量を算出し、さらに前記ロボットの現在の位置・姿勢情報と前記位置・姿勢修正量に基づいて、修正後のロボットの位置・姿勢を取得するステップと、を含む視覚的測位によるナビゲーション方法をさらに開示する。
【発明の効果】
【０００７】
本発明の視覚的測位によるナビゲーション装置およびその方法は、屋内天井のＯＲＢ特徴点を測定・追跡して取得した特徴情報に基づいてシーンの地図を構築することにより、ロボットの正確な測位およびルート計画を実現する。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施例にかかる視覚的測位によるナビゲーション装置のモジュール図である。
【図２】本発明の実施例にかかる視覚的測位によるナビゲーション装置の詳細なモジュール図である。
【図３】本発明の実施例にかかる視覚的測位によるナビゲーション方法のフローチャートである。
【発明を実施するための形態】
【０００９】
本発明が解決しようとする技術問題、技術手段および有益效果をさらにわかりやすく説明するため、以下に図と実施例を結合して本発明をさらに詳細に陳述するが、次に陳述する実施例は単に本発明のいくつかの実施例であり、その全てではないことは明白である。
【００１０】
図１は、本発明実施例にかかる視覚的測位によるナビゲーション装置１００のモジュール図である。図１に示すように、視覚的測位によるナビゲーション装置１００は、動作モジュール１１１、カメラモジュール１１２、画像処理モジュール１１３、および位置・姿勢推定モジュール１１４を備える。図２は、本発明実施例にかかる視覚的測位によるナビゲーション装置２００の詳細なモジュール図である。
【００１１】
図１と図２を結合すると、動作モジュール１１１は、ロボットを駆動し（駆動制御モジュール２１１２）、前記ロボットの現在の位置・姿勢情報をリアルタイムに取得する（位置・姿勢取得ユニット２１１４）。各モジュールは、ロジック、記憶された指令を有する非一時的コンピュータ可読媒体、ファームウェア、及び/又はそれらの組合せを含むように選択的に実施される。記憶された指令及び/又はファームウェアによる実現されるロジックは、このような指令を実行することにより、従来の視覚的測位によるナビゲーション装置が本発明に係る方法を実行可能となるように、プロセッサを提供することができる。例えば、ジャイロスコープにより、ロボットの現在の偏向角度θを取得でき、ロボットのホイール上の光電パルスカウンターにより、ロボットの移動距離ｄを取得できる。ロボットの初期位置と初期角度を０とした場合に、以下の数式：
Ｒｔｈ２＝Ｒｔｈ１＋θ
Ｒｘ２＝Ｒｘ１＋ｄ＊ｃｏｓ（θ）
Ｒｙ２＝Ｒｙ１＋ｄ＊ｓｉｎ（θ）
により累加して、ロボットの現在の位置・姿勢（Ｒｘ、Ｒｙ、Ｒｔｈ）の推定値を取得できる。
【００１２】
カメラモジュール１１２は、適切であれば任意の画像センサを備えてもよい。例えば、相補型金属酸化物半導体（CMOS）センサ、電荷結合素子（CCD）[AO1]センサ、又は任意の他の適切なセンサがある。カメラモジュール１１２は、視覚的測位によるナビゲーション装置１００（例えば、ロボット）の真上に位置することができ、ロボットの移動過程における環境画像（例えば、天井の写真）を採取する。
【００１３】
画像処理モジュール１１３はカメラモジュール１１２に結合でき、前記環境画像に対して特徴抽出と特徴描写を行う。すなわち、前記環境画像に対して前処理を行うことにより、レンズの歪みが特徴描写に及ぼす影響（画像前処理ユニット２１３２）を取り除くとともに、ＯＲＢ（Oriented Fast and Rotated Brief）特徴点検出方法を用いて前記環境画像における特徴点を抽出し、前記特徴点を多次元描写する（特徴描写ユニット２１３４）。
【００１４】
位置・姿勢推定モジュール１１４は画像処理モジュール１１３に結合でき、複数の特徴点に対する多次元描写に基づいて、前記環境画像の特徴点描写をマッチング（マッチングユニット２１４２）し、特徴データベースを構築[AO2]し（特徴データベース２１４４）、前記ロボットの姿勢修正量を算定し、さらに前記ロボットの現在の姿勢と前記姿勢修正量に基づいて、修正後のロボット姿勢を取得する（姿勢修正ユニット２１４６）。また、位置・姿勢推定モジュール１１４は、地図管理ユニット２１４８をさらに備えてもよい。地図管理ユニット２１４８は修正後のロボット位置・姿勢を取得してロボットによる現在環境での地図構築を実現する。
【００１５】
図３は本発明の実施例にかかる視覚的測位によるナビゲーション方法３００のフローチャートであり、以下のステップ３０８、ステップ３１０、ステップ３１２、ステップ３１４、ステップ３１６およびステップ３０６を含む。
【００１６】
ステップ３０８では、ロボット掃除機のカメラモジュールにより、動作過程において天井を撮影し、天井の異なる視角での画像を取得し、撮影した天井の動画を画像処理モジュールへ送信し、処理する。
【００１７】
ステップ３１０では、画像処理モジュールによる特徴抽出は、主に画像における点特徴を抽出することにより行われ、ＯｐｅｎＣＶが固有のＯＲＢ特徴抽出という方法を用いる。このような方法は、演算速度が速いことを特徴とし、一定の騒音防止と回転防止等の特性を有する。ＯＲＢ特徴抽出の方法により画像を処理した後、一連の特徴点データを取得でき、特徴情報を特徴データベースに保存する。特徴データベースは主にすでに分析した画像フレームに対応するデータを記憶し、各グループのデータには以下の情報を保存している。
（１）当該画像フレームを取得する時のロボットの現在の位置・姿勢
（２）画像の特徴点のインデックス番号
（３）画像の特徴点の画素位置
（４）画像の特徴描写
【００１８】
特徴データベースにおいて、各グループの保存データ同士は一定の規定条件を満たす必要がある。即ち、各グループのデータに記録したロボット位置の間の最小間隔は１０ｃｍより大きくされる必要がある。これにより、ロボットが一定の面積で動作を繰り返す時、データベースのサイズが無制限に増大しないことを保証できる。
【００１９】
ステップ３１２では、リアルタイムのＶＳＬＡＭ測位過程において、取得した現在の画像の特徴点描写情報により、確立したデータベースとマッチングする。特徴点描写情報をマッチングする時、ユークリッド距離を特徴の類似性尺度として用い、閾値を設定し、類似性尺度により２フレーム画像における特徴点のマッチペアを得る。マッチング結果が当該現在の環境画像はすでに特徴データベースに存在していることを示した場合に、次の位置・姿勢修正ステップに進み、当該現在の環境画像が特徴データベースに存在していない場合に、特徴データベースに保存する。
【００２０】
ステップ３１４では、ロボットの位置・姿勢修正を算出する前に、２つの画像におけるマッチング画素の画素距離に表される空間におけるカメラ位置間の距離値を算出する必要がある。ここで、ロボットを円心とし、周囲約１メートルの範囲内で天井の高さが一様であるとみなされることができる。現在は垂直視野角のカメラを使用しているため、カメラの内部パラメータが変化しない場合に、画像の歪みを校正した後、画素の画素距離と空間におけるカメラ位置間の距離の比も固定値であると考えられる。前記の値をパラメータｋとし、特徴データベースでのマッチング点のデータを利用して当該パラメータを算出する。まず特徴データベースにおいて現在のフレームにおける特徴点をマッチングし、マッチング点の有るフレームＦｒ１、Ｆｒ２に対して、データに記録したロボット位置により２グループのデータ間の変位距離ｄ（Ｆｒ１、Ｆｒ２）を算出できる。２グループのデータに記録した特徴点情報を利用して、２フレーム画像間の画素の並進量ｐｄ（Ｆｒ１、Ｆｒ２）を算出する。
【００２１】
画像のマッチング点のアフィン変換を算出する。
【００２２】
ｓｒｃ［ｉ］とｄｓｔ［ｉ］はそれぞれマッチング点における第ｉ点であり、
はを示す。
ＳＶＤを用いてアフィン変換マトリックスを分解し、
ここで、ｃｅｎｔｒｏｉｄＡとｃｅｎｔｒｏｉｄＢを画像中心とする。マッチング点の画像の変位変換ｄｘ、ｄｙと回転変換ｄｔｈを取得する。
【００２３】
このように、各グループのデータについて、いずれもｋ値：ｋ＝ｄ（Ｆｒ１、Ｆｒ２）／ｐｄ（Ｆｒ１、Ｆｒ２）を算出することができ、さらにそれぞれＦｒ１、Ｆｒ２のカメラ位置に対する変位と回転
Ｍｏｖｅｍｅｎｔ１＝ｋ＊ｓｑｒｔ（Ｆｒ１．ｄｘ＊Ｆｒ１．ｄｘ＋Ｆｒ１．ｄｙ＊Ｆｒ１．ｄｙ）
Ｍｏｖｅｍｅｎｔ２＝ｋ＊ｓｑｒｔ（Ｆｒ１．ｄｘ＊Ｆｒ１．ｄｘ＋Ｆｒ１．ｄｙ＊Ｆｒ１．ｄｙ）
Ａｎｇｌｅ１＝ｍｅａｎ（Ｆｒ１．ａｎｇｌｅ、Ｆｒ１．ａｎｇｌｅ＋Ｆｒ１．ｄｔｈ）
Ａｎｇｌｅ２＝ｍｅａｎ（Ｆｒ２．ａｎｇｌｅ、Ｆｒ２．ａｎｇｌｅ＋Ｆｒ２．ｄｔｈ）
を算出する。
【００２４】
このように、各グループデータについて、いずれも２グループのカメラ位置・姿勢の推定値を算出することができる。データベースにおいて、各２つのデータ同士は相互に計算を行い、複数の現在のフレームの位置・姿勢の推定値を取得する。これらの位置・姿勢推定値を選別した後、平均値を算出し、偏差が大きい結果を取り除く。その後、残りの結果をｋ−ｍｅａｎｓクラスティング演算を行い、最後の結果を取得する。
【００２５】
ステップ３１６では、画素の画素距離に表される空間におけるカメラ位置間の距離値ｋを取得した後、データベースにおいて１グループのロボットに近い位置の特徴データＤｎを順次抽出する。ＴＤとＤｎとの間に特徴点のマッチング演算を行い、信頼性の高い特徴点セットを選別する。その後、これらの特徴点セットを用いて、各２フレーム画像間の回転角度θと画素の並進量ｐｄを算出する。同時に、データＤｎに当時のロボットの位置・姿勢Ｄｘ、Ｄｙ、Ｄｔｈを保存する。ステップ３１２において取得したパラメータｋと結合して、１グループの現在のロボットの推定位置・姿勢
Ｒｔｈ＝Ｄｔｈ＋θ  
Ｒｘ＝Ｄｘ＋ｐｄ＊ｋ＊ｃｏｓ（Ｒｔｈ）
Ｒｙ＝Ｄｙ＋ｐｄ＊ｋ＊ｓｉｎ（Ｒｔｈ）
を算出できる。
【００２６】
データベースにおける各グループのデータは、全て現在のロボットが取得した画像と位置・姿勢推定を一度行うことができる。これにより、複数グループのロボットの現在の位置・姿勢に関する推定値を取得できる。ｋ−ｍｅａｎｓクラスタリング手法を用いて、ロボット位置・姿勢の最終推定値を求める（ステップ３１８）。推定値とロボット現在の未修正の位置・姿勢の結果とを減算した値はロボットの位置・姿勢修正量である（ステップ３０４）。
【００２７】
ステップ３０６では、画像の取得にはある程度の遅延が存在する場合があることを考慮すると、出力したデータは位置・姿勢修正量であって、位置・姿勢データそのものではない。このように、たとえ短時間の画像遅延が発生したとしても、位置・姿勢修正量は有効な修正效果を奏する。ステップ３１４において出力した位置・姿勢修正量を取得すると、現在の位置・姿勢と修正量を加算して、修正後の位置・姿勢を取得する。ロボットは、該位置・姿勢を信頼性の高い値として、動作制御とルート計画を行う。
【００２８】
さらに、ステップ３０４とステップ３０６の前に、さらにステップ３０２を含んでも良い。ステップ３０２では、ジャイロスコープにより、ロボットの現在の偏向角度θを取得でき、ロボットのホイール上の光電パルスカウンターにより、ロボットの移動距離ｄを取得できる。ロボットの初期位置と初期角度を０とした場合に、以下の数式：
Ｒｔｈ２＝Ｒｔｈ１＋θ；
Ｒｘ２＝Ｒｘ１＋ｄ＊ｃｏｓ（θ）；
Ｒｙ２＝Ｒｙ１＋ｄ＊ｓｉｎ（θ）
により累加して、ロボットの現在の位置・姿勢（Ｒｘ、Ｒｙ、Ｒｔｈ）の推定値を取得できる。
【００２９】
本発明の視覚的測位によるナビゲーション装置およびその方法は、屋内天井のＯＲＢの特徴点を測定・追跡して取得した特徴情報に基づいてシーンの地図を構築することにより、ロボットの正確な測位およびルート計画を実現することができる。
【００３０】
以上は本発明のいくつかの好ましい実施例そのすべてではない。当該領域の普通の技術者にとって、創造性的労働をしなくてもこれらの実施例に基づいて、ほかの実施例を取得することができ、これは本願の保護を求める範囲から逸脱しない。
【図１】
0006475772.tif 000002
【図２】
0006475772.tif 000003
【図３】
0006475772.tif 000004