JP7470518B2 - パン・チルト角算出装置及びそのプログラム - Google Patents

Description

本発明は、カメラのパン・チルト角を算出するパン・チルト角算出装置及びそのプログラムに関する。

近年、映像を用いたスポーツ解析が盛んである。このスポーツ解析は、映像のみを用いることによって選手や道具にセンサを取り付ける必要がなく競技に影響を及ぼさない上、解析されたデータを様々な用途に活用できる。スポーツ解析の具体的な用途例としては、スポーツ中継番組への解析データの提示、審判補助、選手のトレーニングなどがある。

スポーツ解析の一例として、テニスのトーナメントで使用されている審判補助システムが知られている（非特許文献1）。このシステムは、複数台のカメラを用いてテニスボールを追跡し、自動的にボールのイン・アウトを判別する審判補助の役割を担っている。また、このシステムは、同時にボールの３次元空間位置の時系列データを算出しているためスポーツ中継のスタッツ（統計）にも利用されている。

また、スポーツ解析技術では、映像撮影用カメラとして、主に固定カメラが利用されている。この固定カメラは、シンプルな構成のため小型化しやすいというメリットもあるが、広範囲のエリアにおいて高精度な映像解析をしたい場合には問題がある。高精度な映像解析を行うためには、撮影時に高解像度な映像を撮影する必要がある。固定カメラにおいてエリア全体を撮影するため広角レンズを使用した場合、映像の解像度が低下してしまうため解析時に精度が低下してしまう。一方で、広角レンズの代わりに望遠レンズを使用した場合、高解像度な画像が撮影できるが、撮影画角が狭いので非常に多くの固定カメラを設置する必要があり、コストがかかるという問題がある。

これら問題を解決する方法として、少ない台数のパン・チルト・ズームカメラ（ＰＴＺカメラ）を利用する方法が考えられる。ＰＴＺカメラは、パン・チルトにより広範囲なエリアを撮影でき、ズームにより高解像度な映像を撮影できる。ここで、ＰＴＺカメラで撮影を行う際、カメラ校正を行うことがある（特許文献１）。特許文献１に記載の手法は、ＰＺＴカメラの光学中心と回転中心との位置ずれを考慮したカメラモデルを用いて、カメラ校正を行うものである。

特開２０１８－１８９５８０号公報（特願２０１７－９３９２０号公報）

"ホークアイ"［令和１年１２月１１日検索］、インターネット＜URL：https://www.hawkeyeinnovations.com/＞

しかし、特許文献１に記載の手法は、カメラ校正を行うに留まり、実在するカメラを正確に方向制御できないという課題がある。

そこで、本発明は、カメラを正確に方向制御できるパン・チルト角算出装置及びそのプログラムを提供することを課題とする。

前記課題を解決するため、本発明に係るパン・チルト角算出装置は、カメラの光学中心と回転中心との位置ずれを考慮したカメラモデルで予め求めたカメラパラメータを用いて、目標点に対するカメラのパン・チルト角を算出するパン・チルト角算出装置であって、入力手段と、設定手段と、目標点算出手段と、パン・チルト角算出手段と、を備える構成とした。

かかる構成によれば、入力手段は、世界座標系から回転中心におけるパン角及びチルト角がゼロ度のときの雲台座標系への変換を示す外部パラメータ、及び、世界座標系の目標点位置を入力する。
設定手段は、光学中心を原点としたカメラ座標系の目標点位置をスケール係数に依存する関数と、回転中心におけるパン・チルト後のパン・チルト座標系及び雲台座標系の間で回転中心から目標点までの距離が一致することを示す第１式と、外部パラメータにより目標点位置を世界座標系から雲台座標系に変換する第２式と、パン・チルト座標系の目標点位置をスケール係数による回転行列及び並進ベクトルで表した第３式と、を予め設定する。

目標点算出手段は、第２式により目標点位置を雲台座標系に変換し、第１式に第２式及び第３式を代入してスケール係数を算出し、算出したスケール係数を関数に代入してカメラ座標系の目標点位置を算出し、カメラ座標系の目標点位置をパン・チルト座標系に変換する。
パン・チルト角算出手段は、パン・チルト座標系及び雲台座標系の目標点位置からパン・チルト角を算出する。
以上のように、パン・チルト角算出装置は、前記したカメラモデルで求めたパラメータと、前記した第１式とを用いるので、正確なパン・チルト角を算出し、実在するカメラを正確に方向制御することができる。

なお、本発明は、コンピュータを、前記したパン・チルト角算出装置として機能させるためのプログラムで実現することもできる。

本発明によれば、正確なパン・チルト角を算出できるので、カメラを正確に方向制御することができる。

実施形態に係るパン・チルト角算出装置の構成を示すブロック図である。 図１のカメラの外観図である。 実施形態におけるカメラモデルの説明図である。 実施形態において、パン・チルト座標系及び雲台座標系の間で回転中心から目標点までの距離が一致することを説明する説明図である。 実施形態に係るパン・チルト角算出装置の動作を示すフローチャートである。

以下、本発明の実施形態について図面を参照して説明する。但し、以下に説明する形態は、本発明の技術思想を具体化するためのものであって、特定的な記載がない限り、本発明を以下のものに限定しない。また、実施形態において、同一の手段には同一の符号を付し、説明を省略することがある。

（実施形態）
［カメラ制御システムの概略］
図１を参照し、カメラ制御システム１の概略について説明する。
カメラ制御システム１は、カメラＣの姿勢（パン、チルト）を制御するものであり、カメラＣと、カメラ校正装置１０と、カメラパラメータ記憶装置２０と、パン・チルト角算出装置３０とを備える。ここで、カメラ制御システム１は、各装置が同期信号により同期して動作することとする。

カメラＣは、図２に示すように、一般的なＰＴＺカメラであり、後記するパン・チルト角算出装置３０が算出したパン角及びチルト角に従って姿勢を変化させるものである。以後、パン角及びチルト角をパン・チルト角と記載する場合がある。
また、カメラＣは、パン及びチルト可能な雲台Ｃａにマウント（搭載）されており、パン・チルト角をセンサ（不図示）により取得可能である。また、カメラＣは、ズームレンズＣｂを搭載し、ズーム値をセンサにより取得可能である。このセンサにより取得したパン・チルト角及びズーム値は、カメラ校正時に使用される。制御には使用しないが、複数のパン・チルト角の候補から１つを選択する際に使用してもよい。なお、センサが取得したパン値、チルト値及びズーム値と、カメラＣが撮影した撮影画像との時間的整合が取れていることとする。

図１に戻り、カメラ制御システム１の説明を続ける。
カメラ校正装置１０は、カメラＣの光学中心と回転中心との位置ずれを考慮したカメラモデルを用いて、カメラＣのカメラ校正を行うものである。すなわち、カメラ校正装置１０は、このカメラモデルを用いたカメラ校正により、カメラＣのカメラパラメータを推定する。そして、カメラ校正装置１０は、推定したカメラパラメータをカメラパラメータ記憶装置２０に書き込む。
なお、カメラモデル及びカメラパラメータの詳細は、後記する。

ここで、カメラ校正装置１０は、一般的な手法でカメラ校正を行うことができる。例えば、カメラ校正装置１０は、校正パターンによる初期カメラ校正と、その校正結果による設置カメラ校正とによって、カメラ校正を行う（参考文献１，２）。この他、カメラ校正装置１０は、参考文献３に記載の手法でカメラ校正を行ってもよい。

参考文献１：特開２０１８－１８９５８０号公報
参考文献２：加納正規他、“センサを用いた複数のパン・チルト・ズームカメラの校正手法”、第２０回画像の認識・理解シンポジウム論文集、ＰＳ３－３（２０１７）
参考文献３：大久保英彦他、“バーチャルスタジオ用高倍率ズームレンズのキャリブレーション手法”、電子情報通信学会技術研究報告、ＭＶＥ、マルチメディア・仮想環境基礎１０８．２２６、６１－６６.(２００８)

カメラパラメータ記憶装置２０は、カメラ校正装置１０が推定したカメラパラメータを記憶するものであり、例えば、ＨＤＤ（Hard Disk Drive）やＳＳＤ(Solid State Drive)等の一般的な記憶装置である。カメラパラメータ記憶装置２０に記憶されているカメラパラメータは、パン・チルト角算出装置３０により参照される。

パン・チルト角算出装置３０は、カメラＣの光学中心と回転中心との位置ずれを考慮したカメラモデルで予め求めたカメラパラメータを用いて、目標点に対するカメラＣのパン・チルト角を算出するものである。そして、パン・チルト角算出装置３０は、算出したパン・チルト角をカメラＣに出力し、カメラＣを方向制御する。

以下、カメラ制御システム１のカメラモデルについて説明した後、パン・チルト角算出装置３０の構成について説明する。

［カメラモデル］
図３に示すように、カメラＣのモデル化を行う。このカメラモデルは、雲台ＣａにズームレンズＣｂを搭載したカメラＣをマウントしたモデルである。このカメラＣをモデル化することで、推定すべきカメラパラメータを定義できる。そして、そのパラメータが推定できれば、関数化もしくはルックアップテーブル化することで、センサ値から一般的なカメラパラメータが算出できる。

内部パラメータは、焦点距離や画像中心などのカメラＣとレンズに関する特性を表す。本実施形態においては、ピンホールカメラモデルを想定し、レンズ歪も内部パラメータとして扱う。ズームレンズＣｂにおいては、ズームレンズＣｂの設定（ズームとフォーカスの設定）により内部パラメータが変化する。しかし、ここではフォーカスは考慮せず、ズームのみ考える。ズームを変化させるとズーム値ｎが変化し、それに伴い内部パラメータも変化する。このとき、内部パラメータκは、焦点距離ｆ、アスペクト比ａ、画像中心Ｃ_ｕ，Ｃ_ｖ、レンズ歪κ_１，κ_２，κ_３，κ_４のそれぞれが、ズーム値ｎを変数としたパラメータとなり、以下の式（１）で表される。

また、レンズ歪を除き、内部パラメータΚ（ｎ）を行列表記すると、以下の式（２）のようになる。この内部パラメータΚ（ｎ）を内部パラメータ行列と呼ぶ。

一般的に、外部パラメータは、基準となる座標系（世界座標系）とカメラＣの座標系との位置・姿勢の関係を表す。この関係は、並進ベクトルｔ＝［ｔ_ｘ，ｔ_ｙ，ｔ_ｚ］^Ｔと回転角度θ＝［θ_ｘ，θ_ｙ，θ_ｚ］^Ｔにより表現でき、座標変換と等しい。つまり、外部パラメータμは、以下の式（３）で表される。

以降、座標変換のパラメータも外部パラメータと呼ぶ。ここで、ある座標系Σ_Ａから別の座標系Σ_Ｂに座標変換する場合、外部パラメータを^Ｂμ_Ａと表記する。この外部パラメータ^Ｂμ_Ａを剛体変換行列^ＢＭ_Ａで表現すると、以下の式（４）となる。

ここで、^ＢＲ_Ａは回転角度θから求まる回転行列であり、^Ｂｔは並進ベクトルである。ベクトルにおける左上の添え字は座標系を表し、剛体変換行列と回転行列の右下及び左上の添え字は変換前後の座標系を表す。つまり、外部パラメータ^Ｂμ_Ａと剛体変換行列^ＢＭ_Ａは等価である。一般的な外部パラメータは、世界座標系Σ_Ｗとカメラ座標系Σ_Ｃとの位置・姿勢の関係を表すので、カメラ校正で求めるべき外部パラメータは、外部パラメータ^Ｃμ_Ｗ、又は、剛体変換行列^ＣＭ_Ｗである。

以上をふまえ、パン・チルト・ズームカメラのモデル化を考える。図３に示すように、雲台Ｃａにおいて、パン・チルト軸は１点で直交していると仮定し、この点を回転中心と呼ぶ。回転中心においてパン角及びチルト角が共にゼロ度のときの座標系を雲台座標系Σ_ＣＰと呼ぶ。このとき、世界座標系Σ_Ｗから雲台座標系Σ_ＣＰへの外部パラメータは、^ＣＰμ_Ｗとなる。

回転中心においてパン・チルトをした後の座標系をパン・チルト座標系Σ_ＰＴとすると、雲台座標系Σ_ＣＰからパン・チルト座標系Σ_ＰＴへの外部パラメータは、^ＰＴμ_ＣＰとなる。このとき、パン・チルト軸は一点で直交しているため、雲台座標系Σ_ＣＰとパン・チルト座標系Σ_ＰＴの間の並進ベクトルは０となり、回転要素はパン・チルトのみである。カメラＣの光学中心を原点とした座標系をカメラ座標系Σ_Ｃとする。パン・チルト座標系Σ_ＰＴからカメラ座標系Σ_Ｃへの外部パラメータは、^Ｃμ_ＰＴとなる。ここで、カメラ座標系Σ_Ｃの位置がズーム設定により変化するため、外部パラメータ^Ｃμ_ＰＴは、ズーム値ｎに依存するパラメータ^Ｃμ_ＰＴ（ｎ）となる。外部パラメータのどの要素がズーム値ｎに依存するかはカメラモデルの設計次第だが、本実施形態では、以下の式（５）で定義されていることとする。

この式（５）では、並進ベクトルの光軸方向であるＺ軸成分ｔ^Ｃ _ｚのみをズーム値ｎに依存するパラメータと定義している。以上より、パン・チルト・ズームカメラにおいて世界座標系Σ_Ｗからカメラ座標系Σ_Ｃへの外部パラメータ^Ｃμ_Ｗ（ｐ，ｔ，ｎ）は、以下の式（６）で算出できる。この外部パラメータ^Ｃμ_Ｗは、剛体変換行列^ＣＭ_Ｗのベクトル表現である。

カメラパラメータが定義されたので、世界座標系Σ_Ｗにおける３次元空間中の１点の撮影画像への投影を考える。ここでは、レンズ歪については考慮しない。透視投影行列Ｐは、内部パラメータ行列Κと剛体変換行列^ＣＭ_Ｗを用いて、以下の式（７）で表される。

空間中に点Ｑがあり、世界座標系Σ_Ｗにおける座標をＸ＝［Ｘ，Ｙ，Ｚ］^Ｔとする。この点Ｑを透視投影行列Ｐにより撮影画像上に投影した場合、投影される画像座標ｕ＝［ｕ，ｖ］^Ｔは、以下の式（８）で表される。この式（８）では、スケール係数をｓとし、３次元座標Ｘの同次座標系をベクトルＸ～＝［Ｘ，Ｙ，Ｚ，１］^Ｔとし、画像座標ｕの同次座標系をｕ～＝［ｕ，ｖ，１］^Ｔとする。なお、スケール係数ｓは、カメラ座標系Σ_Ｃにおける点ＱのＺ座標と等しくなる。

前記した投影モデルは、レンズ歪のない理想的なレンズの場合なので、レンズ歪を考慮した場合の点Ｑの投影について述べる。世界座標系Σ_Ｗでの点Ｑの座標^ＷＸは、外部パラメータ^Ｃμ_Ｗ（剛体変換行列^ＣＭ_Ｗと等価）によりカメラ座標系Σ_Ｃに変換され、以下の式（９）のように３次元座標^ＣＸで表される。

ここで、３次元座標Ｘの左上の添え字は、座標系を表す。このとき、正規化画像座標ｘ＝［ｘ，ｙ］^Ｔは、以下の式（１０）で表される。この式（１０）では、ベクトル^ＣＸ＝［^ＣＸ，^ＣＹ，^ＣＺ］^Ｔとしている。従って、レンズ歪がある場合の正規化画像座標ｘ_ｄ＝［ｘ_ｄ，ｙ_ｄ］^Ｔは、以下の式（１１）で表される。

なお、ｒは正規化画像の中心からの距離であり、ｒ^２＝ｘ_ｎ ^２＋ｙ_ｎ ^２である。このレンズ歪を考慮した正規化画像座標ｘ_ｄに内部パラメータをかけると、以下の式（１２）のようにレンズ歪を考慮した画像座標ｕ_ｄ＝［ｕ_ｄ，ｖ_ｄ］^Ｔが求められる。

前記したカメラモデルにおいて、内部パラメータκ（ｎ）、外部パラメータ^Ｃμ_ＰＴ（ｎ）及び外部パラメータ^ＣＰμ_Ｗは、未知のパラメータである。従って、カメラ校正装置１０では、これらカメラパラメータ（κ（ｎ）、^Ｃμ_ＰＴ（ｎ）、^ＣＰμ_Ｗ）を推定すればよい。

［パン・チルト角算出装置の構成］
図１に戻り、パン・チルト角算出装置３０の構成について説明する。
図１に示すように、パン・チルト角算出装置３０は、パラメータ入力手段（入力手段）３１と、演算式設定手段（設定手段）３２と、目標点算出手段３３と、パン・チルト角算出手段３４と、パン・チルト角選択手段３５とを備える。

パラメータ入力手段３１は、パン・チルト角の算出に必要な各種パラメータを入力（取得）するものである。ここでは、パラメータ入力手段３１は、カメラパラメータ記憶装置２０に記憶されているカメラパラメータ（κ（ｎ）、^Ｃμ_ＰＴ（ｎ）、^ＣＰμ_Ｗ）を取得する。また、パラメータ入力手段３１は、手動で設定されたパラメータ及びカメラＣからのパラメータ（^ｗＸ_ａ、ｕ_ａ、ｎ_ａ）を取得する。そして、パラメータ入力手段３１は、取得した各種パラメータを目標点算出手段３３に出力する。

＜パラメータ＞
ここで、パラメータ入力手段３１が取得するパラメータについて説明する。
カメラパラメータには、内部パラメータκ（ｎ）、外部パラメータ^Ｃμ_ＰＴ（ｎ）、及び、外部パラメータ^ＣＰμ_Ｗが含まれている。
内部パラメータκ（ｎ）は、カメラＣとレンズに関する特性を表すパラメータである。
外部パラメータ^Ｃμ_ＰＴ（ｎ）は、パン・チルト座標系Σ_ＰＴからカメラ座標系Σ_Ｃへの変換を示す外部パラメータである（剛体変換行列^ＣＭ_ＰＴ（ｎ）と等価）。
外部パラメータ^ＣＰμ_Ｗは、世界座標系Σ_Ｗから雲台座標系Σ_ＣＰへの変換を示す外部パラメータである（剛体変換行列^ＣＰＭ_Ｗと等価）。

設定パラメータには、目標点位置^ｗＸ_ａ、画像座標ｕ_ａ、及び、ズーム値ｎ_ａが含まれている。
目標点位置^ｗＸ_ａは、パン・チルトによりカメラＣを向ける目標点Ａの位置を世界座標系Σ_Ｗで示したものである。
画像座標ｕ_ａは、目標点位置^ｗＸ_ａを画像座標系で示しており、目標点Ａを投影（例えば、透視投影）する画像座標を示すものである。ここで、画像座標ｕ_ａは、レンズ歪のない画像座標を想定している。また、レンズ歪がある場合、レンズ歪係数を用いて、画像座標ｕ_ａからレンズ歪を予め除去することが好ましい（参考文献４）。
ズーム値ｎ_ａは、カメラＣのズームレンズＣｂのズーム値である。このズーム値ｎ_ａは、手動で設定する。

参考文献４：“ＯｐｅｎＣＶ”、［令和１年１２月１１日検索］、インターネット＜URL：https://opencv.org/＞

演算式設定手段３２は、パン・チルト角の算出に必要な各種演算式を予め設定するものである。この演算式には、後記する関数として式（１５）、第１式として式（１６）、第２式として式（１７）、及び、第３式として式（１９）が含まれている。そして、演算式設定手段３２は、設定された演算式を目標点算出手段３３に出力する。

目標点算出手段３３は、パラメータ入力手段３１からのパラメータ及び演算式設定手段３２で設定された演算式を参照し、パン・チルト座標系及び雲台座標系のそれぞれで目標点位置を算出するものである。そして、目標点算出手段３３は、算出した値をパン・チルト角算出手段３４に出力する。
なお、目標点位置及びパン・チルト角の算出は、詳細を後記する。

パン・チルト角算出手段３４は、目標点算出手段３３が算出したパン・チルト座標系及び雲台座標系の目標点位置から、カメラＣのパン・チルト角を算出するものである。そして、パン・チルト角算出手段３４は、算出したパン・チルト角をパン・チルト角選択手段３５に出力する。なお、パン・チルト角算出手段３４は、複数のパン・チルト角を算出し、パン・チルト角選択手段３５に出力することがある。

パン・チルト角選択手段３５は、パン・チルト角算出手段３４が算出した複数のパン・チルト角のうち、前回算出したパン・チルト角に最も近いものを選択するものである。つまり、パン・チルト角選択手段３５は、複数のパン・チルト角が入力された場合、直前のパン・チルト角に最も近い１つのパン・チルト角を選択する。そして、パン・チルト角選択手段３５は、選択したパン・チルト角を、カメラＣの姿勢制御目標値としてカメラＣに出力する。
なお、パン・チルト角選択手段３５は、パン・チルト角算出手段３４からパン・チルト角が１つだけ入力された場合、そのパン・チルト角をそのままカメラＣに出力すればよい。

以上のように、パン・チルト角算出装置３０は、パン角ｐ及びチルト角ｔの組であるパン・チルト角（ｐ，ｔ）をカメラＣに出力することで、カメラＣの姿勢を制御する。これにより、カメラＣが目標点Ａの方向を正確に向き、目標点位置^ｗＸ_ａの目標点Ａが画像座標ｕ_ａに投影されることになる。

＜目標点位置及びパン・チルト角の算出＞
以下、目標点位置及びパン・チルト角の算出を具体的に説明する。
入力されたズーム値ｎ_ａに応じて、パラメータκ（ｎ），^Ｃμ_ＰＴ（ｎ）は一意に定まる。そのため、以後の説明では、（ｎ）の表記を省略する場合がある。

式（８）に式（７）を代入すると、以下の式（１３）となる。なお、外部パラメータ^Ｃμ_ＰＴ（剛体変換行列^ＣＭ_ＰＴと等価）は、式（６）に示すように、式（１３）の剛体変換行列^ＣＭ_Ｗの算出に利用される。

この式（１３）に式（９）及び設定パラメータＸ＝Ｘ_ａ，ｕ＝ｕ_ａを代入して変形すると、以下の式（１４）となる。

ここで、ｆ_ｘ＝ｆ、ｆ_ｙ＝ａｆである。式（１４）を展開し、カメラ座標系Σ_Ｃの目標点位置^ＣＸ_ａをスケール係数ｓに依存する関数として表記すると、以下の式（１５）となる。従って、演算式設定手段３２には、スケール係数ｓの関数^ＣＸ_ａ（ｓ）として、式（１５）を予め設定する。

なお、カメラ座標系Σ_Ｃの座標［^ＣＸ（ｓ），^ＣＹ（ｓ），^ＣＺ（ｓ）］、目標点Ａの画像座標［ｕ_ａ，ｖ_ａ］、画像中心の座標［ｃ_ｕ，ｃ_ｖ］、画像座標ｕ，ｖ方向それぞれの焦点距離ｆ_ｕ，ｆ_ｖ、及び、カメラ座標系Σ_Ｃで目標点位置^ＣＸ_ａをｚ＝１に正規化したときの目標点位置^ＣＸ´_ａを示す。

ここで、目標点Ａが常にカメラＣの前方に位置するため、ｓ＞０が成立する。また、図４には、カメラ座標系Σ_Ｃ、パン・チルト座標系Σ_ＰＴ及び雲台座標系Σ_ＣＰの位置関係を図示した。図４に示すように、カメラ座標系Σ_Ｃの中心（カメラＣの光学中心）αと、パン・チルト座標系Σ_ＰＴ及び雲台座標系Σ_ＣＰの回転中心βとの位置がずれている。その一方、パン・チルト座標系Σ_ＰＴ及び雲台座標系Σ_ＣＰの間では、原点である回転中心βの位置が一致する。このため、回転中心βから目標点Ａまでの距離Ｌがパン・チルト座標系Σ_ＰＴ及び雲台座標系Σ_ＣＰの間で等しくなり、以下の式（１６）が成立する。従って、演算式設定手段３２には、第１式として、雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａ、パン・チルト座標系Σ_ＰＴの目標点位置^ＰＴＸ_ａ、及び、距離Ｌが含まれる式（１６）を予め設定する。なお、||・||_２はL２ノルムを意味する。

雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａは、以下の式（１７）で算出できる。従って、演算式設定手段３２には、第２式として、外部パラメータ^ＣＰＭ_Ｗ、及び、世界座標系Σ_Ｗの目標点位置^ｗＸ_ａが含まれる式（１７）を予め設定する。

以下の式（１８）を式（１５）に代入して整理すると、以下の式（１９）となる。従って、演算式設定手段３２には、第３式として、スケール係数ｓ、カメラ座標系Σ_Ｃからパン・チルト座標系Σ_ＰＴへの回転行列^ＰＴＲ_Ｃ、及び、カメラ座標系Σ_Ｃからパン・チルト座標系Σ_ＰＴへの並進ベクトル^ＰＴｔ´が含まれる式（１９）を予め設定する。なお、^ＰＴｔ´及び^ＰＴＲ_Ｃは、剛体変換行列^ＰＴＭ_Ｃの並進ベクトル成分と回転行列成分である。

まず、目標点算出手段３３は、式（１７）により、設定パラメータとして入力された世界座標系Σ_Ｗの目標点位置^ｗＸ_ａを、雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａに変換する。

次に、目標点算出手段３３は、式（１６）に式（１７）及び式（１９）を代入した方程式を解いて、スケール係数ｓを算出する。このとき、目標点算出手段３３は、一般的な条件において、ｓ＞０が成立するので、スケール係数ｓの解を１つに定めることができる。この一般的な条件とは、距離Ｌが並進ベクトル^ＰＴｔ´の長さを超えることである（Ｌ＞^ＰＴｔ´）。この場合、スケール係数ｓが必ず正負の２つの解になるため、ｓ＞０により１つの解に定められる。

次に、目標点算出手段３３は、算出したスケール係数ｓを式（１５）の関数^ＣＸ_ａ（ｓ）に代入し、カメラ座標系Σ_Ｃの目標点位置^ＣＸ_ａを算出する。さらに、目標点算出手段３３は、以下の式（２０）により、カメラ座標系Σ_Ｃの目標点位置^ＣＸ_ａをパン・チルト座標系Σ_ＰＴの目標点位置^ＰＴＸ_ａに変換する。

以上の演算により、パン・チルト座標系Σ_ＰＴの目標点位置^ＰＴＸ_ａ、及び、雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａが既知となる。これら２つの目標点位置^ＰＴＸ_ａ，^ＣＰＸ_ａと、回転行列^ＰＴＲ_ＣＰ（ｐ，ｔ）との関係は、以下の式（２１）で表される。なお、回転行列^ＰＴＲ_ＣＰ（ｐ，ｔ）は、パン・チルト角（ｐ，ｔ）における雲台座標系Σ_ＣＰからパン・チルト座標系Σ_ＰＴへの回転行列であり、以下の式（２２）で表される。

すなわち、パン・チルト角算出手段３４は、式（２１）及び式（２２）を用いて、既知のパン・チルト座標系Σ_ＰＴの目標点位置^ＰＴＸ_ａ、及び、雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａから、パン・チルト角（ｐ，ｔ）を算出する。具体的には、パン・チルト角算出手段３４は、ｃｏｓ^２ｐ＋ｓｉｎ^２ｐ＝１又はｃｏｓ^２ｔ＋ｓｉｎ^２ｔ＝１の条件を加えて、式（２１）及び式（２２）の連立方程式を解くことで、パン・チルト角（ｐ，ｔ）を算出できる。

ここで、複数のパン・チルト角（ｐ，ｔ）が算出されてしまい、パン・チルト角（ｐ，ｔ）が一意に定まらない場合がある。すなわち、パン・チルト角算出手段３４が、複数のパン・チルト角（ｐ，ｔ）を算出する場合がある。そこで、パン・チルト角選択手段３５は、パン・チルト角算出手段３４が算出した複数のパン・チルト角（ｐ，ｔ）のうち、直前のフレームでのパン・チルト角（ｐ，ｔ）により近い値を採用する。ここで、現在のフレームｆでのパン・チルト角を（ｐ_ｆ，ｔ_ｆ）とし、一つ前のフレームｆ－１でのパン・チルト角を（ｐ_ｆ－１，ｔ_ｆ－１）とする。この場合、パン・チルト角選択手段３５は、関数ｄｉｓｔ（［ｐ_ｆ－ｐ_ｆ－１，ｔ_ｆ－ｔ_ｆ－１］）が最小となるパン・チルト角（ｐ，ｔ）を選択する。また、（ｐ_ｆ－１，ｔ_ｆ－１）は、センサにより取得したパン・チルト角を使用してもよい。

なお、関数ｄｉｓｔは、距離を求める関数であり、ベクトルｖ＝［ｘ，ｙ］^ＴのＬ１ノルムやＬ２ノルムとすればよい。ここで、Ｌ１ノルムの場合、ｄｉｓｔ＝ａｂｓ（ｘ）＋ａｂｓ（ｙ）となる。また、Ｌ２ノルムの場合、ｓｑｒｔ（ｘ×ｘ＋ｙ×ｙ）となる。なお、ａｂｓが絶対値を求める関数であり、ｓｑｒｔが平方根を求める関数である。

［パン・チルト角算出装置の動作］
図５を参照し、パン・チルト角算出装置３０の動作について説明する。
図５に示すように、ステップＳ１において、パラメータ入力手段３１は、パン・チルト角（ｐ，ｔ）の算出に必要な各種パラメータ（κ（ｎ）、^Ｃμ_ＰＴ（ｎ）、^ＣＰμ_Ｗ、^ｗＸ_ａ、ｕ_ａ、ｎ_ａ）を入力する。
ステップＳ２において、演算式設定手段３２は、パン・チルト角（ｐ，ｔ）の算出に必要な各種演算式（式（１５）～式（１９））を予め設定する。

ステップＳ３において、目標点算出手段３３は、ステップＳ１のパラメータ及びステップＳ２の演算式を参照し、パン・チルト座標系Σ_ＰＴの目標点位置^ＰＴＸ_ａ、及び、雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａを算出する。
ステップＳ４において、パン・チルト角算出手段３４は、ステップＳ３で算出したパン・チルト座標系Σ_ＰＴの目標点位置^ＰＴＸ_ａ、及び、雲台座標系Σ_ＣＰの目標点位置^ＣＰＸ_ａから、カメラＣのパン・チルト角（ｐ，ｔ）を算出する。
ステップＳ５において、パン・チルト角選択手段３５は、ステップＳ４で算出した複数のパン・チルト角（ｐ，ｔ）のうち、前回算出したパン・チルト角（ｐ，ｔ）に最も近いものを選択する。

［作用・効果］
以上のように、パン・チルト角算出装置３０は、カメラＣの光学中心と回転中心との位置ずれを考慮したカメラモデルで求めたパラメータと、パン・チルト座標系Σ_ＰＴの及び雲台座標系Σ_ＣＰの間で回転中心から目標点Ａまでの距離Ｌが一致することを示す第１式とを用いる。これにより、パン・チルト角算出装置３０は、正確なパン・チルト角（ｐ，ｔ）を算出できるので、カメラＣを正確に方向制御することができる。

（変形例）
以上、本発明の実施形態を詳述してきたが、本発明は前記した実施形態に限られるものではなく、本発明の要旨を逸脱しない範囲の設計変更等も含まれる。
パン・チルト角選択手段は、予め設定されたカメラの可動域内でパン・チルト角を選択してもよい。

前記した実施形態では、パン・チルト角算出装置を独立したハードウェアとして説明したが、本発明は、これに限定されない。例えば、本発明は、コンピュータが備えるＣＰＵ、メモリ、ハードディスク等のハードウェア資源を、前記したパン・チルト角算出装置として動作させるプログラムで実現することもできる。これらのプログラムは、通信回線を介して配布してもよく、ＣＤ－ＲＯＭやフラッシュメモリ等の記録媒体に書き込んで配布してもよい。

１ カメラ制御システム
１０ カメラ校正装置
２０ カメラパラメータ記憶装置
３０ パン・チルト角算出装置
３１ パラメータ入力手段（入力手段）
３２ 演算式設定手段（設定手段）
３３ 目標点算出手段
３４ パン・チルト角算出手段
３５ パン・チルト角選択手段
Ｃ カメラ

Claims (5)

1. カメラの光学中心と回転中心との位置ずれを考慮したカメラモデルで予め求めたカメラパラメータを用いて、目標点に対する前記カメラのパン・チルト角を算出するパン・チルト角算出装置であって、
   世界座標系から前記回転中心におけるパン角及びチルト角がゼロ度のときの雲台座標系への変換を示す外部パラメータ及び、前記世界座標系の目標点位置を入力する入力手段と、
   前記光学中心を原点としたカメラ座標系の目標点位置をスケール係数に依存する関数と、前記回転中心におけるパン・チルト後のパン・チルト座標系及び前記雲台座標系の間で前記回転中心から前記目標点までの距離が一致することを示す第１式と、前記外部パラメータにより前記目標点位置を前記世界座標系から前記雲台座標系に変換する第２式と、前記パン・チルト座標系の目標点位置を前記スケール係数による回転行列及び並進ベクトルで表した第３式と、を予め設定する設定手段と、
   前記第２式により前記目標点位置を前記雲台座標系に変換し、前記第１式に前記第２式及び前記第３式を代入して前記スケール係数を算出し、算出した前記スケール係数を前記関数に代入して前記カメラ座標系の目標点位置を算出し、前記カメラ座標系の目標点位置を前記パン・チルト座標系に変換する目標点算出手段と、
   前記パン・チルト座標系及び前記雲台座標系の目標点位置から前記パン・チルト角を算出するパン・チルト角算出手段と、
   を備えることを特徴とするパン・チルト角算出装置。
2. 前記設定手段は、
   前記カメラ座標系の目標点位置^ＣＸ_ａを前記スケール係数ｓに依存する関数^ＣＸ_ａ（ｓ）として、前記カメラ座標系の座標［^ＣＸ（ｓ），^ＣＹ（ｓ），^ＣＺ（ｓ）］、前記目標点の画像座標［ｕ_ａ，ｖ_ａ］、画像中心の座標［ｃ_ｕ，ｃ_ｖ］、画像座標ｕ，ｖ方向それぞれの焦点距離ｆ_ｕ，ｆ_ｖ、及び、前記カメラ座標系で目標点位置^ＣＸ_ａをｚ＝１に正規化したときの目標点位置^ＣＸ´_ａが含まれる式（１５）を予め設定し、
   

   

   前記第１式として、前記雲台座標系の目標点位置^ＣＰＸ_ａ、前記パン・チルト座標系の目標点位置^ＰＴＸ_ａ、及び、前記距離Ｌが含まれる式（１６）を予め設定し、
   

   

   前記第２式として、前記外部パラメータ^ＣＰＭ_Ｗ、及び、前記世界座標系の目標点位置^ｗＸ_ａが含まれる式（１７）を予め設定し（但し、添え字～が同次座標系を示す）、
   

   

   前記第３式として、前記スケール係数ｓ、前記カメラ座標系から前記パン・チルト座標系への回転行列^ＰＴＲ_Ｃ、及び、前記カメラ座標系から前記パン・チルト座標系への並進ベクトル^ＰＴｔ´が含まれる式（１９）を予め設定する
   

   

   ことを特徴とする請求項１に記載のパン・チルト角算出装置。
3. 前記パン・チルト角算出手段が算出した複数の前記パン・チルト角のうち、前回算出した前記パン・チルト角に最も近いものを選択するパン・チルト角選択手段、をさらに備えることを特徴とする請求項１又は請求項２に記載のパン・チルト角算出装置。
4. 前記パン・チルト角算出手段は、前記パン・チルト角（ｐ，ｔ）における前記雲台座標系から前記パン・チルト座標系への回転行列^ＰＴＲ_ＣＰ（ｐ，ｔ）が含まれる式（２１）及び式（２２）を用いて、
   

   

   

   前記雲台座標系の目標点位置^ＣＰＸ_ａ、及び、前記パン・チルト座標系の目標点位置^ＰＴＸ_ａから、前記パン・チルト角（ｐ，ｔ）を算出することを特徴とする請求項１から請求項３の何れか一項に記載のパン・チルト角算出装置。
5. コンピュータを、請求項１から請求項４の何れか一項に記載のパン・チルト角算出装置として機能させるためのプログラム。

Images