JP2003269937A - 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 - Google Patents

Abstract

(57)【要約】 【課題】 正確に床面を抽出し、障害物を認識すること
ができる障害物認識装置を提供する。 【解決手段】 距離画像生成部２２２は、視差画像と同
時変換行列を用いて距離画像を生成する。平面検出部２
２３は、距離画像生成部２２２により生成された距離画
像に基づいて平面パラメータを検出する。座標変換部２
２４は、前記同時変換行列をロボット装置１の接地面で
の座標に変換する。床面検出部２２５は、平面検出部２
２３からの平面パラメータと座標変換部２２４からの座
標変換結果を用いて床面を検出し、その平面パラメータ
を障害物認識部２２６に供給する。障害物認識部２２６
は、床面検出部２２５が検出した床面の平面パラメータ
を用いて床面に載っている点を選択し、この点に基づい
て障害物を認識する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は障害物認識装置及び
方法に関し、特に移動型ロボット装置に適用されて床面
上の障害物を認識する障害物認識装置、障害物認識方法
及び障害物認識プログラム、並びに移動型ロボット装置
に関する。

【０００２】

【従来の技術】自律型ロボットにおいて、自身の周りの
環境を認識して経路計画を立ててそれに応じて移動する
能力は言うまでもなく重要である。従来の移動ロボット
と言えば車輪型のロボットの周囲に超音波センサーを床
に平行に配置して、壁などの反射を測定する方法が取ら
れてきた。これは床に平行に測定することで床自身を測
定する事は無く、反射した点は全て障害物として取り扱
うことが出来るために処理を単純化することができる。
しかしながらこうした方法は、特定の高さの障害物しか
測定しないために小さな段差や穴などの凹んだ障害物を
検出できない。また歩行型ロボット（４足歩行ロボッ
ト、２足歩行ロボットなど）ではこうした測距センサー
は、頭や手先など能動的に姿勢を変えることができる位
置に取り付けられることが多い。

【０００３】また、広く知られるようになった４足歩行
ロボット（エンターテインメントロボット）では、頭部
についた測距センサを使い、ロボットの姿勢から床面パ
ラメータを導出して、測定点が床面に属するか障害物と
して扱うかを判断している。

【０００４】上記方法でも測定点は1点（もしくは姿勢
を変えながら繰り返し測定することで数点）しかなく、
ロボットの周囲の状況を把握したとは言い難い。また1
点しか測定していないために測定に対する信頼度が著し
く低い。

【０００５】

【発明が解決しようとする課題】本発明は、前記実情に
鑑みてなされたものであり、正確に床面を抽出し、障害
物を認識することができる障害物認識装置、障害物認識
方法及び障害物認識プログラムの提供を目的とする。

【０００６】また、本発明は、前記障害物認識装置を適
用して床面上の障害物を認識しながら移動することがで
きる移動型ロボット装置の提供を目的とする。

【０００７】

【課題を解決するための手段】本発明に係る障害物認識
装置は、前記課題を解決するために、ロボット装置に設
けた複数の撮像手段から得られた画像データに基づいて
算出された視差画像と、前記ロボット装置に設けた複数
のセンサ手段から得られたセンサデータ出力とに基づい
て前記複数の撮像手段が配設された位置における前記視
差画像に対応する同時変換行列とを用いて距離画像を生
成する距離画像生成手段と、前記距離画像生成手段によ
り生成された距離画像に基づいて平面パラメータを検出
する平面検出手段と、前記複数の撮像手段の配設された
位置における座標系を床面での座標系に変換する座標変
換手段と、前記座標変換手段による座標変換結果と、前
記平面検出手段によって検出された平面パラメータとに
基づいて床面を検出する床面検出手段と、前記床面検出
手段の平面パラメータを用いて床面に載っている点を選
択し、この点に基づいて障害物を認識する障害物認識手
段とを備える。

【０００８】本発明に係る障害物認識方法は、前記課題
を解決するために、ロボット装置に設けた複数の撮像手
段から得られた画像データに基づいて算出された視差画
像と、前記ロボット装置に設けた複数のセンサ手段から
得られたセンサデータ出力とに基づいて前記複数の撮像
手段が配設された位置における前記視差画像に対応する
同時変換行列とを用いて距離画像を生成する距離画像生
成工程と、前記距離画像生成工程により生成された距離
画像に基づいて平面パラメータを検出する平面検出工程
と、前記複数の撮像手段の配設された位置における座標
系を床面での座標系に変換する座標変換工程と、前記座
標変換工程による座標変換結果と、前記平面検出工程に
よって検出された平面パラメータとに基づいて床面を検
出する床面検出工程と、前記床面検出工程の平面パラメ
ータを用いて床面に載っている点を選択し、この点に基
づいて障害物を認識する障害物認識工程とを備える。

【０００９】本発明に係る障害物認識プログラムは、前
記課題を解決するために、ロボット装置によって実行さ
れることによって障害物を認識する障害物認識プログラ
ムにおいて、前記ロボット装置に設けた複数の撮像手段
から得られた画像データに基づいて算出された視差画像
と、前記ロボット装置に設けた複数のセンサ手段から得
られたセンサデータ出力とに基づいて前記複数の撮像手
段が配設された位置における前記視差画像に対応する同
時変換行列とを用いて距離画像を生成する距離画像生成
工程と、前記距離画像生成工程により生成された距離画
像に基づいて平面パラメータを検出する平面検出工程
と、前記複数の撮像手段の配設された位置における座標
系を床面での座標系に変換する座標変換工程と、前記座
標変換工程による座標変換結果と、前記平面検出工程に
よって検出された平面パラメータとに基づいて床面を検
出する床面検出工程と、前記床面検出工程の平面パラメ
ータを用いて床面に載っている点を選択し、この点に基
づいて障害物を認識する障害物認識工程とを備えてな
る。

【００１０】本発明に係る移動型ロボット装置は、前記
課題を解決するために、頭部ユニットと、少なくとも１
以上の可動脚ユニットと、情報処理手段を有する胴体ユ
ニットとを備えてなり、障害物の床面を認識しながら前
記可動脚ユニットを用いて床面を移動する移動型ロボッ
ト装置において、前記頭部ユニットに設けた複数の撮像
手段から得られた画像データに基づいて算出された視差
画像と、前記頭部ユニット、前記可動脚ユニット、前記
胴体ユニットに設けた複数のセンサ手段から得られたセ
ンサデータ出力とに基づいて前記複数の撮像手段が配設
された位置における前記視差画像に対応する同時変換行
列とを用いて距離画像を生成する距離画像生成手段と、
前記距離画像生成手段により生成された距離画像に基づ
いて平面パラメータを検出する平面検出手段と、前記複
数の撮像手段の配設された位置における座標系を床面で
の座標系に変換する座標変換手段と、前記座標変換手段
による座標変換結果と、前記平面検出手段によって検出
された平面パラメータとに基づいて床面を検出する床面
検出手段と、前記床面検出手段の平面パラメータを用い
て床面に載っている点を選択し、この点に基づいて障害
物を認識する障害物認識手段とを前記胴体ユニットに備
えてなる。

【００１１】本発明では、複数の撮像手段として例えば
ステレオカメラなどを用いた２次元の測距センサを導入
している。これにより画像認識による平面の検出とロボ
ットの姿勢との関係からよりロバストにより正確に床面
を抽出することができるようになっている。

【００１２】

【発明の実施の形態】以下、図面を参照しながら本発明
の実施の形態について説明する。この実施の形態は、本
発明に係る障害物認識装置を２足歩行のロボット装置に
適用したものである。

【００１３】図１は、２足歩行のロボット装置の概略を
示すブロック図である。図１に示すように、ロボット装
置１の頭部ユニット２５０には、２台のＣＣＤカメラ２
００Ｒ，２００Ｌが設けられ、このＣＣＤカメラ２００
Ｒ，２００Ｌの後段には、ステレオ画像処理装置２１０
が設けられている。２台のＣＣＤカメラ（以下、右目２
００Ｒ、左目２００Ｌという。）により撮像された右目
画像２０１Ｒ、左目画像２０１Ｌは、ステレオ画像処理
装置２１０に入力される。ステレオ画像処理装置２１０
は、各画像２０１Ｒ，２０１Ｌの視差情報（disparity
data）（距離情報）を計算し、カラー画像（ＹＵＶ：輝
度Ｙ、ＵＶ色差）２０２及び視差画像（ＹＤＲ：輝度
Ｙ、視差Ｄ、信頼度Ｒ）２０３をフレーム毎に左右交互
に算出する。ここで、視差とは、空間中のある点が左目
及び右目に写像される点の違いを示し、そのカメラから
の距離に応じて変化するものである。

【００１４】このカラー画像２０２及び視差画像２０３
はロボット装置１の体幹部２６０に内蔵されたＣＰＵ
（制御部）２２０に入力される。また、ロボット装置１
の各関節にはアクチュエータ２３０が設けられており、
ＣＰＵ２２０からの指令となる制御信号２３１が供給さ
れて、その指令値に応じてモータを駆動する。各関節
（アクチュエータ）には、ポテンショメータが取り付け
られ、その時のモータの回転角がＣＰＵに送られる。こ
のアクチュエータに取り付けられたポテンショメータ、
足底に取り付けられたタッチセンサ及び体幹部に取り付
けられたジャイロ・センサ等の各センサ２４０は、現在
の関節角度、設置情報、及び姿勢情報等の現在のロボッ
ト装置の状態を計測し、センサデータ２４１としてＣＰ
Ｕ２２０へ出力する。ＣＰＵ２２０は、ステレオ画像処
理装置２１０からのカラー画像２０２及び視差画像２０
３と、アクチュエータの全ての関節角度等のセンサデー
タ２４１とが入力され、後述するソフトウェア構成を実
現する。

【００１５】本実施の形態のソフトウェアは、オブジェ
クト単位で構成され、ロボット装置の位置、移動量、周
囲の障害物、及び環境地図等を認識し、ロボット装置が
最終的に取るべき行動についての行動列を出力する各種
認識処理等を行うものである。なお、ロボット装置の位
置を示す座標として、例えば、後述するランドマーク等
の特定の物体等を座標の原点としたワールド基準系のカ
メラ座標系（以下、絶対座標ともいう。）と、ロボット
装置自身を中心（座標の原点）としたロボット中心座標
系（以下、相対座標ともいう。）との２つの座標を使用
する。

【００１６】図２は、２足歩行のロボット装置のソフト
ウェアの構成を示す模式図である。同図において、丸で
表されているのが、オブジェクト又はプロセスと呼ばれ
るエンティティである。オブジェクト同士が非同期に通
信し合うことで、システム全体が動作する。各オブジェ
クトはメッセージ通信と共有メモリを使用したオブジェ
クト間通信方法によりデータの受け渡し及びＩｎｖｏｋ
ｅを行っている。

【００１７】図２に示すように、ソフトウェア３００
は、キネマティックオドメトリ（Kinematics Odometr
y）ＫＩＮＥ３１０、平面抽出部（Plane Extractor）Ｐ
ＬＥＸ３２０、障害物グリッド算出部（Occupancy Gri
d）ＯＧ３３０、ランドマーク位置検出部（Landmark Se
nsor）ＣＬＳ３４０、絶対座標算出部（Localization）
ＬＺ３５０及び行動決定部（Situated behavior Laye
r）ＳＢＬ３６０から構成され、各オブジェクト単位に
て処理がなされる。これらのソフトウェアの詳細な構成
及び動作については後述する。また、ロボット装置につ
いての詳細な構成及び動作についても後述する。

【００１８】ここで、先ず、上述したロボット装置１に
搭載される本発明の障害物認識装置について説明する。
障害物認識装置は、前記平面抽出部（Plane Extracto
r）ＰＬＥＸ３２０を実行したＣＰＵ２２０内にて構成
される。図３には障害物認識装置２２１の機能ブロック
図を示す。障害物認識装置２２１は、後述する視差画像
から距離画像を生成する距離画像生成部２２２と、距離
画像から平面検出をして平面パラメータを算出する平面
検出部２２３と、後述する同時変換行列を座標変換する
座標変換部２２４と、前記座標変換結果と前記平面パラ
メータからから床面を検出する床面検出部２２５と、床
面の平面パラメータから障害物を認識する障害物認識部
２２６とを備えてなる。

【００１９】距離画像生成部２２２は、ロボット装置１
に設けた二つのＣＣＤカメラから得られた画像データに
基づいて算出された視差画像と、ロボット装置１に設け
た複数のセンサ手段から得られたセンサデータ出力とに
基づいて二つのＣＣＤカメラが配設された位置における
前記視差画像に対応する同時変換行列を用いて距離画像
を生成する。平面検出部２２３は、距離画像生成部２２
２により生成された距離画像に基づいて平面パラメータ
を検出する。座標変換部２２４は、前記同時変換行列を
ロボット装置１の接地面での座標に変換する。床面検出
部２２５は、平面検出部２２３からの平面パラメータと
座標変換部２２４からの座標変換結果を用いて床面を検
出し、その平面パラメータを障害物認識部２２６に供給
する。障害物認識部２２６は、床面検出部２２５が検出
した床面の平面パラメータを用いて床面に載っている点
を選択し、この点に基づいて障害物を認識する。

【００２０】上述した如く、ＣＣＤカメラ２００Ｌ，２
００Ｒによって取り込まれた映像は、ステレオ画像処理
装置２１０に入力され、図４に具体的に示す左右画像２
０１Ｒ，２０１Ｌの視差情報（距離情報）からカラー画
像（ＹＵＶ）２０２及び視差画像（ＹＤＲ）２０３が算
出されてＣＰＵ２２０に入力される。また、ロボット装
置１に複数個設けられた各センサからのセンサデータ２
４０が入力される。図２に示すように、これらの視差情
報及び視差画像からなる画像データ３０１と、ロボット
装置の関節角等のデータであるセンサデータ３０２とが
キネマティックオドメトリＫＩＮＥ３１０に入力され
る。

【００２１】キネマティックオドメトリＫＩＮＥ３１０
は、画像データ３０１及びセンサデータ３０２からなる
入力データに基づいて画像データ３０１の画像が撮像さ
れた時間におけるセンサデータ３０２の関節角を割り出
し、この関節角のデータを使用してロボット装置１が中
心に固定されたロボット中心座標系を頭部ユニットに設
けられたカメラの座標系へ変換する。この場合、本実施
の形態においては、ロボット中心座標系からカメラ座標
系の同時変換行列等を導出し、この同時変換行列３１１
とこれに対応する視差画像３１２を障害物認識装置２２
１（平面抽出部ＰＬＥＸ３２０の実行結果）へ出力す
る。

【００２２】障害物認識装置２２１（平面抽出部ＰＬＥ
Ｘ３２０）は、同時変換行列３１１とこれに対応する視
差画像３１２を受け取り、以下の図５に示す処理手順に
したがって障害物を認識する。

【００２３】先ず、障害物認識装置２２１（平面抽出部
ＰＬＥＸ３２０）の座標変換部２２４は同時変換行列３
１１を、また距離画像生成部２２２は同時変換行列３１
１に対応する視差画像３１２を受け取る（ステップＳ６
１）。そして、距離画像生成部２２２は、視差画像３１
２からレンズ歪みとステレオの取り付け誤差を吸収した
キャリブレーションパラメータを利用して、画素毎にカ
メラ座標から見た３次元の位置データ（X,Y,Z）を距離
画像として生成する（ステップＳ６２）。このとき各３
次元データは、視差画像又は距離画像等の入力画像にお
ける信頼度等から得られる信頼度パラメータを個別に有
し、この信頼度パラメータにより選別されて入力され
る。

【００２４】そして、平面検出部２２３は、選別された
３次元データ群からランダムにデータをサンプリング
し、ハフ変換により平面を推定する。即ち、法線ベクト
ルの向きを（θ，φ）、原点からの距離をｄとしたとき
の平面パラメータ（θ，φ，ｄ）を算出し、平面パラメ
ータを投票空間（θ，Ψ，ｄ）＝（θ，φｃｏｓθ，
ｄ）に直接投票して平面を推定する。これにより、平面
検出部２２３は、画像中の支配的な平面のパラメータを
検出する（ステップＳ６３）。この平面のパラメータは
図５に示すパラメータ空間（θ，φ）（投票空間）のヒ
ストグラムにより検出される。パラメータの投票が小さ
いいと障害物、大きいと平面上のものと見なすことがで
きる。

【００２５】投票の際は、３次元データに付随する信頼
度パラメータ又は平面パラメータの算出方法等により、
１票の投票に対して異なる重み付けをして、投票値を異
ならせると共に、投票値の分布から得られるピーク値を
推定するために、ピーク値近傍における重み付き平均処
理等をすることにより、信頼度の高いデータを推定する
ことができる。そして、この平面パラメータを初期パラ
メータとしてイタレーションを行い、平面を決定するこ
とで、更に信頼度が高い平面を決定することができる。
また、最終的に決定される平面を算出した３次元データ
に付随する信頼度パラメータ及びイタレーションにおけ
る残差誤差等を使用して平面の信頼度を算出し、この平
面信頼度を平面のデータと共に出力することにより、後
段の処理が行い易くなる。このように、平面抽出は、投
票、即ちヒストグラムに基づく確率密度関数の推定によ
り３次元データから、その３次元データに含まれる支配
的な平面のパラメータを決定する確率論的方法で行われ
る。こうして得られた平面のパラメータ使用すれば、元
々画像から得られた距離の測定点が平面上からどれくら
いの距離であるかが分かる。

【００２６】次に、座標変換変換部２２４は、カメラ座
標系の同時変換行列３１１から図７に示すようにロボッ
トの足底接地面への変換を求める（ステップＳ６４）。
これにより、カメラ座標系で表現される接地平面のパラ
メータの算出が行われる。そして、床面検出部２２５
は、平面検出部２２３による前記ステップＳ６３での画
像による平面の検出結果と、座標変換部２２４による前
記ステップＳ６４での足底接地平面の照合から、画像中
の平面パラメータから床面に相当するものを選択する
（ステップＳ６５）。

【００２７】次に、障害物認識部２２６は、床面検出部
２２５が前記ステップＳ６５にて選択した平面パラメー
タを使って、元の距離画像から平面上に載っている点を
選択する（ステップＳ６６）。これは以下に示す式１、
式２を用いて、平面からの距離ｄがしきい値Ｄ_ｔｈより
小さい事を使って判断する。

【００２８】

【数１】

【００２９】

【数２】

【００３０】図９には、しきい値Ｄ_ｔｈを１cmとした範
囲で選択された測定点（×印）を示す。この図９にて、
黒く抜かれている点が平面と判断されなかった点であ
る。

【００３１】したがって、障害物認識部２２６は、前記
ステップＳ６６にて平面上（床面）上に載っている点以
外の点（床面上に無い点）を障害物としてステップＳ６
７にて認識することができる。これらの判断結果は床平
面上の点（ｘ、ｙ）とその高さｚで表現される。例え
ば、高さｚ＜０は、平面より凹んでいる点を示す。

【００３２】これにより、高さがロボットより高い障害
物点はそれをくぐることができるので障害物ではないと
いう判断も可能とすることができる。

【００３３】また、ロボットビュー（図９の（ａ））か
ら得た床面抽出画像（図９の（ｂ））の高さｚを０（ｚ
＝０）となるように座標変換すれば、図９の（ｃ）に示
すように、平面上の２次元の位置で床か障害物かを表現
することができる。

【００３４】このように、障害物認識装置は、多数の測
定点を使って平面の検出を行うために安定した平面を抽
出できる。また、画像から得られる平面候補をロボット
の姿勢から得られる床面パラメータと照合することで正
しい平面を選択できる。また、障害物を認識するのでは
なく、実質的には床面を認識するので障害物の形状や大
きさに因らない認識が可能である。また、床面からの距
離で障害物を表現するので細かい段差や凹みまで検出す
ることができる。またロボットの大きさを考慮して跨い
だり、くぐったりする判断も容易である。さらに、2次
元床面上の障害物表現となるので経路計画などに既存の
移動ロボットで用いられる手法が適用可能であるし、3
次元の表現より高速に演算できる。

【００３５】次に、上述した図２に示すロボット装置１
のソフトウェアについて詳細に説明する。図１０は、図
２に示すソフトウェア３００の動作を示すフローチャー
トである。

【００３６】図２に示すソフトウェア３００のキネマテ
ィックオドメトリＫＩＮＥ３１０には、上述したよう
に、画像データ３０１及びセンサデータ３０２が入力さ
れる。この画像データ３０１は、カラー画像及びステレ
オカメラによる視差画像である。また、センサデータ
は、ロボット装置の関節角等のデータである。キネマテ
ィックオドメトリＫＩＮＥ３１０は、これらの入力デー
タ３０１，３０２を受け取り、メモリに格納されていた
それまでの画像及びセンサデータを更新する（ステップ
Ｓ１０１）。

【００３７】次に、これら画像データ３０１とセンサデ
ータ３０２との時間的な対応を取る（ステップＳ１０２
−１）。即ち、画像データ３０１の画像が撮像された時
間におけるセンサデータ３０２の関節角を割り出す。次
いで、この関節角のデータを使用してロボット装置１が
中心に固定されたロボット中心座標系を頭部ユニットに
設けられたカメラの座標系へ変換する（ステップＳ１０
２−２）。この場合、本実施の形態においては、ロボッ
ト中心座標系からカメラ座標系の同時変換行列等を導出
し、この同時変換行列３１１とこれに対応する画像デー
タとを画像認識を行うオブジェクトへ送信する。即ち、
同時変換行列３１１及びこれに対応する視差画像３１２
を平面抽出部ＰＬＥＸ３２０へ出力し、同時変換行列３
１１及びカラー画像３１３をランドマークセンサ部ＣＬ
Ｓ３４０へ出力する。

【００３８】また、センサデータ３０２から得られる歩
行パラメータと、足底センサを使用した歩数のカウント
数とからロボット装置１の移動量を算出し、ロボット装
置１のロボット装置中心座標系における移動量を算出す
る。以下、ロボット装置中心座標系の移動量をオドメト
リともいう。このオドメトリ３１４を障害物グリッド算
出部ＯＧ３３０及び絶対座標算出部ＬＺ３５０へ出力す
る。

【００３９】平面抽出部ＰＬＥＸ３２０は、キネマティ
ックオドメトリＫＩＮＥ３１０にて算出された同時変換
行列３１１と、これに対応するステレオカメラから得ら
れる視差画像３１２とが入力されると、それまでメモリ
に格納されていたこれらのデータを更新する（ステップ
Ｓ１０３）。そして、ステレオカメラのキャリブレーシ
ョンパラメータ等を使用して視差画像３１２から３次元
の位置データ（レンジデータ）を算出する（ステップＳ
１０４−１）。次いで、このレンジデータからハフ変換
等を使用して、例えば壁及びテーブル等の平面を除く平
面を平面として抽出する。また、座標変換行列３１１か
らロボット装置１の足底が接地している平面との対応を
取り、床面を選択し、床面上にない点、例えば所定の閾
値よりも高い位置にあるもの等を障害物として床面から
の距離を算出し、この障害物情報（obstacle）３２１を
障害物グリッド算出部３３０に出力する（ステップS１
０４−２）。

【００４０】障害物グリッド算出部ＯＧ３３０では、キ
ネマティックオドメトリＫＩＮＥ３１０にて算出された
オドメトリ３１４と、平面抽出部ＰＬＥＸ３２０にて算
出された障害物の観測情報（障害物情報）３２１とが入
力されると、メモリに格納されていたそれまでのデータ
を更新する（ステップＳ１０５）。そして、床面上に障
害物があるか否かの確率を保持する障害物グリッドを確
率的手法により更新する（ステップＳ１０６）。

【００４１】この障害物グリッド算出部ＯＧ３３０は、
ロボット装置１を中心とした例えば周囲４ｍの障害物情
報、即ち、上述した環境地図と、ロボット装置１の向く
方角を示す姿勢情報とを保持しており、上述の方法によ
り、環境地図を更新し、この更新した認識結果（障害物
情報３３１）を出力することにより、上位レイヤ、即
ち、本実施の形態においては、経路計画決定部ＳＢＬ３
６０にて障害物を回避する計画を作成することができ
る。

【００４２】ランドマークセンサＣＬＳ３４０は、キネ
マティックオドメトリＫＩＮＥ３１０から同時変換行列
３１１及びカラー画像３１３が入力されると、予めメモ
リ内に格納されていたこれらのデータを更新する（ステ
ップＳ１０７）。そして、カラー画像３１３の画像処理
を行って、予め認識しているカラーランドマークを検出
する。このカラーランドマークのカラー画像３１３上で
の位置及び大きさをカメラ座標系での位置に変換する。
更に、同時変換行列３１１を使用し、カラーランドマー
クのカメラ座標系での位置をロボット中心位置座標系に
おける位置に変換し、ロボット中心位置座標系における
カラーランドマーク位置の情報（カラーランドマーク相
対位置情報）３４１を絶対座標算出部ＬＺ３５０に出力
する（ステップＳ１０８）。

【００４３】絶対座標算出部ＬＺ３５０は、キネマティ
ックオドメトリＫＩＮＥ３１０からのオドメトリ３１４
と、ランドマークセンサ部ＣＬＳ３４０からのカラーラ
ンドマーク相対位置情報３４１とが入力されると、予め
メモリ内に格納されていたこれらのデータが更新される
（ステップＳ１０９）。そして、絶対座標算出部ＬＺ３
５０が予め認識しているカラーランドマークの絶対座標
（ワールド座標系での位置）、カラーランドマーク相対
位置情報３４１、及びオドメトリ３１４を使用し、確率
的手法によりロボット装置の絶対座標（ワールド座標系
での位置）を算出する。そして、この絶対座標位置３５
１を経路計画決定部ＳＢＬ３６０に出力する。

【００４４】経路計画決定部ＳＢＬ３６０は、障害物グ
リッド算出部ＯＧ３３０から障害物グリッド情報３３１
が入力され、絶対座標算出部ＬＺ３５０から絶対座標位
置３５１が入力されると、予めメモリに格納されていた
これらのデータが更新される（ステップＳ１１１）。そ
して、経路計画決定部ＳＢＬ３６０障害物グリッド算出
部ＯＧ３３０からの障害物情報３３１により、ロボット
装置１の周囲に存在する障害物に関する認識結果を取得
し、絶対座標算出部ＬＺ３５０から現在のロボット装置
１の絶対座標を取得することにより、絶対座標系又はロ
ボット装置中心のロボット中心座標系で与えられた目標
地点に対して障害物に衝突せずに歩行可能な経路を生成
し、経路に応じて経路を遂行する動作コマンドを発行す
る。即ち、入力データから状況に応じてロボット装置１
が取るべき行動を決定し、その行動列を出力する（ステ
ップＳ１１２）。

【００４５】また、人間によるナビゲーションの場合に
は、障害物グリッド算出部ＯＧ３３０からロボット装置
の周囲に存在する障害物に関する認識結果及び絶対座標
算出部ＬＺ３５０からの現在のロボット装置の位置の絶
対座標をユーザに提供し、このユーザからの入力に応じ
て動作コマンドを発行させる。

【００４６】図１１は、上述のソフトウェアに入力され
るデータの流れを模式的に示す図である。なお、図１１
において、図１及び図２に示す構成と同一構成要素には
同一符号を付してその詳細な説明は省略する。

【００４７】顔検出部ＦＤＴ（ＦａｃｅＤｅｔｅｃｔｏ
ｒ）３７１は、画像フレーム中から顔領域を検出するオ
ブジェクトであり、カメラ等の画像入力装置からカラー
画像２０２を受け取り、それを例えば９段階のスケール
画像に縮小変換する。この全ての画像の中から顔に相当
する矩形領域を探索する。重なりあった候補領域を削減
して最終的に顔と判断された領域に関する位置、大き
さ、及び特徴量等の情報３７２を出力して、顔識別部Ｆ
Ｉ（ＦａｃｅＩｄｅｎｔｉｆｙ）３７７へ送る。

【００４８】顔識別部ＦＩ３７７は、検出された顔画像
を識別するオブジェクトであり、顔の領域を示す矩形領
域画像からなる情報３７２を顔検出部ＦＤＴ３７１から
受け取り、この顔画像がメモリに格納されている手持ち
の人物辞書のうちでどの人物に相当するかを比較して人
物の識別を行う。そして、顔検出部ＥＤＴ３７１から受
け取った顔画像の顔画像領域の位置、大きさ情報と共に
人物のＩＤ情報３７８をＤＩＬ３７９に出力する。

【００４９】色認識部ＭＣＴ（ＭｕｌｔｉＣｏｌｏｒＴ
ｒａｃｋｅｒ）３７３は、色認識を行うオブジェクトで
あり、カメラ等の画像入力装置からカラー画像２０２を
受け取り、予め保有している複数のカラー・モデル情報
に基づいて色領域を抽出し、連続した領域に分割する。
色認識部ＭＣＴ３７３は、分割した各領域の位置、大き
さ及び特徴量等の情報３７４をＤＩＬ３７９に出力す
る。

【００５０】動き検出部ＭＤＴ（ＭｏｔｉｏｎＤｅｔｅ
ｃｔｏｒ）３７５は、画像の中で動いている部分を検出
する部分であり、検出した動き領域の情報３７６を距離
情報付加部ＤＩＬ３７９に出力する。

【００５１】距離情報付加部ＤＩＬ（Ｄｉｓｔａｎｃｅ
ＩｎｆｏｒｍａｔｉｏｎＬｉｎｋｅｒ）３７９は、入力
された２次元の情報に距離情報を付加して３次元の情報
を出力するオブジェクトであり、顔検出部ＦＩ３７７か
らのＩＤ情報３７８、色認識部ＭＣＴ３７３からの分割
した各領域の位置、大きさ及び特徴量等の情報３７４、
動き検出部ＭＤＴ３７５からの動き領域の情報３７６に
距離情報を付加して３次元の情報３８０を短期記憶部Ｓ
ＴＭ（ＳｈｏｒｔＴｅｒｍＭｅｍｏｒｙ）３８１に出力
する。

【００５２】短期記憶部ＳＴＭ３８１は、ロボット装置
１の外部環境に関する情報を比較的短い時間だけ保持す
るオブジェクトであり、ＡｒｔｈｕｒＤｅｃｏｄｅｒ
（図示せず）から音声認識結果（単語、音源方向、確信
度）を受け取り、色認識部ＭＣＴ３７３から肌色の領域
の位置、大きさと顔領域の位置、大きさを受け取り、顔
識別部ＦＩ３７７から人物のＩＤ情報等を受け取る。ま
た、ロボット装置１の機体上の各センサからロボット装
置の首の方向（関節角）を受け取る。そして、これらの
認識結果及びセンサ出力を統合的に使って、現在どこに
どの人物がいて、しゃべった言葉がどの人物のものであ
り、その人物とはこれまでにどんな対話を行ったのかと
いう情報を保存する。こうした物体、即ちターゲットに
関する物理情報と時間方向でみたイベント（履歴）を出
力として、経路計画決定部（状況依存行動階層）（ＳＢ
Ｌ）３６０等の上位モジュールに渡す。

【００５３】経路計画決定部ＳＢＬは、上述の短期記憶
部ＳＴＭ３８１からの情報を基にロボット装置１の行動
（状況に依存した行動）を決定するオブジェクトであ
る。複数の行動を同時に評価したり、実行したりするこ
とができる。また、行動を切り替えて機体をスリープ状
態にしておき、別の行動を起動することができる。

【００５４】次に、既に詳細に説明した平面抽出部ＰＬ
ＥＸ３２０の障害物検出処理を除いた各オブジェクトに
ついて更に詳細に説明する。

【００５５】障害物グリッド算出部ＯＧ３３０は、所定
の大きさのグリッドに分割された、ロボット中心座標系
の地図情報である環境地図と、例えば環境地図上におい
てｘ軸方向又はｙ軸方向等の所定の方角からロボット装
置が向く方角を示す姿勢情報とを有している。また、環
境地図は、障害物情報が平面抽出部ＰＬＥＸから入力さ
れ、障害物として認識されているグリッド（障害物占有
領域）を有する。この障害物グリッド算出部ＯＧ３３０
は、ロボット装置が移動すると、即ち、オドメトリ３１
４が入力されると、予めメモリ内に格納されていた環境
地図及び姿勢情報を、ロボット装置の姿勢の変化（差分
移動角）及び移動距離（差分移動量）に伴って、予め認
識している環境地図及び姿勢方向の情報を更新する。こ
こで、差分移動量がグリッドサイズより小さければ環境
地図を更新せず、移動量がグリッドのサイズを超える場
合は環境地図を更新する。また、環境地図及びそのグリ
ッドの大きさを必要に応じて適宜変更することにより、
計算量及びメモリコピーコスト等を低減することができ
る。

【００５６】以下、障害物グリッド算出部ＯＧ３３０に
ついて図１２〜図１７を参照して詳細に説明する。図１
２は、障害物グリッド算出部ＯＧ３３０にて認識される
環境地図を示す模式図である。図１２に示すように、環
境地図４００は、ロボット装置（移動体）４０３を中心
とした周囲の地図情報である環境地図（ロボット中心座
標系での地図情報）であり、所定の大きさのグリッド４
１０に分割されている。そして、ロボット装置４０３が
移動すると、常にロボット装置４０３が環境地図４００
の中心に位置する中心グリッド４０１内に位置するよう
に更新されるものである。この図１２に示す環境地図４
００及びグリッド４１０の大きさは、必要に応じて適宜
変更することができる。即ち、例えば、精細な環境地図
が不要であるとき等は、環境地図４００を小さくする
か、又はグリッド４１０の大きさを大きくして解像度を
下げることにより、計算量及びメモリコピーコスト等を
小さくすることができる。

【００５７】また、図１２において、ハッチングして示
すグリッド４０２は、障害物として認識される最小単位
の領域（障害物占有領域）を示す。位置認識装置におけ
る障害物の認識方法としては、例えば、ステレオカメラ
等による視差画像から求める方法、レンジファインダー
等の電波の跳ね返りを利用する方法、及び超音波センサ
等の距離測定センサによる方法等がある。そして、これ
らの方法により認識された障害物の位置を位置認識装置
が環境地図のグリッドの属性として認識（表示）する。
本実施の形態においては、障害物情報は、前述した平面
抽出部ＰＬＥＸ３２０から入力される。

【００５８】障害物グリッド算出部ＯＧ３３０は、上述
したように、データ画像及びセンサデータが入力される
キネマティックオドメトリＫＩＮＥ３１０からオドメト
リ３１４が入力されると、予めメモリ内に格納されてい
た環境地図を更新する。メモリ内には、ロボット装置４
０３を中心としたロボット装置の周囲の障害物等の情報
を有する地図情報である環境地図４００と共に、この地
図上においてのロボット装置（移動体）の向きを示す姿
勢情報４０４とを有するものである。姿勢情報４０４
は、所定の方角、例えばｘ軸方向又はｙ軸方向からロボ
ット装置４０３が向く方角を示すものである。そして、
ロボット装置の姿勢の変化及び移動等に伴って、図１２
に示す予め認識している環境地図及び姿勢方向の情報を
更新する。

【００５９】先ず、環境地図の更新方法について説明す
る。本発明の位置認識装置においては、ロボット装置等
の移動量の大きさによっては環境地図を更新しない。即
ち、移動量が所定の領域（グリッド）内であれば環境地
図を更新せず、移動量がグリッドのサイズを超える場合
は環境地図を更新するものである。

【００６０】図１３（ａ），（ｂ）及び（ｃ），（ｄ）
は移動量に応じた環境地図の更新方法を示す図であっ
て、夫々環境地図を更新しない場合及び環境地図を更新
する場合の環境地図を示す模式図である。図１３（ａ）
に示すように、環境地図５００における中心グリッド５
０１内にロボット装置５０３が位置している。ハッチン
グして示すグリッド５０２は障害物領域を示す。このロ
ボット装置５０３の移動量５０５が小さく、図１３
（ｂ）に示すように、ロボット装置５０３の位置が移動
後のロボット装置５０６においても、そのロボット５０
６の位置が中心グリッド５０１内であった場合、環境地
図５００は更新しない。この場合は、中心グリッド５０
１内におけるロボット装置の移動位置情報だけを更新す
る。

【００６１】一方、図１３（ａ）と同様に、図１３
（ｃ）に示すように、環境地図５００における中心グリ
ッド５０１内に位置したロボット装置５０３の移動量５
１５が大きく中心グリッド５０１を越えた場合、図１３
（ｄ）に示すように、移動後のロボット装置５１３が環
境地図の中心グリッド５１１に位置するように、移動し
たグリッドの数だけ環境地図５００をシフトさせて更新
して更新環境地図５１０を得る。

【００６２】即ち、障害物領域として認識される大きさ
の最小単位であるグリッドの大きさをＣＳ（Cell Siz
e）、グリッド内のロボット装置の位置を（Ｂｘ，Ｂ
ｙ）、ロボット装置の２次元方向の移動量を（ｄｘ，ｄ
ｙ）とすると、グリッドのシフトの大きさ（Ｓｘ，Ｓ
ｙ）は下記数式（３）で示される。

【００６３】

【数３】

【００６４】ここで、上記数式（３）において、［］
は、ガウス記号であって、［］内の値を超えない最大の
整数値を示すものとする。また、グリッド内のロボット
位置（Ｒｘ，Ｒｙ）は、下記数式（４）で示される。

【００６５】

【数４】

【００６６】上記数式（３）及び数式（４）の値を図１
４に示す。図１４（ａ）に示すように、ロボット装置６
０３は、グリッドサイズＣＳを有する複数個のグリッド
に分割された環境地図６００における中心グリッド６０
１内に位置している。環境地図６００内にはハッチング
して示す障害物領域を示す複数個のグリッド６０２を有
する。ここで、中心グリッド６０１内の例えば図中左下
隅の点６０７をグリッド６０１内のロボット装置６０３
の位置原点とすることができる。このロボット装置６０
３の位置は、原点６０７からの距離として位置（Ｂｘ，
Ｂｙ）として表すことができる。このロボット装置６０
３が移動量（ｄｘ，ｄｙ）６０５の移動をしてグリッド
６０８に移動した場合、移動後のロボット装置６１３の
位置は、（Ｂｘ＋ｄｘ，Ｂｙ＋ｄｙ）として表すことが
できる。

【００６７】ロボット装置６０３が移動量（ｄｘ，ｄ
ｙ）６０５の移動をした場合、環境地図シフト量（Ｓ
ｘ，Ｓｙ）は、上記数式（１）から算出することができ
る。ここで、更新環境地図６１０において、環境地図６
００をｘ軸方向のシフト量Ｓｘ及びｙ軸方向のシフト量
Ｓｙだけ、夫々ｘ軸方向の移動量ｄｘ及びｙ軸方向の移
動量ｄｙとは逆方向に環境地図をシフトさせたものとし
て更新することができる。なお、ここでは、ロボット装
置の移動量（ｄｘ，ｄｙ）は右向きを正とし、環境地図
のシフト量（Ｓｘ，Ｓｙ）は、移動方向とは逆方向であ
る左向きを正としている。

【００６８】こうして移動後のロボット装置６１３は、
更新環境地図６１０の中心グリッド６１１に位置する。
ここで、移動後のロボット装置６１３の環境地図シフト
後の更新環境地図６１０の中心グリッド６１１における
ロボット位置（Ａｘ，Ａｙ）は、移動前のロボット装置
６０３のシフト前環境地図６００の中心グリッド６０１
におけるロボット位置（Ｂｘ，Ｂｙ）と、グリッド内移
動量（Ｒｘ，Ｒｙ）６１５とからロボット位置（Ａｘ，
Ａｙ）＝（Ｂｘ＋Ｒｘ，Ｂｙ＋Ｒｙ）＝（Ｂｘ＋ｄｘ−
ＣＳ×Ｓｘ，Ｂｙ＋ｄｙ−ＣＳ×Ｓｙ）として算出する
ことができる。

【００６９】次に、ロボット装置の姿勢方向の情報の更
新について説明する。図１５（ａ）及び（ｂ）は、姿勢
方向の夫々更新前後の様子を示す模式図である。ロボッ
ト装置の姿勢情報に関しては、環境地図を固定した座標
系に対する角度として示すことができる、即ち、ロボッ
ト装置が姿勢方向ｄαだけ変化させた場合、ロボット装
置の姿勢方向の情報のみが更新され、環境地図は更新さ
れない。

【００７０】即ち、図１５（ａ）に示すように、障害物
領域である複数個のグリッド７０２を有する環境地図７
００における中心グリッド７０１に位置するロボット装
置７０３において、移動前のロボット姿勢方向をαとす
る。姿勢方向αは、例えば時計回りを正とし、ｙ軸から
の傾き又はｘ軸からの傾き等として示すことができる。
ここで、ロボット装置７０３が姿勢回転角度ｄαで回転
した場合、図１５（ｂ）に示すように、移動後後のロボ
ット姿勢方向（姿勢情報）は、α＋ｄαとして示すこと
ができる。

【００７１】図１６は、障害物グリッド算出部ＯＧ３３
０のもたらす効果を示す図である。図１６に示すよう
に、環境地図及びロボット装置自身の姿勢方向の情報の
更新、即ちキネマティックオドメトリＫＩＮＥ８０１か
ら障害物グリッド算出部８０２への移動量（オドメト
リ）８０３の受け渡しは、所定の時間毎に自動的に行う
こともできるが、ロボット装置が１歩歩く毎に行うよう
にする。このことにより、移動していないときは更新作
業をしないようにし、更新作業を行うことによる計算コ
スト及びメモリコピーコストをなくすことでロボット装
置の例えばＣＰＵ等の負荷を低減することができる。ま
た、ソフトウェアの構成がオブジェクト単位で処理を行
うような場合は、受け渡すデータは、ロボット装置の移
動差分量・回転差分量（ｄｘ，ｄｙ，ｄα）のみとでよ
く、ロボット装置の絶対座標及び絶対角度を受け渡すよ
りもデータ量が少なく、且つ高い精度を得ることができ
る。

【００７２】また、エンタテーメントロボット装置が位
置認識以外の処理を行う場合、例えば音声認識及び顔認
識等、位置認識と直接関係がなく、位置認識のプライオ
リティを下げてもよい場合がある。このような場合にお
いて、位置認識装置（位置認識のオブジェクト）を停止
すると、位置認識のプライオリティが高くなった際に、
停止していた間に移動があったりする等して、停止前の
位置認識結果と整合が取れなくなるという問題（Ｋｉｄ
Ｎａｐ問題）が生じる。従って、位置認識のプライオリ
ティを下げてよい場合においては、環境地図のグリッド
の大きさＣＳを大きくするか、又は環境地図サイズの大
きさ自身を小さくするかすることにより、位置認識のオ
ブジェクトを停止することなく、ＣＰＵのリソースを低
減することができる。

【００７３】更に、以上説明したような処理で得られる
環境地図及びロボット装置の姿勢情報を図１７に示す操
作盤に表示することができる。図１７は、例えばユーザ
がロボット装置の遠隔操作時に使用する操作盤を示す模
式図である。図１７に示すように、操作盤９００には、
ロボット装置のカメラ画像９０１及び環境地図９１０が
表示される。操作盤９００には、その他、例えばロボッ
ト装置９１３を移動させる操作ボタン９２０等が備えら
れている。

【００７４】移動ロボット装置の重要な利用方法として
は、自律動作以外にユーザが遠隔操作をする利用方法が
ある。従来、ロボットの操作盤にロボット装置に装備さ
れたカメラによって得られる画像を表示するものがあ
る。しかしながら、特に、視野の限られたカメラだけで
はユーザがロボット装置周辺の環境を把握してロボット
装置を操作することは困難であった。しかし、本実施の
形態のように、ロボット装置９１３に装備されたＣＣＤ
等のカメラからのカメラ画像９０１のみのデータで遠隔
操作する従来のロボット制御よりも、遠隔操作用の操作
盤９００に環境地図９１０を表示することによって、ロ
ボット制御の操作性が飛躍的に向上する。

【００７５】ここで、ロボットの姿勢情報が変化しても
環境地図を更新しないものとして説明したが、通常、ロ
ボット装置においてはＣＣＤカメラ等の撮像装置は頭部
ユニット前面に設けられており、カメラ画像９０１はロ
ボット装置の前方の画像を表示する。従って、姿勢情報
が変化してロボット装置の向きが例えば１８０°回転し
た場合、カメラ画像と環境地図との方向が逆になる。よ
って、姿勢情報が、例えば４５°又は９０°等、所定の
角度以上回転した場合は、この回転に伴い環境地図を回
転させることにより、更にロボット制御の操作性が向上
する。

【００７６】次に、ランドマーク位置検出部ＣＬＳ３４
０は、図１８に示すように、例えば、グリーン部分１０
０１、ピンク部分１００２、又はブルー部分１００３等
のカラーを有する人工的なカラーランドマーク１００４
を含む環境において、ロボット装置１のセンサ情報と、
ロボット装置１が行った動作情報によって、ロボット装
置の自己位置（位置及び姿勢）を同定する。

【００７７】例えば、２次元的な作業空間上では、略等
間隔状にグリッド（ｘ，ｙ）を設け、各グリッドの位置
ｌ（localization）毎にロボット装置が存在する確率ｐ
（ｌ）を管理する。この存在確率ｐ（ｌ）は、ロボット
装置の移動（ｍｏｖｅ）、即ち内界観測情報ａ、又はラ
ンドマークの観測、即ち外界観測情報ｓの入力に応じて
更新される。

【００７８】存在確率ｐ（ｌ）は、ロボット装置の以前
の状態、即ち自己位置ｌ'における存在確率ｐ（ｌ'）
と、以前の状態ｌ'において移動ａを行ったときに状態
ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）に依拠す
る。即ち、これまでの各状態ｌ'になる確率ｐ（ｌ'）
と、状態ｌ'において移動ａを行ったときに状態ｌにな
るという遷移確率ｐ（ｌ｜ａ，ｌ'）の積を加算してい
く（若しくは積分する）ことによって、現在の状態、即
ち自己位置ｌになるという存在確率ｐ（ｌ）に収束す
る。従って、外界観測結果としてのロボット装置の移動
ａが観測されたときには、各グリッドでは、下記数式
（５）に従ってロボット装置の存在確率ｐ（ｌ）を更新
することができる。

【００７９】

【数５】

【００８０】また、状態、即ち自己位置ｌにロボット装
置が存在するという存在確率ｐ（ｌ）は、存在確率ｐ
（ｌ）とこの状態ｌにおいてランドマークを観察すると
いう遷移確率ｐ（ｓ｜ｌ）に依拠する。したがって、状
態ｌにおいてランドマークの観測、即ち外界観測情報ｓ
が入力された場合には、下式に従ってロボット装置の存
在確率ｐ（ｌ）を更新することができる。但し、下記数
式（４）に示すように右辺では、正規化のため、ランド
マークを観察するという確率ｐ（ｓ）で除して正規化し
ている。

【００８１】

【数６】

【００８２】図１９は、マルコフ・ローカリゼーション
と拡張カルマン・フィルタを併用したランドマークセン
サＣＬＳ（自己位置同定システム）３４０の機能構成を
模式的に示している。同図に示すように、このランドマ
ークセンサＣＬＳ３４０は、マルコフ・ローカリゼーシ
ョン部（ＭＬ）３４２と、拡張カルマン・フィルタ部
（ＥＫＬ）３４３と、拡張カルマン・フィルタ部３４３
の動作を制御するＥＫＬ制御部３４４とで構成されてい
る。

【００８３】マルコフ・ローカリゼーション部３４２
は、作業空間内の自分の位置を離散的なグリッド上の自
己位置確率密度分布として保持し、ランドマークの観測
に関する外界観測情報ｓと、ロボット装置自身の動作に
関する内界観測情報ａを入力して、自己位置確率密度分
布の更新を行う。そして、各時刻において、自己位置確
率密度分布の最も高い値を持つグリッドを自己位置の推
定結果としてＥＫＬ制御部３４４に出力する。

【００８４】図２０は、マルコフ・ローカリゼーション
部３４２により求められた各グリッド上での自己位置確
率密度分布を表している。同図では、各グリッドにおけ
る確率密度を濃淡により表現している。最も色が濃い、
即ち自己位置確率密度分布が最も高い値を有するグリッ
ドが自己位置の推定結果となる。

【００８５】このマルコフ・ローカリゼーションによる
自己位置同定は、センサのノイズに対してロバストであ
り、同定解の精度は粗いが解の収束速度が速いことを主
な特徴とする。

【００８６】一方、図１９に示す拡張カルマン・フィル
タ部３４３は、自己位置を状態変数［ｘ，ｙ，θ］を実
測値として保持し、環境内に設置されたカラーランドマ
ーク１００４を観測すると、ランドマークからの相対位
置に基づいて自己位置を推定する。また、自身の動作情
報を観測すると、その動作情報に基づいて状態量の推定
を行う。

【００８７】拡張カルマン・フィルタ部３４３は、ロボ
ット装置自身の動作情報ａと状態、即ち、自己位置ｌと
の関係を規定した状態モデルと、自己位置ｌとランドマ
ークの観測情報ｓとの関係を規定した観測モデルとで構
成されている。

【００８８】状態モデルは、ロボット装置が状態（自己
位置）ｌにおいて動作ａを行った場合における理論的な
状態ｌを与える遷移関数Ｆ（ｌ，ａ）を備えている。実
際には理論的な状態ｌに対してノイズ成分ｗが重畳され
ることから、ロボット装置の状態ｌは状態モデルにより
下記数式（７）のように収束する。

【００８９】

【数７】

【００９０】また、観測モデルは、ロボット装置が状
態、即ち自己位置ｉにおいて、ある既知の環境Ｅｎｖ
（例えばランドマークの位置など）に関する観測の理論
値ｓを与える観測関数Ｈ（Ｅｎｖ，ｌ）を備えている。
実際には観測の理論値に対してノイズ成分ｖが重畳され
ることから、観測値ｓは観測モデルにより下記数式
（８）のように収束する。

【００９１】

【数８】

【００９２】なお、状態ｌ並びに観測ｓに重畳されるそ
れぞれのノイズｗ及びｖは、ここではゼロを中央値とす
るガウス分布として仮定する。

【００９３】ロボット装置自身の動作情報ａと自己位置
ｌとの関係を規定した状態モデルと自己位置ｌとランド
マークの観測情報ｓとの関係を規定した観測モデルを備
えた拡張カルマン・フィルタ部３４３において、動作情
報ａは内界観測結果として、ランドマークの観測情報ｓ
は外界観測結果として、夫々既知である。従って、ロボ
ット装置の自己位置同定を、ロボット装置の状態ｌをロ
ボット装置の動作情報ａ及び観測情報ｓにより推定する
という問題に帰着することができる。ここでは、ロボッ
ト装置の動作ａ、状態ｌ、及び観測ｓをそれぞれ下記に
示すガウス分布として表現することができる。

【００９４】

【数９】

【００９５】ある時点でのロボット装置の状態ｌがある
中央値と及び共分散を有するガウス分布をとるものとし
て推定する。そして、ロボット装置の動作ａが観測され
たときには、状態ｌの推定値に関する中央値並びに共分
散を下記数式（９−１），（９−２）により更新するこ
とができる。

【００９６】

【数１０】

【００９７】ここで、▽Ｆｌ，▽Ｆａは、下記とする。

【００９８】

【数１１】

【００９９】同様に、ある時点でのロボット装置の状態
ｌがある中央値及び共分散を有するガウス分布をとるも
のとして推定する。そして、ランドマークの観測情報ｓ
が観測されたときには、状態ｌの推定値に関する中央値
並びに共分散を下記数式（１０−１），（１０−２）に
より更新することができる。

【０１００】

【数１２】

【０１０１】ここで、各パラメータは下記とする。

【０１０２】

【数１３】

【０１０３】拡張カルマン・フィルタ３４３はセンサ情
報に対するロバスト性に優れているので、拡張カルマン
・フィルタ部３４３の推定結果は、ランドマークセンサ
ＣＬＳ３４０全体の出力としている。

【０１０４】ＥＫＬ制御部３４４は、マルコフ・ローカ
リゼーション部３４２の出力結果に応じて拡張カルマン
・フィルタ部３４４の動作を制御する。より具体的に
は、マルコフ・ローカリゼーション部３４２の自己位置
推定結果に基づいて、ランドマークの観測情報ｓに関す
る妥当性を検証する。観測情報ｓの妥当性は、マルコフ
・ローカリゼーション部３４２において最大の存在確率
となるグリッド位置ｍｌｐにおいてランドマークを観測
する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈ
ｒｅｓｈobsを越えたかどうかで判断することができ
る。

【０１０５】グリッド位置ｍｌｐにおいてランドマーク
を観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈ
ｒｅｓｈobsを下回る場合には、センサ・ノイズに対し
てロバストなマルコフ・ローカリゼーション部３４２に
おいてさえ、センサ・ノイズのために同定解が充分に収
束していないことが推測される。このような場合、セン
サ・ノイズに対するロバスト性が低い拡張カルマン・フ
ィルタ部３４３において自己位置を推定しても、精度が
高い解が得られず、むしろ演算時間を浪費するだけであ
る。このため、観測情報ｓが妥当でないと判断された場
合には、切換器３４５を用いて拡張カルマン・フィルタ
部３４３への外界観測情報ｓ、即ちランドマークの観測
情報の入力を遮断して、拡張カルマン・フィルタ部３４
３における自己位置推定値の更新を停止させる。

【０１０６】また、ＥＫＬ制御部３４４は、拡張カルマ
ン・フィルタ部３４３の自己位置推定結果の妥当性も検
証する。自己位置推定結果の妥当性は、推定された状態
ｌの中央値及び共分散を用いて、マルコフ・ローカリゼ
ーション部３４２から出力される存在確率ｐ（ｌ）との
分布比較テストにより判断することができる。分布比較
テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ
−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。

【０１０７】分布比較テストによりマルコフ・ローカリ
ゼーション部３４２と拡張カルマン・フィルタ部３４３
との各確率分布が類似していない場合には、センサ・ノ
イズに対するロバスト性が低い拡張カルマン・フィルタ
部３４３における自己位置推定値の方が、センサ・ノイ
ズの影響により妥当でないと判断することができる。こ
のような場合、ＥＫＬ制御部３４４は拡張カルマン・フ
ィルタ部３４３の再初期化を行わせる。何故ならば、拡
張カルマン・フィルタは再復帰に多大な時間を要するか
らである。

【０１０８】次に、ランドマークセンサＣＬＳ３４０の
動作について説明する。図２１は、ランドマークセンサ
ＣＬＳ３４０の動作を示すフローチャートである。図２
１に示すように、ロボット装置１の移動に関する内界観
測情報ａがランドマークセンサＣＬＳ３４０に入力され
ると、まずマルコフ・ローカリゼーション部３４２にお
いて、上記数式（５）を用いて自己位置推定値の更新処
理が行われる（ステップＳ２０１）。次いで、拡張カル
マン・フィルタ部３４３において、上記数式数（９）を
用いて自己位置推定値の更新処理が行われる（ステップ
Ｓ２０２）。

【０１０９】また、ランドマークの観測に関する外界観
測情報ｓがランドマークセンサＣＬＳ３４０に入力され
ると、まずマルコフ・ローカリゼーション部３４２にお
いて、上記数式（６）を用いて、自己位置推定値の更新
処理が行われる（ステップＳ２１１）。

【０１１０】マルコフ・ローカリゼーション部３４２の
出力結果は、ＥＫＬ制御部３４４に入力されて、観測情
報ｓの妥当性が検証される（ステップＳ２１２）。観測
情報ｓの妥当性は、マルコフ・ローカリゼーション部３
４２において最大の存在確率となるグリッド位置ｍｌｐ
においてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）
が所定の閾値パラメータｔｈｒｅｓｈobsを越えたかど
うかで判断することができる。

【０１１１】グリッド位置ｍｌｐにおいてランドマーク
を観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈ
ｒｅｓｈobsを下回る場合には、センサ・ノイズに対し
てロバストなマルコフ・ローカリゼーション部３４２に
おいてさえ、センサ・ノイズのために同定解が充分に収
束していないことが推測される。このような場合、セン
サ・ノイズに対するロバスト性が低い拡張カルマン・フ
ィルタ部３４３において自己位置を推定しても、精度の
よい解が得られず、むしろ演算時間を浪費するだけであ
る。このため、観測情報ｓが妥当でないと判断された場
合には、切換器３４５を用いて拡張カルマン・フィルタ
部３４３への外界観測情報ｓ、即ちランドマークの観測
情報の入力を遮断して、拡張カルマン・フィルタ部３４
３における自己位置推定値の更新を停止させる。

【０１１２】他方、観測情報ｓを検証した結果、妥当性
を満たす、即ちグリッド位置ｍｌｐにおいてランドマー
クを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔ
ｈｒｅｓｈobsを上回る場合には、さらに拡張カルマン
・フィルタ部３４３において、上記数式（１０−１），
（１０−２）を用いて更新処理が行われる（ステップＳ
２１３）。

【０１１３】拡張カルマン・フィルタ部３４３による自
己位置推定の結果は、ＥＫＬ制御部３４４に入力され
て、その妥当性が検証される（ステップＳ２１４）。拡
張カルマン・フィルタ部３４３による自己位置推定結果
の妥当性は、推定された状態ｌの中央値及び共分散を用
いて、マルコフ・ローカリゼーション部３４２から出力
される存在確率ｐ（ｌ）との分布比較テストにより判断
することができる。分布比較テストの一例は、カイ自乗
テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋ
ｆ）である。

【０１１４】分布比較テストによりマルコフ・ローカリ
ゼーション部３４２と拡張カルマン・フィルタ部３４３
それぞれの確率分布が類似していない場合には、センサ
・ノイズに対するロバスト性が低い拡張カルマン・フィ
ルタ部３４３における自己位置推定値の方が、センサ・
ノイズの影響により妥当でないと判断することができ
る。このような場合、ＥＫＬ制御部３４４は拡張カルマ
ン・フィルタ部３４３の再初期化を行わせる（ステップ
Ｓ２１５）。何故ならば、拡張カルマン・フィルタは再
復帰に多大な時間を要するからである。

【０１１５】このようにして、ランドマークセンサＣＬ
Ｓ３４０では、広い範囲で比較的短い探索時間で探索を
行う大域探索と高精度だが探索時間を要する局所的探索
とを併用して、高精度で、高速且つロバストな自己位置
同定を行うことができる。次に、行動決定部ＳＢＬ３６
０は、障害物グリッド算出部ＯＧ３３０からの障害物情
報３３１により、ロボット装置１の周囲に存在する障害
物に関する認識結果を取得し、絶対座標算出部ＬＺ３５
０から現在のロボット装置１の絶対座標を取得すること
により、絶対座標系又はロボット装置中心のロボット中
心座標系で与えられた目標地点に対して障害物に衝突せ
ずに歩行可能な経路を生成し、経路に応じて経路を遂行
する動作コマンドを発行する。即ち、入力データから状
況に応じてロボット装置１が取るべき行動を決定し、そ
の行動列を出力する。ここで、障害物グリッド算出部Ｏ
Ｇ３３０の、障害物情報３３１に基づいた処理について
は後述する。

【０１１６】障害物グリッド算出部ＯＧ３３０からの障
害物情報３３１により生成される障害物地図上の点は、
図２２に示すように以下の３つの種類に分類される。

【０１１７】第１の点は、障害物の存在する点（図中の
黒色で表される点）である。第２の点は、自由空間（障
害物が存在しない空間）上の点（図中の白で表される
点）である。そして、第３の点は、未観測領域上の点
（図中の斜線で表される点）である。

【０１１８】次に、前記経路計画決定部ＳＢＬ３６０が
採用する経路計画アルゴリズムを図２３のフローチャー
トに示し、以下に詳細を説明する。先ず、現在位置から
目的位置までを結ぶ直線経路の周辺の障害物地図が作成
されるように、視線を目的地方向に向ける（ステップＳ
７１）。そして距離画像を観測し、距離計測を行い、障
害物地図を作成（更新）する（ステップＳ７２）。

【０１１９】次に、生成された障害物地図において、未
観測領域と自由空間領域を移動可能領域とみなして経路
計画を行う（ステップＳ７３）。

【０１２０】この経路計画としては、例えば経路全体の
コストを最小化するＡ^＊探索（Ａ^＊search)いう方法を
用いる。このＡ^＊探索は、評価関数としてｆを用い、ｈ
関数が許容的であるような最良優先探索である。いかな
るヒューリスティック関数に対しても効率最適という点
を用いている。

【０１２１】このステップＳ７３にて例えばＡ^＊探索を
適用することにより生成された経路が移動可能であるな
否かをステップＳ７４にてチェックし、障害物を回避す
ることのできる経路が計画できなかった場合（ＮＯ）に
は、これ以上観測を続けても移動可能な経路が得られる
可能性がないため、その旨を告げて経路計画を終了する
（ステップＳ７５）。

【０１２２】ステップＳ７３にて移動可能経路が例えば
前記Ａ^＊探索を適用して計画できた場合（ＹＥＳ）に
は、ステップＳ７６に進み、出力された経路上に未観測
領域が含まれるか否かを検索する。このステップＳ７６
にて経路上に未観測領域が含まれなかった場合（ＮＯ）
には、ステップＳ７７にて目的地までの経路計画として
移動可能経路を出力する。ステップＳ７６にて未観測領
域が含まれる場合（ＹＥＳ）には、ステップＳ７８に進
んで現在位置から未観測領域までの歩数を計算し、その
歩数が閾値を超えるか否かをチェックする。

【０１２３】ステップＳ７８において歩数が閾値を超え
る場合（ＹＥＳ）には、ステップＳ７９により未知領域
までの移動可能経路を出力する。一方、ステップＳ７８
において未知観測領域までの歩数が閾値に満たない場合
（ＮＯ）は、未観測領域が距離観測されるように視線方
向の制御を行って再観測を行い（ステップＳ８０）、再
び障害物地図の更新を行う。

【０１２４】以上のような経路計画アルゴリズムを採用
した経路計画決定部ＳＢＬ３６０は、未観測領域と自由
空間領域を移動可能領域とみなして経路計画を行い、出
力された経路上に含まれる未観測領域部のみを再観測す
ることにより、目的地に移動する際に不必要な観測及び
距離画像計算処理を行わなわず、効率的で短時間に移動
経路計画を生成することが可能となる。

【０１２５】以下、本発明の実施の形態における上述し
た位置認識装置を搭載した２足歩行タイプのロボット装
置について詳細に説明する。この人間型のロボット装置
は、住環境その他の日常生活上の様々な場面における人
的活動を支援する実用ロボットであり、内部状態（怒
り、悲しみ、喜び、楽しみ等）に応じて行動できるほ
か、人間が行う基本的な動作を表出できるエンターテイ
ンメントロボットである。

【０１２６】図２３に示すように、ロボット装置１は、
体幹部ユニット２の所定の位置に頭部ユニット３が連結
されると共に、左右２つの腕部ユニット４Ｒ／Ｌと、左
右２つの脚部ユニット５Ｒ／Ｌが連結されて構成されて
いる（但し、Ｒ及びＬの各々は、右及び左の各々を示す
接尾辞である。以下において同じ。）。

【０１２７】このロボット装置１が具備する関節自由度
構成を図２５に模式的に示す。頭部ユニット３を支持す
る首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１
０２と、首関節ロール軸１０３という３自由度を有して
いる。

【０１２８】また、上肢を構成する各々の腕部ユニット
４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸
１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０
と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、
手首関節ロール輪１１３と、手部１１４とで構成され
る。手部１１４は、実際には、複数本の指を含む多関節
・多自由度構造体である。ただし、手部１１４の動作
は、ロボット装置１の姿勢制御や歩行制御に対する寄与
や影響が少ないので、本明細書ではゼロ自由度と仮定す
る。したがって、各腕部は７自由度を有するとする。

【０１２９】また、体幹部ユニット２は、体幹ピッチ軸
１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６と
いう３自由度を有する。

【０１３０】また、下肢を構成する各々の脚部ユニット
５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１
１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１
８と、足首関節ピッチ軸１１９と、足首関節ロール軸１
２０と、足部１２１とで構成される。本明細書中では、
股関節ピッチ軸１１６と股関節ロール軸１１７の交点
は、ロボット装置１の股関節位置を定義する。人体の足
部１２１は、実際には多関節・多自由度の足底を含んだ
構造体であるが、ロボット装置１の足底は、ゼロ自由度
とする。したがって、各脚部は、６自由度で構成され
る。

【０１３１】以上を総括すれば、ロボット装置１全体と
しては、合計で３＋７×２＋３＋６×２＝３２自由度を
有することになる。ただし、エンターテインメント向け
のロボット装置１が必ずしも３２自由度に限定されるわ
けではない。設計・制作上の制約条件や要求仕様等に応
じて、自由度、即ち関節数を適宜増減することができる
ことはいうまでもない。

【０１３２】上述したようなロボット装置１がもつ各自
由度は、実際にはアクチュエータを用いて実装される。
外観上で余分な膨らみを排してヒトの自然体形状に近似
させること、２足歩行という不安定構造体に対して姿勢
制御を行うことなどの要請から、アクチュエータは小型
且つ軽量であることが好ましい。

【０１３３】図２６には、ロボット装置１の制御システ
ム構成を模式的に示している。同図に示すように、ロボ
ット装置１は、ヒトの四肢を表現した体幹部ユニット
２，頭部ユニット３，腕部ユニット４Ｒ／Ｌ，脚部ユニ
ット５Ｒ／Ｌと、各ユニット間の協調動作を実現するた
めの適応制御を行う制御ユニット１０とで構成される。

【０１３４】ロボット装置１全体の動作は、制御ユニッ
ト１０によって統括的に制御される。制御ユニット１０
は、ＣＰＵ（Central Processing Unit）や、ＤＲＡ
Ｍ、フラッシュＲＯＭ等の主要回路コンポーネント（図
示しない）で構成される主制御部１１と、電源回路やロ
ボット装置１の各構成要素とのデータやコマンドの授受
を行うインターフェイス（何れも図示しない）などを含
んだ周辺回路１２とで構成される。

【０１３５】この制御ユニット１０の設置場所は、特に
限定されない。図２６では体幹部ユニット２に搭載され
ているが、頭部ユニット３に搭載してもよい。あるい
は、ロボット装置１外に制御ユニット１０を配備して、
ロボット装置１の機体とは有線又は無線で交信するよう
にしてもよい。

【０１３６】図２６に示したロボット装置１内の各関節
自由度は、それぞれに対応するアクチュエータによって
実現される。即ち、頭部ユニット３には、首関節ヨー軸
１０１、首関節ピッチ軸１０２、首関節ロール軸１０３
の各々を表現する首関節ヨー軸アクチュエータＡ２、首
関節ピッチ軸アクチュエータＡ３、首関節ロール軸アク
チュエータＡ４が配設されている。

【０１３７】また、頭部ユニット３には、外部の状況を
撮像するためのＣＣＤ（Charge Coupled Device）カメ
ラが設けられているほか、前方に位置する物体までの距
離を測定するための距離センサ、外部音を集音するため
のマイク、音声を出力するためのスピーカ、使用者から
の「撫でる」や「叩く」といった物理的な働きかけによ
り受けた圧力を検出するためのタッチセンサ等が配設さ
れている。

【０１３８】また、体幹部ユニット２には、体幹ピッチ
軸１０４、体幹ロール軸１０５、体幹ヨー軸１０６の各
々を表現する体幹ピッチ軸アクチュエータＡ５、体幹ロ
ール軸アクチュエータＡ６、体幹ヨー軸アクチュエータ
Ａ７が配設されている。また、体幹部ユニット２には、
このロボット装置１の起動電源となるバッテリを備えて
いる。このバッテリは、充放電可能な電池によって構成
されている。

【０１３９】また、腕部ユニット４Ｒ／Ｌは、上腕ユニ
ット４１Ｒ／Ｌと、肘関節ユニット４２Ｒ／Ｌと、前腕
ユニット４３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸
１０７、肩関節ロール軸１０８、上腕ヨー軸１０９、肘
関節ピッチ軸１１０、前腕ヨー軸１１１、手首関節ピッ
チ軸１１２、手首関節ロール軸１１３の各々表現する肩
関節ピッチ軸アクチュエータＡ８、肩関節ロール軸アク
チュエータＡ９、上腕ヨー軸アクチュエータＡ１０、肘
関節ピッチ軸アクチュエータＡ１１、肘関節ロール軸ア
クチュエータＡ１２、手首関節ピッチ軸アクチュエータ
Ａ１３、手首関節ロール軸アクチュエータＡ１４が配備
されている。

【０１４０】また、脚部ユニット５Ｒ／Ｌは、大腿部ユ
ニット５１Ｒ／Ｌと、膝ユニット５２Ｒ／Ｌと、脛部ユ
ニット５３Ｒ／Ｌに細分化されるが、股関節ヨー軸１１
５、股関節ピッチ軸１１６、股関節ロール軸１１７、膝
関節ピッチ軸１１８、足首関節ピッチ軸１１９、足首関
節ロール軸１２０の各々を表現する股関節ヨー軸アクチ
ュエータＡ１６、股関節ピッチ軸アクチュエータＡ１
７、股関節ロール軸アクチュエータＡ１８、膝関節ピッ
チ軸アクチュエータＡ１９、足首関節ピッチ軸アクチュ
エータＡ２０、足首関節ロール軸アクチュエータＡ２１
が配備されている。各関節に用いられるアクチュエータ
Ａ２，Ａ３・・・は、より好ましくは、ギア直結型で旦つ
サーボ制御系をワンチップ化してモータ・ユニット内に
搭載したタイプの小型ＡＣサーボ・アクチュエータで構
成することができる。

【０１４１】体幹部ユニット２、頭部ユニット３、各腕
部ユニット４Ｒ／Ｌ、各脚部ユニット５Ｒ／Ｌなどの各
機構ユニット毎に、アクチュエータ駆動制御部の副制御
部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌが配備されてい
る。さらに、各脚部ユニット５Ｒ／Ｌの足底が着床した
か否かを検出する接地確認センサ３０Ｒ／Ｌを装着する
とともに、体幹部ユニット２内には、姿勢を計測する姿
勢センサ３１を装備している。

【０１４２】接地確認センサ３０Ｒ／Ｌは、例えば足底
に設置された近接センサ又はマイクロ・スイッチなどで
構成される。また、姿勢センサ３１は、例えば、加速度
センサとジャイロ・センサの組み合わせによって構成さ
れる。

【０１４３】接地確認センサ３０Ｒ／Ｌの出力によっ
て、歩行・走行などの動作期間中において、左右の各脚
部が現在立脚又は遊脚何れの状態であるかを判別するこ
とができる。また、姿勢センサ３１の出力により、体幹
部分の傾きや姿勢を検出することができる。

【０１４４】主制御部１１は、各センサ３０Ｒ／Ｌ，３
１の出力に応答して制御目標をダイナミックに補正する
ことができる。より具体的には、副制御部２０，２１，
２２Ｒ／Ｌ，２３Ｒ／Ｌの各々に対して適応的な制御を
行い、ロボット装置１の上肢、体幹、及び下肢が協調し
て駆動する全身運動パターンを実現できる。

【０１４５】ロボット装置１の機体上での全身運動は、
足部運動、ＺＭＰ（Zero Moment Point）軌道、体幹運
動、上肢運動、腰部高さなどを設定するとともに、これ
らの設定内容にしたがった動作を指示するコマンドを各
副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌに転送す
る。そして、各々の副制御部２０，２１，・・・等で
は、主制御部１１からの受信コマンドを解釈して、各ア
クチュエータＡ２，Ａ３・・・等に対して駆動制御信号
を出力する。ここでいう「ＺＭＰ」とは、歩行中の床反
力によるモーメントがゼロとなる床面上の点のことであ
り、また、「ＺＭＰ軌道」とは、例えばロボット装置１
の歩行動作期間中にＺＭＰが動く軌跡を意味する。な
お、ＺＭＰの概念並びにＺＭＰを歩行ロボットの安定度
判別規範に適用する点については、Miomir Vukobratovi
c著“LEGGED LOCOMOTION ROBOTS”（加藤一郎外著『歩
行ロボットと人工の足』（日刊工業新聞社））に記載さ
れている。

【０１４６】以上のように、ロボット装置１は、各々の
副制御部２０，２１，・・・等が、主制御部１１からの
受信コマンドを解釈して、各アクチュエータＡ２，Ａ３
・・・に対して駆動制御信号を出力し、各ユニットの駆
動を制御している。これにより、ロボット装置１は、目
標の姿勢に安定して遷移し、安定した姿勢で歩行でき
る。

【０１４７】また、ロボット装置１における制御ユニッ
ト１０では、上述したような姿勢制御のほかに、加速度
センサ、タッチセンサ、接地確認センサ等の各種セン
サ、及びＣＣＤカメラからの画像情報、マイクからの音
声情報等を統括して処理している。制御ユニット１０で
は、図示しないが加速度センサ、ジャイロ・センサ、タ
ッチセンサ、距離センサ、マイク、スピーカなどの各種
センサ、各アクチュエータ、ＣＣＤカメラ及びバッテリ
が各々対応するハブを介して主制御部１１と接続されて
いる。

【０１４８】主制御部１１は、上述の各センサから供給
されるセンサデータや画像データ及び音声データを順次
取り込み、これらをそれぞれ内部インターフェイスを介
してＤＲＡＭ内の所定位置に順次格納する。また、主制
御部１１は、バッテリから供給されるバッテリ残量を表
すバッテリ残量データを順次取り込み、これをＤＲＡＭ
内の所定位置に格納する。ＤＲＡＭに格納された各セン
サデータ、画像データ、音声データ及びバッテリ残量デ
ータは、主制御部１１がこのロボット装置１の動作制御
を行う際に利用される。

【０１４９】主制御部１１は、ロボット装置１の電源が
投入された初期時、制御プログラムを読み出し、これを
ＤＲＡＭに格納する。また、主制御部１１は、上述のよ
うに主制御部１１よりＤＲＡＭに順次格納される各セン
サデータ、画像データ、音声データ及びバッテリ残量デ
ータに基づいて自己及び周囲の状況や、使用者からの指
示及び働きかけの有無などを判断する。

【０１５０】さらに、主制御部１１は、この判断結果及
びＤＲＡＭに格納した制御プログラムに基づいて自己の
状況に応じて行動を決定するとともに、当該決定結果に
基づいて必要なアクチュエータを駆動させることにより
ロボット装置１に、いわゆる「身振り」、「手振り」と
いった行動をとらせる。

【０１５１】このようにしてロボット装置１は、制御プ
ログラムに基づいて自己及び周囲の状況を判断し、使用
者からの指示及び働きかけに応じて自律的に行動でき
る。

【０１５２】ところで、このロボット装置１は、内部状
態に応じて自律的に行動することができる。そこで、ロ
ボット装置１における制御プログラムのソフトウェア構
成例について、図２７乃至図３２を用いて説明する。な
お、この制御プログラムは、上述したように、予めフラ
ッシュＲＯＭ１２に格納されており、ロボット装置１の
電源投入初期時において読み出される。

【０１５３】図２７において、デバイス・ドライバ・レ
イヤ４０は、制御プログラムの最下位層に位置し、複数
のデバイス・ドライバからなるデバイス・ドライバ・セ
ット４１から構成されている。この場合、各デバイス・
ドライバは、ＣＣＤカメラやタイマ等の通常のコンピュ
ータで用いられるハードウェアに直接アクセスすること
を許されたオブジェクトであり、対応するハードウェア
からの割り込みを受けて処理を行う。

【０１５４】また、ロボティック・サーバ・オブジェク
ト４２は、デバイス・ドライバ・レイヤ４０の最下位層
に位置し、例えば上述の各種センサやアクチュエータ２
８１〜２８ｎ等のハードウェアにアクセスするためのイ
ンターフェイスを提供するソフトウェア群でなるバーチ
ャル・ロボット４３と、電源の切換えなどを管理するソ
フトウェア群でなるパワーマネージャ４４と、他の種々
のデバイス・ドライバを管理するソフトウェア群でなる
デバイス・ドライバ・マネージャ４５と、ロボット装置
１の機構を管理するソフトウェア群でなるデザインド・
ロボット４６とから構成されている。

【０１５５】マネージャ・オブジェクト４７は、オブジ
ェクト・マネージャ４８及びサービス・マネージャ４９
から構成されている。オブジェクト・マネージャ４８
は、ロボティック・サーバ・オブジェクト４２、ミドル
・ウェア・レイヤ５０、及びアプリケーション・レイヤ
５１に含まれる各ソフトウェア群の起動や終了を管理す
るソフトウェア群であり、サービス・マネージャ４９
は、メモリカードに格納されたコネクションファイルに
記述されている各オブジェクト間の接続情報に基づいて
各オブジェクトの接続を管理するソフトウェア群であ
る。

【０１５６】ミドル・ウェア・レイヤ５０は、ロボティ
ック・サーバ・オブジェクト４２の上位層に位置し、画
像処理や音声処理などのこのロボット装置１の基本的な
機能を提供するソフトウェア群から構成されている。ま
た、アプリケーション・レイヤ５１は、ミドル・ウェア
・レイヤ５０の上位層に位置し、当該ミドル・ウェア・
レイヤ５０を構成する各ソフトウェア群によって処理さ
れた処理結果に基づいてロボット装置１の行動を決定す
るためのソフトウェア群から構成されている。

【０１５７】なお、ミドル・ウェア・レイヤ５０及びア
プリケーション・レイヤ５１の具体なソフトウェア構成
をそれぞれ図２８に示す。

【０１５８】ミドル・ウェア・レイヤ５０は、図２８に
示すように、騒音検出用、温度検出用、明るさ検出用、
音階認識用、距離検出用、姿勢検出用、タッチセンサ
用、動き検出用及び色認識用の各信号処理モジュール６
０〜６８並びに入力セマンティクスコンバータモジュー
ル６９などを有する認識系７０と、出力セマンティクス
コンバータモジュール７８並びに姿勢管理用、トラッキ
ング用、モーション再生用、歩行用、転倒復帰用、ＬＥ
Ｄ点灯用及び音再生用の各信号処理モジュール７１〜７
７などを有する出力系７９とから構成されている。

【０１５９】認識系７０の各信号処理モジュール６０〜
６８は、ロボティック・サーバ・オブジェクト４２のバ
ーチャル・ロボット４３によりＤＲＡＭから読み出され
る各センサデータや画像データ及び音声データのうちの
対応するデータを取り込み、当該データに基づいて所定
の処理を施して、処理結果を入力セマンティクスコンバ
ータモジュール６９に与える。ここで、例えば、バーチ
ャル・ロボット４３は、所定の通信規約によって、信号
の授受或いは変換をする部分として構成されている。

【０１６０】入力セマンティクスコンバータモジュール
６９は、これら各信号処理モジュール６０〜６８から与
えられる処理結果に基づいて、「うるさい」、「暑
い」、「明るい」、「ボールを検出した」、「転倒を検
出した」、「撫でられた」、「叩かれた」、「ドミソの
音階が聞こえた」、「動く物体を検出した」又は「障害
物を検出した」などの自己及び周囲の状況や、使用者か
らの指令及び働きかけを認識し、認識結果をアプリケー
ション・レイヤ４１に出力する。

【０１６１】アプリケーション・レイヤ５１は、図２９
に示すように、行動モデルライブラリ８０、行動切換モ
ジュール８１、学習モジュール８２、感情モデル８３及
び本能モデル８４の５つのモジュールから構成されてい
る。

【０１６２】行動モデルライブラリ８０には、図３０に
示すように、「バッテリ残量が少なくなった場合」、
「転倒復帰する」、「障害物を回避する場合」、「感情
を表現する場合」、「ボールを検出した場合」などの予
め選択されたいくつかの条件項目にそれぞれ対応させ
て、それぞれ独立した行動モデルが設けられている。

【０１６３】そして、これら行動モデルは、それぞれ入
力セマンティクスコンバータモジュール６９から認識結
果が与えられたときや、最後の認識結果が与えられてか
ら一定時間が経過したときなどに、必要に応じて後述の
ように感情モデル８３に保持されている対応する情動の
パラメータ値や、本能モデル８４に保持されている対応
する欲求のパラメータ値を参照しながら続く行動をそれ
ぞれ決定し、決定結果を行動切換モジュール８１に出力
する。

【０１６４】なお、この実施の形態の場合、各行動モデ
ルは、次の行動を決定する手法として、図３１に示すよ
うな１つのノード（状態）ＮＯＤＥ０〜ＮＯＤＥｎから
他のどのノードＮＯＤＥ０〜ＮＯＤＥｎに遷移するかを
各ノードＮＯＤＥ０〜ＮＯＤＥｎに間を接続するアーク
ＡＲＣ１〜ＡＲＣｎ１に対してそれぞれ設定された遷移
確率Ｐ１〜Ｐｎに基づいて確率的に決定する有限確率オ
ートマトンと呼ばれるアルゴリズムを用いる。

【０１６５】具体的に、各行動モデルは、それぞれ自己
の行動モデルを形成するノードＮＯＤＥ０〜ＮＯＤＥｎ
にそれぞれ対応させて、これらノードＮＯＤＥ０〜ＮＯ
ＤＥｎ毎に図２３に示すような状態遷移表９０を有して
いる。

【０１６６】この状態遷移表９０では、そのノードＮＯ
ＤＥ０〜ＮＯＤＥｎにおいて遷移条件とする入力イベン
ト（認識結果）が「入力イベント名」の列に優先順に列
記され、その遷移条件についてのさらなる条件が「デー
タ名」及び「データ範囲」の列における対応する行に記
述されている。

【０１６７】したがって、図３２の状態遷移表９０で表
されるノードＮＯＤＥ１００では、「ボールを検出（Ｂ
ＡＬＬ）」という認識結果が与えられた場合に、当該認
識結果とともに与えられるそのボールの「大きさ（SIZ
E）」が「0から1000」の範囲であることや、「障害物を
検出（OBSTACLE）」という認識結果が与えられた場合
に、当該認識結果とともに与えられるその障害物までの
「距離（DISTANCE）」が「0から100」の範囲であること
が他のノードに遷移するための条件となっている。

【０１６８】また、このノードＮＯＤＥ１００では、認
識結果の入力がない場合においても、行動モデルが周期
的に参照する感情モデル８３及び本能モデル８４にそれ
ぞれ保持された各情動及び各欲求のパラメータ値のう
ち、感情モデル８３に保持された「喜び（Joy）」、
「驚き（Surprise）」又は「悲しみ（Sadness）」の何
れかのパラメータ値が「50から100」の範囲であるとき
には他のノードに遷移することができるようになってい
る。

【０１６９】また、状態遷移表９０では、「他のノード
ヘの遷移確率」の欄における「遷移先ノード」の行にそ
のノードＮＯＤＥ０〜ＮＯＤＥｎから遷移できるノード
名が列記されているとともに、「入力イベント名」、
「データ名」及び「データの範囲」の列に記述された全
ての条件が揃ったときに遷移できる他の各ノードＮＯＤ
Ｅ０〜ＮＯＤＥｎへの遷移確率が「他のノードヘの遷移
確率」の欄内の対応する箇所にそれぞれ記述され、その
ノードＮＯＤＥ０〜ＮＯＤＥｎに遷移する際に出力すべ
き行動が「他のノードヘの遷移確率」の欄における「出
力行動」の行に記述されている。なお、「他のノードヘ
の遷移確率」の欄における各行の確率の和は１００
［％］となっている。

【０１７０】したがって、図３２の状態遷移表９０で表
されるノードＮＯＤＥ１００では、例えば「ボールを検
出（ＢＡＬＬ）」し、そのボールの「ＳＩＺＥ（大き
さ）」が「0から1000」の範囲であるという認識結果が
与えられた場合には、「30［％］」の確率で「ノードＮ
ＯＤＥ１２０（node 120）」に遷移でき、そのとき「Ａ
ＣＴＩＯＮ１」の行動が出力されることとなる。

【０１７１】各行動モデルは、それぞれこのような状態
遷移表９０として記述されたノードＮＯＤＥ０〜 ＮＯ
ＤＥｎが幾つも繋がるようにして構成されており、入力
セマンティクスコンバータモジュール６９から認識結果
が与えられたときなどに、対応するノードＮＯＤＥ０〜
ＮＯＤＥｎの状態遷移表を利用して確率的に次の行動を
決定し、決定結果を行動切換モジュール８１に出力する
ようになされている。

【０１７２】図３０に示す行動切換モジュール８１は、
行動モデルライブラリ８０の各行動モデルからそれぞれ
出力される行動のうち、予め定められた優先順位の高い
行動モデルから出力された行動を選択し、当該行動を実
行すべき旨のコマンド（以下、行動コマンドという。）
をミドル・ウェア・レイヤ５０の出力セマンティクスコ
ンバータモジュール７８に送出する。なお、この実施の
形態においては、図３０において下側に表記された行動
モデルほど優先順位が高く設定されている。

【０１７３】また、行動切換モジュール８１は、行動完
了後に出力セマンティクスコンバータモジュール７８か
ら与えられる行動完了情報に基づいて、その行動が完了
したことを学習モジュール８２、感情モデル８３及び本
能モデル８４に通知する。

【０１７４】一方、学習モジュール８２は、入力セマン
ティクスコンバータモジュール６９から与えられる認識
結果のうち、「叩かれた」や「撫でられた」など、使用
者からの働きかけとして受けた教示の認識結果を入力す
る。

【０１７５】そして、学習モジュール８２は、この認識
結果及び行動切換えモジュール７１からの通知に基づい
て、「叩かれた（叱られた）」ときにはその行動の発現
確率を低下させ、「撫でられた（誉められた）」ときに
はその行動の発現確率を上昇させるように、行動モデル
ライブラリ７０における対応する行動モデルの対応する
遷移確率を変更する。

【０１７６】他方、感情モデル８３は、「喜び（Jo
y）」、「悲しみ（Sadness）」、「怒り（Anger）」、
「驚き（Surprise）」、「嫌悪（Disgust）」及び「恐
れ（Fear）」の合計６つの情動について、各情動毎にそ
の情動の強さを表すパラメータを保持している。そし
て、感情モデル８３は、これら各情動のパラメータ値
を、それぞれ入力セマンティクスコンバータモジュール
６９から与えられる「叩かれた」及び「撫でられた」な
どの特定の認識結果や、経過時間及び行動切換モジュー
ル８１からの通知などに基づいて周期的に更新する。

【０１７７】具体的には、感情モデル８３は、入力セマ
ンティクスコンバータモジュール６９から与えられる認
識結果と、そのときのロボット装置１の行動と、前回更
新してからの経過時間となどに基づいて所定の演算式に
より算出されるそのときのその情動の変動量を△Ｅ
［ｔ］、現在のその情動のパラメータ値をＥ［ｔ］、そ
の情動の感度を表す係数をｋｅとして、下記数式（１
１）によって次の周期におけるその情動のパラメータ値
Ｅ［ｔ＋１］を算出し、これを現在のその情動のパラメ
ータ値Ｅ［ｔ］と置き換えるようにしてその情動のパラ
メータ値を更新する。また、感情モデル８３は、これと
同様にして全ての情動のパラメータ値により更新する。

【０１７８】

【数１４】

【０１７９】なお、各認識結果や出力セマンティクスコ
ンバータモジュール７８からの通知が各情動のパラメー
タ値の変動量△Ｅ［ｔ］にどの程度の影響を与えるかは
予め決められており、例えば「叩かれた」といった認識
結果は「怒り」の情動のパラメータ値の変動量△Ｅ
［ｔ］に大きな影響を与え、「撫でられた」といった認
識結果は「喜び」の情動のパラメータ値の変動量△Ｅ
［ｔ］に大きな影響を与えるようになっている。

【０１８０】ここで、出力セマンティクスコンバータモ
ジュール７８からの通知とは、いわゆる行動のフィード
バック情報（行動完了情報）であり、行動の出現結果の
情報であり、感情モデル８３は、このような情報によっ
ても感情を変化させる。これは、例えば、「叫ぶ」とい
った行動により怒りの感情レベルが下がるといったよう
なことである。なお、出力セマンティクスコンバータモ
ジュール７８からの通知は、上述した学習モジュール８
２にも入力されており、学習モジュール８２は、その通
知に基づいて行動モデルの対応する遷移確率を変更す
る。

【０１８１】なお、行動結果のフィードバックは、行動
切換モジュール８１の出力（感情が付加された行動）に
よりなされるものであってもよい。

【０１８２】一方、本能モデル８４は、「運動欲（exer
cise）」、「愛情欲（affection）」、「食欲（appetit
e）」及び「好奇心（curiosity）」の互いに独立した４
つの欲求について、これら欲求毎にその欲求の強さを表
すパラメータを保持している。そして、本能モデル８４
は、これらの欲求のパラメータ値を、それぞれ入力セマ
ンティクスコンバータモジュール６９から与えられる認
識結果や、経過時間及び行動切換モジュール８１からの
通知などに基づいて周期的に更新する。

【０１８３】具体的には、本能モデル８４は、「運動
欲」、「愛情欲」及び「好奇心」については、認識結
果、経過時間及び出力セマンティクスコンバータモジュ
ール７８からの通知などに基づいて所定の演算式により
算出されるそのときのその欲求の変動量をΔＩ［ｋ］、
現在のその欲求のパラメータ値をＩ［ｋ］、その欲求の
感度を表す係数ｋｉとして、所定周期で下記数式（１
２）を用いて次の周期におけるその欲求のパラメータ値
Ｉ［ｋ＋１］を算出し、この演算結果を現在のその欲求
のパラメータ値Ｉ［ｋ］と置き換えるようにしてその欲
求のパラメータ値を更新する。また、本能モデル８４
は、これと同様にして「食欲」を除く各欲求のパラメー
タ値を更新する。

【０１８４】

【数１５】

【０１８５】なお、認識結果及び出力セマンティクスコ
ンバータモジュール７８からの通知などが各欲求のパラ
メータ値の変動量△Ｉ［ｋ］にどの程度の影響を与える
かは予め決められており、例えば出力セマンティクスコ
ンバータモジュール７８からの通知は、「疲れ」のパラ
メータ値の変動量△Ｉ［ｋ］に大きな影響を与えるよう
になっている。

【０１８６】なお、この具体例においては、各情動及び
各欲求（本能）のパラメータ値がそれぞれ０から１００
までの範囲で変動するように規制されており、また係数
ｋｅ、ｋｉの値も各情動及び各欲求毎に個別に設定され
ている。

【０１８７】一方、ミドル・ウェア・レイヤ５０の出力
セマンティクスコンバータモジュール７８は、図２８に
示すように、上述のようにしてアプリケーション・レイ
ヤ５１の行動切換モジュール８１から与えられる「前
進」、「喜ぶ」、「鳴く」又は「トラッキング（ボール
を追いかける）」といった抽象的な行動コマンドを出力
系７９の対応する信号処理モジュール７１〜７７に与え
る。

【０１８８】そしてこれら信号処理モジュール７１〜７
７は、行動コマンドが与えられると当該行動コマンドに
基づいて、その行動をするために対応するアクチュエー
タに与えるべきサーボ指令値や、スピーカから出力する
音の音声データ及び又はＬＥＤに与える駆動データを生
成し、これらのデータをロボティック・サーバ・オブジ
ェクト４２のバーチャル・ロボット４３及び信号処理回
路を順次介して対応するアクチュエータ又はスピーカ又
はＬＥＤに順次送出する。

【０１８９】このようにしてロボット装置１は、上述し
た制御プログラムに基づいて、自己（内部）及び周囲
（外部）の状況や、使用者からの指示及び働きかけに応
じた自律的な行動ができる。

【０１９０】このような制御プログラムは、ロボット装
置が読取可能な形式で記録された記録媒体を介して提供
される。制御プログラムを記録する記録媒体としては、
磁気読取方式の記録媒体（例えば、磁気テープ、フレキ
シブルディスク、磁気カード）、光学読取方式の記録媒
体（例えば、ＣＤ−ＲＯＭ、ＭＯ、ＣＤ−Ｒ、ＤＶＤ）
等が考えられる。記録媒体には、半導体メモリ（いわゆ
るメモリカード（矩形型、正方形型など形状は問わな
い。）、ＩＣカード）等の記憶媒体も含まれる。また、
制御プログラムは、いわゆるインターネット等を介して
提供されてもよい。

【０１９１】これらの制御プログラムは、専用の読込ド
ライバ装置、又はパーソナルコンピュータ等を介して再
生され、有線又は無線接続によってロボット装置１に伝
送されて読み込まれる。また、ロボット装置１は、半導
体メモリ、又はＩＣカード等の小型化された記憶媒体の
ドライブ装置を備える場合、これら記憶媒体から制御プ
ログラムを直接読み込むこともできる。

【０１９２】本実施の形態においては、障害物認識装置
は、多数の測定点を使って平面の検出を行うために安定
した平面を抽出できる。また、画像から得られる平面候
補をロボットの姿勢から得られる床面パラメータと照合
することで正しい平面を選択できる。また、障害物を認
識するのではなく、実質的には床面を認識するので障害
物の形状や大きさに因らない認識が可能である。また、
床面からの距離で障害物を表現するので細かい段差や凹
みまで検出することができる。またロボットの大きさを
考慮して跨いだり、くぐったりする判断も容易である。
さらに、2次元床面上の障害物表現となるので経路計画
などに既存の移動ロボットで用いられる手法が適用可能
であるし、3次元の表現より高速に演算できる。

【０１９３】

【発明の効果】以上詳細に説明したように本発明に係る
障害物認識装置は、座標変換手段による座標変換結果
と、平面検出手段によって検出された平面パラメータと
に基づいて床面検出手段が床面を検出し、その床面の平
面パラメータを用いて床面に載っている点を選択し、こ
の点に基づいて障害物を認識するので、正確に床面を抽
出し、障害物を認識することができる。

【０１９４】また、本発明に係る障害物認識方法は、座
標変換工程による座標変換結果と、平面検出工程によっ
て検出された平面パラメータとに基づいて床面検出工程
が床面を検出し、その床面の平面パラメータを用いて床
面に載っている点を選択し、この点に基づいて障害物を
認識するので、正確に床面を抽出し、障害物を認識する
ことができる。

【０１９５】また、本発明に係る障害物認識プログラム
は、ロボット装置によって実行されることにより、座標
変換工程による座標変換結果と、平面検出工程によって
検出された平面パラメータとに基づいて床面検出工程が
床面を検出し、その床面の平面パラメータを用いて床面
に載っている点を選択し、この点に基づいて障害物を認
識するので、ロボット装置に正確に床面を抽出させ、障
害物を認識させることができる。

【０１９６】また、本発明に係るロボット装置は、座標
変換手段による座標変換結果と、平面検出手段によって
検出された平面パラメータとに基づいて床面検出手段が
床面を検出し、その床面の平面パラメータを用いて床面
に載っている点を選択し、この点に基づいて障害物を認
識するので、正確に床面を抽出し、障害物を認識するこ
とができる。

【図面の簡単な説明】

【図１】本発明の実施の形態におけるロボット装置の概
略を示すブロック図である。

【図２】本発明の実施の形態におけるロボット装置のソ
フトウェアの構成を示す模式図である。

【図３】障害物認識装置の機能ブロック図である。

【図４】平面抽出部ＰＬＥＸに入力される視差画像の生
成を説明するための図である。

【図５】平面抽出部ＰＬＥＸが障害物を認識する処理手
順を示すフローチャートである。

【図６】平面抽出部ＰＬＥＸが検出する平面のパラメー
タを示す図である。

【図７】カメラ座標系から足底接地平面座標系への変換
処理を説明するための図である。

【図８】平面抽出部ＰＬＥＸにて抽出された平面上の点
を示す図である。

【図９】ロボットビューから床面を抽出し、さらに座標
変換して２次元（床平面）上に障害物を表現したことを
示す図である。

【図１０】本発明の実施の形態におけるロボット装置の
ソフトウェアの動作を示すフローチャートである。

【図１１】同ソフトウェアに入力されるデータの流れを
示す模式図である。

【図１２】本発明の実施の形態における障害物認識装置
（ロボット装置）にて表示される環境地図を示す模式図
である。

【図１３】（ａ），（ｂ）及び（ｃ），（ｄ）は位置認
識装置の移動量に応じた環境地図の更新方法を示す図で
あって、夫々環境地図を更新しない場合及び環境地図を
更新する場合の環境地図を示す模式図である。

【図１４】位置認識装置における環境地図更新の際の各
移動量を示す模式図である。

【図１５】（ａ）及び（ｂ）は、位置認識装置における
姿勢方向の夫々更新前後の様子を示す模式図である。

【図１６】位置認識装置における効果を説明する模式図
である。

【図１７】環境地図及び姿勢情報を表示した操作盤を示
す模式図である。

【図１８】ランドマークセンサＣＬＳにおけるカラーラ
ンドマークを示す模式図である。

【図１９】ランドマークセンサＣＬＳの構成を模式的に
示すブロック図である。

【図２０】マルコフ・ローカリゼーション部により求め
られた各グリッド上での自己位置確率密度分布を示す模
式図である。

【図２１】ランドマークセンサＣＬＳ３４０の動作を示
すフローチャートである。

【図２２】障害物情報により生成された障害物地図上
に、現在位置から目的位置までの経路を示した図であ
る。

【図２３】経路計画アルゴリズムを示すフローチャート
である。

【図２４】本実施の形態におけるロボット装置の外観構
成を示す斜視図である。

【図２５】同ロボット装置の自由度構成モデルを模式的
に示す図である。

【図２６】同ロボット装置の回路構成を示すブロック図
である。

【図２７】同ロボット装置のソフトウェア構成を示すブ
ロック図である。

【図２８】同ロボット装置のソフトウェア構成における
ミドル・ウェア・レイヤの構成を示すブロック図であ
る。

【図２９】同ロボット装置のソフトウェア構成における
アプリケーション・レイヤの構成を示すブロック図であ
る。

【図３０】アプリケーション・レイヤの行動モデルライ
ブラリの構成を示すブロック図である。

【図３１】同ロボット装置の行動決定のための情報とな
る有限確率オートマトンを説明する図である。

【図３２】有限確率オートマトンの各ノードに用意され
た状態遷移表を示す図である。

【符号の説明】

１ ロボット装置、２５０ 頭部ユニット、２００Ｒ，
２００Ｌ ＣＣＤカメラ、２０１Ｒ 右目画像、２０１
Ｌ 左目画像、２０２ カラー画像、２０３視差画像、
２１０ ステレオ画像処理装置、２２０ ＣＰＵ、２３
０ アクチュエータ、２３１ 制御信号、２４０ セン
サ、２４１ センサデータ、２６０体幹部、３００ ソ
フトウェア、３１０ キネマティックオドメトリＫＩＮ
Ｅ３１０、３２０ 平面抽出部ＰＬＥＸ、３３０ 障害
物グリッド算出部ＯＧ、３４０ ランドマーク位置検出
部ＣＬＳ、３５０ 絶対座標算出部ＬＺ、３６０ 行動
決定部ＳＢＬ、４００，５００，５１０，６００，６１
０，７００ 環境地図、４０１，５０１，６０１，７０
１ 中心グリッド、４０２，５０２，６０２障害物グリ
ッド、４０３，５０３，５０６，５１３，６０３，７０
３ ロボット装置、４１０ グリッド、４０４ 情報、
９００ 操作盤

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁷ 識別記号 ＦＩ テーマコート゛(参考） Ｇ０５Ｄ 1/02 Ｈ０４Ｎ 7/18 Ｋ Ｇ０６Ｔ 1/00 ３１５ Ｇ０１Ｂ 11/24 Ｎ Ｈ０４Ｎ 7/18 Ｋ (72)発明者 大橋 武史 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 (72)発明者 福地 正樹 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 (72)発明者 大久保 厚志 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 (72)発明者 ステファン グットマン 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 Ｆターム(参考） 2F065 AA04 AA20 AA31 AA51 BB01 BB05 CC14 CC40 FF05 FF09 FF65 JJ03 JJ05 JJ26 QQ21 QQ31 QQ41 3C007 AS36 CS08 KS12 KT03 KT04 KT11 MS08 WA03 WA13 WA28 5B057 AA05 BA02 CA01 CA08 CA13 CA16 CB01 CB08 CB13 CB16 CC01 CD02 CD03 DA07 DB03 DB06 DB09 DC30 DC32 5C054 CC05 CD03 CF08 CG02 FC01 FC12 FD03 FF07 HA00 5H301 AA02 AA10 BB14 CC03 CC06 GG01 GG09 GG17 LL01 LL02 LL03 LL11 LL14

Claims (8)

【特許請求の範囲】
1. 【請求項１】 ロボット装置に設けた複数の撮像手段か
   ら得られた画像データに基づいて算出された視差画像
   と、前記ロボット装置に設けた複数のセンサ手段から得
   られたセンサデータ出力とに基づいて前記複数の撮像手
   段が配設された位置における前記視差画像に対応する同
   時変換行列とを用いて距離画像を生成する距離画像生成
   手段と、 前記距離画像生成手段により生成された距離画像に基づ
   いて平面パラメータを検出する平面検出手段と、 前記複数の撮像手段の配設された位置における座標系を
   床面での座標系に変換する座標変換手段と、 前記座標変換手段による座標変換結果と、前記平面検出
   手段によって検出された平面パラメータとに基づいて床
   面を検出する床面検出手段と、 前記床面検出手段の平面パラメータを用いて床面に載っ
   ている点を選択し、この点に基づいて障害物を認識する
   障害物認識手段とを備えることを特徴とする障害物認識
   装置。
2. 【請求項２】 前記障害物認識手段は、前記床面検出手
   段にて検出された床面の平面パラメータを用いて平面か
   らの距離が小さいデータを取り除いた残りを障害物とし
   て認識することを特徴とする請求項１記載の障害物認識
   装置。
3. 【請求項３】 前記床面検出手段にて検出した床面と、
   前記障害物認識手段が認識した障害物とを、高さをゼロ
   とした平面座標上に変換して平面上の２次元の位置で床
   面か障害物かを識別することを特徴とする請求項１記載
   の障害物認識装置。
4. 【請求項４】 ロボット装置に設けた複数の撮像手段か
   ら得られた画像データに基づいて算出された視差画像
   と、前記ロボット装置に設けた複数のセンサ手段から得
   られたセンサデータ出力とに基づいて前記複数の撮像手
   段が配設された位置における前記視差画像に対応する同
   時変換行列とを用いて距離画像を生成する距離画像生成
   工程と、 前記距離画像生成工程により生成された距離画像に基づ
   いて平面パラメータを検出する平面検出工程と、 前記複数の撮像手段の配設された位置における座標系を
   床面での座標系に変換する座標変換工程と、 前記座標変換工程による座標変換結果と、前記平面検出
   工程によって検出された平面パラメータとに基づいて床
   面を検出する床面検出工程と、 前記床面検出工程の平面パラメータを用いて床面に載っ
   ている点を選択し、この点に基づいて障害物を認識する
   障害物認識工程とを備える障害物認識方法。
5. 【請求項５】 ロボット装置によって実行されることに
   よって障害物を認識する障害物認識プログラムにおい
   て、 前記ロボット装置に設けた複数の撮像手段から得られた
   画像データに基づいて算出された視差画像と、前記ロボ
   ット装置に設けた複数のセンサ手段から得られたセンサ
   データ出力とに基づいて前記複数の撮像手段が配設され
   た位置における前記視差画像に対応する同時変換行列と
   を用いて距離画像を生成する距離画像生成工程と、 前記距離画像生成工程により生成された距離画像に基づ
   いて平面パラメータを検出する平面検出工程と、 前記複数の撮像手段の配設された位置における座標系を
   床面での座標系に変換する座標変換工程と、 前記座標変換工程による座標変換結果と、前記平面検出
   工程によって検出された平面パラメータとに基づいて床
   面を検出する床面検出工程と、 前記床面検出工程の平面パラメータを用いて床面に載っ
   ている点を選択し、この点に基づいて障害物を認識する
   障害物認識工程とを備えてなることを特徴とする障害物
   認識プログラム。
6. 【請求項６】 頭部ユニットと、少なくとも１以上の可
   動脚ユニットと、情報処理手段を有する胴体ユニットと
   を備えてなり、床面上の障害物を認識しながら前記可動
   脚ユニットを用いて床面を移動する移動型ロボット装置
   において、 前記頭部ユニットに設けた複数の撮像手段から得られた
   画像データに基づいて算出された視差画像と、前記頭部
   ユニット、前記可動脚ユニット、前記胴体ユニットに設
   けた複数のセンサ手段から得られたセンサデータ出力と
   に基づいて前記複数の撮像手段が配設された位置におけ
   る前記視差画像に対応する同時変換行列とを用いて距離
   画像を生成する距離画像生成手段と、 前記距離画像生成手段により生成された距離画像に基づ
   いて平面パラメータを検出する平面検出手段と、 前記複数の撮像手段の配設された位置における座標系を
   床面での座標系に変換する座標変換手段と、 前記座標変換手段による座標変換結果と、前記平面検出
   手段によって検出された平面パラメータとに基づいて床
   面を検出する床面検出手段と、 前記床面検出手段の平面パラメータを用いて床面に載っ
   ている点を選択し、この点に基づいて障害物を認識する
   障害物認識手段とを前記胴体ユニットに備えてなること
   を特徴とする移動型ロボット装置。
7. 【請求項７】 前記障害物認識手段は、前記床面検出手
   段にて検出された床面の平面パラメータを用いて平面か
   らの距離が小さいデータを取り除いた残りを障害物とし
   て認識することを特徴とする請求項６記載の移動型ロボ
   ット装置。
8. 【請求項８】 前記床面検出手段にて検出した床面と、
   前記障害物認識手段が認識した障害物とを、高さをゼロ
   とした平面座標上に変換して平面上の２次元の位置で床
   面か障害物かを識別することを特徴とする請求項６記載
   の移動型ロボット装置。