(57)【要約】【課題】全方向画像を用いてセンサの傾きを推定することができる。【解決手段】センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出しステップとして、Ｓ１０〜Ｓ３０の処理を行う。エッジと前記エッジ方向を球面画像に写像する写像ステップとしてＳ４０及びＳ５０の処理を行う。球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票ステップとして、Ｓ６０の処理を行う。その後、投票空間の中で投票された投票回数の最大となる点を検出する検出ステップとして、Ｓ７０の処理を行い、その検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定ステップとして、Ｓ８０の処理を行う。【選択図】  図３
2004220351.tif 000002
【特許請求の範囲】
【請求項１】
センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出し手段と、
前記エッジと前記エッジ方向を球面画像に写像する写像手段と、
前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票手段と、
前記投票空間の中で前記投票手段によって投票された投票回数の最大となる点を検出する検出手段と、
前記検出手段の検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定手段と
を備えたことを特徴とするセンサの傾き推定装置。
【請求項２】
前記推定手段を第１推定手段としたとき、前記第１推定手段が推定したセンサの傾きを初期値とし、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定手段を備えたことを特徴とする請求項１に記載のセンサの傾き推定装置。
【請求項３】
センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出しステップと、
前記エッジと前記エッジ方向を球面画像に写像する写像ステップと、
前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票ステップと、
前記投票空間の中で投票された投票回数の最大となる点を検出する検出ステップと、
その検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定ステップと
を備えたことを特徴とするセンサの傾き推定方法。
【請求項４】
前記推定ステップを第１推定ステップとしたとき、前記第１推定ステップで推定したセンサの傾きを初期値とし、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定ステップを備えたことを特徴とする請求項３に記載のセンサの傾き推定方法。
【請求項５】
コンピュータを、
センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出しする割り出し手段と、
前記エッジと前記エッジ方向を球面画像に写像する写像手段と、
前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票手段と、
前記投票空間の中で前記投票手段によって投票された投票回数の最大となる点を検出する検出手段と、その検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定手段と
して機能させるためのセンサの傾き推定プログラム。
【請求項６】
前記推定手段を第１推定手段としたとき、
前記コンピュータを、前記第１推定手段が推定したセンサの傾きを初期値とし、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定手段として機能させるための請求項５に記載のセンサの傾き推定プログラム。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、センサの傾き推定装置及びセンサの傾き推定方法、並びにセンサの傾き推定プログラムに関するものである。
【０００２】
【従来の技術】
アクティブセンサの姿勢の推定は、ロボットナビゲーションやコンピュータビジョンにおける最も重要な課題となっている。
【０００３】
近年現れた全方位センサは周りの環境の３６０°視野角の情報を同時に取得できるため、ロボットやセンサの位置及び推定にも大変有効であり、ロボットや自動監視分野では注目が集まっている。
【０００４】
上記のような全方位センサから得られる全方位画像は、３６０°のシーンの情報を持っているため、ロボットの位置及び姿勢推定によく使われ、全方位画像を用いたロボット位置推定については、様々な方法が提案されている。
【０００５】
これらの方法は主としてビュー・ベースド（Ｖｉｅｗ−ｂａｓｅｄ）・マッチングと、幾何特徴マッチングに分けることができる。
ビュー・ベースド・マッチングでは、環境中の学習経路上の各地点の全方位画像から抽出された特徴量を予め記憶し、センサの位置を推定するとき、現位置で得られた全方位画像から同じく特徴量を抽出し、予め記憶した学習画像の特徴量と比較することによって、学習経路にある最も近い地点を求める。
【０００６】
例えば、ノン・モノトニック（Ｎｏｎ−ｍｏｎｏｔｏｎｉｃ）連続ＤＰを用いて、全方位画像から抽出した回転不変特徴量の時空間系列をマッチングすることによって、ロボットの位置推定を行う技術が提案されている（非特許文献１参照）。
【０００７】
又、時空間系列画像を固有空間で表し、画像の固有空間の特徴量と学習画像のトラジェクトリとのマッチングによって、位置推定を行うことも提案されている（非特許文献２参照）。
【０００８】
さらに、全方位画像の時空間系列の自己相関画像から回転不変特徴を抽出し、固有空間内でのそれらの特徴量に対してＫ−Ｌ展開で固有空間を構築し、固有空間内で最も接近した学習画像を検索することによって位置推定を行うことが提案されている（非特許文献３参照）。
【０００９】
一方、幾何特徴マッチングでは、シーンの中にあるエッジやコーナーなどの幾何特徴を用いる（非特許文献４〜６参照）。
非特許文献４では、全方位画像を用いて、環境マップとシーンの垂直エッジをマッチングし、ロボットの位置を推定している。
【００１０】
或いは、時系列の全方位画像のフレーム間でコーナ追跡（非特許文献５参照）や、床のエッジ追跡（非特許文献６参照）で得られた特徴点の対応付けからセンサの移動パラメータ（相対的な位置、姿勢）を推定している。
【００１１】
【非特許文献１】
西村，野崎，岡，”Ｎｏｎ−ｍｏｎｏｔｏｎｉｃ連続ＤＰによるスプッティングに基づく移動ロボットの時系列を用いた大局的な位置推定”，電子情報通信学会論文誌  １９９８年，Ｄ−ＩＩ，Ｖｏｌ．Ｊ８１−Ｄ−ＩＩ，Ｎｏ．８，ｐ．１８７６−１８８４
【非特許文献２】
エヌ・ウィンターズ（Ｎ．  Ｗｉｎｔｅｒｓ），ジェイ・ギャスパー（Ｊ．  Ｇａｓｐａｒ），ジィ・ラセイ（Ｇ．  Ｌａｃｅｙ），ジェイ・サントス−ヴィクタ（Ｊ．  Ｓａｎｔｏｓ−Ｖｉｃｔｏｒ），”ロボットナビゲーションのための全方位画像（Ｏｍｎｉ−ｄｉｒｅｃｔｉｏｎａｌ  Ｖｉｓｉｏｎ  ｆｏｒ  Ｒｏｂｏｔ  Ｎａｖｉｇａｔｉｏｎ  ）”，「全方位画像研究会」における米国電気電子学会論文誌（Ｐｒｏｃ．  ｏｆ  ＩＥＥＥ  Ｗｏｒｋｓｈｏｐ  ｏｎ    Ｏｍｎｉ−ｄｉｒｅｃｔｉｏｎａｌ  Ｖｉｓｉｏｎ  ），米国，２０００年，ｐ．２１−２８
【非特許文献３】
岩佐，粟飯原，横矢，竹村，”全方位画像を用いた記憶に基づく位置推定”，電子情報通信学会論文誌  ２００１年，Ｄ−ＩＩ，Ｖｏｌ．Ｊ８４−Ｄ−ＩＩ，Ｎｏ．２，ｐ．３１０−３２０
【非特許文献４】
八木（Ｙ．  Ｙａｇｉ），西沢（Ｙ．  Ｎｉｓｈｉｚａｗａ  ），谷内田（Ｍ．  Ｙａｃｈｉｄａ），”全方位画像センサ（ＣＯＰＩＣＳ）を備えた移動ロボットのためのマップ・ベースのナビゲーション（Ｍａｐ−Ｂａｓｅｄ  Ｎａｖｉｇａｔｉｏｎ  ｆｏｒ  ａ  Ｍｏｂｉｌｅ  Ｒｏｂｏｔ  ｗｉｔｈ  Ｏｍｎｉｄｉｒｅｃｔｉｏｎａｌ  ＩｍａｇｅＳｅｎｓｏｒ  ＣＯＰＩＣＳ）”，「ロボットとオートメーション」の米国電気電子学会論文誌（ＩＥＥＥ  Ｔｒａｎｓ．  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ），米国，１９９５年，Ｖｏｌ．１１，Ｎｏ．５，ｐ６３４−６４８
【非特許文献５】
ジェイ・ギャスパー（Ｊ．  Ｇａｓｐａｒ），エヌ・ウィンターズ（Ｎ．  Ｗｉｎｔｅｒｓ），ジィ・ラセイ（Ｇ．  Ｌａｃｅｙ），ジェイ・サントス−ヴィクタ（Ｊ．  Ｓａｎｔｏｓ−Ｖｉｃｔｏｒ），”全方位画像カメラを用いたヴィジョン・ベースのナビゲーションと環境画像（Ｖｉｓｉｏｎ  Ｂａｓｅｄ  Ｎａｖｉｇａｔｉｏｎ  ａｎｄ  Ｅｎｖｉｒｏｎｍｅｎｔａｌ  Ｒｅｐｒｅｓｅｎｔａｔｉｏｎｓ  ｗｉｔｈ  ａｎ  Ｏｍｎｉｄｉｒｅｃｔｉｏｎａｌ  Ｃａｍｅｒａ  ）”，「ロボットとオートメーション」の米国電気電子学会論文誌（ＩＥＥＥ  Ｔｒａｎｓ．  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ），米国，２０００年，Ｖｏｌ．１６，Ｎｏ．６，ｐ８９０−８９８
【非特許文献６】
山澤，八木，谷内田，”移動ロボットのナビゲーションのための全方位視覚センサ  Ｈｙｐｅｒ  Ｏｍｎｉ  Ｖｉｓｉｏｎの提案”，電子情報通信学会論文誌  １９９６年，Ｄ−ＩＩ，Ｖｏｌ．Ｊ７９−Ｄ−ＩＩ，Ｎｏ．５，ｐ．６９８−７０７
【００１２】
【発明が解決しようとする課題】
非特許文献１〜３に示された方法は、回転不変特徴を用いるため、センサの回転によらずに位置を推定することが可能であるが、センサの姿勢、特に傾きについては考慮されておらず、センサの傾きを推定することについては提案されていない。
【００１３】
又、非特許文献４〜６に示された方法においても、センサの傾きについては考慮されておらず、センサの傾きを推定することについては提案されていない。
本発明は、全方向画像を用いて、姿勢、すなわち、センサの傾きを推定することができるセンサの傾き推定装置及びセンサの傾き推定方法、並びにセンサの傾き推定プログラムを提供することを目的としている。
【００１４】
【課題を解決するための手段】
上記問題点を解決するため、請求項１の発明は、センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出し手段と、前記エッジと前記エッジ方向を球面画像に写像する写像手段と、前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票手段と、前記投票空間の中で前記投票手段によって投票された投票回数の最大となる点を検出する検出手段と、前記検出手段の検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定手段とを備えたことを特徴とするセンサの傾き推定装置を要旨とするものである。
【００１５】
請求項２の発明は、請求項１において、前記推定手段を第１推定手段としたとき、前記第１推定手段が推定したセンサの傾きを初期値とし、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定手段を備えたことを特徴とする。
【００１６】
請求項３の発明は、センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出しステップと、前記エッジと前記エッジ方向を球面画像に写像する写像ステップと、前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票ステップと、前記投票空間の中で投票された投票回数の最大となる点を検出する検出ステップと、その検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定ステップとを備えたことを特徴とするセンサの傾き推定方法を要旨とするものである。
【００１７】
請求項４の発明は、請求項３において、前記推定ステップを第１推定ステップとしたとき、前記第１推定ステップで推定したセンサの傾きを初期値とし、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定ステップを備えたことを特徴とする。
【００１８】
請求項５の発明は、コンピュータを、センサが取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出しする割り出し手段と、前記エッジと前記エッジ方向を球面画像に写像する写像手段と、前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票手段と、前記投票空間の中で前記投票手段によって投票された投票回数の最大となる点を検出する検出手段と、その検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定手段として機能させるためのセンサの傾き推定プログラムを要旨とするものである。
【００１９】
請求項６の発明は、請求項５において、前記推定手段を第１推定手段としたとき、前記コンピュータを、前記第１推定手段が推定したセンサの傾きを初期値とし、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定手段として機能させるためのセンサの傾き推定プログラムを特徴とする。
【００２０】
ここで、本明細書において、使用する用語を説明する。
１．  センサ座標系（三次元座標系）
センサ座標系は、センサの中心から撮像したときの視点位置と視点の向きで決まる座標系である。
【００２１】
２．  ワールド座標系（三次元座標系）
センサ自体が位置する座標系である。
３．  ３次元垂直エッジ
３次元垂直エッジはワールド座標系において、大地、床面等の水平な基準面に対して垂直な方向を持つエッジをいう。前記エッジは、従って、ワールド座標系が基準面に対して垂直なＺｗ軸を有するとすると、同Ｚｗ軸に対して平行なエッジ方向を有する。なお、３次元垂直エッジ点は、離散的に見た場合、３次元垂直エッジにある点をいう。
【００２２】
（垂直エッジとセンサの傾きとの関係）
ここで、この発明の原理について説明する。
センサの傾きを推定する場合、センサが位置するワールド座標系におけるセンサ座標系の縦軸（以下、Ｚ軸という）の方向を求めればよい。一方、ワールド座標系におけるセンサの傾きと、センサ座標系におけるワールド座標系の傾きはデュアル問題であり、センサ座標系におけるワールド座標系の縦軸Ｚｗの方向が分かれば、ワールド座標系におけるセンサ座標系の縦軸Ｚの方向も分かる。
【００２３】
センサの傾きが未知の場合、センサ座標系おけるワールド座標系の縦軸Ｚｗの方向も未知である。しかし、室内などのような環境の中に３次元垂直方向のエッジが多く含まれている場合、それらのエッジ方向はワールド座標系の縦軸Ｚｗの方向と同じである。このため、センサ座標系における３次元エッジ方向の分布を調べ、最も大きいエッジ方向の分布を検出すれば、センサ座標系におけるワールド座標系の縦軸Ｚｗの方向が推定できる。
【００２４】
本発明は、センサ座標系を基準座標系とし、センサ座標系におけるワールド座標系の傾きを推定するものである。
ワールド座標系における３次元垂直エッジ点Ｐｗとそのエッジ方向Ｅｗ＝（ＸＥｗ，ＹＥｗ，ＺＥｗ）Ｔは、センサ座標系では図７に示すようにそれぞれＰとＥ＝（ＸＥ，ＹＥ，ＺＥ）Ｔとして取得される。なお、ＰとＥはそれぞれベクトルであり、Ｔは転置（転置行列）を表している。さらに、ＰとＥ＝（ＸＥ，ＹＥ，ＺＥ）Ｔはセンサ座標系の球面画像にそれぞれｓ＝（ｘｓ，ｙｓ，ｚｓ）Ｔとｅｄ＝（ｘｅ，ｙｅ，ｚｅ）Ｔに射影（写像）されるとする。なお、ｓとｅｄはそれぞれベクトルである。
【００２５】
又、センサ座標系（Ｘ，Ｙ，Ｚ）におけるワールド座標系の縦軸をＺｗ（ベクトル）とする（図８参照）。
３次元空間において、任意の垂直エッジＰ＋λＥとＺｗは平行直線であるため、同一平面上にある。すなわち、Ｐ，ＥとＺｗは次の関係を満たす。なお、λは任意の値を示している。
【００２６】
（Ｚｗ×Ｐ）ＴＥ＝（Ｐ×Ｅ）ＴＺｗ＝０      ……（１）
ここで、記号×は２つのベクトルの外積を表している。
次に、
ｓ＝Ｐ／‖Ｐ‖
と
ｅｄ＝（Ｐ×Ｅ）／‖Ｐ＋Ｅ‖  −  Ｐ／‖Ｐ‖
の関係から、ｓ，ｅｄとＺｗは次の関係を満たす。
【００２７】
（Ｚｗ×ｓ）Ｔｅｄ＝（ｓ×ｅｄ）ＴＺｗ＝０      ……（２）
式（２）はＺｗが平面（ＸＹＺ）（ｓ×ｅｄ）＝０
にあることを示している。なお、上記平面（ＸＹＺ）（ｓ×ｅｄ）＝０を図８では説明の便宜上、Ａの符号を付している。
【００２８】
すなわち、球面画像に写像された３次元垂直エッジ点ｓにおけるエッジ方向ｅｄと原点Ｏでなす平面（ＸＹＺ）（ｓ×ｅｄ）＝０は、ワールド座標系の縦軸Ｚｗを通る。
【００２９】
図８に示すように、平面（ＸＹＺ）（ｓ×ｅｄ）＝０を、例えば平面Ｚ＝１と交差させれば、その交差線ｍはＺｗと平面Ｚ＝１との交点ｐを通る。
平面Ｚ＝１上の２次元空間（ｘ，ｙ）を投票空間とした場合、球面画像における各エッジ点ｓとそのエッジ方向ｅｄは次の式（３）で投票空間（ｘ，ｙ）に投票される。
【００３０】
（ｘｙ１）（ｓ×ｅｄ）＝０          ……（３）
シーンの中から垂直エッジが多数存在する場合、各垂直エッジから作成された平面（ＸＹＺ）（ｓ×ｅｄ）＝０を上述のように平面Ｚ＝１に投影すれば、その交差線はすべて共通点ｐを通るため、投票空間（ｘ，ｙ）において大きなピークが形成される。すなわち、投票回数の最大となる点が存在する。以下、前記平面Ｚ＝１は投票平面ということがある。
【００３１】
従って、投票空間（ｘ，ｙ）から一番大きなピークを検出することにより、ワールド座標系の縦軸Ｚｗの方向を推定することができ、その値は、（ｘｐ，ｙｐ，１）Ｔになる。
【００３２】
【発明の実施の形態】
以下、本発明のセンサの傾き推定装置を具体化した一実施形態を図１〜図５を参照して説明する。
【００３３】
本実施形態のセンサの傾き推定装置は、アクティブセンサ１１とコンピュータ１６にて構成されている。なお、本実施形態の説明において、単にセンサというときは、アクティブセンサ１１のことを指している。
【００３４】
図１はアクティブセンサの電気的構成を示すブロック図である。図２はアクティブセンサ１１の機械的構成の概略図である。
アクティブセンサ１１は、複数の車輪を有する車体１００を備えており、車体１００内に設けられた図示しない電動モータにより前記車輪が駆動されて環境Ｋ内の任意の位置に向けて自動走行（直線及び曲線に沿った走行を含む）及びが可能である。なお、図２においては、説明の便宜上、環境Ｋは、車体１００に比較して、小さく図示している。
【００３５】
アクティブセンサ１１は、複数の３眼ステレオユニット（以下、単にステレオユニット１２という）、メモリユニット１５、同期信号発生器１７等を備えている。メモリユニット１５、コンピュータ１６、同期信号発生器１７等は車体１００内に格納されている。
【００３６】
前記複数のステレオユニット１２は、全方向画像取得手段に相当する。
又、前記コンピュータ１６は、割り出し手段、写像手段、投票手段、及び推定手段（第１推定手段）に相当する。
【００３７】
アクティブセンサ１１は、３次元空間内の、全ての方向におけるカラー画像と３次元情報を、同時刻にリアルタイムで取得することが可能な装置である。
ステレオユニット１２は、３個のビデオカメラからなり、各ステレオユニット１２は正２０面体の各面上にそれぞれ配置されている。すなわち、本実施形態では、ステレオユニット１２は２０ユニットある。
【００３８】
そして、各ステレオユニット１２は同特性を備え、各面に配置したステレオユニット１２により、全方向のカラー画像と白黒画像（距離画像）を同一時刻にリアルタイムで取得可能なシステムである。これによって、３次元空間上の全ての方向におけるカラー画像と３次元情報を同一時刻に得ることができる。
【００３９】
又、同じ特性を持つステレオユニット１２を正２０面体の各面上に配置したことにより、３次元空間を均等に分割し、高解像度の情報の取得が可能である。なお、このアクティブセンサ１１は、「”実環境センシングのための全方向ステレオシステム（ＳＯＳ）”、電気学会論文誌Ｃ．Ｖｏｌ．１２１−Ｃ，Ｎｏ．５，ｐｐ．８７６−８８１，２００１」に記載されている。
【００４０】
ステレオユニット１２では、図１に示すように、１つの基準ビデオカメラＶＣｓと、一対の参照用ビデオカメラＶＣとからなる。そして、参照用ビデオカメラＶＣは基準ビデオカメラＶＣｓの光軸を交線として、互いに直交する一対の平面に含ませるように配置されている。そして、これらのカメラにより、２つのステレオペアを構成するように配置されている。なお、本実施形態では、ステレオユニット１２は２０ユニットあることから、基準ビデオカメラＶＣｓも２０台あることになる。
【００４１】
各ステレオユニット１２からは、１枚のカラー画像と２枚の白黒画像とからなるステレオ画像が取得され、全方向の２０枚のカラー画像と４０枚の白黒画像を１セットとして１５セット／秒でメモリユニット１５に転送する。メモリユニット１５は、転送されてきた全方向の画像データ（以下、全方向画像という）を記憶する。前記全方向画像には、基準ビデオカメラＶＣｓにて得られた画像データも含まれる。
【００４２】
各ステレオユニット１２の各ビデオカメラには、同期信号発生器１７から共通の外部同期信号が供給されている。このことにより、ディジタル化されたフレームにおいて、完全に同期した画像データが得られる。
【００４３】
コンピュータ１６は、所定の周期毎に、同コンピュータ１６内に備えているＲＯＭ１６ａに予め格納したセンサの傾き推定プログラムを実行する。前記ＲＯＭ１６ａを備えたコンピュータ１６は記憶手段に相当する。
【００４４】
又、コンピュータ１６は、メモリユニット１５にアクセスし、全方向画像をその時々に取得する。
（作用）
さて、以上のように構成されたセンサの傾き推定装置の作用を図３〜図５を参照して説明する。
【００４５】
図３は、コンピュータ１６が実行するセンサの傾き推定プログラムのフローチャートであり、コンピュータ１６は、所定周期毎に実行し、アクティブセンサ１１の傾きを推定する。
【００４６】
以下、各ステップ毎に説明する。
１．  Ｓ１０（エッジの検出）
ステップ（以下、Ｓで表す）１０では、エッジ検出を行う。
【００４７】
すなわち、基準ビデオカメラＶＣｓ（センタカメラ）が取得した全方向画像に対してＬｏＧ（Ｌａｐｌａｓｉａｎ  ｏｆ  Ｇａｕｓｓｉａｎ）フィルタを通し、ゼロクロス点をエッジとして検出する。
【００４８】
２．  Ｓ２０（最大エッジ勾配の算出）
次に、最大エッジ勾配の算出を行う。
すなわち、Ｓｏｂｅｌフィルタにて各エッジ画素におけるエッジの勾配（強度）を計算し、画像中の最大エッジ勾配（すなわち、最大エッジ強度）を求める。
【００４９】
これは、ＬｏＧフィルタはノイズに影響されやすいため、ノイズや照明の影響で偽のエッジ多く存在するためであり、これを除くためのフィルタ処理である。
このフィルタ処理によって、強度が最大エッジ勾配値の５％未満のエッジ画素をノイズとして削除する。
【００５０】
なお、前記基準ビデオカメラＶＣｓ（センタカメラ）が取得した全方向画像は、ステレオ情報は含んでおらず、Ｓ２０にて得られるエッジ画素（ｘｉｊ，ｙｉｊ）は、２次元で表される。
【００５１】
３．  Ｓ３０（エッジの勾配方向と、エッジの２次元方向の算出）
次に、各エッジ画素において、ｘ方向とｙ方向のガウシャン（Ｇａｕｓｓｉａｎ）微分フィルタを用いて、エッジの勾配方向（ｄｘ，ｄｙ）を計算する。そして、画像平面におけるエッジの２次元方向を（ｄｙ，−ｄｙ）として算出する（図４参照）。
【００５２】
なお、本実施形態において、エッジの２次元方向はエッジ方向に相当する。
４．  Ｓ４０（エッジ画素の球面画像への写像）
Ｓ４０では、エッジ画素の球面画像への写像を行うが、その説明の前にセンサ座標系における３次元点と球面画像との対応関係について説明する。
【００５３】
（センサ座標系における３次元点と球面画像との対応関係）
ここの欄では、以下のように定義する。
センサ座標系の３次元点Ｐ：    Ｐ＝（Ｘ，Ｙ，Ｚ）
球面画像の点ｓ          ：    ｓ＝（ｘｓ，ｙｓ，ｚｓ）
【００５４】
【数１】
2004220351.tif 000003
上記のように定義された３次元点Ｐを球面画像に写像すると、球面画像の点ｓは下記の式で表される。
【００５５】
【数２】
2004220351.tif 000004
本実施形態での傾き推定は、アクティブセンサ１１のステレオ情報を使用しないため、３次元の情報は分からないが、使用できるのは、３次元点Ｐがある基準ビデオカメラＶＣｓ（センタカメラ）に写った像（画素（ｘｉｊ，ｙｉｊ））である。本実施形態では、各ステレオユニット１２のセンタカメラの画像を球面画像に写像する方法として、固定距離写像を使用している。固定距離写像は、各画素をカメラから固定距離にある点を写したものと仮定して球面画像を生成する方法である。
【００５６】
ステレオユニット１２においては、センタカメラに写った像（画素（ｘｉｊ，ｙｉｊ））は固定距離Ｄにあるとすると、ステレオユニット１２の座標系における（ｘｉｊ，ｙｉｊ）の３次元値は（ｘｉｊ，ｙｉｊ，１）Ｄになる。
【００５７】
アクティブセンサ１１の設計パラメータから、ステレオユニット１２の座標系（以下、カメラの座標系という）をセンサ座標系に以下のように変換する。
【００５８】
【数３】
2004220351.tif 000005
ここで、Ｒｃはカメラの座標系からセンサ座標系への回転マトリクスであり定数である。Ｌは図５に示すように基準ビデオカメラＶＣｓ（センタカメラ）の中心からアクティブセンサ１１の中心Ｑ（ボール中心）までの距離である。例えば、本実施形態では、設計上、Ｌを１００ｍｍとしている。中心Ｑはセンサ座標系の原点Ｏに相当する。
【００５９】
前記式（４ａ）で３次元点Ｐを球面画像に写像すると、下記式（４ｃ）が得られる。又、式（４ｃ）中のαはＤ／（Ｄ＋Ｌ）であり、αは前記固定距離Ｄで球面画像を生成する際の必要な視野角に応じた常数パラメータである。
【００６０】
例えば、固定距離Ｄを１５００ｍｍ、Ｌを１００ｍｍとして球面画像を合成したとき、
α＝１５００／１６００
となる。
【００６１】
アクティブセンサ１１は、全方向の視野角を持っているため、２０台の基準ビデオカメラＶＣｓ（センタカメラ）が撮像した画像を球面に写像すれば、閉じた球面画像が得られることになる。
【００６２】
（Ｓ４０での処理）
そこで、Ｓ４０では、基準ビデオカメラＶＣｓ（センタカメラ）の画像におけるエッジ画素（ｘｉｊ，ｙｉｊ）は近似的に式（４ｃ）を用いて球面画像にマッピング（写像）する。
【００６３】
ｓｉｊ＝γＲｃ（αｘｉｊ  αｙｉｊ  １）Ｔ      ……（４ｃ）
又、ｓγ＝１／‖Ｒｃ（αｘｉｊ  αｙｉｊ  １）Ｔ‖    である。
５．  Ｓ５０（エッジ方向の球面画像への写像）
次に、エッジ画素（ｘｉｊ，ｙｉｊ）におけるエッジ方向を式（５）で、球面画像にマッピング（写像）する。
【００６４】
ｅｄｉｊ＝βＲｃ（αｄｙｉｊ  −αｄｘｉｊ  ０）Ｔ      ……（５）
なお、式（５）において、
β＝１／‖Ｒｃ（αｄｙｉｊ  −αｄｘｉｊ  ０）Ｔ‖    である。
【００６５】
６．  Ｓ６０（投票）
次に、式（４ｃ）と式（５）を前記式（３）に代入して、下記（６）式を得る。すなわち、式（３）の、ｓ及び、ｅｄはそれぞれｓｉｊ及びｅｄｉｊに相当するため、式（４ｃ）と式（５）を前記式（３）に代入するのである。
【００６６】
【数４】
2004220351.tif 000006
【００６７】
【数５】
2004220351.tif 000007
この式（６）及び式（７）は、全方向画像のエッジとそのエッジ方向で構成する平面Ｚ＝１との交線を意味する。投票する際に、投票平面Ｚ＝１において、その交線が通るセルに１票を足すことによって投票する。
【００６８】
７．  Ｓ７０（最大ピークの検出）
次に、Ｓ６０で行った投票により、シーンの中から垂直エッジが多数存在すると、各垂直エッジから作成された平面（ＸＹＺ）（ｓ×ｅｄ）＝０を上述のように平面Ｚ＝１に投影すると、平面Ｚ＝１に対しては図８に示す交差線ｍのようにすべて共通点ｐを通る。このため、投票空間（ｘ，ｙ）において大きなピークが形成される。すなわち、投票回数の最大となる点（座標）が存在する。
【００６９】
従って、Ｓ７０では、投票空間（ｘ，ｙ）から一番大きなピーク（投票回数）を有する座標（ｘｐ，ｙｐ）を検出する。
８．Ｓ８０（傾きの推定）
Ｓ８０では、Ｓ７０で検出したピークを有する座標（ｘｐ，ｙｐ）に基づいて、ワールド座標系の縦軸Ｚｗの方向を推定する。推定した値は、（ｘｐ，ｙｐ，１）Ｔになる。
【００７０】
上記実施の形態によれば、下記に示す効果を有する。
（１）  本実施形態のセンサの傾き推定装置では、コンピュータ１６が、アクティブセンサ１１（センサ）が取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出し手段として機能する。又、コンピュータ１６を、前記エッジと前記エッジ方向を球面画像に写像する写像手段とした。さらに、コンピュータ１６は、前記球面画像に写像された前記エッジと前記エッジ方向を、センサ座標系に係る所定の平面からなる投票空間に投票する投票手段として機能する。加えて、コンピュータ１６は、前記投票空間の中で投票された投票回数の最大となる点を検出する検出手段と、その検出結果に基づいてアクティブセンサ１１が位置するワールド座標系に対するセンサ座標系の傾きを推定する推定手段とした。
【００７１】
この結果、アクティブセンサ１１の傾きを容易に推定することができる。
（２）  本実施形態では、アクティブセンサ１１（センサ）が取得した全方向画像に基づいて、エッジとそのエッジ方向を割り出す割り出しステップとして、Ｓ１０〜Ｓ３０の処理を行う。そして、前記エッジと前記エッジ方向（エッジの２次元方向）を球面画像に写像する写像ステップとして、Ｓ４０及びＳ５０の処理を行う。
【００７２】
さらに、その後、球面画像に写像された前記エッジと前記エッジ方向（エッジの２次元方向）を、センサ座標系に係る平面Ｚ＝１（所定の平面）からなる投票空間（ｘ，ｙ）に投票する投票ステップとして、Ｓ６０の処理を行う。又、その後、投票空間（ｘ，ｙ）の中で投票された投票回数の最大となる点を検出する検出ステップとして、Ｓ７０の処理を行い、その検出結果に基づいて前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する推定ステップとして、Ｓ８０の処理を行う。
【００７３】
その結果、Ｓ１０〜Ｓ８０の処理にて、上記（１）と同様の効果を奏する。
（３）  本実施形態のコンピュータ１６が実行するセンサの傾き推定プログラムは、コンピュータ１６を、上記（１）に記載した各手段として機能させるようにした。この結果、上記（１）と同様の効果を奏する。
【００７４】
（第２実施形態）
次に第２実施形態について説明する。
第２実施形態では、コンピュータ１６が実行する傾き推定プログラム中、図６に示すようにＳ８０の次にＳ９０のステップを行うことが異なっており、Ｓ１０〜Ｓ８０のステップは、第１実施形態と同様である。
【００７５】
又、第２実施形態のハード構成は第１実施形態のハード構成と同一であるため、その説明は省略する。なお、第２実施形態のコンピュータ１６は、割り出し手段、写像手段、投票手段、検出手段、推定手段（第１推定手段）及び第２推定手段に相当する。
【００７６】
前記第１実施形態では、高精度でセンサの傾きを推定するためには、高解像度の投票空間（ｘ，ｙ）が必要となる。計算コストは解像度に比例するため、第１実施形態では高解像度の投票空間（ｘ，ｙ）での計算コストが高くなる。
【００７７】
そこで、第２実施形態では、低解像度の投票空間にして、計算コストが高くならないように抑制する。
従って、第２実施形態でのＳ６０では、低解像度の投票空間（ｘ，ｙ）を持つものとする。
【００７８】
以下、第２実施形態のＳ９０について説明する。
なお、Ｓ８０では、傾きを推定したワールド座標系の縦軸Ｚｗの方向（すなわち、傾き）を初期値Ｚｗｏとする。すなわち、初期値となる傾きを推定する。そして、Ｓ９０で、前記初期値とその初期値Ｚｗｏの傾きと似た方向を持つエッジから最小２乗法を用いてセンサの傾きを効率的に推定するものである。
【００７９】
Ｓ８０で推定したワールド座標系の縦軸Ｚｗの方向（すなわち、傾き）を初期値Ｚｗｏとする理由は下記の理由による。
「発明の原理」で述べたように、３次元垂直エッジとその方向を球面画像に写像したとき、式（２）が成立する。従って、垂直エッジのみを用いて式（２）の２乗和を最小化すると、ワールド座標系の縦軸Ｚｗの方向を求めることができる。なお、一般にシーンの中には垂直エッジ以外に他の方向のエッジも存在する。それらのエッジは式（２）の外れ値となり、最小２乗法による推定においては大きな影響を及ぼすため、Ｓ８０で推定したワールド座標系の縦軸Ｚｗの方向（すなわち、傾き）を初期値Ｚｗｏとするのである。
【００８０】
そして、エッジの中で所定条件を満たすエッジを選択して、最小２乗法でワールド座標系の縦軸Ｚｗの方向を求める。
以下、具体的に説明する。前記所定条件とは、式（８）であり、エッジの中でこの式（８）を満たすエッジを求める。
【００８１】
【数６】
2004220351.tif 000008
式（８）中、ｎ１＝‖（ｄｙｉｊ  −ｄｘｉｊ  ０）ＲｃＴΔ（ｓｉｊ）‖であり、ｎ２＝‖Ｚｗｏ‖である。又、Ｕは閾値であり、本実施形態では、垂直エッジに近いエッジとして、Ｕ＝ｃｏｓ（５°）としているが、この値に限定するものではない。
【００８２】
なお、式（４ｃ）と式（５）とから、球面画像に写した全方向画像のエッジ点及びその方向を用いて式（２）の２乗和を最小化することは、次の式（９）を最小化することと等価である。
【００８３】
【数７】
2004220351.tif 000009
ただし、式（９）中、Ｚｗは推定したい未知変数であり、ｓｉｊは式（８）を満たす。
【００８４】
前記式（９）は式（１０）と書き換えることができ、本実施形態では、式（１０）を最小化することにより傾きを推定する。Ｚｗは式（１１）のマトリクスＭの最小固有値に対応する固有ベクトルで得られる。
【００８５】
【数８】
2004220351.tif 000010
【００８６】
【数９】
2004220351.tif 000011
第２実施形態では、前記第１実施形態の（１）〜（３）に記載の作用効果の他、下記の作用効果を奏する。
【００８７】
（１）  第２実施形態では、第１実施形態の構成に加えて、コンピュータ１６は、第１推定手段として推定したセンサの傾きを初期値とした。そして、コンピュータ１６を、同初期値とともに前記球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、アクティブセンサ１１が位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定手段とした。
【００８８】
この結果、第１実施形態に比して低解像度の投票空間（ｘ，ｙ）でよくなり、計算コストが高くならないように抑制できる。
（２）  第２実施形態では、Ｓ８０の処理を第１推定ステップとしたとき、前記第１推定ステップで推定したアクティブセンサ１１の傾きを初期値とした。そして、同初期値とともに球面画像に写像したエッジとそのエッジ方向を用いて最小２乗法により、前記センサが位置するワールド座標系に対するセンサ座標系の傾きを推定する第２推定ステップとして、Ｓ９０の処理を行うようにした。
【００８９】
この結果、上記第２実施形態の（１）の作用効果を奏する。
（３）  第２実施形態では、コンピュータ１６が実行するセンサの傾き推定プログラムは、コンピュータ１６を、上記第２実施形態の（１）に記載した各手段として機能させるようにした。この結果、上記第２実施形態の（１）と同様の効果を奏する。
【００９０】
なお、本発明の実施形態は上記実施形態に限定されるものではなく、発明の趣旨を逸脱しない範囲で、適宜に変更して次のように実施することもできる。
（１）  前記実施形態では、アクティブセンサ１１として、「”実環境センシングのための全方向ステレオシステム（ＳＯＳ）”、電気学会論文誌Ｃ．Ｖｏｌ．１２１−Ｃ，Ｎｏ．５，ｐｐ．８７６−８８１．２００１」に記載されているものを使用した。
【００９１】
これに限らず、他の全方向画像を取得できるセンサから得られた全方向画像を入力するようにしてもよい。例えば、前記各実施形態では、アクティブセンサ１１は正２０面体で構成したが、他の正多面体で構成してもよい。
【００９２】
（２）  前記各実施形態では、割り出し手段、写像手段、投票手段、検出手段、推定手段（第１推定手段）及び第２推定手段の各手段を１つのコンピュータ１６にて構成したが、各手段を複数のコンピュータでそれぞれ処理を行ってもよい。あるいは、前記複数の手段を、２つ又は３つのコンピュータで分担して行うようにしてもよい。
【００９３】
【発明の効果】
以上詳述したように請求項１乃至請求項６に記載の発明によれば、全方向画像を用いて、センサの姿勢、すなわち、センサの傾きを推定することができる効果を奏する。
【図面の簡単な説明】
【図１】アクティブセンサ１１の電気的構成のブロック図。
【図２】アクティブセンサ１１の機械的構成の概略図。
【図３】第１実施形態のコンピュータ１６が実行するセンサの傾き推定プログラムのフローチャート。
【図４】エッジの勾配方向とエッジの２次元方向の説明図。
【図５】固定距離Ｄと、基準ビデオカメラＶＣｓ（センタカメラ）の中心からアクティブセンサ１１の中心（ボール中心）までの距離Ｌの説明図。
【図６】第２実施形態のコンピュータ１６が実行するセンサの傾き推定プログラムのフローチャート。
【図７】ワールド座標系とセンサ座標系との関係を示す説明図。
【図８】投票の概念を示す説明図。
【符号の説明】
１１…アクティブセンサ（センサ）
１２…ステレオユニット
１６…コンピュータ（割り出し手段、写像手段、投票手段、検出手段、推定手段（第１推定手段）及び第２推定手段）
ＶＣｓ…基準ビデオカメラ
【図１】
2004220351.tif 000012
【図２】
2004220351.tif 000013
【図３】
2004220351.tif 000014
【図４】
2004220351.tif 000015
【図５】
2004220351.tif 000016
【図６】
2004220351.tif 000017
【図７】
2004220351.tif 000018
【図８】
2004220351.tif 000019