段差部認識装置
【課題】ステレオカメラにより取得されるステレオ画像を利用して、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を含む該段差部を配置位置を精度良く認識することができる装置を提供する。
【解決手段】カメラ3R,3Lのうちの基準画像に、複数条の演算処理領域R3min(k2)を設定すると共に、複数条の演算処理領域R3min(k2)のそれぞれにおける実エッジ投影線L52rの複数の候補位置v(k1)を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線(段差部の先端側エッジの投影線)の位置に対する適合度を表す評価関数の値を射影変換を利用して算出する。実エッジ投影線を推定してなる推定エッジ投影線を、複数条の演算処理領域R3min(k2)のそれぞれにおける推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように決定する。推定エッジ投影線と段差部の踏面の平面パラメータとに基づいて段差部の空間的な配置を認識する。
【解決手段】カメラ3R,3Lのうちの基準画像に、複数条の演算処理領域R3min(k2)を設定すると共に、複数条の演算処理領域R3min(k2)のそれぞれにおける実エッジ投影線L52rの複数の候補位置v(k1)を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線(段差部の先端側エッジの投影線)の位置に対する適合度を表す評価関数の値を射影変換を利用して算出する。実エッジ投影線を推定してなる推定エッジ投影線を、複数条の演算処理領域R3min(k2)のそれぞれにおける推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように決定する。推定エッジ投影線と段差部の踏面の平面パラメータとに基づいて段差部の空間的な配置を認識する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の移動環境に存在する階段等の段差部の空間的な配置位置を、移動体に搭載されたステレオカメラによって取得されるステレオ画像を用いて認識する装置に関する。
【背景技術】
【0002】
従来、例えば自律的に移動可能なロボットに階段の昇降動作を行なわせる場合に、該ロボットに階段を認識させる技術として、例えば特許文献1に見られる技術が提案されている。
【0003】
この技術では、ロボットに搭載したステレオビジョンシステムによってロボットの外界の距離データを取得し、この距離データを基に、階段の踏面に相当する平面部を検出する。そして、検出した平面部を包含する多角形をSklanskyのアルゴリズムや、Melkmanのアルゴリズムによって求め、その求めた多角形から、階段のフロントエッジ、バックエッジ、幅、長さを特定する。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】国際公開公報WO2005/087452
【発明の概要】
【発明が解決しようとする課題】
【0005】
ところで、2足移動ロボット等の脚式移動ロボットに階段等の段差部の昇降動作を行なわせる場合、ロボットの脚の足平部を、段差部の踏面の先端側エッジ(特許文献1で言うところのフロントエッジ)に載せて、該足平部の一部を段差部の踏面からはみ出させることが必要もしくは好ましい場合がある。
【0006】
例えば、階段の踏面の奥行き寸法が、ロボットの足平部の大きさに比して狭い場合には、該足平部の全体を踏面に載せることが不可能な場合がある。あるいは、ロボットの足平部の全体を踏面に載せることが可能であっても、該足平部を振り出すときに、該足平部が階段の蹴り込み等と干渉するのを避けるために、ロボットの足平部の一部を踏面からはみ出させることが好ましい場合もある。
【0007】
そして、このような場合には、ロボットの足平部の目標とする着地位置を適切に決定するために、階段等の段差部の先端側エッジの位置及びその方向を精度よく認識する必要がある。
【0008】
しかるに、前記特許文献1に見られる如き従来の手法では、階段などの段差部の踏面の先端側エッジを検出することができない場合が多々ある。
【0009】
すなわち、ステレオビジョンシステムによって得られるステレオ画像から認識される距離データは、撮像画像のテクスチャ等の影響を受けやすいので、段差部の踏面の先端側エッジ付近での信頼性の高い距離データを得ることができない場合も多々ある。そして、このような場合には、段差部の踏面の先端側エッジ付近は、平面部分として認識できないか、もしくは、平面部分から除外されることとなる。その結果、前記多角形は、段差部の踏面の先端側エッジと異なる境界の形状を有するものとなり、該多角形から認識されるフロントエッジは、段差部の踏面の実際の先端側エッジとの誤差が大きなものとなりやすい。
【0010】
従って、前記特許文献1に見られる如き従来の手法では、階段などの段差部の踏面の先端側エッジを検出することができない場合が多々ある。
【0011】
本発明は、かかる背景に鑑みてなされたものであり、ステレオカメラにより取得されるステレオ画像を利用して、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を含む該段差部の配置位置を精度良く認識することができる装置を提供することを目的とする。
【課題を解決するための手段】
【0012】
ここで、本発明における用語について補足しておく。本発明においては、「段差部」は、下段側踏面とその一段上側の上段側踏面との組を一組以上(一組又は複数組)有するものである。この場合、「段差部」の各「踏面」は、ある仮想的な平面(無限平面)に包含される平坦な面とみなすことができるような面であるとする。そして、本発明における「段差部」は、その一形態として階段を含むものであるが、階段だけを意味するものではない。例えば、床面に設置される踏み台もしくは任意の設置物により構成される段差部も含まれる。
【0013】
また、段差部の上段側踏面の「先端側エッジ」というのは、段差部をその下段側踏面が手前側、上段側踏面が奥側となる視線で見た場合において、上段側踏面の外形の境界線のうち、手前側で横方向(水平方向もしくはそれに近い方向)に直線状に延在する境界線を意味する。
【0014】
換言すれば、段差部の上段側踏面の「先端側エッジ」というのは、段差部をその上段側踏面が手前側、下段側踏面が奥側となる視線で見た場合において、上段側踏面の外形の境界線のうち、奥側で横方向に直線状に延在する境界線を意味する。
【0015】
以上を前提として、本発明を以下に説明する。
【0016】
本発明の段差部認識装置は、上記の目的を達成するために、ステレオカメラを搭載した移動体の移動環境に存在しており、下段側踏面とその一段上側の上段側踏面との組を一組以上有する段差部の空間的な配置位置を、前記ステレオカメラにより取得されるステレオ画像を構成する基準画像及び参照画像を用いて認識する段差部認識装置であって、
前記段差部が前記ステレオカメラにより撮像されている状態において、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する2つの平面部分を少なくとも含む互いに異なる平面部分がそれぞれ投影された領域である複数の平面投影領域を抽出すると共に、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータを決定する平面投影領域抽出手段と、
前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定すると共に、該複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する評価関数算出手段と、
前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数に基づいて決定する推定エッジ投影線決定手段と、
該段差部の空間的な配置位置を認識するためのデータとして、少なくとも前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する段差部空間配置決定手段とを備え、
前記評価関数算出手段は、前記各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を算出するために、各演算処理領域内に各候補位置を境界として隣接する2つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する処理と、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する処理と、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する処理と、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する処理とを実行することを特徴とする(第1発明)。
【0017】
かかる第1発明によれば、前記段差部が前記ステレオカメラにより撮像されている状態において、前記平面投影領域抽出手段の処理が実行される。この処理では、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する2つの平面部分を少なくとも含む複数の平面投影領域が抽出される。さらに、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータが決定される。
【0018】
補足すると、第1発明における平面投影領域抽出手段の処理は、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行い得るものであれば、どのような手法を用いるものであってもよい。その手法は、公知の手法であってもよい。例えば、移動体に搭載したレーザ測距装置等の公知の測距装置を用いて、段差部の各部の距離(移動体からの距離)を測定し、その測定データを基に、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行うようにしてもよい。
【0019】
あるいは、後述する射影変換(平面射影変換)を利用した手法によって、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行なうことも可能である。
【0020】
また、平面投影領域抽出手段により抽出する各平面投影領域は、それぞれに対応する平面部分(段差部の各踏面等)の全体の投影領域である必要はなく、該平面部分の一部分の投影領域であってもよい。例えば、段差部の上段側踏面に対応して抽出される平面投影領域には、該上段側踏面の先端側エッジを基準画像に投影した線が含まれている必要なない。
【0021】
また、平面投影領域抽出手段により抽出する平面投影領域は、段差部の各踏面に対応する平面投影領域に限らず、例えば階段の蹴り込み面や階段の段鼻の先端面に対応する平面投影領域が含まれていてもよい。ただし、段差部の各踏面以外の平面部分に対応する平面投影領域を抽出対象から除外するようにしてもよい。その場合、例えば平面部分の法線方向の向きに基づいて、段差部の各踏面以外の平面部分に対応する平面投影領域を抽出対象から除外するようにすることが可能である。
【0022】
第1発明では、次に、前記評価関数算出手段の処理が実行される。この処理では、評価関数算出手段は、前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定する。
【0023】
この場合、前記実エッジ投影線は、段差部の上段側踏面に対応する平面投影領域と、下段側踏面に対応する平面投影領域との間に存在するので、該実エッジ投影線をそれぞれ内部に含むような複数条の演算処理領域を、前記平面投影領域抽出手段の処理により得られた平面投影領域及び平面パラメータに基づいて設定することが可能である。なお、各演算処理領域は、基準画像の縦方向に細長い形状を有するので、基準画像の横方向での幅が微小なもの(例えば1画素分の幅)となるような領域である。
【0024】
さらに、評価関数算出手段は、複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する。
【0025】
この評価関数は次のように算出される。すなわち、評価関数算出手段は、各演算処理領域内に各候補位置を境界として隣接する2つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する。
【0026】
この場合、各演算処理領域において、任意の1つの候補位置が、実エッジ投影線の位置に一致していると仮定すると、該候補位置を境界として隣接して設定される2つのエッジ推定用射影変換対象領域のうち、該候補位置の上側の領域は、段差部の上段側踏面の投影領域に相当するものとなる。また、候補位置の下側の領域は、段差部の上段側踏面の先端側エッジから下方に起立する面部(階段の段鼻又は蹴り込み面に対応する面部)の投影領域に相当するものとなる。
【0027】
さらに、評価関数算出手段は、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する。例えば、各候補位置を境界として隣接して設定される2つのエッジ推定用射影変換対象領域のうち、該候補位置の上側の領域に対応する射影変換用平面パラメータは、段差部の上段側踏面に対応して平面投影領域抽出手段により決定された平面パラメータに一致するものとされる。
【0028】
また、各候補位置を境界として隣接して設定される2つのエッジ推定用射影変換対象領域のうち、該候補位置の下側の領域に対応する射影変換用平面パラメータは、例えば、段差部の上段側踏面と下段側踏面との間でこれらに対して起立する面に対応して平面投影領域抽出手段により決定された平面パラメータに一致するものとされる。
【0029】
あるいは、当該射影変換用平面パラメータは、例えば段差部の上段側踏面に対してあらかじめ定められた角度(例えば90degまたはそれに近い角度)で起立し、且つ、候補位置に対応する仮想的な先端側エッジ(該先端側エッジを基準画像に投影した線の位置(基準画像の縦方向での位置)が該候補位置に一致するような仮想的な先端側エッジ)の直線を含む平面の位置及び姿勢を表す平面パラメータ(これは上段側踏面に対応して平面投影領域抽出手段により決定された平面パラメータと候補位置とから算出することができる)に一致するものとされる。
【0030】
さらに、評価関数算出手段は、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する。
【0031】
なお、エッジ推定用射影変換対象領域に対応する参照画像中の領域というのは、基準画像のエッジ推定用射影変換対象領域に投影されている実空間の部分が該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面上の部分であると仮定して、該部分を参照画像中に投影した場合に得られる領域を意味する。
【0032】
この場合、各演算処理領域において、ある候補位置が、実エッジ投影線の位置に一致しておれば、各エッジ推定用射影変換対象領域の画像の画素値分布と上記射影変換画像の画素値分布とは互いに一致(もしくはほぼ一致)する。一方、該候補位置が、実エッジ投影線の位置に一致しない場合には、各エッジ推定用射影変換対象領域の画像の画素値分布と上記射影変換画像の画素値分布とは互いに異なるものとなる。
【0033】
そして、評価関数算出手段は、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する。
【0034】
これにより、基準画像の縦方向に細長い(基準画像の横方向での幅が微小な)各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、該候補位置の実エッジ投影線の位置に対する適合度との相関関係を表すデータが得られることとなる。このデータは、換言すれば、各演算処理領域において、基準画像の縦方向でのどの位置が、実エッジ投影線の位置として妥当であるか(適合度が高いか)を示すものとなる。
【0035】
次いで、推定エッジ投影線決定手段の処理が実行される。この処理では、推定エッジ投影線決定手段は、前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数(各演算処理領域及び各候補位置の組毎に算出された評価関数の値)に基づいて決定する。
【0036】
この場合、前記評価関数算出手段の処理によって、各演算処理領域及び各候補位置の組毎に算出された評価関数の値によって、各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、上記評価関数の値との相関関係を表すデータが得られている。従って、ある推定エッジ投影線を仮定した場合、その推定エッジ投影線の、各演算処理領域における位置(基準画像の縦方向での位置)の、実エッジ投影線の実際に位置に対する適合度(妥当性の度合い)が判ることとなる。
【0037】
そして、推定エッジ投影線が実エッジ投影線に一致していれば、その推定エッジ投影線の、各演算処理領域における位置の適合度は、原理的には、該演算処理領域において最も高くなる。
【0038】
そこで、推定エッジ投影線決定手段は、前記複数条の演算処理領域のそれぞれにおける推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように該推定エッジ投影線を決定する。これにより、複数条の演算処理領域のそれぞれにおける実エッジ投影線に対する適合度が極力高いものとなるような推定エッジ投影線、すなわち、実エッジ投影線を推定したものとしての妥当性が高い推定エッジ投影線が決定される。
【0039】
なお、推定エッジ投影線決定手段の処理で決定する推定エッジ投影線は、その合成適合度が厳密に最も高いものである必要はなく、該推定エッジ投影線と基準画像での位置及び方向の一方又は両方が若干異なるような複数の線に比して、該合成適合度が相対的に高くなるようなものでよい。従って、基準画像での位置及び方向の一方又は両方が互いに異なる複数の推定エッジ投影線の候補の中で、最も、前記合成適合度が高いものを、推定エッジ投影線として決定してもよい。
【0040】
次いで、前記段差部空間配置決定手段の処理が実行される。この処理では、段差部空間配置決定手段は、ステレオカメラの撮像領域に存在する段差部の空間的な配置位置を認識するためのデータとして、少なくとも該段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する。
【0041】
この場合、段差部の上段側踏面の先端側エッジは、上段側踏面に対応して前記平面投影領域抽出手段により決定された平面パラメータにより空間的な位置及び姿勢が規定される平面上の線であり、この線を光学的に基準画像に投影してなる線が、上記推定エッジ投影線となる。従って、この推定エッジ投影線と、平面投影領域抽出手段により決定された平面パラメータ(上段側踏面に対応する平面部分の平面パラメータ)とに基づいて、先端側エッジの空間的な位置及び方向を表すデータを決定できることとなる。
【0042】
該データとしては、例えば、先端側エッジ上の2つの点の空間的な位置、あるいは、先端側エッジ上の1つの点の空間的な位置と該先端側エッジの方向ベクトルとの組等を用いることができる。
【0043】
以上説明した第1発明によれば、前記評価関数算出手段の処理によって、各演算処理領域及び各候補位置の組毎に算出される評価関数の値は、基準画像の縦方向に細長い各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、該候補位置の実エッジ投影線の位置に対する適合度との相関関係を表すものとなる。
【0044】
従って、この評価関数の値に基づいて、前記推定エッジ投影線を決定することにより、基準画像の横方向に並ぶ複数条の演算処理領域のそれぞれにおいて適合度が極力高くなるような推定エッジ投影線を決定できる。このため、実エッジ投影線に対する一致度合いの高い推定エッジ投影線を決定できることとなる。
【0045】
よって、第1発明によれば、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を精度よく認識することができる。
【0046】
上記第1発明においては、前記平面投影領域抽出手段は、レーザ測距装置等を使用することなく、前記ステレオ画像(基準画像及び参照画像)を用いて平面投影領域の抽出と平面パラメータの決定とを行なうようにすることも可能である。
【0047】
具体的には、前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する処理と、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出する(第2発明)。
【0048】
この第2発明によれば、前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する。該平面投影領域抽出用局所領域は、基準画像の全体に比して十分に小さい面積の領域である。
【0049】
ここで、ある1つの平面投影領域抽出用局所領域の画像が平面部分の画像である場合には、その画像と、基準画像の該平面投影領域抽出用局所領域に投影されている実空間の部分(ここでは平面部分のうちの局所部分)を参照画像中に投影してなる部分の画像とは、当該平面部分を包含する平面の位置及び姿勢を表す平面パラメータに応じた射影変換(平面射影変換)によって対応付けられる。
【0050】
すなわち、原理的には、平面投影領域抽出用局所領域に対応する参照画像中の画像に、該平面投影領域抽出用局所領域に投影されている平面部分の平面パラメータに応じた射影変換を施すことによって、基準画像の平面投影領域抽出用局所領域の画像と同じ画素値分布を有する画像が得られる。
【0051】
一方、平面投影領域抽出用局所領域の画像が平面部分の画像であっても、実際の平面部分に対応する平面パラメータと異なる平面パラメータを使用して、平面投影領域抽出用局所領域に対応する参照画像中の画像に射影変換を施した場合に得られる画像は、基準画像の平面投影領域抽出用局所領域の画像と異なる画素値分布を有する画像となる。
【0052】
そこで、第2発明では、平面投影領域抽出手段は、次に、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する。
【0053】
これにより、段差部の踏面等の平面部分が投影されている平面投影領域抽出用局所領域にあっては、その平面部分を包含する平面の位置及び姿勢を表す平面投影領域抽出用平面パラメータが適正に決定されることとなる。
【0054】
なお、平面投影領域抽出用局所領域に単一の平面部分とそれ以外の部分とが含まれている場合には、決定される平面投影領域抽出用平面パラメータにより規定される平面は、一般には、該平面投影領域抽出用局所領域に含まれる平面部分を包含する平面とは異なるものとなる。
【0055】
上記の如く各平面投影領域抽出用局所領域に対応する平面投影領域抽出用平面パラメータを決定した場合、同一の平面上に存在する平面投影領域抽出用局所領域に対応する平面投影領域抽出用平面パラメータは、互いに同一(もしくはほぼ同一)となる。
【0056】
そこで、平面投影領域抽出手段は、次に、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出する。なお、ここで、平面投影領域抽出用平面パラメータが互いに同一というのは、厳密に互いに同一であることだけを意味するものではなく、平面投影領域抽出用平面パラメータにより規定される平面の位置及び姿勢がある既定の範囲内でほぼ同一となるような場合も含まれる。
【0057】
これにより、第2発明によれば、レーザ測距装置等の測距装置をステレオカメラとは別に移動体に搭載したりすることなく、ステレオ画像と射影変換(平面射影変換)とを利用した手法によって、基準画像から平面投影領域を抽出することができる。
【0058】
前記第1発明又は第2発明においては、前記評価関数算出手段が各演算処理領域に設定する複数の候補位置は、全ての演算処理領域について共通のN3a個(N3a:2以上の整数)の候補位置であり、前記評価関数算出手段は、第k1番目(k1:1からN3aまでの整数のうちの任意の1つの整数)の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出するために、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域における画像と、該合成射影変換対象領域に対応する前記参照画像中の領域の画像を該合成射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる画像との誤差画像を求める処理を実行し、その誤差画像を用いて、第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出することが好ましい(第3発明)。
【0059】
この第3発明によれば、前記評価関数算出手段は、前記複数の候補位置の任意の1つである第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数を算出する前に事前に、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域に係わる前記誤差画像を求めておく。そして、前記評価関数算出手段は、この誤差画像を用いて第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出する。これにより、各候補位置及び各演算処理領域の各組毎に個別に前記誤差関数の値を算出する場合に比して、各候補位置及び各演算処理領域の各組毎の誤差関数の値の算出を効率よく行なうことができる。
【0060】
前記第1〜第3発明では、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する段差部存在検知手段をさらに備えている場合には、前記平面投影領域抽出手段、評価関数算出手段及び段差部配置決定手段の処理は、前記段差部存在検知手段によって前記ステレオカメラの撮像領域に段差部が存在していることが検知されることを必要条件として実行される処理とされる。
【0061】
この場合、前記段差部存在検知手段は、外部から与えられる情報に基づいて段差部が存在しているか否かを検知するようにすることも可能であるが、前記基準画像及び参照画像を用いて当該検知を行なうこともできる。
【0062】
具体的には、前記段差部存在検知手段は、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知するための段差部存在検知用領域を前記基準画像に設定する処理と、該段差部存在検知用領域の画像と該段差部存在検知用領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである段差部存在検知用平面パラメータを、前記段差部存在検知用領域の画像の画素値分布と該段差部存在検知用領域に対応する領域の画像を該段差部存在検知用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、決定した該段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する(第4発明)。
【0063】
この第4発明によれば、段差部存在検知手段の処理によって決定される前記段差部存在検知用平面パラメータにより規定される平面の法線方向の向きは、前記ステレオカメラの撮像領域に段差部が存在している場合には、該撮像領域が平坦な床面である場合の法線方向の向きよりも傾いたものとなる。このため、前記段差部存在検知用平面パラメータにより規定される平面の法線方向の向きに基づいて、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知することができる。
【0064】
前記第1〜第4発明においては、前記段差部が前記ステレオカメラにより撮像されている状態において、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する段差部種別判断手段をさらに備えており、
前記評価関数算出手段は、
前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第1及び第2のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第1及び第2のエッジ推定用射影変換対象領域のうちの下側の領域である第2のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第3のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定し、
前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第4及び第5のエッジ推定用射影変換対象領域を該演算処理領域内に設定することが好ましい(第5発明)。
【0065】
この第5発明によれば、前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合にあっては、前記評価関数算出手段は、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定する。これにより、前記複数条の演算処理領域のそれぞれに、前記実エッジ投影線が含まれるように該複数条の演算処理領域を基準画像に設定することができる。
【0066】
さらに、前記評価関数算出手段は、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第1及び第2のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第1及び第2のエッジ推定用射影変換対象領域のうちの下側の領域である第2のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第3のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定する。
【0067】
この場合、各演算処理領域及び各候補位置の任意の1つの組において、該演算処理領域に設定される第1〜第3のエッジ推定用射影変換対象領域は、それぞれ、当該候補位置が、実エッジ投影線の位置に一致する場合に、段差部の上段側踏面が投影される領域、該上段側踏面に対して起立する面(段鼻の先端面や蹴り込み面)が投影される領域、段差部の下段側踏面が投影される領域に相当するものとして設定される領域である。そして、これらの第1〜第3のエッジ推定用射影変換対象領域は、候補位置に応じて変化することとなる。
【0068】
従って、ステレオカメラの撮像領域に昇りの段差部が存在する場合には、各演算処理領域及び各候補位置の各組に対応して前記評価関数算出手段により算出される評価関数の値には、該候補位置で隣接する第1及び第2のエッジ推定用射影変換対象領域の画像に加えて、これらと平面パラメータベクトルが異なる第3のエッジ推定用射影変換対象領域の画像の画素値が反映されることとなる。そのため、各演算処理領域の各候補位置の、実エッジ投影線の位置に対する適合度を表すものとしての前記評価関数の値の信頼性を高めることができる。
【0069】
一方、前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合にあっては、前記評価関数算出手段は、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定する。
【0070】
これにより、前記複数条の演算処理領域のそれぞれに、前記実エッジ投影線が含まれるように該複数条の演算処理領域を基準画像に設定することができる。
【0071】
さらに、前記評価関数算出手段は、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第4及び第5のエッジ推定用射影変換対象領域を該演算処理領域内に設定する。
【0072】
ここで、ステレオカメラの撮像流域に存在する段差部が降りの段差部である場合には、基準画像には、段鼻の先端面や蹴り込み面のように、上段側透明に対して起立する姿勢の面は写らない。従って、評価関数算出手段は、上記第4及び第5のエッジ推定用射影変換対象領域を演算処理領域内に設定する。これらの第4及び第5のエッジ推定用射影変換対象領域は、それぞれ、当該候補位置が、実エッジ投影線の位置に一致する場合に、段差部の上段側踏面が投影される領域、段差部の下段側踏面が投影される領域に相当するものとして設定される領域である。そして、これらの第4及び第5のエッジ推定用射影変換対象領域は、候補位置に応じて変化することとなる。
【0073】
従って、ステレオカメラの撮像領域に存在する段差部が降りの段差部である場合には、各演算処理領域及び各候補位置の各組に対応して前記評価関数算出手段により算出される評価関数の値には、該候補位置で隣接する第4及び第5のエッジ推定用射影変換対象領域の画像の画素値が適切に反映されることとなる。そのため、各演算処理領域の各候補位置の、実エッジ投影線の位置に対する適合度を表すものとしての前記評価関数の値の信頼性を適切に確保することとができる。
【0074】
よって、第5発明によれば、ステレオカメラの撮像領域に存在する段差部が昇りの段差部と降りの段差部とのといずれであっても、実エッジ投影線に相当するものとして推定エッジ投影線を適切な信頼性で決定することができる。
【0075】
上記第5発明においては、前記ステレオカメラの撮像領域に存在する段差部が昇りの段差部である場合において、前記平面投影領域抽出手段により、段鼻の先端面や蹴り込み面のように上段側踏面に対して起立した姿勢の平面部分の平面投影領域を抽出するようにした場合には、前記第2のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、当該起立した姿勢の平面部分の平面投影領域に対応する平面パラメータを使用することができる。
【0076】
ただし、段差部の段鼻の先端面や蹴り込み面のように上段側踏面に対して起立した姿勢の平面部分は、一般に、上段側踏面に対して一般に垂直もしくはそれに近い角度で起立する。そこで、前記第5発明においては、前記評価関数算出手段は、前記段差部種別判断手段によって前記段差部が昇りの段差部であると判断された場合に、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第1のエッジ推定用射影変換対象領域と前記第3のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定し、前記第2のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、第1のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面に対してあらかじめ定めた角度(例えば90deg又はそれに近い角度)で交差する平面の位置及び姿勢を表す平面パラメータを設定するようにしてもよい(第6発明)。
【0077】
この第6発明によれば、前記ステレオカメラの撮像領域に存在する段差部が昇りの段差部である場合に、前記平面領域抽出手段により、各踏面に対応する平面投影領域だけを抽出した場合であっても、第1〜第3のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを適切に設定することができる。
【0078】
なお、前記段差部種別判断手段によって前記段差部が降りの段差部であると判断された場合にあっては、前記評価関数算出手段は、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第4のエッジ推定用射影変換対象領域と前記第5のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定するようにすればよい。
【0079】
補足すると、前記第5発明又は第6発明において、前記第4発明の構成を組み込んだ場合には、前記段差部種別判断手段は、前記段差部存在検知手段により決定された段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて、前記段差部が昇りの段差部であるか、降りの段差部であるかを判断することができる。
【0080】
これは、前記段差部が昇りの段差部であるか、降りの段差部であるかに依存して、前記段差部存在検知用平面パラメータにより示される平面の法線が、段差部が存在しない平坦な面の法線方向に対して傾く向きが異なるからである。
【0081】
このように前記段差部種別判断手段を構成することで、段差部存在検知手段により決定された段差部存在検知用平面パラメータを利用して容易に、前記段差部が昇りの段差部であるか、降りの段差部であるかを判断することができる。
【0082】
本発明においては、さらに、前記段差部空間配置決定手段は、前記段差部の上段側踏面の先端側エッジを、該上段側踏面に対応する前記平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する方向で該段差部の下段側踏面に対応する前記平面パラメータにより規定される平面に投影した線を、該下段側踏面の基端側の境界線として決定する手段を含むことが好ましい(第7発明)。
【0083】
この第8発明によれば、前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータに加えて、前記段差部空間配置決定手段により下段側踏面の基端側の境界線が決定される。このため、上段側踏面の一段下側の下段側踏面の奥行きを認識できることとなる。
【0084】
さらに本発明においては、前記移動体には、前記ステレオカメラのよる撮像時に該ステレオカメラの撮像領域にテクスチャを付与する投光器が搭載されていることが好ましい(第8発明)。
【0085】
この第8発明によれば、ある平面部分が投影されている基準画像の任意の領域の画像の画素値分布と、それに対応する参照画像の領域の画像を、適宜の平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との差は、該平面パラメータが、実際の平面部分の平面パラメータに対して相違する場合に顕著なものとなりやすい。
【0086】
このため、前記評価関数算出手段により算出される評価関数の値、あるいは、前記第2発明において平面投影領域抽出手段が決定する平面投影領域抽出用平面パラメータ(ひいては各平面投影領域の平面パラメータ)、あるいは、前記第4発明における段差部存在検知手段が決定する段差部存在検知用平面パラメータの信頼性を高めることができる。
【図面の簡単な説明】
【0087】
【図1】図1(a),(b)はそれぞれ、本発明の一実施形態におけるロボット(移動体)が階段を昇ろうとしている状況、降ろうとしている状況を示す斜視図。
【図2】実施形態のロボットに搭載した投光器によってステレオカメラの撮像領域に付与されるテクスチャを例示する図。
【図3】代表的な階段の構造を説明するための図。
【図4】図1に示すロボットに備えた演算処理装置の機能的構成を示すブロック図。
【図5】図4に示す段差部存在検知部の処理を示すフローチャート。
【図6】図5のSTEP1の処理を説明するための図。
【図7】図7(a),(b)は、図5のSTEP2の処理を説明するための図。
【図8】図4に示す平面投影領域抽出部の処理を示すフローチャート。
【図9】図8のSTEP11,15の処理を説明するための図。
【図10】図8のSTEP13の処理により得られるデータを示すグラフ。
【図11】図4に示すエッジ投影線推定処理部の処理を示すフローチャート。
【図12】図4に示すエッジ投影線推定処理部の処理を示すフローチャート。
【図13】図11のSTEP22の処理を説明するための図。
【図14】図11のSTEP23の処理を説明するための図。
【図15】図11のSTEP30の処理を説明するための図。
【図16】図16(a),(b)は、図11のSTEP32の処理により得られるデータを例示するグラフ。
【図17】図11のSTEP35の処理を説明するための図。
【図18】図11のSTEP36の処理を説明するための図。
【図19】図12のSTEP37の処理を説明するための図。
【図20】図12のSTEP38の処理を説明するための図。
【図21】図12のSTEP45の処理を説明するための図。
【発明を実施するための形態】
【0088】
本発明の一実施形態を図1〜図20を参照して以下に説明する。
【0089】
図1(a),(b)を参照して、本実施形態では、移動体としての脚式移動ロボット1によって階段等の段差部の空間的な配置位置を認識する場合を例にとって説明する。図示例の脚式移動ロボット1(以下、単にロボット1という)は、左右一対の2つの脚体を備える2足歩行ロボットである。
【0090】
このロボット1には、ステレオカメラを構成する左右一対のカメラ3R,3Lと、カメラ3R,3Lの撮像領域に投光する投光器4とが搭載されている。
【0091】
カメラ3Rは、撮像領域に向って右側のカメラ、カメラ3Lは、撮像領域に向って左側のカメラであり、それぞれCCDカメラ等により構成されている。なお、カメラ3R,3Lの撮像画像は、モノトーン画像及びカラー画像のいずれの画像であってもよい。また、図1(a),(b)では、カメラ3R,3Lは、ロボット1の頭部に搭載されているが、ロボット1の胸部等、他の部位に搭載されていてもよい。
【0092】
ロボット1は、これらのカメラ3R,3Lによって、ロボット1の周辺の移動環境(主に、ロボット1の進行方向前方の環境)を撮像することにより、カメラ3Rの撮像画像とカメラ3Lの撮像画像とから構成されるステレオ画像を随時、取得することが可能となっている。
【0093】
また、本実施形態では、ロボット1は、カメラ3R,3Lによる撮像時に、その撮像領域にテクスチャを付与するための補助光SLを投光器4から投光する。この投光器4は、本実施形態では、図2に示す如く、撮像領域の面(図2では平面)に、互いに平行に並ぶ複数の斜行輝線SLaと、この斜行輝線SLaに対して交差する方向で互いに平行に並ぶ複数の斜行輝線SLbとから構成されるテクスチャを形成する所定色(例えば赤色)のレーザ光を上記補助光SLとして出力する。
【0094】
なお、投光器4により形成するテクスチャは、上記のパターンに限らず、ランダムドットなど、一様でない他のパターンの模様であってもよい。
【0095】
そして、ロボット1は、カメラ3R,3Lによって取得したステレオ画像等を基に、周辺の移動環境の床形状や設置物等を認識しつつ移動する。
【0096】
例えば、図1(a),(b)に示すように、ロボット1の進行方向前方に段差部としての階段50(図1(a)では昇り階段、図1(b)では降り階段)が存在している状況において、ロボット1は、カメラ3R,3Lによって取得した進行方向前方の環境のステレオ画像を基に、カメラ3R,3Lの撮像領域に階段50が存在することや、その階段50の空間的な配置位置を認識する。
【0097】
この場合、本実施形態におけるロボット1では、階段50の空間的な配置位置を認識するために、階段50の各段の踏面51の先端側エッジ52及び基端側エッジ53のそれぞれの空間的な配置位置と、各踏面51を包含する平面の空間的な姿勢(法線方向)とが推定される。
【0098】
ここで、ロボット1が認識する階段50の各踏面51の先端側エッジ52及び基端側エッジ53について補足すると、図1(a)に示すロボット1の如く、階段50をその下段側踏面51が手前側、該下段側踏面51の一段上側の上段側踏面51が奥側となる視線で見た場合においては、各踏面51の先端側エッジ52は、該踏面51の外形の境界線のうち、手前側で横方向(階段50の幅方向)に直線状に延在する境界線である。また、この場合において、各踏面51の基端側エッジ53は、該踏面51の外形の境界線のうち、該踏面51の奥側で横方向に直線状に延在する境界線である。
【0099】
また、図1(b)に示すロボット1の如く、階段50をその上段側踏面51が手前側、該上段側踏面51の一段下側の下段側踏面51が奥側となる視線で見た場合にいては、各踏面51の先端側エッジ52は、上該踏面51の外形の境界線のうち、奥側で横方向(階段50の幅方向)に直線状に延在する境界線である。また、この場合において、各踏面51の基端側エッジ53は、該踏面51の外形の境界線のうち、該踏面51の手前側で横方向に直線状に延在する境界線である。
【0100】
また、段差部としての階段の構造に関して図3を参照して補足すると、階段の各段の踏面の先端部は、一般に段鼻と言われる部分である。従って、階段の踏面の先端側エッジというのは、換言すれば、階段の各段の段鼻の上面の先端側境界線を意味する。また、階段の上段側踏面の先端側と、その一段下側の下段側踏面の基端側との間での部分は、一般に蹴込みと言われる。そして、階段の各段の踏面の先端部(段鼻)は、多くの場合、図3に示す如く、その先端面が、上記蹴込みの箇所に起立した姿勢で形成される面部(以降、蹴込み面という)よりも若干突き出るように形成される。
【0101】
但し、本実施形態で、段差部の例として主に説明する階段では、段鼻の先端面が、蹴込み面よりも若干突き出ている必要はない。例えば、図1(a),(b)に簡略的に記載した階段50のように、段鼻の先端面と蹴込み面とが面一もしくはほぼ面一に形成されていてもよい。
【0102】
また、本実施形態で、段差部の例として主に説明する階段は、蹴込みの箇所に、蹴込み面を形成する板部材が設けられていない(蹴込み面が存在しない)構造のものであってもよい。
【0103】
図4を参照して、ロボット1には、CPU、RAM、ROMを含む電子回路ユニットにより構成された演算処理装置10が搭載されている。そして、この演算処理装置10にカメラ3R,3L及び投光器4が接続されている。
【0104】
この演算処理装置10は、カメラ3R,3L及び投光器4の動作制御を行なう機能を有しており、カメラ3R,3Lの撮像動作と、投光器4の補助光SLの出力動作とを互いに同期した所要のタイミング(所定の制御処理周期のタイミング)で行なわせるように制御する。そして、演算処理装置10は、カメラ3R,3Lによりそれぞれ取得される撮像画像の画素値データ(撮像画像を構成する画素毎の輝度、彩度、色相等を表すデータ)を取り込み、この画素値データを、図示しない画像メモリに逐次更新しつつ、記憶保持する。
【0105】
なお、カメラ3R,3Lに入射する光の波長を制限するバンドパスフィルタを備えておき、補助光SLの波長に一致もしくはほぼ一致する波長の光だけをカメラ3R,3Lで撮像するようにしてもよい。
【0106】
さらに、演算処理装置10は、実装されたプログラムによって実現される機能として、カメラ3R,3Lの撮像領域に段差部が存在しているか否かを検知する段差部存在検知部11と、該撮像領域に存在する実際の段差部の平面部分をステレオ画像のうちの基準画像に投影してなる平面投影領域を抽出すると共に各平面投影領域に対応する平面部分の平面パラメータを決定(推定)する平面投影領域抽出部12と、撮像領域に存在する実際の段差部の踏面の先端側エッジを基準画像に投影してなる実エッジ投影線を推定(特定)するための処理を実行するエッジ投影線推定処理部13と、段差部の空間的な配置位置を特定する段差部空間配置決定部14とを備えている。そして、演算処理装置10は、これらの機能部11〜14の処理によって、本発明の段差部認識装置の一実施形態としての機能を実現する。
【0107】
この場合、本実施形態では、カメラ3Rの撮像画像とカメラ3Lの撮像画像とのうちの一方、例えばカメラ3Rの撮像画像を基準画像、他方のカメラ3Lの撮像画像を参照画像として使用する。なお、演算処理装置10は、ロボット1の動作制御を行なう機能を備えていてもよい。
【0108】
以下に、上記段差部存在検知部11、平面投影領域抽出部12、エッジ投影線推定処理部13、段差部空間配置決定部14の処理を詳細に説明する。
【0109】
カメラ3R,3Lによって取得されたロボット1の進行方向前方の撮像画像(ステレオ画像)の画素値データが段差部存在検知部11に与えられる。そして、段差部存在検知部11は、与えられたステレオ画像を使用して、図5のフローチャートに示す処理を実行する。
【0110】
段差部存在検知部11は、まず、STEP1の処理を実行する。このSTEP1では、段差部存在検知部11は、与えられたステレオ画像のうちの基準画像内に、図6に例示する如く、段差部の存在の有無を検知するための領域である段差部存在検知用領域R1を設定する。
【0111】
図6に示す基準画像は、カメラ3R,3Lの撮像領域に階段50(図示例では昇り階段)が存在する場合の基準画像を概略的に例示したものである。この場合、階段50の表面には前記投光器4から出力された補助光SLによって、テクスチャとしての輝線SLa,SLbが付与される。この輝線SLa,SLbも基準画像に投影される。なお、図6では基準画像に投影される輝線SLa,SLbの一部を代表的に示している。
【0112】
STEP1で段差部存在検知部11が設定する段差部存在検知用領域R1は、本実施形態では、その大きさ(基準画像の横方向及び縦方向での幅)があらかじめ定められた方形状の領域である。この段差部存在検知用領域R1の横方向の幅は、階段等の段差部が基準画像に投影されている場合に、該基準画像における段差部の横方向の幅内に十分に収まる程度の大きさに設定されている。
【0113】
また、段差部存在検知用領域R1の縦方向の幅は、階段等の段差部が基準画像に投影されている場合に、段差部存在検知用領域R1内に複数段の踏面の画像が含まれるような大きさに設定されている。
【0114】
そして、段差部存在検知用領域R1は、基準画像内で、ロボット1の進行予定領域(ロボット1を目標とする進行方向に移動させた場合に該ロボット1が通過する領域)に存在するような位置に配置される。
【0115】
次いで、段差部存在検知部11は、STEP2の処理を実行する。このSTEP2では、段差部存在検知部11は、基準画像内に設定した段差部存在検知用領域R1の画像が、ある平面上の部分の投影画像であると仮定し、この仮定にできるだけ適合するような当該平面の空間的な位置及び姿勢を表す平面パラメータを、射影変換(詳しくは平面射影変換)を利用した手法によって決定する。
【0116】
本実施形態では、上記平面パラメータとして、平面の法線方向の単位ベクトル↑nを、基準画像を撮像するカメラ3R(以降、基準カメラ3ということがある)の光学中心Cから該平面までの距離dにより除算してなるベクトル↑m(≡↑n/d)を使用する。
【0117】
例えば、図7(a)又は図7(b)に二点鎖線で示す平面(図では仮想平面)の位置及び姿勢を表す平面パラメータは、図中に参照符号↑m1を付した矢印で示すように、該平面に垂直で、且つ大きさが1/dのベクトルとして表現される。
【0118】
以降、このように定義されるベクトル↑mを平面パラメータベクトルという。なお、本明細書では、“↑”は、ベクトル(縦ベクトル)を表現する記号として用いる。
【0119】
ここで、上記射影変換は、一般的に言えば、ステレオ画像を構成する2つの撮像画像に、階段の踏面等の平面部分を投影した場合に、一方の撮像画像中における平面部分の画像を、他方の撮像画像における当該平面部分の画像に変換するものである。その変換を行なう射影変換行列Pは、カメラ3R,3Lの2つの撮像画像のそれぞれの画像座標(各撮像画像における各画素の位置を表す二次元座標)を同次座標で表現した場合に、公知のように次式(1−1)により表される行列である。
【0120】
P=R+↑t・↑mT ……(1−1)
【0121】
式(1−1)におけるRは、カメラ3R,3Lのそれぞれのカメラ座標系の間の回転行列、↑tはそれらのカメラ座標系の間の並進移動ベクトル、↑mTは上記平面パラメータベクトル↑mを転置したベクトルである。
【0122】
この場合、カメラ3Rのカメラ座標系は、図7(a),(b)に示す如く、カメラ3Rに対して固定された座標系である。このカメラ座標系は、例えば、カメラ3Rの光学中心Cを原点として、光軸Lcの方向をz軸方向、カメラ3Rの撮像面の上下方向(縦方向)をy軸方向、該撮像面の横方向をx軸方向とする3軸座標系である。左カメラ3Lのカメラ座標系についても同様である。
【0123】
上記回転行列R及び並進移動ベクトル↑tは、カメラ3R,3Lの相互の相対的な位置関係及び姿勢関係に依存して定まるものであり、本実施形態ではあらかじめ特定されている。従って、射影変換行列Pは、平面パラメータベクトル↑mによって規定されるものである。
【0124】
補足すると、基準画像から参照画像への射影変換行列と、これの逆変換の射影変換行列(参照画像から基準画像への射影変換行列)とは、いずれも、式(1−1)の右辺の形で表現される。ただし、上記R,↑t,↑mの値は、それぞれの射影変換行列で異なる。
【0125】
具体的には、式(1−1)により表現される射影変換行列Pを基準画像及び参照画像のうちの一方から他方へのへの射影変換行列とした場合、その逆変換の射影変換行列(以降、これをP-1と表記する)は、次式(1−2)の但し書きの式(1−2a)〜(1−2d)により定義されるR’、↑t’、↑m’、↑d’を用いて、上記式(1−1)と同様の形の式(1−2)により表される。なお、RT、(↑m’)Tはそれぞれ、Rの転値行列、(↑m’)の転値ベクトルである。
【0126】
P-1=R'+↑t’・(↑m’)T ……(1−2)
但し、
R’≡RT ……(1−2a)
↑t’≡−RT・↑t ……(1−2b)
↑m’≡R・↑m/d’ ……(1−2c)
↑d’≡d・(1+↑mT・RT・↑t) ……(1−2d)
【0127】
以降の本実施形態の説明では、便宜上、基準画像から参照画像への射影変換行列を式(1−1)により表される射影変換行列Pとし、参照画像から基準画像への射影変換行列を式(1−2)により表される射影変換行列P-1とする。また、平面パラメータベクトル↑mは、便宜上、基準カメラ3(本実施形態ではカメラ3R)のカメラ座標系で見たベクトルとして表記されるものとする。
【0128】
基準画像における段差部存在検知用領域R1の画像が、ある平面上の部分(平面部分)の投影画像である場合には、その段差部存在検知用領域R1の画像と、該段差部存在検知用領域R1に対応する参照画像中の領域(詳しくは、基準画像の段差部存在検知用領域R1に投影されている実空間(実際の空間)の平面部分を、参照画像に投影してなる領域)の画像との間の関係は、上記射影変換によって対応づけられる。
【0129】
より詳しくは、基準画像の段差部存在検知用領域R1に対応する参照画像中の領域の画像を、当該平面部分の平面パラメータベクトル↑mに応じて上記式(1−2)に規定される射影変換行列P-1により射影変換した場合に得られる射影変換画像の画素値分布は、基準画像の段差部存在検知用領域R1の画像の画素値分布に一致もしくはほぼ一致することとなる。
【0130】
なお、上記射影変換画像の各画素位置(基準画像の画像座標系で見た位置)での画素値は、その画素位置を、当該平面部分の平面パラメータベクトル↑mに応じて上記式(1−1)により規定される射影変換行列Pによって変換してなる参照画像内の画素位置(参照画像の画像座標系で見た位置)の画素値に一致する。従って、上記射影変換画像は、換言すれば、基準画像の段差部存在検知用領域R1の全ての画素位置での画素値を、射影変換行列Pにより該段差部存在検知用領域R1の各画素位置に対応付けられる参照画像内の画素位置の画素値で置き換えてなる画像に一致する。
【0131】
STEP2の処理では、段差部存在検知部11は、基準画像内に設定した段差部存在検知用領域R1に平面部分が投影されていると仮定した場合の当該仮想的な平面(仮想平面)の平面パラメータベクトル↑mを、以下に説明する処理によって決定する。
【0132】
すなわち、段差部存在検知部11は、平面パラメータベクトル↑mの暫定値(候補値)を設定する処理、基準画像の段差部存在検知用領域R1の画像の画素値分布と、設定した↑mの暫定値により規定される射影変換によって、段差部存在検知用領域R1に対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数D1の値を算出する処理、並びに、この誤差関数D1の値に応じて平面パラメータベクトル↑mの暫定値を更新する処理を、既定の所定条件が満たされるまで繰り返す。
【0133】
そして、段差部存在検知部11は、上記所定条件が満たされたときの↑mの暫定値を、基準画像の段差部存在検知用領域R1に投影されている仮想平面の平面パラメータベクトル↑mとして決定する。以降、このように段差部存在検知部11が決定する平面パラメータベクトル↑mを段差部存在検知用平面パラメータベクトル↑m1という。
【0134】
ここで、基準画像の段差部存在検知用領域R1内の任意の画素位置Qでの画素値をI1(Q)、基準画像上の画素位置Qを平面パラメータベクトル↑mの暫定値により規定される前記式(1−1)の射影変換行列Pにより射影変換してなる参照画像上の画素位置をf(Q)、この画素位置f(Q)での参照画像の画素値をI2(f(Q))、段差部存在検知用領域R1内のトータルの画素数をN1とする。このとき、上記誤差関数D1は、例えば次式(2−1)により与えられる関数である。
【0135】
なお、撮像画像(基準画像又は参照画像)の各画素の画素値は、本実施形態では、該画素における撮像画像の輝度値である。ただし、撮像画像がカラー画像である場合には、撮像画像の色相又は彩度を画素値として使用してもよい。
【0136】
D1=(1/N1)・Σ(I1(Q)−I2(f(Q)))2 ……(2−1)
【0137】
式(2−1)のΣは、段差部存在検知用領域R1内の全ての画素についての総和を意味する。従って、式(2−1)により定義される誤差関数D1は、基準画像の段差部存在検知用領域R1での、画素値I1(Q)と、これに対応する参照画像の射影変換画像の画素値I2(f(Q))との差の二乗値の平均値を、段差部存在検知用領域R1における基準画像の画素値分布と、これに対応する参照画像の射影変換画像の画素値分布との誤差として表現する関数である。
【0138】
なお、式(2−1)により与えられる誤差関数Dの代わりに、例えば次式(2−2)により与えられる誤差関数D1を使用してもよい。
【0139】
D1=(1/N1)・Σ|(I1(Q)−I2(f(Q))| ……(2−2)
【0140】
この式(2−2)により表される誤差関数D1は、基準画像の段差部存在検知用領域R1での、I1(Q)とI2(f(Q))との差の絶対値の平均値を、段差部存在検知用領域R1における基準画像の画素値分布と、これに対応する参照画像の射影変換画像の画素値分布との誤差として表現する関数である。
【0141】
上記の如く算出される誤差関数D1の値は、↑mの暫定値に依存して変化する。そして、段差部存在検知用領域R1に投影されている部分が、実際の平面部分である場合には、↑mの暫定値が該平面部分を包含する平面の平面パラメータベクトルに一致する場合に、誤差関数D1の値は最小(ほぼゼロ)になる。
【0142】
従って、誤差関数D1の値が最小(もしくはほぼ最小)となるような↑mの暫定値が、段差部存在検知用領域R1に投影されている仮想的な平面部分を包含する仮想平面の位置及び姿勢を表す平面パラメータベクトルとしての適合度が高いものとなる。
【0143】
そこで、段差部存在検知部11は、この誤差関数D1に応じて↑mの暫定値を更新する処理では、公知の収束演算処理手法によって、この誤差関数D1の値を、その最小値により近づけ得るように↑mの暫定値を更新する。そして、段差部存在検知部11は、このような↑mの暫定値の更新を所定条件が成立するまで実行することで、最終的に、上記誤差関数D1の値を最小化(もしくはほぼ最小化)し得るような前記段差部検知用平面パラメータベクトル↑m1を決定する。
【0144】
この場合、上記所定条件は、その条件が成立する場合に、更新後の↑mが、誤差関数Dを最小化し得る値に一致もしくはほぼ一致するとみなし得るものとして、あらかじめ定められた条件である。該所定条件は、例えば、更新前の↑mに応じて算出される誤差関数D1の値と、更新後の↑mに応じて算出される誤差関数D1の値との差の絶対値が、あらかじめ定められた所定の閾値(“0”に近い閾値)以下となるという条件、あるいは、↑mの暫定値の更新回数が所定回数を超えるという条件等の条件である。
【0145】
なお、↑mの暫定値の初期値としては、例えば、ロボット1が現在接地している面(床面等)の平面パラメータベクトルを使用することができる。ただし、該初期値は、あらかじめ定めた固定値であってもよい。また、該初期値は、ロボット1の現在の接地面に対して傾いた平面の平面パラメータベクトルであってもよい。
【0146】
次いで、段差部存在検知部11は、STEP3の処理を実行する。このSTEP3では、段差部存在検知部11は、STEP2で決定した段差部存在検知用平面パラメータベクトル↑m1の向きに基づいて、カメラ3R,3Lの撮像領域内に段差部が存在しているか否かを判断する。
【0147】
ここで、カメラ3R,3Lの撮像領域に段差部が存在せず、該撮像領域がほぼ平坦な床面である場合には、STEP2において上記の如く決定された段差部存在検知用平面パラメータベクトル↑m1の向きは、ロボット1の現在の接地面の法線方向とほぼ同方向になる。
【0148】
一方、カメラ3R,3Lの撮像領域に階段等の段差部が存在している場合には、STEP2で決定した段差部存在検知用平面パラメータベクトル↑m1の向きは、ロボット1の現在の接地面の法線方向に対して比較的大きく傾いたものとなる。
【0149】
例えば図7(a)に示すように、カメラ3R,3Lの撮像領域に昇り階段50が存在する場合、あるいは、図7(b)に示すように、カメラ3R,3Lの撮像領域に降り階段50が存在する場合には、段差部存在検知部11が決定する段差部存在検知用平面パラメータベクトル↑m1の向きは、ピッチ方向(ロボット1の進行方向を横切る方向(ロボット1の左右方向)の軸周り方向)において、ロボット1の現在の接地面の法線方向に対して比較的大きく傾くものとなる。
【0150】
そこで、段差部存在検知部11は、STEP3では、決定した段差部存在検知用平面パラメータベクトル↑m1が、ロボット1の現在の接地面の法線方向に対してピッチ方向に所定角度以上、傾いているか否かによって、カメラ3R,3Lの撮像領域に階段等の段差部が存在しているか否かを判断する。
【0151】
具体的には、段差部存在検知部11は、ロボット1の現在の接地面の法線方向に対する↑m1の傾き角度(ピッチ方向での傾き角度)の大きさが、既定の所定角度以上である場合には、カメラ3R,3Lの撮像領域に階段等の段差部が存在していると判断する。また、段差部存在検知部11は、ロボット1の現在の接地面の法線方向に対する↑m1の傾き角度(ピッチ方向での傾き角度)が所定角度未満である場合には、カメラ3R,3Lの撮像領域に階段等の段差部が存在していないと判断する。
【0152】
なお、ロボット1の現在の接地面と、カメラ3R,3Lとの間の相対的な位置及び姿勢関係は、ロボット1の各関節の変位量(詳しくは、ロボット1の接地している脚とカメラ3R,3Lとの間に存在する各関節の変位量)に応じて定まる。従って、ロボット1の現在の接地面の法線方向に対する↑m1の傾き角度は、基準カメラ3(カメラ3R)のカメラ座標系で見た↑m1の値と、ロボット1の各関節の変位量の計測値(又は該変位量を追従させる目標値)とから算出することができる。
【0153】
そして、段差部存在検知部11は、STEP3の判断結果が肯定的である場合(段差部が存在していると判断した場合)には、さらにSTEP4の処理を実行する。このSTEP4では、カメラ3R,3Lの撮像領域に存在する段差部の種別(昇りの段差部であるか降りの段差部であるかの種別)を判断する。
【0154】
ここで、カメラ3R,3Lの撮像領域に、図7(a)に示す如く、昇りの段差部(図では昇り階段50)が存在する場合には、段差部存在検知部11が決定する段差部存在検知用平面パラメータベクトル↑m1は、ロボット1から見て、昇り勾配斜面の平面パラメータベクトルとなる。また、カメラ3R,3Lの撮像領域に、図7(b)に示す如く、降りの段差部(図では降り階段50)が存在する場合には、段差部存在検知部11が決定する段差部存在検知用平面パラメータベクトル↑m1は、ロボット1から見て、降り勾配斜面の平面パラメータベクトルとなる。
【0155】
そこで、STEP4では、段差部存在検知部11は、↑m1が、ロボット1の現在の接地面の法線方向に対して、ピッチ方向で正負のいずれの向きに傾いているかによって、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する。
【0156】
以上が、段差部存在検知部11の処理の詳細である。
【0157】
段差部存在検知部11により、カメラ3R,3Lの撮像領域に段差部が存在することが検知された場合には、演算処理装置10は、次に、平面投影領域抽出部12の処理を実行する。
【0158】
この平面投影領域抽出部12は、カメラ3R,3Lによって取得された撮像画像(ステレオ画像)を用いて、図8のフローチャートに示す処理を実行する。
【0159】
平面投影領域抽出部12は、まず、STEP11の処理を実行する。このSTEP11では、平面投影領域抽出部12は、与えられたステレオ画像のうちの基準画像内の複数の(N2個の)位置(u1,v(i))(i=1,2,…,N2)に、微小な面積の領域である局所領域R2(i)を設定する。
【0160】
各局所領域R2(i)は、図9に示す如く例えば方形状の領域である。そして、各局所領域R2(i)のサイズ(縦方向及び横方向の幅)は、基準画像に段差部の画像が投影されている場合に、該段差部の各段の踏面内に収まり得るようなサイズにあらかじめ設定されている。
【0161】
なお、図9では、前記投光器4から出力される補助光SLによる斜行輝線SLa,SLbの図示は省略している。このことは、後述する図13、図19においても同様である。
【0162】
補足すると、各局所領域R2(i)の形状は、方形状でなくともよく、例えば、平行四辺形や台形等の形状であってもよい。
【0163】
各局所領域R2(i)の配置位置(u1,v(i))は、基準画像の横方向をu軸、縦方向(上下方向)をv軸とする画像座標系で表した、各局所領域R2(i)の代表点の位置である。その代表点は、図9に示す例では、各局所領域R2(i)の左上隅の点である。ただし、該代表点は、各局所領域R2(i)の他の点、例えば各局所領域R2(i)の中央点、右上隅の点、左下隅の点、右下隅の点等であってもよい。
【0164】
本実施形態では、各局所領域R2(i)の配置位置(u1,v(i))のうち、基準画像の横方向(u軸方向)での位置u1は互いに同一の位置である。その位置u1は、基準画像におけるロボット1の通過予定領域上に各局所領域R2(i)が配置されるように設定される。例えば、位置u1は、ロボット1の通過予定領域の幅方向中央付近の位置に設定される。
【0165】
また、各局所領域R2(i)の配置位置(u1,v(i))のうち、基準画像の縦方向(v軸方向)の位置v(i)(i=1,2,…,N2)は、基準画像の下部から上部まで、所定の微小間隔(例えば1画素分の間隔)を存して並ぶようにあらかじめ設定された位置である。
【0166】
そして、STEP11では、平面投影領域抽出部12は、上記の如く配置位置が決定された各局所領域R2(i)のそれぞれに対して、基準画像の各局所領域R2(i)に投影されている部分が平面部分であると仮定した場合の当該平面部分の平面パラメータベクトル↑mを算出する。以降の説明では、各局所領域R2(i)(i=1,2,…,N2)のそれぞれに対応して算出される平面パラメータベクトル↑mを参照符号↑m_r2(i)を用いて表記する。
【0167】
この場合、各局所領域R2(i)(i=1,2,…,N2)のそれぞれに対応する平面パラメータベクトル↑m_r2(i)の算出は、前記段差部存在検知部11によるSTEP2の処理と同様の処理によって行なわれる。この場合、↑m_r2(i)の算出処理は、前記段差部存在検知用領域R1の代わりに、局所領域R2(i)を使用する(前記誤差関数D1の値を算出するための領域としてR2(i)を使用する)こと以外は、STEP2の処理と同じでよい。
【0168】
次いで、平面投影領域抽出部12は、STEP12の処理を実行する。このSTEP12では、平面投影領域抽出部12は、上記の如く算出した各局所領域R2(i)(i=1,2,…,N2)に対応する平面パラメータベクトル↑m_r2(i)から、この↑m_r2(i)により位置及び姿勢が規定される平面(R2(i)に投影されているとみなす仮想的な平面)の法線方向の単位ベクトルである単位法線ベクトル↑n_r2(i)を算出する。
【0169】
この単位法線ベクトル↑n_r2(i)は、平面パラメータベクトル↑m_r2(i)と同じ向きを有する単位長のベクトルであるから、↑m_r2(i)を、その大きさ(=|↑m_r2(i)|)により除算することによって算出される。
【0170】
次いで、平面投影領域抽出部12は、STEP13の処理を実行する。このSTEP13では、平面投影領域抽出部12は、各局所領域R2(i)における単位法線ベクトル↑n_r2(i)と、現在のロボット座標系のZ軸方向の単位ベクトル↑nzとの内積(スカラー積)を算出する。
【0171】
上記ロボット座標系というのは、図7(a)又は図7(b)に示す如く、ロボット1の現在の接地面に対して固定された座標系であって、該接地面の法線方向をZ軸方向、ロボット1の進行方向をX軸方向、これらのZ軸及びX軸に直交する方向(ロボット1の左右方向)をY軸方向とする座標系である。従って、現在のロボット座標系のZ軸方向の単位ベクトル↑nzというのは、ロボット1の現在の接地面の法線方向の単位ベクトルを意味する。
【0172】
そして、この単位ベクトル↑nzと、単位法線ベクトル↑n_r2(i)との内積を算出する場合、基準カメラ3のカメラ座標系で見たベクトルとしての単位法線ベクトル↑n_r2(i)が、ロボット1の各関節の変位量の計測値(又は目標値)に応じて、現在のロボット座標系で見たベクトルに座標変換される。そして、この座標変換後の単位法線ベクトル↑n_r2(i)と、ロボット1の現在の接地面の法線方向(現在のロボット座標系のZ軸方向)の単位ベクトル↑nzとの内積が算出される。
【0173】
なお、カメラ座標系で見た単位法線ベクトル↑n_r2(i)と、ロボット1の現在の接地面の法線方向の単位ベクトル↑nzをロボット座標系からカメラ座標系に座標変換したベクトルとの内積を算出するようにしてもよい。
【0174】
これにより、図10に例示する如く、基準画像の縦方向での各局所領域R2(i)の位置v(i)と、上記内積の値との間の関係を表すデータが得られることとなる。
【0175】
ここで、カメラ3R,3Lの撮像領域に段差部が存在する場合、その段差部の各踏面は、一般に、ロボット1の現在の接地面とほぼ平行な平面部分となる。従って、基準画像における段差部の踏面の投影領域内に含まれる各局所領域R2(i)(例えば、図9に示すR2(1),R2(ib),R2(N2))に対応して上記の如く算出される内積の値は、“1”またはそれに近い値となる。
【0176】
一方、段差部の踏面の投影領域に含まれない各局所領域R2(i)、又は、段差部の踏面の投影領域が一部しか含まれないような各局所領域R2(i)(例えば、図9に示すR2(ia))に対応する内積の値は、“1”から乖離した値となる。
【0177】
従って、上記内積の値は、各局所領域R2(i)に投影されている部分が、段差部の踏面の部分であるか否かを示す指標となるものである。
【0178】
次いで、平面投影領域抽出部12は、STEP14の処理を実行する。このSTEP14では、平面投影領域抽出部12は、N2個の局所領域R2(i)(i=1,2,…,N2)のうち、STEP13で算出した内積の値が、あらかじめ定めれた所定の閾値Th2以上となる各局所領域R2(i)について、該局所領域R2(i)に投影されている部分の、ロボット座標系で見たZ軸座標位置(すなわち、ロボット1の現在の接地面に対する当該投影部分の高さ)を、該局所領域R2(i)に対応する平面パラメータベクトル↑m_r2(i)を基に算出する。
【0179】
ここで、上記所定の閾値Th2は、“1”よりも若干小さい値、例えば0.999である。従って、内積の値が、この閾値Th2以上となる各局所領域R2(i)は、それに投影されている部分が、ロボット1の現在の接地面とほぼ平行になっているような部分である。
【0180】
そして、閾値Th2以上となる各局所領域R2(i)に対応する平面パラメータベクトル↑m_r2(i)により規定される平面と、ロボット1の現在の接地面を含む平面との間の間隔(詳しくは、ロボット座標系のZ軸方向での間隔)を算出することによって、該局所領域R2(i)に投影されている部分のZ軸座標位置(ロボット1の現在の接地面に対する高さ)が決定される。
【0181】
次いで、平面投影領域抽出部12は、STEP15の処理を実行する。このSTEP15では、平面投影領域抽出部12は、前記STEP14で得られたデータを基に、基準画像における段差部の踏面に対応する平面投影領域を抽出(特定)する。
【0182】
具体的には、平面投影領域抽出部12は、内積の値が前記閾値Th2以上となるv軸方向の区間であって、その区間内の各位置の局所領域R2(i)に投影されている部分の高さ(ロボット1の接地面からの高さ)がほぼ一定となる(各投影部分の相互の高さの差の大きさがあらかじめ定めた所定の閾値以下となる)区間を決定する。なお、各投影部分の高さの相互の差の大きさ(絶対値)に対する閾値は、“0”に近い微小値、例えば10[mm]である。
【0183】
図10に示すΔv1、Δv2、Δv3は、このようにして決定される区間の例を示すものである。このようにして決定される各区間は、STEP11で設定したN2個の局所領域R2(i)(i=1,2,…,N2)のうち、ロボット1の現在の接地面とほぼ平行な単一の平面部分(段差部の各踏面に相当する平面部分)の投影領域となっている局所領域R2(i)の代表点の位置(v軸方向の位置)の範囲に相当するものである。以降、このようにして決定される各区間を同一平面投影区間という。
【0184】
そして、平面投影領域抽出部12は、上記の如く決定した各同一平面投影区間毎に、それに対応する平面投影領域を決定する。この場合、本実施形態では、各平面投影領域は、基準画像の横方向(u軸方向)に局所領域R2(i)(i=1,2,…,N2)と同じ幅を有し、且つ、その横方向の範囲(各平面投影領域の左端と右端との間の範囲)が局所領域R2(i)の横方向の範囲に一致するようにして、基準画像に配置される方形状の領域である。
【0185】
そして、基準画像の縦方向(v軸方向)での各平面投影領域の範囲(各平面投影領域の上端と下端との間の範囲)は、該平面投影領域に対応する同一平面投影区間内に代表点の位置を有する全ての局所領域R2(i)を合成した領域の縦方向の範囲と一致するように決定される。
【0186】
例えば、基準画像のv軸方向の位置がv(ic)からv(ic+n)までの区間が、ある1つの同一平面投影区間であるとしたとき、その同一平面投影区間に対応する平面投影領域の縦方向(v軸方向)の範囲は、該同一平面投影区間内に代表点の位置を有するn+1個の局所領域R2(ic)〜R2(ic+n)のうち、最も下側の局所領域R2(ic)の下端位置と、最も上側の局所領域R2(ic+n)の上端位置との間の範囲として決定される。
【0187】
以上のSTEP15の処理によって、段差部の各踏面に対応する平面投影領域が、基準画像から抽出されることとなる。例えば、図9に破線枠で示す方形状の領域AR2が、平面投影領域として基準画像から抽出される。
【0188】
次いで、平面投影領域抽出部12は、STEP16の処理を実行する。このSTEP16では、平面投影領域抽出部12は、STEP15の処理によって2つ以上の平面投影領域AR2が抽出されたか否かを判断する。そして、この判断結果が否定的である場合には、基準画像に段差部が写っていない可能性があるので、図8の処理(今回の演算処理周期での処理)を終了する。
【0189】
一方、STEP16の判断結果が肯定的である場合には、平面投影領域抽出部12は、STEP17の処理を実行した後、図8の処理を終了する。このSTEP17では、平面投影領域抽出部12は、各平面投影領域AR2に投影されている平面部分を包含する平面の平面パラメータベクトル↑m2を決定する。
【0190】
この場合、平面投影領域抽出部12は、例えば、前記局所領域R2(i)(i=1,2,…,N2)のうち、各平面投影領域AR2に含まれる局所領域R2(i)のそれぞれに対応する平面パラメータベクトル↑m_r2(i)のそれぞれの方位角(基準カメラ3のカメラ座標系で見た方位角)の平均の方位角を有する単位ベクトルを、該平面投影領域AR2に対応する平面の単位法線ベクトル↑n2として決定する。
【0191】
さらに、平面投影領域抽出部12は、各平面投影領域AR2に含まれる局所領域R2(i)のそれぞれに対応して、前記STEP11で推定した平面パラメータベクトル↑m_r2(i)の大きさ(絶対値)の逆数値(=1/|↑m(i)|)の平均値を、該平面投影領域AR2に対応する平面の、基準カメラ3の光学中心Cからの距離d2として決定する。
【0192】
そして、各平面投影領域AR2に対応する平面の単位法線ベクトル↑n2を距離d2で除算してなるベクトル(=↑n2/d2)を該平面投影領域AR2に対応する平面パラメータベクトル↑m2として決定する。
【0193】
なお、各局所領域R2(i)に対応する平面パラメータベクトル↑m_r2(i)を推定する場合と同様の処理(射影変換を利用した収束演算処理)によって、各平面投影領域AR2に対応する平面パラメータベクトル↑m2を改めて推定するようにしてもよい。
【0194】
以上が平面投影領域抽出部12の処理の詳細である。
【0195】
補足すると、本実施形態では、STEP15の処理で前記同一平面投影区間を決定する場合に、各局所領域R2(i)(i=1,2,…,N2)毎の前記内積の値に加えて、各局所領域R2(i)に投影されている部分の高さを参照するようにしたが、内積の値だけから、同一平面投影区間を決定するようにしてもよい。
【0196】
例えば、内積の値が、基準画像の縦方向(v軸方向)で連続的に所定の閾値Th2以上となる区間(その区間内に代表点の位置を有して、縦方向に並ぶ全ての局所領域R2(i)のそれぞれに対応する内積の値が閾値Th2以上となる区間)を同一平面投影区間として決定してもよい。このようにした場合には、STEP14の処理は不要である。
【0197】
また、カメラ3R,3Lの撮像領域に存在する段差部の踏面に対応する平面投影領域だけでなく、段鼻の先端面又は蹴り込み面のように、踏面に対して起立する平面部分に対応する平面投影領域を抽出するようにしてもよい。
【0198】
この場合、例えば各局所領域R2(i)(i=1,2,…,N2)毎の平面パラメータベクトル↑m_r2(i)(又は単位法線ベクトル↑n_r2(i))の向きに着目し、その向きが連続的にほぼ一定に保たれる(ロボット1の現在の接地面の法線方向と同方向の向きでなくてもよい)ような区間を同一平面投影区間として決定する。そして、その決定した各同一平面投影区間に応じて上記と同様に平面投影領域を決定する。これにより、段差部の踏面に対応する平面投影領域だけでなく、段鼻の先端面や段差部の蹴り込み面の如く、踏面に対して起立する平面部分に対応する平面投影領域を抽出することもできる。
【0199】
以上説明した平面投影領域抽出部12によって、段差部の踏面に対応する2つ以上の平面投影領域AR2が抽出された場合には、演算処理装置10は、次に、エッジ投影線推定処理部13の処理を実行する。
【0200】
このエッジ投影線推定処理部13は、カメラ3R,3Lによって取得された撮像画像(ステレオ画像)と、平面投影領域抽出部12で決定された各平面投影領域AR2に対応する平面パラメータベクトル↑m2とを用いて、図11及び図12のフローチャートに示す処理を実行することで、上段側踏面の先端側エッジを基準画像に投影してなる実エッジ投影線を推定したものとしての推定エッジ投影線を特定する。
【0201】
なお、基準画像における実エッジ投影線を推定するということは、基準画像における実エッジ投影線の位置及び方向(又はこれらを規定するパラメータ)を決定することを意味する。
【0202】
エッジ投影線推定処理部13は、まず、STEP21の処理を実行する。このSTEP21では、エッジ投影線推定処理部13は、図5のSTEP4の処理によって前記段差部存在検知部11が特定した段差部(カメラ3R,3Lの撮像領域に存在する段差部)の種別が、ロボット1から見て、昇りの段差部であるか降りの段差部であるかを確認する。
【0203】
エッジ投影線推定処理部13は、カメラ3R,3Lの撮像領域に存在する段差部が、昇りの段差部である場合には、図11に示すSTEP22〜36の処理を実行し、降りの段差部である場合には、図12に示すSTEP37〜51の処理を実行する。
【0204】
段差部が昇りの段差部である場合には、エッジ投影線推定処理部13は、まず、STEP22において、基準画像内に、実エッジ投影線の推定用の画像処理領域であるエッジ推定用領域R3uを設定する。
【0205】
ここで、以降の説明では、説明の便宜上、カメラ3R,3Lの撮像領域に存在する昇りの段差部が、一例として、図1(a)に示した如き昇り階段50であるとする。そして、この昇り階段50が、図13に示す如く基準画像に写っているものとする。
【0206】
さらに、この昇り階段50の任意の1つの下段側踏面51(例えばロボット1から見て最も手前側の下段側踏面)の一段上側の上段側踏面51の先端側エッジ52を基準画像に投影してなる実エッジ投影線L52rを推定する処理をエッジ投影線推定処理部13により実行するものとする。
【0207】
そして、推定しようとする実エッジ投影線L52rに対応する先端側エッジ52を先端に有する上段側踏面51と、その一段下側の下段側踏面51とを、それぞれ参照符号51p,51qを使用して表記する。
【0208】
また、前記平面投影領域抽出部12によって、上段側踏面51pに対応して抽出された平面投影領域AR2を、以降、参照符号AR2pを使用して上段側踏面投影領域AR2pと称する。そして、この上段側踏面投影領域AR2pに対応して決定された平面パラメータベクトル↑m2を参照符号↑m2pを使用して表記する。
【0209】
また、前記平面投影領域抽出部12によって、下段側踏面51qに対応して抽出された平面投影領域AR2を、以降、参照符号AR2qを使用して下段側踏面投影領域AR2qと称する。そして、この下段側踏面投影領域AR2qに対応して決定された平面パラメータベクトル↑m2を参照符号↑m2qを使用して表記する。
【0210】
STEP22において基準画像に設定するエッジ推定用領域R3uは、本実施形態では、図13に示す如く設定される方形状の領域である。具体的には、エッジ推定用領域R3uは、その下端の位置が下段側踏面投影領域AR2qの縦方向の幅内の位置(例えば下段側踏面投影領域AR2qの上端から既定の所定量だけ下側の位置)となり、且つ、R3uの上端の位置が上段側踏面投影領域AR2pの縦方向の幅内の位置(例えば上段側踏面投影領域AR2pの下端から所定量だけ上側の位置)になるように配置される。この場合、エッジ推定用領域R3uの縦方向の幅は、下段側踏面投影領域AR2qと上段側踏面投影領域AR2pとの間の縦方向の間隔に応じて決定される。
【0211】
また、エッジ推定用領域R3uは、その横方向の範囲(左端と右端との間の範囲)が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲に一致するように配置される。この場合、エッジ推定用領域R3uの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅と同じ幅(既定の幅)に決定される。
【0212】
なお、エッジ推定用領域R3uの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅よりも若干小さくてもよい。その場合は、エッジ推定用領域R3uは、その横方向の範囲が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲内に収まるように配置すればよい。
【0213】
以上のSTEP22の処理によって、エッジ推定用領域R3uは、その内部に(上端の辺と下端の辺との間)に推定しようとする単一の実エッジ投影線L52rが存在し、且つ、該エッジ推定用領域R3uの上端の辺及び下端の辺がそれぞれ、上段側踏面投影領域AR2p、下段側踏面投影領域AR2qに含まれるようにして、基準画像内に設定されることとなる。
【0214】
エッジ投影線推定処理部13は、次にSTEP23の処理を実行する。このSTEP23では、エッジ投影線推定処理部13は、実エッジ投影線L52rが基準画像の横方向(u軸方向)に延在するものと仮定して、図14に示す如く、基準画像の縦方向(v軸方向)における実エッジ投影線L52rの位置の候補としての複数(N3a個)の候補位置v(k1)(k1=1,2,…,N3a)を決定する。
【0215】
この場合、N3a個の候補位置v(k1)(k1=1,2,…,N3a)は、エッジ推定用領域R3uの縦方向の範囲(上端と下端との間の範囲)内で、あらかじめ定められた所定の刻み間隔ずつ(例えば1画素分の間隔ずつ)、基準画像の縦方向に異なる位置とされる。また、これらの候補位置v(k1)のうち、最も上側の候補位置としての第1番目の候補位置v(1)は、エッジ推定用領域R3uの上端から所定間隔(例えば1画素分の間隔)だけ下側の位置に決定される。
【0216】
さらに、最も下側の候補位置としての第N3a番目の候補位置v(N3a)は、本実施形態では、この候補位置v(N3a)に応じて後述する射影変換領域R3u3の上端位置を設定したときに、該射影変換領域R3u3の上端位置が、エッジ推定用領域R3uの下端から、該エッジ推定用領域R3u3の縦方向の所定の最小幅(例えば1画素分の幅)だけ上側の位置になるように、該エッジ推定用領域R3u3の縦方向の最小幅での上端位置に応じて決定される。
【0217】
次に、エッジ投影線推定処理部13は、STEP24の処理を実行する。この処理では、各候補位置v(k1)に対応する処理を順番に実行するために、k1の初期値として“1”が設定される。
【0218】
次いで、エッジ投影線推定処理部13は、STEP25の処理を実行する。このSTEP25では、エッジ投影線推定処理部13は、基準画像の縦方向での実エッジ投影線L52rの位置が第k1番目の候補位置v(k1)(k1は現在設定されている値)に一致するものと仮定して、エッジ推定用領域R3u内に、図14に例示する如く、3つの射影変換対象領域R3u1、R3u2、R3u3を設定する。
【0219】
射影変換対象領域R3u1〜R3u3のうち、射影変換対象領域R3u1,R3u2は、第k1番目の候補位置v(k1)で横方向に延在する仮想的なエッジ投影線L52(k1)を境界線として上下に隣接するように設定される領域である。この場合、上側の射影変換対象領域R3u1は、その上端側の辺と下端側の辺とがそれぞれエッジ推定用領域R3uの上端側の辺、仮想的なエッジ投影線L52(k1)に一致し、且つ、横方向の範囲が、エッジ推定用領域R3uの横方向の範囲と一致する領域である。
【0220】
従って、射影変換対象領域R3u1は、その縦方向の幅と下端側の辺の位置(縦方向での位置)とが、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3u1は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、上段側踏面投影領域AR2pに含まれる領域である。
【0221】
また、下側の射影変換対象領域R3u2は、その上端側の辺が仮想的なエッジ投影線L52(k1)に一致すると共に、下端側の辺が仮想的なエッジ投影線L52(k1)とあらかじめ定められた所定間隔Δvu2(例えば15画素分の間隔)を有して該エッジ投影線L52(k1)に平行な線に一致し、且つ、横方向の範囲が、エッジ推定用領域R3uの横方向の範囲と一致する領域である。
【0222】
従って、射影変換対象領域R3u2は、その上端側の辺の位置と下端側の辺の位置とが、それらの間の間隔(すなわちR3u2の縦方向の幅)を一定の間隔Δvu2に保持しつつ、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3u2は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、段鼻の先端面や蹴り込み面のように、昇り階段50の上段側踏面51pの先端側で該上段側踏面51pに対して起立した姿勢で形成される平面部分が投影される領域である。
【0223】
また、射影変換対象領域R3u3は、上記射影変換領域R3u2よりも下側で、エッジ推定用領域R3uの下部に設定される方形状の領域である。この射影変換領域R3u3は、その下端側の辺がエッジ推定用領域R3uの下端側の辺に一致し、且つ、横方向の範囲が、エッジ推定用領域R3uの横方向の範囲と一致する領域である。
【0224】
そして、射影変換対象領域R3u3の上端側の辺の位置は、本実施形態では、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて次のように設定される。
【0225】
すなわち、エッジ投影線推定処理部13は、まず、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pを用いて、仮想的なエッジ投影線L52(k1)に対応する上段側踏面51pの先端側エッジ線(先端側エッジ52を包含する空間的な直線)を求める。この先端側エッジ線は、基準カメラ3の光学中心Cと基準画像の撮像面上の仮想的なエッジ投影線L52(k1)とを包含する平面と、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pにより規定される平面(上段側踏面51pを包含する平面)との交線として算出される。
【0226】
そして、エッジ投影線推定処理部13は、上記の如く求めた先端側エッジ線を含み、且つ、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pにより規定される平面に対して直交する平面(あるいは↑m2pにより規定される平面に対して、90度に近い既定の角度で交差する平面)を決定する。この平面は、階段の段鼻の先端面又は蹴り込み面に相当する仮想的な平面(以降、起立平面ということがある)である。
【0227】
さらに、エッジ投影線推定処理部13は、上記の如く決定した起立平面と、下段側踏面投影領域AR2qに対応する平面パラメータベクトル↑m2qにより規定される平面との交線を、基準画像に投影してなる線に、射影変換対象領域R3u3の上端側の辺が合致するように該上端側の辺の位置を設定する。
【0228】
従って、射影変換対象領域R3u3の上端側の辺は、下段側踏面51qの基端側エッジ53が、上段側踏面51pの先端側エッジ52のほぼ直下に存在するものと仮定した場合における該基端側エッジ53を基準画像に投影してなる線に相当するものとなる。この射影変換対象領域R3u3は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、下段側踏面投影領域AR2qに含まれる領域である。
【0229】
なお、前記平面投影領域抽出部12の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトルにより規定される平面を上記起立平面として使用して、射影変換対象領域R3u3の上端の辺の位置を決定するようにしてもよい。
【0230】
補足すると、段差部の上段側踏面の先端側で該上段側踏面に対して起立する姿勢となる平面部分は、蹴り込みの箇所に板部材が設けられていないような段差部にあっては、段鼻の先端面に相当するものとなる。従って、該平面部分の上下方向の幅は、上段側踏面と下段側踏面との間隔に比して小さなものとなる。さらに、この場合には、段差部の蹴り込みの箇所には、平面部分が存在しないこととなる。
【0231】
そこで、本実施形態では、前記射影変換対象領域R3u2の縦方向の幅Δvu2を比較的小さな幅とし、射影変換対象領域R3u2の下端の辺と、その下側の射影変換対象領域R3u3の上端の辺との間に間隔を設けるようにしている。
【0232】
なお、基準画像に段鼻の先端面に相当する部分が投影されている場合、該基準画像における段鼻の画像の縦方向の幅は、基準カメラ3の光学中心Cから段鼻までの距離や、基準カメラ3の光軸Lcに対する段鼻の方位角(基準画像の縦方向での方位角)に応じて変化する。従って、前記射影変換対象領域R3u2の縦方向の幅Δvu2は、基準カメラ3の光学中心Cから段鼻の近辺に存在する部分(例えば上段側踏面投影領域AR2pの下端に対応する上段側踏面51上の点)までの距離や、基準カメラ3の光軸Lcに対する該部分の方位角等に応じて可変的に設定するようにしてもよい。また、カメラ3R,3Lの解像度や撮像画像の拡縮の倍率等に応じて射影変換対象領域R3u2の縦方向の幅Δvu2を異ならせるようにしてもよい。
【0233】
以上の如くSTEP25の処理を実行した後、次に、エッジ投影線推定処理部13は、STEP26の処理を実行する。このSTEP26では、エッジ推定用領域R3uに設定した3つの射影変換対象領域R3u1,R3u2,R3u3のそれぞれが平面部分の投影領域であると仮定して、射影変換対象領域R3u1,R3u2,R3u3のそれぞれに対応する平面部分を包含する平面についての平面パラメータベクトル↑mu1,↑mu2,↑mu3を決定する。
【0234】
この場合、射影変換対象領域R3u1,R3u3にそれぞれ対応する平面パラメータベクトル↑mu1,↑mu3は、それぞれ、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2p、下段側踏面投影領域AR2qに対応する平面パラメータベクトル↑m2qに一致するベクトルに決定される。
【0235】
また、射影変換対象領域R3u2に対応する平面パラメータベクトル↑mu2は、前記STEP25の処理に関して説明した起立平面(仮想的なエッジ投影線L52(k1)に対応する先端側エッジ線を含み、上段側踏面51pを包含する平面に対して直交又はほぼ直交する平面)の平面パラメータベクトル↑mに一致するベクトルに決定される。
【0236】
なお、前記平面投影領域抽出部12の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトル↑mに、↑mu2を一致させるようにしてもよい。
【0237】
次いで、エッジ投影線推定処理部13は、STEP27の処理を実行する。このSTEP27では、エッジ投影線推定処理部13は、各射影変換対象領域R3u1,R3u2,R3u3について、各射影変換対象領域R3u1,R3u2,R3u3の画像(基準画像)と、これに対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像との誤差を表す誤差画像を作成する。
【0238】
各射影変換対象領域R3u1,R3u2,R3u3に対応する誤差画像は次のように作成される。例えば射影変換対象領域R3u1に対応す誤差画像の作成処理を代表的に説明すると、基準画像の射影変換対象領域R3u1内の任意の位置Qでの画素値をI1(Q)、基準画像内の位置Qを射影変換対象領域R3u1に対応する平面パラメータベクトル↑mu1により規定される前記式(1−1)の射影変換行列Pにより射影変換してなる参照画像内の位置をf(Q)、参照画像内の位置f(Q)での画素値をI2(f(Q))とおく。
【0239】
このとき、射影変換対象領域R3u1に対応する誤差画像は、射影変換対象領域R3u1の各位置Qの画素値を、I1(Q)とI2(f(Q))との差の二乗値(=(I1(Q)−I2(f(Q)))2)又は絶対値(=|I1(Q)−I2(f(Q))|)に置き換えた画像として作成される。
【0240】
射影変換対象領域R3u2,R3u3のそれぞれに対応する誤差画像についても同様である。ただし、この場合、参照画像中の位置f(Q)を求めるための射影変換に使用する平面パラメータベクトルは、↑mu1の代わりに、それぞれの領域に対応して決定された平面パラメータベクトル↑mu2,↑mu3が使用される。
【0241】
次いで、エッジ投影線推定処理部13は、STEP28,29の処理を実行する。STEP28では、エッジ投影線推定処理部13は、処理に用いる候補位置v(k1)の番数k1を“1”だけ増加させる。そして、STEP29では、エッジ投影線推定処理部13は、増加後のk1の値が候補位置v(k1)の総個数であるN3aを超えたか否かを判断する。
【0242】
このSTEP29の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k1番目の候補位置v(k1)を用いて、前記STEP25〜27の処理を実行する。
【0243】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれに対応して、上記の誤差画像が作成される。
【0244】
STEP29の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP30の処理を実行する。このSTEP30では、エッジ投影線推定処理部13は、図15に示す如く、エッジ推定用領域R3u内に、縦方向に細長い複数(N3b個)の演算処理領域R3min(k2)(k2=1,2,…,N3b)を設定する。各演算処理領域R3min(k2)は、基準画像の横方向の幅があらかじめ定められた所定の微小幅(例えば1画素分の幅)を有し、且つ、縦方向の範囲が、エッジ推定用領域R3uの縦方向の範囲に一致する細長方形状(短冊状)の領域である。
【0245】
そして、これらの演算処理領域R3min(k2)は、本実施形態では、基準画像の横方向に順番に隣接して並ぶように配置され、その全体の領域がエッジ推定用領域R3uに合致するものとされる。
【0246】
なお、演算処理領域R3min(k2)は、隣合う演算処理領域の間に若干の間隔を有するように設定してもよい。また、各演算処理領域R3min(k2)の横方向の幅は1画素分の幅よりも多少大きい幅(例えば2画素分の幅、3画素分の幅等)であってもよい。
【0247】
補足すると、演算処理領域R3min(k2)(k2=1,2,…,N3b)の全てを合成したものが、エッジ推定用領域R3uに合致するので、前記STEP23において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対してN3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理であると言える。
【0248】
また、前記STEP25において、v(k1)に応じて射影変換対象領域R3u1,R3u2,R3u3を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対して射影変換対象領域R3u1,R3u2,R3u3を設定する処理であると言える。
【0249】
演算処理領域R3min(k2)を設定するSTEP30の処理は、STEP23やSTEP25よりも後で行なう必要はなく、例えば、STPE22において、演算処理領域R3min(k2)の設定処理を行ってもよい。
【0250】
次いで、エッジ投影線推定処理部13は、STEP31の処理を実行する。この処理では、各演算処理領域R3min(k2)に対応する処理を順番に実行するために、k2の初期値として“1”が設定される。
【0251】
次いで、エッジ投影線推定処理部13は、STEP32の処理を実行する。このSTEP32では、エッジ投影線推定処理部13は、第k2番目(k2は現在設定されている番数)の演算処理領域R3min(k2)におけるN3a個の前記候補位置v(k1)(k1=1,2,…,N3a)のそれぞれについて、実エッジ投影線L52rの位置(縦方向での位置)に対する各候補位置v(k1)の適合度を表す評価関数Eu(k2)の値を算出する。
【0252】
以降、第k2番目の演算処理領域R3min(k2)における第k1番目の候補位置v(k1)の適合度を表す評価関数Eu(k2)をEu(k2)_k1と表記する。
【0253】
演算処理領域R3min(k2)における各候補位置v(k1)に対応する評価関数Eu(k2)_k1の値は、v(k1)に応じて設定された射影変換対象領域R3u1(以降、これをR3u1(k1)と表記する)に対応する前記誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Du1(k2)_k1の値と、v(k1)に応じて設定された射影変換対象領域R3u2(以降、これをR3u2(k1)と表記する)に対応する誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Du2(k2)_k1の値と、v(k1)に応じて設定された射影変換対象領域R3u3(以降、これをR3u3(k1)と表記する)に対応する誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Du3(k2)_k1の値とを線形結合(本実施形態では加算)してなる値として算出される。
【0254】
より具体的には、v(k1)に応じて設定された射影変換対象領域R3u1,R3u2,R3u3のそれぞれに対応する誤差画像の各画素位置Qでの画素値をそれぞれI1uerr(Q),I2uerr(Q),I3uerr(Q)とする。また、演算処理領域R3min(k2)における射影変換対象領域R3u1(k1),R3u2(k1),R3u3(k1)のそれぞれのトータルの画素数をNu1(k2)_k1,Nu2(k2)_k1,Nu3(k3)_k1とする。このとき、誤差関数Du1(k2)_k1,Du2(k2)_k1,Du3(k2)_k1はそれぞれ、次式(3−1),(3−2),(3−3)により算出される。そして、これらのDu1(k2)_k1,Du2(k2)_k1,Du3(k2)_k1の値から次式(4)によって、評価関数Eu(k2)_k1が算出される。
【0255】
Du1(k2)_k1=(1/Nu1(k2)_k1)・ΣI1uerr(Q) ……(3−1)
Du2(k2)_k1=(1/Nu2(k2)_k1)・ΣI2uerr(Q) ……(3−2)
Du3(k2)_k1=(1/Nu3(k2)_k1)・ΣI3uerr(Q) ……(3−3)
Eu(k2)_k1=Du1(k2)_k1+Du2(k2)_k1+Du3(k2)_k1 ……(4)
【0256】
この場合、式(3−1)におけるΣI1uerr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u1(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u1(k1)との重なり領域)での誤差画像の画素値I1uerr(Q)の総和である。
【0257】
また、式(3−2)におけるΣI2uerr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u2(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u2(k1)との重なり領域)での誤差画像の画素値I2uerr(Q)の総和である。
【0258】
また、式(3−3)におけるΣI3uerr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u3(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u3(k1)との重なり領域)での誤差画像の画素値I3uerr(Q)の総和である。
【0259】
STEP32では、以上のようにして、第k2番目の演算処理領域R3min(k2)において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれの適合度(実エッジ投影線L52rの位置としての妥当性の度合い)をそれぞれ表すN3a個の評価関数Eu(k2)_k1(k1=1,2,…,N3a)の値が算出される。これらの評価関数Eu(k2)_k1(k1=1,2,…,N3a)は、第k2番目の演算処理領域R3min(k2)において、基準画像の縦方向での位置と、その位置の適合度(第k2番目の演算処理領域R3min(k2)での実エッジ投影線L52rの位置に対する適合度)との間の関係を表すデータとしての意味を持つ。
【0260】
次いで、エッジ投影線推定処理部13は、STEP33,34の処理を実行する。STEP33では、エッジ投影線推定処理部13は、処理対象とする演算処理領域R3min(k2)の番数k2を“1”だけ増加させる。そして、STEP34では、エッジ投影線推定処理部13は、増加後のk2の値が演算処理領域R3min(k2)の総個数であるN3bを超えたか否かを判断する。
【0261】
このSTEP34の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k2番目の演算処理領域R3min(k2)に関して、前記STEP32の処理を実行する。
【0262】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれと、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれとの各組に対応して、総計N3a×N3b個の評価関数Eu(k2)_k1の値が算出される。
【0263】
これにより、図16(a),(b)に例示する如く、各演算処理領域R3min(k2)毎に、基準画像の縦方向(v軸方向)での所定の刻み間隔毎の複数の候補位置v(k1)(k1=1,2,…,N3a)と、その各候補位置v(k1)の適合度(第k2番目の演算処理領域R3min(k2)での実エッジ投影線L52rの位置に対する適合度)を表す評価関数Eu(k2)_k1の値との間の関係を表すデータが得られることとなる。
【0264】
なお、図16(a),(b)は、それぞれ、第1番目の演算処理領域R3min(1)に関する評価関数Eu(1)のグラフ、第N3b番目の演算処理領域R3min(N3b)に関する評価関数Eu(N3b)のグラフを代表的に示している。
【0265】
STEP34の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP35の処理を実行する。このSTEP35では、エッジ投影線推定処理部13は、エッジ推定用領域R3uに推定エッジ投影線の複数の候補(以降、エッジ投影線候補という)を仮想的に設定する。そして、エッジ投影線推定処理部13は、その各エッジ投影線候補毎に、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおける評価関数Eu(k2)の値の総和TEu(以降、総合評価関数TEuという)を求める。
【0266】
このSTEP35の処理でエッジ投影線推定処理部13が仮想的に設定する複数のエッジ投影線候補は、複数種類の位置及び方向を有するエッジ投影線である。これらのエッジ投影線候補は、具体的には、例えば次のように設定される。
【0267】
すなわち、図17を参照して、エッジ投影線推定処理部13は、N3b個の演算処理領域R3min(k2)を合成した領域としてエッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ同数ずつ、複数個の点を設定する。例えば、前記N3a個の候補位置v(k1)(k1=1,2,…,N3a)、エッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ、N3a個の点を等間隔で設定する。
【0268】
そして、エッジ投影線推定処理部13は、エッジ推定用領域R3uの左端の各設定点と、右端の各設定点とを結ぶ、総計N3a×N3a個の直線をエッジ投影線候補として設定する。
【0269】
図17に示すエッジ投影線候補は、このようにして設定されるエッジ投影線候補のうちの一部を代表的に示したものである。
【0270】
補足すると、エッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ設定する点の個数は、候補位置v(k1)の個数N3aと同じでなくてもよく、例えばN3aよりも少ない個数であってもよい。また、エッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ設定する点の位置(縦方向での位置)は、v(1)とv(N3a)との間の範囲内の位置であれば、候補位置v(k1)と異なる位置であってもよい。
【0271】
また、エッジ投影線候補が通る基準画像上の1つの点の位置と、該エッジ投影線候補の方向(傾斜角度)との組み合わせによって、複数のエッジ投影線候補を設定するようにしてもよい。
【0272】
上記の如く設定される各エッジ投影線候補に対応する総合評価関数TEuの値は、次のように算出される。
【0273】
すなわち、エッジ投影線推定処理部13は、まず、エッジ投影線候補のそれぞれについて、該エッジ投影線候補がN3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれと交差する点の、基準画像の縦方向での位置(各演算処理領域R3min(k2)での該エッジ投影線候補の位置)を算出する。
【0274】
そして、エッジ投影線推定処理部13は、エッジ投影線候補のそれぞれについて、各演算処理領域R3min(k2)での評価関数Eu(k2)の値を、上記の如く算出した該演算処理領域R3min(k2)でのエッジ候補線候補の位置と、該演算処理領域R3min(k2)において前記STEP32の処理により算出したN3a個の評価関数Eu(k2)_1〜Eu(k2)_N3aとに基づいて決定する。
【0275】
具体的には、任意の1つのエッジ投影線候補に関し、例えば第k2番目の演算処理領域R3min(k2)における該エッジ投影線候補の位置(v軸方向での位置)がvxであり、N3a個の前記候補位置v(1)〜v(N3a)のうちのvxに一致する候補位置がv(k1x)であるとする。この場合、第k2番目の演算処理領域R3min(k2)と候補位置v(k1x)とに対応して算出された評価関数Eu(k2)_k1xの値が、第k2番目の演算処理領域R3min(k2)での該エッジ投影線候補の評価関数Eu(k2)の値として決定される。
【0276】
なお、vxに一致する候補位置が存在しない場合には、例えばvxに最も近い候補位置を上記v(k1x)として、第k2番目の演算処理領域R3min(k2)での該エッジ投影線候補の評価関数Eu(k2)の値が決定される。
【0277】
あるいは、vxの前後の2つ以上の候補位置のそれぞれに対応する評価関数Eu(k2)の値から、線形補間等の補間処理によって、vxに対応する評価関数Eu(k2)の値を算出するようにしてもよい。このようにした場合には、その算出した評価関数Eu(k2)の値を、第k2番目の演算処理領域R3min(k2)での該エッジ投影線候補の評価関数Eu(k2)の値として決定するようにすればよい。
【0278】
次いで、エッジ投影線推定処理部13は、各エッジ投影線候補毎に、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおける該エッジ投影線候補の位置に関する評価関数Eu(1)〜Eu(N3b)の値を加え合わせることによって、該エッジ投影線候補に対応する総合評価関数TEuの値を算出する。
【0279】
以上の如くSTEP35の処理を実行した後、次に、エッジ投影線推定処理部13は、STEP36の処理を実行する。このSTEP36では、エッジ投影線推定処理部13は、STEP35で算出した各エッジ投影線候補毎の総合評価関数TEuに基づいて、実エッジ投影線L52rを推定した線としての推定エッジ投影線を決定する。
【0280】
ここで、ある1つのエッジ投影線候補が、実エッジ投影線L52rに合致する場合、原理的には、各演算処理領域R3min(k2)での該エッジ投影線候補の位置に関する評価関数Eu(k2)の値は最小(最も“0”に近い値)となる。従って、該エッジ投影線候補に対応する総合評価関数TEuの値は、他のエッジ投影線候補に対応する総合評価関数TEuの値よりも小さくなる。
【0281】
そこで、STEP36では、エッジ投影線推定処理部13は、STEP35で設定した全てのエッジ投影線候補のうち、対応する総合評価関数TEuの値が最小となるエッジ投影線候補を、推定エッジ投影線として決定する。
【0282】
これにより例えば、図18に例示する如く、エッジ推定用領域R3uの左端の辺上の位置v(k1a)の点と、右端の辺上の位置v(k1b)(図示例ではv(k1b)=v(k1a))の点とを結ぶエッジ投影線候補が、推定エッジ投影線として決定される。
【0283】
以上が、カメラ3R,3Lの撮像領域に昇りの段差部(昇り階段50)が存在する場合に実行されるエッジ投影線推定処理部13の処理である。
【0284】
次に、カメラ3R,3Lの撮像領域に降りの段差部(例えば降り階段50)が存在する場合に実行されるエッジ投影線推定処理部13の処理(図12のSTEP37〜51の処理)を説明する。
【0285】
段差部が降りの段差部である場合には、エッジ投影線推定処理部13は、まず、STEP37において、基準画像内に、STEP22と同様に、実エッジ投影線の推定用の画像処理領域であるエッジ推定用領域R3dを設定する。
【0286】
ここで、以降の説明では、説明の便宜上、カメラ3R,3Lの撮像領域に存在する降りの段差部が、図1(b)に示した如き降り階段50であるとする。さらに、この降り階段50の任意の1つの上段側踏面51(例えばロボット1から見て最も手前側の上段側踏面)の先端側エッジ52を基準画像に投影してなる実エッジ投影線L52rを推定する処理をエッジ投影線推定処理部13により実行するものとする。
【0287】
そして、昇りの段差部に関する説明と同様に、推定しようとする実エッジ投影線L52rに対応する先端側エッジ52を先端に有する上段側踏面51と、その一段下側の下段側踏面51とを、それぞれ参照符号51p,51qを使用して表記する。
【0288】
STEP37において基準画像に設定するエッジ推定用領域R3dは、図19に示す如く設定される方形状の領域である。このエッジ推定用領域R3dは、その下端の位置が上段側踏面投影領域AR2pの縦方向の幅内の位置(例えば上段側踏面投影領域AR2pの上端から既定の所定量だけ下側の位置)となり、且つ、R3dの上端の位置が下段側踏面投影領域AR2qの縦方向の幅内の位置(例えば下段側踏面投影領域AR2qの下端から所定量だけ上側の位置)になるように配置される。この場合、エッジ推定用領域R3dの縦方向の幅は、下段側踏面投影領域ARqと上段側踏面投影領域ARpとの間の縦方向の間隔に応じて決定される。
【0289】
また、エッジ推定用領域R3dは、その横方向の範囲(左端と右端との間の範囲)が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲に一致するように配置される。この場合、エッジ推定用領域R3dの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅と同じ幅(既定の幅)に決定される。
【0290】
なお、エッジ推定用領域R3dの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅よりも若干小さくてもよい。その場合は、エッジ推定用領域R3dは、その横方向の範囲が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲内に収まるように配置すればよい。
【0291】
以上のSTEP37の処理によって、エッジ推定用領域R3dは、その内部に(上端の辺と下端の辺との間)に推定しようとする単一の実エッジ投影線L52rが存在し、且つ、該エッジ推定用領域R3dの上端の辺及び下端の辺がそれぞれ、下段側踏面投影領域AR2q、上段側踏面投影領域AR2pに含まれるようにして、基準画像内に設定されることとなる。
【0292】
エッジ投影線推定処理部13は、STEP37の次にSTEP38の処理を実行する。このSTEP38では、エッジ投影線推定処理部13は、前記STEP23と同様に処理によって、図20に示す如く、基準画像の縦方向(v軸方向)における実エッジ投影線L52rの位置の候補としての複数(N3a個)の候補位置v(k1)(k1=1,2,…,N3a)を決定する。
【0293】
なお、この場合、候補位置v(k1)の個数N3aは、前記STEP23の処理における個数N3aと一般には異なる。
【0294】
また、この場合、N3a個の候補位置v(k1)(k1=1,2,…,N3a)は、エッジ推定用領域R3dの縦方向の範囲(上端と下端との間の範囲)内で、あらかじめ定められた所定の刻み間隔ずつ(例えば1画素分の間隔ずつ)、基準画像の縦方向に異なる位置とされる。そして、これらの候補位置v(k1)のうち、最も上側の候補位置としての第1番目の候補位置v(1)は、エッジ推定用領域R3dの上端から所定間隔(例えば1画素分の間隔)だけ下側の位置、最も下側の候補位置としての第N3a番目の候補位置v(N3a)は、エッジ推定用領域R3dの下端から所定間隔(例えば1画素分の間隔)だけ上側の位置に決定される。
【0295】
次に、エッジ投影線推定処理部13は、STEP39の処理を実行する。この処理は、前記STEP24の処理と同じであり、k1の初期値として“1”が設定される。
【0296】
次いで、エッジ投影線推定処理部13は、STEP40の処理を実行する。このSTEP40では、エッジ投影線推定処理部13は、基準画像の縦方向での実エッジ投影線L52rの位置が第k1番目の候補位置v(k1)(k1は現在設定されている値)に一致するものと仮定して、エッジ推定用領域R3d内に、図20に例示する如く、2つの射影変換対象領域R3d1,R3d2を設定する。
【0297】
カメラ3R,3Lの撮像領域に存在する段差部が降りの段差部(ここでは降り階段50)である場合には、該段差部の段鼻の先端面や蹴り込み面は、基準画像及び参照画像には写らない。このため、この場合にエッジ推定用領域R3dに設定する射影変換対象領域は、R3d1,R3d2の2つである。
【0298】
これらの2つの射影変換対象領域R3d1,R3d2は、より詳しくは、第k1番目の候補位置v(k1)で横方向に延在する仮想的なエッジ投影線L52(k1)を境界線として上下に隣接するように設定される領域である。この場合、上側の射影変換対象領域R3d1は、その上端側の辺と下端側の辺とがそれぞれエッジ推定用領域R3dの上端側の辺、仮想的なエッジ投影線L52(k1)に一致し、且つ、横方向の範囲が、エッジ推定用領域R3dの横方向の範囲と一致する領域である。
【0299】
従って、射影変換対象領域R3d1は、その縦方向の幅と下端側の辺の位置(縦方向での位置)とが、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3d1は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、下段側踏面投影領域AR2qに含まれる領域である。
【0300】
また、下側の射影変換対象領域R3d2は、その上端側の辺と下端側の辺とがそれぞれ仮想的なエッジ投影線L52(k1)、エッジ推定用領域R3dの下端側の辺に一致し、且つ、横方向の範囲が、エッジ推定用領域R3dの横方向の範囲と一致する領域である。
【0301】
従って、射影変換対象領域R3d2は、その縦方向の幅と上端側の辺の位置(縦方向での位置)とが、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3d2は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、上段側踏面投影領域AR2pに含まれる領域である。
【0302】
以上の如くSTEP40の処理を実行した後、次に、エッジ投影線推定処理部13は、STEP41の処理を実行する。このSTEP41では、エッジ推定用領域R3dに設定した2つの射影変換対象領域R3d1,R3d2のそれぞれが平面部分の投影領域であると仮定して、射影変換対象領域R3d1,R3d2のそれぞれに対応する平面部分を包含する平面についての平面パラメータベクトル↑md1,↑md2を決定する。
【0303】
この場合、射影変換対象領域R3d1,R3d2にそれぞれ対応する平面パラメータベクトル↑md1,↑md2は、それぞれ、下段側踏面投影領域AR2qに対応する平面パラメータベクトル↑m2q、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pに一致するベクトルに決定される。
【0304】
次いで、エッジ投影線推定処理部13は、STEP42の処理を実行する。このSTEP42では、エッジ投影線推定処理部13は、各射影変換対象領域R3d1,R3d2について、各射影変換対象領域R3d1,R3d2の画像(基準画像)と、これに対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像との誤差を表す誤差画像を作成する。
【0305】
この場合、各射影変換対象領域R3d1,R3d2に対応する誤差画像は、それぞれに対応する平面パラメータベクトル↑md1,↑md2を使用して、前記STEP27での誤差画像の作成処理と同様の処理によって作成される。
【0306】
次いで、エッジ投影線推定処理部13は、STEP43,44の処理を実行する。STEP43では、エッジ投影線推定処理部13は、処理に用いる候補位置v(k1)の番数k1を“1”だけ増加させる。そして、STEP44では、エッジ投影線推定処理部13は、増加後のk1の値が候補位置v(k1)の総個数であるN3aを超えたか否かを判断する。
【0307】
このSTEP44の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k1番目の候補位置v(k1)を用いて、前記STEP40〜42の処理を実行する。
【0308】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれに対応して、上記の誤差画像が作成される。
【0309】
STEP44の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP45の処理を実行する。このSTEP45では、エッジ投影線推定処理部13は、図21に示す如く、エッジ推定用領域R3d内に、縦方向に細長い複数(N3b個)の演算処理領域R3min(k2)(k2=1,2,…,N3b)を設定する。各演算処理領域R3min(k2)(k2=1,2,…,N3b)は、前記STEP30の処理に関して説明したものと同様の細長方形状(短冊状)の領域である。そして、その設定の仕方は、STEP30の処理と同じである。
【0310】
なお、演算処理領域R3min(k2)の個数N3bは、前記STEP30で設定する演算処理領域R3min(k2)の個数と同じでなくてもよい。
【0311】
補足すると、前記STEP30に関して説明したことと同様に、前記STEP38において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対してN3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理であると言える。
【0312】
また、前記STEP40において、v(k1)に応じて射影変換対象領域R3d1,R3d2を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対して射影変換対象領域R3d1,R3d2を設定する処理であると言える。
【0313】
STEP45の処理は、STEP38やSTEP40よりも後で行なう必要はなく、例えば、STEP37において、演算処理領域R3min(k2)の設定処理を行ってもよい。
【0314】
次いで、エッジ投影線推定処理部13は、STEP46の処理を実行する。この処理では、k2の初期値として“1”が設定される。
【0315】
次いで、エッジ投影線推定処理部13は、STEP47の処理を実行する。このSTEP47では、エッジ投影線推定処理部13は、第k2番目(k2は現在設定されている番数)の演算処理領域R3min(k2)におけるN3a個の前記候補位置v(k1)(k1=1,2,…,N3a)のそれぞれについて、実エッジ投影線L52rの位置(縦方向での位置)に対する各候補位置v(k1)の適合度を表す評価関数Ed(k2)の値を、前記STEP32の処理の場合と同様の処理によって算出する。
【0316】
ただし、この場合、エッジ推定用領域R3dに設定される射影変換対象領域はR3d1,R3d2の2つであるので、演算処理領域R3min(k2)における各候補位置v(k1)に対応する評価関数Ed(k2)_k1の値は、次のように算出される。
【0317】
すなわち、演算処理領域R3min(k2)における各候補位置v(k1)に対応する評価関数Ed(k2)_k1の値は、v(k1)に応じて設定された射影変換対象領域R3d1(以降、これをR3d1(k1)と表記する)に対応する前記誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Dd1(k2)_k1の値と、v(k1)に応じて設定された射影変換対象領域R3d2(以降、これをR3d2(k1)と表記する)に対応する誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Dd2(k2)_k1の値とを線形結合(本実施形態では加算)してなる値として算出される。
【0318】
さらに詳細には、v(k1)に応じて設定された射影変換対象領域R3d1,R3d2のそれぞれに対応する誤差画像の各画素位置Qでの画素値をそれぞれI1derr(Q),I2derr(Q),I3derr(Q)とする。また、演算処理領域R3min(k2)における射影変換対象領域R3d1(k1),R3d2(k1)のそれぞれのトータルの画素数をNd1(k2)_k1,Nd2(k2)_k1とする。このとき、誤差関数Dd1(k2)_k1,Dd2(k2)_k1はそれぞれ、次式(5−1),(5−2)により算出される。そして、これらのDd1(k2)_k1,Dd2(k2)_k1の値から次式(6)によって、評価関数Ed(k2)_k1が算出される。
【0319】
Dd1(k2)_k1=(1/Nd1(k2)_k1)・ΣI1derr(Q) ……(5−1)
Dd2(k2)_k1=(1/Nd2(k2)_k1)・ΣI2derr(Q) ……(5−2)
Ed(k2)_k1=Dd1(k2)_k1+Dd2(k2)_k1 ……(6)
【0320】
この場合、式(5−1)におけるΣI1derr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3d1(k1)(演算処理領域R3min(k2)と射影変換対象領域R3d1(k1)との重なり領域)での誤差画像の画素値I1derr(Q)の総和である。
【0321】
また、式(3−2)におけるΣI2derr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u2(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u2(k1)との重なり領域)での誤差画像の画素値I2derr(Q)の総和である。
【0322】
STEP47では、以上のようにして、第k2番目の演算処理領域R3min(k2)において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれの適合度(実エッジ投影線L52rの位置としての妥当性の度合い)をそれぞれ表すN3a個の評価関数Ed(k2)_k1(k1=1,2,…,N3a)の値が算出される。
【0323】
次いで、エッジ投影線推定処理部13は、STEP48,49の処理を実行する。STEP48では、エッジ投影線推定処理部13は、処理対象とする演算処理領域R3min(k2)の番数k2を“1”だけ増加させる。そして、STEP49では、エッジ投影線推定処理部13は、増加後のk2の値が演算処理領域R3min(k2)の総個数であるN3bを超えたか否かを判断する。
【0324】
このSTEP49の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k2番目の演算処理領域R3min(k2)に関して、前記STEP47の処理を実行する。
【0325】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれと、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれとの各組に対応して、総計N3a×N3b個の評価関数Ed(k2)_k1の値が算出される。
【0326】
これにより、各演算処理領域R3min(k2)毎に、基準画像の縦方向(v軸方向)での所定の刻み間隔毎の複数の候補位置v(k1)(k1=1,2,…,N3a)と、その各候補位置v(k1)の適合度(第k2番目の演算処理領域R3min(k2)での実エッジ投影線L52rの位置に対する適合度)を表す評価関数Ed(k2)_k1の値との間の関係を表すデータが得られることとなる。
【0327】
STEP49の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP50の処理を実行する。このSTEP50では、エッジ投影線推定処理部13は、前記STEP35の処理と同様に、エッジ推定用領域R3dに複数のエッジ投影線候補を仮想的に設定する。そして、エッジ投影線推定処理部13は、その各エッジ投影線候補毎に、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおける評価関数Ed(k2)の値の総和である総合評価関数TEdを求める。
【0328】
このSTEP50における複数のエッジ投影線候補の設定処理と、各エッジ投影線候補に対応する総合評価関数TEdの値の算出処理とは、前記STEP35の処理と同様の処理によって行われる。
【0329】
次に、エッジ投影線推定処理部13は、STEP51の処理を実行する。このSTEP51の処理は、前記STEP36の処理と同様に行なわれる。すなわち、エッジ投影線推定処理部13は、STEP50で設定した全てのエッジ投影線候補のうち、対応する総合評価関数TEdの値が最小となるエッジ投影線候補を、推定エッジ投影線として決定する。
【0330】
以上が、カメラ3R,3Lの撮像領域に降りの段差部(降り階段50)が存在する場合に実行されるエッジ投影線推定処理部13の処理である。
【0331】
以上説明したエッジ投影線推定処理部13の処理によって、カメラ3R,3Lの撮像領域に段差部が存在する場合に、その段差部が、ロボット1から見て、昇りの段差部、降りの段差部のいずれであっても、該段差部の上段側踏面の先端側エッジを基準画像に投影してなる実エッジ投影線が推定される。
【0332】
なお、以上のエッジ投影線推定処理部13の処理の説明では、STEP22〜36の処理、又は、STEP37〜51の処理によって、任意の1つの先端側エッジ52に対応する推定エッジ投影線を求める場合について説明したが、基準画像に投影されている各上段側踏面51の先端側エッジ52毎に、STEP22〜36の処理、又は、STEP37〜51の処理を繰り返すことで、基準画像に写っている各先端側エッジ52に対応する推定エッジ投影線をそれぞれ求めることができる。
【0333】
このエッジ投影線推定処理部13の処理が終了すると、演算処理装置10は次に、段差部空間配置決定部14の処理を実行する。
【0334】
この段差部空間配置決定部14は、平面投影領域抽出部12の処理によって抽出された各平面投影領域AR2の平面パラメータベクトル↑m2と、エッジ投影線推定処理部13の処理によって求められた推定エッジ投影線とを用いて、カメラ3R,3Lにより撮像された段差部の空間的な配置位置(ロボット座標系で見た配置位置)を決定する。
【0335】
この場合、本実施形態では、段差部空間配置決定部14は、段差部の空間的な配置を示すデータとして、例えば、ロボット1の空間的な進行予定領域での段差部の各上段側踏面の先端側エッジ及び各下段側踏面の基端側エッジの空間的な配置位置、並びに、各踏面の空間的な姿勢(法線方向)を決定する。
【0336】
これらのデータは、基準画像上で決定した推定エッジ投影線と、この推定エッジ投影線に対応する先端側エッジを有する上段側踏面の平面パラメータベクトル↑m2(↑m2p)と、該上段側踏面の一段下側の下段側踏面の平面パラメータベクトル↑m2(↑m2q)とを使用して決定される。
【0337】
具体的には、段差部空間配置決定部14は、基準画像における推定エッジ投影線を、推定エッジ投影線に対応する先端側エッジを有する上段側踏面の平面パラメータベクトル↑m2pにより規定される平面に逆投影してなる線と、該先端側エッジに合致する線として決定する。その線は、基準画像の撮像面における推定エッジ投影線と基準カメラ3の光学中心Cとを含む平面と、上記平面パラメータベクトル↑m2pにより規定される平面との交線上の線として決定される。
【0338】
また、段差部空間配置決定部14は、上記の如く空間的な配置を決定した先端側エッジを、下段側踏面の平面パラメータベクトル↑m2qにより規定される平面に所定の方向で投影してなる線を該下段側踏面の基端側エッジとして決定する。この場合、上記所定の方向は、具体的には、先端側エッジを有する上段側踏面の平面パラメータベクトル↑m2pにより規定される平面に直交する方向、あるいは↑m2pにより規定される平面に対して、90度に近い既定の角度で交差する方向である。
【0339】
なお、前記平面投影領域抽出部12の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトルにより規定される平面の法線方向に、先端側エッジと基端側エッジとを含む平面の法線方向が一致もしくはほぼ一致するように、上記所定の方向を決定してもよい。
【0340】
また、段差部空間配置決定部14は、段差部の上段側踏面及び下段側踏面の姿勢(法線方向)を、それぞれに対応する平面パラメータベクトル↑m2p、↑m2qの方向と一致する方向(これは一般には、鉛直方向とは限らない)に決定する。
【0341】
以上により、段差部の各上段側踏面の先端側エッジ及び各下段側踏面の基端側エッジの空間的な配置位置、並びに、各踏面の空間的な姿勢(法線方向)が決定される。
【0342】
そして、ロボット1は、これらのデータによりロボット1に対する段差部の配置位置やその形状を認識しつつ、該段差部を昇降する等の動作を行なう。
【0343】
以上説明した本実施形態によれば、横方向の幅が微小で、且つ縦方向に細長い複数の演算処理領域R3min(k2)(k2=1,2,…,N3b)が基準画像に横方向に並ぶようにして設定される。そして、その各演算処理領域毎に、基準画像の縦方向の複数の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれの適合度(実エッジ投影線L52rの位置に対する適合度)を表す評価関数Eu(k2)_k1又はEd(k2)_k1が、射影変換を利用した演算処理によって算出される。
【0344】
これにより、横方向に並ぶ複数の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおいて、縦方向の位置と、評価関数Eu(k2)_k1又はEd(k2)_k1の値との関係を表すデータが得られることとなる。
【0345】
さらに、エッジ推定用領域R3u又はR3dに、位置や方向が互いに相違する複数のエッジ投影線候補を仮想的に設定され、それぞれのエッジ投影線候補に対して、各演算処理領域R3min(k2)での位置の評価関数Eu(k2)_k1又はEd(k2)_k1の値を合成してなる総合評価関数TEu又はTEdの値が算出される。この総合評価関数TEu又はTEdは、各演算処理領域R3min(k2)でのエッジ投影線候補の位置の適合度(実エッジ投影線L52rの位置に対する適合度)を合成してなる合成適合度の逆数値に相当するものである。
【0346】
そして、この総合評価関数TEu又はTEdの値が最小となるエッジ投影線候補(換言すれば、上記合成適合度が最大となるエッジ投影線候補)が、推定エッジ投影線として決定される。
【0347】
このようにして推定エッジ投影線を決定することにより、実エッジ投影線を推定したものとしての信頼性の高い推定エッジ投影線を決定できる。
【0348】
また、特に、カメラ3R,3Lの撮像領域に昇り階段50などの昇りの段差部が存在する場合に、候補位置v(k1)に依存する3つの射影変換対象領域R3u1,R3u2,R3u1をエッジ推定用領域R3uに前記した如く設定する(ひいては各演算処理領域R3min(k2)に設定する)ので、前記各評価関数Eu(k2)_k1の値に、候補位置v(k1)の位置を境界として隣接する射影変換対象領域R3u1,R3u2だけでなく、下段側踏面に対応する射影変換対象領域R3u3の画像の画素値を反映させることができる。すなわち、前記各評価関数Eu(k2)_k1の値を、各演算処理領域R3min(k2)に含まれる多くの画素の画素値を反映させて算出することができる。
【0349】
そのため、各演算処理領域R3min(k2)における候補位置v(k1)の位置の適合度を表すものとしての各評価関数Eu(k2)_k1の値の信頼性を高めることができる。ひいては、推定エッジ投影線の信頼性を高めることができる。
【0350】
また、本実施形態では、段差部存在検知部11や、平面投影領域抽出部12は、ステレオ画像と射影変換とを利用した処理によって、レーザ測距装置等の測距装置をロボット1に備えることなく、段差部の存在を検知したり、平面投影領域の抽出を行なうことができる。このため、段差部の認識を安価な構成で行なうことができる。
【0351】
ここで、以上説明した実施形態と本発明との対応関係について補足しておく。
【0352】
本実施形態では、前記段差部存在検知部11の処理によって、本発明における段差部存在検知手段が実現される。この場合、段差部存在検知部11は、本発明における段差部種別判断手段としての機能を含んでおり、前記STEP4の処理によって、段差部種別判断手段が実現される。
【0353】
また、前記平面投影領域抽出部12の処理によって、本発明における平面投影領域抽出手段が実現される。この場合、前記局所領域R2(i)(i=1,2,…,N2)が、本発明における平面投影領域抽出用局所領域に相当する。
【0354】
また、前記エッジ投影線推定処理部13の処理によって、本発明における評価関数算出手段と推定エッジ投影線決定手段とが実現される。より詳しくは、前記STEP22〜34の処理及びSTEP37〜49の処理によって、評価関数算出手段が実現され、前記STEP35,36並びにSTEP50,51の処理によって、推定エッジ投影線決定手段が実現される。
【0355】
この場合、射影変換対象領域R3u1,R3u2,R3u3が、それぞれ、本発明における第1のエッジ推定用射影変換対象領域、第2のエッジ推定用射影変換対象領域、第3のエッジ推定用射影変換対象領域に相当し、射影変換対象領域R3d1,R3d2が、それぞれ、本発明における第4のエッジ推定用射影変換対象領域、第5のエッジ推定用射影変換対象領域に相当する。また、前記平面パラメータベクトル↑mu1,↑mu2,↑mu3,↑md1,↑md2がそれぞれ、本発明における射影変換用平面パラメータに相当する。
【0356】
また、前記段差部空間配置決定部14の処理によって、本発明における段差部空間配置決定手段が実現される。
【0357】
次に、以上説明した実施形態の変形態様をいくつか説明しておく。
【0358】
前記実施形態では、移動体として2脚の脚式移動ロボット1を例示したが、移動体は、3脚以上の脚を有するロボット、あるいは、車輪やクローラを備える移動体であってもよい。
【0359】
また、前記実施形態では、カメラ3R,3Lのうちのカメラ3Rを基準カメラ3として使用したが、カメラ3Lを基準カメラとして使用してもよい。
【0360】
また、前記実施形態では、カメラ3R,3Lの撮像領域に昇りの段差部が存在する場合に、候補位置v(k1)に依存する3つの射影変換対象領域R3u1,R3u2,R3u3をエッジ推定用領域R3uに設定するようにしたが、射影変換対象領域R3u1,R3u2の2つの射影変換対象領域だけをエッジ推定用領域R3uに設定(ひいては各演算処理領域R3min(k2)に設定)するようにしてもよい。
【0361】
また、前記実施形態では、各演算処理領域R3min(k2)における複数の候補位置は、全ての演算処理領域R3min(k2)について同じであるが、各演算処理領域R3min(k2)毎の候補位置を異ならせるようにしてもよい。
【0362】
また、前記実施形態では、平面投影領域抽出部12の処理において、複数の局所領域R2(i)(i=1,2,…,N2)の、基準画像の横方向での位置を同じとしたが、さらに、基準画像の横方向での複数の位置のそれぞれおいて、複数の局所領域R2(i)を縦方向に並べるように配置してもよい。
【0363】
このようにした場合には、縦方向での平面部分の分布の変化を、基準画像の横方向の複数位置において、把握することができる。このため、カメラ3R,3Lの撮像領域に存在する段差部の各踏面が、基準画像の横方向に対して比較的大きく傾いているような場合に、その傾きを反映させて、段差部の各踏面等の平面部分に対応する平面投影領域を抽出することができる。従って、該平面部分の傾きに合せて、上端及び下端の辺が基準画像の横方向に対して傾斜したような形状の平面投影領域を抽出するようにすることができる。
【符号の説明】
【0364】
1…脚式移動ロボット(移動体)、3R,3L…カメラ(ステレオカメラ)、4…投光器、11…段差部存在検知部(段差部存在検知手段、段差部種別判断手段)、12…平面投影領域抽出部(平面投影領域抽出手段)、13…エッジ投影線推定処理部(評価関数算出手段、推定エッジ投影線決定手段)、14…段差部空間配置決定部(段差部空間配置決定手段)、STEP4…段差部種別判断手段、STEP22〜34,STEP37〜49…評価関数算出手段、STEP35,36,50,51…推定エッジ投影線決定手段、50…階段(段差部)、51…踏面、52…先端側エッジ。
【技術分野】
【0001】
本発明は、移動体の移動環境に存在する階段等の段差部の空間的な配置位置を、移動体に搭載されたステレオカメラによって取得されるステレオ画像を用いて認識する装置に関する。
【背景技術】
【0002】
従来、例えば自律的に移動可能なロボットに階段の昇降動作を行なわせる場合に、該ロボットに階段を認識させる技術として、例えば特許文献1に見られる技術が提案されている。
【0003】
この技術では、ロボットに搭載したステレオビジョンシステムによってロボットの外界の距離データを取得し、この距離データを基に、階段の踏面に相当する平面部を検出する。そして、検出した平面部を包含する多角形をSklanskyのアルゴリズムや、Melkmanのアルゴリズムによって求め、その求めた多角形から、階段のフロントエッジ、バックエッジ、幅、長さを特定する。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】国際公開公報WO2005/087452
【発明の概要】
【発明が解決しようとする課題】
【0005】
ところで、2足移動ロボット等の脚式移動ロボットに階段等の段差部の昇降動作を行なわせる場合、ロボットの脚の足平部を、段差部の踏面の先端側エッジ(特許文献1で言うところのフロントエッジ)に載せて、該足平部の一部を段差部の踏面からはみ出させることが必要もしくは好ましい場合がある。
【0006】
例えば、階段の踏面の奥行き寸法が、ロボットの足平部の大きさに比して狭い場合には、該足平部の全体を踏面に載せることが不可能な場合がある。あるいは、ロボットの足平部の全体を踏面に載せることが可能であっても、該足平部を振り出すときに、該足平部が階段の蹴り込み等と干渉するのを避けるために、ロボットの足平部の一部を踏面からはみ出させることが好ましい場合もある。
【0007】
そして、このような場合には、ロボットの足平部の目標とする着地位置を適切に決定するために、階段等の段差部の先端側エッジの位置及びその方向を精度よく認識する必要がある。
【0008】
しかるに、前記特許文献1に見られる如き従来の手法では、階段などの段差部の踏面の先端側エッジを検出することができない場合が多々ある。
【0009】
すなわち、ステレオビジョンシステムによって得られるステレオ画像から認識される距離データは、撮像画像のテクスチャ等の影響を受けやすいので、段差部の踏面の先端側エッジ付近での信頼性の高い距離データを得ることができない場合も多々ある。そして、このような場合には、段差部の踏面の先端側エッジ付近は、平面部分として認識できないか、もしくは、平面部分から除外されることとなる。その結果、前記多角形は、段差部の踏面の先端側エッジと異なる境界の形状を有するものとなり、該多角形から認識されるフロントエッジは、段差部の踏面の実際の先端側エッジとの誤差が大きなものとなりやすい。
【0010】
従って、前記特許文献1に見られる如き従来の手法では、階段などの段差部の踏面の先端側エッジを検出することができない場合が多々ある。
【0011】
本発明は、かかる背景に鑑みてなされたものであり、ステレオカメラにより取得されるステレオ画像を利用して、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を含む該段差部の配置位置を精度良く認識することができる装置を提供することを目的とする。
【課題を解決するための手段】
【0012】
ここで、本発明における用語について補足しておく。本発明においては、「段差部」は、下段側踏面とその一段上側の上段側踏面との組を一組以上(一組又は複数組)有するものである。この場合、「段差部」の各「踏面」は、ある仮想的な平面(無限平面)に包含される平坦な面とみなすことができるような面であるとする。そして、本発明における「段差部」は、その一形態として階段を含むものであるが、階段だけを意味するものではない。例えば、床面に設置される踏み台もしくは任意の設置物により構成される段差部も含まれる。
【0013】
また、段差部の上段側踏面の「先端側エッジ」というのは、段差部をその下段側踏面が手前側、上段側踏面が奥側となる視線で見た場合において、上段側踏面の外形の境界線のうち、手前側で横方向(水平方向もしくはそれに近い方向)に直線状に延在する境界線を意味する。
【0014】
換言すれば、段差部の上段側踏面の「先端側エッジ」というのは、段差部をその上段側踏面が手前側、下段側踏面が奥側となる視線で見た場合において、上段側踏面の外形の境界線のうち、奥側で横方向に直線状に延在する境界線を意味する。
【0015】
以上を前提として、本発明を以下に説明する。
【0016】
本発明の段差部認識装置は、上記の目的を達成するために、ステレオカメラを搭載した移動体の移動環境に存在しており、下段側踏面とその一段上側の上段側踏面との組を一組以上有する段差部の空間的な配置位置を、前記ステレオカメラにより取得されるステレオ画像を構成する基準画像及び参照画像を用いて認識する段差部認識装置であって、
前記段差部が前記ステレオカメラにより撮像されている状態において、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する2つの平面部分を少なくとも含む互いに異なる平面部分がそれぞれ投影された領域である複数の平面投影領域を抽出すると共に、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータを決定する平面投影領域抽出手段と、
前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定すると共に、該複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する評価関数算出手段と、
前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数に基づいて決定する推定エッジ投影線決定手段と、
該段差部の空間的な配置位置を認識するためのデータとして、少なくとも前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する段差部空間配置決定手段とを備え、
前記評価関数算出手段は、前記各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を算出するために、各演算処理領域内に各候補位置を境界として隣接する2つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する処理と、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する処理と、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する処理と、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する処理とを実行することを特徴とする(第1発明)。
【0017】
かかる第1発明によれば、前記段差部が前記ステレオカメラにより撮像されている状態において、前記平面投影領域抽出手段の処理が実行される。この処理では、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する2つの平面部分を少なくとも含む複数の平面投影領域が抽出される。さらに、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータが決定される。
【0018】
補足すると、第1発明における平面投影領域抽出手段の処理は、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行い得るものであれば、どのような手法を用いるものであってもよい。その手法は、公知の手法であってもよい。例えば、移動体に搭載したレーザ測距装置等の公知の測距装置を用いて、段差部の各部の距離(移動体からの距離)を測定し、その測定データを基に、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行うようにしてもよい。
【0019】
あるいは、後述する射影変換(平面射影変換)を利用した手法によって、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行なうことも可能である。
【0020】
また、平面投影領域抽出手段により抽出する各平面投影領域は、それぞれに対応する平面部分(段差部の各踏面等)の全体の投影領域である必要はなく、該平面部分の一部分の投影領域であってもよい。例えば、段差部の上段側踏面に対応して抽出される平面投影領域には、該上段側踏面の先端側エッジを基準画像に投影した線が含まれている必要なない。
【0021】
また、平面投影領域抽出手段により抽出する平面投影領域は、段差部の各踏面に対応する平面投影領域に限らず、例えば階段の蹴り込み面や階段の段鼻の先端面に対応する平面投影領域が含まれていてもよい。ただし、段差部の各踏面以外の平面部分に対応する平面投影領域を抽出対象から除外するようにしてもよい。その場合、例えば平面部分の法線方向の向きに基づいて、段差部の各踏面以外の平面部分に対応する平面投影領域を抽出対象から除外するようにすることが可能である。
【0022】
第1発明では、次に、前記評価関数算出手段の処理が実行される。この処理では、評価関数算出手段は、前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定する。
【0023】
この場合、前記実エッジ投影線は、段差部の上段側踏面に対応する平面投影領域と、下段側踏面に対応する平面投影領域との間に存在するので、該実エッジ投影線をそれぞれ内部に含むような複数条の演算処理領域を、前記平面投影領域抽出手段の処理により得られた平面投影領域及び平面パラメータに基づいて設定することが可能である。なお、各演算処理領域は、基準画像の縦方向に細長い形状を有するので、基準画像の横方向での幅が微小なもの(例えば1画素分の幅)となるような領域である。
【0024】
さらに、評価関数算出手段は、複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する。
【0025】
この評価関数は次のように算出される。すなわち、評価関数算出手段は、各演算処理領域内に各候補位置を境界として隣接する2つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する。
【0026】
この場合、各演算処理領域において、任意の1つの候補位置が、実エッジ投影線の位置に一致していると仮定すると、該候補位置を境界として隣接して設定される2つのエッジ推定用射影変換対象領域のうち、該候補位置の上側の領域は、段差部の上段側踏面の投影領域に相当するものとなる。また、候補位置の下側の領域は、段差部の上段側踏面の先端側エッジから下方に起立する面部(階段の段鼻又は蹴り込み面に対応する面部)の投影領域に相当するものとなる。
【0027】
さらに、評価関数算出手段は、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する。例えば、各候補位置を境界として隣接して設定される2つのエッジ推定用射影変換対象領域のうち、該候補位置の上側の領域に対応する射影変換用平面パラメータは、段差部の上段側踏面に対応して平面投影領域抽出手段により決定された平面パラメータに一致するものとされる。
【0028】
また、各候補位置を境界として隣接して設定される2つのエッジ推定用射影変換対象領域のうち、該候補位置の下側の領域に対応する射影変換用平面パラメータは、例えば、段差部の上段側踏面と下段側踏面との間でこれらに対して起立する面に対応して平面投影領域抽出手段により決定された平面パラメータに一致するものとされる。
【0029】
あるいは、当該射影変換用平面パラメータは、例えば段差部の上段側踏面に対してあらかじめ定められた角度(例えば90degまたはそれに近い角度)で起立し、且つ、候補位置に対応する仮想的な先端側エッジ(該先端側エッジを基準画像に投影した線の位置(基準画像の縦方向での位置)が該候補位置に一致するような仮想的な先端側エッジ)の直線を含む平面の位置及び姿勢を表す平面パラメータ(これは上段側踏面に対応して平面投影領域抽出手段により決定された平面パラメータと候補位置とから算出することができる)に一致するものとされる。
【0030】
さらに、評価関数算出手段は、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する。
【0031】
なお、エッジ推定用射影変換対象領域に対応する参照画像中の領域というのは、基準画像のエッジ推定用射影変換対象領域に投影されている実空間の部分が該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面上の部分であると仮定して、該部分を参照画像中に投影した場合に得られる領域を意味する。
【0032】
この場合、各演算処理領域において、ある候補位置が、実エッジ投影線の位置に一致しておれば、各エッジ推定用射影変換対象領域の画像の画素値分布と上記射影変換画像の画素値分布とは互いに一致(もしくはほぼ一致)する。一方、該候補位置が、実エッジ投影線の位置に一致しない場合には、各エッジ推定用射影変換対象領域の画像の画素値分布と上記射影変換画像の画素値分布とは互いに異なるものとなる。
【0033】
そして、評価関数算出手段は、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する。
【0034】
これにより、基準画像の縦方向に細長い(基準画像の横方向での幅が微小な)各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、該候補位置の実エッジ投影線の位置に対する適合度との相関関係を表すデータが得られることとなる。このデータは、換言すれば、各演算処理領域において、基準画像の縦方向でのどの位置が、実エッジ投影線の位置として妥当であるか(適合度が高いか)を示すものとなる。
【0035】
次いで、推定エッジ投影線決定手段の処理が実行される。この処理では、推定エッジ投影線決定手段は、前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数(各演算処理領域及び各候補位置の組毎に算出された評価関数の値)に基づいて決定する。
【0036】
この場合、前記評価関数算出手段の処理によって、各演算処理領域及び各候補位置の組毎に算出された評価関数の値によって、各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、上記評価関数の値との相関関係を表すデータが得られている。従って、ある推定エッジ投影線を仮定した場合、その推定エッジ投影線の、各演算処理領域における位置(基準画像の縦方向での位置)の、実エッジ投影線の実際に位置に対する適合度(妥当性の度合い)が判ることとなる。
【0037】
そして、推定エッジ投影線が実エッジ投影線に一致していれば、その推定エッジ投影線の、各演算処理領域における位置の適合度は、原理的には、該演算処理領域において最も高くなる。
【0038】
そこで、推定エッジ投影線決定手段は、前記複数条の演算処理領域のそれぞれにおける推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように該推定エッジ投影線を決定する。これにより、複数条の演算処理領域のそれぞれにおける実エッジ投影線に対する適合度が極力高いものとなるような推定エッジ投影線、すなわち、実エッジ投影線を推定したものとしての妥当性が高い推定エッジ投影線が決定される。
【0039】
なお、推定エッジ投影線決定手段の処理で決定する推定エッジ投影線は、その合成適合度が厳密に最も高いものである必要はなく、該推定エッジ投影線と基準画像での位置及び方向の一方又は両方が若干異なるような複数の線に比して、該合成適合度が相対的に高くなるようなものでよい。従って、基準画像での位置及び方向の一方又は両方が互いに異なる複数の推定エッジ投影線の候補の中で、最も、前記合成適合度が高いものを、推定エッジ投影線として決定してもよい。
【0040】
次いで、前記段差部空間配置決定手段の処理が実行される。この処理では、段差部空間配置決定手段は、ステレオカメラの撮像領域に存在する段差部の空間的な配置位置を認識するためのデータとして、少なくとも該段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する。
【0041】
この場合、段差部の上段側踏面の先端側エッジは、上段側踏面に対応して前記平面投影領域抽出手段により決定された平面パラメータにより空間的な位置及び姿勢が規定される平面上の線であり、この線を光学的に基準画像に投影してなる線が、上記推定エッジ投影線となる。従って、この推定エッジ投影線と、平面投影領域抽出手段により決定された平面パラメータ(上段側踏面に対応する平面部分の平面パラメータ)とに基づいて、先端側エッジの空間的な位置及び方向を表すデータを決定できることとなる。
【0042】
該データとしては、例えば、先端側エッジ上の2つの点の空間的な位置、あるいは、先端側エッジ上の1つの点の空間的な位置と該先端側エッジの方向ベクトルとの組等を用いることができる。
【0043】
以上説明した第1発明によれば、前記評価関数算出手段の処理によって、各演算処理領域及び各候補位置の組毎に算出される評価関数の値は、基準画像の縦方向に細長い各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、該候補位置の実エッジ投影線の位置に対する適合度との相関関係を表すものとなる。
【0044】
従って、この評価関数の値に基づいて、前記推定エッジ投影線を決定することにより、基準画像の横方向に並ぶ複数条の演算処理領域のそれぞれにおいて適合度が極力高くなるような推定エッジ投影線を決定できる。このため、実エッジ投影線に対する一致度合いの高い推定エッジ投影線を決定できることとなる。
【0045】
よって、第1発明によれば、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を精度よく認識することができる。
【0046】
上記第1発明においては、前記平面投影領域抽出手段は、レーザ測距装置等を使用することなく、前記ステレオ画像(基準画像及び参照画像)を用いて平面投影領域の抽出と平面パラメータの決定とを行なうようにすることも可能である。
【0047】
具体的には、前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する処理と、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出する(第2発明)。
【0048】
この第2発明によれば、前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する。該平面投影領域抽出用局所領域は、基準画像の全体に比して十分に小さい面積の領域である。
【0049】
ここで、ある1つの平面投影領域抽出用局所領域の画像が平面部分の画像である場合には、その画像と、基準画像の該平面投影領域抽出用局所領域に投影されている実空間の部分(ここでは平面部分のうちの局所部分)を参照画像中に投影してなる部分の画像とは、当該平面部分を包含する平面の位置及び姿勢を表す平面パラメータに応じた射影変換(平面射影変換)によって対応付けられる。
【0050】
すなわち、原理的には、平面投影領域抽出用局所領域に対応する参照画像中の画像に、該平面投影領域抽出用局所領域に投影されている平面部分の平面パラメータに応じた射影変換を施すことによって、基準画像の平面投影領域抽出用局所領域の画像と同じ画素値分布を有する画像が得られる。
【0051】
一方、平面投影領域抽出用局所領域の画像が平面部分の画像であっても、実際の平面部分に対応する平面パラメータと異なる平面パラメータを使用して、平面投影領域抽出用局所領域に対応する参照画像中の画像に射影変換を施した場合に得られる画像は、基準画像の平面投影領域抽出用局所領域の画像と異なる画素値分布を有する画像となる。
【0052】
そこで、第2発明では、平面投影領域抽出手段は、次に、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する。
【0053】
これにより、段差部の踏面等の平面部分が投影されている平面投影領域抽出用局所領域にあっては、その平面部分を包含する平面の位置及び姿勢を表す平面投影領域抽出用平面パラメータが適正に決定されることとなる。
【0054】
なお、平面投影領域抽出用局所領域に単一の平面部分とそれ以外の部分とが含まれている場合には、決定される平面投影領域抽出用平面パラメータにより規定される平面は、一般には、該平面投影領域抽出用局所領域に含まれる平面部分を包含する平面とは異なるものとなる。
【0055】
上記の如く各平面投影領域抽出用局所領域に対応する平面投影領域抽出用平面パラメータを決定した場合、同一の平面上に存在する平面投影領域抽出用局所領域に対応する平面投影領域抽出用平面パラメータは、互いに同一(もしくはほぼ同一)となる。
【0056】
そこで、平面投影領域抽出手段は、次に、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出する。なお、ここで、平面投影領域抽出用平面パラメータが互いに同一というのは、厳密に互いに同一であることだけを意味するものではなく、平面投影領域抽出用平面パラメータにより規定される平面の位置及び姿勢がある既定の範囲内でほぼ同一となるような場合も含まれる。
【0057】
これにより、第2発明によれば、レーザ測距装置等の測距装置をステレオカメラとは別に移動体に搭載したりすることなく、ステレオ画像と射影変換(平面射影変換)とを利用した手法によって、基準画像から平面投影領域を抽出することができる。
【0058】
前記第1発明又は第2発明においては、前記評価関数算出手段が各演算処理領域に設定する複数の候補位置は、全ての演算処理領域について共通のN3a個(N3a:2以上の整数)の候補位置であり、前記評価関数算出手段は、第k1番目(k1:1からN3aまでの整数のうちの任意の1つの整数)の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出するために、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域における画像と、該合成射影変換対象領域に対応する前記参照画像中の領域の画像を該合成射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる画像との誤差画像を求める処理を実行し、その誤差画像を用いて、第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出することが好ましい(第3発明)。
【0059】
この第3発明によれば、前記評価関数算出手段は、前記複数の候補位置の任意の1つである第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数を算出する前に事前に、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域に係わる前記誤差画像を求めておく。そして、前記評価関数算出手段は、この誤差画像を用いて第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出する。これにより、各候補位置及び各演算処理領域の各組毎に個別に前記誤差関数の値を算出する場合に比して、各候補位置及び各演算処理領域の各組毎の誤差関数の値の算出を効率よく行なうことができる。
【0060】
前記第1〜第3発明では、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する段差部存在検知手段をさらに備えている場合には、前記平面投影領域抽出手段、評価関数算出手段及び段差部配置決定手段の処理は、前記段差部存在検知手段によって前記ステレオカメラの撮像領域に段差部が存在していることが検知されることを必要条件として実行される処理とされる。
【0061】
この場合、前記段差部存在検知手段は、外部から与えられる情報に基づいて段差部が存在しているか否かを検知するようにすることも可能であるが、前記基準画像及び参照画像を用いて当該検知を行なうこともできる。
【0062】
具体的には、前記段差部存在検知手段は、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知するための段差部存在検知用領域を前記基準画像に設定する処理と、該段差部存在検知用領域の画像と該段差部存在検知用領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである段差部存在検知用平面パラメータを、前記段差部存在検知用領域の画像の画素値分布と該段差部存在検知用領域に対応する領域の画像を該段差部存在検知用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、決定した該段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する(第4発明)。
【0063】
この第4発明によれば、段差部存在検知手段の処理によって決定される前記段差部存在検知用平面パラメータにより規定される平面の法線方向の向きは、前記ステレオカメラの撮像領域に段差部が存在している場合には、該撮像領域が平坦な床面である場合の法線方向の向きよりも傾いたものとなる。このため、前記段差部存在検知用平面パラメータにより規定される平面の法線方向の向きに基づいて、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知することができる。
【0064】
前記第1〜第4発明においては、前記段差部が前記ステレオカメラにより撮像されている状態において、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する段差部種別判断手段をさらに備えており、
前記評価関数算出手段は、
前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第1及び第2のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第1及び第2のエッジ推定用射影変換対象領域のうちの下側の領域である第2のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第3のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定し、
前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第4及び第5のエッジ推定用射影変換対象領域を該演算処理領域内に設定することが好ましい(第5発明)。
【0065】
この第5発明によれば、前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合にあっては、前記評価関数算出手段は、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定する。これにより、前記複数条の演算処理領域のそれぞれに、前記実エッジ投影線が含まれるように該複数条の演算処理領域を基準画像に設定することができる。
【0066】
さらに、前記評価関数算出手段は、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第1及び第2のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第1及び第2のエッジ推定用射影変換対象領域のうちの下側の領域である第2のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第3のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定する。
【0067】
この場合、各演算処理領域及び各候補位置の任意の1つの組において、該演算処理領域に設定される第1〜第3のエッジ推定用射影変換対象領域は、それぞれ、当該候補位置が、実エッジ投影線の位置に一致する場合に、段差部の上段側踏面が投影される領域、該上段側踏面に対して起立する面(段鼻の先端面や蹴り込み面)が投影される領域、段差部の下段側踏面が投影される領域に相当するものとして設定される領域である。そして、これらの第1〜第3のエッジ推定用射影変換対象領域は、候補位置に応じて変化することとなる。
【0068】
従って、ステレオカメラの撮像領域に昇りの段差部が存在する場合には、各演算処理領域及び各候補位置の各組に対応して前記評価関数算出手段により算出される評価関数の値には、該候補位置で隣接する第1及び第2のエッジ推定用射影変換対象領域の画像に加えて、これらと平面パラメータベクトルが異なる第3のエッジ推定用射影変換対象領域の画像の画素値が反映されることとなる。そのため、各演算処理領域の各候補位置の、実エッジ投影線の位置に対する適合度を表すものとしての前記評価関数の値の信頼性を高めることができる。
【0069】
一方、前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合にあっては、前記評価関数算出手段は、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定する。
【0070】
これにより、前記複数条の演算処理領域のそれぞれに、前記実エッジ投影線が含まれるように該複数条の演算処理領域を基準画像に設定することができる。
【0071】
さらに、前記評価関数算出手段は、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第4及び第5のエッジ推定用射影変換対象領域を該演算処理領域内に設定する。
【0072】
ここで、ステレオカメラの撮像流域に存在する段差部が降りの段差部である場合には、基準画像には、段鼻の先端面や蹴り込み面のように、上段側透明に対して起立する姿勢の面は写らない。従って、評価関数算出手段は、上記第4及び第5のエッジ推定用射影変換対象領域を演算処理領域内に設定する。これらの第4及び第5のエッジ推定用射影変換対象領域は、それぞれ、当該候補位置が、実エッジ投影線の位置に一致する場合に、段差部の上段側踏面が投影される領域、段差部の下段側踏面が投影される領域に相当するものとして設定される領域である。そして、これらの第4及び第5のエッジ推定用射影変換対象領域は、候補位置に応じて変化することとなる。
【0073】
従って、ステレオカメラの撮像領域に存在する段差部が降りの段差部である場合には、各演算処理領域及び各候補位置の各組に対応して前記評価関数算出手段により算出される評価関数の値には、該候補位置で隣接する第4及び第5のエッジ推定用射影変換対象領域の画像の画素値が適切に反映されることとなる。そのため、各演算処理領域の各候補位置の、実エッジ投影線の位置に対する適合度を表すものとしての前記評価関数の値の信頼性を適切に確保することとができる。
【0074】
よって、第5発明によれば、ステレオカメラの撮像領域に存在する段差部が昇りの段差部と降りの段差部とのといずれであっても、実エッジ投影線に相当するものとして推定エッジ投影線を適切な信頼性で決定することができる。
【0075】
上記第5発明においては、前記ステレオカメラの撮像領域に存在する段差部が昇りの段差部である場合において、前記平面投影領域抽出手段により、段鼻の先端面や蹴り込み面のように上段側踏面に対して起立した姿勢の平面部分の平面投影領域を抽出するようにした場合には、前記第2のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、当該起立した姿勢の平面部分の平面投影領域に対応する平面パラメータを使用することができる。
【0076】
ただし、段差部の段鼻の先端面や蹴り込み面のように上段側踏面に対して起立した姿勢の平面部分は、一般に、上段側踏面に対して一般に垂直もしくはそれに近い角度で起立する。そこで、前記第5発明においては、前記評価関数算出手段は、前記段差部種別判断手段によって前記段差部が昇りの段差部であると判断された場合に、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第1のエッジ推定用射影変換対象領域と前記第3のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定し、前記第2のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、第1のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面に対してあらかじめ定めた角度(例えば90deg又はそれに近い角度)で交差する平面の位置及び姿勢を表す平面パラメータを設定するようにしてもよい(第6発明)。
【0077】
この第6発明によれば、前記ステレオカメラの撮像領域に存在する段差部が昇りの段差部である場合に、前記平面領域抽出手段により、各踏面に対応する平面投影領域だけを抽出した場合であっても、第1〜第3のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを適切に設定することができる。
【0078】
なお、前記段差部種別判断手段によって前記段差部が降りの段差部であると判断された場合にあっては、前記評価関数算出手段は、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第4のエッジ推定用射影変換対象領域と前記第5のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定するようにすればよい。
【0079】
補足すると、前記第5発明又は第6発明において、前記第4発明の構成を組み込んだ場合には、前記段差部種別判断手段は、前記段差部存在検知手段により決定された段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて、前記段差部が昇りの段差部であるか、降りの段差部であるかを判断することができる。
【0080】
これは、前記段差部が昇りの段差部であるか、降りの段差部であるかに依存して、前記段差部存在検知用平面パラメータにより示される平面の法線が、段差部が存在しない平坦な面の法線方向に対して傾く向きが異なるからである。
【0081】
このように前記段差部種別判断手段を構成することで、段差部存在検知手段により決定された段差部存在検知用平面パラメータを利用して容易に、前記段差部が昇りの段差部であるか、降りの段差部であるかを判断することができる。
【0082】
本発明においては、さらに、前記段差部空間配置決定手段は、前記段差部の上段側踏面の先端側エッジを、該上段側踏面に対応する前記平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する方向で該段差部の下段側踏面に対応する前記平面パラメータにより規定される平面に投影した線を、該下段側踏面の基端側の境界線として決定する手段を含むことが好ましい(第7発明)。
【0083】
この第8発明によれば、前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータに加えて、前記段差部空間配置決定手段により下段側踏面の基端側の境界線が決定される。このため、上段側踏面の一段下側の下段側踏面の奥行きを認識できることとなる。
【0084】
さらに本発明においては、前記移動体には、前記ステレオカメラのよる撮像時に該ステレオカメラの撮像領域にテクスチャを付与する投光器が搭載されていることが好ましい(第8発明)。
【0085】
この第8発明によれば、ある平面部分が投影されている基準画像の任意の領域の画像の画素値分布と、それに対応する参照画像の領域の画像を、適宜の平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との差は、該平面パラメータが、実際の平面部分の平面パラメータに対して相違する場合に顕著なものとなりやすい。
【0086】
このため、前記評価関数算出手段により算出される評価関数の値、あるいは、前記第2発明において平面投影領域抽出手段が決定する平面投影領域抽出用平面パラメータ(ひいては各平面投影領域の平面パラメータ)、あるいは、前記第4発明における段差部存在検知手段が決定する段差部存在検知用平面パラメータの信頼性を高めることができる。
【図面の簡単な説明】
【0087】
【図1】図1(a),(b)はそれぞれ、本発明の一実施形態におけるロボット(移動体)が階段を昇ろうとしている状況、降ろうとしている状況を示す斜視図。
【図2】実施形態のロボットに搭載した投光器によってステレオカメラの撮像領域に付与されるテクスチャを例示する図。
【図3】代表的な階段の構造を説明するための図。
【図4】図1に示すロボットに備えた演算処理装置の機能的構成を示すブロック図。
【図5】図4に示す段差部存在検知部の処理を示すフローチャート。
【図6】図5のSTEP1の処理を説明するための図。
【図7】図7(a),(b)は、図5のSTEP2の処理を説明するための図。
【図8】図4に示す平面投影領域抽出部の処理を示すフローチャート。
【図9】図8のSTEP11,15の処理を説明するための図。
【図10】図8のSTEP13の処理により得られるデータを示すグラフ。
【図11】図4に示すエッジ投影線推定処理部の処理を示すフローチャート。
【図12】図4に示すエッジ投影線推定処理部の処理を示すフローチャート。
【図13】図11のSTEP22の処理を説明するための図。
【図14】図11のSTEP23の処理を説明するための図。
【図15】図11のSTEP30の処理を説明するための図。
【図16】図16(a),(b)は、図11のSTEP32の処理により得られるデータを例示するグラフ。
【図17】図11のSTEP35の処理を説明するための図。
【図18】図11のSTEP36の処理を説明するための図。
【図19】図12のSTEP37の処理を説明するための図。
【図20】図12のSTEP38の処理を説明するための図。
【図21】図12のSTEP45の処理を説明するための図。
【発明を実施するための形態】
【0088】
本発明の一実施形態を図1〜図20を参照して以下に説明する。
【0089】
図1(a),(b)を参照して、本実施形態では、移動体としての脚式移動ロボット1によって階段等の段差部の空間的な配置位置を認識する場合を例にとって説明する。図示例の脚式移動ロボット1(以下、単にロボット1という)は、左右一対の2つの脚体を備える2足歩行ロボットである。
【0090】
このロボット1には、ステレオカメラを構成する左右一対のカメラ3R,3Lと、カメラ3R,3Lの撮像領域に投光する投光器4とが搭載されている。
【0091】
カメラ3Rは、撮像領域に向って右側のカメラ、カメラ3Lは、撮像領域に向って左側のカメラであり、それぞれCCDカメラ等により構成されている。なお、カメラ3R,3Lの撮像画像は、モノトーン画像及びカラー画像のいずれの画像であってもよい。また、図1(a),(b)では、カメラ3R,3Lは、ロボット1の頭部に搭載されているが、ロボット1の胸部等、他の部位に搭載されていてもよい。
【0092】
ロボット1は、これらのカメラ3R,3Lによって、ロボット1の周辺の移動環境(主に、ロボット1の進行方向前方の環境)を撮像することにより、カメラ3Rの撮像画像とカメラ3Lの撮像画像とから構成されるステレオ画像を随時、取得することが可能となっている。
【0093】
また、本実施形態では、ロボット1は、カメラ3R,3Lによる撮像時に、その撮像領域にテクスチャを付与するための補助光SLを投光器4から投光する。この投光器4は、本実施形態では、図2に示す如く、撮像領域の面(図2では平面)に、互いに平行に並ぶ複数の斜行輝線SLaと、この斜行輝線SLaに対して交差する方向で互いに平行に並ぶ複数の斜行輝線SLbとから構成されるテクスチャを形成する所定色(例えば赤色)のレーザ光を上記補助光SLとして出力する。
【0094】
なお、投光器4により形成するテクスチャは、上記のパターンに限らず、ランダムドットなど、一様でない他のパターンの模様であってもよい。
【0095】
そして、ロボット1は、カメラ3R,3Lによって取得したステレオ画像等を基に、周辺の移動環境の床形状や設置物等を認識しつつ移動する。
【0096】
例えば、図1(a),(b)に示すように、ロボット1の進行方向前方に段差部としての階段50(図1(a)では昇り階段、図1(b)では降り階段)が存在している状況において、ロボット1は、カメラ3R,3Lによって取得した進行方向前方の環境のステレオ画像を基に、カメラ3R,3Lの撮像領域に階段50が存在することや、その階段50の空間的な配置位置を認識する。
【0097】
この場合、本実施形態におけるロボット1では、階段50の空間的な配置位置を認識するために、階段50の各段の踏面51の先端側エッジ52及び基端側エッジ53のそれぞれの空間的な配置位置と、各踏面51を包含する平面の空間的な姿勢(法線方向)とが推定される。
【0098】
ここで、ロボット1が認識する階段50の各踏面51の先端側エッジ52及び基端側エッジ53について補足すると、図1(a)に示すロボット1の如く、階段50をその下段側踏面51が手前側、該下段側踏面51の一段上側の上段側踏面51が奥側となる視線で見た場合においては、各踏面51の先端側エッジ52は、該踏面51の外形の境界線のうち、手前側で横方向(階段50の幅方向)に直線状に延在する境界線である。また、この場合において、各踏面51の基端側エッジ53は、該踏面51の外形の境界線のうち、該踏面51の奥側で横方向に直線状に延在する境界線である。
【0099】
また、図1(b)に示すロボット1の如く、階段50をその上段側踏面51が手前側、該上段側踏面51の一段下側の下段側踏面51が奥側となる視線で見た場合にいては、各踏面51の先端側エッジ52は、上該踏面51の外形の境界線のうち、奥側で横方向(階段50の幅方向)に直線状に延在する境界線である。また、この場合において、各踏面51の基端側エッジ53は、該踏面51の外形の境界線のうち、該踏面51の手前側で横方向に直線状に延在する境界線である。
【0100】
また、段差部としての階段の構造に関して図3を参照して補足すると、階段の各段の踏面の先端部は、一般に段鼻と言われる部分である。従って、階段の踏面の先端側エッジというのは、換言すれば、階段の各段の段鼻の上面の先端側境界線を意味する。また、階段の上段側踏面の先端側と、その一段下側の下段側踏面の基端側との間での部分は、一般に蹴込みと言われる。そして、階段の各段の踏面の先端部(段鼻)は、多くの場合、図3に示す如く、その先端面が、上記蹴込みの箇所に起立した姿勢で形成される面部(以降、蹴込み面という)よりも若干突き出るように形成される。
【0101】
但し、本実施形態で、段差部の例として主に説明する階段では、段鼻の先端面が、蹴込み面よりも若干突き出ている必要はない。例えば、図1(a),(b)に簡略的に記載した階段50のように、段鼻の先端面と蹴込み面とが面一もしくはほぼ面一に形成されていてもよい。
【0102】
また、本実施形態で、段差部の例として主に説明する階段は、蹴込みの箇所に、蹴込み面を形成する板部材が設けられていない(蹴込み面が存在しない)構造のものであってもよい。
【0103】
図4を参照して、ロボット1には、CPU、RAM、ROMを含む電子回路ユニットにより構成された演算処理装置10が搭載されている。そして、この演算処理装置10にカメラ3R,3L及び投光器4が接続されている。
【0104】
この演算処理装置10は、カメラ3R,3L及び投光器4の動作制御を行なう機能を有しており、カメラ3R,3Lの撮像動作と、投光器4の補助光SLの出力動作とを互いに同期した所要のタイミング(所定の制御処理周期のタイミング)で行なわせるように制御する。そして、演算処理装置10は、カメラ3R,3Lによりそれぞれ取得される撮像画像の画素値データ(撮像画像を構成する画素毎の輝度、彩度、色相等を表すデータ)を取り込み、この画素値データを、図示しない画像メモリに逐次更新しつつ、記憶保持する。
【0105】
なお、カメラ3R,3Lに入射する光の波長を制限するバンドパスフィルタを備えておき、補助光SLの波長に一致もしくはほぼ一致する波長の光だけをカメラ3R,3Lで撮像するようにしてもよい。
【0106】
さらに、演算処理装置10は、実装されたプログラムによって実現される機能として、カメラ3R,3Lの撮像領域に段差部が存在しているか否かを検知する段差部存在検知部11と、該撮像領域に存在する実際の段差部の平面部分をステレオ画像のうちの基準画像に投影してなる平面投影領域を抽出すると共に各平面投影領域に対応する平面部分の平面パラメータを決定(推定)する平面投影領域抽出部12と、撮像領域に存在する実際の段差部の踏面の先端側エッジを基準画像に投影してなる実エッジ投影線を推定(特定)するための処理を実行するエッジ投影線推定処理部13と、段差部の空間的な配置位置を特定する段差部空間配置決定部14とを備えている。そして、演算処理装置10は、これらの機能部11〜14の処理によって、本発明の段差部認識装置の一実施形態としての機能を実現する。
【0107】
この場合、本実施形態では、カメラ3Rの撮像画像とカメラ3Lの撮像画像とのうちの一方、例えばカメラ3Rの撮像画像を基準画像、他方のカメラ3Lの撮像画像を参照画像として使用する。なお、演算処理装置10は、ロボット1の動作制御を行なう機能を備えていてもよい。
【0108】
以下に、上記段差部存在検知部11、平面投影領域抽出部12、エッジ投影線推定処理部13、段差部空間配置決定部14の処理を詳細に説明する。
【0109】
カメラ3R,3Lによって取得されたロボット1の進行方向前方の撮像画像(ステレオ画像)の画素値データが段差部存在検知部11に与えられる。そして、段差部存在検知部11は、与えられたステレオ画像を使用して、図5のフローチャートに示す処理を実行する。
【0110】
段差部存在検知部11は、まず、STEP1の処理を実行する。このSTEP1では、段差部存在検知部11は、与えられたステレオ画像のうちの基準画像内に、図6に例示する如く、段差部の存在の有無を検知するための領域である段差部存在検知用領域R1を設定する。
【0111】
図6に示す基準画像は、カメラ3R,3Lの撮像領域に階段50(図示例では昇り階段)が存在する場合の基準画像を概略的に例示したものである。この場合、階段50の表面には前記投光器4から出力された補助光SLによって、テクスチャとしての輝線SLa,SLbが付与される。この輝線SLa,SLbも基準画像に投影される。なお、図6では基準画像に投影される輝線SLa,SLbの一部を代表的に示している。
【0112】
STEP1で段差部存在検知部11が設定する段差部存在検知用領域R1は、本実施形態では、その大きさ(基準画像の横方向及び縦方向での幅)があらかじめ定められた方形状の領域である。この段差部存在検知用領域R1の横方向の幅は、階段等の段差部が基準画像に投影されている場合に、該基準画像における段差部の横方向の幅内に十分に収まる程度の大きさに設定されている。
【0113】
また、段差部存在検知用領域R1の縦方向の幅は、階段等の段差部が基準画像に投影されている場合に、段差部存在検知用領域R1内に複数段の踏面の画像が含まれるような大きさに設定されている。
【0114】
そして、段差部存在検知用領域R1は、基準画像内で、ロボット1の進行予定領域(ロボット1を目標とする進行方向に移動させた場合に該ロボット1が通過する領域)に存在するような位置に配置される。
【0115】
次いで、段差部存在検知部11は、STEP2の処理を実行する。このSTEP2では、段差部存在検知部11は、基準画像内に設定した段差部存在検知用領域R1の画像が、ある平面上の部分の投影画像であると仮定し、この仮定にできるだけ適合するような当該平面の空間的な位置及び姿勢を表す平面パラメータを、射影変換(詳しくは平面射影変換)を利用した手法によって決定する。
【0116】
本実施形態では、上記平面パラメータとして、平面の法線方向の単位ベクトル↑nを、基準画像を撮像するカメラ3R(以降、基準カメラ3ということがある)の光学中心Cから該平面までの距離dにより除算してなるベクトル↑m(≡↑n/d)を使用する。
【0117】
例えば、図7(a)又は図7(b)に二点鎖線で示す平面(図では仮想平面)の位置及び姿勢を表す平面パラメータは、図中に参照符号↑m1を付した矢印で示すように、該平面に垂直で、且つ大きさが1/dのベクトルとして表現される。
【0118】
以降、このように定義されるベクトル↑mを平面パラメータベクトルという。なお、本明細書では、“↑”は、ベクトル(縦ベクトル)を表現する記号として用いる。
【0119】
ここで、上記射影変換は、一般的に言えば、ステレオ画像を構成する2つの撮像画像に、階段の踏面等の平面部分を投影した場合に、一方の撮像画像中における平面部分の画像を、他方の撮像画像における当該平面部分の画像に変換するものである。その変換を行なう射影変換行列Pは、カメラ3R,3Lの2つの撮像画像のそれぞれの画像座標(各撮像画像における各画素の位置を表す二次元座標)を同次座標で表現した場合に、公知のように次式(1−1)により表される行列である。
【0120】
P=R+↑t・↑mT ……(1−1)
【0121】
式(1−1)におけるRは、カメラ3R,3Lのそれぞれのカメラ座標系の間の回転行列、↑tはそれらのカメラ座標系の間の並進移動ベクトル、↑mTは上記平面パラメータベクトル↑mを転置したベクトルである。
【0122】
この場合、カメラ3Rのカメラ座標系は、図7(a),(b)に示す如く、カメラ3Rに対して固定された座標系である。このカメラ座標系は、例えば、カメラ3Rの光学中心Cを原点として、光軸Lcの方向をz軸方向、カメラ3Rの撮像面の上下方向(縦方向)をy軸方向、該撮像面の横方向をx軸方向とする3軸座標系である。左カメラ3Lのカメラ座標系についても同様である。
【0123】
上記回転行列R及び並進移動ベクトル↑tは、カメラ3R,3Lの相互の相対的な位置関係及び姿勢関係に依存して定まるものであり、本実施形態ではあらかじめ特定されている。従って、射影変換行列Pは、平面パラメータベクトル↑mによって規定されるものである。
【0124】
補足すると、基準画像から参照画像への射影変換行列と、これの逆変換の射影変換行列(参照画像から基準画像への射影変換行列)とは、いずれも、式(1−1)の右辺の形で表現される。ただし、上記R,↑t,↑mの値は、それぞれの射影変換行列で異なる。
【0125】
具体的には、式(1−1)により表現される射影変換行列Pを基準画像及び参照画像のうちの一方から他方へのへの射影変換行列とした場合、その逆変換の射影変換行列(以降、これをP-1と表記する)は、次式(1−2)の但し書きの式(1−2a)〜(1−2d)により定義されるR’、↑t’、↑m’、↑d’を用いて、上記式(1−1)と同様の形の式(1−2)により表される。なお、RT、(↑m’)Tはそれぞれ、Rの転値行列、(↑m’)の転値ベクトルである。
【0126】
P-1=R'+↑t’・(↑m’)T ……(1−2)
但し、
R’≡RT ……(1−2a)
↑t’≡−RT・↑t ……(1−2b)
↑m’≡R・↑m/d’ ……(1−2c)
↑d’≡d・(1+↑mT・RT・↑t) ……(1−2d)
【0127】
以降の本実施形態の説明では、便宜上、基準画像から参照画像への射影変換行列を式(1−1)により表される射影変換行列Pとし、参照画像から基準画像への射影変換行列を式(1−2)により表される射影変換行列P-1とする。また、平面パラメータベクトル↑mは、便宜上、基準カメラ3(本実施形態ではカメラ3R)のカメラ座標系で見たベクトルとして表記されるものとする。
【0128】
基準画像における段差部存在検知用領域R1の画像が、ある平面上の部分(平面部分)の投影画像である場合には、その段差部存在検知用領域R1の画像と、該段差部存在検知用領域R1に対応する参照画像中の領域(詳しくは、基準画像の段差部存在検知用領域R1に投影されている実空間(実際の空間)の平面部分を、参照画像に投影してなる領域)の画像との間の関係は、上記射影変換によって対応づけられる。
【0129】
より詳しくは、基準画像の段差部存在検知用領域R1に対応する参照画像中の領域の画像を、当該平面部分の平面パラメータベクトル↑mに応じて上記式(1−2)に規定される射影変換行列P-1により射影変換した場合に得られる射影変換画像の画素値分布は、基準画像の段差部存在検知用領域R1の画像の画素値分布に一致もしくはほぼ一致することとなる。
【0130】
なお、上記射影変換画像の各画素位置(基準画像の画像座標系で見た位置)での画素値は、その画素位置を、当該平面部分の平面パラメータベクトル↑mに応じて上記式(1−1)により規定される射影変換行列Pによって変換してなる参照画像内の画素位置(参照画像の画像座標系で見た位置)の画素値に一致する。従って、上記射影変換画像は、換言すれば、基準画像の段差部存在検知用領域R1の全ての画素位置での画素値を、射影変換行列Pにより該段差部存在検知用領域R1の各画素位置に対応付けられる参照画像内の画素位置の画素値で置き換えてなる画像に一致する。
【0131】
STEP2の処理では、段差部存在検知部11は、基準画像内に設定した段差部存在検知用領域R1に平面部分が投影されていると仮定した場合の当該仮想的な平面(仮想平面)の平面パラメータベクトル↑mを、以下に説明する処理によって決定する。
【0132】
すなわち、段差部存在検知部11は、平面パラメータベクトル↑mの暫定値(候補値)を設定する処理、基準画像の段差部存在検知用領域R1の画像の画素値分布と、設定した↑mの暫定値により規定される射影変換によって、段差部存在検知用領域R1に対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数D1の値を算出する処理、並びに、この誤差関数D1の値に応じて平面パラメータベクトル↑mの暫定値を更新する処理を、既定の所定条件が満たされるまで繰り返す。
【0133】
そして、段差部存在検知部11は、上記所定条件が満たされたときの↑mの暫定値を、基準画像の段差部存在検知用領域R1に投影されている仮想平面の平面パラメータベクトル↑mとして決定する。以降、このように段差部存在検知部11が決定する平面パラメータベクトル↑mを段差部存在検知用平面パラメータベクトル↑m1という。
【0134】
ここで、基準画像の段差部存在検知用領域R1内の任意の画素位置Qでの画素値をI1(Q)、基準画像上の画素位置Qを平面パラメータベクトル↑mの暫定値により規定される前記式(1−1)の射影変換行列Pにより射影変換してなる参照画像上の画素位置をf(Q)、この画素位置f(Q)での参照画像の画素値をI2(f(Q))、段差部存在検知用領域R1内のトータルの画素数をN1とする。このとき、上記誤差関数D1は、例えば次式(2−1)により与えられる関数である。
【0135】
なお、撮像画像(基準画像又は参照画像)の各画素の画素値は、本実施形態では、該画素における撮像画像の輝度値である。ただし、撮像画像がカラー画像である場合には、撮像画像の色相又は彩度を画素値として使用してもよい。
【0136】
D1=(1/N1)・Σ(I1(Q)−I2(f(Q)))2 ……(2−1)
【0137】
式(2−1)のΣは、段差部存在検知用領域R1内の全ての画素についての総和を意味する。従って、式(2−1)により定義される誤差関数D1は、基準画像の段差部存在検知用領域R1での、画素値I1(Q)と、これに対応する参照画像の射影変換画像の画素値I2(f(Q))との差の二乗値の平均値を、段差部存在検知用領域R1における基準画像の画素値分布と、これに対応する参照画像の射影変換画像の画素値分布との誤差として表現する関数である。
【0138】
なお、式(2−1)により与えられる誤差関数Dの代わりに、例えば次式(2−2)により与えられる誤差関数D1を使用してもよい。
【0139】
D1=(1/N1)・Σ|(I1(Q)−I2(f(Q))| ……(2−2)
【0140】
この式(2−2)により表される誤差関数D1は、基準画像の段差部存在検知用領域R1での、I1(Q)とI2(f(Q))との差の絶対値の平均値を、段差部存在検知用領域R1における基準画像の画素値分布と、これに対応する参照画像の射影変換画像の画素値分布との誤差として表現する関数である。
【0141】
上記の如く算出される誤差関数D1の値は、↑mの暫定値に依存して変化する。そして、段差部存在検知用領域R1に投影されている部分が、実際の平面部分である場合には、↑mの暫定値が該平面部分を包含する平面の平面パラメータベクトルに一致する場合に、誤差関数D1の値は最小(ほぼゼロ)になる。
【0142】
従って、誤差関数D1の値が最小(もしくはほぼ最小)となるような↑mの暫定値が、段差部存在検知用領域R1に投影されている仮想的な平面部分を包含する仮想平面の位置及び姿勢を表す平面パラメータベクトルとしての適合度が高いものとなる。
【0143】
そこで、段差部存在検知部11は、この誤差関数D1に応じて↑mの暫定値を更新する処理では、公知の収束演算処理手法によって、この誤差関数D1の値を、その最小値により近づけ得るように↑mの暫定値を更新する。そして、段差部存在検知部11は、このような↑mの暫定値の更新を所定条件が成立するまで実行することで、最終的に、上記誤差関数D1の値を最小化(もしくはほぼ最小化)し得るような前記段差部検知用平面パラメータベクトル↑m1を決定する。
【0144】
この場合、上記所定条件は、その条件が成立する場合に、更新後の↑mが、誤差関数Dを最小化し得る値に一致もしくはほぼ一致するとみなし得るものとして、あらかじめ定められた条件である。該所定条件は、例えば、更新前の↑mに応じて算出される誤差関数D1の値と、更新後の↑mに応じて算出される誤差関数D1の値との差の絶対値が、あらかじめ定められた所定の閾値(“0”に近い閾値)以下となるという条件、あるいは、↑mの暫定値の更新回数が所定回数を超えるという条件等の条件である。
【0145】
なお、↑mの暫定値の初期値としては、例えば、ロボット1が現在接地している面(床面等)の平面パラメータベクトルを使用することができる。ただし、該初期値は、あらかじめ定めた固定値であってもよい。また、該初期値は、ロボット1の現在の接地面に対して傾いた平面の平面パラメータベクトルであってもよい。
【0146】
次いで、段差部存在検知部11は、STEP3の処理を実行する。このSTEP3では、段差部存在検知部11は、STEP2で決定した段差部存在検知用平面パラメータベクトル↑m1の向きに基づいて、カメラ3R,3Lの撮像領域内に段差部が存在しているか否かを判断する。
【0147】
ここで、カメラ3R,3Lの撮像領域に段差部が存在せず、該撮像領域がほぼ平坦な床面である場合には、STEP2において上記の如く決定された段差部存在検知用平面パラメータベクトル↑m1の向きは、ロボット1の現在の接地面の法線方向とほぼ同方向になる。
【0148】
一方、カメラ3R,3Lの撮像領域に階段等の段差部が存在している場合には、STEP2で決定した段差部存在検知用平面パラメータベクトル↑m1の向きは、ロボット1の現在の接地面の法線方向に対して比較的大きく傾いたものとなる。
【0149】
例えば図7(a)に示すように、カメラ3R,3Lの撮像領域に昇り階段50が存在する場合、あるいは、図7(b)に示すように、カメラ3R,3Lの撮像領域に降り階段50が存在する場合には、段差部存在検知部11が決定する段差部存在検知用平面パラメータベクトル↑m1の向きは、ピッチ方向(ロボット1の進行方向を横切る方向(ロボット1の左右方向)の軸周り方向)において、ロボット1の現在の接地面の法線方向に対して比較的大きく傾くものとなる。
【0150】
そこで、段差部存在検知部11は、STEP3では、決定した段差部存在検知用平面パラメータベクトル↑m1が、ロボット1の現在の接地面の法線方向に対してピッチ方向に所定角度以上、傾いているか否かによって、カメラ3R,3Lの撮像領域に階段等の段差部が存在しているか否かを判断する。
【0151】
具体的には、段差部存在検知部11は、ロボット1の現在の接地面の法線方向に対する↑m1の傾き角度(ピッチ方向での傾き角度)の大きさが、既定の所定角度以上である場合には、カメラ3R,3Lの撮像領域に階段等の段差部が存在していると判断する。また、段差部存在検知部11は、ロボット1の現在の接地面の法線方向に対する↑m1の傾き角度(ピッチ方向での傾き角度)が所定角度未満である場合には、カメラ3R,3Lの撮像領域に階段等の段差部が存在していないと判断する。
【0152】
なお、ロボット1の現在の接地面と、カメラ3R,3Lとの間の相対的な位置及び姿勢関係は、ロボット1の各関節の変位量(詳しくは、ロボット1の接地している脚とカメラ3R,3Lとの間に存在する各関節の変位量)に応じて定まる。従って、ロボット1の現在の接地面の法線方向に対する↑m1の傾き角度は、基準カメラ3(カメラ3R)のカメラ座標系で見た↑m1の値と、ロボット1の各関節の変位量の計測値(又は該変位量を追従させる目標値)とから算出することができる。
【0153】
そして、段差部存在検知部11は、STEP3の判断結果が肯定的である場合(段差部が存在していると判断した場合)には、さらにSTEP4の処理を実行する。このSTEP4では、カメラ3R,3Lの撮像領域に存在する段差部の種別(昇りの段差部であるか降りの段差部であるかの種別)を判断する。
【0154】
ここで、カメラ3R,3Lの撮像領域に、図7(a)に示す如く、昇りの段差部(図では昇り階段50)が存在する場合には、段差部存在検知部11が決定する段差部存在検知用平面パラメータベクトル↑m1は、ロボット1から見て、昇り勾配斜面の平面パラメータベクトルとなる。また、カメラ3R,3Lの撮像領域に、図7(b)に示す如く、降りの段差部(図では降り階段50)が存在する場合には、段差部存在検知部11が決定する段差部存在検知用平面パラメータベクトル↑m1は、ロボット1から見て、降り勾配斜面の平面パラメータベクトルとなる。
【0155】
そこで、STEP4では、段差部存在検知部11は、↑m1が、ロボット1の現在の接地面の法線方向に対して、ピッチ方向で正負のいずれの向きに傾いているかによって、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する。
【0156】
以上が、段差部存在検知部11の処理の詳細である。
【0157】
段差部存在検知部11により、カメラ3R,3Lの撮像領域に段差部が存在することが検知された場合には、演算処理装置10は、次に、平面投影領域抽出部12の処理を実行する。
【0158】
この平面投影領域抽出部12は、カメラ3R,3Lによって取得された撮像画像(ステレオ画像)を用いて、図8のフローチャートに示す処理を実行する。
【0159】
平面投影領域抽出部12は、まず、STEP11の処理を実行する。このSTEP11では、平面投影領域抽出部12は、与えられたステレオ画像のうちの基準画像内の複数の(N2個の)位置(u1,v(i))(i=1,2,…,N2)に、微小な面積の領域である局所領域R2(i)を設定する。
【0160】
各局所領域R2(i)は、図9に示す如く例えば方形状の領域である。そして、各局所領域R2(i)のサイズ(縦方向及び横方向の幅)は、基準画像に段差部の画像が投影されている場合に、該段差部の各段の踏面内に収まり得るようなサイズにあらかじめ設定されている。
【0161】
なお、図9では、前記投光器4から出力される補助光SLによる斜行輝線SLa,SLbの図示は省略している。このことは、後述する図13、図19においても同様である。
【0162】
補足すると、各局所領域R2(i)の形状は、方形状でなくともよく、例えば、平行四辺形や台形等の形状であってもよい。
【0163】
各局所領域R2(i)の配置位置(u1,v(i))は、基準画像の横方向をu軸、縦方向(上下方向)をv軸とする画像座標系で表した、各局所領域R2(i)の代表点の位置である。その代表点は、図9に示す例では、各局所領域R2(i)の左上隅の点である。ただし、該代表点は、各局所領域R2(i)の他の点、例えば各局所領域R2(i)の中央点、右上隅の点、左下隅の点、右下隅の点等であってもよい。
【0164】
本実施形態では、各局所領域R2(i)の配置位置(u1,v(i))のうち、基準画像の横方向(u軸方向)での位置u1は互いに同一の位置である。その位置u1は、基準画像におけるロボット1の通過予定領域上に各局所領域R2(i)が配置されるように設定される。例えば、位置u1は、ロボット1の通過予定領域の幅方向中央付近の位置に設定される。
【0165】
また、各局所領域R2(i)の配置位置(u1,v(i))のうち、基準画像の縦方向(v軸方向)の位置v(i)(i=1,2,…,N2)は、基準画像の下部から上部まで、所定の微小間隔(例えば1画素分の間隔)を存して並ぶようにあらかじめ設定された位置である。
【0166】
そして、STEP11では、平面投影領域抽出部12は、上記の如く配置位置が決定された各局所領域R2(i)のそれぞれに対して、基準画像の各局所領域R2(i)に投影されている部分が平面部分であると仮定した場合の当該平面部分の平面パラメータベクトル↑mを算出する。以降の説明では、各局所領域R2(i)(i=1,2,…,N2)のそれぞれに対応して算出される平面パラメータベクトル↑mを参照符号↑m_r2(i)を用いて表記する。
【0167】
この場合、各局所領域R2(i)(i=1,2,…,N2)のそれぞれに対応する平面パラメータベクトル↑m_r2(i)の算出は、前記段差部存在検知部11によるSTEP2の処理と同様の処理によって行なわれる。この場合、↑m_r2(i)の算出処理は、前記段差部存在検知用領域R1の代わりに、局所領域R2(i)を使用する(前記誤差関数D1の値を算出するための領域としてR2(i)を使用する)こと以外は、STEP2の処理と同じでよい。
【0168】
次いで、平面投影領域抽出部12は、STEP12の処理を実行する。このSTEP12では、平面投影領域抽出部12は、上記の如く算出した各局所領域R2(i)(i=1,2,…,N2)に対応する平面パラメータベクトル↑m_r2(i)から、この↑m_r2(i)により位置及び姿勢が規定される平面(R2(i)に投影されているとみなす仮想的な平面)の法線方向の単位ベクトルである単位法線ベクトル↑n_r2(i)を算出する。
【0169】
この単位法線ベクトル↑n_r2(i)は、平面パラメータベクトル↑m_r2(i)と同じ向きを有する単位長のベクトルであるから、↑m_r2(i)を、その大きさ(=|↑m_r2(i)|)により除算することによって算出される。
【0170】
次いで、平面投影領域抽出部12は、STEP13の処理を実行する。このSTEP13では、平面投影領域抽出部12は、各局所領域R2(i)における単位法線ベクトル↑n_r2(i)と、現在のロボット座標系のZ軸方向の単位ベクトル↑nzとの内積(スカラー積)を算出する。
【0171】
上記ロボット座標系というのは、図7(a)又は図7(b)に示す如く、ロボット1の現在の接地面に対して固定された座標系であって、該接地面の法線方向をZ軸方向、ロボット1の進行方向をX軸方向、これらのZ軸及びX軸に直交する方向(ロボット1の左右方向)をY軸方向とする座標系である。従って、現在のロボット座標系のZ軸方向の単位ベクトル↑nzというのは、ロボット1の現在の接地面の法線方向の単位ベクトルを意味する。
【0172】
そして、この単位ベクトル↑nzと、単位法線ベクトル↑n_r2(i)との内積を算出する場合、基準カメラ3のカメラ座標系で見たベクトルとしての単位法線ベクトル↑n_r2(i)が、ロボット1の各関節の変位量の計測値(又は目標値)に応じて、現在のロボット座標系で見たベクトルに座標変換される。そして、この座標変換後の単位法線ベクトル↑n_r2(i)と、ロボット1の現在の接地面の法線方向(現在のロボット座標系のZ軸方向)の単位ベクトル↑nzとの内積が算出される。
【0173】
なお、カメラ座標系で見た単位法線ベクトル↑n_r2(i)と、ロボット1の現在の接地面の法線方向の単位ベクトル↑nzをロボット座標系からカメラ座標系に座標変換したベクトルとの内積を算出するようにしてもよい。
【0174】
これにより、図10に例示する如く、基準画像の縦方向での各局所領域R2(i)の位置v(i)と、上記内積の値との間の関係を表すデータが得られることとなる。
【0175】
ここで、カメラ3R,3Lの撮像領域に段差部が存在する場合、その段差部の各踏面は、一般に、ロボット1の現在の接地面とほぼ平行な平面部分となる。従って、基準画像における段差部の踏面の投影領域内に含まれる各局所領域R2(i)(例えば、図9に示すR2(1),R2(ib),R2(N2))に対応して上記の如く算出される内積の値は、“1”またはそれに近い値となる。
【0176】
一方、段差部の踏面の投影領域に含まれない各局所領域R2(i)、又は、段差部の踏面の投影領域が一部しか含まれないような各局所領域R2(i)(例えば、図9に示すR2(ia))に対応する内積の値は、“1”から乖離した値となる。
【0177】
従って、上記内積の値は、各局所領域R2(i)に投影されている部分が、段差部の踏面の部分であるか否かを示す指標となるものである。
【0178】
次いで、平面投影領域抽出部12は、STEP14の処理を実行する。このSTEP14では、平面投影領域抽出部12は、N2個の局所領域R2(i)(i=1,2,…,N2)のうち、STEP13で算出した内積の値が、あらかじめ定めれた所定の閾値Th2以上となる各局所領域R2(i)について、該局所領域R2(i)に投影されている部分の、ロボット座標系で見たZ軸座標位置(すなわち、ロボット1の現在の接地面に対する当該投影部分の高さ)を、該局所領域R2(i)に対応する平面パラメータベクトル↑m_r2(i)を基に算出する。
【0179】
ここで、上記所定の閾値Th2は、“1”よりも若干小さい値、例えば0.999である。従って、内積の値が、この閾値Th2以上となる各局所領域R2(i)は、それに投影されている部分が、ロボット1の現在の接地面とほぼ平行になっているような部分である。
【0180】
そして、閾値Th2以上となる各局所領域R2(i)に対応する平面パラメータベクトル↑m_r2(i)により規定される平面と、ロボット1の現在の接地面を含む平面との間の間隔(詳しくは、ロボット座標系のZ軸方向での間隔)を算出することによって、該局所領域R2(i)に投影されている部分のZ軸座標位置(ロボット1の現在の接地面に対する高さ)が決定される。
【0181】
次いで、平面投影領域抽出部12は、STEP15の処理を実行する。このSTEP15では、平面投影領域抽出部12は、前記STEP14で得られたデータを基に、基準画像における段差部の踏面に対応する平面投影領域を抽出(特定)する。
【0182】
具体的には、平面投影領域抽出部12は、内積の値が前記閾値Th2以上となるv軸方向の区間であって、その区間内の各位置の局所領域R2(i)に投影されている部分の高さ(ロボット1の接地面からの高さ)がほぼ一定となる(各投影部分の相互の高さの差の大きさがあらかじめ定めた所定の閾値以下となる)区間を決定する。なお、各投影部分の高さの相互の差の大きさ(絶対値)に対する閾値は、“0”に近い微小値、例えば10[mm]である。
【0183】
図10に示すΔv1、Δv2、Δv3は、このようにして決定される区間の例を示すものである。このようにして決定される各区間は、STEP11で設定したN2個の局所領域R2(i)(i=1,2,…,N2)のうち、ロボット1の現在の接地面とほぼ平行な単一の平面部分(段差部の各踏面に相当する平面部分)の投影領域となっている局所領域R2(i)の代表点の位置(v軸方向の位置)の範囲に相当するものである。以降、このようにして決定される各区間を同一平面投影区間という。
【0184】
そして、平面投影領域抽出部12は、上記の如く決定した各同一平面投影区間毎に、それに対応する平面投影領域を決定する。この場合、本実施形態では、各平面投影領域は、基準画像の横方向(u軸方向)に局所領域R2(i)(i=1,2,…,N2)と同じ幅を有し、且つ、その横方向の範囲(各平面投影領域の左端と右端との間の範囲)が局所領域R2(i)の横方向の範囲に一致するようにして、基準画像に配置される方形状の領域である。
【0185】
そして、基準画像の縦方向(v軸方向)での各平面投影領域の範囲(各平面投影領域の上端と下端との間の範囲)は、該平面投影領域に対応する同一平面投影区間内に代表点の位置を有する全ての局所領域R2(i)を合成した領域の縦方向の範囲と一致するように決定される。
【0186】
例えば、基準画像のv軸方向の位置がv(ic)からv(ic+n)までの区間が、ある1つの同一平面投影区間であるとしたとき、その同一平面投影区間に対応する平面投影領域の縦方向(v軸方向)の範囲は、該同一平面投影区間内に代表点の位置を有するn+1個の局所領域R2(ic)〜R2(ic+n)のうち、最も下側の局所領域R2(ic)の下端位置と、最も上側の局所領域R2(ic+n)の上端位置との間の範囲として決定される。
【0187】
以上のSTEP15の処理によって、段差部の各踏面に対応する平面投影領域が、基準画像から抽出されることとなる。例えば、図9に破線枠で示す方形状の領域AR2が、平面投影領域として基準画像から抽出される。
【0188】
次いで、平面投影領域抽出部12は、STEP16の処理を実行する。このSTEP16では、平面投影領域抽出部12は、STEP15の処理によって2つ以上の平面投影領域AR2が抽出されたか否かを判断する。そして、この判断結果が否定的である場合には、基準画像に段差部が写っていない可能性があるので、図8の処理(今回の演算処理周期での処理)を終了する。
【0189】
一方、STEP16の判断結果が肯定的である場合には、平面投影領域抽出部12は、STEP17の処理を実行した後、図8の処理を終了する。このSTEP17では、平面投影領域抽出部12は、各平面投影領域AR2に投影されている平面部分を包含する平面の平面パラメータベクトル↑m2を決定する。
【0190】
この場合、平面投影領域抽出部12は、例えば、前記局所領域R2(i)(i=1,2,…,N2)のうち、各平面投影領域AR2に含まれる局所領域R2(i)のそれぞれに対応する平面パラメータベクトル↑m_r2(i)のそれぞれの方位角(基準カメラ3のカメラ座標系で見た方位角)の平均の方位角を有する単位ベクトルを、該平面投影領域AR2に対応する平面の単位法線ベクトル↑n2として決定する。
【0191】
さらに、平面投影領域抽出部12は、各平面投影領域AR2に含まれる局所領域R2(i)のそれぞれに対応して、前記STEP11で推定した平面パラメータベクトル↑m_r2(i)の大きさ(絶対値)の逆数値(=1/|↑m(i)|)の平均値を、該平面投影領域AR2に対応する平面の、基準カメラ3の光学中心Cからの距離d2として決定する。
【0192】
そして、各平面投影領域AR2に対応する平面の単位法線ベクトル↑n2を距離d2で除算してなるベクトル(=↑n2/d2)を該平面投影領域AR2に対応する平面パラメータベクトル↑m2として決定する。
【0193】
なお、各局所領域R2(i)に対応する平面パラメータベクトル↑m_r2(i)を推定する場合と同様の処理(射影変換を利用した収束演算処理)によって、各平面投影領域AR2に対応する平面パラメータベクトル↑m2を改めて推定するようにしてもよい。
【0194】
以上が平面投影領域抽出部12の処理の詳細である。
【0195】
補足すると、本実施形態では、STEP15の処理で前記同一平面投影区間を決定する場合に、各局所領域R2(i)(i=1,2,…,N2)毎の前記内積の値に加えて、各局所領域R2(i)に投影されている部分の高さを参照するようにしたが、内積の値だけから、同一平面投影区間を決定するようにしてもよい。
【0196】
例えば、内積の値が、基準画像の縦方向(v軸方向)で連続的に所定の閾値Th2以上となる区間(その区間内に代表点の位置を有して、縦方向に並ぶ全ての局所領域R2(i)のそれぞれに対応する内積の値が閾値Th2以上となる区間)を同一平面投影区間として決定してもよい。このようにした場合には、STEP14の処理は不要である。
【0197】
また、カメラ3R,3Lの撮像領域に存在する段差部の踏面に対応する平面投影領域だけでなく、段鼻の先端面又は蹴り込み面のように、踏面に対して起立する平面部分に対応する平面投影領域を抽出するようにしてもよい。
【0198】
この場合、例えば各局所領域R2(i)(i=1,2,…,N2)毎の平面パラメータベクトル↑m_r2(i)(又は単位法線ベクトル↑n_r2(i))の向きに着目し、その向きが連続的にほぼ一定に保たれる(ロボット1の現在の接地面の法線方向と同方向の向きでなくてもよい)ような区間を同一平面投影区間として決定する。そして、その決定した各同一平面投影区間に応じて上記と同様に平面投影領域を決定する。これにより、段差部の踏面に対応する平面投影領域だけでなく、段鼻の先端面や段差部の蹴り込み面の如く、踏面に対して起立する平面部分に対応する平面投影領域を抽出することもできる。
【0199】
以上説明した平面投影領域抽出部12によって、段差部の踏面に対応する2つ以上の平面投影領域AR2が抽出された場合には、演算処理装置10は、次に、エッジ投影線推定処理部13の処理を実行する。
【0200】
このエッジ投影線推定処理部13は、カメラ3R,3Lによって取得された撮像画像(ステレオ画像)と、平面投影領域抽出部12で決定された各平面投影領域AR2に対応する平面パラメータベクトル↑m2とを用いて、図11及び図12のフローチャートに示す処理を実行することで、上段側踏面の先端側エッジを基準画像に投影してなる実エッジ投影線を推定したものとしての推定エッジ投影線を特定する。
【0201】
なお、基準画像における実エッジ投影線を推定するということは、基準画像における実エッジ投影線の位置及び方向(又はこれらを規定するパラメータ)を決定することを意味する。
【0202】
エッジ投影線推定処理部13は、まず、STEP21の処理を実行する。このSTEP21では、エッジ投影線推定処理部13は、図5のSTEP4の処理によって前記段差部存在検知部11が特定した段差部(カメラ3R,3Lの撮像領域に存在する段差部)の種別が、ロボット1から見て、昇りの段差部であるか降りの段差部であるかを確認する。
【0203】
エッジ投影線推定処理部13は、カメラ3R,3Lの撮像領域に存在する段差部が、昇りの段差部である場合には、図11に示すSTEP22〜36の処理を実行し、降りの段差部である場合には、図12に示すSTEP37〜51の処理を実行する。
【0204】
段差部が昇りの段差部である場合には、エッジ投影線推定処理部13は、まず、STEP22において、基準画像内に、実エッジ投影線の推定用の画像処理領域であるエッジ推定用領域R3uを設定する。
【0205】
ここで、以降の説明では、説明の便宜上、カメラ3R,3Lの撮像領域に存在する昇りの段差部が、一例として、図1(a)に示した如き昇り階段50であるとする。そして、この昇り階段50が、図13に示す如く基準画像に写っているものとする。
【0206】
さらに、この昇り階段50の任意の1つの下段側踏面51(例えばロボット1から見て最も手前側の下段側踏面)の一段上側の上段側踏面51の先端側エッジ52を基準画像に投影してなる実エッジ投影線L52rを推定する処理をエッジ投影線推定処理部13により実行するものとする。
【0207】
そして、推定しようとする実エッジ投影線L52rに対応する先端側エッジ52を先端に有する上段側踏面51と、その一段下側の下段側踏面51とを、それぞれ参照符号51p,51qを使用して表記する。
【0208】
また、前記平面投影領域抽出部12によって、上段側踏面51pに対応して抽出された平面投影領域AR2を、以降、参照符号AR2pを使用して上段側踏面投影領域AR2pと称する。そして、この上段側踏面投影領域AR2pに対応して決定された平面パラメータベクトル↑m2を参照符号↑m2pを使用して表記する。
【0209】
また、前記平面投影領域抽出部12によって、下段側踏面51qに対応して抽出された平面投影領域AR2を、以降、参照符号AR2qを使用して下段側踏面投影領域AR2qと称する。そして、この下段側踏面投影領域AR2qに対応して決定された平面パラメータベクトル↑m2を参照符号↑m2qを使用して表記する。
【0210】
STEP22において基準画像に設定するエッジ推定用領域R3uは、本実施形態では、図13に示す如く設定される方形状の領域である。具体的には、エッジ推定用領域R3uは、その下端の位置が下段側踏面投影領域AR2qの縦方向の幅内の位置(例えば下段側踏面投影領域AR2qの上端から既定の所定量だけ下側の位置)となり、且つ、R3uの上端の位置が上段側踏面投影領域AR2pの縦方向の幅内の位置(例えば上段側踏面投影領域AR2pの下端から所定量だけ上側の位置)になるように配置される。この場合、エッジ推定用領域R3uの縦方向の幅は、下段側踏面投影領域AR2qと上段側踏面投影領域AR2pとの間の縦方向の間隔に応じて決定される。
【0211】
また、エッジ推定用領域R3uは、その横方向の範囲(左端と右端との間の範囲)が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲に一致するように配置される。この場合、エッジ推定用領域R3uの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅と同じ幅(既定の幅)に決定される。
【0212】
なお、エッジ推定用領域R3uの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅よりも若干小さくてもよい。その場合は、エッジ推定用領域R3uは、その横方向の範囲が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲内に収まるように配置すればよい。
【0213】
以上のSTEP22の処理によって、エッジ推定用領域R3uは、その内部に(上端の辺と下端の辺との間)に推定しようとする単一の実エッジ投影線L52rが存在し、且つ、該エッジ推定用領域R3uの上端の辺及び下端の辺がそれぞれ、上段側踏面投影領域AR2p、下段側踏面投影領域AR2qに含まれるようにして、基準画像内に設定されることとなる。
【0214】
エッジ投影線推定処理部13は、次にSTEP23の処理を実行する。このSTEP23では、エッジ投影線推定処理部13は、実エッジ投影線L52rが基準画像の横方向(u軸方向)に延在するものと仮定して、図14に示す如く、基準画像の縦方向(v軸方向)における実エッジ投影線L52rの位置の候補としての複数(N3a個)の候補位置v(k1)(k1=1,2,…,N3a)を決定する。
【0215】
この場合、N3a個の候補位置v(k1)(k1=1,2,…,N3a)は、エッジ推定用領域R3uの縦方向の範囲(上端と下端との間の範囲)内で、あらかじめ定められた所定の刻み間隔ずつ(例えば1画素分の間隔ずつ)、基準画像の縦方向に異なる位置とされる。また、これらの候補位置v(k1)のうち、最も上側の候補位置としての第1番目の候補位置v(1)は、エッジ推定用領域R3uの上端から所定間隔(例えば1画素分の間隔)だけ下側の位置に決定される。
【0216】
さらに、最も下側の候補位置としての第N3a番目の候補位置v(N3a)は、本実施形態では、この候補位置v(N3a)に応じて後述する射影変換領域R3u3の上端位置を設定したときに、該射影変換領域R3u3の上端位置が、エッジ推定用領域R3uの下端から、該エッジ推定用領域R3u3の縦方向の所定の最小幅(例えば1画素分の幅)だけ上側の位置になるように、該エッジ推定用領域R3u3の縦方向の最小幅での上端位置に応じて決定される。
【0217】
次に、エッジ投影線推定処理部13は、STEP24の処理を実行する。この処理では、各候補位置v(k1)に対応する処理を順番に実行するために、k1の初期値として“1”が設定される。
【0218】
次いで、エッジ投影線推定処理部13は、STEP25の処理を実行する。このSTEP25では、エッジ投影線推定処理部13は、基準画像の縦方向での実エッジ投影線L52rの位置が第k1番目の候補位置v(k1)(k1は現在設定されている値)に一致するものと仮定して、エッジ推定用領域R3u内に、図14に例示する如く、3つの射影変換対象領域R3u1、R3u2、R3u3を設定する。
【0219】
射影変換対象領域R3u1〜R3u3のうち、射影変換対象領域R3u1,R3u2は、第k1番目の候補位置v(k1)で横方向に延在する仮想的なエッジ投影線L52(k1)を境界線として上下に隣接するように設定される領域である。この場合、上側の射影変換対象領域R3u1は、その上端側の辺と下端側の辺とがそれぞれエッジ推定用領域R3uの上端側の辺、仮想的なエッジ投影線L52(k1)に一致し、且つ、横方向の範囲が、エッジ推定用領域R3uの横方向の範囲と一致する領域である。
【0220】
従って、射影変換対象領域R3u1は、その縦方向の幅と下端側の辺の位置(縦方向での位置)とが、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3u1は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、上段側踏面投影領域AR2pに含まれる領域である。
【0221】
また、下側の射影変換対象領域R3u2は、その上端側の辺が仮想的なエッジ投影線L52(k1)に一致すると共に、下端側の辺が仮想的なエッジ投影線L52(k1)とあらかじめ定められた所定間隔Δvu2(例えば15画素分の間隔)を有して該エッジ投影線L52(k1)に平行な線に一致し、且つ、横方向の範囲が、エッジ推定用領域R3uの横方向の範囲と一致する領域である。
【0222】
従って、射影変換対象領域R3u2は、その上端側の辺の位置と下端側の辺の位置とが、それらの間の間隔(すなわちR3u2の縦方向の幅)を一定の間隔Δvu2に保持しつつ、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3u2は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、段鼻の先端面や蹴り込み面のように、昇り階段50の上段側踏面51pの先端側で該上段側踏面51pに対して起立した姿勢で形成される平面部分が投影される領域である。
【0223】
また、射影変換対象領域R3u3は、上記射影変換領域R3u2よりも下側で、エッジ推定用領域R3uの下部に設定される方形状の領域である。この射影変換領域R3u3は、その下端側の辺がエッジ推定用領域R3uの下端側の辺に一致し、且つ、横方向の範囲が、エッジ推定用領域R3uの横方向の範囲と一致する領域である。
【0224】
そして、射影変換対象領域R3u3の上端側の辺の位置は、本実施形態では、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて次のように設定される。
【0225】
すなわち、エッジ投影線推定処理部13は、まず、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pを用いて、仮想的なエッジ投影線L52(k1)に対応する上段側踏面51pの先端側エッジ線(先端側エッジ52を包含する空間的な直線)を求める。この先端側エッジ線は、基準カメラ3の光学中心Cと基準画像の撮像面上の仮想的なエッジ投影線L52(k1)とを包含する平面と、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pにより規定される平面(上段側踏面51pを包含する平面)との交線として算出される。
【0226】
そして、エッジ投影線推定処理部13は、上記の如く求めた先端側エッジ線を含み、且つ、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pにより規定される平面に対して直交する平面(あるいは↑m2pにより規定される平面に対して、90度に近い既定の角度で交差する平面)を決定する。この平面は、階段の段鼻の先端面又は蹴り込み面に相当する仮想的な平面(以降、起立平面ということがある)である。
【0227】
さらに、エッジ投影線推定処理部13は、上記の如く決定した起立平面と、下段側踏面投影領域AR2qに対応する平面パラメータベクトル↑m2qにより規定される平面との交線を、基準画像に投影してなる線に、射影変換対象領域R3u3の上端側の辺が合致するように該上端側の辺の位置を設定する。
【0228】
従って、射影変換対象領域R3u3の上端側の辺は、下段側踏面51qの基端側エッジ53が、上段側踏面51pの先端側エッジ52のほぼ直下に存在するものと仮定した場合における該基端側エッジ53を基準画像に投影してなる線に相当するものとなる。この射影変換対象領域R3u3は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、下段側踏面投影領域AR2qに含まれる領域である。
【0229】
なお、前記平面投影領域抽出部12の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトルにより規定される平面を上記起立平面として使用して、射影変換対象領域R3u3の上端の辺の位置を決定するようにしてもよい。
【0230】
補足すると、段差部の上段側踏面の先端側で該上段側踏面に対して起立する姿勢となる平面部分は、蹴り込みの箇所に板部材が設けられていないような段差部にあっては、段鼻の先端面に相当するものとなる。従って、該平面部分の上下方向の幅は、上段側踏面と下段側踏面との間隔に比して小さなものとなる。さらに、この場合には、段差部の蹴り込みの箇所には、平面部分が存在しないこととなる。
【0231】
そこで、本実施形態では、前記射影変換対象領域R3u2の縦方向の幅Δvu2を比較的小さな幅とし、射影変換対象領域R3u2の下端の辺と、その下側の射影変換対象領域R3u3の上端の辺との間に間隔を設けるようにしている。
【0232】
なお、基準画像に段鼻の先端面に相当する部分が投影されている場合、該基準画像における段鼻の画像の縦方向の幅は、基準カメラ3の光学中心Cから段鼻までの距離や、基準カメラ3の光軸Lcに対する段鼻の方位角(基準画像の縦方向での方位角)に応じて変化する。従って、前記射影変換対象領域R3u2の縦方向の幅Δvu2は、基準カメラ3の光学中心Cから段鼻の近辺に存在する部分(例えば上段側踏面投影領域AR2pの下端に対応する上段側踏面51上の点)までの距離や、基準カメラ3の光軸Lcに対する該部分の方位角等に応じて可変的に設定するようにしてもよい。また、カメラ3R,3Lの解像度や撮像画像の拡縮の倍率等に応じて射影変換対象領域R3u2の縦方向の幅Δvu2を異ならせるようにしてもよい。
【0233】
以上の如くSTEP25の処理を実行した後、次に、エッジ投影線推定処理部13は、STEP26の処理を実行する。このSTEP26では、エッジ推定用領域R3uに設定した3つの射影変換対象領域R3u1,R3u2,R3u3のそれぞれが平面部分の投影領域であると仮定して、射影変換対象領域R3u1,R3u2,R3u3のそれぞれに対応する平面部分を包含する平面についての平面パラメータベクトル↑mu1,↑mu2,↑mu3を決定する。
【0234】
この場合、射影変換対象領域R3u1,R3u3にそれぞれ対応する平面パラメータベクトル↑mu1,↑mu3は、それぞれ、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2p、下段側踏面投影領域AR2qに対応する平面パラメータベクトル↑m2qに一致するベクトルに決定される。
【0235】
また、射影変換対象領域R3u2に対応する平面パラメータベクトル↑mu2は、前記STEP25の処理に関して説明した起立平面(仮想的なエッジ投影線L52(k1)に対応する先端側エッジ線を含み、上段側踏面51pを包含する平面に対して直交又はほぼ直交する平面)の平面パラメータベクトル↑mに一致するベクトルに決定される。
【0236】
なお、前記平面投影領域抽出部12の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトル↑mに、↑mu2を一致させるようにしてもよい。
【0237】
次いで、エッジ投影線推定処理部13は、STEP27の処理を実行する。このSTEP27では、エッジ投影線推定処理部13は、各射影変換対象領域R3u1,R3u2,R3u3について、各射影変換対象領域R3u1,R3u2,R3u3の画像(基準画像)と、これに対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像との誤差を表す誤差画像を作成する。
【0238】
各射影変換対象領域R3u1,R3u2,R3u3に対応する誤差画像は次のように作成される。例えば射影変換対象領域R3u1に対応す誤差画像の作成処理を代表的に説明すると、基準画像の射影変換対象領域R3u1内の任意の位置Qでの画素値をI1(Q)、基準画像内の位置Qを射影変換対象領域R3u1に対応する平面パラメータベクトル↑mu1により規定される前記式(1−1)の射影変換行列Pにより射影変換してなる参照画像内の位置をf(Q)、参照画像内の位置f(Q)での画素値をI2(f(Q))とおく。
【0239】
このとき、射影変換対象領域R3u1に対応する誤差画像は、射影変換対象領域R3u1の各位置Qの画素値を、I1(Q)とI2(f(Q))との差の二乗値(=(I1(Q)−I2(f(Q)))2)又は絶対値(=|I1(Q)−I2(f(Q))|)に置き換えた画像として作成される。
【0240】
射影変換対象領域R3u2,R3u3のそれぞれに対応する誤差画像についても同様である。ただし、この場合、参照画像中の位置f(Q)を求めるための射影変換に使用する平面パラメータベクトルは、↑mu1の代わりに、それぞれの領域に対応して決定された平面パラメータベクトル↑mu2,↑mu3が使用される。
【0241】
次いで、エッジ投影線推定処理部13は、STEP28,29の処理を実行する。STEP28では、エッジ投影線推定処理部13は、処理に用いる候補位置v(k1)の番数k1を“1”だけ増加させる。そして、STEP29では、エッジ投影線推定処理部13は、増加後のk1の値が候補位置v(k1)の総個数であるN3aを超えたか否かを判断する。
【0242】
このSTEP29の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k1番目の候補位置v(k1)を用いて、前記STEP25〜27の処理を実行する。
【0243】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれに対応して、上記の誤差画像が作成される。
【0244】
STEP29の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP30の処理を実行する。このSTEP30では、エッジ投影線推定処理部13は、図15に示す如く、エッジ推定用領域R3u内に、縦方向に細長い複数(N3b個)の演算処理領域R3min(k2)(k2=1,2,…,N3b)を設定する。各演算処理領域R3min(k2)は、基準画像の横方向の幅があらかじめ定められた所定の微小幅(例えば1画素分の幅)を有し、且つ、縦方向の範囲が、エッジ推定用領域R3uの縦方向の範囲に一致する細長方形状(短冊状)の領域である。
【0245】
そして、これらの演算処理領域R3min(k2)は、本実施形態では、基準画像の横方向に順番に隣接して並ぶように配置され、その全体の領域がエッジ推定用領域R3uに合致するものとされる。
【0246】
なお、演算処理領域R3min(k2)は、隣合う演算処理領域の間に若干の間隔を有するように設定してもよい。また、各演算処理領域R3min(k2)の横方向の幅は1画素分の幅よりも多少大きい幅(例えば2画素分の幅、3画素分の幅等)であってもよい。
【0247】
補足すると、演算処理領域R3min(k2)(k2=1,2,…,N3b)の全てを合成したものが、エッジ推定用領域R3uに合致するので、前記STEP23において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対してN3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理であると言える。
【0248】
また、前記STEP25において、v(k1)に応じて射影変換対象領域R3u1,R3u2,R3u3を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対して射影変換対象領域R3u1,R3u2,R3u3を設定する処理であると言える。
【0249】
演算処理領域R3min(k2)を設定するSTEP30の処理は、STEP23やSTEP25よりも後で行なう必要はなく、例えば、STPE22において、演算処理領域R3min(k2)の設定処理を行ってもよい。
【0250】
次いで、エッジ投影線推定処理部13は、STEP31の処理を実行する。この処理では、各演算処理領域R3min(k2)に対応する処理を順番に実行するために、k2の初期値として“1”が設定される。
【0251】
次いで、エッジ投影線推定処理部13は、STEP32の処理を実行する。このSTEP32では、エッジ投影線推定処理部13は、第k2番目(k2は現在設定されている番数)の演算処理領域R3min(k2)におけるN3a個の前記候補位置v(k1)(k1=1,2,…,N3a)のそれぞれについて、実エッジ投影線L52rの位置(縦方向での位置)に対する各候補位置v(k1)の適合度を表す評価関数Eu(k2)の値を算出する。
【0252】
以降、第k2番目の演算処理領域R3min(k2)における第k1番目の候補位置v(k1)の適合度を表す評価関数Eu(k2)をEu(k2)_k1と表記する。
【0253】
演算処理領域R3min(k2)における各候補位置v(k1)に対応する評価関数Eu(k2)_k1の値は、v(k1)に応じて設定された射影変換対象領域R3u1(以降、これをR3u1(k1)と表記する)に対応する前記誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Du1(k2)_k1の値と、v(k1)に応じて設定された射影変換対象領域R3u2(以降、これをR3u2(k1)と表記する)に対応する誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Du2(k2)_k1の値と、v(k1)に応じて設定された射影変換対象領域R3u3(以降、これをR3u3(k1)と表記する)に対応する誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Du3(k2)_k1の値とを線形結合(本実施形態では加算)してなる値として算出される。
【0254】
より具体的には、v(k1)に応じて設定された射影変換対象領域R3u1,R3u2,R3u3のそれぞれに対応する誤差画像の各画素位置Qでの画素値をそれぞれI1uerr(Q),I2uerr(Q),I3uerr(Q)とする。また、演算処理領域R3min(k2)における射影変換対象領域R3u1(k1),R3u2(k1),R3u3(k1)のそれぞれのトータルの画素数をNu1(k2)_k1,Nu2(k2)_k1,Nu3(k3)_k1とする。このとき、誤差関数Du1(k2)_k1,Du2(k2)_k1,Du3(k2)_k1はそれぞれ、次式(3−1),(3−2),(3−3)により算出される。そして、これらのDu1(k2)_k1,Du2(k2)_k1,Du3(k2)_k1の値から次式(4)によって、評価関数Eu(k2)_k1が算出される。
【0255】
Du1(k2)_k1=(1/Nu1(k2)_k1)・ΣI1uerr(Q) ……(3−1)
Du2(k2)_k1=(1/Nu2(k2)_k1)・ΣI2uerr(Q) ……(3−2)
Du3(k2)_k1=(1/Nu3(k2)_k1)・ΣI3uerr(Q) ……(3−3)
Eu(k2)_k1=Du1(k2)_k1+Du2(k2)_k1+Du3(k2)_k1 ……(4)
【0256】
この場合、式(3−1)におけるΣI1uerr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u1(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u1(k1)との重なり領域)での誤差画像の画素値I1uerr(Q)の総和である。
【0257】
また、式(3−2)におけるΣI2uerr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u2(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u2(k1)との重なり領域)での誤差画像の画素値I2uerr(Q)の総和である。
【0258】
また、式(3−3)におけるΣI3uerr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u3(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u3(k1)との重なり領域)での誤差画像の画素値I3uerr(Q)の総和である。
【0259】
STEP32では、以上のようにして、第k2番目の演算処理領域R3min(k2)において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれの適合度(実エッジ投影線L52rの位置としての妥当性の度合い)をそれぞれ表すN3a個の評価関数Eu(k2)_k1(k1=1,2,…,N3a)の値が算出される。これらの評価関数Eu(k2)_k1(k1=1,2,…,N3a)は、第k2番目の演算処理領域R3min(k2)において、基準画像の縦方向での位置と、その位置の適合度(第k2番目の演算処理領域R3min(k2)での実エッジ投影線L52rの位置に対する適合度)との間の関係を表すデータとしての意味を持つ。
【0260】
次いで、エッジ投影線推定処理部13は、STEP33,34の処理を実行する。STEP33では、エッジ投影線推定処理部13は、処理対象とする演算処理領域R3min(k2)の番数k2を“1”だけ増加させる。そして、STEP34では、エッジ投影線推定処理部13は、増加後のk2の値が演算処理領域R3min(k2)の総個数であるN3bを超えたか否かを判断する。
【0261】
このSTEP34の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k2番目の演算処理領域R3min(k2)に関して、前記STEP32の処理を実行する。
【0262】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれと、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれとの各組に対応して、総計N3a×N3b個の評価関数Eu(k2)_k1の値が算出される。
【0263】
これにより、図16(a),(b)に例示する如く、各演算処理領域R3min(k2)毎に、基準画像の縦方向(v軸方向)での所定の刻み間隔毎の複数の候補位置v(k1)(k1=1,2,…,N3a)と、その各候補位置v(k1)の適合度(第k2番目の演算処理領域R3min(k2)での実エッジ投影線L52rの位置に対する適合度)を表す評価関数Eu(k2)_k1の値との間の関係を表すデータが得られることとなる。
【0264】
なお、図16(a),(b)は、それぞれ、第1番目の演算処理領域R3min(1)に関する評価関数Eu(1)のグラフ、第N3b番目の演算処理領域R3min(N3b)に関する評価関数Eu(N3b)のグラフを代表的に示している。
【0265】
STEP34の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP35の処理を実行する。このSTEP35では、エッジ投影線推定処理部13は、エッジ推定用領域R3uに推定エッジ投影線の複数の候補(以降、エッジ投影線候補という)を仮想的に設定する。そして、エッジ投影線推定処理部13は、その各エッジ投影線候補毎に、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおける評価関数Eu(k2)の値の総和TEu(以降、総合評価関数TEuという)を求める。
【0266】
このSTEP35の処理でエッジ投影線推定処理部13が仮想的に設定する複数のエッジ投影線候補は、複数種類の位置及び方向を有するエッジ投影線である。これらのエッジ投影線候補は、具体的には、例えば次のように設定される。
【0267】
すなわち、図17を参照して、エッジ投影線推定処理部13は、N3b個の演算処理領域R3min(k2)を合成した領域としてエッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ同数ずつ、複数個の点を設定する。例えば、前記N3a個の候補位置v(k1)(k1=1,2,…,N3a)、エッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ、N3a個の点を等間隔で設定する。
【0268】
そして、エッジ投影線推定処理部13は、エッジ推定用領域R3uの左端の各設定点と、右端の各設定点とを結ぶ、総計N3a×N3a個の直線をエッジ投影線候補として設定する。
【0269】
図17に示すエッジ投影線候補は、このようにして設定されるエッジ投影線候補のうちの一部を代表的に示したものである。
【0270】
補足すると、エッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ設定する点の個数は、候補位置v(k1)の個数N3aと同じでなくてもよく、例えばN3aよりも少ない個数であってもよい。また、エッジ推定用領域R3uの左端の辺と右端の辺とにそれぞれ設定する点の位置(縦方向での位置)は、v(1)とv(N3a)との間の範囲内の位置であれば、候補位置v(k1)と異なる位置であってもよい。
【0271】
また、エッジ投影線候補が通る基準画像上の1つの点の位置と、該エッジ投影線候補の方向(傾斜角度)との組み合わせによって、複数のエッジ投影線候補を設定するようにしてもよい。
【0272】
上記の如く設定される各エッジ投影線候補に対応する総合評価関数TEuの値は、次のように算出される。
【0273】
すなわち、エッジ投影線推定処理部13は、まず、エッジ投影線候補のそれぞれについて、該エッジ投影線候補がN3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれと交差する点の、基準画像の縦方向での位置(各演算処理領域R3min(k2)での該エッジ投影線候補の位置)を算出する。
【0274】
そして、エッジ投影線推定処理部13は、エッジ投影線候補のそれぞれについて、各演算処理領域R3min(k2)での評価関数Eu(k2)の値を、上記の如く算出した該演算処理領域R3min(k2)でのエッジ候補線候補の位置と、該演算処理領域R3min(k2)において前記STEP32の処理により算出したN3a個の評価関数Eu(k2)_1〜Eu(k2)_N3aとに基づいて決定する。
【0275】
具体的には、任意の1つのエッジ投影線候補に関し、例えば第k2番目の演算処理領域R3min(k2)における該エッジ投影線候補の位置(v軸方向での位置)がvxであり、N3a個の前記候補位置v(1)〜v(N3a)のうちのvxに一致する候補位置がv(k1x)であるとする。この場合、第k2番目の演算処理領域R3min(k2)と候補位置v(k1x)とに対応して算出された評価関数Eu(k2)_k1xの値が、第k2番目の演算処理領域R3min(k2)での該エッジ投影線候補の評価関数Eu(k2)の値として決定される。
【0276】
なお、vxに一致する候補位置が存在しない場合には、例えばvxに最も近い候補位置を上記v(k1x)として、第k2番目の演算処理領域R3min(k2)での該エッジ投影線候補の評価関数Eu(k2)の値が決定される。
【0277】
あるいは、vxの前後の2つ以上の候補位置のそれぞれに対応する評価関数Eu(k2)の値から、線形補間等の補間処理によって、vxに対応する評価関数Eu(k2)の値を算出するようにしてもよい。このようにした場合には、その算出した評価関数Eu(k2)の値を、第k2番目の演算処理領域R3min(k2)での該エッジ投影線候補の評価関数Eu(k2)の値として決定するようにすればよい。
【0278】
次いで、エッジ投影線推定処理部13は、各エッジ投影線候補毎に、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおける該エッジ投影線候補の位置に関する評価関数Eu(1)〜Eu(N3b)の値を加え合わせることによって、該エッジ投影線候補に対応する総合評価関数TEuの値を算出する。
【0279】
以上の如くSTEP35の処理を実行した後、次に、エッジ投影線推定処理部13は、STEP36の処理を実行する。このSTEP36では、エッジ投影線推定処理部13は、STEP35で算出した各エッジ投影線候補毎の総合評価関数TEuに基づいて、実エッジ投影線L52rを推定した線としての推定エッジ投影線を決定する。
【0280】
ここで、ある1つのエッジ投影線候補が、実エッジ投影線L52rに合致する場合、原理的には、各演算処理領域R3min(k2)での該エッジ投影線候補の位置に関する評価関数Eu(k2)の値は最小(最も“0”に近い値)となる。従って、該エッジ投影線候補に対応する総合評価関数TEuの値は、他のエッジ投影線候補に対応する総合評価関数TEuの値よりも小さくなる。
【0281】
そこで、STEP36では、エッジ投影線推定処理部13は、STEP35で設定した全てのエッジ投影線候補のうち、対応する総合評価関数TEuの値が最小となるエッジ投影線候補を、推定エッジ投影線として決定する。
【0282】
これにより例えば、図18に例示する如く、エッジ推定用領域R3uの左端の辺上の位置v(k1a)の点と、右端の辺上の位置v(k1b)(図示例ではv(k1b)=v(k1a))の点とを結ぶエッジ投影線候補が、推定エッジ投影線として決定される。
【0283】
以上が、カメラ3R,3Lの撮像領域に昇りの段差部(昇り階段50)が存在する場合に実行されるエッジ投影線推定処理部13の処理である。
【0284】
次に、カメラ3R,3Lの撮像領域に降りの段差部(例えば降り階段50)が存在する場合に実行されるエッジ投影線推定処理部13の処理(図12のSTEP37〜51の処理)を説明する。
【0285】
段差部が降りの段差部である場合には、エッジ投影線推定処理部13は、まず、STEP37において、基準画像内に、STEP22と同様に、実エッジ投影線の推定用の画像処理領域であるエッジ推定用領域R3dを設定する。
【0286】
ここで、以降の説明では、説明の便宜上、カメラ3R,3Lの撮像領域に存在する降りの段差部が、図1(b)に示した如き降り階段50であるとする。さらに、この降り階段50の任意の1つの上段側踏面51(例えばロボット1から見て最も手前側の上段側踏面)の先端側エッジ52を基準画像に投影してなる実エッジ投影線L52rを推定する処理をエッジ投影線推定処理部13により実行するものとする。
【0287】
そして、昇りの段差部に関する説明と同様に、推定しようとする実エッジ投影線L52rに対応する先端側エッジ52を先端に有する上段側踏面51と、その一段下側の下段側踏面51とを、それぞれ参照符号51p,51qを使用して表記する。
【0288】
STEP37において基準画像に設定するエッジ推定用領域R3dは、図19に示す如く設定される方形状の領域である。このエッジ推定用領域R3dは、その下端の位置が上段側踏面投影領域AR2pの縦方向の幅内の位置(例えば上段側踏面投影領域AR2pの上端から既定の所定量だけ下側の位置)となり、且つ、R3dの上端の位置が下段側踏面投影領域AR2qの縦方向の幅内の位置(例えば下段側踏面投影領域AR2qの下端から所定量だけ上側の位置)になるように配置される。この場合、エッジ推定用領域R3dの縦方向の幅は、下段側踏面投影領域ARqと上段側踏面投影領域ARpとの間の縦方向の間隔に応じて決定される。
【0289】
また、エッジ推定用領域R3dは、その横方向の範囲(左端と右端との間の範囲)が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲に一致するように配置される。この場合、エッジ推定用領域R3dの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅と同じ幅(既定の幅)に決定される。
【0290】
なお、エッジ推定用領域R3dの横方向の幅は、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の幅よりも若干小さくてもよい。その場合は、エッジ推定用領域R3dは、その横方向の範囲が、下段側踏面投影領域AR2q及び上段側踏面投影領域AR2pの横方向の範囲内に収まるように配置すればよい。
【0291】
以上のSTEP37の処理によって、エッジ推定用領域R3dは、その内部に(上端の辺と下端の辺との間)に推定しようとする単一の実エッジ投影線L52rが存在し、且つ、該エッジ推定用領域R3dの上端の辺及び下端の辺がそれぞれ、下段側踏面投影領域AR2q、上段側踏面投影領域AR2pに含まれるようにして、基準画像内に設定されることとなる。
【0292】
エッジ投影線推定処理部13は、STEP37の次にSTEP38の処理を実行する。このSTEP38では、エッジ投影線推定処理部13は、前記STEP23と同様に処理によって、図20に示す如く、基準画像の縦方向(v軸方向)における実エッジ投影線L52rの位置の候補としての複数(N3a個)の候補位置v(k1)(k1=1,2,…,N3a)を決定する。
【0293】
なお、この場合、候補位置v(k1)の個数N3aは、前記STEP23の処理における個数N3aと一般には異なる。
【0294】
また、この場合、N3a個の候補位置v(k1)(k1=1,2,…,N3a)は、エッジ推定用領域R3dの縦方向の範囲(上端と下端との間の範囲)内で、あらかじめ定められた所定の刻み間隔ずつ(例えば1画素分の間隔ずつ)、基準画像の縦方向に異なる位置とされる。そして、これらの候補位置v(k1)のうち、最も上側の候補位置としての第1番目の候補位置v(1)は、エッジ推定用領域R3dの上端から所定間隔(例えば1画素分の間隔)だけ下側の位置、最も下側の候補位置としての第N3a番目の候補位置v(N3a)は、エッジ推定用領域R3dの下端から所定間隔(例えば1画素分の間隔)だけ上側の位置に決定される。
【0295】
次に、エッジ投影線推定処理部13は、STEP39の処理を実行する。この処理は、前記STEP24の処理と同じであり、k1の初期値として“1”が設定される。
【0296】
次いで、エッジ投影線推定処理部13は、STEP40の処理を実行する。このSTEP40では、エッジ投影線推定処理部13は、基準画像の縦方向での実エッジ投影線L52rの位置が第k1番目の候補位置v(k1)(k1は現在設定されている値)に一致するものと仮定して、エッジ推定用領域R3d内に、図20に例示する如く、2つの射影変換対象領域R3d1,R3d2を設定する。
【0297】
カメラ3R,3Lの撮像領域に存在する段差部が降りの段差部(ここでは降り階段50)である場合には、該段差部の段鼻の先端面や蹴り込み面は、基準画像及び参照画像には写らない。このため、この場合にエッジ推定用領域R3dに設定する射影変換対象領域は、R3d1,R3d2の2つである。
【0298】
これらの2つの射影変換対象領域R3d1,R3d2は、より詳しくは、第k1番目の候補位置v(k1)で横方向に延在する仮想的なエッジ投影線L52(k1)を境界線として上下に隣接するように設定される領域である。この場合、上側の射影変換対象領域R3d1は、その上端側の辺と下端側の辺とがそれぞれエッジ推定用領域R3dの上端側の辺、仮想的なエッジ投影線L52(k1)に一致し、且つ、横方向の範囲が、エッジ推定用領域R3dの横方向の範囲と一致する領域である。
【0299】
従って、射影変換対象領域R3d1は、その縦方向の幅と下端側の辺の位置(縦方向での位置)とが、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3d1は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、下段側踏面投影領域AR2qに含まれる領域である。
【0300】
また、下側の射影変換対象領域R3d2は、その上端側の辺と下端側の辺とがそれぞれ仮想的なエッジ投影線L52(k1)、エッジ推定用領域R3dの下端側の辺に一致し、且つ、横方向の範囲が、エッジ推定用領域R3dの横方向の範囲と一致する領域である。
【0301】
従って、射影変換対象領域R3d2は、その縦方向の幅と上端側の辺の位置(縦方向での位置)とが、仮想的なエッジ投影線L52(k1)の位置(=v(k1))に応じて変化するように設定される領域である。この射影変換対象領域R3d2は、仮想的なエッジ投影線L52(k1)が、実エッジ投影線L52rに合致する場合には、上段側踏面投影領域AR2pに含まれる領域である。
【0302】
以上の如くSTEP40の処理を実行した後、次に、エッジ投影線推定処理部13は、STEP41の処理を実行する。このSTEP41では、エッジ推定用領域R3dに設定した2つの射影変換対象領域R3d1,R3d2のそれぞれが平面部分の投影領域であると仮定して、射影変換対象領域R3d1,R3d2のそれぞれに対応する平面部分を包含する平面についての平面パラメータベクトル↑md1,↑md2を決定する。
【0303】
この場合、射影変換対象領域R3d1,R3d2にそれぞれ対応する平面パラメータベクトル↑md1,↑md2は、それぞれ、下段側踏面投影領域AR2qに対応する平面パラメータベクトル↑m2q、上段側踏面投影領域AR2pに対応する平面パラメータベクトル↑m2pに一致するベクトルに決定される。
【0304】
次いで、エッジ投影線推定処理部13は、STEP42の処理を実行する。このSTEP42では、エッジ投影線推定処理部13は、各射影変換対象領域R3d1,R3d2について、各射影変換対象領域R3d1,R3d2の画像(基準画像)と、これに対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像との誤差を表す誤差画像を作成する。
【0305】
この場合、各射影変換対象領域R3d1,R3d2に対応する誤差画像は、それぞれに対応する平面パラメータベクトル↑md1,↑md2を使用して、前記STEP27での誤差画像の作成処理と同様の処理によって作成される。
【0306】
次いで、エッジ投影線推定処理部13は、STEP43,44の処理を実行する。STEP43では、エッジ投影線推定処理部13は、処理に用いる候補位置v(k1)の番数k1を“1”だけ増加させる。そして、STEP44では、エッジ投影線推定処理部13は、増加後のk1の値が候補位置v(k1)の総個数であるN3aを超えたか否かを判断する。
【0307】
このSTEP44の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k1番目の候補位置v(k1)を用いて、前記STEP40〜42の処理を実行する。
【0308】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれに対応して、上記の誤差画像が作成される。
【0309】
STEP44の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP45の処理を実行する。このSTEP45では、エッジ投影線推定処理部13は、図21に示す如く、エッジ推定用領域R3d内に、縦方向に細長い複数(N3b個)の演算処理領域R3min(k2)(k2=1,2,…,N3b)を設定する。各演算処理領域R3min(k2)(k2=1,2,…,N3b)は、前記STEP30の処理に関して説明したものと同様の細長方形状(短冊状)の領域である。そして、その設定の仕方は、STEP30の処理と同じである。
【0310】
なお、演算処理領域R3min(k2)の個数N3bは、前記STEP30で設定する演算処理領域R3min(k2)の個数と同じでなくてもよい。
【0311】
補足すると、前記STEP30に関して説明したことと同様に、前記STEP38において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対してN3a個の候補位置v(k1)(k1=1,2,…,N3a)を設定する処理であると言える。
【0312】
また、前記STEP40において、v(k1)に応じて射影変換対象領域R3d1,R3d2を設定する処理は、換言すれば、各演算処理領域R3min(k2)に対して射影変換対象領域R3d1,R3d2を設定する処理であると言える。
【0313】
STEP45の処理は、STEP38やSTEP40よりも後で行なう必要はなく、例えば、STEP37において、演算処理領域R3min(k2)の設定処理を行ってもよい。
【0314】
次いで、エッジ投影線推定処理部13は、STEP46の処理を実行する。この処理では、k2の初期値として“1”が設定される。
【0315】
次いで、エッジ投影線推定処理部13は、STEP47の処理を実行する。このSTEP47では、エッジ投影線推定処理部13は、第k2番目(k2は現在設定されている番数)の演算処理領域R3min(k2)におけるN3a個の前記候補位置v(k1)(k1=1,2,…,N3a)のそれぞれについて、実エッジ投影線L52rの位置(縦方向での位置)に対する各候補位置v(k1)の適合度を表す評価関数Ed(k2)の値を、前記STEP32の処理の場合と同様の処理によって算出する。
【0316】
ただし、この場合、エッジ推定用領域R3dに設定される射影変換対象領域はR3d1,R3d2の2つであるので、演算処理領域R3min(k2)における各候補位置v(k1)に対応する評価関数Ed(k2)_k1の値は、次のように算出される。
【0317】
すなわち、演算処理領域R3min(k2)における各候補位置v(k1)に対応する評価関数Ed(k2)_k1の値は、v(k1)に応じて設定された射影変換対象領域R3d1(以降、これをR3d1(k1)と表記する)に対応する前記誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Dd1(k2)_k1の値と、v(k1)に応じて設定された射影変換対象領域R3d2(以降、これをR3d2(k1)と表記する)に対応する誤差画像のうちの演算処理領域R3min(k2)に含まれる部分の画像から算出される誤差関数Dd2(k2)_k1の値とを線形結合(本実施形態では加算)してなる値として算出される。
【0318】
さらに詳細には、v(k1)に応じて設定された射影変換対象領域R3d1,R3d2のそれぞれに対応する誤差画像の各画素位置Qでの画素値をそれぞれI1derr(Q),I2derr(Q),I3derr(Q)とする。また、演算処理領域R3min(k2)における射影変換対象領域R3d1(k1),R3d2(k1)のそれぞれのトータルの画素数をNd1(k2)_k1,Nd2(k2)_k1とする。このとき、誤差関数Dd1(k2)_k1,Dd2(k2)_k1はそれぞれ、次式(5−1),(5−2)により算出される。そして、これらのDd1(k2)_k1,Dd2(k2)_k1の値から次式(6)によって、評価関数Ed(k2)_k1が算出される。
【0319】
Dd1(k2)_k1=(1/Nd1(k2)_k1)・ΣI1derr(Q) ……(5−1)
Dd2(k2)_k1=(1/Nd2(k2)_k1)・ΣI2derr(Q) ……(5−2)
Ed(k2)_k1=Dd1(k2)_k1+Dd2(k2)_k1 ……(6)
【0320】
この場合、式(5−1)におけるΣI1derr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3d1(k1)(演算処理領域R3min(k2)と射影変換対象領域R3d1(k1)との重なり領域)での誤差画像の画素値I1derr(Q)の総和である。
【0321】
また、式(3−2)におけるΣI2derr(Q)は、演算処理領域R3min(k2)における射影変換対象領域R3u2(k1)(演算処理領域R3min(k2)と射影変換対象領域R3u2(k1)との重なり領域)での誤差画像の画素値I2derr(Q)の総和である。
【0322】
STEP47では、以上のようにして、第k2番目の演算処理領域R3min(k2)において、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれの適合度(実エッジ投影線L52rの位置としての妥当性の度合い)をそれぞれ表すN3a個の評価関数Ed(k2)_k1(k1=1,2,…,N3a)の値が算出される。
【0323】
次いで、エッジ投影線推定処理部13は、STEP48,49の処理を実行する。STEP48では、エッジ投影線推定処理部13は、処理対象とする演算処理領域R3min(k2)の番数k2を“1”だけ増加させる。そして、STEP49では、エッジ投影線推定処理部13は、増加後のk2の値が演算処理領域R3min(k2)の総個数であるN3bを超えたか否かを判断する。
【0324】
このSTEP49の判断結果が否定的である場合には、エッジ投影線推定処理部13は、増加後の第k2番目の演算処理領域R3min(k2)に関して、前記STEP47の処理を実行する。
【0325】
以上のようにして、N3a個の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれと、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれとの各組に対応して、総計N3a×N3b個の評価関数Ed(k2)_k1の値が算出される。
【0326】
これにより、各演算処理領域R3min(k2)毎に、基準画像の縦方向(v軸方向)での所定の刻み間隔毎の複数の候補位置v(k1)(k1=1,2,…,N3a)と、その各候補位置v(k1)の適合度(第k2番目の演算処理領域R3min(k2)での実エッジ投影線L52rの位置に対する適合度)を表す評価関数Ed(k2)_k1の値との間の関係を表すデータが得られることとなる。
【0327】
STEP49の判断結果が肯定的となった場合には、エッジ投影線推定処理部13は、次に、STEP50の処理を実行する。このSTEP50では、エッジ投影線推定処理部13は、前記STEP35の処理と同様に、エッジ推定用領域R3dに複数のエッジ投影線候補を仮想的に設定する。そして、エッジ投影線推定処理部13は、その各エッジ投影線候補毎に、N3b個の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおける評価関数Ed(k2)の値の総和である総合評価関数TEdを求める。
【0328】
このSTEP50における複数のエッジ投影線候補の設定処理と、各エッジ投影線候補に対応する総合評価関数TEdの値の算出処理とは、前記STEP35の処理と同様の処理によって行われる。
【0329】
次に、エッジ投影線推定処理部13は、STEP51の処理を実行する。このSTEP51の処理は、前記STEP36の処理と同様に行なわれる。すなわち、エッジ投影線推定処理部13は、STEP50で設定した全てのエッジ投影線候補のうち、対応する総合評価関数TEdの値が最小となるエッジ投影線候補を、推定エッジ投影線として決定する。
【0330】
以上が、カメラ3R,3Lの撮像領域に降りの段差部(降り階段50)が存在する場合に実行されるエッジ投影線推定処理部13の処理である。
【0331】
以上説明したエッジ投影線推定処理部13の処理によって、カメラ3R,3Lの撮像領域に段差部が存在する場合に、その段差部が、ロボット1から見て、昇りの段差部、降りの段差部のいずれであっても、該段差部の上段側踏面の先端側エッジを基準画像に投影してなる実エッジ投影線が推定される。
【0332】
なお、以上のエッジ投影線推定処理部13の処理の説明では、STEP22〜36の処理、又は、STEP37〜51の処理によって、任意の1つの先端側エッジ52に対応する推定エッジ投影線を求める場合について説明したが、基準画像に投影されている各上段側踏面51の先端側エッジ52毎に、STEP22〜36の処理、又は、STEP37〜51の処理を繰り返すことで、基準画像に写っている各先端側エッジ52に対応する推定エッジ投影線をそれぞれ求めることができる。
【0333】
このエッジ投影線推定処理部13の処理が終了すると、演算処理装置10は次に、段差部空間配置決定部14の処理を実行する。
【0334】
この段差部空間配置決定部14は、平面投影領域抽出部12の処理によって抽出された各平面投影領域AR2の平面パラメータベクトル↑m2と、エッジ投影線推定処理部13の処理によって求められた推定エッジ投影線とを用いて、カメラ3R,3Lにより撮像された段差部の空間的な配置位置(ロボット座標系で見た配置位置)を決定する。
【0335】
この場合、本実施形態では、段差部空間配置決定部14は、段差部の空間的な配置を示すデータとして、例えば、ロボット1の空間的な進行予定領域での段差部の各上段側踏面の先端側エッジ及び各下段側踏面の基端側エッジの空間的な配置位置、並びに、各踏面の空間的な姿勢(法線方向)を決定する。
【0336】
これらのデータは、基準画像上で決定した推定エッジ投影線と、この推定エッジ投影線に対応する先端側エッジを有する上段側踏面の平面パラメータベクトル↑m2(↑m2p)と、該上段側踏面の一段下側の下段側踏面の平面パラメータベクトル↑m2(↑m2q)とを使用して決定される。
【0337】
具体的には、段差部空間配置決定部14は、基準画像における推定エッジ投影線を、推定エッジ投影線に対応する先端側エッジを有する上段側踏面の平面パラメータベクトル↑m2pにより規定される平面に逆投影してなる線と、該先端側エッジに合致する線として決定する。その線は、基準画像の撮像面における推定エッジ投影線と基準カメラ3の光学中心Cとを含む平面と、上記平面パラメータベクトル↑m2pにより規定される平面との交線上の線として決定される。
【0338】
また、段差部空間配置決定部14は、上記の如く空間的な配置を決定した先端側エッジを、下段側踏面の平面パラメータベクトル↑m2qにより規定される平面に所定の方向で投影してなる線を該下段側踏面の基端側エッジとして決定する。この場合、上記所定の方向は、具体的には、先端側エッジを有する上段側踏面の平面パラメータベクトル↑m2pにより規定される平面に直交する方向、あるいは↑m2pにより規定される平面に対して、90度に近い既定の角度で交差する方向である。
【0339】
なお、前記平面投影領域抽出部12の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトルにより規定される平面の法線方向に、先端側エッジと基端側エッジとを含む平面の法線方向が一致もしくはほぼ一致するように、上記所定の方向を決定してもよい。
【0340】
また、段差部空間配置決定部14は、段差部の上段側踏面及び下段側踏面の姿勢(法線方向)を、それぞれに対応する平面パラメータベクトル↑m2p、↑m2qの方向と一致する方向(これは一般には、鉛直方向とは限らない)に決定する。
【0341】
以上により、段差部の各上段側踏面の先端側エッジ及び各下段側踏面の基端側エッジの空間的な配置位置、並びに、各踏面の空間的な姿勢(法線方向)が決定される。
【0342】
そして、ロボット1は、これらのデータによりロボット1に対する段差部の配置位置やその形状を認識しつつ、該段差部を昇降する等の動作を行なう。
【0343】
以上説明した本実施形態によれば、横方向の幅が微小で、且つ縦方向に細長い複数の演算処理領域R3min(k2)(k2=1,2,…,N3b)が基準画像に横方向に並ぶようにして設定される。そして、その各演算処理領域毎に、基準画像の縦方向の複数の候補位置v(k1)(k1=1,2,…,N3a)のそれぞれの適合度(実エッジ投影線L52rの位置に対する適合度)を表す評価関数Eu(k2)_k1又はEd(k2)_k1が、射影変換を利用した演算処理によって算出される。
【0344】
これにより、横方向に並ぶ複数の演算処理領域R3min(k2)(k2=1,2,…,N3b)のそれぞれにおいて、縦方向の位置と、評価関数Eu(k2)_k1又はEd(k2)_k1の値との関係を表すデータが得られることとなる。
【0345】
さらに、エッジ推定用領域R3u又はR3dに、位置や方向が互いに相違する複数のエッジ投影線候補を仮想的に設定され、それぞれのエッジ投影線候補に対して、各演算処理領域R3min(k2)での位置の評価関数Eu(k2)_k1又はEd(k2)_k1の値を合成してなる総合評価関数TEu又はTEdの値が算出される。この総合評価関数TEu又はTEdは、各演算処理領域R3min(k2)でのエッジ投影線候補の位置の適合度(実エッジ投影線L52rの位置に対する適合度)を合成してなる合成適合度の逆数値に相当するものである。
【0346】
そして、この総合評価関数TEu又はTEdの値が最小となるエッジ投影線候補(換言すれば、上記合成適合度が最大となるエッジ投影線候補)が、推定エッジ投影線として決定される。
【0347】
このようにして推定エッジ投影線を決定することにより、実エッジ投影線を推定したものとしての信頼性の高い推定エッジ投影線を決定できる。
【0348】
また、特に、カメラ3R,3Lの撮像領域に昇り階段50などの昇りの段差部が存在する場合に、候補位置v(k1)に依存する3つの射影変換対象領域R3u1,R3u2,R3u1をエッジ推定用領域R3uに前記した如く設定する(ひいては各演算処理領域R3min(k2)に設定する)ので、前記各評価関数Eu(k2)_k1の値に、候補位置v(k1)の位置を境界として隣接する射影変換対象領域R3u1,R3u2だけでなく、下段側踏面に対応する射影変換対象領域R3u3の画像の画素値を反映させることができる。すなわち、前記各評価関数Eu(k2)_k1の値を、各演算処理領域R3min(k2)に含まれる多くの画素の画素値を反映させて算出することができる。
【0349】
そのため、各演算処理領域R3min(k2)における候補位置v(k1)の位置の適合度を表すものとしての各評価関数Eu(k2)_k1の値の信頼性を高めることができる。ひいては、推定エッジ投影線の信頼性を高めることができる。
【0350】
また、本実施形態では、段差部存在検知部11や、平面投影領域抽出部12は、ステレオ画像と射影変換とを利用した処理によって、レーザ測距装置等の測距装置をロボット1に備えることなく、段差部の存在を検知したり、平面投影領域の抽出を行なうことができる。このため、段差部の認識を安価な構成で行なうことができる。
【0351】
ここで、以上説明した実施形態と本発明との対応関係について補足しておく。
【0352】
本実施形態では、前記段差部存在検知部11の処理によって、本発明における段差部存在検知手段が実現される。この場合、段差部存在検知部11は、本発明における段差部種別判断手段としての機能を含んでおり、前記STEP4の処理によって、段差部種別判断手段が実現される。
【0353】
また、前記平面投影領域抽出部12の処理によって、本発明における平面投影領域抽出手段が実現される。この場合、前記局所領域R2(i)(i=1,2,…,N2)が、本発明における平面投影領域抽出用局所領域に相当する。
【0354】
また、前記エッジ投影線推定処理部13の処理によって、本発明における評価関数算出手段と推定エッジ投影線決定手段とが実現される。より詳しくは、前記STEP22〜34の処理及びSTEP37〜49の処理によって、評価関数算出手段が実現され、前記STEP35,36並びにSTEP50,51の処理によって、推定エッジ投影線決定手段が実現される。
【0355】
この場合、射影変換対象領域R3u1,R3u2,R3u3が、それぞれ、本発明における第1のエッジ推定用射影変換対象領域、第2のエッジ推定用射影変換対象領域、第3のエッジ推定用射影変換対象領域に相当し、射影変換対象領域R3d1,R3d2が、それぞれ、本発明における第4のエッジ推定用射影変換対象領域、第5のエッジ推定用射影変換対象領域に相当する。また、前記平面パラメータベクトル↑mu1,↑mu2,↑mu3,↑md1,↑md2がそれぞれ、本発明における射影変換用平面パラメータに相当する。
【0356】
また、前記段差部空間配置決定部14の処理によって、本発明における段差部空間配置決定手段が実現される。
【0357】
次に、以上説明した実施形態の変形態様をいくつか説明しておく。
【0358】
前記実施形態では、移動体として2脚の脚式移動ロボット1を例示したが、移動体は、3脚以上の脚を有するロボット、あるいは、車輪やクローラを備える移動体であってもよい。
【0359】
また、前記実施形態では、カメラ3R,3Lのうちのカメラ3Rを基準カメラ3として使用したが、カメラ3Lを基準カメラとして使用してもよい。
【0360】
また、前記実施形態では、カメラ3R,3Lの撮像領域に昇りの段差部が存在する場合に、候補位置v(k1)に依存する3つの射影変換対象領域R3u1,R3u2,R3u3をエッジ推定用領域R3uに設定するようにしたが、射影変換対象領域R3u1,R3u2の2つの射影変換対象領域だけをエッジ推定用領域R3uに設定(ひいては各演算処理領域R3min(k2)に設定)するようにしてもよい。
【0361】
また、前記実施形態では、各演算処理領域R3min(k2)における複数の候補位置は、全ての演算処理領域R3min(k2)について同じであるが、各演算処理領域R3min(k2)毎の候補位置を異ならせるようにしてもよい。
【0362】
また、前記実施形態では、平面投影領域抽出部12の処理において、複数の局所領域R2(i)(i=1,2,…,N2)の、基準画像の横方向での位置を同じとしたが、さらに、基準画像の横方向での複数の位置のそれぞれおいて、複数の局所領域R2(i)を縦方向に並べるように配置してもよい。
【0363】
このようにした場合には、縦方向での平面部分の分布の変化を、基準画像の横方向の複数位置において、把握することができる。このため、カメラ3R,3Lの撮像領域に存在する段差部の各踏面が、基準画像の横方向に対して比較的大きく傾いているような場合に、その傾きを反映させて、段差部の各踏面等の平面部分に対応する平面投影領域を抽出することができる。従って、該平面部分の傾きに合せて、上端及び下端の辺が基準画像の横方向に対して傾斜したような形状の平面投影領域を抽出するようにすることができる。
【符号の説明】
【0364】
1…脚式移動ロボット(移動体)、3R,3L…カメラ(ステレオカメラ)、4…投光器、11…段差部存在検知部(段差部存在検知手段、段差部種別判断手段)、12…平面投影領域抽出部(平面投影領域抽出手段)、13…エッジ投影線推定処理部(評価関数算出手段、推定エッジ投影線決定手段)、14…段差部空間配置決定部(段差部空間配置決定手段)、STEP4…段差部種別判断手段、STEP22〜34,STEP37〜49…評価関数算出手段、STEP35,36,50,51…推定エッジ投影線決定手段、50…階段(段差部)、51…踏面、52…先端側エッジ。
【特許請求の範囲】
【請求項1】
ステレオカメラを搭載した移動体の移動環境に存在しており、下段側踏面とその一段上側の上段側踏面との組を一組以上有する段差部の空間的な配置位置を、前記ステレオカメラにより取得されるステレオ画像を構成する基準画像及び参照画像を用いて認識する段差部認識装置であって、
前記段差部が前記ステレオカメラにより撮像されている状態において、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する2つの平面部分を少なくとも含む互いに異なる平面部分がそれぞれ投影された領域である複数の平面投影領域を抽出すると共に、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータを決定する平面投影領域抽出手段と、
前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定すると共に、該複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する評価関数算出手段と、
前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数に基づいて決定する推定エッジ投影線決定手段と、
該段差部の空間的な配置位置を認識するためのデータとして、少なくとも前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する段差部空間配置決定手段とを備え、
前記評価関数算出手段は、前記各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を算出するために、各演算処理領域内に各候補位置を境界として隣接する2つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する処理と、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する処理と、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する処理と、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する処理とを実行することを特徴とする段差部認識装置。
【請求項2】
請求項1記載の段差部認識装置において、
前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する処理と、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出することを特徴とする段差部認識装置。
【請求項3】
請求項1又は2記載の段差部認識装置において、
前記評価関数算出手段が各演算処理領域に設定する複数の候補位置は、全ての演算処理領域について共通のN3a個(N3a:2以上の整数)の候補位置であり、前記評価関数算出手段は、第k1番目(k1:1からN3aまでの整数のうちの任意の1つの整数)の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出するために、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域における画像と、該合成射影変換対象領域に対応する前記参照画像中の領域の画像を該合成射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる画像との誤差画像を求める処理を実行し、その誤差画像を用いて、第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出することを特徴とする段差部認識装置。
【請求項4】
請求項1〜3のいずれか1項に記載の段差部認識装置において、
前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する段差部存在検知手段をさらに備えており、
前記平面投影領域抽出手段、評価関数算出手段及び段差部配置決定手段の処理は、前記段差部存在検知手段によって前記ステレオカメラの撮像領域に段差部が存在していることが検知されることを必要条件として実行される処理であり、
前記段差部存在検知手段は、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知するための段差部存在検知用領域を前記基準画像に設定する処理と、該段差部存在検知用領域の画像と該段差部存在検知用領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである段差部存在検知用平面パラメータを、前記段差部存在検知用領域の画像の画素値分布と該段差部存在検知用領域に対応する領域の画像を該段差部存在検知用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、決定した該段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知することを特徴とする段差部認識装置。
【請求項5】
請求項1〜4のいずれか1項に記載の段差部認識装置において、
前記段差部が前記ステレオカメラにより撮像されている状態において、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する段差部種別判断手段をさらに備えており、
前記評価関数算出手段は、
前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第1及び第2のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第1及び第2のエッジ推定用射影変換対象領域のうちの下側の領域である第2のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第3のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定し、
前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第4及び第5のエッジ推定用射影変換対象領域を該演算処理領域内に設定することを特徴とする段差部認識装置。
【請求項6】
請求項5記載の段差部認識装置において、
前記評価関数算出手段は、前記段差部種別判断手段によって前記段差部が昇りの段差部であると判断された場合に、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第1のエッジ推定用射影変換対象領域と前記第3のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定し、前記第2のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、第1のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する平面の位置及び姿勢を表す平面パラメータを設定することを特徴とする段差部認識装置。
【請求項7】
請求項1〜6のいずれか1項に記載の段差部認識装置において、
前記段差部空間配置決定手段は、前記段差部の上段側踏面の先端側エッジを、該上段側踏面に対応する前記平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する方向で該段差部の下段側踏面に対応する前記平面パラメータにより規定される平面に投影した線を、該下段側踏面の基端側の境界線として決定する手段を含むことを特徴とする段差部認識装置。
【請求項8】
請求項1〜7のいずれか1項に記載の段差部認識装置において、
前記移動体には、前記ステレオカメラのよる撮像時に該ステレオカメラの撮像領域にテクスチャを付与する投光器が搭載されていることを特徴とする段差部認識装置。
【請求項1】
ステレオカメラを搭載した移動体の移動環境に存在しており、下段側踏面とその一段上側の上段側踏面との組を一組以上有する段差部の空間的な配置位置を、前記ステレオカメラにより取得されるステレオ画像を構成する基準画像及び参照画像を用いて認識する段差部認識装置であって、
前記段差部が前記ステレオカメラにより撮像されている状態において、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する2つの平面部分を少なくとも含む互いに異なる平面部分がそれぞれ投影された領域である複数の平面投影領域を抽出すると共に、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータを決定する平面投影領域抽出手段と、
前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定すると共に、該複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する評価関数算出手段と、
前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数に基づいて決定する推定エッジ投影線決定手段と、
該段差部の空間的な配置位置を認識するためのデータとして、少なくとも前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する段差部空間配置決定手段とを備え、
前記評価関数算出手段は、前記各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を算出するために、各演算処理領域内に各候補位置を境界として隣接する2つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する処理と、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する処理と、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する処理と、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する処理とを実行することを特徴とする段差部認識装置。
【請求項2】
請求項1記載の段差部認識装置において、
前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する処理と、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出することを特徴とする段差部認識装置。
【請求項3】
請求項1又は2記載の段差部認識装置において、
前記評価関数算出手段が各演算処理領域に設定する複数の候補位置は、全ての演算処理領域について共通のN3a個(N3a:2以上の整数)の候補位置であり、前記評価関数算出手段は、第k1番目(k1:1からN3aまでの整数のうちの任意の1つの整数)の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出するために、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域における画像と、該合成射影変換対象領域に対応する前記参照画像中の領域の画像を該合成射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる画像との誤差画像を求める処理を実行し、その誤差画像を用いて、第k1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出することを特徴とする段差部認識装置。
【請求項4】
請求項1〜3のいずれか1項に記載の段差部認識装置において、
前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する段差部存在検知手段をさらに備えており、
前記平面投影領域抽出手段、評価関数算出手段及び段差部配置決定手段の処理は、前記段差部存在検知手段によって前記ステレオカメラの撮像領域に段差部が存在していることが検知されることを必要条件として実行される処理であり、
前記段差部存在検知手段は、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知するための段差部存在検知用領域を前記基準画像に設定する処理と、該段差部存在検知用領域の画像と該段差部存在検知用領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである段差部存在検知用平面パラメータを、前記段差部存在検知用領域の画像の画素値分布と該段差部存在検知用領域に対応する領域の画像を該段差部存在検知用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、決定した該段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知することを特徴とする段差部認識装置。
【請求項5】
請求項1〜4のいずれか1項に記載の段差部認識装置において、
前記段差部が前記ステレオカメラにより撮像されている状態において、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する段差部種別判断手段をさらに備えており、
前記評価関数算出手段は、
前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第1及び第2のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第1及び第2のエッジ推定用射影変換対象領域のうちの下側の領域である第2のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第3のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定し、
前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第4及び第5のエッジ推定用射影変換対象領域を該演算処理領域内に設定することを特徴とする段差部認識装置。
【請求項6】
請求項5記載の段差部認識装置において、
前記評価関数算出手段は、前記段差部種別判断手段によって前記段差部が昇りの段差部であると判断された場合に、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第1のエッジ推定用射影変換対象領域と前記第3のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定し、前記第2のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、第1のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する平面の位置及び姿勢を表す平面パラメータを設定することを特徴とする段差部認識装置。
【請求項7】
請求項1〜6のいずれか1項に記載の段差部認識装置において、
前記段差部空間配置決定手段は、前記段差部の上段側踏面の先端側エッジを、該上段側踏面に対応する前記平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する方向で該段差部の下段側踏面に対応する前記平面パラメータにより規定される平面に投影した線を、該下段側踏面の基端側の境界線として決定する手段を含むことを特徴とする段差部認識装置。
【請求項8】
請求項1〜7のいずれか1項に記載の段差部認識装置において、
前記移動体には、前記ステレオカメラのよる撮像時に該ステレオカメラの撮像領域にテクスチャを付与する投光器が搭載されていることを特徴とする段差部認識装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【公開番号】特開2013−72813(P2013−72813A)
【公開日】平成25年4月22日(2013.4.22)
【国際特許分類】
【出願番号】特願2011−213384(P2011−213384)
【出願日】平成23年9月28日(2011.9.28)
【出願人】(000005326)本田技研工業株式会社 (23,863)
【Fターム(参考)】
【公開日】平成25年4月22日(2013.4.22)
【国際特許分類】
【出願日】平成23年9月28日(2011.9.28)
【出願人】(000005326)本田技研工業株式会社 (23,863)
【Fターム(参考)】
[ Back to top ]