特許 7582825 移動体位置決定装置、移動体位置決定方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-11-05

(45)【発行日】2024-11-13

(54)【発明の名称】移動体位置決定装置、移動体位置決定方法及びプログラム

(51)【国際特許分類】

   G01B 11/00 20060101AFI20241106BHJP

   G01B 11/26 20060101ALI20241106BHJP

【ＦＩ】

G01B11/00 H

G01B11/26 H

【請求項の数】 9

(21)【出願番号】P 2020157122

(22)【出願日】2020-09-18

(65)【公開番号】P2022050929

(43)【公開日】2022-03-31

【審査請求日】2022-02-02

【審判番号】

【審判請求日】2023-07-18

(73)【特許権者】

【識別番号】000001443

【氏名又は名称】カシオ計算機株式会社

(74)【代理人】

【識別番号】100095407

【弁理士】

【氏名又は名称】木村 満

(72)【発明者】

【氏名】飯塚 宣男

【合議体】

【審判長】濱本 禎広

【審判官】小島 寛史

【審判官】佐々木 祐

(56)【参考文献】

【文献】特開２００５－３１５７４６（ＪＰ，Ａ）

【文献】特開２０１１－１３４０５８（ＪＰ，Ａ）

【文献】特開２００６－２０９７７０（ＪＰ，Ａ）

【文献】特開２０００－３３７８８７（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

G01C15/00

G01B11/00-11/30

G05D 1/02

(57)【特許請求の範囲】

【請求項1】

３次元空間における高さが異なる任意の位置に設置されている２つの発光装置を含む前記３次元空間の座標位置が既知である複数の発光装置の中から、移動体に取り付けられた撮像装置により撮像される少なくとも２つの発光装置を含む画像に基づいて、前記少なくとも２つの発光装置の画像内の座標位置を取得し、
前記撮像装置から前記少なくとも２つの発光装置の画像内の座標位置までの複数の３次元方向ベクトルを取得し、
取得された前記複数の３次元方向ベクトルと、前記撮像装置と前記少なくとも２つの発光装置それぞれとの前記３次元空間における高低差に基づいて、前記３次元空間における高低差の次元とは異なる複数の２次元のベクトルを取得し、
２次元空間における前記撮像装置と前記少なくとも２つの発光装置の各々との間の位置関係と、前記位置関係に対応する前記複数の２次元のベクトルの各々とが略一致するよう、前記複数の２次元のベクトルの少なくとも１つの２次元のベクトルの長さ及び方向の少なくとも一方を調整し、当該長さ及び方向の少なくとも一方を調整した前記複数の２次元のベクトルと前記撮像装置の姿勢データとに基づいて、前記３次元空間における少なくとも前記移動体の座標位置及び前記移動体が向いている方位のいずれかを決定する、
処理を実行する移動体位置決定装置。

【請求項2】

前記画像を含む、撮像タイミングがそれぞれ異なる複数の画像に基づいて、夫々異なる発光パターンで発光する前記少なくとも２つの発光装置を識別する、
処理を実行する請求項１に記載の移動体位置決定装置。

【請求項3】

前記撮像装置の焦点を始点とし、前記少なくとも２つの発光装置の画像内の座標位置を終点とする前記複数の３次元方向ベクトルと、前記撮像装置が向いている方向を示す前記姿勢データと、を取得し、
取得された前記複数の３次元方向ベクトル、及び、前記少なくとも２つの発光装置の前記３次元空間における高低差に基づいて、前記撮像装置から前記少なくとも２つの発光装置までの３次元空間上の各距離と、前記撮像装置が向く光軸方位と、を取得する、
処理を実行する請求項１又は２に記載の移動体位置決定装置。

【請求項4】

取得された前記各距離のうちの長い方を調整する、
処理を実行する請求項３に記載の移動体位置決定装置。

【請求項5】

前記移動体の座標位置決定のために、前記画像における外縁部に存在する発光装置の座標位置を使用しないように制御する、
処理を実行する請求項１乃至４のいずれかに記載の移動体位置決定装置。

【請求項6】

第１発光装置及びその他の発光装置を含む画像を示す第１画像の取得後に取得される画像が、前記第１発光装置のみを含みその他のいずれの発光装置を含まない第２画像であった場合に、算出される前記移動体の移動ベクトルに基づいて、前記移動体の座標位置を決定する、
処理を実行する請求項１乃至５のいずれかに記載の移動体位置決定装置。

【請求項7】

前記移動体に取り付けられた複数の前記撮像装置から取得される画像に従って、前記移動体の座標位置を決定する、
処理を実行する請求項１乃至６のいずれかに記載の移動体位置決定装置。

【請求項8】

コンピュータに、請求項１乃至７のいずれかに記載の移動体位置決定装置の処理を実行させる方法。

【請求項9】

コンピュータに、請求項１乃至７のいずれかに記載の移動体位置決定装置の処理を実行させるプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体位置決定装置、移動体位置決定方法及びプログラムに関する。

【背景技術】

【0002】

従来より、移動体に取り付けられた撮像装置が空間内の発光装置であるマーカを撮像し、その撮像によって得られた画像に基づいて、移動体の３次元空間の位置や方位を算出する技術が存在する（例えば、特許文献１参照）。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１４－１５７０５１号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、上述した技術では、マーカを同一平面上に配置しなければならないという制約が存在し、マーカの設置の自由度、更には、位置と方位の算出の自由度が低いと言う問題があった。

【0005】

本願発明はこのような問題点に鑑みてなされたものであり、位置と方位の算出の自由度を向上させることを目的とする。

【課題を解決するための手段】

【0006】

上記目的を達成するため、本発明に係る移動体位置決定装置は、
３次元空間における高さが異なる任意の位置に設置されている２つの発光装置を含む前記３次元空間の座標位置が既知である複数の発光装置の中から、移動体に取り付けられた撮像装置により撮像される少なくとも２つの発光装置を含む画像に基づいて、前記少なくとも２つの発光装置の画像内の座標位置を取得し、
前記撮像装置から前記少なくとも２つの発光装置の画像内の座標位置までの複数の３次元方向ベクトルを取得し、
取得された前記複数の３次元方向ベクトルと、前記撮像装置と前記少なくとも２つの発光装置それぞれとの前記３次元空間における高低差に基づいて、前記３次元空間における高低差の次元とは異なる複数の２次元のベクトルを取得し、
２次元空間における前記撮像装置と前記少なくとも２つの発光装置の各々との間の位置関係と、前記位置関係に対応する前記複数の２次元のベクトルの各々とが略一致するよう、前記複数の２次元のベクトルの少なくとも１つの２次元のベクトルの長さ及び方向の少なくとも一方を調整し、当該長さ及び方向の少なくとも一方を調整した前記複数の２次元のベクトルと前記撮像装置の姿勢データとに基づいて、前記３次元空間における少なくとも前記移動体の座標位置及び前記移動体が向いている方位のいずれかを決定する、
処理を実行する。

【発明の効果】

【0007】

本発明によれば、位置と方位の算出の自由度を向上させることができる。

【図面の簡単な説明】

【0008】

【図1】本発明の実施形態に係る可視光通信システムの一例を示す図である。

【図2】サーバの構成の一例を示す図である。

【図3】空間におけるカメラとマーカの配置の一例を示す図である。

【図4】マーカ３次元位置テーブルの一例を示す図である。

【図5】カメラ内部パラメータの一例を示す図である。

【図6】（ａ）は歪み補正前の画像の一例、（ｂ）は歪み補正後の画像の一例を示す図である。

【図7】マーカの画像位置及び３次元位置の一例を示す図である。

【図8】ＸＹ平面ベクトルの一例を示す図である。

【図9】（ａ）はＸＹ平面ベクトルの調整前の一例、（ｂ）、（ｃ）はＸＹ平面ベクトルの調整後の一例を示す図である。

【図10】カメラ位置候補線の一例を示す図である。

【図11】カメラ位置候補線と延長ベクトルの交点取得の一例を示す図である。

【図12】サーバの動作の一例を示す第１のフローチャートである。

【図13】サーバの動作の一例を示す第２のフローチャートである。

【図14】サーバの動作の一例を示す第３のフローチャートである。

【図15】カメラが２台の場合のＸＹ平面ベクトルの一例を示す図である。

【発明を実施するための形態】

【0009】

以下、図面を参照して、本発明の実施形態に係る移動体位置決定装置としての可視光通信システムを説明する。

【0010】

図１は、可視光通信システムの構成を示す図である。図１に示すように、可視光通信システム１は、空間５００内を移動する移動体としてのフォークリフト１００と、フォークリフト１００に取り付けられたカメラ１０１ａと、空間５００の天井や側面等に取り付けられた光源であるマーカ１０２ａ、１０２ｂ、１０２ｃ（以下、マーカ１０２ａ、１０２ｂ、１０２ｃのそれぞれを限定しない場合には、適宜「マーカ１０２」と称する）と、移動体位置決定装置に対応するサーバ２００とを含んで構成される。マーカ１０２は、図示しないＬＥＤ（Light Emitting Diode）を含む。空間５００は、Ｘ軸の座標（Ｘ座標）、Ｙ軸の座標（Ｙ座標）、Ｚ軸の座標（Ｚ座標）からなる３次元のパラメータで定義でき、これにより当該空間５００内に存在する物体の位置を特定可能になっている。ここで、Ｘ軸及びＹ軸は双方とも水平方向であり、互いに直交し、Ｚ軸は垂直方向であり、Ｘ軸及びＹ軸と直交する。

【0011】

本実施形態において、フォークリフト１００に取り付けられたカメラ１０１ａは、マーカ１０２を撮像する。マーカ１０２は、空間５００における３次元位置が既知であり、各種の送信対象の情報で色変調を行い、時系列に沿ってその発光色を変化させて発光することによりその情報を送信する。なお、変調方法は、送信対象の情報をビットデータに変換し、このデータを所定の規則性を持たせて光の三原色（Ｒ（赤）Ｇ（緑）Ｂ（青））の順序に割り当てる方法を採用する。

【0012】

一方、サーバ２００は、カメラ１０１ａによる時系列的に連続した撮像によって得られた光の画像における発光色の変化を復調してマーカ１０２が発する情報を取得するとともに、空間５００におけるカメラ１０１ａの３次元位置をフォークリフト１００の３次元位置として算出する。

【0013】

図２は、サーバ２００の構成の一例を示す図である。図２に示すように、サーバ２００は、制御部２０２、無線通信部２０３、画像処理部２０４、メモリ２０５、操作部２０６、表示部２０７及び通信部２０８を含む。

【0014】

図３は、空間５００におけるカメラ１０１ａとマーカ１０２の配置の一例を示す図である。本実施形態において、サーバ２００は、空間５００における既知であるマーカ１０２の３次元位置に基づいて、未知であるカメラ１０１ａの３次元位置を算出する。

【0015】

カメラ１０１ａは、図示しないレンズを介して入光された光学像を撮像（受光）し、その撮像画角内の画像信号をデジタルデータに変換して画像を生成する。また、カメラ１０１ａは、撮像と画像の生成とを時間的に連続して行い、連続する画像をサーバ２００へ無線通信により送信する。サーバ２００内の無線通信部２０３は、画像を受信して画像処理部２０４へ出力する。画像における各画素や画像領域は、Ｘ座標、Ｙ座標からなる２次元の座標平面でその位置が定義される。

【0016】

再び、図２に戻って説明する。画像処理部２０４は、無線通信部２０３から出力された画像（デジタルデータ）をそのまま制御部２０２へ出力するとともに、当該画像について、表示部２０７に表示させるべく、画質や画像サイズを調整して制御部２０２へ出力する。また、画像処理部２０４は、ＪＰＥＧ（Joint Photographic Experts Group）、ＭＰＥＧ（Moving Photographic Experts Group）等の圧縮符号化方式にて符号化、静止画ファイル化、または、動画ファイル化する機能を有する。

【0017】

制御部２０２は、プロセッサであるＣＰＵ（Central Processing Unit）によって構成される。制御部２０２は、メモリ２０５に記憶されたプログラム（例えば、後述する図１２、図１３、図１４に示すサーバ２００の動作を実現するためのプログラム）に従ってソフトウェア処理を実行することにより、サーバ２００が具備する各種機能を制御する。

【0018】

メモリ２０５は、例えばＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory）である。メモリ２０５は、サーバ２００における制御等に用いられる各種情報（プログラム等）を記憶する。

【0019】

また、メモリ２０５は、図４に示すマーカ３次元位置テーブルや図５に示すカメラ内部パラメータを記憶している。図４に示すマーカ３次元位置テーブルは、マーカ１０２ａ、１０２ｂ、１０２ｃのそれぞれの自己を一意に特定可能なＩＤ（Identification）と、空間５００における当該マーカ１０２ａ、１０２ｂ、１０２ｃの３次元位置（Ｘ座標、Ｙ座標、Ｚ座標）とを対応付けたものである。

【0020】

また、図５に示すカメラ内部パラメータは、画像の歪みを補正する際に用いる画像歪み補正データと、カメラ１０１ａの高さ（Ｚ座標）と、カメラ１０１ａが向いている方向を示す姿勢データとしてのカメラ１０１ａのＺ軸を回転軸とする回転を示す３×３行列であるカメラ方位行列Ｒｚ（θ）と、カメラ１０１ａが向いている方向を示す姿勢データとしてのカメラ１０１ａのＸ軸を回転軸とする回転とＹ軸を回転軸とする回転とを示す３×３行列であるカメラ設置行列Ｒｃとを含んで構成される。

【0021】

再び、図２に戻って説明する。操作部２０６は、テンキーやファンクションキー等によって構成され、ユーザの操作内容を入力するために用いられるインタフェースである。表示部２０７は、例えば、ＬＣＤ（Liquid Crystal Display）、ＰＤＰ（Plasma Display Panel）、ＥＬ（Electroluminescence）ディスプレイ等によって構成される。表示部２０７は、制御部２０２から出力された画像信号に従って画像を表示する。通信部２０８は、例えばＬＡＮ（Local Area Network）カードである。通信部２０８は、外部の通信装置との間で通信を行う。

【0022】

制御部２０２には、画像取得部２３０、マーカ特定部２３２、画像位置補正部２３４及び３次元位置取得部２３６の各機能が構成される。

【0023】

画像取得部２３０は、画像処理部２０４からの画像を取得する。マーカ特定部２３２は、画像内のマーカ１０２の位置（画像位置：画像に対応する２次元平面におけるＸ座標、Ｙ座標）を検出する。ここで、空間５００内におけるマーカ１０２ａ、１０２ｂ、１０２ｃは、自己を一意に特定可能なＩＤで変調された、Ｒ（赤）Ｇ（緑）Ｂ（青）の３色のパターンで巡回的に変化する光を発する。

【0024】

マーカ特定部２３２は、カメラ１０１ａの撮像によって得られた各画像に含まれる巡回的な３色のパターンの光を検出する。更に、マーカ特定部２３２は、この３色の発光のパターンに対応するＩＤの検出、更にＩＤへの復調を試みる。ＩＤが検出された場合、マーカ特定部２３２は、画像内における、そのＩＤに対応するマーカ１０２の画像位置を取得する。

【0025】

更に、マーカ特定部２３２は、メモリ２０５に記憶されたマーカ３次元位置テーブルを参照することにより、取得したＩＤに対応するマーカ１０２の空間５００における３次元位置を取得する。

【0026】

画像位置補正部２３４は、マーカ１０２の画像位置を補正する。具体的には、図６（ａ）に示すように、通常画像６０１ａには歪みが生じており、その歪みは画像の中心からの距離が長いほど大きくなる。そこで、画像位置補正部２３４は、図５のカメラ内部パラメータの画像歪み補正データを用いて、歪みを補正する。画像歪み補正データは、画像の中心からの距離が長いほど補正量が大きくなるデータである。これにより、図６（ｂ）に示すように歪みが補正された画像６０１ｂとなり、当該画像６０１ｂにおけるマーカ１０２ａ、１０２ｂ、１０２ｃの位置（画像位置）が補正されることになる。

【0027】

また、画像位置補正部２３４は、歪み補正後の画像のうち、四隅に存在するマーカ１０２をカメラ１０１ａの３次元位置の取得の際に使用しないように設定する。例えば、図６（ｂ）に示すように、画像６０１ｂの四隅には、三角形の無効領域６０３ａ、６０３ｂ、６０３ｃ、６０３ｄがそれぞれ設定される。マーカ１０２ｃは、無効領域６０３ａ内に存在するため、当該マーカ１０２ｃはカメラ１０１ａの３次元位置の取得の際に使用されず、マーカ１０２ａ、１０２ｂのみがカメラ１０１ａの３次元位置の取得の際に使用されることになる。

【0028】

３次元位置取得部２３６は、空間５００におけるカメラ１０１ａの３次元位置を取得する。具体的には、３次元位置取得部２３６は、図７に示す画像領域７００におけるマーカ１０２ａの画像位置６１０ａ及びマーカ１０２ｂの画像位置と、マーカ３次元テーブルを参照して得られたマーカ１０２ａの３次元位置及びマーカ１０２ｂの３次元位置と、メモリ２０５に記憶された姿勢データであるカメラ方位行列Ｒｚ（θ）及びカメラ設置行列Ｒｃとを用いて既知の手法により、空間５００におけるカメラ１０１ａの３次元位置Ｃ＝（ｃｘ，ｃｙ，ｃｚ）を算出する。

【0029】

具体的には、３次元位置取得部２３６は、カメラ１０１ａを中心とする３次元空間の座標系であるカメラ基準相対系において、マーカ１０２ａの３次元位置と、カメラ内部パラメータ及び姿勢データに基づいて、カメラ１０１ａからマーカ１０２ａへ向かう３次元方向ベクトル（ｖｘ１，ｖｙ１，ｖｚ１）を算出するとともに、マーカ１０２ｂの画像位置と姿勢データとに基づいて、カメラ１０１ｂからマーカ１０２へ向かう３次元方向ベクトル（ｖｘ２，ｖｙ２，ｖｚ２）とを算出する。

【0030】

次に、３次元位置取得部２３６は、カメラ１０１ａからマーカ１０２ａへ向かう３次元方向ベクトル（ｖｘ１，ｖｙ１，ｖｚ１）を３次元空間におけるカメラ１０１ａとマーカ１０２ａとの位置関係に当てはめるために、マーカ１０２ａとカメラ１０１ａの高低差と、カメラ設置行列Ｒｃ１とに基づいて、既知の手法により３次元方向ベクトル（ｖｘ１，ｖｙ１，ｖｚ１）をＸＹ平面に投影したＸＹ平面ベクトルＶ１を算出する。同様に、３次元位置取得部２３６は、カメラ１０１ａからマーカ１０２ｂへ向かう３次元方向ベクトル（ｖｘ２，ｖｙ２，ｖｚ２）を３次元空間におけるカメラ１０１ａとマーカ１０２ｂとの位置関係に当てはめるために、マーカ１０２ｂとカメラ１０１ａの高低差と、カメラ設置行列Ｒｃ２とに基づいて、３次元方向ベクトル（ｖｘ２，ｖｙ２，ｖｚ２）をＸＹ平面に投影したＸＹ平面ベクトルＶ２を算出する。図８は、ＸＹ平面ベクトルＶ１、Ｖ２の一例を示す図である。

【0031】

更に、３次元位置取得部２３６は、ＸＹ平面ベクトルＶ１とＸＹ平面ベクトルＶ２とを調整する。この調整は、カメラ方位行列Ｒｚ（θ）が示すＺ軸回りの回転角度θの誤差等によって、算出した空間５００におけるカメラ１０１ａの３次元位置Ｃ＝（ｃｘ，ｃｙ，ｃｚ）に誤差が生じている場合に、その誤差を小さくするためである。

【0032】

例えば、図９（ａ）に示すように、ＸＹ平面上において、カメラ１０１ａとマーカ１０２ａ、１０２ｂとの位置関係と、ＸＹ平面ベクトルＶ１及びＸＹ平面ベクトルＶ２とが一致しない場合、３次元位置取得部２３６は、ＸＹ平面ベクトルＶ１とＸＹ平面ベクトルＶ２とを調整する。

【0033】

例えば、３次元位置取得部２３６は、カメラ方位行列Ｒｚ（θ）が示すＺ軸回りの回転角度θを調整することにより、カメラ１０１ａとマーカ１０２ａ、１０２ｂとの位置関係と、ＸＹ平面ベクトルＶ１及びＸＹ平面ベクトルＶ２とを一致させることを試みる。

【0034】

また、３次元位置取得部２３６は、図９（ａ）では、ＸＹ平面ベクトルＶ１の方がＸＹ平面ベクトルＶ２より長いため、ＸＹ平面ベクトルＶ１の方が誤差が大きいと見なし、図９（ｂ）に示すように、ＸＹ平面ベクトルＶ２の長さ及び方向を調整することなく、カメラ１０１ａとマーカ１０２ｂとの位置関係と、ＸＹ平面ベクトルＶ２とを一致させる一方、ＸＹ平面ベクトルＶ１の長さ及び方向を調整して、カメラ１０１ａとマーカ１０２ｂとの位置関係と、ＸＹ平面ベクトルＶ２とを一致させる。

【0035】

また、３次元位置取得部２３６は、図９（ｃ）に示すように、ＸＹ平面ベクトルＶ１の長さ及び方向の調整量と、ＸＹ平面ベクトルＶ１の長さ及び方向の調整量との比率を、ＸＹ平面ベクトルＶ１とＸＹ平面ベクトルＶ２との長さの比率に一致させて、ＸＹ平面ベクトルＶ１及びＸＹ平面ベクトルＶ２双方の長さ及び方向を調整して、カメラ１０１ａとマーカ１０２ｂとの位置関係と、ＸＹ平面ベクトルＶ２とを一致させる。

【0036】

３次元位置取得部２３６は、調整後のＸＹ平面ベクトルＶ１、Ｖ２、カメラ方位行列Ｒｚ（θ）を用いて、３次元方向ベクトル（ｖｘ１，ｖｙ１，ｖｚ１）、更には、空間５００におけるカメラ１０１ａの３次元位置Ｃ＝（ｃｘ，ｃｙ，ｃｚ）を算出するとともに、カメラ方位行列Ｒｚ（θ）及びカメラ設置行列Ｒｃに基づいてカメラ１０１ａの方位を算出する。

【0037】

更に、３次元位置取得部２３６は、メモリ２０５に記憶されたカメラ１０１ａの３次元位置Ｃ＝（ｃｘ，ｃｙ，ｃｚ）のうち、最良の３次元位置Ｃ＝（ｃｘ，ｃｙ，ｃｚ）を選択し、最終的なカメラ１０１ａの３次元位置（移動体の座標位置）とする。最良の３次元位置とは、例えば、カメラ１０１ａのそれぞれの３次元位置Ｃのうち、対応する再投影誤差が最小となる位置や、再投影誤差の逆数に従った重み平均により算出される位置である。３次元位置取得部２３６は、求めたカメラ１０１ａの３次元位置及び方位と画像に含まれるマーカ１０２のＩＤとを対応付けてメモリ２０５に記憶させる。

【0038】

また、当初はカメラ１０１ａによって２つ以上のマーカ１０２が撮像されていたが、その後にカメラ１０１ａの移動によって１つのマーカ１０２のみが撮像されることになった場合には、その１つのマーカ１０２の画像位置に基づいて、最終的なカメラ１０１ａの３次元位置及び方位を算出する。

【0039】

具体的には、３次元位置取得部２３６は、カメラ１０１ａを中心とする３次元空間の座標系であるカメラ基準相対系においてカメラ１０１ａからマーカ１０２への３次元方向ベクトルを求める。３次元方向ベクトルは、マーカ１０２の画像位置と姿勢データとに基づいて求められる。図１０に示すように、３次元方向ベクトルＶ１００が求められると、３次元空間におけるカメラ１０１ａの位置（３次元位置）は、マーカ１０２の直下でカメラ１０１ａと同一平面（ＸＹ平面）上の位置Ｃ１０を中心とする円の外縁部であるカメラ位置候補線７１０の線上の何れかの位置となる。

【0040】

更に、３次元位置取得部２３６は、３次元方向ベクトルについてＸＹ平面との傾きを求め、当該傾きと、カメラ１０１ａとマーカ１０２との高低差とに基づいて、カメラ１０１ａとマーカ１０２との水平距離（ＸＹ平面上の距離）を求める。

【0041】

次に、３次元位置取得部２３６は、メモリ２０５に記憶しているマーカ１０２のＩＤに対応するカメラ１０１ａの３次元位置及び方位の履歴に基づいて、図１１に示すように、前々回の測定によるカメラ１０１ａの３次元位置（前々回位置）Ｐ１を始点とし、前回の測定によるカメラ１０１ａの３次元位置（前回位置）を終点とする移動ベクトルＶ１１０を求め、その移動ベクトルＶ１１０を延長した延長ベクトルＶ１２０を求める。更に、３次元位置取得部２３６は、延長ベクトルＶ１２０とカメラ位置候補線７１０との交点Ｃ２０を現在のカメラ１０１ａの３次元位置Ｃ２０とする。

【0042】

次に、３次元位置取得部２３６は、画像内のマーカ１０２の横移動（ＸＹ平面上での移動）からカメラ１０１ａの方位の変化を求め、その方位の変化量を前回の測定における方位に加えることにより、現在のカメラ１０１ａの方位とする。更に、３次元位置取得部２３６は、求めたカメラ１０１ａの３次元位置及び方位と、画像に含まれるマーカ１０２のＩＤとを対応付けてメモリ２０５に記憶させる。

【0043】

次に、フローチャートを参照して、サーバ２００の動作を説明する。図１２、図１３及び図１４は、サーバ２００の動作の一例を示すフローチャートである。

【0044】

制御部２０２内の画像取得部２３０は、画像処理部２０４からのカメラ１０１ａによって撮像された画像を取得する（ステップＳ１０１）。次に、マーカ特定部２３２は、画像を解析して、マーカ１０２のＩＤの取得を試み、ＩＤを取得したか否かを判定する（ステップＳ１０２）。ＩＤが取得されなかった場合には（ステップＳ１０２；ＮＯ）、一連の動作が終了する。

【0045】

一方、ＩＤが取得された場合には（ステップＳ１０２；ＹＥＳ）、マーカ特定部２３２は、マーカ３次元位置テーブルを参照し、取得したＩＤに対応するマーカ１０２の空間５００における３次元位置を取得する（ステップＳ１０３）。

【0046】

画像位置補正部２３４は、カメラ内部パラメータの画像歪み補正データに基づいて、画像の歪みを補正する（ステップＳ１０４）。更に、画像位置補正部２３４は、歪み補正後の画像のうち、無効領域６０３ａ等に存在するマーカ１０２をカメラ１０１ａの３次元位置の取得の際に使用しないように設定する（ステップＳ１０５）。

【0047】

次に、３次元位置取得部２３６は、画像内にマーカ１０２が２点以上存在するか否かを判定する（ステップＳ１０６）。画像内にマーカ１０２が２点以上存在する場合には（ステップＳ１０６；ＹＥＳ）、図１３に示す動作に移行し、３次元位置取得部２３６は、マーカ１０２ａの３次元位置、カメラ内部パラメータ及び姿勢データに基づいて、カメラ基準相対系においてカメラ１０１ａからマーカ１０２へ向かう３次元方向ベクトルを算出する（ステップＳ２０１）。

【0048】

次に、３次元位置取得部２３６は、マーカ１０２とカメラ１０１ａの高低差と、カメラ設置行列Ｒｃとに基づいて、３次元方向ベクトルをＸＹ平面に投影したＸＹ平面ベクトルを算出する（ステップＳ２０２）。

【0049】

次に、３次元位置取得部２３６は、２点以上のマーカ１０２のうち、２つのマーカ１０２を終点とする２つのＸＹ平面ベクトルを選択する（ステップＳ２０３）。

【0050】

次に、３次元位置取得部２３６は、２つのＸＹ平面ベクトルを調整する（ステップＳ２０４）。次に、３次元位置取得部２３６は、調整後のＸＹ平面ベクトル、カメラ方位行列Ｒｚ（θ）を用いて、空間５００におけるカメラ１０１ａの３次元位置を算出するとともに、カメラ方位行列Ｒｚ（θ）及びカメラ設置行列Ｒｃに基づいてカメラ１０１ａの方位を算出し、画像内のマーカ１０２のＩＤとともにメモリ２０５に記憶させる（ステップＳ２０５）。

【0051】

更に、３次元位置取得部２３６は、２つのＸＹ平面ベクトルの組み合わせの全てについてステップＳ２０３～ステップＳ２０５の処理が終了したか否かを判定する（ステップＳ２０６）。終了していない場合には（ステップＳ２０６；ＮＯ）、ステップＳ２０３以降の動作が繰り返される。一方、終了した場合には（ステップＳ２０６；ＹＥＳ）、３次元位置取得部２３６は、最良のカメラ１０１ａの３次元位置及び方位を選択する（ステップＳ２０７）。

【0052】

一方、図１１のステップＳ１０６において、画像内にマーカ１０２が２点以上存在しない場合には（ステップＳ１０６；ＮＯ）、すなわち、画像内にマーカ１０２が１点のみである場合には、図１４に示す動作に移行し、３次元位置取得部２３６は、その１点のマーカ１０２のＩＤを、過去のカメラ１０１ａの３次元位置及び方位の取得の際に既に取得しているか否かを判定する（ステップＳ３０１）。ＩＤを取得済みでない場合には（ステップＳ３０１；ＮＯ）、一連の動作が終了する。

【0053】

一方、ＩＤを取得済みである場合には（ステップＳ３０１；ＹＥＳ）、３次元位置取得部２３６は、カメラ１０１ａを中心とする３次元空間の座標系であるカメラ基準相対系においてカメラ１０１ａからマーカ１０２への３次元方向ベクトルを求める。更に、３次元位置取得部２３６は、３次元方向ベクトルについてＸＹ平面との傾きを求め、当該傾きと、カメラ１０１ａとマーカ１０２との高低差とに基づいて、カメラ１０１ａとマーカ１０２との水平距離（ＸＹ平面上の距離）を求める（ステップＳ３０２）。

【0054】

次に、３次元位置取得部２３６は、メモリ２０５に記憶しているマーカ１０２のＩＤに対応するカメラ１０１ａの３次元位置及び方位の履歴に基づいて、前々回の測定によるカメラ１０１ａの３次元位置（前々回位置）から前回の測定によるカメラ１０１ａの３次元位置（前回位置）へ向かう移動ベクトルを求め、その移動ベクトルを延長した延長ベクトルを求める（ステップＳ３０３）。

【0055】

更に、３次元位置取得部２３６は、延長ベクトルとカメラ位置候補線との交点を現在のカメラ１０１ａの３次元位置とする（ステップＳ３０４）。

【0056】

次に、３次元位置取得部２３６は、画像内のマーカ１０２の横移動からカメラ１０１ａの方位の変化を求め、その方位の変化量を前回の測定における方位に加えることにより、現在のカメラ１０１ａの方位とする（ステップＳ３０５）。更に、３次元位置取得部２３６は、求めたカメラ１０１ａの３次元位置及び方位と、画像に含まれるマーカ１０２のＩＤとを対応付けてメモリ２０５に記憶させる（ステップＳ３０６）。

【0057】

このように本実施形態では、サーバ２００は、カメラ１０１ａによるマーカ１０２の撮像によって得られた画像を取得すると、当該画像の歪みを補正することによってマーカ１０２の位置を補正し、補正後の画像におけるマーカ１０２の画像位置をカメラ１０１ａ（フォークリフト１００）の３次元位置に変換する。本実施形態では、マーカ１０２の設置位置に制約はなく、フォークリフト１００の３次元位置と方位の算出の自由度を向上させることができる。

【0058】

また、本実施形態では、画像の歪みは当該画像の中心からの距離が長いほど大きくなることに鑑み、サーバ２００は、画像の中心からの距離が長いほど補正量が大きくなる画像歪み補正データを用いて画像補正を行うとともに、画像の四隅の無効領域６０３ａ等に存在するマーカ１０２をカメラ１０１ａの３次元位置の取得の際に使用しない。これにより、カメラ１０１ａの３次元位置及び方位の算出精度を向上させることができる。

【0059】

また、本実施形態では、サーバ２００は、ＸＹ平面ベクトルを調整しており、カメラ方位行列Ｒｚ（θ）が示すＺ軸回りの回転角度θの誤差等によって、カメラ１０１ａの３次元位置に誤差が生じている場合には、その誤差を小さくすることができる。

【0060】

また、本実施形態では、２つのマーカ１０２の組み合わせ毎に取得されるカメラ１０１ａの３次元位置及び方位に基づいて、最良のものを選択あるいは取得しており、カメラ１０１ａの３次元位置及び方位の算出精度を向上させることができる。

【0061】

また、本実施形態では、画像内にマーカ１０２が１つしか含まれない場合であっても、そのマーカ１０２の３次元位置を利用して過去にカメラ１０１ａの３次元位置及び方位を算出している場合には、現在のカメラ１０１ａの３次元位置及び方位を算出可能である。このため、カメラ１０１ａの３次元位置及び方位の算出の汎用性を向上させることができる。

【0062】

また、本実施形態では、各マーカ１０２が独立したＩＤを持ち、且つ、そのセットのＩＤ数は極めて多数にすることができるため、非常に広範囲に多数の異なるＩＤを有するマーカ１０２を配置することができる。このため、空間５００内においては、前の状態にかかわらず、２点のマーカ１０２のＩＤを受信した時点で直ちに、マーカ１０２までの絶対距離を求めることができる。

【0063】

なお、本発明は、上記実施形態の説明及び図面によって限定されるものではなく、上記実施形態及び図面に適宜変更等を加えることは可能である。

【0064】

上述した実施形態では、カメラ１０１ａのみであったが複数のカメラを用いてもよい。例えば、図１２に示すように、カメラ１０１ａの他にカメラ１０１ｂが設けられる場合には、カメラ１０１ａがマーカ１０２ａを撮像し、カメラ１０１ｂがマーカ１０２ｂを撮像するとともに、ＸＹ平面におけるカメラ１０１ａからカメラ１０１ｂに向かうＸＹ平面ベクトルＶ３が規定されることにより、フォークリフト１００の３次元位置を精度よく算出することができる。

【0065】

また、上述した実施形態では、マーカ１０２は、時系列に沿って発光色を変化させることによりＩＤを送信したが、マーカ１０２の３次元位置を送信してもよい。この場合には、サーバ２００は、発光のパターンに対応するマーカ１０２の３次元位置を検出することができる。このため、図４に示すマーカ３次元位置テーブルは不要である。

【0066】

また、マーカ１０２はＬＥＤに限定されない。例えば、表示装置を構成するＬＣＤ、ＰＤＰ、ＥＬディスプレイ等の一部に光源が構成されていてもよい。

【0067】

また、サーバ２００は、カメラ１０１ａ、１０１ｂからの画像を取得するものであれば、どのような装置でもよい。

【0068】

また、上記実施形態において、実行されるプログラムは、フレキシブルディスク、ＣＤ－ＲＯＭ（Compact Disc Read - Only Memory）、ＤＶＤ（Digital Versatile Disc）、ＭＯ（Magneto - Optical Disc）等のコンピュータで読み取り可能な記録媒体に格納して配布し、そのプログラムをインストールすることにより、上述の処理を実行するシステムを構成することとしてもよい。

【0069】

また、プログラムをインターネット等のネットワーク上の所定のサーバが有するディスク装置等に格納しておき、例えば、搬送波に重畳させて、ダウンロード等するようにしてもよい。

【0070】

なお、上述の機能を、ＯＳ（Operating System）が分担して実現する場合又はＯＳとアプリケーションとの協働により実現する場合等には、ＯＳ以外の部分のみを媒体に格納して配布してもよく、また、ダウンロード等してもよい。

【0071】

以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。

【0072】

（付記１）
３次元空間における高さが異なる任意の位置に設置されている２つの発光装置を含む前記３次元空間の座標位置が既知である複数の発光装置の中から、移動体に取り付けられた撮像装置により撮像される少なくとも２つの発光装置を含む画像に基づいて、前記少なくとも２つの発光装置の画像内の座標位置を取得し、
前記３次元空間の座標位置及び前記画像内の座標位置に基づいて、前記３次元空間における少なくとも前記移動体の座標位置及び前記移動体が向いている方位のいずれかを決定する、
処理を実行する移動体位置決定装置。

【0073】

（付記２）
前記画像を含む、撮像タイミングがそれぞれ異なる複数の画像に基づいて、夫々異なる発光パターンで発光する前記少なくとも２つの発光装置を識別する、
処理を実行する請求項１に記載の移動体位置決定装置。

【0074】

（付記３）
前記撮像装置の焦点を始点とし、前記少なくとも２つの発光装置の画像内の座標位置を終点とする複数の３次元方向ベクトルと、撮像装置が向いている方向を示す姿勢データと、を取得し、
取得された前記複数の３次元方向ベクトル、及び、前記少なくとも２つの発光装置の前記３次元空間における高低差に基づいて、前記撮像装置から前記少なくとも２つの発光装置までの３次元空間上の各距離と、前記撮像装置が向く光軸方位と、を取得する、
処理を実行する付記１又は２に記載の移動体位置決定装置。

【0075】

（付記４）
取得された前記各距離のうちの長い方を調整する、
処理を実行する付記３に記載の移動体位置決定装置。

【0076】

（付記５）
前記複数の３次元方向ベクトルと、取得された前記各距離と、に基づいて算出される前記３次元空間の座標位置とのずれ量の比に従って、前記各距離を調整する、
処理を実行する付記３に記載の移動体位置決定装置。

【0077】

（付記６）
前記移動体の座標位置決定のために、前記画像における外縁部に存在する発光装置の座標位置を使用しないように制御する、
処理を実行する付記１乃至５のいずれかに記載の移動体位置決定装置。

【0078】

（付記７）
第１発光装置及びその他の発光装置を含む画像を示す第１画像の取得後に取得される画像が、前記第１発光装置のみを含みその他のいずれの発光装置を含まない第２画像であった場合に、算出される前記第１発光装置移動体の移動ベクトルに基づいて、前記移動体の座標位置を決定する、
処理を実行する付記１乃至６のいずれかに記載の移動体位置決定装置。

【0079】

（付記８）
前記移動体に取り付けられた複数の前記撮像装置から取得される画像に従って、前記移動体の座標位置を決定する、
処理を実行する付記１乃至７のいずれかに記載の移動体位置決定装置。

【0080】

（付記９）
コンピュータに、付記１乃至８のいずれかに記載の移動体位置決定装置の処理を実行させる方法。

【0081】

（付記１０）
コンピュータに、付記１乃至８のいずれかに記載の移動体位置決定装置の処理を実行させるプログラム。

【符号の説明】

【0082】

１…可視光通信システム、１００…フォークリフト、１０１ａ、１０１ｂ…カメラ、１０２、１０２ａ、１０２ｂ、１０２ｃ…マーカ、２００…サーバ、２０２…制御部、２０３…無線通信部、２０４…画像処理部、２０５…メモリ、２０６…操作部、２０７…表示部、２０８…通信部、２３０…画像取得部、２３２…マーカ特定部、２３４…画像位置補正部、２３６…３次元位置取得部、５００…空間、６０１ａ、６０１ｂ…画像、６０３ａ、６０３ｂ、６０３ｃ、６０３ｄ…無効領域、６１０ａ、６１０ｂ…マーカの画像位置、７００…画像領域、７１０…カメラ位置候補線、Ｃ１０…中心、Ｃ２０…カメラの３次元位置、Ｐ１…前々回位置、Ｐ２…前回位置、Ｖ１、Ｖ２、Ｖ３…ＸＹ平面ベクトル、Ｖ１００…３次元方向ベクトル、Ｖ１１０…移動ベクトル、Ｖ１２０…延長ベクトル

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】