移動体用測位装置
【課題】より正確な移動体の速度ベクトルを出力することが可能な移動体用測位装置を提供すること。
【解決手段】衛星電波を用いて移動体の位置を算出する位置演算手段と、衛星電波又は慣性航法により前記移動体の速度ベクトルを算出する速度演算手段と、道路が複数のリンクで表現された地図データを記憶した記憶手段と、前記速度ベクトル算出手段により算出された速度ベクトルを、前記リンクのうち移動体が存すると推定されるリンクに投影した投影速度ベクトルを出力する投影手段と、前記投影手段により出力される投影速度ベクトルの採用の可否を判定する投影可否判定手段と、を備え、該投影可否判定手段により前記投影速度ベクトルを採用すべきと判定された場合に、前記投影速度ベクトルを前記移動体の速度ベクトルとして出力することを特徴とする、移動体用測位装置。
【解決手段】衛星電波を用いて移動体の位置を算出する位置演算手段と、衛星電波又は慣性航法により前記移動体の速度ベクトルを算出する速度演算手段と、道路が複数のリンクで表現された地図データを記憶した記憶手段と、前記速度ベクトル算出手段により算出された速度ベクトルを、前記リンクのうち移動体が存すると推定されるリンクに投影した投影速度ベクトルを出力する投影手段と、前記投影手段により出力される投影速度ベクトルの採用の可否を判定する投影可否判定手段と、を備え、該投影可否判定手段により前記投影速度ベクトルを採用すべきと判定された場合に、前記投影速度ベクトルを前記移動体の速度ベクトルとして出力することを特徴とする、移動体用測位装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、衛星電波を用いて移動体の位置や速度を算出する移動体用測位装置に関する。
【背景技術】
【０００２】
従来、ＧＮＳＳ（Global Navigation Satellite System；ＧＰＳ（Global Positioning System）やGalileo、Glonass等を含む）において、搬送波周波数の変化量に基づいて移動体の速度ベクトルを算出する装置についての発明が開示されている（例えば、特許文献１参照）。移動体の速度ベクトルは、その向きによって移動体の方位（進行方向）を特定することができ、その積算値を用いて移動体の位置変化を算出することもできる。また、速度ベクトルは、移動体位置を特定する際に、単独測位やＩＮＳ（Inertial Navigation System；慣性航法装置）と組み合わせて用いることができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００９−２５０４６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところが、ＧＮＳＳにおいて算出される速度ベクトルに関しては、環境によって精度やばらつきが変化するという問題がある。この結果、慣性航法等と組み合わせた場合の蓄積誤差が大きくなる等の不都合が生じうる。図１は、係る不都合が生じる様子を示す図である。
【０００５】
ＧＮＳＳにおいて算出される速度ベクトルの精度は、主として（Ａ）各ＧＮＳＳ衛星から送信される電波に基づく測位誤差、（B）捕捉ＧＮＳＳ衛星の数と位置関係によって決定される。特に、市街地においては、マルチパス（電波が建物等で反射されることにより、衛星と移動体の距離を誤推定すること）の影響等で（Ａ）の測位誤差が生じる。また、建物や懸架物で衛星との間が遮蔽されることにより（Ｂ）の捕捉ＧＮＳＳ衛星の数が減少する。
【０００６】
ＧＮＳＳにおいて推定する速度ベクトルの精度が低下すると、移動体の姿勢の推定精度が低下してしまう。市街地等で捕捉ＧＮＳＳ衛星の数が少ない場合は、ＩＮＳによって位置を推定するが、移動体の姿勢の誤差は蓄積を続けてしまう。
【０００７】
本発明はこのような課題を解決するためのものであり、より正確な移動体の速度ベクトルを出力することが可能な移動体用測位装置を提供することを、主たる目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するための本発明の一態様は、
衛星電波を用いて移動体の位置を算出する位置演算手段と、
衛星電波又は慣性航法により前記移動体の速度ベクトルを算出する速度演算手段と、
道路が複数のリンクで表現された地図データを記憶した記憶手段と、
前記速度ベクトル算出手段により算出された速度ベクトルを、前記複数のリンクのうち移動体が存すると推定されるリンクに投影した投影速度ベクトルを出力する投影手段と、
前記投影手段により出力される投影速度ベクトルの採用の可否を判定する投影可否判定手段と、を備え、
該投影可否判定手段により前記投影速度ベクトルを採用すべきと判定された場合に、前記投影速度ベクトルを前記移動体の速度ベクトルとして出力することを特徴とする、
移動体用測位装置である。
【０００９】
この本発明の一態様によれば、より正確な移動体の速度ベクトルを出力することができる。
【００１０】
本発明の一態様において、
前記投影可否判定手段の判定結果に基づいて出力された前記移動体の速度ベクトルを積算して、前記移動体の位置変化を算出する速度ベクトル積算手段と、
前記位置演算手段により算出された移動体の位置と、前記速度ベクトル積算手段により算出された前記移動体の位置変化を加味して得られた移動体の位置と、のいずれの信頼性が高いかを判定する信頼性判定手段と、を備え、
該信頼性判定手段により、信頼性が高いと判定された方の移動体の位置を出力することを特徴とするものとしてもよい。
【００１１】
こうすれば、単独測位等により算出された移動体の位置と、速度ベクトルを積算して算出された移動体の位置とのうち信頼性の高い方を出力するため、マルチパス等の影響を抑制することができる。
【００１２】
また、本発明の一態様において、
慣性航法により前記移動体の位置及び速度を算出する慣性航法測位演算手段と、
該慣性航法測位演算手段により算出された前記移動体の位置及び速度と、衛星電波により算出された前記移動体の位置及び速度との差分が入力されるカルマンフィルタと、を備え、
前記慣性航法測位演算手段は、前記カルマンフィルタの出力に応じて補正を行なうことを特徴とするものとしてもよい。
【００１３】
こうすれば、より精度の高い慣性航法測位を行なうことができる。
【発明の効果】
【００１４】
本発明によれば、より正確な移動体の速度ベクトルを出力することが可能な移動体用測位装置を提供することができる。
【図面の簡単な説明】
【００１５】
【図１】ＧＮＳＳにおいて算出される速度ベクトルの精度やばらつきが変化し、慣性航法等と組み合わせた場合の蓄積誤差が大きくなるという不都合が生じる様子を示す図である。
【図２】本発明の第１実施例に係る移動体用測位装置１のシステム構成例である。
【図３】車両の速度ベクトルＶをリンクＷに投影して投影速度ベクトルＶ＃を得る様子を示す図である。
【図４】投影速度ベクトルＶ＃を用いることにより、方位誤差を減少させることができる様子を示す図である。
【図５】本発明の第１実施例に係る移動体用測位装置１により実行される特徴的な処理の流れを示すフローチャートである。
【図６】本発明の第２実施例に係る移動体用測位装置２のシステム構成例である。
【図７】本発明の第２実施例に係る移動体用測位装置２により実行される特徴的な処理の流れを示すフローチャートである。
【図８】本発明の第３実施例に係る移動体用測位装置３のシステム構成例である。
【発明を実施するための形態】
【００１６】
以下、本発明を実施するための形態について、添付図面を参照しながら実施例を挙げて説明する。
【実施例】
【００１７】
＜第１実施例＞
以下、図面を参照し、本発明の第１実施例に係る移動体用測位装置１について説明する。移動体用測位装置１は、ＧＮＳＳに適用される装置である。ＧＮＳＳは、衛星からの信号を用いて移動体に搭載された測位装置が移動体の位置を測位する測位システムであり、ＧＰＳ（Global Positioning System）、Galileo、Glonass等の衛星を用いた測位システムを含む。以下の説明ではＧＰＳを基本構成として説明するが、本発明は、ＧＰＳに限らずあらゆるＧＮＳＳに広く適用可能である。移動体は、車両、自動二輪車、鉄道、船舶、航空機、ホークリフト、ロボットや、人の移動に伴い移動する携帯電話等の情報端末等がありうる。
【００１８】
なお、以下ではＧＮＳＳの一例として、ＧＰＳに適用されるものとして説明する。また、移動体が車両であるものとする。
【００１９】
ＧＰＳ衛星は、航法メッセージ（衛星信号）を地球に向けて常時放送している。航法メッセージには、対応するＧＰＳ衛星に関する衛星軌道情報（エフェメリスやアルマナク）、時計の補正値、電離層の補正係数が含まれている。航法メッセージは、Ｃ／Ａコードにより拡散されＬ１波（周波数：1575.42ＭＨz）に乗せられて、地球に向けて常時放送されている。なお、Ｌ１波は、Ｃ／Ａコードで変調されたＳｉｎ波とＰコード（Precision Code）で変調されたＣｏｓ波の合成波であり、直交変調されている。Ｃ／Ａコード及びＰコードは、擬似雑音（Pseudo Noise）符号であり、−１と１が不規則に周期的に並ぶ符号列である。
【００２０】
図２は、本発明の第１実施例に係る移動体用測位装置１のシステム構成例である。移動体用測位装置１は、主要な構成として、ＧＰＳアンテナ１０と、図示しないマイクロコンピュータがプログラムを実行することにより実現される機能ブロックである（但し、回路構成であっても構わない）、測位信号受信部２０、位置演算部２２、速度演算部２４、投影可否判定部２６、投影部２８、及びマップマッチング部３０と、図示しない記憶装置に記憶された地図データベース（図では地図ＤＢと表記した）３２と、を有する。
【００２１】
測位信号受信部２０は、ＧＰＳアンテナ１０が受信した信号について、内部で発生させたレプリカＣ／Ａコードを用いてＣ／Ａコード同期を行ない、航法メッセージを取り出す。Ｃ／Ａコード同期の方法は、多種多様であり、任意の適切な方法が採用されてよい。例えば、ＤＬＬ（Delay―Locked Loop）を用いて、受信したＣ／Ａコードに対するレプリカＣ／Ａコードの相関値がピークとなるコード位相を追尾する方法であってよい。
【００２２】
測位信号受信部２０は、ＧＰＳ衛星と車両（正確には移動体用測位装置１）との間の擬似距離ρを算出する。擬似距離ρは、時計誤差（クロックバイアス）や電波伝搬速度変化による誤差を含んでいる。擬似距離ρは、例えば次式（１）により算出される。式中、Ｎは、ＧＰＳ衛星と車両との間のＣ／Ａコードのビット数に相当し、レプリカＣ／Ａコードの位相及び移動体位置測位装置１内部の受信機時計に基づいて算出される。なお、数値３００は、Ｃ／Ａコードが、１ビットの長さが１μｓであり、１ビットに相当する長さが約３００ｍ（１μｓ×光速）であることに由来する。
【００２３】
ρ＝Ｎ×３００ …（１）
【００２４】
また、測位信号受信部２０は、衛星信号の搬送波位相を測定する機能を備え、内部で発生させたレプリカキャリアを用いて、ドップラーシフトした受信搬送波のドップラー周波数変化量Δｆを測定する。ドップラー周波数変化量Δｆは、レプリカキャリアの周波数ｆｒと既知の搬送波周波数ｆｃ（1575.42ＭＨz）の差分（＝ｆｒ−ｆｃ）として測定される。係る機能は、レプリカキャリアを用いてキャリア相関値を演算して受信キャリアを追尾するＰＬＬ（Phase-Locked Loop）により実現されてよい。
【００２５】
位置演算部２２は、まず、ＧＰＳ衛星の位置（以下、「衛星位置」という）を算出する。具体的には、航法メッセージの衛星軌道情報及び現在の時間に基づいて、ＧＰＳ衛星ｉの、ワールド座標系における現在位置（Ｘｉ、Ｙｉ、Ｚｉ）を算出する（符号ｉは、複数の衛星についてこのような演算を行なう中で、ｉ番目の衛星であることを示す）。ＧＰＳ衛星は、その運動が地球重心を含む一定面内（軌道面）に限定され、その軌道は地球重心を１つの焦点とする楕円運動であるため、ケプラーの方程式を逐次数値計算することで軌道面におけるＧＰＳ衛星の位置が計算できる。そして、ワールド座標系におけるＧＰＳ衛星ｉの位置（Ｘｉ、Ｙｉ、Ｚｉ）は、ＧＰＳ衛星ｉの軌道面とワールド座標系の赤道面が回転関係にあることを考慮して、軌道面におけるＧＰＳ衛星ｉの位置を３次元的に回転座標変換することで得られる。
【００２６】
そして、複数のＧＰＳ衛星の位置と、対応する観測擬似距離ρに基づいて、車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）を測位する。車両の位置は、３つのＧＰＳ衛星に対して得られる位置及び観測擬似距離ρに基づいて、三角測量の原理で導出される。観測擬似距離ρは、前述のように時計誤差を含んでいるため、４つ目のＧＰＳ衛星に対して得られる観測擬似距離ρ及び衛星位置を用いて、時計誤差成分を除去する。
【００２７】
なお、位置演算部２２の演算機能はこのような単独測位に限られず、干渉測位（既知の点に設置された固定局での受信データを併用する方式）であってもよい。干渉測位の場合、上述の如く固定局及び車両にてそれぞれ得られる観測擬似距離ρの一重位相差や２重位相差等を用いて車両の位置が測位される。
【００２８】
速度演算部２４は、ドップラー周波数変化量ΔｆとＧＰＳ衛星の速度ベクトルＶｉに基づいて、車両の速度ベクトルＶを算出する。具体的には、まずドップラー周波数変化量Δｆに基づいて、ＧＰＳ衛星ｉと車両との間の相対速度ベクトル（Ｖ−Ｖ１）を、例えば次式（２）を用いて算出する。
【００２９】
Δｆ＝ｆｃ×｛（Ｖ−Ｖ１）・（Ｘｕ−Ｘｉ，Ｙｕ−Ｙｉ，Ｚｕ−Ｚｉ）／√｛（Ｘｕ−Ｘｉ）２＋（Ｙｕ−Ｙｉ）２＋（Ｚｕ−Ｚｉ）２｝ …（２）
【００３０】
そして、ＧＰＳ衛星ｉと車両の相対速度ベクトル（Ｖ−Ｖ１）と、ＧＰＳ衛星ｉの速度ベクトルＶｉとの差分ベクトルを、車両の速度ベクトルＶとして算出する。ＧＰＳ衛星ｉの速度ベクトルＶｉは、今回の衛星位置から前回の衛星位置を差し引いて、測位演算周期で除することにより求めることができる。
【００３１】
投影可否判定部２６は、投影部２８により算出される投影速度ベクトルの採用の可否を判定する。
【００３２】
先に投影部２８について説明する。投影部２８は、地図データベース３２が有する複数のリンクのうち、車両が沿って走行していると判定されるリンクに車両の速度ベクトルＶを投影し、投影速度ベクトルＶ＃を算出する。車両が沿って走行していると判定されるリンクについての情報（特にリンクの方位）は、マップマッチング部３０から提供される。マップマッチング部３０では、ノードとリンクで道路が表現された地図データベース３２と、各種センサ（ＧＰＳ、ＩＮＳ用センサ、車輪速センサ、舵角センサ等）の情報とから、地図データベース３２上のどの位置を走行しているかを特定している。
【００３３】
図３は、車両の速度ベクトルＶをリンクＷに投影して投影速度ベクトルＶ＃を得る様子を示す図である。このような投影速度ベクトルＶ＃を用いることにより、方位誤差を減少させることができる。図４は、係る様子を示す図である。
【００３４】
投影可否判定部２６は、例えば以下の条件の全て、又は一部を満たす場合に、地図データベース３２が有する特定のリンクに沿って車両が走行していると判定し、従って、投影速度ベクトルＶ＃を採用すべきと判定する。リンクの曲率については、地図データベース３２に含まれるものとしてもよいし、前後数点のノードから推定してもよい。
（１）マップマッチング部３０が特定のリンクにマッチングしている。
（２）車速が所定値以上である。
（３）リンクの長さがある値以上である。
（４）リンクの端点からの距離がある値以上である。
（５）リンクに対応する曲率がある値以下である。
【００３５】
一方、投影速度ベクトルＶ＃を採用すべきと判定しなかった場合は、速度演算部２４が出力した速度ベクトルＶを用いてもよいし、マップマッチング部３０において把握されている速度ベクトルを用いてもよい。
【００３６】
図５は、本発明の第１実施例に係る移動体用測位装置１により実行される特徴的な処理の流れを示すフローチャートである。
【００３７】
まず、位置演算部２２が車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）を、速度演算部２４が車両の速度ベクトルＶを、それぞれ算出する（Ｓ１００）。
【００３８】
次に、投影可否判定部２６が、前述のように投影速度ベクトルＶ＃の採用の可否を判定する（Ｓ１０２）。
【００３９】
投影速度ベクトルＶ＃を採用すべきと判定した場合は、投影部２８に投影を行なわせ、位置演算部２２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）、及び投影部２８が出力した投影速度ベクトルＶ＃を、最終的に出力する（Ｓ１０４）。
【００４０】
一方、投影速度ベクトルＶ＃を採用すべきでないと判定した場合は、位置演算部２２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）、及び速度演算部２４により算出された車両の速度ベクトルＶを、最終的に出力する（Ｓ１０６）。
【００４１】
以上説明した本実施例の移動体用測位装置１によれば、車両がリンクに沿って走行していると判定されるときには、投影部２８が出力した投影速度ベクトルＶ＃を最終的に出力するため、より正確な車両の速度ベクトルを出力することができる。
【００４２】
＜第２実施例＞
以下、図面を参照し、本発明の第２実施例に係る移動体用測位装置２について説明する。
【００４３】
図６は、本発明の第２実施例に係る移動体用測位装置２のシステム構成例である。移動体用測位装置２は、第１実施例に係る移動体用測位装置１が有する構成（図では符号のみ記載した）に加えて、速度ベクトル積算部４０と、信頼性判定部４２と、を有する。
【００４４】
速度ベクトル積算部４０は、例えば、第１実施例の移動体用測位装置１により最終的に出力された速度ベクトルＶ又は投影速度ベクトルＶ＃にサンプル間隔を乗じて、当該サンプリング回における車両の位置変化（ΔＸ，ΔＹ，ΔＺ）を算出する。そして、繰り返しこのような処理を行なう中で、前回に第１実施例の移動体用測位装置１により最終的に出力された車両の位置（Ｘｕ（ｎ−１），Ｙｕ（ｎ−１），Ｚｕ（ｎ−１））に、係る車両の位置変化を加算して、今回の車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）を算出する（次式（３）参照）。
【００４５】
（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）＝（Ｘｕ（ｎ−１）＋ΔＸ，Ｙｕ（ｎ−１）＋ΔＹ，Ｚｕ（ｎ−１）＋ΔＺ） …（３）
【００４６】
ここで、速度ベクトルＶ又は投影速度ベクトルＶ＃をそのまま用いるのではなく、過去の複数のサンプル回における速度ベクトルに適切な重みを付けて加重平均したものを用いてもよい。
【００４７】
信頼性判定部４２は、位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）と、速度ベクトル積算部４０により算出された車両の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）と、のいずれの信頼性が高いかを判定する。
【００４８】
ここで、ＧＮＳＳで算出された位置、及び速度ベクトルは以下のような特徴を有する。（Ａ）位置に関しては、車速が小さい時ほどマルチパスの影響を受けやすい。（Ｂ）速度ベクトル突の方が、位置よりもマルチパスの影響を受けにくい。従って、信頼性判定部４２は、例えば以下の条件の全て、又は一部を満たす場合に、速度ベクトル積算部４０により算出された移動体の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）の信頼性が高いと判定する。
（１）車速がある値以下である。
（２）速度ベクトルの算出に用いた衛星の数がある値以上である。
（３）速度ベクトルの算出における残差平方和がある値以下である。
（４）速度ベクトルの算出に用いた衛星群のＤＯＰ（Dilution of Precision；精度低下率）がある値以下である。
【００４９】
図７は、本発明の第２実施例に係る移動体用測位装置２により実行される特徴的な処理の流れを示すフローチャートである。Ｓ１００〜Ｓ１０６については第１実施例と同様であるため、詳細な説明を省略する。
【００５０】
位置演算部２２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）、及び投影速度ベクトルＶ＃、又は速度ベクトルＶが出力されると（Ｓ１００〜１０６）、位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）と、速度ベクトル積算部４０により算出された車両の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）と、のいずれの信頼性が高いかを判定する（Ｓ１０８）。
【００５１】
位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）の信頼性が高いと判定した場合は、位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）を、車両の位置として出力する（Ｓ１１０）。
【００５２】
一方、速度ベクトル積算部４０により算出された移動体の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）の信頼性が高いと判定した場合は、速度ベクトル積算部４０により算出された車両の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）を、車両の位置として出力する（Ｓ１１２）。
【００５３】
なお、Ｓ１１０又はＳ１１２において、車両の速度ベクトルも併せて出力される。
【００５４】
以上説明した本実施例の移動体用測位装置２によれば、単独測位等により算出された車両の位置と、速度ベクトルを積算して算出された車両の位置とのうち信頼性の高い方を出力するため、マルチパス等の影響を抑制することができる。
【００５５】
＜第３実施例＞
以下、図面を参照し、本発明の第３実施例に係る移動体用測位装置３について説明する。
【００５６】
図８は、本発明の第３実施例に係る移動体用測位装置３のシステム構成例である。移動体用測位装置３は、第１実施例に係る移動体用測位装置１が有する構成（図では符号のみ記載した）に加えて、慣性センサ５０と、ＩＮＳ測位演算部５２と、カルマンフィルタ５４と、車輪速センサ５６と、を有する。
【００５７】
慣性センサ５０は、例えば、加速度センサ（Ｇセンサ）やヨーレートセンサである。
【００５８】
ＩＮＳ測位演算部５２は、慣性航法により車両の位置（Ｘｕｉ，Ｙｕｉ，Ｚｕｉ）を算出する。慣性航法による車両位置の測位方法は、多種多様であり、如何なる方法であってもよい。例えば車両位置は、加速度センサの出力値に、姿勢変換、重力補正、コリオリ力補正を行って２回積分し、当該２回積分により得られる移動距離を、車両位置の前回値に加算することで導出されてよい。ＩＮＳ測位演算部５２により算出される車両位置及び車両速度（ＩＮＳ測位結果）は、測位周期毎に、上述のる移動体用測位装置１が有する構成による車両位置及び速度（ＧＰＳ測位結果）との差分値が取られ、当該差分値がカルマンフィルタ５４に入力され、各種の補正量が決定される。また、車輪速センサ５６により検出される速度との差分もカルマンフィルタ５４に入力されてよい。
【００５９】
カルマンフィルタ５４は、ＩＮＳ測位結果とＧＰＳ測位結果のそれぞれの信頼性に基づき、確率的に最も正しい値となるように、各種補正量ηを推定する。カルマンフィルタ５４における状態方程式は、次式（４）のように設定される。式中、η（ｔｎ）は、時刻ｔ＝ｔｎでの状態変数を表わす。また、ｕ（ｔｎ−１）及びｗ（ｔｎ−１）は、それぞれ、時刻ｔ＝ｔｎ−１での既知入力及び外乱（システム雑音：正規性白色雑音）である。η（ｔｎ）は、ＩＮＳ測位による車両位置や速度、車両姿勢の誤差δｒ（ＩＮＳ）、δｖ（ＩＮＳ）、δε（ＩＮＳ）、ジャイロセンサのバイアス誤差δｂ、加速度センサのドリフト誤差δｄ、タイヤの半径誤差δｓ、及び、ＧＰＳ受信機内の受信機時計の誤差δＣｔを含む。
【００６０】
η（ｔｎ）＝Ｆ・η（ｔｎ−１）＋Ｇ・ｕ（ｔｎ−１）＋Γ・ｗ（ｔｎ−１） …（４）
【００６１】
ある状態において観測量ｚ（前述の差分値）は、η（ｔｎ）との間において、次式（５）のような関係を有する。式中、Ｈは状態空間を観測空間に線形写像する役割を担う観測モデルで、ｖ（ｔｎ）は、ガウス分布に従う雑音である。
【００６２】
ｚ（ｔｎ）＝Ｈ（ｔｎ）・η（ｔｎ）＋ｖ（ｔｎ） …（５）
【００６３】
ＩＮＳ測位演算部５２は、カルマンフィルタ５４の出力に基づいて、姿勢変換、重力補正、コリオリ力補正等の補正を行なう。この結果、ＩＮＳ測位の精度を向上させることができる。
【００６４】
本実施例の移動体用測位装置３によれば、より精度の高いＩＮＳ測位を行なうことができる。
【００６５】
以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
【産業上の利用可能性】
【００６６】
本発明は、自動車製造業や自動車部品製造業等に利用可能である。
【符号の説明】
【００６７】
１、２、３ 移動体用測位装置
１０ ＧＰＳアンテナ
２０ 測位信号受信部
２２ 位置演算部
２４ 速度演算部
２６ 投影可否判定部
２８ 投影部
３０ マップマッチング部
３２ 地図データベース
４０ 速度ベクトル積算部
４２ 信頼性判定部
５０ 慣性センサ
５２ ＩＮＳ測位演算部
５４ カルマンフィルタ
５６ 車輪速センサ
【技術分野】
【０００１】
本発明は、衛星電波を用いて移動体の位置や速度を算出する移動体用測位装置に関する。
【背景技術】
【０００２】
従来、ＧＮＳＳ（Global Navigation Satellite System；ＧＰＳ（Global Positioning System）やGalileo、Glonass等を含む）において、搬送波周波数の変化量に基づいて移動体の速度ベクトルを算出する装置についての発明が開示されている（例えば、特許文献１参照）。移動体の速度ベクトルは、その向きによって移動体の方位（進行方向）を特定することができ、その積算値を用いて移動体の位置変化を算出することもできる。また、速度ベクトルは、移動体位置を特定する際に、単独測位やＩＮＳ（Inertial Navigation System；慣性航法装置）と組み合わせて用いることができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００９−２５０４６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところが、ＧＮＳＳにおいて算出される速度ベクトルに関しては、環境によって精度やばらつきが変化するという問題がある。この結果、慣性航法等と組み合わせた場合の蓄積誤差が大きくなる等の不都合が生じうる。図１は、係る不都合が生じる様子を示す図である。
【０００５】
ＧＮＳＳにおいて算出される速度ベクトルの精度は、主として（Ａ）各ＧＮＳＳ衛星から送信される電波に基づく測位誤差、（B）捕捉ＧＮＳＳ衛星の数と位置関係によって決定される。特に、市街地においては、マルチパス（電波が建物等で反射されることにより、衛星と移動体の距離を誤推定すること）の影響等で（Ａ）の測位誤差が生じる。また、建物や懸架物で衛星との間が遮蔽されることにより（Ｂ）の捕捉ＧＮＳＳ衛星の数が減少する。
【０００６】
ＧＮＳＳにおいて推定する速度ベクトルの精度が低下すると、移動体の姿勢の推定精度が低下してしまう。市街地等で捕捉ＧＮＳＳ衛星の数が少ない場合は、ＩＮＳによって位置を推定するが、移動体の姿勢の誤差は蓄積を続けてしまう。
【０００７】
本発明はこのような課題を解決するためのものであり、より正確な移動体の速度ベクトルを出力することが可能な移動体用測位装置を提供することを、主たる目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するための本発明の一態様は、
衛星電波を用いて移動体の位置を算出する位置演算手段と、
衛星電波又は慣性航法により前記移動体の速度ベクトルを算出する速度演算手段と、
道路が複数のリンクで表現された地図データを記憶した記憶手段と、
前記速度ベクトル算出手段により算出された速度ベクトルを、前記複数のリンクのうち移動体が存すると推定されるリンクに投影した投影速度ベクトルを出力する投影手段と、
前記投影手段により出力される投影速度ベクトルの採用の可否を判定する投影可否判定手段と、を備え、
該投影可否判定手段により前記投影速度ベクトルを採用すべきと判定された場合に、前記投影速度ベクトルを前記移動体の速度ベクトルとして出力することを特徴とする、
移動体用測位装置である。
【０００９】
この本発明の一態様によれば、より正確な移動体の速度ベクトルを出力することができる。
【００１０】
本発明の一態様において、
前記投影可否判定手段の判定結果に基づいて出力された前記移動体の速度ベクトルを積算して、前記移動体の位置変化を算出する速度ベクトル積算手段と、
前記位置演算手段により算出された移動体の位置と、前記速度ベクトル積算手段により算出された前記移動体の位置変化を加味して得られた移動体の位置と、のいずれの信頼性が高いかを判定する信頼性判定手段と、を備え、
該信頼性判定手段により、信頼性が高いと判定された方の移動体の位置を出力することを特徴とするものとしてもよい。
【００１１】
こうすれば、単独測位等により算出された移動体の位置と、速度ベクトルを積算して算出された移動体の位置とのうち信頼性の高い方を出力するため、マルチパス等の影響を抑制することができる。
【００１２】
また、本発明の一態様において、
慣性航法により前記移動体の位置及び速度を算出する慣性航法測位演算手段と、
該慣性航法測位演算手段により算出された前記移動体の位置及び速度と、衛星電波により算出された前記移動体の位置及び速度との差分が入力されるカルマンフィルタと、を備え、
前記慣性航法測位演算手段は、前記カルマンフィルタの出力に応じて補正を行なうことを特徴とするものとしてもよい。
【００１３】
こうすれば、より精度の高い慣性航法測位を行なうことができる。
【発明の効果】
【００１４】
本発明によれば、より正確な移動体の速度ベクトルを出力することが可能な移動体用測位装置を提供することができる。
【図面の簡単な説明】
【００１５】
【図１】ＧＮＳＳにおいて算出される速度ベクトルの精度やばらつきが変化し、慣性航法等と組み合わせた場合の蓄積誤差が大きくなるという不都合が生じる様子を示す図である。
【図２】本発明の第１実施例に係る移動体用測位装置１のシステム構成例である。
【図３】車両の速度ベクトルＶをリンクＷに投影して投影速度ベクトルＶ＃を得る様子を示す図である。
【図４】投影速度ベクトルＶ＃を用いることにより、方位誤差を減少させることができる様子を示す図である。
【図５】本発明の第１実施例に係る移動体用測位装置１により実行される特徴的な処理の流れを示すフローチャートである。
【図６】本発明の第２実施例に係る移動体用測位装置２のシステム構成例である。
【図７】本発明の第２実施例に係る移動体用測位装置２により実行される特徴的な処理の流れを示すフローチャートである。
【図８】本発明の第３実施例に係る移動体用測位装置３のシステム構成例である。
【発明を実施するための形態】
【００１６】
以下、本発明を実施するための形態について、添付図面を参照しながら実施例を挙げて説明する。
【実施例】
【００１７】
＜第１実施例＞
以下、図面を参照し、本発明の第１実施例に係る移動体用測位装置１について説明する。移動体用測位装置１は、ＧＮＳＳに適用される装置である。ＧＮＳＳは、衛星からの信号を用いて移動体に搭載された測位装置が移動体の位置を測位する測位システムであり、ＧＰＳ（Global Positioning System）、Galileo、Glonass等の衛星を用いた測位システムを含む。以下の説明ではＧＰＳを基本構成として説明するが、本発明は、ＧＰＳに限らずあらゆるＧＮＳＳに広く適用可能である。移動体は、車両、自動二輪車、鉄道、船舶、航空機、ホークリフト、ロボットや、人の移動に伴い移動する携帯電話等の情報端末等がありうる。
【００１８】
なお、以下ではＧＮＳＳの一例として、ＧＰＳに適用されるものとして説明する。また、移動体が車両であるものとする。
【００１９】
ＧＰＳ衛星は、航法メッセージ（衛星信号）を地球に向けて常時放送している。航法メッセージには、対応するＧＰＳ衛星に関する衛星軌道情報（エフェメリスやアルマナク）、時計の補正値、電離層の補正係数が含まれている。航法メッセージは、Ｃ／Ａコードにより拡散されＬ１波（周波数：1575.42ＭＨz）に乗せられて、地球に向けて常時放送されている。なお、Ｌ１波は、Ｃ／Ａコードで変調されたＳｉｎ波とＰコード（Precision Code）で変調されたＣｏｓ波の合成波であり、直交変調されている。Ｃ／Ａコード及びＰコードは、擬似雑音（Pseudo Noise）符号であり、−１と１が不規則に周期的に並ぶ符号列である。
【００２０】
図２は、本発明の第１実施例に係る移動体用測位装置１のシステム構成例である。移動体用測位装置１は、主要な構成として、ＧＰＳアンテナ１０と、図示しないマイクロコンピュータがプログラムを実行することにより実現される機能ブロックである（但し、回路構成であっても構わない）、測位信号受信部２０、位置演算部２２、速度演算部２４、投影可否判定部２６、投影部２８、及びマップマッチング部３０と、図示しない記憶装置に記憶された地図データベース（図では地図ＤＢと表記した）３２と、を有する。
【００２１】
測位信号受信部２０は、ＧＰＳアンテナ１０が受信した信号について、内部で発生させたレプリカＣ／Ａコードを用いてＣ／Ａコード同期を行ない、航法メッセージを取り出す。Ｃ／Ａコード同期の方法は、多種多様であり、任意の適切な方法が採用されてよい。例えば、ＤＬＬ（Delay―Locked Loop）を用いて、受信したＣ／Ａコードに対するレプリカＣ／Ａコードの相関値がピークとなるコード位相を追尾する方法であってよい。
【００２２】
測位信号受信部２０は、ＧＰＳ衛星と車両（正確には移動体用測位装置１）との間の擬似距離ρを算出する。擬似距離ρは、時計誤差（クロックバイアス）や電波伝搬速度変化による誤差を含んでいる。擬似距離ρは、例えば次式（１）により算出される。式中、Ｎは、ＧＰＳ衛星と車両との間のＣ／Ａコードのビット数に相当し、レプリカＣ／Ａコードの位相及び移動体位置測位装置１内部の受信機時計に基づいて算出される。なお、数値３００は、Ｃ／Ａコードが、１ビットの長さが１μｓであり、１ビットに相当する長さが約３００ｍ（１μｓ×光速）であることに由来する。
【００２３】
ρ＝Ｎ×３００ …（１）
【００２４】
また、測位信号受信部２０は、衛星信号の搬送波位相を測定する機能を備え、内部で発生させたレプリカキャリアを用いて、ドップラーシフトした受信搬送波のドップラー周波数変化量Δｆを測定する。ドップラー周波数変化量Δｆは、レプリカキャリアの周波数ｆｒと既知の搬送波周波数ｆｃ（1575.42ＭＨz）の差分（＝ｆｒ−ｆｃ）として測定される。係る機能は、レプリカキャリアを用いてキャリア相関値を演算して受信キャリアを追尾するＰＬＬ（Phase-Locked Loop）により実現されてよい。
【００２５】
位置演算部２２は、まず、ＧＰＳ衛星の位置（以下、「衛星位置」という）を算出する。具体的には、航法メッセージの衛星軌道情報及び現在の時間に基づいて、ＧＰＳ衛星ｉの、ワールド座標系における現在位置（Ｘｉ、Ｙｉ、Ｚｉ）を算出する（符号ｉは、複数の衛星についてこのような演算を行なう中で、ｉ番目の衛星であることを示す）。ＧＰＳ衛星は、その運動が地球重心を含む一定面内（軌道面）に限定され、その軌道は地球重心を１つの焦点とする楕円運動であるため、ケプラーの方程式を逐次数値計算することで軌道面におけるＧＰＳ衛星の位置が計算できる。そして、ワールド座標系におけるＧＰＳ衛星ｉの位置（Ｘｉ、Ｙｉ、Ｚｉ）は、ＧＰＳ衛星ｉの軌道面とワールド座標系の赤道面が回転関係にあることを考慮して、軌道面におけるＧＰＳ衛星ｉの位置を３次元的に回転座標変換することで得られる。
【００２６】
そして、複数のＧＰＳ衛星の位置と、対応する観測擬似距離ρに基づいて、車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）を測位する。車両の位置は、３つのＧＰＳ衛星に対して得られる位置及び観測擬似距離ρに基づいて、三角測量の原理で導出される。観測擬似距離ρは、前述のように時計誤差を含んでいるため、４つ目のＧＰＳ衛星に対して得られる観測擬似距離ρ及び衛星位置を用いて、時計誤差成分を除去する。
【００２７】
なお、位置演算部２２の演算機能はこのような単独測位に限られず、干渉測位（既知の点に設置された固定局での受信データを併用する方式）であってもよい。干渉測位の場合、上述の如く固定局及び車両にてそれぞれ得られる観測擬似距離ρの一重位相差や２重位相差等を用いて車両の位置が測位される。
【００２８】
速度演算部２４は、ドップラー周波数変化量ΔｆとＧＰＳ衛星の速度ベクトルＶｉに基づいて、車両の速度ベクトルＶを算出する。具体的には、まずドップラー周波数変化量Δｆに基づいて、ＧＰＳ衛星ｉと車両との間の相対速度ベクトル（Ｖ−Ｖ１）を、例えば次式（２）を用いて算出する。
【００２９】
Δｆ＝ｆｃ×｛（Ｖ−Ｖ１）・（Ｘｕ−Ｘｉ，Ｙｕ−Ｙｉ，Ｚｕ−Ｚｉ）／√｛（Ｘｕ−Ｘｉ）２＋（Ｙｕ−Ｙｉ）２＋（Ｚｕ−Ｚｉ）２｝ …（２）
【００３０】
そして、ＧＰＳ衛星ｉと車両の相対速度ベクトル（Ｖ−Ｖ１）と、ＧＰＳ衛星ｉの速度ベクトルＶｉとの差分ベクトルを、車両の速度ベクトルＶとして算出する。ＧＰＳ衛星ｉの速度ベクトルＶｉは、今回の衛星位置から前回の衛星位置を差し引いて、測位演算周期で除することにより求めることができる。
【００３１】
投影可否判定部２６は、投影部２８により算出される投影速度ベクトルの採用の可否を判定する。
【００３２】
先に投影部２８について説明する。投影部２８は、地図データベース３２が有する複数のリンクのうち、車両が沿って走行していると判定されるリンクに車両の速度ベクトルＶを投影し、投影速度ベクトルＶ＃を算出する。車両が沿って走行していると判定されるリンクについての情報（特にリンクの方位）は、マップマッチング部３０から提供される。マップマッチング部３０では、ノードとリンクで道路が表現された地図データベース３２と、各種センサ（ＧＰＳ、ＩＮＳ用センサ、車輪速センサ、舵角センサ等）の情報とから、地図データベース３２上のどの位置を走行しているかを特定している。
【００３３】
図３は、車両の速度ベクトルＶをリンクＷに投影して投影速度ベクトルＶ＃を得る様子を示す図である。このような投影速度ベクトルＶ＃を用いることにより、方位誤差を減少させることができる。図４は、係る様子を示す図である。
【００３４】
投影可否判定部２６は、例えば以下の条件の全て、又は一部を満たす場合に、地図データベース３２が有する特定のリンクに沿って車両が走行していると判定し、従って、投影速度ベクトルＶ＃を採用すべきと判定する。リンクの曲率については、地図データベース３２に含まれるものとしてもよいし、前後数点のノードから推定してもよい。
（１）マップマッチング部３０が特定のリンクにマッチングしている。
（２）車速が所定値以上である。
（３）リンクの長さがある値以上である。
（４）リンクの端点からの距離がある値以上である。
（５）リンクに対応する曲率がある値以下である。
【００３５】
一方、投影速度ベクトルＶ＃を採用すべきと判定しなかった場合は、速度演算部２４が出力した速度ベクトルＶを用いてもよいし、マップマッチング部３０において把握されている速度ベクトルを用いてもよい。
【００３６】
図５は、本発明の第１実施例に係る移動体用測位装置１により実行される特徴的な処理の流れを示すフローチャートである。
【００３７】
まず、位置演算部２２が車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）を、速度演算部２４が車両の速度ベクトルＶを、それぞれ算出する（Ｓ１００）。
【００３８】
次に、投影可否判定部２６が、前述のように投影速度ベクトルＶ＃の採用の可否を判定する（Ｓ１０２）。
【００３９】
投影速度ベクトルＶ＃を採用すべきと判定した場合は、投影部２８に投影を行なわせ、位置演算部２２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）、及び投影部２８が出力した投影速度ベクトルＶ＃を、最終的に出力する（Ｓ１０４）。
【００４０】
一方、投影速度ベクトルＶ＃を採用すべきでないと判定した場合は、位置演算部２２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）、及び速度演算部２４により算出された車両の速度ベクトルＶを、最終的に出力する（Ｓ１０６）。
【００４１】
以上説明した本実施例の移動体用測位装置１によれば、車両がリンクに沿って走行していると判定されるときには、投影部２８が出力した投影速度ベクトルＶ＃を最終的に出力するため、より正確な車両の速度ベクトルを出力することができる。
【００４２】
＜第２実施例＞
以下、図面を参照し、本発明の第２実施例に係る移動体用測位装置２について説明する。
【００４３】
図６は、本発明の第２実施例に係る移動体用測位装置２のシステム構成例である。移動体用測位装置２は、第１実施例に係る移動体用測位装置１が有する構成（図では符号のみ記載した）に加えて、速度ベクトル積算部４０と、信頼性判定部４２と、を有する。
【００４４】
速度ベクトル積算部４０は、例えば、第１実施例の移動体用測位装置１により最終的に出力された速度ベクトルＶ又は投影速度ベクトルＶ＃にサンプル間隔を乗じて、当該サンプリング回における車両の位置変化（ΔＸ，ΔＹ，ΔＺ）を算出する。そして、繰り返しこのような処理を行なう中で、前回に第１実施例の移動体用測位装置１により最終的に出力された車両の位置（Ｘｕ（ｎ−１），Ｙｕ（ｎ−１），Ｚｕ（ｎ−１））に、係る車両の位置変化を加算して、今回の車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）を算出する（次式（３）参照）。
【００４５】
（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）＝（Ｘｕ（ｎ−１）＋ΔＸ，Ｙｕ（ｎ−１）＋ΔＹ，Ｚｕ（ｎ−１）＋ΔＺ） …（３）
【００４６】
ここで、速度ベクトルＶ又は投影速度ベクトルＶ＃をそのまま用いるのではなく、過去の複数のサンプル回における速度ベクトルに適切な重みを付けて加重平均したものを用いてもよい。
【００４７】
信頼性判定部４２は、位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）と、速度ベクトル積算部４０により算出された車両の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）と、のいずれの信頼性が高いかを判定する。
【００４８】
ここで、ＧＮＳＳで算出された位置、及び速度ベクトルは以下のような特徴を有する。（Ａ）位置に関しては、車速が小さい時ほどマルチパスの影響を受けやすい。（Ｂ）速度ベクトル突の方が、位置よりもマルチパスの影響を受けにくい。従って、信頼性判定部４２は、例えば以下の条件の全て、又は一部を満たす場合に、速度ベクトル積算部４０により算出された移動体の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）の信頼性が高いと判定する。
（１）車速がある値以下である。
（２）速度ベクトルの算出に用いた衛星の数がある値以上である。
（３）速度ベクトルの算出における残差平方和がある値以下である。
（４）速度ベクトルの算出に用いた衛星群のＤＯＰ（Dilution of Precision；精度低下率）がある値以下である。
【００４９】
図７は、本発明の第２実施例に係る移動体用測位装置２により実行される特徴的な処理の流れを示すフローチャートである。Ｓ１００〜Ｓ１０６については第１実施例と同様であるため、詳細な説明を省略する。
【００５０】
位置演算部２２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）、及び投影速度ベクトルＶ＃、又は速度ベクトルＶが出力されると（Ｓ１００〜１０６）、位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）と、速度ベクトル積算部４０により算出された車両の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）と、のいずれの信頼性が高いかを判定する（Ｓ１０８）。
【００５１】
位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）の信頼性が高いと判定した場合は、位置演算部３２により算出された車両の位置（Ｘｕ，Ｙｕ，Ｚｕ）を、車両の位置として出力する（Ｓ１１０）。
【００５２】
一方、速度ベクトル積算部４０により算出された移動体の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）の信頼性が高いと判定した場合は、速度ベクトル積算部４０により算出された車両の位置変化を加味して得られた車両の位置（Ｘｕ＊，Ｙｕ＊，Ｚｕ＊）を、車両の位置として出力する（Ｓ１１２）。
【００５３】
なお、Ｓ１１０又はＳ１１２において、車両の速度ベクトルも併せて出力される。
【００５４】
以上説明した本実施例の移動体用測位装置２によれば、単独測位等により算出された車両の位置と、速度ベクトルを積算して算出された車両の位置とのうち信頼性の高い方を出力するため、マルチパス等の影響を抑制することができる。
【００５５】
＜第３実施例＞
以下、図面を参照し、本発明の第３実施例に係る移動体用測位装置３について説明する。
【００５６】
図８は、本発明の第３実施例に係る移動体用測位装置３のシステム構成例である。移動体用測位装置３は、第１実施例に係る移動体用測位装置１が有する構成（図では符号のみ記載した）に加えて、慣性センサ５０と、ＩＮＳ測位演算部５２と、カルマンフィルタ５４と、車輪速センサ５６と、を有する。
【００５７】
慣性センサ５０は、例えば、加速度センサ（Ｇセンサ）やヨーレートセンサである。
【００５８】
ＩＮＳ測位演算部５２は、慣性航法により車両の位置（Ｘｕｉ，Ｙｕｉ，Ｚｕｉ）を算出する。慣性航法による車両位置の測位方法は、多種多様であり、如何なる方法であってもよい。例えば車両位置は、加速度センサの出力値に、姿勢変換、重力補正、コリオリ力補正を行って２回積分し、当該２回積分により得られる移動距離を、車両位置の前回値に加算することで導出されてよい。ＩＮＳ測位演算部５２により算出される車両位置及び車両速度（ＩＮＳ測位結果）は、測位周期毎に、上述のる移動体用測位装置１が有する構成による車両位置及び速度（ＧＰＳ測位結果）との差分値が取られ、当該差分値がカルマンフィルタ５４に入力され、各種の補正量が決定される。また、車輪速センサ５６により検出される速度との差分もカルマンフィルタ５４に入力されてよい。
【００５９】
カルマンフィルタ５４は、ＩＮＳ測位結果とＧＰＳ測位結果のそれぞれの信頼性に基づき、確率的に最も正しい値となるように、各種補正量ηを推定する。カルマンフィルタ５４における状態方程式は、次式（４）のように設定される。式中、η（ｔｎ）は、時刻ｔ＝ｔｎでの状態変数を表わす。また、ｕ（ｔｎ−１）及びｗ（ｔｎ−１）は、それぞれ、時刻ｔ＝ｔｎ−１での既知入力及び外乱（システム雑音：正規性白色雑音）である。η（ｔｎ）は、ＩＮＳ測位による車両位置や速度、車両姿勢の誤差δｒ（ＩＮＳ）、δｖ（ＩＮＳ）、δε（ＩＮＳ）、ジャイロセンサのバイアス誤差δｂ、加速度センサのドリフト誤差δｄ、タイヤの半径誤差δｓ、及び、ＧＰＳ受信機内の受信機時計の誤差δＣｔを含む。
【００６０】
η（ｔｎ）＝Ｆ・η（ｔｎ−１）＋Ｇ・ｕ（ｔｎ−１）＋Γ・ｗ（ｔｎ−１） …（４）
【００６１】
ある状態において観測量ｚ（前述の差分値）は、η（ｔｎ）との間において、次式（５）のような関係を有する。式中、Ｈは状態空間を観測空間に線形写像する役割を担う観測モデルで、ｖ（ｔｎ）は、ガウス分布に従う雑音である。
【００６２】
ｚ（ｔｎ）＝Ｈ（ｔｎ）・η（ｔｎ）＋ｖ（ｔｎ） …（５）
【００６３】
ＩＮＳ測位演算部５２は、カルマンフィルタ５４の出力に基づいて、姿勢変換、重力補正、コリオリ力補正等の補正を行なう。この結果、ＩＮＳ測位の精度を向上させることができる。
【００６４】
本実施例の移動体用測位装置３によれば、より精度の高いＩＮＳ測位を行なうことができる。
【００６５】
以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
【産業上の利用可能性】
【００６６】
本発明は、自動車製造業や自動車部品製造業等に利用可能である。
【符号の説明】
【００６７】
１、２、３ 移動体用測位装置
１０ ＧＰＳアンテナ
２０ 測位信号受信部
２２ 位置演算部
２４ 速度演算部
２６ 投影可否判定部
２８ 投影部
３０ マップマッチング部
３２ 地図データベース
４０ 速度ベクトル積算部
４２ 信頼性判定部
５０ 慣性センサ
５２ ＩＮＳ測位演算部
５４ カルマンフィルタ
５６ 車輪速センサ
【特許請求の範囲】
【請求項１】
衛星電波を用いて移動体の位置を算出する位置演算手段と、
衛星電波又は慣性航法により前記移動体の速度ベクトルを算出する速度演算手段と、
道路が複数のリンクで表現された地図データを記憶した記憶手段と、
前記速度ベクトル算出手段により算出された速度ベクトルを、前記複数のリンクのうち移動体が存すると推定されるリンクに投影した投影速度ベクトルを出力する投影手段と、
前記投影手段により出力される投影速度ベクトルの採用の可否を判定する投影可否判定手段と、を備え、
該投影可否判定手段により前記投影速度ベクトルを採用すべきと判定された場合に、前記投影速度ベクトルを前記移動体の速度ベクトルとして出力することを特徴とする、
移動体用測位装置。
【請求項２】
請求項１に記載の移動体用測位装置であって、
前記投影可否判定手段の判定結果に基づいて出力された前記移動体の速度ベクトルを積算して、前記移動体の位置変化を算出する速度ベクトル積算手段と、
前記位置演算手段により算出された移動体の位置と、前記速度ベクトル積算手段により算出された前記移動体の位置変化を加味して得られた移動体の位置と、のいずれの信頼性が高いかを判定する信頼性判定手段と、を備え、
該信頼性判定手段により、信頼性が高いと判定された方の移動体の位置を出力することを特徴とする、
移動体用測位装置。
【請求項３】
請求項１に記載の移動体用測位装置であって、
慣性航法により前記移動体の位置及び速度を算出する慣性航法測位演算手段と、
該慣性航法測位演算手段により算出された前記移動体の位置及び速度と、衛星電波により算出された前記移動体の位置及び速度との差分が入力されるカルマンフィルタと、を備え、
前記慣性航法測位演算手段は、前記カルマンフィルタの出力に応じて補正を行なうことを特徴とする、
移動体用測位装置。
【請求項１】
衛星電波を用いて移動体の位置を算出する位置演算手段と、
衛星電波又は慣性航法により前記移動体の速度ベクトルを算出する速度演算手段と、
道路が複数のリンクで表現された地図データを記憶した記憶手段と、
前記速度ベクトル算出手段により算出された速度ベクトルを、前記複数のリンクのうち移動体が存すると推定されるリンクに投影した投影速度ベクトルを出力する投影手段と、
前記投影手段により出力される投影速度ベクトルの採用の可否を判定する投影可否判定手段と、を備え、
該投影可否判定手段により前記投影速度ベクトルを採用すべきと判定された場合に、前記投影速度ベクトルを前記移動体の速度ベクトルとして出力することを特徴とする、
移動体用測位装置。
【請求項２】
請求項１に記載の移動体用測位装置であって、
前記投影可否判定手段の判定結果に基づいて出力された前記移動体の速度ベクトルを積算して、前記移動体の位置変化を算出する速度ベクトル積算手段と、
前記位置演算手段により算出された移動体の位置と、前記速度ベクトル積算手段により算出された前記移動体の位置変化を加味して得られた移動体の位置と、のいずれの信頼性が高いかを判定する信頼性判定手段と、を備え、
該信頼性判定手段により、信頼性が高いと判定された方の移動体の位置を出力することを特徴とする、
移動体用測位装置。
【請求項３】
請求項１に記載の移動体用測位装置であって、
慣性航法により前記移動体の位置及び速度を算出する慣性航法測位演算手段と、
該慣性航法測位演算手段により算出された前記移動体の位置及び速度と、衛星電波により算出された前記移動体の位置及び速度との差分が入力されるカルマンフィルタと、を備え、
前記慣性航法測位演算手段は、前記カルマンフィルタの出力に応じて補正を行なうことを特徴とする、
移動体用測位装置。
【図２】
【図３】
【図５】
【図６】
【図７】
【図８】
【図１】
【図４】
【図３】
【図５】
【図６】
【図７】
【図８】
【図１】
【図４】
【公開番号】特開２０１０−２２３６８４（Ｐ２０１０−２２３６８４Ａ）
【公開日】平成２２年１０月７日（２０１０．１０．７）
【国際特許分類】
【出願番号】特願２００９−６９７６６（Ｐ２００９−６９７６６）
【出願日】平成２１年３月２３日（２００９．３．２３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２２年１０月７日（２０１０．１０．７）
【国際特許分類】
【出願日】平成２１年３月２３日（２００９．３．２３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]