特許 7472946 位置取得装置、位置取得方法及びプログラム

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

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

(11)【特許番号】

(24)【登録日】2024-04-15

(45)【発行日】2024-04-23

(54)【発明の名称】位置取得装置、位置取得方法及びプログラム

(51)【国際特許分類】

   G06T 7/70 20170101AFI20240416BHJP

   G06T 7/579 20170101ALI20240416BHJP

【ＦＩ】

G06T7/70 Z

G06T7/579

【請求項の数】 13

(21)【出願番号】P 2022151682

(22)【出願日】2022-09-22

(65)【公開番号】P2024046355

(43)【公開日】2024-04-03

【審査請求日】2023-06-12

(73)【特許権者】

【識別番号】000001443

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

(74)【代理人】

【識別番号】110001254

【氏名又は名称】弁理士法人光陽国際特許事務所

(72)【発明者】

【氏名】飯塚 宣男

【審査官】▲広▼島 明芳

(56)【参考文献】

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

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

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

Ｇ０６Ｔ ７／００ － ７／９０

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

【請求項1】

自装置を移動させる移動手段と、
前記移動手段による前記自装置の移動速度が所定速度以下である場合に第１の尤度で前記自装置の姿勢角を導出可能な第１導出手段と、
第２撮像手段により取得された画像に所定の数以上の光源が含まれる場合に、前記自装置の３次元位置及び姿勢角を導出可能な第２導出手段であって、前記第１の尤度よりも低い第２の尤度で前記自装置の姿勢角を導出可能な第２導出手段と、
前記第１導出手段による導出結果と、前記第２導出手段による導出結果とに基づいて推定された前記自装置の姿勢角及び３次元位置の尤度を判定する判定手段と、
前記判定された前記自装置の姿勢角及び３次元位置の尤度に応じて、前記移動手段による前記自装置の移動速度を制御する速度制御手段と、
を備える位置取得装置。

【請求項2】

前記移動速度が前記所定速度以下になった場合に、前記第１導出手段による前記自装置の姿勢角の導出の再実行が可能となる請求項１に記載の位置取得装置。

【請求項3】

尤度と最大速度とを対応付けて記憶する記憶手段を備え、
前記速度制御手段は、前記記憶手段から前記自装置の姿勢角及び３次元位置の尤度と対応付けられた最大速度を取得し、前記自装置の移動速度が前記取得された最大速度以下となるように制御する請求項１又は２に記載の位置取得装置。

【請求項4】

前記第２導出手段は、前記移動手段による前記自装置の移動速度が前記所定速度よりも速い場合にも、前記第２の尤度で前記自装置の姿勢角を導出可能である請求項１又は２に記載の位置取得装置。

【請求項5】

前記第２導出手段は、前記自装置の前記姿勢角が既知である場合に、前記第２撮像手段により取得された画像における２以上の光源の数及び位置に基づいて、前記自装置の３次元位置を導出可能である請求項１又は２に記載の位置取得装置。

【請求項6】

前記第２の尤度は、前記第２撮像手段により取得された画像における光源の数が多いほど高くなる請求項１又は２に記載の位置取得装置。

【請求項7】

前記第１導出手段は、第１撮像手段により取得された画像における複数の光源の数及び位置に基づいて、前記自装置の姿勢角を導出可能である請求項１又は２に記載の位置取得装置。

【請求項8】

前記第１導出手段は、前記第１撮像手段により取得された画像に含まれる光源の位置関係と、予め用意されている光源配置データとのパターンマッチングにより、前記自装置の３軸姿勢角を導出する請求項７に記載の位置取得装置。

【請求項9】

前記第１撮像手段により撮像される複数の光源は、恒星である請求項８に記載の位置取得装置。

【請求項10】

前記第２撮像手段により撮像される複数の光源のそれぞれは、光を変調して当該光源の識別情報を発信するものであり、
前記第２導出手段は、前記第２撮像手段により取得された画像から当該画像に含まれる光源の識別情報及び座標位置を検出し、当該検出された光源の識別情報に対応付けられた世界座標系における既知の３次元位置を取得し、前記第２撮像手段により取得された画像内の座標位置及び前記世界座標系における３次元位置に基づいて、前記自装置の３次元位置及び姿勢角を導出する請求項１又は２に記載の位置取得装置。

【請求項11】

前記第１撮像手段により撮像される複数の光源と、前記第２撮像手段により撮像される複数の光源は、異なる請求項７に記載の位置取得装置。

【請求項12】

自装置を移動させる移動手段を備える位置取得装置における位置取得方法であって、
前記移動手段による前記自装置の移動速度が所定速度以下である場合に第１の尤度で前記自装置の姿勢角を導出可能な第１導出工程と、
第２撮像手段により取得された画像に所定の数以上の光源が含まれる場合に、前記自装置の３次元位置及び姿勢角を導出可能な第２導出工程であって、前記第１の尤度よりも低い第２の尤度で前記自装置の姿勢角を導出可能な第２導出工程と、
前記第１導出工程における導出結果と、前記第２導出工程における導出結果とに基づいて推定された前記自装置の姿勢角及び３次元位置の尤度を判定する判定工程と、
前記判定された前記自装置の姿勢角及び３次元位置の尤度に応じて、前記移動手段による前記自装置の移動速度を制御する速度制御工程と、
を含む位置取得方法。

【請求項13】

自装置を移動させる移動手段を備える位置取得装置のコンピュータを、
前記移動手段による前記自装置の移動速度が所定速度以下である場合に第１の尤度で前記自装置の姿勢角を導出可能な第１導出手段、
第２撮像手段により取得された画像に所定の数以上の光源が含まれる場合に、前記自装置の３次元位置及び姿勢角を導出可能な第２導出手段であって、前記第１の尤度よりも低い第２の尤度で前記自装置の姿勢角を導出可能な第２導出手段、
前記第１導出手段による導出結果と、前記第２導出手段による導出結果とに基づいて推定された前記自装置の姿勢角及び３次元位置の尤度を判定する判定手段、
前記判定された前記自装置の姿勢角及び３次元位置の尤度に応じて、前記移動手段による前記自装置の移動速度を制御する速度制御手段、
として機能させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置取得装置、位置取得方法及びプログラムに関する。

【背景技術】

【0002】

従来から、自己位置を推定するための様々な測位システムが利用されている。例えば、可視光通信（ＲＧＢの３色パターン等）により、自己を一意に特定可能なＩＤを発信する複数の発光装置（位置指標）を、移動体に搭載されたカメラで撮像することで、移動体の位置を推定する技術が提案されている（特許文献１参照）。具体的には、各発光装置の設置位置（３次元位置）を予め測定しておき、カメラで撮像された画像から、画像内における発光装置の座標位置及びＩＤを検出し、検出された撮像画像上の座標位置と、各発光装置の３次元位置とに基づいて、移動体の位置を決定する。

【0003】

前述の可視光通信を使った測位では、例えば、自己測位の対象として移動可能な空間を平面に限定し（平面拘束）、高さ、カメラ姿勢角の２自由度（ピッチ、ロール）を固定とすることで、カメラにより得られた画像から位置指標（光源）を２点（時間的に連続して位置指標を検出可能な状態では１点）検出できれば、３自由度（水平位置、水平方位）の測位が可能となっている。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２０２２－０５０９２９号公報

【発明の概要】

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

【0005】

しかしながら、月面等では、一定平面内で移動が行われるとは考えられず、いわゆるオフロードでの活動となるため、自己位置を推定する際には、高さの変動、姿勢角の変動に対応した６自由度の測位が必須である。つまり、前述の可視光通信を使った測位方法では、常時、カメラに撮像された画像内に６点以上の位置指標が含まれている必要がある。

【0006】

一方、月面上等での広域エリアを対象とした測位に可視光通信を利用する場合、位置指標の設置負荷を最小限に抑えつつ、できるだけ高精度で測位を行いたいという要求がある。
このような要求のもと、ｋｍオーダーの広域エリアにおいて、カメラから常に６点の位置指標が見えるように、位置指標を配置するのは困難であった。

【0007】

また、測位システムを移動体に適用する場合、実際は、測位について推定される誤差、すなわち、測位尤度は、時々刻々と変わる。測位結果を信頼できないまま、移動体が走行を継続しても、誤差を増加させてしまうおそれがあった。

【0008】

本発明は、上記の従来技術における問題に鑑みてなされたものであって、自己位置の測位において、測位尤度の低下を防ぎ、測位精度を向上させることを課題とする。

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

【0009】

上記課題を解決するために、本発明の位置取得装置は、
自装置を移動させる移動手段と、
前記移動手段による前記自装置の移動速度が所定速度以下である場合に第１の尤度で前記自装置の姿勢角を導出可能な第１導出手段と、
第２撮像手段により取得された画像に所定の数以上の光源が含まれる場合に、前記自装置の３次元位置及び姿勢角を導出可能な第２導出手段であって、前記第１の尤度よりも低い第２の尤度で前記自装置の姿勢角を導出可能な第２導出手段と、
前記第１導出手段による導出結果と、前記第２導出手段による導出結果とに基づいて推定された前記自装置の姿勢角及び３次元位置の尤度を判定する判定手段と、
前記判定された前記自装置の姿勢角及び３次元位置の尤度に応じて、前記移動手段による前記自装置の移動速度を制御する速度制御手段と、
を備える。

【発明の効果】

【0010】

本発明によれば、自己位置の測位において、測位尤度の低下を防ぎ、測位精度を向上させることができる。

【図面の簡単な説明】

【0011】

【図1】本発明の実施形態における測位システムの構成を示す図である。

【図2】位置指標の配置例を示す測位エリアの上面図である。

【図3】移動体の構成図である。

【図4】移動体の機能ブロック図である。

【図5】測位処理の概略を説明するための図である。

【図6】６点以上の光源ＩＤ受信に基づく姿勢角の尤度の例である。

【図7】６軸センサにより得られた姿勢角の尤度の例である。

【図8】３軸姿勢角・尤度決定処理を示すフローチャートである。

【図9】全体処理を示すフローチャートである。

【図10】移動体の走行状況の変化で、測位尤度と移動速度がどのように変動するかを示した動作例である。

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

【0012】

以下、図面を参照して、本発明に係る位置取得装置の実施形態について説明する。なお、本発明は、図示例に限定されるものではない。

【0013】

〔測位システムの構成〕
図１は、本実施形態における測位システム１の構成を示す図である。測位システム１は、移動体１００、複数の位置指標３Ａ，３Ｂ，３Ｃ，３Ｄ，・・・（以下、それぞれを区別しない場合には、「位置指標３」という。）、複数の恒星４Ａ，４Ｂ，４Ｃ，４Ｄ，４Ｅ，４Ｆ，・・・（以下、それぞれを区別しない場合には、「恒星４」という。）から構成される。測位システム１は、最適な適用場所の１つとして、月面での利用を想定している。

【0014】

本実施形態では、本発明に係る位置取得装置が移動体１００に装備されている。ここでは、移動体１００として、月面探査車を想定しているが、自律走行ローバー、ロボット、人が操縦するローバー（自動運転モードを含む。）等であってもよい。
移動体１００は、起伏や凹凸に富む月面を走行するため、３軸姿勢角と３次元座標の６自由度を測位する。

【0015】

移動体１００には、可視光通信の受信に用いる可視光通信用カメラ３０Ａ，３０Ｂ（第２撮像手段）、恒星４の撮像状態により姿勢角を決定するためのスタートラッカ用カメラ４０（第１撮像手段）が装備されている。スタートラッカ用カメラ４０により撮像されて後述する姿勢角の導出に利用される複数の光源である恒星４と、可視光通信用カメラ３０Ａ，３０Ｂにより撮像されて後述する姿勢角や３次元位置の導出に利用される複数の光源である位置指標３は、異なる。

【0016】

本実施形態では、移動体１００は、移動体１００が移動する前方を向いた可視光通信用カメラ３０Ａと、後方を向いた可視光通信用カメラ３０Ｂの２台の前後カメラを用いて、可視光通信に係る情報を取得する。２台の前後カメラを用いる理由は、可視光通信の検知範囲を確保するためと、レンズ等の光学歪みの補正エラーを防ぐためである。なお、歪み補正が良好にできるのであれば、大広角レンズの単一カメラを用いてもよい。以下、可視光通信用カメラ３０Ａ，３０Ｂのそれぞれを区別しない場合には、「可視光通信用カメラ３０」という。

【0017】

移動体１００の動作エリアである月面には、複数の光源としての位置指標３が間隔をあけて設置されている。
位置指標３は、ＬＥＤ（Light Emitting Diode）を含む。位置指標３は、可視光の波長領域における色変調又は輝度変調により、発光又は反射を制御し、送信対象とする情報を送信する。例えば、位置指標３は、Ｒ（赤）、Ｇ（緑）、Ｂ（青）の３色のパターンにより、少なくとも、位置指標３に固有の識別情報（自己を一意に特定可能な光源ＩＤ）を発信する。複数の位置指標３のそれぞれは、設置時に、世界座標系（Ｘ，Ｙ，Ｚ）における設置位置（３次元座標）が測量されている。各位置指標３について、光源ＩＤと３次元座標との対応関係は、移動体１００内の光源ＩＤ－３次元座標テーブル２１（図３、図４参照）に保存されている。
なお、位置指標３が十分な情報量のデータを送信可能な場合には、自己の月面上の位置と高さ（３次元座標）を直接送信する形態でもよい。

【0018】

位置指標３は、月面の起伏を超えて、長距離離れた場所から見えるように、十分な高さに配置されるか、又は、隆起した領域（高度が高いところ）に配置される。
位置指標３の電源としては、太陽電池等を利用する。月の夜の期間（太陽と反対側に面する期間）にも位置指標３を稼働させる場合には、位置指標３は、その期間に対応した大容量の充電機構を有するものとする。

【0019】

位置指標３の発光部の大きさは、必要とされる通信距離、カメラ解像度、レンズ等から決定される。例えば、可視光通信用カメラ３０として、横画角１００度のレンズの４Ｋ解像度のカメラを用いる場合、１ピクセルはおよそ０．０２５度である。なお、可視光通信用カメラ３０としては、例えばフレームレートが３０ｆｐｓや６０ｆｐｓのものを用いることができる。
可視光通信では、１ピクセルの結像において、安定した色変調又は輝度変調を撮像できればよいが、実際には、光学系、イメージセンサの構造による空間周波数の限界がある。そのため、３ピクセル四方以上で安定した結像となると仮定すると、例えば、光源（位置指標３）を１ｍ四方のサイズとした場合、この光源が０．０７５度の見掛け角となる距離は、１／ｔａｎ（０．０７５度）＝約７６３ｍであり、これが、通信限界距離となる。
ただし、実際には、月面は空気散乱がなく、昼でも周囲は暗いため、ハレーション効果により、背景が暗い中で光源のみがぼやけて大きく見える。これにより、通信限界距離は理論値の２倍から３倍に延びる。

【0020】

図２は、位置指標３（可視光通信の送信装置）の配置例を示す測位エリアの上面図である。可視光通信を利用した測位処理の精度は、位置指標３の設置密度に依存するため、高精度エリア５から一般精度エリア６まで、必要な精度に応じて位置指標３を配置する。例えば、高精度測位を必要とする基地７の周辺や、高速度移動を可能としたい場所には、位置指標３を高密度で設置して、高精度エリア５を構成する。一方、一般精度エリア６には、測位条件を満たすように、最低限の密度以上の密度で位置指標３を配置すればよい。

【0021】

一般精度エリア６における位置指標３の最低限の密度での配置方法の一例としては、位置指標３の任意の２点間が、可視光通信の通信限界距離Ｄ未満となるように、位置指標３を配置していく。通信限界距離Ｄを７６３ｍとすると、例えば、位置指標３の配置間隔Ｌを７００ｍ未満として、１辺がＬの正三角形を敷き詰めた状態の各頂点に、位置指標３を配置すればよい。

【0022】

図２に示すように、移動体１００の前後方向に可視光通信用カメラ３０Ａ，３０Ｂから通信限界距離Ｄ（７６３ｍ）を半径として、横画角１００度で形成される扇型のエリア８が、可視光通信の受信範囲となる。位置指標３を正三角形の各頂点に配置した場合、一般精度エリア６内の任意の位置で可視光通信用カメラ３０Ａ，３０Ｂがどの方位を向いていても、前後の２台のカメラを合わせて、２点以上の光源ＩＤを受信することが可能である。ほとんどの場合は、３点以上の光源ＩＤを受信することになる。

【0023】

なお、位置指標３は、正確な正三角形を構成するように配置される必要はない。また、位置指標３の設置位置や方向に制限がある場合には、より最適化して位置指標３を配置すればよい。また、地形上問題があるエリアには、地形に合わせて個別に位置指標３を配置すればよい。

【0024】

〔移動体の構成〕
図３は、移動体１００の構成図である。図３に示すように、移動体１００は、制御部１０、記憶部２０、可視光通信用カメラ３０（３０Ａ，３０Ｂ）、スタートラッカ用カメラ４０、６軸センサ５０、走行駆動部６０等を備える。

【0025】

制御部１０は、ＣＰＵ（Central Processing Unit）によって構成される。制御部１０は、記憶部２０に記憶されたプログラムに従って、移動体１００の各部を制御する。
制御部１０は、可視光通信処理部１１、測位処理部１２、スタートラッカ姿勢角算出部１３、３軸姿勢角・尤度決定処理部１４、測位尤度評価部１５、移動速度制御部１６を備える。制御部１０を構成する各部は、ＣＰＵと記憶部２０に記憶されているプログラムとの協働によってソフトウェア処理で実現される。制御部１０を構成する各部の詳細については、後述する。

【0026】

記憶部２０は、制御部１０により実行されるプログラムや各種設定データ等を記憶する。プログラムは、コンピュータ読み取り可能なプログラムコードの形態で記憶部２０に格納されている。記憶部２０としては、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）等が用いられる。

【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】

尤度Ｂは、例えば、下記式（１）で示される。

【数1】

【0058】

測位処理部１２（第２導出手段）は、走行駆動部６０による自装置（移動体１００）の移動速度が所定速度よりも速い場合にも、尤度Ｂ（第２の尤度）で自装置の姿勢角を導出可能である。

【0059】

姿勢角の算出に６軸センサ５０のみを利用する場合、時間の経過による角度変動の累積が大きくなるにつれて、姿勢角の尤度Ｃは低下する。尤度Ｃは、６軸センサ５０のキャリブレーション後、６軸センサ５０の使用に伴い、最初は穏やかに低下していくが、途中からある限界に向けて急激に低下する。例えば、移動体１００において、移動方向の変更（転回）が続くと、６軸センサ５０により得られる姿勢角の尤度Ｃが低下していく。
また、６軸センサ５０の挙動としては、応答周波数等の関係で、強烈な変動では、誤差が出やすいが、ゆっくりとした移動なら、より尤度の高い角度推定ができる。

【0060】

図７に、６軸センサ５０により得られた姿勢角の尤度Ｃの例を示す。図７では、横軸に、６軸センサ５０により得られる角度変位量の積算値Ｅを取り、縦軸に、尤度Ｃを取っている。なお、Ｅが閾値Ｅ_０より大きいところでは、尤度Ｃは０とする。
角度変位量は、例えば、変化の前後において、姿勢角を示す３次元ベクトルの始点同士を揃えた場合に、ベクトルの終点同士の距離の絶対値で表される。

【0061】

尤度Ｃは、例えば、下記式（２）で示される。

【数2】

【0062】

ただし、式（２）に数学的な厳密さはない。実際には、誤差の統計的特性、システム全体の総合モデル設計において、最適なものが選択される。

【0063】

測位処理部１２は、スタートラッカ姿勢角算出部１３（第１導出手段）による導出結果（姿勢角）と、測位処理部１２（第２導出手段）による導出結果（３次元位置・姿勢角）とを統合して、自装置の姿勢角及び３次元位置を推定する。なお、本実施形態では、姿勢角の導出結果として、３軸姿勢角・尤度決定処理部１４により統合された結果を用いる。

【0064】

具体的には、測位処理部１２は、導出される姿勢角の尤度の高いスタートラッカ姿勢角算出部１３による導出結果に対して、測位処理部１２による導出結果よりも重み付けを大きくして、スタートラッカ姿勢角算出部１３による導出結果と、測位処理部１２による導出結果とを統合する。

【0065】

測位尤度評価部１５は、最終的な測位の尤度を評価する。具体的には、測位尤度評価部１５は、測位処理部１２により推定された自装置の姿勢角及び３次元位置の尤度を判定する。すなわち、測位尤度評価部１５は、判定手段として機能する。例えば、測位尤度評価部１５は、再投影誤差と姿勢角尤度とに対して重み付け平均を行い、測位全体としての尤度を算出する。

【0066】

本実施形態では、再投影誤差と姿勢角尤度とを組み合わせて、測位の評価値としている。再投影誤差によって誤差尤度を推定することはできるが、再投影の性質上、真値と異なる３軸姿勢角の組み合わせで、再投影結果が良好になってしまう（異なる場所と姿勢角でも、位置指標３に対応する発光点が同じ見掛けになる）場合があるため、再投影誤差だけを、測位の自己評価値とすることはできない。
特に、恒星４と位置指標３を利用した測位システム１では、姿勢角尤度は非常に重要である。

【0067】

移動速度制御部１６は、測位尤度評価部１５により判定された自装置（移動体１００）の姿勢角及び３次元位置の尤度（測位全体の尤度）に応じて、走行駆動部６０（移動手段）による自装置の移動速度を制御する。すなわち、移動速度制御部１６は、速度制御手段として機能する。具体的には、移動速度制御部１６は、記憶部２０の尤度－速度制御テーブル２３を参照して、測位尤度（姿勢角及び３次元位置の尤度）と対応付けられた最大速度（速度、転回の角速度）を取得し、自装置の移動速度が、各時点での測位尤度に応じた最大速度以下となるように、走行駆動部６０を制御し、移動体１００の速度を調整する。

【0068】

なお、再投影誤差が明らかに異常な値となった場合には、移動体１００の移動を一旦停止させて、全ての測位をやり直す必要がある。そのため、姿勢角尤度だけでなく、再投影誤差も評価することが望ましい。
なお、測位尤度のうち、再投影誤差がさほど悪化してないとしても、姿勢角尤度が下がった場合には、速度、又は、方向転回を抑制するようにする。

【0069】

〔移動体の動作〕
次に、移動体１００の動作について説明する。
図８は、移動体１００において実行される３軸姿勢角・尤度決定処理を示すフローチャートである。

【0070】

まず、スタートラッカ姿勢角算出部１３は、スタートラッカによる３軸姿勢角の算出を試みる（ステップＳ１）。スタートラッカ姿勢角算出部１３は、スタートラッカ用カメラ４０により撮像された画像データから恒星４に対応する点源を抽出し、抽出された点源を星図データ２２と照合し、パターンマッチングを行うことで、各点源がいずれの恒星４であるかを特定し、３軸姿勢角を算出する。

【0071】

次に、６軸センサ５０は、３軸加速度センサ及び３軸角速度センサのセンサ値と前状態から、３軸姿勢角を算出する（ステップＳ２）。ここでは、カルマンフィルタ、相補フィルタ等による逐次ドリフト補正も含まれる。

【0072】

また、３軸姿勢角・尤度決定処理部１４は、６軸センサ５０により得られた３軸姿勢角の尤度Ｃを求める（ステップＳ３）。図７に示すように、尤度Ｃは、角度変位量の積算値Ｅが大きくなるにつれて、小さくなっていく。３軸姿勢角・尤度決定処理部１４は、キャリブレーション後の６軸センサ５０に対する角度変位量の積算値Ｅに応じて、３軸姿勢角の尤度Ｃを決定する。

【0073】

次に、３軸姿勢角・尤度決定処理部１４は、スタートラッカによる測定が成立したか否かを判断する（ステップＳ４）。具体的には、３軸姿勢角・尤度決定処理部１４は、スタートラッカ姿勢角算出部１３から測定結果を取得できたか否かを判断する。
スタートラッカによる測定が成立した場合には（ステップＳ４；ＹＥＳ）、３軸姿勢角・尤度決定処理部１４は、スタートラッカにより得られた３軸姿勢角の尤度Ａを「１．０」とする（ステップＳ５）。

【0074】

ステップＳ４において、スタートラッカによる測定が成立しなかった場合には（ステップＳ４；ＮＯ）、３軸姿勢角・尤度決定処理部１４は、測位処理部１２において６自由度ＰｎＰ測位が成立したか否か、すなわち、可視光通信処理部１１により６点以上の光源ＩＤが検出されたか否かを判断する（ステップＳ６）。

【0075】

６自由度ＰｎＰ測位が成立した場合には（ステップＳ６；ＹＥＳ）、３軸姿勢角・尤度決定処理部１４は、６点以上の光源ＩＤ受信に基づく３軸姿勢角を、測位処理部１２から取得する（ステップＳ７）。

【0076】

また、３軸姿勢角・尤度決定処理部１４は、６点以上の光源ＩＤ受信に基づく３軸姿勢角の尤度Ｂを求める（ステップＳ８）。図６に示すように、尤度Ｂは、光源ＩＤの受信数Ｎが多いほど、大きい値となる（１に近付く）。３軸姿勢角・尤度決定処理部１４は、光源ＩＤの受信数Ｎに応じて、３軸姿勢角の尤度Ｂを決定する。

【0077】

ステップＳ５又はステップＳ８の後、３軸姿勢角・尤度決定処理部１４（制御部１０）は、６軸センサ５０のキャリブレーションを行う（ステップＳ９）。具体的には、３軸姿勢角・尤度決定処理部１４は、スタートラッカにより得られた３軸姿勢角があれば、スタートラッカにより得られた３軸姿勢角を利用して、６軸センサ５０のキャリブレーションを行う。一方、３軸姿勢角・尤度決定処理部１４は、スタートラッカにより得られた３軸姿勢角がなければ、６点以上の光源ＩＤ受信に基づく３軸姿勢角を利用して、６軸センサ５０のキャリブレーションを行う。

【0078】

ステップＳ９の後、又は、ステップＳ６において、６自由度ＰｎＰ測位が成立しなかった場合には（ステップＳ６；ＮＯ）、３軸姿勢角・尤度決定処理部１４は、３軸姿勢角を統合し、姿勢角の尤度を統合する（ステップＳ１０）。

【0079】

例えば、スタートラッカにより得られた３軸姿勢角がある場合には、３軸姿勢角・尤度決定処理部１４は、スタートラッカにより得られた３軸姿勢角をそのまま利用すればよい。

【0080】

なお、図８では、スタートラッカによる測定が成立した場合には（ステップＳ４；ＹＥＳ）、６点以上の光源ＩＤ受信に基づく３軸姿勢角を取得する処理（ステップＳ７）を通らないようになっているが、スタートラッカによる測定が成立したか否かにかかわらず、可視光通信用カメラ３０から得られた情報のみに基づいてＰｎＰ測位が成立した結果がある場合には、６点以上の光源ＩＤ受信に基づく３軸姿勢角を取得し、利用することとしてもよい。

【0081】

そして、３軸姿勢角・尤度決定処理部１４は、スタートラッカにより得られた３軸姿勢角、６点以上の光源ＩＤ受信に基づく３軸姿勢角、６軸センサ５０により得られた３軸姿勢角を、尤度に応じて重み付け平均して統合し、現在の３軸姿勢角を算出する。なお、３軸姿勢角・尤度決定処理部１４は、取得できた３軸姿勢角を適宜組み合わせて、統合すればよい。

【0082】

また、３軸姿勢角・尤度決定処理部１４は、３種類の３軸姿勢角のうち、最も尤度が高いもの１つを選択してもよい。
また、３軸姿勢角・尤度決定処理部１４は、各３軸姿勢角の尤度にかかわらず、前状態からの変動量が大きい場合や、姿勢角の制約範囲外や外れ値とみなせる場合には、算出された姿勢角を棄却してもよい。
また、３軸姿勢角・尤度決定処理部１４は、スタートラッカにより得られた３軸姿勢角、６点以上の光源ＩＤ受信に基づく３軸姿勢角、６軸センサ５０により得られた３軸姿勢角の順に重み付けを大きくして、３軸姿勢角を統合してもよい。

【0083】

例えば、スタートラッカによる測定が成立し（ステップＳ４；ＹＥＳ）、かつ、６自由度ＰｎＰ測位が成立した場合には（ステップＳ６；ＹＥＳ）、スタートラッカにより得られた３軸姿勢角について、０．７を乗じ、６自由度ＰｎＰ測位により得られた３軸姿勢角について、０．２を乗じ、６軸センサ５０により得られた３軸姿勢角について、０．１を乗じて、それぞれの値を統合し、現在の３軸姿勢角を算出するようにしてもよい。
また、スタートラッカによる測定は成立していないが（ステップＳ４；ＮＯ）、６自由度ＰｎＰ測位が成立した場合には（ステップＳ６；ＹＥＳ）、６自由度ＰｎＰ測位により得られた３軸姿勢角について、０．８を乗じ、６軸センサ５０により得られた３軸姿勢角について、０．２を乗じて、それぞれの値を統合し、現在の３軸姿勢角を算出するようにしてもよい。
なお、スタートラッカによる測定が成立したものの（ステップＳ４；ＹＥＳ）、６自由度ＰｎＰ測位が成立していない場合において（ステップＳ６；ＮＯ）、スタートラッカにより得られた３軸姿勢角について、０．９を乗じ、６軸センサ５０により得られた３軸姿勢角について、０．１を乗じて、それぞれの値を統合し、現在の３軸姿勢角を算出するようにしてもよい。

【0084】

また、３軸姿勢角・尤度決定処理部１４は、３軸姿勢角の統合に用いた各姿勢角に対応する尤度に基づいて、統合された姿勢角の尤度を統合する。例えば、３軸姿勢角・尤度決定処理部１４は、統合された姿勢角の尤度の平均を取ったり、重み付けしたりして、姿勢角の尤度を求める。
以上で、３軸姿勢角・尤度決定処理が終了する。

【0085】

なお、これまでの説明では、上述した通り、スタートラッカ、６軸センサ５０、ＰｎＰ測位により求められる３軸姿勢角が、それぞれ、車体（移動体１００）の３軸姿勢角と一致するものについて説明したが、スタートラッカ、６軸センサ５０、ＰｎＰ測位により求められる３軸姿勢角が、それぞれ、車体（移動体１００）の３軸姿勢角と一致していない場合には、以下の処理を行うようにしてもよい。すなわち、ステップＳ１０の後に、３軸姿勢角・尤度決定処理部１４により、統合された３軸姿勢角について、設置時の統合された３軸姿勢角からの姿勢角変動を求めるようにしてもよい。ここで、設置時の統合された３軸姿勢角は、記憶部２０に記憶されている。

【0086】

そして、３軸姿勢角・尤度決定処理部１４は、姿勢角変動を、可視光通信用カメラ３０（移動体１００）の設置時の３軸姿勢角に加え、現在の可視光通信用カメラ３０の３軸姿勢角を算出するようにしてもよい。なお、可視光通信用カメラ３０の設置時の３軸姿勢角は、記憶部２０に記憶されている。
また、このようにして算出される可視光通信用カメラ３０の３軸姿勢角の尤度は、ステップＳ１０で統合された３軸姿勢角の尤度と同一である。

【0087】

図９は、移動体１００において実行される全体処理を示すフローチャートである。全体処理では、姿勢角関連以外の処理を中心に説明する。

【0088】

まず、可視光通信処理部１１は、可視光通信用カメラ３０により撮像して得られた撮像画像の画像データを取得し、可視光通信受信処理を行う（ステップＳ１１）。可視光通信処理部１１は、撮像画像から位置指標３に対応する発光点を検出し、撮像画像上の各発光点から、可視光通信により示される光源ＩＤと座標位置の組を取得する。

【0089】

次に、測位処理部１２は、可視光通信により２点以上の光源ＩＤが受信されたか否かを判断する（ステップＳ１２）。
可視光通信により受信された光源ＩＤの数が２未満である場合には（ステップＳ１２；ＮＯ）、処理はステップＳ１１に戻る。

【0090】

ステップＳ１２において、可視光通信により２点以上の光源ＩＤが受信された場合には（ステップＳ１２；ＹＥＳ）、測位処理部１２は、可視光通信により６点以上の光源ＩＤが受信されたか否かを判断する（ステップＳ１３）。

【0091】

可視光通信により６点以上の光源ＩＤが受信された場合には（ステップＳ１３；ＹＥＳ）、測位処理部１２は、各光源ＩＤに基づいて、ＰｎＰ（ｎ≧６）計算により６自由度を求める（ステップＳ１４）。具体的には、測位処理部１２は、検出された光源ＩＤのそれぞれについて、光源ＩＤ－３次元座標テーブル２１から、光源ＩＤに対応する３次元座標を取得し、撮像画像上の座標位置と、世界座標系における３次元座標と、に基づいて、自装置（移動体１００）の３軸姿勢角及び３次元位置を算出する。

【0092】

次に、測位処理部１２は、６点以上の光源ＩＤ受信に基づく３軸姿勢角を３軸姿勢角・尤度決定処理部１４に提供する（ステップＳ１５）。この「６点以上の光源ＩＤ受信に基づく３軸姿勢角」が、３軸姿勢角・尤度決定処理（図８参照）のステップＳ７において取得された３軸姿勢角である。

【0093】

ステップＳ１３において、可視光通信により受信された光源ＩＤの数が６未満である場合（ステップＳ１３；ＮＯ）、又は、ステップＳ１５の後、測位処理部１２は、３軸姿勢角・尤度決定処理部１４から、統合された３軸姿勢角と姿勢角尤度を取得する（ステップＳ１６）。ここで取得される３軸姿勢角及び姿勢角尤度は、３軸姿勢角・尤度決定処理のステップＳ１０において統合された値である。

【0094】

次に、測位処理部１２は、可視光通信により検出された２点以上の位置指標３の光源ＩＤのそれぞれについて、光源ＩＤ－３次元座標テーブル２１から、光源ＩＤに対応する３次元座標を取得し、光源ＩＤごとの撮像画像上の座標位置と世界座標系における３次元座標との組と、ステップＳ１６で取得した統合された３軸姿勢角に基づいて、測位計算（３軸姿勢角及び３次元位置の算出）を行う（ステップＳ１７）。

【0095】

なお、可視光通信用カメラ３０から得られた情報（６点以上の光源ＩＤ）のみに基づいてＰｎＰ測位が成立した測位結果が存在する場合、この測位結果（ステップＳ１４の測位結果）と、３軸姿勢角・尤度決定処理部１４から取得した統合された３軸姿勢角及び２点以上の光源ＩＤに基づく測位結果（ステップＳ１７の測位結果）と、を統合することとしてもよい。

【0096】

次に、測位処理部１２は、ステップＳ１７の測位結果から再投影誤差を求める（ステップＳ１８）。再投影誤差は、座標の尤度を示す値である。

【0097】

次に、測位尤度評価部１５は、ステップＳ１８で求めた再投影誤差と、ステップＳ１６で取得した姿勢角尤度から、測位尤度を求める（ステップＳ１９）。ここで、測位尤度評価部１５は、再投影誤差と姿勢角尤度とを統合して、測位全体の尤度を求める。

【0098】

次に、移動速度制御部１６は、尤度－速度制御テーブル２３を参照して、測位尤度に対応する最大速度（速度、転回の角速度）を取得し、現状の許容最大速度を設定する（ステップＳ２０）。

【0099】

次に、移動速度制御部１６は、移動体１００の速度が許容最大速度をオーバーしているか否かを判断する（ステップＳ２１）。具体的には、移動速度制御部１６は、移動体１００の進行方向における速度が、設定された許容最大速度を超えているか否かを判断する。また、移動速度制御部１６は、移動体１００の転回における角速度が、設定された許容最大角速度を超えているか否かを判断する。
移動体１００の速度が許容最大速度をオーバーしていない場合には（ステップＳ２１；ＮＯ）、処理はステップＳ１１に戻る。

【0100】

ステップＳ２１において、移動体１００の速度が許容最大速度をオーバーしている場合には（ステップＳ２１；ＹＥＳ）、移動速度制御部１６は、許容最大速度まで移動体１００の速度（速度、転回の角速度）を抑制する（ステップＳ２２）。すなわち、移動速度制御部１６は、最大速度以下となるように、走行駆動部６０を制御し、移動体１００の速度を調整する。
ステップＳ２２の後、処理はステップＳ１１に戻る。

【0101】

なお、３軸姿勢角・尤度決定処理（図８参照）において、スタートラッカによる測定が成立するための所定の条件が満たされておらず（ステップＳ４；ＮＯ）、かつ、可視光通信用カメラ３０により取得された画像に６点以上の光源（位置指標３）が含まれ、６自由度ＰｎＰ測位が成立した場合には（ステップＳ６；ＹＥＳ）、測位処理部１２による自装置の姿勢角及び３次元位置の推定を、６自由度ＰｎＰ測位により導出された自装置の姿勢角及び３次元位置に基づいて行ってもよい。すなわち、全体処理（図９参照）のステップＳ１４においてＰｎＰ計算により求められた６自由度を、最終的に推定される自装置の姿勢角及び３次元位置としてもよい。
スタートラッカによる導出結果を利用できない場合には、可視光通信による導出結果のみを用いて、測位処理を行うことができる。

【0102】

図１０は、移動体１００の走行状況の変化で、測位尤度と移動速度がどのように変動するかを示した動作例である。
図１０では、横軸に時間ｔの経過、縦軸に測位尤度及び移動速度を取っている。図１０では、測位尤度を実線で示し、移動速度を一点鎖線で示している。また、縦軸に示した「１．０」の目盛りは、測位尤度に対するものである。測位尤度は、再投影誤差と姿勢角尤度とが統合されることで、算出された値である。

【0103】

まず、移動体１００が移動を開始した直後の時間帯Ｔ１では、主にスタートラッカにより３軸姿勢角が決定され、姿勢角尤度（尤度Ａ）は１．０となる。このため、統合された測位尤度も略１．０となり、移動体１００は加速可能となる。移動体１００の移動速度は、徐々に上がっていく。

【0104】

時間帯Ｔ２では、スタートラッカによる測定が困難となり、６軸センサ５０のみからの情報により３軸姿勢角が決定されるが、路面変動レベルが小さいことから、姿勢角尤度（尤度Ｃ）に急激な低下は見られない。このため、統合された測位尤度も緩やかに低下していく。「路面変動レベルが小さい」とは、直進が多く、路面の起伏が小さい状況である。測位尤度が比較的安定している状態では、移動体１００の移動速度も、ある程度以上の速度に保たれている。

【0105】

時間帯Ｔ３では、６軸センサ５０のみからの情報による３軸姿勢角の決定が継続しているが、路面変動レベルが大きくなってきたことから、姿勢角尤度（尤度Ｃ）が急激に低下していく。このため、統合された測位尤度も急激に低下していく。「路面変動レベルが大きい」とは、進行方向の変動が大きいか、路面の起伏が大きい状況である。測位尤度の低下に伴い、移動体１００の移動速度に制限がかかる。

【0106】

時間帯Ｔ４では、移動体１００に対する速度制限が連続することで、最終的には、スタートラッカによる測定ができる速度になり、測位尤度が急激に回復している。測位尤度の回復に伴い、移動体１００は加速可能となり、移動体１００の移動速度は上がっていく。

【0107】

時間帯Ｔ５では、６軸センサ５０のみからの情報により３軸姿勢角が決定され、路面変動レベルが大きいことから、姿勢角尤度（尤度Ｃ）が急激に低下していく。このため、統合された測位尤度も急激に低下していく。測位尤度の低下に伴い、移動体１００の移動速度に制限がかかる。

【0108】

時間帯Ｔ６では、路面変動レベルは大きいが、可視光通信用カメラ３０による撮像画像から光源ＩＤを６点以上検出可能な高精度エリアに移動体１００がしばしば進入することから、６点以上の光源ＩＤ受信に基づく測位が成立したタイミングで姿勢角尤度（主に尤度Ｂの影響による）が高くなる。姿勢角尤度の上昇に伴い、統合された測位尤度も高くなっていく。測位尤度の上昇に伴い、移動体１００の移動速度も上がっていく。
尤度の統合においては、尤度が上昇する際は、統合される値の最良値になるが、下降する際にはフィルタがかかるため、可視光通信における受信点数が急に減っても、尤度が瞬時に低下することはない。

【0109】

なお、上記の例では、移動体１００に対する速度制限が連続することで、最終的にスタートラッカによる測定ができる速度になり、測位尤度が急激に回復して移動体１００が加速可能となる例について説明したが、測位尤度に閾値を設定しておき、測位尤度が閾値を下回った時点で移動体１００の移動を停止させ、スタートラッカによる測定を可能とするようにしてもよい。

【0110】

以上説明したように、本実施形態によれば、自装置（移動体１００）の姿勢角及び３次元位置の尤度（測位尤度）を判定し、尤度に応じて、自装置の移動速度を制御するので、自己位置の測位において、測位尤度の低下を防ぎ、測位精度を向上させることができる。
例えば、移動体１００の速度や転回の角速度を抑えたり、移動体１００の動きを止めたりするだけで、測位尤度を上げることができる。

【0111】

また、スタートラッカにより導出される姿勢角の尤度（第１の尤度）は、測位処理部１２により導出される６点以上の光源ＩＤ受信に基づく姿勢角の尤度（第２の尤度）より高いので、スタートラッカにより導出された姿勢角を利用することで、測位精度が向上する。

【0112】

また、移動体１００の移動速度が所定速度以下になった場合に、スタートラッカ姿勢角算出部１３による移動体１００の姿勢角の導出の再実行が可能となるため、移動体１００の速度を制限することで、測位尤度を上げることができる。

【0113】

また、移動体１００では、移動速度制御部１６が、尤度－速度制御テーブル２３から測位尤度と対応付けられた最大速度を取得し、自装置の移動速度が、取得された最大速度以下となるように制御する。測位尤度に基づいて、速度や転回の角速度を制御することで、自律的に測位尤度を一定値以上に維持しながら、移動体１００を制御できる。

【0114】

また、測位処理部１２は、自装置の移動速度が所定速度よりも速い場合にも、可視光通信を使った測位により、尤度Ｂ（第２の尤度）で自装置の姿勢角を導出することができる。これにより、スタートラッカによる姿勢角の導出ができない場合にも、自装置の姿勢角を導出することができる。

【0115】

また、測位処理部１２は、自装置の姿勢角が既知である場合に、可視光通信用カメラ３０により取得された画像における２以上の光源（位置指標３）の数及び位置に基づいて、自装置の３次元位置を導出することができる。
具体的には、測位処理部１２は、可視光通信を使った測位において、姿勢角の３自由度についてはスタートラッカや６軸センサ５０から取得した値を用い、残りの３自由度として３次元位置を求める。測位処理部１２は、３軸姿勢角・尤度決定処理部１４により得られた姿勢角を利用して、可視光通信用カメラ３０により取得された画像における位置指標３の位置に基づいて、移動体１００の姿勢角及び３次元位置を推定する。これにより、高さの変動、姿勢角の変動があるような、オフロードに対応した測位に対して、撮像画像から検出される発光点（光源ＩＤ）として必要な数を抑えることができる。
撮像画像から２点以上の発光点（光源ＩＤ）を検出できれば、測位可能であるため、常に６点以上の位置指標３がカメラに捉えられていることが必要であった従来の測位方法と比較して、位置指標３の数や設置密度を抑えることができる。

【0116】

また、６点以上の光源ＩＤ受信に基づく姿勢角の尤度Ｂ（第２の尤度）は、可視光通信用カメラ３０により取得された画像における光源（位置指標３）の数が多いほど高くなるので、可視光通信に係る測位処理では、画像内から検出される光源ＩＤが多いほど、高精度の測位結果が得られる。

【0117】

また、スタートラッカ姿勢角算出部１３は、スタートラッカ用カメラ４０により取得された画像に含まれる恒星４の位置関係と、星図データ２２とのパターンマッチングにより、移動体１００の３軸姿勢角を導出するので、精度良く移動体１００の絶対姿勢角を得ることができる。
特に、月面では、太陽に面した側であっても、天空に恒星４が見えているため、スタートラッカによる姿勢角取得が可能である。

【0118】

また、可視光通信に係る測位処理では、可視光通信用カメラ３０により取得された画像内の位置指標３の座標位置、及び、当該位置指標３の世界座標系における３次元位置に基づいて、移動体１００の３次元位置及び姿勢角を導出することができる。

【0119】

なお、上記実施形態における記述は、本発明に係る位置取得装置の例であり、これに限定されるものではない。装置を構成する各部の細部構成及び細部動作に関しても、本発明の趣旨を逸脱することのない範囲で適宜変更可能である。

【0120】

〔変形例１〕
変形例１では、可視光通信用カメラ３０により撮像される光源を、光源ＩＤを発信可能なものに限定しない。
上記実施形態では、測位が必要な広域エリアにおいて、可視光通信用カメラ３０から２点以上見えるように位置指標３を配置すればよいこととしたが、位置指標３の必要数が少ないほど、また、その１基あたりの設置コストが低いほど、実用性が増す。

【0121】

そこで、可視光通信用カメラ３０の撮像対象となる光源の全てを、光源ＩＤを発信可能な位置指標３とする必要はなく、その一部を、単なる高輝度の補助光源としてもよい。ただし、補助光源の設置位置の３次元座標は、位置指標３と同様、既知である。また、可視光通信用カメラ３０により得られた撮像画像において、周囲の光源ＩＤを発信する位置指標３との位置関係から、補助光源についても個々に識別可能となっており、撮像画像内の補助光源の座標位置を取得できる。補助光源は、光源ＩＤを発信する機能が不要となるため、構造を簡略化でき、コストも抑えられる。

【0122】

また、月面空間は、真空で大気散乱がなく、背景が常に暗いため、昼の状態で利用されることを前提として、太陽光を利用した補助光源を用いてもよい。例えば、補助光源として、太陽光を適宜反射・拡散するものを、空が背景となるような高い位置に設置すれば、無電源の高輝度指標が得られる。

【0123】

無電源の補助光源の構造としては、透過率５０～７０％程度の半透過拡散板により、直方体、球、角筒、円筒等を形成する。補助光源に対して太陽が可視光通信用カメラ３０と同じ側にある場合には、主に半透過拡散板が太陽光を乱反射することで、可視光通信用カメラ３０から補助光源が高輝度に光って見える。一方、補助光源に対して太陽が可視光通信用カメラ３０と反対側にある場合には、主に半透過拡散板が太陽光を透過することで、可視光通信用カメラ３０から補助光源が高輝度に光って見える。

【0124】

このように、光源ＩＤを送信する位置指標３の間を補うように設置される補助光源として、無電源の位置指標を用いることで、広域エリア全体での光源自体のコスト、光源の設置コストの両方を大幅に低減できる。

【0125】

〔変形例２〕
変形例２では、姿勢角を取得するためにスタートラッカを利用しない。
上記実施形態では、移動体１００の絶対的な姿勢角を取得する手段としてスタートラッカを用いる場合について説明したが、本発明は、月面等の宇宙以外の状況でも利用可能である。地球上でも、衛星の位置情報を利用できない広域屋内・地下、地磁気が安定しないエリアにおいて、本発明は有効である。

【0126】

例えば、広域の地下空間において、可視光通信用カメラ３０とは別の撮像手段（第１撮像手段）により撮像される光源として、恒星４に代えて、公知の照明装置等を利用する。ただし、完全にスタートラッカと同様に扱うには、照明装置の１つ１つを特定できるように、照明装置の配置を非一様とする。
具体的には、第１撮像手段により取得された画像に含まれる複数の照明装置の位置関係と、予め用意されている光源配置データ（照明装置の配置が記録されたデータ）とのパターンマッチングにより、自装置の姿勢角を導出する。その他の処理については、上記実施形態における処理と同様である。

【0127】

なお、上記実施形態では、可視光通信用カメラ３０として、２台の前後カメラを使用した場合を例にして説明したが、可視光通信用カメラ３０は、３台以上のカメラにより構成されるものであってもよい。
一連の可視光通信用カメラ３０を用いた測位処理は、１台のカメラでも、２台のカメラでも、３台以上のカメラでも成立するが、カメラの検知範囲と解像度から得られる高精度測位を実現するために、最も推奨される構成が前後２カメラ構成である。

【0128】

また、同じ総画素を振り分ける場合、全周に亘って画角を取るよりも、前後方向のみを切り出して画角を限定した方が、各画素を細かく捉えることができる（より遠くの撮像対象を識別可能となる）。したがって、たいていの場合、画角を２倍にした単一カメラより、画角が同じカメラを前後に２台搭載した方が、可視光通信における光源ＩＤの検知と精度のバランスが良い。
なお、２つの画角で切り取る場合、前向きと９０度横向きに２台のカメラを設置しても数学的処理モデルとしては同様であるが、例えば、２点だけの光源ＩＤを受信したときの精度は、横見掛け角に依存するので、最も角度の離れた画像を取得できるような位置関係（前方と後方等）が最も良い。

【0129】

また、各処理を実行するためのプログラムを格納したコンピュータ読み取り可能な媒体としては、上記の例に限定されない。その他のコンピュータ読み取り可能な媒体として、フラッシュメモリ等の不揮発性メモリ、ＣＤ－ＲＯＭ等の可搬型記録媒体を適用することも可能である。また、プログラムのデータを通信回線を介して提供する媒体として、キャリアウェーブ（搬送波）を適用することとしてもよい。

【符号の説明】

【0130】

１ 測位システム
３Ａ，３Ｂ，３Ｃ，３Ｄ，・・・ 位置指標
４Ａ，４Ｂ，４Ｃ，４Ｄ，４Ｅ，４Ｆ，・・・ 恒星
１０ 制御部
１１ 可視光通信処理部
１２ 測位処理部
１３ スタートラッカ姿勢角算出部
１４ ３軸姿勢角・尤度決定処理部
１５ 測位尤度評価部
１６ 移動速度制御部
２０ 記憶部
２１ 光源ＩＤ－３次元座標テーブル
２２ 星図データ
２３ 尤度－速度制御テーブル
３０（３０Ａ，３０Ｂ） 可視光通信用カメラ
４０ スタートラッカ用カメラ
５０ ６軸センサ
６０ 走行駆動部
１００ 移動体

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】