JP2013228318A - キャリブレーション良否判定装置及び方法 - Google Patents

Abstract

【課題】移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、キャリブレーションが正常に完了したことを判別するキャリブレーション良否判定装置及び方法を提供すること。
【解決手段】キャリブレーション良否判定装置３０は、ＧＰＳの第１のアンテナ及び第２のアンテナとモーションセンサ２０とが所定の距離を置いて配置され、キャリブレーションを行い、キャリブレーション開始時の進行方向を算出し、移動体の速度のうち水平方向の速度を方位ごとに積算し、方位ごとの相対距離を算出し、算出した方位ごとの相対距離を、キャリブレーション開始時の方向を基準とした座標系の距離に変換し、変換した距離のうち、進行方向の距離が所定の距離に達した場合に、進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定し、キャリブレーションの成功か否かを報知する。
【選択図】図１

Description

本発明は、キャリブレーション良否判定装置及び方法に関する。

従来から、光学式センサを移動体の前進方向と横方向に２個取り付けて、移動体の横速度や横滑り角を検出する技術がある。また、慣性センサから得られる横滑り角速度とヨー角速度を加算した値の積分値とＧＰＳ受信機による相対方位角との差分から横滑り角を算出する技術も提案されている（例えば、特許文献１を参照）。さらに、２個のＧＰＳアンテナと、１軸加速度センサと、１軸角速度センサを使用して横滑り角を測定する技術も提案されている（例えば、特許文献２を参照）。

さらに、これらの従来技術よりも精度を格段に向上させた計測装置であって、ＧＰＳ用の２個のアンテナとモーションセンサとを配置し、横速度や横滑り角をリアルタイムに計測することが可能な計測装置も提案されている（例えば、特許文献３）。

特開２００５−１７１９１号公報 特開２００７−３４６１号公報 特願２０１２−０３４４３８

しかしながら、特許文献３の計測装置は、キャリブレーション工程でユーザが正確に直進走行できなかった場合、直進走行しないで行ったキャリブレーションにより調整された絶対姿勢角を用いて計測をしてしまう。

そこで、移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、キャリブレーションが正常に完了したことを判別する装置が求められている。

本発明は、移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、キャリブレーションが正常に完了したことを判別するキャリブレーション良否判定装置及び方法を提供することを目的とする。

本発明では、以下のような解決手段を提供する。
（１） 移動体の進行方向の軸線上に、ＧＰＳ衛星から出力される信号を受信する第１のアンテナと第２のアンテナとが所定の距離を置いて配置され、配置された前記第１のアンテナと前記第２のアンテナとの配置ずれを補正するためのキャリブレーションを行い、判定するキャリブレーション良否判定装置であって、前記第１のアンテナ及び前記第２のアンテナにより受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、前記第１のアンテナと前記第２のアンテナの相対位置を算出する相対位置算出部と、前記相対位置算出部により算出した相対位置に基づいて、前記移動体の絶対姿勢角を計測する姿勢角計測部と、前記キャリブレーションにより、前記姿勢角計測部で計測した絶対姿勢角と、前記移動体の進行方向とのずれ量を計算し、当該ずれ量に基づいて前記絶対姿勢角を調整するキャリブレーション部と、前記キャリブレーション中に、前記第１のアンテナにより受信したＧＰＳ衛星からの信号と、前記第１のアンテナから所定の距離を置いて配置されているモーションセンサにより計測された３軸方向の加速度及び角速度とに基づいて、前記移動体の速度を計測する移動体高精度速度計測部と、前記移動体高精度速度計測部により計測されたキャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する方位算出部と、前記移動体高精度速度計測部により算出された前記移動体の速度のうち水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出する東西南北方向距離算出部と、前記東西南北方向距離算出部によって算出された方位ごとの相対距離を、前記方位算出部により算出されたキャリブレーション開始時の方向を基準とした座標系の距離に変換する座標変換演算部と、前記座標変換演算部により変換された距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定するキャリブレーション判定部と、を備えるキャリブレーション良否判定装置。

（１）の構成によれば、本発明に係るキャリブレーション良否判定装置は、キャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する。次に、移動体の速度のうち水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出し、算出した方位ごとの相対距離を、キャリブレーション開始時の方向を基準とした座標系の距離に変換し、変換した距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定する。

すなわち、本発明に係るキャリブレーション良否判定装置は、所定の条件（例えば、時速３０ｋｍ／ｈ、２００ｍ）で行われたキャリブレーションにおいて、ＧＰＳ衛星からの信号とモーションセンサとを利用して計測した横ぶれ量が閾値（例えば、０．３５ｍ）以下か否かを判定する。
したがって、本発明に係るキャリブレーション良否判定装置は、移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、キャリブレーションが正常に完了したことを判別することができる。

（２） 前記キャリブレーション判定部によって判定された結果を出力する判定結果出力部をさらに備える（１）に記載のキャリブレーション良否判定装置。

したがって、（２）に係るキャリブレーション良否判定装置は、キャリブレーションの成功か否かをユーザに分かりやすいように報知することができる。

（３） 前記判定結果出力部は、前記キャリブレーション判定部によって前記キャリブレーションが正常に完了したと判定された場合、前記キャリブレーション部によって調整された前記絶対姿勢角をさらに出力する、（２）に記載のキャリブレーション良否判定装置。

したがって、（３）に係るキャリブレーション良否判定装置は、キャリブレーションが正常に完了した場合、キャリブレーションによって計測されたアンテナの配置ずれを、例えば、移動体の横速度や横滑り角を高精度に計測する計測装置に、出力することによって、計測装置の精度を向上させることができる。

（４） 前記移動体高精度速度計測部は、前記第１のアンテナにより受信した信号に基づいて、ＧＰＳ衛星から出力される搬送波の周波数のドップラーシフト量から地球座標上における前記移動体の速度を算出する移動体速度計測部と、前記第１のアンテナから所定の距離を置いて配置されているモーションセンサにより３軸方向の加速度と角速度を計測する加速度・角速度計測部と、前記移動体速度計測部により算出された前記移動体の速度と、前記加速度・角速度計測部により計測された加速度及び角速度を利用して、ストラップダウン演算を行うことにより、リアルタイムな前記移動体の速度を算出する高精度速度算出部と、を備える（１）から（３）のいずれかに記載のキャリブレーション良否判定装置。

したがって、（４）に係るキャリブレーション良否判定装置は、キャリブレーションにおいて、成功か否かの正確な判定や、高精度の配置ずれを出力することができる。

（５） （１）に記載のキャリブレーション良否判定装置が実行するキャリブレーション良否判定方法であって、前記相対位置算出部が、前記第１のアンテナ及び前記第２のアンテナにより受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、前記第１のアンテナと前記第２のアンテナの相対位置を算出する相対位置算出工程と、前記姿勢角計測部が、前記相対位置算出工程により算出した相対位置に基づいて、前記移動体の絶対姿勢角を計測する姿勢角計測工程と、前記キャリブレーション部が、前記キャリブレーションにより、前記姿勢角計測工程で計測した絶対姿勢角と、前記移動体の進行方向とのずれ量を計算し、当該ずれ量に基づいて前記絶対姿勢角を調整するキャリブレーション工程と、前記移動体高精度速度計測部が、前記キャリブレーション中に、前記第１のアンテナにより受信したＧＰＳ衛星からの信号と、前記第１のアンテナから所定の距離を置いて配置されているモーションセンサにより計測された３軸方向の加速度及び角速度とに基づいて、前記移動体の速度を計測する移動体高精度速度計測工程と、前記方位算出部が、前記移動体高精度速度計測工程により計測されたキャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する方位算出工程と、前記東西南北方向距離算出部が、前記移動体高精度速度計測工程により算出された前記移動体の速度のうち水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出する東西南北方向距離算出工程と、前記座標変換演算部が、前記東西南北方向距離算出工程によって算出された方位ごとの相対距離を、前記方位算出工程により算出されたキャリブレーション開始時の方向を基準とした座標系の距離に変換する座標変換演算工程と、前記キャリブレーション判定部が、前記座標変換演算工程により変換された距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定するキャリブレーション判定工程と、を備えるキャリブレーション良否判定方法。

したがって、本発明に係るキャリブレーション良否判定方法は、移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、キャリブレーションが正常に完了したことを判別することができる。

本発明によれば、移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、キャリブレーションが正常に完了したことを判別することができる。

本発明の一実施形態であるキャリブレーション良否判定装置の構成を示すブロック図である。 本発明の一実施形態であるキャリブレーション良否判定装置の移動体高精度速度計測部の構成を示すブロック図である。 本発明の一実施形態に係るキャリブレーション良否判定装置の処理内容を示すフローチャートである。 図３に続くフローチャートである。 本発明の一実施形態に係るキャリブレーション良否判定装置が判定する横ぶれ量と、キャリブレーション開始時の方向との関係を示す図である。 本発明の一実施形態に係るキャリブレーション部の具体的な処理についての説明に供する図である。

以下、本発明の実施形態について図を参照しながら説明する。
図１は、本発明の一実施形態であるキャリブレーション良否判定装置３０の構成を示すブロック図である。キャリブレーション良否判定装置３０は、相対位置算出部１２と、姿勢角計測部１３と、キャリブレーション部１４と、移動体高精度速度計測部２１と、方位算出部２２と、東西南北方向距離算出部２３と、座標変換演算部２４と、キャリブレーション判定部２５と、判定結果出力部２６とを備える。以下、各部ごとに説明する。

相対位置算出部１２は、移動体２の進行方向の軸線上に、ＧＰＳ衛星から出力される信号を受信する第１のアンテナ１０と第２のアンテナ１１が所定の距離を置いて配置されており、第１のアンテナ１０及び第２のアンテナ１１により受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、第１のアンテナ１０と第２のアンテナ１１の相対位置を算出する。なお、移動体２とは、例えば、車両のことである。

具体的には、相対位置算出部１２は、第１のアンテナ１０及び第２のアンテナ１１によってＧＰＳ衛星から出力される電波を受信し、搬送波位相をそれぞれ計測する。相対位置算出部１２は、計測したそれぞれの搬送波位相に基づいて、リアルタイムキネマティクス（ＲＴＫ）測位を実施する。これにより、相対位置算出部１２は、第１のアンテナ１０と第２のアンテナ１１の間の相対位置関係を計測することができる。

また、本実施例では、第１のアンテナ１０（マスター側アンテナ）は、移動体２の進行方向において、第２のアンテナ１１（スレーブ側アンテナ）よりも後方に配置されているものとする。また、第１のアンテナ１０と第２のアンテナ１１は、移動体２の屋根の上等に配置されるものとするが、ＧＰＳ衛星からの電波を受信できる位置であれば、外側に限られない。

姿勢角計測部１３は、相対位置算出部１２により算出した相対位置に基づいて、移動体２の絶対姿勢角を計測する。

姿勢角は、アンテナ間の距離を大きくとることで精度は向上する。参考値として、１ｍで０．１５°ＲＭＳ、２ｍで０．１°ＲＭＳの精度を得ることができる。姿勢角計測部１３は、ＧＰＳ衛星の搬送波を使用して姿勢角を計測するため、後述する移動体速度計測部１５と同期して、５〜１００Ｈｚ程度のサンプリング周波数で計測する。

キャリブレーション部１４は、姿勢角計測部１３で計測した絶対姿勢角と、移動体２の進行方向とのずれ量を計算し、当該ずれ量に基づいて絶対姿勢角を調整する。

以下に、キャリブレーション部１４の具体的な処理について図６を参照しながら説明する。
移動体２の正確な姿勢角を計測するためには、第１のアンテナ１０と第２のアンテナ１１を移動体２の車両センターラインにあわせること（軸あわせ）が必要である。
手作業であわせることが精度上難しいので、キャリブレーション部１４により、以下の手順で、キャリブレーション（校正）を計測前に行って、軸あわせを行う。

このようにして、キャリブレーション部１４は、平均取り付け角度誤差を使用することで、姿勢角を車両センターラインに合わせることができる。

移動体高精度速度計測部２１は、キャリブレーション中に、第１のアンテナ１０により受信したＧＰＳ衛星からの信号と、第１のアンテナ１０から所定の距離を置いて配置されているモーションセンサ２０により計測された３軸方向の加速度及び角速度とに基づいて、移動体２の速度を計測する。移動体高精度速度計測部２１の詳細については、図２によって後述する。

方位算出部２２は、移動体高精度速度計測部２１により計測されたキャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する。具体的には、方位算出部２２は、計測開始時のＮＥＤ座標系の速度から式（３）を実行することで計測開始時の方向θ（ｒａｄ）を算出する。ここで、ＶＮは南北方向の速度、ＶＥは東西方向の速度である。
θ ＝ ｔａｎ^−１（ＶＮ／ＶＥ） ・・・（３）

東西南北方向距離算出部２３は、移動体高精度速度計測部２１により算出された水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出する。具体的には、東西南北方向距離算出部２３は、移動体高精度速度計測部２１より得られるＮＥＤ座標系の速度情報を積算することで、計測開始からの方位ごとの相対距離を時系列に求めることができる。

座標変換演算部２４は、東西南北方向距離算出部２３によって算出された方位ごとの相対距離を、方位算出部２２により算出されたキャリブレーション開始時の方向を基準とした座標系の距離に変換する。具体的には、座標変換演算部２４は、東西南北方向距離算出部２３により算出された東西南北の方位の時系列ごとの距離を、式（４）を用いて計測開始時の方向を基準とした、座標系の距離に変換する。

キャリブレーション判定部２５は、座標変換演算部２４により変換された距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定する。

具体的には、キャリブレーション判定部２５は、キャリブレーション開始時の進行方向の距離が、例えば、２００ｍに達した場合に、横ぶれ量が閾値以下か否かを判定する。キャリブレーション判定部２５は、要求精度によって、キャリブレーションの成否を判別する横ぶれ量の値を、選択する。例えば、キャリブレーションの精度を０．１°以内にする場合に許容できる横ぶれ量は、２００ｍ×ｓｉｎ（０．１°）＝０．３５ｍである。すなわち、キャリブレーション判定部は、進行方向の距離が２００ｍに達した場合、横ぶれ量が０．３５ｍ以内であれば、キャリブレーション成功と判定する。
また、キャリブレーション判定部２５は、所定の距離に達するごとに、例えば、１００ｍに達するごとに横ぶれ量が閾値以下か否かを判定するとしてもよい。例えば、キャリブレーション判定部２５は、１００ｍで横ぶれ量が０．１７５ｍ以下、２００ｍで０．３５ｍ以下か否かを判定し、１００ｍと２００ｍとで閾値以下である場合に、キャリブレーション成功と判定する。

判定結果出力部２６は、キャリブレーション判定部２５によって判定された結果を出力する。具体的には、判定結果出力部２６は、キャリブレーション判定部２５によって得られた成否結果を、ユーザにわかりやすいように、例えば、表示器のＬＥＤ等によって、キャリブレーション成功の場合、緑色の点灯で表わし、失敗の場合、赤色の点灯で表わす。

さらに、判定結果出力部２６は、キャリブレーション判定部２５によってキャリブレーションが正常に完了したと判定された場合、キャリブレーション部１４によって調整された絶対姿勢角を出力する。例えば、計測装置は、速度・姿勢角算出部１７において、キャリブレーション部１４により調整された絶対姿勢角を用いて、移動体の姿勢角及び速度を算出する。

次に、移動体高精度速度計測部２１について詳述する。図２は、本発明の一実施形態であるキャリブレーション良否判定装置３０の移動体高精度速度計測部２１の構成を示すブロック図である。移動体高精度速度計測部２１は、移動体速度計測部１５と、加速度・角速度計測部１６と、速度・姿勢角算出部１７とを備える。

移動体速度計測部１５は、第１のアンテナ１０により受信した信号に基づいて、ＧＰＳ衛星から出力される搬送波の周波数のドップラーシフト量から地球座標上における移動体２の速度（ドップラー速度）を算出する。具体的には、移動体速度計測部１５は、第１のアンテナ１０により受信した信号に基づいて、ＧＰＳ衛星からの搬送波のドップラーシフト周波数を使用して移動体２の速度（ＮＥＤ座標系速度：北、東、下座標）を高精度に計測する。また、移動体速度計測部１５は、５〜１００Ｈｚ程度のサンプリング周波数で移動体２の速度を計測する。また、移動体速度計測部１５は、緯度、経度、標高についても、５〜１００Ｈｚ程度のサンプリング周波数で同期して計測する。なお、第１のアンテナ１０とＧＰＳ衛星との距離が、遠ざかる又は近づくと、第１のアンテナ１０が受信する搬送波の位相は、連続的に変化し、周波数が低くなったり高くなったりする。移動体速度計測部１５は、この周波数の変化量から第１のアンテナ１０が出力する速度を取得する。

加速度・角速度計測部１６は、第１のアンテナ１０から所定の距離を置いて配置されているモーションセンサ２０により３軸方向の加速度と角速度を計測する。また、加速度・角速度計測部１６は、モーションセンサ２０の設置地点の３軸方向の加速度・角速度を１００〜１ｋＨｚ程度のサンプリング周波数で計測する。なお、モーションセンサ２０は、例えば、３軸方向の加速度及び角速度を計測することができるＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）により構成される。以下では、モーションセンサ２０をＩＭＵ２０という。

また、ＩＭＵ２０は、図１に示すように、移動体２の進行方向に対して、第２のアンテナ１１が配置されている方向に向けて、ルーフ上にマグネット等で取り付ける。また、第１のアンテナ１０及び第２のアンテナ１１も同様に、マグネット等でルーフ上に取り付ける。また、第１のアンテナ１０と第２のアンテナ１１間の距離を大きくするほど、精度は向上するが、運用上の観点から１ｍ又は２ｍ程度で使用するものとする。

速度・姿勢角算出部１７は、キャリブレーション中において、移動体速度計測部１５により算出された移動体２の速度と、加速度・角速度計測部１６により計測された加速度及び角速度を利用して、ストラップダウン演算を行うことにより、リアルタイムな移動体２の速度を算出する。なお、速度・姿勢角算出部１７は、キャリブレーションの成功後において、キャリブレーション部１４により調整された絶対姿勢角と、移動体速度計測部１５により算出された移動体２の速度と、加速度・角速度計測部１６により計測された加速度及び角速度とを利用して、ストラップダウン演算を行うことにより、リアルタイムな移動体２の姿勢角及び速度を算出する。

速度・姿勢角算出部１７は、図２に示すように、ストラップダウンナビゲータ部１００と、良否判定部１０１と、同期化処理部１０２と、乗算部１０３と、状態推定部１０４と、遅延処理部１０５を備え、リアルタイムな高精度の速度と姿勢角を演算する。

ストラップダウンナビゲータ部１００（自律航法アルゴリズム）は、加速度・角速度計測部１６により計測された加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度（ＮＥＤ方向）、補正位置（緯度、経度、標高）、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出する。詳細には、ストラップダウンナビゲータ部１００は、加速度・角速度計測部１６により計測された加速度及び角速度に対してストラップダウン演算を行い位置、速度、姿勢角等を算出し、さらに状態推定部１０４によって推定演算された調整量でストラップダウン演算によって得られた位置、速度、姿勢角等を補正して出力する。

良否判定部１０１は、移動体速度計測部１５により算出された移動体２の速度に基づく加速度と、加速度・角速度計測部１６により計測された加速度との差分を算出し、当該差分に基づく係数を算出する。

ここで、良否判定部１０１の詳細について説明する。良否判定部１０１は、移動体速度計測部１５によって測定されたドップラー速度から算出した加速度と、加速度・角速度計測部１６によって計測された加速度との差分を算出し、算出した差分に基づいて係数αを算出する。

例えば、良否判定部１０１は、ドップラー速度を微分した加速度と、ＩＭＵ２０が計測した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。そして、良否判定部１０１は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、係数αを算出する。

すなわち、良否判定部１０１は、求めた逆関数に基づいて、ドップラー速度がノイズを含んでいないと判断した場合に良判定を行い、ノイズを含んでいると判断した場合に否判定を行って、それぞれの判定を数値化した係数αを算出する。

また、良否判定部１０１は、姿勢角計測部１３より計測された（絶対）姿勢角と、ストラップダウンナビゲータ部１００により演算される補正姿勢角の差分を算出し、上述した良否判定方法を適用することにより、姿勢角計測部１３より計測された姿勢角の良否をリアルタイムに良否判定することができる。

同期化処理部１０２は、ストラップダウンナビゲータ部１００によるストラップダウン演算により得られた補正速度（リアルタイム補間速度）、補正位置、補正姿勢角と、移動体速度計測部１５から得られたＮＥＤ方向の速度（ドップラー速度）、位置（緯度、経度、標高）、キャリブレーション部１４から得られた姿勢角の同期化処理を行うために、一定時間分これらを遅延させる。

遅延処理部１０５は、移動体速度計測部１５により算出されたＧＰＳ衛星のドップラー速度、位置及びキャリブレーション部１４により調整された絶対姿勢角を遅延処理する。
具体的には、遅延処理部１０５は、良否判定部１０１によって係数αが算出されるための時間だけ、移動体速度計測部１５から得られたＮＥＤ方向の速度（ドップラー速度）、位置（緯度、経度、標高）、キャリブレーション部１４から得られた絶対姿勢角を良否判定演算に用いる時間分だけ遅延させる。

減算部１０８は、遅延処理部１０５で遅延させた移動体２の速度、姿勢角、位置と、同期化処理部１０２で遅延させたリアルタイム補間速度、補正位置、補正姿勢角とを減算して誤差量δｘを求める。
乗算部１０３は、良否判定部１０１により算出された係数αと、減算部１０８により遅延処理部１０５と同期化処理部１０２とを減算して得られた誤差量δｘを乗算する。

状態推定部１０４は、乗算部１０３によって係数αが乗算された誤差量δｘから補正速度、補正位置、補正姿勢角、補正３軸加速度、補正３軸角速度に対する調整量を推定演算する。詳細には、状態推定部１０４は、乗算部１０３によって係数αが乗算された誤差量δｘから補正速度、補正位置、補正姿勢角、補正３軸加速度、補正３軸角速度に対する調整量を正確に推定演算する。

図３、図４は、本発明の一実施形態に係るキャリブレーション良否判定装置３０の処理内容を示すフローチャートである。キャリブレーション良否判定装置３０は、コンピュータ及びその周辺装置が備えるハードウェア並びに該ハードウェアを制御するソフトウェアによって構成され、以下の処理は、キャリブレーション良否判定装置３０の制御部（例えば、ＣＰＵ）が所定のソフトウェアに従い実行する処理である。

ステップＳ１０１において、ＣＰＵは、ストラップダウンナビゲータ部１００の初期設定（初期速度、初期位置、初期姿勢角）、状態推定部１０４の初期設定（誤差共分散行列の初期化）を行う。その後、ＣＰＵは、処理をステップＳ１０２に移す。

ステップＳ１０２において、ＣＰＵ（移動体高精度速度計測部２１）は、移動体高精度速度計測処理を行う。その後、ＣＰＵは、処理をステップＳ１０３に移す。

ステップＳ１０３において、ＣＰＵ（方位算出部２２）は、キャリブレーション開始か否かを判断する。より具体的には、ＣＰＵは、キャリブレーションを開始する開始指令を受け付けた直後か否かを判断する。この判断がＹＥＳの場合、ＣＰＵは、処理をステップＳ１０４に移し、この判断がＮＯの場合、ＣＰＵは、処理をステップＳ１０５に移す。

ステップＳ１０４において、ＣＰＵ（方位算出部２２）は、キャリブレーション開始時の方向を算出する。より具体的には、ＣＰＵは、ＮＥＤ座標系の速度から上述の式（３）を実行し、キャリブレーション開始時の方向θ（ｒａｄ）を算出する。その後、ＣＰＵは、処理をステップＳ１０５に移す。

ステップＳ１０５において、ＣＰＵ（東西南北方向距離算出部２３）は、南北方向の速度を積算する。より具体的には、ＣＰＵは、ステップＳ１０２において算出された速度のうちＮ値を積算し、南北方向の相対距離を算出する。その後、ＣＰＵは、処理をステップＳ１０６に移す。

ステップＳ１０６において、ＣＰＵ（東西南北方向距離算出部２３）は、東西方向の速度を積算する。より具体的には、ＣＰＵは、ステップＳ１０２において算出された速度のうちＥ値を積算し、東西方向の相対距離を算出する。その後、ＣＰＵは、処理をステップＳ１０７に移す。

ステップＳ１０７において、ＣＰＵ（座標変換演算部２４）は、方位を基準とする座標を、キャリブレーション開始時の方向を基準とする座標に変換する。より具体的には、ＣＰＵは、ステップＳ１０５及びステップＳ１０６において算出された方位ごとの相対距離を、ステップＳ１０４において算出されたキャリブレーション開始時の方向を基準とした座標系の距離に変換する。その後、ＣＰＵは、処理をステップＳ１０８に移す。

ステップＳ１０８において、ＣＰＵ（キャリブレーション判定部２５）は、所定の距離に達したか否かを判断する。より具体的には、ＣＰＵは、キャリブレーション開始時の方向を基準とした座標系のうち進行方向の距離が、所定の距離に達したか否かを判断する。この判断がＹＥＳの場合、ＣＰＵは、処理をステップＳ１０９に移し、この判断がＮＯの場合、ＣＰＵは、処理をステップＳ１０２に移す。

ステップＳ１０９において、ＣＰＵ（キャリブレーション判定部２５）は、横ぶれ量が閾値以下か否かを判断する。より具体的には、ＣＰＵは、キャリブレーション開始時の方向を基準とした座標系のうち進行方向に直交する方向の距離が、閾値以下か否かを判断する。この判断がＹＥＳの場合、ＣＰＵは、処理をステップＳ１１０に移し、この判断がＮＯの場合、ＣＰＵは、処理をステップＳ１１１に移す。

ステップＳ１１０において、ＣＰＵ（判定結果出力部２６）は、キャリブレーションの成功を報知する。より具体的には、ＣＰＵは、表示器の緑色のＬＥＤを点灯させる。その後、ＣＰＵは、処理を終了する。

ステップＳ１１１において、ＣＰＵ（判定結果出力部２６）は、キャリブレーションの失敗を報知する。より具体的には、ＣＰＵは、表示器の赤色のＬＥＤを点灯させる。その後、ＣＰＵは、処理を終了する。

図５は、本発明の一実施形態に係るキャリブレーション良否判定装置３０が判定する横ぶれ量と、キャリブレーション開始時の方向との関係を示す図である。図５において、移動体２が計測開始地点Ｐ_ＳからＰ_１に移動した場合、東西南北の方位を基準とすると、Ｐ_１の座標は、（Ｐ_Ｅ、Ｐ_Ｎ）になる。座標Ｐ_Ｅは、移動体高精度速度計測部２１により算出された水平方向の速度のうちＥ値（東西の速度）を積算して算出した東西方向の走行距離である。同様に、Ｐ_Ｎは、移動体高精度速度計測部２１により算出された水平方向の速度のうちＮ値（南北の速度）を積算して算出した南北方向の走行距離である。この座標を計測開始時の方向を基準とした座標（Ｐ_Ｘ、Ｐ_Ｙ）に変換すると、Ｐ_Ｙは、横ぶれ量を示す。

本実施形態によれば、キャリブレーション良否判定装置３０は、キャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する。次に、移動体の速度のうち水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出し、算出した方位ごとの相対距離を、キャリブレーション開始時の方向を基準とした座標系の距離に変換し、変換した距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定し、キャリブレーションの成功か否かをユーザに分かりやすいように報知する。
したがって、キャリブレーション良否判定装置３０は、移動体の横速度や横滑り角を高精度に計測する計測装置のキャリブレーション工程において、ユーザにキャリブレーションを成功させて、キャリブレーションにより調整された絶対姿勢角を用いた高精度な計測を計測装置にさせることができる。

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。

１０ 第１のアンテナ
１１ 第２のアンテナ
１２ 相対位置算出部
１３ 姿勢角計測部
１４ キャリブレーション部
１５ 移動体速度計測部
１６ 加速度・角速度計測部
１７ 速度・姿勢角算出部
２１ 移動体高精度速度計測部
２２ 方位算出部
２３ 東西南北方向距離算出部
２４ 座標変換演算部
２５ キャリブレーション判定部
２６ 判定結果出力部
３０ キャリブレーション良否判定装置
１００ ストラップダウンナビゲータ部
１０１ 良否判定部
１０２ 同期化処理部
１０３ 乗算部
１０４ 状態推定部
１０５ 遅延処理部
１０８ 減算部

Claims (5)

1. 移動体の進行方向の軸線上に、ＧＰＳ衛星から出力される信号を受信する第１のアンテナと第２のアンテナとが所定の距離を置いて配置され、配置された前記第１のアンテナと前記第２のアンテナとの配置ずれを補正するためのキャリブレーションを行い、判定するキャリブレーション良否判定装置であって、
   前記第１のアンテナ及び前記第２のアンテナにより受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、前記第１のアンテナと前記第２のアンテナの相対位置を算出する相対位置算出部と、
   前記相対位置算出部により算出した相対位置に基づいて、前記移動体の絶対姿勢角を計測する姿勢角計測部と、
   前記キャリブレーションにより、前記姿勢角計測部で計測した絶対姿勢角と、前記移動体の進行方向とのずれ量を計算し、当該ずれ量に基づいて前記絶対姿勢角を調整するキャリブレーション部と、
   前記キャリブレーション中に、前記第１のアンテナにより受信したＧＰＳ衛星からの信号と、前記第１のアンテナから所定の距離を置いて配置されているモーションセンサにより計測された３軸方向の加速度及び角速度とに基づいて、前記移動体の速度を計測する移動体高精度速度計測部と、
   前記移動体高精度速度計測部により計測されたキャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する方位算出部と、
   前記移動体高精度速度計測部により算出された前記移動体の速度のうち水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出する東西南北方向距離算出部と、
   前記東西南北方向距離算出部によって算出された方位ごとの相対距離を、前記方位算出部により算出されたキャリブレーション開始時の方向を基準とした座標系の距離に変換する座標変換演算部と、
   前記座標変換演算部により変換された距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定するキャリブレーション判定部と、
   を備えるキャリブレーション良否判定装置。
2. 前記キャリブレーション判定部によって判定された結果を出力する判定結果出力部をさらに備える請求項１に記載のキャリブレーション良否判定装置。
3. 前記判定結果出力部は、前記キャリブレーション判定部によって前記キャリブレーションが正常に完了したと判定された場合、前記キャリブレーション部によって調整された前記絶対姿勢角をさらに出力する、請求項２に記載のキャリブレーション良否判定装置。
4. 前記移動体高精度速度計測部は、
   前記第１のアンテナにより受信した信号に基づいて、ＧＰＳ衛星から出力される搬送波の周波数のドップラーシフト量から地球座標上における前記移動体の速度を算出する移動体速度計測部と、
   前記第１のアンテナから所定の距離を置いて配置されているモーションセンサにより３軸方向の加速度と角速度を計測する加速度・角速度計測部と、
   前記移動体速度計測部により算出された前記移動体の速度と、前記加速度・角速度計測部により計測された加速度及び角速度を利用して、ストラップダウン演算を行うことにより、リアルタイムな前記移動体の速度を算出する速度・姿勢角算出部と、
   を備える請求項１から３のいずれかに記載のキャリブレーション良否判定装置。
5. 請求項１に記載のキャリブレーション良否判定装置が実行するキャリブレーション良否判定方法であって、
   前記相対位置算出部が、前記第１のアンテナ及び前記第２のアンテナにより受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、前記第１のアンテナと前記第２のアンテナの相対位置を算出する相対位置算出工程と、
   前記姿勢角計測部が、前記相対位置算出工程により算出した相対位置に基づいて、前記移動体の絶対姿勢角を計測する姿勢角計測工程と、
   前記キャリブレーション部が、前記キャリブレーションにより、前記姿勢角計測工程で計測した絶対姿勢角と、前記移動体の進行方向とのずれ量を計算し、当該ずれ量に基づいて前記絶対姿勢角を調整するキャリブレーション工程と、
   前記移動体高精度速度計測部が、前記キャリブレーション中に、前記第１のアンテナにより受信したＧＰＳ衛星からの信号と、前記第１のアンテナから所定の距離を置いて配置されているモーションセンサにより計測された３軸方向の加速度及び角速度とに基づいて、前記移動体の速度を計測する移動体高精度速度計測工程と、
   前記方位算出部が、前記移動体高精度速度計測工程により計測されたキャリブレーション開始時の速度から、キャリブレーション開始時の進行方向を算出する方位算出工程と、
   前記東西南北方向距離算出部が、前記移動体高精度速度計測工程により算出された前記移動体の速度のうち水平方向の速度を方位ごとに積算し、東西南北方向の相対距離を算出する東西南北方向距離算出工程と、
   前記座標変換演算部が、前記東西南北方向距離算出工程によって算出された方位ごとの相対距離を、前記方位算出工程により算出されたキャリブレーション開始時の方向を基準とした座標系の距離に変換する座標変換演算工程と、
   前記キャリブレーション判定部が、前記座標変換演算工程により変換された距離のうち、キャリブレーション開始時の進行方向の距離が所定の距離に達した場合に、キャリブレーション開始時の進行方向に直交する方向の距離によって示される横ぶれ量が閾値以下か否かを判定するキャリブレーション判定工程と、
   を備えるキャリブレーション良否判定方法。
   

Images