JP2002090150A

JP2002090150A - 移動体の方位推定装置および位置推定装置

Info

Publication number: JP2002090150A
Application number: JP2000275500A
Authority: JP
Inventors: Yasuyuki Shimada; 靖之島田
Original assignee: Kawasaki Heavy Industries Ltd
Current assignee: Kawasaki Heavy Industries Ltd
Priority date: 2000-09-11
Filing date: 2000-09-11
Publication date: 2002-03-27
Anticipated expiration: 2020-09-11
Also published as: JP3505494B2

Abstract

(57)【要約】【課題】周囲の環境に左右されることなく、方位およ
び位置を常に高精度に推定することができる方位推定装
置および位置推定装置を提供する。【解決手段】ＤＧＰＳ受信器２、速度計３、地磁気セ
ンサ４およびジャイロ５の信頼性を判定し、信頼性の高
い手段２〜５が方位および位置の推定に大きく影響を与
えるように、位置・方位計算部６によって移動体の方位
および位置が推定される。さらに前記信頼性は、各手段
２〜５による検出値に基づいて判定されるので、各手段
２〜５自体の外乱要因に影響されていない通常時の精度
にだけ基づく信頼性ではなく、周囲の環境の影響を受け
た状態での信頼性を判定することができる。したがって
常に高い精度で方位および位置を推定することができ
る。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、移動体の方位を推
定するための装置および移動体の位置を推定するための
装置に関する。

【０００２】

【従来の技術】移動体の位置を推定する技術として、グ
ローバルポジショニングシステム（以下、ＧＰＳと略称
する）によって移動体の位置を推定する装置がある。こ
の装置は、衛星からの電波を受信できない場合、および
衛星からの電波が建物などで反射した反射波の影響を受
けて大きな誤差を生じている場合がある。したがってこ
のようなＧＰＳを用いる装置では、他の原理のセンサと
組合わせて、ＧＰＳによる位置を補間および補正する必
要がある。

【０００３】移動体の方位を推定する技術としても、Ｇ
ＰＳによって移動体の方位を推定する装置がある。この
装置は、位置を推定する装置と同様に、方位が得られな
い場合および大きな誤差を生じている場合がある。また
ＧＰＳから得られる方位は、移動体が低速度で移動する
場合、誤差が大きくなる。また方位を推定する装置とし
て、ジャイロを用いる装置がある。この装置では、移動
体が、低速度で大きな円を描くように移動する場合な
ど、小さな角速度で大きく方向を変える場合などは、高
精度の方位を得ることができない。

【０００４】移動体が低速度で移動する場合にも高精度
に方位を得ることができる装置として、地磁気センサを
用いた装置がある。地磁気センサは、地磁気以外の磁場
発生源が存在する付近では、その磁場の影響を受けて、
大きな誤差を生じてしまう。したがってこのような地磁
気センサを用いた装置においても、他のセンサを用いて
補正する必要がある。

【０００５】このように異種のセンサから導かれる同じ
物理量の差、したがって方位差または位置差から環境変
化による異常検出をして誤差を低減する従来の技術で
は、移動体の速度の変化を考慮したものは少ない。移動
量を考慮したものもあるが、一定時間での移動量の変化
を用いているので、速度の急な変化への応答性を欠いて
おり、精度が低くなってしまう。

【０００６】複数のセンサからの情報に基づいて方位を
推定する技術として、たとえば特開平５−３４１５７に
示される車両用走行方位検出装置がある。この装置は、
地磁気センサによって検出される地磁気成分を、水平面
上で互いに直交する２方向成分に分解し、両地磁気成分
が示す座標位置へ、この座標位置が含まれるべき出力円
の中心座標値から向かう方向に基づいて、車両の走行方
位を検出している。さらにジャイロセンサによって走行
方位変化量を検出し、一定距離走行中に出力円上におい
て地磁気センサによって検出される出力点移動量と、ジ
ャイロセンサによって検出される出力点移動量の差を求
め、この移動量差に基づいて周囲の磁場環境の良否を判
別している。この判別された磁場環境に基づいて、地磁
気センサのサンプリングデータ数を算出し、その数のサ
ンプリングデータの平均値データを求め、複数の平均値
データに基づいて出力円の候補値を算出し、この候補値
に基づいて、中心点を補正している。

【０００７】複数のセンサからの情報に基づいて方位を
推定する他の従来の技術として、たとえば特開平６−２
８８７７６に示される車両用走行方位検出装置がある。
この装置では、ＧＰＳによる方位とジャイロによる方位
とに基づいて、車両の速度に依存したＧＰＳの誤差とジ
ャイロのオフセット値の誤差およびその誤差の時間変化
率とを評価し、この評価に基づいてＧＰＳによる方位と
ジャイロによる方位とから車両の方位を推定している。

【０００８】また、複数のセンサからの情報に基づいて
位置を推定する技術として、特開平５−７９８４９に示
されるＡＶＭシステムがある。このシステムは、ＧＰＳ
による位置検出手段、地磁気センサ、ジャイロおよび車
速センセを備え、衛星からの電波を受信できる場合に
は、ＧＰＳによる位置を移動体位置とし、衛星からの電
波が受信できないときだけ地磁気センサ、ジャイロおよ
び車速センセの出力値に基づいて移動体位置を推定し、
衛星からの電波が受信できる状態に復帰すると、ＧＰＳ
による位置推定に戻り、この復帰時にＧＰＳによる位置
と、地磁気センサ、ジャイロおよび車速センセの出力値
に基づく位置とにずれが生じている場合、ＧＰＳによる
位置に補正している。

【０００９】また、複数のセンサからの情報に基づいて
位置を推定する他の従来の技術として、特開平８−３３
４３４８に示されるナビゲーション装置がある。この装
置は、ジャイロによって相対位置を求めるとともに、Ｇ
ＰＳによって絶対位置を求め、相対位置の車両の移動距
離に関する誤差範囲を基にした補正範囲を決定し、この
補正範囲とＧＰＳによる測位誤差範囲との共通範囲上
に、ＧＰＳによる絶対位置を補正して自車位置としてい
る。これによって移動距離に関する累積誤差を低減して
いる。

【００１０】

【発明が解決しようとする課題】特開平５−３４１５７
に示される車両用走行方位検出装置では、一定距離走行
中における、地磁気センサの出力点移動量とジャイロセ
ンサの出力点移動量の差を求め、この移動量差に基づい
て周囲の磁場環境の良否を判別し、方位推定のための出
力円中心の補正をしており、一定距離移動しなければ、
磁場環境の良否を判断することができず、方位の推定精
度が低下してしまう。またこの装置では、方位の推定
は、磁気センサによる出力点にだけ基づいており、磁場
環境が悪い環境下、したがって磁気センサよりもジャイ
ロの方が信頼できる環境下であっても、磁気センサによ
る出力点に基づいて方位を推定しており、方位の推定精
度が低い。

【００１１】また特開平６−２８８７７６に示される車
両用走行方位検出装置では、ＧＰＳおよびジャイロの誤
差を評価して方位を求めているけれども、これらの誤差
の評価は、装置自体の精度に基づく誤差の評価であり、
周囲の環境の影響に対する誤差の評価がなされていな
い。したがってこの装置は、たとえばＧＰＳが周囲の環
境に影響された場合、方位の推定精度が低くなってしま
う。

【００１２】さらに特開平５−７９８４９に示されるＡ
ＶＭシステムでは、基本的にＧＰＳによって位置を推定
し、衛星からの電波が受信できないときだけ地磁気セン
サ、ジャイロおよび車速センセの出力値に基づいて位置
を推定する構成である。したがってこのシステムは、Ｇ
ＰＳが周囲の環境に影響された場合、位置の推定精度が
低くなってしまう。

【００１３】さらに特開平８−３３４３４８に示される
ナビゲーション装置では、ＧＰＳおよびジャイロが有す
る一定の誤差を考慮し、各誤差範囲内の共通範囲上の位
置を自車位置に推定するだけである。したがってこの装
置は、周囲の環境に影響されて、しばらくの間誤差が一
定以上に大きくなると位置の推定精度が低くなってしま
う。

【００１４】したがって本発明の目的は、周囲の環境に
左右されることなく、方位を常に高精度に推定すること
ができる方位推定装置を提供することである。

【００１５】また本発明の目的は、周囲の環境による影
響されることなく、位置を常に高精度に推定することが
できる位置推定装置を提供することである。

【００１６】

【課題を解決するための手段】本発明は、相互に異なる
原理によって移動体の方位に関連する情報を取得する複
数の方位関連情報手段と、各方位関連情報手段による取
得方位関連情報に基づいて各方位関連情報手段の信頼性
を判定し、信頼性の高い方位関連情報手段による取得方
位関連情報が方位推定値に大きな影響を与えるように、
各方位関連情報手段による取得方位関連情報から移動体
の方位を推定する方位推定演算手段とを含むことを特徴
とする移動体の方位推定装置である。

【００１７】本発明に従えば、各方位関連情報手段の信
頼性を判定し、信頼性の高い方位関連情報手段による取
得方位関連情報が大きく影響を与えるように、方位推定
演算手段によって移動体の方位が推定される。さらに各
方位関連情報手段の信頼性は、各方位関連情報手段によ
る取得方位関連情報に基づいて判定されるので、各方位
関連情報手段自体の外乱要因の影響を受けていない通常
時の精度にだけ基づく信頼性ではなく、周囲の環境の影
響を受けた状態での信頼性を判定することができる。こ
のように方位を推定しようとする環境下において、各方
位関連情報手段のうち信頼性の高い方位関連情報手段が
どの方位関連情報手段であるかを判定し、信頼性の高い
方位関連情報手段に依存するように方位を推定すること
ができる。

【００１８】また本発明は、前記方位推定演算手段は、
各方位関連情報手段による取得方位関連情報に基づく方
位の差を移動体の速度で除算した値に基づいて、各方位
関連情報手段の信頼性を判定することを特徴とする。

【００１９】本発明に従えば、各方位関連情報手段の信
頼性は、各方位関連情報手段による取得方位関連情報に
基づく方位の差を速度で除算した値に基づいて判定され
るので、移動体の速度の影響をなくし、安定した信頼性
の判定が可能となる。しかも方位差を速度で除算した値
に基づいて信頼性判定をすれば、比較的長い区間での傾
向を求める必要がないので移動体の移動距離に拘わら
ず、リアルタイムで異常を検出しつつ方位を推定するこ
とができる。

【００２０】また本発明は、相互に異なる原理によって
移動体の位置に関連する情報を取得する複数の位置関連
情報手段と、各位置関連情報手段による取得位置関連情
報に基づいて各位置関連情報手段の信頼性を判定し、信
頼性の高い位置関連情報手段による取得位置関連情報が
位置推定値に大きな影響を与えるように、各位置関連情
報手段による取得位置関連情報から移動体の位置を推定
する位置推定演算手段とを含むことを特徴とする移動体
の位置推定装置である。

【００２１】本発明に従えば、各位置関連情報手段の信
頼性を判定し、信頼性の高い位置関連情報手段による取
得位置関連情報が大きく影響を与えるように、位置推定
演算手段によって移動体の位置が推定される。さらに各
位置関連情報手段の信頼性は、各位置関連情報手段によ
る取得位置関連情報に基づいて判定されるので、各位置
関連情報手段自体の外乱要因の影響を受けていない通常
時の精度にだけ基づく信頼性ではなく、周囲の環境の影
響を受けた状態での信頼性を判定することができる。こ
のように位置を推定しようとする環境下において、各位
置関連情報手段のうち信頼性の高い位置関連情報手段が
どの位置関連情報手段であるかを判定し、信頼性の高い
位置関連情報手段に依存するように位置を推定すること
ができる。

【００２２】また本発明は、前記位置推定演算手段は、
各位置関連情報手段による取得位置関連情報に基づく位
置の差を移動体の速度で除算した値に基づいて、各位置
関連情報手段の信頼性を判定することを特徴とする。

【００２３】本発明に従えば、各位置関連情報手段の信
頼性は、各位置関連情報手段による取得位置関連情報に
基づく位置の差を速度で除算した値に基づいて判定され
るので、移動体の速度の影響をなくし、安定した信頼性
の判定が可能となる。しかも位置差を速度で除算した値
に基づいて信頼性判定をすれば、比較的長い区間での傾
向を求める必要がないので移動体の移動距離に拘わら
ず、リアルタイムで異常を検出しつつ位置を推定するこ
とができる。

【００２４】

【発明の実施の形態】図１は、本発明の実施の一形態の
移動体の位置推定装置１の概略的構成を示すブロック図
である。位置推定装置１は、たとえば地表面に沿って２
次元的に移動する移動体（後述の図３に図示）２０に搭
載され、この移動体２０の位置を推定するための装置で
ある。移動体２０は、移動する物体であればよく、たと
えば自動車などの車両であってもよいし、また遠隔操縦
される建設機械および産業機械などの機械であってもよ
いし、さらに自律移動ロボットなどの移動するロボット
であってもよい。図面上では、移動体２０として車両を
模式的に示す。

【００２５】位置推定装置１は、位置検出手段であるデ
ィファレンシャル方式グローバルポジショニングシステ
ム（略称ＤＧＰＳ）受信器２と、速度検出手段である速
度計３と、方位検出手段である地磁気センサ４と、方位
角速度検出手段であるジャイロ５と、演算処理手段であ
る位置・方位計算部６とを含む。ＤＧＰＳ受信機２、速
度計３、地磁気センサ４およびジャイロ５は、位置関連
情報手段であり、位置・方位計算部６は、位置推定演算
手段である。以下、ＤＧＰＳ受信器２、速度計３、地磁
気センサ４およびジャイロ５を総称するときは、位置関
連情報手段という場合がある。

【００２６】このように位置推定装置１は、複数の位置
関連情報手段２〜５と、位置・方位計算部６とを含む。
各位置関連情得手段、したがってＤＧＰＳ受信器２、速
度計３、地磁気センサ４およびジャイロ５は、相互に異
なる原理によって移動体２０の位置に関連する情報を取
得する。位置関連情報は、移動体２０の位置Ｐおよび移
動体２０の位置を演算によって求めるための情報、たと
えば移動体２０の速度Ｖ、方位θおよび方位角速度ωを
含む。位置推定演算手段である位置・方位計算部６は、
各位置関連情報手段２〜５による取得位置関連情報に基
づいて、各位置関連情報手段２〜５の信頼性を判定し、
これらのうち信頼性の高い位置関連情報手段２〜５によ
る取得位置関連情報が位置推定値に大きな影響を与える
ように、各位置関連情報手段２〜５による取得位置関連
情報から移動体２０の位置Ｐ０を推定する。

【００２７】また本実施の形態の位置推定装置１は、方
位推定装置７を含んでおり、この方位推定装置７は、前
記地磁気センサ４と、前記ジャイロ５と、前記位置・方
位計算部６とを含む。地磁気センサ４およびジャイロ５
は、位置関連情報手段であるとともに方位関連情報手段
でもあり、位置・方位計算部６は、位置推定演算手段で
あるとともに方位推定演算手段でもある。以下、地磁気
センサ４およびジャイロ５を総称するときは、方位関連
情報手段という場合がある。

【００２８】このように方位推定装置１に含まれる方位
推定装置７は、複数の方位関連情報手段４，５と、位置
・方位計算部６とを含む。各方位関連情報手段、したが
って地磁気センサ４およびジャイロ５は、相互に異なる
原理によって移動体２０の方位に関連する情報を取得す
る。方位関連情報は、移動体２０の方位θおよび移動体
２０の方位を演算によって求めるための情報、たとえば
方位角速度ωを含む。方位推定演算手段である位置・方
位計算部６は、各方位関連情報手段４，５による取得方
位関連情報に基づいて、各方位関連情報手段４，５の信
頼性を判定し、これらのうち信頼性の高い方位関連情報
手段４，５による取得方位関連情報が方位推定値に大き
な影響を与えるように、各方位関連情報手段４，５によ
る取得方位関連情報から移動体２０の方位θ０を推定す
る。

【００２９】図２は、位置推定装置１の具体的構成を示
すブロック図である。ＤＧＰＳ受信器２は、複数の衛星
からの電波信号であるＧＰＳ信号を受信し、このＧＰＳ
信号の表す位置を求めるための情報、および正確な位置
が検出されている基準局から送られる補正情報に基づい
て、経度および緯度で表される移動体２０の絶対位置Ｐ
を高精度に検出する。以下、絶対位置ＰをＤＧＰＳ測位
データという場合がある。

【００３０】速度計３は、移動体２０の速度Ｖを検出す
る。この速度計３は、たとえば移動体２０が車両などの
車輪を有する構成である場合、この車輪の回転速度をエ
ンコーダなどによって検出して、速度Ｖを求める構成で
あってもよい。また速度計３は、たとえば固定物から赤
外線を発信し、移動体２０で反射した反射波を受信し
て、波長の変化から速度Ｖを検出する構成であってもよ
い。

【００３１】地磁気センサ４は、移動体に対する地磁気
方位（地磁気のＮ極の方位）を検出し、地磁気方位に対
する移動体２０の進行方位の成す方位角で表される絶対
方位θを求める。ジャイロ５は、移動体２０の方位の時
間変化率である方位角速度ωを検出する。ジャイロ５
は、たとえば機械式ジャイロ、ガスレートジャイロ、振
動ジャイロおよび光ファイバジャイロなどで実現されて
もよい。

【００３２】位置・方位計算部６は、方位推定部１０
と、位置推定部１１とを有する。方位推定部１０は、地
磁気センサ４およびジャイロ５の各検出値（各取得方位
関連情報）に基づいて、移動体２０の絶対方位（以下
「方位推定値」という場合もある）θを推定する。この
方位推定部１０は、オフセット設定器１２と、方位角速
度減算器１３と、方位角速度積分器１４と、方位減算器
１５と、方位信頼性判定器１６と、方位調整器１７とを
含む。

【００３３】オフセット設定器１２は、移動体２０の初
期状態における絶対方位に基づいて、オフセット値ω１
が設定されて記憶されている。方位角速度減算器１３
は、検出方位角速度ωおよびオフセット値ω１が与えら
れるとともに、方位調整器１７から方位用補正値ω２が
与えられる。この方位角速度減算器１３では、検出方位
角速度ωから方位用補正値ω２およびオフセット値ω１
が減算される。

【００３４】方位角速度積分器１４には、方位角速度減
算器１３から演算方位角速度ω３（＝ω−ω１−ω２）
が与えられる。この方位角速度積分器１４では、演算方
位角速度ω３を積分して、移動体２０の絶対方位を求
め、この演算絶対方位を方位推定値θ０として出力す
る。方位減算器１５には、方位推定値θ０および地磁気
センサ４によって検出される検出絶対方位θが与えられ
る。この方位減算器１５では、方位推定値θ０から検出
絶対方位θを減算し、方位差Δθ（＝θ０−θ）を求め
る。

【００３５】方位信頼性判定器１６には、方位差Δθお
よび速度Ｖが与えられる。この方位信頼性判定器１６で
は、方位差Δθを検出速度Ｖで除算して得られる方位用
判定値Ｊθに基づいて、地磁気センサ４およびジャイロ
５の信頼性、具体的には地磁気センサ４およびジャイロ
５のどちらの信頼性が高いかが判定され、この判定結果
に基づく方位ゲインＧθを方位調整器１７に与える。

【００３６】方位調整器１７には、方位差Δθおよび方
位ゲインＧθが与えられる。方位調整器１７では、方位
差Δθを予め定める伝達関数に従って演算し、方位角速
度減算器１３に与える方位用補正値ω２を求める。この
方位用補正値ω２を求めるための伝達関数における方位
ゲインは、可変であり、方位信頼性判定器１６から与え
られる方位ゲインＧθである。方位ゲインＧθは、地磁
気センサ４およびジャイロ５うち信頼性の高い方の方位
関連情報が方位推定値θ０に大きく影響を与えるよう
に、決定されている。

【００３７】この位置推定装置１では、ジャイロ５によ
る方位角速度に基づいて演算される方位推定値θ０を、
地磁気センサ４による検出絶対方位θに基づいて補正す
るようにして、移動体２０の方位が推定される。つまり
方位推定において積分演算して求める積分型センサであ
るジャイロ５と、方位推定において積分演算せずに求め
る非積分型センサである地磁気センサ４とを組合わせ
て、移動体２０の方位が推定される。

【００３８】図３は各方位関連情報手段４，５の信頼性
判定における速度の影響を説明するための図であって、
図３（１）は移動体２０の移動経路２１を示す平面図で
あり、図３（２）は移動体２０が移動経路２１を移動し
たときの時間ｔと方位差Δθとの関係を示すグラフであ
り、図３（３）は移動体２０が移動経路２１を移動した
ときの時間ｔと方位用判定値（＝方位差／速度）Ｊθと
の関係を示すグラフである。図３（２）で、横軸が時間
ｔであり、縦軸が方位差Δθである。図３（３）で、横
軸が時間ｔであり、縦軸が方位用判定値Ｊθであるであ
る。地磁気センサ４およびジャイロ５の信頼性は、単に
方位差Δθを見ても判定できないが、ジャイロ５自体の
精度による方位差は急激に変化することがないのに対し
て、磁場発生源Ｓ１などの外乱要因の地磁気センサ４へ
の影響による方位差は急激な変化をするので、方位差Δ
θの変化率を見ることによって判定することができる。

【００３９】移動体２０が、図３（１）に示すような外
乱要因である磁場発生源Ｓ１に隣接する直線状の移動経
路２１を移動する場合、ジャイロ５に基づく方位は変化
しないけれども、地磁気センサ４による検出方位θは、
磁場発生源Ｓ１の影響を受けて変化する。したがって方
位差Δθは時間ｔの経過に従って変化する。

【００４０】この方位差Δθと時間ｔとの関係は、グラ
フ線２３で示す移動体２０が低速度で移動した場合と、
グラフ線２４で示す移動体２０が高速度で移動した場合
とで、異なる。換言すれば、単位時間あたりの方位差の
変化率は、移動体の速度によって変動する。したがって
移動体２０の移動速度Ｖが影響するために、方位差Δθ
の時間変化率では、信頼性の判定が困難である。

【００４１】本実施の形態では、方位差Δθを移動体の
検出速度Ｖで除算した方位用判定値Ｊθを用いている。
この方位用判定値Ｊθと時間ｔとの関係は、グラフ線２
５で示す移動体２０が低速度で移動した場合と、グラフ
線２６で示す移動体２０が高速度で移動した場合とが、
同様の関係となる。このように単位移動距離あたりの変
化率に比例する値に換算することができ、移動体の速度
Ｖの影響を受けない値とすることができる。したがって
この方位用判定値Ｊθを用いて、地磁気センサ４および
ジャイロ５のうちいずれの信頼性が高いかを判定するこ
とができる。

【００４２】図４は、方位ゲインＧθを示すグラフであ
る。図４で、横軸が方位用判定値Ｊθであり、縦軸が方
位ゲインＧθである。また図５は、方位調整器１７の状
態遷移を示す図である。方位ゲインＧθは、上述のよう
に方位用判定値Ｊθに基づいて決定される。

【００４３】方位用判定値Ｊθが０以上でありかつ第１
しきい値Ｊθ１未満の小さい値である場合には、地磁気
センサ４およびジャイロ５の両方の信頼性が高いと判断
され、方位調整器１７が異常なし状態３０とされる。こ
の場合、地磁気センサ４およびジャイロ５のいずれの影
響が大きくなっても、高い推定精度を確保できるけれど
も、本実施の形態では、周囲の環境に影響されず、検出
方位角速度ωの分散が小さく安定しているジャイロ５の
影響が大きくなるようにして、方位の推定精度をさらに
高くしている。したがって異常なし状態３０では、方位
ゲインＧθは、地磁気センサ４およびジャイロ５の両方
が方位推定値θ０に影響し、かつジャイロ５による検出
方位角速度ωが方位推定値θ０に大きく影響するよう
に、比較的小さい第１ゲイン値Ｇθ１とされる。

【００４４】また、方位ゲインＧθを比較的小さい値Ｇ
θ１に設定することで、周囲の環境の影響による誤差が
発生したときに方位推定装置７が誤差を除去しようとす
る力が弱まるので、誤差に対する方位差Δθへの感度が
上がり、異常の検出を容易にしている。なお、地磁気セ
ンサ４とジャイロ５の各誤差が小さくなくても偶然同程
度となった場合にもこの状態になり得るが、各センサは
相互に異なる原理によるものなので、この状態は長くは
続かず、いずれ誤差が顕在化し検出できるようになる。

【００４５】方位用判定値Ｊθが第１しきい値Ｊθ１以
上でありかつ第２しきい値Ｊθ２（＞Ｊθ１）未満の中
間的な値である場合には、方位差Δθが地磁気センサ４
およびジャイロ５のいずれの原因によるものであるか判
断しにくい。地磁気センサ４は、周囲に地磁気に対する
外乱要因が存在しない場合には、誤差が発生したとして
も環境の影響による誤差に比べてその大きさは小さく、
しかも図６に示すように短期的なものであるので、誤っ
てジャイロ５の影響とみなし地磁気センサの推定値への
影響を大きくしたとしても、その影響は少ない。これに
対して、ジャイロ５は、誤差が発生すると方位推定値に
次第に蓄積されていくので、誤って地磁気センサ４の影
響と見なしジャイロ５の推定値への影響を大きくする
と、図６に示すように誤差の蓄積が加速され推定値が真
の値から外れていってしまう。したがって、本実施の形
態では、方位差はジャイロ５の誤差による可能性が高い
と判断され、方位調整器１７が積分型抑制状態３１とさ
れる。つまり方位に関して積分型センサであるジャイロ
５の影響を抑制する状態であり、地磁気センサ４の影響
が大きくなるようにしている。したがって方位ゲインＧ
θは、地磁気センサ４およびジャイロ５の両方が方位推
定値θ０に影響し、かつ地磁気センサ４による検出絶対
方位θが方位推定値θ０に大きく影響するように、比較
的大きな第２ゲイン値Ｇθ２（＞Ｇθ１）とされる。

【００４６】方位用判定値Ｊθが第２しきい値Ｊθ２以
上の大きい値である場合には、地磁気センサ４が外乱要
因の影響を受けていると判断され、方位調整器１７が差
無視状態３２とされる。この場合、ジャイロ５を信頼し
て、ジャイロ５だけまたはほぼジャイロ５だけに基づい
て、方位が推定されるようにしている。したがって差無
視状態３２では、方位ゲインＧθは、地磁気センサ４に
よる検出絶対方位θが方位推定値θ０に影響しないまた
はほとんど影響しないように、第１ゲイン値Ｇθ１より
も小さいほぼ０に近い第３ゲイン値Ｇθ３以下の値、し
たがって０またはほぼ０に近い値とされる。しかもこの
場合、方位ゲインＧθは、方位用判定値Ｊθが大きくな
るにつれて小さくなるように決定されており、より好適
に高精度に方位推定が可能である。

【００４７】方位調整器１７は、上述のように信頼性判
定に基づいて状態が決定される。方位用判定値Ｊθが小
さい場合、異常なし状態３０に遷移し、方位用判定値Ｊ
θが中間的な値の場合、積分型抑制状態３１に遷移し、
方位用判定値Ｊθが大きい場合、差無視状態３２に遷移
する。

【００４８】方位に関して非積分型センサである地磁気
センサ４は、累積誤差を有しないので、外乱要因が存在
しない場合、絶対方位を高精度に検出することができる
けれども、外乱要因が存在すると、その影響を受けて検
出絶対方位に大きな誤差が含まれてしまう。また方位に
関して積分型センサであるジャイロ５は、地磁気センサ
４のように外乱要因の影響を受けないけれども、累積誤
差などに起因して、外乱要因がない場合の地磁気センサ
４に比べて精度が低い。

【００４９】この点を踏まえ、上述のように各方位関連
情報取得手段４，５の信頼性を判定し、信頼性の高い方
位関連情報取得手段による取得位置関連情報のうち、信
頼性の高い方が大きく影響を与えるように、方位推定部
１０によって移動体２０の方位が推定される。さらに各
方位関連情報手段４，５の信頼性は、取得位置関連情報
に基づいて判定されるので、方位を推定しようとする環
境下において、各方位関連情報手段４，５のうち信頼性
の高い位置関連情報手段がどれであるかを判定してい
る。したがってリアルタイムで異常を検出しつつ常に高
い精度で方位を推定することができる。

【００５０】さらに各方位関連情報手段４，５の信頼性
は、方位差Δθを速度Ｖで除算した方位用判定値Ｊθに
基づいて判定されるので、移動体２０の速度の影響をな
くし、安定した信頼性の判定が可能となる。

【００５１】再び図２を参照して、位置推定部１１は、
方位推定部１０による方位推定値θ０ならびにＤＧＰＳ
受信器２および速度計３の各検出値、換言すればＤＧＰ
Ｓ受信器２、速度計３、地磁気センサ４およびジャイロ
５の各検出値（各取得位置関連情報）に基づいて、移動
体２０の絶対位置（以下「位置推定値」という場合もあ
る）Ｐを推定する。この位置推定部１１は、速度成分演
算器３５と、速度減算器３６と、速度積分器３７と、位
置減算器３８と、位置信頼性判定器３９と、位置調整器
４０とを含む。

【００５２】速度成分演算器３５には、方位推定値θ０
および検出速度Ｖが与えられる。この速度成分演算器３
５では、移動体２０の速度Ｖの直交２軸方向の成分を演
算する。２軸方向は、経度線および緯度線にそれぞれ平
行な方向であり、緯度線方向の速度成分Ｖｘおよび経度
線方向の速度成分Ｖｙは、次の式（１）および式（２）
によって求められる。Ｖｘ＝Ｖｃｏｓθ０ …（１）Ｖｙ＝Ｖｓｉｎθ０ …（２）

【００５３】速度減算器１３には、各速度成分Ｖｘ，Ｖ
ｙが与えられるとともに、位置調整器４０から位置用補
正値Ｖｘ１，Ｖｙ１が与えられる。この速度減算器１３
では、緯度線方向の検出速度成分Ｖｘから緯度線方向の
位置補正値Ｖｘ１が減算されるとともに、経度線方向の
検出速度成分Ｖｙから経度線方向の位置補正値Ｖｙ１が
減算され、演算速度Ｖｘ２，Ｖｙ２が求められる。

【００５４】速度積分器３７には、演算速度Ｖｘ２（＝
Ｖｘ−Ｖｘ１），Ｖｙ２（＝Ｖｙ−Ｖｙ２）が与えられ
る。この速度積分器３７では、演算速度Ｖｘ２，Ｖｙ２
をそれぞれ積分して、移動体２０の絶対位置を求めて位
置推定値Ｐ０として出力する。位置推定値Ｐ０は、具体
的には各演算速度Ｖｘ２，Ｖｙ２を積分した緯度ｘ０お
よび経度ｙ０で表される。

【００５５】位置減算器３８には、位置推定値Ｐ０およ
び検出絶対位置Ｐが与えられる。この位置減算器３８で
は、位置推定値Ｐ０から検出絶対位置Ｐを減算する。具
体的には、経度および緯度に関して、検出緯度ｘから推
定緯度ｘ０を減算するとともに、検出経度ｙから推定経
度ｙ０を減算して、緯度差Δｘ（＝ｘ−ｘ０）および経
度差Δｙ（＝ｙ−ｙ０）で表される位置差ΔＰ（＝Ｐ０
−Ｐ）を求める。

【００５６】位置信頼性判定器３９には、位置差ΔＰ、
検出速度Ｖが与えられるとともに、ＤＧＰＳ受信器２か
らＤＧＰＳ測位データが有効か無効かの情報が与えられ
る。この位置信頼性判定器３９では、位置差ΔＰを移動
体２０の速度Ｖで除算した位置用判定値ＪＰ、位置差Δ
ＰおよびＤＧＰＳ測位データが有効か無効かに基づい
て、ＤＧＰＳ受信器２、速度計３、地磁気センサ４およ
びジャイロ５の信頼性、具体的にはＤＧＰＳ受信器２
と、速度計３、地磁気センサ４およびジャイロ５を含む
組合体とのどちらの信頼性が高いかが判定され、この判
定結果に基づく位置ゲインＧＰを位置調整器４０に与え
る。冗長的な表現を避けるために、以下、速度計３、地
磁気センサ４およびジャイロ５を含む構成を「組合体」
と記す場合がある。

【００５７】ＤＧＰＳ受信器２は、受信可能なＧＰＳ衛
星数や基準局からの補正情報の有無等の高精度な測位に
必要な条件を満たしていないと判定すると、ＤＧＰＳ測
位データとともにそのデータが有効か無効かを表す判定
データも出力することができる。無効の場合、減算器３
８の出力ΔＰもまた無効なデータとなる。ただし、この
ＤＧＰＳ受信器２の判定は反射波などの外乱要因には充
分対応できるものではない。そこでＤＧＰＳ受信器２の
判定データを基に、位置信頼性判定器でＤＧＰＳ測位デ
ータの有効／無効を調べ、無効の場合は位置ゲインＧＰ
を０に設定して無効データの位置推定値Ｐ０への影響を
遮断し、速度Ｖｘ，Ｖｙを直接積分した値を位置推定値
Ｐ０に利用する。

【００５８】位置調整器４０には、位置差ΔＰおよび位
置ゲインＧＰが与えられる。位置調整器４０では、位置
差ΔＰを予め定める伝達関数に従って演算し、速度減算
器３６に与える位置用補正値Ｖｘ１，Ｖｙ１を求める。
位置用補正値Ｖｘ１，Ｖｙ１を求めるための伝達関数に
おける位置ゲインは、可変であり、位置信頼性判定器３
９から与えられる位置ゲインＧＰである。位置ゲインＧ
Ｐは、ＤＧＰＳ受信器２と、組合体とのうち信頼性の高
い方の位置関連情報が位置推定値に大きく影響を与える
ように、決定されている。

【００５９】この位置推定装置１では、地磁気センサ４
による方位で補正しながらジャイロ５による方位角速度
に基づいて演算される方位推定値θ０と、速度計３によ
る速度と、に基づいて演算される位置推定値Ｐ０を、Ｄ
ＧＰＳ受信器２による検出絶対位置Ｐに基づいて補正す
るようにして、移動体２０の位置が推定される。つまり
位置推定において積分演算して求める積分型センサとな
る速度計３ならびにジャイロ５および地磁気センサ４
と、位置推定において積分せずに求める非積分型センサ
となるＤＧＰＳ受信器２とを組合わせて、移動体２０の
位置が推定される。

【００６０】ＤＧＰＳ受信器２と、組合体との信頼性を
判定するにあたって、ＤＧＰＳ受信器２がＤＧＰＳ信号
を受信できない場合は別として、単に位置差ΔＰを見て
も判定ができない。これに対して組合体において各手段
３〜５の累積誤差などで生じる方位差は急激に変化する
ことがなく、たとえば建物である反射物でのＧＰＳ信号
の反射波などの外乱要因のＤＧＰＳ受信器２への影響に
よる位置差は急激な変化をするので、位置差ΔＰの変化
率を見ることによって、ＤＧＰＳ受信器２および組合体
の信頼性を判定することができる。

【００６１】移動体２０が、図３（１）の地磁気の外乱
要因（磁場発生源Ｓ１）をＧＰＳ信号の外乱要因に置換
えたような、移動経路を移動する場合、組合体に基づい
て演算される位置は変化しないけれども、ＤＧＰＳ受信
器２による検出位置Ｐは、外乱要因の影響を受けて変化
する。したがって位置減算器３８で求められる位置差Δ
Ｐは時間ｔの経過に従って変化する。

【００６２】この位置差ΔＰと時間ｔとの関係は、図３
（１）の縦軸を位置差ΔＰに置換えた関係を示し、移動
体２０の速度によって、異なる関係となってしまう。換
言すれば、単位時間あたりの位置差の変化率が移動体の
速度によって変動する。したがって位置差ΔＰの時間変
化率を見ても、信頼性の判定することが困難である。

【００６３】本実施の形態では、位置差ΔＰを移動体の
検出速度Ｖで除算した位置用判定値ＪＰを用いている。
この位置用判定値ＪＰと時間ｔとの関係は、図３（２）
の縦軸を位置用判定値ＪＰに置換えた関係を示し、移動
体２０の速度に拘わらず同様の関係となる。このように
単位移動距離あたりの変化率に比例する値に換算するこ
とによって、移動体の速度Ｖの影響を受けない値とする
ことができる。したがってこの位置用判定値ＪＰを用い
て、ＧＰＳ信号の反射物による反射波などのＧＰＳ信号
の外乱要因によるＤＧＰＳ受信器２の信頼性低下を判定
することができ、ＤＧＰＳ受信器２および組合体のうち
いずれの信頼性が高いかを判定することができる。

【００６４】信頼性を判定するための位置用判定値ＪＰ
は、緯度線方向および経度線方向の成分毎に速度で除算
した値（Δｘ／Ｖｘ，Δｙ／Ｖｙ）であってもよく、位
置推定値Ｐ０と検出位置Ｐとの距離を速度Ｖで除算した
値（√Δｘ²＋ｙ²／Ｖ）であってもよく、さらに移動体
の方位に基づいて、移動体の進行方向およびこれに垂直
な横方向の位置差成分に分離した値を用いるようにして
もよい。

【００６５】判定を１回で簡略化したい場合は距離を速
度Ｖで除算した値を利用する。これに対して、成分ごと
に計算しておけば、距離が同じでも方向が急変するよう
な場合を検出でき、さらに移動体の方位を使い進行方向
と横方向との成分に分離すれば、たとえば前進中に真横
に移動している等の異常の判定材料の１つとして利用す
ることができる。

【００６６】位置ゲインＧＰは、上述のように位置差Δ
Ｐを速度Ｖで除算した位置用判定値ＪＰ、位置差ΔＰお
よびＤＧＰＳ測位データの有効か無効かの情報に基づい
て、位置信頼性判定器３９で、次の規則１〜規則５に従
って決定される。

【００６７】規則１：ＤＧＰＳ測位データが無効の場
合、位置ゲインＧＰ＝０とする。規則２：位置用判定値ＪＰが、０以上であり、かつ第１
しきい値ＪＰ１未満の場合、位置ゲインＧＰを第１ゲイ
ン値ＧＰ１とする。規則３：位置用判定値ＪＰが、第１しきい値ＪＰ１以上
であり、かつ第２しきい値ＪＰ２未満の場合、位置ゲイ
ンＧＰを、第１ゲイン値ＧＰ１より大きい第２ゲイン値
ＧＰ２とする。規則４：位置用判定値ＪＰが、第２しきい値ＪＰ２以上
の場合、位置ゲインＧＰを、第１ゲイン値ＧＰ１より小
さい第３ゲイン値ＧＰ３とする。規則５：検出絶対位置Ｐが推測航法による推定誤差範囲
外にある場合、位置ゲインＧＰ＝０とする。

【００６８】これらの規則１〜規則５に従って、規則１
および規則５を優先させて、位置ゲインＧＰが決定され
る。ＧＰＳ信号が建物などの遮蔽物で遮蔽されて受信す
ることができない場合、ＤＧＰＳ受信器２に基づく位置
推定は不可能である。したがって判定を容易にするため
に、まず第１のステップで、規則１に従ってＤＧＰＳ測
位データが有効か無効かが、ＤＧＰＳ受信器２からの情
報に基づいて判定され、ＤＧＰＳ測位データが無効の場
合、位置調整器４０は、ＤＧＰＳ受信器２に基づく位置
推定なしの推測航法（デッドレコニングとも呼ばれる）
状態とされる。この推測航法状態では、位置ゲインＧＰ
は、速度計３、地磁気センサ４およびジャイロ５による
検出位置関連情報だけに基づいて、位置推定値Ｐ０を求
めるように０とされる。

【００６９】図７は、規則５に従う判定を説明するため
の図である。規則１の判定条件を満たさなかった場合、
すなわちＤＧＰＳ測位データが無効と判定されると、第
２のステップとして、規則５に従う位置差ΔＰに基づく
判定に進む。ＧＰＳ信号をロスト（遺失）した後、再び
ＧＰＳ信号を捕捉して、受信可能となった直後からＤＧ
ＰＳ受信器２は測位データを有効と判定することがあ
り、この場合、ＤＧＰＳ受信器２に関する環境は、ＧＰ
Ｓ信号受信不可の環境に近い不安定な環境であるので、
ＤＧＰＳ受信器２の信頼性が低下しており、ＤＧＰＳ受
信器２による検出絶対位置Ｐが大きな誤差を含んでいる
可能性が高い。したがって判定を容易にするために、規
則１の後で規則５に従って、検出絶対位置Ｐが、推測航
法による推定誤差範囲外にあるか判定される。検出絶対
位置Ｐが推定誤差範囲外にある場合、ＤＧＰＳ受信器２
の信頼性が極めて低いと判断され、位置調整器４０は、
規則１の場合と同様に、ＤＧＰＳ受信器２に基づく位置
推定なしの推測航法状態とされる。推定誤差範囲は、Ｇ
ＰＳ信号をロストする直前の状態における位置推定値Ｐ
０に基づいて推測航法で位置推定をしたときに、この推
測航法における位置推定に含まれると考えられる誤差を
累積計算して求められる。

【００７０】ＤＧＰＳ受信器２が測位データを有効と判
定し、かつＤＧＰＳ受信器２が位置推定に寄与できる程
度の信頼性を有していれば、推定誤差範囲自体が計算さ
れていないので、規則５による判定条件を満たさない。
この規則５の判定条件を満たす可能性があるのは、ＧＰ
Ｓ信号を一旦ロスト（遺失）した後、再びＧＰＳ信号を
捕捉して受信可能となり、ＤＧＰＳ受信器２が測位デー
タを有効と判定してから、ＤＧＰＳ受信器２が位置推定
に寄与できる程度の信頼性を回復するまでの間である。

【００７１】図７に示すように、移動体２０が時間ｔ＝
０でＧＰＳ信号をロストしてから経路４５を辿るように
移動する場合、ＧＰＳ信号が受信されない無効な状態で
は、推測航法で位置推定をする。この推測航法による位
置推定値Ｐ０は、時間ｔとともに誤差が大きくなるの
で、実際の位置が含まれる可能性のある推定誤差範囲、
したがって位置推定値を中心にした推定誤差範囲は、時
間経過に従って大きくなる。具体的には、時間ｔ＝ｔ１
０における位置４６での推定誤差範囲Ａ１よりも、時間
ｔ＝ｔ１１（＞ｔ１０）における位置４７での推定誤差
範囲Ａ２が大きくなり、さらに推定誤差範囲Ａ２よりも
時間ｔ＝ｔ１２（＞ｔ１１）における位置４８での推定
誤差範囲Ａ３が大きくなる。

【００７２】規則５では、このような推定誤差範囲より
も外側（遠い位置）に、検出絶対位置Ｐがあるか否か判
定される。時間ｔ＝０でＧＰＳ信号をロストし、時間ｔ
１１でＧＰＳ信号を再捕捉した状態で、ＤＧＰＳ受信器
２による検出絶対位置Ｐを中心とした所定範囲Ｂ１が、
時間ｔ１１における推定誤差範囲Ａ２よりも外側領域に
ある場合には、ＤＧＰＳ受信器２による検出絶対位置Ｐ
が大きな誤差を含んでいると判断され、位置ゲインＧＰ
を０とする。さらに時間が経過し、時間ｔ１２で、ＤＧ
ＰＳ受信器２による検出絶対位置Ｐを中心とした所定範
囲Ｂ２が、時間ｔ１２における推定誤差範囲Ａ３と交わ
った場合、したがって各範囲Ａ３，Ｂ２が共通領域を有
する場合、ＤＧＰＳ受信器２は位置推定に寄与できる程
度の信頼性を回復したとみなされ、規則５によるゲイン
決定から除外される。ここで所定範囲Ｂ１、Ｂ２は、Ｄ
ＧＰＳ受信器２自体の外乱要因に影響されていない通常
時の精度などに基づく範囲である。

【００７３】このような規則５の判定条件を満たさなか
った場合、第３のステップとして、位置用判定値ＪＧに
基づく、規則２〜４に従う判定に進む。規則２が満たさ
れるような、位置用判定値ＪＧが０以上でありかつ第１
しきい値ＪＧ１未満の小さい値である場合には、組合体
と、ＤＧＰＳ受信器２との両方の信頼性が高いと判断さ
れ、位置調整器４０が異常なし状態とされる。この場
合、組合体およびＤＧＰＳ受信器２のいずれの影響が大
きくなっても、高い推定精度を確保できるけれども、本
実施の形態では、周囲の環境に影響されない組合体によ
る方位推定値θ０および検出速度Ｖは分散が小さく安定
しているので、組合体の影響が大きくなるようにし、位
置の推定精度をさらに高くしている。したがって異常な
し状態では、位置ゲインＧＰは、組合体およびＤＧＰＳ
受信器２の両方が位置推定値Ｐ０に影響し、かつ組合体
による方位推定値θ０および検出速度Ｖが位置推定値Ｐ
０に大きく影響するように、比較的小さい第１ゲイン値
ＧＰ１とされる。

【００７４】また、位置ゲインＧＰを比較的小さい値Ｇ
Ｐ１に設定することで、周囲の環境の影響による誤差が
発生したときに位置推定装置１１が誤差を除去しようと
する力が弱まるので、誤差に対する位置差ΔＰへの感度
が上がり、異常の検出を容易にしている。なお、ＤＧＰ
Ｓ受信器２と組合体の各誤差が小さくなくても偶然同程
度となった場合にもこの状態になり得るが、各センサは
相互に異なる原理によるものなので、この状態は長く続
かず、いずれ誤差が顕在化し検出できるようになる。

【００７５】規則３が満たされるような、位置用判定値
ＪＧが第１しきい値ＪＧ１以上でありかつ第２しきい値
ＪＧ２（＞ＪＧ１）未満の中間的な値である場合には、
位置差ΔＰが、組合体およびＤＧＰＳ受信器２のいずれ
の原因によるものであるか判断しにくい。ＤＧＰＳ受信
器２は、周囲にＧＰＳ信号に対する外乱要因が存在しな
い場合には、誤差が発生したとしても環境の影響による
誤差に比べてその大きさは小さく、しかも図６に示すよ
うに短期的なものであるので、誤って組合体の影響と見
なしＤＧＰＳ受信器２の推定値への影響を大きくしたと
しても、その影響は少ない。これに対して、組合体は、
誤差が発生すると位置推定値に次第に蓄積されていくの
で、誤ってＤＧＰＳ受信器２の影響と見なし組合体の推
定値への影響を大きくすると、図６に示すように誤差の
蓄積が加速され推定値が真の値から外れていってしま
う。したがって、本実施の形態では、組合体が含む誤差
が大きいと判断され、位置調整器１７が積分型抑制状態
とされ、積分演算して求める積分型センサである組合体
を抑制する状態である。この積分型抑制状態では、ＤＧ
ＰＳ受信器２の影響が大きくなるようにしている。した
がって位置ゲインＧＰは、組合体およびＤＧＰＳ受信器
２の両方が位置推定値Ｐ０に影響し、かつＤＧＰＳ受信
器２による検出絶対位置Ｐが位置推定値Ｐ０に大きく影
響するように、比較的大きな第２ゲイン値ＧＰ２（＞Ｇ
Ｐ１）とされる。

【００７６】規則４が満たされるような、位置用判定値
ＪＰが第２しきい値ＪＰ２以上の大きい値である場合に
は、ＤＧＰＳ受信器２が外乱要因の影響を受けて位置差
ΔＰが生じていると判断され、位置調整器４０が差無視
状態とされる。この場合、組合体を信頼して、ほぼ組合
体だけに基づいて、方位が推定されるようにしている。
したがって差無視状態では、方位ゲインＧＰは、ＤＧＰ
Ｓ受信器２による検出絶対位置Ｐが位置推定値Ｐ０にほ
とんど影響しないように、第１ゲイン値ＧＰ１よりも小
さいほぼ０に近い第３ゲイン値ＧＰ３、したがってほぼ
０に近い値とされる。

【００７７】位置調整器４０は、規則1および規則５を
満たす推測航法状態と、位置信頼性判定値ＪＰに基づい
て位置ゲインＧＰを決定する補正可能状態とで切換ると
ともに、補正可能状態では、図５に示す方位調整器１７
の遷移と同様の遷移をする。

【００７８】位置に関して非積分型センサであるＤＧＰ
Ｓ受信器２は、累積誤差を有しないので、外乱要因が存
在しない場合には、絶対位置を高精度に検出することが
できるけれども、外乱要因が存在すると、その影響を受
けて、検出される絶対位置に大きな誤差が含まれてしま
う。また位置に関して積分型センサである速度計３、地
磁気センサ４およびジャイロ５は、ＤＧＰＳ受信器２の
ように反射波などの影響を受けないけれども、累積誤差
などに起因して、外乱要因がない場合のＤＧＰＳ受信器
２に比べて精度が低い。

【００７９】この点を踏まえ、上述のようにＤＧＰＳ受
信器２、速度計３、地磁気センサ４およびジャイロ５で
ある各位置関連情報取得手段の信頼性、具体的には、組
合体とＤＧＰＳ受信器２とのいずれの信頼性が高いかを
判定し、組合体による方位推定値θ０および速度計３
と、検出絶対位置Ｐのうち、信頼性の高い方が大きく影
響を与えるように、位置推定部１１によって移動体２０
の位置が推定される。さらに取得位置関連情報に基づい
て信頼性が判定されるので、位置を推定しようとする環
境下において、各位置関連情報手段２〜５のうち信頼性
の高い位置関連情報手段がどれであるかを判定し、信頼
性の高い位置関連情報手段４，５に依存するように方位
を推定している。したがってリアルタイムで異常を判定
しつつ常に高い精度で方位を推定することができる。

【００８０】さらに各位置関連情報手段２〜５の信頼性
は、位置用判定値ＪＰに基づいて判定されるので、移動
体２０の速度の影響をなくし、安定した信頼性の判定が
可能となる。

【００８１】図８は、位置推定装置１による方位の推定
結果を示すグラフである。また図９は、図８のセクショ
ンＳ９を拡大して示すグラフである。図８および図９
で、横軸は時間であり、縦軸は方位である。図８および
図９は、移動体が長方形の周回経路を、四周した場合の
方位を示している。図８から明らかなように、破線５１
で示されるジャイロ５よる方位は、周回を重ねる毎に誤
差が累積し、また図９から明らかなように、実線５０で
示される地磁気センサ４の検出方位は、外乱要因がある
場所で誤差を含んでいる。これに対して一点鎖線５２で
示される位置推定装置１、具体的にはこれに含まれる方
位推定装置７による方位推定値θ０は、周回を重ねても
誤差の累積がなく、かつ外乱要因に影響されることな
く、高精度であることが確認できる。

【００８２】図１０は位置推定装置１による位置の推定
結果を示すグラフであって、横軸は、所定の第1方向の
位置Ｅであり、縦軸は第1方向に垂直な第２方向の位置
Ｆである。図１１は図１０の位置の推定結果を進行方向
および横方向に変換して示すグラフであって、横軸は経
路（進行方向）に沿う位置Ｋであり、縦軸は横方向に沿
う位置Ｌである。図１２は図１０の位置の推定における
地磁気センサ４による検出方位θを示すグラフであっ
て、横軸が時間であり、縦軸が方位である。図１３は図
１０の位置の推定におけるＤＧＰＳ受信器２による検出
絶対位置Ｐと位置推定値Ｐ０の差を示すグラフであっ
て、横軸は時間であり、縦軸は位置の差である。図１４
は図１０の位置の推定におけるＤＧＰＳ受信器による検
出絶対位置Ｐの緯度成分である検出緯度ｘと位置推定値
Ｐ０の緯度成分ｘ０とを示すグラフであって、横軸は時
間であり、縦軸は緯度線に沿う位置ｘｍである。ここで
図１０〜図１４における位置は、経度および緯度ではな
く、所定の位置からの距離に換算して示す。

【００８３】図１０〜図１４は、移動体が所定の一定速
度、具体的には約５ｋｍ／ｈで建物６０に沿って、ほぼ
南北に延びる直線状の移動経路を、南の方向に向かって
移動したときの位置推定の結果である。移動経路に沿っ
て、外乱要因によるＧＰＳ信号遮断領域６１および磁場
異常領域６２が存在している。

【００８４】図１０および図１１に実線６５で示される
ように、また図１３に経度線成分を実線６６でかつ緯度
成分を一点鎖線６７で示すように、ＤＧＰＳ受信器２に
よる検出位置は、信号遮断領域６１の影響を受けている
ことが確認される。また図１２に実線７０で示すよう
に、地磁気センサ４が磁場異常領域６２の影響を受け、
さらに誤差が累積して、図１０および図１１に破線７０
で示されるように、推測航法のみよる位置が経路から遠
ざかることが確認される。

【００８５】このようにＤＧＰＳ受信器２による位置お
よび組合体による推測航法による位置が、実際の位置と
異なっても、図１０および図１１に一点鎖線で示すよう
に、一点鎖線６７で示す位置推定装置１による位置推定
値は、外乱要因に影響されることなく、高精度であるこ
とが確認できる。また図１４からも、実線７７で示され
る位置推定装置１による位置推定値は、破線７６で示さ
れるＧＤＰＳ受信器２による位置に影響を受けることな
く、特にＤＧＰＳ信号の再捕捉直後Ｔの影響を受けるこ
となく、高精度であることが確認される。

【００８６】上述の実施の形態は、本発明の例に過ぎ
ず、構成を変更することができる。たとえば方位を推定
するにあたって、地磁気センサ４に代えて、絶対方位が
得られる他のセンサを用いてもよく、たとえばＤＧＰＳ
受信器２による検出方位を用いるようにしてもよい。具
体例を挙げて述べると、図１５（１）に示すように、通
過した２地点Ｐａ，Ｐｂ位置（Ｘａ，Ｙａ；Ｘｂ，Ｙ
ｂ）から方位を演算するようにしてもよいし、また図１
５（２）に示すように移動体２０に２つのＧＰＳアンテ
ナ９０，９１を設け、各ＧＰＳアンテナ９０，９１で受
信したデータから方位を演算するようにしてもよい。ま
た移動体２０の速度を求める手段として、上述のような
速度計３に代えて、たとえば慣性センサによって実現さ
れる加速度検出器で移動体２０の加速度を検出し、この
加速度を積分演算して速度を求める手段を設けるように
してもよい。また位置検出手段は、ＤＧＰＳ受信器２に
代えて、他の方式のＧＰＳ受信器を用いるようにしても
よい。また絶対方位および絶対位置で推定する必要はな
く、相対方位および相対位置を推定する構成であっても
よい。また方位推定装置７は、位置・方位計算部６を含
むと上述したけれども、方位推定装置７では、少なくと
も方位推定部１０を有していればよい。

【００８７】また移動体は、上記例示以外であってもよ
く、たとえば２次元的に移動する移動体としては、船舶
であってもよい。また必ずしも２次元的に移動する移動
体でなくてもよく、３次元的に移動する移動体、たとえ
ば飛行船などであってもよい。

【００８８】

【発明の効果】請求項１記載の本発明によれば、方位を
推定しようとする環境下において、各方位関連情報手段
のうち信頼性の高い方位関連情報手段がどの方位関連情
報手段であるかを判定し、信頼性の高い方位関連情報手
段に依存するように方位を推定しているので、リアルタ
イムで異常を判定しつつ常に高い精度で方位を推定する
ことができる。

【００８９】請求項２記載の本発明によれば、移動体の
速度の影響をなくし、安定した信頼性の判定が可能とな
る。したがって移動体の速度に影響されることなく、さ
らに高い精度で方位を推定することができる。

【００９０】請求項３記載の本発明によれば、位置を推
定しようとする環境下において、各位置関連情報手段の
うち信頼性の高い位置関連情報手段がどの位置関連情報
手段であるかを判定し、信頼性の高い位置関連情報手段
に依存するように位置を推定しているので、リアルタイ
ムで異常を判定しつつ常に高い精度で位置を推定するこ
とができる。

【００９１】請求項４記載の本発明によれば、移動体の
速度の影響をなくし、安定した信頼性の判定が可能とな
る。したがって移動体の速度に影響されることなく、さ
らに高い精度で位置を推定することができる。

【図面の簡単な説明】

【図１】本発明の実施の一形態の移動体の位置推定装置
１の概略的構成を示すブロック図である。

【図２】位置推定装置１の具体的構成を示すブロック図
である。

【図３】各方位関連情報手段４，５の信頼性判定におけ
る速度の影響を説明するための図である。

【図４】方位ゲインＧθを示すグラフである。

【図５】方位調整器１７の状態遷移を示す図である。

【図６】規則５に従う判定を説明するための図である。

【図７】積分型センサと非積分型センサの性質の差異を
示すための図である。

【図８】位置推定装置１による方位の推定結果を示すグ
ラフである。

【図９】図８のセクションＳ９を拡大して示すグラフで
ある。

【図１０】位置推定装置１による位置の推定結果を示す
グラフである。

【図１１】図１０の位置の推定結果を進行方向および横
方向に変換して示すグラフである。

【図１２】図１０の位置の推定における地磁気センサ４
による検出方位θを示すグラフである。

【図１３】図１０の位置の推定におけるＤＧＰＳ受信器
２による検出絶対位置Ｐと位置推定値Ｐ０の差を示すグ
ラフである。

【図１４】図１０の位置の推定におけるＤＧＰＳ受信器
による位置Ｐの経度成分ｘと位置推定値Ｐ０の経度成分
ｘ０とを示すグラフである。

【図１５】本発明の実施の他形態で用いることができる
方位を検出するための他の手段を示す図である。

【符号の説明】

１位置推定装置２ＤＧＰＳ受信器３速度計４地磁気センサ５ジャイロ６位置・方位計算部７方位推定装置１０方位推定部１１位置推定部１３方位角速度減算器１４方位角速度積分器１５方位減算器１６方位信頼性判定器１７方位調整器２０移動体３５速度成分演算器３６速度減算器３７速度積分器３８位置減算器３９位置信頼性判定器４０位置調整器

Claims

【特許請求の範囲】

【請求項１】相互に異なる原理によって移動体の方位
に関連する情報を取得する複数の方位関連情報手段と、各方位関連情報手段による取得方位関連情報に基づいて
各方位関連情報手段の信頼性を判定し、信頼性の高い方
位関連情報手段による取得方位関連情報が方位推定値に
大きな影響を与えるように、各方位関連情報手段による
取得方位関連情報から移動体の方位を推定する方位推定
演算手段とを含むことを特徴とする移動体の方位推定装
置。
【請求項２】前記方位推定演算手段は、各方位関連情報手段による取得方位関連情報に基づく方
位の差を移動体の速度で除算した値に基づいて、各方位
関連情報手段の信頼性を判定することを特徴とする請求
項１記載の移動体の方位推定装置。
【請求項３】相互に異なる原理によって移動体の位置
に関連する情報を取得する複数の位置関連情報手段と、各位置関連情報手段による取得位置関連情報に基づいて
各位置関連情報手段の信頼性を判定し、信頼性の高い位
置関連情報手段による取得位置関連情報が位置推定値に
大きな影響を与えるように、各位置関連情報手段による
取得位置関連情報から移動体の位置を推定する位置推定
演算手段とを含むことを特徴とする移動体の位置推定装
置。
【請求項４】前記位置推定演算手段は、各位置関連情報手段による取得位置関連情報に基づく位
置の差を移動体の速度で除算した値に基づいて、各位置
関連情報手段の信頼性を判定することを特徴とする請求
項３記載の移動体の位置推定装置。