航法装置、航法システム、航法測位方法、および車両
【課題】 搬送波差分型全地球測位システム（ＧＰＳ）受信機とレーザスキャナを有する自律陸上用車両（ＡＧＶ）のための水平航法システムを提供する。高精度の車両航法システムは先進ＡＧＶに強く求められている。高性能のＲＴＫ−ＧＰＳ受信機によっても高い測位精度は得られるが、高い建物やその他の障害物により遮蔽された環境下では、その性能は著しく低下する。
【解決手段】 本発明の航法システムによれば、こうした苛酷な環境下であっても、正確なＧＰＳ測位に必要なデシメートルレベルの測位精度が得られる。水平航法システムは低価格の光ファイバージャイロ（ＦＯＧ）と精密なオドメータから成る。航法誤差は密結合の拡張カルマンフィルタ（ＥＫＦ）を使用して推定する。ＥＫＦの測定値としては二周波数ＧＰＳ受信機からの二重差分型符号と搬送波位相及びレーザスキャナ測定から生じる相対位置がある。
【解決手段】 本発明の航法システムによれば、こうした苛酷な環境下であっても、正確なＧＰＳ測位に必要なデシメートルレベルの測位精度が得られる。水平航法システムは低価格の光ファイバージャイロ（ＦＯＧ）と精密なオドメータから成る。航法誤差は密結合の拡張カルマンフィルタ（ＥＫＦ）を使用して推定する。ＥＫＦの測定値としては二周波数ＧＰＳ受信機からの二重差分型符号と搬送波位相及びレーザスキャナ測定から生じる相対位置がある。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、おもに慣性及び衛星航法を使用した航法システムに関するものである。例えば、搬送波位相ディファレンシャルＧＰＳとレーザスキャナ補強による自律車両航法システムに適用すると好適である。
【背景技術】
【０００２】
本発明は以下の参考文献リストに列挙した文献に引用及び記載された技術を使用しており、明細書中の記載には参照番号を括弧書きしてその引用を示している。
【非特許文献１】ポール・デ・ヨング、クリスチャン・ティベリウス著『整数アンビギュイティ推定の為のラムダ法：実装面』ＬＧＲシリーズ１２ デルフト工業大学 １９９６年８月
【非特許文献２】ルイ・ヒロカワ、コウイチ・サトウ、ケンジ・ナカクキ著『低価格ＭＥＭＳ ＩＭＵを用いた密結合ＧＰＳ／ＩＮＳ航法の設計と評価』ＧＮＳＳシンポジウム２００３ 日本東京 ２００３年１１月
【非特許文献３】テツロウ・イマキイレ、ユウキ・ハタナカ、ヨウタ・クマキ、アツシ・ヤマギワ著『ＧＥＯＮＥＴ：日本の全国ＧＰＳ網』ＧＩＳ開発８巻３号 ２００４年３月
【非特許文献４】Ｊ．タキグチ、Ｊ．ハラム 『屋外環境下で用いる自律移動システムに関する研究』（パート３：チェーンフォームによるノンホロノミック移動ロボットのローカルパス計画）ＩＥＥＥ国際車両電子会議 中国長春 １９９９年９月
【非特許文献５】Ｋ．オオノ、Ｔ・ツボウチ 『屋外におけるディファレンシャルＧＰＳとオドメトリのデータ融合による建物間の移動ロボットの航法』ＩＥＥＥロボットとオートメーション国際会議 第２巻 １９７８−１９８４頁 ２００３年
【非特許文献６】サチン・モディ 『自律案内車両の為の３つの障害回避法の比較』修士論文 シンシナティ大学 ２００２年
【非特許文献７】ポール・デ・ヨング、クリスチャン・ティベリウス 『ラムダ法による整数アンビギュイティ推定』ＩＡＧシンポジウム「地上、空間、宇宙空間へのＧＰＳ適用の傾向」第２１回ＩＵＧＧ総会 アメリカ合衆国コロラド州ブルダー １９９５年７月
【非特許文献８】ロバートＭ．ロジャーズ 『統合航法システムにおける応用数学』ＡＩＡＡエデュケーション・シリーズ第２編 ２００３年１０月
【非特許文献９】Ｓ．スッカリエ、Ｅ．ネボット 『自律陸用車両適用の為の高整合ＩＭＵ／ＧＰＳ航法ループ』ＩＥＥＥロボットとオートメーション会報 １５巻３号 ５７２−５７８頁 １９９９年６月 上記参考文献リスト記載の各文献の内容を引用によって本明細書に組み入れる。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
車両が多くの障害物をすり抜けて行くことを可能にする屋外航法システムの開発は、ＧＰＳ受信機付きの自律陸上用車両にとって重要な課題である（非特許文献４、非特許文献６）。低価格慣性センサと高性能ＧＰＳ受信機を統合した航法システムは広い範囲の移動体システムに適用できる。ＧＰＳとＩＭＵまたはオドメトリによる航法システムは広く用いられている（非特許文献９、非特許文献５）。しかしながら、閉塞性が高い環境においては、高い建物その他の障害物により性能はかなり低下する。密結合ＧＰＳ／ＩＮＳシステムは疎結合システムに比べ、特に衛星視界が劣悪な場合、ＧＰＳ擬似距離の誤差のより良い検出及びより高精度の測位等いくつかの利点がある。
【課題を解決するための手段】
【０００４】
この発明の一側面によれば、物体の位置及び方向を決定する航法装置において、速度検出器の出力とヨーレート検出器の出力とに基づいて物体の位置及び方向を演算する航法演算部と、全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、（１）速度検出器の誤差と、（２）ヨ−レート検出器の誤差と、（３）物体の位置誤差及び方向誤差と、（４）搬送波の整数値バイアスとを推定する推定部とを備え、航法演算部は、推定部が推定した位置誤差及び方向誤差に基づいて物体の位置及び方向を更新することを特徴とする航法装置を提供する。
【０００５】
また、本発明の一つの実施の形態は上記した航法装置を搭載する車両である。このような車両は陸上用車両に好適である。
【０００６】
さらに、本発明の別の側面によれば、物体の位置及び方向を決定する航法方法において、速度検出器の出力とヨーレート検出器の出力とに基づいて物体の位置及び方向を演算し、全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、（１）速度検出器の誤差と、（２）ヨ−レート検出器の誤差と、（３）物体の位置誤差及び方向誤差と、（４）搬送波の整数値バイアスとを推定し、推定した位置誤差及び方向誤差に基づいて物体の位置及び方向を更新することを特徴とする航法測位方法を提供する。
【０００７】
本発明の別の実施の形態によれば、慣性及び衛星航法を用いて自律車両の位置及び方向を決定する航法システムにおいて、（１）自律車両の速度を検出する速度検出器と、（２）自律車両のヨーレートを検出するヨーレート検出器と、（３）既知のランドマークの位置データを記憶するランドマークデータベースと、（４）自律車両に装着し車両から既知のランドマークまでの距離を測定する距離測定器と、（５）速度検出器が検出した速度とヨーレート検出器が検出したヨーレートとに基づいて自律車両の位置及び方向を演算する航法演算部と、（６）全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、速度検出器の誤差と、ヨ−レート検出器の誤差と、物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、航法演算部は、推定部が推定した位置誤差及び方向誤差に基づいて自律車両の位置及び方向を更新することを特徴とする航法システムを提供する。
【０００８】
さらに、本発明の別の実施の形態は上記した航法システムを搭載する車両である。このような車両は陸上用車両に好適である。
【発明の効果】
【０００９】
本発明により、衛星視界が劣悪な場合などにおいても、ＧＰＳの測位精度が向上する。
【発明を実施するための最良の形態】
【００１０】
実施の形態１．
本発明の航法システムは水平ストラップダウン航法演算及び拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ：ＥＫＦ）から成る。図１は車両の水平運動モデルである。局所水平フレームでのストラップダウン演算は、例えば、低価格の光ファイバージャイロ（Ｆｉｂｅｒ Ｏｐｔｉｃ Ｇｙｒｏ：ＦＯＧ）のようなレートジャイロを用いた位置と方向（ヨー角度）の更新、及びＥＫＦにより補正したオドメータの入力によって行われる。ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ：微小電気機械システム）ジャイロや振動ジャイロを使用しても良い。車両の位置をＧＰＳアンテナの位相中心とし、局所ＮＥＤ（Ｎｏｒｔｈ−Ｅａｓｔ−Ｄｏｗｎ：北、東、下方向）フレームにおける位置ダイナミクスは式（１）（２）により求める。
【００１１】
【数４】
ここで、ｖｘとｖｙは車体軸速度であり式（３）を満たす。
【００１２】
【数５】
Ｖｓとｒｓはそれぞれオドメータが測定した補正速度及びＦＯＧが測定したヨーレートである。ｂｒは後方車輪軸とＧＰＳアンテナ位相中心との間の水平距離である。
【００１３】
補正速度及びヨーレートは式（４）（５）により求める。
【００１４】
【数６】
ここでＶｏｄはオドメータ速度、ｒはヨーレート、ｅｏｄはオドメータ倍率誤差、ｅｇはジャイロ倍率誤差、ｂｇはジャイロバイアス誤差である。方向ダイナミクスは式（６）によってモデル化される。
【００１５】
【数７】
ストラップダウン演算は式（１）〜（６）に基づき行われる。ここで、式（１）〜（６）における速度及びヨーレートは図２に示すようにカルマンフィルタが推定するセンサ誤差信号により補正されない場合も補正される場合もある。
【００１６】
ＥＫＦは位置誤差ｐ＝［ｎｅｄ］Ｔ（北、東、下）、センサ誤差ｓ（方向誤差ε、オドメータ倍率誤差ｅｖ、ジャイロ倍率ｅｇ、ジャイロバイアスｂｇ）ＤＤ（ｄｏｕｂｌｅ ｄｉｆｆｅｒｅｎｃｅｄ：二重差分型）搬送波位相の浮動小数点アンビギュイティベクトルａから成る状態量ｘ＝［ｐｓａ］を推定する。状態量の次数は追従する衛星の数による。１２チャネル二周波数受信機であれば次数は２９となる。
【００１７】
図２はＥＫＦ２０９を備えた本発明の一つの実施の形態による航法システムのブロック図である。ＦＯＧ／オドメータ２０８からヨーレートと速度をストラップダウン航法部２１０へ入力する。レーザスキャナ２０７からＥＫＦ２０９へ相対位置情報を入力する。以下に述べるようにランドマークデータベース２０６は少なくともひとつの既知のランドマークの座標データを提供する。ＧＰＳ受信機２０１、ＤＤ演算部２０２、衛星軌道演算部２０４はＥＫＦ２０９へ衛星航法情報を入力する。以下に詳述するように、ラムダ（ＬＡＭＢＤＡ）アンビギュイティ決定部２０３とサイクルスリップ検出部２０５は搬送波の整数値バイアスにおけるアンビギュイティを決定する。
【００１８】
カルマンフィルタ２０９はストラップダウン航法部２１０における誤差とセンサ入力の誤差を補正できるようにフィードバック形式となっている。ステートｘのダイナミクスは線形差分方程式（７）のように表せる。
【００１９】
【数８】
ここでＦはシステムマトリクス、ｗはプロセスノイズベクトルである。
【００２０】
システムマトリクスＦはシステム誤差ダイナミクスから導かれ、以下の式（８）（９）（１０）のように表せる。
【００２１】
【数９】
ここでτｇはジャイロバイアスの相関時間である。
【００２２】
高度、浮動小数点アンビギュイティ、センサ入力といったその他の状態量の誤差ダイナミクスはランダムウォークプロセスモデルとして定式化される。
【００２３】
【数１０】
カルマンフィルタのダイナミクスは離散形で表される。システムФの遷移マトリクスは式（１２）のように演算する。
【００２４】
【数１１】
ここで、τはサンプル時間である。
【００２５】
カルマンフィルタ演算は時間伝播と測定値更新から成る。共分散マトリクスＰの時間伝播は式（１３）により求める。
【００２６】
【数１２】
ここでＱｄはプロセスノイズマトリクスである。
【００２７】
ＥＫＦの測定値更新は以下の式（１４）（１５）（１６）で表せる。
【００２８】
【数１３】
ここで、Ｈは測位マトリクス、Ｋはカルマンゲインマトリクス、Ｒは測位ノイズマトリクス、Δｚは測定値残差である。
【００２９】
位置、方向角、センサ誤差、及び浮動小数点アンビギュイティは式（１７）により更新する。
【００３０】
【数１４】
ここでｍは観測衛星数−１である。
【００３１】
２つの異なる測定値、ＧＰＳによるＤＤ距離、レーザスキャナによる相対距離を用いる。測定値マトリクスＨと測定値剰余Δｚは下記のように設定する。二周波ＤＤ搬送波位相とコード位相はフィルタの測定値として用いる。
【００３２】
整数アンビギュイティを適切に求めた後、ＥＫＦの次数を７まで下げ、アンビギュイティを決定した二周波二重差分型（ＤＤ）搬送波位相のみを測定値として用いる。
【００３３】
ＥＫＦの測定値マトリクスＨは式（１８）のように設定する。
【００３４】
【数１５】
ここでＣｎｅは局所水平フレームとＥＣＥＦ（Ｅａｒｔｈ−Ｃｅｎｔｅｒｅｄ Ｅａｒｔｈ−Ｆｉｘｅｄ）フレーム間の方向余弦マトリクス、Ａは各衛星の幾何学的射影マトリクス、λ１、λ２はそれぞれＧＰＳのＬ１，Ｌ２の波長を表す。
【００３５】
ＧＰＳ測定値更新の測定値マトリクスＲはＤＤ観測に基づいているため相関性が高い。測定値残差Δｚは式（１９）により求める。
【００３６】
【数１６】
ここでΔρｃはＤＤ幾何距離、ΔρＣ１、ΔρＬ１、ΔρＬ２は観測値に基づくＤＤコードと搬送波位相、ａ１とａ２はＬ１とＬ２のＤＤアンビギュイティである。
【００３７】
【数１７】
レーザスキャナが測定した相対位置もまた位置誤差および方向誤差を補正するための追加測定値として用いる。コーナーランドマークはランドマークデータベース中に記憶された既知の位置を持ち、このコーナーランドマークを用いて、ランドマークとレーザスキャナセンサ間の相対位置を測定することにより誤差を推定する。
【００３８】
ラインランドマークは二つの連続するコーナーランドマークを結ぶ線によって成る。ラインランドマークとスキャナ間の相対距離も測定可能である。ラインランドマークの相対位置は測定できないが、相対角度は測定可能である。ラインランドマークを伴うコーナーランドマーク更新は演算効率のために用いる。図３はランドマーク更新の定義を表している。
【００３９】
コーナーランドマークとレーザスキャナ間の距離ｒａはレーザスキャナによって測定する。コーナーランドマークの位置は測定距離の不連続性によって検出可能である。ボディフレームの方向θａもセンサによって測定する。レーザスキャナとコーナーランドマーク間の距離ベクトルは車体軸と航法軸では式（２１）（２２）のように設定する。
【００４０】
【数１８】
センサの位置は車両の位置と方向により式（２３）のように演算する。
【００４１】
【数１９】
ここでｂｓはＧＰＳアンテナとレーザスキャナとの間の距離である。
【００４２】
相対距離ｒｃと車体軸の方向θｃは式（２４）（２５）により求める。
【００４３】
【数２０】
【００４４】
【数２１】
【００４５】
【数２２】
ここでΔθは好ましくは３０度とする。
【００４６】
測定値残差Δｚは式（２８）により設定する。
【００４７】
【数２３】
ここでψｃはランドマークデータベースから取得したラインランドマークの方向角である。
【００４８】
測定値マトリクスＨは式（２９）により設定する。
【００４９】
【数２４】
【００５０】
【数２５】
最小２乗基準を持つ整数アンビギュイティをレストテスト及び残差テストを用いて確認する。レストテストの閾値レベルは測定値数と信頼レベルの関数として設定する。Ｆ分散の９９％信頼レベルが好ましい。
【００５１】
図２０は本発明の一つの実施の形態による物体の位置及び方向を決定する方法を示している。ステップ２００１では、物体の位置及び方向は次の運動方程式により演算される。
【００５２】
【数２６】
ここで、Ｎ，Ｅ，Ｄは位置の成分、φは方向、Ｖは速度検出器が測定した速度、ｒはヨーレート検出器が測定したヨーレート、ｔは時間、ｂは所定の距離である。次に、ステップ２００２では、二重差分型搬送波位相の浮動小数点アンビギュイティベクトルを拡張カルマンフィルタを用いて推定し、搬送波の整数値バイアスにおけるアンビギュイティは推定した二重差分型搬送波位相の浮動小数点アンビギュイティベクトルに基づきラムダ法を用いて決定する。ステップ２００３では、全地球測位衛星から受信した搬送波位相と擬似距離に基づき、（１）速度検出器の誤差、（２）ヨーレート検出器の誤差、（３）対象物の位置誤差及び方向誤差、（４）搬送波の整数値バイアスを推定する。ステップ２００４では、既知のランドマークと物体との間の相対距離を、物体に搭載した距離測定器が測定した距離及び保存されている既知のランドマークの位置データに基づき推定する。さらに、推定した相対距離に基づき観測誤差を推定する。最後にステップ２００５では、推定した位置誤差及び方向誤差に基づき物体の位置及び方向を更新する。
【００５３】
シミュレーション
本発明の航法システム及び方法の実施の形態の設計評価を数値シミュレーションにより行った。図４は種々の誤差要因を含むシミュレーションモデルを示す。シミュレーションで使用したのは、図５に示すいくつかの高い建物の近傍を通過する水平経路である。ＧＰＳ軌道はＧＰＳ週が１２７８のアルマナックを使ってシミュレートした。衛星のマスク角は１０度とした。
【００５４】
高い建物による遮蔽をシミュレートしてＧＰＳ衛星が一部利用できない状態を第一のケースで考慮する。遮蔽はｔ＝５０秒の時マスク角４５度として表す。図６に観測可能な衛星のスカイプロットを示す。観測された衛星の数は１０機であるが、遮蔽の後は３機の衛星しか観測されない。こうした場合には従来のＲＴＫ−ＧＰＳでは定位置を出力できない。ここで述べる密結合ＧＰＳ／ＤＲ航法システムによれば、衛星の数が４機未満であっても、ＤＤ位相とコードにより測定値更新が可能である。
【００５５】
シミュレーションでのセンサ誤差を式（３１）により求める。
【００５６】
ε＝０．２度、ｅｖ＝５％、ｅｇ＝５％、ｂｇ＝０．１度／秒 （３１）
図７にシミュレーションに用いる車両の速度（ｖｅｌｏｃｉｔｙ）、方向（ｈｅａｄｉｎｇ）、ヨーレート（ｙａｗ ｒａｔｅ）を示す。横軸は時間［秒］（ｔｉｍｅ［ｓ］）である。図７（ａ）で実線はオドメトリ（ｏｄｏｍｅｔｒｙ）による測定値、（ｂ）で実線はＤ／Ｒによる測定値、（ｃ）で実線はジャイロ（ｇｙｒｏ）による測定値、破線はそれぞれ真の値（ｔｒｕｅ）を示す。ＥＫＦが推定するセンサ誤差を図８に示す。図８で（ａ）は方向、（ｂ）は速度、（ｃ）はジャイロ誤差（ｇｙｒｏ ｓ／ｆ ｅｒｒｏｒ）、（ｄ）はジャイロバイアス（ｇｙｒｏ ｂｉａｓ）を示す。推定誤差は真の値に収束している。ＥＫＦによる位置誤差を図９に示す。図９で（ａ）は東方向（ｅａｓｔｉｎｇ）、（ｂ）は北方向（ｎｏｒｔｈｉｎｇ）、（ｃ）は上方向（ｕｐ）への移動による誤差を示している。比較の為に、従来の疎結合（ｌｏｏｓｅｌｙ ｃｏｕｐｌｅｄ）ＲＴＫ−ＧＰＳ／ＤＲ航法システムによる位置誤差を同じ図中に示す。密結合（ｔｉｇｈｔｌｙ ｃｏｕｐｌｅｄ）システムによる位置誤差は疎結合システムよりはるかに小さい。第二のケースでは、衛星完全遮蔽をｔ＝５０秒でシミュレートする。この場合は、ランドマーク更新を効果的に用いている。レーザスキャナの最大距離は３０ｍ、距離精度は４ｃｍ、角分解能は１度であると仮定する。図１０に示すように距離が３０ｍ未満であれば、コーナーランドマークの前でランドマーク更新を行う。実線で示したランドマーク測定値更新、及び破線で示した真のセンサ誤差と共にＥＫＦが推定するセンサ誤差を図１１に示す。推定誤差は真の値に収束している。図１２はランドマーク更新を行った場合と行わない場合の位置誤差を示す。図１２で（ａ）は東方向、（ｂ）は北方向への移動による誤差を示し、それぞれ実線はランドマーク更新を行った場合（ｗｉｔｈ ｌａｎｄｍａｒｋ ｕｐｄａｔｅ）、破線はランドマーク更新を行わない場合（ｗｉｔｈｏｕｔ ｌａｎｄｍａｒｋ ｕｐｄａｔｅ）を示している。ＧＰＳ測定値が得られない場合でも位置誤差は小さい。
【００５７】
野外試験（フィールドテスト）
図１３に示す自律陸上用車両を野外試験に使用した。このＡＧＶ（Ａｕｔｏｎｏｍｏｕｓ Ｇｒｏｕｎｄ Ｖｅｈｉｃｌｅ）は一定の地域を自律巡回する監視車両として開発された。レーザスキャナ、距離精度３５ｍｍで角分解能１度のＳＩＣＫ ＬＭＳ２９１、監視用の全方位ビデオ（Ｏｍｎｉ−Ｄｉｒｅｃｔｉｏｎａｌ Ｖｉｄｅｏ：ＯＤＶ）カメラ、二重周波数ＧＰＳ受信機、アシュテックＺエクストリーム（Ａｓｈｔｅｃｈ Ｚ−Ｘｔｒｅｍｅ）、１度／時の精度のＦＯＧ、及び高分解オドメータを備える。この車両のリアルタイム誘導とＧＰＳ／ＤＲ航法はオンボードのＲＴ（リアルタイム）リナックス（登録商標）コンピュータ上で実行する。ここで分析したデータは２００４年７月６日、鎌倉で収集したものである。藤沢でのＧＥＯＮＥＴ［３］の基地局で観測した二周波数ＧＰＳデータは差分補正に使用する。測量基線長は約５ｋｍである。観測された衛星は図６に示すものとほぼ同じである。ＲＴＫ−ＧＰＳ受信機によるアンビギュイティは時間の４０％のみ整数解に基づく位置となり、時間の３２％については解を持たない。
【００５８】
現存のオンボードリアルタイム実装は疎結合航法に基づいている。密結合航法の解は後処理演算により求めた。図１４と図１５に野外試験での演算位置結果を示す。図１４は水平位置、図１５で（ａ）は北方向、（ｂ）は東方向、（ｃ）は下方向（ｄｏｗｎ）の位置を示している。それぞれの図中で黒実線は整数解（ｆｉｘｅｄ）を示し灰色実線は浮動小数点値（ｆｌｏａｔ）である。図１６で（ａ）は観測された衛星の数（ｎｕｍｂｅｒ ｏｆ ｓａｔｓ）、（ｂ）は整数アンビギュイティの推定有効性（ａｍｂｉｇｕｉｔｙ ｖａｌｉｄｉｔｙ）を示す。有効性が１であるなら、整数アンビギュイティが求められていると推定できる。密結合航法システムの場合は、位置の７０％以上が整数解に基づくものである。ＥＫＦによる演算位置とＲＴＫ−ＧＰＳ受信機の整数解を比較してセンチメートルレベルの精度を確認した。ＲＴＫ−ＧＰＳの整数解が得られない場合は、ＤＤ搬送波位相の残差解析によって位置精度を確認した。
【００５９】
ランドマーク更新は経路の二番目のコーナーで行う。図１７は車両の経路とｔ＝２５４秒とｔ＝２６４．３秒におけるレーザスキャナ距離のプロットを示す。レーザスキャナ距離のプロットは経路方向に合わせるため車両の方向により回転する。この図はレーザスキャナによる距離測定によってコーナーが検出可能であることを示している。
【００６０】
図１８に、コーナーランドマークの測定相対距離及び相対角度、ラインランドマークの測定斜度を含むレーザスキャナによる二つ目のコーナーでの測定データの例を示している。図１８で（ａ）は距離（ｒａｎｇｅ）、（ｂ）は角度（ａｎｇｌｅ）、（ｃ）は傾斜度（ｓｌｏｐｅ）をそれぞれ示す。
【００６１】
図１４に示すＧＰＳ／ＤＲ位置は既に高精度であるので、ＧＰＳ測定値更新無しにランドマーク更新を行う。図１９にランドマーク更新による演算位置結果を示す。比較の為に、ランドマーク更新無しの結果とＧＰＳ／ＤＲ位置を同図に示す。演算位置結果はＧＰＳ／ＤＲ位置とほとんど同じである。ＧＰＳ測定値が得られない場合でも、コーナーランドマーク及びラインランドマーク更新により位置精度は向上する。
【００６２】
搬送波位相ディファレンシャルＧＰＳとＡＧＶ用レーザスキャナにより補強された水平航法システムを設計し、その性能を数値シミュレーション及び野外試験によって確認した。本発明の実施の形態によれば、搬送波位相ディファレンシャルＧＰＳとＬＳ補強を利用した密結合航法システムを提案することで衛星視界が劣悪な場合でもデシメートルレベルの測位精度が得られる。
【図面の簡単な説明】
【００６３】
【図１】陸上用の車両の動的モデル図である。
【図２】本発明の一つの実施の形態によるＧＰＳ／ＤＲ航法システムを示す図である。
【図３】ランドマーク更新を示す図である。
【図４】ＧＰＳのシミュレーションモデルを示す図である。
【図５】シミュレーションに用いた水平航法経路を示す図である。
【図６】２００４年７月６日に行った衛星スカイプロットを示す図である。
【図７】シミュレーション時の車両の速度、方向、ヨーレートを示す図である。
【図８】拡張カルマンフィルタが推定したセンサ誤差を示す図である。
【図９】拡張カルマンフィルタが推定した位置誤差を示す図である。
【図１０】ランドマーク更新によるシミュレーションに用いた水平航法経路を示す図である。
【図１１】ランドマーク更新の際の拡張カルマンフィルタが推定したセンサ誤差を示す図である。
【図１２】ランドマーク更新の際の拡張カルマンフィルタが推定した位置誤差を示す図である。
【図１３】自律陸上車両（ＡＧＶ）を示す図である。
【図１４】野外試験時のカルマンフィルタが推定した水平位置を示す図である。
【図１５】野外試験時のＮＥＤ位置を示す図である。
【図１６】衛星数及び整数アンビギュイティの推定有効性を示す図である。
【図１７】野外試験時の車両経路及びレーザスキャナの測定距離のプロットを示す図である。
【図１８】野外試験時のランドマーク更新のための測定データを示す図である。
【図１９】野外試験時のランドマーク更新の有無による演算位置更新を示す図である。
【図２０】本発明の一つの実施の形態による方法を示す図である。
【符号の説明】
【００６４】
２０１ ＧＰＳ受信機、２０２ ＤＤ演算部、２０３ ラムダアンビギュイティ決定部、２０４ 衛星軌道演算部、２０５ サイクルスリップ検出部、２０６ ランドマークデータベース、２０７ レーザスキャナ、２０８ ＦＯＧ／オドメータ、２０９ 拡張カルマンフィルタ、２１０ ストラップダウン航法部。
【技術分野】
【０００１】
本発明は、おもに慣性及び衛星航法を使用した航法システムに関するものである。例えば、搬送波位相ディファレンシャルＧＰＳとレーザスキャナ補強による自律車両航法システムに適用すると好適である。
【背景技術】
【０００２】
本発明は以下の参考文献リストに列挙した文献に引用及び記載された技術を使用しており、明細書中の記載には参照番号を括弧書きしてその引用を示している。
【非特許文献１】ポール・デ・ヨング、クリスチャン・ティベリウス著『整数アンビギュイティ推定の為のラムダ法：実装面』ＬＧＲシリーズ１２ デルフト工業大学 １９９６年８月
【非特許文献２】ルイ・ヒロカワ、コウイチ・サトウ、ケンジ・ナカクキ著『低価格ＭＥＭＳ ＩＭＵを用いた密結合ＧＰＳ／ＩＮＳ航法の設計と評価』ＧＮＳＳシンポジウム２００３ 日本東京 ２００３年１１月
【非特許文献３】テツロウ・イマキイレ、ユウキ・ハタナカ、ヨウタ・クマキ、アツシ・ヤマギワ著『ＧＥＯＮＥＴ：日本の全国ＧＰＳ網』ＧＩＳ開発８巻３号 ２００４年３月
【非特許文献４】Ｊ．タキグチ、Ｊ．ハラム 『屋外環境下で用いる自律移動システムに関する研究』（パート３：チェーンフォームによるノンホロノミック移動ロボットのローカルパス計画）ＩＥＥＥ国際車両電子会議 中国長春 １９９９年９月
【非特許文献５】Ｋ．オオノ、Ｔ・ツボウチ 『屋外におけるディファレンシャルＧＰＳとオドメトリのデータ融合による建物間の移動ロボットの航法』ＩＥＥＥロボットとオートメーション国際会議 第２巻 １９７８−１９８４頁 ２００３年
【非特許文献６】サチン・モディ 『自律案内車両の為の３つの障害回避法の比較』修士論文 シンシナティ大学 ２００２年
【非特許文献７】ポール・デ・ヨング、クリスチャン・ティベリウス 『ラムダ法による整数アンビギュイティ推定』ＩＡＧシンポジウム「地上、空間、宇宙空間へのＧＰＳ適用の傾向」第２１回ＩＵＧＧ総会 アメリカ合衆国コロラド州ブルダー １９９５年７月
【非特許文献８】ロバートＭ．ロジャーズ 『統合航法システムにおける応用数学』ＡＩＡＡエデュケーション・シリーズ第２編 ２００３年１０月
【非特許文献９】Ｓ．スッカリエ、Ｅ．ネボット 『自律陸用車両適用の為の高整合ＩＭＵ／ＧＰＳ航法ループ』ＩＥＥＥロボットとオートメーション会報 １５巻３号 ５７２−５７８頁 １９９９年６月 上記参考文献リスト記載の各文献の内容を引用によって本明細書に組み入れる。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
車両が多くの障害物をすり抜けて行くことを可能にする屋外航法システムの開発は、ＧＰＳ受信機付きの自律陸上用車両にとって重要な課題である（非特許文献４、非特許文献６）。低価格慣性センサと高性能ＧＰＳ受信機を統合した航法システムは広い範囲の移動体システムに適用できる。ＧＰＳとＩＭＵまたはオドメトリによる航法システムは広く用いられている（非特許文献９、非特許文献５）。しかしながら、閉塞性が高い環境においては、高い建物その他の障害物により性能はかなり低下する。密結合ＧＰＳ／ＩＮＳシステムは疎結合システムに比べ、特に衛星視界が劣悪な場合、ＧＰＳ擬似距離の誤差のより良い検出及びより高精度の測位等いくつかの利点がある。
【課題を解決するための手段】
【０００４】
この発明の一側面によれば、物体の位置及び方向を決定する航法装置において、速度検出器の出力とヨーレート検出器の出力とに基づいて物体の位置及び方向を演算する航法演算部と、全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、（１）速度検出器の誤差と、（２）ヨ−レート検出器の誤差と、（３）物体の位置誤差及び方向誤差と、（４）搬送波の整数値バイアスとを推定する推定部とを備え、航法演算部は、推定部が推定した位置誤差及び方向誤差に基づいて物体の位置及び方向を更新することを特徴とする航法装置を提供する。
【０００５】
また、本発明の一つの実施の形態は上記した航法装置を搭載する車両である。このような車両は陸上用車両に好適である。
【０００６】
さらに、本発明の別の側面によれば、物体の位置及び方向を決定する航法方法において、速度検出器の出力とヨーレート検出器の出力とに基づいて物体の位置及び方向を演算し、全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、（１）速度検出器の誤差と、（２）ヨ−レート検出器の誤差と、（３）物体の位置誤差及び方向誤差と、（４）搬送波の整数値バイアスとを推定し、推定した位置誤差及び方向誤差に基づいて物体の位置及び方向を更新することを特徴とする航法測位方法を提供する。
【０００７】
本発明の別の実施の形態によれば、慣性及び衛星航法を用いて自律車両の位置及び方向を決定する航法システムにおいて、（１）自律車両の速度を検出する速度検出器と、（２）自律車両のヨーレートを検出するヨーレート検出器と、（３）既知のランドマークの位置データを記憶するランドマークデータベースと、（４）自律車両に装着し車両から既知のランドマークまでの距離を測定する距離測定器と、（５）速度検出器が検出した速度とヨーレート検出器が検出したヨーレートとに基づいて自律車両の位置及び方向を演算する航法演算部と、（６）全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、速度検出器の誤差と、ヨ−レート検出器の誤差と、物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、航法演算部は、推定部が推定した位置誤差及び方向誤差に基づいて自律車両の位置及び方向を更新することを特徴とする航法システムを提供する。
【０００８】
さらに、本発明の別の実施の形態は上記した航法システムを搭載する車両である。このような車両は陸上用車両に好適である。
【発明の効果】
【０００９】
本発明により、衛星視界が劣悪な場合などにおいても、ＧＰＳの測位精度が向上する。
【発明を実施するための最良の形態】
【００１０】
実施の形態１．
本発明の航法システムは水平ストラップダウン航法演算及び拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ：ＥＫＦ）から成る。図１は車両の水平運動モデルである。局所水平フレームでのストラップダウン演算は、例えば、低価格の光ファイバージャイロ（Ｆｉｂｅｒ Ｏｐｔｉｃ Ｇｙｒｏ：ＦＯＧ）のようなレートジャイロを用いた位置と方向（ヨー角度）の更新、及びＥＫＦにより補正したオドメータの入力によって行われる。ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ：微小電気機械システム）ジャイロや振動ジャイロを使用しても良い。車両の位置をＧＰＳアンテナの位相中心とし、局所ＮＥＤ（Ｎｏｒｔｈ−Ｅａｓｔ−Ｄｏｗｎ：北、東、下方向）フレームにおける位置ダイナミクスは式（１）（２）により求める。
【００１１】
【数４】
ここで、ｖｘとｖｙは車体軸速度であり式（３）を満たす。
【００１２】
【数５】
Ｖｓとｒｓはそれぞれオドメータが測定した補正速度及びＦＯＧが測定したヨーレートである。ｂｒは後方車輪軸とＧＰＳアンテナ位相中心との間の水平距離である。
【００１３】
補正速度及びヨーレートは式（４）（５）により求める。
【００１４】
【数６】
ここでＶｏｄはオドメータ速度、ｒはヨーレート、ｅｏｄはオドメータ倍率誤差、ｅｇはジャイロ倍率誤差、ｂｇはジャイロバイアス誤差である。方向ダイナミクスは式（６）によってモデル化される。
【００１５】
【数７】
ストラップダウン演算は式（１）〜（６）に基づき行われる。ここで、式（１）〜（６）における速度及びヨーレートは図２に示すようにカルマンフィルタが推定するセンサ誤差信号により補正されない場合も補正される場合もある。
【００１６】
ＥＫＦは位置誤差ｐ＝［ｎｅｄ］Ｔ（北、東、下）、センサ誤差ｓ（方向誤差ε、オドメータ倍率誤差ｅｖ、ジャイロ倍率ｅｇ、ジャイロバイアスｂｇ）ＤＤ（ｄｏｕｂｌｅ ｄｉｆｆｅｒｅｎｃｅｄ：二重差分型）搬送波位相の浮動小数点アンビギュイティベクトルａから成る状態量ｘ＝［ｐｓａ］を推定する。状態量の次数は追従する衛星の数による。１２チャネル二周波数受信機であれば次数は２９となる。
【００１７】
図２はＥＫＦ２０９を備えた本発明の一つの実施の形態による航法システムのブロック図である。ＦＯＧ／オドメータ２０８からヨーレートと速度をストラップダウン航法部２１０へ入力する。レーザスキャナ２０７からＥＫＦ２０９へ相対位置情報を入力する。以下に述べるようにランドマークデータベース２０６は少なくともひとつの既知のランドマークの座標データを提供する。ＧＰＳ受信機２０１、ＤＤ演算部２０２、衛星軌道演算部２０４はＥＫＦ２０９へ衛星航法情報を入力する。以下に詳述するように、ラムダ（ＬＡＭＢＤＡ）アンビギュイティ決定部２０３とサイクルスリップ検出部２０５は搬送波の整数値バイアスにおけるアンビギュイティを決定する。
【００１８】
カルマンフィルタ２０９はストラップダウン航法部２１０における誤差とセンサ入力の誤差を補正できるようにフィードバック形式となっている。ステートｘのダイナミクスは線形差分方程式（７）のように表せる。
【００１９】
【数８】
ここでＦはシステムマトリクス、ｗはプロセスノイズベクトルである。
【００２０】
システムマトリクスＦはシステム誤差ダイナミクスから導かれ、以下の式（８）（９）（１０）のように表せる。
【００２１】
【数９】
ここでτｇはジャイロバイアスの相関時間である。
【００２２】
高度、浮動小数点アンビギュイティ、センサ入力といったその他の状態量の誤差ダイナミクスはランダムウォークプロセスモデルとして定式化される。
【００２３】
【数１０】
カルマンフィルタのダイナミクスは離散形で表される。システムФの遷移マトリクスは式（１２）のように演算する。
【００２４】
【数１１】
ここで、τはサンプル時間である。
【００２５】
カルマンフィルタ演算は時間伝播と測定値更新から成る。共分散マトリクスＰの時間伝播は式（１３）により求める。
【００２６】
【数１２】
ここでＱｄはプロセスノイズマトリクスである。
【００２７】
ＥＫＦの測定値更新は以下の式（１４）（１５）（１６）で表せる。
【００２８】
【数１３】
ここで、Ｈは測位マトリクス、Ｋはカルマンゲインマトリクス、Ｒは測位ノイズマトリクス、Δｚは測定値残差である。
【００２９】
位置、方向角、センサ誤差、及び浮動小数点アンビギュイティは式（１７）により更新する。
【００３０】
【数１４】
ここでｍは観測衛星数−１である。
【００３１】
２つの異なる測定値、ＧＰＳによるＤＤ距離、レーザスキャナによる相対距離を用いる。測定値マトリクスＨと測定値剰余Δｚは下記のように設定する。二周波ＤＤ搬送波位相とコード位相はフィルタの測定値として用いる。
【００３２】
整数アンビギュイティを適切に求めた後、ＥＫＦの次数を７まで下げ、アンビギュイティを決定した二周波二重差分型（ＤＤ）搬送波位相のみを測定値として用いる。
【００３３】
ＥＫＦの測定値マトリクスＨは式（１８）のように設定する。
【００３４】
【数１５】
ここでＣｎｅは局所水平フレームとＥＣＥＦ（Ｅａｒｔｈ−Ｃｅｎｔｅｒｅｄ Ｅａｒｔｈ−Ｆｉｘｅｄ）フレーム間の方向余弦マトリクス、Ａは各衛星の幾何学的射影マトリクス、λ１、λ２はそれぞれＧＰＳのＬ１，Ｌ２の波長を表す。
【００３５】
ＧＰＳ測定値更新の測定値マトリクスＲはＤＤ観測に基づいているため相関性が高い。測定値残差Δｚは式（１９）により求める。
【００３６】
【数１６】
ここでΔρｃはＤＤ幾何距離、ΔρＣ１、ΔρＬ１、ΔρＬ２は観測値に基づくＤＤコードと搬送波位相、ａ１とａ２はＬ１とＬ２のＤＤアンビギュイティである。
【００３７】
【数１７】
レーザスキャナが測定した相対位置もまた位置誤差および方向誤差を補正するための追加測定値として用いる。コーナーランドマークはランドマークデータベース中に記憶された既知の位置を持ち、このコーナーランドマークを用いて、ランドマークとレーザスキャナセンサ間の相対位置を測定することにより誤差を推定する。
【００３８】
ラインランドマークは二つの連続するコーナーランドマークを結ぶ線によって成る。ラインランドマークとスキャナ間の相対距離も測定可能である。ラインランドマークの相対位置は測定できないが、相対角度は測定可能である。ラインランドマークを伴うコーナーランドマーク更新は演算効率のために用いる。図３はランドマーク更新の定義を表している。
【００３９】
コーナーランドマークとレーザスキャナ間の距離ｒａはレーザスキャナによって測定する。コーナーランドマークの位置は測定距離の不連続性によって検出可能である。ボディフレームの方向θａもセンサによって測定する。レーザスキャナとコーナーランドマーク間の距離ベクトルは車体軸と航法軸では式（２１）（２２）のように設定する。
【００４０】
【数１８】
センサの位置は車両の位置と方向により式（２３）のように演算する。
【００４１】
【数１９】
ここでｂｓはＧＰＳアンテナとレーザスキャナとの間の距離である。
【００４２】
相対距離ｒｃと車体軸の方向θｃは式（２４）（２５）により求める。
【００４３】
【数２０】
【００４４】
【数２１】
【００４５】
【数２２】
ここでΔθは好ましくは３０度とする。
【００４６】
測定値残差Δｚは式（２８）により設定する。
【００４７】
【数２３】
ここでψｃはランドマークデータベースから取得したラインランドマークの方向角である。
【００４８】
測定値マトリクスＨは式（２９）により設定する。
【００４９】
【数２４】
【００５０】
【数２５】
最小２乗基準を持つ整数アンビギュイティをレストテスト及び残差テストを用いて確認する。レストテストの閾値レベルは測定値数と信頼レベルの関数として設定する。Ｆ分散の９９％信頼レベルが好ましい。
【００５１】
図２０は本発明の一つの実施の形態による物体の位置及び方向を決定する方法を示している。ステップ２００１では、物体の位置及び方向は次の運動方程式により演算される。
【００５２】
【数２６】
ここで、Ｎ，Ｅ，Ｄは位置の成分、φは方向、Ｖは速度検出器が測定した速度、ｒはヨーレート検出器が測定したヨーレート、ｔは時間、ｂは所定の距離である。次に、ステップ２００２では、二重差分型搬送波位相の浮動小数点アンビギュイティベクトルを拡張カルマンフィルタを用いて推定し、搬送波の整数値バイアスにおけるアンビギュイティは推定した二重差分型搬送波位相の浮動小数点アンビギュイティベクトルに基づきラムダ法を用いて決定する。ステップ２００３では、全地球測位衛星から受信した搬送波位相と擬似距離に基づき、（１）速度検出器の誤差、（２）ヨーレート検出器の誤差、（３）対象物の位置誤差及び方向誤差、（４）搬送波の整数値バイアスを推定する。ステップ２００４では、既知のランドマークと物体との間の相対距離を、物体に搭載した距離測定器が測定した距離及び保存されている既知のランドマークの位置データに基づき推定する。さらに、推定した相対距離に基づき観測誤差を推定する。最後にステップ２００５では、推定した位置誤差及び方向誤差に基づき物体の位置及び方向を更新する。
【００５３】
シミュレーション
本発明の航法システム及び方法の実施の形態の設計評価を数値シミュレーションにより行った。図４は種々の誤差要因を含むシミュレーションモデルを示す。シミュレーションで使用したのは、図５に示すいくつかの高い建物の近傍を通過する水平経路である。ＧＰＳ軌道はＧＰＳ週が１２７８のアルマナックを使ってシミュレートした。衛星のマスク角は１０度とした。
【００５４】
高い建物による遮蔽をシミュレートしてＧＰＳ衛星が一部利用できない状態を第一のケースで考慮する。遮蔽はｔ＝５０秒の時マスク角４５度として表す。図６に観測可能な衛星のスカイプロットを示す。観測された衛星の数は１０機であるが、遮蔽の後は３機の衛星しか観測されない。こうした場合には従来のＲＴＫ−ＧＰＳでは定位置を出力できない。ここで述べる密結合ＧＰＳ／ＤＲ航法システムによれば、衛星の数が４機未満であっても、ＤＤ位相とコードにより測定値更新が可能である。
【００５５】
シミュレーションでのセンサ誤差を式（３１）により求める。
【００５６】
ε＝０．２度、ｅｖ＝５％、ｅｇ＝５％、ｂｇ＝０．１度／秒 （３１）
図７にシミュレーションに用いる車両の速度（ｖｅｌｏｃｉｔｙ）、方向（ｈｅａｄｉｎｇ）、ヨーレート（ｙａｗ ｒａｔｅ）を示す。横軸は時間［秒］（ｔｉｍｅ［ｓ］）である。図７（ａ）で実線はオドメトリ（ｏｄｏｍｅｔｒｙ）による測定値、（ｂ）で実線はＤ／Ｒによる測定値、（ｃ）で実線はジャイロ（ｇｙｒｏ）による測定値、破線はそれぞれ真の値（ｔｒｕｅ）を示す。ＥＫＦが推定するセンサ誤差を図８に示す。図８で（ａ）は方向、（ｂ）は速度、（ｃ）はジャイロ誤差（ｇｙｒｏ ｓ／ｆ ｅｒｒｏｒ）、（ｄ）はジャイロバイアス（ｇｙｒｏ ｂｉａｓ）を示す。推定誤差は真の値に収束している。ＥＫＦによる位置誤差を図９に示す。図９で（ａ）は東方向（ｅａｓｔｉｎｇ）、（ｂ）は北方向（ｎｏｒｔｈｉｎｇ）、（ｃ）は上方向（ｕｐ）への移動による誤差を示している。比較の為に、従来の疎結合（ｌｏｏｓｅｌｙ ｃｏｕｐｌｅｄ）ＲＴＫ−ＧＰＳ／ＤＲ航法システムによる位置誤差を同じ図中に示す。密結合（ｔｉｇｈｔｌｙ ｃｏｕｐｌｅｄ）システムによる位置誤差は疎結合システムよりはるかに小さい。第二のケースでは、衛星完全遮蔽をｔ＝５０秒でシミュレートする。この場合は、ランドマーク更新を効果的に用いている。レーザスキャナの最大距離は３０ｍ、距離精度は４ｃｍ、角分解能は１度であると仮定する。図１０に示すように距離が３０ｍ未満であれば、コーナーランドマークの前でランドマーク更新を行う。実線で示したランドマーク測定値更新、及び破線で示した真のセンサ誤差と共にＥＫＦが推定するセンサ誤差を図１１に示す。推定誤差は真の値に収束している。図１２はランドマーク更新を行った場合と行わない場合の位置誤差を示す。図１２で（ａ）は東方向、（ｂ）は北方向への移動による誤差を示し、それぞれ実線はランドマーク更新を行った場合（ｗｉｔｈ ｌａｎｄｍａｒｋ ｕｐｄａｔｅ）、破線はランドマーク更新を行わない場合（ｗｉｔｈｏｕｔ ｌａｎｄｍａｒｋ ｕｐｄａｔｅ）を示している。ＧＰＳ測定値が得られない場合でも位置誤差は小さい。
【００５７】
野外試験（フィールドテスト）
図１３に示す自律陸上用車両を野外試験に使用した。このＡＧＶ（Ａｕｔｏｎｏｍｏｕｓ Ｇｒｏｕｎｄ Ｖｅｈｉｃｌｅ）は一定の地域を自律巡回する監視車両として開発された。レーザスキャナ、距離精度３５ｍｍで角分解能１度のＳＩＣＫ ＬＭＳ２９１、監視用の全方位ビデオ（Ｏｍｎｉ−Ｄｉｒｅｃｔｉｏｎａｌ Ｖｉｄｅｏ：ＯＤＶ）カメラ、二重周波数ＧＰＳ受信機、アシュテックＺエクストリーム（Ａｓｈｔｅｃｈ Ｚ−Ｘｔｒｅｍｅ）、１度／時の精度のＦＯＧ、及び高分解オドメータを備える。この車両のリアルタイム誘導とＧＰＳ／ＤＲ航法はオンボードのＲＴ（リアルタイム）リナックス（登録商標）コンピュータ上で実行する。ここで分析したデータは２００４年７月６日、鎌倉で収集したものである。藤沢でのＧＥＯＮＥＴ［３］の基地局で観測した二周波数ＧＰＳデータは差分補正に使用する。測量基線長は約５ｋｍである。観測された衛星は図６に示すものとほぼ同じである。ＲＴＫ−ＧＰＳ受信機によるアンビギュイティは時間の４０％のみ整数解に基づく位置となり、時間の３２％については解を持たない。
【００５８】
現存のオンボードリアルタイム実装は疎結合航法に基づいている。密結合航法の解は後処理演算により求めた。図１４と図１５に野外試験での演算位置結果を示す。図１４は水平位置、図１５で（ａ）は北方向、（ｂ）は東方向、（ｃ）は下方向（ｄｏｗｎ）の位置を示している。それぞれの図中で黒実線は整数解（ｆｉｘｅｄ）を示し灰色実線は浮動小数点値（ｆｌｏａｔ）である。図１６で（ａ）は観測された衛星の数（ｎｕｍｂｅｒ ｏｆ ｓａｔｓ）、（ｂ）は整数アンビギュイティの推定有効性（ａｍｂｉｇｕｉｔｙ ｖａｌｉｄｉｔｙ）を示す。有効性が１であるなら、整数アンビギュイティが求められていると推定できる。密結合航法システムの場合は、位置の７０％以上が整数解に基づくものである。ＥＫＦによる演算位置とＲＴＫ−ＧＰＳ受信機の整数解を比較してセンチメートルレベルの精度を確認した。ＲＴＫ−ＧＰＳの整数解が得られない場合は、ＤＤ搬送波位相の残差解析によって位置精度を確認した。
【００５９】
ランドマーク更新は経路の二番目のコーナーで行う。図１７は車両の経路とｔ＝２５４秒とｔ＝２６４．３秒におけるレーザスキャナ距離のプロットを示す。レーザスキャナ距離のプロットは経路方向に合わせるため車両の方向により回転する。この図はレーザスキャナによる距離測定によってコーナーが検出可能であることを示している。
【００６０】
図１８に、コーナーランドマークの測定相対距離及び相対角度、ラインランドマークの測定斜度を含むレーザスキャナによる二つ目のコーナーでの測定データの例を示している。図１８で（ａ）は距離（ｒａｎｇｅ）、（ｂ）は角度（ａｎｇｌｅ）、（ｃ）は傾斜度（ｓｌｏｐｅ）をそれぞれ示す。
【００６１】
図１４に示すＧＰＳ／ＤＲ位置は既に高精度であるので、ＧＰＳ測定値更新無しにランドマーク更新を行う。図１９にランドマーク更新による演算位置結果を示す。比較の為に、ランドマーク更新無しの結果とＧＰＳ／ＤＲ位置を同図に示す。演算位置結果はＧＰＳ／ＤＲ位置とほとんど同じである。ＧＰＳ測定値が得られない場合でも、コーナーランドマーク及びラインランドマーク更新により位置精度は向上する。
【００６２】
搬送波位相ディファレンシャルＧＰＳとＡＧＶ用レーザスキャナにより補強された水平航法システムを設計し、その性能を数値シミュレーション及び野外試験によって確認した。本発明の実施の形態によれば、搬送波位相ディファレンシャルＧＰＳとＬＳ補強を利用した密結合航法システムを提案することで衛星視界が劣悪な場合でもデシメートルレベルの測位精度が得られる。
【図面の簡単な説明】
【００６３】
【図１】陸上用の車両の動的モデル図である。
【図２】本発明の一つの実施の形態によるＧＰＳ／ＤＲ航法システムを示す図である。
【図３】ランドマーク更新を示す図である。
【図４】ＧＰＳのシミュレーションモデルを示す図である。
【図５】シミュレーションに用いた水平航法経路を示す図である。
【図６】２００４年７月６日に行った衛星スカイプロットを示す図である。
【図７】シミュレーション時の車両の速度、方向、ヨーレートを示す図である。
【図８】拡張カルマンフィルタが推定したセンサ誤差を示す図である。
【図９】拡張カルマンフィルタが推定した位置誤差を示す図である。
【図１０】ランドマーク更新によるシミュレーションに用いた水平航法経路を示す図である。
【図１１】ランドマーク更新の際の拡張カルマンフィルタが推定したセンサ誤差を示す図である。
【図１２】ランドマーク更新の際の拡張カルマンフィルタが推定した位置誤差を示す図である。
【図１３】自律陸上車両（ＡＧＶ）を示す図である。
【図１４】野外試験時のカルマンフィルタが推定した水平位置を示す図である。
【図１５】野外試験時のＮＥＤ位置を示す図である。
【図１６】衛星数及び整数アンビギュイティの推定有効性を示す図である。
【図１７】野外試験時の車両経路及びレーザスキャナの測定距離のプロットを示す図である。
【図１８】野外試験時のランドマーク更新のための測定データを示す図である。
【図１９】野外試験時のランドマーク更新の有無による演算位置更新を示す図である。
【図２０】本発明の一つの実施の形態による方法を示す図である。
【符号の説明】
【００６４】
２０１ ＧＰＳ受信機、２０２ ＤＤ演算部、２０３ ラムダアンビギュイティ決定部、２０４ 衛星軌道演算部、２０５ サイクルスリップ検出部、２０６ ランドマークデータベース、２０７ レーザスキャナ、２０８ ＦＯＧ／オドメータ、２０９ 拡張カルマンフィルタ、２１０ ストラップダウン航法部。
【特許請求の範囲】
【請求項１】
物体の位置及び方向を判断する航法装置において、
速度検出器の出力とヨーレート検出器の出力とに基づいて前記物体の位置及び方向を演算する航法演算部と、
全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、
前記航法演算部は、前記推定部が推定した位置誤差及び方向誤差に基づいて前記物体の位置及び方向を更新することを特徴とする航法装置。
【請求項２】
前記推定部は、既知のランドマークと前記物体との間の相対距離を前記物体に設置された距離測定器が測定した距離と記憶されている前記既知のランドマークの位置データとに基づいて推定し、推定した相対距離に基づいて前記各誤差を演算することを特徴とする請求項１に記載の航法装置。
【請求項３】
前記推定部は、少なくとも２つの既知のランドマークの幾何データと前記距離測定器の既知のヨー角度とに基づいて前記相対距離を推定することを特徴とする請求項２に記載の航法装置。
【請求項４】
前記航法演算部は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
【数１】
により前記物体の位置及び方向を演算することを特徴とする請求項１に記載の航法装置。
【請求項５】
前記推定部は、
二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定する拡張カルマンフィルタと、
前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記搬送波の整数値バイアスのアンビギュイティを決定するアンビギュイティ決定部とを備えることを特徴とする請求項１に記載の航法装置。
【請求項６】
前記航法演算部は、オドメータの出力とレートジャイロの出力とに基づいて前記物体の位置及び方向を演算することを特徴とする請求項１に記載の航法装置。
【請求項７】
請求項１に記載の航法装置を搭載する車両。
【請求項８】
物体の位置及び方向を得る航法測位方法において、
速度検出器の出力とヨーレート検出器の出力とに基づいて前記物体の位置及び方向を演算する演算工程と、
全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定工程と、
推定した位置誤差及び方向誤差に基づいて前記物体の位置及び方向を更新する更新工程とを備えることを特徴とする航法測位方法。
【請求項９】
前記推定工程は、既知のランドマークと前記物体との間の相対距離を前記物体に設置された距離測定器が測定した距離と記憶されている前記既知のランドマークの位置データとに基づいて推定し、推定した相対距離に基づいて観測誤差を演算することを特徴とする請求項８に記載の航法測位方法。
【請求項１０】
前記推定工程は、少なくとも２つの既知のランドマークの幾何データと前記距離測定器の既知のヨー角度とに基づいて前記相対距離を推定することを特徴とする請求項９に記載の航法測位方法。
【請求項１１】
前記推定工程は、レーザスキャナが測定した距離に基づいて前記相対距離を推定することを特徴とする請求項９に記載の航法測位方法。
【請求項１２】
前記演算工程は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
【数２】
により前記物体の位置及び方向を演算することを特徴とする請求項８に記載の航法測位方法。
【請求項１３】
前記推定工程は、
拡張カルマンフィルタを用いて二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定し、
前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記搬送波の整数値バイアスのアンビギュイティを決定することを特徴とする請求項８に記載の航法測位方法。
【請求項１４】
前記演算工程は、オドメータの出力とレートジャイロの出力とに基づいて前記物体の位置及び方向を演算することを特徴とする請求項８に記載の航法測位方法。
【請求項１５】
慣性及び衛星航法を用いて自律車両の位置及び方向を判断する航法システムにおいて、
前記自律車両の速度を検出する速度検出器と、
前記自律車両のヨーレートを検出するヨーレート検出器と、
既知のランドマークの位置データを記憶するランドマークデータベースと、
前記自律車両に装着され前記自律車両から既知のランドマークまでの距離を測定する距離測定器と、
速度検出器が検出した速度とヨーレート検出器が検出したヨーレートとに基づいて前記自律車両の位置及び方向を演算する航法演算部と、
全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記自律車両の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、
前記航法演算部は、前記推定部が推定した位置誤差及び方向誤差に基づいて前記自律車両の位置及び方向を更新することを特徴とする航法システム。
【請求項１６】
前記推定部は、既知のランドマークと前記自律車両との間の相対距離を前記自律車両に設置された距離測定器が測定した距離と記憶されている前記既知のランドマークの位置データとに基づいて推定し、推定した相対距離に基づいて観測誤差を演算することを特徴とする請求項１５に記載の航法システム。
【請求項１７】
前記推定部は、少なくとも２つの既知のランドマークの幾何データと前記距離測定器の既知のヨー角度とに基づいて前記相対距離を推定することを特徴とする請求項１５に記載の航法システム。
【請求項１８】
前記航法演算部は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
【数３】
により前記自律車両の位置及び方向を演算することを特徴とする請求項１５に記載の航法システム。
【請求項１９】
前記推定部は、
二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定する拡張カルマンフィルタと、
前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記搬送波の整数値バイアスのアンビギュイティを決定するアンビギュイティ決定部とを備えることを特徴とする請求項１５に記載の航法システム。
【請求項２０】
請求項１５に記載の航法システムを装備した車両。
【請求項１】
物体の位置及び方向を判断する航法装置において、
速度検出器の出力とヨーレート検出器の出力とに基づいて前記物体の位置及び方向を演算する航法演算部と、
全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、
前記航法演算部は、前記推定部が推定した位置誤差及び方向誤差に基づいて前記物体の位置及び方向を更新することを特徴とする航法装置。
【請求項２】
前記推定部は、既知のランドマークと前記物体との間の相対距離を前記物体に設置された距離測定器が測定した距離と記憶されている前記既知のランドマークの位置データとに基づいて推定し、推定した相対距離に基づいて前記各誤差を演算することを特徴とする請求項１に記載の航法装置。
【請求項３】
前記推定部は、少なくとも２つの既知のランドマークの幾何データと前記距離測定器の既知のヨー角度とに基づいて前記相対距離を推定することを特徴とする請求項２に記載の航法装置。
【請求項４】
前記航法演算部は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
【数１】
により前記物体の位置及び方向を演算することを特徴とする請求項１に記載の航法装置。
【請求項５】
前記推定部は、
二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定する拡張カルマンフィルタと、
前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記搬送波の整数値バイアスのアンビギュイティを決定するアンビギュイティ決定部とを備えることを特徴とする請求項１に記載の航法装置。
【請求項６】
前記航法演算部は、オドメータの出力とレートジャイロの出力とに基づいて前記物体の位置及び方向を演算することを特徴とする請求項１に記載の航法装置。
【請求項７】
請求項１に記載の航法装置を搭載する車両。
【請求項８】
物体の位置及び方向を得る航法測位方法において、
速度検出器の出力とヨーレート検出器の出力とに基づいて前記物体の位置及び方向を演算する演算工程と、
全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記物体の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定工程と、
推定した位置誤差及び方向誤差に基づいて前記物体の位置及び方向を更新する更新工程とを備えることを特徴とする航法測位方法。
【請求項９】
前記推定工程は、既知のランドマークと前記物体との間の相対距離を前記物体に設置された距離測定器が測定した距離と記憶されている前記既知のランドマークの位置データとに基づいて推定し、推定した相対距離に基づいて観測誤差を演算することを特徴とする請求項８に記載の航法測位方法。
【請求項１０】
前記推定工程は、少なくとも２つの既知のランドマークの幾何データと前記距離測定器の既知のヨー角度とに基づいて前記相対距離を推定することを特徴とする請求項９に記載の航法測位方法。
【請求項１１】
前記推定工程は、レーザスキャナが測定した距離に基づいて前記相対距離を推定することを特徴とする請求項９に記載の航法測位方法。
【請求項１２】
前記演算工程は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
【数２】
により前記物体の位置及び方向を演算することを特徴とする請求項８に記載の航法測位方法。
【請求項１３】
前記推定工程は、
拡張カルマンフィルタを用いて二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定し、
前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記搬送波の整数値バイアスのアンビギュイティを決定することを特徴とする請求項８に記載の航法測位方法。
【請求項１４】
前記演算工程は、オドメータの出力とレートジャイロの出力とに基づいて前記物体の位置及び方向を演算することを特徴とする請求項８に記載の航法測位方法。
【請求項１５】
慣性及び衛星航法を用いて自律車両の位置及び方向を判断する航法システムにおいて、
前記自律車両の速度を検出する速度検出器と、
前記自律車両のヨーレートを検出するヨーレート検出器と、
既知のランドマークの位置データを記憶するランドマークデータベースと、
前記自律車両に装着され前記自律車両から既知のランドマークまでの距離を測定する距離測定器と、
速度検出器が検出した速度とヨーレート検出器が検出したヨーレートとに基づいて前記自律車両の位置及び方向を演算する航法演算部と、
全地球測位衛星から受信した搬送波位相と擬似距離とに基づいて、前記速度検出器の誤差と、前記ヨーレート検出器の誤差と、前記自律車両の位置誤差及び方向誤差と、搬送波の整数値バイアスとを推定する推定部とを備え、
前記航法演算部は、前記推定部が推定した位置誤差及び方向誤差に基づいて前記自律車両の位置及び方向を更新することを特徴とする航法システム。
【請求項１６】
前記推定部は、既知のランドマークと前記自律車両との間の相対距離を前記自律車両に設置された距離測定器が測定した距離と記憶されている前記既知のランドマークの位置データとに基づいて推定し、推定した相対距離に基づいて観測誤差を演算することを特徴とする請求項１５に記載の航法システム。
【請求項１７】
前記推定部は、少なくとも２つの既知のランドマークの幾何データと前記距離測定器の既知のヨー角度とに基づいて前記相対距離を推定することを特徴とする請求項１５に記載の航法システム。
【請求項１８】
前記航法演算部は、ＮとＥとＤとを位置の成分、φを方向、Ｖを前記速度検出器が測定した速度、ｒを前記ヨーレート検出器が測定したヨーレート、ｔを時間、ｂを所定の距離としたとき、運動方程式、
【数３】
により前記自律車両の位置及び方向を演算することを特徴とする請求項１５に記載の航法システム。
【請求項１９】
前記推定部は、
二重差分搬送波位相の浮動小数点アンビギュイティベクトルを推定する拡張カルマンフィルタと、
前記拡張カルマンフィルタが推定した二重差分搬送波位相の浮動小数点アンビギュイティベクトルに基づいて、ラムダ法により、前記搬送波の整数値バイアスのアンビギュイティを決定するアンビギュイティ決定部とを備えることを特徴とする請求項１５に記載の航法システム。
【請求項２０】
請求項１５に記載の航法システムを装備した車両。
【図１】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２】
【図１３】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２】
【図１３】
【公開番号】特開２００６−１３８８３４（Ｐ２００６−１３８８３４Ａ）
【公開日】平成１８年６月１日（２００６．６．１）
【国際特許分類】
【出願番号】特願２００５−９８０６８（Ｐ２００５−９８０６８）
【出願日】平成１７年３月３０日（２００５．３．３０）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り ２００４年１１月１日 アドバンスター・コミュニケーションズ発行の「ＧＰＳワールド２００４年１１月号」に発表
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成１８年６月１日（２００６．６．１）
【国際特許分類】
【出願日】平成１７年３月３０日（２００５．３．３０）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り ２００４年１１月１日 アドバンスター・コミュニケーションズ発行の「ＧＰＳワールド２００４年１１月号」に発表
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]