(57)【特許請求の範囲】
【請求項１】
第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得部と、
前記第１取得部の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす第２角度と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部と、
を備えることを特徴とする測定装置。
【請求項２】
前記第１角度は、前記法線と前記移動体からみた前記地物上の１つの位置の方向とがなす角度であり、前記第２角度は、前記移動体からみた前記１つの位置の方向と前記移動体の進行方向とがなす角度であることを特徴とする請求項１に記載の測定装置。
【請求項３】
前記測定装置は、
前記移動体から前記１の地物上の少なくとも３つの位置までの距離及び前記移動体から見た前記少なくとも３つの位置の方向から、前記少なくとも３つの位置の座標を計算する第１計算部と、
前記少なくとも３つの位置の座標に基づいて前記地物の表面の重心点の座標を計算し、前記移動体からみた前記重心点の方向を決定する第１方向決定部と、
前記少なくとも３つの位置の座標に基づいて前記地物の表面を規定する平面の式を計算し、前記平面の式に基づいて前記地物の表面に対する法線の方向を決定する第２方向決定部と、
前記重心点の方向と前記法線の方向とのなす角度を計算する第２計算部と、を備え、
前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体からみた前記重心点の方向と前記移動体の進行方向とがなす角度、及び、前記重心点の方向と前記法線の方向とのなす角度に基づき、前記移動体の進行方向の変化量を算出することを特徴とする請求項１に記載の測定装置。
【請求項４】
前記測定装置は、
前記移動体から前記１の地物上の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角度を計算する第３計算部と、
前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第１中点決定部と、
前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第４計算部と、を備え、
前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記移動体からみた前記中点の方向と前記法線の方向とがなす角度に基づき、前記移動体の進行方向の変化量を算出することを特徴とする請求項１に記載の測定装置。
【請求項５】
前記測定装置は、
前記移動体から前記１の地物上の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向のなす角度を計算する第５計算部と、
前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づき前記１の地物の中点の座標を決定する第２中点決定部と、
前記移動体からみた前記両端点それぞれの方向が平行であるとみなして、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第６計算部と、を備え、
前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記移動体からみた前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動体の進行方向の変化量を算出することを特徴とする請求項１に記載の測定装置。
【請求項６】
前記地物上の複数の位置のうちの１つを計測したときの前記移動体の位置である移動体位置を基準位置とし、前記複数の位置のうち前記基準位置以外の前記移動体位置で計測した位置の座標を、前記基準位置の座標系における座標に変換する座標変換部を備えることを特徴とする請求項１乃至５のいずれか一項に記載の測定装置。
【請求項７】
前記測定装置は、前記移動体の進行方向の変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第３情報を補正する第３補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
【請求項８】
測定装置により実行される測定方法であって、
第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得工程と、
前記第１取得工程の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす第２角度と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出工程と、
を備えることを特徴とする測定方法。
【請求項９】
コンピュータを備える測定装置によって実行されるプログラムであって、
第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得部、
前記第１取得部の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす第２角度と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部、
として前記コンピュータを機能させることを特徴とするプログラム。
【請求項１０】
請求項９に記載のプログラムを記憶した記憶媒体。
【請求項１１】
第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得部と、
前記第１取得部の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体から前記地物までの距離と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離を算出する算出部と、
を備えることを特徴とする測定装置。
【請求項１２】
前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される車速センサ及び加速度センサの少なくとも一方の前記第２時刻における出力結果から前記移動体の速度を算出するための第１情報を補正する第１補正部を更に備えることを特徴とする請求項１１に記載の測定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の進行方向の変化を検出する技術に関する。
【背景技術】
【０００２】
移動体の前方に存在する物体を認識して、移動体のヨーレートを補正する手法が知られている。特許文献１は、移動体の前方に存在する固定物を認識し、その固定物が自車両に対して相対的に移動する移動軌跡に基づいて、ヨーレートセンサのずれ量を補正する手法を記載している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−６６７７７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載の手法では、上記の移動軌跡の他に、車速、旋回半径からのずれ量、固定物が横方向に相対的に移動する横移動量など、推定ヨーレートの算出に用いるパラメータが多く、これらのパラメータを取得するセンサのずれ量も考慮しなくてはならない。【０００５】
本発明が解決しようとする課題としては、上記のものが例として挙げられる。本発明は、１つのセンサで移動体の移動量とヨー角を算出し、移動距離と進行方向の変化量を得ることが可能な測定装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
請求項１に記載の発明は、測定装置であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす第２角度と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部と、を備えることを特徴とする。
【０００７】
請求項８に記載の発明は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得工程と、前記第１取得工程の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす第２角度と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出工程と、を備えることを特徴とする。
【０００８】
請求項９に記載の発明は、コンピュータを備える測定装置によって実行されるプログラムであって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得部、前記第１取得部の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす第２角度と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部、として前記コンピュータを機能させることを特徴とする。
請求項１１に記載の発明は、測定装置であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の複数の位置までのそれぞれの距離と、前記移動体からみた前記複数の位置の方向と、をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき算出される、前記第１時刻及び前記第２時刻それぞれにおける、前記地物の表面に対する法線と前記移動体からみた前記地物の方向とがなす第１角度と、前記移動体から前記地物までの距離と、に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離を算出する算出部と、を備えることを特徴とする。
【図面の簡単な説明】
【０００９】
【図１】１つの地物を用いて移動距離とヨー角変化量を算出する方法を示す。
【図２】実施例による係数更新装置の構成を示すブロック図である。
【図３】係数更新処理のフローチャートである。
【図４】第１実施例によるビーム角度の算出方法を説明する図である。
【図５】第１実施例によるビーム角度の算出方法を説明する他の図である。
【図６】第１実施例によるビーム角度の算出方法を説明する他の図である。
【図７】第１実施例によるビーム角度の算出方法を説明する他の図である。
【図８】第１実施例によるビーム角度の算出処理のフローチャートである。
【図９】第２実施例によるビーム角度の算出方法を説明する図である。
【図１０】第２実施例によるビーム角度の算出処理のフローチャートである。
【図１１】第３実施例によるビーム角度の算出方法を説明する図である。
【図１２】第３実施例によるビーム角度の算出処理のフローチャートである。
【図１３】地物の計測中における車両の移動による影響を示す。
【図１４】車両の移動を考慮した地物の計測点の座標計算方法を示す。
【図１５】地物の計測点の座標計算のフローチャートである。
【発明を実施するための形態】
【００１０】
本発明の好適な実施形態では、測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、を備える。
【００１１】
上記の測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得し、その取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。
【００１２】
上記の測定装置の一態様は、前記少なくとも２つの位置の座標を取得する第２取得部を更に備え、前記算出部は、前記第１時刻及び前記第２時刻における、（ｉ）前記移動体から前記少なくとも２つの位置までのそれぞれの距離及び前記少なくとも２つの位置の座標に基づいて算出される、前記地物の表面に対する法線と前記移動体からみた前記地物上の１つの位置の方向とがなす角度、並びに（ｉｉ）前記移動体からみた前記１つの位置の方向と前記移動体の進行方向とがなす角度、に基づき前記移動距離及び前記変化量のうち少なくとも一方を算出する。
【００１３】
上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の少なくとも３つの位置までの距離及び前記移動体から見た前記少なくとも３つの位置の方向から、前記少なくとも３つの位置の座標を計算する第１計算部と、前記少なくとも３つの位置の座標に基づいて前記地物の表面の重心点の座標を計算し、前記移動体からみた前記重心点の方向を決定する第１方向決定部と、前記少なくとも３つの位置の座標に基づいて前記地物の表面を規定する平面の式を計算し、前記平面の式に基づいて前記地物の表面に対する法線の方向を決定する第２方向決定部と、前記重心点の方向と前記法線の方向とのなす角度を計算する第２計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記重心点までの距離、前記移動体からみた前記重心点の方向と前記移動体の進行方向とがなす角度、及び、前記重心点の方向と前記法線の方向とのなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。
【００１４】
上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角度を計算する第３計算部と、前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第１中点決定部と、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第４計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。
【００１５】
上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向のなす角度を計算する第５計算部と、前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第２中点決定部と、前記移動体からみた前記両端点それぞれの方向が平行であるとみなして、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第６計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。
【００１６】
上記の測定装置の他の一態様は、前記地物上の複数の位置のうちの１つを計測したときの前記移動体の位置である移動体位置を基準位置とし、前記複数の位置のうち前記基準位置以外の前記移動体位置で計測した位置の座標を、前記基準位置の座標系における座標に変換する座標変換部を備える。この態様では、地物を計測している時間内における移動体の移動を考慮して位置座標を補正することにより、移動体の進行方向の変化量を高精度で算出することができる。
【００１７】
上記の測定装置の他の一態様では、前記算出部は前記移動距離を算出し、前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される車速センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第１情報を補正する第１補正部を更に備える。この態様では、算出される移動距離に基づいて、移動体の車速センサを適切に補正することができる。
【００１８】
上記の測定装置の他の一態様では、前記算出部は前記移動距離を算出し、前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される加速度センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第２情報を補正する第２補正部を更に備える。この態様では、算出される移動距離に基づいて、移動体の加速度センサを適切に補正することができる。
【００１９】
上記の測定装置の他の一態様では、前記算出部は前記変化量を算出し、前記測定装置は、前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第３情報を補正する第３補正部を更に備える。この態様では、算出される進行方向の変化量に基づいて、移動体の角速度センサを適切に補正することができる。
【００２０】
本発明の他の好適な実施形態は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、を備える。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。
【００２１】
本発明の他の好適な実施形態では、コンピュータを備える測定装置によって実行されるプログラムは、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、として前記コンピュータを機能させる。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。このプログラムは、記憶媒体に記憶して利用することができる。
【実施例】
【００２２】
以下、図面を参照して本発明の好適な実施例について説明する。【００２３】
［１］背景
従来のカーナビゲーション装置に搭載されている自車位置推定システムでは、車速センサから算出される速度や加速度センサの出力から算出される速度と、ジャイロセンサの出力から算出されるヨー角とを用いて車両の移動状態を測定し、ＧＰＳ情報と統合することにより現在位置を推定する複合航法システムが用いられている。複合航法システムによる自車位置推定精度向上のためには、車体速度とヨー角を正確に把握する必要がある。【００２４】
車両の車体速度は、車速センサから出力される車軸回転パルスに基づく車輪回転速度に変換係数を乗算することにより得られる。また、車体速度は、加速度センサから出力される検出加速度と、加速度センサの感度係数及びオフセット係数とに基づいて算出することができる。さらに、車両のヨー角は、ジャイロセンサから出力される角速度と、ジャイロセンサの感度係数及びオフセット係数とに基づいて算出することができる。【００２５】
一般的に、車速センサの変換係数、加速度センサの感度とオフセット、及び、ジャイロセンサの感度とオフセットは、温度や振動などによって変化する特性がある。そのため、それらの誤差を含んだ状態で車体速度やヨー角を算出すると、誤差が蓄積し、計算される車体速度やヨー角は実際の値から大きくずれてしまう。従って、車体速度やヨー角を高精度に推定するためには、車速センサの変換係数、加速度センサの感度とオフセット、ジャイロセンサの感度とオフセットを継続的に補正することが必要である。【００２６】
従来のカーナビゲーション装置では、ＧＰＳ情報に基づくドップラー速度を用いて車速センサ出力から車体速度を求める変換係数や加速度センサの感度とオフセットの補正を行っており、ＧＰＳ情報の方位変化量を用いてジャイロセンサの感度とオフセットの補正を行っている。そのため、都市部のようなＧＰＳの受信状態の悪い環境では補正の精度が劣化する。また、トンネルや地下駐車場のようなＧＰＳ測位ができない環境では補正ができない。よって、ＧＰＳの受信状態に関わらず、継続的に変換係数や感度とオフセットの補正を行うことが要求される。【００２７】
本実施例による係数更新装置は、ＧＰＳ情報を用いずに、外界センサによる１つの地物の計測結果に基づいて車両の移動距離とヨー角変化量を算出し、車速センサから車体速度を求めるための変換係数、加速度センサの感度とオフセット、及び、ジャイロセンサの感度とオフセットを補正する。また、係数更新装置は移動距離を所定時間で除算することにより車体速度Ｖを算出することができる。なお、外界センサとしては、ＬｉＤＡＲ（Ｌｉｇｈｔ  Ｄｅｔｅｃｔｉｏｎ  Ａｎｄ  Ｒａｎｇｉｎｇ）が挙げられる。【００２８】
［２］係数の推定方法
［２．１］車速センサから車体速度を求めるための変換係数の推定方法
真の車体速度Ｖ（ｔ）と、車速センサから出力される車軸回転パルスに基づく車輪回転速度ωｐ（ｔ）との関係は、変換係数Ｋを用いて次のように表すことができる。【００２９】
【数１】
0006707627.tif 000002
よって、まず、外界センサによる地物計測値を用いて車両の移動距離ΔＬを求め、それを時間間隔Δｔで除算して計測車体速度を算出する。得られた計測車体速度を真の車体速度Ｖ（ｔ）とみなすと、計測車体速度Ｖ（ｔ）と車速センサから得た車輪回転速度ωｐ（ｔ）とを用いて、式（１）により変換係数Ｋを推定することができる。なお、変換係数Ｋは本発明の第１情報に相当する。【００３０】
［２．２］加速度センサの感度係数とオフセット係数の推定方法
真の車体加速度Ｖ＿ｄｏｔ（ｔ）と、加速度センサで検出する加速度αａｃ（ｔ）との関係は、感度係数Ａとオフセット係数Ｂを用いて次のように表すことができる。【００３１】
【数２】
0006707627.tif 000003
よって外界センサによる地物計測値を用いて車両の移動距離ΔＬを求め、それを時間間隔Δｔで除算して計測車体速度Ｖ（ｔ）を算出し、１時刻前に同様な方法で求めた計測車体速度Ｖ（ｔ−１）と時間間隔Δｔを用いて、計測車体加速度を算出する。【００３２】
【数３】
0006707627.tif 000004
得られた計測車体加速度を真の車体加速度Ｖ＿ｄｏｔ（ｔ）とみなし、次のように置くと、【００３３】
【数４】
0006707627.tif 000005
以下の１次式で表すことができる。【００３４】
【数５】
0006707627.tif 000006
従って、ｘｔとｙｔの複数のデータがあれば、逐次最小二乗法等により、感度係数Ａとオフセット係数Ｂを求めることができる。即ち、外界センサを用いて車両の移動距離ΔＬを取得できれば、加速度センサの感度とオフセットを推定することができる。なお、感度係数Ａとオフセット係数Ｂは本発明の第２情報に相当する。【００３５】
［２．３］ジャイロセンサの感度係数とオフセット係数の推定方法
真の角速度Ψ＿ｄｏｔ（ｔ）と、ジャイロセンサで検出する角速度ωｇ（ｔ）との関係は、感度係数Ｃとオフセット係数Ｄを用いて次のように表すことができる。【００３６】
【数６】
0006707627.tif 000007
外界センサによる地物計測値から、車両のヨー角の変化量（以下、「ヨー角変化量」と呼ぶ。）ΔΨＬを取得できた場合、これをその時間間隔Δｔで除することにより、次のように角速度を算出する。【００３７】
【数７】
0006707627.tif 000008
外界センサによる地物計測値から求めた角速度（「計測ヨーレート」とも呼ぶ。）を真の角速度とみなすと、次のように表すことができる。【００３８】
【数８】
0006707627.tif 000009
よって、次のように置くと、【００３９】
【数９】
0006707627.tif 000010
以下の１次式で表すことができる。【００４０】
【数１０】
0006707627.tif 000011
従って、ｘｔとｙｔの複数のデータがあれば、逐次最小二乗法等により、感度係数Ｃとオフセット係数Ｄを求めることができる。即ち、外界センサを用いて車両のヨー角変化量ΔΨを取得できれば、ジャイロセンサの感度とオフセットを推定することができる。なお、感度係数Ｃとオフセット係数Ｄは本発明の第３情報に相当する。【００４１】
［３］移動距離とヨー角変化量の算出方法
以下、外界センサを用いて車両の移動距離ΔＬとヨー角変化量ΔΨを求める方法を説明する。図１は、１つの地物を用いて移動距離とヨー角変化量を算出する方法を説明する図である。なお、前提として、平面板状である道路標識を地物として利用するものとする。また、外界センサとしてＬｉＤＡＲを用いるものとする。以下の手順により、車両の移動距離ΔＬとヨー角変化量ΔΨを算出する。
（１）現在の車両の地点Ｐ１において、所定の地物Ａまでの距離と方向をＬｉＤＡＲにより計測する。
（２）地物を検出した複数のビームから、地物の重心点又は中点の座標を求め、車両から地物までの距離ｒＡ１と角度θＡ１を求める。
（３）地物が平面板状であることを前提に、地物の法線ベクトルと、ＬｉＤＡＲのビームとの角度φ１を算出する。
（４）所定時間Δｔ経過後、地点Ｐ２において上記（１）〜（３）の処理を行い、距離ｒＡ２、角度θＡ２、角度φ２を算出する。
（５）下記の式により角度φ２とφ１の差分Δφを求める。【００４２】
【数１１】
0006707627.tif 000012
（６）図１（Ａ）からわかるように、地物Ａと車両位置Ｐ１と車両位置Ｐ２で構成される三角形に対する余弦定理を用いることで移動距離ΔＬが求まる。また、ヨー角変化量ΔΨはθＡ２から∠αを引いたものであり、その∠αはθＡ１とΔφを足したものである。よって、距離ｒＡ１、ｒＡ２、角度θＡ１、θＡ２、Δφを用いて、下記の式により移動距離ΔＬとヨー角変化量ΔΨを算出する。【００４３】
【数１２】
0006707627.tif 000013
このように、ＬｉＤＡＲで１つの地物を検出することにより、車両の移動距離ΔＬとヨー角変化量ΔΨを算出することができる。ヨー角変化量ΔΨを積分することにより、ヨー角Ψを算出することができ、ヨー角変化量ΔΨを所定時間Δｔで除算することによりヨーレートΨ＿ｄｏｔ（ｔ）を算出することができる。また、移動距離ΔＬを所定時間Δｔで除算することにより車体速度Ｖを算出することができ、移動距離ΔＬを積分することにより移動距離Ｌを算出することもできる。【００４４】
［４］係数更新装置
次に、上記のようにして算出された移動距離ΔＬ及びヨー角変化量ΔΨを用いて、車速センサの変換係数Ｋ、加速度センサの感度係数Ａ及びオフセットＢ、ジャイロセンサの感度係数Ｃ及びオフセット係数Ｄを更新する係数更新装置について説明する。図２は、実施例による係数更新装置１０の構成を示すブロック図である。図示のように、係数更新装置１０は、車速センサ７と、加速度センサ８と、ジャイロセンサ９と、外界センサ（ＬｉＤＡＲ）１２と、地物計測部１３と、ビーム角度算出部１４と、係数計算部１５と、移動距離・ヨー角変化量計算部１６とを備える。なお、地物計測部１３、ビーム角度算出部１４、係数計算部１５、及び、移動距離・ヨー角変化量計算部１６は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。【００４５】
車速センサ７は、車軸回転パルスに基づき計測される車輪回転速度ωｐ（ｔ）を係数計算部１５へ供給する。加速度センサ８は、車両の加速度αａｃ（ｔ）を検出し、係数計算部１５へ供給する。また、ジャイロセンサ９は、検出した角速度ωｇ（ｔ）を係数計算部１５へ供給する。【００４６】
地物計測部１３は、外界センサ１２の出力に基づいて地物までの距離ｒ及び地物との角度θを計測して移動距離・ヨー角変化量計算部１６へ供給する。また、地物計測部１３は、距離ｒと角度θを、進行方向をｘ、横方向をｙ、高さ方向をｚとする直交座標（ｘ，ｙ，ｚ）に変換し、各計測点の座標としてビーム角度算出部１４へ供給する。【００４７】
ビーム角度算出部１４は、外界センサ１２の出力に基づいて、外界センサ１２からのビームと、平面板状である地物の法線ベクトルとのなす角度φ（以下、「ビーム角度」とも呼ぶ。）を算出し、移動距離・ヨー角変化量計算部１６へ供給する。移動距離・ヨー角変化量計算部１６は、異なる時刻における地物までの距離ｒと、地物との角度θと、ビーム角度φとを用いて、前述の式（１１）、（１２）により、移動距離ΔＬ及びヨー角変化量ΔΨを算出し、係数計算部１５へ供給する。【００４８】
係数計算部１５は、まず、移動距離ΔＬを所定時間Δｔで除算して車体速度Ｖ（ｔ）を算出する。次に、係数計算部１５は、車体速度Ｖ（ｔ）と車速センサ７から出力された車輪回転速度ωｐ（ｔ）に基づいて、式（１）により車速センサ７の変換係数Ｋを算出し、更新する。また、係数計算部１５は、車体速度Ｖ（ｔ）と１時刻前の車体速度Ｖ（ｔ−１）と時間間隔Δｔと、加速度センサ８から出力された加速度αａｃ（ｔ）に基づいて、式（２）〜（３）により加速度センサ８の感度係数Ａ及びオフセットＢを算出し、更新する。さらに、係数計算部１５は、ジャイロセンサ９から供給された角速度ωｇ（ｔ）と、移動距離・ヨー角変化量計算部１６から供給されたヨー角変化量ΔΨに基づいて、式（７）〜（８）によりジャイロセンサの感度係数Ｃ及びオフセット係数Ｄを算出し、更新する。【００４９】
上記の構成において、地物計測部１３は本発明の第１取得部の一例であり、ビーム角度算出部１４は本発明の第２取得部、第１〜第２方向決定部、第１〜第６計算部、第１〜第２中点決定部の一例であり、係数計算部１５は本発明の第１〜第３補正部の一例であり、移動距離・ヨー角変化量計算部１６は本発明の算出部の一例である。また、ヨー角変化量は本発明における進行方向の変化量に相当する。【００５０】
次に、上記の係数更新装置１０による係数更新処理について説明する。図３は、係数更新処理のフローチャートである。まず、係数更新装置１０は、外界センサ１２により１つの地物Ａを計測し、地物との距離ｒ及び地物との角度θを含む計測データを取得する（ステップＳ１０）。次に、係数更新装置１０は、それら計測データを用いて、ビーム角度φを算出する（ステップＳ１１）。なお、ビーム角度φの算出方法については後に詳しく説明する。【００５１】
次に、係数更新装置１０は、地物Ａの１時刻前の計測データがあるか否かを判定する（ステップＳ１２）。１時刻前の計測データが無い場合（ステップＳ１２：ＮＯ）、係数更新装置１０は時刻をインクリメントし（ステップＳ１３）、処理はステップＳ１０へ戻る。【００５２】
一方、１時刻前の計測データがある場合（ステップＳ１２：ＹＥＳ）、係数更新装置１０は、１時刻前の地物Ａの距離ｒＡ１、角度θＡ１、ビーム角度φ１と、現在時刻の地物の距離ｒＡ２、角度θＡ２、ビーム角度φ２を用いて、式（１１）、（１２）によりヨー角変化量ΔΨ及び移動距離ΔＬを算出する（ステップＳ１４）。そして、係数更新装置１０は、移動距離ΔＬと時間間隔Δｔから計測車体速度Ｖ（ｔ）を算出し、ヨー角変化量ΔΨと時間間隔Δｔから、式（７）を用いて真の角速度（計測ヨーレート）Ψ＿ｄｏｔ（ｔ）を算出する（ステップＳ１５）。【００５３】
次に、係数更新装置１０は、得られた計測車体速度Ｖ（ｔ）と車速センサの出力ωｐ（ｔ）から式（１）により変換係数Ｋを算出して更新し、得られた計測車体速度Ｖ（ｔ）と１時刻前の計測車体速度Ｖ（ｔ−１）と時間間隔Δｔと加速度センサの出力αａｃ（ｔ）から式（２）により加速度センサの感度係数Ａとオフセット係数Ｂを算出して更新し、さらに得られた計測ヨーレートΨ＿ｄｏｔ（ｔ）とジャイロセンサの出力ωｇ（ｔ）から式（８）により感度係数Ｃとオフセット係数Ｄを算出して更新する（ステップＳ１６）。【００５４】
［５］ビーム角度の算出方法
次に、上記のビーム角度φの算出方法について詳しく説明する。【００５５】
［５．１］第１実施例
まず、ビーム角度φの算出方法の第１実施例を説明する。第１実施例は、ビーム角度φを正確に求める方法である。【００５６】
［５．１．１］概要説明
ＬｉＤＡＲによる計測によって、車両位置から地物までの距離ｒと、水平角度θと、垂直角度とが得られる。係数更新装置１０のビーム角度算出部１４は、図４（Ａ）に示すように、ＬｉＤＡＲにより得られる距離と角度データを、車両の進行方向をｘ、横方向をｙ、高さ方向をｚとする直交座標（ｘ，ｙ，ｚ）に変換する。【００５７】
図４（Ｂ）は、対象となる地物の例として、円形の道路標識をＬｉＤＡＲにより計測した様子を示す。ＬｉＤＡＲの複数のビームにより、道路標識上に複数の計測点が形成される。ビーム角度算出部１４は、対象とする地物に当たった複数のビームの計測値を取得する。このとき、地物の縁にかかるビームは反射率が低くなるため検出されないので、ビーム角度算出部１４は、所定の閾値以上の反射率を有する計測点を道路標識上の計測点であると判定する。これにより、ビーム角度算出部１４は、道路標識上に存在する複数の計測点を取得する。図４（Ｂ）の白抜きの丸は、閾値以下で検出されなかったビームをイメージしている。【００５８】
次に、ビーム角度算出部１４は、図４（Ｂ）に示すように、各計測点の座標（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、・・（ｘｎ，ｙｎ，ｚｎ）から、重心点の座標Ｇ（ｘｇ，ｙｇ，ｚｇ）を求める。次に、図５に示すように、ビーム角度算出部１４は、重心点の座標Ｇ（ｘｇ，ｙｇ，ｚｇ）を用いて、車両（原点）から重心点（ｘｇ，ｙｇ，ｚｇ）までの方向ベクトルを求める。また、ビーム角度算出部１４は、各計測点の座標（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、・・（ｘｎ，ｙｎ，ｚｎ）から、最小２乗法により、道路標識の表面を規定する平面の式を生成し、その平面の法線ベクトルを求める。そして、ビーム角度算出部１４は、平面の法線ベクトル及び車両から重心点までの方向ベクトルのｘｙ成分から、２つのベクトルのなす角度であるビーム角度φを求める。【００５９】
［５．１．２］詳細説明
次に、上記の算出方法を詳しく説明する。まず、円形の道路標識を含む平面の方程式の求め方を説明する。具体的に、ビーム角度算出部１４は、道路標識上に存在する複数の計測点（ｘｋ，ｙｋ，ｚｋ）から、平面の方程式【００６０】
【数１３】
0006707627.tif 000014
の係数ａ、ｂ、ｃ、ｄを求める。いま、法線ベクトルの長さを１として、【００６１】
【数１４】
0006707627.tif 000015
とする。式（１３）を変形して、【００６２】
【数１５】
0006707627.tif 000016
を得る。【００６３】
式（１５）に（ｘｋ，ｙｋ）を代入して計算する右辺の値と、ｚｋの値の誤差の総和ｅは以下の式となる。【００６４】
【数１６】
0006707627.tif 000017
最小二乗法により、式（１６）を最小にする（α，β，γ）を求める。式（１６）を最小にする（α，β，γ）は、それぞれで式（１６）を偏微分すると０になる。【００６５】
【数１７】
0006707627.tif 000018
式（１７）、（１８）、（１９）をまとめると以下のようになる。【００６６】
【数１８】
0006707627.tif 000019
式（２０）により、逆行列を用いた演算により、以下のように（α，β，γ）を求めることができる。【００６７】
【数１９】
0006707627.tif 000020
式（１５）より、【００６８】
【数２０】
0006707627.tif 000021
であるから、【００６９】
【数２１】
0006707627.tif 000022
となる。【００７０】
式（２２）を式（１４）に代入すると、【００７１】
【数２２】
0006707627.tif 000023
となり、よって【００７２】
【数２３】
0006707627.tif 000024
となる。【００７３】
これを式（２２）に代入するとａ，ｂ，ｄが求まり、最終的に以下の結果を得る。【００７４】
【数２４】
0006707627.tif 000025
次に、平面の法線ベクトルと直線の方向ベクトルについて説明する。まず、一般形で示される平面の方程式【００７５】
【数２５】
0006707627.tif 000026
の法線ベクトルＮＶは以下である（図６（Ａ）参照）。【００７６】
【数２６】
0006707627.tif 000027
一方、一般形で示される直線の方程式【００７７】
【数２７】
0006707627.tif 000028
の方向ベクトルＤＶは以下である（図６（Ｂ）参照）。【００７８】
【数２８】
0006707627.tif 000029
なお、（ｘｇ，ｙｇ，ｚｇ）と（ｘ０，ｙ０，ｚ０）の２点を通る直線の方程式は【００７９】
【数２９】
0006707627.tif 000030
で与えられるため、（ｘ０，ｙ０，ｚ０）が原点（０，０，０）である場合、【００８０】
【数３０】
0006707627.tif 000031
となるので、方向ベクトルＤＶは以下となる（図６（Ｃ）参照）。【００８１】
【数３１】
0006707627.tif 000032
次に、法線ベクトルと方向ベクトルのなす角、即ちビーム角φの算出について説明する。本実施例では、車両の高さ方向の成分を考慮していないので、ｚ軸成分を無視すればよい。したがって、図７（Ａ）に示すように、法線ベクトルと方向ベクトルをｘｙ平面に投影したベクトルをそれぞれＮＶ’、ＤＶ’とすると、法線ベクトルＮＶ’を角度φ回転させて｜ＤＶ’｜／｜ＮＶ’｜倍すると方向ベクトルＤＶ’になるため、以下の式で表せる。【００８２】
【数３２】
0006707627.tif 000033
式（２５）を計算すると、以下の２式が得られる。【００８３】
【数３３】
0006707627.tif 000034
式（２６）、（２７）に対し、“−式（２６）×ｂ＋式（２７）×ａ”によりｃｏｓφを削除すると、【００８４】
【数３４】
0006707627.tif 000035
となるため、以下の式からビーム角度φが求められる。【００８５】
【数３５】
0006707627.tif 000036
なお、本実施例では基本的に道路標識などの平面板状の地物をおもて面から計測することを前提としており、ビーム角度φの範囲は−９０［ｄｅｇ］〜９０［ｄｅｇ］となるので、式（２８）のようにビーム角度φをｓｉｎφで算出することが好ましい。【００８６】
上記のように、第１実施例は、最小２乗法で平面の方程式を求めているため、図７（Ｂ）に示すように、地物が多少曲面形状であったとしても法線ベクトルを求めることができる。さらに、図７（Ｃ）に示すように、地物の面に多少の凹凸があったとしても、最小２乗法による平面の方程式を作成することができ、法線ベクトルを求めることができる。【００８７】
［５．１．３］算出処理
次に、ビーム角度φの算出処理について説明する。図８は、第１実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。【００８８】
まず、ビーム角度算出部１４は、ＬｉＤＡＲにより地物Ａを計測して得たｎ個のデータ（ｘｋ，ｙｋ，ｚｋ）から、重心点Ｇ（ｘｇ，ｙｇ，ｚｇ）を算出し、車両から重心点Ｇへの方向ベクトルＤＶ（ｘｇ，ｙｇ，ｚｇ）とする。また、ビーム角度算出部１４は、車両から重心点Ｇまでの距離ｒＡ［ｔ］と角度θＡ［ｔ］を求める（ステップＳ２１）。【００８９】
次に、ビーム角度算出部１４は、ｎ個のデータ（ｘｋ，ｙｋ，ｚｋ）から、最小２乗法を用いて、平面の方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０を作成し、平面の法線ベクトルＮＶ（ａ，ｂ，ｃ）を求める（ステップＳ２２）。そして、ビーム角度算出部１４は、法線ベクトルＮＶと方向ベクトルＤＶのｘｙ成分から、２つのベクトルがなす角度φを算出する（ステップＳ２３）。以上でビーム角度φの算出処理は終了する。【００９０】
［５．２］第２実施例
次に、ビーム角度φの算出方法の第２実施例を説明する。第２実施例は、ビーム角度φを簡易的に求める方法である。【００９１】
［５．２．１］算出方法
図９は、第２実施例によるビーム角度φの算出方法を示す。図９において、車両の位置を原点Ｏとし、平面板状の地物の両端が点Ｃ、Ｅであるとする。ビーム角度算出部１４は、まず、地物の計測データから、その両端点Ｃ、Ｅまでの距離、即ち、ＯＣ長、ＯＥ長と、その間の角度Δθを抽出する。【００９２】
次に、ビーム角度算出部１４は、三角形△ＯＣＥに対する余弦定理により、下記のようにＣＥ長を求める。【００９３】
【数３６】
0006707627.tif 000037
次に、ビーム角度算出部１４は、求めたＣＥ長を利用して余弦定理を用いて、下記のように∠ＯＣＥの角度を求める。【００９４】
【数３７】
0006707627.tif 000038
次に、ビーム角度算出部１４は、辺ＣＥの中点を点Ｄ（ＣＤ長＝ＤＥ長）とし、三角形△ＯＣＤに対する余弦定理によりＯＤ長を求める。【００９５】
【数３８】
0006707627.tif 000039
次に、ビーム角度算出部１４は、求めたＯＤ長を利用して、∠ＣＤＯの角度を求める。【００９６】
【数３９】
0006707627.tif 000040
そして、ビーム角度算出部１４は、９０度から∠ＣＤＯの角度を引いて角度φを求める。【００９７】
【数４０】
0006707627.tif 000041
こうして、ビーム角度φを簡易的に求めることができる。この方法の利点は、第１実施例よりも計算量が少なくて済むことである。
［５．２．２］算出処理
次に、ビーム角度φの算出処理について説明する。図１０は、第２実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。【００９８】
まず、ビーム角度算出部１４は、地物Ａを検出した複数のビームのうち、横方向に最も離れた２つのビームを選択して地物の両端点Ｃ、Ｅを求め、両端点Ｃ、Ｅまでの距離ＯＣ、ＯＥ及びその間の角度Δθを取得する（ステップＳ３１）。次に、ビーム角度算出部１４は、両端点Ｃ、Ｅの中点Ｄを求め、車両からその中点Ｄまでの距離（ＯＤ長）求める（ステップＳ３２）。次に、ビーム角度算出部１４は、車両と中点Ｄとを結んだ直線ＯＤと地物の平面とがなす角度（∠ＣＤＯ）を求める（ステップＳ３３）。そして、ビーム角度算出部１４は、求めた角度（∠ＣＤＯ）を９０度から減算して角度φを算出する（ステップＳ３４）。以上でビーム角度φの算出処理は終了する。【００９９】
［５．３］第３実施例
次に、ビーム角度φの算出方法の第３実施例を説明する。第３実施例は、ビーム角度φを近似的に求める方法である。【０１００】
［５．３．１］算出方法
図１１は、第３実施例によるビーム角度φの算出方法を示す。図１１において、車両の位置を原点Ｏとし、平面板状の地物の両端が点Ｃ、Ｅであるとする。ビーム角度算出部１４は、まず地物の計測データから、その両端点Ｃ、Ｅまでの距離、即ち、ＯＣ長、ＯＥ長と、その間の角度Δθを抽出する。【０１０１】
次に、直線ＯＥ上に、ＯＣ長と同じ長さとなる点Ｆを設けると、【０１０２】
【数４１】
0006707627.tif 000042
となる。【０１０３】
また、三角形△ＯＣＦは二等辺三角形であるので、【０１０４】
【数４２】
0006707627.tif 000043
となる。【０１０５】
ここで、車両から地物までの距離が数メートル以上であれば、直線ＯＣと直線ＯＥはほぼ平行とみなすことができる。よって、三角形△ＣＦＥを、∠ＣＦＥが直角である直角三角形とみなすと、角度φは以下のように得られる。【０１０６】
【数４３】
0006707627.tif 000044
この方法の利点は、第２実施例よりもさらに計算量が少なくて済むことである。
［５．３．２］算出処理
次に、ビーム角度φの算出処理について説明する。図１２は、第３実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。【０１０７】
まず、ビーム角度算出部１４は、地物Ａを検出した複数のビームのうち横方向に最も離れた２つのビームを選択して地物の両端点Ｃ、Ｅを求め、両端点Ｃ、Ｅまでの距離ＯＣ、ＯＥ及びその間の角度Δθを取得する（ステップＳ４１）。次に、ビーム角度算出部１４は、車両から端点Ｅまでの直線ＯＥ上に点Ｆを設け、前述の式（２９）により点Ｆから端点ＥまでのＦＥ長を算出する（ステップＳ４２）。【０１０８】
次に、ビーム角度算出部１４は、ＯＣ長と角度Δθを用いて、式（３０）によりＣＦ長を算出する（ステップＳ４３）。そして、ビーム角度算出部１４は、ＦＥ長とＣＦ長を用いて、式（３１）により角度φを算出する（ステップＳ４４）。以上でビーム角度φの算出処理は終了する。【０１０９】
［６］地物計測中の車両の移動を考慮した地物の計測点の座標計算
上述したヨー角変化量ΔΨの算出は、地物の法線ベクトルと車両から地物までの方向ベクトルとがなす角度φを求めることにより行われる。しかし、水平方向にスキャンするタイプのＬｉＤＡＲの場合、対象とする地物の左端と右端を計測する時刻が異なるので、その時間内に車両自身が移動すると、計測の基準位置と基準方位が異なることになる。よって、計測の基準位置と基準方位を合わせないままでは、角度φの算出に含まれる誤差が大きくなってしまう。【０１１０】
図１３（Ａ）は、ＬｉＤＡＲにより１つの地物を計測する間に車両が移動する様子を示す。地物の左端を計測するときには車両は位置Ｐ１にあり、地物の右端を計測するときには車両は位置Ｐｎに移動している。【０１１１】
図１３（Ｂ）は、図１３（Ａ）の例においてＬｉＤＡＲで計測した地物上の点の座標（ｒ、θ）を、車両の位置を原点とした（ｘ，ｙ）座標に変換したものを示す。車両の移動に伴って、地物までの距離と方向が変化するため、地物の平面が歪んだものとなってしまう。その結果、地物の中心（又は重心）座標と法線ベクトルとがずれてしまう。【０１１２】
図１４（Ａ）に示すように、水平スキャン型のＬｉＤＡＲのスキャン周期がＴ［ｓ］の場合、スキャン角周波数ω［ｒａｄ／ｓ］は、ω＝２π／Ｔとなり、ｒ［ｍ］先でのビームが移動する速度はｒω［ｍ／ｓ］となる。そこに幅Ｗ［ｍ］の道路標識があった場合、その道路標識をスキャンする時間ΔＴは以下となる。【０１１３】
【数４４】
0006707627.tif 000045
つまり、図１４（Ａ）のようにＬｉＤＡＲのスキャンが時計回りであるとすると、道路標識の左端と右端の計測にΔＴの時間差があることになる。仮に、ＬｉＤＡＲのスキャン周期が１００［ｍｓ］の場合、１０［ｍ］先の６０［ｃｍ］幅の道路標識をビームが計測する時間は、式（３２）にＴ＝０．１、ｒ＝１０、Ｗ＝０．６を代入して、ΔＴ＝０．９５５［ｍｓ］と求められる。車両の速度が１００［ｋｍ／ｈ］の時を想定すると、ΔＴ＝０．９５５［ｍｓ］の間に、車両は２．６５［ｃｍ］進むことになる。また、仮に道路標識がもっと近く、２［ｍ］先の位置にあるとすると、ｒ＝２を代入して、ΔＴ＝４．７７５［ｍｓ］となり、車両は１３．２６［ｃｍ］進むことになる。【０１１４】
従って、車両の速度が速いほど、また対象の地物が近いほど、地物の計測時間内における車両の移動距離が長くなる。その移動距離を考慮せずに計算を行うと、前述のように誤差が大きくなってしまう。よって、車体速度とヨー角変化量の高精度な算出のためには、地物の計測時間内における車両の移動を考慮する必要がある。【０１１５】
上記の考察で見積もりした道路標識の計測時間は数ｍｓ程度であるため、その時間内では車両の速度やヨーレートの変化はほぼ無いとみなすことができる。よって、前の時刻で計算された自車の速度とヨーレートの値を用いて、現在の地物計測中における車両の方向変化分と位置変化分を算出し、その変化分を用いて各計測点の座標を求めればよい。【０１１６】
例えば、図１４（Ｂ）において、Ｐｋ点で計測した地物の座標を、Ｐ１点の座標系を用いた値に変換する。順番としては、まずＰｋ点において破線で示す座標軸へと座標を回転させ、その後、ｘ方向とｙ方向の位置の差分（Δｘｋ，Δｙｋ）を加算する。【０１１７】
まず、Ｐｋ点で計測した距離ｒｋと角度θｋから、以下のようにＰｋ点を原点とするｘｙ座標系の値（ｘｋ，ｙｋ）が得られているものとする。【０１１８】
【数４５】
0006707627.tif 000046
ヨーレートが一定という前提があるため、Ｐ１点からＰｋ点までのΔｔｋ間の方向の変化量Ψｋは次式となる。【０１１９】
【数４６】
0006707627.tif 000047
また、仮想的な中心点Ｏからの回転半径Ｒを想定すると、【０１２０】
【数４７】
0006707627.tif 000048
となるため、ΔｘｋとΔｙｋは以下のように表すことができる。【０１２１】
【数４８】
0006707627.tif 000049
よって、Ｐｋ点で計測する地物の座標は、Ｐ１点を基準位置にすると、以下で計算される。【０１２２】
【数４９】
0006707627.tif 000050
よって、地物のｎ個の計測点に対して上記の計算を行い、算出した複数の座標点を用いて地物の重心点又は中点、及び、法線ベクトルを求めて角度φを算出すればよい。【０１２３】
図１５は、地物計測時間内の車両の移動を考慮した、地物の計測点の座標計算処理のフローチャートである。この処理は、前述の地物計測部１３により行われる。まず、地物計測部１３は、前の時刻で計測（あるいは推定）した自車の速度Ｖ（ｔ-1）とヨーレートΨ＿ｄｏｔ（ｔ−１）を読み出し（ステップＳ５０）、平面形状の道路標識である地物Ａを対象としたｎ点の計測データを取得する（ステップＳ５１）。次に、地物計測部１３は、地物の計測データの左端のデータＰ１を基準の位置及び方位に設定する（ステップＳ５２）。【０１２４】
次に、地物計測部１３は、係数ｋに「１」をセットし（ステップＳ５３）、係数ｋが計測データ数ｎ以下か否かを判定する（ステップＳ５４）。係数ｋが計測データ数ｎ以下である場合（ステップＳ５４：ＹＥＳ）、地物計測部１３は点Ｐ１から点Ｐｋまでの時間差Δｔｋを求める（ステップＳ５５）。【０１２５】
次に、地物計測部１３は、前述の式（３３）を用いて、Ｐ１点からＰｋ点までの角度変化Ψｋを求め（ステップＳ５６）、さらに式（３４）、（３５）を用いてＰ１点からＰｋ点までの位置の差（Δｘｋ，Δｙｋ）を求める（ステップＳ５７）。そして、地物計測部１３は、式（３６）を用いて、Ｐ１点を基準とした座標系におけるＰｋ点での地物の座標を算出する（ステップＳ５８）。【０１２６】
次に、地物計測部１３は、係数ｋをインクリメントし（ステップＳ５９）、ステップＳ５４へ戻る。そして、係数ｋが計測データ数ｎを超えると（ステップＳ５４：ＮＯ）、処理は終了する。こうして得られた計測データを用いてビーム角度φを算出することにより、地物の計測時間内の車両の移動分を補正し、移動距離ΔＬ及びヨー角変化量を高精度で算出することが可能となる。また、ヨー角変化量ΔΨを積分することにより、ヨー角Ψを算出することができ、ヨー角変化量ΔΨを所定時間Δｔで除算することによりヨーレートΨ＿ｄｏｔ（ｔ）を算出することができる。さらに、移動距離ΔＬを所定時間Δｔで除算することにより車体速度Ｖを算出することができ、移動距離ΔＬを積分することにより移動距離Ｌを算出することもできる。【０１２７】
［７］変形例
地物の平面に対してＬｉＤＡＲのビームが小さな角度で入射すると、距離計測値の誤差が大きくなるため、その状態で算出する車体速度とヨー角変化量にも誤差が大きくなる。従って、地物の法線ベクトルと車両から地物までの方向ベクトルとのなす角度φが大きいときは、誤差が大きいと判断し、車速センサの変換係数や、加速度センサとジャイロセンサの感度係数とオフセット係数を算出する際、重み付け値を小さくして平均化処理を行うことが好ましい。これにより、誤差の含まれる算出値の影響を小さくして、感度係数やオフセット係数を算出することができる。
【産業上の利用可能性】
【０１２８】
本発明は、移動体に搭載する装置に利用することができる。
【符号の説明】
【０１２９】
７  車速センサ
８  加速度センサ
９  ジャイロセンサ
１２  外界センサ
１３  地物計測部
１４  ビーム角度算出部
１５  係数計算部
１６  ヨー角変化量計算部
【図１】
0006707627.tif 000051
【図２】
0006707627.tif 000052
【図３】
0006707627.tif 000053
【図４】
0006707627.tif 000054
【図５】
0006707627.tif 000055
【図６】
0006707627.tif 000056
【図７】
0006707627.tif 000057
【図８】
0006707627.tif 000058
【図９】
0006707627.tif 000059
【図１０】
0006707627.tif 000060
【図１１】
0006707627.tif 000061
【図１２】
0006707627.tif 000062
【図１３】
0006707627.tif 000063
【図１４】
0006707627.tif 000064
【図１５】
0006707627.tif 000065