特許 6962040 方位推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6962040

(24)【登録日】2021年10月18日

(45)【発行日】2021年11月5日

(54)【発明の名称】方位推定装置

(51)【国際特許分類】

   G01S 19/53 20100101AFI20211025BHJP

   G01C 21/12 20060101ALI20211025BHJP

   G01S 19/49 20100101ALI20211025BHJP

【ＦＩ】

   G01S19/53

   G01C21/12

   G01S19/49

【請求項の数】6

【全頁数】15

(21)【出願番号】特願2017-136433(P2017-136433)

(22)【出願日】2017年7月12日

(65)【公開番号】特開2019-20166(P2019-20166A)

(43)【公開日】2019年2月7日

【審査請求日】2019年8月21日

(73)【特許権者】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(73)【特許権者】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(74)【代理人】

【識別番号】100106149

【弁理士】

【氏名又は名称】矢作 和行

(74)【代理人】

【識別番号】100121991

【弁理士】

【氏名又は名称】野々部 泰平

(74)【代理人】

【識別番号】100145595

【弁理士】

【氏名又は名称】久保 貴則

(72)【発明者】

【氏名】宮島 朗

(72)【発明者】

【氏名】松岡 克宏

【審査官】 佐藤 宙子

(56)【参考文献】

【文献】 特開２００７−２０６０１０（ＪＰ，Ａ）

【文献】 特許第５８７９９７７（ＪＰ，Ｂ２）

【文献】 特開２０１６−１４７５２２（ＪＰ，Ａ）

【文献】 特開平１０−０６２１９４（ＪＰ，Ａ）

【文献】 特開２０１２−７９３９（ＪＰ，Ａ）

【文献】 特開平４−１３９２２（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｓ ５／００− ５／１４

Ｇ０１Ｓ １９／００−１９／５５

Ｇ０１Ｃ ２１／００−２１／３６

(57)【特許請求の範囲】

【請求項1】

移動体に搭載されて、前記移動体が移動する方位を逐次推定する方位推定装置であって、
ＧＮＳＳ受信機が受信する航法信号のドップラーシフト周波数を時系列的に取得する衛星データ取得部（１０１）と、
前記移動体で用いられているヨーレートセンサから前記移動体のヨーレートを時系列的に取得するセンサデータ取得部（１０２）と、
時系列的に取得された複数の前記ドップラーシフト周波数に基づいて立式される各時刻における前記移動体の方位を推定する方程式であって、時刻毎に変化する未知パラメータが、前記ヨーレートに基づいて算出される方位角変化量により拘束された方程式に基づいて、前記移動体の方位を推定するドップラー方位推定部（１０３）と、
自律航法において前記方位角変化量を加算する基準方位が更新されてからの経過時間に応じて増加することで３つ以上に変化し、方位角の信頼度を表す状態変数（Ｓ）を決定する状態変数決定部（Ｓ１４）と、
前記状態変数決定部が決定した前記状態変数が、前記方位角の信頼度が低いことを示す値であるほど、前記ドップラー方位推定部が推定した方位を前記基準方位として採用するか否かを決定するための複数組の評価パラメータ閾値群から、前記方位角の信頼度が低くても採用条件を満たしやすい前記評価パラメータ閾値群を選択する閾値選択部（Ｓ１６）と、
前記ドップラー方位推定部が方位を推定した場合に、その方位の信頼度を評価するための複数の評価パラメータを算出する評価パラメータ算出部（Ｓ１７）と、
複数の前記評価パラメータと、前記閾値選択部が選択した前記評価パラメータ閾値群とを比較し、前記採用条件を満たすと判断したことに基づいて、前記基準方位として、前記ドップラー方位推定部が推定した方位を採用することに決定する採否判断部（Ｓ１８、Ｓ１９）とを備える方位推定装置。

【請求項2】

移動体に搭載されて、前記移動体が移動する方位を逐次推定する方位推定装置であって、
ＧＮＳＳ受信機が受信する航法信号のドップラーシフト周波数を時系列的に取得する衛星データ取得部（１０１）と、
前記移動体で用いられている速度センサから前記移動体の速度の大きさを時系列的に取得するとともに、前記移動体で用いられているヨーレートセンサから前記移動体のヨーレートを時系列的に取得するセンサデータ取得部（１０２）と、
時系列的に取得された複数の前記ドップラーシフト周波数に基づいて立式される各時刻における前記移動体の方位を推定する方程式であって、時刻毎に変化する未知パラメータが前記移動体の速度の大きさおよびクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の方位を推定するドップラー方位推定部（１０３）と、
自律航法において、前記ヨーレートに基づいて算出される方位角変化量を加算する基準方位が更新されてからの経過時間に応じて増加することで３つ以上に変化し、方位角の信頼度を表す状態変数（Ｓ）を決定する状態変数決定部（Ｓ１４）と、
前記状態変数決定部が決定した前記状態変数が、前記方位角の信頼度が低いことを示す値であるほど、前記ドップラー方位推定部が推定した方位を前記基準方位として採用するか否かを決定するための複数組の評価パラメータ閾値群から、前記方位角の信頼度が低くても採用条件を満たしやすい前記評価パラメータ閾値群を選択する閾値選択部（Ｓ１６）と、
前記ドップラー方位推定部が方位を推定した場合に、その方位の信頼度を評価するための複数の評価パラメータを算出する評価パラメータ算出部（Ｓ１７）と、
複数の前記評価パラメータと、前記閾値選択部が選択した前記評価パラメータ閾値群とを比較し、前記採用条件を満たすと判断したことに基づいて、前記基準方位として、前記ドップラー方位推定部が推定した方位を採用することに決定する採否判断部（Ｓ１８、Ｓ１９）とを備える方位推定装置。

【請求項3】

前記状態変数は、前記移動体の速度が相対的に大きい場合には、前記移動体の速度が相対的に小さい場合よりも、前記経過時間の増加に伴う増加量が少ない、請求項１または２に記載の方位推定装置。

【請求項4】

前記状態変数決定部が決定した前記状態変数に基づいて方位信頼度クラスを決定する信頼度クラス決定部（Ｓ１５）を備え、
前記閾値選択部（Ｓ１６）は、前記信頼度クラス決定部が決定した前記方位信頼度クラスと速度クラスとに基づいて前記評価パラメータ閾値群を選択するようになっており、前記方位信頼度クラスが、前記方位角の信頼度が低いことを示すクラスであるほど、また、前記速度クラスが速度が低いクラスであるほど、低い前記方位角の信頼度で前記採用条件を満たす前記評価パラメータ閾値群を選択する請求項３に記載の方位推定装置。

【請求項5】

前記信頼度クラス決定部は、前記ドップラー方位推定部が一定時間以上連続して前記方位を推定できているときは、前記方位信頼度クラスを最も信頼度が高いクラスに決定する請求項４に記載の方位推定装置。

【請求項6】

前記状態変数決定部は、前記方位推定装置が電源オンになった後の前記状態変数の初期値を、前回の電源オフ時の前記状態変数の値である前回終了値とし、前記前回終了値がない場合には、予め設定された初期値とする請求項１〜５のいずれか１項に記載の方位推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

移動体が移動する方位を推定する方位推定装置に関する。

【背景技術】

【0002】

特許文献１には、時系列で、航法衛星が送信する航法信号の周波数変位量であるドップラーシフト周波数を蓄積するとともに、車速およびヨーレートの一方または両方を時系列で取得し、これらから、高精度に方位を推定する方法が開示されている。

【0003】

また、自律航法と呼ばれる現在位置推定方法が広く知られている。自律航法では、ヨーレートセンサが検出した値から定まる方位角変化量を基準方位に加算して方位を更新する。また、速度も、速度センサあるいは加速度センサの検出値から推定する。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特許第５８７９９７７号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

特許文献１に開示されている方法を用いると高精度に方位を推定できる。そのため、特許文献１に開示されている方法により推定された方位を、自律航法で用いる基準方位にすることが考えられている。

【0006】

しかし、当然、推定した方位には誤差があり、かつ、その誤差は、方位を推定する毎に変化する。そのため、特許文献１に開示されている方法で方位が推定できた場合に、必ずその方位を基準方位にしてしまうと、推定する方位の誤差が増加してしまう恐れがある。

【0007】

一方、自律航法が続くと、ヨーレートセンサが検出するヨーレートの誤差が積算されることにより、方位推定の誤差が増加していくことは広く知られている。したがって、基準方位を更新しない時間が長くなっても、方位推定誤差が大きい状態が継続することになる。

【0008】

本開示は、この事情に基づいて成されたものであり、その目的とするところは、自律航法において推定する方位の誤差増加を抑制することができる方位推定装置を提供することにある。

【課題を解決するための手段】

【0009】

上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は更なる有利な具体例を規定する。特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、開示した技術的範囲を限定するものではない。

【0010】

上記目的を達成するための１つの開示は、
移動体に搭載されて、移動体が移動する方位を逐次推定する方位推定装置であって、
ＧＮＳＳ受信機が受信する航法信号のドップラーシフト周波数を時系列的に取得する衛星データ取得部（１０１）と、
移動体で用いられているヨーレートセンサから移動体のヨーレートを時系列的に取得するセンサデータ取得部（１０２）と、
時系列的に取得された複数のドップラーシフト周波数に基づいて立式される各時刻における移動体の方位を推定する方程式であって、時刻毎に変化する未知パラメータが、ヨーレートに基づいて算出される方位角変化量により拘束された方程式に基づいて、移動体の方位を推定するドップラー方位推定部（１０３）と、
自律航法において方位角変化量を加算する基準方位が更新されてからの経過時間に応じて増加することで３つ以上に変化し、方位角の信頼度を表す状態変数（Ｓ）を決定する状態変数決定部（Ｓ１４）と、
状態変数決定部が決定した状態変数が、方位角の信頼度が低いことを示す値であるほど、ドップラー方位推定部が推定した方位を基準方位として採用するか否かを決定するための複数組の評価パラメータ閾値群から、方位角の信頼度が低くても採用条件を満たしやすい評価パラメータ閾値群を選択する閾値選択部（Ｓ１６）と、
ドップラー方位推定部が方位を推定した場合に、その方位の信頼度を評価するための複数の評価パラメータを算出する評価パラメータ算出部（Ｓ１７）と、
複数の評価パラメータと、閾値選択部が選択した評価パラメータ閾値群とを比較し、採用条件を満たすと判断したことに基づいて、基準方位として、ドップラー方位推定部が推定した方位を採用することに決定する採否判断部（Ｓ１８、Ｓ１９）とを備える方位推定装置である。

【0011】

また、上記目的を達成するための別の開示は、
移動体に搭載されて、移動体が移動する方位を逐次推定する方位推定装置であって、
ＧＮＳＳ受信機が受信する航法信号のドップラーシフト周波数を時系列的に取得する衛星データ取得部（１０１）と、
移動体で用いられている速度センサから移動体の速度の大きさを時系列的に取得するとともに、移動体で用いられているヨーレートセンサから移動体のヨーレートを時系列的に取得するセンサデータ取得部（１０２）と、
時系列的に取得された複数のドップラーシフト周波数に基づいて立式される各時刻における移動体の方位を推定する方程式であって、時刻毎に変化する未知パラメータが移動体の速度の大きさおよびクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、移動体の方位を推定するドップラー方位推定部（１０３）と、
自律航法において、ヨーレートに基づいて算出される方位角変化量を加算する基準方位が更新されてからの経過時間に応じて増加することで３つ以上に変化し、方位角の信頼度を表す状態変数（Ｓ）を決定する状態変数決定部（Ｓ１４）と、
状態変数決定部が決定した状態変数が、方位角の信頼度が低いことを示す値であるほど、ドップラー方位推定部が推定した方位を基準方位として採用するか否かを決定するための複数組の評価パラメータ閾値群から、方位角の信頼度が低くても採用条件を満たしやすい評価パラメータ閾値群を選択する閾値選択部（Ｓ１６）と、
ドップラー方位推定部が方位を推定した場合に、その方位の信頼度を評価するための複数の評価パラメータを算出する評価パラメータ算出部（Ｓ１７）と、
複数の評価パラメータと、閾値選択部が選択した評価パラメータ閾値群とを比較し、採用条件を満たすと判断したことに基づいて、基準方位として、ドップラー方位推定部が推定した方位を採用することに決定する採否判断部（Ｓ１８、Ｓ１９）とを備える方位推定装置である。

【0012】

これら開示した方位推定装置は、方位角の信頼度の大きさを表す状態変数を決定し、この状態変数が、方位角の信頼度が低いことを示すほど、複数の評価パラメータと比較する評価パラメータ閾値群を、方位角の信頼度が低くても採用条件を満たしやすい評価パラメータ閾値群とする。これにより、ドップラー方位推定部が推定した方位が、信頼度が十分に高くなくても、基準方位として採用されることになる。よって、早期に基準方位が更新されやすくなるので、自律航法において推定する方位の誤差増加を抑制することができる。

【図面の簡単な説明】

【0013】

【図1】方位推定装置１００の構成を示すブロック図である。

【図2】図１のドップラー方位推定部１０３が実行する処理を示すフローチャートである。

【図3】図１のＩＮＳ方位決定部１０４が実行する処理を示すフローチャートである。

【図4】図３のＳ１５で用いる方位信頼度クラス決定テーブルを示す図である。

【図5】図３のＳ１６で用いる閾値群選択テーブルを示す図である。

【図6】評価パラメータ閾値群リストを示す図である。

【発明を実施するための形態】

【0014】

以下、実施形態を図面に基づいて説明する。図１に示す方位推定装置１００は、ＧＮＳＳ受信機２、車速センサ３、ジャイロセンサ４、シフトポジションセンサ５とともに、移動体である車両１に搭載されている。車両１の電源がオンになると、方位推定装置１００にも電源が供給される方位推定装置１００に電源が供給されている間、方位推定装置１００は逐次、方位角θを推定する。

【0015】

＜構成の概要＞
ＧＮＳＳ受信機２は、ＧＮＳＳ（Global Navigation Satellite System）が備える航法衛星Ｓｉが送信する航法信号を受信する。なお、ｉは航法衛星の番号である。ＧＮＳＳは、たとえばＧＰＳである。航法信号は搬送波に重畳されて電波として航法衛星Ｓｉから送信される。以下、航法衛星Ｓｉが送信する電波をＧＮＳＳ電波とする。ＧＮＳＳ受信機２は、受信したＧＮＳＳ電波を復調して航法信号を取り出す。

【0016】

そして、取り出した航法信号から、擬似距離ρ_ｉ、ドップラーシフト周波数Ｄ_ｉ、衛星位置（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）、衛星の状態、航法データなどを決定する。航法データには、航法衛星Ｓｉの衛星番号、航法衛星Ｓｉの軌道情報であるエフェメリス、航法衛星Ｓｉが電波を送信した時刻などが含まれている。

【0017】

各航法衛星Ｓｉの衛星位置（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）は、各航法衛星ＳｉのエフェメリスおよびＧＮＳＳ電波を送信した時刻に基づいて算出する。擬似距離ρ_ｉは、航法衛星ＳｉがＧＮＳＳ電波を送信した時刻と、ＧＮＳＳ受信機２がＧＮＳＳ電波を受信した時刻との時刻差すなわち電波伝播時間に、光速Ｃを乗じることで算出する。

【0018】

ドップラーシフト周波数Ｄ_ｉは、航法衛星Ｓｉが送信した電波の搬送波の周波数と、受信したＧＮＳＳ電波の搬送波の周波数の周波数差である。航法衛星Ｓｉが送信する電波の搬送波周波数は予め定まっており、この周波数は、ＧＮＳＳ受信機２が備える所定の記憶部に予め記憶されている。

【0019】

ＧＮＳＳ受信機２は、これらを、受信信号である航法信号のＳ／Ｎ、航法信号を受信した時刻とともに方位推定装置１００に一定周期で出力する。ＧＮＳＳ受信機２が方位推定装置１００に出力する情報は、衛星航法において位置が演算される前の情報である。以下、衛星航法において位置が演算される前の情報を衛星データとする。ＧＮＳＳ受信機２が衛星データを出力する一定周期は、たとえば、２００ミリ秒〜４００ミリ秒の間である。航法衛星Ｓｉは複数存在している。ＧＮＳＳ受信機２は、ＧＮＳＳ電波から復調できる全ての航法信号から衛星データを決定し、決定した全ての衛星データを方位推定装置１００に出力する。

【0020】

車速センサ３は、車両１の車輪の回転速度を検出する。車速センサ３は、車輪の回転速度を示す信号を方位推定装置１００に出力する。

【0021】

ジャイロセンサ４は、車両１のヨー軸、ピッチ軸、ロール軸周りの回転角速度を検出し、検出した回転角速度を示す信号を方位推定装置１００に出力する。ジャイロセンサ４はヨー軸周りの回転角速度すなわちヨーレートを検出するのでヨーレートセンサとして機能する。

【0022】

シフトポジションセンサ５は、車両１のシフト位置を検出し、そのシフト位置を示す信号を方位推定装置１００に出力する。シフトポジションセンサ５が出力する信号により、車両１の移動方向が前進および後進のいずれであるかが定まる。

【0023】

車速センサ３、ジャイロセンサ４、シフトポジションセンサ５は、車両１の動きすなわち挙動を示す信号を出力する挙動検出センサ６である。

【0024】

方位推定装置１００は、図示しないＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータである。ＣＰＵは、ＲＡＭの一時記憶機能を利用しつつＲＯＭなどの非遷移的実体的記録媒体に記憶されているプログラムを実行する。これにより、方位推定装置１００は、衛星データ取得部１０１、センサデータ取得部１０２、ドップラー方位推定部１０３、ＩＮＳ方位決定部１０４としての機能を実行する。これらの機能を実行すると、非遷移的実体的記録媒体に記憶されているプログラムに対応する方法が実行される。なお、方位推定装置１００が実行する機能の一部または全部を、一つあるいは複数のＩＣ等によりハードウェア的に構成してもよい。

【0025】

＜方位推定装置１００が実行する処理の概要＞
次に、方位推定装置１００が実行する処理の概要を説明する。なお、方位推定装置１００が実行する一部の処理については、図２、図３に示すフローチャートを用いて詳述する。

【0026】

衛星データ取得部１０１は、ＧＮＳＳ受信機２から、衛星データを衛星データ取得周期で時系列的に取得し、取得した衛星データを衛星データ蓄積部１１０に記憶する。衛星データ取得周期は、ＧＮＳＳ受信機２が衛星データを出力する周期と同じである。衛星データ蓄積部１１０は、書き込み可能な記憶媒体であり、揮発性でも不揮発性でもよい。衛星データ蓄積部１１０には、たとえば、ＲＡＭを用いることができる。

【0027】

センサデータ取得部１０２は、挙動検出センサ６が検出した信号を、センサ値取得周期で時系列的に取得する。センサ値取得周期は、ＧＮＳＳ受信機２が航法信号を出力する周期よりも短く、たとえば、数十ミリ秒である。センサデータ取得部１０２は、取得した信号をセンサデータ蓄積部１１１に記憶する。センサデータ蓄積部１１１は、書き込み可能な記憶媒体であり、揮発性でも不揮発性でもよい。センサデータ蓄積部１１１には、たとえば、ＲＡＭを用いることができる。また、センサデータ蓄積部１１１には、衛星データ蓄積部１１０と同じ記憶媒体を用いることができる。

【0028】

ドップラー方位推定部１０３は、（１）式に示す方位推定式を用いて、初期時刻における車両１の進行方向の方位角（すなわち方位角初期値）θ^０を算出する。なお、初期時刻とは、方位角初期値θ^０を更新した時刻である。

【数1】

【0029】

（１）式は、特許文献１においては（６）式として記載されている式である。（１）式において、Ｖｓ_ｉは衛星方向速度、ｔは時刻、Ｖ_{ｗｈｅｅｌ}は車速センサ３の検出値、θ_ｇｙｒｏは、車両１の進行方向の方位角の変化量すなわち相対方位角、Ｃｂｖ^０は初期時刻におけるクロックドリフト、ＡはクロックドリフトＣｂｖの時間変化の傾き、Ｇｘ、Ｇｙは、車両１から航法衛星Ｓｉへの視線ベクトルのｘ成分、ｙ成分である。

【0030】

（１）式の左辺にある衛星方向速度Ｖｓ_ｉは、（２）式から算出する。

【数2】

【0031】

（２）式の右辺の第１項は、航法衛星Ｓｉに対する車両１の相対速度Ｖｒ_ｉを意味しており、第２項、第３項、第４項は、航法衛星Ｓｉの車両１の方向への速度である。これらの和は、航法衛星Ｓｉの方向への車両１の速度を意味することから、（２）式が成り立つのである。（２）式において、Ｄ_ｉはドップラーシフト周波数である。よって、（１）式は、ドップラーシフト周波数Ｄ_ｉを用いて方位角初期値θ^０を算出していることになる。

【0032】

図２を用いてドップラー方位推定部１０３の処理を説明する。Ｓ１では、衛星データ蓄積部１１０に記憶されている各航法衛星Ｓｉの衛星位置（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）の時系列データから、各航法衛星Ｓｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を算出する。

【0033】

Ｓ２では、衛星データ蓄積部１１０に記憶されている衛星データに含まれているドップラーシフト周波数Ｄ_ｉを（３）式に代入して、航法衛星Ｓｉに対する自車両の現在の相対速度Ｖｒ_ｉを算出する。なお、（３）式において、Ｃは光速、Ｆは航法衛星Ｓｉが送信するＧＮＳＳ電波の周波数である。

【数3】

【0034】

Ｓ３では、現在の車両１の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。車両１の位置Ｐは、車両１から航法衛星Ｓｉへの視線ベクトル（Ｇｘ_ｉ、Ｇｙ_ｉ、Ｇｚ_ｉ）を算出するために用いる。

【0035】

広く知られているように、品質のよい航法信号を４つ以上受信していれば、航法信号を用いて４つ以上の連立方程式を立式することで、未知数である位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）とクロックドリフトＣｂｖを算出することができる。しかし、ここでの車両１の位置は、次のＳ４において、車両１から航法衛星Ｓｉへの視線ベクトル（Ｇｘ_ｉ、Ｇｙ_ｉ、Ｇｚ_ｉ）を算出するために用いる。航法衛星Ｓｉは遠方に存在するため、航法衛星Ｓｉと車両１との角度を求めるために用いる現在位置は、精度が低くてもよい。したがって、品質のよい信号を４つ以上受信していなくてもよい。

【0036】

そこで、たとえば、このＳ３では、品質がよいとは判断できない信号も含めて４つ以上の航法信号を用いて、現在の車両１の座標（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出することができる。４つ以上の航法信号から現在の車両１の座標（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する場合、擬似距離ρ_ｉと、航法衛星Ｓｉの衛星位置（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓ）とを算出し、擬似距離ρ_ｉの残差が最小となるように車両１の座標（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定する。

【0037】

また、擬似距離ρ_ｉを用いた位置決定以外の精度の低い方法で車両１の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定してもよい。システム等で許容される推定精度に依存するが、車両１の位置誤差が数百ｍの範囲であれば、速度推定誤差は１ｍ／ｓｅｃ以下となり大きな問題はない。そのため、たとえば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、車両１の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定してもよい。

【0038】

Ｓ４では、車両１から航法衛星Ｓｉへの視線ベクトル（Ｇｘ_ｉ、Ｇｙ_ｉ、Ｇｚ_ｉ）を算出する。視線ベクトルのｘ成分、ｙ成分、ｚ成分は、（４）式から算出する。

【数4】

【0039】

（４）式において、ρ^ｔ_ｉは時刻ｔにおける航法衛星Ｓｉの擬似距離、（Ｘ^ｔ_ｓｉ、Ｙ^ｔ_ｓｉ、Ｚ^ｔ_ｓｉ）は時刻ｔにおける航法衛星Ｓｉの衛星位置である。（Ｘ^ｔ_Ｖ、Ｙ^ｔ_Ｖ、Ｚ^ｔ_Ｖ）は時刻ｔにおける車両１の現在位置であり、Ｓ３で算出している。

【0040】

Ｓ５では、前述した（２）式に、Ｓ２で算出した相対速度Ｖｒ_ｉ、Ｓ４で算出した視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）、Ｓ１で算出した航法衛星Ｓｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を代入して、航法衛星Ｓｉの方向への車両１の衛星方向速度Ｖｓ_ｉを算出する。

【0041】

Ｓ６では、（１）式に、Ｓ５で算出した衛星方向速度Ｖｓ_ｉ、センサデータ蓄積部１１１から取得したヨーレートを用いて更新した相対方位角θ_ｇｙｒｏ、Ｓ４で算出した視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）を代入する。これにより、（１）式において未知パラメータは、θ^０、Ｃｂｖ^０、および、Ａの３つとなる。

【0042】

したがって、（１）式に、これら衛星方向速度Ｖｓ_ｉ、相対方位角θ_ｇｙｒｏ、視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）を代入した式を３つ以上立式する。そして、それら３つ以上の式からなる連立方程式を解く。これにより、（１）式において未知パラメータとなっている方位角初期値θ^０を求めることができる。

【0043】

未知パラメータθ^０、Ｃｂｖ^０、Ａは、初期時刻以降であれば、時刻が異なっても同じである。そのため、同一時刻で３つの式を立式する必要はなく、複数の時刻において立式した式数が合計３式以上であれば、未知パラメータを求めることができる。たとえば、３時刻（ｔ_０、ｔ_１、ｔ_２）の観測衛星数がそれぞれ１であったとしても、観測された衛星データを用いて方位角初期値θ^０を求めることができる。

【0044】

ＩＮＳ方位決定部１０４は、慣性センサであるジャイロセンサ４の検出値から求められるヨーレートにセンサ値取得周期を乗じて方位角変化量（すなわち相対方位角θ_ｇｙｒｏ）を算出する。この方位角変化量を基準方位に加算して、方位角θを決定する。基準方位は、ドップラー方位推定部１０３が推定した方位角初期値θ^０である。

【0045】

センサ値取得周期でヨーレートが得られるのでＩＮＳ方位決定部１０４は、センサ値取得周期で方位角θを更新する。方位角θは、基準方位である方位角初期値θ^０に相対方位角θ_ｇｙｒｏを加算して求める。

【0046】

ただし、ＩＮＳ方位決定部１０４は、ドップラー方位推定部１０３が方位角初期値θ^０を更新する毎に、基準方位を最新の方位角初期値θ^０に更新するとは限らない。方位角初期値θ^０にも誤差があり、それまでに用いていた方位角初期値θ^０を用いた方が、誤差の少ない方位角θを決定できる可能性もあるからである。

【0047】

図３を用いて、ＩＮＳ方位決定部１０４が実行する処理を説明する。ＩＮＳ方位決定部１０４は、センサ値取得周期で図３に示す処理を実行する。

【0048】

Ｓ１１では、ドップラー方位推定部１０３が方位角初期値θ^０を算出したか否かを判断する。方位角初期値θ^０を算出するためには、衛星データが必要である。衛星データは、センサ値取得周期よりも長い衛星データ取得周期で取得する。また、電波環境が悪いと、航法信号が受信できない場合もある。そのため、Ｓ１１の判断はＮＯになることもある。前回、この図３を実行した以降にドップラー方位推定部１０３が方位角初期値θ^０を算出した場合にはＳ１１の判断をＹＥＳとする。Ｓ１１の判断がＹＥＳになった場合にはＳ１２に進む。

【0049】

Ｓ１２では、ドップラー方位推定が継続中であるか否かを判断する。ドップラー方位は、ドップラー方位推定部１０３が推定する方位、すなわち、方位角初期値θ^０を意味する。ドップラー方位推定が継続中であるとは、ドップラー方位推定部１０３が継続的に方位を推定できている状態であることを意味する。ドップラー方位推定部１０３が、衛星データ取得周期ごとに、一定時間以上連続して方位推定ができている場合に、ドップラー方位推定が継続中であるとする。一定時間は、衛星データ取得周期の複数回分以上の時間、たとえば、１秒に設定する。

【0050】

ドップラー方位推定が継続中であると、推定した方位の誤差が小さくなる、すなわち、推定した方位の信頼度が高くなる。そのため、Ｓ１２の判断がＹＥＳになった場合には、Ｓ１３において方位信頼度クラスを最高クラスとする。加えて、状態変数Ｓの値を０にする。状態変数ＳはＳ１４で説明する。Ｓ１３を実行後はＳ１９へ進む。

【0051】

Ｓ１２の判断がＮＯになった場合にはＳ１４に進む。Ｓ１４では状態変数Ｓを算出する。本実施形態では、状態変数Ｓは（５）式を用いて算出する。

【数5】

【0052】

（５）式において、Ｓ^０は状態変数Ｓの初期値である。この初期値Ｓ^０は、方位推定装置１００の電源がオンになった後、Ｓ１３が実行されるまで使用される値である。Ｓ１３が実行されると、状態変数Ｓは０になる。よって、Ｓ１３が実行されると、この初期値Ｓ^０は０になる。

【0053】

初期値Ｓ^０は、前回、方位推定装置１００の電源がオフになったときの状態変数Ｓの値である前回終了値が存在している場合にはその前回終了値である。前回終了値が存在していないときは、予め設定した初期値とする。

【0054】

（５）式において、ｔｃはＩＮＳ方位決定部１０４が方位角θの算出に用いる方位角初期値θ^０が更新され、かつ、そのときの方位信頼度クラスが最高クラスになった時刻である。ただし、方位推定装置１００の電源がオンになった後、Ｓ１３が実行されるまでは、ｔｃは電源がオンされた時刻である。

【0055】

ｔは現在の時刻、ｖは車速、Ａ（ｖ）は車速を変数として定まる値である。具体的には、Ａ（ｖ）は、速度ｖが相対的に高いときは相対的に小さくなり、速度ｖが相対的に低いときは相対的に大きくなる。たとえばＡ（ｖ）は、車速が０のときは０、速度ｖがある速度ｖ１よりも高いときはＡ１、速度ｖがその速度ｖ１よりも低くかつ０よりは高いときはＡ２（＞Ａ１）となる関数とすることができる。

【0056】

これに限られず、Ａ（Ｖ）を速度ｖが増加するのに従い、連続的に値が減少する関数とすることもできる。また、Ａ（ｖ）を、速度ｖに応じ値が定まるテーブル形式とすることもできる。

【0057】

状態変数Ｓは、ＩＮＳ方位決定部１０４が方位角θの算出に用いる方位角初期値θ^０を更新してからの経過時間が長くなるほど値が大きくなる。方位角初期値θ^０を更新してからの経過時間が長くなるほど、ヨーレートの誤差が積算されることになるため、方位角θの誤差が大きくなる。状態変数Ｓはこの誤差を評価するための変数である。したがって、状態変数Ｓの値が方位角θの誤差と相関するようにＡ（ｖ）を定める。前述したように、Ａ（ｖ）を、速度ｖが相対的に大きいときは相対的に小さくしている理由は、速度ｖが高いほど、ヨーレートは誤差が小さくなるからである。

【0058】

Ｓ１５では、状態変数Ｓと図４に示す方位信頼度クラス決定テーブルから、方位信頼度クラスを決定する。図４において、ＤＩＲＬＶｎ＿ＬＭＴ（ｎは１〜５）は、状態変数Ｓに対する閾値である。よって、図４に示す方位信頼度クラス決定テーブルは、状態変数Ｓの値から方位信頼度クラスが定まるテーブルである。

【0059】

図４において方位推定演算継続中は、前述したドップラー方位推定が継続中であることを意味する。自律航法は、ＩＮＳ方位決定部１０４が決定した方位角θと速度ｖから車両１の移動量を逐次演算して、車両１の現在位置を更新する測位方法を意味する。

【0060】

図４において、方位信頼度クラスは数値が大きいほど方位角θの信頼度が高い、換言すれば方位角θの誤差が小さいことを意味する。図４では、方位信頼度クラスが７のときは、誤差が０．５度以下であることを意味している。

【0061】

方位信頼度クラスが最高クラスのときの誤差がどの程度になるかは、参照値との差がどの程度に収束するかを、予め計測しておいて定める。参照値は、航空機で用いられる高精度ジャイロセンサ、高分解能車速計などを含んで構成される高精度位置計測システムを、車両１に搭載して測定した値である。電波環境のよい場所において、この参照値とドップラー方位推定部１０３が推定した方位との差がある範囲に収束したとき、その差の範囲を、最高クラスにおける誤差とする。

【0062】

最高クラスよりも低い方位信頼度クラスを定める状態変数Ｓの閾値も、上述の高精度位置計測システムが測定した参照値との差を実際に測定して定める。

【0063】

続くＳ１６では、Ｓ１５で決定した方位信頼度クラスと現在の速度ｖ、および図５に示す閾値群選択テーブルから、評価パラメータ閾値群Ｐを選択する。閾値群選択テーブルは、複数組の評価パラメータ閾値群Ｐから一つの評価パラメータ閾値群Ｐを選択するためのテーブルである。

【0064】

各評価パラメータ閾値群Ｐは、複数の評価パラメータに対する閾値（以下、評価パラメータ閾値）を備えている。評価パラメータ閾値は、ドップラー方位推定部１０３が推定する方位の誤差を評価するための閾値である。評価パラメータは、たとえば、航法衛星Ｓｉ毎のドップラーシフト周波数Ｄ_ｉの残差の最大値、平均、分散、共分散、時系列分散などである。（１）式、（２）式から分かるように、ドップラーシフト周波数Ｄ_ｉが、方位角初期値θ^０の値に影響を与えることから、これらが、ドップラー方位推定部１０３が推定する方位の誤差を評価する評価パラメータとなるのである。

【0065】

図５は、方位信頼度クラスと速度クラスとから評価パラメータ閾値群Ｐが定まるテーブルである。図５に示す評価パラメータ閾値群Ｐ１〜Ｐ４は、それぞれ複数の評価パラメータ閾値からなる。図６には、評価パラメータ閾値群Ｐ１〜Ｐ４が備えている評価パラメータ閾値の例を示している。図６において、ＴＨＲＥ＿ＶＡＲ１＿Ａ、ＴＨＲＥ＿ＶＡＲ１＿Ｂなどが評価パラメータ閾値である。

【0066】

図５に示す閾値群選択テーブルは、方位信頼度クラスが低くなるほど、つまり、方位信頼度クラスが、方位信頼度が低いことを示すクラスになるほど、評価パラメータ閾値が低信頼度を示す値になっている。また、速度クラスが低速側のクラスであるほど、評価パラメータ閾値が低信頼度を示す値になっている。

【0067】

図５に示す評価パラメータ閾値群Ｐを構成する各評価パラメータ閾値は、図４と同様、高精度位置計測システムを車両１に搭載して計測した参照値とドップラー方位推定部１０３が推定した方位との差が、それぞれ、図４に示す方位信頼度クラスに収束した状態で、実際に得られたドップラーシフト周波数Ｄ_ｉなどから算出して決定する。

【0068】

よって、ドップラー方位推定部１０３が方位を推定したときに、そのときのドップラーシフト周波数Ｄ_ｉなどから算出した評価パラメータを評価パラメータ閾値と比較することで、ドップラー方位推定部１０３が推定した方位の信頼度を評価することができる。

【0069】

そこで、Ｓ１７では、ドップラー方位推定部１０３が方位を推定したときのドップラーシフト周波数Ｄ_ｉなどから評価パラメータを算出する。Ｓ１８では、Ｓ１６で選択した評価パラメータ閾値群Ｐを構成する各評価パラメータ閾値と、Ｓ１７で算出した評価パラメータとを比較して、採用条件を満たすか否かを判断する。

【0070】

たとえば、全部の評価パラメータが評価パラメータ閾値を達成しているという条件とすることができる。なお、評価パラメータが評価パラメータ閾値を達成しているとは、評価パラメータのほうが、評価パラメータ閾値よりも、方位の信頼度が高い側の値になっていることを意味する。

【0071】

採用条件は、これ以外にも、複数の評価パラメータのうちの所定割合（たとえば８割）が評価パラメータ閾値を達成している場合に採用条件を満たすとしてもよい。また、各評価パラメータの達成度合いにランクを設けて点数化し、全評価パラメータの合計点数を用いて採用条件を満たしているか否かを判断してもよい。

【0072】

Ｓ１８の判断がＹＥＳになる場合、現時点の方位角θの信頼度よりも、ドップラー方位推定部１０３が推定した方位の信頼度のほうが高いと考えることができる。そこで、Ｓ１８の判断がＹＥＳであればＳ１９に進み、ドップラー方位推定部１０３が算出した方位角初期値θ^０を、ＩＮＳ方位決定部１０４で用いる基準方位として採用する。すなわち、ＩＮＳ方位決定部１０４で用いる方位角初期値θ^０を更新する。また、Ｓ１３を実行した場合にも、このＳ１９を実行する。

【0073】

Ｓ２０では、方位角初期値θ^０を更新してから現時点までの方位角変化量を算出し、その方位角変化量を方位角初期値θ^０に加算することで、方位角θを更新する。

【0074】

Ｓ２１では、Ｓ２０で更新した方位角θと、最新の方位信頼度クラスを、これらを使用するアプリケーションに出力する。アプリケーションの例としては、この方位角θと、車両１の速度ｖとを用いて自律航法を行う位置推定アプリケーションがある。

【0075】

なお、図３の処理において、Ｓ１４は状態変数決定部に相当し、Ｓ１５は信頼度クラス決定部に相当し、Ｓ１６は閾値選択部に相当し、Ｓ１７は評価パラメータ算出部に相当し、Ｓ１８、Ｓ１９は採否判断部に相当する。

【0076】

＜実施形態のまとめ＞
本実施形態の方位推定装置１００は、方位角θの信頼度の大きさを表す状態変数Ｓを算出し（Ｓ１４）、この状態変数Ｓが、方位角θの信頼度が低いことを示すほど、複数の評価パラメータと比較する評価パラメータ閾値群Ｐを、方位角θの信頼度が低くても採用条件を満たしやすい評価パラメータ閾値群Ｐとする。よって、ドップラー方位推定部１０３が推定した方位が、信頼度が十分に高くなくても、基準方位として採用されることになる。これにより、早期に基準方位が更新されやすくなるので、自律航法において推定する方位の誤差増加を抑制することができる。

【0077】

以上、実施形態を説明したが、開示した技術は上述の実施形態に限定されるものではなく、次の変形例も開示した範囲に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変更して実施することができる。

【0078】

＜変形例１＞
前述の実施形態で示した（１）式は、特許文献１にも記載されているように、次の拘束条件１〜３を採用して、衛星方向速度Ｖｓ_ｉと車両１の速度ベクトルの関係式を拘束して得られる式である。なお、Ｖｘ、Ｖｙは、それぞれ、車両１の速度ベクトルのｘ成分、ｙ成分である。

【数6】

【0079】

特許文献１にも開示されているように、拘束条件としてこれら３つの拘束条件を全て用いる必要はない。車速データ、方位角変化量、クロックドリフトＣｂｖの時間変化、いずれか一つのみによる拘束条件としてもよい。また、各々を適宜組み合わせた拘束条件としてもよい。

【0080】

拘束条件として、方位角変化量のみを用いた場合には、（１）式において、Ｖ^ｔ_{ｗｈｅｅｌ}、θ^０、およびＣｂｖ^ｔが未知パラメータとなる。よって、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋１」となる。

【0081】

また、拘束条件として、クロックドリフトＣｂｖの時間変化のみを用いた場合には、（１）式において、Ｖ^ｔ_{ｗｈｅｅｌ}、θ^０、θ^ｔ_ｇｙｒｏ、Ｃｂｖ^０、およびＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋３」となる。

【0082】

また、拘束条件として、クロックドリフトＣｂｖの時間変化および車速データを用いた場合には、（１）式において、θ^０、θ^ｔ_ｇｙｒｏ、Ｃｂｖ^０、およびＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×１＋３」となる。

【0083】

＜変形例２＞
前述の実施形態では、移動体は車であったが、移動体は車以外でもよい。

【符号の説明】

【0084】

１：車両 ２：ＧＮＳＳ受信機 ３：車速センサ ４：ジャイロセンサ ５：シフトポジションセンサ ６：挙動検出センサ １００：方位推定装置 １０１：衛星データ取得部 １０２：センサデータ取得部 １０３：ドップラー方位推定部 １０４：ＩＮＳ方位決定部 １１０：衛星データ蓄積部 １１１：センサデータ蓄積部 Ｓ 状態変数、 Ｓ１４ 状態変数決定部、 Ｓ１５ 信頼度クラス決定部、 Ｓ１６ 閾値選択部、 Ｓ１７ 評価パラメータ算出部、 Ｓ１８、Ｓ１９ 採否判断部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】