JP6877854B2 - 測位装置および測位方法 - Google Patents

Description

本発明は、ＧＮＳＳ（Global Navigation Satellite System ）受信機などを用いて、レーンレベル精度の測位を行う測位装置（ロケータ装置）および測位方法に関するものである。

一般的なカーナビゲーション装置は、ＧＮＳＳ受信機と自律センサとを用いて当該カーナビゲーション装置を搭載した車両（以下「自車」という）の運動の観測および計測を行って、自車の位置を推定する。また、カーナビゲーション装置は、地図データを用いたマップマッチング処理により道路上の自車位置を特定し、自車位置を示す地図の表示や経路案内なども行う。従来のカーナビゲーション装置では、各道路が一本のリンクとして表現された地図データが用いられているため、ＧＮＳＳ受信機に求められる測位精度は走行道路を特定できる程度（オープンスカイで数ｍの測位精度）であった。そのため、車載用のＧＮＳＳ受信機には、安価なものが採用されていた。

しかし近年では、カーナビゲーション装置の性能向上、地図情報の高精細化、自動運転技術の開発などに伴い、レーンレベルで自車位置を推定する技術が求められている。レーンレベルで自車位置を推定する場合は、ＧＮＳＳ測位の高精度化が必要となり、特にマルチパス（ＧＮＳＳ衛星からの電波が道路周辺の建物などに反射してからＧＮＳＳアンテナで受信される現象）が生じる環境では、精度低下を確実に防止する自車位置推定技術が必要となる。そのような自車位置推定技術を実現する上で、以下の技術が必要となる。なお、以下の説明では、ＧＮＳＳ衛星を単に「衛星」ということもあり、ＧＮＳＳ衛星が発する電波を「衛星電波」ということもある。

車載用の安価なＧＮＳＳ受信機は、ＧＮＳＳ衛星が発した電波の伝搬時間からＧＮＳＳ衛星と自車との２点間距離を求め、三角測量の原理で自車位置および受信機時計誤差を求めるコード測位を採用する（電波伝搬時間から求めたＧＮＳＳ衛星と自車との２点間距離は「擬似距離」と呼ばれる）。しかし、マルチパス環境下では、ＧＮＳＳ受信機が電波伝搬時間を正しく観測できないため、測位誤差が生じる（マルチパスに起因する測位誤差を「マルチパス誤差」という）。この問題は、マルチパスが生じにくい高仰角衛星の使用機会を増やすことで問題を小さくできるため、複数の測位衛星システム（例えばＧＰＳ（Global Positioning System），ＱＺＳＳ（Quasi-Zenith Satellite System），ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System），ＢｅｉＤｏｕ（BeiDou Navigation Satellite System），Ｇａｌｉｌｅｏなど）に対応したマルチＧＮＳＳ受信機が車載用として利用されるようになっている。

しかし、マルチＧＮＳＳ受信機を用いても、マルチパスの問題は完全には解消されない。そのため、マルチＧＮＳＳ受信機とは別に、ＳｏＣ（System-on-a-chip）側で各種のＧＮＳＳ衛星から取得したＲａｗデータからマルチパス誤差を推定して測位計算を行い、測位精度の低下を抑制する必要がある。ＳｏＣで測位計算を行う場合は、各ＧＮＳＳのＲａｗデータの仕様および各ＧＮＳＳの時系の差異への対応が必要になる。またＧＰＳのみのＲａｗデータをＳｏＣで扱う場合は、測位に使用する衛星（測位使用衛星）の数がマルチＧＮＳＳ受信機よりも少なくなって不利になるので、その不利の補い方が課題となる。

一方、下記の特許文献１には、マルチパス環境での測位精度の低下を抑止する技術が提案されている。特許文献１の技術では、エポック（epoch）（ＧＰＳ受信機の測位周期）毎のＧＰＳ測位精度にバラツキがあっても自車位置を精度よく求めるために、ドップラー周波数を用いて算出した速度ベクトルを予め定められた時間積算して、自車の軌跡が算出される。そして、自車位置の軌跡を構成する複数の地点について、軌跡上の自車位置とＧＮＳＳ衛星の位置との間の幾何学的２点間距離と、擬似距離との差が最小となるように自車位置の軌跡を平行移動して自車位置が算出される。この技術によると、速度ベクトルを積算した軌跡から自車の方位（進行方向）を求めることができるので、方位に関するパラメータが不要になり、平易に自車位置を計算することができる。また、速度ベクトルを用いることで、測位使用衛星数が不足する場合でも、軌跡を連続的に計算できるので、誤差数ｍの精度で自車位置を推定できる。

また、下記の特許文献２には、カルマンフィルタ法（ＫＦ法：Kalman filter method）を用いてＧＰＳ測位のバラツキを平滑化するＧＰＳ測位に関して、トンネル通過直後の測位精度が低くても、その後に測位精度が低下した状態が継続することを回避するための技術が提案されている。特許文献２の技術では、現エポックで自車位置が存在し得る第一の範囲が設定されるとともに、第一の範囲に基づいて、トンネル通過後に測位を再開した過去エポックに自車位置が存在し得る第二の範囲が設定される。第一の範囲は、ＧＰＳ衛星の配置によって定まる精度低下率（ＤＯＰ：Dilution of Precision）から求められる誤差の共分散行列および擬似距離誤差に基づいて計算される。第二の範囲は、現エポックでの自車の方位と速度に基づいて、第一の範囲をシフトさせて求めることもある。トンネル通過後に測位を再開した過去エポックで測位した自車位置が第二の範囲に含まれなければ、次エポックでのカルマンフィルタ計算では、当該過去エポックの自車位置を使用しないことで、測位精度が低下した状態が継続することが防止される。

特許５６７３０７１号公報 特開２００９−２３６５１７号公報

（１）マルチパス環境での測位精度低下の抑止
本発明が解決する課題の一つは、マルチパス環境での精度低下を抑止し、レーンレベルの精度を確保することになる。特定の衛星電波にマルチパスが発生した場合、ＳｏＣがマルチパス誤差による測位精度の低下を正しく認識できないと、自車位置が誤修正される問題が生じる。この問題を解決するため、ＳｏＣがＧＮＳＳ受信機のＲａｗデータから自車位置の誤差を求める複合測位を行う場合がある。測位使用衛星にマルチパスが生じた衛星が１つでも含まれていると、複合測位した自車位置に誤差が生じるので、マルチパスが生じた衛星を測位使用衛星から除外するか、擬似距離のマルチパス誤差を正しく求めて補正する必要がある。

特許文献１の技術では、測位計算に最小二乗法（ＬＳ法：Least Squares method）が用いられているが、最小二乗法は誤差がランダム性を有することを前提としているため、擬似距離などに大きな誤差や偏った誤差が生じる地点を除外する必要がある。マルチパス誤差を含んだ擬似距離を測位計算に用いるにしても、ランダム性を確保できるように自車位置の軌跡の長さを決める必要がある。しかし、マルチパス発生は場所や時刻によって変化し、局所的に偏った誤差が発生し続けることもあるので、軌跡の長さを決めるのは難しい。精度が低下した自車位置の割合が増える高層ビル街などでは、誤差のランダム性を確保できなくなり、自車位置の精度がレーンレベルまで上がらない。

特許文献２の技術は、自車がトンネルから出て測位が再開された直後の区間にしか対応しておらず、マルチパスの発生区間において、カルマンフィルタの推定値に大きな誤差が残留した場合には対応していない。また、自車上空の測位使用衛星の配置（ＤＯＰ）が良くても、マルチパス影響はないとは言えない。さらに、過去エポックの測位精度が良くて現エポックの測位精度が悪い場合、ならびに、トンネル出口から現エポックまで測位精度が低い状態が続いた場合には、精度の悪い現エポックのＧＰＳ位置でカルマンフィルタの内部データが誤修正される懸念があり、マルチパス環境で測位精度の低下を十分に抑制することは困難である。

（２）マルチＧＮＳＳ受信機を用いたときの計算負荷の低減
マルチＧＮＳＳ受信機、あるいは２周波ＧＰＳ受信機のＲａｗデータを用いてＧＮＳＳ測位を行う場合、測位使用衛星数が多くなるため、行列計算量が単純に増える。測位使用衛星にマルチパスの影響を受けたＲａｗデータが含まれると、エポック毎の収束計算回数が増え、計算不可が上昇する問題が生じる。特許文献１，２にはこの問題への対応策は記載されていない。

本発明は以上のような課題を解決するためになされたものであり、高い測位精度を実現するとともに、エポック毎の計算負荷を低くすることが可能な測位装置を提供することを目的とする。

本発明に係る測位装置は、ＧＮＳＳ（Global Navigation Satellite System ）衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出された車両の位置、速度および方位、ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび測位補強信号を出力するＧＮＳＳ受信機と、自律航法により車両の位置を算出する自律航法部と、Ｒａｗデータに含まれる搬送波位相を用いてＧＮＳＳ衛星と車両の位置との擬似距離を平滑化する擬似距離平滑化部と、ＧＮＳＳ受信機が算出した車両の位置の信頼性を評価するＧＮＳＳ受信機測位誤差評価部と、擬似距離の平滑値、測位補強信号、ＧＮＳＳ衛星の軌道およびＧＮＳＳ受信機が算出した車両の位置の信頼性の評価結果に基づいて車両の位置を算出するＧＮＳＳ測位部と、自律航法部が行う自律航法の誤差を、ＧＮＳＳ測位部が算出した車両の位置に基づいて算出し、自律航法の誤差に基づいて自律航法部が算出した車両の位置を修正する複合測位部と、を備え、複合測位部によって修整された車両の位置を出力する測位装置であって、複合測位部は、自律航法部が算出した車両の位置の履歴およびＧＮＳＳ受信機が算出した車両の位置の履歴から、過去エポックでの自律航法の誤差を算出し、当該過去エポックでの自律航法の誤差に基づいて、自律航法部が算出した車両の位置の履歴および最新の車両の位置を修正するものである。

複合測位部が、自律航法の誤差を算出して自律航法部が算出した車両の位置を修正することにより、高い測位精度が得られる。また、測位処理が分散されることで計算負荷が抑制される。また、測位処理が分散されていると、車両の位置と方位の片方のみを得たり、それぞれの誤差を測位処理の途中で確認したりすることが容易になり、測位のロバスト性を向上できる。

本発明の目的、特徴、態様、および利点は、以下の詳細な説明と添付図面とによって、より明白となる。

実施の形態１に係るロケータ装置の構成を示すブロック図である。 実施の形態１に係るロケータ装置の測位部のメイン処理を示すフローチャートである。 図２のステップＳＴ１０９の処理を示すフローチャートである。 図２のステップＳＴ１１４の処理を示すフローチャートである。 図２のステップＳＴ１１６の処理を示すフローチャートである。 マルチＧＮＳＳ受信機の測位誤差（方位）評価の方法を説明するための図である。 マルチＧＮＳＳ受信機の測位誤差（方位）評価の方法を説明するための図である。 マルチＧＮＳＳ受信機の測位誤差（方位）評価の方法を説明するための図である。 マルチＧＮＳＳ受信機の測位誤差（位置）評価の方法を説明するための図である。 マルチＧＮＳＳ受信機の測位誤差（位置）評価の方法を説明するための図である。 マルチＧＮＳＳ受信機の測位誤差（位置）評価の方法を説明するための図である。 Ｒａｗデータ（擬似距離）の誤差評価および測位使用衛星の選択の処理を説明するための図である。 Ｒａｗデータ（擬似距離）の誤差評価および測位使用衛星の選択の処理を説明するための図である。 Ｒａｗデータ（擬似距離）の誤差評価および測位使用衛星の選択の処理を説明するための図である。 Ｒａｗデータ（擬似距離）の誤差評価および測位使用衛星の選択の処理を説明するための図である。 Ｒａｗデータ（擬似距離）の誤差評価および測位使用衛星の選択の処理を説明するための図である。 Ｒａｗデータ（擬似距離）の誤差評価および測位使用衛星の選択の処理を説明するための図である。 ＧＮＳＳ−ＫＦ解位置の有効性の判断方法を説明するための図である。 ＧＮＳＳ−ＫＦ解位置の有効性の判断方法を説明するための図である。 ＤＲ位置の誤差修正を説明するための図である。 ＤＲ位置の誤差修正を説明するための図である。 実施の形態２に係るロケータ装置における複合測位を説明するための図である。 実施の形態２に係るロケータ装置における複合測位を説明するための図である。 実施の形態３に係るロケータ装置の構成を示すブロック図である。 実施の形態３に係るロケータ装置の測位部のメイン処理を示すフローチャートである。 図２５のステップＳＴ５１４の処理を示すフローチャートである。 ロケータ装置の測位部のハードウェア構成の例を示す図である。 ロケータ装置の測位部のハードウェア構成の例を示す図である。

（１）マルチパス環境での測位精度低下の抑止
先に述べたように、車載用の安価なＧＮＳＳ受信機は、ＧＮＳＳ衛星が発した電波の伝搬時間からＧＮＳＳ衛星と自車との２点間距離を求め、三角測量の原理で自車位置および受信機時計誤差を求めるコード測位を採用する。しかし、マルチパス環境下では、ＧＮＳＳ受信機が電波伝搬時間を正しく観測できないため、マルチパス誤差などの測位誤差が生じる。この問題は、マルチパスが生じにくい高仰角衛星の使用機会を増やすことで問題を小さくできるため、複数の測位衛星システムに対応したマルチＧＮＳＳ受信機が車載用として利用されるようになっている。

しかし、マルチＧＮＳＳ受信機を用いても、マルチパスの問題は完全には解消されない。そのため、マルチＧＮＳＳ受信機とは別に、ＳｏＣ側で各種のＧＮＳＳ衛星から取得したＲａｗデータからマルチパス誤差を推定して測位計算を行い、測位精度の低下を抑制する必要がある。ＳｏＣで測位計算を行う場合は、各ＧＮＳＳのＲａｗデータの仕様および各ＧＮＳＳの時系の差異への対応が必要になる。またＧＰＳのみのＲａｗデータをＳｏＣで扱う場合は、測位に使用する衛星（測位使用衛星）の数がマルチＧＮＳＳ受信機よりも少なくなって不利になるので、その不利の補い方が課題となる。

本発明が解決する課題の一つは、マルチパス環境での精度低下を抑止し、レーンレベルの精度を確保することになる。特定の衛星電波にマルチパスが発生した場合、ＳｏＣがマルチパス誤差による測位精度の低下を正しく認識できないと、自車位置が誤修正される問題が生じる。この問題を解決するため、ＳｏＣがＧＮＳＳ受信機のＲａｗデータから自車位置の誤差を求める複合測位を行う場合がある。測位使用衛星にマルチパスが生じた衛星が１つでも含まれていると、複合測位した自車位置に誤差が生じるので、マルチパスが生じた衛星を測位使用衛星から除外するか、擬似距離のマルチパス誤差を正しく求めて補正する必要がある。

特許文献１の技術では、測位計算に最小二乗法が用いられているが、最小二乗法は誤差がランダム性を有することを前提としているため、擬似距離などに大きな誤差や偏った誤差が生じる地点を除外する必要がある。マルチパス誤差を含んだ擬似距離を測位計算に用いるにしても、ランダム性を確保できるように自車位置の軌跡の長さを決める必要がある。しかし、マルチパス発生は場所や時刻によって変化し、局所的に偏った誤差が発生し続けることもあるので、軌跡の長さを決めるのは難しい。精度が低下した自車位置の割合が増える高層ビル街などでは、誤差のランダム性を確保できなくなり、自車位置の精度がレーンレベルまで上がらない。

特許文献２の技術は、自車がトンネルから出て測位が再開された直後の区間にしか対応しておらず、マルチパスの発生区間において、カルマンフィルタの推定値に大きな誤差が残留した場合には対応していない。また、自車上空の測位使用衛星の配置（ＤＯＰ）が良くても、マルチパス影響はないとは言えない。さらに、過去エポックの測位精度が良くて現エポックの測位精度が悪い場合、ならびに、トンネル出口から現エポックまで測位精度が低い状態が続いた場合には、精度の悪い現エポックのＧＰＳ位置でカルマンフィルタの内部データが誤修正される懸念があり、マルチパス環境で測位精度の低下を十分に抑制することは困難である。

（２）搬送波位相を用いたＧＮＳＳ受信機の高精度化
測位精度を１ｍ程度（ＣＥＰ（Circular Error Probability）５０％確率）まで高くできる精密単独測位（ＰＰＰ：Precise Point Positioning）と呼ばれる技術が提案されている。ＰＰＰでは、ＳＢＡＳ（Satellite Based Augmentation System：静止衛星型衛星航法補強システム）衛星から入手した測位補強データ（衛星時計誤差、衛星軌道誤差、電離層伝搬遅延、対流圏伝搬遅延）を用いて擬似距離の誤差が補正され、さらに、搬送波位相を用いて擬似距離の平滑化（ＣＳ：キャリアスムージング）が行われる。ただし、建造物等により衛星電波が一時的に遮断されると、搬送波位相を連続して使用できない「サイクルスリップ」と呼ばれる問題が生じる。このため、衛星電波の受信再開時に擬似距離平滑値（搬送波位相を用いて平滑化された擬似距離）を正しく初期化することが課題になる。

（３）センサ学習による自律航法の精度安定化
車載用の安価な自律センサは、センサの０点出力の変動量および感度の変動量が大きい。そのような自律センサを用いて、位置誤差の小さな自律航法を行うためには、自律センサの０点出力および感度の学習をより最適に行うことが課題になる。

（４）自律航法誤差を補正する複合測位
自律航法の位置誤差を複合測位によって求める際に、自律センサの０点出力と感度の誤差も同時に求める場合、複合測位の状態量のパラメータが増えるため、行列計算が増加することが懸念される。安価なＳｏＣで測位計算を行う場合は、計算負荷の抑制も課題となる。

（５）自車上空の視界制限による測位精度低下の抑止
自車上空の視界が制限された状態では、測位使用衛星数が少なくなったり、測位使用衛星の配置が特定方向に偏ったりするため、測位使用衛星配置が幾何学的に悪くなり測位精度が低下する。また、オープンスカイ下でも、測位使用衛星数が少なくなったり、測位使用衛星の配置が特定方向に偏ったりすることはある。この測位精度の低下を抑制することが望まれる。

特許文献１の技術では、測位計算に最小二乗法が用いられているが、衛星配置が特定方向に偏った状態で、誤差のランダム性を確保できるよう軌跡の長さを決めることは難しい。また、トンネル出口ではＧＮＳＳ受信機の電波補足した衛星数が少ないので、自車上空の視界が制限された状況に近い状態になる。そのため、特許文献２の技術を応用することも考えられるが、やはり、過去エポックの測位精度が良くて現エポックの測位精度が悪い場合、ならびに、トンネル出口から現エポックまで測位精度が低い状態が続いた場合には、精度の悪い現エポックのＧＰＳ位置でカルマンフィルタの内部データが誤修正される懸念があり、測位精度の低下を十分に抑制することは困難である。

（６）自律航法による測位精度低下の抑止
ＧＮＳＳ受信機による測位結果を用いて自律航法の誤差を修正する疎結合、および、ＧＮＳＳ受信機が取得したＲａｗデータを用いて自律航法の誤差を計算して修正する密結合において、測位精度が低下した自車位置の誤修正を回避するために、測位の方法が複合測位から自律航法に適宜切り替えられることがある。しかし、自律航法を長く続けると、自律航法の積算誤差によって、レーンレベルの測位精度を維持できなくなる。特許文献１では複合測位を行うことが想定されているが、特許文献１にはこの問題への対応策は明記されていない。

（７）ドップラー周波数の観測誤差による測位精度低下の抑止
アンダーパス通過時にドップラー周波数の観測誤差が生じると、ドップラー周波数から算出する自車の方位に誤差が生じ、算出される自車位置が実際の走行レーンの隣のレーンにずれることがある。また、ＧＮＳＳ受信機が測位した自車位置のバラツキを抑えるためのスムージングが強いと、オープンスカイであっても、その後しばらくＧＮＳＳ受信機が測位した自車位置に誤差が残留する。特許文献１，２の技術はいずれもレーンレベルの測位精度の実現を目的にしたものでないため、特許文献１，２にはこの問題への対応策は明記されていない。

（８）衛星電波の受信再開直後の精度低下の抑止
搬送波位相を用いて平滑化した擬似距離をＧＮＳＳ測位に用いる場合、衛星電波の遮断中は擬似距離平滑値を更新できないため、衛星電波の受信再開時に擬似距離を用いて擬似距離平滑値を初期化する。この初期化に大きな誤差を持つ擬似距離が用いられると、擬似距離平滑値に誤差が残留する。擬似距離平滑値に誤差が残留すると、オープンスカイであっても、擬似距離を平滑化しない場合よりＧＮＳＳ測位で計測した位置の誤差が大きくなることがある。特許文献１，２にはこの問題への対応策は記載されていない。

（９）マルチＧＮＳＳ受信機を用いたときの計算負荷の低減
マルチＧＮＳＳ受信機、あるいは２周波ＧＰＳ受信機のＲａｗデータを用いてＧＮＳＳ測位を行う場合、測位使用衛星数が多くなるため、行列計算量が単純に増える。測位使用衛星にマルチパスの影響を受けたＲａｗデータが含まれると、エポック毎の収束計算回数が増え、計算不可が上昇する問題が生じる。特許文献１，２にはこの問題への対応策は記載されていない。

＜実施の形態１＞
図１は、本発明の実施の形態１に係るロケータ装置の構成を示すブロック図である。図１では、ロケータ装置の構成を、当該ロケータ装置を搭載した車両（以下「自車」という）の位置の測位に必要な部分を中心に示している。図１に示すように、当該ロケータ装置は、マルチＧＮＳＳ受信機１０と、測位部１００とを備えている。

マルチＧＮＳＳ受信機１０は、自車の上空に存在する複数のＧＮＳＳ衛星から放射される電波、および、測位補強信号を放送する測位補強衛星であるＳＢＡＳ衛星から放送される電波を受信するマルチＧＮＳＳアンテナを備えている。上記複数のＧＮＳＳ衛星には、ＧＰＳ、ＱＺＳ、ＧＬＯＮＡＳＳ、ＢｅｉＤｏｕ、Ｇａｌｉｌｅｏの各衛星うち、少なくともＧＰＳ衛星が含まれるものとし、マルチＧＮＳＳ受信機１０は、少なくともＧＰＳ衛星からのＬ１信号を受信するものとする。測位補強信号には、衛星時計誤差、衛星軌道誤差、電離層伝搬遅延、対流圏伝搬遅延の各補正情報が含まれている。

マルチＧＮＳＳ受信機１０は、受信した各種の衛星電波に基づき、測位計算に必要なＲａｗデータ（擬似距離（観測値）、ドップラー周波数、搬送波位相、航法メッセージ、ＧＮＳＳ−Ｔｉｍｅなど）を取得して、自車の位置、速度、方位などを計算し、計算した測位結果（自車の位置、速度、方位）と、Ｒａｗデータと、測位補強信号とを測位部１００へと出力する。マルチＧＮＳＳ受信機１０の測位結果である自車の位置、速度、方位は、ＥＣＥＦ（Earth Centered Earth Fixed）座標系の位置、位置の移動量、位置の移動方向で表される。以下、マルチＧＮＳＳ受信機１０の測位結果である自車の位置、速度、方位を、それぞれ「ＧＮＳＳ位置」、「ＧＮＳＳ速度」、「ＧＮＳＳ方位」という。

測位部１００は、ＧＮＳＳ測位に係わる処理を行う要素として、ＧＮＳＳ出力データ変換部１０１、ＧＮＳＳ衛星挙動推定部１０２、ＧＮＳＳ測位部（ＬＳ法）１０３、ＧＮＳＳ受信機時計誤差修正部１０４、擬似距離平滑化部１０５、測位使用衛星選択およびＲａｗ誤差評価部１０６（以下「測位使用衛星選択／Ｒａｗ誤差評価部１０６」という）、ＧＮＳＳ受信機測位誤差評価部１０７およびＧＮＳＳ測位部（ＫＦ法）１０８を備える。また、測位部１００は、複合測位に係わる処理を行う要素として、複合測位部（ＫＦ法）１０９を備えている。さらに、測位部１００は、自律航法に係わる処理を行う要素として、速度センサ１１０、距離計測部１１１、速度センサＳＦ補正部１１２、角速度センサ１１３、ヨー角計測部１１４、角速度センサ補正部１１５および自律航法部１１６を備えている。

ＧＮＳＳ出力データ変換部１０１は、マルチＧＮＳＳ受信機１０が電波を受信したＧＮＳＳ衛星のそれぞれについて、マルチＧＮＳＳ受信機１０が出力したＲａｗデータに含まれる擬似距離の時間差分値であるデルタレンジを算出する。また、ＧＮＳＳ出力データ変換部１０１は、マルチＧＮＳＳ受信機１０が出力したドップラー周波数および搬送波位相を、デルタレンジと同じ単位［ｍ／ｓ］のレンジレートにそれぞれ変換する。

ＧＮＳＳ衛星挙動推定部１０２は、マルチＧＮＳＳ受信機１０が出力した各ＧＮＳＳ衛星の航法メッセージとＳＢＡＳ衛星の測位補強信号とを用いて、ＧＮＳＳ−ＴｉｍｅにおけるＧＮＳＳ衛星の位置と速度を計算する。

ＧＮＳＳ測位部（ＬＳ法）１０３は、ＧＮＳＳ衛星挙動推定部１０２が算出したＧＮＳＳ衛星の位置と、マルチＧＮＳＳ受信機１０が受信した各ＧＮＳＳ衛星の擬似距離とを用いて、最小二乗法により、自車の位置、速度および方位と、マルチＧＮＳＳ受信機１０の受信機時計誤差（バイアス誤差）とを算出する。以下、ＧＮＳＳ測位部（ＬＳ法）１０３が計算した自車の位置、速度および方位を、それぞれ「ＧＮＳＳ−ＬＳ解位置」、「ＧＮＳＳ−ＬＳ解速度」および「ＧＮＳＳ−ＬＳ解方位」という。

ＧＮＳＳ受信機時計誤差修正部１０４は、ＧＮＳＳ測位部（ＬＳ法）１０３が求めた受信機時計誤差をより精密に再計算する。以下、ＧＮＳＳ受信機時計誤差修正部１０４が求めた精密な受信機時計誤差を「精密時計誤差」という。

擬似距離平滑化部１０５は、マルチＧＮＳＳ受信機１０が電波を受信した各衛星の搬送波位相を用いて擬似距離を平滑化する。

測位使用衛星選択／Ｒａｗ誤差評価部１０６は、マルチＧＮＳＳ受信機１０から受信したＲａｗデータに含まれる擬似距離（観測値）、および、擬似距離平滑化部１０５が算出した擬似距離平滑値のそれぞれに含まれる未知誤差を評価するとともに、その評価結果に基づいて、ＧＮＳＳ測位部（ＬＳ法）１０３およびＧＮＳＳ測位部（ＫＦ法）１０８が測位計算に使用するＧＮＳＳ衛星および擬似距離（観測値または平滑値）を選択する。

ＧＮＳＳ受信機測位誤差評価部１０７は、マルチＧＮＳＳ受信機１０による測位結果の誤差を評価することにより、マルチＧＮＳＳ受信機１０の測位結果の信頼性の有無を判断する。

ＧＮＳＳ測位部（ＫＦ法）１０８は、ＳＢＡＳ衛星から取得した測位補強信号と、ＧＮＳＳ受信機時計誤差修正部１０４が算出したマルチＧＮＳＳ受信機１０の精密時計誤差（バイアス誤差およびドリフト誤差）とを用いて、測位計算に使用する擬似距離もしくは擬似距離平滑値に含まれる各種の誤差を修正した上で、ＧＮＳＳ衛星挙動推定部１０２が算出した衛星の軌道（位置、速度）に基づき、カルマンフィルタ法により自車の位置を計算する高精度測位（ＰＰＰ）を行う。

複合測位部（ＫＦ法）１０９は、ＧＮＳＳ出力データ変換部１０１が算出したレンジレートに基づいて、自律航法部１１６における自律航法の誤差（方位の誤差）を計算するとともに、ＧＮＳＳ測位部（ＫＦ法）１０８が算出した自車位置などを用いて、自律航法部１１６における自律航法の誤差（位置の誤差と速度の誤差）を計算する。

速度センサ１１０は、自車の移動距離に応じたパルス信号（車速パルス）を出力する。距離計測部１１１は、一定周期で速度センサ１１０が出力したパルス数を計測し、その計測値から自車の移動距離および速度を計算する。速度センサＳＦ補正部１１２は、ＧＮＳＳ測位により算出された自車速度と速度センサ１１０のパルス数とから、速度センサ１１０が出力する１パルスあたりの距離を表すＳＦ（スケールファクタ）係数を算出し、距離計測部１１１に設定されているＳＦ係数を補正する。

角速度センサ１１３は、ロケータ装置の筐体の垂直方向をセンサ検知軸として、自車の角速度（ヨーレート）に応じた信号、０点出力に加算して出力する。ヨー角計測部１１４は、一定周期で角速度センサ１１３の出力を計測し、その計測結果から自車のヨー角を計算する。角速度センサ補正部１１５は、角速度センサ１１３の０点出力を求め、ヨー角計測部１１４に設定されている角速度センサ１１３の０点出力を補正する。

なお、速度センサ１１０、距離計測部１１１、速度センサＳＦ補正部１１２、角速度センサ１１３、ヨー角計測部１１４および角速度センサ補正部１１５は、必ずしもロケータ装置内に配置されていなくてもよく、例えば自車に搭載されたそれらと同等の機能を有するものを利用してもよい。

自律航法部１１６は、距離計測部１１１が計算した自車の移動距離と、ヨー角計測部１１４が計算した自車のヨー角とを用いて、自律航法（ＤＲ：Dead Reckoning）により、自車位置と自車速度および自車方位を計算して更新する。その際、自律航法部１１６は、複合測位部（ＫＦ法）１０９が求めた自律航法の誤差を用いて、自律航法による自車位置、自車速度および自車方位を修正する。この修正後の自車位置、自車速度および自車方位が、ロケータ装置から出力される。以下、自律航法部１１６が計算した自車の位置、速度および方位を、それぞれ「ＤＲ位置」、「ＤＲ速度」および「ＤＲ方位」という。

次に、実施の形態１に係るロケータ装置の動作を説明する。図２は、測位部１００のメイン処理を示すフローチャートである。図３〜図５は、それぞれＧＮＳＳ測位部（ＬＳ法）１０３、ＧＮＳＳ測位部（ＫＦ法）１０８および複合測位部（ＫＦ法）１０９の各処理を示すフローチャートである。図２のフローは予め定められた周期で実行され、図３〜図５のフローはそれぞれ図２のフローのステップＳＴ１０９、ＳＴ１１４およびＳＴ１１６で実行される。以下、これらのフローチャートに基づいて、実施の形態１に係るロケータ装置の動作を説明する。

図２のメイン処理が開始されると、測位部１００は処理を初期化する（ステップＳＴ１０１）。これにより、測位部１００に設定されているＧＮＳＳ測位解のステータスは「ＬＳ解なし」および「ＫＦ解なし」となり、複合測位解のステータスは「ＫＦ解なし」となる。

続いて、自律航法に関する以下のステップＳＴ１０２からステップＳＴ１０５の処理が行われる。

ステップＳＴ１０２では、距離計測部１１１が、一定周期で計測された速度センサ１１０のパルス数にＳＦ係数を乗算して自車の移動距離を求めるとともに、当該パルス数をローパスフィルタに通した値を用いて自車の移動速度を計算する。

ステップＳＴ１０３は自車の停車中に行われる処理であり、このステップでは、角速度センサ補正部１１５が角速度センサ１１３の０点出力を補正する。すなわち、角速度センサ補正部１１５は、距離計測部１１１が計算した自車の移動距離から自車が停車中であると判断すると、停車中での角速度センサ１１３の出力の平均値を求め、その平均値を角速度センサ１１３の０点出力として、ヨー角計測部１１４に設定する。この処理については、本発明者が出願した特許３１３７７８４号公報および特許３７５１５１３号公報に詳細が説明されている。

ステップＳＴ１０４では、ヨー角計測部１１４が、予め定められたタイミング毎に計測された角速度センサ１１３の出力から０点出力を差し引いたヨー角を計算する。

ステップＳＴ１０５では、自律航法部１１６が、自律航法により自車位置（ＤＲ位置）の測位を行う。すなわち、自律航法部１１６は、速度センサ１１０および角速度センサ１１３の出力信号から計算された自車の移動距離とヨー角を用いて、予め定められた周期毎の移動ベクトル（ＤＲ速度およびＤＲ方向に相当）を求め、その移動ベクトルを前回算出された自車位置に加算することで自車位置を更新する。

次に、ＧＮＳＳ測位および複合測位に関する以下のステップＳＴ１０６からステップＳＴ１１９の処理が行われる。

ステップＳＴ１０６では、ＧＮＳＳ出力データ変換部１０１が、マルチＧＮＳＳ受信機１０が測位に使用しているＧＮＳＳ衛星（測位使用衛星）の数を、マルチＧＮＳＳ受信機１０の測位タイミングで確認する。測位衛星数が４つ以上あれば（ステップＳＴ１０６でＹＥＳ）、ステップＳＴ１０７へ進み、衛星数が４つ未満であれば（ステップＳＴ１０６でＮＯ）、後述するステップＳＴ１１８へ進む。

ステップＳＴ１０７では、ＧＮＳＳ出力データ変換部１０１が、マルチＧＮＳＳ受信機１０が出力したデータの変換処理を行う。具体的には、ＧＮＳＳ出力データ変換部１０１は、式（１）を用いて、マルチＧＮＳＳ受信機１０が出力した擬似距離を、その時間差分値であるデルタレンジに変換する。また、ＧＮＳＳ出力データ変換部１０１は、式（２）および式（３）を用いて、マルチＧＮＳＳ受信機１０が出力したドップラー周波数および搬送波位相を、デルタレンジと同じ単位［ｍ／ｓ］のレンジレートにそれぞれ変換する。さらに、ＧＮＳＳ出力データ変換部１０１は、式（４）を用いて、自律航法部１１６が自律航法により算出した自車の位置および速度から、ＧＮＳＳ衛星のレンジレートの予測値を算出する。これらの処理は、マルチＧＮＳＳ受信機１０が電波を受信したＧＮＳＳ衛星の各々について行われる。

なお、式（１）および式（２）で求められるデルタレンジおよびレンジレートは、ＧＮＳＳ衛星の衛星時計誤差およびマルチＧＮＳＳ受信機１０の受信機時計誤差が補正される前の値であるが、式（４）で求められるレンジレートの予測値は、それらの誤差が補正された後の値となる。

ステップＳＴ１０８では、ＧＮＳＳ受信機測位誤差評価部１０７が、マルチＧＮＳＳ受信機１０による測位結果の誤差を評価して、マルチＧＮＳＳ受信機１０の測位結果の信頼性の有無を判断する。具体的には、ＧＮＳＳ受信機測位誤差評価部１０７は、図６に示すように、マルチＧＮＳＳ受信機１０が算出した自車位置（ＧＮＳＳ位置）の移動方向と、マルチＧＮＳＳ受信機１０が算出した自車方位（ＧＮＳＳ方位）とを比較することで、ＧＮＳＳ方位の誤差を推定する。図７に示すように、自律航法部１１６が算出するＤＲ方向が一定であっても、マルチＧＮＳＳ受信機１０が算出するＧＮＳＳ方位は、ＧＮＳＳ測位の誤差の影響でバラツキが生じやすい。また、ＧＮＳＳ受信機測位誤差評価部１０７は、図８に示すように、測位使用衛星それぞれのドップラー周波数について、自律航法の位置情報から算出したレンジレートの波形と、マルチＧＮＳＳ受信機１０が出力したドップラー周波数を変換して得たレンジレート（受信機時計のドリフト誤差修正したもの）の波形とを比較する。

その結果、ＧＮＳＳ方位の誤差が規定値以下で、且つ、全ての測位使用衛星で上記２つのレンジレートの波形が一致していれば、ＧＮＳＳ受信機測位誤差評価部１０７は、ＧＮＳＳ方位の信頼性が高いと判断する。しかし、ＧＮＳＳ方位の誤差が規定値を超えている場合、もしくは、いずれかの測位使用衛星で上記２つのレンジレートの波形が一致しない場合には、ＧＮＳＳ方位の信頼性が低いと判断する。これらの処理については、本発明者が出願した特許４９８８０２８号公報、特許５８５５２４９号公報に詳細が説明されている。

さらに、ＧＮＳＳ受信機測位誤差評価部１０７は、上記２つのレンジレートが一致していれば、図９に示すように、ＧＮＳＳ位置のエポック間での変化を表す移動ベクトル（Δｐｘ_Ｅ，Δｐｙ_Ｅ，Δｐｙ_Ｅ）とＧＮＳＳ速度およびＧＮＳＳ方位を表す速度ベクトル（ｖｘ_ＥΔｔ，ｖｙ_ＥΔｔ，ｖｚ_ＥΔｔ）とを比較して、両者の差異を確認することで、マルチＧＮＳＳ受信機１０が測定したＧＮＳＳ位置の相対誤差を推定する。ＧＮＳＳ受信機測位誤差評価部１０７は、図９に示す(1)(2)(3)のすべてが規定値以下であれば、ＧＮＳＳ位置の相対位置精度は信頼できると判断し、(1)(2)(3)のいずれかが規定値を超えていれば、ＧＮＳＳ位置の相対位置精度は信頼できないと判断する。

また、ＧＮＳＳ受信機測位誤差評価部１０７は、図１０もしくは図１１に示すように、予め定められたエポック分の直近のＧＮＳＳ位置の履歴およびＤＲ位置の履歴から、ＧＮＳＳ位置の軌跡とＤＲ位置の軌跡とを求め、両者の軌跡が一致するようにAffine変換した上で比較し、軌跡を構成する各位置でのＧＮＳＳ位置とＤＲ位置との差異のバラツキを求める。さらに、ＧＮＳＳ受信機測位誤差評価部１０７は、ＧＮＳＳ位置とＤＲ位置との差異のバラツキと、マルチＧＮＳＳ受信機１０が測位に使用したＧＮＳＳ衛星の幾何学的配置によって定まる精度低下率（ＤＯＰ）の値などから、ＧＮＳＳ位置の絶対誤差を推定する。この処理については、本発明者が出願した特許５８５５２４９号公報に詳細が説明されている。

その結果、ＧＮＳＳ位置の絶対誤差が規定値以下で、且つ、相対位置精度が信頼できれば、ＧＮＳＳ受信機測位誤差評価部１０７は、ＧＮＳＳ位置の絶対位置が信頼できると判断する。しかし、ＧＮＳＳ位置の絶対誤差範囲が規定値を超える場合、もしくは相対位置精度が信頼できない場合には、ＧＮＳＳ受信機測位誤差評価部１０７は、ＧＮＳＳ位置の絶対位置は信頼できないと判断する。

ステップＳＴ１０９では、ＧＮＳＳ測位部（ＬＳ法）１０３が、最小二乗法を用いたＧＮＳＳ測位により自車位置の計測、つまり自車の測位が行われる。この処理では、自車位置（ＧＮＳＳ−ＬＳ解位置）、自車速度（ＧＮＳＳ−ＬＳ解速度）および自車方位（ＧＮＳＳ−ＬＳ解方位）の他、マルチＧＮＳＳ受信機１０の受信機時計誤差（バイアス誤差）も算出される。ステップＳＴ１０９の処理の詳細は、後で図３を用いて説明する。

ステップＳＴ１１０では、ＧＮＳＳ受信機時計誤差修正部１０４が、ＧＮＳＳ測位部（ＬＳ法）１０３により算出された受信機時計誤差（バイアス誤差）、Ｒａｗデータなどを用いて、マルチＧＮＳＳ受信機１０のより精密な受信機時計誤差、すなわち精密時計誤差（バイアス誤差およびドリフト誤差）を計算する。この処理については、本発明者が出願した国際出願ＰＣＴ／ＪＰ２０１６／０５７３３７に詳細が説明されている。

ステップＳＴ１１１では、擬似距離平滑化部１０５が、マルチＧＮＳＳ受信機１０が電波を受信した衛星について、搬送波位相を用いて擬似距離を平滑化する。搬送波位相を用いる擬似距離の平滑化は式（５）により行われる。

ステップＳＴ１１２では、測位使用衛星選択／Ｒａｗ誤差評価部１０６が、マルチＧＮＳＳ受信機１０から受信したＲａｗデータに含まれる擬似距離（観測値）および擬似距離平滑化部１０５が算出した擬似距離平滑値のそれぞれに含まれる未知誤差を評価するとともに、その評価結果に基づいて、ＧＮＳＳ測位部（ＬＳ法）１０３が測位計算に使用するＧＮＳＳ衛星および擬似距離（観測値または平滑値のいずれか）を選択する。

測位使用衛星選択／Ｒａｗ誤差評価部１０６は、マルチＧＮＳＳ受信機１０が電波を受信したＧＮＳＳ衛星について、擬似距離の未知誤差を求める。図１２に示すように、擬似距離の未知誤差は、擬似距離から、既知の誤差と、ＧＮＳＳ衛星の位置と自車位置（自律航法で求めた自車位置（ＤＲ位置））の２点間距離とを差し引いた残差として求められる。図１３に示すように、既知の誤差としては、ＳＢＡＳ衛星の測位補強信号から算出される衛星軌道誤差、衛星時計誤差、電離層伝搬遅延誤差、対流圏伝搬遅延誤差などがある。ここでは、衛星時計誤差として、ＧＮＳＳ受信機時計誤差修正部１０４が算出した精密時計誤差が用いられるものとする。なお、図１３に示すマルチパス誤差は未知誤差に含まれる。

また、測位使用衛星選択／Ｒａｗ誤差評価部１０６は、擬似距離平滑化部１０５が算出した擬似距離平滑値についても、擬似距離平滑値から上記の既知の誤差を差し引いた未知誤差を算出する。

そして、測位使用衛星選択／Ｒａｗ誤差評価部１０６は、図１４または図１５に示すように、擬似距離の観測値（ここでは「擬似距離観測値」という）または擬似距離平滑値の未知誤差が予め定められた閾値以下の衛星を測位使用衛星として選択する。図１４の例では、衛星Ａで瞬時的なマルチパスが生じて擬似距離観測値の未知誤差が大きくなり、衛星Ａが測位使用衛星から除外されている。

さらに、測位使用衛星選択／Ｒａｗ誤差評価部１０６は、測位使用衛星の擬似距離として、擬似距離観測値と擬似距離平滑値のうち未知誤差が小さい方を選択する。通常、擬似距離平滑値の未知誤差は、擬似距離観測値の未知誤差よりも小さくなるが、衛星電波が遮断された直後に擬似距離平滑値に誤差が残留すると、図１６のように、擬似距離観測値よりも未知誤差がかえって大きくなることがある。図１６の例では、衛星Ｂの擬似距離平滑値に誤差が残留した結果、衛星Ｂの擬似距離平滑値の未知誤差が大きくなり、衛星Ｂが測位使用衛星から除外されている。

ステップＳＴ１１３は、高精度測位（ＰＰＰ）を実施可能かどうかを予め確認するための処理である。ステップＳＴ１１３では、測位使用衛星選択／Ｒａｗ誤差評価部１０６が、選択した測位使用衛星のうち、擬似距離として擬似距離平滑値が選択された衛星の数を確認し、その数に基づいて高精度測位が可能か否か判断する。擬似距離として擬似距離平滑値が選択された衛星の数が規定値以上（具体的には４個以上）であれば、高精度測位が可能であると判断され（ステップＳＴ１１３でＹＥＳ）、ステップＳＴ１１４へ進む。擬似距離として擬似距離平滑値が選択された衛星の数が規定値に満たなければ、高精度測位は不可能と判断され（ステップＳＴ１１３でＮＯ）、後述するステップＳＴ１１８へ進む。図１７に示すように、擬似距離平滑値を用いて測位を行うと位置精度は高くなる。

ステップＳＴ１１４では、ＧＮＳＳ測位部（ＫＦ法）１０８が、カルマンフィルタ法を用いたＧＮＳＳ測位により、自車位置の高精度測位（ＰＰＰ）を行う。この処理の詳細は、後で図４を用いて後で説明する。

ステップＳＴ１１５では、複合測位部（ＫＦ法）１０９が、ステップＳＴ１０９で算出されたＧＮＳＳ−ＬＳ解方位を用いて、自律航法における自車方位（ＤＲ方位）の誤差を算出する。この処理は、ステップＳＴ１０８でＧＮＳＳ方位が信頼できると判断され、且つ、ステップＳＴ１０９で算出されたＧＮＳＳ−ＬＳ解方位とマルチＧＮＳＳ受信機１０が出力したＧＮＳＳ方位との差異が規定値以下である場合に行われる。ＧＮＳＳ−ＬＳ解方位は、ＧＮＳＳ衛星のドップラー周波数から計算されており、マルチパスの影響を直接受けないため、ＤＲ方位の誤差を安定的に算出することができる。

ステップＳＴ１１６では、複合測位部（ＫＦ法）１０９が、参照するＧＮＳＳ測位位置を観測値とするカルマンフィルタ法により、自車位置（ＤＲ位置）の誤差および自車速度（ＤＲ速度）の誤差を計算する。この処理の詳細は、後で図５を用いて説明する。

ステップＳＴ１１７では、自律航法部１１６が、ステップＳＴ１１５およびＳＴ１１６で算出された自律航法の誤差（ＤＲ位置、ＤＲ速度およびＤＲ方位の各誤差）を用いて、ステップＳＴ１０５で得た自律航法による測位結果（ＤＲ位置、ＤＲ速度およびＤＲ方位）を修正する。

ステップＳＴ１１８では、速度センサＳＦ補正部１１２が、速度センサ１１０が出力する１パルスあたりの距離を表すＳＦ係数を算出し、距離計測部１１１に設定されているＳＦ係数を補正する。この処理については、本発明者が出願した特許３３２１０９６号公報、特許３７２７４８９号公報、特許５６０６６５６号公報に詳細が説明されている。

ステップＳＴ１１９は自車の走行中に行われる処理であり、角速度センサ補正部１１５が角速度センサ１１３の０点出力を補正する。すなわち、自車の走行中に温度ドリフトにより変動する角速度センサ１１３の０点出力を、自車方位（ＤＲ方位）を基準にして学習し、その学習結果に基づいて０点出力を補正する。この処理については、本発明者が出願した特許３３２１０９６号公報、特許３７２７４８９号公報に詳細が説明されている。なお、自車が走行中か否かは、距離計測部１１１が計算した自車の移動距離から判断できる。ステップＳＴ１１９の処理が完了すると、図２のフローは終了する。

次に、図２のステップＳＴ１０９の処理の詳細を、図３を用いて説明する。図２のステップＳＴ１０９では、図３に示すステップＳＴ２０１からステップＳＴ２１１の処理が行われる。

ステップＳＴ２０１では、ＧＮＳＳ測位部（ＬＳ法）１０３が、測位部１００に設定されているＧＮＳＳ測位解のステータスを「ＬＳ解なし」にする。

ステップＳＴ２０２では、ＧＮＳＳ衛星挙動推定部１０２が、マルチＧＮＳＳ受信機１０が出力した航法メッセージと測位補強信号（衛星軌道誤差）とを用いて、ＧＮＳＳ−Ｔｉｍｅにおける測位使用衛星の挙動（位置と速度）を算出する。なお、ＧＮＳＳ−Ｔｉｍｅは、収束計算中に徐々に正確に算出されるようになる。

ステップＳＴ２０３では、ＧＮＳＳ測位部（ＬＳ法）１０３が、式（６）を用いて、マルチＧＮＳＳ受信機１０が出力した擬似距離を、各種の誤差を差し引いた値に修正する。この処理は、マルチＧＮＳＳ受信機１０が電波を受信した衛星の各々について行われる。

ステップＳＴ２０４では、測位使用衛星選択／Ｒａｗ誤差評価部１０６が、図２のステップＳＴ１１２と同様に、測位使用衛星を選択する。

ステップＳＴ２０５では、ＧＮＳＳ測位部（ＬＳ法）１０３が、式（７）に示す最小二乗法の計算式により、自車の位置（ＧＮＳＳ−ＬＳ解位置）と受信機時計誤差（バイアス誤差）を算出する。

ステップＳＴ２０６では、ＧＮＳＳ測位部（ＬＳ法）１０３が、現エポックにおける収束計算回数を確認する。収束計算回数が規定値以下であれば（ステップＳＴ２０６でＹＥＳ）、ステップＳＴ２０７へ進む。収束計算回数が規定値を超えていれば（ステップＳＴ２０６でＮＯ）、収束できなかったと判断して、図３の処理を抜ける。

ステップＳＴ２０７では、ＧＮＳＳ測位部（ＬＳ法）１０３が、現エポックでのＧＮＳＳ−ＬＳ解位置の変化量を確認する。ＧＮＳＳ−ＬＳ解位置の変化量が規定値以下であれば（ステップＳＴ２０７でＹＥＳ）、収束できたと判断して、ステップＳＴ２０８へ進む。ＧＮＳＳ−ＬＳ解位置の変化量が規定値を超えていれば（ステップＳＴ２０７でＮＯ）、収束できなかったと判断して、時刻を更新してステップＳＴ２０２へ戻る。

ステップＳＴ２０８では、ＧＮＳＳ測位部（ＬＳ法）１０３が、ＧＮＳＳ測位解のステータスを「ＬＳ解あり」とする。

ステップＳＴ２０９では、ＧＮＳＳ測位部（ＬＳ法）１０３が、式（８）を用いて自車の速度（ＧＮＳＳ−ＬＳ解速度）を算出する。

ステップＳＴ２１０では、ＧＮＳＳ測位部（ＬＳ法）１０３が、ステップＳＴ２０９で算出したＧＮＳＳ−ＬＳ解速度のベクトルから、自車の方位（ＧＮＳＳ−ＬＳ解方位）を算出する。

ステップＳＴ２１１では、ＧＮＳＳ測位部（ＬＳ法）１０３が、図９〜図１１を用いて説明したＧＮＳＳ位置の誤差および信頼性の評価方法と同様の手法により、ＧＮＳＳ−ＬＳ解位置の誤差および信頼性を評価する。

次に、図２のステップＳＴ１１４の処理の詳細を、図４を用いて説明する。図２のステップＳＴ１１４では、図４に示すステップＳＴ３０１からステップＳＴ３０９の処理が行われる。

ステップＳＴ３０１では、ＧＮＳＳ測位部（ＫＦ法）１０８が、測位部１００に設定されているＧＮＳＳ測位解のステータスを「ＫＦ解なし」にする。

ステップＳＴ３０２では、ＧＮＳＳ衛星挙動推定部１０２が、図３のステップＳＴ２０２と同様に、ＧＮＳＳ−Ｔｉｍｅにおける測位使用衛星の挙動（位置と速度）を算出する。なお、ＧＮＳＳ−Ｔｉｍｅは、収束計算中に徐々に正確に算出されるようになる。

ステップＳＴ３０３およびステップＳＴ３０４では、図３のステップＳＴ２０３およびステップＳＴ２０４と同様の手法により、ＧＮＳＳ測位部（ＫＦ法）１０８がマルチＧＮＳＳ受信機１０が出力した各衛星の擬似距離を修正し、測位使用衛星選択／Ｒａｗ誤差評価部１０６が測位使用衛星を選択する。

ステップＳＴ３０５では、ＧＮＳＳ測位部（ＫＦ法）１０８が、式（９）に示すカルマンフィルタの計算式により、自車の位置を算出する。以下、ＧＮＳＳ測位部（ＫＦ法）１０８が算出した自車位置を「ＧＮＳＳ−ＫＦ解位置」という。

ステップＳＴ３０６では、ＧＮＳＳ測位部（ＫＦ法）１０８が、現エポックにおける収束計算回数を確認する。収束計算回数が規定値以下であれば（ステップＳＴ３０６でＹＥＳ）、ステップＳＴ３０７へ進む。収束計算回数が規定値を超えていれば（ステップＳＴ３０６でＮＯ）、収束できなかったと判断して、図４の処理を抜ける。

ステップＳＴ３０７では、ＧＮＳＳ測位部（ＫＦ法）１０８が、現エポックでのＧＮＳＳ−ＫＦ解位置の変化量を確認する。ＧＮＳＳ−ＫＦ解位置の変化量が規定値以下であれば（ステップＳＴ３０７でＹＥＳ）、収束できたと判断して、ステップＳＴ３０８へ進む。ＧＮＳＳ−ＫＦ解位置の変化量が規定値を超えていれば（ステップＳＴ３０６でＮＯ）、収束できなかったと判断して、時刻を更新してステップＳＴ３０２へ戻る。

ステップＳＴ３０８では、ＧＮＳＳ測位部（ＫＦ法）１０８が、ＧＮＳＳ測位解のステータスを「ＫＦ解あり」とする。

ステップＳＴ３０９では、ＧＮＳＳ測位部（ＫＦ法）１０８が、図９〜図１１を用いて説明したＧＮＳＳ位置の誤差および信頼性の評価方法と同様の手法により、ＧＮＳＳ−ＫＦ解位置の誤差および信頼性を評価する。なお、ステップＳＴ３０９では、マルチＧＮＳＳ受信機１０の測位結果が信頼できる場合は、図１８のようにＧＮＳＳ位置の測位誤差の範囲内にＧＮＳＳ−ＫＦ解位置が入っていればそのＧＮＳＳ−ＫＦ解位置は有効と判断され、図１９のようにＧＮＳＳ位置の測位誤差の範囲内にＧＮＳＳ−ＫＦ解位置が入っていなければそのＧＮＳＳ−ＫＦ解位置は無効と判断される。また、マルチＧＮＳＳ受信機１０の測位結果が信頼できない場合は、測位使用衛星にＰＰＰ衛星（擬似距離として擬似距離平滑値が選択された衛星）が予め定められた個数以上含まれていれば、ＧＮＳＳ−ＫＦ解位置を有効と判断される。

次に、図２のステップＳＴ１１６の処理の詳細を、図５を用いて説明する。図５をステップＳＴ１１６では、図５に示すステップＳＴ４０１からステップＳＴ４０６の処理が行われる。

ステップＳＴ４０１では、複合測位部（ＫＦ法）１０９が、測位部１００に設定されている複合測位解のステータスを「ＫＦ解なし」にする。

ステップＳＴ４０２では、複合測位部（ＫＦ法）１０９が、図２のステップＳＴ１０５で更新された自車位置（ＤＲ位置）および自車速度（ＤＲ速度）についての各修正量を、カルマンフィルタ法により算出する（ＤＲ位置およびＤＲ速度は、速度センサ１１０および角速度センサ１１３の出力信号から計算された現エポックでの自車の移動ベクトル（速度ベクトル）を、前エポックで算出された自車位置に加算することにより更新されている）。具体的には、複合測位部（ＫＦ法）１０９は、式（１０）に示すカルマンフィルタの計算式により、ＧＮＳＳ測位部（ＫＦ法）１０８が算出したＧＮＳＳ−ＫＦ解位置を基準にして算出する。

ＫＦ解によるＤＲ位置の修正量は、図２０に示すような、速度センサ１１０および角速度センサ１１３の誤差に起因する自車位置の移動量の誤差（速度ベクトルの誤差）と、ＤＲ位置およびＤＲ速度それぞれのプロセス誤差と、ＧＮＳＳ−ＫＦ解位置の観測誤差とに応じたカルマンフィルタゲインにより決まる。ただし、参照するＧＮＳＳ位置が信頼でき、図２１のようにＧＮＳＳ−ＫＦ解位置とＤＲ位置との間に規定値以上の固定的な差異（ずれ）が生じている場合は、複合測位部（ＫＦ法）１０９は、その差異は自律航法による自車位置の残留誤差と判断し、当該差異の平均値をカルマンフィルタの状態変数に制御入力してＤＲ位置を修正することで、ＤＲ位置の修正量を算出する。

ステップＳＴ４０３では、複合測位部（ＫＦ法）１０９が、現エポックにおける収束計算回数を確認する。収束計算回数が規定値以下であれば（ステップＳＴ４０３でＹＥＳ）、ステップＳＴ４０４へ進む。収束計算回数が規定値を超えていれば（ステップＳＴ４０３でＮＯ）、収束できなかったと判断して、図３の処理を抜ける。

ステップＳＴ４０４では、複合測位部（ＫＦ法）１０９が、現エポックでのＫＦ解の変化量を確認する。ＫＦ解の変化量が規定値以下であれば（ステップＳＴ４０４でＹＥＳ）、収束できたと判断して、ステップＳＴ４０５へ進む。ＫＦ解の変化量が規定値を超えていれば（ステップＳＴ４０４でＮＯ）、収束できなかったと判断して、時刻を更新してステップＳＴ４０２へ戻る。

ステップＳＴ４０５では、複合測位部（ＫＦ法）１０９が、複合測位解のステータスを「ＫＦ解あり」とする。

ステップＳＴ４０６は、複合測位部（ＫＦ法）１０９が、図９〜図１１を用いて説明したＧＮＳＳ位置の誤差および信頼性の評価方法と同様の手法により、ＫＦ解の誤差および信頼性を評価する。ただし、ＤＲ位置の移動量と移動方向が規定値以上になる場合は、ＫＦ解は無効と判断する。

また、複合測位部（ＫＦ法）１０９は、ＧＮＳＳ測位部（ＫＦ法）１０８が算出したＧＮＳＳ−ＫＦ解位置が、図４のステップＳＴ３０９で無効と判断された場合、マルチＧＮＳＳ受信機１０が算出したＧＮＳＳ位置（信頼できるもの）とＤＲ位置とを比較し、両者間の距離が予め定められた閾値以上離れていれば、マルチＧＮＳＳ受信機１０の測位結果を用いてカルマンフィルタを初期化する。

実施の形態１に係るロケータ装置では、測位処理が分散されているため、全ての測位結果（位置および方位）が得られなくとも、部分的な測位結果（位置または方位）を得たり、各誤差を測位処理の途中で確認したりできる。測位のロバスト性向上に不可欠な複数手段による同一情報の算出を行うことができ、さらに安価なＳｏＣを使用してコストの削減を図ることができる。

また、マルチＧＮＳＳ受信機１０がオープンスカイで精密単独測位（ＰＰＰ）に未対応であっても、ＰＰＰを行うことができる。また、マルチパス誤差を明らかに含む擬似距離が測位使用衛星から除外されるため、マルチパス環境や自車上空の視界が制限された環境でもＰＰＰを行うことができる。

一般に、ＧＰＳ衛星のＲａｗデータのみを用いるＰＰＰの場合、自車上空の視界が制限されていたり、マルチパス影響を受けたりすると、ＰＰＰに利用できる衛星数が減り、測位精度低下するリスクが高まる。本実施の形態のロケータ装置では、マルチパス影響を受けない高仰角なＧＰＳ衛星以外の衛星を使って測位したマルチＧＮＳＳ受信機１０の測位結果を用いて、ＰＰＰの精度確認を行うことができる。そのため、精度が低下したＰＰＰによる測位結果を複合測位に使用することが未然に抑止され、高層ビルの多い都市部でも測位精度を安定化させることができる。

例えば測位使用衛星数の減少により、ＧＰＳ衛星のみを用いたＰＰＰによる測位結果に位置ずれが生じた場合であっても、信頼できるマルチＧＮＳＳ受信機１０の測位結果を用いて、位置ずれが一定以上大きくなることを防ぐことができるので、都市部でも測位精度が安化できる。

自律航法の位置誤差とは独立して方位誤差を修正できるため、マルチパス影響で位置誤差を安定して算出できない場合でも、方位誤差を連続的に安定して修正できる。自律航法の方位が修正されることにより、自車の走行中に角速度センサの０点出力を補正でき、自律航法の積算誤差を抑えることができる。また、マルチパス影響を受けたＧＮＳＳ測位結果を無理に使用して自律航法の位置精度を低下させずに済むという利点もある。

ＧＮＳＳ受信機の測位結果が、自律航法と整合するような高精密な測位結果か否かを確認した上で、自律航法の修正が行われるので、レーンレベル精度の実現に必要なスムーズな自車位置の更新（修正）を行うことができる。また、自律航法の位置誤差の修正頻度が低下して、レーンレベル精度実現が困難な場合に、信頼できるマルチＧＮＳＳ測位結果により、自律航法を早く初期化することができる。

また、ＧＮＳＳ位置はマルチパス環境などで精度が低下し易いが、本実施の形態では、ＧＮＳＳ位置を独立して誤差評価し、その誤差が小さくＧＮＳＳ位置の信頼性が高いときに、ＧＮＳＳ位置が自律航法の位置修正に使用される。そのため、自律航法の方位誤差、受信機時計誤差、自律センサ誤差を誤修正するリスクは大きく低下する。

［変更例］
実施の形態１では、ＧＳＳＳ受信機として、複数の測位衛星システムに対応したマルチＧＮＳＳ受信機１０が用いられた例を示したが、それに代えて、測位補強信号を入手可能なＧＰＳ受信機を用いてもよい。また、上の説明では、マルチＧＮＳＳ受信機１０の測位結果であるＧＮＳＳ位置が信頼できるときは、各種の演算にＧＮＳＳ位置を使用する例を示したが、マルチＧＮＳＳ受信機１０の測位結果を全く使用せずに、ＧＮＳＳ測位部（ＫＦ法）１０８による測位結果のみを使用してもよい。このような構成にしても、マルチパス環境での測位精度低下を抑止できる。

実施の形態１では、カルマンフィルタによるＧＮＳＳ測位計算において、測位使用衛星の擬似距離として、擬似距離観測値と擬似距離平滑値とが混在する例を示したが、擬似距離平滑値のみを用いるようにしても、レーンレベルの高精度測位を実現できる。

また、実施の形態１では、ドップラー周波数からＧＮＳＳ方位を計算する例を示したが、さらに搬送波位相を用いてドップラー周波数の信頼性を評価してもよい。それにより、自律航法の方位精度が上がり、信頼性の低下したＧＮＳＳ測位結果を無理に使用して自車位置の精度が低下するリスクを下げることができる。

測位部１００を構成するＳｏＣの処理負荷の余裕の範囲内で、ＧＮＳＳ測位部（ＫＦ法）１０８および複合測位部（ＫＦ法）１０９が行うカルマンフィルタ計算の状態変数のパラメータを増やしてもよい。そうすることで、マルチパス環境などでの精度低下をさらに抑制できる。

また、実施の形態１では、自律センサとして、速度センサ１１０と角速度センサ１１３として１軸の角速度センサを用いたが、それらに代えて、３軸ジャイロと３軸加速度センサを使用して、自律航法を３次元で行うようにしてもよい。その場合、自車が起伏、高低差または路面傾斜のある道路を走行しても、高精度な測位が可能になる。

測位補強信号は必ずしもＳＢＡＳ衛星から入手しなくてもよく、ＳＢＡＳと同等以上の精度が得られるのであれば、通信で測位補強信号を入手してもよいし、別の衛星から測位補強信号を入手してもよい。

ロケータ装置が、レーンレベルの車線情報を収録した高精度地図を用いて、レーン単位のマップマッチング（レーンマッチング）を行い、且つ、走行レーンの特定を高い信頼性で行うことができる場合、マップマッチングした走行レーン上の自車位置の座標を基準にして複合測位用のＫＦ解の誤差を求め、その後差をＫＦ解の制御入力に使用してもよい。それにより、マルチパス環境での測位精度低下をさらに抑止できる。レーンマッチングについては、本発明者が出願した特許３５５９１４２号公報、特許３５６８７６８号公報に説明されている。

＜実施の形態２＞
実施の形態２は、実施の形態１における複合測位の機能を拡張する。実施の形態２のロケータ装置の基本的な構成および動作は実施の形態１と同様であるので、ここで、実施の形態１と異なる部分のみを説明する。

実施の形態２では、複合測位部（ＫＦ法）１０９が、図２２に示すように、予め定められたエポック分だけ過去の履歴情報（自律航法による自車位置（ＤＲ位置）、参照するＧＮＳＳ測位結果、Ｒａｗデータを含む）を作成し、複合測位をそれらの履歴情報に対して行う。すなわち、複合測位部（ＫＦ法）１０９は、複合測位によって過去エポックでの自律航法の誤差を算出し、その誤差に基づいて当該過去エポックの履歴情報とともに、それよりも新しい履歴情報および最新の履歴情報についても同様に修正する。

ただし、複合測位が行われた過去エポック（自律航法の誤差が算出された過去エポック）において、参照するＧＮＳＳ位置の誤差が予め定められた閾値を超えていた場合、または、自律航法による自車位置（ＤＲ位置）の誤差の変化量が定められた閾値を超えていた場合には、次のように扱うものとする。すなわち、複合測位部（ＫＦ法）１０９は、最新エポックのＲａｗデータ（擬似距離）を確認し、最新エポックのＲａｗデータに予め定められた閾値以上の誤差がなければ、当該過去エポックの誤差はマルチパス誤差であると判断して、履歴情報に対する修正を行わない。例えば、図２３に示すように、複合測位が行われた過去エポックで、マルチパス誤差によりＧＮＳＳ位置の精度が低下していた場合、複合測位部（ＫＦ法）１０９は、最新エポックの履歴情報を確認し、確認した履歴情報においてＧＮＳＳ測位結果の精度が回復していれば、当該過去エポックでの自律航法の誤差に基づく履歴情報の修正を見送る。

あるいは、上記のような場合には、当該過去エポックの次エポックでも同じ誤差が生じたかを確認し、当該過去エポックと同じ誤差が生じていれば、当該過去エポックの誤差はマルチパス誤差ではなく自律航法の誤差であると判断して、履歴情報に対する修正を行う（次エポックに当該過去エポックと同じ誤差が生じていなければ、履歴情報に対する修正を行わない）ようにしてもよい。

実施の形態２に係るロケータ装置では、複合測位により得られた過去エポックでの自律航法の誤差を用いて、それよりも新しいエポックの履歴情報を修正するため、一時的なＧＮＳＳ測位精度の低下の影響を受けにくい、安定した測位性能を実現できる。

＜実施の形態３＞
図２４は、実施の形態３に係るロケータ装置の構成を示すブロック図である。図２４のロケータ装置の構成は、図１の構成に対し、測位部１００のＧＮＳＳ測位部（ＫＦ法）１０８を省略し、それを複合測位部（ＫＦ法）１０９に統合した構成となっている。

実施の形態３の複合測位部（ＫＦ法）１０９は、ＳＢＡＳ衛星の測位補強信号（衛星軌道誤差、衛星時計誤差、電離層伝搬遅延誤差、対流圏伝搬遅延誤差）と、ＧＮＳＳ受信機時計誤差修正部１０４が算出したマルチＧＮＳＳ受信機１０の精密時計誤差（バイアス誤差およびドリフト誤差）とを用いて、測位計算に使用する擬似距離または擬似距離平滑値、およびレンジレートに含まれる各種の誤差を修正する。さらに、複合測位部（ＫＦ法）１０９は、ＧＮＳＳ衛星挙動推定部１０２が算出したＧＮＳＳ衛星の位置および速度を用いて、自律航法部１１６が算出した自車位置（ＤＲ位置）および自車速度（ＤＲ速度）の誤差をＰＰＰ測位により収束計算する。

実施の形態３に係るロケータ装置の動作を説明する。図２５は、測位部１００のメイン処理を示すフローチャートである。図２６は、複合測位部（ＫＦ法）１０９の処理を示すフローチャートである。図２５のフローは予め定められた周期で実行され、図２６のフローは図２５のステップＳＴ５１４で実行される。

図２５のメイン処理において、ステップＳＴ５０１〜ＳＴ５１３の処理は、図２のステップＳＴ１０１〜ＳＴ１１３と同じである。なお、本実施の形態では、ＧＮＳＳ測位部（ＫＦ法）１０８が省略されているため、ＫＦ解の有無を表すＧＮＳＳ測位解のステータスは設定されない。

ステップＳＴ５１３で、高精度測位（ＰＰＰ測位）が可能であると判断された場合（ステップＳＴ５１３でＹＥＳ）、以下のステップＳＴ５１４からステップＳＴ５１６が行われ、ＰＰＰ測位が不可能と判断された場合には（ステップＳＴ５１３でＮＯ）、ステップＳＴ５１７へ進む。

ステップＳＴ５１４は、図２のステップＳＴ１１４とステップＳＴ１１６とを統合したものである。ステップＳＴ５１４では、複合測位部（ＫＦ法）１０９が、測位使用衛星の擬似距離（観測値または平滑値）、レンジレート、ＧＮＳＳ衛星の軌道（位置および速度）、測位補強信号から算出した各種の誤差、および受信機時計誤差を用いて、自律航法部１１６が算出した自車位置（ＤＲ位置）と自車速度（ＤＲ速度）の修正量を、カルマンフィルタ法によりＰＰＰ測位を行う。この処理の詳細は、後で図２６を用いて説明する。

ステップＳＴ５１５は、図２のステップＳＴ１１５と同様の処理であり、複合測位部（ＫＦ法）１０９が、ステップＳＴ５０９で算出されたＧＮＳＳ−ＬＳ解方位を用いて、自律航法における自車方位（ＤＲ方位）の誤差を算出する。この処理は、ステップＳＴ５０８でＧＮＳＳ方位が信頼できると判断され、且つ、ステップＳＴ５０９で算出されたＧＮＳＳ−ＬＳ解方位とマルチＧＮＳＳ受信機１０が出力したＧＮＳＳ方位との差異が規定値以下である場合に行われる。

ステップＳＴ５１６は、図２のステップＳＴ１１７と同様の処理であり、自律航法部１１６が、ステップＳＴ５１４およびＳＴ５１５で算出された自律航法の誤差（ＤＲ位置、ＤＲ速度およびＤＲ方位の各誤差）を用いて、自律航法による測位結果を修正する。

ステップＳＴ５１７およびステップＳＴ５１８では、それぞれ図２のステップＳＴ１１８およびステップＳＴ１１９と同様の処理が行われる。ステップＳＴ５１８の処理が完了すると、図２５のフローは終了する。

次に、図２５のステップＳＴ５１４の処理の詳細を、図２６を用いて説明する。図２５のステップＳＴ５１４では、図２６に示すステップＳＴ６０１からステップＳＴ６０９の処理が行われる。

ステップＳＴ６０１では、複合測位部（ＫＦ法）１０９が、測位部１００に設定されている複合測位解のステータスを「ＫＦ解なし」にする。

ステップＳＴ６０２は、図３のステップＳＴ２０２と同様の処理であり、ＧＮＳＳ衛星挙動推定部１０２が、マルチＧＮＳＳ受信機１０が出力した航法メッセージと測位補強信号（衛星軌道誤差）とを用いて、ＧＮＳＳ−Ｔｉｍｅにおける測位使用衛星の挙動（位置と速度）を算出する。なお、ＧＮＳＳ−Ｔｉｍｅは、収束計算中に徐々に正確に算出されるようになる。

ステップＳＴ６０３およびステップＳＴ６０４では、複合測位部（ＫＦ法）１０９が、図３のステップＳＴ２０３およびステップＳＴ２０４と同様の手法により、マルチＧＮＳＳ受信機１０が出力した各衛星の擬似距離を修正した上で、測位使用衛星を選択する。

ステップＳＴ６０５では、複合測位部（ＫＦ法）１０９が、カルマンフィルタ計算により、自律航法による自車位置（ＤＲ位置）および自車速度（ＤＲ速度）の各修正量を算出する。このカルマンフィルタの計算式は一般的なものである。すなわち、カルマンフィルタの状態変数は、自律航法部１１６の位置、速度、および受信機時計誤差（バイアス誤差およびドリフト誤差）であり、位置ずれを修正する制御入力がある。観測値は、測位使用衛星の擬似距離とレンジレートである。

ＫＦ解によるＤＲ位置の修正量は、図２０に示すような、速度センサ１１０および角速度センサ１１３の誤差に起因する自車位置の移動量の誤差（速度ベクトルの誤差）と、ＤＲ位置およびＤＲ速度それぞれのプロセス誤差と、ＧＮＳＳ−ＫＦ解位置の観測誤差とに応じたカルマンフィルタゲインにより決まる。しかし、参照するＧＮＳＳ位置が信頼でき、図２１のようにＧＮＳＳ−ＫＦ解位置とＤＲ位置との間に規定値以上の安定的な差異が生じている場合は、複合測位部（ＫＦ法）１０９は、その差異の平均値をカルマンフィルタの状態変数に制御入力することでＤＲ位置を修正する。

ステップＳＴ６０６では、複合測位部（ＫＦ法）１０９が、現エポックにおける収束計算回数を確認する。収束計算回数が規定値以下であれば（ステップＳＴ６０６でＹＥＳ）、ステップＳＴ６０７へ進む。収束計算回数が規定値を超えていれば（ステップＳＴ６０６でＮＯ）、収束できなかったと判断して、図２６の処理を抜ける。

ステップＳＴ６０７では、複合測位部（ＫＦ法）１０９が、現エポックでのＫＦ解の変化量を確認する。ＫＦ解の変化量が規定値以下であれば（ステップＳＴ６０７でＹＥＳ）、収束できたと判断して、ステップＳＴ６０８へ進む。ＫＦ解の変化量が規定値を超えていれば（ステップＳＴ６０７でＮＯ）、収束できなかったと判断して、時刻を更新してステップＳＴ６０２へ戻る。

ステップＳＴ６０８では、複合測位部（ＫＦ法）１０９が、複合測位解のステータスを「ＫＦ解あり」とする。

ステップＳＴ６０９は、複合測位部（ＫＦ法）１０９が、図９〜図１１を用いて説明したＧＮＳＳ位置の誤差および信頼性の評価方法と同様の手法により、ＫＦ解の誤差および信頼性を評価する。

実施の形態３では、カルマンフィルタの計算式を１つに統合したことによって、部分的な結果（自車位置または自車方位）を得ることはできず、また実施の形態１よりも計算負荷が多少上がるが、そのことを除いて、実施の形態１と同様な効果が得られる。

＜ハードウェア構成の例＞
図２７および図２８は、それぞれ測位部１００のハードウェア構成の一例を示す図である。図１に示した測位部１００の各要素は、例えば図２７に示す処理回路５０により実現される。すなわち、処理回路５０は、ＧＮＳＳ衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出した自車の位置、ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび測位補強信号を出力するマルチＧＮＳＳ受信機１０に接続されており、自律航法により自車の位置を算出する自律航法部１１６と、Ｒａｗデータに含まれる搬送波位相を用いてＧＮＳＳ衛星と自車の位置との擬似距離を平滑化する擬似距離平滑化部１０５と、マルチＧＮＳＳ受信機１０が算出した自車の位置の信頼性を評価するＧＮＳＳ受信機測位誤差評価部１０７と、擬似距離の平滑値、測位補強信号およびＧＮＳＳ衛星の軌道に基づいて自車の位置を算出するＧＮＳＳ測位部（ＫＦ法）１０８と、自律航法部１１６が行う自律航法の誤差を、ＧＮＳＳ測位部（ＫＦ法）１０８が算出した自車の位置に基づいて算出し、自律航法の誤差に基づいて自律航法部１１６が算出した自車の位置を修正する複合測位部（ＫＦ法）１０９と、を備える。処理回路５０には、専用のハードウェアが適用されてもよいし、メモリに格納されるプログラムを実行するプロセッサ、中央処理装置（ＣＰＵ（Central Processing Unit）、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Digital Signal Processor））が適用されてもよい。

処理回路５０が専用のハードウェアである場合、処理回路５０は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）、またはこれらを組み合わせたものなどが該当する。測位部１００の各要素の機能のそれぞれは、複数の処理回路で実現されてもよいし、それらの機能がまとめて一つの処理回路で実現されてもよい。

図２８は、処理回路５０がプロセッサ５１を用いて構成されている場合における測位部１００のハードウェア構成を示している。この場合、測位部１００の各要素の機能は、ソフトウェア等（ソフトウェア、ファームウェア、またはソフトウェアとファームウェア）との組み合わせにより実現される。ソフトウェア等はプログラムとして記述され、メモリ５２に格納される。処理回路５０としてのプロセッサ５１は、メモリ５２に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、測位部１００は、処理回路５０により実行されるときに、マルチＧＮＳＳ受信機１０から、ＧＮＳＳ衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出した自車の位置、ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび測位補強信号を受ける処理と、自律航法により自車の位置を算出する第１の測位処理と、Ｒａｗデータに含まれる搬送波位相を用いてＧＮＳＳ衛星と自車の位置との擬似距離を平滑化する処理と、マルチＧＮＳＳ受信機１０が算出した自車の位置の信頼性を評価する処理と、擬似距離の平滑値、測位補強信号およびＧＮＳＳ衛星の軌道に基づいて自車の位置を算出する第２の測位処理と、第１の測位処理における自律航法の誤差を、第２の測位処理で算出した自車の位置に基づいて算出する処理と、自律航法の誤差に基づいて第１の測位処理で算出した自車の位置を修正する処理と、が結果的に実行されることになるプログラムを格納するためのメモリ５２を備える。換言すれば、このプログラムは、測位部１００の各要素の動作の手順や方法をコンピュータに実行させるものであるともいえる。

ここで、メモリ５２は、例えば、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、フラッシュメモリー、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）などの、不揮発性または揮発性の半導体メモリ、ＨＤＤ（Hard Disk Drive）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital Versatile Disc）およびそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であってもよい。

以上、測位部１００の各要素の機能が、ハードウェアおよびソフトウェア等のいずれか一方で実現される構成について説明した。しかしこれに限ったものではなく、測位部１００の一部の要素を専用のハードウェアで実現し、別の一部の要素をソフトウェア等で実現する構成であってもよい。例えば、一部の要素については専用のハードウェアとしての処理回路５０でその機能を実現し、他の一部の要素についてはプロセッサ５１としての処理回路５０がメモリ５２に格納されたプログラムを読み出して実行することによってその機能を実現することが可能である。

このように、測位部１００は、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。

以上のように、本発明に係る測位装置は、ＧＮＳＳ（Global Navigation Satellite System ）衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出された車両の位置、速度および方位、ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび測位補強信号を出力するＧＮＳＳ受信機と、自律航法により車両の位置を算出する自律航法部と、Ｒａｗデータに含まれる搬送波位相を用いてＧＮＳＳ衛星と車両の位置との擬似距離を平滑化する擬似距離平滑化部と、ＧＮＳＳ受信機が算出した車両の位置の信頼性を評価するＧＮＳＳ受信機測位誤差評価部と、擬似距離の平滑値、測位補強信号およびＧＮＳＳ衛星の軌道に基づいて車両の位置を算出するＧＮＳＳ測位部と、自律航法部が行う自律航法の誤差を、ＧＮＳＳ測位部が算出した車両の位置に基づいて算出し、自律航法の誤差に基づいて自律航法部が算出した車両の位置を修正する複合測位部と、を備える。

また、複合測位部は、自律航法部が算出した車両の位置の履歴およびＧＮＳＳ受信機が算出した車両の位置の履歴から、過去エポックでの自律航法の誤差を算出し、当該過去エポックでの自律航法の誤差に基づいて、自律航法部が算出した車両の位置の履歴および最新の車両の位置を修正する。

また、ＧＮＳＳ受信機測位誤差評価部は、（ａ）ＧＮＳＳ受信機が算出した車両の位置の移動ベクトルとＧＮＳＳ受信機が算出した車両の速度および方位を表す速度ベクトルとの差異、（ｂ）ＧＮＳＳ受信機が算出した車両の位置と自律航法部が算出した車両の位置との差異、および、（ｃ）ＧＮＳＳ受信機が測位に使用したＧＮＳＳ衛星の幾何学的配置に基づくＤＯＰ（Dilution of Precision）、のいずれかに基づいて、ＧＮＳＳ受信機が算出した車両の位置の信頼性を評価する。

また、ＧＮＳＳ測位部は、擬似距離または擬似距離の平滑値から、測位補強信号から既知の誤差と、ＧＮＳＳ衛星の位置と自律航法部が算出した車両の位置との２点間距離とを差し引いた残差である未知誤差が、予め定められた閾値以下となるＧＮＳＳ衛星を測位使用衛星に選択する。

また、ＧＮＳＳ測位部は、車両の位置をカルマンフィルタ法の収束計算により算出し、ＧＮＳＳ受信機測位誤差評価部がＧＮＳＳ受信機の測位結果を信頼できると判断した場合、ＧＮＳＳ測位部は、収束計算で得られた車両の位置が、ＧＮＳＳ受信機の測位誤差の範囲に入らなければ、当該車両の位置を無効と判断する。

また、複合測位部は、ＧＮＳＳ衛星のドップラー周波数から計算された車両の方位を基準にして、自律航法の方位誤差を算出し、ＧＮＳＳ測位部が算出した車両の位置を観測値とするカルマンフィルタ法の収束計算により、自律航法の位置誤差を算出する。

また、複合測位部は、ＧＮＳＳ測位部が算出した車両の位置が無効と判断された場合、ＧＮＳＳ受信機が算出した車両の位置と自律航法部が算出した車両の位置との間の距離が予め定められた閾値以上離れていれば、ＧＮＳＳ受信機の測位結果を用いてカルマンフィルタを初期化する。

また、複合測位部は、ＧＮＳＳ測位部が算出した車両の位置と自律航法部が算出した車両の位置との間に固定的な差異が生じている場合は、当該差異をカルマンフィルタの状態変数に制御入力することで、自律航法の誤差を算出する。

また、複合測位部は、自律航法の誤差を算出した過去エポックにおいて、ＧＮＳＳ測位部が算出した車両の位置の誤差が予め定められた閾値を超えていた場合、または、自律航法部が算出した車両の位置の誤差の変化量が予め定められた閾値を超えていた場合には、最新エポックの擬似距離を確認し、最新エポックの擬似距離に閾値以上の誤差がなければ、自律航法部が算出した車両の位置の履歴および最新の車両の位置の修正を行わない。

また、複合測位部は、自律航法の誤差を算出した過去エポックにおいて、ＧＮＳＳ測位部が算出した車両の位置の誤差が予め定められた閾値を超えていた場合、または、自律航法部が算出した車両の位置の誤差の変化量が予め定められた閾値を超えていた場合には、当該過去エポックの次エポックでも同じ誤差が生じたかを確認し、同じ誤差が生じていなければ、自律航法部が算出した車両の位置の履歴および最新の車両の位置の修正を行わない。

さらに、本発明に係る測位装置における測位方法においては、測位装置のＧＮＳＳ受信機が、ＧＮＳＳ（Global Navigation Satellite System ）衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出された車両の位置、速度および方位、ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび測位補強信号を出力し、測位装置の自律航法部が、自律航法により車両の位置を算出し、測位装置の擬似距離平滑化部が、Ｒａｗデータに含まれる搬送波位相を用いてＧＮＳＳ衛星と車両の位置との擬似距離を平滑化し、測位装置のＧＮＳＳ受信機測位誤差評価部が、ＧＮＳＳ受信機が算出した車両の位置の信頼性を評価し、測位装置のＧＮＳＳ測位部が、擬似距離の平滑値、測位補強信号およびＧＮＳＳ衛星の軌道に基づいて車両の位置を算出し、測位装置の複合測位部が、自律航法部が行う自律航法の誤差を、ＧＮＳＳ測位部が算出した車両の位置に基づいて算出し、自律航法の誤差に基づいて自律航法部が算出した車両の位置を修正する。

なお、本発明は、その発明の範囲内において、各実施の形態を自由に組み合わせたり、各実施の形態を適宜、変形、省略することが可能である。

本発明は詳細に説明されたが、上記した説明は、すべての態様において、例示であって、この発明がそれに限定されるものではない。例示されていない無数の変形例が、この発明の範囲から外れることなく想定され得るものと解される。

１０ マルチＧＮＳＳ受信機、１００ 測位部、１０１ ＧＮＳＳ出力データ変換部、１０２ ＧＮＳＳ衛星挙動推定部、１０３ ＧＮＳＳ測位部（ＬＳ法）、１０４ ＧＮＳＳ受信機時計誤差修正部、１０５ 擬似距離平滑化部、１０６ 測位使用衛星選択／Ｒａｗ誤差評価部、１０７ ＧＮＳＳ受信機測位誤差評価部、１０８ ＧＮＳＳ測位部（ＫＦ法）、１０９ 複合測位部（ＫＦ法）、１１０ 速度センサ、１１１ 距離計測部、１１２ 速度センサＳＦ補正部、１１３ 角速度センサ、１１４ ヨー角計測部、１１５ 角速度センサ補正部、１１６ 自律航法部、５０ 処理回路、５１ プロセッサ、５２ メモリ。

Claims (10)

1. ＧＮＳＳ（Global Navigation Satellite System ）衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出された車両の位置、速度および方位、前記ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび前記測位補強信号を出力するＧＮＳＳ受信機と、
   自律航法により前記車両の位置を算出する自律航法部と、
   前記Ｒａｗデータに含まれる搬送波位相を用いて前記ＧＮＳＳ衛星と前記車両の位置との擬似距離を平滑化する擬似距離平滑化部と、
   前記ＧＮＳＳ受信機が算出した前記車両の位置の信頼性を評価するＧＮＳＳ受信機測位誤差評価部と、
   前記擬似距離の平滑値、前記測位補強信号、前記ＧＮＳＳ衛星の軌道および前記ＧＮＳＳ受信機が算出した前記車両の位置の信頼性の評価結果に基づいて前記車両の位置を算出するＧＮＳＳ測位部と、
   前記自律航法部が行う前記自律航法の誤差を、前記ＧＮＳＳ測位部が算出した前記車両の位置に基づいて算出し、前記自律航法の誤差に基づいて前記自律航法部が算出した前記車両の位置を修正する複合測位部と、
   を備え、
   前記複合測位部によって修整された前記車両の位置を出力する測位装置であって、
   前記複合測位部は、前記自律航法部が算出した前記車両の位置の履歴および前記ＧＮＳＳ受信機が算出した前記車両の位置の履歴から、過去エポックでの前記自律航法の誤差を算出し、当該過去エポックでの前記自律航法の誤差に基づいて、前記自律航法部が算出した前記車両の位置の履歴および最新の前記車両の位置を修正する、
   測位装置。
2. 前記ＧＮＳＳ受信機測位誤差評価部は、
   （ａ）前記ＧＮＳＳ受信機が算出した前記車両の位置の移動ベクトルと前記ＧＮＳＳ受信機が算出した前記車両の速度および方位を表す速度ベクトルとの差異、
   （ｂ）ＧＮＳＳ受信機が算出した前記車両の位置と前記自律航法部が算出した前記車両の位置との差異、および、
   （ｃ）ＧＮＳＳ受信機が測位に使用したＧＮＳＳ衛星の幾何学的配置に基づくＤＯＰ（Dilution of Precision）、
   のいずれかに基づいて、前記ＧＮＳＳ受信機が算出した前記車両の位置の信頼性を評価する、
   請求項１記載の測位装置。
3. ＧＮＳＳ測位部は、前記擬似距離または前記擬似距離の平滑値から、前記測位補強信号から既知の誤差と、前記ＧＮＳＳ衛星の位置と自律航法部が算出した前記車両の位置との２点間距離とを差し引いた残差である未知誤差が、予め定められた閾値以下となる前記ＧＮＳＳ衛星を測位使用衛星に選択する、
   請求項１記載の測位装置。
4. 前記ＧＮＳＳ測位部は、前記車両の位置をカルマンフィルタ法の収束計算により算出し、
   前記ＧＮＳＳ受信機測位誤差評価部が前記ＧＮＳＳ受信機の測位結果を信頼できると判断した場合、前記ＧＮＳＳ測位部は、前記収束計算で得られた前記車両の位置が、前記ＧＮＳＳ受信機の測位誤差の範囲に入らなければ、当該車両の位置を無効と判断する、
   請求項１記載の測位装置。
5. 複合測位部は、
   前記ＧＮＳＳ衛星のドップラー周波数から計算された前記車両の方位を基準にして、前記自律航法の方位誤差を算出し、
   前記ＧＮＳＳ測位部が算出した前記車両の位置を観測値とするカルマンフィルタ法の収束計算により、前記自律航法の位置誤差を算出する、
   請求項１記載の測位装置。
6. 複合測位部は、前記ＧＮＳＳ測位部が算出した前記車両の位置が無効と判断された場合、前記ＧＮＳＳ受信機が算出した前記車両の位置と前記自律航法部が算出した前記車両の位置との間の距離が予め定められた閾値以上離れていれば、前記ＧＮＳＳ受信機の測位結果を用いてカルマンフィルタを初期化する、
   請求項４記載の測位装置。
7. 複合測位部は、前記ＧＮＳＳ測位部が算出した前記車両の位置と前記自律航法部が算出した前記車両の位置との間に固定的な差異が生じている場合は、当該差異をカルマンフィルタの状態変数に制御入力することで、前記自律航法の誤差を算出する、
   請求項５記載の測位装置。
8. 複合測位部は、前記自律航法の誤差を算出した過去エポックにおいて、前記ＧＮＳＳ測位部が算出した前記車両の位置の誤差が予め定められた閾値を超えていた場合、または、前記自律航法部が算出した前記車両の位置の誤差の変化量が予め定められた閾値を超えていた場合には、最新エポックの擬似距離を確認し、最新エポックの擬似距離に閾値以上の誤差がなければ、前記自律航法部が算出した前記車両の位置の履歴および最新の前記車両の位置の修正を行わない、
   請求項１記載の測位装置。
9. 複合測位部は、前記自律航法の誤差を算出した過去エポックにおいて、前記ＧＮＳＳ測位部が算出した前記車両の位置の誤差が予め定められた閾値を超えていた場合、または、前記自律航法部が算出した前記車両の位置の誤差の変化量が予め定められた閾値を超えていた場合には、当該過去エポックの次エポックでも同じ誤差が生じたかを確認し、同じ誤差が生じていなければ、前記自律航法部が算出した前記車両の位置の履歴および最新の前記車両の位置の修正を行わない、
   請求項１記載の測位装置。
10. 測位装置における測位方法であって、
    前記測位装置のＧＮＳＳ受信機が、ＧＮＳＳ（Global Navigation Satellite System ）衛星から受信した電波および測位補強衛星から受信した測位補強信号に基づいて算出された車両の位置、速度および方位、前記ＧＮＳＳ衛星の電波から抽出したＲａｗデータおよび前記測位補強信号を出力し、
    前記測位装置の自律航法部が、自律航法により前記車両の位置を算出し、
    前記測位装置の擬似距離平滑化部が、前記Ｒａｗデータに含まれる搬送波位相を用いて前記ＧＮＳＳ衛星と前記車両の位置との擬似距離を平滑化し、
    前記測位装置のＧＮＳＳ受信機測位誤差評価部が、前記ＧＮＳＳ受信機が算出した前記車両の位置の信頼性を評価し、
    前記測位装置のＧＮＳＳ測位部が、前記擬似距離の平滑値、前記測位補強信号、前記ＧＮＳＳ衛星の軌道および前記ＧＮＳＳ受信機が算出した前記車両の位置の信頼性の評価結果に基づいて前記車両の位置を算出し、
    前記測位装置の複合測位部が、前記自律航法部が行う前記自律航法の誤差を、前記ＧＮＳＳ測位部が算出した前記車両の位置に基づいて算出し、前記自律航法の誤差に基づいて前記自律航法部が算出した前記車両の位置を修正し、
    前記測位装置が、前記複合測位部によって修整された前記車両の位置を出力する測位方法であって、
    前記複合測位部は、前記自律航法部が算出した前記車両の位置の履歴および前記ＧＮＳＳ受信機が算出した前記車両の位置の履歴から、過去エポックでの前記自律航法の誤差を算出し、当該過去エポックでの前記自律航法の誤差に基づいて、前記自律航法部が算出した前記車両の位置の履歴および最新の前記車両の位置を修正する、
    測位方法。

Images