(57)【要約】【課題】衛星測位の初期化を短時間に高精度で実行する。【解決手段】衛星測位システムでは、視覚マーカの位置及び姿勢が決定されている。移動体は、視覚マーカ検出部と、測位信号を受信する信号受信部と、視覚マーカの位置及び姿勢が格納された記憶部とを有する。移動体は、視覚マーカの位置及び姿勢に基づいて衛星測位における初期化処理を実行した後、測位信号に基づいて衛星測位を実行する演算部を有し、演算部は、視覚マーカ検出部によって検出された視覚マーカの位置及び姿勢に基づいて、複数の観測点における移動体の地球固定座標系における推定位置を算出する。演算部は、衛星測位によって複数の観測点における移動体の地球固定座標系における測位位置を算出し、演算部は、複数の観測点における推定位置を繋いだ移動軌跡と、複数の観測点における測位位置を繋いだ移動軌跡とに基づいて、視覚マーカの姿勢の変化量を算出する。【選択図】図１
2020008454.tif 000002
【特許請求の範囲】
【請求項１】
地球固定座標系における位置及び姿勢が決定されている視覚マーカと、
前記視覚マーカを検出する視覚マーカ検出部と、人工衛星から測位信号を受信する信号受信部と、前記視覚マーカの前記位置及び前記姿勢に関する情報が格納された記憶部と、前記視覚マーカ検出部によって検出された前記視覚マーカの位置及び姿勢に基づいて衛星測位における初期化処理を実行した後、前記測位信号に基づいて前記地球固定座標系における前記衛星測位を実行する演算部とを有する移動体と
を具備し、
前記演算部は、前記視覚マーカ検出部によって検出された前記視覚マーカの前記位置及び前記姿勢に基づいて、複数の観測点における前記移動体の前記地球固定座標系における推定位置を算出し、
前記衛星測位によって前記複数の観測点における前記移動体の前記地球固定座標系における測位位置を算出し、
前記複数の観測点における前記推定位置を繋いだ第１移動軌跡と、前記複数の観測点における前記測位位置を繋いだ第２移動軌跡とに基づいて、前記視覚マーカの前記姿勢の変化量を算出する
衛星測位システム。
【請求項２】
請求項１に記載された衛星測位システムであって、
前記演算部は、前記衛星測位の開始点を中心に、前記第１移動軌跡と、前記第２移動軌跡とを回転させた場合、
前記複数の観測点中のｉ番目（０≦ｉ≦Ｎ、ｉは整数、０番目は観測開始点、Ｎ番目は観測終点）の観測点における前記測位位置と、前記ｉ番目の前記観測点における前記推定位置との差の二乗を前記複数の前記観測点について足し合わせた和が最小二乗法によって最小となるような回転量を前記視覚マーカの前記姿勢の前記変化量とする
衛星測位システム。
【請求項３】
請求項１または２に記載された衛星測位システムであって、
前記視覚マーカの他に、他の視覚マーカをさらに具備し、
前記演算部は、前記視覚マーカの前記姿勢の前記変化量と、前記他の視覚マーカの前記姿勢の前記変化量とを算出されたそれぞれの前記姿勢の変化量を用いて補正した後、
前記視覚マーカの前記位置及び前記姿勢に基づいて算出された前記移動体の推定位置と、前記他の視覚マーカの前記位置及び前記姿勢に基づいて算出された前記移動体の推定位置との差分を算出し、
それぞれの前記姿勢の前記変化量を比較して、前記差分を視覚マーカの移動変化量に適用する
衛星測位システム。
【請求項４】
請求項１〜３のいずれか１つに記載された衛星測位システムであって、
前記演算部は、前記視覚マーカ検出部によって連続的に複数回検出された前記視覚マーカの前記位置及び前記姿勢のデータに基づいて前記初期化処理を実行する
衛星測位システム。
【請求項５】
地球固定座標系における位置及び姿勢が決定されている視覚マーカと、前記視覚マーカを検出する視覚マーカ検出部と、人工衛星から測位信号を受信する信号受信部と、前記視覚マーカの前記位置及び前記姿勢に関する情報が格納された記憶部と、前記視覚マーカ検出部によって検出された前記視覚マーカの位置及び姿勢に基づいて衛星測位における初期化処理を実行した後、前記測位信号に基づいて前記地球固定座標系における前記衛星測位を実行する演算部とを有する移動体とを準備し、
前記初期化処理の後、前記視覚マーカ検出部によって検出された前記視覚マーカの前記位置及び前記姿勢に基づいて、複数の観測点における前記移動体の前記地球固定座標系における推定位置を算出し、
前記衛星測位によって前記複数の観測点における前記移動体の前記地球固定座標系における測位位置を算出し、
前記複数の観測点における、前記推定位置及び前記測位位置に基づいて、前記視覚マーカの前記姿勢の変化量を算出する
衛星測位方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、人工衛星を利用した衛星測位システム及び衛星測位方法に関する。
【背景技術】
【０００２】
衛星測位方法には、単独搬送波位相測位（Precise Point Positioning, ＰＰＰ）や、ＰＰＰにおいて搬送波位相バイアスの整数解を求めることで精度の向上を図ったＰＰＰ−ＡＲ（Precise Point Positioning-Ambiguity Resolution）がある。これらの測位には、初期化が完了し、充分な測位精度が得られるまでに数１０分程度の時間がかかるという課題があった。
【０００３】
これに対して、地球固定座標系における位置が既知である点に視覚マーカを設置し、この視覚マーカを移動体に搭載したカメラで撮影し、この撮影で得られた移動体の推定位置を初期化に利用することで、ＰＰＰまたはＰＰＰ−ＡＲの初期化時間を大幅に減少させる手法がある（例えば、特許文献１参照）。そして、短時間で充分な測位精度を得る初期化を行うには、視覚マーカの地球固定座標系における位置及び姿勢に関する情報を精密且つ正確に移動体が保持している必要がある。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１８−００９９５９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、移動体が視覚マーカの地球固定座標系における位置及び姿勢に関する情報を精密且つ正確に保持していても、視覚マーカが何らかの要因で元の状態から動いてしまうと、移動体が保持している位置または姿勢に関する情報が真の値からずれることになる。このような場合、初期化を短時間で充分な測位精度で実行できなくなる場合がある。
【０００６】
以上のような事情に鑑み、本発明の目的は、衛星測位の初期化を短時間に高精度で実行する衛星測位システム及び衛星測位方法を提供することにある。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、本発明の一形態に係る衛星測位システムは、視覚マーカと、移動体とを具備する。
視覚マーカにおいては、地球固定座標系における位置及び姿勢が決定されている。
移動体は、視覚マーカを検出する視覚マーカ検出部と、人工衛星から測位信号を受信する信号受信部と、視覚マーカの位置及び姿勢に関する情報が格納された記憶部と、を有する。さらに、移動体は、視覚マーカ検出部によって検出された視覚マーカの位置及び姿勢に基づいて衛星測位における初期化処理を実行した後、測位信号に基づいて地球固定座標系における衛星測位を実行する演算部を有する。
演算部は、視覚マーカ検出部によって検出された視覚マーカの位置及び姿勢に基づいて、複数の観測点における移動体の地球固定座標系における推定位置を算出する。
演算部は、衛星測位によって複数の観測点における移動体の地球固定座標系における測位位置を算出する。
演算部は、複数の観測点における推定位置を繋いだ第１移動軌跡と、複数の観測点における測位位置を繋いだ第２移動軌跡とに基づいて、視覚マーカの姿勢の変化量を算出する。
【０００８】
このような衛星測位システムであれば、複数の観測点における、推定位置及び測位位置を重ねることにより視覚マーカの姿勢の変化量を算出できるので、視覚マーカが元の状態からずれたとしても、姿勢の変化量を補正できる。これにより、衛星測位の初期化をより高い測位精度で実行することができる。
【０００９】
衛星測位システムにおいては、演算部は、衛星測位の開始点を中心に、第１移動軌跡と、第２移動軌跡とを回転させた場合、複数の観測点中のｉ番目（０≦ｉ≦Ｎ、ｉは整数、０番目は観測開始点、Ｎ番目は観測終点）の観測点における測位位置と、ｉ番目の観測点における推定位置との差の二乗を複数の観測点について足し合わせた和が最小二乗法によって最小となるような回転量を視覚マーカの姿勢の変化量としてもよい。
【００１０】
このような衛星測位システムであれば、衛星測位の開始点を中心に、衛星測位された移動体の移動軌跡と、視覚マーカの位置及び姿勢に基づいて算出された移動体の移動軌跡とを回転させたときに、双方の移動軌跡が略合致したときの回転量を視覚マーカの姿勢のずれとする。この回転量に基づき姿勢の変化量を補正するので、衛星測位の初期化をより高い測位精度で実行することができる。
【００１１】
衛星測位システムにおいては、視覚マーカの他に、他の視覚マーカをさらに具備してもよい。
演算部は、視覚マーカの前記姿勢の変化量と、他の視覚マーカの姿勢の変化量とを算出されたそれぞれの前記姿勢の変化量を用いて補正した後、視覚マーカの位置及び姿勢に基づいて算出された移動体の推定位置と、他の視覚マーカの位置及び姿勢に基づいて算出された移動体の推定位置との差分を算出してもよい。
演算部は、それぞれの姿勢の変化量を比較して、差分を視覚マーカの移動変化量に適用してもよい。
【００１２】
このような衛星測位システムであれば、姿勢の変化量がより大きい方の視覚マーカに対して、視覚マーカに基づいて算出された移動体の推定位置と、他の視覚マーカに基づいて算出された移動体の推定位置との差分が視覚マーカの移動変化量として推定される。これにより、位置が移動した視覚マーカの移動変化量を補正することができ、衛星測位の初期化をより高い測位精度で実行することができる。
【００１３】
衛星測位システムにおいては、演算部は、視覚マーカ検出部によって連続的に複数回検出された視覚マーカの位置及び姿勢のデータに基づいて初期化処理を実行してもよい。
【００１４】
このような    衛星測位システムによれば、初期化処理が視覚マーカ検出部によって複数回検出された視覚マーカの位置及び姿勢のデータに基づいて実行されるので、姿勢の変化量の推定精度が向上する。
【発明の効果】
【００１５】
以上述べたように、本発明によれば、衛星測位の初期化を短時間に高精度で実行する衛星測位システム及び衛星測位方法が提供される。
【図面の簡単な説明】
【００１６】
【図１】本実施形態に係る衛星測位システムの一例を示す概念図である。
【図２】衛星測位システムにおける視覚マーカの姿勢の変化量を算出する手法を説明する概要図である。
【図３】衛星測位システムにおける視覚マーカの姿勢の変化量を算出する手法を説明する概要図である。
【図４】衛星測位システムにおける視覚マーカの移動変化量を算出する手法を説明する概要図である。
【発明を実施するための形態】
【００１７】
以下、図面を参照しながら、本発明の実施形態を説明する。各図面には、３次元座標が導入される場合がある。また、同一の部材または同一の機能を有する部材には同一の符号を付し、その部材を説明した後には適宜説明を省略する場合がある。
【００１８】
図１は、本実施形態に係る衛星測位システムの一例を示す概念図である。
【００１９】
図１に示すように、本実施形態に係る衛星測位システム１００は、移動体２００と、視覚マーカ２１０とを具備する。移動体２００は、車両２０１と、カメラ２０２と、ＧＮＳＳアンテナ２０３と、記憶部２０４と、演算部２０５と、マーカ画像処理部２０６とを有する。視覚マーカ２１０は、支持台２１２に支持される。図１の座標系ＥＣは、地球固定座標系ＥＣ、座標系ＡＣは、ＧＮＳＳアンテナ座標系ＡＣ、座標系ＣＣは、カメラ座標系ＣＣを意味する。また、移動体２００の位置とは、地球固定座標系ＥＣにおけるＧＮＳＳアンテナ２０３の位置とする。また、視覚マーカ２１０の「姿勢」を定義する場合は、地球固定座標系ＥＣに対する視覚マーカ２１０に固定された視覚マーカ座標系ＭＣを導入してもよい。
【００２０】
本実施形態に係る衛星測位システム１００の作用を説明する前に、特許文献１記載の先行発明である視覚マーカ２１０を利用したＰＰＰ方式の概要について説明する。
【００２１】
衛星測位システム１００では、ＰＰＰ方式における初期化処理が実行された後、移動体２００が測位衛星（人工衛星）１１、１２、１３、１４のそれぞれから発信される測位信号を受信して、移動体２００の位置が高精度に測定される。
【００２２】
複数の測位衛星１１、１２、１３、１４には、高精度の時計が搭載されている。しかし、時計が刻む時刻には、誤差がある場合があり、また、測位衛星１１、１２、１３、１４の軌道にも誤差がある場合がある。さらに、測位衛星１１、１２、１３、１４から発せられる測位信号は、移動体２００に到達するまでに、電離層及び対流圏を伝搬する。その際に信号が遅延する場合もある。
【００２３】
ＰＰＰ方式では、予め、４つ以上の測位衛星から電波を受け、観測結果及びモデルを用い、衛星軌道や衛星時計の誤差、及び電離層や対流圏を通過することによる信号の遅延量や搬送波位相バイアスなど多数のパラメータを推定する初期化処理が行われる。初期化処理は、測位信号に基づき衛星測位を行おうとする移動体２００に搭載された演算部２０５によって実行される。初期化処理が済めば、地球に固定された地球固定座標系ＥＣにおける移動体２００の正確な位置について測定することができる。
【００２４】
従来から、ＰＰＰ方式の初期化処理には、数１０分〜１時間程度の時間がかかると言われてきた。これは、ＧＮＳＳアンテナ２０３の初期の位置推定精度が高くない場合、この位置を含めた測位パラメータの推定を行う際、十分な精度で推定値を求めるために、多量の衛星信号を処理する必要があるためである。
【００２５】
次に、本実施形態の衛星測位システム１００の作用を説明する。
【００２６】
本実施形態では、衛星測位システム１００に視覚マーカ２１０（例えば、特許第５８４２２４８号公報参照）を組み込む方法（例えば、特開２０１８−００９９５９号公報参照）を採用することにより、ＰＰＰ方式における初期化処理を大幅に短縮している。
【００２７】
例えば、視覚マーカ２１０の地球固定座標系ＥＣにおける位置及び姿勢を予め精密に決定しておく。
【００２８】
ここで、予め決定された視覚マーカ２１０の位置及び姿勢に関するデータは、移動体２００の記憶部２０４に格納されている。該データは、例えば、視覚マーカ２１０のＩＤ番号と対応付けがなされている。移動体２００には、さらに、視覚マーカ検出部（例えば、カメラ２０２）と、測位衛星１１、１２、１３、１４から測位信号を受信する信号受信部（例えば、ＧＮＳＳアンテナ２０３）とが搭載されている。
【００２９】
カメラ２０２とＧＮＳＳアンテナ２０３との相対的な位置関係は予め計測されている。次に、移動体２００が起動して、カメラ２０２の視野に視覚マーカ２１０が入るように移動体２００が移動し、カメラ２０２が視覚マーカ２１０を検出する。
【００３０】
視覚マーカ２１０の正確な位置及び姿勢を予め精密に決定しておくことによって、演算部２０５は、カメラ２０２によって検出された視覚マーカ２１０の位置及び姿勢に基づいて、地球固定座標系ＥＣにおけるＧＮＳＳアンテナ２０３の推定位置を算出する。
【００３１】
例えば、視覚マーカ２１０は、移動体２００に搭載されたカメラ２０２で撮影され、その画像がマーカ画像処理部２０６に入力され、マーカ画像処理部２０６が視覚マーカ２１０の位置、姿勢、ＩＤ等を出力する。演算部２０５は、マーカ画像処理部２０６から出力された情報を受けて、移動体２００に搭載したプログラムによってカメラ２０２に対する視覚マーカ２１０の位置及び姿勢を計算する。なお、マーカ画像処理部２０６と演算部２０５とを含めて、総括的に演算部と称してもよい。
【００３２】
該プログラムによって算出された視覚マーカ２１０の位置及び姿勢のデータと、予め精密に測定した視覚マーカ２１０の位置及び姿勢のデータと、カメラ２０２とＧＮＳＳアンテナ２０３との相対的な位置関係とから、地球固定座標系ＥＣにおけるＧＮＳＳアンテナ２０３の位置及び姿勢（推定位置）を求めることができる。
【００３３】
ここで、カメラ２０２が視覚マーカ２１０の画像を取得するタイミングは、ＧＮＳＳアンテナ２０３が測位衛星１１、１２、１３、１４からの信号を受信するタイミングと、ＧＮＳＳアンテナ２０３から出力される信号をトリガとして同期している。これにより、移動体２００が移動しながら撮影した視覚マーカ２１０から算出した移動体２００の位置と、衛星測位によって求められた移動体２００の位置との同期をとることができる。
【００３４】
こうして得られたＧＮＳＳアンテナ２０３の位置及び姿勢の推定データがＰＰＰ方式の測位パラメータの測定に利用される。例えば、演算部２０５は、カメラ２０２によって検出された視覚マーカ２１０の位置及び姿勢に基づいて衛星測位における初期化処理を実行する。この後、演算部２０５は、測位信号に基づいてＧＮＳＳアンテナ２０３（移動体２００）の地球固定座標系ＥＣにおける測位位置を算出する。
【００３５】
このような初期化処理を導入することにより、従来、衛星測位システムの初期化処理で必要とされた衛星信号の情報量が劇的に減り、初期化処理で要される時間が大幅に短縮される。例えば、その時間は、数分程度である。なお、この段階での初期化処理を第１初期化処理とする。初期化処理は、視覚マーカ２１０の姿勢に変化がない場合、この段階で完了する。
【００３６】
しかしながら、視覚マーカ２１０の姿勢は、何らかの要因（例えば、環境変化、自然災害等）で予め精密に決定された状態からずれる場合がある。このような場合には、視覚マーカ２１０の姿勢の真の値と、衛星測位システム１００が算出した視覚マーカ２１０の姿勢の値とに差異が生まれ、初期化処理が失敗する可能性がある。例えば、並進方向に３０ｍｍ程度、回転方向に０．１度程度の誤差があっても、初期化処理が失敗する可能性がある。なお、予め精密に決定された状態を「元の状態」と呼ぶ。
【００３７】
特許文献１（特開２０１８−００９９５９号公報）では、視覚マーカ２１０の姿勢に関して、衛星測位システムが算出した値と、視覚マーカ２１０の姿勢の真の値とに差異があるか否かを判定する手法が提案されている。しかし、この手法では、視覚マーカ２１０の姿勢の変化量の推定には言及されていない。さらに、視覚マーカ２１０を設置した位置の変化量の推定にも言及されていない。
【００３８】
これに対して、本実施形態に係る衛星測位システム１００においては、演算部２０５がカメラ２０２によって検出された視覚マーカ２１０の位置及び姿勢に基づいて、複数の観測点における移動体２００の地球固定座標系ＥＣにおける推定位置を算出する。次に、演算部２０５は、衛星測位によって複数の観測点における移動体２００の地球固定座標系ＥＣにおける測位位置を算出する。そして、複数の観測点における、推定位置と、測位位置とに基づいて、演算部２０５が視覚マーカ２１０の姿勢の変化量を算出する。
【００３９】
さらに、本実施形態に係る衛星測位システム１００においては、２つの視覚マーカ２１０を利用して、２つの視覚マーカ２１０のいずれかの姿勢の変化量から、演算部２０５が視覚マーカ２１０の移動変化量を推定する。
【００４０】
まず、視覚マーカ２１０の姿勢の変化量を算出する手法について説明する。
【００４１】
図２（ａ）〜図２（ｃ）は、衛星測位システムにおける視覚マーカの姿勢の変化量を算出する手法を説明する概要図である。
【００４２】
移動体２００の移動軌跡の算出方法には、移動しながら視覚マーカ２１０を撮影して、移動体２００の位置を算出する方法と、初期化処理後、衛星測位によって移動体２００の位置を測定する方法の２通りがある。
【００４３】
例えば、図２（ａ）及び図２（ｂ）には、Ｚ軸方向を地球の地面に対する鉛直方向とし、水平方向をＸ軸方向及びＹ軸方向とし、地球の地面に対して鉛直方向に移動体２００を見たときの地球固定座標系ＥＣにおける移動体２００の移動軌跡と、地球固定座標系ＥＣにおける視覚マーカ２１０とが示されている。ここで、図２（ａ）には、移動体２００の推定位置の移動軌跡が示され、図２（ｂ）には、移動体２００の測位位置の移動軌跡が示されている。
【００４４】
また、図２（ａ）の黒丸、図２（ｂ）の白丸は、移動体２００の観測点を表している。それぞれの観測点を線で繋ぐことで、移動体２００の移動軌跡を得ることができる。なお、推定位置と、測位位置とは、同時に算出してもよく、独立して算出してもよい。図２（ａ）、（ｂ）では、視覚マーカ２１０の姿勢が元の状態から変化していないことを想定している。
【００４５】
図２（ａ）では、移動体２００が移動しながら、カメラ２０２が視覚マーカ２１０を撮影した画像に基づき、移動体２００の位置が算出されている。位置の算出は、開始点から終点までに複数の観測点（黒丸）で実行されている。これらの観測点を線で繋ぐと移動軌跡となり、それを移動軌跡Ａとする。
【００４６】
一方、図２（ｂ）には、初期化処理後、衛星測位によって移動体２００の位置が算出されている。位置の算出は、開始点から終点までに複数の観測点（白丸）で実行されている。これらの観測点を線で繋ぐと移動軌跡になり、これを移動軌跡Ｂとする。
【００４７】
視覚マーカ２１０の姿勢が元の状態から変化していない場合、衛星測位システム１００が記憶している視覚マーカ２１０の姿勢の値と、視覚マーカ２１０の姿勢の真の値とが一致し、図２（ａ）、（ｂ）のそれぞれの移動軌跡Ａ、Ｂは、計測誤差の範囲内で略一致する。例えば、移動軌跡Ａ、Ｂのそれぞれが重ねられた状態が図２（ｃ）に示されている。図２（ｃ）に示すように、移動軌跡Ａ、Ｂのそれぞれは、略一致している。
【００４８】
次に、視覚マーカ２１０の姿勢が元の状態からずれた場合の移動体２００の移動軌跡について説明する。一例として、視覚マーカ２１０がＺ軸を中心として姿勢がずれた場合の移動体２００の移動軌跡について説明する。この場合、地球固定座標系ＥＣのＺ軸と、視覚マーカ座標系ＭＣのＺ軸とは、平行になる。
【００４９】
図３（ａ）〜図３（ｃ）は、衛星測位システムにおける視覚マーカの姿勢の変化量を算出する手法を説明する概要図である。
【００５０】
図３（ａ）には、視覚マーカ２１０によって算出された移動体２００の推定位置が示され、図３（ｂ）には、衛星測位によって算出された移動体２００の測位位置が示されている。また、図３（ａ）、（ｂ）には、図２（ａ）、（ｂ）で描いた移動軌跡Ａ、Ｂが重ねて示されている。移動軌跡Ａ、Ｂは、視覚マーカ２１０の姿勢が変化した場合には得ることができない移動軌跡である。
【００５１】
図３（ａ）では、移動体２００が移動しながら視覚マーカ２１０を撮影して、地球固定座標系ＥＣにおける移動体２００の位置が算出されている。図３（ａ）における移動軌跡を移動軌跡Ｃ（第１移動軌跡）としている。
【００５２】
図３（ａ）に示す視覚マーカ２１０のように、元の状態から姿勢の変化が生じた場合、視覚マーカ２１０を用いて算出された地球固定座標系ＥＣにおける移動体２００の位置は、開始点において真の姿勢を利用して求めた位置から偏差σを持って離れることになる。
【００５３】
この偏差σは、視覚マーカ２１０から移動体２００までの距離に応じて決まる量である。例えば、視覚マーカ２１０から移動体２００までの距離が長いほど、視覚マーカ２１０の姿勢変化が大きく偏差に影響を及ぼす。例えば、移動体２００が観測点の開始点から視覚マーカ２１０に近づき、観測点の開始点から観測点の終点に向かうほど視覚マーカ２１０から移動体２００までの距離が単調に短くなる場合は、開始点で偏差σを有していても、終点での偏差は、開始点での偏差よりも小さくなる。
【００５４】
一方、図３（ｂ）には、初期化処理後、衛星測位によって移動体２００の位置を算出した様子が示されている。この場合の移動軌跡を移動軌跡Ｄ（第２移動軌跡）としている。
【００５５】
視覚マーカ２１０を利用した衛星測位の初期化では、視覚マーカ２１０の観測によって得られた移動体２００の位置に偏差σがある場合、初期化直後に得られる測位位置にも偏差σが重畳される。この偏差σは、移動体２００の移動の有無に関らず、時間の経過とともに僅かずつ減少し、測位位置は、最終的に偏差０の場合の測位結果と一致する。
【００５６】
但し、この時間経過にともなう偏差σの減少の割合は極小さい。従って、短時間（数１０秒程度）であれば、一定値の偏差σが測位位置に重畳されているとみなすことができる。従って、初期化処理の後に、衛星測位により短時間で求められた移動体２００の移動軌跡Ｄは、衛星測位を行った各観測点の位置精度に関らず、その地球固定座標系ＥＣにおける方位が高い精度を有している。
【００５７】
すなわち、移動軌跡Ｄの地球固定座標系ＥＣにおける方位は、移動軌跡Ａの方位に略一致することになる。換言すれば、移動軌跡Ｄは、移動軌跡Ａの各観測点が偏差σだけずれた移動軌跡に略一致する。
【００５８】
さらに、視覚マーカ２１０の撮影により算出した移動体２００の開始点と、衛星測位により算出した移動体２００の開始点とは一致する。従って、視覚マーカ２１０の姿勢変化量は、図３（ｃ）のように、それぞれの開始点を重ね合わせた点を支点として、移動軌跡Ｃと移動軌跡Ｄとを回転させたときに、移動軌跡Ｃ、Ｄにおける各観測点が最も近似するときの回転量として算出することができる。なお、衛星測位で観測点毎に初期化処理を実行すると、移動軌跡Ｄが移動軌跡Ｃに略重なってしまい、該回転量が求まらないことになる。回転量を定式化すると、次式のようになる。
【００５９】
【数１】
2020008454.tif 000003
・・・式（１）
【数２】
2020008454.tif 000004
・・・式（２）
【００６０】
ここで、式（２）は、絶対値の中に含まれる行列式であり、視覚マーカ２１０の姿勢変化を示す回転行列である。ｐs,iENU０は、衛星測位によって求められたｉ番目の観測点の位置である。ｐm,iENU０は、視覚マーカ２１０の観測によって求められたｉ番目の観測点の位置である。添え字ENU0は、開始点を原点とし、各座標軸が東、北、天頂方向に対応した地球固定座標系ＥＣにおける位置であることを示している。式（１）では、回転量θによって総和Σが最小となるようなパラメータを成分に含む行列式（２）が求められる。
【００６１】
この式に基づき、演算部２０５は、衛星測位の開始点（支点）を中心に、衛星測位された移動体の移動軌跡と、視覚マーカ２１０の位置及び姿勢に基づいて算出された移動体の移動軌跡とを回転させた場合、複数の観測点中のｉ番目（０≦ｉ≦Ｎ、ｉは整数、０番目は観測開始点、Ｎ番目は観測終点）の観測点において衛星測位された測位位置と、ｉ番目の観測点において視覚マーカ２１０の位置及び姿勢に基づいて算出された移動体の推定位置との差の二乗をすべての観測点について足し合わせた和が最小二乗法によって最小となるような回転量θを視覚マーカ２１０の姿勢の変化量として算出する。
【００６２】
衛星測位に表れる初期化処理の偏差の影響が短時間であれば一定とみなせるという前提の成立性を高めるため、衛星測位の方式として、緩やか且つ連続的な収束特性を持つＰＰＰ方式を採用している。ＰＰＰ方式に加え、Ambiguity Resolution（AR）を行って衛星測位の精度をより高めたい場合は、この設置姿変化推定のためにＰＰＰ−ＡＲとは別にＰＰＰ方式を並行して実行してもよい。
【００６３】
また、演算部２０５は、時間経過にともなう偏差σの減少の割合をさらに小さくし、短時間（数１０秒程度）であれば一定の偏差σが測位位置に重畳されているとみなせるという前提の成立性をより高め、結果として姿勢変化の推定精度を向上させるため、カメラ２０２によって連続的に複数回検出された視覚マーカ２１０の位置及び姿勢のデータに基づいて初期化処理を実行してもよい。この場合、観測点間の距離が大きく離れた各観測点で複数回の初期化処理を実行すると、偏差一定の仮定が成り立たなくなるため、視覚マーカ２１０から充分離れた観測点である、例えば、開始点近傍で連続的に複数回検出された視覚マーカ２１０の位置及び姿勢のデータに基づいて初期化処理を実行する。
【００６４】
表１、２には、二次元平面上の視覚マーカ２１０の姿勢角（°）が変化したとき、本実施形態の手法で回転量（°）を推定した実験結果である。ここで、誤差（°）は、姿勢角と回転角との差の絶対値で定義されている。
【００６５】
【表１】
2020008454.tif 000005
【表２】
2020008454.tif 000006
【００６６】
表１には、姿勢角が変化した場合（０°〜０．５°）の結果が示され、表２には、さらに、＋５０ｍｍの位置変化を加えた場合の結果が示されている。いずれもの場合も姿勢角の推定誤差が０．０５°以内に収まっていることが確認された。特に、表２に示されるように、位置変化が加わっていても、姿勢変化を高精度に推定することができる。
【００６７】
次に、視覚マーカ２１０の設置位置の変化を推定する手法を説明する。視覚マーカ２１０の姿勢変化を算出するだけでは、視覚マーカ２１０の設置位置の変化を推定することはできない。この場合、２つの視覚マーカ２１０Ａ、２１０Ｂを利用して、２つの視覚マーカ２１０Ａ、２１０Ｂのいずれかの姿勢の変化量から、演算部２０５が視覚マーカ２１０の移動変化量を推定する。
【００６８】
図４は、衛星測位システムにおける視覚マーカの移動変化量を算出する手法を説明する概要図である。
【００６９】
図４に示す衛星測位システム１００は、視覚マーカ２１０Ａの他に、他の視覚マーカ２１０Ｂを具備する。
【００７０】
図４に示す衛星測位システム１００においては、演算部２０５が視覚マーカ２１０Ａの姿勢の変化量と、視覚マーカ２１０Ｂの姿勢の変化量とを本実施形態の手法で求めた後、それぞれの変化量を算出されたそれぞれの姿勢の変化量を用いて予め補正する。これにより、視覚マーカ２１０Ａ、２１０Ｂのそれぞれの地球固定座標系ＥＣにおける姿勢精度は高くなっている。
【００７１】
次に、演算部２０５は、視覚マーカ２１０Ａの位置及び姿勢に基づいて移動体２００の推定位置ＰＡを算出し、視覚マーカ２１０Ｂの位置及び姿勢に基づいて移動体２００の推定位置ＰＢを算出する。ここで、視覚マーカ２１０Ａ、２１０Ｂの双方が元の位置からずれた場合、移動体２００の真の位置ＰＲは求まらず、移動体２００の推定位置としては、位置ＰＲからδＡだけずれた推定位置ＰＡと、位置ＰＲからδＢだけずれた推定位置ＰＢとが求まる。
【００７２】
次に、演算部２０５は、推定位置ＰＡと推定位置ＰＢとの差分（δＢ−δＡ）を算出する。差分（δＢ−δＡ）とは、視覚マーカ２１０Ａ、２１０Ｂのそれぞれの設置位置の変化量の差分である。
【００７３】
ここで、視覚マーカ２１０Ａ、２１０Ｂの中、一方の視覚マーカの姿勢の変化量が他方の視覚マーカの姿勢の変化量よりも大きいとする。例えば、一方の視覚マー力の姿勢変化量が０でなく、もう一方の視覚マーカの姿勢変化量が０であるとする。この場合、姿勢量が大きい方の視覚マーカの位置のみが元の位置からずれている可能性が高い。
【００７４】
これは、視覚マーカの位置または姿勢が変化する場合、位置または姿勢のいずれか１つがずれるよりも、位置及び姿勢が連動してずれる可能性が高いからである。つまり、この場合、２つの視覚マーカを観測して求められた位置変化の差分は、そのまま姿勢に変化が起きている視覚マーカの位置変化として推定することが合理的である。
【００７５】
この場合、２つの視覚マーカの位置変化の差分を求めることにより、その差分を用いて２つの視覚マーカのいずれかの視覚マーカの位置変化を推定することが可能となる。すなわち、姿勢の変化量がより大きい方の視覚マーカに対して、位置変化の差分が視覚マーカの移動変化量に適用される。そして、この移動変化量に基づいて、視覚マーカの移動のずれが補正される。
【００７６】
さらに、一般にいずれの視覚マーカの姿勢変化量も０でない場合を含め、視覚マーカの設置状態をモデル化して、位置変化と姿勢変化との対応関係を予め想定し、視覚マーカの姿勢変化の方向や回転量に応じて位置変化を視覚マーカに割り当ててもよい。
【００７７】
以上説明した、衛星測位システム１００によれば、複数の観測点における推定位置を繋いだ移動軌跡Ｃと、複数の観測点における測位位置を繋いだ移動軌跡Ｄとに基づき、移動軌跡Ｃと移動軌跡Ｄとを重ねることにより、視覚マーカ２１０の姿勢の変化量が算出される。例えば、衛星測位の開始点を中心に、衛星測位された移動軌跡Ｃと、移動軌跡Ｄとを回転させたときに、双方の移動軌跡が略合致したときの回転量を視覚マーカ２１０の姿勢のずれとする。
【００７８】
これにより、視覚マーカ２１０の姿勢が元の状態からずれたとしても、姿勢の変化量を精度よく補正できる。姿勢の変化量が精度よく補正されれば、改めて行う初期化処理が高い精度で実行できる。この結果、衛星測位をより高い測位精度で実行することができる。なお、視覚マーカ２１０の姿勢変化が検出され、その姿勢変化量を補正したうえで行われる初期化処理を第２初期化処理とする。すなわち、第２初期化処理は、視覚マーカ２１０の姿勢変化が検出されたときに行われる初期化処理である。
【００７９】
また、衛星測位システム１００であれば、視覚マーカ２１０Ａ、２１０Ｂのそれぞれの姿勢の前記変化量を比較して、差分（δＢ−δＡ）を視覚マーカの移動変化量に適用する。例えば、姿勢の変化量がより大きい方の視覚マーカに対して、視覚マーカに基づいて算出された移動体の推定位置と、他の視覚マーカに基づいて算出された移動体の推定位置との差分が視覚マーカの移動変化量として推定される。これにより、位置が移動した視覚マーカの移動変化量を補正することができ、改めて行う初期化処理が高い精度で実行できる。この結果、衛星測位をより高い測位精度で実行することができる。なお、視覚マーカ２１０の移動変化量を補正したうえで行われる初期化処理を第３初期化処理とする。
【００８０】
また、衛星測位システム１００によれば、初期化処理がカメラ２０２によって複数回検出された視覚マーカ２１０の位置及び姿勢のデータに基づいて実行されるので、姿勢の変化量の推定精度が向上する。
【００８１】
以上、本発明の実施形態について説明したが、本発明は上述の実施形態にのみ限定されるものではなく種々変更を加え得ることは勿論である。各実施形態は、独立の形態とは限らず、技術的に可能な限り複合することができる。
【００８２】
また、本実施形態では、ＧＮＳＳを用いた衛星測位方式としてＰＰＰ方式を用いた場合について説明した。しかしながら、本実施形態は、基準局を不要とする方式であって初期化処理に時間を要する衛星測位方式であれば、ＰＰＰ方式には限られない。
【符号の説明】
【００８３】
１１、１２、１３、１４…測位衛星
１００…衛星測位システム
２００…移動体
２０１…車両
２０２…カメラ
２０３…ＧＮＳＳアンテナ
２０４…記憶部
２０５…演算部
２１０、２１０Ａ、２１０Ｂ…視覚マーカ
２１２…支持台
【図１】
2020008454.tif 000007
【図２】
2020008454.tif 000008
【図３】
2020008454.tif 000009
【図４】
2020008454.tif 000010