ホーム > 特許ランキング > 株式会社豊田自動織機
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024110633
(43)【公開日】2024-08-16
(54)【発明の名称】方位算出装置及び方位算出方法
(51)【国際特許分類】
G01C 21/28 20060101AFI20240808BHJP
【ＦＩ】
G01C21/28
【審査請求】未請求
【請求項の数】6
【出願形態】ＯＬ
(21)【出願番号】P 2023015327
(22)【出願日】2023-02-03
(71)【出願人】
【識別番号】000003218
【氏名又は名称】株式会社豊田自動織機
(74)【代理人】
【識別番号】100105957
【弁理士】
【氏名又は名称】恩田 誠
(74)【代理人】
【識別番号】100068755
【弁理士】
【氏名又は名称】恩田 博宣
(72)【発明者】
【氏名】坂元 優太
【テーマコード（参考）】
2F129
【Ｆターム（参考）】
2F129AA03
2F129BB03
2F129BB21
2F129BB23
2F129BB24
2F129BB25
2F129BB37
2F129BB41
(57)【要約】
【課題】移動体方位を正確に算出できる方位算出装置及び方位算出方法を提供する。
【解決手段】方位算出装置は、位置取得装置が取得した移動体の絶対位置と慣性センサが検出した移動体の相対方位であるジャイロ方位Ｄｇとに基づいて移動体の絶対方位である移動体方位Ｄを算出する。方位算出装置は、移動体の絶対位置の変化に基づいて移動体の絶対方位である基準方位を算出する。方位算出装置は、基準方位とジャイロ方位との方位差である方位オフセットを複数算出する。方位算出装置は、複数の方位オフセットの平均値である方位オフセット平均θａｖｅを算出する。方位算出装置は、方位オフセット平均θａｖｅに基づいてジャイロ方位Ｄｇを回転補正することにより移動体方位Ｄを算出する。
【選択図】図６
【解決手段】方位算出装置は、位置取得装置が取得した移動体の絶対位置と慣性センサが検出した移動体の相対方位であるジャイロ方位Ｄｇとに基づいて移動体の絶対方位である移動体方位Ｄを算出する。方位算出装置は、移動体の絶対位置の変化に基づいて移動体の絶対方位である基準方位を算出する。方位算出装置は、基準方位とジャイロ方位との方位差である方位オフセットを複数算出する。方位算出装置は、複数の方位オフセットの平均値である方位オフセット平均θａｖｅを算出する。方位算出装置は、方位オフセット平均θａｖｅに基づいてジャイロ方位Ｄｇを回転補正することにより移動体方位Ｄを算出する。
【選択図】図６
【特許請求の範囲】
【請求項1】
位置取得装置が取得した移動体の絶対位置と慣性センサが検出した前記移動体の相対方位であるジャイロ方位とに基づいて前記移動体の絶対方位である移動体方位を算出する方位算出装置であって、
前記移動体の絶対位置の変化に基づいて前記移動体の絶対方位である基準方位を算出し、前記基準方位と前記ジャイロ方位との方位差である方位オフセットを複数算出し、
複数の前記方位オフセットの平均値である方位オフセット平均を算出し、
前記方位オフセット平均に基づいて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する
ことを特徴とする方位算出装置。
前記移動体の絶対位置の変化に基づいて前記移動体の絶対方位である基準方位を算出し、前記基準方位と前記ジャイロ方位との方位差である方位オフセットを複数算出し、
複数の前記方位オフセットの平均値である方位オフセット平均を算出し、
前記方位オフセット平均に基づいて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する
ことを特徴とする方位算出装置。
【請求項2】
前記方位オフセット平均を複数算出し、
算出された複数の前記方位オフセット平均のうち、最新の前記方位オフセット平均を用いて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する請求項１に記載の方位算出装置。
算出された複数の前記方位オフセット平均のうち、最新の前記方位オフセット平均を用いて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する請求項１に記載の方位算出装置。
【請求項3】
前記方位オフセット平均を複数算出するとともに複数の前記方位オフセット平均を蓄積し、
前記移動体が停止した場合には、蓄積している複数の前記方位オフセット平均のうち、停止時刻以前の所定期間における複数の前記方位オフセット平均に基づいて、前記移動体の移動が再開する時刻である移動再開時刻における前記方位オフセット平均を推定し、
推定した前記方位オフセット平均を用いて前記移動再開時刻における前記ジャイロ方位を回転補正することにより、前記移動再開時刻における前記移動体方位を算出する請求項１に記載の方位算出装置。
前記移動体が停止した場合には、蓄積している複数の前記方位オフセット平均のうち、停止時刻以前の所定期間における複数の前記方位オフセット平均に基づいて、前記移動体の移動が再開する時刻である移動再開時刻における前記方位オフセット平均を推定し、
推定した前記方位オフセット平均を用いて前記移動再開時刻における前記ジャイロ方位を回転補正することにより、前記移動再開時刻における前記移動体方位を算出する請求項１に記載の方位算出装置。
【請求項4】
前記移動体の絶対位置の変化に基づいて前記移動体の移動距離を算出し、前記移動距離に基づいて前記移動体が移動しているか否かを判定し、
前記移動距離が所定距離以上である場合、前記移動体が移動していると判定し、
前記移動体が移動していると判定した場合、前記基準方位を算出する請求項１に記載の方位算出装置。
前記移動距離が所定距離以上である場合、前記移動体が移動していると判定し、
前記移動体が移動していると判定した場合、前記基準方位を算出する請求項１に記載の方位算出装置。
【請求項5】
前記基準方位をベクトル化した基準方位ベクトルと前記ジャイロ方位をベクトル化したジャイロ方位ベクトルとの和である方位オフセットベクトルを生成し、複数の前記方位オフセットベクトルを合成した合成ベクトルを用いて前記方位オフセット平均を算出する請求項１に記載の方位算出装置。
【請求項6】
方位算出装置が、位置取得装置が取得した移動体の絶対位置と慣性センサが検出した前記移動体の相対方位であるジャイロ方位とに基づいて前記移動体の絶対方位である移動体方位を算出する方位算出方法であって、
前記方位算出装置が、
前記移動体の絶対位置の変化に基づいて前記移動体の絶対方位である基準方位を算出し、前記基準方位と前記ジャイロ方位との方位差である方位オフセットを複数算出し、
複数の前記方位オフセットの平均値である方位オフセット平均を算出し、
前記方位オフセット平均に基づいて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する
ことを特徴とする方位算出方法。
前記方位算出装置が、
前記移動体の絶対位置の変化に基づいて前記移動体の絶対方位である基準方位を算出し、前記基準方位と前記ジャイロ方位との方位差である方位オフセットを複数算出し、
複数の前記方位オフセットの平均値である方位オフセット平均を算出し、
前記方位オフセット平均に基づいて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する
ことを特徴とする方位算出方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、方位算出装置及び方位算出方法に関する。
【背景技術】
【0002】
特許文献１には、位置取得装置が取得した移動体の絶対位置と慣性センサが検出した移動体の相対方位であるジャイロ方位とに基づいて、移動体の絶対方位である移動体方位を算出する方位算出装置が開示されている。具体的には、方位算出装置は、位置取得装置から移動体の絶対位置を取得する。方位算出装置は、移動体の絶対位置の変化に基づいて移動体の絶対方位である基準方位を算出する。また、方位算出装置は、慣性センサから移動体のジャイロ方位を取得する。移動体が直進しているとき、基準方位及びジャイロ方位は変化しない。方位算出装置は、移動体が直進しているときのジャイロ方位を初期方位に設定する。初期方位は、移動体が直進しているときの基準方位によって絶対方位に換算可能である。方位算出装置は、初期方位に対する移動体の回転角から移動体方位を算出する。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特許第３４７３１１３号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
ところで、位置取得装置が取得する移動体の絶対位置には誤差がある。このため、移動体の絶対位置の変化に基づいて算出される基準方位にも誤差が生じる。また、慣性センサが検出するジャイロ方位にも温度ドリフトなどの誤差がある。しかしながら、特許文献１では、移動体の絶対位置及びジャイロ方位の誤差が考慮されていなかった。
【課題を解決するための手段】
【0005】
上記問題点を解決するための方位算出装置は、位置取得装置が取得した移動体の絶対位置と慣性センサが検出した前記移動体の相対方位であるジャイロ方位とに基づいて前記移動体の絶対方位である移動体方位を算出する方位算出装置であって、前記移動体の絶対位置の変化に基づいて前記移動体の絶対方位である基準方位を算出し、前記基準方位と前記ジャイロ方位との方位差である方位オフセットを複数算出し、複数の前記方位オフセットの平均値である方位オフセット平均を算出し、前記方位オフセット平均に基づいて前記ジャイロ方位を回転補正することにより前記移動体方位を算出することを要旨とする。
【0006】
上記構成によれば、方位算出装置は、複数の方位オフセットの平均値である方位オフセット平均に基づいてジャイロ方位を回転補正することにより移動体方位を算出する。これにより、１つの方位オフセットを用いてジャイロ方位を回転補正することにより移動体方位を算出する場合と比較して、移動体の絶対位置及びジャイロ方位の双方の誤差が移動体方位に及ぼす影響を低減できる。よって、移動体方位を正確に算出できる。
【0007】
上記方位算出装置において、前記方位オフセット平均を複数算出し、算出された複数の前記方位オフセット平均のうち、最新の前記方位オフセット平均を用いて前記ジャイロ方位を回転補正することにより前記移動体方位を算出してもよい。
【0008】
上記構成によれば、方位算出装置は、最新の方位オフセット平均を用いてジャイロ方位を回転補正することにより移動体方位を算出する。したがって、最新の方位オフセット平均よりも前に算出された方位オフセット平均を用いてジャイロ方位を回転補正することにより移動体方位を算出する場合と比較して、移動体方位をより正確に算出できる。
【0009】
上記方位算出装置において、前記方位オフセット平均を複数算出するとともに複数の前記方位オフセット平均を蓄積し、前記移動体が停止した場合には、蓄積している複数の前記方位オフセット平均のうち、停止時刻以前の所定期間における複数の前記方位オフセット平均に基づいて、前記移動体の移動が再開する時刻である移動再開時刻における前記方位オフセット平均を推定し、推定した前記方位オフセット平均を用いて前記移動再開時刻における前記ジャイロ方位を回転補正することにより、前記移動再開時刻における前記移動体方位を算出してもよい。
【0010】
ジャイロ方位は、温度ドリフトによって移動体が停止している間も変化している。このため、ジャイロ方位に基づいて算出される方位オフセット平均も変化している。上記構成によれば、方位算出装置は、移動体が停止した場合には、停止時刻以前の所定期間における複数の方位オフセット平均に基づいて、移動再開時刻における方位オフセット平均を推定する。つまり、方位算出装置は、方位オフセット平均が変化することを考慮して、過去の方位オフセット平均から移動再開時刻における方位オフセット平均を推定する。そして、方位算出装置は、推定した方位オフセット平均を用いて移動再開時刻におけるジャイロ方位を回転補正することにより、移動再開時刻における移動体方位を算出する。したがって、移動再開時刻における移動体方位をより正確に算出できる。
【0011】
上記方位算出装置において、前記移動体の絶対位置の変化に基づいて前記移動体の移動距離を算出し、前記移動距離に基づいて前記移動体が移動しているか否かを判定し、前記移動距離が所定距離以上である場合、前記移動体が移動していると判定し、前記移動体が移動していると判定した場合、前記基準方位を算出してもよい。
【0012】
位置取得装置が取得した移動体の絶対位置には誤差がある。このため、実際には移動体が移動していないにも関わらず、誤差により移動体の絶対位置が変化することによって、移動体が移動しているように見えることがある。上記構成によれば、方位算出装置は、移動体の絶対位置の変化に基づいて移動体の移動距離を算出する。方位算出装置は、移動体の移動距離に基づいて移動体が移動しているか否かを判定する。移動体が移動していない場合、移動体の移動距離はゼロに近い値になる。このため、方位算出装置は、移動体の移動距離が所定距離以上である場合、移動体が移動していると判定する。方位算出装置は、移動体が移動していると判定した場合には、移動体の基準方位を算出する。したがって、実際には移動体が移動していないにも関わらず、方位算出装置が基準方位を算出することを回避できる。
【0013】
上記方位算出装置において、前記基準方位をベクトル化した基準方位ベクトルと前記ジャイロ方位をベクトル化したジャイロ方位ベクトルとの和である方位オフセットベクトルを生成し、複数の前記方位オフセットベクトルを合成した合成ベクトルを用いて前記方位オフセット平均を算出してもよい。
【0014】
上記構成によれば、基準方位とジャイロ方位とを算術平均することにより方位オフセットを算出した後、複数の方位オフセットを算術平均することにより方位オフセット平均を算出する場合に生じ得る問題を回避できる。したがって、方位オフセット平均を好適に算出できる。
【0015】
上記問題点を解決するための方位算出方法は、方位算出装置が、位置取得装置が取得した移動体の絶対位置と慣性センサが検出した前記移動体の相対方位であるジャイロ方位とに基づいて前記移動体の絶対方位である移動体方位を算出する方位算出方法であって、前記方位算出装置が、前記移動体の絶対位置の変化に基づいて前記移動体の絶対方位である基準方位を算出し、前記基準方位と前記ジャイロ方位との方位差である方位オフセットを複数算出し、複数の前記方位オフセットの平均値である方位オフセット平均を算出し、前記方位オフセット平均に基づいて前記ジャイロ方位を回転補正することにより前記移動体方位を算出することを要旨とする。
【0016】
上記方法によれば、方位算出装置は、複数の方位オフセットの平均値である方位オフセット平均に基づいてジャイロ方位を回転補正することにより移動体方位を算出する。これにより、１つの方位オフセットを用いてジャイロ方位を回転補正することにより移動体方位を算出する場合と比較して、移動体の絶対位置及びジャイロ方位の双方の誤差が移動体方位に及ぼす影響を低減できる。よって、移動体方位を正確に算出できる。
【発明の効果】
【0017】
本発明によれば、移動体方位を正確に算出できる。
【図面の簡単な説明】
【0018】
【図1】移動体の構成を示すブロック図である。
【図2】移動体の移動軌跡の一例を示す図である。
【図3】方位オフセット平均算出処理を示すフローチャートである。
【図4】方位オフセットを説明する図である。
【図5】方位オフセット平均の算出方法を説明する図である。
【図6】移動体方位算出処理を説明する図である。
【図7】方位オフセット平均推定処理を示すフローチャートである。
【図8】方位オフセット平均推定処理を説明する図である。
【発明を実施するための形態】
【0019】
以下、方位算出装置及び方位算出方法を具体化した一実施形態を図１～図８にしたがって説明する。
図１に示すように、移動体１０は、位置取得装置１１と、慣性センサ（IMU：Inertial Measurement Unit）１２と、方位算出装置１３とを備えている。本実施形態の移動体１０は、車両である。
図１に示すように、移動体１０は、位置取得装置１１と、慣性センサ（IMU：Inertial Measurement Unit）１２と、方位算出装置１３とを備えている。本実施形態の移動体１０は、車両である。
【0020】
位置取得装置１１は、移動体１０の絶対位置を取得する。本実施形態の位置取得装置１１は、ＧＰＳ受信機である。本実施形態の位置取得装置１１は、ＧＰＳ衛星からの電波を受信することにより、移動体１０の絶対位置を算出する。移動体１０の絶対位置は、例えば、移動体１０の経度、緯度、及び高度からなる３次元座標である。
【0021】
慣性センサ１２は、ジャイロセンサである。慣性センサ１２は、移動体１０の角速度を検出する。本実施形態の慣性センサ１２は、前後方向に延びるロール軸周りの角速度、左右方向に延びるピッチ軸周りの角速度、及び上下方向に延びるヨー軸周りの角速度を検出する。慣性センサ１２は、移動体１０の角速度を時間積分することによって移動体１０の回転角を算出する。慣性センサ１２は、慣性センサ１２の電源がオンになったときの方位を初期方位とする。慣性センサ１２は、初期方位を移動体１０の回転角だけ回転させた方位をジャイロ方位Ｄｇとして算出する。つまり、ジャイロ方位Ｄｇは、初期方位を基準とした相対方位である。
【0022】
図２は、時刻ｔ０から時刻ｔ８までの間における移動体１０の移動軌跡の一例を示している。移動体１０は、時刻ｔ０から時刻ｔ８までの間において移動し続けている。図２では、時刻ｔ０から時刻ｔ８までの各時刻における移動体１０の絶対座標Ｐ０～Ｐ８をプロットしている。また、図２では、時刻ｔ１から時刻ｔ８までの各時刻におけるジャイロ方位Ｄｇ１～Ｄｇ８を模式的に示している。
【0023】
＜方位算出装置＞
図１に示すように、方位算出装置１３は、プロセッサ１３ａと記憶部１３ｂとを備える。プロセッサ１３ａとしては、例えば、CPU（Central Processing Unit）、GPU（Graphics Processing Unit）、及びDSP（Digital Signal Processor）を挙げることができる。記憶部１３ｂは、RAM（Random Access Memory）及びROM（Read Only Memory）を含む。記憶部１３ｂは、処理をプロセッサ１３ａに実行させるように構成されたプログラムコードまたは指令を格納している。記憶部１３ｂ、即ち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。方位算出装置１３は、ASIC（Application Specific Integrated Circuit）やFPGA（Field Programmable Gate Array）等のハードウェア回路によって構成されていてもよい。処理回路である方位算出装置１３は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。
図１に示すように、方位算出装置１３は、プロセッサ１３ａと記憶部１３ｂとを備える。プロセッサ１３ａとしては、例えば、CPU（Central Processing Unit）、GPU（Graphics Processing Unit）、及びDSP（Digital Signal Processor）を挙げることができる。記憶部１３ｂは、RAM（Random Access Memory）及びROM（Read Only Memory）を含む。記憶部１３ｂは、処理をプロセッサ１３ａに実行させるように構成されたプログラムコードまたは指令を格納している。記憶部１３ｂ、即ち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。方位算出装置１３は、ASIC（Application Specific Integrated Circuit）やFPGA（Field Programmable Gate Array）等のハードウェア回路によって構成されていてもよい。処理回路である方位算出装置１３は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。
【0024】
方位算出装置１３は、位置取得装置１１及び慣性センサ１２と接続されている。方位算出装置１３は、位置取得装置１１が取得した移動体１０の絶対位置と慣性センサ１２が検出したジャイロ方位Ｄｇとに基づいて、移動体１０の絶対方位である移動体方位Ｄを算出する。なお、以下では、ヨー軸周りの移動体方位Ｄを算出する方法について説明するが、ロール軸周りの移動体方位Ｄ及びピッチ軸周りの移動体方位Ｄについても同様に算出することができる。
【0025】
＜方位算出方法＞
方位算出装置１３が移動体１０の絶対位置とジャイロ方位Ｄｇとに基づいて移動体方位Ｄを算出する方法である方位算出方法について説明する。方位算出装置１３は、後述する方位オフセット平均θａｖｅを算出する方位オフセット平均算出処理と、算出された方位オフセット平均θａｖｅに基づいて移動体方位Ｄを算出する移動体方位算出処理とを行う。
方位算出装置１３が移動体１０の絶対位置とジャイロ方位Ｄｇとに基づいて移動体方位Ｄを算出する方法である方位算出方法について説明する。方位算出装置１３は、後述する方位オフセット平均θａｖｅを算出する方位オフセット平均算出処理と、算出された方位オフセット平均θａｖｅに基づいて移動体方位Ｄを算出する移動体方位算出処理とを行う。
【0026】
＜方位オフセット平均算出処理＞
図３に示すように、ステップＳ１１において、方位算出装置１３は、位置取得装置１１から移動体１０の絶対位置として移動体１０の座標を取得する。方位算出装置１３は、移動体１０の起点座標が設定されていない場合（ステップＳ１２でＮＯ）、ステップＳ１３において、直前のステップＳ１１で取得した移動体１０の座標を起点座標に設定する。移動体１０の起点座標を設定した方位算出装置１３は、ステップＳ１１に戻る。一方、方位算出装置１３は、移動体１０の起点座標が設定されている場合（ステップＳ１２でＹＥＳ）、ステップＳ１４に進む。
図３に示すように、ステップＳ１１において、方位算出装置１３は、位置取得装置１１から移動体１０の絶対位置として移動体１０の座標を取得する。方位算出装置１３は、移動体１０の起点座標が設定されていない場合（ステップＳ１２でＮＯ）、ステップＳ１３において、直前のステップＳ１１で取得した移動体１０の座標を起点座標に設定する。移動体１０の起点座標を設定した方位算出装置１３は、ステップＳ１１に戻る。一方、方位算出装置１３は、移動体１０の起点座標が設定されている場合（ステップＳ１２でＹＥＳ）、ステップＳ１４に進む。
【0027】
位置取得装置１１が取得した移動体１０の座標には誤差がある。このため、実際には移動体１０が移動していないにも関わらず、誤差により移動体１０の座標が変化することによって、移動体１０が移動しているように見えることがある。このため、本実施形態の方位算出装置１３は、ステップＳ１４，Ｓ１５において、移動体１０が移動しているか否かを判定する。
【0028】
ステップＳ１４において、方位算出装置１３は、移動体１０の絶対位置の変化に基づいて移動体１０の移動距離を算出する。具体的には、方位算出装置１３は、起点座標と最新座標との２点間距離を算出する。最新座標は、方位算出装置１３が取得した移動体１０の座標のうち、直前のステップＳ１１で取得した移動体１０の座標である。２点間距離は、起点座標から最新座標までの移動体１０の移動距離である。移動体１０の絶対位置が変化しているものの、実際には移動体１０が移動していない場合、移動体１０の移動距離はゼロに近い値になる。
【0029】
ステップＳ１５において、方位算出装置１３は、移動体１０の移動距離である２点間距離に基づいて、移動体１０が移動しているか否かを判定する。２点間距離が所定距離以上である場合（ステップＳ１５でＹＥＳ）、方位算出装置１３は、移動体１０が移動していると判定する。この場合、方位算出装置１３は、ステップＳ１６に進む。一方、２点間距離が所定距離以上でない場合、すなわち所定距離未満である場合（ステップＳ１５でＮＯ）、方位算出装置１３は、移動体１０が移動していない、すなわち移動体１０が停止していると判定する。この場合、方位算出装置１３は、ステップＳ１１に戻る。
【0030】
ステップＳ１６において、方位算出装置１３は、移動体１０の絶対位置の変化に基づいて、移動体１０の絶対方位である基準方位Ｄｓを算出する。具体的には、方位算出装置１３は、起点座標と最新座標とから移動体１０の基準方位Ｄｓを算出する。
【0031】
ステップＳ１７において、方位算出装置１３は、慣性センサ１２から、移動体１０が最新座標に位置するときの移動体１０のジャイロ方位Ｄｇを取得する。
ステップＳ１８において、方位算出装置１３は、基準方位Ｄｓとジャイロ方位Ｄｇとの方位差である方位オフセットθを算出する。
ステップＳ１８において、方位算出装置１３は、基準方位Ｄｓとジャイロ方位Ｄｇとの方位差である方位オフセットθを算出する。
【0032】
ステップＳ１９において、方位算出装置１３は、最新座標を起点座標に設定する。
図４に示すように、方位オフセットθは、基準方位Ｄｓとジャイロ方位Ｄｇとがなす角である。方位オフセットθは、理論上、移動体１０の動作状態に依らず、各時刻において一定の角度になるはずである。
図４に示すように、方位オフセットθは、基準方位Ｄｓとジャイロ方位Ｄｇとがなす角である。方位オフセットθは、理論上、移動体１０の動作状態に依らず、各時刻において一定の角度になるはずである。
【0033】
例えば、移動体１０は、例えば、時刻ｔａから時刻ｔｂまでの間に曲がったとする。時刻ｔａにおける基準方位Ｄｓａとジャイロ方位Ｄｇａとの方位差を方位オフセットθａとする。時刻ｔｂにおける基準方位Ｄｓｂとジャイロ方位Ｄｇｂとの方位差を方位オフセットθｂとする。時刻ｔｂにおける基準方位Ｄｓｂは、時刻ｔａにおける基準方位Ｄｓａに対して移動体１０の回転角αだけ回転した方位である。また、時刻ｔｂにおけるジャイロ方位Ｄｇｂは、時刻ｔａにおけるジャイロ方位Ｄｇａに対して移動体１０の回転角αだけ回転した方位である。このため、時刻ｔｂにおける方位オフセットθｂは、時刻ｔａにおける方位オフセットθａと等しくなるはずである。
【0034】
しかしながら、実際には、図４において二点鎖線で示すように、基準方位Ｄｓ及びジャイロ方位Ｄｇの双方に誤差が生じている。詳しくは、位置取得装置１１が取得する移動体１０の絶対位置には誤差がある。このため、移動体１０の絶対位置の変化に基づいて算出される基準方位Ｄｓにも誤差が生じる。また、慣性センサ１２が検出するジャイロ方位Ｄｇにも温度ドリフトなどの誤差がある。したがって、基準方位Ｄｓとジャイロ方位Ｄｇとの方位差である方位オフセットθにも誤差が生じる。その結果、方位オフセットθは、一定の角度にはならず、各時刻で異なった角度になる。方位算出装置１３は、複数の方位オフセットθの平均値である方位オフセット平均θａｖｅを算出する。
【0035】
図３に示すように、方位算出装置１３は、Ｎ個の方位オフセットθを算出していない場合（ステップＳ２０でＮＯ）、ステップＳ１１に戻る。Ｎは、２以上の任意の整数である。本実施形態では、Ｎ＝７である。一方、方位算出装置１３は、Ｎ個の方位オフセットθを算出した場合（ステップＳ２０でＹＥＳ）、ステップＳ２１に進む。つまり、方位算出装置１３は、Ｎ個の方位オフセットθを算出するまで、方位オフセットθの算出を繰り返し行う。したがって、方位算出装置１３は、方位オフセットθを複数算出する。
【0036】
ステップＳ２１において、方位算出装置１３は、算出したＮ個の方位オフセットθから方位オフセット平均θａｖｅを算出する。
方位オフセット平均θａｖｅの算出方法として、例えば、次のような方法が考えられる。基準方位Ｄｓとジャイロ方位Ｄｇとを算術平均することによって方位オフセットθを算出する。そして、複数の方位オフセットθを算術平均することによって方位オフセット平均θａｖｅを算出する。しかしながら、この方法では、次のような問題が生じる。
方位オフセット平均θａｖｅの算出方法として、例えば、次のような方法が考えられる。基準方位Ｄｓとジャイロ方位Ｄｇとを算術平均することによって方位オフセットθを算出する。そして、複数の方位オフセットθを算術平均することによって方位オフセット平均θａｖｅを算出する。しかしながら、この方法では、次のような問題が生じる。
【0037】
第１の問題として、任意のＸ軸と基準方位Ｄｓとがなす角度が４５度であり、Ｘ軸とジャイロ方位Ｄｇとがなす角度が３１５度であるとする。この場合、方位オフセットθは、０度になるのが適切である。しかしながら、基準方位Ｄｓとジャイロ方位Ｄｇとの算出平均により方位オフセットθを算出する場合、方位オフセットθは１８０度になってしまう。
【0038】
第２の問題として、Ｘ軸と基準方位Ｄｓとがなす角度が９０度であり、Ｘ軸とジャイロ方位Ｄｇとがなす角度が２７０度であるとする。この場合、方位オフセットθは、０度及び１８０度の両方を取り得る。
【0039】
本実施形態の方位算出装置１３は、このような問題を回避するため、ベクトルを用いて方位オフセット平均θａｖｅを算出する。
図５に示すように、方位算出装置１３は、基準方位Ｄｓをベクトル化した基準方位ベクトルとジャイロ方位Ｄｇをベクトル化したジャイロ方位ベクトルとの和を単位ベクトル化した方位オフセットベクトルＶを生成する。方位算出装置１３は、Ｎ個の方位オフセットベクトルＶを生成する。方位算出装置１３は、Ｎ個の方位オフセットベクトルＶを合成した合成ベクトルＶｔを生成する。方位算出装置１３は、生成した合成ベクトルＶｔとＸ軸とがなす角を方位オフセット平均θａｖｅとする。
図５に示すように、方位算出装置１３は、基準方位Ｄｓをベクトル化した基準方位ベクトルとジャイロ方位Ｄｇをベクトル化したジャイロ方位ベクトルとの和を単位ベクトル化した方位オフセットベクトルＶを生成する。方位算出装置１３は、Ｎ個の方位オフセットベクトルＶを生成する。方位算出装置１３は、Ｎ個の方位オフセットベクトルＶを合成した合成ベクトルＶｔを生成する。方位算出装置１３は、生成した合成ベクトルＶｔとＸ軸とがなす角を方位オフセット平均θａｖｅとする。
【0040】
ステップＳ２２において、方位算出装置１３は、算出した方位オフセット平均θａｖｅを記憶する。
方位算出装置１３は、このような方位オフセット平均算出処理を繰り返し行う。つまり、方位算出装置１３は、方位オフセット平均θａｖｅを複数算出する。そして、方位算出装置１３は、方位オフセット平均θａｖｅを算出する毎に記憶する。つまり、方位算出装置１３は、算出した複数の方位オフセット平均θａｖｅを蓄積する。
方位算出装置１３は、このような方位オフセット平均算出処理を繰り返し行う。つまり、方位算出装置１３は、方位オフセット平均θａｖｅを複数算出する。そして、方位算出装置１３は、方位オフセット平均θａｖｅを算出する毎に記憶する。つまり、方位算出装置１３は、算出した複数の方位オフセット平均θａｖｅを蓄積する。
【0041】
＜移動体方位算出処理＞
上述したように、方位オフセットθは、絶対方位である基準方位Ｄｓと相対方位であるジャイロ方位Ｄｇとの方位差である。言い換えると、ジャイロ方位Ｄｇを方位オフセットθだけ回転させた方位が基準方位Ｄｓである。したがって、方位オフセットθが予め分かっていれば、基準方位Ｄｓを算出しなくても、ジャイロ方位Ｄｇを方位オフセットθだけ回転補正することにより、移動体方位Ｄを算出することができる。ただし、方位オフセットθは、各時刻で異なっている。したがって、方位算出装置１３は、複数の方位オフセットθの平均値である方位オフセット平均θａｖｅに基づいてジャイロ方位Ｄｇを回転補正することにより、移動体方位Ｄを算出する。
上述したように、方位オフセットθは、絶対方位である基準方位Ｄｓと相対方位であるジャイロ方位Ｄｇとの方位差である。言い換えると、ジャイロ方位Ｄｇを方位オフセットθだけ回転させた方位が基準方位Ｄｓである。したがって、方位オフセットθが予め分かっていれば、基準方位Ｄｓを算出しなくても、ジャイロ方位Ｄｇを方位オフセットθだけ回転補正することにより、移動体方位Ｄを算出することができる。ただし、方位オフセットθは、各時刻で異なっている。したがって、方位算出装置１３は、複数の方位オフセットθの平均値である方位オフセット平均θａｖｅに基づいてジャイロ方位Ｄｇを回転補正することにより、移動体方位Ｄを算出する。
【0042】
図６に示すように、方位算出装置１３は、慣性センサ１２から、現在時刻におけるジャイロ方位Ｄｇを取得する。方位算出装置１３は、算出した方位オフセット平均θａｖｅを記憶している。方位算出装置１３は、算出済みの方位オフセット平均θａｖｅを用いて現在時刻におけるジャイロ方位Ｄｇを回転補正することにより、現在時刻における移動体方位Ｄを算出する。具体的には、方位算出装置１３は、現在時刻におけるジャイロ方位Ｄｇを方位オフセット平均θａｖｅだけ回転させた方位を、現在時刻における移動体方位Ｄとして算出する。なお、方位算出装置１３は、複数の方位オフセット平均θａｖｅを算出している場合、算出した複数の方位オフセット平均θａｖｅのうち、最新の方位オフセット平均θａｖｅを用いてジャイロ方位Ｄｇを回転補正することによって、移動体方位Ｄを算出する。
【0043】
【0044】
方位算出装置１３は、位置取得装置１１から時刻ｔ０における移動体１０の座標Ｐ０を取得する。方位算出装置１３は、移動体１０の起点座標が設定されていないため、時刻ｔ０における移動体１０の座標Ｐ０を起点座標に設定する。
【0045】
方位算出装置１３は、位置取得装置１１から時刻ｔ１における移動体１０の座標Ｐ１を取得する。方位算出装置１３は、移動体１０の起点座標がすでに設定されているため、起点座標である時刻ｔ０における移動体１０の座標Ｐ０と、最新座標である時刻ｔ１における移動体１０の座標Ｐ１とから、２点間距離を算出する。２点間距離は、時刻ｔ０から時刻ｔ１における移動体１０の移動距離である。上述したように、時刻ｔ０から時刻ｔ８までの間において移動体１０は移動し続けている。このため、２点間距離は、所定距離以上になる。したがって、方位算出装置１３は、起点座標である時刻ｔ０における移動体１０の座標Ｐ０と、最新座標である時刻ｔ１における移動体１０の座標Ｐ１とから、時刻ｔ１における移動体１０の基準方位Ｄｓ１を算出する。
【0046】
方位算出装置１３は、慣性センサ１２から時刻ｔ１における移動体１０のジャイロ方位Ｄｇ１を取得する。方位算出装置１３は、時刻ｔ１における移動体１０の基準方位Ｄｓ１とジャイロ方位Ｄｇ１との方位差を時刻ｔ１における方位オフセットθとして算出する。方位算出装置１３は、時刻ｔ１における移動体１０の座標Ｐ１を起点座標に設定する。
【0047】
時刻ｔ１における方位オフセットθは、１個目の方位オフセットθである。方位算出装置１３は、７個の方位オフセットθを算出するまで方位オフセットθの算出を繰り返し行う。すなわち、方位算出装置１３は、時刻ｔ１から時刻ｔ７までの各時刻における方位オフセットθを算出するまで、方位オフセットθの算出を繰り返す。なお、時刻ｔ２から時刻ｔ７までの各時刻における方位オフセットθの算出方法は、時刻ｔ１における方位オフセットθの算出方法と同様であるため、説明を省略する。
【0048】
方位算出装置１３は、７個の方位オフセットθを算出すると、方位オフセット平均θａｖｅを算出する。したがって、方位オフセット平均θａｖｅは、時刻ｔ１から時刻ｔ７までの各時刻における方位オフセットθの平均値である。方位算出装置１３は、算出した方位オフセット平均θａｖｅを記憶する。
【0049】
次に、移動体方位算出処理について説明する。
方位算出装置１３は、時刻ｔ８における移動体方位Ｄを算出する場合、次のように算出する。方位算出装置１３は、慣性センサ１２から、時刻ｔ８におけるジャイロ方位Ｄｇ８を取得する。方位算出装置１３は、時刻ｔ１から時刻ｔ７までの各時刻における方位オフセットθの平均値である方位オフセット平均θａｖｅを記憶している。方位算出装置１３は、方位オフセット平均θａｖｅを用いて時刻ｔ８におけるジャイロ方位Ｄｇ８を回転補正することにより、時刻ｔ８における移動体方位Ｄを算出する。
方位算出装置１３は、時刻ｔ８における移動体方位Ｄを算出する場合、次のように算出する。方位算出装置１３は、慣性センサ１２から、時刻ｔ８におけるジャイロ方位Ｄｇ８を取得する。方位算出装置１３は、時刻ｔ１から時刻ｔ７までの各時刻における方位オフセットθの平均値である方位オフセット平均θａｖｅを記憶している。方位算出装置１３は、方位オフセット平均θａｖｅを用いて時刻ｔ８におけるジャイロ方位Ｄｇ８を回転補正することにより、時刻ｔ８における移動体方位Ｄを算出する。
【0050】
＜方位オフセット平均推定処理＞
ところで、移動体１０が停止すると、２点間距離は所定距離未満になるため、方位算出装置１３は、基準方位Ｄｓを算出しない。すると、基準方位Ｄｓとジャイロ方位Ｄｇとの方位差である方位オフセットθが算出されないため、複数の方位オフセットθから算出される方位オフセット平均θａｖｅも算出されない。つまり、移動体１０が停止すると、方位オフセット平均θａｖｅが新たに算出されなくなる。一方で、ジャイロ方位Ｄｇは、温度ドリフトによって移動体１０が停止している間も変化している。このため、ジャイロ方位Ｄｇに基づいて算出される方位オフセット平均θａｖｅも変化している。したがって、移動体１０の移動が再開する時刻である移動再開時刻ｔｒにおける移動体方位Ｄを算出する場合には、最新の方位オフセット平均θａｖｅを用いて移動再開時刻ｔｒにおけるジャイロ方位Ｄｇを回転補正することにより移動体方位Ｄを算出したとしても、移動体方位Ｄを正確に算出できないおそれがある。
ところで、移動体１０が停止すると、２点間距離は所定距離未満になるため、方位算出装置１３は、基準方位Ｄｓを算出しない。すると、基準方位Ｄｓとジャイロ方位Ｄｇとの方位差である方位オフセットθが算出されないため、複数の方位オフセットθから算出される方位オフセット平均θａｖｅも算出されない。つまり、移動体１０が停止すると、方位オフセット平均θａｖｅが新たに算出されなくなる。一方で、ジャイロ方位Ｄｇは、温度ドリフトによって移動体１０が停止している間も変化している。このため、ジャイロ方位Ｄｇに基づいて算出される方位オフセット平均θａｖｅも変化している。したがって、移動体１０の移動が再開する時刻である移動再開時刻ｔｒにおける移動体方位Ｄを算出する場合には、最新の方位オフセット平均θａｖｅを用いて移動再開時刻ｔｒにおけるジャイロ方位Ｄｇを回転補正することにより移動体方位Ｄを算出したとしても、移動体方位Ｄを正確に算出できないおそれがある。
【0051】
図７に示すように、本実施形態の方位算出装置１３は、ステップＳ３１において、移動体１０が停止したか否かを判定する。なお、移動体１０が停止したか否かを判定する方法は、方位オフセット平均算出処理におけるステップＳ１４，Ｓ１５と同様の方法であるため、説明を省略する。
【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】
［２］前記方位オフセット平均を複数算出し、算出された複数の前記方位オフセット平均のうち、最新の前記方位オフセット平均を用いて前記ジャイロ方位を回転補正することにより前記移動体方位を算出する［１］に記載の方位算出装置。
【0079】
［３］前記方位オフセット平均を複数算出するとともに複数の前記方位オフセット平均を蓄積し、前記移動体が停止した場合には、蓄積している複数の前記方位オフセット平均のうち、停止時刻以前の所定期間における複数の前記方位オフセット平均に基づいて、前記移動体の移動が再開する時刻である移動再開時刻における前記方位オフセット平均を推定し、推定した前記方位オフセット平均を用いて前記移動再開時刻における前記ジャイロ方位を回転補正することにより、前記移動再開時刻における前記移動体方位を算出する［１］に記載の方位算出装置。
【0080】
［４］前記移動体の絶対位置の変化に基づいて前記移動体の移動距離を算出し、前記移動距離に基づいて前記移動体が移動しているか否かを判定し、前記移動距離が所定距離以上である場合、前記移動体が移動していると判定し、前記移動体が移動していると判定した場合、前記基準方位を算出する［１］～［３］の何れか１つに記載の方位算出装置。
【0081】
［５］前記基準方位をベクトル化した基準方位ベクトルと前記ジャイロ方位をベクトル化したジャイロ方位ベクトルとの和である方位オフセットベクトルを生成し、複数の前記方位オフセットベクトルを合成した合成ベクトルを用いて前記方位オフセット平均を算出する［１］～［４］の何れか１つに記載の方位算出装置。
【符号の説明】
【0082】
１０…移動体、１１…位置取得装置、１２…慣性センサ、１３…方位算出装置、Ｄ…移動体方位、Ｄｇ…ジャイロ方位、Ｄｓ…基準方位、Ｖ…方位オフセットベクトル、Ｖｔ…合成ベクトル、Ｔ…所定期間、ｔｒ…移動再開時刻、ｔｓ…移動停止時刻、θ…方位オフセット、θａｖｅ…方位オフセット平均。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】