JPH07239236A

JPH07239236A - 移動体の状態量計測方法と装置および移動体の姿勢角演算装置

Info

Publication number: JPH07239236A
Application number: JP2930194A
Authority: JP
Inventors: Masaru Kikuchi; 勝菊池; Masaki Fujinaga; 正樹藤永; Toshiro Sasaki; 敏郎佐々木
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1994-02-28
Filing date: 1994-02-28
Publication date: 1995-09-12

Abstract

(57)【要約】【目的】複合センサ間の出力タイミングのずれや、ジャ
イロに起因する誤差を低減する移動体の状態量計測方法
及び装置を提供する。【構成】ＧＰＳ受信機１１は移動体の長周期（Ｔ１）で
位置を、ジャイロ１２は移動体の角速度、加速度計１３
は加速度を短周期（Ｔ２）で、計測して出力する。信号
同期部１４は、Ｔ１／Ｔ２の整数比以上のタイミング信
号を発生し、これに同期するように、角速度及び加速度
の値を補正する。状態量演算部１６はタイミング信号の
周期で、補正された加速度データを積分して姿勢角を求
め、補正された加速度を積分して速度を、更に積分して
位置を求める。カルマンフィルタ１５は、ＧＰＳ受信機
１１からの位置信号を観測値とし、演算部１６からの姿
勢角や位置を入力して、Ｔ１の周期毎に移動体の位置等
を推定する。フィルタ１５からの位置信号は状態量演算
部１６の補正に用いられる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は移動体の３次元位置など
の状態量計測装置に関し、特にＧＰＳ（Global Positio
ning System）受信機、ジャイロ、加速度計の複合セン
サによる状態量計測方式に関する。

【０００２】

【従来の技術】複合センサを備えた移動体の３次元位置
の計測装置として、例えば特開昭６３−３０２３１７号
に開示された位置速度測定装置がある。

【０００３】図１１に示すように、この計測装置の慣性
基準装置３０は、ジャイロと加速度計による角速度と加
速度に基づいて、航空機の位置、速度及び姿勢角の信号
３３ａをそれぞれ演算して出力する。一方、ＧＰＳ受信
機３１は、ＧＰＳ衛星から電波を受信している。この電
波には時刻、移動体と衛星との距離等のデータを含み、
これよりＧＰＳ受信機３１は、航空機の３次元位置を測
定し、位置及び速度の信号３３ｄを出力する。

【０００４】カルマンフィルタ３２は、慣性基準装置３
０が出力する航空機の位置及び速度３３ｂと、ＧＰＳ受
信機が出力する航空機の位置及び速度３３ｄを入力し
て、両信号の差を観測ベクトルとして誤差ベクトル３３
ｃを出力する。誤差べクトル３３ｃは、慣性基準装置３
０へ入力されその計測値を補正する。

【０００５】ＧＰＳ受信機３１には、ＧＰＳ受信機で検
出された移動体の位置と、衛星との相対関係に応じた観
測雑音の誤差修正量３３ｅを出力する手段を設けてあ
る。この誤差修正量に従って、カルマンフィルタ３２は
実際の雑音に適合した観測雑音による誤差ベクトル３３
ｃを演算できるので、慣性基準装置３０が出力する位
置、速度に含まれる誤差を低減する。

【０００６】

【発明が解決しようとする課題】上記した従来技術で
は、計測タイミングが長周期のＧＰＳ受信機と短周期の
慣性基準装置の出力値の同期化に関する考慮がない。出
力時刻にずれのある複数センサの出力値を同一タイミン
グと見做して用いると、時刻ずれに起因する出力値の誤
差が発生し、複合化による精度の向上が得られないとい
う問題がある。

【０００７】また、従来技術におけるカルマンフィルタ
は、観測値に姿勢角に関する情報が含まれていないため
に、慣性基準装置の姿勢角出力の誤差を低減することが
難しかった。姿勢角は、慣性基準装置で位置や速度信号
を求める際に、加速度計の検知した加速度の地面固定座
標系への変換等に用いる。このため、位置、速度信号の
精度向上には、姿勢角の誤差を補正することが課題とな
る。

【０００８】本発明の目的は、複合センサ間の出力タイ
ミングのずれに起因する誤差を低減する移動体の状態量
計測方法及び装置を提供することにある。

【０００９】本発明の他の目的は、姿勢角に起因する誤
差を低減する移動体の状態量計測方法及び装置を提供す
ることにある。

【００１０】本発明のさらに他の目的は、移動体の運動
モデルに基づいた新規な精度のよい姿勢角演算装置を提
供することにある。

【００１１】

【課題を解決するための手段】本発明の目的は、ＧＰＳ
受信機により長周期で計測される移動体の位置データ
と、ジャイロ及び加速度計により短周期でそれぞれ計測
される移動体の角速度データ及び加速度データと、これ
ら各データを使用して移動体の状態量（位置）を求める
計測方法において、前記ＧＰＳ受信機の長周期の計測タ
イミングを逓倍した所定のタイミング信号を発生し、前
記所定のタイミング信号に同期するように前記角速度デ
ータ及び加速度データを補正することにより達成され
る。

【００１２】本発明の目的は、ＧＰＳ受信機と、ジャイ
ロと、加速度計と、これら各計器の出力に基づいて位置
や姿勢角等を演算する移動体の状態量計測装置におい
て、前記ＧＰＳ受信機からの位置のデータと状態量演算
部からの姿勢角及び位置のデータを入力して、前記ＧＰ
Ｓ受信機の計測タイミング毎に移動体の位置または位置
と姿勢角等を推定するカルマンフィルタと、前記ジャイ
ロからの角速度を地面固定座標系に変換する第１の座標
変換手段及び変換された角速度を積分して前記カルマン
フィルタへ出力する姿勢角を求める第１の積分手段を有
する姿勢角演算手段と、前記加速度検出器からの加速度
を積分して移動体の速度を求める第２の積分手段、前記
速度を地面固定座標系に変換する第２の座標変換手段及
び変換された速度を積分して前記カルマンフィルタへ出
力する位置を求める第３の積分手段を有する位置演算手
段とを具備し、前記ＧＰＳ受信機の計測タイミングより
短い周期で移動体の位置と姿勢角を演算する前記状態量
演算部とを備え、前記第１の積分手段の初期値を前記Ｇ
ＰＳ受信機の計測タイミング毎に、前記カルマンフィル
タから出力される姿勢角または姿勢角補正手段からの姿
勢角により更新するように構成されることにより達成さ
れる。

【００１３】本発明の目的は、ＧＰＳ受信機と、機体固
定座標系の移動体の加速度を計測する加速度計と、上記
の各計測器の出力に基づいて移動体の位置や速度を推定
するカルマンフィルタと、前記加速度計の出力と前記カ
ルマンフィルタの出力に基づいて地面固定座標系の移動
体の姿勢角を演算する装置において、前記カルマンフィ
ルタからの前記機体固定座標系の速度と前記地面固定座
標系の位置と前記加速度計からの加速度を入力する入力
手段と、入力された位置から前記地面固定座標系の速度
を求める微分手段と、移動体に加わる外力の運動モデル
を変形し、地面固定座標系による速度と、機体固定座標
系による速度及び移動体の迎え角と、の関数で表わす姿
勢角の運動モデルに基づいて、前記地面固定座標系によ
る移動体の姿勢角を求める演算手段を備えることにより
達成される。

【００１４】

【作用】本発明の構成によれば、ＧＰＳ受信機の長周期
の計測タイミングをｎ倍した所定のタイミング信号が発
生され、このｎは前記長周期と前記短周期の整数比以上
とされる。前記所定のタイミングとその直前（または直
後）で検出されるジャイロや加速度計の出力との時間差
を求め、その時間差に応じ前記所定のタイミングに同期
して出力するための角速度と加速度の補正値を求める。

【００１５】これによれば、ＧＰＳ受信機からの長周期
の出力と、ジャイロや加速度計からの短周期の出力との
同期が計られるので、計測タイミングのずれによる誤差
が低減でき、これらの出力を使用しカルマンフィルタに
より推定する移動体の状態量の計測精度を向上できる効
果がある。

【００１６】本発明の構成によれば、加速度計の同期さ
れた出力を積分して得られる短周期の移動体の位置デー
タを求める際に、ＧＰＳ受信機からの長周期の出力に基
づきカルマンフィルタにより推定される長期に安定した
位置データによって、上記積分の初期値を更新するの
で、短周期に出力される移動体の位置の精度を向上でき
る効果がある。

【００１７】本発明の構成によれば、ジャイロからの角
速度を積分して得られる姿勢角をカルマンフィルタに与
えるので、カルマンフィルタの状態量の推定精度を向上
できる。それと共に、上記積分の初期値を、カルマンフ
ィルタからの姿勢角又は、カルマンフィルタからの位置
と速度及び加速度計からの加速度に従って求めた姿勢角
によって、ＧＰＳ受信機の計測タイミング毎に更新する
ので、ジャイロに起因して発生する姿勢角の誤差を低減
でき、座標変換を行う状態量の精度を向上できる効果が
ある。

【００１８】本発明の構成によれば、従来のジャイロの
角加速度によらず、カルマンフィルタの出力と加速度系
の出力を使用し、移動体に加わる外力モデルを基にした
新規で精度の高い姿勢角演算装置を提供できる。また、
この姿勢各演算装置を利用して、ジャイロの角加速度に
よる短周期の姿勢角を周期的に補正することも可能であ
る。

【００１９】

【実施例】以下、本発明の一実施例を図１〜図１０を用
いて詳細に説明する。

【００２０】図２は、航空機が自己の位置を計測するた
めに、ＧＰＳ衛星からの電波を受信する様子を示してい
る。航空機１は、電波受信の良好な位置関係にある複数
のＧＰＳ衛星２（２−１〜２−４）を選択して、それら
の衛星から送信される情報２ａ〜２ｄを受信し、航空機
位置の計測用デ−タとする。

【００２１】航空機１に搭載された位置計測装置３は、
航空機の位置、速度、姿勢角などを計測し、計測データ
１ａを地上局４に伝送する。地上局４は、計測データに
基づいて航跡監視などを行う。地上局４は、ＧＰＳ衛星
２からの電波を直接受信し、その電波に含まれている人
工的な誤差（ＳＡ）の大きさを算出し、この結果をアッ
プリンク用地上局５から位置計測装置３に送信して、Ｓ
Ａを取り除けるようにも構成される。

【００２２】図１は、本実施例による移動体の状態量計
測装置の構成を説明する機能ブロック図である。以下で
は航空機を例に説明する。

【００２３】本例の計測装置は、ＧＰＳ受信機１１、ジ
ャイロ１２、加速度計１３のセンサ類を装備する計器シ
ステム６と、計測タイミングのずれを補正する信号同期
部１４、ＧＰＳ受信機１１の出力（航空機の位置）や航
空機の姿勢角を観測値にして、誤差の少ない航空機の位
置や姿勢角等を推定するカルマンフィルタ１５、ジャイ
ロ１２や加速度計１３の出力を基に位置や姿勢角等を演
算する状態量演算部１６、演算部１６の姿勢角を補正す
る姿勢角演算部１７及びカルマンフィルタ１５と状態量
演算部１６の出力を選択的に出力するスイッチ部１８を
具備する計算機装置７から構成されている。なお、計算
機装置７の機能の一部が、地上局４に設けられる場合も
ある。

【００２４】ＧＰＳ受信機１１は、４機以上のＧＰＳ衛
星２と航空機１との距離を各々受信して、この距離信号
１１ａを所定の周期Ｔ１で出力する。なお、距離信号を
基にＧＰＳ受信機で、航空機の３次元位置を演算して出
力する場合もある。本例は、距離を出力する例で説明す
るが、どちらの場合でも航空機の位置信号であることに
は変わりない。

【００２５】ジャイロ１２は機体座標系の角速度を、加
速度計１３は機体座標系の加速度を、ＧＰＳ受信機の周
期Ｔ１とは非同期で短い周期のＴ２で計測して出力す
る。なお、両者の計測周期は相違していてもよいが、以
下では説明を簡単にするために同じとする。

【００２６】周期Ｔ１とＴ２の計測タイミングのずれに
よる誤差は、位置などの計測データの精度を低下させ
る。例えば、地面固定座標系のＸ軸方向の速度（ｄＸ／
ｄｔ）を、地面固定座標系から見た姿勢角（ロール角
φ、ピッチ角θ、ヨー角ψ）と、機体固定座標系の速度
（Ｕ、Ｖ、Ｗ）を使って求める場合を考える。

【００２７】誤差のない真値の姿勢角の場合に、地面固
定座標系の速度ｄｘ／ｄｔ_trueは、式（１）により求め
られる。

【００２８】

【数１】

【００２９】一方、計測タイミングのずれによって生じ
るδθ、δφ、δψの誤差を含んだ計測姿勢角の場合
に、地面固定座標系の速度ｄＸ／ｄｔ_measureは式
（２）により求められる。

【００３０】

【数２】

【００３１】ここで、δ*（δφ、δθ、δψ）が微小
でsin(δ*)≒δ*、cos(δ*)≒１と近似でき、誤差の２
乗項は無視できるものとすると、ｄＸ／ｄｔ_measureは
式（３）のように変形できる。

【００３２】

【数３】

【００３３】この（数３）の誤差Ｅは、ｄＸ／ｄｔ
_measureを積分して求めるＸ座標値にも影響し、位置な
ど他の状態量の精度も低下させることになる。

【００３４】図３は、信号同期部１４の機能ブロック図
である。信号同期部１４は、ＧＰＳ受信機１１の出力１
１ａと、ジャイロ１２や加速度計１３の出力１２ａ、１
３ａの出力時刻を一致させるための補正手段である。

【００３５】信号同期部１４は、ＧＰＳ受信機１１の時
計１１’のクロック信号を入力してｎ（整数）倍し、周
期Ｔ３の基準クロックを作成する逓倍手段１４１、ジャ
イロ１２の出力１２ａを入力する角速度入力手段１４
２、加速度計１３の出力１３ａを入力する加速度入力手
段１４３、入力データを基準クロック時に合わせて補正
する時刻一致化処理手段１４４、１４５及び出力手段１
４６から構成される。

【００３６】逓倍手段１４１は、ＧＰＳ受信機１１の計
測周期Ｔ１と、ジャイロ１２や加速度計１３の計測周期
Ｔ２の整数比ｎ（ｎ≒Ｔ１／Ｔ２……小数点以下切上
げ）の分だけ、時計１１’のクロック信号を逓倍する。
この逓倍数は上記整数比ｎ以上であってもよい。

【００３７】角速度入力手段１４２と加速度入力手段１
４３は、それぞれ計測周期Ｔ２で時系列に角速度データ
１２ａ、加速度データ１３ａを入力して、時刻一致化処
理手段１４４、１４５に渡す。この時刻一致化処理手段
は一つで構成してもよい。

【００３８】図４と図５は、時刻一致化処理手段１４４
（１４５）の動作を示すフローチャートと、タイムチャ
ートである。

【００３９】時刻一致化処理手段１４４（１４５）は、
周期Ｔ２で角速度や加速度を受け取り（ｓ１０１）、そ
の入力データと入力時刻を組にして複数、例えば３回分
を記憶し、最新のデータが入力される度に古い方から捨
てて更新する（ｓ１０２）。次にあるいは並行して、周
期Ｔ３の基準クロックが入力されたかチエックする（ｓ
１０３）。

【００４０】基準クロックが入力されると、基準クロッ
クの入力時刻ｔ３と最新の入力データの時刻ｔ₂₀との時
間差△ｔを求める（ｓ１０４）。時間差△ｔが所定以上
かチエックし（ｓ１０５）、そうであれば最新の入力デ
ータの補正を行う（ｓ１０６）。時間差△ｔが所定以
下、すなわち時刻ずれによる誤差が問題にならない程小
さいときは補正しない。この後、出力手段１４６から読
みだされて、周期Ｔ３で演算部１６へ出力される（ｓ１
０７）。

【００４１】ステップｓ１０６の補正は、図５の角速度
補正値（ｅ）に示すように、基準クロックの入力時刻ｔ
３と、その直前の角速度ω₃の時間差△ｔと、ω₃の値と
その前のω₂、ω₁の値から、直線あるいは最小二乗など
によって補間し、時刻ｔ３における加速度の補正値
ω₃’を求める。同様にして次の補正値ω₄’はω₄、
ω₃、ω₂から求める。

【００４２】本例では、基準クロックの周期Ｔ３≒周期
Ｔ２であるから時々、時間差△ｔが所定値△ｔ_minより
小さい場合が生じ、そのときは補正の必要がない。

【００４３】出力手段１４６は周期Ｔ３で、時刻一致化
処理手段１４４，１４５から補正後の角速度１２ｂ及び
加速度１３ｂを取り出し周期Ｔ３で出力する。ＧＰＳ受
信機の出力１１ａは周期Ｔ３のｎ回毎に出力されるの
で、この時には３つのデータが同時に出力される。

【００４４】なお、ジャイロ１２と加速度計１３の周期
が相違している場合は、短い方を基準にしてｎを設定す
れば、同様にして一致化処理ができる。また、基準クロ
ックの出力の直後の入力データとその時間差に応じて、
当該基準クロックに同期するように補正することもでき
る。

【００４５】このように、本実施例の信号同期部１４の
補正処理によって、ＧＰＳ受信機１１とジャイロ１２や
加速度計１３の計測周期が非同期の場合に、計測タイミ
ングずれによる誤差を低減できる。

【００４６】カルマンフィルタ１５は計測用であり、Ｇ
ＰＳ受信機１１からの距離信号１１ａと、状態量演算部
１６からの姿勢角及び位置を入力して、航空機の位置等
を算出する。

【００４７】図６は、カルマンフィルタの機能ブロック
図である。カルマンフィルタ１５は、観測値推定器１５
１、カルマンゲイン算出器１５２、行列乗算器１５３、
状態変数設定器１５４、行列加算器１５５、１５６、推
定誤差共分散行列算出器１５７、観測行列算出器１５
８、状態遷移行列算出器１５９で構成する。

【００４８】図７に、カルマンフィルタの処理フロ−図
を示す。状態変数設定器１５４は、状態量演算部１６の
出力である姿勢角１６ｂ，速度１６ｃ，位置１６ｅを入
力して、状態変数である航空機の位置、速度、姿勢角、
さらには加速度計の加速度誤差バイアス成分、ジャイロ
の角速度誤差バイアス成分、ＧＰＳ受信機のクロック誤
差バイアス成分などを設定する（ｓ２０１）。

【００４９】観測値推定器１５１は、状態変数設定器１
５４により設定された状態変数の内の位置情報（３変
数）１５０ｅから、ＧＰＳ衛星２の位置と航空機１まで
の距離１５０ｂを算出する（ｓ２０２）。

【００５０】次に、観測行列算出器１５８は、状態変数
設定器１５４の出力する状態変数中の航空機位置１５０
ｊを用いて、観測行列１５０ｈを算出する（ｓ２０
３）。

【００５１】次に、カルマンゲイン算出器１５２は、推
定誤差共分散行列算出器１５７からの出力１５０ｉと、
観測行列１５０ｈ及び、あらかじめ設定されている観測
ノイズ行列（Ｒ_t）を使ってカルマンゲイン行列１５０
ｇを算出する（ｓ２０４）。

【００５２】次に、ＧＰＳ受信機１１の計測値１５０ａ
と観測値推定器１５１の推定値１５０ｂを行列加算器１
５５で減算し、さらに行列乗算器１５３で減算値１５０
ｃとカルマンゲイン行列１５０ｇを乗算する。行列加算
器１５６は、この乗算値１５０ｄと状態変数設定器１５
４からの状態変数１５０ｅを加算して、状態変数、例え
ば航空機位置の最適推定値１５０ｆを算出する（ｓ２０
５）。

【００５３】状態遷移行列算出器１５９は、状態変数設
定器１５４から姿勢角や速度の状態変数１５０ｊを入力
して、推定誤差共分散行列算出器１５７による推定誤差
共分散行列１５０ｉの演算に必要な状態遷移行列１５０
ｋを状態変数毎に算出する（ｓ２０６，ｓ２０７）。な
お、上記したカルマンフィルタの構成は、片山徹著「応
用カルマンフィルタ（朝倉書店、ｐ８８〜ｐ８９）」に
詳しい。

【００５４】本実施例では、このように構成されるカル
マンフィルタ１５に、長期的に安定しているＧＰＳ受信
機１４からの地面固定座標系の距離信号１５ａが入力さ
れる。さらに、同期補正された、加速度検出器１３の加
速度に基づく地面固定座標系からみた航空機位置３変数
（Ｘ、Ｙ、Ｚ）とジャイロ１２の角加速度に基づく地面
固定座標系からみた姿勢角３変数などを状態量演算部１
６から与えられるので、拡張カルマンフィルタの式に従
って、航空機の位置、姿勢角などの状態量の最適値を高
精度に推定できる。

【００５５】加速度検出器１３の加速度に基づく機体固
定座標系の速度３変数（Ｕ、Ｖ、Ｗ）１６ｃは、後述す
る姿勢角演算部１７への出力の必要な場合に入力され、
この状態変数についてもカルマンフィルタ１５による推
定が可能になる。

【００５６】図８は、状態量演算部１６の機能ブロック
図である。この演算部１６は、従来の慣性基準装置の演
算部を基本に構成され、信号同期手段１４により補正さ
れた角速度１２ｂと加速度１３ｂのベクトルを入力し
て、地面固定表座標系での航空機の姿勢角と位置を算出
する。

【００５７】座標変換器１６０は、ジャイロ１２からの
角速度１２ｂと航空機の姿勢角１６ｃを入力し、周知の
変換行列に従って地面固定座標系から見た角速度１６ａ
へ変換する。＃１の積分器１６１は、変換後の角速度１
６ａを積分して地面座標系の姿勢角１６ｂを算出し、カ
ルマンフィルタ１５０に出力する。このとき、積分器１
６１の初期値は、後述する姿勢角演算部１７からの姿勢
角１６ｆによって、周期Ｔ１毎に更新される。

【００５８】一方、＃２の積分器１６２は、加速度計１
３からの加速度１３ｂを積分し、機体固定座標系の速度
１６ｃを算出する。速度１６ｃは、カルマンフィルタ１
５に出力されると共に、＃２の座標変換器１６３で地面
固定座標系の速度１６ｄに変換される。さらに、＃３の
積分器１６４で地面固定座標系の位置１６ｅに変換され
る。

【００５９】ところで、状態量演算部１６により演算さ
れる各状態量は、一般に短期的には精度が高いが、長期
的にはジャイロ１２のドリフトによる誤差の影響が無視
できなくなる。

【００６０】しかし、本実施例によれば、ＧＰＳ受信機
１１の計測タイミングに同期して、＃１積分器１６１の
姿勢角１６ｂの出力をリセットし、カルマンフィルタ１
５０の最適推定値である位置信号に基づく、姿勢角演算
部１７からの姿勢角１６ｆを初期値とする演算を行うの
で、精度の高い姿勢角を算出できる。これによって、座
標変換精度が向上でき、状態量演算部１６から出力され
る各状態量及びカルマンフィルタ１５から出力される各
状態量の計測精度を向上できることは言うまでもない。
なお、姿勢角１６ｆはカルマンフィルタ１５からの姿勢
角を与えてもよい。

【００６１】図９は、姿勢角演算部１７の機能ブロック
図である。

【００６２】本実施例の姿勢角演算部は、航空機に加わ
る外力の運動モデルを変形し、地面固定座標系による速
度と、機体固定座標系による速度及び航空機の迎え角
と、の関数で表わす姿勢角の運動モデルに基づいて、前
記地面固定座標系による航空機の姿勢角を求める演算手
段を備え、カルマンフィルタ１５からの位置及び速度
と、加速度計からの加速度を入力として、航空機の姿勢
角をカルマンフィルタ１５からの出力に同期して演算す
る。

【００６３】まず、本実施例の姿勢角演算部１７による
姿勢角の計測原理を説明する。航空機の機体に働く外力
Ｆは、式（４）に示す機体に働く空気力ベクトルＦ
_a（及び推力)と重力ベクトルＦ_gの和で求まる。ここ
で、空気力ベクトル、重力ベクトルは、それぞれ式
（５）、式（６）のようになる。

【００６４】

【数４】

【００６５】ここで、（Ｘ_a、Ｙ_a、Ｚ_a）と（Ｘ_g、Ｙ_g、
Ｚ_g）は、それぞれ機体固定座標系における空気力及び
重力のＸ軸、Ｙ軸、Ｚ軸成分である。空気力について
は、機体の速度（重心の）Ｖｃに垂直で重力を支える向
きに働く力（揚力：Ｌ）と、機体の速度Ｖｃに平行で機
体の進行に逆らう向きに働く力（抗力：Ｄ）と、推力
（Ｔ）とに分けることができる。揚力Ｌと抗力Ｄは式
（７）、（８）で、機体速度Ｖcは式（９）で表わされ
る。

【００６６】

【数５】

【００６７】

【数６】

【００６８】式（９）で、Ｕ、Ｖ、Ｗはそれぞれ機体固
定座標系における航空機のＸ軸、Ｙ軸、Ｚ軸方向の速度
である。これより、式（５）の右辺の各項は、迎え角α
とすると、式（１０）〜（１２）のように表される。

【００６９】

【数７】

【００７０】式（１０）〜（１２）中の各変数は、式
（１３）〜（１８）で表される。

【００７１】

【数８】

【００７２】一方、機体の重量をｍ、重力加速度をｇと
すると、式（６）の重力ベクトルの右辺の各項は、式
（１９）〜（２１）で表される。

【００７３】

【数９】

【００７４】ここで、φ、θ、ψはそれぞれロ−ル角、
ピッチ角、ヨ−角である。

【００７５】ところで、機体の重心の加速度をａ_cと
し、その機体固定座標系におけるＸ軸、Ｙ軸、Ｚ軸成分
をそれぞれａ_(x)、ａ_(y)、ａ_(z)とすると、外力Ｆと加
速度ａ_cは式（２２）、（２３）のように表される。

【００７６】

【数１０】

【００７７】ここで、図１０を参照して、機体に働く外
力Ｆと、機体に搭載された加速度計１３で検出する加速
度ａ_cの関係を説明する。

【００７８】機体固定座標系のＸ軸、Ｚ軸による２次元
の平面上で、加速度計はＸ軸、Ｚ軸方向の加速度
ａ_(x)、ａ_(z)を検出する。この加速度に機体の重量ｍを
乗ずると、各軸に働く力が求まる。一方、機体の重心１
８０には外力Ｆとして、揚力Ｌ、抗力Ｄ、重力ｍｇ、推
力Ｔが加わっている。

【００７９】従って、外力ＦのＬ、Ｄ、ｍｇ及びＴを軸
成分に分解した各軸毎の外力は、加速度計による加速度
から求まる各軸に働く力と等しい。ここで、外力Ｆの各
軸成分は、迎え角αによるＸ軸への投影、ピッチ角θに
よるＺ軸への投影により求まる。なお、図中のＸ_E、Ｚ_E
平面は、地面固定座標系におけるＸ軸、Ｚ軸を示してい
る。

【００８０】以上のように、式（４）に示す航空機の運
動モデルから求めた外力Ｆは、式（２２）が示す加速度
計から求めた外力と等価である。この式（２２）と、式
（４）、式（１０）〜（１２）、式（１９）〜（２１）
より、式（２４）〜（２６）の関係式が求まる。

【００８１】

【数１１】

【００８２】さらに、式（２４）〜（２６）を変形し、
ロール角φ、ピッチ角θについて求めると、式（２
７）、（２８）のようになる。

【００８３】

【数１２】

【００８４】また、地面固定座標系の速度と、機体固定
座標系の速度との関係式は、式（２９）、（３０）のよ
うになり、式（２７）、（２８）で求まったφ、θとか
ら、ヨー角ψは式（３１）のように求まる。

【００８５】

【数１３】

【００８６】

【数１４】

【００８７】以上に示した式（２７）、（２８）及び
（３１）より、機体固定座標系による機体の加速度と速
度及び地面固定座標系の速度を入力として、機体の姿勢
角（φ、θ、ψ）を算出することができる。なお、上記
の式（７）〜式（１８）は、加藤寛一郎・大屋昭男・柄
沢研治著「航空機力学入門（東京大学出版会）」を参照
した。

【００８８】本実施例の姿勢角演算部１７は、角速度を
積分する従来の方法と異なり、上記メカニズムによる姿
勢角の演算を、以下のようにして実現している。

【００８９】まず、カルマンフィルタ１５から機体の速
度（機体固定座標系）１７０ｃと位置（地面固定座標
系）１７０ａ及び、加速度計１３から機体の加速度（機
体固定座標系）１３ｂを入力し、地面固定座標系から見
た姿勢角（φ、θ、ψ）１７０ｊを、ＧＰＳ受信機１１
の計測周期Ｔ１毎に四則演算装置１７１で算出する。

【００９０】このため、式（３１）の演算に必要な地面
固定座標系の速度(dX/dt、dY/dt）は、位置１７０ａを微
分器１７２で微分して算出する。また、式（２７）、
（２８）の演算に必要な空気密度ρは、入力した位置１
７０ａ中の高度座標１７０ｂを用いて、空気密度演算器
１７３で算出する。

【００９１】その他、演算に必要で航空機機種に依存す
る定数は、あらかじめ航空機個別情報設定器１７７に記
憶してある複数機種のデータの中から、航空機機種選択
情報によって指定して使用する。式（２７）、（２８）
及び（３１）の逆正接、逆正弦、逆余弦の演算は、それ
ぞれ逆正接演算器１７４、逆正弦演算器１７５、逆余弦
演算器１７６で実行する。

【００９２】本実施例の姿勢角演算部によれば、従来の
ジャイロの加速度を利用せずに、長期的に安定している
カルマンフィルタの出力と、航空機に加わる外力モデル
を基にして、姿勢角を高精度に算出できる。

【００９３】また、この姿勢角を使用して、状態量演算
部１６の積分器１３１の初期値を周期的に更新すること
で、ジャイロの角加速度を基にして短周期に出力する姿
勢角の誤差を低減できる。

【００９４】図１０は、カルマンフィルタ１５、状態量
演算部１６及び姿勢角演算部１７からなる計算機装置７
（図１）の処理動作を示すフローチャートである。

【００９５】計算機装置７は、信号同期部１４から入力
した情報に従って、ＧＰＳ受信機１１の周期Ｔ１の計測
があったかどうか判定し（ｓ３０１）、計測があった場
合はカルマンフィルタ１５を起動する（ｓ３０２）。

【００９６】周期Ｔ１毎に起動されるカルマンフィルタ
１５は、状態量演算部１６から、現在時刻の位置、速
度、姿勢角を入力し、ＧＰＳ受信機１１の計測値を観測
値として航空機の位置、速度、姿勢角の最適推定値を算
出する。さらに、カルマンフィルタ１５で推定した位置
や速度、加速度計の計測値による加速度に従って、姿勢
角演算部１７で姿勢角補正値を算出する（ｓ３０３）。

【００９７】一方、ＧＰＳ受信機１１からの計測値がな
い場合は、カルマンフィルタ１５は起動せず、ジャイロ
１２と加速度計１３の計測値を入力し、状態量演算部１
６における積分・座標変換によって航空機の位置、速
度、姿勢角を、周期Ｔ１より短い周期Ｔ２毎に算出する
（ｓ３０４）。

【００９８】姿勢角を算出する際に、直前にステップｓ
３０３による姿勢角の補正入力があった場合には、補正
姿勢角を積分の初期値として使用する。それ以外の周期
には、前回の積分値を初期値として使用する。姿勢角の
補正処理（ｓ３０３）、あるいは積算処理（ｓ３０
４’）が終了した後、計算機装置７の処理を継続して実
行するか否かを判定し（ｓｓ３０５）、継続するのであ
ればＧＰＳ受信機計測の判定処理（ｓ３０１）に戻る。

【００９９】このように、本実施例の移動体の状態量計
測装置によれば、カルマンフィルタによる安定した状態
量を長周期で出力し、ジャイロに起因する誤差を低減し
た状態量演算部による状態量を短周期で出力するように
しているので、複合センサによる状態量の計測精度を大
幅に向上できる。

【０１００】

【発明の効果】本発明によれば、ＧＰＳ受信機からの長
周期の出力と、ジャイロや加速度計からの短周期の出力
との同期が計られるので、複合センサを使用するときの
計測タイミングのずれによる誤差が低減でき、これらの
出力を使用しカルマンフィルタにより推定する移動体の
状態量の計測精度を向上できる効果がある。

【０１０１】また、同期が計られた加速度を積分して短
周期に移動体の位置を求める際に、カルマンフィルタに
より推定される長周期の安定した位置データにより短周
期の位置を補正できるので、短周期に出力される移動体
の位置の精度を向上できる効果がある。

【０１０２】さらに、カルマンフィルタからまたは異な
る原理で求まる長周期の姿勢角により、ジャイロからの
角速度を積分して求める短周期の姿勢角を補正できるの
で、座標変換を伴って短周期に出力される移動体の状態
量の精度を向上できる効果がある。

【０１０３】さらに、本発明の姿勢角演算装置によれ
ば、従来のジャイロの角加速度によらず、移動体に加わ
る外力モデルを基にした新規で精度の高い姿勢角演算装
置を提供できる。

【図面の簡単な説明】

【図１】本発明の移動体の状態量計測装置の一実施例を
示す機能ブロック図である。

【図２】ＧＰＳ衛星を利用して航空機の位置を計測する
ようすを示す概念図である。

【図３】信号同期手段の構成を示す機能ブロック図であ
る。

【図４】信号同期手段の処理を示すフローチャートであ
る。

【図５】信号同期手段の動作を説明するタイムチャート
である。

【図６】カルマンフィルタの構成を示す機能ブロック図
である。

【図７】カルマンフィルタの処理を示すフローチャート
である。

【図８】状態量演算部の構成を示す機能ブロック図であ
る。

【図９】姿勢角演算部の構成を示す機能ブロック図であ
る。

【図１０】航空機に加わる外力を説明するベクトル図で
ある。

【図１１】移動体の状態量計測装置の処理の概要を示す
フローチャートである。

【図１２】従来の航空機の位置速度計測装置の一例を示
す機能ブロック図である。

【符号の説明】

１…航空機、２…ＧＰＳ衛星、４…地上局、６…計器シ
ステム、７…計算機装置、１１…ＧＰＳ受信機、１２…
ジャイロ、１３…加速度計、１４…信号同期部、１５…
カルマンフィルタ、１６…状態量演算部、１７…姿勢角
演算部、１８…スイッチ部、１４１…逓倍手段、１４２
…角速度入力手段、１４３…加速度入力手段、１４４，
１４５…時刻一致化処理手段、１４６…出力手段、１６
０…第１の座標変換器、１６１…第１の積分手段、１６
２…第２の積分手段、１６３…第２の座標変換器、１６
４…第３の積分手段。

Claims

【特許請求の範囲】

【請求項１】ＧＰＳ受信機により長周期で計測される
移動体の位置データと、ジャイロ及び加速度計により短
周期でそれぞれ計測される移動体の角速度データ及び加
速度データと、これら各データを使用して移動体の状態
量（位置）を求める計測方法において、前記ＧＰＳ受信機の長周期の計測タイミングを逓倍した
所定のタイミング信号を発生し、前記所定のタイミング
信号に同期するように前記角速度データ及び加速度デー
タを補正することを特徴とする移動体の状態量計測方
法。
【請求項２】請求項１において、前記所定タイミング信号は、前記長周期と前記短周期の
比の整数倍以上の周期に逓倍して発生することを特徴と
する移動体の状態量計測方法。
【請求項３】請求項１または２において、前記補正は、前記所定のタイミング信号とその直前また
は直後に検出される前記角速度データまたは前記加速度
データの時間差が所定以上のときに、この時間差と前記
特前または直後を含む複数の前記角速度データまたは前
記加速度データの時系列を基に、前記所定のタイミング
信号の発生時刻に一致する角速度データまたは加速度デ
ータを推定することを特徴とする移動体の状態量計測方
法。
【請求項４】ＧＰＳ衛星からの電波を受信して移動体
の位置を計測するＧＰＳ受信機と、移動体の角速度を計
測するジャイロと、移動体の加速度を計測する加速度計
と、上記の各計測値に基づいて誤差を低減した移動体の
位置等を推定するカルマンフィルタを備える移動体の状
態量計測装置において、前記ＧＰＳ受信機の長周期の計測タイミングを逓倍した
所定のタイミング信号を発生し、前記所定のタイミング
信号に同期するように前記角速度データ及び加速度デー
タを補正する信号同期手段と、前記信号同期手段から前記所定のタイミング信号で入力
され、前記補正された加速度データを積分して姿勢角を
求める姿勢角演算手段と、前記補正された加速度を積分
して速度を求め更に積分して位置を求める位置演算手段
を有する状態量演算部を備え、前記ＧＰＳ受信機からの位置のデータと前記状態量演算
部からの姿勢角及び位置のデータを前記カルマンフィル
タに入力して、前記長周期の計測タイミング毎に移動体
の位置等を推定するように構成されることを特徴とする
移動体の状態量計測装置。
【請求項５】請求項４において、前記位置演算手段の積分の初期値を、前記カルマンフィ
ルタから出力される位置のデータにより更新するように
構成されることを特徴とする移動体の状態量計測装置。
【請求項６】ＧＰＳ衛星と移動体間の距離から移動体
の位置を計測するＧＰＳ受信機と、移動体の角速度を計
測するジャイロと、移動体の加速度を計測する加速度計
と、上記の各計測値に基づいて位置や姿勢角等を演算す
る移動体の状態量計測装置において、前記ＧＰＳ受信機からの位置のデータと状態量演算部か
らの姿勢角及び位置のデータを入力して、前記ＧＰＳ受
信機の計測タイミング毎に移動体の位置または位置と姿
勢角等を推定するカルマンフィルタと、前記ジャイロからの角速度を地面固定座標系に変換する
第１の座標変換手段及び変換された角速度を積分して前
記カルマンフィルタへ出力する姿勢角を求める第１の積
分手段を有する姿勢角演算手段と、前記加速度検出器か
らの加速度を積分して移動体の速度を求める第２の積分
手段、前記速度を地面固定座標系に変換する第２の座標
変換手段及び変換された速度を積分して前記カルマンフ
ィルタへ出力する位置を求める第３の積分手段を有する
位置演算手段とを具備し、前記ＧＰＳ受信機の計測タイ
ミングより短い周期で移動体の位置と姿勢角を演算する
前記状態量演算部とを備え、前記第１の積分手段の初期値を前記ＧＰＳ受信機の計測
タイミング毎に、前記カルマンフィルタから出力される
姿勢角または姿勢角補正手段からの姿勢角により更新す
るように構成されることを特徴とする移動体の状態量計
測装置。
【請求項７】請求項６において、前記カルマンフィルタは、前記第２の積分手段からの速
度を入力して機体固定座標系の移動体の速度を推定し、前記姿勢角補正手段は、前記カルマンフィルタからの前
記機体座標系の速度と、前記カルマンフィルタからの地
面固定座標系の位置を微分した速度と、前記加速度計か
らの加速度に基づいて、前記ＧＰＳ受信機の計測タイミ
ング毎に移動体の地面固定座標系の姿勢角を演算するよ
うに構成されていることを特徴とする移動体の状態量計
測装置。
【請求項８】請求項４〜請求項７のいずれか１項にお
いて、前記カルマンフィルタからの移動体の状態量が出力され
ないときに、前記状態量演算部からの位置および／また
は姿勢角を出力するスイッチ手段を備えることを特徴と
する移動体の状態量計測装置。
【請求項９】ＧＰＳ衛星と移動体間の距離から地面固
定座標系の移動体の位置を計測するＧＰＳ受信機と、機
体固定座標系の移動体の加速度を計測する加速度計と、
上記の各計測値に基づいて位置や速度を推定するカルマ
ンフィルタと、前記加速度計の出力と前記カルマンフィ
ルタの出力に基づいて地面固定座標系の移動体の姿勢角
を演算する装置において、前記カルマンフィルタからの前記機体固定座標系の速度
と前記地面固定座標系の位置と前記加速度計からの加速
度を入力する入力手段と、入力された位置から前記地面
固定座標系の速度を求める微分手段と、移動体に加わる外力の運動モデルを変形し、地面固定座
標系による速度と、機体固定座標系による速度及び移動
体の迎え角と、の関数で表わす姿勢角の運動モデルに基
づいて、前記地面固定座標系による移動体の姿勢角を求
める演算手段を備えることを特徴とする移動体の姿勢角
演算装置。