JPWO2017168654A1 - 測定装置、測定方法、及び、プログラム - Google Patents

Abstract

測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得し、その取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離と進行方向の変化量を算出する。

Description

本発明は、移動体の進行方向の変化を検出する技術に関する。

移動体の前方に存在する物体を認識して、移動体のヨーレートを補正する手法が知られている。特許文献１は、移動体の前方に存在する固定物を認識し、その固定物が自車両に対して相対的に移動する移動軌跡に基づいて、ヨーレートセンサのずれ量を補正する手法を記載している。

特開２０１２−６６７７７号公報

特許文献１に記載の手法では、上記の移動軌跡の他に、車速、旋回半径からのずれ量、固定物が横方向に相対的に移動する横移動量など、推定ヨーレートの算出に用いるパラメータが多く、これらのパラメータを取得するセンサのずれ量も考慮しなくてはならない。

本発明が解決しようとする課題としては、上記のものが例として挙げられる。本発明は、１つのセンサで移動体の移動量とヨー角を算出し、移動距離と進行方向の変化量を得ることが可能な測定装置を提供することを目的とする。

請求項１に記載の発明は、測定装置であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、を備えることを特徴とする。

請求項１０に記載の発明は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、を備えることを特徴とする。

請求項１１に記載の発明は、コンピュータを備える測定装置によって実行されるプログラムであって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、として前記コンピュータを機能させることを特徴とする。

１つの地物を用いて移動距離とヨー角変化量を算出する方法を示す。 実施例による係数更新装置の構成を示すブロック図である。 係数更新処理のフローチャートである。 第１実施例によるビーム角度の算出方法を説明する図である。 第１実施例によるビーム角度の算出方法を説明する他の図である。 第１実施例によるビーム角度の算出方法を説明する他の図である。 第１実施例によるビーム角度の算出方法を説明する他の図である。 第１実施例によるビーム角度の算出処理のフローチャートである。 第２実施例によるビーム角度の算出方法を説明する図である。 第２実施例によるビーム角度の算出処理のフローチャートである。 第３実施例によるビーム角度の算出方法を説明する図である。 第３実施例によるビーム角度の算出処理のフローチャートである。 地物の計測中における車両の移動による影響を示す。 車両の移動を考慮した地物の計測点の座標計算方法を示す。 地物の計測点の座標計算のフローチャートである。

本発明の好適な実施形態では、測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、を備える。

上記の測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得し、その取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。

上記の測定装置の一態様は、前記少なくとも２つの位置の座標を取得する第２取得部を更に備え、前記算出部は、前記第１時刻及び前記第２時刻における、（ｉ）前記移動体から前記少なくとも２つの位置までのそれぞれの距離及び前記少なくとも２つの位置の座標に基づいて算出される、前記地物の表面に対する法線と前記移動体からみた前記地物上の１つの位置の方向とがなす角度、並びに（ｉｉ）前記移動体からみた前記１つの位置の方向と前記移動体の進行方向とがなす角度、に基づき前記移動距離及び前記変化量のうち少なくとも一方を算出する。

上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の少なくとも３つの位置までの距離及び前記移動体から見た前記少なくとも３つの位置の方向から、前記少なくとも３つの位置の座標を計算する第１計算部と、前記少なくとも３つの位置の座標に基づいて前記地物の表面の重心点の座標を計算し、前記移動体からみた前記重心点の方向を決定する第１方向決定部と、前記少なくとも３つの位置の座標に基づいて前記地物の表面を規定する平面の式を計算し、前記平面の式に基づいて前記地物の表面に対する法線の方向を決定する第２方向決定部と、前記重心点の方向と前記法線の方向とのなす角度を計算する第２計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記重心点までの距離、前記移動体からみた前記重心点の方向と前記移動体の進行方向とがなす角度、及び、前記重心点の方向と前記法線の方向とのなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。

上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角度を計算する第３計算部と、前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第１中点決定部と、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第４計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。

上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向のなす角度を計算する第５計算部と、前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第２中点決定部と、前記移動体からみた前記両端点それぞれの方向が平行であるとみなして、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第６計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。

上記の測定装置の他の一態様は、前記地物上の複数の位置のうちの１つを計測したときの前記移動体の位置である移動体位置を基準位置とし、前記複数の位置のうち前記基準位置以外の前記移動体位置で計測した位置の座標を、前記基準位置の座標系における座標に変換する座標変換部を備える。この態様では、地物を計測している時間内における移動体の移動を考慮して位置座標を補正することにより、移動体の進行方向の変化量を高精度で算出することができる。

上記の測定装置の他の一態様では、前記算出部は前記移動距離を算出し、前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される車速センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第１情報を補正する第１補正部を更に備える。この態様では、算出される移動距離に基づいて、移動体の車速センサを適切に補正することができる。

上記の測定装置の他の一態様では、前記算出部は前記移動距離を算出し、前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される加速度センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第２情報を補正する第２補正部を更に備える。この態様では、算出される移動距離に基づいて、移動体の加速度センサを適切に補正することができる。

上記の測定装置の他の一態様では、前記算出部は前記変化量を算出し、前記測定装置は、前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第３情報を補正する第３補正部を更に備える。この態様では、算出される進行方向の変化量に基づいて、移動体の角速度センサを適切に補正することができる。

本発明の他の好適な実施形態は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、を備える。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。

本発明の他の好適な実施形態では、コンピュータを備える測定装置によって実行されるプログラムは、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、として前記コンピュータを機能させる。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。このプログラムは、記憶媒体に記憶して利用することができる。

以下、図面を参照して本発明の好適な実施例について説明する。

［１］背景
従来のカーナビゲーション装置に搭載されている自車位置推定システムでは、車速センサから算出される速度や加速度センサの出力から算出される速度と、ジャイロセンサの出力から算出されるヨー角とを用いて車両の移動状態を測定し、ＧＰＳ情報と統合することにより現在位置を推定する複合航法システムが用いられている。複合航法システムによる自車位置推定精度向上のためには、車体速度とヨー角を正確に把握する必要がある。

車両の車体速度は、車速センサから出力される車軸回転パルスに基づく車輪回転速度に変換係数を乗算することにより得られる。また、車体速度は、加速度センサから出力される検出加速度と、加速度センサの感度係数及びオフセット係数とに基づいて算出することができる。さらに、車両のヨー角は、ジャイロセンサから出力される角速度と、ジャイロセンサの感度係数及びオフセット係数とに基づいて算出することができる。

一般的に、車速センサの変換係数、加速度センサの感度とオフセット、及び、ジャイロセンサの感度とオフセットは、温度や振動などによって変化する特性がある。そのため、それらの誤差を含んだ状態で車体速度やヨー角を算出すると、誤差が蓄積し、計算される車体速度やヨー角は実際の値から大きくずれてしまう。従って、車体速度やヨー角を高精度に推定するためには、車速センサの変換係数、加速度センサの感度とオフセット、ジャイロセンサの感度とオフセットを継続的に補正することが必要である。

従来のカーナビゲーション装置では、ＧＰＳ情報に基づくドップラー速度を用いて車速センサ出力から車体速度を求める変換係数や加速度センサの感度とオフセットの補正を行っており、ＧＰＳ情報の方位変化量を用いてジャイロセンサの感度とオフセットの補正を行っている。そのため、都市部のようなＧＰＳの受信状態の悪い環境では補正の精度が劣化する。また、トンネルや地下駐車場のようなＧＰＳ測位ができない環境では補正ができない。よって、ＧＰＳの受信状態に関わらず、継続的に変換係数や感度とオフセットの補正を行うことが要求される。

本実施例による係数更新装置は、ＧＰＳ情報を用いずに、外界センサによる１つの地物の計測結果に基づいて車両の移動距離とヨー角変化量を算出し、車速センサから車体速度を求めるための変換係数、加速度センサの感度とオフセット、及び、ジャイロセンサの感度とオフセットを補正する。また、係数更新装置は移動距離を所定時間で除算することにより車体速度Ｖを算出することができる。なお、外界センサとしては、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）が挙げられる。

［２］係数の推定方法
［２．１］車速センサから車体速度を求めるための変換係数の推定方法
真の車体速度Ｖ_（ｔ）と、車速センサから出力される車軸回転パルスに基づく車輪回転速度ω_ｐ（ｔ）との関係は、変換係数Ｋを用いて次のように表すことができる。

よって、まず、外界センサによる地物計測値を用いて車両の移動距離ΔＬを求め、それを時間間隔Δｔで除算して計測車体速度を算出する。得られた計測車体速度を真の車体速度Ｖ_（ｔ）とみなすと、計測車体速度Ｖ_（ｔ）と車速センサから得た車輪回転速度ω_ｐ（ｔ）とを用いて、式（１）により変換係数Ｋを推定することができる。なお、変換係数Ｋは本発明の第１情報に相当する。

［２．２］加速度センサの感度係数とオフセット係数の推定方法
真の車体加速度Ｖ_{＿ｄｏｔ（ｔ）}と、加速度センサで検出する加速度α_{ａｃ（ｔ）}との関係は、感度係数Ａとオフセット係数Ｂを用いて次のように表すことができる。

よって外界センサによる地物計測値を用いて車両の移動距離ΔＬを求め、それを時間間隔Δｔで除算して計測車体速度Ｖ_（ｔ）を算出し、１時刻前に同様な方法で求めた計測車体速度Ｖ_{（ｔ−１）}と時間間隔Δｔを用いて、計測車体加速度を算出する。

得られた計測車体加速度を真の車体加速度Ｖ_{＿ｄｏｔ（ｔ）}とみなし、次のように置くと、

以下の１次式で表すことができる。

従って、ｘ_ｔとｙ_ｔの複数のデータがあれば、逐次最小二乗法等により、感度係数Ａとオフセット係数Ｂを求めることができる。即ち、外界センサを用いて車両の移動距離ΔＬを取得できれば、加速度センサの感度とオフセットを推定することができる。なお、感度係数Ａとオフセット係数Ｂは本発明の第２情報に相当する。

［２．３］ジャイロセンサの感度係数とオフセット係数の推定方法
真の角速度Ψ_{＿ｄｏｔ（ｔ）}と、ジャイロセンサで検出する角速度ω_ｇ（ｔ）との関係は、感度係数Ｃとオフセット係数Ｄを用いて次のように表すことができる。

外界センサによる地物計測値から、車両のヨー角の変化量（以下、「ヨー角変化量」と呼ぶ。）ΔΨ_Ｌを取得できた場合、これをその時間間隔Δｔで除することにより、次のように角速度を算出する。

外界センサによる地物計測値から求めた角速度（「計測ヨーレート」とも呼ぶ。）を真の角速度とみなすと、次のように表すことができる。

よって、次のように置くと、

以下の１次式で表すことができる。

従って、ｘ_ｔとｙ_ｔの複数のデータがあれば、逐次最小二乗法等により、感度係数Ｃとオフセット係数Ｄを求めることができる。即ち、外界センサを用いて車両のヨー角変化量ΔΨを取得できれば、ジャイロセンサの感度とオフセットを推定することができる。なお、感度係数Ｃとオフセット係数Ｄは本発明の第３情報に相当する。

［３］移動距離とヨー角変化量の算出方法
以下、外界センサを用いて車両の移動距離ΔＬとヨー角変化量ΔΨを求める方法を説明する。図１は、１つの地物を用いて移動距離とヨー角変化量を算出する方法を説明する図である。なお、前提として、平面板状である道路標識を地物として利用するものとする。また、外界センサとしてＬｉＤＡＲを用いるものとする。以下の手順により、車両の移動距離ΔＬとヨー角変化量ΔΨを算出する。
（１）現在の車両の地点Ｐ_１において、所定の地物Ａまでの距離と方向をＬｉＤＡＲにより計測する。
（２）地物を検出した複数のビームから、地物の重心点又は中点の座標を求め、車両から地物までの距離ｒ_Ａ１と角度θ_Ａ１を求める。
（３）地物が平面板状であることを前提に、地物の法線ベクトルと、ＬｉＤＡＲのビームとの角度φ_１を算出する。
（４）所定時間Δｔ経過後、地点Ｐ_２において上記（１）〜（３）の処理を行い、距離ｒ_Ａ２、角度θ_Ａ２、角度φ_２を算出する。
（５）下記の式により角度φ_２とφ_１の差分Δφを求める。

（６）図１（Ａ）からわかるように、地物Ａと車両位置Ｐ_１と車両位置Ｐ_２で構成される三角形に対する余弦定理を用いることで移動距離ΔＬが求まる。また、ヨー角変化量ΔΨはθ_Ａ２から∠αを引いたものであり、その∠αはθ_Ａ１とΔφを足したものである。よって、距離ｒ_Ａ１、ｒ_Ａ２、角度θ_Ａ１、θ_Ａ２、Δφを用いて、下記の式により移動距離ΔＬとヨー角変化量ΔΨを算出する。

このように、ＬｉＤＡＲで１つの地物を検出することにより、車両の移動距離ΔＬとヨー角変化量ΔΨを算出することができる。ヨー角変化量ΔΨを積分することにより、ヨー角Ψを算出することができ、ヨー角変化量ΔΨを所定時間Δｔで除算することによりヨーレートΨ_{＿ｄｏｔ（ｔ）}を算出することができる。また、移動距離ΔＬを所定時間Δｔで除算することにより車体速度Ｖを算出することができ、移動距離ΔＬを積分することにより移動距離Ｌを算出することもできる。

［４］係数更新装置
次に、上記のようにして算出された移動距離ΔＬ及びヨー角変化量ΔΨを用いて、車速センサの変換係数Ｋ、加速度センサの感度係数Ａ及びオフセットＢ、ジャイロセンサの感度係数Ｃ及びオフセット係数Ｄを更新する係数更新装置について説明する。図２は、実施例による係数更新装置１０の構成を示すブロック図である。図示のように、係数更新装置１０は、車速センサ７と、加速度センサ８と、ジャイロセンサ９と、外界センサ（ＬｉＤＡＲ）１２と、地物計測部１３と、ビーム角度算出部１４と、係数計算部１５と、移動距離・ヨー角変化量計算部１６とを備える。なお、地物計測部１３、ビーム角度算出部１４、係数計算部１５、及び、移動距離・ヨー角変化量計算部１６は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。

車速センサ７は、車軸回転パルスに基づき計測される車輪回転速度ω_ｐ（ｔ）を係数計算部１５へ供給する。加速度センサ８は、車両の加速度α_{ａｃ（ｔ）}を検出し、係数計算部１５へ供給する。また、ジャイロセンサ９は、検出した角速度ω_ｇ（ｔ）を係数計算部１５へ供給する。

地物計測部１３は、外界センサ１２の出力に基づいて地物までの距離ｒ及び地物との角度θを計測して移動距離・ヨー角変化量計算部１６へ供給する。また、地物計測部１３は、距離ｒと角度θを、進行方向をｘ、横方向をｙ、高さ方向をｚとする直交座標（ｘ，ｙ，ｚ）に変換し、各計測点の座標としてビーム角度算出部１４へ供給する。

ビーム角度算出部１４は、外界センサ１２の出力に基づいて、外界センサ１２からのビームと、平面板状である地物の法線ベクトルとのなす角度φ（以下、「ビーム角度」とも呼ぶ。）を算出し、移動距離・ヨー角変化量計算部１６へ供給する。移動距離・ヨー角変化量計算部１６は、異なる時刻における地物までの距離ｒと、地物との角度θと、ビーム角度φとを用いて、前述の式（１１）、（１２）により、移動距離ΔＬ及びヨー角変化量ΔΨを算出し、係数計算部１５へ供給する。

係数計算部１５は、まず、移動距離ΔＬを所定時間Δｔで除算して車体速度Ｖ_（ｔ）を算出する。次に、係数計算部１５は、車体速度Ｖ_（ｔ）と車速センサ７から出力された車輪回転速度ω_ｐ（ｔ）に基づいて、式（１）により車速センサ７の変換係数Ｋを算出し、更新する。また、係数計算部１５は、車体速度Ｖ_（ｔ）と１時刻前の車体速度Ｖ_{（ｔ−１）}と時間間隔Δｔと、加速度センサ８から出力された加速度α_{ａｃ（ｔ）}に基づいて、式（２）〜（３）により加速度センサ８の感度係数Ａ及びオフセットＢを算出し、更新する。さらに、係数計算部１５は、ジャイロセンサ９から供給された角速度ω_ｇ（ｔ）と、移動距離・ヨー角変化量計算部１６から供給されたヨー角変化量ΔΨに基づいて、式（７）〜（８）によりジャイロセンサの感度係数Ｃ及びオフセット係数Ｄを算出し、更新する。

上記の構成において、地物計測部１３は本発明の第１取得部の一例であり、ビーム角度算出部１４は本発明の第２取得部、第１〜第２方向決定部、第１〜第６計算部、第１〜第２中点決定部の一例であり、係数計算部１５は本発明の第１〜第３補正部の一例であり、移動距離・ヨー角変化量計算部１６は本発明の算出部の一例である。また、ヨー角変化量は本発明における進行方向の変化量に相当する。

次に、上記の係数更新装置１０による係数更新処理について説明する。図３は、係数更新処理のフローチャートである。まず、係数更新装置１０は、外界センサ１２により１つの地物Ａを計測し、地物との距離ｒ及び地物との角度θを含む計測データを取得する（ステップＳ１０）。次に、係数更新装置１０は、それら計測データを用いて、ビーム角度φを算出する（ステップＳ１１）。なお、ビーム角度φの算出方法については後に詳しく説明する。

次に、係数更新装置１０は、地物Ａの１時刻前の計測データがあるか否かを判定する（ステップＳ１２）。１時刻前の計測データが無い場合（ステップＳ１２：ＮＯ）、係数更新装置１０は時刻をインクリメントし（ステップＳ１３）、処理はステップＳ１０へ戻る。

一方、１時刻前の計測データがある場合（ステップＳ１２：ＹＥＳ）、係数更新装置１０は、１時刻前の地物Ａの距離ｒ_Ａ１、角度θ_Ａ１、ビーム角度φ_１と、現在時刻の地物の距離ｒ_Ａ２、角度θ_Ａ２、ビーム角度φ_２を用いて、式（１１）、（１２）によりヨー角変化量ΔΨ及び移動距離ΔＬを算出する（ステップＳ１４）。そして、係数更新装置１０は、移動距離ΔＬと時間間隔Δｔから計測車体速度Ｖ_（ｔ）を算出し、ヨー角変化量ΔΨと時間間隔Δｔから、式（７）を用いて真の角速度（計測ヨーレート）Ψ_{＿ｄｏｔ（ｔ）}を算出する（ステップＳ１５）。

次に、係数更新装置１０は、得られた計測車体速度Ｖ_（ｔ）と車速センサの出力ω_ｐ（ｔ）から式（１）により変換係数Ｋを算出して更新し、得られた計測車体速度Ｖ_（ｔ）と１時刻前の計測車体速度Ｖ_{（ｔ−１）}と時間間隔Δｔと加速度センサの出力α_{ａｃ（ｔ）}から式（２）により加速度センサの感度係数Ａとオフセット係数Ｂを算出して更新し、さらに得られた計測ヨーレートΨ_{＿ｄｏｔ（ｔ）}とジャイロセンサの出力ω_ｇ（ｔ）から式（８）により感度係数Ｃとオフセット係数Ｄを算出して更新する（ステップＳ１６）。

［５］ビーム角度の算出方法
次に、上記のビーム角度φの算出方法について詳しく説明する。

［５．１］第１実施例
まず、ビーム角度φの算出方法の第１実施例を説明する。第１実施例は、ビーム角度φを正確に求める方法である。

［５．１．１］概要説明
ＬｉＤＡＲによる計測によって、車両位置から地物までの距離ｒと、水平角度θと、垂直角度とが得られる。係数更新装置１０のビーム角度算出部１４は、図４（Ａ）に示すように、ＬｉＤＡＲにより得られる距離と角度データを、車両の進行方向をｘ、横方向をｙ、高さ方向をｚとする直交座標（ｘ，ｙ，ｚ）に変換する。

図４（Ｂ）は、対象となる地物の例として、円形の道路標識をＬｉＤＡＲにより計測した様子を示す。ＬｉＤＡＲの複数のビームにより、道路標識上に複数の計測点が形成される。ビーム角度算出部１４は、対象とする地物に当たった複数のビームの計測値を取得する。このとき、地物の縁にかかるビームは反射率が低くなるため検出されないので、ビーム角度算出部１４は、所定の閾値以上の反射率を有する計測点を道路標識上の計測点であると判定する。これにより、ビーム角度算出部１４は、道路標識上に存在する複数の計測点を取得する。図４（Ｂ）の白抜きの丸は、閾値以下で検出されなかったビームをイメージしている。

次に、ビーム角度算出部１４は、図４（Ｂ）に示すように、各計測点の座標（ｘ_１，ｙ_１，ｚ_１）、（ｘ_２，ｙ_２，ｚ_２）、・・（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）から、重心点の座標Ｇ（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）を求める。次に、図５に示すように、ビーム角度算出部１４は、重心点の座標Ｇ（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）を用いて、車両（原点）から重心点（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）までの方向ベクトルを求める。また、ビーム角度算出部１４は、各計測点の座標（ｘ_１，ｙ_１，ｚ_１）、（ｘ_２，ｙ_２，ｚ_２）、・・（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）から、最小２乗法により、道路標識の表面を規定する平面の式を生成し、その平面の法線ベクトルを求める。そして、ビーム角度算出部１４は、平面の法線ベクトル及び車両から重心点までの方向ベクトルのｘｙ成分から、２つのベクトルのなす角度であるビーム角度φを求める。

［５．１．２］詳細説明
次に、上記の算出方法を詳しく説明する。まず、円形の道路標識を含む平面の方程式の求め方を説明する。具体的に、ビーム角度算出部１４は、道路標識上に存在する複数の計測点（ｘ_ｋ，ｙ_ｋ，ｚ_ｋ）から、平面の方程式

の係数ａ、ｂ、ｃ、ｄを求める。いま、法線ベクトルの長さを１として、

とする。式（１３）を変形して、

を得る。

式（１５）に（ｘ_ｋ，ｙ_ｋ）を代入して計算する右辺の値と、ｚ_ｋの値の誤差の総和ｅは以下の式となる。

最小二乗法により、式（１６）を最小にする（α，β，γ）を求める。式（１６）を最小にする（α，β，γ）は、それぞれで式（１６）を偏微分すると０になる。

式（１７）、（１８）、（１９）をまとめると以下のようになる。

式（２０）により、逆行列を用いた演算により、以下のように（α，β，γ）を求めることができる。

式（１５）より、

であるから、

となる。

式（２２）を式（１４）に代入すると、

となり、よって

となる。

これを式（２２）に代入するとａ，ｂ，ｄが求まり、最終的に以下の結果を得る。

次に、平面の法線ベクトルと直線の方向ベクトルについて説明する。まず、一般形で示される平面の方程式

の法線ベクトルＮ_Ｖは以下である（図６（Ａ）参照）。

一方、一般形で示される直線の方程式

の方向ベクトルＤ_Ｖは以下である（図６（Ｂ）参照）。

なお、（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）と（ｘ_０，ｙ_０，ｚ_０）の２点を通る直線の方程式は

で与えられるため、（ｘ_０，ｙ_０，ｚ_０）が原点（０，０，０）である場合、

となるので、方向ベクトルＤ_Ｖは以下となる（図６（Ｃ）参照）。

次に、法線ベクトルと方向ベクトルのなす角、即ちビーム角φの算出について説明する。本実施例では、車両の高さ方向の成分を考慮していないので、ｚ軸成分を無視すればよい。したがって、図７（Ａ）に示すように、法線ベクトルと方向ベクトルをｘｙ平面に投影したベクトルをそれぞれＮ_Ｖ’、Ｄ_Ｖ’とすると、法線ベクトルＮ_Ｖ’を角度φ回転させて｜Ｄ_Ｖ’｜／｜Ｎ_Ｖ’｜倍すると方向ベクトルＤ_Ｖ’になるため、以下の式で表せる。

式（２５）を計算すると、以下の２式が得られる。

式（２６）、（２７）に対し、“−式（２６）×ｂ＋式（２７）×ａ”によりｃｏｓφを削除すると、

となるため、以下の式からビーム角度φが求められる。

なお、本実施例では基本的に道路標識などの平面板状の地物をおもて面から計測することを前提としており、ビーム角度φの範囲は−９０［ｄｅｇ］〜９０［ｄｅｇ］となるので、式（２８）のようにビーム角度φをｓｉｎφで算出することが好ましい。

上記のように、第１実施例は、最小２乗法で平面の方程式を求めているため、図７（Ｂ）に示すように、地物が多少曲面形状であったとしても法線ベクトルを求めることができる。さらに、図７（Ｃ）に示すように、地物の面に多少の凹凸があったとしても、最小２乗法による平面の方程式を作成することができ、法線ベクトルを求めることができる。

［５．１．３］算出処理
次に、ビーム角度φの算出処理について説明する。図８は、第１実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。

まず、ビーム角度算出部１４は、ＬｉＤＡＲにより地物Ａを計測して得たｎ個のデータ（ｘ_ｋ，ｙ_ｋ，ｚ_ｋ）から、重心点Ｇ（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）を算出し、車両から重心点Ｇへの方向ベクトルＤ_Ｖ（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）とする。また、ビーム角度算出部１４は、車両から重心点Ｇまでの距離ｒ_Ａ［ｔ］と角度θ_Ａ［ｔ］を求める（ステップＳ２１）。

次に、ビーム角度算出部１４は、ｎ個のデータ（ｘ_ｋ，ｙ_ｋ，ｚ_ｋ）から、最小２乗法を用いて、平面の方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０を作成し、平面の法線ベクトルＮ_Ｖ（ａ，ｂ，ｃ）を求める（ステップＳ２２）。そして、ビーム角度算出部１４は、法線ベクトルＮ_Ｖと方向ベクトルＤ_Ｖのｘｙ成分から、２つのベクトルがなす角度φを算出する（ステップＳ２３）。以上でビーム角度φの算出処理は終了する。

［５．２］第２実施例
次に、ビーム角度φの算出方法の第２実施例を説明する。第２実施例は、ビーム角度φを簡易的に求める方法である。

［５．２．１］算出方法
図９は、第２実施例によるビーム角度φの算出方法を示す。図９において、車両の位置を原点Ｏとし、平面板状の地物の両端が点Ｃ、Ｅであるとする。ビーム角度算出部１４は、まず、地物の計測データから、その両端点Ｃ、Ｅまでの距離、即ち、ＯＣ長、ＯＥ長と、その間の角度Δθを抽出する。

次に、ビーム角度算出部１４は、三角形△ＯＣＥに対する余弦定理により、下記のようにＣＥ長を求める。

次に、ビーム角度算出部１４は、求めたＣＥ長を利用して余弦定理を用いて、下記のように∠ＯＣＥの角度を求める。

次に、ビーム角度算出部１４は、辺ＣＥの中点を点Ｄ（ＣＤ長＝ＤＥ長）とし、三角形△ＯＣＤに対する余弦定理によりＯＤ長を求める。

次に、ビーム角度算出部１４は、求めたＯＤ長を利用して、∠ＣＤＯの角度を求める。

そして、ビーム角度算出部１４は、９０度から∠ＣＤＯの角度を引いて角度φを求める。

こうして、ビーム角度φを簡易的に求めることができる。この方法の利点は、第１実施例よりも計算量が少なくて済むことである。
［５．２．２］算出処理
次に、ビーム角度φの算出処理について説明する。図１０は、第２実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。

まず、ビーム角度算出部１４は、地物Ａを検出した複数のビームのうち、横方向に最も離れた２つのビームを選択して地物の両端点Ｃ、Ｅを求め、両端点Ｃ、Ｅまでの距離ＯＣ、ＯＥ及びその間の角度Δθを取得する（ステップＳ３１）。次に、ビーム角度算出部１４は、両端点Ｃ、Ｅの中点Ｄを求め、車両からその中点Ｄまでの距離（ＯＤ長）求める（ステップＳ３２）。次に、ビーム角度算出部１４は、車両と中点Ｄとを結んだ直線ＯＤと地物の平面とがなす角度（∠ＣＤＯ）を求める（ステップＳ３３）。そして、ビーム角度算出部１４は、求めた角度（∠ＣＤＯ）を９０度から減算して角度φを算出する（ステップＳ３４）。以上でビーム角度φの算出処理は終了する。

［５．３］第３実施例
次に、ビーム角度φの算出方法の第３実施例を説明する。第３実施例は、ビーム角度φを近似的に求める方法である。

［５．３．１］算出方法
図１１は、第３実施例によるビーム角度φの算出方法を示す。図１１において、車両の位置を原点Ｏとし、平面板状の地物の両端が点Ｃ、Ｅであるとする。ビーム角度算出部１４は、まず地物の計測データから、その両端点Ｃ、Ｅまでの距離、即ち、ＯＣ長、ＯＥ長と、その間の角度Δθを抽出する。

次に、直線ＯＥ上に、ＯＣ長と同じ長さとなる点Ｆを設けると、

となる。

また、三角形△ＯＣＦは二等辺三角形であるので、

となる。

ここで、車両から地物までの距離が数メートル以上であれば、直線ＯＣと直線ＯＥはほぼ平行とみなすことができる。よって、三角形△ＣＦＥを、∠ＣＦＥが直角である直角三角形とみなすと、角度φは以下のように得られる。

この方法の利点は、第２実施例よりもさらに計算量が少なくて済むことである。
［５．３．２］算出処理
次に、ビーム角度φの算出処理について説明する。図１２は、第３実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。

まず、ビーム角度算出部１４は、地物Ａを検出した複数のビームのうち横方向に最も離れた２つのビームを選択して地物の両端点Ｃ、Ｅを求め、両端点Ｃ、Ｅまでの距離ＯＣ、ＯＥ及びその間の角度Δθを取得する（ステップＳ４１）。次に、ビーム角度算出部１４は、車両から端点Ｅまでの直線ＯＥ上に点Ｆを設け、前述の式（２９）により点Ｆから端点ＥまでのＦＥ長を算出する（ステップＳ４２）。

次に、ビーム角度算出部１４は、ＯＣ長と角度Δθを用いて、式（３０）によりＣＦ長を算出する（ステップＳ４３）。そして、ビーム角度算出部１４は、ＦＥ長とＣＦ長を用いて、式（３１）により角度φを算出する（ステップＳ４４）。以上でビーム角度φの算出処理は終了する。

［６］地物計測中の車両の移動を考慮した地物の計測点の座標計算
上述したヨー角変化量ΔΨの算出は、地物の法線ベクトルと車両から地物までの方向ベクトルとがなす角度φを求めることにより行われる。しかし、水平方向にスキャンするタイプのＬｉＤＡＲの場合、対象とする地物の左端と右端を計測する時刻が異なるので、その時間内に車両自身が移動すると、計測の基準位置と基準方位が異なることになる。よって、計測の基準位置と基準方位を合わせないままでは、角度φの算出に含まれる誤差が大きくなってしまう。

図１３（Ａ）は、ＬｉＤＡＲにより１つの地物を計測する間に車両が移動する様子を示す。地物の左端を計測するときには車両は位置Ｐ_１にあり、地物の右端を計測するときには車両は位置Ｐ_ｎに移動している。

図１３（Ｂ）は、図１３（Ａ）の例においてＬｉＤＡＲで計測した地物上の点の座標（ｒ、θ）を、車両の位置を原点とした（ｘ，ｙ）座標に変換したものを示す。車両の移動に伴って、地物までの距離と方向が変化するため、地物の平面が歪んだものとなってしまう。その結果、地物の中心（又は重心）座標と法線ベクトルとがずれてしまう。

図１４（Ａ）に示すように、水平スキャン型のＬｉＤＡＲのスキャン周期がＴ［ｓ］の場合、スキャン角周波数ω［ｒａｄ／ｓ］は、ω＝２π／Ｔとなり、ｒ［ｍ］先でのビームが移動する速度はｒω［ｍ／ｓ］となる。そこに幅Ｗ［ｍ］の道路標識があった場合、その道路標識をスキャンする時間ΔＴは以下となる。

つまり、図１４（Ａ）のようにＬｉＤＡＲのスキャンが時計回りであるとすると、道路標識の左端と右端の計測にΔＴの時間差があることになる。仮に、ＬｉＤＡＲのスキャン周期が１００［ｍｓ］の場合、１０［ｍ］先の６０［ｃｍ］幅の道路標識をビームが計測する時間は、式（３２）にＴ＝０．１、ｒ＝１０、Ｗ＝０．６を代入して、ΔＴ＝０．９５５［ｍｓ］と求められる。車両の速度が１００［ｋｍ／ｈ］の時を想定すると、ΔＴ＝０．９５５［ｍｓ］の間に、車両は２．６５［ｃｍ］進むことになる。また、仮に道路標識がもっと近く、２［ｍ］先の位置にあるとすると、ｒ＝２を代入して、ΔＴ＝４．７７５［ｍｓ］となり、車両は１３．２６［ｃｍ］進むことになる。

従って、車両の速度が速いほど、また対象の地物が近いほど、地物の計測時間内における車両の移動距離が長くなる。その移動距離を考慮せずに計算を行うと、前述のように誤差が大きくなってしまう。よって、車体速度とヨー角変化量の高精度な算出のためには、地物の計測時間内における車両の移動を考慮する必要がある。

上記の考察で見積もりした道路標識の計測時間は数ｍｓ程度であるため、その時間内では車両の速度やヨーレートの変化はほぼ無いとみなすことができる。よって、前の時刻で計算された自車の速度とヨーレートの値を用いて、現在の地物計測中における車両の方向変化分と位置変化分を算出し、その変化分を用いて各計測点の座標を求めればよい。

例えば、図１４（Ｂ）において、Ｐ_ｋ点で計測した地物の座標を、Ｐ_１点の座標系を用いた値に変換する。順番としては、まずＰ_ｋ点において破線で示す座標軸へと座標を回転させ、その後、ｘ方向とｙ方向の位置の差分（Δｘ_ｋ，Δｙ_ｋ）を加算する。

まず、Ｐ_ｋ点で計測した距離ｒ_ｋと角度θ_ｋから、以下のようにＰ_ｋ点を原点とするｘｙ座標系の値（ｘ_ｋ，ｙ_ｋ）が得られているものとする。

ヨーレートが一定という前提があるため、Ｐ_１点からＰ_ｋ点までのΔｔ_ｋ間の方向の変化量Ψ_ｋは次式となる。

また、仮想的な中心点Ｏからの回転半径Ｒを想定すると、

となるため、Δｘ_ｋとΔｙ_ｋは以下のように表すことができる。

よって、Ｐ_ｋ点で計測する地物の座標は、Ｐ_１点を基準位置にすると、以下で計算される。

よって、地物のｎ個の計測点に対して上記の計算を行い、算出した複数の座標点を用いて地物の重心点又は中点、及び、法線ベクトルを求めて角度φを算出すればよい。

図１５は、地物計測時間内の車両の移動を考慮した、地物の計測点の座標計算処理のフローチャートである。この処理は、前述の地物計測部１３により行われる。まず、地物計測部１３は、前の時刻で計測（あるいは推定）した自車の速度Ｖ_（ｔ-1）とヨーレートΨ_{＿ｄｏｔ（ｔ−１）}を読み出し（ステップＳ５０）、平面形状の道路標識である地物Ａを対象としたｎ点の計測データを取得する（ステップＳ５１）。次に、地物計測部１３は、地物の計測データの左端のデータＰ_１を基準の位置及び方位に設定する（ステップＳ５２）。

次に、地物計測部１３は、係数ｋに「１」をセットし（ステップＳ５３）、係数ｋが計測データ数ｎ以下か否かを判定する（ステップＳ５４）。係数ｋが計測データ数ｎ以下である場合（ステップＳ５４：ＹＥＳ）、地物計測部１３は点Ｐ_１から点Ｐ_ｋまでの時間差Δｔ_ｋを求める（ステップＳ５５）。

次に、地物計測部１３は、前述の式（３３）を用いて、Ｐ_１点からＰ_ｋ点までの角度変化Ψ_ｋを求め（ステップＳ５６）、さらに式（３４）、（３５）を用いてＰ_１点からＰ_ｋ点までの位置の差（Δｘ_ｋ，Δｙ_ｋ）を求める（ステップＳ５７）。そして、地物計測部１３は、式（３６）を用いて、Ｐ_１点を基準とした座標系におけるＰ_ｋ点での地物の座標を算出する（ステップＳ５８）。

次に、地物計測部１３は、係数ｋをインクリメントし（ステップＳ５９）、ステップＳ５４へ戻る。そして、係数ｋが計測データ数ｎを超えると（ステップＳ５４：ＮＯ）、処理は終了する。こうして得られた計測データを用いてビーム角度φを算出することにより、地物の計測時間内の車両の移動分を補正し、移動距離ΔＬ及びヨー角変化量を高精度で算出することが可能となる。また、ヨー角変化量ΔΨを積分することにより、ヨー角Ψを算出することができ、ヨー角変化量ΔΨを所定時間Δｔで除算することによりヨーレートΨ_{＿ｄｏｔ（ｔ）}を算出することができる。さらに、移動距離ΔＬを所定時間Δｔで除算することにより車体速度Ｖを算出することができ、移動距離ΔＬを積分することにより移動距離Ｌを算出することもできる。

［７］変形例
地物の平面に対してＬｉＤＡＲのビームが小さな角度で入射すると、距離計測値の誤差が大きくなるため、その状態で算出する車体速度とヨー角変化量にも誤差が大きくなる。従って、地物の法線ベクトルと車両から地物までの方向ベクトルとのなす角度φが大きいときは、誤差が大きいと判断し、車速センサの変換係数や、加速度センサとジャイロセンサの感度係数とオフセット係数を算出する際、重み付け値を小さくして平均化処理を行うことが好ましい。これにより、誤差の含まれる算出値の影響を小さくして、感度係数やオフセット係数を算出することができる。

本発明は、移動体に搭載する装置に利用することができる。

７ 車速センサ
８ 加速度センサ
９ ジャイロセンサ
１２ 外界センサ
１３ 地物計測部
１４ ビーム角度算出部
１５ 係数計算部
１６ ヨー角変化量計算部

Claims (12)

1. 第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、
   前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、
   を備えることを特徴とする測定装置。
2. 前記少なくとも２つの位置の座標を取得する第２取得部を更に備え、
   前記算出部は、前記第１時刻及び前記第２時刻における、（ｉ）前記移動体から前記少なくとも２つの位置までのそれぞれの距離及び前記少なくとも２つの位置の座標に基づいて算出される、前記地物の表面に対する法線と前記移動体からみた前記地物上の１つの位置の方向とがなす角度、並びに（ｉｉ）前記移動体からみた前記１つの位置の方向と前記移動体の進行方向とがなす角度、に基づき前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
3. 前記第１取得部は、
   前記移動体から前記１の地物の少なくとも３つの位置までの距離及び前記移動体から見た前記少なくとも３つの位置の方向から、前記少なくとも３つの位置の座標を計算する第１計算部と、
   前記少なくとも３つの位置の座標に基づいて前記地物の表面の重心点の座標を計算し、前記移動体からみた前記重心点の方向を決定する第１方向決定部と、
   前記少なくとも３つの位置の座標に基づいて前記地物の表面を規定する平面の式を計算し、前記平面の式に基づいて前記地物の表面に対する法線の方向を決定する第２方向決定部と、
   前記重心点の方向と前記法線の方向とのなす角度を計算する第２計算部と、を備え、
   前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記重心点までの距離、前記移動体からみた前記重心点の方向と前記移動体の進行方向とがなす角度、及び、前記重心点の方向と前記法線の方向とのなす角度に基づき、前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
4. 前記第１取得部は、
   前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角度を計算する第３計算部と、
   前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第１中点決定部と、
   前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第４計算部と、を備え、
   前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づき、前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
5. 前記第１取得部は、
   前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向のなす角度を計算する第５計算部と、
   前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づき前記１の地物の中点の座標を決定する第２中点決定部と、
   前記移動体からみた前記両端点それぞれの方向が平行であるとみなして、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第６計算部と、を備え、
   前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
6. 前記地物上の複数の位置のうちの１つを計測したときの前記移動体の位置である移動体位置を基準位置とし、前記複数の位置のうち前記基準位置以外の前記移動体位置で計測した位置の座標を、前記基準位置の座標系における座標に変換する座標変換部を備えることを特徴とする請求項１乃至５のいずれか一項に記載の測定装置。
7. 前記算出部は前記移動距離を算出し、
   前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される車速センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第１情報を補正する第１補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
8. 前記算出部は前記移動距離を算出し、
   前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される加速度センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第２情報を補正する第２補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
9. 前記算出部は前記変化量を算出し、
   前記測定装置は、前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第３情報を補正する第３補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
10. 測定装置により実行される測定方法であって、
    第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、
    前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、
    を備えることを特徴とする測定方法。
11. コンピュータを備える測定装置によって実行されるプログラムであって、
    第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、
    前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、
    として前記コンピュータを機能させることを特徴とするプログラム。
12. 請求項１１に記載のプログラムを記憶した記憶媒体。

Images