特開 2022-144063 センサ間誤差角推定方法および位置同定方法

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022144063

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

(54)【発明の名称】センサ間誤差角推定方法および位置同定方法

(51)【国際特許分類】

   G01C 9/00 20060101AFI20220926BHJP

   G01C 1/00 20060101ALI20220926BHJP

【ＦＩ】

G01C9/00 Z

G01C1/00 D

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2021044905

(22)【出願日】2021-03-18

(71)【出願人】

【識別番号】000104652

【氏名又は名称】キヤノン電子株式会社

(72)【発明者】

【氏名】柴田 拓馬

(72)【発明者】

【氏名】丹羽 佳人

(57)【要約】

【課題】センサによる検出値を用いて導出された情報から、システム内部の演算処理に依存せずにセンサ間の誤差角を推定する手法を提供する。
【解決手段】システムの状態を推定するために搭載されている第１のセンサと第２のセンサを用いて、姿勢変更前後の状態を取得し、計算されたシステムの状態と真値との推定誤差値を計算することで、センサ間の誤差角を推定する。内部演算処理で誤差角を疑似的に変動させ、計算されたシステムの状態の真値に対する変動関係を関数で近似する。近似した関数を用いて、計算値と真値の誤差が０となるように関数を解くことでセンサ間誤差角を推定する。
【選択図】図４

【特許請求の範囲】

【請求項1】

第１のセンサと第２のセンサとの間のセンサ間誤差角を推定する方法であって、
搭載されている筐体の第１の状態を取得する第１のセンサ、および前記筐体における前記第１の状態とは異なる第２の状態を取得する第２のセンサが搭載された筐体の姿勢を変更する姿勢変更ステップと、
前記姿勢変更ステップの前後で前記第１のセンサと前記第２のセンサにより前記第１の状態と前記第２の状態を取得する状態取得ステップと、
前記状態取得ステップで取得した前記第１の状態および前記第２の状態に基づいて筐体の状態を推定する状態推定ステップと、
前記状態推定ステップで推定される推定値に対応する真値を取得する真値取得ステップと、
前記第１のセンサと前記第２のセンサ間に擬似的な誤差角を設定し、前記推定値と前記真値との間の推定誤差値を導出する推定誤差導出ステップと、
導出された前記推定誤差値を用いて、前記センサ間誤差角を導出するセンサ間誤差角推定ステップと
を含むことを特徴とするセンサ間誤差角推定方法。

【請求項2】

前記第１のセンサは加速度センサであり、前記第１の状態は重力ベクトルであることを特徴とする請求項１に記載のセンサ間誤差角推定方法。

【請求項3】

前記第２のセンサはスタートラッカーであり、前記第２の状態は前記筐体の姿勢情報であることを特徴とする請求項１または２に記載のセンサ間誤差角推定方法。

【請求項4】

前記姿勢変更ステップを複数回実行し、それに伴い前記状態取得ステップを複数回実行するとともに、
前記擬似的な誤差角の値を複数の異なる値に設定し、前記推定誤差導出ステップを複数回繰り返し実行し、
それによって得られた複数の前記推定誤差値に基づいて前記センサ間誤差角を導出することを特徴とする請求項３に記載のセンサ間誤差角推定方法。

【請求項5】

前記推定誤差導出ステップにおいて導出された前記推定誤差値と前記推定誤差値の導出に用いた前記疑似的な誤差角との関係を表す行列を算出する行列算出ステップと、
前記行列を記憶部に記憶する記憶ステップと
を含み、
前記センサ間誤差角推定ステップにおいては、前記記憶部に記憶された前記行列に基づいて、前記真値と前記推定誤差値との誤差を０にするような誤差角を計算することで、前記センサ間誤差角を導出することを特徴とする請求項１から４のいずれか一項に記載のセンサ間誤差角推定方法。

【請求項6】

前記真値取得ステップにおいて、通信手段を介して前記真値を取得することを特徴とする請求項１から５のいずれか一項に記載のセンサ間誤差角推定方法。

【請求項7】

請求項１から６のいずれか一項に記載されたセンサ間誤差角推定方法を用いて推定された前記センサ間誤差角に基づいて、前記筐体の位置を同定することを特徴とする位置同定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、複数のセンサが搭載されたシステムに適用可能である。

【背景技術】

【0002】

センサ間のアライメントを正確に測定しキャリブレーションすることは、センサフュージョンを行うシステムにおいて重要である。しかしながら、キャリブレーション後、システムを稼働する中で、センサ間のアライメントがある誤差角でずれてしまうことが考えられる。例えば、地上での位置同定システムではセンサデータを使用して求めたい行列を最小二乗法にて推定する手法が存在する。

【先行技術文献】

【非特許文献】

【0003】

【非特許文献1】An accuracy of better than 200m in positioning using a portable star tracker, Meysam Izadmehr, and Mehdi Khakian Ghomi, New Astronomy, Vol.74, 2020, doi:10.1016/j.newast.2019.04.004.

【発明の概要】

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

【0004】

しかしながら、前記手法では、特定のシステムにおける使用が保証されているが、センサ間誤差角を推定したい対象のシステムが変化した場合には、推定法の再構成が必要となる。それに対し、システムの内部演算プロセスまたは構成自体が変化した場合でも適用することができるセンサ間誤差角推定方法が求められている。

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

【0005】

上記を鑑み、本発明に係るセンサ間誤差角推定方法は、
第１のセンサと第２のセンサとの間のセンサ間誤差角を推定する方法であって、
搭載されている筐体の第１の状態を取得する第１のセンサ、および前記筐体における前記第１の状態とは異なる第２の状態を取得する第２のセンサが搭載された筐体の姿勢を変更する姿勢変更ステップと、
前記姿勢変更ステップの前後で前記第１のセンサと前記第２のセンサにより前記第１の状態と前記第２の状態を取得する状態取得ステップと、
前記状態取得ステップで取得した前記第１の状態および前記第２の状態に基づいて筐体の状態を推定する状態推定ステップと、
前記状態推定ステップで推定される推定値に対応する真値を取得する真値取得ステップと、
前記第１のセンサと前記第２のセンサ間に擬似的な誤差角を設定し、前記推定値と前記真値との間の推定誤差値を導出する推定誤差導出ステップと、
導出された前記推定誤差値を用いて、前記センサ間誤差角を導出するセンサ間誤差角推定ステップと
を含むことを特徴とする。

【発明の効果】

【0006】

本発明によれば、疑似的にセンサ間に誤差角を与えて、真値と推定値との推定誤差値を導出することで、推定値誤差を最小にする尤もらしいセンサ間誤差角を推定することにより、システムの内部演算プロセスまたは構成に依存せずにセンサ間誤差角の推定を行うことができる。

【図面の簡単な説明】

【0007】

【図1】誤差角推定手法を含む位置同定システムの構成例を示す図。

【図2】図１の構成例におけるセンサ座標系について説明するための図。

【図3】重力方向ベクトルと緯度経度の関係を示す図。

【図4】位置同定システムにおけるセンサ間誤差角推定手法のフローチャート。

【図5】図４のステップＳ３の手順の一例を示すフローチャート。

【図6】図４のステップＳ６の手順の一例を示すフローチャート。

【図7】図４のステップＳ７の手順の一例を示すフローチャート。

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

【0008】

以下、本発明の好適な実施形態について説明を行う。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を限定するものではない。また以下で説明される構成のすべてが本発明の必須構成要件であるとは限らない。以下センサ間の誤差角を推定する手法の一例として、惑星や衛星などの重力を有する星の上の探査機の位置を同定するシステムを例に挙げる。

【0009】

１ 位置推定理論
１－１ 位置推定システム構成
本実施形態における、星上の位置を同定するシステムの構成例は、図１、図２に示すようなものとなる。図 １に示す通り、自己位置同定機２００は、加速度センサ３とスタートラッカー（ＳＴＴ）５を含み、各センサから得られた情報を処理する演算装置１によって、自己位置同定機２００を搭載した探査機の星上の位置を推定する。このとき、星上の位置は、緯度経度情報を用いて定義する。

【0010】

図１のシステムを構成する自己位置同定機２００の一例としては、図２に示すように、加速度センサ３とＳＴＴ５を探査機の筐体３００に搭載する構成が挙げられる。それぞれのセンサに座標系を定義し、加速度センサ（ＡＣＣ）３に固定された座標系をＡＣＣ座標系Ａ、ＳＴＴ５に固定された座標系はＳＴＴ座標系Ｓとする。また本構成例においては、ＳＴＴ座標系Ｓと機体座標系は一致しているとする。仮に一致しない場合であっても、ＳＳＴ座標系Ｓで算出された結果を機体座標系に変換すれば良い。

【0011】

後述するように、位置同定を行うためには座標系Ｓと座標系Ａ間の回転行列が必要となる。しかしながら、様々な要因によって、各センサが機械的な取り付け誤差を生じることで、位置同定の精度に大きく影響を与える。このため、センサ間の回転行列を正確に推定することが要求される。ここで本構成例において、センサ間誤差角の定義はＳＴＴ座標系Ｓを基準とし、図２に示すように加速度センサ３のＡＣＣ座標系ＡがＡＣＣ座標系Ａ’へと変化した角度として定義する。その他、詳細な構成などについては後述する。

【0012】

１－２ 重力ベクトルからの自己位置同定手法
まずは、上述した加速度センサ３のＡＣＣ座標系Ａ’へのズレを考慮せずに、本実施形態において、ある星上の探査機の緯度経度を、重力ベクトルを用いて推定する方法について説明する。この手法は、図３に示す星の固定座標系Ｃに対する重力加速度ベクトルg^c=[g^c _x, g^c _y, g^c _z]を計算し、以下の式を用いて緯度φと経度λを計算する。

【数1】

【数2】

【0013】

重力加速度ベクトルは自己位置同定機２００に搭載されている加速度センサ３の出力から得る。このとき、重力ベクトルは加速度センサ３のＡＣＣ座標系Ａで観測されたベクトルg^Aとなる。自己位置を推定するためには、加速度センサ３によって得られた重力ベクトルg^AをＡＣＣ座標系Ａから星の固定座標系Ｃへ座標変換を行い、式（１）、（２）へ各軸方向の重力ベクトルの大きさを代入する必要がある。

【0014】

１－３ ＡＣＣ座標系Ａから星の固定座標系Ｃへの回転行列の導出
ＡＣＣ座標系Ａから星の固定座標系Ｃへの回転行列R^c/A は、慣性座標系Ｉから星の固定座標系Ｃへの回転行列R^c/Iと、ＡＣＣ座標系Ａから慣性座標系Ｉへの回転行列R^I/Aを用いて、以下の式のように表される。なお、慣性座標系Ｉは、例えば地球における定義であればJ2000系などの基準座標系に代表される、各軸を空間に固定した座標系として定義されるものを使用することができるが、これに限られない。

【数3】

【0015】

式（３）は、グリニッジ恒星時ξとＳＴＴ座標系Ｓから慣性座標系Ｉへの回転行列R^I/S 、ＡＣＣ座標系ＡからＳＴＴ座標系Ｓへの回転行列R^S/Aを用いて以下のように書き直すことができる。

【数4】

【0016】

ここで行列R_zはｚ軸回りの座標回転行列、行列R_NPは歳差章動を考慮する回転行列である。図 １に示すクロック１２から取得される時刻情報により、行列R_z(ξ)、行列R_NP が算出される。また、行列R^I/SはＳＴＴ５によって決定した姿勢情報から求まる行列である。行列R^S/Aはあらかじめ計測したＳＴＴ５と加速度センサ３間の関係を表す回転行列である。これらセンサ情報から求められた回転行列を用いることで回転行列R^c/Aを求めることができる。回転行列R^c/Aと加速度センサ座標系Ａで観測された重力方向ベクトルg^Aを用いて、星の固定座標系Ｃに対する重力方向ベクトル g^cは以下のように計算する事ができる。

【数5】

【0017】

式（５）で求められた各軸の重力方向ベクトルの要素を式（１）、（２）に代入することで、自己位置同定システム１００を搭載した探査機の星上の緯度経度が求められる。

【0018】

１－４ センサ間誤差角の考慮
精度良く位置同定を行うためには、式（４）において加速度センサ座標系ＡからＳＴＴ座標系Ｓへの回転行列R^S/A を使用前に精度良く求めておくことが必要である。しかしながら、上述したように様々な要因（例えば振動等の要因）によりその回転行列が変化する可能性がある。つまり、ＳＴＴ５に対する加速度センサ３間の回転行列が行列R^S/A からR^S/A’ へと変化する（図２）。

【0019】

自己位置同定システム１００を地球上で使用した場合、約０.０１度の角度誤差は地上距離１.０ｋｍ程度に相当するため、著しく自己位置同定の精度を低下させる。このため補正回転行列R^A/A’ を使用時に推定し、星の固定座標系Ｃに対する重力方向ベクトルを以下の式のように補正回転行列R^A/A’ を加えて計算する必要がある。

【数6】

【0020】

１－５ センサ間誤差角の推定手法
本実施形態では、センサ間誤差角行列ΔΘ=(Δθ_x, Δθ_y, Δθ_z) を推定する手法として、内部演算において疑似的にセンサ間誤差角行列ΔΘ_pを補正回転行列R^A/A’に代入し、真値と推定値の推定誤差との関係を表す関数に用いられる行列を記録していく。例えば、R^A/A’は以下の回転行列で表すことができる。

【数7】

【0021】

式（７）の補正回転行列R^A/A’を含む本実施形態例である位置同定手法へ、取り付け誤差を有した加速度センサ３により測定される重力ベクトルg^A’、ＳＴＴ５から得られる姿勢情報行列qを代入し、ある星上での位置を計算する。このとき、擬似的なセンサ間誤差角（擬似誤差角）ΔΘ_pは内部演算で変更され、取得した真の位置情報（ここでは緯度φ_Tと経度λ_T）との推定誤差行列ΔPを最終的に出力する。なお、真の位置情報（真値）の取得については、例えば後述の相対ＶＬＢＩ法などが好適に用いられる。これら一連のプロセスを関数fと定義すると以下の式で表される。

【数8】

【0022】

この推定誤差行列ΔPと関数fへ入力した疑似誤差角行列ΔΘ_pの値の関係を関数で表す。式（８）から、例えば推定誤差行列ΔPと擬似誤差角ΔΘ_pの関係を以下のような線形方程式で表せるとすれば、次式のようになる。

【数9】

【0023】

この式（９）の関係を満たす係数行列G(q)_2×3と行列H(q)_2×1を導出することで、センサ間誤差角行列ΔΘを推定する。係数行列G(q)_2×3と行列H(q)_2×1は、ＳＴＴ５と加速度センサ３を搭載した筐体３００の姿勢を変化させ、各姿勢情報行列q毎に計算される。つまり、加速度センサ３によってＡＣＣ座標系Ａ’に対する重力方向ベクトルg^A’を、またＳＴＴ５においては、撮像した画像から抽出した星ベクトルと、あらかじめ内部に有している星カタログのデータを用いて推定される姿勢情報行列qを変化させながら、各姿勢で疑似誤差角行列ΔΘ_pを変化させ、係数行列G(q)_2×3と行列H(q)_2×1を計算し、記録する。求めたいセンサ間誤差角行列ΔΘは推定誤差行列ΔPが０となる値であるので、式９を変形し、以下の方程式をセンサ間誤差角ΔΘについて解くことで解を得る。

【数10】

【0024】

この場合、行列G^-1 _2n×3は行列G_2n×3の疑似逆行列である。

【0025】

２ センサ間誤差角推定手法
２－１ 位置同定手法におけるセンサ間誤差角推定法のフローチャート
図４は本実施形態に係る位置同定手法におけるセンサ間誤差角推定法を含めたフローチャートの例である。図４に示す手順の一例は、ある星上での自己位置同定システム１００に搭載されている加速度センサ３とＳＴＴ５間に生じているセンサ間の各軸の誤差角を推定する手法が含まれる。

【0026】

なお、この手法は、自己位置同定を目的としない、まったく異なるシステムに搭載することも可能である。例えば、この手法が対象とするシステムは、人工衛星や車など、複数のセンサ情報を用いてある状態を推定するシステムであり、その推定した状態の真値を取得する手段があること、加えてその姿勢依存性を考慮する場合は、システムの姿勢を変更する手段があればよい。

【0027】

図４に示すように、まず加速度センサ３とＳＴＴ５を搭載した筐体３００の姿勢行列qを予め設定された初期値q₁に決める（ステップＳ１）。この筐体３００の姿勢はジンバルなどの機構で変更ができるものであるとする。その後、擬似誤差角行列ΔΘ_pの初期値をステップＳ２においてx,y,zの各軸０度として、自己位置同定（ステップＳ３）による計算を行う。自己位置同定の詳細な処理については後述する。

【0028】

次に、ステップＳ４において、現在の姿勢情報行列qに対しそれまでにステップＳ３で推定した位置と真値との推定誤差行列ΔPと擬似誤差角行列ΔΘ_pの関係を例えば線形関数ΔP=G(q)ΔΘ_p+H (q)（式（９））で近似し、係数行列G(q)と行列H(q)を導出する。これらの行列はステップＳ５において記憶部２１に記録される。ステップＳ４、Ｓ５の詳細な処理についても後述する。

【0029】

ステップＳ６では、擬似誤差角行列ΔΘ_pに依存した推定誤差行列ΔPのデータをm組取得したか否かの判断を行う。この時点で、m組のデータを取得していない場合は、ステップＳ２の前に戻り、内部演算処理において、擬似誤差角行列ΔΘ_pを変更し、再度自己位置の推定を行う。この時、本実施形態におけるステップＳ２では、求めたい精度以下の刻み値で直前の値から変更することを繰り返す。求めたい精度以下での刻み値で変更を加えていくことで、擬似誤差角行列ΔΘ_pに対応した出力値（本構成例では推定誤差行列ΔP）の変化の分解能を上げる効果が得られる。

【0030】

一般的に、位置同定手法の例では、筐体３００の姿勢が変化することが考えられる。本実施形態においては、自己位置同定システム１００を搭載した探査機が傾斜した際のデータも考慮したセンサ間誤差角行列ΔΘを導出するため、姿勢依存のデータをn組取得する。このため、ステップＳ７においては、姿勢依存のデータをn組取得しているかどうかを判定する。

【0031】

この判定処理において、姿勢依存のデータがn組取得されていなければステップＳ１の前に戻り、筐体３００の姿勢をジンバル４によって変更する。具体的には、駆動機構制御部４０により予め決められた角度だけ姿勢変更するための指令を出力し、駆動機構４１を駆動することで姿勢を変更する。

【0032】

姿勢変更後、ステップＳ２の内部演算処理において、擬似誤差角行列ΔΘ_pを再度各軸０度に再設定し、自己位置同定プロセスの計算を行う。これらのデータ数は、擬似誤差角行列ΔΘ_pに対する推定値と真値との推定誤差行列ΔPの関係を表すことができるデータ数を十分に取得できるかで決定される。データ数の設定に関しては、擬似誤差角行列ΔΘ_pが推定誤差行列ΔPと線形の関係にあれば、強い非線形の関係にある場合に比べて多くのデータ数を必要としない。このデータ数は、自己位置同定システム１００を使用するユーザによってあらかじめ設定されても良い。

【0033】

図４の例の場合であれば、擬似誤差角行列ΔΘ_pに依存したデータ数がm組取得され、それらの値に基づいて線形関数ΔP=GΔΘ_p+H が推定される。

【0034】

一方、筐体３００の姿勢依存のデータ数はn組取得されるので、擬似誤差角行列ΔΘ_pと推定誤差行列ΔPの関係を示す係数行列Gと行列Hの最終的なサイズは2n×3と2n×1 となる。最終的に求められた係数行列Gと行列Hは、ステップＳ６とＳ７での条件判定がなされ、ステップＳ８において使用される。

【0035】

ステップＳ８では、これまでに得られた2n×3の係数行列Gと2n×1の行列Hを用いて、式（１０）に基づき推定値と真値との推定誤差行列ΔPが０となるセンサ間誤差角行列ΔΘを求める。

【0036】

なお、上記の説明においては、ステップＳ４、Ｓ５において変換行列（係数行列G(q)と行列H(q)）の導出とデータ保持を行う例について説明したが、タイミングはこれに限られず、ステップＳ６でデータをm組取得したと判定したあとで、実行するように構成しても良いし、ステップＳ７に遷移するまでは取得したデータの保持のみを行い、ステップＳ７で姿勢依存のデータをn組取得したあとで、n組分の変換行列をまとめて導出しても良い。

【0037】

図５は図４の自己位置同定プロセス（ステップＳ３）の手順の一例を示すフローチャート図である。この例では、ステップＳ１１において、時刻が取得される。この時刻は、システム内に設けられたクロック１２などから取得される、もしくは外部から取得することができるように構成される。ステップＳ１２（状態取得ステップ）では自己位置同定に必要となる加速度センサ３によって取得される重力ベクトル、加えてＳＴＴ５によって決定される慣性座標系Ｉに対する姿勢情報行列qを取得する。このとき、各センサからのデータを所定のサンプリング時間で取得する。

【0038】

ステップＳ１３では、センサデータのノイズを除去するためにフィルタリングを行う。２つのセンサ間のサンプリング周波数が異なり、ダウンサンプリングが必要となる場合には、エイリアシングを考慮したフィルターが適用される。

【0039】

ステップＳ１４では取得された時刻、慣性座標系Ｉに対するＳＴＴ座標系Ｓの姿勢情報行列q、ＳＴＴ座標系Ｓと加速センサ座標系Ａ間の座標変換行列R^c/Aを用いて、加速度センサ座標系Ａから星の固定座標系Ｃへの座標変換行列R^c/Aを式（４）に基づいて導出する。

【0040】

この座標変換行列R^c/Aと補正回転行列R^A/A’を用いてステップＳ１５において加速度センサ座標系Ａ’にて求められた重力方向ベクトルg^A’を式（６）に基づいて星の固定座標系Ｃへ変換し、重力方向ベクトルg^cを算出する。この時、センサ間誤差角行列ΔΘの推定を行う場合には、補正回転行列R^A/A’は、ステップＳ４でNoとなることによりステップＳ２にて逐次変更される。位置同定手法を用いて純粋な位置推定を行う（本来の目的である、探査機の位置を同定するために実際に用いられる）場合には、一連のセンサ間誤差角推定手法によって推定されたセンサ間誤差角行列ΔΘから求められる行列R^A/A’がステップＳ１５にて用いられる。ステップＳ１６（状態推定ステップ）では、星の固定座標系に対する重力ベクトルを用いて、筐体３００を搭載した探査機の位置を式（１）、（２）を用いて推定する。

【0041】

図６は図４のステップＳ６における変換行列導出プロセスの手順の一例を示すフローチャート図である。この例では、ステップＳ２１（真値取得ステップ）において、推定値に対応する真値を取得する。ステップＳ２２（推定誤差導出ステップ）では、推定値と真値の推定誤差値である推定誤差行列ΔPを計算する。ステップＳ２３において、推定値と真値の推定誤差行列ΔPと疑似的に図３のステップＳ２で変動させた擬似誤差角行列ΔΘ_pを用いて係数行列Gと行列Hを計算する。

【0042】

図７は図４のステップＳ５におけるデータ保持プロセスの手順の一例を示すフローチャート図である。この例では、ステップＳ３１で図６のステップＳ２３で計算された係数行列G(q)と行列H(q)を記憶部２１に書き込む。ステップＳ３３においては、これまでにステップＳ３１で記憶部２１に記録された係数行列G(q)と行列H(q)をそれぞれ結合したものを出力し、図４のステップＳ６とＳ７の判定処理を通過する時点では、最終的に2n×3の係数行列Gと2n×1の行列Hが出力されていることになる。なお、ステップＳ３１で記憶部２１に係数行列G(q)と行列H(q)を書き込む際に、最初からそれまでの係数行列G(q)と行列H(q)のそれぞれを結合したものとして書き込んでも良い。

【0043】

２－２ 位置同定手法におけるシステム構成
図１は、本実施形態の自己位置同定システム１００の構成例を示す図である。図２に示すように、本実施形態では、加速度センサ３とＳＴＴ５間のセンサ間誤差角行列ΔΘをセンサ間誤差角推定部２によって推定するものについて説明したが、他の態様として、第１のセンサと第２のセンサとの間の誤差角を推定するものについて適用可能である。本構成例においてはそのセンサ間誤差角行列ΔΘと加速度センサ３、ＳＴＴ５、とクロック１２から取得されるデータを用い、上述した方法で自己位置を推定する。自己位置同定演算装置１は、通信情報処理部１０、自己位置推定部１１、クロック１２、信号処理部１３によって構成される。

【0044】

自己位置同定演算装置１は、図２に示すように自己位置同定機２００の筐体３００に搭載されている加速度センサ３の想定しているＡＣＣ座標系Ａ（第１のセンサ座標系）と実際の加速度センサのＡＣＣ座標系Ａ’（ずれ後の第１のセンサ座標系）との間のずれ角であるセンサ間誤差角行列ΔΘを推定する。このとき、センサ間誤差角ΔΘの定義は、予め設定（算出）されたＳＴＴ座標系Ｓに対するＡＣＣ座標系Ａがなす角度が、ＡＣＣ座標系Ａ’へと変化した角度（ずれ角）として定義する。

【0045】

加速度センサ３は、互いに交差し、理想的には直交する３軸方向の加速度のアナログ信号を出力する。なお、他の態様における第１のセンサとしては加速度センサ３に限らず、重力方向ベクトルg^A’を出力することが可能であればよい。

【0046】

ＳＴＴ５は撮像センサ５０と、取得した画像を処理する画像処理部５２と、画像から得られたデータを用いて慣性座標系に対するＳＴＴ座標系Ｓの姿勢を演算して決定する姿勢決定部５１を含む。他の態様における第２のセンサとしては、ＳＴＴではなく、カメラと画像処理から姿勢決定までを行うことが可能である演算処理部の組み合わせであってもよい。また画像処理等の処理系は位置同定演算装置１に含まれていてもよい。

【0047】

信号処理部１３は、センサからのデータをある周期で取得し、ディジタル値への変換処理を行う。２つのセンサのサンプリング周波数を同期させるため、片方若しくは両方のセンサデータのダウンサンプリングを行う必要がある場合には、ローパスフィルタによる処理を行う。ノイズの影響を軽減するためのバタワースフィルターや拡張カルマンフィルタ等の処理も含まれる。

【0048】

ジンバル４は、自己位置同定機２００の姿勢を変更させるための駆動機構４１と、駆動機構４１の制御を所定のタイミングで行う駆動機構制御部４０を含む。自己位置同定機２００の姿勢を変更することが可能であれば必ずしも前述した駆動機構４１を含むジンバル４である必要はなく、所望の姿勢変更手段を適用可能である。

【0049】

送受信部６は、アンテナ７からの信号を処理する復調器／変調器やフィルターを含む。また通信情報処理部１０は、受信／送信する信号を所望のデータに変換する。センサ間誤差角を推定する際に必要とされる真の位置を取得する際には、例えば相対ＶＬＢＩを用いた手法を採用することができる。この手法では、自己位置同定機２００は、特定の電波をアンテナ７から地球局へ送信し、地球上の複数のアンテナで送信した信号と基準となる星からの電波を交互に取得することで高精度な位置決定（星上での緯度経度）を行うことができる。位置決定がされた後、地球局から星上の探査機にデータが送信される。地球局から送信された真の位置情報を取得するために、通信情報処理部１０を介して、そのデータはセンサ間誤差角推定部２に含まれる処理部２０へ入力される。処理部２０では、この情報を用いて図４に示した演算処理が行われる。なお、図１では、通信情報処理部１０は自己位置同定演算装置１に含まれているが、位置情報を取得し演算までを行う処理系を位置同定演算装置１とは別の系として組み合わせてもよい。

【0050】

３ 変形例
上述したように、本実施形態のセンサ間誤差角推定手法の対象は、自己位置同定システムである必要はなく、第１のセンサおよび第２のセンサも、加速度センサとＳＴＴには限られない。上述した実施形態では、ある星上における自己位置同定システムを例に挙げ、加速度センサとＳＴＴ間の誤差角を推定する手順を示したが、人工衛星や車両といった星上を移動する移動体に搭載される複数のセンサ情報を用いて移動体の位置を含む状態を推定するシステムであればよい。

【符号の説明】

【0051】

１：自己位置同定演算装置、２：センサ間誤差角推定部、３：加速度センサ、４：ジンバル、５：スタートラッカー（ＳＴＴ）、６：送受信部、７：アンテナ、１０：通信情報処理部、１１：自己位置推定部、１２：クロック、１３：信号処理部、２０：処理部、２１：記憶部、４０：駆動機構制御部、４１：駆動機構、５０：撮像センサ、５１：姿勢決定部、５２：画像処理部、１００：自己位置同定システム、２００：自己位置同定機、３００：筐体

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】