JP3945279B2 - 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 - Google Patents
障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 Download PDFInfo
- Publication number
- JP3945279B2 JP3945279B2 JP2002073388A JP2002073388A JP3945279B2 JP 3945279 B2 JP3945279 B2 JP 3945279B2 JP 2002073388 A JP2002073388 A JP 2002073388A JP 2002073388 A JP2002073388 A JP 2002073388A JP 3945279 B2 JP3945279 B2 JP 3945279B2
- Authority
- JP
- Japan
- Prior art keywords
- obstacle
- plane
- unit
- floor surface
- floor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims description 42
- 238000001514 detection method Methods 0.000 claims description 63
- 230000033001 locomotion Effects 0.000 claims description 61
- 238000006243 chemical reaction Methods 0.000 claims description 50
- 238000003384 imaging method Methods 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000009466 transformation Effects 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 12
- 230000010365 information processing Effects 0.000 claims description 2
- 230000009471 action Effects 0.000 description 44
- 230000006399 behavior Effects 0.000 description 36
- 238000004364 calculation method Methods 0.000 description 36
- 238000010586 diagram Methods 0.000 description 36
- 238000012545 processing Methods 0.000 description 35
- 230000008451 emotion Effects 0.000 description 31
- 230000007704 transition Effects 0.000 description 25
- 230000004807 localization Effects 0.000 description 21
- 238000009826 distribution Methods 0.000 description 18
- 238000000605 extraction Methods 0.000 description 16
- 230000007613 environmental effect Effects 0.000 description 14
- 230000015654 memory Effects 0.000 description 14
- 210000004394 hip joint Anatomy 0.000 description 11
- 210000002414 leg Anatomy 0.000 description 11
- 244000145841 kine Species 0.000 description 10
- 210000002683 foot Anatomy 0.000 description 8
- 238000005259 measurement Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 7
- 210000000544 articulatio talocruralis Anatomy 0.000 description 6
- 210000000323 shoulder joint Anatomy 0.000 description 6
- 238000003860 storage Methods 0.000 description 6
- 238000012360 testing method Methods 0.000 description 6
- 210000002310 elbow joint Anatomy 0.000 description 5
- 210000003857 wrist joint Anatomy 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000000546 chi-square test Methods 0.000 description 4
- 238000012790 confirmation Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 239000000284 extract Substances 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 3
- 210000000245 forearm Anatomy 0.000 description 3
- 210000000629 knee joint Anatomy 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 210000001364 upper extremity Anatomy 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 230000036528 appetite Effects 0.000 description 2
- 235000019789 appetite Nutrition 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 210000003141 lower extremity Anatomy 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000010606 normalization Methods 0.000 description 2
- 239000004065 semiconductor Substances 0.000 description 2
- 230000035945 sensitivity Effects 0.000 description 2
- 230000006403 short-term memory Effects 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 101150106709 ARC1 gene Proteins 0.000 description 1
- 101100013575 Arabidopsis thaliana FTSHI1 gene Proteins 0.000 description 1
- 238000012935 Averaging Methods 0.000 description 1
- 241000282412 Homo Species 0.000 description 1
- 206010039740 Screaming Diseases 0.000 description 1
- 230000004913 activation Effects 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000000994 depressogenic effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000002996 emotional effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 210000003414 extremity Anatomy 0.000 description 1
- 230000012447 hatching Effects 0.000 description 1
- 210000001624 hip Anatomy 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 210000003127 knee Anatomy 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000000704 physical effect Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 210000000689 upper leg Anatomy 0.000 description 1
- 210000000707 wrist Anatomy 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/97—Determining parameters from multiple pictures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
Landscapes
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Manipulator (AREA)
- Image Processing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Measurement Of Optical Distance (AREA)
- Closed-Circuit Television Systems (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は障害物認識装置及び方法に関し、特に移動型ロボット装置に適用されて床面上の障害物を認識する障害物認識装置、障害物認識方法及び障害物認識プログラム、並びに移動型ロボット装置に関する。
【０００２】
【従来の技術】
自律型ロボットにおいて、自身の周りの環境を認識して経路計画を立ててそれに応じて移動する能力は言うまでもなく重要である。従来の移動ロボットと言えば車輪型のロボットの周囲に超音波センサーを床に平行に配置して、壁などの反射を測定する方法が取られてきた。これは床に平行に測定することで床自身を測定する事は無く、反射した点は全て障害物として取り扱うことが出来るために処理を単純化することができる。しかしながらこうした方法は、特定の高さの障害物しか測定しないために小さな段差や穴などの凹んだ障害物を検出できない。また歩行型ロボット（４足歩行ロボット、２足歩行ロボットなど）ではこうした測距センサーは、頭や手先など能動的に姿勢を変えることができる位置に取り付けられることが多い。
【０００３】
また、広く知られるようになった４足歩行ロボット（エンターテインメントロボット）では、頭部についた測距センサを使い、ロボットの姿勢から床面パラメータを導出して、測定点が床面に属するか障害物として扱うかを判断している。
【０００４】
上記方法でも測定点は1点（もしくは姿勢を変えながら繰り返し測定することで数点）しかなく、ロボットの周囲の状況を把握したとは言い難い。また1点しか測定していないために測定に対する信頼度が著しく低い。
【０００５】
【発明が解決しようとする課題】
本発明は、前記実情に鑑みてなされたものであり、正確に床面を抽出し、障害物を認識することができる障害物認識装置、障害物認識方法及び障害物認識プログラムの提供を目的とする。
【０００６】
また、本発明は、前記障害物認識装置を適用して床面上の障害物を認識しながら移動することができる移動型ロボット装置の提供を目的とする。
【０００７】
【課題を解決するための手段】
本発明に係る障害物認識装置は、前記課題を解決するために、複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成手段と、前記距離画像生成手段により生成された距離画像に基づいて平面パラメータを検出する平面検出手段と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換手段と、前記センサ手段からのセンサデータ出力に基づき前記座標変換手段による座標変換を行って得られた接地平面パラメータと、前記平面検出手段によって検出された平面パラメータとに基づいて床面を検出する床面検出手段と、前記床面検出手段の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識手段とを備える。
【０００８】
本発明に係る障害物認識方法は、前記課題を解決するために、複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成工程と、前記距離画像生成工程により生成された距離画像に基づいて平面パラメータを検出する平面検出工程と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換工程と、前記センサ手段からのセンサデータ出力に基づき前記座標変換工程による座標変換を行って得られた接地平面パラメータと、前記平面検出工程によって検出された平面パラメータとに基づいて床面を検出する床面検出工程と、前記床面検出工程の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識工程とを備える。
【０００９】
本発明に係る障害物認識プログラムは、前記課題を解決するために、障害物を認識する障害物認識プログラムにおいて、複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成工程と、前記距離画像生成工程により生成された距離画像に基づいて平面パラメータを検出する平面検出工程と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換工程と、前記センサ手段からのセンサデータ出力に基づき前記座標変換工程による座標変換を行って得られた接地平面パラメータと、前記平面検出工程によって検出された平面パラメータとに基づいて床面を検出する床面検出工程と、前記床面検出工程の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識工程とを備えてなる。
【００１０】
本発明に係る移動型ロボット装置は、前記課題を解決するために、頭部ユニットと、少なくとも１以上の可動脚ユニットと、情報処理手段を有する胴体ユニットとを備えてなり、障害物の床面を認識しながら前記可動脚ユニットを用いて床面を移動する移動型ロボット装置において、前記頭部ユニットに設けた複数の撮像手段から得られた画像データに基づいて算出された視差画像と、前記頭部ユニット、前記可動脚ユニット、前記胴体ユニットに設けた複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成手段と、前記距離画像生成手段により生成された距離画像に基づいて平面パラメータを検出する平面検出手段と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換手段と、ロボット装置の前記センサ手段からのセンサデータ出力に基づき前記座標変換手段による座標変換を行って得られた接地平面パラメータと、前記平面検出手段によって検出された平面パラメータとに基づいて床面を検出する床面検出手段と、前記床面検出手段の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識手段とを前記胴体ユニットに備えてなる。
【００１１】
本発明では、複数の撮像手段として例えばステレオカメラなどを用いた２次元の測距センサを導入している。これにより画像認識による平面の検出とロボットの姿勢との関係からよりロバストにより正確に床面を抽出することができるようになっている。
【００１２】
【発明の実施の形態】
以下、図面を参照しながら本発明の実施の形態について説明する。この実施の形態は、本発明に係る障害物認識装置を２足歩行のロボット装置に適用したものである。
【００１３】
図１は、２足歩行のロボット装置の概略を示すブロック図である。図１に示すように、ロボット装置１の頭部ユニット２５０には、２台のＣＣＤカメラ２００Ｒ，２００Ｌが設けられ、このＣＣＤカメラ２００Ｒ，２００Ｌの後段には、ステレオ画像処理装置２１０が設けられている。２台のＣＣＤカメラ（以下、右目２００Ｒ、左目２００Ｌという。）により撮像された右目画像２０１Ｒ、左目画像２０１Ｌは、ステレオ画像処理装置２１０に入力される。ステレオ画像処理装置２１０は、各画像２０１Ｒ，２０１Ｌの視差情報（disparity data）（距離情報）を計算し、カラー画像（ＹＵＶ：輝度Ｙ、ＵＶ色差）２０２及び視差画像（ＹＤＲ：輝度Ｙ、視差Ｄ、信頼度Ｒ）２０３をフレーム毎に左右交互に算出する。ここで、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
【００１４】
このカラー画像２０２及び視差画像２０３はロボット装置１の体幹部２６０に内蔵されたＣＰＵ（制御部）２２０に入力される。また、ロボット装置１の各関節にはアクチュエータ２３０が設けられており、ＣＰＵ２２０からの指令となる制御信号２３１が供給されて、その指令値に応じてモータを駆動する。各関節（アクチュエータ）には、ポテンショメータが取り付けられ、その時のモータの回転角がＣＰＵに送られる。このアクチュエータに取り付けられたポテンショメータ、足底に取り付けられたタッチセンサ及び体幹部に取り付けられたジャイロ・センサ等の各センサ２４０は、現在の関節角度、設置情報、及び姿勢情報等の現在のロボット装置の状態を計測し、センサデータ２４１としてＣＰＵ２２０へ出力する。ＣＰＵ２２０は、ステレオ画像処理装置２１０からのカラー画像２０２及び視差画像２０３と、アクチュエータの全ての関節角度等のセンサデータ２４１とが入力され、後述するソフトウェア構成を実現する。
【００１５】
本実施の形態のソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うものである。なお、ロボット装置の位置を示す座標として、例えば、後述するランドマーク等の特定の物体等を座標の原点としたワールド基準系のカメラ座標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心（座標の原点）としたロボット中心座標系（以下、相対座標ともいう。）との２つの座標を使用する。
【００１６】
図２は、２足歩行のロボット装置のソフトウェアの構成を示す模式図である。同図において、丸で表されているのが、オブジェクト又はプロセスと呼ばれるエンティティである。オブジェクト同士が非同期に通信し合うことで、システム全体が動作する。各オブジェクトはメッセージ通信と共有メモリを使用したオブジェクト間通信方法によりデータの受け渡し及びＩｎｖｏｋｅを行っている。
【００１７】
図２に示すように、ソフトウェア３００は、キネマティックオドメトリ（Kinematics Odometry）ＫＩＮＥ３１０、平面抽出部（Plane Extractor）ＰＬＥＸ３２０、障害物グリッド算出部（Occupancy Grid）ＯＧ３３０、ランドマーク位置検出部（Landmark Sensor）ＣＬＳ３４０、絶対座標算出部（Localization）ＬＺ３５０及び行動決定部（Situated behavior Layer）ＳＢＬ３６０から構成され、各オブジェクト単位にて処理がなされる。これらのソフトウェアの詳細な構成及び動作については後述する。また、ロボット装置についての詳細な構成及び動作についても後述する。
【００１８】
ここで、先ず、上述したロボット装置１に搭載される本発明の障害物認識装置について説明する。障害物認識装置は、前記平面抽出部（Plane Extractor）ＰＬＥＸ３２０を実行したＣＰＵ２２０内にて構成される。図３には障害物認識装置２２１の機能ブロック図を示す。障害物認識装置２２１は、後述する視差画像から距離画像を生成する距離画像生成部２２２と、距離画像から平面検出をして平面パラメータを算出する平面検出部２２３と、後述する同時変換行列を座標変換する座標変換部２２４と、前記座標変換結果と前記平面パラメータからから床面を検出する床面検出部２２５と、床面の平面パラメータから障害物を認識する障害物認識部２２６とを備えてなる。
【００１９】
距離画像生成部２２２は、ロボット装置１に設けた二つのＣＣＤカメラから得られた画像データに基づいて算出された視差画像と、ロボット装置１に設けた複数のセンサ手段から得られたセンサデータ出力とに基づいて二つのＣＣＤカメラが配設された位置における前記視差画像に対応する同時変換行列を用いて距離画像を生成する。平面検出部２２３は、距離画像生成部２２２により生成された距離画像に基づいて平面パラメータを検出する。座標変換部２２４は、前記同時変換行列をロボット装置１の接地面での座標に変換する。床面検出部２２５は、平面検出部２２３からの平面パラメータと座標変換部２２４からの座標変換結果を用いて床面を検出し、その平面パラメータを障害物認識部２２６に供給する。障害物認識部２２６は、床面検出部２２５が検出した床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する。
【００２０】
上述した如く、ＣＣＤカメラ２００Ｌ，２００Ｒによって取り込まれた映像は、ステレオ画像処理装置２１０に入力され、図４に具体的に示す左右画像２０１Ｒ，２０１Ｌの視差情報（距離情報）からカラー画像（ＹＵＶ）２０２及び視差画像（ＹＤＲ）２０３が算出されてＣＰＵ２２０に入力される。また、ロボット装置１に複数個設けられた各センサからのセンサデータ２４０が入力される。図２に示すように、これらの視差情報及び視差画像からなる画像データ３０１と、ロボット装置の関節角等のデータであるセンサデータ３０２とがキネマティックオドメトリＫＩＮＥ３１０に入力される。
【００２１】
キネマティックオドメトリＫＩＮＥ３１０は、画像データ３０１及びセンサデータ３０２からなる入力データに基づいて画像データ３０１の画像が撮像された時間におけるセンサデータ３０２の関節角を割り出し、この関節角のデータを使用してロボット装置１が中心に固定されたロボット中心座標系を頭部ユニットに設けられたカメラの座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同時変換行列等を導出し、この同時変換行列３１１とこれに対応する視差画像３１２を障害物認識装置２２１（平面抽出部ＰＬＥＸ３２０の実行結果）へ出力する。
【００２２】
障害物認識装置２２１（平面抽出部ＰＬＥＸ３２０）は、同時変換行列３１１とこれに対応する視差画像３１２を受け取り、以下の図５に示す処理手順にしたがって障害物を認識する。
【００２３】
先ず、障害物認識装置２２１（平面抽出部ＰＬＥＸ３２０）の座標変換部２２４は同時変換行列３１１を、また距離画像生成部２２２は同時変換行列３１１に対応する視差画像３１２を受け取る（ステップＳ６１）。そして、距離画像生成部２２２は、視差画像３１２からレンズ歪みとステレオの取り付け誤差を吸収したキャリブレーションパラメータを利用して、画素毎にカメラ座標から見た３次元の位置データ（X,Y,Z）を距離画像として生成する（ステップＳ６２）。このとき各３次元データは、視差画像又は距離画像等の入力画像における信頼度等から得られる信頼度パラメータを個別に有し、この信頼度パラメータにより選別されて入力される。
【００２４】
そして、平面検出部２２３は、選別された３次元データ群からランダムにデータをサンプリングし、ハフ変換により平面を推定する。即ち、法線ベクトルの向きを（θ，φ）、原点からの距離をｄとしたときの平面パラメータ（θ，φ，ｄ）を算出し、平面パラメータを投票空間（θ，Ψ，ｄ）＝（θ，φｃｏｓθ，ｄ）に直接投票して平面を推定する。これにより、平面検出部２２３は、画像中の支配的な平面のパラメータを検出する（ステップＳ６３）。この平面のパラメータは図５に示すパラメータ空間（θ，φ）（投票空間）のヒストグラムにより検出される。パラメータの投票が小さいいと障害物、大きいと平面上のものと見なすことができる。
【００２５】
投票の際は、３次元データに付随する信頼度パラメータ又は平面パラメータの算出方法等により、１票の投票に対して異なる重み付けをして、投票値を異ならせると共に、投票値の分布から得られるピーク値を推定するために、ピーク値近傍における重み付き平均処理等をすることにより、信頼度の高いデータを推定することができる。そして、この平面パラメータを初期パラメータとしてイタレーションを行い、平面を決定することで、更に信頼度が高い平面を決定することができる。また、最終的に決定される平面を算出した３次元データに付随する信頼度パラメータ及びイタレーションにおける残差誤差等を使用して平面の信頼度を算出し、この平面信頼度を平面のデータと共に出力することにより、後段の処理が行い易くなる。このように、平面抽出は、投票、即ちヒストグラムに基づく確率密度関数の推定により３次元データから、その３次元データに含まれる支配的な平面のパラメータを決定する確率論的方法で行われる。こうして得られた平面のパラメータ使用すれば、元々画像から得られた距離の測定点が平面上からどれくらいの距離であるかが分かる。
【００２６】
次に、座標変換変換部２２４は、カメラ座標系の同時変換行列３１１から図７に示すようにロボットの足底接地面への変換を求める（ステップＳ６４）。これにより、カメラ座標系で表現される接地平面のパラメータの算出が行われる。そして、床面検出部２２５は、平面検出部２２３による前記ステップＳ６３での画像による平面の検出結果と、座標変換部２２４による前記ステップＳ６４での足底接地平面の照合から、画像中の平面パラメータから床面に相当するものを選択する（ステップＳ６５）。
【００２７】
次に、障害物認識部２２６は、床面検出部２２５が前記ステップＳ６５にて選択した平面パラメータを使って、元の距離画像から平面上に載っている点を選択する（ステップＳ６６）。これは以下に示す式１、式２を用いて、平面からの距離ｄがしきい値Ｄｔｈより小さい事を使って判断する。
【００２８】
【数１】
【００２９】
【数２】
【００３０】
図９には、しきい値Ｄｔｈを１cmとした範囲で選択された測定点（×印）を示す。この図９にて、黒く抜かれている点が平面と判断されなかった点である。
【００３１】
したがって、障害物認識部２２６は、前記ステップＳ６６にて平面上（床面）上に載っている点以外の点（床面上に無い点）を障害物としてステップＳ６７にて認識することができる。これらの判断結果は床平面上の点（ｘ、ｙ）とその高さｚで表現される。例えば、高さｚ＜０は、平面より凹んでいる点を示す。
【００３２】
これにより、高さがロボットより高い障害物点はそれをくぐることができるので障害物ではないという判断も可能とすることができる。
【００３３】
また、ロボットビュー（図９の（ａ））から得た床面抽出画像（図９の（ｂ））の高さｚを０（ｚ＝０）となるように座標変換すれば、図９の（ｃ）に示すように、平面上の２次元の位置で床か障害物かを表現することができる。
【００３４】
このように、障害物認識装置は、多数の測定点を使って平面の検出を行うために安定した平面を抽出できる。また、画像から得られる平面候補をロボットの姿勢から得られる床面パラメータと照合することで正しい平面を選択できる。また、障害物を認識するのではなく、実質的には床面を認識するので障害物の形状や大きさに因らない認識が可能である。また、床面からの距離で障害物を表現するので細かい段差や凹みまで検出することができる。またロボットの大きさを考慮して跨いだり、くぐったりする判断も容易である。さらに、2次元床面上の障害物表現となるので経路計画などに既存の移動ロボットで用いられる手法が適用可能であるし、3次元の表現より高速に演算できる。
【００３５】
次に、上述した図２に示すロボット装置１のソフトウェアについて詳細に説明する。図１０は、図２に示すソフトウェア３００の動作を示すフローチャートである。
【００３６】
図２に示すソフトウェア３００のキネマティックオドメトリＫＩＮＥ３１０には、上述したように、画像データ３０１及びセンサデータ３０２が入力される。この画像データ３０１は、カラー画像及びステレオカメラによる視差画像である。また、センサデータは、ロボット装置の関節角等のデータである。キネマティックオドメトリＫＩＮＥ３１０は、これらの入力データ３０１，３０２を受け取り、メモリに格納されていたそれまでの画像及びセンサデータを更新する（ステップＳ１０１）。
【００３７】
次に、これら画像データ３０１とセンサデータ３０２との時間的な対応を取る（ステップＳ１０２−１）。即ち、画像データ３０１の画像が撮像された時間におけるセンサデータ３０２の関節角を割り出す。次いで、この関節角のデータを使用してロボット装置１が中心に固定されたロボット中心座標系を頭部ユニットに設けられたカメラの座標系へ変換する（ステップＳ１０２−２）。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同時変換行列等を導出し、この同時変換行列３１１とこれに対応する画像データとを画像認識を行うオブジェクトへ送信する。即ち、同時変換行列３１１及びこれに対応する視差画像３１２を平面抽出部ＰＬＥＸ３２０へ出力し、同時変換行列３１１及びカラー画像３１３をランドマークセンサ部ＣＬＳ３４０へ出力する。
【００３８】
また、センサデータ３０２から得られる歩行パラメータと、足底センサを使用した歩数のカウント数とからロボット装置１の移動量を算出し、ロボット装置１のロボット装置中心座標系における移動量を算出する。以下、ロボット装置中心座標系の移動量をオドメトリともいう。このオドメトリ３１４を障害物グリッド算出部ＯＧ３３０及び絶対座標算出部ＬＺ３５０へ出力する。
【００３９】
平面抽出部ＰＬＥＸ３２０は、キネマティックオドメトリＫＩＮＥ３１０にて算出された同時変換行列３１１と、これに対応するステレオカメラから得られる視差画像３１２とが入力されると、それまでメモリに格納されていたこれらのデータを更新する（ステップＳ１０３）。そして、ステレオカメラのキャリブレーションパラメータ等を使用して視差画像３１２から３次元の位置データ（レンジデータ）を算出する（ステップＳ１０４−１）。次いで、このレンジデータからハフ変換等を使用して、例えば壁及びテーブル等の平面を除く平面を平面として抽出する。また、座標変換行列３１１からロボット装置１の足底が接地している平面との対応を取り、床面を選択し、床面上にない点、例えば所定の閾値よりも高い位置にあるもの等を障害物として床面からの距離を算出し、この障害物情報（obstacle）３２１を障害物グリッド算出部３３０に出力する（ステップS１０４−２）。
【００４０】
障害物グリッド算出部ＯＧ３３０では、キネマティックオドメトリＫＩＮＥ３１０にて算出されたオドメトリ３１４と、平面抽出部ＰＬＥＸ３２０にて算出された障害物の観測情報（障害物情報）３２１とが入力されると、メモリに格納されていたそれまでのデータを更新する（ステップＳ１０５）。そして、床面上に障害物があるか否かの確率を保持する障害物グリッドを確率的手法により更新する（ステップＳ１０６）。
【００４１】
この障害物グリッド算出部ＯＧ３３０は、ロボット装置１を中心とした例えば周囲４ｍの障害物情報、即ち、上述した環境地図と、ロボット装置１の向く方角を示す姿勢情報とを保持しており、上述の方法により、環境地図を更新し、この更新した認識結果（障害物情報３３１）を出力することにより、上位レイヤ、即ち、本実施の形態においては、経路計画決定部ＳＢＬ３６０にて障害物を回避する計画を作成することができる。
【００４２】
ランドマークセンサＣＬＳ３４０は、キネマティックオドメトリＫＩＮＥ３１０から同時変換行列３１１及びカラー画像３１３が入力されると、予めメモリ内に格納されていたこれらのデータを更新する（ステップＳ１０７）。そして、カラー画像３１３の画像処理を行って、予め認識しているカラーランドマークを検出する。このカラーランドマークのカラー画像３１３上での位置及び大きさをカメラ座標系での位置に変換する。更に、同時変換行列３１１を使用し、カラーランドマークのカメラ座標系での位置をロボット中心位置座標系における位置に変換し、ロボット中心位置座標系におけるカラーランドマーク位置の情報（カラーランドマーク相対位置情報）３４１を絶対座標算出部ＬＺ３５０に出力する（ステップＳ１０８）。
【００４３】
絶対座標算出部ＬＺ３５０は、キネマティックオドメトリＫＩＮＥ３１０からのオドメトリ３１４と、ランドマークセンサ部ＣＬＳ３４０からのカラーランドマーク相対位置情報３４１とが入力されると、予めメモリ内に格納されていたこれらのデータが更新される（ステップＳ１０９）。そして、絶対座標算出部ＬＺ３５０が予め認識しているカラーランドマークの絶対座標（ワールド座標系での位置）、カラーランドマーク相対位置情報３４１、及びオドメトリ３１４を使用し、確率的手法によりロボット装置の絶対座標（ワールド座標系での位置）を算出する。そして、この絶対座標位置３５１を経路計画決定部ＳＢＬ３６０に出力する。
【００４４】
経路計画決定部ＳＢＬ３６０は、障害物グリッド算出部ＯＧ３３０から障害物グリッド情報３３１が入力され、絶対座標算出部ＬＺ３５０から絶対座標位置３５１が入力されると、予めメモリに格納されていたこれらのデータが更新される（ステップＳ１１１）。そして、経路計画決定部ＳＢＬ３６０障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により、ロボット装置１の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部ＬＺ３５０から現在のロボット装置１の絶対座標を取得することにより、絶対座標系又はロボット装置中心のロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。即ち、入力データから状況に応じてロボット装置１が取るべき行動を決定し、その行動列を出力する（ステップＳ１１２）。
【００４５】
また、人間によるナビゲーションの場合には、障害物グリッド算出部ＯＧ３３０からロボット装置の周囲に存在する障害物に関する認識結果及び絶対座標算出部ＬＺ３５０からの現在のロボット装置の位置の絶対座標をユーザに提供し、このユーザからの入力に応じて動作コマンドを発行させる。
【００４６】
図１１は、上述のソフトウェアに入力されるデータの流れを模式的に示す図である。なお、図１１において、図１及び図２に示す構成と同一構成要素には同一符号を付してその詳細な説明は省略する。
【００４７】
顔検出部ＦＤＴ（ＦａｃｅＤｅｔｅｃｔｏｒ）３７１は、画像フレーム中から顔領域を検出するオブジェクトであり、カメラ等の画像入力装置からカラー画像２０２を受け取り、それを例えば９段階のスケール画像に縮小変換する。この全ての画像の中から顔に相当する矩形領域を探索する。重なりあった候補領域を削減して最終的に顔と判断された領域に関する位置、大きさ、及び特徴量等の情報３７２を出力して、顔識別部ＦＩ（ＦａｃｅＩｄｅｎｔｉｆｙ）３７７へ送る。
【００４８】
顔識別部ＦＩ３７７は、検出された顔画像を識別するオブジェクトであり、顔の領域を示す矩形領域画像からなる情報３７２を顔検出部ＦＤＴ３７１から受け取り、この顔画像がメモリに格納されている手持ちの人物辞書のうちでどの人物に相当するかを比較して人物の識別を行う。そして、顔検出部ＥＤＴ３７１から受け取った顔画像の顔画像領域の位置、大きさ情報と共に人物のＩＤ情報３７８をＤＩＬ３７９に出力する。
【００４９】
色認識部ＭＣＴ（ＭｕｌｔｉＣｏｌｏｒＴｒａｃｋｅｒ）３７３は、色認識を行うオブジェクトであり、カメラ等の画像入力装置からカラー画像２０２を受け取り、予め保有している複数のカラー・モデル情報に基づいて色領域を抽出し、連続した領域に分割する。色認識部ＭＣＴ３７３は、分割した各領域の位置、大きさ及び特徴量等の情報３７４をＤＩＬ３７９に出力する。
【００５０】
動き検出部ＭＤＴ（ＭｏｔｉｏｎＤｅｔｅｃｔｏｒ）３７５は、画像の中で動いている部分を検出する部分であり、検出した動き領域の情報３７６を距離情報付加部ＤＩＬ３７９に出力する。
【００５１】
距離情報付加部ＤＩＬ（ＤｉｓｔａｎｃｅＩｎｆｏｒｍａｔｉｏｎＬｉｎｋｅｒ）３７９は、入力された２次元の情報に距離情報を付加して３次元の情報を出力するオブジェクトであり、顔検出部ＦＩ３７７からのＩＤ情報３７８、色認識部ＭＣＴ３７３からの分割した各領域の位置、大きさ及び特徴量等の情報３７４、動き検出部ＭＤＴ３７５からの動き領域の情報３７６に距離情報を付加して３次元の情報３８０を短期記憶部ＳＴＭ（ＳｈｏｒｔＴｅｒｍＭｅｍｏｒｙ）３８１に出力する。
【００５２】
短期記憶部ＳＴＭ３８１は、ロボット装置１の外部環境に関する情報を比較的短い時間だけ保持するオブジェクトであり、ＡｒｔｈｕｒＤｅｃｏｄｅｒ（図示せず）から音声認識結果（単語、音源方向、確信度）を受け取り、色認識部ＭＣＴ３７３から肌色の領域の位置、大きさと顔領域の位置、大きさを受け取り、顔識別部ＦＩ３７７から人物のＩＤ情報等を受け取る。また、ロボット装置１の機体上の各センサからロボット装置の首の方向（関節角）を受け取る。そして、これらの認識結果及びセンサ出力を統合的に使って、現在どこにどの人物がいて、しゃべった言葉がどの人物のものであり、その人物とはこれまでにどんな対話を行ったのかという情報を保存する。こうした物体、即ちターゲットに関する物理情報と時間方向でみたイベント（履歴）を出力として、経路計画決定部（状況依存行動階層）（ＳＢＬ）３６０等の上位モジュールに渡す。
【００５３】
経路計画決定部ＳＢＬは、上述の短期記憶部ＳＴＭ３８１からの情報を基にロボット装置１の行動（状況に依存した行動）を決定するオブジェクトである。複数の行動を同時に評価したり、実行したりすることができる。また、行動を切り替えて機体をスリープ状態にしておき、別の行動を起動することができる。
【００５４】
次に、既に詳細に説明した平面抽出部ＰＬＥＸ３２０の障害物検出処理を除いた各オブジェクトについて更に詳細に説明する。
【００５５】
障害物グリッド算出部ＯＧ３３０は、所定の大きさのグリッドに分割された、ロボット中心座標系の地図情報である環境地図と、例えば環境地図上においてｘ軸方向又はｙ軸方向等の所定の方角からロボット装置が向く方角を示す姿勢情報とを有している。また、環境地図は、障害物情報が平面抽出部ＰＬＥＸから入力され、障害物として認識されているグリッド（障害物占有領域）を有する。この障害物グリッド算出部ＯＧ３３０は、ロボット装置が移動すると、即ち、オドメトリ３１４が入力されると、予めメモリ内に格納されていた環境地図及び姿勢情報を、ロボット装置の姿勢の変化（差分移動角）及び移動距離（差分移動量）に伴って、予め認識している環境地図及び姿勢方向の情報を更新する。ここで、差分移動量がグリッドサイズより小さければ環境地図を更新せず、移動量がグリッドのサイズを超える場合は環境地図を更新する。また、環境地図及びそのグリッドの大きさを必要に応じて適宜変更することにより、計算量及びメモリコピーコスト等を低減することができる。
【００５６】
以下、障害物グリッド算出部ＯＧ３３０について図１２〜図１７を参照して詳細に説明する。
図１２は、障害物グリッド算出部ＯＧ３３０にて認識される環境地図を示す模式図である。図１２に示すように、環境地図４００は、ロボット装置（移動体）４０３を中心とした周囲の地図情報である環境地図（ロボット中心座標系での地図情報）であり、所定の大きさのグリッド４１０に分割されている。そして、ロボット装置４０３が移動すると、常にロボット装置４０３が環境地図４００の中心に位置する中心グリッド４０１内に位置するように更新されるものである。この図１２に示す環境地図４００及びグリッド４１０の大きさは、必要に応じて適宜変更することができる。即ち、例えば、精細な環境地図が不要であるとき等は、環境地図４００を小さくするか、又はグリッド４１０の大きさを大きくして解像度を下げることにより、計算量及びメモリコピーコスト等を小さくすることができる。
【００５７】
また、図１２において、ハッチングして示すグリッド４０２は、障害物として認識される最小単位の領域（障害物占有領域）を示す。位置認識装置における障害物の認識方法としては、例えば、ステレオカメラ等による視差画像から求める方法、レンジファインダー等の電波の跳ね返りを利用する方法、及び超音波センサ等の距離測定センサによる方法等がある。そして、これらの方法により認識された障害物の位置を位置認識装置が環境地図のグリッドの属性として認識（表示）する。本実施の形態においては、障害物情報は、前述した平面抽出部ＰＬＥＸ３２０から入力される。
【００５８】
障害物グリッド算出部ＯＧ３３０は、上述したように、データ画像及びセンサデータが入力されるキネマティックオドメトリＫＩＮＥ３１０からオドメトリ３１４が入力されると、予めメモリ内に格納されていた環境地図を更新する。メモリ内には、ロボット装置４０３を中心としたロボット装置の周囲の障害物等の情報を有する地図情報である環境地図４００と共に、この地図上においてのロボット装置（移動体）の向きを示す姿勢情報４０４とを有するものである。姿勢情報４０４は、所定の方角、例えばｘ軸方向又はｙ軸方向からロボット装置４０３が向く方角を示すものである。そして、ロボット装置の姿勢の変化及び移動等に伴って、図１２に示す予め認識している環境地図及び姿勢方向の情報を更新する。
【００５９】
先ず、環境地図の更新方法について説明する。本発明の位置認識装置においては、ロボット装置等の移動量の大きさによっては環境地図を更新しない。即ち、移動量が所定の領域（グリッド）内であれば環境地図を更新せず、移動量がグリッドのサイズを超える場合は環境地図を更新するものである。
【００６０】
図１３（ａ），（ｂ）及び（ｃ），（ｄ）は移動量に応じた環境地図の更新方法を示す図であって、夫々環境地図を更新しない場合及び環境地図を更新する場合の環境地図を示す模式図である。図１３（ａ）に示すように、環境地図５００における中心グリッド５０１内にロボット装置５０３が位置している。ハッチングして示すグリッド５０２は障害物領域を示す。このロボット装置５０３の移動量５０５が小さく、図１３（ｂ）に示すように、ロボット装置５０３の位置が移動後のロボット装置５０６においても、そのロボット５０６の位置が中心グリッド５０１内であった場合、環境地図５００は更新しない。この場合は、中心グリッド５０１内におけるロボット装置の移動位置情報だけを更新する。
【００６１】
一方、図１３（ａ）と同様に、図１３（ｃ）に示すように、環境地図５００における中心グリッド５０１内に位置したロボット装置５０３の移動量５１５が大きく中心グリッド５０１を越えた場合、図１３（ｄ）に示すように、移動後のロボット装置５１３が環境地図の中心グリッド５１１に位置するように、移動したグリッドの数だけ環境地図５００をシフトさせて更新して更新環境地図５１０を得る。
【００６２】
即ち、障害物領域として認識される大きさの最小単位であるグリッドの大きさをＣＳ（Cell Size）、グリッド内のロボット装置の位置を（Ｂｘ，Ｂｙ）、ロボット装置の２次元方向の移動量を（ｄｘ，ｄｙ）とすると、グリッドのシフトの大きさ（Ｓｘ，Ｓｙ）は下記数式（３）で示される。
【００６３】
【数３】
【００６４】
ここで、上記数式（３）において、［］は、ガウス記号であって、［］内の値を超えない最大の整数値を示すものとする。また、グリッド内のロボット位置（Ｒｘ，Ｒｙ）は、下記数式（４）で示される。
【００６５】
【数４】
【００６６】
上記数式（３）及び数式（４）の値を図１４に示す。図１４（ａ）に示すように、ロボット装置６０３は、グリッドサイズＣＳを有する複数個のグリッドに分割された環境地図６００における中心グリッド６０１内に位置している。環境地図６００内にはハッチングして示す障害物領域を示す複数個のグリッド６０２を有する。ここで、中心グリッド６０１内の例えば図中左下隅の点６０７をグリッド６０１内のロボット装置６０３の位置原点とすることができる。このロボット装置６０３の位置は、原点６０７からの距離として位置（Ｂｘ，Ｂｙ）として表すことができる。このロボット装置６０３が移動量（ｄｘ，ｄｙ）６０５の移動をしてグリッド６０８に移動した場合、移動後のロボット装置６１３の位置は、（Ｂｘ＋ｄｘ，Ｂｙ＋ｄｙ）として表すことができる。
【００６７】
ロボット装置６０３が移動量（ｄｘ，ｄｙ）６０５の移動をした場合、環境地図シフト量（Ｓｘ，Ｓｙ）は、上記数式（１）から算出することができる。ここで、更新環境地図６１０において、環境地図６００をｘ軸方向のシフト量Ｓｘ及びｙ軸方向のシフト量Ｓｙだけ、夫々ｘ軸方向の移動量ｄｘ及びｙ軸方向の移動量ｄｙとは逆方向に環境地図をシフトさせたものとして更新することができる。なお、ここでは、ロボット装置の移動量（ｄｘ，ｄｙ）は右向きを正とし、環境地図のシフト量（Ｓｘ，Ｓｙ）は、移動方向とは逆方向である左向きを正としている。
【００６８】
こうして移動後のロボット装置６１３は、更新環境地図６１０の中心グリッド６１１に位置する。ここで、移動後のロボット装置６１３の環境地図シフト後の更新環境地図６１０の中心グリッド６１１におけるロボット位置（Ａｘ，Ａｙ）は、移動前のロボット装置６０３のシフト前環境地図６００の中心グリッド６０１におけるロボット位置（Ｂｘ，Ｂｙ）と、グリッド内移動量（Ｒｘ，Ｒｙ）６１５とからロボット位置（Ａｘ，Ａｙ）＝（Ｂｘ＋Ｒｘ，Ｂｙ＋Ｒｙ）＝（Ｂｘ＋ｄｘ−ＣＳ×Ｓｘ，Ｂｙ＋ｄｙ−ＣＳ×Ｓｙ）として算出することができる。
【００６９】
次に、ロボット装置の姿勢方向の情報の更新について説明する。図１５（ａ）及び（ｂ）は、姿勢方向の夫々更新前後の様子を示す模式図である。ロボット装置の姿勢情報に関しては、環境地図を固定した座標系に対する角度として示すことができる、即ち、ロボット装置が姿勢方向ｄαだけ変化させた場合、ロボット装置の姿勢方向の情報のみが更新され、環境地図は更新されない。
【００７０】
即ち、図１５（ａ）に示すように、障害物領域である複数個のグリッド７０２を有する環境地図７００における中心グリッド７０１に位置するロボット装置７０３において、移動前のロボット姿勢方向をαとする。姿勢方向αは、例えば時計回りを正とし、ｙ軸からの傾き又はｘ軸からの傾き等として示すことができる。ここで、ロボット装置７０３が姿勢回転角度ｄαで回転した場合、図１５（ｂ）に示すように、移動後後のロボット姿勢方向（姿勢情報）は、α＋ｄαとして示すことができる。
【００７１】
図１６は、障害物グリッド算出部ＯＧ３３０のもたらす効果を示す図である。図１６に示すように、環境地図及びロボット装置自身の姿勢方向の情報の更新、即ちキネマティックオドメトリＫＩＮＥ８０１から障害物グリッド算出部８０２への移動量（オドメトリ）８０３の受け渡しは、所定の時間毎に自動的に行うこともできるが、ロボット装置が１歩歩く毎に行うようにする。このことにより、移動していないときは更新作業をしないようにし、更新作業を行うことによる計算コスト及びメモリコピーコストをなくすことでロボット装置の例えばＣＰＵ等の負荷を低減することができる。また、ソフトウェアの構成がオブジェクト単位で処理を行うような場合は、受け渡すデータは、ロボット装置の移動差分量・回転差分量（ｄｘ，ｄｙ，ｄα）のみとでよく、ロボット装置の絶対座標及び絶対角度を受け渡すよりもデータ量が少なく、且つ高い精度を得ることができる。
【００７２】
また、エンタテーメントロボット装置が位置認識以外の処理を行う場合、例えば音声認識及び顔認識等、位置認識と直接関係がなく、位置認識のプライオリティを下げてもよい場合がある。このような場合において、位置認識装置（位置認識のオブジェクト）を停止すると、位置認識のプライオリティが高くなった際に、停止していた間に移動があったりする等して、停止前の位置認識結果と整合が取れなくなるという問題（ＫｉｄＮａｐ問題）が生じる。従って、位置認識のプライオリティを下げてよい場合においては、環境地図のグリッドの大きさＣＳを大きくするか、又は環境地図サイズの大きさ自身を小さくするかすることにより、位置認識のオブジェクトを停止することなく、ＣＰＵのリソースを低減することができる。
【００７３】
更に、以上説明したような処理で得られる環境地図及びロボット装置の姿勢情報を図１７に示す操作盤に表示することができる。図１７は、例えばユーザがロボット装置の遠隔操作時に使用する操作盤を示す模式図である。図１７に示すように、操作盤９００には、ロボット装置のカメラ画像９０１及び環境地図９１０が表示される。操作盤９００には、その他、例えばロボット装置９１３を移動させる操作ボタン９２０等が備えられている。
【００７４】
移動ロボット装置の重要な利用方法としては、自律動作以外にユーザが遠隔操作をする利用方法がある。従来、ロボットの操作盤にロボット装置に装備されたカメラによって得られる画像を表示するものがある。しかしながら、特に、視野の限られたカメラだけではユーザがロボット装置周辺の環境を把握してロボット装置を操作することは困難であった。しかし、本実施の形態のように、ロボット装置９１３に装備されたＣＣＤ等のカメラからのカメラ画像９０１のみのデータで遠隔操作する従来のロボット制御よりも、遠隔操作用の操作盤９００に環境地図９１０を表示することによって、ロボット制御の操作性が飛躍的に向上する。
【００７５】
ここで、ロボットの姿勢情報が変化しても環境地図を更新しないものとして説明したが、通常、ロボット装置においてはＣＣＤカメラ等の撮像装置は頭部ユニット前面に設けられており、カメラ画像９０１はロボット装置の前方の画像を表示する。従って、姿勢情報が変化してロボット装置の向きが例えば１８０°回転した場合、カメラ画像と環境地図との方向が逆になる。よって、姿勢情報が、例えば４５°又は９０°等、所定の角度以上回転した場合は、この回転に伴い環境地図を回転させることにより、更にロボット制御の操作性が向上する。
【００７６】
次に、ランドマーク位置検出部ＣＬＳ３４０は、図１８に示すように、例えば、グリーン部分１００１、ピンク部分１００２、又はブルー部分１００３等のカラーを有する人工的なカラーランドマーク１００４を含む環境において、ロボット装置１のセンサ情報と、ロボット装置１が行った動作情報によって、ロボット装置の自己位置（位置及び姿勢）を同定する。
【００７７】
例えば、２次元的な作業空間上では、略等間隔状にグリッド（ｘ，ｙ）を設け、各グリッドの位置ｌ（localization）毎にロボット装置が存在する確率ｐ（ｌ）を管理する。この存在確率ｐ（ｌ）は、ロボット装置の移動（ｍｏｖｅ）、即ち内界観測情報ａ、又はランドマークの観測、即ち外界観測情報ｓの入力に応じて更新される。
【００７８】
存在確率ｐ（ｌ）は、ロボット装置の以前の状態、即ち自己位置ｌ'における存在確率ｐ（ｌ'）と、以前の状態ｌ'において移動ａを行ったときに状態ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）に依拠する。即ち、これまでの各状態ｌ'になる確率ｐ（ｌ'）と、状態ｌ'において移動ａを行ったときに状態ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）の積を加算していく（若しくは積分する）ことによって、現在の状態、即ち自己位置ｌになるという存在確率ｐ（ｌ）に収束する。従って、外界観測結果としてのロボット装置の移動ａが観測されたときに
は、各グリッドでは、下記数式（５）に従ってロボット装置の存在確率ｐ（ｌ）を更新することができる。
【００７９】
【数５】
【００８０】
また、状態、即ち自己位置ｌにロボット装置が存在するという存在確率ｐ（ｌ）は、存在確率ｐ（ｌ）とこの状態ｌにおいてランドマークを観察するという遷移確率ｐ（ｓ｜ｌ）に依拠する。したがって、状態ｌにおいてランドマークの観測、即ち外界観測情報ｓが入力された場合には、下式に従ってロボット装置の存在確率ｐ（ｌ）を更新することができる。但し、下記数式（４）に示すように右辺では、正規化のため、ランドマークを観察するという確率ｐ（ｓ）で除して正規化している。
【００８１】
【数６】
【００８２】
図１９は、マルコフ・ローカリゼーションと拡張カルマン・フィルタを併用したランドマークセンサＣＬＳ（自己位置同定システム）３４０の機能構成を模式的に示している。同図に示すように、このランドマークセンサＣＬＳ３４０は、マルコフ・ローカリゼーション部（ＭＬ）３４２と、拡張カルマン・フィルタ部（ＥＫＬ）３４３と、拡張カルマン・フィルタ部３４３の動作を制御するＥＫＬ制御部３４４とで構成されている。
【００８３】
マルコフ・ローカリゼーション部３４２は、作業空間内の自分の位置を離散的なグリッド上の自己位置確率密度分布として保持し、ランドマークの観測に関する外界観測情報ｓと、ロボット装置自身の動作に関する内界観測情報ａを入力して、自己位置確率密度分布の更新を行う。そして、各時刻において、自己位置確率密度分布の最も高い値を持つグリッドを自己位置の推定結果としてＥＫＬ制御部３４４に出力する。
【００８４】
図２０は、マルコフ・ローカリゼーション部３４２により求められた各グリッド上での自己位置確率密度分布を表している。同図では、各グリッドにおける確率密度を濃淡により表現している。最も色が濃い、即ち自己位置確率密度分布が最も高い値を有するグリッドが自己位置の推定結果となる。
【００８５】
このマルコフ・ローカリゼーションによる自己位置同定は、センサのノイズに対してロバストであり、同定解の精度は粗いが解の収束速度が速いことを主な特徴とする。
【００８６】
一方、図１９に示す拡張カルマン・フィルタ部３４３は、自己位置を状態変数［ｘ，ｙ，θ］を実測値として保持し、環境内に設置されたカラーランドマーク１００４を観測すると、ランドマークからの相対位置に基づいて自己位置を推定する。また、自身の動作情報を観測すると、その動作情報に基づいて状態量の推定を行う。
【００８７】
拡張カルマン・フィルタ部３４３は、ロボット装置自身の動作情報ａと状態、即ち、自己位置ｌとの関係を規定した状態モデルと、自己位置ｌとランドマークの観測情報ｓとの関係を規定した観測モデルとで構成されている。
【００８８】
状態モデルは、ロボット装置が状態（自己位置）ｌにおいて動作ａを行った場合における理論的な状態ｌを与える遷移関数Ｆ（ｌ，ａ）を備えている。実際には理論的な状態ｌに対してノイズ成分ｗが重畳されることから、ロボット装置の状態ｌは状態モデルにより下記数式（７）のように収束する。
【００８９】
【数７】
【００９０】
また、観測モデルは、ロボット装置が状態、即ち自己位置ｉにおいて、ある既知の環境Ｅｎｖ（例えばランドマークの位置など）に関する観測の理論値ｓを与える観測関数Ｈ（Ｅｎｖ，ｌ）を備えている。実際には観測の理論値に対してノイズ成分ｖが重畳されることから、観測値ｓは観測モデルにより下記数式（８）のように収束する。
【００９１】
【数８】
【００９２】
なお、状態ｌ並びに観測ｓに重畳されるそれぞれのノイズｗ及びｖは、ここではゼロを中央値とするガウス分布として仮定する。
【００９３】
ロボット装置自身の動作情報ａと自己位置ｌとの関係を規定した状態モデルと自己位置ｌとランドマークの観測情報ｓとの関係を規定した観測モデルを備えた拡張カルマン・フィルタ部３４３において、動作情報ａは内界観測結果として、ランドマークの観測情報ｓは外界観測結果として、夫々既知である。従って、ロボット装置の自己位置同定を、ロボット装置の状態ｌをロボット装置の動作情報ａ及び観測情報ｓにより推定するという問題に帰着することができる。ここでは、ロボット装置の動作ａ、状態ｌ、及び観測ｓをそれぞれ下記に示すガウス分布として表現することができる。
【００９４】
【数９】
【００９５】
ある時点でのロボット装置の状態ｌがある中央値と及び共分散を有するガウス分布をとるものとして推定する。そして、ロボット装置の動作ａが観測されたときには、状態ｌの推定値に関する中央値並びに共分散を下記数式（９−１），（９−２）により更新することができる。
【００９６】
【数１０】
【００９７】
ここで、▽Ｆｌ，▽Ｆａは、下記とする。
【００９８】
【数１１】
【００９９】
同様に、ある時点でのロボット装置の状態ｌがある中央値及び共分散を有するガウス分布をとるものとして推定する。そして、ランドマークの観測情報ｓが観測されたときには、状態ｌの推定値に関する中央値並びに共分散を下記数式（１０−１），（１０−２）により更新することができる。
【０１００】
【数１２】
【０１０１】
ここで、各パラメータは下記とする。
【０１０２】
【数１３】
【０１０３】
拡張カルマン・フィルタ３４３はセンサ情報に対するロバスト性に優れているので、拡張カルマン・フィルタ部３４３の推定結果は、ランドマークセンサＣＬＳ３４０全体の出力としている。
【０１０４】
ＥＫＬ制御部３４４は、マルコフ・ローカリゼーション部３４２の出力結果に応じて拡張カルマン・フィルタ部３４４の動作を制御する。より具体的には、マルコフ・ローカリゼーション部３４２の自己位置推定結果に基づいて、ランドマークの観測情報ｓに関する妥当性を検証する。観測情報ｓの妥当性は、マルコフ・ローカリゼーション部３４２において最大の存在確率となるグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈｒｅｓｈobsを越えたかどうかで判断することができる。
【０１０５】
グリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈobsを下回る場合には、センサ・ノイズに対してロバストなマルコフ・ローカリゼーション部３４２においてさえ、センサ・ノイズのために同定解が充分に収束していないことが推測される。このような場合、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３において自己位置を推定しても、精度が高い解が得られず、むしろ演算時間を浪費するだけである。このため、観測情報ｓが妥当でないと判断された場合には、切換器３４５を用いて拡張カルマン・フィルタ部３４３への外界観測情報ｓ、即ちランドマークの観測情報の入力を遮断して、拡張カルマン・フィルタ部３４３における自己位置推定値の更新を停止させる。
【０１０６】
また、ＥＫＬ制御部３４４は、拡張カルマン・フィルタ部３４３の自己位置推定結果の妥当性も検証する。自己位置推定結果の妥当性は、推定された状態ｌの中央値及び共分散を用いて、マルコフ・ローカリゼーション部３４２から出力される存在確率ｐ（ｌ）との分布比較テストにより判断することができる。分布比較テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。
【０１０７】
分布比較テストによりマルコフ・ローカリゼーション部３４２と拡張カルマン・フィルタ部３４３との各確率分布が類似していない場合には、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３における自己位置推定値の方が、センサ・ノイズの影響により妥当でないと判断することができる。このような場合、ＥＫＬ制御部３４４は拡張カルマン・フィルタ部３４３の再初期化を行わせる。何故ならば、拡張カルマン・フィルタは再復帰に多大な時間を要するからである。
【０１０８】
次に、ランドマークセンサＣＬＳ３４０の動作について説明する。図２１は、ランドマークセンサＣＬＳ３４０の動作を示すフローチャートである。図２１に示すように、ロボット装置１の移動に関する内界観測情報ａがランドマークセンサＣＬＳ３４０に入力されると、まずマルコフ・ローカリゼーション部３４２において、上記数式（５）を用いて自己位置推定値の更新処理が行われる（ステップＳ２０１）。次いで、拡張カルマン・フィルタ部３４３において、上記数式数（９）を用いて自己位置推定値の更新処理が行われる（ステップＳ２０２）。
【０１０９】
また、ランドマークの観測に関する外界観測情報ｓがランドマークセンサＣＬＳ３４０に入力されると、まずマルコフ・ローカリゼーション部３４２において、上記数式（６）を用いて、自己位置推定値の更新処理が行われる（ステップＳ２１１）。
【０１１０】
マルコフ・ローカリゼーション部３４２の出力結果は、ＥＫＬ制御部３４４に入力されて、観測情報ｓの妥当性が検証される（ステップＳ２１２）。観測情報ｓの妥当性は、マルコフ・ローカリゼーション部３４２において最大の存在確率となるグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈｒｅｓｈobsを越えたかどうかで判断することができる。
【０１１１】
グリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈobsを下回る場合には、センサ・ノイズに対してロバストなマルコフ・ローカリゼーション部３４２においてさえ、センサ・ノイズのために同定解が充分に収束していないことが推測される。このような場合、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３において自己位置を推定しても、精度のよい解が得られず、むしろ演算時間を浪費するだけである。このため、観測情報ｓが妥当でないと判断された場合には、切換器３４５を用いて拡張カルマン・フィルタ部３４３への外界観測情報ｓ、即ちランドマークの観測情報の入力を遮断して、拡張カルマン・フィルタ部３４３における自己位置推定値の更新を停止させる。
【０１１２】
他方、観測情報ｓを検証した結果、妥当性を満たす、即ちグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈobsを上回る場合には、さらに拡張カルマン・フィルタ部３４３において、上記数式（１０−１），（１０−２）を用いて更新処理が行われる（ステップＳ２１３）。
【０１１３】
拡張カルマン・フィルタ部３４３による自己位置推定の結果は、ＥＫＬ制御部３４４に入力されて、その妥当性が検証される（ステップＳ２１４）。拡張カルマン・フィルタ部３４３による自己位置推定結果の妥当性は、推定された状態ｌの中央値及び共分散を用いて、マルコフ・ローカリゼーション部３４２から出力される存在確率ｐ（ｌ）との分布比較テストにより判断することができる。分布比較テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。
【０１１４】
分布比較テストによりマルコフ・ローカリゼーション部３４２と拡張カルマン・フィルタ部３４３それぞれの確率分布が類似していない場合には、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３における自己位置推定値の方が、センサ・ノイズの影響により妥当でないと判断することができる。このような場合、ＥＫＬ制御部３４４は拡張カルマン・フィルタ部３４３の再初期化を行わせる（ステップＳ２１５）。何故ならば、拡張カルマン・フィルタは再復帰に多大な時間を要するからである。
【０１１５】
このようにして、ランドマークセンサＣＬＳ３４０では、広い範囲で比較的短い探索時間で探索を行う大域探索と高精度だが探索時間を要する局所的探索とを併用して、高精度で、高速且つロバストな自己位置同定を行うことができる。
次に、行動決定部ＳＢＬ３６０は、障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により、ロボット装置１の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部ＬＺ３５０から現在のロボット装置１の絶対座標を取得することにより、絶対座標系又はロボット装置中心のロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。即ち、入力データから状況に応じてロボット装置１が取るべき行動を決定し、その行動列を出力する。ここで、障害物グリッド算出部ＯＧ３３０の、障害物情報３３１に基づいた処理については後述する。
【０１１６】
障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により生成される障害物地図上の点は、図２２に示すように以下の３つの種類に分類される。
【０１１７】
第１の点は、障害物の存在する点（図中の黒色で表される点）である。第２の点は、自由空間（障害物が存在しない空間）上の点（図中の白で表される点）である。そして、第３の点は、未観測領域上の点（図中の斜線で表される点）である。
【０１１８】
次に、前記経路計画決定部ＳＢＬ３６０が採用する経路計画アルゴリズムを図２３のフローチャートに示し、以下に詳細を説明する。
先ず、現在位置から目的位置までを結ぶ直線経路の周辺の障害物地図が作成されるように、視線を目的地方向に向ける（ステップＳ７１）。そして距離画像を観測し、距離計測を行い、障害物地図を作成（更新）する（ステップＳ７２）。
【０１１９】
次に、生成された障害物地図において、未観測領域と自由空間領域を移動可能領域とみなして経路計画を行う（ステップＳ７３）。
【０１２０】
この経路計画としては、例えば経路全体のコストを最小化するＡ＊探索（Ａ＊search)いう方法を用いる。このＡ＊探索は、評価関数としてｆを用い、ｈ関数が許容的であるような最良優先探索である。いかなるヒューリスティック関数に対しても効率最適という点を用いている。
【０１２１】
このステップＳ７３にて例えばＡ＊探索を適用することにより生成された経路が移動可能であるな否かをステップＳ７４にてチェックし、障害物を回避することのできる経路が計画できなかった場合（ＮＯ）には、これ以上観測を続けても移動可能な経路が得られる可能性がないため、その旨を告げて経路計画を終了する（ステップＳ７５）。
【０１２２】
ステップＳ７３にて移動可能経路が例えば前記Ａ＊探索を適用して計画できた場合（ＹＥＳ）には、ステップＳ７６に進み、出力された経路上に未観測領域が含まれるか否かを検索する。このステップＳ７６にて経路上に未観測領域が含まれなかった場合（ＮＯ）には、ステップＳ７７にて目的地までの経路計画として移動可能経路を出力する。ステップＳ７６にて未観測領域が含まれる場合（ＹＥＳ）には、ステップＳ７８に進んで現在位置から未観測領域までの歩数を計算し、その歩数が閾値を超えるか否かをチェックする。
【０１２３】
ステップＳ７８において歩数が閾値を超える場合（ＹＥＳ）には、ステップＳ７９により未知領域までの移動可能経路を出力する。一方、ステップＳ７８において未知観測領域までの歩数が閾値に満たない場合（ＮＯ）は、未観測領域が距離観測されるように視線方向の制御を行って再観測を行い（ステップＳ８０）、再び障害物地図の更新を行う。
【０１２４】
以上のような経路計画アルゴリズムを採用した経路計画決定部ＳＢＬ３６０は、未観測領域と自由空間領域を移動可能領域とみなして経路計画を行い、出力された経路上に含まれる未観測領域部のみを再観測することにより、目的地に移動する際に不必要な観測及び距離画像計算処理を行わなわず、効率的で短時間に移動経路計画を生成することが可能となる。
【０１２５】
以下、本発明の実施の形態における上述した位置認識装置を搭載した２足歩行タイプのロボット装置について詳細に説明する。この人間型のロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボットである。
【０１２６】
図２３に示すように、ロボット装置１は、体幹部ユニット２の所定の位置に頭部ユニット３が連結されると共に、左右２つの腕部ユニット４Ｒ／Ｌと、左右２つの脚部ユニット５Ｒ／Ｌが連結されて構成されている（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下において同じ。）。
【０１２７】
このロボット装置１が具備する関節自由度構成を図２５に模式的に示す。頭部ユニット３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。
【０１２８】
また、上肢を構成する各々の腕部ユニット４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール輪１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書ではゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。
【０１２９】
また、体幹部ユニット２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。
【０１３０】
また、下肢を構成する各々の脚部ユニット５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足部１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置１の股関節位置を定義する。人体の足部１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、ロボット装置１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
【０１３１】
以上を総括すれば、ロボット装置１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度、即ち関節数を適宜増減することができることはいうまでもない。
【０１３２】
上述したようなロボット装置１がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行うことなどの要請から、アクチュエータは小型且つ軽量であることが好ましい。
【０１３３】
図２６には、ロボット装置１の制御システム構成を模式的に示している。同図に示すように、ロボット装置１は、ヒトの四肢を表現した体幹部ユニット２，頭部ユニット３，腕部ユニット４Ｒ／Ｌ，脚部ユニット５Ｒ／Ｌと、各ユニット間の協調動作を実現するための適応制御を行う制御ユニット１０とで構成される。
【０１３４】
ロボット装置１全体の動作は、制御ユニット１０によって統括的に制御される。制御ユニット１０は、ＣＰＵ（Central Processing Unit）や、ＤＲＡＭ、フラッシュＲＯＭ等の主要回路コンポーネント（図示しない）で構成される主制御部１１と、電源回路やロボット装置１の各構成要素とのデータやコマンドの授受を行うインターフェイス（何れも図示しない）などを含んだ周辺回路１２とで構成される。
【０１３５】
この制御ユニット１０の設置場所は、特に限定されない。図２６では体幹部ユニット２に搭載されているが、頭部ユニット３に搭載してもよい。あるいは、ロボット装置１外に制御ユニット１０を配備して、ロボット装置１の機体とは有線又は無線で交信するようにしてもよい。
【０１３６】
図２６に示したロボット装置１内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。即ち、頭部ユニット３には、首関節ヨー軸１０１、首関節ピッチ軸１０２、首関節ロール軸１０３の各々を表現する首関節ヨー軸アクチュエータＡ２、首関節ピッチ軸アクチュエータＡ３、首関節ロール軸アクチュエータＡ４が配設されている。
【０１３７】
また、頭部ユニット３には、外部の状況を撮像するためのＣＣＤ（Charge Coupled Device）カメラが設けられているほか、前方に位置する物体までの距離を測定するための距離センサ、外部音を集音するためのマイク、音声を出力するためのスピーカ、使用者からの「撫でる」や「叩く」といった物理的な働きかけにより受けた圧力を検出するためのタッチセンサ等が配設されている。
【０１３８】
また、体幹部ユニット２には、体幹ピッチ軸１０４、体幹ロール軸１０５、体幹ヨー軸１０６の各々を表現する体幹ピッチ軸アクチュエータＡ５、体幹ロール軸アクチュエータＡ６、体幹ヨー軸アクチュエータＡ７が配設されている。また、体幹部ユニット２には、このロボット装置１の起動電源となるバッテリを備えている。このバッテリは、充放電可能な電池によって構成されている。
【０１３９】
また、腕部ユニット４Ｒ／Ｌは、上腕ユニット４１Ｒ／Ｌと、肘関節ユニット４２Ｒ／Ｌと、前腕ユニット４３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸１０７、肩関節ロール軸１０８、上腕ヨー軸１０９、肘関節ピッチ軸１１０、前腕ヨー軸１１１、手首関節ピッチ軸１１２、手首関節ロール軸１１３の各々表現する肩関節ピッチ軸アクチュエータＡ８、肩関節ロール軸アクチュエータＡ９、上腕ヨー軸アクチュエータＡ１０、肘関節ピッチ軸アクチュエータＡ１１、肘関節ロール軸アクチュエータＡ１２、手首関節ピッチ軸アクチュエータＡ１３、手首関節ロール軸アクチュエータＡ１４が配備されている。
【０１４０】
また、脚部ユニット５Ｒ／Ｌは、大腿部ユニット５１Ｒ／Ｌと、膝ユニット５２Ｒ／Ｌと、脛部ユニット５３Ｒ／Ｌに細分化されるが、股関節ヨー軸１１５、股関節ピッチ軸１１６、股関節ロール軸１１７、膝関節ピッチ軸１１８、足首関節ピッチ軸１１９、足首関節ロール軸１２０の各々を表現する股関節ヨー軸アクチュエータＡ１６、股関節ピッチ軸アクチュエータＡ１７、股関節ロール軸アクチュエータＡ１８、膝関節ピッチ軸アクチュエータＡ１９、足首関節ピッチ軸アクチュエータＡ２０、足首関節ロール軸アクチュエータＡ２１が配備されている。各関節に用いられるアクチュエータＡ２，Ａ３・・・は、より好ましくは、ギア直結型で旦つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ＡＣサーボ・アクチュエータで構成することができる。
【０１４１】
体幹部ユニット２、頭部ユニット３、各腕部ユニット４Ｒ／Ｌ、各脚部ユニット５Ｒ／Ｌなどの各機構ユニット毎に、アクチュエータ駆動制御部の副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌが配備されている。さらに、各脚部ユニット５Ｒ／Ｌの足底が着床したか否かを検出する接地確認センサ３０Ｒ／Ｌを装着するとともに、体幹部ユニット２内には、姿勢を計測する姿勢センサ３１を装備している。
【０１４２】
接地確認センサ３０Ｒ／Ｌは、例えば足底に設置された近接センサ又はマイクロ・スイッチなどで構成される。また、姿勢センサ３１は、例えば、加速度センサとジャイロ・センサの組み合わせによって構成される。
【０１４３】
接地確認センサ３０Ｒ／Ｌの出力によって、歩行・走行などの動作期間中において、左右の各脚部が現在立脚又は遊脚何れの状態であるかを判別することができる。また、姿勢センサ３１の出力により、体幹部分の傾きや姿勢を検出することができる。
【０１４４】
主制御部１１は、各センサ３０Ｒ／Ｌ，３１の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌの各々に対して適応的な制御を行い、ロボット装置１の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現できる。
【０１４５】
ロボット装置１の機体上での全身運動は、足部運動、ＺＭＰ（Zero Moment Point）軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容にしたがった動作を指示するコマンドを各副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌに転送する。そして、各々の副制御部２０，２１，・・・等では、主制御部１１からの受信コマンドを解釈して、各アクチュエータＡ２，Ａ３・・・等に対して駆動制御信号を出力する。ここでいう「ＺＭＰ」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ＺＭＰ軌道」とは、例えばロボット装置１の歩行動作期間中にＺＭＰが動く軌跡を意味する。なお、ＺＭＰの概念並びにＺＭＰを歩行ロボットの安定度判別規範に適用する点については、Miomir Vukobratovic著“LEGGED LOCOMOTION ROBOTS”（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））に記載されている。
【０１４６】
以上のように、ロボット装置１は、各々の副制御部２０，２１，・・・等が、主制御部１１からの受信コマンドを解釈して、各アクチュエータＡ２，Ａ３・・・に対して駆動制御信号を出力し、各ユニットの駆動を制御している。これにより、ロボット装置１は、目標の姿勢に安定して遷移し、安定した姿勢で歩行できる。
【０１４７】
また、ロボット装置１における制御ユニット１０では、上述したような姿勢制御のほかに、加速度センサ、タッチセンサ、接地確認センサ等の各種センサ、及びＣＣＤカメラからの画像情報、マイクからの音声情報等を統括して処理している。制御ユニット１０では、図示しないが加速度センサ、ジャイロ・センサ、タッチセンサ、距離センサ、マイク、スピーカなどの各種センサ、各アクチュエータ、ＣＣＤカメラ及びバッテリが各々対応するハブを介して主制御部１１と接続されている。
【０１４８】
主制御部１１は、上述の各センサから供給されるセンサデータや画像データ及び音声データを順次取り込み、これらをそれぞれ内部インターフェイスを介してＤＲＡＭ内の所定位置に順次格納する。また、主制御部１１は、バッテリから供給されるバッテリ残量を表すバッテリ残量データを順次取り込み、これをＤＲＡＭ内の所定位置に格納する。ＤＲＡＭに格納された各センサデータ、画像データ、音声データ及びバッテリ残量データは、主制御部１１がこのロボット装置１の動作制御を行う際に利用される。
【０１４９】
主制御部１１は、ロボット装置１の電源が投入された初期時、制御プログラムを読み出し、これをＤＲＡＭに格納する。また、主制御部１１は、上述のように主制御部１１よりＤＲＡＭに順次格納される各センサデータ、画像データ、音声データ及びバッテリ残量データに基づいて自己及び周囲の状況や、使用者からの指示及び働きかけの有無などを判断する。
【０１５０】
さらに、主制御部１１は、この判断結果及びＤＲＡＭに格納した制御プログラムに基づいて自己の状況に応じて行動を決定するとともに、当該決定結果に基づいて必要なアクチュエータを駆動させることによりロボット装置１に、いわゆる「身振り」、「手振り」といった行動をとらせる。
【０１５１】
このようにしてロボット装置１は、制御プログラムに基づいて自己及び周囲の状況を判断し、使用者からの指示及び働きかけに応じて自律的に行動できる。
【０１５２】
ところで、このロボット装置１は、内部状態に応じて自律的に行動することができる。そこで、ロボット装置１における制御プログラムのソフトウェア構成例について、図２７乃至図３２を用いて説明する。なお、この制御プログラムは、上述したように、予めフラッシュＲＯＭ１２に格納されており、ロボット装置１の電源投入初期時において読み出される。
【０１５３】
図２７において、デバイス・ドライバ・レイヤ４０は、制御プログラムの最下位層に位置し、複数のデバイス・ドライバからなるデバイス・ドライバ・セット４１から構成されている。この場合、各デバイス・ドライバは、ＣＣＤカメラやタイマ等の通常のコンピュータで用いられるハードウェアに直接アクセスすることを許されたオブジェクトであり、対応するハードウェアからの割り込みを受けて処理を行う。
【０１５４】
また、ロボティック・サーバ・オブジェクト４２は、デバイス・ドライバ・レイヤ４０の最下位層に位置し、例えば上述の各種センサやアクチュエータ２８１〜２８ｎ等のハードウェアにアクセスするためのインターフェイスを提供するソフトウェア群でなるバーチャル・ロボット４３と、電源の切換えなどを管理するソフトウェア群でなるパワーマネージャ４４と、他の種々のデバイス・ドライバを管理するソフトウェア群でなるデバイス・ドライバ・マネージャ４５と、ロボット装置１の機構を管理するソフトウェア群でなるデザインド・ロボット４６とから構成されている。
【０１５５】
マネージャ・オブジェクト４７は、オブジェクト・マネージャ４８及びサービス・マネージャ４９から構成されている。オブジェクト・マネージャ４８は、ロボティック・サーバ・オブジェクト４２、ミドル・ウェア・レイヤ５０、及びアプリケーション・レイヤ５１に含まれる各ソフトウェア群の起動や終了を管理するソフトウェア群であり、サービス・マネージャ４９は、メモリカードに格納されたコネクションファイルに記述されている各オブジェクト間の接続情報に基づいて各オブジェクトの接続を管理するソフトウェア群である。
【０１５６】
ミドル・ウェア・レイヤ５０は、ロボティック・サーバ・オブジェクト４２の上位層に位置し、画像処理や音声処理などのこのロボット装置１の基本的な機能を提供するソフトウェア群から構成されている。また、アプリケーション・レイヤ５１は、ミドル・ウェア・レイヤ５０の上位層に位置し、当該ミドル・ウェア・レイヤ５０を構成する各ソフトウェア群によって処理された処理結果に基づいてロボット装置１の行動を決定するためのソフトウェア群から構成されている。
【０１５７】
なお、ミドル・ウェア・レイヤ５０及びアプリケーション・レイヤ５１の具体なソフトウェア構成をそれぞれ図２８に示す。
【０１５８】
ミドル・ウェア・レイヤ５０は、図２８に示すように、騒音検出用、温度検出用、明るさ検出用、音階認識用、距離検出用、姿勢検出用、タッチセンサ用、動き検出用及び色認識用の各信号処理モジュール６０〜６８並びに入力セマンティクスコンバータモジュール６９などを有する認識系７０と、出力セマンティクスコンバータモジュール７８並びに姿勢管理用、トラッキング用、モーション再生用、歩行用、転倒復帰用、ＬＥＤ点灯用及び音再生用の各信号処理モジュール７１〜７７などを有する出力系７９とから構成されている。
【０１５９】
認識系７０の各信号処理モジュール６０〜６８は、ロボティック・サーバ・オブジェクト４２のバーチャル・ロボット４３によりＤＲＡＭから読み出される各センサデータや画像データ及び音声データのうちの対応するデータを取り込み、当該データに基づいて所定の処理を施して、処理結果を入力セマンティクスコンバータモジュール６９に与える。ここで、例えば、バーチャル・ロボット４３は、所定の通信規約によって、信号の授受或いは変換をする部分として構成されている。
【０１６０】
入力セマンティクスコンバータモジュール６９は、これら各信号処理モジュール６０〜６８から与えられる処理結果に基づいて、「うるさい」、「暑い」、「明るい」、「ボールを検出した」、「転倒を検出した」、「撫でられた」、「叩かれた」、「ドミソの音階が聞こえた」、「動く物体を検出した」又は「障害物を検出した」などの自己及び周囲の状況や、使用者からの指令及び働きかけを認識し、認識結果をアプリケーション・レイヤ４１に出力する。
【０１６１】
アプリケーション・レイヤ５１は、図２９に示すように、行動モデルライブラリ８０、行動切換モジュール８１、学習モジュール８２、感情モデル８３及び本能モデル８４の５つのモジュールから構成されている。
【０１６２】
行動モデルライブラリ８０には、図３０に示すように、「バッテリ残量が少なくなった場合」、「転倒復帰する」、「障害物を回避する場合」、「感情を表現する場合」、「ボールを検出した場合」などの予め選択されたいくつかの条件項目にそれぞれ対応させて、それぞれ独立した行動モデルが設けられている。
【０１６３】
そして、これら行動モデルは、それぞれ入力セマンティクスコンバータモジュール６９から認識結果が与えられたときや、最後の認識結果が与えられてから一定時間が経過したときなどに、必要に応じて後述のように感情モデル８３に保持されている対応する情動のパラメータ値や、本能モデル８４に保持されている対応する欲求のパラメータ値を参照しながら続く行動をそれぞれ決定し、決定結果を行動切換モジュール８１に出力する。
【０１６４】
なお、この実施の形態の場合、各行動モデルは、次の行動を決定する手法として、図３１に示すような１つのノード（状態）ＮＯＤＥ０〜ＮＯＤＥｎから他のどのノードＮＯＤＥ０〜ＮＯＤＥｎに遷移するかを各ノードＮＯＤＥ０〜ＮＯＤＥｎに間を接続するアークＡＲＣ１〜ＡＲＣｎ１に対してそれぞれ設定された遷移確率Ｐ１〜Ｐｎに基づいて確率的に決定する有限確率オートマトンと呼ばれるアルゴリズムを用いる。
【０１６５】
具体的に、各行動モデルは、それぞれ自己の行動モデルを形成するノードＮＯＤＥ０〜ＮＯＤＥｎにそれぞれ対応させて、これらノードＮＯＤＥ０〜ＮＯＤＥｎ毎に図２３に示すような状態遷移表９０を有している。
【０１６６】
この状態遷移表９０では、そのノードＮＯＤＥ０〜ＮＯＤＥｎにおいて遷移条件とする入力イベント（認識結果）が「入力イベント名」の列に優先順に列記され、その遷移条件についてのさらなる条件が「データ名」及び「データ範囲」の列における対応する行に記述されている。
【０１６７】
したがって、図３２の状態遷移表９０で表されるノードＮＯＤＥ１００では、「ボールを検出（ＢＡＬＬ）」という認識結果が与えられた場合に、当該認識結果とともに与えられるそのボールの「大きさ（SIZE）」が「0から1000」の範囲であることや、「障害物を検出（OBSTACLE）」という認識結果が与えられた場合に、当該認識結果とともに与えられるその障害物までの「距離（DISTANCE）」が「0から100」の範囲であることが他のノードに遷移するための条件となっている。
【０１６８】
また、このノードＮＯＤＥ１００では、認識結果の入力がない場合においても、行動モデルが周期的に参照する感情モデル８３及び本能モデル８４にそれぞれ保持された各情動及び各欲求のパラメータ値のうち、感情モデル８３に保持された「喜び（Joy）」、「驚き（Surprise）」又は「悲しみ（Sadness）」の何れかのパラメータ値が「50から100」の範囲であるときには他のノードに遷移することができるようになっている。
【０１６９】
また、状態遷移表９０では、「他のノードヘの遷移確率」の欄における「遷移先ノード」の行にそのノードＮＯＤＥ０〜ＮＯＤＥｎから遷移できるノード名が列記されているとともに、「入力イベント名」、「データ名」及び「データの範囲」の列に記述された全ての条件が揃ったときに遷移できる他の各ノードＮＯＤＥ０〜ＮＯＤＥｎへの遷移確率が「他のノードヘの遷移確率」の欄内の対応する箇所にそれぞれ記述され、そのノードＮＯＤＥ０〜ＮＯＤＥｎに遷移する際に出力すべき行動が「他のノードヘの遷移確率」の欄における「出力行動」の行に記述されている。なお、「他のノードヘの遷移確率」の欄における各行の確率の和は１００［％］となっている。
【０１７０】
したがって、図３２の状態遷移表９０で表されるノードＮＯＤＥ１００では、例えば「ボールを検出（ＢＡＬＬ）」し、そのボールの「ＳＩＺＥ（大きさ）」が「0から1000」の範囲であるという認識結果が与えられた場合には、「30［％］」の確率で「ノードＮＯＤＥ１２０（node 120）」に遷移でき、そのとき「ＡＣＴＩＯＮ１」の行動が出力されることとなる。
【０１７１】
各行動モデルは、それぞれこのような状態遷移表９０として記述されたノードＮＯＤＥ０〜 ＮＯＤＥｎが幾つも繋がるようにして構成されており、入力セマンティクスコンバータモジュール６９から認識結果が与えられたときなどに、対応するノードＮＯＤＥ０〜ＮＯＤＥｎの状態遷移表を利用して確率的に次の行動を決定し、決定結果を行動切換モジュール８１に出力するようになされている。
【０１７２】
図３０に示す行動切換モジュール８１は、行動モデルライブラリ８０の各行動モデルからそれぞれ出力される行動のうち、予め定められた優先順位の高い行動モデルから出力された行動を選択し、当該行動を実行すべき旨のコマンド（以下、行動コマンドという。）をミドル・ウェア・レイヤ５０の出力セマンティクスコンバータモジュール７８に送出する。なお、この実施の形態においては、図３０において下側に表記された行動モデルほど優先順位が高く設定されている。
【０１７３】
また、行動切換モジュール８１は、行動完了後に出力セマンティクスコンバータモジュール７８から与えられる行動完了情報に基づいて、その行動が完了したことを学習モジュール８２、感情モデル８３及び本能モデル８４に通知する。
【０１７４】
一方、学習モジュール８２は、入力セマンティクスコンバータモジュール６９から与えられる認識結果のうち、「叩かれた」や「撫でられた」など、使用者からの働きかけとして受けた教示の認識結果を入力する。
【０１７５】
そして、学習モジュール８２は、この認識結果及び行動切換えモジュール７１からの通知に基づいて、「叩かれた（叱られた）」ときにはその行動の発現確率を低下させ、「撫でられた（誉められた）」ときにはその行動の発現確率を上昇させるように、行動モデルライブラリ７０における対応する行動モデルの対応する遷移確率を変更する。
【０１７６】
他方、感情モデル８３は、「喜び（Joy）」、「悲しみ（Sadness）」、「怒り（Anger）」、「驚き（Surprise）」、「嫌悪（Disgust）」及び「恐れ（Fear）」の合計６つの情動について、各情動毎にその情動の強さを表すパラメータを保持している。そして、感情モデル８３は、これら各情動のパラメータ値を、それぞれ入力セマンティクスコンバータモジュール６９から与えられる「叩かれた」及び「撫でられた」などの特定の認識結果や、経過時間及び行動切換モジュール８１からの通知などに基づいて周期的に更新する。
【０１７７】
具体的には、感情モデル８３は、入力セマンティクスコンバータモジュール６９から与えられる認識結果と、そのときのロボット装置１の行動と、前回更新してからの経過時間となどに基づいて所定の演算式により算出されるそのときのその情動の変動量を△Ｅ［ｔ］、現在のその情動のパラメータ値をＥ［ｔ］、その情動の感度を表す係数をｋｅとして、下記数式（１１）によって次の周期におけるその情動のパラメータ値Ｅ［ｔ＋１］を算出し、これを現在のその情動のパラメータ値Ｅ［ｔ］と置き換えるようにしてその情動のパラメータ値を更新する。また、感情モデル８３は、これと同様にして全ての情動のパラメータ値により更新する。
【０１７８】
【数１４】
【０１７９】
なお、各認識結果や出力セマンティクスコンバータモジュール７８からの通知が各情動のパラメータ値の変動量△Ｅ［ｔ］にどの程度の影響を与えるかは予め決められており、例えば「叩かれた」といった認識結果は「怒り」の情動のパラメータ値の変動量△Ｅ［ｔ］に大きな影響を与え、「撫でられた」といった認識結果は「喜び」の情動のパラメータ値の変動量△Ｅ［ｔ］に大きな影響を与えるようになっている。
【０１８０】
ここで、出力セマンティクスコンバータモジュール７８からの通知とは、いわゆる行動のフィードバック情報（行動完了情報）であり、行動の出現結果の情報であり、感情モデル８３は、このような情報によっても感情を変化させる。これは、例えば、「叫ぶ」といった行動により怒りの感情レベルが下がるといったようなことである。なお、出力セマンティクスコンバータモジュール７８からの通知は、上述した学習モジュール８２にも入力されており、学習モジュール８２は、その通知に基づいて行動モデルの対応する遷移確率を変更する。
【０１８１】
なお、行動結果のフィードバックは、行動切換モジュール８１の出力（感情が付加された行動）によりなされるものであってもよい。
【０１８２】
一方、本能モデル８４は、「運動欲（exercise）」、「愛情欲（affection）」、「食欲（appetite）」及び「好奇心（curiosity）」の互いに独立した４つの欲求について、これら欲求毎にその欲求の強さを表すパラメータを保持している。そして、本能モデル８４は、これらの欲求のパラメータ値を、それぞれ入力セマンティクスコンバータモジュール６９から与えられる認識結果や、経過時間及び行動切換モジュール８１からの通知などに基づいて周期的に更新する。
【０１８３】
具体的には、本能モデル８４は、「運動欲」、「愛情欲」及び「好奇心」については、認識結果、経過時間及び出力セマンティクスコンバータモジュール７８からの通知などに基づいて所定の演算式により算出されるそのときのその欲求の変動量をΔＩ［ｋ］、現在のその欲求のパラメータ値をＩ［ｋ］、その欲求の感度を表す係数ｋｉとして、所定周期で下記数式（１２）を用いて次の周期におけるその欲求のパラメータ値Ｉ［ｋ＋１］を算出し、この演算結果を現在のその欲求のパラメータ値Ｉ［ｋ］と置き換えるようにしてその欲求のパラメータ値を更新する。また、本能モデル８４は、これと同様にして「食欲」を除く各欲求のパラメータ値を更新する。
【０１８４】
【数１５】
【０１８５】
なお、認識結果及び出力セマンティクスコンバータモジュール７８からの通知などが各欲求のパラメータ値の変動量△Ｉ［ｋ］にどの程度の影響を与えるかは予め決められており、例えば出力セマンティクスコンバータモジュール７８からの通知は、「疲れ」のパラメータ値の変動量△Ｉ［ｋ］に大きな影響を与えるようになっている。
【０１８６】
なお、この具体例においては、各情動及び各欲求（本能）のパラメータ値がそれぞれ０から１００までの範囲で変動するように規制されており、また係数ｋｅ、ｋｉの値も各情動及び各欲求毎に個別に設定されている。
【０１８７】
一方、ミドル・ウェア・レイヤ５０の出力セマンティクスコンバータモジュール７８は、図２８に示すように、上述のようにしてアプリケーション・レイヤ５１の行動切換モジュール８１から与えられる「前進」、「喜ぶ」、「鳴く」又は「トラッキング（ボールを追いかける）」といった抽象的な行動コマンドを出力系７９の対応する信号処理モジュール７１〜７７に与える。
【０１８８】
そしてこれら信号処理モジュール７１〜７７は、行動コマンドが与えられると当該行動コマンドに基づいて、その行動をするために対応するアクチュエータに与えるべきサーボ指令値や、スピーカから出力する音の音声データ及び又はＬＥＤに与える駆動データを生成し、これらのデータをロボティック・サーバ・オブジェクト４２のバーチャル・ロボット４３及び信号処理回路を順次介して対応するアクチュエータ又はスピーカ又はＬＥＤに順次送出する。
【０１８９】
このようにしてロボット装置１は、上述した制御プログラムに基づいて、自己（内部）及び周囲（外部）の状況や、使用者からの指示及び働きかけに応じた自律的な行動ができる。
【０１９０】
このような制御プログラムは、ロボット装置が読取可能な形式で記録された記録媒体を介して提供される。制御プログラムを記録する記録媒体としては、磁気読取方式の記録媒体（例えば、磁気テープ、フレキシブルディスク、磁気カード）、光学読取方式の記録媒体（例えば、ＣＤ−ＲＯＭ、ＭＯ、ＣＤ−Ｒ、ＤＶＤ）等が考えられる。記録媒体には、半導体メモリ（いわゆるメモリカード（矩形型、正方形型など形状は問わない。）、ＩＣカード）等の記憶媒体も含まれる。また、制御プログラムは、いわゆるインターネット等を介して提供されてもよい。
【０１９１】
これらの制御プログラムは、専用の読込ドライバ装置、又はパーソナルコンピュータ等を介して再生され、有線又は無線接続によってロボット装置１に伝送されて読み込まれる。また、ロボット装置１は、半導体メモリ、又はＩＣカード等の小型化された記憶媒体のドライブ装置を備える場合、これら記憶媒体から制御プログラムを直接読み込むこともできる。
【０１９２】
本実施の形態においては、障害物認識装置は、多数の測定点を使って平面の検出を行うために安定した平面を抽出できる。また、画像から得られる平面候補をロボットの姿勢から得られる床面パラメータと照合することで正しい平面を選択できる。また、障害物を認識するのではなく、実質的には床面を認識するので障害物の形状や大きさに因らない認識が可能である。また、床面からの距離で障害物を表現するので細かい段差や凹みまで検出することができる。またロボットの大きさを考慮して跨いだり、くぐったりする判断も容易である。さらに、2次元床面上の障害物表現となるので経路計画などに既存の移動ロボットで用いられる手法が適用可能であるし、3次元の表現より高速に演算できる。
【０１９３】
【発明の効果】
以上詳細に説明したように本発明に係る障害物認識装置は、座標変換手段による座標変換結果と、平面検出手段によって検出された平面パラメータとに基づいて床面検出手段が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、正確に床面を抽出し、障害物を認識することができる。
【０１９４】
また、本発明に係る障害物認識方法は、座標変換工程による座標変換結果と、平面検出工程によって検出された平面パラメータとに基づいて床面検出工程が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、正確に床面を抽出し、障害物を認識することができる。
【０１９５】
また、本発明に係る障害物認識プログラムは、ロボット装置によって実行されることにより、座標変換工程による座標変換結果と、平面検出工程によって検出された平面パラメータとに基づいて床面検出工程が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、ロボット装置に正確に床面を抽出させ、障害物を認識させることができる。
【０１９６】
また、本発明に係るロボット装置は、座標変換手段による座標変換結果と、平面検出手段によって検出された平面パラメータとに基づいて床面検出手段が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、正確に床面を抽出し、障害物を認識することができる。
【図面の簡単な説明】
【図１】本発明の実施の形態におけるロボット装置の概略を示すブロック図である。
【図２】本発明の実施の形態におけるロボット装置のソフトウェアの構成を示す模式図である。
【図３】障害物認識装置の機能ブロック図である。
【図４】平面抽出部ＰＬＥＸに入力される視差画像の生成を説明するための図である。
【図５】平面抽出部ＰＬＥＸが障害物を認識する処理手順を示すフローチャートである。
【図６】平面抽出部ＰＬＥＸが検出する平面のパラメータを示す図である。
【図７】カメラ座標系から足底接地平面座標系への変換処理を説明するための図である。
【図８】平面抽出部ＰＬＥＸにて抽出された平面上の点を示す図である。
【図９】ロボットビューから床面を抽出し、さらに座標変換して２次元（床平面）上に障害物を表現したことを示す図である。
【図１０】本発明の実施の形態におけるロボット装置のソフトウェアの動作を示すフローチャートである。
【図１１】同ソフトウェアに入力されるデータの流れを示す模式図である。
【図１２】本発明の実施の形態における障害物認識装置（ロボット装置）にて表示される環境地図を示す模式図である。
【図１３】（ａ），（ｂ）及び（ｃ），（ｄ）は位置認識装置の移動量に応じた環境地図の更新方法を示す図であって、夫々環境地図を更新しない場合及び環境地図を更新する場合の環境地図を示す模式図である。
【図１４】位置認識装置における環境地図更新の際の各移動量を示す模式図である。
【図１５】（ａ）及び（ｂ）は、位置認識装置における姿勢方向の夫々更新前後の様子を示す模式図である。
【図１６】位置認識装置における効果を説明する模式図である。
【図１７】環境地図及び姿勢情報を表示した操作盤を示す模式図である。
【図１８】ランドマークセンサＣＬＳにおけるカラーランドマークを示す模式図である。
【図１９】ランドマークセンサＣＬＳの構成を模式的に示すブロック図である。
【図２０】マルコフ・ローカリゼーション部により求められた各グリッド上での自己位置確率密度分布を示す模式図である。
【図２１】ランドマークセンサＣＬＳ３４０の動作を示すフローチャートである。
【図２２】障害物情報により生成された障害物地図上に、現在位置から目的位置までの経路を示した図である。
【図２３】経路計画アルゴリズムを示すフローチャートである。
【図２４】本実施の形態におけるロボット装置の外観構成を示す斜視図である。
【図２５】同ロボット装置の自由度構成モデルを模式的に示す図である。
【図２６】同ロボット装置の回路構成を示すブロック図である。
【図２７】同ロボット装置のソフトウェア構成を示すブロック図である。
【図２８】同ロボット装置のソフトウェア構成におけるミドル・ウェア・レイヤの構成を示すブロック図である。
【図２９】同ロボット装置のソフトウェア構成におけるアプリケーション・レイヤの構成を示すブロック図である。
【図３０】アプリケーション・レイヤの行動モデルライブラリの構成を示すブロック図である。
【図３１】同ロボット装置の行動決定のための情報となる有限確率オートマトンを説明する図である。
【図３２】有限確率オートマトンの各ノードに用意された状態遷移表を示す図である。
【符号の説明】
１ ロボット装置、２５０ 頭部ユニット、２００Ｒ，２００Ｌ ＣＣＤカメラ、２０１Ｒ 右目画像、２０１Ｌ 左目画像、２０２ カラー画像、２０３ 視差画像、２１０ ステレオ画像処理装置、２２０ ＣＰＵ、２３０ アクチュエータ、２３１ 制御信号、２４０ センサ、２４１ センサデータ、２６０ 体幹部、３００ ソフトウェア、３１０ キネマティックオドメトリＫＩＮＥ３１０、３２０ 平面抽出部ＰＬＥＸ、３３０ 障害物グリッド算出部ＯＧ、３４０ ランドマーク位置検出部ＣＬＳ、３５０ 絶対座標算出部ＬＺ、３６０ 行動決定部ＳＢＬ、４００，５００，５１０，６００，６１０，７００ 環境地図、４０１，５０１，６０１，７０１ 中心グリッド、４０２，５０２，６０２ 障害物グリッド、４０３，５０３，５０６，５１３，６０３，７０３ ロボット装置、４１０ グリッド、４０４ 情報、９００ 操作盤
【発明の属する技術分野】
本発明は障害物認識装置及び方法に関し、特に移動型ロボット装置に適用されて床面上の障害物を認識する障害物認識装置、障害物認識方法及び障害物認識プログラム、並びに移動型ロボット装置に関する。
【０００２】
【従来の技術】
自律型ロボットにおいて、自身の周りの環境を認識して経路計画を立ててそれに応じて移動する能力は言うまでもなく重要である。従来の移動ロボットと言えば車輪型のロボットの周囲に超音波センサーを床に平行に配置して、壁などの反射を測定する方法が取られてきた。これは床に平行に測定することで床自身を測定する事は無く、反射した点は全て障害物として取り扱うことが出来るために処理を単純化することができる。しかしながらこうした方法は、特定の高さの障害物しか測定しないために小さな段差や穴などの凹んだ障害物を検出できない。また歩行型ロボット（４足歩行ロボット、２足歩行ロボットなど）ではこうした測距センサーは、頭や手先など能動的に姿勢を変えることができる位置に取り付けられることが多い。
【０００３】
また、広く知られるようになった４足歩行ロボット（エンターテインメントロボット）では、頭部についた測距センサを使い、ロボットの姿勢から床面パラメータを導出して、測定点が床面に属するか障害物として扱うかを判断している。
【０００４】
上記方法でも測定点は1点（もしくは姿勢を変えながら繰り返し測定することで数点）しかなく、ロボットの周囲の状況を把握したとは言い難い。また1点しか測定していないために測定に対する信頼度が著しく低い。
【０００５】
【発明が解決しようとする課題】
本発明は、前記実情に鑑みてなされたものであり、正確に床面を抽出し、障害物を認識することができる障害物認識装置、障害物認識方法及び障害物認識プログラムの提供を目的とする。
【０００６】
また、本発明は、前記障害物認識装置を適用して床面上の障害物を認識しながら移動することができる移動型ロボット装置の提供を目的とする。
【０００７】
【課題を解決するための手段】
本発明に係る障害物認識装置は、前記課題を解決するために、複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成手段と、前記距離画像生成手段により生成された距離画像に基づいて平面パラメータを検出する平面検出手段と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換手段と、前記センサ手段からのセンサデータ出力に基づき前記座標変換手段による座標変換を行って得られた接地平面パラメータと、前記平面検出手段によって検出された平面パラメータとに基づいて床面を検出する床面検出手段と、前記床面検出手段の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識手段とを備える。
【０００８】
本発明に係る障害物認識方法は、前記課題を解決するために、複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成工程と、前記距離画像生成工程により生成された距離画像に基づいて平面パラメータを検出する平面検出工程と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換工程と、前記センサ手段からのセンサデータ出力に基づき前記座標変換工程による座標変換を行って得られた接地平面パラメータと、前記平面検出工程によって検出された平面パラメータとに基づいて床面を検出する床面検出工程と、前記床面検出工程の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識工程とを備える。
【０００９】
本発明に係る障害物認識プログラムは、前記課題を解決するために、障害物を認識する障害物認識プログラムにおいて、複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成工程と、前記距離画像生成工程により生成された距離画像に基づいて平面パラメータを検出する平面検出工程と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換工程と、前記センサ手段からのセンサデータ出力に基づき前記座標変換工程による座標変換を行って得られた接地平面パラメータと、前記平面検出工程によって検出された平面パラメータとに基づいて床面を検出する床面検出工程と、前記床面検出工程の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識工程とを備えてなる。
【００１０】
本発明に係る移動型ロボット装置は、前記課題を解決するために、頭部ユニットと、少なくとも１以上の可動脚ユニットと、情報処理手段を有する胴体ユニットとを備えてなり、障害物の床面を認識しながら前記可動脚ユニットを用いて床面を移動する移動型ロボット装置において、前記頭部ユニットに設けた複数の撮像手段から得られた画像データに基づいて算出された視差画像と、前記頭部ユニット、前記可動脚ユニット、前記胴体ユニットに設けた複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成手段と、前記距離画像生成手段により生成された距離画像に基づいて平面パラメータを検出する平面検出手段と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換手段と、ロボット装置の前記センサ手段からのセンサデータ出力に基づき前記座標変換手段による座標変換を行って得られた接地平面パラメータと、前記平面検出手段によって検出された平面パラメータとに基づいて床面を検出する床面検出手段と、前記床面検出手段の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識手段とを前記胴体ユニットに備えてなる。
【００１１】
本発明では、複数の撮像手段として例えばステレオカメラなどを用いた２次元の測距センサを導入している。これにより画像認識による平面の検出とロボットの姿勢との関係からよりロバストにより正確に床面を抽出することができるようになっている。
【００１２】
【発明の実施の形態】
以下、図面を参照しながら本発明の実施の形態について説明する。この実施の形態は、本発明に係る障害物認識装置を２足歩行のロボット装置に適用したものである。
【００１３】
図１は、２足歩行のロボット装置の概略を示すブロック図である。図１に示すように、ロボット装置１の頭部ユニット２５０には、２台のＣＣＤカメラ２００Ｒ，２００Ｌが設けられ、このＣＣＤカメラ２００Ｒ，２００Ｌの後段には、ステレオ画像処理装置２１０が設けられている。２台のＣＣＤカメラ（以下、右目２００Ｒ、左目２００Ｌという。）により撮像された右目画像２０１Ｒ、左目画像２０１Ｌは、ステレオ画像処理装置２１０に入力される。ステレオ画像処理装置２１０は、各画像２０１Ｒ，２０１Ｌの視差情報（disparity data）（距離情報）を計算し、カラー画像（ＹＵＶ：輝度Ｙ、ＵＶ色差）２０２及び視差画像（ＹＤＲ：輝度Ｙ、視差Ｄ、信頼度Ｒ）２０３をフレーム毎に左右交互に算出する。ここで、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
【００１４】
このカラー画像２０２及び視差画像２０３はロボット装置１の体幹部２６０に内蔵されたＣＰＵ（制御部）２２０に入力される。また、ロボット装置１の各関節にはアクチュエータ２３０が設けられており、ＣＰＵ２２０からの指令となる制御信号２３１が供給されて、その指令値に応じてモータを駆動する。各関節（アクチュエータ）には、ポテンショメータが取り付けられ、その時のモータの回転角がＣＰＵに送られる。このアクチュエータに取り付けられたポテンショメータ、足底に取り付けられたタッチセンサ及び体幹部に取り付けられたジャイロ・センサ等の各センサ２４０は、現在の関節角度、設置情報、及び姿勢情報等の現在のロボット装置の状態を計測し、センサデータ２４１としてＣＰＵ２２０へ出力する。ＣＰＵ２２０は、ステレオ画像処理装置２１０からのカラー画像２０２及び視差画像２０３と、アクチュエータの全ての関節角度等のセンサデータ２４１とが入力され、後述するソフトウェア構成を実現する。
【００１５】
本実施の形態のソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うものである。なお、ロボット装置の位置を示す座標として、例えば、後述するランドマーク等の特定の物体等を座標の原点としたワールド基準系のカメラ座標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心（座標の原点）としたロボット中心座標系（以下、相対座標ともいう。）との２つの座標を使用する。
【００１６】
図２は、２足歩行のロボット装置のソフトウェアの構成を示す模式図である。同図において、丸で表されているのが、オブジェクト又はプロセスと呼ばれるエンティティである。オブジェクト同士が非同期に通信し合うことで、システム全体が動作する。各オブジェクトはメッセージ通信と共有メモリを使用したオブジェクト間通信方法によりデータの受け渡し及びＩｎｖｏｋｅを行っている。
【００１７】
図２に示すように、ソフトウェア３００は、キネマティックオドメトリ（Kinematics Odometry）ＫＩＮＥ３１０、平面抽出部（Plane Extractor）ＰＬＥＸ３２０、障害物グリッド算出部（Occupancy Grid）ＯＧ３３０、ランドマーク位置検出部（Landmark Sensor）ＣＬＳ３４０、絶対座標算出部（Localization）ＬＺ３５０及び行動決定部（Situated behavior Layer）ＳＢＬ３６０から構成され、各オブジェクト単位にて処理がなされる。これらのソフトウェアの詳細な構成及び動作については後述する。また、ロボット装置についての詳細な構成及び動作についても後述する。
【００１８】
ここで、先ず、上述したロボット装置１に搭載される本発明の障害物認識装置について説明する。障害物認識装置は、前記平面抽出部（Plane Extractor）ＰＬＥＸ３２０を実行したＣＰＵ２２０内にて構成される。図３には障害物認識装置２２１の機能ブロック図を示す。障害物認識装置２２１は、後述する視差画像から距離画像を生成する距離画像生成部２２２と、距離画像から平面検出をして平面パラメータを算出する平面検出部２２３と、後述する同時変換行列を座標変換する座標変換部２２４と、前記座標変換結果と前記平面パラメータからから床面を検出する床面検出部２２５と、床面の平面パラメータから障害物を認識する障害物認識部２２６とを備えてなる。
【００１９】
距離画像生成部２２２は、ロボット装置１に設けた二つのＣＣＤカメラから得られた画像データに基づいて算出された視差画像と、ロボット装置１に設けた複数のセンサ手段から得られたセンサデータ出力とに基づいて二つのＣＣＤカメラが配設された位置における前記視差画像に対応する同時変換行列を用いて距離画像を生成する。平面検出部２２３は、距離画像生成部２２２により生成された距離画像に基づいて平面パラメータを検出する。座標変換部２２４は、前記同時変換行列をロボット装置１の接地面での座標に変換する。床面検出部２２５は、平面検出部２２３からの平面パラメータと座標変換部２２４からの座標変換結果を用いて床面を検出し、その平面パラメータを障害物認識部２２６に供給する。障害物認識部２２６は、床面検出部２２５が検出した床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する。
【００２０】
上述した如く、ＣＣＤカメラ２００Ｌ，２００Ｒによって取り込まれた映像は、ステレオ画像処理装置２１０に入力され、図４に具体的に示す左右画像２０１Ｒ，２０１Ｌの視差情報（距離情報）からカラー画像（ＹＵＶ）２０２及び視差画像（ＹＤＲ）２０３が算出されてＣＰＵ２２０に入力される。また、ロボット装置１に複数個設けられた各センサからのセンサデータ２４０が入力される。図２に示すように、これらの視差情報及び視差画像からなる画像データ３０１と、ロボット装置の関節角等のデータであるセンサデータ３０２とがキネマティックオドメトリＫＩＮＥ３１０に入力される。
【００２１】
キネマティックオドメトリＫＩＮＥ３１０は、画像データ３０１及びセンサデータ３０２からなる入力データに基づいて画像データ３０１の画像が撮像された時間におけるセンサデータ３０２の関節角を割り出し、この関節角のデータを使用してロボット装置１が中心に固定されたロボット中心座標系を頭部ユニットに設けられたカメラの座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同時変換行列等を導出し、この同時変換行列３１１とこれに対応する視差画像３１２を障害物認識装置２２１（平面抽出部ＰＬＥＸ３２０の実行結果）へ出力する。
【００２２】
障害物認識装置２２１（平面抽出部ＰＬＥＸ３２０）は、同時変換行列３１１とこれに対応する視差画像３１２を受け取り、以下の図５に示す処理手順にしたがって障害物を認識する。
【００２３】
先ず、障害物認識装置２２１（平面抽出部ＰＬＥＸ３２０）の座標変換部２２４は同時変換行列３１１を、また距離画像生成部２２２は同時変換行列３１１に対応する視差画像３１２を受け取る（ステップＳ６１）。そして、距離画像生成部２２２は、視差画像３１２からレンズ歪みとステレオの取り付け誤差を吸収したキャリブレーションパラメータを利用して、画素毎にカメラ座標から見た３次元の位置データ（X,Y,Z）を距離画像として生成する（ステップＳ６２）。このとき各３次元データは、視差画像又は距離画像等の入力画像における信頼度等から得られる信頼度パラメータを個別に有し、この信頼度パラメータにより選別されて入力される。
【００２４】
そして、平面検出部２２３は、選別された３次元データ群からランダムにデータをサンプリングし、ハフ変換により平面を推定する。即ち、法線ベクトルの向きを（θ，φ）、原点からの距離をｄとしたときの平面パラメータ（θ，φ，ｄ）を算出し、平面パラメータを投票空間（θ，Ψ，ｄ）＝（θ，φｃｏｓθ，ｄ）に直接投票して平面を推定する。これにより、平面検出部２２３は、画像中の支配的な平面のパラメータを検出する（ステップＳ６３）。この平面のパラメータは図５に示すパラメータ空間（θ，φ）（投票空間）のヒストグラムにより検出される。パラメータの投票が小さいいと障害物、大きいと平面上のものと見なすことができる。
【００２５】
投票の際は、３次元データに付随する信頼度パラメータ又は平面パラメータの算出方法等により、１票の投票に対して異なる重み付けをして、投票値を異ならせると共に、投票値の分布から得られるピーク値を推定するために、ピーク値近傍における重み付き平均処理等をすることにより、信頼度の高いデータを推定することができる。そして、この平面パラメータを初期パラメータとしてイタレーションを行い、平面を決定することで、更に信頼度が高い平面を決定することができる。また、最終的に決定される平面を算出した３次元データに付随する信頼度パラメータ及びイタレーションにおける残差誤差等を使用して平面の信頼度を算出し、この平面信頼度を平面のデータと共に出力することにより、後段の処理が行い易くなる。このように、平面抽出は、投票、即ちヒストグラムに基づく確率密度関数の推定により３次元データから、その３次元データに含まれる支配的な平面のパラメータを決定する確率論的方法で行われる。こうして得られた平面のパラメータ使用すれば、元々画像から得られた距離の測定点が平面上からどれくらいの距離であるかが分かる。
【００２６】
次に、座標変換変換部２２４は、カメラ座標系の同時変換行列３１１から図７に示すようにロボットの足底接地面への変換を求める（ステップＳ６４）。これにより、カメラ座標系で表現される接地平面のパラメータの算出が行われる。そして、床面検出部２２５は、平面検出部２２３による前記ステップＳ６３での画像による平面の検出結果と、座標変換部２２４による前記ステップＳ６４での足底接地平面の照合から、画像中の平面パラメータから床面に相当するものを選択する（ステップＳ６５）。
【００２７】
次に、障害物認識部２２６は、床面検出部２２５が前記ステップＳ６５にて選択した平面パラメータを使って、元の距離画像から平面上に載っている点を選択する（ステップＳ６６）。これは以下に示す式１、式２を用いて、平面からの距離ｄがしきい値Ｄｔｈより小さい事を使って判断する。
【００２８】
【数１】
【数２】
図９には、しきい値Ｄｔｈを１cmとした範囲で選択された測定点（×印）を示す。この図９にて、黒く抜かれている点が平面と判断されなかった点である。
【００３１】
したがって、障害物認識部２２６は、前記ステップＳ６６にて平面上（床面）上に載っている点以外の点（床面上に無い点）を障害物としてステップＳ６７にて認識することができる。これらの判断結果は床平面上の点（ｘ、ｙ）とその高さｚで表現される。例えば、高さｚ＜０は、平面より凹んでいる点を示す。
【００３２】
これにより、高さがロボットより高い障害物点はそれをくぐることができるので障害物ではないという判断も可能とすることができる。
【００３３】
また、ロボットビュー（図９の（ａ））から得た床面抽出画像（図９の（ｂ））の高さｚを０（ｚ＝０）となるように座標変換すれば、図９の（ｃ）に示すように、平面上の２次元の位置で床か障害物かを表現することができる。
【００３４】
このように、障害物認識装置は、多数の測定点を使って平面の検出を行うために安定した平面を抽出できる。また、画像から得られる平面候補をロボットの姿勢から得られる床面パラメータと照合することで正しい平面を選択できる。また、障害物を認識するのではなく、実質的には床面を認識するので障害物の形状や大きさに因らない認識が可能である。また、床面からの距離で障害物を表現するので細かい段差や凹みまで検出することができる。またロボットの大きさを考慮して跨いだり、くぐったりする判断も容易である。さらに、2次元床面上の障害物表現となるので経路計画などに既存の移動ロボットで用いられる手法が適用可能であるし、3次元の表現より高速に演算できる。
【００３５】
次に、上述した図２に示すロボット装置１のソフトウェアについて詳細に説明する。図１０は、図２に示すソフトウェア３００の動作を示すフローチャートである。
【００３６】
図２に示すソフトウェア３００のキネマティックオドメトリＫＩＮＥ３１０には、上述したように、画像データ３０１及びセンサデータ３０２が入力される。この画像データ３０１は、カラー画像及びステレオカメラによる視差画像である。また、センサデータは、ロボット装置の関節角等のデータである。キネマティックオドメトリＫＩＮＥ３１０は、これらの入力データ３０１，３０２を受け取り、メモリに格納されていたそれまでの画像及びセンサデータを更新する（ステップＳ１０１）。
【００３７】
次に、これら画像データ３０１とセンサデータ３０２との時間的な対応を取る（ステップＳ１０２−１）。即ち、画像データ３０１の画像が撮像された時間におけるセンサデータ３０２の関節角を割り出す。次いで、この関節角のデータを使用してロボット装置１が中心に固定されたロボット中心座標系を頭部ユニットに設けられたカメラの座標系へ変換する（ステップＳ１０２−２）。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同時変換行列等を導出し、この同時変換行列３１１とこれに対応する画像データとを画像認識を行うオブジェクトへ送信する。即ち、同時変換行列３１１及びこれに対応する視差画像３１２を平面抽出部ＰＬＥＸ３２０へ出力し、同時変換行列３１１及びカラー画像３１３をランドマークセンサ部ＣＬＳ３４０へ出力する。
【００３８】
また、センサデータ３０２から得られる歩行パラメータと、足底センサを使用した歩数のカウント数とからロボット装置１の移動量を算出し、ロボット装置１のロボット装置中心座標系における移動量を算出する。以下、ロボット装置中心座標系の移動量をオドメトリともいう。このオドメトリ３１４を障害物グリッド算出部ＯＧ３３０及び絶対座標算出部ＬＺ３５０へ出力する。
【００３９】
平面抽出部ＰＬＥＸ３２０は、キネマティックオドメトリＫＩＮＥ３１０にて算出された同時変換行列３１１と、これに対応するステレオカメラから得られる視差画像３１２とが入力されると、それまでメモリに格納されていたこれらのデータを更新する（ステップＳ１０３）。そして、ステレオカメラのキャリブレーションパラメータ等を使用して視差画像３１２から３次元の位置データ（レンジデータ）を算出する（ステップＳ１０４−１）。次いで、このレンジデータからハフ変換等を使用して、例えば壁及びテーブル等の平面を除く平面を平面として抽出する。また、座標変換行列３１１からロボット装置１の足底が接地している平面との対応を取り、床面を選択し、床面上にない点、例えば所定の閾値よりも高い位置にあるもの等を障害物として床面からの距離を算出し、この障害物情報（obstacle）３２１を障害物グリッド算出部３３０に出力する（ステップS１０４−２）。
【００４０】
障害物グリッド算出部ＯＧ３３０では、キネマティックオドメトリＫＩＮＥ３１０にて算出されたオドメトリ３１４と、平面抽出部ＰＬＥＸ３２０にて算出された障害物の観測情報（障害物情報）３２１とが入力されると、メモリに格納されていたそれまでのデータを更新する（ステップＳ１０５）。そして、床面上に障害物があるか否かの確率を保持する障害物グリッドを確率的手法により更新する（ステップＳ１０６）。
【００４１】
この障害物グリッド算出部ＯＧ３３０は、ロボット装置１を中心とした例えば周囲４ｍの障害物情報、即ち、上述した環境地図と、ロボット装置１の向く方角を示す姿勢情報とを保持しており、上述の方法により、環境地図を更新し、この更新した認識結果（障害物情報３３１）を出力することにより、上位レイヤ、即ち、本実施の形態においては、経路計画決定部ＳＢＬ３６０にて障害物を回避する計画を作成することができる。
【００４２】
ランドマークセンサＣＬＳ３４０は、キネマティックオドメトリＫＩＮＥ３１０から同時変換行列３１１及びカラー画像３１３が入力されると、予めメモリ内に格納されていたこれらのデータを更新する（ステップＳ１０７）。そして、カラー画像３１３の画像処理を行って、予め認識しているカラーランドマークを検出する。このカラーランドマークのカラー画像３１３上での位置及び大きさをカメラ座標系での位置に変換する。更に、同時変換行列３１１を使用し、カラーランドマークのカメラ座標系での位置をロボット中心位置座標系における位置に変換し、ロボット中心位置座標系におけるカラーランドマーク位置の情報（カラーランドマーク相対位置情報）３４１を絶対座標算出部ＬＺ３５０に出力する（ステップＳ１０８）。
【００４３】
絶対座標算出部ＬＺ３５０は、キネマティックオドメトリＫＩＮＥ３１０からのオドメトリ３１４と、ランドマークセンサ部ＣＬＳ３４０からのカラーランドマーク相対位置情報３４１とが入力されると、予めメモリ内に格納されていたこれらのデータが更新される（ステップＳ１０９）。そして、絶対座標算出部ＬＺ３５０が予め認識しているカラーランドマークの絶対座標（ワールド座標系での位置）、カラーランドマーク相対位置情報３４１、及びオドメトリ３１４を使用し、確率的手法によりロボット装置の絶対座標（ワールド座標系での位置）を算出する。そして、この絶対座標位置３５１を経路計画決定部ＳＢＬ３６０に出力する。
【００４４】
経路計画決定部ＳＢＬ３６０は、障害物グリッド算出部ＯＧ３３０から障害物グリッド情報３３１が入力され、絶対座標算出部ＬＺ３５０から絶対座標位置３５１が入力されると、予めメモリに格納されていたこれらのデータが更新される（ステップＳ１１１）。そして、経路計画決定部ＳＢＬ３６０障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により、ロボット装置１の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部ＬＺ３５０から現在のロボット装置１の絶対座標を取得することにより、絶対座標系又はロボット装置中心のロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。即ち、入力データから状況に応じてロボット装置１が取るべき行動を決定し、その行動列を出力する（ステップＳ１１２）。
【００４５】
また、人間によるナビゲーションの場合には、障害物グリッド算出部ＯＧ３３０からロボット装置の周囲に存在する障害物に関する認識結果及び絶対座標算出部ＬＺ３５０からの現在のロボット装置の位置の絶対座標をユーザに提供し、このユーザからの入力に応じて動作コマンドを発行させる。
【００４６】
図１１は、上述のソフトウェアに入力されるデータの流れを模式的に示す図である。なお、図１１において、図１及び図２に示す構成と同一構成要素には同一符号を付してその詳細な説明は省略する。
【００４７】
顔検出部ＦＤＴ（ＦａｃｅＤｅｔｅｃｔｏｒ）３７１は、画像フレーム中から顔領域を検出するオブジェクトであり、カメラ等の画像入力装置からカラー画像２０２を受け取り、それを例えば９段階のスケール画像に縮小変換する。この全ての画像の中から顔に相当する矩形領域を探索する。重なりあった候補領域を削減して最終的に顔と判断された領域に関する位置、大きさ、及び特徴量等の情報３７２を出力して、顔識別部ＦＩ（ＦａｃｅＩｄｅｎｔｉｆｙ）３７７へ送る。
【００４８】
顔識別部ＦＩ３７７は、検出された顔画像を識別するオブジェクトであり、顔の領域を示す矩形領域画像からなる情報３７２を顔検出部ＦＤＴ３７１から受け取り、この顔画像がメモリに格納されている手持ちの人物辞書のうちでどの人物に相当するかを比較して人物の識別を行う。そして、顔検出部ＥＤＴ３７１から受け取った顔画像の顔画像領域の位置、大きさ情報と共に人物のＩＤ情報３７８をＤＩＬ３７９に出力する。
【００４９】
色認識部ＭＣＴ（ＭｕｌｔｉＣｏｌｏｒＴｒａｃｋｅｒ）３７３は、色認識を行うオブジェクトであり、カメラ等の画像入力装置からカラー画像２０２を受け取り、予め保有している複数のカラー・モデル情報に基づいて色領域を抽出し、連続した領域に分割する。色認識部ＭＣＴ３７３は、分割した各領域の位置、大きさ及び特徴量等の情報３７４をＤＩＬ３７９に出力する。
【００５０】
動き検出部ＭＤＴ（ＭｏｔｉｏｎＤｅｔｅｃｔｏｒ）３７５は、画像の中で動いている部分を検出する部分であり、検出した動き領域の情報３７６を距離情報付加部ＤＩＬ３７９に出力する。
【００５１】
距離情報付加部ＤＩＬ（ＤｉｓｔａｎｃｅＩｎｆｏｒｍａｔｉｏｎＬｉｎｋｅｒ）３７９は、入力された２次元の情報に距離情報を付加して３次元の情報を出力するオブジェクトであり、顔検出部ＦＩ３７７からのＩＤ情報３７８、色認識部ＭＣＴ３７３からの分割した各領域の位置、大きさ及び特徴量等の情報３７４、動き検出部ＭＤＴ３７５からの動き領域の情報３７６に距離情報を付加して３次元の情報３８０を短期記憶部ＳＴＭ（ＳｈｏｒｔＴｅｒｍＭｅｍｏｒｙ）３８１に出力する。
【００５２】
短期記憶部ＳＴＭ３８１は、ロボット装置１の外部環境に関する情報を比較的短い時間だけ保持するオブジェクトであり、ＡｒｔｈｕｒＤｅｃｏｄｅｒ（図示せず）から音声認識結果（単語、音源方向、確信度）を受け取り、色認識部ＭＣＴ３７３から肌色の領域の位置、大きさと顔領域の位置、大きさを受け取り、顔識別部ＦＩ３７７から人物のＩＤ情報等を受け取る。また、ロボット装置１の機体上の各センサからロボット装置の首の方向（関節角）を受け取る。そして、これらの認識結果及びセンサ出力を統合的に使って、現在どこにどの人物がいて、しゃべった言葉がどの人物のものであり、その人物とはこれまでにどんな対話を行ったのかという情報を保存する。こうした物体、即ちターゲットに関する物理情報と時間方向でみたイベント（履歴）を出力として、経路計画決定部（状況依存行動階層）（ＳＢＬ）３６０等の上位モジュールに渡す。
【００５３】
経路計画決定部ＳＢＬは、上述の短期記憶部ＳＴＭ３８１からの情報を基にロボット装置１の行動（状況に依存した行動）を決定するオブジェクトである。複数の行動を同時に評価したり、実行したりすることができる。また、行動を切り替えて機体をスリープ状態にしておき、別の行動を起動することができる。
【００５４】
次に、既に詳細に説明した平面抽出部ＰＬＥＸ３２０の障害物検出処理を除いた各オブジェクトについて更に詳細に説明する。
【００５５】
障害物グリッド算出部ＯＧ３３０は、所定の大きさのグリッドに分割された、ロボット中心座標系の地図情報である環境地図と、例えば環境地図上においてｘ軸方向又はｙ軸方向等の所定の方角からロボット装置が向く方角を示す姿勢情報とを有している。また、環境地図は、障害物情報が平面抽出部ＰＬＥＸから入力され、障害物として認識されているグリッド（障害物占有領域）を有する。この障害物グリッド算出部ＯＧ３３０は、ロボット装置が移動すると、即ち、オドメトリ３１４が入力されると、予めメモリ内に格納されていた環境地図及び姿勢情報を、ロボット装置の姿勢の変化（差分移動角）及び移動距離（差分移動量）に伴って、予め認識している環境地図及び姿勢方向の情報を更新する。ここで、差分移動量がグリッドサイズより小さければ環境地図を更新せず、移動量がグリッドのサイズを超える場合は環境地図を更新する。また、環境地図及びそのグリッドの大きさを必要に応じて適宜変更することにより、計算量及びメモリコピーコスト等を低減することができる。
【００５６】
以下、障害物グリッド算出部ＯＧ３３０について図１２〜図１７を参照して詳細に説明する。
図１２は、障害物グリッド算出部ＯＧ３３０にて認識される環境地図を示す模式図である。図１２に示すように、環境地図４００は、ロボット装置（移動体）４０３を中心とした周囲の地図情報である環境地図（ロボット中心座標系での地図情報）であり、所定の大きさのグリッド４１０に分割されている。そして、ロボット装置４０３が移動すると、常にロボット装置４０３が環境地図４００の中心に位置する中心グリッド４０１内に位置するように更新されるものである。この図１２に示す環境地図４００及びグリッド４１０の大きさは、必要に応じて適宜変更することができる。即ち、例えば、精細な環境地図が不要であるとき等は、環境地図４００を小さくするか、又はグリッド４１０の大きさを大きくして解像度を下げることにより、計算量及びメモリコピーコスト等を小さくすることができる。
【００５７】
また、図１２において、ハッチングして示すグリッド４０２は、障害物として認識される最小単位の領域（障害物占有領域）を示す。位置認識装置における障害物の認識方法としては、例えば、ステレオカメラ等による視差画像から求める方法、レンジファインダー等の電波の跳ね返りを利用する方法、及び超音波センサ等の距離測定センサによる方法等がある。そして、これらの方法により認識された障害物の位置を位置認識装置が環境地図のグリッドの属性として認識（表示）する。本実施の形態においては、障害物情報は、前述した平面抽出部ＰＬＥＸ３２０から入力される。
【００５８】
障害物グリッド算出部ＯＧ３３０は、上述したように、データ画像及びセンサデータが入力されるキネマティックオドメトリＫＩＮＥ３１０からオドメトリ３１４が入力されると、予めメモリ内に格納されていた環境地図を更新する。メモリ内には、ロボット装置４０３を中心としたロボット装置の周囲の障害物等の情報を有する地図情報である環境地図４００と共に、この地図上においてのロボット装置（移動体）の向きを示す姿勢情報４０４とを有するものである。姿勢情報４０４は、所定の方角、例えばｘ軸方向又はｙ軸方向からロボット装置４０３が向く方角を示すものである。そして、ロボット装置の姿勢の変化及び移動等に伴って、図１２に示す予め認識している環境地図及び姿勢方向の情報を更新する。
【００５９】
先ず、環境地図の更新方法について説明する。本発明の位置認識装置においては、ロボット装置等の移動量の大きさによっては環境地図を更新しない。即ち、移動量が所定の領域（グリッド）内であれば環境地図を更新せず、移動量がグリッドのサイズを超える場合は環境地図を更新するものである。
【００６０】
図１３（ａ），（ｂ）及び（ｃ），（ｄ）は移動量に応じた環境地図の更新方法を示す図であって、夫々環境地図を更新しない場合及び環境地図を更新する場合の環境地図を示す模式図である。図１３（ａ）に示すように、環境地図５００における中心グリッド５０１内にロボット装置５０３が位置している。ハッチングして示すグリッド５０２は障害物領域を示す。このロボット装置５０３の移動量５０５が小さく、図１３（ｂ）に示すように、ロボット装置５０３の位置が移動後のロボット装置５０６においても、そのロボット５０６の位置が中心グリッド５０１内であった場合、環境地図５００は更新しない。この場合は、中心グリッド５０１内におけるロボット装置の移動位置情報だけを更新する。
【００６１】
一方、図１３（ａ）と同様に、図１３（ｃ）に示すように、環境地図５００における中心グリッド５０１内に位置したロボット装置５０３の移動量５１５が大きく中心グリッド５０１を越えた場合、図１３（ｄ）に示すように、移動後のロボット装置５１３が環境地図の中心グリッド５１１に位置するように、移動したグリッドの数だけ環境地図５００をシフトさせて更新して更新環境地図５１０を得る。
【００６２】
即ち、障害物領域として認識される大きさの最小単位であるグリッドの大きさをＣＳ（Cell Size）、グリッド内のロボット装置の位置を（Ｂｘ，Ｂｙ）、ロボット装置の２次元方向の移動量を（ｄｘ，ｄｙ）とすると、グリッドのシフトの大きさ（Ｓｘ，Ｓｙ）は下記数式（３）で示される。
【００６３】
【数３】
ここで、上記数式（３）において、［］は、ガウス記号であって、［］内の値を超えない最大の整数値を示すものとする。また、グリッド内のロボット位置（Ｒｘ，Ｒｙ）は、下記数式（４）で示される。
【００６５】
【数４】
上記数式（３）及び数式（４）の値を図１４に示す。図１４（ａ）に示すように、ロボット装置６０３は、グリッドサイズＣＳを有する複数個のグリッドに分割された環境地図６００における中心グリッド６０１内に位置している。環境地図６００内にはハッチングして示す障害物領域を示す複数個のグリッド６０２を有する。ここで、中心グリッド６０１内の例えば図中左下隅の点６０７をグリッド６０１内のロボット装置６０３の位置原点とすることができる。このロボット装置６０３の位置は、原点６０７からの距離として位置（Ｂｘ，Ｂｙ）として表すことができる。このロボット装置６０３が移動量（ｄｘ，ｄｙ）６０５の移動をしてグリッド６０８に移動した場合、移動後のロボット装置６１３の位置は、（Ｂｘ＋ｄｘ，Ｂｙ＋ｄｙ）として表すことができる。
【００６７】
ロボット装置６０３が移動量（ｄｘ，ｄｙ）６０５の移動をした場合、環境地図シフト量（Ｓｘ，Ｓｙ）は、上記数式（１）から算出することができる。ここで、更新環境地図６１０において、環境地図６００をｘ軸方向のシフト量Ｓｘ及びｙ軸方向のシフト量Ｓｙだけ、夫々ｘ軸方向の移動量ｄｘ及びｙ軸方向の移動量ｄｙとは逆方向に環境地図をシフトさせたものとして更新することができる。なお、ここでは、ロボット装置の移動量（ｄｘ，ｄｙ）は右向きを正とし、環境地図のシフト量（Ｓｘ，Ｓｙ）は、移動方向とは逆方向である左向きを正としている。
【００６８】
こうして移動後のロボット装置６１３は、更新環境地図６１０の中心グリッド６１１に位置する。ここで、移動後のロボット装置６１３の環境地図シフト後の更新環境地図６１０の中心グリッド６１１におけるロボット位置（Ａｘ，Ａｙ）は、移動前のロボット装置６０３のシフト前環境地図６００の中心グリッド６０１におけるロボット位置（Ｂｘ，Ｂｙ）と、グリッド内移動量（Ｒｘ，Ｒｙ）６１５とからロボット位置（Ａｘ，Ａｙ）＝（Ｂｘ＋Ｒｘ，Ｂｙ＋Ｒｙ）＝（Ｂｘ＋ｄｘ−ＣＳ×Ｓｘ，Ｂｙ＋ｄｙ−ＣＳ×Ｓｙ）として算出することができる。
【００６９】
次に、ロボット装置の姿勢方向の情報の更新について説明する。図１５（ａ）及び（ｂ）は、姿勢方向の夫々更新前後の様子を示す模式図である。ロボット装置の姿勢情報に関しては、環境地図を固定した座標系に対する角度として示すことができる、即ち、ロボット装置が姿勢方向ｄαだけ変化させた場合、ロボット装置の姿勢方向の情報のみが更新され、環境地図は更新されない。
【００７０】
即ち、図１５（ａ）に示すように、障害物領域である複数個のグリッド７０２を有する環境地図７００における中心グリッド７０１に位置するロボット装置７０３において、移動前のロボット姿勢方向をαとする。姿勢方向αは、例えば時計回りを正とし、ｙ軸からの傾き又はｘ軸からの傾き等として示すことができる。ここで、ロボット装置７０３が姿勢回転角度ｄαで回転した場合、図１５（ｂ）に示すように、移動後後のロボット姿勢方向（姿勢情報）は、α＋ｄαとして示すことができる。
【００７１】
図１６は、障害物グリッド算出部ＯＧ３３０のもたらす効果を示す図である。図１６に示すように、環境地図及びロボット装置自身の姿勢方向の情報の更新、即ちキネマティックオドメトリＫＩＮＥ８０１から障害物グリッド算出部８０２への移動量（オドメトリ）８０３の受け渡しは、所定の時間毎に自動的に行うこともできるが、ロボット装置が１歩歩く毎に行うようにする。このことにより、移動していないときは更新作業をしないようにし、更新作業を行うことによる計算コスト及びメモリコピーコストをなくすことでロボット装置の例えばＣＰＵ等の負荷を低減することができる。また、ソフトウェアの構成がオブジェクト単位で処理を行うような場合は、受け渡すデータは、ロボット装置の移動差分量・回転差分量（ｄｘ，ｄｙ，ｄα）のみとでよく、ロボット装置の絶対座標及び絶対角度を受け渡すよりもデータ量が少なく、且つ高い精度を得ることができる。
【００７２】
また、エンタテーメントロボット装置が位置認識以外の処理を行う場合、例えば音声認識及び顔認識等、位置認識と直接関係がなく、位置認識のプライオリティを下げてもよい場合がある。このような場合において、位置認識装置（位置認識のオブジェクト）を停止すると、位置認識のプライオリティが高くなった際に、停止していた間に移動があったりする等して、停止前の位置認識結果と整合が取れなくなるという問題（ＫｉｄＮａｐ問題）が生じる。従って、位置認識のプライオリティを下げてよい場合においては、環境地図のグリッドの大きさＣＳを大きくするか、又は環境地図サイズの大きさ自身を小さくするかすることにより、位置認識のオブジェクトを停止することなく、ＣＰＵのリソースを低減することができる。
【００７３】
更に、以上説明したような処理で得られる環境地図及びロボット装置の姿勢情報を図１７に示す操作盤に表示することができる。図１７は、例えばユーザがロボット装置の遠隔操作時に使用する操作盤を示す模式図である。図１７に示すように、操作盤９００には、ロボット装置のカメラ画像９０１及び環境地図９１０が表示される。操作盤９００には、その他、例えばロボット装置９１３を移動させる操作ボタン９２０等が備えられている。
【００７４】
移動ロボット装置の重要な利用方法としては、自律動作以外にユーザが遠隔操作をする利用方法がある。従来、ロボットの操作盤にロボット装置に装備されたカメラによって得られる画像を表示するものがある。しかしながら、特に、視野の限られたカメラだけではユーザがロボット装置周辺の環境を把握してロボット装置を操作することは困難であった。しかし、本実施の形態のように、ロボット装置９１３に装備されたＣＣＤ等のカメラからのカメラ画像９０１のみのデータで遠隔操作する従来のロボット制御よりも、遠隔操作用の操作盤９００に環境地図９１０を表示することによって、ロボット制御の操作性が飛躍的に向上する。
【００７５】
ここで、ロボットの姿勢情報が変化しても環境地図を更新しないものとして説明したが、通常、ロボット装置においてはＣＣＤカメラ等の撮像装置は頭部ユニット前面に設けられており、カメラ画像９０１はロボット装置の前方の画像を表示する。従って、姿勢情報が変化してロボット装置の向きが例えば１８０°回転した場合、カメラ画像と環境地図との方向が逆になる。よって、姿勢情報が、例えば４５°又は９０°等、所定の角度以上回転した場合は、この回転に伴い環境地図を回転させることにより、更にロボット制御の操作性が向上する。
【００７６】
次に、ランドマーク位置検出部ＣＬＳ３４０は、図１８に示すように、例えば、グリーン部分１００１、ピンク部分１００２、又はブルー部分１００３等のカラーを有する人工的なカラーランドマーク１００４を含む環境において、ロボット装置１のセンサ情報と、ロボット装置１が行った動作情報によって、ロボット装置の自己位置（位置及び姿勢）を同定する。
【００７７】
例えば、２次元的な作業空間上では、略等間隔状にグリッド（ｘ，ｙ）を設け、各グリッドの位置ｌ（localization）毎にロボット装置が存在する確率ｐ（ｌ）を管理する。この存在確率ｐ（ｌ）は、ロボット装置の移動（ｍｏｖｅ）、即ち内界観測情報ａ、又はランドマークの観測、即ち外界観測情報ｓの入力に応じて更新される。
【００７８】
存在確率ｐ（ｌ）は、ロボット装置の以前の状態、即ち自己位置ｌ'における存在確率ｐ（ｌ'）と、以前の状態ｌ'において移動ａを行ったときに状態ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）に依拠する。即ち、これまでの各状態ｌ'になる確率ｐ（ｌ'）と、状態ｌ'において移動ａを行ったときに状態ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）の積を加算していく（若しくは積分する）ことによって、現在の状態、即ち自己位置ｌになるという存在確率ｐ（ｌ）に収束する。従って、外界観測結果としてのロボット装置の移動ａが観測されたときに
は、各グリッドでは、下記数式（５）に従ってロボット装置の存在確率ｐ（ｌ）を更新することができる。
【００７９】
【数５】
また、状態、即ち自己位置ｌにロボット装置が存在するという存在確率ｐ（ｌ）は、存在確率ｐ（ｌ）とこの状態ｌにおいてランドマークを観察するという遷移確率ｐ（ｓ｜ｌ）に依拠する。したがって、状態ｌにおいてランドマークの観測、即ち外界観測情報ｓが入力された場合には、下式に従ってロボット装置の存在確率ｐ（ｌ）を更新することができる。但し、下記数式（４）に示すように右辺では、正規化のため、ランドマークを観察するという確率ｐ（ｓ）で除して正規化している。
【００８１】
【数６】
図１９は、マルコフ・ローカリゼーションと拡張カルマン・フィルタを併用したランドマークセンサＣＬＳ（自己位置同定システム）３４０の機能構成を模式的に示している。同図に示すように、このランドマークセンサＣＬＳ３４０は、マルコフ・ローカリゼーション部（ＭＬ）３４２と、拡張カルマン・フィルタ部（ＥＫＬ）３４３と、拡張カルマン・フィルタ部３４３の動作を制御するＥＫＬ制御部３４４とで構成されている。
【００８３】
マルコフ・ローカリゼーション部３４２は、作業空間内の自分の位置を離散的なグリッド上の自己位置確率密度分布として保持し、ランドマークの観測に関する外界観測情報ｓと、ロボット装置自身の動作に関する内界観測情報ａを入力して、自己位置確率密度分布の更新を行う。そして、各時刻において、自己位置確率密度分布の最も高い値を持つグリッドを自己位置の推定結果としてＥＫＬ制御部３４４に出力する。
【００８４】
図２０は、マルコフ・ローカリゼーション部３４２により求められた各グリッド上での自己位置確率密度分布を表している。同図では、各グリッドにおける確率密度を濃淡により表現している。最も色が濃い、即ち自己位置確率密度分布が最も高い値を有するグリッドが自己位置の推定結果となる。
【００８５】
このマルコフ・ローカリゼーションによる自己位置同定は、センサのノイズに対してロバストであり、同定解の精度は粗いが解の収束速度が速いことを主な特徴とする。
【００８６】
一方、図１９に示す拡張カルマン・フィルタ部３４３は、自己位置を状態変数［ｘ，ｙ，θ］を実測値として保持し、環境内に設置されたカラーランドマーク１００４を観測すると、ランドマークからの相対位置に基づいて自己位置を推定する。また、自身の動作情報を観測すると、その動作情報に基づいて状態量の推定を行う。
【００８７】
拡張カルマン・フィルタ部３４３は、ロボット装置自身の動作情報ａと状態、即ち、自己位置ｌとの関係を規定した状態モデルと、自己位置ｌとランドマークの観測情報ｓとの関係を規定した観測モデルとで構成されている。
【００８８】
状態モデルは、ロボット装置が状態（自己位置）ｌにおいて動作ａを行った場合における理論的な状態ｌを与える遷移関数Ｆ（ｌ，ａ）を備えている。実際には理論的な状態ｌに対してノイズ成分ｗが重畳されることから、ロボット装置の状態ｌは状態モデルにより下記数式（７）のように収束する。
【００８９】
【数７】
また、観測モデルは、ロボット装置が状態、即ち自己位置ｉにおいて、ある既知の環境Ｅｎｖ（例えばランドマークの位置など）に関する観測の理論値ｓを与える観測関数Ｈ（Ｅｎｖ，ｌ）を備えている。実際には観測の理論値に対してノイズ成分ｖが重畳されることから、観測値ｓは観測モデルにより下記数式（８）のように収束する。
【００９１】
【数８】
なお、状態ｌ並びに観測ｓに重畳されるそれぞれのノイズｗ及びｖは、ここではゼロを中央値とするガウス分布として仮定する。
【００９３】
ロボット装置自身の動作情報ａと自己位置ｌとの関係を規定した状態モデルと自己位置ｌとランドマークの観測情報ｓとの関係を規定した観測モデルを備えた拡張カルマン・フィルタ部３４３において、動作情報ａは内界観測結果として、ランドマークの観測情報ｓは外界観測結果として、夫々既知である。従って、ロボット装置の自己位置同定を、ロボット装置の状態ｌをロボット装置の動作情報ａ及び観測情報ｓにより推定するという問題に帰着することができる。ここでは、ロボット装置の動作ａ、状態ｌ、及び観測ｓをそれぞれ下記に示すガウス分布として表現することができる。
【００９４】
【数９】
ある時点でのロボット装置の状態ｌがある中央値と及び共分散を有するガウス分布をとるものとして推定する。そして、ロボット装置の動作ａが観測されたときには、状態ｌの推定値に関する中央値並びに共分散を下記数式（９−１），（９−２）により更新することができる。
【００９６】
【数１０】
ここで、▽Ｆｌ，▽Ｆａは、下記とする。
【００９８】
【数１１】
同様に、ある時点でのロボット装置の状態ｌがある中央値及び共分散を有するガウス分布をとるものとして推定する。そして、ランドマークの観測情報ｓが観測されたときには、状態ｌの推定値に関する中央値並びに共分散を下記数式（１０−１），（１０−２）により更新することができる。
【０１００】
【数１２】
ここで、各パラメータは下記とする。
【０１０２】
【数１３】
拡張カルマン・フィルタ３４３はセンサ情報に対するロバスト性に優れているので、拡張カルマン・フィルタ部３４３の推定結果は、ランドマークセンサＣＬＳ３４０全体の出力としている。
【０１０４】
ＥＫＬ制御部３４４は、マルコフ・ローカリゼーション部３４２の出力結果に応じて拡張カルマン・フィルタ部３４４の動作を制御する。より具体的には、マルコフ・ローカリゼーション部３４２の自己位置推定結果に基づいて、ランドマークの観測情報ｓに関する妥当性を検証する。観測情報ｓの妥当性は、マルコフ・ローカリゼーション部３４２において最大の存在確率となるグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈｒｅｓｈobsを越えたかどうかで判断することができる。
【０１０５】
グリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈobsを下回る場合には、センサ・ノイズに対してロバストなマルコフ・ローカリゼーション部３４２においてさえ、センサ・ノイズのために同定解が充分に収束していないことが推測される。このような場合、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３において自己位置を推定しても、精度が高い解が得られず、むしろ演算時間を浪費するだけである。このため、観測情報ｓが妥当でないと判断された場合には、切換器３４５を用いて拡張カルマン・フィルタ部３４３への外界観測情報ｓ、即ちランドマークの観測情報の入力を遮断して、拡張カルマン・フィルタ部３４３における自己位置推定値の更新を停止させる。
【０１０６】
また、ＥＫＬ制御部３４４は、拡張カルマン・フィルタ部３４３の自己位置推定結果の妥当性も検証する。自己位置推定結果の妥当性は、推定された状態ｌの中央値及び共分散を用いて、マルコフ・ローカリゼーション部３４２から出力される存在確率ｐ（ｌ）との分布比較テストにより判断することができる。分布比較テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。
【０１０７】
分布比較テストによりマルコフ・ローカリゼーション部３４２と拡張カルマン・フィルタ部３４３との各確率分布が類似していない場合には、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３における自己位置推定値の方が、センサ・ノイズの影響により妥当でないと判断することができる。このような場合、ＥＫＬ制御部３４４は拡張カルマン・フィルタ部３４３の再初期化を行わせる。何故ならば、拡張カルマン・フィルタは再復帰に多大な時間を要するからである。
【０１０８】
次に、ランドマークセンサＣＬＳ３４０の動作について説明する。図２１は、ランドマークセンサＣＬＳ３４０の動作を示すフローチャートである。図２１に示すように、ロボット装置１の移動に関する内界観測情報ａがランドマークセンサＣＬＳ３４０に入力されると、まずマルコフ・ローカリゼーション部３４２において、上記数式（５）を用いて自己位置推定値の更新処理が行われる（ステップＳ２０１）。次いで、拡張カルマン・フィルタ部３４３において、上記数式数（９）を用いて自己位置推定値の更新処理が行われる（ステップＳ２０２）。
【０１０９】
また、ランドマークの観測に関する外界観測情報ｓがランドマークセンサＣＬＳ３４０に入力されると、まずマルコフ・ローカリゼーション部３４２において、上記数式（６）を用いて、自己位置推定値の更新処理が行われる（ステップＳ２１１）。
【０１１０】
マルコフ・ローカリゼーション部３４２の出力結果は、ＥＫＬ制御部３４４に入力されて、観測情報ｓの妥当性が検証される（ステップＳ２１２）。観測情報ｓの妥当性は、マルコフ・ローカリゼーション部３４２において最大の存在確率となるグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈｒｅｓｈobsを越えたかどうかで判断することができる。
【０１１１】
グリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈobsを下回る場合には、センサ・ノイズに対してロバストなマルコフ・ローカリゼーション部３４２においてさえ、センサ・ノイズのために同定解が充分に収束していないことが推測される。このような場合、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３において自己位置を推定しても、精度のよい解が得られず、むしろ演算時間を浪費するだけである。このため、観測情報ｓが妥当でないと判断された場合には、切換器３４５を用いて拡張カルマン・フィルタ部３４３への外界観測情報ｓ、即ちランドマークの観測情報の入力を遮断して、拡張カルマン・フィルタ部３４３における自己位置推定値の更新を停止させる。
【０１１２】
他方、観測情報ｓを検証した結果、妥当性を満たす、即ちグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈobsを上回る場合には、さらに拡張カルマン・フィルタ部３４３において、上記数式（１０−１），（１０−２）を用いて更新処理が行われる（ステップＳ２１３）。
【０１１３】
拡張カルマン・フィルタ部３４３による自己位置推定の結果は、ＥＫＬ制御部３４４に入力されて、その妥当性が検証される（ステップＳ２１４）。拡張カルマン・フィルタ部３４３による自己位置推定結果の妥当性は、推定された状態ｌの中央値及び共分散を用いて、マルコフ・ローカリゼーション部３４２から出力される存在確率ｐ（ｌ）との分布比較テストにより判断することができる。分布比較テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。
【０１１４】
分布比較テストによりマルコフ・ローカリゼーション部３４２と拡張カルマン・フィルタ部３４３それぞれの確率分布が類似していない場合には、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３における自己位置推定値の方が、センサ・ノイズの影響により妥当でないと判断することができる。このような場合、ＥＫＬ制御部３４４は拡張カルマン・フィルタ部３４３の再初期化を行わせる（ステップＳ２１５）。何故ならば、拡張カルマン・フィルタは再復帰に多大な時間を要するからである。
【０１１５】
このようにして、ランドマークセンサＣＬＳ３４０では、広い範囲で比較的短い探索時間で探索を行う大域探索と高精度だが探索時間を要する局所的探索とを併用して、高精度で、高速且つロバストな自己位置同定を行うことができる。
次に、行動決定部ＳＢＬ３６０は、障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により、ロボット装置１の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部ＬＺ３５０から現在のロボット装置１の絶対座標を取得することにより、絶対座標系又はロボット装置中心のロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。即ち、入力データから状況に応じてロボット装置１が取るべき行動を決定し、その行動列を出力する。ここで、障害物グリッド算出部ＯＧ３３０の、障害物情報３３１に基づいた処理については後述する。
【０１１６】
障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により生成される障害物地図上の点は、図２２に示すように以下の３つの種類に分類される。
【０１１７】
第１の点は、障害物の存在する点（図中の黒色で表される点）である。第２の点は、自由空間（障害物が存在しない空間）上の点（図中の白で表される点）である。そして、第３の点は、未観測領域上の点（図中の斜線で表される点）である。
【０１１８】
次に、前記経路計画決定部ＳＢＬ３６０が採用する経路計画アルゴリズムを図２３のフローチャートに示し、以下に詳細を説明する。
先ず、現在位置から目的位置までを結ぶ直線経路の周辺の障害物地図が作成されるように、視線を目的地方向に向ける（ステップＳ７１）。そして距離画像を観測し、距離計測を行い、障害物地図を作成（更新）する（ステップＳ７２）。
【０１１９】
次に、生成された障害物地図において、未観測領域と自由空間領域を移動可能領域とみなして経路計画を行う（ステップＳ７３）。
【０１２０】
この経路計画としては、例えば経路全体のコストを最小化するＡ＊探索（Ａ＊search)いう方法を用いる。このＡ＊探索は、評価関数としてｆを用い、ｈ関数が許容的であるような最良優先探索である。いかなるヒューリスティック関数に対しても効率最適という点を用いている。
【０１２１】
このステップＳ７３にて例えばＡ＊探索を適用することにより生成された経路が移動可能であるな否かをステップＳ７４にてチェックし、障害物を回避することのできる経路が計画できなかった場合（ＮＯ）には、これ以上観測を続けても移動可能な経路が得られる可能性がないため、その旨を告げて経路計画を終了する（ステップＳ７５）。
【０１２２】
ステップＳ７３にて移動可能経路が例えば前記Ａ＊探索を適用して計画できた場合（ＹＥＳ）には、ステップＳ７６に進み、出力された経路上に未観測領域が含まれるか否かを検索する。このステップＳ７６にて経路上に未観測領域が含まれなかった場合（ＮＯ）には、ステップＳ７７にて目的地までの経路計画として移動可能経路を出力する。ステップＳ７６にて未観測領域が含まれる場合（ＹＥＳ）には、ステップＳ７８に進んで現在位置から未観測領域までの歩数を計算し、その歩数が閾値を超えるか否かをチェックする。
【０１２３】
ステップＳ７８において歩数が閾値を超える場合（ＹＥＳ）には、ステップＳ７９により未知領域までの移動可能経路を出力する。一方、ステップＳ７８において未知観測領域までの歩数が閾値に満たない場合（ＮＯ）は、未観測領域が距離観測されるように視線方向の制御を行って再観測を行い（ステップＳ８０）、再び障害物地図の更新を行う。
【０１２４】
以上のような経路計画アルゴリズムを採用した経路計画決定部ＳＢＬ３６０は、未観測領域と自由空間領域を移動可能領域とみなして経路計画を行い、出力された経路上に含まれる未観測領域部のみを再観測することにより、目的地に移動する際に不必要な観測及び距離画像計算処理を行わなわず、効率的で短時間に移動経路計画を生成することが可能となる。
【０１２５】
以下、本発明の実施の形態における上述した位置認識装置を搭載した２足歩行タイプのロボット装置について詳細に説明する。この人間型のロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボットである。
【０１２６】
図２３に示すように、ロボット装置１は、体幹部ユニット２の所定の位置に頭部ユニット３が連結されると共に、左右２つの腕部ユニット４Ｒ／Ｌと、左右２つの脚部ユニット５Ｒ／Ｌが連結されて構成されている（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下において同じ。）。
【０１２７】
このロボット装置１が具備する関節自由度構成を図２５に模式的に示す。頭部ユニット３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。
【０１２８】
また、上肢を構成する各々の腕部ユニット４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール輪１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書ではゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。
【０１２９】
また、体幹部ユニット２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。
【０１３０】
また、下肢を構成する各々の脚部ユニット５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足部１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置１の股関節位置を定義する。人体の足部１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、ロボット装置１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
【０１３１】
以上を総括すれば、ロボット装置１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度、即ち関節数を適宜増減することができることはいうまでもない。
【０１３２】
上述したようなロボット装置１がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行うことなどの要請から、アクチュエータは小型且つ軽量であることが好ましい。
【０１３３】
図２６には、ロボット装置１の制御システム構成を模式的に示している。同図に示すように、ロボット装置１は、ヒトの四肢を表現した体幹部ユニット２，頭部ユニット３，腕部ユニット４Ｒ／Ｌ，脚部ユニット５Ｒ／Ｌと、各ユニット間の協調動作を実現するための適応制御を行う制御ユニット１０とで構成される。
【０１３４】
ロボット装置１全体の動作は、制御ユニット１０によって統括的に制御される。制御ユニット１０は、ＣＰＵ（Central Processing Unit）や、ＤＲＡＭ、フラッシュＲＯＭ等の主要回路コンポーネント（図示しない）で構成される主制御部１１と、電源回路やロボット装置１の各構成要素とのデータやコマンドの授受を行うインターフェイス（何れも図示しない）などを含んだ周辺回路１２とで構成される。
【０１３５】
この制御ユニット１０の設置場所は、特に限定されない。図２６では体幹部ユニット２に搭載されているが、頭部ユニット３に搭載してもよい。あるいは、ロボット装置１外に制御ユニット１０を配備して、ロボット装置１の機体とは有線又は無線で交信するようにしてもよい。
【０１３６】
図２６に示したロボット装置１内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。即ち、頭部ユニット３には、首関節ヨー軸１０１、首関節ピッチ軸１０２、首関節ロール軸１０３の各々を表現する首関節ヨー軸アクチュエータＡ２、首関節ピッチ軸アクチュエータＡ３、首関節ロール軸アクチュエータＡ４が配設されている。
【０１３７】
また、頭部ユニット３には、外部の状況を撮像するためのＣＣＤ（Charge Coupled Device）カメラが設けられているほか、前方に位置する物体までの距離を測定するための距離センサ、外部音を集音するためのマイク、音声を出力するためのスピーカ、使用者からの「撫でる」や「叩く」といった物理的な働きかけにより受けた圧力を検出するためのタッチセンサ等が配設されている。
【０１３８】
また、体幹部ユニット２には、体幹ピッチ軸１０４、体幹ロール軸１０５、体幹ヨー軸１０６の各々を表現する体幹ピッチ軸アクチュエータＡ５、体幹ロール軸アクチュエータＡ６、体幹ヨー軸アクチュエータＡ７が配設されている。また、体幹部ユニット２には、このロボット装置１の起動電源となるバッテリを備えている。このバッテリは、充放電可能な電池によって構成されている。
【０１３９】
また、腕部ユニット４Ｒ／Ｌは、上腕ユニット４１Ｒ／Ｌと、肘関節ユニット４２Ｒ／Ｌと、前腕ユニット４３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸１０７、肩関節ロール軸１０８、上腕ヨー軸１０９、肘関節ピッチ軸１１０、前腕ヨー軸１１１、手首関節ピッチ軸１１２、手首関節ロール軸１１３の各々表現する肩関節ピッチ軸アクチュエータＡ８、肩関節ロール軸アクチュエータＡ９、上腕ヨー軸アクチュエータＡ１０、肘関節ピッチ軸アクチュエータＡ１１、肘関節ロール軸アクチュエータＡ１２、手首関節ピッチ軸アクチュエータＡ１３、手首関節ロール軸アクチュエータＡ１４が配備されている。
【０１４０】
また、脚部ユニット５Ｒ／Ｌは、大腿部ユニット５１Ｒ／Ｌと、膝ユニット５２Ｒ／Ｌと、脛部ユニット５３Ｒ／Ｌに細分化されるが、股関節ヨー軸１１５、股関節ピッチ軸１１６、股関節ロール軸１１７、膝関節ピッチ軸１１８、足首関節ピッチ軸１１９、足首関節ロール軸１２０の各々を表現する股関節ヨー軸アクチュエータＡ１６、股関節ピッチ軸アクチュエータＡ１７、股関節ロール軸アクチュエータＡ１８、膝関節ピッチ軸アクチュエータＡ１９、足首関節ピッチ軸アクチュエータＡ２０、足首関節ロール軸アクチュエータＡ２１が配備されている。各関節に用いられるアクチュエータＡ２，Ａ３・・・は、より好ましくは、ギア直結型で旦つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ＡＣサーボ・アクチュエータで構成することができる。
【０１４１】
体幹部ユニット２、頭部ユニット３、各腕部ユニット４Ｒ／Ｌ、各脚部ユニット５Ｒ／Ｌなどの各機構ユニット毎に、アクチュエータ駆動制御部の副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌが配備されている。さらに、各脚部ユニット５Ｒ／Ｌの足底が着床したか否かを検出する接地確認センサ３０Ｒ／Ｌを装着するとともに、体幹部ユニット２内には、姿勢を計測する姿勢センサ３１を装備している。
【０１４２】
接地確認センサ３０Ｒ／Ｌは、例えば足底に設置された近接センサ又はマイクロ・スイッチなどで構成される。また、姿勢センサ３１は、例えば、加速度センサとジャイロ・センサの組み合わせによって構成される。
【０１４３】
接地確認センサ３０Ｒ／Ｌの出力によって、歩行・走行などの動作期間中において、左右の各脚部が現在立脚又は遊脚何れの状態であるかを判別することができる。また、姿勢センサ３１の出力により、体幹部分の傾きや姿勢を検出することができる。
【０１４４】
主制御部１１は、各センサ３０Ｒ／Ｌ，３１の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌの各々に対して適応的な制御を行い、ロボット装置１の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現できる。
【０１４５】
ロボット装置１の機体上での全身運動は、足部運動、ＺＭＰ（Zero Moment Point）軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容にしたがった動作を指示するコマンドを各副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌに転送する。そして、各々の副制御部２０，２１，・・・等では、主制御部１１からの受信コマンドを解釈して、各アクチュエータＡ２，Ａ３・・・等に対して駆動制御信号を出力する。ここでいう「ＺＭＰ」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ＺＭＰ軌道」とは、例えばロボット装置１の歩行動作期間中にＺＭＰが動く軌跡を意味する。なお、ＺＭＰの概念並びにＺＭＰを歩行ロボットの安定度判別規範に適用する点については、Miomir Vukobratovic著“LEGGED LOCOMOTION ROBOTS”（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））に記載されている。
【０１４６】
以上のように、ロボット装置１は、各々の副制御部２０，２１，・・・等が、主制御部１１からの受信コマンドを解釈して、各アクチュエータＡ２，Ａ３・・・に対して駆動制御信号を出力し、各ユニットの駆動を制御している。これにより、ロボット装置１は、目標の姿勢に安定して遷移し、安定した姿勢で歩行できる。
【０１４７】
また、ロボット装置１における制御ユニット１０では、上述したような姿勢制御のほかに、加速度センサ、タッチセンサ、接地確認センサ等の各種センサ、及びＣＣＤカメラからの画像情報、マイクからの音声情報等を統括して処理している。制御ユニット１０では、図示しないが加速度センサ、ジャイロ・センサ、タッチセンサ、距離センサ、マイク、スピーカなどの各種センサ、各アクチュエータ、ＣＣＤカメラ及びバッテリが各々対応するハブを介して主制御部１１と接続されている。
【０１４８】
主制御部１１は、上述の各センサから供給されるセンサデータや画像データ及び音声データを順次取り込み、これらをそれぞれ内部インターフェイスを介してＤＲＡＭ内の所定位置に順次格納する。また、主制御部１１は、バッテリから供給されるバッテリ残量を表すバッテリ残量データを順次取り込み、これをＤＲＡＭ内の所定位置に格納する。ＤＲＡＭに格納された各センサデータ、画像データ、音声データ及びバッテリ残量データは、主制御部１１がこのロボット装置１の動作制御を行う際に利用される。
【０１４９】
主制御部１１は、ロボット装置１の電源が投入された初期時、制御プログラムを読み出し、これをＤＲＡＭに格納する。また、主制御部１１は、上述のように主制御部１１よりＤＲＡＭに順次格納される各センサデータ、画像データ、音声データ及びバッテリ残量データに基づいて自己及び周囲の状況や、使用者からの指示及び働きかけの有無などを判断する。
【０１５０】
さらに、主制御部１１は、この判断結果及びＤＲＡＭに格納した制御プログラムに基づいて自己の状況に応じて行動を決定するとともに、当該決定結果に基づいて必要なアクチュエータを駆動させることによりロボット装置１に、いわゆる「身振り」、「手振り」といった行動をとらせる。
【０１５１】
このようにしてロボット装置１は、制御プログラムに基づいて自己及び周囲の状況を判断し、使用者からの指示及び働きかけに応じて自律的に行動できる。
【０１５２】
ところで、このロボット装置１は、内部状態に応じて自律的に行動することができる。そこで、ロボット装置１における制御プログラムのソフトウェア構成例について、図２７乃至図３２を用いて説明する。なお、この制御プログラムは、上述したように、予めフラッシュＲＯＭ１２に格納されており、ロボット装置１の電源投入初期時において読み出される。
【０１５３】
図２７において、デバイス・ドライバ・レイヤ４０は、制御プログラムの最下位層に位置し、複数のデバイス・ドライバからなるデバイス・ドライバ・セット４１から構成されている。この場合、各デバイス・ドライバは、ＣＣＤカメラやタイマ等の通常のコンピュータで用いられるハードウェアに直接アクセスすることを許されたオブジェクトであり、対応するハードウェアからの割り込みを受けて処理を行う。
【０１５４】
また、ロボティック・サーバ・オブジェクト４２は、デバイス・ドライバ・レイヤ４０の最下位層に位置し、例えば上述の各種センサやアクチュエータ２８１〜２８ｎ等のハードウェアにアクセスするためのインターフェイスを提供するソフトウェア群でなるバーチャル・ロボット４３と、電源の切換えなどを管理するソフトウェア群でなるパワーマネージャ４４と、他の種々のデバイス・ドライバを管理するソフトウェア群でなるデバイス・ドライバ・マネージャ４５と、ロボット装置１の機構を管理するソフトウェア群でなるデザインド・ロボット４６とから構成されている。
【０１５５】
マネージャ・オブジェクト４７は、オブジェクト・マネージャ４８及びサービス・マネージャ４９から構成されている。オブジェクト・マネージャ４８は、ロボティック・サーバ・オブジェクト４２、ミドル・ウェア・レイヤ５０、及びアプリケーション・レイヤ５１に含まれる各ソフトウェア群の起動や終了を管理するソフトウェア群であり、サービス・マネージャ４９は、メモリカードに格納されたコネクションファイルに記述されている各オブジェクト間の接続情報に基づいて各オブジェクトの接続を管理するソフトウェア群である。
【０１５６】
ミドル・ウェア・レイヤ５０は、ロボティック・サーバ・オブジェクト４２の上位層に位置し、画像処理や音声処理などのこのロボット装置１の基本的な機能を提供するソフトウェア群から構成されている。また、アプリケーション・レイヤ５１は、ミドル・ウェア・レイヤ５０の上位層に位置し、当該ミドル・ウェア・レイヤ５０を構成する各ソフトウェア群によって処理された処理結果に基づいてロボット装置１の行動を決定するためのソフトウェア群から構成されている。
【０１５７】
なお、ミドル・ウェア・レイヤ５０及びアプリケーション・レイヤ５１の具体なソフトウェア構成をそれぞれ図２８に示す。
【０１５８】
ミドル・ウェア・レイヤ５０は、図２８に示すように、騒音検出用、温度検出用、明るさ検出用、音階認識用、距離検出用、姿勢検出用、タッチセンサ用、動き検出用及び色認識用の各信号処理モジュール６０〜６８並びに入力セマンティクスコンバータモジュール６９などを有する認識系７０と、出力セマンティクスコンバータモジュール７８並びに姿勢管理用、トラッキング用、モーション再生用、歩行用、転倒復帰用、ＬＥＤ点灯用及び音再生用の各信号処理モジュール７１〜７７などを有する出力系７９とから構成されている。
【０１５９】
認識系７０の各信号処理モジュール６０〜６８は、ロボティック・サーバ・オブジェクト４２のバーチャル・ロボット４３によりＤＲＡＭから読み出される各センサデータや画像データ及び音声データのうちの対応するデータを取り込み、当該データに基づいて所定の処理を施して、処理結果を入力セマンティクスコンバータモジュール６９に与える。ここで、例えば、バーチャル・ロボット４３は、所定の通信規約によって、信号の授受或いは変換をする部分として構成されている。
【０１６０】
入力セマンティクスコンバータモジュール６９は、これら各信号処理モジュール６０〜６８から与えられる処理結果に基づいて、「うるさい」、「暑い」、「明るい」、「ボールを検出した」、「転倒を検出した」、「撫でられた」、「叩かれた」、「ドミソの音階が聞こえた」、「動く物体を検出した」又は「障害物を検出した」などの自己及び周囲の状況や、使用者からの指令及び働きかけを認識し、認識結果をアプリケーション・レイヤ４１に出力する。
【０１６１】
アプリケーション・レイヤ５１は、図２９に示すように、行動モデルライブラリ８０、行動切換モジュール８１、学習モジュール８２、感情モデル８３及び本能モデル８４の５つのモジュールから構成されている。
【０１６２】
行動モデルライブラリ８０には、図３０に示すように、「バッテリ残量が少なくなった場合」、「転倒復帰する」、「障害物を回避する場合」、「感情を表現する場合」、「ボールを検出した場合」などの予め選択されたいくつかの条件項目にそれぞれ対応させて、それぞれ独立した行動モデルが設けられている。
【０１６３】
そして、これら行動モデルは、それぞれ入力セマンティクスコンバータモジュール６９から認識結果が与えられたときや、最後の認識結果が与えられてから一定時間が経過したときなどに、必要に応じて後述のように感情モデル８３に保持されている対応する情動のパラメータ値や、本能モデル８４に保持されている対応する欲求のパラメータ値を参照しながら続く行動をそれぞれ決定し、決定結果を行動切換モジュール８１に出力する。
【０１６４】
なお、この実施の形態の場合、各行動モデルは、次の行動を決定する手法として、図３１に示すような１つのノード（状態）ＮＯＤＥ０〜ＮＯＤＥｎから他のどのノードＮＯＤＥ０〜ＮＯＤＥｎに遷移するかを各ノードＮＯＤＥ０〜ＮＯＤＥｎに間を接続するアークＡＲＣ１〜ＡＲＣｎ１に対してそれぞれ設定された遷移確率Ｐ１〜Ｐｎに基づいて確率的に決定する有限確率オートマトンと呼ばれるアルゴリズムを用いる。
【０１６５】
具体的に、各行動モデルは、それぞれ自己の行動モデルを形成するノードＮＯＤＥ０〜ＮＯＤＥｎにそれぞれ対応させて、これらノードＮＯＤＥ０〜ＮＯＤＥｎ毎に図２３に示すような状態遷移表９０を有している。
【０１６６】
この状態遷移表９０では、そのノードＮＯＤＥ０〜ＮＯＤＥｎにおいて遷移条件とする入力イベント（認識結果）が「入力イベント名」の列に優先順に列記され、その遷移条件についてのさらなる条件が「データ名」及び「データ範囲」の列における対応する行に記述されている。
【０１６７】
したがって、図３２の状態遷移表９０で表されるノードＮＯＤＥ１００では、「ボールを検出（ＢＡＬＬ）」という認識結果が与えられた場合に、当該認識結果とともに与えられるそのボールの「大きさ（SIZE）」が「0から1000」の範囲であることや、「障害物を検出（OBSTACLE）」という認識結果が与えられた場合に、当該認識結果とともに与えられるその障害物までの「距離（DISTANCE）」が「0から100」の範囲であることが他のノードに遷移するための条件となっている。
【０１６８】
また、このノードＮＯＤＥ１００では、認識結果の入力がない場合においても、行動モデルが周期的に参照する感情モデル８３及び本能モデル８４にそれぞれ保持された各情動及び各欲求のパラメータ値のうち、感情モデル８３に保持された「喜び（Joy）」、「驚き（Surprise）」又は「悲しみ（Sadness）」の何れかのパラメータ値が「50から100」の範囲であるときには他のノードに遷移することができるようになっている。
【０１６９】
また、状態遷移表９０では、「他のノードヘの遷移確率」の欄における「遷移先ノード」の行にそのノードＮＯＤＥ０〜ＮＯＤＥｎから遷移できるノード名が列記されているとともに、「入力イベント名」、「データ名」及び「データの範囲」の列に記述された全ての条件が揃ったときに遷移できる他の各ノードＮＯＤＥ０〜ＮＯＤＥｎへの遷移確率が「他のノードヘの遷移確率」の欄内の対応する箇所にそれぞれ記述され、そのノードＮＯＤＥ０〜ＮＯＤＥｎに遷移する際に出力すべき行動が「他のノードヘの遷移確率」の欄における「出力行動」の行に記述されている。なお、「他のノードヘの遷移確率」の欄における各行の確率の和は１００［％］となっている。
【０１７０】
したがって、図３２の状態遷移表９０で表されるノードＮＯＤＥ１００では、例えば「ボールを検出（ＢＡＬＬ）」し、そのボールの「ＳＩＺＥ（大きさ）」が「0から1000」の範囲であるという認識結果が与えられた場合には、「30［％］」の確率で「ノードＮＯＤＥ１２０（node 120）」に遷移でき、そのとき「ＡＣＴＩＯＮ１」の行動が出力されることとなる。
【０１７１】
各行動モデルは、それぞれこのような状態遷移表９０として記述されたノードＮＯＤＥ０〜 ＮＯＤＥｎが幾つも繋がるようにして構成されており、入力セマンティクスコンバータモジュール６９から認識結果が与えられたときなどに、対応するノードＮＯＤＥ０〜ＮＯＤＥｎの状態遷移表を利用して確率的に次の行動を決定し、決定結果を行動切換モジュール８１に出力するようになされている。
【０１７２】
図３０に示す行動切換モジュール８１は、行動モデルライブラリ８０の各行動モデルからそれぞれ出力される行動のうち、予め定められた優先順位の高い行動モデルから出力された行動を選択し、当該行動を実行すべき旨のコマンド（以下、行動コマンドという。）をミドル・ウェア・レイヤ５０の出力セマンティクスコンバータモジュール７８に送出する。なお、この実施の形態においては、図３０において下側に表記された行動モデルほど優先順位が高く設定されている。
【０１７３】
また、行動切換モジュール８１は、行動完了後に出力セマンティクスコンバータモジュール７８から与えられる行動完了情報に基づいて、その行動が完了したことを学習モジュール８２、感情モデル８３及び本能モデル８４に通知する。
【０１７４】
一方、学習モジュール８２は、入力セマンティクスコンバータモジュール６９から与えられる認識結果のうち、「叩かれた」や「撫でられた」など、使用者からの働きかけとして受けた教示の認識結果を入力する。
【０１７５】
そして、学習モジュール８２は、この認識結果及び行動切換えモジュール７１からの通知に基づいて、「叩かれた（叱られた）」ときにはその行動の発現確率を低下させ、「撫でられた（誉められた）」ときにはその行動の発現確率を上昇させるように、行動モデルライブラリ７０における対応する行動モデルの対応する遷移確率を変更する。
【０１７６】
他方、感情モデル８３は、「喜び（Joy）」、「悲しみ（Sadness）」、「怒り（Anger）」、「驚き（Surprise）」、「嫌悪（Disgust）」及び「恐れ（Fear）」の合計６つの情動について、各情動毎にその情動の強さを表すパラメータを保持している。そして、感情モデル８３は、これら各情動のパラメータ値を、それぞれ入力セマンティクスコンバータモジュール６９から与えられる「叩かれた」及び「撫でられた」などの特定の認識結果や、経過時間及び行動切換モジュール８１からの通知などに基づいて周期的に更新する。
【０１７７】
具体的には、感情モデル８３は、入力セマンティクスコンバータモジュール６９から与えられる認識結果と、そのときのロボット装置１の行動と、前回更新してからの経過時間となどに基づいて所定の演算式により算出されるそのときのその情動の変動量を△Ｅ［ｔ］、現在のその情動のパラメータ値をＥ［ｔ］、その情動の感度を表す係数をｋｅとして、下記数式（１１）によって次の周期におけるその情動のパラメータ値Ｅ［ｔ＋１］を算出し、これを現在のその情動のパラメータ値Ｅ［ｔ］と置き換えるようにしてその情動のパラメータ値を更新する。また、感情モデル８３は、これと同様にして全ての情動のパラメータ値により更新する。
【０１７８】
【数１４】
なお、各認識結果や出力セマンティクスコンバータモジュール７８からの通知が各情動のパラメータ値の変動量△Ｅ［ｔ］にどの程度の影響を与えるかは予め決められており、例えば「叩かれた」といった認識結果は「怒り」の情動のパラメータ値の変動量△Ｅ［ｔ］に大きな影響を与え、「撫でられた」といった認識結果は「喜び」の情動のパラメータ値の変動量△Ｅ［ｔ］に大きな影響を与えるようになっている。
【０１８０】
ここで、出力セマンティクスコンバータモジュール７８からの通知とは、いわゆる行動のフィードバック情報（行動完了情報）であり、行動の出現結果の情報であり、感情モデル８３は、このような情報によっても感情を変化させる。これは、例えば、「叫ぶ」といった行動により怒りの感情レベルが下がるといったようなことである。なお、出力セマンティクスコンバータモジュール７８からの通知は、上述した学習モジュール８２にも入力されており、学習モジュール８２は、その通知に基づいて行動モデルの対応する遷移確率を変更する。
【０１８１】
なお、行動結果のフィードバックは、行動切換モジュール８１の出力（感情が付加された行動）によりなされるものであってもよい。
【０１８２】
一方、本能モデル８４は、「運動欲（exercise）」、「愛情欲（affection）」、「食欲（appetite）」及び「好奇心（curiosity）」の互いに独立した４つの欲求について、これら欲求毎にその欲求の強さを表すパラメータを保持している。そして、本能モデル８４は、これらの欲求のパラメータ値を、それぞれ入力セマンティクスコンバータモジュール６９から与えられる認識結果や、経過時間及び行動切換モジュール８１からの通知などに基づいて周期的に更新する。
【０１８３】
具体的には、本能モデル８４は、「運動欲」、「愛情欲」及び「好奇心」については、認識結果、経過時間及び出力セマンティクスコンバータモジュール７８からの通知などに基づいて所定の演算式により算出されるそのときのその欲求の変動量をΔＩ［ｋ］、現在のその欲求のパラメータ値をＩ［ｋ］、その欲求の感度を表す係数ｋｉとして、所定周期で下記数式（１２）を用いて次の周期におけるその欲求のパラメータ値Ｉ［ｋ＋１］を算出し、この演算結果を現在のその欲求のパラメータ値Ｉ［ｋ］と置き換えるようにしてその欲求のパラメータ値を更新する。また、本能モデル８４は、これと同様にして「食欲」を除く各欲求のパラメータ値を更新する。
【０１８４】
【数１５】
なお、認識結果及び出力セマンティクスコンバータモジュール７８からの通知などが各欲求のパラメータ値の変動量△Ｉ［ｋ］にどの程度の影響を与えるかは予め決められており、例えば出力セマンティクスコンバータモジュール７８からの通知は、「疲れ」のパラメータ値の変動量△Ｉ［ｋ］に大きな影響を与えるようになっている。
【０１８６】
なお、この具体例においては、各情動及び各欲求（本能）のパラメータ値がそれぞれ０から１００までの範囲で変動するように規制されており、また係数ｋｅ、ｋｉの値も各情動及び各欲求毎に個別に設定されている。
【０１８７】
一方、ミドル・ウェア・レイヤ５０の出力セマンティクスコンバータモジュール７８は、図２８に示すように、上述のようにしてアプリケーション・レイヤ５１の行動切換モジュール８１から与えられる「前進」、「喜ぶ」、「鳴く」又は「トラッキング（ボールを追いかける）」といった抽象的な行動コマンドを出力系７９の対応する信号処理モジュール７１〜７７に与える。
【０１８８】
そしてこれら信号処理モジュール７１〜７７は、行動コマンドが与えられると当該行動コマンドに基づいて、その行動をするために対応するアクチュエータに与えるべきサーボ指令値や、スピーカから出力する音の音声データ及び又はＬＥＤに与える駆動データを生成し、これらのデータをロボティック・サーバ・オブジェクト４２のバーチャル・ロボット４３及び信号処理回路を順次介して対応するアクチュエータ又はスピーカ又はＬＥＤに順次送出する。
【０１８９】
このようにしてロボット装置１は、上述した制御プログラムに基づいて、自己（内部）及び周囲（外部）の状況や、使用者からの指示及び働きかけに応じた自律的な行動ができる。
【０１９０】
このような制御プログラムは、ロボット装置が読取可能な形式で記録された記録媒体を介して提供される。制御プログラムを記録する記録媒体としては、磁気読取方式の記録媒体（例えば、磁気テープ、フレキシブルディスク、磁気カード）、光学読取方式の記録媒体（例えば、ＣＤ−ＲＯＭ、ＭＯ、ＣＤ−Ｒ、ＤＶＤ）等が考えられる。記録媒体には、半導体メモリ（いわゆるメモリカード（矩形型、正方形型など形状は問わない。）、ＩＣカード）等の記憶媒体も含まれる。また、制御プログラムは、いわゆるインターネット等を介して提供されてもよい。
【０１９１】
これらの制御プログラムは、専用の読込ドライバ装置、又はパーソナルコンピュータ等を介して再生され、有線又は無線接続によってロボット装置１に伝送されて読み込まれる。また、ロボット装置１は、半導体メモリ、又はＩＣカード等の小型化された記憶媒体のドライブ装置を備える場合、これら記憶媒体から制御プログラムを直接読み込むこともできる。
【０１９２】
本実施の形態においては、障害物認識装置は、多数の測定点を使って平面の検出を行うために安定した平面を抽出できる。また、画像から得られる平面候補をロボットの姿勢から得られる床面パラメータと照合することで正しい平面を選択できる。また、障害物を認識するのではなく、実質的には床面を認識するので障害物の形状や大きさに因らない認識が可能である。また、床面からの距離で障害物を表現するので細かい段差や凹みまで検出することができる。またロボットの大きさを考慮して跨いだり、くぐったりする判断も容易である。さらに、2次元床面上の障害物表現となるので経路計画などに既存の移動ロボットで用いられる手法が適用可能であるし、3次元の表現より高速に演算できる。
【０１９３】
【発明の効果】
以上詳細に説明したように本発明に係る障害物認識装置は、座標変換手段による座標変換結果と、平面検出手段によって検出された平面パラメータとに基づいて床面検出手段が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、正確に床面を抽出し、障害物を認識することができる。
【０１９４】
また、本発明に係る障害物認識方法は、座標変換工程による座標変換結果と、平面検出工程によって検出された平面パラメータとに基づいて床面検出工程が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、正確に床面を抽出し、障害物を認識することができる。
【０１９５】
また、本発明に係る障害物認識プログラムは、ロボット装置によって実行されることにより、座標変換工程による座標変換結果と、平面検出工程によって検出された平面パラメータとに基づいて床面検出工程が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、ロボット装置に正確に床面を抽出させ、障害物を認識させることができる。
【０１９６】
また、本発明に係るロボット装置は、座標変換手段による座標変換結果と、平面検出手段によって検出された平面パラメータとに基づいて床面検出手段が床面を検出し、その床面の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識するので、正確に床面を抽出し、障害物を認識することができる。
【図面の簡単な説明】
【図１】本発明の実施の形態におけるロボット装置の概略を示すブロック図である。
【図２】本発明の実施の形態におけるロボット装置のソフトウェアの構成を示す模式図である。
【図３】障害物認識装置の機能ブロック図である。
【図４】平面抽出部ＰＬＥＸに入力される視差画像の生成を説明するための図である。
【図５】平面抽出部ＰＬＥＸが障害物を認識する処理手順を示すフローチャートである。
【図６】平面抽出部ＰＬＥＸが検出する平面のパラメータを示す図である。
【図７】カメラ座標系から足底接地平面座標系への変換処理を説明するための図である。
【図８】平面抽出部ＰＬＥＸにて抽出された平面上の点を示す図である。
【図９】ロボットビューから床面を抽出し、さらに座標変換して２次元（床平面）上に障害物を表現したことを示す図である。
【図１０】本発明の実施の形態におけるロボット装置のソフトウェアの動作を示すフローチャートである。
【図１１】同ソフトウェアに入力されるデータの流れを示す模式図である。
【図１２】本発明の実施の形態における障害物認識装置（ロボット装置）にて表示される環境地図を示す模式図である。
【図１３】（ａ），（ｂ）及び（ｃ），（ｄ）は位置認識装置の移動量に応じた環境地図の更新方法を示す図であって、夫々環境地図を更新しない場合及び環境地図を更新する場合の環境地図を示す模式図である。
【図１４】位置認識装置における環境地図更新の際の各移動量を示す模式図である。
【図１５】（ａ）及び（ｂ）は、位置認識装置における姿勢方向の夫々更新前後の様子を示す模式図である。
【図１６】位置認識装置における効果を説明する模式図である。
【図１７】環境地図及び姿勢情報を表示した操作盤を示す模式図である。
【図１８】ランドマークセンサＣＬＳにおけるカラーランドマークを示す模式図である。
【図１９】ランドマークセンサＣＬＳの構成を模式的に示すブロック図である。
【図２０】マルコフ・ローカリゼーション部により求められた各グリッド上での自己位置確率密度分布を示す模式図である。
【図２１】ランドマークセンサＣＬＳ３４０の動作を示すフローチャートである。
【図２２】障害物情報により生成された障害物地図上に、現在位置から目的位置までの経路を示した図である。
【図２３】経路計画アルゴリズムを示すフローチャートである。
【図２４】本実施の形態におけるロボット装置の外観構成を示す斜視図である。
【図２５】同ロボット装置の自由度構成モデルを模式的に示す図である。
【図２６】同ロボット装置の回路構成を示すブロック図である。
【図２７】同ロボット装置のソフトウェア構成を示すブロック図である。
【図２８】同ロボット装置のソフトウェア構成におけるミドル・ウェア・レイヤの構成を示すブロック図である。
【図２９】同ロボット装置のソフトウェア構成におけるアプリケーション・レイヤの構成を示すブロック図である。
【図３０】アプリケーション・レイヤの行動モデルライブラリの構成を示すブロック図である。
【図３１】同ロボット装置の行動決定のための情報となる有限確率オートマトンを説明する図である。
【図３２】有限確率オートマトンの各ノードに用意された状態遷移表を示す図である。
【符号の説明】
１ ロボット装置、２５０ 頭部ユニット、２００Ｒ，２００Ｌ ＣＣＤカメラ、２０１Ｒ 右目画像、２０１Ｌ 左目画像、２０２ カラー画像、２０３ 視差画像、２１０ ステレオ画像処理装置、２２０ ＣＰＵ、２３０ アクチュエータ、２３１ 制御信号、２４０ センサ、２４１ センサデータ、２６０ 体幹部、３００ ソフトウェア、３１０ キネマティックオドメトリＫＩＮＥ３１０、３２０ 平面抽出部ＰＬＥＸ、３３０ 障害物グリッド算出部ＯＧ、３４０ ランドマーク位置検出部ＣＬＳ、３５０ 絶対座標算出部ＬＺ、３６０ 行動決定部ＳＢＬ、４００，５００，５１０，６００，６１０，７００ 環境地図、４０１，５０１，６０１，７０１ 中心グリッド、４０２，５０２，６０２ 障害物グリッド、４０３，５０３，５０６，５１３，６０３，７０３ ロボット装置、４１０ グリッド、４０４ 情報、９００ 操作盤
Claims (8)
- 複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成手段と、
前記距離画像生成手段により生成された距離画像に基づいて平面パラメータを検出する平面検出手段と、前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換手段と、
前記センサ手段からのセンサデータ出力に基づき前記座標変換手段による座標変換を行って得られた接地平面パラメータと、前記平面検出手段によって検出された平面パラメータとに基づいて床面を検出する床面検出手段と、
前記床面検出手段の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識手段と
を備えることを特徴とする障害物認識装置。 - 前記障害物認識手段は、前記床面検出手段にて検出された床面の平面パラメータを用いて平面からの距離が小さいデータを取り除いた残りを障害物として認識することを特徴とする請求項１記載の障害物認識装置。
- 前記床面検出手段にて検出した床面と、前記障害物認識手段が認識した障害物とを、高さをゼロとした平面座標上に変換して平面上の２次元の位置で床面か障害物かを識別することを特徴とする請求項１記載の障害物認識装置。
- 複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成工程と、
前記距離画像生成工程により生成された距離画像に基づいて平面パラメータを検出する平面検出工程と、
前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換工程と、
前記センサ手段からのセンサデータ出力に基づき前記座標変換工程による座標変換を行って得られた接地平面パラメータと、前記平面検出工程によって検出された平面パラメータとに基づいて床面を検出する床面検出工程と、
前記床面検出工程の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識工程と
を備える障害物認識方法。 - 障害物を認識する障害物認識プログラムにおいて、
複数の撮像手段から得られた画像データに基づいて算出された視差画像と、複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成工程と、
前記距離画像生成工程により生成された距離画像に基づいて平面パラメータを検出する平面検出工程と、
前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換工程と、
前記センサ手段からのセンサデータ出力に基づき前記座標変換工程による座標変換を行って得られた接地平面パラメータと、前記平面検出工程によって検出された平面パラメータとに基づいて床面を検出する床面検出工程と、
前記床面検出工程の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識工程と
を備えてなることを特徴とする障害物認識プログラム。 - 頭部ユニットと、少なくとも１以上の可動脚ユニットと、情報処理手段を有する胴体ユニットとを備えてなり、床面上の障害物を認識しながら前記可動脚ユニットを用いて床面を移動する移動型ロボット装置において、
前記頭部ユニットに設けた複数の撮像手段から得られた画像データに基づいて算出された視差画像と、前記頭部ユニット、前記可動脚ユニット、前記胴体ユニットに設けた複数のセンサ手段から得られたセンサデータ出力とに基づいて前記複数の撮像手段が配設された位置における前記視差画像に対応する同時変換行列とを用いて距離画像を生成する距離画像生成手段と、
前記距離画像生成手段により生成された距離画像に基づいて平面パラメータを検出する平面検出手段と、
前記複数の撮像手段の配設された位置における座標系を床面での座標系に変換する座標変換手段と、
ロボット装置の前記センサ手段からのセンサデータ出力に基づき前記座標変換手段による座標変換を行って得られた接地平面パラメータと、前記平面検出手段によって検出された平面パラメータとに基づいて床面を検出する床面検出手段と、
前記床面検出手段の平面パラメータを用いて床面に載っている点を選択し、この点に基づいて障害物を認識する障害物認識手段と
を前記胴体ユニットに備えてなることを特徴とする移動型ロボット装置。 - 前記障害物認識手段は、前記床面検出手段にて検出された床面の平面パラメータを用いて平面からの距離が小さいデータを取り除いた残りを障害物として認識することを特徴とする請求項６記載の移動型ロボット装置。
- 前記床面検出手段にて検出した床面と、前記障害物認識手段が認識した障害物とを、高さをゼロとした平面座標上に変換して平面上の２次元の位置で床面か障害物かを識別することを特徴とする請求項６記載の移動型ロボット装置。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002073388A JP3945279B2 (ja) | 2002-03-15 | 2002-03-15 | 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 |
| US10/387,647 US7386163B2 (en) | 2002-03-15 | 2003-03-13 | Obstacle recognition apparatus and method, obstacle recognition program, and mobile robot apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002073388A JP3945279B2 (ja) | 2002-03-15 | 2002-03-15 | 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003269937A JP2003269937A (ja) | 2003-09-25 |
| JP3945279B2 true JP3945279B2 (ja) | 2007-07-18 |
Family
ID=29203065
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002073388A Expired - Fee Related JP3945279B2 (ja) | 2002-03-15 | 2002-03-15 | 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 |
Country Status (2)
| Country | Link |
|---|---|
| US (1) | US7386163B2 (ja) |
| JP (1) | JP3945279B2 (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101380260B1 (ko) | 2008-08-25 | 2014-04-01 | 무라다기카이가부시끼가이샤 | 자율 이동 장치 |
| CN103808277A (zh) * | 2013-12-23 | 2014-05-21 | 天津大学 | 一种多传感器点云拼接误差的修正方法 |
| WO2021065510A1 (ja) * | 2019-10-03 | 2021-04-08 | ソニー株式会社 | 情報処理装置、情報処理方法、情報処理システム及びプログラム |
Families Citing this family (200)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040162637A1 (en) | 2002-07-25 | 2004-08-19 | Yulun Wang | Medical tele-robotic system with a master remote station with an arbitrator |
| US6925357B2 (en) | 2002-07-25 | 2005-08-02 | Intouch Health, Inc. | Medical tele-robotic system |
| US7693514B2 (en) * | 2003-08-18 | 2010-04-06 | Honda Motor Co., Ltd. | Information gathering robot |
| JP3994950B2 (ja) * | 2003-09-19 | 2007-10-24 | ソニー株式会社 | 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 |
| US7813836B2 (en) | 2003-12-09 | 2010-10-12 | Intouch Technologies, Inc. | Protocol for a remotely controlled videoconferencing robot |
| DE102004005380A1 (de) * | 2004-02-03 | 2005-09-01 | Isra Vision Systems Ag | Verfahren zur Bestimmung der Lage eines Objekts im Raum |
| US20050204438A1 (en) * | 2004-02-26 | 2005-09-15 | Yulun Wang | Graphical interface for a remote presence system |
| JP4636016B2 (ja) * | 2004-03-17 | 2011-02-23 | ソニー株式会社 | 平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 |
| US20070198178A1 (en) * | 2004-03-31 | 2007-08-23 | Trimby Martin W | Pathfinding system |
| KR100763234B1 (ko) * | 2004-06-11 | 2007-10-04 | 삼성전자주식회사 | 주행 상태를 감지하는 시스템 및 방법 |
| JP4479372B2 (ja) | 2004-06-25 | 2010-06-09 | ソニー株式会社 | 環境地図作成方法、環境地図作成装置、及び移動型ロボット装置 |
| US7343624B1 (en) * | 2004-07-13 | 2008-03-11 | Sonicwall, Inc. | Managing infectious messages as identified by an attachment |
| US8077963B2 (en) | 2004-07-13 | 2011-12-13 | Yulun Wang | Mobile robot with a head-based movement mapping scheme |
| US9154511B1 (en) | 2004-07-13 | 2015-10-06 | Dell Software Inc. | Time zero detection of infectious messages |
| JP4328692B2 (ja) | 2004-08-11 | 2009-09-09 | 国立大学法人東京工業大学 | 物体検出装置 |
| JP4297501B2 (ja) * | 2004-08-11 | 2009-07-15 | 国立大学法人東京工業大学 | 移動体周辺監視装置 |
| JP3937414B2 (ja) | 2004-08-11 | 2007-06-27 | 本田技研工業株式会社 | 平面検出装置及び検出方法 |
| US20060052676A1 (en) * | 2004-09-07 | 2006-03-09 | Yulun Wang | Tele-presence system that allows for remote monitoring/observation and review of a patient and their medical records |
| JP2006085369A (ja) * | 2004-09-15 | 2006-03-30 | Sony Corp | 移動体装置及びその制御方法 |
| JP4261466B2 (ja) | 2004-11-17 | 2009-04-30 | 株式会社日立製作所 | 自律移動装置、並びに物体及び自己位置検出システム |
| US7639841B2 (en) * | 2004-12-20 | 2009-12-29 | Siemens Corporation | System and method for on-road detection of a vehicle using knowledge fusion |
| EP1851728A1 (en) * | 2005-02-08 | 2007-11-07 | Seegrid Corporation | Multidimensional evidence grids and system and methods for applying same |
| US8427472B2 (en) * | 2005-02-08 | 2013-04-23 | Seegrid Corporation | Multidimensional evidence grids and system and methods for applying same |
| JP4341564B2 (ja) * | 2005-02-25 | 2009-10-07 | 株式会社豊田中央研究所 | 対象物判定装置 |
| JP2006239844A (ja) * | 2005-03-04 | 2006-09-14 | Sony Corp | 障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置 |
| JP3906230B2 (ja) * | 2005-03-11 | 2007-04-18 | 株式会社東芝 | 音響信号処理装置、音響信号処理方法、音響信号処理プログラム、及び音響信号処理プログラムを記録したコンピュータ読み取り可能な記録媒体 |
| JP4241673B2 (ja) * | 2005-06-17 | 2009-03-18 | 本田技研工業株式会社 | 移動体の経路生成装置 |
| US9198728B2 (en) * | 2005-09-30 | 2015-12-01 | Intouch Technologies, Inc. | Multi-camera mobile teleconferencing platform |
| KR100727033B1 (ko) * | 2005-12-07 | 2007-06-12 | 한국전자통신연구원 | 네트워크 기반 지능형 서비스 로봇의 비전 처리 장치 및방법과 이를 이용한 시스템 |
| KR100748245B1 (ko) * | 2005-12-09 | 2007-08-10 | 한국전자통신연구원 | 인공표식과 지역좌표계를 이용한 이동로봇의 환경지도 작성방법 및 이동 방법 |
| DE602006009885D1 (de) * | 2005-12-12 | 2009-12-03 | Honda Motor Co Ltd | Steuersystem für einen beweglichen Roboter mit Beinen |
| US8577538B2 (en) * | 2006-07-14 | 2013-11-05 | Irobot Corporation | Method and system for controlling a remote vehicle |
| JP5028568B2 (ja) * | 2006-01-24 | 2012-09-19 | 株式会社国際電気通信基礎技術研究所 | ロボット制御システム |
| US20100171826A1 (en) * | 2006-04-12 | 2010-07-08 | Store Eyes, Inc. | Method for measuring retail display and compliance |
| US8108092B2 (en) | 2006-07-14 | 2012-01-31 | Irobot Corporation | Autonomous behaviors for a remote vehicle |
| US8849679B2 (en) * | 2006-06-15 | 2014-09-30 | Intouch Technologies, Inc. | Remote controlled robot system that provides medical images |
| JP4455551B2 (ja) * | 2006-07-31 | 2010-04-21 | 株式会社東芝 | 音響信号処理装置、音響信号処理方法、音響信号処理プログラム、及び音響信号処理プログラムを記録したコンピュータ読み取り可能な記録媒体 |
| KR100791386B1 (ko) | 2006-08-18 | 2008-01-07 | 삼성전자주식회사 | 이동 로봇의 영역 분리 방법 및 장치 |
| JP4821548B2 (ja) * | 2006-10-02 | 2011-11-24 | コニカミノルタホールディングス株式会社 | 画像処理装置、画像処理装置の制御方法、および画像処理装置の制御プログラム |
| US7974460B2 (en) * | 2007-02-06 | 2011-07-05 | Honeywell International Inc. | Method and system for three-dimensional obstacle mapping for navigation of autonomous vehicles |
| JP4645601B2 (ja) * | 2007-02-13 | 2011-03-09 | トヨタ自動車株式会社 | 環境地図の生成方法及び移動ロボット |
| US8265793B2 (en) | 2007-03-20 | 2012-09-11 | Irobot Corporation | Mobile robot for telecommunication |
| US9160783B2 (en) * | 2007-05-09 | 2015-10-13 | Intouch Technologies, Inc. | Robot system that operates through a network firewall |
| JP5380789B2 (ja) | 2007-06-06 | 2014-01-08 | ソニー株式会社 | 情報処理装置、および情報処理方法、並びにコンピュータ・プログラム |
| US20090018712A1 (en) * | 2007-07-13 | 2009-01-15 | Jerry Richard Duncan | Method and system for remotely monitoring and controlling a vehicle via a virtual environment |
| US9020240B2 (en) | 2007-08-10 | 2015-04-28 | Leica Geosystems Ag | Method and surveying system for noncontact coordinate measurement on an object surface |
| US8036452B2 (en) * | 2007-08-10 | 2011-10-11 | Leica Geosystems Ag | Method and measurement system for contactless coordinate measurement on an object surface |
| JP4953313B2 (ja) * | 2007-09-10 | 2012-06-13 | 公立大学法人首都大学東京 | 環境認識システム、自律型移動体および環境認識プログラム |
| US8116519B2 (en) * | 2007-09-26 | 2012-02-14 | Honda Motor Co., Ltd. | 3D beverage container localizer |
| JP5181602B2 (ja) * | 2007-09-28 | 2013-04-10 | コニカミノルタホールディングス株式会社 | 物体検出装置 |
| JP5566892B2 (ja) * | 2007-10-10 | 2014-08-06 | 本田技研工業株式会社 | 追跡および観測用ロボット |
| KR101408657B1 (ko) * | 2007-12-18 | 2014-06-17 | 삼성전자주식회사 | 원격 제어 로봇 시스템의 사용자 인터페이스 장치 및 방법 |
| JP2009168751A (ja) * | 2008-01-18 | 2009-07-30 | Toyota Motor Corp | 障害物検出システム及び障害物検出方法 |
| US10875182B2 (en) | 2008-03-20 | 2020-12-29 | Teladoc Health, Inc. | Remote presence system mounted to operating room hardware |
| US8179418B2 (en) | 2008-04-14 | 2012-05-15 | Intouch Technologies, Inc. | Robotic based health care system |
| US8170241B2 (en) | 2008-04-17 | 2012-05-01 | Intouch Technologies, Inc. | Mobile tele-presence system with a microphone system |
| CN101581936B (zh) * | 2008-05-16 | 2012-01-25 | 深圳富泰宏精密工业有限公司 | 利用手机控制两足式机器人的系统及方法 |
| US8265425B2 (en) * | 2008-05-20 | 2012-09-11 | Honda Motor Co., Ltd. | Rectangular table detection using hybrid RGB and depth camera sensors |
| KR101457148B1 (ko) * | 2008-05-21 | 2014-10-31 | 삼성전자 주식회사 | 로봇의 위치 추정 장치 및 그 방법 |
| US9193065B2 (en) | 2008-07-10 | 2015-11-24 | Intouch Technologies, Inc. | Docking system for a tele-presence robot |
| US9842192B2 (en) * | 2008-07-11 | 2017-12-12 | Intouch Technologies, Inc. | Tele-presence robot system with multi-cast features |
| US8923602B2 (en) * | 2008-07-22 | 2014-12-30 | Comau, Inc. | Automated guidance and recognition system and method of the same |
| US8099237B2 (en) | 2008-07-25 | 2012-01-17 | Navteq North America, Llc | Open area maps |
| US8825387B2 (en) * | 2008-07-25 | 2014-09-02 | Navteq B.V. | Positioning open area maps |
| US20100023251A1 (en) * | 2008-07-25 | 2010-01-28 | Gale William N | Cost based open area maps |
| US8229176B2 (en) * | 2008-07-25 | 2012-07-24 | Navteq B.V. | End user image open area maps |
| US8339417B2 (en) * | 2008-07-25 | 2012-12-25 | Navteq B.V. | Open area maps based on vector graphics format images |
| US8374780B2 (en) * | 2008-07-25 | 2013-02-12 | Navteq B.V. | Open area maps with restriction content |
| US8417446B2 (en) * | 2008-07-25 | 2013-04-09 | Navteq B.V. | Link-node maps based on open area maps |
| US20100021013A1 (en) * | 2008-07-25 | 2010-01-28 | Gale William N | Open area maps with guidance |
| US8705792B2 (en) * | 2008-08-06 | 2014-04-22 | Toyota Motor Engineering & Manufacturing North America, Inc. | Object tracking using linear features |
| US8340819B2 (en) | 2008-09-18 | 2012-12-25 | Intouch Technologies, Inc. | Mobile videoconferencing robot system with network adaptive driving |
| US8996165B2 (en) | 2008-10-21 | 2015-03-31 | Intouch Technologies, Inc. | Telepresence robot with a camera boom |
| US9672736B2 (en) * | 2008-10-22 | 2017-06-06 | Toyota Motor Engineering & Manufacturing North America, Inc. | Site map interface for vehicular application |
| US8463435B2 (en) | 2008-11-25 | 2013-06-11 | Intouch Technologies, Inc. | Server connectivity control for tele-presence robot |
| US9138891B2 (en) * | 2008-11-25 | 2015-09-22 | Intouch Technologies, Inc. | Server connectivity control for tele-presence robot |
| JP2010162077A (ja) * | 2009-01-13 | 2010-07-29 | Design Office See:Kk | ネットワークシュミレーションゲーム |
| US8849680B2 (en) | 2009-01-29 | 2014-09-30 | Intouch Technologies, Inc. | Documentation through a remote presence robot |
| TWI404918B (zh) * | 2009-03-26 | 2013-08-11 | Univ Yuan Ze | 行動載具適應型障礙物閃避之路徑規劃方法 |
| US8897920B2 (en) | 2009-04-17 | 2014-11-25 | Intouch Technologies, Inc. | Tele-presence robot system with software modularity, projector and laser pointer |
| JP2010287029A (ja) * | 2009-06-11 | 2010-12-24 | Konica Minolta Opto Inc | 周辺表示装置 |
| US8384755B2 (en) | 2009-08-26 | 2013-02-26 | Intouch Technologies, Inc. | Portable remote presence robot |
| US11399153B2 (en) * | 2009-08-26 | 2022-07-26 | Teladoc Health, Inc. | Portable telepresence apparatus |
| US20110169923A1 (en) * | 2009-10-08 | 2011-07-14 | Georgia Tech Research Corporatiotion | Flow Separation for Stereo Visual Odometry |
| JP2011128899A (ja) * | 2009-12-17 | 2011-06-30 | Murata Machinery Ltd | 自律移動装置 |
| JP5205366B2 (ja) * | 2009-12-25 | 2013-06-05 | 本田技研工業株式会社 | 移動可能領域抽出装置、移動可能領域抽出システム、移動可能領域抽出方法、およびプログラム |
| US20110187875A1 (en) * | 2010-02-04 | 2011-08-04 | Intouch Technologies, Inc. | Robot face used in a sterile environment |
| US11154981B2 (en) * | 2010-02-04 | 2021-10-26 | Teladoc Health, Inc. | Robot user interface for telepresence robot system |
| US8670017B2 (en) | 2010-03-04 | 2014-03-11 | Intouch Technologies, Inc. | Remote presence system including a cart that supports a robot face and an overhead camera |
| JP5560794B2 (ja) | 2010-03-16 | 2014-07-30 | ソニー株式会社 | 制御装置、制御方法およびプログラム |
| JP2011201002A (ja) | 2010-03-26 | 2011-10-13 | Sony Corp | ロボット装置、ロボット装置の遠隔制御方法及びプログラム |
| US9014848B2 (en) | 2010-05-20 | 2015-04-21 | Irobot Corporation | Mobile robot system |
| US8935005B2 (en) | 2010-05-20 | 2015-01-13 | Irobot Corporation | Operating a mobile robot |
| US8918213B2 (en) | 2010-05-20 | 2014-12-23 | Irobot Corporation | Mobile human interface robot |
| US10343283B2 (en) | 2010-05-24 | 2019-07-09 | Intouch Technologies, Inc. | Telepresence robot system that can be accessed by a cellular phone |
| US10808882B2 (en) | 2010-05-26 | 2020-10-20 | Intouch Technologies, Inc. | Tele-robotic system with a robot face placed on a chair |
| US9149929B2 (en) * | 2010-05-26 | 2015-10-06 | The Boeing Company | Methods and systems for inspection sensor placement |
| US8824762B2 (en) * | 2010-10-22 | 2014-09-02 | The Johns Hopkins University | Method and system for processing ultrasound data |
| US9264664B2 (en) | 2010-12-03 | 2016-02-16 | Intouch Technologies, Inc. | Systems and methods for dynamic bandwidth allocation |
| KR20120070291A (ko) * | 2010-12-21 | 2012-06-29 | 삼성전자주식회사 | 보행 로봇 및 그의 동시 위치 인식 및 지도 작성 방법 |
| US8930019B2 (en) | 2010-12-30 | 2015-01-06 | Irobot Corporation | Mobile human interface robot |
| US9114838B2 (en) | 2011-01-05 | 2015-08-25 | Sphero, Inc. | Self-propelled device for interpreting input from a controller device |
| US9429940B2 (en) | 2011-01-05 | 2016-08-30 | Sphero, Inc. | Self propelled device with magnetic coupling |
| US10281915B2 (en) | 2011-01-05 | 2019-05-07 | Sphero, Inc. | Multi-purposed self-propelled device |
| US9218316B2 (en) * | 2011-01-05 | 2015-12-22 | Sphero, Inc. | Remotely controlling a self-propelled device in a virtualized environment |
| US9323250B2 (en) | 2011-01-28 | 2016-04-26 | Intouch Technologies, Inc. | Time-dependent navigation of telepresence robots |
| US8718837B2 (en) | 2011-01-28 | 2014-05-06 | Intouch Technologies | Interfacing with a mobile telepresence robot |
| US8805579B2 (en) * | 2011-02-19 | 2014-08-12 | Richard Arthur Skrinde | Submersible robotically operable vehicle system for infrastructure maintenance and inspection |
| US9146559B2 (en) * | 2011-03-18 | 2015-09-29 | The Raymond Corporation | System and method for gathering video data related to operation of an autonomous industrial vehicle |
| CN107272678B (zh) | 2011-04-11 | 2020-11-06 | 克朗设备公司 | 使用经协调路径规划器有效调度多个自动非完整车辆的方法和设备 |
| US10769739B2 (en) | 2011-04-25 | 2020-09-08 | Intouch Technologies, Inc. | Systems and methods for management of information among medical providers and facilities |
| JP2012236244A (ja) * | 2011-05-10 | 2012-12-06 | Sony Corp | ロボット装置、ロボット装置の制御方法、並びにロボット装置制御用プログラム |
| US20140139616A1 (en) | 2012-01-27 | 2014-05-22 | Intouch Technologies, Inc. | Enhanced Diagnostics for a Telepresence Robot |
| US9098611B2 (en) | 2012-11-26 | 2015-08-04 | Intouch Technologies, Inc. | Enhanced video interaction for a user interface of a telepresence network |
| US8655588B2 (en) | 2011-05-26 | 2014-02-18 | Crown Equipment Limited | Method and apparatus for providing accurate localization for an industrial vehicle |
| US8548671B2 (en) | 2011-06-06 | 2013-10-01 | Crown Equipment Limited | Method and apparatus for automatically calibrating vehicle parameters |
| WO2012169174A1 (ja) * | 2011-06-08 | 2012-12-13 | パナソニック株式会社 | 画像処理装置および画像処理方法 |
| US8799201B2 (en) | 2011-07-25 | 2014-08-05 | Toyota Motor Engineering & Manufacturing North America, Inc. | Method and system for tracking objects |
| FR2979428B1 (fr) * | 2011-08-24 | 2013-10-04 | Commissariat Energie Atomique | Methode de localisation d'un objet au moyen d'une grille de reference |
| US20140058634A1 (en) | 2012-08-24 | 2014-02-27 | Crown Equipment Limited | Method and apparatus for using unique landmarks to locate industrial vehicles at start-up |
| JP5930892B2 (ja) * | 2011-09-07 | 2016-06-08 | 本田技研工業株式会社 | 接触状態推定装置及び軌道生成装置 |
| US9056754B2 (en) | 2011-09-07 | 2015-06-16 | Crown Equipment Limited | Method and apparatus for using pre-positioned objects to localize an industrial vehicle |
| JP2013058059A (ja) * | 2011-09-08 | 2013-03-28 | Sony Corp | 情報処理装置、情報処理方法、及び、プログラム |
| US8836751B2 (en) | 2011-11-08 | 2014-09-16 | Intouch Technologies, Inc. | Tele-presence system with a user interface that displays different communication links |
| GB201209382D0 (en) * | 2012-05-25 | 2012-07-11 | Poikos Ltd | Body measurement |
| JP5959899B2 (ja) * | 2012-03-30 | 2016-08-02 | 本田技研工業株式会社 | 接触状態推定装置 |
| US9251313B2 (en) | 2012-04-11 | 2016-02-02 | Intouch Technologies, Inc. | Systems and methods for visualizing and managing telepresence devices in healthcare networks |
| US8902278B2 (en) | 2012-04-11 | 2014-12-02 | Intouch Technologies, Inc. | Systems and methods for visualizing and managing telepresence devices in healthcare networks |
| WO2013173389A1 (en) | 2012-05-14 | 2013-11-21 | Orbotix, Inc. | Operating a computing device by detecting rounded objects in an image |
| EP2852475A4 (en) | 2012-05-22 | 2016-01-20 | Intouch Technologies Inc | SOCIAL BEHAVIOR OF A MEDICAL TELEPRESCENT ROBOT |
| US9361021B2 (en) | 2012-05-22 | 2016-06-07 | Irobot Corporation | Graphical user interfaces including touchpad driving interfaces for telemedicine devices |
| KR101391554B1 (ko) | 2012-07-19 | 2014-05-02 | 고려대학교 산학협력단 | 평면 추출 방법 및 이를 구비하는 이동 로봇의 지도 형성 방법 및 위치 인식 방법 |
| KR101441187B1 (ko) * | 2012-07-19 | 2014-09-18 | 고려대학교 산학협력단 | 자율 보행 로봇 경로 계획 방법 |
| JP6065499B2 (ja) * | 2012-09-26 | 2017-01-25 | 沖電気工業株式会社 | 情報処理装置、プログラム及び情報処理方法 |
| JP2015024453A (ja) | 2013-07-25 | 2015-02-05 | トヨタ自動車株式会社 | 載置判断方法、載置方法、載置判断装置及びロボット |
| US9996974B2 (en) * | 2013-08-30 | 2018-06-12 | Qualcomm Incorporated | Method and apparatus for representing a physical scene |
| US9829882B2 (en) | 2013-12-20 | 2017-11-28 | Sphero, Inc. | Self-propelled device with center of mass drive system |
| US10427254B2 (en) * | 2014-04-30 | 2019-10-01 | The Boeing Company | Flexible manufacturing for aircraft structures |
| US9542746B2 (en) | 2014-06-13 | 2017-01-10 | Xerox Corporation | Method and system for spatial characterization of an imaging system |
| US10453046B2 (en) | 2014-06-13 | 2019-10-22 | Conduent Business Services, Llc | Store shelf imaging system |
| US9659204B2 (en) | 2014-06-13 | 2017-05-23 | Conduent Business Services, Llc | Image processing methods and systems for barcode and/or product label recognition |
| US10176452B2 (en) | 2014-06-13 | 2019-01-08 | Conduent Business Services Llc | Store shelf imaging system and method |
| ES2654335T3 (es) | 2014-10-23 | 2018-02-13 | Comau S.P.A. | Sistema para monitorizar y controlar una instalación industrial |
| US20200409382A1 (en) * | 2014-11-10 | 2020-12-31 | Carnegie Mellon University | Intelligent cleaning robot |
| KR102404258B1 (ko) * | 2015-02-06 | 2022-06-02 | 삼성전자주식회사 | 로봇충전복귀장치 및 그 복귀방법 |
| US9690374B2 (en) | 2015-04-27 | 2017-06-27 | Google Inc. | Virtual/augmented reality transition system and method |
| KR101734654B1 (ko) * | 2015-06-25 | 2017-05-11 | 현대자동차주식회사 | 레이저 스캐너를 이용한 센서 중심 좌표계의 점유 격자 지도를 작성하는 시스템 및 방법 |
| US10265863B2 (en) * | 2015-09-09 | 2019-04-23 | Carbon Robotics, Inc. | Reconfigurable robotic system and methods |
| JP6705636B2 (ja) | 2015-10-14 | 2020-06-03 | 東芝ライフスタイル株式会社 | 電気掃除機 |
| US11927965B2 (en) | 2016-02-29 | 2024-03-12 | AI Incorporated | Obstacle recognition method for autonomous robots |
| US10452071B1 (en) | 2016-02-29 | 2019-10-22 | AI Incorporated | Obstacle recognition method for autonomous robots |
| US11449061B2 (en) | 2016-02-29 | 2022-09-20 | AI Incorporated | Obstacle recognition method for autonomous robots |
| US10788836B2 (en) | 2016-02-29 | 2020-09-29 | AI Incorporated | Obstacle recognition method for autonomous robots |
| US11037320B1 (en) | 2016-03-01 | 2021-06-15 | AI Incorporated | Method for estimating distance using point measurement and color depth |
| US10311590B1 (en) * | 2016-03-01 | 2019-06-04 | Al Incorporated | Method for estimating distance using point measurement and color depth |
| US9928438B2 (en) | 2016-03-10 | 2018-03-27 | Conduent Business Services, Llc | High accuracy localization system and method for retail store profiling via product image recognition and its corresponding dimension database |
| JP6431495B2 (ja) * | 2016-03-25 | 2018-11-28 | 本田技研工業株式会社 | 教師データ生成方法 |
| US10196104B1 (en) | 2016-05-04 | 2019-02-05 | Schaft Inc. | Terrain Evaluation for robot locomotion |
| EP3252658B1 (en) * | 2016-05-30 | 2021-08-11 | Kabushiki Kaisha Toshiba | Information processing apparatus and information processing method |
| US11204610B2 (en) * | 2016-05-30 | 2021-12-21 | Kabushiki Kaisha Toshiba | Information processing apparatus, vehicle, and information processing method using correlation between attributes |
| WO2017206170A1 (zh) * | 2016-06-03 | 2017-12-07 | 尚艳燕 | 一种电动平衡车的控制方法和控制装置 |
| CN105966538B (zh) * | 2016-06-03 | 2018-12-07 | 尚艳燕 | 一种电动平衡车的控制方法和控制装置 |
| WO2017210904A1 (zh) * | 2016-06-08 | 2017-12-14 | 尚艳燕 | 一种电动平衡车的导航方法、导航装置及电动平衡车 |
| KR102442594B1 (ko) * | 2016-06-23 | 2022-09-13 | 한국전자통신연구원 | 조명기를 구비한 스테레오 매칭 시스템에서의 코스트 볼륨 연산장치 및 그 방법 |
| US10210603B2 (en) | 2016-10-17 | 2019-02-19 | Conduent Business Services Llc | Store shelf imaging system and method |
| US10002344B2 (en) | 2016-10-17 | 2018-06-19 | Conduent Business Services, Llc | System and method for retail store promotional price tag detection |
| US10019803B2 (en) | 2016-10-17 | 2018-07-10 | Conduent Business Services, Llc | Store shelf imaging system and method using a vertical LIDAR |
| US10289990B2 (en) | 2016-10-17 | 2019-05-14 | Conduent Business Services, Llc | Store shelf imaging system and method |
| GB2557179B (en) * | 2016-11-29 | 2020-01-01 | Rolls Royce Plc | Methods, apparatus, computer programs and non-transitory computer readable storage mediums for controlling a hyper redundant manipulator |
| CN106482739B (zh) * | 2016-11-30 | 2020-07-17 | 英华达（上海）科技有限公司 | 自动导引运输车导航方法 |
| JP6457469B2 (ja) * | 2016-12-08 | 2019-01-23 | ファナック株式会社 | 移動ロボットの干渉領域設定装置 |
| DE102017100366A1 (de) * | 2017-01-10 | 2018-07-12 | Vorwerk & Co. Interholding Gmbh | Verfahren zum Betrieb eines Bodenbearbeitungsgerätes |
| JP6649306B2 (ja) * | 2017-03-03 | 2020-02-19 | 株式会社東芝 | 情報処理装置、情報処理方法及びプログラム |
| CN106940208A (zh) * | 2017-03-31 | 2017-07-11 | 东华大学 | 机器人目标标定和自身状态监控功能的系统 |
| US10839017B2 (en) | 2017-04-06 | 2020-11-17 | AIBrain Corporation | Adaptive, interactive, and cognitive reasoner of an autonomous robotic system utilizing an advanced memory graph structure |
| US11151992B2 (en) | 2017-04-06 | 2021-10-19 | AIBrain Corporation | Context aware interactive robot |
| US10963493B1 (en) * | 2017-04-06 | 2021-03-30 | AIBrain Corporation | Interactive game with robot system |
| US10810371B2 (en) | 2017-04-06 | 2020-10-20 | AIBrain Corporation | Adaptive, interactive, and cognitive reasoner of an autonomous robotic system |
| US10929759B2 (en) | 2017-04-06 | 2021-02-23 | AIBrain Corporation | Intelligent robot software platform |
| US11862302B2 (en) | 2017-04-24 | 2024-01-02 | Teladoc Health, Inc. | Automated transcription and documentation of tele-health encounters |
| US10483007B2 (en) | 2017-07-25 | 2019-11-19 | Intouch Technologies, Inc. | Modular telehealth cart with thermal imaging and touch screen user interface |
| US11636944B2 (en) | 2017-08-25 | 2023-04-25 | Teladoc Health, Inc. | Connectivity infrastructure for a telehealth platform |
| TWI658431B (zh) * | 2017-10-02 | 2019-05-01 | 緯創資通股份有限公司 | 影像處理方法、影像處理裝置及電腦可讀取記錄媒體 |
| JP6699677B2 (ja) * | 2018-02-06 | 2020-05-27 | ヤマハ株式会社 | 情報処理方法、情報処理装置およびプログラム |
| WO2019167203A1 (ja) * | 2018-02-28 | 2019-09-06 | 本田技研工業株式会社 | 制御装置、作業機械及びプログラム |
| CN108489454A (zh) * | 2018-03-22 | 2018-09-04 | 沈阳上博智像科技有限公司 | 深度距离测量方法、装置、计算机可读存储介质及电子设备 |
| US10617299B2 (en) | 2018-04-27 | 2020-04-14 | Intouch Technologies, Inc. | Telehealth cart that supports a removable tablet with seamless audio/video switching |
| CN108665448B (zh) * | 2018-04-27 | 2022-05-13 | 武汉理工大学 | 一种基于双目视觉的障碍物检测方法 |
| IT201800005091A1 (it) | 2018-05-04 | 2019-11-04 | "Procedimento per monitorare lo stato di funzionamento di una stazione di lavorazione, relativo sistema di monitoraggio e prodotto informatico" | |
| KR102228866B1 (ko) * | 2018-10-18 | 2021-03-17 | 엘지전자 주식회사 | 로봇 및 그의 제어 방법 |
| DE102019200753A1 (de) * | 2019-01-22 | 2020-07-23 | Conti Temic Microelectronic Gmbh | Verfahren zur Überwachung einer Position eines Stützrades eines Anhängers und ein Kamerasystem zur Überwachung einer Position eines Stützrades eines Anhängers sowie ein Kraftfahrzeug mit einem Kamerasystem |
| EP3924868A4 (en) * | 2019-02-11 | 2022-11-30 | 643AI Ltd. | SYSTEMS AND PROCEDURES FOR MANAGEMENT OF MULTIPLE AUTONOMOUS VEHICLES |
| US10809734B2 (en) | 2019-03-13 | 2020-10-20 | Mobile Industrial Robots A/S | Route planning in an autonomous device |
| EP3734388B1 (en) * | 2019-04-29 | 2023-03-15 | Commissariat à l'Energie Atomique et aux Energies Alternatives | Method and apparatus for performing simultaneous localization and mapping |
| US11958183B2 (en) | 2019-09-19 | 2024-04-16 | The Research Foundation For The State University Of New York | Negotiation-based human-robot collaboration via augmented reality |
| CN110989630B (zh) * | 2019-12-30 | 2022-09-30 | 科沃斯机器人股份有限公司 | 自移动机器人控制方法、装置、自移动机器人和存储介质 |
| US11592299B2 (en) | 2020-03-19 | 2023-02-28 | Mobile Industrial Robots A/S | Using static scores to control vehicle operations |
| CN111666876B (zh) * | 2020-06-05 | 2023-06-09 | 阿波罗智联(北京)科技有限公司 | 用于检测障碍物的方法、装置、电子设备和路侧设备 |
| CN114185356B (zh) * | 2022-02-16 | 2022-05-31 | 莱陆科技(天津)有限公司 | 智能识别障碍物并控制机器人通过障碍物的方法及装置 |
| CN115256379A (zh) * | 2022-07-14 | 2022-11-01 | 北京航空航天大学杭州创新研究院 | 一种工业机器人及其工业机器人避障系统 |
Family Cites Families (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4412121A (en) * | 1981-08-28 | 1983-10-25 | S R I International | Implement positioning apparatus and process |
| US4647965A (en) * | 1983-11-02 | 1987-03-03 | Imsand Donald J | Picture processing system for three dimensional movies and video systems |
| US4965499A (en) * | 1987-12-31 | 1990-10-23 | Westinghouse Electric Corp | Parametric path modeling for an optical automatic seam tracker and real time robotic control system |
| US5200818A (en) * | 1991-03-22 | 1993-04-06 | Inbal Neta | Video imaging system with interactive windowing capability |
| US5633995A (en) * | 1991-06-19 | 1997-05-27 | Martin Marietta Corporation | Camera system and methods for extracting 3D model of viewed object |
| JPH06139498A (ja) * | 1992-10-30 | 1994-05-20 | Mitsubishi Electric Corp | 障害物回避装置 |
| KR940017747A (ko) * | 1992-12-29 | 1994-07-27 | 에프. 제이. 스미트 | 영상 처리 디바이스 |
| US5592215A (en) * | 1993-02-03 | 1997-01-07 | Rohm Co., Ltd. | Stereoscopic picture system and stereoscopic display panel therefor |
| JP3369368B2 (ja) * | 1995-10-11 | 2003-01-20 | 富士通株式会社 | 画像処理装置 |
| US5959663A (en) * | 1995-10-19 | 1999-09-28 | Sony Corporation | Stereoscopic image generation method and apparatus thereof |
| JPH1096607A (ja) | 1996-09-25 | 1998-04-14 | Matsushita Electric Ind Co Ltd | 物体検出装置および平面推定方法 |
| US6788802B2 (en) * | 1998-05-21 | 2004-09-07 | Sanyo Electric Co., Ltd. | Optical flow estimation method and image synthesis method |
| US6542249B1 (en) * | 1999-07-20 | 2003-04-01 | The University Of Western Ontario | Three-dimensional measurement method and apparatus |
| US6377013B2 (en) * | 1999-12-24 | 2002-04-23 | Honda Giken Kogyo Kabushiki Kaisha | Control apparatus for legged mobile robot |
| JP2001191283A (ja) * | 1999-12-31 | 2001-07-17 | Sony Corp | ロボット装置及びその制御方法 |
| US6400313B1 (en) * | 2000-01-12 | 2002-06-04 | Honeywell International Inc. | Projection of multi-sensor ray based data histories onto planar grids |
| JP4409035B2 (ja) * | 2000-03-22 | 2010-02-03 | 本田技研工業株式会社 | 画像処理装置、特異箇所検出方法、及び特異箇所検出プログラムを記録した記録媒体 |
| US6788809B1 (en) * | 2000-06-30 | 2004-09-07 | Intel Corporation | System and method for gesture recognition in three dimensions using stereo imaging and color vision |
| JP2002065721A (ja) | 2000-08-29 | 2002-03-05 | Komatsu Ltd | 視覚障害者用環境認識支援装置及びその方法 |
| KR100485040B1 (ko) * | 2000-09-29 | 2005-04-22 | 혼다 기켄 고교 가부시키가이샤 | 2족보행 로봇 |
| US6919909B1 (en) * | 2000-10-24 | 2005-07-19 | Sandia Corporation | Fractional screen video enhancement apparatus |
| JP3994950B2 (ja) * | 2003-09-19 | 2007-10-24 | ソニー株式会社 | 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 |
-
2002
- 2002-03-15 JP JP2002073388A patent/JP3945279B2/ja not_active Expired - Fee Related
-
2003
- 2003-03-13 US US10/387,647 patent/US7386163B2/en not_active Expired - Fee Related
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101380260B1 (ko) | 2008-08-25 | 2014-04-01 | 무라다기카이가부시끼가이샤 | 자율 이동 장치 |
| CN103808277A (zh) * | 2013-12-23 | 2014-05-21 | 天津大学 | 一种多传感器点云拼接误差的修正方法 |
| CN103808277B (zh) * | 2013-12-23 | 2016-07-06 | 天津大学 | 一种多传感器点云拼接误差的修正方法 |
| WO2021065510A1 (ja) * | 2019-10-03 | 2021-04-08 | ソニー株式会社 | 情報処理装置、情報処理方法、情報処理システム及びプログラム |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003269937A (ja) | 2003-09-25 |
| US20040013295A1 (en) | 2004-01-22 |
| US7386163B2 (en) | 2008-06-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3945279B2 (ja) | 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 | |
| JP2003266345A (ja) | 経路計画装置、経路計画方法及び経路計画プログラム並びに移動型ロボット装置 | |
| JP3994950B2 (ja) | 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 | |
| JP4016180B2 (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 | |
| Chen | Kalman filter for robot vision: a survey | |
| US20040230340A1 (en) | Behavior controlling apparatus, behavior control method, behavior control program and mobile robot apparatus | |
| JP4609584B2 (ja) | ロボット装置、顔認識方法及び顔認識装置 | |
| US20060064202A1 (en) | Environment identification device, environment identification method, and robot device | |
| JP3714268B2 (ja) | ロボット装置 | |
| JP4535096B2 (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 | |
| JP3855812B2 (ja) | 距離計測方法、その装置、そのプログラム、その記録媒体及び距離計測装置搭載型ロボット装置 | |
| JP2004110802A (ja) | 環境同定装置、環境同定方法、プログラム及び記録媒体、並びにロボット装置 | |
| JP2003266349A (ja) | 位置認識方法、その装置、そのプログラム、その記録媒体及び位置認識装置搭載型ロボット装置 | |
| US20130116823A1 (en) | Mobile apparatus and walking robot | |
| JP2011209203A (ja) | 自己位置推定装置および自己位置推定方法 | |
| Hueser et al. | Learning of demonstrated grasping skills by stereoscopic tracking of human head configuration | |
| Kim et al. | Providing services using network-based humanoids in a home environment | |
| Lee et al. | Fast perception, planning, and execution for a robotic butler: Wheeled humanoid m-hubo | |
| Kemp et al. | Humanoids | |
| Silva et al. | Navigation and obstacle avoidance: A case study using Pepper robot | |
| JP2009178782A (ja) | 移動体、環境地図生成装置、及び環境地図生成方法 | |
| JP4449410B2 (ja) | ロボット装置及びその物体学習方法 | |
| JP2003271958A (ja) | 画像処理方法、その装置、そのプログラム、その記録媒体及び画像処理装置搭載型ロボット装置 | |
| Jayawardana et al. | Train a robot to climb staircase using vision-base system | |
| Kondaxakis et al. | Robot–robot gesturing for anchoring representations |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20040428 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20060908 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20061010 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20061211 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20070320 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070402 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100420 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110420 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110420 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120420 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120420 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130420 Year of fee payment: 6 |
|
| LAPS | Cancellation because of no payment of annual fees |