JP6257865B2 - 測位装置および測位方法 - Google Patents

測位装置および測位方法 Download PDF

Info

Publication number
JP6257865B2
JP6257865B2 JP2017545051A JP2017545051A JP6257865B2 JP 6257865 B2 JP6257865 B2 JP 6257865B2 JP 2017545051 A JP2017545051 A JP 2017545051A JP 2017545051 A JP2017545051 A JP 2017545051A JP 6257865 B2 JP6257865 B2 JP 6257865B2
Authority
JP
Japan
Prior art keywords
angle
acceleration
axis
calculated
angular velocity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2017545051A
Other languages
English (en)
Other versions
JPWO2017064790A1 (ja
Inventor
忠富 石上
忠富 石上
崇志 入江
崇志 入江
将智 藤井
将智 藤井
浩平 藤本
浩平 藤本
功泰 西馬
功泰 西馬
篤史 毛利
篤史 毛利
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Mitsubishi Electric Corp
Original Assignee
Mitsubishi Electric Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Mitsubishi Electric Corp filed Critical Mitsubishi Electric Corp
Application granted granted Critical
Publication of JP6257865B2 publication Critical patent/JP6257865B2/ja
Publication of JPWO2017064790A1 publication Critical patent/JPWO2017064790A1/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/18—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration in two or more dimensions
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Description

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

Claims (14)

  1. 移動体の測位装置であって、
    ＧＰＳ衛星から電波を受信し、少なくとも前記移動体の現在位置と、前記測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力するＧＮＳＳ受信機と、
    前記移動体の移動に応じたパルス信号を出力する速度センサと、
    前記測位装置において互いに直交する３軸の角速度である３軸角速度を検出する３軸角速度センサと、
    前記測位装置において互いに直交する３軸の加速度である３軸加速度を検出する３軸加速度センサと、
    一定周期ごとに前記速度センサから出力された前記パルス信号に基づいて、前記移動体の移動距離、速度、および前後加速度を算出する距離計側部と、
    前記移動体が走行中である場合において、少なくとも前記ＧＮＳＳ受信機で算出された前記３軸速度、前記距離計側部で算出された前記前後加速度、および前記３軸加速度センサで検出された前記３軸加速度に基づいて、前記移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、前記水平面等速直進走行中であると判断した時に前記３軸加速度センサで検出された前記３軸加速度を零点として算出する３軸加速度センサ零点算出部と、
    前記３軸加速度センサ零点算出部で算出された前記３軸加速度の零点に基づいて、前記移動体に取り付けられた前記測位装置のピッチ方向取付角およびロール方向取付角を算出する装置筐体取付角算出部と、
    前記装置筐体取付角算出部で算出された前記ピッチ方向取付角および前記ロール方向取付角に基づいて、前記３軸角速度センサで検出された前記３軸角速度を座標変換する３軸角速度座標変換部と、
    前記３軸角速度座標変換部で変換された前記３軸角速度から求めた少なくともヨーレートに基づいて、前記移動体の現在位置を推定する位置推定部と、
    を備える、測位装置。
  2. 前記装置筐体取付角算出部は、前記ピッチ方向取付角、前記ロール方向取付角、および予め定められたヨー方向の角度に基づいて、前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向の加速度と、前記距離計側部で算出された前記前後加速度とが整合するときの前記ヨー方向の角度を前記測位装置のヨー方向取付角としてさらに算出し、
    前記３軸角速度座標変換部は、前記ピッチ方向取付角、前記ロール方向取付角、および前記ヨー方向取付角に基づいて、前記３軸角速度を座標変換することを特徴とする、請求項１に記載の測位装置。
  3. 前記装置筐体取付角算出部は、予め定められた角度範囲内における複数の前記ヨー方向の角度と、前記ピッチ方向取付角と、前記ロール方向取付角との全ての組み合わせについて、同一時刻の前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向の加速度と、前記距離計側部で算出された前記前後加速度とが最も整合するときの前記ヨー方向の角度を前記ヨー方向取付角として算出することを特徴とする、請求項２に記載の測位装置。
  4. 前記装置筐体取付角算出部は、前記距離計側部で算出された前記速度と、前記３軸角速度座標変換部で座標変換された前記３軸角速度から求めたヨー角とに基づいて、前記移動体の前後方向と直交する法線方向の加速度である法線加速度を算出するとともに、前記ピッチ方向取付角、前記ロール方向取付角、および予め定められたヨー方向の角度に基づいて、前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向と直交する左右方向の加速度と、前記法線加速度とが整合するときの前記ヨー方向の角度を前記測位装置のヨー方向取付角としてさらに算出し、
    前記３軸加速度座標変換部は、前記ピッチ方向取付角、前記ロール方向取付角、および前記ヨー方向取付角に基づいて、前記３軸加速度を座標変換することを特徴とする、請求項１に記載の測位装置。
  5. 前記装置筐体取付角算出部は、予め定められた角度範囲内における複数の前記ヨー方向の角度と、前記ピッチ方向取付角と、前記ロール方向取付角との全ての組み合わせについて、同一時刻の前記３軸加速度センサで検出された前記３軸加速度を座標変換し、当該座標変換後の前記３軸加速度のうちの前記移動体の前後方向に対して直交する左右方向の加速度と、前記法線加速度とが最も整合するときの前記ヨー方向の角度を前記ヨー方向取付角として算出することを特徴とする、請求項４に記載の測位装置。
  6. 前記装置筐体取付角算出部は、前記３軸加速度のうちの前記移動体の前後方向に対して直交する左右方向の加速度と前記法線加速度とが最も整合するときの前記ヨー方向の角度と、前記３軸加速度のうちの前記移動体の前後方向の加速度と前記距離計側部で算出された前記前後加速度とが最も整合するときの前記ヨー方向の角度とが同一である場合において、前記ヨー方向の角度を前記ヨー方向取付角として算出することを特徴とする、請求項５に記載の測位装置。
  7. 前記装置筐体取付角算出部は、前記角度範囲および当該角度範囲内における前記ヨー方向の角度の分解能を設定可能であることを特徴とする、請求項６に記載の測位装置。
  8. 前記装置筐体取付角算出部は、前記ピッチ方向取付角、前記ロール方向取付角、および予め定められたヨー方向の角度に基づいて、前記３軸角速度センサで検出された前記３軸角速度および前記３軸加速度センサで検出された前記３軸加速度をそれぞれ座標変換し、
    当該座標変換後の前記３軸角速度および前記３軸加速度のそれぞれに基づいて、ピッチ角、ロール角、およびヨー角を含む姿勢角を算出し、当該算出後の前記３軸角速度に基づく前記姿勢角と、前記３軸加速度に基づく前記姿勢角とのうちのいずれかの姿勢角が整合するときの前記ヨー角を前記測位装置のヨー方向取付角としてさらに算出し、
    前記３軸角速度座標変換部は、前記ピッチ方向取付角、前記ロール方向取付角、および前記ヨー方向取付角に基づいて、前記３軸角速度を座標変換することを特徴とする、請求項１に記載の測位装置。
  9. 前記装置筐体取付角算出部は、予め定められた角度範囲内における複数の前記ヨー方向の角度と、前記ピッチ方向取付角と、前記ロール方向取付角との全ての組み合わせについて、同一時刻の前記３軸加速度センサで検出された前記３軸加速度および前記３軸角速度センサで検出された前記３軸角速度をそれぞれ座標変換し、当該座標変換後の前記３軸角速度に基づいて前記姿勢角を算出するとともに、前記座標変換後の前記３軸加速度に基づいて前記ピッチ角、前記ロール角、および前記ヨー角のうちの少なくとも１つの前記姿勢角を算出し、当該算出後の前記３軸角速度に基づく前記姿勢角と、前記３軸加速度に基づく前記姿勢角とが最も整合するときの前記ヨー角を前記測位装置のヨー方向取付角として算出することを特徴とする、請求項８に記載の測位装置。
  10. 前記装置筐体取付角算出部は、前記３軸加速度のうちの前記移動体の前後方向に対して直交する左右方向の加速度と前記前後方向と直交する法線方向の加速度である法線加速度とが最も整合するときの前記ヨー方向の角度と、前記３軸加速度のうちの前記移動体の前後方向の加速度と前記距離計側部で算出された前記前後加速度とが最も整合するときの前記ヨー方向の角度と、前記３軸角速度に基づく前記ピッチ角、前記ロール角、または前記ヨー角と前記３軸加速度に基づく前記ピッチ角、前記ロール角、または前記ヨー角とのいずれかが最も整合するときの前記ヨー角と、のうちの少なくとも２つ以上が同一である場合において、前記ヨー方向の角度または前記ヨー角を前記ヨー方向取付角として算出することを特徴とする、請求項９に記載の測位装置。
  11. 前記装置筐体取付角算出部は、前記角度範囲および当該角度範囲内における前記ヨー方向の角度の分解能を設定可能であることを特徴とする、請求項１０に記載の測位装置。
  12. 前記位置推定部は、前記移動距離および前記姿勢角に基づいて前記移動体の現在位置を推定し、前記測位結果、前記移動距離、および前記姿勢角のそれぞれの予測誤差に応じて、前記測位結果に基づいて前記移動体の現在位置を修正することを特徴とする、請求項８に記載の測位装置。
  13. 前記位置推定部は、前記３軸加速度に基づく前記姿勢角と、前記３軸角速度に基づく前記姿勢角との差異に基づいて、前記姿勢角の前記予測誤差を算出することを特徴とする、請求項１２に記載の測位装置。
  14. 移動体の測位装置で用いられる測位方法であって、
    ＧＰＳ衛星から電波を受信し、少なくとも前記移動体の現在位置と、前記測位装置において互いに直交する３軸の速度である前後方向速度、左右方向速度、および上下方向速度を含む３軸速度とを算出して測位結果として出力し、
    前記移動体の移動に応じたパルス信号を出力し、
    前記測位装置において互いに直交する３軸の角速度である３軸角速度を検出し、
    前記測位装置において互いに直交する３軸の加速度である３軸加速度を検出し、
    一定周期ごとに出力された前記パルス信号に基づいて、前記移動体の移動距離、速度、および前後加速度を算出し、
    前記移動体が走行中である場合において、少なくとも前記測位結果として算出された前記３軸速度、前記算出された前記前後加速度、および前記検出された前記３軸加速度に基づいて、前記移動体が水平面上を等速で直進走行する水平面等速直進走行中であるのか否かを判断し、前記水平面等速直進走行中であると判断した時の前記３軸加速度を零点として算出し、
    前記算出された前記３軸加速度の零点に基づいて、前記移動体に取り付けられた前記測位装置のピッチ方向取付角およびロール方向取付角を算出し、
    前記算出された前記ピッチ方向取付角および前記ロール方向取付角に基づいて、前記検出された前記３軸角速度を座標変換し、
    前記座標変換された前記３軸角速度から求めた少なくともヨーレートに基づいて、前記移動体の現在位置を推定する、測位方法。
JP2017545051A 2015-10-15 2015-10-15 測位装置および測位方法 Active JP6257865B2 (ja)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2015/079155 WO2017064790A1 (ja) 2015-10-15 2015-10-15 測位装置および測位方法

Publications (2)

Publication Number Publication Date
JP6257865B2 true JP6257865B2 (ja) 2018-01-10
JPWO2017064790A1 JPWO2017064790A1 (ja) 2018-01-25

Family

ID=58517978

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2017545051A Active JP6257865B2 (ja) 2015-10-15 2015-10-15 測位装置および測位方法

Country Status (5)

Country Link
US (1) US10746551B2 (ja)
JP (1) JP6257865B2 (ja)
CN (1) CN108139212B (ja)
DE (1) DE112015006920B4 (ja)
WO (1) WO2017064790A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20200094793A (ko) * 2018-03-06 2020-08-07 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
WO2023075206A1 (ko) * 2021-10-28 2023-05-04 캐롯손해보험 주식회사 요 계산 없이 운전 행동을 감지하는 방법 및 장치
KR102544060B1 (ko) * 2021-10-28 2023-06-19 캐롯손해보험 주식회사 요 계산 없이 운전 행동을 감지하는 방법 및 장치

Families Citing this family (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3415954B1 (en) * 2016-02-12 2025-01-22 Panasonic Intellectual Property Management Co., Ltd. Coordinate output method and coordinate output device
DE102016209402B4 (de) * 2016-05-31 2019-05-29 Robert Bosch Gmbh Vorrichtung und Verfahren zur Überwachung eines mobilen Objekts
JP7006080B2 (ja) * 2017-09-25 2022-01-24 カシオ計算機株式会社 移動状態判別装置、電子時計、移動状態判別方法及びプログラム
JP6870635B2 (ja) * 2018-03-08 2021-05-12 セイコーエプソン株式会社 慣性計測装置、移動体、携帯型電子機器、及び電子機器
US11686582B2 (en) 2018-03-22 2023-06-27 Arriver Software Llc Sensor plausibility using GPS road information
CN109109866B (zh) * 2018-08-24 2020-12-18 深圳市国脉畅行科技股份有限公司 车辆行驶状态监测方法、装置、计算机设备及存储介质
FR3087884B1 (fr) * 2018-10-31 2021-04-02 Renault Sas Procede de determination d’un vecteur de deplacement d’un vehicule automobile, procede de determination d’une vitesse du vehicule et vehicule associe
JP7245119B2 (ja) * 2019-06-06 2023-03-23 日立建機株式会社 建設機械
CN110160553B (zh) * 2019-06-06 2023-03-28 中北大学 被约束体微小姿态动态测试装置以及解算方法
CN112444266B (zh) * 2019-08-27 2023-04-14 北京魔门塔科技有限公司 一种参数的标定方法及装置
CN111060126B (zh) * 2019-12-31 2022-06-07 东软睿驰汽车技术（沈阳）有限公司 定位方法、装置及车辆
FR3107589B1 (fr) * 2020-02-21 2022-03-18 Commissariat Energie Atomique Procédé de détermination de la position et de l’orientation d’un véhicule.
JP7620915B2 (ja) 2020-12-02 2025-01-24 大成建設株式会社 掘削補助システム
CN113238072B (zh) * 2021-01-20 2022-11-08 西安应用光学研究所 一种适用于车载光电平台的运动目标解算方法
JP7586013B2 (ja) 2021-07-22 2024-11-19 株式会社デンソー 加速度検出装置および取付角検出方法
JP7593257B2 (ja) 2021-07-22 2024-12-03 株式会社デンソー 加速度検出装置および取付角検出方法
CN114488247A (zh) * 2022-02-19 2022-05-13 北京中科智易科技有限公司 一种基于高精度北斗差分定位分析装备机动能力的方法
CN114739355B (zh) * 2022-04-12 2024-05-17 深圳市德传技术有限公司 基于gnss与三轴加速计的滑坡监测方法、系统及介质
JP2024041548A (ja) * 2022-09-14 2024-03-27 アルプスアルパイン株式会社 自律航法システム
CN116087869B (zh) * 2022-12-30 2024-10-29 泰斗微电子科技有限公司 一种基于加速度计的卫星定向方法、设备及可读存储介质

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6029111A (en) * 1995-12-28 2000-02-22 Magellan Dis, Inc. Vehicle navigation system and method using GPS velocities
JP2843904B2 (ja) 1996-03-04 1999-01-06 防衛庁技術研究本部長 車両用慣性航法装置
JP3375268B2 (ja) 1997-05-27 2003-02-10 株式会社日立製作所 ナビゲーション装置
US7085642B2 (en) * 2002-08-05 2006-08-01 Ford Global Technologies, Llc Method and system for correcting sensor offsets
JP4816339B2 (ja) * 2006-08-31 2011-11-16 ソニー株式会社 ナビゲーション装置、ナビゲーション情報算出方法及びナビゲーション情報算出プログラム
US8548723B2 (en) * 2007-01-18 2013-10-01 Mitsubishi Electric Corporation Car navigation system
WO2008099474A1 (ja) * 2007-02-14 2008-08-21 Pioneer Corporation ナビゲーション装置及びナビゲーション方法並びにナビゲーション用プログラム
JP4781300B2 (ja) * 2007-03-01 2011-09-28 アルパイン株式会社 位置検出装置および位置検出方法
JP2010276562A (ja) * 2009-06-01 2010-12-09 Panasonic Corp 位置検出装置
JP2011209162A (ja) 2010-03-30 2011-10-20 Panasonic Corp ナビゲーション装置
IT1403430B1 (it) * 2010-12-24 2013-10-17 Magneti Marelli Spa Procedimento di calibrazione di un sensore inerziale montato in posizione arbitraria a bordo di un veicolo, e sistema sensore della dinamica di un veicolo montabile a bordo in posizione arbitraria
US9217757B2 (en) * 2011-09-20 2015-12-22 Calamp Corp. Systems and methods for 3-axis accelerometer calibration
US9026263B2 (en) * 2011-11-30 2015-05-05 Alpine Electronics, Inc. Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis
DE112012006602B4 (de) * 2012-06-27 2016-08-04 Mitsubishi Electric Corporation Positioniervorrichtung
JP6201762B2 (ja) * 2014-01-08 2017-09-27 株式会社デンソー 速度推定装置

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20200094793A (ko) * 2018-03-06 2020-08-07 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
KR102304006B1 (ko) 2018-03-06 2021-09-23 주식회사 선택인터내셔날 차량의 움직임을 측정하기 위한 실시간 가속도 센서 보정 장치 및 이를 이용한 가속도 센서 보정 방법
WO2023075206A1 (ko) * 2021-10-28 2023-05-04 캐롯손해보험 주식회사 요 계산 없이 운전 행동을 감지하는 방법 및 장치
KR102544060B1 (ko) * 2021-10-28 2023-06-19 캐롯손해보험 주식회사 요 계산 없이 운전 행동을 감지하는 방법 및 장치

Also Published As

Publication number Publication date
US20180245924A1 (en) 2018-08-30
CN108139212B (zh) 2021-07-09
WO2017064790A1 (ja) 2017-04-20
DE112015006920B4 (de) 2019-06-27
JPWO2017064790A1 (ja) 2018-01-25
US10746551B2 (en) 2020-08-18
DE112015006920T5 (de) 2018-05-30
CN108139212A (zh) 2018-06-08

Similar Documents

Publication Publication Date Title
JP6257865B2 (ja) 測位装置および測位方法
JP5586994B2 (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
US7979207B2 (en) Systems and methods for detecting a vehicle static condition
CN101473193B (zh) 姿势角检测装置与姿势角检测方法
CN103235328B (zh) 一种gnss与mems组合导航的方法
US11118910B2 (en) Angular speed derivation device and angular speed derivation method for deriving angular speed based on output value of triaxial gyro sensor
US9273967B2 (en) Bias estimating method, posture estimating method, bias estimating device, and posture estimating device
US20070156337A1 (en) Systems, methods and apparatuses for continuous in-vehicle and pedestrian navigation
US20090115656A1 (en) Systems and Methods for Global Differential Positioning
US9121929B2 (en) Position output device using satellite navigation system
JP2009236532A (ja) 測位方法、プログラム及び測位装置
JP6383907B2 (ja) 車輌位置計測装置及び方法
EP0870172A1 (en) Improved vehicle navigation system and method using a multiple axes accelerometer
JP4563157B2 (ja) 物体の方位および姿勢検出装置
WO2017090360A1 (ja) センサ誤差算出装置、姿勢角算出装置、センサ誤差算出方法、姿勢角算出方法
US20190113342A1 (en) Angular speed derivation device and angular speed derivation method for deriving angular speed based on output value of triaxial gyro sensor
KR102380586B1 (ko) 오차 추정 장치, 오차 추정 방법 및 항법 시스템
CN111197994B (zh) 位置数据修正方法、装置、计算机设备和存储介质
JP2015004593A (ja) ナビゲーション装置
KR100586894B1 (ko) 차량의 정지상태 판단방법과 이를 이용한 차량 항법정보생성방법 및 차량항법장치
JP7201219B2 (ja) 測位装置、速度測定装置、及びプログラム
CN116449396A (zh) Gnss欺骗信号的检测方法、装置、设备及产品
JP2015135349A (ja) バイアス推定方法、姿勢推定方法、バイアス推定装置及び姿勢推定装置
CN116625362A (zh) 一种室内定位方法、装置、移动终端及存储介质
CN114754765A (zh) 一种车辆三维定位方法和系统

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20171006

A871 Explanation of circumstances concerning accelerated examination

Free format text: JAPANESE INTERMEDIATE CODE: A871

Effective date: 20171006

A975 Report on accelerated examination

Free format text: JAPANESE INTERMEDIATE CODE: A971005

Effective date: 20171027

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20171107

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20171205

R150 Certificate of patent or registration of utility model

Ref document number: 6257865

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

S111 Request for change of ownership or part of ownership

Free format text: JAPANESE INTERMEDIATE CODE: R313111

R350 Written notification of registration of transfer

Free format text: JAPANESE INTERMEDIATE CODE: R350

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250