(57)【要約】【課題】位置推定の誤差を小さくできる測位装置を提供する。【解決手段】ＧＮＳＳ受信機２が受信する航法信号に基づいて衛星航法により測位した位置を取得する衛星測位部１０４と、衛星航法において位置が演算される前の情報である衛星情報と挙動検出センサ７の検出値とに基づいて移動軌跡を推定し、基準点と移動軌跡とに基づいて位置を推定する位置演算部１０８と、衛星測位部１０４が測位した位置の誤差である衛星測位誤差Ｅｓと、自律航法での位置の誤差である自律測位誤差Ｅａを推定する誤差推定部１０７とを備え、位置演算部１０８は、衛星測位誤差Ｅｓが自律測位誤差Ｅａよりも小さいことに基づいて、衛星測位部１０４が取得した位置を基準点とする。【選択図】図１
2019015635.tif 000002
【特許請求の範囲】
【請求項１】
ＧＮＳＳ受信機（２）が受信する航法信号に基づいて衛星航法により測位した位置を取得する衛星測位部（１０４）と、
前記衛星航法において前記位置が演算される前の情報である衛星情報と挙動検出センサ（７）の検出値とに基づいて移動軌跡を推定し、基準点と前記移動軌跡とに基づいて前記位置を推定する自律測位部（１０８）と、
前記衛星測位部が測位した前記位置の誤差である衛星測位誤差と、前記自律測位部が推定した前記位置の誤差である自律測位誤差を推定する誤差推定部（１０７）とを備え、
前記自律測位部は、前記衛星測位誤差が前記自律測位誤差よりも小さいことに基づいて、前記衛星測位部が取得した前記位置を前記基準点とする測位装置。
【請求項２】
前記自律測位部は、前記衛星測位誤差が前記自律測位誤差よりも小さい状態が、予め設定した継続判断時間よりも長い時間継続したことに基づいて、前記衛星測位部が取得した前記位置を前記基準点とする請求項１に記載の測位装置。
【請求項３】
前記航法信号のドップラーシフト量に基づいて移動方位を推定する方位推定部（１０５）を備え、
前記自律測位部は、移動方位と速度とに基づいて前記移動軌跡を推定するものであり、前記挙動検出センサとしてヨーレートセンサの検出値を取得可能であり、前記方位推定部が移動方位を推定できたときは、前記方位推定部が推定した移動方位を用いて前記移動軌跡を推定する一方、前記方位推定部が移動方位を推定できないときは、前記ヨーレートセンサの検出値を用いて方位角変化量を推定し、前記方位角変化量と過去の基準方位とに基づいて移動方位を決定し、
前記誤差推定部は、前記自律測位部が、前記方位推定部が推定した移動方位を用いて前記移動軌跡を推定したときは、前記ヨーレートセンサの検出値を用いて移動方位を推定したときよりも、前記自律測位誤差の増加量を小さくする請求項１または２に記載の測位装置。
【請求項４】
請求項１〜３のいずれか１項において、
前記誤差推定部は、擬似距離残差の変動の程度、前記航法信号のＳ／Ｎ、精度低下率の少なくとも１つに基づいて定まる精度劣化係数を、前記擬似距離残差に乗じることで、前記衛星測位誤差を算出する測位装置。
【発明の詳細な説明】
【技術分野】
【０００１】
測位装置に関し、特に測位の誤差を低減する技術に関する。
【背景技術】
【０００２】
ＧＮＳＳ（Global Navigation Satellite System）が備える航法衛星が送信する航法信号を受信するＧＮＳＳ受信機を備え、ＧＮＳＳ受信機が受信した航法信号に基づいて現在位置を測定する技術が広く知られている。この技術を用いて移動体の位置の変化を逐次測定する技術は、衛星航法と呼ばれることもある。
【０００３】
また、移動体の挙動を検出する種々の挙動検出センサが検出する検出値に基づいて移動体の移動軌跡を推定し、基準点と移動軌跡とに基づいて位置を推定する技術も広く知られている。この技術は、自律航法と呼ばれることもある。衛星航法と自律航法とを組み合わせて位置の変化を逐次測定する技術も知られている。
【０００４】
特許文献１には、衛星航法と自律航法とを組み合わせて車両の位置を測位するシステムが開示されている。特許文献１では、衛星航法の測定データが使用可能か否かを判定し、使用不可の場合、衛星航法から自律航法による測位へ切り替える。換言すれば、特許文献１では、衛星航法の測定データが使用可能であれば衛星航法を行う。衛星航法の測定データが使用可能か否かを判断するために、航法衛星の補足数が測位可能な数以上であって、補足した航法衛星による測位精度が所定以上であることを条件の一つとしている。
【０００５】
また、たとえば特許文献２、３に記載されているように、航法信号から得られる擬似距離などの位置推定前のデータと、挙動検出センサから推定される位置変化量とを統合して位置推定を行う方法も知られている。本明細書では、この方法を、タイトカップリングとする。なお、タイトカップリングも、挙動検出センサの検出値を用いるので、自律航法の一種である。
【０００６】
衛星航法は、４つ以上の衛星から航法信号を同じタイミングで取得して、移動体の三次元座標と時計誤差を未知数とする連立方程式を解くことで、移動体の位置を決定する方法が最も広く知られている。
【０００７】
この他に、衛星航法として、特許文献４に開示されている方法も知られている。特許文献４には、航法信号のドップラー周波数の変位量を用いる方法が開示されている。特許文献４に開示されている方法では、ドップラー周波数の変位量から車両の速度ベクトルを算出し、この速度ベクトルから自車両軌跡を算出する。そして、この自車両軌跡上の点から衛星までの距離と疑似距離との差を評価値とし、評価値が最小になるように自車両軌跡を平行移動させる。この方法も、航法信号を用いている一方、挙動検出センサの検出値は用いていないので、衛星航法の一種である。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２０１７−３３９５号公報
【特許文献２】特開２０１３−１１３７８９号公報
【特許文献３】特開２０１７−９２９４号公報
【特許文献４】特開２０１１−２０９２６８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
タイトカップリングにより位置推定を行う場合には、航法信号を使わない自律航法に比べて位置推定の誤差が小さい。そのため、自律航法としてタイトカップリングによる位置推定方法を行う場合、特許文献１のように、衛星航法の測定データが使用可能であれば衛星航法を行うと、位置推定の誤差を大きくしてしまう恐れがある。
【００１０】
本開示は、この事情に基づいて成されたものであり、その目的とするところは、位置推定の誤差を小さくできる測位装置を提供することにある。
【課題を解決するための手段】
【００１１】
上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は更なる有利な具体例を規定する。特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、開示した技術的範囲を限定するものではない。
【００１２】
上記目的を達成するための１つの開示は、
ＧＮＳＳ受信機（２）が受信する航法信号に基づいて衛星航法により測位した位置を取得する衛星測位部（１０４）と、
衛星航法において位置が演算される前の情報である衛星情報と挙動検出センサ（７）の検出値とに基づいて移動軌跡を推定し、基準点と移動軌跡とに基づいて位置を推定する自律測位部（１０８）と、
衛星測位部が測位した位置の誤差である衛星測位誤差と、自律測位部が推定した位置の誤差である自律測位誤差を推定する誤差推定部（１０７）とを備え、
自律測位部は、衛星測位誤差が自律測位誤差よりも小さいことに基づいて、衛星測位部が取得した位置を基準点とする測位装置である。
【００１３】
本開示の自律測位部は、単純な自律航法ではなく、衛星航法において位置が演算される前の情報である衛星情報と挙動検出センサの検出値とに基づいて移動軌跡を推定する。そのため、単純な自律航法よりも精度がよいことから、衛星航法により測位した位置よりも誤差が小さい可能性もある。
【００１４】
そこで、衛星測位誤差と自律測位誤差を比較する。比較の結果、衛星測位誤差が自律測位誤差よりも小さいことに基づいて、衛星測位部が取得した位置を自律測位部で用いる基準点とする。このように、衛星測位誤差と自律測位誤差を比較して、衛星測位部が取得した位置を基準点とするか否かを決定するので、位置推定の誤差を小さくできる。
【図面の簡単な説明】
【００１５】
【図１】実施形態の測位装置１００の構成図である。
【図２】図１の測位装置１００が実行する処理を示すフローチャートである。
【図３】図１の測位装置１００が、図２の処理と並列に実行する処理を示すフローチャートである。
【図４】図３のＳ２８の処理を詳しく示すフローチャートである。
【図５】図３のＳ３２の処理を詳しく示すフローチャートである。
【図６】図３を実行した後に実行する処理を示すフローチャートである。
【図７】第１実施形態を実行した場合の誤差例変化例を示す図である。
【図８】第２実施形態において、図６に代えて実行する処理を示すフローチャートである。
【図９】第２実施形態を実行した場合の誤差例変化例を示す図である。
【図１０】第３実施形態を実行した場合の誤差例変化例を示す図である。
【発明を実施するための形態】
【００１６】
以下、実施形態を図面に基づいて説明する。図１に示す測位装置１００は、ＧＮＳＳ受信機２、車速センサ３、ジャイロセンサ４、加速度センサ５、シフトポジションセンサ６とともに、移動体である車両１に搭載されている。
【００１７】
＜構成の概要＞
ＧＮＳＳ受信機２は、ＧＮＳＳ（Global Navigation Satellite System）が備える航法衛星Ｓｉが送信する航法信号を受信する。なお、ｉは航法衛星の番号である。ＧＮＳＳは、たとえばＧＰＳである。航法信号は搬送波に重畳されて電波として航法衛星から送信される。以下、航法衛星が送信する電波をＧＮＳＳ電波とする。ＧＮＳＳ受信機２は、受信したＧＮＳＳ電波を復調して航法信号を取り出す。
【００１８】
そして、取り出した航法信号から、擬似距離ρｉ、ドップラーシフト量Ｄｉ、衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、衛星の状態、航法データなどを決定する。航法データには、航法衛星の衛星番号、航法衛星の軌道情報であるエフェメリス、航法衛星が電波を送信した時刻などが含まれている。
【００１９】
各航法衛星Ｓｉの衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）は、各航法衛星ＳｉのエフェメリスおよびＧＮＳＳ電波を送信した時刻に基づいて算出する。擬似距離ρｉは、航法衛星ＳｉがＧＮＳＳ電波を送信した時刻と、ＧＮＳＳ受信機２がＧＮＳＳ電波を受信した時刻との時刻差すなわち電波伝播時間に、光速を乗じることで算出する。
【００２０】
ドップラーシフト量Ｄｉは、航法衛星Ｓｉが送信した電波の搬送波の周波数と、受信したＧＮＳＳ電波の搬送波の周波数の周波数差である。航法衛星Ｓｉが送信する電波の搬送波周波数は予め定まっており、この周波数は、ＧＮＳＳ受信機２が備える所定の記憶部に予め記憶されている。
【００２１】
ＧＮＳＳ受信機２は、これらを、受信信号である航法信号のＳ／Ｎ、航法信号を受信した時刻とともに測位装置１００に一定周期で出力する。ＧＮＳＳ受信機２が測位装置１００に出力する情報は、衛星航法において位置が演算される前の情報である。以下、衛星航法において位置が演算される前の情報を衛星情報とする。ＧＮＳＳ受信機２が衛星情報を出力する一定周期は、たとえば、２００ミリ秒〜４００ミリ秒の間である。航法衛星は複数存在している。ＧＮＳＳ受信機２は、ＧＮＳＳ電波から復調できる全ての航法信号から衛星情報を決定し、決定した全ての衛星情報を測位装置１００に出力する。
【００２２】
車速センサ３は、車両１の車輪の回転速度を検出する。車速センサ３は、車輪の回転速度を示す信号を測位装置１００に出力する。
【００２３】
ジャイロセンサ４と加速度センサ５は慣性センサである。ジャイロセンサ４は、車両１のヨー軸、ピッチ軸、ロール軸周りの回転角速度を検出し、検出した回転角速度を示す信号を測位装置１００に出力する。ジャイロセンサ４はヨー軸周りの回転角速度すなわちヨーレートを検出するのでヨーレートセンサとして機能する。
【００２４】
加速度センサ５は、車両１の前後方向に生じる加速度を検出する。なお、前後方向の加速度に加えて、車両１の幅方向、上下方向の加速度を検出するようになっていてもよい。加速度センサ５は、検出した加速度を示す信号を測位装置１００に出力する。
【００２５】
シフトポジションセンサ６は、車両１のシフト位置を検出し、そのシフト位置を示す信号を測位装置１００に出力する。シフトポジションセンサ６が出力する信号により、車両１の移動方向が前進および後進のいずれであるかが定まる。
【００２６】
車速センサ３、ジャイロセンサ４、加速度センサ５、シフトポジションセンサ６は、車両１の動きすなわち挙動を示す信号を出力する挙動検出センサ７である。
【００２７】
測位装置１００は、図示しないＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータである。ＣＰＵは、ＲＡＭの一時記憶機能を利用しつつＲＯＭなどの非遷移的実体的記録媒体に記憶されているプログラムを実行する。これにより、測位装置１００は、衛星情報取得部１０１、車両情報取得部１０２、センサ補正値推定部１０３、衛星測位部１０４、方位推定部１０５、軌跡演算部１０６、誤差推定部１０７、位置演算部１０８としての機能を実行する。これらの機能を実行すると、非遷移的実体的記録媒体に記憶されているプログラムに対応する方法が実行される。なお、測位装置１００が実行する機能の一部または全部を、一つあるいは複数のＩＣ等によりハードウェア的に構成してもよい。
【００２８】
＜測位装置１００が実行する処理の概要＞
次に、測位装置１００が実行する処理の概要を説明する。なお、測位装置１００が実行する一部の処理については、図２以降に示すフローチャートを用いて詳述する。
【００２９】
衛星情報取得部１０１は、ＧＮＳＳ受信機２から、衛星情報を衛星情報取得周期で取得し、取得した衛星情報を衛星情報蓄積部１１０に記憶する。衛星情報取得周期は、ＧＮＳＳ受信機２が衛星情報を出力する周期と同じである。衛星情報蓄積部１１０は、書き込み可能な記憶媒体であり、揮発性でも不揮発性でもよい。衛星情報蓄積部１１０には、たとえば、ＲＡＭを用いることができる。
【００３０】
車両情報取得部１０２は、挙動検出センサ７が検出した信号を、センサ値取得周期で取得する。センサ値取得周期は、ＧＮＳＳ受信機２が航法信号を出力する周期よりも短く、たとえば、数十ミリ秒である。車両情報取得部１０２は、取得した信号を車両情報蓄積部１１１に記憶する。車両情報蓄積部１１１は、書き込み可能な記憶媒体であり、揮発性でも不揮発性でもよい。車両情報蓄積部１１１には、たとえば、ＲＡＭを用いることができる。また、車両情報蓄積部１１１には、衛星情報蓄積部１１０と同じ記憶媒体を用いることができる。車両情報蓄積部１１１に蓄積されている情報を車両情報とする。
【００３１】
センサ補正値推定部１０３は、挙動検出センサ７が出力する検出値を補正する部分であり、ヨーレート補正値推定部１０３ａ、ピッチレート補正値推定部１０３ｂ、車速補正値推定部１０３ｃを備えている。
【００３２】
ヨーレート補正値推定部１０３ａは、ジャイロセンサ４が検出するヨー軸周りの回転角速度（すなわちヨーレート）を補正する補正値（以下、ヨーレート補正値）を推定する。ヨーレート補正値は次のようにして推定する。
【００３３】
衛星測位部１０４が測位時に決定する車両１の移動方位の２回分の差分からヨー軸周りの方位差分を決定する。また、その２回の測位時刻をｔ１、ｔ２としたとき、時刻ｔ１でのジャイロセンサ４のヨー軸周りの角速度の積算値すなわちヨーレート積算値と、時刻ｔ２でのヨーレート積算値との差分を算出する。
【００３４】
方位差分を真値とみなすと、方位差分とヨーレート積算値の差分との差が、時刻ｔ１−ｔ２の間のヨーレート積算値の誤差である。したがって、方位差分と時刻ｔ１−ｔ２の間のヨーレート積算値との差を、ｔ１−ｔ２で割った値が、単位時間当たりのヨーレート補正値である。ヨーレート補正値推定部１０３ａは、ヨーレート補正値を周期的に更新する。
【００３５】
ピッチレート補正値推定部１０３ｂは、ジャイロセンサ４が検出するピッチ軸周りの回転角速度（すなわちピッチレート）を補正する補正値（以下、ピッチレート補正値）を推定する。ピッチレート補正値は次のようにして推定する。
【００３６】
衛星測位部１０４が測位時に決定する車両１のピッチ角の２回分の差分（以下、ピッチ角差分）を決定する。一方、その２回の測位時刻をｔ１、ｔ２としたとき、時刻ｔ１でのピッチ角推定値と時刻ｔ２でのピッチ角推定値を、それぞれ、ジャイロセンサ４が逐次出力するピッチ軸周りの角速度の積算値すなわちピッチレート積算値から推定する。ピッチ角推定値は、ピッチレート積算値をそのままピッチ角推定値とするのではなく、過去のピッチレート積算値も用いて、現時点でのピッチ角推定値を推定する。ピッチ角は、車両１の加速度の変化の影響を受けるため、変動が大きいからである。
【００３７】
具体的には、ピッチ角推定値は次のようにして決定する。各センサ値取得時点において、最新のピッチレート積算値と過去のピッチレート積算値を用いて、最小二乗法により、時刻に対するピッチレート積算値の増加直線を算出する。そして、その増加直線に現在の時刻を代入して、現在の時刻のピッチ角推定値を決定する。この処理を、時刻ｔ１、ｔ２においてそれぞれ行って、時刻ｔ１におけるピッチ角推定値と、時刻ｔ２におけるピッチ角推定値をそれぞれ求める。
【００３８】
時刻ｔ１におけるピッチ角推定値と時刻ｔ２におけるピッチ角推定値との差分と、衛星測位部１０４が決定したピッチ角から算出したピッチ角差分との差が、時刻ｔ１−ｔ２の間のピッチレート積算値の誤差である。この誤差をｔ１−ｔ２で割った値が、単位時間当たりのピッチレート補正値である。ピッチレート補正値推定部１０３ｂは、ピッチレート補正値を周期的に更新する。
【００３９】
車速補正値推定部１０３ｃは、車速補正値を推定する。この車速補正値は、車速センサ３が検出する車輪の回転速度に乗じる値であり、車速推定値は、車速センサ３の検出値に、タイヤ周長と車速補正値を乗じて算出する。
【００４０】
車速補正値は次のようにして推定する。方位推定部１０５は、衛星情報を用いて速度ベクトルを決定する。方位推定部１０５における速度ベクトルの決定方法は後述する。この速度ベクトルの車両１の進行方向成分が車速である。車速センサ３が検出した車輪回転速度にタイヤ周長を乗じて、補正前の車速を算出する。この補正前の車速を、方位推定部１０５が決定した速度ベクトルから得られた車速にするための係数が、車速補正値である。
【００４１】
衛星測位部１０４は、衛星情報取得部１０１が取得して衛星情報蓄積部１１０に蓄積されている衛星情報を用いて、車両１の現在位置を逐次算出する。衛星測位部１０４における現在位置の算出方法は衛星航法と呼ばれ、衛星情報を用いて現在位置を算出する種々の方法を用いることができる。たとえば、衛星測位部１０４は、特許文献４に記載されている方法を用いることができる。また、４つ以上の衛星から航法信号を同じタイミングで取得して、車両１の三次元座標と時計誤差を未知数とする連立方程式を解く方法でもよい。衛星測位部１０４が算出した車両１の座標を衛星測位座標ＰＶＳとする。
【００４２】
方位推定部１０５は、車両情報蓄積部１１１に蓄積されている車両情報あるいは衛星情報蓄積部１１０に蓄積されている衛星情報を用いて、車両１の移動方位を逐次更新する。たとえば、方位φは、衛星情報であるドップラーシフト量Ｄから車両１の移動方位を推定する。方位推定方法は、図３、図６を用いて後述する。
【００４３】
軌跡演算部１０６は、車速、方位φ、ピッチ角から、車両１の移動軌跡を逐次演算する。車速、方位φ、ピッチ角は、衛星情報および車両情報のいずれかから算出する。車速は、車両情報から算出する場合、車両情報蓄積部１１１に蓄積されている車輪の回転速度とタイヤ周長と車速補正値とから算出する。また、衛星情報であるドップラーシフト量Ｄから車速を推定することもできる。方位φは、方位推定部１０５が逐次推定する移動方位を用いる。
【００４４】
なお、移動軌跡の演算に車両情報から決定した車速、方位φ、ピッチ角を用いる場合、センサ補正値推定部１０３が推定したヨーレート補正値、ピッチレート補正値、車速補正値を用いて補正した値を用いる。
【００４５】
誤差推定部１０７は、衛星測位部１０４が逐次演算する位置の誤差（以下、衛星測位誤差Ｅｓ）と、自律航法での位置の誤差（以下、自律測位誤差Ｅａ）を、逐次推定する。誤差推定部１０７の処理の詳細説明は、フローチャートの説明において行う。
【００４６】
位置演算部１０８は、車両１の現在位置を逐次演算する。車両１の現在位置は、基準点から移動軌跡を延ばすことで決定する。移動軌跡は車両情報を用いて演算することから、自律航法により位置を決定していることになる。よって、位置演算部１０８は、請求項の自律測位部としての機能を備える。また、車両情報は主として慣性センサの検出値であるので、自律航法は慣性航法ともいう。なお、上述した衛星測位部１０４は車両情報を用いないで測位を行う。
【００４７】
本実施形態のおける基準点には、実基準点Ｂｒと仮想基準点Ｂｖとがある。なお、仮想基準点Ｂｖを用いないで、実基準点Ｂｒのみを基準点として車両１の現在位置を逐次演算することもできる。
【００４８】
実基準点Ｂｒには、衛星測位座標ＰＶＳを用いる。ただし、衛星測位座標ＰＶＳが算出されても、衛星測位誤差Ｅｓが自律測位誤差Ｅａ以上であれば、実基準点Ｂｒは更新しない。
【００４９】
＜測位装置１００が実行する処理の流れ＞
測位装置１００は、図２に示す処理と、図３に示す処理とを並列して実行する。たとえば、図２の処理を実行中に、割り込み処理により図３の処理を実行する。まず、図２に示す処理から説明する。
【００５０】
測位装置１００は、図２に示す処理を周期的に実行する。図２を実行する周期は、衛星情報取得周期である。図２に示す処理において、ステップ（以下、ステップを省略）Ｓ２は衛星情報取得部１０１が実行し、Ｓ４〜Ｓ１０、Ｓ１４は衛星測位部１０４が実行し、Ｓ１２はヨーレート補正値推定部１０３ａ、ピッチレート補正値推定部１０３ｂが実行する。
【００５１】
Ｓ２では、ＧＮＳＳ受信機２から衛星情報を取得する。Ｓ４では、Ｓ２で取得した衛星情報を衛星情報蓄積部１１０に記憶する。なお、航法信号あるいは航法データを元にして、衛星情報の一部を測位装置１００が演算してもよい。
【００５２】
Ｓ６では、航法信号の信号品質がよいか否かを判定する。信号品質の判定は、取得したすべての航法信号に対して行う。信号品質の判定には種々の基準を用いることができる。
【００５３】
たとえば、（判定条件Ｊ１）Ｓ／Ｎが所定値以上であること、（判定条件Ｊ２）擬似距離ρｉの残差が判定基準距離以下であること、（判定条件Ｊ３）仰角θｉが判定基準角以上であること、（判定条件Ｊ４）判定条件Ｊ１〜Ｊ３の組み合わせ、などにより信号品質がよいか否かを判定する。
【００５４】
上記判定条件Ｊ２における擬似距離ρｉの残差とは、航法衛星Ｓｉの衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）と自車両の衛星測位座標ＰＶＳ（Ｘｖ，Ｙｖ，Ｚｖ）との間の距離と、擬似距離ρｉとの差である。この残差を算出するためには、自車両の衛星測位座標ＰＶＳ（Ｘｖ，Ｙｖ，Ｚｖ）が必要である。自車両の衛星測位座標ＰＶＳは、後で説明するＳ１０において衛星測位座標ＰＶＳを算出する方法と同じ方法で算出する。したがって、擬似距離ρｉの残差を算出する場合には、先に、衛星測位座標ＰＶＳを算出することになる。
【００５５】
擬似距離ρｉの残差が大きい場合には、マルチパス等の影響が考えられるので、信号品質が良くないと判定する。判定条件Ｊ１は、Ｓ／Ｎが所定値以上であれば信号品質はよいと判定する。判定条件Ｊ３は、仰角θｉが判定基準角以上であれば信号品質はよいと判定する。
【００５６】
Ｓ８では、Ｓ６で信号品質がよいと判定した航法信号の数が４以上であるか否かを判断する。４以上である場合にはＳ１０へ進む。
【００５７】
Ｓ１０では、Ｓ２で取得した擬似距離ρｉ、航法衛星Ｓｉの衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、現在の自車両の座標（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。この座標を、衛星測位座標ＰＶＳという。
【００５８】
衛星測位座標ＰＶＳは、衛星測位座標ＰＶＳ（Ｘｖ，Ｙｖ，Ｚｖ）とクロックバイアスとを未知数とする４つ以上の連立方程式を解くことで求める。衛星測位座標ＰＶＳ（Ｘｖ，Ｙｖ，Ｚｖ）の算出方法は種々の手法を用いて算出できる。
【００５９】
Ｓ１２では、Ｓ１０で算出した衛星測位座標ＰＶＳと、前回までに算出した衛星測位座標ＰＶＳとを用いてヨーレート、ピッチレートを算出する。これらを真値とし、真値と、ジャイロセンサ４の検出値から得られるヨーレート、ピッチレートとの差を、新たなヨーレート補正値、ピッチレート補正値とする。
【００６０】
Ｓ１４ではｋに１を加える。なお、このｋの初期値は０である。つまり、このｋは、衛星測位座標ＰＶＳを算出した回数を意味する。
【００６１】
次に、図３を説明する。図３に示すフローチャートは、センサ値取得周期で繰り返し実行する。なお、特に明記している場合を除き、各パラメータは、最新の時刻における値を意味する。
【００６２】
Ｓ２０では、挙動検出センサ７から車両情報を取得する。Ｓ２２では、Ｓ２０で取得した車両情報を車両情報蓄積部１１１に記憶する。
【００６３】
Ｓ２４では、Ｓ２０で取得したジャイロセンサ４の検出値から求められるヨーレートにヨーレート補正値を加算した補正後ヨーレートに、前回のＳ２４実行時から現時点までの経過時間を乗じて相対方位角変化量を算出する。この相対方位角変化量を、これまでの相対方位角φｔ（−１）ｇｙｒｏに加算して相対方位角φｔｇｙｒｏを更新する。相対方位角φｔｇｙｒｏは、基準時点における車両１の移動方位に対する時刻ｔの相対方位角である。したがって、この相対方位角φｔｇｙｒｏは請求項の基準方位に相当する。また、Ｓ２４では、相対方位角φｔｇｙｒｏを更新するのと同様の方法で、相対ピッチ角も更新する。
【００６４】
Ｓ２６では、前回、このＳ２６を実行してから、今回、このＳ２６を実行するまでの間に、衛星測位座標ＰＶＳが算出されたか否かを判断する。衛星測位座標ＰＶＳは、Ｓ１０で算出している。ただし、衛星測位座標ＰＶＳを算出するには、衛星情報を取得する必要があり、衛星情報を取得する周期は、車両情報を取得する周期よりも長い。また、衛星情報を取得しても、品質のよい航法信号を４つ以上取得していなければ衛星測位座標ＰＶＳは算出されない。また、航法信号を取得した後、演算も必要であり、この演算には時間が必要となる。したがって、Ｓ２６の判断はＮＯになる場合もある。
【００６５】
Ｓ２６の判断がＹＥＳになった場合には、Ｓ２８に進む。Ｓ２８では、ドップラー速度を算出する。ドップラー速度とは、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）、またはその速度ベクトルの大きさを意味する。速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、ドップラーシフト量Ｄを用いて算出することから、ここでは、ドップラー速度と称している。
【００６６】
Ｓ２８の詳細処理は図４に示す。図４において、Ｓ２８２では、Ｓ１０で算出した衛星測位座標ＰＶＳ（Ｘｖ，Ｙｖ，Ｚｖ）、および、Ｓ２で衛星情報蓄積部１１０に記憶した航法衛星Ｓｉの衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、各航法衛星Ｓｉの方向Ｒｉ（θｉ、φｉ）を算出する。各航法衛星Ｓｉの方向Ｒｉは、車両１から航法衛星Ｓｉを見たときの、水平方向に対する仰角θｉ、北方向に対する方位角φｉで表すものとする。
【００６７】
Ｓ２８４では、Ｓ４で算出した各航法衛星Ｓｉの衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、ケプラーの方程式の微分を用いる方法で、各航法衛星Ｓｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を算出する。
【００６８】
Ｓ２８６では、Ｓ２８２で算出した各航法衛星Ｓｉの方向Ｒｉと、Ｓ２８４で算出した各航法衛星Ｓｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）から、Ｖｓａｔｉ＝Ｒｉ［Ｖｘｓｉ，Ｖｙｓｉ，Ｖｚｓｉ］Ｔにより、自車両方向の航法衛星Ｓｉの速度Ｖｓａｔｉを求める。なお、Ｔは行列の転置を意味する。
【００６９】
Ｓ２８８では、（１）式に示した連立方程式を４つ以上立式し、その連立方程式を解く。これにより、車両１の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）および、ＧＮＳＳ受信機２が備える時計のクロックドリフトＣｂｖｔを求めることができる。
【数１】
2019015635.tif 000003
【００７０】
この（１）式において、Ｄｉはドップラーシフト量、Ｃは光束、Ｆは航法衛星Ｓｉが送信する電波の搬送波の周波数である。この（１）式は特許文献２にも開示されているので、（１）式の導出方法の説明は省略する。
【００７１】
ＧＮＳＳ電波を受信した航法衛星が４個以上である場合に、上記（１）式の連立方程式を解くことができる。ただし、ＧＮＳＳ電波を受信していても、信号品質がよいと判定できないＧＮＳＳ電波は除外する。よって、Ｓ８で航法信号の信号品質がよいと判定したＧＮＳＳ電波を４個以上の航法衛星から受信した場合に、（１）式から自車両の速度ベクトルを算出する。
【００７２】
速度ベクトルが得られると、車両１の三次元移動方位が得られたことになる。そこで、Ｓ２９０では、速度ベクトルから定まる、車両１が進行する方位角、ピッチ角を方位初期値φ０、ピッチ角初期値として更新する。図４のここまでの処理は、方位推定部１０５が実行する。
【００７３】
Ｓ２９２では、速度ベクトルから定まる車両１の進行方向の速度と、車速センサ３が検出する車輪の回転速度から算出できる車速とから、車速補正値を更新する。このＳ２９２は、車速補正値推定部１０３ｃが実行する。
【００７４】
説明を図３に戻す。Ｓ２８を実行した場合、図６に示す位置更新処理を実行する。この位置更新処理を説明する前に、Ｓ３０以下を説明する。
【００７５】
Ｓ２６において衛星測位座標ＰＶＳは算出されていないと判断した場合にはＳ３０へ進む。Ｓ３０では、初期値を更新することができるか否かを判断する。この判断は、具体的には、方位初期値φ０を更新してからの航法信号を用いて、（２）式に示す方位推定式を３つ以上、立式できるか否かを判断するものである。
【数２】
2019015635.tif 000004
【００７６】
（２）式は、特許文献２において最終的な速度ベクトル算出式として開示されている式である。（２）式において、Ｖｓは衛星方向速度、ｔは時刻、Ｖｗｈｅｅｌは車速センサ３の検出値、φ０は初期時刻における車両１の進行方向の方位角（すなわち方位初期値）、φｇｙｒｏは、車両１の進行方向の方位角の積算値すなわち相対方位角、Ｃｂｖ０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾き、Ｇｘ、Ｇｙは、車両から航法衛星Ｓｉへの視線ベクトルのｘ成分、ｙ成分である。
【００７７】
本実施形態では、すでに説明したＳ２８で車両１の速度ベクトルを算出している。速度ベクトルが算出できれば、車両１の方位も決定できる。しかし、Ｓ２８で車両１の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するには、衛星測位座標ＰＶＳを必要とする。したがって、信号品質がよいＧＮＳＳ電波を４個以上の航法衛星から受信できなければ、車両１の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。
【００７８】
そのため、たとえば、高層ビルの多い都市部では、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できない時間が長く続くこともある。また、信号品質がよいＧＮＳＳ電波を４個以上受信できる環境であっても、Ｓ２６は、２００ミリ秒〜４００ミリ秒周期の一定周期でしかＹＥＳにならない。このＳ２６がＹＥＳになる周期でしか、車両１の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。
【００７９】
これに対して、挙動検出センサ７の検出値は、走行環境に依存せずに一定周期で取得でき、しかも、たとえば２０ｍｓごとなど、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）が算出できる周期よりも短い周期で取得できる。
【００８０】
そこで、車両１の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出してから、次に、車両１の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するまでの間は、挙動検出センサ７の検出値をもとにして、方位推定および速度推定を行う。この方位推定および速度推定で用いる初期値をＳ３２で決定する。
【００８１】
Ｓ３２では、詳しくは図５に示す処理を実行する。Ｓ３２は方位推定部１０５が実行する。図５において、Ｓ３２２では、Ｓ４で算出した各航法衛星Ｓｉの衛星位置（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、各航法衛星Ｓｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を算出する。
【００８２】
Ｓ３２４では、Ｓ２で取得してＳ４で記憶した衛星情報に含まれているドップラーシフト量Ｄｉを（３）式に代入して、航法衛星Ｓｉに対する自車両の相対速度Ｖｒｉを算出する。
【数３】
2019015635.tif 000005
【００８３】
Ｓ３２６では、現在の車両１の位置Ｐ（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。このＳ３２６を実行する場合、Ｓ２６がＮＯになっているのであるから、品質のよい航法信号を４つ以上受信していない。しかし、このＳ３２６で算出する位置Ｐ（Ｘｖ，Ｙｖ，Ｚｖ）は、次のＳ３２８において、車両１から航法衛星Ｓｉへの視線ベクトル（Ｇｘｉ、Ｇｙｉ、Ｇｚｉ）を算出するために用いる。
【００８４】
航法衛星Ｓｉは遠方に存在するため、航法衛星Ｓｉと車両１との角度を求めるために用いる現在位置は、精度が低くてもよい。したがって、品質のよい信号を４つ以上受信していなくてもよい。
【００８５】
そこで、たとえば、このＳ３２６では、品質がよいとは判断できない信号も含めて４つ以上の航法信号を用いて、Ｓ１０と同様にして、現在の車両１の衛星測位座標ＰＶＳを算出し、この衛星測位座標ＰＶＳを現在の車両１の位置Ｐ（Ｘｖ，Ｙｖ，Ｚｖ）とする。
【００８６】
また、擬似距離ρｉを用いた位置決定以外の精度の低い方法で車両１の位置Ｐ（Ｘｖ，Ｙｖ，Ｚｖ）を決定してもよい。システム等で許容される推定精度に依存するが、車両１の位置誤差が数百ｍの範囲であれば、速度推定誤差は１ｍ／ｓｅｃ以下となり大きな問題はない。そのため、たとえば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、車両１の位置Ｐ（Ｘｖ，Ｙｖ，Ｚｖ）を決定してもよい。
【００８７】
また、精度が低くても良いので、後述する位置更新処理（図６）において、前回、更新した車両１の位置Ｐを、ここでの車両１の位置Ｐ（Ｘｖ，Ｙｖ，Ｚｖ）としてもよい。
【００８８】
Ｓ３２８では、車両１から航法衛星Ｓｉへの視線ベクトル（Ｇｘｉ、Ｇｙｉ、Ｇｚｉ）を算出する。視線ベクトルのｘ成分、ｙ成分、ｚ成分は、（４）式から算出する。
【数４】
2019015635.tif 000006
【００８９】
（４）式において、ρｔｉは時刻ｔにおける航法衛星Ｓｉの擬似距離、（Ｘｔｓｉ、Ｙｔｓｉ、Ｚｔｓｉ）は時刻ｔにおける航法衛星Ｓｉの衛星位置である。これらは、Ｓ４で記憶している。（ＸｔＶ、ＹｔＶ、ＺｔＶ）は時刻ｔにおける車両１の現在位置であり、Ｓ３２６で算出している。
【００９０】
Ｓ３３０では、（５）式に、Ｓ３２４で算出した相対速度Ｖｒｉ、Ｓ３２８で算出した視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）、Ｓ３２２で算出した航法衛星Ｓｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を代入して、航法衛星Ｓｉの方向への車両１の衛星方向速度Ｖｓｉを算出する。
【数５】
2019015635.tif 000007
【００９１】
（５）式の右辺の第１項は、航法衛星Ｓｉに対する車両１の相対速度Ｖｒｉを意味しており、第２〜第４項は、航法衛星Ｓｉの車両１の方向への速度である。これらの和は、航法衛星Ｓｉの方向への車両１の速度を意味することから、（５）式が成り立つのである。
【００９２】
Ｓ３３２では、（２）式に、Ｓ３３０で算出した衛星方向速度Ｖｓｉ、Ｓ２４で更新した相対方位角φｇｙｒｏ、Ｓ３２８で算出した視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）を代入する。これにより、（２）式において未知パラメータは、φ０、Ｃｂｖ０、および、Ａの３つとなる。
【００９３】
したがって、（２）式に、これら衛星方向速度Ｖｓｉ、相対方位角φｇｙｒｏ、視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）を代入した式を３つ以上立式する。そして、それら３つ以上の式からなる連立方程式を解く。これにより、（２）式において未知パラメータとなっている方位初期値φ０を求めることができる。
【００９４】
未知パラメータφ０、Ｃｂｖ０、Ａは、初期時刻以降であれば、時刻が異なっても同じである。そのため、同一時刻で３つの式を立式する必要はなく、複数の時刻において立式した式数が合計３式以上であれば、未知パラメータを求めることができる。たとえば、３時刻（ｔ０、ｔ１、ｔ２）の観測衛星数がそれぞれ１であったとしても、観測された航法衛星からのデータを用いて方位初期値φ０を求めることができる。
【００９５】
また、このＳ３３２では、ピッチ角初期値も求める。ピッチ角初期値の求め方は方位初期値を同様である。すなわち、ピッチ角初期値は、（２）式においてＧｘ、Ｇｙを、Ｇｙ、Ｇｚに置き換え、θをφに置き換えた式を３つ以上立式して求める。
【００９６】
図３に示す処理を終了した場合には、図６に示す位置更新処理を実行する。なお、図３において、Ｓ２０、Ｓ２２は車両情報取得部１０２の処理であり、Ｓ２４以下は方位推定部１０５の処理である。
【００９７】
次に図６を説明する。図６において、Ｓ４１では、車両１が移動する方位を推定した推定方位φｅを決定する。推定方位は、Ｓ２８を実行して速度ベクトルを決定した場合には、その速度ベクトルから決定する。また、その速度ベクトルから推定ピッチ角も決定する。
【００９８】
また、Ｓ３２を実行して車両１から航法衛星Ｓｉへの視線ベクトルを算出した場合には、その視線ベクトルと、航法衛星Ｓｉの位置から、車両１が向いている方位、すなわち推定方位φｅを決定する。
【００９９】
一方、Ｓ３０の判断がＮＯになり、Ｓ３２を実行していない場合には、最新の方位初期値φ０と、Ｓ２４で更新した相対方位角φｇｙｒｏとから、推定方位φｅを算出する。また、最新のピッチ角初期値と、Ｓ２４で更新した相対ピッチ角から、推定ピッチ角も算出する。
【０１００】
Ｓ４２では、α＋ｊｋβが１以上になっていないｊｋに１を加える。このｊｋの初期値は０である。また、ｋは、Ｓ１４で説明したように、衛星測位座標ＰＶＳを算出するごとに１増加する。したがって、衛星測位座標ＰＶＳを算出するごとに、新たなｊｋが生成される。ｊｋの初期値は０であり、時刻ｔｋにおいてｊｋは０となる。時刻ｔｋは、衛星測位座標ＰＶＳ（ｔｋ）を算出するために用いた航法信号を受信した時刻である。そして、ｊｋは、この図６を実行するごとに１ずつ増加する。したがって、ｊｋは、時刻ｔｋ以降に図６の処理を実施した回数を意味する。なお、α、βの意味は、Ｓ４９、Ｓ５１で説明する。
【０１０１】
続くＳ４３では、自律航法位置ＰＤ（ｔ）を更新する。自律航法位置ＰＤ（ｔ）は、前回の自律航法位置ＰＤ（ｔ−１）に、今回のＳ４１で算出した推定方位φｅおよび推定ピッチ角の方向に、最新の車速推定値に自律航法位置ＰＤの更新周期を乗じて求められる距離を加算することで更新する。Ｓ４３の処理により、移動軌跡が更新されたことになる。
【０１０２】
Ｓ４４では、前回、このＳ４４を実行してから、今回、このＳ４４を実行するまでの間に、衛星測位座標ＰＶＳが更新されたか否かを判断する。このＳ４４の判断がＮＯであれば、Ｓ４５〜Ｓ５１を実行することなく、Ｓ５２へ進む。Ｓ４４の判断がＹＥＳであればＳ４５へ進む。
【０１０３】
Ｓ４５では衛星測位誤差Ｅｓを算出する。衛星測位誤差Ｅｓは種々の手法で算出することができる。本実施形態では、擬似距離ρｉの残差（以下、擬似距離残差）を衛星測位誤差Ｅｓとして用いる。
【０１０４】
Ｓ４６では自律測位誤差Ｅａを算出する。自律測位誤差Ｅａは、自律航法の基準点としている実基準点Ｂｒの衛星測位誤差Ｅｓに、自律航法分の誤差を加算した値である。自律航法分の誤差は、自律航法の継続時間に、自律航法の単位時間当たりの誤差を乗じた値である。また、自律航法分の誤差を、自律航法での走行距離に、自律航法の単位距離当たりの誤差を乗じた値としてもよい。
【０１０５】
ただし、本実施形態では、自律航法の単位時間当たりの誤差、あるいは、自律航法の単位距離当たりの誤差は、推定方位φｅをドップラーシフト量Ｄｉに基づいて推定したときは、推定方位φｅをジャイロセンサ４の検出値から決定したときよりも小さくする。自律航法の単位時間当たりの誤差、あるいは、自律航法の短距離当たりの誤差は、実機での計測等により予め設定しておく。
【０１０６】
推定方位φｅをドップラーシフト量Ｄｉに基づいて推定したときとは、Ｓ２８を実行したとき、または、Ｓ３２を実行したときである。一方、推定方位φｅをジャイロセンサ４の検出値から決定するのは、Ｓ３０の判断がＮＯになったときである。
【０１０７】
Ｓ４７では、Ｓ４５で算出した衛星測位誤差Ｅｓが、Ｓ４６で算出した自律測位誤差Ｅａよりも小さいか否かを判断する。この判断がＮＯであればＳ５２に進み、ＹＥＳであればＳ４８に進む。
【０１０８】
Ｓ４８では、衛星測位座標ＰＶＳの信頼度を決定する。衛星測位座標ＰＶＳの信頼度は種々の方法で算出することができる。たとえば、（１）衛星測位座標ＰＶＳの算出に用いた航法信号の数、（２）航法信号のＳ／Ｎ、（３）擬似距離残差、（４）航法衛星Ｓｉの仰角θｉのいずれか１つ、または（１）〜（４）の組み合わせ、と信頼度との間の予め設定した対応関係を用いて、衛星測位座標ＰＶＳの信頼度を決定する。
【０１０９】
Ｓ４９では、（７）式に示す仮想基準点算出式が備えるオフセット項αを更新する。この（７）式は、本実施形態において、（６）式に示す仮想基準点導出関数Ｆ｛Ｐ｝を表す式である。
【数６】
2019015635.tif 000008
【数７】
2019015635.tif 000009
【０１１０】
（６）式、（７）式において、時刻ｔｋにおけるｋは整数であり、時刻ｔｋは、衛星測位座標ＰＶＳを算出するために用いた航法信号が受信できた時点である。
【０１１１】
また、（６）式において、Δｔは、自律航法により車両１の位置Ｐを更新する周期である。この周期を、以下、自律航法周期という。自律航法周期はセンサ値取得周期と同じである。
【０１１２】
また、Δｔが自律航法周期であり、ｊｋは衛星測位座標ＰＶＳが更新されてからの図６の処理の実施回数を意味することから、ｊｋΔｔは、衛星測位座標ＰＶＳが更新されてから現時点までの経過時間を意味する。
【０１１３】
（６）式は次のことを表している。時刻ｔｋ＋ｊｋΔｔにおける仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）は、時刻ｔｋ＋ｊｋΔｔにおいて仮想基準点導出関数Ｆ｛Ｐ｝が示す座標である。
【０１１４】
（７）式は次のことを表している。時刻ｔｋ＋ｊｋΔｔにおける仮想基準点導出関数Ｆ｛Ｐ｝が示す座標は、その時点における最新の衛星測位座標ＰＶＳ（ｔｋ）と、時刻ｔｋにおいて決定した車両１の位置Ｐ（ｔｋ）とを、重み係数を乗じて加算した値である。
【０１１５】
ｊｋは０以上の整数であることから、ｊｋが増加するごとに−ｊｋβの値は減少する。また、この図６では、後述するＳ５３において車両１の移動量ΔＰＤを算出する。したがって、−ｊｋβは、移動量ΔＰＤを算出した回数に応じて値が減少する減少項である。一方、ｊｋβは、移動量ΔＰＤを算出した回数に応じて値が増加する増加項である。また、減少項である−ｊｋβを備える（１−α−ｊｋβ）を第１係数計算式とし、増加項であるｊｋβを備える（α＋ｊｋβ）を第２係数計算式とする。
【０１１６】
この仮想基準点算出式において、車両１の位置Ｐ（ｔｋ）に乗じる（１−α−ｊｋβ）と、衛星測位座標ＰＶＳ（ｔｋ）に乗じる（α＋ｊｋβ）は、重み係数を意味する。時刻ｔｋ以降の仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）は、時刻ｔｋにおける車両１の位置Ｐ（ｔｋ）と、時刻ｔｋにおける衛星測位座標ＰＶＳ（ｔｋ）の間において、（１−α−ｊｋβ）と（α＋ｊｋβ）により定まる比率の位置となる。
【０１１７】
そして、移動量ΔＰＤを算出した回数が増えるほど、仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）は、衛星測位座標ＰＶＳ（ｔｋ）に近くづくことになる。
【０１１８】
また、仮想基準点算出式においてオフセット項αはｊｋによらず一定である。このオフセット項αが大きいほど、仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）は、ｊｋ＝０のときから、衛星測位座標ＰＶＳ（ｔｋ）に近くなる。
【０１１９】
この仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）を算出する理由は、車両１の位置が、不連続に大きく変化してしまうことを抑制するためである。衛星測位座標ＰＶＳ（ｔｋ）の精度が悪いほど、車両１の位置が不連続に大きく変化してしまう。したがって、衛星測位座標ＰＶＳ（ｔｋ）が信頼できるほど、仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）を衛星測位座標ＰＶＳ（ｔｋ）に近づけてもよい。
【０１２０】
そこで、Ｓ４９では、衛星測位座標ＰＶＳの信頼度に応じて、αの値を大きくする。詳しくは、衛星測位座標ＰＶＳの信頼度とαとの対応関係を予め設定しておき、今回、決定した信頼度と、この対応関係から、αの値を更新する。衛星測位座標ＰＶＳの信頼度とαとの対応関係は、衛星測位座標ＰＶＳの信頼度が高いほど、段階的に、あるいは、連続的に、αが大きくなる関係である。
【０１２１】
Ｓ５０では、移動量信頼度を決定する。移動量信頼度は、後述するＳ５３で算出する移動量ΔＰＤの信頼度を意味する。自律航法では、誤差が積算する。つまり、自律航法により車両１の位置Ｐを更新している期間が長いほど、誤差が大きくなる。そこで、このＳ５０では、衛星測位座標ＰＶＳを更新せずに、自律航法のみにより車両１の位置Ｐを更新している期間が長いほど、移動量信頼度を低くする。衛星測位座標ＰＶＳを更新していない期間としては、Ｓ４２で更新したｊｋの値を用いることができる。
【０１２２】
Ｓ５１では、Ｓ５０で決定した移動量信頼度が高いほど、βを小さい値に更新する。この理由は、移動量信頼度が高いほど、仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）を、早く衛星測位座標ＰＶＳ（ｔｋ）に近づける必要性が低くなるからである。
【０１２３】
Ｓ５１では、具体的には、移動量信頼度が高いほど、段階的に、あるいは、連続的に、βが小さくなる予め設定した関係と、Ｓ５０で決定した移動量信頼度から、更新後のβを決定する。
【０１２４】
Ｓ５２では、最新の仮想基準点算出式に、時刻ｔｋにおける車両１の位置Ｐ（ｔｋ）と、時刻ｔｋに受信した航法信号から算出した衛星測位座標ＰＶＳ（ｔｋ）を代入して、時刻ｔｋ＋ｊｋΔｔにおける仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）を算出する。
【０１２５】
Ｓ５３では、時刻ｔｋから現在の時刻ｔｋ＋ｊｋΔｔまでに車両１が移動した移動量ΔＰＤを、（８）式から算出する。（８）式から分かるように、この移動量ΔＰＤは、Ｓ４３で算出した自律航法位置ＰＤを用いて算出する。
【数８】
2019015635.tif 000010
Ｓ５４では、（９）式を用いて、現在の車両１の位置Ｐを更新する。
【数９】
2019015635.tif 000011
【０１２６】
（９）式に示すように、移動量ΔＰＤの基点となる基準点として仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）を用いており、仮想基準点Ｂｖ（ｔｋ＋ｊｋΔｔ）は、ｊｋが大きくなるごとに、衛星測位座標ＰＶＳ（ｔｋ）に近づいていく。これにより、特許文献３で説明されているように、車両１の位置Ｐを表す軌跡が段状に変化することを抑制できる。
【０１２７】
Ｓ５５では、Ｓ５４で決定した車両１の位置Ｐおよびその位置Ｐの誤差を、これらを利用するアプリケーションを実行する機器に出力する。誤差は、Ｓ４４、Ｓ４６で算出した衛星測位誤差Ｅｓおよび自律測位誤差Ｅａのうち小さい側の値である。また、車両１の位置Ｐおよび誤差に加えて、最新の方位、最新の方位誤差、最新のヨー角、最新のピッチ角、最新の車速なども、合わせて出力してもよい。
【０１２８】
なお、図６において、Ｓ４１は方位推定部１０５の処理、Ｓ４３は軌跡演算部１０６の処理、Ｓ４５、Ｓ４６は誤差推定部１０７の処理、残りは位置演算部１０８の処理である。
【０１２９】
＜実施形態のまとめ＞
本実施形態では、方位初期値φ０、ピッチ角初期値を衛星情報であるドップラーシフト量Ｄｉを用いて算出しているため、精度がよい推定方位φｅおよび推定ピッチ角を得ることができる。これにより移動軌跡の正確さが向上するので、移動軌跡を用いる自律航法の、時間経過あるいは移動距離の増加に伴う誤差増加が抑制される。したがって、衛星測位座標ＰＶＳが得られても、基準点を更新しないほうが誤差の拡大を抑制できることがある。
【０１３０】
そこで、本実施形態では、衛星測位座標ＰＶＳが更新された場合（Ｓ４４：ＹＥＳ）、衛星測位誤差Ｅｓと自律測位誤差Ｅａとを比較する。そして、衛星測位誤差Ｅｓの方が自律測位誤差Ｅａよりも小さいと判断した場合に、Ｓ４８以下を実行して仮想基準点導出関数Ｆ｛Ｐ｝を更新する。仮想基準点Ｂｖは、（９）式において車両１の位置Ｐを算出するための基準点として用いられる。
【０１３１】
よって、衛星測位誤差Ｅｓの方が自律測位誤差Ｅａよりも小さい場合には、更新された衛星測位座標ＰＶＳが車両１の位置Ｐを算出するための基準点として用いられる。換言すれば、新たな衛星測位座標ＰＶＳが得られても、衛星測位誤差Ｅｓの方が自律測位誤差Ｅａよりも大きい場合には、その衛星測位座標ＰＶＳを、車両１の位置Ｐを算出するための基準点としては用いない。このようにすることで、位置推定の誤差を小さくできる。
【０１３２】
図７に、本実施形態の誤差変化例を示す。図７は横軸が時刻である。車両１が走行中である場合には、時間経過に伴い走行距離が増加することから、横軸を走行距離と考えてもよい。
【０１３３】
図７の上図の縦軸は衛星測位可能度である。衛星測位可能度は、測位に使うことができる航法信号の数に対応する。航法信号が測位に使うことができるか否かは、信号品質がよいか否かの判定基準により決定することができる。また、図７上図における破線は、衛星測位が可能であると判断できる衛星測位可能度の閾値を示している。
【０１３４】
図７下図の縦軸は測位した位置の誤差を示している。実線は、本実施形態での誤差である。破線は比較例の誤差である。ここでの比較例は、衛星測位可能度が閾値を超えている場合には、常に衛星測位座標ＰＶＳを基準点として更新する測位方法である。
【０１３５】
時刻ｔ０からｔ１０までは衛星測位可能度が閾値を超えていないので、基準点を更新しないで自律航法を行う。なお、図７〜図１０の説明において自律航法は、基準点を更新しないで自律航法を継続することを意味し、衛星測位は、最新の衛星測位座標ＰＶＳを基準点として更新しつつ自律航法を行うことを意味する。
【０１３６】
ｔ０からｔ１０までは自律航法を継続するので、誤差は一定の傾きで直線的に増加する。ｔ１０では衛星測位可能度が閾値を超えるので、衛星測位座標ＰＶＳが更新される。これにより誤差が低下する。その後も、衛星測位により得られた衛星測位座標ＰＶＳが継続的に基準点に更新される。これによりｔ１０〜ｔ１２までは、時間が経過しても誤差は増加していない。比較例では、ｔ２０まで衛星測位により得られた衛星測位座標ＰＶＳにより基準点が継続的に更新される。しかし、衛星測位座標ＰＶＳの誤差は、衛星測位可能度が閾値を超えていても一定ではなく、種々の要因で変動する。
【０１３７】
図７の例では、ｔ１２〜ｔ１３までの間は、衛星測位が可能な状態は続いているが、衛星測位誤差Ｅｓはｔ１２時点よりは大きくなっている。ｔ１２からｔ１３まで、自律航法の誤差も大きくなっているが、自律測位誤差Ｅａは破線で示す衛星測位誤差Ｅｓよりも小さい。そのため、本実施形態では、ｔ１２からｔ１３までの間は、衛星測位が可能な状態であるにもかかわらず、自律航法を行う。
【０１３８】
また、ｔ１４からｔ２０までの間も同様に、衛星測位が可能な状態であるが、自律航法を行う。これにより、衛星測位ができなくなったｔ２０時点における本実施形態の誤差が、比較例よりも小さくなっている。
【０１３９】
ｔ３０では衛星測位が可能になり、一時的に衛星測位に切り替わる。しかし、衛星測位誤差Ｅｓが大きいｔ３１からｔ３２の間は、衛星測位が可能でも自律航法を行う。ｔ３２からｔ３４では衛星測位を行うが、ｔ３４で再び自律航法に切り替わっている。
【０１４０】
このように、衛星測位が可能でも、衛星測位誤差Ｅｓが自律測位誤差Ｅａよりも大きい場合には、基準点を衛星測位座標ＰＶＳに更新しないようにすることで、本実施形態の測位装置１００は、比較例よりも誤差を小さくできる。
【０１４１】
＜第２実施形態＞
次に、第２実施形態を説明する。この第２実施形態以下の説明において、それまでに使用した符号と同一番号の符号を有する要素は、特に言及する場合を除き、それ以前の実施形態における同一符号の要素と同一である。また、構成の一部のみを説明している場合、構成の他の部分については先に説明した実施形態を適用できる。
【０１４２】
第２実施形態では、位置演算部１０８の機能として図８に示すＳ５６の処理が追加されている点が、第１実施形態と異なる。
【０１４３】
第２実施形態では、Ｓ４７の判断において、衛星測位誤差Ｅｓが自律測位誤差Ｅａよりも小さいと判断しても、すぐにはＳ４８以下を実行せず、さらに、Ｓ５６の判断を行う。
【０１４４】
Ｓ５６では、衛星測位誤差Ｅｓが自律測位誤差Ｅａよりも小さい状態が、継続判断時間よりも長い時間継続したか否かを判断する。この判断を行う目的は、衛星測位が可能になった直後は、衛星測位誤差Ｅｓが安定しないことがある点を考慮するためである。衛星測位が可能になった直後は、衛星測位誤差Ｅｓが安定しないことがある理由は、たとえば次の理由である。すなわち、衛星測位ができない状態では、建物等の遮蔽物により観測可能衛星数が少なかった可能性があり、観測可能衛星数が増えた直後は、マルチパスにより一時的に観測できるようになっただけの可能性もあるからである。Ｓ５６の判断はこの目的で行うため、継続判断時間は、衛星測位が可能になってから、衛星測位が可能な状態で安定すると判断するための時間である。この継続判断時間は、実機での計測をもとに経験的に決めることになる。継続判断時間は、たとえば１秒などに設定する。
【０１４５】
Ｓ５６の判断がＮＯである場合は、Ｓ４７の判断がＮＯである場合と同様、Ｓ５２へ進む。一方、Ｓ５６の判断がＹＥＳになった場合には、Ｓ４８以下を実行して、実基準点Ｂｒを最新の衛星測位座標ＰＶＳに更新する。
【０１４６】
図９に、第２実施形態の誤差変化例を示す。図９の下図は図７の下図に対応しており、横軸が時刻あるいは走行距離、縦軸は誤差である。また、図９の上図は図７の上図と同一である。図７と図９において、同じ数値を標記している時刻は、同じ時刻であることを意味する。なお、図９下図の一点鎖線は、図７下図の実線を表している。
【０１４７】
図７では、ｔ１０において自律航法から衛星測位に切り替わっている。しかし、第２実施形態では、ｔ１０において衛星測位が可能になってから継続判断時間が経過するｔ１１までは自律航法を継続する。
【０１４８】
また、図７ではｔ３０〜ｔ３１において一時的に衛星測位に切り替わっている。しかし、ｔ３０〜ｔ３１の時間は継続判断時間よりも短いので、第２実施形態では、ｔ３０でも自律航法を終了しない。また、図７ではｔ３２において衛星測位に切り換えているが、第２実施形態ではｔ３５まで自律航法を継続している。
【０１４９】
自律測位誤差Ｅａは、自律航法に切り替わった時点の衛星測位誤差Ｅｓを基準として計算する。そのため、第１実施形態では、ｔ３４に示すように、衛星測位誤差Ｅｓが一時的に誤差が小さくなった場合、ｔ３４以降に計算される自律測位誤差Ｅａは小さくなる。しかし、ｔ３４において衛星測位誤差Ｅｓが小さくなっているのは一時的であり、安定した状態での衛星測位誤差Ｅｓではない。この衛星測位誤差Ｅｓは、値は小さいが、信頼性が低い。したがって、ｔ３４以降に計算される自律測位誤差Ｅａの信頼性も低くなる。
【０１５０】
それに対して、第２実施形態では、衛星測位誤差Ｅｓが自律測位誤差Ｅａよりも低い状態が継続判断時間よりも長い時間継続したときに、衛星測位に切り替える。そのため、ｔ３５まで衛星測位に切り換えず、自律航法を継続している。
【０１５１】
図９のｔ３５における実線の誤差は、図９にも比較のために破線で示している図７のｔ３５における誤差よりも大きい。しかし、図９に実線で示す誤差の方が、誤差の値は信頼性があると言える。
【０１５２】
＜第３実施形態＞
第３実施形態は、Ｓ４５で算出する衛星測位誤差Ｅｓの算出方法が第１実施形態と異なる。第１実施形態では、擬似距離残差を衛星測位誤差Ｅｓとしていた。これに対して第３実施形態では、さらに、擬似距離残差に精度劣化係数を乗じた値を衛星測位誤差Ｅｓとする。
【０１５３】
精度劣化係数は、擬似距離残差の変動の程度、航法信号のＳ／Ｎ、ＤＯＰ（Dilution Of Precision）の少なくともいずれか一つに基づいて定まる値である。ＤＯＰは精度低下率ともいう。擬似距離残差の変動の程度は、たとえば、擬似距離残差の分散で表せばよい。
【０１５４】
擬似距離残差の変動の程度、航法信号のＳ／Ｎ、ＤＯＰと、精度劣化係数との関係は、予め設定しておく。この関係は、たとえば、擬似距離残差の変動の程度、航法信号のＳ／Ｎ、ＤＯＰから精度劣化係数が定まるテーブルである。
【０１５５】
擬似距離残差の変動の程度が大きければ、衛星航法による測位誤差が大きくなる。また、航法信号のＳ／Ｎ、ＤＯＰが大きい場合も、衛星航法による測位誤差が大きくなる。そこで、第３実施形態では、擬似距離残差の変動の程度、航法信号のＳ／Ｎ、ＤＯＰの少なくともいずれか一つに基づいて精度劣化係数を定め、この精度劣化係数を擬似距離残差に乗じた値を衛星測位誤差Ｅｓとする。
【０１５６】
図１０に、第３実施形態の誤差変化例を示す。図１０の下図は図７の下図に対応しており、横軸が時刻あるいは走行距離、縦軸は誤差である。図１０の上図は図７の上図と同一である。図７と図１０において、同じ数値を標記している時刻は、同じ時刻であることを意味する。図１０下図の一点鎖線は、図７下図の実線を表している。また、図７下図の破線は図７下図の破線、すなわち第１実施形態における衛星測位誤差Ｅｓを表し、図７下図の二点鎖線は第３実施形態における衛星測位誤差Ｅｓを表している。
【０１５７】
第３実施形態の衛星測位誤差Ｅｓは擬似距離残差に精度劣化係数を乗じた値である。そのため、図１０に破線と二点鎖線で示すように、疑似距離残差をそのまま衛星測位誤差Ｅｓにしている第１実施形態の衛星測位誤差Ｅｓ（破線）よりも、第３実施形態の衛星測位誤差Ｅｓ（二点鎖線）は大きくなる。
【０１５８】
これにより、第３実施形態では、第１実施形態よりも自律航法が継続されやすくなるため、第３実施形態ではｔ３３まで自律航法が継続される。ｔ３３からｔ３４まで衛星測位が行われ、ｔ３４から自律航法が行われる。ｔ３４における衛星測位誤差Ｅｓは第１実施形態の衛星測位誤差Ｅｓよりも大きいので、ｔ３４以降の実線で示す第３実施形態の誤差は、破線で示す第１実施形態の誤差よりも大きい。しかし、図１０に実線で示す誤差の方が、誤差の値は信頼性があると言える。
【０１５９】
以上、実施形態を説明したが、開示した技術は上述の実施形態に限定されるものではなく、次の変形例も開示した範囲に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変更して実施することができる。
【０１６０】
＜変形例１＞
仮想基準点Ｂｖを用いないで自律航法を行うこともできる。図６のＳ４８からＳ５２を省略することになる。そして、Ｓ５３では、最新の衛星測位座標ＰＶＳを決定した以降の移動軌跡を決定する。Ｓ５４では、最新の衛星測位座標ＰＶＳを基準点として、その基準点から、移動軌跡分を移動させた位置を現在の位置Ｐとする。
【０１６１】
＜変形例２＞
車両１の位置Ｐを二次座標で決定してもよい。この場合には、ピッチ角、ピッチレートは不要となる。また、ロール角を考慮して車両１の位置を決めてもよい。ロール角、ロールレート、ロールレート補正値は、ヨー角、ヨーレート、ヨーレート補正値と同様にして算出することができる。
【０１６２】
＜変形例３＞
前述の実施形態では、移動体は車であったが、移動体は車以外でもよい。
【符号の説明】
【０１６３】
１：車両    ２：ＧＮＳＳ受信機    ３：車速センサ    ４：ジャイロセンサ    ５：加速度センサ    ６：シフトポジションセンサ    ７：挙動検出センサ    １００：測位装置    １０１：衛星情報取得部    １０２：車両情報取得部    １０３：センサ補正値推定部    １０３ａ：ヨーレート補正値推定部    １０３ｂ：ピッチレート補正値推定部    １０３ｃ：車速補正値推定部    １０４：衛星測位部    １０５：方位推定部    １０６：軌跡演算部    １０７：誤差推定部    １０８：位置演算部    １１０：衛星情報蓄積部    １１１：車両情報蓄積部
【図１】
2019015635.tif 000012
【図２】
2019015635.tif 000013
【図３】
2019015635.tif 000014
【図４】
2019015635.tif 000015
【図５】
2019015635.tif 000016
【図６】
2019015635.tif 000017
【図７】
2019015635.tif 000018
【図８】
2019015635.tif 000019
【図９】
2019015635.tif 000020
【図１０】
2019015635.tif 000021