JP6876999B2 - ナビゲーション装置 - Google Patents

Description

この発明は移動体に搭載されるナビゲーション装置に関し、特に、カルマンフィルタにより誤差推定を行うことで補正されたＧＰＳセンサの観測結果を利用して演算結果を補正する航法演算を行うナビゲーション装置であって、ＧＰＳセンサのＧＰＳ電波受信が途絶した場合にカルマンフィルタを停止し、その後ＧＰＳセンサがＧＰＳ電波を受信した場合には、所定の条件を満たす場合にカルマンフィルタを起動するナビゲーション装置に関する。

従来用いられてきたナビゲーション装置としては、例えば以下の特許文献１等に記載された構成を図４として挙げることができる。すなわち、従来のナビゲーション装置１には、速度センサ２と、例えばジャイロ等の角速度センサ３と、ＧＰＳセンサ４とが設けられている。前記角速度センサ３による角速度観測結果と前記ＧＰＳセンサ４による方位観測結果とが、角速度センサ用カルマンフィルタ５ａに入力されて方位誤差推定が行われる。前記速度センサ２による速度観測結果と前記ＧＰＳセンサ４による速度観測結果とが速度センサ用カルマンフィルタ５ｂに入力されて速度誤差観測が行われる。

次に、角速度観測結果と、速度観測結果と、方位誤差推定結果と、速度誤差推定結果とが誤差補正装置６に入力されることで、誤差補正後の移動体の角速度及び速度が得られる。次に、方位誤差推定結果と誤差補正後の前記移動体の角速度及び速度とが航法演算手段７に入力されることで、前記移動体の位置及び方位角が得られる。つまり、前記ナビゲーション装置１では、前記移動体の角速度観測結果と速度観測結果が、前記ＧＰＳセンサ４による観測結果及びカルマンフィルタにより補正され、航法演算手段７により航法演算が行われる。

特開平７−３０１５４１号公報

しかし、上記のような従来のナビゲーション装置１において、前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶し、その後ＧＰＳ電波を再び受信した場合には、前記ＧＰＳセンサ４により観測された前記移動体の方位及び速度の観測結果と、前記移動体の真の方位及び速度との間の誤差が大きくなる場合がある。そのため、この場合に前記移動体の角速度観測結果と速度観測結果とが前記ＧＰＳセンサ４による観測結果で補正されると、前記ＧＰＳセンサ４による観測結果の誤差により良好な補正結果が得られずに精度が悪化するという課題があった。

また、前記移動体が建物の近傍を通過した場合等にも、前記ＧＰＳセンサ４により観測された前記移動体の方位及び速度と、前記移動体の真の方位及び速度との間の誤差が大きくなる場合がある。そのため、この場合も前記移動体の角速度観測結果と速度観測結果とが前記ＧＰＳセンサ４による観測結果で補正されると、前記ＧＰＳセンサ４による観測結果の誤差により良好な補正結果が得られずに精度が悪化するという課題があった。

この発明はこのような課題を解決するためになされたものであり、前記ＧＰＳセンサのＧＰＳ電波受信が途絶しその後ＧＰＳ電波を再び受信した場合や、前記移動体が建物の近傍を通過した場合等で、前記ＧＰＳセンサによる前記移動体の位置及び速度との観測誤差が大きい場合に、前記方位センサ及び前記速度センサの観測結果を使用した位置及び方位角の航法演算結果の精度が悪化することを防止するナビゲーション装置を提供することを目的とする。

この発明に係るナビゲーション装置は、移動体の速度を観測する速度センサ又は前記移動体の角速度を観測する角速度センサのどちらか一方若しくは両方と、前記移動体の位置又は方位角のどちらか一方若しくは両方を観測するＧＰＳセンサと、前記ＧＰＳセンサの観測結果の誤差推定を行うカルマンフィルタと、前記速度センサ及び前記角速度センサにより観測された前記移動体の速度又は角速度のどちらか一方若しくは両方により前記移動体の位置又は方位角のどちらか一方若しくは両方を求める航法演算を行い、前記航法演算において前記カルマンフィルタにより誤差推定を行うことで補正された前記ＧＰＳセンサの観測結果を利用して前記航法演算の演算結果を補正する航法演算手段とを備えるナビゲーション装置であり、前記ＧＰＳセンサの観測結果を利用して前記航法演算の演算結果を補正しているときに、前記ＧＰＳセンサのＧＰＳ電波受信が途絶した場合には前記ＧＰＳセンサの観測結果を利用して前記航法演算の結果の補正をすることを停止するとともに前記カルマンフィルタを停止し、その後前記ＧＰＳセンサがＧＰＳ電波を受信した場合には、ＧＰＳ電波を受信してから予め決められた時間が経過しており且つ前記ＧＰＳセンサの観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が予め決められた閾値以内である場合に、前記カルマンフィルタを起動し前記ＧＰＳセンサの観測結果を利用して前記航法演算手段は前記航法演算の演算結果の補正を開始する構成であり、また、前記ＧＰＳセンサのＧＰＳ電波受信が途絶して前記カルマンフィルタを停止し、その後前記ＧＰＳセンサがＧＰＳ電波を受信して前記カルマンフィルタを起動する場合には、推定誤差共分散行列又は観測ノイズのどちらか一方あるいは両方は予め設定されていた推定誤差共分散行列初期値及び観測ノイズ設定値を用いる構成であり、また、前記カルマンフィルタは、観測ノイズ設定値を前記ＧＰＳセンサの観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差に応じて変更する機能を有しており、前記ＧＰＳセンサの観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が閾値よりも大きい場合は、前記観測ノイズ設定値を変更することで、前記カルマンフィルタにおける前記観測ノイズに基づく誤差推定の割合を低下させる構成である。

この発明のナビゲーション装置によれば、前記ＧＰＳセンサのＧＰＳ電波受信が途絶した場合には前記ＧＰＳセンサの観測結果を利用して前記航法演算の結果の補正をすることを停止するとともに前記カルマンフィルタを停止し、その後前記ＧＰＳセンサがＧＰＳ電波を受信した場合には、ＧＰＳ電波を受信してから予め決められた時間が経過しており且つ前記ＧＰＳセンサの観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が予め決められた閾値以内である場合に、前記カルマンフィルタを起動し前記ＧＰＳセンサの観測結果を利用して前記航法演算手段は前記航法演算の演算結果の補正を開始するので、前記ＧＰＳセンサによる前記移動体の位置及び速度との観測誤差が大きい場合に、前記方位センサ及び前記速度センサの観測結果を使用した位置又は方位角のどちらか一方若しくは両方の航法演算結果の精度が悪化することを防止することができる。

この発明の実施の形態１に係るナビゲーション装置の航法演算手段のブロック図である。 図１に記載されたナビゲーション装置の動作を示すフローチャートである。 この発明の実施の形態２に係るナビゲーション装置の動作を示すフローチャートである。 従来のナビゲーション装置のブロック図である。

実施の形態１．
以下、この発明の実施の形態１を添付図面の図１に基づいて説明する。なお、従来例と同一または同等部分には同一符号を付して説明する。
図１は、この発明の実施の形態１に係るナビゲーション装置の構成を示す概略図である。ナビゲーション装置１０は例えば車両等の移動体に設けられており、主として、前記移動体の速度を観測する速度センサ２と、前記移動体の角速度を観測する例えばジャイロ等の角速度センサ３と、ＧＰＳ電波を受信することにより前記移動体の位置及び方位角を観測するＧＰＳセンサ４と、前記ＧＰＳセンサ４の観測結果の誤差推定を行う公知のカルマンフィルタ５と、慣性航法により移動体の位置及び方位角を求める航法演算を行う航法演算手段７とから構成されている。

前記速度センサ２が観測した前記移動体の速度ｖは、前記航法演算手段７に設けられた速度演算手段７０に出力される。前記角速度センサ３が観測した前記移動体の角速度Ωは、前記航法演算手段７に設けられた方位角演算手段７１に出力される。前記方位角演算手段７１は積分器（１／ｓ）であり、前記移動体の角速度Ωが積分されて前記移動体の航法演算結果としての方位角Ψが得られる。方位角Ψは前記速度演算手段７０と前記カルマンフィルタ５に設けられた観測手段５０とに出力されるとともに、前記航法演算手段７の方位角出力として出力される。

前記速度演算手段７０は、速度ｖと方位角Ψから、下記の式により前記移動体の南北方向の速度であるｄＮ／ｄｔの値と、東西方向の速度であるｄＥ／ｄｔの値を演算する。
ｄＮ／ｄｔ＝ｖ・ｃｏｓ（Ψ）
ｄＥ／ｄｔ＝ｖ・ｓｉｎ（Ψ）

前記移動体の南北方向の速度ｄＮ／ｄｔ及び東西方向の速度ｄＥ／ｄｔは、位置演算手段７２に出力される。位置演算手段７２は積分器（１／ｓ）であり、前記移動体の南北方向の速度ｄＮ／ｄｔ及び東西方向の速度ｄＥ／ｄｔが積分されて、前記移動体の航法演算結果としての南北方向の位置であるＮ位置Ｎ及び東西方向の位置であるＥ位置Ｅが得られる。前記移動体のＮ位置Ｎ及びＥ位置Ｅは、前記観測手段５０に出力されるとともに、前記航法演算手段７の位置出力として出力される。

前記カルマンフィルタ５の前記観測手段５０には、前記ＧＰＳセンサ４から前記移動体の位置及び方位角の観測結果がＧＰＳによる観測値として入力される。また、前述したように前記移動体の方位角Ψ，Ｎ位置Ｎ及びＥ位置Ｅが、前記観測手段５０に入力される。前記観測手段５０は、ＧＰＳによる観測値と前記移動体の方位角Ψ，Ｎ位置Ｎ及びＥ位置Ｅとの差を観測値として前記カルマンフィルタ５に設けられた誤差の推定量５１へ出力する。

前記誤差の推定量５１へ、前記観測手段５０から観測値が入力される。観測値について、前記誤差の推定量５１によりカルマンフィルタを用いた誤差推定が行われる。次に、誤差推定に基づく推定誤差が前記航法演算手段７にフィードバックされ、航法演算において演算結果である前記移動体の方位角Ψ，前記移動体のＮ位置Ｎ及びＥ位置Ｅを補正するための補正情報として利用される。これにより、前記航法演算手段７における航法演算の演算結果が補正される。

次に、この発明の実施の形態１に係る前記ナビゲーション装置１０の、ＧＰＳ電波途絶時の動作について図２のフローチャートを用いて説明する。
前記ナビゲーション装置１０が航法演算を開始すると、前記航法演算手段７と前記ＧＰＳセンサ４とが起動される（ステップＳ１）。次に、前記ＧＰＳセンサ４がＧＰＳ電波を受信できるかが判定される（ステップＳ２）。

前記ＧＰＳセンサ４がＧＰＳ電波を受信できる場合には、前記カルマンフィルタ５が起動される（ステップＳ３）。次に、前記航法演算手段７の慣性航法によって求められた前記移動体の方位角Ψ，前記移動体のＮ位置Ｎ及びＥ位置Ｅと前記ＧＰＳセンサ４の観測値が前記カルマンフィルタ５に入力され、前記カルマンフィルタ５による誤差推定が行われて、誤差推定による推定誤差を利用して前記移動体の航法演算の結果としての方位角Ψと前記移動体のＮ位置Ｎ及びＥ位置Ｅが補正されて航法演算が行われる（ステップＳ４）。

航法演算中は、前記ナビゲーション装置１０によって前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶したかが判定されている（ステップＳ５）。前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶していない場合は、前記ステップＳ４の航法演算が継続される。前記ＧＰＳセンサ４の電波受信が途絶した場合、前記カルマンフィルタ５の演算が停止される（ステップＳ６）。次に、前記航法演算手段７により、前記ＧＰＳセンサ４の観測値及び前記カルマンフィルタ５を使用せず誤差推定を行わない、前記航法演算手段７のみによる航法演算が行われる（ステップＳ７）。

前記航法演算手段７のみによる航法演算が行われているときに、前記ＧＰＳセンサ４がＧＰＳ電波を受信したら、ＧＰＳ電波を受信してから例えば５秒以上が経過しているかが前記航法演算手段７によって判定される。経過している場合は、前記航法演算手段７のみで航法演算を行い得られた前記移動体のＮ位置Ｎ及びＥ位置Ｅから計算された前記移動体の１秒間における移動距離と、前記ＧＰＳセンサ４の観測結果から得られた前記移動体の位置から前記移動体の１秒間における移動距離とを１秒毎に前記航法演算手段７が比較し、差が予め決められた閾値Ｘ以内であるかを判定する（ステップＳ８）。前記ステップＳ８における判定が偽であれば、前記ステップＳ７の前記航法演算手段７による航法演算が継続される。

前記ステップＳ８における判定が真である場合は、前記カルマンフィルタ５が起動される。この時、前記カルマンフィルタ５の推定誤差共分散行列及び観測値のノイズ（観測ノイズ）には、推定誤差共分散行列の初期値及び観測ノイズ設定値を使用する（ステップＳ９）。これにより、前記ＧＰＳセンサ４の観測値による前記航法演算の演算結果の誤差補正量が前記ＧＰＳセンサ４の電波受信が途絶した直後よりも減少する。次に、前記ステップＳ４から航法演算が継続されることで、前記ＧＰＳセンサ４の観測結果を利用して前記航法演算手段７は前記航法演算の演算結果の補正を開始する。上述のようにして得られた前記移動体のＮ位置Ｎ、Ｅ位置Ｅ及び方位角Ψを基に、前記ナビゲーション装置１０は移動体のナビゲーションを行うことができる。

このような前記ナビゲーション装置１０では、前記ＧＰＳセンサ４の観測結果を利用して航法演算の演算結果を補正しているときに、前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶した場合には前記ＧＰＳセンサ４の観測結果を利用して航法演算の結果の補正をすることを停止するとともに前記カルマンフィルタ５を停止し、その後前記ＧＰＳセンサ４がＧＰＳ電波を受信した場合には、ＧＰＳ電波を受信してから５秒以上が経過しており且つ前記ＧＰＳセンサ４の観測結果に基づいて求められた１秒間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が予め決められた閾値Ｘ以内である場合に、前記カルマンフィルタ５を起動し前記ＧＰＳセンサ４の観測結果を利用して前記航法演算手段７は前記航法演算の演算結果の補正を開始し、また、前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶して前記カルマンフィルタ５を停止し、その後前記ＧＰＳセンサ４がＧＰＳ電波を受信して前記カルマンフィルタ５を起動する場合には、推定誤差共分散行列及び観測ノイズは予め設定されていた推定誤差共分散行列初期値及び観測ノイズ設定値を用い、また、推定誤差共分散行列及び観測ノイズは予め設定されていた推定誤差共分散行列初期値及び観測ノイズ設定値を用いる構成であるので、前記ＧＰＳセンサ４による前記航法演算の誤差補正量が前記ＧＰＳセンサ４の電波受信が途絶した直後よりも減少し、前記角速度センサ３及び前記速度センサ２の観測結果を使用した位置及び方位角の航法演算結果の精度が悪化することを防止することができる。

実施の形態２．
次に、この発明の図３に示す実施の形態２に係るナビゲーション装置を示す。尚、実施の形態２において、図１及び図２の参照符号と同一の符号は、同一または同様な構成要素であるので、その詳細な説明は省略する。

実施の形態２に係るナビゲーション装置の構成は、実施の形態１と同じである。又、図３に示すように、前記ステップＳ４までの動作は実施の形態１と同じである。

前記ステップＳ４の次に、前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が、閾値Ｙよりも大きいか否かが前記航法演算手段７により判定される（ステップＳ１０）。

次に、前記ステップＳ１０における判定が偽であれば、前記ステップＳ５の判定が行われる。前記ステップＳ１０における判定が真であれば、前記カルマンフィルタ５における前記観測ノイズに基づく誤差推定の割合が低下するように、前記観測ノイズ設定値が前記カルマンフィルタ５により変更される（ステップＳ１１）。その後、前記ステップＳ５の判定が行われる。前記ステップＳ５以降の動作は実施の形態１と同じである。

前記ステップＳ１０では、前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が大きくなる場合を、前記移動体が建物の近傍を通過した場合等に前記ＧＰＳセンサ４により観測された前記移動体の方位及び速度と、前記移動体の真の方位及び速度との間の誤差が大きくなる場合とみなしている。そして、前記誤差が閾値Ｙよりも大きい場合に、前記カルマンフィルタ５における前記観測ノイズに基づく誤差推定の割合が低下するように、前記観測ノイズ設定値が前記カルマンフィルタ５により変更されることで、前記誤差が大きくなる場合の航法演算精度への影響を低減している。具体的には、前記ＧＰＳセンサ４の観測値に基づく前記カルマンフィルタ５から前記航法演算手段７に、フィードバックされる推定誤差の大きさを小さくして、前記航法演算の演算結果の補正量を小さくする。

このように、前記カルマンフィルタ５は、観測ノイズ設定値を前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差に応じて変更する機能を有しており、前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が閾値Ｙよりも大きい場合は、前記観測ノイズ設定値を変更することで、前記カルマンフィルタ５における前記観測ノイズに基づく誤差推定の割合を低下させるので、前記ＧＰＳセンサ４の観測結果の誤差による観測精度の悪化を低減することができる。

尚、実施の形態１及び２では、前記ステップＳ８において前記ＧＰＳセンサ４がＧＰＳ電波を受信してから５秒以上が経過しているかを判定し、経過していれば航法演算手段７のみで航法演算を行い得られた前記移動体のＮ位置Ｎ及びＥ位置Ｅから計算された前記移動体の１秒間における移動距離と、前記ＧＰＳセンサ４の観測結果から得られた前記移動体の位置から前記移動体の１秒間における移動距離とを１秒毎に航法演算手段７が比較していたが、前記ＧＰＳセンサ４がＧＰＳ電波を受信してからの経過時間と、前記航法演算手段７が前記移動体の何秒間の移動距離を得るかと、前記航法演算手段７が何秒ごとに前記移動体の移動距離を比較するかとのそれぞれの時間は、上述の時間に限定されるものではない。例えば前記航法演算手段７内にＥＥＰＲＯＭ等の記憶装置を設け、前記それぞれの時間を適当な任意の時間に設定するものであってもよい。

また、実施の形態１及び２では、前記ＧＰＳセンサ４及び前記航法演算手段７は前記移動体の位置及び方位角を観測し或いは求めていたが、前記速度センサ２の観測結果に基づき前記移動体の位置だけを観測し或いは求めてもよいし、または前記角速度センサ３の観測結果に基づき前記移動体の方位角だけを観測し或いは求めてもよい。

また、実施の形態１及び２では、前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶後、前記ＧＰＳセンサ４がＧＰＳ電波を受信してその後前記カルマンフィルタ５が起動される場合に、前記カルマンフィルタ５は推定誤差共分散行列の初期値及び観測ノイズ設定値を用いていたが、推定誤差共分散行列の初期値及び観測ノイズ設定値のどちらか一方だけを用いてもよい。

尚、本発明によるナビゲーション装置の要旨としては、以下の通りである。すなわち、前記移動体の速度を観測する前記速度センサ２と、前記移動体の角速度を観測する前記角速度センサ３と、前記移動体の位置又は方位角のどちらか一方若しくは両方を観測するＧＰＳセンサ４と、前記ＧＰＳセンサ４の観測結果の誤差推定を行う前記カルマンフィルタ５と、前記速度センサ２及び前記角速度センサ３により観測された前記移動体の速度又は角速度のどちらか一方若しくは両方により前記移動体の位置又は方位角のどちらか一方若しくは両方を求める航法演算を行い、前記航法演算において前記カルマンフィルタ５により誤差推定を行うことで補正された前記ＧＰＳセンサ４の観測結果を利用して前記航法演算の演算結果を補正する航法演算手段７とを備えるナビゲーション装置であり、前記ＧＰＳセンサ４の観測結果を利用して前記航法演算の演算結果を補正しているときに、前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶した場合には前記ＧＰＳセンサ４の観測結果を利用して前記航法演算の結果の補正をすることを停止するとともに前記カルマンフィルタ５を停止し、その後前記ＧＰＳセンサ４がＧＰＳ電波を受信した場合には、ＧＰＳ電波を受信してから予め決められた時間が経過しており且つ前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が予め決められた閾値Ｘ以内である場合に、前記カルマンフィルタ５を起動し前記ＧＰＳセンサ４の観測結果を利用して前記航法演算手段７は前記航法演算の演算結果の補正を開始するように構成されており、前記ＧＰＳセンサ４のＧＰＳ電波受信が途絶して前記カルマンフィルタ５を停止し、その後前記ＧＰＳセンサ４がＧＰＳ電波を受信して前記カルマンフィルタ５を起動する場合には、推定誤差共分散行列又は観測ノイズのどちらか一般または両方は予め設定されていた推定誤差共分散行列初期値及び観測ノイズ初期値を用いる構成であり、また、推定誤差共分散行列及び観測ノイズは予め設定されていた推定誤差共分散行列初期値及び観測ノイズ設定値を用いる構成であり、また、前記カルマンフィルタ５は、観測ノイズ設定値を前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差に応じて変更する機能を有しており、前記ＧＰＳセンサ４の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が閾値Ｙよりも大きい場合は、前記観測ノイズ設定値を変更することで、前記カルマンフィルタ５における前記観測ノイズに基づく誤差推定の割合を低下させるように構成されている。

本発明によるナビゲーション装置では、ＧＰＳセンサの観測結果を利用して航法演算の演算結果を補正しているときに、ＧＰＳセンサのＧＰＳ電波受信が途絶した場合にはＧＰＳセンサの観測結果を利用して航法演算の結果の補正をすることを停止するとともにカルマンフィルタを停止し、その後ＧＰＳセンサがＧＰＳ電波を受信した場合には、ＧＰＳ電波を受信してから５秒以上が経過しており且つＧＰＳセンサの観測結果に基づいて求められた１秒間における移動体の移動距離と航法演算に基づく移動体の一定時間における移動距離との差が予め決められた閾値以内である場合に、カルマンフィルタを起動し前記ＧＰＳセンサの観測結果を利用して航法演算手段は航法演算の演算結果の補正を開始し、また、ＧＰＳセンサのＧＰＳ電波受信が途絶してカルマンフィルタを停止し、その後ＧＰＳセンサがＧＰＳ電波を受信してカルマンフィルタを起動する場合には、推定誤差共分散行列又は観測ノイズのどちらか一方あるいは両方は予め設定されていた推定誤差共分散行列初期値及び観測ノイズ初期値を用い、また、カルマンフィルタは、観測ノイズ設定値をＧＰＳセンサの観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差に応じて変更する機能を有しており、ＧＰＳセンサの観測結果に基づいて求められた一定時間における移動体の移動距離と航法演算に基づく移動体の一定時間における移動距離との差が閾値よりも大きい場合は、観測ノイズ設定値を変更することで、カルマンフィルタ５における観測ノイズに基づく誤差推定の割合を低下させるので、角速度センサ及び速度センサの観測結果を使用した位置及び方位角の航法演算結果の精度が悪化することを防止することができる。

１，１０ ナビゲーション装置
２ 速度センサ
３ 角速度センサ
４ ＧＰＳセンサ
５ カルマンフィルタ
７ 航法演算手段
Ｘ，Ｙ 閾値

Claims (1)

1. 移動体の速度を観測する速度センサ（２）と、
   前記移動体の角速度を観測する角速度センサ（３）と、
   前記移動体の位置又は方位角のどちらか一方若しくは両方を観測するＧＰＳセンサ（４）と、
   前記ＧＰＳセンサ（４）の観測結果の誤差推定を行うカルマンフィルタ（５）と、
   前記速度センサ（２）及び前記角速度センサ（３）により観測された前記移動体の速度又は角速度のどちらか一方若しくは両方により前記移動体の位置又は方位角のどちらか一方若しくは両方を求める航法演算を行い、前記航法演算において前記カルマンフィルタ（５）により誤差推定を行うことで補正された前記ＧＰＳセンサ（４）の観測結果を利用して前記航法演算の演算結果を補正する航法演算手段（７）と、
   を備えるナビゲーション装置であり、
   前記ＧＰＳセンサ（４）の観測結果を利用して前記航法演算の演算結果を補正しているときに、前記ＧＰＳセンサ（４）のＧＰＳ電波受信が途絶した場合には前記ＧＰＳセンサ（４）の観測結果を利用して前記航法演算の結果の補正をすることを停止するとともに前記カルマンフィルタ（５）を停止し、その後前記ＧＰＳセンサ（４）がＧＰＳ電波を受信した場合には、ＧＰＳ電波を受信してから予め決められた時間が経過しており且つ前記ＧＰＳセンサ（４）の観測結果に基づいて求められた一定時間における前記移動体の移動距離と前記航法演算に基づく前記移動体の一定時間における移動距離との差が予め決められた閾値（Ｘ）以内である場合に、前記カルマンフィルタ（５）を起動し前記ＧＰＳセンサ（４）の観測結果を利用して前記航法演算手段（７）は前記航法演算の演算結果の補正を開始する構成であり、
   前記ＧＰＳセンサ（４）のＧＰＳ電波受信が途絶して前記カルマンフィルタ（５）を停止し、その後前記ＧＰＳセンサ（４）がＧＰＳ電波を受信して前記カルマンフィルタ（５）を起動する場合には、推定誤差共分散行列又は観測ノイズのどちらか一方或いは両方は、それぞれ予め設定されていた推定誤差共分散行列初期値及び観測ノイズ設定値を用い、
   前記カルマンフィルタ（５）は、前記差に応じて前記観測ノイズ設定値を変更する機能を有しており、前記差が閾値（Ｙ）よりも大きい場合は、前記観測ノイズ設定値を変更することで、前記カルマンフィルタ（５）における前記観測ノイズに基づく誤差推定の割合を低下させる、
   ことを特徴とするナビゲーション装置。

Images