(57)【要約】  本発明は、どのような取付角で設置されても自車位置を推定することが可能な測位装置および測位方法を提供することを目的とする。本発明による測位装置は、ＧＮＳＳ受信機と、速度センサと、３軸加速度センサと、３軸角速度センサと、距離計側部と、移動体が走行中である場合において少なくともＧＮＳＳ受信機で算出された３軸速度、距離計側部で算出された前後加速度および３軸加速度センサで検出された３軸加速度に基づいて、移動体が水平面等速直進走行中であるのか否かを判断し、水平面等速直進走行中であると判断した時の３軸加速度の零点を算出する３軸加速度センサ零点算出部と、３軸加速度の零点に基づいてピッチ方向取付角およびロール方向取付角を算出する装置筐体取付角算出部と、３軸角速度を座標変換する３軸角速度座標変換部と、３軸角速度から求めた少なくともヨーレートに基づいて移動体の現在位置を推定する位置推定部とを備える。
2017064790.tif 000002
【特許請求の範囲】
【請求項１】
移動体の測位装置であって、
ＧＰＳ衛星から電波を受信し、少なくとも前記移動体の現在位置と、前記測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力するＧＮＳＳ受信機と、
前記移動体の移動に応じたパルス信号を出力する速度センサと、
前記測位装置において互いに直交する３軸の角速度である３軸角速度を検出する３軸角速度センサと、
前記測位装置において互いに直交する３軸の加速度である３軸加速度を検出する３軸加速度センサと、
一定周期ごとに前記速度センサから出力された前記パルス信号に基づいて、前記移動体の移動距離、速度、および前後加速度を算出する距離計側部と、
前記移動体が走行中である場合において、少なくとも前記ＧＮＳＳ受信機で算出された前記３軸速度、前記距離計側部で算出された前記前後加速度、および前記３軸加速度センサで検出された前記３軸加速度に基づいて、前記移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、前記水平面等速直進走行中であると判断した時に前記３軸加速度センサで検出された前記３軸加速度を零点として算出する３軸加速度センサ零点算出部と、
前記３軸加速度センサ零点算出部で算出された前記３軸加速度の零点に基づいて、前記移動体に取り付けられた前記測位装置のピッチ方向取付角およびロール方向取付角を算出する装置筐体取付角算出部と、
前記装置筐体取付角算出部で算出された前記ピッチ方向取付角および前記ロール方向取付角に基づいて、前記３軸角速度センサで検出された前記３軸角速度を座標変換する３軸角速度座標変換部と、
前記３軸角速度座標変換部で変換された前記３軸角速度から求めた少なくともヨーレートに基づいて、前記移動体の現在位置を推定する位置推定部と、
を備える、測位装置。
【請求項２】
前記装置筐体取付角算出部は、前記ピッチ方向取付角、前記ロール方向取付角、および予め定められたヨー方向の角度に基づいて、前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向の加速度と、前記距離計側部で算出された前記前後加速度とが整合するときの前記ヨー方向の角度を前記測位装置のヨー方向取付角としてさらに算出し、
前記３軸角速度座標変換部は、前記ピッチ方向取付角、前記ロール方向取付角、および前記ヨー方向取付角に基づいて、前記３軸角速度を座標変換することを特徴とする、請求項１に記載の測位装置。
【請求項３】
前記装置筐体取付角算出部は、予め定められた角度範囲内における複数の前記ヨー方向の角度と、前記ピッチ方向取付角と、前記ロール方向取付角との全ての組み合わせについて、同一時刻の前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向の加速度と、前記距離計側部で算出された前記前後加速度とが最も整合するときの前記ヨー方向の角度を前記ヨー方向取付角として算出することを特徴とする、請求項２に記載の測位装置。
【請求項４】
前記装置筐体取付角算出部は、前記距離計側部で算出された前記速度と、前記３軸角速度座標変換部で座標変換された前記３軸角速度から求めたヨー角とに基づいて、前記移動体の前後方向と直交する法線方向の加速度である法線加速度を算出するとともに、前記ピッチ方向取付角、前記ロール方向取付角、および予め定められたヨー方向の角度に基づいて、前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向と直交する左右方向の加速度と、前記法線加速度とが整合するときの前記ヨー方向の角度を前記測位装置のヨー方向取付角としてさらに算出し、
前記３軸加速度座標変換部は、前記ピッチ方向取付角、前記ロール方向取付角、および前記ヨー方向取付角に基づいて、前記３軸加速度を座標変換することを特徴とする、請求項１に記載の測位装置。
【請求項５】
前記装置筐体取付角算出部は、予め定められた角度範囲内における複数の前記ヨー方向の角度と、前記ピッチ方向取付角と、前記ロール方向取付角との全ての組み合わせについて、同一時刻の前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向に対して直交する左右方向の加速度と、前記法線加速度とが最も整合するときの前記ヨー方向の角度を前記ヨー方向取付角として算出することを特徴とする、請求項４に記載の測位装置。
【請求項６】
前記装置筐体取付角算出部は、前記３軸加速度のうちの前記移動体の前後方向に対して直交する左右方向の加速度と前記法線加速度とが最も整合するときの前記ヨー方向の角度と、前記３軸加速度のうちの前記移動体の前後方向の加速度と前記距離計側部で算出された前記前後加速度とが最も整合するときの前記ヨー方向の角度とが同一である場合において、前記ヨー方向の角度を前記ヨー方向取付角として算出することを特徴とする、請求項５に記載の測位装置。
【請求項７】
前記装置筐体取付角算出部は、前記角度範囲および当該角度範囲内における前記ヨー方向の角度の分解能を設定可能であることを特徴とする、請求項６に記載の測位装置。
【請求項８】
前記装置筐体取付角算出部は、前記ピッチ方向取付角、前記ロール方向取付角、および予め定められたヨー方向の角度に基づいて、前記３軸角速度センサで検出された前記３軸角速度および前記３軸加速度センサで検出された前記３軸加速度をそれぞれ座標変換し、
当該座標変換後の前記３軸角速度および前記３軸加速度のそれぞれに基づいて、ピッチ角、ロール角、およびヨー角を含む姿勢角を算出し、当該算出後の前記３軸角速度に基づく前記姿勢角と、前記３軸加速度に基づく前記姿勢角とのうちのいずれかの姿勢角が整合するときの前記ヨー角を前記測位装置のヨー方向取付角としてさらに算出し、
前記３軸角速度座標変換部は、前記ピッチ方向取付角、前記ロール方向取付角、および前記ヨー方向取付角に基づいて、前記３軸角速度を座標変換することを特徴とする、請求項１に記載の測位装置。
【請求項９】
前記装置筐体取付角算出部は、予め定められた角度範囲内における複数の前記ヨー方向の角度と、前記ピッチ方向取付角と、前記ロール方向取付角との全ての組み合わせについて、同一時刻の前記３軸加速度センサで検出された前記３軸加速度および前記３軸角速度センサで検出された前記３軸角速度をそれぞれ座標変換し、当該座標変換後の前記３軸角速度に基づいて前記姿勢角を算出するとともに、前記座標変換後の前記３軸加速度に基づいて前記ピッチ角、前記ロール角、および前記ヨー角のうちの少なくとも１つの前記姿勢角を算出し、当該算出後の前記３軸角速度に基づく前記姿勢角と、前記３軸加速度に基づく前記姿勢角とが最も整合するときの前記ヨー角を前記測位装置のヨー方向取付角として算出することを特徴とする、請求項８に記載の測位装置。
【請求項１０】
前記装置筐体取付角算出部は、前記３軸加速度のうちの前記移動体の前後方向に対して直交する左右方向の加速度と前記前後方向と直交する法線方向の加速度である法線加速度とが最も整合するときの前記ヨー方向の角度と、前記３軸加速度のうちの前記移動体の前後方向の加速度と前記距離計側部で算出された前記前後加速度とが最も整合するときの前記ヨー方向の角度と、前記３軸角速度に基づく前記ピッチ角、前記ロール角、または前記ヨー角と前記３軸加速度に基づく前記ピッチ角、前記ロール角、または前記ヨー角とのいずれかが最も整合するときの前記ヨー角と、のうちの少なくとも２つ以上が同一である場合において、前記ヨー方向の角度または前記ヨー角を前記ヨー方向取付角として算出することを特徴とする、請求項９に記載の測位装置。
【請求項１１】
前記装置筐体取付角算出部は、前記角度範囲および当該角度範囲内における前記ヨー方向の角度の分解能を設定可能であることを特徴とする、請求項１０に記載の測位装置。
【請求項１２】
前記位置推定部は、前記移動距離および前記姿勢角に基づいて前記移動体の現在位置を推定し、前記測位結果、前記移動距離、および前記姿勢角のそれぞれの予測誤差に応じて、前記測位結果に基づいて前記移動体の現在位置を修正することを特徴とする、請求項８に記載の測位装置。
【請求項１３】
前記位置推定部は、前記３軸加速度に基づく前記姿勢角と、前記３軸角速度に基づく前記姿勢角との差異に基づいて、前記姿勢角の前記予測誤差を算出することを特徴とする、請求項１２に記載の測位装置。
【請求項１４】
移動体の測位装置で用いられる測位方法であって、
ＧＰＳ衛星から電波を受信し、少なくとも前記移動体の現在位置と、前記測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力し、
前記移動体の移動に応じたパルス信号を出力し、
前記測位装置において互いに直交する３軸の角速度である３軸角速度を検出し、
前記測位装置において互いに直交する３軸の加速度である３軸加速度を検出し、
一定周期ごとに出力された前記パルス信号に基づいて、前記移動体の移動距離、速度、および前後加速度を算出し、
前記移動体が走行中である場合において、少なくとも前記測位結果として算出された前記３軸速度、前記算出された前記前後加速度、および前記検出された前記３軸加速度に基づいて、前記移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、前記水平面等速直進走行中であると判断した時の前記３軸加速度を零点として算出し、
前記算出された前記３軸加速度の零点に基づいて、前記移動体に取り付けられた前記測位装置のピッチ方向取付角およびロール方向取付角を算出し、
前記算出された前記ピッチ方向取付角および前記ロール方向取付角に基づいて、前記検出された前記３軸角速度を座標変換し、
前記座標変換された前記３軸角速度から求めた少なくともヨーレートに基づいて、前記移動体の現在位置を推定する、測位方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の測位装置および測位方法に関する。
【背景技術】
【０００２】
車両（移動体）の測位装置は、ＧＮＳＳ（Global Navigation Satellite System）受信機および各種センサを用いて、車両の位置および姿勢角等を推定するものである。測位装置において、検知軸がピッチ方向またはヨー方向に傾斜した角速度センサおよび加速度センサを用いる際には、角速度センサおよび加速度センサの各特性を考慮する必要がある。また、測位装置の筐体が予め定められた取付角で傾斜して車体に設置されている場合は、測位装置の取付角を考慮する必要がある。【０００３】
従来、測位装置の取付角を算出する技術が開示されている（例えば、特許文献１〜３参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第３３７５２６８号公報
【特許文献２】特許第２８４３９０４号公報
【特許文献３】特開２０１１−２０９１６２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
従来では、測位装置がどのような取付角で設置されても自車位置を推定することができるとはいえなかった。【０００６】
本発明は、このような問題を解決するためになされたものであり、どのような取付角で設置されても自車位置を推定することが可能な測位装置および測位方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記の課題を解決するために、本発明による測位装置は、移動体の測位装置であって、ＧＰＳ衛星から電波を受信し、少なくとも移動体の現在位置と、測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力するＧＮＳＳ受信機と、移動体の移動に応じたパルス信号を出力する速度センサと、測位装置において互いに直交する３軸の角速度である３軸角速度を検出する３軸角速度センサと、測位装置において互いに直交する３軸の加速度である３軸加速度を検出する３軸加速度センサと、一定周期ごとに速度センサから出力されたパルス信号に基づいて、移動体の移動距離、速度、および前後加速度を算出する距離計側部と、移動体が走行中である場合において、少なくともＧＮＳＳ受信機で算出された３軸速度、距離計側部で算出された前後加速度、および３軸加速度センサで検出された３軸加速度に基づいて、移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、水平面等速直進走行中であると判断した時に３軸加速度センサで検出された３軸加速度を零点として算出する３軸加速度センサ零点算出部と、３軸加速度センサ零点算出部で算出された３軸加速度の零点に基づいて、移動体に取り付けられた測位装置のピッチ方向取付角およびロール方向取付角を算出する装置筐体取付角算出部と、装置筐体取付角算出部で算出されたピッチ方向取付角およびロール方向取付角に基づいて、３軸角速度センサで検出された３軸角速度を座標変換する３軸角速度座標変換部と、３軸角速度座標変換部で変換された３軸角速度から求めた少なくともヨーレートに基づいて、移動体の現在位置を推定する位置推定部とを備える。【０００８】
また、本発明による測位方法は、移動体の測位装置で用いられる測位方法であって、ＧＰＳ衛星から電波を受信し、少なくとも移動体の現在位置と、測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力し、移動体の移動に応じたパルス信号を出力し、測位装置において互いに直交する３軸の角速度である３軸角速度を検出し、測位装置において互いに直交する３軸の加速度である３軸加速度を検出し、一定周期ごとに出力されたパルス信号に基づいて、移動体の移動距離、速度、および前後加速度を算出し、移動体が走行中である場合において、少なくとも測位結果として算出された３軸速度、算出された前後加速度、および検出された３軸加速度に基づいて、移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、水平面等速直進走行中であると判断した時の３軸加速度を零点として算出し、算出された３軸加速度の零点に基づいて、移動体に取り付けられた測位装置のピッチ方向取付角およびロール方向取付角を算出し、算出されたピッチ方向取付角およびロール方向取付角に基づいて、検出された３軸角速度を座標変換し、座標変換された３軸角速度から求めた少なくともヨーレートに基づいて、移動体の現在位置を推定する。
【発明の効果】
【０００９】
本発明によると、測位装置は、移動体の測位装置であって、ＧＰＳ衛星から電波を受信し、少なくとも移動体の現在位置と、測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力するＧＮＳＳ受信機と、移動体の移動に応じたパルス信号を出力する速度センサと、測位装置において互いに直交する３軸の角速度である３軸角速度を検出する３軸角速度センサと、測位装置において互いに直交する３軸の加速度である３軸加速度を検出する３軸加速度センサと、一定周期ごとに速度センサから出力されたパルス信号に基づいて、移動体の移動距離、速度、および前後加速度を算出する距離計側部と、移動体が走行中である場合において、少なくともＧＮＳＳ受信機で算出された３軸速度、距離計側部で算出された前後加速度、および３軸加速度センサで検出された３軸加速度に基づいて、移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、水平面等速直進走行中であると判断した時に３軸加速度センサで検出された３軸加速度を零点として算出する３軸加速度センサ零点算出部と、３軸加速度センサ零点算出部で算出された３軸加速度の零点に基づいて、移動体に取り付けられた測位装置のピッチ方向取付角およびロール方向取付角を算出する装置筐体取付角算出部と、装置筐体取付角算出部で算出されたピッチ方向取付角およびロール方向取付角に基づいて、３軸角速度センサで検出された３軸角速度を座標変換する３軸角速度座標変換部と、３軸角速度座標変換部で変換された３軸角速度から求めた少なくともヨーレートに基づいて、移動体の現在位置を推定する位置推定部とを備えるため、どのような取付角で設置されても自車位置を推定することが可能となる。【００１０】
また、測位方法は、移動体の測位装置で用いられる測位方法であって、ＧＰＳ衛星から電波を受信し、少なくとも移動体の現在位置と、測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力し、移動体の移動に応じたパルス信号を出力し、測位装置において互いに直交する３軸の角速度である３軸角速度を検出し、測位装置において互いに直交する３軸の加速度である３軸加速度を検出し、一定周期ごとに出力されたパルス信号に基づいて、移動体の移動距離、速度、および前後加速度を算出し、移動体が走行中である場合において、少なくとも測位結果として算出された３軸速度、算出された前後加速度、および検出された３軸加速度に基づいて、移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、水平面等速直進走行中であると判断した時の３軸加速度を零点として算出し、算出された３軸加速度の零点に基づいて、移動体に取り付けられた測位装置のピッチ方向取付角およびロール方向取付角を算出し、算出されたピッチ方向取付角およびロール方向取付角に基づいて、検出された３軸角速度を座標変換し、座標変換された３軸角速度から求めた少なくともヨーレートに基づいて、移動体の現在位置を推定するため、どのような取付角で設置されても自車位置を推定することが可能となる。【００１１】
本発明の目的、特徴、態様、および利点は、以下の詳細な説明と添付図面とによって、より明白となる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施の形態１による測位装置の構成の一例を示すブロック図である。
【図２】本発明の実施の形態１による測位装置のハードウェア構成の一例を示すブロック図である。
【図３】本発明の実施の形態１による測位装置の動作の一例を示すフローチャートである。
【図４】本発明の実施の形態１によるピッチ方向取付角およびロール方向取付角に基づくセンサ座標系から車体座標系への変換状態を説明するための図である。
【図５】本発明の実施の形態２による装置筐体取付角算出部の動作の一例を示すフローチャートである。
【図６】本発明の実施の形態２によるヨー方向の任意角度に基づくセンサ座標系から仮の車体座標系への変換状態を説明するための図である。
【図７】本発明の実施の形態３による装置筐体取付角算出部の動作の一例を示すフローチャートである。
【図８】本発明の実施の形態４による装置筐体取付角算出部の動作の一例を示すフローチャートである。
【図９】本発明の実施の形態４による３軸角速度センサの計測値と３軸加速度センサによる算出値との関係を示す図である。
【図１０】本発明の実施の形態４による３軸角速度センサの計測値と３軸加速度センサによる算出値との関係を示す図である。
【図１１】本発明の実施の形態４による３軸角速度センサの計測値と３軸加速度センサによる算出値との関係を示す図である。
【図１２】車体座標系の定義を説明するための図である。
【図１３】センサ座標系の定義を説明するための図である。
【図１４】ピッチ方向取付角の算出の課題を説明するための図である。
【図１５】ピッチ方向取付角の算出の課題を説明するための図である。
【図１６】ピッチ方向取付角の算出の課題を説明するための図である。
【図１７】ピッチ方向取付角の算出の課題を説明するための図である。
【図１８】ロール方向取付角の算出の課題を説明するための図である。
【図１９】ロール方向取付角の算出の課題を説明するための図である。
【図２０】ロール方向取付角の算出の課題を説明するための図である。
【図２１】ロール方向取付角の算出の課題を説明するための図である。
【図２２】ヨー方向取付角の算出の課題を説明するための図である。
【図２３】ヨー方向取付角の算出の課題を説明するための図である。
【図２４】本発明の実施の形態による測位システムの構成の一例を示すブロック図である。
【発明を実施するための形態】
【００１３】
本発明の実施の形態について、図面に基づいて以下に説明する。
【００１４】
なお、本実施の形態では、図１２に示す車体座標系、および図１３に示すセンサ座標系を用いて説明する。
【００１５】
＜初めに＞
本発明の実施の形態を説明する前に、測位装置の取付角が傾斜している場合について詳細に説明する。
【００１６】
測位装置において、検知軸がピッチ方向またはヨー方向に傾斜した角速度センサおよび加速度センサを用いる際には、以下の角速度センサおよび加速度センサの各特性を考慮する必要がある。
【００１７】
（１）角速度センサ（または加速度センサ）は、検知軸に作用する角速度（または加速度）を計測するが、検知軸が傾斜すると当該検知軸に作用する角速度（または加速度）の感度が傾斜角度に応じて低下し、逆に傾斜方向に存在する他軸の感度が大きくなる。すなわち、角速度センサ（または加速度センサ）の取付角に応じて各軸に配分された角速度（または加速度）が検出される。
【００１８】
（２）３軸の角速度センサ（以下、３軸角速度センサという）または３軸の加速度センサ（以下、３軸加速度センサという）を使用する場合は、オイラー角または四元数（クォータニオン）の公式を用いることによって、任意の取付角だけ３軸角速度センサまたは３軸加速度センサを回転したときのセンサ出力に変換することができる。しかし、２軸または１軸の角速度センサまたは加速度センサを使用する場合は、備えていない検知軸で計測すべき角速度または加速度を計測することができないため、限定条件付きでなければ任意の取付角におけるセンサ出力に変換（復元）することができない。
【００１９】
（３）角速度センサについては、停車中では角速度が発生しないため、停車中のセンサ出力に基づいて零点を求めることができる。
【００２０】
（４）加速度センサについては、加速度センサが傾斜すると傾斜角度に応じた重力の影響を受けるため、停車中のセンサ出力に基づいて加速度センサの傾斜角度を推定することができる。しかし、加速度センサの傾斜が、道路の傾斜によるものなのか、測位装置の筐体の取付角によるものなのか、またはセンサ出力の変動が温度ドリフトによるものなのかを区別することができないため、停車中のセンサ出力に基づいて零点を正確に求めることができない。
【００２１】
車両の位置および姿勢角等を表現するためには、直交３軸座標系を決める必要がある。直交３軸座標系におけるＸ軸、Ｙ軸、およびＺ軸の定義は複数提案されている。本発明では、図１２に示すように、車両の前後方向をＸＢ軸、車両の前後方向に対して直交する左右方向（法線方向）をＹＢ軸、車両のＸ−Ｙ平面に対して上下方向に直交する方向をＺＢ軸方向とする座標系を車体座標系と定義する。なお、各軸における添え字の「Ｂ」は、「Body Frame」を意味している。また、ＡＢｘ，ＡＢｙ、ＡＢｚは各軸方向の加速度を示しており、ωＢｒｏｌｌ，ωＢｐｉｔｃｈ，ωＢｙａｗは各軸方向の角速度を示している。
【００２２】
また、図１３に示すように、測位装置の筐体が予め定められた取付角で傾斜して車体に設置されている場合において、センサ検知軸（ＸＳ軸，ＹＳ軸，ＺＳ軸）のそれぞれは、車体座標系のＸＢ軸，ＹＢ軸，ＺＢ軸に対して、ロール方向の取付角であるロール方向取付角φｒｏｌｌ、ピッチ方向の取付角であるピッチ方向取付角φｐｉｔｃｈ、ヨー方向の取付角であるヨー方向取付角φｙａｗで傾斜する。本発明では、センサ検知軸（ＸＳ軸，ＹＳ軸，ＺＳ軸）の座標系をセンサ座標系と定義し、車体座標系と区別する。
【００２３】
ロール方向取付角、ピッチ方向取付角、およびロール方向取付角が分かれば、式（１）によって、車体座標系からセンサ座標系に座標変換するオイラーの座標変換行列ＣＢＳを求めることができる。また、式（１）で求めた座標変換行列ＣＢＳを式（２）に適用することによって、車体座標系の計測量ＭＢをセンサ座標系の計測量ＭＳに座標変換することができる。ここで、計測量とは、計算目的に応じた姿勢角または加速度等である。
【数１】
2017064790.tif 000003
【数２】
2017064790.tif 000004
【００２４】
同様に、ロール方向取付角、ピッチ方向取付角、およびロール方向取付角が分かれば、式（３）によって、センサ座標系から車体座標系に座標変換するオイラーの座標変換行列ＣＳＢを求めることができる。また、式（３）で求めた座標変換行列ＣＳＢを式（４）に適用することによって、センサ座標系の計測量ＭＳを車体座標系の計測量ＭＢに座標変換することができる。
【数３】
2017064790.tif 000005
【数４】
2017064790.tif 000006
【００２５】
なお、上記のオイラーの座標変換に代えて、式（５）〜式（９）に示す四元数を用いた座標変換行列を用いる場合もある。
【数５】
2017064790.tif 000007
【００２６】
従来、上述の特許文献１〜３では、上記の式（１）〜式（４）の座標変換式をそれぞれ用いることによって、測位装置の取付角を算出する技術が開示されている。
【００２７】
特許文献１では、図１２に示す車体座標系のＸ軸を中心として９０度回転させた車体座標系となっている。また、前後方向（前が正極性）、上下方向（下が正極性）、左右方向（左が正極性）のそれぞれが、車体座標系のＸ軸、Ｙ軸、Ｚ軸に対応している（特許文献１：図３）。特許文献１では、車両の車速信号を測位装置に接続せずに自車位置を推定することを目的としており、２軸以上の加速度センサを用いて車両の前後加速度を検出することができるように、測位装置を車体に取り付けている。以下、特許文献１における加速度センサの出力の零点、および測位装置の取付角の算出について簡単に説明する。
【００２８】
まず、零点の算出方法について説明する。
【００２９】
３軸加速度センサの出力信号の波形が予め定められたレベル以上変動したか否かに基づいて、車両が走行中であるのか、または停車中であるのかを判断する（特許文献１：図３）。停車中であると判断した場合は、加速度センサの各軸の出力を当該各軸の零点として記憶し、以降、加速度センサの各軸の出力から零点を差し引いて、各軸の加速度（Ａｘ，Ａｙ，Ａｚ）［ｍ／ｓ２］を得る（特許文献１：段落［００２０］〜［００２７］）。
【００３０】
次に、ピッチ方向取付角の算出方法について説明する。
【００３１】
零点の検出後に水平面で停車（Ａ＝０）したときに、式（１０）によってピッチ方向取付角θｓｚを算出する（特許文献１：段落［００３６］〜［００３８］）。
【数６】
2017064790.tif 000008
【００３２】
また、傾斜（θｒｚ）している道路で停車（Ａ＝０）したときにピッチ方向取付角θｓｚが０であれば、式（１１）によって道路傾斜角（θｒｚ）を算出する（特許文献１：段落［００３３］〜［００３５］）。
【数７】
2017064790.tif 000009
【００３３】
なお、加速度センサがピッチ方向取付角θｓｚで車体に取り付けられているときに傾斜（θｒｚ）している道路を走行した場合は、ピッチ方向取付角θｓｚを直接求めることができないため、停車中（Ａ＝０）に地図メモリに記憶された道路傾斜角θｒｚを用いて、式（１２）によってピッチ方向取付角θｓｚを算出する（特許文献１：段落［００４１］〜［００４５］，［００８８］）。
【数８】
2017064790.tif 000010
【００３４】
次に、ロール方向取付角の算出方法について説明する。
【００３５】
ピッチ方向取付角θｓｚと同様に、地図メモリに記憶された道路傾斜角θｒｘを用いて、停車中に式（１３）によってロール方向取付角θｓｘを算出する。
【数９】
2017064790.tif 000011
【００３６】
次に、ヨー方向取付角の算出方法について説明する。
【００３７】
直進走行中に式（１４）および式（１５）の連立方程式を解くことによって、前後加速度Ａおよびヨー方向取付角θｓｙを同時に算出する。
【数１０】
2017064790.tif 000012
【００３８】
また、直進走行中の加速度センサの出力Ａｘを積分した速度ＶｘとＧＰＳ速度ＶＧＰＳとの関係から、式（１６）および式（１７）によってヨー方向取付角θｙを算出する（特許文献１：段落［００５１］〜［００５７］）。
【数１１】
2017064790.tif 000013
【００３９】
また、特許文献２では、図１２に示す車体座標系と同じ車体座標系となっている（特許文献２：図２）。特許文献２では、オドメータ（車輪回転速度）および２軸角速度センサ（ヨーレート、ピッチレート）による第１の自律測位演算部と、３軸加速度センサによる第２の自律測位演算部とを備えている（特許文献２：図１）。第１の自律測位演算部および第２の自律測位演算部のそれぞれにおける自律測位結果の差異から、オドメータのスケールファクタ誤差および測位装置の取付角誤差を求めている。第１の自律測位演算部および第２の自律測位演算部では、これらの誤差を補正している。以下、特許文献２における取付角誤差の算出方法について簡単に説明する。
【００４０】
第１の自律測位演算部は、式（１８）によってオドメータ（車輪回転速度ｎ）から速度Ｖ１および移動距離Ｌ１を算出し、２軸角速度（ヨーレートωｙ、ピッチレートωｐ）を積分して方位角θｙおよびピッチ角θｐを算出する。
【数１２】
2017064790.tif 000014
【００４１】
また、第１の自律測位演算部は、速度Ｖ１、方位角θｙ、およびピッチ角θｐを用いて、式（１９）〜（２２）によって各種移動距離を算出する。
【数１３】
2017064790.tif 000015
【００４２】
第２の自律測位演算部は、式（２３）によって３軸加速度センサで計測した加速度（ＡＳｘ，ＡＳｙ，ＡＳｚ）から速度Ｖ２および移動距離Ｌ２を算出する。
【数１４】
2017064790.tif 000016
【００４３】
また、第２の自律測位演算部は、速度Ｖ２、方位角θｙ、およびピッチ角θｐを用いて、式（２４）〜（２８）によって各種移動距離を算出する。
【数１５】
2017064790.tif 000017
【００４４】
第１の自律測位演算部および第２の自律測位演算部は、各種移動距離に基づいて、式（２９）〜（３１）によってスケールファクタ誤差δＳＦ、ピッチ方向取付角誤差δθｐ、およびヨー方向取付角誤差δθｙを算出し、式（３２）〜（３４）によってスケールファクタＳＦ、ピッチ方向取付角θｐ、およびヨー方向取付角θｙを補正する。
【数１６】
2017064790.tif 000018
【数１７】
2017064790.tif 000019
【００４５】
特許文献３では、図１２に示す車体座標系のＺ軸の極性のみが異なる車体座標系となっている。また、前後方向（前が正極性）、左右方向（右が正極性）、上下方向（上が正極性）のそれぞれが、Ｘ軸、Ｙ軸、Ｚ軸に対応している（特許文献３：図２）。特許文献３では、特許文献１と同様、車両の車速信号を測位装置に接続せずに自車位置を推定することを目的としており、装置の取り外しおよび設置が頻繁に繰り返されて取付角が毎回異なる状況を想定している。特許文献３では、１軸角速度センサ、３軸加速度センサ、およびＧＰＳ受信機を備えている。以下、特許文献３における加速度センサの出力の零点、および装置の取付角の算出について簡単に説明する。
【００４６】
まず、零点の算出方法について説明する。
【００４７】
一定時間（例えば、３秒）における３軸加速度センサの出力が、任意のばらつき範囲に収まる場合は停車中であると判断し、任意のばらつき範囲に収まらない場合は走行中であると判断する。停車中であると判断した場合は、加速度センサの各軸の出力の平均値を求め、異なる地点で停車した時に再び平均値を求めるまでその平均値を零点（ＡＳｘｂｉａｓ，ＡＳｙｂｉａｓ，ＡＳｚｂｉａｓ）として保持する。また、温度、道路傾斜によって零点誤差が変化する（特許文献３：段落［００４８］〜［００５８］）。
【００４８】
次に、ピッチ方向取付角の算出方法について説明する。
【００４９】
加速度センサは、温度ドリフトによる零点の変動および重力の影響を受けるため、発車してから１０ｍ以内に限定してピッチ方向取付角を算出する。ヨー方向取付角およびロール方向取付角が小さい場合は、式（３５）〜（３７）によってピッチ方向取付角θｐを簡易的に算出する（特許文献３：段落［００６１］〜［００６５］）。なお、Ａは、車両の前後加速度を示している。
【数１８】
2017064790.tif 000020
【００５０】
次に、ヨー方向取付角の算出方法について説明する。
【００５１】
ピッチ方向取付角の算出と同じタイミングで、ヨー方向取付角を算出する。ピッチ方向取付角およびロール方向取付角が小さい場合は、式（３８）〜（４０）によってヨー方向取付角θｙを簡易的に算出する（特許文献３：段落［００６５］）。
【数１９】
2017064790.tif 000021
【００５２】
次に、ロール方向取付角の算出方法について説明する。
【００５３】
ＧＰＳ速度と１軸角速度センサによるヨーレートとに基づいて遠心力を計算し、遠心力が一定以上であればロール方向取付角を算出する。ヨー方向取付角およびピッチ方向取付角が小さい場合は、式（４１）〜（４３）によってロール方向取付角θｒを簡易的に算出する（特許文献３：段落［００６６］〜［００７４］）。
【数２０】
2017064790.tif 000022
【００５４】
次に、取付角の補正について説明する。
【００５５】
２つ以上の取付角が閾値（例えば、５度）以上であれば、閾値以上の取付角の組み合わせによって取付角を補正する。例えば、ロール方向取付角が０であり、ヨー方向取付角およびピッチ方向取付角が閾値以上である場合は、直進走行時において式（４４）〜（４７）によってヨー方向取付角を補正する（特許文献３：段落［００６６］〜［００８１］）。
【数２１】
2017064790.tif 000023
【００５６】
＜課題１＞
従来では、加速度センサが重力の影響を検知するため、停車時の加速度センサの各軸の出力に基づいて、加速度センサがピッチ方向に傾斜した角度を推定することができる。しかし、加速度センサが傾斜した理由が、道路の傾斜によるものなのか、装置の取付角によるものなのか、あるいは温度ドリフトによるセンサの出力の変動によるものなのかを区別して判断することが難しかった。換言すれば、車両が水平面上に存在することが分かれば、図１４および図１６に示すいずれの場合であっても、停車時および水平面等速走行時における加速度センサの出力から零点を求め、当該零点からピッチ方向取付角を求めることができる（例えば、図１７参照）。しかし、車両が水平面上に存在することが分からない場合は、図１５に示すように車両が傾斜面上に存在しているかもしれないため、加速度センサの出力から零点を求めることができなかった。従って、従来では、車両が水平面上に存在しているのか否かを判断することが課題であった。また、図１８〜２１に示すように、ロール方向についても上記のピッチ方向と同様の課題があった。
【００５７】
上記の課題に対して、特許文献１および特許文献３における加速度センサの零点の算出には、次の問題があった。すなわち、特に有効な条件を付けることなく停車中の加速度センサの出力から零点を求めているため、上記の課題を解消できているとはいえない。また、温度ドリフトによって走行中に零点が変動することがあるが、当該温度ドリフトに対応しているとはいえない。
【００５８】
また、上記の課題に対して、特許文献１におけるピッチ方向およびロール方向の取付角の算出には、以下の問題があった。
【００５９】
（１）零点を算出した後の停車時において、車両が水平面上に停車した時に加速度センサの出力から取付角を求めている。しかし、温度ドリフトによって零点が変動した場合において、零点の変動と取付角とを区別して算出する方法について開示されてない。
【００６０】
（２）車両が水平面上に存在しているのか否かを正しく判断する方法が開示されていないため、取付角の算出精度が高いとはいえない。
【００６１】
（３）地図データに含まれる道路傾斜角または道路バンク角を用いて取付角を算出している。しかし、正しくマップマッチングできていないとき、あるいは道路外または道路傾斜角および道路バンク角が道路データに含まれていない道路では、取付角を算出することができない。
【００６２】
（４）道路傾斜角および道路バンク角は、データ化した区間の平均的な角度であり、停車地点の角度とは微妙に異なるため、取付角の検出精度が高いとはいえない。
【００６３】
また、上記の課題に対して、特許文献２におけるピッチ方向およびロール方向の取付角の算出には、以下の問題があった。
【００６４】
（１）オドメータおよび２軸角速度センサによる第１の自律測位演算部と、３軸加速度センサによる第２の自律測位演算部とにおける各測位結果（各種移動距離）の差異に基づいてピッチ方向取付角を求めている。しかし、加速度を２回積分した移動距離を基準にすると、積分誤差によって取付角の誤検出のリスクが高まる。
【００６５】
（２）角速度センサおよび加速度センサの使用に際して、温度ドリフトを補正する有効な仕組みが開示されていないため、時間経過に従って取付角の算出精度が低下するリスクが高まる。
【００６６】
（３）積分誤差が小さい高精度な加速度センサ、温度ドリフトが小さい角速度センサまたは加速度センサを使用する場合は、コストがかかる。また、ロール方向取付角を算出することができない。
【００６７】
また、上記の課題に対して、特許文献３におけるピッチ方向およびロール方向の取付角の算出には、以下の問題があった。
【００６８】
（１）発車後、一定距離を走行するまでの間にピッチ方向取付角を算出し、予め定められた閾値以上の遠心力が生じる旋回中にロール方向取付角を算出することが開示されている。しかし、このような算出条件では、明確な右左折がないまま走行している間は、取付角を正しく算出することができないと考えられる。
【００６９】
（２）取付角の算出に用いる計算式が簡易的なものであるため、算出精度が高いとはいえない。また、ピッチ方向およびロール方向ともに大きく傾斜して取付けられた場合の対応について開示されていない。
【００７０】
＜課題２＞
ピッチ方向およびロール方向の取付角の算出後、水平面走行時では、図２２および図２３のいずれの状態であってもヨーレートを同様に計測できるため、ヨーレートからヨー方向取付角を算出することができない。
【００７１】
このような課題に対して、特許文献１，２，３におけるヨー方向取付角の算出には、以下の問題があった。
【００７２】
（１）上記の課題１で述べた通り、加速度センサの零点と、ピッチ方向およびロール方向の取付角との算出精度が十分であるとはいえない。また、水平面上の走行を正しく判断することができない。従って、このような誤差を含んだ状態でヨー方向取付角を算出すると、当該ヨー方向取付角にも誤差が生じる。
【００７３】
（２）特許文献１では、ＧＰＳ速度を用いる方法も開示されているが、ＧＰＳ速度の精度が低下した場合の対応について開示されていないため、ＧＰＳ速度の誤差に連動して、ヨー方向取付角に誤差が生じるという問題があった。
【００７４】
（３）特許文献２では、上記の課題１で述べた問題と同じ問題があった。
【００７５】
（４）特許文献１，２，３では、加速度センサの出力からヨー方向取付角を算出することが開示されているが、当該算出結果を他のセンサの出力で検証していないため、算出精度の低下または誤算出を抑制することができないという問題があった。
【００７６】
＜実施の形態１＞
まず、本発明の実施の形態１による測位装置の構成について説明する。なお、測位装置は、移動体である車両（以下、自車ともいう）に設置されているものとする。
【００７７】
図１は、測位装置の構成の一例を示すブロック図である。
【００７８】
図１に示すように、測位装置は、ＧＮＳＳ受信機１と、速度センサ２と、３軸加速度センサ３と、３軸角速度センサ４と、制御部５とを備えている。
【００７９】
ＧＮＳＳ受信機１は、ＧＰＳ（Global Positioning System）衛星からの電波を受信し、ＧＮＳＳ受信機１を設置した車両の自車位置（現在位置）を測位する。ＧＮＳＳ受信機１は、位置、方位、および３軸速度（前後方向速度、左右方向速度、上下方向速度）等の測位結果を出力するとともに、測位計算に用いた各ＧＰＳ衛星のＲａｗデータ（衛星軌道（エフェメリス）、ＧＰＳ衛星と自車との間の電波伝搬時間を換算した疑似距離、衛星電波のドップラー偏移周波数など）も出力する。
【００８０】
速度センサ２は、車両の移動距離に応じたパルス信号を出力する。３軸加速度センサ３は、予め定められた周期ごとにセンサ座標系における３軸の加速度である３軸加速度（Ｘ軸加速度、Ｙ軸加速度、Ｚ軸加速度）を出力する。３軸角速度センサ４は、予め定められた周期ごとにセンサ座標系における３軸の角速度である３軸角速度（ロールレート、ピッチレート、ヨーレート）を出力する。
【００８１】
制御部５は、距離計側部５１と、３軸加速度センサ零点算出部５２と、３軸加速度計算部５３と、３軸角速度センサ零点算出部５４と、３軸角速度計算部５５と、装置筐体取付角算出部５６と、３軸加速度座標変換部５７と、３軸角速度座標変換部５８と、自車位置推定部５９とを備えている。
【００８２】
制御部５は、ＧＮＳＳ受信機１、速度センサ２、３軸加速度センサ３、および３軸角速度センサ４の出力に基づいて、自車の位置、方位、および速度等（以下、自車位置情報ともいう）を算出する。制御部５は、車体座標系において測位装置が傾斜して車体に設置された場合であっても、３軸の取付角（Ｘ軸回りのロール方向取付角、Ｙ軸回りのピッチ方向取付角、Ｚ軸回りのヨー方向取付角）を算出し、当該算出した取付角に基づいて、センサ座標系の角速度および加速度を車体座標系の角速度および加速度に変換して、自車位置情報の算出に用いる。なお、ＧＮＳＳ受信機１は、ＧＮＳＳアンテナで衛星電波を受信して得たＲａｗデータに基づいて自車位置情報を算出するが、このとき算出される自車位置情報を特にＧＰＳ測位結果ともいう。
【００８３】
距離計側部５１は、予め定められた周期ごとに速度センサ２で計測されたパルス信号に基づいて、車両の時々刻々の移動距離、速度、および前後加速度を算出する。
【００８４】
３軸加速度センサ零点算出部５２は、センサ座標系において、車両が水平面上を等速で直進走行（水平面等速直進走行）しているときの、３軸加速度センサ３における各軸の出力を零点として算出する。３軸加速度計算部５３は、３軸加速度センサ３の出力から、３軸加速度センサ零点算出部５２で算出された零点を差し引く。
【００８５】
３軸角速度センサ零点算出部５４は、センサ座標系において、角速度が生じていない停車時の、３軸角速度センサ４における各軸の出力を零点として算出する。３軸角速度計算部５５は、３軸角速度センサ４の出力から、３軸角速度センサ零点算出部５４で算出された零点を差し引く。
【００８６】
装置筐体取付角算出部５６は、３軸の取付角（ロール方向取付角、ピッチ方向取付角、ヨー方向取付角）を算出する。３軸加速度座標変換部５７は、装置筐体取付角算出部５６で算出された３軸の取付角に基づいて、３軸加速度計算部５３の出力を車体座標系に座標変換する。３軸角速度座標変換部５８は、装置筐体取付角算出部５６で算出された３軸の取付角に基づいて、３軸角速度計算部５５の出力を車体座標系に座標変換する。自車位置推定部５９は、予め定められた時間ごとに自車の位置、方位、姿勢角、および速度等を推定する。
【００８７】
図２は、測位装置のハードウェア構成の一例を示すブロック図である。
【００８８】
測位装置における距離計側部５１、３軸加速度センサ零点算出部５２、３軸加速度計算部５３、３軸角速度センサ零点算出部５４、３軸角速度計算部５５、装置筐体取付角算出部５６、３軸加速度座標変換部５７、３軸角速度座標変換部５８、および自車位置推定部５９の各機能は、処理回路により実現される。すなわち、測位装置は、車両の時々刻々の移動距離、速度、および前後加速度を算出し、車両が水平面等速直進走行しているときの３軸加速度センサ３における各軸の出力を零点として算出し、３軸加速度センサ３の出力から３軸加速度センサ零点算出部５２で算出された零点を差し引き、角速度が生じていない停車時の３軸角速度センサ４における各軸の出力を零点として算出し、３軸角速度センサ４の出力から３軸角速度センサ零点算出部５４で算出された零点を差し引き、３軸の取付角を算出し、３軸加速度計算部５３の出力を車体座標系に座標変換し、３軸角速度計算部５５の出力を車体座標系に座標変換し、自車の位置、方位、姿勢角、および速度等を推定するための処理回路を備える。処理回路は、メモリ７に格納されるプログラムを実行するプロセッサ６（中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Digital Signal Processor）ともいう）である。
【００８９】
測位装置における距離計側部５１、３軸加速度センサ零点算出部５２、３軸加速度計算部５３、３軸角速度センサ零点算出部５４、３軸角速度計算部５５、装置筐体取付角算出部５６、３軸加速度座標変換部５７、３軸角速度座標変換部５８、および自車位置推定部５９の各機能は、ソフトウェア、ファームウェア、またはソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェアまたはファームウェアは、プログラムとして記述され、メモリ７に格納される。処理回路は、メモリ７に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、測位装置は、車両の時々刻々の移動距離、速度、および前後加速度を算出するステップ、車両が水平面等速直進走行しているときの３軸加速度センサ３における各軸の出力を零点として算出するステップ、３軸加速度センサ３の出力から３軸加速度センサ零点算出部５２で算出された零点を差し引くステップ、角速度が生じていない停車時の３軸角速度センサ４における各軸の出力を零点として算出するステップ、３軸角速度センサ４の出力から３軸角速度センサ零点算出部５４で算出された零点を差し引くステップ、３軸の取付角を算出するステップ、３軸加速度計算部５３の出力を車体座標系に座標変換するステップ、３軸角速度計算部５５の出力を車体座標系に座標変換するステップ、自車の位置、方位、姿勢角、および速度等を推定するステップが結果的に実行されることになるプログラムを格納するためのメモリ７を備える。また、これらのプログラムは、距離計側部５１、３軸加速度センサ零点算出部５２、３軸加速度計算部５３、３軸角速度センサ零点算出部５４、３軸角速度計算部５５、装置筐体取付角算出部５６、３軸加速度座標変換部５７、３軸角速度座標変換部５８、および自車位置推定部５９の手順または方法をコンピュータに実行させるものであるともいえる。ここで、メモリとは、例えば、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、フラッシュメモリ、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）等の不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital Versatile Disk）等が該当する。
【００９０】
次に、測位装置の動作について説明する。
【００９１】
図３は、測位装置の動作の一例を示すフローチャートである。なお、図３に示す動作は、予め定められた周期ごとに行われるものとする。
【００９２】
ステップＳ１０１において、制御部５は、本周期で初期化が必要か否かの判断を行う。初期化が必要である場合は、ステップＳ１０２に移行する。一方、初期化が必要でない場合は、ステップＳ１０３に移行する。
【００９３】
ステップＳ１０２において、制御部５は、測位処理を初期化する。
【００９４】
ステップＳ１０３において、距離計側部５１は、本周期において速度センサ２が計測したパルス信号数に対して、距離に換算する係数を乗算して移動距離を算出し、移動距離の微分から速度を算出し、速度の微分から前後加速度を算出する。
【００９５】
ステップＳ１０４において、制御部５は、ステップＳ１０３で距離計側部５１が算出した速度に基づいて、車両が停車中であるか否かの判断を行う。停車中である場合は、ステップＳ１０５に移行する。一方、停車中でない（すなわち、走行中である）場合は、ステップＳ１０６に移行する。
【００９６】
ステップＳ１０５において、３軸角速度センサ零点算出部５４は、停車中に３軸角速度センサ４が出力する角速度（ロールレート、ピッチレート、ヨーレート）について、予め定められた閾値以上の角速度が発生していないときの角速度を、３軸角速度センサ４が出力する角速度の零点として算出する。
【００９７】
ステップＳ１０６において、３軸角速度計算部５５は、走行中に３軸角速度センサ４から出力される角速度から、３軸角速度センサ零点算出部５４で算出された角速度の零点を差し引いた角速度に対して時間積分したものを、センサ座標系の姿勢角（ロール角、ピッチ角、ヨー角）として算出する。
【００９８】
ステップＳ１０７において、３軸加速度センサ零点算出部５２は、ピッチ方向取付角およびロール方向取付角が装置筐体取付角算出部５６で算出済みであるか否かの判断を行う。ピッチ方向取付角およびロール方向取付角が装置筐体取付角算出部５６で算出済みである場合は、ステップＳ１１０に移行する。一方、ピッチ方向取付角およびロール方向取付角が装置筐体取付角算出部５６で算出済みでない場合は、ステップＳ１０８に移行する。
【００９９】
ステップＳ１０８において、３軸加速度センサ零点算出部５２は、ステップＳ１０３で距離計側部５１が算出した速度に基づいて、車両が停車中であるか否かの判断を行う。停車中である場合は、ステップＳ１０９に移行する。一方、停車中でない場合は、ステップＳ１１０に移行する。
【０１００】
ステップＳ１０９において、３軸加速度センサ零点算出部５２は、停車中に３軸加速度センサ３が出力する加速度の波形が一定状態にある（予め定められた範囲内に収まっている）ことを確認し、そのときの加速度を、センサ座標系における３軸加速度センサ３が出力する加速度の仮零点として算出する。
【０１０１】
ステップＳ１１０において、３軸加速度センサ零点算出部５２は、ＧＮＳＳ受信機１から測位結果を受けたか否かの判断を行う。測位結果を受けた場合は、ステップＳ１１１に移行する。一方、測位結果を受けていない場合は、ステップＳ１１３に移行する。
【０１０２】
ステップＳ１１１において、３軸加速度センサ零点算出部５２は、ＧＮＳＳ受信機１で算出された測位結果に含まれる３軸速度（前後方向速度、左右方向速度、上下方向速度）、距離計側部５１で算出された前後加速度、および３軸加速度センサ３から出力された各軸の加速度に基づいて、車両が水平面等速直進走行中であるか否かの判断を行う。具体的には、３軸加速度センサ零点算出部５２は、ＧＮＳＳ受信機１で算出された測位結果に含まれる３軸速度（前後方向速度、左右方向速度、上下方向速度）、距離計側部５１で算出された前後加速度、および３軸加速度センサ３から出力された各軸の加速度のそれぞれの変動が予め定められた閾値以下であれば、車両が水平面等速直進走行中であると判断する。車両が水平面等速直進走行中である場合は、ステップＳ１１２に移行する。一方、車両が水平面等速直進走行中でない場合は、ステップＳ１１３に移行する。
【０１０３】
ステップＳ１１２において、３軸加速度センサ零点算出部５２は、３軸加速度センサ３から出力された加速度を、センサ座標系における３軸加速度センサ３が出力する加速度の零点として算出する。
【０１０４】
ステップＳ１１３において、３軸加速度計算部５３は、走行中に３軸加速度センサ３から出力される加速度から、３軸加速度センサ零点算出部５２で算出された加速度の零点（ステップＳ１０９およびステップＳ１１２で算出した零点）を差し引いた加速度を、センサ座標系の加速度として算出する。
【０１０５】
ステップＳ１１４において、装置筐体取付角算出部５６は、測位装置の取付角を算出する。具体的には、装置筐体取付角算出部５６は、ステップＳ１１２で３軸加速度センサ零点算出部５２が零点（ＡＳｘｂｉａｓ，ＡＳｙｂｉａｓ，ＡＳｚｂｉａｓ）を算出している場合は、当該零点を用いて式（４８）および式（４９）によって測位装置の筐体のピッチ方向取付角φｐおよびロール方向取付角φｒを算出する。
【数２２】
2017064790.tif 000024
【０１０６】
また、装置筐体取付角算出部５６は、ステップＳ１１２で零点が算出されておらず、かつステップＳ１０９で３軸加速度センサ零点算出部５２が仮零点（仮ＡＳｘｂｉａｓ，仮ＡＳｙｂｉａｓ，仮ＡＳｚｂｉａｓ）を算出している場合は、当該零点を用いて式（５０）および式（５１）によって測位装置の筐体のピッチ方向仮取付角φｐ―ｔｅｎｔａｔｉｖｅおよびロール方向仮取付角φｒ―ｔｅｎｔａｔｉｖｅを算出する。
【数２３】
2017064790.tif 000025
【０１０７】
ステップＳ１１５において、３軸角速度座標変換部５８は、図４に示すように、ステップＳ１１４で装置筐体取付角算出部５６が算出した取付角（または仮取付角）に基づいて、式（４８）および式（４９）（または式（５０）および式（５１））によってセンサ座標系の姿勢角を車体座標系の姿勢角に座標変換する。
【０１０８】
ステップＳ１１６において、３軸加速度座標変換部５７は、ステップＳ１１４で装置筐体取付角算出部５６が算出した取付角（または仮取付角）に基づいて、式（４８）および式（４９）（または式（５０）および式（５１））によってセンサ座標系の加速度を車体座標系の加速度に座標変換する。当該座標変換によって、以下の事項が可能となる。
【０１０９】
（１）センサ座標系のＺＳ軸と、車体座標系のＺＢ軸とを合わせることができ、車体座標系のヨーレートを復元することができる。
【０１１０】
（２）車体座標系のヨーレートと、距離計側部５１で算出された車体座標系のＸＢ軸の速度とに基づいて法線加速度（左右加速度）を算出することができる。
【０１１１】
（３）前後加速度、左右加速度、重力、およびヨーレートを基準として、ヨー方向取付角を算出することができる（実施の形態２以降で詳述する）。
【０１１２】
ステップＳ１１７において、自車位置推定部５９は、自車位置を更新する。具体的には、自車位置推定部５９は、装置筐体取付角算出部５６で全ての取付角が算出されていない場合は、ステップＳ１０６で３軸角速度計算部５５が算出したセンサ座標系の姿勢角（ロール角、ピッチ角、ヨー角）を合成した角度と、ステップＳ１０３で距離計側部５１が算出した移動距離とに基づいて、自車位置情報を更新する。
【０１１３】
また、自車位置推定部５９は、装置筐体取付角算出部５６でヨー方向取付角のみが算出されていない場合は、移動距離と車体座標系のヨー角とに基づいて、２次元の自車位置情報を更新する。
【０１１４】
また、自車位置推定部５９は、装置筐体取付角算出部５６でヨー方向取付角が算出されている場合は、移動距離と車体座標系の姿勢角とに基づいて、３次元の自車位置情報を更新する。
【０１１５】
ステップＳ１１８において、自車位置推定部５９は、ＧＮＳＳ受信機１から測位結果を受けたか否かの判断を行う。測位結果を受けた場合は、ステップＳ１１９に移行する。一方、測位結果を受けていない場合は、処理を終了する。
【０１１６】
ステップＳ１１９において、自車位置推定部５９は、移動距離、姿勢角、および測位結果のそれぞれの予測誤差に応じて、自車位置情報を測位結果によって適宜修正する。
【０１１７】
以上のことから、本実施の形態１によれば、ＧＮＳＳ受信機１、速度センサ２、および３軸加速度センサ３といった異なる検出手段からの出力を用いることによって、取付角が算出されていない場合であっても水平面等速直進走行であるか否かの判断を行うことができるため、センサ座標系における３軸加速度センサから出力される加速度の零点を走行中に精度良く算出することができる。これにより、温度ドリフトによって零点が変動しても、駐車場のような道路外であっても、零点を素早く算出（補正）することができる。なお、本願発明の発明者が先に開示した技術（特許第５６０６６５６号公報）を採用すれば、ＧＮＳＳ受信機１で算出されたより信頼性の高い３軸の速度を用いて水平面等速直進走行であるか否かの判断を行うことができる。
【０１１８】
また、３軸加速度センサ３から出力される加速度の零点に基づいて、ピッチ方向取付角およびロール方向取付角を算出し、３軸角速度センサ４から出力される角速度をセンサ座標系から車体座標系に変換することができるため、ピッチ方向およびロール方向の取付角の範囲に制約がない。このように算出したピッチ方向取付角およびロール方向取付角を用いて、センサ座標系から車体座標系に座標変換した姿勢角のうちの特にヨー角は、水平面または緩やかな傾斜面では正確に算出されるため、当該ヨー角を用いて更新した２次元の自車位置も正確となる。
【０１１９】
なお、実施の形態１では、ＧＮＳＳ受信機１は、ＧＰＳ衛星の電波を受信して自車位置を測位するものとして説明したが、これに限るものではない。ＧＮＳＳ受信機１は、ＧＰＳ衛星に限らず、例えば、ＧＬＯＮＡＳＳ（Global Navigation Satellite System）衛星、ＢｅｉＤｏｕ衛星、Ｇａｌｉｌｅｏ衛星、準天頂衛星等の電波も受信して自車位置を測位するようにしてもよい。
【０１２０】
上記では、式（４８）〜（５１）を用いて、３軸加速度センサ３の各軸の出力の零点または仮零点から取付角を算出する場合について説明したが、他の計算式に代えてもよい。
【０１２１】
上記の水平面等速直進走行の判断は、本願発明の発明者が先に開示した技術（特許第５６０６６５６号公報）における１軸加速度センサの零点補正条件を用いてもよい。
【０１２２】
＜実施の形態２＞
本発明の実施の形態２では、装置筐体取付角算出部５６が行う処理に特徴を有している。その他の構成および動作は、実施の形態１と同様であるため、ここでは説明を省略する。
【０１２３】
図５は、本実施の形態２による装置筐体取付角算出部５６の動作の一例を示すフローチャートである。なお、図５に示す動作は、図３のステップＳ１１４における処理に対応している。
【０１２４】
ステップＳ２０１において、装置筐体取付角算出部５６は、３軸加速度センサ零点算出部５２でセンサ座標系における３軸加速度センサ３が出力する加速度の零点が更新（算出）されたか否かの判断を行う。具体的には、装置筐体取付角算出部５６は、図３のステップＳ１１２において、３軸加速度センサ零点算出部５２がセンサ座標系における３軸加速度センサ３が出力する加速度の零点を算出したか否かの判断を行う。零点が更新されている場合は、ステップＳ２０２に移行する。一方、零点が更新されていない場合は、ステップＳ２０３に移行する。
【０１２５】
ステップＳ２０２において、装置筐体取付角算出部５６は、３軸加速度センサ零点算出部５２が算出した加速度の零点を用いて、式（４８）および式（４９）によって測位装置の筐体のピッチ方向取付角φｐおよびロール方向取付角φｒを算出する。
【０１２６】
ステップＳ２０３において、装置筐体取付角算出部５６は、３軸加速度センサ零点算出部５２の算出結果を受けて、測位装置の筐体の取付角を算出する準備ができたか否かの判断を行う。具体的には、装置筐体取付角算出部５６は、以下の４つの条件を全て満たした場合に取付角を算出する準備ができたと判断する。
【０１２７】
（１）センサ座標系における３軸加速度センサ３が出力する加速度の零点（ＡＳｘｂｉａｓ，ＡＳｙｂｉａｓ，ＡＳｚｂｉａｓ）を算出済みである。
【０１２８】
（２）３軸角速度計算部５５が算出したセンサ座標系の姿勢角（ロール角、ピッチ角、ヨー角）を合成した角度が予め定められた閾値以下である。
【０１２９】
（３）距離計側部５１が算出した前後加速度が予め定められた閾値以上である。
【０１３０】
（４）ピッチ方向取付角およびロール方向取付角に基づいて、センサ座標系における３軸加速度センサ３が出力する加速度を式（４８）および式（４９）で変換したときのＺ軸加速度の変動が予め定められた範囲内である。
【０１３１】
取付角を算出する準備ができた場合（上記の４つの条件を全て満たした場合）は、ステップＳ２０４に移行する。一方、取付角を算出する準備ができていない場合（上記の４つの条件の全てを満たしていない場合）は、処理を終了する。
【０１３２】
ステップＳ２０４において、装置筐体取付角算出部５６は、ヨー方向取付角の検索範囲（角度範囲）および検索分解能（ヨー方向の角度の分解能）を設定する。例えば、装置筐体取付角算出部５６は、ヨー方向取付角が未算出である場合は、検索範囲として−１８０度から＋１８０度、検索分解能として１０度を設定する。また、装置筐体取付角算出部５６は、ヨー方向取付角が算出済みである場合は、検索範囲として−１５度から＋１５度、検索分解能として１度を設定する。
【０１３３】
ステップＳ２０５において、装置筐体取付角算出部５６は、ヨー方向取付角を検索するための任意角度（任意のヨー方向取付角）の初期値として、検索範囲の下限角度を設定する。
【０１３４】
ステップＳ２０６において、装置筐体取付角算出部５６は、ピッチ方向取付角、ロール方向取付角、および任意のヨー方向取付角に基づいて、式（４８）および式（４９）によって３軸加速度センサ３におけるセンサ座標系の加速度を車体座標系の加速度に座標変換する。具体的には、装置筐体取付角算出部５６は、図６に示すように、センサ座標系のＸＳ軸加速度およびＹＳ軸加速度のそれぞれを、仮の車体座標系のＸＢ軸加速度およびＹＢ軸加速度に座標変換する。
【０１３５】
ステップＳ２０７において、装置筐体取付角算出部５６は、距離計側部５１で算出された前後加速度と、ステップＳ２０６で座標変換された仮の車体座標系のＸ軸加速度ＡＢｘとの差異を求める。
【０１３６】
ステップＳ２０８において、装置筐体取付角算出部５６は、任意のヨー方向取付角に検索分解能の角度を加算する。
【０１３７】
ステップＳ２０９において、装置筐体取付角算出部５６は、任意のヨー方向取付角が検索範囲内であるか否かの判断を行う。任意のヨー方向取付角が検索範囲内である場合は、ステップＳ２０６に移行する。一方、任意のヨー方向取付角が検索範囲内でない場合は、ステップＳ２１０に移行する。
【０１３８】
ステップＳ２１０において、装置筐体取付角算出部５６は、ステップＳ２０７で求めた差異のうちの最も小さい差異となる（最も整合する）ときの任意のヨー方向取付角を選出する。
【０１３９】
ステップＳ２１１において、装置筐体取付角算出部５６は、ステップＳ２１０で選出した任意のヨー方向取付角の統計処理結果を用いて、式（５２）によってヨー方向取付角を更新（算出）する。
ヨー方向取付角  ＋＝  任意のヨー方向取付角（統計処理結果）  ・・・（５２）
但し、−１８０度〜１８０度
【０１４０】
以上のことから、本実施の形態２によれば、ピッチ方向取付角およびロール方向取付角を算出した後は、ＧＮＳＳ受信機１の受信状況に関わらず、速度センサ２、３軸加速度センサ３、および３軸角速度センサ４だけでヨー方向取付角を算出することができるため、広い屋内駐車場を移動中であってもヨー方向取付角を算出することが可能となる。また、車体座標系のＸ軸加速度について、速度センサ２の出力に基づいて算出された前後加速度と、３軸加速度センサ３の出力に基づいて算出されたＸ軸加速度との整合状態を確認しながらヨー方向取付角を探すことができるため、ヨー方向取付角の範囲に制約がなく、ヨー方向取付角を安定して算出することができる。
【０１４１】
また、ヨー方向取付角の算出状況に応じて、検索範囲および検索分解能を替えたり、ヨー方向取付角の妥当性のみを確認したりすることができるため、ヨー方向取付角が未算出である場合は素早くヨー方向取付角を算出し、その後はじっくりとヨー方向取付角を算出することができる。この場合、必要最小限の算出処理となるため、処理負荷が小さくなる。ヨー方向取付角を算出した後、水平面または傾斜面に関わらず、３軸角速度センサ４の角速度をセンサ座標系から車体座標系に座標変換し、ヨー角以外の姿勢角についても正確に算出することができるため、３次元の自車位置の推定をより正確に行うことができるという従来にない顕著な効果を奏する。
【０１４２】
なお、実施の形態２では、Ｘ軸加速度について、同一時刻に検索範囲内の複数の任意のヨー方向取付角を一斉に確認する場合について説明したが、これに限るものではない。例えば、予め定められた時間ごとに任意のヨー方向取付角を変えて整合性を確認することによってもヨー方向取付角を算出することができる。
【０１４３】
上記において、走行中は各センサから出力された情報の収集のみを行い、当該収集した情報を停車中にまとめて処理してもよい。
【０１４４】
実施の形態２では、ヨー方向取付角の算出が進むにつれて検索範囲を狭めて検索分解能を小さくすることについて説明したが、これに限るものではない。例えば、ヨー方向取付角の算出が収束したときは、ヨー方向取付角の妥当性をのみを確認するために、ヨー方向取付角の整合性のみを確認するようにしてもよく、当該確認によって異なる取付角に変更された可能性が高くなったときに、予め定められた検索範囲および検索分解能でヨー方向取付角を算出するようにしてもよい。
【０１４５】
＜実施の形態３＞
本発明の実施の形態３では、装置筐体取付角算出部５６が行う処理に特徴を有している。その他の構成および動作は、実施の形態１，２と同様であるため、ここでは説明を省略する。
【０１４６】
図７は、本実施の形態３による装置筐体取付角算出部５６の動作の一例を示すフローチャートである。なお、図７に示す動作は、図３のステップＳ１１４における処理に対応している。また、図７のステップＳ３０１〜ステップＳ３０６は、図５のステップＳ２０１〜ステップＳ２０６と同様であるため、ここでは説明を省略する。
【０１４７】
ステップＳ３０７において、装置筐体取付角算出部５６は、ピッチ方向取付角、ロール方向取付角、および任意のヨー方向取付角に基づいて、式（４８）および式（４９）によって３軸角速度センサ４におけるセンサ座標系の姿勢角を仮の車体座標系の姿勢角に座標変換する。
【０１４８】
ステップＳ３０８において、装置筐体取付角算出部５６は、距離計側部５１で算出された速度と、ステップＳ３０７で変換されたヨー角とに基づいて法線加速度を算出する。
【０１４９】
ステップＳ３０９において、装置筐体取付角算出部５６は、ステップＳ３０８で算出された法線加速度と、ステップＳ３０６で変換されたＹ軸加速度との差異を求める。
【０１５０】
ステップＳ３１０において、装置筐体取付角算出部５６は、任意のヨー方向取付角に検索分解能の角度を加算する。
【０１５１】
ステップＳ３１１において、装置筐体取付角算出部５６は、任意のヨー方向取付角が検索範囲内であるか否かの判断を行う。任意のヨー方向取付角が検索範囲内である場合は、ステップＳ３０６に移行する。一方、任意のヨー方向取付角が検索範囲内でない場合は、ステップＳ３１２に移行する。
【０１５２】
ステップＳ３１２において、装置筐体取付角算出部５６は、ステップＳ３０９で求めた差異のうちの最も小さい差異となるときの任意のヨー方向取付角を選出する。
【０１５３】
ステップＳ３１３において、装置筐体取付角算出部５６は、ステップＳ３１２で選出した任意のヨー方向取付角の統計処理結果を用いて、式（５２）によってヨー方向取付角を更新（算出）する。
【０１５４】
以上のことから、本実施の形態３によれば、実施の形態２と同様、ピッチ方向取付角およびロール方向取付角を算出した後は、ＧＮＳＳ受信機１の受信状況に関わらず、速度センサ２、３軸加速度センサ３、および３軸角速度センサ４だけでヨー方向取付角を算出することができるため、広い屋内駐車場を移動中であってもヨー方向取付角を算出することが可能となる。また、車体座標系のＹ軸加速度について、ステップＳ３０８で算出された法線加速度と、３軸加速度センサ３の出力に基づいて算出されたＹ軸加速度との整合状態を確認しながらヨー方向取付角を探すことができるため、ヨー方向取付角の範囲に制約がなく、ヨー方向取付角を安定して算出することができる。
【０１５５】
また、ヨー方向取付角の算出状況に応じて、検索範囲および検索分解能を替えたり、ヨー方向取付角の妥当性のみを確認したりすることができるため、ヨー方向取付角が未算出である場合は素早くヨー方向取付角を算出し、その後はじっくりとヨー方向取付角を算出することができる。この場合、必要最小限の算出処理となるため、処理負荷が小さくなる。ヨー方向取付角を算出した後、水平面または傾斜面に関わらず、３軸角速度センサ４の角速度をセンサ座標系から車体座標系に座標変換し、ヨー角以外の姿勢角についても正確に算出することができるため、３次元の自車位置の推定をより正確に行うことができるという従来にない顕著な効果を奏する。
【０１５６】
なお、実施の形態３では、Ｙ軸加速度について、同一時刻に検索範囲内の複数の任意のヨー方向取付角を一斉に確認する場合について説明したが、これに限るものではない。例えば、予め定められた時間ごとに任意のヨー方向取付角を変えて整合性を確認することによってもヨー方向取付角を算出することができる。
【０１５７】
上記において、走行中は各センサから出力された情報の収集のみを行い、当該収集した情報を停車中にまとめて処理してもよい。
【０１５８】
実施の形態３では、ヨー方向取付角の算出が進むにつれて検索範囲を狭めて検索分解能を小さくすることについて説明したが、これに限るものではない。例えば、ヨー方向取付角の算出が収束したときは、ヨー方向取付角の妥当性をのみを確認するために、ヨー方向取付角の整合性のみを確認するようにしてもよく、当該確認によって異なる取付角に変更された可能性が高くなったときに、予め定められた検索範囲および検索分解能でヨー方向取付角を算出するようにしてもよい。
【０１５９】
＜実施の形態４＞
本発明の実施の形態４では、装置筐体取付角算出部５６が行う処理に特徴を有している。その他の構成および動作は、実施の形態１〜３と同様であるため、ここでは説明を省略する。
【０１６０】
図８は、本実施の形態４による装置筐体取付角算出部５６の動作の一例を示すフローチャートである。なお、図８に示す動作は、図３のステップＳ１１４における処理に対応している。また、図８のステップＳ４０１〜ステップＳ４０７は、図７のステップＳ３０１〜ステップＳ３０７と同様であるため、ここでは説明を省略する。
【０１６１】
ステップＳ４０８において、装置筐体取付角算出部５６は、図９〜１１に示すように、式（５３）〜（５５）によってステップＳ４０６で座標変換された加速度から姿勢角を算出する。
【数２４】
2017064790.tif 000026
【０１６２】
ステップＳ４０９において、装置筐体取付角算出部５６は、姿勢角のいずれか１つについて、ステップＳ４０８で算出された姿勢角と、ステップＳ４０７で座標変換された姿勢角との差異を求める。
【０１６３】
ステップＳ４１０において、装置筐体取付角算出部５６は、任意のヨー方向取付角に検索分解能の角度を加算する。
【０１６４】
ステップＳ４１１において、装置筐体取付角算出部５６は、任意のヨー方向取付角が検索範囲内であるか否かの判断を行う。任意のヨー方向取付角が検索範囲内である場合は、ステップＳ４０６に移行する。一方、任意のヨー方向取付角が検索範囲内でない場合は、ステップＳ４１２に移行する。
【０１６５】
ステップＳ４１２において、装置筐体取付角算出部５６は、ステップＳ４０９で求めた差異のうちの最も小さい差異となるときの任意のヨー方向取付角を選出する。
【０１６６】
ステップＳ４１３において、装置筐体取付角算出部５６は、ステップＳ４１２で選出した任意のヨー方向取付角の統計処理結果を用いて、式（５２）によってヨー方向取付角を更新（算出）する。
【０１６７】
以上のことから、本実施の形態４によれば、実施の形態２，３と同様、ピッチ方向取付角およびロール方向取付角を算出した後は、ＧＮＳＳ受信機１の受信状況に関わらず、速度センサ２、３軸加速度センサ３、および３軸角速度センサ４だけでヨー方向取付角を算出することができるため、広い屋内駐車場を移動中であってもヨー方向取付角を算出することが可能となる。また、車体座標系の姿勢角のいずれか１つについて、３軸加速度センサ３の出力に基づいて算出された姿勢角と、３軸角速度センサ４の出力に基づいて算出された姿勢角との整合状態を確認しながらヨー方向取付角を探すことができるため、ヨー方向取付角の範囲に制約がなく、ヨー方向取付角を安定して算出することができる。
【０１６８】
また、ヨー方向取付角の算出状況に応じて、検索範囲および検索分解能を替えたり、ヨー方向取付角の妥当性のみを確認したりすることができるため、ヨー方向取付角が未算出である場合は素早くヨー方向取付角を算出し、その後はじっくりとヨー方向取付角を算出することができる。この場合、必要最小限の算出処理となるため、処理負荷が小さくなる。ヨー方向取付角を算出した後、水平面または傾斜面に関わらず、３軸角速度センサ４の角速度をセンサ座標系から車体座標系に座標変換し、ヨー角以外の姿勢角についても正確に算出することができるため、３次元の自車位置の推定をより正確に行うことができるという従来にない顕著な効果を奏する。
【０１６９】
なお、実施の形態４では、姿勢角（ロール角、ピッチ角、ヨー角）のいずれか１つについて、同一時刻に検索範囲内の複数の任意のヨー方向取付角を一斉に確認する場合について説明したが、これに限るものではない。例えば、予め定められた時間ごとに任意のヨー方向取付角を変えて整合性を確認することによってもヨー方向取付角を算出することができる。
【０１７０】
上記において、走行中は各センサから出力された情報の収集のみを行い、当該収集した情報を停車中にまとめて処理してもよい。
【０１７１】
実施の形態４では、ヨー方向取付角の算出が進むにつれて検索範囲を狭めて検索分解能を小さくすることについて説明したが、これに限るものではない。例えば、ヨー方向取付角の算出が収束したときは、ヨー方向取付角の妥当性をのみを確認するために、ヨー方向取付角の整合性のみを確認するようにしてもよく、当該確認によって異なる取付角に変更された可能性が高くなったときに、予め定められた検索範囲および検索分解能でヨー方向取付角を算出するようにしてもよい。
【０１７２】
＜実施の形態５＞
本発明の実施の形態５では、実施の形態２〜４を統合した処理を行うことを特徴としている。測位装置の構成は、実施の形態１と同様であるため、ここでは説明を省略する。
【０１７３】
装置筐体取付角算出部５６は、任意のヨー方向取付角ごとに、仮の車体座標系のＸ軸加速度、Ｙ軸加速度、および姿勢角（ロール角、ピッチ角、ヨー角）のうちのいずれか２つ以上について整合性を確認し（すなわち、図５のステップＳ２０７、図７のステップＳ３０９、図８のステップＳ４０９を組み合わせた処理を行う）、２つ以上について同一の任意のヨー方向取付角で整合したときの当該任意のヨー方向取付角をヨー方向取付角として選出する。また、ヨー方向取付角が算出された後は、取付角が変更されたか否かをより確実に判断し、新たな取付角の算出を開始する。
【０１７４】
自車位置推定部５９は、予め定められたカルマンフィルタを用いて、移動距離および姿勢角に基づいて自車位置を更新し、測位結果、移動距離、および姿勢角の各予測誤差に応じて自車位置を測位結果で修正する。特に、姿勢角の予測誤差については、３軸加速度センサ３および３軸角速度センサ４のそれぞれの出力に基づく姿勢角の差異から算出する。
【０１７５】
以上のことから、本実施の形態５によれば、実施の形態２〜４と同様、ピッチ方向取付角およびロール方向取付角を算出した後は、ＧＮＳＳ受信機１の受信状況に関わらず、速度センサ２、３軸加速度センサ３、および３軸角速度センサ４だけでヨー方向取付角を算出することができるため、広い屋内駐車場を移動中であってもヨー方向取付角を算出することが可能となる。また、Ｘ軸加速度、Ｙ軸加速度、ピッチ角、ロール角、およびヨー角のうちの少なくとも２つ以上について、同一の任意のヨー方向取付角が得られたか否かを確認することによって、ヨー方向取付角を確実に探したり、取付角が変更されたか否かを確実に判断したりすることができる。
【０１７６】
また、ピッチ方向取付角、ロール方向取付角、およびヨー方向取付角を用いてセンサ座標系から車体座標系に座標変換した姿勢角は、取付角の影響を排除したものになるため、この姿勢角を用いて更新した３次元の自車位置も正確となる。
【０１７７】
なお、実施の形態５では、３軸加速度および３軸角速度のそれぞれによって算出した姿勢角から、姿勢角の予測誤差を求める場合について説明したが、これに限るものではない。例えば、温度ドリフトによる姿勢角の誤差の影響を低減するために、３軸加速度および３軸角速度のそれぞれによって算出した姿勢角を重み付け平均して、自車位置の推定に用いてもよい。また、カルマンフィルタ以外のモデルを用いて自車位置を修正してもよい。
【０１７８】
以上で説明した測位装置は、車載用ナビゲーション装置、すなわちカーナビゲーション装置だけでなく、車両に搭載可能なＰＮＤ（Portable Navigation Device）および携帯通信端末（例えば、携帯電話、スマートフォン、およびタブレット端末など）、並びにサーバなどを適宜に組み合わせてシステムとして構築されるナビゲーション装置あるいはナビゲーション装置以外の装置にも適用することができる。この場合、測位装置の各機能あるいは各構成要素は、上記システムを構築する各機能に分散して配置される。
【０１７９】
具体的には、一例として、測位装置の機能をサーバに配置することができる。例えば、図２４に示すように、車両側にＧＮＳＳ受信機１、速度センサ２、３軸加速度センサ３、および３軸角速度センサ４を備え、サーバ８に図１に示す距離計側部５１、３軸加速度センサ零点算出部５２、３軸加速度計算部５３、３軸角速度センサ零点算出部５４、３軸角速度計算部５５、装置筐体取付角算出部５６、３軸加速度座標変換部５７、３軸角速度座標変換部５８、および自車位置推定部５９を備えることによって、測位システムを構築することができる。なお、サーバ８に備えられる各構成要素は、適宜にサーバ８および車両側に分散して配置するようにしてもよい。
【０１８０】
上記の構成とした場合であっても、上記の実施の形態と同様の効果が得られる。
【０１８１】
また、上記の実施の形態における動作を実行するソフトウェア（測位方法）を、例えばサーバや携帯通信端末に組み込んでもよい。
【０１８２】
具体的には、一例として、上記の測位方法は、移動体の測位装置で用いられる測位方法であって、ＧＰＳ衛星から電波を受信し、少なくとも移動体の現在位置と、測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力し、移動体の移動に応じたパルス信号を出力し、測位装置において互いに直交する３軸の角速度である３軸角速度を検出し、測位装置において互いに直交する３軸の加速度である３軸加速度を検出し、一定周期ごとに出力されたパルス信号に基づいて、移動体の移動距離、速度、および前後加速度を算出し、移動体が走行中である場合において、少なくとも測位結果として算出された３軸速度、算出された前後加速度、および検出された３軸加速度に基づいて、移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、水平面等速直進走行中であると判断した時の３軸加速度を零点として算出し、算出された３軸加速度の零点に基づいて、移動体に取り付けられた測位装置のピッチ方向取付角およびロール方向取付角を算出し、算出されたピッチ方向取付角およびロール方向取付角に基づいて、検出された３軸角速度を座標変換し、座標変換された３軸角速度から求めた少なくともヨーレートに基づいて、移動体の現在位置を推定する。
【０１８３】
上記より、上記の実施の形態における動作を実行するソフトウェアをサーバや携帯通信端末に組み込んで動作させることによって、上記の実施の形態と同様の効果が得られる。
【０１８４】
なお、本発明は、その発明の範囲内において、実施の形態を適宜、変形、省略することが可能である。
【０１８５】
本発明は詳細に説明されたが、上記した説明は、すべての態様において、例示であって、この発明がそれに限定されるものではない。例示されていない無数の変形例が、この発明の範囲から外れることなく想定され得るものと解される。
【符号の説明】
【０１８６】
１  ＧＮＳＳ受信機、２  速度センサ、３  ３軸加速度センサ、４  ３軸角速度センサ、５  制御部、６  プロセッサ、７  メモリ、５１  距離計側部、５２  ３軸加速度センサ零点算出部、５３  ３軸加速度計算部、５４  ３軸角速度センサ零点算出部、５５  ３軸角速度計算部、５６  装置筐体取付角算出部、５７  ３軸加速度座標変換部、５８  ３軸角速度座標変換部、５９  自車位置推定部。
【図１】
2017064790.tif 000027
【図２】
2017064790.tif 000028
【図３】
2017064790.tif 000029
【図４】
2017064790.tif 000030
【図５】
2017064790.tif 000031
【図６】
2017064790.tif 000032
【図７】
2017064790.tif 000033
【図８】
2017064790.tif 000034
【図９】
2017064790.tif 000035
【図１０】
2017064790.tif 000036
【図１１】
2017064790.tif 000037
【図１２】
2017064790.tif 000038
【図１３】
2017064790.tif 000039
【図１４】
2017064790.tif 000040
【図１５】
2017064790.tif 000041
【図１６】
2017064790.tif 000042
【図１７】
2017064790.tif 000043
【図１８】
2017064790.tif 000044
【図１９】
2017064790.tif 000045
【図２０】
2017064790.tif 000046
【図２１】
2017064790.tif 000047
【図２２】
2017064790.tif 000048
【図２３】
2017064790.tif 000049
【図２４】
2017064790.tif 000050
【国際調査報告】
2017064790.tif 000051
2017064790.tif 000052
2017064790.tif 000053
2017064790.tif 000054