特開 2024-172786 方位算出装置及び方位算出方法

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024172786

(43)【公開日】2024-12-12

(54)【発明の名称】方位算出装置及び方位算出方法

(51)【国際特許分類】

   G01C 21/28 20060101AFI20241205BHJP

   G01C 19/00 20130101ALI20241205BHJP

【ＦＩ】

G01C21/28

G01C19/00 Z

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2023090752

(22)【出願日】2023-06-01

(71)【出願人】

【識別番号】000002369

【氏名又は名称】セイコーエプソン株式会社

(74)【代理人】

【識別番号】100090387

【弁理士】

【氏名又は名称】布施 行夫

(74)【代理人】

【識別番号】100090398

【弁理士】

【氏名又は名称】大渕 美千栄

(74)【代理人】

【識別番号】100148323

【弁理士】

【氏名又は名称】川▲崎▼ 通

(74)【代理人】

【識別番号】100168860

【弁理士】

【氏名又は名称】松本 充史

(72)【発明者】

【氏名】伊藤 郁哉

(72)【発明者】

【氏名】長島 了太

【テーマコード（参考）】

2F105

2F129

【Ｆターム（参考）】

2F105AA01

2F105AA02

2F105AA03

2F105AA05

2F105AA06

2F105BB09

2F105BB17

2F129AA03

2F129AA06

2F129AA08

2F129AA11

2F129AA14

2F129BB02

2F129BB03

2F129BB20

2F129BB22

2F129BB26

2F129BB33

2F129GG17

2F129GG18

(57)【要約】

【課題】角速度バイアスが高精度に補正された方位を算出することが可能な方位算出装置を提供すること。
【解決手段】衛星信号又は外界センサーの検出結果に基づいて第１方位を算出する第１方位演算部と、角速度信号に基づいて第２方位を算出する第２方位演算部と、前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算部と、角速度バイアスの温度特性情報を記憶する記憶部と、前記第２方位と前記第３方位とに基づいて温度センサーが検出した温度に対する前記角速度バイアスの温度特性を推定し、前記温度特性情報を更新する温度特性推定部と、前記温度と前記温度特性情報とに基づいて、前記角速度バイアスを予測する角速度バイアス予測部と、を備え、前記第２方位演算部は、予測された前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する、方位算出装置。
【選択図】図１

【特許請求の範囲】

【請求項1】

衛星信号受信機が受信した衛星信号又は外界センサーの検出結果に基づいて、第１方位を算出する第１方位演算部と、
角速度センサーが角速度を検出して出力した角速度信号に基づいて、第２方位を算出する第２方位演算部と、
前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算部と、
角速度バイアスの温度特性情報を記憶する記憶部と、
前記第２方位と前記第３方位とに基づいて、温度センサーが検出した温度に対する前記角速度バイアスの温度特性を推定し、推定した前記温度特性に基づいて、前記記憶部に記憶されている前記温度特性情報を更新する温度特性推定部と、
前記温度センサーが検出した前記温度と前記記憶部に記憶されている前記温度特性情報とに基づいて、前記角速度バイアスを予測する角速度バイアス予測部と、を備え、
前記第２方位演算部は、前記角速度バイアス予測部が予測した前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する、方位算出装置。

【請求項2】

請求項１において、
前記角速度バイアスは、温度変化を変数とする多項式で近似される、方位算出装置。

【請求項3】

請求項２において、
前記温度特性情報は、複数の温度の各々に対する前記多項式の係数の値を含む、方位算出装置。

【請求項4】

請求項２において、
前記多項式は、１次多項式である、方位算出装置。

【請求項5】

請求項１において、
前記角速度バイアスは、前記第２方位の角速度のバイアスである、方位算出装置。

【請求項6】

請求項１において、
前記角速度バイアスは、前記角速度センサーが検出するヨー軸回りの角速度のバイアスである、方位算出装置。

【請求項7】

請求項３において、
前記温度特性推定部は、前記温度特性として前記係数の値を推定し、推定した前記係数の分散を算出し、前記温度特性情報に、前記温度センサーが検出した前記温度に対する前記係数の値が含まれている場合、前記分散が所定の閾値以下のときは、前記温度特性情報に含まれている前記係数の値を、推定した前記係数の値に更新し、前記分散が所定の閾値よりも大きいときは、前記温度特性情報に含まれている前記係数の値を更新しない、方位算出装置。

【請求項8】

衛星信号受信機が受信した衛星信号又は外界センサーの検出結果に基づいて、第１方位を算出する第１方位演算工程と、
角速度センサーが角速度を検出して出力した角速度信号に基づいて、第２方位を算出する第２方位演算工程と、
前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算工程と、
前記第２方位と前記第３方位とに基づいて、温度センサーが検出した温度に対する角速度バイアスの温度特性を推定し、推定した前記温度特性に基づいて、記憶部に記憶されている前記角速度バイアスの温度特性情報を更新する温度特性推定工程と、
前記温度センサーが検出した前記温度と前記記憶部に記憶されている前記温度特性情報とに基づいて、前記角速度バイアスを予測する角速度バイアス予測工程と、を含み、
前記第２方位演算工程では、前記角速度バイアス予測工程で予測した前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する、方位算出方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、方位算出装置及び方位算出方法に関する。

【背景技術】

【0002】

特許文献１には、移動体の停止時の温度検出値と角速度の検出値とに基づいて角速度バイアスの温度特性データを求め、角速度バイアスの温度特性データが有効であるとき、角速度センサーの温度特性と温度とから求めた角速度バイアスを初期値としてＧＳＰ／ＩＮＳ統合演算を行い、角速度バイアス及び角速度をリアルタイムに推定して、移動体の方位を求める、航法装置が記載されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００９－１９２４９５号公報

【発明の概要】

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

【0004】

しかしながら、特許文献１に記載の制御装置では、移動体の停止時の温度検出値と角速度の検出値との関係を蓄積して角速度バイアスの温度特性データを求める必要があるため、移動体が停止する回数が少なく蓄積するデータの量が十分でない場合や、走行時の温度が停止時の温度と異なる場合には、角速度バイアスの推定精度が低下するおそれがある。

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

【0005】

本発明に係る方位算出装置の一態様は、
衛星信号受信機が受信した衛星信号又は外界センサーの検出結果に基づいて、第１方位を算出する第１方位演算部と、
角速度センサーが角速度を検出して出力した角速度信号に基づいて、第２方位を算出する第２方位演算部と、
前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算部と、
角速度バイアスの温度特性情報を記憶する記憶部と、
前記第２方位と前記第３方位とに基づいて、温度センサーが検出した温度に対する前記角速度バイアスの温度特性を推定し、推定した前記温度特性に基づいて、前記記憶部に記憶されている前記温度特性情報を更新する温度特性推定部と、
前記温度センサーが検出した前記温度と前記記憶部に記憶されている前記温度特性情報とに基づいて、前記角速度バイアスを予測する角速度バイアス予測部と、を備え、
前記第２方位演算部は、前記角速度バイアス予測部が予測した前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する。

【0006】

本発明に係る方位算出方法の一態様は、
衛星信号受信機が受信した衛星信号又は外界センサーの検出結果に基づいて、第１方位を算出する第１方位演算工程と、
角速度センサーが角速度を検出して出力した角速度信号に基づいて、第２方位を算出する第２方位演算工程と、
前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算工程と、
前記第２方位と前記第３方位とに基づいて、温度センサーが検出した温度に対する角速度バイアスの温度特性を推定し、推定した前記温度特性に基づいて、記憶部に記憶されている前記角速度バイアスの温度特性情報を更新する温度特性推定工程と、
前記温度センサーが検出した前記温度と前記記憶部に記憶されている前記温度特性情報
とに基づいて、前記角速度バイアスを予測する角速度バイアス予測工程と、を含み、
前記第２方位演算工程では、前記角速度バイアス予測工程で予測した前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する。

【図面の簡単な説明】

【0007】

【図1】第１実施形態の方位算出装置の構成例を示す図。

【図2】Ｘ軸、Ｙ軸及びＺ軸の説明図。

【図3】温度特性情報の一例を示す図。

【図4】角速度バイアスの温度特性の一例を示す図。

【図5】本実施形態の方位算出方法の手順の一例を示すフローチャート図。

【図6】第１実施形態の方位算出方法の具体的な手順の一例を示すフローチャート図。

【図7】第２実施形態の方位算出方法の具体的な手順の一例を示すフローチャート図。

【図8】第３実施形態の方位算出装置の構成例を示す図。

【図9】第３実施形態の方位算出方法の具体的な手順の一例を示すフローチャート図。

【図10】第３実施形態の方位算出方法の具体的な手順の他の一例を示すフローチャート図。

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

【0008】

以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。

【0009】

１．第１実施形態
１－１．方位算出装置
図１は、第１実施形態の方位算出装置の構成例を示す図である。図１に示すように、第１実施形態の方位算出装置１は、第１方位演算部２０、第２方位演算部３０、第３方位演算部４０、角速度バイアス予測部５０、温度特性推定部６０及び記憶部７０を備えている。また、方位算出装置１は、衛星信号受信機１１、車輪速センサー１２、角速度センサー１３、６Ｄｏｆセンサー１４及び温度センサー１５を備えていてもよい。Ｄｏｆは、Degrees Of Freedomの略称である。なお、方位算出装置１は、図１の構成要素の一部を省略又は変更し、あるいは、他の構成要素を付加した構成としてもよい。

【0010】

方位算出装置１は、移動体に搭載される。方位算出装置１が搭載される移動体には、Ｘ軸、Ｙ軸及びＺ軸による３軸座標系であるローカル座標系が定義される。Ｘ軸は移動体の進行方向を正とする軸であり、Ｙ軸は移動体の進行方向と直交する右方向を正とする軸であり、Ｚ軸は移動体が走行する平面に垂直な下方向を正とする軸である。また、移動体が移動する空間にはグローバル座標系が定義される。グローバル座標系は、地球に固定された座標系である。以下の説明では、方位算出装置１が移動体である自動車に搭載される場合を想定する。

【0011】

衛星信号受信機１１は、不図示のアンテナを介して、衛星から送信される衛星信号を受信する。衛星は、衛星測位システムの一部を構成する。衛星測位システムはＧＮＳＳ（Global Navigation Satellite System）であってもよい。ＧＮＳＳとしては、例えば、ＧＰＳ（Global Positioning System）、ＥＧＮＯＳ（European Geostationary Navigation Overlay Service）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（Global Navigation Satellite System）、ＧＡＬＩＬＥＯ、ＢｅｉＤｏｕ（BeiDou Navigation Satellite System）などが挙げられる。

【0012】

車輪速センサー１２は、移動体である自動車の車輪の回転速度を検出し、車輪速信号を出力する。

【0013】

角速度センサー１３は、角速度を検出し、角速度信号を出力する。具体的には、角速度センサー１３は、１つの検出軸を有し、当該検出軸がＺ軸に沿うように移動体に搭載されており、Ｚ軸回りの角速度を検出し、Ｚ軸角速度信号を出力する。例えば、角速度センサー１３は、水晶を材料とする不図示のセンサー素子を有し、角速度を高精度に検出する水晶ジャイロセンサーであってもよい。

【0014】

６Ｄｏｆセンサー１４は、互いに直交する３軸回りの角速度及び当該３軸方向の加速度を検出し、３軸角速度信号及び３軸加速度信号を出力する。具体的には、６Ｄｏｆセンサー１４は、互いに直交する３つの検出軸を有し、当該３つの検出軸がそれぞれＸ軸、Ｙ軸及びＺ軸に沿うように移動体に搭載されている。そして、６Ｄｏｆセンサー１４は、Ｘ軸回りの角速度、Ｙ軸回りの角速度及びＺ軸回りの角速度を検出して３軸角速度信号を出力するとともに、Ｘ軸方向の加速度、Ｙ軸方向の加速度及びＺ軸方向の加速度を検出して３軸加速度信号を出力する。例えば、６Ｄｏｆセンサー１４は、シリコン基板をＭＥＭＳ技術で加工した静電容量型のセンサーであってもよい。

【0015】

例えば、角速度センサー１３及び６Ｄｏｆセンサー１４は、角速度センサー１３の検出軸と６Ｄｏｆセンサー１４の３つの検出軸のいずれか１つが同じ方向となるように１つの筐体に収容され、３軸回りの角速度及び当該３軸方向の加速度を検出するセンサーモジュール１０を構成してもよい。この場合、例えば、図２に示すように、センサーモジュール１０は、３軸がそれぞれＸ軸、Ｙ軸及びＺ軸に沿うように移動体である自動車５に搭載される。そして、角速度センサー１３はＺ軸回りの角速度を検出し、６Ｄｏｆセンサー１４は、Ｘ軸回りの角速度、Ｙ軸回りの角速度、Ｚ軸回りの角速度、Ｘ軸方向の加速度、Ｙ軸方向の加速度及びＺ軸方向の加速度を検出する。センサーモジュール１０は、角速度センサー１３が検出したＺ軸回りの角速度と、６Ｄｏｆセンサー１４が検出した３軸回りの角速度及び３軸方向の加速度とを含むデータを出力してもよい。また、例えば、角速度センサー１３が検出したＺ軸回りの角速度が、６Ｄｏｆセンサー１４が検出したＺ軸回りの角速度よりも精度が高い場合、センサーモジュール１０は、角速度センサー１３が検出したＺ軸回りの角速度と、６Ｄｏｆセンサー１４が検出したＸ軸回りの角速度、Ｙ軸回りの角速度及び３軸方向の加速度とを含むデータを出力してもよい。

【0016】

ただし、角速度センサー１３と６Ｄｏｆセンサー１４とは、互いに別体であってもよい。

【0017】

温度センサー１５は、温度を検出し、温度信号を出力する。例えば、温度センサー１５は、角速度センサー１３の近傍に配置され、角速度センサー１３の温度を検出する。あるいは、角速度センサー１３と温度センサー１５とは、１つの筐体に収容されていてもよい。

【0018】

第１方位演算部２０は、衛星信号受信機１１が受信した衛星信号に基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第１方位Ψ_{ｇｎｓｓ，ｋ}を算出する。第１方位演算部２０は、例えば１秒等の所定の周期で第１方位Ψ_{ｇｎｓｓ，ｋ}を算出する。例えば、第１方位演算部２０は、ドップラー効果により衛星の位置と移動体の速度との関係に応じて衛星信号の周波数がシフトする現象を利用して移動体の速度ベクトルを算出し、速度ベクトルの向きから第１方位Ψ_{ｇｎｓｓ，ｋ}を算出してもよい。また、例えば、第１方位演算部２０は、衛星信号に基づいて、グローバル座標系における移動体の位置を算出し、位置を微分して速度ベクトルを算出し、速度ベクトルの向きから第１方位Ψ_{ｇｎｓｓ
，ｋ}を算出してもよい。また、例えば、移動体の異なる２つの位置にそれぞれ衛星信号を受信するアンテナが設けられており、衛星信号受信機１１が２つのアンテナでそれぞれ受信した衛星信号に基づいて２つのアンテナの位置を算出し、算出した２つの位置から第１方位Ψ_{ｇｎｓｓ，ｋ}を算出してもよい。あるいは、移動体に２つの衛星信号受信機１１が搭載されており、２つの衛星信号受信機１１がそれぞれ受信した衛星信号に基づいて２つの位置を算出し、算出した２つの位置から第１方位Ψ_{ｇｎｓｓ，ｋ}を算出してもよい。なお、第１方位演算部２０は、公知の衛星航法により、グローバル座標系における移動体の位置や速度をさらに算出してもよい。

【0019】

第２方位演算部３０は、角速度センサー１３から出力される角速度信号に基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第２方位Ψ_{ｉｎｓ，ｋ}を算出する。第２方位演算部３０は、角速度バイアス予測部５０が予測した前回の時刻ｋ－１の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された時刻ｋの第２方位Ψ_{ｉｎｓ，ｋ}を算出する。第２方位演算部３０は、例えば０．０１秒等の所定の周期で第２方位Ψ_{ｉｎｓ，ｋ}を算出する。具体的には、第２方位演算部３０は、角速度センサー１３が検出した時刻ｋにおけるＺ軸回りの角速度ω_ｚと、６Ｄｏｆセンサー１４が検出した時刻ｋにおけるＸ軸周りの角速度ω_ｘ及びＹ軸周りの角速度ω_ｙとに基づいて、式（１）により、ロール角φ、ピッチ角θ及びヨー角ψのそれぞれの角速度を算出する。図２に示すように、ロール角φは、自動車５の進行方向に沿うＸ軸を回転軸とする回転角である。また、ピッチ角θは、自動車５の進行方向と直交する右方向に沿うＹ軸を回転軸とする回転角である。また、ヨー角ψは、自動車５が走行する平面に垂直な下方向に沿うＺ軸を回転軸とする回転角である。ヨー角ψは、移動体の相対的な方位を表し、ロール角φ及びピッチ角θは、移動体の姿勢を表す。

【0020】

【数1】

【0021】

そして、第２方位演算部３０は、第３方位演算部４０が算出した前回の時刻ｋ－１における第３方位Ψ_{ｅｓｔ，ｋ－１}を初期方位として、第３方位Ψ_{ｅｓｔ，ｋ－１}に、ヨー角ψの角速度から角速度バイアスδΩ_ｋ－１を減算して積分した値を加算して、時刻ｋにおける第２方位Ψ_{ｉｎｓ，ｋ}を算出する。なお、自動車５の走行制御では、ロール角φ、ピッチ角θ及びヨー角ψのうちヨー角ψが最も重要である。そのため、方位算出装置１は、ヨー角ψを高精度に算出するために、６Ｄｏｆセンサー１４に加えて、Ｚ軸回りの角速度を高精度に検出する角速度センサー１３を備えている。

【0022】

第３方位演算部４０は、第１方位演算部２０が算出した時刻ｋにおける第１方位Ψ_{ｇｎｓｓ，ｋ}と第２方位演算部３０が算出した時刻ｋにおける第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第３方位Ψ_{ｅｓｔ，ｋ}を算出する。第３方位演算部４０は、衛星航法と慣性航法とを複合した公知の複合航法により、例えば１秒等の所定の周期で第３方位Ψ_{ｅｓｔ，ｋ}を算出する。例えば、第３方位演算部４０は、第１方位Ψ_{ｇｎｓｓ，ｋ}が算出されるタイミングでは、第１方位Ψ_{ｇｎｓｓ，ｋ}を第３方位Ψ_{ｅｓｔ，ｋ}とし、第１方位Ψ_{ｇｎｓｓ，ｋ}が算出されないタイミングでは第２方位Ψ_{ｉｎｓ，ｋ}を第３方位Ψ_{ｅｓｔ，ｋ}としてもよい。また、第３方位演算部４０は、カルマンフィルターを用いて、第３方位Ψ_{ｅｓｔ，ｋ}を算出してもよい。なお、第３方位演算部４０は、移動体の速度、姿勢及び方位に基づいて、移動体の位置をさらに算出してもよい。移動体の速度は、第１方位演算部２０が算出した速度であってもよいし、車輪速センサー１２から出力される車輪速信号に基づいて算出される速度であってもよい。
また、移動体の姿勢は、ロール角Φ_{ｅｓｔ，ｋ}及びピッチ角Θ_{ｅｓｔ，ｋ}によって表され、第２方位演算部３０が算出したロール角φ及びピッチ角θであってもよい。

【0023】

記憶部７０は、角速度バイアスδΩ_ｋの温度特性情報７１を記憶する。本実施形態では、角速度バイアスδΩ_ｋは、第２方位演算部３０が算出した第２方位Ψ_{ｉｎｓ，ｋ}の角速度のバイアスであり、式（２）で表される。式（２）におけるδΨ_{ｉｎｓ，ｋ}は、第２方位Ψ_{ｉｎｓ，ｋ}の誤差であり、式（３）に示すように、第２方位Ψ_{ｉｎｓ，ｋ}と第３方位演算部４０が算出した時刻ｋにおける第３方位Ψ_{ｅｓｔ，ｋ}との差分である。式（２）におけるΔｔは、第３方位演算部４０が第３方位Ψ_{ｅｓｔ，ｋ}を算出する周期であり、例えば１秒である。

【0024】

【数2】

【0025】

【数3】

【0026】

角速度バイアスδΩ_ｋは、式（４）のように、時刻ｋにおける温度変化ΔＴ_ｋを変数とするｎ次多項式で近似される。温度変化ΔＴ_ｋは、温度センサー１５が検出した時刻ｋにおける温度Ｔ_ｋと時刻ｋ－１における温度Ｔ_ｋとの差分である。

【0027】

【数4】

【0028】

本実施形態では、温度特性情報７１は、複数の温度の各々に対する多項式（４）の係数ｃ_０，ｋ～ｃ_ｎ，ｋの値を含む。図３は、温度特性情報７１の一例を示す図である。図３の例では、温度特性情報７１は、－４０℃から＋８５℃までの１℃毎の温度に対する係数ｃ_０，ｋ～ｃ_ｎ，ｋの値を含む。なお、温度特性情報７１は、少なくとも１つの温度に対して係数ｃ_０，ｋ～ｃ_ｎ，ｋの値の少なくとも１つを含んでいなくてもよい。

【0029】

温度特性推定部６０は、第２方位演算部３０が算出した第２方位Ψ_{ｉｎｓ，ｋ}と第３方位演算部４０が算出した第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて、温度センサー１５が検出した温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性を推定し、推定した温度特性に基づいて記憶部７０に記憶されている温度特性情報７１を更新する。温度特性推定部６０は、温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性として多項式（４）の係数ｃ_０，ｋ～ｃ_ｎ，ｋの値を推定し、推定した係数ｃ_０，ｋ～ｃ_ｎ，ｋの分散を算出してもよい。そして、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する多項式（４）の係数ｃ_ｉ，ｋの値が含まれている場合、係数ｃ_ｉ，ｋの分散が所定の閾値以下のときは、温度特性情報７１に含まれている係数ｃ_ｉ，ｋの値を、推定した係数ｃ_ｉ，ｋの値に更新し、係数ｃ_ｉ，ｋの分散が所定の閾値よりも大きいときは、温度特性情報７１に含まれている係数ｃ_ｉ，ｋの値を更新しなくてもよい。すなわち、温度特性推定部６０は、温度Ｔ_ｋに対する係数ｃ_ｉ，ｋの値の推定精度が所定値以上の場合のみ、温度特性情報７１に含まれている温度Ｔ_ｋに対する係数ｃ_ｉ，ｋの値を更新してもよい。記憶部７０に、図３に示した温度特性情報７１が記憶されている場合、例えば、温度Ｔ_ｋが２５℃であり、係数ｃ_０
，ｋ～ｃ_ｎ，ｋの分散がすべて所定の閾値以下であれば、温度特性推定部６０は、温度特性情報７１に含まれている２５℃に対する係数ｃ_０，ｋ～ｃ_ｎ，ｋの値ｃ_{０（＋２５℃）}～ｃ_{ｎ（＋２５℃）}を、推定した係数ｃ_０，ｋ～ｃ_ｎ，ｋの値に更新する。

【0030】

なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する多項式（４）の係数ｃ_０，ｋ～ｃ_ｎ，ｋの値が含まれていない場合は、分散の値によらず、温度特性情報７１に含まれている係数ｃ_０，ｋ～ｃ_ｎ，ｋの値を、推定した係数ｃ_０，ｋ～ｃ_ｎ，ｋの値に更新する。

【0031】

温度特性推定部６０は、第３方位Ψ_{ｅｓｔ，ｋ}が更新される所定の周期、例えば１秒等の短い周期で角速度バイアスδΩ_ｋの温度特性を推定する。したがって、温度特性推定部６０が温度特性を推定する各周期において、温度変化ΔＴ_ｋは微小である。図４に示すように、温度変化ΔＴ_ｋが微小であれば、角速度バイアスδΩ_ｋは、温度変化ΔＴ_ｋに対して線形に変化するものとみなすことができる。すなわち、角速度バイアスδΩ_ｋは、式（５）のように、温度変化ΔＴ_ｋの１次多項式で近似される。式（５）は、ｎが１の場合の多項式（４）に相当する。

【0032】

【数5】

【0033】

したがって、温度特性推定部６０は、温度センサー１５から出力される温度信号に応じた温度Ｔ_ｋに対する式（５）の多項式の１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋを推定してもよい。本実施形態では、温度特性推定部６０は、拡張カルマンフィルターを用いて、温度Ｔ_ｋに対する１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋを推定する。

【0034】

本実施形態では、式（６）に示すように、１次係数ｃ_１，ｋ、０次係数ｃ_０，ｋ及び温度Ｔ_ｋを要素とする状態変数ｘ_ｋが定義される。

【0035】

【数6】

【0036】

状態方程式は、式（７）で定義される。式（７）において、Ｆ_ｋは、状態遷移行列であり、式（８）に示すように、３×３の単位行列であるものとする。また、式（７）において、ｖ_ｋは駆動雑音であり、式（９）に示すように、あらかじめ適切な値に設定されたσ_ｖ１，σ_ｖ２，σ_ｖ３を要素とする。

【0037】

【数7】

【0038】

【数8】

【0039】

【数9】

【0040】

本実施形態では、式（１０）に示すように、角速度バイアスδΩ_ｋ、角速度バイアス変化δΩ_ｋ－δΩ_ｋ－１、温度Ｔ_ｋを要素とする観測変数ｙ_ｋが定義される。

【0041】

【数10】

【0042】

観測方程式は、式（１１）で定義される。式（１１）において、Ｈ_ｋは観測ヤコビアンであり、式（１２）で表される。また、式（１１）において、ｗ_ｋは観測雑音であり、式（１３）に示すように、あらかじめ適切な値に設定されたσ_ｗ１，σ_ｗ２，σ_ｗ３を要素とする。

【0043】

【数11】

【0044】

【数12】

【0045】

【数13】

【0046】

温度特性推定部６０は、拡張カルマンフィルターの予測ステップ、観測ステップ及び更新ステップを実行し、角速度バイアスδΩ_ｋの温度特性を推定する。

【0047】

まず、温度特性推定部６０は、予測ステップにおいて、状態方程式（７）に基づく式（１４）により、時刻ｋ－１の更新ステップで更新した状態変数ｘ＾_{ｋ－１｜ｋ－１}から、時刻ｋの状態変数ｘ＾_{ｋ｜ｋ－１}を予測する。すなわち、状態変数ｘ＾_{ｋ－１｜ｋ－１}は時刻ｋ－１の事後推定値であり、状態変数ｘ＾_{ｋ｜ｋ－１}は時刻ｋの事前推定値である。

【0048】

【数14】

【0049】

また、温度特性推定部６０は、予測ステップにおいて、式（１５）により、時刻ｋ－１の更新ステップで更新した状態変数ｘ＾_{ｋ－１｜ｋ－１}の共分散行列Ｐ_{ｋ－１｜ｋ－１}か
ら、時刻ｋの状態変数ｘ＾_{ｋ｜ｋ－１}の共分散行列Ｐ_{ｋ｜ｋ－１}を予測する。式（１５）において、Ｖ_ｋは、駆動雑音ｖ_ｋの共分散行列であり、式（１６）によって算出される。

【0050】

【数15】

【0051】

【数16】

【0052】

次に、温度特性推定部６０は、観測ステップにおいて、観測方程式（１１）に基づく式（１７）により、時刻ｋの予測ステップで予測した状態変数ｘ＾_{ｋ｜ｋ－１}から観測される値を算出する。式（１７）において、温度変化ΔＴ_ｋは、式（１８）のように、状態変数ｘ＾_{ｋ｜ｋ－１}に含まれる温度Ｔ_{ｋ｜ｋ－１}と状態変数ｘ＾_{ｋ－１｜ｋ－１}に含まれる温度Ｔ_{ｋ－１｜ｋ－１}との差分である。また、温度変化ΔＴ_ｋ－１は、式（１９）のように、状態変数ｘ＾_{ｋ－１｜ｋ－１}に含まれる温度Ｔ_{ｋ－１｜ｋ－１}と状態変数ｘ＾_{ｋ－２｜ｋ－２}に含まれる温度Ｔ_{ｋ－２｜ｋ－２}との差分である。

【0053】

【数17】

【0054】

【数18】

【0055】

【数19】

【0056】

そして、温度特性推定部６０は、式（２０）により、実際に観測された時刻ｋの観測変数ｙ_ｋの値と式（１７）によって算出された値との差分である観測残差ｅ_ｋを算出する。

【0057】

【数20】

【0058】

また、温度特性推定部６０は、観測ステップにおいて、式（２１）により、時刻ｋの予測ステップで予測した状態変数ｘ＾_{ｋ｜ｋ－１}の共分散行列Ｐ_{ｋ｜ｋ－１}から、観測残差ｅ_ｋの共分散行列Ｓ_ｋを算出する。式（２１）において、Ｗ_ｋは、観測雑音ｗ_ｋの共分散行列であり、式（２２）によって算出される。また、Ｈ_ｋは、観測ヤコビアン行列であり、式（２３）によって算出される。

【0059】

【数21】

【0060】

【数22】

【0061】

【数23】

【0062】

最後に、温度特性推定部６０は、更新ステップにおいて、時刻ｋの予測ステップで予測した状態変数ｘ＾_{ｋ｜ｋ－１}の共分散行列Ｐ_{ｋ｜ｋ－１}と、時刻ｋの観測ステップで算出した観測残差ｅ_ｋの共分散行列Ｓ_ｋとから、式（２４）により、カルマンゲインＫ_ｋを算出する。

【0063】

【数24】

【0064】

そして、温度特性推定部６０は、更新ステップにおいて、式（２５）により、カルマンゲインＫ_ｋと、時刻ｋの観測ステップで算出した観測残差ｅ_ｋとに基づいて、時刻ｋの予測ステップで予測した状態変数ｘ＾_{ｋ｜ｋ－１}を状態変数ｘ＾_ｋ｜ｋに更新する。前述の通り、状態変数ｘ＾_{ｋ｜ｋ－１}は時刻ｋの事前推定値である。また、状態変数ｘ＾_ｋ｜ｋは時刻ｋの事後推定値である。

【0065】

【数25】

【0066】

このように、温度特性推定部６０は、拡張カルマンフィルターを用いて、状態変数ｘ_ｋに含まれる１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋを推定する。

【0067】

また、温度特性推定部６０は、更新ステップにおいて、式（２６）により、カルマンゲインＫ_ｋに基づいて、時刻ｋの予測ステップで予測した状態変数ｘ＾_{ｋ｜ｋ－１}の共分散行列Ｐ_{ｋ｜ｋ－１}を状態変数ｘ＾_ｋ｜ｋの共分散行列Ｐ_ｋ｜ｋに更新する。

【0068】

【数26】

【0069】

共分散行列Ｐ_ｋ｜ｋにおいて、ｐ_１１は１次係数ｃ_１，ｋの分散であり、ｐ_２２は０次係数ｃ_０，ｋの分散である。温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対
する１次係数ｃ_１，ｋの値が含まれている場合、分散ｐ_１１が所定の閾値以下のときは、温度特性情報７１に含まれている１次係数ｃ_１，ｋの値を、推定した１次係数ｃ_１，ｋの値に更新し、分散ｐ_１１が所定の閾値よりも大きいときは、温度特性情報７１に含まれている１次係数ｃ_１，ｋの値を更新しない。なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する１次係数ｃ_１，ｋの値が含まれていない場合は、分散ｐ_１１の値によらず、温度特性情報７１に含まれている１次係数ｃ_１，ｋの値を、推定した１次係数ｃ_１，ｋの値に更新する。

【0070】

同様に、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する０次係数ｃ_０，ｋの値が含まれている場合、分散ｐ_２２が所定の閾値以下のときは、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を、推定した０次係数ｃ_０，ｋの値に更新し、分散ｐ_２２が所定の閾値よりも大きいときは、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を更新しない。なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する０次係数ｃ_０，ｋの値が含まれていない場合は、分散ｐ_２２の値によらず、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を、推定した０次係数ｃ_０，ｋの値に更新する。

【0071】

角速度バイアス予測部５０は、温度センサー１５が検出した温度Ｔ_ｋと、記憶部７０に記憶されている温度特性情報７１とに基づいて、角速度バイアスδΩ_ｋを予測する。例えば、角速度バイアス予測部５０は、温度Ｔ_ｋ－１と温度Ｔ_ｋと温度特性情報７１に含まれる温度Ｔ_ｋに対する係数ｃ_０，ｋ～ｃ_ｎ，ｋとを多項式（４）に代入して角速度バイアスδΩ_ｋを予測してもよい。

【0072】

このように構成されている方位算出装置１は、移動体である自動車５が停止中か走行中かによらず、例えば１秒等の所定の周期で角速度バイアスδΩ_ｋを推定し、角速度バイアスが補正された高精度の第３方位Ψ_{ｅｓｔ，ｋ}を算出することができる。

【0073】

なお、角速度センサー１３を製造する者や場所と、角速度センサー１３を含む方位算出装置１の一部又は全部のモジュール化を行う者や場所とは、同じであってもよいし、異なってもよい。

【0074】

１－２．方位算出方法
図５は、本実施形態の方位算出方法の手順の一例を示すフローチャート図である。図５に示すように、本実施形態の方位算出方法は、第１方位演算工程Ｓ１０と、第２方位演算工程Ｓ２０と、第３方位演算工程Ｓ３０と、温度特性推定工程Ｓ４０と、角速度バイアス予測工程Ｓ５０と、を含む。本実施形態では、方位算出装置１が図５の手順に従って各工程の処理を行う。方位算出装置１は、不図示のプログラムを実行することにより、各工程の処理を行ってもよい。

【0075】

まず、第１方位演算工程Ｓ１０において、方位算出装置１の第１方位演算部２０が、衛星信号受信機１１が受信した衛星信号に基づいて、第１方位Ψ_{ｇｎｓｓ，ｋ}を算出する。

【0076】

次に、第２方位演算工程Ｓ２０において、方位算出装置１の第２方位演算部３０が、角速度センサー１３が角速度を検出して出力した角速度信号に基づいて、第２方位Ψ_{ｉｎｓ，ｋ}を算出する。第２方位演算部３０は、後述する角速度バイアス予測工程Ｓ５０で予測された前回の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出する。

【0077】

次に、第３方位演算工程Ｓ３０において、方位算出装置１の第３方位演算部４０が、第１方位演算工程Ｓ１０で算出された第１方位Ψ_{ｇｎｓｓ，ｋ}と第２方位演算工程Ｓ２０で
算出された第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて第３方位Ψ_{ｅｓｔ，ｋ}を算出する。

【0078】

次に、温度特性推定工程Ｓ４０において、方位算出装置１の温度特性推定部６０が、第２方位演算工程Ｓ２０で算出された第２方位Ψ_{ｉｎｓ，ｋ}と第３方位演算工程Ｓ３０で算出された第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて、温度センサー１５が検出した温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性を推定し、推定した温度特性に基づいて、記憶部７０に記憶されている角速度バイアスδΩ_ｋの温度特性情報７１を更新する。

【0079】

温度特性推定部６０は、角速度バイアスδΩ_ｋの温度特性として多項式（４）の係数ｃ_０，ｋ～ｃ_ｎ，ｋの値を推定し、推定した係数ｃ_０，ｋ～ｃ_ｎ，ｋの分散を算出し、温度特性情報７１に、温度Ｔ_ｋに対する係数ｃ_ｉ，ｋの値が含まれている場合、係数ｃ_ｉ，ｋの分散が所定の閾値以下のときは、温度特性情報７１に含まれている係数ｃ_ｉ，ｋの値を、推定した係数ｃ_ｉ，ｋの値に更新し、係数ｃ_ｉ，ｋの分散が所定の閾値よりも大きいときは、温度特性情報７１に含まれている係数ｃ_ｉ，ｋの値を更新しなくてもよい。

【0080】

次に、角速度バイアス予測工程Ｓ５０において、方位算出装置１の角速度バイアス予測部５０が、温度センサー１５が検出した温度Ｔ_ｋと記憶部７０に記憶されている温度特性情報７１とに基づいて、角速度バイアスδΩ_ｋを予測する。

【0081】

方位算出装置１は、工程Ｓ６０において方位の算出処理を終了するまで、工程Ｓ１０～Ｓ５０を繰り返し行う。

【0082】

図６は、第１実施形態の方位算出方法の具体的な手順の一例を示すフローチャート図である。

【0083】

図６に示すように、まず、工程Ｓ１において、方位算出装置１は、時刻ｋを０に初期化する。

【0084】

次に、工程Ｓ２において衛星信号受信機１１が衛星を受信できた場合、工程Ｓ１１において、方位算出装置１の第１方位演算部２０は、衛星信号受信機１１が受信した衛星信号に基づいて、第１方位Ψ_{ｇｎｓｓ，ｋ}を算出する。工程Ｓ２において衛星信号受信機１１が衛星を受信できなかった場合、第１方位演算部２０は工程Ｓ１１の処理を行わない。工程Ｓ１１は、図５の第１方位演算工程Ｓ１０に相当する。

【0085】

次に、工程Ｓ２１において、方位算出装置１の第２方位演算部３０は、第２方位Ψ_{ｉｎｓ，ｋ}の初期値を設定する。具体的には、後述する工程Ｓ３１で算出された前回の第３方位Ψ_{ｅｓｔ，ｋ－１}を第２方位Ψ_{ｉｎｓ，ｋ}の初期値に設定する。また、第２方位演算部３０は、工程Ｓ３１で前回の第３方位Ψ_{ｅｓｔ，ｋ－１}が算出されなかった場合は、後述する工程Ｓ２３で算出された前回の第２方位Ψ_{ｉｎｓ，ｋ－１}を第２方位Ψ_{ｉｎｓ，ｋ}の初期値に設定する。また、第２方位演算部３０は、時刻ｋが０のときは、第２方位Ψ_{ｉｎｓ，ｋ}を適当な初期値に設定する。

【0086】

次に、工程Ｓ２２において、第２方位演算部３０は、前出の式（１）により、角速度センサー１３が検出したＺ軸回りの角速度ω_ｚ，ｋに基づいてヨー角ψの角速度を算出し、ヨー角ψの角速度から、後述する工程Ｓ５１で予測された前回の角速度バイアスδΩ_ｋ－１を減算してヨー角ψの角速度のバイアスを補正する。

【0087】

次に、工程Ｓ２３において、工程Ｓ２１で設定した第２方位Ψ_{ｉｎｓ，ｋ}の初期値に、工程Ｓ２２でバイアスが補正されたヨー角ψの角速度を積分した値を加算して、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出する。

【0088】

工程Ｓ２１，Ｓ２２，Ｓ２３は、図５の第２方位演算工程Ｓ２０に相当する。

【0089】

次に、工程Ｓ２８において、工程Ｓ１１で第１方位Ψ_{ｇｎｓｓ，ｋ}が算出された場合、工程Ｓ３１において、方位算出装置１の第３方位演算部４０は、工程Ｓ１１で算出された第１方位Ψ_{ｇｎｓｓ，ｋ}と工程Ｓ２３で算出された第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて、第３方位Ψ_{ｅｓｔ，ｋ}を算出する。工程Ｓ３０において、工程Ｓ１１で第１方位Ψ_{ｇｎｓｓ，ｋ}が算出されなかった場合、第３方位演算部４０は工程Ｓ３１の処理を行わない。工程Ｓ３１は、図５の第３方位演算工程Ｓ３０に相当する。

【0090】

次に、工程Ｓ４１において、方位算出装置１の温度特性推定部６０は、前出の式（３）により、第２方位Ψ_{ｉｎｓ，ｋ}の誤差δΨ_{ｉｎｓ，ｋ}を算出する。

【0091】

次に、工程Ｓ４２において、温度特性推定部６０は、前出の式（２）により、第２方位Ψ_{ｉｎｓ，ｋ}の角速度のバイアスである角速度バイアスδΩ_ｋを算出する。

【0092】

次に、工程Ｓ４３において、温度特性推定部６０は、角速度バイアスδΩ_ｋの温度特性を算出する。具体的には、温度特性推定部６０は、工程Ｓ４２で算出した角速度バイアスδΩ_ｋ及び温度センサー１５が検出した温度Ｔ_ｋに基づいて前出の式（１０）で示される観測変数ｙ_ｋを算出し、前述の拡張カルマンフィルターを用いて、前出の式（６）に示した、前出の多項式（５）の１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋと温度Ｔ_ｋとを要素とする状態変数ｘ_ｋを算出する。

【0093】

次に、工程Ｓ４４において、温度特性推定部６０は、記憶部７０に記憶されている温度特性情報７１を更新する。具体的には、温度特性推定部６０は、前出の式（２６）で示される共分散行列Ｐ_ｋ｜ｋを算出する。

【0094】

そして、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する１次係数ｃ_１，ｋの値が含まれている場合、分散ｐ_１１が所定の閾値以下のときは、温度特性情報７１に含まれている係数ｃ_１，ｋの値を、推定した係数ｃ_１，ｋの値に更新し、分散ｐ_１１が所定の閾値よりも大きいときは、温度特性情報７１に含まれている係数ｃ_１，ｋの値を更新しない。なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する１次係数ｃ_１，ｋの値が含まれていない場合は、分散ｐ_１１の値によらず、温度特性情報７１に含まれている１次係数ｃ_１，ｋの値を、推定した１次係数ｃ_１，ｋの値に更新する。

【0095】

同様に、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する０次係数ｃ_０，ｋの値が含まれている場合、分散ｐ_２２が所定の閾値以下のときは、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を、推定した０次係数ｃ_０，ｋの値に更新し、分散ｐ_２２が所定の閾値よりも大きいときは、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を更新しない。なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する０次係数ｃ_０，ｋの値が含まれていない場合は、分散ｐ_２２の値によらず、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を、推定した０次係数ｃ_０，ｋの値に更新する。

【0096】

工程Ｓ４１，Ｓ４２，Ｓ４３，Ｓ４４は、図５の温度特性推定工程Ｓ４０に相当する。

【0097】

次に、工程Ｓ５１において、方位算出装置１の角速度バイアス予測部５０は、記憶部７０に記憶されている温度特性情報７１に基づいて、第２方位Ψ_{ｉｎｓ，ｋ}の角速度バイアスδΩ_ｋを予測する。例えば、角速度バイアス予測部５０は、温度Ｔ_ｋ－１と温度Ｔ_ｋと温度特性情報７１に含まれる温度Ｔ_ｋに対する１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋと
を１次多項式（５）に代入して角速度バイアスδΩ_ｋを予測してもよい。工程Ｓ５１は、図５の角速度バイアス予測工程Ｓ５０に相当する。

【0098】

そして、方位算出装置１は、工程Ｓ５８において時刻ｋを１だけ増加させ、工程Ｓ６０において方位の算出処理を終了するまで、工程Ｓ２～Ｓ５８を繰り返し行う。

【0099】

このように、方位算出装置１は、移動体である自動車５が停止中か走行中かによらず、例えば１秒等の所定の周期で角速度バイアスδΩ_ｋを推定し、角速度バイアスが補正された高精度な第３方位Ψ_{ｅｓｔ，ｋ}を算出することができる。

【0100】

１－３．作用効果
以上に説明したように、第１実施形態では、方位算出装置１が搭載される移動体が停止中であっても移動中であっても、第１方位演算部２０が衛星信号に基づいて第１方位Ψ_{ｇｎｓｓ，ｋ}を算出することができ、衛星信号が受信できない場合でも、第２方位演算部３０が角速度センサー１３から出力される角速度信号に基づいて第２方位Ψ_{ｉｎｓ，ｋ}を算出することができるので、第３方位演算部４０は、第１方位Ψ_{ｇｎｓｓ，ｋ}と第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて途切れなく周期的に第３方位Ψ_{ｅｓｔ，ｋ}を算出することができる。その結果、移動体が移動中であっても、温度特性推定部６０が、第２方位Ψ_{ｉｎｓ，ｋ}と第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて第２方位Ψ_{ｉｎｓ，ｋ}の角速度バイアスδΩ_ｋの温度特性を周期的に推定し、記憶部７０に記憶されている温度特性情報７１を更新することができるので、第２方位演算部３０は、角速度バイアス予測部５０が温度特性情報７１に基づいて予測した前回の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出することができる。また、温度特性推定部６０は、温度センサー１５が検出した温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性を推定するので、角速度バイアスδΩ_ｋの温度特性を狭い温度範囲毎に精度よく推定することができる。したがって、第１実施形態によれば、方位算出装置１は、移動体が停止中であっても移動中であっても、角速度バイアスが高精度に補正された方位を算出することができる。

【0101】

また、第１実施形態では、角速度バイアスδΩ_ｋは、温度Ｔ_ｋを変数とする多項式で近似されるのではなく、温度変化ΔＴ_ｋを変数とする多項式（４）で近似されるので、温度特性推定部６０は、角速度バイアスδΩ_ｋの温度特性を狭い温度範囲毎に精度よく推定することができる。そして、狭い温度範囲では角速度バイアスδΩ_ｋは温度変化ΔＴ_ｋを変数とする１次多項式（５）によって精度よく近似されるので、温度特性推定部６０による角速度バイアスδΩ_ｋの温度特性の推定精度を維持しつつ推定のための計算負荷を低減させることができる。

【0102】

また、第１実施形態では、温度特性推定部６０は、角速度バイアスδΩ_ｋを近似する多項式（４）の係数ｃ_０，ｋ～ｃ_ｎ，ｋの推定精度が所定値以上の場合のみ、温度特性情報７１に含まれている係数ｃ_０，ｋ～ｃ_ｎ，ｋの値を更新するので、角速度バイアス予測部５０は、温度特性情報７１に基づいて、角速度バイアスδΩ_ｋを精度よく予測することができる。したがって、第１実施形態によれば、方位算出装置１は、角速度バイアスが高精度に補正された方位を算出することができる。

【0103】

２．第２実施形態
以下、第２実施形態について、第１実施形態と同様の構成要素には同じ符号を付し、第１実施形態と重複する説明は省略または簡略し、主に第１実施形態と異なる内容について説明する。

【0104】

第２実施形態では、角速度バイアスδΩ_ｋが第１実施形態と異なる。すなわち、第１実施形態では、角速度バイアスδΩ_ｋは、第２方位演算部３０が算出した第２方位Ψ_{ｉｎｓ
，ｋ}の角速度のバイアスであったが、第２実施形態では、角速度バイアスδΩ_ｋは、角速度センサー１３が検出するＺ軸回りの角速度ω_ｚ，ｋのバイアスである。Ｚ軸はヨー軸に相当するので、換言すれば、角速度バイアスδΩ_ｋは、ヨー軸回りの角速度ω_ｚ，ｋのバイアスである。

【0105】

第２実施形態の方位算出装置１の構成例は、図１と同様であるため、その図示を省略する。ただし、第２実施形態の方位算出装置１では、第２方位演算部３０、角速度バイアス予測部５０及び温度特性推定部６０が行う処理が第１実施形態と異なる。

【0106】

第２方位演算部３０は、角速度センサー１３から出力される角速度信号に基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第２方位Ψ_{ｉｎｓ，ｋ}を算出する。第２方位演算部３０は、角速度バイアス予測部５０が予測した前回の時刻ｋ－１の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された時刻ｋの第２方位Ψ_{ｉｎｓ，ｋ}を算出する。具体的には、第２方位演算部３０は、角速度センサー１３が検出したＺ軸回りの角速度ω_ｚ，ｋから角速度バイアスδΩ_ｋ－１を減算して角速度バイアスが補正されたＺ軸回りの角速度を算出する。そして、第２方位演算部３０は、第３方位演算部４０が算出した前回の時刻ｋ－１における第３方位Ψ_{ｅｓｔ，ｋ－１}を初期方位として、第３方位Ψ_{ｅｓｔ，ｋ－１}に、当該Ｚ軸回りの角速度を前出の式（１）に代入して得られるヨー角ψの角速度を積分した値を加算して、時刻ｋにおける第２方位Ψ_{ｉｎｓ，ｋ}を算出する。

【0107】

温度特性推定部６０は、第２方位演算部３０が算出した第２方位Ψ_{ｉｎｓ，ｋ}と第３方位演算部４０が算出した第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて、温度センサー１５が検出した温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性を推定し、推定した温度特性に基づいて記憶部７０に記憶されている温度特性情報７１を更新する。

【0108】

本実施形態では、温度特性推定部６０は、前出の式（１０）に示した観測変数ｙ_ｋの要素である角速度バイアスδΩ_ｋを式（２７）によって算出する。

【0109】

【数27】

【0110】

温度特性推定部６０は、第３方位演算部４０が算出した第３方位Ψ_{ｅｓｔ，ｋ}、ロール角Φ_{ｅｓｔ，ｋ}及びピッチ角Θ_{ｅｓｔ，ｋ}を用いて、式（２８）により、式（２７）に含まれる角速度ω^ｅｓｔ _ｚ，ｋを算出する。式（２９）において、ΔΘ_{ｅｓｔ，ｋ}及びΔΨ_{ｅｓｔ，ｋ}は、それぞれ式（２９）及び式（３０）で表される。

【0111】

【数28】

【0112】

【数29】

【0113】

【数30】

【0114】

そして、温度特性推定部６０は、第１実施形態と同様の拡張カルマンフィルターを用いて、温度Ｔ_ｋに対する前出の１次多項式（５）の１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋを推定する。

【0115】

角速度バイアス予測部５０は、温度センサー１５が検出した温度Ｔ_ｋと、記憶部７０に記憶されている温度特性情報７１とに基づいて、角速度バイアスδΩ_ｋを予測する。例えば、角速度バイアス予測部５０は、温度Ｔ_ｋ－１と温度Ｔ_ｋと温度特性情報７１に含まれる温度Ｔ_ｋに対する係数ｃ_０，ｋ～ｃ_ｎ，ｋとを前出の多項式（４）に代入して角速度バイアスδΩ_ｋを予測してもよい。

【0116】

第２実施形態の方位算出装置１におけるその他の構成及び機能は、第１実施形態と同様であるため、その説明を省略する。

【0117】

また、第２実施形態の方位算出方法の手順の一例を示すフローチャート図は、図５と同様であるため、その図示及び説明を省略する。ただし、第２実施形態では、図５の第２方位演算工程Ｓ２０、温度特性推定工程Ｓ４０及び角速度バイアス予測工程Ｓ５０の具体的な手順が第１実施形態と異なる。図７は、第２実施形態の方位算出方法の具体的な手順の一例を示すフローチャート図である。図７において、図６と同じ工程には同じ符号が付されている。

【0118】

図７に示すように、まず、工程Ｓ１において、方位算出装置１は、時刻ｋを０に初期化する。

【0119】

次に、工程Ｓ２において衛星信号受信機１１が衛星を受信できた場合、工程Ｓ１１において、方位算出装置１の第１方位演算部２０は、衛星信号受信機１１が受信した衛星信号に基づいて、第１方位Ψ_{ｇｎｓｓ，ｋ}を算出する。工程Ｓ２において衛星信号受信機１１が衛星を受信できなかった場合、第１方位演算部２０は工程Ｓ１１の処理を行わない。工程Ｓ１１は、図５の第１方位演算工程Ｓ１０に相当する。

【0120】

次に、工程Ｓ２１において、方位算出装置１の第２方位演算部３０は、第２方位Ψ_{ｉｎｓ，ｋ}の初期値を設定する。具体的には、後述する工程Ｓ３１で算出された前回の第３方位Ψ_{ｅｓｔ，ｋ－１}を第２方位Ψ_{ｉｎｓ，ｋ}の初期値に設定する。また、第２方位演算部３０は、工程Ｓ３１で前回の第３方位Ψ_{ｅｓｔ，ｋ－１}が算出されなかった場合は、後述する工程Ｓ２３ａで算出された前回の第２方位Ψ_{ｉｎｓ，ｋ－１}を第２方位Ψ_{ｉｎｓ，ｋ}の初期値に設定する。また、第２方位演算部３０は、時刻ｋが０のときは、第２方位Ψ_{ｉｎｓ，ｋ}を適当な初期値に設定する。

【0121】

次に、工程Ｓ２２ａにおいて、第２方位演算部３０は、角速度センサー１３が検出したＺ軸回りの角速度ω_ｚ，ｋから、後述する工程Ｓ５１ａで予測された前回の角速度バイアスδΩ_ｋ－１を減算してヨー軸回りの角速度バイアスを補正する。

【0122】

次に、工程Ｓ２３ａにおいて、工程Ｓ２１で設定した第２方位Ψ_{ｉｎｓ，ｋ}の初期値に、工程Ｓ２２ａでバイアスが補正されたヨー軸回りの角速度を前出の式（１）に代入して得られるヨー角ψの角速度を積分した値を加算して、第２方位Ψ_{ｉｎｓ，ｋ}を算出する。

【0123】

工程Ｓ２１，Ｓ２２ａ，Ｓ２３ａは、図５の第２方位演算工程Ｓ２０に相当する。

【0124】

次に、工程Ｓ２８において、工程Ｓ１１で第１方位Ψ_{ｇｎｓｓ，ｋ}が算出された場合、工程Ｓ３１において、方位算出装置１の第３方位演算部４０は、工程Ｓ１１で算出された
第１方位Ψ_{ｇｎｓｓ，ｋ}と工程Ｓ２３ａで算出された第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて、第３方位Ψ_{ｅｓｔ，ｋ}を算出する。工程Ｓ３０において、工程Ｓ１１で第１方位Ψ_{ｇｎｓｓ，ｋ}が算出されなかった場合、第３方位演算部４０は工程Ｓ３１の処理を行わない。なお、工程Ｓ３１において、第３方位演算部４０は、ロール角Φ_{ｅｓｔ，ｋ}及びピッチ角Θ_{ｅｓｔ，ｋ}も算出する。工程Ｓ３１は、図５の第３方位演算工程Ｓ３０に相当する。

【0125】

次に、工程Ｓ４１において、方位算出装置１の温度特性推定部６０は、前出の式（３）により、第２方位Ψ_{ｉｎｓ，ｋ}の誤差δΨ_{ｉｎｓ，ｋ}を算出する。

【0126】

次に、工程Ｓ４２ａにおいて、温度特性推定部６０は、前出の式（２７）により、ヨー軸回りの角速度ω_ｚ，ｋのバイアスである角速度バイアスδΩ_ｋを算出する。

【0127】

次に、工程Ｓ４３ａにおいて、温度特性推定部６０は、角速度バイアスδΩ_ｋの温度特性を算出する。具体的には、温度特性推定部６０は、工程Ｓ４２ａで算出した角速度バイアスδΩ_ｋ及び温度センサー１５が検出した温度Ｔ_ｋに基づいて前出の式（１０）で示される観測変数ｙ_ｋを算出し、前述の拡張カルマンフィルターを用いて、前出の式（６）に示した、前出の多項式（５）の１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋと温度Ｔ_ｋとを要素とする状態変数ｘ_ｋを算出する。

【0128】

次に、工程Ｓ４４ａにおいて、温度特性推定部６０は、記憶部７０に記憶されている温度特性情報７１を更新する。具体的には、温度特性推定部６０は、前出の式（２６）で示される共分散行列Ｐ_ｋ｜ｋを算出する。

【0129】

そして、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する１次係数ｃ_１，ｋの値が含まれている場合、分散ｐ_１１が所定の閾値以下のときは、温度特性情報７１に含まれている係数ｃ_１，ｋの値を、推定した係数ｃ_１，ｋの値に更新し、分散ｐ_１１が所定の閾値よりも大きいときは、温度特性情報７１に含まれている係数ｃ_１，ｋの値を更新しない。なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する１次係数ｃ_１，ｋの値が含まれていない場合は、分散ｐ_１１の値によらず、温度特性情報７１に含まれている１次係数ｃ_１，ｋの値を、推定した１次係数ｃ_１，ｋの値に更新する。

【0130】

同様に、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する０次係数ｃ_０，ｋの値が含まれている場合、分散ｐ_２２が所定の閾値以下のときは、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を、推定した０次係数ｃ_０，ｋの値に更新し、分散ｐ_２２が所定の閾値よりも大きいときは、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を更新しない。なお、温度特性推定部６０は、温度特性情報７１に、温度Ｔ_ｋに対する０次係数ｃ_０，ｋの値が含まれていない場合は、分散ｐ_２２の値によらず、温度特性情報７１に含まれている０次係数ｃ_０，ｋの値を、推定した０次係数ｃ_０，ｋの値に更新する。

【0131】

工程Ｓ４１，Ｓ４２ａ，Ｓ４３ａ，Ｓ４４ａは、図５の温度特性推定工程Ｓ４０に相当する。

【0132】

次に、工程Ｓ５１ａにおいて、方位算出装置１の角速度バイアス予測部５０は、記憶部７０に記憶されている温度特性情報７１に基づいて、ヨー軸回りの角速度バイアスδΩ_ｋを予測する。例えば、角速度バイアス予測部５０は、温度Ｔ_ｋ－１と温度Ｔ_ｋと温度特性情報７１に含まれる温度Ｔ_ｋに対する１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋとを１次多項式（５）に代入して角速度バイアスδΩ_ｋを予測してもよい。工程Ｓ５１ａは、図５の角速度バイアス予測工程Ｓ５０に相当する。

【0133】

そして、方位算出装置１は、工程Ｓ５８において時刻ｋを１だけ増加させ、工程Ｓ６０において方位の算出処理を終了するまで、工程Ｓ２～Ｓ５８を繰り返し行う。

【0134】

以上に説明したように、第２実施形態では、移動体が移動中であっても、温度特性推定部６０が、第２方位Ψ_{ｉｎｓ，ｋ}と第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて、角速度センサー１３が検出するヨー軸回りの角速度ω_ｚ，ｋの角速度バイアスδΩ_ｋの温度特性を周期的に推定し、記憶部７０に記憶されている温度特性情報７１を更新することができるので、第２方位演算部３０は、角速度バイアス予測部５０が温度特性情報７１に基づいて予測した前回の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出することができる。したがって、第２実施形態によれば、方位算出装置１は、移動体が停止中であっても移動中であっても、角速度バイアスが高精度に補正された方位を算出することができる。その他、第２実施形態によれば、第１実施形態と同様の効果が得られる。

【0135】

３．第３実施形態
以下、第３実施形態について、第１実施形態又は第２実施形態と同様の構成要素には同じ符号を付し、第１実施形態又は第２実施形態と重複する説明は省略または簡略し、主に第１実施形態及び第２実施形態と異なる内容について説明する。

【0136】

図８は、第３実施形態の方位算出装置１の構成例を示す図である。図８において、図１と同様の構成要素には、同じ符号が付されている。図８に示すように、第３実施形態の方位算出装置１は、第１実施形態と同様、第１方位演算部２０、第２方位演算部３０、第３方位演算部４０、角速度バイアス予測部５０、温度特性推定部６０及び記憶部７０を備えている。また、方位算出装置１は、車輪速センサー１２、角速度センサー１３、６Ｄｏｆセンサー１４、温度センサー１５及び外界センサー１６を備えていてもよい。なお、方位算出装置１は、図８の構成要素の一部を省略又は変更し、あるいは、他の構成要素を付加した構成としてもよい。

【0137】

第３実施形態では、第１方位演算部２０が、衛星信号受信機１１に代えて外界センサー１６を用いた処理を行う点が、第１実施形態及び第２実施形態と異なる。

【0138】

外界センサー１６は、移動体の周辺の情報を検出するセンサーであり、例えば、ＡＤやＡＤＡＳ等に用いられる、ＬｉＤＡＲ、カメラ、ミリ波レーダー等であってもよい。ＡＤはAutonomous Drivingの略称であり、ＡＤＡＳはAdvanced Driver Assistance Systemの略称である。ＬｉＤＡＲは、Light Detection and Ranging、あるいは、Laser Imaging Detection and Rangingの略称である。

【0139】

第１方位演算部２０は、外界センサー１６の検出結果に基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第１方位Ψ_{ＥＸＴ，ｋ}を算出する。第１方位演算部２０は、例えば１秒等の所定の周期で第１方位Ψ_{ＥＸＴ，ｋ}を算出する。例えば、第１方位演算部２０は、外界センサー１６の検出結果である移動体の周辺の情報を、記憶部７０に記憶されている地図情報７２と照合することにより、グローバル座標系における移動体の向きである第１方位Ψ_{ＥＸＴ，ｋ}を算出する。なお、第１方位演算部２０は、グローバル座標系における移動体の位置や速度をさらに算出してもよい。

【0140】

第１実施形態又は第２実施形態と同様、第２方位演算部３０は、角速度センサー１３から出力される角速度信号に基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第２方位Ψ_{ｉｎｓ，ｋ}を算出する。第２方位演算部３０は、角速度バイアス予測部５０が予測した前回の時刻ｋ－１の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された時刻ｋの第２方位Ψ_{ｉｎｓ，ｋ}を算出する。

【0141】

第３方位演算部４０は、第１方位演算部２０が算出した時刻ｋにおける第１方位Ψ_{ＥＸＴ，ｋ}と第２方位演算部３０が算出した時刻ｋにおける第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて、時刻ｋにおけるグローバル座標系の移動体の方位である第３方位Ψ_{ｅｓｔ，ｋ}を算出する。第３方位演算部４０は、例えば１秒等の所定の周期で第３方位Ψ_{ｅｓｔ，ｋ}を算出する。例えば、第３方位演算部４０は、第１方位Ψ_{ＥＸＴ，ｋ}が算出されるタイミングでは、第１方位Ψ_{ＥＸＴ，ｋ}を第３方位Ψ_{ｅｓｔ，ｋ}とし、第１方位Ψ_{ＥＸＴ，ｋ}が算出されないタイミングでは第２方位Ψ_{ｉｎｓ，ｋ}を第３方位Ψ_{ｅｓｔ，ｋ}としてもよい。また、第３方位演算部４０は、カルマンフィルターを用いて、第３方位Ψ_{ｅｓｔ，ｋ}を算出してもよい。

【0142】

第１実施形態又は第２実施形態と同様、記憶部７０は、角速度バイアスδΩ_ｋの温度特性情報７１を記憶する。角速度バイアスδΩ_ｋは、前出の式（２）で示されるように、第２方位演算部３０が算出した第２方位Ψ_{ｉｎｓ，ｋ}の角速度のバイアスであってもよい。また、角速度バイアスδΩ_ｋは、前出の式（２７）で示されるように、角速度センサー１３が検出するヨー軸回りの角速度ω_ｚ，ｋのバイアスであってもよい。また、第３実施形態では、記憶部７０は、第１方位演算部２０による第１方位Ψ_{ＥＸＴ，ｋ}の算出に用いられる地図情報７２を記憶する。

【0143】

第１実施形態又は第２実施形態と同様、温度特性推定部６０は、第２方位演算部３０が算出した第２方位Ψ_{ｉｎｓ，ｋ}と第３方位演算部４０が算出した第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて、温度センサー１５が検出した温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性を推定し、推定した温度特性に基づいて記憶部７０に記憶されている温度特性情報７１を更新する。前述の通り、温度特性推定部６０は、拡張カルマンフィルターを用いて、温度Ｔ_ｋに対する１次係数ｃ_１，ｋ及び０次係数ｃ_０，ｋを推定してもよい。

【0144】

第１実施形態又は第２実施形態と同様、角速度バイアス予測部５０は、温度センサー１５が検出した温度Ｔ_ｋと、記憶部７０に記憶されている温度特性情報７１とに基づいて、角速度バイアスδΩ_ｋを予測する。前述の通り、角速度バイアス予測部５０は、温度Ｔ_ｋ－１と温度Ｔ_ｋと温度特性情報７１に含まれる温度Ｔ_ｋに対する係数ｃ_０，ｋ～ｃ_ｎ，ｋとを前出の多項式（４）に代入して角速度バイアスδΩ_ｋを予測してもよい。

【0145】

第３実施形態の方位算出装置１のその他の構成及び機能は、第１実施形態又は第２実施形態の方位算出装置１と同様であるため、その説明を省略する。

【0146】

第３実施形態の方位算出方法の手順の一例を示すフローチャート図は、図５と同様であるため、その図示を省略する。第３実施形態では、図５の第１方位演算工程Ｓ１０において、方位算出装置１の第１方位演算部２０が、外界センサー１６の検出結果に基づいて、第１方位Ψ_{ＥＸＴ，ｋ}を算出する。図５の第２方位演算工程Ｓ２０以降で方位算出装置１が行う処理は、第１方位Ψ_{ｇｎｓｓ，ｋ}に代えて第１方位Ψ_{ＥＸＴ，ｋ}を用いる点を除いて、第１実施形態又は第２実施形態と同様であるので、その説明を省略する。

【0147】

図９は、第３実施形態の方位算出方法の具体的な手順の一例を示すフローチャート図である。図９において、図６と同様の処理を行う工程には同じ符号が付されている。図９では、角速度バイアスδΩ_ｋは、第２方位Ψ_{ｉｎｓ，ｋ}の角速度のバイアスである。

【0148】

図９に示すように、まず、工程Ｓ１において、方位算出装置１は、時刻ｋを０に初期化する。

【0149】

次に、工程Ｓ１２において、方位算出装置１の第１方位演算部２０は、外界センサー１
６の検出結果に基づいて、第１方位Ψ_{ＥＸＴ，ｋ}を算出する。工程Ｓ１２は、図５の第１方位演算工程Ｓ１０に相当する。

【0150】

次に、工程Ｓ２１，Ｓ２２，Ｓ２３において、方位算出装置１の第２方位演算部３０は、図６の工程Ｓ２１，Ｓ２２，Ｓ２３と同様の処理を行い、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出する。工程Ｓ２１，Ｓ２２，Ｓ２３は、図５の第２方位演算工程Ｓ２０に相当する。

【0151】

次に、工程Ｓ３１において、方位算出装置１の第３方位演算部４０は、工程Ｓ１２で算出された第１方位Ψ_{ＥＸＴ，ｋ}と工程Ｓ２３で算出された第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて、第３方位Ψ_{ｅｓｔ，ｋ}を算出する。工程Ｓ３１は、図５の第３方位演算工程Ｓ３０に相当する。

【0152】

次に、工程Ｓ４１，Ｓ４２，Ｓ４３，Ｓ４４において、方位算出装置１の温度特性推定部６０は、図６の工程Ｓ４１，Ｓ４２，Ｓ４３，Ｓ４４と同様の処理を行い、記憶部７０に記憶されている温度特性情報７１を更新する。工程Ｓ４１，Ｓ４２，Ｓ４３，Ｓ４４は、図５の温度特性推定工程Ｓ４０に相当する。

【0153】

次に、工程Ｓ５１において、方位算出装置１の角速度バイアス予測部５０は、図６の工程Ｓ５１と同様の処理を行い、第２方位Ψ_{ｉｎｓ，ｋ}の角速度バイアスδΩ_ｋを予測する。

【0154】

そして、方位算出装置１は、工程Ｓ５８において時刻ｋを１だけ増加させ、工程Ｓ６０において方位の算出処理を終了するまで、工程Ｓ２～Ｓ５８を繰り返し行う。

【0155】

図１０は、第３実施形態の方位算出方法の具体的な手順の他の一例を示すフローチャート図である。図１０において、図７と同様の処理を行う工程には同じ符号が付されている。図１０では、角速度バイアスδΩ_ｋは、角速度センサー１３が検出するヨー軸回りの角速度ω_ｚ，ｋのバイアスである。

【0156】

図１０に示すように、まず、工程Ｓ１において、方位算出装置１は、時刻ｋを０に初期化する。

【0157】

次に、工程Ｓ１２において、方位算出装置１の第１方位演算部２０は、外界センサー１６の検出結果に基づいて、第１方位Ψ_{ＥＸＴ，ｋ}を算出する。工程Ｓ１２は、図５の第１方位演算工程Ｓ１０に相当する。

【0158】

次に、工程Ｓ２１，Ｓ２２ａ，Ｓ２３ａにおいて、方位算出装置１の第２方位演算部３０は、図７の工程Ｓ２１，Ｓ２２ａ，Ｓ２３ａと同様の処理を行い、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出する。工程Ｓ２１，Ｓ２２ａ，Ｓ２３ａは、図５の第２方位演算工程Ｓ２０に相当する。

【0159】

次に、工程Ｓ３１において、方位算出装置１の第３方位演算部４０は、工程Ｓ１２で算出された第１方位Ψ_{ＥＸＴ，ｋ}と工程Ｓ２３ａで算出された第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて、第３方位Ψ_{ｅｓｔ，ｋ}を算出する。工程Ｓ３１は、図５の第３方位演算工程Ｓ３０に相当する。

【0160】

次に、工程Ｓ４１，Ｓ４２ａ，Ｓ４３ａ，Ｓ４４ａにおいて、方位算出装置１の温度特性推定部６０は、図７の工程Ｓ４１，Ｓ４２ａ，Ｓ４３ａ，Ｓ４４ａと同様の処理を行い、記憶部７０に記憶されている温度特性情報７１を更新する。工程Ｓ４１，Ｓ４２ａ，Ｓ
４３ａ，Ｓ４４ａは、図５の温度特性推定工程Ｓ４０に相当する。

【0161】

次に、工程Ｓ５１ａにおいて、方位算出装置１の角速度バイアス予測部５０は、図７の工程Ｓ５１ａと同様の処理を行い、ヨー軸回りの角速度バイアスδΩ_ｋを予測する。

【0162】

そして、方位算出装置１は、工程Ｓ５８において時刻ｋを１だけ増加させ、工程Ｓ６０において方位の算出処理を終了するまで、工程Ｓ２～Ｓ５８を繰り返し行う。

【0163】

以上に説明したように、第３実施形態では、方位算出装置１が搭載される移動体が停止中であっても移動中であっても、第１方位演算部２０が外界センサー１６の検出結果に基づいて第１方位Ψ_{ＥＸＴ，ｋ}を算出することができ、第２方位演算部３０が角速度センサー１３から出力される角速度信号に基づいて第２方位Ψ_{ｉｎｓ，ｋ}を算出することができるので、第３方位演算部４０は、第１方位Ψ_{ＥＸＴ，ｋ}と第２方位Ψ_{ｉｎｓ，ｋ}とに基づいて途切れなく周期的に第３方位Ψ_{ｅｓｔ，ｋ}を算出することができる。その結果、移動体が移動中であっても、温度特性推定部６０が、第２方位Ψ_{ｉｎｓ，ｋ}と第３方位Ψ_{ｅｓｔ，ｋ}とに基づいて角速度バイアスδΩ_ｋの温度特性を周期的に推定し、記憶部７０に記憶されている温度特性情報７１を更新することができるので、第２方位演算部３０は、角速度バイアス予測部５０が温度特性情報７１に基づいて予測した前回の角速度バイアスδΩ_ｋ－１に基づいて、角速度バイアスが補正された第２方位Ψ_{ｉｎｓ，ｋ}を算出することができる。また、温度特性推定部６０は、温度センサー１５が検出した温度Ｔ_ｋに対する角速度バイアスδΩ_ｋの温度特性を推定するので、角速度バイアスδΩ_ｋの温度特性を狭い温度範囲毎に精度よく推定することができる。したがって、第３実施形態によれば、方位算出装置１は、移動体が停止中であっても移動中であっても、角速度バイアスが高精度に補正された方位を算出することができる。その他、第３実施形態によれば、第１実施形態又は第２実施形態と同様の効果が得られる。

【0164】

４．変形例
本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。

【0165】

例えば、上記の各実施形態では、方位算出装置１が自動車５に搭載される例を挙げたが、方位算出装置１は自動車以外の移動体に搭載されてもよい。移動体としては、例えば、自動車の他、トラクターなどの農業機械、ショベルカーなどの建設機械、無人搬送車、ロボット芝刈機、ロボット掃除機、ジェット機やヘリコプター等の航空機、船舶、ロケット、人工衛星、鉄道車両等が挙げられる。

【0166】

また、例えば、上記の各実施形態では、方位算出装置１は、角速度センサー１３と６Ｄｏｆセンサー１４とを備えるが、これらに代えて他のセンサーを備えてもよい。例えば、方位算出装置１は、２つの６Ｄｏｆセンサーを備えてもよい。

【0167】

上述した実施形態および変形例は一例であって、これらに限定されるわけではない。例えば、各実施形態および各変形例を適宜組み合わせることも可能である。

【0168】

本発明は、実施の形態で説明した構成と実質的に同一の構成、例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。

【0169】

上述した実施形態および変形例から以下の内容が導き出される。

【0170】

方位算出装置の一態様は、
衛星信号受信機が受信した衛星信号又は外界センサーの検出結果に基づいて、第１方位を算出する第１方位演算部と、
角速度センサーが角速度を検出して出力した角速度信号に基づいて、第２方位を算出する第２方位演算部と、
前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算部と、
角速度バイアスの温度特性情報を記憶する記憶部と、
前記第２方位と前記第３方位とに基づいて、温度センサーが検出した温度に対する前記角速度バイアスの温度特性を推定し、推定した前記温度特性に基づいて、前記記憶部に記憶されている前記温度特性情報を更新する温度特性推定部と、
前記温度センサーが検出した前記温度と前記記憶部に記憶されている前記温度特性情報とに基づいて、前記角速度バイアスを予測する角速度バイアス予測部と、を備え、
前記第２方位演算部は、前記角速度バイアス予測部が予測した前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する。

【0171】

この方位算出装置では、当該方位算出装置が搭載される移動体が停止中であっても移動中であっても、第１方位演算部が衛星信号又は外界センサーの検出結果に基づいて第１方位を算出することができ、衛星信号が受信できない場合でも、第２方位演算部が角速度信号に基づいて第２方位を算出することができるので、第３方位演算部は、第１方位と第２方位とに基づいて途切れなく周期的に第３方位を算出することができる。その結果、移動体が移動中であっても、温度特性推定部が、第２方位と第３方位とに基づいて角速度バイアスの温度特性を周期的に推定し、温度特性情報を更新することができるので、第２方位演算部は、角速度バイアス予測部が温度特性情報に基づいて予測した前回の角速度バイアスに基づいて、角速度バイアスが補正された第２方位を算出することができる。また、温度特性推定部は、温度センサーが検出した温度に対する角速度バイアスの温度特性を推定するので、角速度バイアスの温度特性を狭い温度範囲毎に精度よく推定することができる。したがって、この方位算出装置によれば、移動体が停止中であっても移動中であっても、角速度バイアスが高精度に補正された方位を算出することができる。

【0172】

前記方位算出装置の一態様において、
前記角速度バイアスは、温度変化を変数とする多項式で近似されてもよい。

【0173】

この方位算出装置によれば、角速度バイアスは、温度を変数とする多項式で近似されるのではなく、温度変化を変数とする多項式で近似されるので、温度特性推定部は、角速度バイアスの温度特性を狭い温度範囲毎に精度よく推定することができる。

【0174】

前記方位算出装置の一態様において、
前記温度特性情報は、複数の温度の各々に対する前記多項式の係数の値を含んでもよい。

【0175】

前記方位算出装置の一態様において、
前記多項式は、１次多項式であってもよい。

【0176】

この方位算出装置によれば、狭い温度範囲では角速度バイアスは温度変化を変数とする１次多項式によって精度よく近似されるので、温度特性推定部による角速度バイアスの温度特性の推定精度を維持しつつ推定のための計算負荷を低減させることができる。

【0177】

前記方位算出装置の一態様において、
前記角速度バイアスは、前記第２方位の角速度のバイアスであってもよい。

【0178】

前記方位算出装置の一態様において、
前記角速度バイアスは、前記角速度センサーが検出するヨー軸回りの角速度のバイアスであってもよい。

【0179】

前記方位算出装置の一態様において、
前記温度特性推定部は、前記温度特性として前記係数の値を推定し、推定した前記係数の分散を算出し、前記温度特性情報に、前記温度センサーが検出した前記温度に対する前記係数の値が含まれている場合、前記分散が所定の閾値以下のときは、前記温度特性情報に含まれている前記係数の値を、推定した前記係数の値に更新し、前記分散が所定の閾値よりも大きいときは、前記温度特性情報に含まれている前記係数の値を更新しなくてもよい。

【0180】

この方位算出装置によれば、温度特性推定部は、角速度バイアスを近似する多項式の係数の推定精度が所定値以上の場合のみ、温度特性情報に含まれている係数の値を更新するので、角速度バイアス予測部は、温度特性情報に基づいて、角速度バイアスを精度よく予測することができる。したがって、この方位算出装置によれば、角速度バイアスが高精度に補正された方位を算出することができる。

【0181】

本発明に係る方位算出方法の一態様は、
衛星信号受信機が受信した衛星信号又は外界センサーの検出結果に基づいて、第１方位を算出する第１方位演算工程と、
角速度センサーが角速度を検出して出力した角速度信号に基づいて、第２方位を算出する第２方位演算工程と、
前記第１方位と前記第２方位とに基づいて第３方位を算出する第３方位演算工程と、
前記第２方位と前記第３方位とに基づいて、温度センサーが検出した温度に対する角速度バイアスの温度特性を推定し、推定した前記温度特性に基づいて、記憶部に記憶されている前記角速度バイアスの温度特性情報を更新する温度特性推定工程と、
前記温度センサーが検出した前記温度と前記記憶部に記憶されている前記温度特性情報とに基づいて、前記角速度バイアスを予測する角速度バイアス予測工程と、を含み、
前記第２方位演算工程では、前記角速度バイアス予測工程で予測した前回の前記角速度バイアスに基づいて、前記角速度バイアスが補正された前記第２方位を算出する。

【0182】

この方位算出方法では、移動体が停止中であっても移動中であっても、第１方位演算工程において衛星信号又は外界センサーの検出結果に基づいて第１方位を算出することができ、衛星信号が受信できない場合でも、第２方位演算工程において角速度信号に基づいて第２方位を算出することができるので、第３方位演算工程において、第１方位と第２方位とに基づいて途切れなく周期的に第３方位を算出することができる。その結果、移動体が移動中であっても、温度特性推定工程において、第２方位と前記第３方位とに基づいて角速度バイアスの温度特性を周期的に推定し、温度特性情報を更新することができるので、第２方位演算工程において、角速度バイアス予測工程で温度特性情報に基づいて予測した前回の角速度バイアスに基づいて、角速度バイアスが補正された第２方位を算出することができる。また、温度特性推定工程において、温度センサーが検出した温度に対する角速度バイアスの温度特性を推定するので、角速度バイアスの温度特性を狭い温度範囲毎に精度よく推定することができる。したがって、この方位算出方法によれば、移動体が停止中であっても移動中であっても、角速度バイアスが高精度に補正された方位を算出することができる。

【符号の説明】

【0183】

１…方位算出装置、５…自動車、１０…センサーモジュール、１１…衛星信号受信機、１２…車輪速センサー、１３…角速度センサー、１４…６Ｄｏｆセンサー、１５…温度センサー、１６…外界センサー、２０…第１方位演算部、３０…第２方位演算部、４０…第３方位演算部、５０…角速度バイアス予測部、６０…温度特性推定部、７０…記憶部、７１…温度特性情報、７２…地図情報

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】