方位姿勢検出装置
【課題】ＧＰＳ手段で検出するＧＰＳ方向と磁気方位センサで検出する磁気方位とを用いて、信頼性の高い方位誤差修正を行なう。
【解決手段】ジャイロ信号を用いて座標変換行列を逐次求めて加速度信号の座標変換を行い、該加速度信号からレベル誤差修正値を演算し、ＧＰＳ方向信号及び磁気方位信号を用いて、方位誤差修正値を演算する方位誤差修正値を演算し、レベル誤差及び方位誤差が修正された前記座標変換行列からロール角、ピッチ角、及び方位角の演算を行うものにおいて、方位誤差修正は、ＧＰＳ方向信号の信頼性が高い条件下においては、ＧＰＳ方向信号を用いて方位誤差修正値を求めると共に、ＧＰＳ方向信号と磁気方位信号との差異を演算し、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が低くない条件下では、前記演算していたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号とを用いて方位誤差修正値を求める。
【解決手段】ジャイロ信号を用いて座標変換行列を逐次求めて加速度信号の座標変換を行い、該加速度信号からレベル誤差修正値を演算し、ＧＰＳ方向信号及び磁気方位信号を用いて、方位誤差修正値を演算する方位誤差修正値を演算し、レベル誤差及び方位誤差が修正された前記座標変換行列からロール角、ピッチ角、及び方位角の演算を行うものにおいて、方位誤差修正は、ＧＰＳ方向信号の信頼性が高い条件下においては、ＧＰＳ方向信号を用いて方位誤差修正値を求めると共に、ＧＰＳ方向信号と磁気方位信号との差異を演算し、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が低くない条件下では、前記演算していたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号とを用いて方位誤差修正値を求める。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、船舶、自動車等の移動体の方位姿勢を検出するストラップダウン方式による方位姿勢検出装置に関する。
【背景技術】
【０００２】
従来、ストラップダウン方式による方位姿勢検出装置では、３軸方向の加速度を検出する３軸加速度計と、３軸方向まわりの角速度を検出する３軸ジャイロとを備えている（特許文献１）。
【０００３】
この方位姿勢検出装置では、ジャイロから求められるジャイロ信号を用いて、センサが取り付けられた機体の座標系（ボディ座標系という）からＸ軸方向が北方向で且つＺ軸方向が重力方向に一致している座標系（ローカル座標系という）への変換する行列を演算し、この行列からローカル座標系に対する各軸回りの回転角である方位角、ピッチ角及びロール角を求めて方位姿勢を出力すると共に、ジャイロ誤差に起因する姿勢誤差を、加速度計により検出された重力加速度検知により修正する構成となっている。
【０００４】
しかしながら、加速度計は重力以外の運動加速度も検知するため、運動加速度の影響を受けて姿勢誤差が発生してしまう、という問題がある。高価で高性能なジャイロ及び加速度計を使用すれば運動加速度の影響を受けにくい方位姿勢検出装置を構成することもできるが、低価格、低精度なジャイロまたは加速度を使用した場合の影響は大きく、運動状態によっては実用に耐えないものになってしまう、という問題がある。
【０００５】
かかる運動加速度の影響を除去するための構成としては、例えば、Ｘ軸方向の速度を検出するＸ速度計を備え、加速度誤差を修正するようにしたものが知られている（特許文献２）。
【０００６】
特許文献２では、移動体が旋回運動を行なう場合に、旋回時に生じる遠心力を移動体が支持できなくなることにより、進行方向速度と実際の軌跡速度とが一致しなくなる横滑りが発生するが、Ｘ軸方向の速度しか検出できず、Ｙ軸方向の速度による加速度成分の修正ができないことによる横滑り誤差が姿勢角の誤差をもたらすことを防止するために、方位角信号とＸ軸方向の速度信号から横滑り角を演算によって求めることが提案されている。
【０００７】
しかしながら、かかる特許文献２の演算を行なうためには、速度計を要し、また、横滑り角演算のための演算装置を要するため、装置が大型化し、高コストになるという問題がある。
【０００８】
また、方位角の補正に関しては、従来、磁気方位センサにより検出された方位信号を用いることが一般的に行われている（特許文献１，３）。例えば、特許文献３では、ジャイロによって検出した走行方位角の修正において、地磁気センサが検出する方位を進行方位であるとして、その地磁気センサが検出する方位にジャイロが検出する方位を追従させる構成となっている。
【０００９】
しかしながら、この特許文献３のような構成では、周辺の磁場環境の影響により、磁気方位センサが誤った方位を検出した場合、誤った方向を進行方位として解釈してしまうことになる。また、無条件で地磁気センサが検出する方位を進行方位として解釈することから、進行方向と磁気方位センサの軸方向が異なる運動状態の場合には、誤った方向を進行方向であると解釈してしまうという問題がある。
【００１０】
かかる地磁気センサの問題点を考慮したものとしては、ＧＰＳ受信機を使用したものが知られている（特許文献４）。
【００１１】
特許文献４では、現時点で移動体が直面している状況に応じて最も信頼性の高い推定値を選択して絶対方位参照値としており、ある条件を満足するときには、磁気コンパスで検出した絶対方位を絶対方位参照値としており、他の条件を満足するときには、ＧＰＳ受信機で検出した進行方向方位を絶対方位参照値としている。
【００１２】
【特許文献１】特許第３０３８４５２号公報
【特許文献２】特開平６−２０１８６３号公報
【特許文献３】特開平１１−９４５７３号公報
【特許文献４】特開２００２−１５８５２５号公報（段落番号００４６、図３）
【発明の開示】
【発明が解決しようとする課題】
【００１３】
しかしながら、特許文献４の絶対方位参照値の決定では、条件の変化に応じて、磁気コンパスで検出した絶対方位からＧＰＳ受信機で検出した進行方向方位に参照値が変化するために、参照値の変動が激しく、参照値としての信頼性に欠けるという問題がある。
【００１４】
本発明は、かかる課題に鑑みなされたもので、ＧＰＳ手段で検出するＧＰＳ方向と磁気方位センサで検出する磁気方位とを用いて、信頼性の高い方位誤差修正を行なうことができる方位姿勢検出装置を提供することをその目的とする。
【００１５】
また、別の目的としては、加速度計で検出する加速度を用いてレベル誤差修正を行なう場合に、簡単な構成で低コストで、信頼性の高いレベル誤差修正を行なうことができる方位姿勢検出装置を提供することである。
【課題を解決するための手段】
【００１６】
上記課題を解決するために、本発明のうち請求項１記載の発明は、３軸まわりの角速度をそれぞれ検出するジャイロと、３軸方向の加速度をそれぞれ検出する加速度計と、ＧＰＳを使用して移動体の移動方向を検出するＧＰＳ手段と、磁気方位角を検出する磁気方位センサと、ジャイロから出力される角速度信号、加速度計から出力される加速度信号、ＧＰＳ手段から出力されるＧＰＳ方向信号及び磁気方位センサから出力される磁気方位信号が入力されてロール角、ピッチ角及び方位角の演算を行う演算手段と、を有し、移動体に搭載されて移動体の方位姿勢を検出する方位姿勢検出装置において、
前記演算手段は、
前記ジャイロ信号を用いて、ジャイロ及び加速度計が取り付けられたボディ座標系からローカル座標系への座標変換行列を逐次求めて更新する座標変換行列更新部と、
前記座標変換行列更新部からの座標変換行列を用いて前記加速度信号の座標変換を行う座標変換部と、
座標変換部で変換された加速度信号を用いて、レベル誤差修正値を演算するレベル誤差修正値演算部と、
ＧＰＳ方向信号及び磁気方位信号を用いて、方位誤差修正値を演算する方位誤差修正値演算部と、
レベル誤差修正値及び方位誤差修正値によって修正された前記座標変換行列からロール角、ピッチ角及び方位角の演算を行う姿勢方位角演算部と、
を備え、前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高い条件下においては、ＧＰＳ方向信号を用いて方位誤差修正値を求めると共に、ＧＰＳ方向信号と磁気方位信号との差異を演算し、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下では、前記演算していたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号とを用いて方位誤差修正値を求めることを特徴とする。
【００１７】
請求項２記載の発明は、請求項１記載のものにおいて、前記ＧＰＳ方向信号の信頼性が高い条件は、移動体の速度が所定値以上であること、求めた方位角の変化が第２所定値以下であること、ＧＰＳ方向信号と磁気方位信号との差異が第３所定値以下であること、の少なくとも１つを含むことを特徴とする。
【００１８】
請求項３記載の発明は、請求項１または２記載の前記方位誤差修正値演算部が、ＧＰＳ方向信号の信頼性が高い条件下においては、方位角とＧＰＳ方向との差を表す値を方位誤差修正値とすることを特徴とする。
【００１９】
請求項４記載の発明は、請求項１ないし３のいずれか１項に記載の前記方位誤差修正値演算部が、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下においては、方位角と、磁気方位角から前記差異に基づく値を差し引いた値との差を表す値を方位誤差修正値とすることを特徴とする。
【００２０】
請求項５記載の発明は、請求項１ないし４のいずれか１項に記載の前記レベル誤差修正演算部が、ＧＰＳ手段から出力されるＧＰＳ速度信号を用いて、加速度信号からの加速度から運動加速度を除去して、レベル誤差修正値を求める、ことを特徴とする。
【発明の効果】
【００２１】
本発明によれば、ＧＰＳ方向信号の信頼性が高い条件下では、ＧＰＳ方向信号を用いて方位誤差修正を行うと共に、ＧＰＳ方向信号と磁気方位信号との差異を演算しておき、ＧＰＳ方向信号の信頼性が高くない条件下では、磁気方位信号のみを使用するのではなく、ＧＰＳ方向信号の信頼性が高いときに演算しておいたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号を用いて方位誤差修正を行うようにしているので、ＧＰＳ方向信号の信頼性が高いときと高くないときとで、参照とするべき信号が大きく変化することなく、周辺の磁場環境の影響があったとしても、その影響を少なくすることができ、より正確に方位誤差の修正を行うことができるようになる。
【００２２】
請求項２記載の発明によれば、ＧＰＳ方向信号の信頼性が高い条件として、移動体の速度、方位変化、またはＧＰＳ方向信号と磁気方位信号との差異を用いることにより、ＧＰＳ方向信号の信頼性を判別することができる。
【００２３】
請求項３記載の発明によれば、ＧＰＳ方向信号の信頼性が高いときに、方位角をＧＰＳ方位に追従させる処理を行なうことができる。
【００２４】
請求項４記載の発明によれば、ＧＰＳ方向信号の信頼性が高くないときに、方位角を磁気方位に磁気方位角から前記差異に基づく値を差し引いた値に追従させる処理を行なうことができる。
【００２５】
請求項５記載の発明によれば、加速度計によって検出される加速度において、運動加速度の影響を除去するのにＧＰＳ手段によって得られるＧＰＳ速度を使用することによって、簡単な構成で低コストでレベル誤差修正を行なうことができる。
【発明を実施するための最良の形態】
【００２６】
以下、図面を用いて本発明の実施の形態を説明する。
【００２７】
図１は、本発明の実施形態による方位姿勢検出装置のブロック図である。方位姿勢検出装置１０は、Ｘ軸まわりの角速度を検出するＸジャイロ１１Ｇ、Ｙ軸まわりの角速度を検出するＹジャイロ１２Ｇ、Ｚ軸まわりの角速度を検出するＺジャイロ１３Ｇ、Ｘ軸方向の加速度を検出するＸ加速度計１１Ａ、Ｙ軸方向の加速度を検出するＹ加速度計１２Ａ、Ｚ軸方向の加速度を検出するＺ加速度計１３Ａ、ＧＰＳアンテナ１４Ａ、ＧＰＳアンテナ１４Ａに接続されるＧＰＳ受信機１４、磁気方位センサ１５、ＣＰＵ、ＲＯＭ、ＲＡＭからなる制御部によって構成されるデータ演算部１６及び表示部１８を備える。Ｘジャイロ１１Ｇ、Ｙジャイロ１２Ｇ、Ｚジャイロ１３Ｇは、それぞれ検出方向が互いに直交するようにして方位姿勢検出装置１０に固定される。同様に、Ｘ加速度計１１Ａ、Ｙ加速度計１２Ａ、Ｚ加速度計１３Ａは、それぞれ検出方向が互いに直交するように且つ対応するジャイロの検出方向が一致するようにして方位姿勢検出装置１０に固定される。
【００２８】
Ｘジャイロ１１Ｇ、Ｙジャイロ１２Ｇ、及びＺジャイロ１３Ｇからの角速度信号、Ｘ加速度計１１Ａ、Ｙ加速度計１２Ａ、及びＺ加速度計１３Ａからの加速度信号、並びに磁気方位センサ１５からの磁気方位信号はデータ演算部１６へと送られ、またＧＰＳ受信機１４からのジャイロ速度信号及びジャイロ方向信号も、所定の周期でデータ演算部１６へと送られる。
【００２９】
データ演算部１６は、上記信号を受信して姿勢角及び方位角を求めるが、そのために、ソフトウエアで構成される座標変換行列更新部２０と、座標変換部２２と、レベル誤差修正値演算部２４と、方位誤差修正値演算部２６と、姿勢方位角演算部２８と、を備えている。
【００３０】
図２に示すように、座標変換行列更新部２０はジャイロ信号を用いて、ジャイロ１１〜１３Ｇ及び加速度計１１〜１３Ａが取り付けられたボディ座標系からローカル座標系への座標変換行列を逐次求めて更新するものであり、座標変換部２２は、該座標変換行列を用いて加速度信号の座標変換を行うものであり、レベル誤差修正値演算部２４は、該座標変換された加速度信号を用いてレベル誤差修正値を演算するものであり、方位誤差修正値演算部２６は、ＧＰＳ方向信号及び磁気方位信号を用いて方位誤差修正値を演算するものであり、レベル誤差修正値及び方位誤差修正値で座標変換行列更新部２０は座標変換行列の修正を行っており、姿勢方位角演算部２８は、修正された座標変換行列からロール角、ピッチ角、方位角を出力している。
【００３１】
以下、各部で行われる処理について、数式を用いて詳細に説明する。
（１）座標変換行列の更新
座標変換行列更新部２０で行われる座標変換行列の更新とは、ボディ座標系とローカル座標系の関係を時々刻々計算することである。具体的な計算方法は任意の方法を採用することができるが、この実施形態においては、以下に概要を示すクォータニオンを使用している。
【００３２】
クォータニオン＜ｑ＞は次式で表される。スカラー成分λとベクトル成分＜ρ＞から成る。尚、以下、ベクトルは、＜＞を挟むことによって表すこととする。
【００３３】
【数１】
ここで、＜ｉ＞，＜ｊ＞，＜ｋ＞は単位クォータニオンであり、以下の性質を持つ。
【００３４】
【数２】
この性質より、クォータニオンｑ1とｑ2の積は下式で表せる。
【００３５】
【数３】
式中の「・」はベクトルの内積、「×」はベクトルの外積を表す。クォータニオンの和、差は通常のベクトル演算と同じであり、各成分の和および差の演算となる。
【００３６】
クォータニオンの共役数＜ｑ＞’は次式で表される。
【００３７】
【数４】
また、式３、式４よりクォータニオン積の共役は下式となることが導かれる。
【００３８】
【数５】
あるｂ座標系のベクトル＜Ｖｂ＞をあるａ座標系に回転座標変換したベクトルを＜Ｖａ＞としたとき、座標変換にクォータニオンを用いた場合の計算式は次のようになる。
【００３９】
【数６】
この式６の性質により、回転の合成は式３に示したクォータニオン積で表せることになる。
【００４０】
式６を成分で展開し、座標変換行列Ｃを用いて、＜Ｖａ＞＝Ｃ＜Ｖｂ＞とし、座標変換行列Ｃの要素を
【００４１】
【数７】
とすると、各要素は、次のように表される。
【００４２】
【数８】
【００４３】
方位姿勢角はローカル座標系に対するボディ座標系の回転角として定義される。まず、ボディ座標系をローカル座標系に一致させた状態から次の順番で回転角を定義する。
・まず、Ｚ軸まわりに方位角ξだけ回転させる。
・次に、Ｙ軸まわりにピッチ角θだけ回転させる。
・最後にＸ軸まわりにロール角φだけ回転させる。
このとき、ボディ座標系からローカル座標系へ座標変換行列の各要素は次のように表される。
【００４４】
【数９】
【００４５】
クォータニオンから方位姿勢角を求めるときは、（式８）〜（式２５）より、次式を用いればよいことが分かる。
【００４６】
【数１０】
【００４７】
以上に概要を示したクォータニオンを用いると、式６より、ボディ座標系のあるベクトル＜Ｖbody＞をローカル座標系に座標変換したベクトルを＜Ｖlocal＞、ボディ座標系→ローカル座標系の回転座標変換を表すクォータニオンを＜ｑbl＞としたとき、＜Ｖbody＞と＜Ｖlocal＞の関係は下式で表せる。
【００４８】
【数１１】
【００４９】
座標変換行列の更新は、下式に示すように、クォータニオン＜ｑbl＞を、クォータニオン積による回転の合成で更新する。この更新は、ジャイロ出力の計測周期（Δｔ）で行われる。
【００５０】
【数１２】
【００５１】
ここで、＜ｑb'l'（ｔ−Δｔ）＞は、前回計測時に計算された、ボディ座標系→ローカル座標系の回転座標変換を表すクォータニオンのことである。
（ア）＜ｑbb'（ｔ）＞は、ボディ座標系の更新であるが、ジャイロが検出したサンプリング周期の変化角度（角速度より計算）により更新される。
（イ）＜ｑl’l（ｔ）＞は、ローカル座標系の更新であるが、レベリング誤差および方位誤差の修正演算（後に、（３），（４）で示す）により更新される。
【００５２】
（ア）の更新において、通常ジャイロの検出する角速度は、Ｘ、Ｙ、Ｚの３軸方向ある。
ただし、各軸で使用する角度（角速度）は、センサの誤差（バイアス誤差、スケールファクタ誤差等）を既に補正した値を使用する。
【００５３】
（イ）の更新においても、修正演算によって求められる回転は３軸方向（北、東、鉛直）ある。北向き、東向き、鉛直下向きの修正角度を各々Φｎ(ｔ)、Φｅ（ｔ）、Φｚ（ｔ）とすると、実際には下式に示すように１次展開近似を用いて表す。
【００５４】
【数１３】
Φｎ(ｔ)、Φｅ（ｔ）、Φｚ（ｔ）の演算は後述する。
【００５５】
（２）座標変換
座標変換部２２で行われる座標変換とは、（１）の座標変換行列更新部２０で求めたクォータニオンを用いて、式２９によりボディ座標系における任意のベクトルをローカル座標系に座標変換することである。即ち、加速度計１１Ａ、１２Ａ、１３Ａにより検出されたボディ座標系加速度ベクトル＜Ａbody＞（Ｘ加速度、Ｙ加速度、Ｚ加速度）を、次式によりローカル座標系加速度ベクトル＜Ａlocal＞（東西加速度、南北加速度、鉛直加速度）に座標変換する。
【００５６】
【数１４】
【００５７】
（３）レベル誤差修正値計算
レベル誤差修正値演算部２４は、（２）の座標変換部２２で求めたローカル座標系加速度のうち、東西加速度および南北加速度の重力加速度成分をゼロに収束するように、即ちローカル座標系が水平になるように、式３１におけるΦｎ（ｔ）、Φｅ（ｔ）（ローカル座標系の北および東方向の修正角度）を求めるものである。
【００５８】
具体的にレベル誤差修正値演算部２４で行う演算は、（２）の座標変換部２２で求めた、東西加速度、南北加速度から、ＧＰＳ受信機１４より取得したＧＰＳ速度をリファレンス速度として用いて運動加速度を除いた結果のフィルタ値に、フィードバックゲイン定数を掛けてレベル誤差修正値を得るものである。尚、このときのローカル系の東西南北とリファレンス（ＧＰＳ）速度の東西南北は、以下の（４）の方位誤差修正値演算部２６に示す方位誤差修正により、一致するよう制御されている。図３に示すように、フィルタ１段目Ｆ１と、フィルタ２段目Ｆ２を構成するように演算を行う。
【００５９】
実際の計算式を以下に示す。
【００６０】
時刻ｔにおける、フィルタ１段目Ｆ１に入力するローカル加速度(南北、東西)を＜Ａin(t)）＞、リファレンス（ＧＰＳ）速度を＜Ｖgps(t)＞、クォータニオン更新周期をΔｔ、フィルタ時定数をＴｆとすると、フィルタ１段目Ｆ１より出力するローカル加速度(南北、東西）＜Ａout(t)＞は、
【００６１】
【数１５】
で表される。
【００６２】
このように、運動加速度の影響を除去するのにリファレンス速度としてＧＰＳ速度を用いて除去することにより、簡単に且つ低コストで構成することができる。
【００６３】
フィルタ２段目Ｆ２の計算内容は式３３のリファレンス速度の項を除いたものである。フィルタ２段目に関しては、本制御演算に関して本質的な内容ではなく、省略可能であるが、加速度計ノイズの影響を減少させる、即ち、加速度計の高周波入力に対する応答を遅らせる為のものである。但しこのフィルタ２段目Ｆ２によりフィードバック系の特性(応答、安定条件等)は影響を受ける。
【００６４】
フィルタ２段目Ｆ２に＜Ａout(t)＞を入力した出力を＜Ａf(t)＞とする（各要素はＡfｎ(t)：南北方向成分、Ａfe(t)：東西方向成分）。この、＜Ａf(t)＞には、傾斜誤差（ローカル系が水平でない誤差）による重力加速度の成分と、フィルタ１段目Ｆ１における運動加速度補正の残差が含まれることになる。この＜Ａf(t)＞が０に収束するように式３１におけるΦｎ（ｔ）：ローカル座標系の北軸回りの修正角度、Φｅ（ｔ）：ローカル座標系の東軸回りの修正角度、を下式により計算する。
【００６５】
【数１６】
尚、上式中、Ｗ0・cos(lat(t))は、アースレート(地球の自転)による角速度入力の修正項であり、Ｗ0は地球の自転角速度、lat(t)は緯度である。但しアースレートによる角速度入力の修正項は、ジャイロ誤差に比較すると小さいので、省略することも可能である。
【００６６】
（４）方位誤差修正値計算
次に、方位誤差修正値演算部２６で行う方位誤差の修正は、ア）ＧＰＳ受信機１４からのＧＰＳ方向（direction : 運動方向）に追従させる状態と、イ）磁気方位センサ１５からの磁気方位に（基準(北)はＧＰＳ方向基準と合わせる）追従させる状態と、の２状態を切替えて行う。これらの状態遷移を示すフローチャートを図４に示す。
【００６７】
ア） ＧＰＳ方向に追従させる状態
ＧＰＳ受信機１４から得られるＧＰＳ速度が閾値以上であり（ステップＳ１２）、方位角変化が閾値以下であり（ステップＳ１４）、ＧＰＳ方向と磁気方位との差が閾値以下である（ステップＳ１６）場合に、ＧＰＳ方向の信頼性が高いと判定して、ＧＰＳ方向を追従する処理を行う（ステップＳ２０）。これらの条件を満足しているときに、ＧＰＳ方向と本方位姿勢検出装置１０のある軸とが一致しているとみなせると考えられるからである。これらの条件以外、またはこれらの条件のいずれか少なくとも１つの条件を満足するときにステップＳ２０に進むようにしてもよい。
【００６８】
具体的には、（方位−ＧＰＳ方向）を方位誤差とみなし、この方位誤差が０に収束するように、式３１におけるΦｚ（ｔ）（ローカル座標系の鉛直方向の修正角度）を計算する。
【００６９】
【数１７】
ここで、Ｔgps は収束の時定数、Δｔはクォータニオン更新周期である。
【００７０】
また、この時、次の磁気方位センサ１５を追従する状態に備えて、ＧＰＳ方向−磁気方位の１次フィルタ値を計算しておく。
【００７１】
計算式は、式３３のリファレンス速度の項を除いたものと同等の内容であり、フィルタ入力値Ｆin(n)＝ＧＰＳ方向(n)−磁気方位(n)、フィルタを出力値Ｆout(n)とすると、
【００７２】
【数１８】
で表される。
【００７３】
イ）磁気方位に追従させる状態
図４のフローチャートにおいて、ＧＰＳ受信機１４から得られるＧＰＳ速度が閾値より小さく、方位角変化が閾値より大きく、またはＧＰＳ方向と磁気方位の差が閾値以上である場合には、ＧＰＳ方向の信頼性よりも磁気方位の信頼性が高いと判定して、磁気方位を追従する処理を行なう。但し、このときに、完全に磁気方位に追従させるのではなく、ＧＰＳ方向を追従する処理を行なっているときに計算していたＧＰＳ方向−磁気方位の１次フィルタ値を用いる。即ち、(方位−磁気方位)−(ＧＰＳ方向−磁気方位の１次フィルタ値)を方位誤差とみなし、この方位誤差が０に収束するように、式３１におけるΦｚ(t)（ローカル座標系の鉛直方向の修正角度）を計算する。
【００７４】
【数１９】
ここで、Ｔmag は収束の時定数、Δｔはクォータニオン更新周期である。
【００７５】
ＧＰＳ方向−磁気方位のフィルタ値は、起動直後はＧＰＳデータ中の磁気偏角（magnetic variation）、または不揮発メモリに記憶した前回動作時の計算値により初期化される。一度でもア）のＧＰＳ方向に追従する状態になった後は、ア）で計算した最新値を使用する。
【００７６】
このように磁気方位センサからの磁気方位にのみ追従させるのではなく、ＧＰＳ方向に追従するときに求めていたＧＰＳ方位と磁気方位との差分の最新値を、磁気方位に加算することにより、周囲の磁場の影響や、磁気方位と真方位との差である磁気偏角が使用場所や時間経過に伴い変化することによる影響により含まれるおそれのある磁気方位の誤差を低減させることができる。また、ア）のＧＰＳ方向追従とこのイ）の磁気方位追従とが切り替えられたときに、追従させるリファレンス方位が大きく変化することを防ぐことができ、システムの安定性を持たせることができる。
【００７７】
（５）姿勢・方位角計算
姿勢方位角演算部２８では、式２６，２７，２８を用いて、クォータニオンより、姿勢角（ロール、ピッチ）および方位角を計算し、表示部１８に更新周期で表示出力する。
【００７８】
以上の処理によって、レベル誤差及び方位誤差が修正された座標変換行列からロール角、ピッチ角及び方位角を出力することができる。
【図面の簡単な説明】
【００７９】
【図１】本発明の実施形態を表す方位姿勢検出装置のブロック図である。
【図２】図１の方位姿勢検出装置のデータ演算部の処理を表すブロック図である。
【図３】レベル誤差修正値演算部での処理を表すブロック図である。
【図４】方位誤差修正値演算部での処理を表すフローチャートである。
【図５】方位誤差修正値演算部での処理を表すブロック図である。
【符号の説明】
【００８０】
１０ 方位姿勢検出装置
１１Ｇ Ｘジャイロ
１２Ｇ Ｙジャイロ
１３Ｇ Ｚジャイロ
１１Ａ Ｘ加速度計
１２Ａ Ｙ加速度計
１３Ａ Ｚ加速度計
１４ ＧＰＳ受信機（ＧＰＳ手段）
１５ 磁気方位センサ
１６ データ演算部
２０ 座標変換行列更新部
２２ 座標変換部
２４ レベル誤差修正値演算部
２６ 方位誤差修正値演算部
２８ 姿勢方位角演算部
【技術分野】
【０００１】
本発明は、船舶、自動車等の移動体の方位姿勢を検出するストラップダウン方式による方位姿勢検出装置に関する。
【背景技術】
【０００２】
従来、ストラップダウン方式による方位姿勢検出装置では、３軸方向の加速度を検出する３軸加速度計と、３軸方向まわりの角速度を検出する３軸ジャイロとを備えている（特許文献１）。
【０００３】
この方位姿勢検出装置では、ジャイロから求められるジャイロ信号を用いて、センサが取り付けられた機体の座標系（ボディ座標系という）からＸ軸方向が北方向で且つＺ軸方向が重力方向に一致している座標系（ローカル座標系という）への変換する行列を演算し、この行列からローカル座標系に対する各軸回りの回転角である方位角、ピッチ角及びロール角を求めて方位姿勢を出力すると共に、ジャイロ誤差に起因する姿勢誤差を、加速度計により検出された重力加速度検知により修正する構成となっている。
【０００４】
しかしながら、加速度計は重力以外の運動加速度も検知するため、運動加速度の影響を受けて姿勢誤差が発生してしまう、という問題がある。高価で高性能なジャイロ及び加速度計を使用すれば運動加速度の影響を受けにくい方位姿勢検出装置を構成することもできるが、低価格、低精度なジャイロまたは加速度を使用した場合の影響は大きく、運動状態によっては実用に耐えないものになってしまう、という問題がある。
【０００５】
かかる運動加速度の影響を除去するための構成としては、例えば、Ｘ軸方向の速度を検出するＸ速度計を備え、加速度誤差を修正するようにしたものが知られている（特許文献２）。
【０００６】
特許文献２では、移動体が旋回運動を行なう場合に、旋回時に生じる遠心力を移動体が支持できなくなることにより、進行方向速度と実際の軌跡速度とが一致しなくなる横滑りが発生するが、Ｘ軸方向の速度しか検出できず、Ｙ軸方向の速度による加速度成分の修正ができないことによる横滑り誤差が姿勢角の誤差をもたらすことを防止するために、方位角信号とＸ軸方向の速度信号から横滑り角を演算によって求めることが提案されている。
【０００７】
しかしながら、かかる特許文献２の演算を行なうためには、速度計を要し、また、横滑り角演算のための演算装置を要するため、装置が大型化し、高コストになるという問題がある。
【０００８】
また、方位角の補正に関しては、従来、磁気方位センサにより検出された方位信号を用いることが一般的に行われている（特許文献１，３）。例えば、特許文献３では、ジャイロによって検出した走行方位角の修正において、地磁気センサが検出する方位を進行方位であるとして、その地磁気センサが検出する方位にジャイロが検出する方位を追従させる構成となっている。
【０００９】
しかしながら、この特許文献３のような構成では、周辺の磁場環境の影響により、磁気方位センサが誤った方位を検出した場合、誤った方向を進行方位として解釈してしまうことになる。また、無条件で地磁気センサが検出する方位を進行方位として解釈することから、進行方向と磁気方位センサの軸方向が異なる運動状態の場合には、誤った方向を進行方向であると解釈してしまうという問題がある。
【００１０】
かかる地磁気センサの問題点を考慮したものとしては、ＧＰＳ受信機を使用したものが知られている（特許文献４）。
【００１１】
特許文献４では、現時点で移動体が直面している状況に応じて最も信頼性の高い推定値を選択して絶対方位参照値としており、ある条件を満足するときには、磁気コンパスで検出した絶対方位を絶対方位参照値としており、他の条件を満足するときには、ＧＰＳ受信機で検出した進行方向方位を絶対方位参照値としている。
【００１２】
【特許文献１】特許第３０３８４５２号公報
【特許文献２】特開平６−２０１８６３号公報
【特許文献３】特開平１１−９４５７３号公報
【特許文献４】特開２００２−１５８５２５号公報（段落番号００４６、図３）
【発明の開示】
【発明が解決しようとする課題】
【００１３】
しかしながら、特許文献４の絶対方位参照値の決定では、条件の変化に応じて、磁気コンパスで検出した絶対方位からＧＰＳ受信機で検出した進行方向方位に参照値が変化するために、参照値の変動が激しく、参照値としての信頼性に欠けるという問題がある。
【００１４】
本発明は、かかる課題に鑑みなされたもので、ＧＰＳ手段で検出するＧＰＳ方向と磁気方位センサで検出する磁気方位とを用いて、信頼性の高い方位誤差修正を行なうことができる方位姿勢検出装置を提供することをその目的とする。
【００１５】
また、別の目的としては、加速度計で検出する加速度を用いてレベル誤差修正を行なう場合に、簡単な構成で低コストで、信頼性の高いレベル誤差修正を行なうことができる方位姿勢検出装置を提供することである。
【課題を解決するための手段】
【００１６】
上記課題を解決するために、本発明のうち請求項１記載の発明は、３軸まわりの角速度をそれぞれ検出するジャイロと、３軸方向の加速度をそれぞれ検出する加速度計と、ＧＰＳを使用して移動体の移動方向を検出するＧＰＳ手段と、磁気方位角を検出する磁気方位センサと、ジャイロから出力される角速度信号、加速度計から出力される加速度信号、ＧＰＳ手段から出力されるＧＰＳ方向信号及び磁気方位センサから出力される磁気方位信号が入力されてロール角、ピッチ角及び方位角の演算を行う演算手段と、を有し、移動体に搭載されて移動体の方位姿勢を検出する方位姿勢検出装置において、
前記演算手段は、
前記ジャイロ信号を用いて、ジャイロ及び加速度計が取り付けられたボディ座標系からローカル座標系への座標変換行列を逐次求めて更新する座標変換行列更新部と、
前記座標変換行列更新部からの座標変換行列を用いて前記加速度信号の座標変換を行う座標変換部と、
座標変換部で変換された加速度信号を用いて、レベル誤差修正値を演算するレベル誤差修正値演算部と、
ＧＰＳ方向信号及び磁気方位信号を用いて、方位誤差修正値を演算する方位誤差修正値演算部と、
レベル誤差修正値及び方位誤差修正値によって修正された前記座標変換行列からロール角、ピッチ角及び方位角の演算を行う姿勢方位角演算部と、
を備え、前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高い条件下においては、ＧＰＳ方向信号を用いて方位誤差修正値を求めると共に、ＧＰＳ方向信号と磁気方位信号との差異を演算し、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下では、前記演算していたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号とを用いて方位誤差修正値を求めることを特徴とする。
【００１７】
請求項２記載の発明は、請求項１記載のものにおいて、前記ＧＰＳ方向信号の信頼性が高い条件は、移動体の速度が所定値以上であること、求めた方位角の変化が第２所定値以下であること、ＧＰＳ方向信号と磁気方位信号との差異が第３所定値以下であること、の少なくとも１つを含むことを特徴とする。
【００１８】
請求項３記載の発明は、請求項１または２記載の前記方位誤差修正値演算部が、ＧＰＳ方向信号の信頼性が高い条件下においては、方位角とＧＰＳ方向との差を表す値を方位誤差修正値とすることを特徴とする。
【００１９】
請求項４記載の発明は、請求項１ないし３のいずれか１項に記載の前記方位誤差修正値演算部が、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下においては、方位角と、磁気方位角から前記差異に基づく値を差し引いた値との差を表す値を方位誤差修正値とすることを特徴とする。
【００２０】
請求項５記載の発明は、請求項１ないし４のいずれか１項に記載の前記レベル誤差修正演算部が、ＧＰＳ手段から出力されるＧＰＳ速度信号を用いて、加速度信号からの加速度から運動加速度を除去して、レベル誤差修正値を求める、ことを特徴とする。
【発明の効果】
【００２１】
本発明によれば、ＧＰＳ方向信号の信頼性が高い条件下では、ＧＰＳ方向信号を用いて方位誤差修正を行うと共に、ＧＰＳ方向信号と磁気方位信号との差異を演算しておき、ＧＰＳ方向信号の信頼性が高くない条件下では、磁気方位信号のみを使用するのではなく、ＧＰＳ方向信号の信頼性が高いときに演算しておいたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号を用いて方位誤差修正を行うようにしているので、ＧＰＳ方向信号の信頼性が高いときと高くないときとで、参照とするべき信号が大きく変化することなく、周辺の磁場環境の影響があったとしても、その影響を少なくすることができ、より正確に方位誤差の修正を行うことができるようになる。
【００２２】
請求項２記載の発明によれば、ＧＰＳ方向信号の信頼性が高い条件として、移動体の速度、方位変化、またはＧＰＳ方向信号と磁気方位信号との差異を用いることにより、ＧＰＳ方向信号の信頼性を判別することができる。
【００２３】
請求項３記載の発明によれば、ＧＰＳ方向信号の信頼性が高いときに、方位角をＧＰＳ方位に追従させる処理を行なうことができる。
【００２４】
請求項４記載の発明によれば、ＧＰＳ方向信号の信頼性が高くないときに、方位角を磁気方位に磁気方位角から前記差異に基づく値を差し引いた値に追従させる処理を行なうことができる。
【００２５】
請求項５記載の発明によれば、加速度計によって検出される加速度において、運動加速度の影響を除去するのにＧＰＳ手段によって得られるＧＰＳ速度を使用することによって、簡単な構成で低コストでレベル誤差修正を行なうことができる。
【発明を実施するための最良の形態】
【００２６】
以下、図面を用いて本発明の実施の形態を説明する。
【００２７】
図１は、本発明の実施形態による方位姿勢検出装置のブロック図である。方位姿勢検出装置１０は、Ｘ軸まわりの角速度を検出するＸジャイロ１１Ｇ、Ｙ軸まわりの角速度を検出するＹジャイロ１２Ｇ、Ｚ軸まわりの角速度を検出するＺジャイロ１３Ｇ、Ｘ軸方向の加速度を検出するＸ加速度計１１Ａ、Ｙ軸方向の加速度を検出するＹ加速度計１２Ａ、Ｚ軸方向の加速度を検出するＺ加速度計１３Ａ、ＧＰＳアンテナ１４Ａ、ＧＰＳアンテナ１４Ａに接続されるＧＰＳ受信機１４、磁気方位センサ１５、ＣＰＵ、ＲＯＭ、ＲＡＭからなる制御部によって構成されるデータ演算部１６及び表示部１８を備える。Ｘジャイロ１１Ｇ、Ｙジャイロ１２Ｇ、Ｚジャイロ１３Ｇは、それぞれ検出方向が互いに直交するようにして方位姿勢検出装置１０に固定される。同様に、Ｘ加速度計１１Ａ、Ｙ加速度計１２Ａ、Ｚ加速度計１３Ａは、それぞれ検出方向が互いに直交するように且つ対応するジャイロの検出方向が一致するようにして方位姿勢検出装置１０に固定される。
【００２８】
Ｘジャイロ１１Ｇ、Ｙジャイロ１２Ｇ、及びＺジャイロ１３Ｇからの角速度信号、Ｘ加速度計１１Ａ、Ｙ加速度計１２Ａ、及びＺ加速度計１３Ａからの加速度信号、並びに磁気方位センサ１５からの磁気方位信号はデータ演算部１６へと送られ、またＧＰＳ受信機１４からのジャイロ速度信号及びジャイロ方向信号も、所定の周期でデータ演算部１６へと送られる。
【００２９】
データ演算部１６は、上記信号を受信して姿勢角及び方位角を求めるが、そのために、ソフトウエアで構成される座標変換行列更新部２０と、座標変換部２２と、レベル誤差修正値演算部２４と、方位誤差修正値演算部２６と、姿勢方位角演算部２８と、を備えている。
【００３０】
図２に示すように、座標変換行列更新部２０はジャイロ信号を用いて、ジャイロ１１〜１３Ｇ及び加速度計１１〜１３Ａが取り付けられたボディ座標系からローカル座標系への座標変換行列を逐次求めて更新するものであり、座標変換部２２は、該座標変換行列を用いて加速度信号の座標変換を行うものであり、レベル誤差修正値演算部２４は、該座標変換された加速度信号を用いてレベル誤差修正値を演算するものであり、方位誤差修正値演算部２６は、ＧＰＳ方向信号及び磁気方位信号を用いて方位誤差修正値を演算するものであり、レベル誤差修正値及び方位誤差修正値で座標変換行列更新部２０は座標変換行列の修正を行っており、姿勢方位角演算部２８は、修正された座標変換行列からロール角、ピッチ角、方位角を出力している。
【００３１】
以下、各部で行われる処理について、数式を用いて詳細に説明する。
（１）座標変換行列の更新
座標変換行列更新部２０で行われる座標変換行列の更新とは、ボディ座標系とローカル座標系の関係を時々刻々計算することである。具体的な計算方法は任意の方法を採用することができるが、この実施形態においては、以下に概要を示すクォータニオンを使用している。
【００３２】
クォータニオン＜ｑ＞は次式で表される。スカラー成分λとベクトル成分＜ρ＞から成る。尚、以下、ベクトルは、＜＞を挟むことによって表すこととする。
【００３３】
【数１】
ここで、＜ｉ＞，＜ｊ＞，＜ｋ＞は単位クォータニオンであり、以下の性質を持つ。
【００３４】
【数２】
この性質より、クォータニオンｑ1とｑ2の積は下式で表せる。
【００３５】
【数３】
式中の「・」はベクトルの内積、「×」はベクトルの外積を表す。クォータニオンの和、差は通常のベクトル演算と同じであり、各成分の和および差の演算となる。
【００３６】
クォータニオンの共役数＜ｑ＞’は次式で表される。
【００３７】
【数４】
また、式３、式４よりクォータニオン積の共役は下式となることが導かれる。
【００３８】
【数５】
あるｂ座標系のベクトル＜Ｖｂ＞をあるａ座標系に回転座標変換したベクトルを＜Ｖａ＞としたとき、座標変換にクォータニオンを用いた場合の計算式は次のようになる。
【００３９】
【数６】
この式６の性質により、回転の合成は式３に示したクォータニオン積で表せることになる。
【００４０】
式６を成分で展開し、座標変換行列Ｃを用いて、＜Ｖａ＞＝Ｃ＜Ｖｂ＞とし、座標変換行列Ｃの要素を
【００４１】
【数７】
とすると、各要素は、次のように表される。
【００４２】
【数８】
【００４３】
方位姿勢角はローカル座標系に対するボディ座標系の回転角として定義される。まず、ボディ座標系をローカル座標系に一致させた状態から次の順番で回転角を定義する。
・まず、Ｚ軸まわりに方位角ξだけ回転させる。
・次に、Ｙ軸まわりにピッチ角θだけ回転させる。
・最後にＸ軸まわりにロール角φだけ回転させる。
このとき、ボディ座標系からローカル座標系へ座標変換行列の各要素は次のように表される。
【００４４】
【数９】
【００４５】
クォータニオンから方位姿勢角を求めるときは、（式８）〜（式２５）より、次式を用いればよいことが分かる。
【００４６】
【数１０】
【００４７】
以上に概要を示したクォータニオンを用いると、式６より、ボディ座標系のあるベクトル＜Ｖbody＞をローカル座標系に座標変換したベクトルを＜Ｖlocal＞、ボディ座標系→ローカル座標系の回転座標変換を表すクォータニオンを＜ｑbl＞としたとき、＜Ｖbody＞と＜Ｖlocal＞の関係は下式で表せる。
【００４８】
【数１１】
【００４９】
座標変換行列の更新は、下式に示すように、クォータニオン＜ｑbl＞を、クォータニオン積による回転の合成で更新する。この更新は、ジャイロ出力の計測周期（Δｔ）で行われる。
【００５０】
【数１２】
【００５１】
ここで、＜ｑb'l'（ｔ−Δｔ）＞は、前回計測時に計算された、ボディ座標系→ローカル座標系の回転座標変換を表すクォータニオンのことである。
（ア）＜ｑbb'（ｔ）＞は、ボディ座標系の更新であるが、ジャイロが検出したサンプリング周期の変化角度（角速度より計算）により更新される。
（イ）＜ｑl’l（ｔ）＞は、ローカル座標系の更新であるが、レベリング誤差および方位誤差の修正演算（後に、（３），（４）で示す）により更新される。
【００５２】
（ア）の更新において、通常ジャイロの検出する角速度は、Ｘ、Ｙ、Ｚの３軸方向ある。
ただし、各軸で使用する角度（角速度）は、センサの誤差（バイアス誤差、スケールファクタ誤差等）を既に補正した値を使用する。
【００５３】
（イ）の更新においても、修正演算によって求められる回転は３軸方向（北、東、鉛直）ある。北向き、東向き、鉛直下向きの修正角度を各々Φｎ(ｔ)、Φｅ（ｔ）、Φｚ（ｔ）とすると、実際には下式に示すように１次展開近似を用いて表す。
【００５４】
【数１３】
Φｎ(ｔ)、Φｅ（ｔ）、Φｚ（ｔ）の演算は後述する。
【００５５】
（２）座標変換
座標変換部２２で行われる座標変換とは、（１）の座標変換行列更新部２０で求めたクォータニオンを用いて、式２９によりボディ座標系における任意のベクトルをローカル座標系に座標変換することである。即ち、加速度計１１Ａ、１２Ａ、１３Ａにより検出されたボディ座標系加速度ベクトル＜Ａbody＞（Ｘ加速度、Ｙ加速度、Ｚ加速度）を、次式によりローカル座標系加速度ベクトル＜Ａlocal＞（東西加速度、南北加速度、鉛直加速度）に座標変換する。
【００５６】
【数１４】
【００５７】
（３）レベル誤差修正値計算
レベル誤差修正値演算部２４は、（２）の座標変換部２２で求めたローカル座標系加速度のうち、東西加速度および南北加速度の重力加速度成分をゼロに収束するように、即ちローカル座標系が水平になるように、式３１におけるΦｎ（ｔ）、Φｅ（ｔ）（ローカル座標系の北および東方向の修正角度）を求めるものである。
【００５８】
具体的にレベル誤差修正値演算部２４で行う演算は、（２）の座標変換部２２で求めた、東西加速度、南北加速度から、ＧＰＳ受信機１４より取得したＧＰＳ速度をリファレンス速度として用いて運動加速度を除いた結果のフィルタ値に、フィードバックゲイン定数を掛けてレベル誤差修正値を得るものである。尚、このときのローカル系の東西南北とリファレンス（ＧＰＳ）速度の東西南北は、以下の（４）の方位誤差修正値演算部２６に示す方位誤差修正により、一致するよう制御されている。図３に示すように、フィルタ１段目Ｆ１と、フィルタ２段目Ｆ２を構成するように演算を行う。
【００５９】
実際の計算式を以下に示す。
【００６０】
時刻ｔにおける、フィルタ１段目Ｆ１に入力するローカル加速度(南北、東西)を＜Ａin(t)）＞、リファレンス（ＧＰＳ）速度を＜Ｖgps(t)＞、クォータニオン更新周期をΔｔ、フィルタ時定数をＴｆとすると、フィルタ１段目Ｆ１より出力するローカル加速度(南北、東西）＜Ａout(t)＞は、
【００６１】
【数１５】
で表される。
【００６２】
このように、運動加速度の影響を除去するのにリファレンス速度としてＧＰＳ速度を用いて除去することにより、簡単に且つ低コストで構成することができる。
【００６３】
フィルタ２段目Ｆ２の計算内容は式３３のリファレンス速度の項を除いたものである。フィルタ２段目に関しては、本制御演算に関して本質的な内容ではなく、省略可能であるが、加速度計ノイズの影響を減少させる、即ち、加速度計の高周波入力に対する応答を遅らせる為のものである。但しこのフィルタ２段目Ｆ２によりフィードバック系の特性(応答、安定条件等)は影響を受ける。
【００６４】
フィルタ２段目Ｆ２に＜Ａout(t)＞を入力した出力を＜Ａf(t)＞とする（各要素はＡfｎ(t)：南北方向成分、Ａfe(t)：東西方向成分）。この、＜Ａf(t)＞には、傾斜誤差（ローカル系が水平でない誤差）による重力加速度の成分と、フィルタ１段目Ｆ１における運動加速度補正の残差が含まれることになる。この＜Ａf(t)＞が０に収束するように式３１におけるΦｎ（ｔ）：ローカル座標系の北軸回りの修正角度、Φｅ（ｔ）：ローカル座標系の東軸回りの修正角度、を下式により計算する。
【００６５】
【数１６】
尚、上式中、Ｗ0・cos(lat(t))は、アースレート(地球の自転)による角速度入力の修正項であり、Ｗ0は地球の自転角速度、lat(t)は緯度である。但しアースレートによる角速度入力の修正項は、ジャイロ誤差に比較すると小さいので、省略することも可能である。
【００６６】
（４）方位誤差修正値計算
次に、方位誤差修正値演算部２６で行う方位誤差の修正は、ア）ＧＰＳ受信機１４からのＧＰＳ方向（direction : 運動方向）に追従させる状態と、イ）磁気方位センサ１５からの磁気方位に（基準(北)はＧＰＳ方向基準と合わせる）追従させる状態と、の２状態を切替えて行う。これらの状態遷移を示すフローチャートを図４に示す。
【００６７】
ア） ＧＰＳ方向に追従させる状態
ＧＰＳ受信機１４から得られるＧＰＳ速度が閾値以上であり（ステップＳ１２）、方位角変化が閾値以下であり（ステップＳ１４）、ＧＰＳ方向と磁気方位との差が閾値以下である（ステップＳ１６）場合に、ＧＰＳ方向の信頼性が高いと判定して、ＧＰＳ方向を追従する処理を行う（ステップＳ２０）。これらの条件を満足しているときに、ＧＰＳ方向と本方位姿勢検出装置１０のある軸とが一致しているとみなせると考えられるからである。これらの条件以外、またはこれらの条件のいずれか少なくとも１つの条件を満足するときにステップＳ２０に進むようにしてもよい。
【００６８】
具体的には、（方位−ＧＰＳ方向）を方位誤差とみなし、この方位誤差が０に収束するように、式３１におけるΦｚ（ｔ）（ローカル座標系の鉛直方向の修正角度）を計算する。
【００６９】
【数１７】
ここで、Ｔgps は収束の時定数、Δｔはクォータニオン更新周期である。
【００７０】
また、この時、次の磁気方位センサ１５を追従する状態に備えて、ＧＰＳ方向−磁気方位の１次フィルタ値を計算しておく。
【００７１】
計算式は、式３３のリファレンス速度の項を除いたものと同等の内容であり、フィルタ入力値Ｆin(n)＝ＧＰＳ方向(n)−磁気方位(n)、フィルタを出力値Ｆout(n)とすると、
【００７２】
【数１８】
で表される。
【００７３】
イ）磁気方位に追従させる状態
図４のフローチャートにおいて、ＧＰＳ受信機１４から得られるＧＰＳ速度が閾値より小さく、方位角変化が閾値より大きく、またはＧＰＳ方向と磁気方位の差が閾値以上である場合には、ＧＰＳ方向の信頼性よりも磁気方位の信頼性が高いと判定して、磁気方位を追従する処理を行なう。但し、このときに、完全に磁気方位に追従させるのではなく、ＧＰＳ方向を追従する処理を行なっているときに計算していたＧＰＳ方向−磁気方位の１次フィルタ値を用いる。即ち、(方位−磁気方位)−(ＧＰＳ方向−磁気方位の１次フィルタ値)を方位誤差とみなし、この方位誤差が０に収束するように、式３１におけるΦｚ(t)（ローカル座標系の鉛直方向の修正角度）を計算する。
【００７４】
【数１９】
ここで、Ｔmag は収束の時定数、Δｔはクォータニオン更新周期である。
【００７５】
ＧＰＳ方向−磁気方位のフィルタ値は、起動直後はＧＰＳデータ中の磁気偏角（magnetic variation）、または不揮発メモリに記憶した前回動作時の計算値により初期化される。一度でもア）のＧＰＳ方向に追従する状態になった後は、ア）で計算した最新値を使用する。
【００７６】
このように磁気方位センサからの磁気方位にのみ追従させるのではなく、ＧＰＳ方向に追従するときに求めていたＧＰＳ方位と磁気方位との差分の最新値を、磁気方位に加算することにより、周囲の磁場の影響や、磁気方位と真方位との差である磁気偏角が使用場所や時間経過に伴い変化することによる影響により含まれるおそれのある磁気方位の誤差を低減させることができる。また、ア）のＧＰＳ方向追従とこのイ）の磁気方位追従とが切り替えられたときに、追従させるリファレンス方位が大きく変化することを防ぐことができ、システムの安定性を持たせることができる。
【００７７】
（５）姿勢・方位角計算
姿勢方位角演算部２８では、式２６，２７，２８を用いて、クォータニオンより、姿勢角（ロール、ピッチ）および方位角を計算し、表示部１８に更新周期で表示出力する。
【００７８】
以上の処理によって、レベル誤差及び方位誤差が修正された座標変換行列からロール角、ピッチ角及び方位角を出力することができる。
【図面の簡単な説明】
【００７９】
【図１】本発明の実施形態を表す方位姿勢検出装置のブロック図である。
【図２】図１の方位姿勢検出装置のデータ演算部の処理を表すブロック図である。
【図３】レベル誤差修正値演算部での処理を表すブロック図である。
【図４】方位誤差修正値演算部での処理を表すフローチャートである。
【図５】方位誤差修正値演算部での処理を表すブロック図である。
【符号の説明】
【００８０】
１０ 方位姿勢検出装置
１１Ｇ Ｘジャイロ
１２Ｇ Ｙジャイロ
１３Ｇ Ｚジャイロ
１１Ａ Ｘ加速度計
１２Ａ Ｙ加速度計
１３Ａ Ｚ加速度計
１４ ＧＰＳ受信機（ＧＰＳ手段）
１５ 磁気方位センサ
１６ データ演算部
２０ 座標変換行列更新部
２２ 座標変換部
２４ レベル誤差修正値演算部
２６ 方位誤差修正値演算部
２８ 姿勢方位角演算部
【特許請求の範囲】
【請求項１】
３軸まわりの角速度をそれぞれ検出するジャイロと、３軸方向の加速度をそれぞれ検出する加速度計と、ＧＰＳを使用して移動体の移動方向を検出するＧＰＳ手段と、磁気方位角を検出する磁気方位センサと、ジャイロから出力される角速度信号、加速度計から出力される加速度信号、ＧＰＳ手段から出力されるＧＰＳ方向信号及び磁気方位センサから出力される磁気方位信号が入力されてロール角、ピッチ角及び方位角の演算を行う演算手段と、を有し、移動体に搭載されて移動体の方位姿勢を検出する方位姿勢検出装置において、
前記演算手段は、
前記ジャイロ信号を用いて、ジャイロ及び加速度計が取り付けられたボディ座標系からローカル座標系への座標変換行列を逐次求めて更新する座標変換行列更新部と、
前記座標変換行列更新部からの座標変換行列を用いて前記加速度信号の座標変換を行う座標変換部と、
座標変換部で変換された加速度信号を用いて、レベル誤差修正値を演算するレベル誤差修正値演算部と、
ＧＰＳ方向信号及び磁気方位信号を用いて、方位誤差修正値を演算する方位誤差修正値演算部と、
レベル誤差修正値及び方位誤差修正値によって修正された前記座標変換行列からロール角、ピッチ角及び方位角の演算を行う姿勢方位角演算部と、
を備え、前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高い条件下においては、ＧＰＳ方向信号を用いて方位誤差修正値を求めると共に、ＧＰＳ方向信号と磁気方位信号との差異を演算し、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下では、前記演算していたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号とを用いて方位誤差修正値を求めることを特徴とする方位姿勢検出装置。
【請求項２】
前記ＧＰＳ方向信号の信頼性が高い条件は、移動体の速度が所定値以上であること、求めた方位角の変化が第２所定値以下であること、ＧＰＳ方向信号と磁気方位信号との差異が第３所定値以下であること、の少なくとも１つを含むことを特徴とする請求項１記載の方位姿勢検出装置。
【請求項３】
前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高い条件下においては、方位角とＧＰＳ方向との差を表す値を方位誤差修正値とすることを特徴とする請求項１または２記載の方位姿勢検出装置。
【請求項４】
前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下においては、方位角と、磁気方位角から前記差異に基づく値を差し引いた値との差を表す値を方位誤差修正値とすることを特徴とする請求項１ないし３のいずれか１項に記載の方位姿勢検出装置。
【請求項５】
前記レベル誤差修正演算部は、ＧＰＳ手段から出力されるＧＰＳ速度信号を用いて、加速度信号からの加速度から運動加速度を除去して、レベル誤差修正値を求める、ことを特徴とする請求項１ないし４のいずれか１項に記載の方位姿勢検出装置。
【請求項１】
３軸まわりの角速度をそれぞれ検出するジャイロと、３軸方向の加速度をそれぞれ検出する加速度計と、ＧＰＳを使用して移動体の移動方向を検出するＧＰＳ手段と、磁気方位角を検出する磁気方位センサと、ジャイロから出力される角速度信号、加速度計から出力される加速度信号、ＧＰＳ手段から出力されるＧＰＳ方向信号及び磁気方位センサから出力される磁気方位信号が入力されてロール角、ピッチ角及び方位角の演算を行う演算手段と、を有し、移動体に搭載されて移動体の方位姿勢を検出する方位姿勢検出装置において、
前記演算手段は、
前記ジャイロ信号を用いて、ジャイロ及び加速度計が取り付けられたボディ座標系からローカル座標系への座標変換行列を逐次求めて更新する座標変換行列更新部と、
前記座標変換行列更新部からの座標変換行列を用いて前記加速度信号の座標変換を行う座標変換部と、
座標変換部で変換された加速度信号を用いて、レベル誤差修正値を演算するレベル誤差修正値演算部と、
ＧＰＳ方向信号及び磁気方位信号を用いて、方位誤差修正値を演算する方位誤差修正値演算部と、
レベル誤差修正値及び方位誤差修正値によって修正された前記座標変換行列からロール角、ピッチ角及び方位角の演算を行う姿勢方位角演算部と、
を備え、前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高い条件下においては、ＧＰＳ方向信号を用いて方位誤差修正値を求めると共に、ＧＰＳ方向信号と磁気方位信号との差異を演算し、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下では、前記演算していたＧＰＳ方向信号と磁気方位信号との差異と、磁気方位信号とを用いて方位誤差修正値を求めることを特徴とする方位姿勢検出装置。
【請求項２】
前記ＧＰＳ方向信号の信頼性が高い条件は、移動体の速度が所定値以上であること、求めた方位角の変化が第２所定値以下であること、ＧＰＳ方向信号と磁気方位信号との差異が第３所定値以下であること、の少なくとも１つを含むことを特徴とする請求項１記載の方位姿勢検出装置。
【請求項３】
前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高い条件下においては、方位角とＧＰＳ方向との差を表す値を方位誤差修正値とすることを特徴とする請求項１または２記載の方位姿勢検出装置。
【請求項４】
前記方位誤差修正値演算部は、ＧＰＳ方向信号の信頼性が高くない条件下で且つ磁気方位信号の信頼性が高い条件下においては、方位角と、磁気方位角から前記差異に基づく値を差し引いた値との差を表す値を方位誤差修正値とすることを特徴とする請求項１ないし３のいずれか１項に記載の方位姿勢検出装置。
【請求項５】
前記レベル誤差修正演算部は、ＧＰＳ手段から出力されるＧＰＳ速度信号を用いて、加速度信号からの加速度から運動加速度を除去して、レベル誤差修正値を求める、ことを特徴とする請求項１ないし４のいずれか１項に記載の方位姿勢検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２００６−１２６１４８（Ｐ２００６−１２６１４８Ａ）
【公開日】平成１８年５月１８日（２００６．５．１８）
【国際特許分類】
【出願番号】特願２００４−３１８４３９（Ｐ２００４−３１８４３９）
【出願日】平成１６年１１月１日（２００４．１１．１）
【出願人】（０００００３３８８）株式会社トキメック (103)
【Ｆターム（参考）】
【公開日】平成１８年５月１８日（２００６．５．１８）
【国際特許分類】
【出願日】平成１６年１１月１日（２００４．１１．１）
【出願人】（０００００３３８８）株式会社トキメック (103)
【Ｆターム（参考）】
[ Back to top ]