JP4694870B2 - 航法装置、航法システム、航法測位方法、および車両 - Google Patents

Description

本発明は、おもに慣性及び衛星航法を使用した航法システムに関するものである。例えば、搬送波位相ディファレンシャルＧＰＳとレーザスキャナ補強による自律車両航法システムに適用すると好適である。

本発明は以下の参考文献リストに列挙した文献に引用及び記載された技術を使用しており、明細書中の記載には参照番号を括弧書きしてその引用を示している。
ポール・デ・ヨング、クリスチャン・ティベリウス著『整数アンビギュイティ推定の為のラムダ法：実装面』ＬＧＲシリーズ１２ デルフト工業大学 １９９６年８月 ルイ・ヒロカワ、コウイチ・サトウ、ケンジ・ナカクキ著『低価格ＭＥＭＳ ＩＭＵを用いた密結合ＧＰＳ／ＩＮＳ航法の設計と評価』ＧＮＳＳシンポジウム２００３ 日本東京 ２００３年１１月 テツロウ・イマキイレ、ユウキ・ハタナカ、ヨウタ・クマキ、アツシ・ヤマギワ著『ＧＥＯＮＥＴ：日本の全国ＧＰＳ網』ＧＩＳ開発８巻３号 ２００４年３月 Ｊ．タキグチ、Ｊ．ハラム 『屋外環境下で用いる自律移動システムに関する研究』（パート３：チェーンフォームによるノンホロノミック移動ロボットのローカルパス計画）ＩＥＥＥ国際車両電子会議 中国長春 １９９９年９月 Ｋ．オオノ、Ｔ・ツボウチ 『屋外におけるディファレンシャルＧＰＳとオドメトリのデータ融合による建物間の移動ロボットの航法』ＩＥＥＥロボットとオートメーション国際会議 第２巻 １９７８−１９８４頁 ２００３年 サチン・モディ 『自律案内車両の為の３つの障害回避法の比較』修士論文 シンシナティ大学 ２００２年 ポール・デ・ヨング、クリスチャン・ティベリウス 『ラムダ法による整数アンビギュイティ推定』ＩＡＧシンポジウム「地上、空間、宇宙空間へのＧＰＳ適用の傾向」第２１回ＩＵＧＧ総会 アメリカ合衆国コロラド州ブルダー １９９５年７月 ロバートＭ．ロジャーズ 『統合航法システムにおける応用数学』ＡＩＡＡエデュケーション・シリーズ第２編 ２００３年１０月 Ｓ．スッカリエ、Ｅ．ネボット 『自律陸用車両適用の為の高整合ＩＭＵ／ＧＰＳ航法ループ』ＩＥＥＥロボットとオートメーション会報 １５巻３号 ５７２−５７８頁 １９９９年６月 上記参考文献リスト記載の各文献の内容を引用によって本明細書に組み入れる。

車両が多くの障害物をすり抜けて行くことを可能にする屋外航法システムの開発は、ＧＰＳ受信機付きの自律陸上用車両にとって重要な課題である（非特許文献４、非特許文献６）。低価格慣性センサと高性能ＧＰＳ受信機を統合した航法システムは広い範囲の移動体システムに適用できる。ＧＰＳとＩＭＵまたはオドメトリによる航法システムは広く用いられている（非特許文献９、非特許文献５）。しかしながら、閉塞性が高い環境においては、高い建物その他の障害物により性能はかなり低下する。密結合ＧＰＳ／ＩＮＳシステムは疎結合システムに比べ、特に衛星視界が劣悪な場合、ＧＰＳ擬似距離の誤差のより良い検出及びより高精度の測位等いくつかの利点がある。

この発明の一側面によれば、物体の位置及び方向を決定する航法装置において、速度検出器の出力とヨーレート検出器の出力とに基づいて物体の位置及び方向を演算する航法演算部と、全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、（１）速度検出器の誤差と、（２）ヨ−レート検出器の誤差と、（３）物体の位置誤差及び方向誤差と、（４）搬送波の整数値バイアスとを推定する推定部とを備え、航法演算部は、推定部が推定した位置誤差及び方向誤差に基づいて物体の位置及び方向を更新することを特徴とする航法装置を提供する。

また、本発明の一つの実施の形態は上記した航法装置を搭載する車両である。このような車両は陸上用車両に好適である。

さらに、本発明の別の側面によれば、物体の位置及び方向を決定する航法方法において、速度検出器の出力とヨーレート検出器の出力とに基づいて物体の位置及び方向を演算し、全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、（１）速度検出器の誤差と、（２）ヨ−レート検出器の誤差と、（３）物体の位置誤差及び方向誤差と、（４）搬送波の整数値バイアスとを推定し、推定した位置誤差及び方向誤差に基づいて物体の位置及び方向を更新することを特徴とする航法測位方法を提供する。

本発明の別の実施の形態によれば、慣性及び衛星航法を用いて自律車両の位置及び方向を決定する航法システムにおいて、（１）自律車両の速度を検出する速度検出器と、（２）自律車両のヨーレートを検出するヨーレート検出器と、（３）既知のランドマークの位置データを記憶するランドマークデータベースと、（４）自律車両に装着し車両から既知のランドマークまでの距離を測定する距離測定器と、（５）速度検出器が検出した速度とヨーレート検出器が検出したヨーレートとに基づいて自律車両の位置及び方向を演算する航法演算部と、（６）全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、速度検出器の誤差と、ヨ−レート検出器の誤差と、物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、航法演算部は、推定部が推定した位置誤差及び方向誤差に基づいて自律車両の位置及び方向を更新することを特徴とする航法システムを提供する。

さらに、本発明の別の実施の形態は上記した航法システムを搭載する車両である。このような車両は陸上用車両に好適である。

本発明により、衛星視界が劣悪な場合などにおいても、ＧＰＳの測位精度が向上する。

実施の形態１．
本発明の航法システムは水平ストラップダウン航法演算及び拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ：ＥＫＦ）から成る。図１は車両の水平運動モデルである。局所水平フレームでのストラップダウン演算は、例えば、低価格の光ファイバージャイロ（Ｆｉｂｅｒ Ｏｐｔｉｃ Ｇｙｒｏ：ＦＯＧ）のようなレートジャイロを用いた位置と方向（ヨー角度）の更新、及びＥＫＦにより補正したオドメータの入力によって行われる。ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ：微小電気機械システム）ジャイロや振動ジャイロを使用しても良い。車両の位置をＧＰＳアンテナの位相中心とし、局所ＮＥＤ（Ｎｏｒｔｈ−Ｅａｓｔ−Ｄｏｗｎ：北、東、下方向）フレームにおける位置ダイナミクスは式（１）（２）により求める。

ここで、ｖ_ｘとｖ_ｙは車体軸速度であり式（３）を満たす。

Ｖ_ｓとｒ_ｓはそれぞれオドメータが測定した補正速度及びＦＯＧが測定したヨーレートである。ｂ_ｒは後方車輪軸とＧＰＳアンテナ位相中心との間の水平距離である。

補正速度及びヨーレートは式（４）（５）により求める。

ここでＶ_ｏｄはオドメータ速度、ｒはヨーレート、ｅ_ｏｄはオドメータ倍率誤差、ｅ_ｇはジャイロ倍率誤差、ｂ_ｇはジャイロバイアス誤差である。方向ダイナミクスは式（６）によってモデル化される。

ストラップダウン演算は式（１）〜（６）に基づき行われる。ここで、式（１）〜（６）における速度及びヨーレートは図２に示すようにカルマンフィルタが推定するセンサ誤差信号により補正されない場合も補正される場合もある。

ＥＫＦは位置誤差ｐ＝［ｎｅｄ］^Ｔ（北、東、下）、センサ誤差ｓ（方向誤差ε、オドメータ倍率誤差ｅ_ｖ、ジャイロ倍率ｅ_ｇ、ジャイロバイアスｂ_ｇ）ＤＤ（ｄｏｕｂｌｅ ｄｉｆｆｅｒｅｎｃｅｄ：二重差分型）搬送波位相の浮動小数点アンビギュイティベクトルａから成る状態量ｘ＝［ｐｓａ］を推定する。状態量の次数は追従する衛星の数による。１２チャネル二周波数受信機であれば次数は２９となる。

図２はＥＫＦ２０９を備えた本発明の一つの実施の形態による航法システムのブロック図である。ＦＯＧ／オドメータ２０８からヨーレートと速度をストラップダウン航法部２１０へ入力する。レーザスキャナ２０７からＥＫＦ２０９へ相対位置情報を入力する。以下に述べるようにランドマークデータベース２０６は少なくともひとつの既知のランドマークの座標データを提供する。ＧＰＳ受信機２０１、ＤＤ演算部２０２、衛星軌道演算部２０４はＥＫＦ２０９へ衛星航法情報を入力する。以下に詳述するように、ラムダ（ＬＡＭＢＤＡ）アンビギュイティ決定部２０３とサイクルスリップ検出部２０５は搬送波の整数値バイアスにおけるアンビギュイティを決定する。

カルマンフィルタ２０９はストラップダウン航法部２１０における誤差とセンサ入力の誤差を補正できるようにフィードバック形式となっている。ステートｘのダイナミクスは線形差分方程式（７）のように表せる。

ここでＦはシステムマトリクス、ｗはプロセスノイズベクトルである。

システムマトリクスＦはシステム誤差ダイナミクスから導かれ、以下の式（８）（９）（１０）のように表せる。

ここでτ_ｇはジャイロバイアスの相関時間である。

高度、浮動小数点アンビギュイティ、センサ入力といったその他の状態量の誤差ダイナミクスはランダムウォークプロセスモデルとして定式化される。

カルマンフィルタのダイナミクスは離散形で表される。システムФの遷移マトリクスは式（１２）のように演算する。

ここで、τはサンプル時間である。

カルマンフィルタ演算は時間伝播と測定値更新から成る。共分散マトリクスＰの時間伝播は式（１３）により求める。

ここでＱ_ｄはプロセスノイズマトリクスである。

ＥＫＦの測定値更新は以下の式（１４）（１５）（１６）で表せる。

ここで、Ｈは測位マトリクス、Ｋはカルマンゲインマトリクス、Ｒは測位ノイズマトリクス、Δｚは測定値残差である。

位置、方向角、センサ誤差、及び浮動小数点アンビギュイティは式（１７）により更新する。

ここでｍは観測衛星数−１である。

２つの異なる測定値、ＧＰＳによるＤＤ距離、レーザスキャナによる相対距離を用いる。測定値マトリクスＨと測定値剰余Δｚは下記のように設定する。二周波ＤＤ搬送波位相とコード位相はフィルタの測定値として用いる。

整数アンビギュイティを適切に求めた後、ＥＫＦの次数を７まで下げ、アンビギュイティを決定した二周波二重差分型（ＤＤ）搬送波位相のみを測定値として用いる。

ＥＫＦの測定値マトリクスＨは式（１８）のように設定する。

ここでＣ_ｎ ^ｅは局所水平フレームとＥＣＥＦ（Ｅａｒｔｈ−Ｃｅｎｔｅｒｅｄ Ｅａｒｔｈ−Ｆｉｘｅｄ）フレーム間の方向余弦マトリクス、Ａは各衛星の幾何学的射影マトリクス、λ_１、λ_２はそれぞれＧＰＳのＬ１，Ｌ２の波長を表す。

ＧＰＳ測定値更新の測定値マトリクスＲはＤＤ観測に基づいているため相関性が高い。測定値残差Δｚは式（１９）により求める。

ここでΔρ_ｃはＤＤ幾何距離、Δρ_Ｃ１、Δρ_Ｌ１、Δρ_Ｌ２は観測値に基づくＤＤコードと搬送波位相、ａ_１とａ_２はＬ１とＬ２のＤＤアンビギュイティである。

レーザスキャナが測定した相対位置もまた位置誤差および方向誤差を補正するための追加測定値として用いる。コーナーランドマークはランドマークデータベース中に記憶された既知の位置を持ち、このコーナーランドマークを用いて、ランドマークとレーザスキャナセンサ間の相対位置を測定することにより誤差を推定する。

ラインランドマークは二つの連続するコーナーランドマークを結ぶ線によって成る。ラインランドマークとスキャナ間の相対距離も測定可能である。ラインランドマークの相対位置は測定できないが、相対角度は測定可能である。ラインランドマークを伴うコーナーランドマーク更新は演算効率のために用いる。図３はランドマーク更新の定義を表している。

コーナーランドマークとレーザスキャナ間の距離ｒ_ａはレーザスキャナによって測定する。コーナーランドマークの位置は測定距離の不連続性によって検出可能である。ボディフレームの方向θ_ａもセンサによって測定する。レーザスキャナとコーナーランドマーク間の距離ベクトルは車体軸と航法軸では式（２１）（２２）のように設定する。

センサの位置は車両の位置と方向により式（２３）のように演算する。

ここでｂ_ｓはＧＰＳアンテナとレーザスキャナとの間の距離である。

相対距離ｒ_ｃと車体軸の方向θ_ｃは式（２４）（２５）により求める。

ここでΔθは好ましくは３０度とする。

測定値残差Δｚは式（２８）により設定する。

ここでψ_ｃはランドマークデータベースから取得したラインランドマークの方向角である。

測定値マトリクスＨは式（２９）により設定する。

最小２乗基準を持つ整数アンビギュイティをレストテスト及び残差テストを用いて確認する。レストテストの閾値レベルは測定値数と信頼レベルの関数として設定する。Ｆ分散の９９％信頼レベルが好ましい。

図２０は本発明の一つの実施の形態による物体の位置及び方向を決定する方法を示している。ステップ２００１では、物体の位置及び方向は次の運動方程式により演算される。

ここで、Ｎ，Ｅ，Ｄは位置の成分、φは方向、Ｖは速度検出器が測定した速度、ｒはヨーレート検出器が測定したヨーレート、ｔは時間、ｂは所定の距離である。次に、ステップ２００２では、二重差分型搬送波位相の浮動小数点アンビギュイティベクトルを拡張カルマンフィルタを用いて推定し、搬送波の整数値バイアスにおけるアンビギュイティは推定した二重差分型搬送波位相の浮動小数点アンビギュイティベクトルに基づきラムダ法を用いて決定する。ステップ２００３では、全地球測位衛星から受信した搬送波位相と擬似距離に基づき、（１）速度検出器の誤差、（２）ヨーレート検出器の誤差、（３）対象物の位置誤差及び方向誤差、（４）搬送波の整数値バイアスを推定する。ステップ２００４では、既知のランドマークと物体との間の相対距離を、物体に搭載した距離測定器が測定した距離及び保存されている既知のランドマークの位置データに基づき推定する。さらに、推定した相対距離に基づき観測誤差を推定する。最後にステップ２００５では、推定した位置誤差及び方向誤差に基づき物体の位置及び方向を更新する。

シミュレーション
本発明の航法システム及び方法の実施の形態の設計評価を数値シミュレーションにより行った。図４は種々の誤差要因を含むシミュレーションモデルを示す。シミュレーションで使用したのは、図５に示すいくつかの高い建物の近傍を通過する水平経路である。ＧＰＳ軌道はＧＰＳ週が１２７８のアルマナックを使ってシミュレートした。衛星のマスク角は１０度とした。

高い建物による遮蔽をシミュレートしてＧＰＳ衛星が一部利用できない状態を第一のケースで考慮する。遮蔽はｔ＝５０秒の時マスク角４５度として表す。図６に観測可能な衛星のスカイプロットを示す。観測された衛星の数は１０機であるが、遮蔽の後は３機の衛星しか観測されない。こうした場合には従来のＲＴＫ−ＧＰＳでは定位置を出力できない。ここで述べる密結合ＧＰＳ／ＤＲ航法システムによれば、衛星の数が４機未満であっても、ＤＤ位相とコードにより測定値更新が可能である。

シミュレーションでのセンサ誤差を式（３１）により求める。

ε＝０．２度、ｅ_ｖ＝５％、ｅ_ｇ＝５％、ｂ_ｇ＝０．１度／秒 （３１）

図７にシミュレーションに用いる車両の速度（ｖｅｌｏｃｉｔｙ）、方向（ｈｅａｄｉｎｇ）、ヨーレート（ｙａｗ ｒａｔｅ）を示す。横軸は時間［秒］（ｔｉｍｅ［ｓ］）である。図７（ａ）で実線はオドメトリ（ｏｄｏｍｅｔｒｙ）による測定値、（ｂ）で実線はＤ／Ｒによる測定値、（ｃ）で実線はジャイロ（ｇｙｒｏ）による測定値、破線はそれぞれ真の値（ｔｒｕｅ）を示す。ＥＫＦが推定するセンサ誤差を図８に示す。図８で（ａ）は方向、（ｂ）は速度、（ｃ）はジャイロ誤差（ｇｙｒｏ ｓ／ｆ ｅｒｒｏｒ）、（ｄ）はジャイロバイアス（ｇｙｒｏ ｂｉａｓ）を示す。推定誤差は真の値に収束している。ＥＫＦによる位置誤差を図９に示す。図９で（ａ）は東方向（ｅａｓｔｉｎｇ）、（ｂ）は北方向（ｎｏｒｔｈｉｎｇ）、（ｃ）は上方向（ｕｐ）への移動による誤差を示している。比較の為に、従来の疎結合（ｌｏｏｓｅｌｙ ｃｏｕｐｌｅｄ）ＲＴＫ−ＧＰＳ／ＤＲ航法システムによる位置誤差を同じ図中に示す。密結合（ｔｉｇｈｔｌｙ ｃｏｕｐｌｅｄ）システムによる位置誤差は疎結合システムよりはるかに小さい。第二のケースでは、衛星完全遮蔽をｔ＝５０秒でシミュレートする。この場合は、ランドマーク更新を効果的に用いている。レーザスキャナの最大距離は３０ｍ、距離精度は４ｃｍ、角分解能は１度であると仮定する。図１０に示すように距離が３０ｍ未満であれば、コーナーランドマークの前でランドマーク更新を行う。実線で示したランドマーク測定値更新、及び破線で示した真のセンサ誤差と共にＥＫＦが推定するセンサ誤差を図１１に示す。推定誤差は真の値に収束している。図１２はランドマーク更新を行った場合と行わない場合の位置誤差を示す。図１２で（ａ）は東方向、（ｂ）は北方向への移動による誤差を示し、それぞれ実線はランドマーク更新を行った場合（ｗｉｔｈ ｌａｎｄｍａｒｋ ｕｐｄａｔｅ）、破線はランドマーク更新を行わない場合（ｗｉｔｈｏｕｔ ｌａｎｄｍａｒｋ ｕｐｄａｔｅ）を示している。ＧＰＳ測定値が得られない場合でも位置誤差は小さい。

野外試験（フィールドテスト）
図１３に示す自律陸上用車両を野外試験に使用した。このＡＧＶ（Ａｕｔｏｎｏｍｏｕｓ Ｇｒｏｕｎｄ Ｖｅｈｉｃｌｅ）は一定の地域を自律巡回する監視車両として開発された。レーザスキャナ、距離精度３５ｍｍで角分解能１度のＳＩＣＫ ＬＭＳ２９１、監視用の全方位ビデオ（Ｏｍｎｉ−Ｄｉｒｅｃｔｉｏｎａｌ Ｖｉｄｅｏ：ＯＤＶ）カメラ、二重周波数ＧＰＳ受信機、アシュテックＺエクストリーム（Ａｓｈｔｅｃｈ Ｚ−Ｘｔｒｅｍｅ）、１度／時の精度のＦＯＧ、及び高分解オドメータを備える。この車両のリアルタイム誘導とＧＰＳ／ＤＲ航法はオンボードのＲＴ（リアルタイム）リナックス（登録商標）コンピュータ上で実行する。ここで分析したデータは２００４年７月６日、鎌倉で収集したものである。藤沢でのＧＥＯＮＥＴ［３］の基地局で観測した二周波数ＧＰＳデータは差分補正に使用する。測量基線長は約５ｋｍである。観測された衛星は図６に示すものとほぼ同じである。ＲＴＫ−ＧＰＳ受信機によるアンビギュイティは時間の４０％のみ整数解に基づく位置となり、時間の３２％については解を持たない。

現存のオンボードリアルタイム実装は疎結合航法に基づいている。密結合航法の解は後処理演算により求めた。図１４と図１５に野外試験での演算位置結果を示す。図１４は水平位置、図１５で（ａ）は北方向、（ｂ）は東方向、（ｃ）は下方向（ｄｏｗｎ）の位置を示している。それぞれの図中で黒実線は整数解（ｆｉｘｅｄ）を示し灰色実線は浮動小数点値（ｆｌｏａｔ）である。図１６で（ａ）は観測された衛星の数（ｎｕｍｂｅｒ ｏｆ ｓａｔｓ）、（ｂ）は整数アンビギュイティの推定有効性（ａｍｂｉｇｕｉｔｙ ｖａｌｉｄｉｔｙ）を示す。有効性が１であるなら、整数アンビギュイティが求められていると推定できる。密結合航法システムの場合は、位置の７０％以上が整数解に基づくものである。ＥＫＦによる演算位置とＲＴＫ−ＧＰＳ受信機の整数解を比較してセンチメートルレベルの精度を確認した。ＲＴＫ−ＧＰＳの整数解が得られない場合は、ＤＤ搬送波位相の残差解析によって位置精度を確認した。

ランドマーク更新は経路の二番目のコーナーで行う。図１７は車両の経路とｔ＝２５４秒とｔ＝２６４．３秒におけるレーザスキャナ距離のプロットを示す。レーザスキャナ距離のプロットは経路方向に合わせるため車両の方向により回転する。この図はレーザスキャナによる距離測定によってコーナーが検出可能であることを示している。

図１８に、コーナーランドマークの測定相対距離及び相対角度、ラインランドマークの測定斜度を含むレーザスキャナによる二つ目のコーナーでの測定データの例を示している。図１８で（ａ）は距離（ｒａｎｇｅ）、（ｂ）は角度（ａｎｇｌｅ）、（ｃ）は傾斜度（ｓｌｏｐｅ）をそれぞれ示す。

図１４に示すＧＰＳ／ＤＲ位置は既に高精度であるので、ＧＰＳ測定値更新無しにランドマーク更新を行う。図１９にランドマーク更新による演算位置結果を示す。比較の為に、ランドマーク更新無しの結果とＧＰＳ／ＤＲ位置を同図に示す。演算位置結果はＧＰＳ／ＤＲ位置とほとんど同じである。ＧＰＳ測定値が得られない場合でも、コーナーランドマーク及びラインランドマーク更新により位置精度は向上する。

搬送波位相ディファレンシャルＧＰＳとＡＧＶ用レーザスキャナにより補強された水平航法システムを設計し、その性能を数値シミュレーション及び野外試験によって確認した。本発明の実施の形態によれば、搬送波位相ディファレンシャルＧＰＳとＬＳ補強を利用した密結合航法システムを提案することで衛星視界が劣悪な場合でもデシメートルレベルの測位精度が得られる。

陸上用の車両の動的モデル図である。 本発明の一つの実施の形態によるＧＰＳ／ＤＲ航法システムを示す図である。 ランドマーク更新を示す図である。 ＧＰＳのシミュレーションモデルを示す図である。 シミュレーションに用いた水平航法経路を示す図である。 ２００４年７月６日に行った衛星スカイプロットを示す図である。 シミュレーション時の車両の速度、方向、ヨーレートを示す図である。 拡張カルマンフィルタが推定したセンサ誤差を示す図である。 拡張カルマンフィルタが推定した位置誤差を示す図である。 ランドマーク更新によるシミュレーションに用いた水平航法経路を示す図である。 ランドマーク更新の際の拡張カルマンフィルタが推定したセンサ誤差を示す図である。 ランドマーク更新の際の拡張カルマンフィルタが推定した位置誤差を示す図である。 自律陸上車両（ＡＧＶ）を示す図である。 野外試験時のカルマンフィルタが推定した水平位置を示す図である。 野外試験時のＮＥＤ位置を示す図である。 衛星数及び整数アンビギュイティの推定有効性を示す図である。 野外試験時の車両経路及びレーザスキャナの測定距離のプロットを示す図である。 野外試験時のランドマーク更新のための測定データを示す図である。 野外試験時のランドマーク更新の有無による演算位置更新を示す図である。 本発明の一つの実施の形態による方法を示す図である。

符号の説明

２０１ ＧＰＳ受信機、２０２ ＤＤ演算部、２０３ ラムダアンビギュイティ決定部、２０４ 衛星軌道演算部、２０５ サイクルスリップ検出部、２０６ ランドマークデータベース、２０７ レーザスキャナ、２０８ ＦＯＧ／オドメータ、２０９ 拡張カルマンフィルタ、２１０ ストラップダウン航法部。

Claims (14)

1. 物体の位置及び方向を判断する航法装置において、
   速度検出器の出力とヨーレート検出器の出力とに基づいて前記物体の位置及び方向を演算する航法演算部と、
   ＧＰＳ（全地球測位システム）受信機の出力に基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記物体の位置誤差及び方向誤差とを推定する推定部とを備え、
   前記航法演算部は、前記推定部が推定した各誤差に基づいて前記物体の位置及び方向を更新し、
   前記推定部は、方向角が予めデータベースに記憶されている線状のラインランドマーク上の２地点と前記物体との間の距離であって前記物体に設置された距離測定器が測定した距離と、前記物体に対して前記ラインランドマーク上の２地点が存在する方向とに基づいて前記ラインランドマークと前記物体との相対角度を演算し、演算した相対角度と前記航法演算部が演算した方向とから前記ラインランドマークの方向角を求め、求めた方向角と前記データベースに記憶されている方向角との残差に基づいて各誤差を推定し、
   前記物体に対して前記ラインランドマーク上の２地点の一方が存在する方向と前記物体に対して前記ラインランドマーク上の２地点の他方が存在する方向とのなす角度は、予め定められた角度であることを特徴とする航法装置。
2. 前記航法演算部は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
   により前記物体の位置及び方向を演算することを特徴とする請求項１に記載の航法装置。
3. 前記推定部は、
   前記ＧＰＳ受信機が全地球測位衛星から受信した信号の二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定する拡張カルマンフィルタと、
   前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記ＧＰＳ受信機が全地球測位衛星から受信した信号の搬送波の整数値バイアスのアンビギュイティを決定するアンビギュイティ決定部とを備えることを特徴とする請求項１又は２に記載の航法装置。
4. 前記航法演算部は、オドメータの出力とレートジャイロの出力とに基づいて前記物体の位置及び方向を演算することを特徴とする請求項１から３のいずれかに記載の航法装置。
5. 請求項１から４のいずれかに記載の航法装置を搭載する車両。
6. 物体の位置及び方向を得る航法測位方法において、
   速度検出器の出力とヨーレート検出器の出力とに基づいて前記物体の位置及び方向を演算する演算工程と、
   ＧＰＳ（全地球測位システム）受信機の出力に基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記物体の位置誤差及び方向誤差とを推定する推定工程と、
   推定した各誤差に基づいて前記物体の位置及び方向を更新する更新工程とを備え、
   前記推定工程は、方向角が予めデータベースに記憶されている線状のラインランドマーク上の２地点と前記物体との間の距離であって前記物体に設置された距離測定器が測定した距離と、前記物体に対して前記ラインランドマーク上の２地点が存在する方向とに基づいて前記ラインランドマークと前記物体との相対角度を演算し、演算した相対角度と前記航法演算部が演算した方向とから前記ラインランドマークの方向角を求め、求めた方向角と前記データベースに記憶されている方向角との残差に基づいて各誤差を推定し、
   前記物体に対して前記ラインランドマーク上の２地点の一方が存在する方向と前記物体に対して前記ラインランドマーク上の２地点の他方が存在する方向とのなす角度は、予め定められた角度であることを特徴とする航法測位方法。
7. 前記推定工程は、レーザスキャナが測定した距離に基づいて前記ラインランドマークの方向角を求めることを特徴とする請求項６に記載の航法測位方法。
8. 前記演算工程は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
   により前記物体の位置及び方向を演算することを特徴とする請求項６又は７に記載の航法測位方法。
9. 前記推定工程は、
   拡張カルマンフィルタを用いて、前記ＧＰＳ受信機が全地球測位衛星から受信した信号の二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定し、
   前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記ＧＰＳ受信機が全地球測位衛星から受信した信号の搬送波の整数値バイアスのアンビギュイティを決定することを特徴とする請求項６から８のいずれかに記載の航法測位方法。
10. 前記演算工程は、オドメータの出力とレートジャイロの出力とに基づいて前記物体の位置及び方向を演算することを特徴とする請求項６から９のいずれかに記載の航法測位方法。
11. 自律車両の位置及び方向を判断する航法システムにおいて、
    前記自律車両の速度を検出する速度検出器と、
    前記自律車両のヨーレートを検出するヨーレート検出器と、
    線状のラインランドマークの方向角を予め記憶するランドマークデータベースと、
    前記自律車両に装着され前記自律車両から前記ラインランドマーク上の２地点までの距離を測定する距離測定器と、
    前記速度検出器が検出した速度と前記ヨーレート検出器が検出したヨーレートとに基づいて前記自律車両の位置及び方向を演算する航法演算部と、
    ＧＰＳ（全地球測位システム）受信機の出力に基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記自律車両の位置誤差及び方向誤差とを推定する推定部とを備え、
    前記航法演算部は、前記推定部が推定した各誤差に基づいて前記自律車両の位置及び方向を更新し、
    前記推定部は、前記距離測定器が測定した距離と、前記自律車両に対して前記ラインランドマーク上の２地点が存在する方向とに基づいて前記ラインランドマークと前記自律車両との相対角度を演算し、演算した相対角度と前記航法演算部が演算した方向とから前記ラインランドマークの方向角を求め、求めた方向角と前記データベースに記憶されている方向角との残差に基づいて各誤差を推定し、
    前記自律車両に対して前記ラインランドマーク上の２地点の一方が存在する方向と前記自律車両に対して前記ラインランドマーク上の２地点の他方が存在する方向とのなす角度は、予め定められた角度であることを特徴とする航法システム。
12. 前記航法演算部は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
    により前記自律車両の位置及び方向を演算することを特徴とする請求項１１に記載の航法システム。
13. 前記推定部は、
    前記ＧＰＳ受信機が全地球測位衛星から受信した信号の二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定する拡張カルマンフィルタと、
    前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記ＧＰＳ受信機が全地球測位衛星から受信した信号の搬送波の整数値バイアスのアンビギュイティを決定するアンビギュイティ決定部とを備えることを特徴とする請求項１１又は１２に記載の航法システム。
14. 請求項１１から１３のいずれかに記載の航法システムを装備した車両。