JP3767372B2 - 衛星追尾用アンテナ制御装置 - Google Patents
衛星追尾用アンテナ制御装置 Download PDFInfo
- Publication number
- JP3767372B2 JP3767372B2 JP2000356062A JP2000356062A JP3767372B2 JP 3767372 B2 JP3767372 B2 JP 3767372B2 JP 2000356062 A JP2000356062 A JP 2000356062A JP 2000356062 A JP2000356062 A JP 2000356062A JP 3767372 B2 JP3767372 B2 JP 3767372B2
- Authority
- JP
- Japan
- Prior art keywords
- absolute
- azimuth
- satellite
- estimated
- posture
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Variable-Direction Aerials And Aerial Arrays (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Description
【０００１】
【発明の属する技術分野】
この発明は衛星追尾用アンテナ制御装置に関し、特に、車両や船舶、航空機等の移動体に搭載可能で、通信衛星との通信を行うための衛星追尾用アンテナ制御装置に関する。
【０００２】
【従来の技術】
通信衛星等の衛星と車両、船舶または航空機等の地上の移動体との衛星通信を行う際には、無指向性アンテナを用いることも多いが、画像データ等のように大量の情報を伝送する場合等には、送信電力や受信分解能の限界から、鋭い指向性を有するアンテナを用いることが望ましい。この場合には、常にアンテナビームを衛星の方向に向けておく必要があり、これを追尾と一般に呼んでいる。図１６は、例えば特開平１０−４８３０５号公報に示された従来の車載用通信衛星追尾装置を示すブロック図である。図において、１０１は、ｘ加速度、ｙ加速度、ｚ加速度の検出を行う３軸加速度センサ、１０２は、ｘ角速度、ｙ角速度、ｚ角速度の検出を行う３軸角速度センサ（レートジャイロ）、１０３はｘ加速度およびｚ加速度の値を用いてピッチ角を生成するピッチ角生成部、１０７はｙ加速度およびｚ加速度の値を用いてロール角を生成するロール角生成部、１０４は緯度、経度、高度、方位の検出を行うＧＰＳ装置（ＧＰＳ：Global Positioning System（全世界測位システム））、１０５は車輪回転計から車輪速の検出を行う車輪速センサ、１０６はアンテナの方位角（アジマス）および仰角（エレベーション）の制御を行うアンテナ方向制御装置である。
【０００３】
動作について説明する。３軸角速度センサ１０２の信号を移動体の回転運動検出に用いるとともに、ピッチ角生成部１０３およびロール角生成部１０７で生成した角度信号を姿勢の基準信号として、また、ＧＰＳ装置１０４で検出した方位信号を方位の基準信号として用いて、移動体の絶対姿勢および絶対方位を推定し、それを用いてアンテナ方向を制御する。
【０００４】
【発明が解決しようとする課題】
上述したように、従来の通信衛星追尾装置においては、３軸角速度センサ１０２の信号や３軸加速度センサ１０１の信号を用いて、移動体の絶対姿勢及び絶対方位を推定しているため、３軸角速度センサ１０２や３軸加速度センサ１０１の持つドリフト誤差やバイアス誤差に起因して、移動体の絶対姿勢及び絶対方位の推定誤差が発生してしまうため、高精度にアンテナ方向を制御することができないという問題点があった。
【０００５】
この発明は、かかる問題点を解決するためになされたものであり、３軸角速度センサや３軸加速度センサの持つドリフト誤差やバイアス誤差による移動体の絶対姿勢および絶対方位推定誤差を補償して、高精度にアンテナ方向の制御を行って衛星を追尾するための衛星追尾用アンテナ制御装置を得ることを目的とする。
【０００６】
【課題を解決するための手段】
この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから上記衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動するとともに、当該衛星相対方向に衛星相対方向探索手段から得られた衛星相対方向を重畳した方向にアンテナあるいはアンテナビームを駆動して衛星を指向し、その際の移動体に対するアンテナあるいはアンテナビームの方位角および仰角を前記姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信機の受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置である。
【０００７】
また、この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動し、衛星相対方向探索手段は、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、その方向を衛星相対方向算出手段より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、上記方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置である。
【０００８】
また、姿勢方位推定手段において、姿勢推定フィルタおよび方位推定フィルタにおける帰還入力から姿勢、方位軸に関する上記角速度センサのドリフト誤差を推定するとともに、移動体の並進加速度推定値が大きい場合には、姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、上記移動体の並進速度推定値が小さい場合、あるいは、ＧＰＳ非受信時の場合において、衛星非見通し時の場合には、上記方位推定フィルタにおける帰還系を切断するとともに、切断時においてはこれまでの帰還入力から推定した角速度センサのドリフト誤差を入力として補償する。
【０００９】
また、姿勢方位推定手段において、帰還入力から推定した角速度センサのドリフト誤差に対して、各姿勢方位軸に備えた角速度センサのドリフト特性に応じた低域通過フィルタを適用する。
【００１０】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値とする。
【００１１】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、先ず最小二乗近似の意味で移動体の絶対姿勢を推定し、次にその推定値を用いて絶対方位参照値を推定する。
【００１２】
また、逐次的に更新して推定した上記移動体の絶対姿勢、あるいは、最小二乗近似の意味で推定した上記移動体の絶対姿勢を参照して姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、切断時においてはこれまでの帰還入力から推定した上記角速度センサのドリフト誤差を入力として補償する。
【００１３】
また、初期衛星捕捉時、あるいは、衛星非見通し時から見通し時への復帰時において、姿勢方位推定手段における方位推定フィルタを、アンテナ駆動手段あるいは衛星相対方向探索手段より得られたアンテナあるいはアンテナビームの方位角および仰角を用いて推定した絶対方位推定値で初期化する。
【００１４】
【発明の実施の形態】
実施の形態１．
図１は、この発明の実施の形態１による衛星追尾用アンテナ制御装置を説明するための図である。図１において、１は移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段、２は移動体の絶対位置と追尾する衛星の絶対位置から衛星の方位角および仰角を算出する衛星絶対方向算出手段、３は移動体の絶対姿勢および絶対方位、衛星の方位角および仰角から移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段、４は移動体に対する方位角および仰角方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動装置、５は衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段、６は衛星追尾用アンテナ、７は移動体のアンテナ取り付け面、８は追尾する衛星である。
【００１５】
この衛星追尾用アンテナ制御装置における動作の説明のために以下の座標系を導入する。
[a0]を慣性座標系とする。これは慣性空間に固定された座標系で、原点は地球中心にある。ただし地球の公転の影響は無視する。
[ae]を地球固定座標系とする。これは地球に固定された座標系で、原点は地球中心にある。xe 、ye軸は赤道面内にあり、xe 軸はグリニッジ子午線(経度零)を通る。またze軸は北極方向を向いている。
[al]を局所水平面座標系とする。これは機体の質量中心に原点を持ち、地球上の局所水平面（接平面）上で規定された座標系である。xl 、yl 、zl軸はそれぞれ地理上の北、東、接平面垂直下方向を向いている。
[ab]を移動体固定座標系とする。これは移動体の質量中心に原点を持ち、移動体に固定された座標系である。xb 、yb 、zb軸がそれぞれ移動体のロール軸、ピッチ軸、ヨー軸に対応する。
[ag]をジャイロ検出軸座標系とする。これは姿勢方位推定手段１に固定された座標系である。xg 、yg 、zg軸がそれぞれ、姿勢方位推定手段１に搭載されたジャイロ検出軸方向と一致する。
[aa]を加速度計検出軸座標系とする。これは姿勢方位推定手段１に固定された座標系である。xa 、ya 、za軸がそれぞれ、姿勢方位推定手段１に搭載された加速度計検出軸方向と一致する。
【００１６】
さらに以下の記号を定義する。
Ａijを座標系[aj]から座標系[ai]への座標変換行列とする。ωijを座標系[aj]に対する座標系[ai]の角速度とする。移動体の絶対位置を表す緯度、経度、高度をそれぞれ
【数１】
また、移動体の絶対姿勢および絶対方位を表すロール角、ピッチ角、真方位角をそれぞれφ、θ、ψで表す。ｖbeを地球固定座標系[ae]に対する移動体の並進速度とする。また、ｇを単位質量あたりに作用する重力ベクトルとする。
【００１７】
衛星絶対方向算出手段２では、追尾する衛星の絶対位置とＧＰＳ受信機（図２の符号１９参照）で受信した移動体の絶対位置から、衛星の方位角および仰角を算出する。地球中心から移動体までの距離ベクトルrb、および地球中心から衛星までの距離ベクトルrsはそれぞれ、Rを地球半径として次式で与えられる。
【００１８】
【数２】
【００１９】
【数３】
【００２０】
ここで右肩添字ｅは表現されている座標系を表す。また下付き添字ｓは追尾する衛星に関する諸量を表すものとする。この時、移動体から衛星までの距離ベクトルは、局所水平面座標系 [al] で次のように表される。
【００２１】
【数４】
【００２２】
これより衛星の方位角αaと仰角αeがｒsb lの３成分を用いて次のように算出される。
【００２３】
【数５】
【００２４】
姿勢方位推定手段１における信号処理は、移動体の回転運動に関する姿勢方位推定フィルタ、および移動体の並進運動に関する位置速度推定フィルタに分類される。
【００２５】
先ず姿勢方位推定フィルタについて説明する。図２は姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。図２において、９は３軸ジャイロ、１０は角速度算出部、１１は姿勢方位角速度推定部、１２は積分演算部、１３は回転系帰還入力算出部、１４は３軸加速度計、１５は座標変換部、１６は絶対姿勢参照値算出部、１７は絶対方位参照値算出部、１８は磁気コンパス、１９はＧＰＳ受信機、４は図１に示したアンテナ駆動装置である。
【００２６】
図２における角速度算出部１０において、３軸ジャイロ９によって検出された角速度ωg = [ωxg ,ωyg ,ωzg]Tを用いて局所水平面座標系[al]に対する移動体固定座標系[ab]の角速度を算出する。なお、ここでは、簡単のため地球の自転の影響は無視した。
【００２７】
【数６】
【００２８】
次に姿勢方位角速度推定部１１において、移動体の回転運動に関する運動方程式から移動体の絶対姿勢角速度(ロール角速度とピッチ角速度)および絶対方位角速度を推定する。すなわち
【００２９】
【数７】
【００３０】
また行列Ｓは現時点の絶対姿勢推定値φ、θから構成される下記の３×３行列である。
【００３１】
【数８】
【００３２】
またurは回転系帰還入力算出部１３から算出された入力量で、この詳細については後述する。
【００３３】
次に積分演算部１２において、推定した絶対姿勢角速度および絶対方位角速度を積分することにより、移動体の絶対姿勢推定値(ロール角推定値とピッチ角推定値)および絶対方位推定値を得る。
【００３４】
一方、座標変換部１５において、３軸加速度計１４によって検出された加速度fa = [fxa ,fya ,fza]Tを移動体固定座標系[ab]で表現する。
【００３５】
【数９】
【００３６】
絶対姿勢参照値算出部１６では、座標変換部１５において算出した加速度を用いて移動体固定座標系[ab]での重力ベクトルを推定し、その方向から移動体の絶対姿勢参照値を推定する。重力ベクトルは、移動体の並進運動に関する運動方程式から次のように求められる。
【００３７】
【数１０】
【００３８】
ここで×は外積演算を表す。移動体の並進運動加速度ｖbe lドットに関しては、それが小さいものと仮定して零と置くか、あるいは、ＧＰＳ受信機１９より検出した絶対位置を数値的に２階微分して算出する。また並進速度ｖbe lに関しては、後述する位置速度推定フィルタより得られた推定値を用いる。こうして求めた移動体固定座標系[ab]での重力ベクトルgbの３成分を用いて、絶対姿勢参照値が次のように求められる。
【００３９】
【数１１】
【００４０】
絶対方位参照値算出部１７では、複数のセンサから絶対方位参照値の候補を得、それらの中から適切な絶対方位参照値を選択する。先ず移動体に搭載した磁気コンパス１８から、移動体の絶対方位が検出される。ただしこの検出値の信頼性は、移動体の周囲環境の磁気的外乱に大きく左右される。また移動体に搭載したＧＰＳ受信機１９から、移動体の進行方向方位が検出される。ただしこの検出値が利用可能な場合は、ＧＰＳが測位時で、かつ移動体が並進運動している場合に限られる。またその信頼性は主に移動体の並進速度に依存している。あるいはもしＧＰＳ受信機１９が干渉測位可能な受信機である場合には、これより移動体の絶対方位が検出される。最後の絶対方位の候補は、衛星見通し時にアンテナ駆動装置４から得られた移動体に対するアンテナあるいはアンテナビームの方位角および仰角を用いて推定される。その手法を以下に述べる。
【００４１】
衛星絶対方向算出手段２より得られた衛星の方位角αaおよび仰角αe、衛星見通し時における移動体に対するアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハット、移動体の絶対姿勢(ロール角φとピッチ角θ) の間には次の関係式が成立している。
【００４２】
【数１２】
【００４３】
ここで絶対姿勢に関して姿勢推定フィルタにおける現時点での推定値を利用すれば、上式は未知数である絶対方位ψに関する方程式となる、これを解くことによって絶対方位に関する正弦sinψと余弦cosψが得られる。これから絶対方位参照値が次のように推定される。
【００４４】
【数１３】
【００４５】
ただしこの推定値が利用可能な場合は、衛星が見通し可能な場合に限られる。
【００４６】
このように搭載したセンサあるいは装置から３通りの絶対方位参照値の候補を得、現時点で移動体が直面している状況に応じて最も信頼性の高い推定値を選択して絶対方位参照値とする。図３はその選択方法の一例を示す図である。磁気コンパス１８は常時絶対方位を検出可能であるが、その精度は周囲環境の磁気的外乱に大きく左右されるため、磁気的外乱には影響されないＧＰＳ受信機１９で検出した進行方向方位を用いてその信頼性を判定する。先ずＧＰＳ受信機１９が未測位あるいは検出した速度があらかじめ決定した閾値未満の場合には（ステップＳ１）、検出した進行方向方位の信頼性は乏しい。その場合において、衛星が見通し時の場合は（ステップＳ２）、アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットから求めた絶対方位推定値を絶対方位参照値とする（ステップＳ３）。衛星が見通し不可の場合は（ステップＳ２）、絶対方位参照値が得られなかったものとして、後述する回転系帰還入力urの方位軸に関する成分を零とする（ステップＳ４）。ＧＰＳ測位時において検出した速度が閾値以上の場合には（ステップＳ１）、ＧＰＳ受信機１９で検出した進行方向方位と磁気コンパス１８で検出した絶対方位を一定時間の間比較し、両者の間の偏差があらかじめ決定した閾値未満なら（ステップＳ５）、磁気コンパス１８で検出した絶対方位は信頼できるものとし、それを絶対方位参照値とする（ステップＳ６）。それ以外の場合には（ステップＳ５）、信頼性に乏しいものとし、ＧＰＳ受信機１９で検出した進行方向方位を絶対方位参照値とする（ステップＳ７）。
あるいは図４に示すように選択してもよい。先ず衛星が見通し時の場合は（ステップＳ８）、アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットから求めた絶対方位推定値を絶対方位参照値とする（ステップＳ９）。次に衛星が見通し不可の場合において（ステップＳ８）、ＧＰＳ受信機１９が未測位あるいは検出した速度があらかじめ決定した閾値未満の場合には（ステップＳ１０）、絶対方位参照値が得られなかったものとして、後述する回転系帰還入力ｕrの方位軸に関する成分を零とする（ステップＳ１１）。ＧＰＳ測位時において検出した速度が閾値以上の場合には（ステップＳ１０）、ＧＰＳ受信機１９で検出した進行方向方位と磁気コンパス１８で検出した絶対方位を一定時間の間比較し、両者の間の偏差があらかじめ決定した閾値未満なら（ステップＳ１２）、磁気コンパス１８で検出した絶対方位は信頼できるものとし、それを絶対方位参照値とする（ステップＳ１３）。それ以外の場合には（ステップＳ１２）、信頼性に乏しいものとし、ＧＰＳ受信機１９で検出した進行方向方位を絶対方位参照値とする（ステップＳ１４）。
【００４７】
回転系帰還入力算出部１３では、３軸ジャイロ９のドリフト誤差による姿勢方位推定フィルタにおける発散を防止するために、先に推定した絶対姿勢参照値と絶対方位参照値、および現時点での絶対姿勢推定値と絶対方位推定値を用いてそれらの間の偏差を計算し、適切なゲインを乗じて帰還入力urとし姿勢方位角速度推定部１１へ帰還する。
【００４８】
次に、姿勢方位推定手段１における信号処理の２つ目である位置速度推定フィルタについて説明する。図５は位置速度推定フィルタにおける信号処理の概略を示すブロック図である。図５において、２０は座標変換部、２１は並進加速度推定部、２２は積分演算部、２３は測地座標速度推定部、２４は積分演算部、２５は絶対位置参照値算出部、２６は絶対速度参照値算出部、２７は並進系帰還入力算出部である。符号１４、１５，１９は、図２と同じであるため、ここでは説明を省略する。
【００４９】
図５における座標変換部１５において、３軸加速度計１４によって検出された加速度fa = [fxa ,fya ,fza]T を移動体固定座標系[ab]で表現する。
【００５０】
【数１４】
【００５１】
さらに座標変換部２０において、前述の姿勢方位推定フィルタより推定された絶対姿勢推定値および絶対方位推定値を用いて、局所水平面座標系[al] に変換する。
【００５２】
【数１５】
【００５３】
次に並進加速度推定部２１において、移動体の並進運動に関する運動方程式から移動体の並進加速度を推定する。すなわち
【００５４】
【数１６】
【００５５】
ここでutは並進系帰還入力算出部２７から算出された入力量で、この詳細については後述する。
【００５６】
次に積分演算部２２において、推定した並進加速度を積分することにより、移動体の並進速度推定値vbe lを得る。
【００５７】
また測地座標速度推定部２３において、現時点の位置速度推定値を用いて、移動体の並進運動に関する運動方程式から移動体の絶対位置を表す測地座標の時間微分値を推定する。すなわち
【００５８】
【数１７】
【００５９】
これを積分演算部２４において積分することにより、移動体の絶対位置推定値
【数１８】
を得る。
【００６０】
絶対位置参照値算出部２５では、ＧＰＳ受信機１９で受信した移動体の絶対位置(緯度、経度、高度)を絶対位置参照値とする。また並進速度参照値算出部２６では、ＧＰＳ受信機１９で受信した移動体の絶対位置を数値的に１階微分することにより移動体の並進速度vbe lを推定し、これを並進速度参照値とする。
【００６１】
並進系帰還入力算出部２７では、３軸加速度計１４のバイアス誤差による位置速度推定フィルタにおける発散を防止するために、絶対位置参照値と並進速度参照値、および現時点での絶対位置推定値と並進速度推定値を用いてそれらの間の偏差を計算し、適切なゲインを乗じて帰還入力utとし並進加速度推定部２１へ帰還する。なおＧＰＳ１９が受信不可で絶対位置参照値あるいは並進速度参照値が得られない場合には、それまでの受信値を保持して算出するか、あるいは対応する帰還入力を零とする。
【００６２】
衛星相対方向算出手段３では、姿勢方位推定手段１で推定した移動体の絶対姿勢φ、θおよび絶対方位ψ、および衛星絶対方向算出手段で算出した衛星の方位角αaおよび仰角αeから、移動体に対する方位角仰βaおよび角βeを算出する。衛星指向方向単位ベクトルに着目すると、これらの諸量の間には以下の関係式が成立している。
【００６３】
【数１９】
【００６４】
従ってこの第３式から移動体に対する衛星の仰角βe、また第１、２式から移動体に対する衛星の方位角βaが算出される。
【００６５】
ここで求めたβa、βeを指令値としてアンテナ駆動装置４において、アンテナあるいはアンテナビームを推定される衛星方向に指向させる。さらにこれとは独立して、衛星相対方向探索手段５においては、衛星から送信される信号の受信レベルを用いて、それが最大となる衛星相対方向が探索される。例えばアンテナあるいはアンテナビームを、衛星相対方向算出手段３で算出した方向を中心として、移動体に対する方位角および仰角方向に微小量駆動し、その近傍でのアンテナ受信レベルを数点に渡って記憶しておき、それらを補間して最大受信レベル位置を決定することにより、最も確からしい衛星相対方向を探索し、その際の方位角および仰角増分信号Δβa、Δβeを衛星相対方向算出手段３より算出した方位角および仰角信号βa、βeに重畳してアンテナあるいはアンテナビームを指向させる。これとともにアンテナ駆動装置４には方位角および仰角を検出するためのエンコーダが備え付けられており、その際の方位角βaハット（＝βa＋Δβa）および仰角βeハット（＝βe＋Δβe）が検出されて姿勢方位推定手段１に入力される。姿勢方位推定手段１では前述したように、これらの値を用いて絶対方位参照値の候補が決定される。
【００６６】
このように、本実施の形態においては、衛星相対方向算出手段３から得られた衛星相対方向に、衛星相対方向探索手段５から得られた衛星相対方向を重畳してアンテナあるいはアンテナビームを駆動するため、移動体の機敏な運動にも対処でき、また角速度センサのドリフト誤差や加速度センサのバイアス誤差により衛星相対方向算出手段３から得られる衛星相対方向に誤差が生じても、衛星見通し時である限り衛星相対方向探索手段５によりその誤差を補償することができ、長時間に渡り高精度にアンテナ方向を制御して衛星を追尾することができる。
【００６７】
また、このように姿勢方位推定手段１における姿勢推定フィルタにおいて、加速度センサ検出値から絶対姿勢参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタフィルタにおいて、磁気方位センサ検出値、ＧＰＳ受信値、およびアンテナ駆動装置４から入力された方位角および仰角から絶対方位参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができるとともに、方位軸に関して比較的安価な角速度センサを利用することができる。
【００６８】
これより衛星相対方向算出手段３から得られる衛星相対方向の推定精度を高めることができる。また障害物等により衛星の見通しが不可な場合においてもアンテナあるいはアンテナビームを高精度に衛星方向に指向させておくことができるので、見通し状態に復帰した場合に迅速に衛星追尾を再開することができる。
【００６９】
実施の形態２．
図６は、この発明を実施するための実施の形態２による衛星追尾用アンテナ制御装置を説明するための図である。図において、３２は本実施の形態における衛星相対方向探索手段であり、衛星相対方向探索手段３２内には、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索部３２ａと、探索された当該衛星相対方向を衛星相対方向算出手段３より得られた衛星相対方向に重畳するための加算器３２ｂ，３２ｃが設けられている。
【００７０】
実施の形態１においてアンテナ駆動装置４は、衛星相対方向算出手段３で得られた衛星相対方向に衛星相対方向探索手段５で得られた衛星相対方向を重畳してアンテナあるいはアンテナビームを駆動したが、これは下記のようにしても同様の効果を実現することができる。
【００７１】
アンテナ駆動装置４は衛星相対方向算出手段３より得られた方位角および仰角方向βa，βeにアンテナあるいはアンテナビームを駆動する。また、衛星相対方向探索手段３２では、衛星相対方向算出手段３より得られた衛星相対方向を中心として衛星相対方向が探索され、その際の方位角および仰角増分信号Δβa、Δβeを衛星相対方向算出手段３より得られた方位角および仰角βa，βeに重畳し、姿勢方位推定手段１へ入力する。姿勢方位推定手段１ではこれを用いて実施の形態１の場合と同様にして絶対方位参照値の候補が推定され、絶対方位参照値を決定し帰還系が構成される。
【００７２】
このように、本実施の形態においては、衛星相対方向探索手段３２において、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、それを衛星相対方向算出手段３より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を姿勢方位推定手段１へ入力するとともに、姿勢方位推定手段１ではその値を用いて絶対方位参照値を推定し帰還系を構成したため、移動体の機敏な運動にも対処でき、また衛星見通し時である限り、角速度センサのドリフト誤差や加速度センサのバイアス誤差による衛星相対方向算出手段３における衛星相対方向推定誤差の発生を補償することができ、長時間に渡り高精度にアンテナ方向を制御して衛星を追尾することができる。
【００７３】
このように姿勢方位推定手段１における姿勢推定フィルタにおいて、加速度センサ検出値から絶対姿勢参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタフィルタにおいて、磁気方位センサ検出値、ＧＰＳ受信値、および、衛星相対方向探索手段３２から入力された方位角および仰角から絶対方位参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができるとともに、方位軸に関して比較的安価な角速度センサを利用することができる。
【００７４】
これより衛星相対方向算出手段３から得られる衛星相対方向の推定精度を高めることができる。また障害物等により衛星の見通しが不可な場合においてもアンテナあるいはアンテナビームを高精度に衛星方向に指向させておくことができるので、見通し状態に復帰した場合に迅速に衛星追尾を再開することができる。
【００７５】
実施の形態３．
上述の実施の形態１および実施の形態２における姿勢方位推定手段１において、アンテナ駆動装置４あるいは衛星相対方向探索手段３２から得られた移動体に対するアンテナあるいはアンテナビームの方位角および仰角から、移動体の絶対方位を推定する手法は以下のようにしてもよい。姿勢方位推定手段１で推定した移動体の絶対姿勢φ、θおよび絶対方位ψ、および、衛星見通し時におけるアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットを用いると、衛星指向方向単位ベクトルに着目して、衛星絶対方向（方位角αaハットと仰角αeハット）を次式から推定することができる。
【００７６】
【数２０】
【００７７】
この第３式から追尾する衛星の仰角推定値αeハット、また第１、２式から方位角推定値αaハットを得る。ここで求めた方位角推定値αaハットと衛星絶対方向算出手段２で算出した実際の衛星の方位角αa、および現時点の姿勢方位推定手段１における絶対方位推定値ψを用いて、移動体の絶対方位参照値の候補が次のように決定される。
【００７８】
【数２１】
【００７９】
なお、本実施の形態においても、上述の実施の形態１及び実施の形態２と同様の効果を得ることができる。
【００８０】
実施の形態４．
上述の実施の形態１および実施の形態２における姿勢方位推定手段１において、姿勢推定フィルタにおける絶対姿勢参照値は３軸加速度計１４を用いて算出したが、これはこの限りではない。例えば移動体のロール軸とピッチ軸のみに配置した２軸加速度計を用いても同様なことが行える。この場合には加速度ベクトルの大きさが重力加速度と同じであると仮定し、ヨー軸方向加速度はその重力加速度からロール軸方向加速度、ピッチ軸方向加速度を除いた成分であるとする。
【００８１】
本実施の形態においても、上述の実施の形態１及び実施の形態２と同様の効果を得ることができる。
【００８２】
また、姿勢推定精度は低下するが、３軸加速度計１４の代わりにロール角、ピッチ角が検出可能な液面傾斜計を備え、その検出値を絶対姿勢参照値としてもよい。この場合には加速度計に比べて検出可能な加速度の帯域は劣るが、姿勢方位推定手段１の低価格化を図ることができる。
【００８３】
実施の形態５．
図７は、この発明を実施するための実施の形態５による衛星追尾用アンテナ制御装置を説明するための部分構成図である。図７は、衛星追尾用アンテナ制御装置における姿勢方位推定手段１の、姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。ドリフト誤差推定部２８において、回転系帰還入力算出部１３から算出された入力量ｕrを用いてジャイロ９の持つドリフト誤差を推定する。ジャイロ９により検出される角速度信号に含まれる誤差として、加法的なドリフト誤差を考慮すると、ドリフト誤差は次のように推定される。
【００８４】
【数２２】
【００８５】
絶対姿勢参照値算出部１６および絶対方位参照値算出部１７において信頼性のある絶対姿勢参照値および絶対方位参照値が得られ、それを用いて帰還入力ｕrが計算された場合には、上式によってドリフト誤差を計算して、その推定値を時々刻々更新する。
【００８６】
回転系帰還入力算出部１３では、移動体の並進速度推定値、並進加速度推定値、ＧＰＳ受信時、非受信時、および、衛星見通し時、非見通し時の場合に応じて、帰還入力が切断される。先ず姿勢推定フィルタについて説明する。絶対姿勢参照値算出部１６において算出される絶対姿勢参照値は、前述したように移動体の並進運動方程式を用いて推定した重力ベクトルの３成分に基づいて導出されている。ロール角参照値φ(ref)は重力ベクトルのｙb軸成分とｚb軸成分の２成分から導出され、ピッチ角参照値θ(ref)は重力ベクトルのｘb軸成分とｚb軸成分の２成分から導出されるため、例えば旋回に伴って移動体のｙb軸成分の運動加速度が大きい場合や、加減速に伴ってｘb軸成分の運動加速度が大きい場合には、算出した絶対姿勢参照値の信頼性が低下する可能性がある。そこで位置速度推定フィルタにおける並進加速度推定部２１において推定した移動体の並進加速度ｖbe lドットを用いてその信頼性を判定する。先ず推定した並進加速度を移動体固定座標系[ａb]に変換する。
【００８７】
【数２３】
【００８８】
ここで推定した並進加速度ｖbe bドットのｙb軸成分とｚb軸成分が大きい場合には、求めたロール角参照値φ(ref)の信頼性は低いものとし、姿勢推定フィルタにおけるロール角推定部における帰還入力を切断する。また推定したｖbe bドットのｘb軸成分とｚb軸成分が大きい場合には、求めたピッチ角参照値θ(ref)の信頼性は低いものとし、姿勢推定フィルタにおけるピッチ角推定部における帰還入力を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、図８に示すように現時点でのジャイロドリフト誤差推定値ｄgを用いて対応する入力ｕrを計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【００８９】
方位推定フィルタについても同様である。絶対方位参照値算出部１７では、磁気コンパス１８から検出された絶対方位、ＧＰＳ受信機１９から検出した移動体の進行方向方位、衛星見通し時においてアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットから推定した絶対方位を候補として、その３通りの候補の中から適切なものを選択して絶対方位参照値とする。信頼性のある絶対方位参照値が得られなかった場合には、方位推定フィルタにおける帰還入力を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、図８に示すように現時点でのジャイロドリフト誤差推定値ｄgを用いて対応する入力を計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【００９０】
また図９は、衛星追尾用アンテナ制御装置における姿勢方位推定手段１の、位置速度推定フィルタにおける信号処理の概略を示すブロック図である。バイアス誤差推定部２９において、並進系帰還入力算出部２７から算出された入力量ｕtを用いて加速度計１４の持つバイアス誤差を推定する。加速度計１４により検出される加速度信号に含まれる誤差として、加法的なバイアス誤差を考慮すると、バイアス誤差は次のように推定される。
【００９１】
【数２４】
【００９２】
ＧＰＳ測位時においては、絶対位置参照値算出部２５および絶対速度参照値算出部２６において絶対位置参照値および絶対速度参照値が得られ、それを用いて帰還入力ｕtが計算された場合には、上式によってバイアス誤差を計算して、その推定値を時々刻々更新する。
【００９３】
ＧＰＳが未測位の場合には位置速度推定フィルタにおける帰還入力を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、図１０に示すように現時点での加速度計バイアス誤差推定値ｂaを用いて対応する入力ｕtを計算し、加速度計１４の持つバイアス誤差の影響を補償する。
【００９４】
このように本実施の形態においては、移動体の運動や受信状況に応じて、姿勢推定フィルタにおける帰還系および方位推定フィルタにおける帰還系を各姿勢方位軸に関して独立に切断することにより、移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合や、あるいは移動体静止時においてＧＰＳから絶対方位参照値が高精度に得られない場合等において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散、あるいは方位推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【００９５】
実施の形態６．
図１１及び図１２は、この発明を実施するための実施の形態６による衛星追尾用アンテナ制御装置を説明するための図であり、特に、図１１は、衛星追尾用アンテナ制御装置における姿勢方位推定手段１の、姿勢方位推定フィルタにおける信号処理の概略を示すブロック図、また図１２は位置速度推定フィルタにおける信号処理の概略を示すブロック図である。これらの図において、３０及び３１は低域通過フィルタであり、低域通過フィルタ３０は、３軸ジャイロ９が有するドリフト特性に対応した低域の所定の値をしきい値とし、それよりも高い周波数の高域成分を除去するためのものである。同様に、低域通過フィルタ３１は３軸加速度計１４が有するバイアス特性に対応した低域の所定の値をしきい値とし、それよりも高い周波数の高域成分を除去するためのものである。
【００９６】
上述の実施の形態５において、ジャイロ９の持つドリフト誤差推定値および加速度計１４の持つバイアス誤差推定値は、回転系帰還入力ｕrおよび並進系帰還入力ｕtから算出し、帰還入力切断時においては、それらを用いて対応する補償入力を計算して、ドリフト誤差およびバイアス誤差の影響を補償した。しかし回転系帰還入力ｕrおよび並進系帰還入力ｕtには、一般に移動体の運動に伴う高域成分（高周波成分）が含まれており、それを用いてドリフト誤差およびバイアス誤差を推定すると、推定値には本来ジャイロや加速度計が有していない高域成分が含まれる場合があり、推定精度を高く保てない可能性がある。したがってこの推定値を用いて補償入力を計算した場合には、ドリフト誤差およびバイアス誤差の影響を高精度に補償できない可能性がある。そこで、本実施の形態においては、搭載したジャイロ９や加速度系１４の持つドリフト特性およびバイアス特性をあらかじめ調べておき、図１１および図１２に示すように、ドリフト誤差推定部２８で推定した推定値およびバイアス誤差推定部２９で推定した推定値に対して、それぞれの特性に応じた低域通過フィルタ３０、３１を時々刻々適用し、ドリフト推定値およびバイアス推定値の推定精度を高める。帰還入力切断時には、これらの高域成分が除去されたドリフト誤差推定値およびバイアス誤差推定値を用いて対応する補償入力を計算し、ドリフト誤差およびバイアス誤差の影響を補償する。
【００９７】
このように、本実施の形態においては、３軸ジャイロ９のドリフト特性に応じた低域通過フィルタ３０を適用することにより、帰還入力から推定した角速度センサドリフト誤差に含まれる移動体の機敏な運動が要因の高域成分を除去することができ、角速度センサが持つドリフト誤差をより高精度に推定することができる。またそれを用いて補償入力を算出することにより、帰還入力切断時においても高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【００９８】
また同様に、本実施の形態においては、３軸加速度計１４のバイアス特性に応じた低域通過フィルタ３１を適用することにより、帰還入力から推定したバイアス誤差に含まれる移動体の機敏な運動が要因の高域成分を除去することができ、バイアス誤差をより高精度に推定することができる。またそれを用いて姿勢方位推定フィルタにおける補償入力を算出することにより、帰還入力切断時においても高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【００９９】
実施の形態７．
実施の形態１あるいは実施の形態２で説明したように、姿勢方位推定手段１における絶対方位推定精度を高めるためには、高精度な絶対方位参照値が必要である。衛星指向方向単位ベクトルに着目すると、衛星見通し時におけるアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハット、移動体の絶対姿勢（ロール角φとピッチ角θ）および絶対方位ψ、衛星の方位角αaおよび仰角αeの間には、以下の関係式が成立しなければならない。
【０１００】
【数２５】
【０１０１】
ここで、ｓr＝sinφ、ｃr＝cosφ、ｓp＝sinθ、ｃp＝cosθ、ｓh＝sinψ、ｃh＝cosψとする。アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットはアンテナ駆動装置４あるいは衛星相対方向探索手段３２から得ることができる。移動体の絶対姿勢および絶対方位は姿勢方位推定手段１から得ることができる。また衛星の方位角αaおよび仰角αeに関しては衛星絶対方向算出手段２より得ることができる。
【０１０２】
上式はこれらの諸量の間で成立しなければならない拘束条件式であるが、実際には満足されず、その主な要因は姿勢方位推定手段１における絶対姿勢推定誤差および絶対方位推定誤差の存在にある。そこで姿勢推定フィルタで得られた現時点での絶対姿勢推定値を初期値として、上記拘束条件式を満足するように絶対姿勢推定値および絶対方位推定値を逐次的に更新し、絶対方位参照値の候補を得る手法を説明する。
【０１０３】
先ず上式を絶対方位に関する方程式と見なせば、第１、２式を変形して
【０１０４】
【数２６】
【０１０５】
次に絶対姿勢に関する方程式と見なせば、第１、２式の左辺からψを消去し、第３式と並べて
【０１０６】
【数２７】
【０１０７】
これら２つの関係式を用いて、次のように絶対姿勢および絶対方位を逐次的に更新し、最終的に得られた絶対方位を絶対方位参照値の候補とする。
【０１０８】
先ず姿勢方位推定フィルタにおける現時点の絶対姿勢推定値（ロール角φとピッチ角θ）をそれぞれφ(i)、θ(i)とする。ｉ＝０にセットする（ステップＳ２１）。
【０１０９】
次にロール角推定値φ(i)およびピッチ角推定値θ(i)を用いて、上記絶対方位に関する方程式をsinψ、cosψに関して解き、これから絶対方位推定値ψを得る。得られた絶対方位推定値をψ (i)とする（ステップＳ２２）。
【０１１０】
次にロール角φおよびピッチ角θを１次の微小項まで次のように展開する。
【０１１１】
【数２８】
【０１１２】
これを上記絶対姿勢に関する方程式に代入して整理すると、未知数Δφ、Δθに関する連立一次方程式を得る。これを解いてΔφ、Δθを得、それを用いて更新したロール角推定値φ(i+1)およびピッチ角推定値θ(i+1)を得る。ｉ←ｉ＋１としてステップＳ２２へ戻る（ステップＳ２３）。
【０１１３】
以下、ステップＳ２２で得られる絶対方位推定値が収束するまで上記手続きを繰り返し、最終的に得られた絶対方位推定値を絶対方位参照値算出部１７における絶対方位参照値の候補として採用する（ステップＳ２４）。
【０１１４】
このように衛星見通し時において前記アンテナ駆動装置４あるいは衛星相対方向探索手段３２から入力された方位角および仰角を用いて、移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値の候補とすることにより、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値とすることにより姿勢方位推定フィルタの絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１１５】
実施の形態８．
上述の実施の形態１あるいは実施の形態２で説明したように、姿勢方位推定手段１における絶対方位推定精度を高めるためには、高精度な絶対方位参照値が必要である。衛星指向方向単位ベクトルに着目すると、衛星見通し時におけるアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハット、移動体の絶対姿勢（ロール角φとピッチ角θ）および絶対方位ψ、衛星の方位角αaおよび仰角αeの間には、以下の関係式が成立しなければならない。
【０１１６】
【数２９】
【０１１７】
ここでｓr＝sinφ、ｃr＝cosφ、ｓp＝sinθ、ｃp＝cosθ、ｓh＝sinψ、ｃh＝cosψとする。アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットはアンテナ駆動装置４あるいは衛星相対方向探索手段３２から得ることができる。絶対姿勢および絶対方位は姿勢方位推定手段１から得ることができる。また衛星の方位角αaおよび仰角αeに関しては衛星絶対方向算出手段２より得ることができる。
【０１１８】
上式はこれらの諸量の間で成立しなければならない拘束条件式であるが、実際には満足されず、その主な要因は姿勢方位推定手段１における絶対姿勢推定誤差および絶対方位推定誤差の存在にある。そこで姿勢推定フィルタで得られた現時点での絶対姿勢推定値を初期値として、上記拘束条件式を満足するような絶対姿勢推定値および絶対方位推定値を得る手法を説明する。
【０１１９】
上式の第３式に着目すると、これは移動体の絶対方位ψには関係なく、絶対姿勢のロール角φおよびピッチ角θのみが満たすべき拘束条件となっている。この第３式を書き直すと
【０１２０】
【数３０】
【０１２１】
ここで
【０１２２】
【数３１】
【０１２３】
ｅaは鉛直下方向単位ベクトル（移動体固定座標系[ab]表現）、ｅβは衛星指向軸方向単位ベクトル（移動体固定座標系[ab]表現）を表している。ｅaおよびｅβが単位ベクトルであることに注意すると、上式は２つのベクトルｅaとｅβの内積演算を定義した拘束条件式であり、図１３に示すようにベクトルeaはベクトルeβを中心軸として角度（π／２＋αe）だけ隔てた円錐面上に分布すべき諸量であることが分かる。しかし姿勢方位推定フィルタによって推定したロール角φおよびピッチ角θには推定誤差が含まれるため、この推定値を用いて生成したベクトルｅaハットは、実際にはこの円錐面上に存在していない。そこで図１３に示すようにベクトルｅβとベクトルｅaハットにより張られる平面γを考え、この平面γと上記円錐面との交線をベクトルｅaの候補値ｅa *として採用する。すなわちベクトルｅaハットに最小二乗近似の意味で最も近い円錐面上のベクトルをベクトルｅaの候補値ｅa *として採用する。
【０１２４】
平面γ上でベクトルｅβに垂直な単位ベクトルは
【０１２５】
【数３２】
【０１２６】
したがってベクトルｅaの候補値ｅa *は次式で与えられる。
【０１２７】
【数３３】
【０１２８】
これより先ずロール角、ピッチ角の候補値が、ベクトルｅa *の３成分を用いて次のように求められる。
【０１２９】
【数３４】
【０１３０】
ここで求めたロール角φ*とピッチ角θ*を用いて、実施の形態７で述べた絶対方位に関する方程式を解くことによって、絶対方位推定値を得ることができ、これを絶対方位参照値の候補とする。
【０１３１】
このように、本実施の形態においては、最小二乗近似の意味で先ず最も確からしい移動体の絶対姿勢を推定し、次にそれを用いて絶対方位を推定したことにより、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値の候補とすることにより方位推定フィルタの絶対方位推定精度を高めることができる。また絶対方位参照値を得るための計算負荷も小さくなる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１３２】
実施の形態９．
図１４は、この発明を実施するための実施の形態９による衛星追尾用アンテナ制御装置を説明するための図である。図１４は特に姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。実施の形態７および実施の形態８において、アンテナ駆動装置４で検出したアンテナあるいはアンテナビームの方位角および仰角を用いて、絶対方位参照値算出部１７で絶対姿勢および絶対方位推定値を得、その絶対方位推定値を絶対方位参照値の候補として用いたが、そこでの絶対姿勢推定値は回転系帰還入力算出部１３における姿勢推定フィルタの帰還信号の構成に用いることができる。
【０１３３】
絶対姿勢参照値算出部１６では、３軸加速度計１４の信号を用いて絶対姿勢参照値が算出される。ただし実施の形態５で説明したように、その精度は移動体の並進運動加速度に大きく依存しているため、実施の形態５では移動体の並進運動加速度を推定し、その値に応じて姿勢推定フィルタの帰還系を切断した。
【０１３４】
しかるにアンテナ駆動装置４で検出したアンテナあるいはアンテナビームの方位角および仰角を用いて、絶対方位参照値算出部１７で推定された絶対姿勢推定値は、移動体の並進運動加速度には依存しないため、この推定値を絶対姿勢参照値算出部１６に入力し、回転系帰還入力算出部１３における姿勢推定フィルタの帰還系切断の判定に利用することができる。例えば３軸加速度計１４の信号を用いて算出された絶対姿勢参照値と、絶対方位参照値算出部１７において推定された絶対姿勢推定値を一定時間の間比較し、その間の偏差が大きい場合には、３軸加速度計１４の信号を用いて算出された絶対姿勢参照値の信頼性は低いものとし、姿勢推定フィルタにおける対応する姿勢軸の帰還系を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、現時点でのジャイロドリフト誤差推定値を用いて対応する入力ｕrを計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【０１３５】
このように、本実施の形態においては、移動体の並進運動加速度には依存しない絶対姿勢推定値を用いて姿勢推定フィルタにおける帰還系を各姿勢方位軸に関して独立に切断することにより、加速度センサを用いても移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１３６】
実施の形態１０．
図１５は、この発明を実施するための実施の形態１０による衛星追尾用アンテナ制御装置を説明するための図である。図１５は特に姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。実施の形態７および実施の形態８において、衛星相対方向探索手段３２より得られた方位角および仰角を用いて絶対姿勢および絶対方位推定値を算出し、その絶対方位推定値を絶対方位参照値の候補として用いたが、実施の形態９で述べたのと同様に、そこでの絶対姿勢推定値は回転系帰還入力算出部１３における帰還信号の構成に用いることができる。
【０１３７】
絶対姿勢参照値算出部１６では、３軸加速度計１４の信号を用いて絶対姿勢参照値が算出される。ただし実施の形態５で説明したように、その精度は移動体の並進運動加速度に大きく依存しているため、実施の形態５では移動体の並進運動加速度を推定し、その値に応じて姿勢推定フィルタの帰還系を切断した。
【０１３８】
しかるに衛星相対方向探索手段３２より得られた方位角および仰角を用いて、絶対方位参照値算出部１７で推定された絶対姿勢推定値は、移動体の並進運動加速度には依存しないため、この推定値を絶対姿勢参照値算出部１６に入力し、回転系帰還入力算出部１３における姿勢推定フィルタの帰還系切断の判定に利用することができる。例えば３軸加速度計１４の信号を用いて算出された絶対姿勢参照値と、絶対方位参照値算出部１７において推定された絶対姿勢推定値を一定時間の間比較し、その間の偏差が大きい場合には、３軸加速度計１４の信号を用いて算出された絶対姿勢参照値の信頼性は低いものとし、姿勢推定フィルタにおける対応する姿勢軸の帰還系を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、現時点でのジャイロドリフト誤差推定値を用いて対応する入力ｕrを計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【０１３９】
このように、本実施の形態においては、移動体の並進運動加速度には依存しない絶対姿勢推定値を用いて姿勢推定フィルタにおける帰還系を各姿勢方位軸に関して独立に切断することにより、加速度センサを用いても移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１４０】
実施の形態１１．
実施の形態１あるいは実施の形態２で説明したように、姿勢方位推定手段１における絶対方位推定精度を高めるためには、高精度な絶対方位参照値が必要である。衛星見通し時である限り、アンテナあるいはアンテナビームの方位角βaハットと仰角βeハットから推定された絶対方位の精度は、周囲環境の磁気的外乱や移動体の運動に依存しないため、一般には磁気コンパス１８やＧＰＳ受信機１９に比べて精度の良い絶対方位参照値を与える。そこで初期衛星補足時、あるいは衛星非見通し時から見通し時への復帰時において、実施の形態１、実施の形態３、実施の形態７あるいは実施の形態８で記述した手法によって絶対方位推定値が得られた時点で、これを絶対方位参照値として姿勢方位推定手段１における方位推定フィルタを初期化する。
【０１４１】
これにより、本実施の形態においては、姿勢方位推定フィルタにおける絶対方位推定誤差を迅速に零に収束させることができ、以後の絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４２】
【発明の効果】
この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動するとともに、当該衛星相対方向に衛星相対方向探索手段から得られた衛星相対方向を重畳した方向にアンテナあるいはアンテナビームを駆動して衛星を指向し、その際の移動体に対するアンテナあるいはアンテナビームの方位角および仰角を前記姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信機の受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置であるので、移動体の機敏な運動にも対処でき、また角速度センサのドリフト誤差や加速度センサのバイアス誤差により衛星相対方向算出手段から得られる衛星相対方向に誤差が生じても、衛星見通し時である限り衛星相対方向探索手段によりその誤差を補償することができ、長時間に渡り高精度にアンテナを制御して衛星を追尾することができる。また、このように姿勢方位推定手段における姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信機の受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができる。また、これより衛星相対方向算出手段から得られる衛星相対方向の推定精度を高めることができる。
【０１４３】
また、この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動し、衛星相対方向探索手段は、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、その方向を衛星相対方向算出手段より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を上記姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置であるので、移動体の機敏な運動にも対処でき、また衛星見通し時である限り、角速度センサのドリフト誤差や加速度センサのバイアス誤差による衛星相対方向算出手段における衛星相対方向推定誤差の発生を補償することができ、長時間に渡り高精度にアンテナを制御して衛星を追尾することができる。また、姿勢方位推定手段における姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができる。また、これより衛星相対方向算出手段から得られる衛星相対方向の推定精度を高めることができる。
【０１４４】
また、姿勢方位推定手段において、姿勢推定フィルタおよび方位推定フィルタにおける帰還入力から姿勢、方位軸に関する上記角速度センサのドリフト誤差を推定するとともに、上記移動体の並進加速度推定値が大きい場合には、姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、上記移動体の並進速度推定値が小さい場合、あるいは、ＧＰＳ非受信時の場合において、衛星非見通し時の場合には、上記方位推定フィルタにおける帰還系を切断するとともに、切断時においてはこれまでの帰還入力から推定した角速度センサのドリフト誤差を入力として補償するので、移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合や、あるいは移動体静止時においてＧＰＳから絶対方位参照値が高精度に得られない場合等において、帰還系による姿勢方位推定精度の低下を防ぐことができる。また切断時にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散、あるいは方位推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４５】
また、姿勢方位推定手段において、帰還入力から推定した角速度センサのドリフト誤差に対して、各姿勢方位軸に備えた角速度センサのドリフト特性に応じた低域通過フィルタを適用するので、帰還入力から推定した角速度センサドリフト誤差に含まれる移動体の機敏な運動が要因の高域成分を除去することができ、角速度センサが持つドリフト誤差をより高精度に推定することができる。またそれを用いて姿勢方位推定フィルタにおける補償入力を算出することにより、帰還入力切断時においても高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４６】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値とするので、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値とすることにより姿勢方位推定フィルタの絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４７】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、先ず最小二乗近似の意味で移動体の絶対姿勢を推定し、次にその推定値を用いて絶対方位参照値を推定するので、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値の候補とすることにより姿勢方位推定フィルタの絶対方位推定精度を高めることができる。また絶対方位参照値を得るための計算負荷も小さくなる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４８】
また、逐次的に更新して推定した上記移動体の絶対姿勢、あるいは、最小二乗近似の意味で推定した上記移動体の絶対姿勢を参照して姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、切断時においてはこれまでの帰還入力から推定した上記角速度センサのドリフト誤差を入力として補償するので、加速度センサを用いても移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度を保持することができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４９】
また、初期衛星捕捉時、あるいは、衛星非見通し時から見通し時への復帰時において、姿勢方位推定手段における方位推定フィルタを、アンテナ駆動手段あるいは衛星相対方向探索手段より得られたアンテナあるいはアンテナビームの方位角および仰角を用いて推定した絶対方位推定値で初期化するので、方位推定フィルタにおける絶対方位推定誤差を迅速に零に収束させることができ、以後の絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態１による衛星追尾用アンテナ制御装置の構成を示したブロック図である。
【図２】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図３】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける最も信頼性の高い方位推定値の選択方法を示した流れ図である。
【図４】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける最も信頼性の高い方位推定値の選択方法の一例を示した流れ図である。
【図５】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおける信号処理の概略を示したブロック図である。
【図６】 本発明の実施の形態２による衛星追尾用アンテナ制御装置の構成を示したブロック図である。
【図７】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図８】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおけるドリフト誤差の補償方法を示したブロック図である。
【図９】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおける信号処理の概略を示したブロック図である。
【図１０】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおけるバイアス誤差の補償方法を示したブロック図である。
【図１１】 本発明の実施の形態６による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図１２】 本発明の実施の形態６による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおける信号処理の概略を示したブロック図である。
【図１３】 本発明の実施の形態８による衛星追尾用アンテナ制御装置において、鉛直下方向単位ベクトルｅaと衛星指向軸方向単位ベクトルｅβとの関係を示した説明図である。
【図１４】 本発明の実施の形態９による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図１５】 本発明の実施の形態１０による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図１６】 従来の衛星追尾装置の構成を示したブロック図である。
【符号の説明】
１ 姿勢方位推定手段、２ 衛星絶対方向算出手段、３ 衛星相対方向算出手段、４ アンテナ駆動装置、５，３２ 衛星相対方向探索手段、６ 衛星追尾用アンテナ、７ 移動体のアンテナ取り付け面、８ 衛星、９ ３軸ジャイロ、１０ 角速度算出部、１１ 姿勢方位角速度推定部、１２ 積分演算部、１３ 回転系帰還入力算出部、１４ ３軸加速度計、１５ 座標変換部、１６ 絶対姿勢参照値算出部、１７ 絶対方位参照値算出部、１８ 磁気コンパス、１９ ＧＰＳ受信機、２０ 座標変換部、２１ 並進加速度推定部、２２ 積分演算部、２３ 測地座標速度推定部、２４ 積分演算部、２５ 絶対位置参照値算出部、２６ 絶対速度参照値算出部、２７ 並進系帰還入力算出部、２８ ドリフト誤差推定部、２９ バイアス誤差推定部、３０，３１ 低域通過フィルタ。
【発明の属する技術分野】
この発明は衛星追尾用アンテナ制御装置に関し、特に、車両や船舶、航空機等の移動体に搭載可能で、通信衛星との通信を行うための衛星追尾用アンテナ制御装置に関する。
【０００２】
【従来の技術】
通信衛星等の衛星と車両、船舶または航空機等の地上の移動体との衛星通信を行う際には、無指向性アンテナを用いることも多いが、画像データ等のように大量の情報を伝送する場合等には、送信電力や受信分解能の限界から、鋭い指向性を有するアンテナを用いることが望ましい。この場合には、常にアンテナビームを衛星の方向に向けておく必要があり、これを追尾と一般に呼んでいる。図１６は、例えば特開平１０−４８３０５号公報に示された従来の車載用通信衛星追尾装置を示すブロック図である。図において、１０１は、ｘ加速度、ｙ加速度、ｚ加速度の検出を行う３軸加速度センサ、１０２は、ｘ角速度、ｙ角速度、ｚ角速度の検出を行う３軸角速度センサ（レートジャイロ）、１０３はｘ加速度およびｚ加速度の値を用いてピッチ角を生成するピッチ角生成部、１０７はｙ加速度およびｚ加速度の値を用いてロール角を生成するロール角生成部、１０４は緯度、経度、高度、方位の検出を行うＧＰＳ装置（ＧＰＳ：Global Positioning System（全世界測位システム））、１０５は車輪回転計から車輪速の検出を行う車輪速センサ、１０６はアンテナの方位角（アジマス）および仰角（エレベーション）の制御を行うアンテナ方向制御装置である。
【０００３】
動作について説明する。３軸角速度センサ１０２の信号を移動体の回転運動検出に用いるとともに、ピッチ角生成部１０３およびロール角生成部１０７で生成した角度信号を姿勢の基準信号として、また、ＧＰＳ装置１０４で検出した方位信号を方位の基準信号として用いて、移動体の絶対姿勢および絶対方位を推定し、それを用いてアンテナ方向を制御する。
【０００４】
【発明が解決しようとする課題】
上述したように、従来の通信衛星追尾装置においては、３軸角速度センサ１０２の信号や３軸加速度センサ１０１の信号を用いて、移動体の絶対姿勢及び絶対方位を推定しているため、３軸角速度センサ１０２や３軸加速度センサ１０１の持つドリフト誤差やバイアス誤差に起因して、移動体の絶対姿勢及び絶対方位の推定誤差が発生してしまうため、高精度にアンテナ方向を制御することができないという問題点があった。
【０００５】
この発明は、かかる問題点を解決するためになされたものであり、３軸角速度センサや３軸加速度センサの持つドリフト誤差やバイアス誤差による移動体の絶対姿勢および絶対方位推定誤差を補償して、高精度にアンテナ方向の制御を行って衛星を追尾するための衛星追尾用アンテナ制御装置を得ることを目的とする。
【０００６】
【課題を解決するための手段】
この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから上記衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動するとともに、当該衛星相対方向に衛星相対方向探索手段から得られた衛星相対方向を重畳した方向にアンテナあるいはアンテナビームを駆動して衛星を指向し、その際の移動体に対するアンテナあるいはアンテナビームの方位角および仰角を前記姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信機の受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置である。
【０００７】
また、この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動し、衛星相対方向探索手段は、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、その方向を衛星相対方向算出手段より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、上記方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置である。
【０００８】
また、姿勢方位推定手段において、姿勢推定フィルタおよび方位推定フィルタにおける帰還入力から姿勢、方位軸に関する上記角速度センサのドリフト誤差を推定するとともに、移動体の並進加速度推定値が大きい場合には、姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、上記移動体の並進速度推定値が小さい場合、あるいは、ＧＰＳ非受信時の場合において、衛星非見通し時の場合には、上記方位推定フィルタにおける帰還系を切断するとともに、切断時においてはこれまでの帰還入力から推定した角速度センサのドリフト誤差を入力として補償する。
【０００９】
また、姿勢方位推定手段において、帰還入力から推定した角速度センサのドリフト誤差に対して、各姿勢方位軸に備えた角速度センサのドリフト特性に応じた低域通過フィルタを適用する。
【００１０】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値とする。
【００１１】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、先ず最小二乗近似の意味で移動体の絶対姿勢を推定し、次にその推定値を用いて絶対方位参照値を推定する。
【００１２】
また、逐次的に更新して推定した上記移動体の絶対姿勢、あるいは、最小二乗近似の意味で推定した上記移動体の絶対姿勢を参照して姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、切断時においてはこれまでの帰還入力から推定した上記角速度センサのドリフト誤差を入力として補償する。
【００１３】
また、初期衛星捕捉時、あるいは、衛星非見通し時から見通し時への復帰時において、姿勢方位推定手段における方位推定フィルタを、アンテナ駆動手段あるいは衛星相対方向探索手段より得られたアンテナあるいはアンテナビームの方位角および仰角を用いて推定した絶対方位推定値で初期化する。
【００１４】
【発明の実施の形態】
実施の形態１．
図１は、この発明の実施の形態１による衛星追尾用アンテナ制御装置を説明するための図である。図１において、１は移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段、２は移動体の絶対位置と追尾する衛星の絶対位置から衛星の方位角および仰角を算出する衛星絶対方向算出手段、３は移動体の絶対姿勢および絶対方位、衛星の方位角および仰角から移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段、４は移動体に対する方位角および仰角方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動装置、５は衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段、６は衛星追尾用アンテナ、７は移動体のアンテナ取り付け面、８は追尾する衛星である。
【００１５】
この衛星追尾用アンテナ制御装置における動作の説明のために以下の座標系を導入する。
[a0]を慣性座標系とする。これは慣性空間に固定された座標系で、原点は地球中心にある。ただし地球の公転の影響は無視する。
[ae]を地球固定座標系とする。これは地球に固定された座標系で、原点は地球中心にある。xe 、ye軸は赤道面内にあり、xe 軸はグリニッジ子午線(経度零)を通る。またze軸は北極方向を向いている。
[al]を局所水平面座標系とする。これは機体の質量中心に原点を持ち、地球上の局所水平面（接平面）上で規定された座標系である。xl 、yl 、zl軸はそれぞれ地理上の北、東、接平面垂直下方向を向いている。
[ab]を移動体固定座標系とする。これは移動体の質量中心に原点を持ち、移動体に固定された座標系である。xb 、yb 、zb軸がそれぞれ移動体のロール軸、ピッチ軸、ヨー軸に対応する。
[ag]をジャイロ検出軸座標系とする。これは姿勢方位推定手段１に固定された座標系である。xg 、yg 、zg軸がそれぞれ、姿勢方位推定手段１に搭載されたジャイロ検出軸方向と一致する。
[aa]を加速度計検出軸座標系とする。これは姿勢方位推定手段１に固定された座標系である。xa 、ya 、za軸がそれぞれ、姿勢方位推定手段１に搭載された加速度計検出軸方向と一致する。
【００１６】
さらに以下の記号を定義する。
Ａijを座標系[aj]から座標系[ai]への座標変換行列とする。ωijを座標系[aj]に対する座標系[ai]の角速度とする。移動体の絶対位置を表す緯度、経度、高度をそれぞれ
【数１】
【００１７】
衛星絶対方向算出手段２では、追尾する衛星の絶対位置とＧＰＳ受信機（図２の符号１９参照）で受信した移動体の絶対位置から、衛星の方位角および仰角を算出する。地球中心から移動体までの距離ベクトルrb、および地球中心から衛星までの距離ベクトルrsはそれぞれ、Rを地球半径として次式で与えられる。
【００１８】
【数２】
【数３】
ここで右肩添字ｅは表現されている座標系を表す。また下付き添字ｓは追尾する衛星に関する諸量を表すものとする。この時、移動体から衛星までの距離ベクトルは、局所水平面座標系 [al] で次のように表される。
【００２１】
【数４】
これより衛星の方位角αaと仰角αeがｒsb lの３成分を用いて次のように算出される。
【００２３】
【数５】
姿勢方位推定手段１における信号処理は、移動体の回転運動に関する姿勢方位推定フィルタ、および移動体の並進運動に関する位置速度推定フィルタに分類される。
【００２５】
先ず姿勢方位推定フィルタについて説明する。図２は姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。図２において、９は３軸ジャイロ、１０は角速度算出部、１１は姿勢方位角速度推定部、１２は積分演算部、１３は回転系帰還入力算出部、１４は３軸加速度計、１５は座標変換部、１６は絶対姿勢参照値算出部、１７は絶対方位参照値算出部、１８は磁気コンパス、１９はＧＰＳ受信機、４は図１に示したアンテナ駆動装置である。
【００２６】
図２における角速度算出部１０において、３軸ジャイロ９によって検出された角速度ωg = [ωxg ,ωyg ,ωzg]Tを用いて局所水平面座標系[al]に対する移動体固定座標系[ab]の角速度を算出する。なお、ここでは、簡単のため地球の自転の影響は無視した。
【００２７】
【数６】
次に姿勢方位角速度推定部１１において、移動体の回転運動に関する運動方程式から移動体の絶対姿勢角速度(ロール角速度とピッチ角速度)および絶対方位角速度を推定する。すなわち
【００２９】
【数７】
また行列Ｓは現時点の絶対姿勢推定値φ、θから構成される下記の３×３行列である。
【００３１】
【数８】
またurは回転系帰還入力算出部１３から算出された入力量で、この詳細については後述する。
【００３３】
次に積分演算部１２において、推定した絶対姿勢角速度および絶対方位角速度を積分することにより、移動体の絶対姿勢推定値(ロール角推定値とピッチ角推定値)および絶対方位推定値を得る。
【００３４】
一方、座標変換部１５において、３軸加速度計１４によって検出された加速度fa = [fxa ,fya ,fza]Tを移動体固定座標系[ab]で表現する。
【００３５】
【数９】
絶対姿勢参照値算出部１６では、座標変換部１５において算出した加速度を用いて移動体固定座標系[ab]での重力ベクトルを推定し、その方向から移動体の絶対姿勢参照値を推定する。重力ベクトルは、移動体の並進運動に関する運動方程式から次のように求められる。
【００３７】
【数１０】
ここで×は外積演算を表す。移動体の並進運動加速度ｖbe lドットに関しては、それが小さいものと仮定して零と置くか、あるいは、ＧＰＳ受信機１９より検出した絶対位置を数値的に２階微分して算出する。また並進速度ｖbe lに関しては、後述する位置速度推定フィルタより得られた推定値を用いる。こうして求めた移動体固定座標系[ab]での重力ベクトルgbの３成分を用いて、絶対姿勢参照値が次のように求められる。
【００３９】
【数１１】
絶対方位参照値算出部１７では、複数のセンサから絶対方位参照値の候補を得、それらの中から適切な絶対方位参照値を選択する。先ず移動体に搭載した磁気コンパス１８から、移動体の絶対方位が検出される。ただしこの検出値の信頼性は、移動体の周囲環境の磁気的外乱に大きく左右される。また移動体に搭載したＧＰＳ受信機１９から、移動体の進行方向方位が検出される。ただしこの検出値が利用可能な場合は、ＧＰＳが測位時で、かつ移動体が並進運動している場合に限られる。またその信頼性は主に移動体の並進速度に依存している。あるいはもしＧＰＳ受信機１９が干渉測位可能な受信機である場合には、これより移動体の絶対方位が検出される。最後の絶対方位の候補は、衛星見通し時にアンテナ駆動装置４から得られた移動体に対するアンテナあるいはアンテナビームの方位角および仰角を用いて推定される。その手法を以下に述べる。
【００４１】
衛星絶対方向算出手段２より得られた衛星の方位角αaおよび仰角αe、衛星見通し時における移動体に対するアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハット、移動体の絶対姿勢(ロール角φとピッチ角θ) の間には次の関係式が成立している。
【００４２】
【数１２】
ここで絶対姿勢に関して姿勢推定フィルタにおける現時点での推定値を利用すれば、上式は未知数である絶対方位ψに関する方程式となる、これを解くことによって絶対方位に関する正弦sinψと余弦cosψが得られる。これから絶対方位参照値が次のように推定される。
【００４４】
【数１３】
ただしこの推定値が利用可能な場合は、衛星が見通し可能な場合に限られる。
【００４６】
このように搭載したセンサあるいは装置から３通りの絶対方位参照値の候補を得、現時点で移動体が直面している状況に応じて最も信頼性の高い推定値を選択して絶対方位参照値とする。図３はその選択方法の一例を示す図である。磁気コンパス１８は常時絶対方位を検出可能であるが、その精度は周囲環境の磁気的外乱に大きく左右されるため、磁気的外乱には影響されないＧＰＳ受信機１９で検出した進行方向方位を用いてその信頼性を判定する。先ずＧＰＳ受信機１９が未測位あるいは検出した速度があらかじめ決定した閾値未満の場合には（ステップＳ１）、検出した進行方向方位の信頼性は乏しい。その場合において、衛星が見通し時の場合は（ステップＳ２）、アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットから求めた絶対方位推定値を絶対方位参照値とする（ステップＳ３）。衛星が見通し不可の場合は（ステップＳ２）、絶対方位参照値が得られなかったものとして、後述する回転系帰還入力urの方位軸に関する成分を零とする（ステップＳ４）。ＧＰＳ測位時において検出した速度が閾値以上の場合には（ステップＳ１）、ＧＰＳ受信機１９で検出した進行方向方位と磁気コンパス１８で検出した絶対方位を一定時間の間比較し、両者の間の偏差があらかじめ決定した閾値未満なら（ステップＳ５）、磁気コンパス１８で検出した絶対方位は信頼できるものとし、それを絶対方位参照値とする（ステップＳ６）。それ以外の場合には（ステップＳ５）、信頼性に乏しいものとし、ＧＰＳ受信機１９で検出した進行方向方位を絶対方位参照値とする（ステップＳ７）。
あるいは図４に示すように選択してもよい。先ず衛星が見通し時の場合は（ステップＳ８）、アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットから求めた絶対方位推定値を絶対方位参照値とする（ステップＳ９）。次に衛星が見通し不可の場合において（ステップＳ８）、ＧＰＳ受信機１９が未測位あるいは検出した速度があらかじめ決定した閾値未満の場合には（ステップＳ１０）、絶対方位参照値が得られなかったものとして、後述する回転系帰還入力ｕrの方位軸に関する成分を零とする（ステップＳ１１）。ＧＰＳ測位時において検出した速度が閾値以上の場合には（ステップＳ１０）、ＧＰＳ受信機１９で検出した進行方向方位と磁気コンパス１８で検出した絶対方位を一定時間の間比較し、両者の間の偏差があらかじめ決定した閾値未満なら（ステップＳ１２）、磁気コンパス１８で検出した絶対方位は信頼できるものとし、それを絶対方位参照値とする（ステップＳ１３）。それ以外の場合には（ステップＳ１２）、信頼性に乏しいものとし、ＧＰＳ受信機１９で検出した進行方向方位を絶対方位参照値とする（ステップＳ１４）。
【００４７】
回転系帰還入力算出部１３では、３軸ジャイロ９のドリフト誤差による姿勢方位推定フィルタにおける発散を防止するために、先に推定した絶対姿勢参照値と絶対方位参照値、および現時点での絶対姿勢推定値と絶対方位推定値を用いてそれらの間の偏差を計算し、適切なゲインを乗じて帰還入力urとし姿勢方位角速度推定部１１へ帰還する。
【００４８】
次に、姿勢方位推定手段１における信号処理の２つ目である位置速度推定フィルタについて説明する。図５は位置速度推定フィルタにおける信号処理の概略を示すブロック図である。図５において、２０は座標変換部、２１は並進加速度推定部、２２は積分演算部、２３は測地座標速度推定部、２４は積分演算部、２５は絶対位置参照値算出部、２６は絶対速度参照値算出部、２７は並進系帰還入力算出部である。符号１４、１５，１９は、図２と同じであるため、ここでは説明を省略する。
【００４９】
図５における座標変換部１５において、３軸加速度計１４によって検出された加速度fa = [fxa ,fya ,fza]T を移動体固定座標系[ab]で表現する。
【００５０】
【数１４】
さらに座標変換部２０において、前述の姿勢方位推定フィルタより推定された絶対姿勢推定値および絶対方位推定値を用いて、局所水平面座標系[al] に変換する。
【００５２】
【数１５】
次に並進加速度推定部２１において、移動体の並進運動に関する運動方程式から移動体の並進加速度を推定する。すなわち
【００５４】
【数１６】
ここでutは並進系帰還入力算出部２７から算出された入力量で、この詳細については後述する。
【００５６】
次に積分演算部２２において、推定した並進加速度を積分することにより、移動体の並進速度推定値vbe lを得る。
【００５７】
また測地座標速度推定部２３において、現時点の位置速度推定値を用いて、移動体の並進運動に関する運動方程式から移動体の絶対位置を表す測地座標の時間微分値を推定する。すなわち
【００５８】
【数１７】
これを積分演算部２４において積分することにより、移動体の絶対位置推定値
【数１８】
【００６０】
絶対位置参照値算出部２５では、ＧＰＳ受信機１９で受信した移動体の絶対位置(緯度、経度、高度)を絶対位置参照値とする。また並進速度参照値算出部２６では、ＧＰＳ受信機１９で受信した移動体の絶対位置を数値的に１階微分することにより移動体の並進速度vbe lを推定し、これを並進速度参照値とする。
【００６１】
並進系帰還入力算出部２７では、３軸加速度計１４のバイアス誤差による位置速度推定フィルタにおける発散を防止するために、絶対位置参照値と並進速度参照値、および現時点での絶対位置推定値と並進速度推定値を用いてそれらの間の偏差を計算し、適切なゲインを乗じて帰還入力utとし並進加速度推定部２１へ帰還する。なおＧＰＳ１９が受信不可で絶対位置参照値あるいは並進速度参照値が得られない場合には、それまでの受信値を保持して算出するか、あるいは対応する帰還入力を零とする。
【００６２】
衛星相対方向算出手段３では、姿勢方位推定手段１で推定した移動体の絶対姿勢φ、θおよび絶対方位ψ、および衛星絶対方向算出手段で算出した衛星の方位角αaおよび仰角αeから、移動体に対する方位角仰βaおよび角βeを算出する。衛星指向方向単位ベクトルに着目すると、これらの諸量の間には以下の関係式が成立している。
【００６３】
【数１９】
従ってこの第３式から移動体に対する衛星の仰角βe、また第１、２式から移動体に対する衛星の方位角βaが算出される。
【００６５】
ここで求めたβa、βeを指令値としてアンテナ駆動装置４において、アンテナあるいはアンテナビームを推定される衛星方向に指向させる。さらにこれとは独立して、衛星相対方向探索手段５においては、衛星から送信される信号の受信レベルを用いて、それが最大となる衛星相対方向が探索される。例えばアンテナあるいはアンテナビームを、衛星相対方向算出手段３で算出した方向を中心として、移動体に対する方位角および仰角方向に微小量駆動し、その近傍でのアンテナ受信レベルを数点に渡って記憶しておき、それらを補間して最大受信レベル位置を決定することにより、最も確からしい衛星相対方向を探索し、その際の方位角および仰角増分信号Δβa、Δβeを衛星相対方向算出手段３より算出した方位角および仰角信号βa、βeに重畳してアンテナあるいはアンテナビームを指向させる。これとともにアンテナ駆動装置４には方位角および仰角を検出するためのエンコーダが備え付けられており、その際の方位角βaハット（＝βa＋Δβa）および仰角βeハット（＝βe＋Δβe）が検出されて姿勢方位推定手段１に入力される。姿勢方位推定手段１では前述したように、これらの値を用いて絶対方位参照値の候補が決定される。
【００６６】
このように、本実施の形態においては、衛星相対方向算出手段３から得られた衛星相対方向に、衛星相対方向探索手段５から得られた衛星相対方向を重畳してアンテナあるいはアンテナビームを駆動するため、移動体の機敏な運動にも対処でき、また角速度センサのドリフト誤差や加速度センサのバイアス誤差により衛星相対方向算出手段３から得られる衛星相対方向に誤差が生じても、衛星見通し時である限り衛星相対方向探索手段５によりその誤差を補償することができ、長時間に渡り高精度にアンテナ方向を制御して衛星を追尾することができる。
【００６７】
また、このように姿勢方位推定手段１における姿勢推定フィルタにおいて、加速度センサ検出値から絶対姿勢参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタフィルタにおいて、磁気方位センサ検出値、ＧＰＳ受信値、およびアンテナ駆動装置４から入力された方位角および仰角から絶対方位参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができるとともに、方位軸に関して比較的安価な角速度センサを利用することができる。
【００６８】
これより衛星相対方向算出手段３から得られる衛星相対方向の推定精度を高めることができる。また障害物等により衛星の見通しが不可な場合においてもアンテナあるいはアンテナビームを高精度に衛星方向に指向させておくことができるので、見通し状態に復帰した場合に迅速に衛星追尾を再開することができる。
【００６９】
実施の形態２．
図６は、この発明を実施するための実施の形態２による衛星追尾用アンテナ制御装置を説明するための図である。図において、３２は本実施の形態における衛星相対方向探索手段であり、衛星相対方向探索手段３２内には、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索部３２ａと、探索された当該衛星相対方向を衛星相対方向算出手段３より得られた衛星相対方向に重畳するための加算器３２ｂ，３２ｃが設けられている。
【００７０】
実施の形態１においてアンテナ駆動装置４は、衛星相対方向算出手段３で得られた衛星相対方向に衛星相対方向探索手段５で得られた衛星相対方向を重畳してアンテナあるいはアンテナビームを駆動したが、これは下記のようにしても同様の効果を実現することができる。
【００７１】
アンテナ駆動装置４は衛星相対方向算出手段３より得られた方位角および仰角方向βa，βeにアンテナあるいはアンテナビームを駆動する。また、衛星相対方向探索手段３２では、衛星相対方向算出手段３より得られた衛星相対方向を中心として衛星相対方向が探索され、その際の方位角および仰角増分信号Δβa、Δβeを衛星相対方向算出手段３より得られた方位角および仰角βa，βeに重畳し、姿勢方位推定手段１へ入力する。姿勢方位推定手段１ではこれを用いて実施の形態１の場合と同様にして絶対方位参照値の候補が推定され、絶対方位参照値を決定し帰還系が構成される。
【００７２】
このように、本実施の形態においては、衛星相対方向探索手段３２において、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、それを衛星相対方向算出手段３より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を姿勢方位推定手段１へ入力するとともに、姿勢方位推定手段１ではその値を用いて絶対方位参照値を推定し帰還系を構成したため、移動体の機敏な運動にも対処でき、また衛星見通し時である限り、角速度センサのドリフト誤差や加速度センサのバイアス誤差による衛星相対方向算出手段３における衛星相対方向推定誤差の発生を補償することができ、長時間に渡り高精度にアンテナ方向を制御して衛星を追尾することができる。
【００７３】
このように姿勢方位推定手段１における姿勢推定フィルタにおいて、加速度センサ検出値から絶対姿勢参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタフィルタにおいて、磁気方位センサ検出値、ＧＰＳ受信値、および、衛星相対方向探索手段３２から入力された方位角および仰角から絶対方位参照値を推定して帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができるとともに、方位軸に関して比較的安価な角速度センサを利用することができる。
【００７４】
これより衛星相対方向算出手段３から得られる衛星相対方向の推定精度を高めることができる。また障害物等により衛星の見通しが不可な場合においてもアンテナあるいはアンテナビームを高精度に衛星方向に指向させておくことができるので、見通し状態に復帰した場合に迅速に衛星追尾を再開することができる。
【００７５】
実施の形態３．
上述の実施の形態１および実施の形態２における姿勢方位推定手段１において、アンテナ駆動装置４あるいは衛星相対方向探索手段３２から得られた移動体に対するアンテナあるいはアンテナビームの方位角および仰角から、移動体の絶対方位を推定する手法は以下のようにしてもよい。姿勢方位推定手段１で推定した移動体の絶対姿勢φ、θおよび絶対方位ψ、および、衛星見通し時におけるアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットを用いると、衛星指向方向単位ベクトルに着目して、衛星絶対方向（方位角αaハットと仰角αeハット）を次式から推定することができる。
【００７６】
【数２０】
この第３式から追尾する衛星の仰角推定値αeハット、また第１、２式から方位角推定値αaハットを得る。ここで求めた方位角推定値αaハットと衛星絶対方向算出手段２で算出した実際の衛星の方位角αa、および現時点の姿勢方位推定手段１における絶対方位推定値ψを用いて、移動体の絶対方位参照値の候補が次のように決定される。
【００７８】
【数２１】
なお、本実施の形態においても、上述の実施の形態１及び実施の形態２と同様の効果を得ることができる。
【００８０】
実施の形態４．
上述の実施の形態１および実施の形態２における姿勢方位推定手段１において、姿勢推定フィルタにおける絶対姿勢参照値は３軸加速度計１４を用いて算出したが、これはこの限りではない。例えば移動体のロール軸とピッチ軸のみに配置した２軸加速度計を用いても同様なことが行える。この場合には加速度ベクトルの大きさが重力加速度と同じであると仮定し、ヨー軸方向加速度はその重力加速度からロール軸方向加速度、ピッチ軸方向加速度を除いた成分であるとする。
【００８１】
本実施の形態においても、上述の実施の形態１及び実施の形態２と同様の効果を得ることができる。
【００８２】
また、姿勢推定精度は低下するが、３軸加速度計１４の代わりにロール角、ピッチ角が検出可能な液面傾斜計を備え、その検出値を絶対姿勢参照値としてもよい。この場合には加速度計に比べて検出可能な加速度の帯域は劣るが、姿勢方位推定手段１の低価格化を図ることができる。
【００８３】
実施の形態５．
図７は、この発明を実施するための実施の形態５による衛星追尾用アンテナ制御装置を説明するための部分構成図である。図７は、衛星追尾用アンテナ制御装置における姿勢方位推定手段１の、姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。ドリフト誤差推定部２８において、回転系帰還入力算出部１３から算出された入力量ｕrを用いてジャイロ９の持つドリフト誤差を推定する。ジャイロ９により検出される角速度信号に含まれる誤差として、加法的なドリフト誤差を考慮すると、ドリフト誤差は次のように推定される。
【００８４】
【数２２】
絶対姿勢参照値算出部１６および絶対方位参照値算出部１７において信頼性のある絶対姿勢参照値および絶対方位参照値が得られ、それを用いて帰還入力ｕrが計算された場合には、上式によってドリフト誤差を計算して、その推定値を時々刻々更新する。
【００８６】
回転系帰還入力算出部１３では、移動体の並進速度推定値、並進加速度推定値、ＧＰＳ受信時、非受信時、および、衛星見通し時、非見通し時の場合に応じて、帰還入力が切断される。先ず姿勢推定フィルタについて説明する。絶対姿勢参照値算出部１６において算出される絶対姿勢参照値は、前述したように移動体の並進運動方程式を用いて推定した重力ベクトルの３成分に基づいて導出されている。ロール角参照値φ(ref)は重力ベクトルのｙb軸成分とｚb軸成分の２成分から導出され、ピッチ角参照値θ(ref)は重力ベクトルのｘb軸成分とｚb軸成分の２成分から導出されるため、例えば旋回に伴って移動体のｙb軸成分の運動加速度が大きい場合や、加減速に伴ってｘb軸成分の運動加速度が大きい場合には、算出した絶対姿勢参照値の信頼性が低下する可能性がある。そこで位置速度推定フィルタにおける並進加速度推定部２１において推定した移動体の並進加速度ｖbe lドットを用いてその信頼性を判定する。先ず推定した並進加速度を移動体固定座標系[ａb]に変換する。
【００８７】
【数２３】
ここで推定した並進加速度ｖbe bドットのｙb軸成分とｚb軸成分が大きい場合には、求めたロール角参照値φ(ref)の信頼性は低いものとし、姿勢推定フィルタにおけるロール角推定部における帰還入力を切断する。また推定したｖbe bドットのｘb軸成分とｚb軸成分が大きい場合には、求めたピッチ角参照値θ(ref)の信頼性は低いものとし、姿勢推定フィルタにおけるピッチ角推定部における帰還入力を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、図８に示すように現時点でのジャイロドリフト誤差推定値ｄgを用いて対応する入力ｕrを計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【００８９】
方位推定フィルタについても同様である。絶対方位参照値算出部１７では、磁気コンパス１８から検出された絶対方位、ＧＰＳ受信機１９から検出した移動体の進行方向方位、衛星見通し時においてアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットから推定した絶対方位を候補として、その３通りの候補の中から適切なものを選択して絶対方位参照値とする。信頼性のある絶対方位参照値が得られなかった場合には、方位推定フィルタにおける帰還入力を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、図８に示すように現時点でのジャイロドリフト誤差推定値ｄgを用いて対応する入力を計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【００９０】
また図９は、衛星追尾用アンテナ制御装置における姿勢方位推定手段１の、位置速度推定フィルタにおける信号処理の概略を示すブロック図である。バイアス誤差推定部２９において、並進系帰還入力算出部２７から算出された入力量ｕtを用いて加速度計１４の持つバイアス誤差を推定する。加速度計１４により検出される加速度信号に含まれる誤差として、加法的なバイアス誤差を考慮すると、バイアス誤差は次のように推定される。
【００９１】
【数２４】
ＧＰＳ測位時においては、絶対位置参照値算出部２５および絶対速度参照値算出部２６において絶対位置参照値および絶対速度参照値が得られ、それを用いて帰還入力ｕtが計算された場合には、上式によってバイアス誤差を計算して、その推定値を時々刻々更新する。
【００９３】
ＧＰＳが未測位の場合には位置速度推定フィルタにおける帰還入力を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、図１０に示すように現時点での加速度計バイアス誤差推定値ｂaを用いて対応する入力ｕtを計算し、加速度計１４の持つバイアス誤差の影響を補償する。
【００９４】
このように本実施の形態においては、移動体の運動や受信状況に応じて、姿勢推定フィルタにおける帰還系および方位推定フィルタにおける帰還系を各姿勢方位軸に関して独立に切断することにより、移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合や、あるいは移動体静止時においてＧＰＳから絶対方位参照値が高精度に得られない場合等において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散、あるいは方位推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【００９５】
実施の形態６．
図１１及び図１２は、この発明を実施するための実施の形態６による衛星追尾用アンテナ制御装置を説明するための図であり、特に、図１１は、衛星追尾用アンテナ制御装置における姿勢方位推定手段１の、姿勢方位推定フィルタにおける信号処理の概略を示すブロック図、また図１２は位置速度推定フィルタにおける信号処理の概略を示すブロック図である。これらの図において、３０及び３１は低域通過フィルタであり、低域通過フィルタ３０は、３軸ジャイロ９が有するドリフト特性に対応した低域の所定の値をしきい値とし、それよりも高い周波数の高域成分を除去するためのものである。同様に、低域通過フィルタ３１は３軸加速度計１４が有するバイアス特性に対応した低域の所定の値をしきい値とし、それよりも高い周波数の高域成分を除去するためのものである。
【００９６】
上述の実施の形態５において、ジャイロ９の持つドリフト誤差推定値および加速度計１４の持つバイアス誤差推定値は、回転系帰還入力ｕrおよび並進系帰還入力ｕtから算出し、帰還入力切断時においては、それらを用いて対応する補償入力を計算して、ドリフト誤差およびバイアス誤差の影響を補償した。しかし回転系帰還入力ｕrおよび並進系帰還入力ｕtには、一般に移動体の運動に伴う高域成分（高周波成分）が含まれており、それを用いてドリフト誤差およびバイアス誤差を推定すると、推定値には本来ジャイロや加速度計が有していない高域成分が含まれる場合があり、推定精度を高く保てない可能性がある。したがってこの推定値を用いて補償入力を計算した場合には、ドリフト誤差およびバイアス誤差の影響を高精度に補償できない可能性がある。そこで、本実施の形態においては、搭載したジャイロ９や加速度系１４の持つドリフト特性およびバイアス特性をあらかじめ調べておき、図１１および図１２に示すように、ドリフト誤差推定部２８で推定した推定値およびバイアス誤差推定部２９で推定した推定値に対して、それぞれの特性に応じた低域通過フィルタ３０、３１を時々刻々適用し、ドリフト推定値およびバイアス推定値の推定精度を高める。帰還入力切断時には、これらの高域成分が除去されたドリフト誤差推定値およびバイアス誤差推定値を用いて対応する補償入力を計算し、ドリフト誤差およびバイアス誤差の影響を補償する。
【００９７】
このように、本実施の形態においては、３軸ジャイロ９のドリフト特性に応じた低域通過フィルタ３０を適用することにより、帰還入力から推定した角速度センサドリフト誤差に含まれる移動体の機敏な運動が要因の高域成分を除去することができ、角速度センサが持つドリフト誤差をより高精度に推定することができる。またそれを用いて補償入力を算出することにより、帰還入力切断時においても高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【００９８】
また同様に、本実施の形態においては、３軸加速度計１４のバイアス特性に応じた低域通過フィルタ３１を適用することにより、帰還入力から推定したバイアス誤差に含まれる移動体の機敏な運動が要因の高域成分を除去することができ、バイアス誤差をより高精度に推定することができる。またそれを用いて姿勢方位推定フィルタにおける補償入力を算出することにより、帰還入力切断時においても高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【００９９】
実施の形態７．
実施の形態１あるいは実施の形態２で説明したように、姿勢方位推定手段１における絶対方位推定精度を高めるためには、高精度な絶対方位参照値が必要である。衛星指向方向単位ベクトルに着目すると、衛星見通し時におけるアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハット、移動体の絶対姿勢（ロール角φとピッチ角θ）および絶対方位ψ、衛星の方位角αaおよび仰角αeの間には、以下の関係式が成立しなければならない。
【０１００】
【数２５】
ここで、ｓr＝sinφ、ｃr＝cosφ、ｓp＝sinθ、ｃp＝cosθ、ｓh＝sinψ、ｃh＝cosψとする。アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットはアンテナ駆動装置４あるいは衛星相対方向探索手段３２から得ることができる。移動体の絶対姿勢および絶対方位は姿勢方位推定手段１から得ることができる。また衛星の方位角αaおよび仰角αeに関しては衛星絶対方向算出手段２より得ることができる。
【０１０２】
上式はこれらの諸量の間で成立しなければならない拘束条件式であるが、実際には満足されず、その主な要因は姿勢方位推定手段１における絶対姿勢推定誤差および絶対方位推定誤差の存在にある。そこで姿勢推定フィルタで得られた現時点での絶対姿勢推定値を初期値として、上記拘束条件式を満足するように絶対姿勢推定値および絶対方位推定値を逐次的に更新し、絶対方位参照値の候補を得る手法を説明する。
【０１０３】
先ず上式を絶対方位に関する方程式と見なせば、第１、２式を変形して
【０１０４】
【数２６】
次に絶対姿勢に関する方程式と見なせば、第１、２式の左辺からψを消去し、第３式と並べて
【０１０６】
【数２７】
これら２つの関係式を用いて、次のように絶対姿勢および絶対方位を逐次的に更新し、最終的に得られた絶対方位を絶対方位参照値の候補とする。
【０１０８】
先ず姿勢方位推定フィルタにおける現時点の絶対姿勢推定値（ロール角φとピッチ角θ）をそれぞれφ(i)、θ(i)とする。ｉ＝０にセットする（ステップＳ２１）。
【０１０９】
次にロール角推定値φ(i)およびピッチ角推定値θ(i)を用いて、上記絶対方位に関する方程式をsinψ、cosψに関して解き、これから絶対方位推定値ψを得る。得られた絶対方位推定値をψ (i)とする（ステップＳ２２）。
【０１１０】
次にロール角φおよびピッチ角θを１次の微小項まで次のように展開する。
【０１１１】
【数２８】
これを上記絶対姿勢に関する方程式に代入して整理すると、未知数Δφ、Δθに関する連立一次方程式を得る。これを解いてΔφ、Δθを得、それを用いて更新したロール角推定値φ(i+1)およびピッチ角推定値θ(i+1)を得る。ｉ←ｉ＋１としてステップＳ２２へ戻る（ステップＳ２３）。
【０１１３】
以下、ステップＳ２２で得られる絶対方位推定値が収束するまで上記手続きを繰り返し、最終的に得られた絶対方位推定値を絶対方位参照値算出部１７における絶対方位参照値の候補として採用する（ステップＳ２４）。
【０１１４】
このように衛星見通し時において前記アンテナ駆動装置４あるいは衛星相対方向探索手段３２から入力された方位角および仰角を用いて、移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値の候補とすることにより、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値とすることにより姿勢方位推定フィルタの絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１１５】
実施の形態８．
上述の実施の形態１あるいは実施の形態２で説明したように、姿勢方位推定手段１における絶対方位推定精度を高めるためには、高精度な絶対方位参照値が必要である。衛星指向方向単位ベクトルに着目すると、衛星見通し時におけるアンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハット、移動体の絶対姿勢（ロール角φとピッチ角θ）および絶対方位ψ、衛星の方位角αaおよび仰角αeの間には、以下の関係式が成立しなければならない。
【０１１６】
【数２９】
ここでｓr＝sinφ、ｃr＝cosφ、ｓp＝sinθ、ｃp＝cosθ、ｓh＝sinψ、ｃh＝cosψとする。アンテナあるいはアンテナビームの方位角βaハットおよび仰角βeハットはアンテナ駆動装置４あるいは衛星相対方向探索手段３２から得ることができる。絶対姿勢および絶対方位は姿勢方位推定手段１から得ることができる。また衛星の方位角αaおよび仰角αeに関しては衛星絶対方向算出手段２より得ることができる。
【０１１８】
上式はこれらの諸量の間で成立しなければならない拘束条件式であるが、実際には満足されず、その主な要因は姿勢方位推定手段１における絶対姿勢推定誤差および絶対方位推定誤差の存在にある。そこで姿勢推定フィルタで得られた現時点での絶対姿勢推定値を初期値として、上記拘束条件式を満足するような絶対姿勢推定値および絶対方位推定値を得る手法を説明する。
【０１１９】
上式の第３式に着目すると、これは移動体の絶対方位ψには関係なく、絶対姿勢のロール角φおよびピッチ角θのみが満たすべき拘束条件となっている。この第３式を書き直すと
【０１２０】
【数３０】
ここで
【０１２２】
【数３１】
ｅaは鉛直下方向単位ベクトル（移動体固定座標系[ab]表現）、ｅβは衛星指向軸方向単位ベクトル（移動体固定座標系[ab]表現）を表している。ｅaおよびｅβが単位ベクトルであることに注意すると、上式は２つのベクトルｅaとｅβの内積演算を定義した拘束条件式であり、図１３に示すようにベクトルeaはベクトルeβを中心軸として角度（π／２＋αe）だけ隔てた円錐面上に分布すべき諸量であることが分かる。しかし姿勢方位推定フィルタによって推定したロール角φおよびピッチ角θには推定誤差が含まれるため、この推定値を用いて生成したベクトルｅaハットは、実際にはこの円錐面上に存在していない。そこで図１３に示すようにベクトルｅβとベクトルｅaハットにより張られる平面γを考え、この平面γと上記円錐面との交線をベクトルｅaの候補値ｅa *として採用する。すなわちベクトルｅaハットに最小二乗近似の意味で最も近い円錐面上のベクトルをベクトルｅaの候補値ｅa *として採用する。
【０１２４】
平面γ上でベクトルｅβに垂直な単位ベクトルは
【０１２５】
【数３２】
したがってベクトルｅaの候補値ｅa *は次式で与えられる。
【０１２７】
【数３３】
これより先ずロール角、ピッチ角の候補値が、ベクトルｅa *の３成分を用いて次のように求められる。
【０１２９】
【数３４】
ここで求めたロール角φ*とピッチ角θ*を用いて、実施の形態７で述べた絶対方位に関する方程式を解くことによって、絶対方位推定値を得ることができ、これを絶対方位参照値の候補とする。
【０１３１】
このように、本実施の形態においては、最小二乗近似の意味で先ず最も確からしい移動体の絶対姿勢を推定し、次にそれを用いて絶対方位を推定したことにより、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値の候補とすることにより方位推定フィルタの絶対方位推定精度を高めることができる。また絶対方位参照値を得るための計算負荷も小さくなる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１３２】
実施の形態９．
図１４は、この発明を実施するための実施の形態９による衛星追尾用アンテナ制御装置を説明するための図である。図１４は特に姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。実施の形態７および実施の形態８において、アンテナ駆動装置４で検出したアンテナあるいはアンテナビームの方位角および仰角を用いて、絶対方位参照値算出部１７で絶対姿勢および絶対方位推定値を得、その絶対方位推定値を絶対方位参照値の候補として用いたが、そこでの絶対姿勢推定値は回転系帰還入力算出部１３における姿勢推定フィルタの帰還信号の構成に用いることができる。
【０１３３】
絶対姿勢参照値算出部１６では、３軸加速度計１４の信号を用いて絶対姿勢参照値が算出される。ただし実施の形態５で説明したように、その精度は移動体の並進運動加速度に大きく依存しているため、実施の形態５では移動体の並進運動加速度を推定し、その値に応じて姿勢推定フィルタの帰還系を切断した。
【０１３４】
しかるにアンテナ駆動装置４で検出したアンテナあるいはアンテナビームの方位角および仰角を用いて、絶対方位参照値算出部１７で推定された絶対姿勢推定値は、移動体の並進運動加速度には依存しないため、この推定値を絶対姿勢参照値算出部１６に入力し、回転系帰還入力算出部１３における姿勢推定フィルタの帰還系切断の判定に利用することができる。例えば３軸加速度計１４の信号を用いて算出された絶対姿勢参照値と、絶対方位参照値算出部１７において推定された絶対姿勢推定値を一定時間の間比較し、その間の偏差が大きい場合には、３軸加速度計１４の信号を用いて算出された絶対姿勢参照値の信頼性は低いものとし、姿勢推定フィルタにおける対応する姿勢軸の帰還系を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、現時点でのジャイロドリフト誤差推定値を用いて対応する入力ｕrを計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【０１３５】
このように、本実施の形態においては、移動体の並進運動加速度には依存しない絶対姿勢推定値を用いて姿勢推定フィルタにおける帰還系を各姿勢方位軸に関して独立に切断することにより、加速度センサを用いても移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１３６】
実施の形態１０．
図１５は、この発明を実施するための実施の形態１０による衛星追尾用アンテナ制御装置を説明するための図である。図１５は特に姿勢方位推定フィルタにおける信号処理の概略を示すブロック図である。実施の形態７および実施の形態８において、衛星相対方向探索手段３２より得られた方位角および仰角を用いて絶対姿勢および絶対方位推定値を算出し、その絶対方位推定値を絶対方位参照値の候補として用いたが、実施の形態９で述べたのと同様に、そこでの絶対姿勢推定値は回転系帰還入力算出部１３における帰還信号の構成に用いることができる。
【０１３７】
絶対姿勢参照値算出部１６では、３軸加速度計１４の信号を用いて絶対姿勢参照値が算出される。ただし実施の形態５で説明したように、その精度は移動体の並進運動加速度に大きく依存しているため、実施の形態５では移動体の並進運動加速度を推定し、その値に応じて姿勢推定フィルタの帰還系を切断した。
【０１３８】
しかるに衛星相対方向探索手段３２より得られた方位角および仰角を用いて、絶対方位参照値算出部１７で推定された絶対姿勢推定値は、移動体の並進運動加速度には依存しないため、この推定値を絶対姿勢参照値算出部１６に入力し、回転系帰還入力算出部１３における姿勢推定フィルタの帰還系切断の判定に利用することができる。例えば３軸加速度計１４の信号を用いて算出された絶対姿勢参照値と、絶対方位参照値算出部１７において推定された絶対姿勢推定値を一定時間の間比較し、その間の偏差が大きい場合には、３軸加速度計１４の信号を用いて算出された絶対姿勢参照値の信頼性は低いものとし、姿勢推定フィルタにおける対応する姿勢軸の帰還系を切断する。さらに帰還入力を切断した場合には、それによるフィルタの発散を防止するために、現時点でのジャイロドリフト誤差推定値を用いて対応する入力ｕrを計算し、ジャイロ９の持つドリフト誤差の影響を補償する。
【０１３９】
このように、本実施の形態においては、移動体の並進運動加速度には依存しない絶対姿勢推定値を用いて姿勢推定フィルタにおける帰還系を各姿勢方位軸に関して独立に切断することにより、加速度センサを用いても移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度を保持することができる。またそれを用いて衛星相対方向算出手段３において高精度に衛星相対方向を推定することができる。
【０１４０】
実施の形態１１．
実施の形態１あるいは実施の形態２で説明したように、姿勢方位推定手段１における絶対方位推定精度を高めるためには、高精度な絶対方位参照値が必要である。衛星見通し時である限り、アンテナあるいはアンテナビームの方位角βaハットと仰角βeハットから推定された絶対方位の精度は、周囲環境の磁気的外乱や移動体の運動に依存しないため、一般には磁気コンパス１８やＧＰＳ受信機１９に比べて精度の良い絶対方位参照値を与える。そこで初期衛星補足時、あるいは衛星非見通し時から見通し時への復帰時において、実施の形態１、実施の形態３、実施の形態７あるいは実施の形態８で記述した手法によって絶対方位推定値が得られた時点で、これを絶対方位参照値として姿勢方位推定手段１における方位推定フィルタを初期化する。
【０１４１】
これにより、本実施の形態においては、姿勢方位推定フィルタにおける絶対方位推定誤差を迅速に零に収束させることができ、以後の絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４２】
【発明の効果】
この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動するとともに、当該衛星相対方向に衛星相対方向探索手段から得られた衛星相対方向を重畳した方向にアンテナあるいはアンテナビームを駆動して衛星を指向し、その際の移動体に対するアンテナあるいはアンテナビームの方位角および仰角を前記姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信機の受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置であるので、移動体の機敏な運動にも対処でき、また角速度センサのドリフト誤差や加速度センサのバイアス誤差により衛星相対方向算出手段から得られる衛星相対方向に誤差が生じても、衛星見通し時である限り衛星相対方向探索手段によりその誤差を補償することができ、長時間に渡り高精度にアンテナを制御して衛星を追尾することができる。また、このように姿勢方位推定手段における姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信機の受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができる。また、これより衛星相対方向算出手段から得られる衛星相対方向の推定精度を高めることができる。
【０１４３】
また、この発明は、アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、移動体の絶対位置と追尾対象の衛星の絶対位置とから衛星の方位角および仰角を算出する衛星絶対方向算出手段と、姿勢方位推定手段により推定された絶対姿勢および絶対方位、および、衛星絶対方向算出手段により算出された方位角および仰角から、移動体に対する衛星の方位角および仰角を算出する衛星相対方向算出手段と、算出された方位角および仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、姿勢方位推定手段は、移動体の３軸まわりの回転角速度を検出する角速度センサと、移動体の３軸方向の並進加速度を検出する加速度センサと、移動体の絶対方位を検出する磁気方位センサと、移動体の絶対位置を検出するＧＰＳ受信機と、移動体の絶対姿勢を推定する姿勢推定フィルタと、移動体の絶対方位を推定する方位推定フィルタとを備え、アンテナ駆動手段は、衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動し、衛星相対方向探索手段は、衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、その方向を衛星相対方向算出手段より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を上記姿勢方位推定手段へ入力し、姿勢方位推定手段は、姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する衛星追尾用アンテナ制御装置であるので、移動体の機敏な運動にも対処でき、また衛星見通し時である限り、角速度センサのドリフト誤差や加速度センサのバイアス誤差による衛星相対方向算出手段における衛星相対方向推定誤差の発生を補償することができ、長時間に渡り高精度にアンテナを制御して衛星を追尾することができる。また、姿勢方位推定手段における姿勢推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、絶対姿勢推定精度を高めることができる。また方位推定フィルタにおいて、上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、磁気方位センサの検出値、ＧＰＳ受信値、および、アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成したので、角速度センサのドリフト誤差による姿勢推定フィルタの発散を防ぐことができるとともに、周囲環境の磁気的外乱や移動速度の影響が少ない絶対方位参照値を得ることができるので、絶対方位推定精度を高めることができる。また、これより衛星相対方向算出手段から得られる衛星相対方向の推定精度を高めることができる。
【０１４４】
また、姿勢方位推定手段において、姿勢推定フィルタおよび方位推定フィルタにおける帰還入力から姿勢、方位軸に関する上記角速度センサのドリフト誤差を推定するとともに、上記移動体の並進加速度推定値が大きい場合には、姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、上記移動体の並進速度推定値が小さい場合、あるいは、ＧＰＳ非受信時の場合において、衛星非見通し時の場合には、上記方位推定フィルタにおける帰還系を切断するとともに、切断時においてはこれまでの帰還入力から推定した角速度センサのドリフト誤差を入力として補償するので、移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合や、あるいは移動体静止時においてＧＰＳから絶対方位参照値が高精度に得られない場合等において、帰還系による姿勢方位推定精度の低下を防ぐことができる。また切断時にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散、あるいは方位推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４５】
また、姿勢方位推定手段において、帰還入力から推定した角速度センサのドリフト誤差に対して、各姿勢方位軸に備えた角速度センサのドリフト特性に応じた低域通過フィルタを適用するので、帰還入力から推定した角速度センサドリフト誤差に含まれる移動体の機敏な運動が要因の高域成分を除去することができ、角速度センサが持つドリフト誤差をより高精度に推定することができる。またそれを用いて姿勢方位推定フィルタにおける補償入力を算出することにより、帰還入力切断時においても高精度な絶対姿勢推定精度および絶対方位推定精度を保持することができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４６】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値とするので、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値とすることにより姿勢方位推定フィルタの絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４７】
また、衛星の方位角および仰角、姿勢方位推定手段によって推定された移動体の絶対姿勢、衛星見通し時においてアンテナ駆動手段あるいは衛星相対方向探索手段から入力された方位角および仰角を用いて、先ず最小二乗近似の意味で移動体の絶対姿勢を推定し、次にその推定値を用いて絶対方位参照値を推定するので、周囲環境の磁気的外乱の影響を受けやすい磁気方位センサ、移動体の進行方向方位（速度ベクトル方位）を出力するＧＰＳ受信装置に比べて、より高精度の絶対方位推定値を得ることができ、それを絶対方位参照値の候補とすることにより姿勢方位推定フィルタの絶対方位推定精度を高めることができる。また絶対方位参照値を得るための計算負荷も小さくなる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４８】
また、逐次的に更新して推定した上記移動体の絶対姿勢、あるいは、最小二乗近似の意味で推定した上記移動体の絶対姿勢を参照して姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、切断時においてはこれまでの帰還入力から推定した上記角速度センサのドリフト誤差を入力として補償するので、加速度センサを用いても移動体の並進運動加速度によって絶対姿勢参照値が高精度に得られない場合において、帰還系による姿勢方位推定精度の低下を防ぐことができる。またその場合にこれまでの帰還入力から推定した角速度センサドリフト誤差を入力して補償することにより、その区間での姿勢推定フィルタの発散を防ぐことができるとともに、高精度な絶対姿勢推定精度を保持することができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【０１４９】
また、初期衛星捕捉時、あるいは、衛星非見通し時から見通し時への復帰時において、姿勢方位推定手段における方位推定フィルタを、アンテナ駆動手段あるいは衛星相対方向探索手段より得られたアンテナあるいはアンテナビームの方位角および仰角を用いて推定した絶対方位推定値で初期化するので、方位推定フィルタにおける絶対方位推定誤差を迅速に零に収束させることができ、以後の絶対方位推定精度を高めることができる。またそれを用いて衛星相対方向算出手段において高精度に衛星相対方向を推定することができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態１による衛星追尾用アンテナ制御装置の構成を示したブロック図である。
【図２】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図３】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける最も信頼性の高い方位推定値の選択方法を示した流れ図である。
【図４】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける最も信頼性の高い方位推定値の選択方法の一例を示した流れ図である。
【図５】 本発明の実施の形態１による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおける信号処理の概略を示したブロック図である。
【図６】 本発明の実施の形態２による衛星追尾用アンテナ制御装置の構成を示したブロック図である。
【図７】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図８】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおけるドリフト誤差の補償方法を示したブロック図である。
【図９】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおける信号処理の概略を示したブロック図である。
【図１０】 本発明の実施の形態５による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおけるバイアス誤差の補償方法を示したブロック図である。
【図１１】 本発明の実施の形態６による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図１２】 本発明の実施の形態６による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の位置速度推定フィルタにおける信号処理の概略を示したブロック図である。
【図１３】 本発明の実施の形態８による衛星追尾用アンテナ制御装置において、鉛直下方向単位ベクトルｅaと衛星指向軸方向単位ベクトルｅβとの関係を示した説明図である。
【図１４】 本発明の実施の形態９による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図１５】 本発明の実施の形態１０による衛星追尾用アンテナ制御装置に設けられた姿勢方位推定手段の姿勢方位推定フィルタにおける信号処理の概略を示したブロック図である。
【図１６】 従来の衛星追尾装置の構成を示したブロック図である。
【符号の説明】
１ 姿勢方位推定手段、２ 衛星絶対方向算出手段、３ 衛星相対方向算出手段、４ アンテナ駆動装置、５，３２ 衛星相対方向探索手段、６ 衛星追尾用アンテナ、７ 移動体のアンテナ取り付け面、８ 衛星、９ ３軸ジャイロ、１０ 角速度算出部、１１ 姿勢方位角速度推定部、１２ 積分演算部、１３ 回転系帰還入力算出部、１４ ３軸加速度計、１５ 座標変換部、１６ 絶対姿勢参照値算出部、１７ 絶対方位参照値算出部、１８ 磁気コンパス、１９ ＧＰＳ受信機、２０ 座標変換部、２１ 並進加速度推定部、２２ 積分演算部、２３ 測地座標速度推定部、２４ 積分演算部、２５ 絶対位置参照値算出部、２６ 絶対速度参照値算出部、２７ 並進系帰還入力算出部、２８ ドリフト誤差推定部、２９ バイアス誤差推定部、３０，３１ 低域通過フィルタ。
Claims (8)
- アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、
上記移動体の絶対位置と追尾対象の衛星の絶対位置とから上記衛星の方位角および仰角を算出する衛星絶対方向算出手段と、
上記姿勢方位推定手段により推定された上記絶対姿勢および上記絶対方位、および、上記衛星絶対方向算出手段により算出された上記方位角および上記仰角から、上記移動体に対する上記衛星の方位角および仰角を算出する衛星相対方向算出手段と、
算出された上記方位角および上記仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、
上記衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、
上記姿勢方位推定手段は、
上記移動体の３軸まわりの回転角速度を検出する角速度センサと、
上記移動体の３軸方向の並進加速度を検出する加速度センサと、
上記移動体の絶対方位を検出する磁気方位センサと、
上記移動体の絶対位置を検出するＧＰＳ受信機と、
上記移動体の絶対姿勢を推定する姿勢推定フィルタと、
上記移動体の絶対方位を推定する方位推定フィルタとを備え、
上記アンテナ駆動手段は、
上記衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動するとともに、当該衛星相対方向に上記衛星相対方向探索手段から得られた衛星相対方向を重畳した方向にアンテナあるいはアンテナビームを駆動して衛星を指向し、その際の移動体に対するアンテナあるいはアンテナビームの方位角および仰角を前記姿勢方位推定手段へ入力し、
上記姿勢方位推定手段は、
上記姿勢推定フィルタにおいて、
上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、
上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、
推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、
上記方位推定フィルタにおいて、
上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、
上記磁気方位センサの検出値、上記ＧＰＳ受信機の受信値、および、上記アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、
推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する
ことを特徴とする衛星追尾用アンテナ制御装置。 - アンテナあるいはアンテナビームの方位角及び仰角に基づいて移動体の絶対姿勢および絶対方位を推定する姿勢方位推定手段と、
上記移動体の絶対位置と追尾対象の衛星の絶対位置とから上記衛星の方位角および仰角を算出する衛星絶対方向算出手段と、
上記姿勢方位推定手段により推定された上記絶対姿勢および上記絶対方位、および、上記衛星絶対方向算出手段により算出された上記方位角および上記仰角から、上記移動体に対する上記衛星の方位角および仰角を算出する衛星相対方向算出手段と、
算出された上記方位角および上記仰角の方向にアンテナあるいはアンテナビームを駆動するアンテナ駆動手段と、
上記衛星から送信される信号の受信レベルを用いて衛星相対方向を探索する衛星相対方向探索手段とを備えた衛星追尾用アンテナ制御装置であって、
上記姿勢方位推定手段は、
上記移動体の３軸まわりの回転角速度を検出する角速度センサと、
上記移動体の３軸方向の並進加速度を検出する加速度センサと、
上記移動体の絶対方位を検出する磁気方位センサと、
上記移動体の絶対位置を検出するＧＰＳ受信機と、
上記移動体の絶対姿勢を推定する姿勢推定フィルタと、
上記移動体の絶対方位を推定する方位推定フィルタとを備え、
上記アンテナ駆動手段は、
上記衛星相対方向算出手段から得られた衛星相対方向にアンテナあるいはアンテナビームを駆動し、
上記衛星相対方向探索手段は、上記衛星から送信される信号の受信レベルを用いて衛星相対方向を探索するとともに、その方向を上記衛星相対方向算出手段より得られた衛星相対方向に重畳して衛星相対方向を補正し、その補正した移動体に対するアンテナあるいはアンテナビームの方位角および仰角を上記姿勢方位推定手段へ入力し、
上記姿勢方位推定手段は、
上記姿勢推定フィルタにおいて、
上記角速度センサにより検出した角速度から絶対姿勢角速度を推定し、当該絶対姿勢角速度を積分して移動体の絶対姿勢を推定し、
上記加速度センサにより検出した加速度から絶対姿勢参照値を算出し、
推定した移動体の絶対姿勢と当該絶対姿勢参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した姿勢角速度を補正する帰還系を構成するとともに、
上記方位推定フィルタにおいて、
上記角速度センサにより検出した角速度から絶対方位角速度を推定し、当該絶対方位角速度を積分して移動体の絶対方位を推定し、
上記磁気方位センサの検出値、上記ＧＰＳ受信機の受信値、および、上記アンテナ駆動手段から入力された方位角および仰角のいずれかから絶対方位参照値を算出し、
推定した移動体の絶対方位と当該絶対方位参照値の偏差を求め、当該偏差から帰還入力を算出し、当該帰還入力を用いて上記角速度センサにより検出した角速度から推定した方位角速度を補正する帰還系を構成する
ことを特徴とする衛星追尾用アンテナ制御装置。 - 上記姿勢方位推定手段において、
上記姿勢推定フィルタおよび上記方位推定フィルタにおける帰還入力から姿勢、方位軸に関する上記角速度センサのドリフト誤差を推定するとともに、
上記移動体の並進加速度推定値が大きい場合には、上記姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、
上記移動体の並進速度推定値が小さい場合、あるいは、ＧＰＳ非受信時の場合において、衛星非見通し時の場合には、上記方位推定フィルタにおける帰還系を切断するとともに、
切断時においてはこれまでの帰還入力から推定した上記角速度センサのドリフト誤差を入力として補償することを特徴とする
請求項１または２に記載の衛星追尾用アンテナ制御装置。 - 上記姿勢方位推定手段において、帰還入力から推定した上記角速度センサのドリフト誤差に対して、各姿勢方位軸に備えた角速度センサのドリフト特性に応じた低域通過フィルタを適用することを特徴とする請求項３に記載の衛星追尾用アンテナ制御装置。
- 上記衛星の方位角および仰角、上記姿勢方位推定手段によって推定された上記移動体の絶対姿勢、上記衛星見通し時において上記アンテナ駆動手段あるいは上記衛星相対方向探索手段から入力された方位角および仰角を用いて、上記移動体の絶対姿勢および絶対方位を逐次的に更新して推定し、得られた絶対方位推定値を絶対方位参照値とすることを特徴とする請求項１ないし４のいずれかに記載の衛星追尾用アンテナ制御装置。
- 上記衛星の方位角および仰角、上記姿勢方位推定手段によって推定された上記移動体の絶対姿勢、上記衛星見通し時において上記アンテナ駆動手段あるいは上記衛星相対方向探索手段から入力された方位角および仰角を用いて、先ず最小二乗近似の意味で移動体の絶対姿勢を推定し、次にその推定値を用いて絶対方位参照値を推定することを特徴とする請求項１ないし４のいずれかに記載の衛星追尾用アンテナ制御装置。
- 逐次的に更新して推定した上記移動体の絶対姿勢、あるいは、最小二乗近似の意味で推定した上記移動体の絶対姿勢を参照して上記姿勢推定フィルタにおける帰還系を各姿勢軸に関して独立に切断するとともに、
切断時においてはこれまでの帰還入力から推定した上記角速度センサのドリフト誤差を入力として補償することを特徴とする
請求項５または６に記載の衛星追尾用アンテナ制御装置。 - 初期衛星捕捉時、あるいは、衛星非見通し時から見通し時への復帰時において、上記姿勢方位推定手段における上記方位推定フィルタを、上記アンテナ駆動手段あるいは上記衛星相対方向探索手段より得られたアンテナあるいはアンテナビームの方位角および仰角を用いて推定した絶対方位推定値で初期化することを特徴とする請求項１ないし７のいずれかに記載の衛星追尾用アンテナ制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000356062A JP3767372B2 (ja) | 2000-11-22 | 2000-11-22 | 衛星追尾用アンテナ制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000356062A JP3767372B2 (ja) | 2000-11-22 | 2000-11-22 | 衛星追尾用アンテナ制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2002158525A JP2002158525A (ja) | 2002-05-31 |
| JP3767372B2 true JP3767372B2 (ja) | 2006-04-19 |
Family
ID=18828370
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2000356062A Expired - Fee Related JP3767372B2 (ja) | 2000-11-22 | 2000-11-22 | 衛星追尾用アンテナ制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3767372B2 (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011199541A (ja) * | 2010-03-19 | 2011-10-06 | Japan Radio Co Ltd | 衛星サーチ時間短縮方法 |
| WO2016194127A1 (ja) * | 2015-06-02 | 2016-12-08 | 三菱電機株式会社 | アンテナ装置 |
| US20220222392A1 (en) * | 2019-06-06 | 2022-07-14 | Hitachi Construction Machinery Co., Ltd. | Construction machine |
Families Citing this family (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100574881B1 (ko) * | 2004-03-11 | 2006-04-27 | 주식회사 에이스테크놀로지 | 이동체 탑재용 안테나 제어장치 및 방법 |
| JP4615287B2 (ja) | 2004-11-01 | 2011-01-19 | 東京計器株式会社 | 方位姿勢検出装置 |
| JP2008228045A (ja) * | 2007-03-14 | 2008-09-25 | Mitsubishi Electric Corp | 衛星追尾用アンテナ装置 |
| JP5022747B2 (ja) * | 2007-03-22 | 2012-09-12 | 古野電気株式会社 | 移動体の姿勢及び方位検出装置 |
| JP5605539B2 (ja) * | 2009-12-15 | 2014-10-15 | 日本電気株式会社 | 移動体位置推定追尾装置、移動体位置推定追尾方法、及び移動体位置推定追尾プログラム |
| JP2012112738A (ja) * | 2010-11-24 | 2012-06-14 | Nec Corp | 追尾装置及び追尾方法 |
| CN105190994A (zh) * | 2013-03-14 | 2015-12-23 | 日本电气株式会社 | 天线设备和天线设备控制方法 |
| JP6332212B2 (ja) * | 2015-09-18 | 2018-05-30 | カシオ計算機株式会社 | 姿勢推定装置、姿勢推定方法及びプログラム |
| WO2017130276A1 (ja) * | 2016-01-25 | 2017-08-03 | スカパーＪｓａｔ株式会社 | アンテナ調整装置およびアンテナ調整方法 |
| JP6581144B2 (ja) * | 2017-04-28 | 2019-09-25 | 株式会社東芝 | 衛星捕捉装置および衛星捕捉方法 |
| JP7107820B2 (ja) * | 2018-11-13 | 2022-07-27 | 古野電気株式会社 | 測位装置、測位方法 |
| EP3772190B1 (en) | 2019-07-30 | 2023-03-08 | Panasonic Intellectual Property Management Co., Ltd. | Communication apparatus and antenna |
| KR102442261B1 (ko) * | 2020-11-06 | 2022-09-13 | 주식회사 문화방송 | 드론에 대한 하이브리드형 방위각 측정 장치 및 그 방법 |
| CN113063440B (zh) * | 2021-02-26 | 2022-12-27 | 上海卫星工程研究所 | 静止轨道微波探测卫星图像定位与配准全物理仿真试验方法和系统 |
| CN115459836A (zh) * | 2022-11-11 | 2022-12-09 | 南京控维通信科技有限公司 | 具备抗风、抗干扰的快速自动对星方法及系统 |
| CN116087869A (zh) * | 2022-12-30 | 2023-05-09 | 泰斗微电子科技有限公司 | 一种基于加速度计的卫星定向方法、设备及可读存储介质 |
| CN116679327A (zh) * | 2023-04-20 | 2023-09-01 | 西安恒达微波技术开发有限公司 | 一种基于固定卫星的天馈伺机电轴标校方法 |
-
2000
- 2000-11-22 JP JP2000356062A patent/JP3767372B2/ja not_active Expired - Fee Related
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011199541A (ja) * | 2010-03-19 | 2011-10-06 | Japan Radio Co Ltd | 衛星サーチ時間短縮方法 |
| WO2016194127A1 (ja) * | 2015-06-02 | 2016-12-08 | 三菱電機株式会社 | アンテナ装置 |
| JPWO2016194127A1 (ja) * | 2015-06-02 | 2017-06-22 | 三菱電機株式会社 | アンテナ装置 |
| US20220222392A1 (en) * | 2019-06-06 | 2022-07-14 | Hitachi Construction Machinery Co., Ltd. | Construction machine |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2002158525A (ja) | 2002-05-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3767372B2 (ja) | 衛星追尾用アンテナ制御装置 | |
| CN110221332B (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
| CN109238262B (zh) | 一种航向姿态解算及罗盘校准抗干扰方法 | |
| Garratt et al. | Integration of GPS/INS/vision sensors to navigate unmanned aerial vehicles | |
| US7970491B2 (en) | Robot localization system | |
| Nebot et al. | Initial calibration and alignment of low‐cost inertial navigation units for land vehicle applications | |
| US9857179B2 (en) | Magnetic anomaly tracking for an inertial navigation system | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN113063429B (zh) | 一种自适应车载组合导航定位方法 | |
| CN111024074B (zh) | 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 | |
| CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量系统 | |
| KR101504063B1 (ko) | 이동 가방 | |
| CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合系统进行航向校正的方法 | |
| JP4191588B2 (ja) | 衛星追尾用アンテナ制御装置 | |
| Skobeleva et al. | Extended Kalman Filter for indoor and outdoor localization of a wheeled mobile robot | |
| CN105928519B (zh) | 基于ins惯性导航与gps导航以及磁力计的导航算法 | |
| CN110068325A (zh) | 一种车载ins/视觉组合导航系统的杆臂误差补偿方法 | |
| US9217639B1 (en) | North-finding using inertial navigation system | |
| WO2013139486A1 (en) | True north seeking and attitude system | |
| KR100781807B1 (ko) | 자율주행 이동물체 및 목표 항로 추적 방법 | |
| US20230349699A1 (en) | Absolute heading estimation with constrained motion | |
| Yuan et al. | A robust multi-state constraint optimization-based orientation estimation system for Satcom-on-the-move | |
| JPH11325951A (ja) | 宇宙航行体の姿勢センサによる軌道決定方法および軌道決定装置 | |
| US9976857B2 (en) | Apparatus for providing an azimuth angle | |
| TWI564546B (zh) | On - board Vehicle Navigation System Calibration Method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20050502 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20050607 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20050728 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20060117 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20060123 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100210 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100210 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110210 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120210 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130210 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130210 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140210 Year of fee payment: 8 |
|
| LAPS | Cancellation because of no payment of annual fees |