JP2003271975A - 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置 - Google Patents
平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置Info
- Publication number
- JP2003271975A JP2003271975A JP2002073387A JP2002073387A JP2003271975A JP 2003271975 A JP2003271975 A JP 2003271975A JP 2002073387 A JP2002073387 A JP 2002073387A JP 2002073387 A JP2002073387 A JP 2002073387A JP 2003271975 A JP2003271975 A JP 2003271975A
- Authority
- JP
- Japan
- Prior art keywords
- plane
- voting
- dimensional data
- parameter
- calculating
- 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.)
- Granted
Links
Landscapes
- Manipulator (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
い平面を抽出することができる平面抽出方法、その装
置、そのプログラム、その記録媒体及び平面抽出装置搭
載型ロボット装置を提供する。 【解決手段】 距離画像等の3次元データから平面を抽
出するには、先ず、3次元データ群から3次元データを
ランダムにサンプリングし、法線ベクトルの向きを
(θ,φ)、原点からの距離をdとしたときの平面パラ
メータ(θ,φ,d)を算出して、平面パラメータを投
票空間(θ,Ψ,d)=(θ,φcosθ,d)に投票
する。サンプルデータの信頼度及びパラメータの算出方
法により投票値に重み付けする。次に、得票値が高くな
ると推定された平面パラメータのイタレーション処理を
して平面パラメータを決定する。最後に、サンプリング
データの信頼度、サンプリング数、イタレーション時の
誤差等から得られた信頼度と共に平面パラメータを出力
する。
Description
等の3次元データから平面を抽出する平面抽出方法、そ
の装置、そのプログラム、その記録媒体及び平面抽出装
置搭載型ロボット装置に関し、特に、平面抽出における
信頼度及び演算速度を高めた平面抽出方法、その装置、
そのプログラム、その記録媒体及び平面抽出装置搭載型
ロボット装置に関する。
(生物)の動作に似た運動を行う機械装置を「ロボッ
ト」という。我が国においてロボットが普及し始めたの
は、1960年代末からであるが、その多くは、工場に
おける生産作業の自動化・無人化等を目的としたマニピ
ュレータ及び搬送ロボット等の産業用ロボット(Indust
rialRobot)であった。
支援する、即ち住環境その他の日常生活上の様々な場面
における人的活動を支援する実用ロボットの開発が進め
られている。このような実用ロボットは、産業用ロボッ
トとは異なり、人間の生活環境の様々な局面において、
個々に個性の相違した人間、又は様々な環境への適応方
法を自ら学習する能力を備えている。例えば、犬又は猫
のように4足歩行の動物の身体メカニズム及びその動作
を模した「ペット型」ロボット、或いは、2足直立歩行
を行う人間等の身体メカニズム及びその動作をモデルに
してデザインされた「人間型」又は「人間形」ロボット
(Humanoid Robot)等のロボット装置は、既に実用化さ
れつつある。
と比較して、例えばエンターテインメント性を重視した
様々な動作等を行うことができるため、エンターテイン
メントロボットと呼称される場合もある。また、そのよ
うなロボット装置には、外部からの情報及び内部の状態
に応じて自律的に動作するものがある。
自身の周りの環境を認識して経路計画を立て、それに応
じて移動する能力は言うまでもなく重要である。環境を
認識するためには、ロボット装置の周囲の障害物情報等
が必要である。従って、ロボット装置は、距離画像又は
視差画像等の3次元データから床となる平面を検出し、
障害物を認識する必要がある。
ては、ハフ変換(Hough Transform)を使用する方法が
ある。ハフ変換には、Forward Position,Backward Pos
ition,Feature Point Pairs等といわれるいくつかの方
法がある。以下、これらのハフ変換について、簡単のた
め、2次元画像平面内の直線検出の例として説明する。
元画像からは何らかの画像処理により、エッジ(直線上
にのると予想される特徴点)が検出されているものとす
る。また、特徴点の数をMとし、元画像の大きさをL×
L、投票先のパラメータ空間(投票箱)の大きさをl
(エル)×l(エル)とする。
順次たどり、各点をパラメータ空間内に写像したときの
曲線が通るスロット1つ1つに投票していく。アルゴリ
ズムの複雑度はO(Ml)である。
内の各スロットを順次操作し、各スロットを元画像に対
し逆写像した直線上にのっている特徴点の数をそのスロ
ットの得票とする。アルゴリズムの複雑度はO(L
l2)である。
あらゆる組み合わせを順次たどり、その2点の組から計
算されるパラメータが属するスロッドに得票する。アル
ゴリズムの複雑度はO(M2)である。
次元データから平面を抽出する方法は、演算量及び使用
メモリが極めて多くなる。これは、次元数が2次元より
も1つ多い上に、全てのデータポイントが特徴点として
の意味を有するためである。従って、これらのハフ変換
は、3次元データからの平面検出というタスクにおいて
は、演算量及び使用メモリの観点から何れも現実的では
ない。
ough Transform:RHT)と呼ばれるハフ変換がある。
このランダム化ハフ変換は、上述したFeature Point Pa
irsの派生であり、上述の2次元画像平面内において
は、特徴点2点のあらゆる組み合わせではなく、ランダ
ムサンプリングによるK通りの組み合わせについてのみ
処理を行うものである。このアルゴリズムの複雑度はO
(K)である。
めにハフ変換を利用した方法が提案されている。例え
ば、特開平10−96607号公報には、3次元データ
に対してハフ変換処理をして平面物体の3次元空間にお
ける位置を推定する物体検出方法及び平面推定方法が開
示されている(以下、従来例1という。)。従来例1に
記載の技術においては、3次元データである視差画像か
らハフ曲線をある平面上にプロットし、これらのハフ曲
線が最も多く通過する平面上の点を求め、これに基づ
き、ある平面上にあると仮定した平面直線群の垂線の傾
き及びY軸までの距離を補正して平面物体を推定してい
る。
平面上の距離データを精度よく測定することを目的とし
た平面推定方法が開示されている(以下、従来例2とい
う)。従来例2に記載の技術においては、ステレオ画像
を矩形小領域に分割してこの小領域毎に距離データを求
め、この小領域毎を水平方向にグループ化した大領域毎
に、これらの領域に含まれるM個の距離データからハフ
変換により平面通過直線を当てはめ、これを全ての大領
域について行うことにより撮像空間における平面を推定
するものである。
は、メモリ容量及び処理時間短縮を図った画像処理装置
が開示されている(以下、従来例3という。)この従来
例3に記載の技術においては、3次元空間から直線群を
入力し、この直線群の法線ベクトルを特徴空間に投票
し、これに基づき所定の本数の直線を含む平面を抽出す
るものである。
1乃至従来例3に記載の技術においては、投票空間に対
して平面パラメータを求め直接投票するものではないた
め、処理が煩雑になると共に、ハフ変換が有する確率推
定としての利点を十分に生かすことができないという問
題点がある。
長くなり、高速化が困難であるという問題点もある。
ら未知の平面パラメータを決定する、即ち、平面を検出
する方法は確立されていない。
提案されたものであり、ノイズに対してロバストであ
り、信頼性が高い平面を抽出することができる平面抽出
方法、その装置、そのプログラム、その記録媒体及び平
面抽出装置搭載型ロボット装置を提供することを目的と
する。
ために、本発明に係る平面抽出方法は、3次元データ群
から3点以上の3次元データをサンプリングし、上記3
次元データによって決まる1つの平面を示す平面パラメ
ータを複数算出する平面算出工程と、この平面算出工程
から得られた複数の平面パラメータを投票空間に投票し
てこの投票結果に基づき平面を決定する平面決定工程と
を有することを特徴とする。
パラメータ算出し、この平面パラメータを投票空間に直
接投票するため、ノイズの多い3次元データからでも平
面を安定して抽出することができる。
トルの向きを(θ,φ)、原点からの距離をdとしたと
き、上記平面パラメータ(θ,φ,d)を算出する工程
であって、上記投票空間は、下記数式(1−1)又は数
式(1−2)で表されるものとすることができる。
タは、元の3次元直交座標空間に対して極座標を形成す
ることから、投票スロット(グリッド)が元の対応空間
内で同一の面積を占めるように投票空間を上記数式(1
−1)又は数式(1−2)として設計することにより、
更にロバストで精度が高いデータが得られる。
基づき3次元データ群に変換されるものであって、各上
記3次元パラメータは上記入力画像に基づき算出される
信頼度パラメータを個別に有し、この信頼度パラメータ
に基づいて上記3次元データ群の3次元データを選別す
るデータ選別工程を有することができ、信頼度が低い3
次元データを破棄すること等ができ、処理結果の信頼性
及び安定性を向上することができる。
ランダムにサンプリングしたものか、又はランダムにサ
ンプリングされた1つの基準点と、この基準点から所定
の距離内でランダムにサンプリングされた他の2点とか
らなるものであって、上記ランダムサンプリングを複数
回繰り返すことができる。更にまた、3点のデータから
直接解法によって上記平面パラメータを算出する工程を
有することができ、サンプリングデータから上記投票空
間へ投票するハフ変換により高精度の平面パラメータを
推定することができる。
空間を所定の領域に分割しこの領域内の3次元データと
することができる。更にまた、3点以上の上記3次元デ
ータから共分散行列を固有値展開して最小固有値に対応
するベクトルを抽出する主成分分析により上記平面パラ
メータを算出するか、又は上記平面が原点を通らない任
意の平面であると仮定し最小二乗法を使用して上記平面
パラメータを算出することができる。
り返し、上記投票の総数が所定の閾値を超えた場合、又
は上記投票数に基づいて決定される投票値が最も高い位
置における投票値の合計が全ての投票値の合計である総
投票値の所定の割合に達した場合に投票をうち切る工程
を有することができるため、処理を高速化することがで
きる。
元データの信頼度及び/又は平面算出方法により異なる
重みを定めて投票する重付投票工程を有するため、デー
タの信頼度が高く精度が向上するため、投票総数を減ら
すことができ、高速処理が可能となる。
の投票のピーク近傍における投票数による重み付き平均
を行う重付平均工程を有することができ、投票空間の量
子化サイズよりも細かい精度でパラメータを推定するこ
とができるため、量子化サイズを大きくして処理の高速
化を図ることができる。
平面に含まれる3次元データが2次元となる座標変換行
列を求め、上記3次元データを該座標変換行列により写
像して出力する写像工程を有することができ、これによ
り、後段の処理が容易になる。
結果により推定された平面パラメータを初期パラメータ
としてイタレーションにより該初期パラメータから最適
パラメータを算出する最適パラメータ算出工程を有する
ことができるため、投票による平面パラメータ推定段階
においては、ある程度の精度で推定しておけば、最適パ
ラメータ計算により精度を向上することができるため、
更に高速化が可能となる。
ける投票ピークの鋭さ、上記イタレーションに使用した
データポイント数、及び上記イタレーションにより算出
される誤差残差からなる群から選択された1以上を信頼
度のパラメータとして使用し、決定される平面の信頼度
を算出する信頼度算出工程を有することができるため、
抽出された平面の精度が明確になり、精度が低いデータ
については破棄する等してデータを選別することができ
る。
タ群から3点以上の3次元データをサンプリングし、上
記3次元データによって決まる1つの平面を示す平面パ
ラメータを複数算出する平面算出手段と、この平面算出
工程から得られた複数の平面パラメータを投票空間に投
票してこの投票結果に基づき平面を決定する平面決定手
段とを有することを特徴とする。
入力情報に基づいて動作を行う自律型のロボット装置で
あって、3次元データ群から3点以上の3次元データを
サンプリングし、上記3次元データによって決まる1つ
の平面を示す平面パラメータを複数算出する平面算出手
段と、この平面算出工程から得られた複数の平面パラメ
ータを投票空間に投票してこの投票結果に基づき平面を
決定する平面決定手段とを有することを特徴とする。
コンピュータに実行させるためのプログラムであって、
3次元データ群から3点以上の3次元データをサンプリ
ングし、上記3次元データによって決まる1つの平面を
示す平面パラメータを複数算出する平面算出工程と、こ
の平面算出工程から得られた複数の平面パラメータを投
票空間に投票してこの投票結果に基づき平面を決定する
平面決定工程とを有することを特徴とする。
ンピュータに実行させるためのプログラムを記録したコ
ンピュータ読み取り可能な記録媒体であって、3次元デ
ータ群から3点以上の3次元データをサンプリングし、
上記3次元データによって決まる1つの平面を示す平面
パラメータを複数算出する平面算出工程と、この平面算
出工程から得られた複数の平面パラメータを投票空間に
投票してこの投票結果に基づき平面を決定する平面決定
工程とを有することを特徴とする。
実施の形態について、図面を参照しながら詳細に説明す
る。本実施の形態においては、平面抽出装置が搭載され
た2足歩行のロボット装置について説明する。
ット装置の概略を示すブロック図である。図1に示すよ
うに、ロボット装置1の頭部ユニット250には、2台
のCCDカメラ200R,200Lが設けられ、このC
CDカメラ200R,200Lの後段には、ステレオ画
像処理装置210が設けられている。2台のCCDカメ
ラ(以下、右目200R、左目200Lという。)によ
り撮像された右目画像201R、左目画像201Lは、
ステレオ画像処理装置210に入力される。ステレオ画
像処理装置210は、各画像201R,201Lの視差
情報(disparity data)(距離情報)を計算し、カラー
画像(YUV:輝度Y、UV色差)202及び視差画像
(YDR:輝度Y、視差D、信頼度R)203をフレー
ム毎に左右交互に算出する。ここで、視差とは、空間中
のある点が左目及び右目に写像される点の違いを示し、
そのカメラからの距離に応じて変化するものである。
はロボット装置1の体幹部260に内蔵されたCPU
(制御部)220に入力される。また、ロボット装置1
の各関節にはアクチュエータ230が設けられており、
CPU220からの指令となる制御信号221が供給さ
れて、その指令値に応じてモータを駆動する。各関節
(アクチュエータ)には、ポテンショメータが取り付け
られ、その時のモータの回転角がCPU220へ送られ
る。このアクチュエータに取り付けられたポテンショメ
ータ、足底に取り付けられたタッチセンサ及び体幹部に
取り付けられたジャイロ・センサ等の各センサ240
は、現在の関節角度、設置情報、及び姿勢情報等の現在
のロボット装置の状態を計測し、センサデータ241と
してCPU220へ出力する。CPU220は、ステレ
オ画像処理装置210からのカラー画像202及び視差
画像203と、各アクチュエータ230の全ての関節角
度等のセンサデータ241とが入力され、後述するソフ
トウェア構成を実現する。
クト単位で構成され、ロボット装置の位置、移動量、周
囲の障害物、及び環境地図等を認識し、ロボット装置が
最終的に取るべき行動についての行動列を出力する各種
認識処理等を行うものである。なお、ロボット装置の位
置を示す座標として、例えば、後述するランドマーク等
の特定の物体等に基づく所定位置を座標の原点としたワ
ールド基準系のカメラ座標系(以下、絶対座標ともい
う。)と、ロボット装置自身を中心(座標の原点)とし
たロボット中心座標系(以下、相対座標ともいう。)と
の2つの座標を使用する。
置のソフトウェアの構成を示す模式図である。同図にお
いて、丸で表されているのが、オブジェクト又はプロセ
スと呼ばれるエンティティである。オブジェクト同士が
非同期に通信し合うことで、システム全体が動作する。
各オブジェクトはメッセージ通信と共有メモリを使用し
たオブジェクト間通信方法によりデータの受け渡し及び
プログラムの起動(Invoke)を行っている。
は、キネマティックオドメトリ(Kinematics Odometr
y)KINE310、平面抽出部(Plane Extractor)P
LEX320、障害物グリッド算出部(Occupancy Gri
d)OG330、ランドマーク位置検出部(Landmark Se
nsor)CLS340、絶対座標算出部(Localization)
LZ350及び行動決定部(Situated behavior Laye
r)SBL360から構成され、各オブジェクト単位に
て処理がなされる。これらのソフトウェアの詳細な構成
及び動作については後述する。また、ロボット装置につ
いての詳細な構成及び動作についても後述する。
搭載される平面抽出装置、即ち、上述した平面抽出部P
LEX320の平面抽出方法について説明する。
らランダムにデータをサンプリングして平面パラメータ
を算出し、ハフ変換(ランダム化ハフ変換:Randomized
Hough Transform)、即ち、平面パラメータを投票空間
に直接投票して平面であるか否かを決定するものであ
る。即ち、下記数式(2)に示すN固の3次元データか
ら、法線ベクトルの向きを(θ,φ)、原点からの距離
をdとしたときの平面パラメータ(θ,φ,d)、即ち
下記数式(3)に示す平面方程式を求める方法である。
確率密度関数の推定により3次元データから、その3次
元データに含まれる支配的な平面のパラメータを決定す
る確率論的方法である。
出方法ついて詳細に説明する。図3は、本実施の形態に
おける平面抽出方法の工程を示すフローチャートであ
る。
による方法、又はレンジセンサ等の操作によって得られ
た視差又は距離画像は、適切なキャリブレーションに基
づく変換により、3次元データに容易に変換することが
でき、この3次元データの配列が入力される。
1,p2,p3,…,pN}からM組のサブセット{p
1 1,p2 1,p3 1,…,pN 1},{p1 2,p2
2,p3 2,…,pN 2},…,{p1 M,p2 M,p
3 M,…,pN M}を取り出す(ステップS1)。この
ときの投票総数はMとなる。次に、各サブセット{p 1
i,p2 i,p3 i,…,pN i}から、パラメータ
(θi,φi,di)を計算する(ステップS2)。次
いで、計算されたパラメータを量子化し、投票先グリッ
ドを決定し、これに対して各パラメータ毎に個別に投票
を行う(ステップS3)。そして、最終的に投票数が多
いパラメータを解とする。即ち、このパラメータを有す
る平面を平面として推定し、後述するイタレーション等
の処理を行い、平面を決定する(ステップS4)。
に行われる処理について説明する。図4は、ステップS
1の前工程を示すフローチャートである。例えば距離画
像又は視差画像等のような形で得られた3次元座標デー
タ群を処理する方法について説明する。図4に示すよう
に、ステレオ視401又はレンジセンサ402からのデ
ータが入力され、キャリブレーション又はパラメータ変
換されて3次元のデータ配列が生成される(ステップS
01)。ここで、3次元座標に変換された3次元データ
群と共に、この3次元データの信頼度rを示す信頼度パ
ラメータがステレオ視401又はレンジセンサ402か
ら入力される(ステップS02)。本実施の形態におい
ては、ステレオ視等に基づく距離画像を入力画像とし、
ステレオ距離計算の過程で得られる信頼度パラメータを
有効利用することができる。また、このような信頼度パ
ラメータが利用不可能である場合、これを定数と見なす
ことで信頼度パラメータが付随しない3次元データにも
直接応用することができる。
を用いて求めることができるが、本実施の形態において
は、テンプレートの分散値による信頼度パラメータの求
め方及びマッチングスコアによる信頼度パラメータの求
め方の2つの方法について説明する。
ータは、ステレオ視による距離画像を入力としている。
ステレオ距離計測では左右のステレオ画像中の画素の対
応関係をテンプレートマッチングによって探索する。例
えば、左のステレオ画像を基準画像、右のステレオ画像
をマッチング先の画像とし、マッチングテンプレート内
の画素の輝度の分散値を求める。そして、探索する全て
のテンプレートの分散値を算出し、分散値の大きさを信
頼度パラメータとすることができる。即ち、分散値が高
い程信頼度が高いことを示し、分散値が低い程、信頼度
が低いことを示す。
の求め方としては、基準画像におけるテンプレートと、
マッチング先画像のエピポーララインに沿って選択され
た領域とを比較し、これらの領域内の画素の輝度の差か
らマッチングスコア値を算出する。そして、マッチング
スコア値、最小マッチングスコア値付近におけるピーク
の幅、及びグラフの急峻度値等からマッチングスコアに
よる信頼度パラメータを求めることができる。即ち、マ
ッチングスコア値が低く、最小スコア値付近におけるピ
ークの幅が小さく、グラフの急峻度値が大きいもの程、
信頼度が高いことを示す。
信頼度パラメータが算出され、この信頼度パラメータに
より、信頼度が所定の閾値より低いデータは切り捨てる
等して入力データの選別が行われる(ステップS0
3)。上述した何れの従来例においても、入力3次元デ
ータの信頼性に関する考慮が全くなされてないため、ノ
イズが多く紛れ込みやすいデータとなり、このノイズに
より性能が悪化するという問題点があったが、本実施の
形態のように、信頼度パラメータによる3次元データの
選別を行うことにより、信頼性及び安定性を向上し、性
能悪化を防止することができる。
データからM組のサブセットを取り出す方法について説
明する。図5は、ステップS1乃至ステップS3の工程
を示すフローチャートである。上述したステップS03
にて選別された3次元データ群(x,y,z)及び各デ
ータに付随する信頼度パラメータ(信頼度r)が出力さ
れ(ステップS11)、これらのデータ群から、平面パ
ラメータを推定するためのサンプルを抽出するサンプリ
ングを行う。サンプリングの方法としては、3次元デー
タ(x,y,z)から 1.全くランダムにサンプリングする方法 2.基準点となる1点を3次元データ全体からランダム
にサンプリングし、残りのデータを基準点近傍点、例え
ば3次元空間内での距離が十分近いデータ群等からサン
プリングする方法 3.3次元データを複数の解像度レベルに亘ってスキャ
ンしながら各小近傍(例えば、3次元空間内での距離が
十分近いデータ群等)のデータをひとまとまりにしてサ
ブセットとする方法等がある。
とは、例えば基準点との距離が閾値dmin乃至d
maxであるような任意の点とすることができる。
の平面が存在することを仮定したとき、又は何らかの付
加情報により支配的な平面が存在することを知ることが
できたとき等、平面領域が大きい場合には画面全体から
高域にサンプリングすると効果的となる。また、小規模
領域におけるデータをサンプリングする場合は、小さい
段差等を多く含む画像に効果的となる。
タの算出方法について説明する。図5に示すステップS
21においては、3種類の平面パラメータの算出方法を
有し、必要に応じて最適な算出方法をとることができ
る。先ず、第1のパラメータ算出方法は、3点の3次元
データから直接解法を使用してパラメータを決定する方
法である。
からの距離という3つのパラメータによって決定され
る。即ち、図6に示すように、法線ベクトルの向きを
(θ,φ)で極座標表示し、原点からの距離をdとした
とき、下記数式(4)が平面αを決定する方程式とな
る。
は、下記数式(5)に置き換えることもできる。
られれば上記数式(4)を満たす(θ,φ,d)を求め
ることは極めて容易であり、どちらか一方が求まれば他
方も直ちに決定される。
られているとき、下記数式(6)で示す平面の方程式
は、ベクトルm=(p1−p0)×(p2−p0)を使
用して、下記数式(7)によって一意に計算される。
3点は同一直線上にはなく(|ベクトルm|≠0)、3
点のうち1点p0は原点と一致しない(|p0|≠0)
ものとする。
による平面パラメータの推定方法である。即ち、画面全
体を適当な分解粒度・解像度でスキャンして入力される
3次元データを3次元空間内の近傍領域毎に分割する等
して得られた各データ群に対し、主成分分析を行って平
面パラメータを推定する方法である。
の3次元データを下記数式(8)とする。
C=PPTは、(理想的には)ランク(階数)2の3×
3行列となる。即ち、Cを固有値分解し、最小固有値に
対応する個有ベクトルを取れば、これが平面の法線ベク
トルとなる。ここで、第1及び第2主成分は、平面を張
る2つのベクトルを与え、これらに垂直なベクトルが第
3主成分となる。こうして主成分分析から下記数式(9
−1)に示す平面方程式は、下記数式(9−2)により
計算される。
る固有ベクトルである。こうして、第2の方法によって
も平面パラメータ(θ,φ,d)は直ちに計算すること
ができる。
メータ算出方法は、最小二乗法を使用して平面パラメー
タを当てはめる方法である。例えば、上述と同様に、画
面全体を適当な分割粒度・解像度でスキャンし、得られ
た距離画像等を例えば矩形小領域に分割する等して、3
次元空間内の近傍領域毎に3次元データ群を分割し、線
形最小二乗法を使用して平面パラメータを算出する方法
である。
状況に巧妙に特化した方法であり、数学的には汎用の方
法ではない。即ち、推定すべき平面は座標原点を通らな
いと仮定して求める方法である。この仮定により、以下
に説明する簡明な解法が適用可能となる。
の3次元データを上記数式(8)とし、この平面の方程
式を下記数式(10)とする。
標原点を通らない任意の平面を表す方程式である。ここ
で、パラメータΓ=(α,β,γ)には、何の制約条件
も付いていない。このとき、解くべき方程式は、下記数
式(11)となる。
般化逆行列P−1を計算すれば、下記数式(12)に示
すパラメータΓが、下記数式(13)に示す誤差残差r
を最小二乗法における意味で最小にする最適解を与え
る。
つ。
説明する。平面を決定するパラメータとして(θ,φ,
d)を使用することは極めて自然である。ここで、平面
の法線ベクトルnは原点を中心とする半径1の単位球の
表面上に存在するとみることができ、下記数式(15)
に示す関係が成り立つ。
考えると、ヤコビアンJは下記数式(16)となるた
め、平面を決定するパラメータ(θ,φ,d)をハフ変
換を使用する際の投票先の空間の直交軸を張るパラメー
タとして利用するのは適切ではない。
ると、図7(a)に示す投票空間500において、各グ
リッド501は同一面積であるであるのに対し、図7
(b)に示すように、実際の空間に示す面積は異なる。
即ち、実際の空間を球体510としたとき、例えば球体
の中心を通り紙面に平行且つ上向きのAの直線を中心軸
とし、上記中心から中心軸に垂直な方向の球体510上
のグリッド511の面積を例えばSとすると、上記中心
から中心軸方向の球体510上のグリッド512の面積
はScosθとなり、グリッドの面積が異なる。従っ
て、図7(a)に示す投票空間500に投票すると、実
際には、異なる面積のグリッド上に投票することにな
り、適切な投票値を得ることができない。
間を素朴に設計して投票した場合、元の対応空間内で異
なる面積のグリッドに投票することになり、ピークが鋭
く現れない場合があり、ノイズに対する脆弱性となる。
(1−1)又は数式(1−2)で定義すれば、上述の問
題を解決することができることを知見した。
間を示す模式図である。図8(a)に示すように、投票
空間600を上記数式(1−1)で定義することによ
り、投票空間600に仕切られた直交グリッド601の
ひとつひとつが、図8(b)に示す実際の3次元空間6
10における平面の微小変化に対し、一様に対応付けら
れるとみなすことができる。上記数式(1−1)のよう
に仮定することにより、例えば紙面に平行な方向の球体
の中心を通る直線を中心軸とすれば、中心軸に平行な方
向のグリッド611及び中心軸に垂直な方向のグリッド
612は共に面積Sとなり、グリッドサイズが等しくな
る。また、投票空間を上記数式(1−2)で表される空
間としても、各グリッドは、元の対応空間内で同一の面
積を占めるものとなる。なお、投票空間は、その各グリ
ッドが対応空間内で同一の面積を占めるように設計すれ
ばよく、上記数式(1−1)及び(1−2)に示される
ものに限らない。
タは、元の3次元直交座標空間に対して極座標を形成す
ることから、投票スロット(グリッド)が元の対応空間
内で同一の大きさとなるように投票空間を上記数式(1
−1)又は(1−2)として設計することにより、更に
ロバストで精度が高いデータが得られ、従ってサンプリ
ング数を減らすことができ、処理が高速化する。
る。一般に、投票の際の1票の重みは同じである場合が
多い。即ち、例えば1の投票に対しその投票値を1とす
る。これに対して、本実施の形態においては、誤差の性
質を考慮して一票の重みを定め、ノイズに対するロバス
ト性を向上する。即ち、1の投票に対し、誤差の評価に
よって、その投票値を例えば0.5又は2等とするもの
である。投票数誤差の評価方法としては以下に説明する
方法等がある。
先を計算(平面パラメータを計算)するために使用した
元の3次元データの信頼度(信頼度パラメータ)を使用
する方法である。この場合、平面パラメータを求めるの
に使用した各3次元データに付随する各信頼度を平均す
る等して投票値に重み付けすることができる。
ために使用した元の3次元データの誤差の標準偏差を使
用する方法である。この誤差とは、例えば、3点から直
接解法によって平面パラメータを求める第1のパラメー
タ算出方法であれば誤差0、主成分分析によって平面パ
ラメータを求める第2のパラメータ算出方法であれば本
来0であるべき最小固有値λ3から計算した値、最小二
乗法によって平面パラメータを求める第3のパラメータ
算出方法であれば誤差残差rから計算した値等とするこ
とができる。
タがステレオ視装置、又はレンジセンサ等から得られる
場合、一般的に視差方向における誤差は極めて小さく、
距離方向における誤差は、距離の測定値に比例する。こ
のことにより、計算元の3次元データの分布を視点中心
の球面に投影したときの立体角が大きい程、測定誤差が
計測結果に与える影響が小さくなるため、この立体角に
応じて投票値に重み付けをする。また、立体角が同じ場
合は実空間でのデータの分布の分散が大きい程、特に、
主成分分析による第2の方法においては、第1主成分と
第2主成分との積λ1λ2が大きい程、測定誤差が計算
結果に与える影響が小さい。このことを考慮して投票値
の重み付けを行うことができる。
投票値に対して異なる重み付けを行うことにより、入力
データのバラツキ、距離画像等を撮像する装置の特性に
よるバラツキ等、及びサンプルリングの仕方によるバラ
ツキ等を低減することができる。また、これらのことか
ら、計算結果の誤差に対するセンシティビティの指標を
得ることができる。
の1票の投票値の重みを異ならせ、ノイズに対してロバ
ストな投票値を得ることができる(ステップS31)。
そして、投票総数が閾値Nmaxを超えたか、又は投票
総数が閾値Nmaxを超え且つ投票値の分布が十分なピ
ークを形成しているか否かを判定する(ステップS3
2)。十分なピークとは、ピーク位置のグリッドにおけ
るデータが例えば全ての投票の数(総投票数)の一定以
上の割合を有するか、又はピーク位置のグリッドにおけ
る投票数の合計(得票数)又は投票値の合計(得票値)
が所定の閾値以上であるか否か等により判定することが
できる。ここで、十分であると判定された場合は、ピー
ク位置又はその近傍のグリッド内の平面パラメータを後
述する方法にて平均処理する。一方、十分でないと判定
された場合は、ステップS21に戻り、3次元データ群
からデータサブセットを抽出して平面パラメータを算出
する。
て説明する。図9及び図10は平面決定工程を示すフロ
ーチャートである。ステップS4の平面決定において
は、図9に示すように、先ず、ピーク値近傍における重
み付き平均処理が行われる(ステップS41)。この重
み付き平均処理により、ピークのより正確な位置を推定
することができる。
とすると、この集合gに含まれる各グリッドGiの得票
値(投票値の合計)をvi、代表パラメータを(θi,
φi,di)とすれば、推定パラメータ(θk,φk,
dk)は下記数式(17)として算出される。
ドの代表パラメータの量子化サイズよりも細かい粒度で
のパラメータ推定が可能となる。ここで、上記数式(1
7)では、ピーク値近傍においては、確立分布がほぼ正
規分布をなすと仮定している。
み付き平均処理により、推定パラメータの初期値(初期
パラメータ)(θ0,φ0,d0)を出力する。次に、
この初期パラメータ(θ0,φ0,d0)と3次元座標
700とからイタレーションを計算する(ステップS4
2)。このイタレーションにより、更に正確な平面パラ
メータを推定する。そして、図10に示すように、ステ
ップS42のイタレーションによるパラメータの変化量
が収束するか、又は所定の回数以上繰り返すかして終了
判定を行い(ステップS43)、その結果の信頼度を判
定し(ステップS44)、終了判定された平面パラメー
タ(θ,φ,d)及びその信頼度を出力する。
フィルタバンク(FBK)により、リアルタイムに多重
解像度化されており、上述のハフ変換による初期パラメ
ータ推定の際には低解像度の3次元データを使用して処
理を高速化すると共に、このイタレーションにおいて
は、高解像度の3次元データを使用することにより、精
度を更に高めるものである。即ち、イタレーションにお
いては、初期パラメータから推定される推定平面の十分
近傍にある点をサンプルとして抽出する。
いて更に詳細に説明する。図11は、ステップS42及
びS43を示すフローチャートである。図11に示すよ
うに、初期パラメータ(θ0,φ0,d0)が入力され
ると、上述したステップS03にて信頼度パラメータで
フィルタ済みの入力3次元データセットの中から、誤差
tolで推定平面にのるデータポイント、即ち、下記数
式(18)の条件を満たすデータサブセットPiを抽出
する(ステップS421)。誤差tolにおいて、誤差
許容範囲の初期値tol0は、予め適当に定めるものと
する。
化逆行列Pi −1を計算し、最小二乗の意味でのデータ
に最適に当てはまる平面のパラメータΓ(αi,βi,
γi)=(−1,−1,…,−1)Pi −1を求める
(ステップS422)。
収束したとみなせるか、又はイタレーションの回数が予
め設定された最大繰り返し数Itermaxに達するか
を判定し(ステップS431)、収束したか又は最大繰
り返し数Itermaxに達している場合はステップS
432へ進む。また、誤差tolを小さくさせて収束を
早めることも可能である。一方、収束していない、又は
所定回数に達していない場合は、次回のイタレーション
における誤差許容値toliを誤差残差riから決定
し、iをインクリメントしてステップS421に戻る。
ら下記数式(20)によって平面パラメータ(θ,φ,
d)を決定する(ステップS342)。
テップS43から出力された平面パラメータ(θ,φ,
d)の信頼度を計算する。この信頼度の計算は、事前知
識を有する場合は、ステップS432において算出され
た平面パラメータ(θ,φ,d)と、事前知識とを比較
し、予想の範囲内であることを確認し、範囲外である場
合はこの値を棄却する。また、上述したステップS3に
おけるピーク値近傍の得票数又は得票値が夫々投票総数
又は投票値の合計に占める割合、及びステップS42の
イタレーションにおいてパラメータ推定に使用した3次
元データの個数等を推定結果の信頼度として使用するこ
とができる。即ち、信頼度は下記数式(21−1)乃至
(21−3)に示す各指数によって計算することができ
る。ここで計算した信頼度は、平面パラメータと共に出
力する。また、規定の信頼度に達しない場合は、そのデ
ータを棄却する等することができる。
された場合には、各ピーク毎に上述したパラメータ推定
処理(ステップS42乃至S44)を繰り返し、最も信
頼度が高いパラメータを平面パラメータとして決定す
る。
φ,d)を有する平面について、z’=0とする座標変
換行列を求め、入力データ群をその変換行列で写像して
出力することにより、後述する後段においての処理が便
利である。
らハフ変換により直接平面パラメータを推定することが
できるため、ハフ変換の確率密度推定としての利点、即
ち、ノイズに対してロバストなデータを得ることがで
き、また、平面パラメータを求める際は、ランダムにサ
ンプリングすることができるため、より速く全体の統計
的な特徴点を把握することができる。また、3次元パラ
メータの投票空間を上記数式(1−1)又は(1−2)
と定義するため、より正確な投票結果を得ることができ
る。更に、ノイズが多い入力3次元データに対して信頼
度パラメータを使用してデータの選別を行うと共に、こ
の選別された3次元データから求められた3次元パラメ
ータ自体の信頼性も考慮するため、ノイズに対して極め
てロバストであり、信頼性及び安定性が高い結果を得る
ことができる。更にまた、投票を適当な回数でうち切
り、イタレーション等により精度を向上することによ
り、ハフ変換により平面パラメータを求めても高速化が
可能である。入力データをハードウェアで複数解像度で
サンプリングするため、データ全体の大局的な構造をつ
かむフェーズでの高速化が可能となる。
ータ空間の量子化サイズと精度との間にトレードオフの
関係がある。即ち、量子化サイズを小さくすると、精度
は向上するものの、サンプリング数を増やさなければな
らず、ピーク検出に時間がかかるが、量子化サイズを大
きくすると、高速化するものの、結果の粒度が粗く精度
が低下する。本実施の形態においては、投票結果のピー
ク周辺で重み付き平均処理をすることにより、投票先の
3次元パラメータ空間の量子化サイズよりも細かい粒度
のパラメータ推定が可能である。更に、ハフ変換の結果
を初期パラメータとしてイタレーションによる最適化を
実行して最終結果を生成するため、投票先の3次元パラ
メータ空間の量子化サイズをある程度の大きさとしても
十分高い精度が得られる。
元パラメータ空間のサイズが大きくなると、処理速度が
遅くなり、また記憶するデータ量が増加するため、カメ
ラと平面との位置関係を別の手段で取得し、その情報を
利用することでパラメータの検索空間を制限することに
より、更に高速化が可能となる。このように、本実施の
形態においては、ノイズにロバストであり精度が高いと
いうハフ変換の利点を生かしつつ、高速化を実現するこ
とができる。
ータを推定する場合、その関係式は非線形の制約条件を
含むため、1ステップの直接解放で解くことは難しい
が、本実施の形態においては、カメラと平面との位置関
係に対して、特殊な仮定、即ち、平面はカメラ位置(原
点)を通らないという仮定をすることで、単なる線形問
題に定式化し直し、この定式化を使用したことにより、
極めて容易に平面方程式(平面パラメータ)が算出でき
る。
20を有する上述した図2に示すロボット装置1のソフ
トウェアの構成及び動作について詳細に説明する。図1
2は、図2に示すソフトウェア300の動作を示すフロ
ーチャートである。
ィックオドメトリKINE310には、上述したよう
に、画像データ301及びセンサデータ302が入力さ
れる。この画像データ301は、カラー画像及びステレ
オカメラによる視差画像である。また、センサデータ3
02は、ロボット装置の関節角等のデータである。キネ
マティックオドメトリKINE310は、これらの入力
データ301,302を受け取り、メモリに格納されて
いたそれまでの画像及びセンサデータを更新する(ステ
ップS101)。
ータ302との時間的な対応を取る(ステップS102
−1)。即ち、画像データ301の画像が撮像された時
間におけるセンサデータ302の関節角を割り出す。次
いで、この関節角のデータを使用してロボット装置1が
中心に固定されたロボット中心座標系を頭部ユニットに
設けられたカメラの座標系へ変換する(ステップS10
2−2)。この場合、本実施の形態においては、ロボッ
ト中心座標系からカメラ座標系の同時変換行列等を導出
し、この同時変換行列311とこれに対応する画像デー
タとを画像認識を行うオブジェクトへ送信する。即ち、
同時変換行列311及びこれに対応する視差画像312
を平面抽出部PLEX320へ出力し、同時変換行列3
11及びカラー画像313をランドマークセンサ部CL
S340へ出力する。
行パラメータと、足底センサを使用した歩数のカウント
数とからロボット装置1の移動量を算出し、ロボット装
置1のロボット装置中心座標系における移動量を算出す
る。以下、ロボット装置中心座標系の移動量をオドメト
リともいう。このオドメトリ314を障害物グリッド算
出部OG330及び絶対座標算出部LZ350へ出力す
る。
ックオドメトリKINE310にて算出された同時変換
行列311と、これに対応するステレオカメラから得ら
れる視差画像312とが入力されると、それまでメモリ
に格納されていたこれらのデータを更新する(ステップ
S103)。そして、ステレオカメラのキャリブレーシ
ョンパラメータ等を使用して視差画像312から3次元
の位置データ(レンジデータ)を算出する(ステップS
104−1)。次いで、このレンジデータからハフ変換
等を使用して、例えば壁及びテーブル等の平面を除く平
面を平面として抽出する。また、座標変換行列311か
らロボット装置1の足底が接地している平面との対応を
取り、床面を選択し、床面上にない点、例えば所定の閾
値よりも高い位置にあるもの等を障害物として床面から
の距離を算出し、この障害物情報(obstacle)321を
障害物グリッド算出部330に出力する(ステップS1
04−2)。
述したように、キネマティックオドメトリKINE31
0にて算出されたオドメトリ314と、平面抽出部PL
EX320にて算出された障害物の観測情報(障害物情
報)321とが入力されると、メモリに格納されていた
それまでのデータを更新する(ステップS105)。そ
して、床面上に障害物があるか否かの確率を保持する障
害物グリッドを確率的手法により更新する(ステップS
106)。
ロボット装置1を中心とした例えば周囲4mの障害物情
報、即ち、上述した環境地図と、ロボット装置1の向く
方角を示す姿勢情報とを保持しており、上述の方法によ
り、環境地図を更新し、この更新した認識結果(障害物
情報331)を出力することにより、上位レイヤ、即
ち、本実施の形態においては、経路計画決定部SBL3
60にて障害物を回避する計画を作成することができ
る。
マティックオドメトリKINE310から同時変換行列
311及びカラー画像313が入力されると、予めメモ
リ内に格納されていたこれらのデータを更新する(ステ
ップS107)。そして、カラー画像313の画像処理
を行って、予め認識しているカラーランドマークを検出
する。このカラーランドマークのカラー画像313上で
の位置及び大きさをカメラ座標系での位置に変換する。
更に、同時変換行列311を使用し、カラーランドマー
クのカメラ座標系での位置をロボット中心位置座標系に
おける位置に変換し、ロボット中心位置座標系における
カラーランドマーク位置の情報(カラーランドマーク相
対位置情報)341を絶対座標算出部LZ350に出力
する(ステップS108)。
ックオドメトリKINE310からのオドメトリ314
と、ランドマークセンサ部CLS340からのカラーラ
ンドマーク相対位置情報341とが入力されると、予め
メモリ内に格納されていたこれらのデータが更新される
(ステップS109)。そして、絶対座標算出部LZ3
50が予め認識しているカラーランドマークの絶対座標
(ワールド座標系での位置)、カラーランドマーク相対
位置情報341、及びオドメトリ314を使用し、確率
的手法によりロボット装置の絶対座標(ワールド座標系
での位置)を算出する。そして、この絶対座標位置35
1を経路計画決定部SBL360に出力する。
リッド算出部OG330から障害物グリッド情報331
が入力され、絶対座標算出部LZ350から絶対座標位
置351が入力されると、予めメモリに格納されていた
これらのデータが更新される(ステップS111)。そ
して、経路計画決定部SBL360障害物グリッド算出
部OG330からの障害物情報331により、ロボット
装置1の周囲に存在する障害物に関する認識結果を取得
し、絶対座標算出部LZ350から現在のロボット装置
1の絶対座標を取得することにより、絶対座標系又はロ
ボット装置中心のロボット中心座標系で与えられた目標
地点に対して障害物に衝突せずに歩行可能な経路を生成
し、経路に応じて経路を遂行する動作コマンドを発行す
る。即ち、入力データから状況に応じてロボット装置1
が取るべき行動を決定し、その行動列を出力する(ステ
ップS112)。
は、障害物グリッド算出部OG330からロボット装置
の周囲に存在する障害物に関する認識結果及び絶対座標
算出部LZ350からの現在のロボット装置の位置の絶
対座標をユーザに提供し、このユーザからの入力に応じ
て動作コマンドを発行させる。
るデータの流れを模式的に示す図である。なお、図13
において、図1及び図2に示す構成と同一構成要素には
同一符号を伏してその詳細な説明は省略する。
r)371は、画像フレーム中から顔領域を検出するオ
ブジェクトであり、カメラ等の画像入力装置からカラー
画像202を受け取り、それを例えば9段階のスケール
画像に縮小変換する。この全ての画像の中から顔に相当
する矩形領域を探索する。重なりあった候補領域を削減
して最終的に顔と判断された領域に関する位置、大き
さ、及び特徴量等の情報372を出力して、顔識別部F
I(FaceIdentify)377へ送る。
を識別するオブジェクトであり、顔の領域を示す矩形領
域画像からなる情報372を顔検出部FDT371から
受け取り、この顔画像がメモリに格納されている手持ち
の人物辞書のうちでどの人物に相当するかを比較して人
物の識別を行う。そして、顔検出部FDT371から受
け取った顔画像の顔画像領域の位置、大きさ情報と共に
人物のID情報378をDIL379に出力する。
racker)373は、色認識を行うオブジェクトで
あり、カメラ等の画像入力装置からカラー画像202を
受け取り、予め保有している複数のカラー・モデル情報
に基づいて色領域を抽出し、連続した領域に分割する。
色認識部MCT373は、分割した各領域の位置、大き
さ及び特徴量等の情報374を距離情報付加部DIL
(DistanceInformationLinke
r)379に出力する。
ctor)375は、画像の中で動いている部分を検出
するオブジェクトであり、検出した動き領域の情報37
6を距離情報付加部DIL379に出力する。
た2次元の情報に距離情報を付加して3次元の情報を出
力するオブジェクトであり、顔検出部FI377からの
ID情報378、色認識部MCT373からの分割した
各領域の位置、大きさ及び特徴量等の情報374並びに
動き検出部MDT375からの動き領域の情報376に
距離情報を付加して3次元情報380を短期記憶部ST
M(ShortTermMemory)381に出力す
る。
1の外部環境に関する情報を比較的短い時間だけ保持す
るオブジェクトであり、ArthurDecoder
(図示せず)から音声認識結果(単語、音源方向、確信
度)を受け取り、色認識部MCT373から肌色の領域
の位置、大きさと顔領域の位置、大きさを受け取り、顔
識別部FI377から人物のID情報等を受け取る。ま
た、ロボット装置1の機体上の各センサからロボット装
置の首の方向(関節角)を受け取る。そして、これらの
認識結果及びセンサ出力を統合的に使って、現在どこに
どの人物がいて、しゃべった言葉がどの人物のものであ
り、その人物とはこれまでにどんな対話を行ったのかと
いう情報を保存する。こうした物体、即ちターゲットに
関する物理情報と時間方向でみたイベント(履歴)を出
力として、経路計画決定部(状況依存行動階層)(SB
L)360等の上位モジュールに渡す。
期記憶部STM381からの情報を基にロボット装置1
の行動(状況に依存した行動)を決定するオブジェクト
である。複数の行動を同時に評価したり、実行したりす
ることができる。また、行動を切り替えて機体をスリー
プ状態にしておき、別の行動を起動することができる。
に詳細に説明する。先ず、上述の実施の形態において説
明した平面抽出部PLEX320について説明する。平
面抽出部PLEX320は、同時変換行列311と、こ
れに対応する図14に示す左目画像201L及び右目画
像201Rから得られた視差画像312を受け取り、以
下の図15に示す処理手順にしたがって障害物を認識す
る。
変換行列311とこれに対応する視差画像312を受け
取る(ステップS61)。そして、視差画像312から
レンズ歪みとステレオの取り付け誤差を吸収したキャリ
ブレーションパラメータを利用して、画素毎にカメラ座
標から見た3次元の位置データ(x,y,z)を距離画
像として生成する(ステップS62)。
ップS62で生成された距離画像からハフ変換等を用い
て画像中の支配的な平面のパラメータを検出する(ステ
ップS63)。この平面のパラメータは図16に示すパ
ラメータ空間(θ,φ)(投票空間)のヒストグラムに
より検出される。パラメータの投票が小さいいと障害
物、大きいと平面上のものと見なすことができる。この
平面のパラメータを使って、元々画像から得られた距離
の測定点が平面上からどれくらいの距離であるかが分か
る。以上は、上述の実施の形態において説明した通りで
ある。
から図17に示すようにロボットの足底接地面への変換
を求める(ステップS64)。これにより、カメラ座標
系で表現される接地平面のパラメータの算出が行われ
る。そして、前記ステップS63での画像による平面の
検出と、前記ステップS64での足底接地平面の照合か
ら、画像中の平面パラメータから床面に相当するものを
選択する(ステップS65)。
平面パラメータを使って、元の距離画像から平面上に載
っている点を選択する(ステップS66)。これは下記
数式(22)、数式(23)を使用して、平面からの距
離dがしきい値Dthより小さい事を使って判断する。
範囲で選択された測定点(×印)を示す。この図18に
て、黒く抜かれている点が平面と判断されなかった点で
ある。
上(床面)上に載っている点以外の点(床面上に無い
点)を障害物としてステップS67にて認識することが
できる。これらの判断結果は床平面上の点(x、y)と
その高さzで表現される。例えば、高さz<0は、平面
より凹んでいる点を示す。
物点はそれをくぐることができるので障害物ではないと
いう判断も可能とすることができる。
から得た床面抽出画像(図19の(b))の高さzを0
(z=0)となるように座標変換すれば、図19の
(c)に示すように、平面上の2次元の位置で床か障害
物かを表現することができる。
定点を使って平面の検出を行うために安定した平面を抽
出できる。また、画像から得られる平面候補をロボット
の姿勢から得られる床面パラメータと照合することで正
しい平面を選択できる。また、障害物を認識するのでは
なく、実質的には床面を認識するので障害物の形状や大
きさに因らない認識が可能である。また、床面からの距
離で障害物を表現するので細かい段差や凹みまで検出す
ることができる。またロボットの大きさを考慮して跨い
だり、くぐったりする判断も容易である。さらに、2次
元床面上の障害物表現となるので経路計画などに既存の
移動ロボットで用いられる手法が適用可能であるし、3
次元の表現より高速に演算できる。
の大きさのグリッドに分割された、ロボット中心座標系
の地図情報である環境地図と、例えば環境地図上におい
てx軸方向又はy軸方向等の所定の方角からロボット装
置が向く方角を示す姿勢情報とを有している。また、環
境地図は、障害物情報が上述した平面抽出部PLEX3
20から入力され、障害物として認識されているグリッ
ド(障害物占有領域)を有する。この障害物グリッド算
出部OG330は、ロボット装置が移動すると、即ち、
オドメトリ314が入力されると、予めメモリ内に格納
されていた環境地図及び姿勢情報を、ロボット装置の姿
勢の変化(差分移動角)及び移動距離(差分移動量)に
伴って、予め認識している環境地図及び姿勢方向の情報
を更新する。ここで、差分移動量がグリッドサイズより
小さければ環境地図を更新せず、移動量がグリッドのサ
イズを超える場合は環境地図を更新する。また、環境地
図及びそのグリッドの大きさを必要に応じて適宜変更す
ることにより、計算量及びメモリコピーコスト等を低減
することができる。
図20に示すように、例えば、グリーン部分1001、
ピンク部分1002、又はブルー部分1003等のカラ
ーを有する人工的なカラーランドマーク1004を含む
環境において、ロボット装置1のセンサ情報と、ロボッ
ト装置1が行った動作情報によって、ロボット装置の自
己位置(位置及び姿勢)を同定する。
間隔状にグリッド(x,y)を設け、各グリッドの位置
l(localization)毎にロボット装置が存在する確率p
(l)を管理する。この存在確率p(l)は、ロボット
装置の移動(move)、即ち内界観測情報a、又はラ
ンドマークの観測、即ち外界観測情報sの入力に応じて
更新される。
の状態、即ち自己位置l'における存在確率p(l')
と、以前の状態l'において移動aを行ったときに状態
lになるという遷移確率p(l|a,l')に依拠す
る。即ち、これまでの各状態l'になる確率p(l')
と、状態l'において移動aを行ったときに状態lにな
るという遷移確率p(l|a,l')の積を加算してい
く(若しくは積分する)ことによって、現在の状態、即
ち自己位置lになるという存在確率p(l)に収束す
る。従って、外界観測結果としてのロボット装置の移動
aが観測されたときには、各グリッドでは、下記数式
(24)に従ってロボット装置の存在確率p(l)を更
新することができる。
置が存在するという存在確率p(l)は、存在確率p
(l)とこの状態lにおいてランドマークを観察すると
いう遷移確率p(s|l)に依拠する。したがって、状
態lにおいてランドマークの観測、即ち外界観測情報s
が入力された場合には、下式に従ってロボット装置の存
在確率p(l)を更新することができる。但し、下記数
式(25)に示すように右辺では、正規化のため、ラン
ドマークを観察するという確率p(s)で除して正規化
している。
と拡張カルマン・フィルタを併用した本実施形態に係る
ランドマークセンサCLS(自己位置同定システム)3
40の機能構成を模式的に示している。同図に示すよう
に、このランドマークセンサCLS340は、マルコフ
・ローカリゼーション部(ML)342と、拡張カルマ
ン・フィルタ部(EKL)343と、拡張カルマン・フ
ィルタ部343の動作を制御するEKL制御部344と
で構成されている。
は、作業空間内の自分の位置を離散的なグリッド上の自
己位置確率密度分布として保持し、ランドマークの観測
に関する外界観測情報sと、ロボット装置自身の動作に
関する内界観測情報aを入力して、自己位置確率密度分
布の更新を行う。そして、各時刻において、自己位置確
率密度分布の最も高い値を持つグリッドを自己位置の推
定結果としてEKL制御部344に出力する。
部342により求められた各グリッド上での自己位置確
率密度分布を表している。同図では、各グリッドにおけ
る確率密度を濃淡により表現している。最も色が濃い、
即ち自己位置確率密度分布が最も高い値を有するグリッ
ドが自己位置の推定結果となる。
自己位置同定は、センサのノイズに対してロバストであ
り、同定解の精度は粗いが解の収束速度が速いことを主
な特徴とする。
タ部343は、自己位置を状態変数[x,y,θ]を実
測値として保持し、環境内に設置されたカラーランドマ
ーク1004を観測すると、ランドマークからの相対位
置に基づいて自己位置を推定する。また、自身の動作情
報を観測すると、その動作情報に基づいて状態量の推定
を行う。
ット装置自身の動作情報aと状態、即ち、自己位置lと
の関係を規定した状態モデルと、自己位置lとランドマ
ークの観測情報sとの関係を規定した観測モデルとで構
成されている。
位置)lにおいて動作aを行った場合における理論的な
状態lを与える遷移関数F(l,a)を備えている。実
際には理論的な状態lに対してノイズ成分wが重畳され
ることから、ロボット装置の状態lは状態モデルにより
下記数式(26)のように収束する。
態、即ち自己位置iにおいて、ある既知の環境Env
(例えばランドマークの位置など)に関する観測の理論
値sを与える観測関数H(Env,l)を備えている。
実際には観測の理論値に対してノイズ成分vが重畳され
ることから、観測値sは観測モデルにより下記数式(2
7)のように収束する。
れぞれのノイズw及びvは、ここではゼロを中央値とす
るガウス分布として仮定する。
lとの関係を規定した状態モデルと自己位置lとランド
マークの観測情報sとの関係を規定した観測モデルを備
えた拡張カルマン・フィルタ部343において、動作情
報aは内界観測結果として、ランドマークの観測情報s
は外界観測結果として、夫々既知である。従って、ロボ
ット装置の自己位置同定を、ロボット装置の状態lをロ
ボット装置の動作情報a及び観測情報sにより推定する
という問題に帰着することができる。ここでは、ロボッ
ト装置の動作a、状態l、及び観測sをそれぞれ下記に
示すガウス分布として表現することができる。
中央値と及び共分散を有するガウス分布をとるものとし
て推定する。そして、ロボット装置の動作aが観測され
たときには、状態lの推定値に関する中央値並びに共分
散を下記数式(28−1),(28−2)により更新す
ることができる。
lがある中央値及び共分散を有するガウス分布をとるも
のとして推定する。そして、ランドマークの観測情報s
が観測されたときには、状態lの推定値に関する中央値
並びに共分散を下記数式(29−1),(29−2)に
より更新することができる。
1)乃至(30−4)とする。
報に対するロバスト性に優れているので、拡張カルマン
・フィルタ部343の推定結果は、ランドマークセンサ
CLS340全体の出力としている。
リゼーション部342の出力結果に応じて拡張カルマン
・フィルタ部344の動作を制御する。より具体的に
は、マルコフ・ローカリゼーション部342の自己位置
推定結果に基づいて、ランドマークの観測情報sに関す
る妥当性を検証する。観測情報sの妥当性は、マルコフ
・ローカリゼーション部342において最大の存在確率
となるグリッド位置mlpにおいてランドマークを観測
する確率p(s|mlp)が所定の閾値パラメータth
reshobsを越えたかどうかで判断することができ
る。
を観測する確率p(s|mlp)が閾値パラメータth
reshobsを下回る場合には、センサ・ノイズに対し
てロバストなマルコフ・ローカリゼーション部342に
おいてさえ、センサ・ノイズのために同定解が充分に収
束していないことが推測される。このような場合、セン
サ・ノイズに対するロバスト性が低い拡張カルマン・フ
ィルタ部343において自己位置を推定しても、精度が
高い解が得られず、むしろ演算時間を浪費するだけであ
る。このため、観測情報sが妥当でないと判断された場
合には、切換器345を用いて拡張カルマン・フィルタ
部343への外界観測情報s、即ちランドマークの観測
情報の入力を遮断して、拡張カルマン・フィルタ部34
3における自己位置推定値の更新を停止させる。
ン・フィルタ部343の自己位置推定結果の妥当性も検
証する。自己位置推定結果の妥当性は、推定された状態
lの中央値及び共分散を用いて、マルコフ・ローカリゼ
ーション部342から出力される存在確率p(l)との
分布比較テストにより判断することができる。分布比較
テストの一例は、カイ自乗テストchi−square
−test(ml,ekf)である。
ゼーション部342と拡張カルマン・フィルタ部343
との各確率分布が類似していない場合には、センサ・ノ
イズに対するロバスト性が低い拡張カルマン・フィルタ
部343における自己位置推定値の方が、センサ・ノイ
ズの影響により妥当でないと判断することができる。こ
のような場合、EKL制御部344は拡張カルマン・フ
ィルタ部343の再初期化を行わせる。何故ならば、拡
張カルマン・フィルタは再復帰に多大な時間を要するか
らである。
動作について説明する。図23は、ランドマークセンサ
CLS340の動作を示すフローチャートである。図2
3に示すように、ロボット装置1の移動に関する内界観
測情報aがランドマークセンサCLS340に入力され
ると、まずマルコフ・ローカリゼーション部342にお
いて、上記数式(24)を用いて自己位置推定値の更新
処理が行われる(ステップS201)。次いで、拡張カ
ルマン・フィルタ部343において、上記数式数(28
−1),(28−2)を用いて自己位置推定値の更新処
理が行われる(ステップS202)。
測情報sがランドマークセンサCLS340に入力され
ると、まずマルコフ・ローカリゼーション部342にお
いて、上記数式(25)を用いて、自己位置推定値の更
新処理が行われる(ステップS211)。
出力結果は、EKL制御部344に入力されて、観測情
報sの妥当性が検証される(ステップS212)。観測
情報sの妥当性は、マルコフ・ローカリゼーション部3
42において最大の存在確率となるグリッド位置mlp
においてランドマークを観測する確率p(s|mlp)
が所定の閾値パラメータthreshobsを越えたかど
うかで判断することができる。
を観測する確率p(s|mlp)が閾値パラメータth
reshobsを下回る場合には、センサ・ノイズに対し
てロバストなマルコフ・ローカリゼーション部342に
おいてさえ、センサ・ノイズのために同定解が充分に収
束していないことが推測される。このような場合、セン
サ・ノイズに対するロバスト性が低い拡張カルマン・フ
ィルタ部343において自己位置を推定しても、精度の
よい解が得られず、むしろ演算時間を浪費するだけであ
る。このため、観測情報sが妥当でないと判断された場
合には、切換器345を用いて拡張カルマン・フィルタ
部343への外界観測情報s、即ちランドマークの観測
情報の入力を遮断して、拡張カルマン・フィルタ部34
3における自己位置推定値の更新を停止させる。
を満たす、即ちグリッド位置mlpにおいてランドマー
クを観測する確率p(s|mlp)が閾値パラメータt
hreshobsを上回る場合には、さらに拡張カルマン
・フィルタ部343において、上記数式(29−1),
(29−2)を用いて更新処理が行われる(ステップS
213)。
己位置推定の結果は、EKL制御部344に入力され
て、その妥当性が検証される(ステップS214)。拡
張カルマン・フィルタ部343による自己位置推定結果
の妥当性は、推定された状態lの中央値及び共分散を用
いて、マルコフ・ローカリゼーション部342から出力
される存在確率p(l)との分布比較テストにより判断
することができる。分布比較テストの一例は、カイ自乗
テストchi−square−test(ml,ek
f)である。
ゼーション部342と拡張カルマン・フィルタ部343
それぞれの確率分布が類似していない場合には、センサ
・ノイズに対するロバスト性が低い拡張カルマン・フィ
ルタ部343における自己位置推定値の方が、センサ・
ノイズの影響により妥当でないと判断することができ
る。このような場合、EKL制御部344は拡張カルマ
ン・フィルタ部343の再初期化を行わせる(ステップ
S215)。何故ならば、拡張カルマン・フィルタは再
復帰に多大な時間を要するからである。
S340では、広い範囲で比較的短い探索時間で探索を
行う大域探索と高精度だが探索時間を要する局所的探索
とを併用して、高精度で、高速且つロバストな自己位置
同定を行うことができる。
リッド算出部OG330からの障害物情報331によ
り、ロボット装置1の周囲に存在する障害物に関する認
識結果を取得し、絶対座標算出部LZ350から現在の
ロボット装置1の絶対座標を取得することにより、絶対
座標系又はロボット装置中心のロボット中心座標系で与
えられた目標地点に対して障害物に衝突せずに歩行可能
な経路を生成し、経路に応じて経路を遂行する動作コマ
ンドを発行する。即ち、入力データから状況に応じてロ
ボット装置1が取るべき行動を決定し、その行動列を出
力する。ここで、障害物グリッド算出部OG330の、
障害物情報331に基づいた処理については後述する。
害物情報331により生成される障害物地図上の点は、
図24に示すように以下の3つの種類に分類される。
黒色で表される点)である。第2の点は、自由空間(障
害物が存在しない空間)上の点(図中の白で表される
点)である。そして、第3の点は、未観測領域上の点
(図中の斜線で表される点)である。
採用する経路計画アルゴリズムを図25のフローチャー
トに示し、以下に詳細を説明する。
線経路の周辺の障害物地図が作成されるように、視線を
目的地方向に向ける(ステップS71)。そして距離画
像を観測し、距離計測を行い、障害物地図を作成(更
新)する(ステップS72)。
観測領域と自由空間領域を移動可能領域とみなして経路
計画を行う(ステップS73)。
コストを最小化するA*探索(A*search)いう方法を
用いる。このA*探索は、評価関数としてfを用い、h
関数が許容的であるような最良優先探索である。いかな
るヒューリスティック関数に対しても効率最適という点
を用いている。
適用することにより生成された経路が移動可能であるな
否かをステップS74にてチェックし、障害物を回避す
ることのできる経路が計画できなかった場合(NO)に
は、これ以上観測を続けても移動可能な経路が得られる
可能性がないため、その旨を告げて経路計画を終了する
(ステップS75)。
前記A*探索を適用して計画できた場合(YES)に
は、ステップS76に進み、出力された経路上に未観測
領域が含まれるか否かを検索する。このステップS76
にて経路上に未観測領域が含まれなかった場合(NO)
には、ステップS77にて目的地までの経路計画として
移動可能経路を出力する。ステップS76にて未観測領
域が含まれる場合(YES)には、ステップS78に進
んで現在位置から未観測領域までの歩数を計算し、その
歩数が閾値を超えるか否かをチェックする。
る場合(YES)には、ステップS79により未知領域
までの移動可能経路を出力してから、ステップS71の
処理に戻る。一方、ステップS78において未知観測領
域までの歩数が閾値に満たない場合(NO)は、未観測
領域が距離観測されるように視線方向の制御を行って再
観測を行い(ステップS80)、再び障害物地図の更新
を行う。
した経路計画決定部SBL360は、未観測領域と自由
空間領域を移動可能領域とみなして経路計画を行い、出
力された経路上に含まれる未観測領域部のみを再観測す
ることにより、目的地に移動する際に不必要な観測及び
距離画像計算処理を行わなわず、効率的で短時間に移動
経路計画を生成することが可能となる。
た平面抽出装置を搭載した2足歩行タイプのロボット装
置について詳細に説明する。この人間型のロボット装置
は、住環境その他の日常生活上の様々な場面における人
的活動を支援する実用ロボットであり、内部状態(怒
り、悲しみ、喜び、楽しみ等)に応じて行動できるほ
か、人間が行う基本的な動作を表出できるエンターテイ
ンメントロボットである。
体幹部ユニット2の所定の位置に頭部ユニット3が連結
されると共に、左右2つの腕部ユニット4R/Lと、左
右2つの脚部ユニット5R/Lが連結されて構成されて
いる(但し、R及びLの各々は、右及び左の各々を示す
接尾辞である。以下において同じ。)。
構成を図27に模式的に示す。頭部ユニット3を支持す
る首関節は、首関節ヨー軸101と、首関節ピッチ軸1
02と、首関節ロール軸103という3自由度を有して
いる。
4R/Lは、肩関節ピッチ軸107と、肩関節ロール軸
108と、上腕ヨー軸109と、肘関節ピッチ軸110
と、前腕ヨー軸111と、手首関節ピッチ軸112と、
手首関節ロール軸113と、手部114とで構成され
る。手部114は、実際には、複数本の指を含む多関節
・多自由度構造体である。ただし、手部114の動作
は、ロボット装置1の姿勢制御や歩行制御に対する寄与
や影響が少ないので、本明細書ではゼロ自由度と仮定す
る。したがって、各腕部は7自由度を有するとする。
104と、体幹ロール軸105と、体幹ヨー軸106と
いう3自由度を有する。
5R/Lは、股関節ヨー軸115と、股関節ピッチ軸1
16と、股関節ロール軸117と、膝関節ピッチ軸11
8と、足首関節ピッチ軸119と、足首関節ロール軸1
20と、足部121とで構成される。本明細書中では、
股関節ピッチ軸116と股関節ロール軸117の交点
は、ロボット装置1の股関節位置を定義する。人体の足
部121は、実際には多関節・多自由度の足底を含んだ
構造体であるが、ロボット装置1の足底は、ゼロ自由度
とする。したがって、各脚部は、6自由度で構成され
る。
しては、合計で3+7×2+3+6×2=32自由度を
有することになる。ただし、エンターテインメント向け
のロボット装置1が必ずしも32自由度に限定されるわ
けではない。設計・制作上の制約条件や要求仕様等に応
じて、自由度、即ち関節数を適宜増減することができる
ことはいうまでもない。
由度は、実際にはアクチュエータを用いて実装される。
外観上で余分な膨らみを排してヒトの自然体形状に近似
させること、2足歩行という不安定構造体に対して姿勢
制御を行うことなどの要請から、アクチュエータは小型
且つ軽量であることが好ましい。
ム構成を模式的に示している。同図に示すように、ロボ
ット装置1は、ヒトの四肢を表現した体幹部ユニット
2,頭部ユニット3,腕部ユニット4R/L,脚部ユニ
ット5R/Lと、各ユニット間の協調動作を実現するた
めの適応制御を行う制御ユニット10とで構成される。
ト10によって統括的に制御される。制御ユニット10
は、CPU(Central Processing Unit)や、DRA
M、フラッシュROM等の主要回路コンポーネント(図
示しない)で構成される主制御部11と、電源回路やロ
ボット装置1の各構成要素とのデータやコマンドの授受
を行うインターフェイス(何れも図示しない)などを含
んだ周辺回路12とで構成される。
ト10の設置場所は、特に限定されない。図28では体
幹部ユニット2に搭載されているが、頭部ユニット3に
搭載してもよい。あるいは、ロボット装置1外に制御ユ
ニット10を配備して、ロボット装置1の機体とは有線
又は無線で交信するようにしてもよい。
自由度は、それぞれに対応するアクチュエータによって
実現される。即ち、頭部ユニット3には、首関節ヨー軸
101、首関節ピッチ軸102、首関節ロール軸103
の各々を表現する首関節ヨー軸アクチュエータA2、首
関節ピッチ軸アクチュエータA3、首関節ロール軸アク
チュエータA4が配設されている。
撮像するためのCCD(Charge Coupled Device)カメ
ラが設けられているほか、前方に位置する物体までの距
離を測定するための距離センサ、外部音を集音するため
のマイク、音声を出力するためのスピーカ、使用者から
の「撫でる」や「叩く」といった物理的な働きかけによ
り受けた圧力を検出するためのタッチセンサ等が配設さ
れている。
軸104、体幹ロール軸105、体幹ヨー軸106の各
々を表現する体幹ピッチ軸アクチュエータA5、体幹ロ
ール軸アクチュエータA6、体幹ヨー軸アクチュエータ
A7が配設されている。また、体幹部ユニット2には、
このロボット装置1の起動電源となるバッテリを備えて
いる。このバッテリは、充放電可能な電池によって構成
されている。
ット41R/Lと、肘関節ユニット42R/Lと、前腕
ユニット43R/Lに細分化されるが、肩関節ピッチ軸
107、肩関節ロール軸108、上腕ヨー軸109、肘
関節ピッチ軸110、前腕ヨー軸111、手首関節ピッ
チ軸112、手首関節ロール軸113の各々表現する肩
関節ピッチ軸アクチュエータA8、肩関節ロール軸アク
チュエータA9、上腕ヨー軸アクチュエータA10、肘
関節ピッチ軸アクチュエータA11、肘関節ロール軸ア
クチュエータA12、手首関節ピッチ軸アクチュエータ
A13、手首関節ロール軸アクチュエータA14が配備
されている。
ニット51R/Lと、膝ユニット5 2R/Lと、脛部ユ
ニット53R/Lに細分化されるが、股関節ヨー軸11
5、股関節ピッチ軸116、股関節ロール軸117、膝
関節ピッチ軸118、足首関節ピッチ軸119、足首関
節ロール軸120の各々を表現する股関節ヨー軸アクチ
ュエータA16、股関節ピッチ軸アクチュエータ
A17、股関節ロール軸アクチュエータA18、膝関節
ピッチ軸アクチュエータA19、足首関節ピッチ軸アク
チュエータA20、足首関節ロール軸アクチュエータA
21が配備されている。各関節に用いられるアクチュエ
ータA2,A3・・・は、より好ましくは、ギア直結型で
旦つサーボ制御系をワンチップ化してモータ・ユニット
内に搭載したタイプの小型ACサーボ・アクチュエータ
で構成することができる。
部ユニット4R/L、各脚部ユニット5R/Lなどの各
機構ユニット毎に、アクチュエータ駆動制御部の副制御
部20,21,22R/L,23R/Lが配備されてい
る。さらに、各脚部ユニット5R/Lの足底が着床した
か否かを検出する接地確認センサ30R/Lを装着する
とともに、体幹部ユニット2内には、姿勢を計測する姿
勢センサ31を装備している。
に設置された近接センサ又はマイクロ・スイッチなどで
構成される。また、姿勢センサ31は、例えば、加速度
センサとジャイロ・センサの組み合わせによって構成さ
れる。
て、歩行・走行などの動作期間中において、左右の各脚
部が現在立脚又は遊脚何れの状態であるかを判別するこ
とができる。また、姿勢センサ31の出力により、体幹
部分の傾きや姿勢を検出することができる。
1の出力に応答して制御目標をダイナミックに補正する
ことができる。より具体的には、副制御部20,21,
22R/L,23R/Lの各々に対して適応的な制御を
行い、ロボット装置1の上肢、体幹、及び下肢が協調し
て駆動する全身運動パターンを実現できる。
足部運動、ZMP(Zero Moment Point)軌道、体幹運
動、上肢運動、腰部高さなどを設定するとともに、これ
らの設定内容にしたがった動作を指示するコマンドを各
副制御部20,21,22R/L,23R/Lに転送す
る。そして、各々の副制御部20,21,・・・等で
は、主制御部11からの受信コマンドを解釈して、各ア
クチュエータA2,A3・・・等に対して駆動制御信号
を出力する。ここでいう「ZMP」とは、歩行中の床反
力によるモーメントがゼロとなる床面上の点のことであ
り、また、「ZMP軌道」とは、例えばロボット装置1
の歩行動作期間中にZMPが動く軌跡を意味する。な
お、ZMPの概念並びにZMPを歩行ロボットの安定度
判別規範に適用する点については、Miomir Vukobratovi
c著“LEGGED LOCOMOTION ROBOTS”(加藤一郎外著『歩
行ロボットと人工の足』(日刊工業新聞社))に記載さ
れている。
副制御部20,21,・・・等が、主制御部11からの
受信コマンドを解釈して、各アクチュエータA2,A3
・・・に対して駆動制御信号を出力し、各ユニットの駆
動を制御している。これにより、ロボット装置1は、目
標の姿勢に安定して遷移し、安定した姿勢で歩行でき
る。
ト10では、上述したような姿勢制御のほかに、加速度
センサ、タッチセンサ、接地確認センサ等の各種セン
サ、及びCCDカメラからの画像情報、マイクからの音
声情報等を統括して処理している。制御ユニット10で
は、図示しないが加速度センサ、ジャイロ・センサ、タ
ッチセンサ、距離センサ、マイク、スピーカなどの各種
センサ、各アクチュエータ、CCDカメラ及びバッテリ
が各々対応するハブを介して主制御部11と接続されて
いる。
されるセンサデータや画像データ及び音声データを順次
取り込み、これらをそれぞれ内部インターフェイスを介
してDRAM内の所定位置に順次格納する。また、主制
御部11は、バッテリから供給されるバッテリ残量を表
すバッテリ残量データを順次取り込み、これをDRAM
内の所定位置に格納する。DRAMに格納された各セン
サデータ、画像データ、音声データ及びバッテリ残量デ
ータは、主制御部11がこのロボット装置1の動作制御
を行う際に利用される。
投入された初期時、制御プログラムを読み出し、これを
DRAMに格納する。また、主制御部11は、上述のよ
うに主制御部11よりDRAMに順次格納される各セン
サデータ、画像データ、音声データ及びバッテリ残量デ
ータに基づいて自己及び周囲の状況や、使用者からの指
示及び働きかけの有無などを判断する。
びDRAMに格納した制御プログラムに基づいて自己の
状況に応じて行動を決定するとともに、当該決定結果に
基づいて必要なアクチュエータを駆動させることにより
ロボット装置1に、いわゆる「身振り」、「手振り」と
いった行動をとらせる。
ログラムに基づいて自己及び周囲の状況を判断し、使用
者からの指示及び働きかけに応じて自律的に行動でき
る。
態に応じて自律的に行動することができる。そこで、ロ
ボット装置1における制御プログラムのソフトウェア構
成例について、図29乃至図34を用いて説明する。
イヤ40は、制御プログラムの最下位層に位置し、複数
のデバイス・ドライバからなるデバイス・ドライバ・セ
ット41から構成されている。この場合、各デバイス・
ドライバは、CCDカメラやタイマ等の通常のコンピュ
ータで用いられるハードウェアに直接アクセスすること
を許されたオブジェクトであり、対応するハードウェア
からの割り込みを受けて処理を行う。
ト42は、デバイス・ドライバ・レイヤ40の最下位層
に位置し、例えば上述の各種センサやアクチュエータ2
81〜28n等のハードウェアにアクセスするためのイ
ンターフェイスを提供するソフトウェア群でなるバーチ
ャル・ロボット43と、電源の切換えなどを管理するソ
フトウェア群でなるパワーマネージャ44と、他の種々
のデバイス・ドライバを管理するソフトウェア群でなる
デバイス・ドライバ・マネージャ45と、ロボット装置
1の機構を管理するソフトウェア群でなるデザインド・
ロボット46とから構成されている。
ェクト・マネージャ48及びサービス・マネージャ49
から構成されている。オブジェクト・マネージャ48
は、ロボティック・サーバ・オブジェクト42、ミドル
・ウェア・レイヤ50、及びアプリケーション・レイヤ
51に含まれる各ソフトウェア群の起動や終了を管理す
るソフトウェア群であり、サービス・マネージャ49
は、メモリカードに格納されたコネクションファイルに
記述されている各オブジェクト間の接続情報に基づいて
各オブジェクトの接続を管理するソフトウェア群であ
る。
ック・サーバ・オブジェクト42の上位層に位置し、画
像処理や音声処理などのこのロボット装置1の基本的な
機能を提供するソフトウェア群から構成されている。ま
た、アプリケーション・レイヤ51は、ミドル・ウェア
・レイヤ50の上位層に位置し、当該ミドル・ウェア・
レイヤ50を構成する各ソフトウェア群によって処理さ
れた処理結果に基づいてロボット装置1の行動を決定す
るためのソフトウェア群から構成されている。
プリケーション・レイヤ51の具体なソフトウェア構成
をそれぞれ図30に示す。
示すように、騒音検出用、温度検出用、明るさ検出用、
音階認識用、距離検出用、姿勢検出用、タッチセンサ
用、動き検出用及び色認識用の各信号処理モジュール6
0〜68並びに入力セマンティクスコンバータモジュー
ル69などを有する認識系70と、出力セマンティクス
コンバータモジュール78並びに姿勢管理用、トラッキ
ング用、モーション再生用、歩行用、転倒復帰用、LE
D点灯用及び音再生用の各信号処理モジュール71〜7
7などを有する出力系79とから構成されている。
68は、ロボティック・サーバ・オブジェクト42のバ
ーチャル・ロボット43によりDRAMから読み出され
る各センサデータや画像データ及び音声データのうちの
対応するデータを取り込み、当該データに基づいて所定
の処理を施して、処理結果を入力セマンティクスコンバ
ータモジュール69に与える。ここで、例えば、バーチ
ャル・ロボット43は、所定の通信規約によって、信号
の授受或いは変換をする部分として構成されている。
69は、これら各信号処理モジュール60〜68から与
えられる処理結果に基づいて、「うるさい」、「暑
い」、「明るい」、「ボールを検出した」、「転倒を検
出した」、「撫でられた」、「叩かれた」、「ドミソの
音階が聞こえた」、「動く物体を検出した」又は「障害
物を検出した」などの自己及び周囲の状況や、使用者か
らの指令及び働きかけを認識し、認識結果をアプリケー
ション・レイヤ41に出力する。
に示すように、行動モデルライブラリ80、行動切換モ
ジュール81、学習モジュール82、感情モデル83及
び本能モデル84の5つのモジュールから構成されてい
る。
示すように、「バッテリ残量が少なくなった場合」、
「転倒復帰する」、「障害物を回避する場合」、「感情
を表現する場合」、「ボールを検出した場合」などの予
め選択されたいくつかの条件項目にそれぞれ対応させ
て、それぞれ独立した行動モデルが設けられている。
力セマンティクスコンバータモジュール69から認識結
果が与えられたときや、最後の認識結果が与えられてか
ら一定時間が経過したときなどに、必要に応じて後述の
ように感情モデル83に保持されている対応する情動の
パラメータ値や、本能モデル84に保持されている対応
する欲求のパラメータ値を参照しながら続く行動をそれ
ぞれ決定し、決定結果を行動切換モジュール81に出力
する。
ルは、次の行動を決定する手法として、図33に示すよ
うな1つのノード(状態)NODE0〜NODEnから
他のどのノードNODE0〜NODEnに遷移するかを
各ノードNODE0〜NODEnに間を接続するアーク
ARC1〜ARCn1に対してそれぞれ設定された遷移
確率P1〜Pnに基づいて確率的に決定する有限確率オ
ートマトンと呼ばれるアルゴリズムを用いる。
の行動モデルを形成するノードNODE0〜NODEn
にそれぞれ対応させて、これらノードNODE0〜NO
DE n毎に図34に示すような状態遷移表90を有して
いる。
DE0〜NODEnにおいて遷移条件とする入力イベン
ト(認識結果)が「入力イベント名」の列に優先順に列
記され、その遷移条件についてのさらなる条件が「デー
タ名」及び「データ範囲」の列における対応する行に記
述されている。
るノードNODE100では、「ボールを検出(BAL
L)」という認識結果が与えられた場合に、当該認識結
果とともに与えられるそのボールの「大きさ(SIZE)」
が「0から1000」の範囲であることや、「障害物を検出
(OBSTACLE)」という認識結果が与えられた場合に、当
該認識結果とともに与えられるその障害物までの「距離
(DISTANCE)」が「0から100」の範囲であることが他の
ノードに遷移するための条件となっている。
識結果の入力がない場合においても、行動モデルが周期
的に参照する感情モデル83及び本能モデル84にそれ
ぞれ保持された各情動及び各欲求のパラメータ値のう
ち、感情モデル83に保持された「喜び(Joy)」、
「驚き(Surprise)」又は「悲しみ(Sadness)」の何
れかのパラメータ値が「50から100」の範囲であるとき
には他のノードに遷移することができるようになってい
る。
ヘの遷移確率」の欄における「遷移先ノード」の行にそ
のノードNODE0〜NODEnから遷移できるノード
名が列記されているとともに、「入力イベント名」、
「データ名」及び「データの範囲」の列に記述された全
ての条件が揃ったときに遷移できる他の各ノードNOD
E0〜NODEnへの遷移確率が「他のノードヘの遷移
確率」の欄内の対応する箇所にそれぞれ記述され、その
ノードNODE0〜NODEnに遷移する際に出力すべ
き行動が「他のノードヘの遷移確率」の欄における「出
力行動」の行に記述されている。なお、「他のノードヘ
の遷移確率」の欄における各行の確率の和は100
[%]となっている。
るノードNODE100では、例えば「ボールを検出
(BALL)」し、そのボールの「SIZE(大き
さ)」が「0から1000」の範囲であるという認識結果が
与えられた場合には、「30[%]」の確率で「ノードN
ODE120(node 120)」に遷移でき、そのとき「A
CTION1」の行動が出力されることとなる。
遷移表90として記述されたノードNODE0〜 NO
DEnが幾つも繋がるようにして構成されており、入力
セマンティクスコンバータモジュール69から認識結果
が与えられたときなどに、対応するノードNODE0〜
NODEnの状態遷移表を利用して確率的に次の行動を
決定し、決定結果を行動切換モジュール81に出力する
ようになされている。
行動モデルライブラリ80の各行動モデルからそれぞれ
出力される行動のうち、予め定められた優先順位の高い
行動モデルから出力された行動を選択し、当該行動を実
行すべき旨のコマンド(以下、行動コマンドという。)
をミドル・ウェア・レイヤ50の出力セマンティクスコ
ンバータモジュール78に送出する。なお、この実施の
形態においては、図32において下側に表記された行動
モデルほど優先順位が高く設定されている。
了後に出力セマンティクスコンバータモジュール78か
ら与えられる行動完了情報に基づいて、その行動が完了
したことを学習モジュール82、感情モデル83及び本
能モデル84に通知する。
ティクスコンバータモジュール69から与えられる認識
結果のうち、「叩かれた」や「撫でられた」など、使用
者からの働きかけとして受けた教示の認識結果を入力す
る。
結果及び行動切換えモジュール71からの通知に基づい
て、「叩かれた(叱られた)」ときにはその行動の発現
確率を低下させ、「撫でられた(誉められた)」ときに
はその行動の発現確率を上昇させるように、行動モデル
ライブラリ70における対応する行動モデルの対応する
遷移確率を変更する。
y)」、「悲しみ(Sadness)」、「怒り(Anger)」、
「驚き(Surprise)」、「嫌悪(Disgust)」及び「恐
れ(Fear)」の合計6つの情動について、各情動毎にそ
の情動の強さを表すパラメータを保持している。そし
て、感情モデル83は、これら各情動のパラメータ値
を、それぞれ入力セマンティクスコンバータモジュール
69から与えられる「叩かれた」及び「撫でられた」な
どの特定の認識結果や、経過時間及び行動切換モジュー
ル81からの通知などに基づいて周期的に更新する。
ンティクスコンバータモジュール69から与えられる認
識結果と、そのときのロボット装置1の行動と、前回更
新してからの経過時間となどに基づいて所定の演算式に
より算出されるそのときのその情動の変動量を△E
[t]、現在のその情動のパラメータ値をE[t]、そ
の情動の感度を表す係数をkeとして、下記数式(3
1)によって次の周期におけるその情動のパラメータ値
E[t+1]を算出し、これを現在のその情動のパラメ
ータ値E[t]と置き換えるようにしてその情動のパラ
メータ値を更新する。また、感情モデル83は、これと
同様にして全ての情動のパラメータ値をにより更新す
る。
ンバータモジュール78からの通知が各情動のパラメー
タ値の変動量△E[t]にどの程度の影響を与えるかは
予め決められており、例えば「叩かれた」といった認識
結果は「怒り」の情動のパラメータ値の変動量△E
[t]に大きな影響を与え、「撫でられた」といった認
識結果は「喜び」の情動のパラメータ値の変動量△E
[t]に大きな影響を与えるようになっている。
ジュール78からの通知とは、いわゆる行動のフィード
バック情報(行動完了情報)であり、行動の出現結果の
情報であり、感情モデル83は、このような情報によっ
ても感情を変化させる。これは、例えば、「叫ぶ」とい
った行動により怒りの感情レベルが下がるといったよう
なことである。なお、出力セマンティクスコンバータモ
ジュール78からの通知は、上述した学習モジュール8
2にも入力されており、学習モジュール82は、その通
知に基づいて行動モデルの対応する遷移確率を変更す
る。
切換モジュール81の出力(感情が付加された行動)に
よりなされるものであってもよい。
cise)」、「愛情欲(affection)」、「食欲(appetit
e)」及び「好奇心(curiosity)」の互いに独立した4
つの欲求について、これら欲求毎にその欲求の強さを表
すパラメータを保持している。そして、本能モデル84
は、これらの欲求のパラメータ値を、それぞれ入力セマ
ンティクスコンバータモジュール69から与えられる認
識結果や、経過時間及び行動切換モジュール81からの
通知などに基づいて周期的に更新する。
欲」、「愛情欲」及び「好奇心」については、認識結
果、経過時間及び出力セマンティクスコンバータモジュ
ール78からの通知などに基づいて所定の演算式により
算出されるそのときのその欲求の変動量をΔI[k]、
現在のその欲求のパラメータ値をI[k]、その欲求の
感度を表す係数kiとして、所定周期で下記数式(3
2)を用いて次の周期におけるその欲求のパラメータ値
I[k+1]を算出し、この演算結果を現在のその欲求
のパラメータ値I[k]と置き換えるようにしてその欲
求のパラメータ値を更新する。また、本能モデル84
は、これと同様にして「食欲」を除く各欲求のパラメー
タ値を更新する。
ンバータモジュール78からの通知などが各欲求のパラ
メータ値の変動量△I[k]にどの程度の影響を与える
かは予め決められており、例えば出力セマンティクスコ
ンバータモジュール78からの通知は、「疲れ」のパラ
メータ値の変動量△I[k]に大きな影響を与えるよう
になっている。
び各欲求(本能)のパラメータ値がそれぞれ0から10
0までの範囲で変動するように規制されており、また係
数k e、kiの値も各情動及び各欲求毎に個別に設定さ
れている。
セマンティクスコンバータモジュール78は、図30に
示すように、上述のようにしてアプリケーション・レイ
ヤ51の行動切換モジュール81から与えられる「前
進」、「喜ぶ」、「鳴く」又は「トラッキング(ボール
を追いかける)」といった抽象的な行動コマンドを出力
系79の対応する信号処理モジュール71〜77に与え
る。
7は、行動コマンドが与えられると当該行動コマンドに
基づいて、その行動をするために対応するアクチュエー
タに与えるべきサーボ指令値や、スピーカから出力する
音の音声データ及び又はLEDに与える駆動データを生
成し、これらのデータをロボティック・サーバ・オブジ
ェクト42のバーチャル・ロボット43及び信号処理回
路を順次介して対応するアクチュエータ又はスピーカ又
はLEDに順次送出する。
た制御プログラムに基づいて、自己(内部)及び周囲
(外部)の状況や、使用者からの指示及び働きかけに応
じた自律的な行動ができる。
置が読取可能な形式で記録された記録媒体を介して提供
される。制御プログラムを記録する記録媒体としては、
磁気読取方式の記録媒体(例えば、磁気テープ、フレキ
シブルディスク、磁気カード)、光学読取方式の記録媒
体(例えば、CD−ROM、MO、CD−R、DVD)
等が考えられる。記録媒体には、半導体メモリ(いわゆ
るメモリカード(矩形型、正方形型など形状は問わな
い。)、ICカード)等の記憶媒体も含まれる。また、
制御プログラムは、いわゆるインターネット等を介して
提供されてもよい。
ライバ装置、又はパーソナルコンピュータ等を介して再
生され、有線又は無線接続によってロボット装置1に伝
送されて読み込まれる。また、ロボット装置1は、半導
体メモリ、又はICカード等の小型化された記憶媒体の
ドライブ装置を備える場合、これら記憶媒体から制御プ
ログラムを直接読み込むこともできる。
CCDカメラ200R,200L及び画像処理回路21
0からなるステレオ距離計測装置から信頼度画像を使用
して求められた正確な補正距離データが情報処理手段に
入力されるため、ロボット装置は、ステレオ距離計測デ
ータを用いた位置認識及び物体認識等、ロボットの後段
の認識精度を高めることができる。
パラメータと、ロボット装置のパラメータとを含み、信
頼度画像の信頼度に基づいてカメラパラメータ及びロボ
ットパラメータを制御することで、CCD画像の撮像条
件及び撮像位置等のカメラパラメータを制御するだけで
はなく、ロボット装置の頭部ユニット、左右2つの腕部
ユニット4R/L及び左右2つの脚部ユニット5R/L
等を動作させるロボットパラメータを種々変更すること
により、更に信頼度が高い距離画像を得ることができ、
ロボット装置の性能が向上する。
平面抽出方法は、3次元データ群から3点以上の3次元
データをサンプリングし、上記3次元データによって決
まる1つの平面を示す平面パラメータを複数算出する平
面算出工程と、この平面算出工程から得られた複数の平
面パラメータを投票空間に投票してこの投票結果に基づ
き平面を決定する平面決定工程とを有するため、平面パ
ラメータを投票空間に直接投票することができるため、
ノイズに対してロバストで、精度が高く、従って、サン
プリング数を減らして処理を高速化することができる。
入力情報に基づいて動作を行う自律型のロボット装置で
あって、3次元データ群から3点以上の3次元データを
サンプリングし、上記3次元データによって決まる1つ
の平面を示す平面パラメータを複数算出する平面算出手
段と、この平面算出工程から得られた複数の平面パラメ
ータを投票空間に投票してこの投票結果に基づき平面を
決定する平面決定手段とを有するため、高い精度で平面
を抽出することができ、この平面検出結果から障害物等
のロボット装置の環境を認識することができる。
略を示すブロック図である。
フトウェアの構成を示す模式図である。
すフローチャートである。
ーチャートである。
するフローチャートである。
示す模式図である。
る。
投票空間を示す模式図である。
ーチャートである。
すフローチャートである。
を示すフローチャートである。
ソフトウェアの動作を示すフローチャートである。
示す模式図である。
生成を説明するための図である。
手順を示すフローチャートである。
タを示す図である。
換処理を説明するための図である。
点を示す図である。
変換して2次元(床平面)乗に障害物を表現したことを
示す図である。
ンドマークを示す模式図である。
示すブロック図である。
られた各グリッド上での自己位置確率密度分布を示す模
式図である。
すフローチャートである。
に、現在位置から目的位置までの経路を示す模式図であ
る。
である。
外観構成を示す斜視図である。
に示す図である。
である。
ロック図である。
ミドル・ウェア・レイヤの構成を示すブロック図であ
る。
アプリケーション・レイヤの構成を示すブロック図であ
る。
ブラリの構成を示すブロック図である。
る有限確率オートマトンを説明する図である。
た状態遷移表を示す図である。
200L CCDカメラ、201R 右目画像、201
L 左目画像、202 カラー画像、203視差画像、
210 ステレオ画像処理装置、220 CPU、23
0 アクチュエータ、231 制御信号、240 セン
サ、241 センサデータ、260体幹部、300 ソ
フトウェア、310 キネマティックオドメトリKIN
E310、320 平面抽出部PLEX、330 障害
物グリッド算出部OG、340 ランドマーク位置検出
部CLS、350 絶対座標算出部LZ、360 行動
決定部SBL、400,500,510,600,61
0,700
Claims (28)
- 【請求項1】 3次元データ群から3点以上の3次元デ
ータをサンプリングし、上記3次元データによって決ま
る1つの平面を示す平面パラメータを複数算出する平面
算出工程と、 この平面算出工程から得られた複数の平面パラメータを
投票空間に投票してこの投票結果に基づき平面を決定す
る平面決定工程とを有することを特徴とする平面抽出方
法。 - 【請求項2】 上記平面算出工程は、上記平面の法線ベ
クトルの向きを(θ,φ)、原点からの距離をdとした
とき、上記平面パラメータ(θ,φ,d)を算出する工
程であって、 上記投票空間は、下記数式(1−1)又は数式(1−
2)で表されるものであることを特徴とする請求項1記
載の平面抽出方法。 【数1】 - 【請求項3】 上記3次元データ群は、入力画像に基づ
き3次元データ群に変換されるものであって、各上記3
次元パラメータは上記入力画像に基づき算出される信頼
度パラメータを個別に有し、この信頼度パラメータに基
づいて上記3次元データ群の3次元データを選別するデ
ータ選別工程を有することを特徴とする請求項1記載の
平面抽出方法。 - 【請求項4】 上記サンプリングデータは、3点をラン
ダムにサンプリングしたものであって、上記ランダムサ
ンプリングを複数回繰り返すことを特徴とする請求項1
記載の平面抽出方法。 - 【請求項5】 上記サンプリングデータは、ランダムに
サンプリングされた1つの基準点と、この基準点から所
定の距離内でランダムにサンプリングされた他の2点と
からなることを特徴とする請求項1記載の平面抽出方
法。 - 【請求項6】 上記サンプリングデータは、上記3次元
データ群を有する3次元空間を所定の領域に分割し、こ
の分割された領域に含まれる3次元データとすることを
特徴とする請求項1記載の平面抽出方法。 - 【請求項7】 上記平面算出工程は、3点の上記3次元
データから直接解法によって上記平面パラメータを算出
する工程を有することを特徴とする請求項1記載の平面
抽出方法。 - 【請求項8】 上記平面算出工程は、3点以上の上記3
次元データから共分散行列を固有値展開して最小固有値
に対応するベクトルを抽出する主成分分析により上記平
面パラメータを算出する工程を有することを特徴とする
請求項1記載の平面抽出方法。 - 【請求項9】 上記平面算出工程は、3点以上の上記3
次元データから上記平面が原点を通らない任意の平面で
あると仮定し最小二乗法を使用して上記平面パラメータ
を算出する工程を有することを特徴とする請求項1記載
の平面抽出方法。 - 【請求項10】 上記平面決定工程は、上記投票を繰り
返し、上記投票の総数が所定の閾値を超えた場合、又は
上記投票数に基づいて決定される投票値が最も高い位置
における投票値の合計が全ての投票値の合計である総投
票値の所定の割合に達した場合に投票をうち切る工程を
有することを特徴とする請求項1記載の平面抽出方法。 - 【請求項11】 上記平面決定工程は、上記3次元デー
タの信頼度及び/又は平面算出方法により異なる重みを
定めて投票する重付投票工程を有することを特徴とする
請求項1記載の平面抽出方法。 - 【請求項12】 上記平面決定工程は、上記投票空間の
投票のピーク近傍における投票数による重み付き平均を
行う重付平均工程を有することを特徴とする請求項1記
載の平面抽出方法。 - 【請求項13】 上記平面決定工程により抽出された平
面に含まれる3次元データが2次元となる座標変換行列
を求め、上記3次元データを該座標変換行列により写像
して出力する写像工程を有する請求項1記載の平面抽出
方法。 - 【請求項14】 上記平面決定工程は、上記投票結果に
より推定された平面パラメータを初期パラメータとして
イタレーションにより該初期パラメータから最適パラメ
ータを算出する最適パラメータ算出工程を有する請求項
1記載の平面抽出方法。 - 【請求項15】 上記平面決定工程は、上記投票におけ
る投票ピークの鋭さ、上記イタレーションに使用したデ
ータポイント数、及び上記イタレーションにより算出さ
れる誤差残差からなる群から選択された1以上を信頼度
のパラメータとして使用し、決定される平面の信頼度を
算出する信頼度算出工程を有することを特徴とする請求
項14記載の平面抽出方法。 - 【請求項16】 上記パラメータ算出工程は、上記初期
パラメータを推定する平面算出工程における上記3次元
データより、解像度が高い3次元データを使用してイタ
レーションを行うことを特徴とする請求項15記載の平
面抽出方法。 - 【請求項17】 3次元データ群から3点以上の3次元
データをサンプリングし、上記3次元データによって決
まる1つの平面を示す平面パラメータを複数算出する平
面算出手段と、 この平面算出工程から得られた複数の平面パラメータを
投票空間に投票してこの投票結果に基づき平面を決定す
る平面決定手段とを有することを特徴とする平面抽出装
置。 - 【請求項18】 上記平面算出手段は、上記平面の法線
ベクトルの向きを(θ,φ)、原点からの距離をdとし
たとき、上記平面パラメータ(θ,φ,d)を算出する
工程であって、 上記投票空間は、下記数式(1−1)又は数式(1−
2)で表されるものであることを特徴とする請求項17
記載の平面抽出装置。 【数2】 - 【請求項19】 上記3次元データ群は、入力画像に基
づき3次元データ群に変換されるものであって、各上記
3次元パラメータは上記入力画像に基づき算出される信
頼度パラメータを個別に有し、この信頼度パラメータに
基づいて上記3次元データ群の3次元データを選別する
データ選別手段を有することを特徴とする請求項17記
載の平面抽出装置。 - 【請求項20】 上記平面算出手段は、上記3次元デー
タから3点をサンプリングし直接解法によって上記平面
パラメータを算出する第1のパラメータ算出手段、上記
3次元データから3点以上をサンプリングし共分散行列
を固有値展開して最小固有値に対応するベクトルを抽出
する主成分分析により上記平面パラメータを算出する第
2のパラメータ手段、及び上記3次元データから3点以
上をサンプリングし上記平面が原点を通らない任意の平
面であると仮定し最小二乗法を使用して上記平面パラメ
ータを算出する第3のパラメータ算出手段からなる群か
ら選択された1以上の算出手段を有することを特徴とす
る請求項17記載の平面抽出装置。 - 【請求項21】 上記平面決定手段は、上記投票を繰り
返し、上記投票の総数が所定の閾値を超えた場合、又は
上記投票数に基づいて決定される投票値が最も高い位置
における投票値の合計が全ての投票値の合計である総投
票値の所定の割合に達した場合に投票をうち切る投票中
止手段を有することを特徴とする請求項17記載の平面
抽出装置。 - 【請求項22】 上記平面決定手段は、上記3次元デー
タの信頼度及び/又は平面算出方法により異なる重みを
定めて投票する重付投票工程を有することを特徴とする
請求項17記載の平面抽出装置。 - 【請求項23】 上記平面決定手段は、上記投票空間の
投票のピーク近傍における投票数による重み付き平均を
行う重付平均手段を有することを特徴とする請求項17
記載の平面抽出装置。 - 【請求項24】 上記平面決定手段は、上記投票結果に
より推定された平面パラメータを初期パラメータとして
イタレーションにより該初期パラメータから最適パラメ
ータを算出する最適パラメータ算出手段を有する請求項
17記載の平面抽出装置。 - 【請求項25】 上記平面決定手段は、上記投票におけ
る投票ピークの鋭さ、上記イタレーションに使用したデ
ータポイント数、及び上記イタレーションにより算出さ
れる誤差残差からなる群から選択された1以上を信頼度
のパラメータとして使用し、決定される平面の信頼度を
算出する信頼度算出手段を有することを特徴とする請求
項24記載の平面抽出装置。 - 【請求項26】 供給された入力情報に基づいて動作を
行う自律型のロボット装置であって、 3次元データ群から3点以上の3次元データをサンプリ
ングし、上記3次元データによって決まる1つの平面を
示す平面パラメータを複数算出する平面算出手段と、 この平面算出工程から得られた複数の平面パラメータを
投票空間に投票してこの投票結果に基づき平面を決定す
る平面決定手段とを有することを特徴とするロボット装
置。 - 【請求項27】 所定の動作をコンピュータに実行させ
るためのプログラムであって、 3次元データ群から3点以上の3次元データをサンプリ
ングし、上記3次元データによって決まる1つの平面を
示す平面パラメータを複数算出する平面算出工程と、 この平面算出工程から得られた複数の平面パラメータを
投票空間に投票してこの投票結果に基づき平面を決定す
る平面決定工程とを有することを特徴とするプログラ
ム。 - 【請求項28】 所定の動作をコンピュータに実行させ
るためのプログラムを記録したコンピュータ読み取り可
能な記録媒体であって、 3次元データ群から3点以上の3次元データをサンプリ
ングし、上記3次元データによって決まる1つの平面を
示す平面パラメータを複数算出する平面算出工程と、 この平面算出工程から得られた複数の平面パラメータを
投票空間に投票してこの投票結果に基づき平面を決定す
る平面決定工程とを有することを特徴とするプログラム
を記録した記録媒体。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2002073387A JP4016180B2 (ja) | 2002-03-15 | 2002-03-15 | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2002073387A JP4016180B2 (ja) | 2002-03-15 | 2002-03-15 | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 |
Related Child Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2007196638A Division JP4535096B2 (ja) | 2007-07-27 | 2007-07-27 | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2003271975A true JP2003271975A (ja) | 2003-09-26 |
JP4016180B2 JP4016180B2 (ja) | 2007-12-05 |
Family
ID=29203064
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2002073387A Expired - Lifetime JP4016180B2 (ja) | 2002-03-15 | 2002-03-15 | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4016180B2 (ja) |
Cited By (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2005326944A (ja) * | 2004-05-12 | 2005-11-24 | Hitachi Ltd | レーザー計測により地図画像を生成する装置及び方法 |
JP2006031423A (ja) * | 2004-07-16 | 2006-02-02 | Sony Corp | 画像処理方法および画像処理装置 |
WO2006090736A1 (ja) * | 2005-02-23 | 2006-08-31 | Aisin Seiki Kabushiki Kaisha | 物体認識装置 |
JP2006276985A (ja) * | 2005-03-28 | 2006-10-12 | Aisin Seiki Co Ltd | 物体認識方法及び装置 |
JP2007114057A (ja) * | 2005-10-20 | 2007-05-10 | Aisin Seiki Co Ltd | 物体認識装置 |
JP2007114056A (ja) * | 2005-10-20 | 2007-05-10 | Aisin Seiki Co Ltd | 物体認識装置 |
JP2007304801A (ja) * | 2006-05-10 | 2007-11-22 | Nec Corp | 立体性認証方法、立体性認証装置および立体性認証プログラム |
WO2007138898A1 (ja) * | 2006-05-25 | 2007-12-06 | Nec Corporation | 認識システム、認識方法および認識プログラム |
JP2008009769A (ja) * | 2006-06-29 | 2008-01-17 | Konica Minolta Holdings Inc | 顔認証システム及び顔認証方法 |
WO2009028489A1 (ja) * | 2007-08-30 | 2009-03-05 | Kabushiki Kaisha Yaskawa Denki | 物体検出方法と物体検出装置およびロボットシステム |
JP2009186364A (ja) * | 2008-02-07 | 2009-08-20 | Nec Corp | データ処理装置、姿勢推定システム、姿勢推定方法およびプログラム |
JP2012069133A (ja) * | 2011-10-24 | 2012-04-05 | Nec Corp | 立体性認証方法、立体性認証装置および立体性認証プログラム |
JP2012128744A (ja) * | 2010-12-16 | 2012-07-05 | Canon Inc | 物体認識装置、物体認識方法、学習装置、学習方法、プログラム、および情報処理システム |
JP2013193545A (ja) * | 2012-03-19 | 2013-09-30 | Nippon Soken Inc | 駐車支援装置 |
JP2013254474A (ja) * | 2012-02-10 | 2013-12-19 | Konan Gakuen | 障害物検出装置 |
JP2015108621A (ja) * | 2013-12-04 | 2015-06-11 | 三菱電機株式会社 | 3d点群センサーデータから平面を抽出する方法 |
JP2017090958A (ja) * | 2015-11-02 | 2017-05-25 | トヨタ自動車株式会社 | 環境地図の更新方法 |
KR20190002900A (ko) * | 2017-06-30 | 2019-01-09 | 현대자동차주식회사 | 로봇의 보행 제어시스템 및 그 제어방법 |
JP2021534988A (ja) * | 2018-08-28 | 2021-12-16 | ディリジェント ロボティクス,インコーポレイテッド | スキルのロボット学習及び実行のためのシステム、装置、及び方法 |
US11833684B2 (en) | 2017-02-25 | 2023-12-05 | Diligent Robotics, Inc. | Systems, apparatus, and methods for robotic learning and execution of skills |
US11904470B2 (en) | 2021-02-08 | 2024-02-20 | Diligent Robotics, Inc. | Systems, apparatuses, and methods for robotic learning and execution of skills including navigation and manipulation functions |
-
2002
- 2002-03-15 JP JP2002073387A patent/JP4016180B2/ja not_active Expired - Lifetime
Cited By (33)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4533659B2 (ja) * | 2004-05-12 | 2010-09-01 | 株式会社日立製作所 | レーザー計測により地図画像を生成する装置及び方法 |
JP2005326944A (ja) * | 2004-05-12 | 2005-11-24 | Hitachi Ltd | レーザー計測により地図画像を生成する装置及び方法 |
JP2006031423A (ja) * | 2004-07-16 | 2006-02-02 | Sony Corp | 画像処理方法および画像処理装置 |
JP4608978B2 (ja) * | 2004-07-16 | 2011-01-12 | ソニー株式会社 | 画像処理方法および画像処理装置 |
WO2006090736A1 (ja) * | 2005-02-23 | 2006-08-31 | Aisin Seiki Kabushiki Kaisha | 物体認識装置 |
JP2006276985A (ja) * | 2005-03-28 | 2006-10-12 | Aisin Seiki Co Ltd | 物体認識方法及び装置 |
US8031908B2 (en) | 2005-10-20 | 2011-10-04 | Aisin Seiki Kabushiki Kaisha | Object recognizing apparatus including profile shape determining section |
JP2007114057A (ja) * | 2005-10-20 | 2007-05-10 | Aisin Seiki Co Ltd | 物体認識装置 |
JP4662147B2 (ja) * | 2005-10-20 | 2011-03-30 | アイシン精機株式会社 | 物体認識装置 |
JP2007114056A (ja) * | 2005-10-20 | 2007-05-10 | Aisin Seiki Co Ltd | 物体認識装置 |
JP4618506B2 (ja) * | 2005-10-20 | 2011-01-26 | アイシン精機株式会社 | 物体認識装置 |
JP2007304801A (ja) * | 2006-05-10 | 2007-11-22 | Nec Corp | 立体性認証方法、立体性認証装置および立体性認証プログラム |
US8208757B2 (en) | 2006-05-25 | 2012-06-26 | Nec Corporation | Recognition system, recognition method, and recognition program |
WO2007138898A1 (ja) * | 2006-05-25 | 2007-12-06 | Nec Corporation | 認識システム、認識方法および認識プログラム |
JPWO2007138898A1 (ja) * | 2006-05-25 | 2009-10-01 | 日本電気株式会社 | 認識システム、認識方法および認識プログラム |
JP4577532B2 (ja) * | 2006-05-25 | 2010-11-10 | 日本電気株式会社 | 認識システム、認識方法および認識プログラム |
JP2008009769A (ja) * | 2006-06-29 | 2008-01-17 | Konica Minolta Holdings Inc | 顔認証システム及び顔認証方法 |
WO2009028489A1 (ja) * | 2007-08-30 | 2009-03-05 | Kabushiki Kaisha Yaskawa Denki | 物体検出方法と物体検出装置およびロボットシステム |
JP5458885B2 (ja) * | 2007-08-30 | 2014-04-02 | 株式会社安川電機 | 物体検出方法と物体検出装置およびロボットシステム |
US8401295B2 (en) | 2008-02-07 | 2013-03-19 | Nec Corporation | Pose estimation |
JP2009186364A (ja) * | 2008-02-07 | 2009-08-20 | Nec Corp | データ処理装置、姿勢推定システム、姿勢推定方法およびプログラム |
US8965114B2 (en) | 2010-12-16 | 2015-02-24 | Canon Kabushiki Kaisha | Object recognition apparatus, object recognition method, learning apparatus, learning method, storage medium and information processing system |
JP2012128744A (ja) * | 2010-12-16 | 2012-07-05 | Canon Inc | 物体認識装置、物体認識方法、学習装置、学習方法、プログラム、および情報処理システム |
JP2012069133A (ja) * | 2011-10-24 | 2012-04-05 | Nec Corp | 立体性認証方法、立体性認証装置および立体性認証プログラム |
JP2013254474A (ja) * | 2012-02-10 | 2013-12-19 | Konan Gakuen | 障害物検出装置 |
JP2013193545A (ja) * | 2012-03-19 | 2013-09-30 | Nippon Soken Inc | 駐車支援装置 |
JP2015108621A (ja) * | 2013-12-04 | 2015-06-11 | 三菱電機株式会社 | 3d点群センサーデータから平面を抽出する方法 |
JP2017090958A (ja) * | 2015-11-02 | 2017-05-25 | トヨタ自動車株式会社 | 環境地図の更新方法 |
US11833684B2 (en) | 2017-02-25 | 2023-12-05 | Diligent Robotics, Inc. | Systems, apparatus, and methods for robotic learning and execution of skills |
KR20190002900A (ko) * | 2017-06-30 | 2019-01-09 | 현대자동차주식회사 | 로봇의 보행 제어시스템 및 그 제어방법 |
KR102324773B1 (ko) * | 2017-06-30 | 2021-11-10 | 현대자동차주식회사 | 로봇의 보행 제어시스템 및 그 제어방법 |
JP2021534988A (ja) * | 2018-08-28 | 2021-12-16 | ディリジェント ロボティクス,インコーポレイテッド | スキルのロボット学習及び実行のためのシステム、装置、及び方法 |
US11904470B2 (en) | 2021-02-08 | 2024-02-20 | Diligent Robotics, Inc. | Systems, apparatuses, and methods for robotic learning and execution of skills including navigation and manipulation functions |
Also Published As
Publication number | Publication date |
---|---|
JP4016180B2 (ja) | 2007-12-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3945279B2 (ja) | 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 | |
JP4016180B2 (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 | |
JP3994950B2 (ja) | 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 | |
US11741701B2 (en) | Autonomous task performance based on visual embeddings | |
Lee et al. | Guided uncertainty-aware policy optimization: Combining learning and model-based strategies for sample-efficient policy learning | |
KR100941209B1 (ko) | 로봇 장치, 얼굴 인식 방법 및 얼굴 인식 장치 | |
JP4535096B2 (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 | |
Chen | Kalman filter for robot vision: a survey | |
JP2003266345A (ja) | 経路計画装置、経路計画方法及び経路計画プログラム並びに移動型ロボット装置 | |
US20040230340A1 (en) | Behavior controlling apparatus, behavior control method, behavior control program and mobile robot apparatus | |
US20060064202A1 (en) | Environment identification device, environment identification method, and robot device | |
JP2003266349A (ja) | 位置認識方法、その装置、そのプログラム、その記録媒体及び位置認識装置搭載型ロボット装置 | |
JP2004110802A (ja) | 環境同定装置、環境同定方法、プログラム及び記録媒体、並びにロボット装置 | |
Hueser et al. | Learning of demonstrated grasping skills by stereoscopic tracking of human head configuration | |
JP4449410B2 (ja) | ロボット装置及びその物体学習方法 | |
JP2003271958A (ja) | 画像処理方法、その装置、そのプログラム、その記録媒体及び画像処理装置搭載型ロボット装置 | |
Arowolo et al. | Development of a human posture recognition system for surveillance application | |
JP4407244B2 (ja) | ロボット装置及びその物体学習方法 | |
JP2003346150A (ja) | 床面認識装置及び床面認識方法並びにロボット装置 | |
Gonzalez | Pose Imitation Constraints for Kinematic Structures | |
Joshi | Antipodal Robotic Grasping using Deep Learning | |
Petrou et al. | RoboCup Rescue 2015-Robot League Team PANDORA (Greece) | |
Bahadori et al. | Intelligent systems for search and rescue |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20040806 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20061128 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070126 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070227 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070501 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070529 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070727 |
|
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: 20070821 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070903 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 4016180 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100928 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100928 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110928 Year of fee payment: 4 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110928 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120928 Year of fee payment: 5 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120928 Year of fee payment: 5 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130928 Year of fee payment: 6 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
EXPY | Cancellation because of completion of term |